From ab123d3e222384c7a316c4b83ef805ae42efb72c Mon Sep 17 00:00:00 2001
From: Paul Szczepanek <paul.szczepanek@arm.com>
Date: Mon, 17 May 2021 14:23:55 +0100
Subject: [PATCH 01/11] advertising start queued up waiting for completion

---
 connectivity/FEATURE_BLE/include/ble/Gap.h    |  20 +-
 .../FEATURE_BLE/include/ble/gap/Events.h      |  38 ++++
 .../FEATURE_BLE/source/generic/GapImpl.cpp    | 195 +++++++++++++++---
 .../FEATURE_BLE/source/generic/GapImpl.h      |  26 +++
 4 files changed, 250 insertions(+), 29 deletions(-)

diff --git a/connectivity/FEATURE_BLE/include/ble/Gap.h b/connectivity/FEATURE_BLE/include/ble/Gap.h
index 067895d628..6d3f529668 100644
--- a/connectivity/FEATURE_BLE/include/ble/Gap.h
+++ b/connectivity/FEATURE_BLE/include/ble/Gap.h
@@ -341,6 +341,18 @@ public:
         {
         }
 
+        /**
+         * Called when an asynchronous advertising command fails.
+         *
+         * @param event Advertising command failed event.
+         *
+         * @see startAdvertising()
+         * @see stopAdvertising()
+         */
+        virtual void onAdvertisingCommandFailed(const AdvertisingCommandFailedEvent &event)
+        {
+        }
+
         /**
          * Called when a scanner receives an advertising or a scan response packet.
          *
@@ -747,7 +759,9 @@ public:
      * @param handle Advertising set handle.
      * @param maxDuration Max duration for advertising (in units of 10ms) - 0 means no limit.
      * @param maxEvents Max number of events produced during advertising - 0 means no limit.
-     * @return BLE_ERROR_NONE on success.
+     * @return BLE_ERROR_NONE on success. This does not guarantee the set has started if
+     * extended advertising is enabled. Register an event handler and wait for onAdvertisingStart
+     * event. An (unlikely) failed start will emit an onAdvertisingCommandFailed instead.
      *
      * @see EventHandler::onAdvertisingStart when the advertising starts.
      * @see EventHandler::onScanRequestReceived when a scan request is received.
@@ -765,7 +779,9 @@ public:
      *  which will not be affected.
      *
      * @param handle Advertising set handle.
-     * @return BLE_ERROR_NONE on success.
+     * @return BLE_ERROR_NONE on success. For extented advertising this does not guarantee
+     * the set is stopped if. Register an event handler and wait for onAdvertisingEnd event.
+     * An (unlikely) failed stop will emit an onAdvertisingCommandFailed instead.
      */
     ble_error_t stopAdvertising(advertising_handle_t handle);
 
diff --git a/connectivity/FEATURE_BLE/include/ble/gap/Events.h b/connectivity/FEATURE_BLE/include/ble/gap/Events.h
index a181f7b762..5e1c817357 100644
--- a/connectivity/FEATURE_BLE/include/ble/gap/Events.h
+++ b/connectivity/FEATURE_BLE/include/ble/gap/Events.h
@@ -604,6 +604,44 @@ private:
     advertising_handle_t advHandle;
 };
 
+/**
+ * Event produced when an async advertising command fails.
+ *
+ * @see ble::Gap::EventHandler::onAdvertisingCommandFailed().
+ */
+struct AdvertisingCommandFailedEvent {
+#if !defined(DOXYGEN_ONLY)
+    /** Create an extended advertising command failed event.
+     *
+     * @param advHandle Advertising set handle.
+     * @param status Error code.
+     */
+    AdvertisingCommandFailedEvent(
+        advertising_handle_t advHandle,
+        ble_error_t status
+    ) :
+        advHandle(advHandle),
+        status(status)
+    {
+    }
+
+#endif
+    /** Get advertising handle. */
+    advertising_handle_t getAdvHandle() const
+    {
+        return advHandle;
+    }
+
+    /** Error code that caused the event. */
+    uint8_t getStatus() const
+    {
+        return status;
+    }
+private:
+    advertising_handle_t advHandle;
+    ble_error_t status;
+};
+
 /**
  * Event produced when advertising ends.
  *
diff --git a/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp b/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
index 5296ae690f..d6fcc12f6d 100644
--- a/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
+++ b/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
@@ -373,6 +373,11 @@ Gap::Gap(
     , _scan_parameters_set(false)
 #endif // BLE_ROLE_OBSERVER
 {
+#if BLE_FEATURE_EXTENDED_ADVERTISING
+    _advertising_enable_queue.handle = ble::INVALID_ADVERTISING_HANDLE;
+    _advertising_enable_queue.next = nullptr;
+    _advertising_enable_pending = false;
+#endif //BLE_FEATURE_EXTENDED_ADVERTISING
     _pal_gap.initialize();
 
     _pal_gap.when_gap_event_received(
@@ -1214,6 +1219,21 @@ ble_error_t Gap::reset()
     _initiating = false;
     set_scan_state(ScanState::idle);
     _scan_requested = false;
+
+#if BLE_FEATURE_EXTENDED_ADVERTISING
+    /* reset pending advertising sets */
+    AdvertisingEnableStackNode_t* next = _advertising_enable_queue.next;
+    _advertising_enable_queue.next = nullptr;
+    _advertising_enable_queue.handle = ble::INVALID_ADVERTISING_HANDLE;
+    _advertising_enable_pending = false;
+    /* free any allocated nodes */
+    while (next) {
+        AdvertisingEnableStackNode_t* node_to_free = next;
+        AdvertisingEnableStackNode_t* next = next->next;
+        delete node_to_free;
+    }
+#endif // BLE_FEATURE_EXTENDED_ADVERTISING
+
 #if BLE_FEATURE_PRIVACY
     _privacy_initialization_pending = false;
 #if BLE_GAP_HOST_BASED_PRIVATE_ADDRESS_RESOLUTION
@@ -1268,6 +1288,7 @@ ble_error_t Gap::reset()
 #endif // BLE_FEATURE_EXTENDED_ADVERTISING
 
     _active_sets.clear();
+    _pending_stop_sets.clear();
     _pending_sets.clear();
     _address_refresh_sets.clear();
     _interruptible_sets.clear();
@@ -2413,23 +2434,10 @@ ble_error_t Gap::startAdvertising(
             _pal_gap.set_advertising_set_random_address(handle, *random_address);
         }
 
-        error = _pal_gap.extended_advertising_enable(
-            /* enable */ true,
-            /* number of advertising sets */ 1,
-            &handle,
-            maxDuration.storage(),
-            &maxEvents
-        );
-
-        if (error) {
-            return error;
-        }
-
-        if (maxDuration.value() || maxEvents) {
-            _interruptible_sets.clear(handle);
-        } else {
-            _interruptible_sets.set(handle);
-        }
+        _event_queue.post([this, handle, maxDuration, maxEvents] {
+            start_advertising_enable(handle, maxDuration, maxEvents);
+            evaluate_advertising_enable_queue();
+        });
 
     } else
 #endif // BLE_FEATURE_EXTENDED_ADVERTISING
@@ -2469,6 +2477,93 @@ ble_error_t Gap::startAdvertising(
 }
 #endif
 
