rtp丢包重传demo

基于ffmpeg, 稍作修改，测试例子： 

ffmpeg -fflags +genpts -re -i 0Cannon.f4v \

 -an -vcodec copy -f rtp rtp://224.0.0.239:5002?localport=5000 \
 -vn -acodec copy -f rtp rtp://224.0.0.239:5004?localport=5006 \
 > live.sdp

sed -i 's/AVP/AVPF/g' live.sdp

#修改rtpdec.c，强制发送RTCP_RTPFB,重新编译后，
./ffplay -sdp_flags +rtcp_to_source ~/video/live.sdp

#receive video - audio, listen on video feedback from clients

./mc 224.0.0.239 5002 224.0.0.239 5004 127.0.0.1 5001 127.0.0.1 5007

这里的127.0.0.1 5001 是客户端丢视频包后请求重传的地址。

/*
mingw: gcc -g -O0 mc.c -lws2_32
*/

#include <stdint.h>

#ifdef _WIN32
#include <winsock2.h>
#include <ws2tcpip.h>

#define HAVE_WINSOCK2_H 1

#ifndef EPROTONOSUPPORT
#define EPROTONOSUPPORT WSAEPROTONOSUPPORT
#endif
#ifndef ETIMEDOUT
#define ETIMEDOUT       WSAETIMEDOUT
#endif
#ifndef ECONNREFUSED
#define ECONNREFUSED    WSAECONNREFUSED
#endif
#ifndef EINPROGRESS
#define EINPROGRESS     WSAEINPROGRESS
#endif

#define SHUT_RD SD_RECEIVE
#define SHUT_WR SD_SEND
#define SHUT_RDWR SD_BOTH

#define getsockopt(a, b, c, d, e) getsockopt(a, b, c, (char*) d, e)
#define setsockopt(a, b, c, d, e) setsockopt(a, b, c, (const char*) d, e)
struct pollfd {
    int fd;
    short events;  /* events to look for */
    short revents; /* events that occurred */
};
typedef int nfds_t;

/* events & revents */
#define POLLIN     0x0001  /* any readable data available */
#define POLLOUT    0x0002  /* file descriptor is writeable */
#define POLLRDNORM POLLIN
#define POLLWRNORM POLLOUT
#define POLLRDBAND 0x0008  /* priority readable data */
#define POLLWRBAND 0x0010  /* priority data can be written */
#define POLLPRI    0x0020  /* high priority readable data */

/* revents only */
#define POLLERR    0x0004  /* errors pending */
#define POLLHUP    0x0080  /* disconnected */
#define POLLNVAL   0x1000  /* invalid file descriptor */

int ff_poll(struct pollfd *fds, nfds_t numfds, int timeout);
#define poll ff_poll

#else
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <poll.h>
#define closesocket close
#endif
#include <time.h>
#include <string.h>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>

#include <fcntl.h> //F_SETFL
#include <errno.h> //EAGAIN
#include <stdint.h>

#define MAX_GROUP 2000
#define MSGBUFSIZE 2048 

#define PKT_Q_S 128

enum {
	MCAST_STATE_READ = 1,
	MCAST_STATE_WRITE,
};

typedef struct mcast{
	int fd;
	uint32_t ssrc;
	int state;

	struct sockaddr_in faddr; /*from addr*/
	socklen_t faddrlen;

	uint8_t *pkt_data[PKT_Q_S];
	int pkt_size[PKT_Q_S];
	int widx; /*current writing index*/

	struct mcast *ref_mcast;
	int ref_idx[PKT_Q_S];
	int ref_seq[PKT_Q_S];
	int ref_count;
	int ref_w;
	
	struct mcast *next;
}mcast_t;

static mcast_t s_mcast[MAX_GROUP];

