Implement starting a task on second core of ESP32 and ESP32-S3

esp-hal-common/.vscode/settings.json

@@ -1,6 +1,6 @@
 {
     "rust-analyzer.cargo.features": [
-        "esp32"
+        "esp32s3"
     ],
     "rust-analyzer.cargo.allFeatures": false,
     "editor.formatOnSave": true,
@@ -10,12 +10,12 @@
         "cargo",
         "check",
         "--features",
-        "esp32",
+        "esp32s3",
         "--message-format=json",
         "-Z",
         "build-std=core",
         "--target",
-        "xtensa-esp32-none-elf",
+        "xtensa-esp32s3-none-elf",
         "--examples",
         "--lib",
     ],

esp-hal-common/src/cpu_control/esp32.rs

@@ -0,0 +1,236 @@
+//! Control CPU Cores
+
+use core::marker::PhantomData;
+
+use xtensa_lx::set_stack_pointer;
+
+use crate::Cpu;
+
+static mut START_CORE1_FUNCTION: Option<&'static mut (dyn FnMut() -> () + 'static)> = None;
+
+pub struct AppCoreGuard<'a> {
+    phantom: PhantomData<&'a ()>,
+}
+
+impl<'a> Drop for AppCoreGuard<'a> {
+    fn drop(&mut self) {
+        unsafe {
+            internal_park_core(Cpu::AppCpu);
+        }
+    }
+}
+
+#[derive(Debug)]
+pub enum Error {
+    CoreAlreadyRunning,
+}
+
+/// Control CPU Cores
+pub struct CpuControl {
+    _cpu_control: crate::system::CpuControl,
+}
+
+unsafe fn internal_park_core(core: Cpu) {
+    let rtc_control = crate::pac::RTC_CNTL::PTR;
+    let rtc_control = &*rtc_control;
+
+    match core {
+        Cpu::ProCpu => {
+            rtc_control
+                .sw_cpu_stall
+                .modify(|_, w| w.sw_stall_procpu_c1().bits(0x21));
+            rtc_control
+                .options0
+                .modify(|_, w| w.sw_stall_procpu_c0().bits(0x02));
+        }
+        Cpu::AppCpu => {
+            rtc_control
+                .sw_cpu_stall
+                .modify(|_, w| w.sw_stall_appcpu_c1().bits(0x21));
+            rtc_control
+                .options0
+                .modify(|_, w| w.sw_stall_appcpu_c0().bits(0x02));
+        }
+    }
+}
+
+impl CpuControl {
+    pub fn new(cpu_control: crate::system::CpuControl) -> CpuControl {
+        CpuControl {
+            _cpu_control: cpu_control,
+        }
+    }
+
+    /// Park the given core
+    pub unsafe fn park_core(&mut self, core: Cpu) {
+        internal_park_core(core);
+    }
+
+    /// Unpark the given core
+    pub fn unpark_core(&mut self, core: Cpu) {
+        let rtc_control = crate::pac::RTC_CNTL::PTR;
+        let rtc_control = unsafe { &*rtc_control };
+
+        match core {
+            Cpu::ProCpu => {
+                rtc_control
+                    .sw_cpu_stall
+                    .modify(|_, w| unsafe { w.sw_stall_procpu_c1().bits(0) });
+                rtc_control
+                    .options0
+                    .modify(|_, w| unsafe { w.sw_stall_procpu_c0().bits(0) });
+            }
+            Cpu::AppCpu => {
+                rtc_control
+                    .sw_cpu_stall
+                    .modify(|_, w| unsafe { w.sw_stall_appcpu_c1().bits(0) });
+                rtc_control
+                    .options0
+                    .modify(|_, w| unsafe { w.sw_stall_appcpu_c0().bits(0) });
+            }
+        }
+    }
+
+    fn flush_cache(&mut self, core: Cpu) {
+        let dport_control = crate::pac::DPORT::PTR;
+        let dport_control = unsafe { &*dport_control };
+
+        match core {
+            Cpu::ProCpu => {
+                dport_control
+                    .pro_cache_ctrl
+                    .modify(|_, w| w.pro_cache_flush_ena().clear_bit());
+                dport_control
+                    .pro_cache_ctrl
+                    .modify(|_, w| w.pro_cache_flush_ena().set_bit());
+                while dport_control
+                    .pro_cache_ctrl
+                    .read()
+                    .pro_cache_flush_done()
+                    .bit_is_clear()
+                {}
+
+                dport_control
+                    .pro_cache_ctrl
+                    .modify(|_, w| w.pro_cache_flush_ena().clear_bit());
+            }
+            Cpu::AppCpu => {
+                dport_control
+                    .app_cache_ctrl
+                    .modify(|_, w| w.app_cache_flush_ena().clear_bit());
+                dport_control
+                    .app_cache_ctrl
+                    .modify(|_, w| w.app_cache_flush_ena().set_bit());
+                while dport_control
+                    .app_cache_ctrl
+                    .read()
+                    .app_cache_flush_done()
+                    .bit_is_clear()
+                {}
+                dport_control
+                    .app_cache_ctrl
+                    .modify(|_, w| w.app_cache_flush_ena().clear_bit());
+            }
+        };
+    }
+
+    fn enable_cache(&mut self, core: Cpu) {
+        let spi0 = unsafe { &(*crate::pac::SPI0::ptr()) };
+
+        let dport_control = crate::pac::DPORT::PTR;
+        let dport_control = unsafe { &*dport_control };
+
+        match core {
+            Cpu::ProCpu => {
+                spi0.cache_fctrl.modify(|_, w| w.cache_req_en().set_bit());
+                dport_control
+                    .pro_cache_ctrl
+                    .modify(|_, w| w.pro_cache_enable().set_bit());
+            }
+            Cpu::AppCpu => {
+                spi0.cache_fctrl.modify(|_, w| w.cache_req_en().set_bit());
+                dport_control
+                    .app_cache_ctrl
+                    .modify(|_, w| w.app_cache_enable().set_bit());
+            }
+        };
+    }
+
+    unsafe fn start_core1_init() -> ! {
+        extern "C" {
+            static mut _stack_end_cpu1: u32;
+        }
+
+        // disables interrupts
+        xtensa_lx::interrupt::set_mask(0);
+
+        // reset cycle compare registers
+        xtensa_lx::timer::set_ccompare0(0);
+        xtensa_lx::timer::set_ccompare1(0);
+        xtensa_lx::timer::set_ccompare2(0);
+
+        // set stack pointer to end of memory: no need to retain stack up to this point
+        set_stack_pointer(&mut _stack_end_cpu1);
+
+        match START_CORE1_FUNCTION.take() {
+            Some(entry) => (*entry)(),
+            None => panic!("No start function set"),
+        }
+
+        panic!("Return from second core's entry");
+    }
+
+    /// Start the APP (second) core
+    ///
+    /// The second core will start running the closure `entry`.
+    #[must_use]
+    pub fn start_app_core<'a: 'b, 'b>(
+        &mut self,
+        entry: &'a mut (dyn FnMut() -> () + 'a),
+    ) -> Result<AppCoreGuard<'b>, Error> {
+        let dport_control = crate::pac::DPORT::PTR;
+        let dport_control = unsafe { &*dport_control };
+
+        if !xtensa_lx::is_debugger_attached()
+            && dport_control
+                .appcpu_ctrl_b
+                .read()
+                .appcpu_clkgate_en()
+                .bit_is_set()
+        {
+            return Err(Error::CoreAlreadyRunning);
+        }
+
+        self.flush_cache(Cpu::AppCpu);
+        self.enable_cache(Cpu::AppCpu);
+
+        unsafe {
+            let entry_fn: &'static mut (dyn FnMut() -> () + 'static) = core::mem::transmute(entry);
+            START_CORE1_FUNCTION = Some(entry_fn);
+        }
+
+        dport_control.appcpu_ctrl_d.write(|w| unsafe {
+            w.appcpu_boot_addr()
+                .bits(Self::start_core1_init as *const u32 as u32)
+        });
+
+        dport_control
+            .appcpu_ctrl_b
+            .modify(|_, w| w.appcpu_clkgate_en().set_bit());
+        dport_control
+            .appcpu_ctrl_c
+            .modify(|_, w| w.appcpu_runstall().clear_bit());
+        dport_control
+            .appcpu_ctrl_a
+            .modify(|_, w| w.appcpu_resetting().set_bit());
+        dport_control
+            .appcpu_ctrl_a
+            .modify(|_, w| w.appcpu_resetting().clear_bit());
+
+        self.unpark_core(Cpu::AppCpu);
+
+        Ok(AppCoreGuard {
+            phantom: PhantomData::default(),
+        })
+    }
+}

