Possibly awful SMP implementation (#4152)

* Test and fix time slicing

* Possibly awful SMP implementation

Co-authored-by: g1ibby <[email protected]>

* Remove __esp_radio_builtin_scheduler

---------

Co-authored-by: g1ibby <[email protected]>

Author: bugadani

esp-hal/Cargo.toml

@@ -136,8 +136,6 @@ default = ["rt", "exception-handler"]
 __bluetooth = []
 # Reserves FROM_CPU_INTR3 for multi-core MCUs.
 __esp_hal_embassy = []
-# Reserves FROM_CPU_INTR2 for RISC-V MCUs.
-__esp_radio_builtin_scheduler = []
 __usb_otg = [
     "dep:embassy-usb-driver",
     "dep:embassy-usb-synopsys-otg",

esp-hal/src/exception_handler/mod.rs

@@ -7,7 +7,12 @@ unsafe extern "C" fn __user_exception(
     cause: xtensa_lx_rt::exception::ExceptionCause,
     context: &TrapFrame,
 ) {
-    panic!("\n\nException occurred '{:?}'\n{:?}", cause, context);
+    panic!(
+        "\n\nException occurred on {:?} '{:?}'\n{:?}",
+        crate::system::Cpu::current(),
+        cause,
+        context
+    );
 }
 
 #[cfg(riscv)]

esp-hal/src/interrupt/mod.rs

@@ -87,7 +87,7 @@ pub mod software;
 #[cfg(feature = "rt")]
 #[unsafe(no_mangle)]
 extern "C" fn EspDefaultHandler() {
-    panic!("Unhandled interrupt");
+    panic!("Unhandled interrupt on {:?}", crate::system::Cpu::current());
 }
 
 /// Default (unhandled) interrupt handler

esp-hal/src/interrupt/software.rs

@@ -170,9 +170,7 @@ pub struct SoftwareInterruptControl<'d> {
     pub software_interrupt0: SoftwareInterrupt<'d, 0>,
     /// Software interrupt 1.
     pub software_interrupt1: SoftwareInterrupt<'d, 1>,
-    /// Software interrupt 2. Not available when using esp-radio's builtin
-    /// scheduler on RISC-V architectures.
-    #[cfg(not(all(feature = "__esp_radio_builtin_scheduler", riscv)))]
+    /// Software interrupt 2.
     pub software_interrupt2: SoftwareInterrupt<'d, 2>,
     #[cfg(not(all(feature = "__esp_hal_embassy", multi_core)))]
     /// Software interrupt 3. Not available when using `esp-hal-embassy`,
@@ -190,7 +188,6 @@ impl<'d> SoftwareInterruptControl<'d> {
             software_interrupt1: SoftwareInterrupt {
                 _lifetime: PhantomData,
             },
-            #[cfg(not(all(feature = "__esp_radio_builtin_scheduler", riscv)))]
             software_interrupt2: SoftwareInterrupt {
                 _lifetime: PhantomData,
             },

esp-preempt/Cargo.toml

@@ -30,7 +30,6 @@ bench = false
 [dependencies]
 esp-hal = { version = "1.0.0-rc.0", path = "../esp-hal", features = [
     "requires-unstable",
-    "__esp_radio_builtin_scheduler",
 ] }
 
 cfg-if = "1"

esp-preempt/src/lib.rs

@@ -3,12 +3,50 @@
 //! This crate requires an esp-hal timer to operate.
 //!
 //! ```rust, no_run
-#![doc = esp_hal::before_snippet!()]
 //! use esp_hal::timer::timg::TimerGroup;
-//!
 //! let timg0 = TimerGroup::new(peripherals.TIMG0);
-//! esp_preempt::start(timg0.timer0);
-//!
+#![doc = esp_hal::before_snippet!()]
+#![cfg_attr(
+    any(riscv, multi_core),
+    doc = "
+
+use esp_hal::interrupt::software::SoftwareInterruptControl;
+let software_interrupt = SoftwareInterruptControl::new(peripherals.SW_INTERRUPT);"
+)]
+#![cfg_attr(
+    xtensa,
+    doc = "
+
+esp_preempt::start(timg0.timer0);"
+)]
+#![cfg_attr(
+    riscv,
+    doc = "
+
+esp_preempt::start(timg0.timer0, software_interrupt.software_interrupt0);"
+)]
+#![cfg_attr(
+    all(xtensa, multi_core),
+    doc = "
+// Optionally, start the scheduler on the second core
+esp_preempt::start_second_core(
+    software_interrupt.software_interrupt0,
+    software_interrupt.software_interrupt1,
+    || {}, // Second core's main function.
+);
+"
+)]
+#![cfg_attr(
+    all(riscv, multi_core),
+    doc = "
+// Optionally, start the scheduler on the second core
+esp_preempt::start_second_core(
+    software_interrupt.software_interrupt1,
+    || {}, // Second core's main function.
+);
+"
+)]
+#![doc = ""]
 //! // You can now start esp-radio:
 //! // let esp_radio_controller = esp_radio::init().unwrap();
 //! # }
@@ -35,10 +73,19 @@ mod wait_queue;
 use core::mem::MaybeUninit;
 
 pub(crate) use esp_alloc::InternalMemory;
+#[cfg(any(multi_core, riscv))]
+use esp_hal::interrupt::software::SoftwareInterrupt;
 use esp_hal::{
     Blocking,
+    system::Cpu,
     timer::{AnyTimer, OneShotTimer},
 };
