Merge remote-tracking branch 'upstream/develop' into pts-settings

21 files changed, 454 insertions, 55 deletions

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 0616c9d7..488ba65d 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -476,6 +476,7 @@ list(APPEND SOURCE_FILES
         components/ble/ImmediateAlertService.cpp
         components/ble/ServiceDiscovery.cpp
         components/ble/HeartRateService.cpp
+        components/ble/MotionService.cpp
         components/firmwarevalidator/FirmwareValidator.cpp
         components/motor/MotorController.cpp
         components/settings/Settings.cpp
@@ -505,6 +506,7 @@ list(APPEND SOURCE_FILES
         components/heartrate/Ptagc.cpp
         components/heartrate/HeartRateController.cpp
 
+        buttonhandler/ButtonHandler.cpp
         touchhandler/TouchHandler.cpp
         )
 
@@ -544,6 +546,7 @@ list(APPEND RECOVERY_SOURCE_FILES
         components/ble/ServiceDiscovery.cpp
         components/ble/NavigationService.cpp
         components/ble/HeartRateService.cpp
+        components/ble/MotionService.cpp
         components/firmwarevalidator/FirmwareValidator.cpp
         components/settings/Settings.cpp
         components/timer/TimerController.cpp
@@ -564,6 +567,7 @@ list(APPEND RECOVERY_SOURCE_FILES
         components/heartrate/Ptagc.cpp
         components/motor/MotorController.cpp
         components/fs/FS.cpp
+        buttonhandler/ButtonHandler.cpp
         touchhandler/TouchHandler.cpp
         )
 
@@ -651,6 +655,7 @@ set(INCLUDE_FILES
         components/ble/ServiceDiscovery.h
         components/ble/BleClient.h
         components/ble/HeartRateService.h
+        components/ble/MotionService.h
         components/settings/Settings.h
         components/timer/TimerController.h
         components/alarm/AlarmController.h
@@ -677,6 +682,7 @@ set(INCLUDE_FILES
         components/heartrate/Ptagc.h
         components/heartrate/HeartRateController.h
         components/motor/MotorController.h
+        buttonhandler/ButtonHandler.h
         touchhandler/TouchHandler.h
         )
 
diff --git a/src/buttonhandler/ButtonActions.h b/src/buttonhandler/ButtonActions.h
new file mode 100644
index 00000000..21be441b
--- /dev/null
+++ b/src/buttonhandler/ButtonActions.h
@@ -0,0 +1,7 @@
+#pragma once
+
+namespace Pinetime {
+  namespace Controllers {
+    enum class ButtonActions { None, Click, DoubleClick, LongPress, LongerPress };
+  }
+}
diff --git a/src/buttonhandler/ButtonHandler.cpp b/src/buttonhandler/ButtonHandler.cpp
new file mode 100644
index 00000000..91e8bbd0
--- /dev/null
+++ b/src/buttonhandler/ButtonHandler.cpp
@@ -0,0 +1,78 @@
+#include "ButtonHandler.h"
+
+using namespace Pinetime::Controllers;
+
+void ButtonTimerCallback(TimerHandle_t xTimer) {
+  auto* sysTask = static_cast<Pinetime::System::SystemTask*>(pvTimerGetTimerID(xTimer));
+  sysTask->PushMessage(Pinetime::System::Messages::HandleButtonTimerEvent);
+}
+
+void ButtonHandler::Init(Pinetime::System::SystemTask* systemTask) {
+  buttonTimer = xTimerCreate("buttonTimer", 0, pdFALSE, systemTask, ButtonTimerCallback);
+}
+
+ButtonActions ButtonHandler::HandleEvent(Events event) {
+  static constexpr TickType_t doubleClickTime = pdMS_TO_TICKS(200);
+  static constexpr TickType_t longPressTime = pdMS_TO_TICKS(400);
+  static constexpr TickType_t longerPressTime = pdMS_TO_TICKS(2000);
+
+  if (event == Events::Press) {
+    buttonPressed = true;
+  } else if (event == Events::Release) {
+    releaseTime = xTaskGetTickCount();
+    buttonPressed = false;
+  }
+
+  switch (state) {
+    case States::Idle:
+      if (event == Events::Press) {
+        xTimerChangePeriod(buttonTimer, doubleClickTime, 0);
+        xTimerStart(buttonTimer, 0);
+        state = States::Pressed;
+      }
+      break;
+    case States::Pressed:
+      if (event == Events::Press) {
+        if (xTaskGetTickCount() - releaseTime < doubleClickTime) {
+          xTimerStop(buttonTimer, 0);
+          state = States::Idle;
+          return ButtonActions::DoubleClick;
+        }
+      } else if (event == Events::Release) {
+        xTimerChangePeriod(buttonTimer, doubleClickTime, 0);
+        xTimerStart(buttonTimer, 0);
+      } else if (event == Events::Timer) {
+        if (buttonPressed) {
+          xTimerChangePeriod(buttonTimer, longPressTime - doubleClickTime, 0);
+          xTimerStart(buttonTimer, 0);
+          state = States::Holding;
+        } else {
+          state = States::Idle;
+          return ButtonActions::Click;
+        }
+      }
+      break;
+    case States::Holding:
+      if (event == Events::Release) {
+        xTimerStop(buttonTimer, 0);
+        state = States::Idle;
+        return ButtonActions::Click;
+      } else if (event == Events::Timer) {
+        xTimerChangePeriod(buttonTimer, longerPressTime - longPressTime - doubleClickTime, 0);
+        xTimerStart(buttonTimer, 0);
+        state = States::LongHeld;
+        return ButtonActions::LongPress;
+      }
+      break;
+    case States::LongHeld:
+      if (event == Events::Release) {
+        xTimerStop(buttonTimer, 0);
+        state = States::Idle;
+      } else if (event == Events::Timer) {
+        state = States::Idle;
+        return ButtonActions::LongerPress;
+      }
+      break;
+  }
+  return ButtonActions::None;
+}
diff --git a/src/buttonhandler/ButtonHandler.h b/src/buttonhandler/ButtonHandler.h
new file mode 100644
index 00000000..44b20f19
--- /dev/null
+++ b/src/buttonhandler/ButtonHandler.h
@@ -0,0 +1,24 @@
+#pragma once
+
+#include "ButtonActions.h"
+#include "systemtask/SystemTask.h"
+#include <FreeRTOS.h>
+#include <timers.h>
+
+namespace Pinetime {
+  namespace Controllers {
+    class ButtonHandler {
+    public:
+      enum class Events : uint8_t { Press, Release, Timer };
+      void Init(Pinetime::System::SystemTask* systemTask);
+      ButtonActions HandleEvent(Events event);
+
+    private:
+      enum class States : uint8_t { Idle, Pressed, Holding, LongHeld };
+      TickType_t releaseTime = 0;
+      TimerHandle_t buttonTimer;
+      bool buttonPressed = false;
+      States state = States::Idle;
+    };
+  }
+}
diff --git a/src/components/ble/HeartRateService.cpp b/src/components/ble/HeartRateService.cpp
index 5b00f492..75a038a2 100644
--- a/src/components/ble/HeartRateService.cpp
+++ b/src/components/ble/HeartRateService.cpp
@@ -8,7 +8,7 @@ constexpr ble_uuid16_t HeartRateService::heartRateServiceUuid;
 constexpr ble_uuid16_t HeartRateService::heartRateMeasurementUuid;
 
 namespace {
-  int HeartRateServiceServiceCallback(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt* ctxt, void* arg) {
+  int HeartRateServiceCallback(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt* ctxt, void* arg) {
     auto* heartRateService = static_cast<HeartRateService*>(arg);
     return heartRateService->OnHeartRateRequested(conn_handle, attr_handle, ctxt);
   }
@@ -19,7 +19,7 @@ HeartRateService::HeartRateService(Pinetime::System::SystemTask& system, Control
   : system {system},
     heartRateController {heartRateController},
     characteristicDefinition {{.uuid = &heartRateMeasurementUuid.u,
-                               .access_cb = HeartRateServiceServiceCallback,
+                               .access_cb = HeartRateServiceCallback,
                                .arg = this,
                                .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY,
                                .val_handle = &heartRateMeasurementHandle},
@@ -56,6 +56,8 @@ int HeartRateService::OnHeartRateRequested(uint16_t connectionHandle, uint16_t a
 }
 
 void HeartRateService::OnNewHeartRateValue(uint8_t heartRateValue) {
+  if(!heartRateMeasurementNotificationEnable) return;
+
   uint8_t buffer[2] = {0, heartRateController.HeartRate()}; // [0] = flags, [1] = hr value
   auto* om = ble_hs_mbuf_from_flat(buffer, 2);
 
@@ -67,3 +69,13 @@ void HeartRateService::OnNewHeartRateValue(uint8_t heartRateValue) {
 
   ble_gattc_notify_custom(connectionHandle, heartRateMeasurementHandle, om);
 }
+
+void HeartRateService::SubscribeNotification(uint16_t connectionHandle, uint16_t attributeHandle) {
+  if(attributeHandle == heartRateMeasurementHandle)
+    heartRateMeasurementNotificationEnable = true;
+}
+
+void HeartRateService::UnsubscribeNotification(uint16_t connectionHandle, uint16_t attributeHandle) {
+  if(attributeHandle == heartRateMeasurementHandle)
+    heartRateMeasurementNotificationEnable = false;
+}
\ No newline at end of file
diff --git a/src/components/ble/HeartRateService.h b/src/components/ble/HeartRateService.h
index 0b16703f..4e4a5a42 100644
--- a/src/components/ble/HeartRateService.h
+++ b/src/components/ble/HeartRateService.h
@@ -2,6 +2,7 @@
 #define min // workaround: nimble's min/max macros conflict with libstdc++
 #define max
 #include <host/ble_gap.h>
+#include <atomic>
 #undef max
 #undef min
 
@@ -18,6 +19,9 @@ namespace Pinetime {
       int OnHeartRateRequested(uint16_t connectionHandle, uint16_t attributeHandle, ble_gatt_access_ctxt* context);
       void OnNewHeartRateValue(uint8_t hearRateValue);
 
+      void SubscribeNotification(uint16_t connectionHandle, uint16_t attributeHandle);
+      void UnsubscribeNotification(uint16_t connectionHandle, uint16_t attributeHandle);
+
     private:
       Pinetime::System::SystemTask& system;
       Controllers::HeartRateController& heartRateController;
@@ -28,10 +32,11 @@ namespace Pinetime {
 
       static constexpr ble_uuid16_t heartRateMeasurementUuid {.u {.type = BLE_UUID_TYPE_16}, .value = heartRateMeasurementId};
 
-      struct ble_gatt_chr_def characteristicDefinition[3];
+      struct ble_gatt_chr_def characteristicDefinition[2];
       struct ble_gatt_svc_def serviceDefinition[2];
 
       uint16_t heartRateMeasurementHandle;
+      std::atomic_bool heartRateMeasurementNotificationEnable {false};
     };
   }
 }
diff --git a/src/components/ble/MotionService.cpp b/src/components/ble/MotionService.cpp
new file mode 100644
index 00000000..b4786ab5
--- /dev/null
+++ b/src/components/ble/MotionService.cpp
@@ -0,0 +1,124 @@
+#include "MotionService.h"
+#include "components/motion//MotionController.h"
+#include "systemtask/SystemTask.h"
+
+using namespace Pinetime::Controllers;
+
+namespace {
+  // 0002yyxx-78fc-48fe-8e23-433b3a1942d0
+  constexpr ble_uuid128_t CharUuid(uint8_t x, uint8_t y) {
+    return ble_uuid128_t{
+      .u = {.type = BLE_UUID_TYPE_128},
+      .value =  { 0xd0, 0x42, 0x19, 0x3a, 0x3b, 0x43, 0x23, 0x8e, 0xfe, 0x48, 0xfc, 0x78, x, y, 0x03, 0x00 }
+    };
+  }
+
+  // 00020000-78fc-48fe-8e23-433b3a1942d0
+  constexpr ble_uuid128_t BaseUuid() {
+    return CharUuid(0x00, 0x00);
+  }
+
+  constexpr ble_uuid128_t motionServiceUuid {BaseUuid()};
+  constexpr ble_uuid128_t stepCountCharUuid {CharUuid(0x01, 0x00)};
+  constexpr ble_uuid128_t motionValuesCharUuid {CharUuid(0x02, 0x00)};
+
+  int MotionServiceCallback(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt* ctxt, void* arg) {
+    auto* motionService = static_cast<MotionService*>(arg);
+    return motionService->OnStepCountRequested(conn_handle, attr_handle, ctxt);
+  }
+}
+
+// TODO Refactoring - remove dependency to SystemTask
+MotionService::MotionService(Pinetime::System::SystemTask& system, Controllers::MotionController& motionController)
+  : system {system},
+    motionController {motionController},
+    characteristicDefinition {{.uuid = &stepCountCharUuid.u,
+                               .access_cb = MotionServiceCallback,
+                               .arg = this,
+                               .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY,
+                               .val_handle = &stepCountHandle},
+                              {.uuid = &motionValuesCharUuid.u,
+                               .access_cb = MotionServiceCallback,
+                               .arg = this,
+                               .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY,
+                               .val_handle = &motionValuesHandle},
+                              {0}},
+    serviceDefinition {
+      {
+       .type = BLE_GATT_SVC_TYPE_PRIMARY,
+       .uuid = &motionServiceUuid.u,
+       .characteristics = characteristicDefinition
+      },
+      {0},
+    } {
+  // TODO refactor to prevent this loop dependency (service depends on controller and controller depends on service)
+  motionController.SetService(this);
+}
+
+void MotionService::Init() {
+  int res = 0;
+  res = ble_gatts_count_cfg(serviceDefinition);
+  ASSERT(res == 0);
+
+  res = ble_gatts_add_svcs(serviceDefinition);
+  ASSERT(res == 0);
+}
+
+int MotionService::OnStepCountRequested(uint16_t connectionHandle, uint16_t attributeHandle, ble_gatt_access_ctxt* context) {
+  if (attributeHandle == stepCountHandle) {
+    NRF_LOG_INFO("Motion-stepcount : handle = %d", stepCountHandle);
+    uint32_t buffer = motionController.NbSteps();
+
+    int res = os_mbuf_append(context->om, &buffer, 4);
+    return (res == 0) ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+  } else if(attributeHandle == motionValuesHandle) {
+    int16_t buffer[3] = { motionController.X(), motionController.Y(), motionController.Z() };
+
+    int res = os_mbuf_append(context->om, buffer, 3 * sizeof(int16_t));
+    return (res == 0) ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+  }
+  return 0;
+}
+
+void MotionService::OnNewStepCountValue(uint32_t stepCount) {
+  if(!stepCountNoficationEnabled) return;
+
+  uint32_t buffer = stepCount;
+  auto* om = ble_hs_mbuf_from_flat(&buffer, 4);
+
+  uint16_t connectionHandle = system.nimble().connHandle();
+
+  if (connectionHandle == 0 || connectionHandle == BLE_HS_CONN_HANDLE_NONE) {
+    return;
+  }
+
+  ble_gattc_notify_custom(connectionHandle, stepCountHandle, om);
+}
+void MotionService::OnNewMotionValues(int16_t x, int16_t y, int16_t z) {
+  if(!motionValuesNoficationEnabled) return;
+
+  int16_t buffer[3] = { motionController.X(), motionController.Y(), motionController.Z() };
+  auto* om = ble_hs_mbuf_from_flat(buffer, 3 * sizeof(int16_t));
+
+  uint16_t connectionHandle = system.nimble().connHandle();
+
+  if (connectionHandle == 0 || connectionHandle == BLE_HS_CONN_HANDLE_NONE) {
+    return;
+  }
+
+  ble_gattc_notify_custom(connectionHandle, motionValuesHandle, om);
+}
+
+void MotionService::SubscribeNotification(uint16_t connectionHandle, uint16_t attributeHandle) {
+  if(attributeHandle == stepCountHandle)
+    stepCountNoficationEnabled = true;
+  else if(attributeHandle == motionValuesHandle)
+    motionValuesNoficationEnabled = true;
+}
+
+void MotionService::UnsubscribeNotification(uint16_t connectionHandle, uint16_t attributeHandle) {
+  if(attributeHandle == stepCountHandle)
+    stepCountNoficationEnabled = false;
+  else if(attributeHandle == motionValuesHandle)
+    motionValuesNoficationEnabled = false;
+}
diff --git a/src/components/ble/MotionService.h b/src/components/ble/MotionService.h
new file mode 100644
index 00000000..1b4ac0a3
--- /dev/null
+++ b/src/components/ble/MotionService.h
@@ -0,0 +1,39 @@
+#pragma once
+#define min // workaround: nimble's min/max macros conflict with libstdc++
+#define max
+#include <host/ble_gap.h>
+#include <atomic>
+#undef max
+#undef min
+
+namespace Pinetime {
+  namespace System {
+    class SystemTask;
+  }
+  namespace Controllers {
+    class MotionController;
+    class MotionService {
+    public:
+      MotionService(Pinetime::System::SystemTask& system, Controllers::MotionController& motionController);
+      void Init();
+      int OnStepCountRequested(uint16_t connectionHandle, uint16_t attributeHandle, ble_gatt_access_ctxt* context);
+      void OnNewStepCountValue(uint32_t stepCount);
+      void OnNewMotionValues(int16_t x, int16_t y, int16_t z);
+
+      void SubscribeNotification(uint16_t connectionHandle, uint16_t attributeHandle);
+      void UnsubscribeNotification(uint16_t connectionHandle, uint16_t attributeHandle);
+
+    private:
+      Pinetime::System::SystemTask& system;
+      Controllers::MotionController& motionController;
+
+      struct ble_gatt_chr_def characteristicDefinition[3];
+      struct ble_gatt_svc_def serviceDefinition[2];
+
+      uint16_t stepCountHandle;
+      uint16_t motionValuesHandle;
+      std::atomic_bool stepCountNoficationEnabled {false};
+      std::atomic_bool motionValuesNoficationEnabled {false};
+    };
+  }
+}
diff --git a/src/components/ble/NimbleController.cpp b/src/components/ble/NimbleController.cpp
index 879421e7..1bcae1bc 100644
--- a/src/components/ble/NimbleController.cpp
+++ b/src/components/ble/NimbleController.cpp
@@ -23,7 +23,8 @@ NimbleController::NimbleController(Pinetime::System::SystemTask& systemTask,
                                    Pinetime::Controllers::NotificationManager& notificationManager,
                                    Controllers::Battery& batteryController,
                                    Pinetime::Drivers::SpiNorFlash& spiNorFlash,
-                                   Controllers::HeartRateController& heartRateController)
+                                   Controllers::HeartRateController& heartRateController,
+                                   Controllers::MotionController& motionController)
   : systemTask {systemTask},
     bleController {bleController},
     dateTimeController {dateTimeController},
@@ -39,6 +40,7 @@ NimbleController::NimbleController(Pinetime::System::SystemTask& systemTask,
     batteryInformationService {batteryController},
     immediateAlertService {systemTask, notificationManager},
     heartRateService {systemTask, heartRateController},
+    motionService{systemTask, motionController},
     serviceDiscovery({&currentTimeClient, &alertNotificationClient}) {
 }
 
@@ -81,6 +83,7 @@ void NimbleController::Init() {
   batteryInformationService.Init();
   immediateAlertService.Init();
   heartRateService.Init();
+  motionService.Init();
 
   int rc;
   rc = ble_hs_util_ensure_addr(0);
@@ -215,6 +218,19 @@ int NimbleController::OnGAPEvent(ble_gap_event* event) {
                    event->subscribe.prev_notify,
                    event->subscribe.cur_notify,
                    event->subscribe.prev_indicate);
+
+      if(event->subscribe.reason == BLE_GAP_SUBSCRIBE_REASON_TERM) {
+        heartRateService.UnsubscribeNotification(event->subscribe.conn_handle, event->subscribe.attr_handle);
+        motionService.UnsubscribeNotification(event->subscribe.conn_handle, event->subscribe.attr_handle);
+      }
+      else if(event->subscribe.prev_notify == 0 && event->subscribe.cur_notify == 1) {
+        heartRateService.SubscribeNotification(event->subscribe.conn_handle, event->subscribe.attr_handle);
+        motionService.SubscribeNotification(event->subscribe.conn_handle, event->subscribe.attr_handle);
+      }
+      else if(event->subscribe.prev_notify == 1 && event->subscribe.cur_notify == 0) {
+        heartRateService.UnsubscribeNotification(event->subscribe.conn_handle, event->subscribe.attr_handle);
+        motionService.UnsubscribeNotification(event->subscribe.conn_handle, event->subscribe.attr_handle);
+      }
       break;
 
     case BLE_GAP_EVENT_MTU:
diff --git a/src/components/ble/NimbleController.h b/src/components/ble/NimbleController.h
index 473bb1af..76f89ba8 100644
--- a/src/components/ble/NimbleController.h
+++ b/src/components/ble/NimbleController.h
@@ -19,6 +19,7 @@
 #include "NavigationService.h"
 #include "ServiceDiscovery.h"
 #include "HeartRateService.h"
+#include "MotionService.h"
 
 namespace Pinetime {
   namespace Drivers {
@@ -43,7 +44,8 @@ namespace Pinetime {
                        Pinetime::Controllers::NotificationManager& notificationManager,
                        Controllers::Battery& batteryController,
                        Pinetime::Drivers::SpiNorFlash& spiNorFlash,
-                       Controllers::HeartRateController& heartRateController);
+                       Controllers::HeartRateController& heartRateController,
+                       Controllers::MotionController& motionController);
       void Init();
       void StartAdvertising();
       int OnGAPEvent(ble_gap_event* event);
@@ -95,6 +97,7 @@ namespace Pinetime {
       BatteryInformationService batteryInformationService;
       ImmediateAlertService immediateAlertService;
       HeartRateService heartRateService;
+      MotionService motionService;
 
       uint8_t addrType; // 1 = Random, 0 = PUBLIC
       uint16_t connectionHandle = BLE_HS_CONN_HANDLE_NONE;
diff --git a/src/components/motion/MotionController.cpp b/src/components/motion/MotionController.cpp
index b0dbada4..a2384d79 100644
--- a/src/components/motion/MotionController.cpp
+++ b/src/components/motion/MotionController.cpp
@@ -3,6 +3,14 @@
 using namespace Pinetime::Controllers;
 
 void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps) {
+  if (this->nbSteps != nbSteps && service != nullptr) {
+    service->OnNewStepCountValue(nbSteps);
+  }
+
+  if(service != nullptr && (this->x != x || this->y != y || this->z != z)) {
+    service->OnNewMotionValues(x, y, z);
+  }
+
   this->x = x;
   this->y = y;
   this->z = z;
@@ -41,3 +49,6 @@ void MotionController::Init(Pinetime::Drivers::Bma421::DeviceTypes types) {
     default: this->deviceType = DeviceTypes::Unknown; break;
   }
 }
+void MotionController::SetService(Pinetime::Controllers::MotionService* service) {
+  this->service = service;
+}
diff --git a/src/components/motion/MotionController.h b/src/components/motion/MotionController.h
index ff715093..c72d8a4a 100644
--- a/src/components/motion/MotionController.h
+++ b/src/components/motion/MotionController.h
@@ -2,6 +2,7 @@
 
 #include <cstdint>
 #include <drivers/Bma421.h>
+#include <components/ble/MotionService.h>
 
 namespace Pinetime {
   namespace Controllers {
@@ -39,6 +40,7 @@ namespace Pinetime {
       }
 
       void Init(Pinetime::Drivers::Bma421::DeviceTypes types);
+      void SetService(Pinetime::Controllers::MotionService* service);
 
     private:
       uint32_t nbSteps;
@@ -48,6 +50,7 @@ namespace Pinetime {
       int16_t lastYForWakeUp = 0;
       bool isSensorOk = false;
       DeviceTypes deviceType = DeviceTypes::Unknown;
+      Pinetime::Controllers::MotionService* service = nullptr;
     };
   }
 }
\ No newline at end of file
diff --git a/src/displayapp/DisplayApp.cpp b/src/displayapp/DisplayApp.cpp
index ad9536da..342c3879 100644
--- a/src/displayapp/DisplayApp.cpp
+++ b/src/displayapp/DisplayApp.cpp
@@ -259,6 +259,20 @@ void DisplayApp::Refresh() {
           }
         }
         break;
+      case Messages::ButtonLongPressed:
+        if (currentApp != Apps::Clock) {
+          LoadApp(Apps::Clock, DisplayApp::FullRefreshDirections::Down);
+        }
+        break;
+      case Messages::ButtonLongerPressed:
+        // Create reboot app and open it instead
+        LoadApp(Apps::SysInfo, DisplayApp::FullRefreshDirections::Up);
+        break;
+      case Messages::ButtonDoubleClicked:
+        if (currentApp != Apps::Notifications && currentApp != Apps::NotificationsPreview) {
+          LoadApp(Apps::Notifications, DisplayApp::FullRefreshDirections::Down);
+        }
+        break;
 
       case Messages::BleFirmwareUpdateStarted:
         LoadApp(Apps::FirmwareUpdate, DisplayApp::FullRefreshDirections::Down);
diff --git a/src/displayapp/DisplayAppRecovery.h b/src/displayapp/DisplayAppRecovery.h
index 9f5fb130..72868159 100644
--- a/src/displayapp/DisplayAppRecovery.h
+++ b/src/displayapp/DisplayAppRecovery.h
@@ -10,7 +10,7 @@
 #include <date/date.h>
 #include <drivers/Watchdog.h>
 #include <components/motor/MotorController.h>
-#include <BootErrors.h>
+#include "BootErrors.h"
 #include "TouchEvents.h"
 #include "Apps.h"
 #include "Messages.h"
diff --git a/src/displayapp/Messages.h b/src/displayapp/Messages.h
index d48b646f..ab0a0608 100644
--- a/src/displayapp/Messages.h
+++ b/src/displayapp/Messages.h
@@ -9,6 +9,9 @@ namespace Pinetime {
         UpdateBleConnection,
         TouchEvent,
         ButtonPushed,
+        ButtonLongPressed,
+        ButtonLongerPressed,
+        ButtonDoubleClicked,
         NewNotification,
         TimerDone,
         BleFirmwareUpdateStarted,
diff --git a/src/displayapp/screens/SystemInfo.cpp b/src/displayapp/screens/SystemInfo.cpp
index 343b72bf..dd223b2f 100644
--- a/src/displayapp/screens/SystemInfo.cpp
+++ b/src/displayapp/screens/SystemInfo.cpp
@@ -209,7 +209,7 @@ std::unique_ptr<Screen> SystemInfo::CreateScreen4() {
   static constexpr uint8_t maxTaskCount = 9;
   TaskStatus_t tasksStatus[maxTaskCount];
 
-  lv_obj_t* infoTask = lv_table_create(lv_scr_act(), NULL);
+  lv_obj_t* infoTask = lv_table_create(lv_scr_act(), nullptr);
   lv_table_set_col_cnt(infoTask, 4);
   lv_table_set_row_cnt(infoTask, maxTaskCount + 1);
   lv_obj_set_style_local_pad_all(infoTask, LV_TABLE_PART_CELL1, LV_STATE_DEFAULT, 0);
@@ -227,35 +227,37 @@ std::unique_ptr<Screen> SystemInfo::CreateScreen4() {
   auto nb = uxTaskGetSystemState(tasksStatus, maxTaskCount, nullptr);
   std::sort(tasksStatus, tasksStatus + nb, sortById);
   for (uint8_t i = 0; i < nb && i < maxTaskCount; i++) {
+    char buffer[7] = {0};
 
-    lv_table_set_cell_value(infoTask, i + 1, 0, std::to_string(tasksStatus[i].xTaskNumber).c_str());
-    char state[2] = {0};
+    sprintf(buffer, "%lu", tasksStatus[i].xTaskNumber);
+    lv_table_set_cell_value(infoTask, i + 1, 0, buffer);
     switch (tasksStatus[i].eCurrentState) {
       case eReady:
       case eRunning:
-        state[0] = 'R';
+        buffer[0] = 'R';
         break;
       case eBlocked:
-        state[0] = 'B';
+        buffer[0] = 'B';
         break;
       case eSuspended:
-        state[0] = 'S';
+        buffer[0] = 'S';
         break;
       case eDeleted:
-        state[0] = 'D';
+        buffer[0] = 'D';
         break;
       default:
-        state[0] = 'I'; // Invalid
+        buffer[0] = 'I'; // Invalid
         break;
     }
-    lv_table_set_cell_value(infoTask, i + 1, 1, state);
+    buffer[1] = '\0';
+    lv_table_set_cell_value(infoTask, i + 1, 1, buffer);
     lv_table_set_cell_value(infoTask, i + 1, 2, tasksStatus[i].pcTaskName);
     if (tasksStatus[i].usStackHighWaterMark < 20) {
-      std::string str1 = std::to_string(tasksStatus[i].usStackHighWaterMark) + " low";
-      lv_table_set_cell_value(infoTask, i + 1, 3, str1.c_str());
+      sprintf(buffer, "%d low", tasksStatus[i].usStackHighWaterMark);
     } else {
-      lv_table_set_cell_value(infoTask, i + 1, 3, std::to_string(tasksStatus[i].usStackHighWaterMark).c_str());
+      sprintf(buffer, "%d", tasksStatus[i].usStackHighWaterMark);
     }
+    lv_table_set_cell_value(infoTask, i + 1, 3, buffer);
   }
   return std::make_unique<Screens::Label>(3, 5, app, infoTask);
 }
diff --git a/src/displayapp/screens/Twos.cpp b/src/displayapp/screens/Twos.cpp
index 4201d501..d12ef906 100644
--- a/src/displayapp/screens/Twos.cpp
+++ b/src/displayapp/screens/Twos.cpp
@@ -1,10 +1,10 @@
 #include "Twos.h"
-#include <lvgl/lvgl.h>
-#include <string>
-#include <charconv>
 #include <array>
-#include <vector>
+#include <cstdio>
+#include <cstdlib>
+#include <lvgl/lvgl.h>
 #include <utility>
+#include <vector>
 
 using namespace Pinetime::Applications::Screens;
 
@@ -265,7 +265,9 @@ void Twos::updateGridDisplay(Tile grid[][4]) {
   for (int row = 0; row < 4; row++) {
     for (int col = 0; col < 4; col++) {
       if (grid[row][col].value) {
-        lv_table_set_cell_value(gridDisplay, row, col, (std::to_string(grid[row][col].value)).c_str());
+        char buffer[7];
+        sprintf(buffer, "%d", grid[row][col].value);
+        lv_table_set_cell_value(gridDisplay, row, col, buffer);
       } else {
         lv_table_set_cell_value(gridDisplay, row, col, "");
       }
diff --git a/src/main.cpp b/src/main.cpp
index fc772355..53f78ce8 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -47,6 +47,7 @@
 #include "systemtask/SystemTask.h"
 #include "drivers/PinMap.h"
 #include "touchhandler/TouchHandler.h"
+#include "buttonhandler/ButtonHandler.h"
 
 #if NRF_LOG_ENABLED
   #include "logging/NrfLogger.h"
@@ -96,8 +97,6 @@ TimerHandle_t debounceTimer;
 TimerHandle_t debounceChargeTimer;
 Pinetime::Controllers::Battery batteryController;
 Pinetime::Controllers::Ble bleController;
-static constexpr uint8_t pinTouchIrq = Pinetime::PinMap::Cst816sIrq;
-static constexpr uint8_t pinPowerPresentIrq = Pinetime::PinMap::PowerPresent;
 
 Pinetime::Controllers::HeartRateController heartRateController;
 Pinetime::Applications::HeartRateTask heartRateApp(heartRateSensor, heartRateController);
@@ -110,6 +109,7 @@ Pinetime::Controllers::MotionController motionController;
 Pinetime::Controllers::TimerController timerController;
 Pinetime::Controllers::AlarmController alarmController {dateTimeController};
 Pinetime::Controllers::TouchHandler touchHandler(touchPanel, lvgl);
+Pinetime::Controllers::ButtonHandler buttonHandler;
 
 Pinetime::Controllers::FS fs {spiNorFlash};
 Pinetime::Controllers::Settings settingsController {fs};
@@ -153,7 +153,8 @@ Pinetime::System::SystemTask systemTask(spi,
                                         displayApp,
                                         heartRateApp,
                                         fs,
-                                        touchHandler);
+                                        touchHandler,
+                                        buttonHandler);
 
 /* Variable Declarations for variables in noinit SRAM 
    Increment NoInit_MagicValue upon adding variables to this area
@@ -176,11 +177,10 @@ void nrfx_gpiote_evt_handler(nrfx_gpiote_pin_t pin, nrf_gpiote_polarity_t action
   if (pin == Pinetime::PinMap::PowerPresent and action == NRF_GPIOTE_POLARITY_TOGGLE) {
     xTimerStartFromISR(debounceChargeTimer, &xHigherPriorityTaskWoken);
     portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
-    return;
+  } else if (pin == Pinetime::PinMap::Button) {
+    xTimerStartFromISR(debounceTimer, &xHigherPriorityTaskWoken);
+    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
   }
-
-  xTimerStartFromISR(debounceTimer, &xHigherPriorityTaskWoken);
-  portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
 }
 
 void DebounceTimerChargeCallback(TimerHandle_t xTimer) {
@@ -188,9 +188,8 @@ void DebounceTimerChargeCallback(TimerHandle_t xTimer) {
   systemTask.PushMessage(Pinetime::System::Messages::OnChargingEvent);
 }
 
-void DebounceTimerCallback(TimerHandle_t xTimer) {
-  xTimerStop(xTimer, 0);
-  systemTask.OnButtonPushed();
+void DebounceTimerCallback(TimerHandle_t /*unused*/) {
+  systemTask.PushMessage(Pinetime::System::Messages::HandleButtonEvent);
 }
 
 void SPIM0_SPIS0_TWIM0_TWIS0_SPI0_TWI0_IRQHandler(void) {
@@ -319,8 +318,8 @@ int main(void) {
   }
   nrf_gpio_cfg_default(Pinetime::PinMap::TwiScl);
 
-  debounceTimer = xTimerCreate("debounceTimer", 200, pdFALSE, (void*) 0, DebounceTimerCallback);
-  debounceChargeTimer = xTimerCreate("debounceTimerCharge", 200, pdFALSE, (void*) 0, DebounceTimerChargeCallback);
+  debounceTimer = xTimerCreate("debounceTimer", 10, pdFALSE, nullptr, DebounceTimerCallback);
+  debounceChargeTimer = xTimerCreate("debounceTimerCharge", 200, pdFALSE, nullptr, DebounceTimerChargeCallback);
 
   // retrieve version stored by bootloader
   Pinetime::BootloaderVersion::SetVersion(NRF_TIMER2->CC[0]);
diff --git a/src/systemtask/Messages.h b/src/systemtask/Messages.h
index 5aa218d2..b7142704 100644
--- a/src/systemtask/Messages.h
+++ b/src/systemtask/Messages.h
@@ -15,7 +15,8 @@ namespace Pinetime {
         BleFirmwareUpdateStarted,
         BleFirmwareUpdateFinished,
         OnTouchEvent,
-        OnButtonEvent,
+        HandleButtonEvent,
+        HandleButtonTimerEvent,
         OnDisplayTaskSleeping,
         EnableSleeping,
         DisableSleeping,
diff --git a/src/systemtask/SystemTask.cpp b/src/systemtask/SystemTask.cpp
index f1c5165a..0a3f9951 100644
--- a/src/systemtask/SystemTask.cpp
+++ b/src/systemtask/SystemTask.cpp
@@ -25,7 +25,6 @@
 #include "main.h"
 #include "BootErrors.h"
 
-
 #include <memory>
 
 using namespace Pinetime::System;
@@ -77,7 +76,8 @@ SystemTask::SystemTask(Drivers::SpiMaster& spi,
                        Pinetime::Applications::DisplayApp& displayApp,
                        Pinetime::Applications::HeartRateTask& heartRateApp,
                        Pinetime::Controllers::FS& fs,
-                       Pinetime::Controllers::TouchHandler& touchHandler)
+                       Pinetime::Controllers::TouchHandler& touchHandler,
+                       Pinetime::Controllers::ButtonHandler& buttonHandler)
   : spi {spi},
     lcd {lcd},
     spiNorFlash {spiNorFlash},
@@ -101,7 +101,15 @@ SystemTask::SystemTask(Drivers::SpiMaster& spi,
     heartRateApp(heartRateApp),
     fs {fs},
     touchHandler {touchHandler},
-    nimbleController(*this, bleController, dateTimeController, notificationManager, batteryController, spiNorFlash, heartRateController) {
+    buttonHandler {buttonHandler},
+    nimbleController(*this,
+                     bleController,
+                     dateTimeController,
+                     notificationManager,
+                     batteryController,
+                     spiNorFlash,
+                     heartRateController,
+                     motionController) {
 }
 
 void SystemTask::Start() {
@@ -162,6 +170,8 @@ void SystemTask::Work() {
   heartRateSensor.Disable();
   heartRateApp.Start();
 
+  buttonHandler.Init(this);
+
   // Button
   nrf_gpio_cfg_output(15);
   nrf_gpio_pin_set(15);
@@ -325,10 +335,25 @@ void SystemTask::Work() {
           ReloadIdleTimer();
           displayApp.PushMessage(Pinetime::Applications::Display::Messages::TouchEvent);
           break;
-        case Messages::OnButtonEvent:
-          ReloadIdleTimer();
-          displayApp.PushMessage(Pinetime::Applications::Display::Messages::ButtonPushed);
-          break;
+        case Messages::HandleButtonEvent: {
+          Controllers::ButtonActions action;
+          if (nrf_gpio_pin_read(Pinetime::PinMap::Button) == 0) {
+            action = buttonHandler.HandleEvent(Controllers::ButtonHandler::Events::Release);
+          } else {
+            action = buttonHandler.HandleEvent(Controllers::ButtonHandler::Events::Press);
+            // This is for faster wakeup, sacrificing special longpress and doubleclick handling while sleeping
+            if (IsSleeping()) {
+              fastWakeUpDone = true;
+              GoToRunning();
+              break;
+            }
+          }
+          HandleButtonAction(action);
+        } break;
+        case Messages::HandleButtonTimerEvent: {
+          auto action = buttonHandler.HandleEvent(Controllers::ButtonHandler::Events::Timer);
+          HandleButtonAction(action);
+        } break;
         case Messages::OnDisplayTaskSleeping:
           if (BootloaderVersion::IsValid()) {
             // First versions of the bootloader do not expose their version and cannot initialize the SPI NOR FLASH
@@ -413,18 +438,36 @@ void SystemTask::UpdateMotion() {
   }
 }
 
-void SystemTask::OnButtonPushed() {
-  if (isGoingToSleep)
+void SystemTask::HandleButtonAction(Controllers::ButtonActions action) {
+  if (IsSleeping()) {
     return;
-  if (!isSleeping) {
-    NRF_LOG_INFO("[systemtask] Button pushed");
-    PushMessage(Messages::OnButtonEvent);
-  } else {
-    if (!isWakingUp) {
-      NRF_LOG_INFO("[systemtask] Button pushed, waking up");
-      GoToRunning();
-    }
   }
+
+  ReloadIdleTimer();
+
+  using Actions = Controllers::ButtonActions;
+
+  switch (action) {
+    case Actions::Click:
+      // If the first action after fast wakeup is a click, it should be ignored.
+      if (!fastWakeUpDone && !isGoingToSleep) {
+        displayApp.PushMessage(Applications::Display::Messages::ButtonPushed);
+      }
+      break;
+    case Actions::DoubleClick:
+      displayApp.PushMessage(Applications::Display::Messages::ButtonDoubleClicked);
+      break;
+    case Actions::LongPress:
+      displayApp.PushMessage(Applications::Display::Messages::ButtonLongPressed);
+      break;
+    case Actions::LongerPress:
+      displayApp.PushMessage(Applications::Display::Messages::ButtonLongerPressed);
+      break;
+    default:
+      return;
+  }
+
+  fastWakeUpDone = false;
 }
 
 void SystemTask::GoToRunning() {
diff --git a/src/systemtask/SystemTask.h b/src/systemtask/SystemTask.h
index 879c1be8..412878b1 100644
--- a/src/systemtask/SystemTask.h
+++ b/src/systemtask/SystemTask.h
@@ -20,6 +20,8 @@
 #include "components/alarm/AlarmController.h"
 #include "components/fs/FS.h"
 #include "touchhandler/TouchHandler.h"
+#include "buttonhandler/ButtonHandler.h"
+#include "buttonhandler/ButtonActions.h"
 
 #ifdef PINETIME_IS_RECOVERY
   #include "displayapp/DisplayAppRecovery.h"
@@ -45,6 +47,7 @@ namespace Pinetime {
   }
   namespace Controllers {
     class TouchHandler;
+    class ButtonHandler;
   }
   namespace System {
     class SystemTask {
@@ -71,12 +74,12 @@ namespace Pinetime {
                  Pinetime::Applications::DisplayApp& displayApp,
                  Pinetime::Applications::HeartRateTask& heartRateApp,
                  Pinetime::Controllers::FS& fs,
-                 Pinetime::Controllers::TouchHandler& touchHandler);
+                 Pinetime::Controllers::TouchHandler& touchHandler,
+                 Pinetime::Controllers::ButtonHandler& buttonHandler);
 
       void Start();
       void PushMessage(Messages msg);
 
-      void OnButtonPushed();
       void OnTouchEvent();
 
       void OnIdle();
@@ -123,6 +126,7 @@ namespace Pinetime {
       Pinetime::Applications::HeartRateTask& heartRateApp;
       Pinetime::Controllers::FS& fs;
       Pinetime::Controllers::TouchHandler& touchHandler;
+      Pinetime::Controllers::ButtonHandler& buttonHandler;
       Pinetime::Controllers::NimbleController nimbleController;
 
       static void Process(void* instance);
@@ -135,6 +139,9 @@ namespace Pinetime {
       TimerHandle_t measureBatteryTimer;
       bool doNotGoToSleep = false;
 
+      void HandleButtonAction(Controllers::ButtonActions action);
+      bool fastWakeUpDone = false;
+
       void GoToRunning();
       void UpdateMotion();
       bool stepCounterMustBeReset = false;