start and stop adv sets in batches

Author: paul-szczepanek-arm

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;

connectivity/FEATURE_BLE/source/generic/GapImpl.h

@@ -563,14 +563,8 @@ class Gap :
 
 #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 @@ class Gap :
     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;