nimble/ll: Add support for vs hci commands registration

This adds API to allow registration of custom HCI VS commands.

nimble/controller/include/controller/ble_ll_hci.h

@@ -43,6 +43,20 @@ extern const uint8_t g_ble_ll_supp_cmds[BLE_LL_SUPP_CMD_LEN];
 
 typedef void (*ble_ll_hci_post_cmd_complete_cb)(void);
 
+#if MYNEWT_VAL(BLE_HCI_VS)
+typedef int (* ble_ll_hci_vs_cb_t)(uint16_t ocf,
+                                   const uint8_t *cmdbuf, uint8_t cmdlen,
+                                   uint8_t *rspbuf, uint8_t *rsplen);
+
+#define BLE_LL_HCI_VS_CMD(_ocf, _cb)    { .ocf = (_ocf), .cb = (_cb) }
+
+struct ble_ll_hci_vs_cmd {
+    uint16_t ocf;
+    ble_ll_hci_vs_cb_t cb;
+    SLIST_ENTRY(ble_ll_hci_vs_cmd) link;
+};
+#endif
+
 /* Initialize LL HCI */
 void ble_ll_hci_init(void);
 
@@ -68,6 +82,10 @@ bool ble_ll_hci_adv_mode_ext(void);
 /* Get TX power compensation rounded to integer dB */
 int8_t ble_ll_get_tx_pwr_compensation(void);
 
+#if MYNEWT_VAL(BLE_HCI_VS)
+void ble_ll_hci_vs_register(struct ble_ll_hci_vs_cmd *cmds, uint32_t num_cmds);
+#endif
+
 #ifdef __cplusplus
 }
 #endif

nimble/controller/src/ble_ll.c

@@ -45,6 +45,7 @@
 #include "controller/ble_ll_trace.h"
 #include "controller/ble_ll_sync.h"
 #include "ble_ll_conn_priv.h"
+#include "ble_ll_hci_priv.h"
 #include "ble_ll_priv.h"
 
 #if MYNEWT_VAL(BLE_LL_DTM)
@@ -1731,6 +1732,10 @@ ble_ll_init(void)
     ble_ll_dtm_init();
 #endif
 
