STM: replace C++ low power ticker wrapper with a low level wrapper

targets/TARGET_STM/lp_ticker.c

@@ -37,11 +37,24 @@
 
 #include "lp_ticker_api.h"
 #include "mbed_error.h"
-
-#if !defined(LPTICKER_DELAY_TICKS) || (LPTICKER_DELAY_TICKS < 3)
-#warning "lpticker_delay_ticks value should be set to 3"
+#include "mbed_power_mgmt.h"
+#include "platform/mbed_critical.h"
+#include <stdbool.h>
+
+/*  lpticker delay is for using C++ Low Power Ticker wrapper,
+ *  which introduces extra delays. We rather want to use the
+ *  low level implementation from this file */
+#if defined(LPTICKER_DELAY_TICKS) && (LPTICKER_DELAY_TICKS > 0)
+#warning "lpticker_delay_ticks usage not recommended"
 #endif
 
+#define LP_TIMER_WRAP(val) (val & 0xFFFF)
+/* Safe guard is the number of ticks between the current tick and the next
+ * tick we want to program an interrupt for. Programing an interrupt in
+ * between is unreliable */
+#define LP_TIMER_SAFE_GUARD 5
+
+
 LPTIM_HandleTypeDef LptimHandle;
 
 const ticker_info_t *lp_ticker_get_info()
@@ -58,11 +71,14 @@ const ticker_info_t *lp_ticker_get_info()
 }
 
 volatile uint8_t  lp_Fired = 0;
+/*  Flag and stored counter to handle delayed programing at low level */
+volatile bool lp_delayed_prog = false;
+volatile bool lp_cmpok = false;
+volatile timestamp_t lp_delayed_counter = 0;
+volatile bool sleep_manager_locked = false;
 
 static int LPTICKER_inited = 0;
-
 static void LPTIM1_IRQHandler(void);
-static void (*irq_handler)(void);
 
 void lp_ticker_init(void)
 {
@@ -168,42 +184,63 @@ void lp_ticker_init(void)
 #endif
 
     __HAL_LPTIM_ENABLE_IT(&LptimHandle, LPTIM_IT_CMPM);
+    __HAL_LPTIM_ENABLE_IT(&LptimHandle, LPTIM_IT_CMPOK);
     HAL_LPTIM_Counter_Start(&LptimHandle, 0xFFFF);
 
     /* Need to write a compare value in order to get LPTIM_FLAG_CMPOK in set_interrupt */
     __HAL_LPTIM_CLEAR_FLAG(&LptimHandle, LPTIM_FLAG_CMPOK);
     __HAL_LPTIM_COMPARE_SET(&LptimHandle, 0);
     while (__HAL_LPTIM_GET_FLAG(&LptimHandle, LPTIM_FLAG_CMPOK) == RESET) {
     }
+    __HAL_LPTIM_CLEAR_FLAG(&LptimHandle, LPTIM_FLAG_CMPOK);
+
+    /* Init is called with Interrupts disabled, so the CMPOK interrupt
+     * will not be handled. Let's mark it is now safe to write to LP counter */
+    lp_cmpok = true;
 }
 
 static void LPTIM1_IRQHandler(void)
 {
-    LptimHandle.Instance = LPTIM1;
+    core_util_critical_section_enter();
 
     if (lp_Fired) {
         lp_Fired = 0;
-        if (irq_handler) {
-            irq_handler();
-        }
+        /* We're already in handler and interrupt might be pending,
+         * so clear the flag, to avoid calling irq_handler twice */
+        __HAL_LPTIM_CLEAR_FLAG(&LptimHandle, LPTIM_FLAG_CMPM);
+        lp_ticker_irq_handler();
     }
 
     /* Compare match interrupt */
     if (__HAL_LPTIM_GET_FLAG(&LptimHandle, LPTIM_FLAG_CMPM) != RESET) {
         if (__HAL_LPTIM_GET_IT_SOURCE(&LptimHandle, LPTIM_IT_CMPM) != RESET) {
             /* Clear Compare match flag */
             __HAL_LPTIM_CLEAR_FLAG(&LptimHandle, LPTIM_FLAG_CMPM);
+            lp_ticker_irq_handler();
+        }
+    }
 
-            if (irq_handler) {
-                irq_handler();
+    if (__HAL_LPTIM_GET_FLAG(&LptimHandle, LPTIM_FLAG_CMPOK) != RESET) {
+        if (__HAL_LPTIM_GET_IT_SOURCE(&LptimHandle, LPTIM_IT_CMPOK) != RESET) {
+            __HAL_LPTIM_CLEAR_FLAG(&LptimHandle, LPTIM_FLAG_CMPOK);
+            lp_cmpok = true;
+            if(sleep_manager_locked) {
+                sleep_manager_unlock_deep_sleep();
+                sleep_manager_locked = false;
+            }
+            if(lp_delayed_prog) {
+                lp_ticker_set_interrupt(lp_delayed_counter);
+                lp_delayed_prog = false;
             }
         }
     }
 
+
 #if defined (__HAL_LPTIM_WAKEUPTIMER_EXTI_CLEAR_FLAG)
     /* EXTI lines are not configured by default */
     __HAL_LPTIM_WAKEUPTIMER_EXTI_CLEAR_FLAG();
 #endif
+    core_util_critical_section_exit();
 }
 
 uint32_t lp_ticker_read(void)
@@ -217,42 +254,107 @@ uint32_t lp_ticker_read(void)
     return lp_time;
 }
 
