MT#63317 kernel support for RTP MID

Change-Id: Ib3582649474fb7a926e47cb61d140bc389ef0d86

kernel-module/extmap_filter.inc.c

@@ -35,8 +35,60 @@ static bool apply_extmap_filter_ext(uint8_t id, size_t len, size_t size,
 	return true;
 }
 
+static unsigned char *insert_skb_data(unsigned char **w, struct sk_buff *skb, struct rtp_parsed *rtp,
+		unsigned int len)
+{
+	unsigned char *ret = *w;
+	unsigned char *end = ret + len;
+	if (end > rtp->payload) {
+		size_t put = end - rtp->payload;
+		skb_put(skb, put);
+		memmove(rtp->payload + put, rtp->payload, rtp->payload_len);
+		rtp->payload += put;
+	}
+	(*w) += len;
+	return ret;
+}
+
+static void add_extmap_ext_short(unsigned char **w, unsigned int *count,
+		struct sk_buff *skb, struct rtp_parsed *rtp, uint8_t id, uint8_t len, char *content)
+{
+	unsigned char *wp = insert_skb_data(w, skb, rtp, 1 + len);
+	*wp++ = (id << 4) | ((len - 1) & 0xf);
+	memcpy(wp, content, len);
+
+	(*count)++;
+}
+static void add_extmap_ext_long(unsigned char **w, unsigned int *count,
+		struct sk_buff *skb, struct rtp_parsed *rtp, uint8_t id, uint8_t len, char *content)
+{
+	unsigned char *wp = insert_skb_data(w, skb, rtp, 2 + len);
+	*wp++ = id;
+	*wp++ = len;
+	memcpy(wp, content, len);
+
+	(*count)++;
+}
+
+static void add_extmap_exts_short(unsigned char **w, unsigned int *count,
+		struct rtpengine_output *o, struct rtp_parsed *rtp, struct sk_buff *skb)
+{
+	if (!o->output.extmap_mid)
+		return;
+	add_extmap_ext_short(w, count, skb, rtp, o->output.extmap_mid, o->output.extmap_mid_len,
+			o->output.extmap_mid_str);
+}
+static void add_extmap_exts_long(unsigned char **w, unsigned int *count,
+		struct rtpengine_output *o, struct rtp_parsed *rtp, struct sk_buff *skb)
+{
+	if (!o->output.extmap_mid)
+		return;
+	add_extmap_ext_long(w, count, skb, rtp, o->output.extmap_mid, o->output.extmap_mid_len,
+			o->output.extmap_mid_str);
+}
+
 static void apply_extmap_filter_finish(unsigned char *r, unsigned char *w, unsigned int count,
-		struct sk_buff *skb, struct rtp_parsed *rtp)
+		struct rtpengine_output *o, struct sk_buff *skb, struct rtp_parsed *rtp)
 {
 	if (r == w)
 		return; // everything left as it was
@@ -53,16 +105,28 @@ static void apply_extmap_filter_finish(unsigned char *r, unsigned char *w, unsig
 		return;
 	}
 
-	// shift up payload and trim packet
+	// shift payload and adjust packet length
+	rtp->rtp_header->v_p_x_cc |= 0x90;
 	size_t size = w - rtp->extension;
 	size_t padded = (size + 3L) & ~3L;
-	memset(w, 0, padded - size);
 	rtp->ext_hdr->length = htons(padded / 4);
-	size_t pull = rtp->extension_len - padded;
-	rtp->extension_len = padded;
-	memmove(rtp->payload - pull, rtp->payload, rtp->payload_len);
-	rtp->payload -= pull;
-	skb_trim(skb, skb->len - pull);
+
+	if (rtp->extension_len >= padded) {
+		// shift up payload and trim packet
+		memset(w, 0, padded - size);
+		size_t pull = rtp->extension_len - padded;
+		rtp->extension_len = padded;
+		memmove(rtp->payload - pull, rtp->payload, rtp->payload_len);
+		rtp->payload -= pull;
+		skb_trim(skb, skb->len - pull);
+	}
+	else {
+		// shift down payload and extend packet for padding
+		size_t put = padded - size;
+		unsigned char *wp = insert_skb_data(&w, skb, rtp, put);
+		memset(wp, 0, put);
+	}
+
 }
 
 static void apply_extmap_filter_short(struct sk_buff *skb, struct rtpengine_output *o, struct rtp_parsed *rtp) {
@@ -87,7 +151,9 @@ static void apply_extmap_filter_short(struct sk_buff *skb, struct rtpengine_outp
 			break;
 	}
 
-	apply_extmap_filter_finish(r, w, count, skb, rtp);
+	add_extmap_exts_short(&w, &count, o, rtp, skb);
+
+	apply_extmap_filter_finish(r, w, count, o, skb, rtp);
 }
 
 static void apply_extmap_filter_long(struct sk_buff *skb, struct rtpengine_output *o, struct rtp_parsed *rtp) {
@@ -111,18 +177,70 @@ static void apply_extmap_filter_long(struct sk_buff *skb, struct rtpengine_outpu
 			break;
 	}
 
-	apply_extmap_filter_finish(r, w, count, skb, rtp);
+	add_extmap_exts_long(&w, &count, o, rtp, skb);
+
+	apply_extmap_filter_finish(r, w, count, o, skb, rtp);
 }
 
