fix(nimble): Add API to find identity address for given RPA address

nimble/host/include/host/ble_esp_gap.h

@@ -316,6 +316,20 @@ void ble_gap_rx_test_evt(const void *buf, uint8_t len);
 void ble_gap_tx_test_evt(const void *buf, uint8_t len);
 void ble_gap_end_test_evt(const void *buf, uint8_t len);
 
+/**
+ * Try to resolve an RPA using all stored peer IRKs and local IRKs.
+ *
+ * - rpa: input Resolvable Private Address
+ * - ida: output Identity Address
+ * - addr_type: output Address type
+ *        * If resolved with peer IRK -> peer identity address is copied
+ *        * If resolved with local IRK -> set to all zeros
+ *        * If not resolved -> unchanged
+ *
+ * Returns: ESP_OK if resolve, ESP_FAIL otherwise.
+ */
+bool ble_gap_rpa_resolve(uint8_t *rpa, uint8_t *ida, uint8_t *addr_type);
+
 #ifdef __cplusplus
 }
 #endif

nimble/host/src/ble_gap.c

@@ -10505,3 +10505,58 @@ int ble_gap_set_host_feat(uint8_t bit_num, uint8_t bit_val)
                              BLE_HCI_OCF_LE_SET_HOST_FEATURE),&cmd, sizeof(cmd), NULL, 0);
 }
 #endif
+
+/* Check if input is an RPA (Resolvable Private Address) */
+static inline bool is_rpa(const uint8_t *addr)
+{
+    /* prand = 3 MSB bytes of address */
+    return ((addr[5] & 0xC0) == 0x40);
+}
+
+bool ble_gap_rpa_resolve(uint8_t *rpa, uint8_t *ida, uint8_t *addr_type)
+{
+    if (!is_rpa(rpa)) {
+        return ESP_FAIL; /* reject public / static random / non-RPA */
+    }
+
+    /* ---- Step 1: Try peer IRKs ---- */
+#if MYNEWT_VAL(BLE_STORE_MAX_BONDS) > 0
+    int rc;
+    int count = 0;
+    ble_addr_t peer_id_addrs[MYNEWT_VAL(BLE_STORE_MAX_BONDS)];
+
+    rc = ble_store_util_bonded_peers(&peer_id_addrs[0], &count, MYNEWT_VAL(BLE_STORE_MAX_BONDS));
+
+    if (rc == 0 && count > 0) {
+        for (int i = 0; i < count; i++) {
+            struct ble_store_value_sec sec = {0};
+            struct ble_store_key_sec key = {0};
+
+            key.peer_addr = peer_id_addrs[i];
+            rc = ble_store_read_peer_sec(&key, &sec);
+            if (rc == 0 && sec.irk_present) {
+                if (ble_hs_pvcy_resolve_with_irk(rpa, sec.irk)) {
+                    /* Found a match -> copy peer identity address */
+                    memcpy(ida, sec.peer_addr.val, 6);
+                    *addr_type = sec.peer_addr.type; /* set type */
+                    return ESP_OK;
+                }
+            }
+        }
+    }
+#endif
+
+    /* ---- Step 2: Try our (local) IRKs ---- */
+    struct ble_store_value_local_irk value = {0};
+    struct ble_store_key_local_irk key = {0};
+
+    (void)ble_store_read_local_irk(&key, &value);
+    if (ble_hs_pvcy_resolve_with_irk(rpa, value.irk)) {
+        /* Match with local IRK -> return ida = 00:00:00:00:00:00 */
+        memset(ida, 0, 6);
+	*addr_type = BLE_ADDR_PUBLIC; /* default type */
+        return ESP_OK;
+    }
+
+    return ESP_FAIL; /* No match */
+}

nimble/host/src/ble_hs_pvcy.c

@@ -453,4 +453,42 @@ ble_hs_pvcy_rpa_config(uint8_t enable)
     return rc;
 }
 #endif
+
+int
+ble_hs_pvcy_rpa_ah(const uint8_t irk[16], const uint8_t prand[3], uint8_t out[3])
+{
+    uint8_t plaintext[16] = {0};
+    uint8_t ciphertext[16] = {0};
+    int rc;
+
+    /* prand goes in MSB of plaintext */
+    memcpy(plaintext, prand, 3);
+
+    rc = ble_sm_alg_encrypt(irk, plaintext, ciphertext);
+    if (rc != 0) {
+        return rc;
+    }
+
+    memcpy(out, ciphertext, 3);
+    return 0;
+}
+
+bool
+ble_hs_pvcy_resolve_with_irk(const uint8_t rpa[6], const uint8_t irk[16])
+{
+    uint8_t hash[3];
+    uint8_t out[3];
+
+    /* hash = most significant 3 bytes of RPA */
+    memcpy(hash, rpa, 3);
+
+    /* prand = least significant 3 bytes of RPA */
+    const uint8_t *prand = rpa + 3;
+
+    if (ble_hs_pvcy_rpa_ah(irk, prand, out) != 0) {
+        return false;
+    }
+
+    return (memcmp(hash, out, 3) == 0);
+}
 #endif

nimble/host/src/ble_hs_pvcy_priv.h

@@ -43,6 +43,7 @@ int ble_hs_pvcy_set_mode(const ble_addr_t *addr, uint8_t priv_mode);
 #if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
 bool ble_hs_pvcy_enabled(void);
 #endif
+bool ble_hs_pvcy_resolve_with_irk(const uint8_t rpa[6], const uint8_t irk[16]);
 
 void ble_hs_pvcy_irk_deinit(void);
 #ifdef __cplusplus