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
-    for (init, byte) in unsafe { INIT_ROM_DATA.assume_init_ref() }
-        .iter()
-        .zip(ROM_DATA.iter())
-    {
-        byte.store(*init, Ordering::SeqCst);
+    let mut flash = Flash::<_, _, FLASH_SIZE>::new_blocking(p.FLASH);
+
+    if let Err(e) = load_rom(&mut flash) {
+        defmt::error!("Unable to initialize rom from flash: {}", e);
     }
 
     // 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(())
+}