Bootloader and Flashloader App

2024-09-11 20:44:10 +02:00 · 2024-09-11 20:44:10 +02:00 · 896fdb839f
commit 896fdb839f
parent deebc88042
32 changed files with 2042 additions and 79 deletions
--- a/.cargo/def-config.toml
+++ b/.cargo/def-config.toml
@ -35,6 +35,8 @@ target = "thumbv7em-none-eabihf" # Cortex-M4F and Cortex-M7F (with FPU)
 [alias]
 rb = "run --bin"
 rrb = "run --release --bin"
+ut = "test --target=x86_64-unknown-linux-gnu"
+genbin = "objcopy --release -- -O binary app.bin"

 [env]
 DEFMT_LOG = "info"
--- a/.gitignore
+++ b/.gitignore
@ -14,3 +14,4 @@ Cargo.lock
 **/*.rs.bk

 /app.map
+/app.bin
--- a/Cargo.toml
+++ b/Cargo.toml
@ -1,11 +1,17 @@
 [workspace]
 resolver = "2"
 members = [
+  "bootloader",
+  "flashloader",
  "examples/simple",
 	"va416xx",
 	"va416xx-hal",
 	"vorago-peb1"
 ]
+exclude = [
+  "flashloader/slot-a-blinky",
+  "flashloader/slot-b-blinky",
+]

 [profile.dev]
 codegen-units = 1
@ -25,3 +31,12 @@ incremental = false
 lto = 'fat'
 opt-level = 3            # <-
 overflow-checks = false  # <-
+
+[profile.small]
+inherits = "release"
+codegen-units = 1
+debug-assertions = false # <-
+lto = true
+opt-level = 'z'            # <-
+overflow-checks = false  # <-
+# strip = true  # Automatically strip symbols from the binary.
--- a/bootloader/Cargo.toml
+++ 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"
--- a/bootloader/src/main.rs
+++ b/bootloader/src/main.rs
@ -0,0 +1,339 @@
+//! Vorago bootloader which can boot from two images.
+//!
+//! Bootloader memory map
+//!
+//! * <0x0>     Bootloader start                         <code up to 0x3FFE bytes>
+//! * <0x3FFE>  Bootloader CRC                           <halfword>
+//! * <0x4000>  App image A start                        <code up to 0x1DFFC (~120K) bytes>
+//! * <0x21FFC> App image A CRC check length             <halfword>
+//! * <0x21FFE> App image A CRC check value              <halfword>
+//! * <0x22000> App image B start                        <code up to 0x1DFFC (~120K) bytes>
+//! * <0x3FFFC> App image B CRC check length             <halfword>
+//! * <0x3FFFE> App image B CRC check value              <halfword>
+//! * <0x40000>                                          <end>
+//!
+//! 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_32_ISO_HDLC};
+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 = 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;
+const BOOTLOADER_END_ADDR: u32 = 0x4000;
+const BOOTLOADER_CRC_ADDR: u32 = 0x3FFC;
+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<u32> = Crc::<u32>::new(&CRC_32_ISO_HDLC);
+
+#[derive(Debug, Copy, Clone, PartialEq, Eq)]
+enum AppSel {
+    A,
+    B,
+}
+
+pub trait WdtInterface {
+    fn feed(&self);
+}
+
+pub struct OptWdt(Option<Wdt>);
+
+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();
+    // 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
+        .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);
+
+    if FLASH_SELF {
+        let mut first_four_bytes: [u8; 4] = [0; 4];
+        read_four_bytes_at_addr_zero(&mut first_four_bytes);
+        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 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);
+
+    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 u32).read_unaligned().to_be() };
+    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 mut first_four_bytes: [u8; 4] = [0; 4];
+    read_four_bytes_at_addr_zero(&mut first_four_bytes);
+    let mut digest = CRC_ALGO.digest();
+    digest.update(&first_four_bytes);
+    digest.update(unsafe {
+        &*core::ptr::slice_from_raw_parts(
+            (BOOTLOADER_START_ADDR + 4) as *const u8,
+            (BOOTLOADER_END_ADDR - BOOTLOADER_START_ADDR - 8) as usize,
+        )
+    });
+    let crc_calc = digest.finalize();
+    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 but I think it is
+        // necessary because somehow the boot will not work if we just continue as usual.
+        // cortex_m::peripheral::SCB::sys_reset();
+    } else if crc_exp != crc_calc {
+        // Bootloader is corrupted. Try to run App A.
+        if DEBUG_PRINTOUTS {
+            rprintln!(
+                "bootloader CRC corrupt, read {} and expected {}. booting image A immediately",
+                crc_calc,
+                crc_exp
+            );
+        }
+        // TODO: Shift out minimal CCSDS frame to notify about bootloader corruption.
+        boot_app(AppSel::A, cp);
+    }
+}
+
+fn read_four_bytes_at_addr_zero(buf: &mut [u8; 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) buf as *mut [u8; 4],  // Input: destination pointer
+        );
+    }
+}
+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 {
+        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 u32).read_unaligned().to_be() };
+    let image_size = unsafe { (image_size_addr as *const u32).read_unaligned().to_be() };
+    // Sanity check.
+    if image_size > APP_A_END_ADDR - APP_A_START_ADDR - 8 {
+        rprintln!("detected invalid app size {}", image_size);
+        return false;
+    }
+    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) -> ! {
+    if DEBUG_PRINTOUTS {
+        rprintln!("booting app {:?}", app_sel);
+    }
+    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?
+}
--- a/examples/simple/Cargo.toml
+++ b/examples/simple/Cargo.toml
@ -8,6 +8,7 @@ cortex-m-rt = "0.7"
 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"
--- a/examples/simple/examples/rtic-empty.rs
+++ 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 {}
+    }
+}
--- 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];
--- a/flashloader/.gitignore
+++ b/flashloader/.gitignore
@ -0,0 +1 @@
+/venv
--- a/flashloader/Cargo.toml
+++ b/flashloader/Cargo.toml
@ -0,0 +1,42 @@
+[package]
+name = "flashloader"
+version = "0.1.0"
+edition = "2021"
+
+[dependencies]
+cortex-m = "0.7"
+cortex-m-rt = "0.7"
+embedded-hal = "1"
+embedded-hal-nb = "1"
+panic-rtt-target = { version = "0.1.3" }
+rtt-target = { version = "0.5" }
+rtt-log = "0.3"
+log = "0.4"
+crc = "3"
+rtic-sync = "1"
+
+[dependencies.once_cell]
+version = "1"
+default-features = false
+features = ["critical-section"]
+
+[dependencies.spacepackets]
+version = "0.11"
+default-features = false
+
+[dependencies.cobs]
+git = "https://github.com/robamu/cobs.rs.git"
+branch = "all_features"
+default-features = false
+
+[dependencies.va416xx-hal]
+path = "../va416xx-hal"
+version = "0.1.0"
+
+[dependencies.rtic]
+version = "2"
+features = ["thumbv7-backend"]
+
+[dependencies.rtic-monotonics]
+version = "2"
+features = ["cortex-m-systick"]
--- a/flashloader/image-loader.py
+++ b/flashloader/image-loader.py
@ -0,0 +1,279 @@
+#!/usr/bin/env python3
+from spacepackets.ecss.defs import PusService
+import toml
+import struct
+import logging
+import argparse
+import time
+import enum
+from tmtccmd.com.serial_base import SerialCfg
+from tmtccmd.com.serial_cobs import SerialCobsComIF
+from tmtccmd.com.ser_utils import prompt_com_port
+from crcmod.predefined import PredefinedCrc
+from spacepackets.ecss.tc import PusTc
+from pathlib import Path
+import dataclasses
+from elftools.elf.elffile import ELFFile
+
+
+BAUD_RATE = 115200
+BOOTLOADER_START_ADDR = 0x0
+BOOTLOADER_END_ADDR = 0x4000
+BOOTLOADER_CRC_ADDR = 0x3FFC
+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
+
+CHUNK_SIZE = 896
+
+MEMORY_SERVICE = 6
+ACTION_SERVICE = 8
+
+RAW_MEMORY_WRITE_SUBSERVICE = 2
+BOOT_NVM_MEMORY_ID = 1
+
+
+class ActionId(enum.IntEnum):
+    CORRUPT_APP_A = 128
+    CORRUPT_APP_B = 129
+
+
+_LOGGER = logging.getLogger(__name__)
+
+
+@dataclasses.dataclass
+class LoadableSegment:
+    name: str
+    offset: int
+    size: int
+    data: bytes
+
+
+def main() -> int:
+    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("-c", "--corrupt", action="store_true", help="Corrupt a target")
+    parser.add_argument(
+        "-t",
+        "--target",
+        choices=["bl", "a", "b"],
+        help="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()
+    file_path = None
+    if args.target:
+        if not args.corrupt:
+            if not args.path:
+                _LOGGER.error("App Path needs to be specified for the flash process")
+                return -1
+            file_path = Path(args.path)
+            if not file_path.exists():
+                _LOGGER.error("File does not exist")
+                return -1
+    if args.ping:
+        _LOGGER.info("Sending ping command")
+        ping_tc = PusTc(apid=0x00, service=PusService.S17_TEST, subservice=1)
+        com_if.send(ping_tc.pack())
+    if args.corrupt:
+        if not args.target:
+            _LOGGER.error("target for corruption command required")
+            return -1
+        if args.target == "bl":
+            _LOGGER.error("can not corrupt bootloader")
+        if args.target == "a":
+            packet = PusTc(
+                apid=0,
+                service=ACTION_SERVICE,
+                subservice=ActionId.CORRUPT_APP_A,
+            )
+            com_if.send(packet.pack())
+        if args.target == "b":
+            packet = PusTc(
+                apid=0,
+                service=ACTION_SERVICE,
+                subservice=ActionId.CORRUPT_APP_B,
+            )
+            com_if.send(packet.pack())
+    else:
+        assert file_path is not None
+        loadable_segments = []
+        _LOGGER.info("Parsing ELF file for loadable sections")
+        total_size = 0
+        with open(file_path, "rb") as app_file:
+            elf_file = ELFFile(app_file)
+
+            for (idx, segment) in enumerate(elf_file.iter_segments("PT_LOAD")):
+                if segment.header.p_filesz == 0:
+                    continue
+                # Basic validity checks of the base addresses.
+                if idx == 0:
+                    if (
+                        args.target == "bl"
+                        and segment.header.p_paddr != BOOTLOADER_START_ADDR
+                    ):
+                        raise ValueError(
+                            f"detected possibly invalid start address {segment.header.p_paddr:#08x} for "
+                            f"bootloader, expected {BOOTLOADER_START_ADDR}"
+                        )
+                    if args.target == "a" and segment.header.p_paddr != APP_A_START_ADDR:
+                        raise ValueError(
+                            f"detected possibly invalid start address {segment.header.p_paddr:#08x} for "
+                            f"App A, expected {APP_A_START_ADDR}"
+                        )
+                    if args.target == "b" and segment.header.p_paddr != APP_B_START_ADDR:
+                        raise ValueError(
+                            f"detected possibly invalid start address {segment.header.p_paddr:#08x} for "
+                            f"App B, expected {APP_B_START_ADDR}"
+                        )
+                name = None
+                for section in elf_file.iter_sections():
+                    if (
+                        section.header.sh_offset == segment.header.p_offset
+                        and section.header.sh_size > 0
+                    ):
+                        name = section.name
+                if name is None:
+                    _LOGGER.warning("no fitting section found for segment")
+                    continue
+                # print(f"Segment Addr: {segment.header.p_paddr}")
+                # print(f"Segment Offset: {segment.header.p_offset}")
+                # print(f"Segment Filesize: {segment.header.p_filesz}")
+                loadable_segments.append(
+                    LoadableSegment(
+                        name=name,
+                        offset=segment.header.p_paddr,
+                        size=segment.header.p_filesz,
+                        data=segment.data(),
+                    )
+                )
+                total_size += segment.header.p_filesz
+            context_str = None
+            if args.target == "bl":
+                context_str = "Bootloader"
+            elif args.target == "a":
+                context_str = "App Slot A"
+            elif args.target == "b":
+                context_str = "App Slot B"
+            _LOGGER.info(
+                f"Flashing {context_str} with image {file_path} (size {total_size})"
+            )
+            for idx, segment in enumerate(loadable_segments):
+                _LOGGER.info(
+                    f"Loadable section {idx} {segment.name} with offset {segment.offset:#08x} and size {segment.size}"
+                )
+            for segment in loadable_segments:
+                segment_end = segment.offset + segment.size
+                current_addr = segment.offset
+                pos_in_segment = 0
+                while pos_in_segment < segment.size:
+                    next_chunk_size = min(segment_end - current_addr, CHUNK_SIZE)
+                    data = segment.data[
+                        pos_in_segment : pos_in_segment + next_chunk_size
+                    ]
+                    _LOGGER.info(
+                        f"Sending memory write command for address {current_addr:#08x} and data with "
+                        f"length {len(data)}"
+                    )
+                    next_packet = pack_memory_write_command(current_addr, data)
+                    com_if.send(next_packet.pack())
+                    current_addr += next_chunk_size
+                    pos_in_segment += next_chunk_size
+                    time.sleep(0.2)
+            if args.target == "bl":
+                _LOGGER.info("Blanking the bootloader checksum")
+                # Blank the checksum. For the bootloader, the bootloader will calculate the
+                # checksum itself on the initial run.
+                checksum_write_packet = pack_memory_write_command(
+                    BOOTLOADER_CRC_ADDR, bytes([0x00, 0x00, 0x00, 0x00])
+                )
+                com_if.send(checksum_write_packet.pack())
+            else:
+                crc_addr = None
+                size_addr = None
+                if args.target == "a":
+                    crc_addr = APP_A_CRC_ADDR
+                    size_addr = APP_A_SIZE_ADDR
+                elif args.target == "b":
+                    crc_addr = APP_B_CRC_ADDR
+                    size_addr = APP_B_SIZE_ADDR
+                assert crc_addr is not None
+                assert size_addr is not None
+                _LOGGER.info(
+                    f"Writing app size {total_size } at address {size_addr:#08x}"
+                )
+                size_write_packet = pack_memory_write_command(
+                    size_addr, struct.pack("!I", total_size)
+                )
+                com_if.send(size_write_packet.pack())
+                time.sleep(0.2)
+                crc_calc = PredefinedCrc("crc-32")
+                for segment in loadable_segments:
+                    crc_calc.update(segment.data)
+                checksum = crc_calc.digest()
+                _LOGGER.info(
+                    f"Writing checksum 0x[{checksum.hex(sep=',')}] at address {crc_addr:#08x}"
+                )
+                checksum_write_packet = pack_memory_write_command(crc_addr, checksum)
+                com_if.send(checksum_write_packet.pack())
+        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()
+    return 0
+
+
+def pack_memory_write_command(addr: int, data: bytes) -> PusTc:
+    app_data = bytearray()
+    app_data.append(BOOT_NVM_MEMORY_ID)
+    # N parameter is always 1 here.
+    app_data.append(1)
+    app_data.extend(struct.pack("!I", addr))
+    app_data.extend(struct.pack("!I", len(data)))
+    app_data.extend(data)
+    return PusTc(
+        apid=0,
+        service=MEMORY_SERVICE,
+        subservice=RAW_MEMORY_WRITE_SUBSERVICE,
+        app_data=app_data,
+    )
+
+
+if __name__ == "__main__":
+    main()
--- a/flashloader/loader.toml
+++ b/flashloader/loader.toml
@ -0,0 +1 @@
+serial_port = "/dev/ttyUSB0"
--- a/flashloader/requirements.txt
+++ b/flashloader/requirements.txt
@ -0,0 +1,5 @@
+spacepackets == 0.24
+tmtccmd == 8.0.2
+toml == 0.10
+pyelftools == 0.31
+crcmod == 1.7
--- a/flashloader/slot-a-blinky/.gitignore
+++ b/flashloader/slot-a-blinky/.gitignore
@ -0,0 +1,2 @@
+/target
+/app.map
--- a/flashloader/slot-a-blinky/Cargo.toml
+++ b/flashloader/slot-a-blinky/Cargo.toml
@ -0,0 +1,42 @@
+[package]
+name = "slot-a-blinky"
+version = "0.1.0"
+edition = "2021"
+
+[workspace]
+
+[dependencies]
+cortex-m-rt = "0.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"] }
+embedded-hal = "1"
+
+[profile.dev]
+codegen-units = 1
+debug = 2
+debug-assertions = true # <-
+incremental = false
+# This is problematic for stepping..
+# opt-level = 'z'         # <-
+overflow-checks = true  # <-
+
+# cargo build/run --release
+[profile.release]
+codegen-units = 1
+debug = 2
+debug-assertions = false # <-
+incremental = false
+lto = 'fat'
+opt-level = 3            # <-
+overflow-checks = false  # <-
+
+[profile.small]
+inherits = "release"
+codegen-units = 1
+debug-assertions = false # <-
+lto = true
+opt-level = 'z'            # <-
+overflow-checks = false  # <-
+# strip = true  # Automatically strip symbols from the binary.
--- a/flashloader/slot-a-blinky/memory.x
+++ b/flashloader/slot-a-blinky/memory.x
@ -0,0 +1,24 @@
+/* Special linker script for application slot A with an offset at address 0x4000 */
+MEMORY
+{
+	FLASH : ORIGIN = 0x00004000, LENGTH = 256K
+	/* RAM is a mandatory region. This RAM refers to the SRAM_0 */
+	RAM : ORIGIN = 0x1FFF8000, LENGTH = 32K
+	SRAM_1 : ORIGIN = 0x20000000, LENGTH = 32K
+}
+
+/* This is where the call stack will be allocated. */
+/* The stack is of the full descending type. */
+/* NOTE Do NOT modify `_stack_start` unless you know what you are doing */
+/* SRAM_0 can be used for all busses: Instruction, Data and System */
+/* SRAM_1 only supports the system bus */
+_stack_start = ORIGIN(RAM) + LENGTH(RAM);
+
+/* Define sections for placing symbols into the extra memory regions above.   */
+/* This makes them accessible from code.                                      */
+SECTIONS {
+  .sram1 (NOLOAD) : ALIGN(8) {
+    *(.sram1 .sram1.*);
+    . = ALIGN(4);
+    } > SRAM_1
+};
--- a/flashloader/slot-a-blinky/src/main.rs
+++ b/flashloader/slot-a-blinky/src/main.rs
@ -0,0 +1,23 @@
+//! Simple blinky example using the HAL
+#![no_main]
+#![no_std]
+
+use cortex_m_rt::entry;
+use embedded_hal::digital::StatefulOutputPin;
+use panic_rtt_target as _;
+use rtt_target::{rprintln, rtt_init_print};
+use va416xx_hal::{gpio::PinsG, pac};
+
+#[entry]
+fn main() -> ! {
+    rtt_init_print!();
+    rprintln!("VA416xx HAL blinky example for App Slot A");
+
+    let mut dp = pac::Peripherals::take().unwrap();
+    let portg = PinsG::new(&mut dp.sysconfig, dp.portg);
+    let mut led = portg.pg5.into_readable_push_pull_output();
+    loop {
+        cortex_m::asm::delay(1_000_000);
+        led.toggle().ok();
+    }
+}
--- a/flashloader/slot-b-blinky/.gitignore
+++ b/flashloader/slot-b-blinky/.gitignore
@ -0,0 +1,2 @@
+/target
+/app.map
--- a/flashloader/slot-b-blinky/Cargo.toml
+++ b/flashloader/slot-b-blinky/Cargo.toml
@ -0,0 +1,42 @@
+[package]
+name = "slot-b-blinky"
+version = "0.1.0"
+edition = "2021"
+
+[workspace]
+
+[dependencies]
+cortex-m-rt = "0.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"] }
+embedded-hal = "1"
+
+[profile.dev]
+codegen-units = 1
+debug = 2
+debug-assertions = true # <-
+incremental = false
+# This is problematic for stepping..
+# opt-level = 'z'         # <-
+overflow-checks = true  # <-
+
+# cargo build/run --release
+[profile.release]
+codegen-units = 1
+debug = 2
+debug-assertions = false # <-
+incremental = false
+lto = 'fat'
+opt-level = 3            # <-
+overflow-checks = false  # <-
+
+[profile.small]
+inherits = "release"
+codegen-units = 1
+debug-assertions = false # <-
+lto = true
+opt-level = 'z'            # <-
+overflow-checks = false  # <-
+# strip = true  # Automatically strip symbols from the binary.
--- a/flashloader/slot-b-blinky/memory.x
+++ b/flashloader/slot-b-blinky/memory.x
@ -0,0 +1,24 @@
+/* Special linker script for application slot B with an offset at address 0x22000 */
+MEMORY
+{
+	FLASH : ORIGIN = 0x00022000, LENGTH = 256K
+	/* RAM is a mandatory region. This RAM refers to the SRAM_0 */
+	RAM : ORIGIN = 0x1FFF8000, LENGTH = 32K
+	SRAM_1 : ORIGIN = 0x20000000, LENGTH = 32K
+}
+
+/* This is where the call stack will be allocated. */
+/* The stack is of the full descending type. */
+/* NOTE Do NOT modify `_stack_start` unless you know what you are doing */
+/* SRAM_0 can be used for all busses: Instruction, Data and System */
+/* SRAM_1 only supports the system bus */
+_stack_start = ORIGIN(RAM) + LENGTH(RAM);
+
+/* Define sections for placing symbols into the extra memory regions above.   */
+/* This makes them accessible from code.                                      */
+SECTIONS {
+  .sram1 (NOLOAD) : ALIGN(8) {
+    *(.sram1 .sram1.*);
+    . = ALIGN(4);
+    } > SRAM_1
+};
--- a/flashloader/slot-b-blinky/src/main.rs
+++ b/flashloader/slot-b-blinky/src/main.rs
@ -0,0 +1,23 @@
+//! Simple blinky example using the HAL
+#![no_main]
+#![no_std]
+
+use cortex_m_rt::entry;
+use embedded_hal::digital::StatefulOutputPin;
+use panic_rtt_target as _;
+use rtt_target::{rprintln, rtt_init_print};
+use va416xx_hal::{gpio::PinsG, pac};
+
+#[entry]
+fn main() -> ! {
+    rtt_init_print!();
+    rprintln!("VA416xx HAL blinky example for App Slot B");
+
+    let mut dp = pac::Peripherals::take().unwrap();
+    let portg = PinsG::new(&mut dp.sysconfig, dp.portg);
+    let mut led = portg.pg5.into_readable_push_pull_output();
+    loop {
+        cortex_m::asm::delay(8_000_000);
+        led.toggle().ok();
+    }
+}
--- a/flashloader/src/lib.rs
+++ b/flashloader/src/lib.rs
@ -0,0 +1,9 @@
+#![no_std]
+
+#[cfg(test)]
+mod tests {
+    #[test]
+    fn simple() {
+        assert_eq!(1 + 1, 2);
+    }
+}
--- a/flashloader/src/main.rs
+++ b/flashloader/src/main.rs
@ -0,0 +1,435 @@
+//! 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                         <code up to 0x3FFE bytes>
+//! * <0x3FFE>  Bootloader CRC                           <halfword>
+//! * <0x4000>  App image A start                        <code up to 0x1DFFC (~120K) bytes>
+//! * <0x21FFC> App image A CRC check length             <halfword>
+//! * <0x21FFE> App image A CRC check value              <halfword>
+//! * <0x22000> App image B start                        <code up to 0x1DFFC (~120K) bytes>
+//! * <0x3FFFC> App image B CRC check length             <halfword>
+//! * <0x3FFFE> App image B CRC check value              <halfword>
+//! * <0x40000>                                          <end>
+#![no_main]
+#![no_std]
+
+use embedded_hal_nb::serial::Read;
+use once_cell::sync::OnceCell;
+use panic_rtt_target as _;
+use va416xx_hal::{clock::Clocks, edac, pac, time::Hertz, wdt::Wdt};
+
+const EXTCLK_FREQ: u32 = 40_000_000;
+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;
+
+pub enum ActionId {
+    CorruptImageA = 128,
+    CorruptImageB = 129,
+}
+pub trait WdtInterface {
+    fn feed(&self);
+}
+
+pub struct OptWdt(Option<Wdt>);
+
+impl WdtInterface for OptWdt {
+    fn feed(&self) {
+        if self.0.is_some() {
+            self.0.as_ref().unwrap().feed();
+        }
+    }
+}
+
+static CLOCKS: OnceCell<Clocks> = OnceCell::new();
+
+pub const APP_A_START_ADDR: u32 = 0x4000;
+pub const APP_A_END_ADDR: u32 = 0x22000;
+pub const APP_B_START_ADDR: u32 = 0x22000;
+pub const APP_B_END_ADDR: u32 = 0x40000;
+
+#[rtic::app(device = pac, dispatchers = [U1, U2, U3])]
+mod app {
+    use super::*;
+    use cortex_m::asm;
+    use embedded_hal_nb::nb;
+    use panic_rtt_target as _;
+    use rtic::Mutex;
+    use rtic_monotonics::systick::prelude::*;
+    use rtic_sync::{
+        channel::{Receiver, Sender},
+        make_channel,
+    };
+    use rtt_target::rprintln;
+    use spacepackets::ecss::PusServiceId;
+    use spacepackets::ecss::{tc::PusTcReader, PusPacket};
+    use va416xx_hal::{
+        clock::ClkgenExt,
+        edac,
+        gpio::PinsG,
+        nvm::Nvm,
+        pac,
+        uart::{self, Uart},
+    };
+
+    use crate::{setup_edac, EXTCLK_FREQ};
+
+    #[derive(Default, Debug, Copy, Clone, PartialEq, Eq)]
+    pub enum CobsReaderStates {
+        #[default]
+        WaitingForStart,
+        WatingForEnd,
+        FrameOverflow,
+    }
+
+    #[local]
+    struct Local {
+        uart_rx: uart::Rx<pac::Uart0>,
+        uart_tx: uart::Tx<pac::Uart0>,
+        cobs_reader_state: CobsReaderStates,
+        tc_tx: TcTx,
+        tc_rx: TcRx,
+        rom_spi: Option<pac::Spi3>,
+    }
+
+    #[shared]
+    struct Shared {
+        decode_buffer_busy: bool,
+        decode_buf: [u8; MAX_PACKET_SIZE],
+    }
+
+    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
+        // 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);
+
+        let gpiob = PinsG::new(&mut cx.device.sysconfig, cx.device.portg);
+        let tx = gpiob.pg0.into_funsel_1();
+        let rx = gpiob.pg1.into_funsel_1();
+
+        let uart0 = Uart::new(
+            cx.device.uart0,
+            (tx, rx),
+            Hertz::from_raw(UART_BAUDRATE),
+            &mut cx.device.sysconfig,
+            &clocks,
+        );
+        let (tx, rx) = uart0.split();
+        let (tc_tx, tc_rx) = make_channel!(usize, 2);
+
+        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,
+                decode_buf: [0; MAX_PACKET_SIZE],
+            },
+            Local {
+                uart_rx: rx,
+                uart_tx: tx,
+                cobs_reader_state: CobsReaderStates::default(),
+                tc_tx,
+                tc_rx,
+                rom_spi: Some(cx.device.spi3),
+            },
+        )
+    }
+
+    // `shared` cannot be accessed from this context
+    #[idle]
+    fn idle(_cx: idle::Context) -> ! {
+        loop {
+            asm::nop();
+        }
+    }
+
+    #[task(
+        priority = 3,
+        local=[
+            read_buf: [u8;MAX_FRAME_SIZE] = [0; MAX_FRAME_SIZE],
+            uart_rx,
+            cobs_reader_state,
+            tc_tx
+        ],
+        shared=[decode_buffer_busy, decode_buf]
+    )]
+    async fn uart_reader_task(mut cx: uart_reader_task::Context) {
+        let mut current_idx = 0;
+        loop {
+            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;
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    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
+                                            );
+                                        }
+                                        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");
+                    }
+                    *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;
+                }
+            }
+        }
+    }
+
+    #[task(
+        priority = 2,
+        local=[
+            read_buf: [u8;MAX_FRAME_SIZE] = [0; MAX_FRAME_SIZE],
+            tc_rx,
+            rom_spi
+        ],
+        shared=[decode_buffer_busy, decode_buf]
+    )]
+    async fn pus_tc_handler(mut cx: pus_tc_handler::Context) {
+        loop {
+            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
+                .decode_buf
+                .lock(|buf| cx.local.read_buf[0..buf.len()].copy_from_slice(buf));
+            cx.shared.decode_buffer_busy.lock(|busy| *busy = false);
+            match PusTcReader::new(cx.local.read_buf) {
+                Ok((pus_tc, _)) => {
+                    if pus_tc.service() == PusServiceId::Action as u8 {
+                        let mut corrupt_image = |base_addr: u32| {
+                            // Safety: We only use this for NVM handling and we only do NVM
+                            // handling here.
+                            let mut sys_cfg = unsafe { pac::Sysconfig::steal() };
+                            let nvm = Nvm::new(
+                                &mut sys_cfg,
+                                cx.local.rom_spi.take().unwrap(),
+                                CLOCKS.get().as_ref().unwrap(),
+                            );
+                            let mut buf = [0u8; 4];
+                            nvm.read_data(base_addr + 32, &mut buf);
+                            buf[0] += 1;
+                            nvm.write_data(base_addr + 32, &buf);
+                            *cx.local.rom_spi = Some(nvm.release(&mut sys_cfg));
+                        };
+                        if pus_tc.subservice() == ActionId::CorruptImageA as u8 {
+                            rprintln!("corrupting App Image A");
+                            corrupt_image(APP_A_START_ADDR);
+                        }
+                        if pus_tc.subservice() == ActionId::CorruptImageB as u8 {
+                            rprintln!("corrupting App Image B");
+                            corrupt_image(APP_B_START_ADDR);
+                        }
+                    }
+                    if pus_tc.service() == PusServiceId::Test as u8 && pus_tc.subservice() == 1 {
+                        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!(target: "TC Handler", "memory ID {} not supported", memory_id);
+                                // TODO: Error reporting
+                                return;
+                            }
+                            let offset = u32::from_be_bytes(app_data[2..6].try_into().unwrap());
+                            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
+                                );
+                                // TODO: Error reporting
+                                return;
+                            }
+                            let data = &app_data[10..10 + data_len as usize];
+                            log::info!("writing {} bytes at offset {} to NVM", data_len, offset);
+                            // Safety: We only use this for NVM handling and we only do NVM
+                            // handling here.
+                            let mut sys_cfg = unsafe { pac::Sysconfig::steal() };
+                            let nvm = Nvm::new(
+                                &mut sys_cfg,
+                                cx.local.rom_spi.take().unwrap(),
+                                CLOCKS.get().as_ref().unwrap(),
+                            );
+                            nvm.write_data(offset, data);
+                            *cx.local.rom_spi = Some(nvm.release(&mut sys_cfg));
+                            log::info!("NVM operation done");
+                        }
+                    }
+                }
+                Err(e) => {
+                    log::warn!("PUS TC error: {}", e);
+                }
+            }
+        }
+    }
+
+    #[task(
+        priority = 1,
+        local=[
+            uart_tx,
+        ],
+        shared=[]
+    )]
+    async fn pus_tm_tx_handler(_cx: pus_tm_tx_handler::Context) {
+        loop {
+            Mono::delay(500.millis()).await;
+        }
+    }
+
+    #[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();
+}
--- a/scripts/memory.x
+++ b/scripts/memory.x
@ -0,0 +1,23 @@
+MEMORY
+{
+	FLASH : ORIGIN = 0x00000000, LENGTH = 256K
+	/* RAM is a mandatory region. This RAM refers to the SRAM_0 */
+	RAM : ORIGIN = 0x1FFF8000, LENGTH = 32K
+	SRAM_1 : ORIGIN = 0x20000000, LENGTH = 32K
+}
+
+/* This is where the call stack will be allocated. */
+/* The stack is of the full descending type. */
+/* NOTE Do NOT modify `_stack_start` unless you know what you are doing */
+/* SRAM_0 can be used for all busses: Instruction, Data and System */
+/* SRAM_1 only supports the system bus */
+_stack_start = ORIGIN(RAM) + LENGTH(RAM);
+
+/* Define sections for placing symbols into the extra memory regions above.   */
+/* This makes them accessible from code.                                      */
+SECTIONS {
+  .sram1 (NOLOAD) : ALIGN(8) {
+    *(.sram1 .sram1.*);
+    . = ALIGN(4);
+    } > SRAM_1
+};
--- a/scripts/memory_app_a.x
+++ b/scripts/memory_app_a.x
@ -0,0 +1,24 @@
+/* Special linker script for application slot A with an offset at address 0x4000 */
+MEMORY
+{
+	FLASH : ORIGIN = 0x00004000, LENGTH = 256K
+	/* RAM is a mandatory region. This RAM refers to the SRAM_0 */
+	RAM : ORIGIN = 0x1FFF8000, LENGTH = 32K
+	SRAM_1 : ORIGIN = 0x20000000, LENGTH = 32K
+}
+
+/* This is where the call stack will be allocated. */
+/* The stack is of the full descending type. */
+/* NOTE Do NOT modify `_stack_start` unless you know what you are doing */
+/* SRAM_0 can be used for all busses: Instruction, Data and System */
+/* SRAM_1 only supports the system bus */
+_stack_start = ORIGIN(RAM) + LENGTH(RAM);
+
+/* Define sections for placing symbols into the extra memory regions above.   */
+/* This makes them accessible from code.                                      */
+SECTIONS {
+  .sram1 (NOLOAD) : ALIGN(8) {
+    *(.sram1 .sram1.*);
+    . = ALIGN(4);
+    } > SRAM_1
+};
--- a/scripts/memory_app_b.x
+++ b/scripts/memory_app_b.x
@ -0,0 +1,24 @@
+/* Special linker script for application slot B with an offset at address 0x22000 */
+MEMORY
+{
+	FLASH : ORIGIN = 0x00022000, LENGTH = 256K
+	/* RAM is a mandatory region. This RAM refers to the SRAM_0 */
+	RAM : ORIGIN = 0x1FFF8000, LENGTH = 32K
+	SRAM_1 : ORIGIN = 0x20000000, LENGTH = 32K
+}
+
+/* This is where the call stack will be allocated. */
+/* The stack is of the full descending type. */
+/* NOTE Do NOT modify `_stack_start` unless you know what you are doing */
+/* SRAM_0 can be used for all busses: Instruction, Data and System */
+/* SRAM_1 only supports the system bus */
+_stack_start = ORIGIN(RAM) + LENGTH(RAM);
+
+/* Define sections for placing symbols into the extra memory regions above.   */
+/* This makes them accessible from code.                                      */
+SECTIONS {
+  .sram1 (NOLOAD) : ALIGN(8) {
+    *(.sram1 .sram1.*);
+    . = ALIGN(4);
+    } > SRAM_1
+};
--- a/va416xx-hal/src/edac.rs
+++ 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()
+    });
+}
--- a/va416xx-hal/src/lib.rs
+++ b/va416xx-hal/src/lib.rs
@ -19,8 +19,10 @@ pub mod prelude;

 pub mod clock;
 pub mod dma;
