9fe5a87fd2fc35c425f1918c3abd04d0e9cf0d4c
[opus.git] / silk / PLC.c
1 /***********************************************************************
2 Copyright (c) 2006-2011, Skype Limited. All rights reserved.
3 Redistribution and use in source and binary forms, with or without
4 modification, (subject to the limitations in the disclaimer below)
5 are permitted provided that the following conditions are met:
6 - Redistributions of source code must retain the above copyright notice,
7 this list of conditions and the following disclaimer.
8 - Redistributions in binary form must reproduce the above copyright
9 notice, this list of conditions and the following disclaimer in the
10 documentation and/or other materials provided with the distribution.
11 - Neither the name of Skype Limited, nor the names of specific
12 contributors, may be used to endorse or promote products derived from
13 this software without specific prior written permission.
14 NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED
15 BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
16 CONTRIBUTORS ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
17 BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
18 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
19 COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
20 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
21 NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
22 USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
23 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
25 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 ***********************************************************************/
27
28 #ifdef HAVE_CONFIG_H
29 #include "config.h"
30 #endif
31
32 #include "main.h"
33 #include "PLC.h"
34
35 #define NB_ATT 2
36 static const opus_int16 HARM_ATT_Q15[NB_ATT]              = { 32440, 31130 }; /* 0.99, 0.95 */
37 static const opus_int16 PLC_RAND_ATTENUATE_V_Q15[NB_ATT]  = { 31130, 26214 }; /* 0.95, 0.8 */
38 static const opus_int16 PLC_RAND_ATTENUATE_UV_Q15[NB_ATT] = { 32440, 29491 }; /* 0.99, 0.9 */
39
40 void silk_PLC_Reset(
41     silk_decoder_state      *psDec              /* I/O Decoder state        */
42 )
43 {
44     psDec->sPLC.pitchL_Q8 = silk_RSHIFT( psDec->frame_length, 1 );
45 }
46
47 void silk_PLC(
48     silk_decoder_state          *psDec,             /* I Decoder state          */
49     silk_decoder_control        *psDecCtrl,         /* I Decoder control        */
50     opus_int16                   frame[],            /* O Concealed signal       */
51     opus_int                     length,             /* I length of residual     */
52     opus_int                     lost                /* I Loss flag              */
53 )
54 {
55     /* PLC control function */
56     if( psDec->fs_kHz != psDec->sPLC.fs_kHz ) {
57         silk_PLC_Reset( psDec );
58         psDec->sPLC.fs_kHz = psDec->fs_kHz;
59     }
60
61     if( lost ) {
62         /****************************/
63         /* Generate Signal          */
64         /****************************/
65         silk_PLC_conceal( psDec, psDecCtrl, frame, length );
66
67         psDec->lossCnt++;
68     } else {
69         /****************************/
70         /* Update state             */
71         /****************************/
72         silk_PLC_update( psDec, psDecCtrl );
73     }
74 }
75
76 /**************************************************/
77 /* Update state of PLC                            */
78 /**************************************************/
79 void silk_PLC_update(
80     silk_decoder_state          *psDec,             /* (I/O) Decoder state          */
81     silk_decoder_control        *psDecCtrl          /* (I/O) Decoder control        */
82 )
83 {
84     opus_int32 LTP_Gain_Q14, temp_LTP_Gain_Q14;
85     opus_int   i, j;
86     silk_PLC_struct *psPLC;
87
88     psPLC = &psDec->sPLC;
89
90     /* Update parameters used in case of packet loss */
91     psDec->prevSignalType = psDec->indices.signalType;
92     LTP_Gain_Q14 = 0;
93     if( psDec->indices.signalType == TYPE_VOICED ) {
94         /* Find the parameters for the last subframe which contains a pitch pulse */
95         for( j = 0; j * psDec->subfr_length < psDecCtrl->pitchL[ psDec->nb_subfr - 1 ]; j++ ) {
96             if( j == psDec->nb_subfr ){
97                 break;
98             }
99             temp_LTP_Gain_Q14 = 0;
100             for( i = 0; i < LTP_ORDER; i++ ) {
101                 temp_LTP_Gain_Q14 += psDecCtrl->LTPCoef_Q14[ ( psDec->nb_subfr - 1 - j ) * LTP_ORDER  + i ];
102             }
103             if( temp_LTP_Gain_Q14 > LTP_Gain_Q14 ) {
104                 LTP_Gain_Q14 = temp_LTP_Gain_Q14;
105                 silk_memcpy( psPLC->LTPCoef_Q14,
106                     &psDecCtrl->LTPCoef_Q14[ silk_SMULBB( psDec->nb_subfr - 1 - j, LTP_ORDER ) ],
107                     LTP_ORDER * sizeof( opus_int16 ) );
108
109                 psPLC->pitchL_Q8 = silk_LSHIFT( psDecCtrl->pitchL[ psDec->nb_subfr - 1 - j ], 8 );
110             }
111         }
112
113 #if USE_SINGLE_TAP
114         silk_memset( psPLC->LTPCoef_Q14, 0, LTP_ORDER * sizeof( opus_int16 ) );
115         psPLC->LTPCoef_Q14[ LTP_ORDER / 2 ] = LTP_Gain_Q14;
116 #endif
117
118         /* Limit LT coefs */
119         if( LTP_Gain_Q14 < V_PITCH_GAIN_START_MIN_Q14 ) {
120             opus_int   scale_Q10;
121             opus_int32 tmp;
122
123             tmp = silk_LSHIFT( V_PITCH_GAIN_START_MIN_Q14, 10 );
124             scale_Q10 = silk_DIV32( tmp, silk_max( LTP_Gain_Q14, 1 ) );
125             for( i = 0; i < LTP_ORDER; i++ ) {
126                 psPLC->LTPCoef_Q14[ i ] = silk_RSHIFT( silk_SMULBB( psPLC->LTPCoef_Q14[ i ], scale_Q10 ), 10 );
127             }
128         } else if( LTP_Gain_Q14 > V_PITCH_GAIN_START_MAX_Q14 ) {
129             opus_int   scale_Q14;
130             opus_int32 tmp;
131
132             tmp = silk_LSHIFT( V_PITCH_GAIN_START_MAX_Q14, 14 );
133             scale_Q14 = silk_DIV32( tmp, silk_max( LTP_Gain_Q14, 1 ) );
134             for( i = 0; i < LTP_ORDER; i++ ) {
135                 psPLC->LTPCoef_Q14[ i ] = silk_RSHIFT( silk_SMULBB( psPLC->LTPCoef_Q14[ i ], scale_Q14 ), 14 );
136             }
137         }
138     } else {
139         psPLC->pitchL_Q8 = silk_LSHIFT( silk_SMULBB( psDec->fs_kHz, 18 ), 8 );
140         silk_memset( psPLC->LTPCoef_Q14, 0, LTP_ORDER * sizeof( opus_int16 ));
141     }
142
143     /* Save LPC coeficients */
144     silk_memcpy( psPLC->prevLPC_Q12, psDecCtrl->PredCoef_Q12[ 1 ], psDec->LPC_order * sizeof( opus_int16 ) );
145     psPLC->prevLTP_scale_Q14 = psDecCtrl->LTP_scale_Q14;
146
147     /* Save Gains */
148     silk_memcpy( psPLC->prevGain_Q16, psDecCtrl->Gains_Q16, psDec->nb_subfr * sizeof( opus_int32 ) );
149 }
150
151 void silk_PLC_conceal(
152     silk_decoder_state          *psDec,             /* I/O Decoder state */
153     silk_decoder_control        *psDecCtrl,         /* I/O Decoder control */
154     opus_int16                   frame[],            /* O concealed signal */
155     opus_int                     length              /* I length of residual */
156 )
157 {
158     opus_int   i, j, k;
159     opus_int16 *B_Q14, exc_buf[ MAX_FRAME_LENGTH ], *exc_buf_ptr;
160     opus_int16 rand_scale_Q14, A_Q12_tmp[ MAX_LPC_ORDER ];
161     opus_int32 rand_seed, harm_Gain_Q15, rand_Gain_Q15;
162     opus_int   lag, idx, sLTP_buf_idx, shift1, shift2;
163     opus_int32 energy1, energy2, *rand_ptr, *pred_lag_ptr;
164     opus_int32 sig_Q10[ MAX_FRAME_LENGTH ], *sig_Q10_ptr, LPC_exc_Q10, LPC_pred_Q10,  LTP_pred_Q14;
165     silk_PLC_struct *psPLC;
166     psPLC = &psDec->sPLC;
167
168     /* Update LTP buffer */
169     silk_memmove( psDec->sLTP_Q16, &psDec->sLTP_Q16[ psDec->frame_length ], psDec->ltp_mem_length * sizeof( opus_int32 ) );
170
171     /* LPC concealment. Apply BWE to previous LPC */
172     silk_bwexpander( psPLC->prevLPC_Q12, psDec->LPC_order, SILK_FIX_CONST( BWE_COEF, 16 ) );
173
174     /* Find random noise component */
175     /* Scale previous excitation signal */
176     exc_buf_ptr = exc_buf;
177     /* FIXME: JMV: Is this the right fix? */
178     for (i=0;i<MAX_FRAME_LENGTH;i++)
179         exc_buf[i] = 0;
180     for( k = ( psDec->nb_subfr >> 1 ); k < psDec->nb_subfr; k++ ) {
181         for( i = 0; i < psDec->subfr_length; i++ ) {
182             exc_buf_ptr[ i ] = ( opus_int16 )silk_RSHIFT(
183                 silk_SMULWW( psDec->exc_Q10[ i + k * psDec->subfr_length ], psPLC->prevGain_Q16[ k ] ), 10 );
184         }
185         exc_buf_ptr += psDec->subfr_length;
186     }
187     /* Find the subframe with lowest energy of the last two and use that as random noise generator */
188     silk_sum_sqr_shift( &energy1, &shift1, exc_buf,                         psDec->subfr_length );
189     silk_sum_sqr_shift( &energy2, &shift2, &exc_buf[ psDec->subfr_length ], psDec->subfr_length );
190
191     if( silk_RSHIFT( energy1, shift2 ) < silk_RSHIFT( energy2, shift1 ) ) {
192         /* First sub-frame has lowest energy */
193         rand_ptr = &psDec->exc_Q10[ silk_max_int( 0, 3 * psDec->subfr_length - RAND_BUF_SIZE ) ];
194     } else {
195         /* Second sub-frame has lowest energy */
196         rand_ptr = &psDec->exc_Q10[ silk_max_int( 0, psDec->frame_length - RAND_BUF_SIZE ) ];
197     }
198
199     /* Setup Gain to random noise component */
200     B_Q14          = psPLC->LTPCoef_Q14;
201     rand_scale_Q14 = psPLC->randScale_Q14;
202
203     /* Setup attenuation gains */
204     harm_Gain_Q15 = HARM_ATT_Q15[ silk_min_int( NB_ATT - 1, psDec->lossCnt ) ];
205     if( psDec->prevSignalType == TYPE_VOICED ) {
206         rand_Gain_Q15 = PLC_RAND_ATTENUATE_V_Q15[  silk_min_int( NB_ATT - 1, psDec->lossCnt ) ];
207     } else {
208         rand_Gain_Q15 = PLC_RAND_ATTENUATE_UV_Q15[ silk_min_int( NB_ATT - 1, psDec->lossCnt ) ];
209     }
210
211     /* First Lost frame */
212     if( psDec->lossCnt == 0 ) {
213         rand_scale_Q14 = 1 << 14;
214
215         /* Reduce random noise Gain for voiced frames */
216         if( psDec->prevSignalType == TYPE_VOICED ) {
217             for( i = 0; i < LTP_ORDER; i++ ) {
218                 rand_scale_Q14 -= B_Q14[ i ];
219             }
220             rand_scale_Q14 = silk_max_16( 3277, rand_scale_Q14 ); /* 0.2 */
221             rand_scale_Q14 = ( opus_int16 )silk_RSHIFT( silk_SMULBB( rand_scale_Q14, psPLC->prevLTP_scale_Q14 ), 14 );
222         } else {
223             /* Reduce random noise for unvoiced frames with high LPC gain */
224             opus_int32 invGain_Q30, down_scale_Q30;
225
226             silk_LPC_inverse_pred_gain( &invGain_Q30, psPLC->prevLPC_Q12, psDec->LPC_order );
227
228             down_scale_Q30 = silk_min_32( silk_RSHIFT( 1 << 30, LOG2_INV_LPC_GAIN_HIGH_THRES ), invGain_Q30 );
229             down_scale_Q30 = silk_max_32( silk_RSHIFT( 1 << 30, LOG2_INV_LPC_GAIN_LOW_THRES ), down_scale_Q30 );
230             down_scale_Q30 = silk_LSHIFT( down_scale_Q30, LOG2_INV_LPC_GAIN_HIGH_THRES );
231
232             rand_Gain_Q15 = silk_RSHIFT( silk_SMULWB( down_scale_Q30, rand_Gain_Q15 ), 14 );
233         }
234     }
235
236     rand_seed    = psPLC->rand_seed;
237     lag          = silk_RSHIFT_ROUND( psPLC->pitchL_Q8, 8 );
238     sLTP_buf_idx = psDec->ltp_mem_length;
239
240     /***************************/
241     /* LTP synthesis filtering */
242     /***************************/
243     sig_Q10_ptr = sig_Q10;
244     for( k = 0; k < psDec->nb_subfr; k++ ) {
245         /* Setup pointer */
246         pred_lag_ptr = &psDec->sLTP_Q16[ sLTP_buf_idx - lag + LTP_ORDER / 2 ];
247         for( i = 0; i < psDec->subfr_length; i++ ) {
248             rand_seed = silk_RAND( rand_seed );
249             idx = silk_RSHIFT( rand_seed, 25 ) & RAND_BUF_MASK;
250
251             /* Unrolled loop */
252             LTP_pred_Q14 = silk_SMULWB(               pred_lag_ptr[  0 ], B_Q14[ 0 ] );
253             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -1 ], B_Q14[ 1 ] );
254             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -2 ], B_Q14[ 2 ] );
255             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -3 ], B_Q14[ 3 ] );
256             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -4 ], B_Q14[ 4 ] );
257             pred_lag_ptr++;
258
259             /* Generate LPC residual */
260             LPC_exc_Q10 = silk_LSHIFT( silk_SMULWB( rand_ptr[ idx ], rand_scale_Q14 ), 2 ); /* Random noise part */
261             LPC_exc_Q10 = silk_ADD32( LPC_exc_Q10, silk_RSHIFT_ROUND( LTP_pred_Q14, 4 ) );  /* Harmonic part */
262
263             /* Update states */
264             psDec->sLTP_Q16[ sLTP_buf_idx ] = silk_LSHIFT( LPC_exc_Q10, 6 );
265             sLTP_buf_idx++;
266
267             /* Save LPC residual */
268             sig_Q10_ptr[ i ] = LPC_exc_Q10;
269         }
270         sig_Q10_ptr += psDec->subfr_length;
271         /* Gradually reduce LTP gain */
272         for( j = 0; j < LTP_ORDER; j++ ) {
273             B_Q14[ j ] = silk_RSHIFT( silk_SMULBB( harm_Gain_Q15, B_Q14[ j ] ), 15 );
274         }
275         /* Gradually reduce excitation gain */
276         rand_scale_Q14 = silk_RSHIFT( silk_SMULBB( rand_scale_Q14, rand_Gain_Q15 ), 15 );
277
278         /* Slowly increase pitch lag */
279         psPLC->pitchL_Q8 += silk_SMULWB( psPLC->pitchL_Q8, PITCH_DRIFT_FAC_Q16 );
280         psPLC->pitchL_Q8 = silk_min_32( psPLC->pitchL_Q8, silk_LSHIFT( silk_SMULBB( MAX_PITCH_LAG_MS, psDec->fs_kHz ), 8 ) );
281         lag = silk_RSHIFT_ROUND( psPLC->pitchL_Q8, 8 );
282     }
283
284     /***************************/
285     /* LPC synthesis filtering */
286     /***************************/
287     sig_Q10_ptr = sig_Q10;
288     /* Preload LPC coeficients to array on stack. Gives small performance gain */
289     silk_memcpy( A_Q12_tmp, psPLC->prevLPC_Q12, psDec->LPC_order * sizeof( opus_int16 ) );
290     silk_assert( psDec->LPC_order >= 10 ); /* check that unrolling works */
291     for( k = 0; k < psDec->nb_subfr; k++ ) {
292         for( i = 0; i < psDec->subfr_length; i++ ){
293             /* partly unrolled */
294             LPC_pred_Q10 = silk_SMULWB(               psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  1 ], A_Q12_tmp[ 0 ] );
295             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  2 ], A_Q12_tmp[ 1 ] );
296             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  3 ], A_Q12_tmp[ 2 ] );
297             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  4 ], A_Q12_tmp[ 3 ] );
298             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  5 ], A_Q12_tmp[ 4 ] );
299             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  6 ], A_Q12_tmp[ 5 ] );
300             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  7 ], A_Q12_tmp[ 6 ] );
301             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  8 ], A_Q12_tmp[ 7 ] );
302             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  9 ], A_Q12_tmp[ 8 ] );
303             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i - 10 ], A_Q12_tmp[ 9 ] );
304
305             for( j = 10; j < psDec->LPC_order; j++ ) {
306                 LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i - j - 1 ], A_Q12_tmp[ j ] );
307             }
308
309             /* Add prediction to LPC residual */
310             sig_Q10_ptr[ i ] = silk_ADD32( sig_Q10_ptr[ i ], LPC_pred_Q10 );
311
312             /* Update states */
313             psDec->sLPC_Q14[ MAX_LPC_ORDER + i ] = silk_LSHIFT( sig_Q10_ptr[ i ], 4 );
314         }
315         sig_Q10_ptr += psDec->subfr_length;
316         /* Update LPC filter state */
317         silk_memcpy( psDec->sLPC_Q14, &psDec->sLPC_Q14[ psDec->subfr_length ], MAX_LPC_ORDER * sizeof( opus_int32 ) );
318     }
319
320     /* Scale with Gain */
321     for( i = 0; i < psDec->frame_length; i++ ) {
322         frame[ i ] = ( opus_int16 )silk_SAT16( silk_RSHIFT_ROUND( silk_SMULWW( sig_Q10[ i ], psPLC->prevGain_Q16[ psDec->nb_subfr - 1 ] ), 10 ) );
323     }
324
325     /**************************************/
326     /* Update states                      */
327     /**************************************/
328     psPLC->rand_seed     = rand_seed;
329     psPLC->randScale_Q14 = rand_scale_Q14;
330     for( i = 0; i < MAX_NB_SUBFR; i++ ) {
331         psDecCtrl->pitchL[ i ] = lag;
332     }
333 }
334
335 /* Glues concealed frames with new good recieved frames             */
336 void silk_PLC_glue_frames(
337     silk_decoder_state          *psDec,             /* I/O decoder state    */
338     opus_int16                   frame[],            /* I/O signal           */
339     opus_int                     length              /* I length of residual */
340 )
341 {
342     opus_int   i, energy_shift;
343     opus_int32 energy;
344     silk_PLC_struct *psPLC;
345     psPLC = &psDec->sPLC;
346
347     if( psDec->lossCnt ) {
348         /* Calculate energy in concealed residual */
349         silk_sum_sqr_shift( &psPLC->conc_energy, &psPLC->conc_energy_shift, frame, length );
350
351         psPLC->last_frame_lost = 1;
352     } else {
353         if( psDec->sPLC.last_frame_lost ) {
354             /* Calculate residual in decoded signal if last frame was lost */
355             silk_sum_sqr_shift( &energy, &energy_shift, frame, length );
356
357             /* Normalize energies */
358             if( energy_shift > psPLC->conc_energy_shift ) {
359                 psPLC->conc_energy = silk_RSHIFT( psPLC->conc_energy, energy_shift - psPLC->conc_energy_shift );
360             } else if( energy_shift < psPLC->conc_energy_shift ) {
361                 energy = silk_RSHIFT( energy, psPLC->conc_energy_shift - energy_shift );
362             }
363
364             /* Fade in the energy difference */
365             if( energy > psPLC->conc_energy ) {
366                 opus_int32 frac_Q24, LZ;
367                 opus_int32 gain_Q16, slope_Q16;
368
369                 LZ = silk_CLZ32( psPLC->conc_energy );
370                 LZ = LZ - 1;
371                 psPLC->conc_energy = silk_LSHIFT( psPLC->conc_energy, LZ );
372                 energy = silk_RSHIFT( energy, silk_max_32( 24 - LZ, 0 ) );
373
374                 frac_Q24 = silk_DIV32( psPLC->conc_energy, silk_max( energy, 1 ) );
375
376                 gain_Q16 = silk_LSHIFT( silk_SQRT_APPROX( frac_Q24 ), 4 );
377                 slope_Q16 = silk_DIV32_16( ( 1 << 16 ) - gain_Q16, length );
378                 /* Make slope 4x steeper to avoid missing onsets after DTX */
379                 slope_Q16 = silk_LSHIFT( slope_Q16, 2 );
380
381                 for( i = 0; i < length; i++ ) {
382                     frame[ i ] = silk_SMULWB( gain_Q16, frame[ i ] );
383                     gain_Q16 += slope_Q16;
384                     if( gain_Q16 > 1 << 16 ) {
385                         break;
386                     }
387                 }
388             }
389         }
390         psPLC->last_frame_lost = 0;
391     }
392 }