NimbleController : CTS & ANS working but not at the same time (conflict during discovery)

8 files changed, 306 insertions, 14 deletions

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index fc22c9a9..93878e85 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -319,6 +319,7 @@ list(APPEND SOURCE_FILES
   Components/Ble/NimbleController.cpp
   Components/Ble/DeviceInformationService.cpp
   Components/Ble/CurrentTimeClient.cpp
+  Components/Ble/AlertNotificationClient.cpp
   drivers/Cst816s.cpp
   FreeRTOS/port.c
   FreeRTOS/port_cmsis_systick.c
@@ -366,6 +367,7 @@ set(INCLUDE_FILES
   Components/Ble/NimbleController.h
   Components/Ble/DeviceInformationService.h
   Components/Ble/CurrentTimeClient.h
+  Components/Ble/AlertNotificationClient.h
   drivers/Cst816s.h
   FreeRTOS/portmacro.h
   FreeRTOS/portmacro_cmsis.h
diff --git a/src/Components/Ble/AlertNotificationClient.cpp b/src/Components/Ble/AlertNotificationClient.cpp
new file mode 100644
index 00000000..e6683e99
--- /dev/null
+++ b/src/Components/Ble/AlertNotificationClient.cpp
@@ -0,0 +1,149 @@
+#include <SystemTask/SystemTask.h>
+#include "NotificationManager.h"
+
+#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::newAlertUuid;
+constexpr ble_uuid16_t AlertNotificationClient::unreadAlertStatusUuid;
+constexpr ble_uuid16_t AlertNotificationClient::controlPointUuid;
+
+int Pinetime::Controllers::AlertNotificationDiscoveryEventCallback(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 Pinetime::Controllers::AlertNotificationCharacteristicsDiscoveryEventCallback(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 Pinetime::Controllers::NewAlertSubcribeCallback(uint16_t conn_handle,
+                     const struct ble_gatt_error *error,
+                     struct ble_gatt_attr *attr,
+                     void *arg) {
+  auto client = static_cast<AlertNotificationClient*>(arg);
+  return client->OnNewAlertSubcribe(conn_handle, error, attr);
+}
+
+int Pinetime::Controllers::AlertNotificationDescriptorDiscoveryEventCallback(uint16_t conn_handle,
+                                                                             const struct ble_gatt_error *error,
+                                                                             uint16_t chr_val_handle,
+                                                                             const struct ble_gatt_dsc *dsc,
+                                                                             void *arg) {
+  NRF_LOG_INFO("ANS VCS");
+  auto client = static_cast<AlertNotificationClient*>(arg);
+  return client->OnDescriptorDiscoveryEventCallback(conn_handle, error, chr_val_handle, dsc);
+}
+
+AlertNotificationClient::AlertNotificationClient(Pinetime::System::SystemTask& systemTask,
+        Pinetime::Controllers::NotificationManager& notificationManager) :
+        systemTask{systemTask}, notificationManager{notificationManager}{
+
+}
+
+void AlertNotificationClient::StartDiscovery(uint16_t connectionHandle) {
+  ble_gattc_disc_svc_by_uuid(connectionHandle, ((ble_uuid_t*)&ansServiceUuid), AlertNotificationDiscoveryEventCallback, this);
+}
+
+bool AlertNotificationClient::OnDiscoveryEvent(uint16_t connectionHandle, const ble_gatt_error *error, const ble_gatt_svc *service) {
+  if(service == nullptr && error->status == BLE_HS_EDONE) {
+    NRF_LOG_INFO("ANS Discovery complete");
+    ble_gattc_disc_all_dscs(connectionHandle, newAlertHandle, ansEndHandle, AlertNotificationDescriptorDiscoveryEventCallback, this);
+    return true;
+  }
+
+  if(service != nullptr && ble_uuid_cmp(((ble_uuid_t*)&ansServiceUuid), &service->uuid.u) == 0) {
+    NRF_LOG_INFO("ANS discovered : 0x%x", service->start_handle);
+    ble_gattc_disc_all_chrs(connectionHandle, service->start_handle, service->end_handle, AlertNotificationCharacteristicsDiscoveryEventCallback, this);
+    ansEndHandle = service->end_handle;
+  }
+  return false;
+}
+
+void AlertNotificationClient::Init() {
+
+}
+
+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) {
+    NRF_LOG_INFO("ANS Characteristic discovery ERROR");
+    return 0;
+  }
+
+  if(characteristic == nullptr && error->status == BLE_HS_EDONE) {
+    NRF_LOG_INFO("ANS Characteristic discovery complete");
+  } else {
+    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) {
+      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) {
+      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) {
+      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) {
+      NRF_LOG_INFO("ANS Characteristic discovered : controlPointUuid");
+      controlPointHandle = characteristic->val_handle;
+    }else
+      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) {
+    NRF_LOG_INFO("ANS New alert subscribe OK");
+  } else {
+    NRF_LOG_INFO("ANS New alert subscribe ERROR");
+  }
+
+  return 0;
+}
+
+int AlertNotificationClient::OnDescriptorDiscoveryEventCallback(uint16_t connectionHandle, const ble_gatt_error *error,
+                                                                uint16_t characteristicValueHandle,
+                                                                const ble_gatt_dsc *descriptor) {
+  if(error->status == 0) {
+    if(characteristicValueHandle == newAlertHandle && 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;
+        uint8_t value[2];
+        value[0] = 1;
+        value[1] = 0;
+        ble_gattc_write_flat(connectionHandle, newAlertDescriptorHandle, value, sizeof(value), NewAlertSubcribeCallback, this);
+      }
+    }
+  }
+  return 0;
+}
+
+void AlertNotificationClient::OnNotification(ble_gap_event *event) {
+  if(event->notify_rx.attr_handle == newAlertHandle) {
+    size_t notifSize = OS_MBUF_PKTLEN(event->notify_rx.om);
+    uint8_t data[notifSize + 1];
+    data[notifSize] = '\0';
+    os_mbuf_copydata(event->notify_rx.om, 0, notifSize, data);
+    char *s = (char *) &data[2];
+    NRF_LOG_INFO("DATA : %s", s);
+
+    notificationManager.Push(Pinetime::Controllers::NotificationManager::Categories::SimpleAlert, s, notifSize + 1);
+    systemTask.PushMessage(Pinetime::System::SystemTask::Messages::OnNewNotification);
+  }
+}
diff --git a/src/Components/Ble/AlertNotificationClient.h b/src/Components/Ble/AlertNotificationClient.h
new file mode 100644
index 00000000..595dbe37
--- /dev/null
+++ b/src/Components/Ble/AlertNotificationClient.h
@@ -0,0 +1,88 @@
+#pragma once
+#include <cstdint>
+#include <array>
+#include <host/ble_gap.h>
+
+
+namespace Pinetime {
+  namespace Controllers {
+    int AlertNotificationDiscoveryEventCallback(uint16_t conn_handle, const struct ble_gatt_error *error,
+                                                                       const struct ble_gatt_svc *service, void *arg);
+    int AlertNotificationCharacteristicsDiscoveryEventCallback(uint16_t conn_handle,
+                                                              const struct ble_gatt_error *error,
+                                                              const struct ble_gatt_chr *chr, void *arg);
+    int NewAlertSubcribeCallback(uint16_t conn_handle,
+                                                        const struct ble_gatt_error *error,
+                                                        struct ble_gatt_attr *attr,
+                                                        void *arg);
+
+    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);
+
+    class AlertNotificationClient {
+      public:
+        explicit AlertNotificationClient(Pinetime::System::SystemTask& systemTask, Pinetime::Controllers::NotificationManager& notificationManager);
+        void Init();
+
+
+        void StartDiscovery(uint16_t connectionHandle);
+        bool OnDiscoveryEvent(uint16_t connectionHandle, const ble_gatt_error *error, const ble_gatt_svc *service);
+        int OnCharacteristicsDiscoveryEvent(uint16_t connectionHandle, const ble_gatt_error *error,
+                                            const ble_gatt_chr *characteristic);
+        int OnNewAlertSubcribe(uint16_t connectionHandle, const ble_gatt_error *error, ble_gatt_attr *attribute);
+        int OnDescriptorDiscoveryEventCallback(uint16_t connectionHandle, const ble_gatt_error *error,
+                                               uint16_t characteristicValueHandle, const ble_gatt_dsc *descriptor);
+        void OnNotification(ble_gap_event *event);
+      private:
+        static constexpr uint16_t ansServiceId {0x1811};
+        static constexpr uint16_t supportedNewAlertCategoryId = 0x2a47;
+        static constexpr uint16_t supportedUnreadAlertCategoryId = 0x2a48;
+        static constexpr uint16_t newAlertId = 0x2a46;
+        static constexpr uint16_t unreadAlertStatusId = 0x2a45;
+        static constexpr uint16_t controlPointId = 0x2a44;
+
+        static constexpr ble_uuid16_t ansServiceUuid {
+                .u { .type = BLE_UUID_TYPE_16 },
+                .value = ansServiceId
+        };
+        static constexpr ble_uuid16_t supportedNewAlertCategoryUuid {
+                .u { .type = BLE_UUID_TYPE_16 },
+                .value = supportedNewAlertCategoryId
+        };
+        static constexpr ble_uuid16_t supportedUnreadAlertCategoryUuid {
+                .u { .type = BLE_UUID_TYPE_16 },
+                .value = supportedUnreadAlertCategoryId
+        };
+        static constexpr ble_uuid16_t newAlertUuid {
+                .u { .type = BLE_UUID_TYPE_16 },
+                .value = newAlertId
+        };
+        static constexpr ble_uuid16_t unreadAlertStatusUuid {
+                .u { .type = BLE_UUID_TYPE_16 },
+                .value = unreadAlertStatusId
+        };
+        static constexpr ble_uuid16_t controlPointUuid {
+                .u { .type = BLE_UUID_TYPE_16 },
+                .value = controlPointId
+        };
+
+        uint16_t ansEndHandle;
+        uint16_t supportedNewAlertCategoryHandle;
+        uint16_t supportedUnreadAlertCategoryHandle;
+        uint16_t newAlertHandle;
+        uint16_t newAlertDescriptorHandle = 0;
+        uint16_t newAlertDefHandle;
+        uint16_t unreadAlertStatusHandle;
+        uint16_t controlPointHandle;
+        bool discoveryDone = false;
+        Pinetime::System::SystemTask& systemTask;
+        Pinetime::Controllers::NotificationManager& notificationManager;
+
+    };
+
+
+  }
+}
\ No newline at end of file
diff --git a/src/Components/Ble/CurrentTimeClient.cpp b/src/Components/Ble/CurrentTimeClient.cpp
index 524c37d5..44065bce 100644
--- a/src/Components/Ble/CurrentTimeClient.cpp
+++ b/src/Components/Ble/CurrentTimeClient.cpp
@@ -37,21 +37,23 @@ void CurrentTimeClient::StartDiscovery(uint16_t connectionHandle) {
   ble_gattc_disc_svc_by_uuid(connectionHandle, ((ble_uuid_t*)&ctsServiceUuid), CurrentTimeDiscoveryEventCallback, this);
 }
 
