Clean up: replace tabs by spaces
[opus.git] / 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, 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 "NSQ.h"
35
36
37 typedef struct {
38     opus_int32 sLPC_Q14[ MAX_SUB_FRAME_LENGTH + NSQ_LPC_BUF_LENGTH ];
39     opus_int32 RandState[ DECISION_DELAY ];
40     opus_int32 Q_Q10[     DECISION_DELAY ];
41     opus_int32 Xq_Q14[    DECISION_DELAY ];
42     opus_int32 Pred_Q15[  DECISION_DELAY ];
43     opus_int32 Shape_Q14[ DECISION_DELAY ];
44     opus_int32 sAR2_Q14[ MAX_SHAPE_LPC_ORDER ];
45     opus_int32 LF_AR_Q14;
46     opus_int32 Diff_Q14;
47     opus_int32 Seed;
48     opus_int32 SeedInit;
49     opus_int32 RD_Q10;
50 } NSQ_del_dec_struct;
51
52 typedef struct {
53     opus_int32 Q_Q10;
54     opus_int32 RD_Q10;
55     opus_int32 xq_Q14;
56     opus_int32 LF_AR_Q14;
57     opus_int32 Diff_Q14;
58     opus_int32 sLTP_shp_Q14;
59     opus_int32 LPC_exc_Q14;
60 } NSQ_sample_struct;
61
62 typedef NSQ_sample_struct  NSQ_sample_pair[ 2 ];
63
64 #if defined(MIPSr1_ASM)
65 #include "mips/NSQ_del_dec_mipsr1.h"
66 #endif
67 static OPUS_INLINE void silk_nsq_del_dec_scale_states(
68     const silk_encoder_state *psEncC,               /* I    Encoder State                       */
69     silk_nsq_state      *NSQ,                       /* I/O  NSQ state                           */
70     NSQ_del_dec_struct  psDelDec[],                 /* I/O  Delayed decision states             */
71     const opus_int16    x16[],                      /* I    Input                               */
72     opus_int32          x_sc_Q10[],                 /* O    Input scaled with 1/Gain in Q10     */
73     const opus_int16    sLTP[],                     /* I    Re-whitened LTP state in Q0         */
74     opus_int32          sLTP_Q15[],                 /* O    LTP state matching scaled input     */
75     opus_int            subfr,                      /* I    Subframe number                     */
76     opus_int            nStatesDelayedDecision,     /* I    Number of del dec states            */
77     const opus_int      LTP_scale_Q14,              /* I    LTP state scaling                   */
78     const opus_int32    Gains_Q16[ MAX_NB_SUBFR ],  /* I                                        */
79     const opus_int      pitchL[ MAX_NB_SUBFR ],     /* I    Pitch lag                           */
80     const opus_int      signal_type,                /* I    Signal type                         */
81     const opus_int      decisionDelay               /* I    Decision delay                      */
82 );
83
84 /******************************************/
85 /* Noise shape quantizer for one subframe */
86 /******************************************/
87 static OPUS_INLINE void silk_noise_shape_quantizer_del_dec(
88     silk_nsq_state      *NSQ,                   /* I/O  NSQ state                           */
89     NSQ_del_dec_struct  psDelDec[],             /* I/O  Delayed decision states             */
90     opus_int            signalType,             /* I    Signal type                         */
91     const opus_int32    x_Q10[],                /* I                                        */
92     opus_int8           pulses[],               /* O                                        */
93     opus_int16          xq[],                   /* O                                        */
94     opus_int32          sLTP_Q15[],             /* I/O  LTP filter state                    */
95     opus_int32          delayedGain_Q10[],      /* I/O  Gain delay buffer                   */
96     const opus_int16    a_Q12[],                /* I    Short term prediction coefs         */
97     const opus_int16    b_Q14[],                /* I    Long term prediction coefs          */
98     const opus_int16    AR_shp_Q13[],           /* I    Noise shaping coefs                 */
99     opus_int            lag,                    /* I    Pitch lag                           */
100     opus_int32          HarmShapeFIRPacked_Q14, /* I                                        */
101     opus_int            Tilt_Q14,               /* I    Spectral tilt                       */
102     opus_int32          LF_shp_Q14,             /* I                                        */
103     opus_int32          Gain_Q16,               /* I                                        */
104     opus_int            Lambda_Q10,             /* I                                        */
105     opus_int            offset_Q10,             /* I                                        */
106     opus_int            length,                 /* I    Input length                        */
107     opus_int            subfr,                  /* I    Subframe number                     */
108     opus_int            shapingLPCOrder,        /* I    Shaping LPC filter order            */
109     opus_int            predictLPCOrder,        /* I    Prediction filter order             */
110     opus_int            warping_Q16,            /* I                                        */
111     opus_int            nStatesDelayedDecision, /* I    Number of states in decision tree   */
112     opus_int            *smpl_buf_idx,          /* I    Index to newest samples in buffers  */
113     opus_int            decisionDelay,          /* I                                        */
114     int                 arch                    /* I                                        */
115 );
116
117 void silk_NSQ_del_dec_c(
118     const silk_encoder_state    *psEncC,                                    /* I/O  Encoder State                   */
119     silk_nsq_state              *NSQ,                                       /* I/O  NSQ state                       */
120     SideInfoIndices             *psIndices,                                 /* I/O  Quantization Indices            */
121     const opus_int16            x16[],                                        /* I    Input                           */
122     opus_int8                   pulses[],                                   /* O    Quantized pulse signal          */
123     const opus_int16            PredCoef_Q12[ 2 * MAX_LPC_ORDER ],          /* I    Short term prediction coefs     */
124     const opus_int16            LTPCoef_Q14[ LTP_ORDER * MAX_NB_SUBFR ],    /* I    Long term prediction coefs      */
125     const opus_int16            AR_Q13[ MAX_NB_SUBFR * MAX_SHAPE_LPC_ORDER ], /* I Noise shaping coefs              */
126     const opus_int              HarmShapeGain_Q14[ MAX_NB_SUBFR ],          /* I    Long term shaping coefs         */
127     const opus_int              Tilt_Q14[ MAX_NB_SUBFR ],                   /* I    Spectral tilt                   */
128     const opus_int32            LF_shp_Q14[ MAX_NB_SUBFR ],                 /* I    Low frequency shaping coefs     */
129     const opus_int32            Gains_Q16[ MAX_NB_SUBFR ],                  /* I    Quantization step sizes         */
130     const opus_int              pitchL[ MAX_NB_SUBFR ],                     /* I    Pitch lags                      */
131     const opus_int              Lambda_Q10,                                 /* I    Rate/distortion tradeoff        */
132     const opus_int              LTP_scale_Q14                               /* I    LTP state scaling               */
133 )
134 {
135     opus_int            i, k, lag, start_idx, LSF_interpolation_flag, Winner_ind, subfr;
136     opus_int            last_smple_idx, smpl_buf_idx, decisionDelay;
137     const opus_int16    *A_Q12, *B_Q14, *AR_shp_Q13;
138     opus_int16          *pxq;
139     VARDECL( opus_int32, sLTP_Q15 );
140     VARDECL( opus_int16, sLTP );
141     opus_int32          HarmShapeFIRPacked_Q14;
142     opus_int            offset_Q10;
143     opus_int32          RDmin_Q10, Gain_Q10;
144     VARDECL( opus_int32, x_sc_Q10 );
145     VARDECL( opus_int32, delayedGain_Q10 );
146     VARDECL( NSQ_del_dec_struct, psDelDec );
147     NSQ_del_dec_struct  *psDD;
148     SAVE_STACK;
149
150     /* Set unvoiced lag to the previous one, overwrite later for voiced */
151     lag = NSQ->lagPrev;
152
153     silk_assert( NSQ->prev_gain_Q16 != 0 );
154
155     /* Initialize delayed decision states */
156     ALLOC( psDelDec, psEncC->nStatesDelayedDecision, NSQ_del_dec_struct );
157     silk_memset( psDelDec, 0, psEncC->nStatesDelayedDecision * sizeof( NSQ_del_dec_struct ) );
158     for( k = 0; k < psEncC->nStatesDelayedDecision; k++ ) {
159         psDD                 = &psDelDec[ k ];
160         psDD->Seed           = ( k + psIndices->Seed ) & 3;
161         psDD->SeedInit       = psDD->Seed;
162         psDD->RD_Q10         = 0;
163         psDD->LF_AR_Q14      = NSQ->sLF_AR_shp_Q14;
164         psDD->Diff_Q14       = NSQ->sDiff_shp_Q14;
165         psDD->Shape_Q14[ 0 ] = NSQ->sLTP_shp_Q14[ psEncC->ltp_mem_length - 1 ];
166         silk_memcpy( psDD->sLPC_Q14, NSQ->sLPC_Q14, NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
167         silk_memcpy( psDD->sAR2_Q14, NSQ->sAR2_Q14, sizeof( NSQ->sAR2_Q14 ) );
168     }
169
170     offset_Q10   = silk_Quantization_Offsets_Q10[ psIndices->signalType >> 1 ][ psIndices->quantOffsetType ];
171     smpl_buf_idx = 0; /* index of oldest samples */
172
173     decisionDelay = silk_min_int( DECISION_DELAY, psEncC->subfr_length );
174
175     /* For voiced frames limit the decision delay to lower than the pitch lag */
176     if( psIndices->signalType == TYPE_VOICED ) {
177         for( k = 0; k < psEncC->nb_subfr; k++ ) {
178             decisionDelay = silk_min_int( decisionDelay, pitchL[ k ] - LTP_ORDER / 2 - 1 );
179         }
180     } else {
181         if( lag > 0 ) {
182             decisionDelay = silk_min_int( decisionDelay, lag - LTP_ORDER / 2 - 1 );
183         }
184     }
185
186     if( psIndices->NLSFInterpCoef_Q2 == 4 ) {
187         LSF_interpolation_flag = 0;
188     } else {
189         LSF_interpolation_flag = 1;
190     }
191
192     ALLOC( sLTP_Q15, psEncC->ltp_mem_length + psEncC->frame_length, opus_int32 );
193     ALLOC( sLTP, psEncC->ltp_mem_length + psEncC->frame_length, opus_int16 );
194     ALLOC( x_sc_Q10, psEncC->subfr_length, opus_int32 );
195     ALLOC( delayedGain_Q10, DECISION_DELAY, opus_int32 );
196     /* Set up pointers to start of sub frame */
197     pxq                   = &NSQ->xq[ psEncC->ltp_mem_length ];
198     NSQ->sLTP_shp_buf_idx = psEncC->ltp_mem_length;
199     NSQ->sLTP_buf_idx     = psEncC->ltp_mem_length;
200     subfr = 0;
201     for( k = 0; k < psEncC->nb_subfr; k++ ) {
202         A_Q12      = &PredCoef_Q12[ ( ( k >> 1 ) | ( 1 - LSF_interpolation_flag ) ) * MAX_LPC_ORDER ];
203         B_Q14      = &LTPCoef_Q14[ k * LTP_ORDER           ];
204         AR_shp_Q13 = &AR_Q13[     k * MAX_SHAPE_LPC_ORDER ];
205
206         /* Noise shape parameters */
207         silk_assert( HarmShapeGain_Q14[ k ] >= 0 );
208         HarmShapeFIRPacked_Q14  =                          silk_RSHIFT( HarmShapeGain_Q14[ k ], 2 );
209         HarmShapeFIRPacked_Q14 |= silk_LSHIFT( (opus_int32)silk_RSHIFT( HarmShapeGain_Q14[ k ], 1 ), 16 );
210
211         NSQ->rewhite_flag = 0;
212         if( psIndices->signalType == TYPE_VOICED ) {
213             /* Voiced */
214             lag = pitchL[ k ];
215
216             /* Re-whitening */
217             if( ( k & ( 3 - silk_LSHIFT( LSF_interpolation_flag, 1 ) ) ) == 0 ) {
218                 if( k == 2 ) {
219                     /* RESET DELAYED DECISIONS */
220                     /* Find winner */
221                     RDmin_Q10 = psDelDec[ 0 ].RD_Q10;
222                     Winner_ind = 0;
223                     for( i = 1; i < psEncC->nStatesDelayedDecision; i++ ) {
224                         if( psDelDec[ i ].RD_Q10 < RDmin_Q10 ) {
225                             RDmin_Q10 = psDelDec[ i ].RD_Q10;
226                             Winner_ind = i;
227                         }
228                     }
229                     for( i = 0; i < psEncC->nStatesDelayedDecision; i++ ) {
230                         if( i != Winner_ind ) {
231                             psDelDec[ i ].RD_Q10 += ( silk_int32_MAX >> 4 );
232                             silk_assert( psDelDec[ i ].RD_Q10 >= 0 );
233                         }
234                     }
235
236                     /* Copy final part of signals from winner state to output and long-term filter states */
237                     psDD = &psDelDec[ Winner_ind ];
238                     last_smple_idx = smpl_buf_idx + decisionDelay;
239                     for( i = 0; i < decisionDelay; i++ ) {
240                         last_smple_idx = ( last_smple_idx - 1 ) & DECISION_DELAY_MASK;
241                         pulses[   i - decisionDelay ] = (opus_int8)silk_RSHIFT_ROUND( psDD->Q_Q10[ last_smple_idx ], 10 );
242                         pxq[ i - decisionDelay ] = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND(
243                             silk_SMULWW( psDD->Xq_Q14[ last_smple_idx ], Gains_Q16[ 1 ] ), 14 ) );
244                         NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - decisionDelay + i ] = psDD->Shape_Q14[ last_smple_idx ];
245                     }
246
247                     subfr = 0;
248                 }
249
250                 /* Rewhiten with new A coefs */
251                 start_idx = psEncC->ltp_mem_length - lag - psEncC->predictLPCOrder - LTP_ORDER / 2;
252                 silk_assert( start_idx > 0 );
253
254                 silk_LPC_analysis_filter( &sLTP[ start_idx ], &NSQ->xq[ start_idx + k * psEncC->subfr_length ],
255                     A_Q12, psEncC->ltp_mem_length - start_idx, psEncC->predictLPCOrder, psEncC->arch );
256
257                 NSQ->sLTP_buf_idx = psEncC->ltp_mem_length;
258                 NSQ->rewhite_flag = 1;
259             }
260         }
261
262         silk_nsq_del_dec_scale_states( psEncC, NSQ, psDelDec, x16, x_sc_Q10, sLTP, sLTP_Q15, k,
263             psEncC->nStatesDelayedDecision, LTP_scale_Q14, Gains_Q16, pitchL, psIndices->signalType, decisionDelay );
264
265         silk_noise_shape_quantizer_del_dec( NSQ, psDelDec, psIndices->signalType, x_sc_Q10, pulses, pxq, sLTP_Q15,
266             delayedGain_Q10, A_Q12, B_Q14, AR_shp_Q13, lag, HarmShapeFIRPacked_Q14, Tilt_Q14[ k ], LF_shp_Q14[ k ],
267             Gains_Q16[ k ], Lambda_Q10, offset_Q10, psEncC->subfr_length, subfr++, psEncC->shapingLPCOrder,
268             psEncC->predictLPCOrder, psEncC->warping_Q16, psEncC->nStatesDelayedDecision, &smpl_buf_idx, decisionDelay, psEncC->arch );
269
270         x16    += psEncC->subfr_length;
271         pulses += psEncC->subfr_length;
272         pxq    += psEncC->subfr_length;
273     }
274
275     /* Find winner */
276     RDmin_Q10 = psDelDec[ 0 ].RD_Q10;
277     Winner_ind = 0;
278     for( k = 1; k < psEncC->nStatesDelayedDecision; k++ ) {
279         if( psDelDec[ k ].RD_Q10 < RDmin_Q10 ) {
280             RDmin_Q10 = psDelDec[ k ].RD_Q10;
281             Winner_ind = k;
282         }
283     }
284
285     /* Copy final part of signals from winner state to output and long-term filter states */
286     psDD = &psDelDec[ Winner_ind ];
287     psIndices->Seed = psDD->SeedInit;
288     last_smple_idx = smpl_buf_idx + decisionDelay;
289     Gain_Q10 = silk_RSHIFT32( Gains_Q16[ psEncC->nb_subfr - 1 ], 6 );
290     for( i = 0; i < decisionDelay; i++ ) {
291         last_smple_idx = ( last_smple_idx - 1 ) & DECISION_DELAY_MASK;
292         pulses[   i - decisionDelay ] = (opus_int8)silk_RSHIFT_ROUND( psDD->Q_Q10[ last_smple_idx ], 10 );
293         pxq[ i - decisionDelay ] = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND(
294             silk_SMULWW( psDD->Xq_Q14[ last_smple_idx ], Gain_Q10 ), 8 ) );
295         NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - decisionDelay + i ] = psDD->Shape_Q14[ last_smple_idx ];
296     }
297     silk_memcpy( NSQ->sLPC_Q14, &psDD->sLPC_Q14[ psEncC->subfr_length ], NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
298     silk_memcpy( NSQ->sAR2_Q14, psDD->sAR2_Q14, sizeof( psDD->sAR2_Q14 ) );
299
300     /* Update states */
301     NSQ->sLF_AR_shp_Q14 = psDD->LF_AR_Q14;
302     NSQ->sDiff_shp_Q14  = psDD->Diff_Q14;
303     NSQ->lagPrev        = pitchL[ psEncC->nb_subfr - 1 ];
304
305     /* Save quantized speech signal */
306     /* DEBUG_STORE_DATA( enc.pcm, &NSQ->xq[psEncC->ltp_mem_length], psEncC->frame_length * sizeof( opus_int16 ) ) */
307     silk_memmove( NSQ->xq,           &NSQ->xq[           psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int16 ) );
308     silk_memmove( NSQ->sLTP_shp_Q14, &NSQ->sLTP_shp_Q14[ psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int32 ) );
309     RESTORE_STACK;
310 }
311
312 /******************************************/
313 /* Noise shape quantizer for one subframe */
314 /******************************************/
315 #ifndef OVERRIDE_silk_noise_shape_quantizer_del_dec
316 static OPUS_INLINE void silk_noise_shape_quantizer_del_dec(
317     silk_nsq_state      *NSQ,                   /* I/O  NSQ state                           */
318     NSQ_del_dec_struct  psDelDec[],             /* I/O  Delayed decision states             */
319     opus_int            signalType,             /* I    Signal type                         */
320     const opus_int32    x_Q10[],                /* I                                        */
321     opus_int8           pulses[],               /* O                                        */
322     opus_int16          xq[],                   /* O                                        */
323     opus_int32          sLTP_Q15[],             /* I/O  LTP filter state                    */
324     opus_int32          delayedGain_Q10[],      /* I/O  Gain delay buffer                   */
325     const opus_int16    a_Q12[],                /* I    Short term prediction coefs         */
326     const opus_int16    b_Q14[],                /* I    Long term prediction coefs          */
327     const opus_int16    AR_shp_Q13[],           /* I    Noise shaping coefs                 */
328     opus_int            lag,                    /* I    Pitch lag                           */
329     opus_int32          HarmShapeFIRPacked_Q14, /* I                                        */
330     opus_int            Tilt_Q14,               /* I    Spectral tilt                       */
331     opus_int32          LF_shp_Q14,             /* I                                        */
332     opus_int32          Gain_Q16,               /* I                                        */
333     opus_int            Lambda_Q10,             /* I                                        */
334     opus_int            offset_Q10,             /* I                                        */
335     opus_int            length,                 /* I    Input length                        */
336     opus_int            subfr,                  /* I    Subframe number                     */
337     opus_int            shapingLPCOrder,        /* I    Shaping LPC filter order            */
338     opus_int            predictLPCOrder,        /* I    Prediction filter order             */
339     opus_int            warping_Q16,            /* I                                        */
340     opus_int            nStatesDelayedDecision, /* I    Number of states in decision tree   */
341     opus_int            *smpl_buf_idx,          /* I    Index to newest samples in buffers  */
342     opus_int            decisionDelay,          /* I                                        */
343     int                 arch                    /* I                                        */
344 )
345 {
346     opus_int     i, j, k, Winner_ind, RDmin_ind, RDmax_ind, last_smple_idx;
347     opus_int32   Winner_rand_state;
348     opus_int32   LTP_pred_Q14, LPC_pred_Q14, n_AR_Q14, n_LTP_Q14;
349     opus_int32   n_LF_Q14, r_Q10, rr_Q10, rd1_Q10, rd2_Q10, RDmin_Q10, RDmax_Q10;
350     opus_int32   q1_Q0, q1_Q10, q2_Q10, exc_Q14, LPC_exc_Q14, xq_Q14, Gain_Q10;
351     opus_int32   tmp1, tmp2, sLF_AR_shp_Q14;
352     opus_int32   *pred_lag_ptr, *shp_lag_ptr, *psLPC_Q14;
353 #ifdef silk_short_prediction_create_arch_coef
354     opus_int32   a_Q12_arch[MAX_LPC_ORDER];
355 #endif
356
357     VARDECL( NSQ_sample_pair, psSampleState );
358     NSQ_del_dec_struct *psDD;
359     NSQ_sample_struct  *psSS;
360     SAVE_STACK;
361
362     /* We're getting desperate to hit the target -- pretend there's
363        no dithering to make hitting the target more likely. */
364     if (Lambda_Q10 > 3072) offset_Q10 = 0;
365     silk_assert( nStatesDelayedDecision > 0 );
366     ALLOC( psSampleState, nStatesDelayedDecision, NSQ_sample_pair );
367
368     shp_lag_ptr  = &NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - lag + HARM_SHAPE_FIR_TAPS / 2 ];
369     pred_lag_ptr = &sLTP_Q15[ NSQ->sLTP_buf_idx - lag + LTP_ORDER / 2 ];
370     Gain_Q10     = silk_RSHIFT( Gain_Q16, 6 );
371
372 #ifdef silk_short_prediction_create_arch_coef
373     silk_short_prediction_create_arch_coef(a_Q12_arch, a_Q12, predictLPCOrder);
374 #endif
375
376     for( i = 0; i < length; i++ ) {
377         /* Perform common calculations used in all states */
378
379         /* Long-term prediction */
380         if( signalType == TYPE_VOICED ) {
381             /* Unrolled loop */
382             /* Avoids introducing a bias because silk_SMLAWB() always rounds to -inf */
383             LTP_pred_Q14 = 2;
384             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[  0 ], b_Q14[ 0 ] );
385             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -1 ], b_Q14[ 1 ] );
386             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -2 ], b_Q14[ 2 ] );
387             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -3 ], b_Q14[ 3 ] );
388             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -4 ], b_Q14[ 4 ] );
389             LTP_pred_Q14 = silk_LSHIFT( LTP_pred_Q14, 1 );                          /* Q13 -> Q14 */
390             pred_lag_ptr++;
391         } else {
392             LTP_pred_Q14 = 0;
393         }
394
395         /* Long-term shaping */
396         if( lag > 0 ) {
397             /* Symmetric, packed FIR coefficients */
398             n_LTP_Q14 = silk_SMULWB( silk_ADD32( shp_lag_ptr[ 0 ], shp_lag_ptr[ -2 ] ), HarmShapeFIRPacked_Q14 );
399             n_LTP_Q14 = silk_SMLAWT( n_LTP_Q14, shp_lag_ptr[ -1 ],                      HarmShapeFIRPacked_Q14 );
400             n_LTP_Q14 = silk_SUB_LSHIFT32( LTP_pred_Q14, n_LTP_Q14, 2 );            /* Q12 -> Q14 */
401             shp_lag_ptr++;
402         } else {
403             n_LTP_Q14 = 0;
404         }
405
406         for( k = 0; k < nStatesDelayedDecision; k++ ) {
407             /* Delayed decision state */
408             psDD = &psDelDec[ k ];
409
410             /* Sample state */
411             psSS = psSampleState[ k ];
412
413             /* Generate dither */
414             psDD->Seed = silk_RAND( psDD->Seed );
415
416             /* Pointer used in short term prediction and shaping */
417             psLPC_Q14 = &psDD->sLPC_Q14[ NSQ_LPC_BUF_LENGTH - 1 + i ];
418             /* Short-term prediction */
419             LPC_pred_Q14 = silk_noise_shape_quantizer_short_prediction(psLPC_Q14, a_Q12, a_Q12_arch, predictLPCOrder, arch);
420             LPC_pred_Q14 = silk_LSHIFT( LPC_pred_Q14, 4 );                              /* Q10 -> Q14 */
421
422             /* Noise shape feedback */
423             silk_assert( ( shapingLPCOrder & 1 ) == 0 );   /* check that order is even */
424             /* Output of lowpass section */
425             tmp2 = silk_SMLAWB( psDD->Diff_Q14, psDD->sAR2_Q14[ 0 ], warping_Q16 );
426             /* Output of allpass section */
427             tmp1 = silk_SMLAWB( psDD->sAR2_Q14[ 0 ], psDD->sAR2_Q14[ 1 ] - tmp2, warping_Q16 );
428             psDD->sAR2_Q14[ 0 ] = tmp2;
429             n_AR_Q14 = silk_RSHIFT( shapingLPCOrder, 1 );
430             n_AR_Q14 = silk_SMLAWB( n_AR_Q14, tmp2, AR_shp_Q13[ 0 ] );
431             /* Loop over allpass sections */
432             for( j = 2; j < shapingLPCOrder; j += 2 ) {
433                 /* Output of allpass section */
434                 tmp2 = silk_SMLAWB( psDD->sAR2_Q14[ j - 1 ], psDD->sAR2_Q14[ j + 0 ] - tmp1, warping_Q16 );
435                 psDD->sAR2_Q14[ j - 1 ] = tmp1;
436                 n_AR_Q14 = silk_SMLAWB( n_AR_Q14, tmp1, AR_shp_Q13[ j - 1 ] );
437                 /* Output of allpass section */
438                 tmp1 = silk_SMLAWB( psDD->sAR2_Q14[ j + 0 ], psDD->sAR2_Q14[ j + 1 ] - tmp2, warping_Q16 );
439                 psDD->sAR2_Q14[ j + 0 ] = tmp2;
440                 n_AR_Q14 = silk_SMLAWB( n_AR_Q14, tmp2, AR_shp_Q13[ j ] );
441             }
442             psDD->sAR2_Q14[ shapingLPCOrder - 1 ] = tmp1;
443             n_AR_Q14 = silk_SMLAWB( n_AR_Q14, tmp1, AR_shp_Q13[ shapingLPCOrder - 1 ] );
444
445             n_AR_Q14 = silk_LSHIFT( n_AR_Q14, 1 );                                      /* Q11 -> Q12 */
446             n_AR_Q14 = silk_SMLAWB( n_AR_Q14, psDD->LF_AR_Q14, Tilt_Q14 );              /* Q12 */
447             n_AR_Q14 = silk_LSHIFT( n_AR_Q14, 2 );                                      /* Q12 -> Q14 */
448
449             n_LF_Q14 = silk_SMULWB( psDD->Shape_Q14[ *smpl_buf_idx ], LF_shp_Q14 );     /* Q12 */
450             n_LF_Q14 = silk_SMLAWT( n_LF_Q14, psDD->LF_AR_Q14, LF_shp_Q14 );            /* Q12 */
451             n_LF_Q14 = silk_LSHIFT( n_LF_Q14, 2 );                                      /* Q12 -> Q14 */
452
453             /* Input minus prediction plus noise feedback                       */
454             /* r = x[ i ] - LTP_pred - LPC_pred + n_AR + n_Tilt + n_LF + n_LTP  */
455             tmp1 = silk_ADD32( n_AR_Q14, n_LF_Q14 );                                    /* Q14 */
456             tmp2 = silk_ADD32( n_LTP_Q14, LPC_pred_Q14 );                               /* Q13 */
457             tmp1 = silk_SUB32( tmp2, tmp1 );                                            /* Q13 */
458             tmp1 = silk_RSHIFT_ROUND( tmp1, 4 );                                        /* Q10 */
459
460             r_Q10 = silk_SUB32( x_Q10[ i ], tmp1 );                                     /* residual error Q10 */
461
462             /* Flip sign depending on dither */
463             if ( psDD->Seed < 0 ) {
464                 r_Q10 = -r_Q10;
465             }
466             r_Q10 = silk_LIMIT_32( r_Q10, -(31 << 10), 30 << 10 );
467
468             /* Find two quantization level candidates and measure their rate-distortion */
469             q1_Q10 = silk_SUB32( r_Q10, offset_Q10 );
470             q1_Q0 = silk_RSHIFT( q1_Q10, 10 );
471             if (Lambda_Q10 > 2048) {
472                 /* For aggressive RDO, the bias becomes more than one pulse. */
473                 int rdo_offset = Lambda_Q10/2 - 512;
474                 if (q1_Q10 > rdo_offset) {
475                     q1_Q0 = silk_RSHIFT( q1_Q10 - rdo_offset, 10 );
476                 } else if (q1_Q10 < -rdo_offset) {
477                     q1_Q0 = silk_RSHIFT( q1_Q10 + rdo_offset, 10 );
478                 } else if (q1_Q10 < 0) {
479                     q1_Q0 = -1;
480                 } else {
481                     q1_Q0 = 0;
482                 }
483             }
484             if( q1_Q0 > 0 ) {
485                 q1_Q10  = silk_SUB32( silk_LSHIFT( q1_Q0, 10 ), QUANT_LEVEL_ADJUST_Q10 );
486                 q1_Q10  = silk_ADD32( q1_Q10, offset_Q10 );
487                 q2_Q10  = silk_ADD32( q1_Q10, 1024 );
488                 rd1_Q10 = silk_SMULBB( q1_Q10, Lambda_Q10 );
489                 rd2_Q10 = silk_SMULBB( q2_Q10, Lambda_Q10 );
490             } else if( q1_Q0 == 0 ) {
491                 q1_Q10  = offset_Q10;
492                 q2_Q10  = silk_ADD32( q1_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
493                 rd1_Q10 = silk_SMULBB( q1_Q10, Lambda_Q10 );
494                 rd2_Q10 = silk_SMULBB( q2_Q10, Lambda_Q10 );
495             } else if( q1_Q0 == -1 ) {
496                 q2_Q10  = offset_Q10;
497                 q1_Q10  = silk_SUB32( q2_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
498                 rd1_Q10 = silk_SMULBB( -q1_Q10, Lambda_Q10 );
499                 rd2_Q10 = silk_SMULBB(  q2_Q10, Lambda_Q10 );
500             } else {            /* q1_Q0 < -1 */
501                 q1_Q10  = silk_ADD32( silk_LSHIFT( q1_Q0, 10 ), QUANT_LEVEL_ADJUST_Q10 );
502                 q1_Q10  = silk_ADD32( q1_Q10, offset_Q10 );
503                 q2_Q10  = silk_ADD32( q1_Q10, 1024 );
504                 rd1_Q10 = silk_SMULBB( -q1_Q10, Lambda_Q10 );
505                 rd2_Q10 = silk_SMULBB( -q2_Q10, Lambda_Q10 );
506             }
507             rr_Q10  = silk_SUB32( r_Q10, q1_Q10 );
508             rd1_Q10 = silk_RSHIFT( silk_SMLABB( rd1_Q10, rr_Q10, rr_Q10 ), 10 );
509             rr_Q10  = silk_SUB32( r_Q10, q2_Q10 );
510             rd2_Q10 = silk_RSHIFT( silk_SMLABB( rd2_Q10, rr_Q10, rr_Q10 ), 10 );
511
512             if( rd1_Q10 < rd2_Q10 ) {
513                 psSS[ 0 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd1_Q10 );
514                 psSS[ 1 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd2_Q10 );
515                 psSS[ 0 ].Q_Q10  = q1_Q10;
516                 psSS[ 1 ].Q_Q10  = q2_Q10;
517             } else {
518                 psSS[ 0 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd2_Q10 );
519                 psSS[ 1 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd1_Q10 );
520                 psSS[ 0 ].Q_Q10  = q2_Q10;
521                 psSS[ 1 ].Q_Q10  = q1_Q10;
522             }
523
524             /* Update states for best quantization */
525
526             /* Quantized excitation */
527             exc_Q14 = silk_LSHIFT32( psSS[ 0 ].Q_Q10, 4 );
528             if ( psDD->Seed < 0 ) {
529                 exc_Q14 = -exc_Q14;
530             }
531
532             /* Add predictions */
533             LPC_exc_Q14 = silk_ADD32( exc_Q14, LTP_pred_Q14 );
534             xq_Q14      = silk_ADD32( LPC_exc_Q14, LPC_pred_Q14 );
535
536             /* Update states */
537             psSS[ 0 ].Diff_Q14     = silk_SUB_LSHIFT32( xq_Q14, x_Q10[ i ], 4 );
538             sLF_AR_shp_Q14         = silk_SUB32( psSS[ 0 ].Diff_Q14, n_AR_Q14 );
539             psSS[ 0 ].sLTP_shp_Q14 = silk_SUB32( sLF_AR_shp_Q14, n_LF_Q14 );
540             psSS[ 0 ].LF_AR_Q14    = sLF_AR_shp_Q14;
541             psSS[ 0 ].LPC_exc_Q14  = LPC_exc_Q14;
542             psSS[ 0 ].xq_Q14       = xq_Q14;
543
544             /* Update states for second best quantization */
545
546             /* Quantized excitation */
547             exc_Q14 = silk_LSHIFT32( psSS[ 1 ].Q_Q10, 4 );
548             if ( psDD->Seed < 0 ) {
549                 exc_Q14 = -exc_Q14;
550             }
551
552             /* Add predictions */
553             LPC_exc_Q14 = silk_ADD32( exc_Q14, LTP_pred_Q14 );
554             xq_Q14      = silk_ADD32( LPC_exc_Q14, LPC_pred_Q14 );
555
556             /* Update states */
557             psSS[ 1 ].Diff_Q14     = silk_SUB_LSHIFT32( xq_Q14, x_Q10[ i ], 4 );
558             sLF_AR_shp_Q14         = silk_SUB32( psSS[ 1 ].Diff_Q14, n_AR_Q14 );
559             psSS[ 1 ].sLTP_shp_Q14 = silk_SUB32( sLF_AR_shp_Q14, n_LF_Q14 );
560             psSS[ 1 ].LF_AR_Q14    = sLF_AR_shp_Q14;
561             psSS[ 1 ].LPC_exc_Q14  = LPC_exc_Q14;
562             psSS[ 1 ].xq_Q14       = xq_Q14;
563         }
564
565         *smpl_buf_idx  = ( *smpl_buf_idx - 1 ) & DECISION_DELAY_MASK;                   /* Index to newest samples              */
566         last_smple_idx = ( *smpl_buf_idx + decisionDelay ) & DECISION_DELAY_MASK;       /* Index to decisionDelay old samples   */
567
568         /* Find winner */
569         RDmin_Q10 = psSampleState[ 0 ][ 0 ].RD_Q10;
570         Winner_ind = 0;
571         for( k = 1; k < nStatesDelayedDecision; k++ ) {
572             if( psSampleState[ k ][ 0 ].RD_Q10 < RDmin_Q10 ) {
573                 RDmin_Q10  = psSampleState[ k ][ 0 ].RD_Q10;
574                 Winner_ind = k;
575             }
576         }
577
578         /* Increase RD values of expired states */
579         Winner_rand_state = psDelDec[ Winner_ind ].RandState[ last_smple_idx ];
580         for( k = 0; k < nStatesDelayedDecision; k++ ) {
581             if( psDelDec[ k ].RandState[ last_smple_idx ] != Winner_rand_state ) {
582                 psSampleState[ k ][ 0 ].RD_Q10 = silk_ADD32( psSampleState[ k ][ 0 ].RD_Q10, silk_int32_MAX >> 4 );
583                 psSampleState[ k ][ 1 ].RD_Q10 = silk_ADD32( psSampleState[ k ][ 1 ].RD_Q10, silk_int32_MAX >> 4 );
584                 silk_assert( psSampleState[ k ][ 0 ].RD_Q10 >= 0 );
585             }
586         }
587
588         /* Find worst in first set and best in second set */
589         RDmax_Q10  = psSampleState[ 0 ][ 0 ].RD_Q10;
590         RDmin_Q10  = psSampleState[ 0 ][ 1 ].RD_Q10;
591         RDmax_ind = 0;
592         RDmin_ind = 0;
593         for( k = 1; k < nStatesDelayedDecision; k++ ) {
594             /* find worst in first set */
595             if( psSampleState[ k ][ 0 ].RD_Q10 > RDmax_Q10 ) {
596                 RDmax_Q10  = psSampleState[ k ][ 0 ].RD_Q10;
597                 RDmax_ind = k;
598             }
599             /* find best in second set */
600             if( psSampleState[ k ][ 1 ].RD_Q10 < RDmin_Q10 ) {
601                 RDmin_Q10  = psSampleState[ k ][ 1 ].RD_Q10;
602                 RDmin_ind = k;
603             }
604         }
605
606         /* Replace a state if best from second set outperforms worst in first set */
607         if( RDmin_Q10 < RDmax_Q10 ) {
608             silk_memcpy( ( (opus_int32 *)&psDelDec[ RDmax_ind ] ) + i,
609                          ( (opus_int32 *)&psDelDec[ RDmin_ind ] ) + i, sizeof( NSQ_del_dec_struct ) - i * sizeof( opus_int32) );
610             silk_memcpy( &psSampleState[ RDmax_ind ][ 0 ], &psSampleState[ RDmin_ind ][ 1 ], sizeof( NSQ_sample_struct ) );
611         }
612
613         /* Write samples from winner to output and long-term filter states */
614         psDD = &psDelDec[ Winner_ind ];
615         if( subfr > 0 || i >= decisionDelay ) {
616             pulses[  i - decisionDelay ] = (opus_int8)silk_RSHIFT_ROUND( psDD->Q_Q10[ last_smple_idx ], 10 );
617             xq[ i - decisionDelay ] = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND(
618                 silk_SMULWW( psDD->Xq_Q14[ last_smple_idx ], delayedGain_Q10[ last_smple_idx ] ), 8 ) );
619             NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - decisionDelay ] = psDD->Shape_Q14[ last_smple_idx ];
620             sLTP_Q15[          NSQ->sLTP_buf_idx     - decisionDelay ] = psDD->Pred_Q15[  last_smple_idx ];
621         }
622         NSQ->sLTP_shp_buf_idx++;
623         NSQ->sLTP_buf_idx++;
624
625         /* Update states */
626         for( k = 0; k < nStatesDelayedDecision; k++ ) {
627             psDD                                     = &psDelDec[ k ];
628             psSS                                     = &psSampleState[ k ][ 0 ];
629             psDD->LF_AR_Q14                          = psSS->LF_AR_Q14;
630             psDD->Diff_Q14                           = psSS->Diff_Q14;
631             psDD->sLPC_Q14[ NSQ_LPC_BUF_LENGTH + i ] = psSS->xq_Q14;
632             psDD->Xq_Q14[    *smpl_buf_idx ]         = psSS->xq_Q14;
633             psDD->Q_Q10[     *smpl_buf_idx ]         = psSS->Q_Q10;
634             psDD->Pred_Q15[  *smpl_buf_idx ]         = silk_LSHIFT32( psSS->LPC_exc_Q14, 1 );
635             psDD->Shape_Q14[ *smpl_buf_idx ]         = psSS->sLTP_shp_Q14;
636             psDD->Seed                               = silk_ADD32_ovflw( psDD->Seed, silk_RSHIFT_ROUND( psSS->Q_Q10, 10 ) );
637             psDD->RandState[ *smpl_buf_idx ]         = psDD->Seed;
638             psDD->RD_Q10                             = psSS->RD_Q10;
639         }
640         delayedGain_Q10[     *smpl_buf_idx ]         = Gain_Q10;
641     }
642     /* Update LPC states */
643     for( k = 0; k < nStatesDelayedDecision; k++ ) {
644         psDD = &psDelDec[ k ];
645         silk_memcpy( psDD->sLPC_Q14, &psDD->sLPC_Q14[ length ], NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
646     }
647     RESTORE_STACK;
648 }
649 #endif /* OVERRIDE_silk_noise_shape_quantizer_del_dec */
650
651 static OPUS_INLINE void silk_nsq_del_dec_scale_states(
652     const silk_encoder_state *psEncC,               /* I    Encoder State                       */
653     silk_nsq_state      *NSQ,                       /* I/O  NSQ state                           */
654     NSQ_del_dec_struct  psDelDec[],                 /* I/O  Delayed decision states             */
655     const opus_int16    x16[],                      /* I    Input                               */
656     opus_int32          x_sc_Q10[],                 /* O    Input scaled with 1/Gain in Q10     */
657     const opus_int16    sLTP[],                     /* I    Re-whitened LTP state in Q0         */
658     opus_int32          sLTP_Q15[],                 /* O    LTP state matching scaled input     */
659     opus_int            subfr,                      /* I    Subframe number                     */
660     opus_int            nStatesDelayedDecision,     /* I    Number of del dec states            */
661     const opus_int      LTP_scale_Q14,              /* I    LTP state scaling                   */
662     const opus_int32    Gains_Q16[ MAX_NB_SUBFR ],  /* I                                        */
663     const opus_int      pitchL[ MAX_NB_SUBFR ],     /* I    Pitch lag                           */
664     const opus_int      signal_type,                /* I    Signal type                         */
665     const opus_int      decisionDelay               /* I    Decision delay                      */
666 )
667 {
668     opus_int            i, k, lag;
669     opus_int32          gain_adj_Q16, inv_gain_Q31, inv_gain_Q26;
670     NSQ_del_dec_struct  *psDD;
671
672     lag          = pitchL[ subfr ];
673     inv_gain_Q31 = silk_INVERSE32_varQ( silk_max( Gains_Q16[ subfr ], 1 ), 47 );
674     silk_assert( inv_gain_Q31 != 0 );
675
676     /* Scale input */
677     inv_gain_Q26 = silk_RSHIFT_ROUND( inv_gain_Q31, 5 );
678     for( i = 0; i < psEncC->subfr_length; i++ ) {
679         x_sc_Q10[ i ] = silk_SMULWW( x16[ i ], inv_gain_Q26 );
680     }
681
682     /* After rewhitening the LTP state is un-scaled, so scale with inv_gain_Q16 */
683     if( NSQ->rewhite_flag ) {
684         if( subfr == 0 ) {
685             /* Do LTP downscaling */
686             inv_gain_Q31 = silk_LSHIFT( silk_SMULWB( inv_gain_Q31, LTP_scale_Q14 ), 2 );
687         }
688         for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {
689             silk_assert( i < MAX_FRAME_LENGTH );
690             sLTP_Q15[ i ] = silk_SMULWB( inv_gain_Q31, sLTP[ i ] );
691         }
692     }
693
694     /* Adjust for changing gain */
695     if( Gains_Q16[ subfr ] != NSQ->prev_gain_Q16 ) {
696         gain_adj_Q16 =  silk_DIV32_varQ( NSQ->prev_gain_Q16, Gains_Q16[ subfr ], 16 );
697
698         /* Scale long-term shaping state */
699         for( i = NSQ->sLTP_shp_buf_idx - psEncC->ltp_mem_length; i < NSQ->sLTP_shp_buf_idx; i++ ) {
700             NSQ->sLTP_shp_Q14[ i ] = silk_SMULWW( gain_adj_Q16, NSQ->sLTP_shp_Q14[ i ] );
701         }
702
703         /* Scale long-term prediction state */
704         if( signal_type == TYPE_VOICED && NSQ->rewhite_flag == 0 ) {
705             for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx - decisionDelay; i++ ) {
706                 sLTP_Q15[ i ] = silk_SMULWW( gain_adj_Q16, sLTP_Q15[ i ] );
707             }
708         }
709
710         for( k = 0; k < nStatesDelayedDecision; k++ ) {
711             psDD = &psDelDec[ k ];
712
713             /* Scale scalar states */
714             psDD->LF_AR_Q14 = silk_SMULWW( gain_adj_Q16, psDD->LF_AR_Q14 );
715             psDD->Diff_Q14 = silk_SMULWW( gain_adj_Q16, psDD->Diff_Q14 );
716
717             /* Scale short-term prediction and shaping states */
718             for( i = 0; i < NSQ_LPC_BUF_LENGTH; i++ ) {
719                 psDD->sLPC_Q14[ i ] = silk_SMULWW( gain_adj_Q16, psDD->sLPC_Q14[ i ] );
720             }
721             for( i = 0; i < MAX_SHAPE_LPC_ORDER; i++ ) {
722                 psDD->sAR2_Q14[ i ] = silk_SMULWW( gain_adj_Q16, psDD->sAR2_Q14[ i ] );
723             }
724             for( i = 0; i < DECISION_DELAY; i++ ) {
725                 psDD->Pred_Q15[  i ] = silk_SMULWW( gain_adj_Q16, psDD->Pred_Q15[  i ] );
726                 psDD->Shape_Q14[ i ] = silk_SMULWW( gain_adj_Q16, psDD->Shape_Q14[ i ] );
727             }
728         }
729
730         /* Save inverse gain */
731         NSQ->prev_gain_Q16 = Gains_Q16[ subfr ];
732     }
733 }