stm32: Add soft timer implementation, using SysTick at 1ms resolution.

This commit adds an implementation of a "software timer" with a 1ms
resolution, using SysTick.  It allows unlimited number of concurrent
timers (limited only by memory needed for each timer entry).  They can be
one-shot or periodic, and associated with a Python callback.

There is a very small overhead added to the SysTick IRQ, which could be
further optimised in the future, eg by patching SysTick_Handler code
dynamically.

7 files changed, 174 insertions, 2 deletions

diff --git a/ports/stm32/Makefile b/ports/stm32/Makefile
index fca92f841..82711eca6 100644
--- a/ports/stm32/Makefile
+++ b/ports/stm32/Makefile
@@ -249,6 +249,7 @@ SRC_C = \
 	irq.c \
 	pendsv.c \
 	systick.c  \
+	softtimer.c  \
 	powerctrl.c \
 	powerctrlboot.c \
 	pybthread.c \
diff --git a/ports/stm32/main.c b/ports/stm32/main.c
index 685dbd10c..a00bdf3bd 100644
--- a/ports/stm32/main.c
+++ b/ports/stm32/main.c
@@ -55,6 +55,7 @@
 #include "gccollect.h"
 #include "factoryreset.h"
 #include "modmachine.h"
+#include "softtimer.h"
 #include "i2c.h"
 #include "spi.h"
 #include "uart.h"
@@ -743,6 +744,7 @@ soft_reset_exit:
     #if MICROPY_PY_NETWORK
     mod_network_deinit();
     #endif
+    soft_timer_deinit();
     timer_deinit();
     uart_deinit_all();
     #if MICROPY_HW_ENABLE_CAN
diff --git a/ports/stm32/mpconfigport.h b/ports/stm32/mpconfigport.h
index b31eed293..11c237238 100644
--- a/ports/stm32/mpconfigport.h
+++ b/ports/stm32/mpconfigport.h
@@ -282,6 +282,8 @@ struct _mp_bluetooth_nimble_root_pointers_t;
     \
     mp_obj_t pyb_extint_callback[PYB_EXTI_NUM_VECTORS]; \
     \
+    struct _soft_timer_entry_t *soft_timer_head; \
+    \
     /* pointers to all Timer objects (if they have been created) */ \
     struct _pyb_timer_obj_t *pyb_timer_obj_all[MICROPY_HW_MAX_TIMER]; \
     \
