Bootloader and Flashloader App

flashloader/.gitignore

@@ -0,0 +1 @@
+/venv

flashloader/Cargo.toml

@@ -0,0 +1,51 @@
+[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"
+embedded-io = "0.6"
+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.satrs]
+version = "0.2"
+default-features = false
+
+[dependencies.ringbuf]
+version = "0.4"
+default-features = false
+
+[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"
+features = ["va41630"]
+
+[dependencies.rtic]
+version = "2"
+features = ["thumbv7-backend"]
+
+[dependencies.rtic-monotonics]
+version = "2"
+features = ["cortex-m-systick"]

flashloader/README.md

@@ -0,0 +1,60 @@
+VA416xx Flashloader Application
+========
+
+This flashloader shows a minimal example for a self-updatable Rust software which exposes
+a simple PUS (CCSDS) interface to update the software. It also provides a Python application
+called the `image-loader.py` which can be used to upload compiled images to the flashloader
+application to write them to the NVM.
+
+The software can quickly be adapted to interface with a real primary on-board software instead of
+the Python script provided here to upload images because it uses a low-level CCSDS based packet
+interface.
+
+## Using the Python image loader
+
+It is recommended to run the script in a dedicated virtual environment. For example, on UNIX
+systems you can use `python3 -m venv venv` and then `source venv/bin/activate` to create
+and activate a virtual environment.
+
+After that, you can use
+
+```sh
+pip install -r requirements.txt
+```
+
+to install all required dependencies.
+
+After that, it is recommended to use `./image-load.py -h` to get an overview of some options.
+The flash loader uses the UART0 interface of the VA416xx board to perform CCSDS based
+communication. The Python image loader application will search for a file named `loader.toml` and
+use the `serial_port` key to determine the serial port to use for serial communication.
+
+### Examples
+
+You can use
+
+```sh
+./image-loader.py -p
+```
+
+to send a ping an verify the connection.
+
+You can use
+
+```sh
+cd flashloader/slot-a-blinky
+cargo build --release
+cd ../..
+./image-loader.py -t a ./slot-a-blinky/target/thumbv7em-none-eabihf/release/slot-a-blinky
+```
+
+to build the slot A sample application and upload it to a running flash loader application
+to write it to slot A.
+
+You can use
+
+```sh
+./image-loader.py -c -t a
+```
+
+to corrupt the image A and test that it switches to image B after a failed CRC check instead.

flashloader/image-loader.py

@@ -0,0 +1,319 @@
+#!/usr/bin/env python3
+from spacepackets.ecss import RequestId
+from spacepackets.ecss.defs import PusService
+from spacepackets.ecss.tm import PusTm
+import toml
+import struct
+import logging
+import argparse
+import threading
+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 spacepackets.ecss.pus_verificator import PusVerificator, StatusField
+from spacepackets.ecss.pus_1_verification import Service1Tm, UnpackParams
+from spacepackets.seqcount import SeqCountProvider
+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
+
+
+SEQ_PROVIDER = SeqCountProvider(bit_width=14)
+
+
+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,
+    )
+    verificator = PusVerificator()
+    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,
+            seq_count=SEQ_PROVIDER.get_and_increment(),
+        )
+        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
+                    ]
+                    next_packet = pack_memory_write_command(current_addr, data)
+                    _LOGGER.info(
+                        f"Sending memory write command for address {current_addr:#08x} and data with "
+                        f"length {len(data)}"
+                    )
+                    verificator.add_tc(next_packet)
+                    com_if.send(next_packet.pack())
+                    current_addr += next_chunk_size
+                    pos_in_segment += next_chunk_size
+                    while True:
+                        data_available = com_if.data_available(0.1)
+                        done = False
+                        if not data_available:
+                            continue
+                        replies = com_if.receive()
+                        for reply in replies:
+                            tm = PusTm.unpack(reply, 0)
+                            if tm.service != 1:
+                                continue
+                            service_1_tm = Service1Tm.from_tm(tm, UnpackParams(0))
+                            check_result = verificator.add_tm(service_1_tm)
+                            # We could send after we have received the step reply, but that can
+                            # somehow lead to overrun errors. I think it's okay to do it like
+                            # this as long as the flash loader only uses polling..
+                            if (
+                                check_result is not None
+                                and check_result.status.completed == StatusField.SUCCESS
+                            ):
+                                done = True
+                                # Still keep a small delay
+                                time.sleep(0.01)
+                        verificator.remove_completed_entries()
+                        if done:
+                            break
+            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())
+    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,
+        seq_count=SEQ_PROVIDER.get_and_increment(),
+        app_data=app_data,
+    )
+
+
+if __name__ == "__main__":
+    main()

