use crate::config::{ConfigError, PlatformLimits, SchedulerConfig, Slice};
use crate::tcb::{TaskControlBlock, KERNEL, TCB_FLAG_USE_MSP, TCB_SP_OFFSET};
use core::arch::asm;
use core::ptr::{read_volatile, write_volatile};
const SYST_CSR: u32 = 0xE000_E010; const SYST_RVR: u32 = 0xE000_E014; const SYST_CVR: u32 = 0xE000_E018; const SHPR3: u32 = 0xE000_ED20; const ICSR: u32 = 0xE000_ED04; const FPCCR: u32 = 0xE000_EF34; const DEMCR: u32 = 0xE000_EDFC; const DWT_CTRL: u32 = 0xE000_1000;
const DWT_CYCCNT: u32 = 0xE000_1004;
const ICSR_PENDSVSET: u32 = 1 << 28;
const SYST_CSR_ENABLE: u32 = 1 << 0;
const SYST_CSR_TICKINT: u32 = 1 << 1;
const SYST_CSR_CLKSOURCE: u32 = 1 << 2;
const MAX_SLICE_CYCLES: u64 = 0x00FF_FFFF;
const MIN_SLICE_CYCLES: u64 = 100;
const NOMINAL_HZ: u32 = 16_000_000;
#[inline]
unsafe fn rd(addr: u32) -> u32 {
read_volatile(addr as *const u32)
}
#[inline]
unsafe fn wr(addr: u32, value: u32) {
write_volatile(addr as *mut u32, value);
}
fn configured_hz() -> u32 {
unsafe { *KERNEL.config.get() }.timer_hz
}
static DWT_OK: core::sync::atomic::AtomicU32 = core::sync::atomic::AtomicU32::new(0);
static LAST_TICK_CYCLES: core::sync::atomic::AtomicU32 = core::sync::atomic::AtomicU32::new(0);
fn cycles_to_ns_with(hz: u32, cycles: u32) -> u64 {
if hz == 0 {
return 0;
}
((cycles as u128) * 1_000_000_000u128 / hz as u128) as u64
}
pub fn cycles_to_ns(cycles: u32) -> u64 {
let hz = configured_hz();
cycles_to_ns_with(if hz == 0 { NOMINAL_HZ } else { hz }, cycles)
}
pub type CriticalToken = u32;
pub unsafe fn critical_enter() -> CriticalToken {
let primask: u32;
asm!("mrs {}, PRIMASK", out(reg) primask, options(nomem, nostack, preserves_flags));
asm!("cpsid i", options(nomem, nostack, preserves_flags));
primask
}
pub unsafe fn critical_exit(token: CriticalToken) {
if token & 1 == 0 {
asm!("cpsie i", options(nomem, nostack, preserves_flags));
}
}
#[no_mangle]
pub unsafe extern "C" fn SysTick_Handler() {
if DWT_OK.load(core::sync::atomic::Ordering::Relaxed) == 1
&& (*KERNEL.config.get()).measure == crate::tcb::Measure::Cycles
{
let now = rd(DWT_CYCCNT);
let last = LAST_TICK_CYCLES.load(core::sync::atomic::Ordering::Relaxed);
if last != 0 {
let period = now.wrapping_sub(last);
let want = (*KERNEL.config.get()).slice_cycles;
let err = period.abs_diff(want);
crate::scheduler::record_period_error(err);
}
LAST_TICK_CYCLES.store(now, core::sync::atomic::Ordering::Relaxed);
}
unsafe { crate::scheduler::on_tick() };
wr(ICSR, ICSR_PENDSVSET);
}
#[no_mangle]
unsafe extern "C" fn rrkernel_schedule_next() -> *mut TaskControlBlock {
let guard = crate::critical::enter();
crate::scheduler::reclaim_finished_tasks();
let t0 = if DWT_OK.load(core::sync::atomic::Ordering::Relaxed) == 1 {
rd(DWT_CYCCNT)
} else {
0
};
let next = crate::scheduler::schedule_next();
if t0 != 0 && (*KERNEL.config.get()).measure == crate::tcb::Measure::Cycles {
let dt = rd(DWT_CYCCNT).wrapping_sub(t0);
crate::scheduler::record_latency(dt);
}
drop(guard);
next
}
#[no_mangle]
pub unsafe extern "C" fn rrkernel_idle_wait() {
*KERNEL.idle_waits.get() += 1;
core::ptr::write_volatile(0xE000_ED04 as *mut u32, 0x0800_0000);
asm!("wfi", options(nomem, nostack, preserves_flags));
}
pub fn idle_waits() -> u64 {
let g = crate::critical::enter();
let n = unsafe { *KERNEL.idle_waits.get() };
drop(g);
n
}
#[no_mangle]
#[unsafe(naked)]
pub unsafe extern "C" fn PendSV_Handler() {
core::arch::naked_asm!(
"isb",
"ldr r3, ={kernel}",
"ldr r1, [r3]",
"cmp r1, #0",
"beq 2f",
"mrs r0, psp",
"ldrb r2, [r1, #{flags_off}]",
"lsls r2, r2, #31",
"beq 1f",
"mrs r0, msp",
"1:",
"subs r0, r0, #32",
"stmia r0!, {{r4-r7}}",
"mov r4, r8",
"mov r5, r9",
"mov r6, r10",
"mov r7, r11",
"stmia r0!, {{r4-r7}}",
"subs r0, r0, #32",
"str r0, [r1, #{sp_off}]",
"ldrb r2, [r1, #{flags_off}]",
"lsls r2, r2, #31",
"beq 2f",
"msr msp, r0",
"2:",
"bl {sched}",
"cmp r0, #0",
"beq 4f",
"ldr r1, [r0, #{sp_off}]",
"ldmia r1!, {{r4-r7}}",
"ldr r2, [r1, #0]",
"mov r8, r2",
"ldr r3, [r1, #4]",
"mov r9, r3",
"ldr r2, [r1, #8]",
"mov r10, r2",
"ldr r3, [r1, #12]",
"mov r11, r3",
"adds r1, r1, #16",
"ldrb r2, [r0, #{flags_off}]",
"lsls r2, r2, #31",
"beq 3f",
"msr msp, r1",
"movs r2, #0",
"ldr r3, ={syst_cvr}",
"str r2, [r3]",
"ldr r0, =0xFFFFFFF9",
"bx r0",
"3:",
"msr psp, r1",
"movs r2, #0",
"ldr r3, ={syst_cvr}",
"str r2, [r3]",
"ldr r0, =0xFFFFFFFD",
"bx r0",
"4:",
"bl {idle}",
"b 2b",
kernel = sym KERNEL,
sched = sym rrkernel_schedule_next,
idle = sym rrkernel_idle_wait,
sp_off = const TCB_SP_OFFSET,
flags_off = const crate::tcb::TCB_FLAGS_OFFSET,
syst_cvr = const SYST_CVR,
);
}
core::arch::global_asm!(
r#"
.syntax unified
.thumb
.section .text.SysTick_linkname, "ax", %progbits
.thumb_func
.global SysTick
.type SysTick, %function
SysTick:
b SysTick_Handler
.section .text.PendSV_linkname, "ax", %progbits
.thumb_func
.global PendSV
.type PendSV, %function
PendSV:
b PendSV_Handler
"#
);
pub fn request_switch() {
unsafe {
wr(ICSR, ICSR_PENDSVSET);
asm!("dsb", options(nomem, nostack, preserves_flags));
}
}
pub fn platform_limits() -> PlatformLimits {
let hz = configured_hz();
let hz = if hz == 0 { NOMINAL_HZ } else { hz };
PlatformLimits {
min_slice_ns: cycles_to_ns_with(hz, MIN_SLICE_CYCLES as u32),
max_slice_ns: Some(cycles_to_ns_with(hz, MAX_SLICE_CYCLES as u32)),
timer_hz: hz,
timer_note: "SysTick, processor clock, 24-bit reload (cycle-exact slice)",
}
}
pub fn plan_timer(cfg: &SchedulerConfig) -> Result<crate::arch::TimerPlan, ConfigError> {
let hz = cfg.timer_hz;
if hz == 0 {
return Err(ConfigError::TimerClockRequired);
}
let cycles = match cfg.slice {
Slice::Cycles(c) => c as u64,
other => other.to_cycles(hz),
};
if cycles == 0 {
return Err(ConfigError::ZeroSlice);
}
if cycles < MIN_SLICE_CYCLES {
return Err(ConfigError::SliceBelowPlatformMinimum {
requested_ns: cycles_to_ns_with(hz, cycles as u32),
minimum_ns: cycles_to_ns_with(hz, MIN_SLICE_CYCLES as u32),
});
}
if cycles > MAX_SLICE_CYCLES {
return Err(ConfigError::SliceAboveTimerRange {
requested_ns: cycles_to_ns_with(hz, cycles.min(u32::MAX as u64) as u32),
maximum_ns: cycles_to_ns_with(hz, MAX_SLICE_CYCLES as u32),
});
}
Ok(crate::arch::TimerPlan {
slice_ns: cycles_to_ns_with(hz, cycles as u32),
slice_cycles: cycles as u32,
timer_hz: hz,
})
}
pub fn start_timer(plan: crate::arch::TimerPlan) -> Result<(), ConfigError> {
unsafe {
wr(FPCCR, rd(FPCCR) & !((1 << 31) | (1 << 30)));
wr(SYST_CSR, 0); wr(SYST_RVR, plan.slice_cycles - 1); wr(SYST_CVR, 0);
let mut shpr3 = rd(SHPR3);
shpr3 |= 0x00FF_0000; shpr3 &= !0x00C0_0000; shpr3 |= 0x0040_0000;
wr(SHPR3, shpr3);
wr(
SYST_CSR,
SYST_CSR_CLKSOURCE | SYST_CSR_TICKINT | SYST_CSR_ENABLE,
);
let ok = dwt_enable();
DWT_OK.store(
if ok { 1 } else { 2 },
core::sync::atomic::Ordering::Relaxed,
);
asm!("dsb", options(nomem, nostack, preserves_flags));
}
Ok(())
}
pub fn retune_timer(plan: crate::arch::TimerPlan) -> Result<(), ConfigError> {
unsafe {
let was_enabled = rd(SYST_CSR) & SYST_CSR_ENABLE != 0;
wr(SYST_CSR, 0);
wr(SYST_RVR, plan.slice_cycles - 1);
wr(SYST_CVR, 0);
if was_enabled {
wr(
SYST_CSR,
SYST_CSR_CLKSOURCE | SYST_CSR_TICKINT | SYST_CSR_ENABLE,
);
}
}
Ok(())
}
pub fn dwt_enable() -> bool {
unsafe {
wr(DEMCR, rd(DEMCR) | (1 << 24)); wr(DWT_CTRL, rd(DWT_CTRL) | 1); rd(DWT_CTRL) & 1 != 0
}
}
pub fn cycles_now() -> u32 {
unsafe { rd(DWT_CYCCNT) }
}
#[inline]
unsafe fn push(sp: &mut *mut u32, value: u32) {
*sp = (*sp).sub(1);
write_volatile(*sp, value);
}
pub unsafe fn adopt_current_task(tcb: *mut TaskControlBlock) -> Result<(), ConfigError> {
unsafe {
(*tcb).flags = TCB_FLAG_USE_MSP;
(*tcb).stack_base = core::ptr::null_mut();
(*tcb).stack_size = 0;
(*tcb).sp = core::ptr::null_mut();
}
Ok(())
}
pub unsafe fn create_task(
tcb: *mut TaskControlBlock,
stack_size: usize,
) -> Result<(), crate::SpawnError> {
let size = stack_size.max(256) + 8;
let block = {
let guard = crate::critical::enter();
let p = unsafe { (*crate::scheduler::arena()).alloc(size, 8) };
drop(guard);
p
};
let block = match block {
Some(p) => p,
None => return Err(crate::SpawnError::ArenaExhausted),
};
unsafe {
(*tcb).stack_base = block;
(*tcb).stack_size = size - 8;
(*tcb).flags = 0;
let top = ((block as usize + size) & !7) as *mut u32;
let mut sp = top;
push(&mut sp, 0x0100_0000); let entry = crate::trampoline::task_trampoline as *const () as usize as u32;
push(&mut sp, entry | 1); push(&mut sp, 0xFFFF_FFF9); push(&mut sp, 0); push(&mut sp, 0); push(&mut sp, 0); push(&mut sp, 0); push(&mut sp, tcb as u32);
for _ in 0..8 {
push(&mut sp, 0);
}
(*tcb).sp = sp as *mut u8;
}
Ok(())
}
pub fn mark_running() {}
pub fn exit_current_task_forever() -> ! {
idle_forever()
}
pub fn idle_forever() -> ! {
loop {
unsafe {
asm!("wfi", options(nomem, nostack, preserves_flags));
}
}
}
pub fn shutdown(_code: i32) -> ! {
unsafe {
let _ = critical_enter(); }
loop {
core::hint::spin_loop();
}
}
pub unsafe fn on_task_reclaimed(_tcb: *mut TaskControlBlock) {}
pub unsafe fn is_pinned(_tcb: *mut TaskControlBlock) -> bool {
false
}
pub fn parks_synchronously() -> bool {
true
}
pub fn cycle_counter() -> Option<u32> {
if DWT_OK.load(core::sync::atomic::Ordering::Relaxed) == 1 {
Some(unsafe { rd(DWT_CYCCNT) })
} else {
None
}
}
pub fn cycle_counter_hz() -> u32 {
let hz = configured_hz();
if hz == 0 {
NOMINAL_HZ
} else {
hz
}
}