Merge pull request #8998 from pan-/ble-extended-advertising-fixes

Ble extended advertising fixes

features/FEATURE_BLE/ble/gap/Events.h

@@ -63,7 +63,7 @@ struct AdvertisingReportEvent {
         advertising_sid_t SID,
         advertising_power_t txPower,
         rssi_t rssi,
-        periodic_interval_t periodicInterval,
+        uint16_t periodicInterval,
         const peer_address_type_t &directAddressType,
         const address_t &directAddress,
         const mbed::Span<const uint8_t> &advertisingData
@@ -133,10 +133,15 @@ struct AdvertisingReportEvent {
         return rssi;
     }
 
+    /** Indicate if periodic interval is valid */
+    bool isPeriodicIntervalPresent() const {
+        return periodicInterval != 0;
+    }
+
     /** Get interval. */
     periodic_interval_t getPeriodicInterval() const
     {
-        return periodicInterval;
+        return periodic_interval_t(periodicInterval);
     }
 
     /** Get target address type in directed advertising. */
@@ -166,7 +171,7 @@ private:
     advertising_sid_t SID;
     advertising_power_t txPower;
     rssi_t rssi;
-    periodic_interval_t periodicInterval;
+    uint16_t periodicInterval;
     peer_address_type_t directAddressType;
     const address_t &directAddress;
     mbed::Span<const uint8_t> advertisingData;

features/FEATURE_BLE/ble/gap/Types.h

@@ -229,7 +229,7 @@ struct advertising_data_status_t : SafeEnum<advertising_data_status_t, uint8_t>
      * Explicit constructor from a raw value.
      */
     explicit advertising_data_status_t(uint8_t raw_value) :
-        SafeEnum(static_cast<advertising_data_status_t>(raw_value))
+        SafeEnum(raw_value)
     {
     }
 

features/FEATURE_BLE/source/BLE.cpp

@@ -142,7 +142,8 @@ BLE::initImplementation(FunctionPointerWithContext<InitializationCompleteCallbac
 
 // this stub is required by ARMCC otherwise link will systematically fail
 MBED_WEAK BLEInstanceBase* createBLEInstance() {
-    MBED_ERROR(MBED_MAKE_ERROR(MBED_MODULE_BLE, MBED_ERROR_CODE_BLE_BACKEND_CREATION_FAILED), "Please provide an implementation for mbed BLE");
+    MBED_ASSERT("No BLE instance implementation.");
+    printf("Please provide an implementation for mbed BLE");
     return NULL;
 }
 

features/FEATURE_BLE/source/generic/GenericGap.cpp

@@ -1589,7 +1589,7 @@ void GenericGap::on_advertising_report(const pal::GapAdvertisingReportEvent &e)
                     /* SID - NO ADI FIELD IN THE PDU */ 0xFF,
                     /* tx power information not available */ 127,
                     advertising.rssi,
-                    /* NO PERIODIC ADVERTISING */ periodic_interval_t(0),
+                    /* NO PERIODIC ADVERTISING */ 0,
                     peer_address_type_t::ANONYMOUS,
                     ble::address_t (),
                     mbed::Span<const uint8_t>(advertising.data.data(), advertising.data.size())
@@ -2141,7 +2141,7 @@ ble_error_t GenericGap::setExtendedAdvertisingParameters(
         params.getChannel39()
     );
 
-    return _pal_gap.set_extended_advertising_parameters(
+    ble_error_t err = _pal_gap.set_extended_advertising_parameters(
         handle,
         event_properties,
         params.getMinPrimaryInterval().value(),
@@ -2158,6 +2158,15 @@ ble_error_t GenericGap::setExtendedAdvertisingParameters(
         /* SID */ (handle % 0x10),
         params.getScanRequestNotification()
     );
+
+    if (err) {
+        return err;
+    }
+
+    return _pal_gap.set_advertising_set_random_address(
+        handle,
+        _random_static_identity_address
+    );
 }
 
 ble_error_t GenericGap::setAdvertisingPayload(
@@ -2305,21 +2314,6 @@ ble_error_t GenericGap::startAdvertising(
     }
 
     if (is_extended_advertising_available()) {
-        ble::address_t random_address;
-
-        if (!getUnresolvableRandomAddress(random_address)) {
-            return BLE_ERROR_INTERNAL_STACK_FAILURE;
-        }
-
-        error = _pal_gap.set_advertising_set_random_address(
-            handle,
-            random_address
-        );
-
-        if (error) {
-            return error;
-        }
-
         error = _pal_gap.extended_advertising_enable(
             /* enable */ true,
             /* number of advertising sets */ 1,
@@ -2648,7 +2642,7 @@ void GenericGap::on_extended_advertising_report(
             advertising_sid,
             tx_power,
             rssi,
-            periodic_interval_t(periodic_advertising_interval),
+            periodic_advertising_interval,
             (PeerAddressType_t::type) direct_address_type.value(),
             (BLEProtocol::AddressBytes_t &) direct_address,
             mbed::make_Span(data, data_length)
@@ -2927,7 +2921,7 @@ ble_error_t GenericGap::createSync(
         return BLE_ERROR_NOT_IMPLEMENTED;
     }
 
-    if (peerAddressType != peer_address_type_t::PUBLIC ||
+    if (peerAddressType != peer_address_type_t::PUBLIC &&
         peerAddressType != peer_address_type_t::RANDOM
     ) {
         return BLE_ERROR_INVALID_PARAM;

features/FEATURE_BLE/targets/TARGET_CORDIO/driver/CordioHCIDriver.cpp

@@ -17,6 +17,7 @@
 #include <stddef.h>
 #include <string.h>
 
+#include "CordioBLE.h"
 #include "CordioHCIDriver.h"
 #include "hci_api.h"
 #include "hci_cmd.h"
@@ -139,10 +140,26 @@ void CordioHCIDriver::handle_reset_sequence(uint8_t *pMsg)
                 HciReadBdAddrCmd();
                 break;
 
-            case HCI_OPCODE_READ_BD_ADDR:
+            case HCI_OPCODE_READ_BD_ADDR: {
                 /* parse and store event parameters */
                 BdaCpy(hciCoreCb.bdAddr, pMsg);
 
+                ble::address_t static_address;
+
+                if (get_random_static_address(static_address)) {
+                    // note: will send the HCI command to send the random address
+                    cordio::BLE::deviceInstance().getGap().setAddress(
+                        BLEProtocol::AddressType::RANDOM_STATIC,
+                        static_address.data()
+                    );
+                } else {
+                    /* send next command in sequence */
+                    HciLeReadBufSizeCmd();
+                }
+                break;
+            }
+
+            case HCI_OPCODE_LE_SET_RAND_ADDR:
                 /* send next command in sequence */
                 HciLeReadBufSizeCmd();
                 break;
@@ -246,6 +263,11 @@ void CordioHCIDriver::handle_reset_sequence(uint8_t *pMsg)
     }
 }
 
+bool CordioHCIDriver::get_random_static_address(ble::address_t& address)
+{
+    return false;
+}
+
 void CordioHCIDriver::signal_reset_sequence_done()
 {
     hci_mbed_os_signal_reset_sequence_done();

features/FEATURE_BLE/targets/TARGET_CORDIO/driver/CordioHCIDriver.h

@@ -19,6 +19,7 @@
 
 #include <stddef.h>
 #include <stdint.h>
+#include <BLETypes.h>
 #include "wsf_buf.h"
 #include "CordioHCITransportDriver.h"
 
@@ -108,6 +109,13 @@ public:
      */
     virtual void handle_reset_sequence(uint8_t *msg);
 
+    /**
+     * Get the random static address of the controller
+     *
+     * @return false if the address has not been set and true otherwise.
+     */
+    virtual bool get_random_static_address(ble::address_t& address);
+
     /**
      * Signal to the stack that the reset sequence has been done.
      */

features/FEATURE_BLE/targets/TARGET_CORDIO/source/CordioBLE.cpp

@@ -48,7 +48,8 @@ wsfHandlerId_t stack_handler_id;
  */
 MBED_WEAK ble::vendor::cordio::CordioHCIDriver& ble_cordio_get_hci_driver()
 {
-    error("Please provide an implementation for the HCI driver");
+    MBED_ASSERT("No HCI driver");
+    printf("Please provide an implementation for the HCI driver");
     ble::vendor::cordio::CordioHCIDriver* bad_instance = NULL;
     return *bad_instance;
 }
@@ -100,7 +101,7 @@ BLE::BLE(CordioHCIDriver& hci_driver) :
     _last_update_us(0)
 {
     _hci_driver = &hci_driver;
-
+    stack_setup();
 }
 
 BLE::~BLE() { }
@@ -400,7 +401,6 @@ void BLE::stack_setup()
 void BLE::start_stack_reset()
 {
     _hci_driver->initialize();
-    stack_setup();
     DmDevReset();
 }
 

features/FEATURE_BLE/targets/TARGET_CORDIO/source/CordioPalGap.cpp

@@ -670,6 +670,76 @@ ble_error_t Gap::set_extended_advertising_parameters(
     bool scan_request_notification
 )
 {
+    uint8_t adv_type;
+
+    if (event_properties.use_legacy_pdu) {
+        if (event_properties.directed == false) {
+            if (event_properties.high_duty_cycle) {
+                return BLE_ERROR_INVALID_PARAM;
+            }
+
+            if (event_properties.connectable && event_properties.scannable == false) {
+                return BLE_ERROR_INVALID_PARAM;
+            }
+
+            if (event_properties.connectable && event_properties.scannable) {
+                adv_type = DM_ADV_CONN_UNDIRECT;
+            } else if (event_properties.scannable) {
+                adv_type = DM_ADV_SCAN_UNDIRECT;
+            } else  {
+                adv_type = DM_ADV_NONCONN_UNDIRECT;
+            }
+        } else {
+            if (event_properties.scannable) {
+                return BLE_ERROR_INVALID_PARAM;
+            }
+
+            if (event_properties.connectable == false) {
+                return BLE_ERROR_INVALID_PARAM;
+            }
+
+            if (event_properties.high_duty_cycle) {
+                adv_type = DM_ADV_CONN_DIRECT;
+            } else {
+                adv_type = DM_ADV_CONN_DIRECT_LO_DUTY;
+            }
+        }
+    } else {
+        if (event_properties.directed == false) {
+            if (event_properties.high_duty_cycle) {
+                return BLE_ERROR_INVALID_PARAM;
+            }
+
+            if (event_properties.connectable && event_properties.scannable) {
+                adv_type = DM_ADV_CONN_UNDIRECT;
+            } else if (event_properties.scannable) {
+                adv_type = DM_ADV_SCAN_UNDIRECT;
+            } else if (event_properties.connectable) {
+                adv_type = DM_EXT_ADV_CONN_UNDIRECT;
+            } else  {
+                adv_type = DM_ADV_NONCONN_UNDIRECT;
+            }
+        } else {
+            // note: not sure how to act with the high duty cycle in scannable
+            // and non connectable mode. These cases looks correct from a Bluetooth
+            // standpoint
+
+            if (event_properties.connectable && event_properties.scannable) {
+                return BLE_ERROR_INVALID_PARAM;
+            } else if (event_properties.connectable) {
+                if (event_properties.high_duty_cycle) {
+                    adv_type = DM_ADV_CONN_DIRECT;
+                } else {
+                    adv_type = DM_ADV_CONN_DIRECT_LO_DUTY;
+                }
+            } else if (event_properties.scannable) {
+                adv_type = DM_EXT_ADV_SCAN_DIRECT;
+            } else {
+                adv_type = DM_EXT_ADV_NONCONN_DIRECT;
+            }
+        }
+    }
+
     DmAdvSetInterval(
         advertising_handle,
         primary_advertising_interval_min,
@@ -709,7 +779,7 @@ ble_error_t Gap::set_extended_advertising_parameters(
 
     DmAdvConfig(
         advertising_handle,
-        event_properties.value(), // TODO: use the raw value here ???
+        adv_type,
         peer_address_type.value(),
         const_cast<uint8_t *>(peer_address.data())
     );
@@ -853,7 +923,7 @@ uint16_t Gap::get_maximum_advertising_data_length()
 
 uint8_t Gap::get_max_number_of_advertising_sets()
 {
-    return HciGetNumSupAdvSets();
+    return std::min(HciGetNumSupAdvSets(), (uint8_t) DM_NUM_ADV_SETS);
 }
 
 ble_error_t Gap::remove_advertising_set(advertising_handle_t advertising_handle)
@@ -911,12 +981,11 @@ ble_error_t Gap::extended_scan_enable(
     if (enable) {
         uint32_t duration_ms = duration * 10;
 
-        DmScanModeExt();
         DmScanStart(
             scanning_phys.value(),
             DM_DISC_MODE_NONE,
             extended_scan_type,
-            filter_duplicates.value(), // TODO: cordio API incomplete ???
+            filter_duplicates.value(),
             duration_ms > 0xFFFF ? 0xFFFF : duration_ms,
             period
         );
@@ -936,15 +1005,13 @@ ble_error_t Gap::periodic_advertising_create_sync(
     uint16_t sync_timeout
 )
 {
-    if (use_periodic_advertiser_list) {
-        DmDevSetExtFilterPolicy(
-            DM_ADV_HANDLE_DEFAULT,
-            DM_FILT_POLICY_MODE_SYNC,
-            HCI_FILT_PER_ADV_LIST
-        );
-    }
+    DmDevSetExtFilterPolicy(
+        DM_ADV_HANDLE_DEFAULT,
+        DM_FILT_POLICY_MODE_SYNC,
+        use_periodic_advertiser_list ? HCI_FILT_PER_ADV_LIST : HCI_FILT_NONE
+    );
 
-    DmSyncStart(
+    dmSyncId_t sync_id = DmSyncStart(
         advertising_sid,
         peer_address_type.value(),
         peer_address.data(),
@@ -952,7 +1019,11 @@ ble_error_t Gap::periodic_advertising_create_sync(
         sync_timeout
     );
 
-    return BLE_ERROR_INVALID_PARAM;
+    if (sync_id == DM_SYNC_ID_NONE) {
+        return BLE_ERROR_INTERNAL_STACK_FAILURE;
+    } else {
+        return BLE_ERROR_NONE;
+    }
 }
 
 ble_error_t Gap::cancel_periodic_advertising_create_sync()

features/FEATURE_BLE/targets/TARGET_CORDIO/stack/cordio_stack/ble-host/sources/stack/cfg/cfg_stack.h

@@ -86,13 +86,13 @@ extern "C" {
 
 /*! \brief Number of supported advertising sets: must be set to 1 for legacy advertising */
 #ifndef DM_NUM_ADV_SETS
-#define DM_NUM_ADV_SETS          1
+#define DM_NUM_ADV_SETS          3
 #endif
 
 /*! \brief Number of scanner and initiator PHYs (LE 1M, LE 2M and LE Coded): must be set to 1 for
     legacy scanner and initiator */
 #ifndef DM_NUM_PHYS
-#define DM_NUM_PHYS              1
+#define DM_NUM_PHYS              3
 #endif
 /**@}*/
 

features/FEATURE_BLE/targets/TARGET_CORDIO/stack/cordio_stack/ble-host/sources/stack/dm/dm_adv.h

@@ -151,7 +151,7 @@ typedef struct
   uint8_t                 advHandle;
   uint8_t                 op;
   uint8_t                 len;
-  uint8_t                 *pData;
+  uint8_t                 pData[];
 } dmAdvPerApiSetData_t;
 
 /* Data structure for DM_ADV_PER_MSG_API_START */

features/FEATURE_BLE/targets/TARGET_CORDIO/stack/cordio_stack/ble-host/sources/stack/dm/dm_adv_ae.c

@@ -1634,13 +1634,13 @@ void DmPerAdvSetData(uint8_t advHandle, uint8_t op, uint8_t len, uint8_t *pData)
   WSF_ASSERT(advHandle < DM_NUM_ADV_SETS);
   WSF_ASSERT(len <= HCI_PER_ADV_DATA_LEN);
 
-  if ((pMsg = WsfMsgAlloc(sizeof(dmAdvPerApiSetData_t))) != NULL)
+  if ((pMsg = WsfMsgAlloc(sizeof(dmAdvPerApiSetData_t) + len)) != NULL)
   {
     pMsg->hdr.event = DM_ADV_PER_MSG_API_SET_DATA;
     pMsg->advHandle = advHandle;
     pMsg->op = op;
     pMsg->len = len;
-    pMsg->pData = pData;
+    memcpy(pMsg->pData, pData, len);
     WsfMsgSend(dmCb.handlerId, pMsg);
   }
 }

features/FEATURE_BLE/targets/TARGET_NORDIC/TARGET_NORDIC_CORDIO/TARGET_NRF5x/NRFCordioHCIDriver.cpp

@@ -313,6 +313,21 @@ void NRFCordioHCIDriver::start_reset_sequence()
     CordioHCIDriver::start_reset_sequence();
 }
 
+bool NRFCordioHCIDriver::get_random_static_address(ble::address_t& address)
+{
+    /* Load address from nRF configuration. */
+    uint64_t devAddr = (((uint64_t)NRF_FICR->DEVICEADDR[0]) <<  0) |
+                       (((uint64_t)NRF_FICR->DEVICEADDR[1]) << 32);
+
+    for (size_t i = 0; i < address.size(); ++i) {
+        address[i] = devAddr >> (i * 8);
+    }
+
+    address[5] |= 0xC0;     /* cf. "Static Address" (Vol C, Part 3, section 10.8.1) */
+    return true;
+}
+
+
 ble::vendor::cordio::CordioHCIDriver& ble_cordio_get_hci_driver() { 
     static NRFCordioHCITransportDriver transport_driver;
 
@@ -344,6 +359,4 @@ void PlatformLoadBdAddress(uint8_t *pDevAddr)
     pDevAddr[i] = devAddr >> (i * 8);
     i++;
   }
-
-  pDevAddr[5] |= 0xC0;     /* cf. "Static Address" (Vol C, Part 3, section 10.8.1) */
 }

features/FEATURE_BLE/targets/TARGET_NORDIC/TARGET_NORDIC_CORDIO/TARGET_NRF5x/NRFCordioHCIDriver.h

@@ -59,6 +59,11 @@ public:
      */
     //virtual void handle_reset_sequence(uint8_t *msg);
 
+    /**
+     * @copydoc cordio::CordioHCIDriver::get_random_static_address
+     */
+    virtual bool get_random_static_address(ble::address_t& address);
+
 private:
     /**
      * Initialize the chip.

features/FEATURE_BLE/targets/TARGET_NORDIC/TARGET_NORDIC_CORDIO/TARGET_NRF5x/mbed_lib.json

@@ -1,6 +1,6 @@
 {
     "name": "cordio-nordic-ll",
     "macros": [
-        "INIT_BROADCASTER", "INIT_OBSERVER", "INIT_CENTRAL", "INIT_PERIPHERAL", "INIT_ENCRYPTED", "LHCI_ENABLE_VS=0", "BB_CLK_RATE_HZ=32768"
+        "INIT_BROADCASTER", "INIT_OBSERVER", "INIT_CENTRAL", "INIT_PERIPHERAL", "INIT_ENCRYPTED", "LHCI_ENABLE_VS=0", "BB_CLK_RATE_HZ=1000000"
     ]
 }