1 /*
2 * Copyright (c) 2018-2021 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <stdint.h>
8
9 #include "hal/ccm.h"
10 #include "hal/radio.h"
11
12 #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_HCI_DRIVER)
13 #define LOG_MODULE_NAME bt_ctlr_lll_chan
14 #include "common/log.h"
15 #include <soc.h>
16 #include "hal/debug.h"
17
18 static uint8_t chan_sel_remap(uint8_t *chan_map, uint8_t chan_index);
19 #if defined(CONFIG_BT_CTLR_CHAN_SEL_2)
20 static uint16_t chan_prn_s(uint16_t counter, uint16_t chan_id);
21 static uint16_t chan_prn_e(uint16_t counter, uint16_t chan_id);
22
23 #if defined(CONFIG_BT_CTLR_BROADCAST_ISO)
24 static uint8_t chan_sel_remap_index(uint8_t *chan_map, uint8_t chan_index);
25 static uint16_t chan_prn_subevent_se(uint16_t chan_id,
26 uint16_t *prn_subevent_lu);
27 static uint8_t chan_d(uint8_t n);
28 #endif /* CONFIG_BT_CTLR_BROADCAST_ISO */
29 #endif /* CONFIG_BT_CTLR_CHAN_SEL_2 */
30
31 #if defined(CONFIG_BT_CONN)
32 /* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.2
33 * Channel Selection algorithm #1
34 */
lll_chan_sel_1(uint8_t * chan_use,uint8_t hop,uint16_t latency,uint8_t * chan_map,uint8_t chan_count)35 uint8_t lll_chan_sel_1(uint8_t *chan_use, uint8_t hop, uint16_t latency, uint8_t *chan_map,
36 uint8_t chan_count)
37 {
38 uint8_t chan_next;
39
40 chan_next = ((*chan_use) + (hop * (1 + latency))) % 37;
41 *chan_use = chan_next;
42
43 if ((chan_map[chan_next >> 3] & (1 << (chan_next % 8))) == 0U) {
44 uint8_t chan_index;
45
46 chan_index = chan_next % chan_count;
47 chan_next = chan_sel_remap(chan_map, chan_index);
48
49 } else {
50 /* channel can be used, return it */
51 }
52
53 return chan_next;
54 }
55 #endif /* CONFIG_BT_CONN */
56
57 #if defined(CONFIG_BT_CTLR_CHAN_SEL_2)
58 /* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3.2
59 * Inputs and basic components
60 */
lll_chan_id(uint8_t * access_addr)61 uint16_t lll_chan_id(uint8_t *access_addr)
62 {
63 uint16_t aa_ls = ((uint16_t)access_addr[1] << 8) | access_addr[0];
64 uint16_t aa_ms = ((uint16_t)access_addr[3] << 8) | access_addr[2];
65
66 return aa_ms ^ aa_ls;
67 }
68
69 /* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3
70 * Channel Selection algorithm #2, and Section 4.5.8.3.1 Overview
71 * Below interface is used for ACL connections.
72 */
lll_chan_sel_2(uint16_t counter,uint16_t chan_id,uint8_t * chan_map,uint8_t chan_count)73 uint8_t lll_chan_sel_2(uint16_t counter, uint16_t chan_id, uint8_t *chan_map,
74 uint8_t chan_count)
75 {
76 uint8_t chan_next;
77 uint16_t prn_e;
78
79 prn_e = chan_prn_e(counter, chan_id);
80 chan_next = prn_e % 37;
81
82 if ((chan_map[chan_next >> 3] & (1 << (chan_next % 8))) == 0U) {
83 uint8_t chan_index;
84
85 chan_index = ((uint32_t)chan_count * prn_e) >> 16;
86 chan_next = chan_sel_remap(chan_map, chan_index);
87
88 } else {
89 /* channel can be used, return it */
90 }
91
92 return chan_next;
93 }
94
95 #if defined(CONFIG_BT_CTLR_BROADCAST_ISO)
96 /* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3
97 * Channel Selection algorithm #2, and Section 4.5.8.3.1 Overview
98 *
99 * Below interface is used for ISO first subevent.
100 */
lll_chan_iso_event(uint16_t counter,uint16_t chan_id,uint8_t * chan_map,uint8_t chan_count,uint16_t * prn_s,uint16_t * remap_idx)101 uint8_t lll_chan_iso_event(uint16_t counter, uint16_t chan_id,
102 uint8_t *chan_map, uint8_t chan_count,
103 uint16_t *prn_s, uint16_t *remap_idx)
104 {
105 uint8_t chan_idx;
106 uint16_t prn_e;
107
108 *prn_s = chan_prn_s(counter, chan_id);
109 prn_e = *prn_s ^ chan_id;
110 chan_idx = prn_e % 37;
111
112 if ((chan_map[chan_idx >> 3] & (1 << (chan_idx % 8))) == 0U) {
113 *remap_idx = ((uint32_t)chan_count * prn_e) >> 16;
114 chan_idx = chan_sel_remap(chan_map, *remap_idx);
115
116 } else {
117 *remap_idx = chan_sel_remap_index(chan_map, chan_idx);
118 }
119
120 return chan_idx;
121 }
122
123 /* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3
124 * Channel Selection algorithm #2, and Section 4.5.8.3.1 Overview
125 *
126 * Below interface is used for ISO next subevent.
127 */
lll_chan_iso_subevent(uint16_t chan_id,uint8_t * chan_map,uint8_t chan_count,uint16_t * prn_subevent_lu,uint16_t * remap_idx)128 uint8_t lll_chan_iso_subevent(uint16_t chan_id, uint8_t *chan_map,
129 uint8_t chan_count, uint16_t *prn_subevent_lu,
130 uint16_t *remap_idx)
131 {
132 uint16_t prn_subevent_se;
133 uint8_t chan_idx;
134 uint8_t d;
135 uint8_t x;
136
137 prn_subevent_se = chan_prn_subevent_se(chan_id, prn_subevent_lu);
138
139 d = chan_d(chan_count);
140
141 /* Sub-expression to get natural number (N - 2d + 1) to be used in the
142 * calculation of d.
143 */
144 if ((chan_count + 1) > (d << 1)) {
145 x = (chan_count + 1) - (d << 1);
146 } else {
147 x = 0;
148 }
149
150 chan_idx = ((((uint32_t)prn_subevent_se * x) >> 16) +
151 d + *remap_idx) % chan_count;
152
153 if ((chan_map[chan_idx >> 3] & (1 << (chan_idx % 8))) == 0U) {
154 *remap_idx = chan_idx;
155 chan_idx = chan_sel_remap(chan_map, *remap_idx);
156 } else {
157 *remap_idx = chan_idx;
158 }
159
160 return chan_idx;
161 }
162 #endif /* CONFIG_BT_CTLR_BROADCAST_ISO */
163 #endif /* CONFIG_BT_CTLR_CHAN_SEL_2 */
164
165 /* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3
166 * Channel Selection algorithm #2, and Section 4.5.8.3.4 Event mapping to used
167 * channel index
168 */
chan_sel_remap(uint8_t * chan_map,uint8_t chan_index)169 static uint8_t chan_sel_remap(uint8_t *chan_map, uint8_t chan_index)
170 {
171 uint8_t chan_next;
172 uint8_t byte_count;
173
174 chan_next = 0U;
175 byte_count = 5U;
176 while (byte_count--) {
177 uint8_t bite;
178 uint8_t bit_count;
179
180 bite = *chan_map;
181 bit_count = 8U;
182 while (bit_count--) {
183 if (bite & 0x01) {
184 if (chan_index == 0U) {
185 break;
186 }
187 chan_index--;
188 }
189 chan_next++;
190 bite >>= 1;
191 }
192
193 if (bit_count < 8) {
194 break;
195 }
196
197 chan_map++;
198 }
199
200 return chan_next;
201 }
202
203 #if defined(CONFIG_BT_CTLR_CHAN_SEL_2)
204 /* Attribution:
205 * http://graphics.stanford.edu/%7Eseander/bithacks.html#ReverseByteWith32Bits
206 */
207 /* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3.2
208 * Inputs and basic components, for below operations
209 */
chan_rev_8(uint8_t b)210 static uint8_t chan_rev_8(uint8_t b)
211 {
212 b = (((uint32_t)b * 0x0802LU & 0x22110LU) |
213 ((uint32_t)b * 0x8020LU & 0x88440LU)) * 0x10101LU >> 16;
214
215 return b;
216 }
217
chan_perm(uint16_t i)218 static uint16_t chan_perm(uint16_t i)
219 {
220 return (chan_rev_8((i >> 8) & 0xFF) << 8) | chan_rev_8(i & 0xFF);
221 }
222
chan_mam(uint16_t a,uint16_t b)223 static uint16_t chan_mam(uint16_t a, uint16_t b)
224 {
225 return ((uint32_t)a * 17U + b) & 0xFFFF;
226 }
227
chan_prn_s(uint16_t counter,uint16_t chan_id)228 static uint16_t chan_prn_s(uint16_t counter, uint16_t chan_id)
229 {
230 uint8_t iterate;
231 uint16_t prn_s;
232
233 prn_s = counter ^ chan_id;
234
235 for (iterate = 0U; iterate < 3; iterate++) {
236 prn_s = chan_perm(prn_s);
237 prn_s = chan_mam(prn_s, chan_id);
238 }
239
240 return prn_s;
241 }
242
chan_prn_e(uint16_t counter,uint16_t chan_id)243 static uint16_t chan_prn_e(uint16_t counter, uint16_t chan_id)
244 {
245 uint16_t prn_e;
246
247 prn_e = chan_prn_s(counter, chan_id);
248 prn_e ^= chan_id;
249
250 return prn_e;
251 }
252
253 #if defined(CONFIG_BT_CTLR_BROADCAST_ISO)
254 /* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3
255 * Channel Selection algorithm #2, and Section 4.5.8.3.4 Event mapping to used
256 * channel index
257 *
258 * Below function is used in the context of next subevent, return remapping
259 * index.
260 */
chan_sel_remap_index(uint8_t * chan_map,uint8_t chan_index)261 static uint8_t chan_sel_remap_index(uint8_t *chan_map, uint8_t chan_index)
262 {
263 uint8_t octet_count;
264 uint8_t remap_index;
265
266 remap_index = 0U;
267 octet_count = 5U;
268 while (octet_count--) {
269 uint8_t octet;
270 uint8_t bit_count;
271
272 octet = *chan_map;
273 bit_count = 8U;
274 while (bit_count--) {
275 if (!chan_index) {
276 return remap_index;
277 }
278 chan_index--;
279
280 if (octet & BIT(0)) {
281 remap_index++;
282 }
283 octet >>= 1;
284 }
285
286 chan_map++;
287 }
288
289 return 0;
290 }
291
292 /* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3
293 * Channel Selection algorithm #2, and Section 4.5.8.3.5 Subevent pseudo-random
294 * number generation
295 */
chan_prn_subevent_se(uint16_t chan_id,uint16_t * prn_subevent_lu)296 static uint16_t chan_prn_subevent_se(uint16_t chan_id,
297 uint16_t *prn_subevent_lu)
298 {
299 uint16_t prn_subevent_se;
300 uint16_t lu;
301
302 lu = *prn_subevent_lu;
303 lu = chan_perm(lu);
304 lu = chan_mam(lu, chan_id);
305
306 *prn_subevent_lu = lu;
307
308 prn_subevent_se = lu ^ chan_id;
309
310 return prn_subevent_se;
311 }
312
313 /* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3
314 * Channel Selection algorithm #2, and Section 4.5.8.3.6 Subevent mapping to
315 * used channel index
316 */
chan_d(uint8_t n)317 static uint8_t chan_d(uint8_t n)
318 {
319 uint8_t x, y;
320
321 /* Sub-expression to get natural number (N - 5) to be used in the
322 * calculation of d.
323 */
324 if (n > 5) {
325 x = n - 5;
326 } else {
327 x = 0;
328 }
329
330 /* Sub-expression to get natural number ((N - 10) / 2) to be used in the
331 * calculation of d.
332 */
333 if (n > 10) {
334 y = (n - 10) >> 1;
335 } else {
336 y = 0;
337 }
338
339 /* Calculate d using the above sub expressions */
340 return MAX(1, MAX(MIN(3, x), MIN(11, y)));
341 }
342 #endif /* CONFIG_BT_CTLR_BROADCAST_ISO */
343
344 #if defined(CONFIG_BT_CTLR_TEST)
345 /* Refer to Bluetooth Specification v5.2 Vol 6, Part C, Section 3 LE Channel
346 * Selection algorithm #2 sample data
347 */
lll_chan_sel_2_ut(void)348 void lll_chan_sel_2_ut(void)
349 {
350 uint8_t chan_map_1[] = {0xFF, 0xFF, 0xFF, 0xFF, 0x1F};
351 uint8_t const chan_map_1_37_used = 37U;
352 uint8_t chan_map_2[] = {0x00, 0x06, 0xE0, 0x00, 0x1E};
353 uint8_t const chan_map_2_9_used = 9U;
354 uint16_t const chan_id = 0x305F;
355 uint8_t m;
356
357 /* Tests when ISO not supported */
358 /* Section 3.1 Sample Data 1 (37 used channels) */
359 m = lll_chan_sel_2(0, chan_id, chan_map_1, chan_map_1_37_used);
360 LL_ASSERT(m == 25U);
361
362 m = lll_chan_sel_2(1, chan_id, chan_map_1, chan_map_1_37_used);
363 LL_ASSERT(m == 20U);
364
365 m = lll_chan_sel_2(2, chan_id, chan_map_1, chan_map_1_37_used);
366 LL_ASSERT(m == 6U);
367
368 m = lll_chan_sel_2(3, chan_id, chan_map_1, chan_map_1_37_used);
369 LL_ASSERT(m == 21U);
370
371 /* Section 3.1 Sample Data 2 (9 used channels) */
372 m = lll_chan_sel_2(6, chan_id, chan_map_2, chan_map_2_9_used);
373 LL_ASSERT(m == 23U);
374
375 m = lll_chan_sel_2(7, chan_id, chan_map_2, chan_map_2_9_used);
376 LL_ASSERT(m == 9U);
377
378 m = lll_chan_sel_2(8, chan_id, chan_map_2, chan_map_2_9_used);
379 LL_ASSERT(m == 34U);
380
381
382 #if defined(CONFIG_BT_CTLR_BROADCAST_ISO)
383 uint16_t prn_subevent_lu;
384 uint16_t prn_subevent_se;
385 uint16_t remap_idx;
386 uint16_t prn_s;
387
388 /* BIS subevent 2, event counter 0, test prnSubEvent_se */
389 prn_s = 56857 ^ chan_id;
390 prn_subevent_lu = prn_s;
391 prn_subevent_se = chan_prn_subevent_se(chan_id, &prn_subevent_lu);
392 LL_ASSERT(prn_subevent_se == 11710);
393
394 /* BIS subevent 3, event counter 0 */
395 prn_subevent_se = chan_prn_subevent_se(chan_id, &prn_subevent_lu);
396 LL_ASSERT(prn_subevent_se == 16649);
397
398 /* BIS subevent 4, event counter 0 */
399 prn_subevent_se = chan_prn_subevent_se(chan_id, &prn_subevent_lu);
400 LL_ASSERT(prn_subevent_se == 38198);
401
402 /* Section 3.1 Sample Data 1 (37 used channels) */
403 /* BIS subevent 1, event counter 0 */
404 m = lll_chan_iso_event(0, chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
405 LL_ASSERT((prn_s ^ chan_id) == 56857);
406 LL_ASSERT(m == 25U);
407 LL_ASSERT(remap_idx == 25U);
408
409 /* BIS subvent 2 */
410 m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
411 LL_ASSERT(remap_idx == 1U);
412 LL_ASSERT(m == 1U);
413
414 /* BIS subvent 3 */
415 m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
416 LL_ASSERT(remap_idx == 16U);
417 LL_ASSERT(m == 16U);
418
419 /* BIS subvent 4 */
420 m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
421 LL_ASSERT(remap_idx == 36U);
422 LL_ASSERT(m == 36U);
423
424 /* BIS subevent 1, event counter 1 */
425 m = lll_chan_iso_event(1, chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
426 LL_ASSERT((prn_s ^ chan_id) == 1685);
427 LL_ASSERT(m == 20U);
428 LL_ASSERT(remap_idx == 20U);
429
430 /* BIS subvent 2 */
431 m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
432 LL_ASSERT(remap_idx == 36U);
433 LL_ASSERT(m == 36U);
434
435 /* BIS subvent 3 */
436 m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
437 LL_ASSERT(remap_idx == 12U);
438 LL_ASSERT(m == 12U);
439
440 /* BIS subvent 4 */
441 m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
442 LL_ASSERT(remap_idx == 34U);
443 LL_ASSERT(m == 34U);
444
445 /* BIS subevent 1, event counter 2 */
446 m = lll_chan_iso_event(2, chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
447 LL_ASSERT((prn_s ^ chan_id) == 38301);
448 LL_ASSERT(m == 6U);
449 LL_ASSERT(remap_idx == 6U);
450
451 /* BIS subvent 2 */
452 m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
453 LL_ASSERT(remap_idx == 18U);
454 LL_ASSERT(m == 18U);
455
456 /* BIS subvent 3 */
457 m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
458 LL_ASSERT(remap_idx == 32U);
459 LL_ASSERT(m == 32U);
460
461 /* BIS subvent 4 */
462 m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
463 LL_ASSERT(remap_idx == 21U);
464 LL_ASSERT(m == 21U);
465
466 /* BIS subevent 1, event counter 3 */
467 m = lll_chan_iso_event(3, chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
468 LL_ASSERT((prn_s ^ chan_id) == 27475);
469 LL_ASSERT(m == 21U);
470 LL_ASSERT(remap_idx == 21U);
471
472 /* BIS subvent 2 */
473 m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
474 LL_ASSERT(remap_idx == 4U);
475 LL_ASSERT(m == 4U);
476
477 /* BIS subvent 3 */
478 m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
479 LL_ASSERT(remap_idx == 22U);
480 LL_ASSERT(m == 22U);
481
482 /* BIS subvent 4 */
483 m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
484 LL_ASSERT(remap_idx == 8U);
485 LL_ASSERT(m == 8U);
486
487 /* Section 3.1 Sample Data 2 (9 used channels) */
488 /* BIS subevent 1, event counter 6 */
489 m = lll_chan_iso_event(6, chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx);
490 LL_ASSERT((prn_s ^ chan_id) == 10975);
491 LL_ASSERT(remap_idx == 4U);
492 LL_ASSERT(m == 23U);
493
494 /* BIS subvent 2 */
495 m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx);
496 LL_ASSERT(remap_idx == 7U);
497 LL_ASSERT(m == 35U);
498
499 /* BIS subvent 3 */
500 m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx);
501 LL_ASSERT(remap_idx == 2U);
502 LL_ASSERT(m == 21U);
503
504 /* BIS subvent 4 */
505 m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx);
506 LL_ASSERT(remap_idx == 8U);
507 LL_ASSERT(m == 36U);
508
509 /* BIS subevent 1, event counter 7 */
510 m = lll_chan_iso_event(7, chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx);
511 LL_ASSERT((prn_s ^ chan_id) == 5490);
512 LL_ASSERT(remap_idx == 0U);
513 LL_ASSERT(m == 9U);
514
515 /* BIS subvent 2 */
516 m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx);
517 LL_ASSERT(remap_idx == 3U);
518 LL_ASSERT(m == 22U);
519
520 /* BIS subvent 3 */
521 m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx);
522 LL_ASSERT(remap_idx == 8U);
523 LL_ASSERT(m == 36U);
524
525 /* BIS subvent 4 */
526 m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx);
527 LL_ASSERT(remap_idx == 5U);
528 LL_ASSERT(m == 33U);
529
530 /* BIS subevent 1, event counter 8 */
531 m = lll_chan_iso_event(8, chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx);
532 LL_ASSERT((prn_s ^ chan_id) == 46970);
533 LL_ASSERT(remap_idx == 6U);
534 LL_ASSERT(m == 34U);
535
536 /* BIS subvent 2 */
537 m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx);
538 LL_ASSERT(remap_idx == 0U);
539 LL_ASSERT(m == 9U);
540
541 /* BIS subvent 3 */
542 m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx);
543 LL_ASSERT(remap_idx == 5U);
544 LL_ASSERT(m == 33U);
545
546 /* BIS subvent 4 */
547 m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx);
548 LL_ASSERT(remap_idx == 1U);
549 LL_ASSERT(m == 10U);
550 #endif /* CONFIG_BT_CTLR_BROADCAST_ISO */
551 }
552 #endif /* CONFIG_BT_CTLR_TEST */
553 #endif /* CONFIG_BT_CTLR_CHAN_SEL_2 */
554
