From 04e90102a4e0fa3f238b7734552e7b5113a37fbb Mon Sep 17 00:00:00 2001
From: Magdalena Kasenberg <magdalena.kasenberg@codecoup.pl>
Date: Thu, 11 Apr 2024 12:58:33 +0200
Subject: [PATCH 01/14] nimble/ll: Add DRBG for Channel Sounding

A dedicated CS DRBG is used to coordinate the randomization of several
transaction types during a CS procedure.
---
 nimble/controller/src/ble_ll_cs_drbg.c      | 689 ++++++++++++++++++++
 nimble/controller/src/ble_ll_cs_drbg_priv.h | 129 ++++
 2 files changed, 818 insertions(+)
 create mode 100644 nimble/controller/src/ble_ll_cs_drbg.c
 create mode 100644 nimble/controller/src/ble_ll_cs_drbg_priv.h

diff --git a/nimble/controller/src/ble_ll_cs_drbg.c b/nimble/controller/src/ble_ll_cs_drbg.c
new file mode 100644
index 0000000000..7de5aab340
--- /dev/null
+++ b/nimble/controller/src/ble_ll_cs_drbg.c
@@ -0,0 +1,689 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <syscfg/syscfg.h>
+#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING)
+#include <stdint.h>
+#include <assert.h>
+#include <os/endian.h>
+#include "nimble/ble.h"
+#include "mbedtls/aes.h"
+#include "ble_ll_cs_drbg_priv.h"
+#if !(BABBLESIM || MYNEWT_VAL(SELFTEST))
+#include "controller/ble_hw.h"
+#endif
+
+static const uint8_t rtt_seq_len[] = {0, 4, 12, 4, 8, 12, 16};
+
+/**
+ * Security function e generates 128-bit encrypted_data from a 128-bit key
+ * and 128-bit data using the AES-128-bit block cypher.
+ */
+int
+ble_ll_cs_drbg_e(const uint8_t *key, const uint8_t *data, uint8_t *out_data)
+{
+    struct ble_encryption_block ecb;
+    int rc = 0;
+
+    /* The cryptographic function uses the leftmost to rightmost
+     * representation (MSO to LSO).
+     */
+    swap_buf(ecb.key, key, BLE_ENC_BLOCK_SIZE);
+    swap_buf(ecb.plain_text, data, BLE_ENC_BLOCK_SIZE);
+
+#if BABBLESIM || MYNEWT_VAL(SELFTEST)
+    /* Use software to encrypt the data */
+    mbedtls_aes_context aes_ctx;
+    mbedtls_aes_init(&aes_ctx);
+    mbedtls_aes_setkey_enc(&aes_ctx, ecb.key, 16 * 8);
+    rc = mbedtls_aes_crypt_ecb(&aes_ctx, MBEDTLS_AES_ENCRYPT, ecb.plain_text, ecb.cipher_text);
+    mbedtls_aes_free(&aes_ctx);
+#else
+    /* Use hardware to encrypt the data */
+    rc = ble_hw_encrypt_block(&ecb);
+#endif
+
+    if (!rc) {
+        swap_buf(out_data, ecb.cipher_text, BLE_ENC_BLOCK_SIZE);
+    } else {
+        rc = -1;
+    }
+
+    return rc;
+}
+
+/**
+ * DRBG chain function f7.
+ * - k - 128-bit key
+ * - in - an input bit string whose length is a multiple of 128 bits and
+ *        generates an output that is 128 bits long using a cipher block
+ *        chaining technique.
+ * - out - processed bit string
+ */
+int
+ble_ll_cs_drbg_f7(const uint8_t *k, const uint8_t *in, uint8_t len, uint8_t *out)
+{
+    int rc = 0;
+    int i;
+    const uint64_t *block;
+    uint64_t *hout = (uint64_t *)out;
+
+    memset(hout, 0, 16);
+
+    /* Starting with the leftmost bits (MSO) of input_bit_string, split into
+     * 128-bit blocks
+     */
+    for (i = len - 16; i >= 0; i -= 16) {
+        block = (uint64_t *)&in[i];
+        /* XOR a 128-bit block in two steps */
+        *hout ^= *block;
+        *(hout + 1) ^= *(block + 1);
+
+        rc = ble_ll_cs_drbg_e(k, out, out);
+        if (rc) {
+            break;
+        }
+    }
+
+    return rc;
+}
+
+/**
+ * DRBG derivation function f8.
+ * - input - 320-bit input bit string
+ * - sm - output, generated 256-bit seed material (SM)
+ */
+int
+ble_ll_cs_drbg_f8(const uint8_t *input, uint8_t *sm)
+{
+    int rc;
+    uint8_t k[16] = {0x0F, 0x0E, 0x0D, 0x0C, 0x0B, 0x0A, 0x09, 0x08,
+                     0x07, 0x06, 0x05, 0x04, 0x03, 0x02, 0x01, 0x00};
+    uint8_t k2[16] = {0};
+    uint8_t x[16] = {0};
+    /* buf contains V || S */
+    uint8_t buf[80] = {0};
+    uint8_t *s = buf;
+    uint8_t *v = buf + 64;
+
+    /* S = 0x0000002800000020 || input_bit_string || 0x80 ||
+     *     0x000000000000000000000000000000
+     */
+    s[15] = 0x80;
+    memcpy(s + 16, input, 40);
+    put_le64(s + 56, 0x0000002800000020);
+
+    /* K2 = f7( K, V || S ) */
+    rc = ble_ll_cs_drbg_f7(k, buf, 80, k2);
+    if (rc) {
+        return rc;
+    }
+
+    /* V = 0x00000001000000000000000000000000 */
+    v[12] = 0x01;
+
+    /* X = f7( K, V || S ) */
+    rc = ble_ll_cs_drbg_f7(k, buf, 80, x);
+    if (rc) {
+        return rc;
+    }
+
+    /* Calculate the most significant part of SM:
+     * SM = e( K2, X )
+     */
+    rc = ble_ll_cs_drbg_e(k2, x, sm + 16);
+    if (rc) {
+        return rc;
+    }
+
+    /* Calculate the least significant part of SM and concatenate
+     * both parts:
+     * SM = SM || e( K2, SM )
+     */
+    rc = ble_ll_cs_drbg_e(k2, sm + 16, sm);
+
+    return rc;
+}
+
+/**
+ * DRBG update function f9 is used to update and refresh a DRBG 128-bit
+ * temporal key K and a 128-bit nonce vector V using a 256-bit seed material (SM)
+ * that may carry fresh entropy. The SM value may also be 0 if f9 is called for
+ * backtracking purposes.
+ * - sm - 256-bit seed material
+ * - k_in - 128-bit key
+ * - v_in - 128-bit nonce vector
+ */
+int
+ble_ll_cs_drbg_f9(const uint8_t *sm, uint8_t *k, uint8_t *v)
+{
+    int rc;
+    uint8_t x[32] = {0};
+    uint64_t *x_p = (uint64_t *)x;
+    uint64_t *sm_p = (uint64_t *)sm;
+
+    /* V = V[127:8] || (( V[7:0] + 1 ) mod 2^8) */
+    v[0]++;
+    rc = ble_ll_cs_drbg_e(k, v, x + 16);
+    if (rc) {
+        return rc;
+    }
+
+    v[0]++;
+    /* Again V = V[127:8] || (( V[7:0] + 1 ) mod 2^8) */
+    rc = ble_ll_cs_drbg_e(k, v, x);
+    if (rc) {
+        return rc;
+    }
+
+    /* X = X ⊕ SM */
+    x_p[0] ^= sm_p[0];
+    x_p[1] ^= sm_p[1];
+    x_p[2] ^= sm_p[2];
+    x_p[3] ^= sm_p[3];
+
+    memcpy(v, x, 16);
+    memcpy(k, x + 16, 16);
+
+    return 0;
+}
+
+/**
+ * DRBG instantiation function h9.
+ * - iv - 128-bit initialization vector (CS_IV)
+ * - in - 64-bit instantiation nonce (CS_IN)
+ * - pv - 128-bit personalization vector (CS_PV)
+ * - key - output, 128-bit temporal key (K_DRBG)
+ * - nonce_v - output, 128-bit nonce vector (V_DRBG)
+ */
+int
+ble_ll_cs_drbg_h9(const uint8_t *iv, const uint8_t *in, const uint8_t *pv,
+                  uint8_t *key, uint8_t *nonce_v)
+{
+    int rc;
+    uint8_t input_bit_string[40] = {0};
+    uint8_t sm[32] = {0};
+
+    /* 320-bit input bit string created from concatenated vectors
+     * CS_IV || CS_IN || CS_PV
+     */
+    memcpy(input_bit_string, pv, 16);
+    memcpy(input_bit_string + 16, in, 8);
+    memcpy(input_bit_string + 24, iv, 16);
+
+    /* Generate seed material (SM) */
+    rc = ble_ll_cs_drbg_f8(input_bit_string, sm);
+    if (rc) {
+        return rc;
+    }
+
+    /* Generate K_DRBG and V_DRBG */
+    memset(key, 0, 16);
+    memset(nonce_v, 0, 16);
+    rc = ble_ll_cs_drbg_f9(sm, key, nonce_v);
+
+    return rc;
+}
+
+/**
+ * Random bit generation function CS_DRBG
+ * - transaction_id - CSTransactionID,
+ * - drbg_ctx - the drbg context, already inited with keys,
+ * - output - output buffer,
+ * - len - number of bytes to be generated.
+ */
+int
+ble_ll_cs_drbg_rand(struct ble_ll_cs_drbg_ctx *drbg_ctx,
+                    uint16_t step_count, uint8_t transaction_id,
+                    uint8_t *output, uint8_t len)
+{
+    int rc;
+    uint8_t rand_len = 0;
+    uint8_t nonce[16];
+    struct ble_ll_cs_transaction_cache *cache = &drbg_ctx->t_cache[transaction_id];
+
+    /* Set the fixed values of the DRGB nonce */
+    memcpy(nonce, drbg_ctx->nonce_v, sizeof(nonce));
+    /* Set the Transaction_Counter */
+    if (cache->last_step_count != step_count) {
+        cache->transaction_counter = 0;
+        cache->last_step_count = step_count;
+    }
+    nonce[0] += cache->transaction_counter;
+    /* Set the Transaction_Identifier */
+    nonce[1] += transaction_id;
+    /* Set the CS Step_Counter */
+    put_le16(&nonce[2], step_count + get_le16(&nonce[2]));
+
+    while (len > 0) {
+        if (cache->free_bytes > 0) {
+            /* Use bytes from previous DRBG invocation */
+
+            if (len < cache->free_bytes) {
+                rand_len = len;
+            } else {
+                rand_len = cache->free_bytes;
+            }
+
+            cache->free_bytes -= rand_len;
+            /* [0] is LSO, [15] is MSO. Return cached bytes starting from MSO. */
+            memcpy(output, cache->random_bytes + cache->free_bytes, rand_len);
+
+            len -= rand_len;
+            output += rand_len;
+        } else {
+            /* Invoke CS_DRBG to get fresh 128-bit sequence */
+            rc = ble_ll_cs_drbg_e(drbg_ctx->key, nonce, cache->random_bytes);
+            if (rc) {
+                return rc;
+            }
+
+            cache->free_bytes = sizeof(cache->random_bytes);
+            /* Increment the transaction counter */
+            ++nonce[0];
+            ++cache->transaction_counter;
+        }
+    }
+
+    return 0;
+}
+
+/** Channel Sounding random number generation function hr1 */
+int
+ble_ll_cs_drbg_rand_hr1(struct ble_ll_cs_drbg_ctx *drbg_ctx, uint16_t step_count,
+                        uint8_t transaction_id, uint8_t r, uint8_t *r_out)
+{
+    int rc;
+    uint16_t t_rand;
+    uint8_t random_bits;
+
+    if (r <= 1) {
+        *r_out = 0;
+
+        return 0;
+    }
+
+    rc = ble_ll_cs_drbg_rand(drbg_ctx, step_count,
+                             transaction_id, &random_bits, 1);
+    if (rc) {
+        return rc;
+    }
+
+    t_rand = r * random_bits;
+
+    if ((t_rand & 0xFF) < (256 % r)) {
+        rc = ble_ll_cs_drbg_rand(drbg_ctx, step_count,
+                                 transaction_id, &random_bits, 1);
+        *r_out = ((256 * random_bits * r) + t_rand) / 65536;
+    } else {
+        *r_out = t_rand / 256;
+    }
+
+    return rc;
+}
+
+int
+ble_ll_cs_drbg_shuffle_cr1(struct ble_ll_cs_drbg_ctx *drbg_ctx, uint16_t step_count,
+                           uint8_t transaction_id, uint8_t *channel_array,
+                           uint8_t *shuffled_array, uint8_t len)
+{
+    int rc;
+    uint8_t i, j;
+
+    for (i = 0; i < len; ++i) {
+        rc = ble_ll_cs_drbg_rand_hr1(drbg_ctx, step_count, transaction_id, i + 1, &j);
+        if (rc) {
+            return rc;
+        }
+
+        if (i != j) {
+            shuffled_array[i] = shuffled_array[j];
+        }
+
+        shuffled_array[j] = channel_array[i];
+    }
+
+    return 0;
+}
+
+static int
+cs_autocorrelation_score(uint32_t sequence)
+{
+    int i, k, score = 0;
+    uint32_t c, s;
+
+    for (k = 1; k <= 3; ++k) {
+        c = 0;
+        s = sequence;
+        for (i = 1; i <= 32 - k; ++i) {
+            c += (s & 1) ^ ((s >> k) & 1);
+            s >>= 1;
+        }
+        score += abs(2 * c - (32 - k));
+    }
+
+    return score;
+}
+
+int
+ble_ll_cs_drbg_generate_aa(struct ble_ll_cs_drbg_ctx *drbg_ctx, uint16_t step_count,
+                           uint32_t *initiator_aa, uint32_t *reflector_aa)
+{
+    int rc;
+    uint8_t buf[16];
+    uint32_t s0, s1, s2, s3;
+
+    rc = ble_ll_cs_drbg_rand(drbg_ctx, step_count,
+                             BLE_LL_CS_DRBG_ACCESS_ADDRESS,
+                             buf, sizeof(buf));
+
+    if (rc) {
+        return rc;
+    }
+
+    /* The buf[15] is the first generated octet */
+    s0 = get_le32(&buf[12]);
+    s1 = get_le32(&buf[8]);
+    s2 = get_le32(&buf[4]);
+    s3 = get_le32(&buf[0]);
+
+    /* The sequence with the lower autocorrelation score is selected
+     * as the CS Access Address. See 2.2.1 Channel Sounding Access Address
+     * selection rules.
+     */
+    if (cs_autocorrelation_score(s0) < cs_autocorrelation_score(s1)) {
+        *initiator_aa = s0;
+    } else {
+        *initiator_aa = s1;
+    }
+
+    if (cs_autocorrelation_score(s2) < cs_autocorrelation_score(s3)) {
+        *reflector_aa = s2;
+    } else {
+        *reflector_aa = s3;
+    }
+
+    return 0;
+}
+
+int
+ble_ll_cs_drbg_rand_marker_position(struct ble_ll_cs_drbg_ctx *drbg_ctx,
+                                    uint16_t step_count, uint8_t rtt_type,
+                                    uint8_t *position1, uint8_t *position2)
+{
+    int rc;
+    uint8_t rand_range;
+
+    if (rtt_type == BLE_LL_CS_RTT_32_BIT_SOUNDING_SEQUENCE) {
+        rand_range = 29;
+    } else { /* BLE_LL_CS_RTT_96_BIT_SOUNDING_SEQUENCE */
+        rand_range = 64;
+    }
+
+    rc = ble_ll_cs_drbg_rand_hr1(drbg_ctx, step_count,
+                                 BLE_LL_CS_DRBG_SEQ_MARKER_POSITION,
+                                 rand_range, position1);
+    if (rc) {
+        return -1;
+    }
+
+    if (rtt_type == BLE_LL_CS_RTT_32_BIT_SOUNDING_SEQUENCE) {
+        *position2 = 0xFF;
+
+        return 0;
+    }
+
+    rc = ble_ll_cs_drbg_rand_hr1(drbg_ctx, step_count,
+                                 BLE_LL_CS_DRBG_SEQ_MARKER_POSITION,
+                                 75, position2);
+    if (rc) {
+        return -1;
+    }
+
+    *position2 += 67;
+    if (*position2 > 92) {
+        /* Omit the second marker */
+        *position2 = 0xFF;
+    }
+
+    return 0;
+}
+
+int
+ble_ll_cs_drbg_rand_marker_selection(struct ble_ll_cs_drbg_ctx *drbg_ctx,
+                                     uint8_t step_count, uint8_t *marker_selection)
+{
+    int rc;
+
+    if (drbg_ctx->marker_selection_free_bits == 0) {
+        rc = ble_ll_cs_drbg_rand(drbg_ctx, step_count,
+                                 BLE_LL_CS_DRBG_SEQ_MARKER_SIGNAL,
+                                 &drbg_ctx->marker_selection_cache, 1);
+        if (rc) {
+            return rc;
+        }
+
+        drbg_ctx->marker_selection_free_bits = 8;
+    }
+
+    *marker_selection = drbg_ctx->marker_selection_cache & 0x80;
+    drbg_ctx->marker_selection_cache <<= 1;
+    --drbg_ctx->marker_selection_free_bits;
+
+    return 0;
+}
+
+int
+ble_ll_cs_drbg_rand_main_mode_steps(struct ble_ll_cs_drbg_ctx *drbg_ctx,
+                                    uint8_t step_count,
+                                    uint8_t main_mode_min_steps,
+                                    uint8_t main_mode_max_steps,
+                                    uint8_t *main_mode_steps)
+{
+    int rc;
+    uint8_t r;
+    uint8_t r_out;
+
+    r = main_mode_max_steps - main_mode_min_steps + 1;
+    rc = ble_ll_cs_drbg_rand_hr1(drbg_ctx, step_count,
+                                 BLE_LL_CS_DRBG_SUBEVT_SUBMODE,
+                                 r, &r_out);
+    if (rc) {
+        return rc;
+    }
+
+    *main_mode_steps = r_out + main_mode_min_steps;
+
+    return 0;
+}
+
+static int
+ble_ll_cs_drbg_apply_marker_signal(struct ble_ll_cs_drbg_ctx *drbg_ctx, uint8_t step_count,
+                                   uint8_t *buf, uint8_t position)
+{
+    int rc;
+    uint16_t *byte_ptr;
+    uint16_t marker_signal;
+    uint8_t marker_selection;
+    uint8_t byte_id = 0;
+    uint8_t bit_offset = 0;
+
+    rc = ble_ll_cs_drbg_rand_marker_selection(drbg_ctx, step_count, &marker_selection);
+    if (rc) {
+        return rc;
+    }
+
+    if (marker_selection) {
+        /* '0011' in transmission order */
+        marker_signal = 0b1100;
+    } else {
+        /* '1100' in transmission order */
+        marker_signal = 0b0011;
+    }
+
+    byte_id = position / 8;
+    byte_ptr = (uint16_t *) &buf[byte_id];
+    bit_offset = position % 8;
+    *byte_ptr &= ~(0xF << bit_offset);
+    *byte_ptr |= ~(marker_signal << bit_offset);
+
+    return 0;
+}
+
+static int
+ble_ll_cs_generate_sounding_sequence(struct ble_ll_cs_drbg_ctx *drbg_ctx,
+                                     uint8_t step_count, uint8_t rtt_type,
+                                     uint8_t *buf, uint8_t sequence_len)
+{
+    int rc;
+    uint8_t i;
+    uint8_t position1;
+    uint8_t position2;
+
+    for (i = 0; i < sequence_len; ++i) {
+        buf[i] = 0b10101010;
+    }
+
+    rc = ble_ll_cs_drbg_rand_marker_position(drbg_ctx, step_count, rtt_type,
+                                             &position1, &position2);
+    if (rc) {
+        return rc;
+    }
+
+    rc = ble_ll_cs_drbg_apply_marker_signal(drbg_ctx, step_count, buf, position1);
+    if (rc) {
+        return rc;
+    }
+
+    if (position2 != 0xFF) {
+        rc = ble_ll_cs_drbg_apply_marker_signal(drbg_ctx, step_count, buf, position2);
+    }
+
+    return rc;
+}
+
+int
+ble_ll_cs_drbg_generate_sync_sequence(struct ble_ll_cs_drbg_ctx *drbg_ctx,
+                                      uint8_t step_count,
+                                      uint8_t rtt_type,
+                                      uint8_t *buf,
+                                      uint8_t *sequence_len)
+{
+    int rc = -1;
+
+    *sequence_len = rtt_seq_len[rtt_type];
+
+    switch (rtt_type) {
+    case BLE_LL_CS_RTT_32_BIT_SOUNDING_SEQUENCE:
+        rc = ble_ll_cs_generate_sounding_sequence(drbg_ctx, step_count, rtt_type,
+                                                  buf, *sequence_len);
+        break;
+    case BLE_LL_CS_RTT_96_BIT_SOUNDING_SEQUENCE:
+        rc = ble_ll_cs_generate_sounding_sequence(drbg_ctx, step_count, rtt_type,
+                                                  buf, *sequence_len);
+        break;
+    case BLE_LL_CS_RTT_32_BIT_RANDOM_SEQUENCE:
+    case BLE_LL_CS_RTT_64_BIT_RANDOM_SEQUENCE:
+    case BLE_LL_CS_RTT_96_BIT_RANDOM_SEQUENCE:
+    case BLE_LL_CS_RTT_128_BIT_RANDOM_SEQUENCE:
+        rc = ble_ll_cs_drbg_rand(drbg_ctx, step_count,
+                                 BLE_LL_CS_DRBG_RAND_SEQ_GENERATION,
+                                 buf, *sequence_len);
+        break;
+    default:
+        break;
+    }
+
+    return rc;
+}
+
+int
+ble_ll_cs_drbg_rand_tone_ext_presence(struct ble_ll_cs_drbg_ctx *drbg_ctx,
+                                      uint8_t step_count, uint8_t *presence)
+{
+    int rc;
+
+    if (drbg_ctx->tone_ext_presence_free_bits == 0) {
+        rc = ble_ll_cs_drbg_rand(drbg_ctx, step_count,
+                                 BLE_LL_CS_DRBG_T_PM_TONE_SLOT_PRESENCE,
+                                 &drbg_ctx->tone_ext_presence_cache, 1);
+        if (rc) {
+            return rc;
+        }
+
+        drbg_ctx->tone_ext_presence_free_bits = 8;
+    }
+
+    *presence = drbg_ctx->tone_ext_presence_cache & 0x80 ? 1 : 0;
+    drbg_ctx->tone_ext_presence_cache <<= 1;
+    --drbg_ctx->tone_ext_presence_free_bits;
+
+    return 0;
+}
+
+int
+ble_ll_cs_drbg_rand_antenna_path_perm_id(struct ble_ll_cs_drbg_ctx *drbg_ctx,
+                                         uint16_t step_count, uint8_t n_ap,
+                                         uint8_t *ap_id)
+{
+    int rc;
+    uint8_t i;
+    uint8_t n_ap_f = 0;
+
+    if (n_ap <= 1) {
+        *ap_id = 0;
+
+        return 0;
+    }
+
+    assert(n_ap <= 4);
+
+    for (i = 1; i <= n_ap; ++i) {
+        n_ap_f *= i;
+    }
+
+    rc = ble_ll_cs_drbg_rand_hr1(drbg_ctx, step_count,
+                                 BLE_LL_CS_DRBG_ANTENNA_PATH_PERMUTATION,
+                                 n_ap_f, ap_id);
+
+    return rc;
+}
+
+void
+ble_ll_cs_drbg_clear_cache(struct ble_ll_cs_drbg_ctx *drbg_ctx)
+{
+    memset(drbg_ctx->t_cache, 0, sizeof(drbg_ctx->t_cache));
+    drbg_ctx->marker_selection_cache = 0;
+    drbg_ctx->marker_selection_free_bits = 0;
+}
+
+int
+ble_ll_cs_drbg_init(struct ble_ll_cs_drbg_ctx *drbg_ctx)
+{
+    /* Calculate temporal key K and nonce vector V */
+    return ble_ll_cs_drbg_h9(drbg_ctx->iv, drbg_ctx->in, drbg_ctx->pv,
+                             drbg_ctx->key, drbg_ctx->nonce_v);
+}
+
+void
+ble_ll_cs_drbg_free(struct ble_ll_cs_drbg_ctx *drbg_ctx)
+{
+    memset(drbg_ctx, 0, sizeof(*drbg_ctx));
+}
+#endif /* BLE_LL_CHANNEL_SOUNDING */
diff --git a/nimble/controller/src/ble_ll_cs_drbg_priv.h b/nimble/controller/src/ble_ll_cs_drbg_priv.h
new file mode 100644
index 0000000000..6e49e06789
--- /dev/null
+++ b/nimble/controller/src/ble_ll_cs_drbg_priv.h
@@ -0,0 +1,129 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_LL_CS_DRBG_PRIV
+#define H_BLE_LL_CS_DRBG_PRIV
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define BLE_LL_CS_DRBG_HOP_CHAN_NON_MODE0       (0x00)
+#define BLE_LL_CS_DRBG_HOP_CHAN_MODE0           (0x01)
+#define BLE_LL_CS_DRBG_SUBEVT_SUBMODE           (0x02)
+#define BLE_LL_CS_DRBG_T_PM_TONE_SLOT_PRESENCE  (0x03)
+#define BLE_LL_CS_DRBG_ANTENNA_PATH_PERMUTATION (0x04)
+#define BLE_LL_CS_DRBG_ACCESS_ADDRESS           (0x05)
+#define BLE_LL_CS_DRBG_SEQ_MARKER_POSITION      (0x06)
+#define BLE_LL_CS_DRBG_SEQ_MARKER_SIGNAL        (0x07)
+#define BLE_LL_CS_DRBG_RAND_SEQ_GENERATION      (0x08)
+#define BLE_LL_CS_DRBG_BACKTRACKING_RESISTANCE  (0x09)
+#define BLE_LL_CS_DRBG_TRANSACTION_IDS_NUMBER   (0x0a)
+
+#define BLE_LL_CS_RTT_AA_ONLY                  (0x00)
+#define BLE_LL_CS_RTT_32_BIT_SOUNDING_SEQUENCE (0x01)
+#define BLE_LL_CS_RTT_96_BIT_SOUNDING_SEQUENCE (0x02)
+#define BLE_LL_CS_RTT_32_BIT_RANDOM_SEQUENCE   (0x03)
+#define BLE_LL_CS_RTT_64_BIT_RANDOM_SEQUENCE   (0x04)
+#define BLE_LL_CS_RTT_96_BIT_RANDOM_SEQUENCE   (0x05)
+#define BLE_LL_CS_RTT_128_BIT_RANDOM_SEQUENCE  (0x06)
+
+struct ble_ll_cs_transaction_cache {
+    uint16_t last_step_count;
+    /* 1-octet CSTransactionCounter per each CS Transaction ID. Should
+     * be reset each time the nonce V CS Step_Counter field is set to
+     * a new value.
+     */
+    uint8_t transaction_counter;
+    /* Random bits cache */
+    uint8_t random_bytes[16];
+    /* The number of cached bytes that have been already used */
+    uint8_t free_bytes;
+};
+
+/* DRBG context */
+struct ble_ll_cs_drbg_ctx {
+    /* Initialization vector, entropy input */
+    uint8_t iv[16];
+    /* Instantiation nonce */
+    uint8_t in[8];
+    /* Personalization vector/string */
+    uint8_t pv[16];
+    /* Temporal key K */
+    uint8_t key[16];
+    /* DRBG nonce/counter (V), the starting value from which the DRBG operates.
+     * Initialized once per LE Connection.
+     */
+    uint8_t nonce_v[16];
+    /* Cache bits generated with single DRBG transation */
+    struct ble_ll_cs_transaction_cache t_cache[BLE_LL_CS_DRBG_TRANSACTION_IDS_NUMBER];
+
+    uint8_t marker_selection_free_bits;
+    uint8_t marker_selection_cache;
+
+    uint8_t tone_ext_presence_free_bits;
+    uint8_t tone_ext_presence_cache;
+};
+
+int ble_ll_cs_drbg_e(const uint8_t *key, const uint8_t *data, uint8_t *out_data);
+int ble_ll_cs_drbg_f7(const uint8_t *k, const uint8_t *in, uint8_t len, uint8_t *out);
+int ble_ll_cs_drbg_f8(const uint8_t *input, uint8_t *sm);
+int ble_ll_cs_drbg_f9(const uint8_t *sm, uint8_t *k, uint8_t *v);
+int ble_ll_cs_drbg_h9(const uint8_t *iv, const uint8_t *in, const uint8_t *pv,
+                      uint8_t *key, uint8_t *nonce_v);
+int ble_ll_cs_drbg_rand(struct ble_ll_cs_drbg_ctx *drbg_ctx,
+                        uint16_t step_count, uint8_t transaction_id,
+                        uint8_t *output, uint8_t len);
+int ble_ll_cs_drbg_rand_hr1(struct ble_ll_cs_drbg_ctx *drbg_ctx,
+                            uint16_t step_count, uint8_t transaction_id,
+                            uint8_t r, uint8_t *r_out);
+int ble_ll_cs_drbg_shuffle_cr1(struct ble_ll_cs_drbg_ctx *drbg_ctx, uint16_t step_count,
+                               uint8_t transaction_id, uint8_t *channel_array,
+                               uint8_t *shuffled_array, uint8_t len);
+int ble_ll_cs_drbg_generate_aa(struct ble_ll_cs_drbg_ctx *drbg_ctx, uint16_t step_count,
+                               uint32_t *initiator_aa, uint32_t *reflector_aa);
+int ble_ll_cs_drbg_rand_marker_position(struct ble_ll_cs_drbg_ctx *drbg_ctx,
+                                        uint16_t step_count, uint8_t rtt_type,
+                                        uint8_t *position1, uint8_t *position2);
+int ble_ll_cs_drbg_rand_marker_selection(struct ble_ll_cs_drbg_ctx *drbg_ctx,
+                                         uint8_t step_count, uint8_t *marker_selection);
+int ble_ll_cs_drbg_rand_main_mode_steps(struct ble_ll_cs_drbg_ctx *drbg_ctx,
+                                        uint8_t step_count,
+                                        uint8_t main_mode_min_steps,
+                                        uint8_t main_mode_max_steps,
+                                        uint8_t *main_mode_steps);
+int ble_ll_cs_drbg_generate_sync_sequence(struct ble_ll_cs_drbg_ctx *drbg_ctx,
+                                          uint8_t step_count,
+                                          uint8_t rtt_type,
+                                          uint8_t *buf,
+                                          uint8_t *sequence_len);
+int ble_ll_cs_drbg_rand_antenna_path_perm_id(struct ble_ll_cs_drbg_ctx *drbg_ctx,
+                                             uint16_t step_count, uint8_t n_ap,
+                                             uint8_t *ap_id);
+int ble_ll_cs_drbg_rand_tone_ext_presence(struct ble_ll_cs_drbg_ctx *drbg_ctx,
+                                          uint8_t step_count, uint8_t *presence);
+int ble_ll_cs_drbg_init(struct ble_ll_cs_drbg_ctx *drbg_ctx);
+void ble_ll_cs_drbg_free(struct ble_ll_cs_drbg_ctx *drbg_ctx);
+void ble_ll_cs_drbg_clear_cache(struct ble_ll_cs_drbg_ctx *drbg_ctx);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_LL_CS_DRBG_PRIV */