esp-hal-common/src/cpu_control/esp32s3.rs

@@ -0,0 +1,172 @@
+//! Control CPU Cores
+
+use core::marker::PhantomData;
+
+use xtensa_lx::set_stack_pointer;
+
+use crate::Cpu;
+
+static mut START_CORE1_FUNCTION: Option<&'static mut (dyn FnMut() -> () + 'static)> = None;
+
+pub struct AppCoreGuard<'a> {
+    phantom: PhantomData<&'a ()>,
+}
+
+impl<'a> Drop for AppCoreGuard<'a> {
+    fn drop(&mut self) {
+        unsafe {
+            internal_park_core(Cpu::AppCpu);
+        }
+    }
+}
+
+#[derive(Debug)]
+pub enum Error {
+    CoreAlreadyRunning,
+}
+
+/// Control CPU Cores
+pub struct CpuControl {
+    _cpu_control: crate::system::CpuControl,
+}
+
+unsafe fn internal_park_core(core: Cpu) {
+    let rtc_control = crate::pac::RTC_CNTL::PTR;
+    let rtc_control = &*rtc_control;
+
+    match core {
+        Cpu::ProCpu => {
+            rtc_control
+                .sw_cpu_stall
+                .modify(|_, w| w.sw_stall_procpu_c1().bits(0x21));
+            rtc_control
+                .options0
+                .modify(|_, w| w.sw_stall_procpu_c0().bits(0x02));
+        }
+        Cpu::AppCpu => {
+            rtc_control
+                .sw_cpu_stall
+                .modify(|_, w| w.sw_stall_appcpu_c1().bits(0x21));
+            rtc_control
+                .options0
+                .modify(|_, w| w.sw_stall_appcpu_c0().bits(0x02));
+        }
+    }
+}
+
+impl CpuControl {
+    pub fn new(cpu_control: crate::system::CpuControl) -> CpuControl {
+        CpuControl {
+            _cpu_control: cpu_control,
+        }
+    }
+
+    /// Park the given core
+    pub unsafe fn park_core(&mut self, core: Cpu) {
+        internal_park_core(core);
+    }
+
+    /// Unpark the given core
+    pub fn unpark_core(&mut self, core: Cpu) {
+        let rtc_control = crate::pac::RTC_CNTL::PTR;
+        let rtc_control = unsafe { &*rtc_control };
+
+        match core {
+            Cpu::ProCpu => {
+                rtc_control
+                    .sw_cpu_stall
+                    .modify(|_, w| unsafe { w.sw_stall_procpu_c1().bits(0) });
+                rtc_control
+                    .options0
+                    .modify(|_, w| unsafe { w.sw_stall_procpu_c0().bits(0) });
+            }
+            Cpu::AppCpu => {
+                rtc_control
+                    .sw_cpu_stall
+                    .modify(|_, w| unsafe { w.sw_stall_appcpu_c1().bits(0) });
+                rtc_control
+                    .options0
+                    .modify(|_, w| unsafe { w.sw_stall_appcpu_c0().bits(0) });
+            }
+        }
+    }
+
+    unsafe fn start_core1_init() -> ! {
+        extern "C" {
+            static mut _stack_end_cpu1: u32;
+        }
+
+        // disables interrupts
+        xtensa_lx::interrupt::set_mask(0);
+
+        // reset cycle compare registers
+        xtensa_lx::timer::set_ccompare0(0);
+        xtensa_lx::timer::set_ccompare1(0);
+        xtensa_lx::timer::set_ccompare2(0);
+
+        // set stack pointer to end of memory: no need to retain stack up to this point
+        set_stack_pointer(&mut _stack_end_cpu1);
+
+        match START_CORE1_FUNCTION.take() {
+            Some(entry) => (*entry)(),
+            None => panic!("No start function set"),
+        }
+
+        panic!("Return from second core's entry");
+    }
+
+    /// Start the APP (second) core
+    ///
+    /// The second core will start running the closure `entry`.
+    #[must_use]
+    pub fn start_app_core<'a: 'b, 'b>(
+        &mut self,
+        entry: &'a mut (dyn FnMut() -> () + 'a),
+    ) -> Result<AppCoreGuard<'b>, Error> {
+        let system_control = crate::pac::SYSTEM::PTR;
+        let system_control = unsafe { &*system_control };
+
+        if !xtensa_lx::is_debugger_attached()
+            && system_control
+                .core_1_control_0
+                .read()
+                .control_core_1_clkgate_en()
+                .bit_is_set()
+        {
+            return Err(Error::CoreAlreadyRunning);
+        }
+
+        unsafe {
+            let entry_fn: &'static mut (dyn FnMut() -> () + 'static) = core::mem::transmute(entry);
+            START_CORE1_FUNCTION = Some(entry_fn);
+        }
+
+        // TODO there is no boot_addr register in SVD or TRM - ESP-IDF uses a ROM
+        // function so we also have to for now
+        const ETS_SET_APPCPU_BOOT_ADDR: usize = 0x40000720;
+        unsafe {
+            let ets_set_appcpu_boot_addr: unsafe extern "C" fn(u32) =
+                core::mem::transmute(ETS_SET_APPCPU_BOOT_ADDR);
+            ets_set_appcpu_boot_addr(Self::start_core1_init as *const u32 as u32);
+        };
+
+        system_control
+            .core_1_control_0
+            .modify(|_, w| w.control_core_1_clkgate_en().set_bit());
+        system_control
+            .core_1_control_0
+            .modify(|_, w| w.control_core_1_runstall().clear_bit());
+        system_control
+            .core_1_control_0
+            .modify(|_, w| w.control_core_1_reseting().set_bit());
+        system_control
+            .core_1_control_0
+            .modify(|_, w| w.control_core_1_reseting().clear_bit());
+
+        self.unpark_core(Cpu::AppCpu);
+
+        Ok(AppCoreGuard {
+            phantom: PhantomData::default(),
+        })
+    }
+}

