From 269b1181b7c59739968b2473246f9edf02f6ed2f Mon Sep 17 00:00:00 2001
From: Markus Reiter <me@reitermark.us>
Date: Thu, 28 Apr 2022 17:25:13 +0200
Subject: [PATCH 01/23] Add SDMMC support.

---
 Cargo.toml   |   1 +
 src/lib.rs   |   1 +
 src/sdmmc.rs | 331 +++++++++++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 333 insertions(+)
 create mode 100644 src/sdmmc.rs

diff --git a/Cargo.toml b/Cargo.toml
index 692c72bf..deefed6c 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -28,6 +28,7 @@ embedded-dma = "0.1"
 bxcan = ">=0.4, <0.7"
 fugit = "0.3.5"
 bitfield = "0.13.2"
+sdio-host = "0.7.0"
 
 [dependencies.rand_core]
 version = "0.6.2"
diff --git a/src/lib.rs b/src/lib.rs
index e0788a66..29117f4d 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -158,6 +158,7 @@ pub mod qspi;
 pub mod rcc;
 pub mod rng;
 pub mod rtc;
+pub mod sdmmc;
 pub mod serial;
 pub mod signature;
 pub mod spi;
diff --git a/src/sdmmc.rs b/src/sdmmc.rs
new file mode 100644
index 00000000..c75d958f
--- /dev/null
+++ b/src/sdmmc.rs
@@ -0,0 +1,331 @@
+use fugit::HertzU32 as Hertz;
+use sdio_host::{
+    common_cmd::{self, ResponseLen},
+    Cmd,
+};
+
+use crate::{
+    gpio::{self, Alternate, PushPull},
+    pac::{sdmmc1, SDMMC1},
+    rcc::{Clocks, Enable, Reset, APB2},
+};
+
+pub trait PinClk {}
+pub trait PinCmd {}
+pub trait PinD0 {}
+pub trait PinD1 {}
+pub trait PinD2 {}
+pub trait PinD3 {}
+pub trait PinD4 {}
+pub trait PinD5 {}
+pub trait PinD6 {}
+pub trait PinD7 {}
+
+pub trait Pins {
+    const BUSWIDTH: BusWidth;
+}
+
+impl<CLK, CMD, D0> Pins for (CLK, CMD, D0)
+where
+    CLK: PinClk,
+    CMD: PinCmd,
+    D0: PinD0,
+{
+    const BUSWIDTH: BusWidth = BusWidth::BusWidth1;
+}
+
+impl<CLK, CMD, D0, D1, D2, D3> Pins for (CLK, CMD, D0, D1, D2, D3)
+where
+    CLK: PinClk,
+    CMD: PinCmd,
+    D0: PinD0,
+    D1: PinD1,
+    D2: PinD2,
+    D3: PinD3,
+{
+    const BUSWIDTH: BusWidth = BusWidth::BusWidth4;
+}
+
+impl<CLK, CMD, D0, D1, D2, D3, D4, D5, D6, D7> Pins for (CLK, CMD, D0, D1, D2, D3, D4, D5, D6, D7)
+where
+    CLK: PinClk,
+    CMD: PinCmd,
+    D0: PinD0,
+    D1: PinD1,
+    D2: PinD2,
+    D3: PinD3,
+    D4: PinD4,
+    D5: PinD5,
+    D6: PinD6,
+    D7: PinD7,
+{
+    const BUSWIDTH: BusWidth = BusWidth::BusWidth8;
+}
+
+macro_rules! pins {
+    ($(CLK: [$($CLK:ty),*] CMD: [$($CMD:ty),*] D0: [$($D0:ty),*] D1: [$($D1:ty),*] D2: [$($D2:ty),*] D3: [$($D3:ty),*] D4: [$($D4:ty),*] D5: [$($D5:ty),*] D6: [$($D6:ty),*] D7: [$($D7:ty),*])+) => {
+        $(
+            $(
+                impl PinClk for $CLK {}
+            )*
+            $(
+                impl PinCmd for $CMD {}
+            )*
+            $(
+                impl PinD0 for $D0 {}
+            )*
+            $(
+                impl PinD1 for $D1 {}
+            )*
+            $(
+                impl PinD2 for $D2 {}
+            )*
+            $(
+                impl PinD3 for $D3 {}
+            )*
+            $(
+                impl PinD4 for $D4 {}
+            )*
+            $(
+                impl PinD5 for $D5 {}
+            )*
+            $(
+                impl PinD6 for $D6 {}
+            )*
+            $(
+                impl PinD7 for $D7 {}
+            )*
+        )+
+    }
+}
+
+#[cfg(any(feature = "stm32l496",))]
+pins! {
+  CLK: [gpio::PC12<Alternate<PushPull, 12>>]
+  CMD: [gpio::PD2<Alternate<PushPull, 12>>]
+  D0: [gpio::PC8<Alternate<PushPull, 12>>]
+  D1: [gpio::PC9<Alternate<PushPull, 12>>]
+  D2: [gpio::PC10<Alternate<PushPull, 12>>]
+  D3: [gpio::PC11<Alternate<PushPull, 12>>]
+  D4: [gpio::PB8<Alternate<PushPull, 12>>]
+  D5: [gpio::PB9<Alternate<PushPull, 12>>]
+  D6: [gpio::PC6<Alternate<PushPull, 12>>]
+  D7: [gpio::PC7<Alternate<PushPull, 12>>]
+}
+
+#[derive(Debug, Copy, Clone, Eq, PartialEq)]
+#[repr(u8)]
+pub enum BusWidth {
+    BusWidth1 = 0,
+    BusWidth4 = 1,
+    BusWidth8 = 2,
+}
+
+#[derive(Debug, Copy, Clone, Eq, PartialEq)]
+#[repr(u8)]
+pub enum ClockFreq {
+    Freq24MHz = 0,    // 48 MHz / (0 + 2) < 25 MHz
+    Freq400KHz = 118, // 48 MHz / (118 + 2) < 400 kHz
+}
+
+#[derive(Debug)]
+pub enum Error {
+    NoCard,
+    SoftwareTimeout,
+    Crc,
+    DataCrcFail,
+    RxOverFlow,
+    Timeout,
+    TxUnderErr,
+}
+
+fn status_to_error(sta: sdmmc1::sta::R) -> Result<(), Error> {
+    if sta.ctimeout().bit_is_set() {
+        return Err(Error::Timeout);
+    } else if sta.ccrcfail().bit() {
+        return Err(Error::Crc);
+    } else if sta.dcrcfail().bit() {
+        return Err(Error::DataCrcFail);
+    } else if sta.rxoverr().bit() {
+        return Err(Error::RxOverFlow);
+    } else if sta.dtimeout().bit() {
+        return Err(Error::Timeout);
+    } else if sta.txunderr().bit() {
+        return Err(Error::TxUnderErr);
+    }
+    Ok(())
+}
+
+fn clear_all_interrupts(icr: &sdmmc1::ICR) {
+    icr.modify(|_, w| {
+        w.ccrcfailc()
+            .set_bit()
+            .ctimeoutc()
+            .set_bit()
+            .ceataendc()
+            .set_bit()
+            .cmdrendc()
+            .set_bit()
+            .cmdsentc()
+            .set_bit()
+            .dataendc()
+            .set_bit()
+            .dbckendc()
+            .set_bit()
+            .dcrcfailc()
+            .set_bit()
+            .dtimeoutc()
+            .set_bit()
+            .sdioitc()
+            .set_bit()
+            .stbiterrc()
+            .set_bit()
+            .rxoverrc()
+            .set_bit()
+            .txunderrc()
+            .set_bit()
+    });
+}
+
+pub struct SdMmc {
+    sdmmc: SDMMC1,
+    clock: Hertz,
+}
+
+impl SdMmc {
+    pub fn new<PINS: Pins>(
+        mut sdmmc: SDMMC1,
+        _pins: PINS,
+        apb2: &mut APB2,
+        clocks: &Clocks,
+    ) -> Self {
+        SDMMC1::enable(apb2);
+        SDMMC1::reset(apb2);
+
+        let clock = clocks.sysclk();
+
+        sdmmc.clkcr.modify(|_, w| unsafe {
+            w.negedge()
+                .clear_bit() // Rising Edge
+                .bypass()
+                .clear_bit() // Disable bypass.
+                .pwrsav()
+                .clear_bit() // Disable power-save.
+                .widbus()
+                .bits(0) // Bus Width 1
+                .hwfc_en()
+                .clear_bit() // Disable hardware flow-control.
+                .clkdiv()
+                .bits(0 as u8) // Clock must be <= 400 kHz while in identification mode.
+                .clken()
+                .clear_bit() // Disable clock.
+        });
+
+        let mut host = Self { sdmmc, clock };
+
+        host.power_card(false);
+
+        host
+    }
+
+    pub fn init(&mut self) -> Result<(), Error> {
+        self.power_card(true);
+
+        // Enable clock.
+        self.sdmmc.clkcr.modify(|_, w| w.clken().set_bit());
+
+        self.cmd(common_cmd::idle())
+    }
+
+    pub fn power_card(&mut self, on: bool) {
+        self.sdmmc
+            .power
+            .modify(|_, w| unsafe { w.pwrctrl().bits(if on { 0b11 } else { 0b00 }) });
+
+        // Wait for 2 ms after power mode change.
+        cortex_m::asm::delay(2 * (self.clock.raw() / 1000));
+    }
+
+    pub fn cmd<R: common_cmd::Resp>(&self, cmd: Cmd<R>) -> Result<(), Error> {
+        while self.sdmmc.sta.read().cmdact().bit_is_set() {}
+
+        // Clear the interrupts before we start
+        clear_all_interrupts(&self.sdmmc.icr);
+
+        self.sdmmc
+            .arg
+            .write(|w| unsafe { w.cmdarg().bits(cmd.arg) });
+
+        let waitresp = match cmd.response_len() {
+            ResponseLen::Zero => 0b00,
+            ResponseLen::R48 => 0b01,
+            ResponseLen::R136 => 0b11,
+        };
+
+        self.sdmmc.cmd.write(|w| unsafe {
+            w.cmdindex()
+                .bits(cmd.cmd)
+                .waitresp()
+                .bits(waitresp)
+                .waitint()
+                .clear_bit()
+                .cpsmen()
+                .set_bit()
+        });
+
+        let mut timeout = 5000 * (self.clock.raw() / 8 / 1000);
+
+        let sta = if cmd.response_len() == ResponseLen::Zero {
+
+            loop {
+                let sta = self.sdmmc.sta.read();
+
+                if sta.cmdact().bit_is_clear()
+                    && (sta.ctimeout().bit_is_set() || sta.cmdsent().bit_is_set())
+                {
+                    break sta;
+                }
+                if timeout == 0 {
+                    return Err(Error::SoftwareTimeout);
+                }
+
+                timeout -= 1;
+            }
+        } else {
+
+            let sta = loop {
+                timeout -= 1;
+
+                if timeout == 0 {
+                    return Err(Error::SoftwareTimeout);
+                }
+
+                let sta = self.sdmmc.sta.read();
+
+                if sta.ccrcfail().bit() || sta.cmdrend().bit() || sta.ctimeout().bit() {
+                    break sta;
+                }
+            };
+
+            if sta.ctimeout().bit() {
+                self.sdmmc.icr.modify(|_, w| w.ctimeoutc().set_bit());
+
+                return Err(Error::Timeout);
+            }
+
+            if sta.ccrcfail().bit() {
+                self.sdmmc.icr.modify(|_, w| w.ccrcfailc().set_bit());
+
+                return Err(Error::Crc);
+            }
+
+            if self.sdmmc.respcmd.read().respcmd().bits() != cmd.cmd {
+                return Err(Error::Crc);
+            }
+
+            sta
+        };
+
+        status_to_error(sta)
+    }
+}

From 72bcc2fddc145ffd87156f2899fc29878de1c9a3 Mon Sep 17 00:00:00 2001
From: Markus Reiter <me@reitermark.us>
Date: Sat, 30 Apr 2022 00:08:38 +0200
Subject: [PATCH 02/23] Improve SDMMC support.

---
 src/sdmmc.rs | 423 ++++++++++++++++++++++++++++++++++++++++++---------
 1 file changed, 351 insertions(+), 72 deletions(-)

