fpv-tools/fpvplay.c

# include <stdio.h>
# include <stdint.h>
# include <stdlib.h>
# include <unistd.h>
# include <string.h>
# include <errno.h>
# include <getopt.h>
# include <sys/types.h>
# include <sys/stat.h>
# include <fcntl.h>
# include <arpa/inet.h>

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;
}