diff --git a/rp2040-hal/Cargo.toml b/rp2040-hal/Cargo.toml index 8eb988de8..4898f57fc 100644 --- a/rp2040-hal/Cargo.toml +++ b/rp2040-hal/Cargo.toml @@ -18,8 +18,8 @@ targets = ["thumbv6m-none-eabi"] [dependencies] cortex-m = "0.7.2" embedded-hal = { version = "0.2.5", features = ["unproven"] } -eh1_0_alpha = { package = "embedded-hal", version = "=1.0.0-rc.1", optional = true } -eh_nb_1_0_alpha = { package = "embedded-hal-nb", version = "=1.0.0-rc.1", optional = true } +eh1_0_alpha = { package = "embedded-hal", version = "=1.0.0-rc.3", optional = true } +eh_nb_1_0_alpha = { package = "embedded-hal-nb", version = "=1.0.0-rc.3", optional = true } embedded-dma = "0.2.0" embedded-io = "0.6.1" fugit = "0.3.6" @@ -181,6 +181,10 @@ required-features = ["critical-section-impl"] name = "pwm_blink" required-features = ["critical-section-impl"] +[[example]] +name = "pwm_blink_embedded_hal_1" +required-features = ["critical-section-impl", "eh1_0_alpha"] + [[example]] name = "rom_funcs" required-features = ["critical-section-impl"] diff --git a/rp2040-hal/examples/pio_blink.rs b/rp2040-hal/examples/pio_blink.rs index 18e69220b..67ac6777f 100644 --- a/rp2040-hal/examples/pio_blink.rs +++ b/rp2040-hal/examples/pio_blink.rs @@ -66,7 +66,7 @@ fn main() -> ! { let (mut pio, sm0, _, _, _) = pac.PIO0.split(&mut pac.RESETS); let installed = pio.install(&program).unwrap(); let (int, frac) = (0, 0); // as slow as possible (0 is interpreted as 65536) - let (sm, _, _) = rp2040_hal::pio::PIOBuilder::from_program(installed) + let (sm, _, _) = rp2040_hal::pio::PIOBuilder::from_installed_program(installed) .set_pins(led_pin_id, 1) .clock_divisor_fixed_point(int, frac) .build(sm0); diff --git a/rp2040-hal/examples/pio_dma.rs b/rp2040-hal/examples/pio_dma.rs index 78e37cd69..a40b33b47 100644 --- a/rp2040-hal/examples/pio_dma.rs +++ b/rp2040-hal/examples/pio_dma.rs @@ -62,7 +62,7 @@ fn main() -> ! { // Initialize and start PIO let (mut pio, sm0, _, _, _) = pac.PIO0.split(&mut pac.RESETS); let installed = pio.install(&program.program).unwrap(); - let (mut sm, rx, tx) = rp2040_hal::pio::PIOBuilder::from_program(installed) + let (mut sm, rx, tx) = rp2040_hal::pio::PIOBuilder::from_installed_program(installed) .out_pins(led_pin_id, 1) .clock_divisor_fixed_point(0, 0) // as slow as possible (0 is interpreted as 65536) .autopull(true) diff --git a/rp2040-hal/examples/pio_proc_blink.rs b/rp2040-hal/examples/pio_proc_blink.rs index 7f5c9d245..75cdebca6 100644 --- a/rp2040-hal/examples/pio_proc_blink.rs +++ b/rp2040-hal/examples/pio_proc_blink.rs @@ -52,7 +52,7 @@ fn main() -> ! { let (mut pio, sm0, _, _, _) = pac.PIO0.split(&mut pac.RESETS); let installed = pio.install(&program.program).unwrap(); let (int, frac) = (0, 0); // as slow as possible (0 is interpreted as 65536) - let (mut sm, _, _) = rp2040_hal::pio::PIOBuilder::from_program(installed) + let (mut sm, _, _) = rp2040_hal::pio::PIOBuilder::from_installed_program(installed) .set_pins(led_pin_id, 1) .clock_divisor_fixed_point(int, frac) .build(sm0); diff --git a/rp2040-hal/examples/pio_side_set.rs b/rp2040-hal/examples/pio_side_set.rs index 89f01b940..5db05f260 100644 --- a/rp2040-hal/examples/pio_side_set.rs +++ b/rp2040-hal/examples/pio_side_set.rs @@ -55,7 +55,7 @@ fn main() -> ! { let (mut pio, sm0, _, _, _) = pac.PIO0.split(&mut pac.RESETS); let installed = pio.install(&program.program).unwrap(); let (int, frac) = (0, 0); // as slow as possible (0 is interpreted as 65536) - let (mut sm, _, _) = rp2040_hal::pio::PIOBuilder::from_program(installed) + let (mut sm, _, _) = rp2040_hal::pio::PIOBuilder::from_installed_program(installed) .side_set_pin_base(led_pin_id) .clock_divisor_fixed_point(int, frac) .build(sm0); diff --git a/rp2040-hal/examples/pio_synchronized.rs b/rp2040-hal/examples/pio_synchronized.rs index 9759162b5..2e19edc35 100644 --- a/rp2040-hal/examples/pio_synchronized.rs +++ b/rp2040-hal/examples/pio_synchronized.rs @@ -64,7 +64,7 @@ fn main() -> ! { let (int, frac) = (256, 0); let installed = pio.install(&program.program).unwrap(); - let (mut sm0, _, _) = rp2040_hal::pio::PIOBuilder::from_program( + let (mut sm0, _, _) = rp2040_hal::pio::PIOBuilder::from_installed_program( // Safety: We won't uninstall the program, ever unsafe { installed.share() }, ) @@ -74,7 +74,7 @@ fn main() -> ! { // The GPIO pin needs to be configured as an output. sm0.set_pindirs([(pin0, hal::pio::PinDir::Output)]); - let (mut sm1, _, _) = rp2040_hal::pio::PIOBuilder::from_program(installed) + let (mut sm1, _, _) = rp2040_hal::pio::PIOBuilder::from_installed_program(installed) .set_pins(pin1, 1) .clock_divisor_fixed_point(int, frac) .build(sm1); diff --git a/rp2040-hal/examples/pwm_blink.rs b/rp2040-hal/examples/pwm_blink.rs index 3572da98d..dc1b77e49 100644 --- a/rp2040-hal/examples/pwm_blink.rs +++ b/rp2040-hal/examples/pwm_blink.rs @@ -104,13 +104,13 @@ fn main() -> ! { // Infinite loop, fading LED up and down loop { // Ramp brightness up - for i in (LOW..=HIGH).skip(100) { + for i in LOW..=HIGH { delay.delay_us(8); channel.set_duty(i); } // Ramp brightness down - for i in (LOW..=HIGH).rev().skip(100) { + for i in (LOW..=HIGH).rev() { delay.delay_us(8); channel.set_duty(i); } diff --git a/rp2040-hal/examples/pwm_blink_embedded_hal_1.rs b/rp2040-hal/examples/pwm_blink_embedded_hal_1.rs new file mode 100644 index 000000000..c1b537d44 --- /dev/null +++ b/rp2040-hal/examples/pwm_blink_embedded_hal_1.rs @@ -0,0 +1,122 @@ +//! # PWM Blink Example +//! +//! If you have an LED connected to pin 25, it will fade the LED using the PWM +//! peripheral. +//! +//! It may need to be adapted to your particular board layout and/or pin assignment. +//! +//! See the `Cargo.toml` file for Copyright and license details. + +#![no_std] +#![no_main] + +// Ensure we halt the program on panic (if we don't mention this crate it won't +// be linked) +use panic_halt as _; + +// Alias for our HAL crate +use rp2040_hal as hal; + +// Some traits we need +use eh1_0_alpha::pwm::SetDutyCycle; +use rp2040_hal::clocks::Clock; + +// A shorter alias for the Peripheral Access Crate, which provides low-level +// register access +use hal::pac; + +/// The linker will place this boot block at the start of our program image. We +/// need this to help the ROM bootloader get our code up and running. +/// Note: This boot block is not necessary when using a rp-hal based BSP +/// as the BSPs already perform this step. +#[link_section = ".boot2"] +#[used] +pub static BOOT2: [u8; 256] = rp2040_boot2::BOOT_LOADER_GENERIC_03H; + +/// The minimum PWM value (i.e. LED brightness) we want +const LOW: u16 = 0; + +/// The maximum PWM value (i.e. LED brightness) we want +const HIGH: u16 = 25000; + +/// External high-speed crystal on the Raspberry Pi Pico board is 12 MHz. Adjust +/// if your board has a different frequency +const XTAL_FREQ_HZ: u32 = 12_000_000u32; + +/// Entry point to our bare-metal application. +/// +/// The `#[rp2040_hal::entry]` macro ensures the Cortex-M start-up code calls this function +/// as soon as all global variables and the spinlock are initialised. +/// +/// The function configures the RP2040 peripherals, then fades the LED in an +/// infinite loop. +#[rp2040_hal::entry] +fn main() -> ! { + // Grab our singleton objects + let mut pac = pac::Peripherals::take().unwrap(); + let core = pac::CorePeripherals::take().unwrap(); + + // Set up the watchdog driver - needed by the clock setup code + let mut watchdog = hal::Watchdog::new(pac.WATCHDOG); + + // Configure the clocks + // + // The default is to generate a 125 MHz system clock + let clocks = hal::clocks::init_clocks_and_plls( + XTAL_FREQ_HZ, + pac.XOSC, + pac.CLOCKS, + pac.PLL_SYS, + pac.PLL_USB, + &mut pac.RESETS, + &mut watchdog, + ) + .ok() + .unwrap(); + + // The single-cycle I/O block controls our GPIO pins + let sio = hal::Sio::new(pac.SIO); + + // Set the pins up according to their function on this particular board + let pins = hal::gpio::Pins::new( + pac.IO_BANK0, + pac.PADS_BANK0, + sio.gpio_bank0, + &mut pac.RESETS, + ); + + // The delay object lets us wait for specified amounts of time (in + // milliseconds) + let mut delay = cortex_m::delay::Delay::new(core.SYST, clocks.system_clock.freq().to_Hz()); + + // Init PWMs + let mut pwm_slices = hal::pwm::Slices::new(pac.PWM, &mut pac.RESETS); + + // Configure PWM4 + let pwm = &mut pwm_slices.pwm4; + pwm.set_ph_correct(); + pwm.enable(); + + // Output channel B on PWM4 to GPIO 25 + let channel = &mut pwm.channel_b; + channel.output_to(pins.gpio25); + + // Infinite loop, fading LED up and down + loop { + // Ramp brightness up + for i in LOW..=HIGH { + delay.delay_us(8); + channel.set_duty_cycle(i).unwrap(); + } + + // Ramp brightness down + for i in (LOW..=HIGH).rev() { + delay.delay_us(8); + channel.set_duty_cycle(i).unwrap(); + } + + delay.delay_ms(500); + } +} + +// End of file diff --git a/rp2040-hal/src/gpio/func.rs b/rp2040-hal/src/gpio/func.rs index 6eb32c188..353180df2 100644 --- a/rp2040-hal/src/gpio/func.rs +++ b/rp2040-hal/src/gpio/func.rs @@ -173,6 +173,7 @@ impl DynFunction { } } } +impl ValidFunction for P {} macro_rules! pin_valid_func { ($bank:ident as $prefix:ident, [$head:ident $(, $func:ident)*], [$($name:tt),+]) => { pin_valid_func!($bank as $prefix, [$($func),*], [$($name),+]); diff --git a/rp2040-hal/src/gpio/mod.rs b/rp2040-hal/src/gpio/mod.rs index 00923ad12..b54f71f67 100644 --- a/rp2040-hal/src/gpio/mod.rs +++ b/rp2040-hal/src/gpio/mod.rs @@ -1434,11 +1434,11 @@ mod eh1 { I: PinId, P: PullType, { - fn is_set_high(&self) -> Result { + fn is_set_high(&mut self) -> Result { Ok(self._is_set_high()) } - fn is_set_low(&self) -> Result { + fn is_set_low(&mut self) -> Result { Ok(self._is_set_low()) } } @@ -1459,11 +1459,11 @@ mod eh1 { I: PinId, P: PullType, { - fn is_high(&self) -> Result { + fn is_high(&mut self) -> Result { Ok(self._is_high()) } - fn is_low(&self) -> Result { + fn is_low(&mut self) -> Result { Ok(self._is_low()) } } @@ -1480,11 +1480,11 @@ mod eh1 { impl<'a, I: PinId, F: super::func::Function, P: PullType> InputPin for super::AsInputPin<'a, I, F, P> { - fn is_high(&self) -> Result { + fn is_high(&mut self) -> Result { Ok(self.0._is_high()) } - fn is_low(&self) -> Result { + fn is_low(&mut self) -> Result { Ok(self.0._is_low()) } } diff --git a/rp2040-hal/src/pio.rs b/rp2040-hal/src/pio.rs index 58705dade..dd9f9da1c 100644 --- a/rp2040-hal/src/pio.rs +++ b/rp2040-hal/src/pio.rs @@ -290,7 +290,7 @@ impl PIO

{ /// ).program; /// let installed = pio.install(&program).unwrap(); /// // Configure a state machine to use the program. -/// let (sm, rx, tx) = PIOBuilder::from_program(installed).build(sm0); +/// let (sm, rx, tx) = PIOBuilder::from_installed_program(installed).build(sm0); /// // Uninitialize the state machine again, freeing the program. /// let (sm, installed) = sm.uninit(rx, tx); /// // Uninstall the program to free instruction memory. @@ -1940,12 +1940,44 @@ pub enum InstallError { } impl PIOBuilder

{ - /// Set config settings based on information from the given [`pio::Program`]. + /// Set config settings based on information from the given [`InstalledProgram`]. + /// Additional configuration may be needed in addition to this. + /// + /// Note: This was formerly called `from_program`. The new function has + /// a different default shift direction, `ShiftDirection::Right`, matching + /// the hardware reset value. + pub fn from_installed_program(p: InstalledProgram

) -> Self { + PIOBuilder { + clock_divisor: (1, 0), + program: p, + jmp_pin: 0, + out_sticky: false, + inline_out: None, + mov_status: MovStatusConfig::Tx(0), + fifo_join: Buffers::RxTx, + pull_threshold: 0, + push_threshold: 0, + out_shiftdir: ShiftDirection::Right, + in_shiftdir: ShiftDirection::Right, + autopull: false, + autopush: false, + set_count: 5, + out_count: 0, + in_base: 0, + side_set_base: 0, + set_base: 0, + out_base: 0, + } + } + + /// Set config settings based on information from the given [`InstalledProgram`]. /// Additional configuration may be needed in addition to this. /// /// Note: The shift direction for both input and output shift registers /// defaults to `ShiftDirection::Left`, which is different from the - /// rp2040 reset value. + /// rp2040 reset value. The alternative [`Self::from_installed_program`], + /// fixes this. + #[deprecated(note = "please use `from_installed_program` instead")] pub fn from_program(p: InstalledProgram

) -> Self { PIOBuilder { clock_divisor: (1, 0), diff --git a/rp2040-hal/src/pwm/mod.rs b/rp2040-hal/src/pwm/mod.rs index b06e1b8c6..365bfccba 100644 --- a/rp2040-hal/src/pwm/mod.rs +++ b/rp2040-hal/src/pwm/mod.rs @@ -76,8 +76,12 @@ //! min_config() leaves those registers in the state they were before it was called (Careful, this can lead to unexpected behavior) //! It's recommended to only call min_config() after calling default_config() on a pin that shares a PWM block. +#[cfg(feature = "eh1_0_alpha")] +use core::convert::Infallible; use core::marker::PhantomData; +#[cfg(feature = "eh1_0_alpha")] +use eh1_0_alpha::pwm::{ErrorType, SetDutyCycle}; use embedded_dma::Word; use embedded_hal::PwmPin; @@ -652,6 +656,23 @@ impl PwmPin for Channel { } } +#[cfg(feature = "eh1_0_alpha")] +impl ErrorType for Channel { + type Error = Infallible; +} + +#[cfg(feature = "eh1_0_alpha")] +impl SetDutyCycle for Channel { + fn max_duty_cycle(&self) -> u16 { + self.get_max_duty() + } + + fn set_duty_cycle(&mut self, duty: u16) -> Result<(), Self::Error> { + self.set_duty(duty); + Ok(()) + } +} + impl PwmPin for Channel { type Duty = u16; diff --git a/rp2040-hal/src/rtc/mod.rs b/rp2040-hal/src/rtc/mod.rs index ec0b38cf6..adb11c4cc 100644 --- a/rp2040-hal/src/rtc/mod.rs +++ b/rp2040-hal/src/rtc/mod.rs @@ -36,6 +36,7 @@ pub use self::datetime::{DateTime, DayOfWeek, Error as DateTimeError}; /// A reference to the real time clock of the system pub struct RealTimeClock { rtc: RTC, + clock: RtcClock, } impl RealTimeClock { @@ -68,7 +69,7 @@ impl RealTimeClock { let freq = clock.freq().to_Hz() - 1; rtc.clkdiv_m1.write(|w| unsafe { w.bits(freq) }); - let mut result = Self { rtc }; + let mut result = Self { rtc, clock }; result.set_leap_year_check(true); // should be on by default, make sure this is the case. result.set_datetime(initial_date)?; Ok(result) @@ -196,6 +197,12 @@ impl RealTimeClock { self.set_match_ena(false); self.set_match_ena(true); } + + /// Free the RTC peripheral and RTC clock + pub fn free(self, resets: &mut RESETS) -> (RTC, RtcClock) { + resets.reset.modify(|_, w| w.rtc().set_bit()); + (self.rtc, self.clock) + } } /// Errors that can occur on methods on [RtcClock] diff --git a/rp2040-hal/src/sio.rs b/rp2040-hal/src/sio.rs index ead75c858..694bf0cc3 100644 --- a/rp2040-hal/src/sio.rs +++ b/rp2040-hal/src/sio.rs @@ -202,79 +202,97 @@ impl SioFifo { } } +macro_rules! concatln { + ($(,)*) => { + "" + }; + ( $e:expr ) => { + $e + }; + ( $e:expr $(, $es:expr)+ $(,)*) => { + ::core::concat!( $e, "\n", concatln!($($es),+) ) + }; +} + // This takes advantage of how AAPCS defines a 64-bit return on 32-bit registers // by packing it into r0[0:31] and r1[32:63]. So all we need to do is put // the remainder in the high order 32 bits of a 64 bit result. We can also // alias the division operators to these for a similar reason r0 is the // result either way and r1 a scratch register, so the caller can't assume it // retains the argument value. -#[cfg(target_arch = "arm")] -core::arch::global_asm!( - ".macro hwdivider_head", - "ldr r2, =(0xd0000000)", // SIO_BASE - // Check the DIRTY state of the divider by shifting it into the C - // status bit. - "ldr r3, [r2, #0x078]", // DIV_CSR - "lsrs r3, #2", // DIRTY = 1, so shift 2 down - // We only need to save the state when DIRTY, otherwise we can just do the - // division directly. - "bcs 2f", - "1:", - // Do the actual division now, we're either not DIRTY, or we've saved the - // state and branched back here so it's safe now. - ".endm", - ".macro hwdivider_tail", - // 8 cycle delay to wait for the result. Each branch takes two cycles - // and fits into a 2-byte Thumb instruction, so this is smaller than - // 8 NOPs. - "b 3f", - "3: b 3f", - "3: b 3f", - "3: b 3f", - "3:", - // Read the quotient last, since that's what clears the dirty flag. - "ldr r1, [r2, #0x074]", // DIV_REMAINDER - "ldr r0, [r2, #0x070]", // DIV_QUOTIENT - // Either return to the caller or back to the state restore. - "bx lr", - "2:", - // Since we can't save the signed-ness of the calculation, we have to make - // sure that there's at least an 8 cycle delay before we read the result. - // The push takes 5 cycles, and we've already spent at least 7 checking - // the DIRTY state to get here. - "push {{r4-r6, lr}}", - // Read the quotient last, since that's what clears the dirty flag. - "ldr r3, [r2, #0x060]", // DIV_UDIVIDEND - "ldr r4, [r2, #0x064]", // DIV_UDIVISOR - "ldr r5, [r2, #0x074]", // DIV_REMAINDER - "ldr r6, [r2, #0x070]", // DIV_QUOTIENT - // If we get interrupted here (before a write sets the DIRTY flag) it's - // fine, since we have the full state, so the interruptor doesn't have to - // restore it. Once the write happens and the DIRTY flag is set, the - // interruptor becomes responsible for restoring our state. - "bl 1b", - // If we are interrupted here, then the interruptor will start an incorrect - // calculation using a wrong divisor, but we'll restore the divisor and - // result ourselves correctly. This sets DIRTY, so any interruptor will - // save the state. - "str r3, [r2, #0x060]", // DIV_UDIVIDEND - // If we are interrupted here, the the interruptor may start the - // calculation using incorrectly signed inputs, but we'll restore the - // result ourselves. This sets DIRTY, so any interruptor will save the - // state. - "str r4, [r2, #0x064]", // DIV_UDIVISOR - // If we are interrupted here, the interruptor will have restored - // everything but the quotient may be wrongly signed. If the calculation - // started by the above writes is still ongoing it is stopped, so it won't - // replace the result we're restoring. DIRTY and READY set, but only - // DIRTY matters to make the interruptor save the state. - "str r5, [r2, #0x074]", // DIV_REMAINDER - // State fully restored after the quotient write. This sets both DIRTY - // and READY, so whatever we may have interrupted can read the result. - "str r6, [r2, #0x070]", // DIV_QUOTIENT - "pop {{r4-r6, pc}}", - ".endm", -); +macro_rules! hwdivider_head { + () => { + concatln!( + "ldr r2, =(0xd0000000)", // SIO_BASE + // Check the DIRTY state of the divider by shifting it into the C + // status bit. + "ldr r3, [r2, #0x078]", // DIV_CSR + "lsrs r3, #2", // DIRTY = 1, so shift 2 down + // We only need to save the state when DIRTY, otherwise we can just do the + // division directly. + "bcs 2f", + "1:", + // Do the actual division now, we're either not DIRTY, or we've saved the + // state and branched back here so it's safe now. + ) + }; +} + +macro_rules! hwdivider_tail { + () => { + concatln!( + // 8 cycle delay to wait for the result. Each branch takes two cycles + // and fits into a 2-byte Thumb instruction, so this is smaller than + // 8 NOPs. + "b 3f", + "3: b 3f", + "3: b 3f", + "3: b 3f", + "3:", + // Read the quotient last, since that's what clears the dirty flag. + "ldr r1, [r2, #0x074]", // DIV_REMAINDER + "ldr r0, [r2, #0x070]", // DIV_QUOTIENT + // Either return to the caller or back to the state restore. + "bx lr", + "2:", + // Since we can't save the signed-ness of the calculation, we have to make + // sure that there's at least an 8 cycle delay before we read the result. + // The push takes 5 cycles, and we've already spent at least 7 checking + // the DIRTY state to get here. + "push {{r4-r6, lr}}", + // Read the quotient last, since that's what clears the dirty flag. + "ldr r3, [r2, #0x060]", // DIV_UDIVIDEND + "ldr r4, [r2, #0x064]", // DIV_UDIVISOR + "ldr r5, [r2, #0x074]", // DIV_REMAINDER + "ldr r6, [r2, #0x070]", // DIV_QUOTIENT + // If we get interrupted here (before a write sets the DIRTY flag) it's + // fine, since we have the full state, so the interruptor doesn't have to + // restore it. Once the write happens and the DIRTY flag is set, the + // interruptor becomes responsible for restoring our state. + "bl 1b", + // If we are interrupted here, then the interruptor will start an incorrect + // calculation using a wrong divisor, but we'll restore the divisor and + // result ourselves correctly. This sets DIRTY, so any interruptor will + // save the state. + "str r3, [r2, #0x060]", // DIV_UDIVIDEND + // If we are interrupted here, the the interruptor may start the + // calculation using incorrectly signed inputs, but we'll restore the + // result ourselves. This sets DIRTY, so any interruptor will save the + // state. + "str r4, [r2, #0x064]", // DIV_UDIVISOR + // If we are interrupted here, the interruptor will have restored + // everything but the quotient may be wrongly signed. If the calculation + // started by the above writes is still ongoing it is stopped, so it won't + // replace the result we're restoring. DIRTY and READY set, but only + // DIRTY matters to make the interruptor save the state. + "str r5, [r2, #0x074]", // DIV_REMAINDER + // State fully restored after the quotient write. This sets both DIRTY + // and READY, so whatever we may have interrupted can read the result. + "str r6, [r2, #0x070]", // DIV_QUOTIENT + "pop {{r4-r6, pc}}", + ) + }; +} macro_rules! division_function { ( @@ -295,9 +313,9 @@ macro_rules! division_function { concat!(stringify!($intrinsic), ":"), )* - "hwdivider_head", + hwdivider_head!(), $($begin),+ , - "hwdivider_tail", + hwdivider_tail!(), ); #[cfg(all(target_arch = "arm", feature = "disable-intrinsics"))] @@ -308,9 +326,9 @@ macro_rules! division_function { ".align 2", concat!("_rphal_", stringify!($name), ":"), - "hwdivider_head", + hwdivider_head!(), $($begin),+ , - "hwdivider_tail", + hwdivider_tail!(), ); #[cfg(target_arch = "arm")] diff --git a/rp2040-hal/src/timer.rs b/rp2040-hal/src/timer.rs index ca6832a3c..4b897c6c9 100644 --- a/rp2040-hal/src/timer.rs +++ b/rp2040-hal/src/timer.rs @@ -167,10 +167,26 @@ macro_rules! impl_delay_traits { impl_delay_traits!(u8, u16, u32, i32); #[cfg(feature = "eh1_0_alpha")] -impl eh1_0_alpha::delay::DelayUs for Timer { +impl eh1_0_alpha::delay::DelayNs for Timer { + fn delay_ns(&mut self, ns: u32) { + // For now, just use microsecond delay, internally. Of course, this + // might cause a much longer delay than necessary. So a more advanced + // implementation would be desirable for sub-microsecond delays. + let us = ns / 1000 + if ns % 1000 == 0 { 0 } else { 1 }; + // With rustc 1.73, this can be replaced by: + // let us = ns.div_ceil(1000); + self.delay_us_internal(us) + } + fn delay_us(&mut self, us: u32) { self.delay_us_internal(us) } + + fn delay_ms(&mut self, ms: u32) { + for _ in 0..ms { + self.delay_us_internal(1000); + } + } } /// Implementation of the embedded_hal::Timer traits using rp2040_hal::timer counter diff --git a/rp2040-hal/src/usb.rs b/rp2040-hal/src/usb.rs index 3a693eaa4..d0dcbb8fa 100644 --- a/rp2040-hal/src/usb.rs +++ b/rp2040-hal/src/usb.rs @@ -172,11 +172,12 @@ struct Inner { out_endpoints: [Option; 16], next_offset: u16, read_setup: bool, + pll: UsbClock, #[cfg(feature = "rp2040-e5")] errata5_state: Option, } impl Inner { - fn new(ctrl_reg: USBCTRL_REGS, ctrl_dpram: USBCTRL_DPRAM) -> Self { + fn new(ctrl_reg: USBCTRL_REGS, ctrl_dpram: USBCTRL_DPRAM, pll: UsbClock) -> Self { Self { ctrl_reg, ctrl_dpram, @@ -184,6 +185,7 @@ impl Inner { out_endpoints: Default::default(), next_offset: 0, read_setup: false, + pll, #[cfg(feature = "rp2040-e5")] errata5_state: None, } @@ -438,7 +440,7 @@ impl UsbBus { pub fn new( ctrl_reg: USBCTRL_REGS, ctrl_dpram: USBCTRL_DPRAM, - _pll: UsbClock, + pll: UsbClock, force_vbus_detect_bit: bool, resets: &mut RESETS, ) -> Self { @@ -476,7 +478,7 @@ impl UsbBus { }); Self { - inner: Mutex::new(RefCell::new(Inner::new(ctrl_reg, ctrl_dpram))), + inner: Mutex::new(RefCell::new(Inner::new(ctrl_reg, ctrl_dpram, pll))), } } @@ -487,6 +489,17 @@ impl UsbBus { inner.ctrl_reg.sie_ctrl.modify(|_, w| w.resume().set_bit()); }); } + + /// Stop and free the Usb resources + pub fn free(self, resets: &mut RESETS) -> (USBCTRL_REGS, USBCTRL_DPRAM, UsbClock) { + critical_section::with(|_cs| { + let inner = self.inner.into_inner().into_inner(); + + inner.ctrl_reg.reset_bring_down(resets); + + (inner.ctrl_reg, inner.ctrl_dpram, inner.pll) + }) + } } impl UsbBusTrait for UsbBus {