esp-hal-common/src/cpu_control/none.rs

@@ -0,0 +1 @@
+

esp-hal-common/src/lib.rs

@@ -73,6 +73,12 @@ pub mod system;
 
 pub mod analog;
 
+#[cfg_attr(feature = "esp32", path = "cpu_control/esp32.rs")]
+#[cfg_attr(feature = "esp32c3", path = "cpu_control/none.rs")]
+#[cfg_attr(feature = "esp32s2", path = "cpu_control/none.rs")]
+#[cfg_attr(feature = "esp32s3", path = "cpu_control/esp32s3.rs")]
+pub mod cpu_control;
+
 /// Enumeration of CPU cores
 /// The actual number of available cores depends on the target.
 pub enum Cpu {

esp-hal-common/src/system.rs

@@ -75,11 +75,17 @@ pub struct SystemClockControl {
     _private: (),
 }
 
+/// Controls the configuration of the chip's clocks.
+pub struct CpuControl {
+    _private: (),
+}
+
 /// The SYSTEM/DPORT splitted into it's different logical parts.
 pub struct SystemParts {
     _private: (),
     pub peripheral_clock_control: PeripheralClockControl,
     pub clock_control: SystemClockControl,
+    pub cpu_control: CpuControl,
 }
 
 /// Extension trait to split a SYSTEM/DPORT peripheral in independent logical
@@ -99,6 +105,7 @@ impl SystemExt for SystemPeripheral {
             _private: (),
             peripheral_clock_control: PeripheralClockControl { _private: () },
             clock_control: SystemClockControl { _private: () },
+            cpu_control: CpuControl { _private: () },
         }
     }
 }

