nimble/ll: Add initial Channel Sounding scheduling

Author: mkasenberg

nimble/controller/include/controller/ble_ll.h

@@ -33,6 +33,10 @@
 #include "hal/hal_timer.h"
 #endif
 
+#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING)
+#include "controller/ble_ll_cs.h"
+#endif
+
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -243,6 +247,9 @@ extern STATS_SECT_DECL(ble_ll_stats) ble_ll_stats;
 #if MYNEWT_VAL(BLE_LL_ISO_BROADCASTER)
 #define BLE_LL_STATE_BIG            (9)
 #endif
+#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING)
+#define BLE_LL_STATE_CS             (10)
+#endif
 
 /* LL Features */
 #define BLE_LL_FEAT_LE_ENCRYPTION       (0x0000000000001)

nimble/controller/include/controller/ble_ll_cs.h

@@ -35,6 +35,7 @@ void ble_ll_cs_capabilities_pdu_make(struct ble_ll_conn_sm *connsm, uint8_t *dpt
 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);
 void ble_ll_cs_start_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr);
+void ble_ll_cs_terminate_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);
@@ -52,6 +53,9 @@ int ble_ll_cs_rx_cs_start_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint
 int ble_ll_cs_rx_cs_start_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf);
 int ble_ll_cs_rx_cs_start_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf);
 void ble_ll_cs_rx_cs_start_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error);
+int ble_ll_cs_rx_cs_terminate_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf);
+void ble_ll_cs_rx_cs_terminate_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr);
+void ble_ll_cs_rx_cs_terminate_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);
@@ -69,6 +73,15 @@ int ble_ll_cs_hci_proc_enable(const uint8_t *cmdbuf, uint8_t cmdlen);
 int ble_ll_cs_hci_test(const uint8_t *cmdbuf, uint8_t cmdlen, uint8_t *rspbuf, uint8_t *rsplen);
 int ble_ll_cs_hci_test_end(void);
 
+uint8_t ble_ll_cs_rtt_tx_make(uint8_t *dptr, uint8_t *hdr_byte);
+void ble_ll_cs_sync_wfr_timer_exp(void);
+void ble_ll_cs_sync_tx_end(struct ble_phy_cs_sync_results *results);
+void ble_ll_cs_sync_rx_end(struct ble_phy_cs_sync_results *results);
+void ble_ll_cs_tone_tx_end(struct ble_phy_cs_tone_results *results);
+void ble_ll_cs_tone_rx_end(struct ble_phy_cs_tone_results *results);
+void ble_ll_cs_proc_rm_from_sched(void *cb_arg);
+void ble_ll_cs_proc_halt(void);
+
 #ifdef __cplusplus
 }
 #endif

nimble/controller/include/controller/ble_ll_ctrl.h

@@ -48,7 +48,8 @@ extern "C" {
 #define BLE_LL_CTRL_PROC_CS_CONF        (16)
 #define BLE_LL_CTRL_PROC_CS_SEC_START   (17)
 #define BLE_LL_CTRL_PROC_CS_START       (18)
-#define BLE_LL_CTRL_PROC_NUM            (19)
+#define BLE_LL_CTRL_PROC_CS_TERMINATE   (19)
+#define BLE_LL_CTRL_PROC_NUM            (20)
 #define BLE_LL_CTRL_PROC_IDLE           (255)
 
 /* Checks if a particular control procedure is running */
@@ -116,14 +117,15 @@ extern "C" {
 #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_TERMINATE_REQ    (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)
+#define BLE_LL_CTRL_CS_TERMINATE_RSP    (0x3A)
 
 /* Maximum opcode value */
-#define BLE_LL_CTRL_OPCODES             (BLE_LL_CTRL_CS_SEC_REQ + 1)
+#define BLE_LL_CTRL_OPCODES             (BLE_LL_CTRL_CS_TERMINATE_RSP + 1)
 
 extern const uint8_t g_ble_ll_ctrl_pkt_lengths[BLE_LL_CTRL_OPCODES];
 
@@ -329,11 +331,12 @@ struct ble_ll_len_req
 #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_TERMINATE_REQ_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)
+#define BLE_LL_CTRL_CS_TERMINATE_RSP_LEN      (4)
 
 /* API */
 struct ble_ll_conn_sm;

nimble/controller/include/controller/ble_ll_sched.h

@@ -71,6 +71,7 @@ extern uint8_t g_ble_ll_sched_offset_ticks;
 #define BLE_LL_SCHED_TYPE_SYNC      (7)
 #define BLE_LL_SCHED_TYPE_SCAN_AUX  (8)
 #define BLE_LL_SCHED_TYPE_BIG       (9)
+#define BLE_LL_SCHED_TYPE_CS        (10)
 #if MYNEWT_VAL(BLE_LL_EXT)
 #define BLE_LL_SCHED_TYPE_EXTERNAL  (255)
 #endif
@@ -222,6 +223,10 @@ uint32_t ble_ll_sched_css_get_conn_interval_us(void);
 int ble_ll_sched_iso_big(struct ble_ll_sched_item *sch, int first, int fixed);
 #endif /* BLE_LL_ISO_BROADCASTER */
 
+#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING)
+int ble_ll_sched_cs_proc(struct ble_ll_sched_item *sch);
+#endif /* BLE_LL_CHANNEL_SOUNDING */
+
 #ifdef __cplusplus
 }
 #endif

nimble/controller/src/ble_ll_conn.c

@@ -2302,7 +2302,7 @@ ble_ll_conn_anchor_get(struct ble_ll_conn_sm *connsm, uint16_t *event_cntr,
     *anchor_usecs = connsm->anchor_point_usecs;
 }
 
-#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) || MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING)
 void
 ble_ll_conn_anchor_event_cntr_get(struct ble_ll_conn_sm *connsm,
                                   uint16_t event_cntr, uint32_t *anchor,

nimble/controller/src/ble_ll_cs.c

@@ -64,8 +64,8 @@
 #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)];
+struct ble_ll_cs_supp_cap g_ble_ll_cs_local_cap;
+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};
@@ -74,8 +74,8 @@ 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];
+uint8_t g_ble_ll_cs_chan_count = 0;
+uint8_t g_ble_ll_cs_chan_indices[72];
 
 #define MIN_CONN_EVENT_COUNT_BEFORE_START 10
 #define OFFSET_FROM_CONN_EVENT_TICKS 10
