Gitiles
Code Review Sign In
gerrit-public.fairphone.software / platform / external / libopus / 232858ab359c214c2b0006f298531ce8565acf08 / . / silk / x86 / NSQ_sse.c
blob: 72f34fd6fc4f5f2ffc2cc2ed0407c9b60e20a88e [file] [log] [blame]
flimc91ee5b2016-01-26 14:33:44 +0100[diff] [blame]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#include "main.h"
36#include "celt/x86/x86cpu.h"
37#include "stack_alloc.h"
38
39static OPUS_INLINE void silk_nsq_scale_states_sse4_1(
40 const silk_encoder_state *psEncC, /* I Encoder State */
41 silk_nsq_state *NSQ, /* I/O NSQ state */
42 const opus_int32 x_Q3[], /* I input in Q3 */
43 opus_int32 x_sc_Q10[], /* O input scaled with 1/Gain */
44 const opus_int16 sLTP[], /* I re-whitened LTP state in Q0 */
45 opus_int32 sLTP_Q15[], /* O LTP state matching scaled input */
46 opus_int subfr, /* I subframe number */
47 const opus_int LTP_scale_Q14, /* I */
48 const opus_int32 Gains_Q16[ MAX_NB_SUBFR ], /* I */
49 const opus_int pitchL[ MAX_NB_SUBFR ], /* I Pitch lag */
50 const opus_int signal_type /* I Signal type */
51);
52
53static OPUS_INLINE void silk_noise_shape_quantizer_10_16_sse4_1(
54 silk_nsq_state *NSQ, /* I/O NSQ state */
55 opus_int signalType, /* I Signal type */
56 const opus_int32 x_sc_Q10[], /* I */
57 opus_int8 pulses[], /* O */
58 opus_int16 xq[], /* O */
59 opus_int32 sLTP_Q15[], /* I/O LTP state */
60 const opus_int16 a_Q12[], /* I Short term prediction coefs */
61 const opus_int16 b_Q14[], /* I Long term prediction coefs */
62 const opus_int16 AR_shp_Q13[], /* I Noise shaping AR coefs */
63 opus_int lag, /* I Pitch lag */
64 opus_int32 HarmShapeFIRPacked_Q14, /* I */
65 opus_int Tilt_Q14, /* I Spectral tilt */
66 opus_int32 LF_shp_Q14, /* I */
67 opus_int32 Gain_Q16, /* I */
68 opus_int offset_Q10, /* I */
69 opus_int length, /* I Input length */
70 opus_int32 table[][4] /* I */
71);
72
73void silk_NSQ_sse4_1(
74 const silk_encoder_state *psEncC, /* I/O Encoder State */
75 silk_nsq_state *NSQ, /* I/O NSQ state */
76 SideInfoIndices *psIndices, /* I/O Quantization Indices */
77 const opus_int32 x_Q3[], /* I Prefiltered input signal */
78 opus_int8 pulses[], /* O Quantized pulse signal */
79 const opus_int16 PredCoef_Q12[ 2 * MAX_LPC_ORDER ], /* I Short term prediction coefs */
80 const opus_int16 LTPCoef_Q14[ LTP_ORDER * MAX_NB_SUBFR ], /* I Long term prediction coefs */
81 const opus_int16 AR2_Q13[ MAX_NB_SUBFR * MAX_SHAPE_LPC_ORDER ], /* I Noise shaping coefs */
82 const opus_int HarmShapeGain_Q14[ MAX_NB_SUBFR ], /* I Long term shaping coefs */
83 const opus_int Tilt_Q14[ MAX_NB_SUBFR ], /* I Spectral tilt */
84 const opus_int32 LF_shp_Q14[ MAX_NB_SUBFR ], /* I Low frequency shaping coefs */
85 const opus_int32 Gains_Q16[ MAX_NB_SUBFR ], /* I Quantization step sizes */
86 const opus_int pitchL[ MAX_NB_SUBFR ], /* I Pitch lags */
87 const opus_int Lambda_Q10, /* I Rate/distortion tradeoff */
88 const opus_int LTP_scale_Q14 /* I LTP state scaling */
89)
90{
91 opus_int k, lag, start_idx, LSF_interpolation_flag;
92 const opus_int16 *A_Q12, *B_Q14, *AR_shp_Q13;
93 opus_int16 *pxq;
94 VARDECL( opus_int32, sLTP_Q15 );
95 VARDECL( opus_int16, sLTP );
96 opus_int32 HarmShapeFIRPacked_Q14;
97 opus_int offset_Q10;
98 VARDECL( opus_int32, x_sc_Q10 );
99
100 opus_int32 table[ 64 ][ 4 ];
101 opus_int32 tmp1;
102 opus_int32 q1_Q10, q2_Q10, rd1_Q20, rd2_Q20;
103
104 SAVE_STACK;
105
106 NSQ->rand_seed = psIndices->Seed;
107
108 /* Set unvoiced lag to the previous one, overwrite later for voiced */
109 lag = NSQ->lagPrev;
110
111 silk_assert( NSQ->prev_gain_Q16 != 0 );
112
113 offset_Q10 = silk_Quantization_Offsets_Q10[ psIndices->signalType >> 1 ][ psIndices->quantOffsetType ];
114
115 /* 0 */
116 q1_Q10 = offset_Q10;
117 q2_Q10 = offset_Q10 + ( 1024 - QUANT_LEVEL_ADJUST_Q10 );
118 rd1_Q20 = q1_Q10 * Lambda_Q10;
119 rd2_Q20 = q2_Q10 * Lambda_Q10;
120
121 table[ 32 ][ 0 ] = q1_Q10;
122 table[ 32 ][ 1 ] = q2_Q10;
123 table[ 32 ][ 2 ] = 2 * (q1_Q10 - q2_Q10);
124 table[ 32 ][ 3 ] = (rd1_Q20 - rd2_Q20) + (q1_Q10 * q1_Q10 - q2_Q10 * q2_Q10);
125
126 /* -1 */
127 q1_Q10 = offset_Q10 - ( 1024 - QUANT_LEVEL_ADJUST_Q10 );
128 q2_Q10 = offset_Q10;
129 rd1_Q20 = - q1_Q10 * Lambda_Q10;
130 rd2_Q20 = q2_Q10 * Lambda_Q10;
131
132 table[ 31 ][ 0 ] = q1_Q10;
133 table[ 31 ][ 1 ] = q2_Q10;
134 table[ 31 ][ 2 ] = 2 * (q1_Q10 - q2_Q10);
135 table[ 31 ][ 3 ] = (rd1_Q20 - rd2_Q20) + (q1_Q10 * q1_Q10 - q2_Q10 * q2_Q10);
136
137 /* > 0 */
138 for (k = 1; k <= 31; k++)
139 {
140 tmp1 = offset_Q10 + silk_LSHIFT( k, 10 );
141
142 q1_Q10 = tmp1 - QUANT_LEVEL_ADJUST_Q10;
143 q2_Q10 = tmp1 - QUANT_LEVEL_ADJUST_Q10 + 1024;
144 rd1_Q20 = q1_Q10 * Lambda_Q10;
145 rd2_Q20 = q2_Q10 * Lambda_Q10;
146
147 table[ 32 + k ][ 0 ] = q1_Q10;
148 table[ 32 + k ][ 1 ] = q2_Q10;
149 table[ 32 + k ][ 2 ] = 2 * (q1_Q10 - q2_Q10);
150 table[ 32 + k ][ 3 ] = (rd1_Q20 - rd2_Q20) + (q1_Q10 * q1_Q10 - q2_Q10 * q2_Q10);
151 }
152
153 /* < -1 */
154 for (k = -32; k <= -2; k++)
155 {
156 tmp1 = offset_Q10 + silk_LSHIFT( k, 10 );
157
158 q1_Q10 = tmp1 + QUANT_LEVEL_ADJUST_Q10;
159 q2_Q10 = tmp1 + QUANT_LEVEL_ADJUST_Q10 + 1024;
160 rd1_Q20 = - q1_Q10 * Lambda_Q10;
161 rd2_Q20 = - q2_Q10 * Lambda_Q10;
162
163 table[ 32 + k ][ 0 ] = q1_Q10;
164 table[ 32 + k ][ 1 ] = q2_Q10;
165 table[ 32 + k ][ 2 ] = 2 * (q1_Q10 - q2_Q10);
166 table[ 32 + k ][ 3 ] = (rd1_Q20 - rd2_Q20) + (q1_Q10 * q1_Q10 - q2_Q10 * q2_Q10);
167 }
168
169 if( psIndices->NLSFInterpCoef_Q2 == 4 ) {
170 LSF_interpolation_flag = 0;
171 } else {
172 LSF_interpolation_flag = 1;
173 }
174
175 ALLOC( sLTP_Q15,
176 psEncC->ltp_mem_length + psEncC->frame_length, opus_int32 );
177 ALLOC( sLTP, psEncC->ltp_mem_length + psEncC->frame_length, opus_int16 );
178 ALLOC( x_sc_Q10, psEncC->subfr_length, opus_int32 );
179 /* Set up pointers to start of sub frame */
180 NSQ->sLTP_shp_buf_idx = psEncC->ltp_mem_length;
181 NSQ->sLTP_buf_idx = psEncC->ltp_mem_length;
182 pxq = &NSQ->xq[ psEncC->ltp_mem_length ];
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 /* Rewhiten with new A coefs */
201 start_idx = psEncC->ltp_mem_length - lag - psEncC->predictLPCOrder - LTP_ORDER / 2;
202 silk_assert( start_idx > 0 );
203
204 silk_LPC_analysis_filter( &sLTP[ start_idx ], &NSQ->xq[ start_idx + k * psEncC->subfr_length ],
205 A_Q12, psEncC->ltp_mem_length - start_idx, psEncC->predictLPCOrder, psEncC->arch );
206
207 NSQ->rewhite_flag = 1;
208 NSQ->sLTP_buf_idx = psEncC->ltp_mem_length;
209 }
210 }
211
212 silk_nsq_scale_states_sse4_1( psEncC, NSQ, x_Q3, x_sc_Q10, sLTP, sLTP_Q15, k, LTP_scale_Q14, Gains_Q16, pitchL, psIndices->signalType );
213
214 if ( opus_likely( ( 10 == psEncC->shapingLPCOrder ) && ( 16 == psEncC->predictLPCOrder) ) )
215 {
216 silk_noise_shape_quantizer_10_16_sse4_1( NSQ, psIndices->signalType, x_sc_Q10, pulses, pxq, sLTP_Q15, A_Q12, B_Q14,
217 AR_shp_Q13, lag, HarmShapeFIRPacked_Q14, Tilt_Q14[ k ], LF_shp_Q14[ k ], Gains_Q16[ k ],
218 offset_Q10, psEncC->subfr_length, &(table[32]) );
219 }
220 else
221 {
222 silk_noise_shape_quantizer( NSQ, psIndices->signalType, x_sc_Q10, pulses, pxq, sLTP_Q15, A_Q12, B_Q14,
223 AR_shp_Q13, lag, HarmShapeFIRPacked_Q14, Tilt_Q14[ k ], LF_shp_Q14[ k ], Gains_Q16[ k ], Lambda_Q10,
224 offset_Q10, psEncC->subfr_length, psEncC->shapingLPCOrder, psEncC->predictLPCOrder );
225 }
226
227 x_Q3 += psEncC->subfr_length;
228 pulses += psEncC->subfr_length;
229 pxq += psEncC->subfr_length;
230 }
231
232 /* Update lagPrev for next frame */
233 NSQ->lagPrev = pitchL[ psEncC->nb_subfr - 1 ];
234
235 /* Save quantized speech and noise shaping signals */
236 /* DEBUG_STORE_DATA( enc.pcm, &NSQ->xq[ psEncC->ltp_mem_length ], psEncC->frame_length * sizeof( opus_int16 ) ) */
237 silk_memmove( NSQ->xq, &NSQ->xq[ psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int16 ) );
238 silk_memmove( NSQ->sLTP_shp_Q14, &NSQ->sLTP_shp_Q14[ psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int32 ) );
239 RESTORE_STACK;
240}
241
242/***********************************/
243/* silk_noise_shape_quantizer_10_16 */
244/***********************************/
245static OPUS_INLINE void silk_noise_shape_quantizer_10_16_sse4_1(
246 silk_nsq_state *NSQ, /* I/O NSQ state */
247 opus_int signalType, /* I Signal type */
248 const opus_int32 x_sc_Q10[], /* I */
249 opus_int8 pulses[], /* O */
250 opus_int16 xq[], /* O */
251 opus_int32 sLTP_Q15[], /* I/O LTP state */
252 const opus_int16 a_Q12[], /* I Short term prediction coefs */
253 const opus_int16 b_Q14[], /* I Long term prediction coefs */
254 const opus_int16 AR_shp_Q13[], /* I Noise shaping AR coefs */
255 opus_int lag, /* I Pitch lag */
256 opus_int32 HarmShapeFIRPacked_Q14, /* I */
257 opus_int Tilt_Q14, /* I Spectral tilt */
258 opus_int32 LF_shp_Q14, /* I */
259 opus_int32 Gain_Q16, /* I */
260 opus_int offset_Q10, /* I */
261 opus_int length, /* I Input length */
262 opus_int32 table[][4] /* I */
263)
264{
265 opus_int i;
266 opus_int32 LTP_pred_Q13, LPC_pred_Q10, n_AR_Q12, n_LTP_Q13;
267 opus_int32 n_LF_Q12, r_Q10, q1_Q0, q1_Q10, q2_Q10;
268 opus_int32 exc_Q14, LPC_exc_Q14, xq_Q14, Gain_Q10;
269 opus_int32 tmp1, tmp2, sLF_AR_shp_Q14;
270 opus_int32 *psLPC_Q14, *shp_lag_ptr, *pred_lag_ptr;
271
272 __m128i xmm_tempa, xmm_tempb;
273
274 __m128i xmm_one;
275
276 __m128i psLPC_Q14_hi_01234567, psLPC_Q14_hi_89ABCDEF;
277 __m128i psLPC_Q14_lo_01234567, psLPC_Q14_lo_89ABCDEF;
278 __m128i a_Q12_01234567, a_Q12_89ABCDEF;
279
280 __m128i sAR2_Q14_hi_76543210, sAR2_Q14_lo_76543210;
281 __m128i AR_shp_Q13_76543210;
282
283 shp_lag_ptr = &NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - lag + HARM_SHAPE_FIR_TAPS / 2 ];
284 pred_lag_ptr = &sLTP_Q15[ NSQ->sLTP_buf_idx - lag + LTP_ORDER / 2 ];
285 Gain_Q10 = silk_RSHIFT( Gain_Q16, 6 );
286
287 /* Set up short term AR state */
288 psLPC_Q14 = &NSQ->sLPC_Q14[ NSQ_LPC_BUF_LENGTH - 1 ];
289
290 sLF_AR_shp_Q14 = NSQ->sLF_AR_shp_Q14;
291 xq_Q14 = psLPC_Q14[ 0 ];
292 LTP_pred_Q13 = 0;
293
294 /* load a_Q12 */
295 xmm_one = _mm_set_epi8( 1, 0, 3, 2, 5, 4, 7, 6, 9, 8, 11, 10, 13, 12, 15, 14 );
296
297 /* load a_Q12[0] - a_Q12[7] */
298 a_Q12_01234567 = _mm_loadu_si128( (__m128i *)(&a_Q12[ 0 ] ) );
299 /* load a_Q12[ 8 ] - a_Q12[ 15 ] */
300 a_Q12_89ABCDEF = _mm_loadu_si128( (__m128i *)(&a_Q12[ 8 ] ) );
301
302 a_Q12_01234567 = _mm_shuffle_epi8( a_Q12_01234567, xmm_one );
303 a_Q12_89ABCDEF = _mm_shuffle_epi8( a_Q12_89ABCDEF, xmm_one );
304
305 /* load AR_shp_Q13 */
306 AR_shp_Q13_76543210 = _mm_loadu_si128( (__m128i *)(&AR_shp_Q13[0] ) );
307
308 /* load psLPC_Q14 */
309 xmm_one = _mm_set_epi8(15, 14, 11, 10, 7, 6, 3, 2, 13, 12, 9, 8, 5, 4, 1, 0 );
310
311 xmm_tempa = _mm_loadu_si128( (__m128i *)(&psLPC_Q14[-16]) );
312 xmm_tempb = _mm_loadu_si128( (__m128i *)(&psLPC_Q14[-12]) );
313
314 xmm_tempa = _mm_shuffle_epi8( xmm_tempa, xmm_one );
315 xmm_tempb = _mm_shuffle_epi8( xmm_tempb, xmm_one );
316
317 psLPC_Q14_hi_89ABCDEF = _mm_unpackhi_epi64( xmm_tempa, xmm_tempb );
318 psLPC_Q14_lo_89ABCDEF = _mm_unpacklo_epi64( xmm_tempa, xmm_tempb );
319
320 xmm_tempa = _mm_loadu_si128( (__m128i *)(&psLPC_Q14[ -8 ]) );
321 xmm_tempb = _mm_loadu_si128( (__m128i *)(&psLPC_Q14[ -4 ]) );
322
323 xmm_tempa = _mm_shuffle_epi8( xmm_tempa, xmm_one );
324 xmm_tempb = _mm_shuffle_epi8( xmm_tempb, xmm_one );
325
326 psLPC_Q14_hi_01234567 = _mm_unpackhi_epi64( xmm_tempa, xmm_tempb );
327 psLPC_Q14_lo_01234567 = _mm_unpacklo_epi64( xmm_tempa, xmm_tempb );
328
329 /* load sAR2_Q14 */
330 xmm_tempa = _mm_loadu_si128( (__m128i *)(&(NSQ->sAR2_Q14[ 0 ]) ) );
331 xmm_tempb = _mm_loadu_si128( (__m128i *)(&(NSQ->sAR2_Q14[ 4 ]) ) );
332
333 xmm_tempa = _mm_shuffle_epi8( xmm_tempa, xmm_one );
334 xmm_tempb = _mm_shuffle_epi8( xmm_tempb, xmm_one );
335
336 sAR2_Q14_hi_76543210 = _mm_unpackhi_epi64( xmm_tempa, xmm_tempb );
337 sAR2_Q14_lo_76543210 = _mm_unpacklo_epi64( xmm_tempa, xmm_tempb );
338
339 /* prepare 1 in 8 * 16bit */
340 xmm_one = _mm_set1_epi16(1);
341
342 for( i = 0; i < length; i++ )
343 {
344 /* Short-term prediction */
345 __m128i xmm_hi_07, xmm_hi_8F, xmm_lo_07, xmm_lo_8F;
346
347 /* Avoids introducing a bias because silk_SMLAWB() always rounds to -inf */
348 LPC_pred_Q10 = 8; /* silk_RSHIFT( predictLPCOrder, 1 ); */
349
350 /* shift psLPC_Q14 */
351 psLPC_Q14_hi_89ABCDEF = _mm_alignr_epi8( psLPC_Q14_hi_01234567, psLPC_Q14_hi_89ABCDEF, 2 );
352 psLPC_Q14_lo_89ABCDEF = _mm_alignr_epi8( psLPC_Q14_lo_01234567, psLPC_Q14_lo_89ABCDEF, 2 );
353
354 psLPC_Q14_hi_01234567 = _mm_srli_si128( psLPC_Q14_hi_01234567, 2 );
355 psLPC_Q14_lo_01234567 = _mm_srli_si128( psLPC_Q14_lo_01234567, 2 );
356
357 psLPC_Q14_hi_01234567 = _mm_insert_epi16( psLPC_Q14_hi_01234567, (xq_Q14 >> 16), 7 );
358 psLPC_Q14_lo_01234567 = _mm_insert_epi16( psLPC_Q14_lo_01234567, (xq_Q14), 7 );
359
360 /* high part, use pmaddwd, results in 4 32-bit */
361 xmm_hi_07 = _mm_madd_epi16( psLPC_Q14_hi_01234567, a_Q12_01234567 );
362 xmm_hi_8F = _mm_madd_epi16( psLPC_Q14_hi_89ABCDEF, a_Q12_89ABCDEF );
363
364 /* low part, use pmulhw, results in 8 16-bit, note we need simulate unsigned * signed, _mm_srai_epi16(psLPC_Q14_lo_01234567, 15) */
365 xmm_tempa = _mm_cmpgt_epi16( _mm_setzero_si128(), psLPC_Q14_lo_01234567 );
366 xmm_tempb = _mm_cmpgt_epi16( _mm_setzero_si128(), psLPC_Q14_lo_89ABCDEF );
367
368 xmm_tempa = _mm_and_si128( xmm_tempa, a_Q12_01234567 );
369 xmm_tempb = _mm_and_si128( xmm_tempb, a_Q12_89ABCDEF );
370
371 xmm_lo_07 = _mm_mulhi_epi16( psLPC_Q14_lo_01234567, a_Q12_01234567 );
372 xmm_lo_8F = _mm_mulhi_epi16( psLPC_Q14_lo_89ABCDEF, a_Q12_89ABCDEF );
373
374 xmm_lo_07 = _mm_add_epi16( xmm_lo_07, xmm_tempa );
375 xmm_lo_8F = _mm_add_epi16( xmm_lo_8F, xmm_tempb );
376
377 xmm_lo_07 = _mm_madd_epi16( xmm_lo_07, xmm_one );
378 xmm_lo_8F = _mm_madd_epi16( xmm_lo_8F, xmm_one );
379
380 /* accumulate */
381 xmm_hi_07 = _mm_add_epi32( xmm_hi_07, xmm_hi_8F );
382 xmm_lo_07 = _mm_add_epi32( xmm_lo_07, xmm_lo_8F );
383
384 xmm_hi_07 = _mm_add_epi32( xmm_hi_07, xmm_lo_07 );
385
386 xmm_hi_07 = _mm_add_epi32( xmm_hi_07, _mm_unpackhi_epi64(xmm_hi_07, xmm_hi_07 ) );
387 xmm_hi_07 = _mm_add_epi32( xmm_hi_07, _mm_shufflelo_epi16(xmm_hi_07, 0x0E ) );
388
389 LPC_pred_Q10 += _mm_cvtsi128_si32( xmm_hi_07 );
390
391 /* Long-term prediction */
392 if ( opus_likely( signalType == TYPE_VOICED ) ) {
393 /* Unrolled loop */
394 /* Avoids introducing a bias because silk_SMLAWB() always rounds to -inf */
395 LTP_pred_Q13 = 2;
396 {
397 __m128i b_Q14_3210, b_Q14_0123, pred_lag_ptr_0123;
398
399 b_Q14_3210 = OP_CVTEPI16_EPI32_M64( b_Q14 );
400 b_Q14_0123 = _mm_shuffle_epi32( b_Q14_3210, 0x1B );
401
402 /* loaded: [0] [-1] [-2] [-3] */
403 pred_lag_ptr_0123 = _mm_loadu_si128( (__m128i *)(&pred_lag_ptr[ -3 ] ) );
404 /* shuffle to [-3] [-2] [-1] [0] and to new xmm */
405 xmm_tempa = _mm_shuffle_epi32( pred_lag_ptr_0123, 0x1B );
406 /*64-bit multiply, a[2] * b[-2], a[0] * b[0] */
407 xmm_tempa = _mm_mul_epi32( xmm_tempa, b_Q14_3210 );
408 /* right shift 2 bytes (16 bits), zero extended */
409 xmm_tempa = _mm_srli_si128( xmm_tempa, 2 );
410
411 /* a[1] * b[-1], a[3] * b[-3] */
412 pred_lag_ptr_0123 = _mm_mul_epi32( pred_lag_ptr_0123, b_Q14_0123 );
413 pred_lag_ptr_0123 = _mm_srli_si128( pred_lag_ptr_0123, 2 );
414
415 pred_lag_ptr_0123 = _mm_add_epi32( pred_lag_ptr_0123, xmm_tempa );
416 /* equal shift right 8 bytes*/
417 xmm_tempa = _mm_shuffle_epi32( pred_lag_ptr_0123, _MM_SHUFFLE( 0, 0, 3, 2 ) );
418 xmm_tempa = _mm_add_epi32( xmm_tempa, pred_lag_ptr_0123 );
419
420 LTP_pred_Q13 += _mm_cvtsi128_si32( xmm_tempa );
421
422 LTP_pred_Q13 = silk_SMLAWB( LTP_pred_Q13, pred_lag_ptr[ -4 ], b_Q14[ 4 ] );
423 pred_lag_ptr++;
424 }
425 }
426
427 /* Noise shape feedback */
428 NSQ->sAR2_Q14[ 9 ] = NSQ->sAR2_Q14[ 8 ];
429 NSQ->sAR2_Q14[ 8 ] = _mm_cvtsi128_si32( _mm_srli_si128(_mm_unpackhi_epi16( sAR2_Q14_lo_76543210, sAR2_Q14_hi_76543210 ), 12 ) );
430
431 sAR2_Q14_hi_76543210 = _mm_slli_si128( sAR2_Q14_hi_76543210, 2 );
432 sAR2_Q14_lo_76543210 = _mm_slli_si128( sAR2_Q14_lo_76543210, 2 );
433
434 sAR2_Q14_hi_76543210 = _mm_insert_epi16( sAR2_Q14_hi_76543210, (xq_Q14 >> 16), 0 );
435 sAR2_Q14_lo_76543210 = _mm_insert_epi16( sAR2_Q14_lo_76543210, (xq_Q14), 0 );
436
437 /* high part, use pmaddwd, results in 4 32-bit */
438 xmm_hi_07 = _mm_madd_epi16( sAR2_Q14_hi_76543210, AR_shp_Q13_76543210 );
439
440 /* low part, use pmulhw, results in 8 16-bit, note we need simulate unsigned * signed,_mm_srai_epi16(sAR2_Q14_lo_76543210, 15) */
441 xmm_tempa = _mm_cmpgt_epi16( _mm_setzero_si128(), sAR2_Q14_lo_76543210 );
442 xmm_tempa = _mm_and_si128( xmm_tempa, AR_shp_Q13_76543210 );
443
444 xmm_lo_07 = _mm_mulhi_epi16( sAR2_Q14_lo_76543210, AR_shp_Q13_76543210 );
445 xmm_lo_07 = _mm_add_epi16( xmm_lo_07, xmm_tempa );
446
447 xmm_lo_07 = _mm_madd_epi16( xmm_lo_07, xmm_one );
448
449 /* accumulate */
450 xmm_hi_07 = _mm_add_epi32( xmm_hi_07, xmm_lo_07 );
451
452 xmm_hi_07 = _mm_add_epi32( xmm_hi_07, _mm_unpackhi_epi64(xmm_hi_07, xmm_hi_07 ) );
453 xmm_hi_07 = _mm_add_epi32( xmm_hi_07, _mm_shufflelo_epi16(xmm_hi_07, 0x0E ) );
454
455 n_AR_Q12 = 5 + _mm_cvtsi128_si32( xmm_hi_07 );
456
457 n_AR_Q12 = silk_SMLAWB( n_AR_Q12, NSQ->sAR2_Q14[ 8 ], AR_shp_Q13[ 8 ] );
458 n_AR_Q12 = silk_SMLAWB( n_AR_Q12, NSQ->sAR2_Q14[ 9 ], AR_shp_Q13[ 9 ] );
459
460 n_AR_Q12 = silk_LSHIFT32( n_AR_Q12, 1 ); /* Q11 -> Q12 */
461 n_AR_Q12 = silk_SMLAWB( n_AR_Q12, sLF_AR_shp_Q14, Tilt_Q14 );
462
463 n_LF_Q12 = silk_SMULWB( NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - 1 ], LF_shp_Q14 );
464 n_LF_Q12 = silk_SMLAWT( n_LF_Q12, sLF_AR_shp_Q14, LF_shp_Q14 );
465
466 silk_assert( lag > 0 || signalType != TYPE_VOICED );
467
468 /* Combine prediction and noise shaping signals */
469 tmp1 = silk_SUB32( silk_LSHIFT32( LPC_pred_Q10, 2 ), n_AR_Q12 ); /* Q12 */
470 tmp1 = silk_SUB32( tmp1, n_LF_Q12 ); /* Q12 */
471 if( lag > 0 ) {
472 /* Symmetric, packed FIR coefficients */
473 n_LTP_Q13 = silk_SMULWB( silk_ADD32( shp_lag_ptr[ 0 ], shp_lag_ptr[ -2 ] ), HarmShapeFIRPacked_Q14 );
474 n_LTP_Q13 = silk_SMLAWT( n_LTP_Q13, shp_lag_ptr[ -1 ], HarmShapeFIRPacked_Q14 );
475 n_LTP_Q13 = silk_LSHIFT( n_LTP_Q13, 1 );
476 shp_lag_ptr++;
477
478 tmp2 = silk_SUB32( LTP_pred_Q13, n_LTP_Q13 ); /* Q13 */
479 tmp1 = silk_ADD_LSHIFT32( tmp2, tmp1, 1 ); /* Q13 */
480 tmp1 = silk_RSHIFT_ROUND( tmp1, 3 ); /* Q10 */
481 } else {
482 tmp1 = silk_RSHIFT_ROUND( tmp1, 2 ); /* Q10 */
483 }
484
485 r_Q10 = silk_SUB32( x_sc_Q10[ i ], tmp1 ); /* residual error Q10 */
486
487 /* Generate dither */
488 NSQ->rand_seed = silk_RAND( NSQ->rand_seed );
489
490 /* Flip sign depending on dither */
491 tmp2 = -r_Q10;
492 if ( NSQ->rand_seed < 0 ) r_Q10 = tmp2;
493
494 r_Q10 = silk_LIMIT_32( r_Q10, -(31 << 10), 30 << 10 );
495
496 /* Find two quantization level candidates and measure their rate-distortion */
497 q1_Q10 = silk_SUB32( r_Q10, offset_Q10 );
498 q1_Q0 = silk_RSHIFT( q1_Q10, 10 );
499
500 q1_Q10 = table[q1_Q0][0];
501 q2_Q10 = table[q1_Q0][1];
502
503 if (r_Q10 * table[q1_Q0][2] - table[q1_Q0][3] < 0)
504 {
505 q1_Q10 = q2_Q10;
506 }
507
508 pulses[ i ] = (opus_int8)silk_RSHIFT_ROUND( q1_Q10, 10 );
509
510 /* Excitation */
511 exc_Q14 = silk_LSHIFT( q1_Q10, 4 );
512
513 tmp2 = -exc_Q14;
514 if ( NSQ->rand_seed < 0 ) exc_Q14 = tmp2;
515
516 /* Add predictions */
517 LPC_exc_Q14 = silk_ADD_LSHIFT32( exc_Q14, LTP_pred_Q13, 1 );
518 xq_Q14 = silk_ADD_LSHIFT32( LPC_exc_Q14, LPC_pred_Q10, 4 );
519
520 /* Update states */
521 psLPC_Q14++;
522 *psLPC_Q14 = xq_Q14;
523 sLF_AR_shp_Q14 = silk_SUB_LSHIFT32( xq_Q14, n_AR_Q12, 2 );
524
525 NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx ] = silk_SUB_LSHIFT32( sLF_AR_shp_Q14, n_LF_Q12, 2 );
526 sLTP_Q15[ NSQ->sLTP_buf_idx ] = silk_LSHIFT( LPC_exc_Q14, 1 );
527 NSQ->sLTP_shp_buf_idx++;
528 NSQ->sLTP_buf_idx++;
529
530 /* Make dither dependent on quantized signal */
531 NSQ->rand_seed = silk_ADD32_ovflw( NSQ->rand_seed, pulses[ i ] );
532 }
533
534 NSQ->sLF_AR_shp_Q14 = sLF_AR_shp_Q14;
535
536 /* Scale XQ back to normal level before saving */
537 psLPC_Q14 = &NSQ->sLPC_Q14[ NSQ_LPC_BUF_LENGTH ];
538
539 /* write back sAR2_Q14 */
540 xmm_tempa = _mm_unpackhi_epi16( sAR2_Q14_lo_76543210, sAR2_Q14_hi_76543210 );
541 xmm_tempb = _mm_unpacklo_epi16( sAR2_Q14_lo_76543210, sAR2_Q14_hi_76543210 );
542 _mm_storeu_si128( (__m128i *)(&NSQ->sAR2_Q14[ 4 ]), xmm_tempa );
543 _mm_storeu_si128( (__m128i *)(&NSQ->sAR2_Q14[ 0 ]), xmm_tempb );
544
545 /* xq[ i ] = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND( silk_SMULWW( psLPC_Q14[ i ], Gain_Q10 ), 8 ) ); */
546 {
547 __m128i xmm_Gain_Q10;
548 __m128i xmm_xq_Q14_3210, xmm_xq_Q14_x3x1, xmm_xq_Q14_7654, xmm_xq_Q14_x7x5;
549
550 /* prepare (1 << 7) in packed 4 32-bits */
551 xmm_tempa = _mm_set1_epi32( (1 << 7) );
552
553 /* prepare Gain_Q10 in packed 4 32-bits */
554 xmm_Gain_Q10 = _mm_set1_epi32( Gain_Q10 );
555
556 /* process xq */
557 for (i = 0; i < length - 7; i += 8)
558 {
559 xmm_xq_Q14_3210 = _mm_loadu_si128( (__m128i *)(&(psLPC_Q14[ i + 0 ] ) ) );
560 xmm_xq_Q14_7654 = _mm_loadu_si128( (__m128i *)(&(psLPC_Q14[ i + 4 ] ) ) );
561
562 /* equal shift right 4 bytes*/
563 xmm_xq_Q14_x3x1 = _mm_shuffle_epi32( xmm_xq_Q14_3210, _MM_SHUFFLE( 0, 3, 2, 1 ) );
564 /* equal shift right 4 bytes*/
565 xmm_xq_Q14_x7x5 = _mm_shuffle_epi32( xmm_xq_Q14_7654, _MM_SHUFFLE( 0, 3, 2, 1 ) );
566
567 xmm_xq_Q14_3210 = _mm_mul_epi32( xmm_xq_Q14_3210, xmm_Gain_Q10 );
568 xmm_xq_Q14_x3x1 = _mm_mul_epi32( xmm_xq_Q14_x3x1, xmm_Gain_Q10 );
569 xmm_xq_Q14_7654 = _mm_mul_epi32( xmm_xq_Q14_7654, xmm_Gain_Q10 );
570 xmm_xq_Q14_x7x5 = _mm_mul_epi32( xmm_xq_Q14_x7x5, xmm_Gain_Q10 );
571
572 xmm_xq_Q14_3210 = _mm_srli_epi64( xmm_xq_Q14_3210, 16 );
573 xmm_xq_Q14_x3x1 = _mm_slli_epi64( xmm_xq_Q14_x3x1, 16 );
574 xmm_xq_Q14_7654 = _mm_srli_epi64( xmm_xq_Q14_7654, 16 );
575 xmm_xq_Q14_x7x5 = _mm_slli_epi64( xmm_xq_Q14_x7x5, 16 );
576
577 xmm_xq_Q14_3210 = _mm_blend_epi16( xmm_xq_Q14_3210, xmm_xq_Q14_x3x1, 0xCC );
578 xmm_xq_Q14_7654 = _mm_blend_epi16( xmm_xq_Q14_7654, xmm_xq_Q14_x7x5, 0xCC );
579
580 /* silk_RSHIFT_ROUND(xq, 8) */
581 xmm_xq_Q14_3210 = _mm_add_epi32( xmm_xq_Q14_3210, xmm_tempa );
582 xmm_xq_Q14_7654 = _mm_add_epi32( xmm_xq_Q14_7654, xmm_tempa );
583
584 xmm_xq_Q14_3210 = _mm_srai_epi32( xmm_xq_Q14_3210, 8 );
585 xmm_xq_Q14_7654 = _mm_srai_epi32( xmm_xq_Q14_7654, 8 );
586
587 /* silk_SAT16 */
588 xmm_xq_Q14_3210 = _mm_packs_epi32( xmm_xq_Q14_3210, xmm_xq_Q14_7654 );
589
590 /* save to xq */
591 _mm_storeu_si128( (__m128i *)(&xq[ i ] ), xmm_xq_Q14_3210 );
592 }
593 }
594 for ( ; i < length; i++)
595 {
596 xq[i] = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND( silk_SMULWW( psLPC_Q14[ i ], Gain_Q10 ), 8 ) );
597 }
598
599 /* Update LPC synth buffer */
600 silk_memcpy( NSQ->sLPC_Q14, &NSQ->sLPC_Q14[ length ], NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
601}
602
603static OPUS_INLINE void silk_nsq_scale_states_sse4_1(
604 const silk_encoder_state *psEncC, /* I Encoder State */
605 silk_nsq_state *NSQ, /* I/O NSQ state */
606 const opus_int32 x_Q3[], /* I input in Q3 */
607 opus_int32 x_sc_Q10[], /* O input scaled with 1/Gain */
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 const opus_int LTP_scale_Q14, /* I */
612 const opus_int32 Gains_Q16[ MAX_NB_SUBFR ], /* I */
613 const opus_int pitchL[ MAX_NB_SUBFR ], /* I Pitch lag */
614 const opus_int signal_type /* I Signal type */
615)
616{
617 opus_int i, lag;
618 opus_int32 gain_adj_Q16, inv_gain_Q31, inv_gain_Q23;
619 __m128i xmm_inv_gain_Q23, xmm_x_Q3_x2x0, xmm_x_Q3_x3x1;
620
621 lag = pitchL[ subfr ];
622 inv_gain_Q31 = silk_INVERSE32_varQ( silk_max( Gains_Q16[ subfr ], 1 ), 47 );
623 silk_assert( inv_gain_Q31 != 0 );
624
625 /* Calculate gain adjustment factor */
626 if( Gains_Q16[ subfr ] != NSQ->prev_gain_Q16 ) {
627 gain_adj_Q16 = silk_DIV32_varQ( NSQ->prev_gain_Q16, Gains_Q16[ subfr ], 16 );
628 } else {
629 gain_adj_Q16 = (opus_int32)1 << 16;
630 }
631
632 /* Scale input */
633 inv_gain_Q23 = silk_RSHIFT_ROUND( inv_gain_Q31, 8 );
634
635 /* prepare inv_gain_Q23 in packed 4 32-bits */
636 xmm_inv_gain_Q23 = _mm_set1_epi32(inv_gain_Q23);
637
638 for( i = 0; i < psEncC->subfr_length - 3; i += 4 ) {
639 xmm_x_Q3_x2x0 = _mm_loadu_si128( (__m128i *)(&(x_Q3[ i ] ) ) );
640
641 /* equal shift right 4 bytes*/
642 xmm_x_Q3_x3x1 = _mm_shuffle_epi32( xmm_x_Q3_x2x0, _MM_SHUFFLE( 0, 3, 2, 1 ) );
643
644 xmm_x_Q3_x2x0 = _mm_mul_epi32( xmm_x_Q3_x2x0, xmm_inv_gain_Q23 );
645 xmm_x_Q3_x3x1 = _mm_mul_epi32( xmm_x_Q3_x3x1, xmm_inv_gain_Q23 );
646
647 xmm_x_Q3_x2x0 = _mm_srli_epi64( xmm_x_Q3_x2x0, 16 );
648 xmm_x_Q3_x3x1 = _mm_slli_epi64( xmm_x_Q3_x3x1, 16 );
649
650 xmm_x_Q3_x2x0 = _mm_blend_epi16( xmm_x_Q3_x2x0, xmm_x_Q3_x3x1, 0xCC );
651
652 _mm_storeu_si128( (__m128i *)(&(x_sc_Q10[ i ] ) ), xmm_x_Q3_x2x0 );
653 }
654
655 for( ; i < psEncC->subfr_length; i++ ) {
656 x_sc_Q10[ i ] = silk_SMULWW( x_Q3[ i ], inv_gain_Q23 );
657 }
658
659 /* Save inverse gain */
660 NSQ->prev_gain_Q16 = Gains_Q16[ subfr ];
661
662 /* After rewhitening the LTP state is un-scaled, so scale with inv_gain_Q16 */
663 if( NSQ->rewhite_flag ) {
664 if( subfr == 0 ) {
665 /* Do LTP downscaling */
666 inv_gain_Q31 = silk_LSHIFT( silk_SMULWB( inv_gain_Q31, LTP_scale_Q14 ), 2 );
667 }
668 for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {
669 silk_assert( i < MAX_FRAME_LENGTH );
670 sLTP_Q15[ i ] = silk_SMULWB( inv_gain_Q31, sLTP[ i ] );
671 }
672 }
673
674 /* Adjust for changing gain */
675 if( gain_adj_Q16 != (opus_int32)1 << 16 ) {
676 /* Scale long-term shaping state */
677 __m128i xmm_gain_adj_Q16, xmm_sLTP_shp_Q14_x2x0, xmm_sLTP_shp_Q14_x3x1;
678
679 /* prepare gain_adj_Q16 in packed 4 32-bits */
680 xmm_gain_adj_Q16 = _mm_set1_epi32(gain_adj_Q16);
681
682 for( i = NSQ->sLTP_shp_buf_idx - psEncC->ltp_mem_length; i < NSQ->sLTP_shp_buf_idx - 3; i += 4 )
683 {
684 xmm_sLTP_shp_Q14_x2x0 = _mm_loadu_si128( (__m128i *)(&(NSQ->sLTP_shp_Q14[ i ] ) ) );
685 /* equal shift right 4 bytes*/
686 xmm_sLTP_shp_Q14_x3x1 = _mm_shuffle_epi32( xmm_sLTP_shp_Q14_x2x0, _MM_SHUFFLE( 0, 3, 2, 1 ) );
687
688 xmm_sLTP_shp_Q14_x2x0 = _mm_mul_epi32( xmm_sLTP_shp_Q14_x2x0, xmm_gain_adj_Q16 );
689 xmm_sLTP_shp_Q14_x3x1 = _mm_mul_epi32( xmm_sLTP_shp_Q14_x3x1, xmm_gain_adj_Q16 );
690
691 xmm_sLTP_shp_Q14_x2x0 = _mm_srli_epi64( xmm_sLTP_shp_Q14_x2x0, 16 );
692 xmm_sLTP_shp_Q14_x3x1 = _mm_slli_epi64( xmm_sLTP_shp_Q14_x3x1, 16 );
693
694 xmm_sLTP_shp_Q14_x2x0 = _mm_blend_epi16( xmm_sLTP_shp_Q14_x2x0, xmm_sLTP_shp_Q14_x3x1, 0xCC );
695
696 _mm_storeu_si128( (__m128i *)(&(NSQ->sLTP_shp_Q14[ i ] ) ), xmm_sLTP_shp_Q14_x2x0 );
697 }
698
699 for( ; i < NSQ->sLTP_shp_buf_idx; i++ ) {
700 NSQ->sLTP_shp_Q14[ i ] = silk_SMULWW( gain_adj_Q16, NSQ->sLTP_shp_Q14[ i ] );
701 }
702
703 /* Scale long-term prediction state */
704 if( signal_type == TYPE_VOICED && NSQ->rewhite_flag == 0 ) {
705 for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {
706 sLTP_Q15[ i ] = silk_SMULWW( gain_adj_Q16, sLTP_Q15[ i ] );
707 }
708 }
709
710 NSQ->sLF_AR_shp_Q14 = silk_SMULWW( gain_adj_Q16, NSQ->sLF_AR_shp_Q14 );
711
712 /* Scale short-term prediction and shaping states */
713 for( i = 0; i < NSQ_LPC_BUF_LENGTH; i++ ) {
714 NSQ->sLPC_Q14[ i ] = silk_SMULWW( gain_adj_Q16, NSQ->sLPC_Q14[ i ] );
715 }
716 for( i = 0; i < MAX_SHAPE_LPC_ORDER; i++ ) {
717 NSQ->sAR2_Q14[ i ] = silk_SMULWW( gain_adj_Q16, NSQ->sAR2_Q14[ i ] );
718 }
719 }
720}