flashloader/loader.toml

@@ -0,0 +1 @@
+serial_port = "/dev/ttyUSB0"

flashloader/requirements.txt

@@ -0,0 +1,5 @@
+spacepackets == 0.24
+tmtccmd == 8.0.2
+toml == 0.10
+pyelftools == 0.31
+crcmod == 1.7

flashloader/slot-a-blinky/.gitignore

@@ -0,0 +1,2 @@
+/target
+/app.map

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.

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
+};

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();
+    }
+}

flashloader/slot-b-blinky/.gitignore

@@ -0,0 +1,2 @@
+/target
+/app.map

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.

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
+};

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();
+    }
+}

flashloader/src/lib.rs

@@ -0,0 +1,9 @@
+#![no_std]
+
+#[cfg(test)]
+mod tests {
+    #[test]
+    fn simple() {
+        assert_eq!(1 + 1, 2);
+    }
+}

flashloader/src/main.rs

@@ -0,0 +1,557 @@
+//! 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_TC_SIZE: usize = 1024;
+const MAX_TC_FRAME_SIZE: usize = cobs::max_encoding_length(MAX_TC_SIZE);
+
+const MAX_TM_SIZE: usize = 128;
+const MAX_TM_FRAME_SIZE: usize = cobs::max_encoding_length(MAX_TM_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();
+        }
+    }
+}
+
+use once_cell::sync::Lazy;
+use ringbuf::{
+    traits::{Consumer, Observer, Producer, SplitRef},
+    CachingCons, StaticProd, StaticRb,
+};
+
+const BUF_RB_SIZE_TX: usize = 1024;
+const SIZES_RB_SIZE_TX: usize = 16;
+
+static mut BUF_RB_TX: Lazy<StaticRb<u8, BUF_RB_SIZE_TX>> =
+    Lazy::new(StaticRb::<u8, BUF_RB_SIZE_TX>::default);
+static mut SIZES_RB_TX: Lazy<StaticRb<usize, SIZES_RB_SIZE_TX>> =
+    Lazy::new(StaticRb::<usize, SIZES_RB_SIZE_TX>::default);
+
+pub struct DataProducer<const BUF_SIZE: usize, const SIZES_LEN: usize> {
+    pub buf_prod: StaticProd<'static, u8, BUF_SIZE>,
+    pub sizes_prod: StaticProd<'static, usize, SIZES_LEN>,
+}
+
+pub struct DataConsumer<const BUF_SIZE: usize, const SIZES_LEN: usize> {
+    pub buf_cons: CachingCons<&'static StaticRb<u8, BUF_SIZE>>,
+    pub sizes_cons: CachingCons<&'static StaticRb<usize, SIZES_LEN>>,
+}
+
+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 embedded_io::Write;
+    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 satrs::pus::verification::VerificationReportCreator;
+    use spacepackets::ecss::PusServiceId;
+    use spacepackets::ecss::{
+        tc::PusTcReader, tm::PusTmCreator, EcssEnumU8, PusPacket, WritablePusPacket,
+    };
+    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>,
+        tx_cons: DataConsumer<BUF_RB_SIZE_TX, SIZES_RB_SIZE_TX>,
+        verif_reporter: VerificationReportCreator,
+    }
+
+    #[shared]
+    struct Shared {
+        decode_buffer_busy: bool,
+        decode_buf: [u8; MAX_TC_SIZE],
+        tx_prod: DataProducer<BUF_RB_SIZE_TX, SIZES_RB_SIZE_TX>,
+    }
+
+    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);
+
+        let verif_reporter = VerificationReportCreator::new(0).unwrap();
+
+        let (buf_prod, buf_cons) = unsafe { BUF_RB_TX.split_ref() };
+        let (sizes_prod, sizes_cons) = unsafe { SIZES_RB_TX.split_ref() };
+
+        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_TC_SIZE],
+                tx_prod: DataProducer {
+                    buf_prod,
+                    sizes_prod,
+                },
+            },
+            Local {
+                uart_rx: rx,
+                uart_tx: tx,
+                cobs_reader_state: CobsReaderStates::default(),
+                tc_tx,
+                tc_rx,
+                rom_spi: Some(cx.device.spi3),
+                tx_cons: DataConsumer {
+                    buf_cons,
+                    sizes_cons,
+                },
+                verif_reporter,
+            },
+        )
+    }
+
+    // `shared` cannot be accessed from this context
+    #[idle]
+    fn idle(_cx: idle::Context) -> ! {
+        loop {
+            asm::nop();
+        }
+    }
+
+    #[task(
+        priority = 4,
+        local=[
+            read_buf: [u8;MAX_TC_FRAME_SIZE] = [0; MAX_TC_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 => (),
+                                uart::Error::ParityError => (),
+                                uart::Error::BreakCondition => (),
+                                uart::Error::TransferPending => (),
+                                uart::Error::BufferTooShort => (),
+                            }
+                        }
+                        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_TC_FRAME_SIZE] = [0; MAX_TC_FRAME_SIZE],
+            src_data_buf: [u8; 16] = [0; 16],
+            verif_buf: [u8; 32] = [0; 32],
+            tc_rx,
+            rom_spi,
+            verif_reporter
+        ],
+        shared=[decode_buffer_busy, decode_buf, tx_prod]
+    )]
+    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, _)) => {
+                    let mut write_and_send = |tm: &PusTmCreator| {
+                        let written_size = tm.write_to_bytes(cx.local.verif_buf).unwrap();
+                        cx.shared.tx_prod.lock(|prod| {
+                            prod.sizes_prod.try_push(tm.len_written()).unwrap();
+                            prod.buf_prod
+                                .push_slice(&cx.local.verif_buf[0..written_size]);
+                        });
+                    };
+                    let token = cx.local.verif_reporter.add_tc(&pus_tc);
+                    let (tm, accepted_token) = cx
+                        .local
+                        .verif_reporter
+                        .acceptance_success(cx.local.src_data_buf, token, 0, 0, &[])
+                        .expect("acceptance success failed");
+                    write_and_send(&tm);
+
+                    let (tm, started_token) = cx
+                        .local
+                        .verif_reporter
+                        .start_success(cx.local.src_data_buf, accepted_token, 0, 0, &[])
+                        .expect("acceptance success failed");
+                    write_and_send(&tm);
+
+                    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));
+                            let tm = cx
+                                .local
+                                .verif_reporter
+                                .completion_success(cx.local.src_data_buf, started_token, 0, 0, &[])
+                                .expect("completion success failed");
+                            write_and_send(&tm);
+                        };
+                        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 {
+                        let tm = cx
+                            .local
+                            .verif_reporter
+                            .step_success(
+                                cx.local.src_data_buf,
+                                &started_token,
+                                0,
+                                0,
+                                &[],
+                                EcssEnumU8::new(0),
+                            )
+                            .expect("step success failed");
+                        write_and_send(&tm);
+                        // 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));
+                            let tm = cx
+                                .local
+                                .verif_reporter
+                                .completion_success(cx.local.src_data_buf, started_token, 0, 0, &[])
+                                .expect("completion success failed");
+                            write_and_send(&tm);
+                            log::info!("NVM operation done");
+                        }
+                    }
+                }
+                Err(e) => {
+                    log::warn!("PUS TC error: {}", e);
+                }
+            }
+        }
+    }
+
+    #[task(
+        priority = 1,
+        local=[
+            read_buf: [u8;MAX_TM_SIZE] = [0; MAX_TM_SIZE],
+            encoded_buf: [u8;MAX_TM_FRAME_SIZE] = [0; MAX_TM_FRAME_SIZE],
+            uart_tx,
+            tx_cons,
+        ],
+        shared=[]
+    )]
+    async fn pus_tm_tx_handler(cx: pus_tm_tx_handler::Context) {
+        loop {
+            while cx.local.tx_cons.sizes_cons.occupied_len() > 0 {
+                let next_size = cx.local.tx_cons.sizes_cons.try_pop().unwrap();
+                cx.local
+                    .tx_cons
+                    .buf_cons
+                    .pop_slice(&mut cx.local.read_buf[0..next_size]);
+                cx.local.encoded_buf[0] = 0;
+                let send_size = cobs::encode(
+                    &cx.local.read_buf[0..next_size],
+                    &mut cx.local.encoded_buf[1..],
+                );
+                cx.local.encoded_buf[send_size + 1] = 0;
+                cx.local
+                    .uart_tx
+                    .write(&cx.local.encoded_buf[0..send_size + 2])
+                    .unwrap();
+                Mono::delay(2.millis()).await;
+            }
+            Mono::delay(30.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();
+}