Minor refactoring of the SILK PLC code to save more stack
[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_energy(opus_int32 *energy1, opus_int *shift1, opus_int32 *energy2, opus_int *shift2,
169       const opus_int32 *exc_Q14, const opus_int32 *prevGain_Q10, int subfr_length, int nb_subfr)
170 {
171     int i, k;
172     VARDECL( opus_int16, exc_buf );
173     opus_int16 *exc_buf_ptr;
174     SAVE_STACK;
175     ALLOC( exc_buf, 2*subfr_length, opus_int16 );
176     /* Find random noise component */
177     /* Scale previous excitation signal */
178     exc_buf_ptr = exc_buf;
179     for( k = 0; k < 2; k++ ) {
180         for( i = 0; i < subfr_length; i++ ) {
181             exc_buf_ptr[ i ] = (opus_int16)silk_SAT16( silk_RSHIFT(
182                 silk_SMULWW( exc_Q14[ i + ( k + nb_subfr - 2 ) * subfr_length ], prevGain_Q10[ k ] ), 8 ) );
183         }
184         exc_buf_ptr += subfr_length;
185     }
186     /* Find the subframe with lowest energy of the last two and use that as random noise generator */
187     silk_sum_sqr_shift( energy1, shift1, exc_buf,                  subfr_length );
188     silk_sum_sqr_shift( energy2, shift2, &exc_buf[ subfr_length ], subfr_length );
189     RESTORE_STACK;
190 }
191
192 static OPUS_INLINE void silk_PLC_conceal(
193     silk_decoder_state                  *psDec,             /* I/O Decoder state        */
194     silk_decoder_control                *psDecCtrl,         /* I/O Decoder control      */
195     opus_int16                          frame[]             /* O LPC residual signal    */
196 )
197 {
198     opus_int   i, j, k;
199     opus_int   lag, idx, sLTP_buf_idx, shift1, shift2;
200     opus_int32 rand_seed, harm_Gain_Q15, rand_Gain_Q15, inv_gain_Q30;
201     opus_int32 energy1, energy2, *rand_ptr, *pred_lag_ptr;
202     opus_int32 LPC_pred_Q10, LTP_pred_Q12;
203     opus_int16 rand_scale_Q14;
204     opus_int16 *B_Q14;
205     opus_int32 *sLPC_Q14_ptr;
206     opus_int16 A_Q12[ MAX_LPC_ORDER ];
207 #ifdef SMALL_FOOTPRINT
208     opus_int16 *sLTP;
209 #else
210     VARDECL( opus_int16, sLTP );
211 #endif
212     VARDECL( opus_int32, sLTP_Q14 );
213     silk_PLC_struct *psPLC = &psDec->sPLC;
214     opus_int32 prevGain_Q10[2];
215     SAVE_STACK;
216
217     ALLOC( sLTP_Q14, psDec->ltp_mem_length + psDec->frame_length, opus_int32 );
218 #ifdef SMALL_FOOTPRINT
219     /* Ugly hack that breaks aliasing rules to save stack: put sLTP at the very end of sLTP_Q14. */
220     sLTP = ((opus_int16*)&sLTP_Q14[psDec->ltp_mem_length + psDec->frame_length])-psDec->ltp_mem_length;
221 #else
222     ALLOC( sLTP, psDec->ltp_mem_length, opus_int16 );
223 #endif
224
225     prevGain_Q10[0] = silk_RSHIFT( psPLC->prevGain_Q16[ 0 ], 6);
226     prevGain_Q10[1] = silk_RSHIFT( psPLC->prevGain_Q16[ 1 ], 6);
227
228     if( psDec->first_frame_after_reset ) {
229        silk_memset( psPLC->prevLPC_Q12, 0, sizeof( psPLC->prevLPC_Q12 ) );
230     }
231
232     silk_PLC_energy(&energy1, &shift1, &energy2, &shift2, psDec->exc_Q14, prevGain_Q10, psDec->subfr_length, psDec->nb_subfr);
233
234     if( silk_RSHIFT( energy1, shift2 ) < silk_RSHIFT( energy2, shift1 ) ) {
235         /* First sub-frame has lowest energy */
236         rand_ptr = &psDec->exc_Q14[ silk_max_int( 0, ( psPLC->nb_subfr - 1 ) * psPLC->subfr_length - RAND_BUF_SIZE ) ];
237     } else {
238         /* Second sub-frame has lowest energy */
239         rand_ptr = &psDec->exc_Q14[ silk_max_int( 0, psPLC->nb_subfr * psPLC->subfr_length - RAND_BUF_SIZE ) ];
240     }
241
242     /* Set up Gain to random noise component */
243     B_Q14          = psPLC->LTPCoef_Q14;
244     rand_scale_Q14 = psPLC->randScale_Q14;
245
246     /* Set up attenuation gains */
247     harm_Gain_Q15 = HARM_ATT_Q15[ silk_min_int( NB_ATT - 1, psDec->lossCnt ) ];
248     if( psDec->prevSignalType == TYPE_VOICED ) {
249         rand_Gain_Q15 = PLC_RAND_ATTENUATE_V_Q15[  silk_min_int( NB_ATT - 1, psDec->lossCnt ) ];
250     } else {
251         rand_Gain_Q15 = PLC_RAND_ATTENUATE_UV_Q15[ silk_min_int( NB_ATT - 1, psDec->lossCnt ) ];
252     }
253
254     /* LPC concealment. Apply BWE to previous LPC */
255     silk_bwexpander( psPLC->prevLPC_Q12, psDec->LPC_order, SILK_FIX_CONST( BWE_COEF, 16 ) );
256
257     /* Preload LPC coeficients to array on stack. Gives small performance gain */
258     silk_memcpy( A_Q12, psPLC->prevLPC_Q12, psDec->LPC_order * sizeof( opus_int16 ) );
259
260     /* First Lost frame */
261     if( psDec->lossCnt == 0 ) {
262         rand_scale_Q14 = 1 << 14;
263
264         /* Reduce random noise Gain for voiced frames */
265         if( psDec->prevSignalType == TYPE_VOICED ) {
266             for( i = 0; i < LTP_ORDER; i++ ) {
267                 rand_scale_Q14 -= B_Q14[ i ];
268             }
269             rand_scale_Q14 = silk_max_16( 3277, rand_scale_Q14 ); /* 0.2 */
270             rand_scale_Q14 = (opus_int16)silk_RSHIFT( silk_SMULBB( rand_scale_Q14, psPLC->prevLTP_scale_Q14 ), 14 );
271         } else {
272             /* Reduce random noise for unvoiced frames with high LPC gain */
273             opus_int32 invGain_Q30, down_scale_Q30;
274
275             invGain_Q30 = silk_LPC_inverse_pred_gain( psPLC->prevLPC_Q12, psDec->LPC_order );
276
277             down_scale_Q30 = silk_min_32( silk_RSHIFT( (opus_int32)1 << 30, LOG2_INV_LPC_GAIN_HIGH_THRES ), invGain_Q30 );
278             down_scale_Q30 = silk_max_32( silk_RSHIFT( (opus_int32)1 << 30, LOG2_INV_LPC_GAIN_LOW_THRES ), down_scale_Q30 );
279             down_scale_Q30 = silk_LSHIFT( down_scale_Q30, LOG2_INV_LPC_GAIN_HIGH_THRES );
280
281             rand_Gain_Q15 = silk_RSHIFT( silk_SMULWB( down_scale_Q30, rand_Gain_Q15 ), 14 );
282         }
283     }
284
285     rand_seed    = psPLC->rand_seed;
286     lag          = silk_RSHIFT_ROUND( psPLC->pitchL_Q8, 8 );
287     sLTP_buf_idx = psDec->ltp_mem_length;
288
289     /* Rewhiten LTP state */
290     idx = psDec->ltp_mem_length - lag - psDec->LPC_order - LTP_ORDER / 2;
291     silk_assert( idx > 0 );
292     silk_LPC_analysis_filter( &sLTP[ idx ], &psDec->outBuf[ idx ], A_Q12, psDec->ltp_mem_length - idx, psDec->LPC_order );
293     /* Scale LTP state */
294     inv_gain_Q30 = silk_INVERSE32_varQ( psPLC->prevGain_Q16[ 1 ], 46 );
295     inv_gain_Q30 = silk_min( inv_gain_Q30, silk_int32_MAX >> 1 );
296     for( i = idx + psDec->LPC_order; i < psDec->ltp_mem_length; i++ ) {
297         sLTP_Q14[ i ] = silk_SMULWB( inv_gain_Q30, sLTP[ i ] );
298     }
299
300     /***************************/
301     /* LTP synthesis filtering */
302     /***************************/
303     for( k = 0; k < psDec->nb_subfr; k++ ) {
304         /* Set up pointer */
305         pred_lag_ptr = &sLTP_Q14[ sLTP_buf_idx - lag + LTP_ORDER / 2 ];
306         for( i = 0; i < psDec->subfr_length; i++ ) {
307             /* Unrolled loop */
308             /* Avoids introducing a bias because silk_SMLAWB() always rounds to -inf */
309             LTP_pred_Q12 = 2;
310             LTP_pred_Q12 = silk_SMLAWB( LTP_pred_Q12, pred_lag_ptr[  0 ], B_Q14[ 0 ] );
311             LTP_pred_Q12 = silk_SMLAWB( LTP_pred_Q12, pred_lag_ptr[ -1 ], B_Q14[ 1 ] );
312             LTP_pred_Q12 = silk_SMLAWB( LTP_pred_Q12, pred_lag_ptr[ -2 ], B_Q14[ 2 ] );
313             LTP_pred_Q12 = silk_SMLAWB( LTP_pred_Q12, pred_lag_ptr[ -3 ], B_Q14[ 3 ] );
314             LTP_pred_Q12 = silk_SMLAWB( LTP_pred_Q12, pred_lag_ptr[ -4 ], B_Q14[ 4 ] );
315             pred_lag_ptr++;
316
317             /* Generate LPC excitation */
318             rand_seed = silk_RAND( rand_seed );
319             idx = silk_RSHIFT( rand_seed, 25 ) & RAND_BUF_MASK;
320             sLTP_Q14[ sLTP_buf_idx ] = silk_LSHIFT32( silk_SMLAWB( LTP_pred_Q12, rand_ptr[ idx ], rand_scale_Q14 ), 2 );
321             sLTP_buf_idx++;
322         }
323
324         /* Gradually reduce LTP gain */
325         for( j = 0; j < LTP_ORDER; j++ ) {
326             B_Q14[ j ] = silk_RSHIFT( silk_SMULBB( harm_Gain_Q15, B_Q14[ j ] ), 15 );
327         }
328         /* Gradually reduce excitation gain */
329         rand_scale_Q14 = silk_RSHIFT( silk_SMULBB( rand_scale_Q14, rand_Gain_Q15 ), 15 );
330
331         /* Slowly increase pitch lag */
332         psPLC->pitchL_Q8 = silk_SMLAWB( psPLC->pitchL_Q8, psPLC->pitchL_Q8, PITCH_DRIFT_FAC_Q16 );
333         psPLC->pitchL_Q8 = silk_min_32( psPLC->pitchL_Q8, silk_LSHIFT( silk_SMULBB( MAX_PITCH_LAG_MS, psDec->fs_kHz ), 8 ) );
334         lag = silk_RSHIFT_ROUND( psPLC->pitchL_Q8, 8 );
335     }
336
337     /***************************/
338     /* LPC synthesis filtering */
339     /***************************/
340     sLPC_Q14_ptr = &sLTP_Q14[ psDec->ltp_mem_length - MAX_LPC_ORDER ];
341
342     /* Copy LPC state */
343     silk_memcpy( sLPC_Q14_ptr, psDec->sLPC_Q14_buf, MAX_LPC_ORDER * sizeof( opus_int32 ) );
344
345     silk_assert( psDec->LPC_order >= 10 ); /* check that unrolling works */
346     for( i = 0; i < psDec->frame_length; i++ ) {
347         /* partly unrolled */
348         /* Avoids introducing a bias because silk_SMLAWB() always rounds to -inf */
349         LPC_pred_Q10 = silk_RSHIFT( psDec->LPC_order, 1 );
350         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, sLPC_Q14_ptr[ MAX_LPC_ORDER + i -  1 ], A_Q12[ 0 ] );
351         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, sLPC_Q14_ptr[ MAX_LPC_ORDER + i -  2 ], A_Q12[ 1 ] );
352         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, sLPC_Q14_ptr[ MAX_LPC_ORDER + i -  3 ], A_Q12[ 2 ] );
353         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, sLPC_Q14_ptr[ MAX_LPC_ORDER + i -  4 ], A_Q12[ 3 ] );
354         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, sLPC_Q14_ptr[ MAX_LPC_ORDER + i -  5 ], A_Q12[ 4 ] );
355         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, sLPC_Q14_ptr[ MAX_LPC_ORDER + i -  6 ], A_Q12[ 5 ] );
356         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, sLPC_Q14_ptr[ MAX_LPC_ORDER + i -  7 ], A_Q12[ 6 ] );
357         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, sLPC_Q14_ptr[ MAX_LPC_ORDER + i -  8 ], A_Q12[ 7 ] );
358         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, sLPC_Q14_ptr[ MAX_LPC_ORDER + i -  9 ], A_Q12[ 8 ] );
359         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, sLPC_Q14_ptr[ MAX_LPC_ORDER + i - 10 ], A_Q12[ 9 ] );
360         for( j = 10; j < psDec->LPC_order; j++ ) {
361             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, sLPC_Q14_ptr[ MAX_LPC_ORDER + i - j - 1 ], A_Q12[ j ] );
362         }
363
364         /* Add prediction to LPC excitation */
365         sLPC_Q14_ptr[ MAX_LPC_ORDER + i ] = silk_ADD_LSHIFT32( sLPC_Q14_ptr[ MAX_LPC_ORDER + i ], LPC_pred_Q10, 4 );
366
367         /* Scale with Gain */
368         frame[ i ] = (opus_int16)silk_SAT16( silk_SAT16( silk_RSHIFT_ROUND( silk_SMULWW( sLPC_Q14_ptr[ MAX_LPC_ORDER + i ], prevGain_Q10[ 1 ] ), 8 ) ) );
369     }
370
371     /* Save LPC state */
372     silk_memcpy( psDec->sLPC_Q14_buf, &sLPC_Q14_ptr[ psDec->frame_length ], MAX_LPC_ORDER * sizeof( opus_int32 ) );
373
374     /**************************************/
375     /* Update states                      */
376     /**************************************/
377     psPLC->rand_seed     = rand_seed;
378     psPLC->randScale_Q14 = rand_scale_Q14;
379     for( i = 0; i < MAX_NB_SUBFR; i++ ) {
380         psDecCtrl->pitchL[ i ] = lag;
381     }
382     RESTORE_STACK;
383 }
384
385 /* Glues concealed frames with new good received frames */
386 void silk_PLC_glue_frames(
387     silk_decoder_state                  *psDec,             /* I/O decoder state        */
388     opus_int16                          frame[],            /* I/O signal               */
389     opus_int                            length              /* I length of signal       */
390 )
391 {
392     opus_int   i, energy_shift;
393     opus_int32 energy;
394     silk_PLC_struct *psPLC;
395     psPLC = &psDec->sPLC;
396
397     if( psDec->lossCnt ) {
398         /* Calculate energy in concealed residual */
399         silk_sum_sqr_shift( &psPLC->conc_energy, &psPLC->conc_energy_shift, frame, length );
400
401         psPLC->last_frame_lost = 1;
402     } else {
403         if( psDec->sPLC.last_frame_lost ) {
404             /* Calculate residual in decoded signal if last frame was lost */
405             silk_sum_sqr_shift( &energy, &energy_shift, frame, length );
406
407             /* Normalize energies */
408             if( energy_shift > psPLC->conc_energy_shift ) {
409                 psPLC->conc_energy = silk_RSHIFT( psPLC->conc_energy, energy_shift - psPLC->conc_energy_shift );
410             } else if( energy_shift < psPLC->conc_energy_shift ) {
411                 energy = silk_RSHIFT( energy, psPLC->conc_energy_shift - energy_shift );
412             }
413
414             /* Fade in the energy difference */
415             if( energy > psPLC->conc_energy ) {
416                 opus_int32 frac_Q24, LZ;
417                 opus_int32 gain_Q16, slope_Q16;
418
419                 LZ = silk_CLZ32( psPLC->conc_energy );
420                 LZ = LZ - 1;
421                 psPLC->conc_energy = silk_LSHIFT( psPLC->conc_energy, LZ );
422                 energy = silk_RSHIFT( energy, silk_max_32( 24 - LZ, 0 ) );
423
424                 frac_Q24 = silk_DIV32( psPLC->conc_energy, silk_max( energy, 1 ) );
425
426                 gain_Q16 = silk_LSHIFT( silk_SQRT_APPROX( frac_Q24 ), 4 );
427                 slope_Q16 = silk_DIV32_16( ( (opus_int32)1 << 16 ) - gain_Q16, length );
428                 /* Make slope 4x steeper to avoid missing onsets after DTX */
429                 slope_Q16 = silk_LSHIFT( slope_Q16, 2 );
430
431                 for( i = 0; i < length; i++ ) {
432                     frame[ i ] = silk_SMULWB( gain_Q16, frame[ i ] );
433                     gain_Q16 += slope_Q16;
434                     if( gain_Q16 > (opus_int32)1 << 16 ) {
435                         break;
436                     }
437                 }
438             }
439         }
440         psPLC->last_frame_lost = 0;
441     }
442 }