Initial Skype commit taken from FreeSwitch, which got it from the IETF draft.
[opus.git] / src / SKP_Silk_NSQ_del_dec.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 \r
30 typedef struct {\r
31     SKP_int   RandState[ DECISION_DELAY ];\r
32     SKP_int32 Q_Q10[     DECISION_DELAY ];\r
33     SKP_int32 Xq_Q10[    DECISION_DELAY ];\r
34     SKP_int32 Pred_Q16[  DECISION_DELAY ];\r
35     SKP_int32 Shape_Q10[ DECISION_DELAY ];\r
36     SKP_int32 Gain_Q16[  DECISION_DELAY ];\r
37     SKP_int32 sLPC_Q14[ MAX_FRAME_LENGTH / NB_SUBFR + NSQ_LPC_BUF_LENGTH ];\r
38     SKP_int32 LF_AR_Q12;\r
39     SKP_int32 Seed;\r
40     SKP_int32 SeedInit;\r
41     SKP_int32 RD_Q10;\r
42 } NSQ_del_dec_struct;\r
43 \r
44 typedef struct {\r
45     SKP_int32 Q_Q10;\r
46     SKP_int32 RD_Q10;\r
47     SKP_int32 xq_Q14;\r
48     SKP_int32 LF_AR_Q12;\r
49     SKP_int32 sLTP_shp_Q10;\r
50     SKP_int32 LPC_exc_Q16;\r
51 } NSQ_sample_struct;\r
52 \r
53 SKP_INLINE void SKP_Silk_copy_del_dec_state(\r
54     NSQ_del_dec_struct  *DD_dst,                /* I    Dst del dec state                   */\r
55     NSQ_del_dec_struct  *DD_src,                /* I    Src del dec state                   */\r
56     SKP_int             LPC_state_idx           /* I    Index to LPC buffer                 */\r
57 );\r
58 \r
59 SKP_INLINE void SKP_Silk_nsq_del_dec_scale_states(\r
60     SKP_Silk_nsq_state  *NSQ,                   /* I/O  NSQ state                           */\r
61     NSQ_del_dec_struct  psDelDec[],             /* I/O  Delayed decision states             */\r
62     const SKP_int16     x[],                    /* I    Input in Q0                         */\r
63     SKP_int32           x_sc_Q10[],             /* O    Input scaled with 1/Gain in Q10     */\r
64     SKP_int             length,                 /* I    Length of input                     */\r
65     SKP_int16           sLTP[],                 /* I    Re-whitened LTP state in Q0         */\r
66     SKP_int32           sLTP_Q16[],             /* O    LTP state matching scaled input     */\r
67     SKP_int             subfr,                  /* I    Subframe number                     */\r
68     SKP_int             nStatesDelayedDecision, /* I    Number of del dec states            */\r
69     SKP_int             smpl_buf_idx,           /* I    Index to newest samples in buffers  */\r
70     const SKP_int       LTP_scale_Q14,          /* I    LTP state scaling                   */\r
71     const SKP_int32     Gains_Q16[ NB_SUBFR ],  /* I                                        */\r
72     const SKP_int       pitchL[ NB_SUBFR ]      /* I    Pitch lag                           */\r
73 );\r
74 \r
75 /******************************************/\r
76 /* Noise shape quantizer for one subframe */\r
77 /******************************************/\r
78 SKP_INLINE void SKP_Silk_noise_shape_quantizer_del_dec(\r
79     SKP_Silk_nsq_state  *NSQ,                   /* I/O  NSQ state                           */\r
80     NSQ_del_dec_struct  psDelDec[],             /* I/O  Delayed decision states             */\r
81     SKP_int             sigtype,                /* I    Signal type                         */\r
82     const SKP_int32     x_Q10[],                /* I                                        */\r
83     SKP_int             q[],                    /* O                                        */\r
84     SKP_int16           xq[],                   /* O                                        */\r
85     SKP_int32           sLTP_Q16[],             /* I/O  LTP filter state                    */\r
86     const SKP_int16     a_Q12[],                /* I    Short term prediction coefs         */\r
87     const SKP_int16     b_Q14[],                /* I    Long term prediction coefs          */\r
88     const SKP_int16     AR_shp_Q13[],           /* I    Noise shaping coefs                 */\r
89     SKP_int             lag,                    /* I    Pitch lag                           */\r
90     SKP_int32           HarmShapeFIRPacked_Q14, /* I                                        */\r
91     SKP_int             Tilt_Q14,               /* I    Spectral tilt                       */\r
92     SKP_int32           LF_shp_Q14,             /* I                                        */\r
93     SKP_int32           Gain_Q16,               /* I                                        */\r
94     SKP_int             Lambda_Q10,             /* I                                        */\r
95     SKP_int             offset_Q10,             /* I                                        */\r
96     SKP_int             length,                 /* I    Input length                        */\r
97     SKP_int             subfr,                  /* I    Subframe number                     */\r
98     SKP_int             shapingLPCOrder,        /* I    Shaping LPC filter order            */\r
99     SKP_int             predictLPCOrder,        /* I    Prediction LPC filter order         */\r
100     SKP_int             nStatesDelayedDecision, /* I    Number of states in decision tree   */\r
101     SKP_int             *smpl_buf_idx,          /* I    Index to newest samples in buffers  */\r
102     SKP_int             decisionDelay           /* I                                        */\r
103 );\r
104 \r
105 void SKP_Silk_NSQ_del_dec(\r
106     SKP_Silk_encoder_state          *psEncC,                                    /* I/O  Encoder State                       */\r
107     SKP_Silk_encoder_control        *psEncCtrlC,                                /* I    Encoder Control                     */\r
108     SKP_Silk_nsq_state              *NSQ,                                       /* I/O  NSQ state                           */\r
109     const SKP_int16                 x[],                                        /* I    Prefiltered input signal            */\r
110     SKP_int                         q[],                                        /* O    Quantized pulse signal              */\r
111     const SKP_int                   LSFInterpFactor_Q2,                         /* I    LSF interpolation factor in Q2      */\r
112     const SKP_int16                 PredCoef_Q12[ 2 * MAX_LPC_ORDER ],          /* I    Prediction coefs                    */\r
113     const SKP_int16                 LTPCoef_Q14[ LTP_ORDER * NB_SUBFR ],        /* I    LT prediction coefs                 */\r
114     const SKP_int16                 AR2_Q13[ NB_SUBFR * SHAPE_LPC_ORDER_MAX ],  /* I                                        */\r
115     const SKP_int                   HarmShapeGain_Q14[ NB_SUBFR ],              /* I                                        */\r
116     const SKP_int                   Tilt_Q14[ NB_SUBFR ],                       /* I    Spectral tilt                       */\r
117     const SKP_int32                 LF_shp_Q14[ NB_SUBFR ],                     /* I                                        */\r
118     const SKP_int32                 Gains_Q16[ NB_SUBFR ],                      /* I                                        */\r
119     const SKP_int                   Lambda_Q10,                                 /* I                                        */\r
120     const SKP_int                   LTP_scale_Q14                               /* I    LTP state scaling                   */\r
121 )\r
122 {\r
123     SKP_int     i, k, lag, start_idx, LSF_interpolation_flag, Winner_ind, subfr;\r
124     SKP_int     last_smple_idx, smpl_buf_idx, decisionDelay, subfr_length;\r
125     const SKP_int16 *A_Q12, *B_Q14, *AR_shp_Q13;\r
126     SKP_int16   *pxq;\r
127     SKP_int32   sLTP_Q16[ 2 * MAX_FRAME_LENGTH ];\r
128     SKP_int16   sLTP[     2 * MAX_FRAME_LENGTH ];\r
129     SKP_int32   HarmShapeFIRPacked_Q14;\r
130     SKP_int     offset_Q10;\r
131     SKP_int32   FiltState[ MAX_LPC_ORDER ], RDmin_Q10;\r
132     SKP_int32   x_sc_Q10[ MAX_FRAME_LENGTH / NB_SUBFR ];\r
133     NSQ_del_dec_struct psDelDec[ DEL_DEC_STATES_MAX ];\r
134     NSQ_del_dec_struct *psDD;\r
135 \r
136     subfr_length = psEncC->frame_length / NB_SUBFR;\r
137 \r
138     /* Set unvoiced lag to the previous one, overwrite later for voiced */\r
139     lag = NSQ->lagPrev;\r
140 \r
141     SKP_assert( NSQ->prev_inv_gain_Q16 != 0 );\r
142 \r
143     /* Initialize delayed decision states */\r
144     SKP_memset( psDelDec, 0, psEncC->nStatesDelayedDecision * sizeof( NSQ_del_dec_struct ) );\r
145     for( k = 0; k < psEncC->nStatesDelayedDecision; k++ ) {\r
146         psDD                 = &psDelDec[ k ];\r
147         psDD->Seed           = ( k + psEncCtrlC->Seed ) & 3;\r
148         psDD->SeedInit       = psDD->Seed;\r
149         psDD->RD_Q10         = 0;\r
150         psDD->LF_AR_Q12      = NSQ->sLF_AR_shp_Q12;\r
151         psDD->Shape_Q10[ 0 ] = NSQ->sLTP_shp_Q10[ psEncC->frame_length - 1 ];\r
152         SKP_memcpy( psDD->sLPC_Q14, NSQ->sLPC_Q14, NSQ_LPC_BUF_LENGTH * sizeof( SKP_int32 ) );\r
153     }\r
154 \r
155     offset_Q10   = SKP_Silk_Quantization_Offsets_Q10[ psEncCtrlC->sigtype ][ psEncCtrlC->QuantOffsetType ];\r
156     smpl_buf_idx = 0; /* index of oldest samples */\r
157 \r
158     decisionDelay = SKP_min_int( DECISION_DELAY, subfr_length );\r
159     /* For voiced frames limit the decision delay to lower than the pitch lag */\r
160     if( psEncCtrlC->sigtype == SIG_TYPE_VOICED ) {\r
161         for( k = 0; k < NB_SUBFR; k++ ) {\r
162             decisionDelay = SKP_min_int( decisionDelay, psEncCtrlC->pitchL[ k ] - LTP_ORDER / 2 - 1 );\r
163         }\r
164     }\r
165 \r
166     if( LSFInterpFactor_Q2 == ( 1 << 2 ) ) {\r
167         LSF_interpolation_flag = 0;\r
168     } else {\r
169         LSF_interpolation_flag = 1;\r
170     }\r
171 \r
172     /* Setup pointers to start of sub frame */\r
173     pxq                   = &NSQ->xq[ psEncC->frame_length ];\r
174     NSQ->sLTP_shp_buf_idx = psEncC->frame_length;\r
175     NSQ->sLTP_buf_idx     = psEncC->frame_length;\r
176     subfr = 0;\r
177     for( k = 0; k < NB_SUBFR; k++ ) {\r
178         A_Q12      = &PredCoef_Q12[ ( ( k >> 1 ) | ( 1 - LSF_interpolation_flag ) ) * MAX_LPC_ORDER ];\r
179         B_Q14      = &LTPCoef_Q14[ k * LTP_ORDER           ];\r
180         AR_shp_Q13 = &AR2_Q13[     k * SHAPE_LPC_ORDER_MAX ];\r
181 \r
182         NSQ->rewhite_flag = 0;\r
183         if( psEncCtrlC->sigtype == SIG_TYPE_VOICED ) {\r
184             /* Voiced */\r
185             lag = psEncCtrlC->pitchL[ k ];\r
186 \r
187             /* Re-whitening */\r
188             if( ( k & ( 3 - SKP_LSHIFT( LSF_interpolation_flag, 1 ) ) ) == 0 ) {\r
189                 if( k == 2 ) {\r
190                     /* RESET DELAYED DECISIONS */\r
191                     /* Find winner */\r
192                     RDmin_Q10 = psDelDec[ 0 ].RD_Q10;\r
193                     Winner_ind = 0;\r
194                     for( i = 1; i < psEncC->nStatesDelayedDecision; i++ ) {\r
195                         if( psDelDec[ i ].RD_Q10 < RDmin_Q10 ) {\r
196                             RDmin_Q10 = psDelDec[ i ].RD_Q10;\r
197                             Winner_ind = i;\r
198                         }\r
199                     }\r
200                     for( i = 0; i < psEncC->nStatesDelayedDecision; i++ ) {\r
201                         if( i != Winner_ind ) {\r
202                             psDelDec[ i ].RD_Q10 += ( SKP_int32_MAX >> 4 );\r
203                             SKP_assert( psDelDec[ i ].RD_Q10 >= 0 );\r
204                         }\r
205                     }\r
206                     \r
207                     /* Copy final part of signals from winner state to output and long-term filter states */\r
208                     psDD = &psDelDec[ Winner_ind ];\r
209                     last_smple_idx = smpl_buf_idx + decisionDelay;\r
210                     for( i = 0; i < decisionDelay; i++ ) {\r
211                         last_smple_idx = ( last_smple_idx - 1 ) & DECISION_DELAY_MASK;\r
212                         q[   i - decisionDelay ] = ( SKP_int )SKP_RSHIFT( psDD->Q_Q10[ last_smple_idx ], 10 );\r
213                         pxq[ i - decisionDelay ] = ( SKP_int16 )SKP_SAT16( SKP_RSHIFT_ROUND( \r
214                             SKP_SMULWW( psDD->Xq_Q10[ last_smple_idx ], \r
215                             psDD->Gain_Q16[ last_smple_idx ] ), 10 ) );\r
216                         NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - decisionDelay + i ] = psDD->Shape_Q10[ last_smple_idx ];\r
217                     }\r
218 \r
219                     subfr = 0;\r
220                 }\r
221 \r
222                 /* Rewhiten with new A coefs */\r
223                 start_idx = psEncC->frame_length - lag - psEncC->predictLPCOrder - LTP_ORDER / 2;\r
224                 start_idx = SKP_LIMIT( start_idx, 0, psEncC->frame_length - psEncC->predictLPCOrder );\r
225                 \r
226                 SKP_memset( FiltState, 0, psEncC->predictLPCOrder * sizeof( SKP_int32 ) );\r
227                 SKP_Silk_MA_Prediction( &NSQ->xq[ start_idx + k * psEncC->subfr_length ], \r
228                     A_Q12, FiltState, sLTP + start_idx, psEncC->frame_length - start_idx, psEncC->predictLPCOrder );\r
229 \r
230                 NSQ->sLTP_buf_idx = psEncC->frame_length;\r
231                 NSQ->rewhite_flag = 1;\r
232             }\r
233         }\r
234 \r
235         /* Noise shape parameters */\r
236         SKP_assert( HarmShapeGain_Q14[ k ] >= 0 );\r
237         HarmShapeFIRPacked_Q14  =                        SKP_RSHIFT( HarmShapeGain_Q14[ k ], 2 );\r
238         HarmShapeFIRPacked_Q14 |= SKP_LSHIFT( ( SKP_int32 )SKP_RSHIFT( HarmShapeGain_Q14[ k ], 1 ), 16 );\r
239 \r
240         SKP_Silk_nsq_del_dec_scale_states( NSQ, psDelDec, x, x_sc_Q10, \r
241             subfr_length, sLTP, sLTP_Q16, k, psEncC->nStatesDelayedDecision, smpl_buf_idx,\r
242             LTP_scale_Q14, Gains_Q16, psEncCtrlC->pitchL );\r
243 \r
244         SKP_Silk_noise_shape_quantizer_del_dec( NSQ, psDelDec, psEncCtrlC->sigtype, x_sc_Q10, q, pxq, sLTP_Q16,\r
245             A_Q12, B_Q14, AR_shp_Q13, lag, HarmShapeFIRPacked_Q14, Tilt_Q14[ k ], LF_shp_Q14[ k ], Gains_Q16[ k ], \r
246             Lambda_Q10, offset_Q10, psEncC->subfr_length, subfr++, psEncC->shapingLPCOrder, psEncC->predictLPCOrder, \r
247             psEncC->nStatesDelayedDecision, &smpl_buf_idx, decisionDelay \r
248         );\r
249         \r
250         x   += psEncC->subfr_length;\r
251         q   += psEncC->subfr_length;\r
252         pxq += psEncC->subfr_length;\r
253     }\r
254 \r
255     /* Find winner */\r
256     RDmin_Q10 = psDelDec[ 0 ].RD_Q10;\r
257     Winner_ind = 0;\r
258     for( k = 1; k < psEncC->nStatesDelayedDecision; k++ ) {\r
259         if( psDelDec[ k ].RD_Q10 < RDmin_Q10 ) {\r
260             RDmin_Q10 = psDelDec[ k ].RD_Q10;\r
261             Winner_ind = k;\r
262         }\r
263     }\r
264     \r
265     /* Copy final part of signals from winner state to output and long-term filter states */\r
266     psDD = &psDelDec[ Winner_ind ];\r
267     psEncCtrlC->Seed = psDD->SeedInit;\r
268     last_smple_idx = smpl_buf_idx + decisionDelay;\r
269     for( i = 0; i < decisionDelay; i++ ) {\r
270         last_smple_idx = ( last_smple_idx - 1 ) & DECISION_DELAY_MASK;\r
271         q[   i - decisionDelay ] = ( SKP_int )SKP_RSHIFT( psDD->Q_Q10[ last_smple_idx ], 10 );\r
272         pxq[ i - decisionDelay ] = ( SKP_int16 )SKP_SAT16( SKP_RSHIFT_ROUND( \r
273             SKP_SMULWW( psDD->Xq_Q10[ last_smple_idx ], psDD->Gain_Q16[ last_smple_idx ] ), 10 ) );\r
274         NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - decisionDelay + i ] = psDD->Shape_Q10[ last_smple_idx ];\r
275         sLTP_Q16[          NSQ->sLTP_buf_idx     - decisionDelay + i ] = psDD->Pred_Q16[  last_smple_idx ];\r
276 \r
277     }\r
278     SKP_memcpy( NSQ->sLPC_Q14, &psDD->sLPC_Q14[ psEncC->subfr_length ], NSQ_LPC_BUF_LENGTH * sizeof( SKP_int32 ) );\r
279 \r
280     /* Update states */\r
281     NSQ->sLF_AR_shp_Q12    = psDD->LF_AR_Q12;\r
282     NSQ->prev_inv_gain_Q16 = NSQ->prev_inv_gain_Q16;\r
283     NSQ->lagPrev           = psEncCtrlC->pitchL[ NB_SUBFR - 1 ];\r
284 \r
285     /* Save quantized speech and noise shaping signals */\r
286     SKP_memcpy( NSQ->xq,           &NSQ->xq[           psEncC->frame_length ], psEncC->frame_length * sizeof( SKP_int16 ) );\r
287     SKP_memcpy( NSQ->sLTP_shp_Q10, &NSQ->sLTP_shp_Q10[ psEncC->frame_length ], psEncC->frame_length * sizeof( SKP_int32 ) );\r
288 \r
289 }\r
290 \r
291 /******************************************/\r
292 /* Noise shape quantizer for one subframe */\r
293 /******************************************/\r
294 SKP_INLINE void SKP_Silk_noise_shape_quantizer_del_dec(\r
295     SKP_Silk_nsq_state  *NSQ,                   /* I/O  NSQ state                           */\r
296     NSQ_del_dec_struct  psDelDec[],             /* I/O  Delayed decision states             */\r
297     SKP_int             sigtype,                /* I    Signal type                         */\r
298     const SKP_int32     x_Q10[],                /* I                                        */\r
299     SKP_int             q[],                    /* O                                        */\r
300     SKP_int16           xq[],                   /* O                                        */\r
301     SKP_int32           sLTP_Q16[],             /* I/O  LTP filter state                    */\r
302     const SKP_int16     a_Q12[],                /* I    Short term prediction coefs         */\r
303     const SKP_int16     b_Q14[],                /* I    Long term prediction coefs          */\r
304     const SKP_int16     AR_shp_Q13[],           /* I    Noise shaping coefs                 */\r
305     SKP_int             lag,                    /* I    Pitch lag                           */\r
306     SKP_int32           HarmShapeFIRPacked_Q14, /* I                                        */\r
307     SKP_int             Tilt_Q14,               /* I    Spectral tilt                       */\r
308     SKP_int32           LF_shp_Q14,             /* I                                        */\r
309     SKP_int32           Gain_Q16,               /* I                                        */\r
310     SKP_int             Lambda_Q10,             /* I                                        */\r
311     SKP_int             offset_Q10,             /* I                                        */\r
312     SKP_int             length,                 /* I    Input length                        */\r
313     SKP_int             subfr,                  /* I    Subframe number                     */\r
314     SKP_int             shapingLPCOrder,        /* I    Shaping LPC filter order            */\r
315     SKP_int             predictLPCOrder,        /* I    Prediction LPC filter order         */\r
316     SKP_int             nStatesDelayedDecision, /* I    Number of states in decision tree   */\r
317     SKP_int             *smpl_buf_idx,          /* I    Index to newest samples in buffers  */\r
318     SKP_int             decisionDelay           /* I                                        */\r
319 )\r
320 {\r
321     SKP_int     i, j, k, Winner_ind, RDmin_ind, RDmax_ind, last_smple_idx;\r
322     SKP_int32   Winner_rand_state;\r
323     SKP_int32   LTP_pred_Q14, LPC_pred_Q10, n_AR_Q10, n_LTP_Q14;\r
324     SKP_int32   n_LF_Q10;\r
325     SKP_int32   r_Q10, rr_Q20, rd1_Q10, rd2_Q10, RDmin_Q10, RDmax_Q10;\r
326     SKP_int32   q1_Q10, q2_Q10;\r
327     SKP_int32   Atmp, dither;\r
328     SKP_int32   exc_Q10, LPC_exc_Q10, xq_Q10;\r
329     SKP_int32   tmp, sLF_AR_shp_Q10;\r
330     SKP_int32   *pred_lag_ptr, *shp_lag_ptr;\r
331     SKP_int32   *psLPC_Q14;\r
332     SKP_int32   a_Q12_tmp[ MAX_LPC_ORDER / 2 ], AR_shp_Q13_tmp[ MAX_LPC_ORDER / 2 ];\r
333     NSQ_sample_struct  psSampleState[ DEL_DEC_STATES_MAX ][ 2 ];\r
334     NSQ_del_dec_struct *psDD;\r
335     NSQ_sample_struct  *psSS;\r
336 \r
337     shp_lag_ptr  = &NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - lag + HARM_SHAPE_FIR_TAPS / 2 ];\r
338     pred_lag_ptr = &sLTP_Q16[ NSQ->sLTP_buf_idx - lag + LTP_ORDER / 2 ];\r
339     \r
340     /* Preload LPC coeficients to array on stack. Gives small performance gain */\r
341     SKP_memcpy( a_Q12_tmp, a_Q12, predictLPCOrder * sizeof( SKP_int16 ) );\r
342     SKP_memcpy( AR_shp_Q13_tmp, AR_shp_Q13, shapingLPCOrder * sizeof( SKP_int16 ) );\r
343 \r
344     for( i = 0; i < length; i++ ) {\r
345         /* Perform common calculations used in all states */\r
346 \r
347         /* Long-term prediction */\r
348         if( sigtype == SIG_TYPE_VOICED ) {\r
349             /* Unrolled loop */\r
350             LTP_pred_Q14 = SKP_SMULWB(               pred_lag_ptr[  0 ], b_Q14[ 0 ] );\r
351             LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -1 ], b_Q14[ 1 ] );\r
352             LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -2 ], b_Q14[ 2 ] );\r
353             LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -3 ], b_Q14[ 3 ] );\r
354             LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -4 ], b_Q14[ 4 ] );\r
355             pred_lag_ptr++;\r
356         } else {\r
357             LTP_pred_Q14 = 0;\r
358         }\r
359 \r
360         /* Long-term shaping */\r
361         if( lag > 0 ) {\r
362             /* Symmetric, packed FIR coefficients */\r
363             n_LTP_Q14 = SKP_SMULWB( SKP_ADD32( shp_lag_ptr[ 0 ], shp_lag_ptr[ -2 ] ), HarmShapeFIRPacked_Q14 );\r
364             n_LTP_Q14 = SKP_SMLAWT( n_LTP_Q14, shp_lag_ptr[ -1 ],                     HarmShapeFIRPacked_Q14 );\r
365             n_LTP_Q14 = SKP_LSHIFT( n_LTP_Q14, 6 );\r
366             shp_lag_ptr++;\r
367         } else {\r
368             n_LTP_Q14 = 0;\r
369         }\r
370 \r
371         for( k = 0; k < nStatesDelayedDecision; k++ ) {\r
372             /* Delayed decision state */\r
373             psDD = &psDelDec[ k ];\r
374 \r
375             /* Sample state */\r
376             psSS = psSampleState[ k ];\r
377 \r
378             /* Generate dither */\r
379             psDD->Seed = SKP_RAND( psDD->Seed );\r
380 \r
381             /* dither = rand_seed < 0 ? 0xFFFFFFFF : 0; */\r
382             dither = SKP_RSHIFT( psDD->Seed, 31 );\r
383             \r
384             /* Pointer used in short term prediction and shaping */\r
385             psLPC_Q14 = &psDD->sLPC_Q14[ NSQ_LPC_BUF_LENGTH - 1 + i ];\r
386             /* Short-term prediction */\r
387             SKP_assert( ( predictLPCOrder  & 1 ) == 0 );    /* check that order is even */\r
388             SKP_assert( ( (SKP_int64)a_Q12 & 3 ) == 0 );    /* check that array starts at 4-byte aligned address */\r
389             SKP_assert( predictLPCOrder >= 10 );            /* check that unrolling works */\r
390 \r
391             /* Partially unrolled */\r
392             Atmp = a_Q12_tmp[ 0 ];          /* read two coefficients at once */\r
393             LPC_pred_Q10 = SKP_SMULWB(               psLPC_Q14[ 0  ], Atmp );\r
394             LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psLPC_Q14[ -1 ], Atmp );\r
395             Atmp = a_Q12_tmp[ 1 ];\r
396             LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -2 ], Atmp );\r
397             LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psLPC_Q14[ -3 ], Atmp );\r
398             Atmp = a_Q12_tmp[ 2 ];\r
399             LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -4 ], Atmp );\r
400             LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psLPC_Q14[ -5 ], Atmp );\r
401             Atmp = a_Q12_tmp[ 3 ];\r
402             LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -6 ], Atmp );\r
403             LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psLPC_Q14[ -7 ], Atmp );\r
404             Atmp = a_Q12_tmp[ 4 ];\r
405             LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -8 ], Atmp );\r
406             LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psLPC_Q14[ -9 ], Atmp );\r
407             for( j = 10; j < predictLPCOrder; j += 2 ) {\r
408                 Atmp = a_Q12_tmp[ j >> 1 ]; /* read two coefficients at once */\r
409                 LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -j     ], Atmp );\r
410                 LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psLPC_Q14[ -j - 1 ], Atmp );\r
411             }\r
412 \r
413             /* Noise shape feedback */\r
414             SKP_assert( ( shapingLPCOrder       & 1 ) == 0 );   /* check that order is even */\r
415             SKP_assert( ( (SKP_int64)AR_shp_Q13 & 3 ) == 0 );   /* check that array starts at 4-byte aligned address */\r
416             SKP_assert( shapingLPCOrder >= 12 );                /* check that unrolling works */\r
417             /* NOTE: the code below loads two int16 values in an int32, and multiplies each using the   */\r
418             /* SMLAWB and SMLAWT instructions. On a big-endian CPU the two int16 variables would be     */\r
419             /* loaded in reverse order and the code will give the wrong result. In that case swapping   */\r
420             /* the SMLAWB and SMLAWT instructions should solve the problem.                             */\r
421 \r
422             /* Partially unrolled */\r
423             Atmp = AR_shp_Q13_tmp[ 0 ];         /* read two coefficients at once */\r
424             n_AR_Q10 = SKP_SMULWB(           psLPC_Q14[ 0  ], Atmp );\r
425             n_AR_Q10 = SKP_SMLAWT( n_AR_Q10, psLPC_Q14[ -1 ], Atmp );\r
426             Atmp = AR_shp_Q13_tmp[ 1 ];\r
427             n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, psLPC_Q14[ -2 ], Atmp );\r
428             n_AR_Q10 = SKP_SMLAWT( n_AR_Q10, psLPC_Q14[ -3 ], Atmp );\r
429             Atmp = AR_shp_Q13_tmp[ 2 ];\r
430             n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, psLPC_Q14[ -4 ], Atmp );\r
431             n_AR_Q10 = SKP_SMLAWT( n_AR_Q10, psLPC_Q14[ -5 ], Atmp );\r
432             Atmp = AR_shp_Q13_tmp[ 3 ];\r
433             n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, psLPC_Q14[ -6 ], Atmp );\r
434             n_AR_Q10 = SKP_SMLAWT( n_AR_Q10, psLPC_Q14[ -7 ], Atmp );\r
435             Atmp = AR_shp_Q13_tmp[ 4 ];\r
436             n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, psLPC_Q14[ -8 ], Atmp );\r
437             n_AR_Q10 = SKP_SMLAWT( n_AR_Q10, psLPC_Q14[ -9 ], Atmp );\r
438             Atmp = AR_shp_Q13_tmp[ 5 ];\r
439             n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, psLPC_Q14[ -10 ], Atmp );\r
440             n_AR_Q10 = SKP_SMLAWT( n_AR_Q10, psLPC_Q14[ -11 ], Atmp );\r
441             for( j = 12; j < shapingLPCOrder; j += 2 ) {\r
442                 Atmp = AR_shp_Q13_tmp[ j >> 1 ];        /* read two coefficients at once */\r
443                 n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, psLPC_Q14[ -j     ], Atmp );\r
444                 n_AR_Q10 = SKP_SMLAWT( n_AR_Q10, psLPC_Q14[ -j - 1 ], Atmp );\r
445             }\r
446             n_AR_Q10 = SKP_RSHIFT( n_AR_Q10, 1 );           /* Q11 -> Q10 */\r
447             n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, psDD->LF_AR_Q12, Tilt_Q14 );\r
448 \r
449             n_LF_Q10   = SKP_LSHIFT( SKP_SMULWB( psDD->Shape_Q10[ *smpl_buf_idx ], LF_shp_Q14 ), 2 ); \r
450             n_LF_Q10   = SKP_SMLAWT( n_LF_Q10, psDD->LF_AR_Q12, LF_shp_Q14 );       \r
451 \r
452             /* Input minus prediction plus noise feedback                       */\r
453             /* r = x[ i ] - LTP_pred - LPC_pred + n_AR + n_Tilt + n_LF + n_LTP  */\r
454             tmp   = SKP_SUB32( LTP_pred_Q14, n_LTP_Q14 );                       /* Add Q14 stuff */\r
455             tmp   = SKP_RSHIFT_ROUND( tmp, 4 );                                 /* round to Q10  */\r
456             tmp   = SKP_ADD32( tmp, LPC_pred_Q10 );                             /* add Q10 stuff */ \r
457             tmp   = SKP_SUB32( tmp, n_AR_Q10 );                                 /* subtract Q10 stuff */ \r
458             tmp   = SKP_SUB32( tmp, n_LF_Q10 );                                 /* subtract Q10 stuff */ \r
459             r_Q10 = SKP_SUB32( x_Q10[ i ], tmp );                               /* residual error Q10 */\r
460             \r
461 \r
462             /* Flip sign depending on dither */\r
463             r_Q10 = ( r_Q10 ^ dither ) - dither;\r
464             r_Q10 = SKP_SUB32( r_Q10, offset_Q10 );\r
465             r_Q10 = SKP_LIMIT( r_Q10, -64 << 10, 64 << 10 );\r
466 \r
467             /* Find two quantization level candidates and measure their rate-distortion */\r
468             if( r_Q10 < -1536 ) {\r
469                 q1_Q10  = SKP_LSHIFT( SKP_RSHIFT_ROUND( r_Q10, 10 ), 10 );\r
470                 r_Q10   = SKP_SUB32( r_Q10, q1_Q10 );\r
471                 rd1_Q10 = SKP_RSHIFT( SKP_SMLABB( SKP_MUL( -SKP_ADD32( q1_Q10, offset_Q10 ), Lambda_Q10 ), r_Q10, r_Q10 ), 10 );\r
472                 rd2_Q10 = SKP_ADD32( rd1_Q10, 1024 );\r
473                 rd2_Q10 = SKP_SUB32( rd2_Q10, SKP_ADD_LSHIFT32( Lambda_Q10, r_Q10, 1 ) );\r
474                 q2_Q10  = SKP_ADD32( q1_Q10, 1024 );\r
475             } else if( r_Q10 > 512 ) {\r
476                 q1_Q10  = SKP_LSHIFT( SKP_RSHIFT_ROUND( r_Q10, 10 ), 10 );\r
477                 r_Q10   = SKP_SUB32( r_Q10, q1_Q10 );\r
478                 rd1_Q10 = SKP_RSHIFT( SKP_SMLABB( SKP_MUL( SKP_ADD32( q1_Q10, offset_Q10 ), Lambda_Q10 ), r_Q10, r_Q10 ), 10 );\r
479                 rd2_Q10 = SKP_ADD32( rd1_Q10, 1024 );\r
480                 rd2_Q10 = SKP_SUB32( rd2_Q10, SKP_SUB_LSHIFT32( Lambda_Q10, r_Q10, 1 ) );\r
481                 q2_Q10  = SKP_SUB32( q1_Q10, 1024 );\r
482             } else {            /* r_Q10 >= -1536 && q1_Q10 <= 512 */\r
483                 rr_Q20  = SKP_SMULBB( offset_Q10, Lambda_Q10 );\r
484                 rd2_Q10 = SKP_RSHIFT( SKP_SMLABB( rr_Q20, r_Q10, r_Q10 ), 10 );\r
485                 rd1_Q10 = SKP_ADD32( rd2_Q10, 1024 );\r
486                 rd1_Q10 = SKP_ADD32( rd1_Q10, SKP_SUB_RSHIFT32( SKP_ADD_LSHIFT32( Lambda_Q10, r_Q10, 1 ), rr_Q20, 9 ) );\r
487                 q1_Q10  = -1024;\r
488                 q2_Q10  = 0;\r
489             }\r
490 \r
491             if( rd1_Q10 < rd2_Q10 ) {\r
492                 psSS[ 0 ].RD_Q10 = SKP_ADD32( psDD->RD_Q10, rd1_Q10 ); \r
493                 psSS[ 1 ].RD_Q10 = SKP_ADD32( psDD->RD_Q10, rd2_Q10 );\r
494                 psSS[ 0 ].Q_Q10 = q1_Q10;\r
495                 psSS[ 1 ].Q_Q10 = q2_Q10;\r
496             } else {\r
497                 psSS[ 0 ].RD_Q10 = SKP_ADD32( psDD->RD_Q10, rd2_Q10 );\r
498                 psSS[ 1 ].RD_Q10 = SKP_ADD32( psDD->RD_Q10, rd1_Q10 );\r
499                 psSS[ 0 ].Q_Q10 = q2_Q10;\r
500                 psSS[ 1 ].Q_Q10 = q1_Q10;\r
501             }\r
502 \r
503             /* Update states for best quantization */\r
504 \r
505             /* Quantized excitation */\r
506             exc_Q10 = SKP_ADD32( offset_Q10, psSS[ 0 ].Q_Q10 );\r
507             exc_Q10 = ( exc_Q10 ^ dither ) - dither;\r
508 \r
509             /* Add predictions */\r
510             LPC_exc_Q10 = exc_Q10 + SKP_RSHIFT_ROUND( LTP_pred_Q14, 4 );\r
511             xq_Q10      = SKP_ADD32( LPC_exc_Q10, LPC_pred_Q10 );\r
512 \r
513             /* Update states */\r
514             sLF_AR_shp_Q10         = SKP_SUB32(  xq_Q10, n_AR_Q10 );\r
515             psSS[ 0 ].sLTP_shp_Q10 = SKP_SUB32(  sLF_AR_shp_Q10, n_LF_Q10 );\r
516             psSS[ 0 ].LF_AR_Q12    = SKP_LSHIFT( sLF_AR_shp_Q10, 2 );\r
517             psSS[ 0 ].xq_Q14       = SKP_LSHIFT( xq_Q10,         4 );\r
518             psSS[ 0 ].LPC_exc_Q16  = SKP_LSHIFT( LPC_exc_Q10,    6 );\r
519 \r
520             /* Update states for second best quantization */\r
521 \r
522             /* Quantized excitation */\r
523             exc_Q10 = SKP_ADD32( offset_Q10, psSS[ 1 ].Q_Q10 );\r
524             exc_Q10 = ( exc_Q10 ^ dither ) - dither;\r
525 \r
526             /* Add predictions */\r
527             LPC_exc_Q10 = exc_Q10 + SKP_RSHIFT_ROUND( LTP_pred_Q14, 4 );\r
528             xq_Q10      = SKP_ADD32( LPC_exc_Q10, LPC_pred_Q10 );\r
529 \r
530             /* Update states */\r
531             sLF_AR_shp_Q10         = SKP_SUB32(  xq_Q10, n_AR_Q10 );\r
532             psSS[ 1 ].sLTP_shp_Q10 = SKP_SUB32(  sLF_AR_shp_Q10, n_LF_Q10 );\r
533             psSS[ 1 ].LF_AR_Q12    = SKP_LSHIFT( sLF_AR_shp_Q10, 2 );\r
534             psSS[ 1 ].xq_Q14       = SKP_LSHIFT( xq_Q10,         4 );\r
535             psSS[ 1 ].LPC_exc_Q16  = SKP_LSHIFT( LPC_exc_Q10,    6 );\r
536         }\r
537 \r
538         *smpl_buf_idx  = ( *smpl_buf_idx - 1 ) & DECISION_DELAY_MASK;                   /* Index to newest samples              */\r
539         last_smple_idx = ( *smpl_buf_idx + decisionDelay ) & DECISION_DELAY_MASK;       /* Index to decisionDelay old samples   */\r
540 \r
541         /* Find winner */\r
542         RDmin_Q10 = psSampleState[ 0 ][ 0 ].RD_Q10;\r
543         Winner_ind = 0;\r
544         for( k = 1; k < nStatesDelayedDecision; k++ ) {\r
545             if( psSampleState[ k ][ 0 ].RD_Q10 < RDmin_Q10 ) {\r
546                 RDmin_Q10   = psSampleState[ k ][ 0 ].RD_Q10;\r
547                 Winner_ind = k;\r
548             }\r
549         }\r
550 \r
551         /* Increase RD values of expired states */\r
552         Winner_rand_state = psDelDec[ Winner_ind ].RandState[ last_smple_idx ];\r
553         for( k = 0; k < nStatesDelayedDecision; k++ ) {\r
554             if( psDelDec[ k ].RandState[ last_smple_idx ] != Winner_rand_state ) {\r
555                 psSampleState[ k ][ 0 ].RD_Q10 = SKP_ADD32( psSampleState[ k ][ 0 ].RD_Q10, ( SKP_int32_MAX >> 4 ) );\r
556                 psSampleState[ k ][ 1 ].RD_Q10 = SKP_ADD32( psSampleState[ k ][ 1 ].RD_Q10, ( SKP_int32_MAX >> 4 ) );\r
557                 SKP_assert( psSampleState[ k ][ 0 ].RD_Q10 >= 0 );\r
558             }\r
559         }\r
560 \r
561         /* Find worst in first set and best in second set */\r
562         RDmax_Q10  = psSampleState[ 0 ][ 0 ].RD_Q10;\r
563         RDmin_Q10  = psSampleState[ 0 ][ 1 ].RD_Q10;\r
564         RDmax_ind = 0;\r
565         RDmin_ind = 0;\r
566         for( k = 1; k < nStatesDelayedDecision; k++ ) {\r
567             /* find worst in first set */\r
568             if( psSampleState[ k ][ 0 ].RD_Q10 > RDmax_Q10 ) {\r
569                 RDmax_Q10  = psSampleState[ k ][ 0 ].RD_Q10;\r
570                 RDmax_ind = k;\r
571             }\r
572             /* find best in second set */\r
573             if( psSampleState[ k ][ 1 ].RD_Q10 < RDmin_Q10 ) {\r
574                 RDmin_Q10  = psSampleState[ k ][ 1 ].RD_Q10;\r
575                 RDmin_ind = k;\r
576             }\r
577         }\r
578 \r
579         /* Replace a state if best from second set outperforms worst in first set */\r
580         if( RDmin_Q10 < RDmax_Q10 ) {\r
581             SKP_Silk_copy_del_dec_state( &psDelDec[ RDmax_ind ], &psDelDec[ RDmin_ind ], i ); \r
582             SKP_memcpy( &psSampleState[ RDmax_ind ][ 0 ], &psSampleState[ RDmin_ind ][ 1 ], sizeof( NSQ_sample_struct ) );\r
583         }\r
584 \r
585         /* Write samples from winner to output and long-term filter states */\r
586         psDD = &psDelDec[ Winner_ind ];\r
587         if( subfr > 0 || i >= decisionDelay ) {\r
588             q[  i - decisionDelay ] = ( SKP_int )SKP_RSHIFT( psDD->Q_Q10[ last_smple_idx ], 10 );\r
589             xq[ i - decisionDelay ] = ( SKP_int16 )SKP_SAT16( SKP_RSHIFT_ROUND( \r
590                 SKP_SMULWW( psDD->Xq_Q10[ last_smple_idx ], psDD->Gain_Q16[ last_smple_idx ] ), 10 ) );\r
591             NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - decisionDelay ] = psDD->Shape_Q10[ last_smple_idx ];\r
592             sLTP_Q16[          NSQ->sLTP_buf_idx     - decisionDelay ] = psDD->Pred_Q16[  last_smple_idx ];\r
593         }\r
594         NSQ->sLTP_shp_buf_idx++;\r
595         NSQ->sLTP_buf_idx++;\r
596 \r
597         /* Update states */\r
598         for( k = 0; k < nStatesDelayedDecision; k++ ) {\r
599             psDD                                     = &psDelDec[ k ];\r
600             psSS                                     = &psSampleState[ k ][ 0 ];\r
601             psDD->LF_AR_Q12                          = psSS->LF_AR_Q12;\r
602             psDD->sLPC_Q14[ NSQ_LPC_BUF_LENGTH + i ] = psSS->xq_Q14;\r
603             psDD->Xq_Q10[    *smpl_buf_idx ]         = SKP_RSHIFT( psSS->xq_Q14, 4 );\r
604             psDD->Q_Q10[     *smpl_buf_idx ]         = psSS->Q_Q10;\r
605             psDD->Pred_Q16[  *smpl_buf_idx ]         = psSS->LPC_exc_Q16;\r
606             psDD->Shape_Q10[ *smpl_buf_idx ]         = psSS->sLTP_shp_Q10;\r
607             psDD->Seed                               = SKP_ADD_RSHIFT32( psDD->Seed, psSS->Q_Q10, 10 );\r
608             psDD->RandState[ *smpl_buf_idx ]         = psDD->Seed;\r
609             psDD->RD_Q10                             = psSS->RD_Q10;\r
610             psDD->Gain_Q16[  *smpl_buf_idx ]         = Gain_Q16;\r
611         }\r
612     }\r
613     /* Update LPC states */\r
614     for( k = 0; k < nStatesDelayedDecision; k++ ) {\r
615         psDD = &psDelDec[ k ];\r
616         SKP_memcpy( psDD->sLPC_Q14, &psDD->sLPC_Q14[ length ], NSQ_LPC_BUF_LENGTH * sizeof( SKP_int32 ) );\r
617     }\r
618 }\r
619 \r
620 SKP_INLINE void SKP_Silk_nsq_del_dec_scale_states(\r
621     SKP_Silk_nsq_state  *NSQ,                   /* I/O  NSQ state                           */\r
622     NSQ_del_dec_struct  psDelDec[],             /* I/O  Delayed decision states             */\r
623     const SKP_int16     x[],                    /* I    Input in Q0                         */\r
624     SKP_int32           x_sc_Q10[],             /* O    Input scaled with 1/Gain in Q10     */\r
625     SKP_int             length,                 /* I    Length of input                     */\r
626     SKP_int16           sLTP[],                 /* I    Re-whitened LTP state in Q0         */\r
627     SKP_int32           sLTP_Q16[],             /* O    LTP state matching scaled input     */\r
628     SKP_int             subfr,                  /* I    Subframe number                     */\r
629     SKP_int             nStatesDelayedDecision, /* I    Number of del dec states            */\r
630     SKP_int             smpl_buf_idx,           /* I    Index to newest samples in buffers  */\r
631     const SKP_int       LTP_scale_Q14,          /* I    LTP state scaling                   */\r
632     const SKP_int32     Gains_Q16[ NB_SUBFR ],  /* I                                        */\r
633     const SKP_int       pitchL[ NB_SUBFR ]      /* I    Pitch lag                           */\r
634 )\r
635 {\r
636     SKP_int            i, k, scale_length, lag;\r
637     SKP_int32          inv_gain_Q16, gain_adj_Q16, inv_gain_Q32;\r
638     NSQ_del_dec_struct *psDD;\r
639 \r
640     inv_gain_Q16 = SKP_DIV32( SKP_int32_MAX, SKP_RSHIFT( Gains_Q16[ subfr ], 1 ) );\r
641     inv_gain_Q16 = SKP_min( inv_gain_Q16, SKP_int16_MAX );\r
642     lag          = pitchL[ subfr ];\r
643     /* After rewhitening the LTP state is un-scaled. So scale with inv_gain_Q16 */\r
644     if( NSQ->rewhite_flag ) {\r
645         inv_gain_Q32 = SKP_LSHIFT( inv_gain_Q16, 16 );\r
646         if( subfr == 0 ) {\r
647             /* Do LTP downscaling */\r
648             inv_gain_Q32 = SKP_LSHIFT( SKP_SMULWB( inv_gain_Q32, LTP_scale_Q14 ), 2 );\r
649         }\r
650         for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {\r
651             SKP_assert( i < MAX_FRAME_LENGTH );\r
652             sLTP_Q16[ i ] = SKP_SMULWB( inv_gain_Q32, sLTP[ i ] );\r
653         }\r
654     }\r
655 \r
656     /* Adjust for changing gain */\r
657     if( inv_gain_Q16 != NSQ->prev_inv_gain_Q16 ) {\r
658         gain_adj_Q16 = SKP_DIV32_varQ( inv_gain_Q16, NSQ->prev_inv_gain_Q16, 16 );\r
659 \r
660         for( k = 0; k < nStatesDelayedDecision; k++ ) {\r
661             psDD = &psDelDec[ k ];\r
662             \r
663             /* Scale scalar states */\r
664             psDD->LF_AR_Q12 = SKP_SMULWW( gain_adj_Q16, psDD->LF_AR_Q12 );\r
665             \r
666             /* scale short term state */\r
667             for( i = 0; i < NSQ_LPC_BUF_LENGTH; i++ ) {\r
668                 psDD->sLPC_Q14[ NSQ_LPC_BUF_LENGTH - i - 1 ] = SKP_SMULWW( gain_adj_Q16, psDD->sLPC_Q14[ NSQ_LPC_BUF_LENGTH - i - 1 ] );\r
669             }\r
670             for( i = 0; i < DECISION_DELAY; i++ ) {\r
671                 psDD->Pred_Q16[  i ] = SKP_SMULWW( gain_adj_Q16, psDD->Pred_Q16[  i ] );\r
672                 psDD->Shape_Q10[ i ] = SKP_SMULWW( gain_adj_Q16, psDD->Shape_Q10[ i ] );\r
673             }\r
674         }\r
675 \r
676         /* Scale long term shaping state */\r
677 \r
678         /* Calculate length to be scaled, Worst case: Next frame is voiced with max lag */\r
679         scale_length = length * NB_SUBFR;                                               /* aprox max lag */\r
680         scale_length = scale_length - SKP_SMULBB( NB_SUBFR - ( subfr + 1 ), length );   /* subtract samples that will be too old in next frame */\r
681         scale_length = SKP_max_int( scale_length, lag + LTP_ORDER );                    /* make sure to scale whole pitch period if voiced */\r
682 \r
683         for( i = NSQ->sLTP_shp_buf_idx - scale_length; i < NSQ->sLTP_shp_buf_idx; i++ ) {\r
684             NSQ->sLTP_shp_Q10[ i ] = SKP_SMULWW( gain_adj_Q16, NSQ->sLTP_shp_Q10[ i ] );\r
685         }\r
686 \r
687         /* Scale LTP predict state */\r
688         if( NSQ->rewhite_flag == 0 ) {\r
689             for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {\r
690                 sLTP_Q16[ i ] = SKP_SMULWW( gain_adj_Q16, sLTP_Q16[ i ] );\r
691             }\r
692         }\r
693     }\r
694 \r
695     /* Scale input */\r
696     for( i = 0; i < length; i++ ) {\r
697         x_sc_Q10[ i ] = SKP_RSHIFT( SKP_SMULBB( x[ i ], ( SKP_int16 )inv_gain_Q16 ), 6 );\r
698     }\r
699 \r
700     /* save inv_gain */\r
701     SKP_assert( inv_gain_Q16 != 0 );\r
702     NSQ->prev_inv_gain_Q16 = inv_gain_Q16;\r
703 }\r
704 \r
705 SKP_INLINE void SKP_Silk_copy_del_dec_state(\r
706     NSQ_del_dec_struct  *DD_dst,                /* I    Dst del dec state                   */\r
707     NSQ_del_dec_struct  *DD_src,                /* I    Src del dec state                   */\r
708     SKP_int             LPC_state_idx           /* I    Index to LPC buffer                 */\r
709 )\r
710 {\r
711     SKP_memcpy( DD_dst->RandState, DD_src->RandState,   DECISION_DELAY * sizeof( SKP_int   ) );\r
712     SKP_memcpy( DD_dst->Q_Q10,     DD_src->Q_Q10,       DECISION_DELAY * sizeof( SKP_int32 ) );\r
713     SKP_memcpy( DD_dst->Pred_Q16,  DD_src->Pred_Q16,    DECISION_DELAY * sizeof( SKP_int32 ) );\r
714     SKP_memcpy( DD_dst->Shape_Q10, DD_src->Shape_Q10,   DECISION_DELAY * sizeof( SKP_int32 ) );\r
715     SKP_memcpy( DD_dst->Xq_Q10,    DD_src->Xq_Q10,      DECISION_DELAY * sizeof( SKP_int32 ) );\r
716 \r
717     SKP_memcpy( &DD_dst->sLPC_Q14[ LPC_state_idx ], &DD_src->sLPC_Q14[ LPC_state_idx ], NSQ_LPC_BUF_LENGTH * sizeof( SKP_int32 ) );\r
718     DD_dst->LF_AR_Q12 = DD_src->LF_AR_Q12;\r
719     DD_dst->Seed      = DD_src->Seed;\r
720     DD_dst->SeedInit  = DD_src->SeedInit;\r
721     DD_dst->RD_Q10    = DD_src->RD_Q10;\r
722 }\r