3 files changed, 34 insertions, 18 deletions

diff --git a/src/SystemTask/SystemTask.cpp b/src/SystemTask/SystemTask.cpp
index b515d1ab..222829d4 100644
--- a/src/SystemTask/SystemTask.cpp
+++ b/src/SystemTask/SystemTask.cpp
@@ -105,22 +105,33 @@ void SystemTask::Work() {
       Messages message = static_cast<Messages >(msg);
       switch(message) {
         case Messages::GoToRunning:
-          isSleeping = false;
+          spi.Wakeup();
+          twiMaster.Wakeup();
+
+          spiNorFlash.Wakeup();
+          lcd.Wakeup();
+          touchPanel.Wakeup();
+
+          displayApp->PushMessage(Applications::DisplayApp::Messages::GoToRunning);
+          displayApp->PushMessage(Applications::DisplayApp::Messages::UpdateBatteryLevel);
+
           xTimerStart(idleTimer, 0);
           nimbleController.StartAdvertising();
+          isSleeping = false;
+          isWakingUp = false;
           break;
         case Messages::GoToSleep:
+          isGoingToSleep = true;
           NRF_LOG_INFO("[SystemTask] Going to sleep");
           xTimerStop(idleTimer, 0);
           displayApp->PushMessage(Pinetime::Applications::DisplayApp::Messages::GoToSleep);
-          isSleeping = true;
           break;
         case Messages::OnNewTime:
           ReloadIdleTimer();
           displayApp->PushMessage(Pinetime::Applications::DisplayApp::Messages::UpdateDateTime);
           break;
         case Messages::OnNewNotification:
-          if(isSleeping) GoToRunning();
+          if(isSleeping && !isWakingUp) GoToRunning();
           displayApp->PushMessage(Pinetime::Applications::DisplayApp::Messages::NewNotification);
           break;
         case Messages::BleConnected:
@@ -130,7 +141,7 @@ void SystemTask::Work() {
           break;
         case Messages::BleFirmwareUpdateStarted:
           doNotGoToSleep = true;
-          if(isSleeping) GoToRunning();
+          if(isSleeping && !isWakingUp) GoToRunning();
           displayApp->PushMessage(Pinetime::Applications::DisplayApp::Messages::BleFirmwareUpdateStarted);
           break;
         case Messages::BleFirmwareUpdateFinished:
@@ -152,6 +163,8 @@ void SystemTask::Work() {
 
           spi.Sleep();
           twiMaster.Sleep();
+          isSleeping = true;
+          isGoingToSleep = false;
           break;
         default: break;
       }
@@ -180,31 +193,27 @@ void SystemTask::Work() {
 }
 
 void SystemTask::OnButtonPushed() {
+  if(isGoingToSleep) return;
   if(!isSleeping) {
     NRF_LOG_INFO("[SystemTask] Button pushed");
     PushMessage(Messages::OnButtonEvent);
     displayApp->PushMessage(Pinetime::Applications::DisplayApp::Messages::ButtonPushed);
   }
   else {
-    NRF_LOG_INFO("[SystemTask] Button pushed, waking up");
-    GoToRunning();
+    if(!isWakingUp) {
+      NRF_LOG_INFO("[SystemTask] Button pushed, waking up");
+      GoToRunning();
+    }
   }
 }
 
 void SystemTask::GoToRunning() {
+  isWakingUp = true;
   PushMessage(Messages::GoToRunning);
-  spi.Wakeup();
-  twiMaster.Wakeup();
-
-  spiNorFlash.Wakeup();
-  lcd.Wakeup();
-  touchPanel.Wakeup();
-
-  displayApp->PushMessage(Applications::DisplayApp::Messages::GoToRunning);
-  displayApp->PushMessage(Applications::DisplayApp::Messages::UpdateBatteryLevel);
 }
 
 void SystemTask::OnTouchEvent() {
+  if(isGoingToSleep) return ;
   NRF_LOG_INFO("[SystemTask] Touch event");
   if(!isSleeping) {
     PushMessage(Messages::OnTouchEvent);
@@ -213,6 +222,9 @@ void SystemTask::OnTouchEvent() {
 }
 
 void SystemTask::PushMessage(SystemTask::Messages msg) {
+  if(msg == Messages::GoToSleep) {
+    isGoingToSleep = true;
+  }
   BaseType_t xHigherPriorityTaskWoken;
   xHigherPriorityTaskWoken = pdFALSE;
   xQueueSendFromISR(systemTaksMsgQueue, &msg, &xHigherPriorityTaskWoken);
@@ -229,6 +241,6 @@ void SystemTask::OnIdle() {
 }
 
 void SystemTask::ReloadIdleTimer() const {
-  if(isSleeping) return;
+  if(isSleeping || isGoingToSleep) return;
   xTimerReset(idleTimer, 0);
 }
diff --git a/src/SystemTask/SystemTask.h b/src/SystemTask/SystemTask.h
index 3812ea91..40277cf3 100644
--- a/src/SystemTask/SystemTask.h
+++ b/src/SystemTask/SystemTask.h
@@ -54,7 +54,9 @@ namespace Pinetime {
         Pinetime::Controllers::Ble& bleController;
         Pinetime::Controllers::DateTime& dateTimeController;
         QueueHandle_t systemTaksMsgQueue;
-        bool isSleeping = false;
+        std::atomic<bool> isSleeping{false};
+        std::atomic<bool> isGoingToSleep{false};
+        std::atomic<bool> isWakingUp{false};
         Pinetime::Drivers::Watchdog watchdog;
         Pinetime::Drivers::WatchdogView watchdogView;
         Pinetime::Controllers::NotificationManager& notificationManager;
diff --git a/src/drivers/TwiMaster.cpp b/src/drivers/TwiMaster.cpp
index 14d12f9d..a9eb5d0c 100644
--- a/src/drivers/TwiMaster.cpp
+++ b/src/drivers/TwiMaster.cpp
@@ -140,9 +140,11 @@ void TwiMaster::Write(uint8_t deviceAddress, const uint8_t *data, size_t size, b
 }
 
 void TwiMaster::Sleep() {
+  while(twiBaseAddress->ENABLE != 0) {
+    twiBaseAddress->ENABLE = (TWIM_ENABLE_ENABLE_Disabled << TWIM_ENABLE_ENABLE_Pos);
+  }
   nrf_gpio_cfg_default(6);
   nrf_gpio_cfg_default(7);
-  twiBaseAddress->ENABLE = 0;
   NRF_LOG_INFO("[TWIMASTER] Sleep");
 }