| # This file is part of Scapy |
| # Scapy is free software: you can redistribute it and/or modify |
| # it under the terms of the GNU General Public License as published by |
| # the Free Software Foundation, either version 2 of the License, or |
| # any later version. |
| # |
| # Scapy is distributed in the hope that it will be useful, |
| # but WITHOUT ANY WARRANTY; without even the implied warranty of |
| # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| # GNU General Public License for more details. |
| # |
| # You should have received a copy of the GNU General Public License |
| # along with Scapy. If not, see <http://www.gnu.org/licenses/>. |
| |
| # Copyright (C) 2016 Gauthier Sebaux |
| |
| # scapy.contrib.description = ProfinetIO Real-Time Cyclic (RTC) |
| # scapy.contrib.status = loads |
| |
| """ |
| PROFINET IO layers for scapy which correspond to Real-Time Cyclic data |
| """ |
| |
| # external imports |
| from __future__ import absolute_import |
| import math |
| import struct |
| |
| # Scapy imports |
| from scapy.all import Packet, bind_layers, Ether, UDP, Field, conf |
| from scapy.fields import BitEnumField, BitField, ByteField,\ |
| FlagsField,\ |
| PacketListField,\ |
| ShortField, StrFixedLenField,\ |
| XBitField, XByteField |
| |
| # local imports |
| from scapy.contrib.pnio import ProfinetIO |
| from scapy.compat import orb |
| from scapy.modules.six.moves import range |
| |
| |
| ##################################### |
| ## PROFINET Real-Time Data Packets ## |
| ##################################### |
| |
| class PNIORealTimeIOxS(Packet): |
| """IOCS and IOPS packets for PROFINET Real-Time payload""" |
| name = "PNIO RTC IOxS" |
| fields_desc = [ |
| BitEnumField("dataState", 1, 1, ["bad", "good"]), |
| BitEnumField("instance", 0, 2, ["subslot", "slot", "device", "controller"]), |
| XBitField("reserved", 0, 4), |
| BitField("extension", 0, 1), |
| ] |
| |
| def extract_padding(self, s): |
| return None, s # No extra payload |
| |
| |
| class PNIORealTimeRawData(Packet): |
| """Raw data packets for PROFINET Real-Time payload. |
| |
| It's a configurable packet whose config only includes a fix length. The |
| config parameter must then be a dict {"length": X}. |
| |
| PROFINET IO specification impose this packet to be followed with an IOPS |
| (PNIORealTimeIOxS)""" |
| __slots__ = ["_config"] |
| name = "PNIO RTC Raw data" |
| fields_desc = [ |
| StrFixedLenField("load", "", length_from=lambda p: p[PNIORealTimeRawData].length()), |
| ] |
| |
| def __init__(self, _pkt="", post_transform=None, _internal=0, _underlayer=None, config=None, **fields): |
| """ |
| length=None means that the length must be managed by the user. If it's |
| defined, the field will always be length-long (padded with b"\\x00" if |
| needed) |
| """ |
| self._config = config |
| Packet.__init__(self, _pkt=_pkt, post_transform=post_transform, |
| _internal=_internal, _underlayer=_underlayer, **fields) |
| |
| def copy(self): |
| pkt = Packet.copy(self) |
| pkt._config = self._config |
| return pkt |
| |
| def clone_with(self, *args, **kargs): |
| pkt = Packet.clone_with(self, *args, **kargs) |
| pkt._config = self._config |
| return pkt |
| |
| def length(self): |
| """Get the length of the raw data""" |
| # Manage the length of the packet if a length is provided |
| return self._config["length"] |
| |
| # Make sure an IOPS follows a data |
| bind_layers(PNIORealTimeRawData, PNIORealTimeIOxS) |
| |
| |
| |
| ############################### |
| ## PROFINET Real-Time Fields ## |
| ############################### |
| |
| class LowerLayerBoundPacketListField(PacketListField): |
| """PacketList which binds each underlayer of packets to the current pkt""" |
| def m2i(self, pkt, m): |
| return self.cls(m, _underlayer=pkt) |
| |
| class NotionalLenField(Field): |
| """A len fields which isn't present in the machine representation, but is |
| computed from a given lambda""" |
| __slots__ = ["length_from", "count_from"] |
| def __init__(self, name, default, length_from=None, count_from=None): |
| Field.__init__(self, name, default) |
| self.length_from = length_from |
| self.count_from = count_from |
| |
| def addfield(self, pkt, s, val): |
| return s # Isn't present in the machine packet |
| |
| def getfield(self, pkt, s): |
| val = None |
| if self.length_from is not None: |
| val = self.length_from(pkt, s) |
| elif self.count_from is not None: |
| val = self.count_from(pkt, s) |
| return s, val |
| |
| |
| ############################### |
| ## PNIORealTime Configuration # |
| ############################### |
| |
| # conf.contribs["PNIO_RTC"] is a dict which contains data layout for each Ethernet |
| # communications. It must be formatted as such: |
| # {(Ether.src, Ether.dst): [(start, type, config), ...]} |
| # start: index of a data field from the END of the data buffer (-1, -2, ...) |
| # type: class to be instanciated to represent these data |
| # config: a config dict, given to the type class constructor |
| conf.contribs["PNIO_RTC"] = {} |
| |
| def _get_ethernet(pkt): |
| """Find the Ethernet packet of underlayer or None""" |
| ether = pkt |
| while ether is not None and not isinstance(ether, Ether): |
| ether = ether.underlayer |
| return ether |
| |
| def pnio_update_config(config): |
| """Update the PNIO RTC config""" |
| conf.contribs["PNIO_RTC"].update(config) |
| |
| def pnio_get_config(pkt): |
| """Retrieve the config for a given communication""" |
| # get the config based on the tuple (Ether.src, Ether.dst) |
| ether = _get_ethernet(pkt) |
| config = None |
| if ether is not None and (ether.src, ether.dst) in conf.contribs["PNIO_RTC"]: |
| config = conf.contribs["PNIO_RTC"][(ether.src, ether.dst)] |
| |
| return config |
| |
| |
| ############################### |
| ## PROFINET Real-Time Packet ## |
| ############################### |
| |
| def _pnio_rtc_guess_payload_class(_pkt, _underlayer=None, *args, **kargs): |
| """A dispatcher for the packet list field which manage the configuration |
| to fin dthe appropriate class""" |
| config = pnio_get_config(_underlayer) |
| |
| if isinstance(config, list): |
| # If we have a valid config, it's a list which describe each data |
| # packets the rest being IOCS |
| cur_index = -len(_pkt) |
| for index, cls, params in config: |
| if cur_index == index: |
| return cls(_pkt, config=params, *args, **kargs) |
| |
| # Not a data => IOCS packet |
| return PNIORealTimeIOxS(_pkt, *args, **kargs) |
| else: |
| # No config => Raw data which dissect the whole _pkt |
| return PNIORealTimeRawData(_pkt, |
| config={"length": len(_pkt)}, |
| *args, **kargs |
| ) |
| |
| |
| _PNIO_DS_FLAGS = [ |
| "primary", |
| "redundancy", |
| "validData", |
| "reserved_1", |
| "run", |
| "no_problem", |
| "reserved_2", |
| "ignore", |
| ] |
| class PNIORealTime(Packet): |
| """PROFINET cyclic real-time""" |
| name = "PROFINET Real-Time" |
| fields_desc = [ |
| NotionalLenField("len", None, length_from=lambda p, s: len(s)), |
| NotionalLenField("dataLen", None, length_from=lambda p, s: len(s[:-4].rstrip(b"\0"))), |
| LowerLayerBoundPacketListField("data", [], _pnio_rtc_guess_payload_class, length_from=lambda p: p.dataLen), |
| StrFixedLenField("padding", "", length_from=lambda p: p[PNIORealTime].padding_length()), |
| ShortField("cycleCounter", 0), |
| FlagsField("dataStatus", 0x35, 8, _PNIO_DS_FLAGS), |
| ByteField("transferStatus", 0) |
| ] |
| overload_fields = { |
| ProfinetIO: {"frameID": 0x8000}, # RT_CLASS_1 |
| } |
| |
| def padding_length(self): |
| """Compute the length of the padding need for the ethernet frame""" |
| fld, val = self.getfield_and_val("data") |
| |
| # use the len field if available to define the padding length, eg for |
| # dissected packets |
| pkt_len = self.getfieldval("len") |
| if pkt_len is not None: |
| return max(0, pkt_len - len(fld.addfield(self, b"", val)) - 4) |
| |
| if isinstance(self.underlayer, ProfinetIO) and \ |
| isinstance(self.underlayer.underlayer, UDP): |
| return max(0, 12 - len(fld.addfield(self, b"", val))) |
| else: |
| return max(0, 40 - len(fld.addfield(self, b"", val))) |
| |
| @staticmethod |
| def analyse_data(packets): |
| """Analyse the data to find heuristical properties and determine |
| location and type of data""" |
| loc = PNIORealTime.find_data(packets) |
| loc = PNIORealTime.analyse_profisafe(packets, loc) |
| pnio_update_config(loc) |
| return loc |
| |
| @staticmethod |
| def find_data(packets): |
| """Analyse a packet list to extract data offsets from packets data.""" |
| # a dictionary to count data offsets (ie != 0x80) |
| # It's formatted: {(src, dst): (total, [count for offset in len])} |
| heuristic = {} |
| |
| # Counts possible data locations |
| # 0x80 are mainly IOxS and trailling 0x00s are just padding |
| for pkt in packets: |
| if PNIORealTime in pkt: |
| pdu = bytes(pkt[PNIORealTime])[:-4].rstrip(b"\0") |
| |
| if (pkt.src, pkt.dst) not in heuristic: |
| heuristic[(pkt.src, pkt.dst)] = (0, []) |
| |
| total, counts = heuristic[(pkt.src, pkt.dst)] |
| |
| if len(counts) < len(pdu): |
| counts.extend([0 for _ in range(len(pdu) - len(counts))]) |
| |
| for i in range(len(pdu)): |
| if orb(pdu[i]) != 0x80: |
| counts[i] += 1 |
| |
| comm = (pkt.src, pkt.dst) |
| heuristic[comm] = (total + 1, counts) |
| |
| # Determine data locations |
| locations = {} |
| for comm in heuristic: |
| total, counts = heuristic[comm] |
| length = len(counts) |
| loc = locations[comm] = [] |
| start = None |
| for i in range(length): |
| if counts[i] > total // 2: # Data if more than half is != 0x80 |
| if start is None: |
| start = i |
| else: |
| if start is not None: |
| loc.append(( |
| start - length, |
| PNIORealTimeRawData, |
| {"length": i - start} |
| )) |
| start = None |
| |
| return locations |
| |
| @staticmethod |
| def analyse_profisafe(packets, locations=None): |
| """Analyse a packet list to find possible PROFISafe profils. |
| |
| It's based on an heuristical analysis of each payload to try to find |
| CRC and control/status byte. |
| |
| locations: possible data locations. If not provided, analyse_pn_rt will |
| be called beforehand. If not given, it calls in the same time |
| analyse_data which update the configuration of the data field""" |
| # get data locations and entropy of bytes |
| if not locations: |
| locations = PNIORealTime.find_data(packets) |
| entropies = PNIORealTime.data_entropy(packets, locations) |
| |
| # Try to find at least 3 high entropy successive bytes (the CRC) |
| for comm in entropies: |
| entropy = dict(entropies[comm]) # Convert tuples to key => value |
| |
| for i in range(len(locations[comm])): |
| # update each location with its value after profisafe analysis |
| locations[comm][i] = \ |
| PNIORealTime.analyse_one_profisafe_location( |
| locations[comm][i], entropy |
| ) |
| |
| return locations |
| |
| @staticmethod |
| def analyse_one_profisafe_location(location, entropy): |
| """Analyse one PNIO RTC data location to find if its a PROFISafe |
| |
| :param location: location to analyse, a tuple (start, type, config) |
| :param entropy: the entropy of each byte of the packet data |
| :returns: the configuration associated with the data |
| """ |
| start, klass, conf = location |
| if conf["length"] >= 4: # Minimal PROFISafe length |
| succ_count = 0 |
| for j in range(start, start + conf["length"]): |
| # Limit for a CRC is set to 6 bit of entropy min |
| if j in entropy and entropy[j] >= 6: |
| succ_count += 1 |
| else: |
| succ_count = 0 |
| # PROFISafe profiles must end with at least 3 bytes of high entropy |
| if succ_count >= 3: # Possible profisafe CRC |
| return ( |
| start, |
| Profisafe, |
| {"CRC": succ_count, "length": conf["length"]} |
| ) |
| # Not a PROFISafe profile |
| return (start, klass, conf) |
| |
| @staticmethod |
| def data_entropy(packets, locations=None): |
| """Analyse a packet list to find the entropy of each data byte |
| |
| locations: possible data locations. If not provided, analyse_pn_rt will |
| be called beforehand. If not given, it calls in the same time |
| analyse_data which update the configuration of the data field""" |
| if not locations: |
| locations = PNIORealTime.find_data(packets) |
| |
| # Retrieve the entropy of each data byte, for each communication |
| entropies = {} |
| for comm in locations: |
| if len(locations[comm]) > 0: # Doesn't append empty data |
| entropies[comm] = [] |
| comm_packets = [] |
| |
| # fetch all packets from the communication |
| for pkt in packets: |
| if PNIORealTime in pkt and (pkt.src, pkt.dst) == comm: |
| comm_packets.append( |
| bytes(pkt[PNIORealTime])[:-4].rstrip(b"\0") |
| ) |
| |
| # Get the entropy |
| for start, dummy, conf in locations[comm]: |
| for i in range(start, start + conf["length"]): |
| entropies[comm].append( |
| (i, entropy_of_byte(comm_packets, i)) |
| ) |
| |
| return entropies |
| |
| @staticmethod |
| def draw_entropy(packets, locations=None): |
| """Plot the entropy of each data byte of PN RT communication""" |
| import matplotlib.pyplot as plt |
| import matplotlib.cm as cm |
| entropies = PNIORealTime.data_entropy(packets, locations) |
| |
| rows = len(entropies) |
| cur_row = 1 |
| for comm in entropies: |
| index = [] |
| vals = [] |
| for i, ent in entropies[comm]: |
| index.append(i) |
| vals.append(ent) |
| |
| # Offsets the indexes to get the index from the beginning |
| offset = -min(index) |
| index = [i + offset for i in index] |
| |
| plt.subplot(rows, 1, cur_row) |
| plt.bar(index, vals, 0.8, color="r") |
| plt.xticks([i + 0.4 for i in index], index) |
| plt.title("Entropy from %s to %s" % comm) |
| cur_row += 1 |
| plt.ylabel("Shannon Entropy") |
| |
| plt.xlabel("Byte offset") # x label only on the last row |
| plt.legend() |
| |
| plt.tight_layout() |
| plt.show() |
| |
| def entropy_of_byte(packets, position): |
| """Compute the entropy of a byte at a given offset""" |
| counter = [0 for _ in range(256)] |
| |
| # Count each byte a appearance |
| for pkt in packets: |
| if -position <= len(pkt): # position must be a negative index |
| counter[orb(pkt[position])] += 1 |
| |
| # Compute the Shannon entropy |
| entropy = 0 |
| length = len(packets) |
| for count in counter: |
| if count > 0: |
| ratio = float(count) / length |
| entropy -= ratio * math.log(ratio, 2) |
| |
| return entropy |
| |
| ############### |
| ## PROFISafe ## |
| ############### |
| |
| class XVarBytesField(XByteField): |
| """Variable length bytes field, from 0 to 8 bytes""" |
| __slots__ = ["length_from"] |
| def __init__(self, name, default, length=None, length_from=None): |
| self.length_from = length_from |
| if length: |
| self.length_from = lambda p, l=length: l |
| Field.__init__(self, name, default, "!Q") |
| |
| def addfield(self, pkt, s, val): |
| length = self.length_from(pkt) |
| return s + struct.pack(self.fmt, self.i2m(pkt, val))[8-length:] |
| |
| def getfield(self, pkt, s): |
| length = self.length_from(pkt) |
| val = struct.unpack(self.fmt, b"\x00"*(8 - length) + s[:length])[0] |
| return s[length:], self.m2i(pkt, val) |
| |
| |
| class Profisafe(PNIORealTimeRawData): |
| """PROFISafe profil to be encapsulated inside the PNRT.data list. |
| |
| It's a configurable packet whose config includes a fix length, and a CRC |
| length. The config parameter must then be a dict {"length": X, "CRC": Y}. |
| """ |
| name = "PROFISafe" |
| fields_desc = [ |
| StrFixedLenField("load", "", length_from=lambda p: p[Profisafe].data_length()), |
| XByteField("Control_Status", 0), |
| XVarBytesField("CRC", 0, length_from=lambda p: p[Profisafe].crc_length()) |
| ] |
| def data_length(self): |
| """Return the length of the data""" |
| ret = self.length() - self.crc_length() - 1 |
| return ret |
| |
| def crc_length(self): |
| """Return the length of the crc""" |
| return self._config["CRC"] |
| |
