From 6d88641ab25a28447290d4509315f393b4741a00 Mon Sep 17 00:00:00 2001
From: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
Date: Fri, 19 Aug 2022 14:48:34 +0200
Subject: [PATCH] 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 8effae542..7aa2e8d19 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 b690eeb24..a46158435 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) --- */