425 lines
16 KiB
Python
425 lines
16 KiB
Python
#!/usr/bin/env python3
|
|
import argparse
|
|
import os
|
|
import select
|
|
import socket
|
|
import struct
|
|
import subprocess
|
|
import time
|
|
from pathlib import Path
|
|
|
|
try:
|
|
import numpy as np
|
|
except ImportError:
|
|
np = None
|
|
|
|
ETH_TYPE = 0x88B6
|
|
FPGA_MAC_DEFAULT = "02:00:00:00:10:89"
|
|
DEFAULT_INPUT = Path("gps_rf_input.bin")
|
|
DEFAULT_MTU = 9000
|
|
DEFAULT_CHUNK_SAMPLES = 17800
|
|
DEFAULT_PRELOAD_SAMPLES = 131072
|
|
SAMPLE_FORMAT_CHOICES = [
|
|
"real4-packed",
|
|
"real8-to-4",
|
|
"raw-low4",
|
|
"raw-high4",
|
|
"packed-high-first",
|
|
"packed-low-first",
|
|
]
|
|
|
|
MAGIC = b"GRF0"
|
|
CMD_RESET = 0x01
|
|
CMD_START = 0x02
|
|
CMD_STOP = 0x03
|
|
CMD_DATA = 0x04
|
|
CMD_PING = 0x05
|
|
RSP_ACK = 0x80
|
|
|
|
ST_OK = 0x00
|
|
ST_NO_SPACE = 0x01
|
|
|
|
HEADER = struct.Struct("!4sBBHQQHHI")
|
|
|
|
|
|
def parse_mac(text: str) -> bytes:
|
|
parts = text.split(":")
|
|
if len(parts) != 6:
|
|
raise ValueError(f"bad MAC address: {text}")
|
|
return bytes(int(part, 16) for part in parts)
|
|
|
|
|
|
def iface_mac(iface: str) -> bytes:
|
|
return parse_mac(Path(f"/sys/class/net/{iface}/address").read_text(encoding="ascii").strip())
|
|
|
|
|
|
def iface_mtu(iface: str) -> int:
|
|
return int(Path(f"/sys/class/net/{iface}/mtu").read_text(encoding="ascii").strip())
|
|
|
|
|
|
def set_iface_mtu(iface: str, mtu: int) -> None:
|
|
current = iface_mtu(iface)
|
|
if current == mtu:
|
|
return
|
|
subprocess.run(["ip", "link", "set", "dev", iface, "mtu", str(mtu)], check=True)
|
|
print(f"MTU set iface={iface} mtu={mtu}")
|
|
|
|
|
|
def default_iface() -> str | None:
|
|
candidates = []
|
|
for item in Path("/sys/class/net").iterdir():
|
|
name = item.name
|
|
if name == "lo":
|
|
continue
|
|
try:
|
|
operstate = (item / "operstate").read_text(encoding="ascii").strip()
|
|
carrier = (item / "carrier").read_text(encoding="ascii").strip()
|
|
except OSError:
|
|
continue
|
|
if operstate == "up" and carrier == "1":
|
|
candidates.append(name)
|
|
for name in candidates:
|
|
if name.startswith("enx") or name.startswith("eth") or name.startswith("eno"):
|
|
return name
|
|
return candidates[0] if candidates else None
|
|
|
|
|
|
def make_real8_to_4_lut(shift: int) -> bytes:
|
|
values = []
|
|
for value in range(256):
|
|
signed = value if value < 128 else value - 256
|
|
quantized = signed >> shift
|
|
if quantized < -8:
|
|
quantized = -8
|
|
elif quantized > 7:
|
|
quantized = 7
|
|
values.append(quantized & 0xF)
|
|
return bytes(values)
|
|
|
|
|
|
def make_low4_lut() -> bytes:
|
|
return bytes(value & 0xF for value in range(256))
|
|
|
|
|
|
def make_high4_lut() -> bytes:
|
|
return bytes((value >> 4) & 0xF for value in range(256))
|
|
|
|
|
|
def pack_high_first(samples4: bytes) -> bytes:
|
|
out = bytearray((len(samples4) + 1) // 2)
|
|
j = 0
|
|
for i in range(0, len(samples4), 2):
|
|
hi = samples4[i] & 0xF
|
|
lo = samples4[i + 1] & 0xF if i + 1 < len(samples4) else 0
|
|
out[j] = (hi << 4) | lo
|
|
j += 1
|
|
return bytes(out)
|
|
|
|
|
|
def swap_nibbles(data: bytes) -> bytes:
|
|
return bytes(((value & 0xF) << 4) | (value >> 4) for value in data)
|
|
|
|
|
|
def normalize_sample_format(sample_format: str) -> str:
|
|
if sample_format == "real4-packed":
|
|
return "packed-high-first"
|
|
return sample_format
|
|
|
|
|
|
def encode_samples(data: bytes, sample_format: str, real8_shift: int) -> tuple[bytes, int]:
|
|
sample_format = normalize_sample_format(sample_format)
|
|
if np is not None and sample_format in ("real8-to-4", "raw-low4", "raw-high4"):
|
|
arr = np.frombuffer(data, dtype=np.uint8)
|
|
if sample_format == "real8-to-4":
|
|
signed = arr.view(np.int8).astype(np.int16)
|
|
q = np.clip(signed >> real8_shift, -8, 7).astype(np.uint8) & 0x0F
|
|
elif sample_format == "raw-low4":
|
|
q = arr & 0x0F
|
|
else:
|
|
q = arr >> 4
|
|
if q.size & 1:
|
|
q = np.concatenate((q, np.zeros(1, dtype=np.uint8)))
|
|
packed = ((q[0::2] << 4) | q[1::2]).astype(np.uint8)
|
|
return packed.tobytes(), len(data)
|
|
|
|
if sample_format == "real8-to-4":
|
|
samples4 = data.translate(make_real8_to_4_lut(real8_shift))
|
|
return pack_high_first(samples4), len(samples4)
|
|
if sample_format == "raw-low4":
|
|
samples4 = data.translate(make_low4_lut())
|
|
return pack_high_first(samples4), len(samples4)
|
|
if sample_format == "raw-high4":
|
|
samples4 = data.translate(make_high4_lut())
|
|
return pack_high_first(samples4), len(samples4)
|
|
if sample_format == "packed-high-first":
|
|
return data, len(data) * 2
|
|
if sample_format == "packed-low-first":
|
|
return swap_nibbles(data), len(data) * 2
|
|
raise ValueError(f"unsupported sample format: {sample_format}")
|
|
|
|
|
|
def build_payload(cmd: int, seq: int, offset: int = 0, total: int = 0,
|
|
sample_count: int = 0, payload: bytes = b"", flags: int = 0) -> bytes:
|
|
header = HEADER.pack(
|
|
MAGIC,
|
|
cmd,
|
|
flags,
|
|
seq & 0xFFFF,
|
|
offset & 0xFFFFFFFFFFFFFFFF,
|
|
total & 0xFFFFFFFFFFFFFFFF,
|
|
sample_count & 0xFFFF,
|
|
len(payload) & 0xFFFF,
|
|
0,
|
|
)
|
|
return header + payload
|
|
|
|
|
|
def parse_ack(payload: bytes):
|
|
if len(payload) < 25:
|
|
return None
|
|
magic = payload[0:4]
|
|
cmd = payload[4]
|
|
status = payload[5]
|
|
seq = struct.unpack("!H", payload[6:8])[0]
|
|
if magic != MAGIC or cmd != RSP_ACK:
|
|
return None
|
|
offset = struct.unpack("!Q", payload[8:16])[0]
|
|
space = struct.unpack("!I", payload[16:20])[0]
|
|
level = struct.unpack("!I", payload[20:24])[0]
|
|
flags = payload[24]
|
|
return status, seq, offset, space, level, flags
|
|
|
|
|
|
def send_frame(sock: socket.socket, iface: str, dst: bytes, src: bytes, payload: bytes) -> None:
|
|
frame = dst + src + struct.pack("!H", ETH_TYPE) + payload
|
|
if len(frame) < 60:
|
|
frame += bytes(60 - len(frame))
|
|
sock.send(frame)
|
|
|
|
|
|
def recv_ack(sock: socket.socket, fpga_mac: bytes, host_mac: bytes, seq: int, timeout: float):
|
|
deadline = time.monotonic() + timeout
|
|
while time.monotonic() < deadline:
|
|
remaining = max(0.0, deadline - time.monotonic())
|
|
readable, _, _ = select.select([sock], [], [], remaining)
|
|
if not readable:
|
|
break
|
|
frame = sock.recv(65535)
|
|
if len(frame) < 14:
|
|
continue
|
|
dst = frame[0:6]
|
|
src = frame[6:12]
|
|
eth_type = struct.unpack("!H", frame[12:14])[0]
|
|
if eth_type != ETH_TYPE or src != fpga_mac or dst not in (host_mac, b"\xff" * 6):
|
|
continue
|
|
ack = parse_ack(frame[14:])
|
|
if ack is None:
|
|
continue
|
|
status, ack_seq, offset, space, level, flags = ack
|
|
if ack_seq == (seq & 0xFFFF):
|
|
return status, offset, space, level, flags
|
|
raise TimeoutError(f"timeout waiting ACK seq={seq & 0xFFFF}")
|
|
|
|
|
|
def transact(sock: socket.socket, iface: str, fpga_mac: bytes, host_mac: bytes,
|
|
seq: int, payload: bytes, timeout: float):
|
|
send_frame(sock, iface, fpga_mac, host_mac, payload)
|
|
return recv_ack(sock, fpga_mac, host_mac, seq, timeout)
|
|
|
|
|
|
def main() -> None:
|
|
parser = argparse.ArgumentParser(description="Send packed GPS RF samples to the VCU118 over raw Ethernet.")
|
|
parser.add_argument("--iface", default=default_iface(), help="Ethernet interface connected to the FPGA")
|
|
parser.add_argument("--input", type=Path, default=DEFAULT_INPUT, help="RF binary to send")
|
|
parser.add_argument("--fpga-mac", default=FPGA_MAC_DEFAULT)
|
|
parser.add_argument("--sample-format", default="real4-packed", choices=SAMPLE_FORMAT_CHOICES)
|
|
parser.add_argument("--real8-shift", type=int, default=4)
|
|
parser.add_argument("--chunk-samples", type=int, default=DEFAULT_CHUNK_SAMPLES)
|
|
parser.add_argument("--timeout", type=float, default=2.0)
|
|
parser.add_argument("--retry-sleep", type=float, default=0.001)
|
|
parser.add_argument("--mtu", type=int, default=DEFAULT_MTU,
|
|
help="Set interface MTU before streaming; 0 leaves the current MTU unchanged")
|
|
parser.add_argument("--preload-samples", type=int, default=DEFAULT_PRELOAD_SAMPLES,
|
|
help="Send this many samples before START; 0 starts before DATA")
|
|
parser.add_argument("--loop", action="store_true")
|
|
parser.add_argument("--no-start", action="store_true")
|
|
parser.add_argument("--no-reset", action="store_true")
|
|
parser.add_argument("--max-samples", type=int, default=0,
|
|
help="Stop after this many RF samples; 0 sends the whole file")
|
|
parser.add_argument("--max-bytes", type=int, default=0,
|
|
help="Read at most this many input bytes; 0 reads the whole file")
|
|
parser.add_argument("--progress-interval", type=float, default=1.0)
|
|
args = parser.parse_args()
|
|
|
|
if os.geteuid() != 0:
|
|
raise SystemExit("Raw Ethernet needs root or CAP_NET_RAW. Run with sudo.")
|
|
if not args.iface:
|
|
raise SystemExit("No active Ethernet interface found. Pass --iface explicitly.")
|
|
if not args.input.exists():
|
|
raise SystemExit(f"Input file not found: {args.input}")
|
|
if args.chunk_samples <= 0 or args.chunk_samples > 65534:
|
|
raise SystemExit("--chunk-samples must be 1..65534")
|
|
if args.mtu < 0:
|
|
raise SystemExit("--mtu must be >= 0")
|
|
if args.preload_samples < 0:
|
|
raise SystemExit("--preload-samples must be >= 0")
|
|
if args.max_samples < 0:
|
|
raise SystemExit("--max-samples must be >= 0")
|
|
if args.max_bytes < 0:
|
|
raise SystemExit("--max-bytes must be >= 0")
|
|
|
|
if args.mtu:
|
|
set_iface_mtu(args.iface, args.mtu)
|
|
current_mtu = iface_mtu(args.iface)
|
|
wire_payload_capacity = current_mtu - HEADER.size
|
|
if wire_payload_capacity <= 0:
|
|
raise SystemExit(f"Interface MTU too small for protocol header: mtu={current_mtu}")
|
|
|
|
host_mac = iface_mac(args.iface)
|
|
fpga_mac = parse_mac(args.fpga_mac)
|
|
sock = socket.socket(socket.AF_PACKET, socket.SOCK_RAW, socket.htons(ETH_TYPE))
|
|
sock.bind((args.iface, 0))
|
|
sample_format = normalize_sample_format(args.sample_format)
|
|
|
|
source_bytes_total = args.input.stat().st_size
|
|
if args.max_bytes:
|
|
source_bytes_total = min(source_bytes_total, args.max_bytes)
|
|
|
|
total_samples = source_bytes_total
|
|
if sample_format.startswith("packed-"):
|
|
total_samples *= 2
|
|
if args.max_samples:
|
|
total_samples = min(total_samples, args.max_samples)
|
|
|
|
seq = int(time.time()) & 0xFFFF
|
|
|
|
def send_start(seq_value: int):
|
|
payload = build_payload(CMD_START, seq_value, total=total_samples)
|
|
return transact(sock, args.iface, fpga_mac, host_mac, seq_value, payload, args.timeout)
|
|
|
|
if not args.no_reset:
|
|
payload = build_payload(CMD_RESET, seq, total=total_samples)
|
|
status, _, space, level, flags = transact(sock, args.iface, fpga_mac, host_mac, seq, payload, args.timeout)
|
|
if status != ST_OK:
|
|
raise SystemExit(f"RESET failed status=0x{status:02x}")
|
|
print(f"RESET ok space={space} level={level} flags=0x{flags:02x}")
|
|
seq += 1
|
|
|
|
started = args.no_start
|
|
if not args.no_start and args.preload_samples == 0:
|
|
status, _, space, level, flags = send_start(seq)
|
|
if status != ST_OK:
|
|
raise SystemExit(f"START failed status=0x{status:02x}")
|
|
print(f"START armed space={space} level={level} flags=0x{flags:02x}")
|
|
seq += 1
|
|
started = True
|
|
|
|
source_chunk = args.chunk_samples
|
|
if sample_format.startswith("packed-"):
|
|
source_chunk = max(1, args.chunk_samples // 2)
|
|
if source_chunk > wire_payload_capacity:
|
|
source_chunk = wire_payload_capacity
|
|
print(f"chunk clipped to {source_chunk * 2} samples for mtu={current_mtu}")
|
|
else:
|
|
max_source_chunk = wire_payload_capacity * 2
|
|
if source_chunk > max_source_chunk:
|
|
source_chunk = max_source_chunk
|
|
print(f"chunk clipped to {source_chunk} samples for mtu={current_mtu}")
|
|
|
|
started_at = time.monotonic()
|
|
last_progress = started_at
|
|
sent_samples_total = 0
|
|
no_space_retries = 0
|
|
min_fifo_level = None
|
|
|
|
while True:
|
|
offset_samples = 0
|
|
with args.input.open("rb") as stream:
|
|
while True:
|
|
if not started and offset_samples >= min(args.preload_samples, total_samples):
|
|
status, _, space, level, flags = send_start(seq)
|
|
if status != ST_OK:
|
|
raise SystemExit(f"START failed status=0x{status:02x}")
|
|
print(f"START armed after preload={offset_samples} space={space} level={level} flags=0x{flags:02x}")
|
|
min_fifo_level = level if min_fifo_level is None else min(min_fifo_level, level)
|
|
seq += 1
|
|
started = True
|
|
|
|
remaining_samples = total_samples - offset_samples
|
|
if remaining_samples <= 0:
|
|
break
|
|
if sample_format.startswith("packed-"):
|
|
read_size = min(source_chunk, remaining_samples // 2)
|
|
if read_size <= 0:
|
|
break
|
|
else:
|
|
read_size = min(source_chunk, remaining_samples)
|
|
raw = stream.read(read_size)
|
|
if not raw:
|
|
break
|
|
packed, sample_count = encode_samples(raw, sample_format, args.real8_shift)
|
|
payload = build_payload(
|
|
CMD_DATA,
|
|
seq,
|
|
offset=offset_samples,
|
|
total=total_samples,
|
|
sample_count=sample_count,
|
|
payload=packed,
|
|
)
|
|
|
|
while True:
|
|
status, ack_offset, space, level, flags = transact(
|
|
sock, args.iface, fpga_mac, host_mac, seq, payload, args.timeout
|
|
)
|
|
if status == ST_OK:
|
|
expected = offset_samples + sample_count
|
|
if ack_offset != expected:
|
|
raise SystemExit(f"ACK offset mismatch got={ack_offset} expected={expected}")
|
|
if started:
|
|
min_fifo_level = level if min_fifo_level is None else min(min_fifo_level, level)
|
|
break
|
|
if status == ST_NO_SPACE:
|
|
no_space_retries += 1
|
|
time.sleep(args.retry_sleep)
|
|
continue
|
|
raise SystemExit(f"DATA failed status=0x{status:02x} offset={offset_samples}")
|
|
|
|
offset_samples += sample_count
|
|
sent_samples_total += sample_count
|
|
seq = (seq + 1) & 0xFFFF
|
|
|
|
now = time.monotonic()
|
|
if now - last_progress >= args.progress_interval:
|
|
elapsed = now - started_at
|
|
rate = sent_samples_total / elapsed if elapsed > 0 else 0.0
|
|
rf_mbps = rate * 4.0 / 1e6
|
|
rf_mbytes = rate / 2.0 / 1e6
|
|
print(
|
|
f"sent={offset_samples}/{total_samples} samples "
|
|
f"rate={rate/1e6:.3f} Msps rf={rf_mbps:.3f} Mbps {rf_mbytes:.3f} MB/s "
|
|
f"fifo_level={level} fifo_space={space} "
|
|
f"no_space={no_space_retries} flags=0x{flags:02x}"
|
|
)
|
|
last_progress = now
|
|
|
|
if not started:
|
|
status, _, space, level, flags = send_start(seq)
|
|
if status != ST_OK:
|
|
raise SystemExit(f"START failed status=0x{status:02x}")
|
|
print(f"START armed after preload={offset_samples} space={space} level={level} flags=0x{flags:02x}")
|
|
min_fifo_level = level if min_fifo_level is None else min(min_fifo_level, level)
|
|
seq += 1
|
|
started = True
|
|
|
|
if not args.loop:
|
|
break
|
|
print("loop restart")
|
|
|
|
if min_fifo_level is None:
|
|
min_fifo_level = 0
|
|
print(f"done sent_samples={sent_samples_total} min_fifo_level={min_fifo_level} no_space={no_space_retries}")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|