InfiniTime.git

diff --git a/src/SystemTask/SystemTask.cpp b/src/SystemTask/SystemTask.cpp
index b515d1abc00dfc5d0f2181095fde9eea5c70be74..222829d4bd55d7d4bb97a550f990c1073c8316bb 100644
--- a/src/SystemTask/SystemTask.cpp
+++ b/src/SystemTask/SystemTask.cpp
@@ -105,22 +105,33 @@     if (xQueueReceive(systemTaksMsgQueue, &msg, isSleeping?2500 : 1000)) {
       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 @@           bleDiscoveryTimer = 5;
           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 @@           touchPanel.Sleep();
 
           spi.Sleep();
           twiMaster.Sleep();
+          isSleeping = true;
+          isGoingToSleep = false;
           break;
         default: break;
       }
@@ -180,31 +193,27 @@   }
 }
 
 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::PushMessage(SystemTask::Messages msg) {
+  if(msg == Messages::GoToSleep) {
+    isGoingToSleep = true;
+  }
   BaseType_t xHigherPriorityTaskWoken;
   xHigherPriorityTaskWoken = pdFALSE;
   xQueueSendFromISR(systemTaksMsgQueue, &msg, &xHigherPriorityTaskWoken);
@@ -229,6 +241,6 @@   PushMessage(Messages::GoToSleep);
 }
 
 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 3812ea91082c910e0bdaed6e14b497daaabbdb93..40277cf30694dc3da8db75338416bfff5b000f46 100644
--- a/src/SystemTask/SystemTask.h
+++ b/src/SystemTask/SystemTask.h
@@ -54,7 +54,9 @@         std::unique_ptr displayApp;
         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 14d12f9de07d2071a467710495e60267515ed9c5..a9eb5d0c6d9cc33bafd24276999c25bf116c13d7 100644
--- a/src/drivers/TwiMaster.cpp
+++ b/src/drivers/TwiMaster.cpp
@@ -140,9 +140,11 @@   }
 }
 
 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");
 }