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;
 184   GstSbcChannelMode ch_mode;
 185   GstMapInfo map;
 186   guint rate, n_blocks, n_subbands, bitpool;
 187   gsize frame_len;
 188
 189   gst_buffer_map (frame->buffer, &map, GST_MAP_READ);
 190
 191   g_assert (map.size >= 6);
 192
 193   frame_len = gst_sbc_parse_header (map.data, &rate, &n_blocks, &ch_mode,
 194       &alloc_method, &n_subbands, &bitpool);
 195
 196   GST_LOG_OBJECT (parse, "frame_len: %u", (guint) frame_len);
 197
 198   if (frame_len == 0)
 199     goto resync;
 200
 201   if (sbcparse->alloc_method != alloc_method
 202       || sbcparse->ch_mode != ch_mode
 203       || sbcparse->rate != rate
 204       || sbcparse->n_blocks != n_blocks
 205       || sbcparse->n_subbands != n_subbands || sbcparse->bitpool != bitpool) {
 206     guint avg_bitrate;
 207     GstCaps *caps;
 208
 209     /* FIXME: do all of these need to be in the caps? */
 210     caps = gst_caps_new_simple ("audio/x-sbc", "rate", G_TYPE_INT, rate,
 211         "channels", G_TYPE_INT, (ch_mode == GST_SBC_CHANNEL_MODE_MONO) ? 1 : 2,
 212         "channel-mode", G_TYPE_STRING, gst_sbc_channel_mode_get_name (ch_mode),
 213         "blocks", G_TYPE_INT, n_blocks, "subbands", G_TYPE_INT, n_subbands,
 214         "allocation-method", G_TYPE_STRING,
 215         gst_sbc_allocation_method_get_name (alloc_method),
 216         "bitpool", G_TYPE_INT, bitpool, "parsed", G_TYPE_BOOLEAN, TRUE, NULL);
 217
 218     GST_INFO_OBJECT (sbcparse, "caps changed to %" GST_PTR_FORMAT, caps);
 219
 220     gst_pad_push_event (GST_BASE_PARSE_SRC_PAD (sbcparse),
 221         gst_event_new_caps (caps));
 222
 223     avg_bitrate = (8 * frame_len * rate) / (n_subbands * n_blocks);
 224     gst_base_parse_set_average_bitrate (parse, avg_bitrate);
 225
 226     gst_base_parse_set_frame_rate (parse, rate, n_subbands * n_blocks, 0, 0);
 227
 228     sbcparse->alloc_method = alloc_method;
 229     sbcparse->ch_mode = ch_mode;
 230     sbcparse->rate = rate;
 231     sbcparse->n_blocks = n_blocks;
 232     sbcparse->n_subbands = n_subbands;
 233     sbcparse->bitpool = bitpool;
 234
 235     gst_caps_unref (caps);
 236   }
 237
 238   if (frame_len > map.size)
 239     goto need_more_data;
 240
 241   GST_BUFFER_OFFSET (frame->buffer) = GST_BUFFER_OFFSET_NONE;
 242   GST_BUFFER_OFFSET_END (frame->buffer) = GST_BUFFER_OFFSET_NONE;
 243
 244   gst_buffer_unmap (frame->buffer, &map);
 245   return gst_base_parse_finish_frame (parse, frame, frame_len);
 246
 247 resync:
 248   {
 249     const guint8 *possible_sync;
 250
 251     GST_DEBUG_OBJECT (parse, "no sync, resyncing");
 252
 253     possible_sync = memchr (map.data, SBC_SYNCBYTE, map.size);
 254
 255     if (possible_sync != NULL)
 256       *skipsize = (gint) (possible_sync - map.data);
 257     else
 258       *skipsize = map.size;
 259
 260     gst_buffer_unmap (frame->buffer, &map);
 261
 262     /* we could optimise things here by looping over the data and checking
 263      * whether the sync is good or not instead of handing control back to
 264      * the base class just to be called again */
 265     return GST_FLOW_OK;
 266   }
 267 need_more_data:
 268   {
 269     GST_LOG_OBJECT (parse, "need %u bytes, but only have %" G_GSIZE_FORMAT,
 270         frame_len, map.size);
 271     gst_base_parse_set_min_frame_size (parse, frame_len);
 272     gst_buffer_unmap (frame->buffer, &map);
 273     return GST_FLOW_OK;
 274   }
 275 }
 276
 277 static GstCaps *
 278 gst_sbc_parse_get_sink_caps (GstBaseParse * parse, GstCaps * filter)
 279 {
 280   GstCaps *peercaps, *templ;
 281   GstCaps *res;
 282
 283   templ = gst_pad_get_pad_template_caps (GST_BASE_PARSE_SINK_PAD (parse));
 284   peercaps = gst_pad_peer_query_caps (GST_BASE_PARSE_SRC_PAD (parse), filter);
 285
 286   if (peercaps) {
 287     guint i, n;
 288
 289     /* Remove the parsed field */
 290     peercaps = gst_caps_make_writable (peercaps);
 291     n = gst_caps_get_size (peercaps);
 292     for (i = 0; i < n; i++) {
 293       GstStructure *s = gst_caps_get_structure (peercaps, i);
 294
 295       gst_structure_remove_field (s, "parsed");
 296     }
 297
 298     res = gst_caps_intersect_full (peercaps, templ, GST_CAPS_INTERSECT_FIRST);
 299     gst_caps_unref (peercaps);
 300     res = gst_caps_make_writable (res);
 301
 302     /* Append the template caps because we still want to accept
 303      * caps without any fields in the case upstream does not
 304      * know anything.
 305      */
 306     gst_caps_append (res, templ);
 307   } else {
 308     res = templ;
 309   }
 310
 311   if (filter) {
 312     GstCaps *intersection;
 313
 314     intersection =
 315         gst_caps_intersect_full (filter, res, GST_CAPS_INTERSECT_FIRST);
 316     gst_caps_unref (res);
 317     res = intersection;
 318   }
 319
 320   return res;
 321 }
 322
 323 static const guint8 crc_table[256] = {
 324   0x00, 0x1D, 0x3A, 0x27, 0x74, 0x69, 0x4E, 0x53,
 325   0xE8, 0xF5, 0xD2, 0xCF, 0x9C, 0x81, 0xA6, 0xBB,
 326   0xCD, 0xD0, 0xF7, 0xEA, 0xB9, 0xA4, 0x83, 0x9E,
 327   0x25, 0x38, 0x1F, 0x02, 0x51, 0x4C, 0x6B, 0x76,
 328   0x87, 0x9A, 0xBD, 0xA0, 0xF3, 0xEE, 0xC9, 0xD4,
 329   0x6F, 0x72, 0x55, 0x48, 0x1B, 0x06, 0x21, 0x3C,
 330   0x4A, 0x57, 0x70, 0x6D, 0x3E, 0x23, 0x04, 0x19,
 331   0xA2, 0xBF, 0x98, 0x85, 0xD6, 0xCB, 0xEC, 0xF1,
 332   0x13, 0x0E, 0x29, 0x34, 0x67, 0x7A, 0x5D, 0x40,
 333   0xFB, 0xE6, 0xC1, 0xDC, 0x8F, 0x92, 0xB5, 0xA8,
 334   0xDE, 0xC3, 0xE4, 0xF9, 0xAA, 0xB7, 0x90, 0x8D,
 335   0x36, 0x2B, 0x0C, 0x11, 0x42, 0x5F, 0x78, 0x65,
 336   0x94, 0x89, 0xAE, 0xB3, 0xE0, 0xFD, 0xDA, 0xC7,
 337   0x7C, 0x61, 0x46, 0x5B, 0x08, 0x15, 0x32, 0x2F,
 338   0x59, 0x44, 0x63, 0x7E, 0x2D, 0x30, 0x17, 0x0A,
 339   0xB1, 0xAC, 0x8B, 0x96, 0xC5, 0xD8, 0xFF, 0xE2,
 340   0x26, 0x3B, 0x1C, 0x01, 0x52, 0x4F, 0x68, 0x75,
 341   0xCE, 0xD3, 0xF4, 0xE9, 0xBA, 0xA7, 0x80, 0x9D,
 342   0xEB, 0xF6, 0xD1, 0xCC, 0x9F, 0x82, 0xA5, 0xB8,
 343   0x03, 0x1E, 0x39, 0x24, 0x77, 0x6A, 0x4D, 0x50,
 344   0xA1, 0xBC, 0x9B, 0x86, 0xD5, 0xC8, 0xEF, 0xF2,
 345   0x49, 0x54, 0x73, 0x6E, 0x3D, 0x20, 0x07, 0x1A,
 346   0x6C, 0x71, 0x56, 0x4B, 0x18, 0x05, 0x22, 0x3F,
 347   0x84, 0x99, 0xBE, 0xA3, 0xF0, 0xED, 0xCA, 0xD7,
 348   0x35, 0x28, 0x0F, 0x12, 0x41, 0x5C, 0x7B, 0x66,
 349   0xDD, 0xC0, 0xE7, 0xFA, 0xA9, 0xB4, 0x93, 0x8E,
 350   0xF8, 0xE5, 0xC2, 0xDF, 0x8C, 0x91, 0xB6, 0xAB,
 351   0x10, 0x0D, 0x2A, 0x37, 0x64, 0x79, 0x5E, 0x43,
 352   0xB2, 0xAF, 0x88, 0x95, 0xC6, 0xDB, 0xFC, 0xE1,
 353   0x5A, 0x47, 0x60, 0x7D, 0x2E, 0x33, 0x14, 0x09,
 354   0x7F, 0x62, 0x45, 0x58, 0x0B, 0x16, 0x31, 0x2C,
 355   0x97, 0x8A, 0xAD, 0xB0, 0xE3, 0xFE, 0xD9, 0xC4
 356 };
 357
 358 static guint8
 359 gst_sbc_calculate_crc8 (const guint8 * data, gint crc_bits)
 360 {
 361   guint8 crc = 0x0f;
 362   guint8 octet;
 363
 364   while (crc_bits >= 8) {
 365     crc = crc_table[crc ^ *data];
 366     crc_bits -= 8;
 367     ++data;
 368   }
 369
 370   octet = *data;
 371   while (crc_bits > 0) {
 372     gchar bit = ((octet ^ crc) & 0x80) >> 7;
 373
 374     crc = ((crc & 0x7f) << 1) ^ (bit ? 0x1d : 0);
 375
 376     octet = octet << 1;
 377     --crc_bits;
 378   }
 379
 380   return crc;
 381 }
 382
 383 static gsize
 384 gst_sbc_calc_framelen (guint subbands, GstSbcChannelMode ch_mode,
 385     guint blocks, guint bitpool)
 386 {
 387   switch (ch_mode) {
 388     case GST_SBC_CHANNEL_MODE_MONO:
 389       return 4 + (subbands * 1) / 2 + (blocks * 1 * bitpool) / 8;
 390     case GST_SBC_CHANNEL_MODE_DUAL:
 391       return 4 + (subbands * 2) / 2 + (blocks * 2 * bitpool) / 8;
 392     case GST_SBC_CHANNEL_MODE_STEREO:
 393       return 4 + (subbands * 2) / 2 + (blocks * bitpool) / 8;
 394     case GST_SBC_CHANNEL_MODE_JOINT_STEREO:
 395       return 4 + (subbands * 2) / 2 + (subbands + blocks * bitpool) / 8;
 396     default:
 397       break;
 398   }
 399
 400   g_return_val_if_reached (0);
 401 }
 402
 403 static gsize
 404 gst_sbc_parse_header (const guint8 * data, guint * rate, guint * n_blocks,
 405     GstSbcChannelMode * ch_mode, GstSbcAllocationMethod * alloc_method,
 406     guint * n_subbands, guint * bitpool)
 407 {
 408   static const guint16 sbc_rates[4] = { 16000, 32000, 44100, 48000 };
 409   static const guint8 sbc_blocks[4] = { 4, 8, 12, 16 };
 410   guint8 crc_data[2 + 1 + 8], crc_bits, i;
 411
 412   GST_MEMDUMP ("header", data, 8);
 413
 414   if (data[0] != SBC_SYNCBYTE)
 415     return 0;
 416
 417   *rate = sbc_rates[(data[1] >> 6) & 0x03];
 418   *n_blocks = sbc_blocks[(data[1] >> 4) & 0x03];
 419   *ch_mode = (GstSbcChannelMode) ((data[1] >> 2) & 0x03);
 420   *alloc_method = (data[1] >> 1) & 0x01;
 421   *n_subbands = (data[1] & 0x01) ? 8 : 4;
 422   *bitpool = data[2];
 423
 424   GST_TRACE ("rate=%u, n_blocks=%u, ch_mode=%u, alloc_method=%u, "
 425       "n_subbands=%u, bitpool=%u", *rate, *n_blocks, *ch_mode, *alloc_method,
 426       *n_subbands, *bitpool);
 427
 428   if (*bitpool < 2)
 429     return 0;
 430
 431   /* check CRC */
 432   crc_data[0] = data[1];
 433   crc_data[1] = data[2];
 434   crc_bits = 16;
 435
 436   /* joint flags and RFA */
 437   if (*ch_mode == GST_SBC_CHANNEL_MODE_JOINT_STEREO)
 438     crc_bits += *n_subbands;
 439
 440   /* scale factors */
 441   if (*ch_mode == GST_SBC_CHANNEL_MODE_MONO)
 442     crc_bits += *n_subbands * 1 * 4;
 443   else
 444     crc_bits += *n_subbands * 2 * 4;
 445
 446   for (i = 16; i < crc_bits; i += 8) {
 447     crc_data[i / 8] = data[1 + (i / 8) + 1];
 448   }
 449
 450   if (i > crc_bits) {
 451     crc_data[(i / 8) - 1] &= 0xF0;
 452   }
 453
 454   GST_MEMDUMP ("crc bytes", crc_data, GST_ROUND_UP_8 (crc_bits) / 8);
 455   if (gst_sbc_calculate_crc8 (crc_data, crc_bits) != data[3]) {
 456     GST_LOG ("header CRC check failed, bits=%u, got 0x%02x, expected 0x%02x",
 457         crc_bits, gst_sbc_calculate_crc8 (crc_data, crc_bits), data[3]);
 458     return 0;
 459   }
 460
 461   return gst_sbc_calc_framelen (*n_subbands, *ch_mode, *n_blocks, *bitpool);
 462 }