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