add RTP support

This patch adds the initial RTP support for libosmo-netif, it's based
on Harald's RTP support available in openBSC.

I have also added a couple of example to show how our new channel
infrastructure interacts with the RTP layer.

Signed-off-by: Pablo Neira Ayuso <pablo@gnumonks.org>

8 files changed, 561 insertions, 1 deletions

diff --git a/configure.ac b/configure.ac
index 692688a..4cfda5c 100644
--- a/configure.ac
+++ b/configure.ac
@@ -31,6 +31,8 @@ AC_COMPILE_IFELSE([AC_LANG_SOURCE([char foo;])],
 CFLAGS="$saved_CFLAGS"
 AC_SUBST(SYMBOL_VISIBILITY)
 
+AC_CHECK_HEADER([endian.h], [], [AC_MSG_ERROR([endian.h not found!]) ])
+
 dnl Generate the output
 AM_CONFIG_HEADER(config.h)
 
diff --git a/examples/Makefile.am b/examples/Makefile.am
index 173b620..c9849db 100644
--- a/examples/Makefile.am
+++ b/examples/Makefile.am
@@ -9,7 +9,9 @@ noinst_PROGRAMS = ipa-stream-client		\
 		  lapd-over-datagram-user	\
 		  lapd-over-datagram-network	\
 		  stream-client			\
-		  stream-server
+		  stream-server			\
+		  rtp-udp-test-client		\
+		  rtp-udp-test-server
 
 ipa_stream_client_SOURCES = ipa-stream-client.c
 ipa_stream_client_LDADD = $(top_builddir)/src/libosmonetif.la \
@@ -36,3 +38,11 @@ stream_client_LDADD = $(top_builddir)/src/libosmonetif.la \
 stream_server_SOURCES = stream-server.c
 stream_server_LDADD = $(top_builddir)/src/libosmonetif.la \
 		      $(LIBOSMOCORE_LIBS) $(LIBOSMOGSM_LIBS)
+
+rtp_udp_test_client_SOURCES = rtp-udp-test-client.c
+rtp_udp_test_client_LDADD = $(top_builddir)/src/libosmonetif.la \
+			    $(LIBOSMOCORE_LIBS) $(LIBOSMOGSM_LIBS)
+
+rtp_udp_test_server_SOURCES = rtp-udp-test-server.c
+rtp_udp_test_server_LDADD = $(top_builddir)/src/libosmonetif.la \
+			    $(LIBOSMOCORE_LIBS) $(LIBOSMOGSM_LIBS)
diff --git a/examples/rtp-udp-test-client.c b/examples/rtp-udp-test-client.c
new file mode 100644
index 0000000..4989a6c
--- /dev/null
+++ b/examples/rtp-udp-test-client.c
@@ -0,0 +1,143 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <signal.h>
+#include <unistd.h>
+#include <arpa/inet.h>
+#include <time.h>
+
+#include <osmocom/core/linuxlist.h>
+#include <osmocom/core/talloc.h>
+#include <osmocom/core/msgb.h>
+#include <osmocom/core/logging.h>
+#include <osmocom/core/application.h>
+#include <osmocom/core/select.h>
+
+#include <osmocom/netif/rtp.h>
+#include <osmocom/netif/datagram.h>
+
+#define DRTP_TEST 0
+
+struct log_info_cat rtp_test_cat[] = {
+	[DRTP_TEST] = {
+		.name = "DRTP_TEST",
+		.description = "RTP client test",
+		.color = "\033[1;35m",
+		.enabled = 1, .loglevel = LOGL_DEBUG,
+	},
+};
+
+const struct log_info rtp_test_log_info = {
+	.filter_fn = NULL,
+	.cat = rtp_test_cat,
+	.num_cat = ARRAY_SIZE(rtp_test_cat),
+};
+
+static struct osmo_dgram_conn *conn;
+static struct osmo_rtp_handle *rtp;
+
+static int read_cb(struct osmo_dgram_conn *conn)
+{
+	struct msgb *msg;
+	int payload_type;
+
+	LOGP(DLINP, LOGL_DEBUG, "received message\n");
+
+	msg = msgb_alloc(RTP_MSGB_SIZE, "RTP/test");
+	if (msg == NULL) {
+		LOGP(DRTP_TEST, LOGL_ERROR, "cannot allocate message\n");
+		return -1;
+	}
+	if (osmo_dgram_conn_recv(conn, msg) < 0) {
+		msgb_free(msg);
+		LOGP(DRTP_TEST, LOGL_ERROR, "cannot receive message\n");
+		return -1;
+	}
+	payload_type = osmo_rtp_parse(rtp, msg);
+	if (payload_type < 0) {
+		msgb_free(msg);
+		LOGP(DRTP_TEST, LOGL_ERROR, "cannot parse RTP message\n");
+		return -1;
+	}
+
+	LOGP(DLINP, LOGL_DEBUG, "received message with RTP payload type: %d\n",
+		payload_type);
+
+	msgb_free(msg);
+	return 0;
+}
+
+void sighandler(int foo)
+{
+	LOGP(DLINP, LOGL_NOTICE, "closing RTP.\n");
+	osmo_dgram_conn_close(conn);
+	osmo_dgram_conn_destroy(conn);
+	osmo_rtp_handle_free(rtp);
+	exit(EXIT_SUCCESS);
+}
+
+static void *tall_test;
+
+int main(int argc, char *argv[])
+{
+	int i;
+	char dummy_data[RTP_PT_GSM_FULL_PAYLOAD_LEN] = {};
+
+	signal(SIGINT, sighandler);
+
+	tall_test = talloc_named_const(NULL, 1, "rtp_test");
+
+	osmo_init_logging(&rtp_test_log_info);
+	log_set_log_level(osmo_stderr_target, LOGL_DEBUG);
+
+	/*
+	 * initialize RTP stuff.
+	 */
+	rtp = osmo_rtp_handle_create(tall_test);
+	if (rtp == NULL) {
+		LOGP(DLINP, LOGL_ERROR, "creating RTP handler\n");
+		exit(EXIT_FAILURE);
+	}
+	osmo_rtp_handle_tx_set_sequence(rtp, random());
+	osmo_rtp_handle_tx_set_ssrc(rtp, random());
+	osmo_rtp_handle_tx_set_timestamp(rtp, time(NULL));
+
+	/*
+	 * initialize datagram socket.
+	 */
+
+	conn = osmo_dgram_conn_create(tall_test);
+	if (conn == NULL) {
+		fprintf(stderr, "cannot create client\n");
+		exit(EXIT_FAILURE);
+	}
+	osmo_dgram_conn_set_local_addr(conn, "127.0.0.1");
+	osmo_dgram_conn_set_local_port(conn, 20001);
+	osmo_dgram_conn_set_remote_addr(conn, "127.0.0.1");
+	osmo_dgram_conn_set_remote_port(conn, 20000);
+	osmo_dgram_conn_set_read_cb(conn, read_cb);
+
+	if (osmo_dgram_conn_open(conn) < 0) {
+		fprintf(stderr, "cannot open client\n");
+		exit(EXIT_FAILURE);
+	}
+
+	for(i=0; i<10; i++) {
+		struct msgb *msg;
+
+		msg = osmo_rtp_build(rtp, RTP_PT_GSM_FULL,
+				     RTP_PT_GSM_FULL_PAYLOAD_LEN,
+				     dummy_data, RTP_PT_GSM_FULL_DURATION);
+		if (msg == NULL) {
+			LOGP(DLINP, LOGL_ERROR, "OOM\n");
+			continue;
+		}
+		osmo_dgram_conn_send(conn, msg);
+	}
+
+	LOGP(DLINP, LOGL_NOTICE, "Entering main loop\n");
+
+	while(1) {
+		osmo_select_main(0);
+	}
+}
diff --git a/examples/rtp-udp-test-server.c b/examples/rtp-udp-test-server.c
new file mode 100644
index 0000000..f1920b8
--- /dev/null
+++ b/examples/rtp-udp-test-server.c
@@ -0,0 +1,145 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <time.h>
+#include <signal.h>
+#include <unistd.h>
+
+#include <osmocom/core/talloc.h>
+#include <osmocom/core/msgb.h>
+#include <osmocom/core/logging.h>
+#include <osmocom/core/application.h>
+#include <osmocom/core/select.h>
+
+#include <osmocom/netif/rtp.h>
+#include <osmocom/netif/datagram.h>
+
+#define DRTP_TEST 0
+
+struct log_info_cat rtp_test_cat[] = {
+	[DRTP_TEST] = {
+		.name = "DRTP_TEST",
+		.description = "RPT-server test",
+		.color = "\033[1;35m",
+		.enabled = 1, .loglevel = LOGL_DEBUG,
+	},
+};
+
+const struct log_info rtp_test_log_info = {
+	.filter_fn = NULL,
+	.cat = rtp_test_cat,
+	.num_cat = ARRAY_SIZE(rtp_test_cat),
+};
+
+static struct osmo_dgram_conn *conn;
+static struct osmo_rtp_handle *rtp;
+
+int read_cb(struct osmo_dgram_conn *conn)
+{
+	struct msgb *msg;
+	char dummy_data[RTP_PT_GSM_FULL_PAYLOAD_LEN] = {};
+	int payload_type;
+
+	LOGP(DRTP_TEST, LOGL_DEBUG, "received message from datagram\n");
+
+	msg = msgb_alloc(RTP_MSGB_SIZE, "RTP/test");
+	if (msg == NULL) {
+		LOGP(DRTP_TEST, LOGL_ERROR, "cannot allocate message\n");
+		return -1;
+	}
+	if (osmo_dgram_conn_recv(conn, msg) < 0) {
+		LOGP(DRTP_TEST, LOGL_ERROR, "cannot receive message\n");
+		return -1;
+	}
+	payload_type = osmo_rtp_parse(rtp, msg);
+	if (payload_type < 0) {
+		msgb_free(msg);
+		LOGP(DRTP_TEST, LOGL_ERROR, "cannot parse RTP message\n");
+		return -1;
+	}
+	LOGP(DLINP, LOGL_DEBUG, "received message with payload type: %d\n",
+		payload_type);
+
+	/*
+	 * ... now build gsm_data_frame, set callref and msg_type based
+	 * on the rtp payload type (map RTP_PT_GSM_FULL to GSM_THCF_FRAME).
+	 * Then, pass it to the RSL layer.
+	 */
+
+	msg = msgb_alloc(1200, "RTP/test");
+	if (msg == NULL) {
+		LOGP(DRTP_TEST, LOGL_ERROR, "cannot allocate message\n");
+		return -1;
+	}
+
+	/* build reply. */
+	msg = osmo_rtp_build(rtp, RTP_PT_GSM_FULL,
+			     RTP_PT_GSM_FULL_PAYLOAD_LEN,
+			     dummy_data, RTP_PT_GSM_FULL_DURATION);
+	if (msg == NULL) {
+		LOGP(DLINP, LOGL_ERROR, "OOM\n");
+		return -1;
+	}
+	osmo_dgram_conn_send(conn, msg);
+
+	return 0;
+}
+
+void sighandler(int foo)
+{
+	LOGP(DLINP, LOGL_NOTICE, "closing RTP.\n");
+	osmo_dgram_conn_close(conn);
+	osmo_dgram_conn_destroy(conn);
+	osmo_rtp_handle_free(rtp);
+	exit(EXIT_SUCCESS);
+}
+
+static void *tall_test;
+
+int main(int argc, char *argv[])
+{
+	signal(SIGINT, sighandler);
+
+	tall_test = talloc_named_const(NULL, 1, "udp_rtp_test");
+
+	osmo_init_logging(&rtp_test_log_info);
+	log_set_log_level(osmo_stderr_target, LOGL_DEBUG);
+
+	/*
+	 * initialize RTP handler.
+	 */
+	rtp = osmo_rtp_handle_create(tall_test);
+	if (rtp == NULL) {
+		LOGP(DRTP_TEST, LOGL_ERROR, "Error init RTP handler\n");
+		exit(EXIT_FAILURE);
+	}
+	osmo_rtp_handle_tx_set_sequence(rtp, random());
+	osmo_rtp_handle_tx_set_ssrc(rtp, random());
+	osmo_rtp_handle_tx_set_timestamp(rtp, time(NULL));
+
+	/*
+	 * initialize datagram server.
+	 */
+
+	conn = osmo_dgram_conn_create(tall_test);
+	if (conn == NULL) {
+		LOGP(DRTP_TEST, LOGL_ERROR, "cannot create UDP socket\n");
+		exit(EXIT_FAILURE);
+	}
+	osmo_dgram_conn_set_local_addr(conn, "127.0.0.1");
+	osmo_dgram_conn_set_local_port(conn, 20000);
+	osmo_dgram_conn_set_remote_addr(conn, "127.0.0.1");
+	osmo_dgram_conn_set_remote_port(conn, 20001);
+	osmo_dgram_conn_set_read_cb(conn, read_cb);
+
+	if (osmo_dgram_conn_open(conn) < 0) {
+		fprintf(stderr, "cannot open client\n");
+		exit(EXIT_FAILURE);
+	}
+
+	LOGP(DRTP_TEST, LOGL_NOTICE, "Entering main loop\n");
+
+	while(1) {
+		osmo_select_main(0);
+	}
+}
diff --git a/include/osmocom/netif/Makefile.am b/include/osmocom/netif/Makefile.am
index 6d2980d..4fe54bb 100644
--- a/include/osmocom/netif/Makefile.am
+++ b/include/osmocom/netif/Makefile.am
@@ -3,6 +3,7 @@ SUBDIRS = channel
 osmonetif_HEADERS = channel.h		\
 		    datagram.h		\
 		    ipa.h		\
+		    rtp.h		\
 		    stream.h
 
 osmonetifdir = $(includedir)/osmocom/netif
diff --git a/include/osmocom/netif/rtp.h b/include/osmocom/netif/rtp.h
new file mode 100644
index 0000000..7a8cc6b
--- /dev/null
+++ b/include/osmocom/netif/rtp.h
@@ -0,0 +1,27 @@
+#ifndef _OSMO_RTP_H_
+#define _OSMO_RTP_H_
+
+/* XXX: RFC specifies that MTU should used, add generic function to obtain
+	existing MTU. */
+#define RTP_MSGB_SIZE  1500
+
+struct osmo_rtp_handle *osmo_rtp_handle_create(void *ctx);
+void osmo_rtp_handle_free(struct osmo_rtp_handle *h);
+
+int osmo_rtp_handle_tx_set_sequence(struct osmo_rtp_handle *h, uint16_t seq);
+int osmo_rtp_handle_tx_set_ssrc(struct osmo_rtp_handle *h, uint32_t ssrc);
+int osmo_rtp_handle_tx_set_timestamp(struct osmo_rtp_handle *h, uint32_t timestamp);
+
+int osmo_rtp_parse(struct osmo_rtp_handle *h, struct msgb *msg);
+struct msgb *osmo_rtp_build(struct osmo_rtp_handle *h, uint8_t payload_type, uint32_t payload_len, const void *data, uint32_t duration);
+
+/* supported RTP payload types. */
+#define RTP_PT_GSM_FULL			3
+#define RTP_PT_GSM_FULL_PAYLOAD_LEN	33
+#define RTP_PT_GSM_FULL_DURATION	160	/* in samples. */
+
+#define RTP_PT_GSM_EFR			97
+#define RTP_PT_GSM_EFR_PAYLOAD_LEN	31
+#define RTP_PT_GSM_EFR_DURATION		160	/* in samples. */
+
+#endif
diff --git a/src/Makefile.am b/src/Makefile.am
index 06def6c..f29c40b 100644
--- a/src/Makefile.am
+++ b/src/Makefile.am
@@ -15,4 +15,5 @@ libosmonetif_la_LIBADD = channel/libosmonetif-channel.la
 libosmonetif_la_SOURCES = channel.c		\
 			  datagram.c		\
 			  ipa.c			\
+			  rtp.c			\
 			  stream.c
diff --git a/src/rtp.c b/src/rtp.c
new file mode 100644
index 0000000..a3afbc4
--- /dev/null
+++ b/src/rtp.c
@@ -0,0 +1,231 @@
+#include <stdint.h>
+#include <sys/time.h>
+#include <endian.h>
+#include <errno.h>
+#include <string.h>	/* for memcpy. */
+#include <arpa/inet.h>	/* for ntohs. */
+
+#include <osmocom/core/talloc.h>
+#include <osmocom/core/msgb.h>
+#include <osmocom/core/logging.h>
+
+#include <osmocom/netif/rtp.h>
+
+/*
+ * Definitions in RFC 3550.
+ */
+
+/* RTP header. */
+struct rtp_hdr {
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+	uint8_t  csrc_count:4,
+		 extension:1,
+		 padding:1,
+		 version:2;
+	uint8_t  payload_type:7,
+		 marker:1;
+#elif __BYTE_ORDER == __BIG_ENDIAN
+	uint8_t  version:2,
+		 padding:1,
+		 extension:1,
+		 csrc_count:4;
+	uint8_t  marker:1,
+		 payload_type:7;
+#endif
+	uint16_t sequence;
+	uint32_t timestamp;
+	uint32_t ssrc;
+} __attribute__((packed));
+
+#define RTP_VERSION	2
+
+/* 5.3.1 RTP Header Extension
+ *
+ * If the X bit in the RTP header is one, a variable-length header
+ * extension MUST be appended to the RTP header, following the CSRC list
+ * if present. The header extension contains a 16-bit length field that
+ * counts the number of 32-bit words in the extension, excluding the
+ * four-octet extension header (therefore zero is a valid length).  Only
+ * a single extension can be appended to the RTP data header.
+ */
+struct rtp_x_hdr {
+	uint16_t by_profile;
+	uint16_t length;
+} __attribute__((packed));
+
+/* RTPC header. */
+struct rtcp_hdr {
+        uint8_t byte0;
+        uint8_t type;
+        uint16_t length;
+} __attribute__((packed));
+
+/*
+ * Internal definitions for this implementation.
+ */
+
+struct osmo_rtp_handle {
+	struct {
+		uint16_t		sequence;
+		uint32_t		timestamp;
+		uint32_t		ssrc;
+		struct timeval		last_tv;
+	} tx;
+};
+
+struct osmo_rtp_handle *osmo_rtp_handle_create(void *ctx)
+{
+	struct osmo_rtp_handle *h;
+
+	h = talloc_zero(ctx, struct osmo_rtp_handle);
+	if (h == NULL) {
+		LOGP(DLMUX, LOGL_ERROR, "OOM\n");
+		return NULL;
+	}
+	return h;
+}
+
+void osmo_rtp_handle_free(struct osmo_rtp_handle *h)
+{
+	DEBUGP(DLMUX, "%s (h=%p)\n", __FUNCTION__, h);
+
+	talloc_free(h);
+}
+
+int osmo_rtp_handle_tx_set_sequence(struct osmo_rtp_handle *h, uint16_t seq)
+{
+	DEBUGP(DLMUX, "%s (handle=%p, seq=%hu)\n", __FUNCTION__, h, seq);
+
+	h->tx.sequence = seq;
+
+	return 0;
+}
+
+int osmo_rtp_handle_tx_set_ssrc(struct osmo_rtp_handle *h, uint32_t ssrc)
+{
+	DEBUGP(DLMUX, "%s (handle=%p, seq=%hu)\n", __FUNCTION__, h, ssrc);
+
+	h->tx.ssrc = ssrc;
+	return 0;
+}
+
+int osmo_rtp_handle_tx_set_timestamp(struct osmo_rtp_handle *h, uint32_t timestamp)
+{
+	DEBUGP(DLMUX, "%s (handle=%p, ts=%hu)\n", __FUNCTION__, h, timestamp);
+
+	h->tx.timestamp = timestamp;
+	return 0;
+}
+
+/* decode and pull RTP header out and return payload_type. The msg->data
+   points to data payload after this is called. This function returns the
+   RTP payload type on success. */
+int osmo_rtp_parse(struct osmo_rtp_handle *h, struct msgb *msg)
+{
+	struct rtp_hdr *rtph = (struct rtp_hdr *)msg->data;
+	struct rtp_x_hdr *rtpxh;
+	uint8_t *payload;
+	int payload_len;
+	int x_len;
+	int csrc_len;
+
+	if (msg->len < sizeof(struct rtp_hdr)) {
+		DEBUGPC(DLMUX, "received RTP frame too short (len = %d)\n",
+			msg->len);
+		return -EINVAL;
+	}
+	if (rtph->version != RTP_VERSION) {
+		DEBUGPC(DLMUX, "received RTP version %d not supported.\n",
+			rtph->version);
+		return -EINVAL;
+	}
+	csrc_len = rtph->csrc_count << 2;
+	payload = msg->data + sizeof(struct rtp_hdr) + csrc_len;
+	payload_len = msg->len - sizeof(struct rtp_hdr) - csrc_len;
+	if (payload_len < 0) {
+		DEBUGPC(DLMUX, "received RTP frame too short (len = %d, "
+			"csrc count = %d)\n", msg->len, rtph->csrc_count);
+		return -EINVAL;
+	}
+	if (rtph->extension) {
+		if (payload_len < sizeof(struct rtp_x_hdr)) {
+			DEBUGPC(DLMUX, "received RTP frame too short for "
+				"extension header\n");
+			return -EINVAL;
+		}
+		rtpxh = (struct rtp_x_hdr *)payload;
+		x_len = ntohs(rtpxh->length) * 4 + sizeof(struct rtp_x_hdr);
+		payload += x_len;
+		payload_len -= x_len;
+		if (payload_len < 0) {
+			DEBUGPC(DLMUX, "received RTP frame too short, "
+				"extension header exceeds frame length\n");
+			return -EINVAL;
+		}
+	}
+	if (rtph->padding) {
+		if (payload_len < 0) {
+			DEBUGPC(DLMUX, "received RTP frame too short for "
+				"padding length\n");
+			return -EINVAL;
+		}
+		payload_len -= payload[payload_len - 1];
+		if (payload_len < 0) {
+			DEBUGPC(DLMUX, "received RTP frame with padding "
+				"greater than payload\n");
+			return -EINVAL;
+		}
+	}
+
+	msgb_pull(msg, msg->len - payload_len);
+
+	return rtph->payload_type;
+}
+
+struct msgb *
+osmo_rtp_build(struct osmo_rtp_handle *h, uint8_t payload_type,
+	       uint32_t payload_len, const void *data, uint32_t duration)
+{
+	struct msgb *msg;
+	struct rtp_hdr *rtph;
+	struct timeval tv, tv_diff = {};
+	long int usec_diff, frame_diff;
+
+	gettimeofday(&tv, NULL);
+	timersub(&tv_diff, &h->tx.last_tv, &tv);
+	h->tx.last_tv = tv;
+
+	usec_diff = tv_diff.tv_sec * 1000000 + tv_diff.tv_usec;
+	frame_diff = (usec_diff / 20000);
+
+	if (abs(frame_diff) > 1) {
+		long int frame_diff_excess = frame_diff - 1;
+
+		LOGP(DLMUX, LOGL_NOTICE,
+			"Correcting frame difference of %ld frames\n",
+			frame_diff_excess);
+		h->tx.sequence += frame_diff_excess;
+		h->tx.timestamp += frame_diff_excess * duration;
+	}
+
+	msg = msgb_alloc(sizeof(struct rtp_hdr) + payload_len, "RTP");
+	if (!msg) {
+		LOGP(DLMUX, LOGL_ERROR, "OOM\n");
+		return NULL;
+	}
+	rtph = (struct rtp_hdr *)msg->data;
+	rtph->version = RTP_VERSION;
+	rtph->padding = 0;
+	rtph->extension = 0;
+	rtph->csrc_count = 0;
+	rtph->marker = 0;
+	rtph->payload_type = payload_type;
+	rtph->sequence = htons(h->tx.sequence++);
+	rtph->timestamp = htonl(h->tx.timestamp);
+	h->tx.timestamp += duration;
+	rtph->ssrc = htonl(h->tx.ssrc);
+	memcpy(msg->data + sizeof(struct rtp_hdr), data, payload_len);
+	msgb_put(msg, sizeof(struct rtp_hdr) + payload_len);
+
+	return msg;
+}