[mynewt-nimble] 02/03: nimble/ll/css: Add HCI vs command to read current slot for connection

[an...@apache.org]

This is an automated email from the ASF dual-hosted git repository.

andk pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/mynewt-nimble.git

commit 6d88641ab25a28447290d4509315f393b4741a00
Author: Andrzej Kaczmarek <an...@codecoup.pl>
AuthorDate: Fri Aug 19 14:48:34 2022 +0200

    nimble/ll/css: Add HCI vs command to read current slot for connection
---
 nimble/controller/src/ble_ll_hci_vs.c | 33 +++++++++++++++++++++++++++++++++
 nimble/include/nimble/hci_common.h    | 10 ++++++++++
 2 files changed, 43 insertions(+)

diff --git a/nimble/controller/src/ble_ll_hci_vs.c b/nimble/controller/src/ble_ll_hci_vs.c
index 8effae54..7aa2e8d1 100644
--- a/nimble/controller/src/ble_ll_hci_vs.c
+++ b/nimble/controller/src/ble_ll_hci_vs.c
@@ -268,6 +268,37 @@ ble_ll_hci_vs_css_set_conn_slot(const uint8_t *cmdbuf, uint8_t cmdlen,
     return BLE_ERR_SUCCESS;
 }
 
+static int
+ble_ll_hci_vs_css_read_conn_slot(const uint8_t *cmdbuf, uint8_t cmdlen,
+                                uint8_t *rspbuf, uint8_t *rsplen)
+{
+    const struct ble_hci_vs_css_read_conn_slot_cp *cmd = (const void *)cmdbuf;
+    struct ble_hci_vs_css_read_conn_slot_rp *rsp = (void *)rspbuf;
+    struct ble_ll_conn_sm *connsm;
+    uint16_t conn_handle;
+
+    if (cmdlen != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (!ble_ll_sched_css_is_enabled()) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    conn_handle = le16toh(cmd->conn_handle);
+    connsm = ble_ll_conn_find_by_handle(conn_handle);
+    if (!connsm) {
+        return BLE_ERR_UNK_CONN_ID;
+    }
+
+    *rsplen = sizeof(*rsp);
+    rsp->opcode = cmd->opcode;
+    rsp->conn_handle = cmd->conn_handle;
+    rsp->slot_idx = htole16(connsm->css_slot_idx);
+
+    return BLE_ERR_SUCCESS;
+}
+
 static int
 ble_ll_hci_vs_css(uint16_t ocf, const uint8_t *cmdbuf, uint8_t cmdlen,
                   uint8_t *rspbuf, uint8_t *rsplen)
@@ -291,6 +322,8 @@ ble_ll_hci_vs_css(uint16_t ocf, const uint8_t *cmdbuf, uint8_t cmdlen,
         return ble_ll_hci_vs_css_set_next_slot(cmdbuf, cmdlen, rspbuf, rsplen);
     case BLE_HCI_VS_CSS_OP_SET_CONN_SLOT:
         return ble_ll_hci_vs_css_set_conn_slot(cmdbuf, cmdlen, rspbuf, rsplen);
+    case BLE_HCI_VS_CSS_OP_READ_CONN_SLOT:
+        return ble_ll_hci_vs_css_read_conn_slot(cmdbuf, cmdlen, rspbuf, rsplen);
     }
 
     return BLE_ERR_INV_HCI_CMD_PARMS;
diff --git a/nimble/include/nimble/hci_common.h b/nimble/include/nimble/hci_common.h
index b690eeb2..a4615843 100644
--- a/nimble/include/nimble/hci_common.h
+++ b/nimble/include/nimble/hci_common.h
@@ -1168,6 +1168,16 @@ struct ble_hci_vs_css_set_conn_slot_cp {
     uint16_t conn_handle;
     uint16_t slot_idx;
 } __attribute__((packed));
+#define BLE_HCI_VS_CSS_OP_READ_CONN_SLOT                0x05
+struct ble_hci_vs_css_read_conn_slot_cp {
+    uint8_t opcode;
+    uint16_t conn_handle;
+} __attribute__((packed));
+struct ble_hci_vs_css_read_conn_slot_rp {
+    uint8_t opcode;
+    uint16_t conn_handle;
+    uint16_t slot_idx;
+} __attribute__((packed));
 
 /* Command Specific Definitions */
 /* --- Set controller to host flow control (OGF 0x03, OCF 0x0031) --- */