avtp: Add fragmentation feature to CVF payloader

Based on `mtu` property, the CVF payloader is now capable of properly
fragmenting H.264 NAL units that are bigger than MTU in several AVTP
packets.

AVTP spec defines two methods for fragmenting H.264 packets, but this
patch only generates non-interleaved FU-A fragments.

Usually, only the last NAL unit from a group of NAL units in a single
buffer will be big enough to be fragmented. Nevertheless, only the last
AVTP packet sent for a group of NAL units will have the M bit set (this
means that the AVTP packet for the last fragment will only have the M
bit set if there's no more NAL units in the group).

ext/avtp/gstavtpcvfpay.c

@@ -68,7 +68,13 @@ enum
 #define DEFAULT_MTU 1500
 
 #define AVTP_CVF_H264_HEADER_SIZE (sizeof(struct avtp_stream_pdu) + sizeof(guint32))
+#define FU_A_TYPE 28
+#define FU_A_HEADER_SIZE (sizeof(guint16))
 
+#define NRI_MASK            0x60
+#define NRI_SHIFT           5
+#define START_SHIFT         7
+#define END_SHIFT           6
 #define NAL_TYPE_MASK       0x1f
 #define FIRST_NAL_VCL_TYPE  0x01
 #define LAST_NAL_VCL_TYPE   0x05
@@ -185,8 +191,7 @@ gst_avtp_cvf_change_state (GstElement * element, GstStateChange transition)
 
     res = avtp_cvf_pdu_init (pdu, AVTP_CVF_FORMAT_SUBTYPE_H264);
     g_assert (res == 0);
-    res = avtp_cvf_pdu_set (pdu, AVTP_CVF_FIELD_TV, 1);
+
-    g_assert (res == 0);
     res =
         avtp_cvf_pdu_set (pdu, AVTP_CVF_FIELD_STREAM_ID,
         avtpbasepayload->streamid);
@@ -285,28 +290,100 @@ gst_avtp_cvf_pay_is_nal_vcl (GstAvtpCvfPay * avtpcvfpay, GstBuffer * nal)
   return nal_type >= FIRST_NAL_VCL_TYPE && nal_type <= LAST_NAL_VCL_TYPE;
 }
 
