WIP: Kernel accounting of task CPU usage

app/gimlet/src/main.rs

@@ -230,4 +230,10 @@ fn system_init() {
             .pc3so()
             .set_bit()
     });
+
+    // Enable timing
+    let ptime = RollingTimer::new_tim5(&p, 200);
+    // SAFETY: we promise to never use TIM5 again.
+    let vtable = unsafe { ptime.into_ptimer() };
+    kern::ptime::set_ptime_vtable(vtable);
 }

app/grapefruit/src/main.rs

@@ -13,7 +13,7 @@ extern crate stm32h7;
 
 use stm32h7::stm32h753 as device;
 
-use drv_stm32h7_startup::ClockConfig;
+use drv_stm32h7_startup::{ClockConfig, rolling_timer::RollingTimer};
 
 use cortex_m_rt::entry;
 
@@ -443,4 +443,10 @@ fn system_init() {
 
     // Turn on the controller.
     p.FMC.bcr1.modify(|_, w| w.fmcen().set_bit());
+
+    // Enable timing
+    let ptime = RollingTimer::new_tim5(&p, 200);
+    // SAFETY: we promise to never use TIM5 again.
+    let vtable = unsafe { ptime.into_ptimer() };
+    kern::ptime::set_ptime_vtable(vtable);
 }

drv/stm32h7-startup/Cargo.toml

@@ -8,6 +8,7 @@ cortex-m = { workspace = true }
 cortex-m-rt = { workspace = true }
 stm32h7 = { workspace = true }
 measurement-handoff = { path = "../../lib/measurement-handoff", optional = true }
+kern = { path = "../../sys/kern" }
 
 [features]
 h743 = ["stm32h7/stm32h743"]

drv/stm32h7-startup/src/lib.rs

@@ -459,5 +459,67 @@ pub mod rolling_timer {
                 }
             }
         }
+
+        pub unsafe fn into_ptimer(self) -> &'static kern::ptime::PTimeVTable {
+            // this is all a bit silly
+            use core::sync::atomic::{AtomicU32, Ordering};
+            fn tickrate() -> u32 {
+                1_000_000
+            }
+
+            fn timekeep() {
+                let tim5 = unsafe { &*device::TIM5::ptr() };
+                let counter = tim5.cnt.read().bits();
+                let mut period = PERIOD.load(Ordering::Relaxed);
+
+                // Does the parity match?
+                let parity = (period & 0b1) ^ (counter >> 31);
+                if parity != 0 {
+                    period += 1;
+                    PERIOD.store(period, Ordering::Relaxed);
+                }
+
+                // Bit  63     (one bit) is unused.
+                // Bits 31..63 (32 bits) are filled by PERIOD
+                // Bits 00..31 (31 bits) are filled by counter
+                // let upper = (period as u64) << 31;
+                // let lower = (now_rolling & 0x7FFF_FFFF) as u64;
+                // let time = upper | lower;
+                // kern::ptime::Instant(time)
+            }
+
+            fn now() -> kern::ptime::Instant {
+                let tim5 = unsafe { &*device::TIM5::ptr() };
+                let counter = tim5.cnt.read().bits();
+                let period = PERIOD.load(Ordering::Relaxed);
+
+                // Bit  63     (one bit) is unused.
+                // Bits 31..63 (32 bits) are filled by PERIOD
+                // Bits 00..31 (31 bits) are filled by (counter & 0x7FFF_FFFF)
+                //
+                // IF the lowest bit of `period` DOES NOT match the uppermost
+                // bit of `counter`, then there has been either a half or full
+                // rollover (0x7FFF_FFFF -> 0x8000_0000, or 0xFFFF_FFFF ->
+                // 0x0000_0000) not accounted for in `period`, so we add
+                // 0x8000_0000.
+                //
+                // Adapted from embassy-stm32's time driver technique
+                let upper = (period as u64) << 31;
+                let lower = (counter ^ ((period & 1) << 31)) as u64;
+                let time = upper + lower;
+                kern::ptime::Instant(time)
+            }
+
+            static PERIOD: AtomicU32 = AtomicU32::new(0);
+            static VTABLE: kern::ptime::PTimeVTable =
+                kern::ptime::PTimeVTable {
+                    now,
+                    timekeep,
+                    tickrate,
+                };
+
+            core::mem::forget(self);
+            &VTABLE
+        }
     }
 }

sys/kern/src/arch/arm_m.rs

