fw/driver/lptim_systick: improve stop mode stability

HXT48 clock is required for deepsleep mode. it effective LCPU and
wakeup time.

If HXT48 clock is disable, LCPU may enter undifinied state, then BLE
meet issue.

For wakeup time, HXT48 ready spend nearly 6.2 ms.

Signed-off-by: HaiLong Yang <cameled@outlook.com>

Author: cameled

src/fw/drivers/sf32lb52/lptim_systick.c

@@ -24,8 +24,8 @@
 #include "bf0_hal_lptim.h"
 #include "bf0_hal_aon.h"
 
-/* LPRC10K frequency nearly 8~9 KHz, fixed to 8KHz for systick */
-#define SYSTICK_CLOCK_HZ       8000
+/* LPRC10K frequency nearly 9 KHz, fixed to 9KHz for systick */
+#define SYSTICK_CLOCK_HZ       9000
 #define SYSTICK_ONE_TICK_HZ    (SYSTICK_CLOCK_HZ / RTC_TICKS_HZ)
 
 #if !defined(configUSE_TICKLESS_IDLE) || (configUSE_TICKLESS_IDLE != 2)
@@ -58,6 +58,9 @@ void lptim_systick_init(void)
   HAL_HPAON_EnableWakeupSrc(HPAON_WAKEUP_SRC_LP2HP_IRQ, AON_PIN_MODE_HIGH); // LP2HP mailbox interrupt
   HAL_HPAON_EnableWakeupSrc(HPAON_WAKEUP_SRC_LP2HP_REQ, AON_PIN_MODE_HIGH); // LP2HP manual wakeup
 
+  // Explicit enable hxt48 for LCPU
+  hwp_hpsys_aon->DSCR |= HPSYS_AON_DSCR_HXT48_REQ;
+
   s_lptim_systick_initialized = true;
 }
 
@@ -77,16 +80,6 @@ void lptim_systick_enable(void)
   __HAL_LPTIM_START_CONTINUOUS(&s_lptim1_handle);
 }
 
