diff --git a/Cargo.toml b/Cargo.toml index 7c743ec..f7041e9 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,7 +1,8 @@ [workspace] resolver = "2" -members = [ - "examples/simple", +members = [ + "bootloader", + "examples/simple", "flashloader", "va416xx", "va416xx-hal", "vorago-peb1" @@ -25,3 +26,14 @@ incremental = false lto = 'fat' opt-level = 3 # <- overflow-checks = false # <- +strip = true + +[profile.small] +inherits = "release" +codegen-units = 1 +debug-assertions = false # <- +lto = true +opt-level = 'z' # <- +overflow-checks = false # <- +strip = true + diff --git a/bootloader/Cargo.toml b/bootloader/Cargo.toml new file mode 100644 index 0000000..d74063d --- /dev/null +++ b/bootloader/Cargo.toml @@ -0,0 +1,16 @@ +[package] +name = "bootloader" +version = "0.1.0" +edition = "2021" + +[dependencies] +cortex-m = "0.7" +cortex-m-rt = "0.7" +embedded-hal = "1" +panic-rtt-target = { version = "0.1.3" } +rtt-target = { version = "0.5" } +crc = "3" + +[dependencies.va416xx-hal] +path = "../va416xx-hal" +version = "0.1.0" diff --git a/bootloader/src/main.rs b/bootloader/src/main.rs new file mode 100644 index 0000000..b29efc4 --- /dev/null +++ b/bootloader/src/main.rs @@ -0,0 +1,260 @@ +//! Vorago bootloader which can boot from two images. +//! +//! Bootloader memory map +//! +//! * <0x0> Bootloader start +//! * <0x3FFE> Bootloader CRC +//! * <0x4000> App image A start +//! * <0x21FFC> App image A CRC check length +//! * <0x21FFE> App image A CRC check value +//! * <0x22000> App image B start +//! * <0x3FFFC> App image B CRC check length +//! * <0x3FFFE> App image B CRC check value +//! * <0x40000> +//! +//! As opposed to the Vorago example code, this bootloader assumes a 40 MHz external clock +//! but does not scale that clock up. +#![no_main] +#![no_std] + +use cortex_m_rt::entry; +use crc::{Crc, CRC_16_IBM_3740}; +use panic_rtt_target as _; +use rtt_target::{rprintln, rtt_init_print}; +use va416xx_hal::{ + clock::{pll_setup_delay, ClkDivSel, ClkselSys}, + edac, + nvm::Nvm, + pac::{self, interrupt}, + prelude::*, + time::Hertz, + wdt::Wdt, +}; + +const EXTCLK_FREQ: u32 = 40_000_000; +const WITH_WDT: bool = true; +const WDT_FREQ_MS: u32 = 50; +const DEBUG_PRINTOUTS: bool = true; + +// Important bootloader addresses and offsets, vector table information. + +const BOOTLOADER_START_ADDR: u32 = 0x0; +const BOOTLOADER_END_ADDR: u32 = 0x4000; +const BOOTLOADER_CRC_ADDR: u32 = 0x3FFE; +const APP_A_START_ADDR: u32 = 0x4000; +pub const APP_A_END_ADDR: u32 = 0x22000; +// The actual size of the image which is relevant for CRC calculation. +const APP_A_SIZE_ADDR: u32 = 0x21FF8; +const APP_A_CRC_ADDR: u32 = 0x21FFC; +const APP_B_START_ADDR: u32 = 0x22000; +pub const APP_B_END_ADDR: u32 = 0x40000; +// The actual size of the image which is relevant for CRC calculation. +const APP_B_SIZE_ADDR: u32 = 0x3FFF8; +const APP_B_CRC_ADDR: u32 = 0x3FFFC; +pub const APP_IMG_SZ: u32 = 0x1E000; + +pub const VECTOR_TABLE_OFFSET: u32 = 0x0; +pub const VECTOR_TABLE_LEN: u32 = 0x350; +pub const RESET_VECTOR_OFFSET: u32 = 0x4; + +const CRC_ALGO: Crc = Crc::::new(&CRC_16_IBM_3740); + +#[derive(Debug, Copy, Clone, PartialEq, Eq)] +enum AppSel { + A, + B, +} + +pub trait WdtInterface { + fn feed(&self); +} + +pub struct OptWdt(Option); + +impl WdtInterface for OptWdt { + fn feed(&self) { + if self.0.is_some() { + self.0.as_ref().unwrap().feed(); + } + } +} + +#[entry] +fn main() -> ! { + rtt_init_print!(); + rprintln!("-- VA416xx bootloader --"); + let mut dp = pac::Peripherals::take().unwrap(); + let cp = cortex_m::Peripherals::take().unwrap(); + setup_edac(&mut dp.sysconfig); + // Use the external clock connected to XTAL_N. + let clocks = dp + .clkgen + .constrain() + .xtal_n_clk_with_src_freq(Hertz::from_raw(EXTCLK_FREQ)) + .freeze(&mut dp.sysconfig) + .unwrap(); + let mut opt_wdt = OptWdt(None); + if WITH_WDT { + opt_wdt.0 = Some(Wdt::start( + &mut dp.sysconfig, + dp.watch_dog, + &clocks, + WDT_FREQ_MS, + )); + } + let nvm = Nvm::new(&mut dp.sysconfig, dp.spi3, &clocks); + // Check bootloader's CRC (and write it if blank) + check_own_crc(&opt_wdt, &nvm, &cp); + + if check_app_crc(AppSel::A, &opt_wdt) { + boot_app(AppSel::A, &cp) + } else if check_app_crc(AppSel::B, &opt_wdt) { + boot_app(AppSel::B, &cp) + } else { + if DEBUG_PRINTOUTS { + rprintln!("both images corrupt! booting image A"); + } + // TODO: Shift a CCSDS packet out to inform host/OBC about image corruption. + // Both images seem to be corrupt. Boot default image A. + boot_app(AppSel::A, &cp) + } +} + +fn check_own_crc(wdt: &OptWdt, nvm: &Nvm, cp: &cortex_m::Peripherals) { + let crc_exp = unsafe { *(BOOTLOADER_CRC_ADDR as *const u16) }; + wdt.feed(); + let crc_calc = CRC_ALGO.checksum(unsafe { + core::slice::from_raw_parts( + BOOTLOADER_START_ADDR as *const u8, + (BOOTLOADER_END_ADDR - BOOTLOADER_START_ADDR - 4) as usize, + ) + }); + wdt.feed(); + if crc_exp == 0x0000 || crc_exp == 0xffff { + if DEBUG_PRINTOUTS { + rprintln!("BL CRC blank - prog new CRC"); + } + // Blank CRC, write it to NVM. + nvm.write_data(BOOTLOADER_CRC_ADDR, &crc_calc.to_be_bytes()); + // The Vorago bootloader resets here. I am not sure why this is done, just continue with + // the regular boot process.. + } else if crc_exp != crc_calc { + // Bootloader is corrupted. Try to run App A. + if DEBUG_PRINTOUTS { + rprintln!("bootloader CRC corrupt. booting image A immediately"); + } + // TODO: Shift out minimal CCSDS frame to notify about bootloader corruption. + boot_app(AppSel::A, cp); + } +} + +fn check_app_crc(app_sel: AppSel, wdt: &OptWdt) -> bool { + if app_sel == AppSel::A { + check_app_given_addr(APP_A_CRC_ADDR, APP_A_START_ADDR, APP_A_SIZE_ADDR, wdt) + } else { + check_app_given_addr(APP_B_CRC_ADDR, APP_B_START_ADDR, APP_B_SIZE_ADDR, wdt) + } +} + +fn check_app_given_addr( + crc_addr: u32, + start_addr: u32, + image_size_addr: u32, + wdt: &OptWdt, +) -> bool { + let crc_exp = unsafe { *(crc_addr as *const u16) }; + let image_size = unsafe { *(image_size_addr as *const u32) }; + wdt.feed(); + let crc_calc = CRC_ALGO.checksum(unsafe { + core::slice::from_raw_parts(start_addr as *const u8, image_size as usize) + }); + wdt.feed(); + if crc_calc == crc_exp { + return true; + } + false +} + +fn boot_app(app_sel: AppSel, cp: &cortex_m::Peripherals) -> ! { + let clkgen = unsafe { pac::Clkgen::steal() }; + clkgen + .ctrl0() + .modify(|_, w| unsafe { w.clksel_sys().bits(ClkselSys::Hbo as u8) }); + pll_setup_delay(); + clkgen + .ctrl0() + .modify(|_, w| unsafe { w.clk_div_sel().bits(ClkDivSel::Div1 as u8) }); + // Clear all interrupts set. + unsafe { + cp.NVIC.icer[0].write(0xFFFFFFFF); + cp.NVIC.icpr[0].write(0xFFFFFFFF); + } + cortex_m::asm::dsb(); + cortex_m::asm::isb(); + unsafe { + if app_sel == AppSel::A { + cp.SCB.vtor.write(APP_A_START_ADDR); + } else { + cp.SCB.vtor.write(APP_B_START_ADDR); + } + } + cortex_m::asm::dsb(); + cortex_m::asm::isb(); + vector_reset(); +} + +pub fn vector_reset() -> ! { + unsafe { + // Set R0 to VTOR address (0xE000ED08) + let vtor_address: u32 = 0xE000ED08; + + // Load VTOR + let vtor: u32 = *(vtor_address as *const u32); + + // Load initial MSP value + let initial_msp: u32 = *(vtor as *const u32); + + // Set SP value (assume MSP is selected) + core::arch::asm!("mov sp, {0}", in(reg) initial_msp); + + // Load reset vector + let reset_vector: u32 = *((vtor + 4) as *const u32); + + // Branch to reset handler + core::arch::asm!("bx {0}", in(reg) reset_vector); + } + unreachable!(); +} + +fn setup_edac(syscfg: &mut pac::Sysconfig) { + // The scrub values are based on the Vorago provided bootloader. + edac::enable_rom_scrub(syscfg, 125); + edac::enable_ram0_scrub(syscfg, 1000); + edac::enable_ram1_scrub(syscfg, 1000); + edac::enable_sbe_irq(); + edac::enable_mbe_irq(); +} + +#[interrupt] +#[allow(non_snake_case)] +fn WATCHDOG() { + let wdt = unsafe { pac::WatchDog::steal() }; + // Clear interrupt. + wdt.wdogintclr().write(|w| unsafe { w.bits(1) }); +} + +#[interrupt] +#[allow(non_snake_case)] +fn EDAC_SBE() { + // TODO: Send some command via UART for notification purposes. Also identify the problematic + // memory. + edac::clear_sbe_irq(); +} + +#[interrupt] +#[allow(non_snake_case)] +fn EDAC_MBE() { + // TODO: Send some command via UART for notification purposes. + edac::clear_mbe_irq(); + // TODO: Reset like the vorago example? +} diff --git a/examples/simple/Cargo.toml b/examples/simple/Cargo.toml index 130ca39..0320c7a 100644 --- a/examples/simple/Cargo.toml +++ b/examples/simple/Cargo.toml @@ -9,6 +9,7 @@ va416xx-hal = { path = "../../va416xx-hal" } panic-rtt-target = { version = "0.1.3" } rtt-target = { version = "0.5" } cortex-m = { version = "0.7", features = ["critical-section-single-core"] } +rtic-sync = { version = "1.3", features = ["defmt-03"] } embedded-hal = "1" embedded-hal-nb = "1" nb = "1" @@ -16,3 +17,11 @@ embedded-io = "0.6" panic-halt = "0.2" vorago-peb1 = { path = "../../vorago-peb1" } accelerometer = "0.12" + +[dependencies.rtic] +version = "2" +features = ["thumbv6-backend"] + +[dependencies.rtic-monotonics] +version = "1" +features = ["cortex-m-systick"] diff --git a/examples/simple/examples/rtic-empty.rs b/examples/simple/examples/rtic-empty.rs new file mode 100644 index 0000000..be6c109 --- /dev/null +++ b/examples/simple/examples/rtic-empty.rs @@ -0,0 +1,30 @@ +//! Empty RTIC project template +#![no_main] +#![no_std] + +#[rtic::app(device = pac)] +mod app { + use panic_rtt_target as _; + use rtt_target::{rprintln, rtt_init_default}; + use va416xx_hal::pac; + + #[local] + struct Local {} + + #[shared] + struct Shared {} + + #[init] + fn init(_ctx: init::Context) -> (Shared, Local) { + rtt_init_default!(); + rprintln!("-- Vorago RTIC template --"); + (Shared {}, Local {}) + } + + // `shared` cannot be accessed from this context + #[idle] + fn idle(_cx: idle::Context) -> ! { + #[allow(clippy::empty_loop)] + loop {} + } +} diff --git a/flashloader/Cargo.toml b/flashloader/Cargo.toml new file mode 100644 index 0000000..8bce51c --- /dev/null +++ b/flashloader/Cargo.toml @@ -0,0 +1,24 @@ +[package] +name = "flashloader" +version = "0.1.0" +edition = "2021" + +[dependencies] +cortex-m = "0.7" +cortex-m-rt = "0.7" +embedded-hal = "1" +panic-rtt-target = { version = "0.1.3" } +rtt-target = { version = "0.5" } +crc = "3" + +[dependencies.va416xx-hal] +path = "../va416xx-hal" +version = "0.1.0" + +[dependencies.rtic] +version = "2" +features = ["thumbv7-backend"] + +[dependencies.rtic-monotonics] +version = "1" +features = ["cortex-m-systick"] diff --git a/flashloader/src/main.rs b/flashloader/src/main.rs new file mode 100644 index 0000000..f1ac91f --- /dev/null +++ b/flashloader/src/main.rs @@ -0,0 +1,150 @@ +//! Vorago flashloader which can be used to flash image A and image B via a simple +//! low-level CCSDS memory interface via a UART wire. +//! +//! This flash loader can be used after the bootloader was flashed to flash the images. +//! You can also use this as an starting application for a software update mechanism. +//! +//! Bootloader memory map +//! +//! * <0x0> Bootloader start +//! * <0x3FFE> Bootloader CRC +//! * <0x4000> App image A start +//! * <0x21FFC> App image A CRC check length +//! * <0x21FFE> App image A CRC check value +//! * <0x22000> App image B start +//! * <0x3FFFC> App image B CRC check length +//! * <0x3FFFE> App image B CRC check value +//! * <0x40000> +#![no_main] +#![no_std] + +use crc::{Crc, CRC_16_IBM_3740}; +use panic_rtt_target as _; +use va416xx_hal::{ + edac, + pac::{self}, + time::Hertz, + wdt::Wdt, +}; + +const EXTCLK_FREQ: u32 = 40_000_000; +const WITH_WDT: bool = true; +const WDT_FREQ_MS: u32 = 50; +const DEBUG_PRINTOUTS: bool = true; + +// Important bootloader addresses and offsets, vector table information. + +const BOOTLOADER_START_ADDR: u32 = 0x0; +const BOOTLOADER_END_ADDR: u32 = 0x4000; +const BOOTLOADER_CRC_ADDR: u32 = 0x3FFE; +const APP_A_START_ADDR: u32 = 0x4000; +pub const APP_A_END_ADDR: u32 = 0x22000; +// The actual size of the image which is relevant for CRC calculation. +const APP_A_SIZE_ADDR: u32 = 0x21FF8; +const APP_A_CRC_ADDR: u32 = 0x21FFC; +const APP_B_START_ADDR: u32 = 0x22000; +pub const APP_B_END_ADDR: u32 = 0x40000; +// The actual size of the image which is relevant for CRC calculation. +const APP_B_SIZE_ADDR: u32 = 0x3FFF8; +const APP_B_CRC_ADDR: u32 = 0x3FFFC; +pub const APP_IMG_SZ: u32 = 0x1E000; + +pub const VECTOR_TABLE_OFFSET: u32 = 0x0; +pub const VECTOR_TABLE_LEN: u32 = 0x350; +pub const RESET_VECTOR_OFFSET: u32 = 0x4; + +const CRC_ALGO: Crc = Crc::::new(&CRC_16_IBM_3740); + +#[derive(Debug, Copy, Clone, PartialEq, Eq)] +enum AppSel { + A, + B, +} + +pub trait WdtInterface { + fn feed(&self); +} + +pub struct OptWdt(Option); + +impl WdtInterface for OptWdt { + fn feed(&self) { + if self.0.is_some() { + self.0.as_ref().unwrap().feed(); + } + } +} + +#[rtic::app(device = pac)] +mod app { + use super::*; + use panic_rtt_target as _; + use rtic_monotonics::systick::Systick; + use rtt_target::{rprintln, rtt_init_default}; + use va416xx_hal::{clock::ClkgenExt, edac, pac}; + + use crate::{setup_edac, EXTCLK_FREQ}; + + #[local] + struct Local {} + + #[shared] + struct Shared {} + + #[init] + fn init(mut cx: init::Context) -> (Shared, Local) { + rtt_init_default!(); + rprintln!("-- Vorago flashloader --"); + // Initialize the systick interrupt & obtain the token to prove that we did + let systick_mono_token = rtic_monotonics::create_systick_token!(); + // Use the external clock connected to XTAL_N. + let clocks = cx + .device + .clkgen + .constrain() + .xtal_n_clk_with_src_freq(Hertz::from_raw(EXTCLK_FREQ)) + .freeze(&mut cx.device.sysconfig) + .unwrap(); + setup_edac(&mut cx.device.sysconfig); + + Systick::start(cx.core.SYST, clocks.sysclk().raw(), systick_mono_token); + (Shared {}, Local {}) + } + + // `shared` cannot be accessed from this context + #[idle] + fn idle(_cx: idle::Context) -> ! { + #[allow(clippy::empty_loop)] + loop {} + } + + #[task(binds = EDAC_SBE, priority = 1)] + fn edac_sbe_isr(_cx: edac_sbe_isr::Context) { + // TODO: Send some command via UART for notification purposes. Also identify the problematic + // memory. + edac::clear_sbe_irq(); + } + + #[task(binds = EDAC_MBE, priority = 1)] + fn edac_mbe_isr(_cx: edac_mbe_isr::Context) { + // TODO: Send some command via UART for notification purposes. + edac::clear_mbe_irq(); + // TODO: Reset like the vorago example? + } + + #[task(binds = WATCHDOG, priority = 1)] + fn watchdog_isr(_cx: watchdog_isr::Context) { + let wdt = unsafe { pac::WatchDog::steal() }; + // Clear interrupt. + wdt.wdogintclr().write(|w| unsafe { w.bits(1) }); + } +} + +fn setup_edac(syscfg: &mut pac::Sysconfig) { + // The scrub values are based on the Vorago provided bootloader. + edac::enable_rom_scrub(syscfg, 125); + edac::enable_ram0_scrub(syscfg, 1000); + edac::enable_ram1_scrub(syscfg, 1000); + edac::enable_sbe_irq(); + edac::enable_mbe_irq(); +} diff --git a/va416xx-hal/src/edac.rs b/va416xx-hal/src/edac.rs new file mode 100644 index 0000000..4333d07 --- /dev/null +++ b/va416xx-hal/src/edac.rs @@ -0,0 +1,66 @@ +use crate::{enable_interrupt, pac}; + +#[inline(always)] +pub fn enable_rom_scrub(syscfg: &mut pac::Sysconfig, counter_reset: u16) { + syscfg + .rom_scrub() + .write(|w| unsafe { w.bits(counter_reset as u32) }) +} + +#[inline(always)] +pub fn enable_ram0_scrub(syscfg: &mut pac::Sysconfig, counter_reset: u16) { + syscfg + .ram0_scrub() + .write(|w| unsafe { w.bits(counter_reset as u32) }) +} + +#[inline(always)] +pub fn enable_ram1_scrub(syscfg: &mut pac::Sysconfig, counter_reset: u16) { + syscfg + .ram1_scrub() + .write(|w| unsafe { w.bits(counter_reset as u32) }) +} + +/// This function enables the SBE related interrupts. The user should also provide a +/// [pac::EDAC_SBE] ISR and use [clear_sbe_irq] inside that ISR at the very least. +#[inline(always)] +pub fn enable_sbe_irq() { + unsafe { + enable_interrupt(pac::Interrupt::EDAC_SBE); + } +} + +/// This function enables the SBE related interrupts. The user should also provide a +/// [pac::EDAC_MBE] ISR and use [clear_mbe_irq] inside that ISR at the very least. +#[inline(always)] +pub fn enable_mbe_irq() { + unsafe { + enable_interrupt(pac::Interrupt::EDAC_MBE); + } +} + +/// This function should be called in the user provided [pac::EDAC_SBE] interrupt-service routine +/// to clear the SBE related interrupts. +#[inline(always)] +pub fn clear_sbe_irq() { + // Safety: This function only clears SBE related IRQs + let syscfg = unsafe { pac::Sysconfig::steal() }; + syscfg.irq_clr().write(|w| { + w.romsbe().set_bit(); + w.ram0sbe().set_bit(); + w.ram1sbe().set_bit() + }); +} + +/// This function should be called in the user provided [pac::EDAC_MBE] interrupt-service routine +/// to clear the MBE related interrupts. +#[inline(always)] +pub fn clear_mbe_irq() { + // Safety: This function only clears SBE related IRQs + let syscfg = unsafe { pac::Sysconfig::steal() }; + syscfg.irq_clr().write(|w| { + w.rommbe().set_bit(); + w.ram0mbe().set_bit(); + w.ram1mbe().set_bit() + }); +} diff --git a/va416xx-hal/src/lib.rs b/va416xx-hal/src/lib.rs index 8a61e50..4590aaf 100644 --- a/va416xx-hal/src/lib.rs +++ b/va416xx-hal/src/lib.rs @@ -12,8 +12,10 @@ pub mod adc; pub mod clock; pub mod dac; pub mod dma; +pub mod edac; pub mod gpio; pub mod i2c; +pub mod nvm; pub mod pwm; pub mod spi; pub mod time; diff --git a/va416xx-hal/src/nvm.rs b/va416xx-hal/src/nvm.rs new file mode 100644 index 0000000..72e245e --- /dev/null +++ b/va416xx-hal/src/nvm.rs @@ -0,0 +1,220 @@ +use embedded_hal::spi::MODE_0; + +use crate::clock::{Clocks, SyscfgExt}; +use crate::pac; +use crate::spi::{mode_to_cpo_cph_bit, Instance, BMSTART_BMSTOP_MASK}; + +const NVM_SER_CLOCK_RATE_DIV: u8 = 4; + +// Commands. The internal FRAM is based on the Cypress FM25V20A device. + +/// Write enable register. +pub const FRAM_WREN: u8 = 0x06; +pub const FRAM_WRDI: u8 = 0x04; +pub const FRAM_RDSR: u8 = 0x05; +/// Write single status register +pub const FRAM_WRSR: u8 = 0x01; +pub const FRAM_READ: u8 = 0x03; +pub const FRAM_WRITE: u8 = 0x02; +pub const FRAM_RDID: u8 = 0x9F; +pub const FRAM_SLEEP: u8 = 0xB9; + +/* Address Masks */ +const ADDR_MSB_MASK: u32 = 0xFF0000; +const ADDR_MID_MASK: u32 = 0x00FF00; +const ADDR_LSB_MASK: u32 = 0x0000FF; + +#[inline(always)] +const fn msb_addr_byte(addr: u32) -> u8 { + ((addr & ADDR_MSB_MASK) >> 16) as u8 +} + +#[inline(always)] +const fn mid_addr_byte(addr: u32) -> u8 { + ((addr & ADDR_MID_MASK) >> 8) as u8 +} + +#[inline(always)] +const fn lsb_addr_byte(addr: u32) -> u8 { + (addr & ADDR_LSB_MASK) as u8 +} + +pub const WPEN_ENABLE_MASK: u8 = 1 << 7; +pub const BP_0_ENABLE_MASK: u8 = 1 << 2; +pub const BP_1_ENABLE_MASK: u8 = 1 << 3; + +pub struct Nvm { + spi: pac::Spi3, +} + +#[derive(Debug, PartialEq, Eq)] +#[cfg_attr(feature = "defmt", defmt::Format)] +pub struct VerifyError { + addr: u32, + found: u8, + expected: u8, +} + +impl Nvm { + pub fn new(syscfg: &mut pac::Sysconfig, spi: pac::Spi3, _clocks: &Clocks) -> Self { + crate::clock::enable_peripheral_clock(syscfg, pac::Spi3::PERIPH_SEL); + // This is done in the C HAL. + syscfg.assert_periph_reset_for_two_cycles(pac::Spi3::PERIPH_SEL); + + let (cpo_bit, cph_bit) = mode_to_cpo_cph_bit(MODE_0); + spi.ctrl0().write(|w| { + unsafe { + w.size().bits(8); + w.scrdv().bits(NVM_SER_CLOCK_RATE_DIV); + // Clear clock phase and polarity. Will be set to correct value for each + // transfer + w.spo().bit(cpo_bit); + w.sph().bit(cph_bit) + } + }); + spi.ctrl1().write(|w| { + w.lbm().clear_bit(); + w.sod().clear_bit(); + w.ms().set_bit(); + w.mdlycap().clear_bit(); + w.blockmode().set_bit(); + unsafe { w.ss().bits(0) }; + w.bmstall().set_bit() + }); + + spi.fifo_clr().write(|w| { + w.rxfifo().set_bit(); + w.txfifo().set_bit() + }); + // Enable the peripheral as the last step as recommended in the + // programmers guide + spi.ctrl1().modify(|_, w| w.enable().set_bit()); + + let mut nvm = Self { spi }; + nvm.disable_write_prot(); + nvm + } + + pub fn disable_write_prot(&mut self) { + self.wait_for_tx_idle(); + self.write_with_bmstop(FRAM_WREN); + self.wait_for_tx_idle(); + self.write_single(FRAM_WRSR); + self.write_with_bmstop(0x00); + } + + pub fn enable_write_prot(&mut self) { + self.wait_for_tx_idle(); + self.write_with_bmstop(FRAM_WREN); + self.wait_for_tx_idle(); + self.write_single(FRAM_WRSR); + self.write_with_bmstop(0x00); + } + + #[inline(always)] + pub fn write_single(&self, word: u8) { + self.spi.data().write(|w| unsafe { w.bits(word as u32) }) + } + + #[inline(always)] + pub fn write_with_bmstop(&self, word: u8) { + self.spi + .data() + .write(|w| unsafe { w.bits(BMSTART_BMSTOP_MASK | word as u32) }) + } + + #[inline(always)] + pub fn wait_for_tx_idle(&self) { + while self.spi.status().read().tfe().bit_is_clear() { + cortex_m::asm::nop(); + } + } + + #[inline(always)] + pub fn wait_for_rx_available(&self) { + while !self.spi.status().read().rne().bit_is_set() { + cortex_m::asm::nop(); + } + } + + #[inline(always)] + pub fn read_single_word(&self) -> u32 { + self.spi.data().read().bits() + } + + pub fn write_data(&self, addr: u32, data: &[u8]) { + self.wait_for_tx_idle(); + self.write_with_bmstop(FRAM_WREN); + self.wait_for_tx_idle(); + self.write_single(FRAM_WRITE); + self.write_single(msb_addr_byte(addr)); + self.write_single(mid_addr_byte(addr)); + self.write_single(lsb_addr_byte(addr)); + for byte in data.iter().take(data.len() - 1) { + while self.spi.status().read().tnf().bit_is_clear() { + cortex_m::asm::nop(); + } + self.write_single(*byte); + self.read_single_word(); + } + while self.spi.status().read().tnf().bit_is_clear() { + cortex_m::asm::nop(); + } + self.write_with_bmstop(*data.last().unwrap()); + self.wait_for_tx_idle(); + } + + pub fn read_data(&self, addr: u32, buf: &mut [u8]) { + self.common_read_start(addr); + for byte in buf { + // Pump the SPI. + self.write_single(0); + self.wait_for_rx_available(); + *byte = self.read_single_word() as u8; + } + self.write_with_bmstop(0); + self.wait_for_tx_idle(); + } + + pub fn verify_data(&self, addr: u32, comp_buf: &[u8]) -> Result<(), VerifyError> { + self.common_read_start(addr); + for (idx, byte) in comp_buf.iter().enumerate() { + // Pump the SPI. + self.write_single(0); + self.wait_for_rx_available(); + let next_word = self.read_single_word() as u8; + if next_word != *byte { + return Err(VerifyError { + addr: addr + idx as u32, + found: next_word, + expected: *byte, + }); + } + } + Ok(()) + } + + /// This function releases the ROM SPI and enables chip write protection again. + pub fn release(self) -> pac::Spi3 { + self.wait_for_tx_idle(); + self.write_with_bmstop(FRAM_WREN); + self.wait_for_tx_idle(); + self.write_single(WPEN_ENABLE_MASK | BP_0_ENABLE_MASK | BP_1_ENABLE_MASK); + self.spi + } + + fn common_read_start(&self, addr: u32) { + self.wait_for_tx_idle(); + self.write_single(FRAM_READ); + self.write_single(msb_addr_byte(addr)); + self.write_single(mid_addr_byte(addr)); + self.write_single(lsb_addr_byte(addr)); + for _ in 0..4 { + // Pump the SPI. + self.write_single(0); + self.wait_for_rx_available(); + // The first 4 data bytes received need to be ignored. + self.read_single_word(); + } + } +} diff --git a/va416xx-hal/src/spi.rs b/va416xx-hal/src/spi.rs index 81dbeb5..42f7574 100644 --- a/va416xx-hal/src/spi.rs +++ b/va416xx-hal/src/spi.rs @@ -27,6 +27,9 @@ use crate::{ // FIFO has a depth of 16. const FILL_DEPTH: usize = 12; +pub const BMSTART_BMSTOP_MASK: u32 = 1 << 31; +pub const BMSKIPDATA_MASK: u32 = 1 << 30; + #[derive(Debug, PartialEq, Eq, Copy, Clone)] pub enum HwChipSelectId { Id0 = 0, @@ -99,6 +102,14 @@ impl OptionalHwCs for NoneT {} impl OptionalHwCs for NoneT {} impl OptionalHwCs for NoneT {} +pub struct RomSpiSck; +pub struct RomSpiMiso; +pub struct RomSpiMosi; + +impl Sealed for RomSpiSck {} +impl Sealed for RomSpiMosi {} +impl Sealed for RomSpiMiso {} + // SPI 0 impl PinSck for Pin {} @@ -137,6 +148,10 @@ impl PinMosi for Pin {} impl PinMiso for Pin {} // SPI3 is shared with the ROM SPI pins and has its own dedicated pins. +// +impl PinSck for RomSpiSck {} +impl PinMosi for RomSpiMosi {} +impl PinMiso for RomSpiMiso {} // SPI 0 HW CS pins @@ -315,6 +330,11 @@ impl SpiConfig { self } + pub fn ser_clock_rate_div(mut self, div: u8) -> Self { + self.ser_clock_rate_div = div; + self + } + pub fn master_mode(mut self, master: bool) -> Self { self.ms = !master; self @@ -391,6 +411,16 @@ impl Instance for pac::Spi2 { } } +impl Instance for pac::Spi3 { + const IDX: u8 = 3; + const PERIPH_SEL: PeripheralSelect = PeripheralSelect::Spi3; + + #[inline(always)] + fn ptr() -> *const SpiRegBlock { + Self::ptr() + } +} + //================================================================================================== // Spi //================================================================================================== @@ -410,7 +440,7 @@ pub struct Spi { pins: Pins, } -fn mode_to_cpo_cph_bit(mode: embedded_hal::spi::Mode) -> (bool, bool) { +pub fn mode_to_cpo_cph_bit(mode: embedded_hal::spi::Mode) -> (bool, bool) { match mode { embedded_hal::spi::MODE_0 => (false, false), embedded_hal::spi::MODE_1 => (false, true), @@ -441,6 +471,11 @@ where }); } + #[inline] + pub fn spi_instance(&self) -> &SpiInstance { + &self.spi + } + #[inline] pub fn clear_tx_fifo(&self) { self.spi.fifo_clr().write(|w| w.txfifo().set_bit()); @@ -601,11 +636,11 @@ where /// to be done once. /// * `syscfg` - Can be passed optionally to enable the peripheral clock pub fn new( - spi: SpiI, - pins: (Sck, Miso, Mosi), - clocks: &crate::clock::Clocks, - spi_cfg: SpiConfig, syscfg: &mut pac::Sysconfig, + spi: SpiI, + clocks: &crate::clock::Clocks, + pins: (Sck, Miso, Mosi), + spi_cfg: SpiConfig, transfer_cfg: Option<&ErasedTransferConfig>, ) -> Self { crate::clock::enable_peripheral_clock(syscfg, SpiI::PERIPH_SEL); @@ -674,33 +709,35 @@ where } } - #[inline] - pub fn cfg_clock(&mut self, spi_clk: impl Into) { - self.inner.cfg_clock(spi_clk); + delegate::delegate! { + to self.inner { + #[inline] + pub fn cfg_clock(&mut self, spi_clk: impl Into); + + #[inline] + pub fn spi_instance(&self) -> &SpiI; + + #[inline] + pub fn cfg_mode(&mut self, mode: Mode); + + #[inline] + pub fn perid(&self) -> u32; + + pub fn cfg_transfer>(&mut self, transfer_cfg: &TransferConfig); + + } } #[inline] - pub fn cfg_mode(&mut self, mode: Mode) { - self.inner.cfg_mode(mode); - } - pub fn set_fill_word(&mut self, fill_word: Word) { self.inner.fill_word = fill_word; } + #[inline] pub fn fill_word(&self) -> Word { self.inner.fill_word } - #[inline] - pub fn perid(&self) -> u32 { - self.inner.perid() - } - - pub fn cfg_transfer>(&mut self, transfer_cfg: &TransferConfig) { - self.inner.cfg_transfer(transfer_cfg); - } - /// Releases the SPI peripheral and associated pins pub fn release(self) -> (SpiI, (Sck, Miso, Mosi), SpiConfig) { (self.inner.spi, self.pins, self.inner.cfg) diff --git a/va416xx-hal/src/wdt.rs b/va416xx-hal/src/wdt.rs index 3875aaa..605faf6 100644 --- a/va416xx-hal/src/wdt.rs +++ b/va416xx-hal/src/wdt.rs @@ -40,7 +40,6 @@ pub fn disable_wdt_interrupts() { impl Wdt { pub fn new( - &self, syscfg: &mut pac::Sysconfig, wdt: pac::WatchDog, clocks: &Clocks,