1 /*
2 * Copyright (c) 2016 Intel Corporation.
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 /**
8 * @file
9 * @brief IEEE 802.15.4 MAC frame related functions implementation
10 *
11 * All references to the spec refer to IEEE 802.15.4-2020.
12 */
13
14 #include <zephyr/logging/log.h>
15 LOG_MODULE_REGISTER(net_ieee802154_frame, CONFIG_NET_L2_IEEE802154_LOG_LEVEL);
16
17 #include "ieee802154_frame.h"
18 #include "ieee802154_security.h"
19
20 #include <zephyr/net/net_core.h>
21 #include <zephyr/net/net_if.h>
22
23 #include <ipv6.h>
24 #include <nbr.h>
25
26 #define dbg_print_fs(fs) \
27 NET_DBG("fs(1): %u/%u/%u/%u/%u/%u", fs->fc.frame_type, fs->fc.security_enabled, \
28 fs->fc.frame_pending, fs->fc.ar, fs->fc.pan_id_comp, fs->fc.reserved); \
29 NET_DBG("fs(2): %u/%u/%u/%u/%u - %u", fs->fc.seq_num_suppr, fs->fc.ie_list, \
30 fs->fc.dst_addr_mode, fs->fc.frame_version, fs->fc.src_addr_mode, fs->sequence)
31
32 #define BUF_TIMEOUT K_MSEC(50)
33
34 #ifdef CONFIG_NET_L2_IEEE802154_SECURITY
35 const uint8_t level_2_authtag_len[4] = {0, IEEE802154_AUTH_TAG_LENGTH_32,
36 IEEE802154_AUTH_TAG_LENGTH_64,
37 IEEE802154_AUTH_TAG_LENGTH_128};
38 #endif
39
ieee802154_validate_fc_seq(uint8_t * buf,uint8_t ** p_buf,uint8_t * length)40 struct ieee802154_fcf_seq *ieee802154_validate_fc_seq(uint8_t *buf, uint8_t **p_buf,
41 uint8_t *length)
42 {
43 struct ieee802154_fcf_seq *fs = (struct ieee802154_fcf_seq *)buf;
44
45 dbg_print_fs(fs);
46
47 /** Basic FC checks */
48 if (fs->fc.frame_type == IEEE802154_FRAME_TYPE_RESERVED ||
49 fs->fc.frame_version >= IEEE802154_VERSION_RESERVED) {
50 return NULL;
51 }
52
53 if (fs->fc.frame_type == IEEE802154_FRAME_TYPE_MULTIPURPOSE) {
54 if (fs->fc.frame_version != 0) {
55 return NULL;
56 }
57 } else {
58 /** Only for versions 2003/2006 */
59 if (fs->fc.frame_version < IEEE802154_VERSION_802154 &&
60 (fs->fc.dst_addr_mode == IEEE802154_ADDR_MODE_RESERVED ||
61 fs->fc.src_addr_mode == IEEE802154_ADDR_MODE_RESERVED ||
62 fs->fc.frame_type >= IEEE802154_FRAME_TYPE_RESERVED)) {
63 return NULL;
64 }
65 }
66
67 if (fs->fc.frame_type == IEEE802154_FRAME_TYPE_BEACON &&
68 (fs->fc.dst_addr_mode != IEEE802154_ADDR_MODE_NONE ||
69 fs->fc.src_addr_mode == IEEE802154_ADDR_MODE_NONE || fs->fc.pan_id_comp)) {
70 /** See section 7.2.2.1.1 */
71 return NULL;
72 } else if (fs->fc.frame_type == IEEE802154_FRAME_TYPE_DATA &&
73 fs->fc.dst_addr_mode == IEEE802154_ADDR_MODE_NONE &&
74 fs->fc.src_addr_mode == IEEE802154_ADDR_MODE_NONE) {
75 /** See section 7.2.2.2.1 */
76 return NULL;
77 } else if (fs->fc.frame_type == IEEE802154_FRAME_TYPE_MAC_COMMAND && fs->fc.frame_pending) {
78 /** See section 7.3 */
79 return NULL;
80 }
81
82 #ifndef CONFIG_NET_L2_IEEE802154_SECURITY
83 if (fs->fc.security_enabled) {
84 return NULL;
85 }
86 #endif
87
88 if (p_buf) {
89 *length -= IEEE802154_FCF_SEQ_LENGTH;
90 *p_buf = buf + IEEE802154_FCF_SEQ_LENGTH;
91 }
92
93 return fs;
94 }
95
validate_addr(uint8_t * buf,uint8_t ** p_buf,uint8_t * length,enum ieee802154_addressing_mode mode,bool pan_id_compression,struct ieee802154_address_field ** addr)96 static inline bool validate_addr(uint8_t *buf, uint8_t **p_buf, uint8_t *length,
97 enum ieee802154_addressing_mode mode, bool pan_id_compression,
98 struct ieee802154_address_field **addr)
99 {
100 uint8_t len = 0;
101
102 *p_buf = buf;
103
104 NET_DBG("Buf %p - mode %d - pan id comp %d", (void *)buf, mode, pan_id_compression);
105
106 if (mode == IEEE802154_ADDR_MODE_NONE) {
107 *addr = NULL;
108 return true;
109 }
110
111 if (!pan_id_compression) {
112 len = IEEE802154_PAN_ID_LENGTH;
113 }
114
115 if (mode == IEEE802154_ADDR_MODE_SHORT) {
116 len += IEEE802154_SHORT_ADDR_LENGTH;
117 } else {
118 /* IEEE802154_ADDR_MODE_EXTENDED */
119 len += IEEE802154_EXT_ADDR_LENGTH;
120 }
121
122 if (len > *length) {
123 return false;
124 }
125
126 *p_buf += len;
127 *length -= len;
128
129 *addr = (struct ieee802154_address_field *)buf;
130
131 return true;
132 }
133
134 #ifdef CONFIG_NET_L2_IEEE802154_SECURITY
135 struct ieee802154_aux_security_hdr *
ieee802154_validate_aux_security_hdr(uint8_t * buf,uint8_t ** p_buf,uint8_t * length)136 ieee802154_validate_aux_security_hdr(uint8_t *buf, uint8_t **p_buf, uint8_t *length)
137 {
138 struct ieee802154_aux_security_hdr *ash = (struct ieee802154_aux_security_hdr *)buf;
139 uint8_t len = IEEE802154_SECURITY_CF_LENGTH + IEEE802154_SECURITY_FRAME_COUNTER_LENGTH;
140
141 /* At least the asf is sized of: control field + frame counter */
142 if (*length < len) {
143 return NULL;
144 }
145
146 /* Only implicit key mode is supported for now */
147 if (ash->control.key_id_mode != IEEE802154_KEY_ID_MODE_IMPLICIT) {
148 return NULL;
149 }
150
151 /* Explicit key must have a key index != 0x00, see section 9.4.2.3 */
152 switch (ash->control.key_id_mode) {
153 case IEEE802154_KEY_ID_MODE_IMPLICIT:
154 break;
155 case IEEE802154_KEY_ID_MODE_INDEX:
156 len += IEEE802154_KEY_ID_FIELD_INDEX_LENGTH;
157 if (*length < len) {
158 return NULL;
159 }
160
161 if (!ash->kif.mode_1.key_index) {
162 return NULL;
163 }
164
165 break;
166 case IEEE802154_KEY_ID_MODE_SRC_4_INDEX:
167 len += IEEE802154_KEY_ID_FIELD_SRC_4_INDEX_LENGTH;
168 if (*length < len) {
169 return NULL;
170 }
171
172 if (!ash->kif.mode_2.key_index) {
173 return NULL;
174 }
175
176 break;
177 case IEEE802154_KEY_ID_MODE_SRC_8_INDEX:
178 len += IEEE802154_KEY_ID_FIELD_SRC_8_INDEX_LENGTH;
179 if (*length < len) {
180 return NULL;
181 }
182
183 if (!ash->kif.mode_3.key_index) {
184 return NULL;
185 }
186
187 break;
188 }
189
190 *p_buf = buf + len;
191 *length -= len;
192
193 return ash;
194 }
195 #endif /* CONFIG_NET_L2_IEEE802154_SECURITY */
196
ieee802514_beacon_header_length(uint8_t * buf,uint8_t length)197 int ieee802514_beacon_header_length(uint8_t *buf, uint8_t length)
198 {
199 struct ieee802154_beacon *beacon = (struct ieee802154_beacon *)buf;
200 struct ieee802154_pas_spec *pas;
201 uint8_t len = IEEE802154_BEACON_SF_SIZE + IEEE802154_BEACON_GTS_SPEC_SIZE;
202
203 if (length < len) {
204 return -EINVAL;
205 }
206
207 /* see section 7.3.1.5 on how to calculate GTS length */
208 if (beacon->gts.desc_count) {
209 len += IEEE802154_BEACON_GTS_DIR_SIZE +
210 beacon->gts.desc_count * IEEE802154_BEACON_GTS_SIZE;
211 }
212
213 if (length < len) {
214 return -EINVAL;
215 }
216
217 /* see section 7.3.1.6 on how to calculate pending address length */
218 pas = (struct ieee802154_pas_spec *)buf + len;
219
220 len += IEEE802154_BEACON_PAS_SPEC_SIZE;
221 if (length < len) {
222 return -EINVAL;
223 }
224
225 if (pas->nb_sap || pas->nb_eap) {
226 len += (pas->nb_sap * IEEE802154_SHORT_ADDR_LENGTH) +
227 (pas->nb_eap * IEEE802154_EXT_ADDR_LENGTH);
228 }
229
230 if (length < len) {
231 return -EINVAL;
232 }
233
234 return len;
235 }
236
validate_mac_command_cfi_to_mhr(struct ieee802154_mhr * mhr,bool ack_requested,bool has_pan_id,uint8_t src_bf,bool src_pan_brdcst_chk,uint8_t dst_bf,bool dst_brdcst_chk)237 static inline bool validate_mac_command_cfi_to_mhr(struct ieee802154_mhr *mhr,
238 bool ack_requested, bool has_pan_id,
239 uint8_t src_bf, bool src_pan_brdcst_chk,
240 uint8_t dst_bf, bool dst_brdcst_chk)
241 {
242 if (mhr->fs->fc.ar != ack_requested || mhr->fs->fc.pan_id_comp == has_pan_id) {
243 return false;
244 }
245
246 if (!(BIT(mhr->fs->fc.src_addr_mode) & src_bf) ||
247 !(BIT(mhr->fs->fc.dst_addr_mode) & dst_bf)) {
248 return false;
249 }
250
251 if (src_pan_brdcst_chk) {
252 if (mhr->src_addr->plain.pan_id != IEEE802154_BROADCAST_PAN_ID) {
253 return false;
254 }
255 }
256
257 if (dst_brdcst_chk) {
258 /* broadcast address is symmetric so no need to swap byte order */
259 if (mhr->dst_addr->plain.addr.short_addr != IEEE802154_BROADCAST_ADDRESS) {
260 return false;
261 }
262 }
263
264 return true;
265 }
266
validate_mac_command(struct ieee802154_mpdu * mpdu,uint8_t * buf,uint8_t length)267 static inline bool validate_mac_command(struct ieee802154_mpdu *mpdu, uint8_t *buf, uint8_t length)
268 {
269 struct ieee802154_command *command = (struct ieee802154_command *)buf;
270 uint8_t len = IEEE802154_CMD_CFI_LENGTH;
271 bool src_pan_brdcst_chk = false;
272 uint8_t src_bf = 0, dst_bf = 0;
273 bool dst_brdcst_chk = false;
274 bool ack_requested = false;
275 bool has_pan_id = true;
276
277 if (length < len) {
278 return false;
279 }
280
281 switch (command->cfi) {
282 case IEEE802154_CFI_UNKNOWN:
283 return false;
284 case IEEE802154_CFI_ASSOCIATION_REQUEST:
285 len += IEEE802154_CMD_ASSOC_REQ_LENGTH;
286 ack_requested = true;
287 src_bf = BIT(IEEE802154_ADDR_MODE_EXTENDED);
288 src_pan_brdcst_chk = true;
289 dst_bf = BIT(IEEE802154_ADDR_MODE_SHORT) | BIT(IEEE802154_ADDR_MODE_EXTENDED);
290
291 break;
292 case IEEE802154_CFI_ASSOCIATION_RESPONSE:
293 len += IEEE802154_CMD_ASSOC_RES_LENGTH;
294 __fallthrough;
295 case IEEE802154_CFI_DISASSOCIATION_NOTIFICATION:
296 if (command->cfi == IEEE802154_CFI_DISASSOCIATION_NOTIFICATION) {
297 len += IEEE802154_CMD_DISASSOC_NOTE_LENGTH;
298 dst_bf = BIT(IEEE802154_ADDR_MODE_SHORT);
299 }
300 __fallthrough;
301 case IEEE802154_CFI_PAN_ID_CONFLICT_NOTIFICATION:
302 ack_requested = true;
303 has_pan_id = false;
304 src_bf = BIT(IEEE802154_ADDR_MODE_EXTENDED);
305 dst_bf |= BIT(IEEE802154_ADDR_MODE_EXTENDED);
306
307 break;
308 case IEEE802154_CFI_DATA_REQUEST:
309 ack_requested = true;
310 src_bf = BIT(IEEE802154_ADDR_MODE_SHORT) | BIT(IEEE802154_ADDR_MODE_EXTENDED);
311
312 if (mpdu->mhr.fs->fc.dst_addr_mode == IEEE802154_ADDR_MODE_NONE) {
313 dst_bf = BIT(IEEE802154_ADDR_MODE_NONE);
314 } else {
315 has_pan_id = false;
316 dst_bf = BIT(IEEE802154_ADDR_MODE_SHORT) |
317 BIT(IEEE802154_ADDR_MODE_EXTENDED);
318 }
319
320 break;
321 case IEEE802154_CFI_ORPHAN_NOTIFICATION:
322 has_pan_id = false;
323 src_bf = BIT(IEEE802154_ADDR_MODE_EXTENDED);
324 dst_bf = BIT(IEEE802154_ADDR_MODE_SHORT);
325
326 break;
327 case IEEE802154_CFI_BEACON_REQUEST:
328 src_bf = BIT(IEEE802154_ADDR_MODE_NONE);
329 dst_bf = BIT(IEEE802154_ADDR_MODE_SHORT);
330 dst_brdcst_chk = true;
331
332 break;
333 case IEEE802154_CFI_COORDINATOR_REALIGNEMENT:
334 len += IEEE802154_CMD_COORD_REALIGN_LENGTH;
335 src_bf = BIT(IEEE802154_ADDR_MODE_EXTENDED);
336
337 if (mpdu->mhr.fs->fc.dst_addr_mode == IEEE802154_ADDR_MODE_SHORT) {
338 dst_bf = BIT(IEEE802154_ADDR_MODE_SHORT);
339 dst_brdcst_chk = true;
340 } else {
341 dst_bf = BIT(IEEE802154_ADDR_MODE_EXTENDED);
342 }
343
344 break;
345 case IEEE802154_CFI_GTS_REQUEST:
346 len += IEEE802154_GTS_REQUEST_LENGTH;
347 ack_requested = true;
348 src_bf = BIT(IEEE802154_ADDR_MODE_SHORT);
349 dst_bf = BIT(IEEE802154_ADDR_MODE_NONE);
350
351 break;
352 default:
353 return false;
354 }
355
356 if (length < len) {
357 return false;
358 }
359
360 if (!validate_mac_command_cfi_to_mhr(&mpdu->mhr, ack_requested, has_pan_id, src_bf,
361 src_pan_brdcst_chk, dst_bf,
362 dst_brdcst_chk)) {
363 return false;
364 }
365
366 mpdu->command = command;
367
368 return true;
369 }
370
validate_payload_and_mfr(struct ieee802154_mpdu * mpdu,uint8_t * buf,uint8_t * p_buf,uint8_t length)371 static inline bool validate_payload_and_mfr(struct ieee802154_mpdu *mpdu, uint8_t *buf,
372 uint8_t *p_buf, uint8_t length)
373 {
374 uint8_t type = mpdu->mhr.fs->fc.frame_type;
375
376 NET_DBG("Header size: %u, payload size %u", (uint32_t)(p_buf - buf), length);
377
378 if (type == IEEE802154_FRAME_TYPE_BEACON) {
379 if (ieee802514_beacon_header_length(p_buf, length) < 0) {
380 return false;
381 }
382 } else if (type == IEEE802154_FRAME_TYPE_DATA) {
383 /** A data frame embeds a payload */
384 if (length == 0U) {
385 return false;
386 }
387 } else if (type == IEEE802154_FRAME_TYPE_ACK) {
388 /** An ACK frame has no payload */
389 if (length) {
390 return false;
391 }
392 } else {
393 if (!validate_mac_command(mpdu, p_buf, length)) {
394 return false;
395 }
396 }
397
398 mpdu->payload_length = length;
399
400 if (length) {
401 mpdu->payload = (void *)p_buf;
402 } else {
403 mpdu->payload = NULL;
404 }
405
406 return true;
407 }
408
ieee802154_validate_frame(uint8_t * buf,uint8_t length,struct ieee802154_mpdu * mpdu)409 bool ieee802154_validate_frame(uint8_t *buf, uint8_t length, struct ieee802154_mpdu *mpdu)
410 {
411 uint8_t *p_buf = NULL;
412
413 if (length > IEEE802154_MTU || length < IEEE802154_MIN_LENGTH) {
414 NET_DBG("Wrong packet length: %d", length);
415 return false;
416 }
417
418 mpdu->mhr.fs = ieee802154_validate_fc_seq(buf, &p_buf, &length);
419 if (!mpdu->mhr.fs) {
420 return false;
421 }
422
423 /* TODO: Support later version's frame types */
424 if (mpdu->mhr.fs->fc.frame_type > IEEE802154_FRAME_TYPE_MAC_COMMAND) {
425 return false;
426 }
427
428 if (!validate_addr(p_buf, &p_buf, &length, mpdu->mhr.fs->fc.dst_addr_mode, false,
429 &mpdu->mhr.dst_addr) ||
430 !validate_addr(p_buf, &p_buf, &length, mpdu->mhr.fs->fc.src_addr_mode,
431 (mpdu->mhr.fs->fc.pan_id_comp), &mpdu->mhr.src_addr)) {
432 return false;
433 }
434
435 #ifdef CONFIG_NET_L2_IEEE802154_SECURITY
436 if (mpdu->mhr.fs->fc.security_enabled) {
437 mpdu->mhr.aux_sec = ieee802154_validate_aux_security_hdr(p_buf, &p_buf, &length);
438 if (!mpdu->mhr.aux_sec) {
439 return false;
440 }
441 }
442 #endif
443
444 return validate_payload_and_mfr(mpdu, buf, p_buf, length);
445 }
446
ieee802154_compute_header_and_authtag_len(struct net_if * iface,struct net_linkaddr * dst,struct net_linkaddr * src,uint8_t * ll_hdr_len,uint8_t * authtag_len)447 void ieee802154_compute_header_and_authtag_len(struct net_if *iface, struct net_linkaddr *dst,
448 struct net_linkaddr *src, uint8_t *ll_hdr_len,
449 uint8_t *authtag_len)
450 {
451 uint8_t hdr_len = sizeof(struct ieee802154_fcf_seq), tag_len = 0;
452 bool broadcast = (dst->len == 0);
453
454 /* PAN ID */
455 hdr_len += IEEE802154_PAN_ID_LENGTH;
456
457 /* Destination Address - see get_dst_addr_mode() */
458 hdr_len += broadcast ? IEEE802154_SHORT_ADDR_LENGTH : dst->len;
459
460 /* Source Address - see data_addr_to_fs_settings() */
461 hdr_len += (src->len > 0) ? src->len : dst->len;
462
463 #ifdef CONFIG_NET_L2_IEEE802154_SECURITY
464 struct ieee802154_security_ctx *sec_ctx;
465 struct ieee802154_context *ctx;
466
467 if (broadcast) {
468 NET_DBG("Broadcast packets are not being encrypted.");
469 goto done;
470 }
471
472 ctx = (struct ieee802154_context *)net_if_l2_data(iface);
473
474 k_sem_take(&ctx->ctx_lock, K_FOREVER);
475
476 sec_ctx = &ctx->sec_ctx;
477 if (sec_ctx->level == IEEE802154_SECURITY_LEVEL_NONE) {
478 goto release;
479 }
480
481 /* Compute aux-sec hdr size and add it to hdr_len */
482 hdr_len += IEEE802154_SECURITY_CF_LENGTH + IEEE802154_SECURITY_FRAME_COUNTER_LENGTH;
483
484 switch (sec_ctx->key_mode) {
485 case IEEE802154_KEY_ID_MODE_IMPLICIT:
486 /* The only mode supported for now,
487 * generate_aux_securiy_hdr() will fail on other modes
488 */
489 break;
490 case IEEE802154_KEY_ID_MODE_INDEX:
491 hdr_len += IEEE802154_KEY_ID_FIELD_INDEX_LENGTH;
492 break;
493 case IEEE802154_KEY_ID_MODE_SRC_4_INDEX:
494 hdr_len += IEEE802154_KEY_ID_FIELD_SRC_4_INDEX_LENGTH;
495 break;
496 case IEEE802154_KEY_ID_MODE_SRC_8_INDEX:
497 hdr_len += IEEE802154_KEY_ID_FIELD_SRC_8_INDEX_LENGTH;
498 }
499
500 if (sec_ctx->level < IEEE802154_SECURITY_LEVEL_ENC) {
501 tag_len = level_2_authtag_len[sec_ctx->level];
502 } else {
503 tag_len = level_2_authtag_len[sec_ctx->level - 4U];
504 }
505
506 release:
507 k_sem_give(&ctx->ctx_lock);
508 done:
509 #endif /* CONFIG_NET_L2_IEEE802154_SECURITY */
510
511 NET_DBG("Computed header size %u", hdr_len);
512 NET_DBG("Computed authtag size: %u", tag_len);
513
514 *ll_hdr_len = hdr_len;
515 *authtag_len = tag_len;
516 }
517
generate_fcf_grounds(uint8_t ** p_buf,bool ack_requested)518 static inline struct ieee802154_fcf_seq *generate_fcf_grounds(uint8_t **p_buf, bool ack_requested)
519 {
520 struct ieee802154_fcf_seq *fs;
521
522 fs = (struct ieee802154_fcf_seq *)*p_buf;
523
524 fs->fc.security_enabled = 0U;
525 fs->fc.frame_pending = 0U;
526 fs->fc.ar = ack_requested;
527 fs->fc.pan_id_comp = 0U;
528 fs->fc.reserved = 0U;
529 /* We support version 2006 only for now */
530 fs->fc.seq_num_suppr = 0U;
531 fs->fc.ie_list = 0U;
532 fs->fc.frame_version = IEEE802154_VERSION_802154_2006;
533
534 *p_buf += sizeof(struct ieee802154_fcf_seq);
535
536 return fs;
537 }
538
get_dst_addr_mode(struct net_linkaddr * dst,bool * broadcast)539 static inline enum ieee802154_addressing_mode get_dst_addr_mode(struct net_linkaddr *dst,
540 bool *broadcast)
541 {
542 if (dst->len == 0) {
543 NET_DBG("Broadcast destination");
544 *broadcast = true;
545 return IEEE802154_ADDR_MODE_SHORT;
546 }
547
548 if (dst->len == IEEE802154_SHORT_ADDR_LENGTH) {
549 uint16_t short_addr = ntohs(*(uint16_t *)(dst->addr));
550 *broadcast = (short_addr == IEEE802154_BROADCAST_ADDRESS);
551 return IEEE802154_ADDR_MODE_SHORT;
552 } else {
553 *broadcast = false;
554 }
555
556 if (dst->len == IEEE802154_EXT_ADDR_LENGTH) {
557 return IEEE802154_ADDR_MODE_EXTENDED;
558 }
559
560 return IEEE802154_ADDR_MODE_NONE;
561 }
562
data_addr_to_fs_settings(struct net_linkaddr * dst,struct ieee802154_fcf_seq * fs,struct ieee802154_frame_params * params)563 static inline bool data_addr_to_fs_settings(struct net_linkaddr *dst, struct ieee802154_fcf_seq *fs,
564 struct ieee802154_frame_params *params)
565 {
566 bool broadcast;
567
568 fs->fc.dst_addr_mode = get_dst_addr_mode(dst, &broadcast);
569 if (fs->fc.dst_addr_mode != IEEE802154_ADDR_MODE_NONE) {
570 fs->fc.pan_id_comp = 1U;
571
572 if (broadcast) {
573 params->dst.short_addr = IEEE802154_BROADCAST_ADDRESS;
574 params->dst.len = IEEE802154_SHORT_ADDR_LENGTH;
575 fs->fc.ar = 0U;
576 } else if (dst->len == IEEE802154_SHORT_ADDR_LENGTH) {
577 params->dst.short_addr = ntohs(*(uint16_t *)(dst->addr));
578 params->dst.len = IEEE802154_SHORT_ADDR_LENGTH;
579 } else {
580 __ASSERT_NO_MSG(dst->len == IEEE802154_EXT_ADDR_LENGTH);
581 memcpy(params->dst.ext_addr, dst->addr, sizeof(params->dst.ext_addr));
582 params->dst.len = IEEE802154_EXT_ADDR_LENGTH;
583 }
584 }
585
586 if (params->short_addr) {
587 fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_SHORT;
588 } else {
589 fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
590 }
591
592 return broadcast;
593 }
594
generate_addressing_fields(struct ieee802154_context * ctx,struct ieee802154_fcf_seq * fs,struct ieee802154_frame_params * params,uint8_t * p_buf)595 static uint8_t *generate_addressing_fields(struct ieee802154_context *ctx,
596 struct ieee802154_fcf_seq *fs,
597 struct ieee802154_frame_params *params, uint8_t *p_buf)
598 {
599 struct ieee802154_address_field *address_field;
600 struct ieee802154_address *src_addr;
601
602 /* destination address */
603 if (fs->fc.dst_addr_mode != IEEE802154_ADDR_MODE_NONE) {
604 address_field = (struct ieee802154_address_field *)p_buf;
605
606 address_field->plain.pan_id = sys_cpu_to_le16(params->dst.pan_id);
607 p_buf += IEEE802154_PAN_ID_LENGTH;
608
609 if (fs->fc.dst_addr_mode == IEEE802154_ADDR_MODE_SHORT) {
610 address_field->plain.addr.short_addr =
611 sys_cpu_to_le16(params->dst.short_addr);
612 p_buf += IEEE802154_SHORT_ADDR_LENGTH;
613 } else {
614 sys_memcpy_swap(address_field->plain.addr.ext_addr, params->dst.ext_addr,
615 IEEE802154_EXT_ADDR_LENGTH);
616 p_buf += IEEE802154_EXT_ADDR_LENGTH;
617 }
618 }
619
620 /* source address */
621 if (fs->fc.src_addr_mode == IEEE802154_ADDR_MODE_NONE) {
622 return p_buf;
623 }
624
625 address_field = (struct ieee802154_address_field *)p_buf;
626
627 if (fs->fc.pan_id_comp) {
628 src_addr = &address_field->comp.addr;
629 } else {
630 address_field->plain.pan_id = sys_cpu_to_le16(params->pan_id);
631 src_addr = &address_field->plain.addr;
632 p_buf += IEEE802154_PAN_ID_LENGTH;
633 }
634
635 if (fs->fc.src_addr_mode == IEEE802154_ADDR_MODE_SHORT) {
636 src_addr->short_addr = sys_cpu_to_le16(params->short_addr);
637 p_buf += IEEE802154_SHORT_ADDR_LENGTH;
638 } else {
639 memcpy(src_addr->ext_addr, ctx->ext_addr, IEEE802154_EXT_ADDR_LENGTH);
640 p_buf += IEEE802154_EXT_ADDR_LENGTH;
641 }
642
643 return p_buf;
644 }
645
646 #ifdef CONFIG_NET_L2_IEEE802154_SECURITY
generate_aux_security_hdr(struct ieee802154_security_ctx * sec_ctx,uint8_t * p_buf)647 static uint8_t *generate_aux_security_hdr(struct ieee802154_security_ctx *sec_ctx, uint8_t *p_buf)
648 {
649 struct ieee802154_aux_security_hdr *aux_sec;
650
651 if (sec_ctx->level == IEEE802154_SECURITY_LEVEL_NONE) {
652 return p_buf;
653 }
654
655 if (sec_ctx->key_mode != IEEE802154_KEY_ID_MODE_IMPLICIT) {
656 /* TODO: Support other key ID modes. */
657 return NULL;
658 }
659
660 aux_sec = (struct ieee802154_aux_security_hdr *)p_buf;
661
662 aux_sec->control.security_level = sec_ctx->level;
663 aux_sec->control.key_id_mode = sec_ctx->key_mode;
664 aux_sec->control.reserved = 0U;
665
666 aux_sec->frame_counter = sys_cpu_to_le32(sec_ctx->frame_counter);
667
668 return p_buf + IEEE802154_SECURITY_CF_LENGTH + IEEE802154_SECURITY_FRAME_COUNTER_LENGTH;
669 }
670 #endif /* CONFIG_NET_L2_IEEE802154_SECURITY */
671
ieee802154_create_data_frame(struct ieee802154_context * ctx,struct net_linkaddr * dst,struct net_linkaddr * src,struct net_buf * buf,uint8_t ll_hdr_len)672 bool ieee802154_create_data_frame(struct ieee802154_context *ctx, struct net_linkaddr *dst,
673 struct net_linkaddr *src, struct net_buf *buf, uint8_t ll_hdr_len)
674 {
675 struct ieee802154_frame_params params = {0};
676 struct ieee802154_fcf_seq *fs;
677 uint8_t *p_buf = buf->data;
678 uint8_t *buf_start = p_buf;
679 bool ret = false;
680 bool broadcast;
681
682 k_sem_take(&ctx->ctx_lock, K_FOREVER);
683
684 fs = generate_fcf_grounds(&p_buf, ctx->ack_requested);
685
686 fs->fc.frame_type = IEEE802154_FRAME_TYPE_DATA;
687 fs->sequence = ctx->sequence++;
688
689 params.dst.pan_id = ctx->pan_id;
690 params.pan_id = ctx->pan_id;
691 if (src->len == IEEE802154_SHORT_ADDR_LENGTH) {
692 params.short_addr = ntohs(*(uint16_t *)(src->addr));
693 if (ctx->short_addr != params.short_addr) {
694 goto out;
695 }
696 } else {
697 uint8_t ext_addr_le[IEEE802154_EXT_ADDR_LENGTH];
698
699 if (src->len != IEEE802154_EXT_ADDR_LENGTH) {
700 goto out;
701 }
702
703 sys_memcpy_swap(ext_addr_le, src->addr, IEEE802154_EXT_ADDR_LENGTH);
704 if (memcmp(ctx->ext_addr, ext_addr_le, src->len)) {
705 goto out;
706 }
707 }
708
709 broadcast = data_addr_to_fs_settings(dst, fs, ¶ms);
710
711 p_buf = generate_addressing_fields(ctx, fs, ¶ms, p_buf);
712
713 #ifdef CONFIG_NET_L2_IEEE802154_SECURITY
714 uint8_t level, authtag_len, payload_len;
715
716 if (broadcast) {
717 /* TODO: This may not always be correct. */
718 NET_DBG("No security hdr needed: broadcasting");
719 goto no_security_hdr;
720 }
721
722 if (ctx->sec_ctx.level == IEEE802154_SECURITY_LEVEL_NONE) {
723 NET_WARN("IEEE 802.15.4 security is enabled but has not been configured.");
724 goto no_security_hdr;
725 }
726
727 fs->fc.security_enabled = 1U;
728
729 p_buf = generate_aux_security_hdr(&ctx->sec_ctx, p_buf);
730 if (!p_buf) {
731 NET_ERR("Unsupported key mode.");
732 goto out;
733 }
734
735 level = ctx->sec_ctx.level;
736 if (level >= IEEE802154_SECURITY_LEVEL_ENC) {
737 level -= 4U;
738 }
739
740 /* Let's encrypt/auth only in the end, if needed */
741 authtag_len = level_2_authtag_len[level];
742 payload_len = buf->len - ll_hdr_len - authtag_len;
743 if (!ieee802154_encrypt_auth(&ctx->sec_ctx, buf_start, ll_hdr_len,
744 payload_len, authtag_len, ctx->ext_addr)) {
745 goto out;
746 };
747
748 no_security_hdr:
749 #endif /* CONFIG_NET_L2_IEEE802154_SECURITY */
750 if ((p_buf - buf_start) != ll_hdr_len) {
751 /* ll_hdr_len was too small? We probably overwrote payload bytes */
752 NET_ERR("Could not generate data frame %zu vs %u", (p_buf - buf_start), ll_hdr_len);
753 goto out;
754 }
755
756 dbg_print_fs(fs);
757
758 ret = true;
759
760 out:
761 k_sem_give(&ctx->ctx_lock);
762 return ret;
763 }
764
765 #ifdef CONFIG_NET_L2_IEEE802154_RFD
766
cfi_to_fs_settings(enum ieee802154_cfi cfi,struct ieee802154_fcf_seq * fs,struct ieee802154_frame_params * params)767 static inline bool cfi_to_fs_settings(enum ieee802154_cfi cfi, struct ieee802154_fcf_seq *fs,
768 struct ieee802154_frame_params *params)
769 {
770 switch (cfi) {
771 case IEEE802154_CFI_DISASSOCIATION_NOTIFICATION:
772 fs->fc.pan_id_comp = 1U;
773
774 __fallthrough;
775 case IEEE802154_CFI_ASSOCIATION_REQUEST:
776 fs->fc.ar = 1U;
777 fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
778
779 if (params->dst.len == IEEE802154_SHORT_ADDR_LENGTH) {
780 fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_SHORT;
781 } else {
782 fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
783 }
784
785 break;
786 case IEEE802154_CFI_ASSOCIATION_RESPONSE:
787 case IEEE802154_CFI_PAN_ID_CONFLICT_NOTIFICATION:
788 fs->fc.ar = 1U;
789 fs->fc.pan_id_comp = 1U;
790 fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
791 fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
792
793 break;
794 case IEEE802154_CFI_DATA_REQUEST:
795 fs->fc.ar = 1U;
796 /* TODO: src/dst addr mode: see section 7.5.5 */
797
798 break;
799 case IEEE802154_CFI_ORPHAN_NOTIFICATION:
800 fs->fc.pan_id_comp = 1U;
801 fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
802 fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_SHORT;
803
804 break;
805 case IEEE802154_CFI_BEACON_REQUEST:
806 fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_NONE;
807 fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_SHORT;
808 break;
809 case IEEE802154_CFI_COORDINATOR_REALIGNEMENT:
810 fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
811 /* TODO: ack_requested and dst addr mode: see section 7.5.10 */
812
813 break;
814 case IEEE802154_CFI_GTS_REQUEST:
815 fs->fc.ar = 1U;
816 fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_SHORT;
817 fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_NONE;
818
819 break;
820 default:
821 return false;
822 }
823
824 return true;
825 }
826
mac_command_length(enum ieee802154_cfi cfi)827 static inline uint8_t mac_command_length(enum ieee802154_cfi cfi)
828 {
829 uint8_t length = 1U; /* cfi is at least present */
830
831 switch (cfi) {
832 case IEEE802154_CFI_ASSOCIATION_REQUEST:
833 case IEEE802154_CFI_DISASSOCIATION_NOTIFICATION:
834 case IEEE802154_CFI_GTS_REQUEST:
835 length += 1U;
836 break;
837 case IEEE802154_CFI_ASSOCIATION_RESPONSE:
838 length += 3U;
839 break;
840 case IEEE802154_CFI_COORDINATOR_REALIGNEMENT:
841 length += 8U;
842 break;
843 default:
844 break;
845 }
846
847 return length;
848 }
849
ieee802154_create_mac_cmd_frame(struct net_if * iface,enum ieee802154_cfi type,struct ieee802154_frame_params * params)850 struct net_pkt *ieee802154_create_mac_cmd_frame(struct net_if *iface, enum ieee802154_cfi type,
851 struct ieee802154_frame_params *params)
852 {
853 struct ieee802154_context *ctx = net_if_l2_data(iface);
854 struct ieee802154_fcf_seq *fs;
855 struct net_pkt *pkt = NULL;
856 uint8_t *p_buf, *p_start;
857
858 k_sem_take(&ctx->ctx_lock, K_FOREVER);
859
860 /* It would be costly to compute the size when actual frames are never
861 * bigger than IEEE802154_MTU bytes less the FCS size, so let's allocate that
862 * size as buffer.
863 */
864 pkt = net_pkt_alloc_with_buffer(iface, IEEE802154_MTU, AF_UNSPEC, 0, BUF_TIMEOUT);
865 if (!pkt) {
866 goto out;
867 }
868
869 p_buf = net_pkt_data(pkt);
870 p_start = p_buf;
871
872 fs = generate_fcf_grounds(
873 &p_buf, type == IEEE802154_CFI_BEACON_REQUEST ? false : ctx->ack_requested);
874
875 fs->fc.frame_type = IEEE802154_FRAME_TYPE_MAC_COMMAND;
876 fs->sequence = ctx->sequence++;
877
878 if (!cfi_to_fs_settings(type, fs, params)) {
879 goto error;
880 }
881
882 p_buf = generate_addressing_fields(ctx, fs, params, p_buf);
883
884 net_buf_add(pkt->buffer, p_buf - p_start);
885
886 /* Let's insert the cfi */
887 ((struct ieee802154_command *)p_buf)->cfi = type;
888
889 dbg_print_fs(fs);
890
891 goto out;
892
893 error:
894 net_pkt_unref(pkt);
895 pkt = NULL;
896
897 out:
898 k_sem_give(&ctx->ctx_lock);
899 return pkt;
900 }
901
ieee802154_mac_cmd_finalize(struct net_pkt * pkt,enum ieee802154_cfi type)902 void ieee802154_mac_cmd_finalize(struct net_pkt *pkt, enum ieee802154_cfi type)
903 {
904 net_buf_add(pkt->buffer, mac_command_length(type));
905 }
906
907 #endif /* CONFIG_NET_L2_IEEE802154_RFD */
908
ieee802154_create_ack_frame(struct net_if * iface,struct net_pkt * pkt,uint8_t seq)909 bool ieee802154_create_ack_frame(struct net_if *iface, struct net_pkt *pkt, uint8_t seq)
910 {
911 uint8_t *p_buf = net_pkt_data(pkt);
912 struct ieee802154_fcf_seq *fs;
913
914 if (!p_buf) {
915 return false;
916 }
917
918 fs = generate_fcf_grounds(&p_buf, false);
919
920 fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_NONE;
921 fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_NONE;
922
923 fs->fc.frame_type = IEEE802154_FRAME_TYPE_ACK;
924 fs->sequence = seq;
925
926 net_buf_add(pkt->buffer, IEEE802154_ACK_PKT_LENGTH);
927
928 return true;
929 }
930
931 #ifdef CONFIG_NET_L2_IEEE802154_SECURITY
ieee802154_decipher_data_frame(struct net_if * iface,struct net_pkt * pkt,struct ieee802154_mpdu * mpdu)932 bool ieee802154_decipher_data_frame(struct net_if *iface, struct net_pkt *pkt,
933 struct ieee802154_mpdu *mpdu)
934 {
935 struct ieee802154_context *ctx = net_if_l2_data(iface);
936 uint8_t level, authtag_len, ll_hdr_len, payload_len;
937 struct ieee802154_mhr *mhr = &mpdu->mhr;
938 struct ieee802154_address *src;
939 bool ret = false;
940
941 k_sem_take(&ctx->ctx_lock, K_FOREVER);
942
943 if (!mhr->fs->fc.security_enabled) {
944 ret = true;
945 goto out;
946 }
947
948 level = ctx->sec_ctx.level;
949
950 /* Section 9.2.4: Incoming frame security procedure, Security Enabled field is set to one
951 *
952 * [...]
953 *
954 * a) Legacy security. If the Frame Version field of the frame to be unsecured is set to
955 * zero, the procedure shall return with a Status of UNSUPPORTED_LEGACY.
956 */
957 if (mhr->aux_sec->control.security_level != level) {
958 goto out;
959 }
960
961 if (level >= IEEE802154_SECURITY_LEVEL_ENC) {
962 level -= 4U;
963 }
964
965 authtag_len = level_2_authtag_len[level];
966 ll_hdr_len = (uint8_t *)mpdu->payload - net_pkt_data(pkt);
967 payload_len = net_pkt_get_len(pkt) - ll_hdr_len - authtag_len;
968
969 /* TODO: Handle src short address.
970 * This will require to look up in nbr cache with short addr
971 * in order to get the extended address related to it.
972 */
973 if (mhr->fs->fc.src_addr_mode != IEEE802154_ADDR_MODE_EXTENDED) {
974 NET_ERR("Only encrypting packages with extended source addresses is supported.");
975 goto out;
976 }
977
978 src = mhr->fs->fc.pan_id_comp ? &mhr->src_addr->comp.addr : &mhr->src_addr->plain.addr;
979
980 if (!ieee802154_decrypt_auth(&ctx->sec_ctx, net_pkt_data(pkt), ll_hdr_len, payload_len,
981 authtag_len, src->ext_addr,
982 sys_le32_to_cpu(mhr->aux_sec->frame_counter))) {
983 NET_ERR("Could not decipher the frame");
984 goto out;
985 }
986
987 /* We remove tag size from buf's length, it is now useless. */
988 pkt->buffer->len -= authtag_len;
989
990 ret = true;
991
992 out:
993 k_sem_give(&ctx->ctx_lock);
994 return ret;
995 }
996 #endif /* CONFIG_NET_L2_IEEE802154_SECURITY */
997
