Add FOP-1 routing and management

oresat_c3/protocols/uslp.py

@@ -65,7 +65,8 @@ def unpack_frame(raw: bytes) -> TransferFrame:
 
     vcid = ((raw[2] & 0b111) << 3) | ((raw[3] >> 5) & 0b111)
 
-    sdls_header_len = get_sdls_header_len(vcid)
+    is_command = bool(raw[6] & 0x40)
+    sdls_header_len = 0 if is_command else get_sdls_header_len(vcid)
 
     frame_props = VarFrameProperties(
         has_insert_zone=sdls_header_len != 0,
@@ -92,6 +93,7 @@ def make_frame(
     control_word: Optional[bytes] = None,
     sequence_number: int = 0,
     bypass: bool = False,
+    command: bool = False,
 ) -> TransferFrame:
     """Create and pack a USLP
 
@@ -113,6 +115,8 @@ def make_frame(
         The anti-replay sequence number for SDLS.
     bypass
         Specify if this frame is bypass (Type-BD) frame.
+    command
+        Specify if this frame is for protocol command.
 
     Returns
     -------
@@ -127,29 +131,29 @@ def make_frame(
         tfdz=payload,
     )
 
-    # USLP transfer frame total length - 1
-    frame_len = len(payload) + PRIMARY_HEADER_LEN + DFH_LEN + FECF_LEN - 1
+    vcf_count_len = 1 if vcf_count is not None else 0
 
     has_clcw = control_word is not None
-    if has_clcw:
-        frame_len += len(control_word)
 
     # USLP transfer frame total length - 1
-    frame_len = len(payload) + PRIMARY_HEADER_LEN + DFH_LEN + FECF_LEN - 1
+    frame_len = len(payload) + PRIMARY_HEADER_LEN + vcf_count_len + DFH_LEN + FECF_LEN - 1
     if has_clcw:
         frame_len += len(control_word)
-    frame_len += get_sdls_len(vcid)
+    if not command:
+        frame_len += get_sdls_len(vcid)
 
     frame_header = PrimaryHeader(
         scid=SPACECRAFT_ID,
         map_id=0,
         vcid=vcid,
         src_dest=src_dest,
         frame_len=frame_len,
-        vcf_count_len=bool(vcf_count),
+        vcf_count_len=vcf_count_len,
         vcf_count=vcf_count,
         op_ctrl_flag=has_clcw,
-        prot_ctrl_cmd_flag=ProtocolCommandFlag.USER_DATA,
+        prot_ctrl_cmd_flag=(
+            ProtocolCommandFlag.PROTOCOL_INFORMATION if command else ProtocolCommandFlag.USER_DATA
+        ),
         bypass_seq_ctrl_flag=(
             BypassSequenceControlFlag.EXPEDITED_QOS
             if bypass
@@ -159,6 +163,7 @@ def make_frame(
 
     frame = TransferFrame(header=frame_header, tfdf=tfdf, op_ctrl_field=control_word)
 
-    apply_sdls(frame, sequence_number, hmac_key)
+    if not command:
+        apply_sdls(frame, sequence_number, hmac_key)
 
     return frame

oresat_c3/services/channel_router.py

@@ -35,6 +35,9 @@ def __init__(self, radios_service: RadiosService, cop_service: CopManagerService
         self._downlink_routes: dict[EdlVcid, SimpleQueue[bytes]] = {}
         self._last_clcw_time = 0.0
 
+    def on_start(self) -> None:
+        self._send_clcws()
+
     def on_loop(self) -> None:
         for dl in self._downlink_routes.values():
             while True:
@@ -47,16 +50,7 @@ def on_loop(self) -> None:
         if self.node.od["status"].value == C3State.EDL:
             now = monotonic()
             if now - self._last_clcw_time >= self._CLCW_INTERVAL:
-                for clcw in self._get_all_clcw():
-                    frame = make_frame(
-                        payload=bytes(1),
-                        vcid=EdlVcid.IDLE,
-                        src_dest=SourceOrDestField.SOURCE,
-                        control_word=clcw.pack(),
-                    )
-                    self._radios_service.send_edl_response(
-                        frame.pack(frame_type=FrameType.VARIABLE)
-                    )
+                self._send_clcws()
                 self._last_clcw_time = now
 
         try:
@@ -66,8 +60,10 @@ def on_loop(self) -> None:
 
         try:
             frame = unpack_frame(message)
+            if frame.op_ctrl_field is not None:
+                self._cop_service.dispatch_clcw(ControlWord.unpack(frame.op_ctrl_field))
             vcid = frame.header.vcid
-            if vcid in self._uplink_routes:
+            if vcid != EdlVcid.IDLE and vcid in self._uplink_routes:
                 self._uplink_routes[vcid].put_nowait(frame)
             else:
                 logger.error(f"No route for VCID {frame.header.vcid}")
@@ -77,6 +73,16 @@ def on_loop(self) -> None:
         except Exception as e:
             logger.exception(f"Failed to unpack frame: {e}")
 
+    def _send_clcws(self) -> None:
+        for clcw in self._get_all_clcw():
+            frame = make_frame(
+                payload=bytes(1),
+                vcid=EdlVcid.IDLE,
+                src_dest=SourceOrDestField.SOURCE,
+                control_word=clcw.pack(),
+            )
+            self._radios_service.send_edl_response(frame.pack(frame_type=FrameType.VARIABLE))
+
     def request_uplink_route(
         self, vcid: EdlVcid, use_cop: bool = False
     ) -> SimpleQueue[TransferFrame]:
@@ -109,13 +115,15 @@ def request_uplink_route(
             self._uplink_routes[vcid] = q
         return q
 
-    def request_downlink_route(self, vcid: EdlVcid) -> SimpleQueue[bytes]:
+    def request_downlink_route(self, vcid: EdlVcid, use_cop: bool = False) -> SimpleQueue[bytes]:
         """Request a downlink Virtual Channel route.
 
         Parameters
         ----------
         vcid
             The VCID used to identify the route.
+        use_cop
+            True enables COP-1 (FOP-1) on this route.
 
         Returns
         -------
@@ -130,11 +138,14 @@ def request_downlink_route(self, vcid: EdlVcid) -> SimpleQueue[bytes]:
 
         if vcid in self._downlink_routes:
             raise KeyError(f"Downlink route for VCID={vcid} already exists")
+        if use_cop:
+            send_queue: SimpleQueue[bytes] = SimpleQueue()
+            q = self._cop_service.create_fop_service(vcid, send_queue)
+            self._downlink_routes[vcid] = send_queue
         else:
-            q: SimpleQueue[bytes] = SimpleQueue()
+            q = SimpleQueue()
             self._downlink_routes[vcid] = q
-            logger.info(f"Created downlink route for VCID {vcid}")
-            return q
+        return q
 
     def _get_all_clcw(self) -> list[ControlWord]:
         clcws = []