esp32-hal/Cargo.toml

@@ -39,6 +39,7 @@ embedded-graphics = "0.7"
 panic-halt        = "0.2"
 ssd1306           = "0.7"
 smart-leds        = "0.3"
+esp-println       = { version = "0.1.0", features = ["esp32"] }
 
 [features]
 default   = ["rt"]

esp32-hal/examples/multicore.rs

@@ -0,0 +1,70 @@
+#![no_std]
+#![no_main]
+
+use core::sync::atomic::{AtomicI32, Ordering};
+
+use esp32_hal::{
+    clock::ClockControl,
+    pac::{Peripherals, TIMG1},
+    prelude::*,
+    CpuControl,
+    RtcCntl,
+    Timer,
+};
+use esp_println::println;
+use nb::block;
+use panic_halt as _;
+use xtensa_lx::mutex::Mutex;
+use xtensa_lx_rt::entry;
+
+#[entry]
+fn main() -> ! {
+    _main();
+}
+
+fn _main() -> ! {
+    let peripherals = Peripherals::take().unwrap();
+    let system = peripherals.DPORT.split();
+    let clocks = ClockControl::boot_defaults(system.clock_control).freeze();
+
+    let mut timer0 = Timer::new(peripherals.TIMG0, clocks.apb_clock);
+    let mut timer1 = Timer::new(peripherals.TIMG1, clocks.apb_clock);
+    let mut rtc_cntl = RtcCntl::new(peripherals.RTC_CNTL);
+
+    // Disable MWDT and RWDT (Watchdog) flash boot protection
+    timer0.disable();
+    timer1.disable();
+    rtc_cntl.set_wdt_global_enable(false);
+
+    timer0.start(1u64.secs());
+    timer1.start(500u64.millis());
+
+    let counter = xtensa_lx::mutex::SpinLockMutex::new(AtomicI32::new(0));
+
+    let mut cpu_control = CpuControl::new(system.cpu_control);
+    let mut cpu1_fnctn = || {
+        cpu1_task(&mut timer1, &counter);
+    };
+    let _guard = cpu_control.start_app_core(&mut cpu1_fnctn).unwrap();
+
+    loop {
+        block!(timer0.wait()).unwrap();
+
+        let count = (&counter).lock(|counter| counter.load(Ordering::Relaxed));
+        println!("Hello World - Core 0! Counter is {}", count);
+    }
+}
+
+fn cpu1_task(timer: &mut Timer<TIMG1>, counter: &xtensa_lx::mutex::SpinLockMutex<AtomicI32>) -> ! {
+    println!("Hello World - Core 1!");
+    loop {
+        block!(timer.wait()).unwrap();
+
+        (&*counter).lock(|counter| {
+            counter.store(
+                counter.load(Ordering::Relaxed).wrapping_add(1),
+                Ordering::Relaxed,
+            );
+        });
+    }
+}

