[mynewt-nimble] 04/07: nimble/ll: Add generic helpers to calculate/remap chan_idx for CSA2

[an...@apache.org]

This is an automated email from the ASF dual-hosted git repository.

andk pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/mynewt-nimble.git

commit d9c921a81527fe2787ab71a7b6364ae30ae44c5a
Author: Andrzej Kaczmarek <an...@codecoup.pl>
AuthorDate: Mon Feb 28 11:42:12 2022 +0100

    nimble/ll: Add generic helpers to calculate/remap chan_idx for CSA2
    
    For events we calculate unmapped chan_idx from prn_e value and then
    remap it if necessary.
    
    For subevents we only need to calculate index of chan_idx on used
    channels list.
---
 nimble/controller/src/ble_ll_utils.c | 69 ++++++++++++++++++++++++++++++++++++
 1 file changed, 69 insertions(+)

diff --git a/nimble/controller/src/ble_ll_utils.c b/nimble/controller/src/ble_ll_utils.c
index ee25f83..c2edb0d 100644
--- a/nimble/controller/src/ble_ll_utils.c
+++ b/nimble/controller/src/ble_ll_utils.c
@@ -269,6 +269,75 @@ ble_ll_utils_csa2_prng(uint16_t counter, uint16_t ch_id)
     return prn_e;
 }
 
+/* Find remap_idx for given chan_idx */
+static uint16_t
+ble_ll_utils_csa2_chan2remap(uint16_t chan_idx, const uint8_t *chan_map)
+{
+    uint16_t remap_idx = 0;
+    uint32_t u32 = 0;
+    unsigned idx;
+
+    for (idx = 0; idx < 37; idx++) {
+        if ((idx % 8) == 0) {
+            u32 = chan_map[idx / 8];
+        }
+        if (u32 & 1) {
+            if (idx == chan_idx) {
+                return remap_idx;
+            }
+            remap_idx++;
+        }
+        u32 >>= 1;
+    }
+
+    BLE_LL_ASSERT(0);
+
+    return 0;
+}
+
+/* Find chan_idx at given remap_idx */
+static uint16_t
+ble_ll_utils_csa2_remap2chan(uint16_t remap_idx, const uint8_t *chan_map)
+{
+    uint32_t u32 = 0;
+    unsigned idx;
+
+    for (idx = 0; idx < 37; idx++) {
+        if ((idx % 8) == 0) {
+            u32 = chan_map[idx / 8];
+        }
+        if (u32 & 1) {
+            if (!remap_idx) {
+                return idx;
+            }
+            remap_idx--;
+        }
+        u32 >>= 1;
+    }
+
+    BLE_LL_ASSERT(0);
+
+    return 0;
+}
+
+static uint16_t
+ble_ll_utils_csa2_calc_chan_idx(uint16_t prn_e, uint8_t num_used_chans,
+                                const uint8_t *chanm_map, uint16_t *remap_idx)
+{
+    uint16_t chan_idx;
+
+    chan_idx = prn_e % 37;
+    if (chanm_map[chan_idx / 8] & (1 << (chan_idx % 8))) {
+        *remap_idx = ble_ll_utils_csa2_chan2remap(chan_idx, chanm_map);
+        return chan_idx;
+    }
+
+    *remap_idx = (num_used_chans * prn_e) / 65536;
+    chan_idx = ble_ll_utils_csa2_remap2chan(*remap_idx, chanm_map);
+
+    return chan_idx;
+}
+
 uint8_t
 ble_ll_utils_calc_dci_csa2(uint16_t event_cntr, uint16_t channel_id,
                            uint8_t num_used_chans, const uint8_t *chanmap)