avformat/mxfdec: fix sorting of index segments
[ffmpeg.git] / 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     int hd;
93     int block_size;
94     int32_t sync_idx;
95     Channel channels[NB_CHANNELS];
96     AudioFrameQueue afq;
97 } AptXContext;
98
99
100 static const int32_t quantize_intervals_LF[65] = {
101       -9948,    9948,   29860,   49808,   69822,   89926,  110144,  130502,
102      151026,  171738,  192666,  213832,  235264,  256982,  279014,  301384,
103      324118,  347244,  370790,  394782,  419250,  444226,  469742,  495832,
104      522536,  549890,  577936,  606720,  636290,  666700,  698006,  730270,
105      763562,  797958,  833538,  870398,  908640,  948376,  989740, 1032874,
106     1077948, 1125150, 1174700, 1226850, 1281900, 1340196, 1402156, 1468282,
107     1539182, 1615610, 1698514, 1789098, 1888944, 2000168, 2125700, 2269750,
108     2438670, 2642660, 2899462, 3243240, 3746078, 4535138, 5664098, 7102424,
109     8897462,
110 };
111 static const int32_t invert_quantize_dither_factors_LF[65] = {
112        9948,   9948,   9962,   9988,  10026,  10078,  10142,  10218,
113       10306,  10408,  10520,  10646,  10784,  10934,  11098,  11274,
114       11462,  11664,  11880,  12112,  12358,  12618,  12898,  13194,
115       13510,  13844,  14202,  14582,  14988,  15422,  15884,  16380,
116       16912,  17484,  18098,  18762,  19480,  20258,  21106,  22030,
117       23044,  24158,  25390,  26760,  28290,  30008,  31954,  34172,
118       36728,  39700,  43202,  47382,  52462,  58762,  66770,  77280,
119       91642, 112348, 144452, 199326, 303512, 485546, 643414, 794914,
120     1000124,
121 };
122 static const int32_t quantize_dither_factors_LF[65] = {
123         0,     4,     7,    10,    13,    16,    19,    22,
124        26,    28,    32,    35,    38,    41,    44,    47,
125        51,    54,    58,    62,    65,    70,    74,    79,
126        84,    90,    95,   102,   109,   116,   124,   133,
127       143,   154,   166,   180,   195,   212,   231,   254,
128       279,   308,   343,   383,   430,   487,   555,   639,
129       743,   876,  1045,  1270,  1575,  2002,  2628,  3591,
130      5177,  8026, 13719, 26047, 45509, 39467, 37875, 51303,
131         0,
132 };
133 static const int16_t quantize_factor_select_offset_LF[65] = {
134       0, -21, -19, -17, -15, -12, -10,  -8,
135      -6,  -4,  -1,   1,   3,   6,   8,  10,
136      13,  15,  18,  20,  23,  26,  29,  31,
137      34,  37,  40,  43,  47,  50,  53,  57,
138      60,  64,  68,  72,  76,  80,  85,  89,
139      94,  99, 105, 110, 116, 123, 129, 136,
140     144, 152, 161, 171, 182, 194, 207, 223,
141     241, 263, 291, 328, 382, 467, 522, 522,
142     522,
143 };
144
145
146 static const int32_t quantize_intervals_MLF[9] = {
147     -89806, 89806, 278502, 494338, 759442, 1113112, 1652322, 2720256, 5190186,
148 };
149 static const int32_t invert_quantize_dither_factors_MLF[9] = {
150     89806, 89806, 98890, 116946, 148158, 205512, 333698, 734236, 1735696,
151 };
152 static const int32_t quantize_dither_factors_MLF[9] = {
153     0, 2271, 4514, 7803, 14339, 32047, 100135, 250365, 0,
154 };
155 static const int16_t quantize_factor_select_offset_MLF[9] = {
156     0, -14, 6, 29, 58, 96, 154, 270, 521,
157 };
158
159
160 static const int32_t quantize_intervals_MHF[3] = {
161     -194080, 194080, 890562,
162 };
163 static const int32_t invert_quantize_dither_factors_MHF[3] = {
164     194080, 194080, 502402,
165 };
166 static const int32_t quantize_dither_factors_MHF[3] = {
167     0, 77081, 0,
168 };
169 static const int16_t quantize_factor_select_offset_MHF[3] = {
170     0, -33, 136,
171 };
172
173
174 static const int32_t quantize_intervals_HF[5] = {
175     -163006, 163006, 542708, 1120554, 2669238,
176 };
177 static const int32_t invert_quantize_dither_factors_HF[5] = {
178     163006, 163006, 216698, 361148, 1187538,
179 };
180 static const int32_t quantize_dither_factors_HF[5] = {
181     0, 13423, 36113, 206598, 0,
182 };
183 static const int16_t quantize_factor_select_offset_HF[5] = {
184     0, -8, 33, 95, 262,
185 };
186
187
188 static const int32_t hd_quantize_intervals_LF[257] = {
189       -2436,    2436,    7308,   12180,   17054,   21930,   26806,   31686,
190       36566,   41450,   46338,   51230,   56124,   61024,   65928,   70836,
191       75750,   80670,   85598,   90530,   95470,  100418,  105372,  110336,
192      115308,  120288,  125278,  130276,  135286,  140304,  145334,  150374,
193      155426,  160490,  165566,  170654,  175756,  180870,  185998,  191138,
194      196294,  201466,  206650,  211850,  217068,  222300,  227548,  232814,
195      238096,  243396,  248714,  254050,  259406,  264778,  270172,  275584,
196      281018,  286470,  291944,  297440,  302956,  308496,  314056,  319640,
197      325248,  330878,  336532,  342212,  347916,  353644,  359398,  365178,
198      370986,  376820,  382680,  388568,  394486,  400430,  406404,  412408,
199      418442,  424506,  430600,  436726,  442884,  449074,  455298,  461554,
200      467844,  474168,  480528,  486922,  493354,  499820,  506324,  512866,
201      519446,  526064,  532722,  539420,  546160,  552940,  559760,  566624,
202      573532,  580482,  587478,  594520,  601606,  608740,  615920,  623148,
203      630426,  637754,  645132,  652560,  660042,  667576,  675164,  682808,
204      690506,  698262,  706074,  713946,  721876,  729868,  737920,  746036,
205      754216,  762460,  770770,  779148,  787594,  796108,  804694,  813354,
206      822086,  830892,  839774,  848736,  857776,  866896,  876100,  885386,
207      894758,  904218,  913766,  923406,  933138,  942964,  952886,  962908,
208      973030,  983254,  993582, 1004020, 1014566, 1025224, 1035996, 1046886,
209     1057894, 1069026, 1080284, 1091670, 1103186, 1114838, 1126628, 1138558,
210     1150634, 1162858, 1175236, 1187768, 1200462, 1213320, 1226346, 1239548,
211     1252928, 1266490, 1280242, 1294188, 1308334, 1322688, 1337252, 1352034,
212     1367044, 1382284, 1397766, 1413494, 1429478, 1445728, 1462252, 1479058,
213     1496158, 1513562, 1531280, 1549326, 1567710, 1586446, 1605550, 1625034,
214     1644914, 1665208, 1685932, 1707108, 1728754, 1750890, 1773542, 1796732,
215     1820488, 1844840, 1869816, 1895452, 1921780, 1948842, 1976680, 2005338,
216     2034868, 2065322, 2096766, 2129260, 2162880, 2197708, 2233832, 2271352,
217     2310384, 2351050, 2393498, 2437886, 2484404, 2533262, 2584710, 2639036,
218     2696578, 2757738, 2822998, 2892940, 2968278, 3049896, 3138912, 3236760,
219     3345312, 3467068, 3605434, 3765154, 3952904, 4177962, 4452178, 4787134,
220     5187290, 5647128, 6159120, 6720518, 7332904, 8000032, 8726664, 9518152,
221     10380372,
222 };
223 static const int32_t hd_invert_quantize_dither_factors_LF[257] = {
224       2436,   2436,   2436,   2436,   2438,   2438,   2438,   2440,
225       2442,   2442,   2444,   2446,   2448,   2450,   2454,   2456,
226       2458,   2462,   2464,   2468,   2472,   2476,   2480,   2484,
227       2488,   2492,   2498,   2502,   2506,   2512,   2518,   2524,
228       2528,   2534,   2540,   2548,   2554,   2560,   2568,   2574,
229       2582,   2588,   2596,   2604,   2612,   2620,   2628,   2636,
230       2646,   2654,   2664,   2672,   2682,   2692,   2702,   2712,
231       2722,   2732,   2742,   2752,   2764,   2774,   2786,   2798,
232       2810,   2822,   2834,   2846,   2858,   2870,   2884,   2896,
233       2910,   2924,   2938,   2952,   2966,   2980,   2994,   3010,
234       3024,   3040,   3056,   3070,   3086,   3104,   3120,   3136,
235       3154,   3170,   3188,   3206,   3224,   3242,   3262,   3280,
236       3300,   3320,   3338,   3360,   3380,   3400,   3422,   3442,
237       3464,   3486,   3508,   3532,   3554,   3578,   3602,   3626,
238       3652,   3676,   3702,   3728,   3754,   3780,   3808,   3836,
239       3864,   3892,   3920,   3950,   3980,   4010,   4042,   4074,
240       4106,   4138,   4172,   4206,   4240,   4276,   4312,   4348,
241       4384,   4422,   4460,   4500,   4540,   4580,   4622,   4664,
242       4708,   4752,   4796,   4842,   4890,   4938,   4986,   5036,
243       5086,   5138,   5192,   5246,   5300,   5358,   5416,   5474,
244       5534,   5596,   5660,   5726,   5792,   5860,   5930,   6002,
245       6074,   6150,   6226,   6306,   6388,   6470,   6556,   6644,
246       6736,   6828,   6924,   7022,   7124,   7228,   7336,   7448,
247       7562,   7680,   7802,   7928,   8058,   8192,   8332,   8476,
248       8624,   8780,   8940,   9106,   9278,   9458,   9644,   9840,
249      10042,  10252,  10472,  10702,  10942,  11194,  11458,  11734,
250      12024,  12328,  12648,  12986,  13342,  13720,  14118,  14540,
251      14990,  15466,  15976,  16520,  17102,  17726,  18398,  19124,
252      19908,  20760,  21688,  22702,  23816,  25044,  26404,  27922,
253      29622,  31540,  33720,  36222,  39116,  42502,  46514,  51334,
254      57218,  64536,  73830,  85890, 101860, 123198, 151020, 183936,
255     216220, 243618, 268374, 293022, 319362, 347768, 378864, 412626, 449596,
256 };
257 static const int32_t hd_quantize_dither_factors_LF[256] = {
258        0,    0,    0,    1,    0,    0,    1,    1,
259        0,    1,    1,    1,    1,    1,    1,    1,
260        1,    1,    1,    1,    1,    1,    1,    1,
261        1,    2,    1,    1,    2,    2,    2,    1,
262        2,    2,    2,    2,    2,    2,    2,    2,
263        2,    2,    2,    2,    2,    2,    2,    3,
264        2,    3,    2,    3,    3,    3,    3,    3,
265        3,    3,    3,    3,    3,    3,    3,    3,
266        3,    3,    3,    3,    3,    4,    3,    4,
267        4,    4,    4,    4,    4,    4,    4,    4,
268        4,    4,    4,    4,    5,    4,    4,    5,
269        4,    5,    5,    5,    5,    5,    5,    5,
270        5,    5,    6,    5,    5,    6,    5,    6,
271        6,    6,    6,    6,    6,    6,    6,    7,
272        6,    7,    7,    7,    7,    7,    7,    7,
273        7,    7,    8,    8,    8,    8,    8,    8,
274        8,    9,    9,    9,    9,    9,    9,    9,
275       10,   10,   10,   10,   10,   11,   11,   11,
276       11,   11,   12,   12,   12,   12,   13,   13,
277       13,   14,   14,   14,   15,   15,   15,   15,
278       16,   16,   17,   17,   17,   18,   18,   18,
279       19,   19,   20,   21,   21,   22,   22,   23,
280       23,   24,   25,   26,   26,   27,   28,   29,
281       30,   31,   32,   33,   34,   35,   36,   37,
282       39,   40,   42,   43,   45,   47,   49,   51,
283       53,   55,   58,   60,   63,   66,   69,   73,
284       76,   80,   85,   89,   95,  100,  106,  113,
285      119,  128,  136,  146,  156,  168,  182,  196,
286      213,  232,  254,  279,  307,  340,  380,  425,
287      480,  545,  626,  724,  847, 1003, 1205, 1471,
288     1830, 2324, 3015, 3993, 5335, 6956, 8229, 8071,
289     6850, 6189, 6162, 6585, 7102, 7774, 8441, 9243,
290 };
291 static const int16_t hd_quantize_factor_select_offset_LF[257] = {
292       0, -22, -21, -21, -20, -20, -19, -19,
293     -18, -18, -17, -17, -16, -16, -15, -14,
294     -14, -13, -13, -12, -12, -11, -11, -10,
295     -10,  -9,  -9,  -8,  -7,  -7,  -6,  -6,
296      -5,  -5,  -4,  -4,  -3,  -3,  -2,  -1,
297      -1,   0,   0,   1,   1,   2,   2,   3,
298       4,   4,   5,   5,   6,   6,   7,   8,
299       8,   9,   9,  10,  11,  11,  12,  12,
300      13,  14,  14,  15,  15,  16,  17,  17,
301      18,  19,  19,  20,  20,  21,  22,  22,
302      23,  24,  24,  25,  26,  26,  27,  28,
303      28,  29,  30,  30,  31,  32,  33,  33,
304      34,  35,  35,  36,  37,  38,  38,  39,
305      40,  41,  41,  42,  43,  44,  44,  45,
306      46,  47,  48,  48,  49,  50,  51,  52,
307      52,  53,  54,  55,  56,  57,  58,  58,
308      59,  60,  61,  62,  63,  64,  65,  66,
309      67,  68,  69,  69,  70,  71,  72,  73,
310      74,  75,  77,  78,  79,  80,  81,  82,
311      83,  84,  85,  86,  87,  89,  90,  91,
312      92,  93,  94,  96,  97,  98,  99, 101,
313     102, 103, 105, 106, 107, 109, 110, 112,
314     113, 115, 116, 118, 119, 121, 122, 124,
315     125, 127, 129, 130, 132, 134, 136, 137,
316     139, 141, 143, 145, 147, 149, 151, 153,
317     155, 158, 160, 162, 164, 167, 169, 172,
318     174, 177, 180, 182, 185, 188, 191, 194,
319     197, 201, 204, 208, 211, 215, 219, 223,
320     227, 232, 236, 241, 246, 251, 257, 263,
321     269, 275, 283, 290, 298, 307, 317, 327,
322     339, 352, 367, 384, 404, 429, 458, 494,
323     522, 522, 522, 522, 522, 522, 522, 522, 522,
324 };
325
326
327 static const int32_t hd_quantize_intervals_MLF[33] = {
328       -21236,   21236,   63830,  106798,  150386,  194832,  240376,  287258,
329       335726,  386034,  438460,  493308,  550924,  611696,  676082,  744626,
330       817986,  896968,  982580, 1076118, 1179278, 1294344, 1424504, 1574386,
331      1751090, 1966260, 2240868, 2617662, 3196432, 4176450, 5658260, 7671068,
332     10380372,
333 };
334 static const int32_t hd_invert_quantize_dither_factors_MLF[33] = {
335     21236,  21236,  21360,  21608,  21978,  22468,  23076,   23806,
336     24660,  25648,  26778,  28070,  29544,  31228,  33158,   35386,
337     37974,  41008,  44606,  48934,  54226,  60840,  69320,   80564,
338     96140, 119032, 155576, 221218, 357552, 622468, 859344, 1153464, 1555840,
339 };
340 static const int32_t hd_quantize_dither_factors_MLF[32] = {
341        0,   31,    62,    93,   123,   152,   183,    214,
342      247,  283,   323,   369,   421,   483,   557,    647,
343      759,  900,  1082,  1323,  1654,  2120,  2811,   3894,
344     5723, 9136, 16411, 34084, 66229, 59219, 73530, 100594,
345 };
346 static const int16_t hd_quantize_factor_select_offset_MLF[33] = {
347       0, -21, -16, -12,  -7,  -2,   3,   8,
348      13,  19,  24,  30,  36,  43,  50,  57,
349      65,  74,  83,  93, 104, 117, 131, 147,
350     166, 189, 219, 259, 322, 427, 521, 521, 521,
351 };
352
353
354 static const int32_t hd_quantize_intervals_MHF[9] = {
355     -95044, 95044, 295844, 528780, 821332, 1226438, 1890540, 3344850, 6450664,
356 };
357 static const int32_t hd_invert_quantize_dither_factors_MHF[9] = {
358     95044, 95044, 105754, 127180, 165372, 39736, 424366, 1029946, 2075866,
359 };
360 static const int32_t hd_quantize_dither_factors_MHF[8] = {
361     0, 2678, 5357, 9548, -31409, 96158, 151395, 261480,
362 };
363 static const int16_t hd_quantize_factor_select_offset_MHF[9] = {
364     0, -17, 5, 30, 62, 105, 177, 334, 518,
365 };
366
367
368 static const int32_t hd_quantize_intervals_HF[17] = {
369      -45754,   45754,  138496,  234896,  337336,  448310,  570738,  708380,
370      866534, 1053262, 1281958, 1577438, 1993050, 2665984, 3900982, 5902844,
371     8897462,
372 };
373 static const int32_t hd_invert_quantize_dither_factors_HF[17] = {
374     45754,  45754,  46988,  49412,  53026,  57950,  64478,   73164,
375     84988, 101740, 126958, 168522, 247092, 425842, 809154, 1192708, 1801910,
376 };
377 static const int32_t hd_quantize_dither_factors_HF[16] = {
378        0,  309,   606,   904,  1231,  1632,  2172,   2956,
379     4188, 6305, 10391, 19643, 44688, 95828, 95889, 152301,
380 };
381 static const int16_t hd_quantize_factor_select_offset_HF[17] = {
382      0, -18,  -8,   2,  13,  25,  38,  53,
383     70,  90, 115, 147, 192, 264, 398, 521, 521,
384 };
385
386 typedef const struct {
387     const int32_t *quantize_intervals;
388     const int32_t *invert_quantize_dither_factors;
389     const int32_t *quantize_dither_factors;
390     const int16_t *quantize_factor_select_offset;
391     int tables_size;
392     int32_t factor_max;
393     int32_t prediction_order;
394 } ConstTables;
395
396 static ConstTables tables[2][NB_SUBBANDS] = {
397     {
398         [LF]  = { quantize_intervals_LF,
399                   invert_quantize_dither_factors_LF,
400                   quantize_dither_factors_LF,
401                   quantize_factor_select_offset_LF,
402                   FF_ARRAY_ELEMS(quantize_intervals_LF),
403                   0x11FF, 24 },
404         [MLF] = { quantize_intervals_MLF,
405                   invert_quantize_dither_factors_MLF,
406                   quantize_dither_factors_MLF,
407                   quantize_factor_select_offset_MLF,
408                   FF_ARRAY_ELEMS(quantize_intervals_MLF),
409                   0x14FF, 12 },
410         [MHF] = { quantize_intervals_MHF,
411                   invert_quantize_dither_factors_MHF,
412                   quantize_dither_factors_MHF,
413                   quantize_factor_select_offset_MHF,
414                   FF_ARRAY_ELEMS(quantize_intervals_MHF),
415                   0x16FF, 6 },
416         [HF]  = { quantize_intervals_HF,
417                   invert_quantize_dither_factors_HF,
418                   quantize_dither_factors_HF,
419                   quantize_factor_select_offset_HF,
420                   FF_ARRAY_ELEMS(quantize_intervals_HF),
421                   0x15FF, 12 },
422     },
423     {
424         [LF]  = { hd_quantize_intervals_LF,
425                   hd_invert_quantize_dither_factors_LF,
426                   hd_quantize_dither_factors_LF,
427                   hd_quantize_factor_select_offset_LF,
428                   FF_ARRAY_ELEMS(hd_quantize_intervals_LF),
429                   0x11FF, 24 },
430         [MLF] = { hd_quantize_intervals_MLF,
431                   hd_invert_quantize_dither_factors_MLF,
432                   hd_quantize_dither_factors_MLF,
433                   hd_quantize_factor_select_offset_MLF,
434                   FF_ARRAY_ELEMS(hd_quantize_intervals_MLF),
435                   0x14FF, 12 },
436         [MHF] = { hd_quantize_intervals_MHF,
437                   hd_invert_quantize_dither_factors_MHF,
438                   hd_quantize_dither_factors_MHF,
439                   hd_quantize_factor_select_offset_MHF,
440                   FF_ARRAY_ELEMS(hd_quantize_intervals_MHF),
441                   0x16FF, 6 },
442         [HF]  = { hd_quantize_intervals_HF,
443                   hd_invert_quantize_dither_factors_HF,
444                   hd_quantize_dither_factors_HF,
445                   hd_quantize_factor_select_offset_HF,
446                   FF_ARRAY_ELEMS(hd_quantize_intervals_HF),
447                   0x15FF, 12 },
448     }
449 };
450
451 static const int16_t quantization_factors[32] = {
452     2048, 2093, 2139, 2186, 2233, 2282, 2332, 2383,
453     2435, 2489, 2543, 2599, 2656, 2714, 2774, 2834,
454     2896, 2960, 3025, 3091, 3158, 3228, 3298, 3371,
455     3444, 3520, 3597, 3676, 3756, 3838, 3922, 4008,
456 };
457
458
459 /* Rounded right shift with optionnal clipping */
460 #define RSHIFT_SIZE(size)                                                     \
461 av_always_inline                                                              \
462 static int##size##_t rshift##size(int##size##_t value, int shift)             \
463 {                                                                             \
464     int##size##_t rounding = (int##size##_t)1 << (shift - 1);                 \
465     int##size##_t mask = ((int##size##_t)1 << (shift + 1)) - 1;               \
466     return ((value + rounding) >> shift) - ((value & mask) == rounding);      \
467 }                                                                             \
468 av_always_inline                                                              \
469 static int##size##_t rshift##size##_clip24(int##size##_t value, int shift)    \
470 {                                                                             \
471     return av_clip_intp2(rshift##size(value, shift), 23);                     \
472 }
473 RSHIFT_SIZE(32)
474 RSHIFT_SIZE(64)
475
476
477 av_always_inline
478 static void aptx_update_codeword_history(Channel *channel)
479 {
480     int32_t cw = ((channel->quantize[0].quantized_sample & 3) << 0) +
481                  ((channel->quantize[1].quantized_sample & 2) << 1) +
482                  ((channel->quantize[2].quantized_sample & 1) << 3);
483     channel->codeword_history = (cw << 8) + (channel->codeword_history << 4);
484 }
485
486 static void aptx_generate_dither(Channel *channel)
487 {
488     int subband;
489     int64_t m;
490     int32_t d;
491
492     aptx_update_codeword_history(channel);
493
494     m = (int64_t)5184443 * (channel->codeword_history >> 7);
495     d = (m << 2) + (m >> 22);
496     for (subband = 0; subband < NB_SUBBANDS; subband++)
497         channel->dither[subband] = d << (23 - 5*subband);
498     channel->dither_parity = (d >> 25) & 1;
499 }
500
501 /*
502  * Convolution filter coefficients for the outer QMF of the QMF tree.
503  * The 2 sets are a mirror of each other.
504  */
505 static const int32_t aptx_qmf_outer_coeffs[NB_FILTERS][FILTER_TAPS] = {
506     {
507         730, -413, -9611, 43626, -121026, 269973, -585547, 2801966,
508         697128, -160481, 27611, 8478, -10043, 3511, 688, -897,
509     },
510     {
511         -897, 688, 3511, -10043, 8478, 27611, -160481, 697128,
512         2801966, -585547, 269973, -121026, 43626, -9611, -413, 730,
513     },
514 };
515
516 /*
517  * Convolution filter coefficients for the inner QMF of the QMF tree.
518  * The 2 sets are a mirror of each other.
519  */
520 static const int32_t aptx_qmf_inner_coeffs[NB_FILTERS][FILTER_TAPS] = {
521     {
522        1033, -584, -13592, 61697, -171156, 381799, -828088, 3962579,
523        985888, -226954, 39048, 11990, -14203, 4966, 973, -1268,
524     },
525     {
526       -1268, 973, 4966, -14203, 11990, 39048, -226954, 985888,
527       3962579, -828088, 381799, -171156, 61697, -13592, -584, 1033,
528     },
529 };
530
531 /*
532  * Push one sample into a circular signal buffer.
533  */
534 av_always_inline
535 static void aptx_qmf_filter_signal_push(FilterSignal *signal, int32_t sample)
536 {
537     signal->buffer[signal->pos            ] = sample;
538     signal->buffer[signal->pos+FILTER_TAPS] = sample;
539     signal->pos = (signal->pos + 1) & (FILTER_TAPS - 1);
540 }
541
542 /*
543  * Compute the convolution of the signal with the coefficients, and reduce
544  * to 24 bits by applying the specified right shifting.
545  */
546 av_always_inline
547 static int32_t aptx_qmf_convolution(FilterSignal *signal,
548                                     const int32_t coeffs[FILTER_TAPS],
549                                     int shift)
550 {
551     int32_t *sig = &signal->buffer[signal->pos];
552     int64_t e = 0;
553     int i;
554
555     for (i = 0; i < FILTER_TAPS; i++)
556         e += MUL64(sig[i], coeffs[i]);
557
558     return rshift64_clip24(e, shift);
559 }
560
561 /*
562  * Half-band QMF analysis filter realized with a polyphase FIR filter.
563  * Split into 2 subbands and downsample by 2.
564  * So for each pair of samples that goes in, one sample goes out,
565  * split into 2 separate subbands.
566  */
567 av_always_inline
568 static void aptx_qmf_polyphase_analysis(FilterSignal signal[NB_FILTERS],
569                                         const int32_t coeffs[NB_FILTERS][FILTER_TAPS],
570                                         int shift,
571                                         int32_t samples[NB_FILTERS],
572                                         int32_t *low_subband_output,
573                                         int32_t *high_subband_output)
574 {
575     int32_t subbands[NB_FILTERS];
576     int i;
577
578     for (i = 0; i < NB_FILTERS; i++) {
579         aptx_qmf_filter_signal_push(&signal[i], samples[NB_FILTERS-1-i]);
580         subbands[i] = aptx_qmf_convolution(&signal[i], coeffs[i], shift);
581     }
582
583     *low_subband_output  = av_clip_intp2(subbands[0] + subbands[1], 23);
584     *high_subband_output = av_clip_intp2(subbands[0] - subbands[1], 23);
585 }
586
587 /*
588  * Two stage QMF analysis tree.
589  * Split 4 input samples into 4 subbands and downsample by 4.
590  * So for each group of 4 samples that goes in, one sample goes out,
591  * split into 4 separate subbands.
592  */
593 static void aptx_qmf_tree_analysis(QMFAnalysis *qmf,
594                                    int32_t samples[4],
595                                    int32_t subband_samples[4])
596 {
597     int32_t intermediate_samples[4];
598     int i;
599
600     /* Split 4 input samples into 2 intermediate subbands downsampled to 2 samples */
601     for (i = 0; i < 2; i++)
602         aptx_qmf_polyphase_analysis(qmf->outer_filter_signal,
603                                     aptx_qmf_outer_coeffs, 23,
604                                     &samples[2*i],
605                                     &intermediate_samples[0+i],
606                                     &intermediate_samples[2+i]);
607
608     /* Split 2 intermediate subband samples into 4 final subbands downsampled to 1 sample */
609     for (i = 0; i < 2; i++)
610         aptx_qmf_polyphase_analysis(qmf->inner_filter_signal[i],
611                                     aptx_qmf_inner_coeffs, 23,
612                                     &intermediate_samples[2*i],
613                                     &subband_samples[2*i+0],
614                                     &subband_samples[2*i+1]);
615 }
616
617 /*
618  * Half-band QMF synthesis filter realized with a polyphase FIR filter.
619  * Join 2 subbands and upsample by 2.
620  * So for each 2 subbands sample that goes in, a pair of samples goes out.
621  */
622 av_always_inline
623 static void aptx_qmf_polyphase_synthesis(FilterSignal signal[NB_FILTERS],
624                                          const int32_t coeffs[NB_FILTERS][FILTER_TAPS],
625                                          int shift,
626                                          int32_t low_subband_input,
627                                          int32_t high_subband_input,
628                                          int32_t samples[NB_FILTERS])
629 {
630     int32_t subbands[NB_FILTERS];
631     int i;
632
633     subbands[0] = low_subband_input + high_subband_input;
634     subbands[1] = low_subband_input - high_subband_input;
635
636     for (i = 0; i < NB_FILTERS; i++) {
637         aptx_qmf_filter_signal_push(&signal[i], subbands[1-i]);
638         samples[i] = aptx_qmf_convolution(&signal[i], coeffs[i], shift);
639     }
640 }
641
642 /*
643  * Two stage QMF synthesis tree.
644  * Join 4 subbands and upsample by 4.
645  * So for each 4 subbands sample that goes in, a group of 4 samples goes out.
646  */
647 static void aptx_qmf_tree_synthesis(QMFAnalysis *qmf,
648                                     int32_t subband_samples[4],
649                                     int32_t samples[4])
650 {
651     int32_t intermediate_samples[4];
652     int i;
653
654     /* Join 4 subbands into 2 intermediate subbands upsampled to 2 samples. */
655     for (i = 0; i < 2; i++)
656         aptx_qmf_polyphase_synthesis(qmf->inner_filter_signal[i],
657                                      aptx_qmf_inner_coeffs, 22,
658                                      subband_samples[2*i+0],
659                                      subband_samples[2*i+1],
660                                      &intermediate_samples[2*i]);
661
662     /* Join 2 samples from intermediate subbands upsampled to 4 samples. */
663     for (i = 0; i < 2; i++)
664         aptx_qmf_polyphase_synthesis(qmf->outer_filter_signal,
665                                      aptx_qmf_outer_coeffs, 21,
666                                      intermediate_samples[0+i],
667                                      intermediate_samples[2+i],
668                                      &samples[2*i]);
669 }
670
671
672 av_always_inline
673 static int32_t aptx_bin_search(int32_t value, int32_t factor,
674                                const int32_t *intervals, int32_t nb_intervals)
675 {
676     int32_t idx = 0;
677     int i;
678
679     for (i = nb_intervals >> 1; i > 0; i >>= 1)
680         if (MUL64(factor, intervals[idx + i]) <= ((int64_t)value << 24))
681             idx += i;
682
683     return idx;
684 }
685
686 static void aptx_quantize_difference(Quantize *quantize,
687                                      int32_t sample_difference,
688                                      int32_t dither,
689                                      int32_t quantization_factor,
690                                      ConstTables *tables)
691 {
692     const int32_t *intervals = tables->quantize_intervals;
693     int32_t quantized_sample, dithered_sample, parity_change;
694     int32_t d, mean, interval, inv, sample_difference_abs;
695     int64_t error;
696
697     sample_difference_abs = FFABS(sample_difference);
698     sample_difference_abs = FFMIN(sample_difference_abs, (1 << 23) - 1);
699
700     quantized_sample = aptx_bin_search(sample_difference_abs >> 4,
701                                        quantization_factor,
702                                        intervals, tables->tables_size);
703
704     d = rshift32_clip24(MULH(dither, dither), 7) - (1 << 23);
705     d = rshift64(MUL64(d, tables->quantize_dither_factors[quantized_sample]), 23);
706
707     intervals += quantized_sample;
708     mean = (intervals[1] + intervals[0]) / 2;
709     interval = (intervals[1] - intervals[0]) * (-(sample_difference < 0) | 1);
710
711     dithered_sample = rshift64_clip24(MUL64(dither, interval) + ((int64_t)av_clip_intp2(mean + d, 23) << 32), 32);
712     error = ((int64_t)sample_difference_abs << 20) - MUL64(dithered_sample, quantization_factor);
713     quantize->error = FFABS(rshift64(error, 23));
714
715     parity_change = quantized_sample;
716     if (error < 0)
717         quantized_sample--;
718     else
719         parity_change--;
720
721     inv = -(sample_difference < 0);
722     quantize->quantized_sample               = quantized_sample ^ inv;
723     quantize->quantized_sample_parity_change = parity_change    ^ inv;
724 }
725
726 static void aptx_encode_channel(Channel *channel, int32_t samples[4], int hd)
727 {
728     int32_t subband_samples[4];
729     int subband;
730     aptx_qmf_tree_analysis(&channel->qmf, samples, subband_samples);
731     aptx_generate_dither(channel);
732     for (subband = 0; subband < NB_SUBBANDS; subband++) {
733         int32_t diff = av_clip_intp2(subband_samples[subband] - channel->prediction[subband].predicted_sample, 23);
734         aptx_quantize_difference(&channel->quantize[subband], diff,
735                                  channel->dither[subband],
736                                  channel->invert_quantize[subband].quantization_factor,
737                                  &tables[hd][subband]);
738     }
739 }
740
741 static void aptx_decode_channel(Channel *channel, int32_t samples[4])
742 {
743     int32_t subband_samples[4];
744     int subband;
745     for (subband = 0; subband < NB_SUBBANDS; subband++)
746         subband_samples[subband] = channel->prediction[subband].previous_reconstructed_sample;
747     aptx_qmf_tree_synthesis(&channel->qmf, subband_samples, samples);
748 }
749
750
751 static void aptx_invert_quantization(InvertQuantize *invert_quantize,
752                                      int32_t quantized_sample, int32_t dither,
753                                      ConstTables *tables)
754 {
755     int32_t qr, idx, shift, factor_select;
756
757     idx = (quantized_sample ^ -(quantized_sample < 0)) + 1;
758     qr = tables->quantize_intervals[idx] / 2;
759     if (quantized_sample < 0)
760         qr = -qr;
761
762     qr = rshift64_clip24(((int64_t)qr<<32) + MUL64(dither, tables->invert_quantize_dither_factors[idx]), 32);
763     invert_quantize->reconstructed_difference = MUL64(invert_quantize->quantization_factor, qr) >> 19;
764
765     /* update factor_select */
766     factor_select = 32620 * invert_quantize->factor_select;
767     factor_select = rshift32(factor_select + (tables->quantize_factor_select_offset[idx] << 15), 15);
768     invert_quantize->factor_select = av_clip(factor_select, 0, tables->factor_max);
769
770     /* update quantization factor */
771     idx = (invert_quantize->factor_select & 0xFF) >> 3;
772     shift = (tables->factor_max - invert_quantize->factor_select) >> 8;
773     invert_quantize->quantization_factor = (quantization_factors[idx] << 11) >> shift;
774 }
775
776 static int32_t *aptx_reconstructed_differences_update(Prediction *prediction,
777                                                       int32_t reconstructed_difference,
778                                                       int order)
779 {
780     int32_t *rd1 = prediction->reconstructed_differences, *rd2 = rd1 + order;
781     int p = prediction->pos;
782
783     rd1[p] = rd2[p];
784     prediction->pos = p = (p + 1) % order;
785     rd2[p] = reconstructed_difference;
786     return &rd2[p];
787 }
788
789 static void aptx_prediction_filtering(Prediction *prediction,
790                                       int32_t reconstructed_difference,
791                                       int order)
792 {
793     int32_t reconstructed_sample, predictor, srd0;
794     int32_t *reconstructed_differences;
795     int64_t predicted_difference = 0;
796     int i;
797
798     reconstructed_sample = av_clip_intp2(reconstructed_difference + prediction->predicted_sample, 23);
799     predictor = av_clip_intp2((MUL64(prediction->s_weight[0], prediction->previous_reconstructed_sample)
800                              + MUL64(prediction->s_weight[1], reconstructed_sample)) >> 22, 23);
801     prediction->previous_reconstructed_sample = reconstructed_sample;
802
803     reconstructed_differences = aptx_reconstructed_differences_update(prediction, reconstructed_difference, order);
804     srd0 = FFDIFFSIGN(reconstructed_difference, 0) << 23;
805     for (i = 0; i < order; i++) {
806         int32_t srd = FF_SIGNBIT(reconstructed_differences[-i-1]) | 1;
807         prediction->d_weight[i] -= rshift32(prediction->d_weight[i] - srd*srd0, 8);
808         predicted_difference += MUL64(reconstructed_differences[-i], prediction->d_weight[i]);
809     }
810
811     prediction->predicted_difference = av_clip_intp2(predicted_difference >> 22, 23);
812     prediction->predicted_sample = av_clip_intp2(predictor + prediction->predicted_difference, 23);
813 }
814
815 static void aptx_process_subband(InvertQuantize *invert_quantize,
816                                  Prediction *prediction,
817                                  int32_t quantized_sample, int32_t dither,
818                                  ConstTables *tables)
819 {
820     int32_t sign, same_sign[2], weight[2], sw1, range;
821
822     aptx_invert_quantization(invert_quantize, quantized_sample, dither, tables);
823
824     sign = FFDIFFSIGN(invert_quantize->reconstructed_difference,
825                       -prediction->predicted_difference);
826     same_sign[0] = sign * prediction->prev_sign[0];
827     same_sign[1] = sign * prediction->prev_sign[1];
828     prediction->prev_sign[0] = prediction->prev_sign[1];
829     prediction->prev_sign[1] = sign | 1;
830
831     range = 0x100000;
832     sw1 = rshift32(-same_sign[1] * prediction->s_weight[1], 1);
833     sw1 = (av_clip(sw1, -range, range) & ~0xF) << 4;
834
835     range = 0x300000;
836     weight[0] = 254 * prediction->s_weight[0] + 0x800000*same_sign[0] + sw1;
837     prediction->s_weight[0] = av_clip(rshift32(weight[0], 8), -range, range);
838
839     range = 0x3C0000 - prediction->s_weight[0];
840     weight[1] = 255 * prediction->s_weight[1] + 0xC00000*same_sign[1];
841     prediction->s_weight[1] = av_clip(rshift32(weight[1], 8), -range, range);
842
843     aptx_prediction_filtering(prediction,
844                               invert_quantize->reconstructed_difference,
845                               tables->prediction_order);
846 }
847
848 static void aptx_invert_quantize_and_prediction(Channel *channel, int hd)
849 {
850     int subband;
851     for (subband = 0; subband < NB_SUBBANDS; subband++)
852         aptx_process_subband(&channel->invert_quantize[subband],
853                              &channel->prediction[subband],
854                              channel->quantize[subband].quantized_sample,
855                              channel->dither[subband],
856                              &tables[hd][subband]);
857 }
858
859 static int32_t aptx_quantized_parity(Channel *channel)
860 {
861     int32_t parity = channel->dither_parity;
862     int subband;
863
864     for (subband = 0; subband < NB_SUBBANDS; subband++)
865         parity ^= channel->quantize[subband].quantized_sample;
866
867     return parity & 1;
868 }
869
870 /* For each sample, ensure that the parity of all subbands of all channels
871  * is 0 except once every 8 samples where the parity is forced to 1. */
872 static int aptx_check_parity(Channel channels[NB_CHANNELS], int32_t *idx)
873 {
874     int32_t parity = aptx_quantized_parity(&channels[LEFT])
875                    ^ aptx_quantized_parity(&channels[RIGHT]);
876
877     int eighth = *idx == 7;
878     *idx = (*idx + 1) & 7;
879
880     return parity ^ eighth;
881 }
882
883 static void aptx_insert_sync(Channel channels[NB_CHANNELS], int32_t *idx)
884 {
885     if (aptx_check_parity(channels, idx)) {
886         int i;
887         Channel *c;
888         static const int map[] = { 1, 2, 0, 3 };
889         Quantize *min = &channels[NB_CHANNELS-1].quantize[map[0]];
890         for (c = &channels[NB_CHANNELS-1]; c >= channels; c--)
891             for (i = 0; i < NB_SUBBANDS; i++)
892                 if (c->quantize[map[i]].error < min->error)
893                     min = &c->quantize[map[i]];
894
895         /* Forcing the desired parity is done by offsetting by 1 the quantized
896          * sample from the subband featuring the smallest quantization error. */
897         min->quantized_sample = min->quantized_sample_parity_change;
898     }
899 }
900
901 static uint16_t aptx_pack_codeword(Channel *channel)
902 {
903     int32_t parity = aptx_quantized_parity(channel);
904     return (((channel->quantize[3].quantized_sample & 0x06) | parity) << 13)
905          | (((channel->quantize[2].quantized_sample & 0x03)         ) << 11)
906          | (((channel->quantize[1].quantized_sample & 0x0F)         ) <<  7)
907          | (((channel->quantize[0].quantized_sample & 0x7F)         ) <<  0);
908 }
909
910 static uint32_t aptxhd_pack_codeword(Channel *channel)
911 {
912     int32_t parity = aptx_quantized_parity(channel);
913     return (((channel->quantize[3].quantized_sample & 0x01E) | parity) << 19)
914          | (((channel->quantize[2].quantized_sample & 0x00F)         ) << 15)
915          | (((channel->quantize[1].quantized_sample & 0x03F)         ) <<  9)
916          | (((channel->quantize[0].quantized_sample & 0x1FF)         ) <<  0);
917 }
918
919 static void aptx_unpack_codeword(Channel *channel, uint16_t codeword)
920 {
921     channel->quantize[0].quantized_sample = sign_extend(codeword >>  0, 7);
922     channel->quantize[1].quantized_sample = sign_extend(codeword >>  7, 4);
923     channel->quantize[2].quantized_sample = sign_extend(codeword >> 11, 2);
924     channel->quantize[3].quantized_sample = sign_extend(codeword >> 13, 3);
925     channel->quantize[3].quantized_sample = (channel->quantize[3].quantized_sample & ~1)
926                                           | aptx_quantized_parity(channel);
927 }
928
929 static void aptxhd_unpack_codeword(Channel *channel, uint32_t codeword)
930 {
931     channel->quantize[0].quantized_sample = sign_extend(codeword >>  0, 9);
932     channel->quantize[1].quantized_sample = sign_extend(codeword >>  9, 6);
933     channel->quantize[2].quantized_sample = sign_extend(codeword >> 15, 4);
934     channel->quantize[3].quantized_sample = sign_extend(codeword >> 19, 5);
935     channel->quantize[3].quantized_sample = (channel->quantize[3].quantized_sample & ~1)
936                                           | aptx_quantized_parity(channel);
937 }
938
939 static void aptx_encode_samples(AptXContext *ctx,
940                                 int32_t samples[NB_CHANNELS][4],
941                                 uint8_t *output)
942 {
943     int channel;
944     for (channel = 0; channel < NB_CHANNELS; channel++)
945         aptx_encode_channel(&ctx->channels[channel], samples[channel], ctx->hd);
946
947     aptx_insert_sync(ctx->channels, &ctx->sync_idx);
948
949     for (channel = 0; channel < NB_CHANNELS; channel++) {
950         aptx_invert_quantize_and_prediction(&ctx->channels[channel], ctx->hd);
951         if (ctx->hd)
952             AV_WB24(output + 3*channel,
953                     aptxhd_pack_codeword(&ctx->channels[channel]));
954         else
955             AV_WB16(output + 2*channel,
956                     aptx_pack_codeword(&ctx->channels[channel]));
957     }
958 }
959
960 static int aptx_decode_samples(AptXContext *ctx,
961                                 const uint8_t *input,
962                                 int32_t samples[NB_CHANNELS][4])
963 {
964     int channel, ret;
965
966     for (channel = 0; channel < NB_CHANNELS; channel++) {
967         aptx_generate_dither(&ctx->channels[channel]);
968
969         if (ctx->hd)
970             aptxhd_unpack_codeword(&ctx->channels[channel],
971                                    AV_RB24(input + 3*channel));
972         else
973             aptx_unpack_codeword(&ctx->channels[channel],
974                                  AV_RB16(input + 2*channel));
975         aptx_invert_quantize_and_prediction(&ctx->channels[channel], ctx->hd);
976     }
977
978     ret = aptx_check_parity(ctx->channels, &ctx->sync_idx);
979
980     for (channel = 0; channel < NB_CHANNELS; channel++)
981         aptx_decode_channel(&ctx->channels[channel], samples[channel]);
982
983     return ret;
984 }
985
986
987 static av_cold int aptx_init(AVCodecContext *avctx)
988 {
989     AptXContext *s = avctx->priv_data;
990     int chan, subband;
991
992     s->hd = avctx->codec->id == AV_CODEC_ID_APTX_HD;
993     s->block_size = s->hd ? 6 : 4;
994
995     if (avctx->frame_size == 0)
996         avctx->frame_size = 256 * s->block_size;
997
998     if (avctx->frame_size % s->block_size) {
999         av_log(avctx, AV_LOG_ERROR,
1000                "Frame size must be a multiple of %d samples\n", s->block_size);
1001         return AVERROR(EINVAL);
1002     }
1003
1004     for (chan = 0; chan < NB_CHANNELS; chan++) {
1005         Channel *channel = &s->channels[chan];
1006         for (subband = 0; subband < NB_SUBBANDS; subband++) {
1007             Prediction *prediction = &channel->prediction[subband];
1008             prediction->prev_sign[0] = 1;
1009             prediction->prev_sign[1] = 1;
1010         }
1011     }
1012
1013     ff_af_queue_init(avctx, &s->afq);
1014     return 0;
1015 }
1016
1017 static int aptx_decode_frame(AVCodecContext *avctx, void *data,
1018                              int *got_frame_ptr, AVPacket *avpkt)
1019 {
1020     AptXContext *s = avctx->priv_data;
1021     AVFrame *frame = data;
1022     int pos, opos, channel, sample, ret;
1023
1024     if (avpkt->size < s->block_size) {
1025         av_log(avctx, AV_LOG_ERROR, "Packet is too small\n");
1026         return AVERROR_INVALIDDATA;
1027     }
1028
1029     /* get output buffer */
1030     frame->channels = NB_CHANNELS;
1031     frame->format = AV_SAMPLE_FMT_S32P;
1032     frame->nb_samples = 4 * avpkt->size / s->block_size;
1033     if ((ret = ff_get_buffer(avctx, frame, 0)) < 0)
1034         return ret;
1035
1036     for (pos = 0, opos = 0; opos < frame->nb_samples; pos += s->block_size, opos += 4) {
1037         int32_t samples[NB_CHANNELS][4];
1038
1039         if (aptx_decode_samples(s, &avpkt->data[pos], samples)) {
1040             av_log(avctx, AV_LOG_ERROR, "Synchronization error\n");
1041             return AVERROR_INVALIDDATA;
1042         }
1043
1044         for (channel = 0; channel < NB_CHANNELS; channel++)
1045             for (sample = 0; sample < 4; sample++)
1046                 AV_WN32A(&frame->data[channel][4*(opos+sample)],
1047                          samples[channel][sample] << 8);
1048     }
1049
1050     *got_frame_ptr = 1;
1051     return s->block_size * frame->nb_samples / 4;
1052 }
1053
1054 static int aptx_encode_frame(AVCodecContext *avctx, AVPacket *avpkt,
1055                              const AVFrame *frame, int *got_packet_ptr)
1056 {
1057     AptXContext *s = avctx->priv_data;
1058     int pos, ipos, channel, sample, output_size, ret;
1059
1060     if ((ret = ff_af_queue_add(&s->afq, frame)) < 0)
1061         return ret;
1062
1063     output_size = s->block_size * frame->nb_samples/4;
1064     if ((ret = ff_alloc_packet2(avctx, avpkt, output_size, 0)) < 0)
1065         return ret;
1066
1067     for (pos = 0, ipos = 0; pos < output_size; pos += s->block_size, ipos += 4) {
1068         int32_t samples[NB_CHANNELS][4];
1069
1070         for (channel = 0; channel < NB_CHANNELS; channel++)
1071             for (sample = 0; sample < 4; sample++)
1072                 samples[channel][sample] = (int32_t)AV_RN32A(&frame->data[channel][4*(ipos+sample)]) >> 8;
1073
1074         aptx_encode_samples(s, samples, avpkt->data + pos);
1075     }
1076
1077     ff_af_queue_remove(&s->afq, frame->nb_samples, &avpkt->pts, &avpkt->duration);
1078     *got_packet_ptr = 1;
1079     return 0;
1080 }
1081
1082 static av_cold int aptx_close(AVCodecContext *avctx)
1083 {
1084     AptXContext *s = avctx->priv_data;
1085     ff_af_queue_close(&s->afq);
1086     return 0;
1087 }
1088
1089
1090 #if CONFIG_APTX_DECODER
1091 AVCodec ff_aptx_decoder = {
1092     .name                  = "aptx",
1093     .long_name             = NULL_IF_CONFIG_SMALL("aptX (Audio Processing Technology for Bluetooth)"),
1094     .type                  = AVMEDIA_TYPE_AUDIO,
1095     .id                    = AV_CODEC_ID_APTX,
1096     .priv_data_size        = sizeof(AptXContext),
1097     .init                  = aptx_init,
1098     .decode                = aptx_decode_frame,
1099     .close                 = aptx_close,
1100     .capabilities          = AV_CODEC_CAP_DR1,
1101     .caps_internal         = FF_CODEC_CAP_INIT_THREADSAFE,
1102     .channel_layouts       = (const uint64_t[]) { AV_CH_LAYOUT_STEREO, 0},
1103     .sample_fmts           = (const enum AVSampleFormat[]) { AV_SAMPLE_FMT_S32P,
1104                                                              AV_SAMPLE_FMT_NONE },
1105 };
1106 #endif
1107
1108 #if CONFIG_APTX_HD_DECODER
1109 AVCodec ff_aptx_hd_decoder = {
1110     .name                  = "aptx_hd",
1111     .long_name             = NULL_IF_CONFIG_SMALL("aptX HD (Audio Processing Technology for Bluetooth)"),
1112     .type                  = AVMEDIA_TYPE_AUDIO,
1113     .id                    = AV_CODEC_ID_APTX_HD,
1114     .priv_data_size        = sizeof(AptXContext),
1115     .init                  = aptx_init,
1116     .decode                = aptx_decode_frame,
1117     .close                 = aptx_close,
1118     .capabilities          = AV_CODEC_CAP_DR1,
1119     .caps_internal         = FF_CODEC_CAP_INIT_THREADSAFE,
1120     .channel_layouts       = (const uint64_t[]) { AV_CH_LAYOUT_STEREO, 0},
1121     .sample_fmts           = (const enum AVSampleFormat[]) { AV_SAMPLE_FMT_S32P,
1122                                                              AV_SAMPLE_FMT_NONE },
1123 };
1124 #endif
1125
1126 #if CONFIG_APTX_ENCODER
1127 AVCodec ff_aptx_encoder = {
1128     .name                  = "aptx",
1129     .long_name             = NULL_IF_CONFIG_SMALL("aptX (Audio Processing Technology for Bluetooth)"),
1130     .type                  = AVMEDIA_TYPE_AUDIO,
1131     .id                    = AV_CODEC_ID_APTX,
1132     .priv_data_size        = sizeof(AptXContext),
1133     .init                  = aptx_init,
1134     .encode2               = aptx_encode_frame,
1135     .close                 = aptx_close,
1136     .capabilities          = AV_CODEC_CAP_SMALL_LAST_FRAME,
1137     .caps_internal         = FF_CODEC_CAP_INIT_THREADSAFE,
1138     .channel_layouts       = (const uint64_t[]) { AV_CH_LAYOUT_STEREO, 0},
1139     .sample_fmts           = (const enum AVSampleFormat[]) { AV_SAMPLE_FMT_S32P,
1140                                                              AV_SAMPLE_FMT_NONE },
1141     .supported_samplerates = (const int[]) {8000, 16000, 24000, 32000, 44100, 48000, 0},
1142 };
1143 #endif
1144
1145 #if CONFIG_APTX_HD_ENCODER
1146 AVCodec ff_aptx_hd_encoder = {
1147     .name                  = "aptx_hd",
1148     .long_name             = NULL_IF_CONFIG_SMALL("aptX HD (Audio Processing Technology for Bluetooth)"),
1149     .type                  = AVMEDIA_TYPE_AUDIO,
1150     .id                    = AV_CODEC_ID_APTX_HD,
1151     .priv_data_size        = sizeof(AptXContext),
1152     .init                  = aptx_init,
1153     .encode2               = aptx_encode_frame,
1154     .close                 = aptx_close,
1155     .capabilities          = AV_CODEC_CAP_SMALL_LAST_FRAME,
1156     .caps_internal         = FF_CODEC_CAP_INIT_THREADSAFE,
1157     .channel_layouts       = (const uint64_t[]) { AV_CH_LAYOUT_STEREO, 0},
1158     .sample_fmts           = (const enum AVSampleFormat[]) { AV_SAMPLE_FMT_S32P,
1159                                                              AV_SAMPLE_FMT_NONE },
1160     .supported_samplerates = (const int[]) {8000, 16000, 24000, 32000, 44100, 48000, 0},
1161 };
1162 #endif