From 395f404f6ace51c1cf4578047291a33c0e3b4e91 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/src/ble_gap.c        |  4 ++++
 nimble/host/src/ble_hs_id.c      | 41 ++++++++++++++++++++++++++++++++
 nimble/host/src/ble_hs_id_priv.h |  1 +
 3 files changed, 46 insertions(+)

diff --git a/nimble/host/src/ble_gap.c b/nimble/host/src/ble_gap.c
index 807796dd52..191980819e 100644
--- a/nimble/host/src/ble_gap.c
+++ b/nimble/host/src/ble_gap.c
@@ -3089,6 +3089,10 @@ ble_gap_ext_adv_set_addr(uint8_t instance, const ble_addr_t *addr)
         return BLE_HS_EDISABLED;
     }
 
+    if (!(ble_hs_id_is_rpa(addr))) {
+        return BLE_HS_EINVAL;
+    }
+
     ble_hs_lock();
     rc = ble_gap_ext_adv_set_addr_no_lock(instance, addr->val);
     ble_hs_unlock();
diff --git a/nimble/host/src/ble_hs_id.c b/nimble/host/src/ble_hs_id.c
index 2dd21aa785..c7e879dca7 100644
--- a/nimble/host/src/ble_hs_id.c
+++ b/nimble/host/src/ble_hs_id.c
@@ -25,6 +25,47 @@ static uint8_t ble_hs_id_pub[6];
 static uint8_t ble_hs_id_rnd[6];
 static const uint8_t ble_hs_misc_null_addr[6];
 
+bool
+ble_hs_id_is_rpa(const ble_addr_t *addr)
+{
+    ble_addr_t invalid_non_rpa_addr, invalid_static_rand_addr;
+    bool rc = 1;
+
+    if (!addr->type) {
+        return 0;
+    }
+
+    /*
+      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] & 0xc0) == 0xc0) {
+        invalid_static_rand_addr.val[5] = invalid_static_rand_addr.val[5] | 0xc0;
+
+        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 0;
+        }
+    } else if ((addr->val[5] | 0x3f) == 0x3f) {
+        invalid_non_rpa_addr.val[5] = invalid_non_rpa_addr.val[5] & 0x3f;
+
+        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 0;
+        }
+    } else {
+        BLE_HS_LOG(ERROR, "Invalid random address \n");
+        return 0;
+    }
+
+    return rc;
+}
 
 void
 ble_hs_id_set_pub(const uint8_t *pub_addr)
diff --git a/nimble/host/src/ble_hs_id_priv.h b/nimble/host/src/ble_hs_id_priv.h
index aa2827d41f..91f729679f 100644
--- a/nimble/host/src/ble_hs_id_priv.h
+++ b/nimble/host/src/ble_hs_id_priv.h
@@ -32,6 +32,7 @@ int ble_hs_id_addr(uint8_t id_addr_type, const uint8_t **out_id_addr,
 int ble_hs_id_use_addr(uint8_t addr_type);
 void ble_hs_id_reset(void);
 void ble_hs_id_rnd_reset(void);
+bool ble_hs_id_is_rpa(const ble_addr_t *addr);
 
 #ifdef __cplusplus
 }