Merge #951

951: (embassy-rp): Implementation of generic flash mutation access r=Dirbaio a=MathiasKoch

I have attempted to utilize the work done in `rp2040-flash` by implementing `embedded-storage` traits on top, for RP2040.

Concerns:
1. ~~Should the DMA be paused where I have put a FIXME note? `DMA_CHx.ctrl_trig().write(|w| { w.set_en(false) })`? If so, how to properly do that without have control over the peripheral for the DMA channels? And if so, I assume we should only re-enable/unpause the ones that were enabled before?~~
2. ~~Should I make sure core2 is halted as part of this code? I am not sure if https://github.com/jannic/rp2040-flash/blob/ea8ab1ac807a7ab2b28a18bb5ca2e42495bb744d/examples/flash_example.rs#L103-L109 is heavy/slow code to run?~~
3. ~~Any good way of making this configurable over `FLASH_SIZE`, `WRITE_SIZE` and `ERASE_SIZE` without doing it as generics or parameters, as those make it possible to do differing configs throughout the same program, which feels wrong? Preferably, a compile-time option?~~


**EDIT:**
I have implemented the flash API here under the assumption that all external QSPI nor flashes are infact `Multiwrite` capable, as this makes it possible to use the ROM function for writes of 1 bytes at a time.

I have also added a HIL test for this, but because HIL tests are running 100% from RAM and I wanted to make sure it still works when running from flash, I have also added an example testing erase/write cycles of entire sectors, as well as single bytes in multi-write style.

Ping `@Dirbaio` 

Co-authored-by: Mathias <[email protected]>
Co-authored-by: Vincent Stakenburg <[email protected]>
Co-authored-by: Joakim Hulthe <[email protected]>
Co-authored-by: Alex Martens <[email protected]>
Co-authored-by: Ulf Lilleengen <[email protected]>
Co-authored-by: Dario Nieuwenhuis <[email protected]>

8 files changed, 613 insertions, 1 deletions

diff --git a/embassy-rp/Cargo.toml b/embassy-rp/Cargo.toml
index d2196be76..04b0c13ce 100644
--- a/embassy-rp/Cargo.toml
+++ b/embassy-rp/Cargo.toml
@@ -54,6 +54,7 @@ critical-section = "1.1"
 futures = { version = "0.3.17", default-features = false, features = ["async-await"] }
 chrono = { version = "0.4", default-features = false, optional = true }
 embedded-io = { version = "0.3.1", features = ["async"], optional = true }
+embedded-storage = { version = "0.3" }
 
 rp2040-pac2 = { git = "https://github.com/embassy-rs/rp2040-pac2", rev="017e3c9007b2d3b6965f0d85b5bf8ce3fa6d7364", features = ["rt"] }
 #rp2040-pac2 = { path = "../../rp2040-pac2", features = ["rt"] }
diff --git a/embassy-rp/src/dma.rs b/embassy-rp/src/dma.rs
index 410c48666..fd281fd5d 100644
--- a/embassy-rp/src/dma.rs
+++ b/embassy-rp/src/dma.rs
@@ -191,7 +191,7 @@ impl<'a, C: Channel> Future for Transfer<'a, C> {
     }
 }
 
-const CHANNEL_COUNT: usize = 12;
+pub(crate) const CHANNEL_COUNT: usize = 12;
 const NEW_AW: AtomicWaker = AtomicWaker::new();
 static CHANNEL_WAKERS: [AtomicWaker; CHANNEL_COUNT] = [NEW_AW; CHANNEL_COUNT];
 
