fix(threads/cortex_m): don't save high regs on first schedule

src/riot-rs-threads/src/arch/cortex_m.rs

@@ -84,9 +84,18 @@ unsafe extern "C" fn PendSV() {
              * so let's use '99' forward ('99f')
              */
             beq 99f
+
+            msr.n psp, r0
+
+            // r1 == 0 means that there was no previous thread.
+            // This is only the case if the scheduler was triggered for the first time,
+            // which also means that next thread has no stored context yet.
+            // Storing and loading of r4-r11 therefore can be skipped.
+            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 +119,15 @@ unsafe extern "C" fn PendSV() {
             cmp r0, #0
             beq 99f
 
+            msr.n psp, r0
+
+            // r1 == 0 means that there was no previous thread.
+            // This is only the case if the scheduler was triggered for the first time,
+            // which also means that next thread has no stored context yet.
+            // Storing and loading of r4-r11 therefore can be skipped.
+            cmp r1, #0
+            beq 99f
+
             //stmia r1!, {{r4-r7}}
             str r4, [r1, #16]
             str r5, [r1, #20]
@@ -134,7 +152,6 @@ unsafe extern "C" fn PendSV() {
             mov r8,  r4
             ldmia r2!, {{r4-r7}}
 
-            msr.n psp, r0
             99:
             ldr r0, 999f
             mov LR, r0
@@ -158,6 +175,7 @@ unsafe extern "C" fn PendSV() {
 /// - `0` in `r0` if the next thread in the runqueue is the currently running thread
 /// - Else it writes into the following registers:
 ///   - `r1`: pointer to [`Thread::high_regs`] from old thread (to store old register state)
+///           or null pointer if there was no previously running thread.
 ///   - `r2`: pointer to [`Thread::high_regs`] from new thread (to load new register state)
 ///   - `r0`: stack-pointer for new thread
 ///
@@ -183,27 +201,28 @@ unsafe fn sched() -> u128 {
                 }
             };
 
-            let current_high_regs;
+            // `current_high_regs` will be null if there is no current thread.
+            // This is only the case once, when the very first thread starts running.
+            // The returned `r1` therefore will be null, and saving/ restoring
+            // the context is skipped.
+            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:
+            // The caller (`PendSV`) expects these three pointers in r0, r1 and r2:
             // r0 = &next.sp
             // r1 = &current.high_regs
             // r2 = &next.high_regs