From 4b9e9150f0d228277601fa57efe8228fe99b9e23 Mon Sep 17 00:00:00 2001
From: Magdalena Kasenberg <magdalena.kasenberg@codecoup.pl>
Date: Tue, 27 Feb 2024 12:36:49 +0100
Subject: [PATCH 02/14] nimble/ll: Add unit tests for CS DRBG

The BT specification provides sample data that can be used to test
the correctness of a CS DRBG implementation.
---
 nimble/controller/pkg.yml                     |   7 +
 .../controller/test/src/ble_ll_cs_drbg_test.c | 369 ++++++++++++++++++
 nimble/controller/test/src/ble_ll_test.c      |   2 +
 nimble/controller/test/syscfg.yml             |   4 +
 4 files changed, 382 insertions(+)
 create mode 100644 nimble/controller/test/src/ble_ll_cs_drbg_test.c

diff --git a/nimble/controller/pkg.yml b/nimble/controller/pkg.yml
index 2f22e3ebf5..e5ba7b600c 100644
--- a/nimble/controller/pkg.yml
+++ b/nimble/controller/pkg.yml
@@ -42,7 +42,14 @@ pkg.deps:
     - nimble
     - nimble/transport
 
+pkg.deps.TEST:
+    - "@apache-mynewt-core/crypto/mbedtls"
+
 pkg.init:
     ble_ll_init:
         - $before:ble_transport_hs_init
         - $before:ble_transport_ll_init
