| /* | |
| * Bluetooth low-complexity, subband codec (SBC) | |
| * | |
| * Copyright (C) 2017 Aurelien Jacobs <aurel@gnuage.org> | |
| * Copyright (C) 2012-2013 Intel Corporation | |
| * Copyright (C) 2008-2010 Nokia Corporation | |
| * Copyright (C) 2004-2010 Marcel Holtmann <marcel@holtmann.org> | |
| * Copyright (C) 2004-2005 Henryk Ploetz <henryk@ploetzli.ch> | |
| * Copyright (C) 2005-2008 Brad Midgley <bmidgley@xmission.com> | |
| * | |
| * This file is part of FFmpeg. | |
| * | |
| * FFmpeg is free software; you can redistribute it and/or | |
| * modify it under the terms of the GNU Lesser General Public | |
| * License as published by the Free Software Foundation; either | |
| * version 2.1 of the License, or (at your option) any later version. | |
| * | |
| * FFmpeg is distributed in the hope that it will be useful, | |
| * but WITHOUT ANY WARRANTY; without even the implied warranty of | |
| * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU | |
| * Lesser General Public License for more details. | |
| * | |
| * You should have received a copy of the GNU Lesser General Public | |
| * License along with FFmpeg; if not, write to the Free Software | |
| * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA | |
| */ | |
| /** | |
| * @file | |
| * SBC decoder implementation | |
| */ | |
| #include "avcodec.h" | |
| #include "codec_internal.h" | |
| #include "decode.h" | |
| #include "libavutil/channel_layout.h" | |
| #include "libavutil/intreadwrite.h" | |
| #include "libavutil/mem_internal.h" | |
| #include "sbc.h" | |
| #include "sbcdec_data.h" | |
| struct sbc_decoder_state { | |
| int32_t V[2][170]; | |
| int offset[2][16]; | |
| }; | |
| typedef struct SBCDecContext { | |
| AVClass *class; | |
| DECLARE_ALIGNED(SBC_ALIGN, struct sbc_frame, frame); | |
| DECLARE_ALIGNED(SBC_ALIGN, struct sbc_decoder_state, dsp); | |
| } SBCDecContext; | |
| /* | |
| * Unpacks a SBC frame at the beginning of the stream in data, | |
| * which has at most len bytes into frame. | |
| * Returns the length in bytes of the packed frame, or a negative | |
| * value on error. The error codes are: | |
| * | |
| * -1 Data stream too short | |
| * -2 Sync byte incorrect | |
| * -3 CRC8 incorrect | |
| * -4 Bitpool value out of bounds | |
| */ | |
| static int sbc_unpack_frame(const uint8_t *data, struct sbc_frame *frame, | |
| size_t len) | |
| { | |
| unsigned int consumed; | |
| /* Will copy the parts of the header that are relevant to crc | |
| * calculation here */ | |
| uint8_t crc_header[11] = { 0 }; | |
| int crc_pos; | |
| int32_t temp; | |
| uint32_t audio_sample; | |
| int ch, sb, blk, bit; /* channel, subband, block and bit standard | |
| counters */ | |
| int bits[2][8]; /* bits distribution */ | |
| uint32_t levels[2][8]; /* levels derived from that */ | |
| if (len < 4) | |
| return -1; | |
| if (data[0] == MSBC_SYNCWORD) { | |
| if (data[1] != 0) | |
| return -2; | |
| if (data[2] != 0) | |
| return -2; | |
| frame->frequency = SBC_FREQ_16000; | |
| frame->blocks = MSBC_BLOCKS; | |
| frame->allocation = LOUDNESS; | |
| frame->mode = MONO; | |
| frame->channels = 1; | |
| frame->subbands = 8; | |
| frame->bitpool = 26; | |
| } else if (data[0] == SBC_SYNCWORD) { | |
| frame->frequency = (data[1] >> 6) & 0x03; | |
| frame->blocks = 4 * ((data[1] >> 4) & 0x03) + 4; | |
| frame->mode = (data[1] >> 2) & 0x03; | |
| frame->channels = frame->mode == MONO ? 1 : 2; | |
| frame->allocation = (data[1] >> 1) & 0x01; | |
| frame->subbands = data[1] & 0x01 ? 8 : 4; | |
| frame->bitpool = data[2]; | |
| if ((frame->mode == MONO || frame->mode == DUAL_CHANNEL) && | |
| frame->bitpool > 16 * frame->subbands) | |
| return -4; | |
| if ((frame->mode == STEREO || frame->mode == JOINT_STEREO) && | |
| frame->bitpool > 32 * frame->subbands) | |
| return -4; | |
| } else | |
| return -2; | |
| consumed = 32; | |
| crc_header[0] = data[1]; | |
| crc_header[1] = data[2]; | |
| crc_pos = 16; | |
| if (frame->mode == JOINT_STEREO) { | |
| if (len * 8 < consumed + frame->subbands) | |
| return -1; | |
| frame->joint = 0x00; | |
| for (sb = 0; sb < frame->subbands - 1; sb++) | |
| frame->joint |= ((data[4] >> (7 - sb)) & 0x01) << sb; | |
| if (frame->subbands == 4) | |
| crc_header[crc_pos / 8] = data[4] & 0xf0; | |
| else | |
| crc_header[crc_pos / 8] = data[4]; | |
| consumed += frame->subbands; | |
| crc_pos += frame->subbands; | |
| } | |
| if (len * 8 < consumed + (4 * frame->subbands * frame->channels)) | |
| return -1; | |
| for (ch = 0; ch < frame->channels; ch++) { | |
| for (sb = 0; sb < frame->subbands; sb++) { | |
| /* FIXME assert(consumed % 4 == 0); */ | |
| frame->scale_factor[ch][sb] = | |
| (data[consumed >> 3] >> (4 - (consumed & 0x7))) & 0x0F; | |
| crc_header[crc_pos >> 3] |= | |
| frame->scale_factor[ch][sb] << (4 - (crc_pos & 0x7)); | |
| consumed += 4; | |
| crc_pos += 4; | |
| } | |
| } | |
| if (data[3] != ff_sbc_crc8(frame->crc_ctx, crc_header, crc_pos)) | |
| return -3; | |
| ff_sbc_calculate_bits(frame, bits); | |
| for (ch = 0; ch < frame->channels; ch++) { | |
| for (sb = 0; sb < frame->subbands; sb++) | |
| levels[ch][sb] = (1 << bits[ch][sb]) - 1; | |
| } | |
| for (blk = 0; blk < frame->blocks; blk++) { | |
| for (ch = 0; ch < frame->channels; ch++) { | |
| for (sb = 0; sb < frame->subbands; sb++) { | |
| uint32_t shift; | |
| if (levels[ch][sb] == 0) { | |
| frame->sb_sample[blk][ch][sb] = 0; | |
| continue; | |
| } | |
| shift = frame->scale_factor[ch][sb] + | |
| 1 + SBCDEC_FIXED_EXTRA_BITS; | |
| audio_sample = 0; | |
| for (bit = 0; bit < bits[ch][sb]; bit++) { | |
| if (consumed > len * 8) | |
| return -1; | |
| if ((data[consumed >> 3] >> (7 - (consumed & 0x7))) & 0x01) | |
| audio_sample |= 1 << (bits[ch][sb] - bit - 1); | |
| consumed++; | |
| } | |
| frame->sb_sample[blk][ch][sb] = (int32_t) | |
| (((((uint64_t) audio_sample << 1) | 1) << shift) / | |
| levels[ch][sb]) - (1 << shift); | |
| } | |
| } | |
| } | |
| if (frame->mode == JOINT_STEREO) { | |
| for (blk = 0; blk < frame->blocks; blk++) { | |
| for (sb = 0; sb < frame->subbands; sb++) { | |
| if (frame->joint & (0x01 << sb)) { | |
| temp = frame->sb_sample[blk][0][sb] + | |
| frame->sb_sample[blk][1][sb]; | |
| frame->sb_sample[blk][1][sb] = | |
| frame->sb_sample[blk][0][sb] - | |
| frame->sb_sample[blk][1][sb]; | |
| frame->sb_sample[blk][0][sb] = temp; | |
| } | |
| } | |
| } | |
| } | |
| if ((consumed & 0x7) != 0) | |
| consumed += 8 - (consumed & 0x7); | |
| return consumed >> 3; | |
| } | |
| static inline void sbc_synthesize_four(struct sbc_decoder_state *state, | |
| struct sbc_frame *frame, | |
| int ch, int blk, AVFrame *output_frame) | |
| { | |
| int i, k, idx; | |
| int32_t *v = state->V[ch]; | |
| int *offset = state->offset[ch]; | |
| for (i = 0; i < 8; i++) { | |
| /* Shifting */ | |
| offset[i]--; | |
| if (offset[i] < 0) { | |
| offset[i] = 79; | |
| memcpy(v + 80, v, 9 * sizeof(*v)); | |
| } | |
| /* Distribute the new matrix value to the shifted position */ | |
| v[offset[i]] = | |
| (int)( (unsigned)ff_synmatrix4[i][0] * frame->sb_sample[blk][ch][0] + | |
| (unsigned)ff_synmatrix4[i][1] * frame->sb_sample[blk][ch][1] + | |
| (unsigned)ff_synmatrix4[i][2] * frame->sb_sample[blk][ch][2] + | |
| (unsigned)ff_synmatrix4[i][3] * frame->sb_sample[blk][ch][3] ) >> 15; | |
| } | |
| /* Compute the samples */ | |
| for (idx = 0, i = 0; i < 4; i++, idx += 5) { | |
| k = (i + 4) & 0xf; | |
| /* Store in output, Q0 */ | |
| AV_WN16A(&output_frame->data[ch][blk * 8 + i * 2], av_clip_int16( | |
| (int)( (unsigned)v[offset[i] + 0] * ff_sbc_proto_4_40m0[idx + 0] + | |
| (unsigned)v[offset[k] + 1] * ff_sbc_proto_4_40m1[idx + 0] + | |
| (unsigned)v[offset[i] + 2] * ff_sbc_proto_4_40m0[idx + 1] + | |
| (unsigned)v[offset[k] + 3] * ff_sbc_proto_4_40m1[idx + 1] + | |
| (unsigned)v[offset[i] + 4] * ff_sbc_proto_4_40m0[idx + 2] + | |
| (unsigned)v[offset[k] + 5] * ff_sbc_proto_4_40m1[idx + 2] + | |
| (unsigned)v[offset[i] + 6] * ff_sbc_proto_4_40m0[idx + 3] + | |
| (unsigned)v[offset[k] + 7] * ff_sbc_proto_4_40m1[idx + 3] + | |
| (unsigned)v[offset[i] + 8] * ff_sbc_proto_4_40m0[idx + 4] + | |
| (unsigned)v[offset[k] + 9] * ff_sbc_proto_4_40m1[idx + 4] ) >> 15)); | |
| } | |
| } | |
| static inline void sbc_synthesize_eight(struct sbc_decoder_state *state, | |
| struct sbc_frame *frame, | |
| int ch, int blk, AVFrame *output_frame) | |
| { | |
| int i, k, idx; | |
| int32_t *v = state->V[ch]; | |
| int *offset = state->offset[ch]; | |
| for (i = 0; i < 16; i++) { | |
| /* Shifting */ | |
| offset[i]--; | |
| if (offset[i] < 0) { | |
| offset[i] = 159; | |
| memcpy(v + 160, v, 9 * sizeof(*v)); | |
| } | |
| /* Distribute the new matrix value to the shifted position */ | |
| v[offset[i]] = | |
| (int)( (unsigned)ff_synmatrix8[i][0] * frame->sb_sample[blk][ch][0] + | |
| (unsigned)ff_synmatrix8[i][1] * frame->sb_sample[blk][ch][1] + | |
| (unsigned)ff_synmatrix8[i][2] * frame->sb_sample[blk][ch][2] + | |
| (unsigned)ff_synmatrix8[i][3] * frame->sb_sample[blk][ch][3] + | |
| (unsigned)ff_synmatrix8[i][4] * frame->sb_sample[blk][ch][4] + | |
| (unsigned)ff_synmatrix8[i][5] * frame->sb_sample[blk][ch][5] + | |
| (unsigned)ff_synmatrix8[i][6] * frame->sb_sample[blk][ch][6] + | |
| (unsigned)ff_synmatrix8[i][7] * frame->sb_sample[blk][ch][7] ) >> 15; | |
| } | |
| /* Compute the samples */ | |
| for (idx = 0, i = 0; i < 8; i++, idx += 5) { | |
| k = (i + 8) & 0xf; | |
| /* Store in output, Q0 */ | |
| AV_WN16A(&output_frame->data[ch][blk * 16 + i * 2], av_clip_int16( | |
| (int)( (unsigned)v[offset[i] + 0] * ff_sbc_proto_8_80m0[idx + 0] + | |
| (unsigned)v[offset[k] + 1] * ff_sbc_proto_8_80m1[idx + 0] + | |
| (unsigned)v[offset[i] + 2] * ff_sbc_proto_8_80m0[idx + 1] + | |
| (unsigned)v[offset[k] + 3] * ff_sbc_proto_8_80m1[idx + 1] + | |
| (unsigned)v[offset[i] + 4] * ff_sbc_proto_8_80m0[idx + 2] + | |
| (unsigned)v[offset[k] + 5] * ff_sbc_proto_8_80m1[idx + 2] + | |
| (unsigned)v[offset[i] + 6] * ff_sbc_proto_8_80m0[idx + 3] + | |
| (unsigned)v[offset[k] + 7] * ff_sbc_proto_8_80m1[idx + 3] + | |
| (unsigned)v[offset[i] + 8] * ff_sbc_proto_8_80m0[idx + 4] + | |
| (unsigned)v[offset[k] + 9] * ff_sbc_proto_8_80m1[idx + 4] ) >> 15)); | |
| } | |
| } | |
| static void sbc_synthesize_audio(struct sbc_decoder_state *state, | |
| struct sbc_frame *frame, AVFrame *output_frame) | |
| { | |
| int ch, blk; | |
| switch (frame->subbands) { | |
| case 4: | |
| for (ch = 0; ch < frame->channels; ch++) | |
| for (blk = 0; blk < frame->blocks; blk++) | |
| sbc_synthesize_four(state, frame, ch, blk, output_frame); | |
| break; | |
| case 8: | |
| for (ch = 0; ch < frame->channels; ch++) | |
| for (blk = 0; blk < frame->blocks; blk++) | |
| sbc_synthesize_eight(state, frame, ch, blk, output_frame); | |
| break; | |
| } | |
| } | |
| static int sbc_decode_init(AVCodecContext *avctx) | |
| { | |
| SBCDecContext *sbc = avctx->priv_data; | |
| int i, ch; | |
| avctx->sample_fmt = AV_SAMPLE_FMT_S16P; | |
| sbc->frame.crc_ctx = av_crc_get_table(AV_CRC_8_EBU); | |
| memset(sbc->dsp.V, 0, sizeof(sbc->dsp.V)); | |
| for (ch = 0; ch < 2; ch++) | |
| for (i = 0; i < FF_ARRAY_ELEMS(sbc->dsp.offset[0]); i++) | |
| sbc->dsp.offset[ch][i] = (10 * i + 10); | |
| return 0; | |
| } | |
| static int sbc_decode_frame(AVCodecContext *avctx, AVFrame *frame, | |
| int *got_frame_ptr, AVPacket *avpkt) | |
| { | |
| SBCDecContext *sbc = avctx->priv_data; | |
| int ret, frame_length; | |
| frame_length = sbc_unpack_frame(avpkt->data, &sbc->frame, avpkt->size); | |
| if (frame_length <= 0) | |
| return frame_length; | |
| av_channel_layout_uninit(&avctx->ch_layout); | |
| avctx->ch_layout.order = AV_CHANNEL_ORDER_UNSPEC; | |
| avctx->ch_layout.nb_channels = sbc->frame.channels; | |
| frame->nb_samples = sbc->frame.blocks * sbc->frame.subbands; | |
| if ((ret = ff_get_buffer(avctx, frame, 0)) < 0) | |
| return ret; | |
| sbc_synthesize_audio(&sbc->dsp, &sbc->frame, frame); | |
| *got_frame_ptr = 1; | |
| return frame_length; | |
| } | |
| const FFCodec ff_sbc_decoder = { | |
| .p.name = "sbc", | |
| CODEC_LONG_NAME("SBC (low-complexity subband codec)"), | |
| .p.type = AVMEDIA_TYPE_AUDIO, | |
| .p.id = AV_CODEC_ID_SBC, | |
| .priv_data_size = sizeof(SBCDecContext), | |
| .init = sbc_decode_init, | |
| FF_CODEC_DECODE_CB(sbc_decode_frame), | |
| .p.capabilities = AV_CODEC_CAP_DR1 | AV_CODEC_CAP_CHANNEL_CONF, | |
| .p.ch_layouts = (const AVChannelLayout[]) { AV_CHANNEL_LAYOUT_MONO, | |
| AV_CHANNEL_LAYOUT_STEREO, | |
| { 0 } }, | |
| .p.sample_fmts = (const enum AVSampleFormat[]) { AV_SAMPLE_FMT_S16P, | |
| AV_SAMPLE_FMT_NONE }, | |
| .p.supported_samplerates = (const int[]) { 16000, 32000, 44100, 48000, 0 }, | |
| }; |