From 8edbd631d34ec932da62b5bb1df6d49a9026c2d3 Mon Sep 17 00:00:00 2001
From: Elena Frank <elena.frank@protonmail.com>
Date: Fri, 26 Jul 2024 12:02:49 +0200
Subject: [PATCH] fix(threads/armv8): don't save high regs on first schedule

With #262, the `current_pid` is not set before the scheduler is triggered
for the first time, and therefore `None` on the first run.
The pointer to `current_high_regs` is therefore set to a null pointer.
This causes nrf52840 to hard fault when trying to store registers r4-r11.

To fix this, we now skip storing/ loading of r4-r11 if the pointer is
zero, i.e. when there was no previously running thread whose context has
to be saved.
---
 src/riot-rs-threads/src/arch/cortex_m.rs | 32 ++++++++++++++----------
 1 file changed, 19 insertions(+), 13 deletions(-)

diff --git a/src/riot-rs-threads/src/arch/cortex_m.rs b/src/riot-rs-threads/src/arch/cortex_m.rs
index e4ff28df3..0230ed3f5 100644
--- a/src/riot-rs-threads/src/arch/cortex_m.rs
+++ b/src/riot-rs-threads/src/arch/cortex_m.rs
@@ -84,9 +84,14 @@ unsafe extern "C" fn PendSV() {
              * so let's use '99' forward ('99f')
              */
             beq 99f
+
+            msr.n psp, r0
+
+            cmp r1, #0
+            beq 99f
+
             stmia r1, {{r4-r11}}
             ldmia r2, {{r4-r11}}
-            msr.n psp, r0
             99:
             movw LR, #0xFFFd
             movt LR, #0xFFFF
@@ -110,6 +115,11 @@ unsafe extern "C" fn PendSV() {
             cmp r0, #0
             beq 99f
 
+            msr.n psp, r0
+
+            cmp r1, #0
+            beq 99f
+
             //stmia r1!, {{r4-r7}}
             str r4, [r1, #16]
             str r5, [r1, #20]
@@ -134,7 +144,6 @@ unsafe extern "C" fn PendSV() {
             mov r8,  r4
             ldmia r2!, {{r4-r7}}
 
-            msr.n psp, r0
             99:
             ldr r0, 999f
             mov LR, r0
@@ -183,25 +192,22 @@ unsafe fn sched() -> u128 {
                 }
             };
 
-            let current_high_regs;
+            let mut current_high_regs = core::ptr::null();
             if let Some(current_pid) = threads.current_pid() {
                 if next_pid == current_pid {
                     return Some(0);
                 }
 
-                threads.threads[usize::from(current_pid)].sp =
-                    cortex_m::register::psp::read() as usize;
-                threads.current_thread = Some(next_pid);
+                let current = &mut threads.threads[usize::from(current_pid)];
+                current.sp = cortex_m::register::psp::read() as usize;
+                current_high_regs = current.data.as_ptr();
+            }
 
-                current_high_regs = threads.threads[usize::from(current_pid)].data.as_ptr();
-            } else {
-                threads.current_thread = Some(next_pid);
-                current_high_regs = core::ptr::null();
-            };
+            threads.current_thread = Some(next_pid);
 
             let next = &threads.threads[usize::from(next_pid)];
-            let next_sp = next.sp as usize;
-            let next_high_regs = next.data.as_ptr() as usize;
+            let next_sp = next.sp;
+            let next_high_regs = next.data.as_ptr();
 
             // PendSV expects these three pointers in r0, r1 and r2:
             // r0 = &next.sp