1 files changed, 677 insertions, 173 deletions

diff --git a/examples/stm32h7rs/src/bin/xspi_memory_mapped.rs b/examples/stm32h7rs/src/bin/xspi_memory_mapped.rs
index 88d914180..59045ca2e 100644
--- a/examples/stm32h7rs/src/bin/xspi_memory_mapped.rs
+++ b/examples/stm32h7rs/src/bin/xspi_memory_mapped.rs
@@ -3,7 +3,8 @@
 
 //! For Nucleo STM32H7S3L8 MB1737, has MX25UW25645GXDI00
 //!
-//! TODO: Currently this only uses single SPI, pending flash chip documentation for octo SPI.
+
+use core::cmp::min;
 
 use defmt::info;
 use embassy_executor::Spawner;
@@ -52,14 +53,16 @@ async fn main(_spawner: Spawner) {
         fifo_threshold: FIFOThresholdLevel::_4Bytes,
         memory_type: MemoryType::Macronix,
         delay_hold_quarter_cycle: true,
-        // memory_type: MemoryType::Micron,
-        // delay_hold_quarter_cycle: false,
         device_size: MemorySize::_32MiB,
         chip_select_high_time: ChipSelectHighTime::_2Cycle,
         free_running_clock: false,
         clock_mode: false,
         wrap_size: WrapSize::None,
-        // 300mhz / (4+1) = 60mhz. Unsure the limit, need to find a MX25UW25645GXDI00 datasheet.
+        // 300 MHz clock / (3 + 1) = 75 MHz. This is above the max for READ instructions so the
+        // FAST READ must be used. The nucleo board's flash  can run at up to 133 MHz in SPI mode
+        // and 200 MHz in OPI mode. This clock prescaler must be even otherwise the clock will not
+        // have symmetric high and low times.
+        // The clock can also be fed by one of the PLLs to allow for more flexible clock rates.
         clock_prescaler: 3,
         sample_shifting: false,
         chip_select_boundary: 0,
@@ -71,28 +74,41 @@ async fn main(_spawner: Spawner) {
 
     // Not necessary, but recommended if using XIP
     cor.SCB.enable_icache();
+    // Note: Enabling data cache can cause issues with DMA transfers.
     cor.SCB.enable_dcache(&mut cor.CPUID);
 
     let xspi = embassy_stm32::xspi::Xspi::new_blocking_xspi(
         p.XSPI2, p.PN6, p.PN2, p.PN3, p.PN4, p.PN5, p.PN8, p.PN9, p.PN10, p.PN11, p.PN1, spi_config,
     );
 
-    let mut flash = FlashMemory::new(xspi).await;
+    let mut flash = SpiFlashMemory::new(xspi);
 
     let flash_id = flash.read_id();
     info!("FLASH ID: {=[u8]:x}", flash_id);
 
-    let mut wr_buf = [0u8; 8];
-    for i in 0..8 {
-        wr_buf[i] = 0x90 + i as u8;
-    }
-    let mut rd_buf = [0u8; 8];
-    flash.erase_sector(0).await;
-    flash.write_memory(0, &wr_buf, true).await;
-    flash.read_memory(0, &mut rd_buf, true);
-    info!("WRITE BUF: {=[u8]:#X}", wr_buf);
-    info!("READ BUF: {=[u8]:#X}", rd_buf);
-    flash.enable_mm().await;
+    // Erase the first sector
+    flash.erase_sector(0);
+
+    // Write some data into the flash. This writes more than one page to test that functionality.
+    let mut wr_buf = [0u8; 512];
+    let base_number: u8 = 0x90;
+    for i in 0..512 {
+        wr_buf[i] = base_number.wrapping_add(i as u8);
+    }
+    flash.write_memory(0, &wr_buf);
+
+    // Read the data back and verify it.
+    let mut rd_buf = [0u8; 512];
+    let start_time = embassy_time::Instant::now();
+    flash.read_memory(0, &mut rd_buf);
+    let elapsed = start_time.elapsed();
+    info!("Read 512 bytes in {} us in SPI mode", elapsed.as_micros());
+    info!("WRITE BUF: {=[u8]:#X}", wr_buf[0..32]);
+    info!("READ BUF: {=[u8]:#X}", rd_buf[0..32]);
+
+    assert_eq!(wr_buf, rd_buf, "Read buffer does not match write buffer");
+
+    flash.enable_mm();
     info!("Enabled memory mapped mode");
 
     let first_u32 = unsafe { *(0x70000000 as *const u32) };
@@ -103,10 +119,53 @@ async fn main(_spawner: Spawner) {
     assert_eq!(second_u32, 0x97969594);
     info!("second_u32 {:08x}", first_u32);
 
-    flash.disable_mm().await;
+    flash.disable_mm();
     info!("Disabled memory mapped mode");
 
+    let flash_id = flash.read_id();
+    info!("FLASH ID: {=[u8]:x}", flash_id);
+
+    let mut flash = flash.into_octo();
+
+    Timer::after_millis(100).await;
+
+    let flash_id = flash.read_id();
+    info!("FLASH ID in OPI mode: {=[u8]:x}", flash_id);
+
+    flash.erase_sector(0);
+
+    let mut rd_buf = [0u8; 512];
+    flash.read_memory(0, &mut rd_buf);
+    info!("READ BUF after erase: {=[u8]:#X}", rd_buf[0..32]);
+
+    assert_eq!(rd_buf, [0xFF; 512], "Read buffer is not all 0xFF after erase");
+
+    flash.write_memory(0, &wr_buf);
+    let start = embassy_time::Instant::now();
+    flash.read_memory(0, &mut rd_buf);
+    let elapsed = start.elapsed();
+    info!("Read 512 bytes in {} us in OPI mode", elapsed.as_micros());
+    info!("READ BUF after write: {=[u8]:#X}", rd_buf[0..32]);
+    assert_eq!(wr_buf, rd_buf, "Read buffer does not match write buffer in OPI mode");
+
+    flash.enable_mm();
+    info!("Enabled memory mapped mode in OPI mode");
+    let first_u32 = unsafe { *(0x70000000 as *const u32) };
+    assert_eq!(first_u32, 0x93929190);
+    info!("first_u32 {:08x}", first_u32);
+    let second_u32 = unsafe { *(0x70000004 as *const u32) };
+    assert_eq!(second_u32, 0x97969594);
+    info!("second_u32 {:08x}", first_u32);
+    flash.disable_mm();
+    info!("Disabled memory mapped mode in OPI mode");
+
+    // Reset back to SPI mode
+    let mut flash = flash.into_spi();
+    let flash_id = flash.read_id();
+    info!("FLASH ID back in SPI mode: {=[u8]:x}", flash_id);
+
     info!("DONE");
+
     // Output pin PE3
     let mut led = Output::new(p.PE3, Level::Low, Speed::Low);
 
@@ -116,80 +175,268 @@ async fn main(_spawner: Spawner) {
     }
 }
 
-const MEMORY_PAGE_SIZE: usize = 8;
-
-const CMD_READ: u8 = 0x0B;
-const _CMD_QUAD_READ: u8 = 0x6B;
-
-const CMD_WRITE_PG: u8 = 0x02;
-const _CMD_QUAD_WRITE_PG: u8 = 0x32;
-
-const CMD_READ_ID: u8 = 0x9F;
-const CMD_READ_ID_OCTO: u16 = 0x9F60;
+const MEMORY_PAGE_SIZE: usize = 256;
 
-const CMD_ENABLE_RESET: u8 = 0x66;
-const CMD_RESET: u8 = 0x99;
-
-const CMD_WRITE_ENABLE: u8 = 0x06;
-
-const CMD_CHIP_ERASE: u8 = 0xC7;
-const CMD_SECTOR_ERASE: u8 = 0x20;
-const CMD_BLOCK_ERASE_32K: u8 = 0x52;
-const CMD_BLOCK_ERASE_64K: u8 = 0xD8;
-
-const CMD_READ_SR: u8 = 0x05;
-const CMD_READ_CR: u8 = 0x35;
-
-const CMD_WRITE_SR: u8 = 0x01;
-const CMD_WRITE_CR: u8 = 0x31;
+/// Implementation of access to flash chip using SPI.
+///
+/// Chip commands are hardcoded as it depends on used chip.
+/// This targets a MX25UW25645GXDI00.
+pub struct SpiFlashMemory<I: Instance> {
+    xspi: Xspi<'static, I, Blocking>,
+}
 
-/// Implementation of access to flash chip.
+/// Implementation of access to flash chip using Octo SPI.
 ///
 /// Chip commands are hardcoded as it depends on used chip.
 /// This targets a MX25UW25645GXDI00.
-pub struct FlashMemory<I: Instance> {
+pub struct OpiFlashMemory<I: Instance> {
     xspi: Xspi<'static, I, Blocking>,
 }
 
-impl<I: Instance> FlashMemory<I> {
-    pub async fn new(xspi: Xspi<'static, I, Blocking>) -> Self {
-        let mut memory = Self { xspi };
+/// SPI mode commands for MX25UW25645G flash memory
+#[allow(dead_code)]
+#[repr(u8)]
+enum SpiCommand {
+    // Array access commands
+    /// Read data bytes using 3-byte address (up to 50 MHz)
+    Read3B = 0x03,
+    /// Fast read data bytes using 3-byte address with 8 dummy cycles (up to 133 MHz)
+    FastRead3B = 0x0B,
+    /// Program 1-256 bytes of data using 3-byte address
+    PageProgram3B = 0x02,
+    /// Erase 4KB sector using 3-byte address
+    SectorErase3B = 0x20,
+    /// Erase 64KB block using 3-byte address
+    BlockErase3B = 0xD8,
+    /// Read data bytes using 4-byte address (up to 50 MHz)
+    Read4B = 0x13,
+    /// Fast read data bytes using 4-byte address with 8 dummy cycles (up to 133 MHz)
+    FastRead4B = 0x0C,
+    /// Program 1-256 bytes of data using 4-byte address
+    PageProgram4B = 0x12,
+    /// Erase 4KB sector using 4-byte address
+    SectorErase4B = 0x21,
+    /// Erase 64KB block using 4-byte address
+    BlockErase4B = 0xDC,
+    /// Erase entire chip (only if no blocks are protected)
+    ChipErase = 0x60,
+
+    // Write Buffer Access commands
+    /// Read data from the 256-byte page buffer
+    ReadBuffer = 0x25,
+    /// Initialize write-to-buffer sequence, clears buffer and writes initial data
+    WriteBufferInitial = 0x22,
+    /// Continue writing data to buffer (used between WRBI and WRCF)
+    WriteBufferContinue = 0x24,
+    /// Confirm write operation, programs buffer contents to flash array
+    WriteBufferConfirm = 0x31,
+
+    // Device operation commands
+    /// Set Write Enable Latch (WEL) bit, required before write/program/erase operations
+    WriteEnable = 0x06,
+    /// Clear Write Enable Latch (WEL) bit
+    WriteDisable = 0x04,
+    /// Select write protection mode (BP mode or Advanced Sector Protection)
+    WriteProtectSelection = 0x68,
+    /// Suspend ongoing program or erase operation to allow read access
+    ProgramEraseSuspend = 0xB0,
+    /// Resume suspended program or erase operation
+    ProgramEraseResume = 0x30,
+    /// Enter deep power-down mode for minimum power consumption
+    DeepPowerDown = 0xB9,
+    /// Exit deep power-down mode and return to standby
+    ReleaseFromDeepPowerDown = 0xAB,
+    /// No operation, can terminate Reset Enable command
+    NoOperation = 0x00,
+    /// Enable reset operation (must precede Reset Memory command)
+    ResetEnable = 0x66,
+    /// Reset device to power-on state (requires prior Reset Enable)
+    ResetMemory = 0x99,
+    /// Protect all sectors using Dynamic Protection Bits (DPB)
+    GangBlockLock = 0x7E,
+    /// Unprotect all sectors by clearing Dynamic Protection Bits (DPB)
+    GangBlockUnlock = 0x98,
+
+    // Register Access commands
+    /// Read 3-byte device identification (manufacturer ID + device ID)
+    ReadIdentification = 0x9F,
+    /// Read Serial Flash Discoverable Parameters (SFDP) table
+    ReadSFDP = 0x5A,
+    /// Read 8-bit Status Register (WIP, WEL, BP bits, etc.)
+    ReadStatusRegister = 0x05,
+    /// Read 8-bit Configuration Register (ODS, TB, PBE bits)
+    ReadConfigurationRegister = 0x15,
+    /// Write Status and/or Configuration Register (1-2 bytes)
+    WriteStatusConfigurationRegister = 0x01,
+    /// Read Configuration Register 2 from specified 4-byte address
+    ReadConfigurationRegister2 = 0x71,
+    /// Write Configuration Register 2 to specified 4-byte address
+    WriteConfigurationRegister2 = 0x72,
+    /// Read 8-bit Security Register (protection status, suspend bits)
+    ReadSecurityRegister = 0x2B,
+    /// Write Security Register to set customer lock-down bit
+    WriteSecurityRegister = 0x2F,
+    /// Read 32-bit Fast Boot Register (boot address and configuration)
+    ReadFastBootRegister = 0x16,
+    /// Write 32-bit Fast Boot Register
+    WriteFastBootRegister = 0x17,
+    /// Erase Fast Boot Register (disable fast boot feature)
+    EraseFastBootRegister = 0x18,
+    /// Set burst/wrap length for read operations (16/32/64 bytes)
+    SetBurstLength = 0xC0,
+    /// Enter 8K-bit secured OTP mode for programming unique identifiers
+    EnterSecuredOTP = 0xB1,
+    /// Exit secured OTP mode and return to main array access
+    ExitSecuredOTP = 0xC1,
+    /// Write Lock Register to control SPB protection mode
+    WriteLockRegister = 0x2C,
+    /// Read Lock Register status
+    ReadLockRegister = 0x2D,
+    /// Program Solid Protection Bit (SPB) for specified sector/block
+    WriteSPB = 0xE3,
+    /// Erase all Solid Protection Bits (SPB)
+    EraseSPB = 0xE4,
+    /// Read Solid Protection Bit (SPB) status for specified sector/block
+    ReadSPB = 0xE2,
+    /// Write Dynamic Protection Bit (DPB) for specified sector
+    WriteDPB = 0xE1,
+    /// Read Dynamic Protection Bit (DPB) status for specified sector
+    ReadDPB = 0xE0,
+    /// Read 64-bit password register (only in Solid Protection mode)
+    ReadPassword = 0x27,
+    /// Write 64-bit password register
+    WritePassword = 0x28,
+    /// Unlock SPB operations using 64-bit password
+    PasswordUnlock = 0x29,
+}
 
-        memory.reset_memory().await;
-        memory.enable_octo();
-        memory
-    }
+/// OPI mode commands for MX25UW25645G flash memory
+#[allow(dead_code)]
+#[repr(u16)]
+enum OpiCommand {
+    // Array access commands
+    /// Read data using 8 I/O lines in STR mode with configurable dummy cycles (up to 200 MHz)
+    OctaRead = 0xEC13,
+    /// Read data using 8 I/O lines in DTR mode with configurable dummy cycles (up to 200 MHz)
+    OctaDTRRead = 0xEE11,
+    /// Program 1-256 bytes using 4-byte address and 8 I/O lines
+    PageProgram4B = 0x12ED,
+    /// Erase 4KB sector using 4-byte address
+    SectorErase4B = 0x21DE,
+    /// Erase 64KB block using 4-byte address
+    BlockErase4B = 0xDC23,
+    /// Erase entire chip (only if no blocks are protected)
+    ChipErase = 0x609F,
+
+    // Write Buffer Access commands
+    /// Read data from the 256-byte page buffer using 4-byte address
+    ReadBuffer = 0x25DA,
+    /// Initialize interruptible write-to-buffer sequence with 4-byte address
+    WriteBufferInitial = 0x22DD,
+    /// Continue writing data to buffer during interruptible sequence
+    WriteBufferContinue = 0x24DB,
+    /// Confirm and execute write operation from buffer to flash array
+    WriteBufferConfirm = 0x31CE,
+
+    // Device operation commands
+    /// Set Write Enable Latch (WEL) bit, required before write/program/erase operations
+    WriteEnable = 0x06F9,
+    /// Clear Write Enable Latch (WEL) bit, aborts write-to-buffer sequence
+    WriteDisable = 0x04FB,
+    /// Select write protection mode (BP mode or Advanced Sector Protection) - OTP bit
+    WriteProtectSelection = 0x6897,
+    /// Suspend ongoing program or erase operation to allow read from other banks
+    ProgramEraseSuspend = 0xB04F,
+    /// Resume suspended program or erase operation
+    ProgramEraseResume = 0x30CF,
+    /// Enter deep power-down mode for minimum power consumption
+    DeepPowerDown = 0xB946,
+    /// Exit deep power-down mode and return to standby
+    ReleaseFromDeepPowerDown = 0xAB54,
+    /// No operation, can terminate Reset Enable command
+    NoOperation = 0x00FF,
+    /// Enable reset operation (must precede Reset Memory command)
+    ResetEnable = 0x6699,
+    /// Reset device to power-on state, clears volatile settings
+    ResetMemory = 0x9966,
+    /// Protect all sectors using Dynamic Protection Bits (DPB)
+    GangBlockLock = 0x7E81,
+    /// Unprotect all sectors by clearing Dynamic Protection Bits (DPB)
+    GangBlockUnlock = 0x9867,
+
+    // Register Access commands
+    /// Read 3-byte device identification with 4-byte dummy address
+    ReadIdentification = 0x9F60,
+    /// Read Serial Flash Discoverable Parameters (SFDP) table with 4-byte address
+    ReadSFDP = 0x5AA5,
+    /// Read 8-bit Status Register with 4-byte dummy address
+    ReadStatusRegister = 0x05FA,
+    /// Read 8-bit Configuration Register with specific address (00000001h)
+    ReadConfigurationRegister = 0x15EA,
+    /// Write 8-bit Status Register with specific address (00000000h) or Configuration Register with address (00000001h)
+    WriteStatusConfigurationRegister = 0x01FE,
+    /// Read Configuration Register 2 from specified 4-byte address
+    ReadConfigurationRegister2 = 0x718E,
+    /// Write Configuration Register 2 to specified 4-byte address
+    WriteConfigurationRegister2 = 0x728D,
+    /// Read 8-bit Security Register with 4-byte dummy address
+    ReadSecurityRegister = 0x2BD4,
+    /// Write Security Register to set customer lock-down bit
+    WriteSecurityRegister = 0x2FD0,
+    /// Set burst/wrap length for read operations with 4-byte dummy address
+    SetBurstLength = 0xC03F,
+    /// Read 32-bit Fast Boot Register with 4-byte dummy address
+    ReadFastBootRegister = 0x16E9,
+    /// Write 32-bit Fast Boot Register with 4-byte dummy address
+    WriteFastBootRegister = 0x17E8,
+    /// Erase Fast Boot Register (disable fast boot feature)
+    EraseFastBootRegister = 0x18E7,
+    /// Enter 8K-bit secured OTP mode for programming unique identifiers
+    EnterSecuredOTP = 0xB14E,
+    /// Exit secured OTP mode and return to main array access
+    ExitSecuredOTP = 0xC13E,
+    /// Write Lock Register to control SPB protection mode with 4-byte dummy address
+    WriteLockRegister = 0x2CD3,
+    /// Read Lock Register status with 4-byte dummy address
+    ReadLockRegister = 0x2DD2,
+    /// Program Solid Protection Bit (SPB) for specified 4-byte address
+    WriteSPB = 0xE31C,
+    /// Erase all Solid Protection Bits (SPB)
+    EraseSPB = 0xE41B,
+    /// Read Solid Protection Bit (SPB) status for specified 4-byte address
+    ReadSPB = 0xE21D,
+    /// Write Dynamic Protection Bit (DPB) for specified 4-byte address
+    WriteDPB = 0xE11E,
+    /// Read Dynamic Protection Bit (DPB) status for specified 4-byte address
+    ReadDPB = 0xE01F,
+    /// Read 64-bit password register with 4-byte dummy address and 20 dummy cycles
+    ReadPassword = 0x27D8,
+    /// Write 64-bit password register with 4-byte dummy address
+    WritePassword = 0x28D7,
+    /// Unlock SPB operations using 64-bit password with 4-byte dummy address
+    PasswordUnlock = 0x29D6,
+}
 
-    async fn qpi_mode(&mut self) {
-        // Enter qpi mode
-        self.exec_command(0x38).await;
+impl<I: Instance> SpiFlashMemory<I> {
+    pub fn new(xspi: Xspi<'static, I, Blocking>) -> Self {
+        let mut memory = Self { xspi };
 
-        // Set read param
-        let transaction = TransferConfig {
-            iwidth: XspiWidth::QUAD,
-            dwidth: XspiWidth::QUAD,
-            instruction: Some(0xC0),
-            ..Default::default()
-        };
-        self.enable_write().await;
-        self.xspi.blocking_write(&[0x30_u8], transaction).unwrap();
-        self.wait_write_finish();
+        memory.reset_memory();
+        memory
     }
 
-    pub async fn disable_mm(&mut self) {
+    pub fn disable_mm(&mut self) {
         self.xspi.disable_memory_mapped_mode();
     }
 
-    pub async fn enable_mm(&mut self) {
-        self.qpi_mode().await;
-
+    pub fn enable_mm(&mut self) {
         let read_config = TransferConfig {
             iwidth: XspiWidth::SING,
             isize: AddressSize::_8bit,
             adwidth: XspiWidth::SING,
-            adsize: AddressSize::_24bit,
+            adsize: AddressSize::_32bit,
             dwidth: XspiWidth::SING,
-            instruction: Some(CMD_READ as u32),
+            instruction: Some(SpiCommand::FastRead4B as u32),
             dummy: DummyCycles::_8,
             ..Default::default()
         };
@@ -198,42 +445,28 @@ impl<I: Instance> FlashMemory<I> {
             iwidth: XspiWidth::SING,
             isize: AddressSize::_8bit,
             adwidth: XspiWidth::SING,
-            adsize: AddressSize::_24bit,
+            adsize: AddressSize::_32bit,
             dwidth: XspiWidth::SING,
-            instruction: Some(CMD_WRITE_PG as u32),
+            instruction: Some(SpiCommand::PageProgram4B as u32),
             dummy: DummyCycles::_0,
             ..Default::default()
         };
         self.xspi.enable_memory_mapped_mode(read_config, write_config).unwrap();
     }
 
-    fn enable_octo(&mut self) {
-        let cr = self.read_cr();
-        // info!("Read cr: {:x}", cr);
-        self.write_cr(cr | 0x02);
-        // info!("Read cr after writing: {:x}", cr);
+    fn into_octo(mut self) -> OpiFlashMemory<I> {
+        self.enable_opi_mode();
+        OpiFlashMemory { xspi: self.xspi }
     }
 
-    pub fn disable_octo(&mut self) {
-        let cr = self.read_cr();
-        self.write_cr(cr & (!(0x02)));
-    }
-
-    async fn exec_command_4(&mut self, cmd: u8) {
-        let transaction = TransferConfig {
-            iwidth: XspiWidth::QUAD,
-            adwidth: XspiWidth::NONE,
-            // adsize: AddressSize::_24bit,
-            dwidth: XspiWidth::NONE,
-            instruction: Some(cmd as u32),
-            address: None,
-            dummy: DummyCycles::_0,
-            ..Default::default()
-        };
-        self.xspi.blocking_command(&transaction).unwrap();
+    fn enable_opi_mode(&mut self) {
+        let cr2_0 = self.read_cr2(0);
+        info!("Read CR2 at 0x0: {:x}", cr2_0);
+        self.enable_write();
+        self.write_cr2(0, cr2_0 | 0x01); // Set bit 0 to enable octo SPI in STR
     }
 
-    async fn exec_command(&mut self, cmd: u8) {
+    fn exec_command(&mut self, cmd: u8) {
         let transaction = TransferConfig {
             iwidth: XspiWidth::SING,
             adwidth: XspiWidth::NONE,
@@ -248,16 +481,14 @@ impl<I: Instance> FlashMemory<I> {
         self.xspi.blocking_command(&transaction).unwrap();
     }
 
-    pub async fn reset_memory(&mut self) {
-        self.exec_command_4(CMD_ENABLE_RESET).await;
-        self.exec_command_4(CMD_RESET).await;
-        self.exec_command(CMD_ENABLE_RESET).await;
-        self.exec_command(CMD_RESET).await;
+    pub fn reset_memory(&mut self) {
+        self.exec_command(SpiCommand::ResetEnable as u8);
+        self.exec_command(SpiCommand::ResetMemory as u8);
         self.wait_write_finish();
     }
 
-    pub async fn enable_write(&mut self) {
-        self.exec_command(CMD_WRITE_ENABLE).await;
+    pub fn enable_write(&mut self) {
+        self.exec_command(SpiCommand::WriteEnable as u8);
     }
 
     pub fn read_id(&mut self) -> [u8; 3] {
@@ -266,92 +497,64 @@ impl<I: Instance> FlashMemory<I> {
             iwidth: XspiWidth::SING,
             isize: AddressSize::_8bit,
             adwidth: XspiWidth::NONE,
-            // adsize: AddressSize::_24bit,
             dwidth: XspiWidth::SING,
-            instruction: Some(CMD_READ_ID as u32),
+            instruction: Some(SpiCommand::ReadIdentification as u32),
             ..Default::default()
         };
-        // info!("Reading id: 0x{:X}", transaction.instruction);
         self.xspi.blocking_read(&mut buffer, transaction).unwrap();
         buffer
     }
 
-    pub fn read_id_8(&mut self) -> [u8; 3] {
-        let mut buffer = [0; 3];
-        let transaction: TransferConfig = TransferConfig {
-            iwidth: XspiWidth::OCTO,
-            isize: AddressSize::_16bit,
-            adwidth: XspiWidth::OCTO,
-            address: Some(0),
-            adsize: AddressSize::_32bit,
-            dwidth: XspiWidth::OCTO,
-            instruction: Some(CMD_READ_ID_OCTO as u32),
-            dummy: DummyCycles::_4,
-            ..Default::default()
-        };
-        info!("Reading id: {:#X}", transaction.instruction);
-        self.xspi.blocking_read(&mut buffer, transaction).unwrap();
-        buffer
-    }
-
-    pub fn read_memory(&mut self, addr: u32, buffer: &mut [u8], use_dma: bool) {
+    pub fn read_memory(&mut self, addr: u32, buffer: &mut [u8]) {
         let transaction = TransferConfig {
             iwidth: XspiWidth::SING,
             adwidth: XspiWidth::SING,
-            adsize: AddressSize::_24bit,
+            adsize: AddressSize::_32bit,
             dwidth: XspiWidth::SING,
-            instruction: Some(CMD_READ as u32),
+            instruction: Some(SpiCommand::FastRead4B as u32),
             dummy: DummyCycles::_8,
-            // dwidth: XspiWidth::QUAD,
-            // instruction: Some(CMD_QUAD_READ as u32),
-            // dummy: DummyCycles::_8,
             address: Some(addr),
             ..Default::default()
         };
-        if use_dma {
-            self.xspi.blocking_read(buffer, transaction).unwrap();
-        } else {
-            self.xspi.blocking_read(buffer, transaction).unwrap();
-        }
+
+        self.xspi.blocking_read(buffer, transaction).unwrap();
     }
 
     fn wait_write_finish(&mut self) {
         while (self.read_sr() & 0x01) != 0 {}
     }
 
-    async fn perform_erase(&mut self, addr: u32, cmd: u8) {
+    fn perform_erase(&mut self, addr: u32, cmd: u8) {
         let transaction = TransferConfig {
             iwidth: XspiWidth::SING,
             adwidth: XspiWidth::SING,
-            adsize: AddressSize::_24bit,
+            adsize: AddressSize::_32bit,
             dwidth: XspiWidth::NONE,
             instruction: Some(cmd as u32),
             address: Some(addr),
             dummy: DummyCycles::_0,
             ..Default::default()
         };
-        self.enable_write().await;
+        self.enable_write();
         self.xspi.blocking_command(&transaction).unwrap();
         self.wait_write_finish();
     }
 
-    pub async fn erase_sector(&mut self, addr: u32) {
-        self.perform_erase(addr, CMD_SECTOR_ERASE).await;
+    pub fn erase_sector(&mut self, addr: u32) {
+        self.perform_erase(addr, SpiCommand::SectorErase4B as u8);
     }
 
-    pub async fn erase_block_32k(&mut self, addr: u32) {
-        self.perform_erase(addr, CMD_BLOCK_ERASE_32K).await;
+    pub fn erase_block_64k(&mut self, addr: u32) {
+        self.perform_erase(addr, SpiCommand::BlockErase4B as u8);
     }
 
-    pub async fn erase_block_64k(&mut self, addr: u32) {
-        self.perform_erase(addr, CMD_BLOCK_ERASE_64K).await;
-    }
-
-    pub async fn erase_chip(&mut self) {
-        self.exec_command(CMD_CHIP_ERASE).await;
+    pub fn erase_chip(&mut self) {
+        self.enable_write();
+        self.exec_command(SpiCommand::ChipErase as u8);
+        self.wait_write_finish();
     }
 
-    async fn write_page(&mut self, addr: u32, buffer: &[u8], len: usize, use_dma: bool) {
+    fn write_page(&mut self, addr: u32, buffer: &[u8], len: usize) {
         assert!(
             (len as u32 + (addr & 0x000000ff)) <= MEMORY_PAGE_SIZE as u32,
             "write_page(): page write length exceeds page boundary (len = {}, addr = {:X}",
@@ -361,48 +564,43 @@ impl<I: Instance> FlashMemory<I> {
 
         let transaction = TransferConfig {
             iwidth: XspiWidth::SING,
-            adsize: AddressSize::_24bit,
+            adsize: AddressSize::_32bit,
             adwidth: XspiWidth::SING,
             dwidth: XspiWidth::SING,
-            instruction: Some(CMD_WRITE_PG as u32),
-            // dwidth: XspiWidth::QUAD,
-            // instruction: Some(CMD_QUAD_WRITE_PG as u32),
+            instruction: Some(SpiCommand::PageProgram4B as u32),
             address: Some(addr),
             dummy: DummyCycles::_0,
             ..Default::default()
         };
-        self.enable_write().await;
-        if use_dma {
-            self.xspi.blocking_write(buffer, transaction).unwrap();
-        } else {
-            self.xspi.blocking_write(buffer, transaction).unwrap();
-        }
+        self.enable_write();
+        self.xspi.blocking_write(buffer, transaction).unwrap();
         self.wait_write_finish();
     }
 
-    pub async fn write_memory(&mut self, addr: u32, buffer: &[u8], use_dma: bool) {
+    pub fn write_memory(&mut self, addr: u32, buffer: &[u8]) {
         let mut left = buffer.len();
         let mut place = addr;
         let mut chunk_start = 0;
 
         while left > 0 {
             let max_chunk_size = MEMORY_PAGE_SIZE - (place & 0x000000ff) as usize;
-            let chunk_size = if left >= max_chunk_size { max_chunk_size } else { left };
+            let chunk_size = min(max_chunk_size, left);
             let chunk = &buffer[chunk_start..(chunk_start + chunk_size)];
-            self.write_page(place, chunk, chunk_size, use_dma).await;
+            self.write_page(place, chunk, chunk_size);
             place += chunk_size as u32;
             left -= chunk_size;
             chunk_start += chunk_size;
         }
     }
 
+    // Note: read_register cannot be used to read the configuration register 2 since there is an
+    // address required for that read.
     fn read_register(&mut self, cmd: u8) -> u8 {
         let mut buffer = [0; 1];
         let transaction: TransferConfig = TransferConfig {
             iwidth: XspiWidth::SING,
             isize: AddressSize::_8bit,
             adwidth: XspiWidth::NONE,
-            adsize: AddressSize::_24bit,
             dwidth: XspiWidth::SING,
             instruction: Some(cmd as u32),
             address: None,
@@ -410,39 +608,345 @@ impl<I: Instance> FlashMemory<I> {
             ..Default::default()
         };
         self.xspi.blocking_read(&mut buffer, transaction).unwrap();
-        // info!("Read w25q64 register: 0x{:x}", buffer[0]);
         buffer[0]
     }
 
-    fn write_register(&mut self, cmd: u8, value: u8) {
-        let buffer = [value; 1];
+    pub fn read_sr(&mut self) -> u8 {
+        self.read_register(SpiCommand::ReadStatusRegister as u8)
+    }
+
+    pub fn read_cr(&mut self) -> u8 {
+        self.read_register(SpiCommand::ReadConfigurationRegister as u8)
+    }
+
+    pub fn write_sr_cr(&mut self, sr: u8, cr: u8) {
+        let buffer = [sr, cr];
         let transaction: TransferConfig = TransferConfig {
             iwidth: XspiWidth::SING,
             isize: AddressSize::_8bit,
-            instruction: Some(cmd as u32),
-            adsize: AddressSize::_24bit,
+            instruction: Some(SpiCommand::WriteStatusConfigurationRegister as u32),
             adwidth: XspiWidth::NONE,
             dwidth: XspiWidth::SING,
             address: None,
             dummy: DummyCycles::_0,
             ..Default::default()
         };
+        self.enable_write();
+        self.xspi.blocking_write(&buffer, transaction).unwrap();
+        self.wait_write_finish();
+    }
+
+    pub fn read_cr2(&mut self, address: u32) -> u8 {
+        let mut buffer = [0; 1];
+        let transaction: TransferConfig = TransferConfig {
+            iwidth: XspiWidth::SING,
+            isize: AddressSize::_8bit,
+            instruction: Some(SpiCommand::ReadConfigurationRegister2 as u32),
+            adsize: AddressSize::_32bit,
+            adwidth: XspiWidth::SING,
+            dwidth: XspiWidth::SING,
+            address: Some(address),
+            dummy: DummyCycles::_0,
+            ..Default::default()
+        };
+        self.xspi.blocking_read(&mut buffer, transaction).unwrap();
+        buffer[0]
+    }
+
+    pub fn write_cr2(&mut self, address: u32, value: u8) {
+        let buffer = [value; 1];
+        let transaction: TransferConfig = TransferConfig {
+            iwidth: XspiWidth::SING,
+            isize: AddressSize::_8bit,
+            instruction: Some(SpiCommand::WriteConfigurationRegister2 as u32),
+            adsize: AddressSize::_32bit,
+            adwidth: XspiWidth::SING,
+            dwidth: XspiWidth::SING,
+            address: Some(address),
+            dummy: DummyCycles::_0,
+            ..Default::default()
+        };
         self.xspi.blocking_write(&buffer, transaction).unwrap();
+        self.wait_write_finish();
+    }
+}
+
+impl<I: Instance> OpiFlashMemory<I> {
+    pub fn into_spi(mut self) -> SpiFlashMemory<I> {
+        self.disable_opi_mode();
+        SpiFlashMemory { xspi: self.xspi }
+    }
+
+    /// Disable OPI mode and return to SPI
+    pub fn disable_opi_mode(&mut self) {
+        // Clear SOPI and DOPI bits in CR2 volatile register
+        let cr2_0 = self.read_cr2(0x00000000);
+        self.write_cr2(0x00000000, cr2_0 & 0xFC); // Clear bits 0 and 1
+    }
+
+    /// Enable memory-mapped mode for OPI
+    pub fn enable_mm(&mut self) {
+        let read_config = TransferConfig {
+            iwidth: XspiWidth::OCTO,
+            isize: AddressSize::_16bit, // 2-byte command for OPI
+            adwidth: XspiWidth::OCTO,
+            adsize: AddressSize::_32bit,
+            dwidth: XspiWidth::OCTO,
+            instruction: Some(OpiCommand::OctaRead as u32),
+            dummy: DummyCycles::_20, // Default dummy cycles for OPI
+            ..Default::default()
+        };
+
+        let write_config = TransferConfig {
+            iwidth: XspiWidth::OCTO,
+            isize: AddressSize::_16bit,
+            adwidth: XspiWidth::OCTO,
+            adsize: AddressSize::_32bit,
+            dwidth: XspiWidth::OCTO,
+            instruction: Some(OpiCommand::PageProgram4B as u32),
+            dummy: DummyCycles::_0,
+            ..Default::default()
+        };
+
+        self.xspi.enable_memory_mapped_mode(read_config, write_config).unwrap();
+    }
+
+    pub fn disable_mm(&mut self) {
+        self.xspi.disable_memory_mapped_mode();
+    }
+
+    /// Execute OPI command (2-byte command)
+    fn exec_command(&mut self, cmd: OpiCommand) {
+        let transaction = TransferConfig {
+            iwidth: XspiWidth::OCTO,
+            isize: AddressSize::_16bit, // 2-byte command
+            adwidth: XspiWidth::NONE,
+            dwidth: XspiWidth::NONE,
+            instruction: Some(cmd as u32),
+            address: None,
+            dummy: DummyCycles::_0,
+            ..Default::default()
+        };
+        self.xspi.blocking_command(&transaction).unwrap();
+    }
+
+    /// Reset memory using OPI commands
+    pub fn reset_memory(&mut self) {
+        self.exec_command(OpiCommand::ResetEnable);
+        self.exec_command(OpiCommand::ResetMemory);
+        self.wait_write_finish();
     }
 
+    /// Enable write using OPI command
+    pub fn enable_write(&mut self) {
+        self.exec_command(OpiCommand::WriteEnable);
+    }
+
+    /// Read device ID in OPI mode
+    pub fn read_id(&mut self) -> [u8; 3] {
+        let mut buffer = [0; 3];
+        let transaction = TransferConfig {
+            iwidth: XspiWidth::OCTO,
+            isize: AddressSize::_16bit,
+            adwidth: XspiWidth::OCTO,
+            adsize: AddressSize::_32bit,
+            dwidth: XspiWidth::OCTO,
+            instruction: Some(OpiCommand::ReadIdentification as u32),
+            address: Some(0x00000000), // Dummy address required
+            dummy: DummyCycles::_4,
+            ..Default::default()
+        };
+        self.xspi.blocking_read(&mut buffer, transaction).unwrap();
+        buffer
+    }
+
+    /// Read memory using OPI mode
+    pub fn read_memory(&mut self, addr: u32, buffer: &mut [u8]) {
+        let transaction = TransferConfig {
+            iwidth: XspiWidth::OCTO,
+            isize: AddressSize::_16bit,
+            adwidth: XspiWidth::OCTO,
+            adsize: AddressSize::_32bit,
+            dwidth: XspiWidth::OCTO,
+            instruction: Some(OpiCommand::OctaRead as u32),
+            address: Some(addr),
+            dummy: DummyCycles::_20, // Default for 200MHz operation
+            ..Default::default()
+        };
+        self.xspi.blocking_read(buffer, transaction).unwrap();
+    }
+
+    /// Wait for write completion using OPI status read
+    fn wait_write_finish(&mut self) {
+        while (self.read_sr() & 0x01) != 0 {}
+    }
+
+    /// Perform erase operation using OPI command
+    fn perform_erase(&mut self, addr: u32, cmd: OpiCommand) {
+        let transaction = TransferConfig {
+            iwidth: XspiWidth::OCTO,
+            isize: AddressSize::_16bit,
+            adwidth: XspiWidth::OCTO,
+            adsize: AddressSize::_32bit,
+            dwidth: XspiWidth::NONE,
+            instruction: Some(cmd as u32),
+            address: Some(addr),
+            dummy: DummyCycles::_0,
+            ..Default::default()
+        };
+        self.enable_write();
+        self.xspi.blocking_command(&transaction).unwrap();
+        self.wait_write_finish();
+    }
+
+    /// Erase 4KB sector using OPI
+    pub fn erase_sector(&mut self, addr: u32) {
+        self.perform_erase(addr, OpiCommand::SectorErase4B);
+    }
+
+    /// Erase 64KB block using OPI
+    pub fn erase_block_64k(&mut self, addr: u32) {
+        self.perform_erase(addr, OpiCommand::BlockErase4B);
+    }
+
+    /// Erase entire chip using OPI
+    pub fn erase_chip(&mut self) {
+        self.enable_write();
+        self.exec_command(OpiCommand::ChipErase);
+        self.wait_write_finish();
+    }
+
+    /// Write single page using OPI
+    fn write_page(&mut self, addr: u32, buffer: &[u8], len: usize) {
+        assert!(
+            (len as u32 + (addr & 0x000000ff)) <= MEMORY_PAGE_SIZE as u32,
+            "write_page(): page write length exceeds page boundary (len = {}, addr = {:X})",
+            len,
+            addr
+        );
+
+        let transaction = TransferConfig {
+            iwidth: XspiWidth::OCTO,
+            isize: AddressSize::_16bit,
+            adwidth: XspiWidth::OCTO,
+            adsize: AddressSize::_32bit,
+            dwidth: XspiWidth::OCTO,
+            instruction: Some(OpiCommand::PageProgram4B as u32),
+            address: Some(addr),
+            dummy: DummyCycles::_0,
+            ..Default::default()
+        };
+        self.enable_write();
+        self.xspi.blocking_write(buffer, transaction).unwrap();
+        self.wait_write_finish();
+    }
+
+    /// Write memory using OPI (handles page boundaries)
+    pub fn write_memory(&mut self, addr: u32, buffer: &[u8]) {
+        let mut left = buffer.len();
+        let mut place = addr;
+        let mut chunk_start = 0;
+
+        while left > 0 {
+            let max_chunk_size = MEMORY_PAGE_SIZE - (place & 0x000000ff) as usize;
+            let chunk_size = min(max_chunk_size, left);
+            let chunk = &buffer[chunk_start..(chunk_start + chunk_size)];
+            self.write_page(place, chunk, chunk_size);
+            place += chunk_size as u32;
+            left -= chunk_size;
+            chunk_start += chunk_size;
+        }
+    }
+
+    /// Read register using OPI mode
+    fn read_register(&mut self, cmd: OpiCommand, dummy_addr: u32, dummy_cycles: DummyCycles) -> u8 {
+        let mut buffer = [0; 1];
+        let transaction = TransferConfig {
+            iwidth: XspiWidth::OCTO,
+            isize: AddressSize::_16bit,
+            adwidth: XspiWidth::OCTO,
+            adsize: AddressSize::_32bit,
+            dwidth: XspiWidth::OCTO,
+            instruction: Some(cmd as u32),
+            address: Some(dummy_addr),
+            dummy: dummy_cycles,
+            ..Default::default()
+        };
+        self.xspi.blocking_read(&mut buffer, transaction).unwrap();
+        buffer[0]
+    }
+
+    /// Read Status Register using OPI
     pub fn read_sr(&mut self) -> u8 {
-        self.read_register(CMD_READ_SR)
+        self.read_register(
+            OpiCommand::ReadStatusRegister,
+            0x00000000, // Dummy address
+            DummyCycles::_4,
+        )
     }
 
+    /// Read Configuration Register using OPI
     pub fn read_cr(&mut self) -> u8 {
-        self.read_register(CMD_READ_CR)
+        self.read_register(
+            OpiCommand::ReadConfigurationRegister,
+            0x00000001, // Address for CR
+            DummyCycles::_4,
+        )
     }
 
-    pub fn write_sr(&mut self, value: u8) {
-        self.write_register(CMD_WRITE_SR, value);
+    /// Write Status/Configuration Register using OPI
+    pub fn write_sr_cr(&mut self, sr: u8, cr: u8) {
+        let transaction = TransferConfig {
+            iwidth: XspiWidth::OCTO,
+            isize: AddressSize::_16bit,
+            adwidth: XspiWidth::OCTO,
+            adsize: AddressSize::_32bit,
+            dwidth: XspiWidth::OCTO,
+            instruction: Some(OpiCommand::WriteStatusConfigurationRegister as u32),
+            address: Some(0x00000000),
+            dummy: DummyCycles::_0,
+            ..Default::default()
+        };
+
+        self.enable_write();
+        self.xspi.blocking_write(&[sr, cr], transaction).unwrap();
+        self.wait_write_finish();
+    }
+
+    /// Read Configuration Register 2 using OPI
+    pub fn read_cr2(&mut self, address: u32) -> u8 {
+        let mut buffer = [0; 1];
+        let transaction = TransferConfig {
+            iwidth: XspiWidth::OCTO,
+            isize: AddressSize::_16bit,
+            adwidth: XspiWidth::OCTO,
+            adsize: AddressSize::_32bit,
+            dwidth: XspiWidth::OCTO,
+            instruction: Some(OpiCommand::ReadConfigurationRegister2 as u32),
+            address: Some(address),
+            dummy: DummyCycles::_4,
+            ..Default::default()
+        };
+        self.xspi.blocking_read(&mut buffer, transaction).unwrap();
+        buffer[0]
     }
 
-    pub fn write_cr(&mut self, value: u8) {
-        self.write_register(CMD_WRITE_CR, value);
+    /// Write Configuration Register 2 using OPI
+    pub fn write_cr2(&mut self, address: u32, value: u8) {
+        let transaction = TransferConfig {
+            iwidth: XspiWidth::OCTO,
+            isize: AddressSize::_16bit,
+            adwidth: XspiWidth::OCTO,
+            adsize: AddressSize::_32bit,
+            dwidth: XspiWidth::OCTO,
+            instruction: Some(OpiCommand::WriteConfigurationRegister2 as u32),
+            address: Some(address),
+            dummy: DummyCycles::_0,
+            ..Default::default()
+        };
+
+        self.enable_write();
+        self.xspi.blocking_write(&[value], transaction).unwrap();
+        self.wait_write_finish();
     }
 }