From 289762c0efc43cf8357309bb98b28a5959b63d26 Mon Sep 17 00:00:00 2001 From: Eric Yanush <> Date: Mon, 20 Mar 2023 14:19:14 -0600 Subject: [PATCH 01/14] Add initial setup of async can for STM32 --- embassy-stm32/src/can/bxcan.rs | 157 ++++++++++++++++++++++++++++++++- 1 file changed, 155 insertions(+), 2 deletions(-) diff --git a/embassy-stm32/src/can/bxcan.rs b/embassy-stm32/src/can/bxcan.rs index bd92b35a..7e5b1cfa 100644 --- a/embassy-stm32/src/can/bxcan.rs +++ b/embassy-stm32/src/can/bxcan.rs @@ -1,9 +1,13 @@ +use core::future::poll_fn; use core::ops::{Deref, DerefMut}; +use core::task::Poll; pub use bxcan; +use bxcan::Frame; use embassy_hal_common::{into_ref, PeripheralRef}; use crate::gpio::sealed::AFType; +use crate::interrupt::{Interrupt, InterruptExt}; use crate::rcc::RccPeripheral; use crate::{peripherals, Peripheral}; @@ -39,8 +43,12 @@ impl<'d, T: Instance> Can<'d, T> { peri: impl Peripheral

+ 'd, rx: impl Peripheral

> + 'd, tx: impl Peripheral

> + 'd, + tx_irq: impl Peripheral

+ 'd, + rx0_irq: impl Peripheral

+ 'd, + rx1_irq: impl Peripheral

+ 'd, + sce_irq: impl Peripheral

+ 'd, ) -> Self { - into_ref!(peri, rx, tx); + into_ref!(peri, rx, tx, tx_irq, rx0_irq, rx1_irq, sce_irq); unsafe { rx.set_as_af(rx.af_num(), AFType::Input); @@ -50,10 +58,75 @@ impl<'d, T: Instance> Can<'d, T> { T::enable(); T::reset(); + tx_irq.unpend(); + tx_irq.set_handler(Self::tx_interrupt); + tx_irq.enable(); + + rx0_irq.unpend(); + rx0_irq.set_handler(Self::rx0_interrupt); + rx0_irq.enable(); + + rx1_irq.unpend(); + rx1_irq.set_handler(Self::rx1_interrupt); + rx1_irq.enable(); + + sce_irq.unpend(); + sce_irq.set_handler(Self::sce_interrupt); + sce_irq.enable(); + Self { can: bxcan::Can::builder(BxcanInstance(peri)).leave_disabled(), } } + + pub async fn transmit_async(&mut self, frame: &Frame) { + defmt::info!("Staring async frame transmission"); + let tx_status = self.queue_transmit(frame).await; + self.wait_transission(tx_status.mailbox()).await; + } + + async fn queue_transmit(&mut self, frame: &Frame) -> bxcan::TransmitStatus { + poll_fn(|cx| { + if let Ok(status) = self.can.transmit(frame) { + defmt::info!("Frame queued successfully in mb{}", status.mailbox()); + return Poll::Ready(status); + } + defmt::info!("Mailboxes full, waiting to queue"); + T::state().tx_waker.register(cx.waker()); + Poll::Pending + }) + .await + } + + async fn wait_transission(&mut self, mb: bxcan::Mailbox) { + poll_fn(|cx| unsafe { + defmt::info!("Waiting for tx to complete"); + if T::regs().tsr().read().tme(mb.index()) { + defmt::info!("TX complete for mb {}", mb); + return Poll::Ready(()); + } + defmt::info!("TX not complete, waiting for signal on mb {}", mb); + T::state().tx_waker.register(cx.waker()); + Poll::Pending + }) + .await; + } + + unsafe fn tx_interrupt(_: *mut ()) { + defmt::info!("bxCAN TX interrupt fired!"); + T::regs().tsr().write(|v| { + v.set_rqcp(0, true); + v.set_rqcp(1, true); + v.set_rqcp(2, true); + }); + T::state().tx_waker.wake(); + } + + unsafe fn rx0_interrupt(_: *mut ()) {} + + unsafe fn rx1_interrupt(_: *mut ()) {} + + unsafe fn sce_interrupt(_: *mut ()) {} } impl<'d, T: Instance> Drop for Can<'d, T> { @@ -80,14 +153,51 @@ impl<'d, T: Instance> DerefMut for Can<'d, T> { } pub(crate) mod sealed { + use embassy_sync::waitqueue::AtomicWaker; + pub struct State { + pub tx_waker: AtomicWaker, + pub rx0_waker: AtomicWaker, + pub rx1_waker: AtomicWaker, + pub sce_waker: AtomicWaker, + } + + impl State { + pub const fn new() -> Self { + Self { + tx_waker: AtomicWaker::new(), + rx0_waker: AtomicWaker::new(), + rx1_waker: AtomicWaker::new(), + sce_waker: AtomicWaker::new(), + } + } + } + pub trait Instance { const REGISTERS: *mut bxcan::RegisterBlock; fn regs() -> &'static crate::pac::can::Can; + fn state() -> &'static State; } } -pub trait Instance: sealed::Instance + RccPeripheral {} +pub trait TXInstance { + type TXInterrupt: crate::interrupt::Interrupt; +} + +pub trait RX0Instance { + type RX0Interrupt: crate::interrupt::Interrupt; +} + +pub trait RX1Instance { + type RX1Interrupt: crate::interrupt::Interrupt; +} + +pub trait SCEInstance { + type SCEInterrupt: crate::interrupt::Interrupt; +} + +pub trait InterruptableInstance: TXInstance + RX0Instance + RX1Instance + SCEInstance {} +pub trait Instance: sealed::Instance + RccPeripheral + InterruptableInstance + 'static {} pub struct BxcanInstance<'a, T>(PeripheralRef<'a, T>); @@ -103,10 +213,39 @@ foreach_peripheral!( fn regs() -> &'static crate::pac::can::Can { &crate::pac::$inst } + + fn state() -> &'static sealed::State { + static STATE: sealed::State = sealed::State::new(); + &STATE + } } impl Instance for peripherals::$inst {} + foreach_interrupt!( + ($inst,can,CAN,TX,$irq:ident) => { + impl TXInstance for peripherals::$inst { + type TXInterrupt = crate::interrupt::$irq; + } + }; + ($inst,can,CAN,RX0,$irq:ident) => { + impl RX0Instance for peripherals::$inst { + type RX0Interrupt = crate::interrupt::$irq; + } + }; + ($inst,can,CAN,RX1,$irq:ident) => { + impl RX1Instance for peripherals::$inst { + type RX1Interrupt = crate::interrupt::$irq; + } + }; + ($inst,can,CAN,SCE,$irq:ident) => { + impl SCEInstance for peripherals::$inst { + type SCEInterrupt = crate::interrupt::$irq; + } + }; + ); + + impl InterruptableInstance for peripherals::$inst {} }; ); @@ -147,3 +286,17 @@ foreach_peripheral!( pin_trait!(RxPin, Instance); pin_trait!(TxPin, Instance); + +trait Index { + fn index(&self) -> usize; +} + +impl Index for bxcan::Mailbox { + fn index(&self) -> usize { + match self { + bxcan::Mailbox::Mailbox0 => 0, + bxcan::Mailbox::Mailbox1 => 1, + bxcan::Mailbox::Mailbox2 => 2, + } + } +} From 9876571887845f60958c148308ae8a59433cd809 Mon Sep 17 00:00:00 2001 From: Eric Yanush <> Date: Mon, 20 Mar 2023 14:25:15 -0600 Subject: [PATCH 02/14] Strip out debug messages... oops --- embassy-stm32/src/can/bxcan.rs | 7 ------- 1 file changed, 7 deletions(-) diff --git a/embassy-stm32/src/can/bxcan.rs b/embassy-stm32/src/can/bxcan.rs index 7e5b1cfa..521049ec 100644 --- a/embassy-stm32/src/can/bxcan.rs +++ b/embassy-stm32/src/can/bxcan.rs @@ -80,7 +80,6 @@ impl<'d, T: Instance> Can<'d, T> { } pub async fn transmit_async(&mut self, frame: &Frame) { - defmt::info!("Staring async frame transmission"); let tx_status = self.queue_transmit(frame).await; self.wait_transission(tx_status.mailbox()).await; } @@ -88,10 +87,8 @@ impl<'d, T: Instance> Can<'d, T> { async fn queue_transmit(&mut self, frame: &Frame) -> bxcan::TransmitStatus { poll_fn(|cx| { if let Ok(status) = self.can.transmit(frame) { - defmt::info!("Frame queued successfully in mb{}", status.mailbox()); return Poll::Ready(status); } - defmt::info!("Mailboxes full, waiting to queue"); T::state().tx_waker.register(cx.waker()); Poll::Pending }) @@ -100,12 +97,9 @@ impl<'d, T: Instance> Can<'d, T> { async fn wait_transission(&mut self, mb: bxcan::Mailbox) { poll_fn(|cx| unsafe { - defmt::info!("Waiting for tx to complete"); if T::regs().tsr().read().tme(mb.index()) { - defmt::info!("TX complete for mb {}", mb); return Poll::Ready(()); } - defmt::info!("TX not complete, waiting for signal on mb {}", mb); T::state().tx_waker.register(cx.waker()); Poll::Pending }) @@ -113,7 +107,6 @@ impl<'d, T: Instance> Can<'d, T> { } unsafe fn tx_interrupt(_: *mut ()) { - defmt::info!("bxCAN TX interrupt fired!"); T::regs().tsr().write(|v| { v.set_rqcp(0, true); v.set_rqcp(1, true); From 8d7abeb06fbe3e19db3cae3f5220725969ecbb81 Mon Sep 17 00:00:00 2001 From: Eric Yanush <> Date: Thu, 6 Apr 2023 08:20:34 -0600 Subject: [PATCH 03/14] Round out the async fns for can --- embassy-stm32/Cargo.toml | 1 + embassy-stm32/src/can/bxcan.rs | 252 ++++++++++++++++++++++++++++++--- 2 files changed, 237 insertions(+), 16 deletions(-) diff --git a/embassy-stm32/Cargo.toml b/embassy-stm32/Cargo.toml index 504caacb..5e45db36 100644 --- a/embassy-stm32/Cargo.toml +++ b/embassy-stm32/Cargo.toml @@ -68,6 +68,7 @@ stm32-fmc = "0.2.4" seq-macro = "0.3.0" cfg-if = "1.0.0" embedded-io = { version = "0.4.0", features = ["async"], optional = true } +heapless = { version = "0.7.5", default-features = false } [dev-dependencies] critical-section = { version = "1.1", features = ["std"] } diff --git a/embassy-stm32/src/can/bxcan.rs b/embassy-stm32/src/can/bxcan.rs index 521049ec..734efdc0 100644 --- a/embassy-stm32/src/can/bxcan.rs +++ b/embassy-stm32/src/can/bxcan.rs @@ -3,18 +3,39 @@ use core::ops::{Deref, DerefMut}; use core::task::Poll; pub use bxcan; -use bxcan::Frame; +use bxcan::{Data, ExtendedId, Frame, Id, StandardId}; use embassy_hal_common::{into_ref, PeripheralRef}; use crate::gpio::sealed::AFType; -use crate::interrupt::{Interrupt, InterruptExt}; +use crate::interrupt::InterruptExt; +use crate::pac::can::vals::{Lec, RirIde}; use crate::rcc::RccPeripheral; +use crate::time::Hertz; use crate::{peripherals, Peripheral}; pub struct Can<'d, T: Instance> { can: bxcan::Can>, } +#[derive(Debug)] +pub enum BusError { + Stuff, + Form, + Acknowledge, + BitRecessive, + BitDominant, + Crc, + Software, + BusOff, + BusPassive, + BusWarning, +} + +pub enum FrameOrError { + Frame(Frame), + Error(BusError), +} + impl<'d, T: Instance> Can<'d, T> { /// Creates a new Bxcan instance, blocking for 11 recessive bits to sync with the CAN bus. pub fn new( @@ -74,12 +95,16 @@ impl<'d, T: Instance> Can<'d, T> { sce_irq.set_handler(Self::sce_interrupt); sce_irq.enable(); - Self { - can: bxcan::Can::builder(BxcanInstance(peri)).leave_disabled(), - } + let can = bxcan::Can::builder(BxcanInstance(peri)).leave_disabled(); + Self { can } } - pub async fn transmit_async(&mut self, frame: &Frame) { + pub fn set_bitrate(&mut self, bitrate: u32) { + let bit_timing = Self::calc_bxcan_timings(T::frequency(), bitrate).unwrap(); + self.can.modify_config().set_bit_timing(bit_timing).leave_disabled(); + } + + pub async fn transmit(&mut self, frame: &Frame) { let tx_status = self.queue_transmit(frame).await; self.wait_transission(tx_status.mailbox()).await; } @@ -95,7 +120,7 @@ impl<'d, T: Instance> Can<'d, T> { .await } - async fn wait_transission(&mut self, mb: bxcan::Mailbox) { + async fn wait_transission(&self, mb: bxcan::Mailbox) { poll_fn(|cx| unsafe { if T::regs().tsr().read().tme(mb.index()) { return Poll::Ready(()); @@ -106,6 +131,45 @@ impl<'d, T: Instance> Can<'d, T> { .await; } + pub async fn receive_frame_or_error(&mut self) -> FrameOrError { + poll_fn(|cx| { + if let Some(frame) = T::state().rx_queue.dequeue() { + return Poll::Ready(FrameOrError::Frame(frame)); + } else if let Some(err) = self.curr_error() { + return Poll::Ready(FrameOrError::Error(err)); + } + T::state().rx_waker.register(cx.waker()); + T::state().err_waker.register(cx.waker()); + Poll::Pending + }) + .await + } + + fn curr_error(&self) -> Option { + let err = unsafe { T::regs().esr().read() }; + if err.boff() { + return Some(BusError::BusOff); + } else if err.epvf() { + return Some(BusError::BusPassive); + } else if err.ewgf() { + return Some(BusError::BusWarning); + } else if let Some(err) = err.lec().into_bus_err() { + return Some(err); + } + None + } + + unsafe fn sce_interrupt(_: *mut ()) { + let msr = T::regs().msr(); + let msr_val = msr.read(); + + if msr_val.erri() { + msr.modify(|v| v.set_erri(true)); + T::state().err_waker.wake(); + return; + } + } + unsafe fn tx_interrupt(_: *mut ()) { T::regs().tsr().write(|v| { v.set_rqcp(0, true); @@ -115,11 +179,146 @@ impl<'d, T: Instance> Can<'d, T> { T::state().tx_waker.wake(); } - unsafe fn rx0_interrupt(_: *mut ()) {} + unsafe fn rx0_interrupt(_: *mut ()) { + Self::receive_fifo(RxFifo::Fifo0); + } - unsafe fn rx1_interrupt(_: *mut ()) {} + unsafe fn rx1_interrupt(_: *mut ()) { + Self::receive_fifo(RxFifo::Fifi1); + } - unsafe fn sce_interrupt(_: *mut ()) {} + unsafe fn receive_fifo(fifo: RxFifo) { + let state = T::state(); + let regs = T::regs(); + let fifo_idx = match fifo { + RxFifo::Fifo0 => 0usize, + RxFifo::Fifi1 => 1usize, + }; + let rfr = regs.rfr(fifo_idx); + let fifo = regs.rx(fifo_idx); + + // If there are no pending messages, there is nothing to do + if rfr.read().fmp() == 0 { + return; + } + + let rir = fifo.rir().read(); + let id = if rir.ide() == RirIde::STANDARD { + Id::from(StandardId::new_unchecked(rir.stid())) + } else { + Id::from(ExtendedId::new_unchecked(rir.exid())) + }; + let data_len = fifo.rdtr().read().dlc() as usize; + let mut data: [u8; 8] = [0; 8]; + data[0..4].copy_from_slice(&fifo.rdlr().read().0.to_ne_bytes()); + data[4..8].copy_from_slice(&fifo.rdhr().read().0.to_ne_bytes()); + + let frame = Frame::new_data(id, Data::new(&data[0..data_len]).unwrap()); + + rfr.modify(|v| v.set_rfom(true)); + + match state.rx_queue.enqueue(frame) { + Ok(_) => {} + Err(_) => defmt::error!("RX queue overflow"), + } + state.rx_waker.wake(); + } + + pub fn calc_bxcan_timings(periph_clock: Hertz, can_bitrate: u32) -> Option { + const BS1_MAX: u8 = 16; + const BS2_MAX: u8 = 8; + const MAX_SAMPLE_POINT_PERMILL: u16 = 900; + + let periph_clock = periph_clock.0; + + if can_bitrate < 1000 { + return None; + } + + // Ref. "Automatic Baudrate Detection in CANopen Networks", U. Koppe, MicroControl GmbH & Co. KG + // CAN in Automation, 2003 + // + // According to the source, optimal quanta per bit are: + // Bitrate Optimal Maximum + // 1000 kbps 8 10 + // 500 kbps 16 17 + // 250 kbps 16 17 + // 125 kbps 16 17 + let max_quanta_per_bit: u8 = if can_bitrate >= 1_000_000 { 10 } else { 17 }; + + // Computing (prescaler * BS): + // BITRATE = 1 / (PRESCALER * (1 / PCLK) * (1 + BS1 + BS2)) -- See the Reference Manual + // BITRATE = PCLK / (PRESCALER * (1 + BS1 + BS2)) -- Simplified + // let: + // BS = 1 + BS1 + BS2 -- Number of time quanta per bit + // PRESCALER_BS = PRESCALER * BS + // ==> + // PRESCALER_BS = PCLK / BITRATE + let prescaler_bs = periph_clock / can_bitrate; + + // Searching for such prescaler value so that the number of quanta per bit is highest. + let mut bs1_bs2_sum = max_quanta_per_bit - 1; + while (prescaler_bs % (1 + bs1_bs2_sum) as u32) != 0 { + if bs1_bs2_sum <= 2 { + return None; // No solution + } + bs1_bs2_sum -= 1; + } + + let prescaler = prescaler_bs / (1 + bs1_bs2_sum) as u32; + if (prescaler < 1) || (prescaler > 1024) { + return None; // No solution + } + + // Now we have a constraint: (BS1 + BS2) == bs1_bs2_sum. + // We need to find such values so that the sample point is as close as possible to the optimal value, + // which is 87.5%, which is 7/8. + // + // Solve[(1 + bs1)/(1 + bs1 + bs2) == 7/8, bs2] (* Where 7/8 is 0.875, the recommended sample point location *) + // {{bs2 -> (1 + bs1)/7}} + // + // Hence: + // bs2 = (1 + bs1) / 7 + // bs1 = (7 * bs1_bs2_sum - 1) / 8 + // + // Sample point location can be computed as follows: + // Sample point location = (1 + bs1) / (1 + bs1 + bs2) + // + // Since the optimal solution is so close to the maximum, we prepare two solutions, and then pick the best one: + // - With rounding to nearest + // - With rounding to zero + let mut bs1 = ((7 * bs1_bs2_sum - 1) + 4) / 8; // Trying rounding to nearest first + let mut bs2 = bs1_bs2_sum - bs1; + assert!(bs1_bs2_sum > bs1); + + let sample_point_permill = 1000 * ((1 + bs1) / (1 + bs1 + bs2)) as u16; + if sample_point_permill > MAX_SAMPLE_POINT_PERMILL { + // Nope, too far; now rounding to zero + bs1 = (7 * bs1_bs2_sum - 1) / 8; + bs2 = bs1_bs2_sum - bs1; + } + + // Check is BS1 and BS2 are in range + if (bs1 < 1) || (bs1 > BS1_MAX) || (bs2 < 1) || (bs2 > BS2_MAX) { + return None; + } + + // Check if final bitrate matches the requested + if can_bitrate != (periph_clock / (prescaler * (1 + bs1 + bs2) as u32)) { + return None; + } + + // One is recommended by DS-015, CANOpen, and DeviceNet + let sjw = 1; + + // Pack into BTR register values + Some((sjw - 1) << 24 | (bs1 as u32 - 1) << 16 | (bs2 as u32 - 1) << 20 | (prescaler as u32 - 1)) + } +} + +enum RxFifo { + Fifo0, + Fifi1, } impl<'d, T: Instance> Drop for Can<'d, T> { @@ -147,20 +346,22 @@ impl<'d, T: Instance> DerefMut for Can<'d, T> { pub(crate) mod sealed { use embassy_sync::waitqueue::AtomicWaker; + use heapless::mpmc::Q8; + pub struct State { pub tx_waker: AtomicWaker, - pub rx0_waker: AtomicWaker, - pub rx1_waker: AtomicWaker, - pub sce_waker: AtomicWaker, + pub rx_waker: AtomicWaker, + pub err_waker: AtomicWaker, + pub rx_queue: Q8, } impl State { pub const fn new() -> Self { Self { tx_waker: AtomicWaker::new(), - rx0_waker: AtomicWaker::new(), - rx1_waker: AtomicWaker::new(), - sce_waker: AtomicWaker::new(), + rx_waker: AtomicWaker::new(), + err_waker: AtomicWaker::new(), + rx_queue: Q8::new(), } } } @@ -293,3 +494,22 @@ impl Index for bxcan::Mailbox { } } } + +trait IntoBusError { + fn into_bus_err(self) -> Option; +} + +impl IntoBusError for Lec { + fn into_bus_err(self) -> Option { + match self { + Lec::STUFF => Some(BusError::Stuff), + Lec::FORM => Some(BusError::Form), + Lec::ACK => Some(BusError::Acknowledge), + Lec::BITRECESSIVE => Some(BusError::BitRecessive), + Lec::BITDOMINANT => Some(BusError::BitDominant), + Lec::CRC => Some(BusError::Crc), + Lec::CUSTOM => Some(BusError::Software), + _ => None, + } + } +} From f8d35806dc773a6310a60115fbf90f48fe409207 Mon Sep 17 00:00:00 2001 From: xoviat Date: Mon, 29 May 2023 19:09:52 -0500 Subject: [PATCH 04/14] stm32/can: move to irq binding use embassy channel --- embassy-stm32/Cargo.toml | 1 - embassy-stm32/src/can/bxcan.rs | 184 +++++++++++++++++++-------------- 2 files changed, 105 insertions(+), 80 deletions(-) diff --git a/embassy-stm32/Cargo.toml b/embassy-stm32/Cargo.toml index 5d4b26a3..4e29bb32 100644 --- a/embassy-stm32/Cargo.toml +++ b/embassy-stm32/Cargo.toml @@ -69,7 +69,6 @@ cfg-if = "1.0.0" embedded-io = { version = "0.4.0", features = ["async"], optional = true } chrono = { version = "^0.4", default-features = false, optional = true} bit_field = "0.10.2" -heapless = { version = "0.7.5", default-features = false } [dev-dependencies] critical-section = { version = "1.1", features = ["std"] } diff --git a/embassy-stm32/src/can/bxcan.rs b/embassy-stm32/src/can/bxcan.rs index 734efdc0..8ae1dcb9 100644 --- a/embassy-stm32/src/can/bxcan.rs +++ b/embassy-stm32/src/can/bxcan.rs @@ -1,17 +1,73 @@ use core::future::poll_fn; +use core::marker::PhantomData; use core::ops::{Deref, DerefMut}; use core::task::Poll; pub use bxcan; use bxcan::{Data, ExtendedId, Frame, Id, StandardId}; +use embassy_cortex_m::interrupt::Interrupt; use embassy_hal_common::{into_ref, PeripheralRef}; +use futures::FutureExt; use crate::gpio::sealed::AFType; use crate::interrupt::InterruptExt; use crate::pac::can::vals::{Lec, RirIde}; use crate::rcc::RccPeripheral; use crate::time::Hertz; -use crate::{peripherals, Peripheral}; +use crate::{interrupt, peripherals, Peripheral}; + +/// Interrupt handler. +pub struct TxInterruptHandler { + _phantom: PhantomData, +} + +impl interrupt::Handler for TxInterruptHandler { + unsafe fn on_interrupt() { + T::regs().tsr().write(|v| { + v.set_rqcp(0, true); + v.set_rqcp(1, true); + v.set_rqcp(2, true); + }); + + T::state().tx_waker.wake(); + } +} + +pub struct Rx0InterruptHandler { + _phantom: PhantomData, +} + +impl interrupt::Handler for Rx0InterruptHandler { + unsafe fn on_interrupt() { + Can::::receive_fifo(RxFifo::Fifo0); + } +} + +pub struct Rx1InterruptHandler { + _phantom: PhantomData, +} + +impl interrupt::Handler for Rx1InterruptHandler { + unsafe fn on_interrupt() { + Can::::receive_fifo(RxFifo::Fifo1); + } +} + +pub struct SceInterruptHandler { + _phantom: PhantomData, +} + +impl interrupt::Handler for SceInterruptHandler { + unsafe fn on_interrupt() { + let msr = T::regs().msr(); + let msr_val = msr.read(); + + if msr_val.erri() { + msr.modify(|v| v.set_erri(true)); + T::state().err_waker.wake(); + } + } +} pub struct Can<'d, T: Instance> { can: bxcan::Can>, @@ -64,12 +120,13 @@ impl<'d, T: Instance> Can<'d, T> { peri: impl Peripheral

+ 'd, rx: impl Peripheral

> + 'd, tx: impl Peripheral

> + 'd, - tx_irq: impl Peripheral

+ 'd, - rx0_irq: impl Peripheral

+ 'd, - rx1_irq: impl Peripheral

+ 'd, - sce_irq: impl Peripheral

+ 'd, + _irqs: impl interrupt::Binding> + + interrupt::Binding> + + interrupt::Binding> + + interrupt::Binding> + + 'd, ) -> Self { - into_ref!(peri, rx, tx, tx_irq, rx0_irq, rx1_irq, sce_irq); + into_ref!(peri, rx, tx); unsafe { rx.set_as_af(rx.af_num(), AFType::Input); @@ -79,21 +136,19 @@ impl<'d, T: Instance> Can<'d, T> { T::enable(); T::reset(); - tx_irq.unpend(); - tx_irq.set_handler(Self::tx_interrupt); - tx_irq.enable(); + unsafe { + T::TXInterrupt::steal().unpend(); + T::TXInterrupt::steal().enable(); - rx0_irq.unpend(); - rx0_irq.set_handler(Self::rx0_interrupt); - rx0_irq.enable(); + T::RX0Interrupt::steal().unpend(); + T::RX0Interrupt::steal().enable(); - rx1_irq.unpend(); - rx1_irq.set_handler(Self::rx1_interrupt); - rx1_irq.enable(); + T::RX1Interrupt::steal().unpend(); + T::RX1Interrupt::steal().enable(); - sce_irq.unpend(); - sce_irq.set_handler(Self::sce_interrupt); - sce_irq.enable(); + T::SCEInterrupt::steal().unpend(); + T::SCEInterrupt::steal().enable(); + } let can = bxcan::Can::builder(BxcanInstance(peri)).leave_disabled(); Self { can } @@ -133,12 +188,11 @@ impl<'d, T: Instance> Can<'d, T> { pub async fn receive_frame_or_error(&mut self) -> FrameOrError { poll_fn(|cx| { - if let Some(frame) = T::state().rx_queue.dequeue() { + if let Poll::Ready(frame) = T::state().rx_queue.recv().poll_unpin(cx) { return Poll::Ready(FrameOrError::Frame(frame)); } else if let Some(err) = self.curr_error() { return Poll::Ready(FrameOrError::Error(err)); } - T::state().rx_waker.register(cx.waker()); T::state().err_waker.register(cx.waker()); Poll::Pending }) @@ -159,69 +213,42 @@ impl<'d, T: Instance> Can<'d, T> { None } - unsafe fn sce_interrupt(_: *mut ()) { - let msr = T::regs().msr(); - let msr_val = msr.read(); - - if msr_val.erri() { - msr.modify(|v| v.set_erri(true)); - T::state().err_waker.wake(); - return; - } - } - - unsafe fn tx_interrupt(_: *mut ()) { - T::regs().tsr().write(|v| { - v.set_rqcp(0, true); - v.set_rqcp(1, true); - v.set_rqcp(2, true); - }); - T::state().tx_waker.wake(); - } - - unsafe fn rx0_interrupt(_: *mut ()) { - Self::receive_fifo(RxFifo::Fifo0); - } - - unsafe fn rx1_interrupt(_: *mut ()) { - Self::receive_fifo(RxFifo::Fifi1); - } - unsafe fn receive_fifo(fifo: RxFifo) { let state = T::state(); let regs = T::regs(); let fifo_idx = match fifo { RxFifo::Fifo0 => 0usize, - RxFifo::Fifi1 => 1usize, + RxFifo::Fifo1 => 1usize, }; let rfr = regs.rfr(fifo_idx); let fifo = regs.rx(fifo_idx); - // If there are no pending messages, there is nothing to do - if rfr.read().fmp() == 0 { - return; + loop { + // If there are no pending messages, there is nothing to do + if rfr.read().fmp() == 0 { + return; + } + + let rir = fifo.rir().read(); + let id = if rir.ide() == RirIde::STANDARD { + Id::from(StandardId::new_unchecked(rir.stid())) + } else { + Id::from(ExtendedId::new_unchecked(rir.exid())) + }; + let data_len = fifo.rdtr().read().dlc() as usize; + let mut data: [u8; 8] = [0; 8]; + data[0..4].copy_from_slice(&fifo.rdlr().read().0.to_ne_bytes()); + data[4..8].copy_from_slice(&fifo.rdhr().read().0.to_ne_bytes()); + + let frame = Frame::new_data(id, Data::new(&data[0..data_len]).unwrap()); + + rfr.modify(|v| v.set_rfom(true)); + + /* + NOTE: consensus was reached that if rx_queue is full, packets should be dropped + */ + let _ = state.rx_queue.try_send(frame); } - - let rir = fifo.rir().read(); - let id = if rir.ide() == RirIde::STANDARD { - Id::from(StandardId::new_unchecked(rir.stid())) - } else { - Id::from(ExtendedId::new_unchecked(rir.exid())) - }; - let data_len = fifo.rdtr().read().dlc() as usize; - let mut data: [u8; 8] = [0; 8]; - data[0..4].copy_from_slice(&fifo.rdlr().read().0.to_ne_bytes()); - data[4..8].copy_from_slice(&fifo.rdhr().read().0.to_ne_bytes()); - - let frame = Frame::new_data(id, Data::new(&data[0..data_len]).unwrap()); - - rfr.modify(|v| v.set_rfom(true)); - - match state.rx_queue.enqueue(frame) { - Ok(_) => {} - Err(_) => defmt::error!("RX queue overflow"), - } - state.rx_waker.wake(); } pub fn calc_bxcan_timings(periph_clock: Hertz, can_bitrate: u32) -> Option { @@ -318,7 +345,7 @@ impl<'d, T: Instance> Can<'d, T> { enum RxFifo { Fifo0, - Fifi1, + Fifo1, } impl<'d, T: Instance> Drop for Can<'d, T> { @@ -345,23 +372,22 @@ impl<'d, T: Instance> DerefMut for Can<'d, T> { } pub(crate) mod sealed { + use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex; + use embassy_sync::channel::Channel; use embassy_sync::waitqueue::AtomicWaker; - use heapless::mpmc::Q8; pub struct State { pub tx_waker: AtomicWaker, - pub rx_waker: AtomicWaker, pub err_waker: AtomicWaker, - pub rx_queue: Q8, + pub rx_queue: Channel, } impl State { pub const fn new() -> Self { Self { tx_waker: AtomicWaker::new(), - rx_waker: AtomicWaker::new(), err_waker: AtomicWaker::new(), - rx_queue: Q8::new(), + rx_queue: Channel::new(), } } } From 16bfbd4e99dbc765c93610557a7857a9013ff756 Mon Sep 17 00:00:00 2001 From: xoviat Date: Tue, 30 May 2023 21:14:25 -0500 Subject: [PATCH 05/14] stm32/can: add hw test and cleanup --- embassy-stm32/src/can/bxcan.rs | 78 ++++++++++++++++------------------ tests/stm32/Cargo.toml | 8 +++- tests/stm32/src/bin/can.rs | 78 ++++++++++++++++++++++++++++++++++ 3 files changed, 121 insertions(+), 43 deletions(-) create mode 100644 tests/stm32/src/bin/can.rs diff --git a/embassy-stm32/src/can/bxcan.rs b/embassy-stm32/src/can/bxcan.rs index 8ae1dcb9..08ba783f 100644 --- a/embassy-stm32/src/can/bxcan.rs +++ b/embassy-stm32/src/can/bxcan.rs @@ -39,6 +39,7 @@ pub struct Rx0InterruptHandler { impl interrupt::Handler for Rx0InterruptHandler { unsafe fn on_interrupt() { + // info!("rx0 irq"); Can::::receive_fifo(RxFifo::Fifo0); } } @@ -49,6 +50,7 @@ pub struct Rx1InterruptHandler { impl interrupt::Handler for Rx1InterruptHandler { unsafe fn on_interrupt() { + // info!("rx1 irq"); Can::::receive_fifo(RxFifo::Fifo1); } } @@ -59,6 +61,7 @@ pub struct SceInterruptHandler { impl interrupt::Handler for SceInterruptHandler { unsafe fn on_interrupt() { + // info!("sce irq"); let msr = T::regs().msr(); let msr_val = msr.read(); @@ -87,36 +90,10 @@ pub enum BusError { BusWarning, } -pub enum FrameOrError { - Frame(Frame), - Error(BusError), -} - impl<'d, T: Instance> Can<'d, T> { - /// Creates a new Bxcan instance, blocking for 11 recessive bits to sync with the CAN bus. - pub fn new( - peri: impl Peripheral

+ 'd, - rx: impl Peripheral

> + 'd, - tx: impl Peripheral

> + 'd, - ) -> Self { - into_ref!(peri, rx, tx); - - unsafe { - rx.set_as_af(rx.af_num(), AFType::Input); - tx.set_as_af(tx.af_num(), AFType::OutputPushPull); - } - - T::enable(); - T::reset(); - - Self { - can: bxcan::Can::builder(BxcanInstance(peri)).enable(), - } - } - /// Creates a new Bxcan instance, keeping the peripheral in sleep mode. /// You must call [Can::enable_non_blocking] to use the peripheral. - pub fn new_disabled( + pub fn new( peri: impl Peripheral

+ 'd, rx: impl Peripheral

> + 'd, tx: impl Peripheral

> + 'd, @@ -136,6 +113,25 @@ impl<'d, T: Instance> Can<'d, T> { T::enable(); T::reset(); + unsafe { + use crate::pac::can::vals::{Errie, Fmpie, Tmeie}; + + T::regs().ier().write(|w| { + // TODO: fix metapac + + w.set_errie(Errie(1)); + w.set_fmpie(0, Fmpie(1)); + w.set_fmpie(1, Fmpie(1)); + w.set_tmeie(Tmeie(1)); + }); + + T::regs().mcr().write(|w| { + // Enable timestamps on rx messages + + w.set_ttcm(true); + }) + } + unsafe { T::TXInterrupt::steal().unpend(); T::TXInterrupt::steal().enable(); @@ -159,12 +155,8 @@ impl<'d, T: Instance> Can<'d, T> { self.can.modify_config().set_bit_timing(bit_timing).leave_disabled(); } - pub async fn transmit(&mut self, frame: &Frame) { - let tx_status = self.queue_transmit(frame).await; - self.wait_transission(tx_status.mailbox()).await; - } - - async fn queue_transmit(&mut self, frame: &Frame) -> bxcan::TransmitStatus { + /// Queues the message to be sent but exerts backpressure + pub async fn write(&mut self, frame: &Frame) -> bxcan::TransmitStatus { poll_fn(|cx| { if let Ok(status) = self.can.transmit(frame) { return Poll::Ready(status); @@ -175,7 +167,7 @@ impl<'d, T: Instance> Can<'d, T> { .await } - async fn wait_transission(&self, mb: bxcan::Mailbox) { + pub async fn flush(&self, mb: bxcan::Mailbox) { poll_fn(|cx| unsafe { if T::regs().tsr().read().tme(mb.index()) { return Poll::Ready(()); @@ -186,12 +178,13 @@ impl<'d, T: Instance> Can<'d, T> { .await; } - pub async fn receive_frame_or_error(&mut self) -> FrameOrError { + /// Returns a tuple of the time the message was received and the message frame + pub async fn read(&mut self) -> Result<(u16, bxcan::Frame), BusError> { poll_fn(|cx| { - if let Poll::Ready(frame) = T::state().rx_queue.recv().poll_unpin(cx) { - return Poll::Ready(FrameOrError::Frame(frame)); + if let Poll::Ready((time, frame)) = T::state().rx_queue.recv().poll_unpin(cx) { + return Poll::Ready(Ok((time, frame))); } else if let Some(err) = self.curr_error() { - return Poll::Ready(FrameOrError::Error(err)); + return Poll::Ready(Err(err)); } T::state().err_waker.register(cx.waker()); Poll::Pending @@ -240,6 +233,7 @@ impl<'d, T: Instance> Can<'d, T> { data[0..4].copy_from_slice(&fifo.rdlr().read().0.to_ne_bytes()); data[4..8].copy_from_slice(&fifo.rdhr().read().0.to_ne_bytes()); + let time = fifo.rdtr().read().time(); let frame = Frame::new_data(id, Data::new(&data[0..data_len]).unwrap()); rfr.modify(|v| v.set_rfom(true)); @@ -247,11 +241,11 @@ impl<'d, T: Instance> Can<'d, T> { /* NOTE: consensus was reached that if rx_queue is full, packets should be dropped */ - let _ = state.rx_queue.try_send(frame); + let _ = state.rx_queue.try_send((time, frame)); } } - pub fn calc_bxcan_timings(periph_clock: Hertz, can_bitrate: u32) -> Option { + pub const fn calc_bxcan_timings(periph_clock: Hertz, can_bitrate: u32) -> Option { const BS1_MAX: u8 = 16; const BS2_MAX: u8 = 8; const MAX_SAMPLE_POINT_PERMILL: u16 = 900; @@ -316,7 +310,7 @@ impl<'d, T: Instance> Can<'d, T> { // - With rounding to zero let mut bs1 = ((7 * bs1_bs2_sum - 1) + 4) / 8; // Trying rounding to nearest first let mut bs2 = bs1_bs2_sum - bs1; - assert!(bs1_bs2_sum > bs1); + core::assert!(bs1_bs2_sum > bs1); let sample_point_permill = 1000 * ((1 + bs1) / (1 + bs1 + bs2)) as u16; if sample_point_permill > MAX_SAMPLE_POINT_PERMILL { @@ -379,7 +373,7 @@ pub(crate) mod sealed { pub struct State { pub tx_waker: AtomicWaker, pub err_waker: AtomicWaker, - pub rx_queue: Channel, + pub rx_queue: Channel, } impl State { diff --git a/tests/stm32/Cargo.toml b/tests/stm32/Cargo.toml index bd8d90ab..70d3eb13 100644 --- a/tests/stm32/Cargo.toml +++ b/tests/stm32/Cargo.toml @@ -7,7 +7,7 @@ autobins = false [features] stm32f103c8 = ["embassy-stm32/stm32f103c8", "not-gpdma"] # Blue Pill -stm32f429zi = ["embassy-stm32/stm32f429zi", "sdmmc", "chrono", "not-gpdma"] # Nucleo +stm32f429zi = ["embassy-stm32/stm32f429zi", "sdmmc", "chrono", "can", "not-gpdma"] # Nucleo stm32g071rb = ["embassy-stm32/stm32g071rb", "not-gpdma"] # Nucleo stm32c031c6 = ["embassy-stm32/stm32c031c6", "not-gpdma"] # Nucleo stm32g491re = ["embassy-stm32/stm32g491re", "not-gpdma"] # Nucleo @@ -19,6 +19,7 @@ stm32u585ai = ["embassy-stm32/stm32u585ai"] # IoT board sdmmc = [] chrono = ["embassy-stm32/chrono", "dep:chrono"] ble = [] +can = [] not-gpdma = [] [dependencies] @@ -49,6 +50,11 @@ name = "ble" path = "src/bin/ble.rs" required-features = [ "ble",] +[[bin]] +name = "can" +path = "src/bin/can.rs" +required-features = [ "can",] + [[bin]] name = "gpio" path = "src/bin/gpio.rs" diff --git a/tests/stm32/src/bin/can.rs b/tests/stm32/src/bin/can.rs new file mode 100644 index 00000000..f39f83e8 --- /dev/null +++ b/tests/stm32/src/bin/can.rs @@ -0,0 +1,78 @@ +#![no_std] +#![no_main] +#![feature(type_alias_impl_trait)] + +// required-features: can + +#[path = "../example_common.rs"] +mod example_common; +use embassy_executor::Spawner; +use embassy_stm32::bind_interrupts; +use embassy_stm32::can::bxcan::filter::Mask32; +use embassy_stm32::can::bxcan::{Fifo, Frame, StandardId}; +use embassy_stm32::can::{Can, Rx0InterruptHandler, Rx1InterruptHandler, SceInterruptHandler, TxInterruptHandler}; +use embassy_stm32::gpio::{Input, Pull}; +use embassy_stm32::peripherals::CAN1; +use example_common::*; +use {defmt_rtt as _, panic_probe as _}; + +bind_interrupts!(struct Irqs { + CAN1_RX0 => Rx0InterruptHandler; + CAN1_RX1 => Rx1InterruptHandler; + CAN1_SCE => SceInterruptHandler; + CAN1_TX => TxInterruptHandler; +}); + +#[embassy_executor::main] +async fn main(_spawner: Spawner) { + let mut p = embassy_stm32::init(config()); + info!("Hello World!"); + + // HW is connected as follows: + // PB13 -> PD0 + // PB12 -> PD1 + + // The next two lines are a workaround for testing without transceiver. + // To synchronise to the bus the RX input needs to see a high level. + // Use `mem::forget()` to release the borrow on the pin but keep the + // pull-up resistor enabled. + let rx_pin = Input::new(&mut p.PD0, Pull::Up); + core::mem::forget(rx_pin); + + let mut can = Can::new(p.CAN1, p.PD0, p.PD1, Irqs); + + info!("Configuring can..."); + + can.modify_filters().enable_bank(0, Fifo::Fifo0, Mask32::accept_all()); + + can.set_bitrate(1_000_000); + can.modify_config() + .set_loopback(true) // Receive own frames + .set_silent(true) + // .set_bit_timing(0x001c0003) + .enable(); + + info!("Can configured"); + + let mut i: u8 = 0; + loop { + let tx_frame = Frame::new_data(unwrap!(StandardId::new(i as _)), [i]); + + info!("Transmitting frame..."); + can.write(&tx_frame).await; + + info!("Receiving frame..."); + let (time, rx_frame) = can.read().await.unwrap(); + + info!("loopback time {}", time); + info!("loopback frame {=u8}", rx_frame.data().unwrap()[0]); + + i += 1; + if i > 10 { + break; + } + } + + info!("Test OK"); + cortex_m::asm::bkpt(); +} From f6c1108bdf444ba9aef1113ec39c7fc814dab85e Mon Sep 17 00:00:00 2001 From: Philipp Scheff Date: Fri, 16 Jun 2023 14:56:28 +0200 Subject: [PATCH 06/14] fix extended can id --- embassy-stm32/src/can/bxcan.rs | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/embassy-stm32/src/can/bxcan.rs b/embassy-stm32/src/can/bxcan.rs index 08ba783f..1ab4dc1f 100644 --- a/embassy-stm32/src/can/bxcan.rs +++ b/embassy-stm32/src/can/bxcan.rs @@ -226,7 +226,10 @@ impl<'d, T: Instance> Can<'d, T> { let id = if rir.ide() == RirIde::STANDARD { Id::from(StandardId::new_unchecked(rir.stid())) } else { - Id::from(ExtendedId::new_unchecked(rir.exid())) + let stid = (rir.stid() & 0x7FF) as u32; + let exid = rir.exid() & 0x3FFFF; + let id = (stid << 18) | (exid as u32); + Id::from(ExtendedId::new_unchecked(id)) }; let data_len = fifo.rdtr().read().dlc() as usize; let mut data: [u8; 8] = [0; 8]; From bbc81146ecb3832e9ec1531ef6d48fec6c7c0d8c Mon Sep 17 00:00:00 2001 From: Catherine Date: Mon, 19 Jun 2023 09:06:41 +0000 Subject: [PATCH 07/14] BDMA: request stop after busy loop in blocking_wait(). Otherwise the channel cannot be used again, since CR.EN remains set and the DMA channel registers are read-only while it is set. --- embassy-stm32/src/dma/bdma.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/embassy-stm32/src/dma/bdma.rs b/embassy-stm32/src/dma/bdma.rs index c0a503e2..e9b75d86 100644 --- a/embassy-stm32/src/dma/bdma.rs +++ b/embassy-stm32/src/dma/bdma.rs @@ -327,6 +327,7 @@ impl<'a, C: Channel> Transfer<'a, C> { pub fn blocking_wait(mut self) { while self.is_running() {} + self.request_stop(); // "Subsequent reads and writes cannot be moved ahead of preceding reads." fence(Ordering::SeqCst); From 990dd5e5db7134193259fcf350c0928b9a64df97 Mon Sep 17 00:00:00 2001 From: Dario Nieuwenhuis Date: Mon, 19 Jun 2023 22:38:27 +0200 Subject: [PATCH 08/14] tests/stm32: do multiple transfers to catch more bugs. --- tests/stm32/src/bin/usart_dma.rs | 29 ++++++++++++++++------------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/tests/stm32/src/bin/usart_dma.rs b/tests/stm32/src/bin/usart_dma.rs index 50dd2893..c34d9574 100644 --- a/tests/stm32/src/bin/usart_dma.rs +++ b/tests/stm32/src/bin/usart_dma.rs @@ -69,24 +69,27 @@ async fn main(_spawner: Spawner) { const LEN: usize = 128; let mut tx_buf = [0; LEN]; let mut rx_buf = [0; LEN]; - for i in 0..LEN { - tx_buf[i] = i as u8; - } let (mut tx, mut rx) = usart.split(); - let tx_fut = async { - tx.write(&tx_buf).await.unwrap(); - }; - let rx_fut = async { - rx.read(&mut rx_buf).await.unwrap(); - }; + for n in 0..42 { + for i in 0..LEN { + tx_buf[i] = (i ^ n) as u8; + } - // note: rx needs to be polled first, to workaround this bug: - // https://github.com/embassy-rs/embassy/issues/1426 - join(rx_fut, tx_fut).await; + let tx_fut = async { + tx.write(&tx_buf).await.unwrap(); + }; + let rx_fut = async { + rx.read(&mut rx_buf).await.unwrap(); + }; - assert_eq!(tx_buf, rx_buf); + // note: rx needs to be polled first, to workaround this bug: + // https://github.com/embassy-rs/embassy/issues/1426 + join(rx_fut, tx_fut).await; + + assert_eq!(tx_buf, rx_buf); + } info!("Test OK"); cortex_m::asm::bkpt(); From 76659d9003104f8edd2472a36149565e4a55c0e6 Mon Sep 17 00:00:00 2001 From: Ulf Lilleengen Date: Mon, 19 Jun 2023 22:37:23 +0200 Subject: [PATCH 09/14] Prevent accidental revert when using firmware updater This change prevents accidentally overwriting the previous firmware before the new one has been marked as booted. --- .../boot/src/firmware_updater/asynch.rs | 34 +++++++++++++++++-- .../boot/src/firmware_updater/blocking.rs | 32 +++++++++++++++-- embassy-boot/boot/src/firmware_updater/mod.rs | 3 ++ embassy-boot/boot/src/lib.rs | 12 +++++-- 4 files changed, 72 insertions(+), 9 deletions(-) diff --git a/embassy-boot/boot/src/firmware_updater/asynch.rs b/embassy-boot/boot/src/firmware_updater/asynch.rs index 0b3f8831..20731ee0 100644 --- a/embassy-boot/boot/src/firmware_updater/asynch.rs +++ b/embassy-boot/boot/src/firmware_updater/asynch.rs @@ -56,6 +56,16 @@ impl FirmwareUpdater { } } + // Make sure we are running a booted firmware to avoid reverting to a bad state. + async fn verify_booted(&mut self, aligned: &mut [u8]) -> Result<(), FirmwareUpdaterError> { + assert_eq!(aligned.len(), STATE::WRITE_SIZE); + if self.get_state(aligned).await? == State::Boot { + Ok(()) + } else { + Err(FirmwareUpdaterError::BadState) + } + } + /// Obtain the current state. /// /// This is useful to check if the bootloader has just done a swap, in order @@ -98,6 +108,8 @@ impl FirmwareUpdater { assert_eq!(_aligned.len(), STATE::WRITE_SIZE); assert!(_update_len <= self.dfu.capacity() as u32); + self.verify_booted(_aligned).await?; + #[cfg(feature = "ed25519-dalek")] { use ed25519_dalek::{PublicKey, Signature, SignatureError, Verifier}; @@ -217,8 +229,16 @@ impl FirmwareUpdater { /// # Safety /// /// Failing to meet alignment and size requirements may result in a panic. - pub async fn write_firmware(&mut self, offset: usize, data: &[u8]) -> Result<(), FirmwareUpdaterError> { + pub async fn write_firmware( + &mut self, + aligned: &mut [u8], + offset: usize, + data: &[u8], + ) -> Result<(), FirmwareUpdaterError> { assert!(data.len() >= DFU::ERASE_SIZE); + assert_eq!(aligned.len(), STATE::WRITE_SIZE); + + self.verify_booted(aligned).await?; self.dfu.erase(offset as u32, (offset + data.len()) as u32).await?; @@ -232,7 +252,14 @@ impl FirmwareUpdater { /// /// Using this instead of `write_firmware` allows for an optimized API in /// exchange for added complexity. - pub async fn prepare_update(&mut self) -> Result<&mut DFU, FirmwareUpdaterError> { + /// + /// # Safety + /// + /// The `aligned` buffer must have a size of STATE::WRITE_SIZE, and follow the alignment rules for the flash being written to. + pub async fn prepare_update(&mut self, aligned: &mut [u8]) -> Result<&mut DFU, FirmwareUpdaterError> { + assert_eq!(aligned.len(), STATE::WRITE_SIZE); + self.verify_booted(aligned).await?; + self.dfu.erase(0, self.dfu.capacity() as u32).await?; Ok(&mut self.dfu) @@ -255,13 +282,14 @@ mod tests { let flash = Mutex::::new(MemFlash::<131072, 4096, 8>::default()); let state = Partition::new(&flash, 0, 4096); let dfu = Partition::new(&flash, 65536, 65536); + let mut aligned = [0; 8]; let update = [0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66]; let mut to_write = [0; 4096]; to_write[..7].copy_from_slice(update.as_slice()); let mut updater = FirmwareUpdater::new(FirmwareUpdaterConfig { dfu, state }); - block_on(updater.write_firmware(0, to_write.as_slice())).unwrap(); + block_on(updater.write_firmware(&mut aligned, 0, to_write.as_slice())).unwrap(); let mut chunk_buf = [0; 2]; let mut hash = [0; 20]; block_on(updater.hash::(update.len() as u32, &mut chunk_buf, &mut hash)).unwrap(); diff --git a/embassy-boot/boot/src/firmware_updater/blocking.rs b/embassy-boot/boot/src/firmware_updater/blocking.rs index 551150c4..f03f53e4 100644 --- a/embassy-boot/boot/src/firmware_updater/blocking.rs +++ b/embassy-boot/boot/src/firmware_updater/blocking.rs @@ -58,6 +58,16 @@ impl BlockingFirmwareUpdater { } } + // Make sure we are running a booted firmware to avoid reverting to a bad state. + fn verify_booted(&mut self, aligned: &mut [u8]) -> Result<(), FirmwareUpdaterError> { + assert_eq!(aligned.len(), STATE::WRITE_SIZE); + if self.get_state(aligned)? == State::Boot { + Ok(()) + } else { + Err(FirmwareUpdaterError::BadState) + } + } + /// Obtain the current state. /// /// This is useful to check if the bootloader has just done a swap, in order @@ -100,6 +110,8 @@ impl BlockingFirmwareUpdater { assert_eq!(_aligned.len(), STATE::WRITE_SIZE); assert!(_update_len <= self.dfu.capacity() as u32); + self.verify_booted(_aligned)?; + #[cfg(feature = "ed25519-dalek")] { use ed25519_dalek::{PublicKey, Signature, SignatureError, Verifier}; @@ -219,8 +231,15 @@ impl BlockingFirmwareUpdater { /// # Safety /// /// Failing to meet alignment and size requirements may result in a panic. - pub fn write_firmware(&mut self, offset: usize, data: &[u8]) -> Result<(), FirmwareUpdaterError> { + pub fn write_firmware( + &mut self, + aligned: &mut [u8], + offset: usize, + data: &[u8], + ) -> Result<(), FirmwareUpdaterError> { assert!(data.len() >= DFU::ERASE_SIZE); + assert_eq!(aligned.len(), STATE::WRITE_SIZE); + self.verify_booted(aligned)?; self.dfu.erase(offset as u32, (offset + data.len()) as u32)?; @@ -234,7 +253,13 @@ impl BlockingFirmwareUpdater { /// /// Using this instead of `write_firmware` allows for an optimized API in /// exchange for added complexity. - pub fn prepare_update(&mut self) -> Result<&mut DFU, FirmwareUpdaterError> { + /// + /// # Safety + /// + /// The `aligned` buffer must have a size of STATE::WRITE_SIZE, and follow the alignment rules for the flash being written to. + pub fn prepare_update(&mut self, aligned: &mut [u8]) -> Result<&mut DFU, FirmwareUpdaterError> { + assert_eq!(aligned.len(), STATE::WRITE_SIZE); + self.verify_booted(aligned)?; self.dfu.erase(0, self.dfu.capacity() as u32)?; Ok(&mut self.dfu) @@ -264,7 +289,8 @@ mod tests { to_write[..7].copy_from_slice(update.as_slice()); let mut updater = BlockingFirmwareUpdater::new(FirmwareUpdaterConfig { dfu, state }); - updater.write_firmware(0, to_write.as_slice()).unwrap(); + let mut aligned = [0; 8]; + updater.write_firmware(&mut aligned, 0, to_write.as_slice()).unwrap(); let mut chunk_buf = [0; 2]; let mut hash = [0; 20]; updater diff --git a/embassy-boot/boot/src/firmware_updater/mod.rs b/embassy-boot/boot/src/firmware_updater/mod.rs index a37984a3..55ce8f36 100644 --- a/embassy-boot/boot/src/firmware_updater/mod.rs +++ b/embassy-boot/boot/src/firmware_updater/mod.rs @@ -26,6 +26,8 @@ pub enum FirmwareUpdaterError { Flash(NorFlashErrorKind), /// Signature errors. Signature(signature::Error), + /// Bad state. + BadState, } #[cfg(feature = "defmt")] @@ -34,6 +36,7 @@ impl defmt::Format for FirmwareUpdaterError { match self { FirmwareUpdaterError::Flash(_) => defmt::write!(fmt, "FirmwareUpdaterError::Flash(_)"), FirmwareUpdaterError::Signature(_) => defmt::write!(fmt, "FirmwareUpdaterError::Signature(_)"), + FirmwareUpdaterError::BadState => defmt::write!(fmt, "FirmwareUpdaterError::BadState"), } } } diff --git a/embassy-boot/boot/src/lib.rs b/embassy-boot/boot/src/lib.rs index 45a87bd0..016362b8 100644 --- a/embassy-boot/boot/src/lib.rs +++ b/embassy-boot/boot/src/lib.rs @@ -51,6 +51,8 @@ impl AsMut<[u8]> for AlignedBuffer { #[cfg(test)] mod tests { + #![allow(unused_imports)] + use embedded_storage::nor_flash::{NorFlash, ReadNorFlash}; #[cfg(feature = "nightly")] use embedded_storage_async::nor_flash::NorFlash as AsyncNorFlash; @@ -120,9 +122,13 @@ mod tests { dfu: flash.dfu(), state: flash.state(), }); - block_on(updater.write_firmware(0, &UPDATE)).unwrap(); + block_on(updater.write_firmware(&mut aligned, 0, &UPDATE)).unwrap(); block_on(updater.mark_updated(&mut aligned)).unwrap(); + // Writing after marking updated is not allowed until marked as booted. + let res: Result<(), FirmwareUpdaterError> = block_on(updater.write_firmware(&mut aligned, 0, &UPDATE)); + assert!(matches!(res, Err::<(), _>(FirmwareUpdaterError::BadState))); + let flash = flash.into_blocking(); let mut bootloader = BootLoader::new(BootLoaderConfig { active: flash.active(), @@ -188,7 +194,7 @@ mod tests { dfu: flash.dfu(), state: flash.state(), }); - block_on(updater.write_firmware(0, &UPDATE)).unwrap(); + block_on(updater.write_firmware(&mut aligned, 0, &UPDATE)).unwrap(); block_on(updater.mark_updated(&mut aligned)).unwrap(); let flash = flash.into_blocking(); @@ -230,7 +236,7 @@ mod tests { dfu: flash.dfu(), state: flash.state(), }); - block_on(updater.write_firmware(0, &UPDATE)).unwrap(); + block_on(updater.write_firmware(&mut aligned, 0, &UPDATE)).unwrap(); block_on(updater.mark_updated(&mut aligned)).unwrap(); let flash = flash.into_blocking(); From 428a4ba3f98d81de28f8f72115ed6eff5401b46a Mon Sep 17 00:00:00 2001 From: Dario Nieuwenhuis Date: Mon, 19 Jun 2023 22:39:08 +0200 Subject: [PATCH 10/14] stm32/gpdma: clear all interrupts after reset. Reset doesn't clear them, this causes subsequent transfers to instantly complete because the TC flag was set from before. --- embassy-stm32/src/dma/gpdma.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/embassy-stm32/src/dma/gpdma.rs b/embassy-stm32/src/dma/gpdma.rs index c600df92..b7bcf779 100644 --- a/embassy-stm32/src/dma/gpdma.rs +++ b/embassy-stm32/src/dma/gpdma.rs @@ -252,6 +252,7 @@ impl<'a, C: Channel> Transfer<'a, C> { super::dmamux::configure_dmamux(&mut *this.channel, request); ch.cr().write(|w| w.set_reset(true)); + ch.fcr().write(|w| w.0 = 0xFFFF_FFFF); // clear all irqs ch.llr().write(|_| {}); // no linked list ch.tr1().write(|w| { w.set_sdw(data_size.into()); From 09982214788def209316ef70cae08eead964e206 Mon Sep 17 00:00:00 2001 From: xoviat Date: Mon, 19 Jun 2023 16:05:59 -0500 Subject: [PATCH 11/14] stm32/can: update interrupts --- embassy-stm32/src/can/bxcan.rs | 59 ++++++++++++++++----------------- examples/stm32f4/src/bin/can.rs | 13 ++++++-- 2 files changed, 40 insertions(+), 32 deletions(-) diff --git a/embassy-stm32/src/can/bxcan.rs b/embassy-stm32/src/can/bxcan.rs index 9cd40fd8..e23ce686 100644 --- a/embassy-stm32/src/can/bxcan.rs +++ b/embassy-stm32/src/can/bxcan.rs @@ -5,12 +5,11 @@ use core::task::Poll; pub use bxcan; use bxcan::{Data, ExtendedId, Frame, Id, StandardId}; -use embassy_cortex_m::interrupt::Interrupt; use embassy_hal_common::{into_ref, PeripheralRef}; use futures::FutureExt; use crate::gpio::sealed::AFType; -use crate::interrupt::InterruptExt; +use crate::interrupt::typelevel::Interrupt; use crate::pac::can::vals::{Lec, RirIde}; use crate::rcc::RccPeripheral; use crate::time::Hertz; @@ -21,7 +20,7 @@ pub struct TxInterruptHandler { _phantom: PhantomData, } -impl interrupt::Handler for TxInterruptHandler { +impl interrupt::typelevel::Handler for TxInterruptHandler { unsafe fn on_interrupt() { T::regs().tsr().write(|v| { v.set_rqcp(0, true); @@ -37,7 +36,7 @@ pub struct Rx0InterruptHandler { _phantom: PhantomData, } -impl interrupt::Handler for Rx0InterruptHandler { +impl interrupt::typelevel::Handler for Rx0InterruptHandler { unsafe fn on_interrupt() { // info!("rx0 irq"); Can::::receive_fifo(RxFifo::Fifo0); @@ -48,7 +47,7 @@ pub struct Rx1InterruptHandler { _phantom: PhantomData, } -impl interrupt::Handler for Rx1InterruptHandler { +impl interrupt::typelevel::Handler for Rx1InterruptHandler { unsafe fn on_interrupt() { // info!("rx1 irq"); Can::::receive_fifo(RxFifo::Fifo1); @@ -59,7 +58,7 @@ pub struct SceInterruptHandler { _phantom: PhantomData, } -impl interrupt::Handler for SceInterruptHandler { +impl interrupt::typelevel::Handler for SceInterruptHandler { unsafe fn on_interrupt() { // info!("sce irq"); let msr = T::regs().msr(); @@ -97,10 +96,10 @@ impl<'d, T: Instance> Can<'d, T> { peri: impl Peripheral

+ 'd, rx: impl Peripheral

> + 'd, tx: impl Peripheral

> + 'd, - _irqs: impl interrupt::Binding> - + interrupt::Binding> - + interrupt::Binding> - + interrupt::Binding> + _irqs: impl interrupt::typelevel::Binding> + + interrupt::typelevel::Binding> + + interrupt::typelevel::Binding> + + interrupt::typelevel::Binding> + 'd, ) -> Self { into_ref!(peri, rx, tx); @@ -111,7 +110,7 @@ impl<'d, T: Instance> Can<'d, T> { T::enable(); T::reset(); - unsafe { + { use crate::pac::can::vals::{Errie, Fmpie, Tmeie}; T::regs().ier().write(|w| { @@ -127,21 +126,21 @@ impl<'d, T: Instance> Can<'d, T> { // Enable timestamps on rx messages w.set_ttcm(true); - }) + }); } unsafe { - T::TXInterrupt::steal().unpend(); - T::TXInterrupt::steal().enable(); + T::TXInterrupt::unpend(); + T::TXInterrupt::enable(); - T::RX0Interrupt::steal().unpend(); - T::RX0Interrupt::steal().enable(); + T::RX0Interrupt::unpend(); + T::RX0Interrupt::enable(); - T::RX1Interrupt::steal().unpend(); - T::RX1Interrupt::steal().enable(); + T::RX1Interrupt::unpend(); + T::RX1Interrupt::enable(); - T::SCEInterrupt::steal().unpend(); - T::SCEInterrupt::steal().enable(); + T::SCEInterrupt::unpend(); + T::SCEInterrupt::enable(); } rx.set_as_af(rx.af_num(), AFType::Input); @@ -169,7 +168,7 @@ impl<'d, T: Instance> Can<'d, T> { } pub async fn flush(&self, mb: bxcan::Mailbox) { - poll_fn(|cx| unsafe { + poll_fn(|cx| { if T::regs().tsr().read().tme(mb.index()) { return Poll::Ready(()); } @@ -194,7 +193,7 @@ impl<'d, T: Instance> Can<'d, T> { } fn curr_error(&self) -> Option { - let err = unsafe { T::regs().esr().read() }; + let err = { T::regs().esr().read() }; if err.boff() { return Some(BusError::BusOff); } else if err.epvf() { @@ -396,19 +395,19 @@ pub(crate) mod sealed { } pub trait TXInstance { - type TXInterrupt: crate::interrupt::Interrupt; + type TXInterrupt: crate::interrupt::typelevel::Interrupt; } pub trait RX0Instance { - type RX0Interrupt: crate::interrupt::Interrupt; + type RX0Interrupt: crate::interrupt::typelevel::Interrupt; } pub trait RX1Instance { - type RX1Interrupt: crate::interrupt::Interrupt; + type RX1Interrupt: crate::interrupt::typelevel::Interrupt; } pub trait SCEInstance { - type SCEInterrupt: crate::interrupt::Interrupt; + type SCEInterrupt: crate::interrupt::typelevel::Interrupt; } pub trait InterruptableInstance: TXInstance + RX0Instance + RX1Instance + SCEInstance {} @@ -440,22 +439,22 @@ foreach_peripheral!( foreach_interrupt!( ($inst,can,CAN,TX,$irq:ident) => { impl TXInstance for peripherals::$inst { - type TXInterrupt = crate::interrupt::$irq; + type TXInterrupt = crate::interrupt::typelevel::$irq; } }; ($inst,can,CAN,RX0,$irq:ident) => { impl RX0Instance for peripherals::$inst { - type RX0Interrupt = crate::interrupt::$irq; + type RX0Interrupt = crate::interrupt::typelevel::$irq; } }; ($inst,can,CAN,RX1,$irq:ident) => { impl RX1Instance for peripherals::$inst { - type RX1Interrupt = crate::interrupt::$irq; + type RX1Interrupt = crate::interrupt::typelevel::$irq; } }; ($inst,can,CAN,SCE,$irq:ident) => { impl SCEInstance for peripherals::$inst { - type SCEInterrupt = crate::interrupt::$irq; + type SCEInterrupt = crate::interrupt::typelevel::$irq; } }; ); diff --git a/examples/stm32f4/src/bin/can.rs b/examples/stm32f4/src/bin/can.rs index e8377b9a..da895505 100644 --- a/examples/stm32f4/src/bin/can.rs +++ b/examples/stm32f4/src/bin/can.rs @@ -4,12 +4,21 @@ use cortex_m_rt::entry; use defmt::*; +use embassy_stm32::bind_interrupts; use embassy_stm32::can::bxcan::filter::Mask32; use embassy_stm32::can::bxcan::{Fifo, Frame, StandardId}; -use embassy_stm32::can::Can; +use embassy_stm32::can::{Can, Rx0InterruptHandler, Rx1InterruptHandler, SceInterruptHandler, TxInterruptHandler}; use embassy_stm32::gpio::{Input, Pull}; +use embassy_stm32::peripherals::CAN1; use {defmt_rtt as _, panic_probe as _}; +bind_interrupts!(struct Irqs { + CAN1_RX0 => Rx0InterruptHandler; + CAN1_RX1 => Rx1InterruptHandler; + CAN1_SCE => SceInterruptHandler; + CAN1_TX => TxInterruptHandler; +}); + #[entry] fn main() -> ! { info!("Hello World!"); @@ -23,7 +32,7 @@ fn main() -> ! { let rx_pin = Input::new(&mut p.PA11, Pull::Up); core::mem::forget(rx_pin); - let mut can = Can::new(p.CAN1, p.PA11, p.PA12); + let mut can = Can::new(p.CAN1, p.PA11, p.PA12, Irqs); can.modify_filters().enable_bank(0, Fifo::Fifo0, Mask32::accept_all()); From 5a075acc6a91f9eb05d065ee31551a4695cc4cdf Mon Sep 17 00:00:00 2001 From: xoviat Date: Mon, 19 Jun 2023 16:11:01 -0500 Subject: [PATCH 12/14] stm32/tests: fix can --- tests/stm32/src/bin/can.rs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tests/stm32/src/bin/can.rs b/tests/stm32/src/bin/can.rs index f39f83e8..33d63d54 100644 --- a/tests/stm32/src/bin/can.rs +++ b/tests/stm32/src/bin/can.rs @@ -4,8 +4,9 @@ // required-features: can -#[path = "../example_common.rs"] -mod example_common; +#[path = "../common.rs"] +mod common; +use common::*; use embassy_executor::Spawner; use embassy_stm32::bind_interrupts; use embassy_stm32::can::bxcan::filter::Mask32; @@ -13,7 +14,6 @@ use embassy_stm32::can::bxcan::{Fifo, Frame, StandardId}; use embassy_stm32::can::{Can, Rx0InterruptHandler, Rx1InterruptHandler, SceInterruptHandler, TxInterruptHandler}; use embassy_stm32::gpio::{Input, Pull}; use embassy_stm32::peripherals::CAN1; -use example_common::*; use {defmt_rtt as _, panic_probe as _}; bind_interrupts!(struct Irqs { From 161d3ce05c812f7ee951b6265735187b4994037a Mon Sep 17 00:00:00 2001 From: Ulf Lilleengen Date: Mon, 19 Jun 2023 23:30:51 +0200 Subject: [PATCH 13/14] Add firmware updater examples to CI CI was not building the a.rs application due to the requirement of b.bin having been built first. Add a feature flag to examples so that CI can build them including a dummy application. Update a.rs application examples so that they compile again. --- ci.sh | 20 +++++++++---------- examples/boot/application/nrf/Cargo.toml | 1 + examples/boot/application/nrf/src/bin/a.rs | 7 +++++-- examples/boot/application/rp/Cargo.toml | 1 + examples/boot/application/rp/src/bin/a.rs | 6 +++++- examples/boot/application/stm32f3/Cargo.toml | 1 + .../boot/application/stm32f3/src/bin/a.rs | 7 +++++-- examples/boot/application/stm32f7/Cargo.toml | 2 ++ .../boot/application/stm32f7/src/bin/a.rs | 13 +++++++++--- examples/boot/application/stm32h7/Cargo.toml | 2 ++ .../boot/application/stm32h7/src/bin/a.rs | 13 +++++++++--- examples/boot/application/stm32l0/Cargo.toml | 1 + .../boot/application/stm32l0/src/bin/a.rs | 17 ++++++++++------ examples/boot/application/stm32l1/Cargo.toml | 1 + .../boot/application/stm32l1/src/bin/a.rs | 8 ++++++-- examples/boot/application/stm32l4/Cargo.toml | 1 + .../boot/application/stm32l4/src/bin/a.rs | 10 +++++++--- examples/boot/application/stm32wl/Cargo.toml | 1 + .../boot/application/stm32wl/src/bin/a.rs | 12 +++++++---- 19 files changed, 88 insertions(+), 36 deletions(-) diff --git a/ci.sh b/ci.sh index 760d6fbf..3fe1b1ce 100755 --- a/ci.sh +++ b/ci.sh @@ -133,16 +133,16 @@ cargo batch \ --- build --release --manifest-path examples/stm32u5/Cargo.toml --target thumbv8m.main-none-eabihf --out-dir out/examples/stm32u5 \ --- build --release --manifest-path examples/stm32wb/Cargo.toml --target thumbv7em-none-eabihf --out-dir out/examples/stm32wb \ --- build --release --manifest-path examples/stm32wl/Cargo.toml --target thumbv7em-none-eabihf --out-dir out/examples/stm32wl \ - --- build --release --manifest-path examples/boot/application/nrf/Cargo.toml --target thumbv7em-none-eabi --features embassy-nrf/nrf52840 --out-dir out/examples/boot/nrf --bin b \ - --- build --release --manifest-path examples/boot/application/nrf/Cargo.toml --target thumbv8m.main-none-eabihf --features embassy-nrf/nrf9160-ns --out-dir out/examples/boot/nrf --bin b \ - --- build --release --manifest-path examples/boot/application/rp/Cargo.toml --target thumbv6m-none-eabi --out-dir out/examples/boot/rp --bin b \ - --- build --release --manifest-path examples/boot/application/stm32f3/Cargo.toml --target thumbv7em-none-eabi --out-dir out/examples/boot/stm32f3 --bin b \ - --- build --release --manifest-path examples/boot/application/stm32f7/Cargo.toml --target thumbv7em-none-eabi --out-dir out/examples/boot/stm32f7 --bin b \ - --- build --release --manifest-path examples/boot/application/stm32h7/Cargo.toml --target thumbv7em-none-eabi --out-dir out/examples/boot/stm32h7 --bin b \ - --- build --release --manifest-path examples/boot/application/stm32l0/Cargo.toml --target thumbv6m-none-eabi --out-dir out/examples/boot/stm32l0 --bin b \ - --- build --release --manifest-path examples/boot/application/stm32l1/Cargo.toml --target thumbv7m-none-eabi --out-dir out/examples/boot/stm32l1 --bin b \ - --- build --release --manifest-path examples/boot/application/stm32l4/Cargo.toml --target thumbv7em-none-eabi --out-dir out/examples/boot/stm32l4 --bin b \ - --- build --release --manifest-path examples/boot/application/stm32wl/Cargo.toml --target thumbv7em-none-eabihf --out-dir out/examples/boot/stm32wl --bin b \ + --- build --release --manifest-path examples/boot/application/nrf/Cargo.toml --target thumbv7em-none-eabi --features embassy-nrf/nrf52840,skip-include --out-dir out/examples/boot/nrf \ + --- build --release --manifest-path examples/boot/application/nrf/Cargo.toml --target thumbv8m.main-none-eabihf --features embassy-nrf/nrf9160-ns,skip-include --out-dir out/examples/boot/nrf \ + --- build --release --manifest-path examples/boot/application/rp/Cargo.toml --target thumbv6m-none-eabi --features skip-include --out-dir out/examples/boot/rp \ + --- build --release --manifest-path examples/boot/application/stm32f3/Cargo.toml --target thumbv7em-none-eabi --features skip-include --out-dir out/examples/boot/stm32f3 \ + --- build --release --manifest-path examples/boot/application/stm32f7/Cargo.toml --target thumbv7em-none-eabi --features skip-include --out-dir out/examples/boot/stm32f7 \ + --- build --release --manifest-path examples/boot/application/stm32h7/Cargo.toml --target thumbv7em-none-eabi --features skip-include --out-dir out/examples/boot/stm32h7 \ + --- build --release --manifest-path examples/boot/application/stm32l0/Cargo.toml --target thumbv6m-none-eabi --features skip-include --out-dir out/examples/boot/stm32l0 \ + --- build --release --manifest-path examples/boot/application/stm32l1/Cargo.toml --target thumbv7m-none-eabi --features skip-include --out-dir out/examples/boot/stm32l1 \ + --- build --release --manifest-path examples/boot/application/stm32l4/Cargo.toml --target thumbv7em-none-eabi --features skip-include --out-dir out/examples/boot/stm32l4 \ + --- build --release --manifest-path examples/boot/application/stm32wl/Cargo.toml --target thumbv7em-none-eabihf --features skip-include --out-dir out/examples/boot/stm32wl \ --- build --release --manifest-path examples/boot/bootloader/nrf/Cargo.toml --target thumbv7em-none-eabi --features embassy-nrf/nrf52840 \ --- build --release --manifest-path examples/boot/bootloader/nrf/Cargo.toml --target thumbv8m.main-none-eabihf --features embassy-nrf/nrf9160-ns \ --- build --release --manifest-path examples/boot/bootloader/rp/Cargo.toml --target thumbv6m-none-eabi \ diff --git a/examples/boot/application/nrf/Cargo.toml b/examples/boot/application/nrf/Cargo.toml index 5939a43b..b98f73f3 100644 --- a/examples/boot/application/nrf/Cargo.toml +++ b/examples/boot/application/nrf/Cargo.toml @@ -24,3 +24,4 @@ cortex-m-rt = "0.7.0" [features] ed25519-dalek = ["embassy-boot/ed25519-dalek"] ed25519-salty = ["embassy-boot/ed25519-salty"] +skip-include = [] diff --git a/examples/boot/application/nrf/src/bin/a.rs b/examples/boot/application/nrf/src/bin/a.rs index 06c23778..021d77f3 100644 --- a/examples/boot/application/nrf/src/bin/a.rs +++ b/examples/boot/application/nrf/src/bin/a.rs @@ -12,6 +12,9 @@ use embassy_nrf::wdt::{self, Watchdog}; use embassy_sync::mutex::Mutex; use panic_reset as _; +#[cfg(feature = "skip-include")] +static APP_B: &[u8] = &[0, 1, 2, 3]; +#[cfg(not(feature = "skip-include"))] static APP_B: &[u8] = include_bytes!("../../b.bin"); #[embassy_executor::main] @@ -55,13 +58,13 @@ async fn main(_spawner: Spawner) { button.wait_for_any_edge().await; if button.is_low() { let mut offset = 0; + let mut magic = [0; 4]; for chunk in APP_B.chunks(4096) { let mut buf: [u8; 4096] = [0; 4096]; buf[..chunk.len()].copy_from_slice(chunk); - updater.write_firmware(offset, &buf).await.unwrap(); + updater.write_firmware(&mut magic, offset, &buf).await.unwrap(); offset += chunk.len(); } - let mut magic = [0; 4]; updater.mark_updated(&mut magic).await.unwrap(); led.set_high(); cortex_m::peripheral::SCB::sys_reset(); diff --git a/examples/boot/application/rp/Cargo.toml b/examples/boot/application/rp/Cargo.toml index 4a2c5dd8..007b6839 100644 --- a/examples/boot/application/rp/Cargo.toml +++ b/examples/boot/application/rp/Cargo.toml @@ -29,6 +29,7 @@ debug = [ "embassy-boot-rp/defmt", "panic-probe" ] +skip-include = [] [profile.release] debug = true diff --git a/examples/boot/application/rp/src/bin/a.rs b/examples/boot/application/rp/src/bin/a.rs index 69850069..c8497494 100644 --- a/examples/boot/application/rp/src/bin/a.rs +++ b/examples/boot/application/rp/src/bin/a.rs @@ -18,7 +18,11 @@ use panic_probe as _; #[cfg(feature = "panic-reset")] use panic_reset as _; +#[cfg(feature = "skip-include")] +static APP_B: &[u8] = &[0, 1, 2, 3]; +#[cfg(not(feature = "skip-include"))] static APP_B: &[u8] = include_bytes!("../../b.bin"); + const FLASH_SIZE: usize = 2 * 1024 * 1024; #[embassy_executor::main] @@ -43,7 +47,7 @@ async fn main(_s: Spawner) { let mut buf: AlignedBuffer<4096> = AlignedBuffer([0; 4096]); defmt::info!("preparing update"); let writer = updater - .prepare_update() + .prepare_update(&mut buf.0[..1]) .map_err(|e| defmt::warn!("E: {:?}", defmt::Debug2Format(&e))) .unwrap(); defmt::info!("writer created, starting write"); diff --git a/examples/boot/application/stm32f3/Cargo.toml b/examples/boot/application/stm32f3/Cargo.toml index 24abd90d..5b3faf8f 100644 --- a/examples/boot/application/stm32f3/Cargo.toml +++ b/examples/boot/application/stm32f3/Cargo.toml @@ -26,3 +26,4 @@ defmt = [ "embassy-stm32/defmt", "embassy-boot-stm32/defmt", ] +skip-include = [] diff --git a/examples/boot/application/stm32f3/src/bin/a.rs b/examples/boot/application/stm32f3/src/bin/a.rs index c94676f0..c0a11d69 100644 --- a/examples/boot/application/stm32f3/src/bin/a.rs +++ b/examples/boot/application/stm32f3/src/bin/a.rs @@ -13,6 +13,9 @@ use embassy_stm32::gpio::{Input, Level, Output, Pull, Speed}; use embassy_sync::mutex::Mutex; use panic_reset as _; +#[cfg(feature = "skip-include")] +static APP_B: &[u8] = &[0, 1, 2, 3]; +#[cfg(not(feature = "skip-include"))] static APP_B: &[u8] = include_bytes!("../../b.bin"); #[embassy_executor::main] @@ -31,13 +34,13 @@ async fn main(_spawner: Spawner) { let mut updater = FirmwareUpdater::new(config); button.wait_for_falling_edge().await; let mut offset = 0; + let mut magic = AlignedBuffer([0; WRITE_SIZE]); for chunk in APP_B.chunks(2048) { let mut buf: [u8; 2048] = [0; 2048]; buf[..chunk.len()].copy_from_slice(chunk); - updater.write_firmware(offset, &buf).await.unwrap(); + updater.write_firmware(magic.as_mut(), offset, &buf).await.unwrap(); offset += chunk.len(); } - let mut magic = AlignedBuffer([0; WRITE_SIZE]); updater.mark_updated(magic.as_mut()).await.unwrap(); led.set_low(); cortex_m::peripheral::SCB::sys_reset(); diff --git a/examples/boot/application/stm32f7/Cargo.toml b/examples/boot/application/stm32f7/Cargo.toml index 529a01aa..b6a6f9cd 100644 --- a/examples/boot/application/stm32f7/Cargo.toml +++ b/examples/boot/application/stm32f7/Cargo.toml @@ -16,6 +16,7 @@ defmt = { version = "0.3", optional = true } defmt-rtt = { version = "0.4", optional = true } panic-reset = { version = "0.1.1" } embedded-hal = { version = "0.2.6" } +embedded-storage = "0.3.0" cortex-m = { version = "0.7.6", features = ["inline-asm", "critical-section-single-core"] } cortex-m-rt = "0.7.0" @@ -26,3 +27,4 @@ defmt = [ "embassy-stm32/defmt", "embassy-boot-stm32/defmt", ] +skip-include = [] diff --git a/examples/boot/application/stm32f7/src/bin/a.rs b/examples/boot/application/stm32f7/src/bin/a.rs index fc2702c9..dea682a9 100644 --- a/examples/boot/application/stm32f7/src/bin/a.rs +++ b/examples/boot/application/stm32f7/src/bin/a.rs @@ -2,6 +2,8 @@ #![no_main] #![feature(type_alias_impl_trait)] +use core::cell::RefCell; + #[cfg(feature = "defmt-rtt")] use defmt_rtt::*; use embassy_boot_stm32::{AlignedBuffer, BlockingFirmwareUpdater, FirmwareUpdaterConfig}; @@ -9,8 +11,13 @@ use embassy_executor::Spawner; use embassy_stm32::exti::ExtiInput; use embassy_stm32::flash::{Flash, WRITE_SIZE}; use embassy_stm32::gpio::{Input, Level, Output, Pull, Speed}; +use embassy_sync::blocking_mutex::Mutex; +use embedded_storage::nor_flash::NorFlash; use panic_reset as _; +#[cfg(feature = "skip-include")] +static APP_B: &[u8] = &[0, 1, 2, 3]; +#[cfg(not(feature = "skip-include"))] static APP_B: &[u8] = include_bytes!("../../b.bin"); #[embassy_executor::main] @@ -27,16 +34,16 @@ async fn main(_spawner: Spawner) { let config = FirmwareUpdaterConfig::from_linkerfile_blocking(&flash); let mut updater = BlockingFirmwareUpdater::new(config); - let mut writer = updater.prepare_update().unwrap(); + let mut magic = AlignedBuffer([0; WRITE_SIZE]); + let writer = updater.prepare_update(magic.as_mut()).unwrap(); button.wait_for_rising_edge().await; let mut offset = 0; let mut buf = AlignedBuffer([0; 4096]); for chunk in APP_B.chunks(4096) { buf.as_mut()[..chunk.len()].copy_from_slice(chunk); writer.write(offset, buf.as_ref()).unwrap(); - offset += chunk.len(); + offset += chunk.len() as u32; } - let mut magic = AlignedBuffer([0; WRITE_SIZE]); updater.mark_updated(magic.as_mut()).unwrap(); led.set_low(); cortex_m::peripheral::SCB::sys_reset(); diff --git a/examples/boot/application/stm32h7/Cargo.toml b/examples/boot/application/stm32h7/Cargo.toml index d7539a53..0a7e19b1 100644 --- a/examples/boot/application/stm32h7/Cargo.toml +++ b/examples/boot/application/stm32h7/Cargo.toml @@ -16,6 +16,7 @@ defmt = { version = "0.3", optional = true } defmt-rtt = { version = "0.4", optional = true } panic-reset = { version = "0.1.1" } embedded-hal = { version = "0.2.6" } +embedded-storage = "0.3.0" cortex-m = { version = "0.7.6", features = ["inline-asm", "critical-section-single-core"] } cortex-m-rt = "0.7.0" @@ -26,3 +27,4 @@ defmt = [ "embassy-stm32/defmt", "embassy-boot-stm32/defmt", ] +skip-include = [] diff --git a/examples/boot/application/stm32h7/src/bin/a.rs b/examples/boot/application/stm32h7/src/bin/a.rs index 1a54464d..71917669 100644 --- a/examples/boot/application/stm32h7/src/bin/a.rs +++ b/examples/boot/application/stm32h7/src/bin/a.rs @@ -2,6 +2,8 @@ #![no_main] #![feature(type_alias_impl_trait)] +use core::cell::RefCell; + #[cfg(feature = "defmt-rtt")] use defmt_rtt::*; use embassy_boot_stm32::{AlignedBuffer, BlockingFirmwareUpdater, FirmwareUpdaterConfig}; @@ -9,8 +11,13 @@ use embassy_executor::Spawner; use embassy_stm32::exti::ExtiInput; use embassy_stm32::flash::{Flash, WRITE_SIZE}; use embassy_stm32::gpio::{Input, Level, Output, Pull, Speed}; +use embassy_sync::blocking_mutex::Mutex; +use embedded_storage::nor_flash::NorFlash; use panic_reset as _; +#[cfg(feature = "skip-include")] +static APP_B: &[u8] = &[0, 1, 2, 3]; +#[cfg(not(feature = "skip-include"))] static APP_B: &[u8] = include_bytes!("../../b.bin"); #[embassy_executor::main] @@ -26,17 +33,17 @@ async fn main(_spawner: Spawner) { led.set_high(); let config = FirmwareUpdaterConfig::from_linkerfile_blocking(&flash); + let mut magic = AlignedBuffer([0; WRITE_SIZE]); let mut updater = BlockingFirmwareUpdater::new(config); - let mut writer = updater.prepare_update().unwrap(); + let writer = updater.prepare_update(magic.as_mut()).unwrap(); button.wait_for_rising_edge().await; let mut offset = 0; let mut buf = AlignedBuffer([0; 4096]); for chunk in APP_B.chunks(4096) { buf.as_mut()[..chunk.len()].copy_from_slice(chunk); writer.write(offset, buf.as_ref()).unwrap(); - offset += chunk.len(); + offset += chunk.len() as u32; } - let mut magic = AlignedBuffer([0; WRITE_SIZE]); updater.mark_updated(magic.as_mut()).unwrap(); led.set_low(); cortex_m::peripheral::SCB::sys_reset(); diff --git a/examples/boot/application/stm32l0/Cargo.toml b/examples/boot/application/stm32l0/Cargo.toml index e90da259..998df4dc 100644 --- a/examples/boot/application/stm32l0/Cargo.toml +++ b/examples/boot/application/stm32l0/Cargo.toml @@ -26,3 +26,4 @@ defmt = [ "embassy-stm32/defmt", "embassy-boot-stm32/defmt", ] +skip-include = [] diff --git a/examples/boot/application/stm32l0/src/bin/a.rs b/examples/boot/application/stm32l0/src/bin/a.rs index 4033ac59..ce80056e 100644 --- a/examples/boot/application/stm32l0/src/bin/a.rs +++ b/examples/boot/application/stm32l0/src/bin/a.rs @@ -4,22 +4,26 @@ #[cfg(feature = "defmt-rtt")] use defmt_rtt::*; -use embassy_boot_stm32::{AlignedBuffer, FirmwareUpdater}; +use embassy_boot_stm32::{AlignedBuffer, FirmwareUpdater, FirmwareUpdaterConfig}; use embassy_embedded_hal::adapter::BlockingAsync; use embassy_executor::Spawner; use embassy_stm32::exti::ExtiInput; use embassy_stm32::flash::{Flash, WRITE_SIZE}; use embassy_stm32::gpio::{Input, Level, Output, Pull, Speed}; +use embassy_sync::mutex::Mutex; use embassy_time::{Duration, Timer}; use panic_reset as _; +#[cfg(feature = "skip-include")] +static APP_B: &[u8] = &[0, 1, 2, 3]; +#[cfg(not(feature = "skip-include"))] static APP_B: &[u8] = include_bytes!("../../b.bin"); #[embassy_executor::main] async fn main(_spawner: Spawner) { let p = embassy_stm32::init(Default::default()); let flash = Flash::new_blocking(p.FLASH); - let mut flash = BlockingAsync::new(flash); + let flash = Mutex::new(BlockingAsync::new(flash)); let button = Input::new(p.PB2, Pull::Up); let mut button = ExtiInput::new(button, p.EXTI2); @@ -28,18 +32,19 @@ async fn main(_spawner: Spawner) { led.set_high(); - let mut updater = FirmwareUpdater::default(); + let config = FirmwareUpdaterConfig::from_linkerfile(&flash); + let mut updater = FirmwareUpdater::new(config); button.wait_for_falling_edge().await; let mut offset = 0; + let mut magic = AlignedBuffer([0; WRITE_SIZE]); for chunk in APP_B.chunks(128) { let mut buf: [u8; 128] = [0; 128]; buf[..chunk.len()].copy_from_slice(chunk); - updater.write_firmware(offset, &buf, &mut flash, 128).await.unwrap(); + updater.write_firmware(magic.as_mut(), offset, &buf).await.unwrap(); offset += chunk.len(); } - let mut magic = AlignedBuffer([0; WRITE_SIZE]); - updater.mark_updated(&mut flash, magic.as_mut()).await.unwrap(); + updater.mark_updated(magic.as_mut()).await.unwrap(); led.set_low(); Timer::after(Duration::from_secs(1)).await; cortex_m::peripheral::SCB::sys_reset(); diff --git a/examples/boot/application/stm32l1/Cargo.toml b/examples/boot/application/stm32l1/Cargo.toml index 8ac0fac8..10b58c17 100644 --- a/examples/boot/application/stm32l1/Cargo.toml +++ b/examples/boot/application/stm32l1/Cargo.toml @@ -26,3 +26,4 @@ defmt = [ "embassy-stm32/defmt", "embassy-boot-stm32/defmt", ] +skip-include = [] diff --git a/examples/boot/application/stm32l1/src/bin/a.rs b/examples/boot/application/stm32l1/src/bin/a.rs index 00ddda63..1e9bf3cb 100644 --- a/examples/boot/application/stm32l1/src/bin/a.rs +++ b/examples/boot/application/stm32l1/src/bin/a.rs @@ -10,9 +10,13 @@ use embassy_executor::Spawner; use embassy_stm32::exti::ExtiInput; use embassy_stm32::flash::{Flash, WRITE_SIZE}; use embassy_stm32::gpio::{Input, Level, Output, Pull, Speed}; +use embassy_sync::mutex::Mutex; use embassy_time::{Duration, Timer}; use panic_reset as _; +#[cfg(feature = "skip-include")] +static APP_B: &[u8] = &[0, 1, 2, 3]; +#[cfg(not(feature = "skip-include"))] static APP_B: &[u8] = include_bytes!("../../b.bin"); #[embassy_executor::main] @@ -31,15 +35,15 @@ async fn main(_spawner: Spawner) { let config = FirmwareUpdaterConfig::from_linkerfile(&flash); let mut updater = FirmwareUpdater::new(config); button.wait_for_falling_edge().await; + let mut magic = AlignedBuffer([0; WRITE_SIZE]); let mut offset = 0; for chunk in APP_B.chunks(128) { let mut buf: [u8; 128] = [0; 128]; buf[..chunk.len()].copy_from_slice(chunk); - updater.write_firmware(offset, &buf).await.unwrap(); + updater.write_firmware(magic.as_mut(), offset, &buf).await.unwrap(); offset += chunk.len(); } - let mut magic = AlignedBuffer([0; WRITE_SIZE]); updater.mark_updated(magic.as_mut()).await.unwrap(); led.set_low(); Timer::after(Duration::from_secs(1)).await; diff --git a/examples/boot/application/stm32l4/Cargo.toml b/examples/boot/application/stm32l4/Cargo.toml index ec79acde..713a6527 100644 --- a/examples/boot/application/stm32l4/Cargo.toml +++ b/examples/boot/application/stm32l4/Cargo.toml @@ -26,3 +26,4 @@ defmt = [ "embassy-stm32/defmt", "embassy-boot-stm32/defmt", ] +skip-include = [] diff --git a/examples/boot/application/stm32l4/src/bin/a.rs b/examples/boot/application/stm32l4/src/bin/a.rs index 54579e4a..a514ab5b 100644 --- a/examples/boot/application/stm32l4/src/bin/a.rs +++ b/examples/boot/application/stm32l4/src/bin/a.rs @@ -10,8 +10,12 @@ use embassy_executor::Spawner; use embassy_stm32::exti::ExtiInput; use embassy_stm32::flash::{Flash, WRITE_SIZE}; use embassy_stm32::gpio::{Input, Level, Output, Pull, Speed}; +use embassy_sync::mutex::Mutex; use panic_reset as _; +#[cfg(feature = "skip-include")] +static APP_B: &[u8] = &[0, 1, 2, 3]; +#[cfg(not(feature = "skip-include"))] static APP_B: &[u8] = include_bytes!("../../b.bin"); #[embassy_executor::main] @@ -29,15 +33,15 @@ async fn main(_spawner: Spawner) { let config = FirmwareUpdaterConfig::from_linkerfile(&flash); let mut updater = FirmwareUpdater::new(config); button.wait_for_falling_edge().await; + let mut magic = AlignedBuffer([0; WRITE_SIZE]); let mut offset = 0; for chunk in APP_B.chunks(2048) { let mut buf: [u8; 2048] = [0; 2048]; buf[..chunk.len()].copy_from_slice(chunk); - updater.write_firmware(offset, &buf).await.unwrap(); + updater.write_firmware(magic.as_mut(), offset, &buf).await.unwrap(); offset += chunk.len(); } - let mut magic = AlignedBuffer([0; WRITE_SIZE]); - updater.mark_updated(&mut flash, magic.as_mut()).await.unwrap(); + updater.mark_updated(magic.as_mut()).await.unwrap(); led.set_low(); cortex_m::peripheral::SCB::sys_reset(); } diff --git a/examples/boot/application/stm32wl/Cargo.toml b/examples/boot/application/stm32wl/Cargo.toml index dfaece6c..4c8bbd73 100644 --- a/examples/boot/application/stm32wl/Cargo.toml +++ b/examples/boot/application/stm32wl/Cargo.toml @@ -26,3 +26,4 @@ defmt = [ "embassy-stm32/defmt", "embassy-boot-stm32/defmt", ] +skip-include = [] diff --git a/examples/boot/application/stm32wl/src/bin/a.rs b/examples/boot/application/stm32wl/src/bin/a.rs index 0c6fa05f..52a197a5 100644 --- a/examples/boot/application/stm32wl/src/bin/a.rs +++ b/examples/boot/application/stm32wl/src/bin/a.rs @@ -4,21 +4,25 @@ #[cfg(feature = "defmt-rtt")] use defmt_rtt::*; -use embassy_boot_stm32::{AlignedBuffer, FirmwareUpdater}; +use embassy_boot_stm32::{AlignedBuffer, FirmwareUpdater, FirmwareUpdaterConfig}; use embassy_embedded_hal::adapter::BlockingAsync; use embassy_executor::Spawner; use embassy_stm32::exti::ExtiInput; use embassy_stm32::flash::{Flash, WRITE_SIZE}; use embassy_stm32::gpio::{Input, Level, Output, Pull, Speed}; +use embassy_sync::mutex::Mutex; use panic_reset as _; +#[cfg(feature = "skip-include")] +static APP_B: &[u8] = &[0, 1, 2, 3]; +#[cfg(not(feature = "skip-include"))] static APP_B: &[u8] = include_bytes!("../../b.bin"); #[embassy_executor::main] async fn main(_spawner: Spawner) { let p = embassy_stm32::init(Default::default()); let flash = Flash::new_blocking(p.FLASH); - let mut flash = Mutex::new(BlockingAsync::new(flash)); + let flash = Mutex::new(BlockingAsync::new(flash)); let button = Input::new(p.PA0, Pull::Up); let mut button = ExtiInput::new(button, p.EXTI0); @@ -30,15 +34,15 @@ async fn main(_spawner: Spawner) { let mut updater = FirmwareUpdater::new(config); button.wait_for_falling_edge().await; //defmt::info!("Starting update"); + let mut magic = AlignedBuffer([0; WRITE_SIZE]); let mut offset = 0; for chunk in APP_B.chunks(2048) { let mut buf: [u8; 2048] = [0; 2048]; buf[..chunk.len()].copy_from_slice(chunk); // defmt::info!("Writing chunk at 0x{:x}", offset); - updater.write_firmware(offset, &buf).await.unwrap(); + updater.write_firmware(magic.as_mut(), offset, &buf).await.unwrap(); offset += chunk.len(); } - let mut magic = AlignedBuffer([0; WRITE_SIZE]); updater.mark_updated(magic.as_mut()).await.unwrap(); //defmt::info!("Marked as updated"); led.set_low(); From 0a551eb7c6646af1b5c7e79849594fafe877e99a Mon Sep 17 00:00:00 2001 From: xoviat Date: Tue, 20 Jun 2023 17:39:00 -0500 Subject: [PATCH 14/14] stm32/can: fix time --- embassy-stm32/src/can/bxcan.rs | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/embassy-stm32/src/can/bxcan.rs b/embassy-stm32/src/can/bxcan.rs index e23ce686..224c39ea 100644 --- a/embassy-stm32/src/can/bxcan.rs +++ b/embassy-stm32/src/can/bxcan.rs @@ -158,10 +158,11 @@ impl<'d, T: Instance> Can<'d, T> { /// Queues the message to be sent but exerts backpressure pub async fn write(&mut self, frame: &Frame) -> bxcan::TransmitStatus { poll_fn(|cx| { + T::state().tx_waker.register(cx.waker()); if let Ok(status) = self.can.transmit(frame) { return Poll::Ready(status); } - T::state().tx_waker.register(cx.waker()); + Poll::Pending }) .await @@ -169,10 +170,11 @@ impl<'d, T: Instance> Can<'d, T> { pub async fn flush(&self, mb: bxcan::Mailbox) { poll_fn(|cx| { + T::state().tx_waker.register(cx.waker()); if T::regs().tsr().read().tme(mb.index()) { return Poll::Ready(()); } - T::state().tx_waker.register(cx.waker()); + Poll::Pending }) .await; @@ -181,12 +183,13 @@ impl<'d, T: Instance> Can<'d, T> { /// Returns a tuple of the time the message was received and the message frame pub async fn read(&mut self) -> Result<(u16, bxcan::Frame), BusError> { poll_fn(|cx| { + T::state().err_waker.register(cx.waker()); if let Poll::Ready((time, frame)) = T::state().rx_queue.recv().poll_unpin(cx) { return Poll::Ready(Ok((time, frame))); } else if let Some(err) = self.curr_error() { return Poll::Ready(Err(err)); } - T::state().err_waker.register(cx.waker()); + Poll::Pending }) .await