Merge pull request #118 from JF002/improve-ble-connection

Improve ble connection

src/CMakeLists.txt

@@ -369,6 +369,7 @@ list(APPEND SOURCE_FILES
         components/ble/MusicService.cpp
         components/ble/BatteryInformationService.cpp
         components/ble/ImmediateAlertService.cpp
+        components/ble/ServiceDiscovery.cpp
         components/firmwarevalidator/FirmwareValidator.cpp
         drivers/Cst816s.cpp
         FreeRTOS/port.c
@@ -447,6 +448,8 @@ set(INCLUDE_FILES
         components/firmwarevalidator/FirmwareValidator.h
         components/ble/BatteryInformationService.h
         components/ble/ImmediateAlertService.h
+        components/ble/ServiceDiscovery.h
+        components/ble/BleClient.h
         drivers/Cst816s.h
         FreeRTOS/portmacro.h
         FreeRTOS/portmacro_cmsis.h

src/components/ble/AlertNotificationClient.cpp

@@ -3,38 +3,68 @@
 
 #include "AlertNotificationClient.h"
 
-
 using namespace Pinetime::Controllers;
 constexpr ble_uuid16_t AlertNotificationClient::ansServiceUuid;
-
 constexpr ble_uuid16_t AlertNotificationClient::supportedNewAlertCategoryUuid;
-constexpr ble_uuid16_t AlertNotificationClient::supportedUnreadAlertCategoryUuid ;
+constexpr ble_uuid16_t AlertNotificationClient::supportedUnreadAlertCategoryUuid;
 constexpr ble_uuid16_t AlertNotificationClient::newAlertUuid;
 constexpr ble_uuid16_t AlertNotificationClient::unreadAlertStatusUuid;
 constexpr ble_uuid16_t AlertNotificationClient::controlPointUuid;
 
-int Pinetime::Controllers::NewAlertSubcribeCallback(uint16_t conn_handle,
+namespace {
+  int
+  OnDiscoveryEventCallback(uint16_t conn_handle, const struct ble_gatt_error *error, const struct ble_gatt_svc *service,
+                           void *arg) {
+    auto client = static_cast<AlertNotificationClient *>(arg);
+    return client->OnDiscoveryEvent(conn_handle, error, service);
+  }
+
+  int OnAlertNotificationCharacteristicDiscoveredCallback(uint16_t conn_handle, const struct ble_gatt_error *error,
+                                                          const struct ble_gatt_chr *chr, void *arg) {
+    auto client = static_cast<AlertNotificationClient *>(arg);
+    return client->OnCharacteristicsDiscoveryEvent(conn_handle, error, chr);
+  }
+
+  int OnAlertNotificationDescriptorDiscoveryEventCallback(uint16_t conn_handle,
+                                                          const struct ble_gatt_error *error,
+                                                          uint16_t chr_val_handle,
+                                                          const struct ble_gatt_dsc *dsc,
+                                                          void *arg) {
+    auto client = static_cast<AlertNotificationClient *>(arg);
+    return client->OnDescriptorDiscoveryEventCallback(conn_handle, error, chr_val_handle, dsc);
+  }
+
+  int NewAlertSubcribeCallback(uint16_t conn_handle,
                                const struct ble_gatt_error *error,
                                struct ble_gatt_attr *attr,
                                void *arg) {
-  auto client = static_cast<AlertNotificationClient*>(arg);
+    auto client = static_cast<AlertNotificationClient *>(arg);
     return client->OnNewAlertSubcribe(conn_handle, error, attr);
+  }
 }
 
-AlertNotificationClient::AlertNotificationClient(Pinetime::System::SystemTask& systemTask,
+AlertNotificationClient::AlertNotificationClient(Pinetime::System::SystemTask &systemTask,
-        Pinetime::Controllers::NotificationManager& notificationManager) :
+                                                 Pinetime::Controllers::NotificationManager &notificationManager) :
-        systemTask{systemTask}, notificationManager{notificationManager}{
+        systemTask{systemTask}, notificationManager{notificationManager} {
-
 }
 
-bool AlertNotificationClient::OnDiscoveryEvent(uint16_t connectionHandle, const ble_gatt_error *error, const ble_gatt_svc *service) {
+bool AlertNotificationClient::OnDiscoveryEvent(uint16_t connectionHandle, const ble_gatt_error *error,
-  if(service == nullptr && error->status == BLE_HS_EDONE) {
+                                               const ble_gatt_svc *service) {
-    NRF_LOG_INFO("ANS Discovery complete");
+  if (service == nullptr && error->status == BLE_HS_EDONE) {
+    if (isDiscovered) {
+      NRF_LOG_INFO("ANS Discovery found, starting characteristics discovery");
+
+      ble_gattc_disc_all_chrs(connectionHandle, ansStartHandle, ansEndHandle,
+                              OnAlertNotificationCharacteristicDiscoveredCallback, this);
+    } else {
+      NRF_LOG_INFO("ANS not found");
+      onServiceDiscovered(connectionHandle);
+    }
     return true;
   }
 
-  if(service != nullptr && ble_uuid_cmp(((ble_uuid_t*)&ansServiceUuid), &service->uuid.u) == 0) {
+  if (service != nullptr && ble_uuid_cmp(((ble_uuid_t *) &ansServiceUuid), &service->uuid.u) == 0) {
-    NRF_LOG_INFO("ANS discovered : 0x%x", service->start_handle);
+    NRF_LOG_INFO("ANS discovered : 0x%x - 0x%x", service->start_handle, service->end_handle);
     ansStartHandle = service->start_handle;
     ansEndHandle = service->end_handle;
     isDiscovered = true;
@@ -44,43 +74,56 @@ bool AlertNotificationClient::OnDiscoveryEvent(uint16_t connectionHandle, const
 
 int AlertNotificationClient::OnCharacteristicsDiscoveryEvent(uint16_t connectionHandle, const ble_gatt_error *error,
                                                              const ble_gatt_chr *characteristic) {
-  if(error->status != 0 && error->status != BLE_HS_EDONE) {
+  if (error->status != 0 && error->status != BLE_HS_EDONE) {
     NRF_LOG_INFO("ANS Characteristic discovery ERROR");
+    onServiceDiscovered(connectionHandle);
     return 0;
   }
 
-  if(characteristic == nullptr && error->status == BLE_HS_EDONE) {
+  if (characteristic == nullptr && error->status == BLE_HS_EDONE) {
     NRF_LOG_INFO("ANS Characteristic discovery complete");
+    if (isCharacteristicDiscovered) {
+      ble_gattc_disc_all_dscs(connectionHandle,
+                              newAlertHandle, ansEndHandle,
+                              OnAlertNotificationDescriptorDiscoveryEventCallback, this);
+    } else
+      onServiceDiscovered(connectionHandle);
   } else {
-    if(characteristic != nullptr && ble_uuid_cmp(((ble_uuid_t*)&supportedNewAlertCategoryUuid), &characteristic->uuid.u) == 0) {
+    if (characteristic != nullptr &&
+        ble_uuid_cmp(((ble_uuid_t *) &supportedNewAlertCategoryUuid), &characteristic->uuid.u) == 0) {
       NRF_LOG_INFO("ANS Characteristic discovered : supportedNewAlertCategoryUuid");
       supportedNewAlertCategoryHandle = characteristic->val_handle;
-    } else if(characteristic != nullptr && ble_uuid_cmp(((ble_uuid_t*)&supportedUnreadAlertCategoryUuid), &characteristic->uuid.u) == 0) {
+    } else if (characteristic != nullptr &&
+               ble_uuid_cmp(((ble_uuid_t *) &supportedUnreadAlertCategoryUuid), &characteristic->uuid.u) == 0) {
       NRF_LOG_INFO("ANS Characteristic discovered : supportedUnreadAlertCategoryUuid");
       supportedUnreadAlertCategoryHandle = characteristic->val_handle;
-    } else if(characteristic != nullptr && ble_uuid_cmp(((ble_uuid_t*)&newAlertUuid), &characteristic->uuid.u) == 0) {
+    } else if (characteristic != nullptr &&
+               ble_uuid_cmp(((ble_uuid_t *) &newAlertUuid), &characteristic->uuid.u) == 0) {
       NRF_LOG_INFO("ANS Characteristic discovered : newAlertUuid");
       newAlertHandle = characteristic->val_handle;
       newAlertDefHandle = characteristic->def_handle;
-    } else if(characteristic != nullptr && ble_uuid_cmp(((ble_uuid_t*)&unreadAlertStatusUuid), &characteristic->uuid.u) == 0) {
+      isCharacteristicDiscovered = true;
+    } else if (characteristic != nullptr &&
+               ble_uuid_cmp(((ble_uuid_t *) &unreadAlertStatusUuid), &characteristic->uuid.u) == 0) {
       NRF_LOG_INFO("ANS Characteristic discovered : unreadAlertStatusUuid");
       unreadAlertStatusHandle = characteristic->val_handle;
-    } else if(characteristic != nullptr && ble_uuid_cmp(((ble_uuid_t*)&controlPointUuid), &characteristic->uuid.u) == 0) {
+    } else if (characteristic != nullptr &&
+               ble_uuid_cmp(((ble_uuid_t *) &controlPointUuid), &characteristic->uuid.u) == 0) {
       NRF_LOG_INFO("ANS Characteristic discovered : controlPointUuid");
       controlPointHandle = characteristic->val_handle;
-    }else
+    } else NRF_LOG_INFO("ANS Characteristic discovered : 0x%x", characteristic->val_handle);
-      NRF_LOG_INFO("ANS Characteristic discovered : 0x%x", characteristic->val_handle);
   }
   return 0;
 }
 
 int AlertNotificationClient::OnNewAlertSubcribe(uint16_t connectionHandle, const ble_gatt_error *error,
                                                 ble_gatt_attr *attribute) {
-  if(error->status == 0) {
+  if (error->status == 0) {
     NRF_LOG_INFO("ANS New alert subscribe OK");
   } else {
     NRF_LOG_INFO("ANS New alert subscribe ERROR");
   }
+  onServiceDiscovered(connectionHandle);
 
   return 0;
 }
@@ -88,35 +131,40 @@ int AlertNotificationClient::OnNewAlertSubcribe(uint16_t connectionHandle, const
 int AlertNotificationClient::OnDescriptorDiscoveryEventCallback(uint16_t connectionHandle, const ble_gatt_error *error,
                                                                 uint16_t characteristicValueHandle,
                                                                 const ble_gatt_dsc *descriptor) {
-  if(error->status == 0) {
+  if (error->status == 0) {
-    if(characteristicValueHandle == newAlertHandle && ble_uuid_cmp(((ble_uuid_t*)&newAlertUuid), &descriptor->uuid.u)) {
+    if (characteristicValueHandle == newAlertHandle &&
-      if(newAlertDescriptorHandle == 0) {
+        ble_uuid_cmp(((ble_uuid_t *) &newAlertUuid), &descriptor->uuid.u)) {
+      if (newAlertDescriptorHandle == 0) {
         NRF_LOG_INFO("ANS Descriptor discovered : %d", descriptor->handle);
         newAlertDescriptorHandle = descriptor->handle;
+        isDescriptorFound = true;
         uint8_t value[2];
         value[0] = 1;
         value[1] = 0;
         ble_gattc_write_flat(connectionHandle, newAlertDescriptorHandle, value, sizeof(value), NewAlertSubcribeCallback, this);
       }
     }
+  } else {
+    if (!isDescriptorFound)
+      onServiceDiscovered(connectionHandle);
   }
   return 0;
 }
 
 void AlertNotificationClient::OnNotification(ble_gap_event *event) {
-  if(event->notify_rx.attr_handle == newAlertHandle) {
+  if (event->notify_rx.attr_handle == newAlertHandle) {
     constexpr size_t stringTerminatorSize = 1; // end of string '\0'
     constexpr size_t headerSize = 3;
-    const auto maxMessageSize {NotificationManager::MaximumMessageSize()};
+    const auto maxMessageSize{NotificationManager::MaximumMessageSize()};
     const auto maxBufferSize{maxMessageSize + headerSize};
 
     const auto dbgPacketLen = OS_MBUF_PKTLEN(event->notify_rx.om);
     size_t bufferSize = min(dbgPacketLen + stringTerminatorSize, maxBufferSize);
-    auto messageSize = min(maxMessageSize, (bufferSize-headerSize));
+    auto messageSize = min(maxMessageSize, (bufferSize - headerSize));
 
     NotificationManager::Notification notif;
-    os_mbuf_copydata(event->notify_rx.om, headerSize, messageSize-1, notif.message.data());
+    os_mbuf_copydata(event->notify_rx.om, headerSize, messageSize - 1, notif.message.data());
-    notif.message[messageSize-1] = '\0';
+    notif.message[messageSize - 1] = '\0';
     notif.category = Pinetime::Controllers::NotificationManager::Categories::SimpleAlert;
     notificationManager.Push(std::move(notif));
 
@@ -124,18 +172,23 @@ void AlertNotificationClient::OnNotification(ble_gap_event *event) {
   }
 }
 
-bool AlertNotificationClient::IsDiscovered() const {
+void AlertNotificationClient::Reset() {
-  return isDiscovered;
+  ansStartHandle = 0;
+  ansEndHandle = 0;
+  supportedNewAlertCategoryHandle = 0;
+  supportedUnreadAlertCategoryHandle = 0;
+  newAlertHandle = 0;
+  newAlertDescriptorHandle = 0;
+  newAlertDefHandle = 0;
+  unreadAlertStatusHandle = 0;
+  controlPointHandle = 0;
+  isDiscovered = false;
+  isCharacteristicDiscovered = false;
+  isDescriptorFound = false;
 }
 
-uint16_t AlertNotificationClient::StartHandle() const {
+void AlertNotificationClient::Discover(uint16_t connectionHandle, std::function<void(uint16_t)> onServiceDiscovered) {
-  return ansStartHandle;
+  NRF_LOG_INFO("[ANS] Starting discovery");
-}
+  this->onServiceDiscovered = onServiceDiscovered;
-
+  ble_gattc_disc_svc_by_uuid(connectionHandle, &ansServiceUuid.u, OnDiscoveryEventCallback, this);
-uint16_t AlertNotificationClient::EndHandle() const {
-  return ansEndHandle;
-}
-
-uint16_t AlertNotificationClient::NewAlerthandle() const {
-  return newAlertHandle;
 }

src/components/ble/AlertNotificationClient.h

@@ -3,16 +3,12 @@
 #include <cstdint>
 #include <array>
 #include <host/ble_gap.h>
+#include "BleClient.h"
 
 
 namespace Pinetime {
   namespace Controllers {
-    int NewAlertSubcribeCallback(uint16_t conn_handle,
+    class AlertNotificationClient : public BleClient {
-                                 const struct ble_gatt_error *error,
-                                 struct ble_gatt_attr *attr,
-                                 void *arg);
-
-    class AlertNotificationClient {
       public:
         explicit AlertNotificationClient(Pinetime::System::SystemTask &systemTask,
                                          Pinetime::Controllers::NotificationManager &notificationManager);
@@ -24,13 +20,9 @@ namespace Pinetime {
         int OnDescriptorDiscoveryEventCallback(uint16_t connectionHandle, const ble_gatt_error *error,
                                                uint16_t characteristicValueHandle, const ble_gatt_dsc *descriptor);
         void OnNotification(ble_gap_event *event);
-        bool IsDiscovered() const;
+        void Reset();
-        uint16_t StartHandle() const;
+        void Discover(uint16_t connectionHandle, std::function<void(uint16_t)> lambda) override;
-        uint16_t EndHandle() const;
 
-        static constexpr const ble_uuid16_t &Uuid() { return ansServiceUuid; }
-
-        uint16_t NewAlerthandle() const;
       private:
         static constexpr uint16_t ansServiceId{0x1811};
         static constexpr uint16_t supportedNewAlertCategoryId = 0x2a47;
@@ -64,18 +56,21 @@ namespace Pinetime {
                 .value = controlPointId
         };
 
-        uint16_t ansStartHandle;
+        uint16_t ansStartHandle = 0;
-        uint16_t ansEndHandle;
+        uint16_t ansEndHandle = 0;
-        uint16_t supportedNewAlertCategoryHandle;
+        uint16_t supportedNewAlertCategoryHandle = 0;
-        uint16_t supportedUnreadAlertCategoryHandle;
+        uint16_t supportedUnreadAlertCategoryHandle = 0;
-        uint16_t newAlertHandle;
+        uint16_t newAlertHandle = 0;
         uint16_t newAlertDescriptorHandle = 0;
-        uint16_t newAlertDefHandle;
+        uint16_t newAlertDefHandle = 0;
-        uint16_t unreadAlertStatusHandle;
+        uint16_t unreadAlertStatusHandle = 0;
-        uint16_t controlPointHandle;
+        uint16_t controlPointHandle = 0;
         bool isDiscovered = false;
         Pinetime::System::SystemTask &systemTask;
         Pinetime::Controllers::NotificationManager &notificationManager;
+        std::function<void(uint16_t)> onServiceDiscovered;
+        bool isCharacteristicDiscovered = false;
+        bool isDescriptorFound = false;
     };
   }
 }

src/components/ble/BleClient.h

@@ -0,0 +1,12 @@
+#pragma once
+
+#include <functional>
+
+namespace Pinetime {
+  namespace Controllers{
+    class BleClient {
+      public:
+        virtual void Discover(uint16_t connectionHandle, std::function<void(uint16_t)> lambda) = 0;
+    };
+  }
+}

src/components/ble/CurrentTimeClient.cpp

@@ -6,7 +6,25 @@ using namespace Pinetime::Controllers;
 constexpr ble_uuid16_t CurrentTimeClient::ctsServiceUuid;
 constexpr ble_uuid16_t CurrentTimeClient::currentTimeCharacteristicUuid;
 
-CurrentTimeClient::CurrentTimeClient(DateTime& dateTimeController) : dateTimeController{dateTimeController} {
+namespace {
+  int OnDiscoveryEventCallback(uint16_t conn_handle, const struct ble_gatt_error *error, const struct ble_gatt_svc *service, void *arg) {
+    auto client = static_cast<CurrentTimeClient *>(arg);
+    return client->OnDiscoveryEvent(conn_handle, error, service);
+  }
+
+  int OnCurrentTimeCharacteristicDiscoveredCallback(uint16_t conn_handle, const struct ble_gatt_error *error,
+                                                    const struct ble_gatt_chr *chr, void *arg) {
+    auto client = static_cast<CurrentTimeClient *>(arg);
+    return client->OnCharacteristicDiscoveryEvent(conn_handle, error, chr);
+  }
+
+  int CurrentTimeReadCallback(uint16_t conn_handle, const struct ble_gatt_error *error, struct ble_gatt_attr *attr, void *arg) {
+    auto client = static_cast<CurrentTimeClient *>(arg);
+    return client->OnCurrentTimeReadResult(conn_handle, error, attr);
+  }
+}
+
+CurrentTimeClient::CurrentTimeClient(DateTime &dateTimeController) : dateTimeController{dateTimeController} {
 
 }
 
@@ -14,14 +32,23 @@ void CurrentTimeClient::Init() {
 
 }
 
-bool CurrentTimeClient::OnDiscoveryEvent(uint16_t connectionHandle, const ble_gatt_error *error, const ble_gatt_svc *service) {
+bool CurrentTimeClient::OnDiscoveryEvent(uint16_t connectionHandle, const ble_gatt_error *error,
-    if(service == nullptr && error->status == BLE_HS_EDONE) {
+                                         const ble_gatt_svc *service) {
-        NRF_LOG_INFO("CTS Discovery complete");
+  if (service == nullptr && error->status == BLE_HS_EDONE) {
+    if (isDiscovered) {
+      NRF_LOG_INFO("CTS found, starting characteristics discovery");
+
+      ble_gattc_disc_all_chrs(connectionHandle, ctsStartHandle, ctsEndHandle,
+                              OnCurrentTimeCharacteristicDiscoveredCallback, this);
+    } else {
+      NRF_LOG_INFO("CTS not found");
+      onServiceDiscovered(connectionHandle);
+    }
     return true;
   }
 
-    if(service != nullptr && ble_uuid_cmp(((ble_uuid_t*)&ctsServiceUuid), &service->uuid.u) == 0) {
+  if (service != nullptr && ble_uuid_cmp(((ble_uuid_t *) &ctsServiceUuid), &service->uuid.u) == 0) {
-        NRF_LOG_INFO("CTS discovered : 0x%x",  service->start_handle);
+    NRF_LOG_INFO("CTS discovered : 0x%x - 0x%x", service->start_handle, service->end_handle);
     isDiscovered = true;
     ctsStartHandle = service->start_handle;
     ctsEndHandle = service->end_handle;
@@ -32,20 +59,30 @@ bool CurrentTimeClient::OnDiscoveryEvent(uint16_t connectionHandle, const ble_ga
 
 int CurrentTimeClient::OnCharacteristicDiscoveryEvent(uint16_t conn_handle, const ble_gatt_error *error,
                                                       const ble_gatt_chr *characteristic) {
-    if(characteristic == nullptr && error->status == BLE_HS_EDONE) {
+  if (characteristic == nullptr && error->status == BLE_HS_EDONE) {
-        NRF_LOG_INFO("CTS Characteristic discovery complete");
+    if (isCharacteristicDiscovered) {
+      NRF_LOG_INFO("CTS Characteristic discovery complete, fetching time");
+      ble_gattc_read(conn_handle, currentTimeHandle, CurrentTimeReadCallback, this);
+    } else {
+      NRF_LOG_INFO("CTS Characteristic discovery unsuccessful");
+      onServiceDiscovered(conn_handle);
+    }
+
     return 0;
   }
 
-    if(characteristic != nullptr && ble_uuid_cmp(((ble_uuid_t*)&currentTimeCharacteristicUuid), &characteristic->uuid.u) == 0) {
+  if (characteristic != nullptr &&
+      ble_uuid_cmp(((ble_uuid_t *) &currentTimeCharacteristicUuid), &characteristic->uuid.u) == 0) {
     NRF_LOG_INFO("CTS Characteristic discovered : 0x%x", characteristic->val_handle);
+    isCharacteristicDiscovered = true;
     currentTimeHandle = characteristic->val_handle;
   }
   return 0;
 }
 
-int CurrentTimeClient::OnCurrentTimeReadResult(uint16_t conn_handle, const ble_gatt_error *error, const ble_gatt_attr *attribute) {
+int CurrentTimeClient::OnCurrentTimeReadResult(uint16_t conn_handle, const ble_gatt_error *error,
-    if(error->status == 0) {
+                                               const ble_gatt_attr *attribute) {
+  if (error->status == 0) {
     // TODO check that attribute->handle equals the handle discovered in OnCharacteristicDiscoveryEvent
     CtsData result;
     os_mbuf_copydata(attribute->om, 0, sizeof(CtsData), &result);
@@ -57,21 +94,18 @@ int CurrentTimeClient::OnCurrentTimeReadResult(uint16_t conn_handle, const ble_g
   } else {
     NRF_LOG_INFO("Error retrieving current time: %d", error->status);
   }
+
+  onServiceDiscovered(conn_handle);
   return 0;
 }
 
-bool CurrentTimeClient::IsDiscovered() const {
+void CurrentTimeClient::Reset() {
-    return isDiscovered;
+  isDiscovered = false;
+  isCharacteristicDiscovered = false;
 }
 
-uint16_t CurrentTimeClient::StartHandle() const {
+void CurrentTimeClient::Discover(uint16_t connectionHandle, std::function<void(uint16_t)> onServiceDiscovered) {
-    return ctsStartHandle;
+  NRF_LOG_INFO("[CTS] Starting discovery");
-}
+  this->onServiceDiscovered = onServiceDiscovered;
-
+  ble_gattc_disc_svc_by_uuid(connectionHandle, &ctsServiceUuid.u, OnDiscoveryEventCallback, this);
-uint16_t CurrentTimeClient::EndHandle() const {
-    return ctsEndHandle;
-}
-
-uint16_t CurrentTimeClient::CurrentTimeHandle() const {
-    return currentTimeHandle;
 }

src/components/ble/CurrentTimeClient.h

@@ -3,25 +3,25 @@
 #include <array>
 
 #include "components/datetime/DateTimeController.h"
+#include "BleClient.h"
 #include <host/ble_gap.h>
 
 namespace Pinetime {
     namespace Controllers {
 
-        class CurrentTimeClient {
+        class CurrentTimeClient : public BleClient {
         public:
             explicit CurrentTimeClient(DateTime& dateTimeController);
             void Init();
+            void Reset();
             bool OnDiscoveryEvent(uint16_t connectionHandle, const ble_gatt_error *error, const ble_gatt_svc *service);
             int OnCharacteristicDiscoveryEvent(uint16_t conn_handle, const ble_gatt_error *error,
                                                const ble_gatt_chr *characteristic);
             int OnCurrentTimeReadResult(uint16_t conn_handle, const ble_gatt_error *error, const ble_gatt_attr *attribute);
-            bool IsDiscovered() const;
-            uint16_t StartHandle() const;
-            uint16_t EndHandle() const;
-            uint16_t CurrentTimeHandle() const;
             static constexpr const ble_uuid16_t* Uuid() { return &CurrentTimeClient::ctsServiceUuid; }
             static constexpr const ble_uuid16_t* CurrentTimeCharacteristicUuid() { return &CurrentTimeClient::currentTimeCharacteristicUuid; }
+            void Discover(uint16_t connectionHandle, std::function<void(uint16_t)> lambda) override;
+
           private:
             typedef struct __attribute__((packed)) {
                 uint16_t year;
@@ -46,11 +46,14 @@ namespace Pinetime {
                     .value = currentTimeCharacteristicId
             };
 
-            uint16_t currentTimeHandle;
             DateTime& dateTimeController;
             bool isDiscovered = false;
             uint16_t ctsStartHandle;
             uint16_t ctsEndHandle;
+
+            bool isCharacteristicDiscovered = false;
+            uint16_t currentTimeHandle;
+            std::function<void(uint16_t)> onServiceDiscovered;
         };
     }
 }

src/components/ble/NimbleController.cpp

@@ -1,10 +1,7 @@
-
 #include "components/datetime/DateTimeController.h"
-
 #include <systemtask/SystemTask.h>
 #include "components/ble/NotificationManager.h"
 #include <hal/nrf_rtc.h>
-
 #include "NimbleController.h"
 #include "MusicService.h"
 #include <services/gatt/ble_svc_gatt.h>
@@ -14,14 +11,8 @@
 #include <host/ble_hs.h>
 #include <host/ble_gap.h>
 
-
-
 using namespace Pinetime::Controllers;
 
-// TODO I'm not satisfied by how this code looks like (AlertNotificationClient and CurrentTimeClient must
-// expose too much data, too many callbacks -> NimbleController -> CTS/ANS client.
-// Let's try to improve this code (and keep it working!)
-
 NimbleController::NimbleController(Pinetime::System::SystemTask& systemTask,
                                    Pinetime::Controllers::Ble& bleController,
         DateTime& dateTimeController,
@@ -40,8 +31,8 @@ NimbleController::NimbleController(Pinetime::System::SystemTask& systemTask,
         currentTimeService{dateTimeController},
         musicService{systemTask},
         batteryInformationService{batteryController},
-        immediateAlertService{systemTask, notificationManager} {
+        immediateAlertService{systemTask, notificationManager},
-
+        serviceDiscovery({&currentTimeClient, &alertNotificationClient}) {
 }
 
 int GAPEventCallback(struct ble_gap_event *event, void *arg) {
@@ -49,33 +40,6 @@ int GAPEventCallback(struct ble_gap_event *event, void *arg) {
   return nimbleController->OnGAPEvent(event);
 }
 
-int CurrentTimeCharacteristicDiscoveredCallback(uint16_t conn_handle, const struct ble_gatt_error *error,
-                                                const struct ble_gatt_chr *chr, void *arg) {
-  auto client = static_cast<NimbleController*>(arg);
-  return client->OnCTSCharacteristicDiscoveryEvent(conn_handle, error, chr);
-}
-
-int AlertNotificationCharacteristicDiscoveredCallback(uint16_t conn_handle, const struct ble_gatt_error *error,
-                                                const struct ble_gatt_chr *chr, void *arg) {
-  auto client = static_cast<NimbleController*>(arg);
-  return client->OnANSCharacteristicDiscoveryEvent(conn_handle, error, chr);
-}
-
-int CurrentTimeReadCallback(uint16_t conn_handle, const struct ble_gatt_error *error,
-                                   struct ble_gatt_attr *attr, void *arg) {
-  auto client = static_cast<NimbleController*>(arg);
-  return client->OnCurrentTimeReadResult(conn_handle, error, attr);
-}
-
-int AlertNotificationDescriptorDiscoveryEventCallback(uint16_t conn_handle,
-                                                                             const struct ble_gatt_error *error,
-                                                                             uint16_t chr_val_handle,
-                                                                             const struct ble_gatt_dsc *dsc,
-                                                                             void *arg) {
-  auto client = static_cast<NimbleController*>(arg);
-  return client->OnANSDescriptorDiscoveryEventCallback(conn_handle, error, chr_val_handle, dsc);
-}
-
 void NimbleController::Init() {
   while (!ble_hs_synced()) {}
 
@@ -108,7 +72,7 @@ void NimbleController::Init() {
 }
 
 void NimbleController::StartAdvertising() {
-  if(ble_gap_adv_active()) return;
+  if(bleController.IsConnected() || ble_gap_conn_active() || ble_gap_adv_active()) return;
 
   ble_svc_gap_device_name_set(deviceName);
 
@@ -158,15 +122,6 @@ void NimbleController::StartAdvertising() {
   // the application has been woken up, for example.
 }
 
-int OnAllSvrDisco(uint16_t conn_handle,
-                                 const struct ble_gatt_error *error,
-                                 const struct ble_gatt_svc *service,
-                                 void *arg) {
-  auto nimbleController = static_cast<NimbleController*>(arg);
-  return nimbleController->OnDiscoveryEvent(conn_handle, error, service);
-  return 0;
-}
-
 int NimbleController::OnGAPEvent(ble_gap_event *event) {
   switch (event->type) {
     case BLE_GAP_EVENT_ADV_COMPLETE:
@@ -197,6 +152,8 @@ int NimbleController::OnGAPEvent(ble_gap_event *event) {
       NRF_LOG_INFO("disconnect; reason=%d", event->disconnect.reason);
 
       /* Connection terminated; resume advertising. */
+      currentTimeClient.Reset();
+      alertNotificationClient.Reset();
       connectionHandle = BLE_HS_CONN_HANDLE_NONE;
       bleController.Disconnect();
       StartAdvertising();
@@ -269,65 +226,8 @@ int NimbleController::OnGAPEvent(ble_gap_event *event) {
   return 0;
 }
 
-int NimbleController::OnDiscoveryEvent(uint16_t i, const ble_gatt_error *error, const ble_gatt_svc *service) {
-  if(service == nullptr && error->status == BLE_HS_EDONE) {
-    NRF_LOG_INFO("Service Discovery complete");
-    if(currentTimeClient.IsDiscovered()) {
-      ble_gattc_disc_all_chrs(connectionHandle, currentTimeClient.StartHandle(), currentTimeClient.EndHandle(),
-                              CurrentTimeCharacteristicDiscoveredCallback, this);
-
-    } else if(alertNotificationClient.IsDiscovered()) {
-      ble_gattc_disc_all_chrs(connectionHandle, alertNotificationClient.StartHandle(), alertNotificationClient.EndHandle(),
-                              AlertNotificationCharacteristicDiscoveredCallback, this);
-    }
-  }
-
-  alertNotificationClient.OnDiscoveryEvent(i, error, service);
-  currentTimeClient.OnDiscoveryEvent(i, error, service);
-  return 0;
-}
-
-int NimbleController::OnCTSCharacteristicDiscoveryEvent(uint16_t connectionHandle, const ble_gatt_error *error,
-                                                        const ble_gatt_chr *characteristic) {
-  if(characteristic == nullptr && error->status == BLE_HS_EDONE) {
-    NRF_LOG_INFO("CTS characteristic Discovery complete");
-    ble_gattc_read(connectionHandle, currentTimeClient.CurrentTimeHandle(), CurrentTimeReadCallback, this);
-    return 0;
-  }
-  return currentTimeClient.OnCharacteristicDiscoveryEvent(connectionHandle, error, characteristic);
-}
-
-int NimbleController::OnANSCharacteristicDiscoveryEvent(uint16_t connectionHandle, const ble_gatt_error *error,
-                                                        const ble_gatt_chr *characteristic) {
-  if(characteristic == nullptr && error->status == BLE_HS_EDONE) {
-    NRF_LOG_INFO("ANS characteristic Discovery complete");
-    ble_gattc_disc_all_dscs(connectionHandle,
-            alertNotificationClient.NewAlerthandle(), alertNotificationClient.EndHandle(),
-            AlertNotificationDescriptorDiscoveryEventCallback, this);
-    return 0;
-  }
-  return alertNotificationClient.OnCharacteristicsDiscoveryEvent(connectionHandle, error, characteristic);
-}
-
-int NimbleController::OnCurrentTimeReadResult(uint16_t connectionHandle, const ble_gatt_error *error, ble_gatt_attr *attribute) {
-  currentTimeClient.OnCurrentTimeReadResult(connectionHandle, error, attribute);
-
-  if (alertNotificationClient.IsDiscovered()) {
-    ble_gattc_disc_all_chrs(connectionHandle, alertNotificationClient.StartHandle(),
-                            alertNotificationClient.EndHandle(),
-                            AlertNotificationCharacteristicDiscoveredCallback, this);
-  }
-  return 0;
-}
-
-int NimbleController::OnANSDescriptorDiscoveryEventCallback(uint16_t connectionHandle, const ble_gatt_error *error,
-                                                            uint16_t characteristicValueHandle,
-                                                            const ble_gatt_dsc *descriptor) {
-  return alertNotificationClient.OnDescriptorDiscoveryEventCallback(connectionHandle, error, characteristicValueHandle, descriptor);
-}
-
 void NimbleController::StartDiscovery() {
-  ble_gattc_disc_all_svcs(connectionHandle, OnAllSvrDisco, this);
+  serviceDiscovery.StartDiscovery(connectionHandle);
 }
 
 

src/components/ble/NimbleController.h

@@ -11,6 +11,7 @@
 #include "MusicService.h"
 #include "BatteryInformationService.h"
 #include "ImmediateAlertService.h"
+#include "ServiceDiscovery.h"
 #include <host/ble_gap.h>
 
 namespace Pinetime {
@@ -71,6 +72,8 @@ namespace Pinetime {
                 .value = {0x23, 0xD1, 0xBC, 0xEA, 0x5F, 0x78, 0x23, 0x15,
                           0xDE, 0xEF, 0x12, 0x12, 0x30, 0x15, 0x00, 0x00}
         };
+
+        ServiceDiscovery serviceDiscovery;
     };
   }
 }

src/components/ble/ServiceDiscovery.cpp

@@ -0,0 +1,31 @@
+#include <libraries/log/nrf_log.h>
+#include "ServiceDiscovery.h"
+using namespace Pinetime::Controllers;
+
+ServiceDiscovery::ServiceDiscovery(std::array<BleClient*, 2>&& clients) : clients{clients} {
+
+}
+
+void ServiceDiscovery::StartDiscovery(uint16_t connectionHandle) {
+  NRF_LOG_INFO("[Discovery] Starting discovery");
+  clientIterator = clients.begin();
+  DiscoverNextService(connectionHandle);
+}
+
+void ServiceDiscovery::OnServiceDiscovered(uint16_t connectionHandle) {
+  clientIterator++;
+  if(clientIterator != clients.end()) {
+    DiscoverNextService(connectionHandle);
+  } else {
+    NRF_LOG_INFO("End of service discovery");
+  }
+}
+
+void ServiceDiscovery::DiscoverNextService(uint16_t connectionHandle) {
+  NRF_LOG_INFO("[Discovery] Discover next service");
+
+  auto discoverNextService = [this](uint16_t connectionHandle){
+    this->OnServiceDiscovered(connectionHandle);
+  };
+  (*clientIterator)->Discover(connectionHandle, discoverNextService);
+}

src/components/ble/ServiceDiscovery.h

@@ -0,0 +1,24 @@
+#pragma once
+
+#include <array>
+#include <functional>
+#include <memory>
+#include "BleClient.h"
+
+namespace Pinetime {
+  namespace Controllers {
+    class ServiceDiscovery {
+      public:
+        ServiceDiscovery(std::array<BleClient*, 2>&& bleClients);
+
+        void StartDiscovery(uint16_t connectionHandle);
+
+
+      private:
+        BleClient** clientIterator;
+        std::array<BleClient*, 2> clients;
+        void OnServiceDiscovered(uint16_t connectionHandle);
+        void DiscoverNextService(uint16_t connectionHandle);
+    };
+  }
+}