From 74eebdcc035bc1d7d07d0c502f76e9b012c0f32b Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Fri, 10 Jan 2025 15:14:18 +0100
Subject: [PATCH] bootloader and flashloader update

---
 bootloader/Cargo.toml              |  1 +
 bootloader/README.md               | 28 +++++-----
 bootloader/src/main.rs             | 36 +++++++++----
 flashloader/Cargo.toml             |  1 +
 flashloader/README.md              |  9 ++++
 flashloader/image-loader.py        | 85 +++++++++++++++++++++++-------
 flashloader/slot-b-blinky/memory.x |  4 +-
 flashloader/src/main.rs            | 33 +++++++++++-
 8 files changed, 153 insertions(+), 44 deletions(-)

diff --git a/bootloader/Cargo.toml b/bootloader/Cargo.toml
index a2e3242..37a4e84 100644
--- a/bootloader/Cargo.toml
+++ b/bootloader/Cargo.toml
@@ -11,6 +11,7 @@ panic-rtt-target = { version = "0.1.3" }
 panic-halt = { version = "0.2" }
 rtt-target = { version = "0.5" }
 crc = "3"
+num_enum = { version = "0.7", default-features = false }
 static_assertions = "1"
 
 [dependencies.va108xx-hal]
diff --git a/bootloader/README.md b/bootloader/README.md
index a920fff..3e2918f 100644
--- a/bootloader/README.md
+++ b/bootloader/README.md
@@ -9,14 +9,15 @@ The bootloader uses the following memory map:
 
 | Address | Notes | Size |
 | ------ | ---- |  ---- |
-| 0x0 | Bootloader start | code up to 0x3FFC bytes |
-| 0x2FFE | Bootloader CRC | word |
-| 0x3000 | App image A start | code up to 0xE7F8 (~58K) bytes |
+| 0x0 | Bootloader start | code up to 0x2FFE bytes |
+| 0x2FFE | Bootloader CRC | half-word |
+| 0x3000 | App image A start | code up to 0xE7F4 (~59K) bytes |
 | 0x117F8 | App image A CRC check length | word |
 | 0x117FC | App image A CRC check value | word |
-| 0x11800 | App image B start | code up to 0xE7F8 (~58K) bytes |
-| 0x1FFF8 | App image B CRC check length | word |
-| 0x1FFFC | App image B CRC check value | word |
+| 0x117FC | App image B start | code up to 0xE7F4 (~59K) bytes |
+| 0x1FFF0 | App image B CRC check length | word |
+| 0x1FFF4 | App image B CRC check value | word |
+| 0x1FFF8 | Reserved section, contains boot select parameter | 8 bytes |
 | 0x20000 | End of NVM | end  |
 
 ## Additional Information
@@ -35,13 +36,16 @@ The bootloader performs the following steps:
 1. The application will calculate the checksum of itself if the bootloader CRC is blank (all zeroes
    or all ones). If the CRC is not blank and the checksum check fails, it will immediately boot
    application image A. Otherwise, it proceeds to the next step.
-2. Check the checksum of App A. If that checksum is valid, it will boot App A. If not, it will
-   proceed to the next step.
-3. Check the checksum of App B. If that checksum is valid, it will boot App B. If not, it will
-   boot App A as the fallback image.
+2. Read the boot slot from a reserved section at the end of the EEPROM. It is assumed that the full
+   128 kB are copied from the ST EEPROM to the code RAM at startup. The boot slot is read from
+   the code RAM directly.
+3. Check the checksum of the boot slot. If that checksum is valid, it will boot that slot. If not,
+   it will proceed to the next step.
+4. Check the checksum of the other slot . If that checksum is valid, it will boot that slot. If
+   not, it will boot App A as the fallback image.
 
-You could adapt and combine this bootloader with a non-volatile memory to select a prefered app
-image, which would be a first step towards an updatable flight software.
+In your actual production application, a command to update the preferred boot slot could be exposed
+to allow performing software updates in a safe way.
 
 Please note that you *MUST* compile the application at slot A and slot B with an appropriate
 `memory.x` file where the base address of the `FLASH` was adapted according to the base address
