add commit to flash function

src/main.rs

@@ -11,7 +11,7 @@
 mod serial;
 
 use core::{
-    mem::MaybeUninit,
+    mem::{MaybeUninit, transmute},
     sync::atomic::{AtomicU8, Ordering},
     task::Poll,
 };
@@ -22,9 +22,10 @@ use embassy_futures::{poll_once, yield_now};
 use embassy_rp::{
     bind_interrupts,
     clocks::ClockConfig,
+    flash::{self, FLASH_BASE, Flash},
     gpio::{Drive, Level, Output, SlewRate},
     interrupt::{self, InterruptExt, Priority},
-    peripherals::{PIO0, USB},
+    peripherals::{FLASH, PIO0, USB},
     pio::{self, Direction, Pio, ShiftConfig, ShiftDirection, StateMachine, program::pio_asm},
     usb::{self, Driver},
 };
@@ -40,7 +41,8 @@ use crate::serial::{Link, PacketBuilder};
 const ADDRESS_PINS: u8 = 15;
 const DATA_PINS: u8 = 8;
 
-const ROM_SIZE: usize = 1024 * 32;
+const ROM_SIZE: usize = 32 * 1024;
+const FLASH_SIZE: usize = 2 * 1024 * 1024;
 
 static ROM_DATA: [AtomicU8; ROM_SIZE] = [const { AtomicU8::new(0) }; ROM_SIZE];
 
@@ -68,12 +70,10 @@ async fn main(spawner: Spawner) -> ! {
 
     let led = unsafe { p.PIN_25.clone_unchecked() };
 
-    // initialize rom data
+    let mut flash = Flash::<_, _, FLASH_SIZE>::new_blocking(p.FLASH);
-    for (init, byte) in unsafe { INIT_ROM_DATA.assume_init_ref() }
+
-        .iter()
+    if let Err(e) = load_rom(&mut flash) {
-        .zip(ROM_DATA.iter())
+        defmt::error!("Unable to initialize rom from flash: {}", e);
-    {
-        byte.store(*init, Ordering::SeqCst);
     }
 
     // Pull from fifo and write to all data pins
@@ -263,8 +263,8 @@ async fn main(spawner: Spawner) -> ! {
 
     let usb = builder.build();
 
-    interrupt::SWI_IRQ_0.set_priority(Priority::P1);
     interrupt::PIO0_IRQ_0.set_priority(Priority::P0);
+    interrupt::SWI_IRQ_0.set_priority(Priority::P1);
     interrupt::USBCTRL_IRQ.set_priority(Priority::P3);
 
     let int_spawner = EXECUTOR_HIGH.start(interrupt::SWI_IRQ_0);
@@ -305,7 +305,7 @@ async fn main(spawner: Spawner) -> ! {
                 for byte in buf.into_iter().take(n) {
                     if let Some(packet) = packet_builder.push(byte) {
                         defmt::debug!("Got packet: {}", packet);
-                        if let Some(response) = link.handle_packet(&packet) {
+                        if let Some(response) = link.handle_packet(&packet, &mut flash) {
                             defmt::info!("Sending Response: {}", response);
                             if let Err(e) = response.send(&mut class).await {
                                 defmt::error!("Unable to send response: {}", e);
@@ -338,3 +338,33 @@ async fn pio_task(
         data_sm.tx().try_push(u32::from(data));
     }
 }
+
+fn store_rom(
+    flash: &mut Flash<'_, FLASH, impl flash::Mode, FLASH_SIZE>,
+) -> Result<(), flash::Error> {
+    let offset =
+        unsafe { (INIT_ROM_DATA.as_ptr() as *const u32).offset_from_unsigned(FLASH_BASE) as u32 };
+    let len = size_of_val(&INIT_ROM_DATA).min(ROM_DATA.len());
+    defmt::info!("Erasing flash at offset {:#x} with size {:#x}", offset, len);
+    flash.blocking_erase(offset, offset + len as u32)?;
+    let rom_buffer = unsafe { transmute::<&[AtomicU8], &[u8]>(&ROM_DATA[..len]) };
+    defmt::info!("Programming flash with buffer at {}", rom_buffer.as_ptr());
+    flash.blocking_write(offset, rom_buffer)?;
+    defmt::info!("Successfully commited rom to flash");
+    Ok(())
+}
+
+fn load_rom(
+    flash: &mut Flash<'_, FLASH, impl flash::Mode, FLASH_SIZE>,
+) -> Result<(), flash::Error> {
+    let offset =
+        unsafe { (INIT_ROM_DATA.as_ptr() as *const u32).offset_from_unsigned(FLASH_BASE) as u32 };
+    let len = size_of_val(&INIT_ROM_DATA).min(ROM_DATA.len());
+    for (i, rom) in ROM_DATA.iter().enumerate().take(len) {
+        let mut init = [0u8];
+        flash.blocking_read(offset + i as u32, &mut init)?;
+        let init = init[0];
+        rom.store(init, Ordering::SeqCst);
+    }
+    Ok(())
+}

src/serial.rs

@@ -1,12 +1,16 @@
 use core::sync::atomic::Ordering;
 
 use defmt::Format;
-use embassy_rp::usb::{self, Driver};
+use embassy_rp::{
+    flash::{self, Flash},
+    peripherals::FLASH,
+    usb::{self, Driver},
+};
 use embassy_usb::{class::cdc_acm::CdcAcmClass, driver::EndpointError};
 use heapless::{String, Vec};
 use num_enum::{IntoPrimitive, TryFromPrimitive};
 
-use crate::ROM_DATA;
+use crate::{FLASH_SIZE, ROM_DATA, store_rom};
 
 const MAX_PAYLOAD: u8 = 30;
 
@@ -131,7 +135,7 @@ impl PacketBuilder {
                 target_size,
                 payload,
             } => {
-                payload.push(byte).unwrap();
+                defmt::unwrap!(payload.push(byte));
                 if payload.len() >= usize::from(*target_size) {
                     let kind = *kind;
                     let payload = payload.clone();
@@ -160,7 +164,11 @@ impl Link {
     }
 
     #[unsafe(link_section = ".data")]
-    pub fn handle_packet(&mut self, packet: &Packet) -> Option<Packet> {
+    pub fn handle_packet(
+        &mut self,
+        packet: &Packet,
+        flash: &mut Flash<'_, FLASH, impl flash::Mode, FLASH_SIZE>,
+    ) -> Option<Packet> {
         if self.name.is_empty() {
             let _ = self.name.push_str("rom");
         }
@@ -169,7 +177,7 @@ impl Link {
                 if packet.payload.len() != 4 {
                     return Some(Packet::error(b"Invalid pointer length"));
                 }
-                self.pointer = u32::from_le_bytes(packet.payload[..].try_into().unwrap());
+                self.pointer = u32::from_le_bytes(defmt::unwrap!(packet.payload[..].try_into()));
                 defmt::info!("Set pointer to {:#010x}", self.pointer);
                 None
             }
@@ -274,7 +282,17 @@ impl Link {
                     }
                 }
             }
-            Kind::CommitFlash => None,
+            Kind::CommitFlash => {
+                if let Err(e) = store_rom(flash) {
+                    defmt::error!("Unable to commit rom to flash: {}", e);
+                    Some(Packet::error(b"Unable to commit rom"))
+                } else {
+                    Some(Packet {
+                        kind: Kind::CommitDone,
+                        payload: Vec::new(),
+                    })
+                }
+            }
             _ => Some(Packet::error(b"Unrecognized packet")),
         }
     }