+
+pkg.cflags.TEST:
+    - -Irepos/mbedtls/include
+    - -Irepos/include/mbedtls
diff --git a/nimble/controller/test/src/ble_ll_cs_drbg_test.c b/nimble/controller/test/src/ble_ll_cs_drbg_test.c
new file mode 100644
index 0000000000..1edb920713
--- /dev/null
+++ b/nimble/controller/test/src/ble_ll_cs_drbg_test.c
@@ -0,0 +1,369 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stdint.h>
+#include <controller/ble_ll_utils.h>
+#include <testutil/testutil.h>
+#include "ble_ll_cs_drbg_priv.h"
+
+extern uint8_t g_ble_ll_cs_chan_indices[72];
+
+static void
+ble_ll_cs_drbg_e_test(void)
+{
+    uint8_t key[16] = {0};
+    uint8_t data[16] = {0};
+    uint8_t out[16] = {0};
+    uint8_t expected_out[16] = {0};
+
+    /* Sample data from BT spec Vol 6, Part C, 1.1 Encrypt Command
+     * Swap because the copy-pasted strings are in leftmost (MSO) to rightmost
+     * (LSO) orientation.
+     */
+    swap_buf(key, (uint8_t [16]) {
+        0x4C, 0x68, 0x38, 0x41, 0x39, 0xF5, 0x74, 0xD8,
+        0x36, 0xBC, 0xF3, 0x4E, 0x9D, 0xFB, 0x01, 0xBF
+                                                      }, 16);
+
+    swap_buf(data, (uint8_t [16]) {
+        0x02, 0x13, 0x24, 0x35, 0x46, 0x57, 0x68, 0x79,
+        0xac, 0xbd, 0xce, 0xdf, 0xe0, 0xf1, 0x02, 0x13
+                                                      }, 16);
+
+    swap_buf(expected_out, (uint8_t [16]) {
+        0x99, 0xad, 0x1b, 0x52, 0x26, 0xa3, 0x7e, 0x3e,
+        0x05, 0x8e, 0x3b, 0x8e, 0x27, 0xc2, 0xc6, 0x66
+                                                      }, 16);
+
+    ble_ll_cs_drbg_e(key, data, out);
+    TEST_ASSERT(memcmp(out, expected_out, sizeof(out)) == 0);
+}
+
+static void
+ble_ll_cs_drbg_f7_test(void)
+{
+    uint8_t v_s[80];
+    uint8_t k[16];
+    uint8_t k2[16] = {0};
+    uint8_t x[16] = {0};
+    uint8_t expected_k2[16] = {0};
+    uint8_t expected_x[16] = {0};
+
+    /* Sample data from BT spec Vol 6, Part C, 7. Deterministic
+     * random bit generator sample data.
+     * Swap because the copy-pasted strings are in leftmost (MSO)
+     * to rightmost (LSO) orientation.
+     */
+    swap_buf(k, (uint8_t [16]) {
+        0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
+        0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F
+                                                      }, 16);
+
+    swap_buf(v_s, (uint8_t [80]) {
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x28, 0x00, 0x00, 0x00, 0x20,
+        0xE1, 0x0B, 0xC2, 0x8A, 0x0B, 0xFD, 0xDF, 0xE9,
+        0x3E, 0x7F, 0x51, 0x86, 0xE0, 0xCA, 0x0B, 0x3B,
+        0x9F, 0xF4, 0x77, 0xC1, 0x86, 0x73, 0x84, 0x0D,
+        0xC9, 0x80, 0xDE, 0xDF, 0x98, 0x82, 0xED, 0x44,
+        0x64, 0xA6, 0x74, 0x96, 0x78, 0x68, 0xF1, 0x43,
+        0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+                                                      }, 80);
+
+    swap_buf(expected_k2, (uint8_t [16]) {
+        0x8B, 0x2B, 0x06, 0xDC, 0x52, 0x2D, 0x3E, 0x0A,
+        0xF0, 0xA5, 0x0C, 0xAF, 0x48, 0x10, 0xE0, 0x35
+                                                      }, 16);
+
+    TEST_ASSERT(ble_ll_cs_drbg_f7(k, v_s, sizeof(v_s), k2) == 0);
+    TEST_ASSERT(memcmp(k2, expected_k2, sizeof(k2)) == 0);
+
+    v_s[76] = 0x01;
+    swap_buf(expected_x, (uint8_t [16]) {
+        0xA3, 0x4F, 0xBE, 0x57, 0xF8, 0xF9, 0x7E, 0x34,
+        0x9D, 0x15, 0xA3, 0x76, 0x79, 0x60, 0x74, 0x64
+                                                      }, 16);
+
+    TEST_ASSERT(ble_ll_cs_drbg_f7(k, v_s, sizeof(v_s), x) == 0);
+    TEST_ASSERT(memcmp(x, expected_x, sizeof(x)) == 0);
+}
+
+static void
+ble_ll_cs_drbg_f8_test(void)
+{
+    uint8_t input_bit_string[40] = {0};
+    uint8_t expected_sm[32] = {0};
+    uint8_t sm[32] = {0};
+
+    /* Sample data from BT spec Vol 6, Part C, 7. Deterministic
+     * random bit generator sample data.
+     * Swap because the copy-pasted strings are in leftmost (MSO)
+     * to rightmost (LSO) orientation.
+     */
+
+    /* 320-bit input bit string created from concatenated vectors
+     * CS_IV || CS_IN || CS_PV
+     */
+    swap_buf(input_bit_string, (uint8_t [40]) {
+        0xE1, 0x0B, 0xC2, 0x8A, 0x0B, 0xFD, 0xDF, 0xE9,
+        0x3E, 0x7F, 0x51, 0x86, 0xE0, 0xCA, 0x0B, 0x3B,
+        0x9F, 0xF4, 0x77, 0xC1, 0x86, 0x73, 0x84, 0x0D,
+        0xC9, 0x80, 0xDE, 0xDF, 0x98, 0x82, 0xED, 0x44,
+        0x64, 0xA6, 0x74, 0x96, 0x78, 0x68, 0xF1, 0x43
+                                                      }, 40);
+
+    swap_buf(expected_sm, (uint8_t [32]) {
+        0xB6, 0x02, 0xB1, 0xB2, 0x8C, 0x6F, 0x0A, 0x3D,
+        0xDA, 0xE6, 0x37, 0xB4, 0x84, 0x25, 0x08, 0x7D,
+        0xDC, 0x18, 0x8C, 0x89, 0xA1, 0xB0, 0xCD, 0xFD,
+        0xA1, 0xE8, 0xFC, 0x66, 0xC9, 0x99, 0x97, 0x50
+                                                      }, 32);
+
+    TEST_ASSERT(ble_ll_cs_drbg_f8(input_bit_string, sm) == 0);
+    TEST_ASSERT(memcmp(sm, expected_sm, sizeof(sm)) == 0);
+}
+
+static void
+ble_ll_cs_drbg_f9_test(void)
+{
+    uint8_t sm[32] = {0};
+    uint8_t k[16] = {0};
+    uint8_t v[16] = {0};
+    uint8_t expected_k[16] = {0};
+    uint8_t expected_v[16] = {0};
+
+    /* First call to f9 from instantiation function h9,
+     * K and V vectors filled with zeros.
+     *
+     * Sample data from BT spec Vol 6, Part C, 7. Deterministic
+     * random bit generator sample data.
+     * Swap because the copy-pasted strings are in leftmost (MSO)
+     * to rightmost (LSO) orientation.
+     */
+
+    swap_buf(sm, (uint8_t [32]) {
+        0xB6, 0x02, 0xB1, 0xB2, 0x8C, 0x6F, 0x0A, 0x3D,
+        0xDA, 0xE6, 0x37, 0xB4, 0x84, 0x25, 0x08, 0x7D,
+        0xDC, 0x18, 0x8C, 0x89, 0xA1, 0xB0, 0xCD, 0xFD,
+        0xA1, 0xE8, 0xFC, 0x66, 0xC9, 0x99, 0x97, 0x50
+                                                      }, 32);
+
+    swap_buf(expected_k, (uint8_t [16]) {
+        0xEE, 0xE0, 0x4D, 0x7C, 0x76, 0x11, 0x3A, 0x5C,
+        0xEC, 0x99, 0x2A, 0xE3, 0x20, 0xC2, 0x4D, 0x27
+                                                      }, 16);
+
+    swap_buf(expected_v, (uint8_t [16]) {
+        0xDF, 0x90, 0x56, 0x47, 0xC1, 0x06, 0x6E, 0x6F,
+        0x52, 0xC0, 0x3E, 0xDF, 0xB8, 0x2B, 0x69, 0x28
+                                                      }, 16);
+
+    TEST_ASSERT(ble_ll_cs_drbg_f9(sm, k, v) == 0);
+    TEST_ASSERT(memcmp(k, expected_k, sizeof(k)) == 0);
+    TEST_ASSERT(memcmp(v, expected_v, sizeof(v)) == 0);
+}
+
+static void
+cs_drbg_init(struct ble_ll_cs_drbg_ctx *ctx)
+{
+    memset(ctx, 0, sizeof(*ctx));
+
+    /* CS_IV = CS_IV_P || CS_IV_C */
+    swap_buf(ctx->iv, (uint8_t [16]) {
+        0xE1, 0x0B, 0xC2, 0x8A, 0x0B, 0xFD, 0xDF, 0xE9,
+        0x3E, 0x7F, 0x51, 0x86, 0xE0, 0xCA, 0x0B, 0x3B
+                                                      }, 16);
+
+    /* CS_IN = CS_IN_P || CS_IN_C */
+    swap_buf(ctx->in, (uint8_t [8]) {
+        0x9F, 0xF4, 0x77, 0xC1, 0x86, 0x73, 0x84, 0x0D
+                                                      }, 8);
+
+    /* CS_PV = CS_PV_P || CS_PV_C */
+    swap_buf(ctx->pv, (uint8_t [16]) {
+        0xC9, 0x80, 0xDE, 0xDF, 0x98, 0x82, 0xED, 0x44,
+        0x64, 0xA6, 0x74, 0x96, 0x78, 0x68, 0xF1, 0x43
+                                                      }, 16);
+
+    ble_ll_cs_drbg_init(ctx);
+}
+
+static void
+ble_ll_cs_drbg_rand_test(void)
+{
+    struct ble_ll_cs_drbg_ctx ctx;
+    uint8_t output[20] = {0};
+    uint8_t expected_output[20] = {0};
+
+    /* Test if subsequent drgb generator calls returns expected bit sequences. */
+
+    cs_drbg_init(&ctx);
+
+    /* First round - request full 128-bit batch */
+    swap_buf(expected_output, (uint8_t [16]) {
+        0x79, 0x74, 0x1F, 0xD1, 0x8F, 0x57, 0x7B, 0x45,
+        0xD0, 0x9A, 0x66, 0x5A, 0x7F, 0x1F, 0x28, 0x58
+                                                      }, 16);
+
+    TEST_ASSERT(ble_ll_cs_drbg_rand(
+        &ctx, 0x00, BLE_LL_CS_DRBG_HOP_CHAN_NON_MODE0,
+        output, 16) == 0);
+    TEST_ASSERT(memcmp(output, expected_output, 16) == 0);
+}
+
+static void
+ble_ll_cs_drbg_chan_selection_3b_test(void)
+{
+    struct ble_ll_cs_drbg_ctx ctx;
+    uint8_t filtered_channels[72] = {0};
+    uint8_t shuffled_channels[72] = {0};
+    uint8_t expected_shuffled_channels[19] = {0};
+
+    cs_drbg_init(&ctx);
+
+    memcpy(filtered_channels, (uint8_t [19]) {
+        2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12,
+        13, 14, 15, 16, 17, 18, 19, 20
+                                      }, 19);
+
+    memcpy(expected_shuffled_channels, (uint8_t [19]) {
+        11, 7, 14, 18, 9, 19, 10, 8, 5, 2, 4, 15, 16, 13, 12, 6, 17, 20, 3
+                                                                             }, 19);
+    assert(ble_ll_cs_drbg_shuffle_cr1(&ctx, 0x00, BLE_LL_CS_DRBG_HOP_CHAN_MODE0,
+                                      filtered_channels, shuffled_channels, 19) == 0);
+    assert(memcmp(shuffled_channels, expected_shuffled_channels, 19) == 0);
+
+    memcpy(expected_shuffled_channels, (uint8_t [19]) {
+        6, 12, 5, 10, 3, 2, 18, 17, 16, 8, 11, 7, 19, 4, 13, 20, 9, 15, 14
+                                                                             }, 19);
+    assert(ble_ll_cs_drbg_shuffle_cr1(&ctx, 0x03, BLE_LL_CS_DRBG_HOP_CHAN_NON_MODE0,
+                                      filtered_channels, shuffled_channels, 19) == 0);
+    assert(memcmp(shuffled_channels, expected_shuffled_channels, 19) == 0);
+}
+
+static void
+ble_ll_cs_drbg_generate_aa_test(void)
+{
+    struct ble_ll_cs_drbg_ctx ctx;
+    uint32_t initiator_aa;
+    uint32_t reflector_aa;
+    uint32_t expected_initiator_aa;
+    uint32_t expected_reflector_aa;
+
+    cs_drbg_init(&ctx);
+
+    /* Step 0 */
+    assert(ble_ll_cs_drbg_generate_aa(&ctx, 0, &initiator_aa, &reflector_aa) == 0);
+    expected_initiator_aa = get_be32((uint8_t [4]) {0x6C, 0x37, 0x6A, 0xB8});
+    expected_reflector_aa = get_be32((uint8_t [4]) {0xF0, 0x79, 0xBC, 0x3A});
+    assert(initiator_aa == expected_initiator_aa);
+    assert(reflector_aa == expected_reflector_aa);
+
+    /* Step 1 */
+    assert(ble_ll_cs_drbg_generate_aa(&ctx, 1, &initiator_aa, &reflector_aa) == 0);
+    expected_initiator_aa = get_be32((uint8_t [4]) {0x01, 0x1C, 0xAE, 0x4E});
+    expected_reflector_aa = get_be32((uint8_t [4]) {0xD0, 0x6A, 0xCD, 0xDA});
+    assert(initiator_aa == expected_initiator_aa);
+    assert(reflector_aa == expected_reflector_aa);
+
+    /* Step 2 */
+    assert(ble_ll_cs_drbg_generate_aa(&ctx, 2, &initiator_aa, &reflector_aa) == 0);
+    expected_initiator_aa = get_be32((uint8_t [4]) {0x64, 0x06, 0x12, 0x14});
+    expected_reflector_aa = get_be32((uint8_t [4]) {0x28, 0x94, 0x2F, 0x38});
+    assert(initiator_aa == expected_initiator_aa);
+    assert(reflector_aa == expected_reflector_aa);
+
+    /* Step 14 */
+    assert(ble_ll_cs_drbg_generate_aa(&ctx, 14, &initiator_aa, &reflector_aa) == 0);
+    expected_initiator_aa = get_be32((uint8_t [4]) {0xF7, 0x21, 0x97, 0x86});
+    expected_reflector_aa = get_be32((uint8_t [4]) {0x57, 0x17, 0x64, 0x70});
+    assert(initiator_aa == expected_initiator_aa);
+    assert(reflector_aa == expected_reflector_aa);
+}
+
+static void
+ble_ll_cs_drbg_rand_marker_position_test(void)
+{
+    uint8_t position1;
+    uint8_t position2;
+    struct ble_ll_cs_drbg_ctx ctx;
+
+    cs_drbg_init(&ctx);
+
+    /* Step 9 */
+    assert(ble_ll_cs_drbg_rand_marker_position(
+           &ctx, 9, BLE_LL_CS_RTT_32_BIT_SOUNDING_SEQUENCE,
+           &position1, &position2) == 0);
+    assert(position1 == 12 && position2 == 0xFF);
+
+    assert(ble_ll_cs_drbg_rand_marker_position(
+           &ctx, 9, BLE_LL_CS_RTT_32_BIT_SOUNDING_SEQUENCE,
+           &position1, &position2) == 0);
+    assert(position1 == 4 && position2 == 0xFF);
+
+    /* Step 14 */
+    assert(ble_ll_cs_drbg_rand_marker_position(
+        &ctx, 14, BLE_LL_CS_RTT_32_BIT_SOUNDING_SEQUENCE,
+        &position1, &position2) == 0);
+    assert(position1 == 8 && position2 == 0xFF);
+
+    assert(ble_ll_cs_drbg_rand_marker_position(
+        &ctx, 14, BLE_LL_CS_RTT_32_BIT_SOUNDING_SEQUENCE,
+        &position1, &position2) == 0);
+    assert(position1 == 11 && position2 == 0xFF);
+}
+
+static void
+ble_ll_cs_drbg_rand_marker_selection_test(void)
+{
+    uint8_t marker_selection;
+    struct ble_ll_cs_drbg_ctx ctx;
+
+    cs_drbg_init(&ctx);
+
+    /* Step 9 */
+    assert(ble_ll_cs_drbg_rand_marker_selection(&ctx, 0x09, &marker_selection) == 0);
+    assert(marker_selection == 0x00);
+
+    assert(ble_ll_cs_drbg_rand_marker_selection(&ctx, 0x09, &marker_selection) == 0);
+    assert(marker_selection == 0x80);
+
+    memset(ctx.t_cache, 0, sizeof(ctx.t_cache));
+
+    /* Step 14 */
+    assert(ble_ll_cs_drbg_rand_marker_selection(&ctx, 14, &marker_selection) == 0);
+    assert(marker_selection == 0x80);
+
+    assert(ble_ll_cs_drbg_rand_marker_selection(&ctx, 14, &marker_selection) == 0);
+    assert(marker_selection == 0x80);
+}
+
+TEST_SUITE(ble_ll_cs_drbg_test_suite) {
+    ble_ll_cs_drbg_e_test();
+    ble_ll_cs_drbg_f7_test();
+    ble_ll_cs_drbg_f8_test();
+    ble_ll_cs_drbg_f9_test();
+    ble_ll_cs_drbg_rand_test();
+    ble_ll_cs_drbg_chan_selection_3b_test();
+    ble_ll_cs_drbg_generate_aa_test();
+    ble_ll_cs_drbg_rand_marker_position_test();
+    ble_ll_cs_drbg_rand_marker_selection_test();
+}
diff --git a/nimble/controller/test/src/ble_ll_test.c b/nimble/controller/test/src/ble_ll_test.c
index 818430c577..93e472db38 100644
--- a/nimble/controller/test/src/ble_ll_test.c
+++ b/nimble/controller/test/src/ble_ll_test.c
@@ -27,6 +27,7 @@ TEST_SUITE_DECL(ble_ll_crypto_test_suite);
 TEST_SUITE_DECL(ble_ll_csa2_test_suite);
 TEST_SUITE_DECL(ble_ll_isoal_test_suite);
 TEST_SUITE_DECL(ble_ll_iso_test_suite);
+TEST_SUITE_DECL(ble_ll_cs_drbg_test_suite);
 
 int
 main(int argc, char **argv)
@@ -36,6 +37,7 @@ main(int argc, char **argv)
     ble_ll_csa2_test_suite();
     ble_ll_isoal_test_suite();
     ble_ll_iso_test_suite();
+    ble_ll_cs_drbg_test_suite();
 
     return tu_any_failed;
 }
diff --git a/nimble/controller/test/syscfg.yml b/nimble/controller/test/syscfg.yml
index f7a3c4ef5e..cf6a5de8e6 100644
--- a/nimble/controller/test/syscfg.yml
+++ b/nimble/controller/test/syscfg.yml
@@ -27,3 +27,7 @@ syscfg.vals:
     NATIVE_SOCKETS_PRIO: 3
 
     BLE_TRANSPORT_ISO_SIZE: 255
+
+    MBEDTLS_AES_C: 1
+    BLE_CHANNEL_SOUNDING: 1
+    BLE_LL_CHANNEL_SOUNDING: 1

From 9cf2dbcaa5f876cd47b411f750363270aef5707f Mon Sep 17 00:00:00 2001
From: Magdalena Kasenberg <magdalena.kasenberg@codecoup.pl>
Date: Thu, 14 Mar 2024 10:35:31 +0100
Subject: [PATCH 03/14] nimble/host: ble_cs: Fix 'defined but not used' warning

ble_cs.c contains initial implementation that caused warnings when
running command:
newt test @apache-mynewt-nimble/nimble
---
 nimble/host/src/ble_cs.c | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/nimble/host/src/ble_cs.c b/nimble/host/src/ble_cs.c
