From 1c96d51717e18af10ef73547df0533024979a618 Mon Sep 17 00:00:00 2001
From: Aidan MacDonald <amachronic@protonmail.com>
Date: Thu, 14 Nov 2024 15:06:07 +0000
Subject: [PATCH] arm: add ARMv7-M support for thread context switching

On ARMv7-M, stm/ldm instructions can't include SP, so we must
load and store that separately. This changes the order of
registers in the context struct, but it doesn't seem to be
accessed anywhere else so this shouldn't cause any problems.

Change-Id: Ie1cd23272f23384e030f51f0b76739624fa7332b
---
 firmware/asm/arm/thread.c | 11 +++++++++++
 firmware/asm/arm/thread.h | 15 +++++++++++++++
 2 files changed, 26 insertions(+)

diff --git a/firmware/asm/arm/thread.c b/firmware/asm/arm/thread.c
index 30df56e0d9..e6f180f4ec 100644
--- a/firmware/asm/arm/thread.c
+++ b/firmware/asm/arm/thread.c
@@ -61,7 +61,12 @@ static void __attribute__((naked)) USED_ATTR start_thread(void)
 static inline void store_context(void* addr)
 {
     asm volatile(
+#if defined(CPU_ARM_MICRO) && ARCH_VERSION >= 7
+        "stmia  %0, { r4-r11, lr }     \n"
+        "str    sp, [%0, #36]          \n"
+#else
         "stmia  %0, { r4-r11, sp, lr } \n"
+#endif
         : : "r" (addr)
     );
 }
@@ -85,7 +90,13 @@ static inline void load_context(const void* addr)
         "ldmiane %0, { r0, pc }         \n"
 #endif
 
+#if defined(CPU_ARM_MICRO) && ARCH_VERSION >= 7
+        "mov     r0, %0                 \n"
+        "ldmia   r0, { r4-r11, lr }     \n"
+        "ldr     sp, [r0, #36]          \n"
+#else
         "ldmia   %0, { r4-r11, sp, lr } \n" /* Load regs r4 to r14 from context */
+#endif
         END_ARM_ASM_SYNTAX_UNIFIED
         : : "r" (addr) : "r0" /* only! */
     );
diff --git a/firmware/asm/arm/thread.h b/firmware/asm/arm/thread.h
index 533a088979..1cfc3741da 100644
--- a/firmware/asm/arm/thread.h
+++ b/firmware/asm/arm/thread.h
@@ -21,6 +21,20 @@
 
 #include "config.h"
 
+#if defined(CPU_ARM_MICRO) && ARCH_VERSION >= 7
+/*
+ * Cortex-M cannot load/store SP with ldm/stm so we need to store it
+ * separately. This makes it slightly more efficient to store SP last
+ * so as not to split the register list.
+ */
+struct regs
+{
+    uint32_t r[8];  /*  0-28 - Registers r4-r11 */
+    uint32_t lr;    /*    32 - r14 (lr) */
+    uint32_t sp;    /*    36 - Stack pointer (r13) */
+    uint32_t start; /*    40 - Thread start address, or NULL when started */
+};
+#else
 struct regs
 {
     uint32_t r[8];  /*  0-28 - Registers r4-r11 */
@@ -28,6 +42,7 @@ struct regs
     uint32_t lr;    /*    36 - r14 (lr) */
     uint32_t start; /*    40 - Thread start address, or NULL when started */
 };
+#endif
 
 #if (CONFIG_PLATFORM & PLATFORM_HOSTED)
   #include <errno.h>