mmx_optimized.cpp (13450B)
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 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 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 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 221 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 295 FIRFilterMMX::FIRFilterMMX() : FIRFilter() 296 { 297 filterCoeffsAlign = NULL; 298 filterCoeffsUnalign = NULL; 299 } 300 301 302 FIRFilterMMX::~FIRFilterMMX() 303 { 304 delete[] filterCoeffsUnalign; 305 } 306 307 308 // (overloaded) Calculates filter coefficients for MMX routine 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 336 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