use crate::config::{ConfigError, PlatformLimits, SchedulerConfig, Slice};
use crate::tcb::{TaskControlBlock, KERNEL};
use core::arch::asm;
use core::ptr::{read_volatile, write_volatile};
pub const FRAME_SIZE: usize = 72;
pub const OFF_PAD: usize = 0;
pub const OFF_R0: usize = 8;
pub const OFF_LR: usize = 60;
pub const OFF_PC: usize = 64;
pub const OFF_CPSR: usize = 68;
const CPSR_TASK_ENTRY: usize = 0x13;
const CPSR_I: u32 = 1 << 7;
const MODE_SVC: usize = 0x13;
pub const IRQ_VECTOR_OFFSET: usize = 0x18;
#[derive(Clone, Copy)]
pub struct ArmConfig {
pub gic_dist: usize,
pub gic_cpu: usize,
pub timer_irq: u32,
pub physical_timer: bool,
pub secure_fiq: bool,
}
impl ArmConfig {
pub const fn qemu_virt() -> Self {
ArmConfig {
gic_dist: 0x0800_0000,
gic_cpu: 0x0801_0000,
timer_irq: 30,
physical_timer: true,
secure_fiq: false,
}
}
pub const fn secure() -> Self {
ArmConfig {
secure_fiq: true,
..ArmConfig::qemu_virt()
}
}
}
static mut ARM_CONFIG: ArmConfig = ArmConfig::qemu_virt();
pub fn configure(cfg: ArmConfig) {
unsafe {
write_volatile(core::ptr::addr_of_mut!(ARM_CONFIG), cfg);
}
}
#[inline]
fn config() -> ArmConfig {
unsafe { read_volatile(core::ptr::addr_of!(ARM_CONFIG)) }
}
#[inline]
fn gicd(offset: usize) -> *mut u32 {
(config().gic_dist + offset) as *mut u32
}
#[inline]
fn gicc(offset: usize) -> *mut u32 {
let bank = if config().secure_fiq { 0x1000 } else { 0x0000 };
(config().gic_cpu + bank + offset) as *mut u32
}
const GICD_CTLR: usize = 0x000;
const GICD_IGROUPR0: usize = 0x080;
const GICD_ISENABLER0: usize = 0x100;
const GICD_IPRIORITYR: usize = 0x400;
const GICC_CTLR: usize = 0x000;
const GICC_PMR: usize = 0x004;
const GICC_IAR: usize = 0x00C;
const GICC_EOIR: usize = 0x010;
unsafe fn gic_enable_timer_irq(irq: u32) {
let word = (irq / 32) as usize;
let bit = 1u32 << (irq % 32);
let secure = config().secure_fiq;
let group_bits: u32 = if secure { 0 } else { bit | 1 };
let g = read_volatile(gicd(GICD_IGROUPR0 + word * 4));
write_volatile(
gicd(GICD_IGROUPR0 + word * 4),
(g & !(bit | 1)) | group_bits,
);
write_volatile(gicd(GICD_IPRIORITYR + irq as usize) as *mut u8, 0xA0);
write_volatile(gicd(GICD_ISENABLER0 + word * 4), bit | 1);
write_volatile(gicc(GICC_PMR), 0xFF);
write_volatile(gicc(GICC_CTLR), 0b11);
write_volatile(gicd(GICD_CTLR), 0b11);
}
#[inline]
pub unsafe fn gic_acknowledge() -> u32 {
read_volatile(gicc(GICC_IAR))
}
#[inline]
pub unsafe fn gic_eoi(irq: u32) {
write_volatile(gicc(GICC_EOIR), irq);
}
macro_rules! mrc {
($name:ident, $($op:expr),+) => {
#[inline]
unsafe fn $name() -> u32 {
let v: u32;
asm!(concat!("mrc p15, 0, {}, ", $($op),+), out(reg) v, options(nomem, nostack));
v
}
};
}
macro_rules! mcr {
($name:ident, $($op:expr),+) => {
#[inline]
unsafe fn $name(value: u32) {
asm!(concat!("mcr p15, 0, {}, ", $($op),+), in(reg) value, options(nomem, nostack));
}
};
}
mrc!(read_cntfrq, "c14, c0, 0");
mrc!(read_cntp_tval, "c14, c2, 0");
mcr!(write_cntp_tval, "c14, c2, 0");
mcr!(write_cntp_ctl, "c14, c2, 1");
mcr!(write_cntv_tval, "c14, c3, 0");
mcr!(write_cntv_ctl, "c14, c3, 1");
#[inline]
pub fn timer_hz() -> u32 {
unsafe { read_cntfrq() }
}
#[inline]
unsafe fn ticks_since_expiry() -> u32 {
0u32.wrapping_sub(read_cntp_tval())
}
#[inline]
unsafe fn rearm_timer() {
let slice = (*KERNEL.config.get()).slice_cycles;
if config().physical_timer {
write_cntp_tval(slice);
write_cntp_ctl(1); } else {
write_cntv_tval(slice);
write_cntv_ctl(1);
}
}
pub type CriticalToken = u32;
const CPSR_F: u32 = 1 << 6;
static TRACE_HOOK: core::sync::atomic::AtomicUsize = core::sync::atomic::AtomicUsize::new(0);
pub fn set_trace_hook(hook: fn(usize, usize)) {
TRACE_HOOK.store(
hook as *const () as usize,
core::sync::atomic::Ordering::Relaxed,
);
}
#[inline]
fn trace(tag: usize, value: usize) {
let raw = TRACE_HOOK.load(core::sync::atomic::Ordering::Relaxed);
if raw != 0 {
let hook: fn(usize, usize) =
unsafe { core::mem::transmute::<usize, fn(usize, usize)>(raw) };
hook(tag, value);
}
}
#[inline]
pub fn current_cpsr() -> u32 {
let v: u32;
unsafe {
asm!("mrs {}, cpsr", out(reg) v, options(nomem, nostack, preserves_flags));
}
v
}
pub unsafe fn critical_enter() -> CriticalToken {
let cpsr: u32;
asm!("mrs {}, cpsr", out(reg) cpsr, options(nomem, nostack, preserves_flags));
trace(1, cpsr as usize);
asm!("cpsid if", options(nomem, nostack));
let token = !cpsr & (CPSR_I | CPSR_F);
trace(2, token as usize);
token
}
pub unsafe fn critical_exit(token: CriticalToken) {
trace(3, token as usize);
if token & CPSR_I != 0 {
asm!("cpsie i", options(nomem, nostack));
}
if token & CPSR_F != 0 {
asm!("cpsie f", options(nomem, nostack));
}
trace(4, current_cpsr() as usize);
}
const GIC_NO_ELIGIBLE: u32 = 0x3FE;
#[no_mangle]
unsafe extern "C" fn rrkernel_irq_switch() -> *mut TaskControlBlock {
let irq = gic_acknowledge();
trace(6, irq as usize);
crate::scheduler::on_tick();
if (*KERNEL.config.get()).measure != crate::tcb::Measure::Off {
crate::scheduler::record_period_error(ticks_since_expiry());
}
let next = crate::scheduler::schedule_next();
if irq < GIC_NO_ELIGIBLE {
gic_eoi(irq);
}
next
}
#[no_mangle]
unsafe extern "C" fn rrkernel_after_switch() {
let measuring = (*KERNEL.config.get()).measure != crate::tcb::Measure::Off;
let overshoot = if measuring { ticks_since_expiry() } else { 0 };
crate::scheduler::reclaim_finished_tasks();
rearm_timer();
if measuring {
crate::scheduler::record_latency(overshoot);
}
}
#[no_mangle]
#[unsafe(naked)]
pub unsafe extern "C" fn rrkernel_irq_entry() {
core::arch::naked_asm!(
"sub lr, lr, #4",
"srsdb sp!, #{svc_mode}",
"cps #{svc_mode}",
"push {{r0-r12, lr}}",
"sub sp, sp, #8",
"ldr r0, ={kernel}",
"ldr r1, [r0]",
"cmp r1, #0",
"beq 1f",
"str sp, [r1]",
"1:",
"bl {trap_switch}",
"cmp r0, #0",
"beq 4f",
"ldr sp, [r0]",
"bl {after_switch}",
"add sp, sp, #8",
"pop {{r0-r12, lr}}",
"rfeia sp!",
"4:",
"wfi",
"b 1b",
kernel = sym KERNEL,
trap_switch = sym rrkernel_irq_switch,
after_switch = sym rrkernel_after_switch,
svc_mode = const MODE_SVC,
);
}
core::arch::global_asm!(
r#"
.section .text, "ax"
/* VBAR must be 32-byte aligned; the table is 8 words: reset, undef, SVC,
* prefetch abort, data abort, reserved, IRQ, FIQ. */
.align 5
.globl rrkernel_vectors
rrkernel_vectors:
b rrkernel_unexpected_trap
b rrkernel_unexpected_trap
b rrkernel_unexpected_trap
b rrkernel_unexpected_trap
b rrkernel_unexpected_trap
b rrkernel_unexpected_trap
b rrkernel_irq_entry
/* FIQ shares the handler: a secure CPU receives the tick here (group 0) and a
* non-secure one on IRQ (group 1). The handler acknowledges through the GIC,
* so it does not care which line it arrived on. */
b rrkernel_irq_entry
.globl rrkernel_unexpected_trap
rrkernel_unexpected_trap:
b rrkernel_unexpected_trap
"#
);
const MIN_SLICE_CYCLES: u64 = 100;
const NOMINAL_HZ: u32 = 62_500_000;
fn configured_hz() -> u32 {
unsafe { *KERNEL.config.get() }.timer_hz
}
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 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, u32::MAX)),
timer_hz: hz,
timer_note: "ARM generic timer (CNTP/CNTV) + GICv2, SRSDB/RFEIA switch",
}
}
pub fn plan_timer(cfg: &SchedulerConfig) -> Result<crate::arch::TimerPlan, ConfigError> {
let hz = if cfg.timer_hz != 0 {
cfg.timer_hz
} else {
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 > u32::MAX as u64 {
return Err(ConfigError::SliceAboveTimerRange {
requested_ns: cycles_to_ns_with(hz, u32::MAX),
maximum_ns: cycles_to_ns_with(hz, u32::MAX),
});
}
Ok(crate::arch::TimerPlan {
slice_ns: cycles_to_ns_with(hz, cycles as u32),
slice_cycles: cycles as u32,
timer_hz: hz,
})
}
unsafe extern "C" {
static rrkernel_vectors: u8;
}
pub fn start_timer(_plan: crate::arch::TimerPlan) -> Result<(), ConfigError> {
unsafe {
let vbar = core::ptr::addr_of!(rrkernel_vectors) as u32;
asm!("mcr p15, 0, {}, c12, c0, 0", in(reg) vbar, options(nomem, nostack));
let cfg = config();
gic_enable_timer_irq(cfg.timer_irq);
rearm_timer();
asm!("cpsie i", options(nomem, nostack));
trace(5, current_cpsr() as usize);
}
Ok(())
}
pub fn retune_timer(_plan: crate::arch::TimerPlan) -> Result<(), ConfigError> {
unsafe {
rearm_timer();
}
Ok(())
}
pub unsafe fn adopt_current_task(tcb: *mut TaskControlBlock) -> Result<(), ConfigError> {
unsafe {
(*tcb).sp = core::ptr::null_mut();
(*tcb).stack_base = core::ptr::null_mut();
(*tcb).stack_size = 0;
(*tcb).flags = 0;
}
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;
let top = (block as usize + size) & !7;
let frame = (top - FRAME_SIZE) as *mut u8;
core::ptr::write_bytes(frame, 0, FRAME_SIZE);
let w = |off: usize, v: usize| {
core::ptr::write_volatile(frame.add(off) as *mut usize, v);
};
w(OFF_R0, tcb as usize);
let entry = crate::trampoline::task_trampoline as *const () as usize;
w(OFF_PC, entry);
w(OFF_CPSR, CPSR_TASK_ENTRY | ((entry & 1) << 5));
(*tcb).sp = frame;
(*tcb).flags = 0;
}
Ok(())
}
pub fn mark_running() {}
pub fn exit_current_task_forever() -> ! {
idle_forever()
}
pub fn idle_forever() -> ! {
trace(7, current_cpsr() as usize);
loop {
unsafe {
asm!("wfi", options(nomem, nostack, preserves_flags));
}
}
}
pub fn shutdown(_code: i32) -> ! {
unsafe {
let _ = critical_enter();
}
loop {
core::hint::spin_loop();
}
}
pub fn request_switch() {
unsafe {
let sgir = 1u32 << 24;
write_volatile(gicd(0xF00), sgir);
}
}
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> {
None
}
pub fn cycle_counter_hz() -> u32 {
0
}