index cef622206b..2ae985e225 100644
--- a/nimble/host/src/ble_cs.c
+++ b/nimble/host/src/ble_cs.c
@@ -689,7 +689,6 @@ ble_hs_hci_evt_le_cs_test_end_complete(uint8_t subevent, const void *data,
 int
 ble_cs_initiator_procedure_start(const struct ble_cs_initiator_procedure_start_params *params)
 {
-    struct ble_hci_le_cs_rd_loc_supp_cap_rp rsp;
     struct ble_cs_rd_rem_supp_cap_cp cmd;
     int rc;
 
@@ -701,6 +700,12 @@ ble_cs_initiator_procedure_start(const struct ble_cs_initiator_procedure_start_p
      * 5. Start the CS Security Start procedure
      */
 
+    (void) ble_cs_set_chan_class;
+    (void) ble_cs_remove_config;
+    (void) ble_cs_wr_cached_rem_fae;
+    (void) ble_cs_wr_cached_rem_supp_cap;
+    (void) ble_cs_rd_loc_supp_cap;
+
     cs_state.cb = params->cb;
     cs_state.cb_arg = params->cb_arg;
 

From 0cc7a0ecdcb091dd178f23478e0a78ee5c2d22ac Mon Sep 17 00:00:00 2001
From: Magdalena Kasenberg <magdalena.kasenberg@codecoup.pl>
Date: Sun, 14 Apr 2024 12:53:10 +0200
Subject: [PATCH 04/14] nimble/hci: Update Channel Sounding HCI

Blueooth 6.0. spec contains Optional_TX_SNR_Capability field as
a CS capability. Companion signal parameter was removed.
---
 nimble/host/src/ble_cs.c           | 5 +----
 nimble/include/nimble/hci_common.h | 9 ++++++---
 2 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/nimble/host/src/ble_cs.c b/nimble/host/src/ble_cs.c
index 2ae985e225..478e2bf6bc 100644
--- a/nimble/host/src/ble_cs.c
+++ b/nimble/host/src/ble_cs.c
@@ -115,7 +115,6 @@ struct ble_cs_create_config_cp {
     uint8_t channel_selection_type;
     uint8_t ch3c_shape;
     uint8_t ch3c_jump;
-    uint8_t companion_signal_enable;
 } __attribute__((packed));
 
 struct ble_cs_remove_config_cp {
@@ -361,7 +360,7 @@ ble_cs_create_config(const struct ble_cs_create_config_cp *cmd)
     cp.channel_selection_type = cmd->channel_selection_type;
     cp.ch3c_shape = cmd->ch3c_shape;
     cp.ch3c_jump = cmd->ch3c_jump;
-    cp.companion_signal_enable = cmd->companion_signal_enable;
+    cp.reserved = 0x00;
 
     return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
                                         BLE_HCI_OCF_LE_CS_CREATE_CONFIG),
@@ -531,8 +530,6 @@ ble_hs_hci_evt_le_cs_rd_rem_fae_complete(uint8_t subevent, const void *data,
     /* Ignore these as used only with #3c algorithm */
     cmd.ch3c_shape = 0x00;
     cmd.ch3c_jump = 0x00;
-    /* EDLC/ECLD attack protection not supported */
-    cmd.companion_signal_enable = 0x00;
 
     /* Create CS config */
     rc = ble_cs_create_config(&cmd);
diff --git a/nimble/include/nimble/hci_common.h b/nimble/include/nimble/hci_common.h
index 24843de3c2..9b77321bcf 100644
--- a/nimble/include/nimble/hci_common.h
+++ b/nimble/include/nimble/hci_common.h
@@ -1178,6 +1178,7 @@ struct ble_hci_le_cs_rd_loc_supp_cap_rp {
     uint16_t optional_t_fcs_times_supported;
     uint16_t optional_t_pm_times_supported;
     uint8_t t_sw_time_supported;
+    uint8_t optional_tx_snr_capability;
 } __attribute__((packed));
 
 #define BLE_HCI_OCF_LE_CS_RD_REM_SUPP_CAP                (0x008A)
@@ -1207,6 +1208,7 @@ struct ble_hci_le_cs_wr_cached_rem_supp_cap_cp {
     uint16_t optional_t_fcs_times_supported;
     uint16_t optional_t_pm_times_supported;
     uint8_t t_sw_time_supported;
+    uint8_t optional_tx_snr_capability;
 } __attribute__((packed));
 struct ble_hci_le_cs_wr_cached_rem_supp_cap_rp {
     uint16_t conn_handle;
@@ -1261,7 +1263,7 @@ struct ble_hci_le_cs_create_config_cp {
     uint8_t channel_selection_type;
     uint8_t ch3c_shape;
     uint8_t ch3c_jump;
-    uint8_t companion_signal_enable;
+    uint8_t reserved;
 } __attribute__((packed));
 
 #define BLE_HCI_OCF_LE_CS_REMOVE_CONFIG                  (0x0091)
@@ -1322,7 +1324,7 @@ struct ble_hci_le_cs_test_cp {
     uint8_t t_pm_time;
     uint8_t t_sw_time;
     uint8_t tone_antenna_config_selection;
-    uint8_t companion_signal_enable;
+    uint8_t reserved;
     uint16_t drbg_nonce;
     uint16_t override_config;
     uint8_t override_parameters_length;
@@ -2230,6 +2232,7 @@ struct ble_hci_ev_le_subev_cs_rd_rem_supp_cap_complete {
     uint16_t optional_t_fcs_times_supported;
     uint16_t optional_t_pm_times_supported;
     uint8_t t_sw_time_supported;
+    uint8_t optional_tx_snr_capability;
 } __attribute__((packed));
 
 #define BLE_HCI_LE_SUBEV_CS_RD_REM_FAE_COMPLETE        (0x2D)
@@ -2268,7 +2271,7 @@ struct ble_hci_ev_le_subev_cs_config_complete {
     uint8_t channel_selection_type;
     uint8_t ch3c_shape;
     uint8_t ch3c_jump;
-    uint8_t companion_signal_enable;
+    uint8_t reserved;
     uint8_t t_ip1_time;
     uint8_t t_ip2_time;
     uint8_t t_fcs_time;

From 5a8ff985b068abeaa0ad853c7fe1a9c2fe20b178 Mon Sep 17 00:00:00 2001
From: Magdalena Kasenberg <magdalena.kasenberg@codecoup.pl>
Date: Sun, 14 Apr 2024 09:48:14 +0200
Subject: [PATCH 05/14] nimble/ll: Add Channel Sounding state machine

Initialize the CS state machine with the connection state machine.
---
 .../include/controller/ble_ll_conn.h          |  8 ++++
 nimble/controller/src/ble_ll_conn.c           | 13 +++++++
 nimble/controller/src/ble_ll_cs.c             | 26 +++++++++++++
 nimble/controller/src/ble_ll_cs_priv.h        | 38 +++++++++++++++++++
 4 files changed, 85 insertions(+)
 create mode 100644 nimble/controller/src/ble_ll_cs_priv.h

diff --git a/nimble/controller/include/controller/ble_ll_conn.h b/nimble/controller/include/controller/ble_ll_conn.h
index 2b0c606dd6..656bfeb03b 100644
--- a/nimble/controller/include/controller/ble_ll_conn.h
+++ b/nimble/controller/include/controller/ble_ll_conn.h
@@ -202,6 +202,10 @@ struct ble_ll_conn_subrate_req_params {
     uint16_t supervision_tmo;
 };
 
+#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING)
+struct ble_ll_cs_sm;
+#endif
+
 /* Connection state machine */
 struct ble_ll_conn_sm
 {
@@ -400,6 +404,10 @@ struct ble_ll_conn_sm
     uint16_t css_slot_idx_pending;
     uint8_t css_period_idx;
 #endif
+
+#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING)
+    struct ble_ll_cs_sm *cssm;
+#endif
 };
 
 /* Role */
diff --git a/nimble/controller/src/ble_ll_conn.c b/nimble/controller/src/ble_ll_conn.c
index 52446b6471..19ee2fbebd 100644
--- a/nimble/controller/src/ble_ll_conn.c
+++ b/nimble/controller/src/ble_ll_conn.c
@@ -1964,6 +1964,11 @@ ble_ll_conn_set_csa(struct ble_ll_conn_sm *connsm, bool chsel)
     connsm->data_chan_index = ble_ll_conn_calc_dci(connsm, 1);
 }
 
+#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING)
+void ble_ll_cs_sm_init(struct ble_ll_conn_sm *connsm);
+void ble_ll_cs_sm_free(struct ble_ll_conn_sm *connsm);
+#endif
+
 /**
  * Create a new connection state machine. This is done once per
  * connection when the HCI command "create connection" is issued to the
@@ -2086,6 +2091,10 @@ ble_ll_conn_sm_new(struct ble_ll_conn_sm *connsm)
         ble_ll_conn_css_update_list(connsm);
     }
 #endif
+
+#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING)
+    ble_ll_cs_sm_init(connsm);
+#endif
 }
 
 void
@@ -2211,6 +2220,10 @@ ble_ll_conn_end(struct ble_ll_conn_sm *connsm, uint8_t ble_err)
     }
 #endif
 
+#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING)
+    ble_ll_cs_sm_free(connsm);
+#endif
+
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_CTRL_TO_HOST_FLOW_CONTROL)
     ble_ll_conn_cth_flow_free_credit(connsm, connsm->cth_flow_pending);
 #endif
diff --git a/nimble/controller/src/ble_ll_cs.c b/nimble/controller/src/ble_ll_cs.c
index a9a181a849..dc3cfe94fb 100644
--- a/nimble/controller/src/ble_ll_cs.c
+++ b/nimble/controller/src/ble_ll_cs.c
@@ -27,6 +27,8 @@
 #include "controller/ble_ll_hci.h"
 #include "controller/ble_ll_cs.h"
 
+static struct ble_ll_cs_sm g_ble_ll_cs_sm[MYNEWT_VAL(BLE_MAX_CONNECTIONS)];
+
 int
 ble_ll_cs_hci_rd_loc_supp_cap(uint8_t *rspbuf, uint8_t *rsplen)
 {
@@ -116,4 +118,28 @@ ble_ll_cs_hci_test_end(void)
     return BLE_ERR_UNSUPPORTED;
 }
 
+void
+ble_ll_cs_sm_init(struct ble_ll_conn_sm *connsm)
+{
+    uint8_t i;
+
+    for (i = 0; i < ARRAY_SIZE(g_ble_ll_cs_sm); i++) {
+        if (g_ble_ll_cs_sm[i].connsm == NULL) {
+            connsm->cssm = &g_ble_ll_cs_sm[i];
+            memset(connsm->cssm, 0, sizeof(*connsm->cssm));
+            connsm->cssm->connsm = connsm;
+            break;
+        }
+    }
+}
+
+void
+ble_ll_cs_sm_free(struct ble_ll_conn_sm *connsm)
+{
+    if (connsm->cssm) {
+        memset(connsm->cssm, 0, sizeof(*connsm->cssm));
+        connsm->cssm = NULL;
+    }
+}
+
 #endif /* BLE_LL_CHANNEL_SOUNDING */
diff --git a/nimble/controller/src/ble_ll_cs_priv.h b/nimble/controller/src/ble_ll_cs_priv.h
new file mode 100644
index 0000000000..3f0d03346e
--- /dev/null
+++ b/nimble/controller/src/ble_ll_cs_priv.h
@@ -0,0 +1,38 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_LL_CS_PRIV
+#define H_BLE_LL_CS_PRIV
+
+#include <stdint.h>
+#include "controller/ble_ll_conn.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct ble_ll_cs_sm {
+    struct ble_ll_conn_sm *connsm;
+};
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_LL_CS_PRIV */

From 7e1f6eb252d51ff9597a3b6155f79632abb72440 Mon Sep 17 00:00:00 2001
From: Magdalena Kasenberg <magdalena.kasenberg@codecoup.pl>
Date: Sun, 14 Apr 2024 10:02:05 +0200
Subject: [PATCH 06/14] nimble/ll: Add Channel Sounding LL CTRL opcodes

Add lengths and opcodes of Channel Sounding LL CTRL PDUs.
---
 .../include/controller/ble_ll_ctrl.h          | 38 ++++++++++++++++++-
 nimble/controller/src/ble_ll_ctrl.c           | 16 ++++++++
 2 files changed, 53 insertions(+), 1 deletion(-)

diff --git a/nimble/controller/include/controller/ble_ll_ctrl.h b/nimble/controller/include/controller/ble_ll_ctrl.h
index 89502a45a3..72e0bf552c 100644
--- a/nimble/controller/include/controller/ble_ll_ctrl.h
+++ b/nimble/controller/include/controller/ble_ll_ctrl.h
@@ -100,9 +100,25 @@ extern "C" {
 #define BLE_LL_CTRL_SUBRATE_IND         (0x27)
 #define BLE_LL_CTRL_CHAN_REPORTING_IND  (0x28)
 #define BLE_LL_CTRL_CHAN_STATUS_IND     (0x29)
+#define BLE_LL_CTRL_PERIODIC_SYNC_WR_IND  (0x2A)
+#define BLE_LL_CTRL_FEATURE_EXT_REQ     (0x2B)
+#define BLE_LL_CTRL_FEATURE_EXT_RSP     (0x2C)
+#define BLE_LL_CTRL_CS_SEC_RSP          (0x2D)
+#define BLE_LL_CTRL_CS_CAPABILITIES_REQ (0x2E)
+#define BLE_LL_CTRL_CS_CAPABILITIES_RSP (0x2F)
+#define BLE_LL_CTRL_CS_CONFIG_REQ       (0x30)
+#define BLE_LL_CTRL_CS_CONFIG_RSP       (0x31)
+#define BLE_LL_CTRL_CS_REQ              (0x32)
+#define BLE_LL_CTRL_CS_RSP              (0x33)
+#define BLE_LL_CTRL_CS_IND              (0x34)
+#define BLE_LL_CTRL_CS_TERMINATE_IND    (0x35)
+#define BLE_LL_CTRL_CS_FAE_REQ          (0x36)
+#define BLE_LL_CTRL_CS_FAE_RSP          (0x37)
+#define BLE_LL_CTRL_CS_CHANNEL_MAP_IND  (0x38)
+#define BLE_LL_CTRL_CS_SEC_REQ          (0x39)
 
 /* Maximum opcode value */
-#define BLE_LL_CTRL_OPCODES             (BLE_LL_CTRL_CHAN_STATUS_IND + 1)
+#define BLE_LL_CTRL_OPCODES             (BLE_LL_CTRL_CS_SEC_REQ + 1)
 
 extern const uint8_t g_ble_ll_ctrl_pkt_lengths[BLE_LL_CTRL_OPCODES];
 
@@ -292,6 +308,26 @@ struct ble_ll_len_req
 #define BLE_LL_CTRL_CHAN_REPORTING_IND_LEN      (3)
 #define BLE_LL_CTRL_CHAN_STATUS_IND_LEN         (10)
 
+/* Not implemented */
+#define BLE_LL_CTRL_PERIODIC_SYNC_WR_IND_LEN  (0)
+#define BLE_LL_CTRL_FEATURE_EXT_REQ_LEN       (0)
+#define BLE_LL_CTRL_FEATURE_EXT_RSP_LEN       (0)
+
+/* Channel Sounding */
+#define BLE_LL_CTRL_CS_SEC_RSP_LEN            (20)
+#define BLE_LL_CTRL_CS_CAPABILITIES_REQ_LEN   (25)
+#define BLE_LL_CTRL_CS_CAPABILITIES_RSP_LEN   (25)
+#define BLE_LL_CTRL_CS_CONFIG_REQ_LEN         (27)
+#define BLE_LL_CTRL_CS_CONFIG_RSP_LEN         (1)
+#define BLE_LL_CTRL_CS_REQ_LEN                (28)
+#define BLE_LL_CTRL_CS_RSP_LEN                (21)
+#define BLE_LL_CTRL_CS_IND_LEN                (18)
+#define BLE_LL_CTRL_CS_TERMINATE_IND_LEN      (4)
+#define BLE_LL_CTRL_CS_FAE_REQ_LEN            (0)
+#define BLE_LL_CTRL_CS_FAE_RSP_LEN            (72)
+#define BLE_LL_CTRL_CS_CHANNEL_MAP_IND_LEN    (12)
+#define BLE_LL_CTRL_CS_SEC_REQ_LEN            (20)
+
 /* API */
 struct ble_ll_conn_sm;
 void ble_ll_ctrl_proc_start(struct ble_ll_conn_sm *connsm, int ctrl_proc,
diff --git a/nimble/controller/src/ble_ll_ctrl.c b/nimble/controller/src/ble_ll_ctrl.c
index a05b5c9d8b..d2b7bca9bc 100644
--- a/nimble/controller/src/ble_ll_ctrl.c
+++ b/nimble/controller/src/ble_ll_ctrl.c
@@ -128,6 +128,22 @@ const uint8_t g_ble_ll_ctrl_pkt_lengths[BLE_LL_CTRL_OPCODES] =
     BLE_LL_CTRL_SUBRATE_IND_LEN,
     BLE_LL_CTRL_CHAN_REPORTING_IND_LEN,
     BLE_LL_CTRL_CHAN_STATUS_IND_LEN,
+    BLE_LL_CTRL_PERIODIC_SYNC_WR_IND_LEN,
+    BLE_LL_CTRL_FEATURE_EXT_REQ_LEN,
+    BLE_LL_CTRL_FEATURE_EXT_RSP_LEN,
+    BLE_LL_CTRL_CS_SEC_RSP_LEN,
+    BLE_LL_CTRL_CS_CAPABILITIES_REQ_LEN,
+    BLE_LL_CTRL_CS_CAPABILITIES_RSP_LEN,
+    BLE_LL_CTRL_CS_CONFIG_REQ_LEN,
+    BLE_LL_CTRL_CS_CONFIG_RSP_LEN,
+    BLE_LL_CTRL_CS_REQ_LEN,
+    BLE_LL_CTRL_CS_RSP_LEN,
+    BLE_LL_CTRL_CS_IND_LEN,
+    BLE_LL_CTRL_CS_TERMINATE_IND_LEN,
+    BLE_LL_CTRL_CS_FAE_REQ_LEN,
+    BLE_LL_CTRL_CS_FAE_RSP_LEN,
+    BLE_LL_CTRL_CS_CHANNEL_MAP_IND_LEN,
+    BLE_LL_CTRL_CS_SEC_REQ_LEN,
 };
 
 /**

From b46518c5da305a396ab29be3c2ffc34fbf7c6a3d Mon Sep 17 00:00:00 2001
From: Magdalena Kasenberg <magdalena.kasenberg@codecoup.pl>
Date: Sun, 14 Apr 2024 10:10:14 +0200
Subject: [PATCH 07/14] nimble/ll: Add Channel Sounding capabilities

Implement the LE CS Read Local Supported Capabilities command.
Enable basic CS capabilities in local controller.
---
 .../controller/include/controller/ble_ll_cs.h |   3 +
 nimble/controller/src/ble_ll.c                |  11 ++
 nimble/controller/src/ble_ll_cs.c             | 104 +++++++++++++++++-
 nimble/controller/src/ble_ll_cs_priv.h        |  25 +++++
 nimble/controller/src/ble_ll_ctrl.c           |   4 +
 5 files changed, 145 insertions(+), 2 deletions(-)

diff --git a/nimble/controller/include/controller/ble_ll_cs.h b/nimble/controller/include/controller/ble_ll_cs.h
index fb7cb4a59b..ebc5f731e2 100644
--- a/nimble/controller/include/controller/ble_ll_cs.h
+++ b/nimble/controller/include/controller/ble_ll_cs.h
@@ -28,6 +28,9 @@
 extern "C" {
 #endif
 
+void ble_ll_cs_init(void);
+void ble_ll_cs_reset(void);
+
 /* HCI handlers */
 int ble_ll_cs_hci_rd_loc_supp_cap(uint8_t *rspbuf, uint8_t *rsplen);
 int ble_ll_cs_hci_rd_rem_supp_cap(const uint8_t *cmdbuf, uint8_t cmdlen);
diff --git a/nimble/controller/src/ble_ll.c b/nimble/controller/src/ble_ll.c
index a0d6dbbc03..b35b819b2c 100644
--- a/nimble/controller/src/ble_ll.c
+++ b/nimble/controller/src/ble_ll.c
@@ -52,6 +52,9 @@
 #if MYNEWT_VAL(BLE_LL_ISO_BROADCASTER)
 #include "controller/ble_ll_iso_big.h"
 #endif
+#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING)
+#include "controller/ble_ll_cs.h"
+#endif
 #if MYNEWT_VAL(BLE_LL_EXT)
 #include "controller/ble_ll_ext.h"
 #endif
@@ -1704,6 +1707,10 @@ ble_ll_reset(void)
     ble_ll_iso_reset();
 #endif
 
+#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING)
+    ble_ll_cs_reset();
+#endif
+
     /* Re-initialize the PHY */
     rc = ble_phy_init();
 
@@ -1990,6 +1997,10 @@ ble_ll_init(void)
     ble_ll_iso_big_init();
 #endif
 
+#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING)
+    ble_ll_cs_init();
+#endif
+
 #if MYNEWT_VAL(BLE_LL_EXT)
     ble_ll_ext_init();
 #endif
diff --git a/nimble/controller/src/ble_ll_cs.c b/nimble/controller/src/ble_ll_cs.c
index dc3cfe94fb..23b717a323 100644
--- a/nimble/controller/src/ble_ll_cs.c
+++ b/nimble/controller/src/ble_ll_cs.c
@@ -26,13 +26,77 @@
 #include "controller/ble_ll_conn.h"
 #include "controller/ble_ll_hci.h"
 #include "controller/ble_ll_cs.h"
-
+#include "controller/ble_ll_cs_priv.h"
+
+#define T_IP1_CAP_ID_10US 0
+#define T_IP1_CAP_ID_20US 1
+#define T_IP1_CAP_ID_30US 2
+#define T_IP1_CAP_ID_40US 3
+#define T_IP1_CAP_ID_50US 4
+#define T_IP1_CAP_ID_60US 5
+#define T_IP1_CAP_ID_80US 6
+#define T_IP1_CAP_ID_145US 7
+
+#define T_IP2_CAP_ID_10US 0
+#define T_IP2_CAP_ID_20US 1
+#define T_IP2_CAP_ID_30US 2
+#define T_IP2_CAP_ID_40US 3
+#define T_IP2_CAP_ID_50US 4
+#define T_IP2_CAP_ID_60US 5
+#define T_IP2_CAP_ID_80US 6
+#define T_IP2_CAP_ID_145US 7
+
+#define T_FCS_CAP_ID_15US 0
+#define T_FCS_CAP_ID_20US 1
+#define T_FCS_CAP_ID_30US 2
+#define T_FCS_CAP_ID_40US 3
+#define T_FCS_CAP_ID_50US 4
+#define T_FCS_CAP_ID_60US 5
+#define T_FCS_CAP_ID_80US 6
+#define T_FCS_CAP_ID_100US 7
+#define T_FCS_CAP_ID_120US 8
+#define T_FCS_CAP_ID_150US 9
+
+#define T_PM_CAP_ID_10US 0
+#define T_PM_CAP_ID_20US 1
+#define T_PM_CAP_ID_40US 2
+
+static struct ble_ll_cs_supp_cap g_ble_ll_cs_local_cap;
 static struct ble_ll_cs_sm g_ble_ll_cs_sm[MYNEWT_VAL(BLE_MAX_CONNECTIONS)];
 
 int
 ble_ll_cs_hci_rd_loc_supp_cap(uint8_t *rspbuf, uint8_t *rsplen)
 {
-    return BLE_ERR_UNSUPPORTED;
+    const struct ble_ll_cs_supp_cap *cap = &g_ble_ll_cs_local_cap;
+    struct ble_hci_le_cs_rd_loc_supp_cap_rp *rsp = (void *)rspbuf;
+
+    rsp->num_config_supported = cap->max_number_of_configs;
+    rsp->max_consecutive_procedures_supported = htole16(cap->max_number_of_procedures);
+    rsp->num_antennas_supported = cap->number_of_antennas;
+    rsp->max_antenna_paths_supported = cap->max_number_of_antenna_paths;
+    rsp->roles_supported = cap->roles_supported;
+    rsp->optional_modes_supported = cap->mode_types;
+    rsp->rtt_capability = cap->rtt_capability;
+    rsp->rtt_aa_only_n = cap->rtt_aa_only_n;
+    rsp->rtt_sounding_n = cap->rtt_sounding_n;
+    rsp->rtt_random_payload_n = cap->rtt_random_sequence_n;
+    rsp->optional_nadm_sounding_capability = htole16(cap->nadm_sounding_capability);
+    rsp->optional_nadm_random_capability = htole16(cap->nadm_random_sequence_capability);
+    rsp->optional_cs_sync_phys_supported = cap->cs_sync_phy_capability;
+    rsp->optional_subfeatures_supported = htole16(0x000f &
+                                                  (cap->no_fae << 1 |
+                                                   cap->channel_selection << 2 |
+                                                   cap->sounding_pct_estimate << 3));
+    rsp->optional_t_ip1_times_supported = htole16(cap->t_ip1_capability & ~(1 << T_IP1_CAP_ID_145US));
+    rsp->optional_t_ip2_times_supported = htole16(cap->t_ip2_capability & ~(1 << T_IP2_CAP_ID_145US));
+    rsp->optional_t_fcs_times_supported = htole16(cap->t_fcs_capability & ~(1 << T_FCS_CAP_ID_150US));
+    rsp->optional_t_pm_times_supported = htole16(cap->t_pm_capability & ~(1 << T_PM_CAP_ID_40US));
+    rsp->t_sw_time_supported = cap->t_sw;
+    rsp->optional_tx_snr_capability = cap->tx_snr_capablity;
+
+    *rsplen = sizeof(*rsp);
+
+    return BLE_ERR_SUCCESS;
 }
 
 int
@@ -118,6 +182,42 @@ ble_ll_cs_hci_test_end(void)
     return BLE_ERR_UNSUPPORTED;
 }
 
+void
+ble_ll_cs_init(void)
+{
+    struct ble_ll_cs_supp_cap *cap = &g_ble_ll_cs_local_cap;
+
+    /* Set local CS capabilities. Only basic features supported for now. */
+    cap->mode_types = 0x00;
+    cap->rtt_capability = 0x00;
+    cap->rtt_aa_only_n = 0x00;
+    cap->rtt_sounding_n = 0x00;
+    cap->rtt_random_sequence_n = 0x00;
+    cap->nadm_sounding_capability = 0x0000;
+    cap->nadm_random_sequence_capability = 0x0000;
+    cap->cs_sync_phy_capability = 0x00;
+    cap->number_of_antennas = 0x01;
+    cap->max_number_of_antenna_paths = 0x01;
+    cap->roles_supported = 0x03;
+    cap->no_fae = 0x00;
+    cap->channel_selection = 0x00;
+    cap->sounding_pct_estimate = 0x00;
+    cap->max_number_of_configs = 0x04;
+    cap->max_number_of_procedures = 0x0001;
+    cap->t_sw = 0x0A;
+    cap->t_ip1_capability = 1 << T_IP1_CAP_ID_145US;
+    cap->t_ip2_capability = 1 << T_IP2_CAP_ID_145US;
+    cap->t_fcs_capability = 1 << T_FCS_CAP_ID_150US;
+    cap->t_pm_capability = 1 << T_PM_CAP_ID_40US;
+    cap->tx_snr_capablity = 0x00;
+}
+
+void
+ble_ll_cs_reset(void)
+{
+    ble_ll_cs_init();
+}
+
 void
 ble_ll_cs_sm_init(struct ble_ll_conn_sm *connsm)
 {
diff --git a/nimble/controller/src/ble_ll_cs_priv.h b/nimble/controller/src/ble_ll_cs_priv.h
index 3f0d03346e..480ee53dbd 100644
--- a/nimble/controller/src/ble_ll_cs_priv.h
+++ b/nimble/controller/src/ble_ll_cs_priv.h
@@ -27,6 +27,31 @@
 extern "C" {
 #endif
 
+struct ble_ll_cs_supp_cap {
+    uint8_t mode_types;
+    uint8_t roles_supported;
+    uint8_t rtt_capability;
+    uint8_t rtt_aa_only_n;
+    uint8_t rtt_sounding_n;
+    uint8_t rtt_random_sequence_n;
+    uint16_t nadm_sounding_capability;
+    uint16_t nadm_random_sequence_capability;
+    uint8_t cs_sync_phy_capability;
+    uint8_t number_of_antennas;
+    uint8_t max_number_of_antenna_paths;
+    uint8_t no_fae;
+    uint8_t channel_selection;
+    uint8_t sounding_pct_estimate;
+    uint8_t max_number_of_configs;
+    uint16_t max_number_of_procedures;
+    uint16_t t_ip1_capability;
+    uint16_t t_ip2_capability;
+    uint16_t t_fcs_capability;
+    uint16_t t_pm_capability;
+    uint8_t t_sw;
+    uint8_t tx_snr_capablity;
+};
+
 struct ble_ll_cs_sm {
     struct ble_ll_conn_sm *connsm;
 };
diff --git a/nimble/controller/src/ble_ll_ctrl.c b/nimble/controller/src/ble_ll_ctrl.c
index d2b7bca9bc..62323958c8 100644
--- a/nimble/controller/src/ble_ll_ctrl.c
+++ b/nimble/controller/src/ble_ll_ctrl.c
@@ -49,6 +49,10 @@
 #include "console/console.h"
 #endif
 
+#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING)
+#include "controller/ble_ll_cs.h"
+#endif
+
 /*
  * XXX:
  *  1) Do I need to keep track of which procedures have already been done?

From 42f6a6dda8f5cac669ee2a03da3b74d7eef9e1f4 Mon Sep 17 00:00:00 2001
From: Magdalena Kasenberg <magdalena.kasenberg@codecoup.pl>
Date: Sun, 14 Apr 2024 12:51:53 +0200
Subject: [PATCH 08/14] nimble/ll: Add CS Capabilities Exchange procedure

Implements:
- LE CS Read Remote Supported Capabilities command,
- LE CS Write Cached Remote Supported Capabilities command.
---
 .../controller/include/controller/ble_ll_cs.h |   6 +
 .../include/controller/ble_ll_ctrl.h          |   3 +-
 nimble/controller/src/ble_ll_cs.c             | 247 +++++++++++++++++-
 nimble/controller/src/ble_ll_cs_priv.h        |   1 +
 nimble/controller/src/ble_ll_ctrl.c           |  19 ++
 5 files changed, 272 insertions(+), 4 deletions(-)

diff --git a/nimble/controller/include/controller/ble_ll_cs.h b/nimble/controller/include/controller/ble_ll_cs.h
index ebc5f731e2..1da8265424 100644
--- a/nimble/controller/include/controller/ble_ll_cs.h
+++ b/nimble/controller/include/controller/ble_ll_cs.h
@@ -31,6 +31,12 @@ extern "C" {
 void ble_ll_cs_init(void);
 void ble_ll_cs_reset(void);
 
+void ble_ll_cs_capabilities_pdu_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr);
+
+int ble_ll_cs_rx_capabilities_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf);
+void ble_ll_cs_rx_capabilities_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr);
+void ble_ll_cs_rx_capabilities_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error);
+
 /* HCI handlers */
 int ble_ll_cs_hci_rd_loc_supp_cap(uint8_t *rspbuf, uint8_t *rsplen);
 int ble_ll_cs_hci_rd_rem_supp_cap(const uint8_t *cmdbuf, uint8_t cmdlen);
diff --git a/nimble/controller/include/controller/ble_ll_ctrl.h b/nimble/controller/include/controller/ble_ll_ctrl.h
index 72e0bf552c..6d5343eb73 100644
--- a/nimble/controller/include/controller/ble_ll_ctrl.h
+++ b/nimble/controller/include/controller/ble_ll_ctrl.h
@@ -43,7 +43,8 @@ extern "C" {
 #define BLE_LL_CTRL_PROC_CIS_CREATE     (11)
 #define BLE_LL_CTRL_PROC_SUBRATE_REQ    (12)
 #define BLE_LL_CTRL_PROC_SUBRATE_UPDATE (13)
-#define BLE_LL_CTRL_PROC_NUM            (14)
+#define BLE_LL_CTRL_PROC_CS_CAP_XCHG    (14)
+#define BLE_LL_CTRL_PROC_NUM            (15)
 #define BLE_LL_CTRL_PROC_IDLE           (255)
 
 /* Checks if a particular control procedure is running */
diff --git a/nimble/controller/src/ble_ll_cs.c b/nimble/controller/src/ble_ll_cs.c
index 23b717a323..fc04f21539 100644
--- a/nimble/controller/src/ble_ll_cs.c
+++ b/nimble/controller/src/ble_ll_cs.c
@@ -26,7 +26,8 @@
 #include "controller/ble_ll_conn.h"
 #include "controller/ble_ll_hci.h"
 #include "controller/ble_ll_cs.h"
-#include "controller/ble_ll_cs_priv.h"
+#include "ble_ll_conn_priv.h"
+#include "ble_ll_cs_priv.h"
 
 #define T_IP1_CAP_ID_10US 0
 #define T_IP1_CAP_ID_20US 1
@@ -99,17 +100,257 @@ ble_ll_cs_hci_rd_loc_supp_cap(uint8_t *rspbuf, uint8_t *rsplen)
     return BLE_ERR_SUCCESS;
 }
 
+void
+ble_ll_cs_capabilities_pdu_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    const struct ble_ll_cs_supp_cap *cap = &g_ble_ll_cs_local_cap;
+
+    *dptr = cap->mode_types;
+    dptr[1] = cap->rtt_capability;
+    dptr[2] = cap->rtt_aa_only_n;
+    dptr[3] = cap->rtt_sounding_n;
+    dptr[4] = cap->rtt_random_sequence_n;
+    put_le16(dptr + 5, cap->nadm_sounding_capability);
+    put_le16(dptr + 7, cap->nadm_random_sequence_capability);
+    dptr[9] = cap->cs_sync_phy_capability;
+    dptr[10] = cap->number_of_antennas | cap->max_number_of_antenna_paths << 4;
+    dptr[11] = cap->roles_supported |
+               cap->no_fae << 3 |
+               cap->channel_selection << 4 |
+               cap->sounding_pct_estimate << 5;
+    dptr[12] = cap->max_number_of_configs;
+    put_le16(dptr + 13, cap->max_number_of_procedures);
+    dptr[15] = cap->t_sw;
+    put_le16(dptr + 16, cap->t_ip1_capability & ~(1 << T_IP1_CAP_ID_145US));
+    put_le16(dptr + 18, cap->t_ip2_capability & ~(1 << T_IP2_CAP_ID_145US));
+    put_le16(dptr + 20, cap->t_fcs_capability & ~(1 << T_FCS_CAP_ID_150US));
+    put_le16(dptr + 22, cap->t_pm_capability & ~(1 << T_PM_CAP_ID_40US));
+    dptr[24] = cap->tx_snr_capablity << 1;
+}
+
+static void
+ble_ll_cs_update_rem_capabilities(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    struct ble_ll_cs_supp_cap *cap = &connsm->cssm->remote_cap;
+
+    cap->mode_types = *dptr & 0x01;
+    cap->rtt_capability = dptr[1] & 0x05;
+    cap->rtt_aa_only_n = dptr[2];
+    cap->rtt_sounding_n = dptr[3];
+    cap->rtt_random_sequence_n = dptr[4];
+    cap->nadm_sounding_capability = get_le16(dptr + 5) & 0x01;
+    cap->nadm_random_sequence_capability = get_le16(dptr + 7) & 0x01;
+    cap->cs_sync_phy_capability = dptr[9] & 0x06;
+
+    cap->number_of_antennas = dptr[10] & 0b00001111;
+    cap->max_number_of_antenna_paths = dptr[10] >> 4;
+
+    cap->roles_supported = dptr[11] & 0b00000011;
+    cap->no_fae = (dptr[11] & 0b00001000) >> 3;
+    cap->channel_selection = (dptr[11] & 0b00010000) >> 4;
+    cap->sounding_pct_estimate = (dptr[11] & 0b00100000) >> 5;
+
+    cap->max_number_of_configs = dptr[12];
+    cap->max_number_of_procedures = get_le16(dptr + 13);
+    cap->t_sw = dptr[15];
+    cap->t_ip1_capability = (get_le16(dptr + 16) & 0x00FF) | (1 << T_IP1_CAP_ID_145US);
+    cap->t_ip2_capability = (get_le16(dptr + 18) & 0x00FF) | (1 << T_IP1_CAP_ID_145US);
+    cap->t_fcs_capability = (get_le16(dptr + 20) & 0x03FF) | (1 << T_FCS_CAP_ID_150US);
+    cap->t_pm_capability = (get_le16(dptr + 22) & 0x07) | (1 << T_PM_CAP_ID_40US);
+    cap->tx_snr_capablity = (dptr[24] >> 1) & 0b01111111;
+
+    /* The capabilites contain info about allowed values for
+     * CS procedures. Ignore the RFU values here.
+     * We will be able to reject/renegotiate unsupported values
+     * if the remote controller will use them in the procedures.
+     */
+
+    if (cap->number_of_antennas > 4) {
+        cap->number_of_antennas = 4;
+    }
+
+    if (cap->max_number_of_antenna_paths > 4) {
+        cap->max_number_of_antenna_paths = 4;
+    }
+
+    if (cap->max_number_of_antenna_paths < cap->number_of_antennas) {
+        cap->number_of_antennas = cap->max_number_of_antenna_paths;
+    }
+
+    if (cap->max_number_of_configs > 4) {
+        cap->max_number_of_configs = 4;
+    }
+
+    if (!(cap->t_sw == 0x00 ||
+          cap->t_sw == 0x01 ||
+          cap->t_sw == 0x02 ||
+          cap->t_sw == 0x04 ||
+          cap->t_sw == 0x0A)) {
+        /* If the remote does not support a valid duration of the antenna switch period,
+         * lets assume it does not support the antenna switching at all.
+         */
+        cap->number_of_antennas = 1;
+        cap->t_sw = 0;
+    }
+}
+
+static void
+ble_ll_cs_ev_rd_rem_supp_cap(struct ble_ll_conn_sm *connsm, uint8_t status)
+{
+    const struct ble_ll_cs_supp_cap *cap = &connsm->cssm->remote_cap;
+    struct ble_hci_ev_le_subev_cs_rd_rem_supp_cap_complete *ev;
+    struct ble_hci_ev *hci_ev;
+
+    if (ble_ll_hci_is_le_event_enabled(
+            BLE_HCI_LE_SUBEV_CS_RD_REM_SUPP_CAP_COMPLETE)) {
+        hci_ev = ble_transport_alloc_evt(0);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+            hci_ev->length = sizeof(*ev);
+            ev = (void *) hci_ev->data;
+
+            memset(ev, 0, sizeof(*ev));
+            ev->subev_code = BLE_HCI_LE_SUBEV_CS_RD_REM_SUPP_CAP_COMPLETE;
+            ev->status = status;
+            ev->conn_handle = htole16(connsm->conn_handle);
+
+            if (status == BLE_ERR_SUCCESS) {
+                ev->num_config_supported = cap->max_number_of_configs;
+                ev->max_consecutive_procedures_supported = htole16(cap->max_number_of_procedures);
+                ev->num_antennas_supported = cap->number_of_antennas;
+                ev->max_antenna_paths_supported = cap->max_number_of_antenna_paths;
+                ev->roles_supported = cap->roles_supported;
+                ev->optional_modes_supported = cap->mode_types;
+                ev->rtt_capability = cap->rtt_capability;
+                ev->rtt_aa_only_n = cap->rtt_aa_only_n;
+                ev->rtt_sounding_n = cap->rtt_sounding_n;
+                ev->rtt_random_payload_n = cap->rtt_random_sequence_n;
+                ev->optional_nadm_sounding_capability = htole16(cap->nadm_sounding_capability);
+                ev->optional_nadm_random_capability = htole16(cap->nadm_random_sequence_capability);
+                ev->optional_cs_sync_phys_supported = cap->cs_sync_phy_capability;
+                ev->optional_subfeatures_supported = htole16(cap->no_fae << 1 |
+                                                            cap->channel_selection << 2 |
+                                                            cap->sounding_pct_estimate << 3);
+                ev->optional_t_ip1_times_supported = htole16(cap->t_ip1_capability & ~(1 << T_IP1_CAP_ID_145US));
+                ev->optional_t_ip2_times_supported = htole16(cap->t_ip2_capability & ~(1 << T_IP2_CAP_ID_145US));
+                ev->optional_t_fcs_times_supported = htole16(cap->t_fcs_capability & ~(1 << T_FCS_CAP_ID_150US));
+                ev->optional_t_pm_times_supported = htole16(cap->t_pm_capability & ~(1 << T_PM_CAP_ID_40US));
+                ev->t_sw_time_supported = cap->t_sw;
+                ev->optional_tx_snr_capability = cap->tx_snr_capablity;
+            }
+
+            ble_ll_hci_event_send(hci_ev);
+        }
+    }
+}
+
+int
+ble_ll_cs_rx_capabilities_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+                              uint8_t *rspbuf)
+{
+    ble_ll_cs_update_rem_capabilities(connsm, dptr);
+
+    ble_ll_cs_capabilities_pdu_make(connsm, rspbuf);
+
+    return BLE_LL_CTRL_CS_CAPABILITIES_RSP;
+}
+
+void
+ble_ll_cs_rx_capabilities_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error)
+{
+    /* Stop the control procedure and send an event to the host */
+    ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_CAP_XCHG);
+    ble_ll_cs_ev_rd_rem_supp_cap(connsm, ble_error);
+}
+
+void
+ble_ll_cs_rx_capabilities_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    if (!IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CAP_XCHG)) {
+        /* Ignore */
+        return;
+    }
+
+    ble_ll_cs_update_rem_capabilities(connsm, dptr);
+
+    /* Stop the control procedure and send an event to the host */
+    ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_CAP_XCHG);
+    ble_ll_cs_ev_rd_rem_supp_cap(connsm, BLE_ERR_SUCCESS);
+}
+
 int
 ble_ll_cs_hci_rd_rem_supp_cap(const uint8_t *cmdbuf, uint8_t cmdlen)
 {
-    return BLE_ERR_UNSUPPORTED;
+    const struct ble_hci_le_cs_rd_rem_supp_cap_cp *cmd = (const void *)cmdbuf;
+    struct ble_ll_conn_sm *connsm;
+
+    if (cmdlen != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* If no connection handle exit with error */
+    connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+    if (!connsm) {
+        return BLE_ERR_UNK_CONN_ID;
+    }
+
+    /* If already pending exit with error */
+    if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CAP_XCHG)) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_CAP_XCHG, NULL);
+
+    return BLE_ERR_SUCCESS;
 }
 
 int
 ble_ll_cs_hci_wr_cached_rem_supp_cap(const uint8_t *cmdbuf, uint8_t cmdlen,
                                      uint8_t *rspbuf, uint8_t *rsplen)
 {
-    return BLE_ERR_UNSUPPORTED;
+    const struct ble_hci_le_cs_wr_cached_rem_supp_cap_cp *cmd = (const void *)cmdbuf;
+    struct ble_hci_le_cs_wr_cached_rem_supp_cap_rp *rsp = (void *)rspbuf;
+    struct ble_ll_cs_supp_cap *cap;
+    struct ble_ll_conn_sm *connsm;
+    uint16_t subfeatures;
+
+    connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+    if (!connsm) {
+        return BLE_ERR_UNK_CONN_ID;
+    }
+
+    cap = &connsm->cssm->remote_cap;
+
+    cap->max_number_of_configs = cmd->num_config_supported;
+    cap->max_number_of_procedures = le16toh(cmd->max_consecutive_procedures_supported);
+    cap->number_of_antennas = cmd->num_antennas_supported;
+    cap->max_number_of_antenna_paths = cmd->max_antenna_paths_supported;
+    cap->roles_supported = cmd->roles_supported;
+    cap->mode_types = cmd->optional_modes_supported;
+    cap->rtt_capability = cmd->rtt_capability;
+    cap->rtt_aa_only_n = cmd->rtt_aa_only_n;
+    cap->rtt_sounding_n = cmd->rtt_sounding_n;
+    cap->rtt_random_sequence_n = cmd->rtt_random_payload_n;
+    cap->nadm_sounding_capability = le16toh(cmd->optional_nadm_sounding_capability);
+    cap->nadm_random_sequence_capability = le16toh(cmd->optional_nadm_random_capability);
+    cap->cs_sync_phy_capability = cmd->optional_cs_sync_phys_supported;
+
+    subfeatures = le16toh(cmd->optional_subfeatures_supported);
+    cap->no_fae = (subfeatures >> 1) & 1;
+    cap->channel_selection = (subfeatures >> 2) & 1;
+    cap->sounding_pct_estimate = (subfeatures >> 3) & 1;
+
+    cap->t_ip1_capability = le16toh(cmd->optional_t_ip1_times_supported) | (1 << T_IP1_CAP_ID_145US);
+    cap->t_ip2_capability = le16toh(cmd->optional_t_ip2_times_supported) | (1 << T_IP2_CAP_ID_145US);
+    cap->t_fcs_capability = le16toh(cmd->optional_t_fcs_times_supported) | (1 << T_FCS_CAP_ID_150US);
+    cap->t_pm_capability = le16toh(cmd->optional_t_pm_times_supported) | (1 << T_PM_CAP_ID_40US);
+    cap->t_sw = cmd->t_sw_time_supported;
+    cap->tx_snr_capablity = cmd->optional_tx_snr_capability;
+
+    rsp->conn_handle = cmd->conn_handle;
+    *rsplen = sizeof(*rsp);
+
+    return BLE_ERR_SUCCESS;
 }
 
 int
