openthread/tests/scripts/thread-cert/mac802154.py

#!/usr/bin/env python3
#
#  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 KeyIdMode0Exception(Exception):
    """
    Raised when key id mode of packet is 0.
    Such packet wouldn't be handled in test scripts,
    but it's not abnormal behavior.
    """
    pass


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 InformationElement:
    """Class representing 802.15.4 MAC Information Element."""

    def __init__(self, id, length, content):
        self._id = id
        self._length = length
        self._content = content

    @property
    def id(self):
        return self._id

    @property
    def length(self):
        return self._length

    @property
    def content(self):
        return self._content


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

    class CommandIdentifier:
        DATA_REQUEST = 4

    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.command_type = command_type

        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."""

    IEEE802154_HEADER_IE_TYPE_MASK = 0x8000
    IEEE802154_HEADER_IE_ID_MASK = 0x7F80
    IEEE802154_HEADER_IE_LENGTH_MASK = 0x007F

    IEEE802154_HEADER_IE_HT1 = 0x7E
    IEEE802154_HEADER_IE_HT2 = 0x7F

    IEEE802154_VERSION_2015 = 0x02

    def parse(self, data):
        """Parse a MAC 802.15.4 frame

        Format of MAC 802.15.4 Frame:
        | Frame Header | ASH | IE | Frame Payload | MIC | FCS |

        Frame Header:  Frame Control | Sequence Number | Addressing Field
        ASH: Auxiliary Security Header (optional)
        IE: Header IE | Payload IE (optional)
        Frame Payload: Data payload (optional)
        MIC: To authenticate the frame (optional)

        In the parsing process, Frame Header is parsed first. Then it can be
        determined if security is enabled for this frame. If it's enabled,
        then ASH should be parsed.
        After that, go directly to the end to parse FCS first instead
        of parsing IE or Frame Payload. This is because sometimes, IE doesn't
        have a termination IE and parsing needs to know the start of MIC to determine
        where's the end of IEs(In such cases there are no payload).
        After parsing FCS, MIC is parsed if security is enabled.
        Then the end of IE(or payload) is known. And at last IEs and payload
        are parsed.
        And after parsing everything, build MacHeader and MacPayload with
        the things parsed.
        """
        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
        ie_present = bool(fc & 0x0200)

        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

        dest_addr_present = dest_addr_mode != MacHeader.AddressMode.NOT_PRESENT
        src_addr_present = source_addr_mode != MacHeader.AddressMode.NOT_PRESENT

        if frame_version < 2:
            dest_pan_present = dest_addr_present
        else:
            dest_pan_present = True
            if not src_addr_present:
                dest_pan_present = src_pan_present != panid_compression
            elif not dest_addr_present:
                dest_pan_present = False
            else:
                if dest_addr_mode == MacHeader.AddressMode.EXTENDED and source_addr_mode == MacHeader.AddressMode.EXTENDED and panid_compression:
                    dest_pan_present = False

        if dest_pan_present:
            dest_pan_id = struct.unpack("<H", data.read(2))[0]
        else:
            dest_pan_id = None

        dest_address = self._parse_address(data, dest_addr_mode)

        src_pan_present = not panid_compression
        while src_pan_present:
            if frame_version < 2:
                break
            if frame_version == 3:
                assert False
            if dest_addr_mode == 0:
                break
            if dest_addr_mode == 2:
                break
            if dest_addr_mode == 3 and source_addr_mode == 2:
                break
            src_pan_present = False

        if src_pan_present:
            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

        # Record the beginning position of header IE.
        # Or the position of payload if there is no header IE.
        cur_pos = data.tell()
        header_ie_start = cur_pos

        data.seek(-2, io.SEEK_END)
        fcs_start = data.tell()
        fcs = self._parse_fcs(data, fcs_start)

        data.seek(fcs_start)
        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 = fcs_start
            mic = None

        # There may be no termination IE. In such case, there is no payload, get
        # IE until the beginning position of MIC or a termination IE.
        header_ie_list = []
        data.seek(cur_pos)
        while ie_present and cur_pos + 2 < payload_end:
            header_ie = struct.unpack("<H", data.read(2))[0]
            id = (header_ie & MacFrame.IEEE802154_HEADER_IE_ID_MASK) >> 7
            # Currently, payload IE doesn't exist in the code. So only HT2 is required.
            # TODO: support HT1 when there are Payload IEs in our code
            assert id != MacFrame.IEEE802154_HEADER_IE_HT1, \
                'Currently there should be no HT1!'
            header_ie_length = (header_ie & MacFrame.IEEE802154_HEADER_IE_LENGTH_MASK)
            assert cur_pos + 2 + header_ie_length <= payload_end, \
                'Parsing Header IE error, IE id:{} length:{}'.format(id, header_ie_length)
            header_ie_content = data.read(header_ie_length)
            header_ie_list.append(InformationElement(id, header_ie_length, header_ie_content))
            cur_pos += 2 + header_ie_length
            if id == MacFrame.IEEE802154_HEADER_IE_HT2:
                break
        header_ie_end = cur_pos

        payload_pos = cur_pos
        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)

            extra_open_fields = bytearray([])

            if ie_present:
                data.seek(header_ie_start)
                extra_open_fields += data.read(header_ie_end - header_ie_start)

            message_info = MessageInfo()
            message_info.aux_sec_hdr = aux_sec_header
            message_info.aux_sec_hdr_bytes = aux_sec_hdr_bytes
            message_info.extra_open_fields = extra_open_fields
            message_info.mhr_bytes = mhr_bytes

            open_payload = []
            private_payload = payload

            # Check end of MAC frame
            if frame_type == MacHeader.FrameType.COMMAND:
                if frame_version < MacFrame.IEEE802154_VERSION_2015:
                    extra_open_fields.append(payload[0])
                    open_payload = payload[0:1]
                    private_payload = payload[1:]
                    message_info.open_payload_length = 1

            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(bytearray(open_payload) + sec_obj.decrypt(private_payload, mic, message_info))

        else:
            self.payload = MacPayload(payload)

        if frame_type == MacHeader.FrameType.COMMAND:
            command_type = self.payload.data[0]
        else:
            command_type = None

        # 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,
        )

    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 == 0:
            raise KeyIdMode0Exception
        elif 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:
            pass

        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