+#[cfg(multi_core)]
+use esp_hal::{
+    peripherals::CPU_CTRL,
+    system::{CpuControl, Stack},
+    time::{Duration, Instant},
+};
 pub(crate) use scheduler::SCHEDULER;
 
 use crate::timer::TimeDriver;
@@ -105,6 +152,8 @@ where
 
 /// Starts the scheduler.
 ///
+/// The current context will be converted into the main task, and will be pinned to the first core.
+///
 /// # The `timer` argument
 ///
 /// The `timer` argument is a timer source that is used by the scheduler to
@@ -119,7 +168,10 @@ where
     multi_core,
     doc = " \nNote that `esp_radio::init()` must be called on the same core as `esp_preempt::start()`."
 )]
-pub fn start(timer: impl TimerSource) {
+pub fn start(timer: impl TimerSource, #[cfg(riscv)] int0: SoftwareInterrupt<'static, 0>) {
+    trace!("Starting scheduler for the first core");
+    assert_eq!(Cpu::current(), Cpu::ProCpu);
+
     SCHEDULER.with(move |scheduler| {
         scheduler.setup(TimeDriver::new(timer.timer()));
 
@@ -138,11 +190,71 @@ pub fn start(timer: impl TimerSource) {
         );
         task::allocate_main_task(scheduler, stack_slice);
 
-        task::setup_multitasking();
+        task::setup_multitasking(
+            #[cfg(riscv)]
+            int0,
+        );
 
-        unwrap!(scheduler.time_driver.as_mut()).start();
         task::yield_task();
     })
 }
 
+/// Starts the scheduler on the second CPU core.
+///
+/// Note that the scheduler must be started first, before starting the second core.
+///
+/// The supplied stack and function will be used as the main thread of the second core. The thread
+/// will be pinned to the second core.
+#[cfg(multi_core)]
+pub fn start_second_core<const STACK_SIZE: usize>(
+    cpu_control: CPU_CTRL,
+    #[cfg(xtensa)] int0: SoftwareInterrupt<'static, 0>,
+    int1: SoftwareInterrupt<'static, 1>,
+    stack: &'static mut Stack<STACK_SIZE>,
+    func: impl FnOnce() + Send + 'static,
+) {
+    trace!("Starting scheduler for the second core");
+
+    #[cfg(xtensa)]
+    task::setup_smp(int0);
+
+    struct SecondCoreStack {
+        stack: *mut [MaybeUninit<u32>],
+    }
+    unsafe impl Send for SecondCoreStack {}
+    let stack_ptrs = SecondCoreStack {
+        stack: core::ptr::slice_from_raw_parts_mut(
+            stack.bottom().cast::<MaybeUninit<u32>>(),
+            STACK_SIZE,
+        ),
+    };
+
+    let mut cpu_control = CpuControl::new(cpu_control);
+    let guard = cpu_control
+        .start_app_core(stack, move || {
+            trace!("Second core running");
+            task::setup_smp(int1);
+            SCHEDULER.with(move |scheduler| {
+                // Make sure the whole struct is captured, not just a !Send field.
+                let ptrs = stack_ptrs;
+                assert!(
+                    scheduler.time_driver.is_some(),
+                    "The scheduler must be started on the first core first."
+                );
+                task::allocate_main_task(scheduler, ptrs.stack);
+                task::yield_task();
+                trace!("Second core scheduler initialized");
+            });
+
+            func();
+
+            loop {
+                SCHEDULER.sleep_until(Instant::EPOCH + Duration::MAX);
+            }
+        })
+        .unwrap();
+
+    core::mem::forget(guard);
+}
+
 const TICK_RATE: u32 = esp_config::esp_config_int!(u32, "ESP_PREEMPT_CONFIG_TICK_RATE_HZ");

esp-preempt/src/run_queue.rs

@@ -1,5 +1,6 @@
 use crate::task::{TaskExt, TaskPtr, TaskQueue, TaskReadyQueueElement, TaskState};
 
+#[derive(Clone, Copy)]
 pub(crate) struct MaxPriority {
     max: usize,
     mask: usize,
@@ -71,7 +72,39 @@ impl RunQueue {
         let current_prio = self.ready_priority.ready();
         debug!("pop - from level: {}", current_prio);
 
-        let popped = self.ready_tasks[current_prio].pop();
+        cfg_if::cfg_if! {
+            if #[cfg(multi_core)] {
+                use esp_hal::system::Cpu;
+
+                let other = match Cpu::current() {
+                    Cpu::AppCpu => Cpu::ProCpu,
+                    Cpu::ProCpu => Cpu::AppCpu,
+                };
+
+                // Look iteratively through priority levels - this will ensure we can find a task even if all high priority tasks are pinned to the other CPU.
+                let mut priority_level = self.ready_priority;
+
+                let mut current_prio = current_prio;
+                let mut popped;
+                loop {
+                    popped = self.ready_tasks[current_prio].pop_if(|t| t.pinned_to != Some(other));
+
+                    if popped.is_none() {
+                        priority_level.unmark(current_prio);
+                        if current_prio == 0 {
+                            break;
+                        }
+                        current_prio = priority_level.ready();
+                        continue;
+                    }
+
+                    break;
+                }
+
+            } else {
+                let popped = self.ready_tasks[current_prio].pop();
+            }
+        }
 
         if self.ready_tasks[current_prio].is_empty() {
             self.ready_priority.unmark(current_prio);