diff --git a/nimble/controller/src/ble_ll_cs_priv.h b/nimble/controller/src/ble_ll_cs_priv.h
index 480ee53dbd..dbee15cefe 100644
--- a/nimble/controller/src/ble_ll_cs_priv.h
+++ b/nimble/controller/src/ble_ll_cs_priv.h
@@ -54,6 +54,7 @@ struct ble_ll_cs_supp_cap {
 
 struct ble_ll_cs_sm {
     struct ble_ll_conn_sm *connsm;
+    struct ble_ll_cs_supp_cap remote_cap;
 };
 
 #ifdef __cplusplus
diff --git a/nimble/controller/src/ble_ll_ctrl.c b/nimble/controller/src/ble_ll_ctrl.c
index 62323958c8..5b6335ad3c 100644
--- a/nimble/controller/src/ble_ll_ctrl.c
+++ b/nimble/controller/src/ble_ll_ctrl.c
@@ -2065,6 +2065,11 @@ ble_ll_ctrl_rx_reject_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
         ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_SUBRATE_UPDATE);
         break;
 #endif
+#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING)
+    case BLE_LL_CTRL_PROC_CS_CAP_XCHG:
+        ble_ll_cs_rx_capabilities_req_rejected(connsm, ble_error);
+        break;
+#endif
 
     default:
         break;
@@ -2587,6 +2592,12 @@ ble_ll_ctrl_proc_init(struct ble_ll_conn_sm *connsm, int ctrl_proc, void *data)
                                          &connsm->subrate_trans);
             break;
 #endif
+#endif
+#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING)
+        case BLE_LL_CTRL_PROC_CS_CAP_XCHG:
+            opcode = BLE_LL_CTRL_CS_CAPABILITIES_REQ;
+            ble_ll_cs_capabilities_pdu_make(connsm, ctrdata);
+            break;
 #endif
         default:
             BLE_LL_ASSERT(0);
@@ -3057,6 +3068,14 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om)
     case BLE_LL_CTRL_SUBRATE_IND:
         rsp_opcode = ble_ll_ctrl_rx_subrate_ind(connsm, dptr, rspdata);
         break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING)
+    case BLE_LL_CTRL_CS_CAPABILITIES_REQ:
+        rsp_opcode = ble_ll_cs_rx_capabilities_req(connsm, dptr, rspdata);
+        break;
+    case BLE_LL_CTRL_CS_CAPABILITIES_RSP:
+        ble_ll_cs_rx_capabilities_rsp(connsm, dptr);
+        break;
 #endif
     default:
         /* Nothing to do here */

From 75d0d97ca6530c81a93f356d5f63860c9eae0f4c Mon Sep 17 00:00:00 2001
From: Magdalena Kasenberg <magdalena.kasenberg@codecoup.pl>
Date: Mon, 15 Apr 2024 09:09:13 +0200
Subject: [PATCH 09/14] nimble/ll: Add LE CS Set Default Settings command

The HCI_LE_CS_Set_Default_Settings command is used by a Host to set
default CS settings in the local Controller for the connection.
---
 nimble/controller/src/ble_ll_cs.c      | 63 +++++++++++++++++++++++++-
 nimble/controller/src/ble_ll_cs_priv.h | 17 +++++++
 2 files changed, 79 insertions(+), 1 deletion(-)