diff --git a/ports/stm32/pendsv.h b/ports/stm32/pendsv.h
index 9851a5ece..dd3f8f2cb 100644
--- a/ports/stm32/pendsv.h
+++ b/ports/stm32/pendsv.h
@@ -27,6 +27,7 @@
 #define MICROPY_INCLUDED_STM32_PENDSV_H
 
 enum {
+    PENDSV_DISPATCH_SOFT_TIMER,
     #if MICROPY_PY_NETWORK && MICROPY_PY_LWIP
     PENDSV_DISPATCH_LWIP,
     #if MICROPY_PY_NETWORK_CYW43
@@ -39,9 +40,7 @@ enum {
     PENDSV_DISPATCH_MAX
 };
 
-#if (MICROPY_PY_NETWORK && MICROPY_PY_LWIP) || (MICROPY_PY_BLUETOOTH && MICROPY_BLUETOOTH_NIMBLE)
 #define PENDSV_DISPATCH_NUM_SLOTS PENDSV_DISPATCH_MAX
-#endif
 
 typedef void (*pendsv_dispatch_t)(void);
 
diff --git a/ports/stm32/softtimer.c b/ports/stm32/softtimer.c
new file mode 100644
index 000000000..c598bac17
--- /dev/null
+++ b/ports/stm32/softtimer.c
@@ -0,0 +1,112 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2019 Damien P. George
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <stdint.h>
+#include "py/runtime.h"
+#include "irq.h"
+#include "softtimer.h"
+
+#define TICKS_PERIOD 0x80000000
+#define TICKS_DIFF(t1, t0) ((int32_t)(((t1 - t0 + TICKS_PERIOD / 2) & (TICKS_PERIOD - 1)) - TICKS_PERIOD / 2))
+
+extern __IO uint32_t uwTick;
+
+volatile uint32_t soft_timer_next;
+
+void soft_timer_deinit(void) {
+    MP_STATE_PORT(soft_timer_head) = NULL;
+}
+
+STATIC void soft_timer_schedule_systick(uint32_t ticks_ms) {
+    uint32_t irq_state = disable_irq();
+    uint32_t uw_tick = uwTick;
+    if (TICKS_DIFF(ticks_ms, uw_tick) <= 0) {
+        soft_timer_next = uw_tick + 1;
+    } else {
+        soft_timer_next = ticks_ms;
+    }
+    enable_irq(irq_state);
+}
+
+// Must be executed at IRQ_PRI_PENDSV
+void soft_timer_handler(void) {
+    uint32_t ticks_ms = uwTick;
+    soft_timer_entry_t *head = MP_STATE_PORT(soft_timer_head);
+    while (head != NULL && TICKS_DIFF(head->expiry_ms, ticks_ms) <= 0) {
+        mp_sched_schedule(head->callback, MP_OBJ_FROM_PTR(head));
+        if (head->mode == SOFT_TIMER_MODE_PERIODIC) {
+            head->expiry_ms += head->delta_ms;
+            // Shift this node along to its new position
+            soft_timer_entry_t *cur = head;
+            while (cur->next != NULL && TICKS_DIFF(head->expiry_ms, cur->next->expiry_ms) >= 0) {
+                cur = cur->next;
+            }
+            if (cur != head) {
+                soft_timer_entry_t *next = head->next;
+                head->next = cur->next;
+                cur->next = head;
+                head = next;
+            }
+        } else {
+            head = head->next;
+        }
+    }
+    MP_STATE_PORT(soft_timer_head) = head;
+    if (head == NULL) {
+        // No more timers left, set largest delay possible
+        soft_timer_next = uwTick;
+    } else {
+        // Set soft_timer_next so SysTick calls us back at the correct time
+        soft_timer_schedule_systick(head->expiry_ms);
+    }
+}
+
+void soft_timer_insert(soft_timer_entry_t *entry) {
+    uint32_t irq_state = raise_irq_pri(IRQ_PRI_PENDSV);
+    soft_timer_entry_t **head_ptr = &MP_STATE_PORT(soft_timer_head);
+    while (*head_ptr != NULL && TICKS_DIFF(entry->expiry_ms, (*head_ptr)->expiry_ms) >= 0) {
+        head_ptr = &(*head_ptr)->next;
+    }
+    entry->next = *head_ptr;
+    *head_ptr = entry;
+    if (head_ptr == &MP_STATE_PORT(soft_timer_head)) {
+        // This new timer became the earliest one so set soft_timer_next
+        soft_timer_schedule_systick((*head_ptr)->expiry_ms);
+    }
+    restore_irq_pri(irq_state);
+}
+
+void soft_timer_remove(soft_timer_entry_t *entry) {
+    uint32_t irq_state = raise_irq_pri(IRQ_PRI_PENDSV);
+    soft_timer_entry_t **cur = &MP_STATE_PORT(soft_timer_head);
+    while (*cur != NULL) {
+        if (*cur == entry) {
+            *cur = entry->next;
+            break;
+        }
+    }
+    restore_irq_pri(irq_state);
+}
diff --git a/ports/stm32/softtimer.h b/ports/stm32/softtimer.h
new file mode 100644
index 000000000..2550446d8
--- /dev/null
+++ b/ports/stm32/softtimer.h
@@ -0,0 +1,50 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2019 Damien P. George
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#ifndef MICROPY_INCLUDED_STM32_SOFTTIMER_H
+#define MICROPY_INCLUDED_STM32_SOFTTIMER_H
+
+#include "py/obj.h"
+
+#define SOFT_TIMER_MODE_ONE_SHOT (1)
+#define SOFT_TIMER_MODE_PERIODIC (2)
+
+typedef struct _soft_timer_entry_t {
+    mp_obj_base_t base; // so struct can be used as an object and still be traced by GC
+    struct _soft_timer_entry_t *next;
+    uint32_t mode;
+    uint32_t expiry_ms;
+    uint32_t delta_ms; // for periodic mode
+    mp_obj_t callback;
+} soft_timer_entry_t;
+
+extern volatile uint32_t soft_timer_next;
+
+void soft_timer_deinit(void);
+void soft_timer_handler(void);
+void soft_timer_insert(soft_timer_entry_t *entry);
+void soft_timer_remove(soft_timer_entry_t *entry);
+
+#endif // MICROPY_INCLUDED_STM32_SOFTTIMER_H
diff --git a/ports/stm32/systick.c b/ports/stm32/systick.c
index d92f9d75d..85c99b9ae 100644
--- a/ports/stm32/systick.c
+++ b/ports/stm32/systick.c
@@ -27,7 +27,9 @@
 #include "py/runtime.h"
 #include "py/mphal.h"
 #include "irq.h"
+#include "pendsv.h"
 #include "systick.h"
+#include "softtimer.h"
 #include "pybthread.h"
 
 extern __IO uint32_t uwTick;
@@ -52,6 +54,10 @@ void SysTick_Handler(void) {
         f(uw_tick);
     }
 
+    if (soft_timer_next == uw_tick) {
+        pendsv_schedule_dispatch(PENDSV_DISPATCH_SOFT_TIMER, soft_timer_handler);
+    }
+
     #if MICROPY_PY_THREAD
     if (pyb_thread_enabled) {
         if (pyb_thread_cur->timeslice == 0) {