--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rtp-tfo-trace.c	Sun Apr 02 05:36:35 2023 +0000
@@ -0,0 +1,338 @@
+/*
+ * This program reads a pcap file containing RTP packets of a PSTN call
+ * (PCMU or PCMA, 160 samples per RTP packet), flowing in one or both
+ * directions, and looks for TFO IS messages.
+ */
+
+#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 link_hdr_len, ethertype_offset;
+
+static struct onedir {
+	int		init_flag;
+	unsigned	last_seq;
+	unsigned	last_tstamp;
+	unsigned	stream_ssrc;
+	u_char		is_hunt_buf[320];
+	int		is_state;
+	unsigned	is_hunt_fill;
+	unsigned	is_offset;
+	unsigned	is_alignment;
+	unsigned	is_bit_count;
+	unsigned	is_rx_word;
+} rx_state, tx_state;
+
+static const u_char hdr_pattern[20] =	{0, 1, 0, 1, 0, 1, 1, 0, 1, 0,
+					 0, 1, 1, 0, 1, 0, 1, 0, 0, 1};
+
+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, st, dir_str)
+	u_char *rtp_hdr;
+	unsigned pkt_idx;
+	struct onedir *st;
+	char *dir_str;
+{
+	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 (st->init_flag) {
+		if (cur_ssrc != st->stream_ssrc) {
+			printf(
+		"error in %s packet #%u: SSRC change from 0x%08X to 0x%08X\n",
+				dir_str, pkt_idx, st->stream_ssrc, cur_ssrc);
+		} else if (cur_seq != st->last_seq + 1 &&
+			   (cur_seq != 0 || st->last_seq != 0xFFFF)) {
+			printf(
+		"error in %s packet #%u: seq break from 0x%04X to 0x%04X\n",
+				dir_str, pkt_idx, st->last_seq, cur_seq);
+		} else if (cur_tstamp != st->last_tstamp + 160) {
+			printf(
+	      "error in %s packet #%u: timestamp break from 0x%08X to 0x%08X\n",
+				dir_str, pkt_idx, st->last_tstamp, cur_tstamp);
+		}
+	} else
+		st->init_flag = 1;
+	st->last_seq = cur_seq;
+	st->last_tstamp = cur_tstamp;
+	st->stream_ssrc = cur_ssrc;
+}
+
+static void
+is_rx_hunt(input_pos, pkt_idx, st, dir_str)
+	unsigned input_pos;
+	unsigned pkt_idx;
+	struct onedir *st;
+	char *dir_str;
+{
+	unsigned offset, n;
+
+	for (offset = 0; offset < 16; offset++) {
+		for (n = 0; n < 20; n++)
+			if ((st->is_hunt_buf[offset + n*16] & 1) !=
+			    hdr_pattern[n])
+				break;
+		if (n == 20)
+			break;
+	}
+	if (n != 20)
+		return;
+	st->is_offset = offset;
+	st->is_alignment = input_pos * 16 + offset;
+	st->is_state = 1;
+	st->is_bit_count = 0;
+	st->is_rx_word = 0;
+	st->is_hunt_fill = 0;
+}
+
+static void
+is_process_cmd(pkt_idx, st, dir_str)
+	unsigned pkt_idx;
+	struct onedir *st;
+	char *dir_str;
+{
+	int cont;
+
+	printf("#%u: %s (align %u) ", pkt_idx, dir_str, st->is_alignment);
+	switch (st->is_rx_word) {
+	case 0x05D:
+		printf("IS_REQ\n");
+		cont = 1;
+		break;
+	case 0x0BA:
+		printf("IS_ACK\n");
+		cont = 1;
+		break;
+	case 0x0E7:
+		printf("IS_IPE\n");
+		cont = 1;
+		break;
+	case 0x129:
+		printf("IS_FILL\n");
+		cont = 0;
+		break;
+	case 0x174:
+		printf("IS_DUP\n");
+		cont = 0;
+		break;
+	case 0x193:
+		printf("IS_SYL\n");
+		cont = 0;
+		break;
+	default:
+		printf("Unknown IS_Command 0x%03X\n", st->is_rx_word);
+		cont = 0;
+	}
+	if (cont) {
+		st->is_state = 2;
+		st->is_bit_count = 0;
+		st->is_rx_word = 0;
+	} else
+		st->is_state = 0;
+}
+
+static void
+is_process_ext(pkt_idx, st, dir_str)
+	unsigned pkt_idx;
+	struct onedir *st;
+	char *dir_str;
+{
+	printf("#%u: %s IS_Extension: 0x%05X", pkt_idx, dir_str,
+		st->is_rx_word);
+	if (st->is_rx_word & 0x80200) {
+		printf(" (bad sync)\n");
+		st->is_state = 0;
+		return;
+	}
+	switch (st->is_rx_word & 3) {
+	case 0:
+		printf(" (final)\n");
+		st->is_state = 0;
+		return;
+	case 3:
+		printf(" (continue)\n");
+		st->is_state = 2;
+		st->is_bit_count = 0;
+		st->is_rx_word = 0;
+		return;
+	default:
+		printf(" (bad EX)\n");
+		st->is_state = 0;
+	}
+}
+
+static void
+is_rx_process(input, input_pos, pkt_idx, st, dir_str)
+	uint8_t *input;
+	unsigned input_pos;
+	unsigned pkt_idx;
+	struct onedir *st;
+	char *dir_str;
+{
+	unsigned new_bit;
+
+	memmove(st->is_hunt_buf, st->is_hunt_buf + 16, 304);
+	memcpy(st->is_hunt_buf + 304, input, 16);
+	if (!st->is_state) {
+		if (st->is_hunt_fill < 20)
+			st->is_hunt_fill++;
+		if (st->is_hunt_fill == 20)
+			is_rx_hunt(input_pos, pkt_idx, st, dir_str);
+		return;
+	}
+	new_bit = input[st->is_offset] & 1;
+	st->is_rx_word <<= 1;
+	st->is_rx_word |= new_bit;
+	st->is_bit_count++;
+	if (st->is_state == 1 && st->is_bit_count == 10)
+		is_process_cmd(pkt_idx, st, dir_str);
+	else if (st->is_state == 2 && st->is_bit_count == 20)
+		is_process_ext(pkt_idx, st, dir_str);
+}
+
+static void
+process_packet_onedir(pkt, caplen, pkt_idx, st, dir_str)
+	u_char *pkt;
+	unsigned caplen, pkt_idx;
+	struct onedir *st;
+	char *dir_str;
+{
+	unsigned udplen, payload_len;
+	unsigned is_chunk;
+
+	udplen = (pkt[24] << 8) | pkt[25];
+	if (caplen < udplen + 20) {
+		printf("error: %s packet #%u is truncated in the capture\n",
+			dir_str, pkt_idx);
+		return;
+	}
+	if (udplen < 20) {
+		printf(
+	    "error in %s packet #%u: UDP length is too short for RTP header\n",
+			dir_str, pkt_idx);
+		return;
+	}
+	if (pkt[28] != 0x80) {
+		printf(
+		"error in %s packet #%u: unsupported RTP header structure\n",
+			dir_str, pkt_idx);
+		return;
+	}
+	rtp_stream_logic(pkt + 28, pkt_idx, st, dir_str);
+	payload_len = udplen - 20;
+	if (payload_len != 160) {
+		printf("error in %s packet #%u: wrong payload length\n",
+			dir_str, pkt_idx);
+		return;
+	}
+	for (is_chunk = 0; is_chunk < 10; is_chunk++)
+		is_rx_process(pkt + 40 + is_chunk * 16, is_chunk, pkt_idx, st,
+			      dir_str);
+}
+
+static void
+process_packet(pkt, caplen, pkt_idx)
+	u_char *pkt;
+	unsigned caplen, pkt_idx;
+{
+	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 + 12, &match_ip_addr, 4) &&
+	    !bcmp(pkt + 20, &match_udp_port, 2))
+		process_packet_onedir(pkt, caplen, pkt_idx, &tx_state, "-->");
+	else if (!bcmp(pkt + 16, &match_ip_addr, 4) &&
+		 !bcmp(pkt + 22, &match_udp_port, 2))
+		process_packet_onedir(pkt, caplen, pkt_idx, &rx_state, "<--");
+}
+
+main(argc, argv)
+	char **argv;
+{
+	char errbuf[PCAP_ERRBUF_SIZE];
+	u_char *pkt;
+	struct pcap_pkthdr pkthdr;
+	unsigned pkt_idx;
+
+	if (argc != 4) {
+		fprintf(stderr, "usage: %s pcap-file ip-addr udp-port\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();
+	match_ip_addr = inet_addr(argv[2]);
+	if (match_ip_addr == INADDR_NONE) {
+		fprintf(stderr, "error: IP address argument is invalid\n");
+		exit(1);
+	}
+	match_udp_port = htons(strtoul(argv[3], 0, 0));
+	for (pkt_idx = 0; ; pkt_idx++) {
+		pkt = pcap_next(pcap, &pkthdr);
+		if (!pkt)
+			break;
+		process_packet(pkt, (unsigned) pkthdr.caplen, pkt_idx);
+	}
+	if (!tx_state.init_flag)
+		printf(
+	"Warning: found no packets with src matching specified IP:port\n");
+	if (!rx_state.init_flag)
+		printf(
+	"Warning: found no packets with dest matching specified IP:port\n");
+	exit(0);
+}