1#!/usr/bin/env python3 2# 3# Copyright (c) 2016, The OpenThread Authors. 4# All rights reserved. 5# 6# Redistribution and use in source and binary forms, with or without 7# modification, are permitted provided that the following conditions are met: 8# 1. Redistributions of source code must retain the above copyright 9# notice, this list of conditions and the following disclaimer. 10# 2. Redistributions in binary form must reproduce the above copyright 11# notice, this list of conditions and the following disclaimer in the 12# documentation and/or other materials provided with the distribution. 13# 3. Neither the name of the copyright holder nor the 14# names of its contributors may be used to endorse or promote products 15# derived from this software without specific prior written permission. 16# 17# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27# POSSIBILITY OF SUCH DAMAGE. 28# 29 30import io 31import ipaddress 32import struct 33 34import common 35import ipv6 36 37 38class LowpanIPHC: 39 """ 40 Class representing a compressed IP header. 41 42 More details: 43 - URL: https://tools.ietf.org/html/rfc6282 44 - section: 3.1. LOWPAN_IPHC Encoding Format 45 """ 46 47 def __init__(self, tf, nh, hlim, cid, sac, sam, m, dac, dam): 48 self._tf = tf 49 self._nh = nh 50 self._hlim = hlim 51 self._cid = cid 52 self._sac = sac 53 self._sam = sam 54 self._m = m 55 self._dac = dac 56 self._dam = dam 57 58 @property 59 def tf(self): 60 return self._tf 61 62 @property 63 def nh(self): 64 return self._nh 65 66 @property 67 def hlim(self): 68 return self._hlim 69 70 @property 71 def cid(self): 72 return self._cid 73 74 @property 75 def sac(self): 76 return self._sac 77 78 @property 79 def sam(self): 80 return self._sam 81 82 @property 83 def m(self): 84 return self._m 85 86 @property 87 def dac(self): 88 return self._dac 89 90 @property 91 def dam(self): 92 return self._dam 93 94 @classmethod 95 def from_bytes(cls, data_bytes): 96 data_byte = data_bytes[0] 97 98 hdr = (data_byte >> 5) & 0x07 99 if hdr != 0x03: 100 raise RuntimeError("Not a 6LowPAN packet.") 101 102 tf = (data_byte >> 3) & 0x03 103 nh = (data_byte >> 2) & 0x01 104 hlim = data_byte & 0x03 105 106 data_byte = data_bytes[1] 107 108 cid = (data_byte >> 7) & 0x01 109 sac = (data_byte >> 6) & 0x01 110 sam = (data_byte >> 4) & 0x03 111 m = (data_byte >> 3) & 0x01 112 dac = (data_byte >> 2) & 0x01 113 dam = data_byte & 0x03 114 115 return cls(tf, nh, hlim, cid, sac, sam, m, dac, dam) 116 117 def __repr__(self): 118 return "LowpanIPHC(tf={}, nh={}, hlim={}, cid={}, sac={}, sam={}, m={}, dac={}, dam={})".format( 119 self.tf, 120 self.nh, 121 self.hlim, 122 self.cid, 123 self.sac, 124 self.sam, 125 self.m, 126 self.dac, 127 self.dam, 128 ) 129 130 131class LowpanNHC: 132 """ 133 Class representing a compressed extension header. 134 135 More details: 136 - URL: https://tools.ietf.org/html/rfc6282 137 - section: 4.1. LOWPAN_NHC Format 138 139 """ 140 141 NHC_EID_HOP_BY_HOP = 0 142 NHC_EID_ROUTING = 1 143 NHC_EID_FRAGMENT = 2 144 NHC_EID_DST_OPT = 3 145 NHC_EID_MOBILITY = 4 146 NHC_EID_IPV6_HDR = 7 147 148 def __init__(self, eid, nh): 149 self._eid = eid 150 self._nh = nh 151 152 @property 153 def eid(self): 154 return self._eid 155 156 @property 157 def nh(self): 158 return self._nh 159 160 @classmethod 161 def from_bytes(cls, data_bytes): 162 header_byte = data_bytes[0] 163 164 eid = (header_byte >> 1) & 0x07 165 nh = header_byte & 0x01 166 167 return cls(eid, nh) 168 169 def __repr__(self): 170 return "LowpanNHC(eid={}, nh={})".format(self.eid, self.nh) 171 172 173class LowpanUDPHC: 174 """ 175 Class representing compressed UDP header. 176 177 More details: 178 - URL: https://tools.ietf.org/html/rfc6282 179 - section: 4.3.3. UDP LOWPAN_NHC Format 180 181 """ 182 183 def __init__(self, c, p): 184 self._c = c 185 self._p = p 186 187 @property 188 def c(self): 189 return self._c 190 191 @property 192 def p(self): 193 return self._p 194 195 @classmethod 196 def from_bytes(cls, data_bytes): 197 data_byte = data_bytes[0] 198 199 hdr = (data_byte >> 3) & 0x1F 200 if hdr != 0x1E: 201 raise RuntimeError("Not a 6LowPAN UDP header.") 202 203 c = (data_byte >> 2) & 0x01 204 p = data_byte & 0x03 205 206 return cls(c, p) 207 208 def __repr__(self): 209 return "LowpanUDPHC(c={}, p={})".format(self.c, self.p) 210 211 212class LowpanHopByHopFactory: 213 """ Factory that produces HopByHop extension header. """ 214 215 def __init__(self, hop_by_hop_options_factory): 216 self._hop_by_hop_options_factory = hop_by_hop_options_factory 217 218 def parse(self, data, next_header, message_info): 219 ext_header_length = ord(data.read(1)) 220 221 ext_header_data = data.read(ext_header_length) 222 223 options = self._hop_by_hop_options_factory.parse(io.BytesIO(ext_header_data), message_info) 224 225 ext_header = ipv6.HopByHop(next_header, options) 226 227 message_info.payload_length += len(ext_header) 228 229 return ext_header 230 231 232class LowpanExtensionHeadersFactory: 233 """ Factory that produces extension headers. """ 234 235 NHC_NH_INLINE = 0 236 NHC_NH_COMPRESSED = 1 237 238 def __init__(self, ext_headers_factories): 239 self._ext_headers_factories = (ext_headers_factories if ext_headers_factories is not None else {}) 240 241 def _decompress_nh(self, hc, data): 242 if hc.nh == self.NHC_NH_INLINE: 243 return ord(data.read(1)) 244 245 elif hc.nh == self.NHC_NH_COMPRESSED: 246 return None 247 248 def _get_ext_headers_factory(self, eid): 249 try: 250 return self._ext_headers_factories[eid] 251 252 except BaseException: 253 raise RuntimeError("Could not find an extension header factory for the EID type: {}".format(eid)) 254 255 def parse(self, data, message_info): 256 nhc = LowpanNHC.from_bytes(bytearray(data.read(1))) 257 258 next_header = self._decompress_nh(nhc, data) 259 260 factory = self._get_ext_headers_factory(nhc.eid) 261 262 return factory.parse(data, next_header, message_info) 263 264 265class LowpanUdpHeaderFactory: 266 """ Factory producing UDP header. """ 267 268 UDP_HC_C_INLINE = 0 269 UDP_HC_C_ELIDED = 1 270 271 UDP_HC_P_BOTH_FULL = 0 272 UDP_HC_P_DST_COMPR = 1 273 UDP_HC_P_SRC_COMPR = 2 274 UDP_HC_P_BOTH_COMPR = 3 275 276 def _decompress_udp_ports(self, udphc, data): 277 if udphc.p == self.UDP_HC_P_BOTH_FULL: 278 src_port = struct.unpack(">H", data.read(2))[0] 279 dst_port = struct.unpack(">H", data.read(2))[0] 280 281 elif udphc.p == self.UDP_HC_P_DST_COMPR: 282 src_port = struct.unpack(">H", data.read(2))[0] 283 dst_port = 0xf000 + ord(data.read(1)) 284 285 elif udphc.p == self.UDP_HC_P_SRC_COMPR: 286 src_port = 0xf000 + ord(data.read(1)) 287 dst_port = struct.unpack(">H", data.read(2))[0] 288 289 elif udphc.p == self.UDP_HC_P_BOTH_COMPR: 290 udp_ports_byte = ord(data.read(1)) 291 src_port = 0xf0b0 + ((udp_ports_byte >> 4) & 0x0F) 292 dst_port = 0xf0b0 + (udp_ports_byte & 0x0F) 293 294 return src_port, dst_port 295 296 def _decompress_udp_checksum(self, udphc, data): 297 if udphc.c == self.UDP_HC_C_INLINE: 298 checksum = struct.unpack(">H", data.read(2))[0] 299 300 if udphc.c == self.UDP_HC_C_ELIDED: 301 checksum = 0 302 303 return checksum 304 305 def parse(self, data, message_info): 306 udphc = LowpanUDPHC.from_bytes(bytearray(data.read(1))) 307 308 src_port, dst_port = self._decompress_udp_ports(udphc, data) 309 310 checksum = self._decompress_udp_checksum(udphc, data) 311 312 header = ipv6.UDPHeader(src_port, dst_port, checksum=checksum) 313 314 return header 315 316 317class Context: 318 319 def __init__(self, prefix, prefix_length=None): 320 if isinstance(prefix, str): 321 prefix, prefix_length = prefix.split("/") 322 prefix_length = int(prefix_length) 323 324 a = ipaddress.ip_address(prefix) 325 326 self._prefix = bytearray(a.packed) 327 self._prefix_length = prefix_length 328 329 elif isinstance(prefix, bytearray): 330 self._prefix = prefix 331 self._prefix_length = (prefix_length if prefix_length is not None else len(self._prefix) * 8) 332 333 @property 334 def prefix(self): 335 return self._prefix[:self.prefix_length_all_bytes] 336 337 @property 338 def prefix_full_bytes(self): 339 return self._prefix[:self.prefix_length_full_bytes] 340 341 @property 342 def prefix_length(self): 343 return self._prefix_length 344 345 @property 346 def prefix_length_full_bytes(self): 347 return int(self._prefix_length / 8) 348 349 @property 350 def prefix_length_rest_bits(self): 351 return int(self._prefix_length % 8) 352 353 @property 354 def prefix_length_all_bytes(self): 355 if self.prefix_length_rest_bits > 0: 356 return self.prefix_length_full_bytes + 1 357 358 return self.prefix_length_full_bytes 359 360 361class ContextManager(dict): 362 """ Class representing Context Manager. """ 363 364 def __check_index(self, index): 365 if index < 0 or index > 15: 366 raise IndexError("Invalid index: {}. Valid index is in range [0, 15]".format(index)) 367 368 def __check_type(self, value): 369 if not isinstance(value, Context): 370 raise TypeError("Invalid value type: {}".format(type(value))) 371 372 def __getitem__(self, index): 373 self.__check_index(index) 374 375 return super(ContextManager, self).__getitem__(index) 376 377 def __setitem__(self, index, value): 378 self.__check_index(index) 379 self.__check_type(value) 380 381 return super(ContextManager, self).__setitem__(index, value) 382 383 384class LowpanIpv6HeaderFactory: 385 """ Factory that produces IPv6 header. """ 386 387 IPV6_LINKLOCAL_PREFIX = bytearray([0xfe, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]) 388 389 SHORT_ADDR_PADDING_BYTES = bytearray([0x00, 0x00, 0x00, 0xff, 0xfe, 0x00]) 390 391 IPHC_TF_4B = 0 392 IPHC_TF_3B = 1 393 IPHC_TF_1B = 2 394 IPHC_TF_ELIDED = 3 395 396 IPHC_NH_INLINE = 0 397 IPHC_NH_COMPRESSED = 1 398 399 IPHC_HLIM_CALCULATE = -1 400 IPHC_HLIM_INLINE = 0 401 IPHC_HLIM_1 = 1 402 IPHC_HLIM_64 = 2 403 IPHC_HLIM_255 = 3 404 405 IPHC_CID_CLEAR = 0 406 IPHC_CID_SET = 1 407 408 IPHC_SAC_STATELESS = 0 409 IPHC_SAC_STATEFUL = 1 410 411 IPHC_SAM_128B = 0 412 IPHC_SAM_UNSPECIFIED = 0 413 IPHC_SAM_64B = 1 414 IPHC_SAM_16B = 2 415 IPHC_SAM_0B = 3 416 IPHC_SAM_ELIDED = 3 417 418 IPHC_M_NO = 0 419 IPHC_M_YES = 1 420 421 IPHC_DAC_STATELESS = 0 422 IPHC_DAC_STATEFUL = 1 423 424 IPHC_DAM_128B = 0 425 IPHC_DAM_64B = 1 426 IPHC_DAM_48B = 1 427 IPHC_DAM_32B = 2 428 IPHC_DAM_16B = 2 429 IPHC_DAM_8B = 3 430 IPHC_DAM_0B = 3 431 IPHC_DAM_ELIDED = 3 432 433 IPHC_M_DAM_00 = 0 434 IPHC_M_DAM_01 = 1 435 IPHC_M_DAM_10 = 2 436 IPHC_M_DAM_11 = 3 437 438 def __init__(self, context_manager=None): 439 self._context_manager = context_manager 440 441 def _flow_label(self, data_bytes): 442 flow_label = (data_bytes[0] & 0x0F) << 16 443 flow_label += data_bytes[1] << 8 444 flow_label += data_bytes[2] 445 return flow_label 446 447 def _traffic_class(self, dscp, ecn): 448 return (dscp << 2) | ecn 449 450 def _unpack_dscp(self, data_byte): 451 return data_byte & 0x3F 452 453 def _unpack_ecn(self, data_byte): 454 return data_byte >> 6 455 456 def _decompress_tf_4bytes(self, data): 457 data_bytes = [b for b in bytearray(data.read(4))] 458 459 dscp = self._unpack_dscp(data_bytes[0]) 460 ecn = self._unpack_ecn(data_bytes[0]) 461 462 traffic_class = self._traffic_class(dscp, ecn) 463 flow_label = self._flow_label(data_bytes[1:]) 464 465 return traffic_class, flow_label 466 467 def _decompress_tf_3bytes(self, data): 468 data_bytes = [b for b in bytearray(data.read(3))] 469 470 ecn = self._unpack_ecn(data_bytes[0]) 471 472 traffic_class = self._traffic_class(dscp=0, ecn=ecn) 473 flow_label = self._flow_label(data_bytes) 474 475 return traffic_class, flow_label 476 477 def _decompress_tf_1byte(self, data): 478 data_byte = ord(data.read(1)) 479 480 dscp = self._unpack_dscp(data_byte) 481 ecn = self._unpack_ecn(data_byte) 482 483 traffic_class = self._traffic_class(dscp, ecn) 484 flow_label = 0 485 486 return traffic_class, flow_label 487 488 def _decompress_tf(self, iphc, data): 489 if iphc.tf == self.IPHC_TF_4B: 490 return self._decompress_tf_4bytes(data) 491 492 elif iphc.tf == self.IPHC_TF_3B: 493 return self._decompress_tf_3bytes(data) 494 495 elif iphc.tf == self.IPHC_TF_1B: 496 return self._decompress_tf_1byte(data) 497 498 elif iphc.tf == self.IPHC_TF_ELIDED: 499 return 0, 0 500 501 def _decompress_nh(self, hc, data): 502 if hc.nh == self.IPHC_NH_INLINE: 503 return ord(data.read(1)) 504 505 elif hc.nh == self.IPHC_NH_COMPRESSED: 506 return None 507 508 def _decompress_hlim(self, iphc, data): 509 if iphc.hlim == self.IPHC_HLIM_INLINE: 510 return ord(data.read(1)) 511 512 elif iphc.hlim == self.IPHC_HLIM_1: 513 return 1 514 515 elif iphc.hlim == self.IPHC_HLIM_64: 516 return 64 517 518 elif iphc.hlim == self.IPHC_HLIM_255: 519 return 255 520 521 def _decompress_cid(self, iphc, data): 522 if iphc.cid == self.IPHC_CID_SET: 523 cid = ord(data.read(1)) 524 525 sci = (cid >> 4) & 0x0F 526 dci = cid & 0x0F 527 528 return sci, dci 529 530 elif iphc.cid == self.IPHC_CID_CLEAR: 531 return 0, 0 532 533 def _decompress_src_addr_stateless(self, iphc, src_mac_addr, data): 534 if iphc.sam == self.IPHC_SAM_128B: 535 return bytearray(data.read(16)) 536 537 elif iphc.sam == self.IPHC_SAM_64B: 538 return self.IPV6_LINKLOCAL_PREFIX + bytearray(data.read(8)) 539 540 elif iphc.sam == self.IPHC_SAM_16B: 541 return (self.IPV6_LINKLOCAL_PREFIX + self.SHORT_ADDR_PADDING_BYTES + bytearray(data.read(2))) 542 543 elif iphc.sam == self.IPHC_SAM_ELIDED: 544 return self.IPV6_LINKLOCAL_PREFIX + src_mac_addr.convert_to_iid() 545 546 def _merge_prefix_with_address(self, prefix, prefix_length, address_bytes): 547 required_bytes = 16 548 549 prefix_length_full_bytes = int(prefix_length / 8) 550 prefix_length_rest_bits = int(prefix_length % 8) 551 552 prefix_length_all_bytes = prefix_length_full_bytes 553 554 if prefix_length_rest_bits > 0: 555 prefix_length_all_bytes += 1 556 557 # Case in which some bytes overlap 558 if (prefix_length_all_bytes + len(address_bytes)) > required_bytes: 559 ################################################################### 560 # Example: 561 # 562 # Total address length: 128 bits 563 # * prefix length: 68 bits 564 # * address length: 64 bits 565 # 566 # overlap: 4 bits ==> the last 4 bits of the address must be replaced by the last 4 bits of prefix 567 # 568 # Result: 569 # +--------------------+---------------------+ 570 # | prefix (68 bits) | address (64 bits) | 571 # +--------------------+---------------------+ 572 ################################################################### 573 574 src_addr = prefix[:prefix_length_full_bytes] 575 required_bytes -= prefix_length_full_bytes 576 577 if prefix_length_rest_bits > 0: 578 prefix_overlapping_byte = prefix[prefix_length_all_bytes - 1] 579 address_overlapping_byte = address_bytes[-required_bytes] 580 581 overlapping_byte = prefix_overlapping_byte & ~(0xff >> prefix_length_rest_bits) 582 overlapping_byte |= address_overlapping_byte & (0xff >> prefix_length_rest_bits) 583 584 src_addr += bytearray([overlapping_byte]) 585 required_bytes -= 1 586 587 if required_bytes > 0: 588 src_addr += address_bytes[-required_bytes:] 589 590 else: 591 required_bytes -= prefix_length_all_bytes 592 required_bytes -= len(address_bytes) 593 594 src_addr = (prefix[:prefix_length_all_bytes] + bytearray([0x00] * required_bytes) + address_bytes) 595 596 return src_addr 597 598 def _decompress_src_addr_stateful(self, iphc, src_mac_addr, sci, data): 599 if iphc.sam == self.IPHC_SAM_UNSPECIFIED: 600 return bytearray([0x00] * 16) 601 602 elif iphc.sam == self.IPHC_SAM_64B: 603 context = self._context_manager[sci] 604 605 return self._merge_prefix_with_address( 606 prefix=context.prefix, 607 prefix_length=context.prefix_length, 608 address_bytes=bytearray(data.read(8)), 609 ) 610 611 elif iphc.sam == self.IPHC_SAM_16B: 612 context = self._context_manager[sci] 613 address_bytes = self.SHORT_ADDR_PADDING_BYTES + bytearray(data.read(2)) 614 615 return self._merge_prefix_with_address( 616 prefix=context.prefix, 617 prefix_length=context.prefix_length, 618 address_bytes=address_bytes, 619 ) 620 621 elif iphc.sam == self.IPHC_SAM_0B: 622 context = self._context_manager[sci] 623 624 return self._merge_prefix_with_address( 625 prefix=context.prefix, 626 prefix_length=context.prefix_length, 627 address_bytes=src_mac_addr.convert_to_iid(), 628 ) 629 630 def _decompress_src_addr(self, iphc, src_mac_addr, sci, data): 631 if iphc.sac == self.IPHC_SAC_STATELESS: 632 return self._decompress_src_addr_stateless(iphc, src_mac_addr, data) 633 634 elif iphc.sac == self.IPHC_SAC_STATEFUL: 635 return self._decompress_src_addr_stateful(iphc, src_mac_addr, sci, data) 636 637 def _decompress_unicast_dst_addr_stateless(self, iphc, dst_mac_addr, data): 638 if iphc.dam == self.IPHC_DAM_128B: 639 return bytearray(data.read(16)) 640 641 elif iphc.dam == self.IPHC_DAM_64B: 642 return self.IPV6_LINKLOCAL_PREFIX + bytearray(data.read(8)) 643 644 elif iphc.dam == self.IPHC_DAM_16B: 645 return (self.IPV6_LINKLOCAL_PREFIX + self.SHORT_ADDR_PADDING_BYTES + bytearray(data.read(2))) 646 647 elif iphc.dam == self.IPHC_DAM_ELIDED: 648 return self.IPV6_LINKLOCAL_PREFIX + dst_mac_addr.convert_to_iid() 649 650 def _decompress_unicast_dst_addr_stateful(self, iphc, dst_mac_addr, dci, data): 651 if iphc.dam == self.IPHC_DAM_128B: 652 raise RuntimeError("Reserved") 653 654 elif iphc.dam == self.IPHC_DAM_64B: 655 context = self._context_manager[dci] 656 657 return self._merge_prefix_with_address( 658 prefix=context.prefix, 659 prefix_length=context.prefix_length, 660 address_bytes=bytearray(data.read(8)), 661 ) 662 663 elif iphc.dam == self.IPHC_DAM_16B: 664 context = self._context_manager[dci] 665 address_bytes = self.SHORT_ADDR_PADDING_BYTES + bytearray(data.read(2)) 666 667 return self._merge_prefix_with_address( 668 prefix=context.prefix, 669 prefix_length=context.prefix_length, 670 address_bytes=address_bytes, 671 ) 672 673 elif iphc.dam == self.IPHC_DAM_0B: 674 context = self._context_manager[dci] 675 676 return self._merge_prefix_with_address( 677 prefix=context.prefix, 678 prefix_length=context.prefix_length, 679 address_bytes=dst_mac_addr.convert_to_iid(), 680 ) 681 682 def _decompress_unicast_dst_addr(self, iphc, dst_mac_addr, dci, data): 683 if iphc.dac == self.IPHC_DAC_STATELESS: 684 return self._decompress_unicast_dst_addr_stateless(iphc, dst_mac_addr, data) 685 686 elif iphc.dac == self.IPHC_DAC_STATEFUL: 687 return self._decompress_unicast_dst_addr_stateful(iphc, dst_mac_addr, dci, data) 688 689 def _decompress_multicast_dst_addr_stateless(self, iphc, data): 690 if iphc.dam == self.IPHC_DAM_128B: 691 return bytearray(data.read(16)) 692 693 elif iphc.dam == self.IPHC_DAM_48B: 694 addr48b = bytearray(data.read(6)) 695 return (bytearray([0xff, addr48b[0]]) + bytearray([0x00] * 9) + addr48b[1:]) 696 697 elif iphc.dam == self.IPHC_DAM_32B: 698 addr32b = bytearray(data.read(4)) 699 return (bytearray([0xff, addr32b[0]]) + bytearray([0x00] * 11) + addr32b[1:]) 700 701 elif iphc.dam == self.IPHC_DAM_8B: 702 return (bytearray([0xff, 0x02]) + bytearray([0x00] * 13) + data.read(1)) 703 704 def _decompress_multicast_dst_addr_stateful(self, iphc, dci, data): 705 if iphc.dam == self.IPHC_M_DAM_00: 706 context = self._context_manager[dci] 707 708 addr48b = bytearray(data.read(6)) 709 710 p_bytes_count = 8 711 712 prefix = context.prefix[:p_bytes_count] 713 prefix_length = context.prefix_length 714 715 missing_bytes = p_bytes_count - len(prefix) 716 717 if missing_bytes > 0: 718 prefix += bytearray([0x00] * missing_bytes) 719 720 return (bytearray([0xff]) + addr48b[:2] + bytearray([prefix_length]) + prefix + addr48b[2:]) 721 722 elif iphc.dam == self.IPHC_M_DAM_01: 723 raise RuntimeError("Reserved") 724 725 elif iphc.dam == self.IPHC_M_DAM_10: 726 raise RuntimeError("Reserved") 727 728 elif iphc.dam == self.IPHC_M_DAM_11: 729 raise RuntimeError("Reserved") 730 731 def _decompress_multicast_dst_addr(self, iphc, dci, data): 732 if iphc.dac == self.IPHC_DAC_STATELESS: 733 return self._decompress_multicast_dst_addr_stateless(iphc, data) 734 735 elif iphc.dac == self.IPHC_DAC_STATEFUL: 736 return self._decompress_multicast_dst_addr_stateful(iphc, dci, data) 737 738 def _decompress_dst_addr(self, iphc, dst_mac_addr, dci, data): 739 if iphc.m == self.IPHC_M_NO: 740 return self._decompress_unicast_dst_addr(iphc, dst_mac_addr, dci, data) 741 742 elif iphc.m == self.IPHC_M_YES: 743 return self._decompress_multicast_dst_addr(iphc, dci, data) 744 745 def set_lowpan_context(self, cid, prefix): 746 self._context_manager[cid] = Context(prefix) 747 748 def parse(self, data, message_info): 749 iphc = LowpanIPHC.from_bytes(bytearray(data.read(2))) 750 751 sci, dci = self._decompress_cid(iphc, data) 752 753 traffic_class, flow_label = self._decompress_tf(iphc, data) 754 755 next_header = self._decompress_nh(iphc, data) 756 757 hop_limit = self._decompress_hlim(iphc, data) 758 759 src_address = self._decompress_src_addr(iphc, message_info.source_mac_address, sci, data) 760 761 dst_address = self._decompress_dst_addr(iphc, message_info.destination_mac_address, dci, data) 762 763 header = ipv6.IPv6Header(src_address, dst_address, traffic_class, flow_label, hop_limit) 764 765 header.next_header = next_header 766 767 return header 768 769 770class LowpanDecompressor: 771 """ Class decompressing 6LoWPAN packets. """ 772 773 def __init__( 774 self, 775 lowpan_ip_header_factory, 776 lowpan_extension_headers_factory, 777 lowpan_udp_header_factory, 778 ): 779 self._lowpan_ip_header_factory = lowpan_ip_header_factory 780 self._lowpan_extension_headers_factory = (lowpan_extension_headers_factory) 781 self._lowpan_udp_header_factory = lowpan_udp_header_factory 782 783 def _is_ipv6_extension_header(self, header_first_byte): 784 return ((header_first_byte >> 4) & 0x0F) == 0x0E 785 786 def _is_udp_header(self, header_first_byte): 787 return ((header_first_byte >> 4) & 0x0F) == 0x0F 788 789 def _peek_n_bytes(self, data, n): 790 read_data = data.read(n) 791 data.seek(-n, io.SEEK_CUR) 792 return read_data 793 794 def _is_next_header_compressed(self, header): 795 return header.next_header is None 796 797 def set_lowpan_context(self, cid, prefix): 798 self._lowpan_ip_header_factory.set_lowpan_context(cid, prefix) 799 800 def decompress(self, data, message_info): 801 ipv6_header = self._lowpan_ip_header_factory.parse(data, message_info) 802 803 previous_header = ipv6_header 804 805 extension_headers = [] 806 udp_header = None 807 808 if self._is_next_header_compressed(ipv6_header): 809 810 while data.tell() < len(data.getvalue()): 811 header_first_byte = ord(self._peek_n_bytes(data, 1)) 812 813 if self._is_ipv6_extension_header(header_first_byte): 814 extension_header = self._lowpan_extension_headers_factory.parse(data, message_info) 815 extension_headers.append(extension_header) 816 817 # Update next header field in the previous header 818 previous_header.next_header = extension_header.type 819 previous_header = extension_header 820 821 if not self._is_next_header_compressed(extension_header): 822 # There is not more compressed headers 823 break 824 825 elif self._is_udp_header(header_first_byte): 826 udp_header = self._lowpan_udp_header_factory.parse(data, message_info) 827 828 # Update next header field in the previous header 829 previous_header.next_header = udp_header.type 830 831 # There is not more headers after UDP header 832 break 833 834 return ipv6_header, extension_headers, udp_header 835 836 837class LowpanMeshHeader(object): 838 """ Class representing 6LoWPAN mesh header (RFC 4944 5.2). """ 839 840 def __init__(self, hops_left, originator_address, final_destination_address): 841 self._hops_left = hops_left 842 self._originator_address = originator_address 843 self._final_destination_address = final_destination_address 844 845 @property 846 def hops_left(self): 847 return self._hops_left 848 849 @property 850 def originator_address(self): 851 return self._originator_address 852 853 @property 854 def final_destination_address(self): 855 return self._final_destination_address 856 857 858class LowpanMeshHeaderFactory: 859 860 def _parse_address(self, data, is_short): 861 if is_short: 862 return common.MacAddress.from_rloc16(bytearray(data.read(2))) 863 else: 864 return common.MacAddress.from_eui64(bytearray(data.read(8))) 865 866 def parse(self, data, message_info): 867 data_byte = ord(data.read(1)) 868 869 is_short_originator_address = bool(data_byte & 0x20) 870 is_short_final_destination_address = bool(data_byte & 0x10) 871 872 if (data_byte & 0x0F) != 0x0F: 873 hops_left = data_byte & 0x0F 874 else: 875 hops_left = ord(data.read(1)) 876 877 originator_address = self._parse_address(data, is_short_originator_address) 878 final_destination_address = self._parse_address(data, is_short_final_destination_address) 879 880 return LowpanMeshHeader(hops_left, originator_address, final_destination_address) 881 882 883class LowpanFragmentationHeader(object): 884 885 def __init__(self, datagram_size, datagram_tag, datagram_offset=0): 886 self._datagram_size = datagram_size 887 self._datagram_tag = datagram_tag 888 self._datagram_offset = datagram_offset 889 890 @property 891 def datagram_size(self): 892 return self._datagram_size 893 894 @property 895 def datagram_tag(self): 896 return self._datagram_tag 897 898 @property 899 def datagram_offset(self): 900 return self._datagram_offset 901 902 @property 903 def is_first(self): 904 return self.datagram_offset == 0 905 906 @classmethod 907 def from_bytes(cls, data): 908 datagram_size = struct.unpack(">H", data.read(2))[0] 909 has_offset = ((datagram_size >> 11) & 0x1F) == 0x1C 910 911 datagram_size &= 0x7FF 912 datagram_tag = struct.unpack(">H", data.read(2))[0] 913 datagram_offset = 0 914 915 if has_offset: 916 datagram_offset = ord(data.read(1)) 917 918 return cls(datagram_size, datagram_tag, datagram_offset) 919 920 921class LowpanFragmentsBuffer(object): 922 923 def __init__(self, buffer_size): 924 self._buffer = [None] * buffer_size 925 self._position = 0 926 927 def write(self, data): 928 if (self._position + len(data)) > len(self._buffer): 929 raise ValueError("Write failure. Data length is bigger than the destination buffer length.") 930 931 for i, byte in enumerate(data): 932 self._buffer[self._position + i] = byte 933 934 self._position += len(data) 935 return len(data) 936 937 def seek(self, offset): 938 if offset >= len(self._buffer): 939 raise ValueError("Could not seek current offset. Offset value is bigger than the buffer length.") 940 941 self._position = offset 942 943 def tell(self): 944 return self._position 945 946 def whole_packet_received(self): 947 return all([byte is not None for byte in self._buffer]) 948 949 def read(self): 950 if not self.whole_packet_received(): 951 raise ValueError("Only a part of the packet has been stored in the buffer.") 952 953 return bytearray(self._buffer) 954 955 def __len__(self): 956 return len(self._buffer) 957 958 959class LowpanFragmentsBuffersManager(object): 960 961 def __init__(self): 962 self._fragments_buffers = {} 963 964 def _create_key(self, message_info, datagram_tag): 965 key = (bytes(message_info.source_mac_address.mac_address) + 966 bytes(message_info.destination_mac_address.mac_address) + bytes(datagram_tag)) 967 return key 968 969 def _allocate_fragments_buffer(self, key, datagram_size): 970 if datagram_size is None or datagram_size < 0: 971 raise ValueError("Could not allocate fragments buffer. Invalid datagram size: {}".format(datagram_size)) 972 973 fragments_buffer = LowpanFragmentsBuffer(datagram_size) 974 975 self._fragments_buffers[key] = fragments_buffer 976 return fragments_buffer 977 978 def get_fragments_buffer(self, message_info, datagram_tag, datagram_size=None): 979 key = self._create_key(message_info, datagram_tag) 980 981 if key not in self._fragments_buffers: 982 self._allocate_fragments_buffer(key, datagram_size) 983 984 return self._fragments_buffers[key] 985 986 def free_fragments_buffer(self, message_info, datagram_tag): 987 key = self._create_key(message_info, datagram_tag) 988 989 del self._fragments_buffers[key] 990 991 992class LowpanParser(object): 993 994 def __init__( 995 self, 996 lowpan_mesh_header_factory, 997 lowpan_decompressor, 998 lowpan_fragements_buffers_manager, 999 ipv6_packet_factory, 1000 ): 1001 self._lowpan_mesh_header_factory = lowpan_mesh_header_factory 1002 self._lowpan_decompressor = lowpan_decompressor 1003 self._lowpan_fragments_buffers_manager = (lowpan_fragements_buffers_manager) 1004 self._ipv6_packet_factory = ipv6_packet_factory 1005 1006 def _peek_n_bytes(self, data, n): 1007 data_bytes = data.read(n) 1008 data.seek(-n, io.SEEK_CUR) 1009 return data_bytes 1010 1011 def _is_mesh_header(self, first_byte): 1012 return ((first_byte >> 6) & 0x03) == 0x02 1013 1014 def _is_first_fragmentation_header(self, first_byte): 1015 return ((first_byte >> 3) & 0x1F) == 0x18 1016 1017 def _is_subsequent_fragmentation_header(self, first_byte): 1018 return ((first_byte >> 3) & 0x1F) == 0x1C 1019 1020 def _is_iphc(self, first_byte): 1021 return ((first_byte >> 5) & 0x07) == 0x03 1022 1023 def _decompress_iphc(self, data, message_info): 1024 return self._lowpan_decompressor.decompress(data, message_info) 1025 1026 def _handle_first_fragmentation_header(self, data, message_info): 1027 fragmentation_header = LowpanFragmentationHeader.from_bytes(data) 1028 1029 fragments_buffer = self._lowpan_fragments_buffers_manager.get_fragments_buffer( 1030 message_info, 1031 fragmentation_header.datagram_tag, 1032 fragmentation_header.datagram_size, 1033 ) 1034 1035 ipv6_header, extension_headers, udp_header = self._decompress_iphc(data, message_info) 1036 1037 uncompressed_data = data.read() 1038 1039 # Update payload lengths 1040 ipv6_header.payload_length = fragmentation_header.datagram_size - len(ipv6_header) 1041 1042 fragments_buffer.seek(0) 1043 fragments_buffer.write(ipv6_header.to_bytes()) 1044 1045 for extension_header in extension_headers: 1046 fragments_buffer.write(extension_header.to_bytes()) 1047 1048 if udp_header is not None: 1049 fragments_buffer.write(udp_header.to_bytes()) 1050 1051 fragments_buffer.write(uncompressed_data) 1052 1053 if fragments_buffer.whole_packet_received(): 1054 data = io.BytesIO(fragments_buffer.read()) 1055 1056 self._lowpan_fragments_buffers_manager.free_fragments_buffer(message_info, 1057 fragmentation_header.datagram_tag) 1058 1059 return self._ipv6_packet_factory.parse(data, message_info) 1060 1061 return None 1062 1063 def _handle_subsequent_fragmentation_header(self, data, message_info): 1064 fragmentation_header = LowpanFragmentationHeader.from_bytes(data) 1065 1066 fragments_buffer = self._lowpan_fragments_buffers_manager.get_fragments_buffer( 1067 message_info, 1068 fragmentation_header.datagram_tag, 1069 fragmentation_header.datagram_size, 1070 ) 1071 1072 offset = fragmentation_header.datagram_offset * 8 1073 1074 fragments_buffer.seek(offset) 1075 fragments_buffer.write(data.read()) 1076 1077 if fragments_buffer.whole_packet_received(): 1078 data = io.BytesIO(fragments_buffer.read()) 1079 1080 self._lowpan_fragments_buffers_manager.free_fragments_buffer(message_info, 1081 fragmentation_header.datagram_tag) 1082 1083 return self._ipv6_packet_factory.parse(data, message_info) 1084 1085 return None 1086 1087 def _handle_iphc_header(self, data, message_info): 1088 ipv6_header, extension_headers, udp_header = self._decompress_iphc(data, message_info) 1089 1090 uncompressed_data = data.read() 1091 1092 decompressed_data = bytearray([]) 1093 1094 for extension_header in extension_headers: 1095 decompressed_data += extension_header.to_bytes() 1096 1097 if udp_header is not None: 1098 udp_header.payload_length = len(uncompressed_data) 1099 1100 decompressed_data += udp_header.to_bytes() 1101 1102 decompressed_data += uncompressed_data 1103 1104 ipv6_header.payload_length = len(decompressed_data) 1105 1106 decompressed_data = ipv6_header.to_bytes() + decompressed_data 1107 1108 return self._ipv6_packet_factory.parse(io.BytesIO(decompressed_data), message_info) 1109 1110 def set_lowpan_context(self, cid, prefix): 1111 self._lowpan_decompressor.set_lowpan_context(cid, prefix) 1112 1113 def parse(self, data, message_info): 1114 1115 while data.tell() < len(data.getvalue()): 1116 first_byte = ord(self._peek_n_bytes(data, n=1)) 1117 1118 if self._is_mesh_header(first_byte): 1119 mesh_header = self._lowpan_mesh_header_factory.parse(data, message_info) 1120 1121 message_info.source_mac_address = (mesh_header.originator_address) 1122 message_info.destination_mac_address = (mesh_header.final_destination_address) 1123 1124 elif self._is_first_fragmentation_header(first_byte): 1125 return self._handle_first_fragmentation_header(data, message_info) 1126 1127 elif self._is_subsequent_fragmentation_header(first_byte): 1128 return self._handle_subsequent_fragmentation_header(data, message_info) 1129 1130 elif self._is_iphc(first_byte): 1131 return self._handle_iphc_header(data, message_info) 1132 1133 else: 1134 raise RuntimeError("Unsupported header type: 0x{:02x}".format(first_byte)) 1135