Support to BlueNRG_2 BLE component

connectivity/FEATURE_BLE/libraries/cordio_stack/ble-host/sources/hci/dual_chip/hci_evt.c

@@ -367,6 +367,30 @@ static void hciEvtParseLeConnCmpl(hciEvt_t *pMsg, uint8_t *p, uint8_t len)
   pMsg->hdr.status = pMsg->leConnCmpl.status;
 }
 
+#ifndef CORDIO_RPA_SWAP_WORKAROUND
+#define CORDIO_RPA_SWAP_WORKAROUND 0
+#endif
+#if CORDIO_RPA_SWAP_WORKAROUND
+/*************************************************************************************************/
+/*!
+ *  \brief  Check if address bits are all 0s or all 1s.
+ *
+ *  \param  p       Pointer to address.
+ *
+ *  \return TRUE if address is invalid.
+ */
+/*************************************************************************************************/
+static bool_t isAddressInvalid(uint8_t *p)
+{
+    for (int i = 0; i < 6; i++) {
+        if ((p[i] != 0xff) && (p[i] != 0x00)) {
+            return FALSE; // meaning valid address
+        }
+    }
+    return TRUE;
+}
+#endif // CORDIO_RPA_SWAP_WORKAROUND
+
 /*************************************************************************************************/
 /*!
  *  \brief  Parse an HCI event.
@@ -384,9 +408,21 @@ static void hciEvtParseLeEnhancedConnCmpl(hciEvt_t *pMsg, uint8_t *p, uint8_t le
   BSTREAM_TO_UINT16(pMsg->leConnCmpl.handle, p);
   BSTREAM_TO_UINT8(pMsg->leConnCmpl.role, p);
   BSTREAM_TO_UINT8(pMsg->leConnCmpl.addrType, p);
-  BSTREAM_TO_BDA(pMsg->leConnCmpl.peerAddr, p);
-  BSTREAM_TO_BDA(pMsg->leConnCmpl.localRpa, p);
-  BSTREAM_TO_BDA(pMsg->leConnCmpl.peerRpa, p);
+
+#if CORDIO_RPA_SWAP_WORKAROUND
+  if (isAddressInvalid(p)) {
+      memset(pMsg->leConnCmpl.peerRpa, 0x00, BDA_ADDR_LEN);
+      p += BDA_ADDR_LEN;
+      BSTREAM_TO_BDA(pMsg->leConnCmpl.localRpa, p);
+      BSTREAM_TO_BDA(pMsg->leConnCmpl.peerAddr, p);
+  } else
+#endif // CORDIO_RPA_SWAP_WORKAROUND
+  {
+      BSTREAM_TO_BDA(pMsg->leConnCmpl.peerAddr, p);
+      BSTREAM_TO_BDA(pMsg->leConnCmpl.localRpa, p);
+      BSTREAM_TO_BDA(pMsg->leConnCmpl.peerRpa, p);
+  }
+
   BSTREAM_TO_UINT16(pMsg->leConnCmpl.connInterval, p);
   BSTREAM_TO_UINT16(pMsg->leConnCmpl.connLatency, p);
   BSTREAM_TO_UINT16(pMsg->leConnCmpl.supTimeout, p);

connectivity/FEATURE_BLE/source/cordio/mbed_lib.json

@@ -76,6 +76,16 @@
             "help": "Maximum number of EATT channels per DM connection.",
             "value": 1,
             "macro_name": "EATT_CONN_CHAN_MAX"
+        },
+        "rpa-swap-workaround": {
+            "help": "Check connection complete event if it needs the addresses swapped due to bug in controller",
+            "value": null,
+            "macro_name": "CORDIO_RPA_SWAP_WORKAROUND"
+        }
+    },
+    "target_overrides": {
+        "NUCLEO_WB55RG": {
+            "rpa-swap-workaround": true
         }
     }
-}
+}

connectivity/FEATURE_BLE/source/cordio/source/PalGapImpl.h

@@ -401,30 +401,18 @@ class PalGap final : public ble::PalGap {
 
         static GapConnectionCompleteEvent convert(const hciLeConnCmplEvt_t *conn_evt)
         {
-            const bdAddr_t *peer_rpa = &conn_evt->peerRpa;
-            const bdAddr_t *peer_address = &conn_evt->peerAddr;
-
-#if defined(TARGET_MCU_STM32WB55xx)
-            const bdAddr_t invalidAddress = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
-            if (conn_evt->addrType == DM_ADDR_RANDOM &&
-                memcmp(peer_address, invalidAddress, sizeof(invalidAddress)) == 0 &&
-                memcmp(peer_rpa, invalidAddress, sizeof(invalidAddress) != 0)
-            ) {
-                std::swap(peer_rpa, peer_address);
-            }
-#endif
             return GapConnectionCompleteEvent(
                 conn_evt->status,
                 // note the usage of the stack handle, not the HCI handle
                 conn_evt->hdr.param,
                 (connection_role_t::type) conn_evt->role,
                 (peer_address_type_t::type) conn_evt->addrType,
-                *peer_address,
+                conn_evt->peerAddr,
                 conn_evt->connInterval,
                 conn_evt->connLatency,
                 conn_evt->supTimeout,
                 conn_evt->localRpa,
-                *peer_rpa
+                conn_evt->peerRpa
             );
         }
     };

connectivity/drivers/ble/FEATURE_BLE/CMakeLists.txt

@@ -1,6 +1,10 @@
 # Copyright (c) 2020 ARM Limited. All rights reserved.
 # SPDX-License-Identifier: Apache-2.0
 
+if("BlueNRG_2" IN_LIST MBED_TARGET_LABELS)
+    add_subdirectory(COMPONENT_BlueNRG_2)
+endif()
+
 if("BlueNRG_MS" IN_LIST MBED_TARGET_LABELS)
     add_subdirectory(COMPONENT_BlueNRG_MS)
 endif()