-int CurrentTimeClient::OnDiscoveryEvent(uint16_t connectionHandle, const ble_gatt_error *error, const ble_gatt_svc *service) {
-  if(service == nullptr && error->status == BLE_HS_EDONE)
-    NRF_LOG_INFO("Discovery complete");
+bool CurrentTimeClient::OnDiscoveryEvent(uint16_t connectionHandle, const ble_gatt_error *error, const ble_gatt_svc *service) {
+  if(service == nullptr && error->status == BLE_HS_EDONE) {
+    NRF_LOG_INFO("CTS Discovery complete");
+    return true;
+  }
 
   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",  service->start_handle);
     ble_gattc_disc_chrs_by_uuid(connectionHandle, service->start_handle, service->end_handle, ((ble_uuid_t*)&currentTimeCharacteristicUuid), CurrentTimeCharacteristicDiscoveredCallback, this);
   }
-  return 0;
+  return false;
 }
 
 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)
-    NRF_LOG_INFO("Characteristic discovery complete");
+    NRF_LOG_INFO("CTS Characteristic discovery complete");
 
   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);
diff --git a/src/Components/Ble/CurrentTimeClient.h b/src/Components/Ble/CurrentTimeClient.h
index 94892cef..43deadcc 100644
--- a/src/Components/Ble/CurrentTimeClient.h
+++ b/src/Components/Ble/CurrentTimeClient.h
@@ -17,7 +17,7 @@ namespace Pinetime {
       public:
         explicit CurrentTimeClient(DateTime& dateTimeController);
         void Init();
-        int OnDiscoveryEvent(uint16_t connectionHandle, const ble_gatt_error *error, const ble_gatt_svc *service);
+        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);
diff --git a/src/Components/Ble/NimbleController.cpp b/src/Components/Ble/NimbleController.cpp
index cedc9f60..5e3d58eb 100644
--- a/src/Components/Ble/NimbleController.cpp
+++ b/src/Components/Ble/NimbleController.cpp
@@ -1,5 +1,10 @@
+
 #include <Components/DateTime/DateTimeController.h>
 
