| /* |
| * Copyright (c) 2018-2021 Nordic Semiconductor ASA |
| * |
| * SPDX-License-Identifier: Apache-2.0 |
| */ |
| |
| #include <stdint.h> |
| |
| #include "hal/ccm.h" |
| #include "hal/radio.h" |
| |
| #include <soc.h> |
| #include "hal/debug.h" |
| |
| static uint8_t chan_sel_remap(uint8_t *chan_map, uint8_t chan_index); |
| #if defined(CONFIG_BT_CTLR_CHAN_SEL_2) |
| static uint16_t chan_prn_s(uint16_t counter, uint16_t chan_id); |
| static uint16_t chan_prn_e(uint16_t counter, uint16_t chan_id); |
| |
| #if defined(CONFIG_BT_CTLR_BROADCAST_ISO) |
| static uint8_t chan_sel_remap_index(uint8_t *chan_map, uint8_t chan_index); |
| static uint16_t chan_prn_subevent_se(uint16_t chan_id, |
| uint16_t *prn_subevent_lu); |
| static uint8_t chan_d(uint8_t n); |
| #endif /* CONFIG_BT_CTLR_BROADCAST_ISO */ |
| #endif /* CONFIG_BT_CTLR_CHAN_SEL_2 */ |
| |
| #if defined(CONFIG_BT_CONN) |
| /* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.2 |
| * Channel Selection algorithm #1 |
| */ |
| uint8_t lll_chan_sel_1(uint8_t *chan_use, uint8_t hop, uint16_t latency, uint8_t *chan_map, |
| uint8_t chan_count) |
| { |
| uint8_t chan_next; |
| |
| chan_next = ((*chan_use) + (hop * (1 + latency))) % 37; |
| *chan_use = chan_next; |
| |
| if ((chan_map[chan_next >> 3] & (1 << (chan_next % 8))) == 0U) { |
| uint8_t chan_index; |
| |
| chan_index = chan_next % chan_count; |
| chan_next = chan_sel_remap(chan_map, chan_index); |
| |
| } else { |
| /* channel can be used, return it */ |
| } |
| |
| return chan_next; |
| } |
| #endif /* CONFIG_BT_CONN */ |
| |
| #if defined(CONFIG_BT_CTLR_CHAN_SEL_2) |
| /* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3.2 |
| * Inputs and basic components |
| */ |
| uint16_t lll_chan_id(uint8_t *access_addr) |
| { |
| uint16_t aa_ls = ((uint16_t)access_addr[1] << 8) | access_addr[0]; |
| uint16_t aa_ms = ((uint16_t)access_addr[3] << 8) | access_addr[2]; |
| |
| return aa_ms ^ aa_ls; |
| } |
| |
| /* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3 |
| * Channel Selection algorithm #2, and Section 4.5.8.3.1 Overview |
| * Below interface is used for ACL connections. |
| */ |
| uint8_t lll_chan_sel_2(uint16_t counter, uint16_t chan_id, uint8_t *chan_map, |
| uint8_t chan_count) |
| { |
| uint8_t chan_next; |
| uint16_t prn_e; |
| |
| prn_e = chan_prn_e(counter, chan_id); |
| chan_next = prn_e % 37; |
| |
| if ((chan_map[chan_next >> 3] & (1 << (chan_next % 8))) == 0U) { |
| uint8_t chan_index; |
| |
| chan_index = ((uint32_t)chan_count * prn_e) >> 16; |
| chan_next = chan_sel_remap(chan_map, chan_index); |
| |
| } else { |
| /* channel can be used, return it */ |
| } |
| |
| return chan_next; |
| } |
| |
| #if defined(CONFIG_BT_CTLR_BROADCAST_ISO) |
| /* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3 |
| * Channel Selection algorithm #2, and Section 4.5.8.3.1 Overview |
| * |
| * Below interface is used for ISO first subevent. |
| */ |
| uint8_t 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) |
| { |
| uint8_t chan_idx; |
| uint16_t prn_e; |
| |
| *prn_s = chan_prn_s(counter, chan_id); |
| prn_e = *prn_s ^ chan_id; |
| chan_idx = prn_e % 37; |
| |
| if ((chan_map[chan_idx >> 3] & (1 << (chan_idx % 8))) == 0U) { |
| *remap_idx = ((uint32_t)chan_count * prn_e) >> 16; |
| chan_idx = chan_sel_remap(chan_map, *remap_idx); |
| |
| } else { |
| *remap_idx = chan_sel_remap_index(chan_map, chan_idx); |
| } |
| |
| return chan_idx; |
| } |
| |
| /* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3 |
| * Channel Selection algorithm #2, and Section 4.5.8.3.1 Overview |
| * |
| * Below interface is used for ISO next subevent. |
| */ |
| uint8_t 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) |
| { |
| uint16_t prn_subevent_se; |
| uint8_t chan_idx; |
| uint8_t d; |
| uint8_t x; |
| |
| prn_subevent_se = chan_prn_subevent_se(chan_id, prn_subevent_lu); |
| |
| d = chan_d(chan_count); |
| |
| /* Sub-expression to get natural number (N - 2d + 1) to be used in the |
| * calculation of d. |
| */ |
| if ((chan_count + 1) > (d << 1)) { |
| x = (chan_count + 1) - (d << 1); |
| } else { |
| x = 0; |
| } |
| |
| chan_idx = ((((uint32_t)prn_subevent_se * x) >> 16) + |
| d + *remap_idx) % chan_count; |
| |
| if ((chan_map[chan_idx >> 3] & (1 << (chan_idx % 8))) == 0U) { |
| *remap_idx = chan_idx; |
| chan_idx = chan_sel_remap(chan_map, *remap_idx); |
| } else { |
| *remap_idx = chan_idx; |
| } |
| |
| return chan_idx; |
| } |
| #endif /* CONFIG_BT_CTLR_BROADCAST_ISO */ |
| #endif /* CONFIG_BT_CTLR_CHAN_SEL_2 */ |
| |
| /* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3 |
| * Channel Selection algorithm #2, and Section 4.5.8.3.4 Event mapping to used |
| * channel index |
| */ |
| static uint8_t chan_sel_remap(uint8_t *chan_map, uint8_t chan_index) |
| { |
| uint8_t chan_next; |
| uint8_t byte_count; |
| |
| chan_next = 0U; |
| byte_count = 5U; |
| while (byte_count--) { |
| uint8_t bite; |
| uint8_t bit_count; |
| |
| bite = *chan_map; |
| bit_count = 8U; |
| while (bit_count--) { |
| if (bite & 0x01) { |
| if (chan_index == 0U) { |
| break; |
| } |
| chan_index--; |
| } |
| chan_next++; |
| bite >>= 1; |
| } |
| |
| if (bit_count < 8) { |
| break; |
| } |
| |
| chan_map++; |
| } |
| |
| return chan_next; |
| } |
| |
| #if defined(CONFIG_BT_CTLR_CHAN_SEL_2) |
| /* Attribution: |
| * http://graphics.stanford.edu/%7Eseander/bithacks.html#ReverseByteWith32Bits |
| */ |
| /* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3.2 |
| * Inputs and basic components, for below operations |
| */ |
| static uint8_t chan_rev_8(uint8_t b) |
| { |
| b = (((uint32_t)b * 0x0802LU & 0x22110LU) | |
| ((uint32_t)b * 0x8020LU & 0x88440LU)) * 0x10101LU >> 16; |
| |
| return b; |
| } |
| |
| static uint16_t chan_perm(uint16_t i) |
| { |
| return (chan_rev_8((i >> 8) & 0xFF) << 8) | chan_rev_8(i & 0xFF); |
| } |
| |
| static uint16_t chan_mam(uint16_t a, uint16_t b) |
| { |
| return ((uint32_t)a * 17U + b) & 0xFFFF; |
| } |
| |
| static uint16_t chan_prn_s(uint16_t counter, uint16_t chan_id) |
| { |
| uint8_t iterate; |
| uint16_t prn_s; |
| |
| prn_s = counter ^ chan_id; |
| |
| for (iterate = 0U; iterate < 3; iterate++) { |
| prn_s = chan_perm(prn_s); |
| prn_s = chan_mam(prn_s, chan_id); |
| } |
| |
| return prn_s; |
| } |
| |
| static uint16_t chan_prn_e(uint16_t counter, uint16_t chan_id) |
| { |
| uint16_t prn_e; |
| |
| prn_e = chan_prn_s(counter, chan_id); |
| prn_e ^= chan_id; |
| |
| return prn_e; |
| } |
| |
| #if defined(CONFIG_BT_CTLR_BROADCAST_ISO) |
| /* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3 |
| * Channel Selection algorithm #2, and Section 4.5.8.3.4 Event mapping to used |
| * channel index |
| * |
| * Below function is used in the context of next subevent, return remapping |
| * index. |
| */ |
| static uint8_t chan_sel_remap_index(uint8_t *chan_map, uint8_t chan_index) |
| { |
| uint8_t octet_count; |
| uint8_t remap_index; |
| |
| remap_index = 0U; |
| octet_count = 5U; |
| while (octet_count--) { |
| uint8_t octet; |
| uint8_t bit_count; |
| |
| octet = *chan_map; |
| bit_count = 8U; |
| while (bit_count--) { |
| if (!chan_index) { |
| return remap_index; |
| } |
| chan_index--; |
| |
| if (octet & BIT(0)) { |
| remap_index++; |
| } |
| octet >>= 1; |
| } |
| |
| chan_map++; |
| } |
| |
| return 0; |
| } |
| |
| /* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3 |
| * Channel Selection algorithm #2, and Section 4.5.8.3.5 Subevent pseudo-random |
| * number generation |
| */ |
| static uint16_t chan_prn_subevent_se(uint16_t chan_id, |
| uint16_t *prn_subevent_lu) |
| { |
| uint16_t prn_subevent_se; |
| uint16_t lu; |
| |
| lu = *prn_subevent_lu; |
| lu = chan_perm(lu); |
| lu = chan_mam(lu, chan_id); |
| |
| *prn_subevent_lu = lu; |
| |
| prn_subevent_se = lu ^ chan_id; |
| |
| return prn_subevent_se; |
| } |
| |
| /* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3 |
| * Channel Selection algorithm #2, and Section 4.5.8.3.6 Subevent mapping to |
| * used channel index |
| */ |
| static uint8_t chan_d(uint8_t n) |
| { |
| uint8_t x, y; |
| |
| /* Sub-expression to get natural number (N - 5) to be used in the |
| * calculation of d. |
| */ |
| if (n > 5) { |
| x = n - 5; |
| } else { |
| x = 0; |
| } |
| |
| /* Sub-expression to get natural number ((N - 10) / 2) to be used in the |
| * calculation of d. |
| */ |
| if (n > 10) { |
| y = (n - 10) >> 1; |
| } else { |
| y = 0; |
| } |
| |
| /* Calculate d using the above sub expressions */ |
| return MAX(1, MAX(MIN(3, x), MIN(11, y))); |
| } |
| #endif /* CONFIG_BT_CTLR_BROADCAST_ISO */ |
| |
| #if defined(CONFIG_BT_CTLR_TEST) |
| /* Refer to Bluetooth Specification v5.2 Vol 6, Part C, Section 3 LE Channel |
| * Selection algorithm #2 sample data |
| */ |
| void lll_chan_sel_2_ut(void) |
| { |
| uint8_t chan_map_1[] = {0xFF, 0xFF, 0xFF, 0xFF, 0x1F}; |
| uint8_t const chan_map_1_37_used = 37U; |
| uint8_t chan_map_2[] = {0x00, 0x06, 0xE0, 0x00, 0x1E}; |
| uint8_t const chan_map_2_9_used = 9U; |
| uint16_t const chan_id = 0x305F; |
| uint8_t m; |
| |
| /* Tests when ISO not supported */ |
| /* Section 3.1 Sample Data 1 (37 used channels) */ |
| m = lll_chan_sel_2(0, chan_id, chan_map_1, chan_map_1_37_used); |
| LL_ASSERT(m == 25U); |
| |
| m = lll_chan_sel_2(1, chan_id, chan_map_1, chan_map_1_37_used); |
| LL_ASSERT(m == 20U); |
| |
| m = lll_chan_sel_2(2, chan_id, chan_map_1, chan_map_1_37_used); |
| LL_ASSERT(m == 6U); |
| |
| m = lll_chan_sel_2(3, chan_id, chan_map_1, chan_map_1_37_used); |
| LL_ASSERT(m == 21U); |
| |
| /* Section 3.1 Sample Data 2 (9 used channels) */ |
| m = lll_chan_sel_2(6, chan_id, chan_map_2, chan_map_2_9_used); |
| LL_ASSERT(m == 23U); |
| |
| m = lll_chan_sel_2(7, chan_id, chan_map_2, chan_map_2_9_used); |
| LL_ASSERT(m == 9U); |
| |
| m = lll_chan_sel_2(8, chan_id, chan_map_2, chan_map_2_9_used); |
| LL_ASSERT(m == 34U); |
| |
| |
| #if defined(CONFIG_BT_CTLR_BROADCAST_ISO) |
| uint16_t prn_subevent_lu; |
| uint16_t prn_subevent_se; |
| uint16_t remap_idx; |
| uint16_t prn_s; |
| |
| /* BIS subevent 2, event counter 0, test prnSubEvent_se */ |
| prn_s = 56857 ^ chan_id; |
| prn_subevent_lu = prn_s; |
| prn_subevent_se = chan_prn_subevent_se(chan_id, &prn_subevent_lu); |
| LL_ASSERT(prn_subevent_se == 11710); |
| |
| /* BIS subevent 3, event counter 0 */ |
| prn_subevent_se = chan_prn_subevent_se(chan_id, &prn_subevent_lu); |
| LL_ASSERT(prn_subevent_se == 16649); |
| |
| /* BIS subevent 4, event counter 0 */ |
| prn_subevent_se = chan_prn_subevent_se(chan_id, &prn_subevent_lu); |
| LL_ASSERT(prn_subevent_se == 38198); |
| |
| /* Section 3.1 Sample Data 1 (37 used channels) */ |
| /* BIS subevent 1, event counter 0 */ |
| m = lll_chan_iso_event(0, chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx); |
| LL_ASSERT((prn_s ^ chan_id) == 56857); |
| LL_ASSERT(m == 25U); |
| LL_ASSERT(remap_idx == 25U); |
| |
| /* BIS subvent 2 */ |
| m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx); |
| LL_ASSERT(remap_idx == 1U); |
| LL_ASSERT(m == 1U); |
| |
| /* BIS subvent 3 */ |
| m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx); |
| LL_ASSERT(remap_idx == 16U); |
| LL_ASSERT(m == 16U); |
| |
| /* BIS subvent 4 */ |
| m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx); |
| LL_ASSERT(remap_idx == 36U); |
| LL_ASSERT(m == 36U); |
| |
| /* BIS subevent 1, event counter 1 */ |
| m = lll_chan_iso_event(1, chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx); |
| LL_ASSERT((prn_s ^ chan_id) == 1685); |
| LL_ASSERT(m == 20U); |
| LL_ASSERT(remap_idx == 20U); |
| |
| /* BIS subvent 2 */ |
| m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx); |
| LL_ASSERT(remap_idx == 36U); |
| LL_ASSERT(m == 36U); |
| |
| /* BIS subvent 3 */ |
| m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx); |
| LL_ASSERT(remap_idx == 12U); |
| LL_ASSERT(m == 12U); |
| |
| /* BIS subvent 4 */ |
| m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx); |
| LL_ASSERT(remap_idx == 34U); |
| LL_ASSERT(m == 34U); |
| |
| /* BIS subevent 1, event counter 2 */ |
| m = lll_chan_iso_event(2, chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx); |
| LL_ASSERT((prn_s ^ chan_id) == 38301); |
| LL_ASSERT(m == 6U); |
| LL_ASSERT(remap_idx == 6U); |
| |
| /* BIS subvent 2 */ |
| m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx); |
| LL_ASSERT(remap_idx == 18U); |
| LL_ASSERT(m == 18U); |
| |
| /* BIS subvent 3 */ |
| m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx); |
| LL_ASSERT(remap_idx == 32U); |
| LL_ASSERT(m == 32U); |
| |
| /* BIS subvent 4 */ |
| m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx); |
| LL_ASSERT(remap_idx == 21U); |
| LL_ASSERT(m == 21U); |
| |
| /* BIS subevent 1, event counter 3 */ |
| m = lll_chan_iso_event(3, chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx); |
| LL_ASSERT((prn_s ^ chan_id) == 27475); |
| LL_ASSERT(m == 21U); |
| LL_ASSERT(remap_idx == 21U); |
| |
| /* BIS subvent 2 */ |
| m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx); |
| LL_ASSERT(remap_idx == 4U); |
| LL_ASSERT(m == 4U); |
| |
| /* BIS subvent 3 */ |
| m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx); |
| LL_ASSERT(remap_idx == 22U); |
| LL_ASSERT(m == 22U); |
| |
| /* BIS subvent 4 */ |
| m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx); |
| LL_ASSERT(remap_idx == 8U); |
| LL_ASSERT(m == 8U); |
| |
| /* Section 3.1 Sample Data 2 (9 used channels) */ |
| /* BIS subevent 1, event counter 6 */ |
| m = lll_chan_iso_event(6, chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx); |
| LL_ASSERT((prn_s ^ chan_id) == 10975); |
| LL_ASSERT(remap_idx == 4U); |
| LL_ASSERT(m == 23U); |
| |
| /* BIS subvent 2 */ |
| m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx); |
| LL_ASSERT(remap_idx == 7U); |
| LL_ASSERT(m == 35U); |
| |
| /* BIS subvent 3 */ |
| m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx); |
| LL_ASSERT(remap_idx == 2U); |
| LL_ASSERT(m == 21U); |
| |
| /* BIS subvent 4 */ |
| m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx); |
| LL_ASSERT(remap_idx == 8U); |
| LL_ASSERT(m == 36U); |
| |
| /* BIS subevent 1, event counter 7 */ |
| m = lll_chan_iso_event(7, chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx); |
| LL_ASSERT((prn_s ^ chan_id) == 5490); |
| LL_ASSERT(remap_idx == 0U); |
| LL_ASSERT(m == 9U); |
| |
| /* BIS subvent 2 */ |
| m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx); |
| LL_ASSERT(remap_idx == 3U); |
| LL_ASSERT(m == 22U); |
| |
| /* BIS subvent 3 */ |
| m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx); |
| LL_ASSERT(remap_idx == 8U); |
| LL_ASSERT(m == 36U); |
| |
| /* BIS subvent 4 */ |
| m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx); |
| LL_ASSERT(remap_idx == 5U); |
| LL_ASSERT(m == 33U); |
| |
| /* BIS subevent 1, event counter 8 */ |
| m = lll_chan_iso_event(8, chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx); |
| LL_ASSERT((prn_s ^ chan_id) == 46970); |
| LL_ASSERT(remap_idx == 6U); |
| LL_ASSERT(m == 34U); |
| |
| /* BIS subvent 2 */ |
| m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx); |
| LL_ASSERT(remap_idx == 0U); |
| LL_ASSERT(m == 9U); |
| |
| /* BIS subvent 3 */ |
| m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx); |
| LL_ASSERT(remap_idx == 5U); |
| LL_ASSERT(m == 33U); |
| |
| /* BIS subvent 4 */ |
| m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx); |
| LL_ASSERT(remap_idx == 1U); |
| LL_ASSERT(m == 10U); |
| #endif /* CONFIG_BT_CTLR_BROADCAST_ISO */ |
| } |
| #endif /* CONFIG_BT_CTLR_TEST */ |
| #endif /* CONFIG_BT_CTLR_CHAN_SEL_2 */ |