git.sesse.net Git - ffmpeg/blob - libavcodec/sbcdec.c

   1 /*
   2  * Bluetooth low-complexity, subband codec (SBC)
   3  *
   4  * Copyright (C) 2017  Aurelien Jacobs <aurel@gnuage.org>
   5  * Copyright (C) 2012-2013  Intel Corporation
   6  * Copyright (C) 2008-2010  Nokia Corporation
   7  * Copyright (C) 2004-2010  Marcel Holtmann <marcel@holtmann.org>
   8  * Copyright (C) 2004-2005  Henryk Ploetz <henryk@ploetzli.ch>
   9  * Copyright (C) 2005-2008  Brad Midgley <bmidgley@xmission.com>
  10  *
  11  * This file is part of FFmpeg.
  12  *
  13  * FFmpeg is free software; you can redistribute it and/or
  14  * modify it under the terms of the GNU Lesser General Public
  15  * License as published by the Free Software Foundation; either
  16  * version 2.1 of the License, or (at your option) any later version.
  17  *
  18  * FFmpeg is distributed in the hope that it will be useful,
  19  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  20  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  21  * Lesser General Public License for more details.
  22  *
  23  * You should have received a copy of the GNU Lesser General Public
  24  * License along with FFmpeg; if not, write to the Free Software
  25  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
  26  */
  27
  28 /**
  29  * @file
  30  * SBC decoder implementation
  31  */
  32
  33 #include "avcodec.h"
  34 #include "internal.h"
  35 #include "libavutil/intreadwrite.h"
  36 #include "sbc.h"
  37 #include "sbcdec_data.h"
  38
  39 struct sbc_decoder_state {
  40     int32_t V[2][170];
  41     int offset[2][16];
  42 };
  43
  44 typedef struct SBCDecContext {
  45     AVClass *class;
  46     DECLARE_ALIGNED(SBC_ALIGN, struct sbc_frame, frame);
  47     DECLARE_ALIGNED(SBC_ALIGN, struct sbc_decoder_state, dsp);
  48 } SBCDecContext;
  49
  50 /*
  51  * Unpacks a SBC frame at the beginning of the stream in data,
  52  * which has at most len bytes into frame.
  53  * Returns the length in bytes of the packed frame, or a negative
  54  * value on error. The error codes are:
  55  *
  56  *  -1   Data stream too short
  57  *  -2   Sync byte incorrect
  58  *  -3   CRC8 incorrect
  59  *  -4   Bitpool value out of bounds
  60  */
  61 static int sbc_unpack_frame(const uint8_t *data, struct sbc_frame *frame,
  62                             size_t len)
  63 {
  64     unsigned int consumed;
  65     /* Will copy the parts of the header that are relevant to crc
  66      * calculation here */
  67     uint8_t crc_header[11] = { 0 };
  68     int crc_pos;
  69     int32_t temp;
  70
  71     uint32_t audio_sample;
  72     int ch, sb, blk, bit;   /* channel, subband, block and bit standard
  73                                counters */
  74     int bits[2][8];         /* bits distribution */
  75     uint32_t levels[2][8];  /* levels derived from that */
  76
  77     if (len < 4)
  78         return -1;
  79
  80     if (data[0] == MSBC_SYNCWORD) {
  81         if (data[1] != 0)
  82             return -2;
  83         if (data[2] != 0)
  84             return -2;
  85
  86         frame->frequency = SBC_FREQ_16000;
  87         frame->blocks = MSBC_BLOCKS;
  88         frame->allocation = LOUDNESS;
  89         frame->mode = MONO;
  90         frame->channels = 1;
  91         frame->subbands = 8;
  92         frame->bitpool = 26;
  93     } else if (data[0] == SBC_SYNCWORD) {
  94         frame->frequency  = (data[1] >> 6) & 0x03;
  95         frame->blocks = 4 * ((data[1] >> 4) & 0x03) + 4;
  96         frame->mode = (data[1] >> 2) & 0x03;
  97         frame->channels = frame->mode == MONO ? 1 : 2;
  98         frame->allocation = (data[1] >> 1) & 0x01;
  99         frame->subbands = data[1] & 0x01 ? 8 : 4;
 100         frame->bitpool = data[2];
 101
 102         if ((frame->mode == MONO || frame->mode == DUAL_CHANNEL) &&
 103             frame->bitpool > 16 * frame->subbands)
 104             return -4;
 105
 106         if ((frame->mode == STEREO || frame->mode == JOINT_STEREO) &&
 107             frame->bitpool > 32 * frame->subbands)
 108             return -4;
 109     } else
 110         return -2;
 111
 112     consumed = 32;
 113     crc_header[0] = data[1];
 114     crc_header[1] = data[2];
 115     crc_pos = 16;
 116
 117     if (frame->mode == JOINT_STEREO) {
 118         if (len * 8 < consumed + frame->subbands)
 119             return -1;
 120
 121         frame->joint = 0x00;
 122         for (sb = 0; sb < frame->subbands - 1; sb++)
 123             frame->joint |= ((data[4] >> (7 - sb)) & 0x01) << sb;
 124         if (frame->subbands == 4)
 125             crc_header[crc_pos / 8] = data[4] & 0xf0;
 126         else
 127             crc_header[crc_pos / 8] = data[4];
 128
 129         consumed += frame->subbands;
 130         crc_pos += frame->subbands;
 131     }
 132
 133     if (len * 8 < consumed + (4 * frame->subbands * frame->channels))
 134         return -1;
 135
 136     for (ch = 0; ch < frame->channels; ch++) {
 137         for (sb = 0; sb < frame->subbands; sb++) {
 138             /* FIXME assert(consumed % 4 == 0); */
 139             frame->scale_factor[ch][sb] =
 140                 (data[consumed >> 3] >> (4 - (consumed & 0x7))) & 0x0F;
 141             crc_header[crc_pos >> 3] |=
 142                 frame->scale_factor[ch][sb] << (4 - (crc_pos & 0x7));
 143
 144             consumed += 4;
 145             crc_pos += 4;
 146         }
 147     }
 148
 149     if (data[3] != ff_sbc_crc8(frame->crc_ctx, crc_header, crc_pos))
 150         return -3;
 151
 152     ff_sbc_calculate_bits(frame, bits);
 153
 154     for (ch = 0; ch < frame->channels; ch++) {
 155         for (sb = 0; sb < frame->subbands; sb++)
 156             levels[ch][sb] = (1 << bits[ch][sb]) - 1;
 157     }
 158
 159     for (blk = 0; blk < frame->blocks; blk++) {
 160         for (ch = 0; ch < frame->channels; ch++) {
 161             for (sb = 0; sb < frame->subbands; sb++) {
 162                 uint32_t shift;
 163
 164                 if (levels[ch][sb] == 0) {
 165                     frame->sb_sample[blk][ch][sb] = 0;
 166                     continue;
 167                 }
 168
 169                 shift = frame->scale_factor[ch][sb] +
 170                         1 + SBCDEC_FIXED_EXTRA_BITS;
 171
 172                 audio_sample = 0;
 173                 for (bit = 0; bit < bits[ch][sb]; bit++) {
 174                     if (consumed > len * 8)
 175                         return -1;
 176
 177                     if ((data[consumed >> 3] >> (7 - (consumed & 0x7))) & 0x01)
 178                         audio_sample |= 1 << (bits[ch][sb] - bit - 1);
 179
 180                     consumed++;
 181                 }
 182
 183                 frame->sb_sample[blk][ch][sb] = (int32_t)
 184                     (((((uint64_t) audio_sample << 1) | 1) << shift) /
 185                     levels[ch][sb]) - (1 << shift);
 186             }
 187         }
 188     }
 189
 190     if (frame->mode == JOINT_STEREO) {
 191         for (blk = 0; blk < frame->blocks; blk++) {
 192             for (sb = 0; sb < frame->subbands; sb++) {
 193                 if (frame->joint & (0x01 << sb)) {
 194                     temp = frame->sb_sample[blk][0][sb] +
 195                            frame->sb_sample[blk][1][sb];
 196                     frame->sb_sample[blk][1][sb] =
 197                         frame->sb_sample[blk][0][sb] -
 198                         frame->sb_sample[blk][1][sb];
 199                     frame->sb_sample[blk][0][sb] = temp;
 200                 }
 201             }
 202         }
 203     }
 204
 205     if ((consumed & 0x7) != 0)
 206         consumed += 8 - (consumed & 0x7);
 207
 208     return consumed >> 3;
 209 }
 210
 211 static inline void sbc_synthesize_four(struct sbc_decoder_state *state,
 212                                        struct sbc_frame *frame,
 213                                        int ch, int blk, AVFrame *output_frame)
 214 {
 215     int i, k, idx;
 216     int32_t *v = state->V[ch];
 217     int *offset = state->offset[ch];
 218
 219     for (i = 0; i < 8; i++) {
 220         /* Shifting */
 221         offset[i]--;
 222         if (offset[i] < 0) {
 223             offset[i] = 79;
 224             memcpy(v + 80, v, 9 * sizeof(*v));
 225         }
 226
 227         /* Distribute the new matrix value to the shifted position */
 228         v[offset[i]] =
 229             (int)( (unsigned)ff_synmatrix4[i][0] * frame->sb_sample[blk][ch][0] +
 230                    (unsigned)ff_synmatrix4[i][1] * frame->sb_sample[blk][ch][1] +
 231                    (unsigned)ff_synmatrix4[i][2] * frame->sb_sample[blk][ch][2] +
 232                    (unsigned)ff_synmatrix4[i][3] * frame->sb_sample[blk][ch][3] ) >> 15;
 233     }
 234
 235     /* Compute the samples */
 236     for (idx = 0, i = 0; i < 4; i++, idx += 5) {
 237         k = (i + 4) & 0xf;
 238
 239         /* Store in output, Q0 */
 240         AV_WN16A(&output_frame->data[ch][blk * 8 + i * 2], av_clip_int16(
 241          (int)( (unsigned)v[offset[i] + 0] * ff_sbc_proto_4_40m0[idx + 0] +
 242                 (unsigned)v[offset[k] + 1] * ff_sbc_proto_4_40m1[idx + 0] +
 243                 (unsigned)v[offset[i] + 2] * ff_sbc_proto_4_40m0[idx + 1] +
 244                 (unsigned)v[offset[k] + 3] * ff_sbc_proto_4_40m1[idx + 1] +
 245                 (unsigned)v[offset[i] + 4] * ff_sbc_proto_4_40m0[idx + 2] +
 246                 (unsigned)v[offset[k] + 5] * ff_sbc_proto_4_40m1[idx + 2] +
 247                 (unsigned)v[offset[i] + 6] * ff_sbc_proto_4_40m0[idx + 3] +
 248                 (unsigned)v[offset[k] + 7] * ff_sbc_proto_4_40m1[idx + 3] +
 249                 (unsigned)v[offset[i] + 8] * ff_sbc_proto_4_40m0[idx + 4] +
 250                 (unsigned)v[offset[k] + 9] * ff_sbc_proto_4_40m1[idx + 4] ) >> 15));
 251     }
 252 }
 253
 254 static inline void sbc_synthesize_eight(struct sbc_decoder_state *state,
 255                                         struct sbc_frame *frame,
 256                                         int ch, int blk, AVFrame *output_frame)
 257 {
 258     int i, k, idx;
 259     int32_t *v = state->V[ch];
 260     int *offset = state->offset[ch];
 261
 262     for (i = 0; i < 16; i++) {
 263         /* Shifting */
 264         offset[i]--;
 265         if (offset[i] < 0) {
 266             offset[i] = 159;
 267             memcpy(v + 160, v, 9 * sizeof(*v));
 268         }
 269
 270         /* Distribute the new matrix value to the shifted position */
 271         v[offset[i]] =
 272              (int)( (unsigned)ff_synmatrix8[i][0] * frame->sb_sample[blk][ch][0] +
 273                     (unsigned)ff_synmatrix8[i][1] * frame->sb_sample[blk][ch][1] +
 274                     (unsigned)ff_synmatrix8[i][2] * frame->sb_sample[blk][ch][2] +
 275                     (unsigned)ff_synmatrix8[i][3] * frame->sb_sample[blk][ch][3] +
 276                     (unsigned)ff_synmatrix8[i][4] * frame->sb_sample[blk][ch][4] +
 277                     (unsigned)ff_synmatrix8[i][5] * frame->sb_sample[blk][ch][5] +
 278                     (unsigned)ff_synmatrix8[i][6] * frame->sb_sample[blk][ch][6] +
 279                     (unsigned)ff_synmatrix8[i][7] * frame->sb_sample[blk][ch][7] ) >> 15;
 280     }
 281
 282     /* Compute the samples */
 283     for (idx = 0, i = 0; i < 8; i++, idx += 5) {
 284         k = (i + 8) & 0xf;
 285
 286         /* Store in output, Q0 */
 287         AV_WN16A(&output_frame->data[ch][blk * 16 + i * 2], av_clip_int16(
 288          (int)( (unsigned)v[offset[i] + 0] * ff_sbc_proto_8_80m0[idx + 0] +
 289                 (unsigned)v[offset[k] + 1] * ff_sbc_proto_8_80m1[idx + 0] +
 290                 (unsigned)v[offset[i] + 2] * ff_sbc_proto_8_80m0[idx + 1] +
 291                 (unsigned)v[offset[k] + 3] * ff_sbc_proto_8_80m1[idx + 1] +
 292                 (unsigned)v[offset[i] + 4] * ff_sbc_proto_8_80m0[idx + 2] +
 293                 (unsigned)v[offset[k] + 5] * ff_sbc_proto_8_80m1[idx + 2] +
 294                 (unsigned)v[offset[i] + 6] * ff_sbc_proto_8_80m0[idx + 3] +
 295                 (unsigned)v[offset[k] + 7] * ff_sbc_proto_8_80m1[idx + 3] +
 296                 (unsigned)v[offset[i] + 8] * ff_sbc_proto_8_80m0[idx + 4] +
 297                 (unsigned)v[offset[k] + 9] * ff_sbc_proto_8_80m1[idx + 4] ) >> 15));
 298     }
 299 }
 300
 301 static void sbc_synthesize_audio(struct sbc_decoder_state *state,
 302                                  struct sbc_frame *frame, AVFrame *output_frame)
 303 {
 304     int ch, blk;
 305
 306     switch (frame->subbands) {
 307     case 4:
 308         for (ch = 0; ch < frame->channels; ch++)
 309             for (blk = 0; blk < frame->blocks; blk++)
 310                 sbc_synthesize_four(state, frame, ch, blk, output_frame);
 311         break;
 312
 313     case 8:
 314         for (ch = 0; ch < frame->channels; ch++)
 315             for (blk = 0; blk < frame->blocks; blk++)
 316                 sbc_synthesize_eight(state, frame, ch, blk, output_frame);
 317         break;
 318     }
 319 }
 320
 321 static int sbc_decode_init(AVCodecContext *avctx)
 322 {
 323     SBCDecContext *sbc = avctx->priv_data;
 324     int i, ch;
 325
 326     avctx->sample_fmt = AV_SAMPLE_FMT_S16P;
 327
 328     sbc->frame.crc_ctx = av_crc_get_table(AV_CRC_8_EBU);
 329
 330     memset(sbc->dsp.V, 0, sizeof(sbc->dsp.V));
 331     for (ch = 0; ch < 2; ch++)
 332         for (i = 0; i < FF_ARRAY_ELEMS(sbc->dsp.offset[0]); i++)
 333             sbc->dsp.offset[ch][i] = (10 * i + 10);
 334     return 0;
 335 }
 336
 337 static int sbc_decode_frame(AVCodecContext *avctx,
 338                             void *data, int *got_frame_ptr,
 339                             AVPacket *avpkt)
 340 {
 341     SBCDecContext *sbc = avctx->priv_data;
 342     AVFrame *frame = data;
 343     int ret, frame_length;
 344
 345     if (!sbc)
 346         return AVERROR(EIO);
 347
 348     frame_length = sbc_unpack_frame(avpkt->data, &sbc->frame, avpkt->size);
 349     if (frame_length <= 0)
 350         return frame_length;
 351
 352     avctx->channels = sbc->frame.channels;
 353
 354     frame->nb_samples = sbc->frame.blocks * sbc->frame.subbands;
 355     if ((ret = ff_get_buffer(avctx, frame, 0)) < 0)
 356         return ret;
 357
 358     sbc_synthesize_audio(&sbc->dsp, &sbc->frame, frame);
 359
 360     *got_frame_ptr = 1;
 361
 362     return frame_length;
 363 }
 364
 365 AVCodec ff_sbc_decoder = {
 366     .name                  = "sbc",
 367     .long_name             = NULL_IF_CONFIG_SMALL("SBC (low-complexity subband codec)"),
 368     .type                  = AVMEDIA_TYPE_AUDIO,
 369     .id                    = AV_CODEC_ID_SBC,
 370     .priv_data_size        = sizeof(SBCDecContext),
 371     .init                  = sbc_decode_init,
 372     .decode                = sbc_decode_frame,
 373     .capabilities          = AV_CODEC_CAP_DR1,
 374     .caps_internal         = FF_CODEC_CAP_INIT_THREADSAFE,
 375     .channel_layouts       = (const uint64_t[]) { AV_CH_LAYOUT_MONO,
 376                                                   AV_CH_LAYOUT_STEREO, 0},
 377     .sample_fmts           = (const enum AVSampleFormat[]) { AV_SAMPLE_FMT_S16P,
 378                                                              AV_SAMPLE_FMT_NONE },
 379     .supported_samplerates = (const int[]) { 16000, 32000, 44100, 48000, 0 },
 380 };