From af143c96fcc579c4f64901dd8d3e8d6b489b3658 Mon Sep 17 00:00:00 2001
From: raiden00pl <raiden00@railab.me>
Date: Fri, 17 Dec 2021 15:37:40 +0100
Subject: [PATCH] arch/arm/src/nrf52/nrf52_sdc.c: public device address and
 static device address support

---
 arch/arm/src/nrf52/Kconfig     |   8 +++
 arch/arm/src/nrf52/nrf52_sdc.c | 116 +++++++++++++++++++++++++++++++++
 2 files changed, 124 insertions(+)

diff --git a/arch/arm/src/nrf52/Kconfig b/arch/arm/src/nrf52/Kconfig
index feb16a8dc1..3205806500 100644
--- a/arch/arm/src/nrf52/Kconfig
+++ b/arch/arm/src/nrf52/Kconfig
@@ -730,5 +730,13 @@ config NRF52_BLE_TTY_NAME
 	default "/dev/ttyHCI0"
 	depends on UART_BTH4
 
+config NRF52_SDC_FICR_STATIC_ADDR
+	bool "Configure factory generated static random address"
+	default n
+
+config NRF52_SDC_PUB_ADDR
+	hex "Configure BT public address"
+	default 0x0000000000
+
 endif
 
diff --git a/arch/arm/src/nrf52/nrf52_sdc.c b/arch/arm/src/nrf52/nrf52_sdc.c
index 310daea565..97e5d1bc0c 100644
--- a/arch/arm/src/nrf52/nrf52_sdc.c
+++ b/arch/arm/src/nrf52/nrf52_sdc.c
@@ -44,6 +44,8 @@
 #include "ram_vectors.h"
 #include "arm_arch.h"
 
+#include "hardware/nrf52_ficr.h"
+
 #include <mpsl.h>
 #include <sdc.h>
 #include <sdc_hci.h>
@@ -79,6 +81,11 @@
 #define MEMPOOL_SIZE  ((CONFIG_NRF52_SDC_SLAVE_COUNT * SLAVE_MEM_SIZE) + \
                        (SDC_MASTER_COUNT * MASTER_MEM_SIZE))
 
+#if (CONFIG_NRF52_SDC_PUB_ADDR > 0) ||          \
+  defined(CONFIG_NRF52_SDC_FICR_STATIC_ADDR)
+#  define HAVE_BTADDR_CONFIGURE
+#endif
+
 /****************************************************************************
  * Private Types
  ****************************************************************************/
@@ -92,6 +99,25 @@ struct nrf52_sdc_dev_s
   struct work_s work;
 };
 
+begin_packed_struct struct sdc_hci_cmd_vs_zephyr_write_bd_addr_s
+{
+  uint8_t bd_addr[6];
+} end_packed_struct;
+
+begin_packed_struct struct sdc_hci_cmd_le_set_random_address_s
+{
+  uint8_t bd_addr[6];
+} end_packed_struct;
+
+/****************************************************************************
+ * External Function Prototypes
+ ****************************************************************************/
+
+uint8_t sdc_hci_cmd_vs_zephyr_write_bd_addr(
+  FAR const struct sdc_hci_cmd_vs_zephyr_write_bd_addr_s *p_params);
+uint8_t sdc_hci_cmd_le_set_random_address(
+  FAR const struct sdc_hci_cmd_le_set_random_address_s *p_params);
+
 /****************************************************************************
  * Private Function Prototypes
  ****************************************************************************/
@@ -377,6 +403,85 @@ static void radio_handler(void)
   MPSL_IRQ_RADIO_Handler();
 }
 
+#ifdef HAVE_BTADDR_CONFIGURE
+/****************************************************************************
+ * Name: nrf52_sdc_btaddr_configure
+ ****************************************************************************/
+
+static int nrf52_sdc_btaddr_configure(void)
+{
+#ifdef CONFIG_NRF52_SDC_FICR_STATIC_ADDR
+  struct sdc_hci_cmd_le_set_random_address_s   rand_addr;
+#endif
+#if CONFIG_NRF52_SDC_PUB_ADDR > 0
+  struct sdc_hci_cmd_vs_zephyr_write_bd_addr_s pub_addr;
+#endif
+#ifdef CONFIG_NRF52_SDC_FICR_STATIC_ADDR
+  uint32_t                              regval   = 0;
+  uint32_t                              addr[2];
+  uint32_t                              addrtype = 0;
+#endif
+  int                                   ret      = OK;
+
+#ifdef CONFIG_NRF52_SDC_FICR_STATIC_ADDR
+  /* Get device address type */
+
+  addrtype = getreg32(NRF52_FICR_DEVICEADDRTYPE);
+
+  /* Get device addr from FICR */
+
+  addr[0] = getreg32(NRF52_FICR_DEVICEADDR0);
+  addr[1] = getreg32(NRF52_FICR_DEVICEADDR1);
+
+  if ((addrtype & 0x01) == FICR_DEVICEADDRTYPE_RANDOM)
+    {
+      /* Configure static random address */
+
+      memcpy(&rand_addr.bd_addr[0], &addr[0], 4);
+      memcpy(&rand_addr.bd_addr[4], &addr[1], 2);
+
+      /* The two most significant bits of the address shall be set */
+
+      rand_addr.bd_addr[4] |= 0x0c;
+
+      ret = sdc_hci_cmd_le_set_random_address(&rand_addr);
+      if (ret < 0)
+        {
+          wlerr("sdc_hci_cmd_le_set_random_address failed: %d\n", ret);
+          goto errout;
+        }
+    }
+  else
+    {
+      wlerr("Static random address not available\n");
+      ret = -EINVAL;
+      goto errout;
+    }
+#endif
+
+#if CONFIG_NRF52_SDC_PUB_ADDR > 0
+  /* Configure public address if available */
+
+  pub_addr.bd_addr[0] = (CONFIG_NRF52_SDC_PUB_ADDR >> (8 * 5)) & 0xff;
+  pub_addr.bd_addr[1] = (CONFIG_NRF52_SDC_PUB_ADDR >> (8 * 4)) & 0xff;
+  pub_addr.bd_addr[2] = (CONFIG_NRF52_SDC_PUB_ADDR >> (8 * 3)) & 0xff;
+  pub_addr.bd_addr[3] = (CONFIG_NRF52_SDC_PUB_ADDR >> (8 * 2)) & 0xff;
+  pub_addr.bd_addr[4] = (CONFIG_NRF52_SDC_PUB_ADDR >> (8 * 1)) & 0xff;
+  pub_addr.bd_addr[5] = (CONFIG_NRF52_SDC_PUB_ADDR >> (8 * 0)) & 0xff;
+
+  ret = sdc_hci_cmd_vs_zephyr_write_bd_addr(&pub_addr);
+  if (ret < 0)
+    {
+      wlerr("sdc_hci_cmd_vs_zephyr_write_bd_addr failed: %d\n", ret);
+      goto errout;
+    }
+#endif
+
+errout:
+  return ret;
+}
+#endif
+
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
@@ -552,6 +657,17 @@ int nrf52_sdc_initialize(void)
     }
 #endif
 
+#ifdef HAVE_BTADDR_CONFIGURE
+  /* Configure BT address */
+
+  ret = nrf52_sdc_btaddr_configure();
+  if (ret < 0)
+    {
+      wlerr("Could not configure BT addr: %d\n", ret);
+      return ret;
+    }
+#endif
+
   /* Finally enable SoftDevice Controller */
 
   ret = sdc_enable(on_hci, g_sdc_dev.mempool);