+#include <SystemTask/SystemTask.h>
+#include <Components/Ble/NotificationManager.h>
+#include <hal/nrf_rtc.h>
+
 #include "NimbleController.h"
 #include <services/gatt/ble_svc_gatt.h>
 #include <services/gap/ble_svc_gap.h>
@@ -7,15 +12,22 @@
 #include <host/ble_hs_id.h>
 #include <host/ble_hs.h>
 #include <host/ble_gap.h>
-#include <hal/nrf_rtc.h>
+
+
 
 using namespace Pinetime::Controllers;
 
 // TODO c++ify the following code
 // - cts should be in it own class
 
-NimbleController::NimbleController(DateTime &datetimeController) : dateTimeController{datetimeController},
-                                                                   currentTimeClient{datetimeController} {
+NimbleController::NimbleController(Pinetime::System::SystemTask& systemTask,
+        DateTime& dateTimeController,
+        Pinetime::Controllers::NotificationManager& notificationManager) :
+        systemTask{systemTask},
+        dateTimeController{dateTimeController},
+        notificationManager{notificationManager},
+        currentTimeClient{dateTimeController},
+        alertNotificationClient{systemTask, notificationManager} {
 
 }
 
@@ -83,6 +95,15 @@ void NimbleController::StartAdvertising() {
 
 }
 
+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:
@@ -102,7 +123,7 @@ int NimbleController::OnGAPEvent(ble_gap_event *event) {
         StartAdvertising();
       } else {
         connectionHandle = event->connect.conn_handle;
-        currentTimeClient.StartDiscovery(connectionHandle);
+        ble_gattc_disc_all_svcs(connectionHandle, OnAllSvrDisco, this);
       }
     }
       break;
