| #!/usr/bin/env python | |
| # | |
| # Copyright (c) 2016, The OpenThread Authors. | |
| # All rights reserved. | |
| # | |
| # Redistribution and use in source and binary forms, with or without | |
| # modification, are permitted provided that the following conditions are met: | |
| # 1. Redistributions of source code must retain the above copyright | |
| # notice, this list of conditions and the following disclaimer. | |
| # 2. Redistributions in binary form must reproduce the above copyright | |
| # notice, this list of conditions and the following disclaimer in the | |
| # documentation and/or other materials provided with the distribution. | |
| # 3. Neither the name of the copyright holder nor the | |
| # names of its contributors may be used to endorse or promote products | |
| # derived from this software without specific prior written permission. | |
| # | |
| # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" | |
| # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE | |
| # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE | |
| # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE | |
| # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR | |
| # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF | |
| # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS | |
| # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN | |
| # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) | |
| # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | |
| # POSSIBILITY OF SUCH DAMAGE. | |
| # | |
| import collections | |
| import io | |
| import ipaddress | |
| import struct | |
| import sys | |
| import common | |
| import config | |
| import ipv6 | |
| class LowpanIPHC: | |
| """ | |
| Class representing a compressed IP header. | |
| More details: | |
| - URL: https://tools.ietf.org/html/rfc6282 | |
| - section: 3.1. LOWPAN_IPHC Encoding Format | |
| """ | |
| def __init__(self, tf, nh, hlim, cid, sac, sam, m, dac, dam): | |
| self._tf = tf | |
| self._nh = nh | |
| self._hlim = hlim | |
| self._cid = cid | |
| self._sac = sac | |
| self._sam = sam | |
| self._m = m | |
| self._dac = dac | |
| self._dam = dam | |
| @property | |
| def tf(self): | |
| return self._tf | |
| @property | |
| def nh(self): | |
| return self._nh | |
| @property | |
| def hlim(self): | |
| return self._hlim | |
| @property | |
| def cid(self): | |
| return self._cid | |
| @property | |
| def sac(self): | |
| return self._sac | |
| @property | |
| def sam(self): | |
| return self._sam | |
| @property | |
| def m(self): | |
| return self._m | |
| @property | |
| def dac(self): | |
| return self._dac | |
| @property | |
| def dam(self): | |
| return self._dam | |
| @classmethod | |
| def from_bytes(cls, data_bytes): | |
| data_byte = data_bytes[0] | |
| hdr = (data_byte >> 5) & 0x07 | |
| if hdr != 0x03: | |
| raise RuntimeError("Not a 6LowPAN packet.") | |
| tf = (data_byte >> 3) & 0x03 | |
| nh = (data_byte >> 2) & 0x01 | |
| hlim = data_byte & 0x03 | |
| data_byte = data_bytes[1] | |
| cid = (data_byte >> 7) & 0x01 | |
| sac = (data_byte >> 6) & 0x01 | |
| sam = (data_byte >> 4) & 0x03 | |
| m = (data_byte >> 3) & 0x01 | |
| dac = (data_byte >> 2) & 0x01 | |
| dam = data_byte & 0x03 | |
| return cls(tf, nh, hlim, cid, sac, sam, m, dac, dam) | |
| def __repr__(self): | |
| return "LowpanIPHC(tf={}, nh={}, hlim={}, cid={}, sac={}, sam={}, m={}, dac={}, dam={})".format(self.tf, self.nh, self.hlim, self.cid, self.sac, self.sam, self.m, self.dac, self.dam) | |
| class LowpanNHC: | |
| """ | |
| Class representing a compressed extension header. | |
| More details: | |
| - URL: https://tools.ietf.org/html/rfc6282 | |
| - section: 4.1. LOWPAN_NHC Format | |
| """ | |
| NHC_EID_HOP_BY_HOP = 0 | |
| NHC_EID_ROUTING = 1 | |
| NHC_EID_FRAGMENT = 2 | |
| NHC_EID_DST_OPT = 3 | |
| NHC_EID_MOBILITY = 4 | |
| NHC_EID_IPV6_HDR = 7 | |
| def __init__(self, eid, nh): | |
| self._eid = eid | |
| self._nh = nh | |
| @property | |
| def eid(self): | |
| return self._eid | |
| @property | |
| def nh(self): | |
| return self._nh | |
| @classmethod | |
| def from_bytes(cls, data_bytes): | |
| header_byte = data_bytes[0] | |
| eid = (header_byte >> 1) & 0x07 | |
| nh = header_byte & 0x01 | |
| return cls(eid, nh) | |
| def __repr__(self): | |
| return "LowpanNHC(eid={}, nh={})".format(self.eid, self.nh) | |
| class LowpanUDPHC: | |
| """ | |
| Class representing compressed UDP header. | |
| More details: | |
| - URL: https://tools.ietf.org/html/rfc6282 | |
| - section: 4.3.3. UDP LOWPAN_NHC Format | |
| """ | |
| def __init__(self, c, p): | |
| self._c = c | |
| self._p = p | |
| @property | |
| def c(self): | |
| return self._c | |
| @property | |
| def p(self): | |
| return self._p | |
| @classmethod | |
| def from_bytes(cls, data_bytes): | |
| data_byte = data_bytes[0] | |
| hdr = (data_byte >> 3) & 0x1f | |
| if hdr != 0x1e: | |
| raise RuntimeError("Not a 6LowPAN UDP header.") | |
| c = (data_byte >> 2) & 0x01 | |
| p = data_byte & 0x03 | |
| return cls(c, p) | |
| def __repr__(self): | |
| return "LowpanUDPHC(c={}, p={})".format(self.c, self.p) | |
| class LowpanHopByHopFactory: | |
| """ Factory that produces HopByHop extension header. """ | |
| def __init__(self, hop_by_hop_options_factory): | |
| self._hop_by_hop_options_factory = hop_by_hop_options_factory | |
| def parse(self, data, next_header, message_info): | |
| ext_header_length = ord(data.read(1)) | |
| ext_header_data = data.read(ext_header_length) | |
| options = self._hop_by_hop_options_factory.parse(io.BytesIO(ext_header_data), message_info) | |
| ext_header = ipv6.HopByHop(next_header, options) | |
| message_info.payload_length += len(ext_header) | |
| return ext_header | |
| class LowpanExtensionHeadersFactory: | |
| """ Factory that produces extension headers. """ | |
| NHC_NH_INLINE = 0 | |
| NHC_NH_COMPRESSED = 1 | |
| def __init__(self, ext_headers_factories): | |
| self._ext_headers_factories = ext_headers_factories if ext_headers_factories is not None else {} | |
| def _decompress_nh(self, hc, data): | |
| if hc.nh == self.NHC_NH_INLINE: | |
| return ord(data.read(1)) | |
| elif hc.nh == self.NHC_NH_COMPRESSED: | |
| return None | |
| def _get_ext_headers_factory(self, eid): | |
| try: | |
| return self._ext_headers_factories[eid] | |
| except: | |
| raise RuntimeError("Could not find an extension header factory for the EID type: {}".format(eid)) | |
| def parse(self, data, message_info): | |
| nhc = LowpanNHC.from_bytes(bytearray(data.read(1))) | |
| next_header = self._decompress_nh(nhc, data) | |
| factory = self._get_ext_headers_factory(nhc.eid) | |
| return factory.parse(data, next_header, message_info) | |
| class LowpanUdpHeaderFactory: | |
| """ Factory producing UDP header. """ | |
| UDP_HC_C_INLINE = 0 | |
| UDP_HC_C_ELIDED = 1 | |
| UDP_HC_P_BOTH_FULL = 0 | |
| UDP_HC_P_DST_COMPR = 1 | |
| UDP_HC_P_SRC_COMPR = 2 | |
| UDP_HC_P_BOTH_COMPR = 3 | |
| def _decompress_udp_ports(self, udphc, data): | |
| if udphc.p == self.UDP_HC_P_BOTH_FULL: | |
| src_port = struct.unpack(">H", data.read(2))[0] | |
| dst_port = struct.unpack(">H", data.read(2))[0] | |
| elif udphc.p == self.UDP_HC_P_DST_COMPR: | |
| src_port = struct.unpack(">H", data.read(2))[0] | |
| dst_port = 0xf000 + ord(data.read(1)) | |
| elif udphc.p == self.UDP_HC_P_SRC_COMPR: | |
| src_port = 0xf000 + ord(data.read(1)) | |
| dst_port = struct.unpack(">H", data.read(2))[0] | |
| elif udphc.p == self.UDP_HC_P_BOTH_COMPR: | |
| udp_ports_byte = ord(data.read(1)) | |
| src_port = 0xf0b0 + ((udp_ports_byte >> 4) & 0x0F) | |
| dst_port = 0xf0b0 + (udp_ports_byte & 0x0F) | |
| return src_port, dst_port | |
| def _decompress_udp_checksum(self, udphc, data): | |
| if udphc.c == self.UDP_HC_C_INLINE: | |
| checksum = struct.unpack(">H", data.read(2))[0] | |
| if udphc.c == self.UDP_HC_C_ELIDED: | |
| checksum = 0 | |
| return checksum | |
| def parse(self, data, message_info): | |
| udphc = LowpanUDPHC.from_bytes(bytearray(data.read(1))) | |
| src_port, dst_port = self._decompress_udp_ports(udphc, data) | |
| checksum = self._decompress_udp_checksum(udphc, data) | |
| header = ipv6.UDPHeader(src_port, dst_port, checksum=checksum) | |
| return header | |
| class Context(): | |
| def __init__(self, prefix, prefix_length=None): | |
| if isinstance(prefix, str): | |
| if sys.version_info[0] == 2: | |
| prefix = prefix.decode("utf-8") | |
| prefix, prefix_length = prefix.split("/") | |
| prefix_length = int(prefix_length) | |
| a = ipaddress.ip_address(prefix) | |
| self._prefix = bytearray(a.packed) | |
| self._prefix_length = prefix_length | |
| elif isinstance(prefix, bytearray): | |
| self._prefix = prefix | |
| self._prefix_length = prefix_length if prefix_length is not None else len(self._prefix) * 8 | |
| @property | |
| def prefix(self): | |
| return self._prefix[:self.prefix_length_all_bytes] | |
| @property | |
| def prefix_full_bytes(self): | |
| return self._prefix[:self.prefix_length_full_bytes] | |
| @property | |
| def prefix_length(self): | |
| return self._prefix_length | |
| @property | |
| def prefix_length_full_bytes(self): | |
| return int(self._prefix_length / 8) | |
| @property | |
| def prefix_length_rest_bits(self): | |
| return int(self._prefix_length % 8) | |
| @property | |
| def prefix_length_all_bytes(self): | |
| if self.prefix_length_rest_bits > 0: | |
| return (self.prefix_length_full_bytes + 1) | |
| return self.prefix_length_full_bytes | |
| class ContextManager(dict): | |
| """ Class representing Context Manager. """ | |
| def __check_index(self, index): | |
| if index < 0 or index > 15: | |
| raise IndexError("Invalid index: {}. Valid index is in range [0, 15]".format(index)) | |
| def __check_type(self, value): | |
| if not isinstance(value, Context): | |
| raise TypeError("Invalid value type: {}".format(type(value))) | |
| def __getitem__(self, index): | |
| self.__check_index(index) | |
| return super(ContextManager, self).__getitem__(index) | |
| def __setitem__(self, index, value): | |
| self.__check_index(index) | |
| self.__check_type(value) | |
| return super(ContextManager, self).__setitem__(index, value) | |
| class LowpanIpv6HeaderFactory: | |
| """ Factory that produces IPv6 header. """ | |
| IPV6_LINKLOCAL_PREFIX = bytearray([0xfe, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]) | |
| SHORT_ADDR_PADDING_BYTES = bytearray([0x00, 0x00, 0x00, 0xff, 0xfe, 0x00]) | |
| IPHC_TF_4B = 0 | |
| IPHC_TF_3B = 1 | |
| IPHC_TF_1B = 2 | |
| IPHC_TF_ELIDED = 3 | |
| IPHC_NH_INLINE = 0 | |
| IPHC_NH_COMPRESSED = 1 | |
| IPHC_HLIM_CALCULATE = -1 | |
| IPHC_HLIM_INLINE = 0 | |
| IPHC_HLIM_1 = 1 | |
| IPHC_HLIM_64 = 2 | |
| IPHC_HLIM_255 = 3 | |
| IPHC_CID_CLEAR = 0 | |
| IPHC_CID_SET = 1 | |
| IPHC_SAC_STATELESS = 0 | |
| IPHC_SAC_STATEFUL = 1 | |
| IPHC_SAM_128B = 0 | |
| IPHC_SAM_UNSPECIFIED = 0 | |
| IPHC_SAM_64B = 1 | |
| IPHC_SAM_16B = 2 | |
| IPHC_SAM_0B = 3 | |
| IPHC_SAM_ELIDED = 3 | |
| IPHC_M_NO = 0 | |
| IPHC_M_YES = 1 | |
| IPHC_DAC_STATELESS = 0 | |
| IPHC_DAC_STATEFUL = 1 | |
| IPHC_DAM_128B = 0 | |
| IPHC_DAM_64B = 1 | |
| IPHC_DAM_48B = 1 | |
| IPHC_DAM_32B = 2 | |
| IPHC_DAM_16B = 2 | |
| IPHC_DAM_8B = 3 | |
| IPHC_DAM_0B = 3 | |
| IPHC_DAM_ELIDED = 3 | |
| IPHC_M_DAM_00 = 0 | |
| IPHC_M_DAM_01 = 1 | |
| IPHC_M_DAM_10 = 2 | |
| IPHC_M_DAM_11 = 3 | |
| def __init__(self, context_manager=None): | |
| self._context_manager = context_manager | |
| def _flow_label(self, data_bytes): | |
| flow_label = (data_bytes[0] & 0x0F) << 16 | |
| flow_label += data_bytes[1] << 8 | |
| flow_label += data_bytes[2] | |
| return flow_label | |
| def _traffic_class(self, dscp, ecn): | |
| return (dscp << 2) | ecn | |
| def _unpack_dscp(self, data_byte): | |
| return (data_byte & 0x3F) | |
| def _unpack_ecn(self, data_byte): | |
| return (data_byte >> 6) | |
| def _decompress_tf_4bytes(self, data): | |
| data_bytes = [b for b in bytearray(data.read(4))] | |
| dscp = self._unpack_dscp(data_bytes[0]) | |
| ecn = self._unpack_ecn(data_bytes[0]) | |
| traffic_class = self._traffic_class(dscp, ecn) | |
| flow_label = self._flow_label(data_bytes[1:]) | |
| return traffic_class, flow_label | |
| def _decompress_tf_3bytes(self, data): | |
| data_bytes = [b for b in bytearray(data.read(3))] | |
| ecn = self._unpack_ecn(data_bytes[0]) | |
| traffic_class = self._traffic_class(dscp=0, ecn=ecn) | |
| flow_label = self._flow_label(data_bytes) | |
| return traffic_class, flow_label | |
| def _decompress_tf_1byte(self, data): | |
| data_byte = ord(data.read(1)) | |
| dscp = self._unpack_dscp(data_byte) | |
| ecn = self._unpack_ecn(data_byte) | |
| traffic_class = self._traffic_class(dscp, ecn) | |
| flow_label = 0 | |
| return traffic_class, flow_label | |
| def _decompress_tf(self, iphc, data): | |
| if iphc.tf == self.IPHC_TF_4B: | |
| return self._decompress_tf_4bytes(data) | |
| elif iphc.tf == self.IPHC_TF_3B: | |
| return self._decompress_tf_3bytes(data) | |
| elif iphc.tf == self.IPHC_TF_1B: | |
| return self._decompress_tf_1byte(data) | |
| elif iphc.tf == self.IPHC_TF_ELIDED: | |
| return 0, 0 | |
| def _decompress_nh(self, hc, data): | |
| if hc.nh == self.IPHC_NH_INLINE: | |
| return ord(data.read(1)) | |
| elif hc.nh == self.IPHC_NH_COMPRESSED: | |
| return None | |
| def _decompress_hlim(self, iphc, data): | |
| if iphc.hlim == self.IPHC_HLIM_INLINE: | |
| return ord(data.read(1)) | |
| elif iphc.hlim == self.IPHC_HLIM_1: | |
| return 1 | |
| elif iphc.hlim == self.IPHC_HLIM_64: | |
| return 64 | |
| elif iphc.hlim == self.IPHC_HLIM_255: | |
| return 255 | |
| def _decompress_cid(self, iphc, data): | |
| if iphc.cid == self.IPHC_CID_SET: | |
| cid = ord(data.read(1)) | |
| sci = (cid >> 4) & 0x0f | |
| dci = cid & 0x0f | |
| return sci, dci | |
| elif iphc.cid == self.IPHC_CID_CLEAR: | |
| return 0, 0 | |
| def _decompress_src_addr_stateless(self, iphc, src_mac_addr, data): | |
| if iphc.sam == self.IPHC_SAM_128B: | |
| return bytearray(data.read(16)) | |
| elif iphc.sam == self.IPHC_SAM_64B: | |
| return self.IPV6_LINKLOCAL_PREFIX + bytearray(data.read(8)) | |
| elif iphc.sam == self.IPHC_SAM_16B: | |
| return self.IPV6_LINKLOCAL_PREFIX + self.SHORT_ADDR_PADDING_BYTES + bytearray(data.read(2)) | |
| elif iphc.sam == self.IPHC_SAM_ELIDED: | |
| return self.IPV6_LINKLOCAL_PREFIX + src_mac_addr.convert_to_iid() | |
| def _merge_prefix_with_address(self, prefix, prefix_length, address_bytes): | |
| required_bytes = 16 | |
| prefix_length_full_bytes = int(prefix_length / 8) | |
| prefix_length_rest_bits = int(prefix_length % 8) | |
| prefix_length_all_bytes = prefix_length_full_bytes | |
| if prefix_length_rest_bits > 0: | |
| prefix_length_all_bytes += 1 | |
| # Case in which some bytes overlap | |
| if (prefix_length_all_bytes + len(address_bytes)) > required_bytes: | |
| ############################################################################################### | |
| # Example: | |
| # | |
| # Total address length: 128 bits | |
| # * prefix length: 68 bits | |
| # * address length: 64 bits | |
| # | |
| # overlap: 4 bits ==> the last 4 bits of the address must be replaced by the last 4 bits of prefix | |
| # | |
| # Result: | |
| # +--------------------+---------------------+ | |
| # | prefix (68 bits) | address (64 bits) | | |
| # +--------------------+---------------------+ | |
| ############################################################################################### | |
| src_addr = prefix[:prefix_length_full_bytes] | |
| required_bytes -= prefix_length_full_bytes | |
| if prefix_length_rest_bits > 0: | |
| prefix_overlapping_byte = prefix[prefix_length_all_bytes - 1] | |
| address_overlapping_byte = address_bytes[-required_bytes] | |
| overlapping_byte = prefix_overlapping_byte & ~(0xff >> prefix_length_rest_bits) | |
| overlapping_byte |= address_overlapping_byte & (0xff >> prefix_length_rest_bits) | |
| src_addr += bytearray([overlapping_byte]) | |
| required_bytes -= 1 | |
| if required_bytes > 0: | |
| src_addr += address_bytes[-required_bytes:] | |
| else: | |
| required_bytes -= prefix_length_all_bytes | |
| required_bytes -= len(address_bytes) | |
| src_addr = prefix[:prefix_length_all_bytes] + bytearray([0x00] * required_bytes) + address_bytes | |
| return src_addr | |
| def _decompress_src_addr_stateful(self, iphc, src_mac_addr, sci, data): | |
| if iphc.sam == self.IPHC_SAM_UNSPECIFIED: | |
| return bytearray([0x00] * 16) | |
| elif iphc.sam == self.IPHC_SAM_64B: | |
| context = self._context_manager[sci] | |
| return self._merge_prefix_with_address(prefix=context.prefix, | |
| prefix_length=context.prefix_length, | |
| address_bytes=bytearray(data.read(8))) | |
| elif iphc.sam == self.IPHC_SAM_16B: | |
| context = self._context_manager[sci] | |
| address_bytes = self.SHORT_ADDR_PADDING_BYTES + bytearray(data.read(2)) | |
| return self._merge_prefix_with_address(prefix=context.prefix, | |
| prefix_length=context.prefix_length, | |
| address_bytes=address_bytes) | |
| elif iphc.sam == self.IPHC_SAM_0B: | |
| context = self._context_manager[sci] | |
| return self._merge_prefix_with_address(prefix=context.prefix, | |
| prefix_length=context.prefix_length, | |
| address_bytes=src_mac_addr.convert_to_iid()) | |
| def _decompress_src_addr(self, iphc, src_mac_addr, sci, data): | |
| if iphc.sac == self.IPHC_SAC_STATELESS: | |
| return self._decompress_src_addr_stateless(iphc, src_mac_addr, data) | |
| elif iphc.sac == self.IPHC_SAC_STATEFUL: | |
| return self._decompress_src_addr_stateful(iphc, src_mac_addr, sci, data) | |
| def _decompress_unicast_dst_addr_stateless(self, iphc, dst_mac_addr, data): | |
| if iphc.dam == self.IPHC_DAM_128B: | |
| return bytearray(data.read(16)) | |
| elif iphc.dam == self.IPHC_DAM_64B: | |
| return self.IPV6_LINKLOCAL_PREFIX + bytearray(data.read(8)) | |
| elif iphc.dam == self.IPHC_DAM_16B: | |
| return self.IPV6_LINKLOCAL_PREFIX + self.SHORT_ADDR_PADDING_BYTES + bytearray(data.read(2)) | |
| elif iphc.dam == self.IPHC_DAM_ELIDED: | |
| return self.IPV6_LINKLOCAL_PREFIX + dst_mac_addr.convert_to_iid() | |
| def _decompress_unicast_dst_addr_stateful(self, iphc, dst_mac_addr, dci, data): | |
| if iphc.dam == self.IPHC_DAM_128B: | |
| raise RuntimeError("Reserved") | |
| elif iphc.dam == self.IPHC_DAM_64B: | |
| context = self._context_manager[dci] | |
| return self._merge_prefix_with_address(prefix=context.prefix, | |
| prefix_length=context.prefix_length, | |
| address_bytes=bytearray(data.read(8))) | |
| elif iphc.dam == self.IPHC_DAM_16B: | |
| context = self._context_manager[dci] | |
| address_bytes = self.SHORT_ADDR_PADDING_BYTES + bytearray(data.read(2)) | |
| return self._merge_prefix_with_address(prefix=context.prefix, | |
| prefix_length=context.prefix_length, | |
| address_bytes=address_bytes) | |
| elif iphc.dam == self.IPHC_DAM_0B: | |
| context = self._context_manager[dci] | |
| return self._merge_prefix_with_address(prefix=context.prefix, | |
| prefix_length=context.prefix_length, | |
| address_bytes=dst_mac_addr.convert_to_iid()) | |
| def _decompress_unicast_dst_addr(self, iphc, dst_mac_addr, dci, data): | |
| if iphc.dac == self.IPHC_DAC_STATELESS: | |
| return self._decompress_unicast_dst_addr_stateless(iphc, dst_mac_addr, data) | |
| elif iphc.dac == self.IPHC_DAC_STATEFUL: | |
| return self._decompress_unicast_dst_addr_stateful(iphc, dst_mac_addr, dci, data) | |
| def _decompress_multicast_dst_addr_stateless(self, iphc, data): | |
| if iphc.dam == self.IPHC_DAM_128B: | |
| return bytearray(data.read(16)) | |
| elif iphc.dam == self.IPHC_DAM_48B: | |
| addr48b = bytearray(data.read(6)) | |
| return bytearray([0xff, addr48b[0]]) + bytearray([0x00] * 9) + addr48b[1:] | |
| elif iphc.dam == self.IPHC_DAM_32B: | |
| addr32b = bytearray(data.read(4)) | |
| return bytearray([0xFF, addr32b[0]]) + bytearray([0x00] * 11) + addr32b[1:] | |
| elif iphc.dam == self.IPHC_DAM_8B: | |
| return bytearray([0xFF, 0x02]) + bytearray([0x00] * 13) + data.read(1) | |
| def _decompress_multicast_dst_addr_stateful(self, iphc, dci, data): | |
| if iphc.dam == self.IPHC_M_DAM_00: | |
| context = self._context_manager[dci] | |
| addr48b = bytearray(data.read(6)) | |
| p_bytes_count = 8 | |
| prefix = context.prefix[:p_bytes_count] | |
| prefix_length = context.prefix_length | |
| missing_bytes = p_bytes_count - len(prefix) | |
| if missing_bytes > 0: | |
| prefix += bytearray([0x00] * missing_bytes) | |
| return bytearray([0xff]) + addr48b[:2] + bytearray([prefix_length]) + prefix + addr48b[2:] | |
| elif iphc.dam == self.IPHC_M_DAM_01: | |
| raise RuntimeError("Reserved") | |
| elif iphc.dam == self.IPHC_M_DAM_10: | |
| raise RuntimeError("Reserved") | |
| elif iphc.dam == self.IPHC_M_DAM_11: | |
| raise RuntimeError("Reserved") | |
| def _decompress_multicast_dst_addr(self, iphc, dci, data): | |
| if iphc.dac == self.IPHC_DAC_STATELESS: | |
| return self._decompress_multicast_dst_addr_stateless(iphc, data) | |
| elif iphc.dac == self.IPHC_DAC_STATEFUL: | |
| return self._decompress_multicast_dst_addr_stateful(iphc, dci, data) | |
| def _decompress_dst_addr(self, iphc, dst_mac_addr, dci, data): | |
| if iphc.m == self.IPHC_M_NO: | |
| return self._decompress_unicast_dst_addr(iphc, dst_mac_addr, dci, data) | |
| elif iphc.m == self.IPHC_M_YES: | |
| return self._decompress_multicast_dst_addr(iphc, dci, data) | |
| def parse(self, data, message_info): | |
| iphc = LowpanIPHC.from_bytes(bytearray(data.read(2))) | |
| sci, dci = self._decompress_cid(iphc, data) | |
| traffic_class, flow_label = self._decompress_tf(iphc, data) | |
| next_header = self._decompress_nh(iphc, data) | |
| hop_limit = self._decompress_hlim(iphc, data) | |
| src_address = self._decompress_src_addr(iphc, message_info.source_mac_address, sci, data) | |
| dst_address = self._decompress_dst_addr(iphc, message_info.destination_mac_address, dci, data) | |
| header = ipv6.IPv6Header(src_address, dst_address, traffic_class, flow_label, hop_limit) | |
| header.next_header = next_header | |
| return header | |
| class LowpanDecompressor: | |
| """ Class decompressing 6LoWPAN packets. """ | |
| def __init__(self, lowpan_ip_header_factory, lowpan_extension_headers_factory, lowpan_udp_header_factory): | |
| self._lowpan_ip_header_factory = lowpan_ip_header_factory | |
| self._lowpan_extension_headers_factory = lowpan_extension_headers_factory | |
| self._lowpan_udp_header_factory = lowpan_udp_header_factory | |
| def _is_ipv6_extension_header(self, header_first_byte): | |
| return ((header_first_byte >> 4) & 0x0f) == 0x0e | |
| def _is_udp_header(self, header_first_byte): | |
| return ((header_first_byte >> 4) & 0x0f) == 0x0f | |
| def _peek_n_bytes(self, data, n): | |
| read_data = data.read(n) | |
| data.seek(-n, io.SEEK_CUR) | |
| return read_data | |
| def _is_next_header_compressed(self, header): | |
| return (header.next_header is None) | |
| def decompress(self, data, message_info): | |
| ipv6_header = self._lowpan_ip_header_factory.parse(data, message_info) | |
| previous_header = ipv6_header | |
| extension_headers = [] | |
| udp_header = None | |
| if self._is_next_header_compressed(ipv6_header): | |
| while data.tell() < len(data.getvalue()): | |
| header_first_byte = ord(self._peek_n_bytes(data, 1)) | |
| if self._is_ipv6_extension_header(header_first_byte): | |
| extension_header = self._lowpan_extension_headers_factory.parse(data, message_info) | |
| extension_headers.append(extension_header) | |
| # Update next header field in the previous header | |
| previous_header.next_header = extension_header.type | |
| previous_header = extension_header | |
| if not self._is_next_header_compressed(extension_header): | |
| # There is not more compressed headers | |
| break | |
| elif self._is_udp_header(header_first_byte): | |
| udp_header = self._lowpan_udp_header_factory.parse(data, message_info) | |
| # Update next header field in the previous header | |
| previous_header.next_header = udp_header.type | |
| # There is not more headers after UDP header | |
| break | |
| return ipv6_header, extension_headers, udp_header | |
| class LowpanMeshHeader(object): | |
| """ Class representing 6LoWPAN mesh header (RFC 4944 5.2). """ | |
| def __init__(self, hops_left, originator_address, final_destination_address): | |
| self._hops_left = hops_left | |
| self._originator_address = originator_address | |
| self._final_destination_address = final_destination_address | |
| @property | |
| def hops_left(self): | |
| return self._hops_left | |
| @property | |
| def originator_address(self): | |
| return self._originator_address | |
| @property | |
| def final_destination_address(self): | |
| return self._final_destination_address | |
| class LowpanMeshHeaderFactory: | |
| def _parse_address(self, data, is_short): | |
| if is_short: | |
| return common.MacAddress.from_rloc16(bytearray(data.read(2))) | |
| else: | |
| return common.MacAddress.from_eui64(bytearray(data.read(8))) | |
| def parse(self, data, message_info): | |
| data_byte = ord(data.read(1)) | |
| is_short_originator_address = bool(data_byte & 0x20) | |
| is_short_final_destination_address = bool(data_byte & 0x10) | |
| if (data_byte & 0x0f) != 0x0f: | |
| hops_left = (data_byte & 0x0f) | |
| else: | |
| hops_left = ord(data.read(1)) | |
| originator_address = self._parse_address(data, is_short_originator_address) | |
| final_destination_address = self._parse_address(data, is_short_final_destination_address) | |
| return LowpanMeshHeader(hops_left, originator_address, final_destination_address) | |
| class LowpanFragmentationHeader(object): | |
| def __init__(self, datagram_size, datagram_tag, datagram_offset=0): | |
| self._datagram_size = datagram_size | |
| self._datagram_tag = datagram_tag | |
| self._datagram_offset = datagram_offset | |
| @property | |
| def datagram_size(self): | |
| return self._datagram_size | |
| @property | |
| def datagram_tag(self): | |
| return self._datagram_tag | |
| @property | |
| def datagram_offset(self): | |
| return self._datagram_offset | |
| @property | |
| def is_first(self): | |
| return self.datagram_offset == 0 | |
| @classmethod | |
| def from_bytes(cls, data): | |
| datagram_size = struct.unpack(">H", data.read(2))[0] | |
| has_offset = ((datagram_size >> 11) & 0x1f) == 0x1c | |
| datagram_size &= 0x7ff | |
| datagram_tag = struct.unpack(">H", data.read(2))[0] | |
| datagram_offset = 0 | |
| if has_offset: | |
| datagram_offset = ord(data.read(1)) | |
| return cls(datagram_size, datagram_tag, datagram_offset) | |
| class LowpanFragmentsBuffer(object): | |
| def __init__(self, buffer_size): | |
| self._buffer = [None] * buffer_size | |
| self._position = 0 | |
| def write(self, data): | |
| if (self._position + len(data)) > len(self._buffer): | |
| raise ValueError("Write failure. Data length is bigger than the destination buffer length.") | |
| for i, byte in enumerate(data): | |
| self._buffer[self._position + i] = byte | |
| self._position += len(data) | |
| return len(data) | |
| def seek(self, offset): | |
| if offset >= len(self._buffer): | |
| raise ValueError("Could not seek current offset. Offset value is bigger than the buffer length.") | |
| self._position = offset | |
| def tell(self): | |
| return self._position | |
| def whole_packet_received(self): | |
| return all([byte is not None for byte in self._buffer]) | |
| def read(self): | |
| if not self.whole_packet_received(): | |
| raise ValueError("Only a part of the packet has been stored in the buffer.") | |
| return bytearray(self._buffer) | |
| def __len__(self): | |
| return len(self._buffer) | |
| class LowpanFragmentsBuffersManager(object): | |
| def __init__(self): | |
| self._fragments_buffers = {} | |
| def _create_key(self, message_info, datagram_tag): | |
| key = bytes(message_info.source_mac_address.mac_address) +\ | |
| bytes(message_info.destination_mac_address.mac_address) +\ | |
| bytes(datagram_tag) | |
| return key | |
| def _allocate_fragments_buffer(self, key, datagram_size): | |
| if datagram_size is None or datagram_size < 0: | |
| raise ValueError("Could not allocate fragments buffer. Invalid datagram size: {}".format(datagram_size)) | |
| fragments_buffer = LowpanFragmentsBuffer(datagram_size) | |
| self._fragments_buffers[key] = fragments_buffer | |
| return fragments_buffer | |
| def get_fragments_buffer(self, message_info, datagram_tag, datagram_size=None): | |
| key = self._create_key(message_info, datagram_tag) | |
| if not key in self._fragments_buffers: | |
| self._allocate_fragments_buffer(key, datagram_size) | |
| return self._fragments_buffers[key] | |
| def free_fragments_buffer(self, message_info, datagram_tag): | |
| key = self._create_key(message_info, datagram_tag) | |
| del self._fragments_buffers[key] | |
| class LowpanParser(object): | |
| def __init__(self, lowpan_mesh_header_factory, lowpan_decompressor, lowpan_fragements_buffers_manager, ipv6_packet_factory): | |
| self._lowpan_mesh_header_factory = lowpan_mesh_header_factory | |
| self._lowpan_decompressor = lowpan_decompressor | |
| self._lowpan_fragments_buffers_manager = lowpan_fragements_buffers_manager | |
| self._ipv6_packet_factory = ipv6_packet_factory | |
| def _peek_n_bytes(self, data, n): | |
| data_bytes = data.read(n) | |
| data.seek(-n, io.SEEK_CUR) | |
| return data_bytes | |
| def _is_mesh_header(self, first_byte): | |
| return (((first_byte >> 6) & 0x03) == 0x02) | |
| def _is_first_fragmentation_header(self, first_byte): | |
| return (((first_byte >> 3) & 0x1f) == 0x18) | |
| def _is_subsequent_fragmentation_header(self, first_byte): | |
| return (((first_byte >> 3) & 0x1f) == 0x1c) | |
| def _is_iphc(self, first_byte): | |
| return (((first_byte >> 5) & 0x07) == 0x03) | |
| def _decompress_iphc(self, data, message_info): | |
| return self._lowpan_decompressor.decompress(data, message_info) | |
| def _handle_first_fragmentation_header(self, data, message_info): | |
| fragmentation_header = LowpanFragmentationHeader.from_bytes(data) | |
| fragments_buffer = self._lowpan_fragments_buffers_manager.get_fragments_buffer( | |
| message_info, fragmentation_header.datagram_tag, fragmentation_header.datagram_size) | |
| ipv6_header, extension_headers, udp_header = self._decompress_iphc(data, message_info) | |
| uncompressed_data = data.read() | |
| # Update payload lengths | |
| ipv6_header.payload_length = fragmentation_header.datagram_size - len(ipv6_header) | |
| fragments_buffer.seek(0) | |
| fragments_buffer.write(ipv6_header.to_bytes()) | |
| for extension_header in extension_headers: | |
| fragments_buffer.write(extension_header.to_bytes()) | |
| if udp_header is not None: | |
| fragments_buffer.write(udp_header.to_bytes()) | |
| fragments_buffer.write(uncompressed_data) | |
| if fragments_buffer.whole_packet_received(): | |
| data = io.BytesIO(fragments_buffer.read()) | |
| self._lowpan_fragments_buffers_manager.free_fragments_buffer(message_info, | |
| fragmentation_header.datagram_tag) | |
| return self._ipv6_packet_factory.parse(data, message_info) | |
| return None | |
| def _handle_subsequent_fragmentation_header(self, data, message_info): | |
| fragmentation_header = LowpanFragmentationHeader.from_bytes(data) | |
| fragments_buffer = self._lowpan_fragments_buffers_manager.get_fragments_buffer( | |
| message_info, fragmentation_header.datagram_tag, fragmentation_header.datagram_size) | |
| offset = (fragmentation_header.datagram_offset * 8) | |
| fragments_buffer.seek(offset) | |
| fragments_buffer.write(data.read()) | |
| if fragments_buffer.whole_packet_received(): | |
| data = io.BytesIO(fragments_buffer.read()) | |
| self._lowpan_fragments_buffers_manager.free_fragments_buffer(message_info, | |
| fragmentation_header.datagram_tag) | |
| return self._ipv6_packet_factory.parse(data, message_info) | |
| return None | |
| def _handle_iphc_header(self, data, message_info): | |
| ipv6_header, extension_headers, udp_header = self._decompress_iphc(data, message_info) | |
| uncompressed_data = data.read() | |
| decompressed_data = bytearray([]) | |
| for extension_header in extension_headers: | |
| decompressed_data += extension_header.to_bytes() | |
| if udp_header is not None: | |
| udp_header.payload_length = len(uncompressed_data) | |
| decompressed_data += udp_header.to_bytes() | |
| decompressed_data += uncompressed_data | |
| ipv6_header.payload_length = len(decompressed_data) | |
| decompressed_data = ipv6_header.to_bytes() + decompressed_data | |
| return self._ipv6_packet_factory.parse(io.BytesIO(decompressed_data), message_info) | |
| def parse(self, data, message_info): | |
| while data.tell() < len(data.getvalue()): | |
| first_byte = ord(self._peek_n_bytes(data, n=1)) | |
| if self._is_mesh_header(first_byte): | |
| mesh_header = self._lowpan_mesh_header_factory.parse(data, message_info) | |
| message_info.source_mac_address = mesh_header.originator_address | |
| message_info.destination_mac_address = mesh_header.final_destination_address | |
| elif self._is_first_fragmentation_header(first_byte): | |
| return self._handle_first_fragmentation_header(data, message_info) | |
| elif self._is_subsequent_fragmentation_header(first_byte): | |
| return self._handle_subsequent_fragmentation_header(data, message_info) | |
| elif self._is_iphc(first_byte): | |
| return self._handle_iphc_header(data, message_info) | |
| else: | |
| raise RuntimeError("Unsupported header type: 0x{:02x}".format(first_byte)) | |