-static void apply_extmap_filter(struct sk_buff *skb, struct rtpengine_output *o, struct rtp_parsed *rtp) {
-	if (!rtp->ext_hdr)
-		return;
+static void add_extmap_hdr_long(struct sk_buff *skb, struct rtpengine_output *o, struct rtp_parsed *rtp) {
+	unsigned char *w = rtp->payload; // header goes here
+	unsigned int count = 0;
+
+	unsigned char *hdr = insert_skb_data(&w, skb, rtp, 4);
+	rtp->ext_hdr = (void *) hdr;
+
+	*hdr++ = 0x10;
+	*hdr++ = 0x00;
+	hdr += 2; // length
+
+	rtp->extension = (void *) hdr;
+	rtp->extension_len = 0;
+
+	unsigned char *r = w;
+
+	add_extmap_exts_long(&w, &count, o, rtp, skb);
+
+	apply_extmap_filter_finish(r, w, count, o, skb, rtp);
+}
 
-	if (ntohs(rtp->ext_hdr->undefined) == 0xbede)
-		apply_extmap_filter_short(skb, o, rtp);
-	else if ((ntohs(rtp->ext_hdr->undefined) & 0xfff0) == 0x0100)
-		apply_extmap_filter_long(skb, o, rtp);
-	// else: leave untouched
+static void add_extmap_hdr_short(struct sk_buff *skb, struct rtpengine_output *o, struct rtp_parsed *rtp) {
+	unsigned char *w = rtp->payload; // header goes here
+	unsigned int count = 0;
+
+	unsigned char *hdr = insert_skb_data(&w, skb, rtp, 4);
+	rtp->ext_hdr = (void *) hdr;
+
+	*hdr++ = 0xbe;
+	*hdr++ = 0xde;
+	hdr += 2; // length
+
+	rtp->extension = (void *) hdr;
+	rtp->extension_len = 0;
+
+	unsigned char *r = w;
+
+	add_extmap_exts_short(&w, &count, o, rtp, skb);
+
+	apply_extmap_filter_finish(r, w, count, o, skb, rtp);
+}
+
+static void apply_extmap_filter(struct sk_buff *skb, struct rtpengine_output *o, struct rtp_parsed *rtp) {
+	if (rtp->ext_hdr) {
+		if (ntohs(rtp->ext_hdr->undefined) == 0xbede)
+			apply_extmap_filter_short(skb, o, rtp);
+		else if ((ntohs(rtp->ext_hdr->undefined) & 0xfff0) == 0x0100)
+			apply_extmap_filter_long(skb, o, rtp);
+		// else: leave untouched
+	}
+	else {
+		// add extension header?
+		if (!o->output.extmap_mid)
+		{} // nothing
+		else if (o->output.extmap_mid > 14)
+			add_extmap_hdr_long(skb, o, rtp);
+		else
+			add_extmap_hdr_short(skb, o, rtp);
+	}
 }
 
 

kernel-module/xt_RTPENGINE.c

@@ -63,7 +63,7 @@ MODULE_ALIAS("ip6t_RTPENGINE");
 
 
 #define MAX_ID 64 /* - 1 */
-#define MAX_SKB_TAIL_ROOM (sizeof(((struct rtpengine_srtp *) 0)->mki) + 20 + 16)
+#define MAX_SKB_TAIL_ROOM (sizeof(((struct rtpengine_srtp *) 0)->mki) + 20 + 16 + 22)
 
 #define MIPF		"%i:%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x:%u"
 #define MIPP(x)		(x).family,		\
@@ -1807,10 +1807,17 @@ static int proc_list_show(struct seq_file *f, void *v) {
 		}
 
 		if (o->output.extmap) {
-			seq_printf(f, "        Allowed RTP extensions:");
+			seq_printf(f, "       Allowed RTP extensions:");
 			for (j = 0; j < o->output.num_extmap_filter; j++)
 				seq_printf(f, " %u", (unsigned int) o->output.extmap_filter[j]);
 			seq_printf(f, "\n");
+
+			if (o->output.extmap_mid) {
+				seq_printf(f, "       Media ID (ext %u): '%.*s'\n",
+						(int) o->output.extmap_mid,
+						(int) o->output.extmap_mid_len,
+						o->output.extmap_mid_str);
+			}
 		}
 
 		proc_list_crypto_print(f, &o->encrypt_rtp, &o->output.encrypt, "encryption");

kernel-module/xt_RTPENGINE.h

@@ -129,6 +129,10 @@ struct rtpengine_output_info {
 	uint8_t				extmap_filter[RTPE_NUM_EXTMAP_FILTER]; // must be sorted
 	unsigned int			num_extmap_filter;
 
+	uint8_t				extmap_mid;
+	uint8_t				extmap_mid_len;
+	char				extmap_mid_str[255];
+
 	struct interface_stats_block	*iface_stats; // for egress stats
 	struct stream_stats		*stats; // for egress stats
 	struct ssrc_stats		*ssrc_stats[RTPE_NUM_SSRC_TRACKING];

t/kernel-extmap-filter.c

@@ -12,6 +12,10 @@ struct rtpengine_output {
 	struct {
 		uint8_t extmap_filter[32];
 		unsigned int num_extmap_filter;
+
+		uint8_t extmap_mid;
+		uint8_t extmap_mid_len;
+		char extmap_mid_str[32];
 	} output;
 };
 
@@ -41,6 +45,9 @@ struct rtp_parsed {
 static void skb_trim(struct sk_buff *s, unsigned int len) {
 	s->len = len;
 }
+static void skb_put(struct sk_buff *s, unsigned int len) {
+	s->len += len;
+}
 
 #include "../kernel-module/extmap_filter.inc.c"