@@ -410,6 +410,8 @@ ble_ll_cs_rx_security_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
 
     ble_ll_cs_drbg_init(&connsm->cssm->drbg_ctx);
 
+    connsm->cssm->start_procedure_count = 0;
+
     return BLE_LL_CTRL_CS_SEC_RSP;
 }
 
@@ -454,6 +456,8 @@ ble_ll_cs_rx_security_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
 
     rc = ble_ll_cs_drbg_init(drbg_ctx);
 
+    connsm->cssm->start_procedure_count = 0;
+
     /* 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 :
@@ -1289,7 +1293,16 @@ ble_ll_cs_hci_proc_enable(const uint8_t *cmdbuf, uint8_t cmdlen)
         /* Start scheduling CS procedures */
         ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_START, NULL);
     } else {
-        /* TODO: Terminate the CS measurement early */
+        if (!connsm->cssm->measurement_enabled) {
+            return BLE_ERR_CMD_DISALLOWED;
+        }
+
+        connsm->cssm->terminate_measurement = 1;
+        connsm->cssm->terminate_config_id = cmd->config_id;
+        connsm->cssm->terminate_error_code = BLE_ERR_SUCCESS;
+
+        /* Terminate the CS measurement early */
+        ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_TERMINATE, NULL);
     }
 
     return BLE_ERR_SUCCESS;
@@ -1740,6 +1753,89 @@ ble_ll_cs_rx_cs_start_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error)
     ble_ll_cs_ev_cs_proc_enable_complete(connsm, connsm->cssm->config_req_id, ble_error);
 }
 
+void
+ble_ll_cs_terminate_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    struct ble_ll_cs_sm *cssm = connsm->cssm;
+    uint8_t config_id = cssm->terminate_config_id;
+
+    assert(config_id < ARRAY_SIZE(cssm->config));
+
+    *dptr = config_id;
+    put_le16(dptr + 1, cssm->start_procedure_count + cssm->procedure_count);
+    dptr[3] = cssm->terminate_error_code;
+}
+
+int
+ble_ll_cs_rx_cs_terminate_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+                              uint8_t *rspbuf)
+{
+    struct ble_ll_cs_sm *cssm = connsm->cssm;
+    uint8_t config_id = *dptr & 0b00111111;
+    uint16_t procedure_count = get_le16(dptr + 1);
+    struct ble_ll_cs_proc_params *params;
+
+    if (config_id >= ARRAY_SIZE(cssm->config)) {
+        ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_TERMINATE_REQ,
+                                     BLE_ERR_INV_LMP_LL_PARM, rspbuf);
+        return BLE_LL_CTRL_REJECT_IND_EXT;
+    }
+
+    params = &cssm->config[config_id].proc_params;
+
+    if (procedure_count > cssm->start_procedure_count + params->max_procedure_count ||
+        procedure_count < cssm->start_procedure_count) {
+        ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_TERMINATE_REQ,
+                                     BLE_ERR_CMD_DISALLOWED, rspbuf);
+        return BLE_LL_CTRL_REJECT_IND_EXT;
+    }
+
+    cssm->terminate_measurement = 1;
+
+    *rspbuf = config_id;
+    put_le16(rspbuf + 1, cssm->start_procedure_count + cssm->procedure_count);
+    rspbuf[3] = BLE_ERR_SUCCESS;
+
+    return BLE_LL_CTRL_CS_TERMINATE_RSP;
+}
+
+void
+ble_ll_cs_rx_cs_terminate_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    struct ble_ll_cs_sm *cssm = connsm->cssm;
+    uint8_t config_id = *dptr & 0b00111111;
+    uint8_t error_code = dptr[3];
+    uint16_t procedure_count = get_le16(dptr + 1);
+    uint16_t total_procedure_count;
+
+    if (config_id != cssm->config_req_id ||
+        !IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_TERMINATE)) {
+        return;
+    }
+
+    total_procedure_count = cssm->start_procedure_count + cssm->procedure_count;
+    if (total_procedure_count < procedure_count) {
+        cssm->start_procedure_count = procedure_count;
+    } else {
+        cssm->start_procedure_count = total_procedure_count;
+    }
+
+    ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_TERMINATE);
+
+    /* TODO: In d1.0r06 the LL_CS_TERMINATE_IND was replaced with
+     * LL_CS_TERMINATE_REQ/RSP, but not all descriptions and diagrams
+     * have been updated yet.
+     */
+    ble_ll_cs_ev_cs_proc_enable_complete(connsm, config_id, error_code);
+}
+
+void
+ble_ll_cs_rx_cs_terminate_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error)
+{
+    ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_TERMINATE);
+    ble_ll_cs_ev_cs_proc_enable_complete(connsm, connsm->cssm->config_req_id, ble_error);
+}
+
 int
 ble_ll_cs_hci_test(const uint8_t *cmdbuf, uint8_t cmdlen,
                    uint8_t *rspbuf, uint8_t *rsplen)