From b178958c0799f16847a70c7a5c717a39528407ec Mon Sep 17 00:00:00 2001
From: Damien George <damien.p.george@gmail.com>
Date: Fri, 8 Feb 2019 01:03:51 +1100
Subject: [PATCH] stm32/pendsv: Clean up pendsv IRQ handler and eliminate
 duplicate code.

---
 ports/stm32/pendsv.c   | 56 ++++++++++++++++++------------------------
 ports/stm32/pendsv.h   |  4 ---
 ports/stm32/stm32_it.c |  9 -------
 3 files changed, 24 insertions(+), 45 deletions(-)

diff --git a/ports/stm32/pendsv.c b/ports/stm32/pendsv.c
index b5fe42f70d..a6a9afc14e 100644
--- a/ports/stm32/pendsv.c
+++ b/ports/stm32/pendsv.c
@@ -61,11 +61,16 @@ void pendsv_kbd_intr(void) {
     }
 }
 
-void pendsv_isr_handler(void) {
-    // re-jig the stack so that when we return from this interrupt handler
+__attribute__((naked)) void PendSV_Handler(void) {
+    // Handle a PendSV interrupt
+    //
+    // For the case of an asynchronous exception, re-jig the
+    // stack so that when we return from this interrupt handler
     // it returns instead to nlr_jump with argument pendsv_object
     // note that stack has a different layout if DEBUG is enabled
     //
+    // For the case of a thread switch, swap stacks.
+    //
     // on entry to this (naked) function, stack has the following layout:
     //
     // stack layout with DEBUG disabled:
@@ -88,20 +93,30 @@ void pendsv_isr_handler(void) {
     //   sp[1]: 0xfffffff9
     //   sp[0]: ?
 
-#if MICROPY_PY_THREAD
     __asm volatile (
+        // Check if there is an active object to throw via nlr_jump
         "ldr r1, pendsv_object_ptr\n"
         "ldr r0, [r1]\n"
         "cmp r0, 0\n"
         "beq .no_obj\n"
+        #if defined(PENDSV_DEBUG)
+        "str r0, [sp, #8]\n"            // store to r0 on stack
+        #else
         "str r0, [sp, #0]\n"            // store to r0 on stack
+        #endif
         "mov r0, #0\n"
         "str r0, [r1]\n"                // clear pendsv_object
         "ldr r0, nlr_jump_ptr\n"
+        #if defined(PENDSV_DEBUG)
+        "str r0, [sp, #32]\n"           // store to pc on stack
+        #else
         "str r0, [sp, #24]\n"           // store to pc on stack
+        #endif
         "bx lr\n"                       // return from interrupt; will return to nlr_jump
-
         ".no_obj:\n"                    // pendsv_object==NULL
+
+        #if MICROPY_PY_THREAD
+        // Do a thread context switch
         "push {r4-r11, lr}\n"
         "vpush {s16-s31}\n"
         "mrs r5, primask\n"             // save PRIMASK in r5
@@ -115,37 +130,14 @@ void pendsv_isr_handler(void) {
         "vpop {s16-s31}\n"
         "pop {r4-r11, lr}\n"
         "bx lr\n"                       // return from interrupt; will return to new thread
-        ".align 2\n"
-        "pendsv_object_ptr: .word pendsv_object\n"
-        "nlr_jump_ptr: .word nlr_jump\n"
-    );
-#else
-    __asm volatile (
-        "ldr r0, pendsv_object_ptr\n"
-        "ldr r0, [r0]\n"
-#if defined(PENDSV_DEBUG)
-        "str r0, [sp, #8]\n"
-#else
-        "str r0, [sp, #0]\n"
-#endif
-        "ldr r0, nlr_jump_ptr\n"
-#if defined(PENDSV_DEBUG)
-        "str r0, [sp, #32]\n"
-#else
-        "str r0, [sp, #24]\n"
-#endif
+        #else
+        // Spurious pendsv, just return
         "bx lr\n"
+        #endif
+
+        // Data
         ".align 2\n"
         "pendsv_object_ptr: .word pendsv_object\n"
         "nlr_jump_ptr: .word nlr_jump\n"
     );
-#endif
-
-    /*
-    uint32_t x[2] = {0x424242, 0xdeaddead};
-    printf("PendSV: %p\n", x);
-    for (uint32_t *p = (uint32_t*)(((uint32_t)x - 15) & 0xfffffff0), i = 64; i > 0; p += 4, i -= 4) {
-        printf(" %p: %08x %08x %08x %08x\n", p, (uint)p[0], (uint)p[1], (uint)p[2], (uint)p[3]);
-    }
-    */
 }
diff --git a/ports/stm32/pendsv.h b/ports/stm32/pendsv.h
index 0d9fde6878..52ce805366 100644
--- a/ports/stm32/pendsv.h
+++ b/ports/stm32/pendsv.h
@@ -29,8 +29,4 @@
 void pendsv_init(void);
 void pendsv_kbd_intr(void);
 
-// since we play tricks with the stack, the compiler must not generate a
-// prelude for this function
-void pendsv_isr_handler(void) __attribute__((naked));
-
 #endif // MICROPY_INCLUDED_STM32_PENDSV_H
diff --git a/ports/stm32/stm32_it.c b/ports/stm32/stm32_it.c
index 49b8ff1a8b..88e7f5bfec 100644
--- a/ports/stm32/stm32_it.c
+++ b/ports/stm32/stm32_it.c
@@ -290,15 +290,6 @@ void SVC_Handler(void) {
 void DebugMon_Handler(void) {
 }
 
-/**
-  * @brief  This function handles PendSVC exception.
-  * @param  None
-  * @retval None
-  */
-void PendSV_Handler(void) {
-    pendsv_isr_handler();
-}
-
 /******************************************************************************/
 /*                 STM32F4xx Peripherals Interrupt Handlers                   */
 /*  Add here the Interrupt Handler for the used peripheral(s) (PPP), for the  */