nimble/ll: Add initial Channel Sounding scheduling

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_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
 {
@@ -274,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;
@@ -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 */

nimble/controller/include/controller/ble_ll_cs.h

@@ -28,6 +28,35 @@
 extern "C" {
 #endif
 
+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);
+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);
+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);
+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);
+int ble_ll_cs_rx_cs_start_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf);
+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);
 int ble_ll_cs_hci_rd_rem_supp_cap(const uint8_t *cmdbuf, uint8_t cmdlen);
@@ -44,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

@@ -43,7 +43,13 @@ 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_CS_FAE_REQ     (15)
+#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_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 */
@@ -100,14 +106,33 @@ 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_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_CHAN_STATUS_IND + 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];
 
 /* 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)
@@ -292,6 +317,27 @@ 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_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;
 void ble_ll_ctrl_proc_start(struct ble_ll_conn_sm *connsm, int ctrl_proc,

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/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

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

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
@@ -2289,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,
@@ -3971,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 {