Initial Skype commit taken from FreeSwitch, which got it from the IETF draft.
[opus.git] / src / SKP_Silk_PLC.c
1 /***********************************************************************\r
2 Copyright (c) 2006-2010, 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 "SKP_Silk_main.h"\r
29 #include "SKP_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 SKP_Silk_PLC_Reset(\r
37     SKP_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 SKP_Silk_PLC(\r
44     SKP_Silk_decoder_state      *psDec,             /* I Decoder state          */\r
45     SKP_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         SKP_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         SKP_Silk_PLC_conceal( psDec, psDecCtrl, signal, length );\r
62     } else {\r
63         /****************************/\r
64         /* Update state             */\r
65         /****************************/\r
66         SKP_Silk_PLC_update( psDec, psDecCtrl, signal, length );\r
67     }\r
68 }\r
69 \r
70 /**************************************************/\r
71 /* Update state of PLC                            */\r
72 /**************************************************/\r
73 void SKP_Silk_PLC_update(\r
74     SKP_Silk_decoder_state      *psDec,             /* (I/O) Decoder state          */\r
75     SKP_Silk_decoder_control    *psDecCtrl,         /* (I/O) Decoder control        */\r
76     SKP_int16                   signal[],\r
77     SKP_int                     length\r
78 )\r
79 {\r
80     SKP_int32 LTP_Gain_Q14, temp_LTP_Gain_Q14;\r
81     SKP_int   i, j;\r
82     SKP_Silk_PLC_struct *psPLC;\r
83 \r
84     psPLC = &psDec->sPLC;\r
85 \r
86     /* Update parameters used in case of packet loss */\r
87     psDec->prev_sigtype = psDecCtrl->sigtype;\r
88     LTP_Gain_Q14 = 0;\r
89     if( psDecCtrl->sigtype == SIG_TYPE_VOICED ) {\r
90         /* Find the parameters for the last subframe which contains a pitch pulse */\r
91         for( j = 0; j * psDec->subfr_length  < psDecCtrl->pitchL[ NB_SUBFR - 1 ]; j++ ) {\r
92             temp_LTP_Gain_Q14 = 0;\r
93             for( i = 0; i < LTP_ORDER; i++ ) {\r
94                 temp_LTP_Gain_Q14 += psDecCtrl->LTPCoef_Q14[ ( NB_SUBFR - 1 - j ) * LTP_ORDER  + i ];\r
95             }\r
96             if( temp_LTP_Gain_Q14 > LTP_Gain_Q14 ) {\r
97                 LTP_Gain_Q14 = temp_LTP_Gain_Q14;\r
98                 SKP_memcpy( psPLC->LTPCoef_Q14,\r
99                     &psDecCtrl->LTPCoef_Q14[ SKP_SMULBB( NB_SUBFR - 1 - j, LTP_ORDER ) ],\r
100                     LTP_ORDER * sizeof( SKP_int16 ) );\r
101 \r
102                 psPLC->pitchL_Q8 = SKP_LSHIFT( psDecCtrl->pitchL[ NB_SUBFR - 1 - j ], 8 );\r
103             }\r
104         }\r
105 \r
106 #if USE_SINGLE_TAP\r
107         SKP_memset( psPLC->LTPCoef_Q14, 0, LTP_ORDER * sizeof( SKP_int16 ) );\r
108         psPLC->LTPCoef_Q14[ LTP_ORDER / 2 ] = LTP_Gain_Q14;\r
109 #endif\r
110 \r
111         /* Limit LT coefs */\r
112         if( LTP_Gain_Q14 < V_PITCH_GAIN_START_MIN_Q14 ) {\r
113             SKP_int   scale_Q10;\r
114             SKP_int32 tmp;\r
115 \r
116             tmp = SKP_LSHIFT( V_PITCH_GAIN_START_MIN_Q14, 10 );\r
117             scale_Q10 = SKP_DIV32( tmp, SKP_max( LTP_Gain_Q14, 1 ) );\r
118             for( i = 0; i < LTP_ORDER; i++ ) {\r
119                 psPLC->LTPCoef_Q14[ i ] = SKP_RSHIFT( SKP_SMULBB( psPLC->LTPCoef_Q14[ i ], scale_Q10 ), 10 );\r
120             }\r
121         } else if( LTP_Gain_Q14 > V_PITCH_GAIN_START_MAX_Q14 ) {\r
122             SKP_int   scale_Q14;\r
123             SKP_int32 tmp;\r
124 \r
125             tmp = SKP_LSHIFT( V_PITCH_GAIN_START_MAX_Q14, 14 );\r
126             scale_Q14 = SKP_DIV32( tmp, SKP_max( LTP_Gain_Q14, 1 ) );\r
127             for( i = 0; i < LTP_ORDER; i++ ) {\r
128                 psPLC->LTPCoef_Q14[ i ] = SKP_RSHIFT( SKP_SMULBB( psPLC->LTPCoef_Q14[ i ], scale_Q14 ), 14 );\r
129             }\r
130         }\r
131     } else {\r
132         psPLC->pitchL_Q8 = SKP_LSHIFT( SKP_SMULBB( psDec->fs_kHz, 18 ), 8 );\r
133         SKP_memset( psPLC->LTPCoef_Q14, 0, LTP_ORDER * sizeof( SKP_int16 ));\r
134     }\r
135 \r
136     /* Save LPC coeficients */\r
137     SKP_memcpy( psPLC->prevLPC_Q12, psDecCtrl->PredCoef_Q12[ 1 ], psDec->LPC_order * sizeof( SKP_int16 ) );\r
138     psPLC->prevLTP_scale_Q14 = psDecCtrl->LTP_scale_Q14;\r
139 \r
140     /* Save Gains */\r
141     SKP_memcpy( psPLC->prevGain_Q16, psDecCtrl->Gains_Q16, NB_SUBFR * sizeof( SKP_int32 ) );\r
142 }\r
143 \r
144 void SKP_Silk_PLC_conceal(\r
145     SKP_Silk_decoder_state      *psDec,             /* I/O Decoder state */\r
146     SKP_Silk_decoder_control    *psDecCtrl,         /* I/O Decoder control */\r
147     SKP_int16                   signal[],           /* O concealed signal */\r
148     SKP_int                     length              /* I length of residual */\r
149 )\r
150 {\r
151     SKP_int   i, j, k;\r
152     SKP_int16 *B_Q14, exc_buf[ MAX_FRAME_LENGTH ], *exc_buf_ptr;\r
153     SKP_int16 rand_scale_Q14, A_Q12_tmp[ MAX_LPC_ORDER ];\r
154     SKP_int32 rand_seed, harm_Gain_Q15, rand_Gain_Q15;\r
155     SKP_int   lag, idx, shift1, shift2;\r
156     SKP_int32 energy1, energy2, *rand_ptr, *pred_lag_ptr, Atmp;\r
157     SKP_int32 sig_Q10[ MAX_FRAME_LENGTH ], *sig_Q10_ptr, LPC_exc_Q10, LPC_pred_Q10,  LTP_pred_Q14;\r
158     SKP_Silk_PLC_struct *psPLC;\r
159 \r
160     psPLC = &psDec->sPLC;\r
161 \r
162     /* Update LTP buffer */\r
163     SKP_memcpy( psDec->sLTP_Q16, &psDec->sLTP_Q16[ psDec->frame_length ], psDec->frame_length * sizeof( SKP_int32 ) );\r
164 \r
165     /* LPC concealment. Apply BWE to previous LPC */\r
166     SKP_Silk_bwexpander( psPLC->prevLPC_Q12, psDec->LPC_order, BWE_COEF_Q16 );\r
167 \r
168     /* Find random noise component */\r
169     /* Scale previous excitation signal */\r
170     exc_buf_ptr = exc_buf;\r
171     for( k = ( NB_SUBFR >> 1 ); k < NB_SUBFR; k++ ) {\r
172         for( i = 0; i < psDec->subfr_length; i++ ) {\r
173             exc_buf_ptr[ i ] = ( SKP_int16 )SKP_RSHIFT( \r
174                 SKP_SMULWW( psDec->exc_Q10[ i + k * psDec->subfr_length ], psPLC->prevGain_Q16[ k ] ), 10 );\r
175         }\r
176         exc_buf_ptr += psDec->subfr_length;\r
177     }\r
178     /* Find the subframe with lowest energy of the last two and use that as random noise generator */ \r
179     SKP_Silk_sum_sqr_shift( &energy1, &shift1, exc_buf,                         psDec->subfr_length );\r
180     SKP_Silk_sum_sqr_shift( &energy2, &shift2, &exc_buf[ psDec->subfr_length ], psDec->subfr_length );\r
181         \r
182     if( SKP_RSHIFT( energy1, shift2 ) < SKP_RSHIFT( energy1, shift2 ) ) {\r
183         /* First sub-frame has lowest energy */\r
184         rand_ptr = &psDec->exc_Q10[ SKP_max_int( 0, 3 * psDec->subfr_length - RAND_BUF_SIZE ) ];\r
185     } else {\r
186         /* Second sub-frame has lowest energy */\r
187         rand_ptr = &psDec->exc_Q10[ SKP_max_int( 0, psDec->frame_length - RAND_BUF_SIZE ) ];\r
188     }\r
189 \r
190     /* Setup Gain to random noise component */ \r
191     B_Q14          = psPLC->LTPCoef_Q14;\r
192     rand_scale_Q14 = psPLC->randScale_Q14;\r
193 \r
194     /* Setup attenuation gains */\r
195     harm_Gain_Q15 = HARM_ATT_Q15[ SKP_min_int( NB_ATT - 1, psDec->lossCnt ) ];\r
196     if( psDec->prev_sigtype == SIG_TYPE_VOICED ) {\r
197         rand_Gain_Q15 = PLC_RAND_ATTENUATE_V_Q15[  SKP_min_int( NB_ATT - 1, psDec->lossCnt ) ];\r
198     } else {\r
199         rand_Gain_Q15 = PLC_RAND_ATTENUATE_UV_Q15[ SKP_min_int( NB_ATT - 1, psDec->lossCnt ) ];\r
200     }\r
201 \r
202     /* First Lost frame */\r
203     if( psDec->lossCnt == 0 ) {\r
204         rand_scale_Q14 = (1 << 14 );\r
205     \r
206         /* Reduce random noise Gain for voiced frames */\r
207         if( psDec->prev_sigtype == SIG_TYPE_VOICED ) {\r
208             for( i = 0; i < LTP_ORDER; i++ ) {\r
209                 rand_scale_Q14 -= B_Q14[ i ];\r
210             }\r
211             rand_scale_Q14 = SKP_max_16( 3277, rand_scale_Q14 ); /* 0.2 */\r
212             rand_scale_Q14 = ( SKP_int16 )SKP_RSHIFT( SKP_SMULBB( rand_scale_Q14, psPLC->prevLTP_scale_Q14 ), 14 );\r
213         }\r
214 \r
215         /* Reduce random noise for unvoiced frames with high LPC gain */\r
216         if( psDec->prev_sigtype == SIG_TYPE_UNVOICED ) {\r
217             SKP_int32 invGain_Q30, down_scale_Q30;\r
218             \r
219             SKP_Silk_LPC_inverse_pred_gain( &invGain_Q30, psPLC->prevLPC_Q12, psDec->LPC_order );\r
220             \r
221             down_scale_Q30 = SKP_min_32( SKP_RSHIFT( ( 1 << 30 ), LOG2_INV_LPC_GAIN_HIGH_THRES ), invGain_Q30 );\r
222             down_scale_Q30 = SKP_max_32( SKP_RSHIFT( ( 1 << 30 ), LOG2_INV_LPC_GAIN_LOW_THRES ), down_scale_Q30 );\r
223             down_scale_Q30 = SKP_LSHIFT( down_scale_Q30, LOG2_INV_LPC_GAIN_HIGH_THRES );\r
224             \r
225             rand_Gain_Q15 = SKP_RSHIFT( SKP_SMULWB( down_scale_Q30, rand_Gain_Q15 ), 14 );\r
226         }\r
227     }\r
228 \r
229     rand_seed           = psPLC->rand_seed;\r
230     lag                 = SKP_RSHIFT_ROUND( psPLC->pitchL_Q8, 8 );\r
231     psDec->sLTP_buf_idx = psDec->frame_length;\r
232 \r
233     /***************************/\r
234     /* LTP synthesis filtering */\r
235     /***************************/\r
236     sig_Q10_ptr = sig_Q10;\r
237     for( k = 0; k < NB_SUBFR; k++ ) {\r
238         /* Setup pointer */\r
239         pred_lag_ptr = &psDec->sLTP_Q16[ psDec->sLTP_buf_idx - lag + LTP_ORDER / 2 ];\r
240         for( i = 0; i < psDec->subfr_length; i++ ) {\r
241             rand_seed = SKP_RAND( rand_seed );\r
242             idx = SKP_RSHIFT( rand_seed, 25 ) & RAND_BUF_MASK;\r
243 \r
244             /* Unrolled loop */\r
245             LTP_pred_Q14 = SKP_SMULWB(               pred_lag_ptr[  0 ], B_Q14[ 0 ] );\r
246             LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -1 ], B_Q14[ 1 ] );\r
247             LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -2 ], B_Q14[ 2 ] );\r
248             LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -3 ], B_Q14[ 3 ] );\r
249             LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -4 ], B_Q14[ 4 ] );\r
250             pred_lag_ptr++;\r
251             \r
252             /* Generate LPC residual */\r
253             LPC_exc_Q10 = SKP_LSHIFT( SKP_SMULWB( rand_ptr[ idx ], rand_scale_Q14 ), 2 ); /* Random noise part */\r
254             LPC_exc_Q10 = SKP_ADD32( LPC_exc_Q10, SKP_RSHIFT_ROUND( LTP_pred_Q14, 4 ) );  /* Harmonic part */\r
255             \r
256             /* Update states */\r
257             psDec->sLTP_Q16[ psDec->sLTP_buf_idx ] = SKP_LSHIFT( LPC_exc_Q10, 6 );\r
258             psDec->sLTP_buf_idx++;\r
259                 \r
260             /* Save LPC residual */\r
261             sig_Q10_ptr[ i ] = LPC_exc_Q10;\r
262         }\r
263         sig_Q10_ptr += psDec->subfr_length;\r
264         /* Gradually reduce LTP gain */\r
265         for( j = 0; j < LTP_ORDER; j++ ) {\r
266             B_Q14[ j ] = SKP_RSHIFT( SKP_SMULBB( harm_Gain_Q15, B_Q14[ j ] ), 15 );\r
267         }\r
268         /* Gradually reduce excitation gain */\r
269         rand_scale_Q14 = SKP_RSHIFT( SKP_SMULBB( rand_scale_Q14, rand_Gain_Q15 ), 15 );\r
270 \r
271         /* Slowly increase pitch lag */\r
272         psPLC->pitchL_Q8 += SKP_SMULWB( psPLC->pitchL_Q8, PITCH_DRIFT_FAC_Q16 );\r
273         psPLC->pitchL_Q8 = SKP_min_32( psPLC->pitchL_Q8, SKP_LSHIFT( SKP_SMULBB( MAX_PITCH_LAG_MS, psDec->fs_kHz ), 8 ) );\r
274         lag = SKP_RSHIFT_ROUND( psPLC->pitchL_Q8, 8 );\r
275     }\r
276 \r
277     /***************************/\r
278     /* LPC synthesis filtering */\r
279     /***************************/\r
280     sig_Q10_ptr = sig_Q10;\r
281     /* Preload LPC coeficients to array on stack. Gives small performance gain */\r
282     SKP_memcpy( A_Q12_tmp, psPLC->prevLPC_Q12, psDec->LPC_order * sizeof( SKP_int16 ) );\r
283     SKP_assert( psDec->LPC_order >= 10 ); /* check that unrolling works */\r
284     for( k = 0; k < NB_SUBFR; k++ ) {\r
285         for( i = 0; i < psDec->subfr_length; i++ ){\r
286             /* unrolled */\r
287             Atmp = *( ( SKP_int32* )&A_Q12_tmp[ 0 ] );    /* read two coefficients at once */\r
288             LPC_pred_Q10 = SKP_SMULWB(               psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  1 ], Atmp );\r
289             LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  2 ], Atmp );\r
290             Atmp = *( ( SKP_int32* )&A_Q12_tmp[ 2 ] );\r
291             LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  3 ], Atmp );\r
292             LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  4 ], Atmp );\r
293             Atmp = *( ( SKP_int32* )&A_Q12_tmp[ 4 ] );\r
294             LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  5 ], Atmp );\r
295             LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  6 ], Atmp );\r
296             Atmp = *( ( SKP_int32* )&A_Q12_tmp[ 6 ] );\r
297             LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  7 ], Atmp );\r
298             LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  8 ], Atmp );\r
299             Atmp = *( ( SKP_int32* )&A_Q12_tmp[ 8 ] );\r
300             LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  9 ], Atmp );\r
301             LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i - 10 ], Atmp );\r
302             for( j = 10 ; j < psDec->LPC_order ; j+=2 ) {\r
303                 Atmp = *( ( SKP_int32* )&A_Q12_tmp[ j ] );\r
304                 LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  1 - j ], Atmp );\r
305                 LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  2 - j ], Atmp );\r
306             }\r
307 \r
308             /* Add prediction to LPC residual */\r
309             sig_Q10_ptr[ i ] = SKP_ADD32( sig_Q10_ptr[ i ], LPC_pred_Q10 );\r
310                 \r
311             /* Update states */\r
312             psDec->sLPC_Q14[ MAX_LPC_ORDER + i ] = SKP_LSHIFT( sig_Q10_ptr[ i ], 4 );\r
313         }\r
314         sig_Q10_ptr += psDec->subfr_length;\r
315         /* Update LPC filter state */\r
316         SKP_memcpy( psDec->sLPC_Q14, &psDec->sLPC_Q14[ psDec->subfr_length ], MAX_LPC_ORDER * sizeof( SKP_int32 ) );\r
317     }\r
318 \r
319     /* Scale with Gain */\r
320     for( i = 0; i < psDec->frame_length; i++ ) {\r
321         signal[ i ] = ( SKP_int16 )SKP_SAT16( SKP_RSHIFT_ROUND( SKP_SMULWW( sig_Q10[ i ], psPLC->prevGain_Q16[ NB_SUBFR - 1 ] ), 10 ) );\r
322     }\r
323 \r
324     /**************************************/\r
325     /* Update states                      */\r
326     /**************************************/\r
327     psPLC->rand_seed     = rand_seed;\r
328     psPLC->randScale_Q14 = rand_scale_Q14;\r
329     for( i = 0; i < NB_SUBFR; i++ ) {\r
330         psDecCtrl->pitchL[ i ] = lag;\r
331     }\r
332 }\r
333 \r
334 /* Glues concealed frames with new good recieved frames             */\r
335 void SKP_Silk_PLC_glue_frames(\r
336     SKP_Silk_decoder_state      *psDec,             /* I/O decoder state    */\r
337     SKP_Silk_decoder_control    *psDecCtrl,         /* I/O Decoder control  */\r
338     SKP_int16                   signal[],           /* I/O signal           */\r
339     SKP_int                     length              /* I length of residual */\r
340 )\r
341 {\r
342     SKP_int   i, energy_shift;\r
343     SKP_int32 energy;\r
344     SKP_Silk_PLC_struct *psPLC;\r
345     psPLC = &psDec->sPLC;\r
346 \r
347     if( psDec->lossCnt ) {\r
348         /* Calculate energy in concealed residual */\r
349         SKP_Silk_sum_sqr_shift( &psPLC->conc_energy, &psPLC->conc_energy_shift, signal, length );\r
350         \r
351         psPLC->last_frame_lost = 1;\r
352     } else {\r
353         if( psDec->sPLC.last_frame_lost ) {\r
354             /* Calculate residual in decoded signal if last frame was lost */\r
355             SKP_Silk_sum_sqr_shift( &energy, &energy_shift, signal, length );\r
356 \r
357             /* Normalize energies */\r
358             if( energy_shift > psPLC->conc_energy_shift ) {\r
359                 psPLC->conc_energy = SKP_RSHIFT( psPLC->conc_energy, energy_shift - psPLC->conc_energy_shift );\r
360             } else if( energy_shift < psPLC->conc_energy_shift ) {\r
361                 energy = SKP_RSHIFT( energy, psPLC->conc_energy_shift - energy_shift );\r
362             }\r
363 \r
364             /* Fade in the energy difference */\r
365             if( energy > psPLC->conc_energy ) {\r
366                 SKP_int32 frac_Q24, LZ;\r
367                 SKP_int32 gain_Q12, slope_Q12;\r
368 \r
369                 LZ = SKP_Silk_CLZ32( psPLC->conc_energy );\r
370                 LZ = LZ - 1;\r
371                 psPLC->conc_energy = SKP_LSHIFT( psPLC->conc_energy, LZ );\r
372                 energy = SKP_RSHIFT( energy, SKP_max_32( 24 - LZ, 0 ) );\r
373                 \r
374                 frac_Q24 = SKP_DIV32( psPLC->conc_energy, SKP_max( energy, 1 ) );\r
375                 \r
376                 gain_Q12 = SKP_Silk_SQRT_APPROX( frac_Q24 );\r
377                 slope_Q12 = SKP_DIV32_16( ( 1 << 12 ) - gain_Q12, length );\r
378 \r
379                 for( i = 0; i < length; i++ ) {\r
380                     signal[ i ] = SKP_RSHIFT( SKP_MUL( gain_Q12, signal[ i ] ), 12 );\r
381                     gain_Q12 += slope_Q12;\r
382                     gain_Q12 = SKP_min( gain_Q12, ( 1 << 12 ) );\r
383                 }\r
384             }\r
385         }\r
386         psPLC->last_frame_lost = 0;\r
387 \r
388     }\r
389 }\r