From 6ab40a1eb165d8d93201f75682d3700762abc5d5 Mon Sep 17 00:00:00 2001
From: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
Date: Mon, 14 Jan 2019 23:36:34 +0100
Subject: [PATCH] nimble/ll: Add API to set controller public address

ble_ll_set_public_addr() can be used to set controller public address.
This API can be called in e.g. hal_bsp_init() to set public address
loaded from some vendor specific location. Address set in this way will
take precedence over address configured in syscfg or stored in HW.

NOTE: This is *NOT* intended to be used to modify public address on
already running device. Using it for such purpose may yield unknown
results.
---
 nimble/controller/include/controller/ble_ll.h |  9 +++++
 nimble/controller/src/ble_ll.c                | 33 ++++++++++++++-----
 2 files changed, 33 insertions(+), 9 deletions(-)

diff --git a/nimble/controller/include/controller/ble_ll.h b/nimble/controller/include/controller/ble_ll.h
index f6d203f2b..31a93c9a5 100644
--- a/nimble/controller/include/controller/ble_ll.h
+++ b/nimble/controller/include/controller/ble_ll.h
@@ -483,6 +483,15 @@ void ble_ll_event_send(struct ble_npl_event *ev);
 /* Hand received pdu's to LL task  */
 void ble_ll_rx_pdu_in(struct os_mbuf *rxpdu);
 
+/*
+ * Set public address
+ *
+ * This can be used to set controller public address from vendor specific storage,
+ * usually should be done in hal_bsp_init().
+ * Shall be *only* called before LL is initialized, i.e. before sysinit stage.
+ */
+int ble_ll_set_public_addr(const uint8_t *addr);
+
 /* Set random address */
 int ble_ll_set_random_addr(uint8_t *addr, bool hci_adv_ext);
 
diff --git a/nimble/controller/src/ble_ll.c b/nimble/controller/src/ble_ll.c
index a2b59ef81..041416788 100644
--- a/nimble/controller/src/ble_ll.c
+++ b/nimble/controller/src/ble_ll.c
@@ -458,6 +458,14 @@ ble_ll_is_valid_random_addr(uint8_t *addr)
     return rc;
 }
 
+int
+ble_ll_set_public_addr(const uint8_t *addr)
+{
+    memcpy(g_dev_addr, addr, BLE_DEV_ADDR_LEN);
+
+    return BLE_ERR_SUCCESS;
+}
+
 /**
  * Called from the HCI command parser when the set random address command
  * is received.
@@ -1412,6 +1420,12 @@ ble_ll_pdu_max_tx_octets_get(uint32_t usecs, int phy_mode)
     return max(27, octets);
 }
 
+static inline bool
+ble_ll_is_addr_empty(const uint8_t *addr)
+{
+    return memcmp(addr, BLE_ADDR_ANY, BLE_DEV_ADDR_LEN) == 0;
+}
+
 /**
  * Initialize the Link Layer. Should be called only once
  *
@@ -1434,16 +1448,17 @@ ble_ll_init(void)
     ble_ll_trace_init();
     ble_phy_trace_init();
 
-    /* Retrieve the public device address if not set by syscfg */
-    memcpy(&addr.val[0], MYNEWT_VAL_BLE_PUBLIC_DEV_ADDR, BLE_DEV_ADDR_LEN);
-    if (!memcmp(&addr.val[0], ((ble_addr_t *)BLE_ADDR_ANY)->val,
-                BLE_DEV_ADDR_LEN)) {
-        rc = ble_hw_get_public_addr(&addr);
-        if (!rc) {
-            memcpy(g_dev_addr, &addr.val[0], BLE_DEV_ADDR_LEN);
+    /* Set public device address if not already set */
+    if (ble_ll_is_addr_empty(g_dev_addr)) {
+        /* Use sycfg address if configured, otherwise try to read from HW */
+        if (!ble_ll_is_addr_empty(MYNEWT_VAL(BLE_PUBLIC_DEV_ADDR))) {
+            memcpy(g_dev_addr, MYNEWT_VAL(BLE_PUBLIC_DEV_ADDR), BLE_DEV_ADDR_LEN);
+        } else {
+            rc = ble_hw_get_public_addr(&addr);
+            if (!rc) {
+                memcpy(g_dev_addr, &addr.val[0], BLE_DEV_ADDR_LEN);
+            }
         }
-    } else {
-        memcpy(g_dev_addr, &addr.val[0], BLE_DEV_ADDR_LEN);
     }
 
 #ifdef BLE_XCVR_RFCLK