esp32-hal/src/lib.rs

@@ -3,6 +3,7 @@
 pub use embedded_hal as ehal;
 pub use esp_hal_common::{
     clock,
+    cpu_control::CpuControl,
     efuse,
     gpio as gpio_types,
     i2c,

esp32s3-hal/Cargo.toml

@@ -39,6 +39,7 @@ embedded-graphics = "0.7"
 panic-halt        = "0.2"
 ssd1306           = "0.7"
 smart-leds        = "0.3"
+esp-println       = { version = "0.1.0", features = ["esp32s3"] }
 
 [features]
 default = ["rt"]

esp32s3-hal/examples/multicore.rs

@@ -0,0 +1,66 @@
+#![no_std]
+#![no_main]
+
+use core::sync::atomic::{AtomicI32, Ordering};
+
+use esp32s3_hal::{
+    clock::ClockControl,
+    pac::{Peripherals, TIMG1},
+    prelude::*,
+    CpuControl,
+    RtcCntl,
+    Timer,
+};
+use esp_println::println;
+use nb::block;
+use panic_halt as _;
+use xtensa_lx::mutex::Mutex;
+use xtensa_lx_rt::entry;
+
+#[entry]
+fn main() -> ! {
+    let peripherals = Peripherals::take().unwrap();
+    let system = peripherals.SYSTEM.split();
+    let clocks = ClockControl::boot_defaults(system.clock_control).freeze();
+
+    let mut timer0 = Timer::new(peripherals.TIMG0, clocks.apb_clock);
+    let mut timer1 = Timer::new(peripherals.TIMG1, clocks.apb_clock);
+    let mut rtc_cntl = RtcCntl::new(peripherals.RTC_CNTL);
+
+    // Disable MWDT and RWDT (Watchdog) flash boot protection
+    timer0.disable();
+    timer1.disable();
+    rtc_cntl.set_wdt_global_enable(false);
+
+    timer0.start(1u64.secs());
+    timer1.start(500u64.millis());
+
+    let counter = xtensa_lx::mutex::SpinLockMutex::new(AtomicI32::new(0));
+
+    let mut cpu_control = CpuControl::new(system.cpu_control);
+    let mut cpu1_fnctn = || {
+        cpu1_task(&mut timer1, &counter);
+    };
+    let _guard = cpu_control.start_app_core(&mut cpu1_fnctn).unwrap();
+
+    loop {
+        block!(timer0.wait()).unwrap();
+
+        let count = (&counter).lock(|counter| counter.load(Ordering::Relaxed));
+        println!("Hello World - Core 0! Counter is {}", count);
+    }
+}
+
+fn cpu1_task(timer: &mut Timer<TIMG1>, counter: &xtensa_lx::mutex::SpinLockMutex<AtomicI32>) -> ! {
+    println!("Hello World - Core 1!");
+    loop {
+        block!(timer.wait()).unwrap();
+
+        (&*counter).lock(|counter| {
+            counter.store(
+                counter.load(Ordering::Relaxed).wrapping_add(1),
+                Ordering::Relaxed,
+            );
+        });
+    }
+}

esp32s3-hal/src/lib.rs

@@ -3,6 +3,7 @@
 pub use embedded_hal as ehal;
 pub use esp_hal_common::{
     clock,
+    cpu_control::CpuControl,
     efuse,
     gpio as gpio_types,
     i2c,