Avoiding the term "mode" in opus_demo
[opus.git] / silk / x86 / VAD_sse.c
1 /* Copyright (c) 2014, Cisco Systems, INC
2    Written by XiangMingZhu WeiZhou MinPeng YanWang
3
4    Redistribution and use in source and binary forms, with or without
5    modification, are permitted provided that the following conditions
6    are met:
7
8    - Redistributions of source code must retain the above copyright
9    notice, this list of conditions and the following disclaimer.
10
11    - Redistributions in binary form must reproduce the above copyright
12    notice, this list of conditions and the following disclaimer in the
13    documentation and/or other materials provided with the distribution.
14
15    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
16    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
17    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
18    A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
19    OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
20    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
21    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
22    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
23    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
24    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25    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 <xmmintrin.h>
33 #include <emmintrin.h>
34 #include <smmintrin.h>
35
36 #include "main.h"
37 #include "stack_alloc.h"
38
39 /* Weighting factors for tilt measure */
40 static const opus_int32 tiltWeights[ VAD_N_BANDS ] = { 30000, 6000, -12000, -12000 };
41
42 /***************************************/
43 /* Get the speech activity level in Q8 */
44 /***************************************/
45 opus_int silk_VAD_GetSA_Q8_sse4_1(                  /* O    Return value, 0 if success                  */
46     silk_encoder_state          *psEncC,            /* I/O  Encoder state                               */
47     const opus_int16            pIn[]               /* I    PCM input                                   */
48 )
49 {
50     opus_int   SA_Q15, pSNR_dB_Q7, input_tilt;
51     opus_int   decimated_framelength1, decimated_framelength2;
52     opus_int   decimated_framelength;
53     opus_int   dec_subframe_length, dec_subframe_offset, SNR_Q7, i, b, s;
54     opus_int32 sumSquared, smooth_coef_Q16;
55     opus_int16 HPstateTmp;
56     VARDECL( opus_int16, X );
57     opus_int32 Xnrg[ VAD_N_BANDS ];
58     opus_int32 NrgToNoiseRatio_Q8[ VAD_N_BANDS ];
59     opus_int32 speech_nrg, x_tmp;
60     opus_int   X_offset[ VAD_N_BANDS ];
61     opus_int   ret = 0;
62     silk_VAD_state *psSilk_VAD = &psEncC->sVAD;
63
64     SAVE_STACK;
65
66     /* Safety checks */
67     silk_assert( VAD_N_BANDS == 4 );
68     silk_assert( MAX_FRAME_LENGTH >= psEncC->frame_length );
69     silk_assert( psEncC->frame_length <= 512 );
70     silk_assert( psEncC->frame_length == 8 * silk_RSHIFT( psEncC->frame_length, 3 ) );
71
72     /***********************/
73     /* Filter and Decimate */
74     /***********************/
75     decimated_framelength1 = silk_RSHIFT( psEncC->frame_length, 1 );
76     decimated_framelength2 = silk_RSHIFT( psEncC->frame_length, 2 );
77     decimated_framelength = silk_RSHIFT( psEncC->frame_length, 3 );
78     /* Decimate into 4 bands:
79        0       L      3L       L              3L                             5L
80                -      --       -              --                             --
81                8       8       2               4                              4
82
83        [0-1 kHz| temp. |1-2 kHz|    2-4 kHz    |            4-8 kHz           |
84
85        They're arranged to allow the minimal ( frame_length / 4 ) extra
86        scratch space during the downsampling process */
87     X_offset[ 0 ] = 0;
88     X_offset[ 1 ] = decimated_framelength + decimated_framelength2;
89     X_offset[ 2 ] = X_offset[ 1 ] + decimated_framelength;
90     X_offset[ 3 ] = X_offset[ 2 ] + decimated_framelength2;
91     ALLOC( X, X_offset[ 3 ] + decimated_framelength1, opus_int16 );
92
93     /* 0-8 kHz to 0-4 kHz and 4-8 kHz */
94     silk_ana_filt_bank_1( pIn, &psSilk_VAD->AnaState[  0 ],
95         X, &X[ X_offset[ 3 ] ], psEncC->frame_length );
96
97     /* 0-4 kHz to 0-2 kHz and 2-4 kHz */
98     silk_ana_filt_bank_1( X, &psSilk_VAD->AnaState1[ 0 ],
99         X, &X[ X_offset[ 2 ] ], decimated_framelength1 );
100
101     /* 0-2 kHz to 0-1 kHz and 1-2 kHz */
102     silk_ana_filt_bank_1( X, &psSilk_VAD->AnaState2[ 0 ],
103         X, &X[ X_offset[ 1 ] ], decimated_framelength2 );
104
105     /*********************************************/
106     /* HP filter on lowest band (differentiator) */
107     /*********************************************/
108     X[ decimated_framelength - 1 ] = silk_RSHIFT( X[ decimated_framelength - 1 ], 1 );
109     HPstateTmp = X[ decimated_framelength - 1 ];
110     for( i = decimated_framelength - 1; i > 0; i-- ) {
111         X[ i - 1 ]  = silk_RSHIFT( X[ i - 1 ], 1 );
112         X[ i ]     -= X[ i - 1 ];
113     }
114     X[ 0 ] -= psSilk_VAD->HPstate;
115     psSilk_VAD->HPstate = HPstateTmp;
116
117     /*************************************/
118     /* Calculate the energy in each band */
119     /*************************************/
120     for( b = 0; b < VAD_N_BANDS; b++ ) {
121         /* Find the decimated framelength in the non-uniformly divided bands */
122         decimated_framelength = silk_RSHIFT( psEncC->frame_length, silk_min_int( VAD_N_BANDS - b, VAD_N_BANDS - 1 ) );
123
124         /* Split length into subframe lengths */
125         dec_subframe_length = silk_RSHIFT( decimated_framelength, VAD_INTERNAL_SUBFRAMES_LOG2 );
126         dec_subframe_offset = 0;
127
128         /* Compute energy per sub-frame */
129         /* initialize with summed energy of last subframe */
130         Xnrg[ b ] = psSilk_VAD->XnrgSubfr[ b ];
131         for( s = 0; s < VAD_INTERNAL_SUBFRAMES; s++ ) {
132             __m128i xmm_X, xmm_acc;
133             sumSquared = 0;
134
135             xmm_acc = _mm_setzero_si128();
136
137             for( i = 0; i < dec_subframe_length - 7; i += 8 )
138             {
139                 xmm_X   = _mm_loadu_si128( (__m128i *)&(X[ X_offset[ b ] + i + dec_subframe_offset ] ) );
140                 xmm_X   = _mm_srai_epi16( xmm_X, 3 );
141                 xmm_X   = _mm_madd_epi16( xmm_X, xmm_X );
142                 xmm_acc = _mm_add_epi32( xmm_acc, xmm_X );
143             }
144
145             xmm_acc = _mm_add_epi32( xmm_acc, _mm_unpackhi_epi64( xmm_acc, xmm_acc ) );
146             xmm_acc = _mm_add_epi32( xmm_acc, _mm_shufflelo_epi16( xmm_acc, 0x0E ) );
147
148             sumSquared += _mm_cvtsi128_si32( xmm_acc );
149
150             for( ; i < dec_subframe_length; i++ ) {
151                 /* The energy will be less than dec_subframe_length * ( silk_int16_MIN / 8 ) ^ 2.            */
152                 /* Therefore we can accumulate with no risk of overflow (unless dec_subframe_length > 128)  */
153                 x_tmp = silk_RSHIFT(
154                     X[ X_offset[ b ] + i + dec_subframe_offset ], 3 );
155                 sumSquared = silk_SMLABB( sumSquared, x_tmp, x_tmp );
156
157                 /* Safety check */
158                 silk_assert( sumSquared >= 0 );
159             }
160
161             /* Add/saturate summed energy of current subframe */
162             if( s < VAD_INTERNAL_SUBFRAMES - 1 ) {
163                 Xnrg[ b ] = silk_ADD_POS_SAT32( Xnrg[ b ], sumSquared );
164             } else {
165                 /* Look-ahead subframe */
166                 Xnrg[ b ] = silk_ADD_POS_SAT32( Xnrg[ b ], silk_RSHIFT( sumSquared, 1 ) );
167             }
168
169             dec_subframe_offset += dec_subframe_length;
170         }
171         psSilk_VAD->XnrgSubfr[ b ] = sumSquared;
172     }
173
174     /********************/
175     /* Noise estimation */
176     /********************/
177     silk_VAD_GetNoiseLevels( &Xnrg[ 0 ], psSilk_VAD );
178
179     /***********************************************/
180     /* Signal-plus-noise to noise ratio estimation */
181     /***********************************************/
182     sumSquared = 0;
183     input_tilt = 0;
184     for( b = 0; b < VAD_N_BANDS; b++ ) {
185         speech_nrg = Xnrg[ b ] - psSilk_VAD->NL[ b ];
186         if( speech_nrg > 0 ) {
187             /* Divide, with sufficient resolution */
188             if( ( Xnrg[ b ] & 0xFF800000 ) == 0 ) {
189                 NrgToNoiseRatio_Q8[ b ] = silk_DIV32( silk_LSHIFT( Xnrg[ b ], 8 ), psSilk_VAD->NL[ b ] + 1 );
190             } else {
191                 NrgToNoiseRatio_Q8[ b ] = silk_DIV32( Xnrg[ b ], silk_RSHIFT( psSilk_VAD->NL[ b ], 8 ) + 1 );
192             }
193
194             /* Convert to log domain */
195             SNR_Q7 = silk_lin2log( NrgToNoiseRatio_Q8[ b ] ) - 8 * 128;
196
197             /* Sum-of-squares */
198             sumSquared = silk_SMLABB( sumSquared, SNR_Q7, SNR_Q7 );          /* Q14 */
199
200             /* Tilt measure */
201             if( speech_nrg < ( (opus_int32)1 << 20 ) ) {
202                 /* Scale down SNR value for small subband speech energies */
203                 SNR_Q7 = silk_SMULWB( silk_LSHIFT( silk_SQRT_APPROX( speech_nrg ), 6 ), SNR_Q7 );
204             }
205             input_tilt = silk_SMLAWB( input_tilt, tiltWeights[ b ], SNR_Q7 );
206         } else {
207             NrgToNoiseRatio_Q8[ b ] = 256;
208         }
209     }
210
211     /* Mean-of-squares */
212     sumSquared = silk_DIV32_16( sumSquared, VAD_N_BANDS ); /* Q14 */
213
214     /* Root-mean-square approximation, scale to dBs, and write to output pointer */
215     pSNR_dB_Q7 = (opus_int16)( 3 * silk_SQRT_APPROX( sumSquared ) ); /* Q7 */
216
217     /*********************************/
218     /* Speech Probability Estimation */
219     /*********************************/
220     SA_Q15 = silk_sigm_Q15( silk_SMULWB( VAD_SNR_FACTOR_Q16, pSNR_dB_Q7 ) - VAD_NEGATIVE_OFFSET_Q5 );
221
222     /**************************/
223     /* Frequency Tilt Measure */
224     /**************************/
225     psEncC->input_tilt_Q15 = silk_LSHIFT( silk_sigm_Q15( input_tilt ) - 16384, 1 );
226
227     /**************************************************/
228     /* Scale the sigmoid output based on power levels */
229     /**************************************************/
230     speech_nrg = 0;
231     for( b = 0; b < VAD_N_BANDS; b++ ) {
232         /* Accumulate signal-without-noise energies, higher frequency bands have more weight */
233         speech_nrg += ( b + 1 ) * silk_RSHIFT( Xnrg[ b ] - psSilk_VAD->NL[ b ], 4 );
234     }
235
236     /* Power scaling */
237     if( speech_nrg <= 0 ) {
238         SA_Q15 = silk_RSHIFT( SA_Q15, 1 );
239     } else if( speech_nrg < 32768 ) {
240         if( psEncC->frame_length == 10 * psEncC->fs_kHz ) {
241             speech_nrg = silk_LSHIFT_SAT32( speech_nrg, 16 );
242         } else {
243             speech_nrg = silk_LSHIFT_SAT32( speech_nrg, 15 );
244         }
245
246         /* square-root */
247         speech_nrg = silk_SQRT_APPROX( speech_nrg );
248         SA_Q15 = silk_SMULWB( 32768 + speech_nrg, SA_Q15 );
249     }
250
251     /* Copy the resulting speech activity in Q8 */
252     psEncC->speech_activity_Q8 = silk_min_int( silk_RSHIFT( SA_Q15, 7 ), silk_uint8_MAX );
253
254     /***********************************/
255     /* Energy Level and SNR estimation */
256     /***********************************/
257     /* Smoothing coefficient */
258     smooth_coef_Q16 = silk_SMULWB( VAD_SNR_SMOOTH_COEF_Q18, silk_SMULWB( (opus_int32)SA_Q15, SA_Q15 ) );
259
260     if( psEncC->frame_length == 10 * psEncC->fs_kHz ) {
261         smooth_coef_Q16 >>= 1;
262     }
263
264     for( b = 0; b < VAD_N_BANDS; b++ ) {
265         /* compute smoothed energy-to-noise ratio per band */
266         psSilk_VAD->NrgRatioSmth_Q8[ b ] = silk_SMLAWB( psSilk_VAD->NrgRatioSmth_Q8[ b ],
267             NrgToNoiseRatio_Q8[ b ] - psSilk_VAD->NrgRatioSmth_Q8[ b ], smooth_coef_Q16 );
268
269         /* signal to noise ratio in dB per band */
270         SNR_Q7 = 3 * ( silk_lin2log( psSilk_VAD->NrgRatioSmth_Q8[b] ) - 8 * 128 );
271         /* quality = sigmoid( 0.25 * ( SNR_dB - 16 ) ); */
272         psEncC->input_quality_bands_Q15[ b ] = silk_sigm_Q15( silk_RSHIFT( SNR_Q7 - 16 * 128, 4 ) );
273     }
274
275     RESTORE_STACK;
276     return( ret );
277 }