Manage lp_ticker delay at low level

LP TICKER mbed-os wrapper needs to be disabled as it introduces too much latencies.

LP TICKER wrapper has been disabled and we need to managed the HW constraints at low level:
- main HW constraint is that once the comparator has been programmed once,
driver cannot program it again before CMPOK HW flag is set, which takes about 3 30us cycles.

To make it even more complex, the driver also needs to cope with "LP ticker workaround"

See commit:

LP ticker workaround

    There is an errata in LPTIM specification that explains that CMP Flag
    condition is not an exact match (COUNTER = MATCH) but rather a
    comparison (COUNTER >= MATCH).

Also the disable interrupt is more complete now:
- always check sleep manager status and restore it
- remove irq_handler as comparator is always programed and might get called
eventually when LP TICK is restarted
- reset delayed_prog

Also in set_interrupt, make sure interrupt does not fire early.
If needed, we decide to slightly delay the tick to cope with the HW limitation to
make sure it will fire as soon as HW is capable.

Functions are called under critical section as they may be called from
the IRQ handler now, not only from driver layer.

Author: LMESTM

targets/TARGET_STM/lp_ticker.c

@@ -37,11 +37,21 @@
 
 #include "lp_ticker_api.h"
 #include "mbed_error.h"
+#include "mbed_power_mgmt.h"
+#include "platform/mbed_critical.h"
+#include <stdbool.h>
 
 #if !defined(LPTICKER_DELAY_TICKS) || (LPTICKER_DELAY_TICKS < 3)
 #warning "lpticker_delay_ticks value should be set to 3"
 #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,9 +68,13 @@ 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);
 
@@ -168,21 +182,32 @@ 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 handler. Let's mark it is now safe to write to LP counter */
+    lp_cmpok = true;
 }
 
 static void LPTIM1_IRQHandler(void)
 {
+    core_util_critical_section_enter();
+
     LptimHandle.Instance = LPTIM1;
 
     if (lp_Fired) {
         lp_Fired = 0;
+        /* 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);
         if (irq_handler) {
             irq_handler();
         }
@@ -193,17 +218,33 @@ static void LPTIM1_IRQHandler(void)
         if (__HAL_LPTIM_GET_IT_SOURCE(&LptimHandle, LPTIM_IT_CMPM) != RESET) {
             /* Clear Compare match flag */
             __HAL_LPTIM_CLEAR_FLAG(&LptimHandle, LPTIM_FLAG_CMPM);
-
             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,49 +258,102 @@ 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);
+    /* Always store the last requested timestamp */
+    lp_delayed_counter = timestamp;
+    NVIC_EnableIRQ(LPTIM1_IRQn);
 
-    if(timestamp < lp_ticker_read()) {
-        /*  Workaround, because limitation */
-        __HAL_LPTIM_CLEAR_FLAG(&LptimHandle, LPTIM_FLAG_CMPOK);
-        __HAL_LPTIM_COMPARE_SET(&LptimHandle, ~0);
+    /* 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 {
-        __HAL_LPTIM_CLEAR_FLAG(&LptimHandle, LPTIM_FLAG_CMPOK);
-        __HAL_LPTIM_COMPARE_SET(&LptimHandle, timestamp);
-    }
-
-    lp_ticker_clear_interrupt();
+        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 */
+        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;
+    /* In case we fire interrupt now, then cancel pending programing */
+    lp_delayed_prog = false;
     irq_handler = (void (*)(void))lp_ticker_irq_handler;
     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;
+    }
+    irq_handler = NULL;
+    lp_delayed_prog = false;
     NVIC_DisableIRQ(LPTIM1_IRQn);
+    NVIC_ClearPendingIRQ(LPTIM1_IRQn);
     LptimHandle.Instance = LPTIM1;
+
+    core_util_critical_section_exit();
 }
 
 void lp_ticker_clear_interrupt(void)
 {
+    core_util_critical_section_enter();
     LptimHandle.Instance = LPTIM1;
     __HAL_LPTIM_CLEAR_FLAG(&LptimHandle, LPTIM_FLAG_CMPM);
     NVIC_ClearPendingIRQ(LPTIM1_IRQn);
+    core_util_critical_section_exit();
 }
 
 void lp_ticker_free(void)