avcodec/aacdec_fixed: use 64bit to avoid overflow in rounding in apply_dependent_coup...
[ffmpeg.git] / libavcodec / dss_sp.c
1 /*
2  * Digital Speech Standard - Standard Play mode (DSS SP) audio decoder.
3  * Copyright (C) 2014 Oleksij Rempel <linux@rempel-privat.de>
4  *
5  * This file is part of FFmpeg.
6  *
7  * FFmpeg is free software; you can redistribute it and/or
8  * modify it under the terms of the GNU Lesser General Public
9  * License as published by the Free Software Foundation; either
10  * version 2.1 of the License, or (at your option) any later version.
11  *
12  * FFmpeg is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
15  * Lesser General Public License for more details.
16  *
17  * You should have received a copy of the GNU Lesser General Public
18  * License along with FFmpeg; if not, write to the Free Software
19  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
20  */
21
22 #include "libavutil/channel_layout.h"
23 #include "libavutil/common.h"
24 #include "libavutil/mem.h"
25 #include "libavutil/opt.h"
26
27 #include "avcodec.h"
28 #include "get_bits.h"
29 #include "internal.h"
30
31 #define SUBFRAMES 4
32 #define PULSE_MAX 8
33
34 #define DSS_SP_FRAME_SIZE        42
35 #define DSS_SP_SAMPLE_COUNT     (66 * SUBFRAMES)
36 #define DSS_SP_FORMULA(a, b, c) ((int)((((a) * (1 << 15)) + (b) * (unsigned)(c)) + 0x4000) >> 15)
37
38 typedef struct DssSpSubframe {
39     int16_t gain;
40     int32_t combined_pulse_pos;
41     int16_t pulse_pos[7];
42     int16_t pulse_val[7];
43 } DssSpSubframe;
44
45 typedef struct DssSpFrame {
46     int16_t filter_idx[14];
47     int16_t sf_adaptive_gain[SUBFRAMES];
48     int16_t pitch_lag[SUBFRAMES];
49     struct DssSpSubframe sf[SUBFRAMES];
50 } DssSpFrame;
51
52 typedef struct DssSpContext {
53     AVCodecContext *avctx;
54     int32_t excitation[288 + 6];
55     int32_t history[187];
56     DssSpFrame fparam;
57     int32_t working_buffer[SUBFRAMES][72];
58     int32_t audio_buf[15];
59     int32_t err_buf1[15];
60     int32_t lpc_filter[14];
61     int32_t filter[15];
62     int32_t vector_buf[72];
63     int noise_state;
64     int32_t err_buf2[15];
65
66     int pulse_dec_mode;
67
68     DECLARE_ALIGNED(16, uint8_t, bits)[DSS_SP_FRAME_SIZE +
69                                        AV_INPUT_BUFFER_PADDING_SIZE];
70 } DssSpContext;
71
72 /*
73  * Used for the coding/decoding of the pulse positions for the MP-MLQ codebook.
74  */
75 static const uint32_t dss_sp_combinatorial_table[PULSE_MAX][72] = {
76     {       0,         0,         0,          0,          0,          0,
77             0,         0,         0,          0,          0,          0,
78             0,         0,         0,          0,          0,          0,
79             0,         0,         0,          0,          0,          0,
80             0,         0,         0,          0,          0,          0,
81             0,         0,         0,          0,          0,          0,
82             0,         0,         0,          0,          0,          0,
83             0,         0,         0,          0,          0,          0,
84             0,         0,         0,          0,          0,          0,
85             0,         0,         0,          0,          0,          0,
86             0,         0,         0,          0,          0,          0,
87             0,         0,         0,          0,          0,          0 },
88     {       0,         1,         2,          3,          4,          5,
89             6,         7,         8,          9,         10,         11,
90            12,        13,        14,         15,         16,         17,
91            18,        19,        20,         21,         22,         23,
92            24,        25,        26,         27,         28,         29,
93            30,        31,        32,         33,         34,         35,
94            36,        37,        38,         39,         40,         41,
95            42,        43,        44,         45,         46,         47,
96            48,        49,        50,         51,         52,         53,
97            54,        55,        56,         57,         58,         59,
98            60,        61,        62,         63,         64,         65,
99            66,        67,        68,         69,         70,         71 },
100     {       0,         0,         1,          3,          6,         10,
101            15,        21,        28,         36,         45,         55,
102            66,        78,        91,        105,        120,        136,
103           153,       171,       190,        210,        231,        253,
104           276,       300,       325,        351,        378,        406,
105           435,       465,       496,        528,        561,        595,
106           630,       666,       703,        741,        780,        820,
107           861,       903,       946,        990,       1035,       1081,
108          1128,      1176,      1225,       1275,       1326,       1378,
109          1431,      1485,      1540,       1596,       1653,       1711,
110          1770,      1830,      1891,       1953,       2016,       2080,
111          2145,      2211,      2278,       2346,       2415,       2485 },
112     {       0,         0,         0,          1,          4,         10,
113            20,        35,        56,         84,        120,        165,
114           220,       286,       364,        455,        560,        680,
115           816,       969,      1140,       1330,       1540,       1771,
116          2024,      2300,      2600,       2925,       3276,       3654,
117          4060,      4495,      4960,       5456,       5984,       6545,
118          7140,      7770,      8436,       9139,       9880,      10660,
119         11480,     12341,     13244,      14190,      15180,      16215,
120         17296,     18424,     19600,      20825,      22100,      23426,
121         24804,     26235,     27720,      29260,      30856,      32509,
122         34220,     35990,     37820,      39711,      41664,      43680,
123         45760,     47905,     50116,      52394,      54740,      57155 },
124     {       0,         0,         0,          0,          1,          5,
125            15,        35,        70,        126,        210,        330,
126           495,       715,      1001,       1365,       1820,       2380,
127          3060,      3876,      4845,       5985,       7315,       8855,
128         10626,     12650,     14950,      17550,      20475,      23751,
129         27405,     31465,     35960,      40920,      46376,      52360,
130         58905,     66045,     73815,      82251,      91390,     101270,
131        111930,    123410,    135751,     148995,     163185,     178365,
132        194580,    211876,    230300,     249900,     270725,     292825,
133        316251,    341055,    367290,     395010,     424270,     455126,
134        487635,    521855,    557845,     595665,     635376,     677040,
135        720720,    766480,    814385,     864501,     916895,     971635 },
136     {       0,         0,         0,          0,          0,          1,
137             6,        21,        56,        126,        252,        462,
138           792,      1287,      2002,       3003,       4368,       6188,
139          8568,     11628,     15504,      20349,      26334,      33649,
140         42504,     53130,     65780,      80730,      98280,     118755,
141        142506,    169911,    201376,     237336,     278256,     324632,
142        376992,    435897,    501942,     575757,     658008,     749398,
143        850668,    962598,   1086008,    1221759,    1370754,    1533939,
144       1712304,   1906884,   2118760,    2349060,    2598960,    2869685,
145       3162510,   3478761,   3819816,    4187106,    4582116,    5006386,
146       5461512,   5949147,   6471002,    7028847,    7624512,    8259888,
147       8936928,   9657648,  10424128,   11238513,   12103014,   13019909 },
148     {       0,         0,         0,          0,          0,          0,
149             1,         7,        28,         84,        210,        462,
150           924,      1716,      3003,       5005,       8008,      12376,
151         18564,     27132,     38760,      54264,      74613,     100947,
152        134596,    177100,    230230,     296010,     376740,     475020,
153        593775,    736281,    906192,    1107568,    1344904,    1623160,
154       1947792,   2324784,   2760681,    3262623,    3838380,    4496388,
155       5245786,   6096454,   7059052,    8145060,    9366819,   10737573,
156      12271512,  13983816,  15890700,   18009460,   20358520,   22957480,
157      25827165,  28989675,  32468436,   36288252,   40475358,   45057474,
158      50063860,  55525372,  61474519,   67945521,   74974368,   82598880,
159      90858768,  99795696, 109453344,  119877472,  131115985,  143218999 },
160     {       0,         0,         0,          0,          0,          0,
161             0,         1,         8,         36,        120,        330,
162           792,      1716,      3432,       6435,      11440,      19448,
163         31824,     50388,     77520,     116280,     170544,     245157,
164        346104,    480700,    657800,     888030,    1184040,    1560780,
165       2035800,   2629575,   3365856,    4272048,    5379616,    6724520,
166       8347680,  10295472,  12620256,   15380937,   18643560,   22481940,
167      26978328,  32224114,  38320568,   45379620,   53524680,   62891499,
168      73629072,  85900584,  99884400,  115775100,  133784560,  154143080,
169     177100560, 202927725, 231917400,  264385836,  300674088,  341149446,
170     386206920, 436270780, 491796152,  553270671,  621216192,  696190560,
171     778789440, 869648208, 969443904, 1078897248, 1198774720, 1329890705 },
172 };
173
174 static const int16_t dss_sp_filter_cb[14][32] = {
175     { -32653, -32587, -32515, -32438, -32341, -32216, -32062, -31881,
176       -31665, -31398, -31080, -30724, -30299, -29813, -29248, -28572,
177       -27674, -26439, -24666, -22466, -19433, -16133, -12218,  -7783,
178        -2834,   1819,   6544,  11260,  16050,  20220,  24774,  28120 },
179
180     { -27503, -24509, -20644, -17496, -14187, -11277,  -8420,  -5595,
181        -3013,   -624,   1711,   3880,   5844,   7774,   9739,  11592,
182        13364,  14903,  16426,  17900,  19250,  20586,  21803,  23006,
183        24142,  25249,  26275,  27300,  28359,  29249,  30118,  31183 },
184
185     { -27827, -24208, -20943, -17781, -14843, -11848,  -9066,  -6297,
186        -3660,   -910,   1918,   5025,   8223,  11649,  15086,  18423,
187            0,      0,      0,      0,      0,      0,      0,      0,
188            0,      0,      0,      0,      0,      0,      0,      0 },
189
190     { -17128, -11975,  -8270,  -5123,  -2296,    183,   2503,   4707,
191         6798,   8945,  11045,  13239,  15528,  18248,  21115,  24785,
192            0,      0,      0,      0,      0,      0,      0,      0,
193            0,      0,      0,      0,      0,      0,      0,      0 },
194
195     { -21557, -17280, -14286, -11644,  -9268,  -7087,  -4939,  -2831,
196         -691,   1407,   3536,   5721,   8125,  10677,  13721,  17731,
197            0,      0,      0,      0,      0,      0,      0,      0,
198            0,      0,      0,      0,      0,      0,      0,      0 },
199
200     { -15030, -10377,  -7034,  -4327,  -1900,    364,   2458,   4450,
201         6422,   8374,  10374,  12486,  14714,  16997,  19626,  22954,
202            0,      0,      0,      0,      0,      0,      0,      0,
203            0,      0,      0,      0,      0,      0,      0,      0 },
204
205     { -16155, -12362,  -9698,  -7460,  -5258,  -3359,  -1547,    219,
206         1916,   3599,   5299,   6994,   8963,  11226,  13716,  16982,
207            0,      0,      0,      0,      0,      0,      0,      0,
208            0,      0,      0,      0,      0,      0,      0,      0 },
209
210     { -14742,  -9848,  -6921,  -4648,  -2769,  -1065,    499,   2083,
211         3633,   5219,   6857,   8580,  10410,  12672,  15561,  20101,
212            0,      0,      0,      0,      0,      0,      0,      0,
213            0,      0,      0,      0,      0,      0,      0,      0 },
214
215     { -11099,  -7014,  -3855,  -1025,   1680,   4544,   7807,  11932,
216            0,      0,      0,      0,      0,      0,      0,      0,
217            0,      0,      0,      0,      0,      0,      0,      0,
218            0,      0,      0,      0,      0,      0,      0,      0 },
219
220     {  -9060,  -4570,  -1381,   1419,   4034,   6728,   9865,  14149,
221            0,      0,      0,      0,      0,      0,      0,      0,
222            0,      0,      0,      0,      0,      0,      0,      0,
223            0,      0,      0,      0,      0,      0,      0,      0 },
224
225     { -12450,  -7985,  -4596,  -1734,    961,   3629,   6865,  11142,
226            0,      0,      0,      0,      0,      0,      0,      0,
227            0,      0,      0,      0,      0,      0,      0,      0,
228            0,      0,      0,      0,      0,      0,      0,      0 },
229
230     { -11831,  -7404,  -4010,  -1096,   1606,   4291,   7386,  11482,
231            0,      0,      0,      0,      0,      0,      0,      0,
232            0,      0,      0,      0,      0,      0,      0,      0,
233            0,      0,      0,      0,      0,      0,      0,      0 },
234
235     { -13404,  -9250,  -5995,  -3312,   -890,   1594,   4464,   8198,
236            0,      0,      0,      0,      0,      0,      0,      0,
237            0,      0,      0,      0,      0,      0,      0,      0,
238            0,      0,      0,      0,      0,      0,      0,      0 },
239
240     { -11239,  -7220,  -4040,  -1406,    971,   3321,   6006,   9697,
241            0,      0,      0,      0,      0,      0,      0,      0,
242            0,      0,      0,      0,      0,      0,      0,      0,
243            0,      0,      0,      0,      0,      0,      0,      0 },
244 };
245
246 static const uint16_t  dss_sp_fixed_cb_gain[64] = {
247        0,    4,    8,   13,   17,   22,   26,   31,
248       35,   40,   44,   48,   53,   58,   63,   69,
249       76,   83,   91,   99,  109,  119,  130,  142,
250      155,  170,  185,  203,  222,  242,  265,  290,
251      317,  346,  378,  414,  452,  494,  540,  591,
252      646,  706,  771,  843,  922, 1007, 1101, 1204,
253     1316, 1438, 1572, 1719, 1879, 2053, 2244, 2453,
254     2682, 2931, 3204, 3502, 3828, 4184, 4574, 5000,
255 };
256
257 static const int16_t  dss_sp_pulse_val[8] = {
258     -31182, -22273, -13364, -4455, 4455, 13364, 22273, 31182
259 };
260
261 static const uint16_t binary_decreasing_array[] = {
262     32767, 16384, 8192, 4096, 2048, 1024, 512, 256,
263     128, 64, 32, 16, 8, 4, 2,
264 };
265
266 static const uint16_t dss_sp_unc_decreasing_array[] = {
267     32767, 26214, 20972, 16777, 13422, 10737, 8590, 6872,
268     5498, 4398, 3518, 2815, 2252, 1801, 1441,
269 };
270
271 static const uint16_t dss_sp_adaptive_gain[] = {
272      102,  231,  360,  488,  617,  746,  875, 1004,
273     1133, 1261, 1390, 1519, 1648, 1777, 1905, 2034,
274     2163, 2292, 2421, 2550, 2678, 2807, 2936, 3065,
275     3194, 3323, 3451, 3580, 3709, 3838, 3967, 4096,
276 };
277
278 static const int32_t dss_sp_sinc[67] = {
279       262,   293,   323,   348,   356,   336,   269,   139,
280       -67,  -358,  -733, -1178, -1668, -2162, -2607, -2940,
281     -3090, -2986, -2562, -1760,  -541,  1110,  3187,  5651,
282      8435, 11446, 14568, 17670, 20611, 23251, 25460, 27125,
283     28160, 28512, 28160,
284     27125, 25460, 23251, 20611, 17670, 14568, 11446,  8435,
285      5651,  3187,  1110,  -541, -1760, -2562, -2986, -3090,
286     -2940, -2607, -2162, -1668, -1178,  -733,  -358,   -67,
287       139,   269,   336,   356,   348,   323,   293,   262,
288 };
289
290 static av_cold int dss_sp_decode_init(AVCodecContext *avctx)
291 {
292     DssSpContext *p = avctx->priv_data;
293     avctx->channel_layout = AV_CH_LAYOUT_MONO;
294     avctx->sample_fmt     = AV_SAMPLE_FMT_S16;
295     avctx->channels       = 1;
296     avctx->sample_rate    = 11025;
297
298     memset(p->history, 0, sizeof(p->history));
299     p->pulse_dec_mode = 1;
300     p->avctx          = avctx;
301
302     return 0;
303 }
304
305 static void dss_sp_unpack_coeffs(DssSpContext *p, const uint8_t *src)
306 {
307     GetBitContext gb;
308     DssSpFrame *fparam = &p->fparam;
309     int i;
310     int subframe_idx;
311     uint32_t combined_pitch;
312     uint32_t tmp;
313     uint32_t pitch_lag;
314
315     for (i = 0; i < DSS_SP_FRAME_SIZE; i += 2) {
316         p->bits[i]     = src[i + 1];
317         p->bits[i + 1] = src[i];
318     }
319
320     init_get_bits(&gb, p->bits, DSS_SP_FRAME_SIZE * 8);
321
322     for (i = 0; i < 2; i++)
323         fparam->filter_idx[i] = get_bits(&gb, 5);
324     for (; i < 8; i++)
325         fparam->filter_idx[i] = get_bits(&gb, 4);
326     for (; i < 14; i++)
327         fparam->filter_idx[i] = get_bits(&gb, 3);
328
329     for (subframe_idx = 0; subframe_idx < 4; subframe_idx++) {
330         fparam->sf_adaptive_gain[subframe_idx] = get_bits(&gb, 5);
331
332         fparam->sf[subframe_idx].combined_pulse_pos = get_bits_long(&gb, 31);
333
334         fparam->sf[subframe_idx].gain = get_bits(&gb, 6);
335
336         for (i = 0; i < 7; i++)
337             fparam->sf[subframe_idx].pulse_val[i] = get_bits(&gb, 3);
338     }
339
340     for (subframe_idx = 0; subframe_idx < 4; subframe_idx++) {
341         unsigned int C72_binomials[PULSE_MAX] = {
342             72, 2556, 59640, 1028790, 13991544, 156238908, 1473109704,
343             3379081753
344         };
345         unsigned int combined_pulse_pos =
346             fparam->sf[subframe_idx].combined_pulse_pos;
347         int index = 6;
348
349         if (combined_pulse_pos < C72_binomials[PULSE_MAX - 1]) {
350             if (p->pulse_dec_mode) {
351                 int pulse, pulse_idx;
352                 pulse              = PULSE_MAX - 1;
353                 pulse_idx          = 71;
354                 combined_pulse_pos =
355                     fparam->sf[subframe_idx].combined_pulse_pos;
356
357                 /* this part seems to be close to g723.1 gen_fcb_excitation()
358                  * RATE_6300 */
359
360                 /* TODO: what is 7? size of subframe? */
361                 for (i = 0; i < 7; i++) {
362                     for (;
363                          combined_pulse_pos <
364                          dss_sp_combinatorial_table[pulse][pulse_idx];
365                          --pulse_idx)
366                         ;
367                     combined_pulse_pos -=
368                         dss_sp_combinatorial_table[pulse][pulse_idx];
369                     pulse--;
370                     fparam->sf[subframe_idx].pulse_pos[i] = pulse_idx;
371                 }
372             }
373         } else {
374             p->pulse_dec_mode = 0;
375
376             /* why do we need this? */
377             fparam->sf[subframe_idx].pulse_pos[6] = 0;
378
379             for (i = 71; i >= 0; i--) {
380                 if (C72_binomials[index] <= combined_pulse_pos) {
381                     combined_pulse_pos -= C72_binomials[index];
382
383                     fparam->sf[subframe_idx].pulse_pos[6 - index] = i;
384
385                     if (!index)
386                         break;
387                     --index;
388                 }
389                 --C72_binomials[0];
390                 if (index) {
391                     int a;
392                     for (a = 0; a < index; a++)
393                         C72_binomials[a + 1] -= C72_binomials[a];
394                 }
395             }
396         }
397     }
398
399     combined_pitch = get_bits(&gb, 24);
400
401     fparam->pitch_lag[0] = (combined_pitch % 151) + 36;
402
403     combined_pitch /= 151;
404
405     for (i = 1; i < SUBFRAMES - 1; i++) {
406         fparam->pitch_lag[i] = combined_pitch % 48;
407         combined_pitch      /= 48;
408     }
409     if (combined_pitch > 47) {
410         av_log (p->avctx, AV_LOG_WARNING, "combined_pitch was too large\n");
411         combined_pitch = 0;
412     }
413     fparam->pitch_lag[i] = combined_pitch;
414
415     pitch_lag = fparam->pitch_lag[0];
416     for (i = 1; i < SUBFRAMES; i++) {
417         if (pitch_lag > 162) {
418             fparam->pitch_lag[i] += 162 - 23;
419         } else {
420             tmp = pitch_lag - 23;
421             if (tmp < 36)
422                 tmp = 36;
423             fparam->pitch_lag[i] += tmp;
424         }
425         pitch_lag = fparam->pitch_lag[i];
426     }
427 }
428
429 static void dss_sp_unpack_filter(DssSpContext *p)
430 {
431     int i;
432
433     for (i = 0; i < 14; i++)
434         p->lpc_filter[i] = dss_sp_filter_cb[i][p->fparam.filter_idx[i]];
435 }
436
437 static void dss_sp_convert_coeffs(int32_t *lpc_filter, int32_t *coeffs)
438 {
439     int a, a_plus, i;
440
441     coeffs[0] = 0x2000;
442     for (a = 0; a < 14; a++) {
443         a_plus         = a + 1;
444         coeffs[a_plus] = lpc_filter[a] >> 2;
445         if (a_plus / 2 >= 1) {
446             for (i = 1; i <= a_plus / 2; i++) {
447                 int coeff_1, coeff_2, tmp;
448
449                 coeff_1 = coeffs[i];
450                 coeff_2 = coeffs[a_plus - i];
451
452                 tmp = DSS_SP_FORMULA(coeff_1, lpc_filter[a], coeff_2);
453                 coeffs[i] = av_clip_int16(tmp);
454
455                 tmp = DSS_SP_FORMULA(coeff_2, lpc_filter[a], coeff_1);
456                 coeffs[a_plus - i] = av_clip_int16(tmp);
457             }
458         }
459     }
460 }
461
462 static void dss_sp_add_pulses(int32_t *vector_buf,
463                               const struct DssSpSubframe *sf)
464 {
465     int i;
466
467     for (i = 0; i < 7; i++)
468         vector_buf[sf->pulse_pos[i]] += (dss_sp_fixed_cb_gain[sf->gain] *
469                                          dss_sp_pulse_val[sf->pulse_val[i]] +
470                                          0x4000) >> 15;
471 }
472
473 static void dss_sp_gen_exc(int32_t *vector, int32_t *prev_exc,
474                            int pitch_lag, int gain)
475 {
476     int i;
477
478     /* do we actually need this check? we can use just [a3 - i % a3]
479      * for both cases */
480     if (pitch_lag < 72)
481         for (i = 0; i < 72; i++)
482             vector[i] = prev_exc[pitch_lag - i % pitch_lag];
483     else
484         for (i = 0; i < 72; i++)
485             vector[i] = prev_exc[pitch_lag - i];
486
487     for (i = 0; i < 72; i++) {
488         int tmp = gain * vector[i] >> 11;
489         vector[i] = av_clip_int16(tmp);
490     }
491 }
492
493 static void dss_sp_scale_vector(int32_t *vec, int bits, int size)
494 {
495     int i;
496
497     if (bits < 0)
498         for (i = 0; i < size; i++)
499             vec[i] = vec[i] >> -bits;
500     else
501         for (i = 0; i < size; i++)
502             vec[i] = vec[i] * (1 << bits);
503 }
504
505 static void dss_sp_update_buf(int32_t *hist, int32_t *vector)
506 {
507     int i;
508
509     for (i = 114; i > 0; i--)
510         vector[i + 72] = vector[i];
511
512     for (i = 0; i < 72; i++)
513         vector[72 - i] = hist[i];
514 }
515
516 static void dss_sp_shift_sq_sub(const int32_t *filter_buf,
517                                 int32_t *error_buf, int32_t *dst)
518 {
519     int a;
520
521     for (a = 0; a < 72; a++) {
522         int i, tmp;
523
524         tmp = dst[a] * filter_buf[0];
525
526         for (i = 14; i > 0; i--)
527             tmp -= error_buf[i] * (unsigned)filter_buf[i];
528
529         for (i = 14; i > 0; i--)
530             error_buf[i] = error_buf[i - 1];
531
532         tmp = (int)(tmp + 4096U) >> 13;
533
534         error_buf[1] = tmp;
535
536         dst[a] = av_clip_int16(tmp);
537     }
538 }
539
540 static void dss_sp_shift_sq_add(const int32_t *filter_buf, int32_t *audio_buf,
541                                 int32_t *dst)
542 {
543     int a;
544
545     for (a = 0; a < 72; a++) {
546         int i, tmp = 0;
547
548         audio_buf[0] = dst[a];
549
550         for (i = 14; i >= 0; i--)
551             tmp += audio_buf[i] * filter_buf[i];
552
553         for (i = 14; i > 0; i--)
554             audio_buf[i] = audio_buf[i - 1];
555
556         tmp = (tmp + 4096) >> 13;
557
558         dst[a] = av_clip_int16(tmp);
559     }
560 }
561
562 static void dss_sp_vec_mult(const int32_t *src, int32_t *dst,
563                             const int16_t *mult)
564 {
565     int i;
566
567     dst[0] = src[0];
568
569     for (i = 1; i < 15; i++)
570         dst[i] = (src[i] * mult[i] + 0x4000) >> 15;
571 }
572
573 static int dss_sp_get_normalize_bits(int32_t *vector_buf, int16_t size)
574 {
575     unsigned int val;
576     int max_val;
577     int i;
578
579     val = 1;
580     for (i = 0; i < size; i++)
581         val |= FFABS(vector_buf[i]);
582
583     for (max_val = 0; val <= 0x4000; ++max_val)
584         val *= 2;
585     return max_val;
586 }
587
588 static int dss_sp_vector_sum(DssSpContext *p, int size)
589 {
590     int i, sum = 0;
591     for (i = 0; i < size; i++)
592         sum += FFABS(p->vector_buf[i]);
593     return sum;
594 }
595
596 static void dss_sp_sf_synthesis(DssSpContext *p, int32_t lpc_filter,
597                                 int32_t *dst, int size)
598 {
599     int32_t tmp_buf[15];
600     int32_t noise[72];
601     int bias, vsum_2 = 0, vsum_1 = 0, v36, normalize_bits;
602     int i, tmp;
603
604     if (size > 0) {
605         vsum_1 = dss_sp_vector_sum(p, size);
606
607         if (vsum_1 > 0xFFFFF)
608             vsum_1 = 0xFFFFF;
609     }
610
611     normalize_bits = dss_sp_get_normalize_bits(p->vector_buf, size);
612
613     dss_sp_scale_vector(p->vector_buf, normalize_bits - 3, size);
614     dss_sp_scale_vector(p->audio_buf, normalize_bits, 15);
615     dss_sp_scale_vector(p->err_buf1, normalize_bits, 15);
616
617     v36 = p->err_buf1[1];
618
619     dss_sp_vec_mult(p->filter, tmp_buf, binary_decreasing_array);
620     dss_sp_shift_sq_add(tmp_buf, p->audio_buf, p->vector_buf);
621
622     dss_sp_vec_mult(p->filter, tmp_buf, dss_sp_unc_decreasing_array);
623     dss_sp_shift_sq_sub(tmp_buf, p->err_buf1, p->vector_buf);
624
625     /* lpc_filter can be negative */
626     lpc_filter = lpc_filter >> 1;
627     if (lpc_filter >= 0)
628         lpc_filter = 0;
629
630     if (size > 1) {
631         for (i = size - 1; i > 0; i--) {
632             tmp = DSS_SP_FORMULA(p->vector_buf[i], lpc_filter,
633                                  p->vector_buf[i - 1]);
634             p->vector_buf[i] = av_clip_int16(tmp);
635         }
636     }
637
638     tmp              = DSS_SP_FORMULA(p->vector_buf[0], lpc_filter, v36);
639     p->vector_buf[0] = av_clip_int16(tmp);
640
641     dss_sp_scale_vector(p->vector_buf, -normalize_bits, size);
642     dss_sp_scale_vector(p->audio_buf, -normalize_bits, 15);
643     dss_sp_scale_vector(p->err_buf1, -normalize_bits, 15);
644
645     if (size > 0)
646         vsum_2 = dss_sp_vector_sum(p, size);
647
648     if (vsum_2 >= 0x40)
649         tmp = (vsum_1 << 11) / vsum_2;
650     else
651         tmp = 1;
652
653     bias     = 409 * tmp >> 15 << 15;
654     tmp      = (bias + 32358 * p->noise_state) >> 15;
655     noise[0] = av_clip_int16(tmp);
656
657     for (i = 1; i < size; i++) {
658         tmp      = (bias + 32358 * noise[i - 1]) >> 15;
659         noise[i] = av_clip_int16(tmp);
660     }
661
662     p->noise_state = noise[size - 1];
663     for (i = 0; i < size; i++) {
664         tmp    = (p->vector_buf[i] * noise[i]) >> 11;
665         dst[i] = av_clip_int16(tmp);
666     }
667 }
668
669 static void dss_sp_update_state(DssSpContext *p, int32_t *dst)
670 {
671     int i, offset = 6, counter = 0, a = 0;
672
673     for (i = 0; i < 6; i++)
674         p->excitation[i] = p->excitation[288 + i];
675
676     for (i = 0; i < 72 * SUBFRAMES; i++)
677         p->excitation[6 + i] = dst[i];
678
679     do {
680         int tmp = 0;
681
682         for (i = 0; i < 6; i++)
683             tmp += p->excitation[offset--] * dss_sp_sinc[a + i * 11];
684
685         offset += 7;
686
687         tmp >>= 15;
688         dst[counter] = av_clip_int16(tmp);
689
690         counter++;
691
692         a = (a + 1) % 11;
693         if (!a)
694             offset++;
695     } while (offset < FF_ARRAY_ELEMS(p->excitation));
696 }
697
698 static void dss_sp_32to16bit(int16_t *dst, int32_t *src, int size)
699 {
700     int i;
701
702     for (i = 0; i < size; i++)
703         dst[i] = av_clip_int16(src[i]);
704 }
705
706 static int dss_sp_decode_one_frame(DssSpContext *p,
707                                    int16_t *abuf_dst, const uint8_t *abuf_src)
708 {
709     int i, j;
710
711     dss_sp_unpack_coeffs(p, abuf_src);
712
713     dss_sp_unpack_filter(p);
714
715     dss_sp_convert_coeffs(p->lpc_filter, p->filter);
716
717     for (j = 0; j < SUBFRAMES; j++) {
718         dss_sp_gen_exc(p->vector_buf, p->history,
719                        p->fparam.pitch_lag[j],
720                        dss_sp_adaptive_gain[p->fparam.sf_adaptive_gain[j]]);
721
722         dss_sp_add_pulses(p->vector_buf, &p->fparam.sf[j]);
723
724         dss_sp_update_buf(p->vector_buf, p->history);
725
726         for (i = 0; i < 72; i++)
727             p->vector_buf[i] = p->history[72 - i];
728
729         dss_sp_shift_sq_sub(p->filter,
730                             p->err_buf2, p->vector_buf);
731
732         dss_sp_sf_synthesis(p, p->lpc_filter[0],
733                             &p->working_buffer[j][0], 72);
734     }
735
736     dss_sp_update_state(p, &p->working_buffer[0][0]);
737
738     dss_sp_32to16bit(abuf_dst,
739                      &p->working_buffer[0][0], 264);
740     return 0;
741 }
742
743 static int dss_sp_decode_frame(AVCodecContext *avctx, void *data,
744                                int *got_frame_ptr, AVPacket *avpkt)
745 {
746     DssSpContext *p    = avctx->priv_data;
747     AVFrame *frame     = data;
748     const uint8_t *buf = avpkt->data;
749     int buf_size       = avpkt->size;
750
751     int16_t *out;
752     int ret;
753
754     if (buf_size < DSS_SP_FRAME_SIZE) {
755         if (buf_size)
756             av_log(avctx, AV_LOG_WARNING,
757                    "Expected %d bytes, got %d - skipping packet.\n",
758                    DSS_SP_FRAME_SIZE, buf_size);
759         *got_frame_ptr = 0;
760         return AVERROR_INVALIDDATA;
761     }
762
763     frame->nb_samples = DSS_SP_SAMPLE_COUNT;
764     if ((ret = ff_get_buffer(avctx, frame, 0)) < 0)
765         return ret;
766
767     out = (int16_t *)frame->data[0];
768
769     dss_sp_decode_one_frame(p, out, buf);
770
771     *got_frame_ptr = 1;
772
773     return DSS_SP_FRAME_SIZE;
774 }
775
776 AVCodec ff_dss_sp_decoder = {
777     .name           = "dss_sp",
778     .long_name      = NULL_IF_CONFIG_SMALL("Digital Speech Standard - Standard Play mode (DSS SP)"),
779     .type           = AVMEDIA_TYPE_AUDIO,
780     .id             = AV_CODEC_ID_DSS_SP,
781     .priv_data_size = sizeof(DssSpContext),
782     .init           = dss_sp_decode_init,
783     .decode         = dss_sp_decode_frame,
784     .capabilities   = AV_CODEC_CAP_DR1,
785 };