From c53702ee742c6050906f8f0384746dd8fc1716d4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 7 Jul 2024 13:48:50 +0200 Subject: [PATCH] BL/FL progress --- bootloader/src/main.rs | 62 +++++++++- examples/simple/Cargo.toml | 2 +- examples/simple/examples/spi.rs | 18 +-- flashloader/.gitignore | 1 + flashloader/Cargo.toml | 2 +- flashloader/image-loader.py | 86 ++++++++++++++ flashloader/loader.toml | 1 + flashloader/requirements.txt | 3 + flashloader/src/main.rs | 175 +++++++++++++++++---------- va416xx-hal/src/nvm.rs | 44 +++++-- va416xx-hal/src/spi.rs | 201 +++++++++++++++++++++++++------- va416xx-hal/src/uart.rs | 72 +++++++++--- 12 files changed, 527 insertions(+), 140 deletions(-) create mode 100644 flashloader/.gitignore create mode 100755 flashloader/image-loader.py create mode 100644 flashloader/loader.toml create mode 100644 flashloader/requirements.txt diff --git a/bootloader/src/main.rs b/bootloader/src/main.rs index 7866166..d81898a 100644 --- a/bootloader/src/main.rs +++ b/bootloader/src/main.rs @@ -32,10 +32,17 @@ use va416xx_hal::{ }; const EXTCLK_FREQ: u32 = 40_000_000; -const WITH_WDT: bool = true; +const WITH_WDT: bool = false; const WDT_FREQ_MS: u32 = 50; const DEBUG_PRINTOUTS: bool = true; +// Dangerous option! An image with this option set to true will flash itself from RAM directly +// into the NVM. This can be used as a recovery option from a direct RAM flash to fix the NVM +// boot process. Please note that this will flash an image which will also always perform the +// self-flash itself. It is recommended that you use a tool like probe-rs, Keil IDE, or a flash +// loader to boot a bootloader without this feature. +const FLASH_SELF: bool = false; + // Important bootloader addresses and offsets, vector table information. const BOOTLOADER_START_ADDR: u32 = 0x0; @@ -85,6 +92,8 @@ fn main() -> ! { rprintln!("-- VA416xx bootloader --"); let mut dp = pac::Peripherals::take().unwrap(); let cp = cortex_m::Peripherals::take().unwrap(); + // Disable ROM protection. + dp.sysconfig.rom_prot().write(|w| unsafe { w.bits(1) }); setup_edac(&mut dp.sysconfig); // Use the external clock connected to XTAL_N. let clocks = dp @@ -103,6 +112,48 @@ fn main() -> ! { )); } let nvm = Nvm::new(&mut dp.sysconfig, dp.spi3, &clocks); + + if FLASH_SELF { + let bootloader_data = { + unsafe { + &*core::ptr::slice_from_raw_parts( + (BOOTLOADER_START_ADDR + 4) as *const u8, + (BOOTLOADER_END_ADDR - BOOTLOADER_START_ADDR - 8) as usize, + ) + } + }; + let mut first_four_bytes: [u8; 4] = [0; 4]; + unsafe { + core::arch::asm!( + "ldr r0, [{0}]", // Load 4 bytes from src into r0 register + "str r0, [{1}]", // Store r0 register into first_four_bytes + in(reg) BOOTLOADER_START_ADDR as *const u8, // Input: src pointer (0x0) + in(reg) &mut first_four_bytes as *mut [u8; 4], // Input: destination pointer + ); + } + let mut digest = CRC_ALGO.digest(); + digest.update(&first_four_bytes); + digest.update(bootloader_data); + let bootloader_crc = digest.finalize(); + + nvm.write_data(0x0, &first_four_bytes); + nvm.write_data(0x4, bootloader_data); + if let Err(e) = nvm.verify_data(0x0, &first_four_bytes) { + rprintln!("verification of self-flash to NVM failed: {:?}", e); + } + if let Err(e) = nvm.verify_data(0x4, bootloader_data) { + rprintln!("verification of self-flash to NVM failed: {:?}", e); + } + + nvm.write_data(BOOTLOADER_CRC_ADDR, &bootloader_crc.to_be_bytes()); + if let Err(e) = nvm.verify_data(BOOTLOADER_CRC_ADDR, &bootloader_crc.to_be_bytes()) { + rprintln!( + "error: CRC verification for bootloader self-flash failed: {:?}", + e + ); + } + } + // Check bootloader's CRC (and write it if blank) check_own_crc(&opt_wdt, &nvm, &cp); @@ -114,7 +165,6 @@ fn main() -> ! { if DEBUG_PRINTOUTS { rprintln!("both images corrupt! booting image A"); } - loop {} // 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) @@ -124,8 +174,11 @@ fn main() -> ! { fn check_own_crc(wdt: &OptWdt, nvm: &Nvm, cp: &cortex_m::Peripherals) { let crc_exp = unsafe { *(BOOTLOADER_CRC_ADDR as *const u16) }; wdt.feed(); + // I'd prefer to use [core::slice::from_raw_parts], but that is problematic + // because the address of the bootloader is 0x0, so the NULL check fails and the functions + // panics. let crc_calc = CRC_ALGO.checksum(unsafe { - core::slice::from_raw_parts( + &*core::ptr::slice_from_raw_parts( BOOTLOADER_START_ADDR as *const u8, (BOOTLOADER_END_ADDR - BOOTLOADER_START_ADDR - 4) as usize, ) @@ -150,6 +203,9 @@ fn check_own_crc(wdt: &OptWdt, nvm: &Nvm, cp: &cortex_m::Peripherals) { } fn check_app_crc(app_sel: AppSel, wdt: &OptWdt) -> bool { + if DEBUG_PRINTOUTS { + rprintln!("Checking image {:?}", app_sel); + } if app_sel == AppSel::A { check_app_given_addr(APP_A_CRC_ADDR, APP_A_START_ADDR, APP_A_SIZE_ADDR, wdt) } else { diff --git a/examples/simple/Cargo.toml b/examples/simple/Cargo.toml index 0320c7a..3ffcf88 100644 --- a/examples/simple/Cargo.toml +++ b/examples/simple/Cargo.toml @@ -23,5 +23,5 @@ version = "2" features = ["thumbv6-backend"] [dependencies.rtic-monotonics] -version = "1" +version = "2" features = ["cortex-m-systick"] diff --git a/examples/simple/examples/spi.rs b/examples/simple/examples/spi.rs index e9e5401..e918236 100644 --- a/examples/simple/examples/spi.rs +++ b/examples/simple/examples/spi.rs @@ -9,12 +9,13 @@ use embedded_hal::spi::{Mode, SpiBus, MODE_0}; use panic_rtt_target as _; use rtt_target::{rprintln, rtt_init_print}; use simple_examples::peb1; -use va416xx_hal::spi::{Spi, TransferConfig}; +use va416xx_hal::spi::{clk_div_for_target_clock, Spi, TransferConfig}; use va416xx_hal::{ gpio::{PinsB, PinsC}, pac, prelude::*, spi::SpiConfig, + time::Hertz, }; #[derive(PartialEq, Debug)] @@ -56,21 +57,24 @@ fn main() -> ! { pins_c.pc1.into_funsel_1(), ); - let mut spi_cfg = SpiConfig::default(); + let mut spi_cfg = SpiConfig::default().clk_div( + clk_div_for_target_clock(Hertz::from_raw(SPI_SPEED_KHZ), &clocks) + .expect("invalid target clock"), + ); if EXAMPLE_SEL == ExampleSelect::Loopback { spi_cfg = spi_cfg.loopback(true) } - let transfer_cfg = - TransferConfig::new_no_hw_cs(SPI_SPEED_KHZ.kHz(), SPI_MODE, BLOCKMODE, false); + let transfer_cfg = TransferConfig::new_no_hw_cs(None, Some(SPI_MODE), BLOCKMODE, false); // Create SPI peripheral. let mut spi0 = Spi::new( + &mut dp.sysconfig, + &clocks, dp.spi0, (sck, miso, mosi), - &clocks, spi_cfg, - &mut dp.sysconfig, Some(&transfer_cfg.downgrade()), - ); + ) + .expect("creating SPI peripheral failed"); spi0.set_fill_word(FILL_WORD); loop { let mut tx_buf: [u8; 3] = [1, 2, 3]; diff --git a/flashloader/.gitignore b/flashloader/.gitignore new file mode 100644 index 0000000..f9606a3 --- /dev/null +++ b/flashloader/.gitignore @@ -0,0 +1 @@ +/venv diff --git a/flashloader/Cargo.toml b/flashloader/Cargo.toml index a01e8a4..ec98f54 100644 --- a/flashloader/Cargo.toml +++ b/flashloader/Cargo.toml @@ -38,5 +38,5 @@ version = "2" features = ["thumbv7-backend"] [dependencies.rtic-monotonics] -version = "1" +version = "2" features = ["cortex-m-systick"] diff --git a/flashloader/image-loader.py b/flashloader/image-loader.py new file mode 100755 index 0000000..66029a0 --- /dev/null +++ b/flashloader/image-loader.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python3 +from spacepackets.ecss.defs import PusService +import toml +import logging +import argparse +import time +from tmtccmd.com.serial_base import SerialCfg +from tmtccmd.com.serial_cobs import SerialCobsComIF +from tmtccmd.com.ser_utils import prompt_com_port +from spacepackets.ecss.tc import PusTc +from pathlib import Path + + +BAUD_RATE = 115200 +BOOTLOADER_START_ADDR = 0x0 +BOOTLOADER_END_ADDR = 0x4000 +BOOTLOADER_CRC_ADDR = 0x3FFE +APP_A_START_ADDR = 0x4000 +APP_A_END_ADDR = 0x22000 +# The actual size of the image which is relevant for CRC calculation. +APP_A_SIZE_ADDR = 0x21FF8 +APP_A_CRC_ADDR = 0x21FFC +APP_B_START_ADDR = 0x22000 +APP_B_END_ADDR = 0x40000 +# The actual size of the image which is relevant for CRC calculation. +APP_B_SIZE_ADDR = 0x3FFF8 +APP_B_CRC_ADDR = 0x3FFFC +APP_IMG_SZ = 0x1E000 + +_LOGGER = logging.getLogger(__name__) + + +def main(): + print("Python VA416XX Image Loader Application") + logging.basicConfig( + format="[%(asctime)s] [%(levelname)s] %(message)s", level=logging.DEBUG + ) + parser = argparse.ArgumentParser( + prog="image-loader", description="Python VA416XX Image Loader Application" + ) + parser.add_argument("-p", "--ping", action="store_true", help="Send ping command") + parser.add_argument( + "-f", + "--flash", + choices=["bl", "a", "b"], + help="Flash target (Bootloader or slot A or B)", + ) + parser.add_argument( + "path", nargs="?", default=None, help="Path to the App to flash" + ) + args = parser.parse_args() + serial_port = None + if Path("loader.toml").exists(): + with open("loader.toml", "r") as toml_file: + parsed_toml = toml.loads(toml_file.read()) + if "serial_port" in parsed_toml: + serial_port = parsed_toml["serial_port"] + if serial_port is None: + serial_port = prompt_com_port() + serial_cfg = SerialCfg( + com_if_id="ser_cobs", + serial_port=serial_port, + baud_rate=BAUD_RATE, + serial_timeout=0.1, + ) + com_if = SerialCobsComIF(serial_cfg) + com_if.open() + if args.flash and not args.path: + _LOGGER.error("App Path needs to be specified for the flash process") + ping_tc = PusTc(apid=0x00, service=PusService.S17_TEST, subservice=1) + if args.ping: + _LOGGER.info("Sending ping command") + com_if.send(ping_tc.pack()) + if args.flash: + while True: + data_available = com_if.data_available(0.4) + if data_available: + reply = com_if.receive() + # TODO: Parse replies + print("Received replies: {}", reply) + break + com_if.close() + + +if __name__ == "__main__": + main() diff --git a/flashloader/loader.toml b/flashloader/loader.toml new file mode 100644 index 0000000..bfcf1ac --- /dev/null +++ b/flashloader/loader.toml @@ -0,0 +1 @@ +serial_port = "/dev/ttyUSB0" diff --git a/flashloader/requirements.txt b/flashloader/requirements.txt new file mode 100644 index 0000000..e25be91 --- /dev/null +++ b/flashloader/requirements.txt @@ -0,0 +1,3 @@ +spacepackets == 0.24 +tmtccmd == 8.0.1 +toml == 0.10 diff --git a/flashloader/src/main.rs b/flashloader/src/main.rs index 0c85248..4ac4abc 100644 --- a/flashloader/src/main.rs +++ b/flashloader/src/main.rs @@ -28,6 +28,8 @@ const COBS_FRAME_SEPARATOR: u8 = 0x0; const MAX_PACKET_SIZE: usize = 1024; const MAX_FRAME_SIZE: usize = cobs::max_encoding_length(MAX_PACKET_SIZE); const UART_BAUDRATE: u32 = 115200; +const SERIAL_RX_WIRETAPPING: bool = false; +const COBS_RX_DEBUGGING: bool = false; const BOOT_NVM_MEMORY_ID: u8 = 1; @@ -52,8 +54,8 @@ mod app { use super::*; use embedded_hal_nb::nb; use panic_rtt_target as _; - use rtic_monotonics::systick::ExtU32; - use rtic_monotonics::systick::Systick; + use rtic::Mutex; + use rtic_monotonics::systick::prelude::*; use rtic_sync::{ channel::{Receiver, Sender}, make_channel, @@ -99,13 +101,14 @@ mod app { pub type TcTx = Sender<'static, usize, 2>; pub type TcRx = Receiver<'static, usize, 2>; + rtic_monotonics::systick_monotonic!(Mono, 10_000); + #[init] fn init(mut cx: init::Context) -> (Shared, Local) { //rtt_init_default!(); rtt_log::init(); 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 @@ -130,8 +133,11 @@ mod app { let (tx, rx) = uart0.split(); let (tc_tx, tc_rx) = make_channel!(usize, 2); - Systick::start(cx.core.SYST, clocks.sysclk().raw(), systick_mono_token); + Mono::start(cx.core.SYST, clocks.sysclk().raw()); CLOCKS.set(clocks).unwrap(); + pus_tc_handler::spawn().unwrap(); + uart_reader_task::spawn().unwrap(); + pus_tm_tx_handler::spawn().unwrap(); ( Shared { decode_buffer_busy: false, @@ -168,68 +174,108 @@ mod app { async fn uart_reader_task(mut cx: uart_reader_task::Context) { let mut current_idx = 0; loop { - match nb::block!(cx.local.uart_rx.read()) { - Ok(byte) => match cx.local.cobs_reader_state { - CobsReaderStates::WaitingForStart => { - if byte == COBS_FRAME_SEPARATOR { - *cx.local.cobs_reader_state = CobsReaderStates::WatingForEnd; + match cx.local.uart_rx.read() { + Ok(byte) => { + if SERIAL_RX_WIRETAPPING { + log::debug!("RX Byte: 0x{:x?}", byte); + } + handle_single_rx_byte(&mut cx, byte, &mut current_idx) + } + Err(e) => { + match e { + nb::Error::Other(e) => { + log::warn!("UART error: {:?}", e); + match e { + uart::Error::Overrun => { + cx.local.uart_rx.clear_fifo(); + } + uart::Error::FramingError => todo!(), + uart::Error::ParityError => todo!(), + uart::Error::BreakCondition => todo!(), + uart::Error::TransferPending => todo!(), + uart::Error::BufferTooShort => todo!(), + } + } + nb::Error::WouldBlock => { + // Delay for a short period before polling again. + Mono::delay(400.micros()).await; } } - CobsReaderStates::WatingForEnd => { - if byte == COBS_FRAME_SEPARATOR { - let mut sending_failed = false; - let mut decoding_error = false; - let mut decode_buffer_busy = false; - cx.shared.decode_buffer_busy.lock(|busy| { - if *busy { - decode_buffer_busy = true; - } else { - cx.shared.decode_buf.lock(|buf| { - match cobs::decode(&cx.local.read_buf[..current_idx], buf) { - Ok(packet_len) => { - if cx.local.tc_tx.try_send(packet_len).is_err() { - sending_failed = true; - } - } - Err(_) => { - decoding_error = true; - } + } + } + } + } + + fn handle_single_rx_byte( + cx: &mut uart_reader_task::Context, + byte: u8, + current_idx: &mut usize, + ) { + match cx.local.cobs_reader_state { + CobsReaderStates::WaitingForStart => { + if byte == COBS_FRAME_SEPARATOR { + if COBS_RX_DEBUGGING { + log::debug!("COBS start marker detected"); + } + *cx.local.cobs_reader_state = CobsReaderStates::WatingForEnd; + *current_idx = 0; + } + } + CobsReaderStates::WatingForEnd => { + if byte == COBS_FRAME_SEPARATOR { + if COBS_RX_DEBUGGING { + log::debug!("COBS end marker detected"); + } + let mut sending_failed = false; + let mut decoding_error = false; + let mut decode_buffer_busy = false; + cx.shared.decode_buffer_busy.lock(|busy| { + if *busy { + decode_buffer_busy = true; + } else { + cx.shared.decode_buf.lock(|buf| { + match cobs::decode(&cx.local.read_buf[..*current_idx], buf) { + Ok(packet_len) => { + if COBS_RX_DEBUGGING { + log::debug!( + "COBS decoded packet with length {}", + packet_len + ); } - }); - *busy = true; + if cx.local.tc_tx.try_send(packet_len).is_err() { + sending_failed = true; + } + *busy = true; + } + Err(_) => { + decoding_error = true; + } } }); - if sending_failed { - log::warn!("sending TC packet failed, queue full"); - } - if decoding_error { - log::warn!("decoding error"); - } - if decode_buffer_busy { - log::warn!("decode buffer busy. data arriving too fast"); - } - } else if current_idx >= cx.local.read_buf.len() { - *cx.local.cobs_reader_state = CobsReaderStates::FrameOverflow; - } else { - cx.local.read_buf[current_idx] = byte; - current_idx += 1; } + }); + if sending_failed { + log::warn!("sending TC packet failed, queue full"); } - CobsReaderStates::FrameOverflow => { - if byte == COBS_FRAME_SEPARATOR { - *cx.local.cobs_reader_state = CobsReaderStates::WaitingForStart; - current_idx = 0; - } + if decoding_error { + log::warn!("decoding error"); } - }, - Err(e) => match e { - uart::Error::Overrun => todo!(), - uart::Error::FramingError => todo!(), - uart::Error::ParityError => todo!(), - uart::Error::BreakCondition => todo!(), - uart::Error::TransferPending => todo!(), - uart::Error::BufferTooShort => todo!(), - }, + if decode_buffer_busy { + log::warn!("decode buffer busy. data arriving too fast"); + } + *cx.local.cobs_reader_state = CobsReaderStates::WaitingForStart; + } else if *current_idx >= cx.local.read_buf.len() { + *cx.local.cobs_reader_state = CobsReaderStates::FrameOverflow; + } else { + cx.local.read_buf[*current_idx] = byte; + *current_idx += 1; + } + } + CobsReaderStates::FrameOverflow => { + if byte == COBS_FRAME_SEPARATOR { + *cx.local.cobs_reader_state = CobsReaderStates::WaitingForStart; + *current_idx = 0; + } } } } @@ -245,7 +291,8 @@ mod app { )] async fn pus_tc_handler(mut cx: pus_tc_handler::Context) { loop { - let _ = cx.local.tc_rx.recv().await; + let packet_len = cx.local.tc_rx.recv().await.expect("all senders down"); + log::info!(target: "TC Handler", "received packet with length {}", packet_len); // We still copy the data to a local buffer, so the exchange buffer can already be used // for the next packet / decode process. cx.shared @@ -255,20 +302,21 @@ mod app { match PusTcReader::new(cx.local.read_buf) { Ok((pus_tc, _)) => { if pus_tc.service() == PusServiceId::Test as u8 && pus_tc.subservice() == 1 { - log::info!("Received ping TC"); + log::info!(target: "TC Handler", "received ping TC"); } else if pus_tc.service() == PusServiceId::MemoryManagement as u8 { // Raw memory write TC if pus_tc.subservice() == 2 { let app_data = pus_tc.app_data(); if app_data.len() < 10 { log::warn!( + target: "TC Handler", "app data for raw memory write is too short: {}", app_data.len() ); } let memory_id = app_data[0]; if memory_id != BOOT_NVM_MEMORY_ID { - log::warn!("memory ID {} not supported", memory_id); + log::warn!(target: "TC Handler", "memory ID {} not supported", memory_id); // TODO: Error reporting return; } @@ -276,6 +324,7 @@ mod app { let data_len = u32::from_be_bytes(app_data[6..10].try_into().unwrap()); if 10 + data_len as usize > app_data.len() { log::warn!( + target: "TC Handler", "invalid data length {} for raw mem write detected", data_len ); @@ -300,7 +349,7 @@ mod app { } Err(e) => { log::warn!("PUS TC error: {}", e); - }, + } } } } @@ -314,7 +363,7 @@ mod app { )] async fn pus_tm_tx_handler(_cx: pus_tm_tx_handler::Context) { loop { - Systick::delay(500.millis()).await; + Mono::delay(500.millis()).await; } } diff --git a/va416xx-hal/src/nvm.rs b/va416xx-hal/src/nvm.rs index 5892df0..621bf93 100644 --- a/va416xx-hal/src/nvm.rs +++ b/va416xx-hal/src/nvm.rs @@ -2,9 +2,11 @@ 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}; +use crate::spi::{ + mode_to_cpo_cph_bit, spi_clk_config_from_div, Instance, WordProvider, BMSTART_BMSTOP_MASK, +}; -const NVM_SER_CLOCK_RATE_DIV: u8 = 4; +const NVM_CLOCK_DIV: u16 = 2; // Commands. The internal FRAM is based on the Cypress FM25V20A device. @@ -61,11 +63,12 @@ impl Nvm { // This is done in the C HAL. syscfg.assert_periph_reset_for_two_cycles(pac::Spi3::PERIPH_SEL); + let spi_clk_cfg = spi_clk_config_from_div(NVM_CLOCK_DIV).unwrap(); 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); + w.size().bits(u8::word_reg()); + w.scrdv().bits(spi_clk_cfg.scrdv()); // Clear clock phase and polarity. Will be set to correct value for each // transfer w.spo().bit(cpo_bit); @@ -73,14 +76,13 @@ impl Nvm { } }); 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.bmstart().set_bit(); w.bmstall().set_bit() }); + spi.clkprescale() + .write(|w| unsafe { w.bits(spi_clk_cfg.prescale_val() as u32) }); spi.fifo_clr().write(|w| { w.rxfifo().set_bit(); @@ -101,6 +103,16 @@ impl Nvm { self.wait_for_tx_idle(); self.write_single(FRAM_WRSR); self.write_with_bmstop(0x00); + self.wait_for_tx_idle(); + } + + pub fn read_rdsr(&self) -> u8 { + self.write_single(FRAM_RDSR); + self.write_with_bmstop(0x00); + self.wait_for_rx_available(); + self.read_single_word(); + self.wait_for_rx_available(); + (self.read_single_word() & 0xff) as u8 } pub fn enable_write_prot(&mut self) { @@ -132,6 +144,18 @@ impl Nvm { while self.spi().status().read().tfe().bit_is_clear() { cortex_m::asm::nop(); } + while self.spi().status().read().busy().bit_is_set() { + cortex_m::asm::nop(); + } + self.clear_fifos() + } + + #[inline(always)] + pub fn clear_fifos(&self) { + self.spi().fifo_clr().write(|w| { + w.rxfifo().set_bit(); + w.txfifo().set_bit() + }) } #[inline(always)] @@ -188,6 +212,8 @@ impl Nvm { self.wait_for_rx_available(); let next_word = self.read_single_word() as u8; if next_word != *byte { + self.write_with_bmstop(0); + self.wait_for_tx_idle(); return Err(VerifyError { addr: addr + idx as u32, found: next_word, @@ -195,6 +221,8 @@ impl Nvm { }); } } + self.write_with_bmstop(0); + self.wait_for_tx_idle(); Ok(()) } diff --git a/va416xx-hal/src/spi.rs b/va416xx-hal/src/spi.rs index 42f7574..85faac4 100644 --- a/va416xx-hal/src/spi.rs +++ b/va416xx-hal/src/spi.rs @@ -8,7 +8,7 @@ use core::{convert::Infallible, marker::PhantomData, ops::Deref}; use embedded_hal::spi::Mode; use crate::{ - clock::{PeripheralSelect, SyscfgExt}, + clock::{Clocks, PeripheralSelect, SyscfgExt}, gpio::{ AltFunc1, AltFunc2, AltFunc3, Pin, PA0, PA1, PA2, PA3, PA4, PA5, PA6, PA7, PA8, PA9, PB0, PB1, PB10, PB11, PB12, PB13, PB14, PB15, PB2, PB3, PB4, PB5, PB6, PB7, PB8, PB9, PC0, PC1, @@ -27,6 +27,8 @@ use crate::{ // FIFO has a depth of 16. const FILL_DEPTH: usize = 12; +pub const DEFAULT_CLK_DIV: u16 = 2; + pub const BMSTART_BMSTOP_MASK: u32 = 1 << 31; pub const BMSKIPDATA_MASK: u32 = 1 << 30; @@ -211,7 +213,7 @@ pub trait TransferConfigProvider { fn sod(&mut self, sod: bool); fn blockmode(&mut self, blockmode: bool); fn mode(&mut self, mode: Mode); - fn frequency(&mut self, spi_clk: Hertz); + fn clk_div(&mut self, clk_div: u16); fn hw_cs_id(&self) -> u8; } @@ -219,8 +221,8 @@ pub trait TransferConfigProvider { /// and might change for transfers to different SPI slaves #[derive(Copy, Clone)] pub struct TransferConfig { - pub spi_clk: Hertz, - pub mode: Mode, + pub clk_div: Option, + pub mode: Option, /// This only works if the Slave Output Disable (SOD) bit of the [`SpiConfig`] is set to /// false pub hw_cs: Option, @@ -234,8 +236,8 @@ pub struct TransferConfig { /// Type erased variant of the transfer configuration. This is required to avoid generics in /// the SPI constructor. pub struct ErasedTransferConfig { - pub spi_clk: Hertz, - pub mode: Mode, + pub clk_div: Option, + pub mode: Option, pub sod: bool, /// If this is enabled, all data in the FIFO is transmitted in a single frame unless /// the BMSTOP bit is set on a dataword. A frame is defined as CSn being active for the @@ -245,9 +247,14 @@ pub struct ErasedTransferConfig { } impl TransferConfig { - pub fn new_no_hw_cs(spi_clk: impl Into, mode: Mode, blockmode: bool, sod: bool) -> Self { + pub fn new_no_hw_cs( + clk_div: Option, + mode: Option, + blockmode: bool, + sod: bool, + ) -> Self { TransferConfig { - spi_clk: spi_clk.into(), + clk_div, mode, hw_cs: None, sod, @@ -258,14 +265,14 @@ impl TransferConfig { impl TransferConfig { pub fn new( - spi_clk: impl Into, - mode: Mode, + clk_div: Option, + mode: Option, hw_cs: Option, blockmode: bool, sod: bool, ) -> Self { TransferConfig { - spi_clk: spi_clk.into(), + clk_div, mode, hw_cs, sod, @@ -275,7 +282,7 @@ impl TransferConfig { pub fn downgrade(self) -> ErasedTransferConfig { ErasedTransferConfig { - spi_clk: self.spi_clk, + clk_div: self.clk_div, mode: self.mode, sod: self.sod, blockmode: self.blockmode, @@ -295,11 +302,11 @@ impl TransferConfigProvider for TransferConfig { } fn mode(&mut self, mode: Mode) { - self.mode = mode; + self.mode = Some(mode); } - fn frequency(&mut self, spi_clk: Hertz) { - self.spi_clk = spi_clk; + fn clk_div(&mut self, clk_div: u16) { + self.clk_div = Some(clk_div); } fn hw_cs_id(&self) -> u8 { @@ -307,13 +314,9 @@ impl TransferConfigProvider for TransferConfig { } } -#[derive(Default)] /// Configuration options for the whole SPI bus. See Programmer Guide p.92 for more details pub struct SpiConfig { - /// Serial clock rate divider. Together with the CLKPRESCALE register, it determines - /// the SPI clock rate in master mode. 0 by default. Specifying a higher value - /// limits the maximum attainable SPI speed - pub ser_clock_rate_div: u8, + clk_div: u16, /// By default, configure SPI for master mode (ms == false) ms: bool, /// Slave output disable. Useful if separate GPIO pins or decoders are used for CS control @@ -324,14 +327,26 @@ pub struct SpiConfig { pub master_delayer_capture: bool, } +impl Default for SpiConfig { + fn default() -> Self { + Self { + clk_div: DEFAULT_CLK_DIV, + ms: Default::default(), + slave_output_disable: Default::default(), + loopback_mode: Default::default(), + master_delayer_capture: Default::default(), + } + } +} + impl SpiConfig { pub fn loopback(mut self, enable: bool) -> Self { self.loopback_mode = enable; self } - pub fn ser_clock_rate_div(mut self, div: u8) -> Self { - self.ser_clock_rate_div = div; + pub fn clk_div(mut self, clk_div: u16) -> Self { + self.clk_div = clk_div; self } @@ -449,10 +464,105 @@ pub fn mode_to_cpo_cph_bit(mode: embedded_hal::spi::Mode) -> (bool, bool) { } } +#[derive(Debug)] +pub struct SpiClkConfig { + prescale_val: u16, + scrdv: u8, +} + +impl SpiClkConfig { + pub fn prescale_val(&self) -> u16 { + self.prescale_val + } + pub fn scrdv(&self) -> u8 { + self.scrdv + } +} + +#[derive(Debug)] +pub enum SpiClkConfigError { + DivIsZero, + DivideValueNotEven, + ScrdvValueTooLarge, +} + +#[inline] +pub fn spi_clk_config_from_div(mut div: u16) -> Result { + if div == 0 { + return Err(SpiClkConfigError::DivIsZero); + } + if div % 2 != 0 { + return Err(SpiClkConfigError::DivideValueNotEven); + } + let mut prescale_val = 0; + + // find largest (even) prescale value that divides into div + for i in (2..=0xfe).rev().step_by(2) { + if div % i == 0 { + prescale_val = i; + break; + } + } + + if prescale_val == 0 { + return Err(SpiClkConfigError::DivideValueNotEven); + } + + div /= prescale_val; + if div > u8::MAX as u16 + 1 { + return Err(SpiClkConfigError::ScrdvValueTooLarge); + } + Ok(SpiClkConfig { + prescale_val, + scrdv: (div - 1) as u8, + }) +} + +#[inline] +pub fn clk_div_for_target_clock(spi_clk: impl Into, clocks: &Clocks) -> Option { + let spi_clk = spi_clk.into(); + if spi_clk > clocks.apb1() { + return None; + } + + // Step 1: Calculate raw divider. + let raw_div = clocks.apb1().raw() / spi_clk.raw(); + let remainder = clocks.apb1().raw() % spi_clk.raw(); + + // Step 2: Round up if necessary. + let mut rounded_div = if remainder * 2 >= spi_clk.raw() { + raw_div + 1 + } else { + raw_div + }; + + if rounded_div % 2 != 0 { + // Take slower clock conservatively. + rounded_div += 1; + } + if rounded_div > u16::MAX as u32 { + return None; + } + Some(rounded_div as u16) +} + impl SpiBase where >::Error: core::fmt::Debug, { + #[inline] + pub fn cfg_clock_from_div(&mut self, div: u16) -> Result<(), SpiClkConfigError> { + let val = spi_clk_config_from_div(div)?; + self.spi_instance() + .ctrl0() + .modify(|_, w| unsafe { w.scrdv().bits(val.scrdv as u8) }); + self.spi_instance() + .clkprescale() + .write(|w| unsafe { w.bits(val.prescale_val as u32) }); + Ok(()) + } + + /* #[inline] pub fn cfg_clock(&mut self, spi_clk: impl Into) { let clk_prescale = @@ -461,6 +571,7 @@ where .clkprescale() .write(|w| unsafe { w.bits(clk_prescale) }); } + */ #[inline] pub fn cfg_mode(&mut self, mode: Mode) { @@ -521,9 +632,13 @@ where pub fn cfg_transfer>( &mut self, transfer_cfg: &TransferConfig, - ) { - self.cfg_clock(transfer_cfg.spi_clk); - self.cfg_mode(transfer_cfg.mode); + ) -> Result<(), SpiClkConfigError> { + if let Some(trans_clk_div) = transfer_cfg.clk_div { + self.cfg_clock_from_div(trans_clk_div)?; + } + if let Some(mode) = transfer_cfg.mode { + self.cfg_mode(mode); + } self.blockmode = transfer_cfg.blockmode; self.spi.ctrl1().modify(|_, w| { if transfer_cfg.sod { @@ -543,6 +658,7 @@ where } w }); + Ok(()) } /// Sends a word to the slave @@ -637,42 +753,43 @@ where /// * `syscfg` - Can be passed optionally to enable the peripheral clock pub fn new( syscfg: &mut pac::Sysconfig, - spi: SpiI, clocks: &crate::clock::Clocks, + spi: SpiI, pins: (Sck, Miso, Mosi), spi_cfg: SpiConfig, transfer_cfg: Option<&ErasedTransferConfig>, - ) -> Self { + ) -> Result { crate::clock::enable_peripheral_clock(syscfg, SpiI::PERIPH_SEL); // This is done in the C HAL. syscfg.assert_periph_reset_for_two_cycles(SpiI::PERIPH_SEL); let SpiConfig { - ser_clock_rate_div, + clk_div, ms, slave_output_disable, loopback_mode, master_delayer_capture, } = spi_cfg; - let mut mode = embedded_hal::spi::MODE_0; - let mut clk_prescale = 0x02; + let mut init_mode = embedded_hal::spi::MODE_0; let mut ss = 0; let mut init_blockmode = false; let apb1_clk = clocks.apb1(); if let Some(transfer_cfg) = transfer_cfg { - mode = transfer_cfg.mode; - clk_prescale = - apb1_clk.raw() / (transfer_cfg.spi_clk.raw() * (ser_clock_rate_div as u32 + 1)); + if let Some(mode) = transfer_cfg.mode { + init_mode = mode; + } + //self.cfg_clock_from_div(transfer_cfg.clk_div); if transfer_cfg.hw_cs != HwChipSelectId::Invalid { ss = transfer_cfg.hw_cs as u8; } init_blockmode = transfer_cfg.blockmode; } - let (cpo_bit, cph_bit) = mode_to_cpo_cph_bit(mode); + let spi_clk_cfg = spi_clk_config_from_div(clk_div)?; + let (cpo_bit, cph_bit) = mode_to_cpo_cph_bit(init_mode); spi.ctrl0().write(|w| { unsafe { w.size().bits(Word::word_reg()); - w.scrdv().bits(ser_clock_rate_div); + w.scrdv().bits(spi_clk_cfg.scrdv); // Clear clock phase and polarity. Will be set to correct value for each // transfer w.spo().bit(cpo_bit); @@ -687,16 +804,17 @@ where w.blockmode().bit(init_blockmode); unsafe { w.ss().bits(ss) } }); + spi.clkprescale() + .write(|w| unsafe { w.bits(spi_clk_cfg.prescale_val as u32) }); spi.fifo_clr().write(|w| { w.rxfifo().set_bit(); w.txfifo().set_bit() }); - spi.clkprescale().write(|w| unsafe { w.bits(clk_prescale) }); // Enable the peripheral as the last step as recommended in the // programmers guide spi.ctrl1().modify(|_, w| w.enable().set_bit()); - Spi { + Ok(Spi { inner: SpiBase { spi, cfg: spi_cfg, @@ -706,13 +824,13 @@ where word: PhantomData, }, pins, - } + }) } delegate::delegate! { to self.inner { #[inline] - pub fn cfg_clock(&mut self, spi_clk: impl Into); + pub fn cfg_clock_from_div(&mut self, div: u16) -> Result<(), SpiClkConfigError>; #[inline] pub fn spi_instance(&self) -> &SpiI; @@ -723,8 +841,9 @@ where #[inline] pub fn perid(&self) -> u32; - pub fn cfg_transfer>(&mut self, transfer_cfg: &TransferConfig); - + pub fn cfg_transfer>( + &mut self, transfer_cfg: &TransferConfig + ) -> Result<(), SpiClkConfigError>; } } diff --git a/va416xx-hal/src/uart.rs b/va416xx-hal/src/uart.rs index 8f75201..c31f44d 100644 --- a/va416xx-hal/src/uart.rs +++ b/va416xx-hal/src/uart.rs @@ -3,7 +3,6 @@ //! ## Examples //! //! - [UART simple example](https://egit.irs.uni-stuttgart.de/rust/va416xx-rs/src/branch/main/examples/simple/examples/uart.rs) -use core::marker::PhantomData; use core::ops::Deref; use embedded_hal_nb::serial::Read; @@ -312,27 +311,23 @@ pub struct UartWithIrqBase { /// Serial receiver pub struct Rx { - _usart: PhantomData, + uart: Uart, } /// Serial transmitter pub struct Tx { - _usart: PhantomData, + uart: Uart, } -impl Rx { - fn new() -> Self { - Self { - _usart: PhantomData, - } +impl Rx { + fn new(uart: Uart) -> Self { + Self { uart } } } impl Tx { - fn new() -> Self { - Self { - _usart: PhantomData, - } + fn new(uart: Uart) -> Self { + Self { uart } } } @@ -342,6 +337,12 @@ pub trait Instance: Deref { const IRQ_RX: pac::Interrupt; const IRQ_TX: pac::Interrupt; + /// Retrieve the peripheral structure. + /// + /// # Safety + /// + /// This circumvents the safety guarantees of the HAL. + unsafe fn steal() -> Self; fn ptr() -> *const uart_base::RegisterBlock; } @@ -351,6 +352,9 @@ impl Instance for Uart0 { const IRQ_RX: pac::Interrupt = pac::Interrupt::UART0_RX; const IRQ_TX: pac::Interrupt = pac::Interrupt::UART0_TX; + unsafe fn steal() -> Self { + pac::Peripherals::steal().uart0 + } fn ptr() -> *const uart_base::RegisterBlock { Uart0::ptr() as *const _ } @@ -362,6 +366,9 @@ impl Instance for Uart1 { const IRQ_RX: pac::Interrupt = pac::Interrupt::UART1_RX; const IRQ_TX: pac::Interrupt = pac::Interrupt::UART1_TX; + unsafe fn steal() -> Self { + pac::Peripherals::steal().uart1 + } fn ptr() -> *const uart_base::RegisterBlock { Uart1::ptr() as *const _ } @@ -373,6 +380,9 @@ impl Instance for Uart2 { const IRQ_RX: pac::Interrupt = pac::Interrupt::UART2_RX; const IRQ_TX: pac::Interrupt = pac::Interrupt::UART2_TX; + unsafe fn steal() -> Self { + pac::Peripherals::steal().uart2 + } fn ptr() -> *const uart_base::RegisterBlock { Uart2::ptr() as *const _ } @@ -525,8 +535,8 @@ impl Uart { Uart { inner: UartBase { uart, - tx: Tx::new(), - rx: Rx::new(), + tx: Tx::new(unsafe { UartInstance::steal() }), + rx: Rx::new(unsafe { UartInstance::steal() }), }, pins, } @@ -544,8 +554,8 @@ impl Uart { Uart { inner: UartBase { uart, - tx: Tx::new(), - rx: Rx::new(), + tx: Tx::new(unsafe { UartInstance::steal() }), + rx: Rx::new(unsafe { UartInstance::steal() }), }, pins, } @@ -630,6 +640,36 @@ impl Uart { } } +impl Rx { + /// Direct access to the peripheral structure. + /// + /// # Safety + /// + /// You must ensure that only registers related to the operation of the RX side are used. + pub unsafe fn uart(&self) -> &Uart { + &self.uart + } + + pub fn clear_fifo(&self) { + self.uart.fifo_clr().write(|w| w.rxfifo().set_bit()); + } +} + +impl Tx { + /// Direct access to the peripheral structure. + /// + /// # Safety + /// + /// You must ensure that only registers related to the operation of the TX side are used. + pub unsafe fn uart(&self) -> &Uart { + &self.uart + } + + pub fn clear_fifo(&self) { + self.uart.fifo_clr().write(|w| w.txfifo().set_bit()); + } +} + #[derive(Default, Debug)] pub struct IrqUartError { overflow: bool,