From 66e8783872d3a8321a1d7715701dc2aed92d75df 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/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 dc9f8fd3f4..48f0eb3180 100644
--- a/nimble/controller/src/ble_ll_cs.c
+++ b/nimble/controller/src/ble_ll_cs.c
@@ -938,7 +938,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 55b1c5d9ca..f9da086468 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
diff --git a/nimble/include/nimble/ble.h b/nimble/include/nimble/ble.h
index 6979e945bf..3991749551 100644
--- a/nimble/include/nimble/ble.h
+++ b/nimble/include/nimble/ble.h
@@ -264,6 +264,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
 };