From 82ed1681f06e3e0bd7445534ba9bf46c2454b493 Mon Sep 17 00:00:00 2001
From: Pavlo Hamov <pavlo_hamov@jabil.com>
Date: Thu, 3 Oct 2019 09:52:22 +0300
Subject: [PATCH] drivers: ipm_stm32wb: add workaround for conn_complete

Enhanced connection complete message lacks of
the peer address (FF::FF), which leads to pairing/bonding failure.

Fixes: #19509

Signed-off-by: Pavlo Hamov <pavlo_hamov@jabil.com>
---
 drivers/bluetooth/hci/ipm_stm32wb.c | 30 +++++++++++++++++++++++++++++
 1 file changed, 30 insertions(+)

diff --git a/drivers/bluetooth/hci/ipm_stm32wb.c b/drivers/bluetooth/hci/ipm_stm32wb.c
index 38a0515bb92..e1a7e85cf60 100644
--- a/drivers/bluetooth/hci/ipm_stm32wb.c
+++ b/drivers/bluetooth/hci/ipm_stm32wb.c
@@ -96,6 +96,35 @@ static void syscmd_status_not(SHCI_TL_CmdStatus_t status)
 	BT_DBG("status:%d", status);
 }
 
+/*
+ * https://github.com/zephyrproject-rtos/zephyr/issues/19509
+ * Tested on nucleo_wb55rg (stm32wb55rg) BLE stack (v1.2.0)
+ * Unresolved Resolvable Private Addresses (RPA)
+ * is reported in the peer_rpa field, and not in the peer address,
+ * as it should, when this happens the peer address is set to all FFs
+ * 0A 00 01 08 01 01 FF FF FF FF FF FF 00 00 00 00 00 00 0C AA C5 B3 3D 6B ...
+ * If such message is passed to HCI core than pairing will essentially fail.
+ * Solution: Rewrite the event with the RPA in the PEER address field
+ */
+static void tryfix_event(TL_Evt_t *tev)
+{
+	struct bt_hci_evt_le_meta_event *mev = (void *)&tev->payload;
+
+	if (tev->evtcode != BT_HCI_EVT_LE_META_EVENT ||
+	    mev->subevent != BT_HCI_EVT_LE_ENH_CONN_COMPLETE) {
+		return;
+	}
+
+	struct bt_hci_evt_le_enh_conn_complete *evt =
+			(void *)((u8_t *)mev + (sizeof(*mev)));
+
+	if (!bt_addr_cmp(&evt->peer_addr.a, BT_ADDR_NONE)) {
+		BT_WARN("Invalid peer addr %s", bt_addr_le_str(&evt->peer_addr));
+		bt_addr_copy(&evt->peer_addr.a, &evt->peer_rpa);
+		evt->peer_addr.type = BT_ADDR_LE_RANDOM;
+	}
+}
+
 void TM_EvtReceivedCb(TL_EvtPacket_t *hcievt)
 {
 	struct net_buf *buf;
@@ -119,6 +148,7 @@ void TM_EvtReceivedCb(TL_EvtPacket_t *hcievt)
 					     K_FOREVER);
 			break;
 		}
+		tryfix_event(&hcievt->evtserial.evt);
 		net_buf_add_mem(buf, &hcievt->evtserial.evt,
 				hcievt->evtserial.evt.plen + 2);
 		break;