diff --git a/embassy-rp/src/flash.rs b/embassy-rp/src/flash.rs
new file mode 100644
index 000000000..d09cc62fb
--- /dev/null
+++ b/embassy-rp/src/flash.rs
@@ -0,0 +1,463 @@
+use core::marker::PhantomData;
+
+use embassy_hal_common::Peripheral;
+use embedded_storage::nor_flash::{
+    check_erase, check_read, check_write, ErrorType, MultiwriteNorFlash, NorFlash, NorFlashError, NorFlashErrorKind,
+    ReadNorFlash,
+};
+
+use crate::peripherals::FLASH;
+
+pub const FLASH_BASE: usize = 0x10000000;
+
+// **NOTE**:
+//
+// These limitations are currently enforced because of using the
+// RP2040 boot-rom flash functions, that are optimized for flash compatibility
+// rather than performance.
+pub const PAGE_SIZE: usize = 256;
+pub const WRITE_SIZE: usize = 1;
+pub const READ_SIZE: usize = 1;
+pub const ERASE_SIZE: usize = 4096;
+
+/// Error type for NVMC operations.
+#[derive(Debug, Copy, Clone, PartialEq, Eq)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub enum Error {
+    /// Opration using a location not in flash.
+    OutOfBounds,
+    /// Unaligned operation or using unaligned buffers.
+    Unaligned,
+    Other,
+}
+
+impl From<NorFlashErrorKind> for Error {
+    fn from(e: NorFlashErrorKind) -> Self {
+        match e {
+            NorFlashErrorKind::NotAligned => Self::Unaligned,
+            NorFlashErrorKind::OutOfBounds => Self::OutOfBounds,
+            _ => Self::Other,
+        }
+    }
+}
+
+impl NorFlashError for Error {
+    fn kind(&self) -> NorFlashErrorKind {
+        match self {
+            Self::OutOfBounds => NorFlashErrorKind::OutOfBounds,
+            Self::Unaligned => NorFlashErrorKind::NotAligned,
+            Self::Other => NorFlashErrorKind::Other,
+        }
+    }
+}
+
+pub struct Flash<'d, T: Instance, const FLASH_SIZE: usize>(PhantomData<&'d mut T>);
+
+impl<'d, T: Instance, const FLASH_SIZE: usize> Flash<'d, T, FLASH_SIZE> {
+    pub fn new(_flash: impl Peripheral<P = T> + 'd) -> Self {
+        Self(PhantomData)
+    }
+
+    pub fn read(&mut self, offset: u32, bytes: &mut [u8]) -> Result<(), Error> {
+        check_read(self, offset, bytes.len())?;
+
+        let flash_data = unsafe { core::slice::from_raw_parts((FLASH_BASE as u32 + offset) as *const u8, bytes.len()) };
+
+        bytes.copy_from_slice(flash_data);
+        Ok(())
+    }
+
+    pub fn capacity(&self) -> usize {
+        FLASH_SIZE
+    }
+
+    pub fn erase(&mut self, from: u32, to: u32) -> Result<(), Error> {
+        check_erase(self, from, to)?;
+
+        trace!(
+            "Erasing from 0x{:x} to 0x{:x}",
+            FLASH_BASE as u32 + from,
+            FLASH_BASE as u32 + to
+        );
+
+        let len = to - from;
+
+        unsafe { self.in_ram(|| ram_helpers::flash_range_erase(from, len, true)) };
+
+        Ok(())
+    }
+
+    pub fn write(&mut self, offset: u32, bytes: &[u8]) -> Result<(), Error> {
+        check_write(self, offset, bytes.len())?;
+
+        trace!("Writing {:?} bytes to 0x{:x}", bytes.len(), FLASH_BASE as u32 + offset);
+
+        let end_offset = offset as usize + bytes.len();
+
+        let padded_offset = (offset as *const u8).align_offset(PAGE_SIZE);
+        let start_padding = core::cmp::min(padded_offset, bytes.len());
+
+        // Pad in the beginning
+        if start_padding > 0 {
+            let start = PAGE_SIZE - padded_offset;
+            let end = start + start_padding;
+
+            let mut pad_buf = [0xFF_u8; PAGE_SIZE];
+            pad_buf[start..end].copy_from_slice(&bytes[..start_padding]);
+
+            let unaligned_offset = offset as usize - start;
+
+            unsafe { self.in_ram(|| ram_helpers::flash_range_program(unaligned_offset as u32, &pad_buf, true)) }
+        }
+
+        let remaining_len = bytes.len() - start_padding;
+        let end_padding = start_padding + PAGE_SIZE * (remaining_len / PAGE_SIZE);
+
+        // Write aligned slice of length in multiples of 256 bytes
+        // If the remaining bytes to be written is more than a full page.
+        if remaining_len >= PAGE_SIZE {
+            let mut aligned_offset = if start_padding > 0 {
+                offset as usize + padded_offset
+            } else {
+                offset as usize
+            };
+
+            if bytes.as_ptr() as usize >= 0x2000_0000 {
+                let aligned_data = &bytes[start_padding..end_padding];
+
+                unsafe { self.in_ram(|| ram_helpers::flash_range_program(aligned_offset as u32, aligned_data, true)) }
+            } else {
+                for chunk in bytes[start_padding..end_padding].chunks_exact(PAGE_SIZE) {
+                    let mut ram_buf = [0xFF_u8; PAGE_SIZE];
+                    ram_buf.copy_from_slice(chunk);
+                    unsafe { self.in_ram(|| ram_helpers::flash_range_program(aligned_offset as u32, &ram_buf, true)) }
+                    aligned_offset += PAGE_SIZE;
+                }
+            }
+        }
+
+        // Pad in the end
+        let rem_offset = (end_offset as *const u8).align_offset(PAGE_SIZE);
+        let rem_padding = remaining_len % PAGE_SIZE;
+        if rem_padding > 0 {
+            let mut pad_buf = [0xFF_u8; PAGE_SIZE];
+            pad_buf[..rem_padding].copy_from_slice(&bytes[end_padding..]);
+
+            let unaligned_offset = end_offset - (PAGE_SIZE - rem_offset);
+
+            unsafe { self.in_ram(|| ram_helpers::flash_range_program(unaligned_offset as u32, &pad_buf, true)) }
+        }
+
+        Ok(())
+    }
+
+    /// Make sure to uphold the contract points with rp2040-flash.
+    /// - interrupts must be disabled
+    /// - DMA must not access flash memory
+    unsafe fn in_ram(&mut self, operation: impl FnOnce()) {
+        let dma_status = &mut [false; crate::dma::CHANNEL_COUNT];
+
+        // TODO: Make sure CORE1 is paused during the entire duration of the RAM function
+
+        critical_section::with(|_| {
+            // Pause all DMA channels for the duration of the ram operation
+            for (number, status) in dma_status.iter_mut().enumerate() {
+                let ch = crate::pac::DMA.ch(number as _);
+                *status = ch.ctrl_trig().read().en();
+                if *status {
+                    ch.ctrl_trig().modify(|w| w.set_en(false));
+                }
+            }
+
+            // Run our flash operation in RAM
+            operation();
+
+            // Re-enable previously enabled DMA channels
+            for (number, status) in dma_status.iter().enumerate() {
+                let ch = crate::pac::DMA.ch(number as _);
+                if *status {
+                    ch.ctrl_trig().modify(|w| w.set_en(true));
+                }
+            }
+        });
+    }
+}
+
+impl<'d, T: Instance, const FLASH_SIZE: usize> ErrorType for Flash<'d, T, FLASH_SIZE> {
+    type Error = Error;
+}
+
+impl<'d, T: Instance, const FLASH_SIZE: usize> ReadNorFlash for Flash<'d, T, FLASH_SIZE> {
+    const READ_SIZE: usize = READ_SIZE;
+
+    fn read(&mut self, offset: u32, bytes: &mut [u8]) -> Result<(), Self::Error> {
+        self.read(offset, bytes)
+    }
+
+    fn capacity(&self) -> usize {
+        self.capacity()
+    }
+}
+
+impl<'d, T: Instance, const FLASH_SIZE: usize> MultiwriteNorFlash for Flash<'d, T, FLASH_SIZE> {}
+
+impl<'d, T: Instance, const FLASH_SIZE: usize> NorFlash for Flash<'d, T, FLASH_SIZE> {
+    const WRITE_SIZE: usize = WRITE_SIZE;
+
+    const ERASE_SIZE: usize = ERASE_SIZE;
+
+    fn erase(&mut self, from: u32, to: u32) -> Result<(), Self::Error> {
+        self.erase(from, to)
+    }
+
+    fn write(&mut self, offset: u32, bytes: &[u8]) -> Result<(), Self::Error> {
+        self.write(offset, bytes)
+    }
+}
+
+#[allow(dead_code)]
+mod ram_helpers {
+    use core::marker::PhantomData;
+
+    use crate::rom_data;
+
+    #[repr(C)]
+    struct FlashFunctionPointers<'a> {
+        connect_internal_flash: unsafe extern "C" fn() -> (),
+        flash_exit_xip: unsafe extern "C" fn() -> (),
+        flash_range_erase: Option<unsafe extern "C" fn(addr: u32, count: usize, block_size: u32, block_cmd: u8) -> ()>,
+        flash_range_program: Option<unsafe extern "C" fn(addr: u32, data: *const u8, count: usize) -> ()>,
+        flash_flush_cache: unsafe extern "C" fn() -> (),
+        flash_enter_cmd_xip: unsafe extern "C" fn() -> (),
+        phantom: PhantomData<&'a ()>,
+    }
+
+    #[allow(unused)]
+    fn flash_function_pointers(erase: bool, write: bool) -> FlashFunctionPointers<'static> {
+        FlashFunctionPointers {
+            connect_internal_flash: rom_data::connect_internal_flash::ptr(),
+            flash_exit_xip: rom_data::flash_exit_xip::ptr(),
+            flash_range_erase: if erase {
+                Some(rom_data::flash_range_erase::ptr())
+            } else {
+                None
+            },
+            flash_range_program: if write {
+                Some(rom_data::flash_range_program::ptr())
+            } else {
+                None
+            },
+            flash_flush_cache: rom_data::flash_flush_cache::ptr(),
+            flash_enter_cmd_xip: rom_data::flash_enter_cmd_xip::ptr(),
+            phantom: PhantomData,
+        }
+    }
+
+    #[allow(unused)]
+    /// # Safety
+    ///
+    /// `boot2` must contain a valid 2nd stage boot loader which can be called to re-initialize XIP mode
+    unsafe fn flash_function_pointers_with_boot2(erase: bool, write: bool, boot2: &[u32; 64]) -> FlashFunctionPointers {
+        let boot2_fn_ptr = (boot2 as *const u32 as *const u8).offset(1);
+        let boot2_fn: unsafe extern "C" fn() -> () = core::mem::transmute(boot2_fn_ptr);
+        FlashFunctionPointers {
+            connect_internal_flash: rom_data::connect_internal_flash::ptr(),
+            flash_exit_xip: rom_data::flash_exit_xip::ptr(),
+            flash_range_erase: if erase {
+                Some(rom_data::flash_range_erase::ptr())
+            } else {
+                None
+            },
+            flash_range_program: if write {
+                Some(rom_data::flash_range_program::ptr())
+            } else {
+                None
+            },
+            flash_flush_cache: rom_data::flash_flush_cache::ptr(),
+            flash_enter_cmd_xip: boot2_fn,
+            phantom: PhantomData,
+        }
+    }
+
+    /// Erase a flash range starting at `addr` with length `len`.
+    ///
+    /// `addr` and `len` must be multiples of 4096
+    ///
+    /// If `use_boot2` is `true`, a copy of the 2nd stage boot loader
+    /// is used to re-initialize the XIP engine after flashing.
+    ///
+    /// # Safety
+    ///
+    /// Nothing must access flash while this is running.
+    /// Usually this means:
+    ///   - interrupts must be disabled
+    ///   - 2nd core must be running code from RAM or ROM with interrupts disabled
+    ///   - DMA must not access flash memory
+    ///
+    /// `addr` and `len` parameters must be valid and are not checked.
+    pub unsafe fn flash_range_erase(addr: u32, len: u32, use_boot2: bool) {
+        let mut boot2 = [0u32; 256 / 4];
+        let ptrs = if use_boot2 {
+            rom_data::memcpy44(&mut boot2 as *mut _, super::FLASH_BASE as *const _, 256);
+            flash_function_pointers_with_boot2(true, false, &boot2)
+        } else {
+            flash_function_pointers(true, false)
+        };
+
+        core::sync::atomic::compiler_fence(core::sync::atomic::Ordering::SeqCst);
+
+        write_flash_inner(addr, len, None, &ptrs as *const FlashFunctionPointers);
+    }
+
+    /// Erase and rewrite a flash range starting at `addr` with data `data`.
+    ///
+    /// `addr` and `data.len()` must be multiples of 4096
+    ///
+    /// If `use_boot2` is `true`, a copy of the 2nd stage boot loader
+    /// is used to re-initialize the XIP engine after flashing.
+    ///
+    /// # Safety
+    ///
+    /// Nothing must access flash while this is running.
+    /// Usually this means:
+    ///   - interrupts must be disabled
+    ///   - 2nd core must be running code from RAM or ROM with interrupts disabled
+    ///   - DMA must not access flash memory
+    ///
+    /// `addr` and `len` parameters must be valid and are not checked.
+    pub unsafe fn flash_range_erase_and_program(addr: u32, data: &[u8], use_boot2: bool) {
+        let mut boot2 = [0u32; 256 / 4];
+        let ptrs = if use_boot2 {
+            rom_data::memcpy44(&mut boot2 as *mut _, super::FLASH_BASE as *const _, 256);
+            flash_function_pointers_with_boot2(true, true, &boot2)
+        } else {
+            flash_function_pointers(true, true)
+        };
+
+        core::sync::atomic::compiler_fence(core::sync::atomic::Ordering::SeqCst);
+
+        write_flash_inner(
+            addr,
+            data.len() as u32,
+            Some(data),
+            &ptrs as *const FlashFunctionPointers,
+        );
+    }
+
+    /// Write a flash range starting at `addr` with data `data`.
+    ///
+    /// `addr` and `data.len()` must be multiples of 256
+    ///
+    /// If `use_boot2` is `true`, a copy of the 2nd stage boot loader
+    /// is used to re-initialize the XIP engine after flashing.
+    ///
+    /// # Safety
+    ///
+    /// Nothing must access flash while this is running.
+    /// Usually this means:
+    ///   - interrupts must be disabled
+    ///   - 2nd core must be running code from RAM or ROM with interrupts disabled
+    ///   - DMA must not access flash memory
+    ///
+    /// `addr` and `len` parameters must be valid and are not checked.
+    pub unsafe fn flash_range_program(addr: u32, data: &[u8], use_boot2: bool) {
+        let mut boot2 = [0u32; 256 / 4];
+        let ptrs = if use_boot2 {
+            rom_data::memcpy44(&mut boot2 as *mut _, super::FLASH_BASE as *const _, 256);
+            flash_function_pointers_with_boot2(false, true, &boot2)
+        } else {
+            flash_function_pointers(false, true)
+        };
+
+        core::sync::atomic::compiler_fence(core::sync::atomic::Ordering::SeqCst);
+
+        write_flash_inner(
+            addr,
+            data.len() as u32,
+            Some(data),
+            &ptrs as *const FlashFunctionPointers,
+        );
+    }
+
+    /// # Safety
+    ///
+    /// Nothing must access flash while this is running.
+    /// Usually this means:
+    ///   - interrupts must be disabled
+    ///   - 2nd core must be running code from RAM or ROM with interrupts disabled
+    ///   - DMA must not access flash memory
+    /// Length of data must be a multiple of 4096
+    /// addr must be aligned to 4096
+    #[inline(never)]
+    #[link_section = ".data.ram_func"]
+    unsafe fn write_flash_inner(addr: u32, len: u32, data: Option<&[u8]>, ptrs: *const FlashFunctionPointers) {
+        /*
+         Should be equivalent to:
+            rom_data::connect_internal_flash();
+            rom_data::flash_exit_xip();
+            rom_data::flash_range_erase(addr, len, 1 << 31, 0); // if selected
+            rom_data::flash_range_program(addr, data as *const _, len); // if selected
+            rom_data::flash_flush_cache();
+            rom_data::flash_enter_cmd_xip();
+        */
+        #[cfg(target_arch = "arm")]
+        core::arch::asm!(
+            "mov r8, r0",
+            "mov r9, r2",
+            "mov r10, r1",
+            "ldr r4, [{ptrs}, #0]",
+            "blx r4", // connect_internal_flash()
+
+            "ldr r4, [{ptrs}, #4]",
+            "blx r4", // flash_exit_xip()
+
+            "mov r0, r8", // r0 = addr
+            "mov r1, r10", // r1 = len
+            "movs r2, #1",
+            "lsls r2, r2, #31", // r2 = 1 << 31
+            "movs r3, #0", // r3 = 0
+            "ldr r4, [{ptrs}, #8]",
+            "cmp r4, #0",
+            "beq 1f",
+            "blx r4", // flash_range_erase(addr, len, 1 << 31, 0)
+            "1:",
+
+            "mov r0, r8", // r0 = addr
+            "mov r1, r9", // r0 = data
+            "mov r2, r10", // r2 = len
+            "ldr r4, [{ptrs}, #12]",
+            "cmp r4, #0",
+            "beq 1f",
+            "blx r4", // flash_range_program(addr, data, len);
+            "1:",
+
+            "ldr r4, [{ptrs}, #16]",
+            "blx r4", // flash_flush_cache();
+
+            "ldr r4, [{ptrs}, #20]",
+            "blx r4", // flash_enter_cmd_xip();
+            ptrs = in(reg) ptrs,
+            // Registers r8-r15 are not allocated automatically,
+            // so assign them manually. We need to use them as
+            // otherwise there are not enough registers available.
+            in("r0") addr,
+            in("r2") data.map(|d| d.as_ptr()).unwrap_or(core::ptr::null()),
+            in("r1") len,
+            out("r3") _,
+            out("r4") _,
+            lateout("r8") _,
+            lateout("r9") _,
+            lateout("r10") _,
+            clobber_abi("C"),
+        );
+    }
+}
+
+mod sealed {
+    pub trait Instance {}
+}
+
+pub trait Instance: sealed::Instance {}
+
+impl sealed::Instance for FLASH {}
+impl Instance for FLASH {}
diff --git a/embassy-rp/src/lib.rs b/embassy-rp/src/lib.rs
index e784399d4..f608f1768 100644
--- a/embassy-rp/src/lib.rs
+++ b/embassy-rp/src/lib.rs
@@ -20,6 +20,7 @@ pub mod uart;
 pub mod usb;
 
 mod clocks;
