rist: Add element that inserts the RTP header extension

Currently can suppress the TS null packets, but can't insert
the seqnum extension yet.

Part-of: <https://gitlab.freedesktop.org/gstreamer/gst-plugins-bad/-/merge_requests/1153>

4 files changed, 304 insertions, 1 deletions

diff --git a/gst/rist/gstrist.h b/gst/rist/gstrist.h
index b4bcb6f04..c37fb9d0f 100644
--- a/gst/rist/gstrist.h
+++ b/gst/rist/gstrist.h
@@ -50,10 +50,18 @@ GType gst_rist_src_get_type (void);
 
 #define GST_TYPE_RIST_SINK          (gst_rist_sink_get_type())
 #define GST_RIST_SINK(obj)          (G_TYPE_CHECK_INSTANCE_CAST((obj),GST_TYPE_RIST_SINK,GstRistSink))
-typedef struct _GstRistSink GstRistSink; 
+typedef struct _GstRistSink GstRistSink;
 typedef struct {
   GstBinClass parent;
 } GstRistSinkClass;
 GType gst_rist_sink_get_type (void);
 
+#define GST_TYPE_RIST_RTP_EXT      (gst_rist_rtp_ext_get_type())
+#define GST_RIST_RTP_EXT(obj)      (G_TYPE_CHECK_INSTANCE_CAST((obj),GST_TYPE_RIST_RTP_EXT,GstRistRtpExt))
+typedef struct _GstRistRtpExt GstRistRtpExt;
+typedef struct {
+  GstElementClass parent;
+} GstRistRtpExtClass;
+GType gst_rist_rtp_ext_get_type (void);
+
 #endif
diff --git a/gst/rist/gstristplugin.c b/gst/rist/gstristplugin.c
index be0110c0c..2ca207fc2 100644
--- a/gst/rist/gstristplugin.c
+++ b/gst/rist/gstristplugin.c
@@ -43,6 +43,9 @@ plugin_init (GstPlugin * plugin)
   if (!gst_element_register (plugin, "roundrobin", GST_RANK_NONE,
           GST_TYPE_ROUND_ROBIN))
     return FALSE;
+  if (!gst_element_register (plugin, "ristrtpext", GST_RANK_NONE,
+          GST_TYPE_RIST_RTP_EXT))
+    return FALSE;
 
   return TRUE;
 }
