Cosmetics

[rawv.git] / net.cpp

/* network submodule for rawv
 * Copyright (C) 2010,2011,2012  Kirill Smelkov <kirr@navytux.spb.ru>
 * Copyright (C) 2011  Marine Bridge and Navigation Systems (http://mns.spb.ru/)
 *
 * This library is free software: you can Use, Study, Modify and Redistribute
 * it under the terms of the GNU Lesser General Public License version 2.1, or
 * any later version. This library is distributed WITHOUT ANY WARRANTY. See
 * COPYING.LIB file for full License terms.
 */

#include "rawv.h"

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <string.h>

using std::min;

namespace rawv {

/******** NetTx ********/

/** whether an `addr` is IPv4 multicast */
int in_multicast(const struct sockaddr_in *addr)
{
    /* class D  224.0.0.0 - 239.255.255.255 */
    return ((ntohl(addr->sin_addr.s_addr) & 0xf0000000) == 0xe0000000);
}

/** whether an `addr` is IPv4 broadcast
 *
 * http://en.wikipedia.org/wiki/IPv4_subnetting_reference
 */
int in_broadcast(const struct sockaddr_in *addr)
{
    unsigned long ip = ntohl(addr->sin_addr.s_addr);

    /* class A  0.0.0.0 - 127.255.255.255 /8 */
    if ((ip & 0x80000000) == 0x00000000)
        return (ip & 0x00ffffff) == 0x00ffffff;

    /* class B  128.0.0.0 - 191.255.255.255 /16 */
    if ((ip & 0xc0000000) == 0x80000000)
        return (ip & 0x0000ffff) == 0x0000ffff;

    /* class C  192.0.0.0 - 223.255.255.255  /24 */
    if ((ip & 0xe0000000) == 0xc0000000)
        return (ip & 0x000000ff) == 0x000000ff;

    return 0;
}


NetTx::NetTx(const char *dest, int port, int mtu)
{
    int one=1;  /* XXX has to be uint8_t on win32 */

    sk = socket(PF_INET, SOCK_DGRAM, 0);
    if (sk == -1)
        die_errno("socket");

    tx_addr.sin_family  = AF_INET;
    tx_addr.sin_port    = htons(port);
    tx_addr.sin_addr.s_addr = inet_addr(dest);
    if (tx_addr.sin_addr.s_addr == INADDR_NONE)
        die("E: inet_addr(%s) fail", dest);

    if (in_broadcast(&tx_addr)) {
        fprintf(stderr, "tx: broadcasting...\n");

        if (setsockopt(sk, SOL_SOCKET, SO_BROADCAST, &one, sizeof(one)) == -1)
            die_errno("setsockopt(SO_BROADCAST)");
    }

    if (in_multicast(&tx_addr)) {
        fprintf(stderr, "tx: multicasting...\n");

        if (setsockopt(sk, IPPROTO_IP, IP_MULTICAST_TTL, &one, sizeof(one)) == -1)
            die_errno("setsockopt(IP_MULTICAST_TTL)");

        if (setsockopt(sk, IPPROTO_IP, IP_MULTICAST_LOOP, &one, sizeof(one)) == -1)
            die_errno("setsockopt(IP_MULTICAST_LOOP)");
    }

    this->mtu = mtu;
}


NetTx::~NetTx()
{
    close(sk);
}


bool NetTx::v_query_framebuf(Frame *f)
{
    /* always use source's buffer - we'll transmit frames directly from it */
    return false;
}


/* upon receiving new frame tx it to network */
void NetTx::v_on_frame(const Frame *f)
{
#define RAWV_MAX_FRAG_NLINES    16  /* there is no limit, it's just me being lazy */
    struct iovec  iov[1+RAWV_MAX_FRAG_NLINES];   /* header + payload */
    int iovlen;
    struct msghdr msg;

    int pixsize, frag_nlines, nfragment, line, i;
    int fheight = f->height;
    int bt = f->interlace_tb_swapped;

    struct rawv_header h;

    switch (f->pixfmt_4cc) {
        case MKTAG32('Y','U','Y','V'):
        case MKTAG32('Y','U','Y','2'):
            pixsize = 2;
            break;

        // TODO GREY

        default:
            die("tx_frame: don't know pixfmt 0x%08x", f->pixfmt_4cc);
    }

    frag_nlines = (mtu - sizeof(struct rawv_header)) / (f->width * pixsize);
    if (bt) {
        if (bt < 0)
            die("TODO bt=-1 in NetTx");

        /* make frag_nlines and fheight even, if lines are swapped in captured frame */
        frag_nlines &= ~1U;
        fheight &= ~1U;
    }

    if (frag_nlines > RAWV_MAX_FRAG_NLINES)
        die("FIXME frag_nlines > RAWV_MAX_FRAG_NLINES (%i > %i)", frag_nlines, RAWV_MAX_FRAG_NLINES);

    nfragment  = 0;

    for (line=0; line<fheight; line+= frag_nlines, nfragment++) {
        h.magic     = MKTAG32('R','A','W','V');
        h.version   = 1;
        h.__reserved_for_flags = 0x00;
        h.nframe    = htons(f->sequence);
        h.nfragment = htons(nfragment);
        h.fragments_total = htons((fheight + frag_nlines-1) / frag_nlines);
        h.width     = htons(f->width);
        h.height    = htons(fheight);
        h.pixfmt    = f->pixfmt_4cc;

        h.frag_startline = htons(line);
        h.frag_nlines = htons(min(frag_nlines, fheight - line));

        /* temp: helps debugging bitstream on the wire */
        h.__reserved[0] = 0xaaaaaaaa;
        h.__reserved[1] = 0xffffffff;

        iov[0].iov_base = &h;
        iov[0].iov_len  = sizeof(h);
        iovlen = 1;

        if ((f->width * pixsize == f->bytesperline) &&
            !bt) {
            /* fastpath for stride=width, progressive */
            iov[iovlen].iov_base = f->start + line*f->bytesperline;
            iov[iovlen].iov_len  = frag_nlines*f->bytesperline;

            ++iovlen;
        }
        else {
            /* strided and/or top/bottom swapped frame */
            for (i=0; i<frag_nlines; ++i) {
                iov[iovlen].iov_base = f->start + (line+i+bt)*f->bytesperline;
                iov[iovlen].iov_len  = f->width * pixsize;

                ++iovlen;
                bt = -bt;
            }
        }

        msg.msg_name    = &tx_addr;
        msg.msg_namelen = sizeof(tx_addr);

        msg.msg_iov     = iov;
        msg.msg_iovlen  = iovlen;

        msg.msg_control = NULL;
        msg.msg_controllen = 0;
        msg.msg_flags   = 0;

        /* XXX what to do if not whole pkt sent? */
        if (-1 == sendmsg(sk, &msg, 0 /*flags*/))
            die_errno("sendmsg");
    }
}



/******** NetRx ********/

/* recommended minimum for socket rx buffer */
#define SK_RCVBUF_MIN   (1024*1024)

NetRx::NetRx(const char *mcast_group, int port, int mtu)
{
    int one=1;  /* XXX has to be uint8_t on win32 */
    int rcvbuf;
    socklen_t rcvbuf_len;
    struct ip_mreqn mreq;

    sk = socket(PF_INET, SOCK_DGRAM, 0);
    if (sk == -1)
        die_errno("socket");

    /* ensure rx buffer is big enough */
    rcvbuf_len = sizeof(rcvbuf);
    if (-1 == getsockopt(sk, SOL_SOCKET, SO_RCVBUF, &rcvbuf, &rcvbuf_len))
        die_errno("getsockopt(SO_RCVBUF)");

    if (rcvbuf < SK_RCVBUF_MIN) {
        rcvbuf = SK_RCVBUF_MIN;

        if (-1 == setsockopt(sk, SOL_SOCKET, SO_RCVBUF, &rcvbuf, sizeof(rcvbuf)))
            warn("W: can't increase sk rx buffer to %i bytes\n", rcvbuf);

        rcvbuf_len = sizeof(rcvbuf);
        if (-1 == getsockopt(sk, SOL_SOCKET, SO_RCVBUF, &rcvbuf, &rcvbuf_len))
            die_errno("getsockopt(SO_RCVBUF)");

        if (rcvbuf < SK_RCVBUF_MIN)
            warn("W: can't increase sk rx buffer to %i bytes (got only %i)\n",
                    SK_RCVBUF_MIN, rcvbuf);
    }


    /* allow multiple receivers */
    if (-1 == setsockopt(sk, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)))
        die_errno("setsockopt(SO_REUSEADDR)");

