Fix lldp org specific dissection clean #1151

scapy/contrib/lldp.py

@@ -28,7 +28,7 @@
             - IEEE 802.1AB 2016 - LLDP protocol, topology and MIB description
 
     :TODO:
-        - organization specific TLV e.g. ProfiNet
+        - organization specific TLV e.g. ProfiNet (see LLDPDUGenericOrganisationSpecific for a starting point)
 
     :NOTES:
         - you can find the layer configuration options at the end of this file
@@ -45,11 +45,11 @@
 from scapy.layers.dot11 import Packet
 from scapy.layers.l2 import Ether, Dot1Q, bind_layers, \
     BitField, StrLenField, ByteEnumField, BitEnumField, \
-    BitFieldLenField, ShortField, Padding, Scapy_Exception, \
-    XStrLenField
+    BitFieldLenField, ShortField, Scapy_Exception, \
+    XStrLenField, ByteField, EnumField, ThreeBytesField
 from scapy.modules.six.moves import range
 from scapy.data import ETHER_TYPES
-from scapy.compat import orb, raw
+from scapy.compat import orb
 
 LLDP_NEAREST_BRIDGE_MAC = '01:80:c2:00:00:0e'
 LLDP_NEAREST_NON_TPMR_BRIDGE_MAC = '01:80:c2:00:00:03'
@@ -123,8 +123,11 @@ class LLDPDU(Packet):
 
     def guess_payload_class(self, payload):
         # type is a 7-bit bitfield spanning bits 1..7 -> div 2
-        lldpdu_tlv_type = orb(payload[0]) // 2
-        return LLDPDU_CLASS_TYPES[lldpdu_tlv_type]
+        try:
+            lldpdu_tlv_type = orb(payload[0]) // 2
+            return LLDPDU_CLASS_TYPES.get(lldpdu_tlv_type, conf.raw_layer)
+        except IndexError:
+            return conf.raw_layer
 
     @staticmethod
     def _dot1q_headers_size(layer):
@@ -664,6 +667,38 @@ def do_build(self):
         self._check()
         return super(LLDPDUManagementAddress, self).do_build()
 
+
+class ThreeBytesEnumField(EnumField, ThreeBytesField):
+
+    def __init__(self, name, default, enum):
+        EnumField.__init__(self, name, default, enum, "!I")
+
+
+class LLDPDUGenericOrganisationSpecific(LLDPDU):
+
+    ORG_UNIQUE_CODE_PNO = 0x000ecf
+    ORG_UNIQUE_CODE_IEEE_802_1 = 0x0080c2
+    ORG_UNIQUE_CODE_IEEE_802_3 = 0x00120f
+    ORG_UNIQUE_CODE_TIA_TR_41_MED = 0x0012bb
+    ORG_UNIQUE_CODE_HYTEC = 0x30b216
+
+    ORG_UNIQUE_CODES = {
+        ORG_UNIQUE_CODE_PNO: "PROFIBUS International (PNO)",
+        ORG_UNIQUE_CODE_IEEE_802_1: "IEEE 802.1",
+        ORG_UNIQUE_CODE_IEEE_802_3: "IEEE 802.3",
+        ORG_UNIQUE_CODE_TIA_TR_41_MED: "TIA TR-41 Committee . Media Endpoint Discovery",
+        ORG_UNIQUE_CODE_HYTEC: "Hytec Geraetebau GmbH"
+    }
+
+    fields_desc = [
+        BitEnumField('_type', 127, 7, LLDPDU.TYPES),
+        BitFieldLenField('_length', None, 9, length_of='data', adjust=lambda pkt, x: len(pkt.data) + 4),
+        ThreeBytesEnumField('org_code', 0, ORG_UNIQUE_CODES),
+        ByteField('subtype', 0x00),
+        XStrLenField('data', '', length_from=lambda pkt: pkt._length - 4)
+    ]
+
+# 0x09 .. 0x7e is reserved for future standardization and for now treated as Raw() data
 LLDPDU_CLASS_TYPES = {
     0x00: LLDPDUEndOfLLDPDU,
     0x01: LLDPDUChassisID,
@@ -674,8 +709,7 @@ def do_build(self):
     0x06: LLDPDUSystemDescription,
     0x07: LLDPDUSystemCapabilities,
     0x08: LLDPDUManagementAddress,
-    range(0x09, 0x7e): None,  # reserved - future standardization
-    127: None  # organisation specific TLV
+    127: LLDPDUGenericOrganisationSpecific
 }
 
 class LLDPConfiguration(object):

scapy/contrib/lldp.uts

@@ -228,3 +228,47 @@ assert frm[LLDPDUSystemName].system_name == b'things will'
 assert frm[LLDPDUManagementAddress].object_id == b'burn'
 assert frm[LLDPDUSystemDescription].description == b'without tests.'
 assert frm[LLDPDUPortDescription].description == b'always!'
+
++ organisation specific layers
+
+= ThreeBytesEnumField tests
+
+three_b_enum_field = ThreeBytesEnumField('test', 0x00,
+                                         {
+                                             0x0e: 'fourteen',
+                                             0x00: 'zero',
+                                             0x5566: 'five-six',
+                                             0x0e0000: 'fourteen-zero-zero',
+                                             0x0e0100: 'fourteen-one-zero',
+                                             0x112233: '1#2#3'
+                                         })
+
+assert(three_b_enum_field.i2repr(None, 0) == 'zero')
+assert(three_b_enum_field.i2repr(None, 0x0e) == 'fourteen')
+assert(three_b_enum_field.i2repr(None, 0x5566) == 'five-six')
+assert(three_b_enum_field.i2repr(None, 0x112233) == '1#2#3')
+assert(three_b_enum_field.i2repr(None, 0x0e0000) == 'fourteen-zero-zero')
+assert(three_b_enum_field.i2repr(None, 0x0e0100) == 'fourteen-one-zero')
+assert(three_b_enum_field.i2repr(None, 0x01) == '1')
+assert(three_b_enum_field.i2repr(None, 0x49763) == '300899')
+
+= LLDPDUGenericOrganisationSpecific tests
+
+frm = Ether(src='01:01:01:01:01:01', dst=LLDP_NEAREST_BRIDGE_MAC)/\
+      LLDPDUChassisID(subtype=LLDPDUChassisID.SUBTYPE_MAC_ADDRESS, id=b'\x06\x05\x04\x03\x02\x01')/\
+      LLDPDUPortID(subtype=LLDPDUPortID.SUBTYPE_MAC_ADDRESS, id=b'\x01\x02\x03\x04\x05\x06')/\
+      LLDPDUTimeToLive()/\
+      LLDPDUGenericOrganisationSpecific(org_code=LLDPDUGenericOrganisationSpecific.ORG_UNIQUE_CODE_PNO,
+                                        subtype=0x42,
+                                        data=b'FooBar'*5
+                                        )/\
+      LLDPDUEndOfLLDPDU()
+
+frm = frm.build()
+frm = Ether(frm)
+org_spec_layer = frm[LLDPDUGenericOrganisationSpecific]
+assert(org_spec_layer)
+assert(org_spec_layer._type == 127)
+assert(org_spec_layer.org_code == LLDPDUGenericOrganisationSpecific.ORG_UNIQUE_CODE_PNO)
+assert(org_spec_layer.subtype == 0x42)
+assert(org_spec_layer._length == 34)

scapy/fields.py

@@ -1049,7 +1049,6 @@ def i2repr_one(self, pkt, x):
                     return ret
         return lhex(x)
 
-
 class _MultiEnumField(_EnumField):
     def __init__(self, name, default, enum, depends_on, fmt = "H"):