-void lptim_systick_pause(void)
-{
-  /* NOP */
-}
-
-void lptim_systick_resume(void)
-{
-  /* NOP */
-}
-
 void lptim_systick_tickless_idle(uint32_t ticks_from_now)
 {
   // In tickless idle mode, use OCWE instead.

src/fw/freertos_application.c

@@ -84,31 +84,24 @@ static const RtcTicks EARLY_WAKEUP_TICKS = 2;
 static const RtcTicks MIN_STOP_TICKS = 5;
 #elif defined(MICRO_FAMILY_SF32LB52)
 //! Stop mode until this number of ticks before the next scheduled task
-static const RtcTicks EARLY_WAKEUP_TICKS = 10; // relative large to avid tasks.c:1960 assert
+static const RtcTicks EARLY_WAKEUP_TICKS = 2;
 //! Stop mode until this number of ticks before the next scheduled task
-static const RtcTicks MIN_STOP_TICKS = 15;
+static const RtcTicks MIN_STOP_TICKS = 5;
 #else
 #error "Unknown micro family"
 #endif
 
 // 1 second ticks so that we only wake up once every regular timer interval.
 static const RtcTicks MAX_STOP_TICKS = RTC_TICKS_HZ;
 
-extern void vPortSuppressTicksAndSleep( TickType_t xExpectedIdleTime ) {
 #if !defined(MICRO_FAMILY_SF32LB52)
+extern void vPortSuppressTicksAndSleep( TickType_t xExpectedIdleTime ) {
   if (!rtc_alarm_is_initialized() || !sleep_mode_is_allowed()) {
     // the RTC is not yet initialized to the point where it can wake us from sleep or sleep/stop
     // is disabled. Just returning will cause a busy loop where the caller thought we slept for
     // 0 ticks and will reevaluate what to do next (probably just try again).
     return;
   }
-#else
-  if (!lptim_systick_is_initialized() || !sleep_mode_is_allowed() ||
-      !ipc_queue_check_idle()) {
-    // To avoid LCPU enter incorrect state, make sure ipc queue is empty before enter stop mode.
-    return;
-  }
-#endif
 
   // Note: all tasks are suspended at this point, but we can still be interrupted
   // so the critical section is necessary. taskENTER_CRITICAL() is not used here
@@ -155,8 +148,6 @@ extern void vPortSuppressTicksAndSleep( TickType_t xExpectedIdleTime ) {
       // (and RTC0 is shut down), then we just end up measuring 0 here -- no
       // harm, no foul.
       uint32_t rtc_start = NRF_RTC0->COUNTER;
-#elif defined(MICRO_FAMILY_SF32LB52)
-      uint32_t counter_start = LPTIM1->CNT;
 #else
       // We assume that a WFI to trigger sleep mode will not last longer than 1
       // SysTick. (The SysTick INT doesn't automatically get suppressed) Thus,
@@ -181,13 +172,6 @@ extern void vPortSuppressTicksAndSleep( TickType_t xExpectedIdleTime ) {
         rtc_end += 0x1000000;
       uint32_t rtc_elapsed = rtc_end - rtc_start;
       uint32_t cycles_elapsed = rtc_elapsed * SystemCoreClock / 32768;
-#elif defined(MICRO_FAMILY_SF32LB52)
-      uint32_t counter_stop = LPTIM1->CNT;
-      if (counter_stop < counter_start) {
-        counter_stop += 0x10000;
-      }
-      uint32_t counter_elapsed = counter_stop - counter_start;
-      uint32_t cycles_elapsed = (counter_elapsed * RTC_TICKS_HZ) / 8000;
 #else
       uint32_t systick_stop = SysTick->VAL;
       uint32_t cycles_elapsed;
@@ -205,17 +189,12 @@ extern void vPortSuppressTicksAndSleep( TickType_t xExpectedIdleTime ) {
 
       // Go into stop mode until the wakeup_tick.
       s_last_ticks_commanded_in_stop = stop_duration;
-#if !defined(MICRO_FAMILY_SF32LB52)
+
       rtc_alarm_set(stop_duration);
       enter_stop_mode();
 
       RtcTicks ticks_elapsed = rtc_alarm_get_elapsed_ticks();
-#else
-      lptim_systick_tickless_idle((uint32_t)stop_duration);
-      enter_stop_mode();
 
-      uint32_t ticks_elapsed = lptim_systick_get_elapsed_ticks();
-#endif
       s_last_ticks_elapsed_in_stop = ticks_elapsed;
       vTaskStepTick(ticks_elapsed);
 
@@ -238,6 +217,96 @@ extern void vPortSuppressTicksAndSleep( TickType_t xExpectedIdleTime ) {
   __enable_irq();
 }
 
+#else
+extern void vPortSuppressTicksAndSleep( TickType_t xExpectedIdleTime ) {
+  if (!lptim_systick_is_initialized() || !sleep_mode_is_allowed() ||
+      !ipc_queue_check_idle()) {
+    // To avoid LCPU enter incorrect state, make sure ipc queue is empty before enter stop mode.
+    return;
+  }
+
+  // Note: all tasks are suspended at this point, but we can still be interrupted
+  // so the critical section is necessary. taskENTER_CRITICAL() is not used here
+  // as that method would mask interrupts that should exit the low-power mode.
+  // The __disable_irq() function sets the PRIMASK bit which globally prevents
+  // interrupt execution while still allowing interrupts to wake the processor
+  // from WFI.
+  // Conversely, taskEnter_CRITICAL() sets the BASEPRI register, which masks
+  // interrupts with priorities lower than configMAX_SYSCALL_INTERRUPT_PRIORITY
+  // from executing and from waking the processor.
+  // See: http://infocenter.arm.com/help/topic/com.arm.doc.dui0552a/BABGGICD.html#BGBHDHAI
+  __disable_irq();
+
+  power_tracking_stop(PowerSystemMcuCoreRun);
+
+  if (eTaskConfirmSleepModeStatus() != eAbortSleep) {
+    if (xExpectedIdleTime < MIN_STOP_TICKS || !stop_mode_is_allowed()) {
+      uint32_t counter_start = LPTIM1->CNT;
+
+      power_tracking_start(PowerSystemMcuCoreSleep);
+      // HAL_GPIO_WritePin(DEBUG_PIN_CONFIG.gpio, DEBUG_PIN_CONFIG.gpio_pin, false);
+  
+      __DSB();  // Drain any pending memory writes before entering sleep.
+
+      __WFI();
+      __NOP();
+      __NOP();
+      __NOP();
+      __NOP();
+      __NOP();
+      __NOP();
+      __NOP();
+      __NOP();
+      __NOP();
+      __NOP();
+
+      __ISB();  // Let the pipeline catch up (force the WFI to activate before moving on).
+
+      // HAL_GPIO_WritePin(DEBUG_PIN_CONFIG.gpio, DEBUG_PIN_CONFIG.gpio_pin, true);
+      power_tracking_stop(PowerSystemMcuCoreSleep);
+
+      uint32_t counter_stop = LPTIM1->CNT;
+      if (counter_stop < counter_start) {
+        counter_stop += 0x10000;
+      }
+      uint32_t counter_elapsed = counter_stop - counter_start;
+      uint32_t cycles_elapsed = (counter_elapsed * RTC_TICKS_HZ) / 9000;
+
+      s_analytics_device_sleep_cpu_cycles += cycles_elapsed;
+      s_analytics_app_sleep_cpu_cycles += cycles_elapsed;
+    } else {
+      const RtcTicks stop_duration = MIN(xExpectedIdleTime - EARLY_WAKEUP_TICKS, MAX_STOP_TICKS);
+
+      // Go into stop mode until the wakeup_tick.
+      s_last_ticks_commanded_in_stop = stop_duration;
+
+      lptim_systick_tickless_idle((uint32_t)stop_duration);
+
+      enter_stop_mode();
+
+      uint32_t ticks_elapsed = lptim_systick_get_elapsed_ticks();
+
+      s_last_ticks_elapsed_in_stop = ticks_elapsed;
+      vTaskStepTick(ticks_elapsed);
+
+      PBL_LOG(LOG_LEVEL_DEBUG, "Expected idle time remain: %lu", (uint32_t)xExpectedIdleTime - ticks_elapsed);
+
+      // Update the task watchdog every time we come out of STOP mode (which is
+      // at least once/second) since the timer peripheral will not have been
+      // incremented
+      task_watchdog_step_elapsed_time_ms((ticks_elapsed * 1000) / RTC_TICKS_HZ);
+
+      s_analytics_device_stop_ticks += ticks_elapsed;
+      s_analytics_app_stop_ticks += ticks_elapsed;
+    }
+  }
+
+  power_tracking_start(PowerSystemMcuCoreRun);
+
+  __enable_irq();
+}
+#endif
+
 void vApplicationStackOverflowHook(TaskHandle_t task_handle, signed char *name) {
   PebbleTask task = pebble_task_get_task_for_handle(task_handle);
 

src/fw/kernel/util/stop.c

@@ -107,8 +107,6 @@ void enter_stop_mode(void) {
   uint32_t dll2_freq;
   int clk_src;
 
-  lptim_systick_pause();
-
   clear_interrupt_setting();
 
   /* Wait flash cache idle */
@@ -125,6 +123,9 @@ void enter_stop_mode(void) {
   HAL_RCC_HCPU_DisableDLL1();
   HAL_RCC_HCPU_DisableDLL2();
 
+  HAL_HPAON_DISABLE_PAD();
+  HAL_HPAON_DISABLE_VHP();
+  
   HAL_HPAON_CLEAR_HP_ACTIVE();
   HAL_HPAON_SET_POWER_MODE(AON_PMR_DEEP_SLEEP);
 
@@ -139,6 +140,9 @@ void enter_stop_mode(void) {
   __NOP();
   __NOP();
   __NOP();
+  
+  HAL_HPAON_ENABLE_PAD();
+  HAL_HPAON_ENABLE_VHP();
 
   HAL_HPAON_SET_HP_ACTIVE();
   HAL_HPAON_CLEAR_POWER_MODE();
@@ -157,8 +161,6 @@ void enter_stop_mode(void) {
   HAL_Delay_us(0);
 
   restore_interrupt_setting();
-
-  lptim_systick_resume();
 }
 #else /* STM32 */
 void enter_stop_mode(void) {