onur/gps
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
38a96dd88f4905ab508b6c1919398881ef093902
gps/targets/fpga/vcu118/flow/ethernet_rf/send_gps_rf.c
T
onur 07ddf298f9 first draft
2026-07-12 11:01:51 +03:00

1094 lines
30 KiB
C
Raw Blame History

#define _GNU_SOURCE
#include <arpa/inet.h>
#include <errno.h>
#include <inttypes.h>
#include <linux/if_packet.h>
#include <linux/if_ether.h>
#include <net/if.h>
#include <signal.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/select.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <time.h>
#include <unistd.h>
#define ETH_RF_TYPE 0x88B6
#define DEFAULT_IFACE "enx14ebb6850432"
#define DEFAULT_FPGA_MAC "02:00:00:00:10:89"
#define DEFAULT_INPUT "gps_rf_input.bin"
#define DEFAULT_MTU 9000
#define DEFAULT_CHUNK_SAMPLES 17800
#define DEFAULT_PRELOAD_SAMPLES 131072
#define DEFAULT_TIMEOUT_SEC 2.0
#define DEFAULT_RETRY_SLEEP_US 1000
#define DEFAULT_PROGRESS_INTERVAL 1.0
#define HEADER_LEN 32
#define ACK_MIN_LEN 25
#define MIN_ETH_FRAME 60
#define MAX_FRAME_SIZE 20000
#define CMD_RESET 0x01
#define CMD_START 0x02
#define CMD_STOP 0x03
#define CMD_DATA 0x04
#define CMD_PING 0x05
#define RSP_ACK 0x80
#define ST_OK 0x00
#define ST_NO_SPACE 0x01
enum sample_format {
FMT_REAL4_PACKED,
FMT_REAL4_SIGNMAG,
FMT_REAL8_TO_4,
FMT_RAW_LOW4,
FMT_RAW_HIGH4,
FMT_PACKED_HIGH_FIRST,
FMT_PACKED_LOW_FIRST,
};
struct options {
const char *iface;
const char *input;
const char *fpga_mac_text;
enum sample_format format;
int real8_shift;
uint32_t chunk_samples;
int mtu;
uint64_t preload_samples;
uint64_t max_samples;
uint64_t max_bytes;
double timeout_sec;
useconds_t retry_sleep_us;
double progress_interval;
bool loop;
bool no_start;
bool no_reset;
bool stop_only;
};
struct ack {
uint8_t status;
uint16_t seq;
uint64_t offset;
uint32_t space;
uint32_t level;
uint8_t flags;
};
static volatile sig_atomic_t stop_requested = 0;
static void request_stop(int signo) {
(void)signo;
stop_requested = 1;
}
static void install_signal_handlers(void) {
struct sigaction sa;
memset(&sa, 0, sizeof(sa));
sa.sa_handler = request_stop;
sigemptyset(&sa.sa_mask);
sigaction(SIGINT, &sa, NULL);
sigaction(SIGTERM, &sa, NULL);
}
static double monotonic_sec(void) {
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
return (double)ts.tv_sec + (double)ts.tv_nsec / 1000000000.0;
}
static uint16_t get_be16(const uint8_t *p) {
return ((uint16_t)p[0] << 8) | (uint16_t)p[1];
}
static uint32_t get_be32(const uint8_t *p) {
return ((uint32_t)p[0] << 24) | ((uint32_t)p[1] << 16) |
((uint32_t)p[2] << 8) | (uint32_t)p[3];
}
static uint64_t get_be64(const uint8_t *p) {
uint64_t v = 0;
for (int i = 0; i < 8; i++) {
v = (v << 8) | p[i];
}
return v;
}
static void put_be16(uint8_t *p, uint16_t v) {
p[0] = (uint8_t)(v >> 8);
p[1] = (uint8_t)v;
}
static void put_be32(uint8_t *p, uint32_t v) {
p[0] = (uint8_t)(v >> 24);
p[1] = (uint8_t)(v >> 16);
p[2] = (uint8_t)(v >> 8);
p[3] = (uint8_t)v;
}
static void put_be64(uint8_t *p, uint64_t v) {
for (int i = 7; i >= 0; i--) {
p[i] = (uint8_t)v;
v >>= 8;
}
}
static void usage(const char *argv0) {
fprintf(stderr,
"usage: %s [options]\n"
"\n"
"Send packed GPS RF samples to the VCU118 over raw Ethernet.\n"
"\n"
"options:\n"
" --iface IFACE\n"
" --input PATH\n"
" --fpga-mac MAC\n"
" --sample-format real4-packed|real4-signmag|real8-to-4|raw-low4|raw-high4|packed-high-first|packed-low-first\n"
" --real8-shift N\n"
" --chunk-samples N\n"
" --mtu N set interface MTU first; 0 keeps current MTU\n"
" --preload-samples N send N samples before START; 0 starts before DATA\n"
" --max-samples N 0 sends the whole file\n"
" --max-bytes N 0 reads the whole file\n"
" --timeout SEC\n"
" --retry-sleep-us N\n"
" --progress-interval SEC\n"
" --loop\n"
" --no-start\n"
" --no-reset\n"
" --stop-only send STOP, RESET, status PING and exit\n"
" -h, --help\n",
argv0);
}
static int parse_u64(const char *text, uint64_t *out) {
char *end = NULL;
errno = 0;
unsigned long long value = strtoull(text, &end, 0);
if (errno || end == text || *end != '\0') {
return -1;
}
*out = (uint64_t)value;
return 0;
}
static int parse_u32(const char *text, uint32_t *out) {
uint64_t value = 0;
if (parse_u64(text, &value) != 0 || value > UINT32_MAX) {
return -1;
}
*out = (uint32_t)value;
return 0;
}
static int parse_double(const char *text, double *out) {
char *end = NULL;
errno = 0;
double value = strtod(text, &end);
if (errno || end == text || *end != '\0' || value < 0.0) {
return -1;
}
*out = value;
return 0;
}
static int parse_sample_format(const char *text, enum sample_format *format) {
if (strcmp(text, "real4-packed") == 0) {
*format = FMT_REAL4_PACKED;
} else if (strcmp(text, "real4-signmag") == 0) {
*format = FMT_REAL4_SIGNMAG;
} else if (strcmp(text, "real8-to-4") == 0) {
*format = FMT_REAL8_TO_4;
} else if (strcmp(text, "raw-low4") == 0) {
*format = FMT_RAW_LOW4;
} else if (strcmp(text, "raw-high4") == 0) {
*format = FMT_RAW_HIGH4;
} else if (strcmp(text, "packed-high-first") == 0) {
*format = FMT_PACKED_HIGH_FIRST;
} else if (strcmp(text, "packed-low-first") == 0) {
*format = FMT_PACKED_LOW_FIRST;
} else {
return -1;
}
return 0;
}
static bool format_is_packed(enum sample_format format) {
return format == FMT_REAL4_PACKED ||
format == FMT_REAL4_SIGNMAG ||
format == FMT_PACKED_HIGH_FIRST ||
format == FMT_PACKED_LOW_FIRST;
}
static uint8_t signmag4_to_twos4(uint8_t value) {
uint8_t magnitude = value & 0x07;
if ((value & 0x08) == 0 || magnitude == 0) {
return magnitude;
}
return (uint8_t)((16u - magnitude) & 0x0F);
}
static int parse_mac(const char *text, uint8_t mac[6]) {
unsigned int b[6];
if (sscanf(text, "%x:%x:%x:%x:%x:%x",
&b[0], &b[1], &b[2], &b[3], &b[4], &b[5]) != 6) {
return -1;
}
for (int i = 0; i < 6; i++) {
if (b[i] > 255) {
return -1;
}
mac[i] = (uint8_t)b[i];
}
return 0;
}
static int get_iface_mac(const char *iface, uint8_t mac[6]) {
int fd = socket(AF_INET, SOCK_DGRAM, 0);
if (fd < 0) {
perror("socket(AF_INET)");
return -1;
}
struct ifreq ifr;
memset(&ifr, 0, sizeof(ifr));
snprintf(ifr.ifr_name, sizeof(ifr.ifr_name), "%s", iface);
if (ioctl(fd, SIOCGIFHWADDR, &ifr) != 0) {
perror("SIOCGIFHWADDR");
close(fd);
return -1;
}
memcpy(mac, ifr.ifr_hwaddr.sa_data, 6);
close(fd);
return 0;
}
static int get_iface_mtu(const char *iface) {
int fd = socket(AF_INET, SOCK_DGRAM, 0);
if (fd < 0) {
perror("socket(AF_INET)");
return -1;
}
struct ifreq ifr;
memset(&ifr, 0, sizeof(ifr));
snprintf(ifr.ifr_name, sizeof(ifr.ifr_name), "%s", iface);
if (ioctl(fd, SIOCGIFMTU, &ifr) != 0) {
perror("SIOCGIFMTU");
close(fd);
return -1;
}
close(fd);
return ifr.ifr_mtu;
}
static int set_iface_mtu(const char *iface, int mtu) {
int current = get_iface_mtu(iface);
if (current < 0) {
return -1;
}
if (current == mtu) {
return 0;
}
int fd = socket(AF_INET, SOCK_DGRAM, 0);
if (fd < 0) {
perror("socket(AF_INET)");
return -1;
}
struct ifreq ifr;
memset(&ifr, 0, sizeof(ifr));
snprintf(ifr.ifr_name, sizeof(ifr.ifr_name), "%s", iface);
ifr.ifr_mtu = mtu;
if (ioctl(fd, SIOCSIFMTU, &ifr) != 0) {
perror("SIOCSIFMTU");
close(fd);
return -1;
}
printf("MTU set iface=%s mtu=%d\n", iface, mtu);
close(fd);
return 0;
}
static size_t encode_samples(enum sample_format format,
const uint8_t *src,
size_t src_len,
int real8_shift,
uint8_t *dst,
uint16_t *sample_count) {
if (format == FMT_REAL4_PACKED || format == FMT_PACKED_HIGH_FIRST) {
memcpy(dst, src, src_len);
*sample_count = (uint16_t)(src_len * 2);
return src_len;
}
if (format == FMT_REAL4_SIGNMAG) {
for (size_t i = 0; i < src_len; i++) {
uint8_t hi = signmag4_to_twos4(src[i] >> 4);
uint8_t lo = signmag4_to_twos4(src[i] & 0x0F);
dst[i] = (uint8_t)((hi << 4) | lo);
}
*sample_count = (uint16_t)(src_len * 2);
return src_len;
}
if (format == FMT_PACKED_LOW_FIRST) {
for (size_t i = 0; i < src_len; i++) {
dst[i] = (uint8_t)((src[i] << 4) | (src[i] >> 4));
}
*sample_count = (uint16_t)(src_len * 2);
return src_len;
}
size_t j = 0;
for (size_t i = 0; i < src_len; i += 2) {
uint8_t hi = 0;
uint8_t lo = 0;
if (format == FMT_REAL8_TO_4) {
int s0 = (int)(int8_t)src[i];
int q0 = s0 >> real8_shift;
if (q0 < -8) {
q0 = -8;
} else if (q0 > 7) {
q0 = 7;
}
hi = (uint8_t)q0 & 0x0F;
} else if (format == FMT_RAW_LOW4) {
hi = src[i] & 0x0F;
} else {
hi = src[i] >> 4;
}
if (i + 1 < src_len) {
if (format == FMT_REAL8_TO_4) {
int s1 = (int)(int8_t)src[i + 1];
int q1 = s1 >> real8_shift;
if (q1 < -8) {
q1 = -8;
} else if (q1 > 7) {
q1 = 7;
}
lo = (uint8_t)q1 & 0x0F;
} else if (format == FMT_RAW_LOW4) {
lo = src[i + 1] & 0x0F;
} else {
lo = src[i + 1] >> 4;
}
}
dst[j++] = (uint8_t)((hi << 4) | lo);
}
*sample_count = (uint16_t)src_len;
return j;
}
static size_t build_frame(uint8_t *frame,
const uint8_t dst_mac[6],
const uint8_t src_mac[6],
uint8_t cmd,
uint16_t seq,
uint64_t offset,
uint64_t total,
uint16_t sample_count,
const uint8_t *payload,
uint16_t payload_len) {
memcpy(frame, dst_mac, 6);
memcpy(frame + 6, src_mac, 6);
put_be16(frame + 12, ETH_RF_TYPE);
uint8_t *h = frame + 14;
h[0] = 'G';
h[1] = 'R';
h[2] = 'F';
h[3] = '0';
h[4] = cmd;
h[5] = 0;
put_be16(h + 6, seq);
put_be64(h + 8, offset);
put_be64(h + 16, total);
put_be16(h + 24, sample_count);
put_be16(h + 26, payload_len);
put_be32(h + 28, 0);
if (payload_len > 0) {
memcpy(h + HEADER_LEN, payload, payload_len);
}
size_t frame_len = 14 + HEADER_LEN + payload_len;
if (frame_len < MIN_ETH_FRAME) {
memset(frame + frame_len, 0, MIN_ETH_FRAME - frame_len);
frame_len = MIN_ETH_FRAME;
}
return frame_len;
}
static int parse_ack_payload(const uint8_t *payload, size_t len, struct ack *ack) {
if (len < ACK_MIN_LEN) {
return -1;
}
if (payload[0] != 'G' || payload[1] != 'R' || payload[2] != 'F' || payload[3] != '0') {
return -1;
}
if (payload[4] != RSP_ACK) {
return -1;
}
ack->status = payload[5];
ack->seq = get_be16(payload + 6);
ack->offset = get_be64(payload + 8);
ack->space = get_be32(payload + 16);
ack->level = get_be32(payload + 20);
ack->flags = payload[24];
return 0;
}
static int recv_ack(int fd,
const uint8_t fpga_mac[6],
const uint8_t host_mac[6],
uint16_t seq,
double timeout_sec,
struct ack *ack) {
uint8_t frame[65536];
double deadline = monotonic_sec() + timeout_sec;
while (monotonic_sec() < deadline) {
if (stop_requested) {
return -1;
}
double remaining = deadline - monotonic_sec();
if (remaining < 0.0) {
remaining = 0.0;
}
struct timeval tv;
tv.tv_sec = (time_t)remaining;
tv.tv_usec = (suseconds_t)((remaining - (double)tv.tv_sec) * 1000000.0);
fd_set rfds;
FD_ZERO(&rfds);
FD_SET(fd, &rfds);
int rc = select(fd + 1, &rfds, NULL, NULL, &tv);
if (rc < 0) {
if (errno == EINTR) {
if (stop_requested) {
return -1;
}
continue;
}
perror("select");
return -1;
}
if (rc == 0) {
break;
}
ssize_t n = recv(fd, frame, sizeof(frame), 0);
if (n < 0) {
if (errno == EINTR) {
continue;
}
perror("recv");
return -1;
}
if (n < 14 + ACK_MIN_LEN) {
continue;
}
if (get_be16(frame + 12) != ETH_RF_TYPE) {
continue;
}
if (memcmp(frame + 6, fpga_mac, 6) != 0) {
continue;
}
if (memcmp(frame, host_mac, 6) != 0 && memcmp(frame, "\xff\xff\xff\xff\xff\xff", 6) != 0) {
continue;
}
if (parse_ack_payload(frame + 14, (size_t)n - 14, ack) != 0) {
continue;
}
if (ack->seq == seq) {
return 0;
}
}
fprintf(stderr, "timeout waiting ACK seq=%u\n", seq);
return -1;
}
static int transact(int fd,
const uint8_t fpga_mac[6],
const uint8_t host_mac[6],
uint8_t *frame,
size_t frame_len,
uint16_t seq,
double timeout_sec,
struct ack *ack) {
ssize_t sent = send(fd, frame, frame_len, 0);
if (sent < 0) {
perror("send");
return -1;
}
if ((size_t)sent != frame_len) {
fprintf(stderr, "short send: %zd/%zu\n", sent, frame_len);
return -1;
}
return recv_ack(fd, fpga_mac, host_mac, seq, timeout_sec, ack);
}
static int open_packet_socket(const char *iface) {
unsigned int ifindex = if_nametoindex(iface);
if (ifindex == 0) {
perror("if_nametoindex");
return -1;
}
int fd = socket(AF_PACKET, SOCK_RAW, htons(ETH_RF_TYPE));
if (fd < 0) {
perror("socket(AF_PACKET)");
return -1;
}
struct sockaddr_ll addr;
memset(&addr, 0, sizeof(addr));
addr.sll_family = AF_PACKET;
addr.sll_protocol = htons(ETH_RF_TYPE);
addr.sll_ifindex = (int)ifindex;
if (bind(fd, (struct sockaddr *)&addr, sizeof(addr)) != 0) {
perror("bind(AF_PACKET)");
close(fd);
return -1;
}
return fd;
}
static int send_control(int fd,
const uint8_t fpga_mac[6],
const uint8_t host_mac[6],
uint8_t *frame,
uint8_t cmd,
uint16_t seq,
uint64_t total_samples,
double timeout_sec,
struct ack *ack) {
size_t frame_len = build_frame(frame, fpga_mac, host_mac, cmd, seq, 0,
total_samples, 0, NULL, 0);
return transact(fd, fpga_mac, host_mac, frame, frame_len, seq, timeout_sec, ack);
}
static bool send_stop_reset(int fd,
const uint8_t fpga_mac[6],
const uint8_t host_mac[6],
uint8_t *frame,
uint16_t *seq,
uint64_t total_samples,
double timeout_sec,
FILE *log) {
struct ack ack;
bool ok = true;
if (send_control(fd, fpga_mac, host_mac, frame, CMD_STOP, *seq, total_samples, timeout_sec, &ack) == 0 &&
ack.status == ST_OK) {
fprintf(log, "STOP ok space=%" PRIu32 " level=%" PRIu32 " flags=0x%02x\n", ack.space, ack.level, ack.flags);
} else {
fprintf(log, "STOP failed or not acknowledged\n");
ok = false;
}
(*seq)++;
usleep(1000);
if (send_control(fd, fpga_mac, host_mac, frame, CMD_RESET, *seq, total_samples, timeout_sec, &ack) == 0 &&
ack.status == ST_OK) {
fprintf(log, "RESET ok space=%" PRIu32 " level=%" PRIu32 " flags=0x%02x\n", ack.space, ack.level, ack.flags);
} else {
fprintf(log, "RESET failed or not acknowledged\n");
ok = false;
}
(*seq)++;
usleep(1000);
if (send_control(fd, fpga_mac, host_mac, frame, CMD_PING, *seq, total_samples, timeout_sec, &ack) == 0 &&
ack.status == ST_OK) {
fprintf(log, "STATUS ok space=%" PRIu32 " level=%" PRIu32 " flags=0x%02x\n", ack.space, ack.level, ack.flags);
} else {
fprintf(log, "STATUS failed or not acknowledged\n");
ok = false;
}
(*seq)++;
return ok;
}
static int parse_args(int argc, char **argv, struct options *opt) {
*opt = (struct options){
.iface = DEFAULT_IFACE,
.input = DEFAULT_INPUT,
.fpga_mac_text = DEFAULT_FPGA_MAC,
.format = FMT_REAL4_SIGNMAG,
.real8_shift = 4,
.chunk_samples = DEFAULT_CHUNK_SAMPLES,
.mtu = DEFAULT_MTU,
.preload_samples = DEFAULT_PRELOAD_SAMPLES,
.max_samples = 0,
.max_bytes = 0,
.timeout_sec = DEFAULT_TIMEOUT_SEC,
.retry_sleep_us = DEFAULT_RETRY_SLEEP_US,
.progress_interval = DEFAULT_PROGRESS_INTERVAL,
.loop = false,
.no_start = false,
.no_reset = false,
.stop_only = false,
};
for (int i = 1; i < argc; i++) {
const char *arg = argv[i];
const char *value = NULL;
if (strcmp(arg, "-h") == 0 || strcmp(arg, "--help") == 0) {
usage(argv[0]);
exit(0);
} else if (strcmp(arg, "--loop") == 0) {
opt->loop = true;
continue;
} else if (strcmp(arg, "--no-start") == 0) {
opt->no_start = true;
continue;
} else if (strcmp(arg, "--no-reset") == 0) {
opt->no_reset = true;
continue;
} else if (strcmp(arg, "--stop-only") == 0) {
opt->stop_only = true;
continue;
}
if (i + 1 >= argc) {
fprintf(stderr, "missing value for %s\n", arg);
return -1;
}
value = argv[++i];
if (strcmp(arg, "--iface") == 0) {
opt->iface = value;
} else if (strcmp(arg, "--input") == 0) {
opt->input = value;
} else if (strcmp(arg, "--fpga-mac") == 0) {
opt->fpga_mac_text = value;
} else if (strcmp(arg, "--sample-format") == 0) {
if (parse_sample_format(value, &opt->format) != 0) {
fprintf(stderr, "bad sample format: %s\n", value);
return -1;
}
} else if (strcmp(arg, "--real8-shift") == 0) {
uint64_t parsed = 0;
if (parse_u64(value, &parsed) != 0 || parsed > 7) {
fprintf(stderr, "bad --real8-shift: %s\n", value);
return -1;
}
opt->real8_shift = (int)parsed;
} else if (strcmp(arg, "--chunk-samples") == 0) {
if (parse_u32(value, &opt->chunk_samples) != 0 || opt->chunk_samples == 0 || opt->chunk_samples > 65534) {
fprintf(stderr, "bad --chunk-samples: %s\n", value);
return -1;
}
} else if (strcmp(arg, "--mtu") == 0) {
uint64_t parsed = 0;
if (parse_u64(value, &parsed) != 0 || parsed > 65535) {
fprintf(stderr, "bad --mtu: %s\n", value);
return -1;
}
opt->mtu = (int)parsed;
} else if (strcmp(arg, "--preload-samples") == 0) {
if (parse_u64(value, &opt->preload_samples) != 0) {
fprintf(stderr, "bad --preload-samples: %s\n", value);
return -1;
}
} else if (strcmp(arg, "--max-samples") == 0) {
if (parse_u64(value, &opt->max_samples) != 0) {
fprintf(stderr, "bad --max-samples: %s\n", value);
return -1;
}
} else if (strcmp(arg, "--max-bytes") == 0) {
if (parse_u64(value, &opt->max_bytes) != 0) {
fprintf(stderr, "bad --max-bytes: %s\n", value);
return -1;
}
} else if (strcmp(arg, "--timeout") == 0) {
if (parse_double(value, &opt->timeout_sec) != 0) {
fprintf(stderr, "bad --timeout: %s\n", value);
return -1;
}
} else if (strcmp(arg, "--retry-sleep-us") == 0) {
uint64_t parsed = 0;
if (parse_u64(value, &parsed) != 0 || parsed > 10000000) {
fprintf(stderr, "bad --retry-sleep-us: %s\n", value);
return -1;
}
opt->retry_sleep_us = (useconds_t)parsed;
} else if (strcmp(arg, "--retry-sleep") == 0) {
double parsed = 0.0;
if (parse_double(value, &parsed) != 0 || parsed > 10.0) {
fprintf(stderr, "bad --retry-sleep: %s\n", value);
return -1;
}
opt->retry_sleep_us = (useconds_t)(parsed * 1000000.0);
} else if (strcmp(arg, "--progress-interval") == 0) {
if (parse_double(value, &opt->progress_interval) != 0) {
fprintf(stderr, "bad --progress-interval: %s\n", value);
return -1;
}
} else {
fprintf(stderr, "unknown option: %s\n", arg);
return -1;
}
}
return 0;
}
int main(int argc, char **argv) {
struct options opt;
if (parse_args(argc, argv, &opt) != 0) {
usage(argv[0]);
return 2;
}
install_signal_handlers();
uint8_t fpga_mac[6];
uint8_t host_mac[6];
if (parse_mac(opt.fpga_mac_text, fpga_mac) != 0) {
fprintf(stderr, "bad FPGA MAC address: %s\n", opt.fpga_mac_text);
return 1;
}
if (opt.mtu > 0 && set_iface_mtu(opt.iface, opt.mtu) != 0) {
return 1;
}
int current_mtu = get_iface_mtu(opt.iface);
if (current_mtu <= HEADER_LEN) {
fprintf(stderr, "interface MTU too small: %d\n", current_mtu);
return 1;
}
size_t payload_capacity = (size_t)current_mtu - HEADER_LEN;
if (payload_capacity + 14 + HEADER_LEN > MAX_FRAME_SIZE) {
fprintf(stderr, "MTU too large for static frame buffer: mtu=%d\n", current_mtu);
return 1;
}
if (get_iface_mac(opt.iface, host_mac) != 0) {
return 1;
}
uint8_t *frame = malloc(MAX_FRAME_SIZE);
if (!frame) {
fprintf(stderr, "allocation failed\n");
return 1;
}
int fd = open_packet_socket(opt.iface);
if (fd < 0) {
free(frame);
return 1;
}
uint16_t seq = (uint16_t)time(NULL);
struct ack ack;
if (opt.stop_only) {
bool ok = send_stop_reset(fd, fpga_mac, host_mac, frame, &seq, 0, opt.timeout_sec, stdout);
close(fd);
free(frame);
return ok ? 0 : 1;
}
struct stat st;
if (stat(opt.input, &st) != 0) {
perror("stat(input)");
close(fd);
free(frame);
return 1;
}
uint64_t source_bytes_total = (uint64_t)st.st_size;
if (opt.max_bytes && opt.max_bytes < source_bytes_total) {
source_bytes_total = opt.max_bytes;
}
uint64_t total_samples = source_bytes_total;
if (format_is_packed(opt.format)) {
total_samples *= 2;
}
if (opt.max_samples && opt.max_samples < total_samples) {
total_samples = opt.max_samples;
}
size_t source_chunk = opt.chunk_samples;
if (format_is_packed(opt.format)) {
source_chunk = opt.chunk_samples / 2;
if (source_chunk == 0) {
source_chunk = 1;
}
if (source_chunk > payload_capacity) {
source_chunk = payload_capacity;
printf("chunk clipped to %zu samples for mtu=%d\n", source_chunk * 2, current_mtu);
}
} else {
size_t max_source_chunk = payload_capacity * 2;
if (source_chunk > max_source_chunk) {
source_chunk = max_source_chunk;
printf("chunk clipped to %zu samples for mtu=%d\n", source_chunk, current_mtu);
}
}
uint8_t *source_buf = malloc(source_chunk);
uint8_t *payload_buf = malloc(payload_capacity);
if (!source_buf || !payload_buf) {
fprintf(stderr, "allocation failed\n");
close(fd);
free(source_buf);
free(payload_buf);
free(frame);
return 1;
}
if (!opt.no_reset) {
if (send_control(fd, fpga_mac, host_mac, frame, CMD_RESET, seq, total_samples, opt.timeout_sec, &ack) != 0) {
if (stop_requested) {
goto interrupted;
}
goto fail;
}
if (ack.status != ST_OK) {
fprintf(stderr, "RESET failed status=0x%02x\n", ack.status);
goto fail;
}
printf("RESET ok space=%" PRIu32 " level=%" PRIu32 " flags=0x%02x\n", ack.space, ack.level, ack.flags);
seq++;
}
bool started = opt.no_start;
if (!opt.no_start && opt.preload_samples == 0) {
if (send_control(fd, fpga_mac, host_mac, frame, CMD_START, seq, total_samples, opt.timeout_sec, &ack) != 0) {
if (stop_requested) {
goto interrupted;
}
goto fail;
}
if (ack.status != ST_OK) {
fprintf(stderr, "START failed status=0x%02x\n", ack.status);
goto fail;
}
printf("START armed space=%" PRIu32 " level=%" PRIu32 " flags=0x%02x\n", ack.space, ack.level, ack.flags);
seq++;
started = true;
}
double started_at = monotonic_sec();
double last_progress = started_at;
uint64_t sent_samples_total = 0;
uint64_t no_space_retries = 0;
uint32_t min_fifo_level = UINT32_MAX;
do {
FILE *stream = fopen(opt.input, "rb");
if (!stream) {
perror("fopen(input)");
goto fail;
}
uint64_t offset_samples = 0;
while (offset_samples < total_samples) {
if (stop_requested) {
fclose(stream);
goto interrupted;
}
if (!started && offset_samples >= opt.preload_samples && offset_samples >= total_samples) {
break;
}
if (!started && offset_samples >= opt.preload_samples) {
if (send_control(fd, fpga_mac, host_mac, frame, CMD_START, seq, total_samples, opt.timeout_sec, &ack) != 0) {
fclose(stream);
if (stop_requested) {
goto interrupted;
}
goto fail;
}
if (ack.status != ST_OK) {
fprintf(stderr, "START failed status=0x%02x\n", ack.status);
fclose(stream);
goto fail;
}
printf("START armed after preload=%" PRIu64 " space=%" PRIu32 " level=%" PRIu32 " flags=0x%02x\n",
offset_samples, ack.space, ack.level, ack.flags);
if (ack.level < min_fifo_level) {
min_fifo_level = ack.level;
}
seq++;
started = true;
}
uint64_t remaining_samples = total_samples - offset_samples;
size_t read_size = source_chunk;
if (format_is_packed(opt.format)) {
uint64_t remaining_bytes = remaining_samples / 2;
if (remaining_bytes == 0) {
break;
}
if (read_size > remaining_bytes) {
read_size = (size_t)remaining_bytes;
}
} else {
if (read_size > remaining_samples) {
read_size = (size_t)remaining_samples;
}
}
size_t raw_len = fread(source_buf, 1, read_size, stream);
if (stop_requested) {
fclose(stream);
goto interrupted;
}
if (raw_len == 0) {
break;
}
uint16_t sample_count = 0;
size_t payload_len = encode_samples(opt.format, source_buf, raw_len, opt.real8_shift,
payload_buf, &sample_count);
if (payload_len > payload_capacity) {
fprintf(stderr, "encoded payload exceeds MTU capacity: %zu > %zu\n", payload_len, payload_capacity);
fclose(stream);
goto fail;
}
size_t frame_len = build_frame(frame, fpga_mac, host_mac, CMD_DATA, seq, offset_samples,
total_samples, sample_count, payload_buf, (uint16_t)payload_len);
while (true) {
if (stop_requested) {
fclose(stream);
goto interrupted;
}
if (transact(fd, fpga_mac, host_mac, frame, frame_len, seq, opt.timeout_sec, &ack) != 0) {
fclose(stream);
if (stop_requested) {
goto interrupted;
}
goto fail;
}
if (ack.status == ST_OK) {
uint64_t expected = offset_samples + sample_count;
if (ack.offset != expected) {
fprintf(stderr, "ACK offset mismatch got=%" PRIu64 " expected=%" PRIu64 "\n",
ack.offset, expected);
fclose(stream);
goto fail;
}
if (started && ack.level < min_fifo_level) {
min_fifo_level = ack.level;
}
break;
}
if (ack.status == ST_NO_SPACE) {
no_space_retries++;
if (opt.retry_sleep_us) {
usleep(opt.retry_sleep_us);
}
continue;
}
fprintf(stderr, "DATA failed status=0x%02x offset=%" PRIu64 "\n", ack.status, offset_samples);
fclose(stream);
goto fail;
}
offset_samples += sample_count;
sent_samples_total += sample_count;
seq++;
double now = monotonic_sec();
if (now - last_progress >= opt.progress_interval) {
double elapsed = now - started_at;
double rate = elapsed > 0.0 ? (double)sent_samples_total / elapsed : 0.0;
double rf_mbps = rate * 4.0 / 1000000.0;
double rf_mbytes = rate / 2.0 / 1000000.0;
printf("sent=%" PRIu64 "/%" PRIu64 " samples rate=%.3f Msps fifo_level=%" PRIu32
" rf=%.3f Mbps %.3f MB/s fifo_space=%" PRIu32 " no_space=%" PRIu64
" flags=0x%02x\n",
offset_samples, total_samples, rate / 1000000.0, ack.level,
rf_mbps, rf_mbytes, ack.space, no_space_retries, ack.flags);
last_progress = now;
}
}
fclose(stream);
if (!started) {
if (send_control(fd, fpga_mac, host_mac, frame, CMD_START, seq, total_samples, opt.timeout_sec, &ack) != 0) {
if (stop_requested) {
goto interrupted;
}
goto fail;
}
if (ack.status != ST_OK) {
fprintf(stderr, "START failed status=0x%02x\n", ack.status);
goto fail;
}
printf("START armed after preload=%" PRIu64 " space=%" PRIu32 " level=%" PRIu32 " flags=0x%02x\n",
total_samples, ack.space, ack.level, ack.flags);
if (ack.level < min_fifo_level) {
min_fifo_level = ack.level;
}
seq++;
started = true;
}
if (opt.loop) {
printf("loop restart\n");
}
} while (opt.loop);
if (min_fifo_level == UINT32_MAX) {
min_fifo_level = 0;
}
printf("done sent_samples=%" PRIu64 " min_fifo_level=%" PRIu32 " no_space=%" PRIu64 "\n",
sent_samples_total, min_fifo_level, no_space_retries);
(void)send_stop_reset(fd, fpga_mac, host_mac, frame, &seq, total_samples, opt.timeout_sec, stdout);
close(fd);
free(source_buf);
free(payload_buf);
free(frame);
return 0;
interrupted: {
stop_requested = 0;
fprintf(stderr, "\ninterrupt: sending STOP to FPGA RF stream\n");
bool stop_ok = send_stop_reset(fd, fpga_mac, host_mac, frame, &seq, total_samples, opt.timeout_sec, stderr);
close(fd);
free(source_buf);
free(payload_buf);
free(frame);
return stop_ok ? 0 : 130;
}
fail:
close(fd);
free(source_buf);
free(payload_buf);
free(frame);
return 1;
}
Reference in New Issue View Git Blame Copy Permalink