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