diff --git a/src/sdmmc.rs b/src/sdmmc.rs
index c75d958f..18944c79 100644
--- a/src/sdmmc.rs
+++ b/src/sdmmc.rs
@@ -1,7 +1,10 @@
+use core::{fmt, hint::black_box};
+
 use fugit::HertzU32 as Hertz;
 use sdio_host::{
     common_cmd::{self, ResponseLen},
-    Cmd,
+    sd::{CardCapacity, CardStatus, CurrentState, SDStatus, CIC, CID, CSD, OCR, RCA, SCR, SD},
+    sd_cmd, Cmd,
 };
 
 use crate::{
@@ -22,7 +25,7 @@ pub trait PinD6 {}
 pub trait PinD7 {}
 
 pub trait Pins {
-    const BUSWIDTH: BusWidth;
+    const BUS_WIDTH: BusWidth;
 }
 
 impl<CLK, CMD, D0> Pins for (CLK, CMD, D0)
@@ -31,7 +34,7 @@ where
     CMD: PinCmd,
     D0: PinD0,
 {
-    const BUSWIDTH: BusWidth = BusWidth::BusWidth1;
+    const BUS_WIDTH: BusWidth = BusWidth::One;
 }
 
 impl<CLK, CMD, D0, D1, D2, D3> Pins for (CLK, CMD, D0, D1, D2, D3)
@@ -43,7 +46,7 @@ where
     D2: PinD2,
     D3: PinD3,
 {
-    const BUSWIDTH: BusWidth = BusWidth::BusWidth4;
+    const BUS_WIDTH: BusWidth = BusWidth::Four;
 }
 
 impl<CLK, CMD, D0, D1, D2, D3, D4, D5, D6, D7> Pins for (CLK, CMD, D0, D1, D2, D3, D4, D5, D6, D7)
@@ -59,7 +62,7 @@ where
     D6: PinD6,
     D7: PinD7,
 {
-    const BUSWIDTH: BusWidth = BusWidth::BusWidth8;
+    const BUS_WIDTH: BusWidth = BusWidth::Eight;
 }
 
 macro_rules! pins {
@@ -116,16 +119,21 @@ pins! {
 #[derive(Debug, Copy, Clone, Eq, PartialEq)]
 #[repr(u8)]
 pub enum BusWidth {
-    BusWidth1 = 0,
-    BusWidth4 = 1,
-    BusWidth8 = 2,
+    One = 0,
+    Four = 1,
+    Eight = 2,
 }
 
 #[derive(Debug, Copy, Clone, Eq, PartialEq)]
 #[repr(u8)]
 pub enum ClockFreq {
-    Freq24MHz = 0,    // 48 MHz / (0 + 2) < 25 MHz
-    Freq400KHz = 118, // 48 MHz / (118 + 2) < 400 kHz
+    Freq24MHz = 0,
+    Freq16MHz = 1,
+    Freq12MHz = 2,
+    Freq8MHz = 8,
+    Freq4MHz = 10,
+    Freq1MHz = 46,
+    Freq400KHz = 118,
 }
 
 #[derive(Debug)]
@@ -137,23 +145,23 @@ pub enum Error {
     RxOverFlow,
     Timeout,
     TxUnderErr,
+    RespCmdMismatch,
 }
 
-fn status_to_error(sta: sdmmc1::sta::R) -> Result<(), Error> {
-    if sta.ctimeout().bit_is_set() {
-        return Err(Error::Timeout);
-    } else if sta.ccrcfail().bit() {
-        return Err(Error::Crc);
-    } else if sta.dcrcfail().bit() {
-        return Err(Error::DataCrcFail);
-    } else if sta.rxoverr().bit() {
-        return Err(Error::RxOverFlow);
-    } else if sta.dtimeout().bit() {
-        return Err(Error::Timeout);
-    } else if sta.txunderr().bit() {
-        return Err(Error::TxUnderErr);
-    }
-    Ok(())
+macro_rules! try_datapath {
+    ($sta:ident) => {
+        if $sta.rxoverr().bit() {
+            return Err(Error::RxOverFlow);
+        }
+
+        if $sta.dcrcfail().bit() {
+            return Err(Error::DataCrcFail);
+        }
+
+        if $sta.dtimeout().bit() {
+            return Err(Error::Timeout);
+        }
+    };
 }
 
 fn clear_all_interrupts(icr: &sdmmc1::ICR) {
@@ -187,9 +195,65 @@ fn clear_all_interrupts(icr: &sdmmc1::ICR) {
     });
 }
 
+#[derive(Default)]
+pub struct SdCard {
+    capacity: CardCapacity,
+    ocr: OCR<SD>,
+    cid: CID<SD>,
+    rca: RCA<SD>,
+    csd: CSD<SD>,
+    scr: SCR,
+}
+
+/// TODO:  Remove when https://github.com/jkristell/sdio-host/issues/11 is fixed.
+impl fmt::Debug for SdCard {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        f.debug_struct("SdCard")
+            .field("capacity", &self.capacity)
+            .field("cid", &self.cid)
+            .field("csd", &self.csd)
+            .field("ocr", &self.ocr)
+            .field("rca", &self.rca.address())
+            .field("scr", &self.scr)
+            .finish()
+    }
+}
+
+#[derive(Clone, Copy, Debug)]
+/// Indicates transfer direction
+enum Dir {
+    HostToCard = 0,
+    CardToHost = 1,
+}
+
+impl SdCard {
+    pub fn get_address(&self) -> u16 {
+        self.rca.address()
+    }
+
+    pub fn supports_widebus(&self) -> bool {
+        self.scr.bus_width_four()
+    }
+}
+
+#[derive(Debug, Clone, Copy, PartialEq)]
+pub enum SdCardType {
+    Sdsc,
+    SdhcSdxc,
+}
+
+#[derive(Debug, Clone, Copy, PartialEq)]
+pub enum SdCardVersion {
+    V1,
+    V2,
+}
+
+#[derive(Debug)]
 pub struct SdMmc {
     sdmmc: SDMMC1,
     clock: Hertz,
+    bus_width: BusWidth,
+    card: Option<SdCard>,
 }
 
 impl SdMmc {
@@ -216,25 +280,197 @@ impl SdMmc {
                 .hwfc_en()
                 .clear_bit() // Disable hardware flow-control.
                 .clkdiv()
-                .bits(0 as u8) // Clock must be <= 400 kHz while in identification mode.
+                .bits(ClockFreq::Freq400KHz as u8) // Clock must be <= 400 kHz while in identification mode.
                 .clken()
                 .clear_bit() // Disable clock.
         });
 
-        let mut host = Self { sdmmc, clock };
+        let mut host = Self {
+            sdmmc,
+            clock,
+            bus_width: PINS::BUS_WIDTH,
+            card: None,
+        };
 
         host.power_card(false);
 
         host
     }
 
-    pub fn init(&mut self) -> Result<(), Error> {
+    pub fn init(&mut self, freq: ClockFreq) -> Result<(), Error> {
         self.power_card(true);
 
         // Enable clock.
         self.sdmmc.clkcr.modify(|_, w| w.clken().set_bit());
 
-        self.cmd(common_cmd::idle())
+        self.cmd(common_cmd::idle())?;
+
+        let check_pattern = 0xaa;
+        self.cmd(sd_cmd::send_if_cond(1, check_pattern))?;
+        let cic = CIC::from(self.sdmmc.resp1.read().bits());
+
+        let card_version = if cic.pattern() == 0xAA {
+            SdCardVersion::V2
+        } else {
+            SdCardVersion::V1
+        };
+
+        let mut card = SdCard::default();
+
+        let high_capacity = card_version == SdCardVersion::V2;
+        let voltage_window = 1 << 5;
+
+        let mut timeout = 0xffff;
+        let ocr: OCR<SD> = loop {
+            if timeout == 0 {
+                return Err(Error::SoftwareTimeout);
+            }
+
+            timeout -= 1;
+
+            match self.app_cmd(sd_cmd::sd_send_op_cond(
+                high_capacity,
+                false,
+                false,
+                voltage_window,
+            )) {
+                Ok(()) => (),
+                Err(Error::Crc) => (),
+                Err(err) => return Err(err),
+            }
+
+            let ocr = OCR::from(self.sdmmc.resp1.read().bits());
+            if !ocr.is_busy() {
+                // Power up done
+                break ocr;
+            }
+        };
+
+        card.capacity = if ocr.high_capacity() {
+            CardCapacity::HighCapacity
+        } else {
+            CardCapacity::StandardCapacity
+        };
+        card.ocr = ocr;
+
+        self.cmd(common_cmd::all_send_cid())?;
+        card.cid = CID::from([
+            self.sdmmc.resp1.read().bits(),
+            self.sdmmc.resp2.read().bits(),
+            self.sdmmc.resp3.read().bits(),
+            self.sdmmc.resp4.read().bits(),
+        ]);
+
+        self.cmd(sd_cmd::send_relative_address())?;
+        card.rca = RCA::from(self.sdmmc.resp1.read().bits());
+
+        self.cmd(common_cmd::send_csd(card.rca.address()))?;
+        card.csd = CSD::from([
+            self.sdmmc.resp1.read().bits(),
+            self.sdmmc.resp2.read().bits(),
+            self.sdmmc.resp3.read().bits(),
+            self.sdmmc.resp4.read().bits(),
+        ]);
+
+        self.select_card(card.rca.address())?;
+        card.scr = self.get_scr(card.rca.address())?;
+
+        //  `app_cmd` will select this card from now on.
+        self.card = Some(card);
+
+        // Wait before setting the bus witdth and frequency to avoid timeouts on SDSC cards
+        while !self.card_ready()? {}
+
+        self.set_bus(self.bus_width, freq)?;
+
+        Ok(())
+    }
+
+    fn set_bus(&self, bus_width: BusWidth, freq: ClockFreq) -> Result<(), Error> {
+        let card_widebus = self.card()?.supports_widebus();
+
+        let bus_width = match bus_width {
+            BusWidth::Eight | BusWidth::Four if card_widebus => BusWidth::Four,
+            _ => BusWidth::One,
+        };
+
+        self.app_cmd(sd_cmd::set_bus_width(bus_width == BusWidth::Four))?;
+
+        self.sdmmc.clkcr.modify(|_, w| unsafe {
+            w.clkdiv()
+                .bits(freq as u8)
+                .widbus()
+                .bits(bus_width as u8)
+                .clken()
+                .set_bit()
+        });
+        Ok(())
+    }
+
+    fn start_datapath_transfer(&self, length_bytes: u32, block_size: u8, direction: Dir) {
+        // Block size up to 2^14 bytes
+        assert!(block_size <= 14);
+
+        loop {
+            let sta = self.sdmmc.sta.read();
+
+            if sta.cmdact().bit_is_clear()
+                && sta.rxact().bit_is_clear()
+                && sta.txact().bit_is_clear()
+            {
+                break;
+            }
+        }
+
+        self.sdmmc
+            .dtimer
+            .modify(|_, w| unsafe { w.datatime().bits(0xFFFFFFFF) });
+
+        self.sdmmc
+            .dlen
+            .modify(|_, w| unsafe { w.datalength().bits(length_bytes) });
+
+        self.sdmmc.dctrl.modify(|_, w| unsafe {
+            w.dblocksize()
+                .bits(block_size)
+                .dtdir()
+                .bit(match direction {
+                    Dir::HostToCard => false,
+                    Dir::CardToHost => true,
+                })
+                .dtmode()
+                .clear_bit()
+                .dten()
+                .set_bit()
+        });
+    }
+
+    fn get_scr(&self, rca: u16) -> Result<SCR, Error> {
+        self.cmd(common_cmd::set_block_length(8))?;
+        self.cmd(common_cmd::app_cmd(rca))?;
+        self.start_datapath_transfer(8, 3, Dir::CardToHost);
+        self.cmd(sd_cmd::send_scr())?;
+
+        let mut scr = [0x00; 2];
+
+        'outer: for n in scr.iter_mut().rev() {
+            loop {
+                let sta = self.sdmmc.sta.read();
+
+                try_datapath!(sta);
+
+                if sta.dbckend().bit() {
+                    break 'outer;
+                }
+
+                if sta.rxdavl().bit_is_set() {
+                    *n = self.sdmmc.fifo.read().bits().swap_bytes();
+                    continue 'outer;
+                }
+            }
+        }
+
+        return Ok(SCR::from(scr));
     }
 
     pub fn power_card(&mut self, on: bool) {
@@ -246,6 +482,70 @@ impl SdMmc {
         cortex_m::asm::delay(2 * (self.clock.raw() / 1000));
     }
 
+    fn read_status(&mut self) -> Result<CardStatus<SdCard>, Error> {
+        let card = self.card()?;
+
+        self.cmd(common_cmd::card_status(card.get_address(), false))?;
+
+        let r1 = self.sdmmc.resp1.read().bits();
+        Ok(CardStatus::from(r1))
+    }
+
+    /// Check if card is done writing/reading and back in transfer state
+    fn card_ready(&mut self) -> Result<bool, Error> {
+        Ok(self.read_status()?.state() == CurrentState::Transfer)
+    }
+
+    /// Read the SDStatus struct
+    pub fn read_sd_status(&mut self) -> Result<SDStatus, Error> {
+        self.card()?;
+        self.cmd(common_cmd::set_block_length(64))?;
+        self.start_datapath_transfer(64, 6, Dir::CardToHost);
+        self.app_cmd(sd_cmd::sd_status())?;
+
+        let mut sd_status = [0u32; 16];
+
+        'outer: for i in 0..2 {
+            loop {
+                let sta = self.sdmmc.sta.read();
+
+                try_datapath!(sta);
+
+                if sta.dbckend().bit() {
+                    break 'outer;
+                }
+
+                if sta.rxfifohf().bit() {
+                    for j in 0..8 {
+                        sd_status[15 - i * 8 - j] = self.sdmmc.fifo.read().bits().swap_bytes();
+                    }
+
+                    continue 'outer;
+                }
+            }
+        }
+
+        Ok(SDStatus::from(sd_status))
+    }
+
+    pub fn card(&self) -> Result<&SdCard, Error> {
+        self.card.as_ref().ok_or(Error::NoCard)
+    }
+
+    fn select_card(&self, rca: u16) -> Result<(), Error> {
+        let r = self.cmd(common_cmd::select_card(rca));
+        match (r, rca) {
+            (Err(Error::Timeout), 0) => Ok(()),
+            (r, _) => r,
+        }
+    }
+
+    pub fn app_cmd<R: common_cmd::Resp>(&self, cmd: Cmd<R>) -> Result<(), Error> {
+        let rca = self.card().map(|card| card.get_address()).unwrap_or(0);
+        self.cmd(common_cmd::app_cmd(rca))?;
+        self.cmd(cmd)
+    }
+
     pub fn cmd<R: common_cmd::Resp>(&self, cmd: Cmd<R>) -> Result<(), Error> {
         while self.sdmmc.sta.read().cmdact().bit_is_set() {}
 
@@ -273,59 +573,38 @@ impl SdMmc {
                 .set_bit()
         });
 
-        let mut timeout = 5000 * (self.clock.raw() / 8 / 1000);
+        let timeout = 5000 * (self.clock.raw() / 8 / 1000);
+        for _ in 0..timeout {
+            let sta = self.sdmmc.sta.read();
 
-        let sta = if cmd.response_len() == ResponseLen::Zero {
+            if sta.cmdact().bit_is_set() {
+                // Command transfer still in progress.
+                continue;
+            }
 
-            loop {
-                let sta = self.sdmmc.sta.read();
+            if cmd.response_len() == ResponseLen::Zero {
+                if sta.ctimeout().bit() {
+                    return Err(Error::Timeout);
+                }
 
-                if sta.cmdact().bit_is_clear()
-                    && (sta.ctimeout().bit_is_set() || sta.cmdsent().bit_is_set())
-                {
-                    break sta;
+                if sta.cmdsent().bit() {
+                    return Ok(());
                 }
-                if timeout == 0 {
-                    return Err(Error::SoftwareTimeout);
+            } else {
+                if sta.ctimeout().bit() {
+                    return Err(Error::Timeout);
                 }
 
-                timeout -= 1;
-            }
-        } else {
-
-            let sta = loop {
-                timeout -= 1;
-
-                if timeout == 0 {
-                    return Err(Error::SoftwareTimeout);
+                if sta.ccrcfail().bit() {
+                    return Err(Error::Crc);
                 }
 
-                let sta = self.sdmmc.sta.read();
-
-                if sta.ccrcfail().bit() || sta.cmdrend().bit() || sta.ctimeout().bit() {
-                    break sta;
+                if sta.cmdrend().bit() {
+                    return Ok(());
                 }
-            };
-
-            if sta.ctimeout().bit() {
-                self.sdmmc.icr.modify(|_, w| w.ctimeoutc().set_bit());
-
-                return Err(Error::Timeout);
             }
+        }
 
-            if sta.ccrcfail().bit() {
-                self.sdmmc.icr.modify(|_, w| w.ccrcfailc().set_bit());
-
-                return Err(Error::Crc);
-            }
-
-            if self.sdmmc.respcmd.read().respcmd().bits() != cmd.cmd {
-                return Err(Error::Crc);
-            }
-
-            sta
-        };
-
-        status_to_error(sta)
+        Err(Error::SoftwareTimeout)
     }
 }

From 21fe2a927e5e9fe181ab2ad3aaf3911cde98c694 Mon Sep 17 00:00:00 2001
From: Markus Reiter <me@reitermark.us>
Date: Sat, 30 Apr 2022 17:10:45 +0200
Subject: [PATCH 03/23] Implement SDMMC reading.

---
 Cargo.toml   |   1 +
 src/sdmmc.rs | 243 +++++++++++++++++++++++++++++++++++++++++----------
 2 files changed, 198 insertions(+), 46 deletions(-)

diff --git a/Cargo.toml b/Cargo.toml
index deefed6c..437383a5 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -29,6 +29,7 @@ bxcan = ">=0.4, <0.7"
 fugit = "0.3.5"
 bitfield = "0.13.2"
 sdio-host = "0.7.0"
+embedded-sdmmc = "0.3.0"
 
 [dependencies.rand_core]
 version = "0.6.2"
diff --git a/src/sdmmc.rs b/src/sdmmc.rs
index 18944c79..e81d758a 100644
--- a/src/sdmmc.rs
+++ b/src/sdmmc.rs
@@ -1,4 +1,4 @@
-use core::{fmt, hint::black_box};
+use core::{fmt, ops::ControlFlow};
 
 use fugit::HertzU32 as Hertz;
 use sdio_host::{
@@ -128,9 +128,8 @@ pub enum BusWidth {
 #[repr(u8)]
 pub enum ClockFreq {
     Freq24MHz = 0,
-    Freq16MHz = 1,
     Freq12MHz = 2,
-    Freq8MHz = 8,
+    Freq8MHz = 4,
     Freq4MHz = 10,
     Freq1MHz = 46,
     Freq400KHz = 118,
@@ -145,7 +144,7 @@ pub enum Error {
     RxOverFlow,
     Timeout,
     TxUnderErr,
-    RespCmdMismatch,
+    WrongResponseSize,
 }
 
 macro_rules! try_datapath {
@@ -197,7 +196,6 @@ fn clear_all_interrupts(icr: &sdmmc1::ICR) {
 
 #[derive(Default)]
 pub struct SdCard {
-    capacity: CardCapacity,
     ocr: OCR<SD>,
     cid: CID<SD>,
     rca: RCA<SD>,
@@ -209,7 +207,6 @@ pub struct SdCard {
 impl fmt::Debug for SdCard {
     fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
         f.debug_struct("SdCard")
-            .field("capacity", &self.capacity)
             .field("cid", &self.cid)
             .field("csd", &self.csd)
             .field("ocr", &self.ocr)
@@ -219,7 +216,7 @@ impl fmt::Debug for SdCard {
     }
 }
 
-#[derive(Clone, Copy, Debug)]
+#[derive(Debug, Clone, Copy)]
 /// Indicates transfer direction
 enum Dir {
     HostToCard = 0,
@@ -227,7 +224,20 @@ enum Dir {
 }
 
 impl SdCard {
-    pub fn get_address(&self) -> u16 {
+    /// Size in bytes.
+    pub fn size(&self) -> u64 {
+        self.csd.card_size()
+    }
+
+    fn capacity(&self) -> CardCapacity {
+        if self.ocr.high_capacity() {
+            CardCapacity::HighCapacity
+        } else {
+            CardCapacity::StandardCapacity
+        }
+    }
+
+    pub fn address(&self) -> u16 {
         self.rca.address()
     }
 
@@ -249,20 +259,15 @@ pub enum SdCardVersion {
 }
 
 #[derive(Debug)]
-pub struct SdMmc {
+pub struct Sdmmc {
     sdmmc: SDMMC1,
     clock: Hertz,
     bus_width: BusWidth,
     card: Option<SdCard>,
 }
 
-impl SdMmc {
-    pub fn new<PINS: Pins>(
-        mut sdmmc: SDMMC1,
-        _pins: PINS,
-        apb2: &mut APB2,
-        clocks: &Clocks,
-    ) -> Self {
+impl Sdmmc {
+    pub fn new<PINS: Pins>(sdmmc: SDMMC1, _pins: PINS, apb2: &mut APB2, clocks: &Clocks) -> Self {
         SDMMC1::enable(apb2);
         SDMMC1::reset(apb2);
 
@@ -297,6 +302,7 @@ impl SdMmc {
         host
     }
 
+    #[inline]
     pub fn init(&mut self, freq: ClockFreq) -> Result<(), Error> {
         self.power_card(true);
 
@@ -321,7 +327,7 @@ impl SdMmc {
         let voltage_window = 1 << 5;
 
         let mut timeout = 0xffff;
-        let ocr: OCR<SD> = loop {
+        card.ocr = loop {
             if timeout == 0 {
                 return Err(Error::SoftwareTimeout);
             }
@@ -346,13 +352,6 @@ impl SdMmc {
             }
         };
 
-        card.capacity = if ocr.high_capacity() {
-            CardCapacity::HighCapacity
-        } else {
-            CardCapacity::StandardCapacity
-        };
-        card.ocr = ocr;
-
         self.cmd(common_cmd::all_send_cid())?;
         card.cid = CID::from([
             self.sdmmc.resp1.read().bits(),
@@ -386,6 +385,110 @@ impl SdMmc {
         Ok(())
     }
 
+    pub fn read_block(&mut self, addr: u32, buf: &mut [u8; 512]) -> Result<(), Error> {
+        let card = self.card()?;
+
+        let addr = match card.capacity() {
+            CardCapacity::StandardCapacity => addr * 512,
+            _ => addr,
+        };
+
+        self.cmd(common_cmd::set_block_length(512))?;
+
+        self.start_datapath_transfer(512, 9, Dir::CardToHost);
+        self.cmd(common_cmd::read_single_block(addr))?;
+
+        let mut i = 0;
+        loop {
+            match self.read_fifo_hf(|bits| {
+                buf[i..(i + 4)].copy_from_slice(&bits.to_be_bytes());
+                i += 4;
+            })? {
+                ControlFlow::Break(()) => {
+                    if i == buf.len() {
+                        return Ok(());
+                    } else {
+                        return Err(Error::WrongResponseSize);
+                    }
+                }
+                ControlFlow::Continue(()) => continue,
+            }
+        }
+    }
+
+    #[inline]
+    pub fn read_blocks(&mut self, addr: u32, buf: &mut [u8]) -> Result<(), Error> {
+        let card = self.card()?;
+
+        assert!(
+            buf.len() % 512 == 0,
+            "Buffer length must be a multiple of 512."
+        );
+
+        let addr = match card.capacity() {
+            CardCapacity::StandardCapacity => addr * 512,
+            _ => addr,
+        };
+
+        self.cmd(common_cmd::set_block_length(512))?;
+
+        self.start_datapath_transfer(buf.len() as u32, 9, Dir::CardToHost);
+        self.cmd(common_cmd::read_multiple_blocks(addr))?;
+
+        let mut i = 0;
+        loop {
+            match self.read_fifo_hf(|bits| {
+                buf[i..(i + 4)].copy_from_slice(&bits.to_be_bytes());
+                i += 4;
+            })? {
+                ControlFlow::Break(()) => {
+                    self.cmd(common_cmd::stop_transmission())?;
+
+                    if i == buf.len() {
+                        return Ok(());
+                    } else {
+                        return Err(Error::WrongResponseSize);
+                    }
+                }
+                ControlFlow::Continue(()) => continue,
+            }
+        }
+    }
+
+    pub fn write_block(&mut self, addr: u32, block: &[u8; 512]) -> Result<(), Error> {
+        self.card()?;
+
+        todo!()
+    }
+
+    /// Read eight 32-bit values from a half-full FIFO.
+    #[inline]
+    fn read_fifo_hf(&mut self, mut f: impl FnMut(u32) -> ()) -> Result<ControlFlow<(), ()>, Error> {
+        // TODO: Better timeout value.
+        let timeout: u32 = 0xffff_ffff;
+        for _ in 0..timeout {
+            let sta = self.sdmmc.sta.read();
+
+            try_datapath!(sta);
+
+            if sta.dbckend().bit() {
+                return Ok(ControlFlow::Break(()));
+            }
+
+            if sta.rxfifohf().bit() {
+                for _ in 0..8 {
+                    let bits = u32::from_be(self.sdmmc.fifo.read().bits());
+                    f(bits)
+                }
+
+                return Ok(ControlFlow::Continue(()));
+            }
+        }
+
+        Err(Error::SoftwareTimeout)
+    }
+
+    #[inline]
     fn set_bus(&self, bus_width: BusWidth, freq: ClockFreq) -> Result<(), Error> {
         let card_widebus = self.card()?.supports_widebus();
 
@@ -451,7 +554,7 @@ impl SdMmc {
         self.start_datapath_transfer(8, 3, Dir::CardToHost);
         self.cmd(sd_cmd::send_scr())?;
 
-        let mut scr = [0x00; 2];
+        let mut scr = [0; 2];
 
         'outer: for n in scr.iter_mut().rev() {
             loop {
@@ -459,12 +562,14 @@ impl SdMmc {
 
                 try_datapath!(sta);
 
-                if sta.dbckend().bit() {
+                if sta.dataend().bit() {
                     break 'outer;
                 }
 
                 if sta.rxdavl().bit_is_set() {
-                    *n = self.sdmmc.fifo.read().bits().swap_bytes();
+                    let bits = u32::from_be(self.sdmmc.fifo.read().bits());
+                    *n = bits.to_le();
+
                     continue 'outer;
                 }
             }
@@ -485,7 +590,7 @@ impl SdMmc {
     fn read_status(&mut self) -> Result<CardStatus<SdCard>, Error> {
         let card = self.card()?;
 
-        self.cmd(common_cmd::card_status(card.get_address(), false))?;
+        self.cmd(common_cmd::card_status(card.address(), false))?;
 
         let r1 = self.sdmmc.resp1.read().bits();
         Ok(CardStatus::from(r1))
@@ -505,27 +610,22 @@ impl SdMmc {
 
         let mut sd_status = [0u32; 16];
 
-        'outer: for i in 0..2 {
-            loop {
-                let sta = self.sdmmc.sta.read();
-
-                try_datapath!(sta);
-
-                if sta.dbckend().bit() {
-                    break 'outer;
-                }
-
-                if sta.rxfifohf().bit() {
-                    for j in 0..8 {
-                        sd_status[15 - i * 8 - j] = self.sdmmc.fifo.read().bits().swap_bytes();
+        let mut i = sd_status.len();
+        loop {
+            match self.read_fifo_hf(|bits| {
+                i -= 1;
+                sd_status[i] = bits.to_le();
+            })? {
+                ControlFlow::Break(()) => {
+                    return if i == 0 {
+                        Ok(SDStatus::from(sd_status))
+                    } else {
+                        Err(Error::WrongResponseSize)
                     }
-
-                    continue 'outer;
                 }
+                ControlFlow::Continue(()) => continue,
             }
         }
-
-        Ok(SDStatus::from(sd_status))
     }
 
     pub fn card(&self) -> Result<&SdCard, Error> {
@@ -541,7 +641,7 @@ impl SdMmc {
     }
 
     pub fn app_cmd<R: common_cmd::Resp>(&self, cmd: Cmd<R>) -> Result<(), Error> {
-        let rca = self.card().map(|card| card.get_address()).unwrap_or(0);
+        let rca = self.card().map(|card| card.address()).unwrap_or(0);
         self.cmd(common_cmd::app_cmd(rca))?;
         self.cmd(cmd)
     }
@@ -607,4 +707,55 @@ impl SdMmc {
 
         Err(Error::SoftwareTimeout)
     }
+
+    pub fn into_block_device(self) -> SdmmcBlockDevice<Sdmmc> {
+        SdmmcBlockDevice {
+            sdmmc: core::cell::RefCell::new(self),
+        }
+    }
+}
+
+pub struct SdmmcBlockDevice<SDMMC> {
+    sdmmc: core::cell::RefCell<SDMMC>,
+}
+
+impl embedded_sdmmc::BlockDevice for SdmmcBlockDevice<Sdmmc> {
+    type Error = Error;
+
+    fn read(
+        &self,
+        blocks: &mut [embedded_sdmmc::Block],
+        start_block_idx: embedded_sdmmc::BlockIdx,
+        _reason: &str,
+    ) -> Result<(), Self::Error> {
+        let start = start_block_idx.0;
+        let mut sdmmc = self.sdmmc.borrow_mut();
+        for block_idx in start..(start + blocks.len() as u32) {
+            sdmmc.read_block(
+                block_idx,
+                &mut blocks[(block_idx - start) as usize].contents,
+            )?;
+        }
+        Ok(())
+    }
+
+    fn write(
+        &self,
+        blocks: &[embedded_sdmmc::Block],
+        start_block_idx: embedded_sdmmc::BlockIdx,
+    ) -> Result<(), Self::Error> {
+        let start = start_block_idx.0;
+        let mut sdmmc = self.sdmmc.borrow_mut();
+        for block_idx in start..(start + blocks.len() as u32) {
+            sdmmc.write_block(block_idx, &blocks[(block_idx - start) as usize].contents)?;
+        }
+        Ok(())
+    }
+
+    fn num_blocks(&self) -> Result<embedded_sdmmc::BlockCount, Self::Error> {
+        let sdmmc = self.sdmmc.borrow_mut();
+        Ok(embedded_sdmmc::BlockCount(
+            (sdmmc.card()?.size() / 512u64) as u32,
+        ))
+    }
 }

From 152b55df5850f1f0916a7f8087a79f456be65687 Mon Sep 17 00:00:00 2001
From: Markus Reiter <me@reitermark.us>
Date: Sat, 30 Apr 2022 20:07:53 +0200
Subject: [PATCH 04/23] Implement SDMMC writing.

---
 .github/workflows/ci.yml |   8 +-
 Cargo.toml               |   6 +-
 src/sdmmc.rs             | 192 ++++++++++++++++++++++++++-------------
 3 files changed, 139 insertions(+), 67 deletions(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 84357e1e..3f5ae0db 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -25,10 +25,10 @@ jobs:
           - { id: stm32l462, additional-features: ",stm32-usbd" }
           - { id: stm32l471, additional-features: "" }
           - { id: stm32l475, additional-features: "" } # USB_OTG not supported by PAC
-          - { id: stm32l476, additional-features: ",otg_fs" }
-          - { id: stm32l486, additional-features: ",otg_fs" }
-          - { id: stm32l496, additional-features: ",otg_fs" }
-          - { id: stm32l4a6, additional-features: ",otg_fs" }
+          - { id: stm32l476, additional-features: ",otg_fs,sdmmc,embedded-sdmmc" }
+          - { id: stm32l486, additional-features: ",otg_fs,sdmmc,embedded-sdmmc" }
+          - { id: stm32l496, additional-features: ",otg_fs,sdmmc,embedded-sdmmc" }
+          - { id: stm32l4a6, additional-features: ",otg_fs,sdmmc,embedded-sdmmc" }
 
     steps:
       - uses: actions/checkout@v2
diff --git a/Cargo.toml b/Cargo.toml
index 437383a5..3cc961c5 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -28,8 +28,8 @@ embedded-dma = "0.1"
 bxcan = ">=0.4, <0.7"
 fugit = "0.3.5"
 bitfield = "0.13.2"
-sdio-host = "0.7.0"
-embedded-sdmmc = "0.3.0"
+sdio-host = { version = "0.7.0", optional = true }
+embedded-sdmmc = { version = "0.3.0", optional = true }
 
 [dependencies.rand_core]
 version = "0.6.2"
@@ -71,6 +71,8 @@ features = ["rt", "stm32l432", "stm32-usbd"]
 rt = ["stm32l4/rt"]
 unproven = ["embedded-hal/unproven"]
 otg_fs = ["synopsys-usb-otg"]
+sdmmc = ["dep:sdio-host"]
+embedded-sdmmc = ["dep:embedded-sdmmc", "sdmmc"]
 
 # L4x1
 stm32l431 = [ "stm32l4/stm32l4x1" ]
diff --git a/src/sdmmc.rs b/src/sdmmc.rs
index e81d758a..fd42b9c8 100644
--- a/src/sdmmc.rs
+++ b/src/sdmmc.rs
@@ -1,4 +1,9 @@
-use core::{fmt, ops::ControlFlow};
+#![cfg(feature = "sdmmc")]
+
+use core::{
+    fmt,
+    ops::{ControlFlow, Deref, DerefMut},
+};
 
 use fugit::HertzU32 as Hertz;
 use sdio_host::{
@@ -12,7 +17,6 @@ use crate::{
     pac::{sdmmc1, SDMMC1},
     rcc::{Clocks, Enable, Reset, APB2},
 };
-
 pub trait PinClk {}
 pub trait PinCmd {}
 pub trait PinD0 {}
@@ -147,12 +151,8 @@ pub enum Error {
     WrongResponseSize,
 }
 
-macro_rules! try_datapath {
-    ($sta:ident) => {
-        if $sta.rxoverr().bit() {
-            return Err(Error::RxOverFlow);
-        }
-
+macro_rules! datapath_err {
+    ($sta:expr) => {
         if $sta.dcrcfail().bit() {
             return Err(Error::DataCrcFail);
         }
@@ -163,6 +163,26 @@ macro_rules! try_datapath {
     };
 }
 
+macro_rules! datapath_rx_err {
+    ($sta:expr) => {
+        if $sta.rxoverr().bit() {
+            return Err(Error::RxOverFlow);
+        }
+
+        datapath_err!($sta)
+    };
+}
+
+macro_rules! datapath_tx_err {
+    ($sta:expr) => {
+        if $sta.rxoverr().bit() {
+            return Err(Error::RxOverFlow);
+        }
+
+        datapath_err!($sta)
+    };
+}
+
 fn clear_all_interrupts(icr: &sdmmc1::ICR) {
     icr.modify(|_, w| {
         w.ccrcfailc()
@@ -385,7 +405,21 @@ impl Sdmmc {
         Ok(())
     }
 
-    pub fn read_block(&mut self, addr: u32, buf: &mut [u8; 512]) -> Result<(), Error> {
+    #[inline]
+    pub fn read_block<B: DerefMut<Target = [u8; 512]>>(
+        &mut self,
+        addr: u32,
+        block: B,
+    ) -> Result<(), Error> {
+        self.read_blocks(addr, &mut [block])
+    }
+
+    #[inline]
+    pub fn read_blocks<B: DerefMut<Target = [u8; 512]>>(
+        &mut self,
+        addr: u32,
+        blocks: &mut [B],
+    ) -> Result<(), Error> {
         let card = self.card()?;
 
         let addr = match card.capacity() {
@@ -395,17 +429,28 @@ impl Sdmmc {
 
         self.cmd(common_cmd::set_block_length(512))?;
 
-        self.start_datapath_transfer(512, 9, Dir::CardToHost);
-        self.cmd(common_cmd::read_single_block(addr))?;
+        let bytes = blocks.len() * 512;
+        self.start_datapath_transfer(bytes as u32, 9, Dir::CardToHost);
+
+        match blocks.len() {
+            0 => return Ok(()),
+            1 => self.cmd(common_cmd::read_single_block(addr))?,
+            _ => self.cmd(common_cmd::read_multiple_blocks(addr))?,
+        }
 
         let mut i = 0;
         loop {
             match self.read_fifo_hf(|bits| {
-                buf[i..(i + 4)].copy_from_slice(&bits.to_be_bytes());
+                let start = i % 512;
+                blocks[i / 512][start..(start + 4)].copy_from_slice(&bits.to_be_bytes());
                 i += 4;
             })? {
                 ControlFlow::Break(()) => {
-                    if i == buf.len() {
+                    if blocks.len() > 1 {
+                        self.cmd(common_cmd::stop_transmission())?;
+                    }
+
+                    if i == bytes {
                         return Ok(());
                     } else {
                         return Err(Error::WrongResponseSize);
@@ -416,60 +461,89 @@ impl Sdmmc {
         }
     }
 
-    #[inline]
-    pub fn read_blocks(&mut self, addr: u32, buf: &mut [u8]) -> Result<(), Error> {
-        let card = self.card()?;
+    pub fn write_block<B: Deref<Target = [u8; 512]>>(
+        &mut self,
+        addr: u32,
+        block: B,
+    ) -> Result<(), Error> {
+        self.write_blocks(addr, &[block])
+    }
 
-        assert!(
-            buf.len() % 512 == 0,
-            "Buffer length must be a multiple of 512."
-        );
+    fn write_blocks<B: Deref<Target = [u8; 512]>>(
+        &mut self,
+        addr: u32,
+        blocks: &[B],
+    ) -> Result<(), Error> {
+        let card = self.card()?;
 
         let addr = match card.capacity() {
             CardCapacity::StandardCapacity => addr * 512,
             _ => addr,
         };
 
+        let bytes = blocks.len() * 512;
         self.cmd(common_cmd::set_block_length(512))?;
+        self.start_datapath_transfer(bytes as u32, 9, Dir::HostToCard);
 
-        self.start_datapath_transfer(buf.len() as u32, 9, Dir::CardToHost);
-        self.cmd(common_cmd::read_multiple_blocks(addr))?;
+        match blocks.len() {
+            0 => return Ok(()),
+            1 => self.cmd(common_cmd::write_single_block(addr))?,
+            _ => self.cmd(common_cmd::write_multiple_blocks(addr))?,
+        }
 
         let mut i = 0;
         loop {
-            match self.read_fifo_hf(|bits| {
-                buf[i..(i + 4)].copy_from_slice(&bits.to_be_bytes());
-                i += 4;
-            })? {
-                ControlFlow::Break(()) => {
-                    self.cmd(common_cmd::stop_transmission())?;
+            let sta = self.sdmmc.sta.read();
 
-                    if i == buf.len() {
-                        return Ok(());
-                    } else {
-                        return Err(Error::WrongResponseSize);
+            datapath_tx_err!(sta);
+
+            if i == bytes {
+                // If we sent all data, wait for transfer to end.
+                if sta.dbckend().bit() {
+                    if blocks.len() > 1 {
+                        self.cmd(common_cmd::stop_transmission())?;
+                    }
+
+                    break;
+                }
+            } else {
+                // If the FIFO is half-empty, send some data.
+                if sta.txfifohe().bit() {
+                    for _ in 0..8 {
+                        let block = &blocks[i / 512];
+                        let start = i % 512;
+
+                        let bits = u32::from_be_bytes([
+                            block[start],
+                            block[start + 1],
+                            block[start + 2],
+                            block[start + 3],
+                        ]);
+                        self.sdmmc.fifo.write(|w| unsafe { w.bits(bits.to_be()) });
+                        i += 4;
                     }
                 }
-                ControlFlow::Continue(()) => continue,
             }
         }
-    }
 
-    pub fn write_block(&mut self, addr: u32, block: &[u8; 512]) -> Result<(), Error> {
-        self.card()?;
+        let timeout: u32 = 0xffff_ffff;
+        for _ in 0..timeout {
+            if self.card_ready()? {
+                return Ok(());
+            }
+        }
 
-        todo!()
+        Err(Error::SoftwareTimeout)
     }
 
     /// Read eight 32-bit values from a half-full FIFO.
     #[inline]
     fn read_fifo_hf(&mut self, mut f: impl FnMut(u32) -> ()) -> Result<ControlFlow<(), ()>, Error> {
-        // TODO: Better timeout value.
         let timeout: u32 = 0xffff_ffff;
         for _ in 0..timeout {
             let sta = self.sdmmc.sta.read();
 
-            try_datapath!(sta);
+            datapath_rx_err!(sta);
 
             if sta.dbckend().bit() {
                 return Ok(ControlFlow::Break(()));
@@ -556,26 +630,31 @@ impl Sdmmc {
 
         let mut scr = [0; 2];
 
-        'outer: for n in scr.iter_mut().rev() {
-            loop {
-                let sta = self.sdmmc.sta.read();
+        let mut i = scr.len();
 
-                try_datapath!(sta);
+        let timeout: u32 = 0xffff_ffff;
+        for _ in 0..timeout {
+            let sta = self.sdmmc.sta.read();
 
-                if sta.dataend().bit() {
-                    break 'outer;
-                }
+            datapath_rx_err!(sta);
 
+            if i == 0 {
+                if sta.dbckend().bit() {
+                    return Ok(SCR::from(scr));
+                }
+            } else {
                 if sta.rxdavl().bit_is_set() {
+                    i -= 1;
+
                     let bits = u32::from_be(self.sdmmc.fifo.read().bits());
-                    *n = bits.to_le();
+                    scr[i] = bits.to_le();
 
-                    continue 'outer;
+                    continue;
                 }
             }
         }
 
-        return Ok(SCR::from(scr));
+        return Err(Error::SoftwareTimeout);
     }
 
     pub fn power_card(&mut self, on: bool) {
@@ -719,6 +798,7 @@ pub struct SdmmcBlockDevice<SDMMC> {
     sdmmc: core::cell::RefCell<SDMMC>,
 }
 
+#[cfg(feature = "embedded-sdmmc")]
 impl embedded_sdmmc::BlockDevice for SdmmcBlockDevice<Sdmmc> {
     type Error = Error;
 
@@ -728,15 +808,8 @@ impl embedded_sdmmc::BlockDevice for SdmmcBlockDevice<Sdmmc> {
         start_block_idx: embedded_sdmmc::BlockIdx,
         _reason: &str,
     ) -> Result<(), Self::Error> {
-        let start = start_block_idx.0;
         let mut sdmmc = self.sdmmc.borrow_mut();
-        for block_idx in start..(start + blocks.len() as u32) {
-            sdmmc.read_block(
-                block_idx,
-                &mut blocks[(block_idx - start) as usize].contents,
-            )?;
-        }
-        Ok(())
+        sdmmc.read_blocks(start_block_idx.0, blocks)
     }
 
     fn write(
@@ -746,10 +819,7 @@ impl embedded_sdmmc::BlockDevice for SdmmcBlockDevice<Sdmmc> {
     ) -> Result<(), Self::Error> {
         let start = start_block_idx.0;
         let mut sdmmc = self.sdmmc.borrow_mut();
-        for block_idx in start..(start + blocks.len() as u32) {
-            sdmmc.write_block(block_idx, &blocks[(block_idx - start) as usize].contents)?;
-        }
-        Ok(())
+        sdmmc.write_blocks(start_block_idx.0, blocks)
     }
 
     fn num_blocks(&self) -> Result<embedded_sdmmc::BlockCount, Self::Error> {

From 12f7bc87b1b36ac5c2b6e0827da41eb322588426 Mon Sep 17 00:00:00 2001
From: Markus Reiter <me@reitermark.us>
Date: Sat, 30 Apr 2022 20:32:06 +0200
Subject: [PATCH 05/23] Implement more SDMMC pins.

---
 src/sdmmc.rs | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/src/sdmmc.rs b/src/sdmmc.rs
index fd42b9c8..518cb115 100644
--- a/src/sdmmc.rs
+++ b/src/sdmmc.rs
@@ -106,7 +106,12 @@ macro_rules! pins {
     }
 }
 
-#[cfg(any(feature = "stm32l496",))]
+#[cfg(any(
+    feature = "stm32l476",
+    feature = "stm32l486",
+    feature = "stm32l496",
+    feature = "stm32l4a6"
+))]
 pins! {
   CLK: [gpio::PC12<Alternate<PushPull, 12>>]
   CMD: [gpio::PD2<Alternate<PushPull, 12>>]

From 4035c7c3cea73b70e96a38ad911c55ea61155634 Mon Sep 17 00:00:00 2001
From: Markus Reiter <me@reitermark.us>
Date: Sun, 1 May 2022 01:40:46 +0200
Subject: [PATCH 06/23] Clean up SDMMC code.

---
 src/sdmmc.rs | 211 ++++++++++++++++++++++++++-------------------------
 1 file changed, 108 insertions(+), 103 deletions(-)

diff --git a/src/sdmmc.rs b/src/sdmmc.rs
index 518cb115..c26f35ef 100644
--- a/src/sdmmc.rs
+++ b/src/sdmmc.rs
@@ -1,5 +1,7 @@
 #![cfg(feature = "sdmmc")]
 
+//! SDMMC peripheral abstraction
+
 use core::{
     fmt,
     ops::{ControlFlow, Deref, DerefMut},
@@ -8,7 +10,10 @@ use core::{
 use fugit::HertzU32 as Hertz;
 use sdio_host::{
     common_cmd::{self, ResponseLen},
-    sd::{CardCapacity, CardStatus, CurrentState, SDStatus, CIC, CID, CSD, OCR, RCA, SCR, SD},
+    sd::{
+        BusWidth, CardCapacity, CardStatus, CurrentState, SDStatus, CIC, CID, CSD, OCR, RCA, SCR,
+        SD,
+    },
     sd_cmd, Cmd,
 };
 
@@ -125,41 +130,47 @@ pins! {
   D7: [gpio::PC7<Alternate<PushPull, 12>>]
 }
 
-#[derive(Debug, Copy, Clone, Eq, PartialEq)]
-#[repr(u8)]
-pub enum BusWidth {
-    One = 0,
-    Four = 1,
-    Eight = 2,
-}
-
+/// SDMMC clock frequency.
 #[derive(Debug, Copy, Clone, Eq, PartialEq)]
 #[repr(u8)]
 pub enum ClockFreq {
+    /// 24 MHz
     Freq24MHz = 0,
+    /// 12 MHz
     Freq12MHz = 2,
+    /// 8 MHz
     Freq8MHz = 4,
+    /// 4 MHz
     Freq4MHz = 10,
+    /// 1 MHz
     Freq1MHz = 46,
+    /// 400 kHz
     Freq400KHz = 118,
 }
 
+/// Error during SDMMC operations.
 #[derive(Debug)]
 pub enum Error {
+    /// No card detected.
     NoCard,
-    SoftwareTimeout,
-    Crc,
-    DataCrcFail,
-    RxOverFlow,
+    /// Command CRC check failed.
+    CommandCrc,
+    /// Data block CRC check failed.
+    DataCrc,
+    /// Timeout in hardware.
     Timeout,
-    TxUnderErr,
-    WrongResponseSize,
+    /// Timeout in software.
+    SoftwareTimeout,
+    /// Receive FIFO overrun.
+    RxOverrun,
+    /// Transmit FIFO underrun.
+    TxUnderrun,
 }
 
 macro_rules! datapath_err {
     ($sta:expr) => {
         if $sta.dcrcfail().bit() {
-            return Err(Error::DataCrcFail);
+            return Err(Error::DataCrc);
         }
 
         if $sta.dtimeout().bit() {
@@ -171,7 +182,7 @@ macro_rules! datapath_err {
 macro_rules! datapath_rx_err {
     ($sta:expr) => {
         if $sta.rxoverr().bit() {
-            return Err(Error::RxOverFlow);
+            return Err(Error::RxOverrun);
         }
 
         datapath_err!($sta)
@@ -181,7 +192,7 @@ macro_rules! datapath_rx_err {
 macro_rules! datapath_tx_err {
     ($sta:expr) => {
         if $sta.rxoverr().bit() {
-            return Err(Error::RxOverFlow);
+            return Err(Error::RxOverrun);
         }
 
         datapath_err!($sta)
@@ -219,6 +230,7 @@ fn clear_all_interrupts(icr: &sdmmc1::ICR) {
     });
 }
 
+/// An initialized SD card.
 #[derive(Default)]
 pub struct SdCard {
     ocr: OCR<SD>,
@@ -228,7 +240,7 @@ pub struct SdCard {
     scr: SCR,
 }
 
-/// TODO:  Remove when https://github.com/jkristell/sdio-host/issues/11 is fixed.
+// TODO:  Remove when https://github.com/jkristell/sdio-host/issues/11 is fixed.
 impl fmt::Debug for SdCard {
     fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
         f.debug_struct("SdCard")
@@ -241,48 +253,42 @@ impl fmt::Debug for SdCard {
     }
 }
 
-#[derive(Debug, Clone, Copy)]
-/// Indicates transfer direction
-enum Dir {
-    HostToCard = 0,
-    CardToHost = 1,
-}
-
 impl SdCard {
-    /// Size in bytes.
+    /// Get the card's address.
+    pub fn address(&self) -> u16 {
+        self.rca.address()
+    }
+
+    /// Get the card size in bytes.
     pub fn size(&self) -> u64 {
         self.csd.card_size()
     }
 
-    fn capacity(&self) -> CardCapacity {
+    /// Get the card's capacity type.
+    pub fn capacity(&self) -> CardCapacity {
         if self.ocr.high_capacity() {
             CardCapacity::HighCapacity
         } else {
             CardCapacity::StandardCapacity
         }
     }
-
-    pub fn address(&self) -> u16 {
-        self.rca.address()
-    }
-
-    pub fn supports_widebus(&self) -> bool {
-        self.scr.bus_width_four()
-    }
 }
 
-#[derive(Debug, Clone, Copy, PartialEq)]
-pub enum SdCardType {
-    Sdsc,
-    SdhcSdxc,
+#[derive(Debug, Clone, Copy)]
+/// Indicates transfer direction
+enum Dir {
+    HostToCard = 0,
+    CardToHost = 1,
 }
 
+/// SD card version.
 #[derive(Debug, Clone, Copy, PartialEq)]
-pub enum SdCardVersion {
+enum SdCardVersion {
     V1,
     V2,
 }
 
+/// An SDMMC controller.
 #[derive(Debug)]
 pub struct Sdmmc {
     sdmmc: SDMMC1,
@@ -366,7 +372,7 @@ impl Sdmmc {
                 voltage_window,
             )) {
                 Ok(()) => (),
-                Err(Error::Crc) => (),
+                Err(Error::CommandCrc) => (),
                 Err(err) => return Err(err),
             }
 
@@ -444,28 +450,34 @@ impl Sdmmc {
         }
 
         let mut i = 0;
-        loop {
-            match self.read_fifo_hf(|bits| {
-                let start = i % 512;
-                blocks[i / 512][start..(start + 4)].copy_from_slice(&bits.to_be_bytes());
-                i += 4;
-            })? {
-                ControlFlow::Break(()) => {
+        let timeout: u32 = 0xffff_ffff;
+        for _ in 0..timeout {
+            let sta = self.sdmmc.sta.read();
+
+            datapath_rx_err!(sta);
+
+            if i == bytes {
+                if sta.dbckend().bit() {
                     if blocks.len() > 1 {
                         self.cmd(common_cmd::stop_transmission())?;
                     }
 
-                    if i == bytes {
-                        return Ok(());
-                    } else {
-                        return Err(Error::WrongResponseSize);
-                    }
+                    return Ok(());
+                }
+            } else if sta.rxfifohf().bit() {
+                for _ in 0..8 {
+                    let bits = u32::from_be(self.sdmmc.fifo.read().bits());
+                    let start = i % 512;
+                    blocks[i / 512][start..(start + 4)].copy_from_slice(&bits.to_be_bytes());
+                    i += 4;
                 }
-                ControlFlow::Continue(()) => continue,
             }
         }
+
+        Err(Error::SoftwareTimeout)
     }
 
+    #[inline]
     pub fn write_block<B: Deref<Target = [u8; 512]>>(
         &mut self,
         addr: u32,
@@ -474,6 +486,7 @@ impl Sdmmc {
         self.write_blocks(addr, &[block])
     }
 
+    #[inline]
     fn write_blocks<B: Deref<Target = [u8; 512]>>(
         &mut self,
         addr: u32,
@@ -541,38 +554,12 @@ impl Sdmmc {
         Err(Error::SoftwareTimeout)
     }
 
-    /// Read eight 32-bit values from a half-full FIFO.
-    #[inline]
-    fn read_fifo_hf(&mut self, mut f: impl FnMut(u32) -> ()) -> Result<ControlFlow<(), ()>, Error> {
-        let timeout: u32 = 0xffff_ffff;
-        for _ in 0..timeout {
-            let sta = self.sdmmc.sta.read();
-
-            datapath_rx_err!(sta);
-
-            if sta.dbckend().bit() {
-                return Ok(ControlFlow::Break(()));
-            }
-
-            if sta.rxfifohf().bit() {
-                for _ in 0..8 {
-                    let bits = u32::from_be(self.sdmmc.fifo.read().bits());
-                    f(bits)
-                }
-
-                return Ok(ControlFlow::Continue(()));
-            }
-        }
-
-        Err(Error::SoftwareTimeout)
-    }
-
     #[inline]
     fn set_bus(&self, bus_width: BusWidth, freq: ClockFreq) -> Result<(), Error> {
-        let card_widebus = self.card()?.supports_widebus();
+        let card = self.card()?;
 
         let bus_width = match bus_width {
-            BusWidth::Eight | BusWidth::Four if card_widebus => BusWidth::Four,
+            BusWidth::Eight | BusWidth::Four if card.scr.bus_width_four() => BusWidth::Four,
             _ => BusWidth::One,
         };
 
@@ -582,7 +569,12 @@ impl Sdmmc {
             w.clkdiv()
                 .bits(freq as u8)
                 .widbus()
-                .bits(bus_width as u8)
+                .bits(match bus_width {
+                    BusWidth::One => 0,
+                    BusWidth::Four => 1,
+                    BusWidth::Eight => 2,
+                    _ => unimplemented!(),
+                })
                 .clken()
                 .set_bit()
         });
@@ -662,7 +654,7 @@ impl Sdmmc {
         return Err(Error::SoftwareTimeout);
     }
 
-    pub fn power_card(&mut self, on: bool) {
+    fn power_card(&mut self, on: bool) {
         self.sdmmc
             .power
             .modify(|_, w| unsafe { w.pwrctrl().bits(if on { 0b11 } else { 0b00 }) });
@@ -680,12 +672,12 @@ impl Sdmmc {
         Ok(CardStatus::from(r1))
     }
 
-    /// Check if card is done writing/reading and back in transfer state
+    /// Check if card is done writing/reading and back in transfer state.
     fn card_ready(&mut self) -> Result<bool, Error> {
         Ok(self.read_status()?.state() == CurrentState::Transfer)
     }
 
-    /// Read the SDStatus struct
+    /// Read the [`SDStatus`](sdio_host::sd::SDStatus).
     pub fn read_sd_status(&mut self) -> Result<SDStatus, Error> {
         self.card()?;
         self.cmd(common_cmd::set_block_length(64))?;
@@ -695,27 +687,36 @@ impl Sdmmc {
         let mut sd_status = [0u32; 16];
 
         let mut i = sd_status.len();
-        loop {
-            match self.read_fifo_hf(|bits| {
-                i -= 1;
-                sd_status[i] = bits.to_le();
-            })? {
-                ControlFlow::Break(()) => {
-                    return if i == 0 {
-                        Ok(SDStatus::from(sd_status))
-                    } else {
-                        Err(Error::WrongResponseSize)
-                    }
+        let timeout: u32 = 0xffff_ffff;
+        for _ in 0..timeout {
+            let sta = self.sdmmc.sta.read();
+
+            datapath_rx_err!(sta);
+
+            if i == 0 {
+                if sta.dbckend().bit() {
+                    return Ok(SDStatus::from(sd_status));
+                }
+            } else if sta.rxfifohf().bit() {
+                for _ in 0..8 {
+                    i -= 1;
+                    let bits = u32::from_be(self.sdmmc.fifo.read().bits());
+                    sd_status[i] = bits.to_le();
                 }
-                ControlFlow::Continue(()) => continue,
             }
         }
+
+        Err(Error::SoftwareTimeout)
     }
 
+    /// Get the initialized card, if present.
+    #[inline]
     pub fn card(&self) -> Result<&SdCard, Error> {
         self.card.as_ref().ok_or(Error::NoCard)
     }
 
+    /// Select the card with the given address.
+    #[inline]
     fn select_card(&self, rca: u16) -> Result<(), Error> {
         let r = self.cmd(common_cmd::select_card(rca));
         match (r, rca) {
@@ -724,13 +725,16 @@ impl Sdmmc {
         }
     }
 
-    pub fn app_cmd<R: common_cmd::Resp>(&self, cmd: Cmd<R>) -> Result<(), Error> {
+    /// Send an app command to the card.
+    #[inline]
+    fn app_cmd<R: common_cmd::Resp>(&self, cmd: Cmd<R>) -> Result<(), Error> {
         let rca = self.card().map(|card| card.address()).unwrap_or(0);
         self.cmd(common_cmd::app_cmd(rca))?;
         self.cmd(cmd)
     }
 
-    pub fn cmd<R: common_cmd::Resp>(&self, cmd: Cmd<R>) -> Result<(), Error> {
+    /// Send a command to the card.
+    fn cmd<R: common_cmd::Resp>(&self, cmd: Cmd<R>) -> Result<(), Error> {
         while self.sdmmc.sta.read().cmdact().bit_is_set() {}
 
         // Clear the interrupts before we start
@@ -780,7 +784,7 @@ impl Sdmmc {
                 }
 
                 if sta.ccrcfail().bit() {
-                    return Err(Error::Crc);
+                    return Err(Error::CommandCrc);
                 }
 
                 if sta.cmdrend().bit() {
@@ -792,6 +796,7 @@ impl Sdmmc {
         Err(Error::SoftwareTimeout)
     }
 
+    /// Create an [`SdmmcBlockDevice`], which implements the [`BlockDevice`](embedded-sdmmc::BlockDevice) trait.
     pub fn into_block_device(self) -> SdmmcBlockDevice<Sdmmc> {
         SdmmcBlockDevice {
             sdmmc: core::cell::RefCell::new(self),
@@ -799,6 +804,7 @@ impl Sdmmc {
     }
 }
 
+/// Type implementing the [`BlockDevice`](embedded-sdmmc::BlockDevice) trait.
 pub struct SdmmcBlockDevice<SDMMC> {
     sdmmc: core::cell::RefCell<SDMMC>,
 }
@@ -822,7 +828,6 @@ impl embedded_sdmmc::BlockDevice for SdmmcBlockDevice<Sdmmc> {
         blocks: &[embedded_sdmmc::Block],
         start_block_idx: embedded_sdmmc::BlockIdx,
     ) -> Result<(), Self::Error> {
-        let start = start_block_idx.0;
         let mut sdmmc = self.sdmmc.borrow_mut();
         sdmmc.write_blocks(start_block_idx.0, blocks)
     }

From 7adef98c7d3542e4c7716e38d04f70fc4c952572 Mon Sep 17 00:00:00 2001
From: Markus Reiter <me@reitermark.us>
Date: Sun, 1 May 2022 04:03:41 +0200
Subject: [PATCH 07/23] Improve configuration of 48 MHz clock.

---
 examples/rng.rs |  15 +++++-
 src/rcc.rs      | 121 ++++++++++++++++++++++++++++++++++++++----------
 2 files changed, 111 insertions(+), 25 deletions(-)

diff --git a/examples/rng.rs b/examples/rng.rs
index 1c3b9477..afb8428b 100644
--- a/examples/rng.rs
+++ b/examples/rng.rs
@@ -12,6 +12,7 @@ use crate::hal::prelude::*;
 use crate::hal::serial::{Config, Serial};
 use crate::hal::stm32;
 use hal::hal::blocking::rng::Read;
+use hal::rcc::Clk48Source;
 
 macro_rules! uprint {
     ($serial:expr, $($arg:tt)*) => {
@@ -39,7 +40,19 @@ fn main() -> ! {
 
     let clocks = rcc
         .cfgr
-        .hsi48(true) // needed for RNG
+        // needed for RNG
+        .clk48_source({
+            if cfg!(any(
+                feature = "stm32l476",
+                feature = "stm32l486",
+                feature = "stm32l496",
+                feature = "stm32l4a6"
+            )) {
+                Clk48Source::Hsi48
+            } else {
+                Clk48Source::Msi
+            }
+        })
         .sysclk(64.MHz())
         .pclk1(32.MHz())
         .freeze(&mut flash.acr, &mut pwr);
diff --git a/src/rcc.rs b/src/rcc.rs
index fe4ea85b..151389da 100644
--- a/src/rcc.rs
+++ b/src/rcc.rs
@@ -81,7 +81,7 @@ impl RccExt for RCC {
                 hse: None,
                 lse: None,
                 msi: None,
-                hsi48: false,
+                clk48_source: None,
                 lsi: false,
                 hclk: None,
                 pclk1: None,
@@ -148,11 +148,13 @@ impl CRRCR {
     }
 
     /// Checks if the 48 MHz HSI is enabled
+    #[inline]
     pub fn is_hsi48_on(&mut self) -> bool {
         self.crrcr().read().hsi48on().bit()
     }
 
     /// Checks if the 48 MHz HSI is ready
+    #[inline]
     pub fn is_hsi48_ready(&mut self) -> bool {
         self.crrcr().read().hsi48rdy().bit()
     }
@@ -347,7 +349,6 @@ pub struct CFGR {
     hse: Option<HseConfig>,
     lse: Option<LseConfig>,
     msi: Option<MsiFreq>,
-    hsi48: bool,
     lsi: bool,
     hclk: Option<u32>,
     pclk1: Option<u32>,
@@ -355,6 +356,7 @@ pub struct CFGR {
     sysclk: Option<u32>,
     pll_source: Option<PllSource>,
     pll_config: Option<PllConfig>,
+    clk48_source: Option<Clk48Source>,
 }
 
 impl CFGR {
@@ -382,12 +384,6 @@ impl CFGR {
         self
     }
 
-    /// Enable the 48 MHz USB, RNG, SDMMC HSI clock source. Not available on all stm32l4x6 series
-    pub fn hsi48(mut self, on: bool) -> Self {
-        self.hsi48 = on;
-        self
-    }
-
     /// Enables the MSI with the specified speed
     pub fn msi(mut self, range: MsiFreq) -> Self {
         self.msi = Some(range);
@@ -431,6 +427,11 @@ impl CFGR {
         self
     }
 
+    pub fn clk48_source(mut self, source: Clk48Source) -> Self {
+        self.clk48_source = Some(source);
+        self
+    }
+
     /// Freezes the clock configuration, making it effective
     pub fn freeze(&self, acr: &mut ACR, pwr: &mut Pwr) -> Clocks {
         let rcc = unsafe { &*RCC::ptr() };
@@ -553,18 +554,40 @@ impl CFGR {
             while rcc.cr.read().msirdy().bit_is_clear() {}
         }
 
-        // Turn on USB, RNG Clock using the HSI48 CLK source
-        if self.hsi48 {
-            // p. 180 in ref-manual
-            rcc.crrcr.modify(|_, w| w.hsi48on().set_bit());
-
-            // Wait until HSI48 is running
-            while rcc.crrcr.read().hsi48rdy().bit_is_clear() {}
-        }
+        // Select 48 MHz clock source for SDMMC, USB, RNG, etc.
+        match self.clk48_source {
+            None => {
+                unsafe { rcc.ccipr.modify(|_, w| w.clk48sel().bits(0b00)) };
+            }
+            #[cfg(any(
+                feature = "stm32l476",
+                feature = "stm32l486",
+                feature = "stm32l496",
+                feature = "stm32l4a6"
+            ))]
+            Some(Clk48Source::Hsi48) => {
+                // Turn on USB, RNG Clock using the HSI48 CLK source.
+
+                // p. 180 in ref-manual
+                rcc.crrcr.modify(|_, w| w.hsi48on().set_bit());
+
+                // Wait until HSI48 is running.
+                while rcc.crrcr.read().hsi48rdy().bit_is_clear() {}
+
+                unsafe { rcc.ccipr.modify(|_, w| w.clk48sel().bits(0b00)) };
+            }
+            Some(Clk48Source::PllSai1) => {
+                unsafe { rcc.ccipr.modify(|_, w| w.clk48sel().bits(0b01)) };
 
-        // Select MSI as clock source for usb48, rng ...
-        if let Some(MsiFreq::RANGE48M) = self.msi {
-            unsafe { rcc.ccipr.modify(|_, w| w.clk48sel().bits(0b11)) };
+                unimplemented!()
+            }
+            Some(Clk48Source::Pll) => {
+                unsafe { rcc.ccipr.modify(|_, w| w.clk48sel().bits(0b10)) };
+            }
+            Some(Clk48Source::Msi) => {
+                assert_eq!(Some(MsiFreq::RANGE48M), self.msi);
+                unsafe { rcc.ccipr.modify(|_, w| w.clk48sel().bits(0b11)) };
+            }
         }
 
         //
@@ -716,6 +739,8 @@ impl CFGR {
             })
         }
 
+        let mut pll48m1clk = None;
+
         let sysclk_src_bits;
         let mut msi = self.msi;
         if let Some(pllconf) = pllconf {
@@ -723,7 +748,14 @@ impl CFGR {
             let r = pllconf.r.to_division_factor();
             let clock_speed = clock_speed / (pllconf.m as u32 + 1);
             let vco = clock_speed * pllconf.n as u32;
-            let output_clock = vco / r;
+            let pllclk = vco / r;
+
+            let q = (vco + 48_000_000 - 1) / 48_000_000;
+            pll48m1clk = Some((vco / q).Hz());
+
+            if self.clk48_source == Some(Clk48Source::Pll) {
+                assert_eq!(q, 48_000_000);
+            }
 
             assert!(r <= 8); // Allowed max output divider
             assert!(pllconf.n >= 8); // Allowed min multiplier
@@ -732,7 +764,7 @@ impl CFGR {
             assert!(clock_speed <= 16_000_000); // VCO input clock max
             assert!(vco >= 64_000_000); // VCO output min
             assert!(vco <= 334_000_000); // VCO output max
-            assert!(output_clock <= 80_000_000); // Max output clock
+            assert!(pllclk <= 80_000_000); // Max output clock
 
             // use PLL as source
             sysclk_src_bits = 0b11;
@@ -750,6 +782,8 @@ impl CFGR {
                     .bits(pllconf.r.to_bits())
                     .plln()
                     .bits(pllconf.n)
+                    .pllq()
+                    .bits(q as u8)
             });
 
             rcc.cr.modify(|_, w| w.pllon().set_bit());
@@ -810,15 +844,16 @@ impl CFGR {
             lsi: lsi_used,
             lse: self.lse.is_some(),
             msi,
-            hsi48: self.hsi48,
             pclk1: pclk1.Hz(),
             pclk2: pclk2.Hz(),
             ppre1,
             ppre2,
+            pll48m1clk,
             sysclk: sysclk.Hz(),
             timclk1: timclk1.Hz(),
             timclk2: timclk2.Hz(),
             pll_source: pllconf.map(|_| pll_source),
+            clk48_source: self.clk48_source,
         }
     }
 }
@@ -900,13 +935,31 @@ impl PllSource {
     }
 }
 
+#[derive(Clone, Copy, Debug, PartialEq)]
+/// 48 MHz clock source used by USB OTG FS, RNG and SDMMC.
+pub enum Clk48Source {
+    /// HSI48 clock
+    #[cfg(any(
+        feature = "stm32l476",
+        feature = "stm32l486",
+        feature = "stm32l496",
+        feature = "stm32l4a6"
+    ))]
+    Hsi48,
+    /// PLLSAI1 “Q” clock (PLL48M2CLK)
+    PllSai1,
+    /// PLL “Q” clock (PLL48M1CLK)
+    Pll,
+    /// MSI clock
+    Msi,
+}
+
 /// Frozen clock frequencies
 ///
 /// The existence of this value indicates that the clock configuration can no longer be changed
 #[derive(Clone, Copy, Debug)]
 pub struct Clocks {
     hclk: Hertz,
-    hsi48: bool,
     msi: Option<MsiFreq>,
     lsi: bool,
     lse: bool,
@@ -914,10 +967,12 @@ pub struct Clocks {
     pclk2: Hertz,
     ppre1: u8,
     ppre2: u8,
+    pll48m1clk: Option<Hertz>,
     sysclk: Hertz,
     timclk1: Hertz,
     timclk2: Hertz,
     pll_source: Option<PllSource>,
+    clk48_source: Option<Clk48Source>,
 }
 
 impl Clocks {
@@ -928,7 +983,25 @@ impl Clocks {
 
     /// Returns status of HSI48
     pub fn hsi48(&self) -> bool {
-        self.hsi48
+        #[cfg(any(
+            feature = "stm32l476",
+            feature = "stm32l486",
+            feature = "stm32l496",
+            feature = "stm32l4a6"
+        ))]
+        {
+            self.clk48_source == Some(Clk48Source::Hsi48)
+        }
+
+        #[cfg(not(any(
+            feature = "stm32l476",
+            feature = "stm32l486",
+            feature = "stm32l496",
+            feature = "stm32l4a6"
+        )))]
+        {
+            false
+        }
     }
 
     // Returns the status of the MSI

From 7cd43ed2a0aea3cde6901c39bb5b1989a095f32f Mon Sep 17 00:00:00 2001
From: Markus Reiter <me@reitermark.us>
Date: Sun, 1 May 2022 18:07:18 +0200
Subject: [PATCH 08/23] Implement `embedded_sdmmc::TimeSource` for `Rtc`.

---
 src/rtc.rs | 67 ++++++++++++++++++++++++++++++++++++++++++++++--------
 1 file changed, 58 insertions(+), 9 deletions(-)

diff --git a/src/rtc.rs b/src/rtc.rs
index 944a0744..1c04674f 100644
--- a/src/rtc.rs
+++ b/src/rtc.rs
@@ -202,23 +202,54 @@ impl Rtc {
         })
     }
 
-    /// Get date and time.
-    pub fn get_datetime(&self) -> PrimitiveDateTime {
+    /// Get raw microsecond.
+    ///
+    /// Reading `RTC_SSR` locks the values in the higher-order calendar shadow registers
+    /// until `RTC_DR` is read, so this must be called before `date_raw`.
+    #[inline]
+    fn microsecond_raw(&self) -> u32 {
         let sync_p = self.rtc_config.sync_prescaler as u32;
         let ssr = self.rtc.ssr.read();
         let micro = 1_000_000u32 / (sync_p + 1) * (sync_p - ssr.ss().bits() as u32);
+
+        micro
+    }
+
+    /// Get raw hour, minute and second.
+    ///
+    /// Reading `RTC_TR` locks the values in the higher-order calendar shadow registers
+    /// until `RTC_DR` is read, so this must be called before `date_raw`.
+    #[inline]
+    fn time_raw(&self) -> (u8, u8, u8) {
         let tr = self.rtc.tr.read();
-        let second = bcd2_to_byte((tr.st().bits(), tr.su().bits()));
-        let minute = bcd2_to_byte((tr.mnt().bits(), tr.mnu().bits()));
         let hour = bcd2_to_byte((tr.ht().bits(), tr.hu().bits()));
-        // Reading either RTC_SSR or RTC_TR locks the values in the higher-order
-        // calendar shadow registers until RTC_DR is read.
+        let minute = bcd2_to_byte((tr.mnt().bits(), tr.mnu().bits()));
+        let second = bcd2_to_byte((tr.st().bits(), tr.su().bits()));
+
+        (hour, minute, second)
+    }
+
+    /// Get raw year (since 1970), month and day.
+    ///
+    /// Must be called after `time_raw` and/or `microsecond_raw`.
+    #[inline]
+    fn date_raw(&self) -> (u16, u8, u8) {
         let dr = self.rtc.dr.read();
 
-        // let weekday = dr.wdu().bits();
-        let day = bcd2_to_byte((dr.dt().bits(), dr.du().bits()));
+        let year = bcd2_to_byte((dr.yt().bits(), dr.yu().bits())) as u16;
         let month = bcd2_to_byte((dr.mt().bit() as u8, dr.mu().bits()));
-        let year = bcd2_to_byte((dr.yt().bits(), dr.yu().bits())) as u16 + 1970_u16;
+        let day = bcd2_to_byte((dr.dt().bits(), dr.du().bits()));
+        // let weekday = dr.wdu().bits();
+
+        (year, month, day)
+    }
+
+    /// Get date and time.
+    pub fn get_datetime(&self) -> PrimitiveDateTime {
+        let (hour, minute, second) = self.time_raw();
+        let micro = self.microsecond_raw();
+        let (year, month, day) = self.date_raw();
+        let year = year + 1970;
 
         let time = Time::from_hms_micro(hour, minute, second, micro).unwrap();
         let date = Date::from_calendar_date(year.into(), month.try_into().unwrap(), day).unwrap();
@@ -809,3 +840,21 @@ fn bcd2_to_byte(bcd: (u8, u8)) -> u8 {
 
     tmp + (value & 0x0F)
 }
+
+#[cfg(feature = "embedded-sdmmc")]
+impl embedded_sdmmc::TimeSource for Rtc {
+    #[inline]
+    fn get_timestamp(&self) -> embedded_sdmmc::Timestamp {
+        let (hour, minute, second) = self.time_raw();
+        let (year, month, day) = self.date_raw();
+
+        embedded_sdmmc::Timestamp {
+            year_since_1970: year.try_into().unwrap(),
+            zero_indexed_month: month - 1,
+            zero_indexed_day: day - 1,
+            hours: hour,
+            minutes: minute,
+            seconds: second,
+        }
+    }
+}

From 3ac9a728cc14875aaeda8dfb9250d67d20ed9bf5 Mon Sep 17 00:00:00 2001
From: Markus Reiter <me@reitermark.us>
Date: Sun, 1 May 2022 23:13:35 +0200
Subject: [PATCH 09/23] Fix SDMMC endianness.

---
 src/sdmmc.rs | 27 +++++++++++++--------------
 1 file changed, 13 insertions(+), 14 deletions(-)

diff --git a/src/sdmmc.rs b/src/sdmmc.rs
index c26f35ef..0a862239 100644
--- a/src/sdmmc.rs
+++ b/src/sdmmc.rs
@@ -149,7 +149,7 @@ pub enum ClockFreq {
 }
 
 /// Error during SDMMC operations.
-#[derive(Debug)]
+#[derive(Debug, Clone, Copy)]
 pub enum Error {
     /// No card detected.
     NoCard,
@@ -344,7 +344,7 @@ impl Sdmmc {
 
         let check_pattern = 0xaa;
         self.cmd(sd_cmd::send_if_cond(1, check_pattern))?;
-        let cic = CIC::from(self.sdmmc.resp1.read().bits());
+        let cic = CIC::from(self.sdmmc.resp1.read().bits().to_le());
 
         let card_version = if cic.pattern() == 0xAA {
             SdCardVersion::V2
@@ -376,7 +376,7 @@ impl Sdmmc {
                 Err(err) => return Err(err),
             }
 
-            let ocr = OCR::from(self.sdmmc.resp1.read().bits());
+            let ocr = OCR::from(self.sdmmc.resp1.read().bits().to_le());
             if !ocr.is_busy() {
                 // Power up done
                 break ocr;
@@ -385,21 +385,21 @@ impl Sdmmc {
 
         self.cmd(common_cmd::all_send_cid())?;
         card.cid = CID::from([
-            self.sdmmc.resp1.read().bits(),
-            self.sdmmc.resp2.read().bits(),
-            self.sdmmc.resp3.read().bits(),
-            self.sdmmc.resp4.read().bits(),
+          self.sdmmc.resp4.read().bits().to_le(),
+          self.sdmmc.resp3.read().bits().to_le(),
+          self.sdmmc.resp2.read().bits().to_le(),
+          self.sdmmc.resp1.read().bits().to_le(),
         ]);
 
         self.cmd(sd_cmd::send_relative_address())?;
-        card.rca = RCA::from(self.sdmmc.resp1.read().bits());
+        card.rca = RCA::from(self.sdmmc.resp1.read().bits().to_le());
 
         self.cmd(common_cmd::send_csd(card.rca.address()))?;
         card.csd = CSD::from([
-            self.sdmmc.resp1.read().bits(),
-            self.sdmmc.resp2.read().bits(),
-            self.sdmmc.resp3.read().bits(),
-            self.sdmmc.resp4.read().bits(),
+          self.sdmmc.resp4.read().bits().to_le(),
+          self.sdmmc.resp3.read().bits().to_le(),
+          self.sdmmc.resp2.read().bits().to_le(),
+          self.sdmmc.resp1.read().bits().to_le(),
         ]);
 
         self.select_card(card.rca.address())?;
@@ -668,8 +668,7 @@ impl Sdmmc {
 
         self.cmd(common_cmd::card_status(card.address(), false))?;
 
-        let r1 = self.sdmmc.resp1.read().bits();
-        Ok(CardStatus::from(r1))
+        Ok(CardStatus::from(self.sdmmc.resp1.read().bits().to_le()))
     }
 
     /// Check if card is done writing/reading and back in transfer state.

From ab85cd6f012919f284f06934084a00860d8b1497 Mon Sep 17 00:00:00 2001
From: Markus Reiter <me@reitermark.us>
Date: Mon, 2 May 2022 03:08:45 +0200
Subject: [PATCH 10/23] Add `fatfs` implementation.

---
 .github/workflows/ci.yml |   8 +-
 Cargo.toml               |   5 ++
 src/sdmmc.rs             | 172 +++++++++++++++++++++++++++++++++++++--
 3 files changed, 173 insertions(+), 12 deletions(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 3f5ae0db..494221b2 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -25,10 +25,10 @@ jobs:
           - { id: stm32l462, additional-features: ",stm32-usbd" }
           - { id: stm32l471, additional-features: "" }
           - { id: stm32l475, additional-features: "" } # USB_OTG not supported by PAC
-          - { id: stm32l476, additional-features: ",otg_fs,sdmmc,embedded-sdmmc" }
-          - { id: stm32l486, additional-features: ",otg_fs,sdmmc,embedded-sdmmc" }
-          - { id: stm32l496, additional-features: ",otg_fs,sdmmc,embedded-sdmmc" }
-          - { id: stm32l4a6, additional-features: ",otg_fs,sdmmc,embedded-sdmmc" }
+          - { id: stm32l476, additional-features: ",otg_fs,sdmmc,embedded-sdmmc,fatfs" }
+          - { id: stm32l486, additional-features: ",otg_fs,sdmmc,embedded-sdmmc,fatfs" }
+          - { id: stm32l496, additional-features: ",otg_fs,sdmmc,embedded-sdmmc,fatfs" }
+          - { id: stm32l4a6, additional-features: ",otg_fs,sdmmc,embedded-sdmmc,fatfs" }
 
     steps:
       - uses: actions/checkout@v2
diff --git a/Cargo.toml b/Cargo.toml
index 3cc961c5..fded741e 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -30,6 +30,7 @@ fugit = "0.3.5"
 bitfield = "0.13.2"
 sdio-host = { version = "0.7.0", optional = true }
 embedded-sdmmc = { version = "0.3.0", optional = true }
+fatfs = { version = "0.4.0", default-features = false, optional = true }
 
 [dependencies.rand_core]
 version = "0.6.2"
@@ -73,6 +74,7 @@ unproven = ["embedded-hal/unproven"]
 otg_fs = ["synopsys-usb-otg"]
 sdmmc = ["dep:sdio-host"]
 embedded-sdmmc = ["dep:embedded-sdmmc", "sdmmc"]
+fatfs = ["dep:fatfs", "sdmmc"]
 
 # L4x1
 stm32l431 = [ "stm32l4/stm32l4x1" ]
@@ -207,3 +209,6 @@ required-features = ["rt"]
 [[example]]
 name = "adc_dma"
 required-features = ["rt"]
+
+[patch.crates-io]
+fatfs = { git = "https://github.com/rafalh/rust-fatfs" }
diff --git a/src/sdmmc.rs b/src/sdmmc.rs
index 0a862239..ccf1e4cd 100644
--- a/src/sdmmc.rs
+++ b/src/sdmmc.rs
@@ -385,10 +385,10 @@ impl Sdmmc {
 
         self.cmd(common_cmd::all_send_cid())?;
         card.cid = CID::from([
-          self.sdmmc.resp4.read().bits().to_le(),
-          self.sdmmc.resp3.read().bits().to_le(),
-          self.sdmmc.resp2.read().bits().to_le(),
-          self.sdmmc.resp1.read().bits().to_le(),
+            self.sdmmc.resp4.read().bits().to_le(),
+            self.sdmmc.resp3.read().bits().to_le(),
+            self.sdmmc.resp2.read().bits().to_le(),
+            self.sdmmc.resp1.read().bits().to_le(),
         ]);
 
         self.cmd(sd_cmd::send_relative_address())?;
@@ -396,10 +396,10 @@ impl Sdmmc {
 
         self.cmd(common_cmd::send_csd(card.rca.address()))?;
         card.csd = CSD::from([
-          self.sdmmc.resp4.read().bits().to_le(),
-          self.sdmmc.resp3.read().bits().to_le(),
-          self.sdmmc.resp2.read().bits().to_le(),
-          self.sdmmc.resp1.read().bits().to_le(),
+            self.sdmmc.resp4.read().bits().to_le(),
+            self.sdmmc.resp3.read().bits().to_le(),
+            self.sdmmc.resp2.read().bits().to_le(),
+            self.sdmmc.resp1.read().bits().to_le(),
         ]);
 
         self.select_card(card.rca.address())?;
@@ -838,3 +838,159 @@ impl embedded_sdmmc::BlockDevice for SdmmcBlockDevice<Sdmmc> {
         ))
     }
 }
+
+#[cfg(feature = "fatfs")]
+use fatfs::{IntoStorage, IoBase, IoError, Read, Seek, SeekFrom, Write};
+
+impl IoError for Error {
+    fn is_interrupted(&self) -> bool {
+        false
+    }
+
+    fn new_unexpected_eof_error() -> Self {
+        unimplemented!()
+    }
+
+    fn new_write_zero_error() -> Self {
+        unimplemented!()
+    }
+}
+
+#[derive(Debug)]
+pub struct FatFsCursor<SDMMC> {
+    sdmmc: SDMMC,
+    pos: u64,
+    partition_info: Option<(u32, u32)>,
+}
+
+impl FatFsCursor<Sdmmc> {
+    pub fn new(sdmmc: Sdmmc) -> Self {
+        Self {
+            sdmmc,
+            pos: 0,
+            partition_info: None,
+        }
+    }
+
+    pub fn partition_info(&mut self) -> Result<(u32, u32), Error> {
+        if let Some(partition_info) = self.partition_info {
+            return Ok(partition_info);
+        }
+
+        let mut block = [0; 512];
+        self.sdmmc.read_block(0, &mut block)?;
+
+        // TODO: Support other partitions.
+        let partition1_info = &block[446..][..16];
+        let lba_start = u32::from_le_bytes([
+            partition1_info[8],
+            partition1_info[9],
+            partition1_info[10],
+            partition1_info[11],
+        ]);
+
+        let num_blocks = u32::from_le_bytes([
+            partition1_info[12],
+            partition1_info[13],
+            partition1_info[14],
+            partition1_info[15],
+        ]);
+
+        Ok(*self.partition_info.get_or_insert((lba_start, num_blocks)))
+    }
+}
+
+#[cfg(feature = "fatfs")]
+impl IntoStorage<FatFsCursor<Sdmmc>> for Sdmmc {
+    fn into_storage(self) -> FatFsCursor<Sdmmc> {
+        FatFsCursor::new(self)
+    }
+}
+
+#[cfg(feature = "fatfs")]
+impl<SDMMC> IoBase for FatFsCursor<SDMMC> {
+    type Error = fatfs::Error<Error>;
+}
+
+#[cfg(feature = "fatfs")]
+impl Seek for FatFsCursor<Sdmmc> {
+    fn seek(&mut self, pos: SeekFrom) -> Result<u64, Self::Error> {
+        let itm = unsafe { &mut *cortex_m::peripheral::ITM::PTR };
+        // cortex_m::itm::write_fmt(&mut itm.stim[0], format_args!("Seek from {} to {:?}\n", self.pos, pos));
+
+        // TODO: Use `checked_add_signed` when stable.
+        let new_pos = match pos {
+            SeekFrom::Start(offset) => offset as i128,
+            SeekFrom::End(offset) => {
+                let end = self.partition_info()?.1 * 512;
+                end as i128 + offset as i128
+            }
+            SeekFrom::Current(offset) => self.pos as i128 + offset as i128,
+        };
+
+        if new_pos < 0 || new_pos > u64::MAX as i128 {
+            // Seek to negative or overflowing position.
+            return Err(Self::Error::InvalidInput);
+        }
+        let new_pos = new_pos as u64;
+
+        self.pos = new_pos;
+        Ok(self.pos)
+    }
+}
+
+#[cfg(feature = "fatfs")]
+impl Read for FatFsCursor<Sdmmc> {
+    #[track_caller]
+    fn read(&mut self, buf: &mut [u8]) -> Result<usize, Self::Error> {
+        let (start, end) = self.partition_info()?;
+
+        let end = end as u64 * 512;
+        let pos = self.pos.min(end);
+
+        let addr = start + (pos / 512) as u32;
+        let offset = (pos % 512) as usize;
+        let len = buf.len().min(512 - offset);
+
+        let mut block = [0; 512];
+        self.sdmmc.read_block(addr, &mut block)?;
+        buf[0..len].copy_from_slice(&block[offset..(offset + len)]);
+        self.pos += len as u64;
+
+        Ok(len)
+    }
+
+    // TODO: Add `read_exact` implementation which supports reading multiple blocks.
+}
+
+#[cfg(feature = "fatfs")]
+impl Write for FatFsCursor<Sdmmc> {
+    fn write(&mut self, buf: &[u8]) -> Result<usize, Self::Error> {
+        let (start, end) = self.partition_info()?;
+
+        let end = end as u64 * 512;
+        let pos = self.pos;
+
+        if pos + buf.len() as u64 >= end {
+            return Err(Self::Error::NotEnoughSpace);
+        }
+
+        let addr = start + (pos / 512) as u32;
+        let offset = (pos % 512) as usize;
+        let len = buf.len().min(512 - offset);
+
+        let mut block = [0; 512];
+        self.sdmmc.read_block(addr, &mut block)?;
+        block[offset..(offset + len)].copy_from_slice(&buf[0..len]);
+        self.sdmmc.write_block(addr, &block)?;
+        self.pos += len as u64;
+
+        Ok(len)
+    }
+
+    // TODO: Add `write_exact` implementation which supports writing multiple blocks.
+
+    fn flush(&mut self) -> Result<(), Self::Error> {
+        Ok(())
+    }
+}

From 1bc395e12fc72829974c4393a1a7732489b70631 Mon Sep 17 00:00:00 2001
From: Markus Reiter <me@reitermark.us>
Date: Mon, 2 May 2022 04:26:53 +0200
Subject: [PATCH 11/23] Fix examples.

---
 examples/rng.rs        | 17 +++++++++++++----
 examples/usb_serial.rs | 24 +++++++++++++++++++++++-
 2 files changed, 36 insertions(+), 5 deletions(-)

diff --git a/examples/rng.rs b/examples/rng.rs
index afb8428b..fb49ef8b 100644
--- a/examples/rng.rs
+++ b/examples/rng.rs
@@ -40,16 +40,25 @@ fn main() -> ! {
 
     let clocks = rcc
         .cfgr
-        // needed for RNG
+        // Needed for RNG.
         .clk48_source({
-            if cfg!(any(
+            #[cfg(any(
                 feature = "stm32l476",
                 feature = "stm32l486",
                 feature = "stm32l496",
                 feature = "stm32l4a6"
-            )) {
+            ))]
+            {
                 Clk48Source::Hsi48
-            } else {
+            }
+
+            #[cfg(not(any(
+                feature = "stm32l476",
+                feature = "stm32l486",
+                feature = "stm32l496",
+                feature = "stm32l4a6"
+            )))]
+            {
                 Clk48Source::Msi
             }
         })
diff --git a/examples/usb_serial.rs b/examples/usb_serial.rs
index 39c30921..4a411387 100644
--- a/examples/usb_serial.rs
+++ b/examples/usb_serial.rs
@@ -6,6 +6,7 @@
 extern crate panic_semihosting;
 
 use cortex_m_rt::entry;
+use stm32l4xx_hal::rcc::Clk48Source;
 use stm32l4xx_hal::usb::{Peripheral, UsbBus};
 use stm32l4xx_hal::{prelude::*, stm32};
 use usb_device::prelude::*;
@@ -43,7 +44,28 @@ fn main() -> ! {
 
     let _clocks = rcc
         .cfgr
-        .hsi48(true)
+        // Needed for USB.
+        .clk48_source({
+            #[cfg(any(
+                feature = "stm32l476",
+                feature = "stm32l486",
+                feature = "stm32l496",
+                feature = "stm32l4a6"
+            ))]
+            {
+                Clk48Source::Hsi48
+            }
+
+            #[cfg(not(any(
+                feature = "stm32l476",
+                feature = "stm32l486",
+                feature = "stm32l496",
+                feature = "stm32l4a6"
+            )))]
+            {
+                Clk48Source::Msi
+            }
+        })
         .sysclk(80.MHz())
         .freeze(&mut flash.acr, &mut pwr);
 

From 78e4758821ace1bb5a87c77207ba7343333d0b09 Mon Sep 17 00:00:00 2001
From: Markus Reiter <me@reitermark.us>
Date: Tue, 3 May 2022 01:04:39 +0200
Subject: [PATCH 12/23] Improve SDMMC reliability.

---
 src/sdmmc.rs | 218 ++++++++++++++++++++++++++++++++++-----------------
 1 file changed, 147 insertions(+), 71 deletions(-)

diff --git a/src/sdmmc.rs b/src/sdmmc.rs
index ccf1e4cd..8404c412 100644
--- a/src/sdmmc.rs
+++ b/src/sdmmc.rs
@@ -167,38 +167,73 @@ pub enum Error {
     TxUnderrun,
 }
 
-macro_rules! datapath_err {
-    ($sta:expr) => {
+macro_rules! sta_rx_tx_err {
+    ($sta:expr, $sdmmc:expr) => {
         if $sta.dcrcfail().bit() {
+            clear_static_data_flags(&$sdmmc.icr);
+
             return Err(Error::DataCrc);
         }
 
         if $sta.dtimeout().bit() {
+            clear_static_data_flags(&$sdmmc.icr);
+
             return Err(Error::Timeout);
         }
     };
 }
 
-macro_rules! datapath_rx_err {
-    ($sta:expr) => {
-        if $sta.rxoverr().bit() {
+macro_rules! sta_rx {
+    ($sdmmc:expr) => {{
+        let sta = $sdmmc.sta.read();
+
+        if sta.rxoverr().bit() {
+            clear_static_data_flags(&$sdmmc.icr);
+
             return Err(Error::RxOverrun);
         }
 
-        datapath_err!($sta)
-    };
+        sta_rx_tx_err!(sta, $sdmmc);
+
+        sta
+    }};
 }
 
-macro_rules! datapath_tx_err {
-    ($sta:expr) => {
-        if $sta.rxoverr().bit() {
-            return Err(Error::RxOverrun);
+macro_rules! sta_tx {
+    ($sdmmc:expr) => {{
+        let sta = $sdmmc.sta.read();
+
+        if sta.txunderr().bit() {
+            clear_static_data_flags(&$sdmmc.icr);
+
+            return Err(Error::TxUnderrun);
         }
 
-        datapath_err!($sta)
-    };
+        sta_rx_tx_err!(sta, $sdmmc);
+
+        sta
+    }};
 }
 
+#[inline]
+fn clear_static_data_flags(icr: &sdmmc1::ICR) {
+    icr.modify(|_, w| {
+        w.dcrcfailc()
+            .set_bit()
+            .dtimeoutc()
+            .set_bit()
+            .txunderrc()
+            .set_bit()
+            .rxoverrc()
+            .set_bit()
+            .dataendc()
+            .set_bit()
+            .dbckendc()
+            .set_bit()
+    })
+}
+
+#[inline]
 fn clear_all_interrupts(icr: &sdmmc1::ICR) {
     icr.modify(|_, w| {
         w.ccrcfailc()
@@ -440,40 +475,64 @@ impl Sdmmc {
 
         self.cmd(common_cmd::set_block_length(512))?;
 
-        let bytes = blocks.len() * 512;
-        self.start_datapath_transfer(bytes as u32, 9, Dir::CardToHost);
+        let block_count = blocks.len();
+        let byte_count = block_count * 512;
+        self.start_datapath_transfer(byte_count as u32, 9, Dir::CardToHost);
 
-        match blocks.len() {
+        match block_count {
             0 => return Ok(()),
             1 => self.cmd(common_cmd::read_single_block(addr))?,
             _ => self.cmd(common_cmd::read_multiple_blocks(addr))?,
         }
 
+        let mut current_block = &mut blocks[0];
+
         let mut i = 0;
         let timeout: u32 = 0xffff_ffff;
         for _ in 0..timeout {
-            let sta = self.sdmmc.sta.read();
-
-            datapath_rx_err!(sta);
+            let sta = sta_rx!(self.sdmmc);
 
-            if i == bytes {
+            // If we received all data, wait for transfer to end.
+            if i == byte_count {
                 if sta.dbckend().bit() {
-                    if blocks.len() > 1 {
+                    clear_static_data_flags(&self.sdmmc.icr);
+
+                    if block_count > 1 {
                         self.cmd(common_cmd::stop_transmission())?;
                     }
 
                     return Ok(());
                 }
-            } else if sta.rxfifohf().bit() {
-                for _ in 0..8 {
-                    let bits = u32::from_be(self.sdmmc.fifo.read().bits());
-                    let start = i % 512;
-                    blocks[i / 512][start..(start + 4)].copy_from_slice(&bits.to_be_bytes());
-                    i += 4;
+            }
+            // If the FIFO is half-full, receive some data.
+            else if sta.rxfifohf().bit() {
+                let offset = i % 512;
+
+                // SAFETY: We always read exactly 32 bytes from the FIFO into
+                // a 512-byte block. We move to the next block if needed before
+                // the next iteration.
+                //
+                // NOTE: This is needed since checked access takes to long and
+                // results in a FIFO overrun error on higher clock speeds.
+                unsafe {
+                    for j in 0..8 {
+                        let current_block = current_block.as_mut_ptr() as *mut [u8; 4];
+                        let current_bytes = &mut *current_block.add(offset / 4 + j);
+
+                        let bytes = u32::from_be(self.sdmmc.fifo.read().bits()).to_be_bytes();
+                        *current_bytes = bytes;
+                    }
+                }
+
+                i += 4 * 8;
+
+                if i % 512 == 0 && i != byte_count {
+                    current_block = &mut blocks[i / 512];
                 }
             }
         }
 
+        clear_static_data_flags(&self.sdmmc.icr);
         Err(Error::SoftwareTimeout)
     }
 
@@ -499,9 +558,9 @@ impl Sdmmc {
             _ => addr,
         };
 
-        let bytes = blocks.len() * 512;
+        let byte_count = blocks.len() * 512;
         self.cmd(common_cmd::set_block_length(512))?;
-        self.start_datapath_transfer(bytes as u32, 9, Dir::HostToCard);
+        self.start_datapath_transfer(byte_count as u32, 9, Dir::HostToCard);
 
         match blocks.len() {
             0 => return Ok(()),
@@ -509,41 +568,54 @@ impl Sdmmc {
             _ => self.cmd(common_cmd::write_multiple_blocks(addr))?,
         }
 
+        let mut current_block = &blocks[0];
+
         let mut i = 0;
         loop {
-            let sta = self.sdmmc.sta.read();
-
-            datapath_tx_err!(sta);
+            let sta = sta_tx!(self.sdmmc);
 
-            if i == bytes {
-                // If we sent all data, wait for transfer to end.
+            // If we sent all data, wait for transfer to end.
+            if i == byte_count {
                 if sta.dbckend().bit() {
+                    clear_static_data_flags(&self.sdmmc.icr);
+
                     if blocks.len() > 1 {
                         self.cmd(common_cmd::stop_transmission())?;
                     }
 
                     break;
                 }
-            } else {
-                // If the FIFO is half-empty, send some data.
-                if sta.txfifohe().bit() {
-                    for _ in 0..8 {
-                        let block = &blocks[i / 512];
-                        let start = i % 512;
-
-                        let bits = u32::from_be_bytes([
-                            block[start],
-                            block[start + 1],
-                            block[start + 2],
-                            block[start + 3],
-                        ]);
-                        self.sdmmc.fifo.write(|w| unsafe { w.bits(bits.to_be()) });
-                        i += 4;
+            }
+            // If the FIFO is half-empty, send some data.
+            else if sta.txfifohe().bit() {
+                let offset = i % 512;
+
+                // SAFETY: We always write exactly 32 bytes into the FIFO from
+                // a 512-byte block. We move to the next block if needed before
+                // the next iteration.
+                //
+                // NOTE: This is needed since checked access takes to long and
+                // results in a FIFO underrun error on higher clock speeds.
+                unsafe {
+                    for j in 0..8 {
+                        let current_block = current_block.as_ptr() as *const [u8; 4];
+                        let current_bytes = &*current_block.add(offset / 4 + j);
+
+                        let bits = u32::from_be_bytes(*current_bytes).to_be();
+                        self.sdmmc.fifo.write(|w| w.bits(bits));
                     }
                 }
+
+                i += 4 * 8;
+
+                if i % 512 == 0 && i != byte_count {
+                    current_block = &blocks[i / 512];
+                }
             }
         }
 
+        clear_static_data_flags(&self.sdmmc.icr);
+
         let timeout: u32 = 0xffff_ffff;
         for _ in 0..timeout {
             if self.card_ready()? {
@@ -631,23 +703,22 @@ impl Sdmmc {
 
         let timeout: u32 = 0xffff_ffff;
         for _ in 0..timeout {
-            let sta = self.sdmmc.sta.read();
-
-            datapath_rx_err!(sta);
+            let sta = sta_rx!(self.sdmmc);
 
+            // If we received all data, wait for transfer to end.
             if i == 0 {
                 if sta.dbckend().bit() {
+                    clear_static_data_flags(&self.sdmmc.icr);
+
                     return Ok(SCR::from(scr));
                 }
-            } else {
-                if sta.rxdavl().bit_is_set() {
-                    i -= 1;
+            } else if sta.rxdavl().bit_is_set() {
+                i -= 1;
 
-                    let bits = u32::from_be(self.sdmmc.fifo.read().bits());
-                    scr[i] = bits.to_le();
+                let bits = u32::from_be(self.sdmmc.fifo.read().bits());
+                scr[i] = bits.to_le();
 
-                    continue;
-                }
+                continue;
             }
         }
 
@@ -688,12 +759,13 @@ impl Sdmmc {
         let mut i = sd_status.len();
         let timeout: u32 = 0xffff_ffff;
         for _ in 0..timeout {
-            let sta = self.sdmmc.sta.read();
-
-            datapath_rx_err!(sta);
+            let sta = sta_rx!(self.sdmmc);
 
+            // If we received all data, wait for transfer to end.
             if i == 0 {
                 if sta.dbckend().bit() {
+                    clear_static_data_flags(&self.sdmmc.icr);
+
                     return Ok(SDStatus::from(sd_status));
                 }
             } else if sta.rxfifohf().bit() {
@@ -764,29 +836,33 @@ impl Sdmmc {
         for _ in 0..timeout {
             let sta = self.sdmmc.sta.read();
 
+            // Command transfer still in progress.
             if sta.cmdact().bit_is_set() {
-                // Command transfer still in progress.
                 continue;
             }
 
-            if cmd.response_len() == ResponseLen::Zero {
-                if sta.ctimeout().bit() {
-                    return Err(Error::Timeout);
-                }
+            if sta.ctimeout().bit() {
+                self.sdmmc.icr.modify(|_, w| w.ctimeoutc().set_bit());
 
+                return Err(Error::Timeout);
+            }
+
+            if cmd.response_len() == ResponseLen::Zero {
                 if sta.cmdsent().bit() {
+                    self.sdmmc.icr.modify(|_, w| w.cmdsentc().set_bit());
+
                     return Ok(());
                 }
             } else {
-                if sta.ctimeout().bit() {
-                    return Err(Error::Timeout);
-                }
-
                 if sta.ccrcfail().bit() {
+                    self.sdmmc.icr.modify(|_, w| w.ccrcfailc().set_bit());
+
                     return Err(Error::CommandCrc);
                 }
 
                 if sta.cmdrend().bit() {
+                    self.sdmmc.icr.modify(|_, w| w.cmdrendc().set_bit());
+
                     return Ok(());
                 }
             }

From 9b3d004fce6caff97a3dccbf63f19e9cd75753d8 Mon Sep 17 00:00:00 2001
From: Markus Reiter <me@reitermark.us>
Date: Tue, 3 May 2022 04:18:05 +0200
Subject: [PATCH 13/23] Clean up SDMMC flags.

---
 src/sdmmc.rs | 63 ++++++++++++++++++++++------------------------------
 1 file changed, 27 insertions(+), 36 deletions(-)

diff --git a/src/sdmmc.rs b/src/sdmmc.rs
index 8404c412..cd2c79f9 100644
--- a/src/sdmmc.rs
+++ b/src/sdmmc.rs
@@ -215,6 +215,20 @@ macro_rules! sta_tx {
     }};
 }
 
+#[inline]
+fn clear_static_command_flags(icr: &sdmmc1::ICR) {
+    icr.modify(|_, w| {
+        w.ccrcfailc()
+            .set_bit()
+            .ctimeoutc()
+            .set_bit()
+            .cmdrendc()
+            .set_bit()
+            .cmdsentc()
+            .set_bit()
+    })
+}
+
 #[inline]
 fn clear_static_data_flags(icr: &sdmmc1::ICR) {
     icr.modify(|_, w| {
@@ -235,34 +249,10 @@ fn clear_static_data_flags(icr: &sdmmc1::ICR) {
 
 #[inline]
 fn clear_all_interrupts(icr: &sdmmc1::ICR) {
-    icr.modify(|_, w| {
-        w.ccrcfailc()
-            .set_bit()
-            .ctimeoutc()
-            .set_bit()
-            .ceataendc()
-            .set_bit()
-            .cmdrendc()
-            .set_bit()
-            .cmdsentc()
-            .set_bit()
-            .dataendc()
-            .set_bit()
-            .dbckendc()
-            .set_bit()
-            .dcrcfailc()
-            .set_bit()
-            .dtimeoutc()
-            .set_bit()
-            .sdioitc()
-            .set_bit()
-            .stbiterrc()
-            .set_bit()
-            .rxoverrc()
-            .set_bit()
-            .txunderrc()
-            .set_bit()
-    });
+    clear_static_command_flags(icr);
+    clear_static_data_flags(icr);
+
+    icr.modify(|_, w| w.sdioitc().set_bit());
 }
 
 /// An initialized SD card.
@@ -772,7 +762,9 @@ impl Sdmmc {
                 for _ in 0..8 {
                     i -= 1;
                     let bits = u32::from_be(self.sdmmc.fifo.read().bits());
-                    sd_status[i] = bits.to_le();
+                    unsafe {
+                        *sd_status.get_unchecked_mut(i) = bits.to_le();
+                    }
                 }
             }
         }
@@ -808,9 +800,6 @@ impl Sdmmc {
     fn cmd<R: common_cmd::Resp>(&self, cmd: Cmd<R>) -> Result<(), Error> {
         while self.sdmmc.sta.read().cmdact().bit_is_set() {}
 
-        // Clear the interrupts before we start
-        clear_all_interrupts(&self.sdmmc.icr);
-
         self.sdmmc
             .arg
             .write(|w| unsafe { w.cmdarg().bits(cmd.arg) });
@@ -842,32 +831,34 @@ impl Sdmmc {
             }
 
             if sta.ctimeout().bit() {
-                self.sdmmc.icr.modify(|_, w| w.ctimeoutc().set_bit());
+                clear_static_command_flags(&self.sdmmc.icr);
 
                 return Err(Error::Timeout);
             }
 
             if cmd.response_len() == ResponseLen::Zero {
                 if sta.cmdsent().bit() {
-                    self.sdmmc.icr.modify(|_, w| w.cmdsentc().set_bit());
+                    clear_static_command_flags(&self.sdmmc.icr);
 
                     return Ok(());
                 }
             } else {
                 if sta.ccrcfail().bit() {
-                    self.sdmmc.icr.modify(|_, w| w.ccrcfailc().set_bit());
+                    clear_static_command_flags(&self.sdmmc.icr);
 
                     return Err(Error::CommandCrc);
                 }
 
                 if sta.cmdrend().bit() {
-                    self.sdmmc.icr.modify(|_, w| w.cmdrendc().set_bit());
+                    clear_static_command_flags(&self.sdmmc.icr);
 
                     return Ok(());
                 }
             }
         }
 
+        clear_static_command_flags(&self.sdmmc.icr);
+
         Err(Error::SoftwareTimeout)
     }
 

From caf50087de5cac965fde0869ead6b08075b1c010 Mon Sep 17 00:00:00 2001
From: Markus Reiter <me@reitermark.us>
Date: Tue, 3 May 2022 07:18:35 +0200
Subject: [PATCH 14/23] Add buffer for SDMMC access.

---
 src/sdmmc.rs | 32 ++++++++++++++++++++++----------
 1 file changed, 22 insertions(+), 10 deletions(-)

diff --git a/src/sdmmc.rs b/src/sdmmc.rs
index cd2c79f9..8561a8ec 100644
--- a/src/sdmmc.rs
+++ b/src/sdmmc.rs
@@ -928,6 +928,8 @@ pub struct FatFsCursor<SDMMC> {
     sdmmc: SDMMC,
     pos: u64,
     partition_info: Option<(u32, u32)>,
+    block: [u8; 512],
+    current_block: Option<u32>,
 }
 
 impl FatFsCursor<Sdmmc> {
@@ -936,6 +938,8 @@ impl FatFsCursor<Sdmmc> {
             sdmmc,
             pos: 0,
             partition_info: None,
+            block: [0; 512],
+            current_block: None,
         }
     }
 
@@ -982,9 +986,6 @@ impl<SDMMC> IoBase for FatFsCursor<SDMMC> {
 #[cfg(feature = "fatfs")]
 impl Seek for FatFsCursor<Sdmmc> {
     fn seek(&mut self, pos: SeekFrom) -> Result<u64, Self::Error> {
-        let itm = unsafe { &mut *cortex_m::peripheral::ITM::PTR };
-        // cortex_m::itm::write_fmt(&mut itm.stim[0], format_args!("Seek from {} to {:?}\n", self.pos, pos));
-
         // TODO: Use `checked_add_signed` when stable.
         let new_pos = match pos {
             SeekFrom::Start(offset) => offset as i128,
@@ -1019,9 +1020,14 @@ impl Read for FatFsCursor<Sdmmc> {
         let offset = (pos % 512) as usize;
         let len = buf.len().min(512 - offset);
 
-        let mut block = [0; 512];
-        self.sdmmc.read_block(addr, &mut block)?;
-        buf[0..len].copy_from_slice(&block[offset..(offset + len)]);
+        // Only read the block if we have not already read it.
+        if self.current_block != Some(addr) {
+            self.sdmmc.read_block(addr, &mut self.block)?;
+            self.current_block = Some(addr);
+        }
+
+        buf[0..len].copy_from_slice(&self.block[offset..(offset + len)]);
+
         self.pos += len as u64;
 
         Ok(len)
@@ -1046,10 +1052,16 @@ impl Write for FatFsCursor<Sdmmc> {
         let offset = (pos % 512) as usize;
         let len = buf.len().min(512 - offset);
 
-        let mut block = [0; 512];
-        self.sdmmc.read_block(addr, &mut block)?;
-        block[offset..(offset + len)].copy_from_slice(&buf[0..len]);
-        self.sdmmc.write_block(addr, &block)?;
+        // Only read the block if we have not already read it.
+        if self.current_block != Some(addr) {
+            self.current_block = None;
+            self.sdmmc.read_block(addr, &mut self.block)?;
+        }
+
+        self.block[offset..(offset + len)].copy_from_slice(&buf[0..len]);
+        self.sdmmc.write_block(addr, &self.block)?;
+        self.current_block = Some(addr);
+
         self.pos += len as u64;
 
         Ok(len)

From 4114f56e71efc8b2bd79eed4122e775a61ad8d5f Mon Sep 17 00:00:00 2001
From: Markus Reiter <me@reitermark.us>
Date: Fri, 6 May 2022 07:13:11 +0200
Subject: [PATCH 15/23] Improve `fatfs` support.

---
 src/rtc.rs   |  28 +++++++++++++
 src/sdmmc.rs | 113 +++++++++++++++++++++++++++++++++++++++++++--------
 2 files changed, 124 insertions(+), 17 deletions(-)

diff --git a/src/rtc.rs b/src/rtc.rs
index 1c04674f..39562b50 100644
--- a/src/rtc.rs
+++ b/src/rtc.rs
@@ -45,6 +45,7 @@ use crate::{
 };
 
 /// Interrupt event
+#[derive(Debug)]
 pub enum Event {
     WakeupTimer,
     AlarmA,
@@ -52,6 +53,7 @@ pub enum Event {
     Timestamp,
 }
 
+#[derive(Debug)]
 pub enum Alarm {
     AlarmA,
     AlarmB,
@@ -67,6 +69,7 @@ impl From<Alarm> for Event {
 }
 
 /// RTC Abstraction
+#[derive(Debug)]
 pub struct Rtc {
     rtc: RTC,
     rtc_config: RtcConfig,
@@ -858,3 +861,28 @@ impl embedded_sdmmc::TimeSource for Rtc {
         }
     }
 }
+
+#[cfg(feature = "fatfs")]
+impl fatfs::TimeProvider for Rtc {
+    fn get_current_date(&self) -> fatfs::Date {
+        let (year, month, day) = self.date_raw();
+
+        fatfs::Date::new(year.into(), month.into(), day.into())
+    }
+
+    fn get_current_date_time(&self) -> fatfs::DateTime {
+        let (hour, minute, second) = self.time_raw();
+        let micro = self.microsecond_raw();
+        let (year, month, day) = self.date_raw();
+
+        let time = fatfs::Time::new(
+            hour.into(),
+            minute.into(),
+            second.into(),
+            (micro / 1000) as u16,
+        );
+        let date = fatfs::Date::new(year.into(), month.into(), day.into());
+
+        fatfs::DateTime::new(date, time)
+    }
+}
diff --git a/src/sdmmc.rs b/src/sdmmc.rs
index 8561a8ec..43fec44f 100644
--- a/src/sdmmc.rs
+++ b/src/sdmmc.rs
@@ -165,6 +165,24 @@ pub enum Error {
     RxOverrun,
     /// Transmit FIFO underrun.
     TxUnderrun,
+    /// Invalid input.
+    InvalidInput,
+}
+
+impl fmt::Display for Error {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        match self {
+            Self::NoCard => "no card",
+            Self::CommandCrc => "command CRC check failed",
+            Self::DataCrc => "data block CRC check failed",
+            Self::Timeout => "timeout",
+            Self::SoftwareTimeout => "software timeout",
+            Self::RxOverrun => "receive FIFO overrun",
+            Self::TxUnderrun => "transmit FIFO underrun",
+            Self::InvalidInput => "invalid input",
+        }
+        .fmt(f)
+    }
 }
 
 macro_rules! sta_rx_tx_err {
@@ -932,8 +950,8 @@ pub struct FatFsCursor<SDMMC> {
     current_block: Option<u32>,
 }
 
-impl FatFsCursor<Sdmmc> {
-    pub fn new(sdmmc: Sdmmc) -> Self {
+impl<SDMMC> FatFsCursor<SDMMC> {
+    pub fn new(sdmmc: SDMMC) -> Self {
         Self {
             sdmmc,
             pos: 0,
@@ -942,14 +960,19 @@ impl FatFsCursor<Sdmmc> {
             current_block: None,
         }
     }
+}
 
+impl<SDMMC> FatFsCursor<SDMMC>
+where
+    SDMMC: AsMut<Sdmmc>,
+{
     pub fn partition_info(&mut self) -> Result<(u32, u32), Error> {
         if let Some(partition_info) = self.partition_info {
             return Ok(partition_info);
         }
 
         let mut block = [0; 512];
-        self.sdmmc.read_block(0, &mut block)?;
+        self.sdmmc.as_mut().read_block(0, &mut block)?;
 
         // TODO: Support other partitions.
         let partition1_info = &block[446..][..16];
@@ -971,20 +994,33 @@ impl FatFsCursor<Sdmmc> {
     }
 }
 
+impl AsMut<Sdmmc> for Sdmmc {
+    fn as_mut(&mut self) -> &mut Self {
+        self
+    }
+}
+
 #[cfg(feature = "fatfs")]
-impl IntoStorage<FatFsCursor<Sdmmc>> for Sdmmc {
-    fn into_storage(self) -> FatFsCursor<Sdmmc> {
+impl<'sdmmc> IntoStorage<FatFsCursor<&'sdmmc mut Sdmmc>> for &'sdmmc mut Sdmmc {
+    fn into_storage(self) -> FatFsCursor<&'sdmmc mut Sdmmc> {
         FatFsCursor::new(self)
     }
 }
 
 #[cfg(feature = "fatfs")]
 impl<SDMMC> IoBase for FatFsCursor<SDMMC> {
-    type Error = fatfs::Error<Error>;
+    type Error = Error;
 }
 
 #[cfg(feature = "fatfs")]
-impl Seek for FatFsCursor<Sdmmc> {
+impl<SDMMC> IoBase for &mut FatFsCursor<SDMMC> {
+    type Error = Error;
+}
+
+impl<SDMMC> Seek for FatFsCursor<SDMMC>
+where
+    SDMMC: AsMut<Sdmmc>,
+{
     fn seek(&mut self, pos: SeekFrom) -> Result<u64, Self::Error> {
         // TODO: Use `checked_add_signed` when stable.
         let new_pos = match pos {
@@ -1008,13 +1044,29 @@ impl Seek for FatFsCursor<Sdmmc> {
 }
 
 #[cfg(feature = "fatfs")]
-impl Read for FatFsCursor<Sdmmc> {
-    #[track_caller]
+impl<SDMMC> Seek for &mut FatFsCursor<SDMMC>
+where
+    SDMMC: AsMut<Sdmmc>,
+{
+    fn seek(&mut self, pos: SeekFrom) -> Result<u64, Self::Error> {
+        (*self).seek(pos)
+    }
+}
+
+#[cfg(feature = "fatfs")]
+impl<SDMMC> Read for FatFsCursor<SDMMC>
+where
+    SDMMC: AsMut<Sdmmc>,
+{
     fn read(&mut self, buf: &mut [u8]) -> Result<usize, Self::Error> {
         let (start, end) = self.partition_info()?;
 
         let end = end as u64 * 512;
-        let pos = self.pos.min(end);
+        let pos = self.pos;
+
+        if pos >= end {
+            return Ok(0);
+        }
 
         let addr = start + (pos / 512) as u32;
         let offset = (pos % 512) as usize;
@@ -1022,7 +1074,7 @@ impl Read for FatFsCursor<Sdmmc> {
 
         // Only read the block if we have not already read it.
         if self.current_block != Some(addr) {
-            self.sdmmc.read_block(addr, &mut self.block)?;
+            self.sdmmc.as_mut().read_block(addr, &mut self.block)?;
             self.current_block = Some(addr);
         }
 
@@ -1037,15 +1089,28 @@ impl Read for FatFsCursor<Sdmmc> {
 }
 
 #[cfg(feature = "fatfs")]
-impl Write for FatFsCursor<Sdmmc> {
+impl<SDMMC> Read for &mut FatFsCursor<SDMMC>
+where
+    SDMMC: AsMut<Sdmmc>,
+{
+    fn read(&mut self, buf: &mut [u8]) -> Result<usize, Self::Error> {
+        (*self).read(buf)
+    }
+}
+
+#[cfg(feature = "fatfs")]
+impl<SDMMC> Write for FatFsCursor<SDMMC>
+where
+    SDMMC: AsMut<Sdmmc>,
+{
     fn write(&mut self, buf: &[u8]) -> Result<usize, Self::Error> {
         let (start, end) = self.partition_info()?;
 
         let end = end as u64 * 512;
         let pos = self.pos;
 
-        if pos + buf.len() as u64 >= end {
-            return Err(Self::Error::NotEnoughSpace);
+        if pos >= end {
+            return Ok(0);
         }
 
         let addr = start + (pos / 512) as u32;
@@ -1053,13 +1118,13 @@ impl Write for FatFsCursor<Sdmmc> {
         let len = buf.len().min(512 - offset);
 
         // Only read the block if we have not already read it.
-        if self.current_block != Some(addr) {
+        if self.current_block != Some(addr) && len != 512 {
             self.current_block = None;
-            self.sdmmc.read_block(addr, &mut self.block)?;
+            self.sdmmc.as_mut().read_block(addr, &mut self.block)?;
         }
 
         self.block[offset..(offset + len)].copy_from_slice(&buf[0..len]);
-        self.sdmmc.write_block(addr, &self.block)?;
+        self.sdmmc.as_mut().write_block(addr, &self.block)?;
         self.current_block = Some(addr);
 
         self.pos += len as u64;
@@ -1073,3 +1138,17 @@ impl Write for FatFsCursor<Sdmmc> {
         Ok(())
     }
 }
+
+#[cfg(feature = "fatfs")]
+impl<SDMMC> Write for &mut FatFsCursor<SDMMC>
+where
+    SDMMC: AsMut<Sdmmc>,
+{
+    fn write(&mut self, buf: &[u8]) -> Result<usize, Self::Error> {
+        (*self).write(buf)
+    }
+
+    fn flush(&mut self) -> Result<(), Self::Error> {
+        (*self).flush()
+    }
+}

From 511c2608b20d1b7ee0ac8769146bdbc6a1b853e5 Mon Sep 17 00:00:00 2001
From: Markus Reiter <me@reitermark.us>
Date: Mon, 23 May 2022 17:21:36 +0200
Subject: [PATCH 16/23] Add missing `Debug` for DMA channels.

---
 src/dma.rs | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/dma.rs b/src/dma.rs
index 946bffc1..833adcba 100644
--- a/src/dma.rs
+++ b/src/dma.rs
@@ -541,6 +541,7 @@ macro_rules! dma {
                     /// A singleton that represents a single DMAx channel (channel X in this case)
                     ///
                     /// This singleton has exclusive access to the registers of the DMAx channel X
+                    #[derive(Debug)]
                     pub struct $CX;
 
                     impl $CX {
@@ -594,7 +595,9 @@ macro_rules! dma {
                         #[inline]
                         pub fn listen(&mut self, event: Event) {
                             match event {
-                                Event::HalfTransfer => self.ccr().modify(|_, w| w.htie().set_bit()),
+                                Event::HalfTransfer => {
+                                    self.ccr().modify(|_, w| w.htie().set_bit())
+                                },
                                 Event::TransferComplete => {
                                     self.ccr().modify(|_, w| w.tcie().set_bit())
                                 }

From b7fc5757d8c766f5673ea43d3d01ee39f1157527 Mon Sep 17 00:00:00 2001
From: Markus Reiter <me@reitermark.us>
Date: Mon, 23 May 2022 17:22:37 +0200
Subject: [PATCH 17/23] Clamp FatFs dates.

---
 src/rtc.rs | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/rtc.rs b/src/rtc.rs
index 39562b50..f2721d87 100644
--- a/src/rtc.rs
+++ b/src/rtc.rs
@@ -866,6 +866,7 @@ impl embedded_sdmmc::TimeSource for Rtc {
 impl fatfs::TimeProvider for Rtc {
     fn get_current_date(&self) -> fatfs::Date {
         let (year, month, day) = self.date_raw();
+        let year = (year + 1970).max(1980).min(2107);
 
         fatfs::Date::new(year.into(), month.into(), day.into())
     }
@@ -874,6 +875,7 @@ impl fatfs::TimeProvider for Rtc {
         let (hour, minute, second) = self.time_raw();
         let micro = self.microsecond_raw();
         let (year, month, day) = self.date_raw();
+        let year = (year + 1970).max(1980).min(2107);
 
         let time = fatfs::Time::new(
             hour.into(),

From 99580bedbede94a90fb5a2660512317aca10c171 Mon Sep 17 00:00:00 2001
From: Markus Reiter <me@reitermark.us>
Date: Mon, 23 May 2022 17:25:48 +0200
Subject: [PATCH 18/23] Add SDMMC DMA support.

---
 src/sdmmc.rs | 541 ++++++++++++++++++++++++++++++++++++---------------
 1 file changed, 389 insertions(+), 152 deletions(-)

diff --git a/src/sdmmc.rs b/src/sdmmc.rs
index 43fec44f..5f871caf 100644
--- a/src/sdmmc.rs
+++ b/src/sdmmc.rs
@@ -5,6 +5,8 @@
 use core::{
     fmt,
     ops::{ControlFlow, Deref, DerefMut},
+    slice,
+    sync::atomic::{self, Ordering},
 };
 
 use fugit::HertzU32 as Hertz;
@@ -18,6 +20,8 @@ use sdio_host::{
 };
 
 use crate::{
+    dma::dma2,
+    dmamux::{DmaInput, DmaMux},
     gpio::{self, Alternate, PushPull},
     pac::{sdmmc1, SDMMC1},
     rcc::{Clocks, Enable, Reset, APB2},
@@ -190,13 +194,13 @@ macro_rules! sta_rx_tx_err {
         if $sta.dcrcfail().bit() {
             clear_static_data_flags(&$sdmmc.icr);
 
-            return Err(Error::DataCrc);
-        }
-
-        if $sta.dtimeout().bit() {
+            Err(Error::DataCrc)
+        } else if $sta.dtimeout().bit() {
             clear_static_data_flags(&$sdmmc.icr);
 
-            return Err(Error::Timeout);
+            Err(Error::Timeout)
+        } else {
+            Ok($sta)
         }
     };
 }
@@ -208,12 +212,10 @@ macro_rules! sta_rx {
         if sta.rxoverr().bit() {
             clear_static_data_flags(&$sdmmc.icr);
 
-            return Err(Error::RxOverrun);
+            Err(Error::RxOverrun)
+        } else {
+            sta_rx_tx_err!(sta, $sdmmc)
         }
-
-        sta_rx_tx_err!(sta, $sdmmc);
-
-        sta
     }};
 }
 
@@ -224,43 +226,41 @@ macro_rules! sta_tx {
         if sta.txunderr().bit() {
             clear_static_data_flags(&$sdmmc.icr);
 
-            return Err(Error::TxUnderrun);
+            Err(Error::TxUnderrun)
+        } else {
+            sta_rx_tx_err!(sta, $sdmmc)
         }
-
-        sta_rx_tx_err!(sta, $sdmmc);
-
-        sta
     }};
 }
 
 #[inline]
 fn clear_static_command_flags(icr: &sdmmc1::ICR) {
-    icr.modify(|_, w| {
-        w.ccrcfailc()
-            .set_bit()
-            .ctimeoutc()
+    icr.write(|w| {
+        w.cmdsentc()
             .set_bit()
             .cmdrendc()
             .set_bit()
-            .cmdsentc()
+            .ctimeoutc()
+            .set_bit()
+            .ccrcfailc()
             .set_bit()
     })
 }
 
 #[inline]
 fn clear_static_data_flags(icr: &sdmmc1::ICR) {
-    icr.modify(|_, w| {
-        w.dcrcfailc()
+    icr.write(|w| {
+        w.dbckendc()
             .set_bit()
-            .dtimeoutc()
-            .set_bit()
-            .txunderrc()
+            .dataendc()
             .set_bit()
             .rxoverrc()
             .set_bit()
-            .dataendc()
+            .txunderrc()
+            .set_bit()
+            .dtimeoutc()
             .set_bit()
-            .dbckendc()
+            .dcrcfailc()
             .set_bit()
     })
 }
@@ -460,20 +460,16 @@ impl Sdmmc {
     }
 
     #[inline]
-    pub fn read_block<B: DerefMut<Target = [u8; 512]>>(
-        &mut self,
-        addr: u32,
-        block: B,
-    ) -> Result<(), Error> {
-        self.read_blocks(addr, &mut [block])
+    pub fn read_block(&mut self, addr: u32, block: &mut DataBlock) -> Result<(), Error> {
+        self.read_blocks(addr, slice::from_mut(block))
     }
 
-    #[inline]
-    pub fn read_blocks<B: DerefMut<Target = [u8; 512]>>(
+    pub(crate) fn start_read_transfer(
         &mut self,
         addr: u32,
-        blocks: &mut [B],
-    ) -> Result<(), Error> {
+        words: &mut [u32],
+        dma: bool,
+    ) -> Result<bool, Error> {
         let card = self.card()?;
 
         let addr = match card.capacity() {
@@ -483,29 +479,37 @@ impl Sdmmc {
 
         self.cmd(common_cmd::set_block_length(512))?;
 
-        let block_count = blocks.len();
-        let byte_count = block_count * 512;
-        self.start_datapath_transfer(byte_count as u32, 9, Dir::CardToHost);
+        let byte_count = words.len() * 4;
+        self.start_datapath_transfer(byte_count as u32, 9, Dir::CardToHost, dma);
 
+        let block_count = words.len() / 128;
         match block_count {
-            0 => return Ok(()),
+            0 => return Ok(false),
             1 => self.cmd(common_cmd::read_single_block(addr))?,
             _ => self.cmd(common_cmd::read_multiple_blocks(addr))?,
         }
 
-        let mut current_block = &mut blocks[0];
+        Ok(block_count > 1)
+    }
+
+    #[inline]
+    pub fn read_blocks(&mut self, addr: u32, blocks: &mut [DataBlock]) -> Result<(), Error> {
+        let words = DataBlock::blocks_to_words_mut(blocks);
+        let mut word_count = words.len();
+
+        let stop_transmission = self.start_read_transfer(addr, words, false)?;
+        let mut it = words.into_iter();
 
-        let mut i = 0;
         let timeout: u32 = 0xffff_ffff;
         for _ in 0..timeout {
-            let sta = sta_rx!(self.sdmmc);
+            let sta = sta_rx!(self.sdmmc)?;
 
             // If we received all data, wait for transfer to end.
-            if i == byte_count {
+            if word_count == 0 {
                 if sta.dbckend().bit() {
                     clear_static_data_flags(&self.sdmmc.icr);
 
-                    if block_count > 1 {
+                    if stop_transmission {
                         self.cmd(common_cmd::stop_transmission())?;
                     }
 
@@ -514,29 +518,14 @@ impl Sdmmc {
             }
             // If the FIFO is half-full, receive some data.
             else if sta.rxfifohf().bit() {
-                let offset = i % 512;
-
-                // SAFETY: We always read exactly 32 bytes from the FIFO into
-                // a 512-byte block. We move to the next block if needed before
-                // the next iteration.
-                //
-                // NOTE: This is needed since checked access takes to long and
-                // results in a FIFO overrun error on higher clock speeds.
-                unsafe {
-                    for j in 0..8 {
-                        let current_block = current_block.as_mut_ptr() as *mut [u8; 4];
-                        let current_bytes = &mut *current_block.add(offset / 4 + j);
-
-                        let bytes = u32::from_be(self.sdmmc.fifo.read().bits()).to_be_bytes();
-                        *current_bytes = bytes;
+                for _ in 0..8 {
+                    unsafe {
+                        let word = it.next().unwrap_unchecked();
+                        *word = self.sdmmc.fifo.read().bits();
                     }
                 }
 
-                i += 4 * 8;
-
-                if i % 512 == 0 && i != byte_count {
-                    current_block = &mut blocks[i / 512];
-                }
+                word_count -= 8;
             }
         }
 
@@ -545,20 +534,12 @@ impl Sdmmc {
     }
 
     #[inline]
-    pub fn write_block<B: Deref<Target = [u8; 512]>>(
-        &mut self,
-        addr: u32,
-        block: B,
-    ) -> Result<(), Error> {
-        self.write_blocks(addr, &[block])
-    }
-
-    #[inline]
-    fn write_blocks<B: Deref<Target = [u8; 512]>>(
+    pub(crate) fn start_write_transfer(
         &mut self,
         addr: u32,
-        blocks: &[B],
-    ) -> Result<(), Error> {
+        words: &[u32],
+        dma: bool,
+    ) -> Result<bool, Error> {
         let card = self.card()?;
 
         let addr = match card.capacity() {
@@ -566,28 +547,42 @@ impl Sdmmc {
             _ => addr,
         };
 
-        let byte_count = blocks.len() * 512;
+        let byte_count = words.len() * 4;
         self.cmd(common_cmd::set_block_length(512))?;
-        self.start_datapath_transfer(byte_count as u32, 9, Dir::HostToCard);
+        self.start_datapath_transfer(byte_count as u32, 9, Dir::HostToCard, dma);
 
-        match blocks.len() {
-            0 => return Ok(()),
+        let block_count = words.len() / 128;
+        match block_count {
+            0 => (),
             1 => self.cmd(common_cmd::write_single_block(addr))?,
             _ => self.cmd(common_cmd::write_multiple_blocks(addr))?,
         }
 
-        let mut current_block = &blocks[0];
+        Ok(block_count > 1)
+    }
+
+    #[inline]
+    pub fn write_block(&mut self, addr: u32, block: &DataBlock) -> Result<(), Error> {
+        self.write_blocks(addr, slice::from_ref(block))
+    }
+
+    #[inline]
+    fn write_blocks(&mut self, addr: u32, blocks: &[DataBlock]) -> Result<(), Error> {
+        let words = DataBlock::blocks_to_words(blocks);
+        let mut word_count = words.len();
+
+        let stop_transmission = self.start_write_transfer(addr, words, false)?;
+        let mut it = words.into_iter();
 
-        let mut i = 0;
         loop {
-            let sta = sta_tx!(self.sdmmc);
+            let sta = sta_tx!(self.sdmmc)?;
 
             // If we sent all data, wait for transfer to end.
-            if i == byte_count {
+            if word_count == 0 {
                 if sta.dbckend().bit() {
                     clear_static_data_flags(&self.sdmmc.icr);
 
-                    if blocks.len() > 1 {
+                    if stop_transmission {
                         self.cmd(common_cmd::stop_transmission())?;
                     }
 
@@ -596,29 +591,14 @@ impl Sdmmc {
             }
             // If the FIFO is half-empty, send some data.
             else if sta.txfifohe().bit() {
-                let offset = i % 512;
-
-                // SAFETY: We always write exactly 32 bytes into the FIFO from
-                // a 512-byte block. We move to the next block if needed before
-                // the next iteration.
-                //
-                // NOTE: This is needed since checked access takes to long and
-                // results in a FIFO underrun error on higher clock speeds.
-                unsafe {
-                    for j in 0..8 {
-                        let current_block = current_block.as_ptr() as *const [u8; 4];
-                        let current_bytes = &*current_block.add(offset / 4 + j);
-
-                        let bits = u32::from_be_bytes(*current_bytes).to_be();
-                        self.sdmmc.fifo.write(|w| w.bits(bits));
-                    }
+                for _ in 0..8 {
+                    self.sdmmc.fifo.write(|w| unsafe {
+                        let word = it.next().unwrap_unchecked();
+                        w.bits(*word)
+                    });
                 }
 
-                i += 4 * 8;
-
-                if i % 512 == 0 && i != byte_count {
-                    current_block = &blocks[i / 512];
-                }
+                word_count -= 8;
             }
         }
 
@@ -661,7 +641,13 @@ impl Sdmmc {
         Ok(())
     }
 
-    fn start_datapath_transfer(&self, length_bytes: u32, block_size: u8, direction: Dir) {
+    fn start_datapath_transfer(
+        &self,
+        length_bytes: u32,
+        block_size: u8,
+        direction: Dir,
+        dma: bool,
+    ) {
         // Block size up to 2^14 bytes
         assert!(block_size <= 14);
 
@@ -696,13 +682,15 @@ impl Sdmmc {
                 .clear_bit()
                 .dten()
                 .set_bit()
+                .dmaen()
+                .bit(dma)
         });
     }
 
     fn get_scr(&self, rca: u16) -> Result<SCR, Error> {
         self.cmd(common_cmd::set_block_length(8))?;
         self.cmd(common_cmd::app_cmd(rca))?;
-        self.start_datapath_transfer(8, 3, Dir::CardToHost);
+        self.start_datapath_transfer(8, 3, Dir::CardToHost, false);
         self.cmd(sd_cmd::send_scr())?;
 
         let mut scr = [0; 2];
@@ -711,7 +699,7 @@ impl Sdmmc {
 
         let timeout: u32 = 0xffff_ffff;
         for _ in 0..timeout {
-            let sta = sta_rx!(self.sdmmc);
+            let sta = sta_rx!(self.sdmmc)?;
 
             // If we received all data, wait for transfer to end.
             if i == 0 {
@@ -759,7 +747,7 @@ impl Sdmmc {
     pub fn read_sd_status(&mut self) -> Result<SDStatus, Error> {
         self.card()?;
         self.cmd(common_cmd::set_block_length(64))?;
-        self.start_datapath_transfer(64, 6, Dir::CardToHost);
+        self.start_datapath_transfer(64, 6, Dir::CardToHost, false);
         self.app_cmd(sd_cmd::sd_status())?;
 
         let mut sd_status = [0u32; 16];
@@ -767,7 +755,7 @@ impl Sdmmc {
         let mut i = sd_status.len();
         let timeout: u32 = 0xffff_ffff;
         for _ in 0..timeout {
-            let sta = sta_rx!(self.sdmmc);
+            let sta = sta_rx!(self.sdmmc)?;
 
             // If we received all data, wait for transfer to end.
             if i == 0 {
@@ -840,6 +828,7 @@ impl Sdmmc {
         });
 
         let timeout = 5000 * (self.clock.raw() / 8 / 1000);
+        let mut res = Err(Error::SoftwareTimeout);
         for _ in 0..timeout {
             let sta = self.sdmmc.sta.read();
 
@@ -849,35 +838,30 @@ impl Sdmmc {
             }
 
             if sta.ctimeout().bit() {
-                clear_static_command_flags(&self.sdmmc.icr);
-
-                return Err(Error::Timeout);
+                res = Err(Error::Timeout);
+                break;
             }
 
             if cmd.response_len() == ResponseLen::Zero {
                 if sta.cmdsent().bit() {
-                    clear_static_command_flags(&self.sdmmc.icr);
-
-                    return Ok(());
+                    res = Ok(());
+                    break;
                 }
             } else {
                 if sta.ccrcfail().bit() {
-                    clear_static_command_flags(&self.sdmmc.icr);
-
-                    return Err(Error::CommandCrc);
+                    res = Err(Error::CommandCrc);
+                    break;
                 }
 
                 if sta.cmdrend().bit() {
-                    clear_static_command_flags(&self.sdmmc.icr);
-
-                    return Ok(());
+                    res = Ok(());
+                    break;
                 }
             }
         }
 
         clear_static_command_flags(&self.sdmmc.icr);
-
-        Err(Error::SoftwareTimeout)
+        res
     }
 
     /// Create an [`SdmmcBlockDevice`], which implements the [`BlockDevice`](embedded-sdmmc::BlockDevice) trait.
@@ -888,6 +872,213 @@ impl Sdmmc {
     }
 }
 
+#[derive(Debug, Clone)]
+#[repr(align(4))]
+pub struct DataBlock(pub [u8; 512]);
+
+impl DataBlock {
+    pub const fn new() -> Self {
+        Self([0; 512])
+    }
+
+    pub fn blocks_to_words(blocks: &[DataBlock]) -> &[u32] {
+        let mut word_count = blocks.len() * 128;
+        // SAFETY: `DataBlock` is 4-byte aligned.
+        unsafe { slice::from_raw_parts(blocks.as_ptr() as *mut u32, word_count) }
+    }
+
+    pub fn blocks_to_words_mut(blocks: &mut [DataBlock]) -> &mut [u32] {
+        let mut word_count = blocks.len() * 128;
+        // SAFETY: `DataBlock` is 4-byte aligned.
+        unsafe { slice::from_raw_parts_mut(blocks.as_mut_ptr() as *mut u32, word_count) }
+    }
+}
+
+#[derive(Debug)]
+pub struct SdmmcDma {
+    sdmmc: Sdmmc,
+    channel: dma2::C5,
+}
+
+impl SdmmcDma {
+    #[inline]
+    pub fn new(sdmmc: Sdmmc, mut channel: dma2::C5) -> Self {
+        channel.set_peripheral_address(sdmmc.sdmmc.fifo.as_ptr() as u32, false);
+        channel.set_request_line(DmaInput::SdMmc1).unwrap();
+        channel.ccr().modify(|_, w| {
+            w
+                // memory to memory mode disabled
+                .mem2mem()
+                .clear_bit()
+                // medium channel priority level
+                .pl()
+                .very_high()
+                // 32-bit memory size
+                .msize()
+                .bits32()
+                // 32-bit peripheral size
+                .psize()
+                .bits32()
+                // circular mode disabled
+                .circ()
+                .clear_bit()
+        });
+
+        Self { sdmmc, channel }
+    }
+
+    #[inline]
+    pub fn read_block(&mut self, addr: u32, block: &mut DataBlock) -> Result<(), Error> {
+        self.read_blocks(addr, slice::from_mut(block))
+    }
+
+    #[inline]
+    pub fn read_blocks(&mut self, addr: u32, blocks: &mut [DataBlock]) -> Result<(), Error> {
+        let words = DataBlock::blocks_to_words_mut(blocks);
+
+        self.channel.ccr().modify(|_, w| {
+            w
+                // read from peripheral/write to memory
+                .dir()
+                .clear_bit()
+        });
+        self.channel
+            .set_memory_address(words.as_mut_ptr() as u32, true);
+        self.channel.set_transfer_length(words.len() as u16);
+        atomic::compiler_fence(Ordering::Release);
+
+        let stop_transmission = self.sdmmc.start_read_transfer(addr, words, true)?;
+        self.channel.start();
+
+        // self.sdmmc.sdmmc.mask.modify(|_, w| {
+        //     w.dbckendie()
+        //         .set_bit()
+        //         .dataendie()
+        //         .set_bit()
+        //         .dcrcfailie()
+        //         .set_bit()
+        //         .dtimeoutie()
+        //         .set_bit()
+        //         .rxoverrie()
+        //         .set_bit()
+        // });
+
+        let res = loop {
+            match sta_rx!(self.sdmmc.sdmmc) {
+                Ok(sta) => {
+                    if sta.dbckend().bit_is_set() {
+                        break Ok(());
+                    }
+                }
+                Err(err) => break Err(err),
+            }
+        };
+
+        clear_static_data_flags(&self.sdmmc.sdmmc.icr);
+        self.channel.stop();
+
+        if stop_transmission {
+            res.or(self.sdmmc.cmd(common_cmd::stop_transmission()))
+        } else {
+            res
+        }
+    }
+
+    #[inline]
+    pub fn write_block(&mut self, addr: u32, block: &DataBlock) -> Result<(), Error> {
+        self.write_blocks(addr, slice::from_ref(block))
+    }
+
+    #[inline]
+    fn write_blocks(&mut self, addr: u32, blocks: &[DataBlock]) -> Result<(), Error> {
+        let words = DataBlock::blocks_to_words(blocks);
+
+        self.channel.ccr().modify(|_, w| {
+            w
+                // read from memory/write to peripheral
+                .dir()
+                .set_bit()
+        });
+        self.channel.set_memory_address(words.as_ptr() as u32, true);
+        self.channel.set_transfer_length(words.len() as u16);
+        atomic::compiler_fence(Ordering::Release);
+
+        let stop_transmission = self.sdmmc.start_write_transfer(addr, words, true)?;
+        self.channel.start();
+
+        let res = loop {
+            match sta_tx!(self.sdmmc.sdmmc) {
+                Ok(sta) => {
+                    if sta.dbckend().bit_is_set() {
+                        break Ok(());
+                    }
+                }
+                Err(err) => break Err(err),
+            }
+        };
+
+        clear_static_data_flags(&self.sdmmc.sdmmc.icr);
+        self.channel.stop();
+
+        let res = if stop_transmission {
+            res.or(self.sdmmc.cmd(common_cmd::stop_transmission()))
+        } else {
+            res
+        };
+
+        res?;
+
+        let timeout: u32 = 0xffff_ffff;
+        for _ in 0..timeout {
+            if self.sdmmc.card_ready()? {
+                return Ok(());
+            }
+        }
+
+        Err(Error::SoftwareTimeout)
+    }
+
+    #[inline]
+    pub fn split(mut self) -> (Sdmmc, dma2::C5) {
+        (self.sdmmc, self.channel)
+    }
+
+    #[inline]
+    pub fn interrupt_handler() {
+        let sdmmc = unsafe { &*(SDMMC1::PTR) };
+
+        let sta = sdmmc.sta.read();
+
+        log::info!("irq sta = {:024b}", sta.bits());
+
+        if sta.dbckend().bit_is_set()
+            || sta.dcrcfail().bit_is_set()
+            || sta.dtimeout().bit_is_set()
+            || sta.rxoverr().bit_is_set()
+            || sta.txunderr().bit_is_set()
+        {
+            sdmmc.mask.modify(|_, w| {
+                w.dbckendie()
+                    .clear_bit()
+                    .dataendie()
+                    .clear_bit()
+                    .dcrcfailie()
+                    .clear_bit()
+                    .dtimeoutie()
+                    .clear_bit()
+                    .txunderrie()
+                    .clear_bit()
+                    .rxoverrie()
+                    .clear_bit()
+                    .txfifoheie()
+                    .clear_bit()
+                    .rxfifohfie()
+                    .clear_bit()
+            });
+        }
+    }
+}
+
 /// Type implementing the [`BlockDevice`](embedded-sdmmc::BlockDevice) trait.
 pub struct SdmmcBlockDevice<SDMMC> {
     sdmmc: core::cell::RefCell<SDMMC>,
@@ -904,7 +1095,14 @@ impl embedded_sdmmc::BlockDevice for SdmmcBlockDevice<Sdmmc> {
         _reason: &str,
     ) -> Result<(), Self::Error> {
         let mut sdmmc = self.sdmmc.borrow_mut();
-        sdmmc.read_blocks(start_block_idx.0, blocks)
+
+        for block in blocks {
+            // TODO: `embedded_sdmmc::Block` should be aligned to 4 bytes.
+            let data_block = unsafe { &mut *(block.contents.as_mut_ptr() as *mut _) };
+            sdmmc.read_block(start_block_idx.0, data_block)?;
+        }
+
+        Ok(())
     }
 
     fn write(
@@ -913,7 +1111,14 @@ impl embedded_sdmmc::BlockDevice for SdmmcBlockDevice<Sdmmc> {
         start_block_idx: embedded_sdmmc::BlockIdx,
     ) -> Result<(), Self::Error> {
         let mut sdmmc = self.sdmmc.borrow_mut();
-        sdmmc.write_blocks(start_block_idx.0, blocks)
+
+        for block in blocks {
+            // TODO: `embedded_sdmmc::Block` should be aligned to 4 bytes.
+            let data_block = unsafe { &*(block.contents.as_ptr() as *const _) };
+            sdmmc.write_block(start_block_idx.0, data_block)?;
+        }
+
+        Ok(())
     }
 
     fn num_blocks(&self) -> Result<embedded_sdmmc::BlockCount, Self::Error> {
@@ -946,7 +1151,7 @@ pub struct FatFsCursor<SDMMC> {
     sdmmc: SDMMC,
     pos: u64,
     partition_info: Option<(u32, u32)>,
-    block: [u8; 512],
+    block: DataBlock,
     current_block: Option<u32>,
 }
 
@@ -956,26 +1161,64 @@ impl<SDMMC> FatFsCursor<SDMMC> {
             sdmmc,
             pos: 0,
             partition_info: None,
-            block: [0; 512],
+            block: DataBlock([0; 512]),
             current_block: None,
         }
     }
 }
 
+pub trait SdmmcIo {
+    fn read_block(&mut self, addr: u32, block: &mut DataBlock) -> Result<(), Error>;
+    fn write_block(&mut self, addr: u32, block: &DataBlock) -> Result<(), Error>;
+}
+
+impl SdmmcIo for Sdmmc {
+    fn read_block(&mut self, addr: u32, block: &mut DataBlock) -> Result<(), Error> {
+        Self::read_block(self, addr, block)
+    }
+
+    fn write_block(&mut self, addr: u32, block: &DataBlock) -> Result<(), Error> {
+        Self::write_block(self, addr, block)
+    }
+}
+
+impl SdmmcIo for SdmmcDma {
+    fn read_block(&mut self, addr: u32, block: &mut DataBlock) -> Result<(), Error> {
+        Self::read_block(self, addr, block)
+    }
+
+    fn write_block(&mut self, addr: u32, block: &DataBlock) -> Result<(), Error> {
+        Self::write_block(self, addr, block)
+    }
+}
+
+impl<T> SdmmcIo for &mut T
+where
+    T: SdmmcIo,
+{
+    fn read_block(&mut self, addr: u32, block: &mut DataBlock) -> Result<(), Error> {
+        (*self).read_block(addr, block)
+    }
+
+    fn write_block(&mut self, addr: u32, block: &DataBlock) -> Result<(), Error> {
+        (*self).write_block(addr, block)
+    }
+}
+
 impl<SDMMC> FatFsCursor<SDMMC>
 where
-    SDMMC: AsMut<Sdmmc>,
+    SDMMC: SdmmcIo,
 {
     pub fn partition_info(&mut self) -> Result<(u32, u32), Error> {
         if let Some(partition_info) = self.partition_info {
             return Ok(partition_info);
         }
 
-        let mut block = [0; 512];
-        self.sdmmc.as_mut().read_block(0, &mut block)?;
+        let mut block = DataBlock([0; 512]);
+        self.sdmmc.read_block(0, &mut block)?;
 
         // TODO: Support other partitions.
-        let partition1_info = &block[446..][..16];
+        let partition1_info = &block.0[446..][..16];
         let lba_start = u32::from_le_bytes([
             partition1_info[8],
             partition1_info[9],
@@ -994,12 +1237,6 @@ where
     }
 }
 
-impl AsMut<Sdmmc> for Sdmmc {
-    fn as_mut(&mut self) -> &mut Self {
-        self
-    }
-}
-
 #[cfg(feature = "fatfs")]
 impl<'sdmmc> IntoStorage<FatFsCursor<&'sdmmc mut Sdmmc>> for &'sdmmc mut Sdmmc {
     fn into_storage(self) -> FatFsCursor<&'sdmmc mut Sdmmc> {
@@ -1019,7 +1256,7 @@ impl<SDMMC> IoBase for &mut FatFsCursor<SDMMC> {
 
 impl<SDMMC> Seek for FatFsCursor<SDMMC>
 where
-    SDMMC: AsMut<Sdmmc>,
+    SDMMC: SdmmcIo,
 {
     fn seek(&mut self, pos: SeekFrom) -> Result<u64, Self::Error> {
         // TODO: Use `checked_add_signed` when stable.
@@ -1046,7 +1283,7 @@ where
 #[cfg(feature = "fatfs")]
 impl<SDMMC> Seek for &mut FatFsCursor<SDMMC>
 where
-    SDMMC: AsMut<Sdmmc>,
+    SDMMC: SdmmcIo,
 {
     fn seek(&mut self, pos: SeekFrom) -> Result<u64, Self::Error> {
         (*self).seek(pos)
@@ -1056,7 +1293,7 @@ where
 #[cfg(feature = "fatfs")]
 impl<SDMMC> Read for FatFsCursor<SDMMC>
 where
-    SDMMC: AsMut<Sdmmc>,
+    SDMMC: SdmmcIo,
 {
     fn read(&mut self, buf: &mut [u8]) -> Result<usize, Self::Error> {
         let (start, end) = self.partition_info()?;
@@ -1074,11 +1311,11 @@ where
 
         // Only read the block if we have not already read it.
         if self.current_block != Some(addr) {
-            self.sdmmc.as_mut().read_block(addr, &mut self.block)?;
+            self.sdmmc.read_block(addr, &mut self.block)?;
             self.current_block = Some(addr);
         }
 
-        buf[0..len].copy_from_slice(&self.block[offset..(offset + len)]);
+        buf[0..len].copy_from_slice(&self.block.0[offset..(offset + len)]);
 
         self.pos += len as u64;
 
@@ -1091,7 +1328,7 @@ where
 #[cfg(feature = "fatfs")]
 impl<SDMMC> Read for &mut FatFsCursor<SDMMC>
 where
-    SDMMC: AsMut<Sdmmc>,
+    SDMMC: SdmmcIo,
 {
     fn read(&mut self, buf: &mut [u8]) -> Result<usize, Self::Error> {
         (*self).read(buf)
@@ -1101,7 +1338,7 @@ where
 #[cfg(feature = "fatfs")]
 impl<SDMMC> Write for FatFsCursor<SDMMC>
 where
-    SDMMC: AsMut<Sdmmc>,
+    SDMMC: SdmmcIo,
 {
     fn write(&mut self, buf: &[u8]) -> Result<usize, Self::Error> {
         let (start, end) = self.partition_info()?;
@@ -1120,11 +1357,11 @@ where
         // Only read the block if we have not already read it.
         if self.current_block != Some(addr) && len != 512 {
             self.current_block = None;
-            self.sdmmc.as_mut().read_block(addr, &mut self.block)?;
+            self.sdmmc.read_block(addr, &mut self.block)?;
         }
 
-        self.block[offset..(offset + len)].copy_from_slice(&buf[0..len]);
-        self.sdmmc.as_mut().write_block(addr, &self.block)?;
+        self.block.0[offset..(offset + len)].copy_from_slice(&buf[0..len]);
+        self.sdmmc.write_block(addr, &self.block)?;
         self.current_block = Some(addr);
 
         self.pos += len as u64;
@@ -1142,7 +1379,7 @@ where
 #[cfg(feature = "fatfs")]
 impl<SDMMC> Write for &mut FatFsCursor<SDMMC>
 where
-    SDMMC: AsMut<Sdmmc>,
+    SDMMC: SdmmcIo,
 {
     fn write(&mut self, buf: &[u8]) -> Result<usize, Self::Error> {
         (*self).write(buf)

From 6c861b466c36a35099073e8d430e008b296d2df4 Mon Sep 17 00:00:00 2001
From: Markus Reiter <me@reitermark.us>
Date: Thu, 26 May 2022 19:27:12 +0200
Subject: [PATCH 19/23] Fix clock assertion.

---
 src/rcc.rs | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/rcc.rs b/src/rcc.rs
index 151389da..31c946cb 100644
--- a/src/rcc.rs
+++ b/src/rcc.rs
@@ -754,7 +754,7 @@ impl CFGR {
             pll48m1clk = Some((vco / q).Hz());
 
             if self.clk48_source == Some(Clk48Source::Pll) {
-                assert_eq!(q, 48_000_000);
+                assert_eq!(pll48m1clk, 48_000_000);
             }
 
             assert!(r <= 8); // Allowed max output divider

From 72b1597eeb5cc7bc0d27e50d0a6e791a2e492b16 Mon Sep 17 00:00:00 2001
From: Markus Reiter <me@reitermark.us>
Date: Thu, 26 May 2022 20:16:30 +0200
Subject: [PATCH 20/23] Fix PLL 48 MHz clock config.

---
 src/rcc.rs | 31 +++++++++++++++++++++----------
 1 file changed, 21 insertions(+), 10 deletions(-)

diff --git a/src/rcc.rs b/src/rcc.rs
index 31c946cb..e45c2ae9 100644
--- a/src/rcc.rs
+++ b/src/rcc.rs
@@ -750,16 +750,24 @@ impl CFGR {
             let vco = clock_speed * pllconf.n as u32;
             let pllclk = vco / r;
 
-            let q = (vco + 48_000_000 - 1) / 48_000_000;
-            pll48m1clk = Some((vco / q).Hz());
-
-            if self.clk48_source == Some(Clk48Source::Pll) {
+            let q;
+            (q, pll48m1clk) = if self.clk48_source == Some(Clk48Source::Pll) {
+                let q = match (vco + 48_000_000 - 1) / 48_000_000 {
+                    0..=2 => PllDivider::Div2,
+                    3..=4 => PllDivider::Div4,
+                    5..=6 => PllDivider::Div6,
+                    7.. => PllDivider::Div8,
+                };
+
+                let pll48m1clk = vco / q.to_division_factor();
+                // TODO: Assert with tolerance.
                 assert_eq!(pll48m1clk, 48_000_000);
-            }
 
-            assert!(r <= 8); // Allowed max output divider
-            assert!(pllconf.n >= 8); // Allowed min multiplier
-            assert!(pllconf.n <= 86); // Allowed max multiplier
+                (Some(q), Some(pll48m1clk.Hz()))
+            } else {
+                (None, None)
+            };
+
             assert!(clock_speed >= 4_000_000); // VCO input clock min
             assert!(clock_speed <= 16_000_000); // VCO input clock max
             assert!(vco >= 64_000_000); // VCO output min
@@ -783,7 +791,9 @@ impl CFGR {
                     .plln()
                     .bits(pllconf.n)
                     .pllq()
-                    .bits(q as u8)
+                    .bits(q.unwrap_or(PllDivider::Div2).to_bits())
+                    .pllqen()
+                    .bit(q.is_some())
             });
 
             rcc.cr.modify(|_, w| w.pllon().set_bit());
@@ -904,7 +914,8 @@ impl PllConfig {
     ///
     /// PLL output = ((SourceClk / input_divider) * multiplier) / output_divider
     pub fn new(input_divider: u8, multiplier: u8, output_divider: PllDivider) -> Self {
-        assert!(input_divider > 0);
+        assert!(input_divider >= 1 && input_divider <= 8);
+        assert!(multiplier >= 8 && multiplier <= 86);
 
         PllConfig {
             m: input_divider - 1,

From e117686347387b14335f20dc1bb69e1cdad97ff0 Mon Sep 17 00:00:00 2001
From: Markus Reiter <me@reitermark.us>
Date: Sun, 29 May 2022 18:35:49 +0200
Subject: [PATCH 21/23] Improve `Sdmmc` software timeout errors.

---
 src/sdmmc.rs | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/sdmmc.rs b/src/sdmmc.rs
index 5f871caf..a92bd34b 100644
--- a/src/sdmmc.rs
+++ b/src/sdmmc.rs
@@ -164,7 +164,7 @@ pub enum Error {
     /// Timeout in hardware.
     Timeout,
     /// Timeout in software.
-    SoftwareTimeout,
+    SoftwareTimeout(&'static str),
     /// Receive FIFO overrun.
     RxOverrun,
     /// Transmit FIFO underrun.
@@ -180,7 +180,7 @@ impl fmt::Display for Error {
             Self::CommandCrc => "command CRC check failed",
             Self::DataCrc => "data block CRC check failed",
             Self::Timeout => "timeout",
-            Self::SoftwareTimeout => "software timeout",
+            Self::SoftwareTimeout(op) => return write!(f, "software timeout during {}", op),
             Self::RxOverrun => "receive FIFO overrun",
             Self::TxUnderrun => "transmit FIFO underrun",
             Self::InvalidInput => "invalid input",
@@ -403,7 +403,7 @@ impl Sdmmc {
         let mut timeout = 0xffff;
         card.ocr = loop {
             if timeout == 0 {
-                return Err(Error::SoftwareTimeout);
+                return Err(Error::SoftwareTimeout("init"));
             }
 
             timeout -= 1;
@@ -530,7 +530,7 @@ impl Sdmmc {
         }
 
         clear_static_data_flags(&self.sdmmc.icr);
-        Err(Error::SoftwareTimeout)
+        Err(Error::SoftwareTimeout("read_blocks"))
     }
 
     #[inline]
@@ -611,7 +611,7 @@ impl Sdmmc {
             }
         }
 
-        Err(Error::SoftwareTimeout)
+        Err(Error::SoftwareTimeout("write_blocks"))
     }
 
     #[inline]
@@ -718,7 +718,7 @@ impl Sdmmc {
             }
         }
 
-        return Err(Error::SoftwareTimeout);
+        return Err(Error::SoftwareTimeout("get_scr"));
     }
 
     fn power_card(&mut self, on: bool) {
@@ -775,7 +775,7 @@ impl Sdmmc {
             }
         }
 
-        Err(Error::SoftwareTimeout)
+        Err(Error::SoftwareTimeout("read_sd_status"))
     }
 
     /// Get the initialized card, if present.
@@ -828,7 +828,7 @@ impl Sdmmc {
         });
 
         let timeout = 5000 * (self.clock.raw() / 8 / 1000);
-        let mut res = Err(Error::SoftwareTimeout);
+        let mut res = Err(Error::SoftwareTimeout("cmd"));
         for _ in 0..timeout {
             let sta = self.sdmmc.sta.read();
 
@@ -1035,7 +1035,7 @@ impl SdmmcDma {
             }
         }
 
-        Err(Error::SoftwareTimeout)
+        Err(Error::SoftwareTimeout("write_blocks"))
     }
 
     #[inline]

From 870a8aa4f89de42c687e404a9aab30109e997c89 Mon Sep 17 00:00:00 2001
From: Markus Reiter <me@reitermark.us>
Date: Sat, 11 Jun 2022 04:33:03 +0200
Subject: [PATCH 22/23] Add `block_count` method.

---
 src/sdmmc.rs | 72 ++++++++++++++++++++++------------------------------
 1 file changed, 31 insertions(+), 41 deletions(-)

diff --git a/src/sdmmc.rs b/src/sdmmc.rs
index a92bd34b..d0fcf75e 100644
--- a/src/sdmmc.rs
+++ b/src/sdmmc.rs
@@ -302,6 +302,10 @@ impl SdCard {
         self.rca.address()
     }
 
+    pub fn block_count(&self) -> u64 {
+        self.csd.block_count()
+    }
+
     /// Get the card size in bytes.
     pub fn size(&self) -> u64 {
         self.csd.card_size()
@@ -874,20 +878,28 @@ impl Sdmmc {
 
 #[derive(Debug, Clone)]
 #[repr(align(4))]
-pub struct DataBlock(pub [u8; 512]);
+pub struct DataBlock([u8; 512]);
 
 impl DataBlock {
     pub const fn new() -> Self {
         Self([0; 512])
     }
 
-    pub fn blocks_to_words(blocks: &[DataBlock]) -> &[u32] {
+    pub fn as_bytes(&self) -> &[u8] {
+        &self.0
+    }
+
+    pub fn as_bytes_mut(&mut self) -> &mut [u8] {
+        &mut self.0
+    }
+
+    pub(crate) fn blocks_to_words(blocks: &[DataBlock]) -> &[u32] {
         let mut word_count = blocks.len() * 128;
         // SAFETY: `DataBlock` is 4-byte aligned.
         unsafe { slice::from_raw_parts(blocks.as_ptr() as *mut u32, word_count) }
     }
 
-    pub fn blocks_to_words_mut(blocks: &mut [DataBlock]) -> &mut [u32] {
+    pub(crate) fn blocks_to_words_mut(blocks: &mut [DataBlock]) -> &mut [u32] {
         let mut word_count = blocks.len() * 128;
         // SAFETY: `DataBlock` is 4-byte aligned.
         unsafe { slice::from_raw_parts_mut(blocks.as_mut_ptr() as *mut u32, word_count) }
@@ -1043,39 +1055,10 @@ impl SdmmcDma {
         (self.sdmmc, self.channel)
     }
 
+    /// Get the initialized card, if present.
     #[inline]
-    pub fn interrupt_handler() {
-        let sdmmc = unsafe { &*(SDMMC1::PTR) };
-
-        let sta = sdmmc.sta.read();
-
-        log::info!("irq sta = {:024b}", sta.bits());
-
-        if sta.dbckend().bit_is_set()
-            || sta.dcrcfail().bit_is_set()
-            || sta.dtimeout().bit_is_set()
-            || sta.rxoverr().bit_is_set()
-            || sta.txunderr().bit_is_set()
-        {
-            sdmmc.mask.modify(|_, w| {
-                w.dbckendie()
-                    .clear_bit()
-                    .dataendie()
-                    .clear_bit()
-                    .dcrcfailie()
-                    .clear_bit()
-                    .dtimeoutie()
-                    .clear_bit()
-                    .txunderrie()
-                    .clear_bit()
-                    .rxoverrie()
-                    .clear_bit()
-                    .txfifoheie()
-                    .clear_bit()
-                    .rxfifohfie()
-                    .clear_bit()
-            });
-        }
+    pub fn card(&self) -> Result<&SdCard, Error> {
+        self.sdmmc.card()
     }
 }
 
@@ -1123,9 +1106,8 @@ impl embedded_sdmmc::BlockDevice for SdmmcBlockDevice<Sdmmc> {
 
     fn num_blocks(&self) -> Result<embedded_sdmmc::BlockCount, Self::Error> {
         let sdmmc = self.sdmmc.borrow_mut();
-        Ok(embedded_sdmmc::BlockCount(
-            (sdmmc.card()?.size() / 512u64) as u32,
-        ))
+        let block_count = sdmmc.card()?.block_count();
+        Ok(embedded_sdmmc::BlockCount(block_count as u32))
     }
 }
 
@@ -1165,6 +1147,14 @@ impl<SDMMC> FatFsCursor<SDMMC> {
             current_block: None,
         }
     }
+
+    pub fn sdmmc(&mut self) -> &SDMMC {
+        &self.sdmmc
+    }
+
+    pub fn sdmmc_mut(&mut self) -> &mut SDMMC {
+        &mut self.sdmmc
+    }
 }
 
 pub trait SdmmcIo {
@@ -1218,7 +1208,7 @@ where
         self.sdmmc.read_block(0, &mut block)?;
 
         // TODO: Support other partitions.
-        let partition1_info = &block.0[446..][..16];
+        let partition1_info = &block.as_bytes()[446..][..16];
         let lba_start = u32::from_le_bytes([
             partition1_info[8],
             partition1_info[9],
@@ -1315,7 +1305,7 @@ where
             self.current_block = Some(addr);
         }
 
-        buf[0..len].copy_from_slice(&self.block.0[offset..(offset + len)]);
+        buf[0..len].copy_from_slice(&self.block.as_bytes()[offset..(offset + len)]);
 
         self.pos += len as u64;
 
@@ -1360,7 +1350,7 @@ where
             self.sdmmc.read_block(addr, &mut self.block)?;
         }
 
-        self.block.0[offset..(offset + len)].copy_from_slice(&buf[0..len]);
+        self.block.as_bytes_mut()[offset..(offset + len)].copy_from_slice(&buf[0..len]);
         self.sdmmc.write_block(addr, &self.block)?;
         self.current_block = Some(addr);
 

From 717f1d3241823599f74d35f0418ee11a313de366 Mon Sep 17 00:00:00 2001
From: Markus Reiter <me@reitermark.us>
Date: Sat, 11 Jun 2022 04:37:07 +0200
Subject: [PATCH 23/23] Improve SD write performance.

---
 src/sdmmc.rs | 147 ++++++++++++++++++++++++++++++++++++---------------
 1 file changed, 105 insertions(+), 42 deletions(-)

diff --git a/src/sdmmc.rs b/src/sdmmc.rs
index d0fcf75e..50cdf07f 100644
--- a/src/sdmmc.rs
+++ b/src/sdmmc.rs
@@ -5,7 +5,7 @@
 use core::{
     fmt,
     ops::{ControlFlow, Deref, DerefMut},
-    slice,
+    ptr, slice,
     sync::atomic::{self, Ordering},
 };
 
@@ -894,13 +894,13 @@ impl DataBlock {
     }
 
     pub(crate) fn blocks_to_words(blocks: &[DataBlock]) -> &[u32] {
-        let mut word_count = blocks.len() * 128;
+        let word_count = blocks.len() * 128;
         // SAFETY: `DataBlock` is 4-byte aligned.
         unsafe { slice::from_raw_parts(blocks.as_ptr() as *mut u32, word_count) }
     }
 
     pub(crate) fn blocks_to_words_mut(blocks: &mut [DataBlock]) -> &mut [u32] {
-        let mut word_count = blocks.len() * 128;
+        let word_count = blocks.len() * 128;
         // SAFETY: `DataBlock` is 4-byte aligned.
         unsafe { slice::from_raw_parts_mut(blocks.as_mut_ptr() as *mut u32, word_count) }
     }
@@ -1128,13 +1128,40 @@ impl IoError for Error {
     }
 }
 
+#[derive(Debug, Clone, Copy)]
+pub struct PartitionInfo {
+    start: u32,
+    len_bytes: u64,
+}
+
+impl PartitionInfo {
+    pub const fn new(lba_start: u32, num_blocks: u32) -> Self {
+        Self {
+            start: lba_start,
+            len_bytes: num_blocks as u64 * 512,
+        }
+    }
+
+    #[inline]
+    pub const fn start(&self) -> u32 {
+        self.start
+    }
+
+    #[inline]
+    pub const fn len_bytes(&self) -> u64 {
+        self.len_bytes
+    }
+}
+
 #[derive(Debug)]
 pub struct FatFsCursor<SDMMC> {
     sdmmc: SDMMC,
     pos: u64,
-    partition_info: Option<(u32, u32)>,
+    partition_index: u8,
+    partition_info: Option<PartitionInfo>,
     block: DataBlock,
     current_block: Option<u32>,
+    dirty: bool,
 }
 
 impl<SDMMC> FatFsCursor<SDMMC> {
@@ -1142,9 +1169,11 @@ impl<SDMMC> FatFsCursor<SDMMC> {
         Self {
             sdmmc,
             pos: 0,
+            partition_index: 0,
             partition_info: None,
             block: DataBlock([0; 512]),
             current_block: None,
+            dirty: false,
         }
     }
 
@@ -1163,20 +1192,24 @@ pub trait SdmmcIo {
 }
 
 impl SdmmcIo for Sdmmc {
+    #[inline(always)]
     fn read_block(&mut self, addr: u32, block: &mut DataBlock) -> Result<(), Error> {
         Self::read_block(self, addr, block)
     }
 
+    #[inline(always)]
     fn write_block(&mut self, addr: u32, block: &DataBlock) -> Result<(), Error> {
         Self::write_block(self, addr, block)
     }
 }
 
 impl SdmmcIo for SdmmcDma {
+    #[inline(always)]
     fn read_block(&mut self, addr: u32, block: &mut DataBlock) -> Result<(), Error> {
         Self::read_block(self, addr, block)
     }
 
+    #[inline(always)]
     fn write_block(&mut self, addr: u32, block: &DataBlock) -> Result<(), Error> {
         Self::write_block(self, addr, block)
     }
@@ -1186,10 +1219,12 @@ impl<T> SdmmcIo for &mut T
 where
     T: SdmmcIo,
 {
+    #[inline(always)]
     fn read_block(&mut self, addr: u32, block: &mut DataBlock) -> Result<(), Error> {
         (*self).read_block(addr, block)
     }
 
+    #[inline(always)]
     fn write_block(&mut self, addr: u32, block: &DataBlock) -> Result<(), Error> {
         (*self).write_block(addr, block)
     }
@@ -1199,7 +1234,7 @@ impl<SDMMC> FatFsCursor<SDMMC>
 where
     SDMMC: SdmmcIo,
 {
-    pub fn partition_info(&mut self) -> Result<(u32, u32), Error> {
+    pub fn partition_info(&mut self) -> Result<PartitionInfo, Error> {
         if let Some(partition_info) = self.partition_info {
             return Ok(partition_info);
         }
@@ -1207,28 +1242,31 @@ where
         let mut block = DataBlock([0; 512]);
         self.sdmmc.read_block(0, &mut block)?;
 
-        // TODO: Support other partitions.
-        let partition1_info = &block.as_bytes()[446..][..16];
+        let start = self.partition_index as usize * 16;
+        let partition_info = &block.0[446..][start..(start + 16)];
         let lba_start = u32::from_le_bytes([
-            partition1_info[8],
-            partition1_info[9],
-            partition1_info[10],
-            partition1_info[11],
+            partition_info[8],
+            partition_info[9],
+            partition_info[10],
+            partition_info[11],
         ]);
 
         let num_blocks = u32::from_le_bytes([
-            partition1_info[12],
-            partition1_info[13],
-            partition1_info[14],
-            partition1_info[15],
+            partition_info[12],
+            partition_info[13],
+            partition_info[14],
+            partition_info[15],
         ]);
 
-        Ok(*self.partition_info.get_or_insert((lba_start, num_blocks)))
+        Ok(*self
+            .partition_info
+            .get_or_insert(PartitionInfo::new(lba_start, num_blocks)))
     }
 }
 
 #[cfg(feature = "fatfs")]
 impl<'sdmmc> IntoStorage<FatFsCursor<&'sdmmc mut Sdmmc>> for &'sdmmc mut Sdmmc {
+    #[inline]
     fn into_storage(self) -> FatFsCursor<&'sdmmc mut Sdmmc> {
         FatFsCursor::new(self)
     }
@@ -1249,20 +1287,24 @@ where
     SDMMC: SdmmcIo,
 {
     fn seek(&mut self, pos: SeekFrom) -> Result<u64, Self::Error> {
-        // TODO: Use `checked_add_signed` when stable.
+        // TODO: Replace when `u64::checked_add_signed(i64)` becomes stable.
+        let checked_add_signed = |v: u64, o: i64| -> Result<u64, Self::Error> {
+            if o.is_negative() {
+                v.checked_sub(o.abs() as u64)
+            } else {
+                v.checked_add(o as u64)
+            }
+            .ok_or(Self::Error::InvalidInput)
+        };
         let new_pos = match pos {
-            SeekFrom::Start(offset) => offset as i128,
+            SeekFrom::Start(offset) => Ok(offset),
             SeekFrom::End(offset) => {
-                let end = self.partition_info()?.1 * 512;
-                end as i128 + offset as i128
+                let end = self.partition_info()?.len_bytes();
+                checked_add_signed(end as u64, offset)
             }
-            SeekFrom::Current(offset) => self.pos as i128 + offset as i128,
-        };
+            SeekFrom::Current(offset) => checked_add_signed(self.pos, offset),
+        }?;
 
-        if new_pos < 0 || new_pos > u64::MAX as i128 {
-            // Seek to negative or overflowing position.
-            return Err(Self::Error::InvalidInput);
-        }
         let new_pos = new_pos as u64;
 
         self.pos = new_pos;
@@ -1275,6 +1317,7 @@ impl<SDMMC> Seek for &mut FatFsCursor<SDMMC>
 where
     SDMMC: SdmmcIo,
 {
+    #[inline(always)]
     fn seek(&mut self, pos: SeekFrom) -> Result<u64, Self::Error> {
         (*self).seek(pos)
     }
@@ -1286,12 +1329,10 @@ where
     SDMMC: SdmmcIo,
 {
     fn read(&mut self, buf: &mut [u8]) -> Result<usize, Self::Error> {
-        let (start, end) = self.partition_info()?;
+        let PartitionInfo { start, len_bytes } = self.partition_info()?;
 
-        let end = end as u64 * 512;
         let pos = self.pos;
-
-        if pos >= end {
+        if pos >= len_bytes {
             return Ok(0);
         }
 
@@ -1301,11 +1342,16 @@ where
 
         // Only read the block if we have not already read it.
         if self.current_block != Some(addr) {
+            self.flush()?;
+            self.current_block = None;
             self.sdmmc.read_block(addr, &mut self.block)?;
             self.current_block = Some(addr);
         }
 
-        buf[0..len].copy_from_slice(&self.block.as_bytes()[offset..(offset + len)]);
+        // SAFETY: `offset` and `len` are ensured to fit within the slices.
+        unsafe {
+            ptr::copy_nonoverlapping(self.block.0.as_ptr().add(offset), buf.as_mut_ptr(), len);
+        }
 
         self.pos += len as u64;
 
@@ -1320,6 +1366,7 @@ impl<SDMMC> Read for &mut FatFsCursor<SDMMC>
 where
     SDMMC: SdmmcIo,
 {
+    #[inline(always)]
     fn read(&mut self, buf: &mut [u8]) -> Result<usize, Self::Error> {
         (*self).read(buf)
     }
@@ -1331,12 +1378,10 @@ where
     SDMMC: SdmmcIo,
 {
     fn write(&mut self, buf: &[u8]) -> Result<usize, Self::Error> {
-        let (start, end) = self.partition_info()?;
+        let PartitionInfo { start, len_bytes } = self.partition_info()?;
 
-        let end = end as u64 * 512;
         let pos = self.pos;
-
-        if pos >= end {
+        if pos >= len_bytes {
             return Ok(0);
         }
 
@@ -1344,16 +1389,25 @@ where
         let offset = (pos % 512) as usize;
         let len = buf.len().min(512 - offset);
 
-        // Only read the block if we have not already read it.
-        if self.current_block != Some(addr) && len != 512 {
-            self.current_block = None;
-            self.sdmmc.read_block(addr, &mut self.block)?;
+        // Flush if current block is not the one we're writing to.
+        if self.current_block != Some(addr) {
+            self.flush()?;
+
+            // Read the block unless we write the full block.
+            if len != 512 {
+                self.current_block = None;
+                self.sdmmc.read_block(addr, &mut self.block)?;
+            }
+
+            self.current_block = Some(addr);
         }
 
-        self.block.as_bytes_mut()[offset..(offset + len)].copy_from_slice(&buf[0..len]);
-        self.sdmmc.write_block(addr, &self.block)?;
-        self.current_block = Some(addr);
+        // SAFETY: `offset` and `len` are ensured to fit within the slices.
+        unsafe {
+            ptr::copy_nonoverlapping(buf.as_ptr(), self.block.0.as_mut_ptr().add(offset), len);
+        }
 
+        self.dirty = true;
         self.pos += len as u64;
 
         Ok(len)
@@ -1361,7 +1415,14 @@ where
 
     // TODO: Add `write_exact` implementation which supports writing multiple blocks.
 
+    #[inline]
     fn flush(&mut self) -> Result<(), Self::Error> {
+        if self.dirty {
+            self.sdmmc
+                .write_block(self.current_block.unwrap(), &self.block)?;
+            self.dirty = false;
+        }
+
         Ok(())
     }
 }
@@ -1371,10 +1432,12 @@ impl<SDMMC> Write for &mut FatFsCursor<SDMMC>
 where
     SDMMC: SdmmcIo,
 {
+    #[inline(always)]
     fn write(&mut self, buf: &[u8]) -> Result<usize, Self::Error> {
         (*self).write(buf)
     }
 
+    #[inline(always)]
     fn flush(&mut self) -> Result<(), Self::Error> {
         (*self).flush()
     }