diff --git a/bootloader/src/main.rs b/bootloader/src/main.rs
index 2ac00f7..4df8675 100644
--- a/bootloader/src/main.rs
+++ b/bootloader/src/main.rs
@@ -5,6 +5,7 @@ use bootloader::NvmInterface;
 use cortex_m_rt::entry;
 use crc::{Crc, CRC_16_IBM_3740};
 use embedded_hal::delay::DelayNs;
+use num_enum::TryFromPrimitive;
 #[cfg(not(feature = "rtt-panic"))]
 use panic_halt as _;
 #[cfg(feature = "rtt-panic")]
@@ -59,8 +60,9 @@ const APP_B_SIZE_ADDR: u32 = APP_B_END_ADDR - 8;
 // Four bytes reserved, even when only 2 byte CRC is used. Leaves flexibility to switch to CRC32.
 // 0x1FFFC
 const APP_B_CRC_ADDR: u32 = APP_B_END_ADDR - 4;
-// 0x20000
-pub const APP_B_END_ADDR: u32 = NVM_SIZE;
+// 0x20000. 8 bytes at end of EEPROM reserved for preferred image parameter. This reserved
+// size should be a multiple of 8 due to alignment requirements.
+pub const APP_B_END_ADDR: u32 = NVM_SIZE - 8;
 pub const APP_IMG_SZ: u32 = (APP_B_END_ADDR - APP_A_START_ADDR) / 2;
 
 static_assertions::const_assert!((APP_B_END_ADDR - BOOTLOADER_END_ADDR) % 2 == 0);
