add reply handling

flashloader/Cargo.toml

@@ -16,6 +16,14 @@ 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
@@ -32,6 +40,7 @@ default-features = false
 
 [dependencies.va416xx-hal]
 path = "../va416xx-hal"
+features = ["va41630"]
 
 [dependencies.rtic]
 version = "2"

flashloader/image-loader.py

@@ -1,9 +1,12 @@
 #!/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
@@ -11,6 +14,9 @@ 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
@@ -57,6 +63,9 @@ class LoadableSegment:
     data: bytes
 
 
+SEQ_PROVIDER = SeqCountProvider(bit_width=14)
+
+
 def main() -> int:
     print("Python VA416XX Image Loader Application")
     logging.basicConfig(
@@ -91,6 +100,7 @@ def main() -> int:
         baud_rate=BAUD_RATE,
         serial_timeout=0.1,
     )
+    verificator = PusVerificator()
     com_if = SerialCobsComIF(serial_cfg)
     com_if.open()
     file_path = None
@@ -105,7 +115,12 @@ def main() -> int:
                 return -1
     if args.ping:
         _LOGGER.info("Sending ping command")
-        ping_tc = PusTc(apid=0x00, service=PusService.S17_TEST, subservice=1)
+        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:
@@ -148,12 +163,18 @@ def main() -> int:
                             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:
+                    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:
+                    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}"
@@ -203,15 +224,40 @@ def main() -> int:
                     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)}"
                     )
-                    next_packet = pack_memory_write_command(current_addr, data)
+                    verificator.add_tc(next_packet)
                     com_if.send(next_packet.pack())
                     current_addr += next_chunk_size
                     pos_in_segment += next_chunk_size
-                    time.sleep(0.2)
+                    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
@@ -232,7 +278,7 @@ def main() -> int:
                 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}"
+                    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)
@@ -248,13 +294,6 @@ def main() -> int:
                 )
                 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
 
@@ -271,6 +310,7 @@ def pack_memory_write_command(addr: int, data: bytes) -> PusTc:
         apid=0,
         service=MEMORY_SERVICE,
         subservice=RAW_MEMORY_WRITE_SUBSERVICE,
+        seq_count=SEQ_PROVIDER.get_and_increment(),
         app_data=app_data,
     )
 

flashloader/src/main.rs

@@ -25,8 +25,13 @@ 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 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;
@@ -51,6 +56,30 @@ impl WdtInterface for OptWdt {
     }
 }
 
+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;
@@ -72,8 +101,11 @@ mod app {
         make_channel,
     };
     use rtt_target::rprintln;
+    use satrs::pus::verification::VerificationReportCreator;
     use spacepackets::ecss::PusServiceId;
-    use spacepackets::ecss::{tc::PusTcReader, PusPacket};
+    use spacepackets::ecss::{
+        tc::PusTcReader, tm::PusTmCreator, EcssEnumU8, PusPacket, WritablePusPacket,
+    };
     use va416xx_hal::{
         clock::ClkgenExt,
         edac,
@@ -101,12 +133,15 @@ mod app {
         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_PACKET_SIZE],
+        decode_buf: [u8; MAX_TC_SIZE],
+        tx_prod: DataProducer<BUF_RB_SIZE_TX, SIZES_RB_SIZE_TX>,
     }
 
     pub type TcTx = Sender<'static, usize, 2>;
@@ -144,6 +179,11 @@ mod app {
         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();
@@ -152,7 +192,11 @@ mod app {
         (
             Shared {
                 decode_buffer_busy: false,
-                decode_buf: [0; MAX_PACKET_SIZE],
+                decode_buf: [0; MAX_TC_SIZE],
+                tx_prod: DataProducer {
+                    buf_prod,
+                    sizes_prod,
+                },
             },
             Local {
                 uart_rx: rx,
@@ -161,6 +205,11 @@ mod app {
                 tc_tx,
                 tc_rx,
                 rom_spi: Some(cx.device.spi3),
+                tx_cons: DataConsumer {
+                    buf_cons,
+                    sizes_cons,
+                },
+                verif_reporter,
             },
         )
     }
@@ -174,9 +223,9 @@ mod app {
     }
 
     #[task(
-        priority = 3,
+        priority = 4,
         local=[
-            read_buf: [u8;MAX_FRAME_SIZE] = [0; MAX_FRAME_SIZE],
+            read_buf: [u8;MAX_TC_FRAME_SIZE] = [0; MAX_TC_FRAME_SIZE],
             uart_rx,
             cobs_reader_state,
             tc_tx
@@ -295,11 +344,14 @@ mod app {
     #[task(
         priority = 2,
         local=[
-            read_buf: [u8;MAX_FRAME_SIZE] = [0; MAX_FRAME_SIZE],
+            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
+            rom_spi,
+            verif_reporter
         ],
-        shared=[decode_buffer_busy, decode_buf]
+        shared=[decode_buffer_busy, decode_buf, tx_prod]
     )]
     async fn pus_tc_handler(mut cx: pus_tc_handler::Context) {
         loop {
@@ -313,6 +365,29 @@ mod app {
             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
@@ -328,6 +403,12 @@ mod app {
                             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");
@@ -341,6 +422,19 @@ mod app {
                     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();
@@ -380,6 +474,12 @@ mod app {
                             );
                             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");
                         }
                     }
@@ -394,14 +494,34 @@ mod app {
     #[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 {
-            // cx.local.uart_tx.write_all();
+            while cx.local.tx_cons.sizes_cons.occupied_len() > 0 {
-            Mono::delay(500.millis()).await;
+                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;
         }
     }