1 /*
2 * Copyright (c) 2017-2021 Apple Inc. All rights reserved.
3 *
4 * @APPLE_OSREFERENCE_LICENSE_HEADER_START@
5 *
6 * This file contains Original Code and/or Modifications of Original Code
7 * as defined in and that are subject to the Apple Public Source License
8 * Version 2.0 (the 'License'). You may not use this file except in
9 * compliance with the License. The rights granted to you under the License
10 * may not be used to create, or enable the creation or redistribution of,
11 * unlawful or unlicensed copies of an Apple operating system, or to
12 * circumvent, violate, or enable the circumvention or violation of, any
13 * terms of an Apple operating system software license agreement.
14 *
15 * Please obtain a copy of the License at
16 * http://www.opensource.apple.com/apsl/ and read it before using this file.
17 *
18 * The Original Code and all software distributed under the License are
19 * distributed on an 'AS IS' basis, WITHOUT WARRANTY OF ANY KIND, EITHER
20 * EXPRESS OR IMPLIED, AND APPLE HEREBY DISCLAIMS ALL SUCH WARRANTIES,
21 * INCLUDING WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY,
22 * FITNESS FOR A PARTICULAR PURPOSE, QUIET ENJOYMENT OR NON-INFRINGEMENT.
23 * Please see the License for the specific language governing rights and
24 * limitations under the License.
25 *
26 * @APPLE_OSREFERENCE_LICENSE_HEADER_END@
27 */
28 /*
29 *
30 * Copyright (c) 2008, Swedish Institute of Computer Science
31 * All rights reserved.
32 *
33 * Additional fixes for AVR contributed by:
34 *
35 * Colin O'Flynn [email protected]
36 * Eric Gnoske [email protected]
37 * Blake Leverett [email protected]
38 * Mike Vidales [email protected]
39 * Kevin Brown [email protected]
40 * Nate Bohlmann [email protected]
41 *
42 * Additional fixes for MSP430 contributed by:
43 * Joakim Eriksson
44 * Niclas Finne
45 * Nicolas Tsiftes
46 *
47 * All rights reserved.
48 *
49 * Redistribution and use in source and binary forms, with or without
50 * modification, are permitted provided that the following conditions are met:
51 *
52 * * Redistributions of source code must retain the above copyright
53 * notice, this list of conditions and the following disclaimer.
54 * * Redistributions in binary form must reproduce the above copyright
55 * notice, this list of conditions and the following disclaimer in
56 * the documentation and/or other materials provided with the
57 * distribution.
58 * * Neither the name of the copyright holders nor the names of
59 * contributors may be used to endorse or promote products derived
60 * from this software without specific prior written permission.
61 *
62 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
63 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
64 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
65 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
66 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
67 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
68 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
69 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
70 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
71 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
72 * POSSIBILITY OF SUCH DAMAGE.
73 *
74 */
75 /*
76 * \brief This file is where the main functions that relate to frame
77 * manipulation will reside.
78 */
79
80 /**
81 * \file
82 * \brief 802.15.4 frame creation and parsing functions
83 *
84 * This file converts to and from a structure to a packed 802.15.4
85 * frame.
86 */
87
88 /**
89 * \addtogroup frame802154
90 * @{
91 */
92
93 #include "cc.h"
94 #include "frame802154.h"
95 //#include "net/llsec/llsec802154.h"
96 #include "linkaddr.h"
97 #include <string.h>
98
99 /**
100 * \brief Structure that contains the lengths of the various addressing and security fields
101 * in the 802.15.4 header. This structure is used in \ref frame802154_create()
102 */
103 typedef struct {
104 uint8_t dest_pid_len; /**< Length (in bytes) of destination PAN ID field */
105 uint8_t dest_addr_len; /**< Length (in bytes) of destination address field */
106 uint8_t src_pid_len; /**< Length (in bytes) of source PAN ID field */
107 uint8_t src_addr_len; /**< Length (in bytes) of source address field */
108 uint8_t aux_sec_len; /**< Length (in bytes) of aux security header field */
109 } field_length_t;
110
111 /*----------------------------------------------------------------------------*/
112 CC_INLINE static uint8_t
addr_len(uint8_t mode)113 addr_len(uint8_t mode)
114 {
115 switch (mode) {
116 case FRAME802154_SHORTADDRMODE: /* 16-bit address */
117 return 2;
118 case FRAME802154_LONGADDRMODE: /* 64-bit address */
119 return 8;
120 default:
121 return 0;
122 }
123 }
124 /*----------------------------------------------------------------------------*/
125 #if LLSEC802154_USES_EXPLICIT_KEYS
126 static uint8_t
get_key_id_len(uint8_t key_id_mode)127 get_key_id_len(uint8_t key_id_mode)
128 {
129 switch (key_id_mode) {
130 case FRAME802154_1_BYTE_KEY_ID_MODE:
131 return 1;
132 case FRAME802154_5_BYTE_KEY_ID_MODE:
133 return 5;
134 case FRAME802154_9_BYTE_KEY_ID_MODE:
135 return 9;
136 default:
137 return 0;
138 }
139 }
140 #endif /* LLSEC802154_USES_EXPLICIT_KEYS */
141 /*----------------------------------------------------------------------------*/
142 static void
field_len(frame802154_t * p,field_length_t * flen)143 field_len(frame802154_t *p, field_length_t *flen)
144 {
145 /* init flen to zeros */
146 memset(flen, 0, sizeof(field_length_t));
147
148 /* Determine lengths of each field based on fcf and other args */
149 if (p->fcf.dest_addr_mode & 3) {
150 flen->dest_pid_len = 2;
151 }
152 if (p->fcf.src_addr_mode & 3) {
153 flen->src_pid_len = 2;
154 }
155
156 /* Set PAN ID compression bit if src pan id matches dest pan id. */
157 if (p->fcf.dest_addr_mode & 3 && p->fcf.src_addr_mode & 3 &&
158 p->src_pid == p->dest_pid) {
159 p->fcf.panid_compression = 1;
160
161 /* compressed header, only do dest pid */
162 flen->src_pid_len = 0;
163 } else {
164 p->fcf.panid_compression = 0;
165 }
166
167 /* determine address lengths */
168 flen->dest_addr_len = addr_len(p->fcf.dest_addr_mode & 3);
169 flen->src_addr_len = addr_len(p->fcf.src_addr_mode & 3);
170
171 #if LLSEC802154_SECURITY_LEVEL
172 /* Aux security header */
173 if (p->fcf.security_enabled & 1) {
174 flen->aux_sec_len = 5
175 #if LLSEC802154_USES_EXPLICIT_KEYS
176 + get_key_id_len(p->aux_hdr.security_control.key_id_mode);
177 #endif /* LLSEC802154_USES_EXPLICIT_KEYS */
178 ;
179 }
180 #endif /* LLSEC802154_SECURITY_LEVEL */
181 }
182 /*----------------------------------------------------------------------------*/
183 /**
184 * \brief Calculates the length of the frame header. This function is
185 * meant to be called by a higher level function, that interfaces to a MAC.
186 *
187 * \param p Pointer to frame802154_t_t struct, which specifies the
188 * frame to send.
189 *
190 * \return The length of the frame header.
191 */
192 int
frame802154_hdrlen(frame802154_t * p)193 frame802154_hdrlen(frame802154_t *p)
194 {
195 field_length_t flen;
196 field_len(p, &flen);
197 return 3 + flen.dest_pid_len + flen.dest_addr_len +
198 flen.src_pid_len + flen.src_addr_len + flen.aux_sec_len;
199 }
200 /*----------------------------------------------------------------------------*/
201 /**
202 * \brief Creates a frame for transmission over the air. This function is
203 * meant to be called by a higher level function, that interfaces to a MAC.
204 *
205 * \param p Pointer to frame802154_t struct, which specifies the
206 * frame to send.
207 *
208 * \param buf Pointer to the buffer to use for the frame.
209 *
210 * \return The length of the frame header
211 */
212 int
frame802154_create(frame802154_t * p,uint8_t * buf)213 frame802154_create(frame802154_t *p, uint8_t *buf)
214 {
215 int c;
216 field_length_t flen;
217 uint8_t pos;
218 #if LLSEC802154_USES_EXPLICIT_KEYS
219 uint8_t key_id_mode;
220 #endif /* LLSEC802154_USES_EXPLICIT_KEYS */
221
222 field_len(p, &flen);
223
224 /* OK, now we have field lengths. Time to actually construct */
225 /* the outgoing frame, and store it in buf */
226 buf[0] = (uint8_t)((p->fcf.frame_type & 7) |
227 ((p->fcf.security_enabled & 1) << 3) |
228 ((p->fcf.frame_pending & 1) << 4) |
229 ((p->fcf.ack_required & 1) << 5) |
230 ((p->fcf.panid_compression & 1) << 6));
231 buf[1] = (uint8_t)(((p->fcf.dest_addr_mode & 3) << 2) |
232 ((p->fcf.frame_version & 3) << 4) |
233 ((p->fcf.src_addr_mode & 3) << 6));
234
235 /* sequence number */
236 buf[2] = p->seq;
237 pos = 3;
238
239 /* Destination PAN ID */
240 if (flen.dest_pid_len == 2) {
241 buf[pos++] = p->dest_pid & 0xff;
242 buf[pos++] = (p->dest_pid >> 8) & 0xff;
243 }
244
245 /* Destination address */
246 for (c = flen.dest_addr_len; c > 0; c--) {
247 buf[pos++] = p->dest_addr[c - 1];
248 }
249
250 /* Source PAN ID */
251 if (flen.src_pid_len == 2) {
252 buf[pos++] = p->src_pid & 0xff;
253 buf[pos++] = (p->src_pid >> 8) & 0xff;
254 }
255
256 /* Source address */
257 for (c = flen.src_addr_len; c > 0; c--) {
258 buf[pos++] = p->src_addr[c - 1];
259 }
260
261 #if LLSEC802154_SECURITY_LEVEL
262 /* Aux header */
263 if (flen.aux_sec_len) {
264 buf[pos++] = p->aux_hdr.security_control.security_level
265 #if LLSEC802154_USES_EXPLICIT_KEYS
266 | (p->aux_hdr.security_control.key_id_mode << 3)
267 #endif /* LLSEC802154_USES_EXPLICIT_KEYS */
268 ;
269 memcpy(buf + pos, p->aux_hdr.frame_counter.u8, 4);
270 pos += 4;
271
272 #if LLSEC802154_USES_EXPLICIT_KEYS
273 key_id_mode = p->aux_hdr.security_control.key_id_mode;
274 if (key_id_mode) {
275 c = (key_id_mode - 1) * 4;
276 memcpy(buf + pos, p->aux_hdr.key_source.u8, c);
277 pos += c;
278 buf[pos++] = p->aux_hdr.key_index;
279 }
280 #endif /* LLSEC802154_USES_EXPLICIT_KEYS */
281 }
282 #endif /* LLSEC802154_SECURITY_LEVEL */
283
284 return (int)pos;
285 }
286 /*----------------------------------------------------------------------------*/
287 /**
288 * \brief Parses an input frame. Scans the input frame to find each
289 * section, and stores the information of each section in a
290 * frame802154_t structure.
291 *
292 * \param data The input data from the radio chip.
293 * \param len The size of the input data
294 * \param pf The frame802154_t struct to store the parsed frame information.
295 */
296 size_t
frame802154_parse(uint8_t * data,size_t len,frame802154_t * pf,uint8_t ** payload)297 frame802154_parse(uint8_t *data, size_t len, frame802154_t *pf, uint8_t **payload)
298 {
299 uint8_t *p;
300 frame802154_fcf_t fcf;
301 size_t c;
302 #if LLSEC802154_USES_EXPLICIT_KEYS
303 uint8_t key_id_mode;
304 #endif /* LLSEC802154_USES_EXPLICIT_KEYS */
305
306 if (len < 3) {
307 *payload = NULL;
308 return 0;
309 }
310
311 p = data;
312
313 /* decode the FCF */
314 fcf.frame_type = p[0] & 7;
315 fcf.security_enabled = (p[0] >> 3) & 1;
316 fcf.frame_pending = (p[0] >> 4) & 1;
317 fcf.ack_required = (p[0] >> 5) & 1;
318 fcf.panid_compression = (p[0] >> 6) & 1;
319
320 fcf.dest_addr_mode = (p[1] >> 2) & 3;
321 fcf.frame_version = (p[1] >> 4) & 3;
322 fcf.src_addr_mode = (p[1] >> 6) & 3;
323
324 /* copy fcf and seqNum */
325 memcpy(&pf->fcf, &fcf, sizeof(frame802154_fcf_t));
326 pf->seq = p[2];
327 p += 3; /* Skip first three bytes */
328
329 /* Destination address, if any */
330 if (fcf.dest_addr_mode) {
331 /* Destination PAN */
332 pf->dest_pid = (uint16_t)(p[0] + (p[1] << 8));
333 p += 2;
334
335 /* Destination address */
336 /* l = addr_len(fcf.dest_addr_mode); */
337 /* for(c = 0; c < l; c++) { */
338 /* pf->dest_addr.u8[c] = p[l - c - 1]; */
339 /* } */
340 /* p += l; */
341 if (fcf.dest_addr_mode == FRAME802154_SHORTADDRMODE) {
342 linkaddr_copy((linkaddr_t *)(uintptr_t)&(pf->dest_addr), &linkaddr_null);
343 pf->dest_addr[0] = p[1];
344 pf->dest_addr[1] = p[0];
345 p += 2;
346 } else if (fcf.dest_addr_mode == FRAME802154_LONGADDRMODE) {
347 for (c = 0; c < 8; c++) {
348 pf->dest_addr[c] = p[7 - c];
349 }
350 p += 8;
351 }
352 } else {
353 linkaddr_copy((linkaddr_t *)(uintptr_t)&(pf->dest_addr), &linkaddr_null);
354 pf->dest_pid = 0;
355 }
356
357 /* Source address, if any */
358 if (fcf.src_addr_mode) {
359 /* Source PAN */
360 if (!fcf.panid_compression) {
361 pf->src_pid = (uint16_t)(p[0] + (p[1] << 8));
362 p += 2;
363 } else {
364 pf->src_pid = pf->dest_pid;
365 }
366
367 /* Source address */
368 /* l = addr_len(fcf.src_addr_mode); */
369 /* for(c = 0; c < l; c++) { */
370 /* pf->src_addr.u8[c] = p[l - c - 1]; */
371 /* } */
372 /* p += l; */
373 if (fcf.src_addr_mode == FRAME802154_SHORTADDRMODE) {
374 linkaddr_copy((linkaddr_t *)(uintptr_t)&(pf->src_addr), &linkaddr_null);
375 pf->src_addr[0] = p[1];
376 pf->src_addr[1] = p[0];
377 p += 2;
378 } else if (fcf.src_addr_mode == FRAME802154_LONGADDRMODE) {
379 for (c = 0; c < 8; c++) {
380 pf->src_addr[c] = p[7 - c];
381 }
382 p += 8;
383 }
384 } else {
385 linkaddr_copy((linkaddr_t *)(uintptr_t)&(pf->src_addr), &linkaddr_null);
386 pf->src_pid = 0;
387 }
388
389 #if LLSEC802154_SECURITY_LEVEL
390 if (fcf.security_enabled) {
391 pf->aux_hdr.security_control.security_level = p[0] & 7;
392 #if LLSEC802154_USES_EXPLICIT_KEYS
393 pf->aux_hdr.security_control.key_id_mode = (p[0] >> 3) & 3;
394 #endif /* LLSEC802154_USES_EXPLICIT_KEYS */
395 p += 1;
396
397 memcpy(pf->aux_hdr.frame_counter.u8, p, 4);
398 p += 4;
399
400 #if LLSEC802154_USES_EXPLICIT_KEYS
401 key_id_mode = pf->aux_hdr.security_control.key_id_mode;
402 if (key_id_mode) {
403 c = (key_id_mode - 1) * 4;
404 memcpy(pf->aux_hdr.key_source.u8, p, c);
405 p += c;
406 pf->aux_hdr.key_index = p[0];
407 p += 1;
408 }
409 #endif /* LLSEC802154_USES_EXPLICIT_KEYS */
410 }
411 #endif /* LLSEC802154_SECURITY_LEVEL */
412
413 /* header length */
414 c = p - data;
415
416
417 if (c <= len) {
418 /* payload length */
419 pf->payload_len = (int)(len - c);
420 /* payload */
421 *payload = p;
422 } else {
423 *payload = NULL;
424 }
425
426 /* return header length if successful */
427 return c > len ? 0 : c;
428 }
429 /** \} */
430