diff --git a/nimble/controller/src/ble_ll_cs.c b/nimble/controller/src/ble_ll_cs.c
index fc04f21539..316561a1b5 100644
--- a/nimble/controller/src/ble_ll_cs.c
+++ b/nimble/controller/src/ble_ll_cs.c
@@ -363,7 +363,68 @@ int
 ble_ll_cs_hci_set_def_settings(const uint8_t *cmdbuf, uint8_t cmdlen,
                                uint8_t *rspbuf, uint8_t *rsplen)
 {
-    return BLE_ERR_UNSUPPORTED;
+    const struct ble_hci_le_cs_set_def_settings_cp *cmd = (const void *)cmdbuf;
+    struct ble_hci_le_cs_set_def_settings_rp *rsp = (void *)rspbuf;
+    const struct ble_ll_cs_supp_cap *cap = &g_ble_ll_cs_local_cap;
+    struct ble_ll_conn_sm *connsm;
+    struct ble_ll_cs_sm *cssm;
+    uint8_t i;
+
+    connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+    if (!connsm) {
+        return BLE_ERR_UNK_CONN_ID;
+    }
+
+    cssm = connsm->cssm;
+
+    /* Check if a disabled role is used in CS configs */
+    for (i = 0; i < ARRAY_SIZE(cssm->config); i++) {
+        struct ble_ll_cs_config *conf = &cssm->config[i];
+
+        if (conf->config_enabled && (1 << conf->role) & ~cmd->role_enable) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+    }
+
+    if ((cmd->role_enable & ~cap->roles_supported) != 0 ||
+        (cap->number_of_antennas < cmd->cs_sync_antenna_selection &&
+         cmd->cs_sync_antenna_selection < 0xFE)) {
+        /* Unsupported role or antenna selection used */
+        return BLE_ERR_UNSUPPORTED;
+    }
+
+    /* Allowed Transmit_Power_Level range: -127 to +20,
+     * (Complement system + special meaning for 0x7E and 0x7F)
+     */
+    if (!(IN_RANGE(cmd->max_tx_power, 0x00, 0x14) ||
+          IN_RANGE(cmd->max_tx_power, 0x7E, 0xFF))) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (cmd->max_tx_power == 0x7E) {
+        /* TODO: Set transmitter to minimum transmit power level
+         * supported by the board.
+         */
+        cssm->max_tx_power = 0x80;
+    } else if (cmd->max_tx_power == 0x7F) {
+        /* TODO: Set transmitter to maximum transmit power level
+         * supported by the board.
+         */
+        cssm->max_tx_power = 0x14;
+    } else {
+        /* TODO: Set transmitter to the nearest transmit power level
+         * supported by the board.
+         */
+        cssm->max_tx_power = cmd->max_tx_power;
+    }
+
+    cssm->roles_enabled = cmd->role_enable;
+    cssm->cs_sync_antenna_selection = cmd->cs_sync_antenna_selection;
+
+    rsp->conn_handle = cmd->conn_handle;
+    *rsplen = sizeof(*rsp);
+
+    return BLE_ERR_SUCCESS;
 }
 
 int
diff --git a/nimble/controller/src/ble_ll_cs_priv.h b/nimble/controller/src/ble_ll_cs_priv.h
index dbee15cefe..67c32be0e3 100644
--- a/nimble/controller/src/ble_ll_cs_priv.h
+++ b/nimble/controller/src/ble_ll_cs_priv.h
@@ -27,6 +27,8 @@
 extern "C" {
 #endif
 
+#define BLE_LL_CS_CONFIG_MAX_NUM 4
+
 struct ble_ll_cs_supp_cap {
     uint8_t mode_types;
     uint8_t roles_supported;
@@ -52,9 +54,24 @@ struct ble_ll_cs_supp_cap {
     uint8_t tx_snr_capablity;
 };
 
+struct ble_ll_cs_config {
+    uint8_t config_enabled;
+    /* The role to use in CS procedure
+     * 0x00 = Initiator,
+     * 0x01 = Reflector
+     */
+    uint8_t role;
+};
+
 struct ble_ll_cs_sm {
     struct ble_ll_conn_sm *connsm;
     struct ble_ll_cs_supp_cap remote_cap;
+    struct ble_ll_cs_config config[BLE_LL_CS_CONFIG_MAX_NUM];
+
+    /* Default Settings */
+    uint8_t roles_enabled;
+    uint8_t cs_sync_antenna_selection;
+    uint8_t max_tx_power;
 };
 
 #ifdef __cplusplus

From 23356834f159ef58034590c21ac0b07334d73c20 Mon Sep 17 00:00:00 2001
From: Magdalena Kasenberg <magdalena.kasenberg@codecoup.pl>
Date: Mon, 15 Apr 2024 12:56:39 +0200
Subject: [PATCH 10/14] nimble/ll: Add CS Mode-0 FAE Table Request procedure

Implements:
- LE CS Read Remote FAE Table command
- LE CS Write Cached Remote FAE Table command
---
 .../controller/include/controller/ble_ll_cs.h |  3 +
 .../include/controller/ble_ll_ctrl.h          |  7 +-
 nimble/controller/src/ble_ll_conn.c           |  8 +-
 nimble/controller/src/ble_ll_cs.c             | 91 ++++++++++++++++++-
 nimble/controller/src/ble_ll_cs_priv.h        |  4 +
 nimble/controller/src/ble_ll_ctrl.c           | 13 +++
 6 files changed, 120 insertions(+), 6 deletions(-)

diff --git a/nimble/controller/include/controller/ble_ll_cs.h b/nimble/controller/include/controller/ble_ll_cs.h
index 1da8265424..4fe96bb20e 100644
--- a/nimble/controller/include/controller/ble_ll_cs.h
+++ b/nimble/controller/include/controller/ble_ll_cs.h
@@ -36,6 +36,9 @@ void ble_ll_cs_capabilities_pdu_make(struct ble_ll_conn_sm *connsm, uint8_t *dpt
 int ble_ll_cs_rx_capabilities_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf);
 void ble_ll_cs_rx_capabilities_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr);
 void ble_ll_cs_rx_capabilities_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error);
+int ble_ll_cs_rx_fae_req(struct ble_ll_conn_sm *connsm, struct os_mbuf *om);
+void ble_ll_cs_rx_fae_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr);
+void ble_ll_cs_rx_fae_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error);
 
 /* HCI handlers */
 int ble_ll_cs_hci_rd_loc_supp_cap(uint8_t *rspbuf, uint8_t *rsplen);
diff --git a/nimble/controller/include/controller/ble_ll_ctrl.h b/nimble/controller/include/controller/ble_ll_ctrl.h
index 6d5343eb73..bc94f9a195 100644
--- a/nimble/controller/include/controller/ble_ll_ctrl.h
+++ b/nimble/controller/include/controller/ble_ll_ctrl.h
@@ -44,7 +44,8 @@ extern "C" {
 #define BLE_LL_CTRL_PROC_SUBRATE_REQ    (12)
 #define BLE_LL_CTRL_PROC_SUBRATE_UPDATE (13)
 #define BLE_LL_CTRL_PROC_CS_CAP_XCHG    (14)
-#define BLE_LL_CTRL_PROC_NUM            (15)
+#define BLE_LL_CTRL_PROC_CS_FAE_REQ     (15)
+#define BLE_LL_CTRL_PROC_NUM            (16)
 #define BLE_LL_CTRL_PROC_IDLE           (255)
 
 /* Checks if a particular control procedure is running */
@@ -124,7 +125,9 @@ extern "C" {
 extern const uint8_t g_ble_ll_ctrl_pkt_lengths[BLE_LL_CTRL_OPCODES];
 
 /* Maximum LL control PDU size */
-#if MYNEWT_VAL(BLE_ISO)
+#if MYNEWT_VAL(BLE_CHANNEL_SOUNDING)
+#define BLE_LL_CTRL_MAX_PDU_LEN         (73)
+#elif MYNEWT_VAL(BLE_ISO)
 #define BLE_LL_CTRL_MAX_PDU_LEN         (42)
 #elif MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
 #define BLE_LL_CTRL_MAX_PDU_LEN         (35)
diff --git a/nimble/controller/src/ble_ll_conn.c b/nimble/controller/src/ble_ll_conn.c
index 19ee2fbebd..6b9c9d498f 100644
--- a/nimble/controller/src/ble_ll_conn.c
+++ b/nimble/controller/src/ble_ll_conn.c
@@ -3984,9 +3984,13 @@ ble_ll_conn_enqueue_pkt(struct ble_ll_conn_sm *connsm, struct os_mbuf *om,
         return;
     }
 
-    /* Set mbuf length and packet length if a control PDU */
+    /* Set overall packet length if a control PDU */
     if (hdr_byte == BLE_LL_LLID_CTRL) {
-        om->om_len = length;
+        /* Set mbuf length if not chained mbufs */
+        if (SLIST_NEXT(om, om_next) == NULL) {
+            om->om_len = length;
+        }
+
         OS_MBUF_PKTHDR(om)->omp_len = length;
         num_pkt = 0;
     } else {
diff --git a/nimble/controller/src/ble_ll_cs.c b/nimble/controller/src/ble_ll_cs.c
index 316561a1b5..58ca028951 100644
--- a/nimble/controller/src/ble_ll_cs.c
+++ b/nimble/controller/src/ble_ll_cs.c
@@ -28,6 +28,7 @@
 #include "controller/ble_ll_cs.h"
 #include "ble_ll_conn_priv.h"
 #include "ble_ll_cs_priv.h"
+#include "os/os_mbuf.h"
 
 #define T_IP1_CAP_ID_10US 0
 #define T_IP1_CAP_ID_20US 1
@@ -427,17 +428,103 @@ ble_ll_cs_hci_set_def_settings(const uint8_t *cmdbuf, uint8_t cmdlen,
     return BLE_ERR_SUCCESS;
 }
 
+int
+ble_ll_cs_rx_fae_req(struct ble_ll_conn_sm *connsm, struct os_mbuf *om)
+{
+    /* Space for response code */
+    om->om_len = 1;
+    OS_MBUF_PKTLEN(om) = om->om_len;
+    os_mbuf_append(om, connsm->cssm->local_fae_table, 72);
+
+    return BLE_LL_CTRL_CS_FAE_RSP;
+}
+
+static void
+ble_ll_cs_ev_rd_rem_fae_complete(struct ble_ll_conn_sm *connsm, uint8_t status)
+{
+    struct ble_hci_ev_le_subev_cs_rd_rem_fae_complete *ev;
+    struct ble_hci_ev *hci_ev;
+
+    if (ble_ll_hci_is_le_event_enabled(
+            BLE_HCI_LE_SUBEV_CS_RD_REM_FAE_COMPLETE)) {
+        hci_ev = ble_transport_alloc_evt(0);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+            hci_ev->length = sizeof(*ev);
+            ev = (void *) hci_ev->data;
+
+            memset(ev, 0, sizeof(*ev));
+            ev->subev_code = BLE_HCI_LE_SUBEV_CS_RD_REM_FAE_COMPLETE;
+            ev->status = status;
+            ev->conn_handle = htole16(connsm->conn_handle);
+
+            if (status == BLE_ERR_SUCCESS) {
+                memcpy(ev->remote_fae_table, connsm->cssm->remote_fae_table, 72);
+            }
+
+            ble_ll_hci_event_send(hci_ev);
+        }
+    }
+}
+
+void
+ble_ll_cs_rx_fae_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    if (!IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_FAE_REQ)) {
+        /* Ignore */
+        return;
+    }
+
+    memcpy(connsm->cssm->remote_fae_table, dptr, 72);
+
+    /* Stop the control procedure and send an event to the host */
+    ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_FAE_REQ);
+    ble_ll_cs_ev_rd_rem_fae_complete(connsm, BLE_ERR_SUCCESS);
+}
+
+void
+ble_ll_cs_rx_fae_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error)
+{
+    /* Stop the control procedure and send an event to the host */
+    ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_FAE_REQ);
+    ble_ll_cs_ev_rd_rem_fae_complete(connsm, ble_error);
+}
+
 int
 ble_ll_cs_hci_rd_rem_fae(const uint8_t *cmdbuf, uint8_t cmdlen)
 {
-    return BLE_ERR_UNSUPPORTED;
+    const struct ble_hci_le_cs_rd_rem_fae_cp *cmd = (const void *)cmdbuf;
+    struct ble_ll_conn_sm *connsm;
+
+    connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+    if (!connsm) {
+        return BLE_ERR_UNK_CONN_ID;
+    }
+
+    ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_FAE_REQ, NULL);
+
+    return BLE_ERR_SUCCESS;
 }
 
 int
 ble_ll_cs_hci_wr_cached_rem_fae(const uint8_t *cmdbuf, uint8_t cmdlen,
                                 uint8_t *rspbuf, uint8_t *rsplen)
 {
-    return BLE_ERR_UNSUPPORTED;
+    const struct ble_hci_le_cs_wr_cached_rem_fae_cp *cmd = (const void *)cmdbuf;
+    struct ble_hci_le_cs_wr_cached_rem_fae_rp *rsp = (void *)rspbuf;
+    struct ble_ll_conn_sm *connsm;
+
+    connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+    if (!connsm) {
+        return BLE_ERR_UNK_CONN_ID;
+    }
+
+    memcpy(connsm->cssm->remote_fae_table, cmd->remote_fae_table, 72);
+
+    rsp->conn_handle = cmd->conn_handle;
+    *rsplen = sizeof(*rsp);
+
+    return BLE_ERR_SUCCESS;
 }
 
 int
diff --git a/nimble/controller/src/ble_ll_cs_priv.h b/nimble/controller/src/ble_ll_cs_priv.h
index 67c32be0e3..b8518e0539 100644
--- a/nimble/controller/src/ble_ll_cs_priv.h
+++ b/nimble/controller/src/ble_ll_cs_priv.h
@@ -72,6 +72,10 @@ struct ble_ll_cs_sm {
     uint8_t roles_enabled;
     uint8_t cs_sync_antenna_selection;
     uint8_t max_tx_power;
+
+    /* Cached FAE tables */
+    uint8_t remote_fae_table[72];
+    uint8_t local_fae_table[72];
 };
 
 #ifdef __cplusplus
diff --git a/nimble/controller/src/ble_ll_ctrl.c b/nimble/controller/src/ble_ll_ctrl.c
index 5b6335ad3c..d1ed35c9a2 100644
--- a/nimble/controller/src/ble_ll_ctrl.c
+++ b/nimble/controller/src/ble_ll_ctrl.c
@@ -2069,6 +2069,9 @@ ble_ll_ctrl_rx_reject_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
     case BLE_LL_CTRL_PROC_CS_CAP_XCHG:
         ble_ll_cs_rx_capabilities_req_rejected(connsm, ble_error);
         break;
+    case BLE_LL_CTRL_PROC_CS_FAE_REQ:
+        ble_ll_cs_rx_fae_req_rejected(connsm, ble_error);
+        break;
 #endif
 
     default:
@@ -2598,6 +2601,10 @@ ble_ll_ctrl_proc_init(struct ble_ll_conn_sm *connsm, int ctrl_proc, void *data)
             opcode = BLE_LL_CTRL_CS_CAPABILITIES_REQ;
             ble_ll_cs_capabilities_pdu_make(connsm, ctrdata);
             break;
+        case BLE_LL_CTRL_PROC_CS_FAE_REQ:
+            opcode = BLE_LL_CTRL_CS_FAE_REQ;
+            /* No command parameters in LL_CS_FAE_REQ PDU */
+            break;
 #endif
         default:
             BLE_LL_ASSERT(0);
@@ -3076,6 +3083,12 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om)
     case BLE_LL_CTRL_CS_CAPABILITIES_RSP:
         ble_ll_cs_rx_capabilities_rsp(connsm, dptr);
         break;
+    case BLE_LL_CTRL_CS_FAE_REQ:
+        rsp_opcode = ble_ll_cs_rx_fae_req(connsm, om);
+        break;
+    case BLE_LL_CTRL_CS_FAE_RSP:
+        ble_ll_cs_rx_fae_rsp(connsm, dptr);
+        break;
 #endif
     default:
         /* Nothing to do here */

From 8f52501708fa38de590ff99ea315d233d2239b5a Mon Sep 17 00:00:00 2001
From: Magdalena Kasenberg <magdalena.kasenberg@codecoup.pl>
Date: Mon, 15 Apr 2024 13:28:50 +0200
Subject: [PATCH 11/14] nimble/ll: Add CS Configuration procedure

Implements:
- LE CS Create Config command
- LE CS Remove Config command
---
 .../include/controller/ble_ll_conn.h          |   2 +-
 .../controller/include/controller/ble_ll_cs.h |   4 +
 .../include/controller/ble_ll_ctrl.h          |   3 +-
 nimble/controller/src/ble_ll_cs.c             | 459 +++++++++++++++++-
 nimble/controller/src/ble_ll_cs_priv.h        |  38 ++
 nimble/controller/src/ble_ll_ctrl.c           |  13 +
 6 files changed, 515 insertions(+), 4 deletions(-)

diff --git a/nimble/controller/include/controller/ble_ll_conn.h b/nimble/controller/include/controller/ble_ll_conn.h
index 656bfeb03b..c99559d6e2 100644
--- a/nimble/controller/include/controller/ble_ll_conn.h
+++ b/nimble/controller/include/controller/ble_ll_conn.h
@@ -278,7 +278,7 @@ struct ble_ll_conn_sm
     uint8_t vers_nr;
     uint8_t conn_features;
     uint8_t remote_features[7];
-    uint16_t pending_ctrl_procs;
+    uint32_t pending_ctrl_procs;
     uint16_t event_cntr;
     uint16_t completed_pkts;
     uint16_t comp_id;
diff --git a/nimble/controller/include/controller/ble_ll_cs.h b/nimble/controller/include/controller/ble_ll_cs.h
index 4fe96bb20e..d1f78ba30f 100644
--- a/nimble/controller/include/controller/ble_ll_cs.h
+++ b/nimble/controller/include/controller/ble_ll_cs.h
@@ -32,6 +32,7 @@ void ble_ll_cs_init(void);
 void ble_ll_cs_reset(void);
 
 void ble_ll_cs_capabilities_pdu_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr);
+void ble_ll_cs_config_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr);
 
 int ble_ll_cs_rx_capabilities_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf);
 void ble_ll_cs_rx_capabilities_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr);
@@ -39,6 +40,9 @@ void ble_ll_cs_rx_capabilities_req_rejected(struct ble_ll_conn_sm *connsm, uint8
 int ble_ll_cs_rx_fae_req(struct ble_ll_conn_sm *connsm, struct os_mbuf *om);
 void ble_ll_cs_rx_fae_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr);
 void ble_ll_cs_rx_fae_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error);
+int ble_ll_cs_rx_config_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf);
+void ble_ll_cs_rx_config_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr);
+void ble_ll_cs_rx_config_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error);
 
 /* HCI handlers */
 int ble_ll_cs_hci_rd_loc_supp_cap(uint8_t *rspbuf, uint8_t *rsplen);
