Accuracy improvements to help float implementations
[opus.git] / silk / fixed / noise_shape_analysis_FIX.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_FIX.h"
33 #include "tuning_parameters.h"
34
35 /* Compute gain to make warped filter coefficients have a zero mean log frequency response on a     */
36 /* non-warped frequency scale. (So that it can be implemented with a minimum-phase monic filter.)   */
37 static inline opus_int32 warped_gain( /* gain in Q16*/
38     const opus_int32     *coefs_Q24,
39     opus_int             lambda_Q16,
40     opus_int             order
41 ) {
42     opus_int   i;
43     opus_int32 gain_Q24;
44
45     lambda_Q16 = -lambda_Q16;
46     gain_Q24 = coefs_Q24[ order - 1 ];
47     for( i = order - 2; i >= 0; i-- ) {
48         gain_Q24 = silk_SMLAWB( coefs_Q24[ i ], gain_Q24, lambda_Q16 );
49     }
50     gain_Q24  = silk_SMLAWB( SILK_FIX_CONST( 1.0, 24 ), gain_Q24, -lambda_Q16 );
51     return silk_INVERSE32_varQ( gain_Q24, 40 );
52 }
53
54 /* Convert warped filter coefficients to monic pseudo-warped coefficients and limit maximum     */
55 /* amplitude of monic warped coefficients by using bandwidth expansion on the true coefficients */
56 static inline void limit_warped_coefs(
57     opus_int32           *coefs_syn_Q24,
58     opus_int32           *coefs_ana_Q24,
59     opus_int             lambda_Q16,
60     opus_int32           limit_Q24,
61     opus_int             order
62 ) {
63     opus_int   i, iter, ind = 0;
64     opus_int32 tmp, maxabs_Q24, chirp_Q16, gain_syn_Q16, gain_ana_Q16;
65     opus_int32 nom_Q16, den_Q24;
66
67     /* Convert to monic coefficients */
68     lambda_Q16 = -lambda_Q16;
69     for( i = order - 1; i > 0; i-- ) {
70         coefs_syn_Q24[ i - 1 ] = silk_SMLAWB( coefs_syn_Q24[ i - 1 ], coefs_syn_Q24[ i ], lambda_Q16 );
71         coefs_ana_Q24[ i - 1 ] = silk_SMLAWB( coefs_ana_Q24[ i - 1 ], coefs_ana_Q24[ i ], lambda_Q16 );
72     }
73     lambda_Q16 = -lambda_Q16;
74     nom_Q16  = silk_SMLAWB( SILK_FIX_CONST( 1.0, 16 ), -lambda_Q16,        lambda_Q16 );
75     den_Q24  = silk_SMLAWB( SILK_FIX_CONST( 1.0, 24 ), coefs_syn_Q24[ 0 ], lambda_Q16 );
76     gain_syn_Q16 = silk_DIV32_varQ( nom_Q16, den_Q24, 24 );
77     den_Q24  = silk_SMLAWB( SILK_FIX_CONST( 1.0, 24 ), coefs_ana_Q24[ 0 ], lambda_Q16 );
78     gain_ana_Q16 = silk_DIV32_varQ( nom_Q16, den_Q24, 24 );
79     for( i = 0; i < order; i++ ) {
80         coefs_syn_Q24[ i ] = silk_SMULWW( gain_syn_Q16, coefs_syn_Q24[ i ] );
81         coefs_ana_Q24[ i ] = silk_SMULWW( gain_ana_Q16, coefs_ana_Q24[ i ] );
82     }
83
84     for( iter = 0; iter < 10; iter++ ) {
85         /* Find maximum absolute value */
86         maxabs_Q24 = -1;
87         for( i = 0; i < order; i++ ) {
88             tmp = silk_max( silk_abs_int32( coefs_syn_Q24[ i ] ), silk_abs_int32( coefs_ana_Q24[ i ] ) );
89             if( tmp > maxabs_Q24 ) {
90                 maxabs_Q24 = tmp;
91                 ind = i;
92             }
93         }
94         if( maxabs_Q24 <= limit_Q24 ) {
95             /* Coefficients are within range - done */
96             return;
97         }
98
99         /* Convert back to true warped coefficients */
100         for( i = 1; i < order; i++ ) {
101             coefs_syn_Q24[ i - 1 ] = silk_SMLAWB( coefs_syn_Q24[ i - 1 ], coefs_syn_Q24[ i ], lambda_Q16 );
102             coefs_ana_Q24[ i - 1 ] = silk_SMLAWB( coefs_ana_Q24[ i - 1 ], coefs_ana_Q24[ i ], lambda_Q16 );
103         }
104         gain_syn_Q16 = silk_INVERSE32_varQ( gain_syn_Q16, 32 );
105         gain_ana_Q16 = silk_INVERSE32_varQ( gain_ana_Q16, 32 );
106         for( i = 0; i < order; i++ ) {
107             coefs_syn_Q24[ i ] = silk_SMULWW( gain_syn_Q16, coefs_syn_Q24[ i ] );
108             coefs_ana_Q24[ i ] = silk_SMULWW( gain_ana_Q16, coefs_ana_Q24[ i ] );
109         }
110
111         /* Apply bandwidth expansion */
112         chirp_Q16 = SILK_FIX_CONST( 0.99, 16 ) - silk_DIV32_varQ(
113             silk_SMULWB( maxabs_Q24 - limit_Q24, silk_SMLABB( SILK_FIX_CONST( 0.8, 10 ), SILK_FIX_CONST( 0.1, 10 ), iter ) ),
114             silk_MUL( maxabs_Q24, ind + 1 ), 22 );
115         silk_bwexpander_32( coefs_syn_Q24, order, chirp_Q16 );
116         silk_bwexpander_32( coefs_ana_Q24, order, chirp_Q16 );
117
118         /* Convert to monic warped coefficients */
119         lambda_Q16 = -lambda_Q16;
120         for( i = order - 1; i > 0; i-- ) {
121             coefs_syn_Q24[ i - 1 ] = silk_SMLAWB( coefs_syn_Q24[ i - 1 ], coefs_syn_Q24[ i ], lambda_Q16 );
122             coefs_ana_Q24[ i - 1 ] = silk_SMLAWB( coefs_ana_Q24[ i - 1 ], coefs_ana_Q24[ i ], lambda_Q16 );
123         }
124         lambda_Q16 = -lambda_Q16;
125         nom_Q16  = silk_SMLAWB( SILK_FIX_CONST( 1.0, 16 ), -lambda_Q16,        lambda_Q16 );
126         den_Q24  = silk_SMLAWB( SILK_FIX_CONST( 1.0, 24 ), coefs_syn_Q24[ 0 ], lambda_Q16 );
127         gain_syn_Q16 = silk_DIV32_varQ( nom_Q16, den_Q24, 24 );
128         den_Q24  = silk_SMLAWB( SILK_FIX_CONST( 1.0, 24 ), coefs_ana_Q24[ 0 ], lambda_Q16 );
129         gain_ana_Q16 = silk_DIV32_varQ( nom_Q16, den_Q24, 24 );
130         for( i = 0; i < order; i++ ) {
131             coefs_syn_Q24[ i ] = silk_SMULWW( gain_syn_Q16, coefs_syn_Q24[ i ] );
132             coefs_ana_Q24[ i ] = silk_SMULWW( gain_ana_Q16, coefs_ana_Q24[ i ] );
133         }
134     }
135     silk_assert( 0 );
136 }
137
138 /**************************************************************/
139 /* Compute noise shaping coefficients and initial gain values */
140 /**************************************************************/
141 void silk_noise_shape_analysis_FIX(
142     silk_encoder_state_FIX          *psEnc,                                 /* I/O  Encoder state FIX                                                           */
143     silk_encoder_control_FIX        *psEncCtrl,                             /* I/O  Encoder control FIX                                                         */
144     const opus_int16                *pitch_res,                             /* I    LPC residual from pitch analysis                                            */
145     const opus_int16                *x                                      /* I    Input signal [ frame_length + la_shape ]                                    */
146 )
147 {
148     silk_shape_state_FIX *psShapeSt = &psEnc->sShape;
149     opus_int     k, i, nSamples, Qnrg, b_Q14, warping_Q16, scale = 0;
150     opus_int32   SNR_adj_dB_Q7, HarmBoost_Q16, HarmShapeGain_Q16, Tilt_Q16, tmp32;
151     opus_int32   nrg, pre_nrg_Q30, log_energy_Q7, log_energy_prev_Q7, energy_variation_Q7;
152     opus_int32   delta_Q16, BWExp1_Q16, BWExp2_Q16, gain_mult_Q16, gain_add_Q16, strength_Q16, b_Q8;
153     opus_int32   auto_corr[     MAX_SHAPE_LPC_ORDER + 1 ];
154     opus_int32   refl_coef_Q16[ MAX_SHAPE_LPC_ORDER ];
155     opus_int32   AR1_Q24[       MAX_SHAPE_LPC_ORDER ];
156     opus_int32   AR2_Q24[       MAX_SHAPE_LPC_ORDER ];
157     opus_int16   x_windowed[    SHAPE_LPC_WIN_MAX ];
158     const opus_int16 *x_ptr, *pitch_res_ptr;
159
160     /* Point to start of first LPC analysis block */
161     x_ptr = x - psEnc->sCmn.la_shape;
162
163     /****************/
164     /* GAIN CONTROL */
165     /****************/
166     SNR_adj_dB_Q7 = psEnc->sCmn.SNR_dB_Q7;
167
168     /* Input quality is the average of the quality in the lowest two VAD bands */
169     psEncCtrl->input_quality_Q14 = ( opus_int )silk_RSHIFT( (opus_int32)psEnc->sCmn.input_quality_bands_Q15[ 0 ]
170         + psEnc->sCmn.input_quality_bands_Q15[ 1 ], 2 );
171
172     /* Coding quality level, between 0.0_Q0 and 1.0_Q0, but in Q14 */
173     psEncCtrl->coding_quality_Q14 = silk_RSHIFT( silk_sigm_Q15( silk_RSHIFT_ROUND( SNR_adj_dB_Q7 -
174         SILK_FIX_CONST( 20.0, 7 ), 4 ) ), 1 );
175
176     /* Reduce coding SNR during low speech activity */
177     if( psEnc->sCmn.useCBR == 0 ) {
178         b_Q8 = SILK_FIX_CONST( 1.0, 8 ) - psEnc->sCmn.speech_activity_Q8;
179         b_Q8 = silk_SMULWB( silk_LSHIFT( b_Q8, 8 ), b_Q8 );
180         SNR_adj_dB_Q7 = silk_SMLAWB( SNR_adj_dB_Q7,
181             silk_SMULBB( SILK_FIX_CONST( -BG_SNR_DECR_dB, 7 ) >> ( 4 + 1 ), b_Q8 ),                                       /* Q11*/
182             silk_SMULWB( SILK_FIX_CONST( 1.0, 14 ) + psEncCtrl->input_quality_Q14, psEncCtrl->coding_quality_Q14 ) );     /* Q12*/
183     }
184
185     if( psEnc->sCmn.indices.signalType == TYPE_VOICED ) {
186         /* Reduce gains for periodic signals */
187         SNR_adj_dB_Q7 = silk_SMLAWB( SNR_adj_dB_Q7, SILK_FIX_CONST( HARM_SNR_INCR_dB, 8 ), psEnc->LTPCorr_Q15 );
188     } else {
189         /* For unvoiced signals and low-quality input, adjust the quality slower than SNR_dB setting */
190         SNR_adj_dB_Q7 = silk_SMLAWB( SNR_adj_dB_Q7,
191             silk_SMLAWB( SILK_FIX_CONST( 6.0, 9 ), -SILK_FIX_CONST( 0.4, 18 ), psEnc->sCmn.SNR_dB_Q7 ),
192             SILK_FIX_CONST( 1.0, 14 ) - psEncCtrl->input_quality_Q14 );
193     }
194
195     /*************************/
196     /* SPARSENESS PROCESSING */
197     /*************************/
198     /* Set quantizer offset */
199     if( psEnc->sCmn.indices.signalType == TYPE_VOICED ) {
200         /* Initally set to 0; may be overruled in process_gains(..) */
201         psEnc->sCmn.indices.quantOffsetType = 0;
202         psEncCtrl->sparseness_Q8 = 0;
203     } else {
204         /* Sparseness measure, based on relative fluctuations of energy per 2 milliseconds */
205         nSamples = silk_LSHIFT( psEnc->sCmn.fs_kHz, 1 );
206         energy_variation_Q7 = 0;
207         log_energy_prev_Q7  = 0;
208         pitch_res_ptr = pitch_res;
209         for( k = 0; k < silk_SMULBB( SUB_FRAME_LENGTH_MS, psEnc->sCmn.nb_subfr ) / 2; k++ ) {
210             silk_sum_sqr_shift( &nrg, &scale, pitch_res_ptr, nSamples );
211             nrg += silk_RSHIFT( nSamples, scale );           /* Q(-scale)*/
212
213             log_energy_Q7 = silk_lin2log( nrg );
214             if( k > 0 ) {
215                 energy_variation_Q7 += silk_abs( log_energy_Q7 - log_energy_prev_Q7 );
216             }
217             log_energy_prev_Q7 = log_energy_Q7;
218             pitch_res_ptr += nSamples;
219         }
220
221         psEncCtrl->sparseness_Q8 = silk_RSHIFT( silk_sigm_Q15( silk_SMULWB( energy_variation_Q7 -
222             SILK_FIX_CONST( 5.0, 7 ), SILK_FIX_CONST( 0.1, 16 ) ) ), 7 );
223
224         /* Set quantization offset depending on sparseness measure */
225         if( psEncCtrl->sparseness_Q8 > SILK_FIX_CONST( SPARSENESS_THRESHOLD_QNT_OFFSET, 8 ) ) {
226             psEnc->sCmn.indices.quantOffsetType = 0;
227         } else {
228             psEnc->sCmn.indices.quantOffsetType = 1;
229         }
230
231         /* Increase coding SNR for sparse signals */
232         SNR_adj_dB_Q7 = silk_SMLAWB( SNR_adj_dB_Q7, SILK_FIX_CONST( SPARSE_SNR_INCR_dB, 15 ), psEncCtrl->sparseness_Q8 - SILK_FIX_CONST( 0.5, 8 ) );
233     }
234
235     /*******************************/
236     /* Control bandwidth expansion */
237     /*******************************/
238     /* More BWE for signals with high prediction gain */
239     strength_Q16 = silk_SMULWB( psEncCtrl->predGain_Q16, SILK_FIX_CONST( FIND_PITCH_WHITE_NOISE_FRACTION, 16 ) );
240     BWExp1_Q16 = BWExp2_Q16 = silk_DIV32_varQ( SILK_FIX_CONST( BANDWIDTH_EXPANSION, 16 ),
241         silk_SMLAWW( SILK_FIX_CONST( 1.0, 16 ), strength_Q16, strength_Q16 ), 16 );
242     delta_Q16  = silk_SMULWB( SILK_FIX_CONST( 1.0, 16 ) - silk_SMULBB( 3, psEncCtrl->coding_quality_Q14 ),
243         SILK_FIX_CONST( LOW_RATE_BANDWIDTH_EXPANSION_DELTA, 16 ) );
244     BWExp1_Q16 = silk_SUB32( BWExp1_Q16, delta_Q16 );
245     BWExp2_Q16 = silk_ADD32( BWExp2_Q16, delta_Q16 );
246     /* BWExp1 will be applied after BWExp2, so make it relative */
247     BWExp1_Q16 = silk_DIV32_16( silk_LSHIFT( BWExp1_Q16, 14 ), silk_RSHIFT( BWExp2_Q16, 2 ) );
248
249     if( psEnc->sCmn.warping_Q16 > 0 ) {
250         /* Slightly more warping in analysis will move quantization noise up in frequency, where it's better masked */
251         warping_Q16 = silk_SMLAWB( psEnc->sCmn.warping_Q16, psEncCtrl->coding_quality_Q14, SILK_FIX_CONST( 0.01, 18 ) );
252     } else {
253         warping_Q16 = 0;
254     }
255
256     /********************************************/
257     /* Compute noise shaping AR coefs and gains */
258     /********************************************/
259     for( k = 0; k < psEnc->sCmn.nb_subfr; k++ ) {
260         /* Apply window: sine slope followed by flat part followed by cosine slope */
261         opus_int shift, slope_part, flat_part;
262         flat_part = psEnc->sCmn.fs_kHz * 3;
263         slope_part = silk_RSHIFT( psEnc->sCmn.shapeWinLength - flat_part, 1 );
264
265         silk_apply_sine_window( x_windowed, x_ptr, 1, slope_part );
266         shift = slope_part;
267         silk_memcpy( x_windowed + shift, x_ptr + shift, flat_part * sizeof(opus_int16) );
268         shift += flat_part;
269         silk_apply_sine_window( x_windowed + shift, x_ptr + shift, 2, slope_part );
270
271         /* Update pointer: next LPC analysis block */
272         x_ptr += psEnc->sCmn.subfr_length;
273
274         if( psEnc->sCmn.warping_Q16 > 0 ) {
275             /* Calculate warped auto correlation */
276             silk_warped_autocorrelation_FIX( auto_corr, &scale, x_windowed, warping_Q16, psEnc->sCmn.shapeWinLength, psEnc->sCmn.shapingLPCOrder );
277         } else {
278             /* Calculate regular auto correlation */
279             silk_autocorr( auto_corr, &scale, x_windowed, psEnc->sCmn.shapeWinLength, psEnc->sCmn.shapingLPCOrder + 1 );
280         }
281
282         /* Add white noise, as a fraction of energy */
283         auto_corr[0] = silk_ADD32( auto_corr[0], silk_max_32( silk_SMULWB( silk_RSHIFT( auto_corr[ 0 ], 4 ),
284             SILK_FIX_CONST( SHAPE_WHITE_NOISE_FRACTION, 20 ) ), 1 ) );
285
286         /* Calculate the reflection coefficients using schur */
287         nrg = silk_schur64( refl_coef_Q16, auto_corr, psEnc->sCmn.shapingLPCOrder );
288         silk_assert( nrg >= 0 );
289
290         /* Convert reflection coefficients to prediction coefficients */
291         silk_k2a_Q16( AR2_Q24, refl_coef_Q16, psEnc->sCmn.shapingLPCOrder );
292
293         Qnrg = -scale;          /* range: -12...30*/
294         silk_assert( Qnrg >= -12 );
295         silk_assert( Qnrg <=  30 );
296
297         /* Make sure that Qnrg is an even number */
298         if( Qnrg & 1 ) {
299             Qnrg -= 1;
300             nrg >>= 1;
301         }
302
303         tmp32 = silk_SQRT_APPROX( nrg );
304         Qnrg >>= 1;             /* range: -6...15*/
305
306         psEncCtrl->Gains_Q16[ k ] = silk_LSHIFT_SAT32( tmp32, 16 - Qnrg );
307
308         if( psEnc->sCmn.warping_Q16 > 0 ) {
309             /* Adjust gain for warping */
310             gain_mult_Q16 = warped_gain( AR2_Q24, warping_Q16, psEnc->sCmn.shapingLPCOrder );
311             silk_assert( psEncCtrl->Gains_Q16[ k ] >= 0 );
312             psEncCtrl->Gains_Q16[ k ] = silk_SMULWW( psEncCtrl->Gains_Q16[ k ], gain_mult_Q16 );
313             if( psEncCtrl->Gains_Q16[ k ] < 0 ) {
314                 psEncCtrl->Gains_Q16[ k ] = silk_int32_MAX;
315             }
316         }
317
318         /* Bandwidth expansion for synthesis filter shaping */
319         silk_bwexpander_32( AR2_Q24, psEnc->sCmn.shapingLPCOrder, BWExp2_Q16 );
320
321         /* Compute noise shaping filter coefficients */
322         silk_memcpy( AR1_Q24, AR2_Q24, psEnc->sCmn.shapingLPCOrder * sizeof( opus_int32 ) );
323
324         /* Bandwidth expansion for analysis filter shaping */
325         silk_assert( BWExp1_Q16 <= SILK_FIX_CONST( 1.0, 16 ) );
326         silk_bwexpander_32( AR1_Q24, psEnc->sCmn.shapingLPCOrder, BWExp1_Q16 );
327
328         /* Ratio of prediction gains, in energy domain */
329         pre_nrg_Q30 = silk_LPC_inverse_pred_gain_Q24( AR2_Q24, psEnc->sCmn.shapingLPCOrder );
330         nrg         = silk_LPC_inverse_pred_gain_Q24( AR1_Q24, psEnc->sCmn.shapingLPCOrder );
331
332         /*psEncCtrl->GainsPre[ k ] = 1.0f - 0.7f * ( 1.0f - pre_nrg / nrg ) = 0.3f + 0.7f * pre_nrg / nrg;*/
333         pre_nrg_Q30 = silk_LSHIFT32( silk_SMULWB( pre_nrg_Q30, SILK_FIX_CONST( 0.7, 15 ) ), 1 );
334         psEncCtrl->GainsPre_Q14[ k ] = ( opus_int ) SILK_FIX_CONST( 0.3, 14 ) + silk_DIV32_varQ( pre_nrg_Q30, nrg, 14 );
335
336         /* Convert to monic warped prediction coefficients and limit absolute values */
337         limit_warped_coefs( AR2_Q24, AR1_Q24, warping_Q16, SILK_FIX_CONST( 3.999, 24 ), psEnc->sCmn.shapingLPCOrder );
338
339         /* Convert from Q24 to Q13 and store in int16 */
340         for( i = 0; i < psEnc->sCmn.shapingLPCOrder; i++ ) {
341             psEncCtrl->AR1_Q13[ k * MAX_SHAPE_LPC_ORDER + i ] = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND( AR1_Q24[ i ], 11 ) );
342             psEncCtrl->AR2_Q13[ k * MAX_SHAPE_LPC_ORDER + i ] = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND( AR2_Q24[ i ], 11 ) );
343         }
344     }
345
346     /*****************/
347     /* Gain tweaking */
348     /*****************/
349     /* Increase gains during low speech activity and put lower limit on gains */
350     gain_mult_Q16 = silk_log2lin( -silk_SMLAWB( -SILK_FIX_CONST( 16.0, 7 ), SNR_adj_dB_Q7, SILK_FIX_CONST( 0.16, 16 ) ) );
351     gain_add_Q16  = silk_log2lin(  silk_SMLAWB(  SILK_FIX_CONST( 16.0, 7 ), SILK_FIX_CONST( MIN_QGAIN_DB, 7 ), SILK_FIX_CONST( 0.16, 16 ) ) );
352     silk_assert( gain_mult_Q16 > 0 );
353     for( k = 0; k < psEnc->sCmn.nb_subfr; k++ ) {
354         psEncCtrl->Gains_Q16[ k ] = silk_SMULWW( psEncCtrl->Gains_Q16[ k ], gain_mult_Q16 );
355         silk_assert( psEncCtrl->Gains_Q16[ k ] >= 0 );
356         psEncCtrl->Gains_Q16[ k ] = silk_ADD_POS_SAT32( psEncCtrl->Gains_Q16[ k ], gain_add_Q16 );
357     }
358
359     gain_mult_Q16 = SILK_FIX_CONST( 1.0, 16 ) + silk_RSHIFT_ROUND( silk_MLA( SILK_FIX_CONST( INPUT_TILT, 26 ),
360         psEncCtrl->coding_quality_Q14, SILK_FIX_CONST( HIGH_RATE_INPUT_TILT, 12 ) ), 10 );
361     for( k = 0; k < psEnc->sCmn.nb_subfr; k++ ) {
362         psEncCtrl->GainsPre_Q14[ k ] = silk_SMULWB( gain_mult_Q16, psEncCtrl->GainsPre_Q14[ k ] );
363     }
364
365     /************************************************/
366     /* Control low-frequency shaping and noise tilt */
367     /************************************************/
368     /* Less low frequency shaping for noisy inputs */
369     strength_Q16 = silk_MUL( SILK_FIX_CONST( LOW_FREQ_SHAPING, 4 ), silk_SMLAWB( SILK_FIX_CONST( 1.0, 12 ),
370         SILK_FIX_CONST( LOW_QUALITY_LOW_FREQ_SHAPING_DECR, 13 ), psEnc->sCmn.input_quality_bands_Q15[ 0 ] - SILK_FIX_CONST( 1.0, 15 ) ) );
371     strength_Q16 = silk_RSHIFT( silk_MUL( strength_Q16, psEnc->sCmn.speech_activity_Q8 ), 8 );
372     if( psEnc->sCmn.indices.signalType == TYPE_VOICED ) {
373         /* Reduce low frequencies quantization noise for periodic signals, depending on pitch lag */
374         /*f = 400; freqz([1, -0.98 + 2e-4 * f], [1, -0.97 + 7e-4 * f], 2^12, Fs); axis([0, 1000, -10, 1])*/
375         opus_int fs_kHz_inv = silk_DIV32_16( SILK_FIX_CONST( 0.2, 14 ), psEnc->sCmn.fs_kHz );
376         for( k = 0; k < psEnc->sCmn.nb_subfr; k++ ) {
377             b_Q14 = fs_kHz_inv + silk_DIV32_16( SILK_FIX_CONST( 3.0, 14 ), psEncCtrl->pitchL[ k ] );
378             /* Pack two coefficients in one int32 */
379             psEncCtrl->LF_shp_Q14[ k ]  = silk_LSHIFT( SILK_FIX_CONST( 1.0, 14 ) - b_Q14 - silk_SMULWB( strength_Q16, b_Q14 ), 16 );
380             psEncCtrl->LF_shp_Q14[ k ] |= (opus_uint16)( b_Q14 - SILK_FIX_CONST( 1.0, 14 ) );
381         }
382         silk_assert( SILK_FIX_CONST( HARM_HP_NOISE_COEF, 24 ) < SILK_FIX_CONST( 0.5, 24 ) ); /* Guarantees that second argument to SMULWB() is within range of an opus_int16*/
383         Tilt_Q16 = - SILK_FIX_CONST( HP_NOISE_COEF, 16 ) -
384             silk_SMULWB( SILK_FIX_CONST( 1.0, 16 ) - SILK_FIX_CONST( HP_NOISE_COEF, 16 ),
385                 silk_SMULWB( SILK_FIX_CONST( HARM_HP_NOISE_COEF, 24 ), psEnc->sCmn.speech_activity_Q8 ) );
386     } else {
387         b_Q14 = silk_DIV32_16( 21299, psEnc->sCmn.fs_kHz ); /* 1.3_Q0 = 21299_Q14*/
388         /* Pack two coefficients in one int32 */
389         psEncCtrl->LF_shp_Q14[ 0 ]  = silk_LSHIFT( SILK_FIX_CONST( 1.0, 14 ) - b_Q14 -
390             silk_SMULWB( strength_Q16, silk_SMULWB( SILK_FIX_CONST( 0.6, 16 ), b_Q14 ) ), 16 );
391         psEncCtrl->LF_shp_Q14[ 0 ] |= (opus_uint16)( b_Q14 - SILK_FIX_CONST( 1.0, 14 ) );
392         for( k = 1; k < psEnc->sCmn.nb_subfr; k++ ) {
393             psEncCtrl->LF_shp_Q14[ k ] = psEncCtrl->LF_shp_Q14[ 0 ];
394         }
395         Tilt_Q16 = -SILK_FIX_CONST( HP_NOISE_COEF, 16 );
396     }
397
398     /****************************/
399     /* HARMONIC SHAPING CONTROL */
400     /****************************/
401     /* Control boosting of harmonic frequencies */
402     HarmBoost_Q16 = silk_SMULWB( silk_SMULWB( SILK_FIX_CONST( 1.0, 17 ) - silk_LSHIFT( psEncCtrl->coding_quality_Q14, 3 ),
403         psEnc->LTPCorr_Q15 ), SILK_FIX_CONST( LOW_RATE_HARMONIC_BOOST, 16 ) );
404
405     /* More harmonic boost for noisy input signals */
406     HarmBoost_Q16 = silk_SMLAWB( HarmBoost_Q16,
407         SILK_FIX_CONST( 1.0, 16 ) - silk_LSHIFT( psEncCtrl->input_quality_Q14, 2 ), SILK_FIX_CONST( LOW_INPUT_QUALITY_HARMONIC_BOOST, 16 ) );
408
409     if( USE_HARM_SHAPING && psEnc->sCmn.indices.signalType == TYPE_VOICED ) {
410         /* More harmonic noise shaping for high bitrates or noisy input */
411         HarmShapeGain_Q16 = silk_SMLAWB( SILK_FIX_CONST( HARMONIC_SHAPING, 16 ),
412                 SILK_FIX_CONST( 1.0, 16 ) - silk_SMULWB( SILK_FIX_CONST( 1.0, 18 ) - silk_LSHIFT( psEncCtrl->coding_quality_Q14, 4 ),
413                 psEncCtrl->input_quality_Q14 ), SILK_FIX_CONST( HIGH_RATE_OR_LOW_QUALITY_HARMONIC_SHAPING, 16 ) );
414
415         /* Less harmonic noise shaping for less periodic signals */
416         HarmShapeGain_Q16 = silk_SMULWB( silk_LSHIFT( HarmShapeGain_Q16, 1 ),
417             silk_SQRT_APPROX( silk_LSHIFT( psEnc->LTPCorr_Q15, 15 ) ) );
418     } else {
419         HarmShapeGain_Q16 = 0;
420     }
421
422     /*************************/
423     /* Smooth over subframes */
424     /*************************/
425     for( k = 0; k < MAX_NB_SUBFR; k++ ) {
426         psShapeSt->HarmBoost_smth_Q16 =
427             silk_SMLAWB( psShapeSt->HarmBoost_smth_Q16,     HarmBoost_Q16     - psShapeSt->HarmBoost_smth_Q16,     SILK_FIX_CONST( SUBFR_SMTH_COEF, 16 ) );
428         psShapeSt->HarmShapeGain_smth_Q16 =
429             silk_SMLAWB( psShapeSt->HarmShapeGain_smth_Q16, HarmShapeGain_Q16 - psShapeSt->HarmShapeGain_smth_Q16, SILK_FIX_CONST( SUBFR_SMTH_COEF, 16 ) );
430         psShapeSt->Tilt_smth_Q16 =
431             silk_SMLAWB( psShapeSt->Tilt_smth_Q16,          Tilt_Q16          - psShapeSt->Tilt_smth_Q16,          SILK_FIX_CONST( SUBFR_SMTH_COEF, 16 ) );
432
433         psEncCtrl->HarmBoost_Q14[ k ]     = ( opus_int )silk_RSHIFT_ROUND( psShapeSt->HarmBoost_smth_Q16,     2 );
434         psEncCtrl->HarmShapeGain_Q14[ k ] = ( opus_int )silk_RSHIFT_ROUND( psShapeSt->HarmShapeGain_smth_Q16, 2 );
435         psEncCtrl->Tilt_Q14[ k ]          = ( opus_int )silk_RSHIFT_ROUND( psShapeSt->Tilt_smth_Q16,          2 );
436     }
437 }