Renaming the SKP_ prefix to silk_
[opus.git] / silk / silk_NSQ_del_dec.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, (subject to the limitations in the disclaimer below)
5 are permitted provided that the following conditions 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 Skype Limited, nor the names of specific
12 contributors, may be used to endorse or promote products derived from
13 this software without specific prior written permission.
14 NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED
15 BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
16 CONTRIBUTORS ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
17 BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
18 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
19 COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
20 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
21 NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
22 USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
23 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
25 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 ***********************************************************************/
27
28 #ifdef HAVE_CONFIG_H
29 #include "config.h"
30 #endif
31
32 #include "silk_main.h"
33
34 typedef struct {
35     opus_int32 sLPC_Q14[ MAX_FRAME_LENGTH / MAX_NB_SUBFR + NSQ_LPC_BUF_LENGTH ];
36     opus_int32 RandState[ DECISION_DELAY ];
37     opus_int32 Q_Q10[     DECISION_DELAY ];
38     opus_int32 Xq_Q10[    DECISION_DELAY ];
39     opus_int32 Pred_Q16[  DECISION_DELAY ];
40     opus_int32 Shape_Q10[ DECISION_DELAY ];
41     opus_int32 sAR2_Q14[ MAX_SHAPE_LPC_ORDER ];
42     opus_int32 LF_AR_Q12;
43     opus_int32 Seed;
44     opus_int32 SeedInit;
45     opus_int32 RD_Q10;
46 } NSQ_del_dec_struct;
47
48 typedef struct {
49     opus_int32 Q_Q10;
50     opus_int32 RD_Q10;
51     opus_int32 xq_Q14;
52     opus_int32 LF_AR_Q12;
53     opus_int32 sLTP_shp_Q10;
54     opus_int32 LPC_exc_Q16;
55 } NSQ_sample_struct;
56
57 static inline void silk_nsq_del_dec_scale_states(
58     const silk_encoder_state *psEncC,               /* I    Encoder State                       */
59     silk_nsq_state      *NSQ,                       /* I/O  NSQ state                           */
60     NSQ_del_dec_struct  psDelDec[],                 /* I/O  Delayed decision states             */
61     const opus_int16     x[],                        /* I    Input in Q0                         */
62     opus_int32           x_sc_Q10[],                 /* O    Input scaled with 1/Gain in Q10     */
63     const opus_int16     sLTP[],                     /* I    Re-whitened LTP state in Q0         */
64     opus_int32           sLTP_Q16[],                 /* O    LTP state matching scaled input     */
65     opus_int             subfr,                      /* I    Subframe number                     */
66     opus_int             nStatesDelayedDecision,     /* I    Number of del dec states            */
67     opus_int             smpl_buf_idx,               /* I    Index to newest samples in buffers  */
68     const opus_int       LTP_scale_Q14,              /* I    LTP state scaling                   */
69     const opus_int32     Gains_Q16[ MAX_NB_SUBFR ],  /* I                                        */
70     const opus_int       pitchL[ MAX_NB_SUBFR ]      /* I    Pitch lag                           */
71 );
72
73 /******************************************/
74 /* Noise shape quantizer for one subframe */
75 /******************************************/
76 static inline void silk_noise_shape_quantizer_del_dec(
77     silk_nsq_state      *NSQ,                   /* I/O  NSQ state                           */
78     NSQ_del_dec_struct  psDelDec[],             /* I/O  Delayed decision states             */
79     opus_int             signalType,             /* I    Signal type                         */
80     const opus_int32     x_Q10[],                /* I                                        */
81     opus_int8            pulses[],               /* O                                        */
82     opus_int16           xq[],                   /* O                                        */
83     opus_int32           sLTP_Q16[],             /* I/O  LTP filter state                    */
84     opus_int32           delayedGain_Q16[],      /* I/O  Gain delay buffer                   */
85     const opus_int16     a_Q12[],                /* I    Short term prediction coefs         */
86     const opus_int16     b_Q14[],                /* I    Long term prediction coefs          */
87     const opus_int16     AR_shp_Q13[],           /* I    Noise shaping coefs                 */
88     opus_int             lag,                    /* I    Pitch lag                           */
89     opus_int32           HarmShapeFIRPacked_Q14, /* I                                        */
90     opus_int             Tilt_Q14,               /* I    Spectral tilt                       */
91     opus_int32           LF_shp_Q14,             /* I                                        */
92     opus_int32           Gain_Q16,               /* I                                        */
93     opus_int             Lambda_Q10,             /* I                                        */
94     opus_int             offset_Q10,             /* I                                        */
95     opus_int             length,                 /* I    Input length                        */
96     opus_int             subfr,                  /* I    Subframe number                     */
97     opus_int             shapingLPCOrder,        /* I    Shaping LPC filter order            */
98     opus_int             predictLPCOrder,        /* I    Prediction filter order             */
99     opus_int             warping_Q16,            /* I                                        */
100     opus_int             nStatesDelayedDecision, /* I    Number of states in decision tree   */
101     opus_int             *smpl_buf_idx,          /* I    Index to newest samples in buffers  */
102     opus_int             decisionDelay           /* I                                        */
103 );
104
105 void silk_NSQ_del_dec(
106     const silk_encoder_state        *psEncC,                                    /* I/O  Encoder State                       */
107     silk_nsq_state                  *NSQ,                                       /* I/O  NSQ state                           */
108     SideInfoIndices                 *psIndices,                                 /* I/O  Quantization Indices                */
109     const opus_int16                 x[],                                        /* I    Prefiltered input signal            */
110     opus_int8                        pulses[],                                   /* O    Quantized pulse signal              */
111     const opus_int16                 PredCoef_Q12[ 2 * MAX_LPC_ORDER ],          /* I    Prediction coefs                    */
112     const opus_int16                 LTPCoef_Q14[ LTP_ORDER * MAX_NB_SUBFR ],    /* I    LT prediction coefs                 */
113     const opus_int16                 AR2_Q13[ MAX_NB_SUBFR * MAX_SHAPE_LPC_ORDER ], /* I                                     */
114     const opus_int                   HarmShapeGain_Q14[ MAX_NB_SUBFR ],          /* I                                        */
115     const opus_int                   Tilt_Q14[ MAX_NB_SUBFR ],                   /* I    Spectral tilt                       */
116     const opus_int32                 LF_shp_Q14[ MAX_NB_SUBFR ],                 /* I                                        */
117     const opus_int32                 Gains_Q16[ MAX_NB_SUBFR ],                  /* I                                        */
118     const opus_int                   pitchL[ MAX_NB_SUBFR ],                     /* I                                        */
119     const opus_int                   Lambda_Q10,                                 /* I                                        */
120     const opus_int                   LTP_scale_Q14                               /* I    LTP state scaling                   */
121 )
122 {
123     opus_int     i, k, lag, start_idx, LSF_interpolation_flag, Winner_ind, subfr;
124     opus_int     last_smple_idx, smpl_buf_idx, decisionDelay;
125     const opus_int16 *A_Q12, *B_Q14, *AR_shp_Q13;
126     opus_int16   *pxq;
127     opus_int32   sLTP_Q16[ 2 * MAX_FRAME_LENGTH ];
128     opus_int16   sLTP[     2 * MAX_FRAME_LENGTH ];
129     opus_int32   HarmShapeFIRPacked_Q14;
130     opus_int     offset_Q10;
131     opus_int32   RDmin_Q10;
132     opus_int32   x_sc_Q10[ MAX_SUB_FRAME_LENGTH ];
133     opus_int32   delayedGain_Q16[  DECISION_DELAY ];
134     NSQ_del_dec_struct psDelDec[ MAX_DEL_DEC_STATES ];
135     NSQ_del_dec_struct *psDD;
136
137     /* Set unvoiced lag to the previous one, overwrite later for voiced */
138     lag = NSQ->lagPrev;
139
140     silk_assert( NSQ->prev_inv_gain_Q16 != 0 );
141
142     /* Initialize delayed decision states */
143     silk_memset( psDelDec, 0, psEncC->nStatesDelayedDecision * sizeof( NSQ_del_dec_struct ) );
144     for( k = 0; k < psEncC->nStatesDelayedDecision; k++ ) {
145         psDD                 = &psDelDec[ k ];
146         psDD->Seed           = ( k + psIndices->Seed ) & 3;
147         psDD->SeedInit       = psDD->Seed;
148         psDD->RD_Q10         = 0;
149         psDD->LF_AR_Q12      = NSQ->sLF_AR_shp_Q12;
150         psDD->Shape_Q10[ 0 ] = NSQ->sLTP_shp_Q10[ psEncC->ltp_mem_length - 1 ];
151         silk_memcpy( psDD->sLPC_Q14, NSQ->sLPC_Q14, NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
152         silk_memcpy( psDD->sAR2_Q14, NSQ->sAR2_Q14, sizeof( NSQ->sAR2_Q14 ) );
153     }
154
155     offset_Q10   = silk_Quantization_Offsets_Q10[ psIndices->signalType >> 1 ][ psIndices->quantOffsetType ];
156     smpl_buf_idx = 0; /* index of oldest samples */
157
158     decisionDelay = silk_min_int( DECISION_DELAY, psEncC->subfr_length );
159
160     /* For voiced frames limit the decision delay to lower than the pitch lag */
161     if( psIndices->signalType == TYPE_VOICED ) {
162         for( k = 0; k < psEncC->nb_subfr; k++ ) {
163             decisionDelay = silk_min_int( decisionDelay, pitchL[ k ] - LTP_ORDER / 2 - 1 );
164         }
165     } else {
166         if( lag > 0 ) {
167             decisionDelay = silk_min_int( decisionDelay, lag - LTP_ORDER / 2 - 1 );
168         }
169     }
170
171     if( psIndices->NLSFInterpCoef_Q2 == 4 ) {
172         LSF_interpolation_flag = 0;
173     } else {
174         LSF_interpolation_flag = 1;
175     }
176
177     /* Setup pointers to start of sub frame */
178     pxq                   = &NSQ->xq[ psEncC->ltp_mem_length ];
179     NSQ->sLTP_shp_buf_idx = psEncC->ltp_mem_length;
180     NSQ->sLTP_buf_idx     = psEncC->ltp_mem_length;
181     subfr = 0;
182     for( k = 0; k < psEncC->nb_subfr; k++ ) {
183         A_Q12      = &PredCoef_Q12[ ( ( k >> 1 ) | ( 1 - LSF_interpolation_flag ) ) * MAX_LPC_ORDER ];
184         B_Q14      = &LTPCoef_Q14[ k * LTP_ORDER           ];
185         AR_shp_Q13 = &AR2_Q13[     k * MAX_SHAPE_LPC_ORDER ];
186
187         /* Noise shape parameters */
188         silk_assert( HarmShapeGain_Q14[ k ] >= 0 );
189         HarmShapeFIRPacked_Q14  =                          silk_RSHIFT( HarmShapeGain_Q14[ k ], 2 );
190         HarmShapeFIRPacked_Q14 |= silk_LSHIFT( ( opus_int32 )silk_RSHIFT( HarmShapeGain_Q14[ k ], 1 ), 16 );
191
192         NSQ->rewhite_flag = 0;
193         if( psIndices->signalType == TYPE_VOICED ) {
194             /* Voiced */
195             lag = pitchL[ k ];
196
197             /* Re-whitening */
198             if( ( k & ( 3 - silk_LSHIFT( LSF_interpolation_flag, 1 ) ) ) == 0 ) {
199                 if( k == 2 ) {
200                     /* RESET DELAYED DECISIONS */
201                     /* Find winner */
202                     RDmin_Q10 = psDelDec[ 0 ].RD_Q10;
203                     Winner_ind = 0;
204                     for( i = 1; i < psEncC->nStatesDelayedDecision; i++ ) {
205                         if( psDelDec[ i ].RD_Q10 < RDmin_Q10 ) {
206                             RDmin_Q10 = psDelDec[ i ].RD_Q10;
207                             Winner_ind = i;
208                         }
209                     }
210                     for( i = 0; i < psEncC->nStatesDelayedDecision; i++ ) {
211                         if( i != Winner_ind ) {
212                             psDelDec[ i ].RD_Q10 += ( silk_int32_MAX >> 4 );
213                             silk_assert( psDelDec[ i ].RD_Q10 >= 0 );
214                         }
215                     }
216
217                     /* Copy final part of signals from winner state to output and long-term filter states */
218                     psDD = &psDelDec[ Winner_ind ];
219                     last_smple_idx = smpl_buf_idx + decisionDelay;
220                     for( i = 0; i < decisionDelay; i++ ) {
221                         last_smple_idx = ( last_smple_idx - 1 ) & DECISION_DELAY_MASK;
222                         pulses[   i - decisionDelay ] = ( opus_int8 )silk_RSHIFT_ROUND( psDD->Q_Q10[ last_smple_idx ], 10 );
223                         pxq[ i - decisionDelay ] = ( opus_int16 )silk_SAT16( silk_RSHIFT_ROUND(
224                             silk_SMULWW( psDD->Xq_Q10[ last_smple_idx ], Gains_Q16[ 1 ] ), 10 ) );
225                         NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - decisionDelay + i ] = psDD->Shape_Q10[ last_smple_idx ];
226                     }
227
228                     subfr = 0;
229                 }
230
231                 /* Rewhiten with new A coefs */
232                 start_idx = psEncC->ltp_mem_length - lag - psEncC->predictLPCOrder - LTP_ORDER / 2;
233                 silk_assert( start_idx > 0 );
234
235                 silk_LPC_analysis_filter( &sLTP[ start_idx ], &NSQ->xq[ start_idx + k * psEncC->subfr_length ],
236                     A_Q12, psEncC->ltp_mem_length - start_idx, psEncC->predictLPCOrder );
237
238                 NSQ->sLTP_buf_idx = psEncC->ltp_mem_length;
239                 NSQ->rewhite_flag = 1;
240             }
241         }
242
243         silk_nsq_del_dec_scale_states( psEncC, NSQ, psDelDec, x, x_sc_Q10, sLTP, sLTP_Q16, k,
244             psEncC->nStatesDelayedDecision, smpl_buf_idx, LTP_scale_Q14, Gains_Q16, pitchL );
245
246         silk_noise_shape_quantizer_del_dec( NSQ, psDelDec, psIndices->signalType, x_sc_Q10, pulses, pxq, sLTP_Q16,
247             delayedGain_Q16, A_Q12, B_Q14, AR_shp_Q13, lag, HarmShapeFIRPacked_Q14, Tilt_Q14[ k ], LF_shp_Q14[ k ],
248             Gains_Q16[ k ], Lambda_Q10, offset_Q10, psEncC->subfr_length, subfr++, psEncC->shapingLPCOrder,
249             psEncC->predictLPCOrder, psEncC->warping_Q16, psEncC->nStatesDelayedDecision, &smpl_buf_idx, decisionDelay );
250
251         x      += psEncC->subfr_length;
252         pulses += psEncC->subfr_length;
253         pxq    += psEncC->subfr_length;
254     }
255
256     /* Find winner */
257     RDmin_Q10 = psDelDec[ 0 ].RD_Q10;
258     Winner_ind = 0;
259     for( k = 1; k < psEncC->nStatesDelayedDecision; k++ ) {
260         if( psDelDec[ k ].RD_Q10 < RDmin_Q10 ) {
261             RDmin_Q10 = psDelDec[ k ].RD_Q10;
262             Winner_ind = k;
263         }
264     }
265
266     /* Copy final part of signals from winner state to output and long-term filter states */
267     psDD = &psDelDec[ Winner_ind ];
268     psIndices->Seed = psDD->SeedInit;
269     last_smple_idx = smpl_buf_idx + decisionDelay;
270     for( i = 0; i < decisionDelay; i++ ) {
271         last_smple_idx = ( last_smple_idx - 1 ) & DECISION_DELAY_MASK;
272         pulses[   i - decisionDelay ] = ( opus_int8 )silk_RSHIFT_ROUND( psDD->Q_Q10[ last_smple_idx ], 10 );
273         pxq[ i - decisionDelay ] = ( opus_int16 )silk_SAT16( silk_RSHIFT_ROUND(
274             silk_SMULWW( psDD->Xq_Q10[ last_smple_idx ], Gains_Q16[ psEncC->nb_subfr - 1 ] ), 10 ) );
275         NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - decisionDelay + i ] = psDD->Shape_Q10[ last_smple_idx ];
276         sLTP_Q16[          NSQ->sLTP_buf_idx     - decisionDelay + i ] = psDD->Pred_Q16[  last_smple_idx ];
277     }
278     silk_memcpy( NSQ->sLPC_Q14, &psDD->sLPC_Q14[ psEncC->subfr_length ], NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
279     silk_memcpy( NSQ->sAR2_Q14, psDD->sAR2_Q14, sizeof( psDD->sAR2_Q14 ) );
280
281     /* Update states */
282     NSQ->sLF_AR_shp_Q12 = psDD->LF_AR_Q12;
283     NSQ->lagPrev        = pitchL[ psEncC->nb_subfr - 1 ];
284
285     /* Save quantized speech and noise shaping signals */
286     silk_memmove( NSQ->xq,           &NSQ->xq[           psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int16 ) );
287     silk_memmove( NSQ->sLTP_shp_Q10, &NSQ->sLTP_shp_Q10[ psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int32 ) );
288
289 #ifdef SAVE_ALL_INTERNAL_DATA
290     DEBUG_STORE_DATA( xq.dat,       &pxq[ -psEncC->frame_length ],       psEncC->frame_length * sizeof( opus_int16 ) );
291     DEBUG_STORE_DATA( q.dat,        &pulses[ -psEncC->frame_length ],    psEncC->frame_length * sizeof( opus_int8 ) );
292     DEBUG_STORE_DATA( sLTP_Q16.dat, &sLTP_Q16[ psEncC->ltp_mem_length ], psEncC->frame_length * sizeof( opus_int32 ) );
293 #endif
294 }
295
296 /******************************************/
297 /* Noise shape quantizer for one subframe */
298 /******************************************/
299 static inline void silk_noise_shape_quantizer_del_dec(
300     silk_nsq_state  *NSQ,                   /* I/O  NSQ state                           */
301     NSQ_del_dec_struct  psDelDec[],             /* I/O  Delayed decision states             */
302     opus_int             signalType,             /* I    Signal type                         */
303     const opus_int32     x_Q10[],                /* I                                        */
304     opus_int8            pulses[],               /* O                                        */
305     opus_int16           xq[],                   /* O                                        */
306     opus_int32           sLTP_Q16[],             /* I/O  LTP filter state                    */
307     opus_int32           delayedGain_Q16[],      /* I/O  Gain delay buffer                   */
308     const opus_int16     a_Q12[],                /* I    Short term prediction coefs         */
309     const opus_int16     b_Q14[],                /* I    Long term prediction coefs          */
310     const opus_int16     AR_shp_Q13[],           /* I    Noise shaping coefs                 */
311     opus_int             lag,                    /* I    Pitch lag                           */
312     opus_int32           HarmShapeFIRPacked_Q14, /* I                                        */
313     opus_int             Tilt_Q14,               /* I    Spectral tilt                       */
314     opus_int32           LF_shp_Q14,             /* I                                        */
315     opus_int32           Gain_Q16,               /* I                                        */
316     opus_int             Lambda_Q10,             /* I                                        */
317     opus_int             offset_Q10,             /* I                                        */
318     opus_int             length,                 /* I    Input length                        */
319     opus_int             subfr,                  /* I    Subframe number                     */
320     opus_int             shapingLPCOrder,        /* I    Shaping LPC filter order            */
321     opus_int             predictLPCOrder,        /* I    Prediction filter order             */
322     opus_int             warping_Q16,            /* I                                        */
323     opus_int             nStatesDelayedDecision, /* I    Number of states in decision tree   */
324     opus_int             *smpl_buf_idx,          /* I    Index to newest samples in buffers  */
325     opus_int             decisionDelay           /* I                                        */
326 )
327 {
328     opus_int     i, j, k, Winner_ind, RDmin_ind, RDmax_ind, last_smple_idx;
329     opus_int32   Winner_rand_state;
330     opus_int32   LTP_pred_Q14, LPC_pred_Q10, n_AR_Q10, n_LTP_Q14, LTP_Q10;
331     opus_int32   n_LF_Q10, r_Q10, rr_Q10, rd1_Q10, rd2_Q10, RDmin_Q10, RDmax_Q10;
332     opus_int32   q1_Q10, q2_Q10, dither, exc_Q10, LPC_exc_Q10, xq_Q10;
333     opus_int32   tmp1, tmp2, sLF_AR_shp_Q10;
334     opus_int32   *pred_lag_ptr, *shp_lag_ptr, *psLPC_Q14;
335     NSQ_sample_struct  psSampleState[ MAX_DEL_DEC_STATES ][ 2 ];
336     NSQ_del_dec_struct *psDD;
337     NSQ_sample_struct  *psSS;
338
339     shp_lag_ptr  = &NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - lag + HARM_SHAPE_FIR_TAPS / 2 ];
340     pred_lag_ptr = &sLTP_Q16[ NSQ->sLTP_buf_idx - lag + LTP_ORDER / 2 ];
341
342     for( i = 0; i < length; i++ ) {
343         /* Perform common calculations used in all states */
344
345         /* Long-term prediction */
346         if( signalType == TYPE_VOICED ) {
347             /* Unrolled loop */
348             LTP_pred_Q14 = silk_SMULWB(               pred_lag_ptr[  0 ], b_Q14[ 0 ] );
349             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -1 ], b_Q14[ 1 ] );
350             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -2 ], b_Q14[ 2 ] );
351             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -3 ], b_Q14[ 3 ] );
352             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -4 ], b_Q14[ 4 ] );
353             pred_lag_ptr++;
354         } else {
355             LTP_pred_Q14 = 0;
356         }
357
358         /* Long-term shaping */
359         if( lag > 0 ) {
360             /* Symmetric, packed FIR coefficients */
361             n_LTP_Q14 = silk_SMULWB( silk_ADD32( shp_lag_ptr[ 0 ], shp_lag_ptr[ -2 ] ), HarmShapeFIRPacked_Q14 );
362             n_LTP_Q14 = silk_SMLAWT( n_LTP_Q14, shp_lag_ptr[ -1 ],                     HarmShapeFIRPacked_Q14 );
363             n_LTP_Q14 = silk_LSHIFT( n_LTP_Q14, 6 );
364             shp_lag_ptr++;
365
366             LTP_Q10 = silk_RSHIFT( silk_SUB32( LTP_pred_Q14, n_LTP_Q14 ), 4 );
367         } else {
368             LTP_Q10 = 0;
369         }
370
371         for( k = 0; k < nStatesDelayedDecision; k++ ) {
372             /* Delayed decision state */
373             psDD = &psDelDec[ k ];
374
375             /* Sample state */
376             psSS = psSampleState[ k ];
377
378             /* Generate dither */
379             psDD->Seed = silk_RAND( psDD->Seed );
380
381             /* dither = rand_seed < 0 ? 0xFFFFFFFF : 0; */
382             dither = silk_RSHIFT( psDD->Seed, 31 );
383
384             /* Pointer used in short term prediction and shaping */
385             psLPC_Q14 = &psDD->sLPC_Q14[ NSQ_LPC_BUF_LENGTH - 1 + i ];
386             /* Short-term prediction */
387             silk_assert( predictLPCOrder >= 10 );            /* check that unrolling works */
388             silk_assert( ( predictLPCOrder  & 1 ) == 0 );    /* check that order is even */
389             silk_assert( ( (opus_int64)a_Q12 & 3 ) == 0 );    /* check that array starts at 4-byte aligned address */
390             /* Partially unrolled */
391             LPC_pred_Q10 = silk_SMULWB(               psLPC_Q14[  0 ], a_Q12[ 0 ] );
392             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -1 ], a_Q12[ 1 ] );
393             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -2 ], a_Q12[ 2 ] );
394             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -3 ], a_Q12[ 3 ] );
395             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -4 ], a_Q12[ 4 ] );
396             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -5 ], a_Q12[ 5 ] );
397             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -6 ], a_Q12[ 6 ] );
398             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -7 ], a_Q12[ 7 ] );
399             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -8 ], a_Q12[ 8 ] );
400             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -9 ], a_Q12[ 9 ] );
401             for( j = 10; j < predictLPCOrder; j ++ ) {
402                 LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -j ], a_Q12[ j ] );
403             }
404
405             /* Noise shape feedback */
406             silk_assert( ( shapingLPCOrder & 1 ) == 0 );   /* check that order is even */
407             /* Output of lowpass section */
408             tmp2 = silk_SMLAWB( psLPC_Q14[ 0 ], psDD->sAR2_Q14[ 0 ], warping_Q16 );
409             /* Output of allpass section */
410             tmp1 = silk_SMLAWB( psDD->sAR2_Q14[ 0 ], psDD->sAR2_Q14[ 1 ] - tmp2, warping_Q16 );
411             psDD->sAR2_Q14[ 0 ] = tmp2;
412             n_AR_Q10 = silk_SMULWB( tmp2, AR_shp_Q13[ 0 ] );
413             /* Loop over allpass sections */
414             for( j = 2; j < shapingLPCOrder; j += 2 ) {
415                 /* Output of allpass section */
416                 tmp2 = silk_SMLAWB( psDD->sAR2_Q14[ j - 1 ], psDD->sAR2_Q14[ j + 0 ] - tmp1, warping_Q16 );
417                 psDD->sAR2_Q14[ j - 1 ] = tmp1;
418                 n_AR_Q10 = silk_SMLAWB( n_AR_Q10, tmp1, AR_shp_Q13[ j - 1 ] );
419                 /* Output of allpass section */
420                 tmp1 = silk_SMLAWB( psDD->sAR2_Q14[ j + 0 ], psDD->sAR2_Q14[ j + 1 ] - tmp2, warping_Q16 );
421                 psDD->sAR2_Q14[ j + 0 ] = tmp2;
422                 n_AR_Q10 = silk_SMLAWB( n_AR_Q10, tmp2, AR_shp_Q13[ j ] );
423             }
424             psDD->sAR2_Q14[ shapingLPCOrder - 1 ] = tmp1;
425             n_AR_Q10 = silk_SMLAWB( n_AR_Q10, tmp1, AR_shp_Q13[ shapingLPCOrder - 1 ] );
426
427             n_AR_Q10 = silk_RSHIFT( n_AR_Q10, 1 );           /* Q11 -> Q10 */
428             n_AR_Q10 = silk_SMLAWB( n_AR_Q10, psDD->LF_AR_Q12, Tilt_Q14 );
429
430             n_LF_Q10 = silk_LSHIFT( silk_SMULWB( psDD->Shape_Q10[ *smpl_buf_idx ], LF_shp_Q14 ), 2 );
431             n_LF_Q10 = silk_SMLAWT( n_LF_Q10, psDD->LF_AR_Q12, LF_shp_Q14 );
432
433             /* Input minus prediction plus noise feedback                       */
434             /* r = x[ i ] - LTP_pred - LPC_pred + n_AR + n_Tilt + n_LF + n_LTP  */
435             tmp1  = silk_ADD32( LTP_Q10, LPC_pred_Q10 );                         /* add Q10 stuff */
436             tmp1  = silk_SUB32( tmp1, n_AR_Q10 );                                /* subtract Q10 stuff */
437             tmp1  = silk_SUB32( tmp1, n_LF_Q10 );                                /* subtract Q10 stuff */
438             r_Q10 = silk_SUB32( x_Q10[ i ], tmp1 );                              /* residual error Q10 */
439
440             /* Flip sign depending on dither */
441             r_Q10 = r_Q10 ^ dither;
442             r_Q10 = silk_LIMIT_32( r_Q10, -31 << 10, 30 << 10 );
443
444             /* Find two quantization level candidates and measure their rate-distortion */
445             q1_Q10 = silk_SUB32( r_Q10, offset_Q10 );
446             q1_Q10 = silk_RSHIFT( q1_Q10, 10 );
447             if( q1_Q10 > 0 ) {
448                 q1_Q10  = silk_SUB32( silk_LSHIFT( q1_Q10, 10 ), QUANT_LEVEL_ADJUST_Q10 );
449                 q1_Q10  = silk_ADD32( q1_Q10, offset_Q10 );
450                 q2_Q10  = silk_ADD32( q1_Q10, 1024 );
451                 rd1_Q10 = silk_SMULBB( q1_Q10, Lambda_Q10 );
452                 rd2_Q10 = silk_SMULBB( q2_Q10, Lambda_Q10 );
453             } else if( q1_Q10 == 0 ) {
454                 q1_Q10  = offset_Q10;
455                 q2_Q10  = silk_ADD32( q1_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
456                 rd1_Q10 = silk_SMULBB( q1_Q10, Lambda_Q10 );
457                 rd2_Q10 = silk_SMULBB( q2_Q10, Lambda_Q10 );
458             } else if( q1_Q10 == -1 ) {
459                 q2_Q10  = offset_Q10;
460                 q1_Q10  = silk_SUB32( q2_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
461                 rd1_Q10 = silk_SMULBB( -q1_Q10, Lambda_Q10 );
462                 rd2_Q10 = silk_SMULBB(  q2_Q10, Lambda_Q10 );
463             } else {            /* Q1_Q10 < -1 */
464                 q1_Q10  = silk_ADD32( silk_LSHIFT( q1_Q10, 10 ), QUANT_LEVEL_ADJUST_Q10 );
465                 q1_Q10  = silk_ADD32( q1_Q10, offset_Q10 );
466                 q2_Q10  = silk_ADD32( q1_Q10, 1024 );
467                 rd1_Q10 = silk_SMULBB( -q1_Q10, Lambda_Q10 );
468                 rd2_Q10 = silk_SMULBB( -q2_Q10, Lambda_Q10 );
469             }
470             rr_Q10  = silk_SUB32( r_Q10, q1_Q10 );
471             rd1_Q10 = silk_RSHIFT( silk_SMLABB( rd1_Q10, rr_Q10, rr_Q10 ), 10 );
472             rr_Q10  = silk_SUB32( r_Q10, q2_Q10 );
473             rd2_Q10 = silk_RSHIFT( silk_SMLABB( rd2_Q10, rr_Q10, rr_Q10 ), 10 );
474
475             if( rd1_Q10 < rd2_Q10 ) {
476                 psSS[ 0 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd1_Q10 );
477                 psSS[ 1 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd2_Q10 );
478                 psSS[ 0 ].Q_Q10  = q1_Q10;
479                 psSS[ 1 ].Q_Q10  = q2_Q10;
480             } else {
481                 psSS[ 0 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd2_Q10 );
482                 psSS[ 1 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd1_Q10 );
483                 psSS[ 0 ].Q_Q10  = q2_Q10;
484                 psSS[ 1 ].Q_Q10  = q1_Q10;
485             }
486
487             /* Update states for best quantization */
488
489             /* Quantized excitation */
490             exc_Q10 = psSS[ 0 ].Q_Q10 ^ dither;
491
492             /* Add predictions */
493             LPC_exc_Q10 = exc_Q10 + silk_RSHIFT_ROUND( LTP_pred_Q14, 4 );
494             xq_Q10      = silk_ADD32( LPC_exc_Q10, LPC_pred_Q10 );
495
496             /* Update states */
497             sLF_AR_shp_Q10         = silk_SUB32(  xq_Q10, n_AR_Q10 );
498             psSS[ 0 ].sLTP_shp_Q10 = silk_SUB32(  sLF_AR_shp_Q10, n_LF_Q10 );
499             psSS[ 0 ].LF_AR_Q12    = silk_LSHIFT( sLF_AR_shp_Q10, 2 );
500             psSS[ 0 ].xq_Q14       = silk_LSHIFT( xq_Q10,         4 );
501             psSS[ 0 ].LPC_exc_Q16  = silk_LSHIFT( LPC_exc_Q10,    6 );
502
503             /* Update states for second best quantization */
504
505             /* Quantized excitation */
506             exc_Q10 = psSS[ 1 ].Q_Q10 ^ dither;
507
508             /* Add predictions */
509             LPC_exc_Q10 = exc_Q10 + silk_RSHIFT_ROUND( LTP_pred_Q14, 4 );
510             xq_Q10      = silk_ADD32( LPC_exc_Q10, LPC_pred_Q10 );
511
512             /* Update states */
513             sLF_AR_shp_Q10         = silk_SUB32(  xq_Q10, n_AR_Q10 );
514             psSS[ 1 ].sLTP_shp_Q10 = silk_SUB32(  sLF_AR_shp_Q10, n_LF_Q10 );
515             psSS[ 1 ].LF_AR_Q12    = silk_LSHIFT( sLF_AR_shp_Q10, 2 );
516             psSS[ 1 ].xq_Q14       = silk_LSHIFT( xq_Q10,         4 );
517             psSS[ 1 ].LPC_exc_Q16  = silk_LSHIFT( LPC_exc_Q10,    6 );
518         }
519
520         *smpl_buf_idx  = ( *smpl_buf_idx - 1 ) & DECISION_DELAY_MASK;                   /* Index to newest samples              */
521         last_smple_idx = ( *smpl_buf_idx + decisionDelay ) & DECISION_DELAY_MASK;       /* Index to decisionDelay old samples   */
522
523         /* Find winner */
524         RDmin_Q10 = psSampleState[ 0 ][ 0 ].RD_Q10;
525         Winner_ind = 0;
526         for( k = 1; k < nStatesDelayedDecision; k++ ) {
527             if( psSampleState[ k ][ 0 ].RD_Q10 < RDmin_Q10 ) {
528                 RDmin_Q10  = psSampleState[ k ][ 0 ].RD_Q10;
529                 Winner_ind = k;
530             }
531         }
532
533         /* Increase RD values of expired states */
534         Winner_rand_state = psDelDec[ Winner_ind ].RandState[ last_smple_idx ];
535         for( k = 0; k < nStatesDelayedDecision; k++ ) {
536             if( psDelDec[ k ].RandState[ last_smple_idx ] != Winner_rand_state ) {
537                 psSampleState[ k ][ 0 ].RD_Q10 = silk_ADD32( psSampleState[ k ][ 0 ].RD_Q10, ( silk_int32_MAX >> 4 ) );
538                 psSampleState[ k ][ 1 ].RD_Q10 = silk_ADD32( psSampleState[ k ][ 1 ].RD_Q10, ( silk_int32_MAX >> 4 ) );
539                 silk_assert( psSampleState[ k ][ 0 ].RD_Q10 >= 0 );
540             }
541         }
542
543         /* Find worst in first set and best in second set */
544         RDmax_Q10  = psSampleState[ 0 ][ 0 ].RD_Q10;
545         RDmin_Q10  = psSampleState[ 0 ][ 1 ].RD_Q10;
546         RDmax_ind = 0;
547         RDmin_ind = 0;
548         for( k = 1; k < nStatesDelayedDecision; k++ ) {
549             /* find worst in first set */
550             if( psSampleState[ k ][ 0 ].RD_Q10 > RDmax_Q10 ) {
551                 RDmax_Q10  = psSampleState[ k ][ 0 ].RD_Q10;
552                 RDmax_ind = k;
553             }
554             /* find best in second set */
555             if( psSampleState[ k ][ 1 ].RD_Q10 < RDmin_Q10 ) {
556                 RDmin_Q10  = psSampleState[ k ][ 1 ].RD_Q10;
557                 RDmin_ind = k;
558             }
559         }
560
561         /* Replace a state if best from second set outperforms worst in first set */
562         if( RDmin_Q10 < RDmax_Q10 ) {
563             silk_memcpy( ((opus_int32 *)&psDelDec[ RDmax_ind ]) + i,
564                         ((opus_int32 *)&psDelDec[ RDmin_ind ]) + i, sizeof( NSQ_del_dec_struct ) - i * sizeof( opus_int32) );
565             silk_memcpy( &psSampleState[ RDmax_ind ][ 0 ], &psSampleState[ RDmin_ind ][ 1 ], sizeof( NSQ_sample_struct ) );
566         }
567
568         /* Write samples from winner to output and long-term filter states */
569         psDD = &psDelDec[ Winner_ind ];
570         if( subfr > 0 || i >= decisionDelay ) {
571             pulses[  i - decisionDelay ] = ( opus_int8 )silk_RSHIFT_ROUND( psDD->Q_Q10[ last_smple_idx ], 10 );
572             xq[ i - decisionDelay ] = ( opus_int16 )silk_SAT16( silk_RSHIFT_ROUND(
573                 silk_SMULWW( psDD->Xq_Q10[ last_smple_idx ], delayedGain_Q16[ last_smple_idx ] ), 10 ) );
574             NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - decisionDelay ] = psDD->Shape_Q10[ last_smple_idx ];
575             sLTP_Q16[          NSQ->sLTP_buf_idx     - decisionDelay ] = psDD->Pred_Q16[  last_smple_idx ];
576         }
577         NSQ->sLTP_shp_buf_idx++;
578         NSQ->sLTP_buf_idx++;
579
580         /* Update states */
581         for( k = 0; k < nStatesDelayedDecision; k++ ) {
582             psDD                                     = &psDelDec[ k ];
583             psSS                                     = &psSampleState[ k ][ 0 ];
584             psDD->LF_AR_Q12                          = psSS->LF_AR_Q12;
585             psDD->sLPC_Q14[ NSQ_LPC_BUF_LENGTH + i ] = psSS->xq_Q14;
586             psDD->Xq_Q10[    *smpl_buf_idx ]         = silk_RSHIFT( psSS->xq_Q14, 4 );
587             psDD->Q_Q10[     *smpl_buf_idx ]         = psSS->Q_Q10;
588             psDD->Pred_Q16[  *smpl_buf_idx ]         = psSS->LPC_exc_Q16;
589             psDD->Shape_Q10[ *smpl_buf_idx ]         = psSS->sLTP_shp_Q10;
590             psDD->Seed                               = silk_ADD32( psDD->Seed, silk_RSHIFT_ROUND( psSS->Q_Q10, 10 ) );
591             psDD->RandState[ *smpl_buf_idx ]         = psDD->Seed;
592             psDD->RD_Q10                             = psSS->RD_Q10;
593         }
594         delayedGain_Q16[     *smpl_buf_idx ]         = Gain_Q16;
595     }
596     /* Update LPC states */
597     for( k = 0; k < nStatesDelayedDecision; k++ ) {
598         psDD = &psDelDec[ k ];
599         silk_memcpy( psDD->sLPC_Q14, &psDD->sLPC_Q14[ length ], NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
600     }
601 }
602
603 static inline void silk_nsq_del_dec_scale_states(
604     const silk_encoder_state *psEncC,               /* I    Encoder State                       */
605     silk_nsq_state      *NSQ,                       /* I/O  NSQ state                           */
606     NSQ_del_dec_struct  psDelDec[],                 /* I/O  Delayed decision states             */
607     const opus_int16     x[],                        /* I    Input in Q0                         */
608     opus_int32           x_sc_Q10[],                 /* O    Input scaled with 1/Gain in Q10     */
609     const opus_int16     sLTP[],                     /* I    Re-whitened LTP state in Q0         */
610     opus_int32           sLTP_Q16[],                 /* O    LTP state matching scaled input     */
611     opus_int             subfr,                      /* I    Subframe number                     */
612     opus_int             nStatesDelayedDecision,     /* I    Number of del dec states            */
613     opus_int             smpl_buf_idx,               /* I    Index to newest samples in buffers  */
614     const opus_int       LTP_scale_Q14,              /* I    LTP state scaling                   */
615     const opus_int32     Gains_Q16[ MAX_NB_SUBFR ],  /* I                                        */
616     const opus_int       pitchL[ MAX_NB_SUBFR ]      /* I    Pitch lag                           */
617 )
618 {
619     opus_int            i, k, lag;
620     opus_int32          inv_gain_Q16, gain_adj_Q16, inv_gain_Q32;
621     NSQ_del_dec_struct *psDD;
622
623     inv_gain_Q16 = silk_INVERSE32_varQ( silk_max( Gains_Q16[ subfr ], 1 ), 32 );
624     inv_gain_Q16 = silk_min( inv_gain_Q16, silk_int16_MAX );
625     lag          = pitchL[ subfr ];
626
627     /* After rewhitening the LTP state is un-scaled, so scale with inv_gain_Q16 */
628     if( NSQ->rewhite_flag ) {
629         inv_gain_Q32 = silk_LSHIFT( inv_gain_Q16, 16 );
630         if( subfr == 0 ) {
631             /* Do LTP downscaling */
632             inv_gain_Q32 = silk_LSHIFT( silk_SMULWB( inv_gain_Q32, LTP_scale_Q14 ), 2 );
633         }
634         for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {
635             silk_assert( i < MAX_FRAME_LENGTH );
636             sLTP_Q16[ i ] = silk_SMULWB( inv_gain_Q32, sLTP[ i ] );
637         }
638     }
639
640     /* Adjust for changing gain */
641     if( inv_gain_Q16 != NSQ->prev_inv_gain_Q16 ) {
642         gain_adj_Q16 = silk_DIV32_varQ( inv_gain_Q16, NSQ->prev_inv_gain_Q16, 16 );
643
644         /* Scale long-term shaping state */
645         for( i = NSQ->sLTP_shp_buf_idx - psEncC->ltp_mem_length; i < NSQ->sLTP_shp_buf_idx; i++ ) {
646             NSQ->sLTP_shp_Q10[ i ] = silk_SMULWW( gain_adj_Q16, NSQ->sLTP_shp_Q10[ i ] );
647         }
648
649         /* Scale long-term prediction state */
650         if( NSQ->rewhite_flag == 0 ) {
651             for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {
652                 sLTP_Q16[ i ] = silk_SMULWW( gain_adj_Q16, sLTP_Q16[ i ] );
653             }
654         }
655
656         for( k = 0; k < nStatesDelayedDecision; k++ ) {
657             psDD = &psDelDec[ k ];
658
659             /* Scale scalar states */
660             psDD->LF_AR_Q12 = silk_SMULWW( gain_adj_Q16, psDD->LF_AR_Q12 );
661
662             /* Scale short-term prediction and shaping states */
663             for( i = 0; i < NSQ_LPC_BUF_LENGTH; i++ ) {
664                 psDD->sLPC_Q14[ i ] = silk_SMULWW( gain_adj_Q16, psDD->sLPC_Q14[ i ] );
665             }
666             for( i = 0; i < MAX_SHAPE_LPC_ORDER; i++ ) {
667                 psDD->sAR2_Q14[ i ] = silk_SMULWW( gain_adj_Q16, psDD->sAR2_Q14[ i ] );
668             }
669             for( i = 0; i < DECISION_DELAY; i++ ) {
670                 psDD->Pred_Q16[  i ] = silk_SMULWW( gain_adj_Q16, psDD->Pred_Q16[  i ] );
671                 psDD->Shape_Q10[ i ] = silk_SMULWW( gain_adj_Q16, psDD->Shape_Q10[ i ] );
672             }
673         }
674     }
675
676     /* Scale input */
677     for( i = 0; i < psEncC->subfr_length; i++ ) {
678         x_sc_Q10[ i ] = silk_RSHIFT( silk_SMULBB( x[ i ], ( opus_int16 )inv_gain_Q16 ), 6 );
679     }
680
681     /* save inv_gain */
682     silk_assert( inv_gain_Q16 != 0 );
683     NSQ->prev_inv_gain_Q16 = inv_gain_Q16;
684 }