1 /* ieee802154_mcxw_utils.c - NXP MCXW 802.15.4 driver utils*/
2
3 /*
4 * Copyright 2025 NXP
5 *
6 * SPDX-License-Identifier: Apache-2.0
7 *
8 */
9
10 #include <zephyr/kernel.h>
11 #include <zephyr/arch/cpu.h>
12 #include <zephyr/sys/util.h>
13 #include <zephyr/sys/byteorder.h>
14
15 #include <errno.h>
16
17 #include "ieee802154_mcxw_utils.h"
18
19 /* TODO IEEE 802.15.4 MAC Multipurpose frame format */
20 /* TODO add function checks */
21
22 enum offset_fcf_fields {
23 OffsetFrameType = 0x00,
24 OffsetSecurityEnabled = 0x03,
25 OffsetFramePending = 0x04,
26 OffsetAR = 0x05,
27 OffsetPanIdCompression = 0x06,
28 OffsetSeqNumberSuppression = 0x08,
29 OffsetIEPresent = 0x09,
30 OffsetDstAddrMode = 0x0A,
31 OffsetFrameVersion = 0x0C,
32 OffsetSrcAddrMode = 0x0E,
33 };
34
35 enum mask_fcf_fields {
36 MaskFrameType = (0x7 << OffsetFrameType),
37 MaskSecurityEnabled = (0x01 << OffsetSecurityEnabled),
38 MaskFramePending = (0x01 << OffsetFramePending),
39 MaskAR = (0x01 << OffsetAR),
40 MaskPanIdCompression = (0x01 << OffsetPanIdCompression),
41 MaskSeqNumberSuppression = (0x01 << OffsetSeqNumberSuppression),
42 MaskIEPresent = (0x01 << OffsetIEPresent),
43 MaskDstAddrMode = (0x03 << OffsetDstAddrMode),
44 MaskFrameVersion = (0x03 << OffsetFrameVersion),
45 MaskSrcAddrMode = (0x03 << OffsetSrcAddrMode),
46 };
47
48 enum modes_dst_addr {
49 ModeDstAddrNone = 0x00,
50 ModeDstAddrShort = (0x02 << OffsetDstAddrMode),
51 ModeDstAddrExt = (0x03 << OffsetDstAddrMode),
52 };
53
54 enum version_frame {
55 VersionIeee2003 = 0x00,
56 VersionIeee2006 = 0x01,
57 VersionIeee2015 = 0x02,
58 };
59
60 enum modes_src_addr {
61 ModeSrcAddrNone = 0x00,
62 ModeSrcAddrShort = (0x02 << OffsetSrcAddrMode),
63 ModeSrcAddrExt = (0x03 << OffsetSrcAddrMode),
64 };
65
66 enum offset_scf_fields {
67 OffsetSecurityLevel = 0x00,
68 OffsetKeyIdMode = 0x03,
69 OffsetFrameCntSuppression = 0x05,
70 OffsetASNinNonce = 0x06,
71 };
72
73 enum mask_scf_fields {
74 MaskSecurityLevel = (0x07 << OffsetSecurityLevel),
75 MaskKeyIdMode = (0x03 << OffsetKeyIdMode),
76 MaskFrameCntSuppression = (0x1 << OffsetFrameCntSuppression),
77 MaskASNinNonce = (0x01 << OffsetASNinNonce),
78 };
79
get_frame_control_field(uint8_t * pdu,uint16_t length)80 static uint16_t get_frame_control_field(uint8_t *pdu, uint16_t length)
81 {
82 if ((pdu == NULL) || (length < 3)) {
83 return 0x00;
84 }
85
86 return (uint16_t)(pdu[0] | (pdu[1] << 8));
87 }
88
is_security_enabled(uint16_t fcf)89 static bool is_security_enabled(uint16_t fcf)
90 {
91 if (fcf) {
92 return (bool)(fcf & MaskSecurityEnabled);
93 }
94
95 return false;
96 }
97
is_ie_present(uint16_t fcf)98 static bool is_ie_present(uint16_t fcf)
99 {
100 if (fcf) {
101 return (bool)(fcf & MaskIEPresent);
102 }
103
104 return false;
105 }
106
get_frame_version(uint16_t fcf)107 static uint8_t get_frame_version(uint16_t fcf)
108 {
109 if (fcf) {
110 return (uint8_t)((fcf & MaskFrameVersion) >> OffsetFrameVersion);
111 }
112
113 return 0xFF;
114 }
115
is_frame_version_2015_fcf(uint16_t fcf)116 static bool is_frame_version_2015_fcf(uint16_t fcf)
117 {
118 if (fcf) {
119 return get_frame_version(fcf) == VersionIeee2015;
120 }
121
122 return false;
123 }
124
is_frame_version_2015(uint8_t * pdu,uint16_t length)125 bool is_frame_version_2015(uint8_t *pdu, uint16_t length)
126 {
127 uint16_t fcf = get_frame_control_field(pdu, length);
128
129 if (fcf) {
130 return get_frame_version(fcf) == VersionIeee2015;
131 }
132
133 return false;
134 }
135
is_sequence_number_suppression(uint16_t fcf)136 static bool is_sequence_number_suppression(uint16_t fcf)
137 {
138 if (fcf) {
139 return (bool)(fcf & MaskSeqNumberSuppression);
140 }
141
142 return false;
143 }
144
is_dst_panid_present(uint16_t fcf)145 static bool is_dst_panid_present(uint16_t fcf)
146 {
147 bool present;
148
149 if (!fcf) {
150 return false;
151 }
152
153 if (is_frame_version_2015_fcf(fcf)) {
154 switch (fcf & (MaskDstAddrMode | MaskSrcAddrMode | MaskPanIdCompression)) {
155 case (ModeDstAddrNone | ModeSrcAddrNone):
156 case (ModeDstAddrShort | ModeSrcAddrNone | MaskPanIdCompression):
157 case (ModeDstAddrExt | ModeSrcAddrNone | MaskPanIdCompression):
158 case (ModeDstAddrNone | ModeSrcAddrShort):
159 case (ModeDstAddrNone | ModeSrcAddrExt):
160 case (ModeDstAddrNone | ModeSrcAddrShort | MaskPanIdCompression):
161 case (ModeDstAddrNone | ModeSrcAddrExt | MaskPanIdCompression):
162 case (ModeDstAddrExt | ModeSrcAddrExt | MaskPanIdCompression):
163 present = false;
164 break;
165 default:
166 present = true;
167 }
168 } else {
169 present = (bool)(fcf & MaskDstAddrMode);
170 }
171
172 return present;
173 }
174
is_src_panid_present(uint16_t fcf)175 static bool is_src_panid_present(uint16_t fcf)
176 {
177 bool present;
178
179 if (!fcf) {
180 return false;
181 }
182
183 if (is_frame_version_2015_fcf(fcf)) {
184 switch (fcf & (MaskDstAddrMode | MaskSrcAddrMode | MaskPanIdCompression)) {
185 case (ModeDstAddrNone | ModeSrcAddrShort):
186 case (ModeDstAddrNone | ModeSrcAddrExt):
187 case (ModeDstAddrShort | ModeSrcAddrShort):
188 case (ModeDstAddrShort | ModeSrcAddrExt):
189 case (ModeDstAddrExt | ModeSrcAddrShort):
190 present = true;
191 break;
192 default:
193 present = false;
194 }
195
196 } else {
197 present = ((fcf & MaskSrcAddrMode) != 0) && ((fcf & MaskPanIdCompression) == 0);
198 }
199
200 return present;
201 }
202
calculate_addr_field_size(uint16_t fcf)203 static uint8_t calculate_addr_field_size(uint16_t fcf)
204 {
205 uint8_t size = 2;
206
207 if (!fcf) {
208 return 0;
209 }
210
211 if (!is_sequence_number_suppression(fcf)) {
212 size += 1;
213 }
214
215 if (is_dst_panid_present(fcf)) {
216 size += 2;
217 }
218
219 /* destination addressing mode */
220 switch (fcf & MaskDstAddrMode) {
221 case ModeDstAddrShort:
222 size += 2;
223 break;
224 case ModeDstAddrExt:
225 size += 8;
226 break;
227 default:
228 break;
229 }
230
231 if (is_src_panid_present(fcf)) {
232 size += 2;
233 }
234
235 /* source addressing mode */
236 switch (fcf & MaskSrcAddrMode) {
237 case ModeSrcAddrShort:
238 size += 2;
239 break;
240 case ModeSrcAddrExt:
241 size += 8;
242 break;
243 default:
244 break;
245 }
246
247 return size;
248 }
249
get_keyid_mode(uint8_t * pdu,uint16_t length)250 static uint8_t get_keyid_mode(uint8_t *pdu, uint16_t length)
251 {
252 uint16_t fcf = get_frame_control_field(pdu, length);
253 uint8_t ash_start;
254
255 if (is_security_enabled(fcf)) {
256 ash_start = calculate_addr_field_size(fcf);
257 return (uint8_t)((pdu[ash_start] & MaskKeyIdMode) >> OffsetKeyIdMode);
258 }
259
260 return 0xFF;
261 }
262
is_keyid_mode_1(uint8_t * pdu,uint16_t length)263 bool is_keyid_mode_1(uint8_t *pdu, uint16_t length)
264 {
265 uint8_t key_mode = get_keyid_mode(pdu, length);
266
267 if (key_mode == 0x01) {
268 return true;
269 }
270
271 return false;
272 }
273
set_frame_counter(uint8_t * pdu,uint16_t length,uint32_t fc)274 void set_frame_counter(uint8_t *pdu, uint16_t length, uint32_t fc)
275 {
276 uint16_t fcf = get_frame_control_field(pdu, length);
277
278 if (is_security_enabled(fcf)) {
279 uint8_t ash_start = calculate_addr_field_size(fcf);
280 uint8_t scf = pdu[ash_start];
281
282 /* check that Frame Counter Suppression is not set */
283 if (!(scf & MaskFrameCntSuppression)) {
284 sys_put_le32(fc, &pdu[ash_start + 1]);
285 }
286 }
287 }
288
get_asn_size(uint8_t * pdu,uint16_t length)289 static uint8_t get_asn_size(uint8_t *pdu, uint16_t length)
290 {
291 uint16_t fcf = get_frame_control_field(pdu, length);
292
293 if (is_security_enabled(fcf)) {
294 uint8_t ash_start = calculate_addr_field_size(fcf);
295 uint8_t scf = pdu[ash_start];
296 uint8_t size = 1;
297
298 /* Frame Counter Suppression is not set */
299 if (!(scf & MaskFrameCntSuppression)) {
300 size += 4;
301 }
302
303 uint8_t key_mode = get_keyid_mode(pdu, length);
304
305 switch (key_mode) {
306 case 0x01:
307 size += 1;
308 break;
309 case 0x02:
310 size += 5;
311 case 0x03:
312 size += 9;
313 default:
314 break;
315 }
316
317 return size;
318 }
319 return 0;
320 }
321
get_csl_ie_content_start(uint8_t * pdu,uint16_t length)322 static uint8_t *get_csl_ie_content_start(uint8_t *pdu, uint16_t length)
323 {
324 uint16_t fcf = get_frame_control_field(pdu, length);
325
326 if (is_ie_present(fcf)) {
327 uint8_t ie_start_idx = calculate_addr_field_size(fcf) + get_asn_size(pdu, length);
328 uint8_t *cur_ie = &pdu[ie_start_idx];
329
330 uint8_t ie_header = (uint16_t)(cur_ie[0] | (cur_ie[1] << 8));
331 uint8_t ie_length = ie_header & 0x7F;
332 uint8_t ie_el_id = ie_header & 0x7F80;
333
334 while ((ie_el_id != 0x7e) && (ie_el_id != 0x7f)) {
335 if (ie_el_id == 0x1a) {
336 return (cur_ie + 2);
337 }
338 cur_ie += (2 + ie_length);
339 ie_header = (uint16_t)(cur_ie[0] | (cur_ie[1] << 8));
340 ie_length = ie_header & 0x7F;
341 ie_el_id = ie_header & 0x7F80;
342 }
343 }
344
345 return NULL;
346 }
347
set_csl_ie(uint8_t * pdu,uint16_t length,uint16_t period,uint16_t phase)348 void set_csl_ie(uint8_t *pdu, uint16_t length, uint16_t period, uint16_t phase)
349 {
350 uint8_t *csl_ie_content = get_csl_ie_content_start(pdu, length);
351
352 if (csl_ie_content) {
353 sys_put_le16(phase, csl_ie_content);
354 sys_put_le16(period, csl_ie_content + 2);
355 }
356 }
357
