| #!/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. | |
| # | |
| """ | |
| This module provides simple 802.15.4 MAC parser. | |
| """ | |
| import io | |
| import struct | |
| import config | |
| from common import MacAddress, MacAddressType, MessageInfo | |
| from net_crypto import AuxiliarySecurityHeader, CryptoEngine, MacCryptoMaterialCreator | |
| class DeviceDescriptors: | |
| """Class representing 802.15.4 Device Descriptors.""" | |
| device_descriptors = {} | |
| @classmethod | |
| def add(cls, short_address, extended_address): | |
| short_address = cls._get_short_address_value(short_address) | |
| cls.device_descriptors[short_address] = extended_address | |
| @classmethod | |
| def get_extended(cls, short_address): | |
| short_address = cls._get_short_address_value(short_address) | |
| return cls.device_descriptors[short_address] | |
| @staticmethod | |
| def _get_short_address_value(short_address): | |
| if isinstance(short_address, MacAddress): | |
| short_address = short_address.rloc | |
| return short_address | |
| class MacHeader: | |
| """Class representing 802.15.4 MAC header.""" | |
| class FrameType: | |
| BEACON = 0 | |
| DATA = 1 | |
| ACK = 2 | |
| COMMAND = 3 | |
| class AddressMode: | |
| NOT_PRESENT = 0 | |
| SHORT = 2 | |
| EXTENDED = 3 | |
| def __init__(self, frame_type, frame_pending, ack_request, frame_version, seq, | |
| dest_pan_id=None, dest_address=None, src_pan_id=None, src_address=None, command_type=None, | |
| aux_sec_header=None, mic=None, | |
| fcs=None): | |
| self.frame_type = frame_type | |
| self.frame_pending = frame_pending | |
| self.ack_request = ack_request | |
| self.frame_version = frame_version | |
| self.seq = seq | |
| self.dest_pan_id = dest_pan_id | |
| self.dest_address = dest_address | |
| self.src_pan_id = src_pan_id | |
| self.src_address = src_address | |
| self.aux_sec_header = aux_sec_header | |
| self.mic = mic | |
| self.fcs = fcs | |
| class MacPayload: | |
| """Class representing 802.15.4 MAC payload.""" | |
| def __init__(self, data): | |
| self.data = bytearray(data) | |
| class MacFrame: | |
| """Class representing 802.15.4 MAC frame.""" | |
| def parse(self, data): | |
| mhr_start = data.tell() | |
| fc, seq = struct.unpack("<HB", data.read(3)) | |
| frame_type = fc & 0x0007 | |
| security_enabled = bool(fc & 0x0008) | |
| frame_pending = bool(fc & 0x0010) | |
| ack_request = bool(fc & 0x0020) | |
| panid_compression = bool(fc & 0x0040) | |
| dest_addr_mode = (fc & 0x0c00) >> 10 | |
| frame_version = (fc & 0x3000) >> 12 | |
| source_addr_mode = (fc & 0xc000) >> 14 | |
| if frame_type == MacHeader.FrameType.ACK: | |
| fcs = self._parse_fcs(data, data.tell()) | |
| self.header = MacHeader(frame_type, frame_pending, ack_request, frame_version, seq, fcs=fcs) | |
| self.payload = None | |
| return | |
| # Presence of PAN Ids is not fully implemented yet but should be enough for Thread. | |
| dest_pan_id = struct.unpack("<H", data.read(2))[0] | |
| dest_address = self._parse_address(data, dest_addr_mode) | |
| if not panid_compression: | |
| src_pan_id = struct.unpack("<H", data.read(2))[0] | |
| else: | |
| src_pan_id = dest_pan_id | |
| src_address = self._parse_address(data, source_addr_mode) | |
| mhr_end = data.tell() | |
| if security_enabled: | |
| aux_sec_header = self._parse_aux_sec_header(data) | |
| aux_sec_header_end = data.tell() | |
| else: | |
| aux_sec_header = None | |
| # Check end of MAC frame | |
| if frame_type == MacHeader.FrameType.COMMAND: | |
| command_type = ord(data.read(1)) | |
| else: | |
| command_type = None | |
| payload_pos = data.tell() | |
| data.seek(-2, io.SEEK_END) | |
| fcs_start = data.tell() | |
| if aux_sec_header and aux_sec_header.security_level: | |
| mic, payload_end = self._parse_mic(data, aux_sec_header.security_level) | |
| else: | |
| payload_end = data.tell() | |
| mic = None | |
| fcs = self._parse_fcs(data, fcs_start) | |
| # Create Header object | |
| self.header = MacHeader(frame_type, frame_pending, ack_request, frame_version, seq, | |
| dest_pan_id, dest_address, src_pan_id, src_address, command_type, | |
| aux_sec_header, mic, | |
| fcs) | |
| # Create Payload object | |
| payload_len = payload_end - payload_pos | |
| data.seek(payload_pos) | |
| payload = data.read(payload_len) | |
| if security_enabled: | |
| mhr_len = mhr_end - mhr_start | |
| data.seek(mhr_start) | |
| mhr_bytes = data.read(mhr_len) | |
| aux_sec_header_len = aux_sec_header_end - mhr_end | |
| aux_sec_hdr_bytes = data.read(aux_sec_header_len) | |
| non_payload_fields = bytearray([]) | |
| if command_type is not None: | |
| non_payload_fields.append(command_type) | |
| message_info = MessageInfo() | |
| message_info.aux_sec_hdr = aux_sec_header | |
| message_info.aux_sec_hdr_bytes = aux_sec_hdr_bytes | |
| message_info.nonpayload_fields = non_payload_fields | |
| message_info.mhr_bytes = mhr_bytes | |
| if src_address.type == MacAddressType.SHORT: | |
| message_info.source_mac_address = DeviceDescriptors.get_extended(src_address).mac_address | |
| else: | |
| message_info.source_mac_address = src_address.mac_address | |
| sec_obj = CryptoEngine(MacCryptoMaterialCreator(config.DEFAULT_MASTER_KEY)) | |
| self.payload = MacPayload(sec_obj.decrypt(payload, mic, message_info)) | |
| else: | |
| self.payload = MacPayload(payload) | |
| def _parse_address(self, data, mode): | |
| if mode == MacHeader.AddressMode.SHORT: | |
| return MacAddress(data.read(2), MacAddressType.SHORT, big_endian=False) | |
| if mode == MacHeader.AddressMode.EXTENDED: | |
| return MacAddress(data.read(8), MacAddressType.LONG, big_endian=False) | |
| else: | |
| return None | |
| def _parse_aux_sec_header(self, data): | |
| security_control, frame_counter = struct.unpack("<BL", data.read(5)) | |
| security_level = security_control & 0x07 | |
| key_id_mode = (security_control & 0x18) >> 3 | |
| if key_id_mode == 1: | |
| key_id = data.read(1) | |
| elif key_id_mode == 2: | |
| key_id = data.read(5) | |
| elif key_id_mode == 3: | |
| key_id = data.read(9) | |
| else: | |
| key_source = None | |
| key_index = None | |
| return AuxiliarySecurityHeader(key_id_mode, security_level, frame_counter, key_id, big_endian=False) | |
| def _parse_mic(self, data, security_level): | |
| if security_level in (1, 5): | |
| data.seek(-4, io.SEEK_CUR) | |
| payload_end = data.tell() | |
| mic = data.read(4) | |
| elif security_level in (2, 6): | |
| data.seek(-8, io.SEEK_CUR) | |
| payload_end = data.tell() | |
| mic = data.read(8) | |
| elif security_level in (3, 7): | |
| data.seek(-16, io.SEEK_CUR) | |
| payload_end = data.tell() | |
| mic = data.read(16) | |
| else: | |
| payload_end = data.tell() | |
| return mic, payload_end | |
| def _parse_fcs(self, data, fcs_start): | |
| data.seek(fcs_start) | |
| fcs = bytearray(data.read(2)) | |
| return fcs |