+#if BLE_FEATURE_EXTENDED_ADVERTISING
+void Gap::start_advertising_enable(
+    advertising_handle_t handle,
+    adv_duration_t maxDuration,
+    uint8_t maxEvents
+)
+{
+    /* remember the parameters that will be enabled when the last command completes */
+    if (_advertising_enable_queue.handle == ble::INVALID_ADVERTISING_HANDLE) {
+        /* special case when there is only one pending */
+        _advertising_enable_queue.handle = handle;
+        _advertising_enable_queue.max_duration = maxDuration;
+        _advertising_enable_queue.max_events = maxEvents;
+        _advertising_enable_queue.next = nullptr;
+    } else {
+        AdvertisingEnableStackNode_t** next = &_advertising_enable_queue.next;
+        /* move down the queue until we're over a nullptr */
+        if (*next) {
+            while ((*next)->next) {
+                next = &((*next)->next);
+            }
+            next = &(*next)->next;
+        }
+        *next = new AdvertisingEnableStackNode_t();
+        if (!*next) {
+            tr_error("Out of memory creating pending advertising enable node for handle %d", handle);
+            return;
+        }
+        (*next)->handle = handle;
+        (*next)->max_duration = maxDuration;
+        (*next)->max_events = maxEvents;
+        (*next)->next = nullptr;
+    }
+}
+
+void Gap::evaluate_advertising_enable_queue()
+{
+    tr_info("Evaluating pending advertising sets to be started");
+    if (_advertising_enable_pending) {
+        return;
+    }
+
+    if (_advertising_enable_queue.handle == ble::INVALID_ADVERTISING_HANDLE) {
+        /* no set pending to be enabled*/
+        return;
+    }
+
+    ble_error_t error = _pal_gap.extended_advertising_enable(
+        /* enable */ true,
+        /* number of advertising sets */ 1,
+        &_advertising_enable_queue.handle,
+        _advertising_enable_queue.max_duration.storage(),
+        &_advertising_enable_queue.max_events
+    );
+
+    if (error) {
+        tr_error("Failed to start advertising set with error: %s", to_string(error));
+        if (_event_handler) {
+            _event_handler->onAdvertisingCommandFailed(
+                AdvertisingCommandFailedEvent(
+                    _advertising_enable_queue.handle,
+                    error
+                )
+            );
+        }
+    } else {
+        _advertising_enable_pending = true;
+        if (_advertising_enable_queue.max_duration.value() || _advertising_enable_queue.max_events) {
+            _interruptible_sets.clear(_advertising_enable_queue.handle);
+        } else {
+            _interruptible_sets.set(_advertising_enable_queue.handle);
+        }
+    }
+
+    /* if there's anything else waiting, queue it up, otherwise mark the head node handle as invalid */
+    if (_advertising_enable_queue.next) {
+        AdvertisingEnableStackNode_t* next = _advertising_enable_queue.next;
+        _advertising_enable_queue.handle = next->handle;
+        _advertising_enable_queue.max_duration = next->max_duration;
+        _advertising_enable_queue.max_events = next->max_events;
+        _advertising_enable_queue.next = next->next;
+        delete next;
+    } else {
+        _advertising_enable_queue.handle = ble::INVALID_ADVERTISING_HANDLE;
+    }
+}
+#endif //BLE_FEATURE_EXTENDED_ADVERTISING
 
 #if BLE_ROLE_BROADCASTER
 ble_error_t Gap::stopAdvertising(advertising_handle_t handle)
@@ -2499,17 +2594,23 @@ ble_error_t Gap::stopAdvertising(advertising_handle_t handle)
 
 #if BLE_FEATURE_EXTENDED_ADVERTISING
     if (is_extended_advertising_available()) {
-        status = _pal_gap.extended_advertising_enable(
-            /*enable ? */ false,
-            /* number of advertising sets */ 1,
-            &handle,
-            nullptr,
-            nullptr
-        );
+        _event_queue.post([this, handle] {
+            /* if any already pending to stop delay the command execution */
+            bool delay = false;
+            for (size_t i = 0; i < BLE_GAP_MAX_ADVERTISING_SETS; ++i) {
+                if (_pending_stop_sets.get(i)) {
+                    delay = true;
+                    break;
+                }
+            }
+            _pending_stop_sets.set(handle);
+            if (!delay) {
+                evaluate_advertising_stop();
+            }
+        });
+
+        return BLE_ERROR_NONE;
 
-        if (status) {
-            return status;
-        }
     } else
 #endif // BLE_FEATURE_EXTENDED_ADVERTISING
     {
@@ -2531,6 +2632,36 @@ ble_error_t Gap::stopAdvertising(advertising_handle_t handle)
 
     return status;
 }
+
+#if BLE_FEATURE_EXTENDED_ADVERTISING
+void Gap::evaluate_advertising_stop()
+{
+    // refresh for address for all connectable advertising sets
+    for (size_t i = 0; i < BLE_GAP_MAX_ADVERTISING_SETS; ++i) {
+        if (_pending_stop_sets.get(i)) {
+            ble_error_t status = _pal_gap.extended_advertising_enable(
+                /* enable */ false,
+                /* number of advertising sets */ 1,
+                (advertising_handle_t*)&i,
+                nullptr,
+                nullptr
+            );
+            if (status) {
+                _event_handler->onAdvertisingCommandFailed(
+                    AdvertisingCommandFailedEvent(
+                        (advertising_handle_t)i,
+                        status
+                    )
+                );
+                tr_error("Could not stop advertising set %u, error: %s", i, to_string(status));
+            }
+            break;
+        }
+    }
+
+}
+#endif // BLE_FEATURE_EXTENDED_ADVERTISING
+
 #endif
 
 
