2 files changed, 403 insertions, 317 deletions

diff --git a/embassy-stm32/src/sdmmc/mod.rs b/embassy-stm32/src/sdmmc/mod.rs
index 74a6f13fa..f862d73b1 100644
--- a/embassy-stm32/src/sdmmc/mod.rs
+++ b/embassy-stm32/src/sdmmc/mod.rs
@@ -9,7 +9,6 @@ use core::task::Poll;
 
 use embassy_hal_internal::drop::OnDrop;
 use embassy_hal_internal::{Peri, PeripheralType};
-use embassy_sync::mutex::Mutex;
 use embassy_sync::waitqueue::AtomicWaker;
 use sdio_host::common_cmd::{self, Resp, ResponseLen};
 use sdio_host::emmc::{EMMC, ExtCSD};
@@ -165,6 +164,55 @@ pub enum Error {
     StBitErr,
 }
 
+pub trait Addressable: Sized {
+    type Ext;
+
+    /// Get this peripheral's address on the SDMMC bus
+    fn get_address(&self) -> u16;
+
+    /// Is this a standard or high capacity peripheral?
+    fn get_capacity(&self) -> CardCapacity;
+
+    /// Size in bytes
+    fn size(&self) -> u64;
+
+    async fn write_block<'a>(
+        &mut self,
+        sdmmc: &mut Sdmmc<'a>,
+        block_idx: u32,
+        buffer: &DataBlock,
+    ) -> Result<(), Error> {
+        sdmmc.write_block(self, block_idx, buffer).await
+    }
+
+    async fn write_blocks<'a>(
+        &mut self,
+        sdmmc: &mut Sdmmc<'a>,
+        block_idx: u32,
+        blocks: &[DataBlock],
+    ) -> Result<(), Error> {
+        sdmmc.write_blocks(self, block_idx, blocks).await
+    }
+
+    async fn read_block<'a>(
+        &mut self,
+        sdmmc: &mut Sdmmc<'a>,
+        block_idx: u32,
+        buffer: &mut DataBlock,
+    ) -> Result<(), Error> {
+        sdmmc.read_block(self, block_idx, buffer).await
+    }
+
+    async fn read_blocks<'a>(
+        &mut self,
+        sdmmc: &mut Sdmmc<'a>,
+        block_idx: u32,
+        blocks: &mut [DataBlock],
+    ) -> Result<(), Error> {
+        sdmmc.read_blocks(self, block_idx, blocks).await
+    }
+}
+
 #[derive(Clone, Copy, Debug, Default)]
 /// SD Card
 pub struct Card {
@@ -184,6 +232,178 @@ pub struct Card {
     pub status: SDStatus,
 }
 
+impl Card {
+    /// Switch mode using CMD6.
+    ///
+    /// Attempt to set a new signalling mode. The selected
+    /// signalling mode is returned. Expects the current clock
+    /// frequency to be > 12.5MHz.
+    ///
+    /// SD only.
+    pub async fn switch_signalling_mode<'a>(
+        &mut self,
+        sdmmc: &mut Sdmmc<'a>,
+        cmd_block: &mut CmdBlock,
+        signalling: Signalling,
+    ) -> Result<Signalling, Error> {
+        // NB PLSS v7_10 4.3.10.4: "the use of SET_BLK_LEN command is not
+        // necessary"
+
+        let set_function = 0x8000_0000
+            | match signalling {
+                // See PLSS v7_10 Table 4-11
+                Signalling::DDR50 => 0xFF_FF04,
+                Signalling::SDR104 => 0xFF_1F03,
+                Signalling::SDR50 => 0xFF_1F02,
+                Signalling::SDR25 => 0xFF_FF01,
+                Signalling::SDR12 => 0xFF_FF00,
+            };
+
+        // Arm `OnDrop` after the buffer, so it will be dropped first
+        let on_drop = OnDrop::new(|| sdmmc.on_drop());
+
+        let transfer = sdmmc.prepare_datapath_read(
+            &sdmmc.config,
+            #[cfg(sdmmc_v1)]
+            &mut self.dma,
+            cmd_block.as_mut(),
+            64,
+            6,
+        );
+        sdmmc.enable_interrupts();
+        sdmmc.cmd(sd_cmd::cmd6(set_function), true)?; // CMD6
+
+        let res = sdmmc.complete_datapath_transfer(true).await;
+
+        // Host is allowed to use the new functions at least 8
+        // clocks after the end of the switch command
+        // transaction. We know the current clock period is < 80ns,
+        // so a total delay of 640ns is required here
+        for _ in 0..300 {
+            cortex_m::asm::nop();
+        }
+
+        match res {
+            Ok(_) => {
+                on_drop.defuse();
+                sdmmc.stop_datapath();
+                drop(transfer);
+
+                // Function Selection of Function Group 1
+                let selection = (u32::from_be(cmd_block[4]) >> 24) & 0xF;
+
+                match selection {
+                    0 => Ok(Signalling::SDR12),
+                    1 => Ok(Signalling::SDR25),
+                    2 => Ok(Signalling::SDR50),
+                    3 => Ok(Signalling::SDR104),
+                    4 => Ok(Signalling::DDR50),
+                    _ => Err(Error::UnsupportedCardType),
+                }
+            }
+            Err(e) => Err(e),
+        }
+    }
+
+    /// Reads the SCR register.
+    ///
+    /// SD only.
+    pub async fn get_scr<'a>(&mut self, sdmmc: &mut Sdmmc<'a>, cmd_block: &mut CmdBlock) -> Result<(), Error> {
+        // Read the 64-bit SCR register
+        sdmmc.cmd(common_cmd::set_block_length(8), false)?; // CMD16
+        sdmmc.cmd(common_cmd::app_cmd(self.rca), false)?;
+
+        let scr = &mut cmd_block.0[..2];
+
+        // Arm `OnDrop` after the buffer, so it will be dropped first
+        let on_drop = OnDrop::new(|| sdmmc.on_drop());
+
+        let transfer = sdmmc.prepare_datapath_read(
+            &sdmmc.config,
+            #[cfg(sdmmc_v1)]
+            &mut self.dma,
+            scr,
+            8,
+            3,
+        );
+        sdmmc.enable_interrupts();
+        sdmmc.cmd(sd_cmd::send_scr(), true)?;
+
+        let res = sdmmc.complete_datapath_transfer(true).await;
+
+        if res.is_ok() {
+            on_drop.defuse();
+            sdmmc.stop_datapath();
+            drop(transfer);
+
+            unsafe {
+                let scr_bytes = &*(&scr as *const _ as *const [u8; 8]);
+                self.scr = SCR(u64::from_be_bytes(*scr_bytes));
+            }
+        }
+        res
+    }
+
+    /// Reads the SD Status (ACMD13)
+    ///
+    /// SD only.
+    pub async fn read_sd_status<'a>(&mut self, sdmmc: &mut Sdmmc<'a>, cmd_block: &mut CmdBlock) -> Result<(), Error> {
+        let rca = self.rca;
+
+        sdmmc.cmd(common_cmd::set_block_length(64), false)?; // CMD16
+        sdmmc.cmd(common_cmd::app_cmd(rca), false)?; // APP
+
+        let status = cmd_block;
+
+        // Arm `OnDrop` after the buffer, so it will be dropped first
+        let on_drop = OnDrop::new(|| sdmmc.on_drop());
+
+        let transfer = sdmmc.prepare_datapath_read(
+            &sdmmc.config,
+            #[cfg(sdmmc_v1)]
+            &mut self.dma,
+            status.as_mut(),
+            64,
+            6,
+        );
+        sdmmc.enable_interrupts();
+        sdmmc.cmd(sd_cmd::sd_status(), true)?;
+
+        let res = sdmmc.complete_datapath_transfer(true).await;
+
+        if res.is_ok() {
+            on_drop.defuse();
+            sdmmc.stop_datapath();
+            drop(transfer);
+
+            for byte in status.iter_mut() {
+                *byte = u32::from_be(*byte);
+            }
+            self.status = status.0.into();
+        }
+        res
+    }
+}
+
+impl Addressable for Card {
+    type Ext = SD;
+
+    /// Get this peripheral's address on the SDMMC bus
+    fn get_address(&self) -> u16 {
+        self.rca
+    }
+
+    /// Is this a standard or high capacity peripheral?
+    fn get_capacity(&self) -> CardCapacity {
+        self.card_type
+    }
+
+    /// Size in bytes
+    fn size(&self) -> u64 {
+        u64::from(self.csd.block_count()) * 512
+    }
+}
+
 #[derive(Clone, Copy, Debug, Default)]
 /// eMMC storage
 pub struct Emmc {
@@ -201,6 +421,66 @@ pub struct Emmc {
     pub ext_csd: ExtCSD,
 }
 
+impl Emmc {
+    /// Gets the EXT_CSD register.
+    ///
+    /// eMMC only.
+    async fn read_ext_csd<'a>(&mut self, sdmmc: &mut Sdmmc<'a>) -> Result<(), Error> {
+        // Note: cmd_block can't be used because ExtCSD is too long to fit.
+        let mut data_block = DataBlock([0u8; 512]);
+
+        // NOTE(unsafe) DataBlock uses align 4
+        let buffer = unsafe { &mut *((&mut data_block.0) as *mut [u8; 512] as *mut [u32; 128]) };
+
+        sdmmc.cmd(common_cmd::set_block_length(512), false).unwrap(); // CMD16
+
+        let transfer = sdmmc.prepare_datapath_read(
+            &sdmmc.config,
+            #[cfg(sdmmc_v1)]
+            &mut self.dma,
+            buffer,
+            512,
+            9,
+        );
+        sdmmc.enable_interrupts();
+        sdmmc.cmd(emmc_cmd::send_ext_csd(), true)?;
+
+        // Arm `OnDrop` after the buffer, so it will be dropped first
+        let on_drop = OnDrop::new(|| sdmmc.on_drop());
+
+        let res = sdmmc.complete_datapath_transfer(true).await;
+
+        if res.is_ok() {
+            on_drop.defuse();
+            sdmmc.stop_datapath();
+            drop(transfer);
+
+            self.ext_csd = unsafe { core::mem::transmute::<_, [u32; 128]>(data_block.0) }.into();
+        }
+
+        res
+    }
+}
+
+impl Addressable for Emmc {
+    type Ext = EMMC;
+
+    /// Get this peripheral's address on the SDMMC bus
+    fn get_address(&self) -> u16 {
+        self.rca
+    }
+
+    /// Is this a standard or high capacity peripheral?
+    fn get_capacity(&self) -> CardCapacity {
+        self.capacity
+    }
+
+    /// Size in bytes
+    fn size(&self) -> u64 {
+        u64::from(self.ext_csd.sector_count()) * 512
+    }
+}
+
 #[repr(u8)]
 enum PowerCtrl {
     Off = 0b00,
@@ -386,12 +666,6 @@ pub struct Sdmmc<'d> {
     clock: Hertz,
     /// Current signalling scheme to card
     signalling: Signalling,
-    /// Card
-    card: Option<SdmmcPeripheral>,
-
-    /// An optional buffer to be used for commands
-    /// This should be used if there are special memory location requirements for dma
-    cmd_block: Option<&'d mut CmdBlock>,
 }
 
 const CLK_AF: AfType = AfType::output(OutputType::PushPull, Speed::VeryHigh);