+/*  This function should always be called from critical section */
 void lp_ticker_set_interrupt(timestamp_t timestamp)
 {
-    LptimHandle.Instance = LPTIM1;
-    irq_handler = (void (*)(void))lp_ticker_irq_handler;
+    core_util_critical_section_enter();
 
-    /* CMPOK is set by hardware to inform application that the APB bus write operation to the LPTIM_CMP register has been successfully completed */
-    /* Any successive write before the CMPOK flag be set, will lead to unpredictable results */
-    /* LPTICKER_DELAY_TICKS value prevents OS to call this set interrupt function before CMPOK */
-    MBED_ASSERT(__HAL_LPTIM_GET_FLAG(&LptimHandle, LPTIM_FLAG_CMPOK) == SET);
-    __HAL_LPTIM_CLEAR_FLAG(&LptimHandle, LPTIM_FLAG_CMPOK);
-    __HAL_LPTIM_COMPARE_SET(&LptimHandle, timestamp);
+    /* Always store the last requested timestamp */
+    lp_delayed_counter = timestamp;
+    NVIC_EnableIRQ(LPTIM1_IRQn);
 
-    lp_ticker_clear_interrupt();
+    /* CMPOK is set by hardware to inform application that the APB bus write operation to the
+     * LPTIM_CMP register has been successfully completed.
+     * Any successive write before the CMPOK flag be set, will lead to unpredictable results
+     * We need to prevent to set a new comparator value before CMPOK flag is set by HW */
+    if (lp_cmpok == false) {
+        /* if this is not safe to write, then delay the programing to the
+         * time when CMPOK interrupt will trigger */
+        lp_delayed_prog = true;
+    } else {
+        timestamp_t last_read_counter = lp_ticker_read();
+        lp_ticker_clear_interrupt();
+
+        /*  HW is not able to trig a very short term interrupt, that is
+         *  not less than few ticks away (LP_TIMER_SAFE_GUARD). So let's make sure it'
+         *  s at least current tick + LP_TIMER_SAFE_GUARD */
+        for(uint8_t i = 0; i < LP_TIMER_SAFE_GUARD; i++) {
+            if (LP_TIMER_WRAP(last_read_counter  + i) == timestamp) {
+                timestamp = LP_TIMER_WRAP(timestamp + LP_TIMER_SAFE_GUARD);
+            }
+        }
+        /* Then check if this target timestamp is not in the past, or close to wrap-around
+         * Let's assume last_read_counter = 0xFFFC, and we want to program timestamp = 0x100
+         * The interrupt will not fire before the CMPOK flag is OK, so there are 2 cases:
+         * in case CMPOK flag is set by HW after or at wrap-around, then this will fire only @0x100
+         * in case CMPOK flag is set before, it will indeed fire early, as for the wrap-around case.
+         * But that will take at least 3 cycles and the interrupt fires at the end of a cycle.
+         * In our case 0xFFFC + 3 => at the transition between 0xFFFF and 0.
+         * If last_read_counter was 0xFFFB, it should be at the transition between 0xFFFE and 0xFFFF.
+         * There might be crossing cases where it would also fire @ 0xFFFE, but by the time we read the counter,
+         * it may already have moved to the next one, so for now we've taken this as margin of error.
+         */
+         if((timestamp < last_read_counter) && (last_read_counter <= (0xFFFF - LP_TIMER_SAFE_GUARD))) {
+            /*  Workaround, because limitation */
+            __HAL_LPTIM_COMPARE_SET(&LptimHandle, ~0);
+        } else {
+            /*  It is safe to write */
+            __HAL_LPTIM_COMPARE_SET(&LptimHandle, timestamp);
+        }
 
-    NVIC_EnableIRQ(LPTIM1_IRQn);
+        /* We just programed the CMP so we'll need to wait for cmpok before
+         * next programing */
+        lp_cmpok = false;
+        /*  Prevent from sleeping after compare register was set as we need CMPOK
+         *  interrupt to fire (in ~3x30us cycles) before we can safely enter deep sleep mode */
+        if(!sleep_manager_locked) {
+            sleep_manager_lock_deep_sleep();
+            sleep_manager_locked = true;
+        }
+    }
+    core_util_critical_section_exit();
 }
 
 void lp_ticker_fire_interrupt(void)
 {
+    core_util_critical_section_enter();
     lp_Fired = 1;
-    irq_handler = (void (*)(void))lp_ticker_irq_handler;
+    /* In case we fire interrupt now, then cancel pending programing */
+    lp_delayed_prog = false;
     NVIC_SetPendingIRQ(LPTIM1_IRQn);
     NVIC_EnableIRQ(LPTIM1_IRQn);
+    core_util_critical_section_exit();
 }
 
 void lp_ticker_disable_interrupt(void)
 {
+    core_util_critical_section_enter();
+
+    if(!lp_cmpok) {
+        while (__HAL_LPTIM_GET_FLAG(&LptimHandle, LPTIM_FLAG_CMPOK) == RESET) {
+        }
+        __HAL_LPTIM_CLEAR_FLAG(&LptimHandle, LPTIM_FLAG_CMPOK);
+        lp_cmpok = true;
+    }
+    /*  now that CMPOK is set, allow deep sleep again */
+    if(sleep_manager_locked) {
+        sleep_manager_unlock_deep_sleep();
+        sleep_manager_locked = false;
+    }
+    lp_delayed_prog = false;
+    lp_Fired = 0;
     NVIC_DisableIRQ(LPTIM1_IRQn);
-    LptimHandle.Instance = LPTIM1;
+    NVIC_ClearPendingIRQ(LPTIM1_IRQn);
+
+    core_util_critical_section_exit();
 }
 
 void lp_ticker_clear_interrupt(void)
 {
-    LptimHandle.Instance = LPTIM1;
+    core_util_critical_section_enter();
     __HAL_LPTIM_CLEAR_FLAG(&LptimHandle, LPTIM_FLAG_CMPM);
     NVIC_ClearPendingIRQ(LPTIM1_IRQn);
+    core_util_critical_section_exit();
 }
 
 void lp_ticker_free(void)