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