From 5cf3b089e88c8cf0642155089822706ff7433cae Mon Sep 17 00:00:00 2001
From: Paul Szczeanek <paul.szczepanek@arm.com>
Date: Fri, 18 Sep 2020 16:53:29 +0100
Subject: [PATCH] remember state for searching

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

diff --git a/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp b/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
index 0babd2f71de..a50492f912a 100644
--- a/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
+++ b/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
@@ -3055,11 +3055,13 @@ void Gap::on_address_resolution_completed(
 #endif //BLE_FEATURE_CONNECTABLE
 
 #if BLE_ROLE_OBSERVER
+    uint8_t items_searched = 0;
     while(true) {
-        PendingAdvertisingReportEvent *event = _reports_pending_address_resolution.pop(
+        PendingAdvertisingReportEvent *event = _reports_pending_address_resolution.continue_pop(
             [peer_resolvable_address](PendingAdvertisingReportEvent &event){
                 return (event.get_pending_event().getPeerAddress() == peer_resolvable_address);
-            }
+            },
+            &items_searched
         );
 
         if (!event) {
diff --git a/connectivity/FEATURE_BLE/source/generic/GapImpl.h b/connectivity/FEATURE_BLE/source/generic/GapImpl.h
index 0ac1ccdc11a..1ac73bde42d 100644
--- a/connectivity/FEATURE_BLE/source/generic/GapImpl.h
+++ b/connectivity/FEATURE_BLE/source/generic/GapImpl.h
@@ -424,6 +424,26 @@ class Gap :
             return nullptr;
         }
 
+        /** Return pointer to the first element that fulfills the passed in condition and remove the entry
+         * that was pointing to the item. Transfers ownership to caller.
+         *
+         * @param compare_func The condition that is checked for all the items start from last put in.
+         * @param elements_searched Pointer to the number of items already searched. This is updated in the method.
+         * @return First element that fulfills the passed in condition or nullptr if no such item found.
+         */
+        EventType* continue_pop(mbed::Callback<bool(EventType&)> compare_func, IndexType *events_searched)
+        {
+            for (IndexType i = *events_searched; i < _current_size ; ++i) {
+                (*events_searched)++;
+                if (compare_func(*_pointers[_current_index])) {
+                    return pop();
+                }
+                increment_current_index();
+            }
+
+            return nullptr;
+        }
+
         /** Return pointer to the first element that fulfills the passed in condition. Does not remove item from list.
          *
          * @param compare_func The condition that is checked for all the items start from last put in.
@@ -462,6 +482,7 @@ class Gap :
     private:
         EventType* _pointers[MAX_EVENTS];
         IndexType _current_size = 0;
+        /* this helps us find the event faster */
         IndexType _current_index = 0;
     };