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