    rx_addr.sin_family = AF_INET;
    rx_addr.sin_port   = htons(port);
    rx_addr.sin_addr.s_addr = INADDR_ANY;

    if (-1 == bind(sk, (sockaddr *)&rx_addr, sizeof(rx_addr)))
        die_errno("bind");

    // XXX is_broadcast ?
    if (mcast_group) {  // XXX or is_multicast(mcast_group) ?
        mreq.imr_multiaddr.s_addr = inet_addr(mcast_group);
        mreq.imr_address.s_addr   = INADDR_ANY;
        mreq.imr_ifindex          = 0; /* any */

        if (mreq.imr_multiaddr.s_addr == INADDR_NONE)
            die("E: inet_addr(%s) fail", mcast_group);

        if (-1 == setsockopt(sk, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)))
            die_errno("setsockopt(IP_ADD_MEMBERSHIP)");
    }


    /* set socket non-blocking, so that we could avoid tons of select and just
     * loop on read until EAGAIN
     *
     * FIXME does not work as expected - read comments below near recv()
     */
    if (-1 == fcntl(sk, F_SETFL, O_NONBLOCK))
        die_errno("fcntl(F_SETFL, O_NONBLOCK)");


    fragbuf.resize(mtu);

    f.start  = NULL;
    f.length = 0;
    f.width  = f.height = f.bytesperline = 0;
    f.pixfmt_4cc = 0;
    f.sequence = 0;
    f.interlace_tb_swapped = 0; /* always progressive on the wire */

    nframe    = 0; // XXX better -1 ?
    nfragment = 0;
    fragments_total = 0;
    frag_received = 0;
    frag_dropped_total = 0;
}


