From 3806c69e667a4d4a0e5793dfc86c7f7301551fbc Mon Sep 17 00:00:00 2001 From: "Lelik P. Korchagin" Date: Thu, 12 May 2022 11:33:47 +0300 Subject: [PATCH 1/1] * initial import --- .gitignore | 6 ++ Makefile | 25 +++++ fpvcopy.c | 119 +++++++++++++++++++++ fpvdump.c | 110 +++++++++++++++++++ fpvextract.c | 98 +++++++++++++++++ fpvplay.c | 292 +++++++++++++++++++++++++++++++++++++++++++++++++++ fpvstat.c | 190 +++++++++++++++++++++++++++++++++ 7 files changed, 840 insertions(+) create mode 100644 .gitignore create mode 100644 Makefile create mode 100644 fpvcopy.c create mode 100644 fpvdump.c create mode 100644 fpvextract.c create mode 100644 fpvplay.c create mode 100644 fpvstat.c diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..b43e52f --- /dev/null +++ b/.gitignore @@ -0,0 +1,6 @@ +*.fpv +*.rej +*.orig +*.o +*~ +core diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..c26b574 --- /dev/null +++ b/Makefile @@ -0,0 +1,25 @@ +LDFLAGS=-lrt -ggdb +CPPFLAGS=-Wall -DGND -D_GNU_SOURCE -ggdb + +all: fpvextract fpvplay fpvdump fpvcopy fpvstat + +%.o: %.c %.h + $(CC) -c -o $@ $< $(CPPFLAGS) + +fpvplay: fpvplay.o + $(CC) -o $@ $^ $(LDFLAGS) + +fpvextract: fpvextract.o + $(CC) -o $@ $^ $(LDFLAGS) + +fpvdump: fpvdump.o + $(CC) -o $@ $^ $(LDFLAGS) + +fpvcopy: fpvcopy.o + $(CC) -o $@ $^ $(LDFLAGS) + +fpvstat: fpvstat.o + $(CC) -o $@ $^ $(LDFLAGS) + +clean: + rm -f fpvextract fpvplay fpvdump fpvcopy *~ *.o diff --git a/fpvcopy.c b/fpvcopy.c new file mode 100644 index 0000000..6ec6bff --- /dev/null +++ b/fpvcopy.c @@ -0,0 +1,119 @@ +# include +# include +# include +# include +# include +# include +# include +# include +# include +# include +# include + +struct fpv +{ + uint32_t len; + uint8_t type; + uint32_t ms; + uint8_t pad[8]; +} __attribute__((packed)); + + +static const char short_options[] = "s:e:"; +static const struct option long_options[] = +{ + { "start", required_argument, NULL, 's' }, + { "end", required_argument, NULL, 'e' }, + { 0, 0, NULL, 0 } +}; + +static int opt_s = 0; +static int opt_e = -1; + +size_t in(int fd, uint8_t *data, size_t len) +{ + int res = 0; + + while (len > 0) { + size_t n = read(fd, data, len); + if (n > 0) { + len -= n; + data += n; + res += n; + } else { + if (n < 0) { + if (errno != EAGAIN) { + printf("read: %s\n", strerror(errno)); + } else { + continue; + } + } + break; + } + } + return res; +} + +int main(int argc, char *argv[]) +{ + struct fpv fpv; + static uint8_t data[8 * 1024 * 1024]; + int i; + + for (;;) { + int c; + + if ((c = getopt_long_only(argc, argv, short_options, long_options, &i)) == -1) { + break; + } + + switch (c) { + case 's': + opt_s = atoi(optarg); + break; + + case 'e': + opt_e = atoi(optarg); + break; + + case 0: + break; + + default: + return 1; + } + } + + if ((argc - optind) < 1) { + return 1; + } + + for (i = 0; (optind + i) < argc; i++) { + int fd = open(argv[optind + i], O_RDONLY); + + fprintf(stderr, "%s\n", argv[optind + i]); + if (fd >= 0) { + while (1) { + if (in(fd, (uint8_t*)&fpv, sizeof(fpv)) < 0) { + break; + } + + if (in(fd, data, fpv.len) < 0) { + break; + } + + if (opt_e > 0 && fpv.ms >= opt_e) { + break; + } + + if (fpv.ms >= opt_s) { + fpv.ms -= opt_s; + write(fileno(stdout), &fpv, sizeof(fpv)); + write(fileno(stdout), data, fpv.len); + } + } + close(fd); + } + } + return 0; +} diff --git a/fpvdump.c b/fpvdump.c new file mode 100644 index 0000000..1d76463 --- /dev/null +++ b/fpvdump.c @@ -0,0 +1,110 @@ +# include +# include +# include +# include +# include +# include +# include +# include +# include +# include +# include + +struct fpv +{ + uint32_t len; + uint8_t type; + uint32_t ms; + uint8_t pad[8]; +} __attribute__((packed)); + + +static const char short_options[] = "s:e:"; +static const struct option long_options[] = +{ + { "start", required_argument, NULL, 's' }, + { "end", required_argument, NULL, 'e' }, + { 0, 0, NULL, 0 } +}; + +static int opt_s = 0; +static int opt_e = 0; + +size_t in(int fd, uint8_t *data, size_t len) +{ + int res = 0; + + while (len > 0) { + size_t n = read(fd, data, len); + if (n > 0) { + len -= n; + data += n; + res += n; + } else { + if (n < 0) { + if (errno != EAGAIN) { + printf("read: %s\n", strerror(errno)); + } else { + continue; + } + } + break; + } + } + return res; +} + +int main(int argc, char *argv[]) +{ + struct fpv fpv; + static uint8_t data[8 * 1024 * 1024]; + int i; + + for (;;) { + int c; + + if ((c = getopt_long_only(argc, argv, short_options, long_options, &i)) == -1) { + break; + } + + switch (c) { + case 's': + opt_s = atoi(optarg); + break; + + case 'e': + opt_e = atoi(optarg); + break; + + case 0: + break; + + default: + return 1; + } + } + + if ((argc - optind) < 1) { + return 1; + } + + for (i = 0; (optind + i) < argc; i++) { + int fd = open(argv[optind + i], O_RDONLY); + + if (fd >= 0) { + while (1) { + if (in(fd, (uint8_t*)&fpv, sizeof(fpv)) > 0) { + if (in(fd, data, fpv.len) > 0) { + if (fpv.ms >= opt_s) { + printf("%1u %10u %10u\n", fpv.type, fpv.ms, fpv.len); + } + continue; + } + } + break; + } + close(fd); + } + } + return 0; +} diff --git a/fpvextract.c b/fpvextract.c new file mode 100644 index 0000000..5585b35 --- /dev/null +++ b/fpvextract.c @@ -0,0 +1,98 @@ +# include +# include +# include +# include +# include +# include +# include +# include +# include + +struct fpv +{ + uint32_t len; + uint8_t type; + uint32_t ms; + uint8_t pad[8]; +} __attribute__((packed)); + +size_t out(int fd, uint8_t *data, size_t len) +{ + int res = 0; + + while (len > 0) { + size_t n; + + if ((n = write(fd, data, len)) > 0) { + len -= n; + data += n; + res += n; + } else { + if (errno != EAGAIN) { + printf("write: %s\n", strerror(errno)); + } else { + continue; + } + break; + } + } + return res; +} + +size_t in(int fd, uint8_t *data, size_t len) +{ + int res = 0; + + while (len > 0) { + size_t n = read(fd, data, len); + if (n > 0) { + len -= n; + data += n; + res += n; + } else { + if (n < 0) { + if (errno != EAGAIN) { + printf("read: %s\n", strerror(errno)); + } else { + continue; + } + } + break; + } + } + return res; +} + +int main(int argc, char *argv[]) +{ + struct fpv fpv; + static uint8_t data[8 * 1024 * 1024]; + int i; + + for (i = 1; i < argc; i++) { + int fd = open(argv[i], O_RDONLY); + + if (fd >= 0) { + while (1) { + if (in(fd, (uint8_t*)&fpv, sizeof(fpv)) > 0) { + if (in(fd, data, fpv.len) > 0) { + switch (fpv.type) { + case 0: + out(fileno(stdout), data, fpv.len); + break; + + default: + continue; + } + } else { + break; + } + } else { + break; + } + } + close(fd); + } + } + return 0; +} diff --git a/fpvplay.c b/fpvplay.c new file mode 100644 index 0000000..c5f375e --- /dev/null +++ b/fpvplay.c @@ -0,0 +1,292 @@ +# include +# include +# include +# include +# include +# include +# include +# include +# include +# include +# include + +struct fpv +{ + uint32_t len; + uint8_t type; + uint32_t ms; + uint8_t pad[8]; +} __attribute__((packed)); + +size_t out(int fd, struct sockaddr_in *to, uint8_t *data, size_t len) +{ + int res = 0; + + while (len > 0) { + size_t n = len; + + if (n > 1400) { + n = 1400; + } + + if ((n = sendto(fd, data, n, 0, (struct sockaddr*)to, sizeof(struct sockaddr_in))) > 0) { + len -= n; + data += n; + res += n; + } else { + if (errno != EAGAIN) { + printf("write: %s\n", strerror(errno)); + } else { + continue; + } + break; + } + } + return res; +} + +enum +{ + NUT_CODED_SLICE_NON_IDR = 1, // Coded slice of a non-IDR picture + NUT_CODED_SLICE_IDR = 5, // Coded slice of an IDR picture + NUT_SPS = 7, // Sequence parameter set + NUT_PPS = 8 // Picture parameter set +}; + +struct parameter_set +{ + uint8_t data[256]; + size_t length; + int found; +}; + +size_t out_v(int fd, struct sockaddr_in *to, uint8_t *data, int len) +{ + static struct parameter_set sps = { .data = { 0, }, .length = 0, .found = 0 }; + static struct parameter_set pps = { .data = { 0, }, .length = 0, .found = 0 }; + static struct parameter_set *ps = NULL; + + static struct { uint8_t data[4]; int length; } marker = { .length = 0, .data = { 0, } }; + struct { uint8_t data[4096]; int length; } pkt = { .length = 0 }; + int p; + + for (p = 0; p < len; p++) { + int c = data[p]; + + if (marker.length == 4) { + if (memcmp(marker.data, "\x00\x00\x00\x01", 4) == 0) { + switch (c & 0x1F) { + case NUT_SPS: + memset(ps = &sps, 0, sizeof(sps)); + sps.found = 1; + break; + + case NUT_PPS: + memset(ps = &pps, 0, sizeof(pps)); + pps.found = 1; + break; + + case NUT_CODED_SLICE_NON_IDR: + case NUT_CODED_SLICE_IDR: + if (sps.found == 0 && sps.length > 0) { + int i; for (i = 0; i < sps.length; i++) { + if (pkt.length < sizeof(pkt.data)) { + pkt.data[pkt.length++] = sps.data[i]; + } + } + } + sps.found = 0; + + if (pps.found == 0 && pps.length > 0) { + int i; for (i = 0; i < pps.length; i++) { + if (pkt.length < sizeof(pkt.data)) { + pkt.data[pkt.length++] = pps.data[i]; + } + } + } + pps.found = 0; + + default: + ps = NULL; + break; + } + + marker.data[0] = 0; + marker.data[1] = 0; + marker.data[2] = 0; + marker.data[3] = 0; + marker.length = 0; + } else { + marker.data[0] = marker.data[1]; + marker.data[1] = marker.data[2]; + marker.data[2] = marker.data[3]; + marker.data[3] = c & 0xFF; + } + } else { + marker.data[marker.length++] = c & 0xFF;; + } + + if (ps != NULL) { + if (ps->length < sizeof(ps->data)) { + ps->data[ps->length++] = c & 0xFF; + } else { + memset(ps, 0, sizeof(*ps)); + ps = NULL; + } + } + if (pkt.length < sizeof(pkt.data)) { + pkt.data[pkt.length++] = c & 0xFF; + } + } + + return out(fd, to, pkt.data, pkt.length); +} + +size_t in(int fd, uint8_t *data, size_t len) +{ + int res = 0; + + while (len > 0) { + size_t n = read(fd, data, len); + if (n > 0) { + len -= n; + data += n; + res += n; + } else { + if (n < 0) { + if (errno != EAGAIN) { + printf("read: %s\n", strerror(errno)); + } else { + continue; + } + } + break; + } + } + return res; +} + +static const char short_options[] = "s:e:t:"; +static const struct option long_options[] = +{ + { "start", required_argument, NULL, 's' }, + { "end", required_argument, NULL, 'e' }, + { "to", required_argument, NULL, 't' }, + { 0, 0, NULL, 0 } +}; + +static int opt_s = -1; +static int opt_e = -1; +static char *opt_t = "127.255.255.255"; + +int main(int argc, char *argv[]) +{ + struct fpv fpv; + static uint8_t data[8 * 1024 * 1024]; + int i; + + for (;;) { + int c; + + if ((c = getopt_long_only(argc, argv, short_options, long_options, &i)) == -1) { + break; + } + + switch (c) { + case 's': + opt_s = atoi(optarg); + break; + + case 'e': + opt_e = atoi(optarg); + break; + + case 't': + opt_t = optarg; + break; + + case 0: + break; + + default: + return 1; + } + } + + + struct sockaddr_in all; + int on = 1; + int udp; + + memset(&all, 0, sizeof(all)); + all.sin_family = AF_INET; + all.sin_addr.s_addr = inet_addr(opt_t); + all.sin_port = htons(0); + + udp = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); + setsockopt(udp, SOL_SOCKET, SO_BROADCAST, &on, sizeof(on)); + + if ((argc - optind) < 1) { + return 1; + } + + for (i = 0; (optind + i) < argc; i++) { + int fd = open(argv[optind + i], O_RDONLY); + int ms = 0; + + if (fd >= 0) { + while (1) { + if (in(fd, (uint8_t*)&fpv, sizeof(fpv)) > 0) { + int play = 1; + + if (opt_s > 0 && fpv.ms < opt_s) { + play = 0; + } + + if (opt_e > 0 && fpv.ms >= opt_e) { + break; + } + + if (play) { + int sleep = fpv.ms - ms; + if (sleep > 0) { + usleep(sleep * 1000); + } + } + ms = fpv.ms; + + if (in(fd, data, fpv.len) > 0) { + if (!play) { + continue; + } + + switch (fpv.type) { + case 0: + all.sin_port = htons(5000); + break; + + case 2: + all.sin_port = htons(14550); + break; + + case 5: + all.sin_port = htons(5003); + break; + + default: + continue; + } + + out(udp, &all, data, fpv.len); + } else { + break; + } + } else { + break; + } + } + close(fd); + } + } + return 0; +} diff --git a/fpvstat.c b/fpvstat.c new file mode 100644 index 0000000..c74ceff --- /dev/null +++ b/fpvstat.c @@ -0,0 +1,190 @@ +# include +# include +# include +# include +# include +# include +# include +# include +# include +# include +# include + +struct fpv +{ + uint32_t len; + uint8_t type; + uint32_t ms; + uint8_t pad[8]; +} __attribute__((packed)); + +struct cpu +{ + uint8_t load; + uint8_t temp; +} __attribute__((packed)); + +struct air +{ + struct + { + int8_t rssi; + uint32_t lost; + } __attribute__((packed)) uplink; + + struct + { + int8_t rssi; + uint32_t lost; + } __attribute__((packed)) rc; + + struct cpu cpu; + uint32_t blocks; + uint32_t skipped; + uint32_t failed; + long long time; + uint16_t bitrate; + uint16_t measured_bitrate; + uint8_t cts; + uint8_t undervoltage; +} __attribute__((packed)); + +struct gnd +{ + uint32_t broken; + uint32_t lost; + uint32_t skipped; + uint32_t received; + uint32_t real_bitrate; + uint32_t measured_bitrate; + uint32_t bitrate; + uint32_t uplink_lost; + uint32_t downlink_lost; + uint32_t unused[3]; + int8_t air_rssi; + int8_t joystick; + struct + { + struct cpu gnd; + struct cpu air; + } __attribute__((packed)) cpu; + uint32_t nics; + struct + { + + uint32_t packets; + int8_t rssi; + int8_t type; + } __attribute__((packed)) nic[6]; +} __attribute__((packed)); + + +static const char short_options[] = "s:e:"; +static const struct option long_options[] = +{ + { "start", required_argument, NULL, 's' }, + { "end", required_argument, NULL, 'e' }, + { 0, 0, NULL, 0 } +}; + +static int opt_s = 0; +static int opt_e = 0; + +size_t in(int fd, uint8_t *data, size_t len) +{ + int res = 0; + + while (len > 0) { + size_t n = read(fd, data, len); + if (n > 0) { + len -= n; + data += n; + res += n; + } else { + if (n < 0) { + if (errno != EAGAIN) { + printf("read: %s\n", strerror(errno)); + } else { + continue; + } + } + break; + } + } + return res; +} + +int main(int argc, char *argv[]) +{ + struct fpv fpv; + static uint8_t data[8 * 1024 * 1024]; + int i; + + for (;;) { + int c; + + if ((c = getopt_long_only(argc, argv, short_options, long_options, &i)) == -1) { + break; + } + + switch (c) { + case 's': + opt_s = atoi(optarg); + break; + + case 'e': + opt_e = atoi(optarg); + break; + + case 0: + break; + + default: + return 1; + } + } + + if ((argc - optind) < 1) { + return 1; + } + + for (i = 0; (optind + i) < argc; i++) { + int fd = open(argv[optind + i], O_RDONLY); + + if (fd >= 0) { + while (1) { + if (in(fd, (uint8_t*)&fpv, sizeof(fpv)) > 0) { + if (in(fd, data, fpv.len) > 0) { + switch (fpv.type) { + case 5: + { + struct gnd *gnd = (struct gnd *)data; + for (i = 0; i < gnd->nics; i++) { + printf(" %5d", gnd->nic[i].rssi); + } + printf(" %5d", gnd->lost); + printf(" %5d", gnd->broken); + printf(" %5d", gnd->air_rssi); + printf(" %5d", gnd->uplink_lost); + printf(" %5d", gnd->uplink_lost); + printf(" %5d", gnd->cpu.air.load); + printf(" %5d", gnd->cpu.air.temp); + printf(" %5d", gnd->cpu.gnd.load); + printf(" %5d", gnd->cpu.gnd.temp); + printf("\n"); + continue; + } + break; + + default: + continue; + } + } + } + break; + } + close(fd); + } + } + return 0; +} -- 2.39.5