fixup! fixup! fixup! riot-rs-threads: cortex_m: make sched() less h…

…acky

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

@@ -2,7 +2,6 @@ use super::Arch;
 use core::arch::asm;
 use core::ptr::write_volatile;
 use cortex_m::peripheral::SCB;
-use critical_section::CriticalSection;
 
 use crate::{cleanup, THREADS};
 
@@ -75,7 +74,6 @@ unsafe extern "C" fn PendSV() {
     unsafe {
         asm!(
             "
-            mrs r0, psp
             bl {sched}
             cmp r0, #0
             /* label rules:
@@ -107,7 +105,6 @@ unsafe extern "C" fn PendSV() {
     unsafe {
         asm!(
             "
-            mrs r0, psp
             bl sched
             cmp r0, #0
             beq 99f
@@ -166,10 +163,10 @@ unsafe extern "C" fn PendSV() {
 /// This function is called in PendSV.
 // TODO: make arch independent, or move to arch
 #[no_mangle]
-unsafe fn sched(old_sp: usize) -> u128 {
+unsafe fn sched() -> u128 {
     loop {
         if let Some(res) = critical_section::with(|cs| {
-            let threads = &mut *THREADS.as_ptr(cs);
+            let threads = unsafe { &mut *THREADS.as_ptr(cs) };
             let next_pid = match threads.runqueue.get_next() {
                 Some(pid) => pid,
                 None => {
@@ -178,18 +175,23 @@ unsafe fn sched(old_sp: usize) -> u128 {
                 }
             };
 
-            let current_pid = threads.current_pid().unwrap();
-            if next_pid == current_pid {
-                return Some(0);
-            }
+            let current_high_regs;
+            if let Some(current_pid) = threads.current_pid() {
+                if next_pid == current_pid {
+                    return Some(0);
+                }
 
-            threads.threads[current_pid as usize].sp = old_sp;
-            threads.current_thread = Some(next_pid);
+                threads.threads[current_pid as usize].sp = cortex_m::register::psp::read() as usize;
+                threads.current_thread = Some(next_pid);
 
-            let next = &threads.threads[next_pid as usize];
+                current_high_regs = threads.threads[current_pid as usize].data.as_ptr();
+            } else {
+                threads.current_thread = Some(next_pid);
+                current_high_regs = core::ptr::null();
+            };
 
+            let next = &threads.threads[next_pid as usize];
             let next_sp = next.sp as usize;
-            let current_high_regs = threads.threads[current_pid as usize].data.as_ptr() as usize;
             let next_high_regs = next.data.as_ptr() as usize;
 
             // PendSV expects these three pointers in r0, r1 and r2:
@@ -200,7 +202,7 @@ unsafe fn sched(old_sp: usize) -> u128 {
             // So let's use that.
             let res: u128 =
                 //  (r0)                     (r1)                        (r2)
-                    (next_sp as u128) |  ((current_high_regs as u128) << 32) | ((next_high_regs as u128) << 64);
+                (next_sp as u128) |  ((current_high_regs as u128) << 32) | ((next_high_regs as u128) << 64);
             Some(res)
         }) {
             break res;