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