From 18cfa156d628873dfe0e12bcaad19bd802f32994 Mon Sep 17 00:00:00 2001
From: Damien George <damien.p.george@gmail.com>
Date: Fri, 8 Feb 2019 01:11:21 +1100
Subject: [PATCH] stm32/pendsv: Add ability to schedule callbacks at pendsv IRQ
 level.

---
 ports/stm32/pendsv.c | 40 ++++++++++++++++++++++++++++++++++++++++
 ports/stm32/pendsv.h |  5 +++++
 2 files changed, 45 insertions(+)

diff --git a/ports/stm32/pendsv.c b/ports/stm32/pendsv.c
index a6a9afc14e..6c6b3c7bff 100644
--- a/ports/stm32/pendsv.c
+++ b/ports/stm32/pendsv.c
@@ -38,7 +38,15 @@
 // mp_kbd_exception which is in the root-pointer set.
 void *pendsv_object;
 
+#if PENDSV_DISPATCH_NUM_SLOTS > 0
+uint32_t pendsv_dispatch_active;
+pendsv_dispatch_t pendsv_dispatch_table[PENDSV_DISPATCH_NUM_SLOTS];
+#endif
+
 void pendsv_init(void) {
+    #if PENDSV_DISPATCH_NUM_SLOTS > 0
+    pendsv_dispatch_active = false;
+    #endif
     // set PendSV interrupt at lowest priority
     NVIC_SetPriority(PendSV_IRQn, IRQ_PRI_PENDSV);
 }
@@ -61,6 +69,23 @@ void pendsv_kbd_intr(void) {
     }
 }
 
+#if PENDSV_DISPATCH_NUM_SLOTS > 0
+void pendsv_schedule_dispatch(size_t slot, pendsv_dispatch_t f) {
+    pendsv_dispatch_table[slot] = f;
+    pendsv_dispatch_active = true;
+    SCB->ICSR = SCB_ICSR_PENDSVSET_Msk;
+}
+
+void pendsv_dispatch_handler(void) {
+    for (size_t i = 0; i < PENDSV_DISPATCH_NUM_SLOTS; ++i) {
+        if (pendsv_dispatch_table[i] != NULL) {
+            pendsv_dispatch_table[i] = NULL;
+            pendsv_dispatch_table[i]();
+        }
+    }
+}
+#endif
+
 __attribute__((naked)) void PendSV_Handler(void) {
     // Handle a PendSV interrupt
     //
@@ -94,6 +119,18 @@ __attribute__((naked)) void PendSV_Handler(void) {
     //   sp[0]: ?
 
     __asm volatile (
+        #if PENDSV_DISPATCH_NUM_SLOTS > 0
+        // Check if there are any pending calls to dispatch to
+        "ldr r1, pendsv_dispatch_active_ptr\n"
+        "ldr r0, [r1]\n"
+        "cmp r0, 0\n"
+        "beq .no_dispatch\n"
+        "mov r2, #0\n"
+        "str r2, [r1]\n"                // clear pendsv_dispatch_active
+        "b pendsv_dispatch_handler\n"   // jump to the handler
+        ".no_dispatch:\n"
+        #endif
+
         // Check if there is an active object to throw via nlr_jump
         "ldr r1, pendsv_object_ptr\n"
         "ldr r0, [r1]\n"
@@ -137,6 +174,9 @@ __attribute__((naked)) void PendSV_Handler(void) {
 
         // Data
         ".align 2\n"
+        #if PENDSV_DISPATCH_NUM_SLOTS > 0
+        "pendsv_dispatch_active_ptr: .word pendsv_dispatch_active\n"
+        #endif
         "pendsv_object_ptr: .word pendsv_object\n"
         "nlr_jump_ptr: .word nlr_jump\n"
     );
diff --git a/ports/stm32/pendsv.h b/ports/stm32/pendsv.h
index 52ce805366..c8cc64d762 100644
--- a/ports/stm32/pendsv.h
+++ b/ports/stm32/pendsv.h
@@ -26,7 +26,12 @@
 #ifndef MICROPY_INCLUDED_STM32_PENDSV_H
 #define MICROPY_INCLUDED_STM32_PENDSV_H
 
+#define PENDSV_DISPATCH_NUM_SLOTS (0)
+
+typedef void (*pendsv_dispatch_t)(void);
+
 void pendsv_init(void);
 void pendsv_kbd_intr(void);
+void pendsv_schedule_dispatch(size_t slot, pendsv_dispatch_t f);
 
 #endif // MICROPY_INCLUDED_STM32_PENDSV_H