NetRx::~NetRx()
{
    close(sk);  /* this will also leave multicast group */
}


void NetRx::v_start_capture()
{
    die("TODO NetRx::v_start_capture()");
}


void NetRx::v_stop_capture()
{
    die("TODO NetRx::v_stop_capture()");
}


int NetRx::handle_recv()
{
    int err;
    int count = 0;

    /* loop till we get EAGAIN. This way we avoid lots of unneeded select
     * calls
     *
     * FIXME does not work as expected: Under load we seldomly fetch more than
     * a couple of packets in one burst, and so go back to select, back to
     * here, back to select, back here, etc...
     */
    while (1) {
        err = recv(sk, &fragbuf[0], fragbuf.size(), /*flags*/ 0);
        if (err == -1)
            switch (errno) {
                case EAGAIN:
                    //fprintf(stderr, "Looped %i\n", count);
                    return 0;

                default:
                    die_errno("recv");
            }

        ++count;
        __handle_recv(err);
    }
}


#define NFRAME_RESYNC   100
#define NFRAME_MAX      (1 << (sizeof ( ((struct rawv_header *)0) ->nframe) *8))

void NetRx::__handle_recv(unsigned len)
{
    struct rawv_header *h;
    unsigned pixsize, fragsize, framesize, i;
    int fragstride;
    bool ok;

    /* TODO die is not appropriate on RX path */

    if (len < sizeof(h))
        die("recv: len(pkt) < header (%i < %i)", len, sizeof(h));

    h = (struct rawv_header *)&fragbuf[0];
#if __BYTE_ORDER != __BIG_ENDIAN
    /* decode header coming from the wire
     *
     * we access header fields more than once, so it makes sense to first
     * decode them all in one go and then use the result.
     */
    struct rawv_header __h;

#   define ASSERT(cond) do { (void) sizeof(char [1 - 2*!(cond)]); } while (0)
#   define H8(field)    do { ASSERT(sizeof(h->field)==1); __h.field = h->field; } while (0)
#   define H16(field)   do { ASSERT(sizeof(h->field)==2); __h.field = ntohs(h->field); } while (0)
#   define _32(field)   do { ASSERT(sizeof(h->field)==4); __h.field =      (h->field); } while (0)

    _32 (magic);    /* no need to change endiannes - characters go in network-order always */
    H8  (version);
    H8  (__reserved_for_flags);
    H16 (nframe);
    H16 (nfragment);
    H16 (fragments_total);
    H16 (width);
    H16 (height);
    _32 (pixfmt);   /* no need to change endiannes - characters go in network-order always */
    H16 (frag_startline);
    H16 (frag_nlines);
    /* NOTE: don't forget padding __reserved[2] */

#undef  H8
#undef  H16
#undef  _32

    h = &__h;
#endif  /* __BYTE_ORDER != __BIG_ENDIAN */


    if (h->magic != MKTAG32('R','A','W','V'))
        die("wrong magic (0x%08x)", h->magic);

    if (h->version != 1)
        die("unsupported version (%i)", h->version);

    /* ignoring flags for now */

//  fprintf(stderr, "nframe: %i, frag: %i / %i\r", h->nframe, h->nfragment, h->fragments_total);

    if (h->nframe != nframe) {
        if (abs(h->nframe - nframe) > NFRAME_RESYNC) {
            /* too big nframe jump means we have to re-sync this stream */
            fprintf(stderr, "Resync...\n");
        }
        else {
            if ((h->nframe - nframe) % NFRAME_MAX < NFRAME_MAX/2)
                /* newer frame */;
            else {
                /* old-frame fragment. Too late, sorry */
                fprintf(stderr, "old (nframe=%i frag: %i / %i)\n",
                        h->nframe, h->nfragment, h->fragments_total);
                return;
            }
        }
        /* start of new frame. We have to flush current one first */
        if (f.pixfmt_4cc)
            flush_frame();

        //fprintf(stderr, "\n F-%i", h->nframe);
    }

    //fprintf(stderr, " %i", h->nfragment);

    switch (h->pixfmt) {
        case MKTAG32('Y','U','Y','V'):
        case MKTAG32('Y','U','Y','2'):
            pixsize = 2;
            break;

        case MKTAG32('Y','8','0','0'):
        case MKTAG32('Y','8',' ',' '):
        case MKTAG32('G','R','E','Y'):
            pixsize = 1;
            break;

        default:
            pixsize = 0;    // make gcc happy
            die("recv: W: unsupported pixfmt 0x%08x", h->pixfmt);
    }

    /* ensure we have enough data in payload */
    fragsize = pixsize * h->width * h->frag_nlines;
    if (len < sizeof(h) + fragsize)
        die("recv: len(pkt) < header + fragsize (%i < %i + %i)",
                len, sizeof(h), fragsize);

    /* ensure header is self-consistent */
    if (h->frag_startline + h->frag_nlines > h->height)
        die("recv: frag_startline + frag_nlines > height (%i + %i > %i)",
                h->frag_startline, h->frag_nlines, h->height);

    if (!f.pixfmt_4cc) {
        /* first fragment of a new frame */
        f.pixfmt_4cc = h->pixfmt;
        f.width  = h->width;
        f.height = h->height;

        nframe = h->nframe;
        nfragment = h->nfragment;
        fragments_total = h->fragments_total;

        f.sequence = nframe;

        /*
         * upon receiving first fragment, setup frame buffer memory, either in
         * one of subscribers or in our ->framebuf[].
         */
        f.start = NULL;
        f.length = 0;
        f.bytesperline = 0;

        ok = query_sink_framebuf(&f);

        /*
         * no one from subscriber provided frame buffer - we will use our own
         * ->framebuf[]
         */
        if (!ok) {
            /* reallocate framebuf if needed */
            framesize = pixsize * h->width * h->height;
            if (framebuf.size() < framesize)
                framebuf.resize(framesize);

            f.bytesperline = h->width * pixsize;

            f.start  = &framebuf[0];
            f.length = f.height * f.bytesperline;
        }
    }
    else {
        /* check that frame parameters has not changed */
        if ( !(fragments_total == h->fragments_total &&
               f.pixfmt_4cc == h->pixfmt &&
               f.width == h->width &&
               f.height == h->height)
           )
            die("recv: frame params changed inside one frame: "
                "(%iF 0x%08x %ix%i) and (%iF 0x%08x %ix%i)",
                fragments_total, f.pixfmt_4cc, f.width, f.height,
                h->fragments_total, h->pixfmt, h->width, h->height);
    }

    /* this fragment ok */
    frag_received += 1;


    /* put fragment video data into frame */
    fragstride = h->width * pixsize;
    if (f.bytesperline == fragstride) {
        /* special case -- framebuf is packed, so we can use one large memcpy */
        memcpy(f.start + h->frag_startline * f.bytesperline,
               &fragbuf[0]  + sizeof(*h),
               fragsize);
    }
    else {
        for (i=0; i<h->frag_nlines; ++i)
            memcpy(f.start + (h->frag_startline + i) * f.bytesperline,
                   &fragbuf[0] + sizeof(*h) + i*fragstride,
                   fragstride);
    }



    /* whole-frame received detection.
     *
     * TODO this can be reworked so we also support intra-frame fragments *
     *      reordering on the wire.
     */
    if (h->frag_startline + h->frag_nlines == h->height)
        flush_frame();
}


void NetRx::flush_frame()
{
    unsigned dropped;

    /* flush currently-accumulated frame to subscribers */
    notify_v_subscribers(&f);

    /* update loss statistic */
    dropped = (fragments_total - frag_received);
    frag_dropped_total += dropped;
    if (dropped)
        fprintf(stderr, "Dropped %u fragments\n", dropped);

    /* bump sequence number */
    nframe   += 1;
    nfragment = 0;
    fragments_total = 0;
    frag_received = 0;

    /* and clear current frame */
    f.start = NULL;     /* XXX ok ? */
    f.length = 0;
    f.width = f.height = f.bytesperline = 0;
    f.pixfmt_4cc = 0;
    f.sequence = 0;
}

}   // rawv::