]> git.sesse.net Git - ffmpeg/blob - libavcodec/aptx.c
Merge commit '3152058bf1dca318898550efacf0286f4836cae6'
[ffmpeg] / libavcodec / aptx.c
1 /*
2  * Audio Processing Technology codec for Bluetooth (aptX)
3  *
4  * Copyright (C) 2017  Aurelien Jacobs <aurel@gnuage.org>
5  *
6  * This file is part of FFmpeg.
7  *
8  * FFmpeg is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU Lesser General Public
10  * License as published by the Free Software Foundation; either
11  * version 2.1 of the License, or (at your option) any later version.
12  *
13  * FFmpeg is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
16  * Lesser General Public License for more details.
17  *
18  * You should have received a copy of the GNU Lesser General Public
19  * License along with FFmpeg; if not, write to the Free Software
20  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
21  */
22
23 #include "libavutil/intreadwrite.h"
24 #include "avcodec.h"
25 #include "internal.h"
26 #include "mathops.h"
27 #include "audio_frame_queue.h"
28
29
30 enum channels {
31     LEFT,
32     RIGHT,
33     NB_CHANNELS
34 };
35
36 enum subbands {
37     LF,  // Low Frequency (0-5.5 kHz)
38     MLF, // Medium-Low Frequency (5.5-11kHz)
39     MHF, // Medium-High Frequency (11-16.5kHz)
40     HF,  // High Frequency (16.5-22kHz)
41     NB_SUBBANDS
42 };
43
44 #define NB_FILTERS 2
45 #define FILTER_TAPS 16
46
47 typedef struct {
48     int pos;
49     int32_t buffer[2*FILTER_TAPS];
50 } FilterSignal;
51
52 typedef struct {
53     FilterSignal outer_filter_signal[NB_FILTERS];
54     FilterSignal inner_filter_signal[NB_FILTERS][NB_FILTERS];
55 } QMFAnalysis;
56
57 typedef struct {
58     int32_t quantized_sample;
59     int32_t quantized_sample_parity_change;
60     int32_t error;
61 } Quantize;
62
63 typedef struct {
64     int32_t quantization_factor;
65     int32_t factor_select;
66     int32_t reconstructed_difference;
67 } InvertQuantize;
68
69 typedef struct {
70     int32_t prev_sign[2];
71     int32_t s_weight[2];
72     int32_t d_weight[24];
73     int32_t pos;
74     int32_t reconstructed_differences[48];
75     int32_t previous_reconstructed_sample;
76     int32_t predicted_difference;
77     int32_t predicted_sample;
78 } Prediction;
79
80 typedef struct {
81     int32_t codeword_history;
82     int32_t dither_parity;
83     int32_t dither[NB_SUBBANDS];
84
85     QMFAnalysis qmf;
86     Quantize quantize[NB_SUBBANDS];
87     InvertQuantize invert_quantize[NB_SUBBANDS];
88     Prediction prediction[NB_SUBBANDS];
89 } Channel;
90
91 typedef struct {
92     int32_t sync_idx;
93     Channel channels[NB_CHANNELS];
94     AudioFrameQueue afq;
95 } AptXContext;
96
97
98 static const int32_t quantize_intervals_LF[65] = {
99       -9948,    9948,   29860,   49808,   69822,   89926,  110144,  130502,
100      151026,  171738,  192666,  213832,  235264,  256982,  279014,  301384,
101      324118,  347244,  370790,  394782,  419250,  444226,  469742,  495832,
102      522536,  549890,  577936,  606720,  636290,  666700,  698006,  730270,
103      763562,  797958,  833538,  870398,  908640,  948376,  989740, 1032874,
104     1077948, 1125150, 1174700, 1226850, 1281900, 1340196, 1402156, 1468282,
105     1539182, 1615610, 1698514, 1789098, 1888944, 2000168, 2125700, 2269750,
106     2438670, 2642660, 2899462, 3243240, 3746078, 4535138, 5664098, 7102424,
107     8897462,
108 };
109 static const int32_t invert_quantize_dither_factors_LF[65] = {
110        9948,   9948,   9962,   9988,  10026,  10078,  10142,  10218,
111       10306,  10408,  10520,  10646,  10784,  10934,  11098,  11274,
112       11462,  11664,  11880,  12112,  12358,  12618,  12898,  13194,
113       13510,  13844,  14202,  14582,  14988,  15422,  15884,  16380,
114       16912,  17484,  18098,  18762,  19480,  20258,  21106,  22030,
115       23044,  24158,  25390,  26760,  28290,  30008,  31954,  34172,
116       36728,  39700,  43202,  47382,  52462,  58762,  66770,  77280,
117       91642, 112348, 144452, 199326, 303512, 485546, 643414, 794914,
118     1000124,
119 };
120 static const int32_t quantize_dither_factors_LF[65] = {
121         0,     4,     7,    10,    13,    16,    19,    22,
122        26,    28,    32,    35,    38,    41,    44,    47,
123        51,    54,    58,    62,    65,    70,    74,    79,
124        84,    90,    95,   102,   109,   116,   124,   133,
125       143,   154,   166,   180,   195,   212,   231,   254,
126       279,   308,   343,   383,   430,   487,   555,   639,
127       743,   876,  1045,  1270,  1575,  2002,  2628,  3591,
128      5177,  8026, 13719, 26047, 45509, 39467, 37875, 51303,
129         0,
130 };
131 static const int16_t quantize_factor_select_offset_LF[65] = {
132       0, -21, -19, -17, -15, -12, -10,  -8,
133      -6,  -4,  -1,   1,   3,   6,   8,  10,
134      13,  15,  18,  20,  23,  26,  29,  31,
135      34,  37,  40,  43,  47,  50,  53,  57,
136      60,  64,  68,  72,  76,  80,  85,  89,
137      94,  99, 105, 110, 116, 123, 129, 136,
138     144, 152, 161, 171, 182, 194, 207, 223,
139     241, 263, 291, 328, 382, 467, 522, 522,
140     522,
141 };
142
143
144 static const int32_t quantize_intervals_MLF[9] = {
145     -89806, 89806, 278502, 494338, 759442, 1113112, 1652322, 2720256, 5190186,
146 };
147 static const int32_t invert_quantize_dither_factors_MLF[9] = {
148     89806, 89806, 98890, 116946, 148158, 205512, 333698, 734236, 1735696,
149 };
150 static const int32_t quantize_dither_factors_MLF[9] = {
151     0, 2271, 4514, 7803, 14339, 32047, 100135, 250365, 0,
152 };
153 static const int16_t quantize_factor_select_offset_MLF[9] = {
154     0, -14, 6, 29, 58, 96, 154, 270, 521,
155 };
156
157
158 static const int32_t quantize_intervals_MHF[3] = {
159     -194080, 194080, 890562,
160 };
161 static const int32_t invert_quantize_dither_factors_MHF[3] = {
162     194080, 194080, 502402,
163 };
164 static const int32_t quantize_dither_factors_MHF[3] = {
165     0, 77081, 0,
166 };
167 static const int16_t quantize_factor_select_offset_MHF[3] = {
168     0, -33, 136,
169 };
170
171
172 static const int32_t quantize_intervals_HF[5] = {
173     -163006, 163006, 542708, 1120554, 2669238,
174 };
175 static const int32_t invert_quantize_dither_factors_HF[5] = {
176     163006, 163006, 216698, 361148, 1187538,
177 };
178 static const int32_t quantize_dither_factors_HF[5] = {
179     0, 13423, 36113, 206598, 0,
180 };
181 static const int16_t quantize_factor_select_offset_HF[5] = {
182     0, -8, 33, 95, 262,
183 };
184
185 typedef const struct {
186     const int32_t *quantize_intervals;
187     const int32_t *invert_quantize_dither_factors;
188     const int32_t *quantize_dither_factors;
189     const int16_t *quantize_factor_select_offset;
190     int tables_size;
191     int32_t quantized_bits;
192     int32_t prediction_order;
193 } ConstTables;
194
195 static ConstTables tables[NB_SUBBANDS] = {
196     [LF]  = { quantize_intervals_LF,
197               invert_quantize_dither_factors_LF,
198               quantize_dither_factors_LF,
199               quantize_factor_select_offset_LF,
200               FF_ARRAY_ELEMS(quantize_intervals_LF),
201               7, 24 },
202     [MLF] = { quantize_intervals_MLF,
203               invert_quantize_dither_factors_MLF,
204               quantize_dither_factors_MLF,
205               quantize_factor_select_offset_MLF,
206               FF_ARRAY_ELEMS(quantize_intervals_MLF),
207               4, 12 },
208     [MHF] = { quantize_intervals_MHF,
209               invert_quantize_dither_factors_MHF,
210               quantize_dither_factors_MHF,
211               quantize_factor_select_offset_MHF,
212               FF_ARRAY_ELEMS(quantize_intervals_MHF),
213               2, 6 },
214     [HF]  = { quantize_intervals_HF,
215               invert_quantize_dither_factors_HF,
216               quantize_dither_factors_HF,
217               quantize_factor_select_offset_HF,
218               FF_ARRAY_ELEMS(quantize_intervals_HF),
219               3, 12 },
220 };
221
222 static const int16_t quantization_factors[32] = {
223     2048, 2093, 2139, 2186, 2233, 2282, 2332, 2383,
224     2435, 2489, 2543, 2599, 2656, 2714, 2774, 2834,
225     2896, 2960, 3025, 3091, 3158, 3228, 3298, 3371,
226     3444, 3520, 3597, 3676, 3756, 3838, 3922, 4008,
227 };
228
229
230 /* Rounded right shift with optionnal clipping */
231 #define RSHIFT_SIZE(size)                                                     \
232 av_always_inline                                                              \
233 static int##size##_t rshift##size(int##size##_t value, int shift)             \
234 {                                                                             \
235     int##size##_t rounding = (int##size##_t)1 << (shift - 1);                 \
236     int##size##_t mask = ((int##size##_t)1 << (shift + 1)) - 1;               \
237     return ((value + rounding) >> shift) - ((value & mask) == rounding);      \
238 }                                                                             \
239 av_always_inline                                                              \
240 static int##size##_t rshift##size##_clip24(int##size##_t value, int shift)    \
241 {                                                                             \
242     return av_clip_intp2(rshift##size(value, shift), 23);                     \
243 }
244 RSHIFT_SIZE(32)
245 RSHIFT_SIZE(64)
246
247
248 av_always_inline
249 static void aptx_update_codeword_history(Channel *channel)
250 {
251     int32_t cw = ((channel->quantize[0].quantized_sample & 3) << 0) +
252                  ((channel->quantize[1].quantized_sample & 2) << 1) +
253                  ((channel->quantize[2].quantized_sample & 1) << 3);
254     channel->codeword_history = (cw << 8) + (channel->codeword_history << 4);
255 }
256
257 static void aptx_generate_dither(Channel *channel)
258 {
259     int subband;
260     int64_t m;
261     int32_t d;
262
263     aptx_update_codeword_history(channel);
264
265     m = (int64_t)5184443 * (channel->codeword_history >> 7);
266     d = (m << 2) + (m >> 22);
267     for (subband = 0; subband < NB_SUBBANDS; subband++)
268         channel->dither[subband] = d << (23 - 5*subband);
269     channel->dither_parity = (d >> 25) & 1;
270 }
271
272 /*
273  * Convolution filter coefficients for the outer QMF of the QMF tree.
274  * The 2 sets are a mirror of each other.
275  */
276 static const int32_t aptx_qmf_outer_coeffs[NB_FILTERS][FILTER_TAPS] = {
277     {
278         730, -413, -9611, 43626, -121026, 269973, -585547, 2801966,
279         697128, -160481, 27611, 8478, -10043, 3511, 688, -897,
280     },
281     {
282         -897, 688, 3511, -10043, 8478, 27611, -160481, 697128,
283         2801966, -585547, 269973, -121026, 43626, -9611, -413, 730,
284     },
285 };
286
287 /*
288  * Convolution filter coefficients for the inner QMF of the QMF tree.
289  * The 2 sets are a mirror of each other.
290  */
291 static const int32_t aptx_qmf_inner_coeffs[NB_FILTERS][FILTER_TAPS] = {
292     {
293        1033, -584, -13592, 61697, -171156, 381799, -828088, 3962579,
294        985888, -226954, 39048, 11990, -14203, 4966, 973, -1268,
295     },
296     {
297       -1268, 973, 4966, -14203, 11990, 39048, -226954, 985888,
298       3962579, -828088, 381799, -171156, 61697, -13592, -584, 1033,
299     },
300 };
301
302 /*
303  * Push one sample into a circular signal buffer.
304  */
305 av_always_inline
306 static void aptx_qmf_filter_signal_push(FilterSignal *signal, int32_t sample)
307 {
308     signal->buffer[signal->pos            ] = sample;
309     signal->buffer[signal->pos+FILTER_TAPS] = sample;
310     signal->pos = (signal->pos + 1) & (FILTER_TAPS - 1);
311 }
312
313 /*
314  * Compute the convolution of the signal with the coefficients, and reduce
315  * to 24 bits by applying the specified right shifting.
316  */
317 av_always_inline
318 static int32_t aptx_qmf_convolution(FilterSignal *signal,
319                                     const int32_t coeffs[FILTER_TAPS],
320                                     int shift)
321 {
322     int32_t *sig = &signal->buffer[signal->pos];
323     int64_t e = 0;
324     int i;
325
326     for (i = 0; i < FILTER_TAPS; i++)
327         e += MUL64(sig[i], coeffs[i]);
328
329     return rshift64_clip24(e, shift);
330 }
331
332 /*
333  * Half-band QMF analysis filter realized with a polyphase FIR filter.
334  * Split into 2 subbands and downsample by 2.
335  * So for each pair of samples that goes in, one sample goes out,
336  * split into 2 separate subbands.
337  */
338 av_always_inline
339 static void aptx_qmf_polyphase_analysis(FilterSignal signal[NB_FILTERS],
340                                         const int32_t coeffs[NB_FILTERS][FILTER_TAPS],
341                                         int shift,
342                                         int32_t samples[NB_FILTERS],
343                                         int32_t *low_subband_output,
344                                         int32_t *high_subband_output)
345 {
346     int32_t subbands[NB_FILTERS];
347     int i;
348
349     for (i = 0; i < NB_FILTERS; i++) {
350         aptx_qmf_filter_signal_push(&signal[i], samples[NB_FILTERS-1-i]);
351         subbands[i] = aptx_qmf_convolution(&signal[i], coeffs[i], shift);
352     }
353
354     *low_subband_output  = av_clip_intp2(subbands[0] + subbands[1], 23);
355     *high_subband_output = av_clip_intp2(subbands[0] - subbands[1], 23);
356 }
357
358 /*
359  * Two stage QMF analysis tree.
360  * Split 4 input samples into 4 subbands and downsample by 4.
361  * So for each group of 4 samples that goes in, one sample goes out,
362  * split into 4 separate subbands.
363  */
364 static void aptx_qmf_tree_analysis(QMFAnalysis *qmf,
365                                    int32_t samples[4],
366                                    int32_t subband_samples[4])
367 {
368     int32_t intermediate_samples[4];
369     int i;
370
371     /* Split 4 input samples into 2 intermediate subbands downsampled to 2 samples */
372     for (i = 0; i < 2; i++)
373         aptx_qmf_polyphase_analysis(qmf->outer_filter_signal,
374                                     aptx_qmf_outer_coeffs, 23,
375                                     &samples[2*i],
376                                     &intermediate_samples[0+i],
377                                     &intermediate_samples[2+i]);
378
379     /* Split 2 intermediate subband samples into 4 final subbands downsampled to 1 sample */
380     for (i = 0; i < 2; i++)
381         aptx_qmf_polyphase_analysis(qmf->inner_filter_signal[i],
382                                     aptx_qmf_inner_coeffs, 23,
383                                     &intermediate_samples[2*i],
384                                     &subband_samples[2*i+0],
385                                     &subband_samples[2*i+1]);
386 }
387
388 /*
389  * Half-band QMF synthesis filter realized with a polyphase FIR filter.
390  * Join 2 subbands and upsample by 2.
391  * So for each 2 subbands sample that goes in, a pair of samples goes out.
392  */
393 av_always_inline
394 static void aptx_qmf_polyphase_synthesis(FilterSignal signal[NB_FILTERS],
395                                          const int32_t coeffs[NB_FILTERS][FILTER_TAPS],
396                                          int shift,
397                                          int32_t low_subband_input,
398                                          int32_t high_subband_input,
399                                          int32_t samples[NB_FILTERS])
400 {
401     int32_t subbands[NB_FILTERS];
402     int i;
403
404     subbands[0] = low_subband_input + high_subband_input;
405     subbands[1] = low_subband_input - high_subband_input;
406
407     for (i = 0; i < NB_FILTERS; i++) {
408         aptx_qmf_filter_signal_push(&signal[i], subbands[1-i]);
409         samples[i] = aptx_qmf_convolution(&signal[i], coeffs[i], shift);
410     }
411 }
412
413 /*
414  * Two stage QMF synthesis tree.
415  * Join 4 subbands and upsample by 4.
416  * So for each 4 subbands sample that goes in, a group of 4 samples goes out.
417  */
418 static void aptx_qmf_tree_synthesis(QMFAnalysis *qmf,
419                                     int32_t subband_samples[4],
420                                     int32_t samples[4])
421 {
422     int32_t intermediate_samples[4];
423     int i;
424
425     /* Join 4 subbands into 2 intermediate subbands upsampled to 2 samples. */
426     for (i = 0; i < 2; i++)
427         aptx_qmf_polyphase_synthesis(qmf->inner_filter_signal[i],
428                                      aptx_qmf_inner_coeffs, 22,
429                                      subband_samples[2*i+0],
430                                      subband_samples[2*i+1],
431                                      &intermediate_samples[2*i]);
432
433     /* Join 2 samples from intermediate subbands upsampled to 4 samples. */
434     for (i = 0; i < 2; i++)
435         aptx_qmf_polyphase_synthesis(qmf->outer_filter_signal,
436                                      aptx_qmf_outer_coeffs, 21,
437                                      intermediate_samples[0+i],
438                                      intermediate_samples[2+i],
439                                      &samples[2*i]);
440 }
441
442
443 av_always_inline
444 static int32_t aptx_bin_search(int32_t value, int32_t factor,
445                                const int32_t *intervals, int32_t nb_intervals)
446 {
447     int32_t idx = 0;
448     int i;
449
450     for (i = nb_intervals >> 1; i > 0; i >>= 1)
451         if (MUL64(factor, intervals[idx + i]) <= ((int64_t)value << 24))
452             idx += i;
453
454     return idx;
455 }
456
457 static void aptx_quantize_difference(Quantize *quantize,
458                                      int32_t sample_difference,
459                                      int32_t dither,
460                                      int32_t quantization_factor,
461                                      ConstTables *tables)
462 {
463     const int32_t *intervals = tables->quantize_intervals;
464     int32_t quantized_sample, dithered_sample, parity_change;
465     int32_t d, mean, interval, inv;
466     int64_t error;
467
468     quantized_sample = aptx_bin_search(FFABS(sample_difference) >> 4,
469                                        quantization_factor,
470                                        intervals, tables->tables_size);
471
472     d = rshift32_clip24(MULH(dither, dither), 7) - (1 << 23);
473     d = rshift64(MUL64(d, tables->quantize_dither_factors[quantized_sample]), 23);
474
475     intervals += quantized_sample;
476     mean = (intervals[1] + intervals[0]) / 2;
477     interval = (intervals[1] - intervals[0]) * (-(sample_difference < 0) | 1);
478
479     dithered_sample = rshift64_clip24(MUL64(dither, interval) + ((int64_t)(mean + d) << 32), 32);
480     error = ((int64_t)FFABS(sample_difference) << 20) - MUL64(dithered_sample, quantization_factor);
481     quantize->error = FFABS(rshift64(error, 23));
482
483     parity_change = quantized_sample;
484     if (error < 0)
485         quantized_sample--;
486     else
487         parity_change--;
488
489     inv = -(sample_difference < 0);
490     quantize->quantized_sample               = quantized_sample ^ inv;
491     quantize->quantized_sample_parity_change = parity_change    ^ inv;
492 }
493
494 static void aptx_encode_channel(Channel *channel, int32_t samples[4])
495 {
496     int32_t subband_samples[4];
497     int subband;
498     aptx_qmf_tree_analysis(&channel->qmf, samples, subband_samples);
499     aptx_generate_dither(channel);
500     for (subband = 0; subband < NB_SUBBANDS; subband++) {
501         int32_t diff = av_clip_intp2(subband_samples[subband] - channel->prediction[subband].predicted_sample, 23);
502         aptx_quantize_difference(&channel->quantize[subband], diff,
503                                  channel->dither[subband],
504                                  channel->invert_quantize[subband].quantization_factor,
505                                  &tables[subband]);
506     }
507 }
508
509 static void aptx_decode_channel(Channel *channel, int32_t samples[4])
510 {
511     int32_t subband_samples[4];
512     int subband;
513     for (subband = 0; subband < NB_SUBBANDS; subband++)
514         subband_samples[subband] = channel->prediction[subband].previous_reconstructed_sample;
515     aptx_qmf_tree_synthesis(&channel->qmf, subband_samples, samples);
516 }
517
518
519 static void aptx_invert_quantization(InvertQuantize *invert_quantize,
520                                      int32_t quantized_sample, int32_t dither,
521                                      ConstTables *tables)
522 {
523     int32_t qr, idx, shift, factor_select;
524
525     idx = (quantized_sample ^ -(quantized_sample < 0)) + 1;
526     qr = tables->quantize_intervals[idx] / 2;
527     if (quantized_sample < 0)
528         qr = -qr;
529
530     qr = rshift64_clip24(((int64_t)qr<<32) + MUL64(dither, tables->invert_quantize_dither_factors[idx]), 32);
531     invert_quantize->reconstructed_difference = MUL64(invert_quantize->quantization_factor, qr) >> 19;
532
533     shift = 24 - tables->quantized_bits;
534
535     /* update factor_select */
536     factor_select = 32620 * invert_quantize->factor_select;
537     factor_select = rshift32(factor_select + (tables->quantize_factor_select_offset[idx] << 15), 15);
538     invert_quantize->factor_select = av_clip(factor_select, 0, (shift << 8) | 0xFF);
539
540     /* update quantization factor */
541     idx = (invert_quantize->factor_select & 0xFF) >> 3;
542     shift -= invert_quantize->factor_select >> 8;
543     invert_quantize->quantization_factor = (quantization_factors[idx] << 11) >> shift;
544 }
545
546 static int32_t *aptx_reconstructed_differences_update(Prediction *prediction,
547                                                       int32_t reconstructed_difference,
548                                                       int order)
549 {
550     int32_t *rd1 = prediction->reconstructed_differences, *rd2 = rd1 + order;
551     int p = prediction->pos;
552
553     rd1[p] = rd2[p];
554     prediction->pos = p = (p + 1) % order;
555     rd2[p] = reconstructed_difference;
556     return &rd2[p];
557 }
558
559 static void aptx_prediction_filtering(Prediction *prediction,
560                                       int32_t reconstructed_difference,
561                                       int order)
562 {
563     int32_t reconstructed_sample, predictor, srd0;
564     int32_t *reconstructed_differences;
565     int64_t predicted_difference = 0;
566     int i;
567
568     reconstructed_sample = av_clip_intp2(reconstructed_difference + prediction->predicted_sample, 23);
569     predictor = av_clip_intp2((MUL64(prediction->s_weight[0], prediction->previous_reconstructed_sample)
570                              + MUL64(prediction->s_weight[1], reconstructed_sample)) >> 22, 23);
571     prediction->previous_reconstructed_sample = reconstructed_sample;
572
573     reconstructed_differences = aptx_reconstructed_differences_update(prediction, reconstructed_difference, order);
574     srd0 = FFDIFFSIGN(reconstructed_difference, 0) << 23;
575     for (i = 0; i < order; i++) {
576         int32_t srd = FF_SIGNBIT(reconstructed_differences[-i-1]) | 1;
577         prediction->d_weight[i] -= rshift32(prediction->d_weight[i] - srd*srd0, 8);
578         predicted_difference += MUL64(reconstructed_differences[-i], prediction->d_weight[i]);
579     }
580
581     prediction->predicted_difference = av_clip_intp2(predicted_difference >> 22, 23);
582     prediction->predicted_sample = av_clip_intp2(predictor + prediction->predicted_difference, 23);
583 }
584
585 static void aptx_process_subband(InvertQuantize *invert_quantize,
586                                  Prediction *prediction,
587                                  int32_t quantized_sample, int32_t dither,
588                                  ConstTables *tables)
589 {
590     int32_t sign, same_sign[2], weight[2], sw1, range;
591
592     aptx_invert_quantization(invert_quantize, quantized_sample, dither, tables);
593
594     sign = FFDIFFSIGN(invert_quantize->reconstructed_difference,
595                       -prediction->predicted_difference);
596     same_sign[0] = sign * prediction->prev_sign[0];
597     same_sign[1] = sign * prediction->prev_sign[1];
598     prediction->prev_sign[0] = prediction->prev_sign[1];
599     prediction->prev_sign[1] = sign | 1;
600
601     range = 0x100000;
602     sw1 = rshift32(-same_sign[1] * prediction->s_weight[1], 1);
603     sw1 = (av_clip(sw1, -range, range) & ~0xF) << 4;
604
605     range = 0x300000;
606     weight[0] = 254 * prediction->s_weight[0] + 0x800000*same_sign[0] + sw1;
607     prediction->s_weight[0] = av_clip(rshift32(weight[0], 8), -range, range);
608
609     range = 0x3C0000 - prediction->s_weight[0];
610     weight[1] = 255 * prediction->s_weight[1] + 0xC00000*same_sign[1];
611     prediction->s_weight[1] = av_clip(rshift32(weight[1], 8), -range, range);
612
613     aptx_prediction_filtering(prediction,
614                               invert_quantize->reconstructed_difference,
615                               tables->prediction_order);
616 }
617
618 static void aptx_invert_quantize_and_prediction(Channel *channel)
619 {
620     int subband;
621     for (subband = 0; subband < NB_SUBBANDS; subband++)
622         aptx_process_subband(&channel->invert_quantize[subband],
623                              &channel->prediction[subband],
624                              channel->quantize[subband].quantized_sample,
625                              channel->dither[subband],
626                              &tables[subband]);
627 }
628
629 static int32_t aptx_quantized_parity(Channel *channel)
630 {
631     int32_t parity = channel->dither_parity;
632     int subband;
633
634     for (subband = 0; subband < NB_SUBBANDS; subband++)
635         parity ^= channel->quantize[subband].quantized_sample;
636
637     return parity & 1;
638 }
639
640 /* For each sample, ensure that the parity of all subbands of all channels
641  * is 0 except once every 8 samples where the parity is forced to 1. */
642 static int aptx_check_parity(Channel channels[NB_CHANNELS], int32_t *idx)
643 {
644     int32_t parity = aptx_quantized_parity(&channels[LEFT])
645                    ^ aptx_quantized_parity(&channels[RIGHT]);
646
647     int eighth = *idx == 7;
648     *idx = (*idx + 1) & 7;
649
650     return parity ^ eighth;
651 }
652
653 static void aptx_insert_sync(Channel channels[NB_CHANNELS], int32_t *idx)
654 {
655     if (aptx_check_parity(channels, idx)) {
656         int i;
657         Channel *c;
658         static const int map[] = { 1, 2, 0, 3 };
659         Quantize *min = &channels[NB_CHANNELS-1].quantize[map[0]];
660         for (c = &channels[NB_CHANNELS-1]; c >= channels; c--)
661             for (i = 0; i < NB_SUBBANDS; i++)
662                 if (c->quantize[map[i]].error < min->error)
663                     min = &c->quantize[map[i]];
664
665         /* Forcing the desired parity is done by offsetting by 1 the quantized
666          * sample from the subband featuring the smallest quantization error. */
667         min->quantized_sample = min->quantized_sample_parity_change;
668     }
669 }
670
671 static uint16_t aptx_pack_codeword(Channel *channel)
672 {
673     int32_t parity = aptx_quantized_parity(channel);
674     return (((channel->quantize[3].quantized_sample & 0x06) | parity) << 13)
675          | (((channel->quantize[2].quantized_sample & 0x03)         ) << 11)
676          | (((channel->quantize[1].quantized_sample & 0x0F)         ) <<  7)
677          | (((channel->quantize[0].quantized_sample & 0x7F)         ) <<  0);
678 }
679
680 static void aptx_unpack_codeword(Channel *channel, uint16_t codeword)
681 {
682     channel->quantize[0].quantized_sample = sign_extend(codeword >>  0, 7);
683     channel->quantize[1].quantized_sample = sign_extend(codeword >>  7, 4);
684     channel->quantize[2].quantized_sample = sign_extend(codeword >> 11, 2);
685     channel->quantize[3].quantized_sample = sign_extend(codeword >> 13, 3);
686     channel->quantize[3].quantized_sample = (channel->quantize[3].quantized_sample & ~1)
687                                           | aptx_quantized_parity(channel);
688 }
689
690 static void aptx_encode_samples(AptXContext *ctx,
691                                 int32_t samples[NB_CHANNELS][4],
692                                 uint8_t output[2*NB_CHANNELS])
693 {
694     int channel;
695     for (channel = 0; channel < NB_CHANNELS; channel++)
696         aptx_encode_channel(&ctx->channels[channel], samples[channel]);
697
698     aptx_insert_sync(ctx->channels, &ctx->sync_idx);
699
700     for (channel = 0; channel < NB_CHANNELS; channel++) {
701         aptx_invert_quantize_and_prediction(&ctx->channels[channel]);
702         AV_WB16(output + 2*channel, aptx_pack_codeword(&ctx->channels[channel]));
703     }
704 }
705
706 static int aptx_decode_samples(AptXContext *ctx,
707                                 const uint8_t input[2*NB_CHANNELS],
708                                 int32_t samples[NB_CHANNELS][4])
709 {
710     int channel, ret;
711
712     for (channel = 0; channel < NB_CHANNELS; channel++) {
713         uint16_t codeword;
714         aptx_generate_dither(&ctx->channels[channel]);
715
716         codeword = AV_RB16(input + 2*channel);
717         aptx_unpack_codeword(&ctx->channels[channel], codeword);
718         aptx_invert_quantize_and_prediction(&ctx->channels[channel]);
719     }
720
721     ret = aptx_check_parity(ctx->channels, &ctx->sync_idx);
722
723     for (channel = 0; channel < NB_CHANNELS; channel++)
724         aptx_decode_channel(&ctx->channels[channel], samples[channel]);
725
726     return ret;
727 }
728
729
730 static av_cold int aptx_init(AVCodecContext *avctx)
731 {
732     AptXContext *s = avctx->priv_data;
733     int chan, subband;
734
735     if (avctx->frame_size == 0)
736         avctx->frame_size = 1024;
737
738     if (avctx->frame_size & 3) {
739         av_log(avctx, AV_LOG_ERROR, "Frame size must be a multiple of 4 samples\n");
740         return AVERROR(EINVAL);
741     }
742
743     for (chan = 0; chan < NB_CHANNELS; chan++) {
744         Channel *channel = &s->channels[chan];
745         for (subband = 0; subband < NB_SUBBANDS; subband++) {
746             Prediction *prediction = &channel->prediction[subband];
747             prediction->prev_sign[0] = 1;
748             prediction->prev_sign[1] = 1;
749         }
750     }
751
752     ff_af_queue_init(avctx, &s->afq);
753     return 0;
754 }
755
756 static int aptx_decode_frame(AVCodecContext *avctx, void *data,
757                              int *got_frame_ptr, AVPacket *avpkt)
758 {
759     AptXContext *s = avctx->priv_data;
760     AVFrame *frame = data;
761     int pos, channel, sample, ret;
762
763     if (avpkt->size < 4) {
764         av_log(avctx, AV_LOG_ERROR, "Packet is too small\n");
765         return AVERROR_INVALIDDATA;
766     }
767
768     /* get output buffer */
769     frame->channels = NB_CHANNELS;
770     frame->format = AV_SAMPLE_FMT_S32P;
771     frame->nb_samples = avpkt->size & ~3;
772     if ((ret = ff_get_buffer(avctx, frame, 0)) < 0)
773         return ret;
774
775     for (pos = 0; pos < frame->nb_samples; pos += 4) {
776         int32_t samples[NB_CHANNELS][4];
777
778         if (aptx_decode_samples(s, &avpkt->data[pos], samples)) {
779             av_log(avctx, AV_LOG_ERROR, "Synchronization error\n");
780             return AVERROR_INVALIDDATA;
781         }
782
783         for (channel = 0; channel < NB_CHANNELS; channel++)
784             for (sample = 0; sample < 4; sample++)
785                 AV_WN32A(&frame->data[channel][4*(sample+pos)],
786                          samples[channel][sample] << 8);
787     }
788
789     *got_frame_ptr = 1;
790     return frame->nb_samples;
791 }
792
793 static int aptx_encode_frame(AVCodecContext *avctx, AVPacket *avpkt,
794                              const AVFrame *frame, int *got_packet_ptr)
795 {
796     AptXContext *s = avctx->priv_data;
797     int pos, channel, sample, ret;
798
799     if ((ret = ff_af_queue_add(&s->afq, frame)) < 0)
800         return ret;
801
802     if ((ret = ff_alloc_packet2(avctx, avpkt, frame->nb_samples, 0)) < 0)
803         return ret;
804
805     for (pos = 0; pos < frame->nb_samples; pos += 4) {
806         int32_t samples[NB_CHANNELS][4];
807
808         for (channel = 0; channel < NB_CHANNELS; channel++)
809             for (sample = 0; sample < 4; sample++)
810                 samples[channel][sample] = (int32_t)AV_RN32A(&frame->data[channel][4*(sample+pos)]) >> 8;
811
812         aptx_encode_samples(s, samples, avpkt->data + pos);
813     }
814
815     ff_af_queue_remove(&s->afq, frame->nb_samples, &avpkt->pts, &avpkt->duration);
816     *got_packet_ptr = 1;
817     return 0;
818 }
819
820 static av_cold int aptx_close(AVCodecContext *avctx)
821 {
822     AptXContext *s = avctx->priv_data;
823     ff_af_queue_close(&s->afq);
824     return 0;
825 }
826
827
828 #if CONFIG_APTX_DECODER
829 AVCodec ff_aptx_decoder = {
830     .name                  = "aptx",
831     .long_name             = NULL_IF_CONFIG_SMALL("aptX (Audio Processing Technology for Bluetooth)"),
832     .type                  = AVMEDIA_TYPE_AUDIO,
833     .id                    = AV_CODEC_ID_APTX,
834     .priv_data_size        = sizeof(AptXContext),
835     .init                  = aptx_init,
836     .decode                = aptx_decode_frame,
837     .close                 = aptx_close,
838     .capabilities          = AV_CODEC_CAP_DR1,
839     .channel_layouts       = (const uint64_t[]) { AV_CH_LAYOUT_STEREO, 0},
840     .sample_fmts           = (const enum AVSampleFormat[]) { AV_SAMPLE_FMT_S32P,
841                                                              AV_SAMPLE_FMT_NONE },
842 };
843 #endif
844
845 #if CONFIG_APTX_ENCODER
846 AVCodec ff_aptx_encoder = {
847     .name                  = "aptx",
848     .long_name             = NULL_IF_CONFIG_SMALL("aptX (Audio Processing Technology for Bluetooth)"),
849     .type                  = AVMEDIA_TYPE_AUDIO,
850     .id                    = AV_CODEC_ID_APTX,
851     .priv_data_size        = sizeof(AptXContext),
852     .init                  = aptx_init,
853     .encode2               = aptx_encode_frame,
854     .close                 = aptx_close,
855     .channel_layouts       = (const uint64_t[]) { AV_CH_LAYOUT_STEREO, 0},
856     .sample_fmts           = (const enum AVSampleFormat[]) { AV_SAMPLE_FMT_S32P,
857                                                              AV_SAMPLE_FMT_NONE },
858     .supported_samplerates = (const int[]) {8000, 16000, 24000, 32000, 44100, 48000, 0},
859 };
860 #endif