434 lines
14 KiB
Python
Executable File
434 lines
14 KiB
Python
Executable File
#!/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_NETWORK_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
|