+pub mod flash;
 mod reset;
 
 // Reexports
@@ -95,6 +96,8 @@ embassy_hal_common::peripherals! {
     USB,
 
     RTC,
+
+    FLASH,
 }
 
 #[link_section = ".boot2"]
diff --git a/examples/rp/Cargo.toml b/examples/rp/Cargo.toml
index ec9896b77..31f688305 100644
--- a/examples/rp/Cargo.toml
+++ b/examples/rp/Cargo.toml
@@ -30,6 +30,7 @@ byte-slice-cast = { version = "1.2.0", default-features = false }
 embedded-hal-1 = { package = "embedded-hal", version = "=1.0.0-alpha.9" }
 embedded-hal-async = { version = "0.1.0-alpha.3" }
 embedded-io = { version = "0.3.1", features = ["async", "defmt"] }
+embedded-storage = { version = "0.3" }
 static_cell = "1.0.0"
 
 [profile.release]
diff --git a/examples/rp/src/bin/flash.rs b/examples/rp/src/bin/flash.rs
new file mode 100644
index 000000000..8d6b379f4
--- /dev/null
+++ b/examples/rp/src/bin/flash.rs
@@ -0,0 +1,89 @@
+#![no_std]
+#![no_main]
+#![feature(type_alias_impl_trait)]
+
+use defmt::*;
+use embassy_executor::Spawner;
+use embassy_rp::flash::{ERASE_SIZE, FLASH_BASE};
+use embassy_rp::peripherals::FLASH;
+use embassy_time::{Duration, Timer};
+use {defmt_rtt as _, panic_probe as _};
+
+const ADDR_OFFSET: u32 = 0x100000;
+const FLASH_SIZE: usize = 2 * 1024 * 1024;
+
+#[embassy_executor::main]
+async fn main(_spawner: Spawner) {
+    let p = embassy_rp::init(Default::default());
+    info!("Hello World!");
+
+    // add some delay to give an attached debug probe time to parse the
+    // defmt RTT header. Reading that header might touch flash memory, which
+    // interferes with flash write operations.
+    // https://github.com/knurling-rs/defmt/pull/683
+    Timer::after(Duration::from_millis(10)).await;
+
+    let mut flash = embassy_rp::flash::Flash::<_, FLASH_SIZE>::new(p.FLASH);
+    erase_write_sector(&mut flash, 0x00);
+
+    multiwrite_bytes(&mut flash, ERASE_SIZE as u32);
+
+    loop {}
+}
+
+fn multiwrite_bytes(flash: &mut embassy_rp::flash::Flash<'_, FLASH, FLASH_SIZE>, offset: u32) {
+    info!(">>>> [multiwrite_bytes]");
+    let mut read_buf = [0u8; ERASE_SIZE];
+    defmt::unwrap!(flash.read(ADDR_OFFSET + offset, &mut read_buf));
+
+    info!("Addr of flash block is {:x}", ADDR_OFFSET + offset + FLASH_BASE as u32);
+    info!("Contents start with {=[u8]}", read_buf[0..4]);
+
+    defmt::unwrap!(flash.erase(ADDR_OFFSET + offset, ADDR_OFFSET + offset + ERASE_SIZE as u32));
+
+    defmt::unwrap!(flash.read(ADDR_OFFSET + offset, &mut read_buf));
+    info!("Contents after erase starts with {=[u8]}", read_buf[0..4]);
+    if read_buf.iter().any(|x| *x != 0xFF) {
+        defmt::panic!("unexpected");
+    }
+
+    defmt::unwrap!(flash.write(ADDR_OFFSET + offset, &[0x01]));
+    defmt::unwrap!(flash.write(ADDR_OFFSET + offset + 1, &[0x02]));
+    defmt::unwrap!(flash.write(ADDR_OFFSET + offset + 2, &[0x03]));
+    defmt::unwrap!(flash.write(ADDR_OFFSET + offset + 3, &[0x04]));
+
+    defmt::unwrap!(flash.read(ADDR_OFFSET + offset, &mut read_buf));
+    info!("Contents after write starts with {=[u8]}", read_buf[0..4]);
+    if &read_buf[0..4] != &[0x01, 0x02, 0x03, 0x04] {
+        defmt::panic!("unexpected");
+    }
+}
+
+fn erase_write_sector(flash: &mut embassy_rp::flash::Flash<'_, FLASH, FLASH_SIZE>, offset: u32) {
+    info!(">>>> [erase_write_sector]");
+    let mut buf = [0u8; ERASE_SIZE];
+    defmt::unwrap!(flash.read(ADDR_OFFSET + offset, &mut buf));
+
+    info!("Addr of flash block is {:x}", ADDR_OFFSET + offset + FLASH_BASE as u32);
+    info!("Contents start with {=[u8]}", buf[0..4]);
+
+    defmt::unwrap!(flash.erase(ADDR_OFFSET + offset, ADDR_OFFSET + offset + ERASE_SIZE as u32));
+
+    defmt::unwrap!(flash.read(ADDR_OFFSET + offset, &mut buf));
+    info!("Contents after erase starts with {=[u8]}", buf[0..4]);
+    if buf.iter().any(|x| *x != 0xFF) {
+        defmt::panic!("unexpected");
+    }
+
+    for b in buf.iter_mut() {
+        *b = 0xDA;
+    }
+
+    defmt::unwrap!(flash.write(ADDR_OFFSET + offset, &buf));
+
+    defmt::unwrap!(flash.read(ADDR_OFFSET + offset, &mut buf));
+    info!("Contents after write starts with {=[u8]}", buf[0..4]);
+    if buf.iter().any(|x| *x != 0xDA) {
+        defmt::panic!("unexpected");
+    }
+}
diff --git a/tests/rp/Cargo.toml b/tests/rp/Cargo.toml
index e1a6dce41..069b7fb8c 100644
--- a/tests/rp/Cargo.toml
+++ b/tests/rp/Cargo.toml
@@ -22,6 +22,7 @@ embedded-hal-async = { version = "=0.1.0-alpha.3" }
 panic-probe = { version = "0.3.0", features = ["print-defmt"] }
 futures = { version = "0.3.17", default-features = false, features = ["async-await"] }
 embedded-io = { version = "0.3.1", features = ["async"] }
