/*
 * This program is an aid for analyzing arbitrary RTP streams:
 * it reads a pcap file, extracts packets belonging to a particular
 * RTP stream as identified by a source or destination IP:port,
 * and displays a one-line summary of every RTP packet.
 * Discontinuities are treated as non-fatal errors: an error message
 * is printed, but analysis continues.
 */

#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <strings.h>
#include <pcap/pcap.h>

static pcap_t *pcap;
static in_addr_t match_ip_addr;
static u_short match_udp_port;
static unsigned iphdr_addr_offset, udphdr_port_offset;
static unsigned link_hdr_len, ethertype_offset;
static int stream_init_flag, toad_init_flag;
static unsigned last_seq, last_tstamp, stream_ssrc;
static struct timeval last_pkt_time;
static int do_hexdump;

static void
check_dl_type()
{
	int dltype;

	dltype = pcap_datalink(pcap);
	switch (dltype) {
	case DLT_EN10MB:
		link_hdr_len = 14;
		ethertype_offset = 12;
		break;
	case DLT_RAW:
		link_hdr_len = 0;
		break;
	case DLT_LINUX_SLL:
		link_hdr_len = 16;
		ethertype_offset = 14;
		break;
	default:
		fprintf(stderr, "error: unsupported data link type %d\n",
			dltype);
		exit(1);
	}
}

static void
rtp_stream_logic(rtp_hdr, pkt_idx)
	u_char *rtp_hdr;
	unsigned pkt_idx;
{
	unsigned cur_seq, cur_tstamp, cur_ssrc;

	cur_seq = (rtp_hdr[2] << 8) | rtp_hdr[3];
	cur_tstamp = (rtp_hdr[4] << 24) | (rtp_hdr[5] << 16) |
			(rtp_hdr[6] << 8) | rtp_hdr[7];
	cur_ssrc = (rtp_hdr[8] << 24) | (rtp_hdr[9] << 16) |
			(rtp_hdr[10] << 8) | rtp_hdr[11];
	if (stream_init_flag) {
		if (cur_ssrc != stream_ssrc) {
			printf(
		"error in packet #%u: SSRC change from 0x%08X to 0x%08X\n",
				pkt_idx, stream_ssrc, cur_ssrc);
		} else if (cur_seq != last_seq + 1 &&
			   (cur_seq != 0 || last_seq != 0xFFFF)) {
			printf(
		"error in packet #%u: seq break from 0x%04X to 0x%04X\n",
				pkt_idx, last_seq, cur_seq);
		} else if (cur_tstamp != last_tstamp + 160) {
			printf(
		"error in packet #%u: timestamp break from 0x%08X to 0x%08X\n",
				pkt_idx, last_tstamp, cur_tstamp);
		}
	} else
		stream_init_flag = 1;
	last_seq = cur_seq;
	last_tstamp = cur_tstamp;
	stream_ssrc = cur_ssrc;
}

static void
hexdump_payload(payload, payload_len)
	u_char *payload;
	unsigned payload_len;
{
	unsigned off;

	for (off = 0; off < payload_len; off++) {
		if ((off & 3) == 0)
			putchar(' ');
		printf(" %02X", payload[off]);
		if ((off & 15) == 15 || off == payload_len - 1)
			putchar('\n');
	}
}

static void
process_packet(pkt, caplen, pkt_idx, pkt_time)
	u_char *pkt;
	unsigned caplen, pkt_idx;
	struct timeval *pkt_time;
{
	unsigned udplen, payload_len;
	struct timeval toad;
	unsigned toad_ms, toad_us;

