git.sesse.net Git - ffmpeg/blob - 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 };