+#if MYNEWT_VAL(BLE_HCI_VS)
+    ble_ll_hci_vs_init();
+#endif
+
 #if MYNEWT
     /* Initialize the LL task */
     os_task_init(&g_ble_ll_task, "ble_ll", ble_ll_task, NULL,

nimble/controller/src/ble_ll_hci.c

@@ -36,6 +36,7 @@
 #include "controller/ble_ll_iso.h"
 #include "ble_ll_priv.h"
 #include "ble_ll_conn_priv.h"
+#include "ble_ll_hci_priv.h"
 
 #if MYNEWT_VAL(BLE_LL_DTM)
 #include "ble_ll_dtm_priv.h"
@@ -1525,47 +1526,6 @@ ble_ll_hci_status_params_cmd_proc(const uint8_t *cmdbuf, uint8_t len,
     return rc;
 }
 
-#if MYNEWT_VAL(BLE_HCI_VS)
-static int
-ble_ll_hci_vs_rd_static_addr(uint8_t *rspbuf, uint8_t *rsplen)
-{
-    struct ble_hci_vs_rd_static_addr_rp *rsp = (void *) rspbuf;
-    ble_addr_t addr;
-
-    if (ble_hw_get_static_addr(&addr) < 0) {
-        return BLE_ERR_UNSPECIFIED;
-    }
-
-    memcpy(rsp->addr, addr.val, sizeof(rsp->addr));
-
-    *rsplen = sizeof(*rsp);
-    return BLE_ERR_SUCCESS;
-}
-
-static int
-ble_ll_hci_vs_cmd_proc(const uint8_t *cmdbuf, uint8_t len, uint16_t ocf,
-                       uint8_t *rspbuf, uint8_t *rsplen)
-{
-    int rc;
-
-    /* Assume error; if all pass rc gets set to 0 */
-    rc = BLE_ERR_INV_HCI_CMD_PARMS;
-
-    switch (ocf) {
-    case BLE_HCI_OCF_VS_RD_STATIC_ADDR:
-        if (len == 0) {
-            rc = ble_ll_hci_vs_rd_static_addr(rspbuf, rsplen);
-        }
-        break;
-    default:
-        rc = BLE_ERR_UNKNOWN_HCI_CMD;
-        break;
-    }
-
-    return rc;
-}
-#endif
-
 /**
  * Called to process an HCI command from the host.
  *
@@ -1625,7 +1585,7 @@ ble_ll_hci_cmd_proc(struct ble_npl_event *ev)
     case BLE_HCI_OGF_LE:
         rc = ble_ll_hci_le_cmd_proc(cmd->data, cmd->length, ocf, rspbuf, &rsplen, &post_cb);
         break;
-#if MYNEWT_VAL(BLE_HCI_VS)
+#if MYNEWT_VAL(BLE_LL_HCI_VS)
     case BLE_HCI_OGF_VENDOR:
         rc = ble_ll_hci_vs_cmd_proc(cmd->data, cmd->length, ocf, rspbuf, &rsplen);
         break;

nimble/controller/src/ble_ll_hci_priv.h

@@ -0,0 +1,37 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_LL_HCI_PRIV_
+#define H_BLE_LL_HCI_PRIV_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if MYNEWT_VAL(BLE_HCI_VS)
+void ble_ll_hci_vs_init(void);
+int ble_ll_hci_vs_cmd_proc(const uint8_t *cmdbuf, uint8_t cmdlen, uint16_t ocf,
+                           uint8_t *rspbuf, uint8_t *rsplen);
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_LL_HCI_ */

nimble/controller/src/ble_ll_hci_vs.c

@@ -0,0 +1,119 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stdint.h>
+#include "syscfg/syscfg.h"
+#include "controller/ble_ll.h"
+#include "controller/ble_ll_hci.h"
+#include "controller/ble_hw.h"
+
+#if MYNEWT_VAL(BLE_LL_HCI_VS)
+
+SLIST_HEAD(ble_ll_hci_vs_list, ble_ll_hci_vs_cmd);
+static struct ble_ll_hci_vs_list g_ble_ll_hci_vs_list;
+
+static int
+ble_ll_hci_vs_rd_static_addr(uint16_t ocf,
+                             const uint8_t *cmdbuf, uint8_t cmdlen,
+                             uint8_t *rspbuf, uint8_t *rsplen)
+{
+    struct ble_hci_vs_rd_static_addr_rp *rsp = (void *) rspbuf;
+    ble_addr_t addr;
+
+    if (cmdlen != 0) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (ble_hw_get_static_addr(&addr) < 0) {
+        return BLE_ERR_UNSPECIFIED;
+    }
+
+    memcpy(rsp->addr, addr.val, sizeof(rsp->addr));
+
+    *rsplen = sizeof(*rsp);
+
+    return BLE_ERR_SUCCESS;
+}
+
+static struct ble_ll_hci_vs_cmd g_ble_ll_hci_vs_cmds[] = {
+    BLE_LL_HCI_VS_CMD(BLE_HCI_OCF_VS_RD_STATIC_ADDR,
+                      ble_ll_hci_vs_rd_static_addr),
+};
+
+static struct ble_ll_hci_vs_cmd *
+ble_ll_hci_vs_find_by_ocf(uint16_t ocf)
+{
+    struct ble_ll_hci_vs_cmd *entry;
+
+    entry = SLIST_FIRST(&g_ble_ll_hci_vs_list);
+    while (entry) {
+        if (entry->ocf == ocf) {
+            return entry;
+        }
+
+        entry = SLIST_NEXT(entry, link);
+    }
+
+    return NULL;
+}
+
+int
+ble_ll_hci_vs_cmd_proc(const uint8_t *cmdbuf, uint8_t cmdlen, uint16_t ocf,
+                       uint8_t *rspbuf, uint8_t *rsplen)
+{
+    struct ble_ll_hci_vs_cmd *cmd;
+    int rc;
+
+    cmd = ble_ll_hci_vs_find_by_ocf(ocf);
+    if (!cmd) {
+        rc = BLE_ERR_UNKNOWN_HCI_CMD;
+    } else {
+        rc = cmd->cb(ocf, cmdbuf, cmdlen, rspbuf, rsplen);
+    }
+
+    return rc;
+}
+
+void
+ble_ll_hci_vs_register(struct ble_ll_hci_vs_cmd *cmds, uint32_t num_cmds)
+{
+    uint32_t i;
+
+    /* Assume all cmds are registered early on init, so just assert in case of
+     * invalid request since it means something is wrong with the code itself.
+     */
+
+    for (i = 0; i < num_cmds; i++, cmds++) {
+        BLE_LL_ASSERT(cmds->cb != NULL);
+        BLE_LL_ASSERT(ble_ll_hci_vs_find_by_ocf(cmds->ocf) == NULL);
+
+        SLIST_INSERT_HEAD(&g_ble_ll_hci_vs_list, cmds, link);
+    }
+}
+
+void
+ble_ll_hci_vs_init(void)
+{
+    SLIST_INIT(&g_ble_ll_hci_vs_list);
+
+    ble_ll_hci_vs_register(g_ble_ll_hci_vs_cmds,
+                           ARRAY_SIZE(g_ble_ll_hci_vs_cmds));
+}
+
+#endif

nimble/controller/syscfg.yml

@@ -348,6 +348,11 @@ syscfg.defs:
              depending on specified HCI command length.
         value: 0
 
+    BLE_LL_HCI_VS:
+        description: >
+            Enables support for vendor-specific HCI commands.
+        value: MYNEWT_VAL(BLE_HCI_VS)
+
     BLE_LL_HCI_VS_EVENT_ON_ASSERT:
         description: >
             This options enables controller to send a vendor-specific event on