@@ -3354,6 +3485,11 @@ void Gap::on_advertising_set_started(const mbed::Span<const uint8_t>& handles)
 {
     tr_info("Advertising set started - handles=%s", mbed_trace_array(handles.data(), handles.size()));
 
+    _event_queue.post([this] {
+        _advertising_enable_pending = false;
+        evaluate_advertising_enable_queue();
+    });
+
     for (const auto &handle : handles) {
         _active_sets.set(handle);
         _pending_sets.clear(handle);
@@ -3394,6 +3530,11 @@ void Gap::on_advertising_set_terminated(
         return;
     }
 
+    _event_queue.post([this, advertising_handle] {
+        _pending_stop_sets.clear(advertising_handle);
+        evaluate_advertising_stop();
+    });
+
     if (!_event_handler) {
         return;
     }
diff --git a/connectivity/FEATURE_BLE/source/generic/GapImpl.h b/connectivity/FEATURE_BLE/source/generic/GapImpl.h
index d006738975..8c048d5d26 100644
--- a/connectivity/FEATURE_BLE/source/generic/GapImpl.h
+++ b/connectivity/FEATURE_BLE/source/generic/GapImpl.h
@@ -562,6 +562,17 @@ private:
     ~Gap();
 
 #if BLE_ROLE_BROADCASTER
+#if BLE_FEATURE_EXTENDED_ADVERTISING
+    void start_advertising_enable(
+        advertising_handle_t handle,
+        adv_duration_t maxDuration,
+        uint8_t maxEvents
+    );
+
+    void evaluate_advertising_enable_queue();
+    void evaluate_advertising_stop();
+#endif // BLE_FEATURE_EXTENDED_ADVERTISING
+
     ble_error_t setAdvertisingData(
         advertising_handle_t handle,
         Span<const uint8_t> payload,
@@ -980,6 +991,9 @@ private:
     };
 
     BitArray<BLE_GAP_MAX_ADVERTISING_SETS> _existing_sets;
+#if BLE_FEATURE_EXTENDED_ADVERTISING
+    BitArray<BLE_GAP_MAX_ADVERTISING_SETS> _pending_stop_sets;
+#endif // BLE_FEATURE_EXTENDED_ADVERTISING
     BitArray<BLE_GAP_MAX_ADVERTISING_SETS> _active_sets;
     BitArray<BLE_GAP_MAX_ADVERTISING_SETS> _active_periodic_sets;
     BitArray<BLE_GAP_MAX_ADVERTISING_SETS> _connectable_payload_size_exceeded;
@@ -989,6 +1003,18 @@ private:
     BitArray<BLE_GAP_MAX_ADVERTISING_SETS> _interruptible_sets;
     BitArray<BLE_GAP_MAX_ADVERTISING_SETS> _adv_started_from_refresh;
 
+#if BLE_FEATURE_EXTENDED_ADVERTISING
+    struct AdvertisingEnableStackNode_t {
+        adv_duration_t max_duration;
+        advertising_handle_t handle;
+        uint8_t max_events;
+        AdvertisingEnableStackNode_t* next;
+    };
+
+    /* to simplify code and avoid allocation unless multiple requests issued we keep one node as member */
+    AdvertisingEnableStackNode_t _advertising_enable_queue;
+    bool _advertising_enable_pending;
+#endif // BLE_FEATURE_EXTENDED_ADVERTISING
 
     bool _user_manage_connection_parameter_requests;
 #if BLE_ROLE_OBSERVER

From 30cd6071cf1c27895a7bb3b1fa5fb36485536556 Mon Sep 17 00:00:00 2001
From: Paul Szczepanek <paul.szczepanek@arm.com>
Date: Tue, 18 May 2021 10:24:16 +0100
Subject: [PATCH 02/11] remove stopping sets since controller resets them

---
 .../FEATURE_BLE/source/generic/GapImpl.cpp    | 65 +++++--------------
 1 file changed, 16 insertions(+), 49 deletions(-)

diff --git a/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp b/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
index d6fcc12f6d..636b910682 100644
--- a/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
+++ b/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
@@ -1220,20 +1220,6 @@ ble_error_t Gap::reset()
     set_scan_state(ScanState::idle);
     _scan_requested = false;
 
-#if BLE_FEATURE_EXTENDED_ADVERTISING
-    /* reset pending advertising sets */
-    AdvertisingEnableStackNode_t* next = _advertising_enable_queue.next;
-    _advertising_enable_queue.next = nullptr;
-    _advertising_enable_queue.handle = ble::INVALID_ADVERTISING_HANDLE;
-    _advertising_enable_pending = false;
-    /* free any allocated nodes */
-    while (next) {
-        AdvertisingEnableStackNode_t* node_to_free = next;
-        AdvertisingEnableStackNode_t* next = next->next;
-        delete node_to_free;
-    }
-#endif // BLE_FEATURE_EXTENDED_ADVERTISING
-
 #if BLE_FEATURE_PRIVACY
     _privacy_initialization_pending = false;
 #if BLE_GAP_HOST_BASED_PRIVATE_ADDRESS_RESOLUTION
@@ -1249,44 +1235,25 @@ ble_error_t Gap::reset()
 #endif
 
 #if BLE_ROLE_BROADCASTER
+    /* clear advertising set data on the controller */
+    _pal_gap.clear_advertising_sets();
 #if BLE_FEATURE_EXTENDED_ADVERTISING
-    if (is_extended_advertising_available()) {
-        /* stop all advertising sets */
-        for (size_t i = 0; i < BLE_GAP_MAX_ADVERTISING_SETS; ++i) {
-            if (_active_sets.get(i)) {
-                _pal_gap.extended_advertising_enable(
-                    /* enable */ false,
-                    /* number of advertising sets */ 1,
-                    (advertising_handle_t *) &i,
-                    nullptr,
-                    nullptr
-                );
-            }
-#if BLE_FEATURE_PERIODIC_ADVERTISING
-            if (_active_periodic_sets.get(i)) {
-                _pal_gap.periodic_advertising_enable(
-                    /* enable */ false,
-                    (advertising_handle_t) i
-                );
-            }
-            _active_periodic_sets.clear();
-#endif // BLE_FEATURE_PERIODIC_ADVERTISING
-        }
-
-        /* clear state of all advertising sets */
-        _existing_sets.clear();
-
-        /* clear advertising set data on the controller */
-        _pal_gap.clear_advertising_sets();
-    } else
-#else // BLE_FEATURE_EXTENDED_ADVERTISING
-    {
-        if (_active_sets.get(LEGACY_ADVERTISING_HANDLE)) {
-            _pal_gap.advertising_enable(false);
-        }
+    /* reset pending advertising sets */
+    AdvertisingEnableStackNode_t* next = _advertising_enable_queue.next;
+    _advertising_enable_queue.next = nullptr;
+    _advertising_enable_queue.handle = ble::INVALID_ADVERTISING_HANDLE;
+    _advertising_enable_pending = false;
+    /* free any allocated nodes */
+    while (next) {
+        AdvertisingEnableStackNode_t* node_to_free = next;
+        AdvertisingEnableStackNode_t* next = next->next;
+        delete node_to_free;
     }
+    _existing_sets.clear();
+#if BLE_FEATURE_PERIODIC_ADVERTISING
+    _active_periodic_sets.clear();
+#endif // BLE_FEATURE_PERIODIC_ADVERTISING
 #endif // BLE_FEATURE_EXTENDED_ADVERTISING
-
     _active_sets.clear();
     _pending_stop_sets.clear();
     _pending_sets.clear();

From 910b7a6438fb5612eb1dd97128f7c5cc1beaf537 Mon Sep 17 00:00:00 2001
From: Paul Szczepanek <paul.szczepanek@arm.com>
Date: Tue, 18 May 2021 10:39:20 +0100
Subject: [PATCH 03/11] clear the pal gap queue on reset

---
 connectivity/FEATURE_BLE/source/generic/GapImpl.cpp | 1 +
 connectivity/FEATURE_BLE/source/pal/PalEventQueue.h | 4 ++++
 2 files changed, 5 insertions(+)

diff --git a/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp b/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
index 636b910682..2e405b1a4b 100644
--- a/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
+++ b/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
@@ -1214,6 +1214,7 @@ ble_error_t Gap::reset()
     /* Notify that the instance is about to shut down */
 //    shutdownCallChain.call(this);
     shutdownCallChain.clear();
+    _event_queue.clear();
 
     _event_handler = nullptr;
     _initiating = false;
diff --git a/connectivity/FEATURE_BLE/source/pal/PalEventQueue.h b/connectivity/FEATURE_BLE/source/pal/PalEventQueue.h
index e5fbb4cd6d..727859c97e 100644
--- a/connectivity/FEATURE_BLE/source/pal/PalEventQueue.h
+++ b/connectivity/FEATURE_BLE/source/pal/PalEventQueue.h
@@ -52,6 +52,10 @@ public:
      * BLEInstanceBase::process
      */
     virtual bool post(const mbed::Callback<void()>& event) = 0;
+
+    /** Remove all pending events.
+     */
+    virtual void clear() = 0;
 };
 
 } // namespace ble

From 4767b7e1ac9b03fdd4212fb19fb3b3d31facb33f Mon Sep 17 00:00:00 2001
From: Paul Szczepanek <paul.szczepanek@arm.com>
Date: Tue, 18 May 2021 20:58:26 +0100
Subject: [PATCH 04/11] fix missing observer ifdefs

---
 .../FEATURE_BLE/source/generic/GapImpl.cpp    | 20 +++++++++++++++----
 .../FEATURE_BLE/source/generic/GapImpl.h      |  2 ++
 2 files changed, 18 insertions(+), 4 deletions(-)

diff --git a/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp b/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
index 2e405b1a4b..8b4a9370b6 100644
--- a/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
+++ b/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
@@ -1218,8 +1218,6 @@ ble_error_t Gap::reset()
 
     _event_handler = nullptr;
     _initiating = false;
-    set_scan_state(ScanState::idle);
-    _scan_requested = false;
 
 #if BLE_FEATURE_PRIVACY
     _privacy_initialization_pending = false;
@@ -1231,6 +1229,8 @@ ble_error_t Gap::reset()
 #endif // BLE_FEATURE_PRIVACY
 
 #if BLE_ROLE_OBSERVER
+    set_scan_state(ScanState::idle);
+    _scan_requested = false;
     _scan_parameters_set = false;
     _scan_timeout.detach();
 #endif
@@ -2415,12 +2415,14 @@ ble_error_t Gap::startAdvertising(
             return BLE_ERROR_INVALID_PARAM;
         }
 
+#if BLE_ROLE_OBSERVER
         // Address can be updated if the device is not scanning or advertising
         if ((_scan_state == ScanState::idle) && !_active_sets.get(LEGACY_ADVERTISING_HANDLE)) {
             _pal_gap.set_random_address(*random_address);
         } else {
             tr_error("could not update address, device scanning/advertising");
         }
+#endif // BLE_ROLE_OBSERVER
 
         error = _pal_gap.advertising_enable(true);
         if (error) {
@@ -3433,7 +3435,11 @@ void Gap::on_legacy_advertising_stopped()
     _pending_sets.clear(LEGACY_ADVERTISING_HANDLE);
 
     // restart advertising if it was stopped to refresh the address
-    if (_address_refresh_sets.get(LEGACY_ADVERTISING_HANDLE) && (_scan_state == ScanState::idle)) {
+    if (_address_refresh_sets.get(LEGACY_ADVERTISING_HANDLE)
+#if BLE_ROLE_OBSERVER
+        && (_scan_state == ScanState::idle)
+#endif //BLE_ROLE_OBSERVER
+    ) {
         _address_refresh_sets.clear(LEGACY_ADVERTISING_HANDLE);
         startAdvertising(LEGACY_ADVERTISING_HANDLE);
         _adv_started_from_refresh.set(LEGACY_ADVERTISING_HANDLE);
@@ -4238,7 +4244,11 @@ bool Gap::is_advertising() const
 }
 
 bool Gap::is_radio_active() const {
-    return _initiating || (_scan_state != ScanState::idle) || is_advertising();
+    return _initiating ||
+#if BLE_ROLE_OBSERVER
+        (_scan_state != ScanState::idle) ||
+#endif // BLE_ROLE_OBSERVER
+        is_advertising();
 }
 
 void Gap::update_advertising_set_connectable_attribute(
@@ -4303,12 +4313,14 @@ const address_t *Gap::get_random_address(controller_operation_t operation, size_
     // it to the address to use to determine if the address is correct or not.
     if (_initiating) {
         address_in_use = &resolvable_address;
+#if BLE_ROLE_OBSERVER
     } else if (_scan_state != ScanState::idle) {
         if (central_non_resolvable) {
             address_in_use = &non_resolvable_address;
         } else {
             address_in_use = &resolvable_address;
         }
+#endif //BLE_ROLE_OBSERVER
     } else if (advertising_use_main_address && (_active_sets.get(set_id) || _pending_sets.get(set_id))) {
         if (!_set_is_connectable.get(set_id) && peripheral_non_resolvable) {
             address_in_use = &non_resolvable_address;
diff --git a/connectivity/FEATURE_BLE/source/generic/GapImpl.h b/connectivity/FEATURE_BLE/source/generic/GapImpl.h
index 8c048d5d26..b2b6c1a9b9 100644
--- a/connectivity/FEATURE_BLE/source/generic/GapImpl.h
+++ b/connectivity/FEATURE_BLE/source/generic/GapImpl.h
@@ -909,6 +909,7 @@ private:
 #endif // BLE_FEATURE_PRIVACY
     ble::address_t _random_static_identity_address;
 
+#if BLE_ROLE_OBSERVER
     enum class ScanState : uint8_t {
         idle,
         scan,
@@ -925,6 +926,7 @@ private:
     scan_period_t _scan_requested_period = scan_period_t(0);
 
     bool _scan_requested = false;
+#endif // BLE_ROLE_OBSERVER
 
 #if BLE_GAP_HOST_BASED_PRIVATE_ADDRESS_RESOLUTION
     enum class ConnectionToHostResolvedAddressState : uint8_t {

From e99741dd3d3f0637d78bc34f3f9583fe1a2958bc Mon Sep 17 00:00:00 2001
From: Paul Szczepanek <paul.szczepanek@arm.com>
Date: Tue, 1 Jun 2021 15:45:08 +0100
Subject: [PATCH 05/11] change function names

---
 .../FEATURE_BLE/source/generic/GapImpl.cpp     | 18 +++++++++---------
 .../FEATURE_BLE/source/generic/GapImpl.h       |  6 +++---
 2 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp b/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
index 8b4a9370b6..6d30e32652 100644
--- a/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
+++ b/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
@@ -2403,8 +2403,8 @@ ble_error_t Gap::startAdvertising(
         }
 
         _event_queue.post([this, handle, maxDuration, maxEvents] {
-            start_advertising_enable(handle, maxDuration, maxEvents);
-            evaluate_advertising_enable_queue();
+            queue_advertising_start(handle, maxDuration, maxEvents);
+            process_enable_queue();
         });
 
     } else
@@ -2448,7 +2448,7 @@ ble_error_t Gap::startAdvertising(
 #endif
 
 #if BLE_FEATURE_EXTENDED_ADVERTISING
-void Gap::start_advertising_enable(
+void Gap::queue_advertising_start(
     advertising_handle_t handle,
     adv_duration_t maxDuration,
     uint8_t maxEvents
@@ -2470,7 +2470,7 @@ void Gap::start_advertising_enable(
             }
             next = &(*next)->next;
         }
-        *next = new AdvertisingEnableStackNode_t();
+        *next = new(std::nothrow) AdvertisingEnableStackNode_t();
         if (!*next) {
             tr_error("Out of memory creating pending advertising enable node for handle %d", handle);
             return;
@@ -2482,7 +2482,7 @@ void Gap::start_advertising_enable(
     }
 }
 
-void Gap::evaluate_advertising_enable_queue()
+void Gap::process_enable_queue()
 {
     tr_info("Evaluating pending advertising sets to be started");
     if (_advertising_enable_pending) {
@@ -2575,7 +2575,7 @@ ble_error_t Gap::stopAdvertising(advertising_handle_t handle)
             }
             _pending_stop_sets.set(handle);
             if (!delay) {
-                evaluate_advertising_stop();
+                process_stop();
             }
         });
 
@@ -2604,7 +2604,7 @@ ble_error_t Gap::stopAdvertising(advertising_handle_t handle)
 }
 
 #if BLE_FEATURE_EXTENDED_ADVERTISING
-void Gap::evaluate_advertising_stop()
+void Gap::process_stop()
 {
     // refresh for address for all connectable advertising sets
     for (size_t i = 0; i < BLE_GAP_MAX_ADVERTISING_SETS; ++i) {
@@ -3461,7 +3461,7 @@ void Gap::on_advertising_set_started(const mbed::Span<const uint8_t>& handles)
 
     _event_queue.post([this] {
         _advertising_enable_pending = false;
-        evaluate_advertising_enable_queue();
+        process_enable_queue();
     });
 
     for (const auto &handle : handles) {
@@ -3506,7 +3506,7 @@ void Gap::on_advertising_set_terminated(
 
     _event_queue.post([this, advertising_handle] {
         _pending_stop_sets.clear(advertising_handle);
-        evaluate_advertising_stop();
+        process_stop();
     });
 
     if (!_event_handler) {
diff --git a/connectivity/FEATURE_BLE/source/generic/GapImpl.h b/connectivity/FEATURE_BLE/source/generic/GapImpl.h
index b2b6c1a9b9..f63f94e7b2 100644
--- a/connectivity/FEATURE_BLE/source/generic/GapImpl.h
+++ b/connectivity/FEATURE_BLE/source/generic/GapImpl.h
@@ -563,14 +563,14 @@ private:
 
 #if BLE_ROLE_BROADCASTER
 #if BLE_FEATURE_EXTENDED_ADVERTISING
-    void start_advertising_enable(
+    void queue_advertising_start(
         advertising_handle_t handle,
         adv_duration_t maxDuration,
         uint8_t maxEvents
     );
 
-    void evaluate_advertising_enable_queue();
-    void evaluate_advertising_stop();
+    void process_enable_queue();
+    void process_stop();
 #endif // BLE_FEATURE_EXTENDED_ADVERTISING
 
     ble_error_t setAdvertisingData(

From 8dedd43e7caa10d6ed5759694498ec67c2d7958f Mon Sep 17 00:00:00 2001
From: Paul Szczepanek <paul.szczepanek@arm.com>
Date: Tue, 1 Jun 2021 22:37:43 +0100
Subject: [PATCH 06/11] add status to start and stop events

---
 connectivity/FEATURE_BLE/include/ble/Gap.h    | 16 +----
 .../FEATURE_BLE/include/ble/gap/Events.h      | 63 +++++++------------
 2 files changed, 25 insertions(+), 54 deletions(-)

diff --git a/connectivity/FEATURE_BLE/include/ble/Gap.h b/connectivity/FEATURE_BLE/include/ble/Gap.h
index 6d3f529668..a5477aa5a6 100644
--- a/connectivity/FEATURE_BLE/include/ble/Gap.h
+++ b/connectivity/FEATURE_BLE/include/ble/Gap.h
@@ -341,18 +341,6 @@ public:
         {
         }
 
-        /**
-         * Called when an asynchronous advertising command fails.
-         *
-         * @param event Advertising command failed event.
-         *
-         * @see startAdvertising()
-         * @see stopAdvertising()
-         */
-        virtual void onAdvertisingCommandFailed(const AdvertisingCommandFailedEvent &event)
-        {
-        }
-
         /**
          * Called when a scanner receives an advertising or a scan response packet.
          *
@@ -761,7 +749,7 @@ public:
      * @param maxEvents Max number of events produced during advertising - 0 means no limit.
      * @return BLE_ERROR_NONE on success. This does not guarantee the set has started if
      * extended advertising is enabled. Register an event handler and wait for onAdvertisingStart
-     * event. An (unlikely) failed start will emit an onAdvertisingCommandFailed instead.
+     * event. An (unlikely) failed start the status of the event will contain an error.
      *
      * @see EventHandler::onAdvertisingStart when the advertising starts.
      * @see EventHandler::onScanRequestReceived when a scan request is received.
@@ -781,7 +769,7 @@ public:
      * @param handle Advertising set handle.
      * @return BLE_ERROR_NONE on success. For extented advertising this does not guarantee
      * the set is stopped if. Register an event handler and wait for onAdvertisingEnd event.
-     * An (unlikely) failed stop will emit an onAdvertisingCommandFailed instead.
+     * An (unlikely) failed stop the event status will contain the error code.
      */
     ble_error_t stopAdvertising(advertising_handle_t handle);
 
diff --git a/connectivity/FEATURE_BLE/include/ble/gap/Events.h b/connectivity/FEATURE_BLE/include/ble/gap/Events.h
index 5e1c817357..a676ba14d6 100644
--- a/connectivity/FEATURE_BLE/include/ble/gap/Events.h
+++ b/connectivity/FEATURE_BLE/include/ble/gap/Events.h
@@ -586,9 +586,10 @@ struct AdvertisingStartEvent {
     /** Create an advertising start event.
      *
      * @param advHandle Advertising set handle.
+     * @param status Advertising set start command status.
      */
-    AdvertisingStartEvent(advertising_handle_t advHandle) :
-        advHandle(advHandle)
+    AdvertisingStartEvent(advertising_handle_t advHandle, ble_error_t status = BLE_ERROR_NONE) :
+        advHandle(advHandle), status(status)
     {
     }
 
@@ -600,43 +601,12 @@ struct AdvertisingStartEvent {
         return advHandle;
     }
 
-private:
-    advertising_handle_t advHandle;
-};
-
-/**
- * Event produced when an async advertising command fails.
- *
- * @see ble::Gap::EventHandler::onAdvertisingCommandFailed().
- */
-struct AdvertisingCommandFailedEvent {
-#if !defined(DOXYGEN_ONLY)
-    /** Create an extended advertising command failed event.
-     *
-     * @param advHandle Advertising set handle.
-     * @param status Error code.
-     */
-    AdvertisingCommandFailedEvent(
-        advertising_handle_t advHandle,
-        ble_error_t status
-    ) :
-        advHandle(advHandle),
-        status(status)
-    {
-    }
-
-#endif
-    /** Get advertising handle. */
-    advertising_handle_t getAdvHandle() const
-    {
-        return advHandle;
-    }
-
-    /** Error code that caused the event. */
-    uint8_t getStatus() const
+    /** Get status of operation. */
+    ble_error_t getStatus() const
     {
         return status;
     }
+
 private:
     advertising_handle_t advHandle;
     ble_error_t status;
@@ -648,7 +618,8 @@ private:
  * @see ble::Gap::EventHandler::onAdvertisingEnd().
  *
  * @note The connection handle, connected flag and completed_event fields are
- * valid if the flag legacy is not set to true.
+ * valid if the flag legacy is not set to true. If status is different from BLE_ERROR_NONE
+ * the completed_events field is not valid and the set may still be active.
  */
 struct AdvertisingEndEvent {
 #if !defined(DOXYGEN_ONLY)
@@ -659,18 +630,21 @@ struct AdvertisingEndEvent {
      * @param connection Connection handle.
      * @param completed_events Number of events created during before advertising end.
      * @param connected True if connection has been established.
+     * @param status Error code if stop command failed.
      */
     AdvertisingEndEvent(
         advertising_handle_t advHandle,
         connection_handle_t connection,
         uint8_t completed_events,
-        bool connected
+        bool connected,
+        ble_error_t status = BLE_ERROR_NONE
     ) :
         advHandle(advHandle),
         connection(connection),
         completed_events(completed_events),
         connected(connected),
-        legacy(false)
+        legacy(false),
+        status(status)
     {
     }
 
@@ -681,7 +655,8 @@ struct AdvertisingEndEvent {
         connection(),
         completed_events(0),
         connected(false),
-        legacy(true)
+        legacy(true),
+        status(BLE_ERROR_NONE)
     {
     }
 
@@ -721,12 +696,20 @@ struct AdvertisingEndEvent {
         return legacy;
     }
 
+    /** Get the result of the stop advertising event. If the status is not BLE_ERROR_NONE the set
+     * may still be active. */
+    ble_error_t getStatus() const
+    {
+        return status;
+    }
+
 private:
     advertising_handle_t advHandle;
     connection_handle_t connection;
     uint8_t completed_events;
     bool connected;
     bool legacy;
+    ble_error_t status;
 };
 
 /**

From 0670470af6f956794f54d14011d55708523c2769 Mon Sep 17 00:00:00 2001
From: Paul Szczepanek <paul.szczepanek@arm.com>
Date: Tue, 1 Jun 2021 22:38:13 +0100
Subject: [PATCH 07/11] add config for number of pending adv sets starting

---
 connectivity/FEATURE_BLE/mbed_lib.json | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/connectivity/FEATURE_BLE/mbed_lib.json b/connectivity/FEATURE_BLE/mbed_lib.json
index 8cb752c076..29c51855de 100644
--- a/connectivity/FEATURE_BLE/mbed_lib.json
+++ b/connectivity/FEATURE_BLE/mbed_lib.json
@@ -108,6 +108,11 @@
             "value": 16,
             "macro_name": "BLE_GAP_HOST_PRIVACY_RESOLVED_CACHE_SIZE"
         },
+        "ble-gap-host-max-advertising-start-commands": {
+            "help": "There can only be one outstanding advertising set command sent to the controller. This determines how many advertising set start commands can be queued on the host. Must be between 1 and BLE_GAP_MAX_ADVERTISING_SETS.",
+            "value": 4,
+            "macro_name": "BLE_GAP_HOST_MAX_OUTSTANDING_ADVERTISING_START_COMMANDS"
+        },
 
         "ble-passkey-display-reversed-digits-deprecation": {
           "help": "This option is part of the deprecation process. Set this to false to remove the deprecation notice. When set to true, the digits in the SecurityManager passkeyDiplay event are reversed. When set to false the digits are in the correct order.",

From 8ec7cfd820528fc5ce79d9cefc23a290c31881e6 Mon Sep 17 00:00:00 2001
From: Paul Szczepanek <paul.szczepanek@arm.com>
Date: Tue, 1 Jun 2021 22:38:39 +0100
Subject: [PATCH 08/11] start and stop adv sets in batches

---
 .../FEATURE_BLE/source/generic/GapImpl.cpp    | 212 ++++++++----------
 .../FEATURE_BLE/source/generic/GapImpl.h      |  27 +--
 2 files changed, 105 insertions(+), 134 deletions(-)

diff --git a/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp b/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
index 6d30e32652..ba7fce28e9 100644
--- a/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
+++ b/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
@@ -374,9 +374,7 @@ Gap::Gap(
 #endif // BLE_ROLE_OBSERVER
 {
 #if BLE_FEATURE_EXTENDED_ADVERTISING
-    _advertising_enable_queue.handle = ble::INVALID_ADVERTISING_HANDLE;
-    _advertising_enable_queue.next = nullptr;
-    _advertising_enable_pending = false;
+    _advertising_enable_command_params.number_of_handles = 0;
 #endif //BLE_FEATURE_EXTENDED_ADVERTISING
     _pal_gap.initialize();
 
@@ -1214,7 +1212,6 @@ ble_error_t Gap::reset()
     /* Notify that the instance is about to shut down */
 //    shutdownCallChain.call(this);
     shutdownCallChain.clear();
-    _event_queue.clear();
 
     _event_handler = nullptr;
     _initiating = false;
@@ -1240,16 +1237,9 @@ ble_error_t Gap::reset()
     _pal_gap.clear_advertising_sets();
 #if BLE_FEATURE_EXTENDED_ADVERTISING
     /* reset pending advertising sets */
-    AdvertisingEnableStackNode_t* next = _advertising_enable_queue.next;
-    _advertising_enable_queue.next = nullptr;
-    _advertising_enable_queue.handle = ble::INVALID_ADVERTISING_HANDLE;
-    _advertising_enable_pending = false;
-    /* free any allocated nodes */
-    while (next) {
-        AdvertisingEnableStackNode_t* node_to_free = next;
-        AdvertisingEnableStackNode_t* next = next->next;
-        delete node_to_free;
-    }
+    _advertising_enable_command_params.number_of_handles = 0;
+    _process_enable_queue_pending = false;
+    _process_disable_queue_pending = false;
     _existing_sets.clear();
 #if BLE_FEATURE_PERIODIC_ADVERTISING
     _active_periodic_sets.clear();
@@ -2402,10 +2392,23 @@ ble_error_t Gap::startAdvertising(
             _pal_gap.set_advertising_set_random_address(handle, *random_address);
         }
 
-        _event_queue.post([this, handle, maxDuration, maxEvents] {
-            queue_advertising_start(handle, maxDuration, maxEvents);
-            process_enable_queue();
-        });
+        /* remember the parameters that will be enabled when the last command completes */
+        if (_advertising_enable_command_params.number_of_handles == BLE_GAP_HOST_MAX_OUTSTANDING_ADVERTISING_START_COMMANDS) {
+            return BLE_ERROR_NO_MEM;
+        }
+
+        const uint8_t i = _advertising_enable_command_params.number_of_handles;
+        _advertising_enable_command_params.handles[i] = handle;
+        _advertising_enable_command_params.max_durations[i] = maxDuration;
+        _advertising_enable_command_params.max_events[i] = maxEvents;
+        _advertising_enable_command_params.number_of_handles++;
+
+        /* we delay the processing to gather as many calls as we can in one go */
+        if (!_process_enable_queue_pending) {
+            _process_enable_queue_pending = _event_queue.post([this] {
+                process_enable_queue();
+            });
+        }
 
     } else
 #endif // BLE_FEATURE_EXTENDED_ADVERTISING
@@ -2448,90 +2451,51 @@ ble_error_t Gap::startAdvertising(
 #endif
 
 #if BLE_FEATURE_EXTENDED_ADVERTISING
-void Gap::queue_advertising_start(
-    advertising_handle_t handle,
-    adv_duration_t maxDuration,
-    uint8_t maxEvents
-)
-{
-    /* remember the parameters that will be enabled when the last command completes */
-    if (_advertising_enable_queue.handle == ble::INVALID_ADVERTISING_HANDLE) {
-        /* special case when there is only one pending */
-        _advertising_enable_queue.handle = handle;
-        _advertising_enable_queue.max_duration = maxDuration;
-        _advertising_enable_queue.max_events = maxEvents;
-        _advertising_enable_queue.next = nullptr;
-    } else {
-        AdvertisingEnableStackNode_t** next = &_advertising_enable_queue.next;
-        /* move down the queue until we're over a nullptr */
-        if (*next) {
-            while ((*next)->next) {
-                next = &((*next)->next);
-            }
-            next = &(*next)->next;
-        }
-        *next = new(std::nothrow) AdvertisingEnableStackNode_t();
-        if (!*next) {
-            tr_error("Out of memory creating pending advertising enable node for handle %d", handle);
-            return;
-        }
-        (*next)->handle = handle;
-        (*next)->max_duration = maxDuration;
-        (*next)->max_events = maxEvents;
-        (*next)->next = nullptr;
-    }
-}
-
 void Gap::process_enable_queue()
 {
     tr_info("Evaluating pending advertising sets to be started");
-    if (_advertising_enable_pending) {
+    if (!_advertising_enable_command_params.number_of_handles) {
+        /* no set pending to be enabled */
         return;
     }
 
-    if (_advertising_enable_queue.handle == ble::INVALID_ADVERTISING_HANDLE) {
-        /* no set pending to be enabled*/
-        return;
+    for (size_t i = 0; i < BLE_GAP_MAX_ADVERTISING_SETS; ++i) {
+        if (_pending_sets.get(i)) {
+            /* we have to wait until nothing is pending */
+            return;
+        }
     }
 
     ble_error_t error = _pal_gap.extended_advertising_enable(
         /* enable */ true,
-        /* number of advertising sets */ 1,
-        &_advertising_enable_queue.handle,
-        _advertising_enable_queue.max_duration.storage(),
-        &_advertising_enable_queue.max_events
+        _advertising_enable_command_params.number_of_handles,
+        _advertising_enable_command_params.handles,
+        (uint16_t*)&_advertising_enable_command_params.max_durations,
+        _advertising_enable_command_params.max_events
     );
 
     if (error) {
         tr_error("Failed to start advertising set with error: %s", to_string(error));
         if (_event_handler) {
-            _event_handler->onAdvertisingCommandFailed(
-                AdvertisingCommandFailedEvent(
-                    _advertising_enable_queue.handle,
-                    error
-                )
-            );
+            for (size_t i = 0; i < _advertising_enable_command_params.number_of_handles; ++i) {
+                _pending_sets.clear(_advertising_enable_command_params.handles[i]);
+                _event_handler->onAdvertisingStart(
+                    AdvertisingStartEvent(_advertising_enable_command_params.handles[i], error)
+                );
+            }
         }
     } else {
-        _advertising_enable_pending = true;
-        if (_advertising_enable_queue.max_duration.value() || _advertising_enable_queue.max_events) {
-            _interruptible_sets.clear(_advertising_enable_queue.handle);
-        } else {
-            _interruptible_sets.set(_advertising_enable_queue.handle);
+        for (size_t i = 0; i < _advertising_enable_command_params.number_of_handles; ++i) {
+            if (_advertising_enable_command_params.max_durations[i].value() || _advertising_enable_command_params.max_events[i]) {
+                _interruptible_sets.clear(_advertising_enable_command_params.handles[i]);
+            } else {
+                _interruptible_sets.set(_advertising_enable_command_params.handles[i]);
+            }
         }
     }
 
-    /* if there's anything else waiting, queue it up, otherwise mark the head node handle as invalid */
-    if (_advertising_enable_queue.next) {
-        AdvertisingEnableStackNode_t* next = _advertising_enable_queue.next;
-        _advertising_enable_queue.handle = next->handle;
-        _advertising_enable_queue.max_duration = next->max_duration;
-        _advertising_enable_queue.max_events = next->max_events;
-        _advertising_enable_queue.next = next->next;
-        delete next;
-    } else {
-        _advertising_enable_queue.handle = ble::INVALID_ADVERTISING_HANDLE;
-    }
+    _advertising_enable_command_params.number_of_handles = 0;
+    _process_enable_queue_pending = false;
 }
 #endif //BLE_FEATURE_EXTENDED_ADVERTISING
 
@@ -2564,22 +2528,15 @@ ble_error_t Gap::stopAdvertising(advertising_handle_t handle)
 
 #if BLE_FEATURE_EXTENDED_ADVERTISING
     if (is_extended_advertising_available()) {
-        _event_queue.post([this, handle] {
-            /* if any already pending to stop delay the command execution */
-            bool delay = false;
-            for (size_t i = 0; i < BLE_GAP_MAX_ADVERTISING_SETS; ++i) {
-                if (_pending_stop_sets.get(i)) {
-                    delay = true;
-                    break;
-                }
-            }
-            _pending_stop_sets.set(handle);
-            if (!delay) {
-                process_stop();
-            }
-        });
+        _pending_stop_sets.set(handle);
+        /* delay execution of command to accumulate multiple sets */
+        if (!_process_disable_queue_pending) {
+            _process_disable_queue_pending = _event_queue.post([this] {
+                process_disable_queue();
+            });
+        }
 
-        return BLE_ERROR_NONE;
+        status = BLE_ERROR_NONE;
 
     } else
 #endif // BLE_FEATURE_EXTENDED_ADVERTISING
@@ -2604,31 +2561,44 @@ ble_error_t Gap::stopAdvertising(advertising_handle_t handle)
 }
 
 #if BLE_FEATURE_EXTENDED_ADVERTISING
-void Gap::process_stop()
+void Gap::process_disable_queue()
 {
+    advertising_handle_t sets[BLE_GAP_MAX_ADVERTISING_SETS];
+    uint8_t number_of_handles = 0;
     // refresh for address for all connectable advertising sets
     for (size_t i = 0; i < BLE_GAP_MAX_ADVERTISING_SETS; ++i) {
         if (_pending_stop_sets.get(i)) {
-            ble_error_t status = _pal_gap.extended_advertising_enable(
-                /* enable */ false,
-                /* number of advertising sets */ 1,
-                (advertising_handle_t*)&i,
-                nullptr,
-                nullptr
-            );
-            if (status) {
-                _event_handler->onAdvertisingCommandFailed(
-                    AdvertisingCommandFailedEvent(
-                        (advertising_handle_t)i,
+            sets[number_of_handles] = i;
+            number_of_handles++;
+            _pending_stop_sets.clear(i);
+        }
+    }
+
+    if (number_of_handles) {
+        ble_error_t status = _pal_gap.extended_advertising_enable(
+            /* enable */ false,
+             number_of_handles,
+             (advertising_handle_t*)&sets,
+             nullptr,
+             nullptr
+        );
+        if (status) {
+            for (size_t i = 0; i < number_of_handles; ++i) {
+                _event_handler->onAdvertisingEnd(
+                    AdvertisingEndEvent(
+                        (advertising_handle_t)sets[i],
+                        0/*connection*/,
+                        0/*completed_events*/,
+                        false/*connected*/,
                         status
                     )
                 );
                 tr_error("Could not stop advertising set %u, error: %s", i, to_string(status));
             }
-            break;
         }
     }
 
+    _process_disable_queue_pending = false;
 }
 #endif // BLE_FEATURE_EXTENDED_ADVERTISING
 
@@ -3459,11 +3429,6 @@ void Gap::on_advertising_set_started(const mbed::Span<const uint8_t>& handles)
 {
     tr_info("Advertising set started - handles=%s", mbed_trace_array(handles.data(), handles.size()));
 
-    _event_queue.post([this] {
-        _advertising_enable_pending = false;
-        process_enable_queue();
-    });
-
     for (const auto &handle : handles) {
         _active_sets.set(handle);
         _pending_sets.clear(handle);
@@ -3475,6 +3440,13 @@ void Gap::on_advertising_set_started(const mbed::Span<const uint8_t>& handles)
             );
         }
     }
+
+    /* delay processing to minimise churn (if multiple events are pending that would trigger it) */
+    if (!_process_enable_queue_pending) {
+        _process_enable_queue_pending = _event_queue.post([this] {
+            process_enable_queue();
+        });
+    }
 }
 
 void Gap::on_advertising_set_terminated(
@@ -3504,10 +3476,12 @@ void Gap::on_advertising_set_terminated(
         return;
     }
 
-    _event_queue.post([this, advertising_handle] {
-        _pending_stop_sets.clear(advertising_handle);
-        process_stop();
-    });
+    /* postpone as other events may still be pending */
+    if (!_process_disable_queue_pending) {
+        _process_disable_queue_pending = _event_queue.post([this] {
+            process_disable_queue();
+        });
+    }
 
     if (!_event_handler) {
         return;
diff --git a/connectivity/FEATURE_BLE/source/generic/GapImpl.h b/connectivity/FEATURE_BLE/source/generic/GapImpl.h
index f63f94e7b2..96f0c336da 100644
--- a/connectivity/FEATURE_BLE/source/generic/GapImpl.h
+++ b/connectivity/FEATURE_BLE/source/generic/GapImpl.h
@@ -563,14 +563,8 @@ private:
 
 #if BLE_ROLE_BROADCASTER
 #if BLE_FEATURE_EXTENDED_ADVERTISING
-    void queue_advertising_start(
-        advertising_handle_t handle,
-        adv_duration_t maxDuration,
-        uint8_t maxEvents
-    );
-
     void process_enable_queue();
-    void process_stop();
+    void process_disable_queue();
 #endif // BLE_FEATURE_EXTENDED_ADVERTISING
 
     ble_error_t setAdvertisingData(
@@ -1006,16 +1000,19 @@ private:
     BitArray<BLE_GAP_MAX_ADVERTISING_SETS> _adv_started_from_refresh;
 
 #if BLE_FEATURE_EXTENDED_ADVERTISING
-    struct AdvertisingEnableStackNode_t {
-        adv_duration_t max_duration;
-        advertising_handle_t handle;
-        uint8_t max_events;
-        AdvertisingEnableStackNode_t* next;
+#if BLE_GAP_HOST_MAX_OUTSTANDING_ADVERTISING_START_COMMANDS < 1 || BLE_GAP_HOST_MAX_OUTSTANDING_ADVERTISING_START_COMMANDS > BLE_GAP_MAX_ADVERTISING_SETS
+#error "BLE_GAP_HOST_MAX_OUTSTANDING_ADVERTISING_START_COMMANDS must be at least 1 and not bigget than BLE_GAP_MAX_ADVERTISING_SETS"
+#endif
+    struct AdvertisingEnableCommandParams_t {
+        adv_duration_t max_durations[BLE_GAP_HOST_MAX_OUTSTANDING_ADVERTISING_START_COMMANDS];
+        advertising_handle_t handles[BLE_GAP_HOST_MAX_OUTSTANDING_ADVERTISING_START_COMMANDS];
+        uint8_t max_events[BLE_GAP_HOST_MAX_OUTSTANDING_ADVERTISING_START_COMMANDS];
+        uint8_t number_of_handles;
     };
 
-    /* to simplify code and avoid allocation unless multiple requests issued we keep one node as member */
-    AdvertisingEnableStackNode_t _advertising_enable_queue;
-    bool _advertising_enable_pending;
+    AdvertisingEnableCommandParams_t _advertising_enable_command_params;
+    bool _process_enable_queue_pending = false;
+    bool _process_disable_queue_pending = false;
 #endif // BLE_FEATURE_EXTENDED_ADVERTISING
 
     bool _user_manage_connection_parameter_requests;

From b21ee6b0a0157203a12e7cd32e17443bb6455c3c Mon Sep 17 00:00:00 2001
From: Paul Szczepanek <paul.szczepanek@arm.com>
Date: Wed, 2 Jun 2021 12:06:12 +0100
Subject: [PATCH 09/11] add information about privacy refresh to config option

---
 connectivity/FEATURE_BLE/mbed_lib.json | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/connectivity/FEATURE_BLE/mbed_lib.json b/connectivity/FEATURE_BLE/mbed_lib.json
index 29c51855de..2d1b11ef40 100644
--- a/connectivity/FEATURE_BLE/mbed_lib.json
+++ b/connectivity/FEATURE_BLE/mbed_lib.json
@@ -109,7 +109,7 @@
             "macro_name": "BLE_GAP_HOST_PRIVACY_RESOLVED_CACHE_SIZE"
         },
         "ble-gap-host-max-advertising-start-commands": {
-            "help": "There can only be one outstanding advertising set command sent to the controller. This determines how many advertising set start commands can be queued on the host. Must be between 1 and BLE_GAP_MAX_ADVERTISING_SETS.",
+            "help": "There can only be one outstanding advertising set command sent to the controller. This determines how many advertising set start commands can be queued on the host. Must be between 1 and BLE_GAP_MAX_ADVERTISING_SETS. If privacy is used, this must be at equal or higher than then number of simultaneously active sets with a private address.",
             "value": 4,
             "macro_name": "BLE_GAP_HOST_MAX_OUTSTANDING_ADVERTISING_START_COMMANDS"
         },

From c51dc2f6cc3dd91b90727506c61f0cd85b4e7c49 Mon Sep 17 00:00:00 2001
From: Paul Szczepanek <paul.szczepanek@arm.com>
Date: Wed, 2 Jun 2021 12:06:43 +0100
Subject: [PATCH 10/11] set to pending only after issuing command

---
 .../FEATURE_BLE/source/generic/GapImpl.cpp    | 36 +++++++++++++------
 1 file changed, 26 insertions(+), 10 deletions(-)

diff --git a/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp b/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
index ba7fce28e9..7e1c081ef2 100644
--- a/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
+++ b/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
@@ -2392,11 +2392,19 @@ ble_error_t Gap::startAdvertising(
             _pal_gap.set_advertising_set_random_address(handle, *random_address);
         }
 
-        /* remember the parameters that will be enabled when the last command completes */
+        /* check we hanve't run out of space to remember parameters */
         if (_advertising_enable_command_params.number_of_handles == BLE_GAP_HOST_MAX_OUTSTANDING_ADVERTISING_START_COMMANDS) {
-            return BLE_ERROR_NO_MEM;
+            /* try to process early */
+            tr_debug("Processing enable queue early as run out of queue space");
+            process_enable_queue();
+            /* if the processing didn't clear the handles we cannot continue */
+            if (_advertising_enable_command_params.number_of_handles) {
+                tr_debug("Cannot enable set as no memory to record the parameters");
+                return BLE_ERROR_NO_MEM;
+            }
         }
 
+        /* remember the parameters that will be enabled in the delayed processing */
         const uint8_t i = _advertising_enable_command_params.number_of_handles;
         _advertising_enable_command_params.handles[i] = handle;
         _advertising_enable_command_params.max_durations[i] = maxDuration;
@@ -2442,9 +2450,9 @@ ble_error_t Gap::startAdvertising(
         }
 
         _interruptible_sets.set(LEGACY_ADVERTISING_HANDLE);
-    }
 
-    _pending_sets.set(handle);
+        _pending_sets.set(handle);
+    }
 
     return error;
 }
@@ -2456,12 +2464,14 @@ void Gap::process_enable_queue()
     tr_info("Evaluating pending advertising sets to be started");
     if (!_advertising_enable_command_params.number_of_handles) {
         /* no set pending to be enabled */
+        tr_debug("No sets to be enabled");
         return;
     }
 
     for (size_t i = 0; i < BLE_GAP_MAX_ADVERTISING_SETS; ++i) {
         if (_pending_sets.get(i)) {
             /* we have to wait until nothing is pending */
+            tr_debug("Cannot enable sets as pending sets present");
             return;
         }
     }
@@ -2491,6 +2501,7 @@ void Gap::process_enable_queue()
             } else {
                 _interruptible_sets.set(_advertising_enable_command_params.handles[i]);
             }
+            _pending_sets.set(_advertising_enable_command_params.handles[i]);
         }
     }
 
@@ -2553,9 +2564,9 @@ ble_error_t Gap::stopAdvertising(advertising_handle_t handle)
         }
 
         _advertising_timeout.detach();
-    }
 
-    _pending_sets.set(handle);
+        _pending_sets.set(handle);
+    }
 
     return status;
 }
@@ -2575,14 +2586,15 @@ void Gap::process_disable_queue()
     }
 
     if (number_of_handles) {
-        ble_error_t status = _pal_gap.extended_advertising_enable(
+        ble_error_t error = _pal_gap.extended_advertising_enable(
             /* enable */ false,
              number_of_handles,
              (advertising_handle_t*)&sets,
              nullptr,
              nullptr
         );
-        if (status) {
+
+        if (error) {
             for (size_t i = 0; i < number_of_handles; ++i) {
                 _event_handler->onAdvertisingEnd(
                     AdvertisingEndEvent(
@@ -2590,10 +2602,14 @@ void Gap::process_disable_queue()
                         0/*connection*/,
                         0/*completed_events*/,
                         false/*connected*/,
-                        status
+                        error
                     )
                 );
-                tr_error("Could not stop advertising set %u, error: %s", i, to_string(status));
+                tr_error("Could not stop advertising set %u, error: %s", i, to_string(error));
+            }
+        } else {
+            for (size_t i = 0; i < number_of_handles; ++i) {
+                _pending_sets.set(sets[i]);
             }
         }
     }

From 2d9a781dd84b7c908cff9fd466328b7f440fd458 Mon Sep 17 00:00:00 2001
From: Paul Szczepanek <paul.szczepanek@arm.com>
Date: Tue, 8 Jun 2021 09:48:45 +0100
Subject: [PATCH 11/11] fix typos and traces

---
 connectivity/FEATURE_BLE/source/generic/GapImpl.cpp | 6 +++---
 connectivity/FEATURE_BLE/source/generic/GapImpl.h   | 2 +-
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp b/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
index 7e1c081ef2..32573b6710 100644
--- a/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
+++ b/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
@@ -2392,14 +2392,14 @@ ble_error_t Gap::startAdvertising(
             _pal_gap.set_advertising_set_random_address(handle, *random_address);
         }
 
-        /* check we hanve't run out of space to remember parameters */
+        /* check we haven't run out of space to remember parameters */
         if (_advertising_enable_command_params.number_of_handles == BLE_GAP_HOST_MAX_OUTSTANDING_ADVERTISING_START_COMMANDS) {
             /* try to process early */
             tr_debug("Processing enable queue early as run out of queue space");
             process_enable_queue();
             /* if the processing didn't clear the handles we cannot continue */
             if (_advertising_enable_command_params.number_of_handles) {
-                tr_debug("Cannot enable set as no memory to record the parameters");
+                tr_warn("Cannot enable set as no memory to record the parameters");
                 return BLE_ERROR_NO_MEM;
             }
         }
@@ -2471,7 +2471,7 @@ void Gap::process_enable_queue()
     for (size_t i = 0; i < BLE_GAP_MAX_ADVERTISING_SETS; ++i) {
         if (_pending_sets.get(i)) {
             /* we have to wait until nothing is pending */
-            tr_debug("Cannot enable sets as pending sets present");
+            tr_info("Cannot enable sets as pending sets present, will retry later");
             return;
         }
     }
diff --git a/connectivity/FEATURE_BLE/source/generic/GapImpl.h b/connectivity/FEATURE_BLE/source/generic/GapImpl.h
index 96f0c336da..d9e74f1303 100644
--- a/connectivity/FEATURE_BLE/source/generic/GapImpl.h
+++ b/connectivity/FEATURE_BLE/source/generic/GapImpl.h
@@ -1001,7 +1001,7 @@ private:
 
 #if BLE_FEATURE_EXTENDED_ADVERTISING
 #if BLE_GAP_HOST_MAX_OUTSTANDING_ADVERTISING_START_COMMANDS < 1 || BLE_GAP_HOST_MAX_OUTSTANDING_ADVERTISING_START_COMMANDS > BLE_GAP_MAX_ADVERTISING_SETS
-#error "BLE_GAP_HOST_MAX_OUTSTANDING_ADVERTISING_START_COMMANDS must be at least 1 and not bigget than BLE_GAP_MAX_ADVERTISING_SETS"
+#error "BLE_GAP_HOST_MAX_OUTSTANDING_ADVERTISING_START_COMMANDS must be at least 1 and not bigger than BLE_GAP_MAX_ADVERTISING_SETS"
 #endif
     struct AdvertisingEnableCommandParams_t {
         adv_duration_t max_durations[BLE_GAP_HOST_MAX_OUTSTANDING_ADVERTISING_START_COMMANDS];