1b6dbf04bce8d56f860f23e0dcbd0011ed747fc0
[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;
241                         if( last_smple_idx < 0 ) last_smple_idx += DECISION_DELAY;
242                         pulses[   i - decisionDelay ] = (opus_int8)silk_RSHIFT_ROUND( psDD->Q_Q10[ last_smple_idx ], 10 );
243                         pxq[ i - decisionDelay ] = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND(
244                             silk_SMULWW( psDD->Xq_Q14[ last_smple_idx ], Gains_Q16[ 1 ] ), 14 ) );
245                         NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - decisionDelay + i ] = psDD->Shape_Q14[ last_smple_idx ];
246                     }
247
248                     subfr = 0;
249                 }
250
251                 /* Rewhiten with new A coefs */
252                 start_idx = psEncC->ltp_mem_length - lag - psEncC->predictLPCOrder - LTP_ORDER / 2;
253                 silk_assert( start_idx > 0 );
254
255                 silk_LPC_analysis_filter( &sLTP[ start_idx ], &NSQ->xq[ start_idx + k * psEncC->subfr_length ],
256                     A_Q12, psEncC->ltp_mem_length - start_idx, psEncC->predictLPCOrder, psEncC->arch );
257
258                 NSQ->sLTP_buf_idx = psEncC->ltp_mem_length;
259                 NSQ->rewhite_flag = 1;
260             }
261         }
262
263         silk_nsq_del_dec_scale_states( psEncC, NSQ, psDelDec, x16, x_sc_Q10, sLTP, sLTP_Q15, k,
264             psEncC->nStatesDelayedDecision, LTP_scale_Q14, Gains_Q16, pitchL, psIndices->signalType, decisionDelay );
265
266         silk_noise_shape_quantizer_del_dec( NSQ, psDelDec, psIndices->signalType, x_sc_Q10, pulses, pxq, sLTP_Q15,
267             delayedGain_Q10, A_Q12, B_Q14, AR_shp_Q13, lag, HarmShapeFIRPacked_Q14, Tilt_Q14[ k ], LF_shp_Q14[ k ],
268             Gains_Q16[ k ], Lambda_Q10, offset_Q10, psEncC->subfr_length, subfr++, psEncC->shapingLPCOrder,
269             psEncC->predictLPCOrder, psEncC->warping_Q16, psEncC->nStatesDelayedDecision, &smpl_buf_idx, decisionDelay, psEncC->arch );
270
271         x16    += psEncC->subfr_length;
272         pulses += psEncC->subfr_length;
273         pxq    += psEncC->subfr_length;
274     }
275
276     /* Find winner */
277     RDmin_Q10 = psDelDec[ 0 ].RD_Q10;
278     Winner_ind = 0;
279     for( k = 1; k < psEncC->nStatesDelayedDecision; k++ ) {
280         if( psDelDec[ k ].RD_Q10 < RDmin_Q10 ) {
281             RDmin_Q10 = psDelDec[ k ].RD_Q10;
282             Winner_ind = k;
283         }
284     }
285
286     /* Copy final part of signals from winner state to output and long-term filter states */
287     psDD = &psDelDec[ Winner_ind ];
288     psIndices->Seed = psDD->SeedInit;
289     last_smple_idx = smpl_buf_idx + decisionDelay;
290     Gain_Q10 = silk_RSHIFT32( Gains_Q16[ psEncC->nb_subfr - 1 ], 6 );
291     for( i = 0; i < decisionDelay; i++ ) {
292         last_smple_idx = ( last_smple_idx - 1 ) % DECISION_DELAY;
293         if( last_smple_idx < 0 ) last_smple_idx += DECISION_DELAY;
294
295         pulses[   i - decisionDelay ] = (opus_int8)silk_RSHIFT_ROUND( psDD->Q_Q10[ last_smple_idx ], 10 );
296         pxq[ i - decisionDelay ] = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND(
297             silk_SMULWW( psDD->Xq_Q14[ last_smple_idx ], Gain_Q10 ), 8 ) );
298         NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - decisionDelay + i ] = psDD->Shape_Q14[ last_smple_idx ];
299     }
300     silk_memcpy( NSQ->sLPC_Q14, &psDD->sLPC_Q14[ psEncC->subfr_length ], NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
301     silk_memcpy( NSQ->sAR2_Q14, psDD->sAR2_Q14, sizeof( psDD->sAR2_Q14 ) );
302
303     /* Update states */
304     NSQ->sLF_AR_shp_Q14 = psDD->LF_AR_Q14;
305     NSQ->sDiff_shp_Q14  = psDD->Diff_Q14;
306     NSQ->lagPrev        = pitchL[ psEncC->nb_subfr - 1 ];
307
308     /* Save quantized speech signal */
309     /* DEBUG_STORE_DATA( enc.pcm, &NSQ->xq[psEncC->ltp_mem_length], psEncC->frame_length * sizeof( opus_int16 ) ) */
310     silk_memmove( NSQ->xq,           &NSQ->xq[           psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int16 ) );
311     silk_memmove( NSQ->sLTP_shp_Q14, &NSQ->sLTP_shp_Q14[ psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int32 ) );
312     RESTORE_STACK;
313 }
314
315 /******************************************/
316 /* Noise shape quantizer for one subframe */
317 /******************************************/
318 #ifndef OVERRIDE_silk_noise_shape_quantizer_del_dec
319 static OPUS_INLINE void silk_noise_shape_quantizer_del_dec(
320     silk_nsq_state      *NSQ,                   /* I/O  NSQ state                           */
321     NSQ_del_dec_struct  psDelDec[],             /* I/O  Delayed decision states             */
322     opus_int            signalType,             /* I    Signal type                         */
323     const opus_int32    x_Q10[],                /* I                                        */
324     opus_int8           pulses[],               /* O                                        */
325     opus_int16          xq[],                   /* O                                        */
326     opus_int32          sLTP_Q15[],             /* I/O  LTP filter state                    */
327     opus_int32          delayedGain_Q10[],      /* I/O  Gain delay buffer                   */
328     const opus_int16    a_Q12[],                /* I    Short term prediction coefs         */
329     const opus_int16    b_Q14[],                /* I    Long term prediction coefs          */
330     const opus_int16    AR_shp_Q13[],           /* I    Noise shaping coefs                 */
331     opus_int            lag,                    /* I    Pitch lag                           */
332     opus_int32          HarmShapeFIRPacked_Q14, /* I                                        */
333     opus_int            Tilt_Q14,               /* I    Spectral tilt                       */
334     opus_int32          LF_shp_Q14,             /* I                                        */
335     opus_int32          Gain_Q16,               /* I                                        */
336     opus_int            Lambda_Q10,             /* I                                        */
337     opus_int            offset_Q10,             /* I                                        */
338     opus_int            length,                 /* I    Input length                        */
339     opus_int            subfr,                  /* I    Subframe number                     */
340     opus_int            shapingLPCOrder,        /* I    Shaping LPC filter order            */
341     opus_int            predictLPCOrder,        /* I    Prediction filter order             */
342     opus_int            warping_Q16,            /* I                                        */
343     opus_int            nStatesDelayedDecision, /* I    Number of states in decision tree   */
344     opus_int            *smpl_buf_idx,          /* I    Index to newest samples in buffers  */
345     opus_int            decisionDelay,          /* I                                        */
346     int                 arch                    /* I                                        */
347 )
348 {
349     opus_int     i, j, k, Winner_ind, RDmin_ind, RDmax_ind, last_smple_idx;
350     opus_int32   Winner_rand_state;
351     opus_int32   LTP_pred_Q14, LPC_pred_Q14, n_AR_Q14, n_LTP_Q14;
352     opus_int32   n_LF_Q14, r_Q10, rr_Q10, rd1_Q10, rd2_Q10, RDmin_Q10, RDmax_Q10;
353     opus_int32   q1_Q0, q1_Q10, q2_Q10, exc_Q14, LPC_exc_Q14, xq_Q14, Gain_Q10;
354     opus_int32   tmp1, tmp2, sLF_AR_shp_Q14;
355     opus_int32   *pred_lag_ptr, *shp_lag_ptr, *psLPC_Q14;
356 #ifdef silk_short_prediction_create_arch_coef
357     opus_int32   a_Q12_arch[MAX_LPC_ORDER];
358 #endif
359
360     VARDECL( NSQ_sample_pair, psSampleState );
361     NSQ_del_dec_struct *psDD;
362     NSQ_sample_struct  *psSS;
363     SAVE_STACK;
364
365     /* We're getting desperate to hit the target -- pretend there's
366        no dithering to make hitting the target more likely. */
367     if (Lambda_Q10 > 3072) offset_Q10 = 0;
368     silk_assert( nStatesDelayedDecision > 0 );
369     ALLOC( psSampleState, nStatesDelayedDecision, NSQ_sample_pair );
370
371     shp_lag_ptr  = &NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - lag + HARM_SHAPE_FIR_TAPS / 2 ];
372     pred_lag_ptr = &sLTP_Q15[ NSQ->sLTP_buf_idx - lag + LTP_ORDER / 2 ];
373     Gain_Q10     = silk_RSHIFT( Gain_Q16, 6 );
374
375 #ifdef silk_short_prediction_create_arch_coef
376     silk_short_prediction_create_arch_coef(a_Q12_arch, a_Q12, predictLPCOrder);
377 #endif
378
379     for( i = 0; i < length; i++ ) {
380         /* Perform common calculations used in all states */
381
382         /* Long-term prediction */
383         if( signalType == TYPE_VOICED ) {
384             /* Unrolled loop */
385             /* Avoids introducing a bias because silk_SMLAWB() always rounds to -inf */
386             LTP_pred_Q14 = 2;
387             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[  0 ], b_Q14[ 0 ] );
388             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -1 ], b_Q14[ 1 ] );
389             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -2 ], b_Q14[ 2 ] );
390             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -3 ], b_Q14[ 3 ] );
391             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -4 ], b_Q14[ 4 ] );
392             LTP_pred_Q14 = silk_LSHIFT( LTP_pred_Q14, 1 );                          /* Q13 -> Q14 */
393             pred_lag_ptr++;
394         } else {
395             LTP_pred_Q14 = 0;
396         }
397
398         /* Long-term shaping */
399         if( lag > 0 ) {
400             /* Symmetric, packed FIR coefficients */
401             n_LTP_Q14 = silk_SMULWB( silk_ADD32( shp_lag_ptr[ 0 ], shp_lag_ptr[ -2 ] ), HarmShapeFIRPacked_Q14 );
402             n_LTP_Q14 = silk_SMLAWT( n_LTP_Q14, shp_lag_ptr[ -1 ],                      HarmShapeFIRPacked_Q14 );
403             n_LTP_Q14 = silk_SUB_LSHIFT32( LTP_pred_Q14, n_LTP_Q14, 2 );            /* Q12 -> Q14 */
404             shp_lag_ptr++;
405         } else {
406             n_LTP_Q14 = 0;
407         }
408
409         for( k = 0; k < nStatesDelayedDecision; k++ ) {
410             /* Delayed decision state */
411             psDD = &psDelDec[ k ];
412
413             /* Sample state */
414             psSS = psSampleState[ k ];
415
416             /* Generate dither */
417             psDD->Seed = silk_RAND( psDD->Seed );
418
419             /* Pointer used in short term prediction and shaping */
420             psLPC_Q14 = &psDD->sLPC_Q14[ NSQ_LPC_BUF_LENGTH - 1 + i ];
421             /* Short-term prediction */
422             LPC_pred_Q14 = silk_noise_shape_quantizer_short_prediction(psLPC_Q14, a_Q12, a_Q12_arch, predictLPCOrder, arch);
423             LPC_pred_Q14 = silk_LSHIFT( LPC_pred_Q14, 4 );                              /* Q10 -> Q14 */
424
425             /* Noise shape feedback */
426             silk_assert( ( shapingLPCOrder & 1 ) == 0 );   /* check that order is even */
427             /* Output of lowpass section */
428             tmp2 = silk_SMLAWB( psDD->Diff_Q14, psDD->sAR2_Q14[ 0 ], warping_Q16 );
429             /* Output of allpass section */
430             tmp1 = silk_SMLAWB( psDD->sAR2_Q14[ 0 ], psDD->sAR2_Q14[ 1 ] - tmp2, warping_Q16 );
431             psDD->sAR2_Q14[ 0 ] = tmp2;
432             n_AR_Q14 = silk_RSHIFT( shapingLPCOrder, 1 );
433             n_AR_Q14 = silk_SMLAWB( n_AR_Q14, tmp2, AR_shp_Q13[ 0 ] );
434             /* Loop over allpass sections */
435             for( j = 2; j < shapingLPCOrder; j += 2 ) {
436                 /* Output of allpass section */
437                 tmp2 = silk_SMLAWB( psDD->sAR2_Q14[ j - 1 ], psDD->sAR2_Q14[ j + 0 ] - tmp1, warping_Q16 );
438                 psDD->sAR2_Q14[ j - 1 ] = tmp1;
439                 n_AR_Q14 = silk_SMLAWB( n_AR_Q14, tmp1, AR_shp_Q13[ j - 1 ] );
440                 /* Output of allpass section */
441                 tmp1 = silk_SMLAWB( psDD->sAR2_Q14[ j + 0 ], psDD->sAR2_Q14[ j + 1 ] - tmp2, warping_Q16 );
442                 psDD->sAR2_Q14[ j + 0 ] = tmp2;
443                 n_AR_Q14 = silk_SMLAWB( n_AR_Q14, tmp2, AR_shp_Q13[ j ] );
444             }
445             psDD->sAR2_Q14[ shapingLPCOrder - 1 ] = tmp1;
446             n_AR_Q14 = silk_SMLAWB( n_AR_Q14, tmp1, AR_shp_Q13[ shapingLPCOrder - 1 ] );
447
448             n_AR_Q14 = silk_LSHIFT( n_AR_Q14, 1 );                                      /* Q11 -> Q12 */
449             n_AR_Q14 = silk_SMLAWB( n_AR_Q14, psDD->LF_AR_Q14, Tilt_Q14 );              /* Q12 */
450             n_AR_Q14 = silk_LSHIFT( n_AR_Q14, 2 );                                      /* Q12 -> Q14 */
451
452             n_LF_Q14 = silk_SMULWB( psDD->Shape_Q14[ *smpl_buf_idx ], LF_shp_Q14 );     /* Q12 */
453             n_LF_Q14 = silk_SMLAWT( n_LF_Q14, psDD->LF_AR_Q14, LF_shp_Q14 );            /* Q12 */
454             n_LF_Q14 = silk_LSHIFT( n_LF_Q14, 2 );                                      /* Q12 -> Q14 */
455
456             /* Input minus prediction plus noise feedback                       */
457             /* r = x[ i ] - LTP_pred - LPC_pred + n_AR + n_Tilt + n_LF + n_LTP  */
458             tmp1 = silk_ADD32( n_AR_Q14, n_LF_Q14 );                                    /* Q14 */
459             tmp2 = silk_ADD32( n_LTP_Q14, LPC_pred_Q14 );                               /* Q13 */
460             tmp1 = silk_SUB32( tmp2, tmp1 );                                            /* Q13 */
461             tmp1 = silk_RSHIFT_ROUND( tmp1, 4 );                                        /* Q10 */
462
463             r_Q10 = silk_SUB32( x_Q10[ i ], tmp1 );                                     /* residual error Q10 */
464
465             /* Flip sign depending on dither */
466             if ( psDD->Seed < 0 ) {
467                 r_Q10 = -r_Q10;
468             }
469             r_Q10 = silk_LIMIT_32( r_Q10, -(31 << 10), 30 << 10 );
470
471             /* Find two quantization level candidates and measure their rate-distortion */
472             q1_Q10 = silk_SUB32( r_Q10, offset_Q10 );
473             q1_Q0 = silk_RSHIFT( q1_Q10, 10 );
474             if (Lambda_Q10 > 2048) {
475                 /* For aggressive RDO, the bias becomes more than one pulse. */
476                 int rdo_offset = Lambda_Q10/2 - 512;
477                 if (q1_Q10 > rdo_offset) {
478                     q1_Q0 = silk_RSHIFT( q1_Q10 - rdo_offset, 10 );
479                 } else if (q1_Q10 < -rdo_offset) {
480                     q1_Q0 = silk_RSHIFT( q1_Q10 + rdo_offset, 10 );
481                 } else if (q1_Q10 < 0) {
482                     q1_Q0 = -1;
483                 } else {
484                     q1_Q0 = 0;
485                 }
486             }
487             if( q1_Q0 > 0 ) {
488                 q1_Q10  = silk_SUB32( silk_LSHIFT( q1_Q0, 10 ), QUANT_LEVEL_ADJUST_Q10 );
489                 q1_Q10  = silk_ADD32( q1_Q10, offset_Q10 );
490                 q2_Q10  = silk_ADD32( q1_Q10, 1024 );
491                 rd1_Q10 = silk_SMULBB( q1_Q10, Lambda_Q10 );
492                 rd2_Q10 = silk_SMULBB( q2_Q10, Lambda_Q10 );
493             } else if( q1_Q0 == 0 ) {
494                 q1_Q10  = offset_Q10;
495                 q2_Q10  = silk_ADD32( q1_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
496                 rd1_Q10 = silk_SMULBB( q1_Q10, Lambda_Q10 );
497                 rd2_Q10 = silk_SMULBB( q2_Q10, Lambda_Q10 );
498             } else if( q1_Q0 == -1 ) {
499                 q2_Q10  = offset_Q10;
500                 q1_Q10  = silk_SUB32( q2_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
501                 rd1_Q10 = silk_SMULBB( -q1_Q10, Lambda_Q10 );
502                 rd2_Q10 = silk_SMULBB(  q2_Q10, Lambda_Q10 );
503             } else {            /* q1_Q0 < -1 */
504                 q1_Q10  = silk_ADD32( silk_LSHIFT( q1_Q0, 10 ), QUANT_LEVEL_ADJUST_Q10 );
505                 q1_Q10  = silk_ADD32( q1_Q10, offset_Q10 );
506                 q2_Q10  = silk_ADD32( q1_Q10, 1024 );
507                 rd1_Q10 = silk_SMULBB( -q1_Q10, Lambda_Q10 );
508                 rd2_Q10 = silk_SMULBB( -q2_Q10, Lambda_Q10 );
509             }
510             rr_Q10  = silk_SUB32( r_Q10, q1_Q10 );
511             rd1_Q10 = silk_RSHIFT( silk_SMLABB( rd1_Q10, rr_Q10, rr_Q10 ), 10 );
512             rr_Q10  = silk_SUB32( r_Q10, q2_Q10 );
513             rd2_Q10 = silk_RSHIFT( silk_SMLABB( rd2_Q10, rr_Q10, rr_Q10 ), 10 );
514
515             if( rd1_Q10 < rd2_Q10 ) {
516                 psSS[ 0 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd1_Q10 );
517                 psSS[ 1 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd2_Q10 );
518                 psSS[ 0 ].Q_Q10  = q1_Q10;
519                 psSS[ 1 ].Q_Q10  = q2_Q10;
520             } else {
521                 psSS[ 0 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd2_Q10 );
522                 psSS[ 1 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd1_Q10 );
523                 psSS[ 0 ].Q_Q10  = q2_Q10;
524                 psSS[ 1 ].Q_Q10  = q1_Q10;
525             }
526
527             /* Update states for best quantization */
528
529             /* Quantized excitation */
530             exc_Q14 = silk_LSHIFT32( psSS[ 0 ].Q_Q10, 4 );
531             if ( psDD->Seed < 0 ) {
532                 exc_Q14 = -exc_Q14;
533             }
534
535             /* Add predictions */
536             LPC_exc_Q14 = silk_ADD32( exc_Q14, LTP_pred_Q14 );
537             xq_Q14      = silk_ADD32( LPC_exc_Q14, LPC_pred_Q14 );
538
539             /* Update states */
540             psSS[ 0 ].Diff_Q14     = silk_SUB_LSHIFT32( xq_Q14, x_Q10[ i ], 4 );
541             sLF_AR_shp_Q14         = silk_SUB32( psSS[ 0 ].Diff_Q14, n_AR_Q14 );
542             psSS[ 0 ].sLTP_shp_Q14 = silk_SUB32( sLF_AR_shp_Q14, n_LF_Q14 );
543             psSS[ 0 ].LF_AR_Q14    = sLF_AR_shp_Q14;
544             psSS[ 0 ].LPC_exc_Q14  = LPC_exc_Q14;
545             psSS[ 0 ].xq_Q14       = xq_Q14;
546
547             /* Update states for second best quantization */
548
549             /* Quantized excitation */
550             exc_Q14 = silk_LSHIFT32( psSS[ 1 ].Q_Q10, 4 );
551             if ( psDD->Seed < 0 ) {
552                 exc_Q14 = -exc_Q14;
553             }
554
555             /* Add predictions */
556             LPC_exc_Q14 = silk_ADD32( exc_Q14, LTP_pred_Q14 );
557             xq_Q14      = silk_ADD32( LPC_exc_Q14, LPC_pred_Q14 );
558
559             /* Update states */
560             psSS[ 1 ].Diff_Q14     = silk_SUB_LSHIFT32( xq_Q14, x_Q10[ i ], 4 );
561             sLF_AR_shp_Q14         = silk_SUB32( psSS[ 1 ].Diff_Q14, n_AR_Q14 );
562             psSS[ 1 ].sLTP_shp_Q14 = silk_SUB32( sLF_AR_shp_Q14, n_LF_Q14 );
563             psSS[ 1 ].LF_AR_Q14    = sLF_AR_shp_Q14;
564             psSS[ 1 ].LPC_exc_Q14  = LPC_exc_Q14;
565             psSS[ 1 ].xq_Q14       = xq_Q14;
566         }
567
568         *smpl_buf_idx  = ( *smpl_buf_idx - 1 ) % DECISION_DELAY;
569         if( *smpl_buf_idx < 0 ) *smpl_buf_idx += DECISION_DELAY;
570         last_smple_idx = ( *smpl_buf_idx + decisionDelay ) % DECISION_DELAY;
571
572         /* Find winner */
573         RDmin_Q10 = psSampleState[ 0 ][ 0 ].RD_Q10;
574         Winner_ind = 0;
575         for( k = 1; k < nStatesDelayedDecision; k++ ) {
576             if( psSampleState[ k ][ 0 ].RD_Q10 < RDmin_Q10 ) {
577                 RDmin_Q10  = psSampleState[ k ][ 0 ].RD_Q10;
578                 Winner_ind = k;
579             }
580         }
581
582         /* Increase RD values of expired states */
583         Winner_rand_state = psDelDec[ Winner_ind ].RandState[ last_smple_idx ];
584         for( k = 0; k < nStatesDelayedDecision; k++ ) {
585             if( psDelDec[ k ].RandState[ last_smple_idx ] != Winner_rand_state ) {
586                 psSampleState[ k ][ 0 ].RD_Q10 = silk_ADD32( psSampleState[ k ][ 0 ].RD_Q10, silk_int32_MAX >> 4 );
587                 psSampleState[ k ][ 1 ].RD_Q10 = silk_ADD32( psSampleState[ k ][ 1 ].RD_Q10, silk_int32_MAX >> 4 );
588                 silk_assert( psSampleState[ k ][ 0 ].RD_Q10 >= 0 );
589             }
590         }
591
592         /* Find worst in first set and best in second set */
593         RDmax_Q10  = psSampleState[ 0 ][ 0 ].RD_Q10;
594         RDmin_Q10  = psSampleState[ 0 ][ 1 ].RD_Q10;
595         RDmax_ind = 0;
596         RDmin_ind = 0;
597         for( k = 1; k < nStatesDelayedDecision; k++ ) {
598             /* find worst in first set */
599             if( psSampleState[ k ][ 0 ].RD_Q10 > RDmax_Q10 ) {
600                 RDmax_Q10  = psSampleState[ k ][ 0 ].RD_Q10;
601                 RDmax_ind = k;
602             }
603             /* find best in second set */
604             if( psSampleState[ k ][ 1 ].RD_Q10 < RDmin_Q10 ) {
605                 RDmin_Q10  = psSampleState[ k ][ 1 ].RD_Q10;
606                 RDmin_ind = k;
607             }
608         }
609
610         /* Replace a state if best from second set outperforms worst in first set */
611         if( RDmin_Q10 < RDmax_Q10 ) {
612             silk_memcpy( ( (opus_int32 *)&psDelDec[ RDmax_ind ] ) + i,
613                          ( (opus_int32 *)&psDelDec[ RDmin_ind ] ) + i, sizeof( NSQ_del_dec_struct ) - i * sizeof( opus_int32) );
614             silk_memcpy( &psSampleState[ RDmax_ind ][ 0 ], &psSampleState[ RDmin_ind ][ 1 ], sizeof( NSQ_sample_struct ) );
615         }
616
617         /* Write samples from winner to output and long-term filter states */
618         psDD = &psDelDec[ Winner_ind ];
619         if( subfr > 0 || i >= decisionDelay ) {
620             pulses[  i - decisionDelay ] = (opus_int8)silk_RSHIFT_ROUND( psDD->Q_Q10[ last_smple_idx ], 10 );
621             xq[ i - decisionDelay ] = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND(
622                 silk_SMULWW( psDD->Xq_Q14[ last_smple_idx ], delayedGain_Q10[ last_smple_idx ] ), 8 ) );
623             NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - decisionDelay ] = psDD->Shape_Q14[ last_smple_idx ];
624             sLTP_Q15[          NSQ->sLTP_buf_idx     - decisionDelay ] = psDD->Pred_Q15[  last_smple_idx ];
625         }
626         NSQ->sLTP_shp_buf_idx++;
627         NSQ->sLTP_buf_idx++;
628
629         /* Update states */
630         for( k = 0; k < nStatesDelayedDecision; k++ ) {
631             psDD                                     = &psDelDec[ k ];
632             psSS                                     = &psSampleState[ k ][ 0 ];
633             psDD->LF_AR_Q14                          = psSS->LF_AR_Q14;
634             psDD->Diff_Q14                           = psSS->Diff_Q14;
635             psDD->sLPC_Q14[ NSQ_LPC_BUF_LENGTH + i ] = psSS->xq_Q14;
636             psDD->Xq_Q14[    *smpl_buf_idx ]         = psSS->xq_Q14;
637             psDD->Q_Q10[     *smpl_buf_idx ]         = psSS->Q_Q10;
638             psDD->Pred_Q15[  *smpl_buf_idx ]         = silk_LSHIFT32( psSS->LPC_exc_Q14, 1 );
639             psDD->Shape_Q14[ *smpl_buf_idx ]         = psSS->sLTP_shp_Q14;
640             psDD->Seed                               = silk_ADD32_ovflw( psDD->Seed, silk_RSHIFT_ROUND( psSS->Q_Q10, 10 ) );
641             psDD->RandState[ *smpl_buf_idx ]         = psDD->Seed;
642             psDD->RD_Q10                             = psSS->RD_Q10;
643         }
644         delayedGain_Q10[     *smpl_buf_idx ]         = Gain_Q10;
645     }
646     /* Update LPC states */
647     for( k = 0; k < nStatesDelayedDecision; k++ ) {
648         psDD = &psDelDec[ k ];
649         silk_memcpy( psDD->sLPC_Q14, &psDD->sLPC_Q14[ length ], NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
650     }
651     RESTORE_STACK;
652 }
653 #endif /* OVERRIDE_silk_noise_shape_quantizer_del_dec */
654
655 static OPUS_INLINE void silk_nsq_del_dec_scale_states(
656     const silk_encoder_state *psEncC,               /* I    Encoder State                       */
657     silk_nsq_state      *NSQ,                       /* I/O  NSQ state                           */
658     NSQ_del_dec_struct  psDelDec[],                 /* I/O  Delayed decision states             */
659     const opus_int16    x16[],                      /* I    Input                               */
660     opus_int32          x_sc_Q10[],                 /* O    Input scaled with 1/Gain in Q10     */
661     const opus_int16    sLTP[],                     /* I    Re-whitened LTP state in Q0         */
662     opus_int32          sLTP_Q15[],                 /* O    LTP state matching scaled input     */
663     opus_int            subfr,                      /* I    Subframe number                     */
664     opus_int            nStatesDelayedDecision,     /* I    Number of del dec states            */
665     const opus_int      LTP_scale_Q14,              /* I    LTP state scaling                   */
666     const opus_int32    Gains_Q16[ MAX_NB_SUBFR ],  /* I                                        */
667     const opus_int      pitchL[ MAX_NB_SUBFR ],     /* I    Pitch lag                           */
668     const opus_int      signal_type,                /* I    Signal type                         */
669     const opus_int      decisionDelay               /* I    Decision delay                      */
670 )
671 {
672     opus_int            i, k, lag;
673     opus_int32          gain_adj_Q16, inv_gain_Q31, inv_gain_Q26;
674     NSQ_del_dec_struct  *psDD;
675
676     lag          = pitchL[ subfr ];
677     inv_gain_Q31 = silk_INVERSE32_varQ( silk_max( Gains_Q16[ subfr ], 1 ), 47 );
678     silk_assert( inv_gain_Q31 != 0 );
679
680     /* Scale input */
681     inv_gain_Q26 = silk_RSHIFT_ROUND( inv_gain_Q31, 5 );
682     for( i = 0; i < psEncC->subfr_length; i++ ) {
683         x_sc_Q10[ i ] = silk_SMULWW( x16[ i ], inv_gain_Q26 );
684     }
685
686     /* After rewhitening the LTP state is un-scaled, so scale with inv_gain_Q16 */
687     if( NSQ->rewhite_flag ) {
688         if( subfr == 0 ) {
689             /* Do LTP downscaling */
690             inv_gain_Q31 = silk_LSHIFT( silk_SMULWB( inv_gain_Q31, LTP_scale_Q14 ), 2 );
691         }
692         for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {
693             silk_assert( i < MAX_FRAME_LENGTH );
694             sLTP_Q15[ i ] = silk_SMULWB( inv_gain_Q31, sLTP[ i ] );
695         }
696     }
697
698     /* Adjust for changing gain */
699     if( Gains_Q16[ subfr ] != NSQ->prev_gain_Q16 ) {
700         gain_adj_Q16 =  silk_DIV32_varQ( NSQ->prev_gain_Q16, Gains_Q16[ subfr ], 16 );
701
702         /* Scale long-term shaping state */
703         for( i = NSQ->sLTP_shp_buf_idx - psEncC->ltp_mem_length; i < NSQ->sLTP_shp_buf_idx; i++ ) {
704             NSQ->sLTP_shp_Q14[ i ] = silk_SMULWW( gain_adj_Q16, NSQ->sLTP_shp_Q14[ i ] );
705         }
706
707         /* Scale long-term prediction state */
708         if( signal_type == TYPE_VOICED && NSQ->rewhite_flag == 0 ) {
709             for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx - decisionDelay; i++ ) {
710                 sLTP_Q15[ i ] = silk_SMULWW( gain_adj_Q16, sLTP_Q15[ i ] );
711             }
712         }
713
714         for( k = 0; k < nStatesDelayedDecision; k++ ) {
715             psDD = &psDelDec[ k ];
716
717             /* Scale scalar states */
718             psDD->LF_AR_Q14 = silk_SMULWW( gain_adj_Q16, psDD->LF_AR_Q14 );
719             psDD->Diff_Q14 = silk_SMULWW( gain_adj_Q16, psDD->Diff_Q14 );
720
721             /* Scale short-term prediction and shaping states */
722             for( i = 0; i < NSQ_LPC_BUF_LENGTH; i++ ) {
723                 psDD->sLPC_Q14[ i ] = silk_SMULWW( gain_adj_Q16, psDD->sLPC_Q14[ i ] );
724             }
725             for( i = 0; i < MAX_SHAPE_LPC_ORDER; i++ ) {
726                 psDD->sAR2_Q14[ i ] = silk_SMULWW( gain_adj_Q16, psDD->sAR2_Q14[ i ] );
727             }
728             for( i = 0; i < DECISION_DELAY; i++ ) {
729                 psDD->Pred_Q15[  i ] = silk_SMULWW( gain_adj_Q16, psDD->Pred_Q15[  i ] );
730                 psDD->Shape_Q14[ i ] = silk_SMULWW( gain_adj_Q16, psDD->Shape_Q14[ i ] );
731             }
732         }
733
734         /* Save inverse gain */
735         NSQ->prev_gain_Q16 = Gains_Q16[ subfr ];
736     }
737 }