@@ -68,13 +70,15 @@ static_assertions::const_assert!((APP_B_END_ADDR - BOOTLOADER_END_ADDR) % 2 == 0
 pub const VECTOR_TABLE_OFFSET: u32 = 0x0;
 pub const VECTOR_TABLE_LEN: u32 = 0xC0;
 pub const RESET_VECTOR_OFFSET: u32 = 0x4;
+pub const PREFERRED_SLOT_OFFSET: u32 = 0x20000 - 1;
 
 const CRC_ALGO: Crc<u16> = Crc::<u16>::new(&CRC_16_IBM_3740);
 
-#[derive(Debug, Copy, Clone, PartialEq, Eq)]
+#[derive(Debug, Copy, Clone, PartialEq, Eq, TryFromPrimitive)]
+#[repr(u8)]
 enum AppSel {
-    A,
-    B,
+    A = 0,
+    B = 1,
 }
 
 pub struct NvmWrapper(pub M95M01);
@@ -154,10 +158,24 @@ fn main() -> ! {
     // Check bootloader's CRC (and write it if blank)
     check_own_crc(&dp.sysconfig, &cp, &mut nvm, &mut timer);
 
-    if check_app_crc(AppSel::A) {
-        boot_app(&dp.sysconfig, &cp, AppSel::A, &mut timer)
-    } else if check_app_crc(AppSel::B) {
-        boot_app(&dp.sysconfig, &cp, AppSel::B, &mut timer)
+    // This is technically read from the EEPROM. We assume that the full 128 kB were copied
+    // from the EEPROM to the code RAM and read the boot slot from the code ram directly.
+    let preferred_app = AppSel::try_from(unsafe {
+        (PREFERRED_SLOT_OFFSET as *const u8)
+            .read_unaligned()
+            .to_be()
+    })
+    .unwrap_or(AppSel::A);
+    let other_app = if preferred_app == AppSel::A {
+        AppSel::B
+    } else {
+        AppSel::A
+    };
+
+    if check_app_crc(preferred_app) {
+        boot_app(&dp.sysconfig, &cp, preferred_app, &mut timer)
+    } else if check_app_crc(other_app) {
+        boot_app(&dp.sysconfig, &cp, other_app, &mut timer)
     } else {
         if DEBUG_PRINTOUTS && RTT_PRINTOUT {
             rprintln!("both images corrupt! booting image A");
diff --git a/flashloader/Cargo.toml b/flashloader/Cargo.toml
index b88151f..65e701a 100644
--- a/flashloader/Cargo.toml
+++ b/flashloader/Cargo.toml
@@ -11,6 +11,7 @@ embedded-hal-nb = "1"
 embedded-io = "0.6"
 panic-rtt-target = { version = "0.1.3" }
 rtt-target = { version = "0.5" }
+num_enum = { version = "0.7", default-features = false }
 log = "0.4"
 crc = "3"
 
diff --git a/flashloader/README.md b/flashloader/README.md
index 86a8d00..242b9f9 100644
--- a/flashloader/README.md
+++ b/flashloader/README.md
@@ -59,6 +59,15 @@ to write it to slot A.
 
 You can use
 
+```sh
+./image-loader.py -s a
+```
+
+to select the Slot A as a boot slot. The boot slot is stored in a reserved section in EEPROM
+and will be read and used by the bootloader to determine which slot to boot.
+
+You can use
+
 ```sh
 ./image-loader.py -c -t a
 ```
diff --git a/flashloader/image-loader.py b/flashloader/image-loader.py
index ee3178d..e260502 100755
--- a/flashloader/image-loader.py
+++ b/flashloader/image-loader.py
@@ -30,20 +30,21 @@ BOOTLOADER_CRC_ADDR = BOOTLOADER_END_ADDR - 2
 BOOTLOADER_MAX_SIZE = BOOTLOADER_END_ADDR - BOOTLOADER_START_ADDR - 2
 
 APP_A_START_ADDR = 0x3000
-APP_A_END_ADDR = 0x11800
+APP_B_END_ADDR = 0x20000 - 8
+IMG_SLOT_SIZE = (APP_B_END_ADDR - APP_A_START_ADDR) // 2
+
+APP_A_END_ADDR = APP_A_START_ADDR + IMG_SLOT_SIZE
 # The actual size of the image which is relevant for CRC calculation.
 APP_A_SIZE_ADDR = APP_A_END_ADDR - 8
 APP_A_CRC_ADDR = APP_A_END_ADDR - 4
 APP_A_MAX_SIZE = APP_A_END_ADDR - APP_A_START_ADDR - 8
 
 APP_B_START_ADDR = APP_A_END_ADDR
-APP_B_END_ADDR = 0x20000
 # The actual size of the image which is relevant for CRC calculation.
 APP_B_SIZE_ADDR = APP_B_END_ADDR - 8
 APP_B_CRC_ADDR = APP_B_END_ADDR - 4
 APP_B_MAX_SIZE = APP_A_END_ADDR - APP_A_START_ADDR - 8
 
-APP_IMG_SZ = (APP_B_END_ADDR - APP_A_START_ADDR) // 2
 
 CHUNK_SIZE = 400
 
@@ -58,6 +59,7 @@ PING_PAYLOAD_SIZE = 0
 class ActionId(enum.IntEnum):
     CORRUPT_APP_A = 128
     CORRUPT_APP_B = 129
+    SET_BOOT_SLOT = 130
 
 
 _LOGGER = logging.getLogger(__name__)
@@ -78,11 +80,37 @@ class Target(enum.Enum):
     APP_B = 2
 
 
+class AppSel(enum.IntEnum):
+    APP_A = 0
+    APP_B = 1
+
+
 class ImageLoader:
     def __init__(self, com_if: ComInterface, verificator: PusVerificator) -> None:
         self.com_if = com_if
         self.verificator = verificator
 
+    def handle_boot_sel_cmd(self, target: AppSel):
+        _LOGGER.info("Sending ping command")
+        action_tc = PusTc(
+            apid=0x00,
+            service=PusService.S8_FUNC_CMD,
+            subservice=ActionId.SET_BOOT_SLOT,
+            seq_count=SEQ_PROVIDER.get_and_increment(),
+            app_data=bytes([target]),
+        )
+        self.verificator.add_tc(action_tc)
+        self.com_if.send(bytes(action_tc.pack()))
+        data_available = self.com_if.data_available(0.4)
+        if not data_available:
+            _LOGGER.warning("no reply received for boot image selection command")
+        for reply in self.com_if.receive():
+            result = self.verificator.add_tm(
+                Service1Tm.from_tm(PusTm.unpack(reply, 0), UnpackParams(0))
+            )
+            if result is not None and result.completed:
+                _LOGGER.info("received boot image selection command confirmation")
+
     def handle_ping_cmd(self):
         _LOGGER.info("Sending ping command")
         ping_tc = PusTc(
@@ -106,7 +134,6 @@ class ImageLoader:
                 _LOGGER.info("received ping completion reply")
 
     def handle_corruption_cmd(self, target: Target):
-
         if target == Target.BOOTLOADER:
             _LOGGER.error("can not corrupt bootloader")
         if target == Target.APP_A:
@@ -131,7 +158,8 @@ class ImageLoader:
         _LOGGER.info("Parsing ELF file for loadable sections")
         total_size = 0
         loadable_segments, total_size = create_loadable_segments(target, file_path)
-        segments_info_str(target, loadable_segments, total_size, file_path)
+        check_segments(target, total_size)
+        print_segments_info(target, loadable_segments, total_size, file_path)
         result = self._perform_flashing_algorithm(loadable_segments)
         if result != 0:
             return result
@@ -251,6 +279,9 @@ def main() -> int:
         prog="image-loader", description="Python VA416XX Image Loader Application"
     )
     parser.add_argument("-p", "--ping", action="store_true", help="Send ping command")
+    parser.add_argument(
+        "-s", "--sel", choices=["a", "b"], help="Set boot slot (Slot A or B)"
+    )
     parser.add_argument("-c", "--corrupt", action="store_true", help="Corrupt a target")
     parser.add_argument(
         "-t",
@@ -286,6 +317,14 @@ def main() -> int:
         target = Target.APP_A
     elif args.target == "b":
         target = Target.APP_B
+
+    boot_sel = None
+    if args.sel:
+        if args.sel == "a":
+            boot_sel = AppSel.APP_A
+        elif args.sel == "b":
+            boot_sel = AppSel.APP_B
+
     image_loader = ImageLoader(com_if, verificator)
     file_path = None
     result = -1
@@ -293,6 +332,8 @@ def main() -> int:
         image_loader.handle_ping_cmd()
         com_if.close()
         return 0
+    if args.sel and boot_sel is not None:
+        image_loader.handle_boot_sel_cmd(boot_sel)
     if target:
         if not args.corrupt:
             if not args.path:
@@ -307,9 +348,9 @@ def main() -> int:
             return -1
         image_loader.handle_corruption_cmd(target)
     else:
-        assert file_path is not None
-        assert target is not None
-        result = image_loader.handle_flash_cmd(target, file_path)
+        if file_path is not None:
+            assert target is not None
+            result = image_loader.handle_flash_cmd(target, file_path)
 
     com_if.close()
     return result
@@ -377,7 +418,22 @@ def create_loadable_segments(
     return loadable_segments, total_size
 
 
-def segments_info_str(
+def check_segments(
+    target: Target,
+    total_size: int,
+):
+    # Set context string and perform basic sanity checks.
+    if target == Target.BOOTLOADER and total_size > BOOTLOADER_MAX_SIZE:
+        raise ValueError(
+            f"provided bootloader app larger than allowed {total_size} bytes"
+        )
+    elif target == Target.APP_A and total_size > APP_A_MAX_SIZE:
+        raise ValueError(f"provided App A larger than allowed {total_size} bytes")
+    elif target == Target.APP_B and total_size > APP_B_MAX_SIZE:
+        raise ValueError(f"provided App B larger than allowed {total_size} bytes")
+
+
+def print_segments_info(
     target: Target,
     loadable_segments: List[LoadableSegment],
     total_size: int,
@@ -385,21 +441,10 @@ def segments_info_str(
 ):
     # Set context string and perform basic sanity checks.
     if target == Target.BOOTLOADER:
-        if total_size > BOOTLOADER_MAX_SIZE:
-            _LOGGER.error(
-                f"provided bootloader app larger than allowed {total_size} bytes"
-            )
-            return -1
         context_str = "Bootloader"
     elif target == Target.APP_A:
-        if total_size > APP_A_MAX_SIZE:
-            _LOGGER.error(f"provided App A larger than allowed {total_size} bytes")
-            return -1
         context_str = "App Slot A"
     elif target == Target.APP_B:
-        if total_size > APP_B_MAX_SIZE:
-            _LOGGER.error(f"provided App B larger than allowed {total_size} bytes")
-            return -1
         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):
diff --git a/flashloader/slot-b-blinky/memory.x b/flashloader/slot-b-blinky/memory.x
index 4f7b2cf..e499487 100644
--- a/flashloader/slot-b-blinky/memory.x
+++ b/flashloader/slot-b-blinky/memory.x
@@ -1,7 +1,7 @@
-/* Special linker script for application slot B with an offset at address 0x11800 */
+/* Special linker script for application slot B */
 MEMORY
 {
-	FLASH : ORIGIN = 0x00011800, LENGTH = 0xE800
+	FLASH : ORIGIN = 0x000117FC, LENGTH = 0xE800
 	RAM : ORIGIN = 0x10000000, LENGTH = 0x08000 /* 32K */
 }
 
diff --git a/flashloader/src/main.rs b/flashloader/src/main.rs
index eb6e356..2f122da 100644
--- a/flashloader/src/main.rs
+++ b/flashloader/src/main.rs
@@ -3,6 +3,7 @@
 #![no_main]
 #![no_std]
 
+use num_enum::TryFromPrimitive;
 use once_cell::sync::Lazy;
 use panic_rtt_target as _;
 use ringbuf::{
@@ -26,6 +27,14 @@ const RX_DEBUGGING: bool = false;
 pub enum ActionId {
     CorruptImageA = 128,
     CorruptImageB = 129,
+    SetBootSlot = 130,
+}
+
+#[derive(Debug, Copy, Clone, PartialEq, Eq, TryFromPrimitive)]
+#[repr(u8)]
+enum AppSel {
+    A = 0,
+    B = 1,
 }
 
 // Larger buffer for TC to be able to hold the possibly large memory write packets.
@@ -58,10 +67,12 @@ pub struct DataConsumer<const BUF_SIZE: usize, const SIZES_LEN: usize> {
 }
 
 pub const APP_A_START_ADDR: u32 = 0x3000;
-pub const APP_A_END_ADDR: u32 = 0x11800;
+pub const APP_A_END_ADDR: u32 = 0x117FC;
 pub const APP_B_START_ADDR: u32 = APP_A_END_ADDR;
 pub const APP_B_END_ADDR: u32 = 0x20000;
 
+pub const PREFERRED_SLOT_OFFSET: u32 = 0x20000 - 1;
+
 #[rtic::app(device = pac, dispatchers = [OC20, OC21, OC22])]
 mod app {
     use super::*;
@@ -346,6 +357,26 @@ mod app {
                 rprintln!("corrupting App Image B");
                 corrupt_image(APP_B_START_ADDR);
             }
+            if pus_tc.subservice() == ActionId::SetBootSlot as u8 {
+                if pus_tc.app_data().is_empty() {
+                    log::warn!(target: "TC Handler", "App data for preferred image command too short");
+                }
+                let app_sel_result = AppSel::try_from(pus_tc.app_data()[0]);
+                if app_sel_result.is_err() {
+                    log::warn!("Invalid app selection value: {}", pus_tc.app_data()[0]);
+                }
+                log::info!(target: "TC Handler", "received boot selection command with app select: {:?}", app_sel_result.unwrap());
+                cx.local
+                    .nvm
+                    .write(PREFERRED_SLOT_OFFSET as usize, &[pus_tc.app_data()[0]])
+                    .expect("writing to NVM failed");
+                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.service() == PusServiceId::Test as u8 && pus_tc.subservice() == 1 {
             log::info!(target: "TC Handler", "received ping TC");