3 files changed, 97 insertions, 52 deletions

diff --git a/embassy-stm32/src/sdmmc/mod.rs b/embassy-stm32/src/sdmmc/mod.rs
index 12086cd3a..e716fc348 100644
--- a/embassy-stm32/src/sdmmc/mod.rs
+++ b/embassy-stm32/src/sdmmc/mod.rs
@@ -7,6 +7,7 @@ use core::marker::PhantomData;
 use core::slice;
 use core::task::Poll;
 
+use aligned::{A4, Aligned};
 use embassy_hal_internal::{Peri, PeripheralType};
 use embassy_sync::waitqueue::AtomicWaker;
 use sdio_host::Cmd;
@@ -139,16 +140,38 @@ impl Default for Signalling {
     }
 }
 
+const fn aligned_mut(x: &mut [u32]) -> &mut Aligned<A4, [u8]> {
+    let len = x.len() * 4;
+    unsafe { core::mem::transmute(slice::from_raw_parts_mut(x.as_mut_ptr() as _, len)) }
+}
+
 const fn slice8_mut(x: &mut [u32]) -> &mut [u8] {
     let len = x.len() * 4;
     unsafe { slice::from_raw_parts_mut(x.as_mut_ptr() as _, len) }
 }
 
+#[allow(unused)]
+const fn slice32_mut(x: &mut Aligned<A4, [u8]>) -> &mut [u32] {
+    let len = (size_of_val(x) + 4 - 1) / 4;
+    unsafe { slice::from_raw_parts_mut(x as *mut Aligned<A4, [u8]> as *mut _, len) }
+}
+
+const fn aligned_ref(x: &[u32]) -> &Aligned<A4, [u8]> {
+    let len = x.len() * 4;
+    unsafe { core::mem::transmute(slice::from_raw_parts(x.as_ptr() as _, len)) }
+}
+
 const fn slice8_ref(x: &[u32]) -> &[u8] {
     let len = x.len() * 4;
     unsafe { slice::from_raw_parts(x.as_ptr() as _, len) }
 }
 
+#[allow(unused)]
+const fn slice32_ref(x: &Aligned<A4, [u8]>) -> &[u32] {
+    let len = (size_of_val(x) + 4 - 1) / 4;
+    unsafe { slice::from_raw_parts(x as *const Aligned<A4, [u8]> as *const _, len) }
+}
+
 /// Errors
 #[non_exhaustive]
 #[derive(Debug, Copy, Clone, PartialEq, Eq)]
@@ -187,6 +210,11 @@ enum PowerCtrl {
     On = 0b11,
 }
 
