nimble/ll: Add CS Mode-0 FAE Table Request procedure

Implements:
- LE CS Read Remote FAE Table command
- LE CS Write Cached Remote FAE Table command

Author: mkasenberg

nimble/controller/include/controller/ble_ll_cs.h

@@ -35,6 +35,8 @@ void ble_ll_cs_capabilities_pdu_make(struct ble_ll_conn_sm *connsm, uint8_t *dpt
 
 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);
 
 /* HCI handlers */
 int ble_ll_cs_hci_rd_loc_supp_cap(uint8_t *rspbuf, uint8_t *rsplen);

nimble/controller/include/controller/ble_ll_ctrl.h

@@ -44,7 +44,8 @@ extern "C" {
 #define BLE_LL_CTRL_PROC_SUBRATE_REQ    (12)
 #define BLE_LL_CTRL_PROC_SUBRATE_UPDATE (13)
 #define BLE_LL_CTRL_PROC_CS_CAP_XCHG    (14)
-#define BLE_LL_CTRL_PROC_NUM            (15)
+#define BLE_LL_CTRL_PROC_CS_FAE_REQ     (15)
+#define BLE_LL_CTRL_PROC_NUM            (16)
 #define BLE_LL_CTRL_PROC_IDLE           (255)
 
 /* Checks if a particular control procedure is running */
@@ -124,7 +125,9 @@ extern "C" {
 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         (72)
+#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)

nimble/controller/src/ble_ll_cs.c

@@ -348,17 +348,88 @@ ble_ll_cs_hci_set_def_settings(const uint8_t *cmdbuf, uint8_t cmdlen,
     return BLE_ERR_SUCCESS;
 }
 
+int
+ble_ll_cs_rx_fae_req(struct ble_ll_conn_sm *connsm, uint8_t *rspbuf)
+{
+    memcpy(rspbuf, connsm->cssm->local_fae_table, 72);
+
+    return BLE_LL_CTRL_CS_FAE_RSP;
+}
+
+static void
+ble_ll_cs_ev_rd_rem_fae_complete(struct ble_ll_conn_sm *connsm, uint8_t status)
+{
+    struct ble_hci_ev_le_subev_cs_rd_rem_fae_complete *ev;
+    struct ble_hci_ev *hci_ev;
+
+    if (ble_ll_hci_is_le_event_enabled(
+            BLE_HCI_LE_SUBEV_CS_RD_REM_FAE_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;
+
+            ev->subev_code = BLE_HCI_LE_SUBEV_CS_RD_REM_FAE_COMPLETE;
+            ev->status = status;
+            ev->conn_handle = htole16(connsm->conn_handle);
+            memcpy(ev->remote_fae_table, connsm->cssm->remote_fae_table, 72);
+
+            ble_ll_hci_event_send(hci_ev);
+        }
+    }
+}
+
+void
+ble_ll_cs_rx_fae_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    if (!IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_FAE_REQ)) {
+        /* Should never happen */
+        return;
+    }
+
+    memcpy(connsm->cssm->remote_fae_table, dptr, 72);
+
+    /* Stop the control procedure and send an event to the host */
+    ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_FAE_REQ);
+    ble_ll_cs_ev_rd_rem_fae_complete(connsm, BLE_ERR_SUCCESS);
+}
+
 int
 ble_ll_cs_hci_rd_rem_fae(const uint8_t *cmdbuf, uint8_t cmdlen)
 {
-    return BLE_ERR_UNSUPPORTED;
+    const struct ble_hci_le_cs_rd_rem_fae_cp *cmd = (const void *)cmdbuf;
+    struct ble_ll_conn_sm *connsm;
+
+    connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+    if (!connsm) {
+        return BLE_ERR_UNK_CONN_ID;
+    }
+
+    ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_FAE_REQ, NULL);
+
+    return BLE_ERR_SUCCESS;
 }
 
 int
 ble_ll_cs_hci_wr_cached_rem_fae(const uint8_t *cmdbuf, uint8_t cmdlen,
                                 uint8_t *rspbuf, uint8_t *rsplen)
 {
-    return BLE_ERR_UNSUPPORTED;
+    const struct ble_hci_le_cs_wr_cached_rem_fae_cp *cmd = (const void *)cmdbuf;
+    struct ble_hci_le_cs_wr_cached_rem_fae_rp *rsp = (void *)rspbuf;
+    struct ble_ll_conn_sm *connsm;
+
+    connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+    if (!connsm) {
+        return BLE_ERR_UNK_CONN_ID;
+    }
+
+    memcpy(connsm->cssm->remote_fae_table, cmd->remote_fae_table, 72);
+
+    rsp->conn_handle = cmd->conn_handle;
+    *rsplen = sizeof(*rsp);
+
+    return BLE_ERR_SUCCESS;
 }
 
 int

nimble/controller/src/ble_ll_cs_priv.h

@@ -72,6 +72,10 @@ struct ble_ll_cs_sm {
     uint8_t roles_enabled;
     uint8_t cs_sync_antenna_selection;
     uint8_t max_tx_power;
+
+    /* Cached FAE tables */
+    uint8_t remote_fae_table[72];
+    uint8_t local_fae_table[72];
 };
 
 #ifdef __cplusplus

nimble/controller/src/ble_ll_ctrl.c

@@ -2574,6 +2574,10 @@ ble_ll_ctrl_proc_init(struct ble_ll_conn_sm *connsm, int ctrl_proc, void *data)
             opcode = BLE_LL_CTRL_CS_CAPABILITIES_REQ;
             ble_ll_cs_capabilities_pdu_make(connsm, ctrdata);
             break;
+        case BLE_LL_CTRL_PROC_CS_FAE_REQ:
+            opcode = BLE_LL_CTRL_CS_FAE_REQ;
+            /* No command parameters in LL_CS_FAE_REQ PDU */
+            break;
 #endif
         default:
             BLE_LL_ASSERT(0);
@@ -3043,6 +3047,12 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om)
     case BLE_LL_CTRL_CS_CAPABILITIES_RSP:
         ble_ll_cs_rx_capabilities_rsp(connsm, dptr);
         break;
+    case BLE_LL_CTRL_CS_FAE_REQ:
+        rsp_opcode = ble_ll_cs_rx_fae_req(connsm, rspdata);
+        break;
+    case BLE_LL_CTRL_CS_FAE_RSP:
+        ble_ll_cs_rx_fae_rsp(connsm, dptr);
+        break;
 #endif
     default:
         /* Nothing to do here */