gst/rtp/rtpulpfeccommon.c

   1 /* GStreamer plugin for forward error correction
   2  * Copyright (C) 2017 Pexip
   3  *
   4  * This library is free software; you can redistribute it and/or
   5  * modify it under the terms of the GNU Lesser General Public
   6  * License as published by the Free Software Foundation; either
   7  * version 2.1 of the License, or (at your option) any later version.
   8  *
   9  * This library is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * Lesser General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU Lesser General Public
  15  * License along with this library; if not, write to the Free Software
  16  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
  17  *
  18  * Author: Mikhail Fludkov <misha@pexip.com>
  19  */
  20
  21 #include <string.h>
  22 #include "rtpulpfeccommon.h"
  23
  24 #define MIN_RTP_HEADER_LEN 12
  25
  26 typedef struct
  27 {
  28 #if G_BYTE_ORDER == G_LITTLE_ENDIAN
  29   unsigned int csrc_count:4;    /* CSRC count */
  30   unsigned int extension:1;     /* header extension flag */
  31   unsigned int padding:1;       /* padding flag */
  32   unsigned int version:2;       /* protocol version */
  33   unsigned int payload_type:7;  /* payload type */
  34   unsigned int marker:1;        /* marker bit */
  35 #elif G_BYTE_ORDER == G_BIG_ENDIAN
  36   unsigned int version:2;       /* protocol version */
  37   unsigned int padding:1;       /* padding flag */
  38   unsigned int extension:1;     /* header extension flag */
  39   unsigned int csrc_count:4;    /* CSRC count */
  40   unsigned int marker:1;        /* marker bit */
  41   unsigned int payload_type:7;  /* payload type */
  42 #else
  43 #error "G_BYTE_ORDER should be big or little endian."
  44 #endif
  45   unsigned int seq:16;          /* sequence number */
  46   unsigned int timestamp:32;    /* timestamp */
  47   unsigned int ssrc:32;         /* synchronization source */
  48   guint8 csrclist[4];           /* optional CSRC list, 32 bits each */
  49 } RtpHeader;
  50
  51 static gsize
  52 fec_level_hdr_get_size (gboolean l_bit)
  53 {
  54   return sizeof (RtpUlpFecLevelHeader) - (l_bit ? 0 : 4);
  55 }
  56
  57 static guint64
  58 fec_level_hdr_get_mask (RtpUlpFecLevelHeader const *fec_lvl_hdr, gboolean l_bit)
  59 {
  60   return ((guint64) g_ntohs (fec_lvl_hdr->mask) << 32) |
  61       (l_bit ? g_ntohl (fec_lvl_hdr->mask_continued) : 0);
  62 }
  63
  64 static void
  65 fec_level_hdr_set_mask (RtpUlpFecLevelHeader * fec_lvl_hdr, gboolean l_bit,
  66     guint64 mask)
  67 {
  68   fec_lvl_hdr->mask = g_htons (mask >> 32);
  69   if (l_bit)
  70     fec_lvl_hdr->mask_continued = g_htonl (mask);
  71 }
  72
  73 static guint16
  74 fec_level_hdr_get_protection_len (RtpUlpFecLevelHeader * fec_lvl_hdr)
  75 {
  76   return g_ntohs (fec_lvl_hdr->protection_len);
  77 }
  78
  79 static void
  80 fec_level_hdr_set_protection_len (RtpUlpFecLevelHeader * fec_lvl_hdr,
  81     guint16 len)
  82 {
  83   fec_lvl_hdr->protection_len = g_htons (len);
  84 }
  85
  86 static RtpUlpFecLevelHeader *
  87 fec_hdr_get_level_hdr (RtpUlpFecHeader const *fec_hdr)
  88 {
  89   return (RtpUlpFecLevelHeader *) (fec_hdr + 1);
  90 }
  91
  92 static guint64
  93 fec_hdr_get_mask (RtpUlpFecHeader const *fec_hdr)
  94 {
  95   return fec_level_hdr_get_mask (fec_hdr_get_level_hdr (fec_hdr), fec_hdr->L);
  96 }
  97
  98 static guint16
  99 fec_hdr_get_seq_base (RtpUlpFecHeader const *fec_hdr, gboolean is_ulpfec,
 100     guint16 fec_seq)
 101 {
 102   guint16 seq = g_ntohs (fec_hdr->seq);
 103   if (is_ulpfec)
 104     return seq;
 105   return fec_seq - seq;
 106 }
 107
 108 static guint16
 109 fec_hdr_get_packets_len_recovery (RtpUlpFecHeader const *fec_hdr)
 110 {
 111   return g_htons (fec_hdr->len);
 112 }
 113
 114 static guint32
 115 fec_hdr_get_timestamp_recovery (RtpUlpFecHeader const *fec_hdr)
 116 {
 117   return g_ntohl (fec_hdr->timestamp);
 118 }
 119
 120 static void
 121 _xor_mem (guint8 * restrict dst, const guint8 * restrict src, gsize length)
 122 {
 123   guint i;
 124
 125   for (i = 0; i < (length / sizeof (guint64)); ++i) {
 126     *((guint64 *) dst) ^= *((const guint64 *) src);
 127     dst += sizeof (guint64);
 128     src += sizeof (guint64);
 129   }
 130   for (i = 0; i < (length % sizeof (guint64)); ++i)
 131     dst[i] ^= src[i];
 132 }
 133
 134 guint16
 135 rtp_ulpfec_hdr_get_protection_len (RtpUlpFecHeader const *fec_hdr)
 136 {
 137   return fec_level_hdr_get_protection_len (fec_hdr_get_level_hdr (fec_hdr));
 138 }
 139
 140 RtpUlpFecHeader *
 141 rtp_ulpfec_buffer_get_fechdr (GstRTPBuffer * rtp)
 142 {
 143   return (RtpUlpFecHeader *) gst_rtp_buffer_get_payload (rtp);
 144 }
 145
 146 guint64
 147 rtp_ulpfec_buffer_get_mask (GstRTPBuffer * rtp)
 148 {
 149   return fec_hdr_get_mask (rtp_ulpfec_buffer_get_fechdr (rtp));
 150 }
 151
 152 guint16
 153 rtp_ulpfec_buffer_get_seq_base (GstRTPBuffer * rtp)
 154 {
 155   return g_ntohs (rtp_ulpfec_buffer_get_fechdr (rtp)->seq);
 156 }
 157
 158 guint
 159 rtp_ulpfec_get_headers_len (gboolean fec_mask_long)
 160 {
 161   return sizeof (RtpUlpFecHeader) + fec_level_hdr_get_size (fec_mask_long);
 162 }
 163
 164 guint64
 165 rtp_ulpfec_packet_mask_from_seqnum (guint16 seq,
 166     guint16 fec_seq_base, gboolean fec_mask_long)
 167 {
 168   gint seq_delta = gst_rtp_buffer_compare_seqnum (fec_seq_base, seq);
 169   if (seq_delta >= 0
 170       && seq_delta <= RTP_ULPFEC_SEQ_BASE_OFFSET_MAX (fec_mask_long))
 171     return 1ULL << (RTP_ULPFEC_SEQ_BASE_OFFSET_MAX (TRUE) - seq_delta);
 172   return 0;
 173 }
 174
 175 gboolean
 176 rtp_ulpfec_mask_is_long (guint64 mask)
 177 {
 178   return (mask & 0xffffffff) ? TRUE : FALSE;
 179 }
 180
 181 gboolean
 182 rtp_ulpfec_buffer_is_valid (GstRTPBuffer * rtp)
 183 {
 184   guint payload_len = gst_rtp_buffer_get_payload_len (rtp);
 185   RtpUlpFecHeader *fec_hdr;
 186   guint fec_hdrs_len;
 187   guint fec_packet_len;
 188
 189   if (payload_len < sizeof (RtpUlpFecHeader))
 190     goto toosmall;
 191
 192   fec_hdr = rtp_ulpfec_buffer_get_fechdr (rtp);
 193   if (fec_hdr->E)
 194     goto invalidcontent;
 195
 196   fec_hdrs_len = rtp_ulpfec_get_headers_len (fec_hdr->L);
 197   if (payload_len < fec_hdrs_len)
 198     goto toosmall;
 199
 200   fec_packet_len = fec_hdrs_len + rtp_ulpfec_hdr_get_protection_len (fec_hdr);
 201   if (fec_packet_len != payload_len)
 202     goto lengthmismatch;
 203
 204   return TRUE;
 205 toosmall:
 206   GST_WARNING ("FEC packet too small");
 207   return FALSE;
 208
 209 lengthmismatch:
 210   GST_WARNING ("invalid FEC packet (declared length %u, real length %u)",
 211       fec_packet_len, payload_len);
 212   return FALSE;
 213
 214 invalidcontent:
 215   GST_WARNING ("FEC Header contains invalid fields: %u", fec_hdr->E);
 216   return FALSE;
 217 }
 218
 219
 220 void
 221 rtp_buffer_to_ulpfec_bitstring (GstRTPBuffer * rtp, GArray * dst_arr,
 222     gboolean fec_buffer, gboolean fec_mask_long)
 223 {
 224   if (G_UNLIKELY (fec_buffer)) {
 225     guint payload_len = gst_rtp_buffer_get_payload_len (rtp);
 226     g_array_set_size (dst_arr, MAX (payload_len, dst_arr->len));
 227     memcpy (dst_arr->data, gst_rtp_buffer_get_payload (rtp), payload_len);
 228   } else {
 229     const guint8 *src = rtp->data[0];
 230     guint len = gst_rtp_buffer_get_packet_len (rtp) - MIN_RTP_HEADER_LEN;
 231     guint dst_offset = rtp_ulpfec_get_headers_len (fec_mask_long);
 232     guint src_offset = MIN_RTP_HEADER_LEN;
 233     guint8 *dst;
 234
 235     g_array_set_size (dst_arr, MAX (dst_offset + len, dst_arr->len));
 236     dst = (guint8 *) dst_arr->data;
 237
 238     *((guint64 *) dst) ^= *((const guint64 *) src);
 239     ((RtpUlpFecHeader *) dst)->len ^= g_htons (len);
 240     _xor_mem (dst + dst_offset, src + src_offset, len);
 241   }
 242 }
 243
 244 GstBuffer *
 245 rtp_ulpfec_bitstring_to_media_rtp_buffer (GArray * arr,
 246     gboolean fec_mask_long, guint32 ssrc, guint16 seq)
 247 {
 248   guint fec_hdrs_len = rtp_ulpfec_get_headers_len (fec_mask_long);
 249   guint payload_len =
 250       fec_hdr_get_packets_len_recovery ((RtpUlpFecHeader *) arr->data);
 251   GstMapInfo ret_info = GST_MAP_INFO_INIT;
 252   GstMemory *ret_mem;
 253   GstBuffer *ret;
 254
 255   if (payload_len > arr->len - fec_hdrs_len)
 256     return NULL;                // Not enough data
 257
 258   ret_mem = gst_allocator_alloc (NULL, MIN_RTP_HEADER_LEN + payload_len, NULL);
 259   gst_memory_map (ret_mem, &ret_info, GST_MAP_READWRITE);
 260
 261   /* Filling 12 bytes of RTP header */
 262   *((guint64 *) ret_info.data) = *((guint64 *) arr->data);
 263   ((RtpHeader *) ret_info.data)->version = 2;
 264   ((RtpHeader *) ret_info.data)->seq = g_htons (seq);
 265   ((RtpHeader *) ret_info.data)->ssrc = g_htonl (ssrc);
 266   /* Filling payload */
 267   memcpy (ret_info.data + MIN_RTP_HEADER_LEN,
 268       arr->data + fec_hdrs_len, payload_len);
 269
 270   gst_memory_unmap (ret_mem, &ret_info);
 271   ret = gst_buffer_new ();
 272   gst_buffer_append_memory (ret, ret_mem);
 273   return ret;
 274 }
 275
 276 GstBuffer *
 277 rtp_ulpfec_bitstring_to_fec_rtp_buffer (GArray * arr,
 278     guint16 seq_base, gboolean fec_mask_long, guint64 fec_mask,
 279     gboolean marker, guint8 pt, guint16 seq, guint32 timestamp, guint32 ssrc)
 280 {
 281   GstRTPBuffer rtp = GST_RTP_BUFFER_INIT;
 282   GstBuffer *ret;
 283
 284   /* Filling FEC headers */
 285   {
 286     RtpUlpFecHeader *hdr = (RtpUlpFecHeader *) arr->data;
 287     RtpUlpFecLevelHeader *lvlhdr;
 288     hdr->E = 0;
 289     hdr->L = fec_mask_long;
 290     hdr->seq = g_htons (seq_base);
 291
 292     lvlhdr = fec_hdr_get_level_hdr (hdr);
 293     fec_level_hdr_set_protection_len (lvlhdr,
 294         arr->len - rtp_ulpfec_get_headers_len (fec_mask_long));
 295     fec_level_hdr_set_mask (lvlhdr, fec_mask_long, fec_mask);
 296   }
 297
 298   /* Filling RTP header, copying payload */
 299   ret = gst_rtp_buffer_new_allocate (arr->len, 0, 0);
 300   if (!gst_rtp_buffer_map (ret, GST_MAP_READWRITE, &rtp))
 301     g_assert_not_reached ();
 302
 303   gst_rtp_buffer_set_marker (&rtp, marker);
 304   gst_rtp_buffer_set_payload_type (&rtp, pt);
 305   gst_rtp_buffer_set_seq (&rtp, seq);
 306   gst_rtp_buffer_set_timestamp (&rtp, timestamp);
 307   gst_rtp_buffer_set_ssrc (&rtp, ssrc);
 308
 309   memcpy (gst_rtp_buffer_get_payload (&rtp), arr->data, arr->len);
 310
 311   gst_rtp_buffer_unmap (&rtp);
 312
 313   return ret;
 314 }
 315
 316 /**
 317  * rtp_ulpfec_map_info_map:
 318  * @buffer: (transfer: full) #GstBuffer
 319  * @info: #RtpUlpFecMapInfo
 320  *
 321  * Maps the contents of @buffer into @info. If @buffer made of many #GstMemory
 322  * objects, merges them together to create a new buffer made of single
 323  * continious #GstMemory.
 324  *
 325  * Returns: %TRUE if @buffer could be mapped
 326  **/
 327 gboolean
 328 rtp_ulpfec_map_info_map (GstBuffer * buffer, RtpUlpFecMapInfo * info)
 329 {
 330   /* We need to make sure we are working with continious memory chunk.
 331    * If not merge all memories together */
 332   if (gst_buffer_n_memory (buffer) > 1) {
 333     GstBuffer *new_buffer = gst_buffer_new ();
 334     GstMemory *mem = gst_buffer_get_all_memory (buffer);
 335     gst_buffer_append_memory (new_buffer, mem);
 336
 337     /* We supposed to own the old buffer, but we don't use it here, so unref */
 338     gst_buffer_unref (buffer);
 339     buffer = new_buffer;
 340   }
 341
 342   if (!gst_rtp_buffer_map (buffer,
 343           GST_MAP_READ | GST_RTP_BUFFER_MAP_FLAG_SKIP_PADDING, &info->rtp)) {
 344     /* info->rtp.buffer = NULL is an indication for rtp_ulpfec_map_info_unmap()
 345      * that mapping has failed */
 346     g_assert (NULL == info->rtp.buffer);
 347     gst_buffer_unref (buffer);
 348     return FALSE;
 349   }
 350   return TRUE;
 351 }
 352
 353 /**
 354  * rtp_ulpfec_map_info_unmap:
 355  * @info: #RtpUlpFecMapInfo
 356  *
 357  * Unmap @info previously mapped with rtp_ulpfec_map_info_map() and unrefs the
 358  * buffer. For convinience can even be called even if rtp_ulpfec_map_info_map
 359  * returned FALSE
 360  **/
 361 void
 362 rtp_ulpfec_map_info_unmap (RtpUlpFecMapInfo * info)
 363 {
 364   GstBuffer *buffer = info->rtp.buffer;
 365
 366   if (buffer) {
 367     gst_rtp_buffer_unmap (&info->rtp);
 368     gst_buffer_unref (buffer);
 369   }
 370 }
 371
 372 void
 373 rtp_ulpfec_log_rtppacket (GstDebugCategory * cat, GstDebugLevel level,
 374     gpointer object, const gchar * name, GstRTPBuffer * rtp)
 375 {
 376   guint seq;
 377   guint ssrc;
 378   guint timestamp;
 379   guint pt;
 380
 381   if (level > gst_debug_category_get_threshold (cat))
 382     return;
 383
 384   seq = gst_rtp_buffer_get_seq (rtp);
 385   ssrc = gst_rtp_buffer_get_ssrc (rtp);
 386   timestamp = gst_rtp_buffer_get_timestamp (rtp);
 387   pt = gst_rtp_buffer_get_payload_type (rtp);
 388
 389   GST_CAT_LEVEL_LOG (cat, level, object,
 390       "%-22s: [%c%c%c%c] ssrc=0x%08x pt=%u tstamp=%u seq=%u size=%u(%u,%u)",
 391       name,
 392       gst_rtp_buffer_get_marker (rtp) ? 'M' : ' ',
 393       gst_rtp_buffer_get_extension (rtp) ? 'X' : ' ',
 394       gst_rtp_buffer_get_padding (rtp) ? 'P' : ' ',
 395       gst_rtp_buffer_get_csrc_count (rtp) > 0 ? 'C' : ' ',
 396       ssrc, pt, timestamp, seq,
 397       gst_rtp_buffer_get_packet_len (rtp),
 398       gst_rtp_buffer_get_packet_len (rtp) - MIN_RTP_HEADER_LEN,
 399       gst_rtp_buffer_get_payload_len (rtp));
 400 }
 401
 402 void
 403 rtp_ulpfec_log_fec_packet (GstDebugCategory * cat, GstDebugLevel level,
 404     gpointer object, GstRTPBuffer * fecrtp)
 405 {
 406   RtpUlpFecHeader *fec_hdr;
 407   RtpUlpFecLevelHeader *fec_level_hdr;
 408
 409   if (level > gst_debug_category_get_threshold (cat))
 410     return;
 411
 412   fec_hdr = gst_rtp_buffer_get_payload (fecrtp);
 413   GST_CAT_LEVEL_LOG (cat, level, object,
 414       "%-22s: [%c%c%c%c%c%c] pt=%u tstamp=%u seq=%u recovery_len=%u",
 415       "fec header",
 416       fec_hdr->E ? 'E' : ' ',
 417       fec_hdr->L ? 'L' : ' ',
 418       fec_hdr->P ? 'P' : ' ',
 419       fec_hdr->X ? 'X' : ' ',
 420       fec_hdr->CC ? 'C' : ' ',
 421       fec_hdr->M ? 'M' : ' ',
 422       fec_hdr->pt,
 423       fec_hdr_get_timestamp_recovery (fec_hdr),
 424       fec_hdr_get_seq_base (fec_hdr, TRUE,
 425           gst_rtp_buffer_get_seq (fecrtp)),
 426       fec_hdr_get_packets_len_recovery (fec_hdr));
 427
 428   fec_level_hdr = fec_hdr_get_level_hdr (fec_hdr);
 429   GST_CAT_LEVEL_LOG (cat, level, object,
 430       "%-22s: protection_len=%u mask=0x%012lx",
 431       "fec level header",
 432       g_ntohs (fec_level_hdr->protection_len),
 433       fec_level_hdr_get_mask (fec_level_hdr, fec_hdr->L));
 434 }