nimble/ll: Add VS HCI command to set public device address

The public device address set by this command is applied only after HCI
Reset, thus it can be issues at any time.

To reset back to original public address set address to 0x000000000000.

Author: andrzej-kaczmarek

nimble/controller/include/controller/ble_ll_addr.h

@@ -25,6 +25,7 @@ extern "C" {
 #endif
 
 int ble_ll_addr_init(void);
+int ble_ll_addr_wr_public_addr(const uint8_t *addr);
 
 /* Address provider APIs - should be implemented by packages supporting
  * relevant APIs

nimble/controller/src/ble_ll_addr.c

@@ -28,6 +28,22 @@
 uint8_t g_dev_addr[BLE_DEV_ADDR_LEN];
 uint8_t g_random_addr[BLE_DEV_ADDR_LEN];
 
+static uint8_t g_ble_ll_addr_public[BLE_DEV_ADDR_LEN];
+
+static bool
+is_addr_empty(const uint8_t *addr)
+{
+    return memcmp(addr, BLE_ADDR_ANY, BLE_DEV_ADDR_LEN) == 0;
+}
+
+int
+ble_ll_addr_wr_public_addr(const uint8_t *addr)
+{
+    memcpy(g_ble_ll_addr_public, addr, BLE_DEV_ADDR_LEN);
+
+    return 0;
+}
+
 int
 ble_ll_addr_init(void)
 {
@@ -43,10 +59,15 @@ ble_ll_addr_init(void)
         pub_dev_addr >>= 8;
     }
 
-    /* Set public address from provider API, if available */
+    if (!is_addr_empty(g_ble_ll_addr_public)) {
+        /* Use public address set externally */
+        memcpy(g_dev_addr, g_ble_ll_addr_public, BLE_DEV_ADDR_LEN);
+    } else {
+        /* Set public address from provider API, if available */
 #if MYNEWT_API_ble_addr_provider_public
-    ble_ll_addr_provide_public(g_dev_addr);
+        ble_ll_addr_provide_public(g_dev_addr);
 #endif
+    }
 
 #if MYNEWT_VAL(BLE_LL_ADDR_INIT_RANDOM)
     /* Set random address from provider API, if available */

nimble/controller/src/ble_ll_hci_vs.c

@@ -390,6 +390,26 @@ ble_ll_hci_vs_set_scan_cfg(uint16_t ocf, const uint8_t *cmdbuf, uint8_t cmdlen,
 }
 #endif
 
+#if MYNEWT_VAL(BLE_LL_HCI_VS_WR_PUBLIC_ADDR)
+static int
+ble_ll_hci_vs_wr_public_addr(uint16_t ocf, const uint8_t *cmdbuf, uint8_t cmdlen,
+                             uint8_t *rspbuf, uint8_t *rsplen)
+{
+    const struct ble_hci_vs_wr_public_addr_cp *cp = (const void *)cmdbuf;
+    int rc;
+
+    if (cmdlen != sizeof(*cp)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (ble_ll_addr_wr_public_addr(cp->addr)) {
+        return BLE_ERR_UNSPECIFIED;
+    }
+
+    return 0;
+}
+#endif
+
 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),
@@ -424,6 +444,10 @@ static struct ble_ll_hci_vs_cmd g_ble_ll_hci_vs_cmds[] = {
     BLE_LL_HCI_VS_CMD(BLE_HCI_OCF_VS_SET_SCAN_CFG,
                       ble_ll_hci_vs_set_scan_cfg)
 #endif
+#if MYNEWT_VAL(BLE_LL_HCI_VS_WR_PUBLIC_ADDR)
+    BLE_LL_HCI_VS_CMD(BLE_HCI_OCF_VS_WR_PUBLIC_ADDR,
+                      ble_ll_hci_vs_wr_public_addr)
+#endif
 };
 
 static struct ble_ll_hci_vs_cmd *

nimble/controller/syscfg.yml

@@ -456,7 +456,14 @@ syscfg.defs:
             - BLE_LL_HCI_VS if 1
             - BLE_LL_CFG_FEAT_LL_EXT_ADV if 1
             - BLE_LL_ROLE_OBSERVER if 1
-
+    BLE_LL_HCI_VS_WR_PUBLIC_ADDR:
+        description: >
+            Enables HCI command to set public device address.
+            The new address is applied after HCI Reset. To reset back to default
+            public address, set address to 0x000000000000.
+        value: 0
+        restrictions:
+            - BLE_LL_HCI_VS if 1
 
     BLE_LL_HCI_VS_EVENT_ON_ASSERT:
         description: >

nimble/include/nimble/hci_common.h

@@ -1406,6 +1406,12 @@ struct ble_hci_vs_set_scan_cfg_cp {
     int8_t rssi_threshold;
 } __attribute__((packed));
 
+/* Set Public Bluetooth Address */
+#define BLE_HCI_OCF_VS_WR_PUBLIC_ADDR                   (MYNEWT_VAL(BLE_HCI_VS_OCF_OFFSET) + (0x000C))
+struct ble_hci_vs_wr_public_addr_cp {
+    uint8_t addr[6];
+} __attribute__((packed));
+
 /* Command Specific Definitions */
 /* --- Set controller to host flow control (OGF 0x03, OCF 0x0031) --- */
 #define BLE_HCI_CTLR_TO_HOST_FC_OFF         (0)