gst/audioparsers/gstsbcparse.c

   1 /* GStreamer SBC audio parser
   2  * Copyright (C) 2012 Collabora Ltd. <tim.muller@collabora.co.uk>
   3  *
   4  * This library is free software; you can redistribute it and/or
   5  * modify it under the terms of the GNU Library General Public
   6  * License as published by the Free Software Foundation; either
   7  * version 2 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  * Library General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU Library General Public
  15  * License along with this library; if not, write to the
  16  * Free Software Foundation, Inc., 51 Franklin St, Fifth Floor,
  17  * Boston, MA 02110-1301, USA.
  18  */
  19
  20 #ifdef HAVE_CONFIG_H
  21 #include "config.h"
  22 #endif
  23
  24 /**
  25  * SECTION:element-sbcparse
  26  * @see_also: sbcdec, sbcenc
  27  *
  28  * The sbcparse element will parse a bluetooth SBC audio stream into
  29  * frames and timestamp them properly.
  30  *
  31  * Since: 1.2.0
  32  */
  33
  34 #ifdef HAVE_CONFIG_H
  35 #include "config.h"
  36 #endif
  37
  38 #include "gstsbcparse.h"
  39
  40 #include <string.h>
  41 #include <gst/tag/tag.h>
  42 #include <gst/audio/audio.h>
  43
  44 #include <gst/base/gstbitreader.h>
  45 #include <gst/base/gstbytereader.h>
  46
  47 #define SBC_SYNCBYTE 0x9C
  48
  49 GST_DEBUG_CATEGORY_STATIC (sbcparse_debug);
  50 #define GST_CAT_DEFAULT sbcparse_debug
  51
  52 static GstStaticPadTemplate src_factory = GST_STATIC_PAD_TEMPLATE ("src",
  53     GST_PAD_SRC,
  54     GST_PAD_ALWAYS,
  55     GST_STATIC_CAPS ("audio/x-sbc, parsed = (boolean) true, "
  56         "channels = (int) [ 1, 2 ], "
  57         "rate = (int) { 16000, 32000, 44100, 48000 }")
  58     );
  59
  60 static GstStaticPadTemplate sink_factory = GST_STATIC_PAD_TEMPLATE ("sink",
  61     GST_PAD_SINK,
  62     GST_PAD_ALWAYS,
  63     GST_STATIC_CAPS ("audio/x-sbc")
  64     );
  65
  66 static gboolean gst_sbc_parse_start (GstBaseParse * parse);
  67 static gboolean gst_sbc_parse_stop (GstBaseParse * parse);
  68 static GstFlowReturn gst_sbc_parse_handle_frame (GstBaseParse * parse,
  69     GstBaseParseFrame * frame, gint * skipsize);
  70 static GstCaps *gst_sbc_parse_get_sink_caps (GstBaseParse * parse,
  71     GstCaps * filter);
  72
  73 static guint8 gst_sbc_calculate_crc8 (const guint8 * data, gint bits_crc);
  74 static gsize gst_sbc_calc_framelen (guint subbands, GstSbcChannelMode ch_mode,
  75     guint blocks, guint bitpool);
  76 static gsize gst_sbc_parse_header (const guint8 * data, guint * rate,
  77     guint * n_blocks, GstSbcChannelMode * ch_mode,
  78     GstSbcAllocationMethod * alloc_method, guint * n_subbands, guint * bitpool);
  79
  80 #define parent_class gst_sbc_parse_parent_class
  81 G_DEFINE_TYPE (GstSbcParse, gst_sbc_parse, GST_TYPE_BASE_PARSE);
  82
  83 static void
  84 gst_sbc_parse_class_init (GstSbcParseClass * klass)
  85 {
  86   GstBaseParseClass *baseparse_class = GST_BASE_PARSE_CLASS (klass);
  87   GstElementClass *element_class = GST_ELEMENT_CLASS (klass);
  88
  89   GST_DEBUG_CATEGORY_INIT (sbcparse_debug, "sbcparse", 0, "SBC audio parser");
  90
  91   baseparse_class->start = GST_DEBUG_FUNCPTR (gst_sbc_parse_start);
  92   baseparse_class->stop = GST_DEBUG_FUNCPTR (gst_sbc_parse_stop);
  93
  94   baseparse_class->handle_frame =
  95       GST_DEBUG_FUNCPTR (gst_sbc_parse_handle_frame);
  96   baseparse_class->get_sink_caps =
  97       GST_DEBUG_FUNCPTR (gst_sbc_parse_get_sink_caps);
  98
  99   gst_element_class_add_pad_template (element_class,
 100       gst_static_pad_template_get (&src_factory));
 101   gst_element_class_add_pad_template (element_class,
 102       gst_static_pad_template_get (&sink_factory));
 103
 104   gst_element_class_set_static_metadata (element_class, "SBC audio parser",
 105       "Codec/Parser/Audio", "Parses an SBC bluetooth audio stream",
 106       "Tim-Philipp Müller <tim.muller@collabora.co.uk>");
 107 }
 108
 109 static void
 110 gst_sbc_parse_reset (GstSbcParse * sbcparse)
 111 {
 112   sbcparse->alloc_method = GST_SBC_ALLOCATION_METHOD_INVALID;
 113   sbcparse->ch_mode = GST_SBC_CHANNEL_MODE_INVALID;
 114   sbcparse->rate = -1;
 115   sbcparse->n_blocks = -1;
 116   sbcparse->n_subbands = -1;
 117   sbcparse->bitpool = -1;
 118 }
 119
 120 static void
 121 gst_sbc_parse_init (GstSbcParse * sbcparse)
 122 {
 123   gst_sbc_parse_reset (sbcparse);
 124 }
 125
 126 static gboolean
 127 gst_sbc_parse_start (GstBaseParse * parse)
 128 {
 129   gst_base_parse_set_min_frame_size (parse,
 130       gst_sbc_calc_framelen (4, GST_SBC_CHANNEL_MODE_MONO, 4, 2));
 131
 132   gst_base_parse_set_has_timing_info (parse, FALSE);
 133
 134   gst_base_parse_set_syncable (parse, TRUE);
 135
 136   return TRUE;
 137 }
 138
 139 static gboolean
 140 gst_sbc_parse_stop (GstBaseParse * parse)
 141 {
 142   gst_sbc_parse_reset (GST_SBC_PARSE (parse));
 143   return TRUE;
 144 }
 145
 146 static const gchar *
 147 gst_sbc_channel_mode_get_name (GstSbcChannelMode ch_mode)
 148 {
 149   switch (ch_mode) {
 150     case GST_SBC_CHANNEL_MODE_MONO:
 151       return "mono";
 152     case GST_SBC_CHANNEL_MODE_DUAL:
 153       return "dual";
 154     case GST_SBC_CHANNEL_MODE_STEREO:
 155       return "stereo";
 156     case GST_SBC_CHANNEL_MODE_JOINT_STEREO:
 157       return "joint";
 158     default:
 159       break;
 160   }
 161   return "invalid";
 162 }
 163
 164 static const gchar *
 165 gst_sbc_allocation_method_get_name (GstSbcAllocationMethod alloc_method)
 166 {
 167   switch (alloc_method) {
 168     case GST_SBC_ALLOCATION_METHOD_SNR:
 169       return "snr";
 170     case GST_SBC_ALLOCATION_METHOD_LOUDNESS:
 171       return "loudness";
 172     default:
 173       break;
 174   }
 175   return "invalid";
 176 }
 177
 178 static GstFlowReturn
 179 gst_sbc_parse_handle_frame (GstBaseParse * parse, GstBaseParseFrame * frame,
 180     gint * skipsize)
 181 {
 182   GstSbcParse *sbcparse = GST_SBC_PARSE (parse);
 183   GstSbcAllocationMethod alloc_method = GST_SBC_ALLOCATION_METHOD_INVALID;
 184   GstSbcChannelMode ch_mode = GST_SBC_CHANNEL_MODE_INVALID;
 185   GstMapInfo map;
 186   guint rate = 0, n_blocks = 0, n_subbands = 0, bitpool = 0;
 187   gsize frame_len, next_len;
 188   gint i, max_frames;
 189
 190   gst_buffer_map (frame->buffer, &map, GST_MAP_READ);
 191
 192   g_assert (map.size >= 6);
 193
 194   frame_len = gst_sbc_parse_header (map.data, &rate, &n_blocks, &ch_mode,
 195       &alloc_method, &n_subbands, &bitpool);
 196
 197   GST_LOG_OBJECT (parse, "frame_len: %u", (guint) frame_len);
 198
 199   if (frame_len == 0)
 200     goto resync;
 201
 202   if (sbcparse->alloc_method != alloc_method
 203       || sbcparse->ch_mode != ch_mode
 204       || sbcparse->rate != rate
 205       || sbcparse->n_blocks != n_blocks
 206       || sbcparse->n_subbands != n_subbands || sbcparse->bitpool != bitpool) {
 207     guint avg_bitrate;
 208     GstCaps *caps;
 209
 210     /* FIXME: do all of these need to be in the caps? */
 211     caps = gst_caps_new_simple ("audio/x-sbc", "rate", G_TYPE_INT, rate,
 212         "channels", G_TYPE_INT, (ch_mode == GST_SBC_CHANNEL_MODE_MONO) ? 1 : 2,
 213         "channel-mode", G_TYPE_STRING, gst_sbc_channel_mode_get_name (ch_mode),
 214         "blocks", G_TYPE_INT, n_blocks, "subbands", G_TYPE_INT, n_subbands,
 215         "allocation-method", G_TYPE_STRING,
 216         gst_sbc_allocation_method_get_name (alloc_method),
 217         "bitpool", G_TYPE_INT, bitpool, "parsed", G_TYPE_BOOLEAN, TRUE, NULL);
 218
 219     GST_INFO_OBJECT (sbcparse, "caps changed to %" GST_PTR_FORMAT, caps);
 220
 221     gst_pad_push_event (GST_BASE_PARSE_SRC_PAD (sbcparse),
 222         gst_event_new_caps (caps));
 223
 224     avg_bitrate = (8 * frame_len * rate) / (n_subbands * n_blocks);
 225     gst_base_parse_set_average_bitrate (parse, avg_bitrate);
 226
 227     gst_base_parse_set_frame_rate (parse, rate, n_subbands * n_blocks, 0, 0);
 228
 229     sbcparse->alloc_method = alloc_method;
 230     sbcparse->ch_mode = ch_mode;
 231     sbcparse->rate = rate;
 232     sbcparse->n_blocks = n_blocks;
 233     sbcparse->n_subbands = n_subbands;
 234     sbcparse->bitpool = bitpool;
 235
 236     gst_caps_unref (caps);
 237   }
 238
 239   if (frame_len > map.size)
 240     goto need_more_data;
 241
 242   GST_BUFFER_OFFSET (frame->buffer) = GST_BUFFER_OFFSET_NONE;
 243   GST_BUFFER_OFFSET_END (frame->buffer) = GST_BUFFER_OFFSET_NONE;
 244
 245   /* completely arbitrary limit, we only process data we already have,
 246    * so we aren't introducing latency here */
 247   max_frames = MIN (map.size / frame_len, n_blocks * n_subbands * 5);
 248   GST_LOG_OBJECT (sbcparse, "parsing up to %d frames", max_frames);
 249
 250   for (i = 1; i < max_frames; ++i) {
 251     next_len = gst_sbc_parse_header (map.data + (i * frame_len), &rate,
 252         &n_blocks, &ch_mode, &alloc_method, &n_subbands, &bitpool);
 253
 254     if (next_len != frame_len || sbcparse->alloc_method != alloc_method ||
 255         sbcparse->ch_mode != ch_mode || sbcparse->rate != rate ||
 256         sbcparse->n_blocks != n_blocks || sbcparse->n_subbands != n_subbands ||
 257         sbcparse->bitpool != bitpool) {
 258       break;
 259     }
 260   }
 261   GST_LOG_OBJECT (sbcparse, "packing %d SBC frames into next output buffer", i);
 262
 263   /* Note: local n_subbands and n_blocks variables might be tainted if we
 264    * bailed out of the loop above because of a header configuration mismatch */
 265   gst_base_parse_set_frame_rate (parse, rate,
 266       sbcparse->n_subbands * sbcparse->n_blocks * i, 0, 0);
 267
 268   gst_buffer_unmap (frame->buffer, &map);
 269   return gst_base_parse_finish_frame (parse, frame, i * frame_len);
 270
 271 resync:
 272   {
 273     const guint8 *possible_sync;
 274
 275     GST_DEBUG_OBJECT (parse, "no sync, resyncing");
 276
 277     possible_sync = memchr (map.data, SBC_SYNCBYTE, map.size);
 278
 279     if (possible_sync != NULL)
 280       *skipsize = (gint) (possible_sync - map.data);
 281     else
 282       *skipsize = map.size;
 283
 284     gst_buffer_unmap (frame->buffer, &map);
 285
 286     /* we could optimise things here by looping over the data and checking
 287      * whether the sync is good or not instead of handing control back to
 288      * the base class just to be called again */
 289     return GST_FLOW_OK;
 290   }
 291 need_more_data:
 292   {
 293     GST_LOG_OBJECT (parse,
 294         "need %" G_GSIZE_FORMAT " bytes, but only have %" G_GSIZE_FORMAT,
 295         frame_len, map.size);
 296     gst_base_parse_set_min_frame_size (parse, frame_len);
 297     gst_buffer_unmap (frame->buffer, &map);
 298     return GST_FLOW_OK;
 299   }
 300 }
 301
 302 static void
 303 remove_fields (GstCaps * caps)
 304 {
 305   guint i, n;
 306
 307   n = gst_caps_get_size (caps);
 308   for (i = 0; i < n; i++) {
 309     GstStructure *s = gst_caps_get_structure (caps, i);
 310
 311     gst_structure_remove_field (s, "parsed");
 312   }
 313 }
 314
 315 static GstCaps *
 316 gst_sbc_parse_get_sink_caps (GstBaseParse * parse, GstCaps * filter)
 317 {
 318   GstCaps *peercaps, *templ;
 319   GstCaps *res;
 320
 321   templ = gst_pad_get_pad_template_caps (GST_BASE_PARSE_SINK_PAD (parse));
 322   peercaps = gst_pad_peer_query_caps (GST_BASE_PARSE_SRC_PAD (parse), filter);
 323
 324   if (peercaps) {
 325     /* Remove the parsed field */
 326     peercaps = gst_caps_make_writable (peercaps);
 327     remove_fields (peercaps);
 328
 329     res = gst_caps_intersect_full (peercaps, templ, GST_CAPS_INTERSECT_FIRST);
 330     gst_caps_unref (peercaps);
 331     res = gst_caps_make_writable (res);
 332
 333     /* Append the template caps because we still want to accept
 334      * caps without any fields in the case upstream does not
 335      * know anything.
 336      */
 337     gst_caps_append (res, templ);
 338   } else {
 339     res = templ;
 340   }
 341
 342   if (filter) {
 343     GstCaps *intersection;
 344
 345     intersection =
 346         gst_caps_intersect_full (filter, res, GST_CAPS_INTERSECT_FIRST);
 347     gst_caps_unref (res);
 348     res = intersection;
 349   }
 350
 351   return res;
 352 }
 353
 354 static const guint8 crc_table[256] = {
 355   0x00, 0x1D, 0x3A, 0x27, 0x74, 0x69, 0x4E, 0x53,
 356   0xE8, 0xF5, 0xD2, 0xCF, 0x9C, 0x81, 0xA6, 0xBB,
 357   0xCD, 0xD0, 0xF7, 0xEA, 0xB9, 0xA4, 0x83, 0x9E,
 358   0x25, 0x38, 0x1F, 0x02, 0x51, 0x4C, 0x6B, 0x76,
 359   0x87, 0x9A, 0xBD, 0xA0, 0xF3, 0xEE, 0xC9, 0xD4,
 360   0x6F, 0x72, 0x55, 0x48, 0x1B, 0x06, 0x21, 0x3C,
 361   0x4A, 0x57, 0x70, 0x6D, 0x3E, 0x23, 0x04, 0x19,
 362   0xA2, 0xBF, 0x98, 0x85, 0xD6, 0xCB, 0xEC, 0xF1,
 363   0x13, 0x0E, 0x29, 0x34, 0x67, 0x7A, 0x5D, 0x40,
 364   0xFB, 0xE6, 0xC1, 0xDC, 0x8F, 0x92, 0xB5, 0xA8,
 365   0xDE, 0xC3, 0xE4, 0xF9, 0xAA, 0xB7, 0x90, 0x8D,
 366   0x36, 0x2B, 0x0C, 0x11, 0x42, 0x5F, 0x78, 0x65,
 367   0x94, 0x89, 0xAE, 0xB3, 0xE0, 0xFD, 0xDA, 0xC7,
 368   0x7C, 0x61, 0x46, 0x5B, 0x08, 0x15, 0x32, 0x2F,
 369   0x59, 0x44, 0x63, 0x7E, 0x2D, 0x30, 0x17, 0x0A,
 370   0xB1, 0xAC, 0x8B, 0x96, 0xC5, 0xD8, 0xFF, 0xE2,
 371   0x26, 0x3B, 0x1C, 0x01, 0x52, 0x4F, 0x68, 0x75,
 372   0xCE, 0xD3, 0xF4, 0xE9, 0xBA, 0xA7, 0x80, 0x9D,
 373   0xEB, 0xF6, 0xD1, 0xCC, 0x9F, 0x82, 0xA5, 0xB8,
 374   0x03, 0x1E, 0x39, 0x24, 0x77, 0x6A, 0x4D, 0x50,
 375   0xA1, 0xBC, 0x9B, 0x86, 0xD5, 0xC8, 0xEF, 0xF2,
 376   0x49, 0x54, 0x73, 0x6E, 0x3D, 0x20, 0x07, 0x1A,
 377   0x6C, 0x71, 0x56, 0x4B, 0x18, 0x05, 0x22, 0x3F,
 378   0x84, 0x99, 0xBE, 0xA3, 0xF0, 0xED, 0xCA, 0xD7,
 379   0x35, 0x28, 0x0F, 0x12, 0x41, 0x5C, 0x7B, 0x66,
 380   0xDD, 0xC0, 0xE7, 0xFA, 0xA9, 0xB4, 0x93, 0x8E,
 381   0xF8, 0xE5, 0xC2, 0xDF, 0x8C, 0x91, 0xB6, 0xAB,
 382   0x10, 0x0D, 0x2A, 0x37, 0x64, 0x79, 0x5E, 0x43,
 383   0xB2, 0xAF, 0x88, 0x95, 0xC6, 0xDB, 0xFC, 0xE1,
 384   0x5A, 0x47, 0x60, 0x7D, 0x2E, 0x33, 0x14, 0x09,
 385   0x7F, 0x62, 0x45, 0x58, 0x0B, 0x16, 0x31, 0x2C,
 386   0x97, 0x8A, 0xAD, 0xB0, 0xE3, 0xFE, 0xD9, 0xC4
 387 };
 388
 389 static guint8
 390 gst_sbc_calculate_crc8 (const guint8 * data, gint crc_bits)
 391 {
 392   guint8 crc = 0x0f;
 393   guint8 octet;
 394
 395   while (crc_bits >= 8) {
 396     crc = crc_table[crc ^ *data];
 397     crc_bits -= 8;
 398     ++data;
 399   }
 400
 401   octet = *data;
 402   while (crc_bits > 0) {
 403     gchar bit = ((octet ^ crc) & 0x80) >> 7;
 404
 405     crc = ((crc & 0x7f) << 1) ^ (bit ? 0x1d : 0);
 406
 407     octet = octet << 1;
 408     --crc_bits;
 409   }
 410
 411   return crc;
 412 }
 413
 414 static gsize
 415 gst_sbc_calc_framelen (guint subbands, GstSbcChannelMode ch_mode,
 416     guint blocks, guint bitpool)
 417 {
 418   switch (ch_mode) {
 419     case GST_SBC_CHANNEL_MODE_MONO:
 420       return 4 + (subbands * 1) / 2 + (blocks * 1 * bitpool) / 8;
 421     case GST_SBC_CHANNEL_MODE_DUAL:
 422       return 4 + (subbands * 2) / 2 + (blocks * 2 * bitpool) / 8;
 423     case GST_SBC_CHANNEL_MODE_STEREO:
 424       return 4 + (subbands * 2) / 2 + (blocks * bitpool) / 8;
 425     case GST_SBC_CHANNEL_MODE_JOINT_STEREO:
 426       return 4 + (subbands * 2) / 2 + (subbands + blocks * bitpool) / 8;
 427     default:
 428       break;
 429   }
 430
 431   g_return_val_if_reached (0);
 432 }
 433
 434 static gsize
 435 gst_sbc_parse_header (const guint8 * data, guint * rate, guint * n_blocks,
 436     GstSbcChannelMode * ch_mode, GstSbcAllocationMethod * alloc_method,
 437     guint * n_subbands, guint * bitpool)
 438 {
 439   static const guint16 sbc_rates[4] = { 16000, 32000, 44100, 48000 };
 440   static const guint8 sbc_blocks[4] = { 4, 8, 12, 16 };
 441   guint8 crc_data[2 + 1 + 8], crc_bits, i;
 442
 443   GST_MEMDUMP ("header", data, 8);
 444
 445   if (data[0] != SBC_SYNCBYTE)
 446     return 0;
 447
 448   *rate = sbc_rates[(data[1] >> 6) & 0x03];
 449   *n_blocks = sbc_blocks[(data[1] >> 4) & 0x03];
 450   *ch_mode = (GstSbcChannelMode) ((data[1] >> 2) & 0x03);
 451   *alloc_method = (data[1] >> 1) & 0x01;
 452   *n_subbands = (data[1] & 0x01) ? 8 : 4;
 453   *bitpool = data[2];
 454
 455   GST_TRACE ("rate=%u, n_blocks=%u, ch_mode=%u, alloc_method=%u, "
 456       "n_subbands=%u, bitpool=%u", *rate, *n_blocks, *ch_mode, *alloc_method,
 457       *n_subbands, *bitpool);
 458
 459   if (*bitpool < 2)
 460     return 0;
 461
 462   /* check CRC */
 463   crc_data[0] = data[1];
 464   crc_data[1] = data[2];
 465   crc_bits = 16;
 466
 467   /* joint flags and RFA */
 468   if (*ch_mode == GST_SBC_CHANNEL_MODE_JOINT_STEREO)
 469     crc_bits += *n_subbands;
 470
 471   /* scale factors */
 472   if (*ch_mode == GST_SBC_CHANNEL_MODE_MONO)
 473     crc_bits += *n_subbands * 1 * 4;
 474   else
 475     crc_bits += *n_subbands * 2 * 4;
 476
 477   for (i = 16; i < crc_bits; i += 8) {
 478     crc_data[i / 8] = data[1 + (i / 8) + 1];
 479   }
 480
 481   if (i > crc_bits) {
 482     crc_data[(i / 8) - 1] &= 0xF0;
 483   }
 484
 485   GST_MEMDUMP ("crc bytes", crc_data, GST_ROUND_UP_8 (crc_bits) / 8);
 486   if (gst_sbc_calculate_crc8 (crc_data, crc_bits) != data[3]) {
 487     GST_LOG ("header CRC check failed, bits=%u, got 0x%02x, expected 0x%02x",
 488         crc_bits, gst_sbc_calculate_crc8 (crc_data, crc_bits), data[3]);
 489     return 0;
 490   }
 491
 492   return gst_sbc_calc_framelen (*n_subbands, *ch_mode, *n_blocks, *bitpool);
 493 }