diff --git a/rp235x-hal-examples/riscv_examples.txt b/rp235x-hal-examples/riscv_examples.txt index 0800802a5..7360604e8 100644 --- a/rp235x-hal-examples/riscv_examples.txt +++ b/rp235x-hal-examples/riscv_examples.txt @@ -9,7 +9,10 @@ block_loop dht11 gpio_dyn_pin_array gpio_in_out +gpio_irq_example i2c +i2c_async +i2c_async_cancelled lcd_display mem_to_mem_dma pio_blink @@ -17,8 +20,10 @@ pio_dma pio_proc_blink pio_side_set pio_synchronized +powman_test pwm_blink pwm_blink_embedded_hal_1 +pwm_irq_input rom_funcs rosc_as_system_clock spi diff --git a/rp235x-hal-examples/rp235x_riscv.x b/rp235x-hal-examples/rp235x_riscv.x index b97a0ad01..bc069a534 100644 --- a/rp235x-hal-examples/rp235x_riscv.x +++ b/rp235x-hal-examples/rp235x_riscv.x @@ -80,6 +80,51 @@ PROVIDE(__pre_init = default_pre_init); /* A PAC/HAL defined routine that should initialize custom interrupt controller if needed. */ PROVIDE(_setup_interrupts = default_setup_interrupts); +PROVIDE(TIMER0_IRQ_0 = DefaultIrqHandler); +PROVIDE(TIMER0_IRQ_1 = DefaultIrqHandler); +PROVIDE(TIMER0_IRQ_2 = DefaultIrqHandler); +PROVIDE(TIMER0_IRQ_3 = DefaultIrqHandler); +PROVIDE(TIMER1_IRQ_0 = DefaultIrqHandler); +PROVIDE(TIMER1_IRQ_1 = DefaultIrqHandler); +PROVIDE(TIMER1_IRQ_2 = DefaultIrqHandler); +PROVIDE(TIMER1_IRQ_3 = DefaultIrqHandler); +PROVIDE(PWM_IRQ_WRAP_0 = DefaultIrqHandler); +PROVIDE(PWM_IRQ_WRAP_1 = DefaultIrqHandler); +PROVIDE(DMA_IRQ_0 = DefaultIrqHandler); +PROVIDE(DMA_IRQ_1 = DefaultIrqHandler); +PROVIDE(DMA_IRQ_2 = DefaultIrqHandler); +PROVIDE(DMA_IRQ_3 = DefaultIrqHandler); +PROVIDE(USBCTRL_IRQ = DefaultIrqHandler); +PROVIDE(PIO0_IRQ_0 = DefaultIrqHandler); +PROVIDE(PIO0_IRQ_1 = DefaultIrqHandler); +PROVIDE(PIO1_IRQ_0 = DefaultIrqHandler); +PROVIDE(PIO1_IRQ_1 = DefaultIrqHandler); +PROVIDE(PIO2_IRQ_0 = DefaultIrqHandler); +PROVIDE(PIO2_IRQ_1 = DefaultIrqHandler); +PROVIDE(IO_IRQ_BANK0 = DefaultIrqHandler); +PROVIDE(IO_IRQ_BANK0_NS = DefaultIrqHandler); +PROVIDE(IO_IRQ_QSPI = DefaultIrqHandler); +PROVIDE(IO_IRQ_QSPI_NS = DefaultIrqHandler); +PROVIDE(SIO_IRQ_FIFO = DefaultIrqHandler); +PROVIDE(SIO_IRQ_BELL = DefaultIrqHandler); +PROVIDE(SIO_IRQ_FIFO_NS = DefaultIrqHandler); +PROVIDE(SIO_IRQ_BELL_NS = DefaultIrqHandler); +PROVIDE(SIO_IRQ_MTIMECMP = DefaultIrqHandler); +PROVIDE(CLOCKS_IRQ = DefaultIrqHandler); +PROVIDE(SPI0_IRQ = DefaultIrqHandler); +PROVIDE(SPI1_IRQ = DefaultIrqHandler); +PROVIDE(UART0_IRQ = DefaultIrqHandler); +PROVIDE(UART1_IRQ = DefaultIrqHandler); +PROVIDE(ADC_IRQ_FIFO = DefaultIrqHandler); +PROVIDE(I2C0_IRQ = DefaultIrqHandler); +PROVIDE(I2C1_IRQ = DefaultIrqHandler); +PROVIDE(OTP_IRQ = DefaultIrqHandler); +PROVIDE(TRNG_IRQ = DefaultIrqHandler); +PROVIDE(PLL_SYS_IRQ = DefaultIrqHandler); +PROVIDE(PLL_USB_IRQ = DefaultIrqHandler); +PROVIDE(POWMAN_IRQ_POW = DefaultIrqHandler); +PROVIDE(POWMAN_IRQ_TIMER = DefaultIrqHandler); + /* # Multi-processing hook function fn _mp_hook() -> bool; diff --git a/rp235x-hal-examples/src/bin/gpio_irq_example.rs b/rp235x-hal-examples/src/bin/gpio_irq_example.rs index 04a457405..111c85b3f 100644 --- a/rp235x-hal-examples/src/bin/gpio_irq_example.rs +++ b/rp235x-hal-examples/src/bin/gpio_irq_example.rs @@ -29,20 +29,14 @@ use panic_halt as _; // Alias for our HAL crate use rp235x_hal as hal; -// Some things we need +// Some traits we need use embedded_hal::digital::StatefulOutputPin; -// Our interrupt macro -use hal::pac::interrupt; - -// Some short-cuts to useful types +// Some more helpful aliases use core::cell::RefCell; use critical_section::Mutex; use hal::gpio; -// The GPIO interrupt type we're going to generate -use gpio::Interrupt::EdgeLow; - /// Tell the Boot ROM about our application #[link_section = ".start_block"] #[used] @@ -70,7 +64,7 @@ type LedAndButton = (LedPin, ButtonPin); /// This how we transfer our Led and Button pins into the Interrupt Handler. /// We'll have the option hold both using the LedAndButton type. /// This will make it a bit easier to unpack them later. -static GLOBAL_PINS: Mutex>> = Mutex::new(RefCell::new(None)); +static GLOBAL_STATE: Mutex>> = Mutex::new(RefCell::new(None)); /// Entry point to our bare-metal application. /// @@ -120,20 +114,25 @@ fn main() -> ! { // Trigger on the 'falling edge' of the input pin. // This will happen as the button is being pressed - in_pin.set_interrupt_enabled(EdgeLow, true); + in_pin.set_interrupt_enabled(gpio::Interrupt::EdgeLow, true); // Give away our pins by moving them into the `GLOBAL_PINS` variable. // We won't need to access them in the main thread again critical_section::with(|cs| { - GLOBAL_PINS.borrow(cs).replace(Some((led, in_pin))); + GLOBAL_STATE.borrow(cs).replace(Some((led, in_pin))); }); - // Unmask the IO_BANK0 IRQ so that the NVIC interrupt controller - // will jump to the interrupt function when the interrupt occurs. - // We do this last so that the interrupt can't go off while - // it is in the middle of being configured + // Unmask the IRQ for I/O Bank 0 so that the RP2350's interrupt controller + // (NVIC in Arm mode, or Xh3irq in RISC-V mode) will jump to the interrupt + // function when the interrupt occurs. We do this last so that the interrupt + // can't go off while it is in the middle of being configured + unsafe { + hal::arch::interrupt_unmask(hal::pac::Interrupt::IO_IRQ_BANK0); + } + + // Enable interrupts on this core unsafe { - cortex_m::peripheral::NVIC::unmask(hal::pac::Interrupt::IO_IRQ_BANK0); + hal::arch::interrupt_enable(); } loop { @@ -142,38 +141,39 @@ fn main() -> ! { } } -#[allow(static_mut_refs)] // See https://github.com/rust-embedded/cortex-m/pull/561 -#[interrupt] +/// This is the interrupt handler that fires when GPIO Bank 0 detects an event +/// (like an edge). +/// +/// We give it an unmangled name so that it replaces the default (empty) +/// handler. These handlers are referred to by name from the Interrupt Vector +/// Table created by cortex-m-rt. +#[allow(non_snake_case)] +#[no_mangle] fn IO_IRQ_BANK0() { - // The `#[interrupt]` attribute covertly converts this to `&'static mut Option` - static mut LED_AND_BUTTON: Option = None; - - // This is one-time lazy initialisation. We steal the variables given to us - // via `GLOBAL_PINS`. - if LED_AND_BUTTON.is_none() { - critical_section::with(|cs| { - *LED_AND_BUTTON = GLOBAL_PINS.borrow(cs).take(); - }); - } - - // Need to check if our Option contains our pins - if let Some(gpios) = LED_AND_BUTTON { - // borrow led and button by *destructuring* the tuple - // these will be of type `&mut LedPin` and `&mut ButtonPin`, so we don't have - // to move them back into the static after we use them - let (led, button) = gpios; - // Check if the interrupt source is from the pushbutton going from high-to-low. - // Note: this will always be true in this example, as that is the only enabled GPIO interrupt source - if button.interrupt_status(EdgeLow) { - // toggle can't fail, but the embedded-hal traits always allow for it - // we can discard the return value by assigning it to an unnamed variable - let _ = led.toggle(); - - // Our interrupt doesn't clear itself. - // Do that now so we don't immediately jump back to this interrupt handler. - button.clear_interrupt(EdgeLow); + // Enter a critical section to ensure this code cannot be concurrently + // executed on the other core. This also protects us if the main thread + // decides to execute this function (which it shouldn't, but we can't stop + // them if they wanted to). + critical_section::with(|cs| { + // Grab a mutable reference to the global state, using the CS token as + // proof we have turned off interrupts. Performs a run-time borrow check + // of the RefCell to ensure no-one else is currently borrowing it (and + // they shouldn't, because we're in a critical section right now). + let mut maybe_state = GLOBAL_STATE.borrow_ref_mut(cs); + // Need to check if our Option contains our pins + if let Some((led, button)) = maybe_state.as_mut() { + // Check if the interrupt source is from the pushbutton going from high-to-low. + // Note: this will always be true in this example, as that is the only enabled GPIO interrupt source + if button.interrupt_status(gpio::Interrupt::EdgeLow) { + // toggle can't fail, but the embedded-hal traits always allow for it + // we can discard the return value by assigning it to an unnamed variable + let _ = led.toggle(); + // Our interrupt doesn't clear itself. + // Do that now so we don't immediately jump back to this interrupt handler. + button.clear_interrupt(gpio::Interrupt::EdgeLow); + } } - } + }); } /// Program metadata for `picotool info` diff --git a/rp235x-hal-examples/src/bin/i2c_async.rs b/rp235x-hal-examples/src/bin/i2c_async.rs index 291cc1439..0c76800ea 100644 --- a/rp235x-hal-examples/src/bin/i2c_async.rs +++ b/rp235x-hal-examples/src/bin/i2c_async.rs @@ -23,7 +23,6 @@ use hal::{ gpio::bank0::{Gpio20, Gpio21}, gpio::{FunctionI2C, Pin, PullUp}, i2c::Controller, - pac::interrupt, Clock, I2C, }; @@ -42,7 +41,8 @@ pub static IMAGE_DEF: hal::block::ImageDef = hal::block::ImageDef::secure_exe(); const XTAL_FREQ_HZ: u32 = 12_000_000u32; /// Bind the interrupt handler with the peripheral -#[interrupt] +#[no_mangle] +#[allow(non_snake_case)] unsafe fn I2C0_IRQ() { use hal::async_utils::AsyncPeripheral; I2C::::on_interrupt(); @@ -97,14 +97,16 @@ async fn demo() { clocks.system_clock.freq(), ); - // Unmask the interrupt in the NVIC to let the core wake up & enter the interrupt handler. - // Each core has its own NVIC so these needs to executed from the core where the IRQ are - // expected. + // Unmask the IRQ for I2C0. We do this after the driver init so that the + // interrupt can't go off while it is in the middle of being configured unsafe { - cortex_m::peripheral::NVIC::unpend(hal::pac::Interrupt::I2C0_IRQ); - cortex_m::peripheral::NVIC::unmask(hal::pac::Interrupt::I2C0_IRQ); + hal::arch::interrupt_unmask(hal::pac::Interrupt::I2C0_IRQ); } + // Enable interrupts on this core + unsafe { + hal::arch::interrupt_enable(); + } // Asynchronously write three bytes to the I²C device with 7-bit address 0x2C i2c.write(0x76u8, &[1, 2, 3]).await.unwrap(); diff --git a/rp235x-hal-examples/src/bin/i2c_async_cancelled.rs b/rp235x-hal-examples/src/bin/i2c_async_cancelled.rs index f85240e9c..8ab45316f 100644 --- a/rp235x-hal-examples/src/bin/i2c_async_cancelled.rs +++ b/rp235x-hal-examples/src/bin/i2c_async_cancelled.rs @@ -27,7 +27,6 @@ use hal::{ gpio::bank0::{Gpio20, Gpio21}, gpio::{FunctionI2C, Pin, PullUp}, i2c::Controller, - pac::interrupt, Clock, I2C, }; @@ -44,7 +43,8 @@ pub static IMAGE_DEF: hal::block::ImageDef = hal::block::ImageDef::secure_exe(); /// Adjust if your board has a different frequency const XTAL_FREQ_HZ: u32 = 12_000_000u32; -#[interrupt] +#[no_mangle] +#[allow(non_snake_case)] unsafe fn I2C0_IRQ() { use hal::async_utils::AsyncPeripheral; I2C::::on_interrupt(); @@ -99,10 +99,15 @@ async fn demo() { clocks.system_clock.freq(), ); - // Unmask the interrupt in the NVIC to let the core wake up & enter the interrupt handler. + // Unmask the IRQ for I2C0. We do this after the driver init so that the + // interrupt can't go off while it is in the middle of being configured unsafe { - cortex_m::peripheral::NVIC::unpend(hal::pac::Interrupt::I2C0_IRQ); - cortex_m::peripheral::NVIC::unmask(hal::pac::Interrupt::I2C0_IRQ); + hal::arch::interrupt_unmask(hal::pac::Interrupt::I2C0_IRQ); + } + + // Enable interrupts on this core + unsafe { + hal::arch::interrupt_enable(); } let mut cnt = 0; diff --git a/rp235x-hal-examples/src/bin/powman_test.rs b/rp235x-hal-examples/src/bin/powman_test.rs index 80a517ee0..43c2f98e9 100644 --- a/rp235x-hal-examples/src/bin/powman_test.rs +++ b/rp235x-hal-examples/src/bin/powman_test.rs @@ -13,9 +13,6 @@ use rp235x_hal::{ uart::{DataBits, StopBits, UartConfig, UartPeripheral}, }; -use cortex_m_rt::exception; -use pac::interrupt; - // Some traits we need use core::fmt::Write; use hal::fugit::RateExtU32; @@ -67,7 +64,7 @@ impl GlobalUart { /// Entry point to our bare-metal application. /// -/// The `#[hal::entry]` macro ensures the Cortex-M start-up code calls this function +/// The `#[hal::entry]` macro ensures the start-up code calls this function /// as soon as all global variables and the spinlock are initialised. /// /// The function configures the rp235x peripherals, then writes to the UART in @@ -76,11 +73,6 @@ impl GlobalUart { fn main() -> ! { // Grab our singleton objects let mut pac = pac::Peripherals::take().unwrap(); - let mut cp = cortex_m::Peripherals::take().unwrap(); - - // Enable the cycle counter - cp.DCB.enable_trace(); - cp.DWT.enable_cycle_counter(); // Set up the watchdog driver - needed by the clock setup code let mut watchdog = hal::Watchdog::new(pac.WATCHDOG); @@ -99,6 +91,8 @@ fn main() -> ! { // The single-cycle I/O block controls our GPIO pins let sio = hal::Sio::new(pac.SIO); + let mut mtimer = sio.machine_timer; + mtimer.set_enabled(true); // Set the pins to their default state let pins = gpio::Pins::new( @@ -130,30 +124,38 @@ fn main() -> ! { print_aot_status(&mut powman); _ = writeln!(&GLOBAL_UART, "AOT time: 0x{:016x}", powman.aot_get_time()); + // Unmask the IRQ for POWMAN's Timer. We do this after the driver init so + // that the interrupt can't go off while it is in the middle of being + // configured + unsafe { + hal::arch::interrupt_unmask(hal::pac::Interrupt::POWMAN_IRQ_TIMER); + } + + // Enable interrupts on this core unsafe { - cortex_m::peripheral::NVIC::unmask(pac::Interrupt::POWMAN_IRQ_TIMER); + hal::arch::interrupt_enable(); } _ = writeln!(&GLOBAL_UART, "Starting AOT..."); powman.aot_start(); // we don't know what oscillator we're on, so give it time to start whatever it is - cortex_m::asm::delay(150_000); + hal::arch::delay(150_000); print_aot_status(&mut powman); rollover_test(&mut powman); - loop_test(&mut powman); + loop_test(&mut powman, &mtimer); alarm_test(&mut powman); let source = AotClockSource::Xosc(FractionalFrequency::from_hz(12_000_000)); _ = writeln!(&GLOBAL_UART, "Switching AOT to {}", source); powman.aot_set_clock(source).expect("selecting XOSC"); print_aot_status(&mut powman); - loop_test(&mut powman); + loop_test(&mut powman, &mtimer); let source = AotClockSource::Lposc(FractionalFrequency::from_hz(32768)); _ = writeln!(&GLOBAL_UART, "Switching AOT to {}", source); powman.aot_set_clock(source).expect("selecting LPOSC"); print_aot_status(&mut powman); - loop_test(&mut powman); + loop_test(&mut powman, &mtimer); _ = writeln!(&GLOBAL_UART, "Rebooting now"); @@ -202,7 +204,7 @@ fn rollover_test(powman: &mut Powman) { } /// In this function, we see how long it takes to pass a certain number of ticks. -fn loop_test(powman: &mut Powman) { +fn loop_test(powman: &mut Powman, mtimer: &hal::sio::MachineTimer) { let start_loop = 0; let end_loop = 2_000; // 2 seconds _ = writeln!( @@ -214,24 +216,24 @@ fn loop_test(powman: &mut Powman) { powman.aot_set_time(start_loop); powman.aot_start(); - let start_clocks = cortex_m::peripheral::DWT::cycle_count(); + let start_clocks = mtimer.read(); loop { let now = powman.aot_get_time(); if now == end_loop { break; } } - let end_clocks = cortex_m::peripheral::DWT::cycle_count(); - // Compare our AOT against our CPU clock speed - let delta_clocks = end_clocks.wrapping_sub(start_clocks) as u64; + let end_clocks = mtimer.read(); + // Compare our AOT against our Machine Timer + let delta_clocks = end_clocks.wrapping_sub(start_clocks); let delta_ticks = end_loop - start_loop; let cycles_per_tick = delta_clocks / delta_ticks; - // Assume we're running at 150 MHz - let ms_per_tick = (cycles_per_tick as f32 * 1000.0) / 150_000_000.0; + // Assume we're running at 1 MHz MTimer + let ms_per_tick = (cycles_per_tick as f32 * 1000.0) / 1_000_000.0; let percent = ((ms_per_tick - 1.0) / 1.0) * 100.0; _ = writeln!( &GLOBAL_UART, - "Loop complete ... {delta_ticks} ticks in {delta_clocks} CPU clock cycles = {cycles_per_tick} cycles/tick ~= {ms_per_tick} ms/tick ({percent:.3}%)", + "Loop complete ... {delta_ticks} ticks in {delta_clocks} MTimer cycles = {cycles_per_tick} cycles/tick ~= {ms_per_tick} ms/tick ({percent:.3}%)", ) ; } @@ -258,8 +260,9 @@ fn alarm_test(powman: &mut Powman) { &GLOBAL_UART, "Sleeping until alarm (* = wakeup, ! = POWMAN interrupt)...", ); + while !powman.aot_alarm_ringing() { - cortex_m::asm::wfe(); + hal::arch::wfe(); _ = write!(&GLOBAL_UART, "*",); } @@ -278,25 +281,12 @@ fn alarm_test(powman: &mut Powman) { _ = writeln!(&GLOBAL_UART, "Alarm cleared OK"); } -#[interrupt] +#[no_mangle] #[allow(non_snake_case)] fn POWMAN_IRQ_TIMER() { Powman::static_aot_alarm_interrupt_disable(); _ = write!(&GLOBAL_UART, "!"); - cortex_m::asm::sev(); -} - -#[exception] -unsafe fn HardFault(ef: &cortex_m_rt::ExceptionFrame) -> ! { - let _ = writeln!(&GLOBAL_UART, "HARD FAULT:\n{:#?}", ef); - - hal::reboot::reboot( - hal::reboot::RebootKind::BootSel { - msd_disabled: false, - picoboot_disabled: false, - }, - hal::reboot::RebootArch::Normal, - ); + hal::arch::sev(); } #[panic_handler] diff --git a/rp235x-hal-examples/src/bin/pwm_irq_input.rs b/rp235x-hal-examples/src/bin/pwm_irq_input.rs index 6a411b3f2..72d114d08 100644 --- a/rp235x-hal-examples/src/bin/pwm_irq_input.rs +++ b/rp235x-hal-examples/src/bin/pwm_irq_input.rs @@ -19,19 +19,14 @@ use panic_halt as _; // Alias for our HAL crate use rp235x_hal as hal; -// Some things we need +// Some traits we need use embedded_hal::digital::OutputPin; -// Our interrupt macro -use hal::pac::interrupt; - -// Shorter alias for gpio and pwm modules -use hal::gpio; -use hal::pwm; - -// Some short-cuts to useful types for sharing data with the interrupt handlers +// Some more helpful aliases use core::cell::RefCell; use critical_section::Mutex; +use hal::gpio; +use hal::pwm; /// Tell the Boot ROM about our application #[link_section = ".start_block"] @@ -71,7 +66,7 @@ type LedInputAndPwm = (LedPin, InputPwmPin, PwmSlice); /// This how we transfer our LED pin, input pin and PWM slice into the Interrupt Handler. /// We'll have the option hold both using the LedAndInput type. /// This will make it a bit easier to unpack them later. -static GLOBAL_PINS: Mutex>> = Mutex::new(RefCell::new(None)); +static GLOBAL_STATE: Mutex>> = Mutex::new(RefCell::new(None)); /// Entry point to our bare-metal application. /// @@ -140,15 +135,20 @@ fn main() -> ! { // Give away our pins by moving them into the `GLOBAL_PINS` variable. // We won't need to access them in the main thread again critical_section::with(|cs| { - GLOBAL_PINS.borrow(cs).replace(Some((led, input_pin, pwm))); + GLOBAL_STATE.borrow(cs).replace(Some((led, input_pin, pwm))); }); - // Unmask the IO_BANK0 IRQ so that the NVIC interrupt controller - // will jump to the interrupt function when the interrupt occurs. - // We do this last so that the interrupt can't go off while - // it is in the middle of being configured + // Unmask the IRQ for I/O Bank 0 so that the RP2350's interrupt controller + // (NVIC in Arm mode, or Xh3irq in RISC-V mode) will jump to the interrupt + // function when the interrupt occurs. We do this last so that the interrupt + // can't go off while it is in the middle of being configured unsafe { - cortex_m::peripheral::NVIC::unmask(hal::pac::Interrupt::IO_IRQ_BANK0); + hal::arch::interrupt_unmask(hal::pac::Interrupt::IO_IRQ_BANK0); + } + + // Enable interrupts on this core + unsafe { + hal::arch::interrupt_enable(); } loop { @@ -157,56 +157,59 @@ fn main() -> ! { } } -#[allow(static_mut_refs)] // See https://github.com/rust-embedded/cortex-m/pull/561 -#[interrupt] +/// This is the interrupt handler that fires when GPIO Bank 0 detects an event +/// (like an edge). +/// +/// We give it an unmangled name so that it replaces the default (empty) +/// handler. These handlers are referred to by name from the Interrupt Vector +/// Table created by cortex-m-rt. +#[allow(non_snake_case)] +#[no_mangle] fn IO_IRQ_BANK0() { - // The `#[interrupt]` attribute covertly converts this to `&'static mut Option` - static mut LED_INPUT_AND_PWM: Option = None; - - // This is one-time lazy initialisation. We steal the variables given to us - // via `GLOBAL_PINS`. - if LED_INPUT_AND_PWM.is_none() { - critical_section::with(|cs| { - *LED_INPUT_AND_PWM = GLOBAL_PINS.borrow(cs).take(); - }); - } - - // Need to check if our Option contains our pins and pwm slice - // borrow led, input and pwm by *destructuring* the tuple - // these will be of type `&mut LedPin`, `&mut InputPwmPin` and `&mut PwmSlice`, so we - // don't have to move them back into the static after we use them - if let Some((led, input, pwm)) = LED_INPUT_AND_PWM { - // Check if the interrupt source is from the input pin going from high-to-low. - // Note: this will always be true in this example, as that is the only enabled GPIO interrupt source - if input.interrupt_status(gpio::Interrupt::EdgeLow) { - // Read the width of the last pulse from the PWM Slice counter - let pulse_width_us = pwm.get_counter(); - - // if the PWM signal indicates low, turn off the LED - if pulse_width_us < LOW_US { - // set_low can't fail, but the embedded-hal traits always allow for it - // we can discard the Result - let _ = led.set_low(); - } - // if the PWM signal indicates high, turn on the LED - else if pulse_width_us > HIGH_US { - // set_high can't fail, but the embedded-hal traits always allow for it - // we can discard the Result - let _ = led.set_high(); + // Enter a critical section to ensure this code cannot be concurrently + // executed on the other core. This also protects us if the main thread + // decides to execute this function (which it shouldn't, but we can't stop + // them if they wanted to). + critical_section::with(|cs| { + // Grab a mutable reference to the global state, using the CS token as + // proof we have turned off interrupts. Performs a run-time borrow check + // of the RefCell to ensure no-one else is currently borrowing it (and + // they shouldn't, because we're in a critical section right now). + let mut maybe_state = GLOBAL_STATE.borrow_ref_mut(cs); + // Need to check if our Option contains our pins and pwm slice + if let Some((led, input, pwm)) = maybe_state.as_mut() { + // Check if the interrupt source is from the input pin going from high-to-low. + // Note: this will always be true in this example, as that is the only enabled GPIO interrupt source + if input.interrupt_status(gpio::Interrupt::EdgeLow) { + // Read the width of the last pulse from the PWM Slice counter + let pulse_width_us = pwm.get_counter(); + + // if the PWM signal indicates low, turn off the LED + if pulse_width_us < LOW_US { + // set_low can't fail, but the embedded-hal traits always allow for it + // we can discard the Result + let _ = led.set_low(); + } + // if the PWM signal indicates high, turn on the LED + else if pulse_width_us > HIGH_US { + // set_high can't fail, but the embedded-hal traits always allow for it + // we can discard the Result + let _ = led.set_high(); + } + + // If the PWM signal was in the dead-zone between LOW and HIGH, don't change the LED's + // state. The dead-zone avoids the LED flickering rapidly when receiving a signal close + // to the mid-point, 1500 us in this case. + + // Reset the pwm counter back to 0, ready for the next pulse + pwm.set_counter(0); + + // Our interrupt doesn't clear itself. + // Do that now so we don't immediately jump back to this interrupt handler. + input.clear_interrupt(gpio::Interrupt::EdgeLow); } - - // If the PWM signal was in the dead-zone between LOW and HIGH, don't change the LED's - // state. The dead-zone avoids the LED flickering rapidly when receiving a signal close - // to the mid-point, 1500 us in this case. - - // Reset the pwm counter back to 0, ready for the next pulse - pwm.set_counter(0); - - // Our interrupt doesn't clear itself. - // Do that now so we don't immediately jump back to this interrupt handler. - input.clear_interrupt(gpio::Interrupt::EdgeLow); } - } + }); } /// Program metadata for `picotool info` diff --git a/rp235x-hal/src/arch.rs b/rp235x-hal/src/arch.rs index a87fe9bc6..05aa88d08 100644 --- a/rp235x-hal/src/arch.rs +++ b/rp235x-hal/src/arch.rs @@ -4,42 +4,78 @@ #[cfg(all(target_arch = "arm", target_os = "none"))] mod inner { + #[doc(inline)] pub use cortex_m::asm::{delay, dsb, nop, sev, wfe, wfi}; + + #[doc(inline)] pub use cortex_m::interrupt::{disable as interrupt_disable, enable as interrupt_enable}; - /// Are interrupts current enabled? + /// Are interrupts currently enabled? pub fn interrupts_enabled() -> bool { cortex_m::register::primask::read().is_active() } - /// Run the closure without interrupts + /// Check if an IRQ is pending + pub fn interrrupt_is_pending(irq: rp235x_pac::Interrupt) -> bool { + cortex_m::peripheral::NVIC::is_pending(irq) + } + + /// Enable an RP235x IRQ /// - /// No critical-section token because we haven't blocked the second core - pub fn interrupt_free(f: F) -> T - where - F: FnOnce() -> T, - { - let active = interrupts_enabled(); - if active { - interrupt_disable(); - } - let t = f(); - if active { - unsafe { - interrupt_enable(); - } - } - t + /// # Safety + /// + /// This function is unsafe because it can break mask-based critical + /// sections. Do not call inside a critical section. + pub unsafe fn interrupt_unmask(irq: rp235x_pac::Interrupt) { + unsafe { cortex_m::peripheral::NVIC::unmask(irq) } + } + + /// Disable an RP235x IRQ + pub fn interrupt_mask(irq: rp235x_pac::Interrupt) { + cortex_m::peripheral::NVIC::mask(irq) + } + + /// Check if an RP235x IRQ is enabled + pub fn interrupt_is_enabled(irq: rp235x_pac::Interrupt) -> bool { + cortex_m::peripheral::NVIC::is_enabled(irq) + } + + /// Mark an RP235x IRQ as pending + pub fn interrupt_pend(irq: rp235x_pac::Interrupt) { + cortex_m::peripheral::NVIC::pend(irq) } } #[cfg(all(target_arch = "riscv32", target_os = "none"))] mod inner { + #[doc(inline)] pub use riscv::asm::{delay, nop, wfi}; - pub use riscv::interrupt::machine::{ - disable as interrupt_disable, enable as interrupt_enable, free as interrupt_free, + + #[doc(inline)] + pub use riscv::interrupt::machine::disable as interrupt_disable; + + #[doc(inline)] + pub use crate::xh3irq::{ + is_enabled as interrupt_is_enabled, is_pending as interrrupt_is_pending, + mask as interrupt_mask, pend as interrupt_pend, unmask as interrupt_unmask, }; + /// Enable interrupts + /// + /// Enable the Machine External interrupt as well as the global interrupt + /// flag. + /// + /// # Safety + /// + /// Do not call from inside a critical section. + #[inline(always)] + pub unsafe fn interrupt_enable() { + unsafe { + riscv::register::mie::set_mext(); + riscv::interrupt::machine::enable(); + } + } + /// Send Event #[inline(always)] pub fn sev() { @@ -50,18 +86,10 @@ mod inner { } /// Wait for Event - /// - /// This is the interrupt-safe version of WFI. pub fn wfe() { - let active = interrupts_enabled(); - if active { - interrupt_disable(); - } - wfi(); - if active { - unsafe { - interrupt_enable(); - } + unsafe { + // This is how h3.block is encoded. + core::arch::asm!("slt x0, x0, x0"); } } @@ -73,43 +101,475 @@ mod inner { core::sync::atomic::compiler_fence(core::sync::atomic::Ordering::SeqCst); } - /// Are interrupts current enabled? + /// Are interrupts currently enabled? #[inline(always)] pub fn interrupts_enabled() -> bool { riscv::register::mstatus::read().mie() } + + #[no_mangle] + #[allow(non_snake_case)] + fn MachineExternal() { + loop { + let Some(irq) = crate::xh3irq::get_next_interrupt() else { + return; + }; + match irq { + rp235x_pac::Interrupt::TIMER0_IRQ_0 => { + extern "Rust" { + fn TIMER0_IRQ_0(); + } + unsafe { + TIMER0_IRQ_0(); + } + } + rp235x_pac::Interrupt::TIMER0_IRQ_1 => { + extern "Rust" { + fn TIMER0_IRQ_1(); + } + unsafe { + TIMER0_IRQ_1(); + } + } + rp235x_pac::Interrupt::TIMER0_IRQ_2 => { + extern "Rust" { + fn TIMER0_IRQ_2(); + } + unsafe { + TIMER0_IRQ_2(); + } + } + rp235x_pac::Interrupt::TIMER0_IRQ_3 => { + extern "Rust" { + fn TIMER0_IRQ_3(); + } + unsafe { + TIMER0_IRQ_3(); + } + } + rp235x_pac::Interrupt::TIMER1_IRQ_0 => { + extern "Rust" { + fn TIMER1_IRQ_0(); + } + unsafe { + TIMER1_IRQ_0(); + } + } + rp235x_pac::Interrupt::TIMER1_IRQ_1 => { + extern "Rust" { + fn TIMER1_IRQ_1(); + } + unsafe { + TIMER1_IRQ_1(); + } + } + rp235x_pac::Interrupt::TIMER1_IRQ_2 => { + extern "Rust" { + fn TIMER1_IRQ_2(); + } + unsafe { + TIMER1_IRQ_2(); + } + } + rp235x_pac::Interrupt::TIMER1_IRQ_3 => { + extern "Rust" { + fn TIMER1_IRQ_3(); + } + unsafe { + TIMER1_IRQ_3(); + } + } + rp235x_pac::Interrupt::PWM_IRQ_WRAP_0 => { + extern "Rust" { + fn PWM_IRQ_WRAP_0(); + } + unsafe { + PWM_IRQ_WRAP_0(); + } + } + rp235x_pac::Interrupt::PWM_IRQ_WRAP_1 => { + extern "Rust" { + fn PWM_IRQ_WRAP_1(); + } + unsafe { + PWM_IRQ_WRAP_1(); + } + } + rp235x_pac::Interrupt::DMA_IRQ_0 => { + extern "Rust" { + fn DMA_IRQ_0(); + } + unsafe { + DMA_IRQ_0(); + } + } + rp235x_pac::Interrupt::DMA_IRQ_1 => { + extern "Rust" { + fn DMA_IRQ_1(); + } + unsafe { + DMA_IRQ_1(); + } + } + rp235x_pac::Interrupt::DMA_IRQ_2 => { + extern "Rust" { + fn DMA_IRQ_2(); + } + unsafe { + DMA_IRQ_2(); + } + } + rp235x_pac::Interrupt::DMA_IRQ_3 => { + extern "Rust" { + fn DMA_IRQ_3(); + } + unsafe { + DMA_IRQ_3(); + } + } + rp235x_pac::Interrupt::USBCTRL_IRQ => { + extern "Rust" { + fn USBCTRL_IRQ(); + } + unsafe { + USBCTRL_IRQ(); + } + } + rp235x_pac::Interrupt::PIO0_IRQ_0 => { + extern "Rust" { + fn PIO0_IRQ_0(); + } + unsafe { + PIO0_IRQ_0(); + } + } + rp235x_pac::Interrupt::PIO0_IRQ_1 => { + extern "Rust" { + fn PIO0_IRQ_1(); + } + unsafe { + PIO0_IRQ_1(); + } + } + rp235x_pac::Interrupt::PIO1_IRQ_0 => { + extern "Rust" { + fn PIO1_IRQ_0(); + } + unsafe { + PIO1_IRQ_0(); + } + } + rp235x_pac::Interrupt::PIO1_IRQ_1 => { + extern "Rust" { + fn PIO1_IRQ_1(); + } + unsafe { + PIO1_IRQ_1(); + } + } + rp235x_pac::Interrupt::PIO2_IRQ_0 => { + extern "Rust" { + fn PIO2_IRQ_0(); + } + unsafe { + PIO2_IRQ_0(); + } + } + rp235x_pac::Interrupt::PIO2_IRQ_1 => { + extern "Rust" { + fn PIO2_IRQ_1(); + } + unsafe { + PIO2_IRQ_1(); + } + } + rp235x_pac::Interrupt::IO_IRQ_BANK0 => { + extern "Rust" { + fn IO_IRQ_BANK0(); + } + unsafe { + IO_IRQ_BANK0(); + } + } + rp235x_pac::Interrupt::IO_IRQ_BANK0_NS => { + extern "Rust" { + fn IO_IRQ_BANK0_NS(); + } + unsafe { + IO_IRQ_BANK0_NS(); + } + } + rp235x_pac::Interrupt::IO_IRQ_QSPI => { + extern "Rust" { + fn IO_IRQ_QSPI(); + } + unsafe { + IO_IRQ_QSPI(); + } + } + rp235x_pac::Interrupt::IO_IRQ_QSPI_NS => { + extern "Rust" { + fn IO_IRQ_QSPI_NS(); + } + unsafe { + IO_IRQ_QSPI_NS(); + } + } + rp235x_pac::Interrupt::SIO_IRQ_FIFO => { + extern "Rust" { + fn SIO_IRQ_FIFO(); + } + unsafe { + SIO_IRQ_FIFO(); + } + } + rp235x_pac::Interrupt::SIO_IRQ_BELL => { + extern "Rust" { + fn SIO_IRQ_BELL(); + } + unsafe { + SIO_IRQ_BELL(); + } + } + rp235x_pac::Interrupt::SIO_IRQ_FIFO_NS => { + extern "Rust" { + fn SIO_IRQ_FIFO_NS(); + } + unsafe { + SIO_IRQ_FIFO_NS(); + } + } + rp235x_pac::Interrupt::SIO_IRQ_BELL_NS => { + extern "Rust" { + fn SIO_IRQ_BELL_NS(); + } + unsafe { + SIO_IRQ_BELL_NS(); + } + } + rp235x_pac::Interrupt::SIO_IRQ_MTIMECMP => { + extern "Rust" { + fn SIO_IRQ_MTIMECMP(); + } + unsafe { + SIO_IRQ_MTIMECMP(); + } + } + rp235x_pac::Interrupt::CLOCKS_IRQ => { + extern "Rust" { + fn CLOCKS_IRQ(); + } + unsafe { + CLOCKS_IRQ(); + } + } + rp235x_pac::Interrupt::SPI0_IRQ => { + extern "Rust" { + fn SPI0_IRQ(); + } + unsafe { + SPI0_IRQ(); + } + } + rp235x_pac::Interrupt::SPI1_IRQ => { + extern "Rust" { + fn SPI1_IRQ(); + } + unsafe { + SPI1_IRQ(); + } + } + rp235x_pac::Interrupt::UART0_IRQ => { + extern "Rust" { + fn UART0_IRQ(); + } + unsafe { + UART0_IRQ(); + } + } + rp235x_pac::Interrupt::UART1_IRQ => { + extern "Rust" { + fn UART1_IRQ(); + } + unsafe { + UART1_IRQ(); + } + } + rp235x_pac::Interrupt::ADC_IRQ_FIFO => { + extern "Rust" { + fn ADC_IRQ_FIFO(); + } + unsafe { + ADC_IRQ_FIFO(); + } + } + rp235x_pac::Interrupt::I2C0_IRQ => { + extern "Rust" { + fn I2C0_IRQ(); + } + unsafe { + I2C0_IRQ(); + } + } + rp235x_pac::Interrupt::I2C1_IRQ => { + extern "Rust" { + fn I2C1_IRQ(); + } + unsafe { + I2C1_IRQ(); + } + } + rp235x_pac::Interrupt::OTP_IRQ => { + extern "Rust" { + fn OTP_IRQ(); + } + unsafe { + OTP_IRQ(); + } + } + rp235x_pac::Interrupt::TRNG_IRQ => { + extern "Rust" { + fn TRNG_IRQ(); + } + unsafe { + TRNG_IRQ(); + } + } + rp235x_pac::Interrupt::PLL_SYS_IRQ => { + extern "Rust" { + fn PLL_SYS_IRQ(); + } + unsafe { + PLL_SYS_IRQ(); + } + } + rp235x_pac::Interrupt::PLL_USB_IRQ => { + extern "Rust" { + fn PLL_USB_IRQ(); + } + unsafe { + PLL_USB_IRQ(); + } + } + rp235x_pac::Interrupt::POWMAN_IRQ_POW => { + extern "Rust" { + fn POWMAN_IRQ_POW(); + } + unsafe { + POWMAN_IRQ_POW(); + } + } + rp235x_pac::Interrupt::POWMAN_IRQ_TIMER => { + extern "Rust" { + fn POWMAN_IRQ_TIMER(); + } + unsafe { + POWMAN_IRQ_TIMER(); + } + } + } + } + } + + /// Our default IRQ handler. + /// + /// Just spins. + #[no_mangle] + #[allow(non_snake_case)] + fn DefaultIrqHandler() { + // Spin, so you can attach a debugger if you get stuck here. + // This is the also the default functionality used in cortex-m-rt. + loop { + crate::arch::nop(); + } + } } #[cfg(not(all(any(target_arch = "arm", target_arch = "riscv32"), target_os = "none")))] mod inner { /// Placeholder function to disable interrupts pub fn interrupt_disable() {} + /// Placeholder function to enable interrupts - pub fn interrupt_enable() {} + /// + /// # Safety + /// + /// Do not call from inside a critical section. + pub unsafe fn interrupt_enable() {} + /// Placeholder function to check if interrupts are enabled pub fn interrupts_enabled() -> bool { false } + + /// Placeholder function to wait for an interrupt + pub fn wfi() {} + /// Placeholder function to wait for an event pub fn wfe() {} + /// Placeholder function to do nothing pub fn nop() {} + /// Placeholder function to emit a data synchronisation barrier pub fn dsb() {} - /// Placeholder function to run a closure with interrupts disabled - pub fn interrupt_free(f: F) -> T - where - F: FnOnce() -> T, - { - f() - } + /// Placeholder function to wait for some clock cycles pub fn delay(_: u32) {} + /// Placeholder function to emit an event pub fn sev() {} + + /// Placeholder function to check if an IRQ is pending + pub fn interrrupt_is_pending(_irq: rp235x_pac::Interrupt) -> bool { + false + } + + /// Placeholder function to enable an IRQ + /// + /// # Safety + /// + /// This function is unsafe because it can break mask-based critical + /// sections. Do not call inside a critical section. + pub unsafe fn interrupt_unmask(_irq: rp235x_pac::Interrupt) {} + + /// Placeholder function to disable an IRQ + pub fn interrupt_mask(_irq: rp235x_pac::Interrupt) {} + + /// Placeholder function to check if an IRQ is enabled + pub fn interrupt_is_enabled(_irq: rp235x_pac::Interrupt) -> bool { + false + } + + /// Placeholder function to mark an IRQ as pending + pub fn interrupt_pend(_irq: rp235x_pac::Interrupt) {} } -pub use inner::*; +#[doc(inline)] +pub use inner::{ + delay, dsb, interrrupt_is_pending, interrupt_disable, interrupt_enable, interrupt_is_enabled, + interrupt_mask, interrupt_pend, interrupt_unmask, interrupts_enabled, nop, sev, wfe, wfi, +}; + +/// Run the closure without interrupts +/// +/// No critical-section token because we haven't blocked the second core +pub fn interrupt_free(f: F) -> T +where + F: FnOnce() -> T, +{ + let active = interrupts_enabled(); + if active { + interrupt_disable(); + } + let t = f(); + if active { + unsafe { + interrupt_enable(); + } + } + t +} /// Create a static variable which we can grab a mutable reference to exactly once. #[macro_export] diff --git a/rp235x-hal/src/lib.rs b/rp235x-hal/src/lib.rs index 788741ba5..a98908e5a 100644 --- a/rp235x-hal/src/lib.rs +++ b/rp235x-hal/src/lib.rs @@ -70,6 +70,7 @@ pub mod uart; pub mod usb; pub mod vector_table; pub mod watchdog; +pub mod xh3irq; pub mod xosc; // Provide access to common datastructures to avoid repeating ourselves diff --git a/rp235x-hal/src/sio.rs b/rp235x-hal/src/sio.rs index 15bf2455b..7ba94ab80 100644 --- a/rp235x-hal/src/sio.rs +++ b/rp235x-hal/src/sio.rs @@ -57,6 +57,11 @@ pub struct SioGpioQspi { _private: (), } +/// Marker struct for ownership of SIO Machine Timer +pub struct MachineTimer { + _private: (), +} + /// Struct containing ownership markers for managing ownership of the SIO registers. pub struct Sio { _sio: pac::SIO, @@ -70,6 +75,8 @@ pub struct Sio { pub interp0: Interp0, /// Interpolator 1 pub interp1: Interp1, + /// RISC-V Machine Timer + pub machine_timer: MachineTimer, } impl Sio { @@ -88,6 +95,7 @@ impl Sio { lane0: Interp1Lane0 { _private: () }, lane1: Interp1Lane1 { _private: () }, }, + machine_timer: MachineTimer { _private: () }, } } @@ -194,6 +202,33 @@ impl SioFifo { } } +impl MachineTimer { + /// Read the SIO's Machine Timer + pub fn read(&self) -> u64 { + let sio = unsafe { &(*pac::SIO::ptr()) }; + loop { + let mtimeh = sio.mtimeh().read().mtimeh().bits(); + let mtime = sio.mtime().read().mtime().bits(); + let mtimeh2 = sio.mtimeh().read().mtimeh().bits(); + if mtimeh == mtimeh2 { + return u64::from(mtimeh) << 32 | u64::from(mtime); + } + } + } + + /// Set whether the timer is enabled + pub fn set_enabled(&mut self, enabled: bool) { + let sio = unsafe { &(*pac::SIO::ptr()) }; + sio.mtime_ctrl().write(|w| w.en().variant(enabled)); + } + + /// Set the speed the clock runs at + pub fn set_fullspeed(&mut self, fullspeed: bool) { + let sio = unsafe { &(*pac::SIO::ptr()) }; + sio.mtime_ctrl().write(|w| w.fullspeed().variant(fullspeed)); + } +} + /// This type is just used to limit us to Spinlocks `0..=31` pub trait SpinlockValid: Sealed {} diff --git a/rp235x-hal/src/xh3irq.rs b/rp235x-hal/src/xh3irq.rs new file mode 100644 index 000000000..44ef57de1 --- /dev/null +++ b/rp235x-hal/src/xh3irq.rs @@ -0,0 +1,242 @@ +//! Hazard3 Interrupt Controller (Xh3irq) Driver +//! +//! > Xh3irq controls up to 512 external interrupts, with up to 16 levels of +//! > pre-emption. It is architected as a layer on top of the standard mip.meip +//! > external interrupt line, and all standard RISC-V interrupt behaviour still +//! > applies. +//! +//! See [Section 3.8.6.1](https://rptl.io/rp2350-datasheet#extension-xh3irq-section) for more details + +/// The Machine External Interrupt Enable Array +/// +/// The array contains a read-write bit for each external interrupt request: a +/// `1` bit indicates that interrupt is currently enabled. At reset, all +/// external interrupts are disabled. +/// +/// If enabled, an external interrupt can cause assertion of the standard RISC-V +/// machine external interrupt pending flag (`mip.meip`), and therefore cause +/// the processor to enter the external interrupt vector. See `meipa`. +/// +/// There are up to 512 external interrupts. The upper half of this register +/// contains a 16-bit window into the full 512-bit vector. The window is indexed +/// by the 5 LSBs of the write data. +pub const RVCSR_MEIEA_OFFSET: u32 = 0xbe0; + +/// Machine External Interrupt Pending Array +/// +/// Contains a read-only bit for each external interrupt request. Similarly to +/// `meiea`, this register is a window into an array of up to 512 external +/// interrupt flags. The status appears in the upper 16 bits of the value read +/// from `meipa`, and the lower 5 bits of the value _written_ by the same CSR +/// instruction (or 0 if no write takes place) select a 16-bit window of the +/// full interrupt pending array. +/// +/// A `1` bit indicates that interrupt is currently asserted. IRQs are assumed +/// to be level-sensitive, and the relevant `meipa` bit is cleared by servicing +/// the requestor so that it deasserts its interrupt request. +/// +/// When any interrupt of sufficient priority is both set in `meipa` and enabled +/// in `meiea`, the standard RISC-V external interrupt pending bit `mip.meip` is +/// asserted. In other words, `meipa` is filtered by `meiea` to generate the +/// standard `mip.meip` flag. +pub const RVCSR_MEIPA_OFFSET: u32 = 0xbe1; + +/// Machine External Interrupt Force Array +// +/// Contains a read-write bit for every interrupt request. Writing a 1 to a bit +/// in the interrupt force array causes the corresponding bit to become pending +/// in `meipa`. Software can use this feature to manually trigger a particular +/// interrupt. +/// +/// There are no restrictions on using `meifa` inside of an interrupt. The more +/// useful case here is to schedule some lower- priority handler from within a +/// high-priority interrupt, so that it will execute before the core returns to +/// the foreground code. Implementers may wish to reserve some external IRQs +/// with their external inputs tied to 0 for this purpose. +/// +/// Bits can be cleared by software, and are cleared automatically by hardware +/// upon a read of `meinext` which returns the corresponding IRQ number in +/// `meinext.irq` with `mienext.noirq` clear (no matter whether `meinext.update` +/// is written). +/// +/// `meifa` implements the same array window indexing scheme as `meiea` and +/// `meipa`. +pub const RVCSR_MEIFA_OFFSET: u32 = 0xbe2; + +/// Machine External Interrupt Priority Array +/// +/// Each interrupt has an (up to) 4-bit priority value associated with it, and +/// each access to this register reads and/or writes a 16-bit window containing +/// four such priority values. When less than 16 priority levels are available, +/// the LSBs of the priority fields are hardwired to 0. +/// +/// When an interrupt's priority is lower than the current preemption priority +/// `meicontext.preempt`, it is treated as not being pending for the purposes of +/// `mip.meip`. The pending bit in `meipa` will still assert, but the machine +/// external interrupt pending bit `mip.meip` will not, so the processor will +/// ignore this interrupt. See `meicontext`. +pub const RVCSR_MEIPRA_OFFSET: u32 = 0xbe3; + +/// Machine External Get Next Interrupt +/// +/// Contains the index of the highest-priority external interrupt which is both +/// asserted in `meipa` and enabled in `meiea`, left- shifted by 2 so that it +/// can be used to index an array of 32-bit function pointers. If there is no +/// such interrupt, the MSB is set. +/// +/// When multiple interrupts of the same priority are both pending and enabled, +/// the lowest-numbered wins. Interrupts with priority less than +/// `meicontext.ppreempt` -- the _previous_ preemption priority -- are treated +/// as though they are not pending. This is to ensure that a preempting +/// interrupt frame does not service interrupts which may be in progress in the +/// frame that was preempted. +pub const RVCSR_MEINEXT_OFFSET: u32 = 0xbe4; + +/// Check if a given interrupt is pending +#[cfg(all(target_arch = "riscv32", target_os = "none"))] +pub fn is_pending(irq: rp235x_pac::Interrupt) -> bool { + let (index, bits) = interrupt_to_mask(irq); + let index = index as u32; + let mut csr_rdata: u32; + // Do a CSR Read-Set on RVCSR_MEIPA_OFFSET + unsafe { + core::arch::asm!( + "csrrs {0}, 0xbe1, {1}", + out(reg) csr_rdata, + in(reg) index + ); + } + let bitmask = (bits as u32) << 16; + (csr_rdata & bitmask) != 0 +} + +/// Enable an interrupt +/// +/// # Safety +/// +/// This function is unsafe because it can break mask-based critical +/// sections. Do not call inside a critical section. +#[cfg(all(target_arch = "riscv32", target_os = "none"))] +pub unsafe fn unmask(irq: rp235x_pac::Interrupt) { + let mask_index = interrupt_to_mask_index(irq); + // Do a RISC-V CSR Set on RVCSR_MEIEA_OFFSET + unsafe { + core::arch::asm!( + "csrs 0xbe0, {0}", + in(reg) mask_index + ); + } +} + +/// Disable an interrupt +#[cfg(all(target_arch = "riscv32", target_os = "none"))] +pub fn mask(irq: rp235x_pac::Interrupt) { + let mask_index = interrupt_to_mask_index(irq); + // Do a RISC-V CSR Clear on RVCSR_MEIEA_OFFSET + unsafe { + core::arch::asm!( + "csrc 0xbe0, {0}", + in(reg) mask_index + ); + } +} + +/// Check if an interrupt is enabled +#[cfg(all(target_arch = "riscv32", target_os = "none"))] +pub fn is_enabled(irq: rp235x_pac::Interrupt) -> bool { + let (index, bits) = interrupt_to_mask(irq); + let index = index as u32; + let mut csr_rdata: u32; + // Do a CSR Read-Set on RVCSR_MEIEA_OFFSET + unsafe { + core::arch::asm!( + "csrrs {0}, 0xbe0, {1}", + out(reg) csr_rdata, + in(reg) index + ); + } + let bitmask = (bits as u32) << 16; + (csr_rdata & bitmask) != 0 +} + +/// Set an interrupt as pending, even if it isn't. +#[cfg(all(target_arch = "riscv32", target_os = "none"))] +pub fn pend(irq: rp235x_pac::Interrupt) { + let mask_index = interrupt_to_mask_index(irq); + // Do a RISC-V CSR Set on RVCSR_MEIFA_OFFSET + unsafe { + core::arch::asm!( + "csrs 0xbe2, {0}", + in(reg) mask_index + ); + } +} + +/// Check which interrupt should be handled next +/// +/// Also updates the state so next time you call this you'll get a different +/// answer. +#[cfg(all(target_arch = "riscv32", target_os = "none"))] +pub fn get_next_interrupt() -> Option { + const NOIRQ: u32 = 0x8000_0000; + + let mut csr_rdata: u32; + // Do a CSR Read-Set-Immediate on MEINEXT to set the UPDATE bit + unsafe { + core::arch::asm!( + "csrrsi {0}, 0xbe4, 0x01", + out(reg) csr_rdata, + ); + } + + if (csr_rdata & NOIRQ) != 0 { + return None; + } + + let irq_no = (csr_rdata >> 2) as u8; + let irq = rp235x_pac::Interrupt::try_from(irq_no).unwrap(); + Some(irq) +} + +/// Convert an IRQ into a window selection value and separate a bitmask for that +/// window. +pub const fn interrupt_to_mask(irq: rp235x_pac::Interrupt) -> (u16, u16) { + let irq = irq as u16; + // Select a bank of 16 interrupts out of the 512 total + let index = irq / 16; + // Which interrupt out of the 16 we've selected + let bitmask = 1 << (irq % 16); + (index, bitmask) +} + +/// Convert an IRQ into a 32-bit value that selects a window and a bit within +/// that window. +pub const fn interrupt_to_mask_index(irq: rp235x_pac::Interrupt) -> u32 { + let (index, bits) = interrupt_to_mask(irq); + (bits as u32) << 16 | index as u32 +} + +#[cfg(test)] +mod test { + use super::*; + + #[test] + fn test_interrupt_to_mask() { + let (index, bitmask) = interrupt_to_mask(rp235x_pac::Interrupt::ADC_IRQ_FIFO); + assert_eq!(rp235x_pac::Interrupt::ADC_IRQ_FIFO as u16, 35); + // ADC_IRQ_FIFO is IRQ 35, so that's index 2, bit 3 + assert_eq!(index, 2); + assert_eq!(bitmask, 1 << 3); + } + + #[test] + fn test_interrupt_to_mask_index() { + let mask = interrupt_to_mask_index(rp235x_pac::Interrupt::ADC_IRQ_FIFO); + assert_eq!(rp235x_pac::Interrupt::ADC_IRQ_FIFO as u16, 35); + // ADC_IRQ_FIFO is IRQ 35, so that's index 2, bit 3 + // This value is in (bitmask | index) format, and 1 << 3 is 0x0008 + assert_eq!(mask, 0x0008_0002); + } +} + +// End of file