From ad9914ed4952ffa34f7bd681cf6176b4aec0d1f4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=C5=81ukasz=20Rymanowski?= <lukasz.rymanowski@codecoup.pl>
Date: Fri, 30 Nov 2018 16:06:38 +0100
Subject: [PATCH] nimble/ll: Fix Connect Ind send on directed advertising

It solves following issue:

Scenario:
1. Nimble has peer device in the resolving list.
2. Peer device is in Device Privacy Mode
3. Peer device is using its RPA and our identity address as InitA in
directed advertising

Issue:
Nimble send Connect Ind with its RPA instead of InitA

This should fix: LL/CON/INI/BV-10-C and LL/CON/INI/BV-21-C
---
 nimble/controller/src/ble_ll_conn.c | 80 +++++++++++++++++------------
 1 file changed, 48 insertions(+), 32 deletions(-)

diff --git a/nimble/controller/src/ble_ll_conn.c b/nimble/controller/src/ble_ll_conn.c
index c960e729b..a528b7b25 100644
--- a/nimble/controller/src/ble_ll_conn.c
+++ b/nimble/controller/src/ble_ll_conn.c
@@ -2684,10 +2684,14 @@ ble_ll_conn_event_end(struct ble_npl_event *ev)
  * @param m
  * @param adva
  * @param addr_type     Address type of ADVA from received advertisement.
+ * @param inita
+ * @param inita_type     Address type of INITA from received advertisement.
+
  * @param txoffset      The tx window offset for this connection
  */
 static void
 ble_ll_conn_req_pdu_update(struct os_mbuf *m, uint8_t *adva, uint8_t addr_type,
+                           uint8_t *inita, uint8_t inita_type,
                            uint16_t txoffset, int rpa_index)
 {
     uint8_t hdr;
@@ -2715,46 +2719,54 @@ ble_ll_conn_req_pdu_update(struct os_mbuf *m, uint8_t *adva, uint8_t addr_type,
 
     dptr = m->om_data;
 
-    /* Get pointer to our device address */
-    connsm = g_ble_ll_conn_create_sm;
-    if ((connsm->own_addr_type & 1) == 0) {
-        addr = g_dev_addr;
+    if (inita) {
+        memcpy(dptr, inita, BLE_DEV_ADDR_LEN);
+        if (inita_type) {
+            hdr |= BLE_ADV_PDU_HDR_TXADD_RAND;
+        }
     } else {
-        hdr |= BLE_ADV_PDU_HDR_TXADD_RAND;
-        addr = g_random_addr;
-    }
+        /* Get pointer to our device address */
+        connsm = g_ble_ll_conn_create_sm;
+        if ((connsm->own_addr_type & 1) == 0) {
+            addr = g_dev_addr;
+        } else {
+            hdr |= BLE_ADV_PDU_HDR_TXADD_RAND;
+            addr = g_random_addr;
+        }
 
     /* XXX: do this ahead of time? Calculate the local rpa I mean */
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
-    if (connsm->own_addr_type > BLE_HCI_ADV_OWN_ADDR_RANDOM) {
-        rl = NULL;
-        is_rpa = ble_ll_is_rpa(adva, addr_type);
-        if (is_rpa) {
-            if (rpa_index >= 0) {
-                rl = &g_ble_ll_resolv_list[rpa_index];
+        if (connsm->own_addr_type > BLE_HCI_ADV_OWN_ADDR_RANDOM) {
+            rl = NULL;
+            is_rpa = ble_ll_is_rpa(adva, addr_type);
+            if (is_rpa) {
+                if (rpa_index >= 0) {
+                    rl = &g_ble_ll_resolv_list[rpa_index];
+                }
+            } else {
+                if (ble_ll_resolv_enabled()) {
+                    rl = ble_ll_resolv_list_find(adva, addr_type);
+                }
             }
-        } else {
-            if (ble_ll_resolv_enabled()) {
-                rl = ble_ll_resolv_list_find(adva, addr_type);
-            }
-        }
 
-        /*
-         * If peer in on resolving list, we use RPA generated with Local IRK
-         * from resolving list entry. In other case, we need to use our identity
-         * address (see  Core 5.0, Vol 6, Part B, section 6.4).
-         */
-        if (rl) {
-            hdr |= BLE_ADV_PDU_HDR_TXADD_RAND;
-            ble_ll_resolv_get_priv_addr(rl, 1, dptr);
-            addr = NULL;
+            /*
+             * If peer in on resolving list, we use RPA generated with Local IRK
+             * from resolving list entry. In other case, we need to use our identity
+             * address (see  Core 5.0, Vol 6, Part B, section 6.4).
+             */
+            if (rl) {
+                hdr |= BLE_ADV_PDU_HDR_TXADD_RAND;
+                ble_ll_resolv_get_priv_addr(rl, 1, dptr);
+                addr = NULL;
+            }
         }
-    }
 #endif
 
-    if (addr) {
-        memcpy(dptr, addr, BLE_DEV_ADDR_LEN);
+        if (addr) {
+            memcpy(dptr, addr, BLE_DEV_ADDR_LEN);
+        }
     }
+
     memcpy(dptr + BLE_DEV_ADDR_LEN, adva, BLE_DEV_ADDR_LEN);
     put_le16(dptr + 20, txoffset);
 
@@ -2875,7 +2887,9 @@ ble_ll_conn_req_txend_init(void *arg)
  * @param adva Address of advertiser
  */
 int
-ble_ll_conn_request_send(uint8_t addr_type, uint8_t *adva, uint16_t txoffset,
+ble_ll_conn_request_send(uint8_t addr_type, uint8_t *adva,
+                         uint8_t inita_type, uint8_t *inita,
+                         uint16_t txoffset,
                          int rpa_index, uint8_t end_trans)
 {
     struct os_mbuf *m;
@@ -2883,7 +2897,8 @@ ble_ll_conn_request_send(uint8_t addr_type, uint8_t *adva, uint16_t txoffset,
 
     /* XXX: TODO: assume we are already on correct phy */
     m = ble_ll_scan_get_pdu();
-    ble_ll_conn_req_pdu_update(m, adva, addr_type, txoffset, rpa_index);
+    ble_ll_conn_req_pdu_update(m, adva, addr_type, inita, inita_type,
+                               txoffset, rpa_index);
     if (end_trans == BLE_PHY_TRANSITION_NONE) {
         ble_phy_set_txend_cb(ble_ll_conn_req_txend, NULL);
     } else {
@@ -3433,6 +3448,7 @@ ble_ll_init_rx_isr_end(uint8_t *rxbuf, uint8_t crcok,
 
     /* Setup to transmit the connect request */
     rc = ble_ll_conn_request_send(addr_type, adv_addr,
+                                  init_addr_type, init_addr,
                                   connsm->tx_win_off, index,
                                   conn_req_end_trans);
     if (rc) {