From 9d674f0212bf2a51fccd192632a16f104ba9629c Mon Sep 17 00:00:00 2001
From: Mathias <mk@blackbird.online>
Date: Fri, 16 Sep 2022 12:40:39 +0200
Subject: [PATCH] First iteration attempt on implementing generic flash
 mutation access for RP2040

---
 embassy-rp/Cargo.toml   |   1 +
 embassy-rp/src/flash.rs | 100 ++++++++++++++++++++++++++++++++++++++++
 embassy-rp/src/lib.rs   |   1 +
 3 files changed, 102 insertions(+)
 create mode 100644 embassy-rp/src/flash.rs

diff --git a/embassy-rp/Cargo.toml b/embassy-rp/Cargo.toml
index df0af8df..ce78acd3 100644
--- a/embassy-rp/Cargo.toml
+++ b/embassy-rp/Cargo.toml
@@ -52,6 +52,7 @@ cortex-m = "0.7.6"
 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-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/flash.rs b/embassy-rp/src/flash.rs
new file mode 100644
index 00000000..3b640509
--- /dev/null
+++ b/embassy-rp/src/flash.rs
@@ -0,0 +1,100 @@
+use embedded_storage::nor_flash::{
+    ErrorType, MultiwriteNorFlash, NorFlash, NorFlashError, NorFlashErrorKind, ReadNorFlash,
+};
+
+/// 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,
+}
+
+impl NorFlashError for Error {
+    fn kind(&self) -> NorFlashErrorKind {
+        match self {
+            Self::OutOfBounds => NorFlashErrorKind::OutOfBounds,
+            Self::Unaligned => NorFlashErrorKind::NotAligned,
+        }
+    }
+}
+
+pub struct Flash<const FLASH_SIZE: usize>;
+
+impl<const FLASH_SIZE: usize> ErrorType for Flash<FLASH_SIZE> {
+    type Error = Error;
+}
+
+impl<const FLASH_SIZE: usize> MultiwriteNorFlash for Flash<FLASH_SIZE> {}
+
+impl<const FLASH_SIZE: usize> ReadNorFlash for Flash<FLASH_SIZE> {
+    const READ_SIZE: usize = 1;
+
+    fn read(&mut self, offset: u32, bytes: &mut [u8]) -> Result<(), Self::Error> {
+        if offset as usize >= FLASH_SIZE || offset as usize + bytes.len() > FLASH_SIZE {
+            return Err(Error::OutOfBounds);
+        }
+
+        let flash_data = unsafe { core::slice::from_raw_parts(offset as *const u8, bytes.len()) };
+        bytes.copy_from_slice(flash_data);
+        Ok(())
+    }
+
+    fn capacity(&self) -> usize {
+        FLASH_SIZE
+    }
+}
+
+impl<const FLASH_SIZE: usize> NorFlash for Flash<FLASH_SIZE> {
+    const WRITE_SIZE: usize = 4;
+
+    const ERASE_SIZE: usize = 4096;
+
+    fn erase(&mut self, from: u32, to: u32) -> Result<(), Self::Error> {
+        if to < from || to as usize > FLASH_SIZE {
+            return Err(Error::OutOfBounds);
+        }
+        if from as usize % Self::ERASE_SIZE != 0 || to as usize % Self::ERASE_SIZE != 0 {
+            return Err(Error::Unaligned);
+        }
+
+        let len = to - from;
+
+        // Make sure to uphold the contract point with rp2040-flash.
+        // - interrupts must be disabled
+        // - DMA must not access flash memory
+        // FIXME: Pause all DMA channels for the duration of the flash_write?
+
+        critical_section::with(|_| {
+            unsafe { rp2040_flash::flash::flash_range_erase(from, len, true) };
+        });
+
+        // Re-enable DMA channels
+
+        Ok(())
+    }
+
+    fn write(&mut self, offset: u32, bytes: &[u8]) -> Result<(), Self::Error> {
+        if offset as usize + bytes.len() > FLASH_SIZE {
+            return Err(Error::OutOfBounds);
+        }
+        if offset as usize % 4 != 0 || bytes.len() as usize % 4 != 0 {
+            return Err(Error::Unaligned);
+        }
+
+        // Make sure to uphold the contract point with rp2040-flash.
+        // - interrupts must be disabled
+        // - DMA must not access flash memory
+        // FIXME: Pause all DMA channels for the duration of the flash_write?
+
+        critical_section::with(|_| {
+            unsafe { rp2040_flash::flash::flash_range_program(offset, bytes, true) };
+        });
+
+        // Re-enable DMA channels
+
+        Ok(())
+    }
+}
diff --git a/embassy-rp/src/lib.rs b/embassy-rp/src/lib.rs
index 9ac98d22..ce013b09 100644
--- a/embassy-rp/src/lib.rs
+++ b/embassy-rp/src/lib.rs
@@ -19,6 +19,7 @@ pub mod uart;
 pub mod usb;
 
 mod clocks;
+pub mod flash;
 mod reset;
 
 // Reexports