@@ -1033,8 +1033,9 @@ cfg_if::cfg_if! {
 /// pointer while you have access to `task`, and as long as the `task` being
 /// stored is actually in the task table, you'll be okay.
 pub unsafe fn set_current_task(task: &task::Task) {
-    CURRENT_TASK_PTR.store(task as *const _ as *mut _, Ordering::Relaxed);
-    crate::profiling::event_context_switch(task as *const _ as usize);
+    let task: *const task::Task = task;
+    CURRENT_TASK_PTR.store(task.cast_mut(), Ordering::Relaxed);
+    crate::profiling::event_context_switch(task as usize);
 }
 
 /// Reads the tick counter.
@@ -1067,6 +1068,9 @@ static TICKS: [AtomicU32; 2] = {
 #[unsafe(no_mangle)]
 pub unsafe extern "C" fn SysTick() {
     crate::profiling::event_timer_isr_enter();
+    if let Some(ptimer) = crate::ptime::ptimer() {
+        (ptimer.timekeep)();
+    }
     with_task_table(|tasks| {
         // Load the time before this tick event.
         let t0 = TICKS[0].load(Ordering::Relaxed);
@@ -1206,15 +1210,18 @@ unsafe extern "C" fn pendsv_entry() {
 
     // Safety: we're dereferencing the current task pointer, which we're
     // trusting the rest of this module to maintain correctly.
-    let current = usize::from(unsafe { (*current).descriptor().index });
+    let current = unsafe {
+        let current = &mut *current;
+        current.account_task_active_time();
+        usize::from(current.descriptor().index)
+    };
 
     with_task_table(|tasks| {
         let next = task::select(current, tasks);
-        apply_memory_protection(next);
         // Safety: next comes from the task table and we don't use it again
         // until next kernel entry, so we meet set_current_task's requirements.
         unsafe {
-            set_current_task(next);
+            next.switch_to();
         }
     });
     crate::profiling::event_secondary_syscall_exit();
@@ -1622,6 +1629,10 @@ unsafe extern "C" fn handle_fault(task: *mut task::Task) {
     // ARMv6-M, to reduce complexity, does not distinguish fault causes.
     let fault = FaultInfo::InvalidOperation(0);
 
+    unsafe {
+        (*task).account_task_active_time();
+    }
+
     // We are now going to force a fault on our current task and directly
     // switch to a task to run.
     with_task_table(|tasks| {
@@ -1635,11 +1646,10 @@ unsafe extern "C" fn handle_fault(task: *mut task::Task) {
             panic!("attempt to return to Task #{idx} after fault");
         }
 
-        apply_memory_protection(next);
         // Safety: next comes from the task table and we don't use it again
         // until next kernel entry, so we meet set_current_task's requirements.
         unsafe {
-            set_current_task(next);
+            next.switch_to();
         }
     });
 }
@@ -1831,6 +1841,10 @@ unsafe extern "C" fn handle_fault(
         arch::asm!("vstm {0}, {{s16-s31}}", in(reg) fpsave);
     }
 
+    unsafe {
+        (*task).account_task_active_time();
+    }
+
     // We are now going to force a fault on our current task and directly
     // switch to a task to run.  (It may be tempting to use PendSV here,
     // but that won't work on ARMv8-M in the presence of MPU faults on
@@ -1848,12 +1862,11 @@ unsafe extern "C" fn handle_fault(
             panic!("attempt to return to Task #{idx} after fault");
         }
 
-        apply_memory_protection(next);
         // Safety: this leaks a pointer aliasing next into static scope, but
         // we're not going to read it back until the next kernel entry, so we
         // won't be aliasing/racing.
         unsafe {
-            set_current_task(next);
+            next.switch_to();
         }
     });
 }

sys/kern/src/lib.rs

@@ -42,6 +42,7 @@ pub mod fail;
 pub mod header;
 pub mod kipc;
 pub mod profiling;
+pub mod ptime;
 pub mod startup;
 pub mod syscalls;
 pub mod task;

sys/kern/src/profiling.rs

@@ -82,6 +82,8 @@ pub struct EventsTable {
 
     /// Called whenever the current task changes, with a pointer to the task's
     /// control block.
+    ///
+    /// TODO(AJM): This probably should be `fn(*const/mut Task)`, not `usize`.
     pub context_switch: fn(usize),
 }
 
@@ -90,7 +92,8 @@ pub struct EventsTable {
 /// You can call this more than once if you need to, though that seems odd at
 /// first glance.
 pub fn configure_events_table(table: &'static EventsTable) {
-    EVENTS_TABLE.store(table as *const _ as *mut _, Ordering::Relaxed);
+    let table: *const EventsTable = table;
+    EVENTS_TABLE.store(table.cast_mut(), Ordering::Relaxed);
 }
 
 /// Internal pointer written by `configure_events_table` and read by `table`. If

sys/kern/src/ptime.rs

@@ -0,0 +1,50 @@
+// This Source Code Form is subject to the terms of the Mozilla Public
+// License, v. 2.0. If a copy of the MPL was not distributed with this
+// file, You can obtain one at https://mozilla.org/MPL/2.0/.
+
+//! Precise time
+//!
+//! Currently only used for profiling
+
+use core::sync::atomic::{AtomicPtr, Ordering};
+
+#[derive(Debug, Clone, Copy)]
+pub struct Instant(pub u64);
+#[derive(Debug, Clone, Copy)]
+pub struct Duration(pub u64);
+
+impl Duration {
+    pub const ZERO: Self = Self(0);
+}
+
+pub type NowFunc = fn() -> Instant;
+// TODO: do we want this to return an Instant? It's probably a bit cheaper
+// than calling timekeep() -> now(), but I'm not sure if we *need* it for
+// anything, as systick doesn't do anything with it.
+pub type TimeKeepFunc = fn();
+pub type TickRateFunc = fn() -> u32;
+
+pub struct PTimeVTable {
+    pub now: NowFunc,
+    pub timekeep: TimeKeepFunc,
+    pub tickrate: TickRateFunc,
+}
+
+// hate this
+#[unsafe(no_mangle)]
+pub(crate) static mut PTIME_LAST_SWITCH: Instant = Instant(0);
+
+static PTIME_VTABLE: AtomicPtr<PTimeVTable> =
+    AtomicPtr::new(core::ptr::null_mut());
+
+pub fn set_ptime_vtable(vtable: &'static PTimeVTable) {
+    let vtable: *const PTimeVTable = vtable;
+    let vtable: *mut PTimeVTable = vtable.cast_mut();
+    PTIME_VTABLE.store(vtable, Ordering::Release)
+}
+
+pub fn ptimer() -> Option<&'static PTimeVTable> {
+    let vtable: *mut PTimeVTable = PTIME_VTABLE.load(Ordering::Acquire);
+    let vtable: *const PTimeVTable = vtable.cast_const();
+    unsafe { vtable.as_ref() }
+}

sys/kern/src/startup.rs

@@ -51,8 +51,9 @@ pub unsafe fn start_kernel(tick_divisor: u32) -> ! {
     let task_descs = &HUBRIS_TASK_DESCS;
     // Safety: this reference will remain unique so long as the "only called
     // once per boot" contract on this function is upheld.
-    let task_table =
-        unsafe { &mut *core::ptr::addr_of_mut!(HUBRIS_TASK_TABLE_SPACE) };
+    let task_table = unsafe {
+        core::ptr::addr_of_mut!(HUBRIS_TASK_TABLE_SPACE).as_mut_unchecked()
+    };
 
     // Initialize our RAM data structures.
 
@@ -64,21 +65,22 @@ pub unsafe fn start_kernel(tick_divisor: u32) -> ! {
     // states.
 
     // Now, generate the task table.
-    // Safety: MaybeUninit<[T]> -> [MaybeUninit<T>] is defined as safe.
     let task_table: &mut [MaybeUninit<Task>; HUBRIS_TASK_COUNT] =
-        unsafe { &mut *(task_table as *mut _ as *mut _) };
-    for (i, task) in task_table.iter_mut().enumerate() {
-        task.write(Task::from_descriptor(&task_descs[i]));
-    }
+        task_table.as_mut();
+
+    task_table
+        .iter_mut()
+        .zip(task_descs.iter())
+        .for_each(|(t, d)| {
+            let task = t.write(Task::from_descriptor(d));
+
+            // With init done, set up initial register state etc.
+            crate::arch::reinitialize(task);
+        });
 
     // Safety: we have fully initialized this and can shed the uninit part.
     let task_table: &mut [Task; HUBRIS_TASK_COUNT] =
-        unsafe { &mut *(task_table as *mut _ as *mut _) };
-
-    // With that done, set up initial register state etc.
-    for task in task_table.iter_mut() {
-        crate::arch::reinitialize(task);
-    }
+        unsafe { core::mem::transmute(task_table) };
 
     // Great! Pick our first task. We'll act like we're scheduling after the
     // last task, which will cause a scan from 0 on.
@@ -99,22 +101,14 @@ pub(crate) fn with_task_table<R>(body: impl FnOnce(&mut [Task]) -> R) -> R {
     }
     let task_table: *mut MaybeUninit<[Task; HUBRIS_TASK_COUNT]> =
         core::ptr::addr_of_mut!(HUBRIS_TASK_TABLE_SPACE);
-    // Pointer cast valid as MaybeUninit<[T; N]> and [MaybeUninit<T>; N] have
-    // same in-memory representation. At the time of this writing
-    // MaybeUninit::transpose is not yet stable.
-    let task_table: *mut [MaybeUninit<Task>; HUBRIS_TASK_COUNT] =
-        task_table as _;
-    // This pointer cast is doing the equivalent of
-    // MaybeUninit::array_assume_init, which at the time of this writing is not
-    // stable.
-    let task_table: *mut [Task; HUBRIS_TASK_COUNT] = task_table as _;
+
     // Safety: we have observed `TASK_TABLE_IN_USE` being false, which means the
     // task table is initialized (note that at reset it starts out true) and
     // that we're not already within a call to with_task_table. Thus, we can
     // produce a reference to the task table without aliasing, and we can be
     // confident that the memory it's pointing to is initialized and shed the
     // MaybeUninit.
-    let task_table = unsafe { &mut *task_table };
+    let task_table = unsafe { task_table.as_mut_unchecked().assume_init_mut() };
 
     let r = body(task_table);