#ifdef _WIN32
int ff_poll(struct pollfd *fds, nfds_t numfds, int timeout)
{
    fd_set read_set;
    fd_set write_set;
    fd_set exception_set;
    nfds_t i;
    int n;
    int rc;

#if HAVE_WINSOCK2_H
    if (numfds >= FD_SETSIZE) {
        errno = EINVAL;
        return -1;
    }
#endif /* HAVE_WINSOCK2_H */

    FD_ZERO(&read_set);
    FD_ZERO(&write_set);
    FD_ZERO(&exception_set);

    n = 0;
    for (i = 0; i < numfds; i++) {
        if (fds[i].fd < 0)
            continue;
#if !HAVE_WINSOCK2_H
        if (fds[i].fd >= FD_SETSIZE) {
            errno = EINVAL;
            return -1;
        }
#endif /* !HAVE_WINSOCK2_H */

        if (fds[i].events & POLLIN)
            FD_SET(fds[i].fd, &read_set);
        if (fds[i].events & POLLOUT)
            FD_SET(fds[i].fd, &write_set);
        if (fds[i].events & POLLERR)
            FD_SET(fds[i].fd, &exception_set);

        if (fds[i].fd >= n)
            n = fds[i].fd + 1;
    }

    if (n == 0)
        /* Hey!? Nothing to poll, in fact!!! */
        return 0;

    if (timeout < 0) {
        rc = select(n, &read_set, &write_set, &exception_set, NULL);
    } else {
        struct timeval tv;
        tv.tv_sec  = timeout / 1000;
        tv.tv_usec = 1000 * (timeout % 1000);
        rc         = select(n, &read_set, &write_set, &exception_set, &tv);
    }

    if (rc < 0)
        return rc;

    for (i = 0; i < numfds; i++) {
        fds[i].revents = 0;

        if (FD_ISSET(fds[i].fd, &read_set))
            fds[i].revents |= POLLIN;
        if (FD_ISSET(fds[i].fd, &write_set))
            fds[i].revents |= POLLOUT;
        if (FD_ISSET(fds[i].fd, &exception_set))
            fds[i].revents |= POLLERR;
    }

    return rc;
}
#endif

int ff_socket_nonblock(int socket, int enable)
{
#if HAVE_WINSOCK2_H
    u_long param = enable;
    return ioctlsocket(socket, FIONBIO, ¶m);
#else
    if (enable)
        return fcntl(socket, F_SETFL, fcntl(socket, F_GETFL) | O_NONBLOCK);
    else
        return fcntl(socket, F_SETFL, fcntl(socket, F_GETFL) & ~O_NONBLOCK);
#endif /* HAVE_WINSOCK2_H */
}

#define AV_RB16(x) (((x)[0] << 8) | (x)[1])
#define AV_RB32(x) (((x)[0] << 24) | ((x)[1] << 16) | ((x)[2] << 8) | (x)[3])
#define FFMIN(x, y) ((x) < (y) ? (x) : (y) )

#define RTP_PT_PRIVATE 96
#define RTP_VERSION 2

#define RTP_FLAG_KEY    0x1 ///< RTP packet contains a keyframe
#define RTP_FLAG_MARKER 0x2 ///< RTP marker bit was set for this packet

/* RTCP packet types http://tools.ietf.org/html/rfc4585#section-6 */
enum RTCPType {
    RTCP_FIR    = 192,
    RTCP_NACK, // 193
    RTCP_SMPTETC,// 194
    RTCP_IJ,   // 195
    RTCP_SR     = 200,
    RTCP_RR,   // 201
    RTCP_SDES, // 202
    RTCP_BYE,  // 203
    RTCP_APP,  // 204
    RTCP_RTPFB,// 205
    RTCP_PSFB, // 206
    RTCP_XR,   // 207
    RTCP_AVB,  // 208
    RTCP_RSI,  // 209
    RTCP_TOKEN,// 210
};

static char* mcast_dump(mcast_t *mc)
{
	static char buf[512];
	char *ptr = buf, *end = buf + sizeof(buf)-16;
	int i;
	ptr += sprintf(ptr, "ssrc %x seq ", mc->ssrc);
	for(i = 0; i < mc->ref_count && ptr < end; ++i){
		ptr += sprintf(ptr, "%d ", mc->ref_seq[i]);
	}

	return buf;
}

static int find_cached_rtp(mcast_t *mc, uint16_t first_missing, uint16_t following_mask)
{/*return >0 means found and fillter*/
	int i, j, k, cnt = 0;
	uint16_t seq;
	uint8_t *ptr;
	mcast_t *m = &s_mcast[0];
	for(; m; m = m->next){
		if(m->ssrc == mc->ssrc){
			break;
		}
	}
	if(!m){
		return -1;
	}

	for(i = 0; i < PKT_Q_S; ++i){
		if(!m->pkt_data[i]){continue;}
		seq = AV_RB16(m->pkt_data[i]+2);
		if(seq == first_missing){
			break;
		}
	}
	if(i >= PKT_Q_S){
		return -2;
	}

	cnt = 0;
	mc->ref_mcast = m;
	m->ref_seq[cnt] = seq;
	m->ref_idx[cnt++] = i;

	for(j = 1; j <= 16; ++j){
		if(following_mask & (1<<(j-1))){
			k = (i+ j)%PKT_Q_S;
			ptr = m->pkt_data[k];
			if(ptr && AV_RB16(ptr+2) == (seq+j)){
				m->ref_seq[cnt] = seq+j;
				m->ref_idx[cnt++] = k;
			}
		}
	}

	m->ref_count = cnt;
	m->ref_w = 0;
	return cnt;
}

