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"""
30    This module provides simple 802.15.4 MAC parser.
31"""
32
33import io
34import struct
35
36import config
37from common import MacAddress, MacAddressType, MessageInfo
38from net_crypto import (
39    AuxiliarySecurityHeader,
40    CryptoEngine,
41    MacCryptoMaterialCreator,
42)
43
44
45class KeyIdMode0Exception(Exception):
46    """
47    Raised when key id mode of packet is 0.
48    Such packet wouldn't be handled in test scripts,
49    but it's not abnormal behavior.
50    """
51    pass
52
53
54class DeviceDescriptors:
55    """Class representing 802.15.4 Device Descriptors."""
56
57    device_descriptors = {}
58
59    @classmethod
60    def add(cls, short_address, extended_address):
61        short_address = cls._get_short_address_value(short_address)
62        cls.device_descriptors[short_address] = extended_address
63
64    @classmethod
65    def get_extended(cls, short_address):
66        short_address = cls._get_short_address_value(short_address)
67        return cls.device_descriptors[short_address]
68
69    @staticmethod
70    def _get_short_address_value(short_address):
71        if isinstance(short_address, MacAddress):
72            short_address = short_address.rloc
73        return short_address
74
75
76class InformationElement:
77    """Class representing 802.15.4 MAC Information Element."""
78
79    def __init__(self, id, length, content):
80        self._id = id
81        self._length = length
82        self._content = content
83
84    @property
85    def id(self):
86        return self._id
87
88    @property
89    def length(self):
90        return self._length
91
92    @property
93    def content(self):
94        return self._content
95
96
97class MacHeader:
98    """Class representing 802.15.4 MAC header."""
99
100    class FrameType:
101        BEACON = 0
102        DATA = 1
103        ACK = 2
104        COMMAND = 3
105
106    class AddressMode:
107        NOT_PRESENT = 0
108        SHORT = 2
109        EXTENDED = 3
110
111    class CommandIdentifier:
112        DATA_REQUEST = 4
113
114    def __init__(
115        self,
116        frame_type,
117        frame_pending,
118        ack_request,
119        frame_version,
120        seq,
121        dest_pan_id=None,
122        dest_address=None,
123        src_pan_id=None,
124        src_address=None,
125        command_type=None,
126        aux_sec_header=None,
127        mic=None,
128        fcs=None,
129    ):
130
131        self.frame_type = frame_type
132        self.frame_pending = frame_pending
133        self.ack_request = ack_request
134        self.frame_version = frame_version
135        self.seq = seq
136
137        self.dest_pan_id = dest_pan_id
138        self.dest_address = dest_address
139        self.src_pan_id = src_pan_id
140        self.src_address = src_address
141        self.command_type = command_type
142
143        self.aux_sec_header = aux_sec_header
144        self.mic = mic
145
146        self.fcs = fcs
147
148
149class MacPayload:
150    """Class representing 802.15.4 MAC payload."""
151
152    def __init__(self, data):
153        self.data = bytearray(data)
154
155
156class MacFrame:
157    """Class representing 802.15.4 MAC frame."""
158
159    IEEE802154_HEADER_IE_TYPE_MASK = 0x8000
160    IEEE802154_HEADER_IE_ID_MASK = 0x7F80
161    IEEE802154_HEADER_IE_LENGTH_MASK = 0x007F
162
163    IEEE802154_HEADER_IE_HT1 = 0x7E
164    IEEE802154_HEADER_IE_HT2 = 0x7F
165
166    IEEE802154_VERSION_2015 = 0x02
167
168    def parse(self, data):
169        """Parse a MAC 802.15.4 frame
170
171        Format of MAC 802.15.4 Frame:
172        | Frame Header | ASH | IE | Frame Payload | MIC | FCS |
173
174        Frame Header:  Frame Control | Sequence Number | Addressing Field
175        ASH: Auxiliary Security Header (optional)
176        IE: Header IE | Payload IE (optional)
177        Frame Payload: Data payload (optional)
178        MIC: To authenticate the frame (optional)
179
180        In the parsing process, Frame Header is parsed first. Then it can be
181        determined if security is enabled for this frame. If it's enabled,
182        then ASH should be parsed.
183        After that, go directly to the end to parse FCS first instead
184        of parsing IE or Frame Payload. This is because sometimes, IE doesn't
185        have a termination IE and parsing needs to know the start of MIC to determine
186        where's the end of IEs(In such cases there are no payload).
187        After parsing FCS, MIC is parsed if security is enabled.
188        Then the end of IE(or payload) is known. And at last IEs and payload
189        are parsed.
190        And after parsing everything, build MacHeader and MacPayload with
191        the things parsed.
192        """
193        mhr_start = data.tell()
194
195        fc, seq = struct.unpack("<HB", data.read(3))
196
197        frame_type = fc & 0x0007
198        security_enabled = bool(fc & 0x0008)
199        frame_pending = bool(fc & 0x0010)
200        ack_request = bool(fc & 0x0020)
201        panid_compression = bool(fc & 0x0040)
202        dest_addr_mode = (fc & 0x0C00) >> 10
203        frame_version = (fc & 0x3000) >> 12
204        source_addr_mode = (fc & 0xC000) >> 14
205        ie_present = bool(fc & 0x0200)
206
207        if frame_type == MacHeader.FrameType.ACK:
208            fcs = self._parse_fcs(data, data.tell())
209            self.header = MacHeader(
210                frame_type,
211                frame_pending,
212                ack_request,
213                frame_version,
214                seq,
215                fcs=fcs,
216            )
217            self.payload = None
218            return
219
220        dest_addr_present = dest_addr_mode != MacHeader.AddressMode.NOT_PRESENT
221        src_addr_present = source_addr_mode != MacHeader.AddressMode.NOT_PRESENT
222
223        if frame_version < 2:
224            dest_pan_present = dest_addr_present
225        else:
226            dest_pan_present = True
227            if not src_addr_present:
228                dest_pan_present = src_pan_present != panid_compression
229            elif not dest_addr_present:
230                dest_pan_present = False
231            else:
232                if dest_addr_mode == MacHeader.AddressMode.EXTENDED and source_addr_mode == MacHeader.AddressMode.EXTENDED and panid_compression:
233                    dest_pan_present = False
234
235        if dest_pan_present:
236            dest_pan_id = struct.unpack("<H", data.read(2))[0]
237        else:
238            dest_pan_id = None
239
240        dest_address = self._parse_address(data, dest_addr_mode)
241
242        src_pan_present = not panid_compression
243        while src_pan_present:
244            if frame_version < 2:
245                break
246            if frame_version == 3:
247                assert False
248            if dest_addr_mode == 0:
249                break
250            if dest_addr_mode == 2:
251                break
252            if dest_addr_mode == 3 and source_addr_mode == 2:
253                break
254            src_pan_present = False
255
256        if src_pan_present:
257            src_pan_id = struct.unpack("<H", data.read(2))[0]
258        else:
259            src_pan_id = dest_pan_id
260
261        src_address = self._parse_address(data, source_addr_mode)
262
263        mhr_end = data.tell()
264
265        if security_enabled:
266            aux_sec_header = self._parse_aux_sec_header(data)
267            aux_sec_header_end = data.tell()
268        else:
269            aux_sec_header = None
270
271        # Record the beginning position of header IE.
272        # Or the position of payload if there is no header IE.
273        cur_pos = data.tell()
274        header_ie_start = cur_pos
275
276        data.seek(-2, io.SEEK_END)
277        fcs_start = data.tell()
278        fcs = self._parse_fcs(data, fcs_start)
279
280        data.seek(fcs_start)
281        if aux_sec_header and aux_sec_header.security_level:
282            mic, payload_end = self._parse_mic(data, aux_sec_header.security_level)
283        else:
284            payload_end = fcs_start
285            mic = None
286
287        # There may be no termination IE. In such case, there is no payload, get
288        # IE until the beginning position of MIC or a termination IE.
289        header_ie_list = []
290        data.seek(cur_pos)
291        while ie_present and cur_pos + 2 < payload_end:
292            header_ie = struct.unpack("<H", data.read(2))[0]
293            id = (header_ie & MacFrame.IEEE802154_HEADER_IE_ID_MASK) >> 7
294            # Currently, payload IE doesn't exist in the code. So only HT2 is required.
295            # TODO: support HT1 when there are Payload IEs in our code
296            assert id != MacFrame.IEEE802154_HEADER_IE_HT1, \
297                'Currently there should be no HT1!'
298            header_ie_length = (header_ie & MacFrame.IEEE802154_HEADER_IE_LENGTH_MASK)
299            assert cur_pos + 2 + header_ie_length <= payload_end, \
300                'Parsing Header IE error, IE id:{} length:{}'.format(id, header_ie_length)
301            header_ie_content = data.read(header_ie_length)
302            header_ie_list.append(InformationElement(id, header_ie_length, header_ie_content))
303            cur_pos += 2 + header_ie_length
304            if id == MacFrame.IEEE802154_HEADER_IE_HT2:
305                break
306        header_ie_end = cur_pos
307
308        payload_pos = cur_pos
309        payload_len = payload_end - payload_pos
310        data.seek(payload_pos)
311        payload = data.read(payload_len)
312
313        if security_enabled:
314            mhr_len = mhr_end - mhr_start
315            data.seek(mhr_start)
316            mhr_bytes = data.read(mhr_len)
317
318            aux_sec_header_len = aux_sec_header_end - mhr_end
319            aux_sec_hdr_bytes = data.read(aux_sec_header_len)
320
321            extra_open_fields = bytearray([])
322
323            if ie_present:
324                data.seek(header_ie_start)
325                extra_open_fields += data.read(header_ie_end - header_ie_start)
326
327            message_info = MessageInfo()
328            message_info.aux_sec_hdr = aux_sec_header
329            message_info.aux_sec_hdr_bytes = aux_sec_hdr_bytes
330            message_info.extra_open_fields = extra_open_fields
331            message_info.mhr_bytes = mhr_bytes
332
333            open_payload = []
334            private_payload = payload
335
336            # Check end of MAC frame
337            if frame_type == MacHeader.FrameType.COMMAND:
338                if frame_version < MacFrame.IEEE802154_VERSION_2015:
339                    extra_open_fields.append(payload[0])
340                    open_payload = payload[0:1]
341                    private_payload = payload[1:]
342                    message_info.open_payload_length = 1
343
344            if src_address.type == MacAddressType.SHORT:
345                message_info.source_mac_address = DeviceDescriptors.get_extended(src_address).mac_address
346            else:
347                message_info.source_mac_address = src_address.mac_address
348
349            sec_obj = CryptoEngine(MacCryptoMaterialCreator(config.DEFAULT_NETWORK_KEY))
350            self.payload = MacPayload(bytearray(open_payload) + sec_obj.decrypt(private_payload, mic, message_info))
351
352        else:
353            self.payload = MacPayload(payload)
354
355        if frame_type == MacHeader.FrameType.COMMAND:
356            command_type = self.payload.data[0]
357        else:
358            command_type = None
359
360        # Create Header object
361        self.header = MacHeader(
362            frame_type,
363            frame_pending,
364            ack_request,
365            frame_version,
366            seq,
367            dest_pan_id,
368            dest_address,
369            src_pan_id,
370            src_address,
371            command_type,
372            aux_sec_header,
373            mic,
374            fcs,
375        )
376
377    def _parse_address(self, data, mode):
378        if mode == MacHeader.AddressMode.SHORT:
379            return MacAddress(data.read(2), MacAddressType.SHORT, big_endian=False)
380
381        if mode == MacHeader.AddressMode.EXTENDED:
382            return MacAddress(data.read(8), MacAddressType.LONG, big_endian=False)
383
384        else:
385            return None
386
387    def _parse_aux_sec_header(self, data):
388        security_control, frame_counter = struct.unpack("<BL", data.read(5))
389
390        security_level = security_control & 0x07
391        key_id_mode = (security_control & 0x18) >> 3
392
393        if key_id_mode == 0:
394            raise KeyIdMode0Exception
395        elif key_id_mode == 1:
396            key_id = data.read(1)
397        elif key_id_mode == 2:
398            key_id = data.read(5)
399        elif key_id_mode == 3:
400            key_id = data.read(9)
401        else:
402            pass
403
404        return AuxiliarySecurityHeader(
405            key_id_mode,
406            security_level,
407            frame_counter,
408            key_id,
409            big_endian=False,
410        )
411
412    def _parse_mic(self, data, security_level):
413        if security_level in (1, 5):
414            data.seek(-4, io.SEEK_CUR)
415            payload_end = data.tell()
416            mic = data.read(4)
417        elif security_level in (2, 6):
418            data.seek(-8, io.SEEK_CUR)
419            payload_end = data.tell()
420            mic = data.read(8)
421        elif security_level in (3, 7):
422            data.seek(-16, io.SEEK_CUR)
423            payload_end = data.tell()
424            mic = data.read(16)
425        else:
426            payload_end = data.tell()
427
428        return mic, payload_end
429
430    def _parse_fcs(self, data, fcs_start):
431        data.seek(fcs_start)
432        fcs = bytearray(data.read(2))
433        return fcs
434