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,16 +1,22 @@
 #!/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
@@ -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,
     )