@@ -725,8 +999,6 @@ impl<'d> Sdmmc<'d> {
             config,
             clock: SD_INIT_FREQ,
             signalling: Default::default(),
-            card: None,
-            cmd_block: None,
         }
     }
 
@@ -889,9 +1161,9 @@ impl<'d> Sdmmc<'d> {
     }
 
     /// Query the card status (CMD13, returns R1)
-    fn read_status<Ext>(&self, card: &SdmmcPeripheral) -> Result<CardStatus<Ext>, Error>
+    fn read_status<A: Addressable>(&self, card: &A) -> Result<CardStatus<A::Ext>, Error>
     where
-        CardStatus<Ext>: From<u32>,
+        CardStatus<A::Ext>: From<u32>,
     {
         let regs = self.info.regs;
         let rca = card.get_address();
@@ -1080,9 +1352,14 @@ impl<'d> Sdmmc<'d> {
 
     /// 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,
+        card: &mut impl Addressable,
+        block_idx: u32,
+        buffer: &mut DataBlock,
+    ) -> Result<(), Error> {
         let _scoped_block_stop = self.info.rcc.block_stop();
-        let card_capacity = self.card()?.get_capacity();
+        let card_capacity = card.get_capacity();
 
         // NOTE(unsafe) DataBlock uses align 4
         let buffer = unsafe { &mut *((&mut buffer.0) as *mut [u8; 512] as *mut [u32; 128]) };
@@ -1120,9 +1397,14 @@ impl<'d> Sdmmc<'d> {
 
     /// Read multiple data blocks.
     #[inline]
-    pub async fn read_blocks(&mut self, block_idx: u32, blocks: &mut [DataBlock]) -> Result<(), Error> {
+    pub async fn read_blocks(
+        &mut self,
+        card: &mut impl Addressable,
+        block_idx: u32,
+        blocks: &mut [DataBlock],
+    ) -> Result<(), Error> {
         let _scoped_block_stop = self.info.rcc.block_stop();
-        let card_capacity = self.card()?.get_capacity();
+        let card_capacity = card.get_capacity();
 
         // NOTE(unsafe) reinterpret buffer as &mut [u32]
         let buffer = unsafe {
@@ -1167,9 +1449,16 @@ impl<'d> Sdmmc<'d> {
     }
 
     /// Write a data block.
-    pub async fn write_block(&mut self, block_idx: u32, buffer: &DataBlock) -> Result<(), Error> {
+    pub async fn write_block<A: Addressable>(
+        &mut self,
+        card: &mut A,
+        block_idx: u32,
+        buffer: &DataBlock,
+    ) -> Result<(), Error>
+    where
+        CardStatus<A::Ext>: From<u32>,
+    {
         let _scoped_block_stop = self.info.rcc.block_stop();
-        let card = self.card.as_mut().ok_or(Error::NoCard)?;
 
         // NOTE(unsafe) DataBlock uses align 4
         let buffer = unsafe { &*((&buffer.0) as *const [u8; 512] as *const [u32; 128]) };
@@ -1205,13 +1494,8 @@ impl<'d> Sdmmc<'d> {
                 // TODO: Make this configurable
                 let mut timeout: u32 = 0x00FF_FFFF;
 
-                let card = self.card.as_ref().unwrap();
                 while timeout > 0 {
-                    let ready_for_data = match card {
-                        SdmmcPeripheral::Emmc(_) => self.read_status::<EMMC>(card)?.ready_for_data(),
-                        SdmmcPeripheral::SdCard(_) => self.read_status::<SD>(card)?.ready_for_data(),
-                    };
-
+                    let ready_for_data = self.read_status(card)?.ready_for_data();
                     if ready_for_data {
                         return Ok(());
                     }
@@ -1224,9 +1508,16 @@ impl<'d> Sdmmc<'d> {
     }
 
     /// Write multiple data blocks.
-    pub async fn write_blocks(&mut self, block_idx: u32, blocks: &[DataBlock]) -> Result<(), Error> {
+    pub async fn write_blocks<A: Addressable>(
+        &mut self,
+        card: &mut A,
+        block_idx: u32,
+        blocks: &[DataBlock],
+    ) -> Result<(), Error>
+    where
+        CardStatus<A::Ext>: From<u32>,
+    {
         let _scoped_block_stop = self.info.rcc.block_stop();
-        let card = self.card.as_mut().ok_or(Error::NoCard)?;
 
         // NOTE(unsafe) reinterpret buffer as &[u32]
         let buffer = unsafe {
@@ -1272,12 +1563,11 @@ impl<'d> Sdmmc<'d> {
                 // TODO: Make this configurable
                 let mut timeout: u32 = 0x00FF_FFFF;
 
-                // Try to read card status (ACMD13)
                 while timeout > 0 {
-                    match self.read_sd_status().await {
-                        Ok(_) => return Ok(()),
-                        Err(Error::Timeout) => (), // Try again
-                        Err(e) => return Err(e),
+                    let ready_for_data = self.read_status(card)?.ready_for_data();
+
+                    if ready_for_data {
+                        return Ok(());
                     }
                     timeout -= 1;
                 }
@@ -1287,31 +1577,17 @@ impl<'d> Sdmmc<'d> {
         }
     }
 
-    /// Get a reference to the initialized card
-    ///
-    /// # Errors
-    ///
-    /// Returns Error::NoCard if [`init_sd_card`](#method.init_sd_card) or
-    /// [`init_emmc`](#method.init_emmc) has not previously succeeded
-    #[inline]
-    pub fn card(&self) -> Result<&SdmmcPeripheral, Error> {
-        self.card.as_ref().ok_or(Error::NoCard)
-    }
-
     /// Get the current SDMMC bus clock
     pub fn clock(&self) -> Hertz {
         self.clock
     }
 
-    /// Set a specific cmd buffer rather than using the default stack allocated one.
-    /// This is required if stack RAM cannot be used with DMA and usually manifests
-    /// itself as an indefinite wait on a dma transfer because the dma peripheral
-    /// cannot access the memory.
-    pub fn set_cmd_block(&mut self, cmd_block: &'d mut CmdBlock) {
-        self.cmd_block = Some(cmd_block)
-    }
-
-    async fn init_internal(&mut self, freq: Hertz, mut card: SdmmcPeripheral) -> Result<(), Error> {
+    async fn init_internal(
+        &mut self,
+        cmd_block: &mut CmdBlock,
+        freq: Hertz,
+        card: &mut SdmmcPeripheral,
+    ) -> Result<(), Error> {
         let regs = self.info.regs;
 
         let bus_width = match (self.d3.is_some(), self.d7.is_some()) {
@@ -1346,7 +1622,7 @@ impl<'d> Sdmmc<'d> {
         self.cmd(common_cmd::idle(), false)?;
 
         match card {
-            SdmmcPeripheral::SdCard(ref mut card) => {
+            SdmmcPeripheral::SdCard(card) => {
                 // Check if cards supports CMD8 (with pattern)
                 self.cmd(sd_cmd::send_if_cond(1, 0xAA), false)?;
                 let cic = CIC::from(regs.respr(0).read().cardstatus());
@@ -1387,7 +1663,7 @@ impl<'d> Sdmmc<'d> {
                 }
                 card.ocr = ocr;
             }
-            SdmmcPeripheral::Emmc(ref mut emmc) => {
+            SdmmcPeripheral::Emmc(emmc) => {
                 let ocr = loop {
                     let high_voltage = 0b0 << 7;
                     let access_mode = 0b10 << 29;
@@ -1423,14 +1699,14 @@ impl<'d> Sdmmc<'d> {
         let cid = (cid0 << 96) | (cid1 << 64) | (cid2 << 32) | (cid3);
 
         match card {
-            SdmmcPeripheral::SdCard(ref mut card) => {
+            SdmmcPeripheral::SdCard(card) => {
                 card.cid = cid.into();
 
                 self.cmd(sd_cmd::send_relative_address(), false)?;
                 let rca = RCA::<SD>::from(regs.respr(0).read().cardstatus());
                 card.rca = rca.address();
             }
-            SdmmcPeripheral::Emmc(ref mut emmc) => {
+            SdmmcPeripheral::Emmc(emmc) => {
                 emmc.cid = cid.into();
 
                 emmc.rca = 1u16.into();
@@ -1448,10 +1724,10 @@ impl<'d> Sdmmc<'d> {
         self.select_card(Some(card.get_address()))?;
 
         let bus_width = match card {
-            SdmmcPeripheral::SdCard(ref mut card) => {
+            SdmmcPeripheral::SdCard(card) => {
                 card.csd = csd.into();
 
-                self.get_scr(card).await?;
+                card.get_scr(self, cmd_block).await?;
 
                 if !card.scr.bus_width_four() {
                     BusWidth::One
@@ -1459,7 +1735,7 @@ impl<'d> Sdmmc<'d> {
                     BusWidth::Four
                 }
             }
-            SdmmcPeripheral::Emmc(ref mut emmc) => {
+            SdmmcPeripheral::Emmc(emmc) => {
                 emmc.csd = csd.into();
 
                 bus_width
@@ -1475,7 +1751,7 @@ impl<'d> Sdmmc<'d> {
         };
 
         match card {
-            SdmmcPeripheral::SdCard(ref mut card) => {
+            SdmmcPeripheral::SdCard(card) => {
                 let acmd_arg = match bus_width {
                     BusWidth::Four if card.scr.bus_width_four() => 2,
                     _ => 0,
@@ -1483,7 +1759,7 @@ impl<'d> Sdmmc<'d> {
                 self.cmd(common_cmd::app_cmd(card.rca), false)?;
                 self.cmd(sd_cmd::cmd6(acmd_arg), false)?;
             }
-            SdmmcPeripheral::Emmc(_) => {
+            SdmmcPeripheral::Emmc(emmc) => {
                 // Write bus width to ExtCSD byte 183
                 self.cmd(
                     emmc_cmd::modify_ext_csd(emmc_cmd::AccessMode::WriteByte, 183, widbus),
@@ -1492,7 +1768,7 @@ impl<'d> Sdmmc<'d> {
 
                 // Wait for ready after R1b response
                 loop {
-                    let status = self.read_status::<EMMC>(&card)?;
+                    let status = self.read_status(emmc)?;
 
                     if status.ready_for_data() {
                         break;
@@ -1515,32 +1791,30 @@ impl<'d> Sdmmc<'d> {
             self.clkcr_set_clkdiv(25_000_000, bus_width)?;
         }
 
-        self.card = Some(card);
-
         match card {
-            SdmmcPeripheral::SdCard(_) => {
+            SdmmcPeripheral::SdCard(card) => {
                 // Read status
-                self.read_sd_status().await?;
+                card.read_sd_status(self, cmd_block).await?;
 
                 if freq.0 > 25_000_000 {
                     // Switch to SDR25
-                    self.signalling = self.switch_signalling_mode(Signalling::SDR25).await?;
+                    self.signalling = card.switch_signalling_mode(self, cmd_block, Signalling::SDR25).await?;
 
                     if self.signalling == Signalling::SDR25 {
                         // Set final clock frequency
                         self.clkcr_set_clkdiv(freq.0, bus_width)?;
 
-                        if self.read_status::<SD>(self.card.as_ref().unwrap())?.state() != CurrentState::Transfer {
+                        if self.read_status(card)?.state() != CurrentState::Transfer {
                             return Err(Error::SignalingSwitchFailed);
                         }
                     }
                 }
 
                 // Read status after signalling change
-                self.read_sd_status().await?;
+                card.read_sd_status(self, cmd_block).await?;
             }
-            SdmmcPeripheral::Emmc(_) => {
-                self.read_ext_csd().await?;
+            SdmmcPeripheral::Emmc(emmc) => {
+                emmc.read_ext_csd(self).await?;
             }
         }
 
@@ -1550,223 +1824,35 @@ impl<'d> Sdmmc<'d> {
     /// Initializes card (if present) and sets the bus at the specified frequency.
     ///
     /// SD only.
-    pub async fn init_sd_card(&mut self, freq: Hertz) -> Result<(), Error> {
+    pub async fn init_sd_card(&mut self, cmd_block: &mut CmdBlock, freq: Hertz) -> Result<Card, Error> {
         let _scoped_block_stop = self.info.rcc.block_stop();
 
-        self.init_internal(freq, SdmmcPeripheral::SdCard(Card::default())).await
-    }
-
-    /// Switch mode using CMD6.
-    ///
-    /// Attempt to set a new signalling mode. The selected
-    /// signalling mode is returned. Expects the current clock
-    /// frequency to be > 12.5MHz.
-    ///
-    /// SD only.
-    async fn switch_signalling_mode(&mut self, signalling: Signalling) -> Result<Signalling, Error> {
-        let _ = self.card.as_mut().ok_or(Error::NoCard)?.get_sd_card();
-        // NB PLSS v7_10 4.3.10.4: "the use of SET_BLK_LEN command is not
-        // necessary"
-
-        let set_function = 0x8000_0000
-            | match signalling {
-                // See PLSS v7_10 Table 4-11
-                Signalling::DDR50 => 0xFF_FF04,
-                Signalling::SDR104 => 0xFF_1F03,
-                Signalling::SDR50 => 0xFF_1F02,
-                Signalling::SDR25 => 0xFF_FF01,
-                Signalling::SDR12 => 0xFF_FF00,
-            };
-
-        let status = match self.cmd_block.as_deref_mut() {
-            Some(x) => x,
-            None => &mut CmdBlock::new(),
-        };
-
-        // Arm `OnDrop` after the buffer, so it will be dropped first
-        let on_drop = OnDrop::new(|| self.on_drop());
-
-        let transfer = self.prepare_datapath_read(
-            &self.config,
-            #[cfg(sdmmc_v1)]
-            &mut self.dma,
-            status.as_mut(),
-            64,
-            6,
-        );
-        self.enable_interrupts();
-        self.cmd(sd_cmd::cmd6(set_function), true)?; // CMD6
-
-        let res = self.complete_datapath_transfer(true).await;
-
-        // Host is allowed to use the new functions at least 8
-        // clocks after the end of the switch command
-        // transaction. We know the current clock period is < 80ns,
-        // so a total delay of 640ns is required here
-        for _ in 0..300 {
-            cortex_m::asm::nop();
-        }
-
-        match res {
-            Ok(_) => {
-                on_drop.defuse();
-                self.stop_datapath();
-                drop(transfer);
-
-                // Function Selection of Function Group 1
-                let selection = (u32::from_be(status[4]) >> 24) & 0xF;
-
-                match selection {
-                    0 => Ok(Signalling::SDR12),
-                    1 => Ok(Signalling::SDR25),
-                    2 => Ok(Signalling::SDR50),
-                    3 => Ok(Signalling::SDR104),
-                    4 => Ok(Signalling::DDR50),
-                    _ => Err(Error::UnsupportedCardType),
-                }
-            }
-            Err(e) => Err(e),
-        }
-    }
-
-    /// Reads the SCR register.
-    ///
-    /// SD only.
-    async fn get_scr(&mut self, card: &mut Card) -> Result<(), Error> {
-        // Read the 64-bit SCR register
-        self.cmd(common_cmd::set_block_length(8), false)?; // CMD16
-        self.cmd(common_cmd::app_cmd(card.rca), false)?;
-
-        let cmd_block = match self.cmd_block.as_deref_mut() {
-            Some(x) => x,
-            None => &mut CmdBlock::new(),
-        };
-        let scr = &mut cmd_block.0[..2];
-
-        // Arm `OnDrop` after the buffer, so it will be dropped first
-        let on_drop = OnDrop::new(|| self.on_drop());
-
-        let transfer = self.prepare_datapath_read(
-            &self.config,
-            #[cfg(sdmmc_v1)]
-            &mut self.dma,
-            scr,
-            8,
-            3,
-        );
-        self.enable_interrupts();
-        self.cmd(sd_cmd::send_scr(), true)?;
+        let mut card = SdmmcPeripheral::SdCard(Card::default());
+        self.init_internal(cmd_block, freq, &mut card).await?;
 
-        let res = self.complete_datapath_transfer(true).await;
-
-        if res.is_ok() {
-            on_drop.defuse();
-            self.stop_datapath();
-            drop(transfer);
-
-            unsafe {
-                let scr_bytes = &*(&scr as *const _ as *const [u8; 8]);
-                card.scr = SCR(u64::from_be_bytes(*scr_bytes));
-            }
-        }
-        res
-    }
-
-    /// Reads the SD Status (ACMD13)
-    ///
-    /// SD only.
-    async fn read_sd_status(&mut self) -> Result<(), Error> {
-        let card = self.card.as_mut().ok_or(Error::NoCard)?.get_sd_card();
-        let rca = card.rca;
-
-        let cmd_block = match self.cmd_block.as_deref_mut() {
-            Some(x) => x,
-            None => &mut CmdBlock::new(),
+        let card = match card {
+            SdmmcPeripheral::SdCard(card) => card,
+            _ => unreachable!(),
         };
 
-        self.cmd(common_cmd::set_block_length(64), false)?; // CMD16
-        self.cmd(common_cmd::app_cmd(rca), false)?; // APP
-
-        let status = cmd_block;
-
-        // Arm `OnDrop` after the buffer, so it will be dropped first
-        let on_drop = OnDrop::new(|| self.on_drop());
-
-        let transfer = self.prepare_datapath_read(
-            &self.config,
-            #[cfg(sdmmc_v1)]
-            &mut self.dma,
-            status.as_mut(),
-            64,
-            6,
-        );
-        self.enable_interrupts();
-        self.cmd(sd_cmd::sd_status(), true)?;
-
-        let res = self.complete_datapath_transfer(true).await;
-
-        if res.is_ok() {
-            on_drop.defuse();
-            self.stop_datapath();
-            drop(transfer);
-
-            for byte in status.iter_mut() {
-                *byte = u32::from_be(*byte);
-            }
-            card.status = status.0.into();
-        }
-        res
+        Ok(card)
     }
 
     /// Initializes eMMC and sets the bus at the specified frequency.
     ///
     /// eMMC only.
-    pub async fn init_emmc(&mut self, freq: Hertz) -> Result<(), Error> {
+    pub async fn init_emmc(&mut self, cmd_block: &mut CmdBlock, freq: Hertz) -> Result<Emmc, Error> {
         let _scoped_block_stop = self.info.rcc.block_stop();
 
-        self.init_internal(freq, SdmmcPeripheral::Emmc(Emmc::default())).await
-    }
-
-    /// Gets the EXT_CSD register.
-    ///
-    /// eMMC only.
-    async fn read_ext_csd(&mut self) -> Result<(), Error> {
-        let mut card = self.card.take().ok_or(Error::NoCard)?;
-        let emmc = card.get_emmc();
-
-        // Note: cmd_block can't be used because ExtCSD is too long to fit.
-        let mut data_block = DataBlock([0u8; 512]);
-
-        // NOTE(unsafe) DataBlock uses align 4
-        let buffer = unsafe { &mut *((&mut data_block.0) as *mut [u8; 512] as *mut [u32; 128]) };
-
-        self.cmd(common_cmd::set_block_length(512), false).unwrap(); // CMD16
-
-        let transfer = self.prepare_datapath_read(
-            &self.config,
-            #[cfg(sdmmc_v1)]
-            &mut self.dma,
-            buffer,
-            512,
-            9,
-        );
-        self.enable_interrupts();
-        self.cmd(emmc_cmd::send_ext_csd(), true)?;
-
-        // Arm `OnDrop` after the buffer, so it will be dropped first
-        let on_drop = OnDrop::new(|| self.on_drop());
-
-        let res = self.complete_datapath_transfer(true).await;
-
-        if res.is_ok() {
-            on_drop.defuse();
-            self.stop_datapath();
-            drop(transfer);
+        let mut card = SdmmcPeripheral::Emmc(Emmc::default());
+        self.init_internal(cmd_block, freq, &mut card).await?;
 
-            emmc.ext_csd = unsafe { core::mem::transmute::<_, [u32; 128]>(data_block.0) }.into();
-        }
+        let card = match card {
+            SdmmcPeripheral::Emmc(card) => card,
+            _ => unreachable!(),
+        };
 
-        res
+        Ok(card)
     }
 }
 
@@ -1874,47 +1960,47 @@ foreach_peripheral!(
     };
 );
 
-impl<'d> block_device_driver::BlockDevice<512> for Sdmmc<'d> {
-    type Error = Error;
-    type Align = aligned::A4;
-
-    async fn read(
-        &mut self,
-        block_address: u32,
-        buf: &mut [aligned::Aligned<Self::Align, [u8; 512]>],
-    ) -> Result<(), Self::Error> {
-        let _scoped_block_stop = self.info.rcc.block_stop();
-        // TODO: I think block_address needs to be adjusted by the partition start offset
-        if buf.len() == 1 {
-            let block = unsafe { &mut *(&mut buf[0] as *mut _ as *mut crate::sdmmc::DataBlock) };
-            self.read_block(block_address, block).await?;
-        } else {
-            let blocks: &mut [DataBlock] =
-                unsafe { core::slice::from_raw_parts_mut(buf.as_mut_ptr() as *mut DataBlock, buf.len()) };
-            self.read_blocks(block_address, blocks).await?;
-        }
-        Ok(())
-    }
-
-    async fn write(
-        &mut self,
-        block_address: u32,
-        buf: &[aligned::Aligned<Self::Align, [u8; 512]>],
-    ) -> Result<(), Self::Error> {
-        let _scoped_block_stop = self.info.rcc.block_stop();
-        // TODO: I think block_address needs to be adjusted by the partition start offset
-        if buf.len() == 1 {
-            let block = unsafe { &*(&buf[0] as *const _ as *const crate::sdmmc::DataBlock) };
-            self.write_block(block_address, block).await?;
-        } else {
-            let blocks: &[DataBlock] =
-                unsafe { core::slice::from_raw_parts(buf.as_ptr() as *const DataBlock, buf.len()) };
-            self.write_blocks(block_address, blocks).await?;
-        }
-        Ok(())
-    }
-
-    async fn size(&mut self) -> Result<u64, Self::Error> {
-        Ok(self.card()?.size())
-    }
-}
+// impl<'d, A: Addressable> block_device_driver::BlockDevice<512> for Sdmmc<'d>, A {
+//     type Error = Error;
+//     type Align = aligned::A4;
+//
+//     async fn read(
+//         &mut self,
+//         block_address: u32,
+//         buf: &mut [aligned::Aligned<Self::Align, [u8; 512]>],
+//     ) -> Result<(), Self::Error> {
+//         let _scoped_block_stop = self.info.rcc.block_stop();
+//         // TODO: I think block_address needs to be adjusted by the partition start offset
+//         if buf.len() == 1 {
+//             let block = unsafe { &mut *(&mut buf[0] as *mut _ as *mut crate::sdmmc::DataBlock) };
+//             self.read_block(block_address, block).await?;
+//         } else {
+//             let blocks: &mut [DataBlock] =
+//                 unsafe { core::slice::from_raw_parts_mut(buf.as_mut_ptr() as *mut DataBlock, buf.len()) };
+//             self.read_blocks(block_address, blocks).await?;
+//         }
+//         Ok(())
+//     }
+//
+//     async fn write(
+//         &mut self,
+//         block_address: u32,
+//         buf: &[aligned::Aligned<Self::Align, [u8; 512]>],
+//     ) -> Result<(), Self::Error> {
+//         let _scoped_block_stop = self.info.rcc.block_stop();
+//         // TODO: I think block_address needs to be adjusted by the partition start offset
+//         if buf.len() == 1 {
+//             let block = unsafe { &*(&buf[0] as *const _ as *const crate::sdmmc::DataBlock) };
+//             self.write_block(block_address, block).await?;
+//         } else {
+//             let blocks: &[DataBlock] =
+//                 unsafe { core::slice::from_raw_parts(buf.as_ptr() as *const DataBlock, buf.len()) };
+//             self.write_blocks(block_address, blocks).await?;
+//         }
+//         Ok(())
+//     }
+//
+//     async fn size(&mut self) -> Result<u64, Self::Error> {
+//         Ok(self.card()?.size())
+//     }
+// }
diff --git a/embassy-stm32/src/time.rs b/embassy-stm32/src/time.rs
index 532877f70..88a28ee3d 100644
--- a/embassy-stm32/src/time.rs
+++ b/embassy-stm32/src/time.rs
@@ -4,7 +4,7 @@ use core::fmt::Display;
 use core::ops::{Div, Mul};
 
 /// Hertz
-#[derive(Eq, PartialEq, Ord, PartialOrd, Clone, Copy, Debug)]
+#[derive(Eq, PartialEq, Ord, PartialOrd, Clone, Copy, Debug, Default)]
 pub struct Hertz(pub u32);
 
 impl Display for Hertz {