diff --git a/embassy-stm32/src/can.rs b/embassy-stm32/src/can.rs deleted file mode 100644 index f97e900ea..000000000 --- a/embassy-stm32/src/can.rs +++ /dev/null @@ -1,120 +0,0 @@ -//! Async low power Serial. -//! -//! The peripheral is autmatically enabled and disabled as required to save power. -//! Lowest power consumption can only be guaranteed if the send receive futures -//! are dropped correctly (e.g. not using `mem::forget()`). - -use bxcan; -use bxcan::Interrupts; -use core::future::Future; -use embassy::interrupt::Interrupt; -use embassy::util::InterruptFuture; -use nb; -use nb::block; - -use crate::interrupt; - -/// Interface to the Serial peripheral -pub struct Can { - can: bxcan::Can, - tx_int: T::TInterrupt, - rx_int: T::RInterrupt, -} - -impl Can { - pub fn new(mut can: bxcan::Can, tx_int: T::TInterrupt, rx_int: T::RInterrupt) -> Self { - // Sync to the bus and start normal operation. - can.enable_interrupts( - Interrupts::TRANSMIT_MAILBOX_EMPTY | Interrupts::FIFO0_MESSAGE_PENDING, - ); - block!(can.enable()).unwrap(); - - Can { - can: can, - tx_int: tx_int, - rx_int: rx_int, - } - } - - /// Sends can frame. - /// - /// This method async-blocks until the frame is transmitted. - pub fn transmit<'a>(&'a mut self, frame: &'a bxcan::Frame) -> impl Future + 'a { - async move { - let fut = InterruptFuture::new(&mut self.tx_int); - // Infallible - self.can.transmit(frame).unwrap(); - - fut.await; - } - } - - /// Receive can frame. - /// - /// This method async-blocks until the frame is received. - pub fn receive<'a>(&'a mut self) -> impl Future + 'a { - async move { - let mut frame: Option; - - loop { - let fut = InterruptFuture::new(&mut self.rx_int); - frame = match self.can.receive() { - Ok(frame) => Some(frame), - Err(nb::Error::WouldBlock) => None, - Err(nb::Error::Other(_)) => None, // Ignore overrun errors. - }; - if frame.is_some() { - break; - } - fut.await; - } - - frame.unwrap() - } - } -} - -mod private { - pub trait Sealed {} -} - -pub trait Instance: bxcan::Instance + private::Sealed { - type TInterrupt: Interrupt; - type RInterrupt: Interrupt; -} - -macro_rules! can { - ($($can:ident => ($tint:ident, $rint:ident),)+) => { - $( - impl private::Sealed for crate::hal::can::Can {} - impl Instance for crate::hal::can::Can { - type TInterrupt = interrupt::$tint; - type RInterrupt = interrupt::$rint; - } - )+ - } -} - -#[cfg(any( - feature = "stm32f401", - feature = "stm32f405", - feature = "stm32f407", - feature = "stm32f410", - feature = "stm32f411", - feature = "stm32f412", - feature = "stm32f413", - feature = "stm32f415", - feature = "stm32f417", - feature = "stm32f423", - feature = "stm32f427", - feature = "stm32f429", - feature = "stm32f437", - feature = "stm32f439", - feature = "stm32f446", - feature = "stm32f469", - feature = "stm32f479", -))] -can! { - CAN1 => (CAN1_TX, CAN1_RX0), - CAN2 => (CAN2_TX, CAN2_RX0), -} diff --git a/embassy-stm32/src/f4/mod.rs b/embassy-stm32/src/f4/mod.rs deleted file mode 100644 index 9edde82ca..000000000 --- a/embassy-stm32/src/f4/mod.rs +++ /dev/null @@ -1,2 +0,0 @@ -pub mod serial; -pub mod spi; diff --git a/embassy-stm32/src/f4/serial.rs b/embassy-stm32/src/f4/serial.rs deleted file mode 100644 index 78aaa8944..000000000 --- a/embassy-stm32/src/f4/serial.rs +++ /dev/null @@ -1,357 +0,0 @@ -//! Async Serial. - -use core::future::Future; -use core::marker::PhantomData; -use embassy::interrupt::Interrupt; -use embassy::traits::uart::{Error, Read, ReadUntilIdle, Write}; -use embassy::util::InterruptFuture; -use futures::{select_biased, FutureExt}; - -use crate::hal::{ - dma, - dma::config::DmaConfig, - dma::traits::{Channel, DMASet, PeriAddress, Stream}, - dma::{MemoryToPeripheral, PeripheralToMemory, Transfer}, - rcc::Clocks, - serial, - serial::config::{Config as SerialConfig, DmaConfig as SerialDmaConfig}, - serial::{Event as SerialEvent, Pins}, -}; -use crate::interrupt; -use crate::pac; - -/// Interface to the Serial peripheral -pub struct Serial< - USART: PeriAddress + WithInterrupt, - TSTREAM: Stream + WithInterrupt, - RSTREAM: Stream + WithInterrupt, - CHANNEL: Channel, -> { - tx_stream: Option, - rx_stream: Option, - usart: Option, - tx_int: TSTREAM::Interrupt, - rx_int: RSTREAM::Interrupt, - usart_int: USART::Interrupt, - channel: PhantomData, -} - -// static mut INSTANCE: *const Serial, Stream2> = ptr::null_mut(); - -impl Serial -where - USART: serial::Instance - + PeriAddress - + DMASet - + DMASet - + WithInterrupt, - TSTREAM: Stream + WithInterrupt, - RSTREAM: Stream + WithInterrupt, - CHANNEL: Channel, -{ - // Leaking futures is forbidden! - pub unsafe fn new( - usart: USART, - streams: (TSTREAM, RSTREAM), - pins: PINS, - tx_int: TSTREAM::Interrupt, - rx_int: RSTREAM::Interrupt, - usart_int: USART::Interrupt, - mut config: SerialConfig, - clocks: Clocks, - ) -> Self - where - PINS: Pins, - { - config.dma = SerialDmaConfig::TxRx; - - let (usart, _) = serial::Serial::new(usart, pins, config, clocks) - .unwrap() - .release(); - - let (tx_stream, rx_stream) = streams; - - Serial { - tx_stream: Some(tx_stream), - rx_stream: Some(rx_stream), - usart: Some(usart), - tx_int: tx_int, - rx_int: rx_int, - usart_int: usart_int, - channel: PhantomData, - } - } -} - -impl Read for Serial -where - USART: serial::Instance - + PeriAddress - + DMASet - + DMASet - + WithInterrupt - + 'static, - TSTREAM: Stream + WithInterrupt + 'static, - RSTREAM: Stream + WithInterrupt + 'static, - CHANNEL: Channel + 'static, -{ - type ReadFuture<'a> = impl Future> + 'a; - - /// Receives serial data. - /// - /// The future is pending until the buffer is completely filled. - fn read<'a>(&'a mut self, buf: &'a mut [u8]) -> Self::ReadFuture<'a> { - let static_buf = unsafe { core::mem::transmute::<&'a mut [u8], &'static mut [u8]>(buf) }; - - async move { - let rx_stream = self.rx_stream.take().unwrap(); - let usart = self.usart.take().unwrap(); - - let mut rx_transfer = Transfer::init( - rx_stream, - usart, - static_buf, - None, - DmaConfig::default() - .transfer_complete_interrupt(true) - .memory_increment(true) - .double_buffer(false), - ); - - let fut = InterruptFuture::new(&mut self.rx_int); - rx_transfer.start(|_usart| {}); - fut.await; - - let (rx_stream, usart, _, _) = rx_transfer.free(); - self.rx_stream.replace(rx_stream); - self.usart.replace(usart); - - Ok(()) - } - } -} - -impl Write for Serial -where - USART: serial::Instance - + PeriAddress - + DMASet - + DMASet - + WithInterrupt - + 'static, - TSTREAM: Stream + WithInterrupt + 'static, - RSTREAM: Stream + WithInterrupt + 'static, - CHANNEL: Channel + 'static, -{ - type WriteFuture<'a> = impl Future> + 'a; - - /// Sends serial data. - fn write<'a>(&'a mut self, buf: &'a [u8]) -> Self::WriteFuture<'a> { - #[allow(mutable_transmutes)] - let static_buf = unsafe { core::mem::transmute::<&'a [u8], &'static mut [u8]>(buf) }; - - async move { - let tx_stream = self.tx_stream.take().unwrap(); - let usart = self.usart.take().unwrap(); - - let mut tx_transfer = Transfer::init( - tx_stream, - usart, - static_buf, - None, - DmaConfig::default() - .transfer_complete_interrupt(true) - .memory_increment(true) - .double_buffer(false), - ); - - let fut = InterruptFuture::new(&mut self.tx_int); - - tx_transfer.start(|_usart| {}); - fut.await; - - let (tx_stream, usart, _buf, _) = tx_transfer.free(); - - self.tx_stream.replace(tx_stream); - self.usart.replace(usart); - - Ok(()) - } - } -} - -impl ReadUntilIdle for Serial -where - USART: serial::Instance - + PeriAddress - + DMASet - + DMASet - + WithInterrupt - + 'static, - TSTREAM: Stream + WithInterrupt + 'static, - RSTREAM: Stream + WithInterrupt + 'static, - CHANNEL: Channel + 'static, -{ - type ReadUntilIdleFuture<'a> = impl Future> + 'a; - - /// Receives serial data. - /// - /// The future is pending until either the buffer is completely full, or the RX line falls idle after receiving some data. - /// - /// Returns the number of bytes read. - fn read_until_idle<'a>(&'a mut self, buf: &'a mut [u8]) -> Self::ReadUntilIdleFuture<'a> { - let static_buf = unsafe { core::mem::transmute::<&'a mut [u8], &'static mut [u8]>(buf) }; - - async move { - let rx_stream = self.rx_stream.take().unwrap(); - let usart = self.usart.take().unwrap(); - - unsafe { - /* __HAL_UART_ENABLE_IT(&uart->UartHandle, UART_IT_IDLE); */ - (*USART::ptr()).cr1.modify(|_, w| w.idleie().set_bit()); - - /* __HAL_UART_CLEAR_IDLEFLAG(&uart->UartHandle); */ - (*USART::ptr()).sr.read(); - (*USART::ptr()).dr.read(); - }; - - let mut rx_transfer = Transfer::init( - rx_stream, - usart, - static_buf, - None, - DmaConfig::default() - .transfer_complete_interrupt(true) - .memory_increment(true) - .double_buffer(false), - ); - - let total_bytes = RSTREAM::get_number_of_transfers() as usize; - - let fut = InterruptFuture::new(&mut self.rx_int); - let fut_idle = InterruptFuture::new(&mut self.usart_int); - - rx_transfer.start(|_usart| {}); - - futures::future::select(fut, fut_idle).await; - - let (rx_stream, usart, _, _) = rx_transfer.free(); - - let remaining_bytes = RSTREAM::get_number_of_transfers() as usize; - - unsafe { - (*USART::ptr()).cr1.modify(|_, w| w.idleie().clear_bit()); - } - self.rx_stream.replace(rx_stream); - self.usart.replace(usart); - - Ok(total_bytes - remaining_bytes) - } - } -} - -mod private { - pub trait Sealed {} -} - -pub trait WithInterrupt: private::Sealed { - type Interrupt: Interrupt; -} - -macro_rules! dma { - ($($PER:ident => ($dma:ident, $stream:ident),)+) => { - $( - impl private::Sealed for dma::$stream {} - impl WithInterrupt for dma::$stream { - type Interrupt = interrupt::$PER; - } - )+ - } - } - -macro_rules! usart { - ($($PER:ident => ($usart:ident),)+) => { - $( - impl private::Sealed for pac::$usart {} - impl WithInterrupt for pac::$usart { - type Interrupt = interrupt::$PER; - } - )+ - } -} - -dma! { - DMA2_STREAM0 => (DMA2, Stream0), - DMA2_STREAM1 => (DMA2, Stream1), - DMA2_STREAM2 => (DMA2, Stream2), - DMA2_STREAM3 => (DMA2, Stream3), - DMA2_STREAM4 => (DMA2, Stream4), - DMA2_STREAM5 => (DMA2, Stream5), - DMA2_STREAM6 => (DMA2, Stream6), - DMA2_STREAM7 => (DMA2, Stream7), - DMA1_STREAM0 => (DMA1, Stream0), - DMA1_STREAM1 => (DMA1, Stream1), - DMA1_STREAM2 => (DMA1, Stream2), - DMA1_STREAM3 => (DMA1, Stream3), - DMA1_STREAM4 => (DMA1, Stream4), - DMA1_STREAM5 => (DMA1, Stream5), - DMA1_STREAM6 => (DMA1, Stream6), -} - -#[cfg(any(feature = "stm32f401", feature = "stm32f410", feature = "stm32f411",))] -usart! { - USART1 => (USART1), - USART2 => (USART2), - USART6 => (USART6), -} - -#[cfg(any(feature = "stm32f405", feature = "stm32f407"))] -usart! { - USART1 => (USART1), - USART2 => (USART2), - USART3 => (USART3), - USART6 => (USART6), - - UART4 => (UART4), - UART5 => (UART5), -} - -#[cfg(feature = "stm32f412")] -usart! { - USART1 => (USART1), - USART2 => (USART2), - USART3 => (USART3), - USART6 => (USART6), -} - -#[cfg(feature = "stm32f413")] -usart! { - USART1 => (USART1), - USART2 => (USART2), - USART3 => (USART3), - USART6 => (USART6), - USART7 => (USART7), - USART8 => (USART8), - - UART5 => (UART5), - UART9 => (UART9), - UART10 => (UART10), -} - -#[cfg(any( - feature = "stm32f427", - feature = "stm32f429", - feature = "stm32f446", - feature = "stm32f469" -))] -usart! { - USART1 => (USART1), - USART2 => (USART2), - USART3 => (USART3), - USART6 => (USART6), - - UART4 => (UART4), - UART5 => (UART5), -// UART7 => (UART7), -// UART8 => (UART8), -} diff --git a/embassy-stm32/src/f4/spi.rs b/embassy-stm32/src/f4/spi.rs deleted file mode 100644 index 65bf7287a..000000000 --- a/embassy-stm32/src/f4/spi.rs +++ /dev/null @@ -1,475 +0,0 @@ -//! Async SPI - -use embassy::time; - -use core::{future::Future, marker::PhantomData, mem, ops::Deref, pin::Pin, ptr}; -use embassy::{interrupt::Interrupt, traits::spi::FullDuplex, util::InterruptFuture}; -use nb; - -pub use crate::hal::spi::{Mode, Phase, Polarity}; -use crate::hal::{ - bb, dma, - dma::config::DmaConfig, - dma::traits::{Channel, DMASet, PeriAddress, Stream}, - dma::{MemoryToPeripheral, PeripheralToMemory, Transfer}, - rcc::Clocks, - spi::Pins, - time::Hertz, -}; -use crate::interrupt; -use crate::pac; -use futures::future; - -#[derive(Debug, Clone, Copy, PartialEq, Eq)] -#[cfg_attr(feature = "defmt", derive(defmt::Format))] -#[non_exhaustive] -pub enum Error { - TxBufferTooLong, - RxBufferTooLong, - Overrun, - ModeFault, - Crc, -} - -fn read_sr(spi: &T) -> nb::Result { - let sr = spi.sr.read(); - Err(if sr.ovr().bit_is_set() { - nb::Error::Other(Error::Overrun) - } else if sr.modf().bit_is_set() { - nb::Error::Other(Error::ModeFault) - } else if sr.crcerr().bit_is_set() { - nb::Error::Other(Error::Crc) - } else if sr.rxne().bit_is_set() { - // NOTE(read_volatile) read only 1 byte (the svd2rust API only allows - // reading a half-word) - return Ok(unsafe { ptr::read_volatile(&spi.dr as *const _ as *const u8) }); - } else { - nb::Error::WouldBlock - }) -} - -fn write_sr(spi: &T, byte: u8) -> nb::Result<(), Error> { - let sr = spi.sr.read(); - Err(if sr.ovr().bit_is_set() { - // Read from the DR to clear the OVR bit - let _ = spi.dr.read(); - nb::Error::Other(Error::Overrun) - } else if sr.modf().bit_is_set() { - // Write to CR1 to clear MODF - spi.cr1.modify(|_r, w| w); - nb::Error::Other(Error::ModeFault) - } else if sr.crcerr().bit_is_set() { - // Clear the CRCERR bit - spi.sr.modify(|_r, w| { - w.crcerr().clear_bit(); - w - }); - nb::Error::Other(Error::Crc) - } else if sr.txe().bit_is_set() { - // NOTE(write_volatile) see note above - unsafe { ptr::write_volatile(&spi.dr as *const _ as *mut u8, byte) } - return Ok(()); - } else { - nb::Error::WouldBlock - }) -} - -/// Interface to the Serial peripheral -pub struct Spi< - SPI: PeriAddress + WithInterrupt, - TSTREAM: Stream + WithInterrupt, - RSTREAM: Stream + WithInterrupt, - CHANNEL: Channel, -> { - tx_stream: Option, - rx_stream: Option, - spi: Option, - tx_int: TSTREAM::Interrupt, - rx_int: RSTREAM::Interrupt, - spi_int: SPI::Interrupt, - channel: PhantomData, -} - -impl Spi -where - SPI: Instance - + PeriAddress - + DMASet - + DMASet - + WithInterrupt, - TSTREAM: Stream + WithInterrupt, - RSTREAM: Stream + WithInterrupt, - CHANNEL: Channel, -{ - // Leaking futures is forbidden! - pub unsafe fn new( - spi: SPI, - streams: (TSTREAM, RSTREAM), - pins: PINS, - tx_int: TSTREAM::Interrupt, - rx_int: RSTREAM::Interrupt, - spi_int: SPI::Interrupt, - mode: Mode, - freq: Hertz, - clocks: Clocks, - ) -> Self - where - PINS: Pins, - { - let (tx_stream, rx_stream) = streams; - - // let spi1: crate::pac::SPI1 = unsafe { mem::transmute(()) }; - // let mut hspi = crate::hal::spi::Spi::spi1( - // spi1, - // ( - // crate::hal::spi::NoSck, - // crate::hal::spi::NoMiso, - // crate::hal::spi::NoMosi, - // ), - // mode, - // freq, - // clocks, - // ); - - unsafe { SPI::enable_clock() }; - - let clock = SPI::clock_speed(clocks); - - // disable SS output - // spi.cr2 - // .write(|w| w.ssoe().clear_bit().rxdmaen().set_bit().txdmaen().set_bit()); - spi.cr2.write(|w| w.ssoe().clear_bit()); - - let br = match clock.0 / freq.0 { - 0 => unreachable!(), - 1..=2 => 0b000, - 3..=5 => 0b001, - 6..=11 => 0b010, - 12..=23 => 0b011, - 24..=47 => 0b100, - 48..=95 => 0b101, - 96..=191 => 0b110, - _ => 0b111, - }; - - // mstr: master configuration - // lsbfirst: MSB first - // ssm: enable software slave management (NSS pin free for other uses) - // ssi: set nss high = master mode - // dff: 8 bit frames - // bidimode: 2-line unidirectional - // spe: enable the SPI bus - spi.cr1.write(|w| { - w.cpha() - .bit(mode.phase == Phase::CaptureOnSecondTransition) - .cpol() - .bit(mode.polarity == Polarity::IdleHigh) - .mstr() - .set_bit() - .br() - .bits(br) - .lsbfirst() - .clear_bit() - .ssm() - .set_bit() - .ssi() - .set_bit() - .rxonly() - .clear_bit() - .dff() - .clear_bit() - .bidimode() - .clear_bit() - .spe() - .set_bit() - }); - - Self { - tx_stream: Some(tx_stream), - rx_stream: Some(rx_stream), - spi: Some(spi), - tx_int: tx_int, - rx_int: rx_int, - spi_int: spi_int, - channel: PhantomData, - } - } -} - -impl FullDuplex for Spi -where - SPI: Instance - + PeriAddress - + DMASet - + DMASet - + WithInterrupt - + 'static, - TSTREAM: Stream + WithInterrupt + 'static, - RSTREAM: Stream + WithInterrupt + 'static, - CHANNEL: Channel + 'static, -{ - type Error = Error; - - type WriteFuture<'a> = impl Future> + 'a; - type ReadFuture<'a> = impl Future> + 'a; - type WriteReadFuture<'a> = impl Future> + 'a; - - fn read<'a>(&'a mut self, buf: &'a mut [u8]) -> Self::ReadFuture<'a> { - #[allow(mutable_transmutes)] - let static_buf: &'static mut [u8] = unsafe { mem::transmute(buf) }; - - async move { - let rx_stream = self.rx_stream.take().unwrap(); - let spi = self.spi.take().unwrap(); - - spi.cr2.modify(|_, w| w.errie().set_bit()); - - let mut rx_transfer = Transfer::init( - rx_stream, - spi, - static_buf, - None, - DmaConfig::default() - .transfer_complete_interrupt(true) - .memory_increment(true) - .double_buffer(false), - ); - - let fut = InterruptFuture::new(&mut self.rx_int); - let fut_err = InterruptFuture::new(&mut self.spi_int); - - rx_transfer.start(|_spi| {}); - future::select(fut, fut_err).await; - - let (rx_stream, spi, _buf, _) = rx_transfer.free(); - - spi.cr2.modify(|_, w| w.errie().clear_bit()); - self.rx_stream.replace(rx_stream); - self.spi.replace(spi); - - Ok(()) - } - } - - fn write<'a>(&'a mut self, buf: &'a [u8]) -> Self::WriteFuture<'a> { - #[allow(mutable_transmutes)] - let static_buf: &'static mut [u8] = unsafe { mem::transmute(buf) }; - - async move { - let tx_stream = self.tx_stream.take().unwrap(); - let spi = self.spi.take().unwrap(); - - // let mut tx_transfer = Transfer::init( - // tx_stream, - // spi, - // static_buf, - // None, - // DmaConfig::default() - // .transfer_complete_interrupt(true) - // .memory_increment(true) - // .double_buffer(false), - // ); - // - // let fut = InterruptFuture::new(&mut self.tx_int); - // - // tx_transfer.start(|_spi| {}); - // fut.await; - - // let (tx_stream, spi, _buf, _) = tx_transfer.free(); - - for i in 0..(static_buf.len() - 1) { - let byte = static_buf[i]; - nb::block!(write_sr(&spi, byte)); - } - - self.tx_stream.replace(tx_stream); - self.spi.replace(spi); - - Ok(()) - } - } - - fn read_write<'a>( - &'a mut self, - read_buf: &'a mut [u8], - write_buf: &'a [u8], - ) -> Self::WriteReadFuture<'a> { - #[allow(mutable_transmutes)] - let write_static_buf: &'static mut [u8] = unsafe { mem::transmute(write_buf) }; - let read_static_buf: &'static mut [u8] = unsafe { mem::transmute(read_buf) }; - - async move { - let tx_stream = self.tx_stream.take().unwrap(); - let rx_stream = self.rx_stream.take().unwrap(); - let spi_tx = self.spi.take().unwrap(); - let spi_rx: SPI = unsafe { mem::transmute_copy(&spi_tx) }; - - spi_rx - .cr2 - .modify(|_, w| w.errie().set_bit().txeie().set_bit().rxneie().set_bit()); - - // let mut tx_transfer = Transfer::init( - // tx_stream, - // spi_tx, - // write_static_buf, - // None, - // DmaConfig::default() - // .transfer_complete_interrupt(true) - // .memory_increment(true) - // .double_buffer(false), - // ); - // - // let mut rx_transfer = Transfer::init( - // rx_stream, - // spi_rx, - // read_static_buf, - // None, - // DmaConfig::default() - // .transfer_complete_interrupt(true) - // .memory_increment(true) - // .double_buffer(false), - // ); - // - // let tx_fut = InterruptFuture::new(&mut self.tx_int); - // let rx_fut = InterruptFuture::new(&mut self.rx_int); - // let rx_fut_err = InterruptFuture::new(&mut self.spi_int); - // - // rx_transfer.start(|_spi| {}); - // tx_transfer.start(|_spi| {}); - // - // time::Timer::after(time::Duration::from_millis(500)).await; - // - // // tx_fut.await; - // // future::select(rx_fut, rx_fut_err).await; - // - // let (rx_stream, spi_rx, _buf, _) = rx_transfer.free(); - // let (tx_stream, _, _buf, _) = tx_transfer.free(); - - for i in 0..(read_static_buf.len() - 1) { - let byte = write_static_buf[i]; - loop { - let fut = InterruptFuture::new(&mut self.spi_int); - match write_sr(&spi_tx, byte) { - Ok(()) => break, - _ => {} - } - fut.await; - } - - loop { - let fut = InterruptFuture::new(&mut self.spi_int); - match read_sr(&spi_tx) { - Ok(byte) => { - read_static_buf[i] = byte; - break; - } - _ => {} - } - fut.await; - } - } - - spi_rx.cr2.modify(|_, w| { - w.errie() - .clear_bit() - .txeie() - .clear_bit() - .rxneie() - .clear_bit() - }); - self.rx_stream.replace(rx_stream); - self.tx_stream.replace(tx_stream); - self.spi.replace(spi_rx); - - Ok(()) - } - } -} - -mod private { - pub trait Sealed {} -} - -pub trait WithInterrupt: private::Sealed { - type Interrupt: Interrupt; -} - -pub trait Instance: Deref + private::Sealed { - unsafe fn enable_clock(); - fn clock_speed(clocks: Clocks) -> Hertz; -} - -macro_rules! dma { - ($($PER:ident => ($dma:ident, $stream:ident),)+) => { - $( - impl private::Sealed for dma::$stream {} - impl WithInterrupt for dma::$stream { - type Interrupt = interrupt::$PER; - } - )+ - } - } - -macro_rules! spi { - ($($PER:ident => ($SPI:ident, $pclkX:ident, $apbXenr:ident, $en:expr),)+) => { - $( - impl private::Sealed for pac::$SPI {} - impl Instance for pac::$SPI { - unsafe fn enable_clock() { - const EN_BIT: u8 = $en; - // NOTE(unsafe) self reference will only be used for atomic writes with no side effects. - let rcc = &(*pac::RCC::ptr()); - // Enable clock. - bb::set(&rcc.$apbXenr, EN_BIT); - // Stall the pipeline to work around erratum 2.1.13 (DM00037591) - cortex_m::asm::dsb(); - } - - fn clock_speed(clocks: Clocks) -> Hertz { - clocks.$pclkX() - } - } - impl WithInterrupt for pac::$SPI { - type Interrupt = interrupt::$PER; - } - )+ - } -} - -dma! { - DMA2_STREAM0 => (DMA2, Stream0), - DMA2_STREAM1 => (DMA2, Stream1), - DMA2_STREAM2 => (DMA2, Stream2), - DMA2_STREAM3 => (DMA2, Stream3), - DMA2_STREAM4 => (DMA2, Stream4), - DMA2_STREAM5 => (DMA2, Stream5), - DMA2_STREAM6 => (DMA2, Stream6), - DMA2_STREAM7 => (DMA2, Stream7), - DMA1_STREAM0 => (DMA1, Stream0), - DMA1_STREAM1 => (DMA1, Stream1), - DMA1_STREAM2 => (DMA1, Stream2), - DMA1_STREAM3 => (DMA1, Stream3), - DMA1_STREAM4 => (DMA1, Stream4), - DMA1_STREAM5 => (DMA1, Stream5), - DMA1_STREAM6 => (DMA1, Stream6), -} - -#[cfg(any( - feature = "stm32f401", - feature = "stm32f410", - feature = "stm32f411", - feature = "stm32f446", -))] -spi! { - SPI1 => (SPI1, pclk2, apb2enr, 12), - SPI2 => (SPI2, pclk1, apb2enr, 14), -// SPI6 => (SPI6, pclk2, apb2enr, 21), - SPI4 => (SPI3, pclk2, apb2enr, 13), -// SPI5 => (SPI3, pclk2, apb2enr, 20), -} - -#[cfg(any(feature = "stm32f405", feature = "stm32f407"))] -spi! { - SPI1 => (SPI1, pclk2, apb2enr, 12), - SPI3 => (SPI3, pclk1, apb2enr, 15), -} diff --git a/embassy-stm32/src/interrupt.rs b/embassy-stm32/src/interrupt.rs deleted file mode 100644 index 214327300..000000000 --- a/embassy-stm32/src/interrupt.rs +++ /dev/null @@ -1,59 +0,0 @@ -use core::sync::atomic::{compiler_fence, Ordering}; - -use crate::pac::NVIC_PRIO_BITS; - -// Re-exports -pub use cortex_m::interrupt::{CriticalSection, Mutex}; -pub use embassy::interrupt::{declare, take, Interrupt}; - -#[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd)] -#[cfg_attr(feature = "defmt", derive(defmt::Format))] -#[repr(u8)] -pub enum Priority { - Level0 = 0, - Level1 = 1, - Level2 = 2, - Level3 = 3, - Level4 = 4, - Level5 = 5, - Level6 = 6, - Level7 = 7, - Level8 = 8, - Level9 = 9, - Level10 = 10, - Level11 = 11, - Level12 = 12, - Level13 = 13, - Level14 = 14, - Level15 = 15, -} - -impl From for Priority { - fn from(priority: u8) -> Self { - match priority >> (8 - NVIC_PRIO_BITS) { - 0 => Self::Level0, - 1 => Self::Level1, - 2 => Self::Level2, - 3 => Self::Level3, - 4 => Self::Level4, - 5 => Self::Level5, - 6 => Self::Level6, - 7 => Self::Level7, - 8 => Self::Level8, - 9 => Self::Level9, - 10 => Self::Level10, - 11 => Self::Level11, - 12 => Self::Level12, - 13 => Self::Level13, - 14 => Self::Level14, - 15 => Self::Level15, - _ => unreachable!(), - } - } -} - -impl From for u8 { - fn from(p: Priority) -> Self { - (p as u8) << (8 - NVIC_PRIO_BITS) - } -} diff --git a/embassy-stm32/src/rtc.rs b/embassy-stm32/src/rtc.rs deleted file mode 100644 index b1abba325..000000000 --- a/embassy-stm32/src/rtc.rs +++ /dev/null @@ -1,504 +0,0 @@ -use crate::hal::bb; -use crate::hal::rcc::Clocks; -use core::cell::Cell; -use core::convert::TryInto; -use core::sync::atomic::{compiler_fence, AtomicU32, Ordering}; - -use embassy::interrupt::InterruptExt; -use embassy::time::{Clock, TICKS_PER_SECOND}; - -use crate::interrupt; -use crate::interrupt::{CriticalSection, Interrupt, Mutex}; - -// RTC timekeeping works with something we call "periods", which are time intervals -// of 2^15 ticks. The RTC counter value is 16 bits, so one "overflow cycle" is 2 periods. -// -// A `period` count is maintained in parallel to the RTC hardware `counter`, like this: -// - `period` and `counter` start at 0 -// - `period` is incremented on overflow (at counter value 0) -// - `period` is incremented "midway" between overflows (at counter value 0x8000) -// -// Therefore, when `period` is even, counter is in 0..0x7FFF. When odd, counter is in 0x8000..0xFFFF -// This allows for now() to return the correct value even if it races an overflow. -// -// To get `now()`, `period` is read first, then `counter` is read. If the counter value matches -// the expected range for the `period` parity, we're done. If it doesn't, this means that -// a new period start has raced us between reading `period` and `counter`, so we assume the `counter` value -// corresponds to the next period. -// -// `period` is a 32bit integer, so It overflows on 2^32 * 2^15 / 32768 seconds of uptime, which is 136 years. -fn calc_now(period: u32, counter: u16) -> u64 { - ((period as u64) << 15) + ((counter as u32 ^ ((period & 1) << 15)) as u64) -} - -struct AlarmState { - timestamp: Cell, - callback: Cell>, -} - -impl AlarmState { - fn new() -> Self { - Self { - timestamp: Cell::new(u64::MAX), - callback: Cell::new(None), - } - } -} - -// TODO: This is sometimes wasteful, try to find a better way -const ALARM_COUNT: usize = 3; - -/// RTC timer that can be used by the executor and to set alarms. -/// -/// It can work with Timers 2, 3, 4, 5, 9 and 12. Timers 9 and 12 only have one alarm available, -/// while the others have three each. -/// This timer works internally with a unit of 2^15 ticks, which means that if a call to -/// [`embassy::time::Clock::now`] is blocked for that amount of ticks the returned value will be -/// wrong (an old value). The current default tick rate is 32768 ticks per second. -pub struct RTC { - rtc: T, - irq: T::Interrupt, - - /// Number of 2^23 periods elapsed since boot. - period: AtomicU32, - - /// Timestamp at which to fire alarm. u64::MAX if no alarm is scheduled. - alarms: Mutex<[AlarmState; ALARM_COUNT]>, - - clocks: Clocks, -} - -impl RTC { - pub fn new(rtc: T, irq: T::Interrupt, clocks: Clocks) -> Self { - Self { - rtc, - irq, - period: AtomicU32::new(0), - alarms: Mutex::new([AlarmState::new(), AlarmState::new(), AlarmState::new()]), - clocks, - } - } - - pub fn start(&'static self) { - self.rtc.enable_clock(); - self.rtc.stop_and_reset(); - - let multiplier = if T::ppre(&self.clocks) == 1 { 1 } else { 2 }; - let freq = T::pclk(&self.clocks) * multiplier; - let psc = freq / TICKS_PER_SECOND as u32 - 1; - let psc: u16 = psc.try_into().unwrap(); - - self.rtc.set_psc_arr(psc, u16::MAX); - // Mid-way point - self.rtc.set_compare(0, 0x8000); - self.rtc.set_compare_interrupt(0, true); - - self.irq.set_handler(|ptr| unsafe { - let this = &*(ptr as *const () as *const Self); - this.on_interrupt(); - }); - self.irq.set_handler_context(self as *const _ as *mut _); - self.irq.unpend(); - self.irq.enable(); - - self.rtc.start(); - } - - fn on_interrupt(&self) { - if self.rtc.overflow_interrupt_status() { - self.rtc.overflow_clear_flag(); - self.next_period(); - } - - // Half overflow - if self.rtc.compare_interrupt_status(0) { - self.rtc.compare_clear_flag(0); - self.next_period(); - } - - for n in 1..=ALARM_COUNT { - if self.rtc.compare_interrupt_status(n) { - self.rtc.compare_clear_flag(n); - interrupt::free(|cs| self.trigger_alarm(n, cs)); - } - } - } - - fn next_period(&self) { - interrupt::free(|cs| { - let period = self.period.fetch_add(1, Ordering::Relaxed) + 1; - let t = (period as u64) << 15; - - for n in 1..=ALARM_COUNT { - let alarm = &self.alarms.borrow(cs)[n - 1]; - let at = alarm.timestamp.get(); - - let diff = at - t; - if diff < 0xc000 { - self.rtc.set_compare(n, at as u16); - self.rtc.set_compare_interrupt(n, true); - } - } - }) - } - - fn trigger_alarm(&self, n: usize, cs: &CriticalSection) { - self.rtc.set_compare_interrupt(n, false); - - let alarm = &self.alarms.borrow(cs)[n - 1]; - alarm.timestamp.set(u64::MAX); - - // Call after clearing alarm, so the callback can set another alarm. - if let Some((f, ctx)) = alarm.callback.get() { - f(ctx); - } - } - - fn set_alarm_callback(&self, n: usize, callback: fn(*mut ()), ctx: *mut ()) { - interrupt::free(|cs| { - let alarm = &self.alarms.borrow(cs)[n - 1]; - alarm.callback.set(Some((callback, ctx))); - }) - } - - fn set_alarm(&self, n: usize, timestamp: u64) { - interrupt::free(|cs| { - let alarm = &self.alarms.borrow(cs)[n - 1]; - alarm.timestamp.set(timestamp); - - let t = self.now(); - if timestamp <= t { - self.trigger_alarm(n, cs); - return; - } - - let diff = timestamp - t; - if diff < 0xc000 { - let safe_timestamp = timestamp.max(t + 3); - self.rtc.set_compare(n, safe_timestamp as u16); - self.rtc.set_compare_interrupt(n, true); - } else { - self.rtc.set_compare_interrupt(n, false); - } - }) - } - - pub fn alarm1(&'static self) -> Alarm { - Alarm { n: 1, rtc: self } - } - pub fn alarm2(&'static self) -> Option> { - if T::REAL_ALARM_COUNT >= 2 { - Some(Alarm { n: 2, rtc: self }) - } else { - None - } - } - pub fn alarm3(&'static self) -> Option> { - if T::REAL_ALARM_COUNT >= 3 { - Some(Alarm { n: 3, rtc: self }) - } else { - None - } - } -} - -impl embassy::time::Clock for RTC { - fn now(&self) -> u64 { - let period = self.period.load(Ordering::Relaxed); - compiler_fence(Ordering::Acquire); - let counter = self.rtc.counter(); - calc_now(period, counter) - } -} - -pub struct Alarm { - n: usize, - rtc: &'static RTC, -} - -impl embassy::time::Alarm for Alarm { - fn set_callback(&self, callback: fn(*mut ()), ctx: *mut ()) { - self.rtc.set_alarm_callback(self.n, callback, ctx); - } - - fn set(&self, timestamp: u64) { - self.rtc.set_alarm(self.n, timestamp); - } - - fn clear(&self) { - self.rtc.set_alarm(self.n, u64::MAX); - } -} - -mod sealed { - pub trait Sealed {} -} - -pub trait Instance: sealed::Sealed + Sized + 'static { - type Interrupt: Interrupt; - const REAL_ALARM_COUNT: usize; - - fn enable_clock(&self); - fn set_compare(&self, n: usize, value: u16); - fn set_compare_interrupt(&self, n: usize, enable: bool); - fn compare_interrupt_status(&self, n: usize) -> bool; - fn compare_clear_flag(&self, n: usize); - fn overflow_interrupt_status(&self) -> bool; - fn overflow_clear_flag(&self); - // This method should ensure that the values are really updated before returning - fn set_psc_arr(&self, psc: u16, arr: u16); - fn stop_and_reset(&self); - fn start(&self); - fn counter(&self) -> u16; - fn ppre(clocks: &Clocks) -> u8; - fn pclk(clocks: &Clocks) -> u32; -} - -#[allow(unused_macros)] -macro_rules! impl_timer { - ($module:ident: ($TYPE:ident, $INT:ident, $apbenr:ident, $enrbit:expr, $apbrstr:ident, $rstrbit:expr, $ppre:ident, $pclk: ident), 3) => { - mod $module { - use super::*; - use crate::hal::pac::{$TYPE, RCC}; - - impl sealed::Sealed for $TYPE {} - - impl Instance for $TYPE { - type Interrupt = interrupt::$INT; - const REAL_ALARM_COUNT: usize = 3; - - fn enable_clock(&self) { - // NOTE(unsafe) It will only be used for atomic operations - unsafe { - let rcc = &*RCC::ptr(); - - bb::set(&rcc.$apbenr, $enrbit); - bb::set(&rcc.$apbrstr, $rstrbit); - bb::clear(&rcc.$apbrstr, $rstrbit); - } - } - - fn set_compare(&self, n: usize, value: u16) { - // NOTE(unsafe) these registers accept all the range of u16 values - match n { - 0 => self.ccr1.write(|w| unsafe { w.bits(value.into()) }), - 1 => self.ccr2.write(|w| unsafe { w.bits(value.into()) }), - 2 => self.ccr3.write(|w| unsafe { w.bits(value.into()) }), - 3 => self.ccr4.write(|w| unsafe { w.bits(value.into()) }), - _ => {} - } - } - - fn set_compare_interrupt(&self, n: usize, enable: bool) { - if n > 3 { - return; - } - let bit = n as u8 + 1; - unsafe { - if enable { - bb::set(&self.dier, bit); - } else { - bb::clear(&self.dier, bit); - } - } - } - - fn compare_interrupt_status(&self, n: usize) -> bool { - let status = self.sr.read(); - match n { - 0 => status.cc1if().bit_is_set(), - 1 => status.cc2if().bit_is_set(), - 2 => status.cc3if().bit_is_set(), - 3 => status.cc4if().bit_is_set(), - _ => false, - } - } - - fn compare_clear_flag(&self, n: usize) { - if n > 3 { - return; - } - let bit = n as u8 + 1; - unsafe { - bb::clear(&self.sr, bit); - } - } - - fn overflow_interrupt_status(&self) -> bool { - self.sr.read().uif().bit_is_set() - } - - fn overflow_clear_flag(&self) { - unsafe { - bb::clear(&self.sr, 0); - } - } - - fn set_psc_arr(&self, psc: u16, arr: u16) { - // NOTE(unsafe) All u16 values are valid - self.psc.write(|w| unsafe { w.bits(psc.into()) }); - self.arr.write(|w| unsafe { w.bits(arr.into()) }); - - unsafe { - // Set URS, generate update, clear URS - bb::set(&self.cr1, 2); - self.egr.write(|w| w.ug().set_bit()); - bb::clear(&self.cr1, 2); - } - } - - fn stop_and_reset(&self) { - unsafe { - bb::clear(&self.cr1, 0); - } - self.cnt.reset(); - } - - fn start(&self) { - unsafe { bb::set(&self.cr1, 0) } - } - - fn counter(&self) -> u16 { - self.cnt.read().bits() as u16 - } - - fn ppre(clocks: &Clocks) -> u8 { - clocks.$ppre() - } - - fn pclk(clocks: &Clocks) -> u32 { - clocks.$pclk().0 - } - } - } - }; - - ($module:ident: ($TYPE:ident, $INT:ident, $apbenr:ident, $enrbit:expr, $apbrstr:ident, $rstrbit:expr, $ppre:ident, $pclk: ident), 1) => { - mod $module { - use super::*; - use crate::hal::pac::{$TYPE, RCC}; - - impl sealed::Sealed for $TYPE {} - - impl Instance for $TYPE { - type Interrupt = interrupt::$INT; - const REAL_ALARM_COUNT: usize = 1; - - fn enable_clock(&self) { - // NOTE(unsafe) It will only be used for atomic operations - unsafe { - let rcc = &*RCC::ptr(); - - bb::set(&rcc.$apbenr, $enrbit); - bb::set(&rcc.$apbrstr, $rstrbit); - bb::clear(&rcc.$apbrstr, $rstrbit); - } - } - - fn set_compare(&self, n: usize, value: u16) { - // NOTE(unsafe) these registers accept all the range of u16 values - match n { - 0 => self.ccr1.write(|w| unsafe { w.bits(value.into()) }), - 1 => self.ccr2.write(|w| unsafe { w.bits(value.into()) }), - _ => {} - } - } - - fn set_compare_interrupt(&self, n: usize, enable: bool) { - if n > 1 { - return; - } - let bit = n as u8 + 1; - unsafe { - if enable { - bb::set(&self.dier, bit); - } else { - bb::clear(&self.dier, bit); - } - } - } - - fn compare_interrupt_status(&self, n: usize) -> bool { - let status = self.sr.read(); - match n { - 0 => status.cc1if().bit_is_set(), - 1 => status.cc2if().bit_is_set(), - _ => false, - } - } - - fn compare_clear_flag(&self, n: usize) { - if n > 1 { - return; - } - let bit = n as u8 + 1; - unsafe { - bb::clear(&self.sr, bit); - } - } - - fn overflow_interrupt_status(&self) -> bool { - self.sr.read().uif().bit_is_set() - } - - fn overflow_clear_flag(&self) { - unsafe { - bb::clear(&self.sr, 0); - } - } - - fn set_psc_arr(&self, psc: u16, arr: u16) { - // NOTE(unsafe) All u16 values are valid - self.psc.write(|w| unsafe { w.bits(psc.into()) }); - self.arr.write(|w| unsafe { w.bits(arr.into()) }); - - unsafe { - // Set URS, generate update, clear URS - bb::set(&self.cr1, 2); - self.egr.write(|w| w.ug().set_bit()); - bb::clear(&self.cr1, 2); - } - } - - fn stop_and_reset(&self) { - unsafe { - bb::clear(&self.cr1, 0); - } - self.cnt.reset(); - } - - fn start(&self) { - unsafe { bb::set(&self.cr1, 0) } - } - - fn counter(&self) -> u16 { - self.cnt.read().bits() as u16 - } - - fn ppre(clocks: &Clocks) -> u8 { - clocks.$ppre() - } - - fn pclk(clocks: &Clocks) -> u32 { - clocks.$pclk().0 - } - } - } - }; -} - -#[cfg(not(feature = "stm32f410"))] -impl_timer!(tim2: (TIM2, TIM2, apb1enr, 0, apb1rstr, 0, ppre1, pclk1), 3); - -#[cfg(not(feature = "stm32f410"))] -impl_timer!(tim3: (TIM3, TIM3, apb1enr, 1, apb1rstr, 1, ppre1, pclk1), 3); - -#[cfg(not(feature = "stm32f410"))] -impl_timer!(tim4: (TIM4, TIM4, apb1enr, 2, apb1rstr, 2, ppre1, pclk1), 3); - -impl_timer!(tim5: (TIM5, TIM5, apb1enr, 3, apb1rstr, 3, ppre1, pclk1), 3); - -impl_timer!(tim9: (TIM9, TIM1_BRK_TIM9, apb2enr, 16, apb2rstr, 16, ppre2, pclk2), 1); - -#[cfg(not(any(feature = "stm32f401", feature = "stm32f410", feature = "stm32f411")))] -impl_timer!(tim12: (TIM12, TIM8_BRK_TIM12, apb1enr, 6, apb1rstr, 6, ppre1, pclk1), 1);