apps/streamer/rtp.c

Go to the documentation of this file.

/*
 * This code includes an implememtation of RFC1889 (RTP)
 *
 * (C)2004 Alexander Melichenko <alexlp@freemail.ru>
 * The code was optimized for use in Elphel 313 cameras
 *
 * This file consist a portin of code from the UCL Common Code Library (2.4.18)
 * Copyright (c) 1998-2001 University College London
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <sys/time.h>
#include <string.h>
#include <unistd.h>
#include <sys/param.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/ip.h>
#include <netinet/udp.h>
#include <linux/if_ether.h>
//#include <linux/if_packet.h>
#include <errno.h>

#include "str.h"
#include "net_udp.h"
#include "rtp.h"

#include <sys/types.h>
#include <sys/uio.h>
#include <unistd.h>

#define SECS_BETWEEN_1900_1970 2208988800u
void ntp64_time(uint32_t * ntp_sec, uint32_t * ntp_frac)
{
    struct timeval now;
    gettimeofday(&now, NULL);
    /* NB ntp_frac is in units of 1 / (2^32 - 1) secs. */
    *ntp_sec = now.tv_sec + SECS_BETWEEN_1900_1970;
    *ntp_frac =
        (now.tv_usec << 12) + (now.tv_usec << 8) -
        ((now.tv_usec * 3650) >> 6);
}

static void init_rng(const int8_t  *s)
{
    static uint32_t seed;
    if (s == NULL) {
        /* This should never happen, but just in case */
        s = "ARANDOMSTRINGSOWEDONTCOREDUMP";
    }
    if (seed == 0) {
        pid_t p = getpid();
        while (*s) {
            seed += (uint32_t) * s++;
            seed = seed * 31 + 1;
        }
        seed = 1 + seed * 31 + (uint32_t) p;
        srand48(seed);
    }
}

int32_t rtp_init(struct param *p)
{
    static int8_t  hname[MAXHOSTNAMELEN];
    int32_t res;

    gethostname(hname, MAXHOSTNAMELEN);
    init_rng(hname);
    
    if (p->dport % 2) {
        return -1;
    }

    res = udp_init4(p);


    if (res != 0) {
        return -1;
    }
    
    p->ip_id = 0;
    p->my_ssrc = (uint32_t) lrand48();
    p->rtp_seq = (uint16_t) lrand48();

    return 0;
}


/*
 * compute an IP header checksum.
 * don't modify the packet.
 */
uint16_t
in_chksum(const uint16_t *addr, register uint32_t len)
{
    int32_t nleft = len;
    const uint16_t *w = addr;
    uint16_t answer;
    int32_t sum = 0;

    while (nleft > 1)  {
        sum += *w++;
        nleft -= 2;
    }

    if (nleft == 1)
        sum += htons(*(uint8_t  *)w<<8);

    sum = (sum >> 16) + (sum & 0xffff);     /* add hi 16 to low 16 */
    sum += (sum >> 16);                     /* add carry */
    answer = ~sum;                          /* truncate to 16 bits */

    return (answer);
}


#define SOURCE_UDP_PORT 20020
int
rtp_send_data(struct param *p, uint32_t rtp_ts, int8_t  pt, int32_t m, int8_t  *data, int32_t data_len, unsigned char *h_buf, int h_offset, int h_len) {
    int32_t buffer_len, rc;
    int8_t  *buffer = data;
    rtp_packet *packet;
//    int32_t stack_offs=0;
    struct iphdr  *iph;
    struct udphdr *udph;
//    struct sockaddr_in s_in;
//    struct sockaddr_ll s_ll;
//    uint32_t res;

    buffer_len = data_len + RTP_HEADER_SIZE;

        // header
        unsigned char *h_ptr = h_buf;
        h_ptr += h_offset;

        // add RTP header;
        h_ptr -= sizeof(rtp_packet);
        h_len += sizeof(rtp_packet);
        packet = (rtp_packet *)((void *)h_ptr);

        /* ...and the actual packet header... */
        /* RTP header */
        packet->v = 2;
        packet->p = 0;
        packet->x = 0;
        packet->cc = 0;
        packet->m = m;
        packet->pt = pt;
        packet->seq = htons(p->rtp_seq++);
        packet->ts = htonl(rtp_ts);
        packet->ssrc = htonl(p->my_ssrc);

        if(p->raw) {
                buffer_len += 8;
                h_ptr -= sizeof(struct udphdr);
                h_len += sizeof(struct udphdr);
                udph = (struct udphdr *)h_ptr;
                udph->len       = htons(sizeof(struct udphdr) + buffer_len);
//              udph->len       = htons(sizeof(struct udphdr) + data_len);
                udph->check     = 0;
                udph->dest      = htons(p->dport);
                udph->source    = htons(SOURCE_UDP_PORT);
        
                h_ptr -= sizeof(struct iphdr);
                h_len += sizeof(struct iphdr);
                iph = (struct iphdr *)h_ptr;
                iph->version = 4;      // ipv4
                iph->ihl        = 5;       // ip header len in words
                iph->tos        = 0;
                iph->tot_len    = htons(sizeof(struct iphdr) + sizeof(struct udphdr) + buffer_len);
//              iph->tot_len    = htons(sizeof(struct iphdr) + sizeof(struct udphdr) + data_len);
                iph->id         = htons((uint16_t) p->ip_id);
                iph->frag_off   = htons(IP_DF);
                iph->ttl        = (uint8_t) p->ttl;
                iph->protocol   = IPPROTO_UDP;
                iph->saddr      = p->src_addr4.s_addr;
                iph->daddr      = (int)p->dst_addr4.s_addr;
                iph->check      = 0;
                iph->check      = in_chksum((uint16_t *) iph, iph->ihl << 2);

/*
                memset(&s_ll, 0, sizeof(s_ll));
        
                s_ll.sll_family   = AF_PACKET;
                s_ll.sll_protocol = htons(ETH_P_IP);
                s_ll.sll_ifindex  = p->eth_index;
                s_ll.sll_halen    = ETH_ALEN;   // MAC

                if(ismulticast(p)){
                        // Generate multicast ethernet address
                        memcpy(s_ll.sll_addr, "\x01\x00\x5e", 3);
                        res = htonl(s_in.sin_addr.s_addr)  & 0x7fffff;
                        memcpy(s_ll.sll_addr + 3, (int8_t *)(&res), 3);
                } else {
                        memcpy(s_ll.sll_addr, p->dst_mac, 6);
                }

                stack_offs += IP_UDP_SIZE;
*/
        }

        struct iovec iov[2];
        int iovcnt = sizeof(iov) / sizeof(struct iovec);
        // [NET]/RTP/MJPEG headers
        iov[0].iov_base = h_ptr;
        iov[0].iov_len = h_len;
        // frame
        iov[1].iov_base = buffer;
        iov[1].iov_len = data_len;

        if(!p->raw) {
                rc = writev(p->fd_udp, iov, iovcnt);
        } else {
//              iov[1].iov_len = 8;
                p->msg.msg_iov = iov;
                p->msg.msg_iovlen = iovcnt;
//              p->msg.msg_control = 0;
//              p->msg.msg_flags = 0;
//              p->msg.msg_name = (void *)&s_ll;
//              p->msg.msg_namelen = sizeof(s_ll);
                rc = sendmsg(p->fd_raw, &p->msg, 0);
                p->ip_id++;
        }
        return rc;
}


void rtp_done(struct param * p) {
    udp_exit4(p);
}


int32_t ismulticast(struct param *p) {
    if(IN_MULTICAST(ntohl(p->dst_addr4.s_addr)))
                return 1;
    return 0;
}
        

---