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