From 20e0cd1b0eae4f4dad08ae6e97b77a237237d526 Mon Sep 17 00:00:00 2001
From: Vincent Coubard <vincent.coubard@arm.com>
Date: Fri, 7 Dec 2018 12:35:20 +0000
Subject: [PATCH] BLE: Inject random static address during reset sequence.

---
 .../TARGET_NRF5x/NRFCordioHCIDriver.cpp         | 17 +++++++++++++++--
 .../TARGET_NRF5x/NRFCordioHCIDriver.h           |  5 +++++
 2 files changed, 20 insertions(+), 2 deletions(-)

diff --git a/features/FEATURE_BLE/targets/TARGET_NORDIC/TARGET_NORDIC_CORDIO/TARGET_NRF5x/NRFCordioHCIDriver.cpp b/features/FEATURE_BLE/targets/TARGET_NORDIC/TARGET_NORDIC_CORDIO/TARGET_NRF5x/NRFCordioHCIDriver.cpp
index 7f8be870f4..a1d6124d85 100644
--- a/features/FEATURE_BLE/targets/TARGET_NORDIC/TARGET_NORDIC_CORDIO/TARGET_NRF5x/NRFCordioHCIDriver.cpp
+++ b/features/FEATURE_BLE/targets/TARGET_NORDIC/TARGET_NORDIC_CORDIO/TARGET_NRF5x/NRFCordioHCIDriver.cpp
@@ -297,6 +297,21 @@ void NRFCordioHCIDriver::start_reset_sequence()
     CordioHCIDriver::start_reset_sequence();
 }
 
+bool NRFCordioHCIDriver::get_random_static_address(ble::address_t& address)
+{
+    /* Load address from nRF configuration. */
+    uint64_t devAddr = (((uint64_t)NRF_FICR->DEVICEADDR[0]) <<  0) |
+                       (((uint64_t)NRF_FICR->DEVICEADDR[1]) << 32);
+
+    for (size_t i = 0; i < address.size(); ++i) {
+        address[i] = devAddr >> (i * 8);
+    }
+
+    address[5] |= 0xC0;     /* cf. "Static Address" (Vol C, Part 3, section 10.8.1) */
+    return true;
+}
+
+
 ble::vendor::cordio::CordioHCIDriver& ble_cordio_get_hci_driver() { 
     static NRFCordioHCITransportDriver transport_driver;
 
@@ -328,6 +343,4 @@ void PlatformLoadBdAddress(uint8_t *pDevAddr)
     pDevAddr[i] = devAddr >> (i * 8);
     i++;
   }
-
-  pDevAddr[5] |= 0xC0;     /* cf. "Static Address" (Vol C, Part 3, section 10.8.1) */
 }
diff --git a/features/FEATURE_BLE/targets/TARGET_NORDIC/TARGET_NORDIC_CORDIO/TARGET_NRF5x/NRFCordioHCIDriver.h b/features/FEATURE_BLE/targets/TARGET_NORDIC/TARGET_NORDIC_CORDIO/TARGET_NRF5x/NRFCordioHCIDriver.h
index 2efed9d315..0904d2bd22 100644
--- a/features/FEATURE_BLE/targets/TARGET_NORDIC/TARGET_NORDIC_CORDIO/TARGET_NRF5x/NRFCordioHCIDriver.h
+++ b/features/FEATURE_BLE/targets/TARGET_NORDIC/TARGET_NORDIC_CORDIO/TARGET_NRF5x/NRFCordioHCIDriver.h
@@ -59,6 +59,11 @@ public:
      */
     //virtual void handle_reset_sequence(uint8_t *msg);
 
+    /**
+     * @copydoc cordio::CordioHCIDriver::get_random_static_address
+     */
+    virtual bool get_random_static_address(ble::address_t& address);
+
 private:
     /**
      * Initialize the chip.