Add timers to preempt (#4053)

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

esp-preempt/src/lib.rs

@@ -28,6 +28,7 @@ mod queue;
 mod semaphore;
 mod task;
 mod timer;
+mod timer_queue;
 
 use core::ffi::c_void;
 
@@ -274,6 +275,7 @@ impl esp_radio_preempt_driver::Scheduler for Scheduler {
         // allocate the main task
         task::allocate_main_task();
         timer::setup_multitasking();
+        timer_queue::create_timer_task();
 
         TIMER.with(|t| {
             let t = unwrap!(t.as_mut());

esp-preempt/src/timer_queue.rs

@@ -0,0 +1,300 @@
+use alloc::boxed::Box;
+use core::{
+    cell::{RefCell, UnsafeCell},
+    ffi::c_void,
+    ptr::NonNull,
+};
+
+use esp_radio_preempt_driver::{
+    Scheduler,
+    register_timer_implementation,
+    timer::{TimerImplementation, TimerPtr},
+};
+use esp_sync::NonReentrantMutex;
+
+use crate::{SCHEDULER, timer::yield_task};
+
+static TIMER_QUEUE: TimerQueue = TimerQueue::new();
+
+struct TimerQueueInner {
+    // A linked list of active timers
+    head: Option<NonNull<Timer>>,
+    next_wakeup: u64,
+}
+
+unsafe impl Send for TimerQueueInner {}
+
+impl TimerQueueInner {
+    const fn new() -> Self {
+        Self {
+            head: None,
+            next_wakeup: 0,
+        }
+    }
+
+    fn enqueue(&mut self, timer: &Timer) {
+        let head = self.head;
+        let props = timer.properties(self);
+        let due = props.next_due;
+
+        if !props.enqueued {
+            props.enqueued = true;
+
+            props.next = head;
+            self.head = Some(NonNull::from(timer));
+        }
+
+        if due < self.next_wakeup {
+            self.next_wakeup = due;
+        }
+    }
+
+    fn dequeue(&mut self, timer: &Timer) -> bool {
+        let mut current = self.head;
+        let mut prev: Option<NonNull<Timer>> = None;
+
+        // Scan through the queue until we find the timer
+        while let Some(current_timer) = current {
+            if core::ptr::eq(current_timer.as_ptr(), timer) {
+                // If we find the timer, remove it from the queue by bypassing it in the linked
+                // list. The previous element, if any, will point at the next element.
+
+                let timer_props = timer.properties(self);
+                let next = timer_props.next.take();
+                timer_props.enqueued = false;
+
+                if let Some(mut p) = prev {
+                    unsafe { p.as_mut().properties(self).next = next };
+                } else {
+                    self.head = next;
+                }
+                return true;
+            }
+
+            prev = current;
+            current = unsafe { current_timer.as_ref().properties(self).next };
+        }
+
+        false
+    }
+}
+
+struct TimerQueue {
+    inner: NonReentrantMutex<TimerQueueInner>,
+}
+
+unsafe impl Send for TimerQueue {}
+
+impl TimerQueue {
+    const fn new() -> Self {
+        Self {
+            inner: NonReentrantMutex::new(TimerQueueInner::new()),
+        }
+    }
+
+    /// Trigger due timers.
+    ///
+    /// The timer queue needs to be re-processed when a new timer is armed, because the new timer
+    /// may need to be triggered before the next scheduled wakeup.
+    fn process(&self) {
+        let mut timers = self.inner.with(|q| {
+            q.next_wakeup = u64::MAX;
+            q.head.take()
+        });
+
+        while let Some(current) = timers {
+            debug!("Checking timer: {:x}", current.addr());
+            let current_timer = unsafe { current.as_ref() };
+
+            let run_callback = self.inner.with(|q| {
+                let props = current_timer.properties(q);
+
+                // Remove current timer from the list.
+                timers = props.next.take();
+
+                if !props.is_active || props.drop {
+                    return false;
+                }
+
+                if props.next_due > SCHEDULER.now() {
+                    // Not our time yet.
+                    return false;
+                }
+
+                if props.periodic {
+                    props.next_due += props.period;
+                }
+                props.is_active = props.periodic;
+                true
+            });
+
+            if run_callback {
+                debug!("Triggering timer: {:x}", current_timer as *const _ as usize);
+                (current_timer.callback.borrow_mut())();
+            }
+
+            self.inner.with(|q| {
+                let props = current_timer.properties(q);
+                // Set this AFTER the callback so that the callback doesn't leave us in an unknown
+                // "queued?" state.
+                props.enqueued = false;
+
+                if props.drop {
+                    debug!("Dropping timer {:x} (delayed)", current.addr());
+                    let boxed = unsafe { Box::from_raw(current.as_ptr()) };
+                    core::mem::drop(boxed);
+                } else if props.is_active {
+                    let next_due = props.next_due;
+                    if next_due < q.next_wakeup {
+                        q.next_wakeup = next_due;
+                    }
+
+                    debug!("Re-queueing timer {:x}", current_timer as *const _ as usize);
+                    q.enqueue(current_timer);
+                } else {
+                    debug!("Timer {:x} inactive", current_timer as *const _ as usize);
+                }
+            });
+        }
+    }
+
+    fn next_wakeup(&self) -> u64 {
+        self.inner.with(|q| q.next_wakeup)
+    }
+}
+
+struct TimerProperties {
+    is_active: bool,
+    next_due: u64,
+    period: u64,
+    periodic: bool,
+    drop: bool,
+
+    enqueued: bool,
+    next: Option<NonNull<Timer>>,
+}
+
+struct TimerQueueCell<T>(UnsafeCell<T>);
+
+impl<T> TimerQueueCell<T> {
+    const fn new(inner: T) -> Self {
+        Self(UnsafeCell::new(inner))
+    }
+
+    fn get_mut<'a>(&'a self, _q: &'a mut TimerQueueInner) -> &'a mut T {
+        unsafe { &mut *self.0.get() }
+    }
+}
+
+pub struct Timer {
+    callback: RefCell<Box<dyn FnMut() + Send>>,
+    // Timer properties, not available in `callback` due to how the timer is constructed.
+    timer_properties: TimerQueueCell<TimerProperties>,
+}
+
+impl Timer {
+    pub fn new(callback: Box<dyn FnMut() + Send>) -> Self {
+        Timer {
+            callback: RefCell::new(callback),
+            timer_properties: TimerQueueCell::new(TimerProperties {
+                is_active: false,
+                next_due: 0,
+                period: 0,
+                periodic: false,
+                drop: false,
+
+                enqueued: false,
+                next: None,
+            }),
+        }
+    }
+
+    unsafe fn from_ptr<'a>(ptr: TimerPtr) -> &'a Self {
+        unsafe { ptr.cast::<Self>().as_mut() }
+    }
+
+    fn arm(&self, q: &mut TimerQueueInner, timeout: u64, periodic: bool) {
+        let next_due = crate::SCHEDULER.now() + timeout;
+
+        let props = self.properties(q);
+        props.is_active = true;
+        props.next_due = next_due;
+        props.period = timeout;
+        props.periodic = periodic;
+        debug!(
+            "Arming timer: {:x} @ {} ({})",
+            self as *const _ as usize, next_due, timeout
+        );
+
+        q.enqueue(self);
+    }
+
+    fn disarm(&self, q: &mut TimerQueueInner) {
+        self.properties(q).is_active = false;
+        debug!("Disarming timer: {:x}", self as *const _ as usize);
+
+        // We don't dequeue the timer - processing the queue will just skip it. If we re-arm,
+        // the timer may already be in the queue.
+    }
+
+    fn properties<'a>(&'a self, q: &'a mut TimerQueueInner) -> &'a mut TimerProperties {
+        self.timer_properties.get_mut(q)
+    }
+}
+
+impl TimerImplementation for Timer {
+    fn create(callback: Box<dyn FnMut() + Send>) -> TimerPtr {
+        let timer = Box::new(Timer::new(callback));
+        NonNull::from(Box::leak(timer)).cast()
+    }
+
+    unsafe fn delete(timer: TimerPtr) {
+        debug!("Deleting timer: {:x}", timer.addr());
+        TIMER_QUEUE.inner.with(|q| {
+            let timer = unsafe { Box::from_raw(timer.cast::<Timer>().as_ptr()) };
+
+            // There are two cases:
+            // - We can remove the timer from the queue - we can drop it.
+            // - We can't remove the timer from the queue. There are the following cases:
+            //   - The timer isn't in the queue. We can drop it.
+            //   - The timer is in the queue and the queue is being processed. We need to mark it to
+            //     be dropped by the timer queue.
+            if q.dequeue(&timer) {
+                core::mem::drop(timer);
+            } else {
+                timer.properties(q).drop = true;
+                core::mem::forget(timer);
+            }
+        })
+    }
+
+    unsafe fn arm(timer: TimerPtr, timeout: u64, periodic: bool) {
+        let timer = unsafe { Timer::from_ptr(timer) };
+        TIMER_QUEUE.inner.with(|q| timer.arm(q, timeout, periodic))
+    }
+
+    unsafe fn disarm(timer: TimerPtr) {
+        let timer = unsafe { Timer::from_ptr(timer) };
+        TIMER_QUEUE.inner.with(|q| timer.disarm(q))
+    }
+}
+
+register_timer_implementation!(Timer);
+
+/// Initializes the `timer` task for the Wi-Fi driver.
+pub(crate) fn create_timer_task() {
+    // schedule the timer task
+    SCHEDULER.task_create(timer_task, core::ptr::null_mut(), 8192);
+}
+
+/// Entry point for the timer task responsible for handling scheduled timer
+/// events.
+pub(crate) extern "C" fn timer_task(_: *mut c_void) {
+    loop {
+        debug!("Timer task");
+        TIMER_QUEUE.process();
+        while SCHEDULER.now() < TIMER_QUEUE.next_wakeup() {
+            yield_task();
+        }
+    }
+}

esp-radio-preempt-driver/src/lib.rs

@@ -31,9 +31,13 @@
 pub mod mutex;
 pub mod queue;
 pub mod semaphore;
+pub mod timer;
 
 use core::ffi::c_void;
 
+// Timer callbacks need to be heap-allocated.
+extern crate alloc;
+
 use crate::semaphore::SemaphorePtr;
 
 unsafe extern "Rust" {

esp-radio-preempt-driver/src/timer.rs

@@ -0,0 +1,130 @@
+//! Timers
+
+use alloc::boxed::Box;
+use core::ptr::NonNull;
+
+/// Pointer to an opaque timer created by the driver implementation.
+pub type TimerPtr = NonNull<()>;
+
+unsafe extern "Rust" {
+    fn esp_preempt_timer_create(callback: Box<dyn FnMut() + Send>) -> TimerPtr;
+    fn esp_preempt_timer_delete(timer: TimerPtr);
+
+    fn esp_preempt_timer_arm(timer: TimerPtr, timeout: u64, periodic: bool);
+    fn esp_preempt_timer_disarm(timer: TimerPtr);
+}
+
+pub trait TimerImplementation {
+    /// Creates a new timer instance from the given callback.
+    fn create(callback: Box<dyn FnMut() + Send>) -> TimerPtr;
+
+    /// Deletes a timer instance.
+    ///
+    /// # Safety
+    ///
+    /// `timer` must be a pointer returned from [`Self::create`].
+    unsafe fn delete(timer: TimerPtr);
+
+    /// Configures the timer to be triggered after the given timeout.
+    ///
+    /// The timeout is specified in microsecond. If the timer is set to be periodic,
+    /// the timer will be triggered with a constant frequency.
+    ///
+    /// # Safety
+    ///
+    /// `timer` must be a pointer returned from [`Self::create`].
+    unsafe fn arm(timer: TimerPtr, timeout: u64, periodic: bool);
+
+    /// Stops the timer.
+    ///
+    /// # Safety
+    ///
+    /// `timer` must be a pointer returned from [`Self::create`].
+    unsafe fn disarm(timer: TimerPtr);
+}
+
+#[macro_export]
+macro_rules! register_timer_implementation {
+    ($t: ty) => {
+        #[unsafe(no_mangle)]
+        #[inline]
+        fn esp_preempt_timer_create(callback: Box<dyn FnMut() + Send>) -> $crate::timer::TimerPtr {
+            <$t as $crate::timer::TimerImplementation>::create(callback)
+        }
+
+        #[unsafe(no_mangle)]
+        #[inline]
+        fn esp_preempt_timer_delete(timer: $crate::timer::TimerPtr) {
+            unsafe { <$t as $crate::timer::TimerImplementation>::delete(timer) }
+        }
+
+        #[unsafe(no_mangle)]
+        #[inline]
+        fn esp_preempt_timer_arm(timer: $crate::timer::TimerPtr, timeout: u64, periodic: bool) {
+            unsafe { <$t as $crate::timer::TimerImplementation>::arm(timer, timeout, periodic) }
+        }
+
+        #[unsafe(no_mangle)]
+        #[inline]
+        fn esp_preempt_timer_disarm(timer: $crate::timer::TimerPtr) {
+            unsafe { <$t as $crate::timer::TimerImplementation>::disarm(timer) }
+        }
+    };
+}
+
+#[repr(transparent)]
+pub struct TimerHandle(TimerPtr);
+impl TimerHandle {
+    /// Creates a new timer instance from the given callback.
+    pub fn new(callback: Box<dyn FnMut() + Send>) -> Self {
+        let ptr = unsafe { esp_preempt_timer_create(callback) };
+        Self(ptr)
+    }
+
+    /// Converts this object into a pointer without dropping it.
+    pub fn leak(self) -> TimerPtr {
+        let ptr = self.0;
+        core::mem::forget(self);
+        ptr
+    }
+
+    /// Recovers the object from a leaked pointer.
+    ///
+    /// # Safety
+    ///
+    /// - The caller must only use pointers created using [`Self::leak`].
+    /// - The caller must ensure the pointer is not shared.
+    pub unsafe fn from_ptr(ptr: TimerPtr) -> Self {
+        Self(ptr)
+    }
+
+    /// Creates a reference to this object from a leaked pointer.
+    ///
+    /// This function is used in the esp-radio code to interact with the timer.
+    ///
+    /// # Safety
+    ///
+    /// - The caller must only use pointers created using [`Self::leak`].
+    pub unsafe fn ref_from_ptr(ptr: &TimerPtr) -> &Self {
+        unsafe { core::mem::transmute(ptr) }
+    }
+
+    /// Configures the timer to be triggered after the given timeout.
+    ///
+    /// The timeout is specified in microsecond. If the timer is set to be periodic,
+    /// the timer will be triggered with a constant frequency.
+    pub fn arm(&self, timeout: u64, periodic: bool) {
+        unsafe { esp_preempt_timer_arm(self.0, timeout, periodic) }
+    }
+
+    /// Stops the timer.
+    pub fn disarm(&self) {
+        unsafe { esp_preempt_timer_disarm(self.0) }
+    }
+}
+
+impl Drop for TimerHandle {
+    fn drop(&mut self) {
+        unsafe { esp_preempt_timer_delete(self.0) };
+    }
+}

esp-radio/src/compat/mod.rs

@@ -6,6 +6,8 @@ pub mod misc;
 pub mod mutex;
 pub mod queue;
 pub mod semaphore;
+
+#[cfg(any(feature = "wifi", all(feature = "ble", npl)))]
 pub mod timer_compat;
 
 pub(crate) const OSI_FUNCS_TIME_BLOCKING: u32 = u32::MAX;

esp-radio/src/compat/timer_compat.rs

@@ -1,251 +1,103 @@
 use alloc::boxed::Box;
 
-use esp_sync::NonReentrantMutex;
+use esp_radio_preempt_driver::timer::TimerHandle;
 
-use crate::binary::{
-    c_types,
-    include::{esp_timer_create_args_t, ets_timer},
+use crate::{
+    binary::{c_types::c_void, include::ets_timer},
+    preempt::timer::TimerPtr,
 };
 
-#[derive(Clone, Copy, Debug)]
-pub(crate) struct TimerCallback {
-    f: unsafe extern "C" fn(*mut c_types::c_void),
-    args: *mut c_types::c_void,
-}
-
-impl TimerCallback {
-    fn new(f: unsafe extern "C" fn(*mut c_types::c_void), args: *mut c_types::c_void) -> Self {
-        Self { f, args }
-    }
-
-    pub(crate) fn call(self) {
-        unsafe { (self.f)(self.args) };
-    }
-}
-
-impl From<&esp_timer_create_args_t> for TimerCallback {
-    fn from(args: &esp_timer_create_args_t) -> Self {
-        Self::new(unwrap!(args.callback), args.arg)
-    }
-}
-
-#[repr(C)]
-#[derive(Debug, Clone)]
-pub(crate) struct Timer {
-    pub ets_timer: *mut ets_timer,
-    pub started: u64,
-    pub timeout: u64,
-    pub active: bool,
-    pub periodic: bool,
-    pub callback: TimerCallback,
-
-    next: Option<Box<Timer>>,
-}
-
-#[cfg(any(feature = "wifi", all(feature = "ble", npl)))]
-impl Timer {
-    pub(crate) fn id(&self) -> usize {
-        self.ets_timer as usize
-    }
-}
-
-pub(crate) struct TimerQueue {
-    head: Option<Box<Timer>>,
-}
-
-impl TimerQueue {
-    const fn new() -> Self {
-        Self { head: None }
-    }
-
-    pub(crate) unsafe fn find_next_due(
-        &mut self,
-        current_timestamp: u64,
-    ) -> Option<&mut Box<Timer>> {
-        let mut current = self.head.as_mut();
-        while let Some(timer) = current {
-            if timer.active && current_timestamp - timer.started >= timer.timeout {
-                return Some(timer);
-            }
-            current = timer.next.as_mut();
-        }
-
-        None
-    }
-}
-
-#[cfg(any(feature = "wifi", all(feature = "ble", npl)))]
-impl TimerQueue {
-    fn find(&mut self, ets_timer: *mut ets_timer) -> Option<&mut Box<Timer>> {
-        let mut current = self.head.as_mut();
-        while let Some(timer) = current {
-            if core::ptr::eq(timer.ets_timer, ets_timer) {
-                return Some(timer);
-            }
-            current = timer.next.as_mut();
-        }
-
-        None
-    }
-
-    #[cfg(feature = "wifi")]
-    fn remove(&mut self, ets_timer: *mut ets_timer) {
-        if let Some(head) = self.head.as_mut()
-            && core::ptr::eq(head.ets_timer, ets_timer)
-        {
-            self.head = head.next.take();
-            return;
-        }
-
-        let timer = self.find(ets_timer);
-        if let Some(to_remove) = timer {
-            let tail = to_remove.next.take();
-
-            let mut current = self.head.as_mut();
-            let before = {
-                let mut found = None;
-                while let Some(before) = current {
-                    if core::ptr::eq(before.next.as_mut().unwrap().ets_timer, ets_timer) {
-                        found = Some(before);
-                        break;
-                    }
-                    current = before.next.as_mut();
-                }
-                found
-            };
-
-            if let Some(before) = before {
-                let to_remove = before.next.take().unwrap();
-                let to_remove = Box::into_raw(to_remove);
-                unsafe {
-                    crate::compat::malloc::free(to_remove as *mut _);
-                }
-                before.next = tail;
-            }
-        }
-    }
-
-    fn push(&mut self, to_add: Box<Timer>) -> Result<(), ()> {
-        if self.head.is_none() {
-            self.head = Some(to_add);
-            return Ok(());
-        }
-
-        let mut current = self.head.as_mut();
-        while let Some(timer) = current {
-            if timer.next.is_none() {
-                timer.next = Some(to_add);
-                break;
-            }
-            current = timer.next.as_mut();
-        }
-        Ok(())
-    }
-}
-
-unsafe impl Send for TimerQueue {}
-
-pub(crate) static TIMERS: NonReentrantMutex<TimerQueue> = NonReentrantMutex::new(TimerQueue::new());
-
-#[cfg(any(feature = "wifi", all(feature = "ble", npl)))]
 pub(crate) fn compat_timer_arm(ets_timer: *mut ets_timer, tmout: u32, repeat: bool) {
     compat_timer_arm_us(ets_timer, tmout * 1000, repeat);
 }
 
-#[cfg(any(feature = "wifi", all(feature = "ble", npl)))]
 pub(crate) fn compat_timer_arm_us(ets_timer: *mut ets_timer, us: u32, repeat: bool) {
-    let systick = crate::preempt::now();
-    let ticks = crate::time::micros_to_ticks(us as u64);
-
     trace!(
         "timer_arm_us {:x} current: {} ticks: {} repeat: {}",
-        ets_timer as usize, systick, ticks, repeat
+        ets_timer as usize,
+        crate::preempt::now(),
+        crate::time::micros_to_ticks(us as u64),
+        repeat
     );
 
-    TIMERS.with(|timers| {
-        if let Some(timer) = timers.find(ets_timer) {
-            timer.started = systick;
-            timer.timeout = ticks;
-            timer.active = true;
-            timer.periodic = repeat;
-        } else {
-            trace!("timer_arm_us {:x} not found", ets_timer as usize);
-        }
-    })
+    let ets_timer = unwrap!(unsafe { ets_timer.as_mut() }, "ets_timer is null");
+
+    let timer = unwrap!(TimerPtr::new(ets_timer.priv_.cast()), "timer is null");
+    let timer = unsafe { TimerHandle::ref_from_ptr(&timer) };
+
+    timer.arm(us as u64, repeat);
 }
 
-#[cfg(any(feature = "wifi", all(feature = "ble", npl)))]
 pub(crate) fn compat_timer_disarm(ets_timer: *mut ets_timer) {
     trace!("timer disarm");
-    TIMERS.with(|timers| {
-        if let Some(timer) = timers.find(ets_timer) {
-            trace!("timer_disarm {:x}", timer.id());
-            timer.active = false;
-        } else {
-            trace!("timer_disarm {:x} not found", ets_timer as usize);
-        }
-    })
+    let ets_timer = unwrap!(unsafe { ets_timer.as_mut() }, "ets_timer is null");
+
+    if let Some(timer) = TimerPtr::new(ets_timer.priv_.cast()) {
+        let timer = unsafe { TimerHandle::ref_from_ptr(&timer) };
+
+        timer.disarm();
+    }
+}
+
+fn delete_timer(ets_timer: &mut ets_timer) {
+    if let Some(timer) = TimerPtr::new(ets_timer.priv_.cast()) {
+        let timer = unsafe { TimerHandle::from_ptr(timer) };
+
+        core::mem::drop(timer);
+    }
 }
 
-#[cfg(feature = "wifi")]
 pub(crate) fn compat_timer_done(ets_timer: *mut ets_timer) {
     trace!("timer done");
-    TIMERS.with(|timers| {
-        if let Some(timer) = timers.find(ets_timer) {
-            trace!("timer_done {:x}", timer.id());
-            timer.active = false;
 
-            unsafe {
-                (*ets_timer).priv_ = core::ptr::null_mut();
-                (*ets_timer).expire = 0;
-            }
+    let ets_timer = unwrap!(unsafe { ets_timer.as_mut() }, "ets_timer is null");
 
-            timers.remove(ets_timer);
-        } else {
-            trace!("timer_done {:x} not found", ets_timer as usize);
-        }
-    })
+    delete_timer(ets_timer);
 }
 
-#[cfg(any(feature = "wifi", all(feature = "ble", npl)))]
 pub(crate) fn compat_timer_setfn(
     ets_timer: *mut ets_timer,
-    pfunction: unsafe extern "C" fn(*mut c_types::c_void),
-    parg: *mut c_types::c_void,
+    pfunction: unsafe extern "C" fn(*mut c_void),
+    parg: *mut c_void,
 ) {
     trace!(
         "timer_setfn {:x} {:?} {:?}",
         ets_timer as usize, pfunction, parg
     );
-    let set = TIMERS.with(|timers| unsafe {
-        if let Some(timer) = timers.find(ets_timer) {
-            timer.callback = TimerCallback::new(pfunction, parg);
-            timer.active = false;
 
-            (*ets_timer).expire = 0;
+    let ets_timer = unwrap!(unsafe { ets_timer.as_mut() }, "ets_timer is null");
 
-            true
-        } else {
-            (*ets_timer).next = core::ptr::null_mut();
-            (*ets_timer).period = 0;
-            (*ets_timer).func = None;
-            (*ets_timer).priv_ = core::ptr::null_mut();
+    // This function is expected to create timers. For the simplicity of the preempt API, we
+    // will not update existing timers, but create new ones.
+    delete_timer(ets_timer);
 
-            let timer =
-                crate::compat::malloc::calloc(1, core::mem::size_of::<Timer>()) as *mut Timer;
-            (*timer).next = None;
-            (*timer).ets_timer = ets_timer;
-            (*timer).started = 0;
-            (*timer).timeout = 0;
-            (*timer).active = false;
-            (*timer).periodic = false;
-            (*timer).callback = TimerCallback::new(pfunction, parg);
-
-            timers.push(Box::from_raw(timer)).is_ok()
-        }
-    });
-
-    if !set {
-        warn!("Failed to set timer function {:x}", ets_timer as usize);
+    // Unfortunately, Rust can't optimize this into a single fat pointer, so this will
+    // allocate on the heap. We could optimize for C functions in the preempt API, but that would
+    // require some unfortunate unsafe code.
+    struct CCallback {
+        func: unsafe extern "C" fn(*mut c_void),
+        data: *mut c_void,
     }
+    unsafe impl Send for CCallback {}
+
+    impl CCallback {
+        unsafe fn call(&mut self) {
+            unsafe { (self.func)(self.data) }
+        }
+    }
+
+    let mut callback = CCallback {
+        func: pfunction,
+        data: parg,
+    };
+
+    let timer = TimerHandle::new(Box::new(move || unsafe { callback.call() }))
+        .leak()
+        .cast()
+        .as_ptr();
+
+    ets_timer.next = core::ptr::null_mut();
+    ets_timer.period = 0;
+    ets_timer.func = None;
+    ets_timer.priv_ = timer;
 }

esp-radio/src/lib.rs

@@ -129,7 +129,6 @@ use crate::wifi::WifiError;
 use crate::{
     preempt::yield_task,
     radio::{setup_radio_isr, shutdown_radio_isr},
-    tasks::init_tasks,
 };
 
 // can't use instability on inline module definitions, see https://github.com/rust-lang/rust/issues/54727
@@ -177,10 +176,6 @@ unstable_module! {
 }
 
 pub(crate) mod common_adapter;
-
-#[doc(hidden)]
-pub mod tasks;
-
 pub(crate) mod memory_fence;
 
 pub(crate) static ESP_RADIO_LOCK: RawMutex = RawMutex::new();
@@ -261,7 +256,6 @@ pub fn init<'d>() -> Result<Controller<'d>, InitializationError> {
     // This initializes the task switcher
     preempt::enable();
 
-    init_tasks();
     yield_task();
 
     wifi_set_log_verbose();

esp-radio/src/tasks.rs

@@ -1,40 +0,0 @@
-use crate::{
-    compat::timer_compat::TIMERS,
-    preempt::{task_create, yield_task},
-};
-
-/// Initializes the `timer` task for the Wi-Fi driver.
-pub(crate) fn init_tasks() {
-    // schedule the timer task
-    unsafe {
-        task_create(timer_task, core::ptr::null_mut(), 8192);
-    }
-}
-
-/// Entry point for the timer task responsible for handling scheduled timer
-/// events.
-pub(crate) extern "C" fn timer_task(_param: *mut esp_wifi_sys::c_types::c_void) {
-    loop {
-        let current_timestamp = crate::preempt::now();
-        let to_run = TIMERS.with(|timers| {
-            let to_run = unsafe { timers.find_next_due(current_timestamp) }?;
-
-            to_run.active = to_run.periodic;
-
-            if to_run.periodic {
-                to_run.started = current_timestamp;
-            }
-
-            Some(to_run.callback)
-        });
-
-        // run the due timer callback NOT in an interrupt free context
-        if let Some(to_run) = to_run {
-            trace!("trigger timer....");
-            to_run.call();
-            trace!("timer callback called");
-        } else {
-            yield_task();
-        }
-    }
-}