Merge remote-tracking branch 'qatar/master'
[ffmpeg.git] / libavcodec / sipr.c
1 /*
2  * SIPR / ACELP.NET decoder
3  *
4  * Copyright (c) 2008 Vladimir Voroshilov
5  * Copyright (c) 2009 Vitor Sessak
6  *
7  * This file is part of FFmpeg.
8  *
9  * FFmpeg is free software; you can redistribute it and/or
10  * modify it under the terms of the GNU Lesser General Public
11  * License as published by the Free Software Foundation; either
12  * version 2.1 of the License, or (at your option) any later version.
13  *
14  * FFmpeg is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
17  * Lesser General Public License for more details.
18  *
19  * You should have received a copy of the GNU Lesser General Public
20  * License along with FFmpeg; if not, write to the Free Software
21  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
22  */
23
24 #include <math.h>
25 #include <stdint.h>
26 #include <string.h>
27
28 #include "libavutil/mathematics.h"
29 #include "avcodec.h"
30 #define ALT_BITSTREAM_READER_LE
31 #include "get_bits.h"
32 #include "dsputil.h"
33
34 #include "lsp.h"
35 #include "celp_math.h"
36 #include "acelp_vectors.h"
37 #include "acelp_pitch_delay.h"
38 #include "acelp_filters.h"
39 #include "celp_filters.h"
40
41 #define MAX_SUBFRAME_COUNT   5
42
43 #include "sipr.h"
44 #include "siprdata.h"
45
46 typedef struct {
47     const char *mode_name;
48     uint16_t bits_per_frame;
49     uint8_t subframe_count;
50     uint8_t frames_per_packet;
51     float pitch_sharp_factor;
52
53     /* bitstream parameters */
54     uint8_t number_of_fc_indexes;
55     uint8_t ma_predictor_bits;  ///< size in bits of the switched MA predictor
56
57     /** size in bits of the i-th stage vector of quantizer */
58     uint8_t vq_indexes_bits[5];
59
60     /** size in bits of the adaptive-codebook index for every subframe */
61     uint8_t pitch_delay_bits[5];
62
63     uint8_t gp_index_bits;
64     uint8_t fc_index_bits[10]; ///< size in bits of the fixed codebook indexes
65     uint8_t gc_index_bits;     ///< size in bits of the gain  codebook indexes
66 } SiprModeParam;
67
68 static const SiprModeParam modes[MODE_COUNT] = {
69     [MODE_16k] = {
70         .mode_name          = "16k",
71         .bits_per_frame     = 160,
72         .subframe_count     = SUBFRAME_COUNT_16k,
73         .frames_per_packet  = 1,
74         .pitch_sharp_factor = 0.00,
75
76         .number_of_fc_indexes = 10,
77         .ma_predictor_bits    = 1,
78         .vq_indexes_bits      = {7, 8, 7, 7, 7},
79         .pitch_delay_bits     = {9, 6},
80         .gp_index_bits        = 4,
81         .fc_index_bits        = {4, 5, 4, 5, 4, 5, 4, 5, 4, 5},
82         .gc_index_bits        = 5
83     },
84
85     [MODE_8k5] = {
86         .mode_name          = "8k5",
87         .bits_per_frame     = 152,
88         .subframe_count     = 3,
89         .frames_per_packet  = 1,
90         .pitch_sharp_factor = 0.8,
91
92         .number_of_fc_indexes = 3,
93         .ma_predictor_bits    = 0,
94         .vq_indexes_bits      = {6, 7, 7, 7, 5},
95         .pitch_delay_bits     = {8, 5, 5},
96         .gp_index_bits        = 0,
97         .fc_index_bits        = {9, 9, 9},
98         .gc_index_bits        = 7
99     },
100
101     [MODE_6k5] = {
102         .mode_name          = "6k5",
103         .bits_per_frame     = 232,
104         .subframe_count     = 3,
105         .frames_per_packet  = 2,
106         .pitch_sharp_factor = 0.8,
107
108         .number_of_fc_indexes = 3,
109         .ma_predictor_bits    = 0,
110         .vq_indexes_bits      = {6, 7, 7, 7, 5},
111         .pitch_delay_bits     = {8, 5, 5},
112         .gp_index_bits        = 0,
113         .fc_index_bits        = {5, 5, 5},
114         .gc_index_bits        = 7
115     },
116
117     [MODE_5k0] = {
118         .mode_name          = "5k0",
119         .bits_per_frame     = 296,
120         .subframe_count     = 5,
121         .frames_per_packet  = 2,
122         .pitch_sharp_factor = 0.85,
123
124         .number_of_fc_indexes = 1,
125         .ma_predictor_bits    = 0,
126         .vq_indexes_bits      = {6, 7, 7, 7, 5},
127         .pitch_delay_bits     = {8, 5, 8, 5, 5},
128         .gp_index_bits        = 0,
129         .fc_index_bits        = {10},
130         .gc_index_bits        = 7
131     }
132 };
133
134 const float ff_pow_0_5[] = {
135     1.0/(1 <<  1), 1.0/(1 <<  2), 1.0/(1 <<  3), 1.0/(1 <<  4),
136     1.0/(1 <<  5), 1.0/(1 <<  6), 1.0/(1 <<  7), 1.0/(1 <<  8),
137     1.0/(1 <<  9), 1.0/(1 << 10), 1.0/(1 << 11), 1.0/(1 << 12),
138     1.0/(1 << 13), 1.0/(1 << 14), 1.0/(1 << 15), 1.0/(1 << 16)
139 };
140
141 static void dequant(float *out, const int *idx, const float *cbs[])
142 {
143     int i;
144     int stride  = 2;
145     int num_vec = 5;
146
147     for (i = 0; i < num_vec; i++)
148         memcpy(out + stride*i, cbs[i] + stride*idx[i], stride*sizeof(float));
149
150 }
151
152 static void lsf_decode_fp(float *lsfnew, float *lsf_history,
153                           const SiprParameters *parm)
154 {
155     int i;
156     float lsf_tmp[LP_FILTER_ORDER];
157
158     dequant(lsf_tmp, parm->vq_indexes, lsf_codebooks);
159
160     for (i = 0; i < LP_FILTER_ORDER; i++)
161         lsfnew[i] = lsf_history[i] * 0.33 + lsf_tmp[i] + mean_lsf[i];
162
163     ff_sort_nearly_sorted_floats(lsfnew, LP_FILTER_ORDER - 1);
164
165     /* Note that a minimum distance is not enforced between the last value and
166        the previous one, contrary to what is done in ff_acelp_reorder_lsf() */
167     ff_set_min_dist_lsf(lsfnew, LSFQ_DIFF_MIN, LP_FILTER_ORDER - 1);
168     lsfnew[9] = FFMIN(lsfnew[LP_FILTER_ORDER - 1], 1.3 * M_PI);
169
170     memcpy(lsf_history, lsf_tmp, LP_FILTER_ORDER * sizeof(*lsf_history));
171
172     for (i = 0; i < LP_FILTER_ORDER - 1; i++)
173         lsfnew[i] = cos(lsfnew[i]);
174     lsfnew[LP_FILTER_ORDER - 1] *= 6.153848 / M_PI;
175 }
176
177 /** Apply pitch lag to the fixed vector (AMR section 6.1.2). */
178 static void pitch_sharpening(int pitch_lag_int, float beta,
179                              float *fixed_vector)
180 {
181     int i;
182
183     for (i = pitch_lag_int; i < SUBFR_SIZE; i++)
184         fixed_vector[i] += beta * fixed_vector[i - pitch_lag_int];
185 }
186
187 /**
188  * Extract decoding parameters from the input bitstream.
189  * @param parms          parameters structure
190  * @param pgb            pointer to initialized GetBitContext structure
191  */
192 static void decode_parameters(SiprParameters* parms, GetBitContext *pgb,
193                               const SiprModeParam *p)
194 {
195     int i, j;
196
197     if (p->ma_predictor_bits)
198         parms->ma_pred_switch       = get_bits(pgb, p->ma_predictor_bits);
199
200     for (i = 0; i < 5; i++)
201         parms->vq_indexes[i]        = get_bits(pgb, p->vq_indexes_bits[i]);
202
203     for (i = 0; i < p->subframe_count; i++) {
204         parms->pitch_delay[i]       = get_bits(pgb, p->pitch_delay_bits[i]);
205         if (p->gp_index_bits)
206             parms->gp_index[i]      = get_bits(pgb, p->gp_index_bits);
207
208         for (j = 0; j < p->number_of_fc_indexes; j++)
209             parms->fc_indexes[i][j] = get_bits(pgb, p->fc_index_bits[j]);
210
211         parms->gc_index[i]          = get_bits(pgb, p->gc_index_bits);
212     }
213 }
214
215 static void sipr_decode_lp(float *lsfnew, const float *lsfold, float *Az,
216                            int num_subfr)
217 {
218     double lsfint[LP_FILTER_ORDER];
219     int i,j;
220     float t, t0 = 1.0 / num_subfr;
221
222     t = t0 * 0.5;
223     for (i = 0; i < num_subfr; i++) {
224         for (j = 0; j < LP_FILTER_ORDER; j++)
225             lsfint[j] = lsfold[j] * (1 - t) + t * lsfnew[j];
226
227         ff_amrwb_lsp2lpc(lsfint, Az, LP_FILTER_ORDER);
228         Az += LP_FILTER_ORDER;
229         t += t0;
230     }
231 }
232
233 /**
234  * Evaluate the adaptive impulse response.
235  */
236 static void eval_ir(const float *Az, int pitch_lag, float *freq,
237                     float pitch_sharp_factor)
238 {
239     float tmp1[SUBFR_SIZE+1], tmp2[LP_FILTER_ORDER+1];
240     int i;
241
242     tmp1[0] = 1.;
243     for (i = 0; i < LP_FILTER_ORDER; i++) {
244         tmp1[i+1] = Az[i] * ff_pow_0_55[i];
245         tmp2[i  ] = Az[i] * ff_pow_0_7 [i];
246     }
247     memset(tmp1 + 11, 0, 37 * sizeof(float));
248
249     ff_celp_lp_synthesis_filterf(freq, tmp2, tmp1, SUBFR_SIZE,
250                                  LP_FILTER_ORDER);
251
252     pitch_sharpening(pitch_lag, pitch_sharp_factor, freq);
253 }
254
255 /**
256  * Evaluate the convolution of a vector with a sparse vector.
257  */
258 static void convolute_with_sparse(float *out, const AMRFixed *pulses,
259                                   const float *shape, int length)
260 {
261     int i, j;
262
263     memset(out, 0, length*sizeof(float));
264     for (i = 0; i < pulses->n; i++)
265         for (j = pulses->x[i]; j < length; j++)
266             out[j] += pulses->y[i] * shape[j - pulses->x[i]];
267 }
268
269 /**
270  * Apply postfilter, very similar to AMR one.
271  */
272 static void postfilter_5k0(SiprContext *ctx, const float *lpc, float *samples)
273 {
274     float buf[SUBFR_SIZE + LP_FILTER_ORDER];
275     float *pole_out = buf + LP_FILTER_ORDER;
276     float lpc_n[LP_FILTER_ORDER];
277     float lpc_d[LP_FILTER_ORDER];
278     int i;
279
280     for (i = 0; i < LP_FILTER_ORDER; i++) {
281         lpc_d[i] = lpc[i] * ff_pow_0_75[i];
282         lpc_n[i] = lpc[i] * ff_pow_0_5 [i];
283     };
284
285     memcpy(pole_out - LP_FILTER_ORDER, ctx->postfilter_mem,
286            LP_FILTER_ORDER*sizeof(float));
287
288     ff_celp_lp_synthesis_filterf(pole_out, lpc_d, samples, SUBFR_SIZE,
289                                  LP_FILTER_ORDER);
290
291     memcpy(ctx->postfilter_mem, pole_out + SUBFR_SIZE - LP_FILTER_ORDER,
292            LP_FILTER_ORDER*sizeof(float));
293
294     ff_tilt_compensation(&ctx->tilt_mem, 0.4, pole_out, SUBFR_SIZE);
295
296     memcpy(pole_out - LP_FILTER_ORDER, ctx->postfilter_mem5k0,
297            LP_FILTER_ORDER*sizeof(*pole_out));
298
299     memcpy(ctx->postfilter_mem5k0, pole_out + SUBFR_SIZE - LP_FILTER_ORDER,
300            LP_FILTER_ORDER*sizeof(*pole_out));
301
302     ff_celp_lp_zero_synthesis_filterf(samples, lpc_n, pole_out, SUBFR_SIZE,
303                                       LP_FILTER_ORDER);
304
305 }
306
307 static void decode_fixed_sparse(AMRFixed *fixed_sparse, const int16_t *pulses,
308                                 SiprMode mode, int low_gain)
309 {
310     int i;
311
312     switch (mode) {
313     case MODE_6k5:
314         for (i = 0; i < 3; i++) {
315             fixed_sparse->x[i] = 3 * (pulses[i] & 0xf) + i;
316             fixed_sparse->y[i] = pulses[i] & 0x10 ? -1 : 1;
317         }
318         fixed_sparse->n = 3;
319         break;
320     case MODE_8k5:
321         for (i = 0; i < 3; i++) {
322             fixed_sparse->x[2*i    ] = 3 * ((pulses[i] >> 4) & 0xf) + i;
323             fixed_sparse->x[2*i + 1] = 3 * ( pulses[i]       & 0xf) + i;
324
325             fixed_sparse->y[2*i    ] = (pulses[i] & 0x100) ? -1.0: 1.0;
326
327             fixed_sparse->y[2*i + 1] =
328                 (fixed_sparse->x[2*i + 1] < fixed_sparse->x[2*i]) ?
329                 -fixed_sparse->y[2*i    ] : fixed_sparse->y[2*i];
330         }
331
332         fixed_sparse->n = 6;
333         break;
334     case MODE_5k0:
335     default:
336         if (low_gain) {
337             int offset = (pulses[0] & 0x200) ? 2 : 0;
338             int val = pulses[0];
339
340             for (i = 0; i < 3; i++) {
341                 int index = (val & 0x7) * 6 + 4 - i*2;
342
343                 fixed_sparse->y[i] = (offset + index) & 0x3 ? -1 : 1;
344                 fixed_sparse->x[i] = index;
345
346                 val >>= 3;
347             }
348             fixed_sparse->n = 3;
349         } else {
350             int pulse_subset = (pulses[0] >> 8) & 1;
351
352             fixed_sparse->x[0] = ((pulses[0] >> 4) & 15) * 3 + pulse_subset;
353             fixed_sparse->x[1] = ( pulses[0]       & 15) * 3 + pulse_subset + 1;
354
355             fixed_sparse->y[0] = pulses[0] & 0x200 ? -1 : 1;
356             fixed_sparse->y[1] = -fixed_sparse->y[0];
357             fixed_sparse->n = 2;
358         }
359         break;
360     }
361 }
362
363 static void decode_frame(SiprContext *ctx, SiprParameters *params,
364                          float *out_data)
365 {
366     int i, j;
367     int subframe_count = modes[ctx->mode].subframe_count;
368     int frame_size = subframe_count * SUBFR_SIZE;
369     float Az[LP_FILTER_ORDER * MAX_SUBFRAME_COUNT];
370     float *excitation;
371     float ir_buf[SUBFR_SIZE + LP_FILTER_ORDER];
372     float lsf_new[LP_FILTER_ORDER];
373     float *impulse_response = ir_buf + LP_FILTER_ORDER;
374     float *synth = ctx->synth_buf + 16; // 16 instead of LP_FILTER_ORDER for
375                                         // memory alignment
376     int t0_first = 0;
377     AMRFixed fixed_cb;
378
379     memset(ir_buf, 0, LP_FILTER_ORDER * sizeof(float));
380     lsf_decode_fp(lsf_new, ctx->lsf_history, params);
381
382     sipr_decode_lp(lsf_new, ctx->lsp_history, Az, subframe_count);
383
384     memcpy(ctx->lsp_history, lsf_new, LP_FILTER_ORDER * sizeof(float));
385
386     excitation = ctx->excitation + PITCH_DELAY_MAX + L_INTERPOL;
387
388     for (i = 0; i < subframe_count; i++) {
389         float *pAz = Az + i*LP_FILTER_ORDER;
390         float fixed_vector[SUBFR_SIZE];
391         int T0,T0_frac;
392         float pitch_gain, gain_code, avg_energy;
393
394         ff_decode_pitch_lag(&T0, &T0_frac, params->pitch_delay[i], t0_first, i,
395                             ctx->mode == MODE_5k0, 6);
396
397         if (i == 0 || (i == 2 && ctx->mode == MODE_5k0))
398             t0_first = T0;
399
400         ff_acelp_interpolatef(excitation, excitation - T0 + (T0_frac <= 0),
401                               ff_b60_sinc, 6,
402                               2 * ((2 + T0_frac)%3 + 1), LP_FILTER_ORDER,
403                               SUBFR_SIZE);
404
405         decode_fixed_sparse(&fixed_cb, params->fc_indexes[i], ctx->mode,
406                             ctx->past_pitch_gain < 0.8);
407
408         eval_ir(pAz, T0, impulse_response, modes[ctx->mode].pitch_sharp_factor);
409
410         convolute_with_sparse(fixed_vector, &fixed_cb, impulse_response,
411                               SUBFR_SIZE);
412
413         avg_energy =
414             (0.01 + ff_dot_productf(fixed_vector, fixed_vector, SUBFR_SIZE))/
415                 SUBFR_SIZE;
416
417         ctx->past_pitch_gain = pitch_gain = gain_cb[params->gc_index[i]][0];
418
419         gain_code = ff_amr_set_fixed_gain(gain_cb[params->gc_index[i]][1],
420                                           avg_energy, ctx->energy_history,
421                                           34 - 15.0/(0.05*M_LN10/M_LN2),
422                                           pred);
423
424         ff_weighted_vector_sumf(excitation, excitation, fixed_vector,
425                                 pitch_gain, gain_code, SUBFR_SIZE);
426
427         pitch_gain *= 0.5 * pitch_gain;
428         pitch_gain = FFMIN(pitch_gain, 0.4);
429
430         ctx->gain_mem = 0.7 * ctx->gain_mem + 0.3 * pitch_gain;
431         ctx->gain_mem = FFMIN(ctx->gain_mem, pitch_gain);
432         gain_code *= ctx->gain_mem;
433
434         for (j = 0; j < SUBFR_SIZE; j++)
435             fixed_vector[j] = excitation[j] - gain_code * fixed_vector[j];
436
437         if (ctx->mode == MODE_5k0) {
438             postfilter_5k0(ctx, pAz, fixed_vector);
439
440             ff_celp_lp_synthesis_filterf(ctx->postfilter_syn5k0 + LP_FILTER_ORDER + i*SUBFR_SIZE,
441                                          pAz, excitation, SUBFR_SIZE,
442                                          LP_FILTER_ORDER);
443         }
444
445         ff_celp_lp_synthesis_filterf(synth + i*SUBFR_SIZE, pAz, fixed_vector,
446                                      SUBFR_SIZE, LP_FILTER_ORDER);
447
448         excitation += SUBFR_SIZE;
449     }
450
451     memcpy(synth - LP_FILTER_ORDER, synth + frame_size - LP_FILTER_ORDER,
452            LP_FILTER_ORDER * sizeof(float));
453
454     if (ctx->mode == MODE_5k0) {
455         for (i = 0; i < subframe_count; i++) {
456             float energy = ff_dot_productf(ctx->postfilter_syn5k0 + LP_FILTER_ORDER + i*SUBFR_SIZE,
457                                            ctx->postfilter_syn5k0 + LP_FILTER_ORDER + i*SUBFR_SIZE,
458                                            SUBFR_SIZE);
459             ff_adaptive_gain_control(&synth[i * SUBFR_SIZE],
460                                      &synth[i * SUBFR_SIZE], energy,
461                                      SUBFR_SIZE, 0.9, &ctx->postfilter_agc);
462         }
463
464         memcpy(ctx->postfilter_syn5k0, ctx->postfilter_syn5k0 + frame_size,
465                LP_FILTER_ORDER*sizeof(float));
466     }
467     memmove(ctx->excitation, excitation - PITCH_DELAY_MAX - L_INTERPOL,
468            (PITCH_DELAY_MAX + L_INTERPOL) * sizeof(float));
469
470     ff_acelp_apply_order_2_transfer_function(out_data, synth,
471                                              (const float[2]) {-1.99997   , 1.000000000},
472                                              (const float[2]) {-1.93307352, 0.935891986},
473                                              0.939805806,
474                                              ctx->highpass_filt_mem,
475                                              frame_size);
476 }
477
478 static av_cold int sipr_decoder_init(AVCodecContext * avctx)
479 {
480     SiprContext *ctx = avctx->priv_data;
481     int i;
482
483     if      (avctx->bit_rate > 12200) ctx->mode = MODE_16k;
484     else if (avctx->bit_rate > 7500 ) ctx->mode = MODE_8k5;
485     else if (avctx->bit_rate > 5750 ) ctx->mode = MODE_6k5;
486     else                              ctx->mode = MODE_5k0;
487
488     av_log(avctx, AV_LOG_DEBUG, "Mode: %s\n", modes[ctx->mode].mode_name);
489
490     if (ctx->mode == MODE_16k)
491         ff_sipr_init_16k(ctx);
492
493     for (i = 0; i < LP_FILTER_ORDER; i++)
494         ctx->lsp_history[i] = cos((i+1) * M_PI / (LP_FILTER_ORDER + 1));
495
496     for (i = 0; i < 4; i++)
497         ctx->energy_history[i] = -14;
498
499     avctx->sample_fmt = AV_SAMPLE_FMT_FLT;
500
501     return 0;
502 }
503
504 static int sipr_decode_frame(AVCodecContext *avctx, void *datap,
505                              int *data_size, AVPacket *avpkt)
506 {
507     SiprContext *ctx = avctx->priv_data;
508     const uint8_t *buf=avpkt->data;
509     SiprParameters parm;
510     const SiprModeParam *mode_par = &modes[ctx->mode];
511     GetBitContext gb;
512     float *data = datap;
513     int subframe_size = ctx->mode == MODE_16k ? L_SUBFR_16k : SUBFR_SIZE;
514     int i, out_size;
515
516     ctx->avctx = avctx;
517     if (avpkt->size < (mode_par->bits_per_frame >> 3)) {
518         av_log(avctx, AV_LOG_ERROR,
519                "Error processing packet: packet size (%d) too small\n",
520                avpkt->size);
521
522         *data_size = 0;
523         return -1;
524     }
525
526     out_size = mode_par->frames_per_packet * subframe_size *
527                mode_par->subframe_count *
528                av_get_bytes_per_sample(avctx->sample_fmt);
529     if (*data_size < out_size) {
530         av_log(avctx, AV_LOG_ERROR,
531                "Error processing packet: output buffer (%d) too small\n",
532                *data_size);
533
534         *data_size = 0;
535         return -1;
536     }
537
538     init_get_bits(&gb, buf, mode_par->bits_per_frame);
539
540     for (i = 0; i < mode_par->frames_per_packet; i++) {
541         decode_parameters(&parm, &gb, mode_par);
542
543         if (ctx->mode == MODE_16k)
544             ff_sipr_decode_frame_16k(ctx, &parm, data);
545         else
546             decode_frame(ctx, &parm, data);
547
548         data += subframe_size * mode_par->subframe_count;
549     }
550
551     *data_size = out_size;
552
553     return mode_par->bits_per_frame >> 3;
554 }
555
556 AVCodec ff_sipr_decoder = {
557     .name           = "sipr",
558     .type           = AVMEDIA_TYPE_AUDIO,
559     .id             = CODEC_ID_SIPR,
560     .priv_data_size = sizeof(SiprContext),
561     .init           = sipr_decoder_init,
562     .decode         = sipr_decode_frame,
563     .long_name = NULL_IF_CONFIG_SMALL("RealAudio SIPR / ACELP.NET"),
564 };