static int rtcp_parse_packet(mcast_t *mc, uint8_t *buf, int len)
{
    int payload_len;
    while (len >= 4) {
        payload_len = FFMIN(len, (AV_RB16(buf + 2) + 1) * 4);
		
		//printf("rtcp %u len %u\n", buf[1], payload_len);

		switch (buf[1]){
			case RTCP_SR://rtcp_send_sr
				break;
			case RTCP_BYE:
				return -RTCP_BYE;
				break;
			case RTCP_RR:
				break;
			case RTCP_RTPFB:
				if(buf[0] == ((RTP_VERSION << 6)|1) && MCAST_STATE_READ == mc->state){
					uint32_t ssrc = AV_RB32(buf+8);
					uint16_t first_missing = AV_RB16(buf+12);
					uint16_t missing_mask = AV_RB16(buf+14);
					//printf(" missing seq %u %04x on ssrc %x\n", first_missing, missing_mask, ssrc);
					if(mc->ssrc && mc->ssrc != ssrc){
						mc->ssrc = 0;
						return -1;
					}
					mc->ssrc = ssrc;
					mc->ref_w = mc->ref_count = 0;
					if(find_cached_rtp(mc, first_missing, missing_mask) > 0){
						mcast_t *mr = mc->ref_mcast;
						mr->faddr = mc->faddr;
						mr->faddrlen = mc->faddrlen;
						mr->state = MCAST_STATE_WRITE;
					}
				}
				break;
		}

        buf += payload_len;
        len -= payload_len;
    }
    return -1;
}


static int rtp_parse_one_packet(mcast_t *mc, uint8_t *buf, int len)
{/* ref ff_rtp_send_data/avio_flush --> udp boud per seq.*/
	unsigned int ssrc;
    int payload_type, seq, flags;
    int ext, csrc;
    uint32_t timestamp;
	uint8_t *ptr;

	if (len < 12 || (buf[0] & 0xc0) != (RTP_VERSION << 6)){
		fprintf(stderr, "bad rtp len %d %x\n", len, buf[0]);
        return -1;
	}

    csrc         = buf[0] & 0x0f;
    ext          = buf[0] & 0x10;
    payload_type = buf[1] & 0x7f;
    if (buf[1] & 0x80)
        flags = RTP_FLAG_MARKER;
    seq       = AV_RB16(buf + 2);
    timestamp = AV_RB32(buf + 4);
    ssrc      = AV_RB32(buf + 8);

	if(mc->widx >=0){
		ptr = mc->pkt_data[mc->widx];
		if(mc->ssrc != ssrc 
				|| (ptr && seq != (AV_RB16(ptr + 2) + 1)%65536) ){
			mc->widx = -1; /*reset*/
			return 0;
		}

		mc->widx = (mc->widx + 1) % PKT_Q_S;
	}else{/*first time*/
		mc->ssrc = ssrc;
		mc->widx = 0; 
	}

	if(mc->pkt_data[mc->widx]){
		free(mc->pkt_data[mc->widx]);
	}
	mc->pkt_data[mc->widx] = buf;
	mc->pkt_size[mc->widx] = len;
	mc->ssrc = ssrc;

	//printf("==rtp seq %u payload %x %u\n", seq, buf[0], len);

	return 0;
}

static int parse_packet(mcast_t *mc, uint8_t *buf, int len)
{
	int is_rtcp = (RTCP_FIR <= buf[1] && buf[1] <= RTCP_IJ)
		|| (RTCP_SR <= buf[1] && buf[1] <= RTCP_TOKEN);

	return is_rtcp ? rtcp_parse_packet(mc, buf, len) :
		rtp_parse_one_packet(mc, buf, len);
}

static int is_multcast_addr(char *str)
{
	if(!str){
		return 0;
	}

	int a = atoi(str);
	if(224 <= a && a <=239){
		return 1;
	}
	return 0;
}