diff --git a/nimble/controller/include/controller/ble_ll_ctrl.h b/nimble/controller/include/controller/ble_ll_ctrl.h
index bc94f9a195..ac35f70b73 100644
--- a/nimble/controller/include/controller/ble_ll_ctrl.h
+++ b/nimble/controller/include/controller/ble_ll_ctrl.h
@@ -45,7 +45,8 @@ extern "C" {
 #define BLE_LL_CTRL_PROC_SUBRATE_UPDATE (13)
 #define BLE_LL_CTRL_PROC_CS_CAP_XCHG    (14)
 #define BLE_LL_CTRL_PROC_CS_FAE_REQ     (15)
-#define BLE_LL_CTRL_PROC_NUM            (16)
+#define BLE_LL_CTRL_PROC_CS_CONF        (16)
+#define BLE_LL_CTRL_PROC_NUM            (17)
 #define BLE_LL_CTRL_PROC_IDLE           (255)
 
 /* Checks if a particular control procedure is running */
diff --git a/nimble/controller/src/ble_ll_cs.c b/nimble/controller/src/ble_ll_cs.c
index 58ca028951..cda7ed2d5b 100644
--- a/nimble/controller/src/ble_ll_cs.c
+++ b/nimble/controller/src/ble_ll_cs.c
@@ -65,6 +65,12 @@
 
 static struct ble_ll_cs_supp_cap g_ble_ll_cs_local_cap;
 static struct ble_ll_cs_sm g_ble_ll_cs_sm[MYNEWT_VAL(BLE_MAX_CONNECTIONS)];
+static const uint8_t t_ip1[] = {10, 20, 30, 40, 50, 60, 80, 145};
+static const uint8_t t_ip2[] = {10, 20, 30, 40, 50, 60, 80, 145};
+static const uint8_t t_fcs[] = {15, 20, 30, 40, 50, 60, 80, 100, 120, 150};
+static const uint8_t t_pm[] = {10, 20, 40};
+
+void ble_ll_ctrl_rej_ext_ind_make(uint8_t rej_opcode, uint8_t err, uint8_t *ctrdata);
 
 int
 ble_ll_cs_hci_rd_loc_supp_cap(uint8_t *rspbuf, uint8_t *rsplen)
@@ -527,16 +533,465 @@ ble_ll_cs_hci_wr_cached_rem_fae(const uint8_t *cmdbuf, uint8_t cmdlen,
     return BLE_ERR_SUCCESS;
 }
 
+void
+ble_ll_cs_config_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    uint8_t config_id = connsm->cssm->config_req_id;
+    uint8_t action = connsm->cssm->config_req_action;
+    const struct ble_ll_cs_config *conf;
+
+    assert(config_id < ARRAY_SIZE(connsm->cssm->config));
+
+    *dptr = config_id | action << 6;
+
+    if (action == 0x00) {
+        /* Removing the config, all remaining fields are RFU. */
+        memset(dptr + 1, 0, 26);
+
+        return;
+    }
+
+    conf = &connsm->cssm->tmp_config;
+    memcpy(dptr + 1, conf->chan_map, 10);
+    dptr[11] = conf->chan_map_repetition;
+    dptr[12] = conf->main_mode;
+    dptr[13] = conf->sub_mode;
+    dptr[14] = conf->main_mode_min_steps;
+    dptr[15] = conf->main_mode_max_steps;
+    dptr[16] = conf->main_mode_repetition;
+    dptr[17] = conf->mode_0_steps;
+    dptr[18] = conf->cs_sync_phy;
+    dptr[19] = conf->rtt_type |
+               conf->role << 4;
+    dptr[20] = conf->chan_sel |
+               conf->ch3cshape << 4;
+    dptr[21] = conf->ch3cjump;
+    dptr[22] = conf->t_ip1_index;
+    dptr[23] = conf->t_ip2_index;
+    dptr[24] = conf->t_fcs_index;
+    dptr[25] = conf->t_pm_index;
+    /* RFU octet */
+    dptr[26] = 0x00;
+}
+
+static int
+ble_ll_cs_verify_config(struct ble_ll_cs_config *conf)
+{
+    if (conf->chan_map[9] & 0x80) {
+        return 1;
+    }
+
+    if (conf->chan_map_repetition < 1) {
+        return 1;
+    }
+
+    /* Valid combinations of Main_Mode and Sub_Mode selections */
+    if (conf->main_mode == 0x01) {
+        if (conf->sub_mode != 0xFF) {
+            return 1;
+        }
+    } else if (conf->main_mode == 0x02) {
+        if (conf->sub_mode != 0x01 &&
+            conf->sub_mode != 0x03 &&
+            conf->sub_mode != 0xFF) {
+            return 1;
+        }
+    } else if (conf->main_mode == 0x03) {
+        if (conf->sub_mode != 0x02 &&
+            conf->sub_mode != 0xFF) {
+            return 1;
+        }
+    } else {
+        return 1;
+    }
+
+    if (conf->sub_mode == 0xFF) {
+        /* RFU if Sub_Mode is None */
+        conf->main_mode_min_steps = 0x00;
+        conf->main_mode_max_steps = 0x00;
+    }
+
+    if (conf->main_mode_repetition > 0x03) {
+        return 1;
+    }
+
+    if (conf->mode_0_steps < 1 || conf->mode_0_steps > 3) {
+        return 1;
+    }
+
+    if (conf->cs_sync_phy & 0xF0) {
+        return 1;
+    }
+
+    if (conf->rtt_type > 0x06) {
+        return 1;
+    }
+
+    if (conf->chan_sel > 0x01) {
+        return 1;
+    }
+
+    if (conf->chan_sel == 0x01) {
+        if (conf->ch3cshape > 0x01) {
+            return 1;
+        }
+
+        if (!IN_RANGE(conf->ch3cjump, 2, 8)) {
+            return 1;
+        }
+    }
+
+    if (conf->t_ip1_index > 7) {
+        return 1;
+    }
+
+    if (conf->t_ip2_index > 7) {
+        return 1;
+    }
+
+    if (conf->t_fcs_index > 9) {
+        return 1;
+    }
+
+    if (conf->t_pm_index > 2) {
+        return 1;
+    }
+
+    return 0;
+}
+
+static void
+ble_ll_cs_ev_config_complete(struct ble_ll_conn_sm *connsm, uint8_t config_id,
+                             uint8_t action, uint8_t status)
+{
+    struct ble_hci_ev_le_subev_cs_config_complete *ev;
+    const struct ble_ll_cs_config *conf;
+    struct ble_hci_ev *hci_ev;
+
+    if (ble_ll_hci_is_le_event_enabled(
+            BLE_HCI_LE_SUBEV_CS_CONFIG_COMPLETE)) {
+        hci_ev = ble_transport_alloc_evt(0);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+            hci_ev->length = sizeof(*ev);
+            ev = (void *) hci_ev->data;
+
+            memset(ev, 0, sizeof(*ev));
+            ev->subev_code = BLE_HCI_LE_SUBEV_CS_CONFIG_COMPLETE;
+            ev->status = status;
+            ev->conn_handle = htole16(connsm->conn_handle);
+            ev->config_id = config_id;
+            ev->action = action;
+
+            if (action != 0x00 && status == BLE_ERR_SUCCESS) {
+                conf = &connsm->cssm->config[config_id];
+                ev->main_mode_type = conf->main_mode;
+                ev->sub_mode_type = conf->sub_mode;
+                ev->min_main_mode_steps = conf->main_mode_min_steps;
+                ev->max_main_mode_steps = conf->main_mode_max_steps;
+                ev->main_mode_repetition = conf->main_mode_repetition;
+                ev->mode_0_steps = conf->mode_0_steps;
+                ev->role = conf->role;
+                ev->rtt_type = conf->rtt_type;
+                ev->cs_sync_phy = conf->cs_sync_phy;
+                memcpy(ev->channel_map, conf->chan_map, 10);
+                ev->channel_map_repetition = conf->chan_map_repetition;
+                ev->channel_selection_type = conf->chan_sel;
+                ev->ch3c_shape = conf->ch3cshape;
+                ev->ch3c_jump = conf->ch3cjump;
+                ev->reserved = 0x00;
+                ev->t_ip1_time = conf->t_ip1;
+                ev->t_ip2_time = conf->t_ip2;
+                ev->t_fcs_time = conf->t_fcs;
+                ev->t_pm_time = conf->t_pm;
+            }
+
+            ble_ll_hci_event_send(hci_ev);
+        }
+    }
+}
+
+int
+ble_ll_cs_rx_config_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+                        uint8_t *rspbuf)
+{
+    struct ble_ll_cs_config *conf;
+    uint8_t config_id = *dptr & 0b00111111;
+    uint8_t action = (*dptr & 0b11000000) >> 6;
+    struct ble_ll_cs_sm *cssm = connsm->cssm;
+
+    if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CONF)) {
+        if (CONN_IS_CENTRAL(connsm)) {
+            /* Reject CS config initiated by peripheral */
+            ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_CONFIG_REQ,
+                                         BLE_ERR_LMP_COLLISION, rspbuf);
+            return BLE_LL_CTRL_REJECT_IND_EXT;
+        } else {
+            /* Take no further action in the Peripheral-initiated procedure
+             * and proceed to handle the Central-initiated procedure.
+             */
+            ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_CONF);
+        }
+    }
+
+    if (config_id >= ARRAY_SIZE(cssm->config)) {
+        ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_CONFIG_REQ,
+                                     BLE_ERR_INV_LMP_LL_PARM, rspbuf);
+        return BLE_LL_CTRL_REJECT_IND_EXT;
+    }
+
+    conf = &cssm->config[config_id];
+    if (conf->config_in_use) {
+        /* CS procedure in progress exit with error */
+        ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_CONFIG_REQ,
+                                     BLE_ERR_CMD_DISALLOWED, rspbuf);
+        return BLE_LL_CTRL_REJECT_IND_EXT;
+    }
+
+    /* Respond with LL_CS_CONFIG_RSP PDU */
+    *rspbuf = config_id | action << 6;
+
+    if (action == 0x00) {
+        /* CS configuration removed. */
+        memset(conf, 0, sizeof(*conf));
+
+        return BLE_LL_CTRL_CS_CONFIG_RSP;
+    }
+
+    conf = &cssm->tmp_config;
+    memset(conf, 0, sizeof(*conf));
+    memcpy(conf->chan_map, dptr + 1, 10);
+    conf->chan_map_repetition = dptr[11];
+    conf->main_mode = dptr[12];
+    conf->sub_mode = dptr[13];
+    conf->main_mode_min_steps = dptr[14];
+    conf->main_mode_max_steps = dptr[15];
+    conf->main_mode_repetition = dptr[16];
+    conf->mode_0_steps = dptr[17];
+    conf->cs_sync_phy = dptr[18];
+    conf->rtt_type = dptr[19] & 0b00001111;
+    conf->role = (~dptr[19] >> 4) & 0b00000001;
+    conf->chan_sel = (dptr[20] & 0b00001111);
+    conf->ch3cshape = (dptr[20] & 0b11110000) >> 4;
+    conf->ch3cjump = dptr[21];
+    conf->t_ip1_index = dptr[22];
+    conf->t_ip2_index = dptr[23];
+    conf->t_fcs_index = dptr[24];
+    conf->t_pm_index = dptr[25];
+
+    if (ble_ll_cs_verify_config(conf)) {
+        ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_CONFIG_REQ,
+                                     BLE_ERR_UNSUPP_LMP_LL_PARM, rspbuf);
+        return BLE_LL_CTRL_REJECT_IND_EXT;
+    }
+
+    conf->t_ip1 = t_ip1[conf->t_ip1_index];
+    conf->t_ip2 = t_ip2[conf->t_ip2_index];
+    conf->t_fcs = t_fcs[conf->t_fcs_index];
+    conf->t_pm = t_pm[conf->t_pm_index];
+    conf->config_enabled = 1;
+
+    memcpy(&cssm->config[config_id], conf, sizeof(*conf));
+    memset(conf, 0, sizeof(*conf));
+
+    return BLE_LL_CTRL_CS_CONFIG_RSP;
+}
+
+void
+ble_ll_cs_rx_config_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    uint8_t config_id = *dptr & 0b00111111;
+    uint8_t action = (*dptr & 0b11000000) >> 6;
+    struct ble_ll_cs_sm *cssm = connsm->cssm;
+
+    if (config_id != cssm->config_req_id ||
+        !IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CONF)) {
+        return;
+    }
+
+    /* Configure CS config locally */
+    memcpy(&cssm->config[config_id], &cssm->tmp_config, sizeof(cssm->tmp_config));
+    memset(&cssm->tmp_config, 0, sizeof(cssm->tmp_config));
+
+    /* Stop the control procedure and send an event to the host */
+    ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_CONF);
+    ble_ll_cs_ev_config_complete(connsm, config_id, action, BLE_ERR_SUCCESS);
+}
+
+void
+ble_ll_cs_rx_config_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error)
+{
+    struct ble_ll_cs_sm *cssm = connsm->cssm;
+
+    memset(&cssm->tmp_config, 0, sizeof(cssm->tmp_config));
+
+    /* Stop the control procedure and send an event to the host */
+    ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_CONF);
+    ble_ll_cs_ev_config_complete(connsm, cssm->config_req_id,
+                                 cssm->config_req_action, ble_error);
+}
+
+static int
+ble_ll_cs_select_capability(uint8_t capability_values_count,
+                            uint8_t *out_index, uint16_t local_capability,
+                            uint16_t remote_capability)
+{
+    uint16_t common_capability = local_capability & remote_capability;
+    uint8_t i;
+
+    for (i = 0; i < capability_values_count; i++) {
+        if ((common_capability >> i) & 1) {
+            *out_index = i;
+            return 0;
+        }
+    }
+
+    return 1;
+}
+
 int
 ble_ll_cs_hci_create_config(const uint8_t *cmdbuf, uint8_t cmdlen)
 {
-    return BLE_ERR_UNSUPPORTED;
+    const struct ble_hci_le_cs_create_config_cp *cmd = (const void *)cmdbuf;
+    struct ble_ll_conn_sm *connsm;
+    struct ble_ll_cs_sm *cssm;
+    struct ble_ll_cs_config *conf;
+
+    if (cmdlen != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* If no connection handle exit with error */
+    connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+    if (!connsm) {
+        return BLE_ERR_UNK_CONN_ID;
+    }
+
+    cssm = connsm->cssm;
+    if (cmd->config_id >= ARRAY_SIZE(cssm->config)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    conf = &cssm->config[cmd->config_id];
+
+    /* If already pending or CS procedure in progress exit with error */
+    if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CONF) ||
+        conf->config_in_use) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    /* Save the CS configuration in temporary variable as the config
+     * might be rejected by the remote.
+     */
+    conf = &cssm->tmp_config;
+    memset(conf, 0, sizeof(*conf));
+    conf->config_enabled = 1;
+    conf->main_mode = cmd->main_mode_type;
+    conf->sub_mode = cmd->sub_mode_type;
+    conf->main_mode_min_steps = cmd->min_main_mode_steps;
+    conf->main_mode_max_steps = cmd->max_main_mode_steps;
+    conf->main_mode_repetition = cmd->main_mode_repetition;
+    conf->mode_0_steps = cmd->mode_0_steps;
+    conf->role = cmd->role;
+    conf->rtt_type = cmd->rtt_type;
+    conf->cs_sync_phy = cmd->cs_sync_phy;
+    memcpy(conf->chan_map, cmd->channel_map, 10);
+    conf->chan_map_repetition = cmd->channel_map_repetition;
+    conf->chan_sel = cmd->channel_selection_type;
+    conf->ch3cshape = cmd->ch3c_shape;
+    conf->ch3cjump = cmd->ch3c_jump;
+
+    if (ble_ll_cs_select_capability(ARRAY_SIZE(t_ip1), &conf->t_ip1_index,
+                                    cssm->remote_cap.t_ip1_capability,
+                                    g_ble_ll_cs_local_cap.t_ip1_capability)) {
+        memset(conf, 0, sizeof(*conf));
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (ble_ll_cs_select_capability(ARRAY_SIZE(t_ip2), &conf->t_ip2_index,
+                                    cssm->remote_cap.t_ip2_capability,
+                                    g_ble_ll_cs_local_cap.t_ip2_capability)) {
+        memset(conf, 0, sizeof(*conf));
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (ble_ll_cs_select_capability(ARRAY_SIZE(t_fcs), &conf->t_fcs_index,
+                                    cssm->remote_cap.t_fcs_capability,
+                                    g_ble_ll_cs_local_cap.t_fcs_capability)) {
+        memset(conf, 0, sizeof(*conf));
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (ble_ll_cs_select_capability(ARRAY_SIZE(t_pm), &conf->t_pm_index,
+                                    cssm->remote_cap.t_pm_capability,
+                                    g_ble_ll_cs_local_cap.t_pm_capability)) {
+        memset(conf, 0, sizeof(*conf));
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    conf->t_ip1 = t_ip1[conf->t_ip1_index];
+    conf->t_ip2 = t_ip2[conf->t_ip2_index];
+    conf->t_fcs = t_fcs[conf->t_fcs_index];
+    conf->t_pm = t_pm[conf->t_pm_index];
+
+    if (ble_ll_cs_verify_config(conf)) {
+        assert(0);
+        memset(conf, 0, sizeof(*conf));
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (cmd->create_context == 0x01) {
+        /* Configure the CS config in the remote controller */
+        cssm->config_req_id = cmd->config_id;
+        cssm->config_req_action = 0x01;
+        ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_CONF, NULL);
+    } else {
+        ble_ll_cs_ev_config_complete(connsm, cmd->config_id, 0x01, BLE_ERR_SUCCESS);
+    }
+
+    return BLE_ERR_SUCCESS;
 }
 
 int
 ble_ll_cs_hci_remove_config(const uint8_t *cmdbuf, uint8_t cmdlen)
 {
-    return BLE_ERR_UNSUPPORTED;
+    const struct ble_hci_le_cs_remove_config_cp *cmd = (const void *)cmdbuf;
+    struct ble_ll_conn_sm *connsm;
+    struct ble_ll_cs_sm *cssm;
+    struct ble_ll_cs_config *conf;
+
+    if (cmdlen != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* If no connection handle exit with error */
+    connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+    if (!connsm) {
+        return BLE_ERR_UNK_CONN_ID;
+    }
+
+    cssm = connsm->cssm;
+    if (cmd->config_id >= ARRAY_SIZE(cssm->config)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    conf = &cssm->config[cmd->config_id];
+
+    /* If already pending or CS procedure in progress exit with error */
+    if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CONF) ||
+        conf->config_in_use) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    /* Remove the CS config locally */
+    memset(conf, 0, sizeof(*conf));
+
+    /* Configure the CS config in the remote controller */
+    cssm->config_req_id = cmd->config_id;
+    cssm->config_req_action = 0x00;
+    ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_CONF, NULL);
+
+    return BLE_ERR_SUCCESS;
 }
 
 int
diff --git a/nimble/controller/src/ble_ll_cs_priv.h b/nimble/controller/src/ble_ll_cs_priv.h
index b8518e0539..26685479c2 100644
--- a/nimble/controller/src/ble_ll_cs_priv.h
+++ b/nimble/controller/src/ble_ll_cs_priv.h
@@ -22,6 +22,7 @@
 
 #include <stdint.h>
 #include "controller/ble_ll_conn.h"
+#include "ble_ll_cs_drbg_priv.h"
 
 #ifdef __cplusplus
 extern "C" {
@@ -55,12 +56,44 @@ struct ble_ll_cs_supp_cap {
 };
 
 struct ble_ll_cs_config {
+    uint8_t config_in_use;
     uint8_t config_enabled;
     /* The role to use in CS procedure
      * 0x00 = Initiator,
      * 0x01 = Reflector
      */
     uint8_t role;
+    /* Map of allowed channels for this CS config */
+    uint8_t chan_map[10];
+    /* The number of times the map represented by the Channel_Map field is to
+     * be cycled through for non-mode 0 steps within a CS procedure
+     */
+    uint8_t chan_map_repetition;
+    uint8_t main_mode;
+    uint8_t sub_mode;
+    uint8_t main_mode_min_steps;
+    uint8_t main_mode_max_steps;
+    uint8_t main_mode_repetition;
+    uint8_t mode_0_steps;
+    /* PHY used for mode 0, 1 and 3 (use LE 1M PHY)*/
+    uint8_t cs_sync_phy;
+    /* Type of RTT (Round-Trip Time) packets */
+    uint8_t rtt_type;
+    /* The Channel Selection Algorithm to use */
+    uint8_t chan_sel;
+    /* Parameters for #3c algorithm */
+    uint8_t ch3cshape;
+    uint8_t ch3cjump;
+    /* Timings (indexes) selected from capabilities */
+    uint8_t t_ip1_index;
+    uint8_t t_ip2_index;
+    uint8_t t_fcs_index;
+    uint8_t t_pm_index;
+    /* Timings (usec) selected from capabilities */
+    uint8_t t_ip1;
+    uint8_t t_ip2;
+    uint8_t t_fcs;
+    uint8_t t_pm;
 };
 
 struct ble_ll_cs_sm {
@@ -76,6 +109,11 @@ struct ble_ll_cs_sm {
     /* Cached FAE tables */
     uint8_t remote_fae_table[72];
     uint8_t local_fae_table[72];
+
+    /* Arguments for ble_ll_cs_config_req_make */
+    uint8_t config_req_id;
+    uint8_t config_req_action;
+    struct ble_ll_cs_config tmp_config;
 };
 
 #ifdef __cplusplus
diff --git a/nimble/controller/src/ble_ll_ctrl.c b/nimble/controller/src/ble_ll_ctrl.c
index d1ed35c9a2..f789ffdd66 100644
--- a/nimble/controller/src/ble_ll_ctrl.c
+++ b/nimble/controller/src/ble_ll_ctrl.c
@@ -2072,6 +2072,9 @@ ble_ll_ctrl_rx_reject_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
     case BLE_LL_CTRL_PROC_CS_FAE_REQ:
         ble_ll_cs_rx_fae_req_rejected(connsm, ble_error);
         break;
+    case BLE_LL_CTRL_PROC_CS_CONF:
+        ble_ll_cs_rx_config_req_rejected(connsm, ble_error);
+        break;
 #endif
 
     default:
@@ -2605,6 +2608,10 @@ ble_ll_ctrl_proc_init(struct ble_ll_conn_sm *connsm, int ctrl_proc, void *data)
             opcode = BLE_LL_CTRL_CS_FAE_REQ;
             /* No command parameters in LL_CS_FAE_REQ PDU */
             break;
+        case BLE_LL_CTRL_PROC_CS_CONF:
+            opcode = BLE_LL_CTRL_CS_CONFIG_REQ;
+            ble_ll_cs_config_req_make(connsm, ctrdata);
+            break;
 #endif
         default:
             BLE_LL_ASSERT(0);
@@ -3089,6 +3096,12 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om)
     case BLE_LL_CTRL_CS_FAE_RSP:
         ble_ll_cs_rx_fae_rsp(connsm, dptr);
         break;
+    case BLE_LL_CTRL_CS_CONFIG_REQ:
+        rsp_opcode = ble_ll_cs_rx_config_req(connsm, dptr, rspdata);
+        break;
+    case BLE_LL_CTRL_CS_CONFIG_RSP:
+        ble_ll_cs_rx_config_rsp(connsm, dptr);
+        break;
 #endif
     default:
         /* Nothing to do here */

From 3dba5ee9384b2da9acb71d54d0e30df46924e432 Mon Sep 17 00:00:00 2001
From: Magdalena Kasenberg <magdalena.kasenberg@codecoup.pl>
Date: Tue, 16 Apr 2024 09:11:20 +0200
Subject: [PATCH 12/14] nimble/ll: Add CS Security Start procedure

Implements LE CS Security Enable command.
---
 .../controller/include/controller/ble_ll_cs.h |   4 +
 .../include/controller/ble_ll_ctrl.h          |   3 +-
 nimble/controller/src/ble_ll_cs.c             | 128 +++++++++++++++++-
 nimble/controller/src/ble_ll_cs_priv.h        |   3 +
 nimble/controller/src/ble_ll_ctrl.c           |  19 +++
 5 files changed, 155 insertions(+), 2 deletions(-)

diff --git a/nimble/controller/include/controller/ble_ll_cs.h b/nimble/controller/include/controller/ble_ll_cs.h
index d1f78ba30f..35a5d1b8cf 100644
--- a/nimble/controller/include/controller/ble_ll_cs.h
+++ b/nimble/controller/include/controller/ble_ll_cs.h
@@ -33,6 +33,7 @@ void ble_ll_cs_reset(void);
 
 void ble_ll_cs_capabilities_pdu_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr);
 void ble_ll_cs_config_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr);
+void ble_ll_cs_security_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr);
 
 int ble_ll_cs_rx_capabilities_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf);
 void ble_ll_cs_rx_capabilities_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr);
