Initial Skype commit taken from FreeSwitch, which got it from the IETF draft.
[opus.git] / src / SKP_Silk_decode_core.c
1 /***********************************************************************\r
2 Copyright (c) 2006-2010, 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 "SKP_Silk_main.h"\r
29 \r
30 /**********************************************************/\r
31 /* Core decoder. Performs inverse NSQ operation LTP + LPC */\r
32 /**********************************************************/\r
33 void SKP_Silk_decode_core(\r
34     SKP_Silk_decoder_state      *psDec,                             /* I/O  Decoder state               */\r
35     SKP_Silk_decoder_control    *psDecCtrl,                         /* I    Decoder control             */\r
36     SKP_int16                   xq[],                               /* O    Decoded speech              */\r
37     const SKP_int               q[ MAX_FRAME_LENGTH ]               /* I    Pulse signal                */\r
38 )\r
39 {\r
40     SKP_int     i, k, lag = 0, start_idx, NLSF_interpolation_flag, sigtype, LTP_scale_Q14;\r
41     SKP_int16   *A_Q12, *B_Q14, *pxq, A_Q12_tmp[ MAX_LPC_ORDER ];\r
42     SKP_int16   sLTP[ MAX_FRAME_LENGTH ];\r
43     SKP_int32   Gain_Q16, *pred_lag_ptr, *pexc_Q10, *pres_Q10, LTP_pred_Q14, LPC_pred_Q10;\r
44     SKP_int32   rand_seed, offset_Q10, dither;\r
45     SKP_int32   vec_Q10[ MAX_FRAME_LENGTH / NB_SUBFR ], Atmp;\r
46     SKP_int32   inv_gain_Q16, inv_gain_Q32, gain_adj_Q16, FiltState[ MAX_LPC_ORDER ];\r
47     SKP_assert( psDec->prev_inv_gain_Q16 != 0 );\r
48     \r
49     offset_Q10 = SKP_Silk_Quantization_Offsets_Q10[ psDecCtrl->sigtype ][ psDecCtrl->QuantOffsetType ];\r
50 \r
51     if( psDecCtrl->NLSFInterpCoef_Q2 < ( 1 << 2 ) ) {\r
52         NLSF_interpolation_flag = 1;\r
53     } else {\r
54         NLSF_interpolation_flag = 0;\r
55     }\r
56 \r
57 \r
58     /* Decode excitation */\r
59     rand_seed = psDecCtrl->Seed;\r
60     for( i = 0; i < psDec->frame_length; i++ ) {\r
61         rand_seed = SKP_RAND( rand_seed );\r
62         /* dither = rand_seed < 0 ? 0xFFFFFFFF : 0; */\r
63         dither = SKP_RSHIFT( rand_seed, 31 );\r
64 \r
65         psDec->exc_Q10[ i ] = SKP_LSHIFT( ( SKP_int32 )q[ i ], 10 ) + offset_Q10;\r
66         psDec->exc_Q10[ i ] = ( psDec->exc_Q10[ i ] ^ dither ) - dither;\r
67 \r
68         rand_seed += q[ i ];\r
69     }\r
70 \r
71 \r
72     pexc_Q10 = psDec->exc_Q10;\r
73     pres_Q10 = psDec->res_Q10;\r
74     pxq      = &psDec->outBuf[ psDec->frame_length ];\r
75     psDec->sLTP_buf_idx = psDec->frame_length;\r
76     /* Loop over subframes */\r
77     for( k = 0; k < NB_SUBFR; k++ ) {\r
78         A_Q12 = psDecCtrl->PredCoef_Q12[ k >> 1 ];\r
79 \r
80         /* Preload LPC coeficients to array on stack. Gives small performance gain */        \r
81         SKP_memcpy( A_Q12_tmp, A_Q12, psDec->LPC_order * sizeof( SKP_int16 ) ); \r
82         B_Q14         = &psDecCtrl->LTPCoef_Q14[ k * LTP_ORDER ];\r
83         Gain_Q16      = psDecCtrl->Gains_Q16[ k ];\r
84         LTP_scale_Q14 = psDecCtrl->LTP_scale_Q14;\r
85         sigtype       = psDecCtrl->sigtype;\r
86 \r
87         inv_gain_Q16  = SKP_DIV32( SKP_int32_MAX, SKP_RSHIFT( Gain_Q16, 1 ) );\r
88         inv_gain_Q16  = SKP_min( inv_gain_Q16, SKP_int16_MAX );\r
89 \r
90         /* Calculate Gain adjustment factor */\r
91         gain_adj_Q16 = ( SKP_int32 )1 << 16;\r
92         if( inv_gain_Q16 != psDec->prev_inv_gain_Q16 ) {\r
93             gain_adj_Q16 =  SKP_DIV32_varQ( inv_gain_Q16, psDec->prev_inv_gain_Q16, 16 );\r
94         }\r
95 \r
96         /* Avoid abrupt transition from voiced PLC to unvoiced normal decoding */\r
97         if( psDec->lossCnt && psDec->prev_sigtype == SIG_TYPE_VOICED &&\r
98             psDecCtrl->sigtype == SIG_TYPE_UNVOICED && k < ( NB_SUBFR >> 1 ) ) {\r
99             \r
100             SKP_memset( B_Q14, 0, LTP_ORDER * sizeof( SKP_int16 ) );\r
101             B_Q14[ LTP_ORDER/2 ] = ( SKP_int16 )1 << 12; /* 0.25 */\r
102         \r
103             sigtype = SIG_TYPE_VOICED;\r
104             psDecCtrl->pitchL[ k ] = psDec->lagPrev;\r
105             LTP_scale_Q14 = ( SKP_int )1 << 14;\r
106         }\r
107         if( sigtype == SIG_TYPE_VOICED ) {\r
108             /* Voiced */\r
109             \r
110             lag = psDecCtrl->pitchL[ k ];\r
111             /* Re-whitening */\r
112             if( ( k & ( 3 - SKP_LSHIFT( NLSF_interpolation_flag, 1 ) ) ) == 0 ) {\r
113                 /* Rewhiten with new A coefs */\r
114                 start_idx = psDec->frame_length - lag - psDec->LPC_order - LTP_ORDER / 2;\r
115                 start_idx = SKP_LIMIT( start_idx, 0, psDec->frame_length - psDec->LPC_order );\r
116 \r
117                 SKP_Silk_MA_Prediction( &psDec->outBuf[ start_idx + k * ( psDec->frame_length >> 2 ) ], \r
118                     A_Q12, FiltState, sLTP + start_idx, psDec->frame_length - start_idx, psDec->LPC_order );\r
119 \r
120                 /* After rewhitening the LTP state is unscaled */\r
121                 inv_gain_Q32 = SKP_LSHIFT( inv_gain_Q16, 16 );\r
122                 if( k == 0 ) {\r
123                     /* Do LTP downscaling */\r
124                     inv_gain_Q32 = SKP_LSHIFT( SKP_SMULWB( inv_gain_Q32, psDecCtrl->LTP_scale_Q14 ), 2 );\r
125                 }\r
126                 for( i = 0; i < (lag + LTP_ORDER/2); i++ ) {\r
127                     psDec->sLTP_Q16[ psDec->sLTP_buf_idx - i - 1 ] = SKP_SMULWB( inv_gain_Q32, sLTP[ psDec->frame_length - i - 1 ] );\r
128                 }\r
129             } else {\r
130                 /* Update LTP state when Gain changes */\r
131                 if( gain_adj_Q16 != ( SKP_int32 )1 << 16 ) {\r
132                     for( i = 0; i < ( lag + LTP_ORDER / 2 ); i++ ) {\r
133                         psDec->sLTP_Q16[ psDec->sLTP_buf_idx - i - 1 ] = SKP_SMULWW( gain_adj_Q16, psDec->sLTP_Q16[ psDec->sLTP_buf_idx - i - 1 ] );\r
134                     }\r
135                 }\r
136             }\r
137         }\r
138         \r
139         /* Scale short term state */\r
140         for( i = 0; i < MAX_LPC_ORDER; i++ ) {\r
141             psDec->sLPC_Q14[ i ] = SKP_SMULWW( gain_adj_Q16, psDec->sLPC_Q14[ i ] );\r
142         }\r
143 \r
144         /* Save inv_gain */\r
145         SKP_assert( inv_gain_Q16 != 0 );\r
146         psDec->prev_inv_gain_Q16 = inv_gain_Q16;\r
147 \r
148 \r
149         /* Long-term prediction */\r
150         if( sigtype == SIG_TYPE_VOICED ) {\r
151             /* Setup pointer */\r
152             pred_lag_ptr = &psDec->sLTP_Q16[ psDec->sLTP_buf_idx - lag + LTP_ORDER / 2 ];\r
153             for( i = 0; i < psDec->subfr_length; i++ ) {\r
154                 /* Unrolled loop */\r
155                 LTP_pred_Q14 = SKP_SMULWB(               pred_lag_ptr[  0 ], B_Q14[ 0 ] );\r
156                 LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -1 ], B_Q14[ 1 ] );\r
157                 LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -2 ], B_Q14[ 2 ] );\r
158                 LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -3 ], B_Q14[ 3 ] );\r
159                 LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -4 ], B_Q14[ 4 ] );\r
160                 pred_lag_ptr++;\r
161             \r
162                 /* Generate LPC residual */ \r
163                 pres_Q10[ i ] = SKP_ADD32( pexc_Q10[ i ], SKP_RSHIFT_ROUND( LTP_pred_Q14, 4 ) );\r
164             \r
165                 /* Update states */\r
166                 psDec->sLTP_Q16[ psDec->sLTP_buf_idx ] = SKP_LSHIFT( pres_Q10[ i ], 6 );\r
167                 psDec->sLTP_buf_idx++;\r
168             }\r
169         } else {\r
170             SKP_memcpy( pres_Q10, pexc_Q10, psDec->subfr_length * sizeof( SKP_int32 ) );\r
171         }\r
172 \r
173 \r
174         /* Short term prediction */\r
175         /* NOTE: the code below loads two int16 values in an int32, and multiplies each using the   */\r
176         /* SMLAWB and SMLAWT instructions. On a big-endian CPU the two int16 variables would be     */\r
177         /* loaded in reverse order and the code will give the wrong result. In that case swapping   */\r
178         /* the SMLAWB and SMLAWT instructions should solve the problem.                             */\r
179         if( psDec->LPC_order == 16 ) {\r
180             for( i = 0; i < psDec->subfr_length; i++ ) {\r
181                 /* unrolled */\r
182                 Atmp = *( ( SKP_int32* )&A_Q12_tmp[ 0 ] );    /* read two coefficients at once */\r
183                 LPC_pred_Q10 = SKP_SMULWB(               psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  1 ], Atmp );\r
184                 LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  2 ], Atmp );\r
185                 Atmp = *( ( SKP_int32* )&A_Q12_tmp[ 2 ] );\r
186                 LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  3 ], Atmp );\r
187                 LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  4 ], Atmp );\r
188                 Atmp = *( ( SKP_int32* )&A_Q12_tmp[ 4 ] );\r
189                 LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  5 ], Atmp );\r
190                 LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  6 ], Atmp );\r
191                 Atmp = *( ( SKP_int32* )&A_Q12_tmp[ 6 ] );\r
192                 LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  7 ], Atmp );\r
193                 LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  8 ], Atmp );\r
194                 Atmp = *( ( SKP_int32* )&A_Q12_tmp[ 8 ] );\r
195                 LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  9 ], Atmp );\r
196                 LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i - 10 ], Atmp );\r
197                 Atmp = *( ( SKP_int32* )&A_Q12_tmp[ 10 ] );\r
198                 LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i - 11 ], Atmp );\r
199                 LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i - 12 ], Atmp );\r
200                 Atmp = *( ( SKP_int32* )&A_Q12_tmp[ 12 ] );\r
201                 LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i - 13 ], Atmp );\r
202                 LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i - 14 ], Atmp );\r
203                 Atmp = *( ( SKP_int32* )&A_Q12_tmp[ 14 ] );\r
204                 LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i - 15 ], Atmp );\r
205                 LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i - 16 ], Atmp );\r
206                 \r
207                 /* Add prediction to LPC residual */\r
208                 vec_Q10[ i ] = SKP_ADD32( pres_Q10[ i ], LPC_pred_Q10 );\r
209                 \r
210                 /* Update states */\r
211                 psDec->sLPC_Q14[ MAX_LPC_ORDER + i ] = SKP_LSHIFT( vec_Q10[ i ], 4 );\r
212             }\r
213         } else {\r
214             SKP_assert( psDec->LPC_order == 10 );\r
215             for( i = 0; i < psDec->subfr_length; i++ ) {\r
216                 /* unrolled */\r
217                 Atmp = *( ( SKP_int32* )&A_Q12_tmp[ 0 ] );    /* read two coefficients at once */\r
218                 LPC_pred_Q10 = SKP_SMULWB(               psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  1 ], Atmp );\r
219                 LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  2 ], Atmp );\r
220                 Atmp = *( ( SKP_int32* )&A_Q12_tmp[ 2 ] );\r
221                 LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  3 ], Atmp );\r
222                 LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  4 ], Atmp );\r
223                 Atmp = *( ( SKP_int32* )&A_Q12_tmp[ 4 ] );\r
224                 LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  5 ], Atmp );\r
225                 LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  6 ], Atmp );\r
226                 Atmp = *( ( SKP_int32* )&A_Q12_tmp[ 6 ] );\r
227                 LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  7 ], Atmp );\r
228                 LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  8 ], Atmp );\r
229                 Atmp = *( ( SKP_int32* )&A_Q12_tmp[ 8 ] );\r
230                 LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i -  9 ], Atmp );\r
231                 LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psDec->sLPC_Q14[ MAX_LPC_ORDER + i - 10 ], Atmp );\r
232                 \r
233                 /* Add prediction to LPC residual */\r
234                 vec_Q10[ i ] = SKP_ADD32( pres_Q10[ i ], LPC_pred_Q10 );\r
235                 \r
236                 /* Update states */\r
237                 psDec->sLPC_Q14[ MAX_LPC_ORDER + i ] = SKP_LSHIFT( vec_Q10[ i ], 4 );\r
238             }\r
239         }\r
240 \r
241         /* Scale with Gain */\r
242         for( i = 0; i < psDec->subfr_length; i++ ) {\r
243             pxq[ i ] = ( SKP_int16 )SKP_SAT16( SKP_RSHIFT_ROUND( SKP_SMULWW( vec_Q10[ i ], Gain_Q16 ), 10 ) );\r
244         }\r
245 \r
246         /* Update LPC filter state */\r
247         SKP_memcpy( psDec->sLPC_Q14, &psDec->sLPC_Q14[ psDec->subfr_length ], MAX_LPC_ORDER * sizeof( SKP_int32 ) );\r
248         pexc_Q10 += psDec->subfr_length;\r
249         pres_Q10 += psDec->subfr_length;\r
250         pxq      += psDec->subfr_length;\r
251     }\r
252     \r
253     /* Copy to output */\r
254     SKP_memcpy( xq, &psDec->outBuf[ psDec->frame_length ], psDec->frame_length * sizeof( SKP_int16 ) );\r
255 \r
256 }\r