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->addr;
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->addr ? src->len : dst->len;
462
463 #ifdef CONFIG_NET_L2_IEEE802154_SECURITY
464 if (broadcast) {
465 NET_DBG("Broadcast packets are not being encrypted.");
466 goto done;
467 }
468
469 struct ieee802154_context *ctx = (struct ieee802154_context *)net_if_l2_data(iface);
470
471 k_sem_take(&ctx->ctx_lock, K_FOREVER);
472
473 struct ieee802154_security_ctx *sec_ctx = &ctx->sec_ctx;
474 if (sec_ctx->level == IEEE802154_SECURITY_LEVEL_NONE) {
475 goto release;
476 }
477
478 /* Compute aux-sec hdr size and add it to hdr_len */
479 hdr_len += IEEE802154_SECURITY_CF_LENGTH + IEEE802154_SECURITY_FRAME_COUNTER_LENGTH;
480
481 switch (sec_ctx->key_mode) {
482 case IEEE802154_KEY_ID_MODE_IMPLICIT:
483 /* The only mode supported for now,
484 * generate_aux_securiy_hdr() will fail on other modes
485 */
486 break;
487 case IEEE802154_KEY_ID_MODE_INDEX:
488 hdr_len += IEEE802154_KEY_ID_FIELD_INDEX_LENGTH;
489 break;
490 case IEEE802154_KEY_ID_MODE_SRC_4_INDEX:
491 hdr_len += IEEE802154_KEY_ID_FIELD_SRC_4_INDEX_LENGTH;
492 break;
493 case IEEE802154_KEY_ID_MODE_SRC_8_INDEX:
494 hdr_len += IEEE802154_KEY_ID_FIELD_SRC_8_INDEX_LENGTH;
495 }
496
497 if (sec_ctx->level < IEEE802154_SECURITY_LEVEL_ENC) {
498 tag_len = level_2_authtag_len[sec_ctx->level];
499 } else {
500 tag_len = level_2_authtag_len[sec_ctx->level - 4U];
501 }
502
503 release:
504 k_sem_give(&ctx->ctx_lock);
505 done:
506 #endif /* CONFIG_NET_L2_IEEE802154_SECURITY */
507
508 NET_DBG("Computed header size %u", hdr_len);
509 NET_DBG("Computed authtag size: %u", tag_len);
510
511 *ll_hdr_len = hdr_len;
512 *authtag_len = tag_len;
513 }
514
generate_fcf_grounds(uint8_t ** p_buf,bool ack_requested)515 static inline struct ieee802154_fcf_seq *generate_fcf_grounds(uint8_t **p_buf, bool ack_requested)
516 {
517 struct ieee802154_fcf_seq *fs;
518
519 fs = (struct ieee802154_fcf_seq *)*p_buf;
520
521 fs->fc.security_enabled = 0U;
522 fs->fc.frame_pending = 0U;
523 fs->fc.ar = ack_requested;
524 fs->fc.pan_id_comp = 0U;
525 fs->fc.reserved = 0U;
526 /* We support version 2006 only for now */
527 fs->fc.seq_num_suppr = 0U;
528 fs->fc.ie_list = 0U;
529 fs->fc.frame_version = IEEE802154_VERSION_802154_2006;
530
531 *p_buf += sizeof(struct ieee802154_fcf_seq);
532
533 return fs;
534 }
535
get_dst_addr_mode(struct net_linkaddr * dst,bool * broadcast)536 static inline enum ieee802154_addressing_mode get_dst_addr_mode(struct net_linkaddr *dst,
537 bool *broadcast)
538 {
539 if (!dst->addr) {
540 NET_DBG("Broadcast destination");
541 *broadcast = true;
542 return IEEE802154_ADDR_MODE_SHORT;
543 }
544
545 if (dst->len == IEEE802154_SHORT_ADDR_LENGTH) {
546 uint16_t short_addr = ntohs(*(uint16_t *)(dst->addr));
547 *broadcast = (short_addr == IEEE802154_BROADCAST_ADDRESS);
548 return IEEE802154_ADDR_MODE_SHORT;
549 } else {
550 *broadcast = false;
551 }
552
553 if (dst->len == IEEE802154_EXT_ADDR_LENGTH) {
554 return IEEE802154_ADDR_MODE_EXTENDED;
555 }
556
557 return IEEE802154_ADDR_MODE_NONE;
558 }
559
data_addr_to_fs_settings(struct net_linkaddr * dst,struct ieee802154_fcf_seq * fs,struct ieee802154_frame_params * params)560 static inline bool data_addr_to_fs_settings(struct net_linkaddr *dst, struct ieee802154_fcf_seq *fs,
561 struct ieee802154_frame_params *params)
562 {
563 bool broadcast;
564
565 fs->fc.dst_addr_mode = get_dst_addr_mode(dst, &broadcast);
566 if (fs->fc.dst_addr_mode != IEEE802154_ADDR_MODE_NONE) {
567 fs->fc.pan_id_comp = 1U;
568
569 if (broadcast) {
570 params->dst.short_addr = IEEE802154_BROADCAST_ADDRESS;
571 params->dst.len = IEEE802154_SHORT_ADDR_LENGTH;
572 fs->fc.ar = 0U;
573 } else if (dst->len == IEEE802154_SHORT_ADDR_LENGTH) {
574 params->dst.short_addr = ntohs(*(uint16_t *)(dst->addr));
575 params->dst.len = IEEE802154_SHORT_ADDR_LENGTH;
576 } else {
577 __ASSERT_NO_MSG(dst->len == IEEE802154_EXT_ADDR_LENGTH);
578 memcpy(params->dst.ext_addr, dst->addr, sizeof(params->dst.ext_addr));
579 params->dst.len = IEEE802154_EXT_ADDR_LENGTH;
580 }
581 }
582
583 if (params->short_addr) {
584 fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_SHORT;
585 } else {
586 fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
587 }
588
589 return broadcast;
590 }
591
generate_addressing_fields(struct ieee802154_context * ctx,struct ieee802154_fcf_seq * fs,struct ieee802154_frame_params * params,uint8_t * p_buf)592 static uint8_t *generate_addressing_fields(struct ieee802154_context *ctx,
593 struct ieee802154_fcf_seq *fs,
594 struct ieee802154_frame_params *params, uint8_t *p_buf)
595 {
596 struct ieee802154_address_field *address_field;
597
598 /* destination address */
599 if (fs->fc.dst_addr_mode != IEEE802154_ADDR_MODE_NONE) {
600 address_field = (struct ieee802154_address_field *)p_buf;
601
602 address_field->plain.pan_id = sys_cpu_to_le16(params->dst.pan_id);
603 p_buf += IEEE802154_PAN_ID_LENGTH;
604
605 if (fs->fc.dst_addr_mode == IEEE802154_ADDR_MODE_SHORT) {
606 address_field->plain.addr.short_addr =
607 sys_cpu_to_le16(params->dst.short_addr);
608 p_buf += IEEE802154_SHORT_ADDR_LENGTH;
609 } else {
610 sys_memcpy_swap(address_field->plain.addr.ext_addr, params->dst.ext_addr,
611 IEEE802154_EXT_ADDR_LENGTH);
612 p_buf += IEEE802154_EXT_ADDR_LENGTH;
613 }
614 }
615
616 /* source address */
617 if (fs->fc.src_addr_mode == IEEE802154_ADDR_MODE_NONE) {
618 return p_buf;
619 }
620
621 address_field = (struct ieee802154_address_field *)p_buf;
622 struct ieee802154_address *src_addr;
623
624 if (fs->fc.pan_id_comp) {
625 src_addr = &address_field->comp.addr;
626 } else {
627 address_field->plain.pan_id = sys_cpu_to_le16(params->pan_id);
628 src_addr = &address_field->plain.addr;
629 p_buf += IEEE802154_PAN_ID_LENGTH;
630 }
631
632 if (fs->fc.src_addr_mode == IEEE802154_ADDR_MODE_SHORT) {
633 src_addr->short_addr = sys_cpu_to_le16(params->short_addr);
634 p_buf += IEEE802154_SHORT_ADDR_LENGTH;
635 } else {
636 memcpy(src_addr->ext_addr, ctx->ext_addr, IEEE802154_EXT_ADDR_LENGTH);
637 p_buf += IEEE802154_EXT_ADDR_LENGTH;
638 }
639
640 return p_buf;
641 }
642
643 #ifdef CONFIG_NET_L2_IEEE802154_SECURITY
generate_aux_security_hdr(struct ieee802154_security_ctx * sec_ctx,uint8_t * p_buf)644 static uint8_t *generate_aux_security_hdr(struct ieee802154_security_ctx *sec_ctx, uint8_t *p_buf)
645 {
646 struct ieee802154_aux_security_hdr *aux_sec;
647
648 if (sec_ctx->level == IEEE802154_SECURITY_LEVEL_NONE) {
649 return p_buf;
650 }
651
652 if (sec_ctx->key_mode != IEEE802154_KEY_ID_MODE_IMPLICIT) {
653 /* TODO: Support other key ID modes. */
654 return NULL;
655 }
656
657 aux_sec = (struct ieee802154_aux_security_hdr *)p_buf;
658
659 aux_sec->control.security_level = sec_ctx->level;
660 aux_sec->control.key_id_mode = sec_ctx->key_mode;
661 aux_sec->control.reserved = 0U;
662
663 aux_sec->frame_counter = sys_cpu_to_le32(sec_ctx->frame_counter);
664
665 return p_buf + IEEE802154_SECURITY_CF_LENGTH + IEEE802154_SECURITY_FRAME_COUNTER_LENGTH;
666 }
667 #endif /* CONFIG_NET_L2_IEEE802154_SECURITY */
668
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)669 bool ieee802154_create_data_frame(struct ieee802154_context *ctx, struct net_linkaddr *dst,
670 struct net_linkaddr *src, struct net_buf *buf, uint8_t ll_hdr_len)
671 {
672 struct ieee802154_frame_params params = {0};
673 struct ieee802154_fcf_seq *fs;
674 uint8_t *p_buf = buf->data;
675 uint8_t *buf_start = p_buf;
676 bool ret = false;
677 bool broadcast;
678
679 k_sem_take(&ctx->ctx_lock, K_FOREVER);
680
681 fs = generate_fcf_grounds(&p_buf, ctx->ack_requested);
682
683 fs->fc.frame_type = IEEE802154_FRAME_TYPE_DATA;
684 fs->sequence = ctx->sequence++;
685
686 params.dst.pan_id = ctx->pan_id;
687 params.pan_id = ctx->pan_id;
688 if (src->addr && src->len == IEEE802154_SHORT_ADDR_LENGTH) {
689 params.short_addr = ntohs(*(uint16_t *)(src->addr));
690 if (ctx->short_addr != params.short_addr) {
691 goto out;
692 }
693 } else {
694 if (src->len != IEEE802154_EXT_ADDR_LENGTH) {
695 goto out;
696 }
697
698 uint8_t ext_addr_le[IEEE802154_EXT_ADDR_LENGTH];
699
700 sys_memcpy_swap(ext_addr_le, src->addr, IEEE802154_EXT_ADDR_LENGTH);
701 if (memcmp(ctx->ext_addr, ext_addr_le, src->len)) {
702 goto out;
703 }
704 }
705
706 broadcast = data_addr_to_fs_settings(dst, fs, ¶ms);
707
708 p_buf = generate_addressing_fields(ctx, fs, ¶ms, p_buf);
709
710 #ifdef CONFIG_NET_L2_IEEE802154_SECURITY
711 if (broadcast) {
712 /* TODO: This may not always be correct. */
713 NET_DBG("No security hdr needed: broadcasting");
714 goto no_security_hdr;
715 }
716
717 if (ctx->sec_ctx.level == IEEE802154_SECURITY_LEVEL_NONE) {
718 NET_WARN("IEEE 802.15.4 security is enabled but has not been configured.");
719 goto no_security_hdr;
720 }
721
722 fs->fc.security_enabled = 1U;
723
724 p_buf = generate_aux_security_hdr(&ctx->sec_ctx, p_buf);
725 if (!p_buf) {
726 NET_ERR("Unsupported key mode.");
727 goto out;
728 }
729
730 uint8_t level = ctx->sec_ctx.level;
731
732 if (level >= IEEE802154_SECURITY_LEVEL_ENC) {
733 level -= 4U;
734 }
735
736 uint8_t authtag_len = level_2_authtag_len[level];
737 uint8_t payload_len = buf->len - ll_hdr_len - authtag_len;
738
739 /* Let's encrypt/auth only in the end, if needed */
740 if (!ieee802154_encrypt_auth(&ctx->sec_ctx, buf_start, ll_hdr_len,
741 payload_len, authtag_len, ctx->ext_addr)) {
742 goto out;
743 };
744
745 no_security_hdr:
746 #endif /* CONFIG_NET_L2_IEEE802154_SECURITY */
747 if ((p_buf - buf_start) != ll_hdr_len) {
748 /* ll_hdr_len was too small? We probably overwrote payload bytes */
749 NET_ERR("Could not generate data frame %zu vs %u", (p_buf - buf_start), ll_hdr_len);
750 goto out;
751 }
752
753 dbg_print_fs(fs);
754
755 ret = true;
756
757 out:
758 k_sem_give(&ctx->ctx_lock);
759 return ret;
760 }
761
762 #ifdef CONFIG_NET_L2_IEEE802154_RFD
763
cfi_to_fs_settings(enum ieee802154_cfi cfi,struct ieee802154_fcf_seq * fs,struct ieee802154_frame_params * params)764 static inline bool cfi_to_fs_settings(enum ieee802154_cfi cfi, struct ieee802154_fcf_seq *fs,
765 struct ieee802154_frame_params *params)
766 {
767 switch (cfi) {
768 case IEEE802154_CFI_DISASSOCIATION_NOTIFICATION:
769 fs->fc.pan_id_comp = 1U;
770
771 __fallthrough;
772 case IEEE802154_CFI_ASSOCIATION_REQUEST:
773 fs->fc.ar = 1U;
774 fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
775
776 if (params->dst.len == IEEE802154_SHORT_ADDR_LENGTH) {
777 fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_SHORT;
778 } else {
779 fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
780 }
781
782 break;
783 case IEEE802154_CFI_ASSOCIATION_RESPONSE:
784 case IEEE802154_CFI_PAN_ID_CONFLICT_NOTIFICATION:
785 fs->fc.ar = 1U;
786 fs->fc.pan_id_comp = 1U;
787 fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
788 fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
789
790 break;
791 case IEEE802154_CFI_DATA_REQUEST:
792 fs->fc.ar = 1U;
793 /* TODO: src/dst addr mode: see section 7.5.5 */
794
795 break;
796 case IEEE802154_CFI_ORPHAN_NOTIFICATION:
797 fs->fc.pan_id_comp = 1U;
798 fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
799 fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_SHORT;
800
801 break;
802 case IEEE802154_CFI_BEACON_REQUEST:
803 fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_NONE;
804 fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_SHORT;
805 break;
806 case IEEE802154_CFI_COORDINATOR_REALIGNEMENT:
807 fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
808 /* TODO: ack_requested and dst addr mode: see section 7.5.10 */
809
810 break;
811 case IEEE802154_CFI_GTS_REQUEST:
812 fs->fc.ar = 1U;
813 fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_SHORT;
814 fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_NONE;
815
816 break;
817 default:
818 return false;
819 }
820
821 return true;
822 }
823
mac_command_length(enum ieee802154_cfi cfi)824 static inline uint8_t mac_command_length(enum ieee802154_cfi cfi)
825 {
826 uint8_t length = 1U; /* cfi is at least present */
827
828 switch (cfi) {
829 case IEEE802154_CFI_ASSOCIATION_REQUEST:
830 case IEEE802154_CFI_DISASSOCIATION_NOTIFICATION:
831 case IEEE802154_CFI_GTS_REQUEST:
832 length += 1U;
833 break;
834 case IEEE802154_CFI_ASSOCIATION_RESPONSE:
835 length += 3U;
836 break;
837 case IEEE802154_CFI_COORDINATOR_REALIGNEMENT:
838 length += 8U;
839 break;
840 default:
841 break;
842 }
843
844 return length;
845 }
846
ieee802154_create_mac_cmd_frame(struct net_if * iface,enum ieee802154_cfi type,struct ieee802154_frame_params * params)847 struct net_pkt *ieee802154_create_mac_cmd_frame(struct net_if *iface, enum ieee802154_cfi type,
848 struct ieee802154_frame_params *params)
849 {
850 struct ieee802154_context *ctx = net_if_l2_data(iface);
851 struct ieee802154_fcf_seq *fs;
852 struct net_pkt *pkt = NULL;
853 uint8_t *p_buf, *p_start;
854
855 k_sem_take(&ctx->ctx_lock, K_FOREVER);
856
857 /* It would be costly to compute the size when actual frames are never
858 * bigger than IEEE802154_MTU bytes less the FCS size, so let's allocate that
859 * size as buffer.
860 */
861 pkt = net_pkt_alloc_with_buffer(iface, IEEE802154_MTU, AF_UNSPEC, 0, BUF_TIMEOUT);
862 if (!pkt) {
863 goto out;
864 }
865
866 p_buf = net_pkt_data(pkt);
867 p_start = p_buf;
868
869 fs = generate_fcf_grounds(
870 &p_buf, type == IEEE802154_CFI_BEACON_REQUEST ? false : ctx->ack_requested);
871
872 fs->fc.frame_type = IEEE802154_FRAME_TYPE_MAC_COMMAND;
873 fs->sequence = ctx->sequence++;
874
875 if (!cfi_to_fs_settings(type, fs, params)) {
876 goto error;
877 }
878
879 p_buf = generate_addressing_fields(ctx, fs, params, p_buf);
880
881 net_buf_add(pkt->buffer, p_buf - p_start);
882
883 /* Let's insert the cfi */
884 ((struct ieee802154_command *)p_buf)->cfi = type;
885
886 dbg_print_fs(fs);
887
888 goto out;
889
890 error:
891 net_pkt_unref(pkt);
892 pkt = NULL;
893
894 out:
895 k_sem_give(&ctx->ctx_lock);
896 return pkt;
897 }
898
ieee802154_mac_cmd_finalize(struct net_pkt * pkt,enum ieee802154_cfi type)899 void ieee802154_mac_cmd_finalize(struct net_pkt *pkt, enum ieee802154_cfi type)
900 {
901 net_buf_add(pkt->buffer, mac_command_length(type));
902 }
903
904 #endif /* CONFIG_NET_L2_IEEE802154_RFD */
905
ieee802154_create_ack_frame(struct net_if * iface,struct net_pkt * pkt,uint8_t seq)906 bool ieee802154_create_ack_frame(struct net_if *iface, struct net_pkt *pkt, uint8_t seq)
907 {
908 uint8_t *p_buf = net_pkt_data(pkt);
909 struct ieee802154_fcf_seq *fs;
910
911 if (!p_buf) {
912 return false;
913 }
914
915 fs = generate_fcf_grounds(&p_buf, false);
916
917 fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_NONE;
918 fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_NONE;
919
920 fs->fc.frame_type = IEEE802154_FRAME_TYPE_ACK;
921 fs->sequence = seq;
922
923 net_buf_add(pkt->buffer, IEEE802154_ACK_PKT_LENGTH);
924
925 return true;
926 }
927
928 #ifdef CONFIG_NET_L2_IEEE802154_SECURITY
ieee802154_decipher_data_frame(struct net_if * iface,struct net_pkt * pkt,struct ieee802154_mpdu * mpdu)929 bool ieee802154_decipher_data_frame(struct net_if *iface, struct net_pkt *pkt,
930 struct ieee802154_mpdu *mpdu)
931 {
932 struct ieee802154_context *ctx = net_if_l2_data(iface);
933 bool ret = false;
934
935 k_sem_take(&ctx->ctx_lock, K_FOREVER);
936
937 uint8_t level = ctx->sec_ctx.level;
938
939 if (!mpdu->mhr.fs->fc.security_enabled) {
940 ret = true;
941 goto out;
942 }
943
944 /* Section 9.2.4: Incoming frame security procedure, Security Enabled field is set to one
945 *
946 * [...]
947 *
948 * a) Legacy security. If the Frame Version field of the frame to be unsecured is set to
949 * zero, the procedure shall return with a Status of UNSUPPORTED_LEGACY.
950 */
951 if (mpdu->mhr.aux_sec->control.security_level != level) {
952 goto out;
953 }
954
955 if (level >= IEEE802154_SECURITY_LEVEL_ENC) {
956 level -= 4U;
957 }
958
959 uint8_t authtag_len = level_2_authtag_len[level];
960 uint8_t ll_hdr_len = (uint8_t *)mpdu->payload - net_pkt_data(pkt);
961 uint8_t payload_len = net_pkt_get_len(pkt) - ll_hdr_len - authtag_len;
962 uint8_t ext_addr_le[IEEE802154_EXT_ADDR_LENGTH];
963
964 /* TODO: Handle src short address.
965 * This will require to look up in nbr cache with short addr
966 * in order to get the extended address related to it.
967 */
968 if (net_pkt_lladdr_src(pkt)->len != IEEE802154_EXT_ADDR_LENGTH) {
969 NET_ERR("Decrypting packages with short source addresses is not supported.");
970 goto out;
971 }
972
973 sys_memcpy_swap(ext_addr_le, net_pkt_lladdr_src(pkt)->addr, net_pkt_lladdr_src(pkt)->len);
974 if (!ieee802154_decrypt_auth(&ctx->sec_ctx, net_pkt_data(pkt), ll_hdr_len, payload_len,
975 authtag_len, ext_addr_le,
976 sys_le32_to_cpu(mpdu->mhr.aux_sec->frame_counter))) {
977 NET_ERR("Could not decipher the frame");
978 goto out;
979 }
980
981 /* We remove tag size from buf's length, it is now useless. */
982 pkt->buffer->len -= authtag_len;
983
984 ret = true;
985
986 out:
987 k_sem_give(&ctx->ctx_lock);
988 return ret;
989 }
990 #endif /* CONFIG_NET_L2_IEEE802154_SECURITY */
991
