bern_arch/cortex_m/
scheduler.rs

1//! ARM Cortex-M implementation of [`IScheduler`] and context switch.
2
3use core::arch::{asm, naked_asm};
4use core::mem;
5use cortex_m::peripheral::SCB;
6
7use crate::arch::register::{StackFrame, StackFrameExtension, StackSettings};
8use crate::arch::Arch;
9use crate::scheduler::IScheduler;
10
11/// Pendable service call.
12///
13/// Storing and loading registers in context switch.
14///
15/// Exception is triggered by `cortex_m::peripheral::SCB::PendSV()`.
16#[no_mangle]
17#[unsafe(naked)]
18pub unsafe extern "C" fn PendSV() {
19    // Based on "Definitive Guide to Cortex-M3/4", p. 349
20    #[cfg(has_fpu)]
21    unsafe {
22        naked_asm!(
23            "mrs      r1, psp",
24            "mov      r2, lr",
25            "tst      r2, #0x10", // was FPU used?
26            "ite      eq",
27            "subeq    r0, r1, #104", // psp with FPU registers after register push
28            "subne    r0, r1, #40",  // psp without FPU registers after register push
29            "push     {{r1,r2}}",
30            "bl       check_stack", // in: psp (r0), out: store context (1), stack would overflow (0)
31            "pop      {{r1,r2}}",
32            "cmp      r0, #1", // stack invalid?
33            "itt      ne",     // if stack invalid
34            "movne    r0, 0",  // set psp to 0, signal `switch_context` an error
35            "bne      0",
36            // else store
37            "tst      r2, #0x10", // was FPU used?
38            "it       eq",
39            "vstmdbeq r1!, {{s16-s31}}", // push FPU registers
40            "mrs      r3, control",
41            "stmdb    r1!, {{r2-r11}}", // push LR, control and remaining registers
42            "mov      r0, r1",
43            "0:       bl switch_context",
44            "ldmia    r0!, {{r2-r11}}",
45            "msr      control, r3",
46            "isb",
47            "mov      lr, r2",
48            "tst      lr, #0x10", // was FPU used?
49            "it       eq",
50            "vldmiaeq r0!, {{s16-s31}}", // pop FPU registers
51            "msr      psp, r0",
52            "bx       lr",
53        )
54    }
55
56    #[cfg(not(has_fpu))]
57    unsafe {
58        asm!(
59            "mrs      r1, psp",
60            "mov      r2, lr",
61            "sub      r0, r1, #40", // psp without FPU registers after register push
62            "push     {{r1,r2}}",
63            "bl       check_stack", // in: psp (r0), out: store context (1), stack would overflow (0)
64            "pop      {{r1,r2}}",
65            "cmp      r0, #1", // stack invalid?
66            "itt      ne",     // if stack invalid
67            "movne    r0, 0",  // set psp to 0, signal `switch_context` an error
68            "bne      0",
69            // else store
70            "mrs      r3, control",
71            "stmdb    r1!, {{r2-r11}}", // push LR, control and remaining registers
72            "mov      r0, r1",
73            "0:       bl switch_context",
74            "ldmia    r0!, {{r2-r11}}",
75            "msr      control, r3",
76            "isb",
77            "mov      lr, r2",
78            "msr      psp, r0",
79            "bx       lr",
80            options(noreturn),
81        )
82    }
83}
84
85impl IScheduler for Arch {
86    unsafe fn init_task_stack(
87        stack_ptr: *mut usize,
88        entry: *const usize,
89        arg: *const usize,
90        exit: *const usize,
91    ) -> *mut usize {
92        let mut stack_offset = mem::size_of::<StackFrame>() / mem::size_of::<usize>();
93        let mut stack_frame: &mut StackFrame =
94            mem::transmute(&mut *stack_ptr.offset(-(stack_offset as isize)));
95        stack_frame.r0 = arg as u32;
96        stack_frame.lr = exit as u32;
97        stack_frame.pc = entry as u32;
98        stack_frame.xpsr = 0x01000000; // Thumb mode
99
100        // we don't have to initialize r4-r11
101        stack_offset += mem::size_of::<StackFrameExtension>() / mem::size_of::<usize>();
102
103        stack_offset += mem::size_of::<StackSettings>() / mem::size_of::<usize>();
104        let mut stack_settings: &mut StackSettings =
105            mem::transmute(&mut *stack_ptr.offset(-(stack_offset as isize)));
106        stack_settings.exception_lr = 0xFFFFFFFD; // thread mode using psp
107        stack_settings.control = 0x3; // unprivileged thread mode
108
109        stack_ptr.offset(-(stack_offset as isize))
110    }
111
112    fn start_first_task(stack_ptr: *const usize) -> () {
113        unsafe {
114            asm!(
115            "ldmia r0!, {{r2,r3}}",
116            "msr   psp, r0",            // set process stack pointer -> task stack
117            "msr   control, r3",
118            "isb",
119            "pop   {{r4-r11}}",
120            "pop   {{r0-r3,r12,lr}}",   // force function entry
121            "pop   {{pc}}",             // 'jump' to the task entry function we put on the stack
122            in("r0") stack_ptr as u32,
123            options(noreturn),
124            )
125        }
126    }
127
128    #[inline]
129    fn trigger_context_switch() {
130        SCB::set_pendsv();
131    }
132}
bern_arch/cortex_m/scheduler.rs

bern_arch/cortex_m/
scheduler.rs
