From 310d741ca7d65244d51ccc896ff255bb0e5cc246 Mon Sep 17 00:00:00 2001
From: Rahul Tank <rahul.tank@espressif.com>
Date: Fri, 15 Sep 2023 08:24:27 +0530
Subject: [PATCH] host/ble_gap.c : Check allowed random address

Add a check to validate whether the random address being set is
allowed as per spec
---
 nimble/host/include/host/ble_gap.h | 12 ++++++++++++
 nimble/host/src/ble_gap.c          | 30 ++++++++++++++++++++++++++++++
 2 files changed, 42 insertions(+)

diff --git a/nimble/host/include/host/ble_gap.h b/nimble/host/include/host/ble_gap.h
index 2a311db2a8..9ac6608339 100644
--- a/nimble/host/include/host/ble_gap.h
+++ b/nimble/host/include/host/ble_gap.h
@@ -292,6 +292,18 @@ struct hci_conn_update;
 
 /** @} */
 
+/**
+ * @defgroup Mask for checking random address validity
+ * @{
+ */
+/** Static random address check mask. */
+#define BLE_STATIC_RAND_ADDR_MASK           0xC0
+
+/** Non RPA check mask. */
+#define BLE_NON_RPA_MASK                    0x3F
+
+/** @} */
+
 /** Connection security state */
 struct ble_gap_sec_state {
     /** If connection is encrypted */
diff --git a/nimble/host/src/ble_gap.c b/nimble/host/src/ble_gap.c
index 9390708f9b..cab710fcb9 100644
--- a/nimble/host/src/ble_gap.c
+++ b/nimble/host/src/ble_gap.c
@@ -3076,6 +3076,7 @@ int
 ble_gap_ext_adv_set_addr(uint8_t instance, const ble_addr_t *addr)
 {
     int rc;
+    ble_addr_t invalid_non_rpa_addr, invalid_static_rand_addr;
 
     if (instance >= BLE_ADV_INSTANCES || addr->type != BLE_ADDR_RANDOM) {
         return BLE_HS_EINVAL;
@@ -3085,6 +3086,35 @@ ble_gap_ext_adv_set_addr(uint8_t instance, const ble_addr_t *addr)
         return BLE_HS_EDISABLED;
     }
 
+    /*
+       A static address is a 48-bit randomly generated address and shall meet the following requirements:
+       The two most significant bits of the address shall be equal to 1
+       All bits of the random part of the address shall not be equal to 1
+       All bits of the random part of the address shall not be equal to 0
+     */
+
+    memset(&invalid_non_rpa_addr.val, 0xff, BLE_DEV_ADDR_LEN);
+    memset(&invalid_static_rand_addr.val, 0x00, BLE_DEV_ADDR_LEN);
+
+    if ((addr->val[5] & BLE_STATIC_RAND_ADDR_MASK) == BLE_STATIC_RAND_ADDR_MASK) {
+        invalid_static_rand_addr.val[5] = invalid_static_rand_addr.val[5] | BLE_STATIC_RAND_ADDR_MASK;
+
+        if (memcmp(invalid_non_rpa_addr.val, addr->val, BLE_DEV_ADDR_LEN) == 0 ||
+            memcmp(invalid_static_rand_addr.val, addr->val, BLE_DEV_ADDR_LEN) == 0) {
+            return BLE_HS_EINVAL;
+        }
+    } else if ((addr->val[5] | BLE_NON_RPA_MASK) == BLE_NON_RPA_MASK) {
+        invalid_non_rpa_addr.val[5] = invalid_non_rpa_addr.val[5] & BLE_NON_RPA_MASK;
+
+        if (memcmp(invalid_non_rpa_addr.val, addr->val, BLE_DEV_ADDR_LEN) == 0 ||
+            memcmp(invalid_static_rand_addr.val, addr->val, BLE_DEV_ADDR_LEN) == 0) {
+            return BLE_HS_EINVAL;
+        }
+    } else {
+        BLE_HS_LOG(ERROR, "Invalid random address \n");
+        return BLE_HS_EINVAL;
+    }
+
     ble_hs_lock();
     rc = ble_gap_ext_adv_set_addr_no_lock(instance, addr->val);
     ble_hs_unlock();