@@ -155,6 +176,25 @@ int NimbleController::OnGAPEvent(ble_gap_event *event) {
        */
     }
       return BLE_GAP_REPEAT_PAIRING_RETRY;
+
+    case BLE_GAP_EVENT_NOTIFY_RX: {
+      /* Peer sent us a notification or indication. */
+      size_t notifSize = OS_MBUF_PKTLEN(event->notify_rx.om);
+
+      NRF_LOG_INFO("received %s; conn_handle=%d attr_handle=%d "
+                   "attr_len=%d",
+                   event->notify_rx.indication ?
+                   "indication" :
+                   "notification",
+                   event->notify_rx.conn_handle,
+                   event->notify_rx.attr_handle,
+                   notifSize);
+
+      alertNotificationClient.OnNotification(event);
+      return 0;
+    }
+      /* Attribute data is contained in event->notify_rx.attr_data. */
+
     default:
       NRF_LOG_INFO("Advertising event : %d", event->type);
       break;
@@ -162,6 +202,12 @@ 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) {
+  alertNotificationClient.OnDiscoveryEvent(i, error, service);
+//  currentTimeClient.OnDiscoveryEvent(i, error, service);
+  return 0;
+}
+
 
 
 
diff --git a/src/Components/Ble/NimbleController.h b/src/Components/Ble/NimbleController.h
index 2a396949..c4eab52b 100644
--- a/src/Components/Ble/NimbleController.h
+++ b/src/Components/Ble/NimbleController.h
@@ -1,7 +1,7 @@
 #pragma once
 
 #include <cstdint>
-
+#include "AlertNotificationClient.h"
 #include "DeviceInformationService.h"
 #include "CurrentTimeClient.h"
 #include <host/ble_gap.h>
@@ -11,16 +11,21 @@ namespace Pinetime {
     class DateTime;
     class NimbleController {
       public:
-        NimbleController(DateTime& dateTimeController);
+        NimbleController(Pinetime::System::SystemTask& systemTask, DateTime& dateTimeController, Pinetime::Controllers::NotificationManager& notificationManager);
         void Init();
         void StartAdvertising();
         int OnGAPEvent(ble_gap_event *event);
 
+        int OnDiscoveryEvent(uint16_t i, const ble_gatt_error *pError, const ble_gatt_svc *pSvc);
       private:
         static constexpr char* deviceName = "Pinetime-JF";
+        Pinetime::System::SystemTask& systemTask;
         DateTime& dateTimeController;
+        Pinetime::Controllers::NotificationManager& notificationManager;
+
         DeviceInformationService deviceInformationService;
         CurrentTimeClient currentTimeClient;
+        AlertNotificationClient alertNotificationClient;
         uint8_t addrType;
         uint16_t connectionHandle;
     };
diff --git a/src/SystemTask/SystemTask.cpp b/src/SystemTask/SystemTask.cpp
index ab30d5aa..5dceb89b 100644
--- a/src/SystemTask/SystemTask.cpp
+++ b/src/SystemTask/SystemTask.cpp
@@ -22,7 +22,7 @@ SystemTask::SystemTask(Drivers::SpiMaster &spi, Drivers::St7789 &lcd, Drivers::C
                        Pinetime::Controllers::NotificationManager& notificationManager) :
                        spi{spi}, lcd{lcd}, touchPanel{touchPanel}, lvgl{lvgl}, batteryController{batteryController},
                        bleController{bleController}, dateTimeController{dateTimeController},
-                       watchdog{}, watchdogView{watchdog}, notificationManager{notificationManager}, nimbleController({dateTimeController}) {
+                       watchdog{}, watchdogView{watchdog}, notificationManager{notificationManager}, nimbleController(*this, dateTimeController, notificationManager) {
   systemTaksMsgQueue = xQueueCreate(10, 1);
 }