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

nimble/controller/include/controller/ble_ll_cs.h

@@ -36,6 +36,9 @@ 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);
 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);
 
 /* 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         (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)

nimble/controller/src/ble_ll_conn.c

@@ -3961,9 +3961,13 @@ ble_ll_conn_enqueue_pkt(struct ble_ll_conn_sm *connsm, struct os_mbuf *om,
     struct ble_mbuf_hdr *ble_hdr;
     int lifo;
 
-    /* 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;
     }
 

nimble/controller/src/ble_ll_cs.c

@@ -28,6 +28,7 @@
 #include "controller/ble_ll_cs.h"
 #include "ble_ll_conn_priv.h"
 #include "ble_ll_cs_priv.h"
+#include "os/os_mbuf.h"
 
 #define T_IP1_CAP_ID_10US 0
 #define T_IP1_CAP_ID_20US 1
@@ -427,17 +428,103 @@ 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, struct os_mbuf *om)
+{
+    /* Space for response code */
+    om->om_len = 1;
+    OS_MBUF_PKTLEN(om) = om->om_len;
+    os_mbuf_append(om, 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;
+
+            memset(ev, 0, sizeof(*ev));
+            ev->subev_code = BLE_HCI_LE_SUBEV_CS_RD_REM_FAE_COMPLETE;
+            ev->status = status;
+            ev->conn_handle = htole16(connsm->conn_handle);
+
+            if (status == BLE_ERR_SUCCESS) {
+                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)) {
+        /* Ignore */
+        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);
+}
+
+void
+ble_ll_cs_rx_fae_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error)
+{
+    /* 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_error);
+}
+
 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

@@ -2064,6 +2064,9 @@ ble_ll_ctrl_rx_reject_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
     case BLE_LL_CTRL_PROC_CS_CAP_XCHG:
         ble_ll_cs_rx_capabilities_req_rejected(connsm, ble_error);
         break;
+    case BLE_LL_CTRL_PROC_CS_FAE_REQ:
+        ble_ll_cs_rx_fae_req_rejected(connsm, ble_error);
+        break;
 #endif
 
     default:
@@ -2593,6 +2596,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);
@@ -3071,6 +3078,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, om);
+        break;
+    case BLE_LL_CTRL_CS_FAE_RSP:
+        ble_ll_cs_rx_fae_rsp(connsm, dptr);
+        break;
 #endif
     default:
         /* Nothing to do here */