nimble/ll: Add CS Configuration procedure

mkasenberg · mkasenberg · commit f297a41bcce5 · 2024-04-17T11:21:32.000+02:00
Implements:
- LE CS Create Config command
- LE CS Remove Config command
diff --git a/nimble/controller/include/controller/ble_ll_conn.h b/nimble/controller/include/controller/ble_ll_conn.h
@@ -276,7 +276,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
@@ -32,11 +32,14 @@ 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);
 int ble_ll_cs_rx_fae_req(struct ble_ll_conn_sm *connsm, uint8_t *rspbuf);
 void ble_ll_cs_rx_fae_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr);
+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);
 
 /* 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
@@ -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
@@ -31,6 +31,10 @@
 
 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};
 
 int
 ble_ll_cs_hci_rd_loc_supp_cap(uint8_t *rspbuf, uint8_t *rsplen)
@@ -432,16 +436,300 @@ 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 = &connsm->cssm->config[config_id];
+
+    *dptr = config_id | action << 6;
+
+    if (action == 0x00) {
+        /* Removing the config, all remaining fields are RFU. */
+        memset(dptr + 1, 0, 26);
+
+        return;
+    }
+
+    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;
+    dptr[23] = conf->t_ip2;
+    dptr[24] = conf->t_fcs;
+    dptr[25] = conf->t_pm;
+    /* RFU octet */
+    dptr[26] = 0x00;
+}
+
+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;
+
+    if (config_id >= BLE_LL_CS_CONFIG_MAX_NUM) {
+        /* LL_REJECT_EXT_IND */
+        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 = &connsm->cssm->config[config_id];
+    memset(conf, 0, sizeof(*conf));
+
+    /* Respond with LL_CS_CONFIG_RSP PDU */
+    *rspbuf = config_id | action << 6;
+
+    if (action == 0x00) {
+        /* CS configuration removed. */
+        return BLE_LL_CTRL_CS_CONFIG_RSP;
+    }
+
+    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 = dptr[22];
+    conf->t_ip2 = dptr[23];
+    conf->t_fcs = dptr[24];
+    conf->t_pm = dptr[25];
+    conf->config_enabled = true;
+
+    return BLE_LL_CTRL_CS_CONFIG_RSP;
+}
+
+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) {
+                /* CS configuration removed. */
+                return;
+            }
+
+            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);
+        }
+    }
+}
+
+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;
+
+    if (config_id != connsm->cssm->config_req_id ||
+        !IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CONF)) {
+        return;
+    }
+
+    /* 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);
+}
+
+static int
+ble_ll_cs_select_capability(const uint8_t *capability_values, uint8_t len,
+                            uint8_t *out_value, uint16_t local_capability,
+                            uint16_t remote_capability)
+{
+    uint16_t common_capability = local_capability & remote_capability;
+    uint8_t i;
+
+    for (i = 0; i < len; i++) {
+        if ((common_capability >> i) & 1) {
+            *out_value = capability_values[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_config *conf;
+    struct ble_ll_cs_sm *cssm;
+
+    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;
+    }
+
+    cssm = connsm->cssm;
+
+    /* If already pending exit with error */
+    if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CONF)) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    /* Configure CS config locally */
+    conf = &cssm->config[cmd->config_id];
+    conf->config_enabled = true;
+    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(t_ip1, ARRAY_SIZE(t_ip1), &conf->t_ip1,
+                                    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(t_ip2, ARRAY_SIZE(t_ip2), &conf->t_ip2,
+                                    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(t_fcs, ARRAY_SIZE(t_fcs), &conf->t_fcs,
+                                    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(t_pm, ARRAY_SIZE(t_pm), &conf->t_pm,
+                                    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;
+    }
+
+    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) || 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;
+    }
+
+    cssm = connsm->cssm;
+
+    /* If already pending exit with error */
+    if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CONF)) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    /* Remove the CS config locally */
+    conf = &cssm->config[cmd->config_id];
+    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
@@ -22,6 +22,7 @@
 
 #include <stdint.h>
 #include "controller/ble_ll_conn.h"
+#include "ble_ll_cs_drbg_priv.h"
 
 #ifdef __cplusplus
 extern "C" {
@@ -61,6 +62,32 @@ struct ble_ll_cs_config {
      * 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 (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 +103,10 @@ 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;
 };
 
 #ifdef __cplusplus
diff --git a/nimble/controller/src/ble_ll_ctrl.c b/nimble/controller/src/ble_ll_ctrl.c
