Lab033: add priority link scheduler
This commit is contained in:
LittleSam129
195
protocol/link_packet.py
Normal file
195
protocol/link_packet.py
Normal file
@@ -0,0 +1,195 @@
|
||||
"""Common Lab033 link packet shared by commands, telemetry, and video.
|
||||
|
||||
The fixed 32-byte header uses network byte order and no implicit padding::
|
||||
|
||||
!4sBBBBHIQHII
|
||||
|
||||
Offset Size Field
|
||||
0 4 magic (b"SLP1")
|
||||
4 1 version
|
||||
5 1 traffic_class
|
||||
6 1 direction
|
||||
7 1 flags
|
||||
8 2 stream_id
|
||||
10 4 sequence_number
|
||||
14 8 generation_time_us
|
||||
22 2 deadline_ms
|
||||
24 4 payload_length
|
||||
28 4 packet_crc32
|
||||
|
||||
``packet_crc32`` is IEEE CRC32 over the complete header with that field set
|
||||
to zero, followed by the complete payload.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, replace
|
||||
from enum import IntEnum
|
||||
import struct
|
||||
import zlib
|
||||
|
||||
|
||||
MAGIC = b"SLP1"
|
||||
VERSION = 1
|
||||
HEADER_FORMAT = "!4sBBBBHIQHII"
|
||||
HEADER_SIZE = struct.calcsize(HEADER_FORMAT)
|
||||
SUPPORTED_FLAGS_MASK = 0
|
||||
|
||||
|
||||
class TrafficClass(IntEnum):
|
||||
"""Traffic classes in descending scheduling priority."""
|
||||
|
||||
EMERGENCY = 1
|
||||
CONTROL = 2
|
||||
TELEMETRY = 3
|
||||
VIDEO = 4
|
||||
|
||||
|
||||
class Direction(IntEnum):
|
||||
"""Logical direction of a packet on the shared modelled resource."""
|
||||
|
||||
GROUND_TO_ROVER = 1
|
||||
ROVER_TO_GROUND = 2
|
||||
|
||||
|
||||
class LinkPacketError(ValueError):
|
||||
"""Base class for malformed Lab033 link packets."""
|
||||
|
||||
|
||||
class LinkPacketCRCError(LinkPacketError):
|
||||
"""The Lab033 packet header or payload failed CRC32 validation."""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class LinkPacket:
|
||||
"""Decoded common packet, including its CRC-validated payload."""
|
||||
|
||||
traffic_class: TrafficClass
|
||||
direction: Direction
|
||||
stream_id: int
|
||||
sequence_number: int
|
||||
generation_time_us: int
|
||||
deadline_ms: int
|
||||
payload: bytes
|
||||
flags: int = 0
|
||||
version: int = VERSION
|
||||
packet_crc32: int = 0
|
||||
|
||||
|
||||
def crc32(data: bytes) -> int:
|
||||
"""Return unsigned IEEE CRC32."""
|
||||
|
||||
return zlib.crc32(data) & 0xFFFFFFFF
|
||||
|
||||
|
||||
def _validated(packet: LinkPacket) -> LinkPacket:
|
||||
if not isinstance(packet, LinkPacket):
|
||||
raise TypeError("packet must be LinkPacket")
|
||||
try:
|
||||
traffic_class = TrafficClass(packet.traffic_class)
|
||||
direction = Direction(packet.direction)
|
||||
except ValueError as error:
|
||||
raise LinkPacketError("unsupported enumeration value") from error
|
||||
if packet.version != VERSION:
|
||||
raise LinkPacketError("unsupported link packet version")
|
||||
if packet.flags & ~SUPPORTED_FLAGS_MASK:
|
||||
raise LinkPacketError("unsupported link packet flags")
|
||||
if not 0 <= packet.stream_id <= 0xFFFF:
|
||||
raise LinkPacketError("stream_id is outside uint16")
|
||||
if not 0 <= packet.sequence_number <= 0xFFFFFFFF:
|
||||
raise LinkPacketError("sequence_number is outside uint32")
|
||||
if not 0 <= packet.generation_time_us <= 0xFFFFFFFFFFFFFFFF:
|
||||
raise LinkPacketError("generation_time_us is outside uint64")
|
||||
if not 0 <= packet.deadline_ms <= 0xFFFF:
|
||||
raise LinkPacketError("deadline_ms is outside uint16")
|
||||
if not isinstance(packet.payload, (bytes, bytearray)):
|
||||
raise TypeError("payload must be bytes or bytearray")
|
||||
payload = bytes(packet.payload)
|
||||
if len(payload) > 0xFFFFFFFF:
|
||||
raise LinkPacketError("payload is outside uint32 length")
|
||||
return replace(
|
||||
packet,
|
||||
traffic_class=traffic_class,
|
||||
direction=direction,
|
||||
payload=payload,
|
||||
)
|
||||
|
||||
|
||||
def _pack_header(packet: LinkPacket, crc_value: int) -> bytes:
|
||||
return struct.pack(
|
||||
HEADER_FORMAT,
|
||||
MAGIC,
|
||||
packet.version,
|
||||
int(packet.traffic_class),
|
||||
int(packet.direction),
|
||||
packet.flags,
|
||||
packet.stream_id,
|
||||
packet.sequence_number,
|
||||
packet.generation_time_us,
|
||||
packet.deadline_ms,
|
||||
len(packet.payload),
|
||||
crc_value,
|
||||
)
|
||||
|
||||
|
||||
def encode_link_packet(packet: LinkPacket) -> bytes:
|
||||
"""Serialize a common packet and calculate its CRC32."""
|
||||
|
||||
packet = _validated(packet)
|
||||
header_without_crc = _pack_header(packet, 0)
|
||||
packet_crc32 = crc32(header_without_crc + packet.payload)
|
||||
return _pack_header(packet, packet_crc32) + packet.payload
|
||||
|
||||
|
||||
def decode_link_packet(wire_packet: bytes) -> LinkPacket:
|
||||
"""Deserialize a common packet and validate its format and CRC32."""
|
||||
|
||||
if not isinstance(wire_packet, (bytes, bytearray)):
|
||||
raise TypeError("wire_packet must be bytes or bytearray")
|
||||
wire_packet = bytes(wire_packet)
|
||||
if len(wire_packet) < HEADER_SIZE:
|
||||
raise LinkPacketError("link packet is shorter than its header")
|
||||
(
|
||||
magic,
|
||||
version,
|
||||
traffic_class,
|
||||
direction,
|
||||
flags,
|
||||
stream_id,
|
||||
sequence_number,
|
||||
generation_time_us,
|
||||
deadline_ms,
|
||||
payload_length,
|
||||
received_crc32,
|
||||
) = struct.unpack(HEADER_FORMAT, wire_packet[:HEADER_SIZE])
|
||||
if magic != MAGIC:
|
||||
raise LinkPacketError("invalid link packet magic")
|
||||
if len(wire_packet) != HEADER_SIZE + payload_length:
|
||||
raise LinkPacketError("payload_length does not match packet length")
|
||||
try:
|
||||
packet = LinkPacket(
|
||||
traffic_class=TrafficClass(traffic_class),
|
||||
direction=Direction(direction),
|
||||
stream_id=stream_id,
|
||||
sequence_number=sequence_number,
|
||||
generation_time_us=generation_time_us,
|
||||
deadline_ms=deadline_ms,
|
||||
payload=wire_packet[HEADER_SIZE:],
|
||||
flags=flags,
|
||||
version=version,
|
||||
packet_crc32=received_crc32,
|
||||
)
|
||||
except ValueError as error:
|
||||
raise LinkPacketError("unsupported enumeration value") from error
|
||||
packet = _validated(packet)
|
||||
calculated_crc32 = crc32(_pack_header(packet, 0) + packet.payload)
|
||||
if calculated_crc32 != received_crc32:
|
||||
raise LinkPacketCRCError(
|
||||
"link packet CRC mismatch: "
|
||||
f"received 0x{received_crc32:08X}, "
|
||||
f"calculated 0x{calculated_crc32:08X}"
|
||||
)
|
||||
return packet
|
||||
|
||||
|
||||
assert HEADER_SIZE == 32
|
||||
Reference in New Issue
Block a user