michael@0: ////////////////////////////////////////////////////////////////////////////////
michael@0: ///
michael@0: /// MMX optimized routines. All MMX optimized functions have been gathered into 
michael@0: /// this single source code file, regardless to their class or original source 
michael@0: /// code file, in order to ease porting the library to other compiler and 
michael@0: /// processor platforms.
michael@0: ///
michael@0: /// The MMX-optimizations are programmed using MMX compiler intrinsics that
michael@0: /// are supported both by Microsoft Visual C++ and GCC compilers, so this file
michael@0: /// should compile with both toolsets.
michael@0: ///
michael@0: /// NOTICE: If using Visual Studio 6.0, you'll need to install the "Visual C++ 
michael@0: /// 6.0 processor pack" update to support compiler intrinsic syntax. The update
michael@0: /// is available for download at Microsoft Developers Network, see here:
michael@0: /// http://msdn.microsoft.com/en-us/vstudio/aa718349.aspx
michael@0: ///
michael@0: /// Author        : Copyright (c) Olli Parviainen
michael@0: /// Author e-mail : oparviai 'at' iki.fi
michael@0: /// SoundTouch WWW: http://www.surina.net/soundtouch
michael@0: ///
michael@0: ////////////////////////////////////////////////////////////////////////////////
michael@0: //
michael@0: // Last changed  : $Date: 2014-01-07 12:25:40 -0600 (Tue, 07 Jan 2014) $
michael@0: // File revision : $Revision: 4 $
michael@0: //
michael@0: // $Id: mmx_optimized.cpp 184 2014-01-07 18:25:40Z oparviai $
michael@0: //
michael@0: ////////////////////////////////////////////////////////////////////////////////
michael@0: //
michael@0: // License :
michael@0: //
michael@0: //  SoundTouch audio processing library
michael@0: //  Copyright (c) Olli Parviainen
michael@0: //
michael@0: //  This library is free software; you can redistribute it and/or
michael@0: //  modify it under the terms of the GNU Lesser General Public
michael@0: //  License as published by the Free Software Foundation; either
michael@0: //  version 2.1 of the License, or (at your option) any later version.
michael@0: //
michael@0: //  This library is distributed in the hope that it will be useful,
michael@0: //  but WITHOUT ANY WARRANTY; without even the implied warranty of
michael@0: //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
michael@0: //  Lesser General Public License for more details.
michael@0: //
michael@0: //  You should have received a copy of the GNU Lesser General Public
michael@0: //  License along with this library; if not, write to the Free Software
michael@0: //  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
michael@0: //
michael@0: ////////////////////////////////////////////////////////////////////////////////
michael@0: 
michael@0: #include "STTypes.h"
michael@0: 
michael@0: #ifdef SOUNDTOUCH_ALLOW_MMX
michael@0: // MMX routines available only with integer sample type
michael@0: 
michael@0: using namespace soundtouch;
michael@0: 
michael@0: //////////////////////////////////////////////////////////////////////////////
michael@0: //
michael@0: // implementation of MMX optimized functions of class 'TDStretchMMX'
michael@0: //
michael@0: //////////////////////////////////////////////////////////////////////////////
michael@0: 
michael@0: #include "TDStretch.h"
michael@0: #include <mmintrin.h>
michael@0: #include <limits.h>
michael@0: #include <math.h>
michael@0: 
michael@0: 
michael@0: // Calculates cross correlation of two buffers
michael@0: double TDStretchMMX::calcCrossCorr(const short *pV1, const short *pV2, double &dnorm) const
michael@0: {
michael@0:     const __m64 *pVec1, *pVec2;
michael@0:     __m64 shifter;
michael@0:     __m64 accu, normaccu;
michael@0:     long corr, norm;
michael@0:     int i;
michael@0:    
michael@0:     pVec1 = (__m64*)pV1;
michael@0:     pVec2 = (__m64*)pV2;
michael@0: 
michael@0:     shifter = _m_from_int(overlapDividerBits);
michael@0:     normaccu = accu = _mm_setzero_si64();
michael@0: 
michael@0:     // Process 4 parallel sets of 2 * stereo samples or 4 * mono samples 
michael@0:     // during each round for improved CPU-level parallellization.
michael@0:     for (i = 0; i < channels * overlapLength / 16; i ++)
michael@0:     {
michael@0:         __m64 temp, temp2;
michael@0: 
michael@0:         // dictionary of instructions:
michael@0:         // _m_pmaddwd   : 4*16bit multiply-add, resulting two 32bits = [a0*b0+a1*b1 ; a2*b2+a3*b3]
michael@0:         // _mm_add_pi32 : 2*32bit add
michael@0:         // _m_psrad     : 32bit right-shift
michael@0: 
michael@0:         temp = _mm_add_pi32(_mm_sra_pi32(_mm_madd_pi16(pVec1[0], pVec2[0]), shifter),
michael@0:                             _mm_sra_pi32(_mm_madd_pi16(pVec1[1], pVec2[1]), shifter));
michael@0:         temp2 = _mm_add_pi32(_mm_sra_pi32(_mm_madd_pi16(pVec1[0], pVec1[0]), shifter),
michael@0:                             _mm_sra_pi32(_mm_madd_pi16(pVec1[1], pVec1[1]), shifter));
michael@0:         accu = _mm_add_pi32(accu, temp);
michael@0:         normaccu = _mm_add_pi32(normaccu, temp2);
michael@0: 
michael@0:         temp = _mm_add_pi32(_mm_sra_pi32(_mm_madd_pi16(pVec1[2], pVec2[2]), shifter),
michael@0:                             _mm_sra_pi32(_mm_madd_pi16(pVec1[3], pVec2[3]), shifter));
michael@0:         temp2 = _mm_add_pi32(_mm_sra_pi32(_mm_madd_pi16(pVec1[2], pVec1[2]), shifter),
michael@0:                             _mm_sra_pi32(_mm_madd_pi16(pVec1[3], pVec1[3]), shifter));
michael@0:         accu = _mm_add_pi32(accu, temp);
michael@0:         normaccu = _mm_add_pi32(normaccu, temp2);
michael@0: 
michael@0:         pVec1 += 4;
michael@0:         pVec2 += 4;
michael@0:     }
michael@0: 
michael@0:     // copy hi-dword of mm0 to lo-dword of mm1, then sum mmo+mm1
michael@0:     // and finally store the result into the variable "corr"
michael@0: 
michael@0:     accu = _mm_add_pi32(accu, _mm_srli_si64(accu, 32));
michael@0:     corr = _m_to_int(accu);
michael@0: 
michael@0:     normaccu = _mm_add_pi32(normaccu, _mm_srli_si64(normaccu, 32));
michael@0:     norm = _m_to_int(normaccu);
michael@0: 
michael@0:     // Clear MMS state
michael@0:     _m_empty();
michael@0: 
michael@0:     // Normalize result by dividing by sqrt(norm) - this step is easiest 
michael@0:     // done using floating point operation
michael@0:     dnorm = (double)norm;
michael@0: 
michael@0:     return (double)corr / sqrt(dnorm < 1e-9 ? 1.0 : dnorm);
michael@0:     // Note: Warning about the missing EMMS instruction is harmless
michael@0:     // as it'll be called elsewhere.
michael@0: }
michael@0: 
michael@0: 
michael@0: /// Update cross-correlation by accumulating "norm" coefficient by previously calculated value
michael@0: double TDStretchMMX::calcCrossCorrAccumulate(const short *pV1, const short *pV2, double &dnorm) const
michael@0: {
michael@0:     const __m64 *pVec1, *pVec2;
michael@0:     __m64 shifter;
michael@0:     __m64 accu;
michael@0:     long corr, lnorm;
michael@0:     int i;
michael@0:    
michael@0:     // cancel first normalizer tap from previous round
michael@0:     lnorm = 0;
michael@0:     for (i = 1; i <= channels; i ++)
michael@0:     {
michael@0:         lnorm -= (pV1[-i] * pV1[-i]) >> overlapDividerBits;
michael@0:     }
michael@0: 
michael@0:     pVec1 = (__m64*)pV1;
michael@0:     pVec2 = (__m64*)pV2;
michael@0: 
michael@0:     shifter = _m_from_int(overlapDividerBits);
michael@0:     accu = _mm_setzero_si64();
michael@0: 
michael@0:     // Process 4 parallel sets of 2 * stereo samples or 4 * mono samples 
michael@0:     // during each round for improved CPU-level parallellization.
michael@0:     for (i = 0; i < channels * overlapLength / 16; i ++)
michael@0:     {
michael@0:         __m64 temp;
michael@0: 
michael@0:         // dictionary of instructions:
michael@0:         // _m_pmaddwd   : 4*16bit multiply-add, resulting two 32bits = [a0*b0+a1*b1 ; a2*b2+a3*b3]
michael@0:         // _mm_add_pi32 : 2*32bit add
michael@0:         // _m_psrad     : 32bit right-shift
michael@0: 
michael@0:         temp = _mm_add_pi32(_mm_sra_pi32(_mm_madd_pi16(pVec1[0], pVec2[0]), shifter),
michael@0:                             _mm_sra_pi32(_mm_madd_pi16(pVec1[1], pVec2[1]), shifter));
michael@0:         accu = _mm_add_pi32(accu, temp);
michael@0: 
michael@0:         temp = _mm_add_pi32(_mm_sra_pi32(_mm_madd_pi16(pVec1[2], pVec2[2]), shifter),
michael@0:                             _mm_sra_pi32(_mm_madd_pi16(pVec1[3], pVec2[3]), shifter));
michael@0:         accu = _mm_add_pi32(accu, temp);
michael@0: 
michael@0:         pVec1 += 4;
michael@0:         pVec2 += 4;
michael@0:     }
michael@0: 
michael@0:     // copy hi-dword of mm0 to lo-dword of mm1, then sum mmo+mm1
michael@0:     // and finally store the result into the variable "corr"
michael@0: 
michael@0:     accu = _mm_add_pi32(accu, _mm_srli_si64(accu, 32));
michael@0:     corr = _m_to_int(accu);
michael@0: 
michael@0:     // Clear MMS state
michael@0:     _m_empty();
michael@0: 
michael@0:     // update normalizer with last samples of this round
michael@0:     pV1 = (short *)pVec1;
michael@0:     for (int j = 1; j <= channels; j ++)
michael@0:     {
michael@0:         lnorm += (pV1[-j] * pV1[-j]) >> overlapDividerBits;
michael@0:     }
michael@0:     dnorm += (double)lnorm;
michael@0: 
michael@0:     // Normalize result by dividing by sqrt(norm) - this step is easiest 
michael@0:     // done using floating point operation
michael@0:     return (double)corr / sqrt((dnorm < 1e-9) ? 1.0 : dnorm);
michael@0: }
michael@0: 
michael@0: 
michael@0: void TDStretchMMX::clearCrossCorrState()
michael@0: {
michael@0:     // Clear MMS state
michael@0:     _m_empty();
michael@0:     //_asm EMMS;
michael@0: }
michael@0: 
michael@0: 
michael@0: 
michael@0: // MMX-optimized version of the function overlapStereo
michael@0: void TDStretchMMX::overlapStereo(short *output, const short *input) const
michael@0: {
michael@0:     const __m64 *pVinput, *pVMidBuf;
michael@0:     __m64 *pVdest;
michael@0:     __m64 mix1, mix2, adder, shifter;
michael@0:     int i;
michael@0: 
michael@0:     pVinput  = (const __m64*)input;
michael@0:     pVMidBuf = (const __m64*)pMidBuffer;
michael@0:     pVdest   = (__m64*)output;
michael@0: 
michael@0:     // mix1  = mixer values for 1st stereo sample
michael@0:     // mix1  = mixer values for 2nd stereo sample
michael@0:     // adder = adder for updating mixer values after each round
michael@0:     
michael@0:     mix1  = _mm_set_pi16(0, overlapLength,   0, overlapLength);
michael@0:     adder = _mm_set_pi16(1, -1, 1, -1);
michael@0:     mix2  = _mm_add_pi16(mix1, adder);
michael@0:     adder = _mm_add_pi16(adder, adder);
michael@0: 
michael@0:     // Overlaplength-division by shifter. "+1" is to account for "-1" deduced in
michael@0:     // overlapDividerBits calculation earlier.
michael@0:     shifter = _m_from_int(overlapDividerBits + 1);
michael@0: 
michael@0:     for (i = 0; i < overlapLength / 4; i ++)
michael@0:     {
michael@0:         __m64 temp1, temp2;
michael@0:                 
michael@0:         // load & shuffle data so that input & mixbuffer data samples are paired
michael@0:         temp1 = _mm_unpacklo_pi16(pVMidBuf[0], pVinput[0]);     // = i0l m0l i0r m0r
michael@0:         temp2 = _mm_unpackhi_pi16(pVMidBuf[0], pVinput[0]);     // = i1l m1l i1r m1r
michael@0: 
michael@0:         // temp = (temp .* mix) >> shifter
michael@0:         temp1 = _mm_sra_pi32(_mm_madd_pi16(temp1, mix1), shifter);
michael@0:         temp2 = _mm_sra_pi32(_mm_madd_pi16(temp2, mix2), shifter);
michael@0:         pVdest[0] = _mm_packs_pi32(temp1, temp2); // pack 2*2*32bit => 4*16bit
michael@0: 
michael@0:         // update mix += adder
michael@0:         mix1 = _mm_add_pi16(mix1, adder);
michael@0:         mix2 = _mm_add_pi16(mix2, adder);
michael@0: 
michael@0:         // --- second round begins here ---
michael@0: 
michael@0:         // load & shuffle data so that input & mixbuffer data samples are paired
michael@0:         temp1 = _mm_unpacklo_pi16(pVMidBuf[1], pVinput[1]);       // = i2l m2l i2r m2r
michael@0:         temp2 = _mm_unpackhi_pi16(pVMidBuf[1], pVinput[1]);       // = i3l m3l i3r m3r
michael@0: 
michael@0:         // temp = (temp .* mix) >> shifter
michael@0:         temp1 = _mm_sra_pi32(_mm_madd_pi16(temp1, mix1), shifter);
michael@0:         temp2 = _mm_sra_pi32(_mm_madd_pi16(temp2, mix2), shifter);
michael@0:         pVdest[1] = _mm_packs_pi32(temp1, temp2); // pack 2*2*32bit => 4*16bit
michael@0: 
michael@0:         // update mix += adder
michael@0:         mix1 = _mm_add_pi16(mix1, adder);
michael@0:         mix2 = _mm_add_pi16(mix2, adder);
michael@0: 
michael@0:         pVinput  += 2;
michael@0:         pVMidBuf += 2;
michael@0:         pVdest   += 2;
michael@0:     }
michael@0: 
michael@0:     _m_empty(); // clear MMS state
michael@0: }
michael@0: 
michael@0: 
michael@0: //////////////////////////////////////////////////////////////////////////////
michael@0: //
michael@0: // implementation of MMX optimized functions of class 'FIRFilter'
michael@0: //
michael@0: //////////////////////////////////////////////////////////////////////////////
michael@0: 
michael@0: #include "FIRFilter.h"
michael@0: 
michael@0: 
michael@0: FIRFilterMMX::FIRFilterMMX() : FIRFilter()
michael@0: {
michael@0:     filterCoeffsUnalign = NULL;
michael@0: }
michael@0: 
michael@0: 
michael@0: FIRFilterMMX::~FIRFilterMMX()
michael@0: {
michael@0:     delete[] filterCoeffsUnalign;
michael@0: }
michael@0: 
michael@0: 
michael@0: // (overloaded) Calculates filter coefficients for MMX routine
michael@0: void FIRFilterMMX::setCoefficients(const short *coeffs, uint newLength, uint uResultDivFactor)
michael@0: {
michael@0:     uint i;
michael@0:     FIRFilter::setCoefficients(coeffs, newLength, uResultDivFactor);
michael@0: 
michael@0:     // Ensure that filter coeffs array is aligned to 16-byte boundary
michael@0:     delete[] filterCoeffsUnalign;
michael@0:     filterCoeffsUnalign = new short[2 * newLength + 8];
michael@0:     filterCoeffsAlign = (short *)SOUNDTOUCH_ALIGN_POINTER_16(filterCoeffsUnalign);
michael@0: 
michael@0:     // rearrange the filter coefficients for mmx routines 
michael@0:     for (i = 0;i < length; i += 4) 
michael@0:     {
michael@0:         filterCoeffsAlign[2 * i + 0] = coeffs[i + 0];
michael@0:         filterCoeffsAlign[2 * i + 1] = coeffs[i + 2];
michael@0:         filterCoeffsAlign[2 * i + 2] = coeffs[i + 0];
michael@0:         filterCoeffsAlign[2 * i + 3] = coeffs[i + 2];
michael@0: 
michael@0:         filterCoeffsAlign[2 * i + 4] = coeffs[i + 1];
michael@0:         filterCoeffsAlign[2 * i + 5] = coeffs[i + 3];
michael@0:         filterCoeffsAlign[2 * i + 6] = coeffs[i + 1];
michael@0:         filterCoeffsAlign[2 * i + 7] = coeffs[i + 3];
michael@0:     }
michael@0: }
michael@0: 
michael@0: 
michael@0: 
michael@0: // mmx-optimized version of the filter routine for stereo sound
michael@0: uint FIRFilterMMX::evaluateFilterStereo(short *dest, const short *src, uint numSamples) const
michael@0: {
michael@0:     // Create stack copies of the needed member variables for asm routines :
michael@0:     uint i, j;
michael@0:     __m64 *pVdest = (__m64*)dest;
michael@0: 
michael@0:     if (length < 2) return 0;
michael@0: 
michael@0:     for (i = 0; i < (numSamples - length) / 2; i ++)
michael@0:     {
michael@0:         __m64 accu1;
michael@0:         __m64 accu2;
michael@0:         const __m64 *pVsrc = (const __m64*)src;
michael@0:         const __m64 *pVfilter = (const __m64*)filterCoeffsAlign;
michael@0: 
michael@0:         accu1 = accu2 = _mm_setzero_si64();
michael@0:         for (j = 0; j < lengthDiv8 * 2; j ++)
michael@0:         {
michael@0:             __m64 temp1, temp2;
michael@0: 
michael@0:             temp1 = _mm_unpacklo_pi16(pVsrc[0], pVsrc[1]);  // = l2 l0 r2 r0
michael@0:             temp2 = _mm_unpackhi_pi16(pVsrc[0], pVsrc[1]);  // = l3 l1 r3 r1
michael@0: 
michael@0:             accu1 = _mm_add_pi32(accu1, _mm_madd_pi16(temp1, pVfilter[0]));  // += l2*f2+l0*f0 r2*f2+r0*f0
michael@0:             accu1 = _mm_add_pi32(accu1, _mm_madd_pi16(temp2, pVfilter[1]));  // += l3*f3+l1*f1 r3*f3+r1*f1
michael@0: 
michael@0:             temp1 = _mm_unpacklo_pi16(pVsrc[1], pVsrc[2]);  // = l4 l2 r4 r2
michael@0: 
michael@0:             accu2 = _mm_add_pi32(accu2, _mm_madd_pi16(temp2, pVfilter[0]));  // += l3*f2+l1*f0 r3*f2+r1*f0
michael@0:             accu2 = _mm_add_pi32(accu2, _mm_madd_pi16(temp1, pVfilter[1]));  // += l4*f3+l2*f1 r4*f3+r2*f1
michael@0: 
michael@0:             // accu1 += l2*f2+l0*f0 r2*f2+r0*f0
michael@0:             //       += l3*f3+l1*f1 r3*f3+r1*f1
michael@0: 
michael@0:             // accu2 += l3*f2+l1*f0 r3*f2+r1*f0
michael@0:             //          l4*f3+l2*f1 r4*f3+r2*f1
michael@0: 
michael@0:             pVfilter += 2;
michael@0:             pVsrc += 2;
michael@0:         }
michael@0:         // accu >>= resultDivFactor
michael@0:         accu1 = _mm_srai_pi32(accu1, resultDivFactor);
michael@0:         accu2 = _mm_srai_pi32(accu2, resultDivFactor);
michael@0: 
michael@0:         // pack 2*2*32bits => 4*16 bits
michael@0:         pVdest[0] = _mm_packs_pi32(accu1, accu2);
michael@0:         src += 4;
michael@0:         pVdest ++;
michael@0:     }
michael@0: 
michael@0:    _m_empty();  // clear emms state
michael@0: 
michael@0:     return (numSamples & 0xfffffffe) - length;
michael@0: }
michael@0: 
michael@0: #endif  // SOUNDTOUCH_ALLOW_MMX