+pub mod edac;
 pub mod gpio;
 pub mod i2c;
+pub mod nvm;
 pub mod pwm;
 pub mod spi;
 pub mod time;
--- a/va416xx-hal/src/nvm.rs
+++ b/va416xx-hal/src/nvm.rs
@ -0,0 +1,267 @@
+use embedded_hal::spi::MODE_0;
+
+use crate::clock::{Clocks, SyscfgExt};
+use crate::pac;
+use crate::spi::{
+    mode_to_cpo_cph_bit, spi_clk_config_from_div, Instance, WordProvider, BMSTART_BMSTOP_MASK,
+};
+
+const NVM_CLOCK_DIV: u16 = 2;
+
+// 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: Option<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 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(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);
+                w.sph().bit(cph_bit)
+            }
+        });
+        spi.ctrl1().write(|w| {
+            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();
+            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: Some(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);
+        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) {
+        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 spi(&self) -> &pac::Spi3 {
+        self.spi.as_ref().unwrap()
+    }
+
+    #[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();
+        }
+        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)]
+    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 {
+                self.write_with_bmstop(0);
+                self.wait_for_tx_idle();
+                return Err(VerifyError {
+                    addr: addr + idx as u32,
+                    found: next_word,
+                    expected: *byte,
+                });
+            }
+        }
+        self.write_with_bmstop(0);
+        self.wait_for_tx_idle();
+        Ok(())
+    }
+
+    /// Enable write-protection and disables the peripheral clock.
+    pub fn shutdown(&mut self, sys_cfg: &mut pac::Sysconfig) {
+        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);
+        crate::clock::disable_peripheral_clock(sys_cfg, pac::Spi3::PERIPH_SEL);
+    }
+
+    /// This function calls [Self::shutdown] and gives back the peripheral structure.
+    pub fn release(mut self, sys_cfg: &mut pac::Sysconfig) -> pac::Spi3 {
+        self.shutdown(sys_cfg);
+        self.spi.take().unwrap()
+    }
+
+    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();
+        }
+    }
+}
+
+/// Call [Self::shutdown] on drop.
+impl Drop for Nvm {
+    fn drop(&mut self) {
+        if self.spi.is_some() {
+            self.shutdown(unsafe { &mut pac::Sysconfig::steal() });
+        }
+    }
+}
--- 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, PB12, PB13, PB14, PB15, PB2, PB3, PB4, PC0, PC1, PC10, PC11, PC7, PC8, PC9, PE12,
@ -29,6 +29,11 @@ use crate::gpio::{PB10, PB11, PB5, PB6, PB7, PB8, PB9, PE10, PE11, PF2, PF3, PF4
 // 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;
+
 #[derive(Debug, PartialEq, Eq, Copy, Clone)]
 pub enum HwChipSelectId {
    Id0 = 0,
@ -106,6 +111,14 @@ impl OptionalHwCs<pac::Spi1> for NoneT {}
 impl OptionalHwCs<pac::Spi2> for NoneT {}
 impl OptionalHwCs<pac::Spi3> 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<pac::Spi0> for Pin<PB15, AltFunc1> {}
@ -152,6 +165,10 @@ impl PinMosi<pac::Spi2> for Pin<PF7, AltFunc2> {}
 impl PinMiso<pac::Spi2> for Pin<PF6, AltFunc2> {}

 // SPI3 is shared with the ROM SPI pins and has its own dedicated pins.
+//
+impl PinSck<pac::Spi3> for RomSpiSck {}
+impl PinMosi<pac::Spi3> for RomSpiMosi {}
+impl PinMiso<pac::Spi3> for RomSpiMiso {}

 // SPI 0 HW CS pins

@ -211,7 +228,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 +236,8 @@ pub trait TransferConfigProvider {
 /// and might change for transfers to different SPI slaves
 #[derive(Copy, Clone)]
 pub struct TransferConfig<HwCs> {
-    pub spi_clk: Hertz,
-    pub mode: Mode,
+    pub clk_div: Option<u16>,
+    pub mode: Option<Mode>,
    /// This only works if the Slave Output Disable (SOD) bit of the [`SpiConfig`] is set to
    /// false
    pub hw_cs: Option<HwCs>,
@ -234,8 +251,8 @@ pub struct TransferConfig<HwCs> {
 /// 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<u16>,
+    pub mode: Option<Mode>,
    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 +262,14 @@ pub struct ErasedTransferConfig {
 }

 impl TransferConfig<NoneT> {
-    pub fn new_no_hw_cs(spi_clk: impl Into<Hertz>, mode: Mode, blockmode: bool, sod: bool) -> Self {
+    pub fn new_no_hw_cs(
+        clk_div: Option<u16>,
+        mode: Option<Mode>,
+        blockmode: bool,
+        sod: bool,
+    ) -> Self {
        TransferConfig {
-            spi_clk: spi_clk.into(),
+            clk_div,
            mode,
            hw_cs: None,
            sod,
@ -258,14 +280,14 @@ impl TransferConfig<NoneT> {

 impl<HwCs: HwCsProvider> TransferConfig<HwCs> {
    pub fn new(
-        spi_clk: impl Into<Hertz>,
-        mode: Mode,
+        clk_div: Option<u16>,
+        mode: Option<Mode>,
        hw_cs: Option<HwCs>,
        blockmode: bool,
        sod: bool,
    ) -> Self {
        TransferConfig {
-            spi_clk: spi_clk.into(),
+            clk_div,
            mode,
            hw_cs,
            sod,
@ -275,7 +297,7 @@ impl<HwCs: HwCsProvider> TransferConfig<HwCs> {

    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 +317,11 @@ impl<HwCs: HwCsProvider> TransferConfigProvider for TransferConfig<HwCs> {
    }

    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 +329,9 @@ impl<HwCs: HwCsProvider> TransferConfigProvider for TransferConfig<HwCs> {
    }
 }

-#[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,12 +342,29 @@ 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 clk_div(mut self, clk_div: u16) -> Self {
+        self.clk_div = clk_div;
+        self
+    }
+
    pub fn master_mode(mut self, master: bool) -> Self {
        self.ms = !master;
        self
@ -406,6 +441,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
 //==================================================================================================
@ -425,7 +470,7 @@ pub struct Spi<SpiInstance, Pins, Word = u8> {
    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),
@ -434,10 +479,105 @@ 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<SpiClkConfig, SpiClkConfigError> {
+    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<Hertz>, clocks: &Clocks) -> Option<u16> {
+    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<SpiInstance: Instance, Word: WordProvider> SpiBase<SpiInstance, Word>
 where
    <Word as TryFrom<u32>>::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<Hertz>) {
        let clk_prescale =
@ -446,6 +586,7 @@ where
            .clkprescale()
            .write(|w| unsafe { w.bits(clk_prescale) });
    }
+        */

    #[inline]
    pub fn cfg_mode(&mut self, mode: Mode) {
@ -456,6 +597,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());
@ -501,9 +647,13 @@ where
    pub fn cfg_transfer<HwCs: OptionalHwCs<SpiInstance>>(
        &mut self,
        transfer_cfg: &TransferConfig<HwCs>,
-    ) {
-        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 {
@ -523,6 +673,7 @@ where
            }
            w
        });
+        Ok(())
    }

    /// Sends a word to the slave
@ -616,43 +767,44 @@ where
    ///     to be done once.
    /// * `syscfg` - Can be passed optionally to enable the peripheral clock
    pub fn new(
+        syscfg: &mut pac::Sysconfig,
+        clocks: &crate::clock::Clocks,
        spi: SpiI,
        pins: (Sck, Miso, Mosi),
-        clocks: &crate::clock::Clocks,
        spi_cfg: SpiConfig,
-        syscfg: &mut pac::Sysconfig,
        transfer_cfg: Option<&ErasedTransferConfig>,
-    ) -> Self {
+    ) -> Result<Self, SpiClkConfigError> {
        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);
@ -667,16 +819,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,
@ -686,36 +839,39 @@ where
                word: PhantomData,
            },
            pins,
+        })
+    }
+
+    delegate::delegate! {
+        to self.inner {
+            #[inline]
+            pub fn cfg_clock_from_div(&mut self, div: u16) -> Result<(), SpiClkConfigError>;
+
+            #[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<HwCs: OptionalHwCs<SpiI>>(
+                &mut self, transfer_cfg: &TransferConfig<HwCs>
+            ) -> Result<(), SpiClkConfigError>;
        }
    }

    #[inline]
-    pub fn cfg_clock(&mut self, spi_clk: impl Into<Hertz>) {
-        self.inner.cfg_clock(spi_clk);
-    }
-
-    #[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<HwCs: OptionalHwCs<SpiI>>(&mut self, transfer_cfg: &TransferConfig<HwCs>) {
-        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)
--- 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;
@ -336,27 +335,23 @@ pub struct UartWithIrqBase<UART> {

 /// Serial receiver
 pub struct Rx<Uart> {
-    _usart: PhantomData<Uart>,
+    uart: Uart,
 }

 /// Serial transmitter
 pub struct Tx<Uart> {
-    _usart: PhantomData<Uart>,
+    uart: Uart,
 }

-impl<Uart> Rx<Uart> {
-    fn new() -> Self {
-        Self {
-            _usart: PhantomData,
-        }
+impl<Uart: Instance> Rx<Uart> {
+    fn new(uart: Uart) -> Self {
+        Self { uart }
    }
 }

 impl<Uart> Tx<Uart> {
-    fn new() -> Self {
-        Self {
-            _usart: PhantomData,
-        }
+    fn new(uart: Uart) -> Self {
+        Self { uart }
    }
 }

@ -366,6 +361,12 @@ pub trait Instance: Deref<Target = uart_base::RegisterBlock> {
    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;
 }

@ -375,6 +376,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 _
    }
@ -386,6 +390,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 _
    }
@ -397,6 +404,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 _
    }
@ -551,8 +561,8 @@ impl<TxPinInst: TxPin<UartInstance>, RxPinInst: RxPin<UartInstance>, UartInstanc
        Uart {
            inner: UartBase {
                uart,
-                tx: Tx::new(),
-                rx: Rx::new(),
+                tx: Tx::new(unsafe { UartInstance::steal() }),
+                rx: Rx::new(unsafe { UartInstance::steal() }),
            },
            pins,
        }
@ -570,8 +580,8 @@ impl<TxPinInst: TxPin<UartInstance>, RxPinInst: RxPin<UartInstance>, UartInstanc
        Uart {
            inner: UartBase {
                uart,
-                tx: Tx::new(),
-                rx: Rx::new(),
+                tx: Tx::new(unsafe { UartInstance::steal() }),
+                rx: Rx::new(unsafe { UartInstance::steal() }),
            },
            pins,
        }
@ -656,6 +666,36 @@ impl<TxPinInst: TxPin<UartInstance>, RxPinInst: RxPin<UartInstance>, UartInstanc
    }
 }

+impl<Uart: Instance> Rx<Uart> {
+    /// 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<Uart: Instance> Tx<Uart> {
+    /// 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,
--- 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,