1 ////////////////////////////////////////////////////////////////////////////////
2 ///
3 /// MMX optimized routines. All MMX optimized functions have been gathered into
4 /// this single source code file, regardless to their class or original source
5 /// code file, in order to ease porting the library to other compiler and
6 /// processor platforms.
7 ///
8 /// The MMX-optimizations are programmed using MMX compiler intrinsics that
9 /// are supported both by Microsoft Visual C++ and GCC compilers, so this file
10 /// should compile with both toolsets.
11 ///
12 /// NOTICE: If using Visual Studio 6.0, you'll need to install the "Visual C++
13 /// 6.0 processor pack" update to support compiler intrinsic syntax. The update
14 /// is available for download at Microsoft Developers Network, see here:
15 /// http://msdn.microsoft.com/en-us/vstudio/aa718349.aspx
16 ///
17 /// Author : Copyright (c) Olli Parviainen
18 /// Author e-mail : oparviai 'at' iki.fi
19 /// SoundTouch WWW: http://www.surina.net/soundtouch
20 ///
21 ////////////////////////////////////////////////////////////////////////////////
22 //
23 // Last changed : $Date: 2017-03-05 15:56:03 +0200 (su, 05 maalis 2017) $
24 // File revision : $Revision: 4 $
25 //
26 // $Id: mmx_optimized.cpp 247 2017-03-05 13:56:03Z oparviai $
27 //
28 ////////////////////////////////////////////////////////////////////////////////
29 //
30 // License :
31 //
32 // SoundTouch audio processing library
33 // Copyright (c) Olli Parviainen
34 //
35 // This library is free software; you can redistribute it and/or
36 // modify it under the terms of the GNU Lesser General Public
37 // License as published by the Free Software Foundation; either
38 // version 2.1 of the License, or (at your option) any later version.
39 //
40 // This library is distributed in the hope that it will be useful,
41 // but WITHOUT ANY WARRANTY; without even the implied warranty of
42 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
43 // Lesser General Public License for more details.
44 //
45 // You should have received a copy of the GNU Lesser General Public
46 // License along with this library; if not, write to the Free Software
47 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
48 //
49 ////////////////////////////////////////////////////////////////////////////////
50
51 #include "STTypes.h"
52
53 #ifdef SOUNDTOUCH_ALLOW_MMX
54 // MMX routines available only with integer sample type
55
56 using namespace soundtouch;
57
58 //////////////////////////////////////////////////////////////////////////////
59 //
60 // implementation of MMX optimized functions of class 'TDStretchMMX'
61 //
62 //////////////////////////////////////////////////////////////////////////////
63
64 #include "TDStretch.h"
65 #include <mmintrin.h>
66 #include <limits.h>
67 #include <math.h>
68
69
70 // Calculates cross correlation of two buffers
calcCrossCorr(const short * pV1,const short * pV2,double & dnorm)71 double TDStretchMMX::calcCrossCorr(const short *pV1, const short *pV2, double &dnorm)
72 {
73 const __m64 *pVec1, *pVec2;
74 __m64 shifter;
75 __m64 accu, normaccu;
76 long corr, norm;
77 int i;
78
79 pVec1 = (__m64*)pV1;
80 pVec2 = (__m64*)pV2;
81
82 shifter = _m_from_int(overlapDividerBitsNorm);
83 normaccu = accu = _mm_setzero_si64();
84
85 // Process 4 parallel sets of 2 * stereo samples or 4 * mono samples
86 // during each round for improved CPU-level parallellization.
87 for (i = 0; i < channels * overlapLength / 16; i ++)
88 {
89 __m64 temp, temp2;
90
91 // dictionary of instructions:
92 // _m_pmaddwd : 4*16bit multiply-add, resulting two 32bits = [a0*b0+a1*b1 ; a2*b2+a3*b3]
93 // _mm_add_pi32 : 2*32bit add
94 // _m_psrad : 32bit right-shift
95
96 temp = _mm_add_pi32(_mm_sra_pi32(_mm_madd_pi16(pVec1[0], pVec2[0]), shifter),
97 _mm_sra_pi32(_mm_madd_pi16(pVec1[1], pVec2[1]), shifter));
98 temp2 = _mm_add_pi32(_mm_sra_pi32(_mm_madd_pi16(pVec1[0], pVec1[0]), shifter),
99 _mm_sra_pi32(_mm_madd_pi16(pVec1[1], pVec1[1]), shifter));
100 accu = _mm_add_pi32(accu, temp);
101 normaccu = _mm_add_pi32(normaccu, temp2);
102
103 temp = _mm_add_pi32(_mm_sra_pi32(_mm_madd_pi16(pVec1[2], pVec2[2]), shifter),
104 _mm_sra_pi32(_mm_madd_pi16(pVec1[3], pVec2[3]), shifter));
105 temp2 = _mm_add_pi32(_mm_sra_pi32(_mm_madd_pi16(pVec1[2], pVec1[2]), shifter),
106 _mm_sra_pi32(_mm_madd_pi16(pVec1[3], pVec1[3]), shifter));
107 accu = _mm_add_pi32(accu, temp);
108 normaccu = _mm_add_pi32(normaccu, temp2);
109
110 pVec1 += 4;
111 pVec2 += 4;
112 }
113
114 // copy hi-dword of mm0 to lo-dword of mm1, then sum mmo+mm1
115 // and finally store the result into the variable "corr"
116
117 accu = _mm_add_pi32(accu, _mm_srli_si64(accu, 32));
118 corr = _m_to_int(accu);
119
120 normaccu = _mm_add_pi32(normaccu, _mm_srli_si64(normaccu, 32));
121 norm = _m_to_int(normaccu);
122
123 // Clear MMS state
124 _m_empty();
125
126 if (norm > (long)maxnorm)
127 {
128 // modify 'maxnorm' inside critical section to avoid multi-access conflict if in OpenMP mode
129 #pragma omp critical
130 if (norm > (long)maxnorm)
131 {
132 maxnorm = norm;
133 }
134 }
135
136 // Normalize result by dividing by sqrt(norm) - this step is easiest
137 // done using floating point operation
138 dnorm = (double)norm;
139
140 return (double)corr / sqrt(dnorm < 1e-9 ? 1.0 : dnorm);
141 // Note: Warning about the missing EMMS instruction is harmless
142 // as it'll be called elsewhere.
143 }
144
145
146 /// Update cross-correlation by accumulating "norm" coefficient by previously calculated value
calcCrossCorrAccumulate(const short * pV1,const short * pV2,double & dnorm)147 double TDStretchMMX::calcCrossCorrAccumulate(const short *pV1, const short *pV2, double &dnorm)
148 {
149 const __m64 *pVec1, *pVec2;
150 __m64 shifter;
151 __m64 accu;
152 long corr, lnorm;
153 int i;
154
155 // cancel first normalizer tap from previous round
156 lnorm = 0;
157 for (i = 1; i <= channels; i ++)
158 {
159 lnorm -= (pV1[-i] * pV1[-i]) >> overlapDividerBitsNorm;
160 }
161
162 pVec1 = (__m64*)pV1;
163 pVec2 = (__m64*)pV2;
164
165 shifter = _m_from_int(overlapDividerBitsNorm);
166 accu = _mm_setzero_si64();
167
168 // Process 4 parallel sets of 2 * stereo samples or 4 * mono samples
169 // during each round for improved CPU-level parallellization.
170 for (i = 0; i < channels * overlapLength / 16; i ++)
171 {
172 __m64 temp;
173
174 // dictionary of instructions:
175 // _m_pmaddwd : 4*16bit multiply-add, resulting two 32bits = [a0*b0+a1*b1 ; a2*b2+a3*b3]
176 // _mm_add_pi32 : 2*32bit add
177 // _m_psrad : 32bit right-shift
178
179 temp = _mm_add_pi32(_mm_sra_pi32(_mm_madd_pi16(pVec1[0], pVec2[0]), shifter),
180 _mm_sra_pi32(_mm_madd_pi16(pVec1[1], pVec2[1]), shifter));
181 accu = _mm_add_pi32(accu, temp);
182
183 temp = _mm_add_pi32(_mm_sra_pi32(_mm_madd_pi16(pVec1[2], pVec2[2]), shifter),
184 _mm_sra_pi32(_mm_madd_pi16(pVec1[3], pVec2[3]), shifter));
185 accu = _mm_add_pi32(accu, temp);
186
187 pVec1 += 4;
188 pVec2 += 4;
189 }
190
191 // copy hi-dword of mm0 to lo-dword of mm1, then sum mmo+mm1
192 // and finally store the result into the variable "corr"
193
194 accu = _mm_add_pi32(accu, _mm_srli_si64(accu, 32));
195 corr = _m_to_int(accu);
196
197 // Clear MMS state
198 _m_empty();
199
200 // update normalizer with last samples of this round
201 pV1 = (short *)pVec1;
202 for (int j = 1; j <= channels; j ++)
203 {
204 lnorm += (pV1[-j] * pV1[-j]) >> overlapDividerBitsNorm;
205 }
206 dnorm += (double)lnorm;
207
208 if (lnorm > (long)maxnorm)
209 {
210 maxnorm = lnorm;
211 }
212
213 // Normalize result by dividing by sqrt(norm) - this step is easiest
214 // done using floating point operation
215 return (double)corr / sqrt((dnorm < 1e-9) ? 1.0 : dnorm);
216 }
217
218
clearCrossCorrState()219 void TDStretchMMX::clearCrossCorrState()
220 {
221 // Clear MMS state
222 _m_empty();
223 //_asm EMMS;
224 }
225
226
227
228 // MMX-optimized version of the function overlapStereo
overlapStereo(short * output,const short * input) const229 void TDStretchMMX::overlapStereo(short *output, const short *input) const
230 {
231 const __m64 *pVinput, *pVMidBuf;
232 __m64 *pVdest;
233 __m64 mix1, mix2, adder, shifter;
234 int i;
235
236 pVinput = (const __m64*)input;
237 pVMidBuf = (const __m64*)pMidBuffer;
238 pVdest = (__m64*)output;
239
240 // mix1 = mixer values for 1st stereo sample
241 // mix1 = mixer values for 2nd stereo sample
242 // adder = adder for updating mixer values after each round
243
244 mix1 = _mm_set_pi16(0, overlapLength, 0, overlapLength);
245 adder = _mm_set_pi16(1, -1, 1, -1);
246 mix2 = _mm_add_pi16(mix1, adder);
247 adder = _mm_add_pi16(adder, adder);
248
249 // Overlaplength-division by shifter. "+1" is to account for "-1" deduced in
250 // overlapDividerBits calculation earlier.
251 shifter = _m_from_int(overlapDividerBitsPure + 1);
252
253 for (i = 0; i < overlapLength / 4; i ++)
254 {
255 __m64 temp1, temp2;
256
257 // load & shuffle data so that input & mixbuffer data samples are paired
258 temp1 = _mm_unpacklo_pi16(pVMidBuf[0], pVinput[0]); // = i0l m0l i0r m0r
259 temp2 = _mm_unpackhi_pi16(pVMidBuf[0], pVinput[0]); // = i1l m1l i1r m1r
260
261 // temp = (temp .* mix) >> shifter
262 temp1 = _mm_sra_pi32(_mm_madd_pi16(temp1, mix1), shifter);
263 temp2 = _mm_sra_pi32(_mm_madd_pi16(temp2, mix2), shifter);
264 pVdest[0] = _mm_packs_pi32(temp1, temp2); // pack 2*2*32bit => 4*16bit
265
266 // update mix += adder
267 mix1 = _mm_add_pi16(mix1, adder);
268 mix2 = _mm_add_pi16(mix2, adder);
269
270 // --- second round begins here ---
271
272 // load & shuffle data so that input & mixbuffer data samples are paired
273 temp1 = _mm_unpacklo_pi16(pVMidBuf[1], pVinput[1]); // = i2l m2l i2r m2r
274 temp2 = _mm_unpackhi_pi16(pVMidBuf[1], pVinput[1]); // = i3l m3l i3r m3r
275
276 // temp = (temp .* mix) >> shifter
277 temp1 = _mm_sra_pi32(_mm_madd_pi16(temp1, mix1), shifter);
278 temp2 = _mm_sra_pi32(_mm_madd_pi16(temp2, mix2), shifter);
279 pVdest[1] = _mm_packs_pi32(temp1, temp2); // pack 2*2*32bit => 4*16bit
280
281 // update mix += adder
282 mix1 = _mm_add_pi16(mix1, adder);
283 mix2 = _mm_add_pi16(mix2, adder);
284
285 pVinput += 2;
286 pVMidBuf += 2;
287 pVdest += 2;
288 }
289
290 _m_empty(); // clear MMS state
291 }
292
293
294 //////////////////////////////////////////////////////////////////////////////
295 //
296 // implementation of MMX optimized functions of class 'FIRFilter'
297 //
298 //////////////////////////////////////////////////////////////////////////////
299
300 #include "FIRFilter.h"
301
302
FIRFilterMMX()303 FIRFilterMMX::FIRFilterMMX() : FIRFilter()
304 {
305 filterCoeffsAlign = NULL;
306 filterCoeffsUnalign = NULL;
307 }
308
309
~FIRFilterMMX()310 FIRFilterMMX::~FIRFilterMMX()
311 {
312 delete[] filterCoeffsUnalign;
313 }
314
315
316 // (overloaded) Calculates filter coefficients for MMX routine
setCoefficients(const short * coeffs,uint newLength,uint uResultDivFactor)317 void FIRFilterMMX::setCoefficients(const short *coeffs, uint newLength, uint uResultDivFactor)
318 {
319 uint i;
320 FIRFilter::setCoefficients(coeffs, newLength, uResultDivFactor);
321
322 // Ensure that filter coeffs array is aligned to 16-byte boundary
323 delete[] filterCoeffsUnalign;
324 filterCoeffsUnalign = new short[2 * newLength + 8];
325 filterCoeffsAlign = (short *)SOUNDTOUCH_ALIGN_POINTER_16(filterCoeffsUnalign);
326
327 // rearrange the filter coefficients for mmx routines
328 for (i = 0;i < length; i += 4)
329 {
330 filterCoeffsAlign[2 * i + 0] = coeffs[i + 0];
331 filterCoeffsAlign[2 * i + 1] = coeffs[i + 2];
332 filterCoeffsAlign[2 * i + 2] = coeffs[i + 0];
333 filterCoeffsAlign[2 * i + 3] = coeffs[i + 2];
334
335 filterCoeffsAlign[2 * i + 4] = coeffs[i + 1];
336 filterCoeffsAlign[2 * i + 5] = coeffs[i + 3];
337 filterCoeffsAlign[2 * i + 6] = coeffs[i + 1];
338 filterCoeffsAlign[2 * i + 7] = coeffs[i + 3];
339 }
340 }
341
342
343
344 // mmx-optimized version of the filter routine for stereo sound
evaluateFilterStereo(short * dest,const short * src,uint numSamples) const345 uint FIRFilterMMX::evaluateFilterStereo(short *dest, const short *src, uint numSamples) const
346 {
347 // Create stack copies of the needed member variables for asm routines :
348 uint i, j;
349 __m64 *pVdest = (__m64*)dest;
350
351 if (length < 2) return 0;
352
353 for (i = 0; i < (numSamples - length) / 2; i ++)
354 {
355 __m64 accu1;
356 __m64 accu2;
357 const __m64 *pVsrc = (const __m64*)src;
358 const __m64 *pVfilter = (const __m64*)filterCoeffsAlign;
359
360 accu1 = accu2 = _mm_setzero_si64();
361 for (j = 0; j < lengthDiv8 * 2; j ++)
362 {
363 __m64 temp1, temp2;
364
365 temp1 = _mm_unpacklo_pi16(pVsrc[0], pVsrc[1]); // = l2 l0 r2 r0
366 temp2 = _mm_unpackhi_pi16(pVsrc[0], pVsrc[1]); // = l3 l1 r3 r1
367
368 accu1 = _mm_add_pi32(accu1, _mm_madd_pi16(temp1, pVfilter[0])); // += l2*f2+l0*f0 r2*f2+r0*f0
369 accu1 = _mm_add_pi32(accu1, _mm_madd_pi16(temp2, pVfilter[1])); // += l3*f3+l1*f1 r3*f3+r1*f1
370
371 temp1 = _mm_unpacklo_pi16(pVsrc[1], pVsrc[2]); // = l4 l2 r4 r2
372
373 accu2 = _mm_add_pi32(accu2, _mm_madd_pi16(temp2, pVfilter[0])); // += l3*f2+l1*f0 r3*f2+r1*f0
374 accu2 = _mm_add_pi32(accu2, _mm_madd_pi16(temp1, pVfilter[1])); // += l4*f3+l2*f1 r4*f3+r2*f1
375
376 // accu1 += l2*f2+l0*f0 r2*f2+r0*f0
377 // += l3*f3+l1*f1 r3*f3+r1*f1
378
379 // accu2 += l3*f2+l1*f0 r3*f2+r1*f0
380 // l4*f3+l2*f1 r4*f3+r2*f1
381
382 pVfilter += 2;
383 pVsrc += 2;
384 }
385 // accu >>= resultDivFactor
386 accu1 = _mm_srai_pi32(accu1, resultDivFactor);
387 accu2 = _mm_srai_pi32(accu2, resultDivFactor);
388
389 // pack 2*2*32bits => 4*16 bits
390 pVdest[0] = _mm_packs_pi32(accu1, accu2);
391 src += 4;
392 pVdest ++;
393 }
394
395 _m_empty(); // clear emms state
396
397 return (numSamples & 0xfffffffe) - length;
398 }
399
400 #endif // SOUNDTOUCH_ALLOW_MMX
401