diff --git a/gst/rist/gstristrtpext.c b/gst/rist/gstristrtpext.c
new file mode 100644
index 000000000..ce9396ec9
--- /dev/null
+++ b/gst/rist/gstristrtpext.c
@@ -0,0 +1,291 @@
+/* GStreamer RIST plugin
+ * Copyright (C) 2019 Net Insight AB
+ *     Author: Olivier Crete <olivier.crete@collabora.com>
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Library General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU Library General Public
+ * License along with this library; if not, write to the
+ * Free Software Foundation, Inc., 51 Franklin St, Fifth Floor,
+ * Boston, MA 02110-1301, USA.
+ */
+
+/**
+ * SECTION:element-ristrtpext
+ * @title: ristrtpext
+ * @see_also: ristsink
+ *
+ * This elements adds the RTP header extension defined by the RIST profile.
+ *
+ * If the GstRistRtpExt::drop-null-ts-packets property is set, then it
+ * will try to parse a MPEG Transport Stream inside the RTP packets
+ * and look for "null" packets among the first 7 TS packets and remove
+ * them, and mark their removal in the header.
+ *
+ * If the GstRistRtpExt::sequence-number-extension property is set, it will add
+ * a RTP sequence number roll-over counter to the RTP header extension. This
+ * code assumes that packets inserted to this element are never more than half
+ * of the sequence number space (2^15) away from the latest. Re-transmissions
+ * should therefore be done after processing with this element.
+ *
+ * If the GstRistRtpExt::drop-null-ts-packets and
+ * GstRistRtpExt::sequence-number-extension properties are both FALSE, it is
+ * pass through.
+ */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <gst/rtp/rtp.h>
+
+#include "gstrist.h"
+
+GST_DEBUG_CATEGORY_STATIC (gst_rist_rtp_ext_debug);
+#define GST_CAT_DEFAULT gst_rist_rtp_ext_debug
+
+enum
+{
+  PROP_DROP_NULL_TS_PACKETS = 1,
+  PROP_SEQUENCE_NUMBER_EXTENSION
+};
+
+static GstStaticPadTemplate src_templ = GST_STATIC_PAD_TEMPLATE ("src",
+    GST_PAD_SRC,
+    GST_PAD_ALWAYS,
+    GST_STATIC_CAPS ("application/x-rtp"));
+
+
+static GstStaticPadTemplate sink_templ = GST_STATIC_PAD_TEMPLATE ("sink",
+    GST_PAD_SINK,
+    GST_PAD_ALWAYS,
+    GST_STATIC_CAPS ("application/x-rtp"));
+
+
+struct _GstRistRtpExt
+{
+  GstElement parent;
+
+  GstPad *srcpad, *sinkpad;
+
+  gboolean drop_null;
+  gboolean seqnumext;
+};
+
+G_DEFINE_TYPE_WITH_CODE (GstRistRtpExt, gst_rist_rtp_ext, GST_TYPE_ELEMENT,
+    GST_DEBUG_CATEGORY_INIT (gst_rist_rtp_ext_debug, "ristrtpext", 0,
+        "RIST RTP Extension"));
+
+static GstFlowReturn
+gst_rist_rtp_ext_chain (GstPad * pad, GstObject * parent, GstBuffer * buffer)
+{
+  GstRistRtpExt *self = GST_RIST_RTP_EXT (parent);
+  GstRTPBuffer rtp = GST_RTP_BUFFER_INIT;
+  guint16 seqnumext_val = 0;
+  gboolean drop_null = self->drop_null;
+  gboolean ts_packet_size = 0;
+  guint ts_packet_count = 0;
+  guint16 bits = 0;
+  guint8 npd_bits = 0;
+  gboolean num_packets_deleted = 0;
+
+  if (!self->drop_null && !self->seqnumext)
+    return gst_pad_push (self->srcpad, buffer);
+
+  if (self->drop_null) {
+    if (!gst_rtp_buffer_map (buffer, GST_MAP_READ, &rtp)) {
+      GST_ELEMENT_ERROR (self, STREAM, MUX, (NULL),
+          ("Could not map RTP buffer"));
+      goto mapping_error;
+    }
+
+    if (gst_rtp_buffer_get_payload_type (&rtp) == GST_RTP_PAYLOAD_MP2T) {
+      if (gst_rtp_buffer_get_payload_len (&rtp) % 188 == 0) {
+        ts_packet_size = 188;
+        ts_packet_count = gst_rtp_buffer_get_payload_len (&rtp) / 188;
+      } else if (gst_rtp_buffer_get_payload_len (&rtp) % 204 == 0) {
+        ts_packet_size = 204;
+        ts_packet_count = gst_rtp_buffer_get_payload_len (&rtp) / 204;
+      } else {
+        drop_null = FALSE;
+      }
+    }
+    gst_rtp_buffer_unmap (&rtp);
+  }
+
+  buffer = gst_buffer_make_writable (buffer);
+
+  if (!gst_rtp_buffer_map (buffer, GST_MAP_READWRITE, &rtp)) {
+    GST_ELEMENT_ERROR (self, STREAM, MUX, (NULL), ("Could not map RTP buffer"));
+    goto mapping_error;
+  }
+
+  if (drop_null) {
+    guint8 *data = gst_rtp_buffer_get_payload (&rtp);
+    guint plen = gst_rtp_buffer_get_payload_len (&rtp);
+    guint offset = 0;
+    guint i;
+
+    if (gst_rtp_buffer_get_padding (&rtp)) {
+      GST_ELEMENT_ERROR (self, STREAM, MUX, (NULL),
+          ("FIXME: Can not remove null TS packets if RTP padding is present"));
+      goto mapping_error;
+    }
+
+    for (i = 0; i < MIN (ts_packet_count, 7); i++) {
+      guint16 pid;
+
+      /* Look for sync byte (0x47) at the start of TS packets */
+      if (data[offset] != 0x47) {
+        GST_ELEMENT_ERROR (self, STREAM, MUX, (NULL),
+            ("Buffer does not contain valid MP2T data,"
+                " the sync byte is not present"));
+        goto error_mapped;
+      }
+
+      pid = ((data[offset + 1] & 0x1F) << 8) | data[offset + 2];
+      /* is NULL packet (PID == 0x1FFF means null) */
+      if (pid == 0x1FFF) {
+        guint remaining_plen = plen - (num_packets_deleted * ts_packet_size);
+
+        num_packets_deleted++;
+        npd_bits |= 1 << (6 - i);
+        if (offset + ts_packet_size < remaining_plen)
+          memmove (data + offset, data + offset + ts_packet_size,
+              remaining_plen - offset - ts_packet_size);
+      }
+    }
+  }
+
+  if (gst_rtp_buffer_get_extension (&rtp)) {
+    GST_ELEMENT_ERROR (self, STREAM, MUX, (NULL),
+        ("RTP buffer already has an extension set"));
+    goto error_mapped;
+  }
+
+  bits = 0;
+  bits |= drop_null << 15;      /* N */
+  bits |= self->seqnumext << 14;        /* E */
+  bits |= (ts_packet_count & 7) << 10;  /* Size */
+  bits |= (ts_packet_size == 204) << 7; /* T */
+  bits |= (npd_bits & 0x7F);
+
+  gst_rtp_buffer_set_extension (&rtp, TRUE);
+  gst_rtp_buffer_set_extension_data (&rtp, bits, self->seqnumext ? 1 : 0);
+
+  if (self->seqnumext) {
+    guint8 *data;
+    guint wordlen;
+
+    gst_rtp_buffer_get_extension_data (&rtp, &bits, (void **) &data, &wordlen);
+    GST_WRITE_UINT16_BE (data, seqnumext_val);
+    data[2] = data[3] = 0;
+  }
+
+  gst_rtp_buffer_unmap (&rtp);
+
+  if (num_packets_deleted != 0)
+    gst_buffer_resize (buffer, 0,
+        gst_buffer_get_size (buffer) - (ts_packet_size * num_packets_deleted));
+
+  return gst_pad_push (self->srcpad, buffer);
+
+mapping_error:
+  gst_buffer_unref (buffer);
+  return GST_FLOW_ERROR;
+
+error_mapped:
+  gst_rtp_buffer_unmap (&rtp);
+  gst_buffer_unref (buffer);
+  return GST_FLOW_ERROR;
+}
+
+static void
+gst_rist_rtp_ext_init (GstRistRtpExt * self)
+{
+  self->sinkpad = gst_pad_new_from_static_template (&sink_templ,
+      sink_templ.name_template);
+  self->srcpad = gst_pad_new_from_static_template (&src_templ,
+      src_templ.name_template);
+
+  GST_PAD_SET_PROXY_ALLOCATION (self->sinkpad);
+  GST_PAD_SET_PROXY_CAPS (self->sinkpad);
+  gst_pad_set_chain_function (self->sinkpad, gst_rist_rtp_ext_chain);
+
+  gst_element_add_pad (GST_ELEMENT (self), self->sinkpad);
+  gst_element_add_pad (GST_ELEMENT (self), self->srcpad);
+}
+
+static void
+gst_rist_rtp_ext_get_property (GObject * object, guint prop_id,
+    GValue * value, GParamSpec * pspec)
+{
+  GstRistRtpExt *self = GST_RIST_RTP_EXT (object);
+
+  switch (prop_id) {
+    case PROP_DROP_NULL_TS_PACKETS:
+      g_value_set_boolean (value, self->drop_null);
+      break;
+    case PROP_SEQUENCE_NUMBER_EXTENSION:
+      g_value_set_boolean (value, self->seqnumext);
+      break;
+    default:
+      G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
+      break;
+  }
+}
+
+static void
+gst_rist_rtp_ext_set_property (GObject * object, guint prop_id,
+    const GValue * value, GParamSpec * pspec)
+{
+  GstRistRtpExt *self = GST_RIST_RTP_EXT (object);
+
+  switch (prop_id) {
+    case PROP_DROP_NULL_TS_PACKETS:
+      self->drop_null = g_value_get_boolean (value);
+      break;
+    case PROP_SEQUENCE_NUMBER_EXTENSION:
+      self->seqnumext = g_value_get_boolean (value);
+      break;
+    default:
+      G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
+      break;
+  }
+}
+
+static void
+gst_rist_rtp_ext_class_init (GstRistRtpExtClass * klass)
+{
+  GstElementClass *element_class = (GstElementClass *) klass;
+  GObjectClass *object_class = (GObjectClass *) klass;
+
+  gst_element_class_set_metadata (element_class,
+      "RIST RTP Extension adder", "Filter/Network",
+      "Adds RIST TR-06-2 RTP Header extension",
+      "Olivier Crete <olivier.crete@collabora.com");
+  gst_element_class_add_static_pad_template (element_class, &src_templ);
+  gst_element_class_add_static_pad_template (element_class, &sink_templ);
+
+  object_class->get_property = gst_rist_rtp_ext_get_property;
+  object_class->set_property = gst_rist_rtp_ext_set_property;
+
+  g_object_class_install_property (object_class, PROP_DROP_NULL_TS_PACKETS,
+      g_param_spec_boolean ("drop-null-ts-packets", "Drop null TS packets",
+          "Drop null MPEG-TS packet and replace them with a custom header"
+          " extension.", FALSE,
+          G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS | G_PARAM_CONSTRUCT));
+  g_object_class_install_property (object_class, PROP_SEQUENCE_NUMBER_EXTENSION,
+      g_param_spec_boolean ("sequence-number-extension",
+          "Sequence Number Extension",
+          "Add sequence number extension to packets.", FALSE,
+          G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS | G_PARAM_CONSTRUCT));
+}
diff --git a/gst/rist/meson.build b/gst/rist/meson.build
index 9bd5ccd64..9a4df2d81 100644
--- a/gst/rist/meson.build
+++ b/gst/rist/meson.build
@@ -5,6 +5,7 @@ rist_sources = [
   'gstristsrc.c',
   'gstristsink.c',
   'gstristplugin.c',
+  'gstristrtpext.c'
 ]
 
 gstrist = library('gstrist',