@@ -43,6 +44,9 @@ void ble_ll_cs_rx_fae_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_er
 int ble_ll_cs_rx_config_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf);
 void ble_ll_cs_rx_config_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr);
 void ble_ll_cs_rx_config_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error);
+int ble_ll_cs_rx_security_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf);
+void ble_ll_cs_rx_security_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr);
+void ble_ll_cs_rx_security_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error);
 
 /* HCI handlers */
 int ble_ll_cs_hci_rd_loc_supp_cap(uint8_t *rspbuf, uint8_t *rsplen);
diff --git a/nimble/controller/include/controller/ble_ll_ctrl.h b/nimble/controller/include/controller/ble_ll_ctrl.h
index ac35f70b73..6c9e909303 100644
--- a/nimble/controller/include/controller/ble_ll_ctrl.h
+++ b/nimble/controller/include/controller/ble_ll_ctrl.h
@@ -46,7 +46,8 @@ extern "C" {
 #define BLE_LL_CTRL_PROC_CS_CAP_XCHG    (14)
 #define BLE_LL_CTRL_PROC_CS_FAE_REQ     (15)
 #define BLE_LL_CTRL_PROC_CS_CONF        (16)
-#define BLE_LL_CTRL_PROC_NUM            (17)
+#define BLE_LL_CTRL_PROC_CS_SEC_START   (17)
+#define BLE_LL_CTRL_PROC_NUM            (18)
 #define BLE_LL_CTRL_PROC_IDLE           (255)
 
 /* Checks if a particular control procedure is running */
diff --git a/nimble/controller/src/ble_ll_cs.c b/nimble/controller/src/ble_ll_cs.c
index cda7ed2d5b..c111dd5945 100644
--- a/nimble/controller/src/ble_ll_cs.c
+++ b/nimble/controller/src/ble_ll_cs.c
@@ -360,10 +360,136 @@ ble_ll_cs_hci_wr_cached_rem_supp_cap(const uint8_t *cmdbuf, uint8_t cmdlen,
     return BLE_ERR_SUCCESS;
 }
 
+int
+ble_ll_cs_rx_security_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+                          uint8_t *rspbuf)
+{
+    uint8_t *iv = connsm->cssm->drbg_ctx.iv;
+    uint8_t *in = connsm->cssm->drbg_ctx.in;
+    uint8_t *pv = connsm->cssm->drbg_ctx.pv;
+
+    if (!connsm->flags.encrypted) {
+        ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_SEC_REQ,
+                                     BLE_ERR_INSUFFICIENT_SEC, rspbuf);
+        return BLE_LL_CTRL_REJECT_IND_EXT;
+    }
+
+    /* Vectors concatenation is done in the follwing manner:
+     * CS_IV = CS_IV_P || CS_IV_C
+     * The CS_IV_C is concatenated with the CS_IV_P. The least significant
+     * octet of CS_IV_C becomes the least significant octet of CS_IV. The most
+     * significant octet of CS_IV_P becomes the most significant octet of CS_IV.
+     */
+
+    /* Save Central's vector */
+    memcpy(iv, dptr, 8);
+    memcpy(in, dptr + 8, 4);
+    memcpy(pv, dptr + 12, 8);
+
+    /* Generate Peripheral's vector */
+    ble_ll_rand_data_get(iv + 8, 8);
+    ble_ll_rand_data_get(in + 4, 4);
+    ble_ll_rand_data_get(pv + 8, 8);
+
+    memcpy(rspbuf, iv + 8, 8);
+    memcpy(rspbuf + 8, in + 4, 4);
+    memcpy(rspbuf + 12, pv + 8, 8);
+
+    ble_ll_cs_drbg_init(&connsm->cssm->drbg_ctx);
+
+    return BLE_LL_CTRL_CS_SEC_RSP;
+}
+
+static void
+ble_ll_cs_ev_sec_enable_complete(struct ble_ll_conn_sm *connsm, uint8_t status)
+{
+    struct ble_hci_ev_le_subev_cs_sec_enable_complete *ev;
+    struct ble_hci_ev *hci_ev;
+
+    if (ble_ll_hci_is_le_event_enabled(
+            BLE_HCI_LE_SUBEV_CS_SEC_ENABLE_COMPLETE)) {
+        hci_ev = ble_transport_alloc_evt(0);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+            hci_ev->length = sizeof(*ev);
+            ev = (void *) hci_ev->data;
+
+            ev->subev_code = BLE_HCI_LE_SUBEV_CS_SEC_ENABLE_COMPLETE;
+            ev->status = status;
+            ev->conn_handle = htole16(connsm->conn_handle);
+
+            ble_ll_hci_event_send(hci_ev);
+        }
+    }
+}
+
+void
+ble_ll_cs_rx_security_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    int rc = 0;
+    struct ble_ll_cs_drbg_ctx *drbg_ctx = &connsm->cssm->drbg_ctx;
+
+    if (!IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_SEC_START)) {
+        /* Ignore */
+        return;
+    }
+
+    /* Save Peripheral's vector */
+    memcpy(drbg_ctx->iv + 8, dptr, 8);
+    memcpy(drbg_ctx->in + 4, dptr + 8, 4);
+    memcpy(drbg_ctx->pv + 8, dptr + 12, 8);
+
+    rc = ble_ll_cs_drbg_init(drbg_ctx);
+
+    /* Stop the control procedure and send an event to the host */
+    ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_SEC_START);
+    ble_ll_cs_ev_sec_enable_complete(connsm, rc ? BLE_ERR_INV_LMP_LL_PARM :
+                                     BLE_ERR_SUCCESS);
+}
+
+void
+ble_ll_cs_rx_security_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error)
+{
+    /* Stop the control procedure and send an event to the host */
+    ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_SEC_START);
+    ble_ll_cs_ev_sec_enable_complete(connsm, ble_error);
+}
+
+void
+ble_ll_cs_security_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    uint8_t *iv = connsm->cssm->drbg_ctx.iv;
+    uint8_t *in = connsm->cssm->drbg_ctx.in;
+    uint8_t *pv = connsm->cssm->drbg_ctx.pv;
+
+    /* Generate Central's vector */
+    ble_ll_rand_data_get(iv, 8);
+    ble_ll_rand_data_get(in, 4);
+    ble_ll_rand_data_get(pv, 8);
+
+    memcpy(dptr, iv, 8);
+    memcpy(dptr + 8, in, 4);
+    memcpy(dptr + 12, pv, 8);
+}
+
 int
 ble_ll_cs_hci_sec_enable(const uint8_t *cmdbuf, uint8_t cmdlen)
 {
-    return BLE_ERR_UNSUPPORTED;
+    const struct ble_hci_le_cs_sec_enable_cp *cmd = (const void *)cmdbuf;
+    struct ble_ll_conn_sm *connsm;
+
+    connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+    if (!connsm) {
+        return BLE_ERR_UNK_CONN_ID;
+    }
+
+    if (!connsm->flags.encrypted) {
+        return BLE_ERR_INSUFFICIENT_SEC;
+    }
+
+    ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_SEC_START, NULL);
+
+    return BLE_ERR_SUCCESS;
 }
 
 int
diff --git a/nimble/controller/src/ble_ll_cs_priv.h b/nimble/controller/src/ble_ll_cs_priv.h
index 26685479c2..b56391489e 100644
--- a/nimble/controller/src/ble_ll_cs_priv.h
+++ b/nimble/controller/src/ble_ll_cs_priv.h
@@ -114,6 +114,9 @@ struct ble_ll_cs_sm {
     uint8_t config_req_id;
     uint8_t config_req_action;
     struct ble_ll_cs_config tmp_config;
+
+    /* DRBG context, initialized onece per LE Connection */
+    struct ble_ll_cs_drbg_ctx drbg_ctx;
 };
 
 #ifdef __cplusplus
diff --git a/nimble/controller/src/ble_ll_ctrl.c b/nimble/controller/src/ble_ll_ctrl.c
index f789ffdd66..dee9982dc9 100644
--- a/nimble/controller/src/ble_ll_ctrl.c
+++ b/nimble/controller/src/ble_ll_ctrl.c
@@ -2075,6 +2075,9 @@ ble_ll_ctrl_rx_reject_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
     case BLE_LL_CTRL_PROC_CS_CONF:
         ble_ll_cs_rx_config_req_rejected(connsm, ble_error);
         break;
+    case BLE_LL_CTRL_PROC_CS_SEC_START:
+        ble_ll_cs_rx_security_req_rejected(connsm, ble_error);
+        break;
 #endif
 
     default:
@@ -2612,6 +2615,12 @@ ble_ll_ctrl_proc_init(struct ble_ll_conn_sm *connsm, int ctrl_proc, void *data)
             opcode = BLE_LL_CTRL_CS_CONFIG_REQ;
             ble_ll_cs_config_req_make(connsm, ctrdata);
             break;
+#if MYNEWT_VAL(BLE_LL_ROLE_CENTRAL)
+        case BLE_LL_CTRL_PROC_CS_SEC_START:
+            opcode = BLE_LL_CTRL_CS_SEC_REQ;
+            ble_ll_cs_security_req_make(connsm, ctrdata);
+            break;
+#endif
 #endif
         default:
             BLE_LL_ASSERT(0);
@@ -3102,6 +3111,16 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om)
     case BLE_LL_CTRL_CS_CONFIG_RSP:
         ble_ll_cs_rx_config_rsp(connsm, dptr);
         break;
+#if MYNEWT_VAL(BLE_LL_ROLE_PERIPHERAL)
+    case BLE_LL_CTRL_CS_SEC_REQ:
+        rsp_opcode = ble_ll_cs_rx_security_req(connsm, dptr, rspdata);
+        break;
+#endif
+#if MYNEWT_VAL(BLE_LL_ROLE_CENTRAL)
+    case BLE_LL_CTRL_CS_SEC_RSP:
+        ble_ll_cs_rx_security_rsp(connsm, dptr);
+        break;
+#endif
 #endif
     default:
         /* Nothing to do here */

From bb54321a3e653d1a413fa5e14401297a4aad8200 Mon Sep 17 00:00:00 2001
From: Magdalena Kasenberg <magdalena.kasenberg@codecoup.pl>
Date: Tue, 16 Apr 2024 10:17:16 +0200
Subject: [PATCH 13/14] nimble/ll: Add LE CS Set Channel Classification command

---
 nimble/controller/src/ble_ll_cs.c | 64 ++++++++++++++++++++++++++++++-
 1 file changed, 63 insertions(+), 1 deletion(-)

diff --git a/nimble/controller/src/ble_ll_cs.c b/nimble/controller/src/ble_ll_cs.c
index c111dd5945..9600d45a74 100644
--- a/nimble/controller/src/ble_ll_cs.c
+++ b/nimble/controller/src/ble_ll_cs.c
@@ -69,6 +69,12 @@ static const uint8_t t_ip1[] = {10, 20, 30, 40, 50, 60, 80, 145};
 static const uint8_t t_ip2[] = {10, 20, 30, 40, 50, 60, 80, 145};
 static const uint8_t t_fcs[] = {15, 20, 30, 40, 50, 60, 80, 100, 120, 150};
 static const uint8_t t_pm[] = {10, 20, 40};
+static const uint8_t default_channel_classification[10] = {
+    0xFC, 0xFF, 0x7F, 0xFC, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x1F
+};
+static uint8_t g_ble_ll_cs_chan_class[10];
+static uint8_t g_ble_ll_cs_chan_count = 0;
+static uint8_t g_ble_ll_cs_chan_indices[72];
 
 void ble_ll_ctrl_rej_ext_ind_make(uint8_t rej_opcode, uint8_t err, uint8_t *ctrdata);
 
@@ -1120,10 +1126,64 @@ ble_ll_cs_hci_remove_config(const uint8_t *cmdbuf, uint8_t cmdlen)
     return BLE_ERR_SUCCESS;
 }
 
+static int
+ble_ll_cs_proc_set_chan_class(const uint8_t *channel_classification)
+{
+    uint8_t i, j, next_id, byte;
+
+    /* TODO:
+     * 1. The interval between two successive commands sent shall be at least 1 second.
+     * Otherwise, the Controller shall return the error code Command Disallowed (0x0C).
+     *
+     * 2. Combine the Host chan_class with local chan_class capabilities?
+     */
+
+    if (channel_classification[0] & 0b00000011 ||
+        channel_classification[2] & 0b10000000 ||
+        channel_classification[3] & 0b00000011 ||
+        channel_classification[9] & 0b11100000) {
+        /* Channels 0, 1, 23, 24, 25, 77, 78, and the bit 79 (non-channel)
+         * are RFU. At least 15 channels shall be enabled.
+         */
+        return -1;
+    }
+
+    for (i = 0, j = 0; i < ARRAY_SIZE(g_ble_ll_cs_chan_class); ++i) {
+        byte = channel_classification[i];
+        next_id = i * 8;
+
+        while (byte) {
+            if (byte & 1) {
+                g_ble_ll_cs_chan_indices[j++] = next_id;
+            }
+            ++next_id;
+            byte >>= 1;
+        }
+    }
+
+    g_ble_ll_cs_chan_count = j;
+    if (g_ble_ll_cs_chan_count < 15) {
+        return -1;
+    }
+
+    memcpy(g_ble_ll_cs_chan_class, channel_classification,
+           sizeof(g_ble_ll_cs_chan_class));
+
+    return 0;
+}
+
 int
 ble_ll_cs_hci_set_chan_class(const uint8_t *cmdbuf, uint8_t cmdlen)
 {
-    return BLE_ERR_UNSUPPORTED;
+    int rc;
+    const struct ble_hci_le_cs_set_chan_class_cp *cmd = (const void *)cmdbuf;
+
+    rc = ble_ll_cs_proc_set_chan_class(cmd->channel_classification);
+    if (rc) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    return BLE_ERR_SUCCESS;
 }
 
 int
@@ -1180,6 +1240,8 @@ ble_ll_cs_init(void)
     cap->t_fcs_capability = 1 << T_FCS_CAP_ID_150US;
     cap->t_pm_capability = 1 << T_PM_CAP_ID_40US;
     cap->tx_snr_capablity = 0x00;
+
+    ble_ll_cs_proc_set_chan_class(default_channel_classification);
 }
 
 void

From 082a16dcf8d2f288be05f476a5e078c1c5b860c3 Mon Sep 17 00:00:00 2001
From: Magdalena Kasenberg <magdalena.kasenberg@codecoup.pl>
Date: Tue, 16 Apr 2024 09:53:20 +0200
Subject: [PATCH 14/14] nimble/ll: Add LE CS Set Procedure Parameters command

---
 nimble/controller/src/ble_ll_cs.c      | 56 +++++++++++++++++++++++++-
 nimble/controller/src/ble_ll_cs_priv.h | 20 +++++++++
 nimble/include/nimble/ble.h            |  3 ++
 3 files changed, 78 insertions(+), 1 deletion(-)

diff --git a/nimble/controller/src/ble_ll_cs.c b/nimble/controller/src/ble_ll_cs.c
index 9600d45a74..1d16891f10 100644
--- a/nimble/controller/src/ble_ll_cs.c
+++ b/nimble/controller/src/ble_ll_cs.c
@@ -1190,7 +1190,61 @@ int
 ble_ll_cs_hci_set_proc_params(const uint8_t *cmdbuf, uint8_t cmdlen,
                               uint8_t *rspbuf, uint8_t *rsplen)
 {
-    return BLE_ERR_UNSUPPORTED;
+    const struct ble_hci_le_cs_set_proc_params_cp *cmd = (const void *)cmdbuf;
+    struct ble_hci_le_cs_set_proc_params_rp *rsp = (void *)rspbuf;
+    struct ble_ll_conn_sm *connsm;
+    struct ble_ll_cs_config *conf;
+    struct ble_ll_cs_pref_proc_params *params;
+
+    if (cmdlen != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* If no connection handle exit with error */
+    connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+    if (!connsm) {
+        return BLE_ERR_UNK_CONN_ID;
+    }
+
+    if (cmd->config_id >= ARRAY_SIZE(connsm->cssm->config)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    conf = &connsm->cssm->config[cmd->config_id];
+
+    /* If CS configuration with Config_ID does not exists */
+    if (!conf->config_enabled) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* If CS measurement is enabled exit with error */
+    if (connsm->cssm->measurement_enabled) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    if (g_ble_ll_cs_chan_count < 15) {
+        return BLE_ERR_INSUFFICIENT_CHAN;
+    }
+
+    params = &conf->pref_proc_params;
+    params->max_procedure_len = htole16(cmd->max_procedure_len);
+    params->min_procedure_interval = htole16(cmd->min_procedure_interval);
+    params->max_procedure_interval = htole16(cmd->max_procedure_interval);
+    params->max_procedure_count = htole16(cmd->max_procedure_count);
+    params->min_subevent_len = get_le24(cmd->min_subevent_len);
+    params->max_subevent_len = get_le24(cmd->max_subevent_len);
+    params->aci = cmd->tone_antenna_config_selection;
+    params->phy = cmd->phy;
+    params->tx_power_delta = cmd->tx_power_delta;
+    params->preferred_peer_antenna = cmd->preferred_peer_antenna;
+    params->snr_control_initiator = cmd->snr_control_initiator & 0x0F;
+    params->snr_control_reflector = cmd->snr_control_reflector & 0x0F;
+    params->params_ready = 1;
+
+    rsp->conn_handle = cmd->conn_handle;
+    *rsplen = sizeof(*rsp);
+
+    return BLE_ERR_SUCCESS;
 }
 
 int
diff --git a/nimble/controller/src/ble_ll_cs_priv.h b/nimble/controller/src/ble_ll_cs_priv.h
index b56391489e..cfd8c02b03 100644
--- a/nimble/controller/src/ble_ll_cs_priv.h
+++ b/nimble/controller/src/ble_ll_cs_priv.h
@@ -55,6 +55,22 @@ struct ble_ll_cs_supp_cap {
     uint8_t tx_snr_capablity;
 };
 
+struct ble_ll_cs_pref_proc_params {
+    uint16_t max_procedure_len;
+    uint16_t min_procedure_interval;
+    uint16_t max_procedure_interval;
+    uint16_t max_procedure_count;
+    uint32_t min_subevent_len;
+    uint32_t max_subevent_len;
+    uint8_t aci;
+    uint8_t phy;
+    uint8_t tx_power_delta;
+    uint8_t preferred_peer_antenna;
+    uint8_t snr_control_initiator;
+    uint8_t snr_control_reflector;
+    uint8_t params_ready;
+};
+
 struct ble_ll_cs_config {
     uint8_t config_in_use;
     uint8_t config_enabled;
@@ -94,6 +110,8 @@ struct ble_ll_cs_config {
     uint8_t t_ip2;
     uint8_t t_fcs;
     uint8_t t_pm;
+    /* CS procedure parameters preferred by our Host */
+    struct ble_ll_cs_pref_proc_params pref_proc_params;
 };
 
 struct ble_ll_cs_sm {
@@ -117,6 +135,8 @@ struct ble_ll_cs_sm {
 
     /* DRBG context, initialized onece per LE Connection */
     struct ble_ll_cs_drbg_ctx drbg_ctx;
+
+    uint8_t measurement_enabled;
 };
 
 #ifdef __cplusplus
diff --git a/nimble/include/nimble/ble.h b/nimble/include/nimble/ble.h
index d5e41a37c8..337dcd125c 100644
--- a/nimble/include/nimble/ble.h
+++ b/nimble/include/nimble/ble.h
@@ -300,6 +300,9 @@ enum ble_error_codes
     BLE_ERR_LIMIT_REACHED       = 0x43,
     BLE_ERR_OPERATION_CANCELLED = 0x44,
     BLE_ERR_PACKET_TOO_LONG     = 0x45,
+    BLE_ERR_TOO_LATE            = 0x46,
+    BLE_ERR_TOO_EARLY           = 0x47,
+    BLE_ERR_INSUFFICIENT_CHAN   = 0x48,
     BLE_ERR_MAX                 = 0xff
 };