	if (caplen < link_hdr_len + 28)
		return;
	if (link_hdr_len) {
		if (pkt[ethertype_offset] != 0x08)
			return;
		if (pkt[ethertype_offset+1] != 0x00)
			return;
		pkt += link_hdr_len;
		caplen -= link_hdr_len;
	}
	/* check IP header */
	if (pkt[0] != 0x45)
		return;
	if (pkt[9] != 17)	/* UDP */
		return;
	if (bcmp(pkt + iphdr_addr_offset, &match_ip_addr, 4))
		return;
	/* check UDP header */
	if (bcmp(pkt + 20 + udphdr_port_offset, &match_udp_port, 2))
		return;
	/* it is our target - now scrutinize it */
	udplen = (pkt[24] << 8) | pkt[25];
	if (caplen < udplen + 20) {
		fprintf(stderr,
			"error: packet #%u is truncated in the capture\n",
			pkt_idx);
		exit(1);
	}
	if (udplen < 20) {
		fprintf(stderr,
		"error in packet #%u: UDP length is too short for RTP header\n",
			pkt_idx);
		exit(1);
	}
	if (pkt[28] != 0x80) {
		fprintf(stderr,
		"error in packet #%u: unsupported RTP header structure\n",
			pkt_idx);
		exit(1);
	}
	rtp_stream_logic(pkt + 28, pkt_idx);
	payload_len = udplen - 20;
	printf("pkt #%u seq 0x%04X ts 0x%08X: pt=%u M=%u len=%u", pkt_idx,
		last_seq, last_tstamp, pkt[29] & 0x7F, pkt[29] >> 7,
		payload_len);
	if (toad_init_flag && timercmp(pkt_time, &last_pkt_time, >=)) {
		timersub(pkt_time, &last_pkt_time, &toad);
		if (toad.tv_sec)
			printf(", toad %u.%06u s", (unsigned) toad.tv_sec,
				(unsigned) toad.tv_usec);
		else {
			toad_ms = toad.tv_usec / 1000;
			toad_us = toad.tv_usec % 1000;
			printf(", toad %u.%03u ms", toad_ms, toad_us);
		}
	}
	putchar('\n');
	if (do_hexdump)
		hexdump_payload(pkt + 40, payload_len);
	bcopy(pkt_time, &last_pkt_time, sizeof(struct timeval));
	toad_init_flag = 1;
}

main(argc, argv)
	char **argv;
{
	char errbuf[PCAP_ERRBUF_SIZE];
	u_char *pkt;
	struct pcap_pkthdr pkthdr;
	unsigned pkt_idx;

	switch (argc) {
	case 5:
		do_hexdump = 0;
		break;
	case 6:
		if (strcmp(argv[5], "hex"))
			goto usage;
		do_hexdump = 1;
		break;
	default:
	usage:
		fprintf(stderr,
			"usage: %s pcap-file src|dest ip-addr udp-port [hex]\n",
			argv[0]);
		exit(1);
	}
	pcap = pcap_open_offline(argv[1], errbuf);
	if (!pcap) {
		fprintf(stderr, "%s: %s\n", argv[1], errbuf);
		exit(1);
	}
	check_dl_type();
	if (!strcmp(argv[2], "src")) {
		iphdr_addr_offset = 12;
		udphdr_port_offset = 0;
	} else if (!strcmp(argv[2], "dest")) {
		iphdr_addr_offset = 16;
		udphdr_port_offset = 2;
	} else {
		fprintf(stderr,
		"error: direction argument must be \"src\" or \"dest\"\n");
		exit(1);
	}
	match_ip_addr = inet_addr(argv[3]);
	if (match_ip_addr == INADDR_NONE) {
		fprintf(stderr, "error: IP address argument is invalid\n");
		exit(1);
	}
	match_udp_port = htons(strtoul(argv[4], 0, 0));
	for (pkt_idx = 0; ; pkt_idx++) {
		pkt = pcap_next(pcap, &pkthdr);
		if (!pkt)
			break;
		process_packet(pkt, (unsigned) pkthdr.caplen, pkt_idx,
				&pkthdr.ts);
	}
	if (!stream_init_flag) {
		fprintf(stderr, "error: specified RTP stream not found\n");
		exit(1);
	}
	exit(0);
}