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
|
||||
222
protocol/priority_scheduler.py
Normal file
222
protocol/priority_scheduler.py
Normal file
@@ -0,0 +1,222 @@
|
||||
"""Deterministic non-preemptive queue schedulers for Lab033."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from enum import Enum
|
||||
from typing import Iterable
|
||||
|
||||
from protocol.link_packet import (
|
||||
HEADER_SIZE,
|
||||
LinkPacket,
|
||||
TrafficClass,
|
||||
encode_link_packet,
|
||||
)
|
||||
|
||||
|
||||
TIME_EPSILON_SECONDS = 1e-12
|
||||
|
||||
|
||||
class SchedulerMode(str, Enum):
|
||||
FIFO = "fifo"
|
||||
STRICT_PRIORITY = "strict_priority"
|
||||
LATEST_STATE = "latest_state"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class QueuedPacket:
|
||||
"""A packet plus a stable global arrival-order tie breaker."""
|
||||
|
||||
packet: LinkPacket
|
||||
arrival_order: int
|
||||
|
||||
@property
|
||||
def generation_time_seconds(self) -> float:
|
||||
return self.packet.generation_time_us / 1_000_000.0
|
||||
|
||||
@property
|
||||
def wire_size_bytes(self) -> int:
|
||||
return HEADER_SIZE + len(self.packet.payload)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Replacement:
|
||||
removed: QueuedPacket
|
||||
replacement: QueuedPacket
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ScheduledPacket:
|
||||
queued: QueuedPacket
|
||||
start_seconds: float
|
||||
end_seconds: float
|
||||
blocked_by: QueuedPacket | None
|
||||
blocking_delay_seconds: float
|
||||
wire_packet: bytes
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class QueueSample:
|
||||
time_seconds: float
|
||||
packets: int
|
||||
bytes: int
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ScheduleResult:
|
||||
mode: SchedulerMode
|
||||
channel_bitrate_bps: float
|
||||
transmitted: tuple[ScheduledPacket, ...]
|
||||
replacements: tuple[Replacement, ...]
|
||||
queue_samples: tuple[QueueSample, ...]
|
||||
|
||||
|
||||
def transmission_duration_seconds(
|
||||
packet_size_bytes: int,
|
||||
channel_bitrate_bps: float,
|
||||
) -> float:
|
||||
"""Return exact serialization duration for a complete packet."""
|
||||
|
||||
if packet_size_bytes < 0:
|
||||
raise ValueError("packet size must not be negative")
|
||||
if channel_bitrate_bps <= 0.0:
|
||||
raise ValueError("channel bitrate must be positive")
|
||||
return packet_size_bytes * 8.0 / channel_bitrate_bps
|
||||
|
||||
|
||||
def _selection_key(item: QueuedPacket, mode: SchedulerMode) -> tuple[int, int]:
|
||||
if mode is SchedulerMode.FIFO:
|
||||
return (0, item.arrival_order)
|
||||
return (int(item.packet.traffic_class), item.arrival_order)
|
||||
|
||||
|
||||
def _enqueue(
|
||||
ready: list[QueuedPacket],
|
||||
item: QueuedPacket,
|
||||
mode: SchedulerMode,
|
||||
replacements: list[Replacement],
|
||||
) -> None:
|
||||
if (
|
||||
mode is SchedulerMode.LATEST_STATE
|
||||
and item.packet.traffic_class
|
||||
in (TrafficClass.CONTROL, TrafficClass.TELEMETRY)
|
||||
):
|
||||
retained = []
|
||||
for old in ready:
|
||||
if (
|
||||
old.packet.traffic_class == item.packet.traffic_class
|
||||
and old.packet.stream_id == item.packet.stream_id
|
||||
):
|
||||
replacements.append(Replacement(old, item))
|
||||
else:
|
||||
retained.append(old)
|
||||
ready[:] = retained
|
||||
ready.append(item)
|
||||
|
||||
|
||||
def schedule_packets(
|
||||
packets: Iterable[QueuedPacket],
|
||||
mode: SchedulerMode,
|
||||
channel_bitrate_bps: float,
|
||||
) -> ScheduleResult:
|
||||
"""Serialize all packets through one non-preemptive shared resource.
|
||||
|
||||
The caller supplies a unique ``arrival_order``. Packets are admitted by
|
||||
``(generation_time, arrival_order)``. FIFO selects the smallest arrival
|
||||
order; priority modes select the smallest ``TrafficClass`` and preserve
|
||||
arrival order within a class. Latest-state replacement only touches items
|
||||
still waiting in ``ready`` and therefore can never remove an active packet.
|
||||
"""
|
||||
|
||||
try:
|
||||
mode = SchedulerMode(mode)
|
||||
except ValueError as error:
|
||||
raise ValueError("unsupported scheduler mode") from error
|
||||
if channel_bitrate_bps <= 0.0:
|
||||
raise ValueError("channel bitrate must be positive")
|
||||
arrivals = sorted(
|
||||
tuple(packets),
|
||||
key=lambda item: (
|
||||
item.generation_time_seconds,
|
||||
item.arrival_order,
|
||||
),
|
||||
)
|
||||
if len({item.arrival_order for item in arrivals}) != len(arrivals):
|
||||
raise ValueError("arrival_order values must be unique")
|
||||
|
||||
ready: list[QueuedPacket] = []
|
||||
transmitted: list[ScheduledPacket] = []
|
||||
replacements: list[Replacement] = []
|
||||
samples: list[QueueSample] = []
|
||||
blocker_by_order: dict[int, tuple[QueuedPacket, float]] = {}
|
||||
cursor = 0.0
|
||||
arrival_index = 0
|
||||
|
||||
def sample(time_seconds: float) -> None:
|
||||
samples.append(
|
||||
QueueSample(
|
||||
time_seconds=time_seconds,
|
||||
packets=len(ready),
|
||||
bytes=sum(item.wire_size_bytes for item in ready),
|
||||
)
|
||||
)
|
||||
|
||||
while arrival_index < len(arrivals) or ready:
|
||||
if not ready:
|
||||
cursor = max(cursor, arrivals[arrival_index].generation_time_seconds)
|
||||
while (
|
||||
arrival_index < len(arrivals)
|
||||
and arrivals[arrival_index].generation_time_seconds
|
||||
<= cursor + TIME_EPSILON_SECONDS
|
||||
):
|
||||
_enqueue(ready, arrivals[arrival_index], mode, replacements)
|
||||
arrival_index += 1
|
||||
sample(cursor)
|
||||
selected_index = min(
|
||||
range(len(ready)),
|
||||
key=lambda index: _selection_key(ready[index], mode),
|
||||
)
|
||||
selected = ready.pop(selected_index)
|
||||
start = max(cursor, selected.generation_time_seconds)
|
||||
wire_packet = encode_link_packet(selected.packet)
|
||||
end = start + transmission_duration_seconds(
|
||||
len(wire_packet), channel_bitrate_bps
|
||||
)
|
||||
|
||||
while (
|
||||
arrival_index < len(arrivals)
|
||||
and arrivals[arrival_index].generation_time_seconds
|
||||
<= end + TIME_EPSILON_SECONDS
|
||||
):
|
||||
arriving = arrivals[arrival_index]
|
||||
if arriving.generation_time_seconds > start + TIME_EPSILON_SECONDS:
|
||||
blocker_by_order[arriving.arrival_order] = (
|
||||
selected,
|
||||
max(0.0, end - arriving.generation_time_seconds),
|
||||
)
|
||||
_enqueue(ready, arriving, mode, replacements)
|
||||
arrival_index += 1
|
||||
|
||||
blocker, blocking_delay = blocker_by_order.get(
|
||||
selected.arrival_order, (None, 0.0)
|
||||
)
|
||||
transmitted.append(
|
||||
ScheduledPacket(
|
||||
queued=selected,
|
||||
start_seconds=start,
|
||||
end_seconds=end,
|
||||
blocked_by=blocker,
|
||||
blocking_delay_seconds=blocking_delay,
|
||||
wire_packet=wire_packet,
|
||||
)
|
||||
)
|
||||
cursor = end
|
||||
sample(cursor)
|
||||
|
||||
return ScheduleResult(
|
||||
mode=mode,
|
||||
channel_bitrate_bps=channel_bitrate_bps,
|
||||
transmitted=tuple(transmitted),
|
||||
replacements=tuple(replacements),
|
||||
queue_samples=tuple(samples),
|
||||
)
|
||||
Reference in New Issue
Block a user