From fa5d206c2c101cc036cd1ac6cc15449a7ee7f64a Mon Sep 17 00:00:00 2001
From: =?utf8?q?Olivier=20Cr=C3=AAte?= <olivier.crete@collabora.com>
Date: Tue, 23 Jul 2019 17:26:16 -0400
Subject: [PATCH] rist: Insert RTP seqnum extension header

Part-of: <https://gitlab.freedesktop.org/gstreamer/gst-plugins-bad/-/merge_requests/1153>
---
 gst/rist/gstristrtpext.c | 95 ++++++++++++++++++++++++++++++++++++++++++++----
 1 file changed, 87 insertions(+), 8 deletions(-)

diff --git a/gst/rist/gstristrtpext.c b/gst/rist/gstristrtpext.c
index cc8f8c8..1b38d42 100644
--- a/gst/rist/gstristrtpext.c
+++ b/gst/rist/gstristrtpext.c
@@ -77,19 +77,89 @@ struct _GstRistRtpExt
   GstPad *srcpad, *sinkpad;
 
   gboolean drop_null;
-  gboolean seqnumext;
+  gboolean add_seqnumext;
+
+  guint32 extseqnum;
 };
 
 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"));
 
+
+/*
+ * rtp_ext_seq:
+ * @extseq: (inout): a previous extended seqs
+ * @seq: a new seq
+ *
+ * Update the @extseq field with the extended seq of @seq
+ * For the first call of the method, @extseq should point to a location
+ * with a value of -1.
+ *
+ * This function is able to handle both forward and backward seqs taking
+ * into account:
+ *   - seq wraparound making sure that the returned value is properly increased.
+ *   - seq unwraparound making sure that the returned value is properly decreased.
+ *
+ * Returns: The extended seq of @seq or 0 if the result can't go anywhere backwards.
+ *
+ * NOTE: This is a calque of gst_rtp_buffer_ext_timestamp() but with
+ * s/32/16/ and s/64/32/ and s/0xffffffff/0xffff/ and s/timestamp/seqnum/.
+ */
+static guint32
+rtp_ext_seq (guint32 * extseqnum, guint16 seqnum)
+{
+  guint32 result, ext;
+
+  g_return_val_if_fail (extseqnum != NULL, -1);
+
+  ext = *extseqnum;
+
+  if (ext == -1) {
+    result = seqnum;
+  } else {
+    /* pick wraparound counter from previous seqnum and add to new seqnum */
+    result = seqnum + (ext & ~(0xffff));
+
+    /* check for seqnum wraparound */
+    if (result < ext) {
+      guint32 diff = ext - result;
+
+      if (diff > G_MAXINT16) {
+        /* seqnum went backwards more than allowed, we wrap around and get
+         * updated extended seqnum. */
+        result += (1 << 16);
+      }
+    } else {
+      guint32 diff = result - ext;
+
+      if (diff > G_MAXINT16) {
+        if (result < (1 << 16)) {
+          GST_WARNING
+              ("Cannot unwrap, any wrapping took place yet. Returning 0 without updating extended seqnum.");
+          return 0;
+        } else {
+          /* seqnum went forwards more than allowed, we unwrap around and get
+           * updated extended seqnum. */
+          result -= (1 << 16);
+          /* We don't want the extended seqnum storage to go back, ever */
+          return result;
+        }
+      }
+    }
+  }
+
+  *extseqnum = result;
+
+  return result;
+}
+
+
 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;
@@ -97,7 +167,7 @@ gst_rist_rtp_ext_chain (GstPad * pad, GstObject * parent, GstBuffer * buffer)
   guint8 npd_bits = 0;
   gboolean num_packets_deleted = 0;
 
-  if (!self->drop_null && !self->seqnumext)
+  if (!self->drop_null && !self->add_seqnumext)
     return gst_pad_push (self->srcpad, buffer);
 
   if (self->drop_null) {
@@ -180,14 +250,21 @@ gst_rist_rtp_ext_chain (GstPad * pad, GstObject * parent, GstBuffer * buffer)
   bits |= (npd_bits & 0x7F);
 
   gst_rtp_buffer_set_extension (&rtp, TRUE);
-  gst_rtp_buffer_set_extension_data (&rtp, bits, self->seqnumext ? 1 : 0);
+  gst_rtp_buffer_set_extension_data (&rtp, bits, self->add_seqnumext ? 1 : 0);
 
-  if (self->seqnumext) {
+  if (self->add_seqnumext) {
     guint8 *data;
     guint wordlen;
+    guint16 seqnum = gst_rtp_buffer_get_seq (&rtp);
+    guint32 extseqnum;
+
+    if (GST_BUFFER_IS_DISCONT (buffer))
+      self->extseqnum = -1;
+
+    extseqnum = rtp_ext_seq (&self->extseqnum, seqnum);
 
     gst_rtp_buffer_get_extension_data (&rtp, &bits, (void **) &data, &wordlen);
-    GST_WRITE_UINT16_BE (data, seqnumext_val);
+    GST_WRITE_UINT16_BE (data, (extseqnum >> 16));
     data[2] = data[3] = 0;
   }
 
@@ -212,6 +289,8 @@ error_mapped:
 static void
 gst_rist_rtp_ext_init (GstRistRtpExt * self)
 {
+  self->extseqnum = -1;
+
   self->sinkpad = gst_pad_new_from_static_template (&sink_templ,
       sink_templ.name_template);
   self->srcpad = gst_pad_new_from_static_template (&src_templ,
@@ -236,7 +315,7 @@ gst_rist_rtp_ext_get_property (GObject * object, guint prop_id,
       g_value_set_boolean (value, self->drop_null);
       break;
     case PROP_SEQUENCE_NUMBER_EXTENSION:
-      g_value_set_boolean (value, self->seqnumext);
+      g_value_set_boolean (value, self->add_seqnumext);
       break;
     default:
       G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
@@ -255,7 +334,7 @@ gst_rist_rtp_ext_set_property (GObject * object, guint prop_id,
       self->drop_null = g_value_get_boolean (value);
       break;
     case PROP_SEQUENCE_NUMBER_EXTENSION:
-      self->seqnumext = g_value_get_boolean (value);
+      self->add_seqnumext = g_value_get_boolean (value);
       break;
     default:
       G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
-- 
2.7.4