static int mcast_open(char *ip, int port)
{
	struct sockaddr_in saddr;
	struct in_addr iaddr;

	struct ip_mreq mreq;
	int flag, ret;
	int fd = socket(AF_INET, SOCK_DGRAM, 0);
	if(fd < 0){
		perror("socket");
		goto end;
	}
	
	flag = 1;	
	ret = setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(flag));
	if(ret < 0){
		perror("Reusing ADDR failed");
		goto end;
	}
	
	memset(&saddr, 0, sizeof(saddr));
	saddr.sin_family = AF_INET;
	saddr.sin_addr.s_addr = htonl(INADDR_ANY); 
	saddr.sin_port = htons(port);;
	ret = bind(fd, (struct sockaddr *)&saddr,sizeof(struct sockaddr_in));
	if(ret < 0){
		perror("err bind");
		goto end;
	}

	ff_socket_nonblock(fd, 1);
	
	if(is_multcast_addr(ip)){
		memset(&iaddr, 0, sizeof(struct in_addr));
		iaddr.s_addr = INADDR_ANY;
		setsockopt(fd, IPPROTO_IP, IP_MULTICAST_IF, &iaddr, sizeof(iaddr));
		//IP_MULTICAST_TTL

		mreq.imr_multiaddr.s_addr = inet_addr(ip);
		mreq.imr_interface.s_addr = htonl(INADDR_ANY);
		ret = setsockopt(fd, IPPROTO_IP,IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
		if (ret < 0){
			perror("set sock opt");
			goto end;
		}
	}
	
	return fd;	
end:
	if(fd >= 0){
		closesocket(fd);
	}
	return -1;
}

int main(int ac, char *av[])
{
	struct sockaddr_in saddr;
	int nbytes, addrlen;
	
	int flag, ret, i, cnt;
	
	if(ac < 3 || !(ac&1)){
		return printf("usage: mc 224.0.0.239 5000 [remote_ip remote_port ... feedback_ip feedback_port]\n");
	}

	#ifdef _WIN32
	WSADATA wsd;
	WSAStartup(MAKEWORD(1, 1), &wsd);
	#endif

	cnt = 0;
	for(i = 1; i < ac && i < MAX_GROUP; i += 2){
		int fd = mcast_open(av[i], atoi(av[i+1]));
		if(fd < 0){
			printf("error of %d ip %s\n", errno, av[i]);
		}else{
			s_mcast[cnt].fd = fd;
			s_mcast[cnt].state = MCAST_STATE_READ;
			s_mcast[cnt].widx = -1;
			s_mcast[cnt].next = s_mcast + cnt+1;
			++cnt;
		}
	}
	
	if(cnt < 1){
		return -1;
	}
	s_mcast[cnt-1].next = NULL;


	while (1) {
		struct pollfd pe[MAX_GROUP] = {{0}};
		for(i = 0; i < cnt; ++i){
			pe[i].fd = s_mcast[i].fd;
			if(MCAST_STATE_WRITE == s_mcast[i].state){
				pe[i].events = POLLOUT;
			}else{
				pe[i].events = POLLIN;
			}
		}

		do{
			ret = poll(pe, cnt, 500);
			if(ret < 0 && errno != EAGAIN && errno != EINTR){
				printf("error of %d\n", errno);
				goto end;
			}
		}while(ret < 0);
	
		for(i = 0; i < cnt; ++i){	
			mcast_t *mc = s_mcast + i;
			if(pe[i].revents & POLLIN){
				mc->faddrlen = sizeof(mc->faddr);
				uint8_t *msgbuf = malloc(MSGBUFSIZE);
				if(!msgbuf){
					printf("malloc for read err\n");
					goto end;
				}

				nbytes = recvfrom(pe[i].fd, msgbuf, MSGBUFSIZE, 0, (struct sockaddr *)&mc->faddr, &mc->faddrlen);
				if(nbytes < 0 && errno != EAGAIN && errno != EINTR) {
					printf("read error ret %d err %d\n", nbytes, errno);
					goto end;
				}

				if(nbytes > 0){
					parse_packet(mc, msgbuf, nbytes);
				}else{
					free(msgbuf);
				}
			}

			if(pe[i].revents & POLLOUT){
				if(mc->ref_w < mc->ref_count){
					int idx = mc->ref_idx[mc->ref_w];
					sendto(mc->fd, mc->pkt_data[idx], mc->pkt_size[idx], 0, (struct sockaddr *)&mc->faddr, mc->faddrlen);
					mc->ref_w++;
					
					if(mc->ref_w >= mc->ref_count){
						printf("sent %s\n", mcast_dump(mc));		
						mc->state = MCAST_STATE_READ;
					}
				}
			}
		}

	}

end:
	for(i = 0; i < cnt; ++i){
		if(s_mcast[i].fd > 0){
			closesocket(s_mcast[i].fd);
		}
	}
	#ifdef _WIN32
    WSACleanup();
	#endif

	return 0;
}

todo: 

考虑怎么动态指定重传地址。