+embedded-storage = { version = "0.3" }
 
 [profile.dev]
 debug = 2
diff --git a/tests/rp/src/bin/flash.rs b/tests/rp/src/bin/flash.rs
new file mode 100644
index 000000000..897e3804f
--- /dev/null
+++ b/tests/rp/src/bin/flash.rs
@@ -0,0 +1,54 @@
+#![no_std]
+#![no_main]
+#![feature(type_alias_impl_trait)]
+
+use defmt::*;
+use embassy_executor::Spawner;
+use embassy_rp::flash::{ERASE_SIZE, FLASH_BASE};
+use embassy_time::{Duration, Timer};
+use {defmt_rtt as _, panic_probe as _};
+
+const ADDR_OFFSET: u32 = 0x4000;
+
+#[embassy_executor::main]
+async fn main(_spawner: Spawner) {
+    let p = embassy_rp::init(Default::default());
+    info!("Hello World!");
+
+    // add some delay to give an attached debug probe time to parse the
+    // defmt RTT header. Reading that header might touch flash memory, which
+    // interferes with flash write operations.
+    // https://github.com/knurling-rs/defmt/pull/683
+    Timer::after(Duration::from_millis(10)).await;
+
+    let mut flash = embassy_rp::flash::Flash::<_, { 2 * 1024 * 1024 }>::new(p.FLASH);
+
+    let mut buf = [0u8; ERASE_SIZE];
+    defmt::unwrap!(flash.read(ADDR_OFFSET, &mut buf));
+
+    info!("Addr of flash block is {:x}", ADDR_OFFSET + FLASH_BASE as u32);
+    info!("Contents start with {=[u8]}", buf[0..4]);
+
+    defmt::unwrap!(flash.erase(ADDR_OFFSET, ADDR_OFFSET + ERASE_SIZE as u32));
+
+    defmt::unwrap!(flash.read(ADDR_OFFSET, &mut buf));
+    info!("Contents after erase starts with {=[u8]}", buf[0..4]);
+    if buf.iter().any(|x| *x != 0xFF) {
+        defmt::panic!("unexpected");
+    }
+
+    for b in buf.iter_mut() {
+        *b = 0xDA;
+    }
+
+    defmt::unwrap!(flash.write(ADDR_OFFSET, &mut buf));
+
+    defmt::unwrap!(flash.read(ADDR_OFFSET, &mut buf));
+    info!("Contents after write starts with {=[u8]}", buf[0..4]);
+    if buf.iter().any(|x| *x != 0xDA) {
+        defmt::panic!("unexpected");
+    }
+
+    info!("Test OK");
+    cortex_m::asm::bkpt();
+}