From 983eec13f0667808c9bc06c6cd4696957ba9160d 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] nimble/controller: Add LE CS Set Procedure Parameters command

---
 nimble/controller/src/ble_ll_cs.c      | 52 +++++++++++++++++++++++++-
 nimble/controller/src/ble_ll_cs_priv.h | 20 ++++++++++
 2 files changed, 71 insertions(+), 1 deletion(-)

diff --git a/nimble/controller/src/ble_ll_cs.c b/nimble/controller/src/ble_ll_cs.c
index 9e8116cdc..52d586186 100644
--- a/nimble/controller/src/ble_ll_cs.c
+++ b/nimble/controller/src/ble_ll_cs.c
@@ -924,7 +924,57 @@ 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) || cmd->config_id >= BLE_LL_CS_CONFIG_MAX_NUM) {
+        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;
+    }
+
+    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 (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;
+    params->snr_control_reflector = cmd->snr_control_reflector;
+    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 55b1c5d9c..f9da08646 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 {
     bool config_enabled;
     /* The role to use in CS procedure
@@ -88,6 +104,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 {
@@ -110,6 +128,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