SILK update
[opus.git] / src_common / 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 \r
63         psDec->lossCnt++;\r
64     } else {\r
65         /****************************/\r
66         /* Update state             */\r
67         /****************************/\r
68         SKP_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 SKP_Silk_PLC_update(\r
76     SKP_Silk_decoder_state      *psDec,             /* (I/O) Decoder state          */\r
77     SKP_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     SKP_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->prev_sigtype = psDecCtrl->sigtype;\r
90     LTP_Gain_Q14 = 0;\r
91     if( psDecCtrl->sigtype == SIG_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 SKP_Silk_PLC_conceal(\r
150     SKP_Silk_decoder_state      *psDec,             /* I/O Decoder state */\r
151     SKP_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     SKP_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     SKP_Silk_bwexpander( psPLC->prevLPC_Q12, psDec->LPC_order, BWE_COEF_Q16 );\r
171 \r
172     /* Find random noise component */\r
173     /* Scale previous excitation signal */\r
174     exc_buf_ptr = exc_buf;\r
175     for( k = ( psDec->nb_subfr >> 1 ); k < psDec->nb_subfr; k++ ) {\r
176         for( i = 0; i < psDec->subfr_length; i++ ) {\r
177             exc_buf_ptr[ i ] = ( SKP_int16 )SKP_RSHIFT( \r
178                 SKP_SMULWW( psDec->exc_Q10[ i + k * psDec->subfr_length ], psPLC->prevGain_Q16[ k ] ), 10 );\r
179         }\r
180         exc_buf_ptr += psDec->subfr_length;\r
181     }\r
182     /* Find the subframe with lowest energy of the last two and use that as random noise generator */ \r
183     SKP_Silk_sum_sqr_shift( &energy1, &shift1, exc_buf,                         psDec->subfr_length );\r
184     SKP_Silk_sum_sqr_shift( &energy2, &shift2, &exc_buf[ psDec->subfr_length ], psDec->subfr_length );\r
185         \r
186     if( SKP_RSHIFT( energy1, shift2 ) < SKP_RSHIFT( energy2, shift1 ) ) {\r
187         /* First sub-frame has lowest energy */\r
188         rand_ptr = &psDec->exc_Q10[ SKP_max_int( 0, 3 * psDec->subfr_length - RAND_BUF_SIZE ) ];\r
189     } else {\r
190         /* Second sub-frame has lowest energy */\r
191         rand_ptr = &psDec->exc_Q10[ SKP_max_int( 0, psDec->frame_length - RAND_BUF_SIZE ) ];\r
192     }\r
193 \r
194     /* Setup Gain to random noise component */ \r
195     B_Q14          = psPLC->LTPCoef_Q14;\r
196     rand_scale_Q14 = psPLC->randScale_Q14;\r
197 \r
198     /* Setup attenuation gains */\r
199     harm_Gain_Q15 = HARM_ATT_Q15[ SKP_min_int( NB_ATT - 1, psDec->lossCnt ) ];\r
200     if( psDec->prev_sigtype == SIG_TYPE_VOICED ) {\r
201         rand_Gain_Q15 = PLC_RAND_ATTENUATE_V_Q15[  SKP_min_int( NB_ATT - 1, psDec->lossCnt ) ];\r
202     } else {\r
203         rand_Gain_Q15 = PLC_RAND_ATTENUATE_UV_Q15[ SKP_min_int( NB_ATT - 1, psDec->lossCnt ) ];\r
204     }\r
205 \r
206     /* First Lost frame */\r
207     if( psDec->lossCnt == 0 ) {\r
208         rand_scale_Q14 = (1 << 14 );\r
209     \r
210         /* Reduce random noise Gain for voiced frames */\r
211         if( psDec->prev_sigtype == SIG_TYPE_VOICED ) {\r
212             for( i = 0; i < LTP_ORDER; i++ ) {\r
213                 rand_scale_Q14 -= B_Q14[ i ];\r
214             }\r
215             rand_scale_Q14 = SKP_max_16( 3277, rand_scale_Q14 ); /* 0.2 */\r
216             rand_scale_Q14 = ( SKP_int16 )SKP_RSHIFT( SKP_SMULBB( rand_scale_Q14, psPLC->prevLTP_scale_Q14 ), 14 );\r
217         }\r
218 \r
219         /* Reduce random noise for unvoiced frames with high LPC gain */\r
220         if( psDec->prev_sigtype == SIG_TYPE_UNVOICED ) {\r
221             SKP_int32 invGain_Q30, down_scale_Q30;\r
222             \r
223             SKP_Silk_LPC_inverse_pred_gain( &invGain_Q30, psPLC->prevLPC_Q12, psDec->LPC_order );\r
224             \r
225             down_scale_Q30 = SKP_min_32( SKP_RSHIFT( ( 1 << 30 ), LOG2_INV_LPC_GAIN_HIGH_THRES ), invGain_Q30 );\r
226             down_scale_Q30 = SKP_max_32( SKP_RSHIFT( ( 1 << 30 ), LOG2_INV_LPC_GAIN_LOW_THRES ), down_scale_Q30 );\r
227             down_scale_Q30 = SKP_LSHIFT( down_scale_Q30, LOG2_INV_LPC_GAIN_HIGH_THRES );\r
228             \r
229             rand_Gain_Q15 = SKP_RSHIFT( SKP_SMULWB( down_scale_Q30, rand_Gain_Q15 ), 14 );\r
230         }\r
231     }\r
232 \r
233     rand_seed    = psPLC->rand_seed;\r
234     lag          = SKP_RSHIFT_ROUND( psPLC->pitchL_Q8, 8 );\r
235     sLTP_buf_idx = psDec->ltp_mem_length;\r
236 \r
237     /***************************/\r
238     /* LTP synthesis filtering */\r
239     /***************************/\r
240     sig_Q10_ptr = sig_Q10;\r
241     for( k = 0; k < psDec->nb_subfr; k++ ) {\r
242         /* Setup pointer */\r
243         pred_lag_ptr = &psDec->sLTP_Q16[ sLTP_buf_idx - lag + LTP_ORDER / 2 ];\r
244         for( i = 0; i < psDec->subfr_length; i++ ) {\r
245             rand_seed = SKP_RAND( rand_seed );\r
246             idx = SKP_RSHIFT( rand_seed, 25 ) & RAND_BUF_MASK;\r
247 \r
248             /* Unrolled loop */\r
249             LTP_pred_Q14 = SKP_SMULWB(               pred_lag_ptr[  0 ], B_Q14[ 0 ] );\r
250             LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -1 ], B_Q14[ 1 ] );\r
251             LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -2 ], B_Q14[ 2 ] );\r
252             LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -3 ], B_Q14[ 3 ] );\r
253             LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -4 ], B_Q14[ 4 ] );\r
254             pred_lag_ptr++;\r
255             \r
256             /* Generate LPC residual */\r
257             LPC_exc_Q10 = SKP_LSHIFT( SKP_SMULWB( rand_ptr[ idx ], rand_scale_Q14 ), 2 ); /* Random noise part */\r
258             LPC_exc_Q10 = SKP_ADD32( LPC_exc_Q10, SKP_RSHIFT_ROUND( LTP_pred_Q14, 4 ) );  /* Harmonic part */\r
259             \r
260             /* Update states */\r
261             psDec->sLTP_Q16[ sLTP_buf_idx ] = SKP_LSHIFT( LPC_exc_Q10, 6 );\r
262             sLTP_buf_idx++;\r
263                 \r
264             /* Save LPC residual */\r
265             sig_Q10_ptr[ i ] = LPC_exc_Q10;\r
266         }\r
267         sig_Q10_ptr += psDec->subfr_length;\r
268         /* Gradually reduce LTP gain */\r
269         for( j = 0; j < LTP_ORDER; j++ ) {\r
270             B_Q14[ j ] = SKP_RSHIFT( SKP_SMULBB( harm_Gain_Q15, B_Q14[ j ] ), 15 );\r
271         }\r
272         /* Gradually reduce excitation gain */\r
273         rand_scale_Q14 = SKP_RSHIFT( SKP_SMULBB( rand_scale_Q14, rand_Gain_Q15 ), 15 );\r
274 \r
275         /* Slowly increase pitch lag */\r
276         psPLC->pitchL_Q8 += SKP_SMULWB( psPLC->pitchL_Q8, PITCH_DRIFT_FAC_Q16 );\r
277         psPLC->pitchL_Q8 = SKP_min_32( psPLC->pitchL_Q8, SKP_LSHIFT( SKP_SMULBB( MAX_PITCH_LAG_MS, psDec->fs_kHz ), 8 ) );\r
278         lag = SKP_RSHIFT_ROUND( psPLC->pitchL_Q8, 8 );\r
279     }\r
280 \r
281     /***************************/\r
282     /* LPC synthesis filtering */\r
283     /***************************/\r
284     sig_Q10_ptr = sig_Q10;\r
285     /* Preload LPC coeficients to array on stack. Gives small performance gain */\r
286     SKP_memcpy( A_Q12_tmp, psPLC->prevLPC_Q12, psDec->LPC_order * sizeof( SKP_int16 ) );\r
287     SKP_assert( psDec->LPC_order >= 10 ); /* check that unrolling works */\r
288     for( k = 0; k < psDec->nb_subfr; k++ ) {\r
289         for( i = 0; i < psDec->subfr_length; i++ ){\r
290             /* partly unrolled */\r
291             LPC_pred_Q10 = SKP_SMULWB(               psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  1 ], A_Q12_tmp[ 0 ] );\r
292             LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  2 ], A_Q12_tmp[ 1 ] );\r
293             LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  3 ], A_Q12_tmp[ 2 ] );\r
294             LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  4 ], A_Q12_tmp[ 3 ] );\r
295             LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  5 ], A_Q12_tmp[ 4 ] );\r
296             LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  6 ], A_Q12_tmp[ 5 ] );\r
297             LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  7 ], A_Q12_tmp[ 6 ] );\r
298             LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  8 ], A_Q12_tmp[ 7 ] );\r
299             LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  9 ], A_Q12_tmp[ 8 ] );\r
300             LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i - 10 ], A_Q12_tmp[ 9 ] );\r
301 \r
302             for( j = 10; j < psDec->LPC_order; j++ ) {\r
303                 LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i - j - 1 ], A_Q12_tmp[ j ] );\r
304             }\r
305 \r
306             /* Add prediction to LPC residual */\r
307             sig_Q10_ptr[ i ] = SKP_ADD32( sig_Q10_ptr[ i ], LPC_pred_Q10 );\r
308                 \r
309             /* Update states */\r
310             psDec->sLPC_Q14[ MAX_LPC_ORDER + i ] = SKP_LSHIFT( sig_Q10_ptr[ i ], 4 );\r
311         }\r
312         sig_Q10_ptr += psDec->subfr_length;\r
313         /* Update LPC filter state */\r
314         SKP_memcpy( psDec->sLPC_Q14, &psDec->sLPC_Q14[ psDec->subfr_length ], MAX_LPC_ORDER * sizeof( SKP_int32 ) );\r
315     }\r
316 \r
317     /* Scale with Gain */\r
318     for( i = 0; i < psDec->frame_length; i++ ) {\r
319         signal[ i ] = ( SKP_int16 )SKP_SAT16( SKP_RSHIFT_ROUND( SKP_SMULWW( sig_Q10[ i ], psPLC->prevGain_Q16[ psDec->nb_subfr - 1 ] ), 10 ) );\r
320     }\r
321 \r
322     /**************************************/\r
323     /* Update states                      */\r
324     /**************************************/\r
325     psPLC->rand_seed     = rand_seed;\r
326     psPLC->randScale_Q14 = rand_scale_Q14;\r
327     for( i = 0; i < MAX_NB_SUBFR; i++ ) {\r
328         psDecCtrl->pitchL[ i ] = lag;\r
329     }\r
330 }\r
331 \r
332 /* Glues concealed frames with new good recieved frames             */\r
333 void SKP_Silk_PLC_glue_frames(\r
334     SKP_Silk_decoder_state      *psDec,             /* I/O decoder state    */\r
335     SKP_Silk_decoder_control    *psDecCtrl,         /* I/O Decoder control  */\r
336     SKP_int16                   signal[],           /* I/O signal           */\r
337     SKP_int                     length              /* I length of residual */\r
338 )\r
339 {\r
340     SKP_int   i, energy_shift;\r
341     SKP_int32 energy;\r
342     SKP_Silk_PLC_struct *psPLC;\r
343     psPLC = &psDec->sPLC;\r
344 \r
345     if( psDec->lossCnt ) {\r
346         /* Calculate energy in concealed residual */\r
347         SKP_Silk_sum_sqr_shift( &psPLC->conc_energy, &psPLC->conc_energy_shift, signal, length );\r
348         \r
349         psPLC->last_frame_lost = 1;\r
350     } else {\r
351         if( psDec->sPLC.last_frame_lost ) {\r
352             /* Calculate residual in decoded signal if last frame was lost */\r
353             SKP_Silk_sum_sqr_shift( &energy, &energy_shift, signal, length );\r
354 \r
355             /* Normalize energies */\r
356             if( energy_shift > psPLC->conc_energy_shift ) {\r
357                 psPLC->conc_energy = SKP_RSHIFT( psPLC->conc_energy, energy_shift - psPLC->conc_energy_shift );\r
358             } else if( energy_shift < psPLC->conc_energy_shift ) {\r
359                 energy = SKP_RSHIFT( energy, psPLC->conc_energy_shift - energy_shift );\r
360             }\r
361 \r
362             /* Fade in the energy difference */\r
363             if( energy > psPLC->conc_energy ) {\r
364                 SKP_int32 frac_Q24, LZ;\r
365                 SKP_int32 gain_Q12, slope_Q12;\r
366 \r
367                 LZ = SKP_Silk_CLZ32( psPLC->conc_energy );\r
368                 LZ = LZ - 1;\r
369                 psPLC->conc_energy = SKP_LSHIFT( psPLC->conc_energy, LZ );\r
370                 energy = SKP_RSHIFT( energy, SKP_max_32( 24 - LZ, 0 ) );\r
371                 \r
372                 frac_Q24 = SKP_DIV32( psPLC->conc_energy, SKP_max( energy, 1 ) );\r
373                 \r
374                 gain_Q12 = SKP_Silk_SQRT_APPROX( frac_Q24 );\r
375                 slope_Q12 = SKP_DIV32_16( ( 1 << 12 ) - gain_Q12, length );\r
376 \r
377                 for( i = 0; i < length; i++ ) {\r
378                     signal[ i ] = SKP_RSHIFT( SKP_MUL( gain_Q12, signal[ i ] ), 12 );\r
379                     gain_Q12 += slope_Q12;\r
380                     gain_Q12 = SKP_min( gain_Q12, ( 1 << 12 ) );\r
381                 }\r
382             }\r
383         }\r
384         psPLC->last_frame_lost = 0;\r
385 \r
386     }\r
387 }\r