From 7dae30c293e5bb0e75c5dac6aec5212bf6af7ed1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 12 Sep 2024 18:54:37 +0200 Subject: [PATCH] add reply handling --- flashloader/Cargo.toml | 9 +++ flashloader/image-loader.py | 66 +++++++++++++---- flashloader/src/main.rs | 144 +++++++++++++++++++++++++++++++++--- 3 files changed, 194 insertions(+), 25 deletions(-) diff --git a/flashloader/Cargo.toml b/flashloader/Cargo.toml index 5fdc459..15ffcf1 100644 --- a/flashloader/Cargo.toml +++ b/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" diff --git a/flashloader/image-loader.py b/flashloader/image-loader.py index d010e63..56b1fa7 100755 --- a/flashloader/image-loader.py +++ b/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, ) diff --git a/flashloader/src/main.rs b/flashloader/src/main.rs index 0f4f64d..1c51be4 100644 --- a/flashloader/src/main.rs +++ b/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> = + Lazy::new(StaticRb::::default); +static mut SIZES_RB_TX: Lazy> = + Lazy::new(StaticRb::::default); + +pub struct DataProducer { + pub buf_prod: StaticProd<'static, u8, BUF_SIZE>, + pub sizes_prod: StaticProd<'static, usize, SIZES_LEN>, +} + +pub struct DataConsumer { + pub buf_cons: CachingCons<&'static StaticRb>, + pub sizes_cons: CachingCons<&'static StaticRb>, +} + static CLOCKS: OnceCell = 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, + tx_cons: DataConsumer, + 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, } 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(); - Mono::delay(500.millis()).await; + 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; } }