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 #define ONE_64BIT G_GUINT64_CONSTANT(1)
 165
 166 guint64
 167 rtp_ulpfec_packet_mask_from_seqnum (guint16 seq,
 168     guint16 fec_seq_base, gboolean fec_mask_long)
 169 {
 170   gint seq_delta = gst_rtp_buffer_compare_seqnum (fec_seq_base, seq);
 171   if (seq_delta >= 0
 172       && seq_delta <= RTP_ULPFEC_SEQ_BASE_OFFSET_MAX (fec_mask_long)) {
 173     return ONE_64BIT << (RTP_ULPFEC_SEQ_BASE_OFFSET_MAX (TRUE) - seq_delta);
 174   }
 175   return 0;
 176 }
 177
 178 gboolean
 179 rtp_ulpfec_mask_is_long (guint64 mask)
 180 {
 181   return (mask & 0xffffffff) ? TRUE : FALSE;
 182 }
 183
 184 gboolean
 185 rtp_ulpfec_buffer_is_valid (GstRTPBuffer * rtp)
 186 {
 187   guint payload_len = gst_rtp_buffer_get_payload_len (rtp);
 188   RtpUlpFecHeader *fec_hdr;
 189   guint fec_hdrs_len;
 190   guint fec_packet_len;
 191
 192   if (payload_len < sizeof (RtpUlpFecHeader))
 193     goto toosmall;
 194
 195   fec_hdr = rtp_ulpfec_buffer_get_fechdr (rtp);
 196   if (fec_hdr->E)
 197     goto invalidcontent;
 198
 199   fec_hdrs_len = rtp_ulpfec_get_headers_len (fec_hdr->L);
 200   if (payload_len < fec_hdrs_len)
 201     goto toosmall;
 202
 203   fec_packet_len = fec_hdrs_len + rtp_ulpfec_hdr_get_protection_len (fec_hdr);
 204   if (fec_packet_len != payload_len)
 205     goto lengthmismatch;
 206
 207   return TRUE;
 208 toosmall:
 209   GST_WARNING ("FEC packet too small");
 210   return FALSE;
 211
 212 lengthmismatch:
 213   GST_WARNING ("invalid FEC packet (declared length %u, real length %u)",
 214       fec_packet_len, payload_len);
 215   return FALSE;
 216
 217 invalidcontent:
 218   GST_WARNING ("FEC Header contains invalid fields: %u", fec_hdr->E);
 219   return FALSE;
 220 }
 221
 222
 223 void
 224 rtp_buffer_to_ulpfec_bitstring (GstRTPBuffer * rtp, GArray * dst_arr,
 225     gboolean fec_buffer, gboolean fec_mask_long)
 226 {
 227   if (G_UNLIKELY (fec_buffer)) {
 228     guint payload_len = gst_rtp_buffer_get_payload_len (rtp);
 229     g_array_set_size (dst_arr, MAX (payload_len, dst_arr->len));
 230     memcpy (dst_arr->data, gst_rtp_buffer_get_payload (rtp), payload_len);
 231   } else {
 232     const guint8 *src = rtp->data[0];
 233     guint len = gst_rtp_buffer_get_packet_len (rtp) - MIN_RTP_HEADER_LEN;
 234     guint dst_offset = rtp_ulpfec_get_headers_len (fec_mask_long);
 235     guint src_offset = MIN_RTP_HEADER_LEN;
 236     guint8 *dst;
 237
 238     g_array_set_size (dst_arr, MAX (dst_offset + len, dst_arr->len));
 239     dst = (guint8 *) dst_arr->data;
 240
 241     *((guint64 *) dst) ^= *((const guint64 *) src);
 242     ((RtpUlpFecHeader *) dst)->len ^= g_htons (len);
 243     _xor_mem (dst + dst_offset, src + src_offset, len);
 244   }
 245 }
 246
 247 GstBuffer *
 248 rtp_ulpfec_bitstring_to_media_rtp_buffer (GArray * arr,
 249     gboolean fec_mask_long, guint32 ssrc, guint16 seq)
 250 {
 251   guint fec_hdrs_len = rtp_ulpfec_get_headers_len (fec_mask_long);
 252   guint payload_len =
 253       fec_hdr_get_packets_len_recovery ((RtpUlpFecHeader *) arr->data);
 254   GstMapInfo ret_info = GST_MAP_INFO_INIT;
 255   GstMemory *ret_mem;
 256   GstBuffer *ret;
 257
 258   if (payload_len > arr->len - fec_hdrs_len)
 259     return NULL;                // Not enough data
 260
 261   ret_mem = gst_allocator_alloc (NULL, MIN_RTP_HEADER_LEN + payload_len, NULL);
 262   gst_memory_map (ret_mem, &ret_info, GST_MAP_READWRITE);
 263
 264   /* Filling 12 bytes of RTP header */
 265   *((guint64 *) ret_info.data) = *((guint64 *) arr->data);
 266   ((RtpHeader *) ret_info.data)->version = 2;
 267   ((RtpHeader *) ret_info.data)->seq = g_htons (seq);
 268   ((RtpHeader *) ret_info.data)->ssrc = g_htonl (ssrc);
 269   /* Filling payload */
 270   memcpy (ret_info.data + MIN_RTP_HEADER_LEN,
 271       arr->data + fec_hdrs_len, payload_len);
 272
 273   gst_memory_unmap (ret_mem, &ret_info);
 274   ret = gst_buffer_new ();
 275   gst_buffer_append_memory (ret, ret_mem);
 276   return ret;
 277 }
 278
 279 GstBuffer *
 280 rtp_ulpfec_bitstring_to_fec_rtp_buffer (GArray * arr,
 281     guint16 seq_base, gboolean fec_mask_long, guint64 fec_mask,
 282     gboolean marker, guint8 pt, guint16 seq, guint32 timestamp, guint32 ssrc)
 283 {
 284   GstRTPBuffer rtp = GST_RTP_BUFFER_INIT;
 285   GstBuffer *ret;
 286
 287   /* Filling FEC headers */
 288   {
 289     RtpUlpFecHeader *hdr = (RtpUlpFecHeader *) arr->data;
 290     RtpUlpFecLevelHeader *lvlhdr;
 291     hdr->E = 0;
 292     hdr->L = fec_mask_long;
 293     hdr->seq = g_htons (seq_base);
 294
 295     lvlhdr = fec_hdr_get_level_hdr (hdr);
 296     fec_level_hdr_set_protection_len (lvlhdr,
 297         arr->len - rtp_ulpfec_get_headers_len (fec_mask_long));
 298     fec_level_hdr_set_mask (lvlhdr, fec_mask_long, fec_mask);
 299   }
 300
 301   /* Filling RTP header, copying payload */
 302   ret = gst_rtp_buffer_new_allocate (arr->len, 0, 0);
 303   if (!gst_rtp_buffer_map (ret, GST_MAP_READWRITE, &rtp))
 304     g_assert_not_reached ();
 305
 306   gst_rtp_buffer_set_marker (&rtp, marker);
 307   gst_rtp_buffer_set_payload_type (&rtp, pt);
 308   gst_rtp_buffer_set_seq (&rtp, seq);
 309   gst_rtp_buffer_set_timestamp (&rtp, timestamp);
 310   gst_rtp_buffer_set_ssrc (&rtp, ssrc);
 311
 312   memcpy (gst_rtp_buffer_get_payload (&rtp), arr->data, arr->len);
 313
 314   gst_rtp_buffer_unmap (&rtp);
 315
 316   return ret;
 317 }
 318
 319 /**
 320  * rtp_ulpfec_map_info_map:
 321  * @buffer: (transfer: full) #GstBuffer
 322  * @info: #RtpUlpFecMapInfo
 323  *
 324  * Maps the contents of @buffer into @info. If @buffer made of many #GstMemory
 325  * objects, merges them together to create a new buffer made of single
 326  * continious #GstMemory.
 327  *
 328  * Returns: %TRUE if @buffer could be mapped
 329  **/
 330 gboolean
 331 rtp_ulpfec_map_info_map (GstBuffer * buffer, RtpUlpFecMapInfo * info)
 332 {
 333   /* We need to make sure we are working with continious memory chunk.
 334    * If not merge all memories together */
 335   if (gst_buffer_n_memory (buffer) > 1) {
 336     GstBuffer *new_buffer = gst_buffer_new ();
 337     GstMemory *mem = gst_buffer_get_all_memory (buffer);
 338     gst_buffer_append_memory (new_buffer, mem);
 339
 340     /* We supposed to own the old buffer, but we don't use it here, so unref */
 341     gst_buffer_unref (buffer);
 342     buffer = new_buffer;
 343   }
 344
 345   if (!gst_rtp_buffer_map (buffer,
 346           GST_MAP_READ | GST_RTP_BUFFER_MAP_FLAG_SKIP_PADDING, &info->rtp)) {
 347     /* info->rtp.buffer = NULL is an indication for rtp_ulpfec_map_info_unmap()
 348      * that mapping has failed */
 349     g_assert (NULL == info->rtp.buffer);
 350     gst_buffer_unref (buffer);
 351     return FALSE;
 352   }
 353   return TRUE;
 354 }
 355
 356 /**
 357  * rtp_ulpfec_map_info_unmap:
 358  * @info: #RtpUlpFecMapInfo
 359  *
 360  * Unmap @info previously mapped with rtp_ulpfec_map_info_map() and unrefs the
 361  * buffer. For convinience can even be called even if rtp_ulpfec_map_info_map
 362  * returned FALSE
 363  **/
 364 void
 365 rtp_ulpfec_map_info_unmap (RtpUlpFecMapInfo * info)
 366 {
 367   GstBuffer *buffer = info->rtp.buffer;
 368
 369   if (buffer) {
 370     gst_rtp_buffer_unmap (&info->rtp);
 371     gst_buffer_unref (buffer);
 372   }
 373 }
 374
 375 #ifndef GST_DISABLE_GST_DEBUG
 376 void
 377 rtp_ulpfec_log_rtppacket (GstDebugCategory * cat, GstDebugLevel level,
 378     gpointer object, const gchar * name, GstRTPBuffer * rtp)
 379 {
 380   guint seq;
 381   guint ssrc;
 382   guint timestamp;
 383   guint pt;
 384
 385   if (level > gst_debug_category_get_threshold (cat))
 386     return;
 387
 388   seq = gst_rtp_buffer_get_seq (rtp);
 389   ssrc = gst_rtp_buffer_get_ssrc (rtp);
 390   timestamp = gst_rtp_buffer_get_timestamp (rtp);
 391   pt = gst_rtp_buffer_get_payload_type (rtp);
 392
 393   GST_CAT_LEVEL_LOG (cat, level, object,
 394       "%-22s: [%c%c%c%c] ssrc=0x%08x pt=%u tstamp=%u seq=%u size=%u(%u,%u)",
 395       name,
 396       gst_rtp_buffer_get_marker (rtp) ? 'M' : ' ',
 397       gst_rtp_buffer_get_extension (rtp) ? 'X' : ' ',
 398       gst_rtp_buffer_get_padding (rtp) ? 'P' : ' ',
 399       gst_rtp_buffer_get_csrc_count (rtp) > 0 ? 'C' : ' ',
 400       ssrc, pt, timestamp, seq,
 401       gst_rtp_buffer_get_packet_len (rtp),
 402       gst_rtp_buffer_get_packet_len (rtp) - MIN_RTP_HEADER_LEN,
 403       gst_rtp_buffer_get_payload_len (rtp));
 404 }
 405 #endif /* GST_DISABLE_GST_DEBUG */
 406
 407 #ifndef GST_DISABLE_GST_DEBUG
 408 void
 409 rtp_ulpfec_log_fec_packet (GstDebugCategory * cat, GstDebugLevel level,
 410     gpointer object, GstRTPBuffer * fecrtp)
 411 {
 412   RtpUlpFecHeader *fec_hdr;
 413   RtpUlpFecLevelHeader *fec_level_hdr;
 414
 415   if (level > gst_debug_category_get_threshold (cat))
 416     return;
 417
 418   fec_hdr = gst_rtp_buffer_get_payload (fecrtp);
 419   GST_CAT_LEVEL_LOG (cat, level, object,
 420       "%-22s: [%c%c%c%c%c%c] pt=%u tstamp=%u seq=%u recovery_len=%u",
 421       "fec header",
 422       fec_hdr->E ? 'E' : ' ',
 423       fec_hdr->L ? 'L' : ' ',
 424       fec_hdr->P ? 'P' : ' ',
 425       fec_hdr->X ? 'X' : ' ',
 426       fec_hdr->CC ? 'C' : ' ',
 427       fec_hdr->M ? 'M' : ' ',
 428       fec_hdr->pt,
 429       fec_hdr_get_timestamp_recovery (fec_hdr),
 430       fec_hdr_get_seq_base (fec_hdr, TRUE,
 431           gst_rtp_buffer_get_seq (fecrtp)),
 432       fec_hdr_get_packets_len_recovery (fec_hdr));
 433
 434   fec_level_hdr = fec_hdr_get_level_hdr (fec_hdr);
 435   GST_CAT_LEVEL_LOG (cat, level, object,
 436       "%-22s: protection_len=%u mask=0x%012" G_GINT64_MODIFIER "x",
 437       "fec level header",
 438       g_ntohs (fec_level_hdr->protection_len),
 439       fec_level_hdr_get_mask (fec_level_hdr, fec_hdr->L));
 440 }
 441 #endif /* GST_DISABLE_GST_DEBUG */