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