+static GstBuffer *
+gst_avtpcvpay_fragment_nal (GstAvtpCvfPay * avtpcvfpay, GstBuffer * nal,
+    gsize * offset, gboolean * last_fragment)
+{
+  GstBuffer *fragment_header, *fragment;
+  guint8 nal_header, nal_type, nal_nri, fu_indicator, fu_header;
+  gsize available, nal_size, fragment_size, remaining;
+  GstMapInfo map;
+
+  nal_size = gst_buffer_get_size (nal);
+
+  /* If NAL + header will be smaller than MTU, nothing to fragment */
+  if (*offset == 0 && (nal_size + AVTP_CVF_H264_HEADER_SIZE) <= avtpcvfpay->mtu) {
+    *last_fragment = TRUE;
+    *offset = nal_size;
+    GST_DEBUG_OBJECT (avtpcvfpay, "Generated fragment with size %lu", nal_size);
+    return gst_buffer_ref (nal);
+  }
+
+  /* We're done with this buffer */
+  if (*offset == nal_size) {
+    return NULL;
+  }
+
+  *last_fragment = FALSE;
+
+  /* Remaining size is smaller than MTU, so this is the last fragment */
+  remaining = nal_size - *offset + AVTP_CVF_H264_HEADER_SIZE + FU_A_HEADER_SIZE;
+  if (remaining <= avtpcvfpay->mtu) {
+    *last_fragment = TRUE;
+  }
+
+  fragment_header = gst_buffer_new_allocate (NULL, FU_A_HEADER_SIZE, NULL);
+  if (G_UNLIKELY (fragment_header == NULL)) {
+    GST_ERROR_OBJECT (avtpcvfpay, "Could not allocate memory for buffer");
+    return NULL;
+  }
+
+  /* NAL header info is spread to all FUs */
+  gst_buffer_extract (nal, 0, &nal_header, 1);
+  nal_type = nal_header & NAL_TYPE_MASK;
+  nal_nri = (nal_header & NRI_MASK) >> NRI_SHIFT;
+
+  fu_indicator = (nal_nri << NRI_SHIFT) | FU_A_TYPE;
+  fu_header = ((*offset == 0) << START_SHIFT) |
+      ((*last_fragment == TRUE) << END_SHIFT) | nal_type;
+
+  gst_buffer_map (fragment_header, &map, GST_MAP_WRITE);
+  map.data[0] = fu_indicator;
+  map.data[1] = fu_header;
+  gst_buffer_unmap (fragment_header, &map);
+
+  available =
+      avtpcvfpay->mtu - AVTP_CVF_H264_HEADER_SIZE -
+      gst_buffer_get_size (fragment_header);
+
+  /* NAL unit header is not sent, but spread into FU indicator and header,
+   * and reconstructed on depayloader */
+  if (*offset == 0)
+    *offset = 1;
+
+  fragment_size =
+      available < (nal_size - *offset) ? available : (nal_size - *offset);
+
+  fragment =
+      gst_buffer_append (fragment_header, gst_buffer_copy_region (nal,
+          GST_BUFFER_COPY_MEMORY, *offset, fragment_size));
+
+  *offset += fragment_size;
+
+  GST_DEBUG_OBJECT (avtpcvfpay, "Generated fragment with size %lu",
+      fragment_size);
+
+  return fragment;
+}
+
 static gboolean
 gst_avtp_cvf_pay_prepare_avtp_packets (GstAvtpCvfPay * avtpcvfpay,
     GPtrArray * nals, GPtrArray * avtp_packets)
 {
   GstAvtpBasePayload *avtpbasepayload = GST_AVTP_BASE_PAYLOAD (avtpcvfpay);
-  GstBuffer *header, *packet, *nal;
+  GstBuffer *header, *nal;
   GstMapInfo map;
   gint i;
 
   for (i = 0; i < nals->len; i++) {
-    int res;
-    struct avtp_stream_pdu *pdu;
     guint64 avtp_time, h264_time;
-
+    gboolean last_fragment;
-    /* Copy saved header to reuse common fields and change what is needed */
+    GstBuffer *fragment;
-    header = gst_buffer_copy (avtpcvfpay->header);
+    gsize offset;
-    gst_buffer_map (header, &map, GST_MAP_WRITE);
-    pdu = (struct avtp_stream_pdu *) map.data;
 
     nal = g_ptr_array_index (nals, i);
     GST_LOG_OBJECT (avtpcvfpay,
-        "Preparing AVTP packet for NAL whose size is %lu",
+        "Preparing AVTP packets for NAL whose size is %lu",
         gst_buffer_get_size (nal));
 
     /* Calculate timestamps. Note that we do it twice, one using DTS as base,
@@ -323,37 +400,77 @@ gst_avtp_cvf_pay_prepare_avtp_packets (GstAvtpCvfPay * avtpcvfpay,
         avtpbasepayload->tu + avtpbasepayload->processing_deadline +
         avtpbasepayload->latency;
 
-    res = avtp_cvf_pdu_set (pdu, AVTP_CVF_FIELD_TIMESTAMP, avtp_time);
+    offset = 0;
-    g_assert (res == 0);
+    while ((fragment =
-    res =
+            gst_avtpcvpay_fragment_nal (avtpcvfpay, nal, &offset,
-        avtp_cvf_pdu_set (pdu, AVTP_CVF_FIELD_SEQ_NUM,
+                &last_fragment))) {
-        avtpbasepayload->seqnum++);
+      GstBuffer *packet;
-    g_assert (res == 0);
+      struct avtp_stream_pdu *pdu;
+      gint res;
 
-    /* Set M only if last NAL and it is a VCL NAL */
+      /* Copy header to reuse common fields and change what is needed */
-    res = avtp_cvf_pdu_set (pdu, AVTP_CVF_FIELD_M,
+      header = gst_buffer_copy (avtpcvfpay->header);
-        i == nals->len - 1 && gst_avtp_cvf_pay_is_nal_vcl (avtpcvfpay, nal));
+      gst_buffer_map (header, &map, GST_MAP_WRITE);
-    g_assert (res == 0);
+      pdu = (struct avtp_stream_pdu *) map.data;
 
       /* Stream data len includes AVTP H264 header len as this is part of
        * the payload too. It's just the uint32_t with the h264 timestamp*/
       res =
           avtp_cvf_pdu_set (pdu, AVTP_CVF_FIELD_STREAM_DATA_LEN,
-        gst_buffer_get_size (nal) + sizeof (uint32_t));
+          gst_buffer_get_size (fragment) + sizeof (uint32_t));
       g_assert (res == 0);
 
+      res =
+          avtp_cvf_pdu_set (pdu, AVTP_CVF_FIELD_SEQ_NUM,
+          avtpbasepayload->seqnum++);
+      g_assert (res == 0);
+
+      /* Although AVTP_TIMESTAMP is only set on the very last fragment, IEEE 1722
+       * doesn't mention such need for H264_TIMESTAMP. So, we set it for all
+       * fragments */
       res = avtp_cvf_pdu_set (pdu, AVTP_CVF_FIELD_H264_TIMESTAMP, h264_time);
       g_assert (res == 0);
-
       res = avtp_cvf_pdu_set (pdu, AVTP_CVF_FIELD_H264_PTV, 1);
       g_assert (res == 0);
-    /* TODO check if NALs can be grouped, or need to be
-     * fragmented */
 
-    gst_buffer_unmap (header, &map);
+      /* Only last fragment should have M, AVTP_TS and TV fields set */
-    packet = gst_buffer_append (header, nal);
+      if (last_fragment) {
+        gboolean M;
+
+        res = avtp_cvf_pdu_set (pdu, AVTP_CVF_FIELD_TV, 1);
+        g_assert (res == 0);
+
+        res = avtp_cvf_pdu_set (pdu, AVTP_CVF_FIELD_TIMESTAMP, avtp_time);
+        g_assert (res == 0);
+
+        /* Set M only if last NAL and it is a VCL NAL */
+        M = (i == nals->len - 1)
+            && gst_avtp_cvf_pay_is_nal_vcl (avtpcvfpay, nal);
+        res = avtp_cvf_pdu_set (pdu, AVTP_CVF_FIELD_M, M);
+        g_assert (res == 0);
+
+        if (M) {
+          GST_LOG_OBJECT (avtpcvfpay, "M packet sent, PTS: %" GST_TIME_FORMAT
+              " DTS: %" GST_TIME_FORMAT " AVTP_TS: %" GST_TIME_FORMAT
+              " H264_TS: %" GST_TIME_FORMAT "\navtp_time: %lu h264_time: %lu",
+              GST_TIME_ARGS (h264_time),
+              GST_TIME_ARGS (avtp_time), GST_TIME_ARGS ((guint32) avtp_time),
+              GST_TIME_ARGS ((guint32) h264_time), avtp_time, h264_time);
+        }
+      }
+
+      packet = gst_buffer_append (header, fragment);
+
+      /* Keep original timestamps */
+      GST_BUFFER_PTS (packet) = GST_BUFFER_PTS (nal);
+      GST_BUFFER_DTS (packet) = GST_BUFFER_DTS (nal);
 
       g_ptr_array_add (avtp_packets, packet);
+
+      gst_buffer_unmap (header, &map);
+    }
+
+    gst_buffer_unref (nal);
   }
 
   GST_LOG_OBJECT (avtpcvfpay, "Prepared %u AVTP packets", avtp_packets->len);