From f0e85818f503f6fc680949721e35dc31b8ea0d4f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=C5=81ukasz=20Rymanowski?= <lukasz.rymanowski@codecoup.pl>
Date: Wed, 5 Dec 2018 10:57:02 +0100
Subject: [PATCH] nimble/ll: Fix RPA address in the LE Enhanced Connection
 Complete

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 on which Nimble send Connect Ind with its identity address

Issue:
Nimble incorretly set Local RPA address in the LE Enhanced Connect Completed event.

This should fix: LL/CON/INI/BV-10-C and LL/CON/INI/BV-21-C
---
 nimble/controller/include/controller/ble_ll_conn.h | 6 ++++++
 nimble/controller/src/ble_ll_conn.c                | 6 ++++++
 nimble/controller/src/ble_ll_conn_hci.c            | 8 +++++++-
 3 files changed, 19 insertions(+), 1 deletion(-)

diff --git a/nimble/controller/include/controller/ble_ll_conn.h b/nimble/controller/include/controller/ble_ll_conn.h
index 5d13b8d57..38289a0b4 100644
--- a/nimble/controller/include/controller/ble_ll_conn.h
+++ b/nimble/controller/include/controller/ble_ll_conn.h
@@ -269,6 +269,12 @@ struct ble_ll_conn_sm
     uint32_t slave_cur_window_widening;
     uint32_t last_rxd_pdu_cputime;  /* Used exclusively for supervision timer */
 
+    /*
+     * Used to mark that direct advertising from the peer was using
+     * identity address as InitA
+     */
+    uint8_t inita_identity_used;
+
     /* address information */
     uint8_t own_addr_type;
     uint8_t peer_addr_type;
diff --git a/nimble/controller/src/ble_ll_conn.c b/nimble/controller/src/ble_ll_conn.c
index a528b7b25..abdd90c3c 100644
--- a/nimble/controller/src/ble_ll_conn.c
+++ b/nimble/controller/src/ble_ll_conn.c
@@ -849,6 +849,8 @@ ble_ll_conn_init_wfr_timer_exp(void)
         STATS_INC(ble_ll_stats, aux_missed_adv);
         ble_ll_event_send(&scansm->scan_sched_ev);
     }
+
+    connsm->inita_identity_used = 0;
 #endif
 }
 /**
@@ -3456,6 +3458,10 @@ ble_ll_init_rx_isr_end(uint8_t *rxbuf, uint8_t crcok,
         goto init_rx_isr_exit;
     }
 
+    if (init_addr && !inita_is_rpa) {
+        connsm->inita_identity_used = 1;
+    }
+
     CONN_F_CONN_REQ_TXD(connsm) = 1;
 
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
diff --git a/nimble/controller/src/ble_ll_conn_hci.c b/nimble/controller/src/ble_ll_conn_hci.c
index 526bad00d..7402895c0 100644
--- a/nimble/controller/src/ble_ll_conn_hci.c
+++ b/nimble/controller/src/ble_ll_conn_hci.c
@@ -163,7 +163,13 @@ ble_ll_conn_comp_event_send(struct ble_ll_conn_sm *connsm, uint8_t status,
             if (enh_enabled) {
                 memset(evdata, 0, 2 * BLE_DEV_ADDR_LEN);
                 if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
-                    if (connsm->own_addr_type > BLE_HCI_ADV_OWN_ADDR_RANDOM) {
+                    if (connsm->inita_identity_used) {
+                        /* If it was direct advertising we were replying to and we used
+                         * identity address there (which might be just fine), we should
+                         * we should take it into account here in this event.
+                         */
+                        rpa = NULL;
+                    } else  if (connsm->own_addr_type > BLE_HCI_ADV_OWN_ADDR_RANDOM) {
                         rpa = ble_ll_scan_get_local_rpa();
                     } else {
                         rpa = NULL;