+enum DatapathMode {
+    Block(BlockSize),
+    Byte,
+}
+
 fn get_waitresp_val(rlen: ResponseLen) -> u8 {
     match rlen {
         common_cmd::ResponseLen::Zero => 0,
@@ -768,12 +796,16 @@ impl<'d> Sdmmc<'d> {
     #[allow(unused_variables)]
     fn prepare_datapath_read<'a>(
         &'a self,
-        buffer: &'a mut [u32],
-        block_size: BlockSize,
-        byte_mode: bool,
+        buffer: &'a mut Aligned<A4, [u8]>,
+        mode: DatapathMode,
     ) -> WrappedTransfer<'a> {
         let regs = self.info.regs;
 
+        let (byte_mode, block_size) = match mode {
+            DatapathMode::Block(block_size) => (false, block_size as u8),
+            DatapathMode::Byte => (true, 0),
+        };
+
         // Command AND Data state machines must be idle
         self.wait_idle();
         self.clear_interrupt_flags();
@@ -783,8 +815,11 @@ impl<'d> Sdmmc<'d> {
         // SAFETY: No other functions use the dma
         #[cfg(sdmmc_v1)]
         let transfer = unsafe {
-            self.dma
-                .read_unchecked(regs.fifor().as_ptr() as *mut u32, buffer, DMA_TRANSFER_OPTIONS)
+            self.dma.read_unchecked(
+                regs.fifor().as_ptr() as *mut u32,
+                slice32_mut(buffer),
+                DMA_TRANSFER_OPTIONS,
+            )
         };
         #[cfg(sdmmc_v2)]
         let transfer = {
@@ -817,14 +852,14 @@ impl<'d> Sdmmc<'d> {
     /// # Safety
     ///
     /// `buffer` must be valid for the whole transfer and word aligned
-    fn prepare_datapath_write<'a>(
-        &'a self,
-        buffer: &'a [u32],
-        block_size: BlockSize,
-        byte_mode: bool,
-    ) -> WrappedTransfer<'a> {
+    fn prepare_datapath_write<'a>(&'a self, buffer: &'a Aligned<A4, [u8]>, mode: DatapathMode) -> WrappedTransfer<'a> {
         let regs = self.info.regs;
 
+        let (byte_mode, block_size) = match mode {
+            DatapathMode::Block(block_size) => (false, block_size as u8),
+            DatapathMode::Byte => (true, 0),
+        };
+
         // Command AND Data state machines must be idle
         self.wait_idle();
         self.clear_interrupt_flags();
@@ -834,8 +869,11 @@ impl<'d> Sdmmc<'d> {
         // SAFETY: No other functions use the dma
         #[cfg(sdmmc_v1)]
         let transfer = unsafe {
-            self.dma
-                .write_unchecked(buffer, regs.fifor().as_ptr() as *mut u32, DMA_TRANSFER_OPTIONS)
+            self.dma.write_unchecked(
+                slice32_ref(buffer),
+                regs.fifor().as_ptr() as *mut u32,
+                DMA_TRANSFER_OPTIONS,
+            )
         };
         #[cfg(sdmmc_v2)]
         let transfer = {
diff --git a/embassy-stm32/src/sdmmc/sd.rs b/embassy-stm32/src/sdmmc/sd.rs
index 6190226b8..20318bbfa 100644
--- a/embassy-stm32/src/sdmmc/sd.rs
+++ b/embassy-stm32/src/sdmmc/sd.rs
@@ -5,7 +5,10 @@ use sdio_host::emmc::{EMMC, ExtCSD};
 use sdio_host::sd::{BusWidth, CIC, CID, CSD, CardCapacity, CardStatus, CurrentState, OCR, RCA, SCR, SD, SDStatus};
 use sdio_host::{common_cmd, emmc_cmd, sd_cmd};
 
-use crate::sdmmc::{BlockSize, Error, Sdmmc, Signalling, block_size, bus_width_vals, slice8_mut, slice8_ref};
+use crate::sdmmc::{
+    BlockSize, DatapathMode, Error, Sdmmc, Signalling, aligned_mut, aligned_ref, block_size, bus_width_vals,
+    slice8_mut, slice8_ref,
+};
 use crate::time::{Hertz, mhz};
 
 /// Aligned data block for SDMMC transfers.
@@ -230,10 +233,8 @@ impl<'a, 'b> StorageDevice<'a, 'b, Card> {
             };
 
         let buffer = &mut cmd_block.0[..64 / 4];
-
-        let transfer = self
-            .sdmmc
-            .prepare_datapath_read(buffer, block_size(size_of_val(buffer)), false);
+        let mode = DatapathMode::Block(block_size(size_of_val(buffer)));
+        let transfer = self.sdmmc.prepare_datapath_read(aligned_mut(buffer), mode);
 
         self.sdmmc.cmd(sd_cmd::cmd6(set_function), true)?; // CMD6
 
@@ -272,7 +273,9 @@ impl<'a, 'b> StorageDevice<'a, 'b, Card> {
 
         // Arm `OnDrop` after the buffer, so it will be dropped first
 
-        let transfer = self.sdmmc.prepare_datapath_read(scr, BlockSize::Size8, false);
+        let transfer = self
+            .sdmmc
+            .prepare_datapath_read(aligned_mut(scr), DatapathMode::Block(BlockSize::Size8));
         self.sdmmc.cmd(sd_cmd::send_scr(), true)?;
 
         self.sdmmc.complete_datapath_transfer(transfer, true).await?;
@@ -290,10 +293,9 @@ impl<'a, 'b> StorageDevice<'a, 'b, Card> {
         self.sdmmc.cmd(common_cmd::app_cmd(rca), false)?; // APP
 
         let buffer = &mut cmd_block.as_mut()[..64 / 4];
+        let mode = DatapathMode::Block(block_size(size_of_val(buffer)));
 
-        let transfer = self
-            .sdmmc
-            .prepare_datapath_read(buffer, block_size(size_of_val(buffer)), false);
+        let transfer = self.sdmmc.prepare_datapath_read(aligned_mut(buffer), mode);
         self.sdmmc.cmd(sd_cmd::sd_status(), true)?;
 
         self.sdmmc.complete_datapath_transfer(transfer, true).await?;
@@ -398,9 +400,10 @@ impl<'a, 'b> StorageDevice<'a, 'b, Emmc> {
             .cmd(common_cmd::set_block_length(size_of::<DataBlock>() as u32), false)
             .unwrap(); // CMD16
 
-        let transfer = self
-            .sdmmc
-            .prepare_datapath_read(&mut data_block.0, block_size(size_of::<DataBlock>()), false);
+        let transfer = self.sdmmc.prepare_datapath_read(
+            aligned_mut(&mut data_block.0),
+            DatapathMode::Block(block_size(size_of::<DataBlock>())),
+        );
         self.sdmmc.cmd(emmc_cmd::send_ext_csd(), true)?;
 
         self.sdmmc.complete_datapath_transfer(transfer, true).await?;
@@ -418,7 +421,7 @@ impl<'a, 'b, A: Addressable> StorageDevice<'a, 'b, A> {
 
     /// Read a data block.
     #[inline]
-    pub async fn read_block(&mut self, block_idx: u32, buffer: &mut DataBlock) -> Result<(), Error> {
+    pub async fn read_block(&mut self, block_idx: u32, data_block: &mut DataBlock) -> Result<(), Error> {
         let _scoped_block_stop = self.sdmmc.info.rcc.block_stop();
         let card_capacity = self.info.get_capacity();
 
@@ -431,9 +434,10 @@ impl<'a, 'b, A: Addressable> StorageDevice<'a, 'b, A> {
         self.sdmmc
             .cmd(common_cmd::set_block_length(size_of::<DataBlock>() as u32), false)?; // CMD16
 
-        let transfer = self
-            .sdmmc
-            .prepare_datapath_read(&mut buffer.0, block_size(size_of::<DataBlock>()), false);
+        let transfer = self.sdmmc.prepare_datapath_read(
+            aligned_mut(&mut data_block.0),
+            DatapathMode::Block(block_size(size_of::<DataBlock>())),
+        );
         self.sdmmc.cmd(common_cmd::read_single_block(address), true)?;
 
         self.sdmmc.complete_datapath_transfer(transfer, true).await?;
@@ -464,9 +468,10 @@ impl<'a, 'b, A: Addressable> StorageDevice<'a, 'b, A> {
         self.sdmmc
             .cmd(common_cmd::set_block_length(size_of::<DataBlock>() as u32), false)?; // CMD16
 
-        let transfer = self
-            .sdmmc
-            .prepare_datapath_read(buffer, block_size(size_of::<DataBlock>()), false);
+        let transfer = self.sdmmc.prepare_datapath_read(
+            aligned_mut(buffer),
+            DatapathMode::Block(block_size(size_of::<DataBlock>())),
+        );
         self.sdmmc.cmd(common_cmd::read_multiple_blocks(address), true)?;
 
         self.sdmmc.complete_datapath_transfer(transfer, false).await?;
@@ -497,9 +502,10 @@ impl<'a, 'b, A: Addressable> StorageDevice<'a, 'b, A> {
         #[cfg(sdmmc_v1)]
         self.sdmmc.cmd(common_cmd::write_single_block(address), true)?;
 
-        let transfer = self
-            .sdmmc
-            .prepare_datapath_write(&buffer.0, block_size(size_of::<DataBlock>()), false);
+        let transfer = self.sdmmc.prepare_datapath_write(
+            aligned_ref(&buffer.0),
+            DatapathMode::Block(block_size(size_of::<DataBlock>())),
+        );
 
         #[cfg(sdmmc_v2)]
         self.sdmmc.cmd(common_cmd::write_single_block(address), true)?;
@@ -548,10 +554,10 @@ impl<'a, 'b, A: Addressable> StorageDevice<'a, 'b, A> {
         self.sdmmc.cmd(common_cmd::write_multiple_blocks(address), true)?; // CMD25
 
         // Setup write command
-        let transfer = self
-            .sdmmc
-            .prepare_datapath_write(buffer, block_size(size_of::<DataBlock>()), false);
-
+        let transfer = self.sdmmc.prepare_datapath_write(
+            aligned_ref(buffer),
+            DatapathMode::Block(block_size(size_of::<DataBlock>())),
+        );
         #[cfg(sdmmc_v2)]
         self.sdmmc.cmd(common_cmd::write_multiple_blocks(address), true)?; // CMD25
 
diff --git a/embassy-stm32/src/sdmmc/sdio.rs b/embassy-stm32/src/sdmmc/sdio.rs
index 1412b21fc..e436d68cb 100644
--- a/embassy-stm32/src/sdmmc/sdio.rs
+++ b/embassy-stm32/src/sdmmc/sdio.rs
@@ -1,10 +1,11 @@
 use core::ops::{Deref, DerefMut};
 
+use aligned::{A4, Aligned};
 use sdio_host::common_cmd::{R1, Rz, cmd};
 use sdio_host::sd::BusWidth;
 use sdio_host::sd_cmd;
 
-use crate::sdmmc::{Error, Sdmmc, block_size, slice8_mut, slice8_ref};
+use crate::sdmmc::{DatapathMode, Error, Sdmmc, aligned_mut, aligned_ref, block_size, slice8_mut, slice8_ref};
 use crate::time::Hertz;
 
 /// Aligned data block for SDMMC transfers.
@@ -39,7 +40,7 @@ impl DerefMut for DataBlock {
 /// Storage Device
 pub struct SerialDataInterface<'a, 'b> {
     /// Inner member
-    pub sdmmc: &'a mut Sdmmc<'b>,
+    sdmmc: &'a mut Sdmmc<'b>,
 }
 
 /// Card Storage Device
@@ -101,9 +102,10 @@ impl<'a, 'b> SerialDataInterface<'a, 'b> {
             )
         };
 
-        let transfer = self
-            .sdmmc
-            .prepare_datapath_read(buffer, block_size(size_of::<DataBlock>()), false);
+        let transfer = self.sdmmc.prepare_datapath_read(
+            aligned_mut(buffer),
+            DatapathMode::Block(block_size(size_of::<DataBlock>())),
+        );
         self.sdmmc.cmd(cmd::<Rz>(53, arg), true)?;
 
         self.sdmmc.complete_datapath_transfer(transfer, false).await?;
@@ -113,12 +115,10 @@ impl<'a, 'b> SerialDataInterface<'a, 'b> {
     }
 
     /// Read in multibyte mode using cmd53
-    pub async fn cmd53_byte_read(&mut self, arg: u32, buffer: &mut [u32]) -> Result<(), Error> {
+    pub async fn cmd53_byte_read(&mut self, arg: u32, buffer: &mut Aligned<A4, [u8]>) -> Result<(), Error> {
         let _scoped_block_stop = self.sdmmc.info.rcc.block_stop();
 
-        let transfer = self
-            .sdmmc
-            .prepare_datapath_read(buffer, block_size(size_of::<DataBlock>()), true);
+        let transfer = self.sdmmc.prepare_datapath_read(buffer, DatapathMode::Byte);
         self.sdmmc.cmd(cmd::<Rz>(53, arg), true)?;
 
         self.sdmmc.complete_datapath_transfer(transfer, false).await?;
@@ -142,9 +142,10 @@ impl<'a, 'b> SerialDataInterface<'a, 'b> {
         #[cfg(sdmmc_v1)]
         self.sdmmc.cmd(cmd::<Rz>(53, arg), true)?;
 
-        let transfer = self
-            .sdmmc
-            .prepare_datapath_read(buffer, block_size(size_of::<DataBlock>()), false);
+        let transfer = self.sdmmc.prepare_datapath_write(
+            aligned_ref(buffer),
+            DatapathMode::Block(block_size(size_of::<DataBlock>())),
+        );
 
         #[cfg(sdmmc_v2)]
         self.sdmmc.cmd(cmd::<Rz>(53, arg), true)?;
@@ -164,7 +165,7 @@ impl<'a, 'b> SerialDataInterface<'a, 'b> {
 
         let transfer = self
             .sdmmc
-            .prepare_datapath_write(buffer, block_size(size_of::<DataBlock>()), true);
+            .prepare_datapath_write(aligned_ref(buffer), DatapathMode::Byte);
 
         #[cfg(sdmmc_v2)]
         self.sdmmc.cmd(cmd::<Rz>(53, arg), true)?;