SILK update with LBRR and some bugfixes
[opus.git] / src_common / SKP_Silk_VAD.c
1 /***********************************************************************\r
2 Copyright (c) 2006-2011, Skype Limited. All rights reserved. \r
3 Redistribution and use in source and binary forms, with or without \r
4 modification, (subject to the limitations in the disclaimer below) \r
5 are permitted provided that the following conditions are met:\r
6 - Redistributions of source code must retain the above copyright notice,\r
7 this list of conditions and the following disclaimer.\r
8 - Redistributions in binary form must reproduce the above copyright \r
9 notice, this list of conditions and the following disclaimer in the \r
10 documentation and/or other materials provided with the distribution.\r
11 - Neither the name of Skype Limited, nor the names of specific \r
12 contributors, may be used to endorse or promote products derived from \r
13 this software without specific prior written permission.\r
14 NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED \r
15 BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND \r
16 CONTRIBUTORS ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,\r
17 BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND \r
18 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE \r
19 COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, \r
20 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT\r
21 NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF \r
22 USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON \r
23 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT \r
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE \r
25 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\r
26 ***********************************************************************/\r
27 \r
28 #include <stdlib.h>\r
29 #include "SKP_Silk_main.h"\r
30 \r
31 #define SKP_SILK_VAD_HANDLE_10MS_FRAMES     1\r
32 \r
33 /**********************************/\r
34 /* Initialization of the Silk VAD */\r
35 /**********************************/\r
36 SKP_int SKP_Silk_VAD_Init(                              /* O    Return value, 0 if success                  */ \r
37     SKP_Silk_VAD_state              *psSilk_VAD         /* I/O  Pointer to Silk VAD state                   */ \r
38 )\r
39 {\r
40     SKP_int b, ret = 0;\r
41 \r
42     /* reset state memory */\r
43     SKP_memset( psSilk_VAD, 0, sizeof( SKP_Silk_VAD_state ) );\r
44 \r
45     /* init noise levels */\r
46     /* Initialize array with approx pink noise levels (psd proportional to inverse of frequency) */\r
47     for( b = 0; b < VAD_N_BANDS; b++ ) {\r
48         psSilk_VAD->NoiseLevelBias[ b ] = SKP_max_32( SKP_DIV32_16( VAD_NOISE_LEVELS_BIAS, b + 1 ), 1 );\r
49     }\r
50 \r
51     /* Initialize state */\r
52     for( b = 0; b < VAD_N_BANDS; b++ ) {\r
53         psSilk_VAD->NL[ b ]     = SKP_MUL( 100, psSilk_VAD->NoiseLevelBias[ b ] );\r
54         psSilk_VAD->inv_NL[ b ] = SKP_DIV32( SKP_int32_MAX, psSilk_VAD->NL[ b ] );\r
55     }\r
56     psSilk_VAD->counter = 15;\r
57 \r
58     /* init smoothed energy-to-noise ratio*/\r
59     for( b = 0; b < VAD_N_BANDS; b++ ) {\r
60         psSilk_VAD->NrgRatioSmth_Q8[ b ] = 100 * 256;       /* 100 * 256 --> 20 dB SNR */\r
61     }\r
62 \r
63     return( ret );\r
64 }\r
65 \r
66 /* Weighting factors for tilt measure */\r
67 const static SKP_int32 tiltWeights[ VAD_N_BANDS ] = { 30000, 6000, -12000, -12000 };\r
68 \r
69 /***************************************/\r
70 /* Get the speech activity level in Q8 */\r
71 /***************************************/\r
72 SKP_int SKP_Silk_VAD_GetSA_Q8(                                  /* O    Return value, 0 if success      */\r
73     SKP_Silk_VAD_state          *psSilk_VAD,                    /* I/O  Silk VAD state                  */\r
74     SKP_int                     *pSA_Q8,                        /* O    Speech activity level in Q8     */\r
75     SKP_int                     *pSNR_dB_Q7,                    /* O    SNR for current frame in Q7     */\r
76     SKP_int                     pQuality_Q15[ VAD_N_BANDS ],    /* O    Smoothed SNR for each band      */\r
77     SKP_int                     *pTilt_Q15,                     /* O    current frame's frequency tilt  */\r
78     const SKP_int16             pIn[],                          /* I    PCM input       [framelength]   */\r
79     const SKP_int               framelength,                    /* I    Input frame length              */\r
80     const SKP_int               fs_kHz                          /* I    Input frame sample frequency    */\r
81 )\r
82 {\r
83     SKP_int   SA_Q15, input_tilt;\r
84     SKP_int32 scratch[ 3 * MAX_FRAME_LENGTH / 2 ];\r
85     SKP_int   decimated_framelength, dec_subframe_length, dec_subframe_offset, SNR_Q7, i, b, s;\r
86     SKP_int32 sumSquared, smooth_coef_Q16;\r
87     SKP_int16 HPstateTmp;\r
88 \r
89     SKP_int16 X[ VAD_N_BANDS ][ MAX_FRAME_LENGTH / 2 ];\r
90     SKP_int32 Xnrg[ VAD_N_BANDS ];\r
91     SKP_int32 NrgToNoiseRatio_Q8[ VAD_N_BANDS ];\r
92     SKP_int32 speech_nrg, x_tmp;\r
93     SKP_int   ret = 0;\r
94     \r
95 #if SKP_SILK_VAD_HANDLE_10MS_FRAMES\r
96     SKP_int   normalizer;\r
97 #endif\r
98 \r
99     /* Safety checks */\r
100     SKP_assert( VAD_N_BANDS == 4 );\r
101     SKP_assert( MAX_FRAME_LENGTH >= framelength );\r
102     SKP_assert( framelength <= 512 );\r
103     SKP_assert( framelength == 8 * SKP_RSHIFT( framelength, 3 ) );\r
104 \r
105     /***********************/\r
106     /* Filter and Decimate */\r
107     /***********************/\r
108     /* 0-8 kHz to 0-4 kHz and 4-8 kHz */\r
109     SKP_Silk_ana_filt_bank_1( pIn,          &psSilk_VAD->AnaState[  0 ], &X[ 0 ][ 0 ], &X[ 3 ][ 0 ], &scratch[ 0 ], framelength );        \r
110     \r
111     /* 0-4 kHz to 0-2 kHz and 2-4 kHz */\r
112     SKP_Silk_ana_filt_bank_1( &X[ 0 ][ 0 ], &psSilk_VAD->AnaState1[ 0 ], &X[ 0 ][ 0 ], &X[ 2 ][ 0 ], &scratch[ 0 ], SKP_RSHIFT( framelength, 1 ) );\r
113     \r
114     /* 0-2 kHz to 0-1 kHz and 1-2 kHz */\r
115     SKP_Silk_ana_filt_bank_1( &X[ 0 ][ 0 ], &psSilk_VAD->AnaState2[ 0 ], &X[ 0 ][ 0 ], &X[ 1 ][ 0 ], &scratch[ 0 ], SKP_RSHIFT( framelength, 2 ) );\r
116 \r
117     /*********************************************/\r
118     /* HP filter on lowest band (differentiator) */\r
119     /*********************************************/\r
120     decimated_framelength = SKP_RSHIFT( framelength, 3 );\r
121     X[ 0 ][ decimated_framelength - 1 ] = SKP_RSHIFT( X[ 0 ][ decimated_framelength - 1 ], 1 );\r
122     HPstateTmp = X[ 0 ][ decimated_framelength - 1 ];\r
123     for( i = decimated_framelength - 1; i > 0; i-- ) {\r
124         X[ 0 ][ i - 1 ]  = SKP_RSHIFT( X[ 0 ][ i - 1 ], 1 );\r
125         X[ 0 ][ i ]     -= X[ 0 ][ i - 1 ];\r
126     }\r
127     X[ 0 ][ 0 ] -= psSilk_VAD->HPstate;\r
128     psSilk_VAD->HPstate = HPstateTmp;\r
129 \r
130     /*************************************/\r
131     /* Calculate the energy in each band */\r
132     /*************************************/\r
133     for( b = 0; b < VAD_N_BANDS; b++ ) {        \r
134         /* Find the decimated framelength in the non-uniformly divided bands */\r
135         decimated_framelength = SKP_RSHIFT( framelength, SKP_min_int( VAD_N_BANDS - b, VAD_N_BANDS - 1 ) );\r
136 \r
137         /* Split length into subframe lengths */\r
138         dec_subframe_length = SKP_RSHIFT( decimated_framelength, VAD_INTERNAL_SUBFRAMES_LOG2 );\r
139         dec_subframe_offset = 0;\r
140 \r
141         /* Compute energy per sub-frame */\r
142         /* initialize with summed energy of last subframe */\r
143         Xnrg[ b ] = psSilk_VAD->XnrgSubfr[ b ];\r
144         for( s = 0; s < VAD_INTERNAL_SUBFRAMES; s++ ) {\r
145             sumSquared = 0;\r
146             for( i = 0; i < dec_subframe_length; i++ ) {\r
147                 /* The energy will be less than dec_subframe_length * ( SKP_int16_MIN / 8 ) ^ 2.            */\r
148                 /* Therefore we can accumulate with no risk of overflow (unless dec_subframe_length > 128)  */\r
149                 x_tmp = SKP_RSHIFT( X[ b ][ i + dec_subframe_offset ], 3 );\r
150                 sumSquared = SKP_SMLABB( sumSquared, x_tmp, x_tmp );\r
151 \r
152                 /* Safety check */\r
153                 SKP_assert( sumSquared >= 0 );\r
154             }\r
155 \r
156             /* Add/saturate summed energy of current subframe */\r
157             if( s < VAD_INTERNAL_SUBFRAMES - 1 ) {\r
158                 Xnrg[ b ] = SKP_ADD_POS_SAT32( Xnrg[ b ], sumSquared );\r
159             } else {\r
160                 /* Look-ahead subframe */\r
161                 Xnrg[ b ] = SKP_ADD_POS_SAT32( Xnrg[ b ], SKP_RSHIFT( sumSquared, 1 ) );\r
162             }\r
163 \r
164             dec_subframe_offset += dec_subframe_length;\r
165         }\r
166         psSilk_VAD->XnrgSubfr[ b ] = sumSquared; \r
167     }\r
168 \r
169     /********************/\r
170     /* Noise estimation */\r
171     /********************/\r
172     SKP_Silk_VAD_GetNoiseLevels( &Xnrg[ 0 ], psSilk_VAD );\r
173 \r
174     /***********************************************/\r
175     /* Signal-plus-noise to noise ratio estimation */\r
176     /***********************************************/\r
177     sumSquared = 0;\r
178     input_tilt = 0;\r
179     for( b = 0; b < VAD_N_BANDS; b++ ) {\r
180         speech_nrg = Xnrg[ b ] - psSilk_VAD->NL[ b ];\r
181         if( speech_nrg > 0 ) {\r
182             /* Divide, with sufficient resolution */\r
183             if( ( Xnrg[ b ] & 0xFF800000 ) == 0 ) {\r
184                 NrgToNoiseRatio_Q8[ b ] = SKP_DIV32( SKP_LSHIFT( Xnrg[ b ], 8 ), psSilk_VAD->NL[ b ] + 1 );\r
185             } else {\r
186                 NrgToNoiseRatio_Q8[ b ] = SKP_DIV32( Xnrg[ b ], SKP_RSHIFT( psSilk_VAD->NL[ b ], 8 ) + 1 );\r
187             }\r
188 \r
189             /* Convert to log domain */\r
190             SNR_Q7 = SKP_Silk_lin2log( NrgToNoiseRatio_Q8[ b ] ) - 8 * 128;\r
191 \r
192             /* Sum-of-squares */\r
193             sumSquared = SKP_SMLABB( sumSquared, SNR_Q7, SNR_Q7 );          /* Q14 */\r
194 \r
195             /* Tilt measure */\r
196             if( speech_nrg < ( 1 << 20 ) ) {\r
197                 /* Scale down SNR value for small subband speech energies */\r
198                 SNR_Q7 = SKP_SMULWB( SKP_LSHIFT( SKP_Silk_SQRT_APPROX( speech_nrg ), 6 ), SNR_Q7 );\r
199             }\r
200             input_tilt = SKP_SMLAWB( input_tilt, tiltWeights[ b ], SNR_Q7 );\r
201         } else {\r
202             NrgToNoiseRatio_Q8[ b ] = 256;\r
203         }\r
204     }\r
205 \r
206     /* Mean-of-squares */\r
207     sumSquared = SKP_DIV32_16( sumSquared, VAD_N_BANDS ); /* Q14 */\r
208 \r
209     /* Root-mean-square approximation, scale to dBs, and write to output pointer */\r
210     *pSNR_dB_Q7 = ( SKP_int16 )( 3 * SKP_Silk_SQRT_APPROX( sumSquared ) ); /* Q7 */\r
211 \r
212     /*********************************/\r
213     /* Speech Probability Estimation */\r
214     /*********************************/\r
215     SA_Q15 = SKP_Silk_sigm_Q15( SKP_SMULWB( VAD_SNR_FACTOR_Q16, *pSNR_dB_Q7 ) - VAD_NEGATIVE_OFFSET_Q5 );\r
216 \r
217     /**************************/\r
218     /* Frequency Tilt Measure */\r
219     /**************************/\r
220     *pTilt_Q15 = SKP_LSHIFT( SKP_Silk_sigm_Q15( input_tilt ) - 16384, 1 );\r
221 \r
222     /**************************************************/\r
223     /* Scale the sigmoid output based on power levels */\r
224     /**************************************************/\r
225     speech_nrg = 0;\r
226     for( b = 0; b < VAD_N_BANDS; b++ ) {\r
227         /* Accumulate signal-without-noise energies, higher frequency bands have more weight */\r
228         speech_nrg += ( b + 1 ) * SKP_RSHIFT( Xnrg[ b ] - psSilk_VAD->NL[ b ], 4 );\r
229     }\r
230 \r
231     /* Power scaling */\r
232     if( speech_nrg <= 0 ) {\r
233         SA_Q15 = SKP_RSHIFT( SA_Q15, 1 ); \r
234     } else if( speech_nrg < 32768 ) {\r
235         \r
236 #if SKP_SILK_VAD_HANDLE_10MS_FRAMES\r
237         /* Energy normalization of frames shorter than 320 samples */\r
238         normalizer = 0;\r
239         while( SKP_LSHIFT( framelength, normalizer ) < 320 ) {\r
240             normalizer++;\r
241         }\r
242         speech_nrg = SKP_LSHIFT_SAT32( speech_nrg, 15 + normalizer );\r
243 #else\r
244         speech_nrg = SKP_LSHIFT_SAT32( speech_nrg, 15 );\r
245 #endif\r
246         /* square-root */\r
247         speech_nrg = SKP_Silk_SQRT_APPROX( speech_nrg );\r
248         SA_Q15 = SKP_SMULWB( 32768 + speech_nrg, SA_Q15 ); \r
249     }\r
250 \r
251     /* Copy the resulting speech activity in Q8 to *pSA_Q8 */\r
252     *pSA_Q8 = SKP_min_int( SKP_RSHIFT( SA_Q15, 7 ), SKP_uint8_MAX );\r
253 \r
254     /***********************************/\r
255     /* Energy Level and SNR estimation */\r
256     /***********************************/\r
257     /* Smoothing coefficient */\r
258     smooth_coef_Q16 = SKP_SMULWB( VAD_SNR_SMOOTH_COEF_Q18, SKP_SMULWB( SA_Q15, SA_Q15 ) );\r
259     \r
260 #if SKP_SILK_VAD_HANDLE_10MS_FRAMES\r
261     if( framelength == 10 * fs_kHz ) {\r
262         smooth_coef_Q16 >>= 1;\r
263     } else {\r
264        SKP_assert( framelength == 20 * fs_kHz );\r
265     }\r
266 #endif\r
267 \r
268     for( b = 0; b < VAD_N_BANDS; b++ ) {\r
269         /* compute smoothed energy-to-noise ratio per band */\r
270         psSilk_VAD->NrgRatioSmth_Q8[ b ] = SKP_SMLAWB( psSilk_VAD->NrgRatioSmth_Q8[ b ], \r
271             NrgToNoiseRatio_Q8[ b ] - psSilk_VAD->NrgRatioSmth_Q8[ b ], smooth_coef_Q16 );\r
272 \r
273         /* signal to noise ratio in dB per band */\r
274         SNR_Q7 = 3 * ( SKP_Silk_lin2log( psSilk_VAD->NrgRatioSmth_Q8[b] ) - 8 * 128 );\r
275         /* quality = sigmoid( 0.25 * ( SNR_dB - 16 ) ); */\r
276         pQuality_Q15[ b ] = SKP_Silk_sigm_Q15( SKP_RSHIFT( SNR_Q7 - 16 * 128, 4 ) );\r
277     }\r
278 \r
279     return( ret );\r
280 }\r
281 \r
282 /**************************/\r
283 /* Noise level estimation */\r
284 /**************************/\r
285 void SKP_Silk_VAD_GetNoiseLevels(\r
286     const SKP_int32                 pX[ VAD_N_BANDS ],  /* I    subband energies                            */\r
287     SKP_Silk_VAD_state              *psSilk_VAD         /* I/O  Pointer to Silk VAD state                   */ \r
288 )\r
289 {\r
290     SKP_int   k;\r
291     SKP_int32 nl, nrg, inv_nrg;\r
292     SKP_int   coef, min_coef;\r
293 \r
294     /* Initially faster smoothing */\r
295     if( psSilk_VAD->counter < 1000 ) { /* 1000 = 20 sec */\r
296         min_coef = SKP_DIV32_16( SKP_int16_MAX, SKP_RSHIFT( psSilk_VAD->counter, 4 ) + 1 );  \r
297     } else {\r
298         min_coef = 0;\r
299     }\r
300 \r
301     for( k = 0; k < VAD_N_BANDS; k++ ) {\r
302         /* Get old noise level estimate for current band */\r
303         nl = psSilk_VAD->NL[ k ];\r
304         SKP_assert( nl >= 0 );\r
305         \r
306         /* Add bias */\r
307         nrg = SKP_ADD_POS_SAT32( pX[ k ], psSilk_VAD->NoiseLevelBias[ k ] ); \r
308         SKP_assert( nrg > 0 );\r
309         \r
310         /* Invert energies */\r
311         inv_nrg = SKP_DIV32( SKP_int32_MAX, nrg );\r
312         SKP_assert( inv_nrg >= 0 );\r
313         \r
314         /* Less update when subband energy is high */\r
315         if( nrg > SKP_LSHIFT( nl, 3 ) ) {\r
316             coef = VAD_NOISE_LEVEL_SMOOTH_COEF_Q16 >> 3;\r
317         } else if( nrg < nl ) {\r
318             coef = VAD_NOISE_LEVEL_SMOOTH_COEF_Q16;\r
319         } else {\r
320             coef = SKP_SMULWB( SKP_SMULWW( inv_nrg, nl ), VAD_NOISE_LEVEL_SMOOTH_COEF_Q16 << 1 );\r
321         }\r
322 \r
323         /* Initially faster smoothing */\r
324         coef = SKP_max_int( coef, min_coef );\r
325 \r
326         /* Smooth inverse energies */\r
327         psSilk_VAD->inv_NL[ k ] = SKP_SMLAWB( psSilk_VAD->inv_NL[ k ], inv_nrg - psSilk_VAD->inv_NL[ k ], coef );\r
328         SKP_assert( psSilk_VAD->inv_NL[ k ] >= 0 );\r
329 \r
330         /* Compute noise level by inverting again */\r
331         nl = SKP_DIV32( SKP_int32_MAX, psSilk_VAD->inv_NL[ k ] );\r
332         SKP_assert( nl >= 0 );\r
333 \r
334         /* Limit noise levels (guarantee 7 bits of head room) */\r
335         nl = SKP_min( nl, 0x00FFFFFF );\r
336 \r
337         /* Store as part of state */\r
338         psSilk_VAD->NL[ k ] = nl;\r
339     }\r
340 \r
341     /* Increment frame counter */\r
342     psSilk_VAD->counter++;\r
343 }\r