Add cache to ha_mgr

Upon re-connecting a HA, there's no need to rediscover it's services and characteristics.

src/asha_bt.cpp

@@ -249,13 +249,22 @@ static void start_scan()
 static void discover_services()
 {
     if (scan_state != ScanState::ServiceDiscovery) return;
-    LOG_INFO("Device paired. Discovering ASHA service\n");
-    auto res = gatt_client_discover_primary_services(&scan_gatt_event_handler, curr_scan.ha.conn_handle);
-    //auto res = gatt_client_discover_primary_services_by_uuid128(&scan_gatt_event_handler, curr_scan.ha.conn_handle, AshaUUID::service);
-    if (res != ERROR_CODE_SUCCESS) {
-        LOG_ERROR("Could not register service query: %d\n", static_cast<int>(res));
-        scan_state = ScanState::Disconnecting;
-        gap_disconnect(curr_scan.ha.conn_handle);
+    HA cached = ha_mgr.get_from_cache(curr_scan.ha.addr);
+    if (cached) {
+        LOG_INFO("Hearing aid found in cache. Skipping discovery\n");
+        cached.conn_handle = curr_scan.ha.conn_handle;
+        curr_scan.ha = cached;
+        scan_state = ScanState::Finalizing;
+        finalise_curr_discovery();
+    } else { 
+        LOG_INFO("Device paired. Discovering ASHA service\n");
+        auto res = gatt_client_discover_primary_services(&scan_gatt_event_handler, curr_scan.ha.conn_handle);
+        //auto res = gatt_client_discover_primary_services_by_uuid128(&scan_gatt_event_handler, curr_scan.ha.conn_handle, AshaUUID::service);
+        if (res != ERROR_CODE_SUCCESS) {
+            LOG_ERROR("Could not register service query: %d\n", static_cast<int>(res));
+            scan_state = ScanState::Disconnecting;
+            gap_disconnect(curr_scan.ha.conn_handle);
+        }
     }
 }
 

src/hearing_aid.cpp

@@ -302,6 +302,16 @@ bool HA::is_streaming_audio()
     return is_any_of(state, AudioPacketReady, AudioPacketSending, AudioPacketSent, ASPStartOk);
 }
 
+void HA::reset_uncached_vars()
+{
+    state = HA::State::Cached;
+    conn_handle = HCI_CON_HANDLE_INVALID;
+    cid = 0;
+    other_ha = nullptr;
+    audio_packet = nullptr;
+    audio_index  = 0u;
+}
+
 HA::ROP::ROP()
 {}
 
@@ -351,6 +361,8 @@ HAManager::HAManager()
     // We should only ever have at most two 
     // hearing aids so reserve capacity up front
     hearing_aids.reserve(max_num_ha);
+
+    cache.resize(ha_cache_size);
 }
 
 HA& HAManager::get_by_addr(bd_addr_t const addr)
@@ -394,6 +406,26 @@ bool HAManager::set_complete()
             (hearing_aids.size() == 2 && hearing_aids[0].rop.id == hearing_aids[1].rop.id);
 }
 
+HA& HAManager::get_from_cache(const bd_addr_t addr)
+{
+    for (auto& ha : cache) {
+        if (bd_addr_cmp(addr, ha.addr) == 0) {
+            return ha;
+        }
+    }
+    return invalid_ha;
+}
+
+void HAManager::add_to_cache(HA const& ha)
+{
+    if (!get_from_cache(ha.addr)) {
+        uint32_t i = cache_write_index & ha_cache_mask;
+        cache[i] = ha;
+        cache[i].reset_uncached_vars();
+        ++cache_write_index;
+    }
+}
+
 HA& HAManager::add(HA const& new_ha)
 {
     if (exists(new_ha)) {
@@ -415,6 +447,7 @@ HA& HAManager::add(HA const& new_ha)
         }
     }
     hearing_aids.emplace_back(new_ha);
+    add_to_cache(new_ha);
     assert(hearing_aids.size() <= 2);
     if (hearing_aids.size() == 2) {
         hearing_aids[0].other_ha = &hearing_aids[1];

src/hearing_aid.hpp

@@ -12,13 +12,18 @@ namespace asha
 
 constexpr size_t max_num_ha = 2;
 
+constexpr uint32_t ha_cache_size = 4ul;
+
+constexpr uint32_t ha_cache_mask = ha_cache_size - 1ul;
+
 class HA
 {
 public:
     enum class Side {Left = 0, Right = 1};
     enum class Mode {Mono = 0, Binaural = 1};
     enum class State {
         Invalid,
+        Cached,
         GATTConnected,
         L2ConnStart,
         L2ConnCompleted,
@@ -71,6 +76,8 @@ class HA
     bool is_creating_l2cap_channel();
     bool is_streaming_audio();
 
+    void reset_uncached_vars();
+
     // Current state of the hearing aid
     State state = State::Invalid;
 
@@ -160,11 +167,16 @@ class HAManager
     bool exists(HA const& ha);
     bool set_complete();
 
+    HA& get_from_cache(const bd_addr_t addr);
+    void add_to_cache(HA const& ha);
+
     HA& add(HA const& new_ha);
     void remove_by_conn_handle(hci_con_handle_t handle);
 
     std::vector<HA> hearing_aids;
 private:
+    std::vector<HA> cache;
+    uint32_t cache_write_index = 0;
     HA invalid_ha = {};
 };