From ad36fd84d50df3d859c6ebaca0dd1f746b51fa98 Mon Sep 17 00:00:00 2001
From: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
Date: Mon, 28 Feb 2022 11:49:14 +0100
Subject: [PATCH] nimble/ll: Use common helper to calculate chan_idx for dci

---
 .../include/controller/ble_ll_utils.h         |  6 ++
 nimble/controller/src/ble_ll_utils.c          | 63 ++++++++++++++-----
 2 files changed, 54 insertions(+), 15 deletions(-)

diff --git a/nimble/controller/include/controller/ble_ll_utils.h b/nimble/controller/include/controller/ble_ll_utils.h
index 48d76a6c3..12d14edb0 100644
--- a/nimble/controller/include/controller/ble_ll_utils.h
+++ b/nimble/controller/include/controller/ble_ll_utils.h
@@ -23,6 +23,12 @@ uint32_t ble_ll_utils_calc_access_addr(void);
 uint8_t ble_ll_utils_remapped_channel(uint8_t remap_index, const uint8_t *chanmap);
 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);
+uint16_t ble_ll_utils_dci_iso_event(uint16_t counter, uint16_t chan_id,
+                                    uint16_t *prn_sub_lu, uint8_t num_used_chans,
+                                    const uint8_t *chan_map, uint16_t *remap_idx);
+uint16_t ble_ll_utils_dci_iso_subevent(uint16_t chan_id, uint16_t *prn_sub_lu,
+                                       uint8_t num_used_chans, const uint8_t *chan_map,
+                                       uint16_t *remap_idx);
 uint8_t ble_ll_utils_calc_num_used_chans(const uint8_t *chan_map);
 uint32_t ble_ll_utils_calc_window_widening(uint32_t anchor_point,
                                            uint32_t last_anchor_point,
diff --git a/nimble/controller/src/ble_ll_utils.c b/nimble/controller/src/ble_ll_utils.c
index c2edb0d87..1b2857665 100644
--- a/nimble/controller/src/ble_ll_utils.c
+++ b/nimble/controller/src/ble_ll_utils.c
@@ -342,28 +342,61 @@ 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)
 {
-    uint16_t channel_unmapped;
-    uint8_t remap_index;
-
     uint16_t prn_e;
-    uint8_t bitpos;
+    uint16_t chan_idx;
+    uint16_t remap_idx;
 
     prn_e = ble_ll_utils_csa2_prng(event_cntr, channel_id);
 
-    channel_unmapped = prn_e % 37;
+    chan_idx = ble_ll_utils_csa2_calc_chan_idx(prn_e, num_used_chans, chanmap,
+                                               &remap_idx);
 
-    /*
-     * If unmapped channel is the channel index of a used channel it is used
-     * as channel index.
-     */
-    bitpos = 1 << (channel_unmapped & 0x07);
-    if (chanmap[channel_unmapped >> 3] & bitpos) {
-        return channel_unmapped;
-    }
+    return chan_idx;
+}
 
-    remap_index = (num_used_chans * prn_e) / 0x10000;
+uint16_t
+ble_ll_utils_dci_iso_event(uint16_t counter, uint16_t chan_id,
+                           uint16_t *prn_sub_lu, uint8_t num_used_chans,
+                           const uint8_t *chan_map, uint16_t *remap_idx)
+{
+    uint16_t prn_s;
+    uint16_t prn_e;
+    uint16_t chan_idx;
 
-    return ble_ll_utils_remapped_channel(remap_index, chanmap);
+    prn_s = ble_ll_utils_csa2_prn_s(counter, chan_id);
+    prn_e = prn_s ^ chan_id;
+
+    *prn_sub_lu = prn_s;
+
+    chan_idx = ble_ll_utils_csa2_calc_chan_idx(prn_e, num_used_chans, chan_map,
+                                               remap_idx);
+
+    return chan_idx;
+}
+
+uint16_t
+ble_ll_utils_dci_iso_subevent(uint16_t chan_id, uint16_t *prn_sub_lu,
+                              uint8_t num_used_chans, const uint8_t *chan_map,
+                              uint16_t *remap_idx)
+{
+    uint16_t prn_sub_se;
+    uint16_t chan_idx;
+    uint16_t d;
+
+    *prn_sub_lu = ble_ll_utils_csa2_perm(*prn_sub_lu);
+    *prn_sub_lu = ble_ll_utils_csa2_mam(*prn_sub_lu, chan_id);
+    prn_sub_se = *prn_sub_lu ^ chan_id;
+
+    /* Core 5.3, Vol 6, Part B, 4.5.8.3.6 (enjoy!) */
+    /* TODO optimize this somehow */
+    d = max(1, max(min(3, num_used_chans - 5),
+                   min(11, (num_used_chans - 10) / 2)));
+    *remap_idx = (*remap_idx + d + prn_sub_se *
+                  (num_used_chans - 2 * d + 1) / 65536) % num_used_chans;
+
+    chan_idx = ble_ll_utils_csa2_remap2chan(*remap_idx, chan_map);
+
+    return chan_idx;
 }
 #endif