use lpc55_hal as hal; use hal::{FlashGordon, drivers::flash::GenericArray, raw as pac, traits::flash::WriteErase}; use shared::config::{ ConfigContainer, ConfigString, ConfigV1, ConfigVersion, DeviceConfig, TlvList, }; use crate::{CONFIG_RESERVED, CONFIG_SLOT_COUNT, FLASH_PAGE_SIZE, SLOT_SIZE, config_base}; const SLOT_MAGIC: u32 = u32::from_le_bytes(*b"SLOT"); const FLASH_WORD_SIZE: usize = 16; const READ_SINGLE_WORD: u32 = 0x3; const READ_ECC_OFF: u32 = 1 << 2; const COMMAND_TIMEOUT: u32 = 1_000_000; static mut CONFIG_BUF: [u8; SLOT_SIZE] = [0u8; SLOT_SIZE]; #[cfg(feature = "hid")] pub struct ConfigState { pub running: DeviceConfig, pub active_slot: u8, } #[cfg(feature = "hid")] pub struct FlashState { pub flash: FlashGordon, } #[derive(Clone, Copy, defmt::Format)] enum FlashError { IllegalCommand, Failed, Timeout, } fn halt() -> ! { loop { cortex_m::asm::wfi(); } } fn slot_page_address() -> usize { config_base() } fn slot_address(index: usize) -> usize { config_base() + CONFIG_RESERVED + index * SLOT_SIZE } fn config_buf() -> &'static mut [u8; SLOT_SIZE] { // Safety: flash access is not reentrant and the buffer is only borrowed for one operation at a time. unsafe { &mut *core::ptr::addr_of_mut!(CONFIG_BUF) } } fn read_word( flash: &pac::flash::RegisterBlock, address: usize, ) -> Result<[u8; FLASH_WORD_SIZE], FlashError> { flash.int_clr_status.write(|w| { w.done() .set_bit() .ecc_err() .set_bit() .err() .set_bit() .fail() .set_bit() }); flash .starta .write(|w| unsafe { w.starta().bits((address / FLASH_WORD_SIZE) as u32) }); flash.dataw[0].write(|w| unsafe { w.bits(READ_ECC_OFF) }); flash.cmd.write(|w| unsafe { w.bits(READ_SINGLE_WORD) }); let mut timeout = COMMAND_TIMEOUT; while flash.int_status.read().done().bit_is_clear() { timeout -= 1; if timeout == 0 { return Err(FlashError::Timeout); } } let status = flash.int_status.read(); if status.err().bit_is_set() { return Err(FlashError::IllegalCommand); } if status.fail().bit_is_set() { return Err(FlashError::Failed); } let mut word = [0u8; FLASH_WORD_SIZE]; for (chunk, register) in word.chunks_mut(4).zip(flash.dataw.iter()) { chunk.copy_from_slice(®ister.read().bits().to_ne_bytes()); } Ok(word) } fn read_flash(address: usize, buf: &mut [u8]) -> Result<(), FlashError> { // Safety: the flash controller registers are only ever accessed here and in the HAL, never concurrently. let flash = unsafe { &*pac::FLASH::ptr() }; for (index, chunk) in buf.chunks_mut(FLASH_WORD_SIZE).enumerate() { let word = read_word(flash, address + index * FLASH_WORD_SIZE)?; chunk.copy_from_slice(&word[..chunk.len()]); } Ok(()) } fn read_active_slot() -> Option { let mut page = [0u8; FLASH_PAGE_SIZE]; read_flash(slot_page_address(), &mut page).ok()?; if u32::from_le_bytes([page[0], page[1], page[2], page[3]]) != SLOT_MAGIC { return None; } let slots = &page[4..8]; let index = slots[0]; if slots.iter().any(|&slot| slot != index) || usize::from(index) >= CONFIG_SLOT_COUNT { return None; } Some(usize::from(index)) } fn load_slot(index: usize) -> Option { let buf = config_buf(); if let Err(error) = read_flash(slot_address(index), buf) { defmt::error!("config: slot {} unreadable: {}", index, error); return None; } match ConfigContainer::parse(&buf[..]) { Ok((container, _)) => match DeviceConfig::try_from(&container.config) { Ok(config) => Some(config), Err(error) => { defmt::warn!("config: slot {} invalid: {}", index, error); None } }, Err(error) => { defmt::debug!("config: slot {} skipped: {}", index, error); None } } } fn device_serial() -> Option> { DeviceConfig::serial_from_uuid(&hal::uuid()) } fn default_config() -> DeviceConfig { match DeviceConfig::with_defaults(device_serial()) { Ok(config) => config, Err(_) => halt(), } } fn write_slot( flash: &mut FlashGordon, index: usize, container: &ConfigContainer, ) -> Result<(), ()> { let buf = config_buf(); let len = container.encode(buf).map_err(|_| ())?; let base = slot_address(index); let pages = len.div_ceil(FLASH_PAGE_SIZE); for page in 0..pages { let address = base + page * FLASH_PAGE_SIZE; flash .erase_page(address / FLASH_PAGE_SIZE) .map_err(|_| ())?; } for page in 0..pages { let start = page * FLASH_PAGE_SIZE; let end = (start + FLASH_PAGE_SIZE).min(len); let mut page_buf = [0xffu8; FLASH_PAGE_SIZE]; page_buf[..end - start].copy_from_slice(&buf[start..end]); flash .write_native(base + start, GenericArray::from_slice(&page_buf)) .map_err(|_| ())?; } Ok(()) } fn write_active_slot(flash: &mut FlashGordon, index: usize) -> Result<(), ()> { let address = slot_page_address(); let mut page = [0xffu8; FLASH_PAGE_SIZE]; page[..4].copy_from_slice(&SLOT_MAGIC.to_le_bytes()); page[4..8].fill(index as u8); flash .erase_page(address / FLASH_PAGE_SIZE) .map_err(|_| ())?; flash .write_native(address, GenericArray::from_slice(&page)) .map_err(|_| ())?; Ok(()) } fn slow_clock_for_flash() { // Safety: the device resets after writing, so no peripheral depends on the core clock source. let syscon = unsafe { &*pac::SYSCON::ptr() }; syscon.fmccr.modify(|_, w| w.flashtim().flashtim11()); syscon.mainclksela.modify(|_, w| w.sel().enum_0x3()); syscon.mainclkselb.modify(|_, w| w.sel().enum_0x0()); syscon.ahbclkdiv.modify(|_, w| unsafe { w.div().bits(0) }); } fn persist_and_reset( flash: &mut FlashGordon, slot: usize, container: &ConfigContainer, ) -> Result<(), ()> { defmt::info!("config: persisting to slot {}", slot); cortex_m::interrupt::free(|_| { slow_clock_for_flash(); if write_slot(flash, slot, container).is_err() { defmt::error!("config: slot write failed"); return; } if write_active_slot(flash, slot).is_err() { defmt::error!("config: slot page write failed"); return; } cortex_m::peripheral::SCB::sys_reset(); }); Err(()) } fn create_default(flash: &mut FlashGordon) -> DeviceConfig { let config = default_config(); let Ok(tlvs) = config.to_tlvs() else { return config; }; defmt::info!("config: creating default"); let container = ConfigContainer::new(ConfigVersion::V1(ConfigV1 { tlvs })); let _ = persist_and_reset(flash, 0, &container); config } #[cfg(feature = "hid")] fn commit_tlvs(state: &mut FlashState, config: &ConfigState, tlvs: TlvList) -> Result<(), ()> { let next_slot = (usize::from(config.active_slot) + 1) % CONFIG_SLOT_COUNT; let container = ConfigContainer::new(ConfigVersion::V1(ConfigV1 { tlvs })); persist_and_reset(&mut state.flash, next_slot, &container) } #[cfg(feature = "hid")] pub fn commit(state: &mut FlashState, config: &ConfigState) -> Result<(), ()> { commit_tlvs(state, config, config.running.to_tlvs().map_err(|_| ())?) } #[cfg(feature = "hid")] pub fn commit_defaults(state: &mut FlashState, config: &ConfigState) -> Result<(), ()> { commit_tlvs(state, config, default_config().to_tlvs().map_err(|_| ())?) } pub fn load(flash: &mut FlashGordon) -> (DeviceConfig, u8) { slow_clock_for_flash(); let active = read_active_slot(); if let Some(index) = active && let Some(config) = load_slot(index) { return (config, index as u8); } for index in (0..CONFIG_SLOT_COUNT).rev() { if Some(index) == active { continue; } if let Some(config) = load_slot(index) { return (config, index as u8); } } (create_default(flash), 0) }