#include #include #include class IOMemoryDescriptor; #include #include #include "AppleUSBAudioClip.h" #include "AppleUSBAudioCommon.h" extern "C" { // floating point types typedef float Float32; typedef double Float64; #define FLOATLIB FALSE #if FLOATLIB void CoeffsFilterOrder2 (Float32 *Coeff, Float32 CutOffFreq, Float32 AttAtCutOffFreq , Float64 SamplingRate); #else Boolean CoeffsFilterOrder2Table (Float32 *Coeff, UInt32 samplingRate); #endif void MonoFilter (Float32 *in, Float32 *low, Float32 *high, UInt32 frames, UInt32 samplingRate); void StereoFilter (Float32 *in, Float32 *low, Float32 *high, UInt32 frames, UInt32 samplingRate, PreviousValues *theValue); // aml 2.21.02 added new stereo filter void StereoFilter4thOrder (Float32 *in, Float32 *low, Float32 *high, UInt32 frames, UInt32 samplingRate, PreviousValues *section1State, PreviousValues *section2State); // aml 2.21.02 added new stereo filter with phase compensation void StereoFilter4thOrderPhaseComp (Float32 *in, Float32 *low, Float32 *high, UInt32 frames, UInt32 samplingRate, PreviousValues *section1State, PreviousValues *section2State, PreviousValues *phaseCompState); UInt32 CalculateOffset (UInt64 nanoseconds, UInt32 sampleRate) { return (UInt32)(((double)sampleRate / 1000000000.0) * nanoseconds); } inline static SInt16 Endian16_Swap(SInt16 inValue) { return (((((UInt16)inValue)<<8) & 0xFF00) | ((((UInt16)inValue)>>8) & 0x00FF)); } inline static SInt32 Endian32_Swap(SInt32 inValue) { return (((((UInt32)inValue)<<24) & 0xFF000000) | ((((UInt32)inValue)<< 8) & 0x00FF0000) | ((((UInt32)inValue)>> 8) & 0x0000FF00) | ((((UInt32)inValue)>>24) & 0x000000FF)); } #if defined(__ppc__) static inline SInt16 SInt16BigToNativeEndian(SInt16 inValue) { return inValue; } static inline SInt16 SInt16NativeToBigEndian(SInt16 inValue) { return inValue; } static inline SInt16 SInt16LittleToNativeEndian(SInt16 inValue) { return Endian16_Swap(inValue); } static inline SInt16 SInt16NativeToLittleEndian(SInt16 inValue) { return Endian16_Swap(inValue); } static inline SInt32 SInt32BigToNativeEndian(SInt32 inValue) { return inValue; } static inline SInt32 SInt32NativeToBigEndian(SInt32 inValue) { return inValue; } static inline SInt32 SInt32LittleToNativeEndian(SInt32 inValue) { return Endian32_Swap(inValue); } static inline SInt32 SInt32NativeToLittleEndian(SInt32 inValue) { return Endian32_Swap(inValue); } #elif defined(__i386__) static inline SInt16 SInt16BigToNativeEndian(SInt16 inValue) { return Endian16_Swap(inValue); } static inline SInt16 SInt16NativeToBigEndian(SInt16 inValue) { return Endian16_Swap(inValue); } static inline SInt16 SInt16LittleToNativeEndian(SInt16 inValue) { return inValue; } static inline SInt16 SInt16NativeToLittleEndian(SInt16 inValue) { return inValue; } static inline SInt32 SInt32BigToNativeEndian(SInt32 inValue) { return Endian32_Swap(inValue); } static inline SInt32 SInt32NativeToBigEndian(SInt32 inValue) { return Endian32_Swap(inValue); } static inline SInt32 SInt32LittleToNativeEndian(SInt32 inValue) { return inValue; } static inline SInt32 SInt32NativeToLittleEndian(SInt32 inValue) { return inValue; } #endif inline static Float32 ClipFloat32(Float32 inSample) { if(inSample > 1.0) return 1.0; if(inSample < -1.0) return -1.0; return inSample; } // Float32 -> SInt8 #define kMaxSampleSInt8 ((Float32)0x7F) static void ClipFloat32ToSInt8_4(const Float32* inInputBuffer, SInt8* outOutputBuffer, UInt32 inNumberSamples) { register UInt32 theLeftOvers = inNumberSamples % 4; while(inNumberSamples > theLeftOvers) { register Float32 theFloat32Value1 = *(inInputBuffer + 0); register Float32 theFloat32Value2 = *(inInputBuffer + 1); register Float32 theFloat32Value3 = *(inInputBuffer + 2); register Float32 theFloat32Value4 = *(inInputBuffer + 3); inInputBuffer += 4; theFloat32Value1 = ClipFloat32(theFloat32Value1); theFloat32Value2 = ClipFloat32(theFloat32Value2); theFloat32Value3 = ClipFloat32(theFloat32Value3); theFloat32Value4 = ClipFloat32(theFloat32Value4); *(outOutputBuffer + 0) = (SInt8)(theFloat32Value1 * kMaxSampleSInt8); *(outOutputBuffer + 1) = (SInt8)(theFloat32Value2 * kMaxSampleSInt8); *(outOutputBuffer + 2) = (SInt8)(theFloat32Value3 * kMaxSampleSInt8); *(outOutputBuffer + 3) = (SInt8)(theFloat32Value4 * kMaxSampleSInt8); outOutputBuffer += 4; inNumberSamples -= 4; } while(inNumberSamples > 0) { register Float32 theFloat32Value = *inInputBuffer; ++inInputBuffer; theFloat32Value = ClipFloat32(theFloat32Value); *outOutputBuffer = (SInt8)(theFloat32Value * kMaxSampleSInt8); ++outOutputBuffer; --inNumberSamples; } } // Float32 -> SInt16 #define kMaxSampleSInt16 ((Float32)0x7FFF) static void ClipFloat32ToSInt16LE_4(const Float32* inInputBuffer, SInt16* outOutputBuffer, UInt32 inNumberSamples) { register UInt32 theLeftOvers = inNumberSamples % 4; while(inNumberSamples > theLeftOvers) { register Float32 theFloat32Value1 = *(inInputBuffer + 0); register Float32 theFloat32Value2 = *(inInputBuffer + 1); register Float32 theFloat32Value3 = *(inInputBuffer + 2); register Float32 theFloat32Value4 = *(inInputBuffer + 3); inInputBuffer += 4; theFloat32Value1 = ClipFloat32(theFloat32Value1); theFloat32Value2 = ClipFloat32(theFloat32Value2); theFloat32Value3 = ClipFloat32(theFloat32Value3); theFloat32Value4 = ClipFloat32(theFloat32Value4); *(outOutputBuffer + 0) = SInt16NativeToLittleEndian((SInt16)(theFloat32Value1 * kMaxSampleSInt16)); *(outOutputBuffer + 1) = SInt16NativeToLittleEndian((SInt16)(theFloat32Value2 * kMaxSampleSInt16)); *(outOutputBuffer + 2) = SInt16NativeToLittleEndian((SInt16)(theFloat32Value3 * kMaxSampleSInt16)); *(outOutputBuffer + 3) = SInt16NativeToLittleEndian((SInt16)(theFloat32Value4 * kMaxSampleSInt16)); outOutputBuffer += 4; inNumberSamples -= 4; } while(inNumberSamples > 0) { register Float32 theFloat32Value = *inInputBuffer; ++inInputBuffer; theFloat32Value = ClipFloat32(theFloat32Value); *outOutputBuffer = SInt16NativeToLittleEndian((SInt16)(theFloat32Value * kMaxSampleSInt16)); ++outOutputBuffer; --inNumberSamples; } } // Float32 -> SInt24 //#define kMaxSampleSInt24 ((Float32)0x007FFFFF) #define kMaxSampleSInt24 ((Float32)0x7FFFFFFF) // we use the MaxSInt32 value because of how we munge the data static void ClipFloat32ToSInt24LE_4(const Float32* inInputBuffer, SInt32* outOutputBuffer, UInt32 inNumberSamples) { register UInt32 theLeftOvers = inNumberSamples % 4; while(inNumberSamples > theLeftOvers) { register Float32 theFloat32Value1 = *(inInputBuffer + 0); register Float32 theFloat32Value2 = *(inInputBuffer + 1); register Float32 theFloat32Value3 = *(inInputBuffer + 2); register Float32 theFloat32Value4 = *(inInputBuffer + 3); inInputBuffer += 4; theFloat32Value1 = ClipFloat32(theFloat32Value1); theFloat32Value2 = ClipFloat32(theFloat32Value2); theFloat32Value3 = ClipFloat32(theFloat32Value3); theFloat32Value4 = ClipFloat32(theFloat32Value4); #if defined(__ppc__) register SInt32 theSInt32Value1 = (SInt32)(theFloat32Value1 * kMaxSampleSInt24); register SInt32 theSInt32Value2 = (SInt32)(theFloat32Value2 * kMaxSampleSInt24); register SInt32 theSInt32Value3 = (SInt32)(theFloat32Value3 * kMaxSampleSInt24); register SInt32 theSInt32Value4 = (SInt32)(theFloat32Value4 * kMaxSampleSInt24); // each sample in the 4 registers looks like this: 123G, where G // is the unused byte we need to munge all four so that they look // like this in three registers: 3213 2132 1321. We want to avoid // any non-aligned memory writes if at all possible. register SInt32 theOutputValue1 = ((((UInt32)theSInt32Value1) << 16) & 0xFF000000) | (((UInt32)theSInt32Value1) & 0x00FF0000) | ((((UInt32)theSInt32Value1) >> 16) & 0x0000FF00) | ((((UInt32)theSInt32Value2) >> 8) & 0x000000FF); register SInt32 theOutputValue2 = ((((UInt32)theSInt32Value2) << 8) & 0xFF000000) | ((((UInt32)theSInt32Value2) >> 8) & 0x00FF0000) | (((UInt32)theSInt32Value3) & 0x0000FF00) | ((((UInt32)theSInt32Value3) >> 16) & 0x000000FF); register SInt32 theOutputValue3 = (((UInt32)theSInt32Value3) & 0xFF000000) | ((((UInt32)theSInt32Value4) << 8) & 0x00FF0000) | ((((UInt32)theSInt32Value4) >> 8) & 0x0000FF00) | ((((UInt32)theSInt32Value4) >> 24) & 0x000000FF); // store everything back to memory *(outOutputBuffer + 0) = theOutputValue1; *(outOutputBuffer + 1) = theOutputValue2; *(outOutputBuffer + 2) = theOutputValue3; #elif defined(__i386__) #error this needs to be done #endif outOutputBuffer += 3; inNumberSamples -= 4; } SInt8* theOutputBuffer = (SInt8*)outOutputBuffer; while(inNumberSamples > 0) { register Float32 theFloat32Value = *inInputBuffer; ++inInputBuffer; theFloat32Value = ClipFloat32(theFloat32Value); #if defined(__ppc__) register SInt32 theSInt32Value = (SInt32)(theFloat32Value * kMaxSampleSInt24); // we have 123G. we want 321 *(theOutputBuffer + 0) = (SInt8)((((UInt32)theSInt32Value) >> 8) & 0x000000FF); *(theOutputBuffer + 1) = (SInt8)((((UInt32)theSInt32Value) >> 16) & 0x000000FF); *(theOutputBuffer + 2) = (SInt8)((((UInt32)theSInt32Value) >> 24) & 0x000000FF); theOutputBuffer += 3; #elif defined(__i386__) #error this needs to be done #endif --inNumberSamples; } } // Float32 -> SInt32 #define kMaxSampleSInt32 ((Float32)0x7FFFFFFF) static void ClipFloat32ToSInt32LE_4(const Float32* inInputBuffer, SInt32* outOutputBuffer, UInt32 inNumberSamples) { register UInt32 theLeftOvers = inNumberSamples % 4; while(inNumberSamples > theLeftOvers) { register Float32 theFloat32Value1 = *(inInputBuffer + 0); register Float32 theFloat32Value2 = *(inInputBuffer + 1); register Float32 theFloat32Value3 = *(inInputBuffer + 2); register Float32 theFloat32Value4 = *(inInputBuffer + 3); inInputBuffer += 4; theFloat32Value1 = ClipFloat32(theFloat32Value1); theFloat32Value2 = ClipFloat32(theFloat32Value2); theFloat32Value3 = ClipFloat32(theFloat32Value3); theFloat32Value4 = ClipFloat32(theFloat32Value4); *(outOutputBuffer + 0) = SInt32NativeToLittleEndian((SInt32)(theFloat32Value1 * kMaxSampleSInt32)); *(outOutputBuffer + 1) = SInt32NativeToLittleEndian((SInt32)(theFloat32Value2 * kMaxSampleSInt32)); *(outOutputBuffer + 2) = SInt32NativeToLittleEndian((SInt32)(theFloat32Value3 * kMaxSampleSInt32)); *(outOutputBuffer + 3) = SInt32NativeToLittleEndian((SInt32)(theFloat32Value4 * kMaxSampleSInt32)); outOutputBuffer += 4; inNumberSamples -= 4; } while(inNumberSamples > 0) { register Float32 theFloat32Value = *inInputBuffer; ++inInputBuffer; theFloat32Value = ClipFloat32(theFloat32Value); *outOutputBuffer = SInt32NativeToLittleEndian((SInt32)(theFloat32Value * kMaxSampleSInt32)); ++outOutputBuffer; --inNumberSamples; } } IOReturn clipAppleUSBAudioToOutputStream(const void* mixBuf, void* sampleBuf, UInt32 firstSampleFrame, UInt32 numSampleFrames, const IOAudioStreamFormat *streamFormat) { if(!streamFormat) { return kIOReturnBadArgument; } UInt32 theNumberSamples = numSampleFrames * streamFormat->fNumChannels; UInt32 theFirstSample = firstSampleFrame * streamFormat->fNumChannels; Float32* theMixBuffer = ((Float32*)mixBuf) + theFirstSample; switch(streamFormat->fBitWidth) { case 8: { SInt8* theOutputBufferSInt8 = ((SInt8*)sampleBuf) + theFirstSample; ClipFloat32ToSInt8_4(theMixBuffer, theOutputBufferSInt8, theNumberSamples); } break; case 16: { SInt16* theOutputBufferSInt16 = ((SInt16*)sampleBuf) + theFirstSample; ClipFloat32ToSInt16LE_4(theMixBuffer, theOutputBufferSInt16, theNumberSamples); } break; case 20: case 24: { SInt32* theOutputBufferSInt24 = (SInt32*)(((UInt8*)sampleBuf) + (theFirstSample * 3)); ClipFloat32ToSInt24LE_4(theMixBuffer, theOutputBufferSInt24, theNumberSamples); } break; case 32: { SInt32* theOutputBufferSInt32 = ((SInt32*)sampleBuf) + theFirstSample; ClipFloat32ToSInt32LE_4(theMixBuffer, theOutputBufferSInt32, theNumberSamples); } break; }; return kIOReturnSuccess; } IOReturn clipAppleUSBAudioToOutputStream_old (const void *mixBuf, void *sampleBuf, UInt32 firstSampleFrame, UInt32 numSampleFrames, const IOAudioStreamFormat *streamFormat) { UInt32 sampleIndex, maxSampleIndex; float * floatMixBuf; SInt8 * outputBuf8; SInt16 * outputBuf16; SInt32 * outputBuf24; SInt32 * outputBuf32; SInt32 sample32[1]; #ifdef DEBUGLOG UInt32 count; UInt32 nextStart; #endif if (!streamFormat) { return kIOReturnBadArgument; } // debug6IOLog("clipAppleUSBAudioToOutputStream(%p, %p, 0x%lx, 0x%lx, %p)\n", mixBuf, sampleBuf, firstSampleFrame, numSampleFrames, streamFormat); floatMixBuf = (float *)mixBuf; outputBuf8 = (SInt8 *)sampleBuf; outputBuf16 = (SInt16 *)sampleBuf; outputBuf24 = (SInt32 *)sampleBuf; outputBuf32 = (SInt32 *)sampleBuf; maxSampleIndex = (firstSampleFrame + numSampleFrames) * (streamFormat->fNumChannels); #ifdef DEBUGLOG count = 0; nextStart = firstSampleFrame * streamFormat->fNumChannels; #endif for (sampleIndex = (firstSampleFrame * streamFormat->fNumChannels); sampleIndex < maxSampleIndex; sampleIndex++) { float inSample; inSample = floatMixBuf[sampleIndex]; if (inSample > 1.0) { inSample = 1.0; } else if (inSample < -1.0) { inSample = -1.0; } if (streamFormat->fBitWidth == 8) { if (inSample >= 0) { outputBuf8[sampleIndex] = (SInt8)(inSample * 127.0); } else { outputBuf8[sampleIndex] = (SInt8)(inSample * 127.0); } } else if (streamFormat->fBitWidth == 16) { if (inSample >= 0) { outputBuf16[sampleIndex] = HostToUSBWord((SInt16)(inSample * 32767.0)); #if 0 #ifdef DEBUGLOG if (inSample == 0 && sampleIndex >= nextStart) { count = 1; while (floatMixBuf[sampleIndex + count] == 0.0 && (sampleIndex + count) < maxSampleIndex) { count++; } if (count >= 3) { debug5IOLog ("got %ld samples of silence back to back, sampleIndex = 0x%lX, firstSampleFrame = 0x%lX, numSampleFrames = 0x%lX\n", count, sampleIndex, firstSampleFrame, numSampleFrames); nextStart = sampleIndex + count; } } #endif #endif } else { outputBuf16[sampleIndex] = HostToUSBWord((SInt16)(inSample * 32767.0)); } } else if (streamFormat->fBitWidth == 20) { if (inSample >= 0) { sample32[0] = (SInt32)(inSample * 524287.0); ((UInt8 *)outputBuf24)[sampleIndex * 3 + 2] = ((UInt8 *)sample32)[1]; ((UInt8 *)outputBuf24)[sampleIndex * 3 + 1] = ((UInt8 *)sample32)[2]; ((UInt8 *)outputBuf24)[sampleIndex * 3] = ((UInt8 *)sample32)[3]; } else { sample32[0] = (SInt32)(inSample * 524287.0); ((UInt8 *)outputBuf24)[sampleIndex * 3 + 2] = ((UInt8 *)sample32)[1]; ((UInt8 *)outputBuf24)[sampleIndex * 3 + 1] = ((UInt8 *)sample32)[2]; ((UInt8 *)outputBuf24)[sampleIndex * 3] = ((UInt8 *)sample32)[3]; } } else if (streamFormat->fBitWidth == 24) { if (inSample >= 0) { sample32[0] = (SInt32)(inSample * 8388607.0); ((UInt8 *)outputBuf24)[sampleIndex * 3 + 2] = ((UInt8 *)sample32)[1]; ((UInt8 *)outputBuf24)[sampleIndex * 3 + 1] = ((UInt8 *)sample32)[2]; ((UInt8 *)outputBuf24)[sampleIndex * 3] = ((UInt8 *)sample32)[3]; } else { sample32[0] = (SInt32)(inSample * 8388607.0); ((UInt8 *)outputBuf24)[sampleIndex * 3 + 2] = ((UInt8 *)sample32)[1]; ((UInt8 *)outputBuf24)[sampleIndex * 3 + 1] = ((UInt8 *)sample32)[2]; ((UInt8 *)outputBuf24)[sampleIndex * 3] = ((UInt8 *)sample32)[3]; } } else if (streamFormat->fBitWidth == 32) { if (inSample >= 0) { sample32[0] = (SInt32)(inSample * 2147483647.0); ((UInt8 *)outputBuf32)[sampleIndex * 3 + 3] = ((UInt8 *)sample32)[0]; ((UInt8 *)outputBuf32)[sampleIndex * 3 + 2] = ((UInt8 *)sample32)[1]; ((UInt8 *)outputBuf32)[sampleIndex * 3 + 1] = ((UInt8 *)sample32)[2]; ((UInt8 *)outputBuf32)[sampleIndex * 3] = ((UInt8 *)sample32)[3]; } else { sample32[0] = (SInt32)(inSample * 2147483647.0); ((UInt8 *)outputBuf32)[sampleIndex * 3 + 3] = ((UInt8 *)sample32)[0]; ((UInt8 *)outputBuf32)[sampleIndex * 3 + 2] = ((UInt8 *)sample32)[1]; ((UInt8 *)outputBuf32)[sampleIndex * 3 + 1] = ((UInt8 *)sample32)[2]; ((UInt8 *)outputBuf32)[sampleIndex * 3] = ((UInt8 *)sample32)[3]; } } } return kIOReturnSuccess; } // aml 2.21.02 added second filter state for 4th order filter // aml 2.21.02 added more filter state for phase compensator // aml 3.4.02 added srcPhase IOReturn clipAppleUSBAudioToOutputStreamiSub (const void *mixBuf, void *sampleBuf, PreviousValues * filterState, PreviousValues * filterState2, PreviousValues * phaseCompState, Float32 *low, Float32 *high, UInt32 firstSampleFrame, UInt32 numSampleFrames, UInt32 sampleRate, const IOAudioStreamFormat *streamFormat, SInt16 *iSubBufferMemory, UInt32 *loopCount, SInt32 *iSubBufferOffset, UInt32 iSubBufferLen, iSubAudioFormatType* iSubFormat, float* srcPhase, float* srcState) { UInt32 sampleIndex, maxSampleIndex; float *floatMixBuf; SInt16 *outputBuf; float highSample; Float32 iSubSampleFloat; SInt16 iSubSampleInt; // aml 3.6.02 storage for src float x0, x1, temp; // aml 3.6.02 src variables - should calculate phaseInc somewhere else, change only if the SR changes float phaseInc = ((float)sampleRate)/((float)(iSubFormat->outputSampleRate)); // phase increment = Fs_in/Fs_out float phase = *srcPhase; // current phase location floatMixBuf = (float *)mixBuf; outputBuf = (SInt16 *)sampleBuf; maxSampleIndex = (firstSampleFrame + numSampleFrames) * (streamFormat->fNumChannels); // Filter out the highs and lows for use with the iSub if (1 == streamFormat->fNumChannels) { MonoFilter (&floatMixBuf[firstSampleFrame * streamFormat->fNumChannels], &low[firstSampleFrame * streamFormat->fNumChannels], &high[firstSampleFrame * streamFormat->fNumChannels], numSampleFrames, sampleRate); } else if (2 == streamFormat->fNumChannels) { // aml 2.21.02 changed to 4th order version // aml 2.21.02 changed to 4th order version with phase compensation StereoFilter4thOrderPhaseComp (&floatMixBuf[firstSampleFrame * streamFormat->fNumChannels], &low[firstSampleFrame * streamFormat->fNumChannels], &high[firstSampleFrame * streamFormat->fNumChannels], numSampleFrames, sampleRate, filterState, filterState2, phaseCompState); } for (sampleIndex = (firstSampleFrame * streamFormat->fNumChannels); sampleIndex < maxSampleIndex; sampleIndex++) { highSample = high[sampleIndex]; if (highSample > 1.0) { highSample = 1.0; } else if (highSample < -1.0) { highSample = -1.0; } outputBuf[sampleIndex] = HostToUSBWord ((SInt16)(highSample * 32767.0)); } // aml 3.01.02 adding num channels check if ((iSubFormat->numChannels == 1) && (streamFormat->fNumChannels == 2)) { // aml 3.6.02 linear interpolation src (takes the edge of the zoh version, without wasting too many // cycles since we have a 4th order lp in front of us, down -90 dB at Nyquist for 6kHz sample rate) // STEREO->MONO sampleIndex = (firstSampleFrame * streamFormat->fNumChannels); while (sampleIndex < maxSampleIndex) { if (phase >= 1.0) { phase -= 1.0; sampleIndex+=2; } else { // check for beginning of frame case, use saved last sample if needed if (sampleIndex == (firstSampleFrame * streamFormat->fNumChannels)) { x0 = *srcState; } else { // mix x[n-1] to mono x0 = low[sampleIndex-2]; temp = low[sampleIndex-1]; x0 = 0.5*(x0 + temp); } // mix x[n] to mono x1 = low[sampleIndex]; temp = low[sampleIndex+1]; x1 = 0.5*(x1 + temp); // linearly interpolate between x0 and x1 iSubSampleFloat = x0 + phase*(x1 - x0); // clip if (iSubSampleFloat > 1.0) { iSubSampleFloat = 1.0; } else if (iSubSampleFloat < -1.0) { iSubSampleFloat = -1.0; } // convert to fixed if (iSubSampleFloat >= 0) { iSubSampleInt = (SInt16) (iSubSampleFloat * 32767.0); } else { iSubSampleInt = (SInt16) (iSubSampleFloat * 32768.0); } // check for end of buffer condition if (*iSubBufferOffset >= (SInt32)iSubBufferLen) { *iSubBufferOffset = 0; (*loopCount)++; } // byteswap to USB format and copy to iSub buffer iSubBufferMemory[(*iSubBufferOffset)++] = ((((UInt16)iSubSampleInt) << 8) & 0xFF00) | ((((UInt16)iSubSampleInt) >> 8) & 0x00FF); // increment phase and update input buffer pointer phase += phaseInc; } } if (phase < 1) { // mix and save last sample in buffer to mono if it will be needed for the next loop x1 = low[maxSampleIndex-2]; temp = low[maxSampleIndex-1]; *srcState = 0.5*(x1 + temp); } else { *srcState = 0; } // cache current phase for use next time we enter the clip loop *srcPhase = phase; } else { // STEREO->STEREO, MONO->MONO for (sampleIndex = (firstSampleFrame * streamFormat->fNumChannels); sampleIndex < maxSampleIndex; sampleIndex++) { iSubSampleFloat = low[sampleIndex]; if (iSubSampleFloat > 1.0) { iSubSampleFloat = 1.0; } else if (iSubSampleFloat < -1.0) { iSubSampleFloat = -1.0; } iSubSampleInt = (SInt16) (iSubSampleFloat * 32767.0); if (*iSubBufferOffset >= (SInt32)iSubBufferLen) { *iSubBufferOffset = 0; (*loopCount)++; } iSubBufferMemory[(*iSubBufferOffset)++] = ((((UInt16)iSubSampleInt) << 8) & 0xFF00) | ((((UInt16)iSubSampleInt) >> 8) & 0x00FF); } } return kIOReturnSuccess; } IOReturn convertFromAppleUSBAudioInputStream_NoWrap (const void *sampleBuf, void *destBuf, UInt32 firstSampleFrame, UInt32 numSampleFrames, const IOAudioStreamFormat *streamFormat) { UInt32 numSamplesLeft; float * floatDestBuf; SInt8 * inputBuf8; SInt8 * inputBuf24; SInt16 * inputBuf16; SInt32 * inputBuf32; floatDestBuf = (float *)destBuf; // debug4IOLog ("destBuf = %p, firstSampleFrame = %ld, numSampleFrames = %ld\n", destBuf, firstSampleFrame, numSampleFrames); switch (streamFormat->fBitWidth) { case 8: inputBuf8 = &(((SInt8 *)sampleBuf)[firstSampleFrame * streamFormat->fNumChannels]); numSamplesLeft = numSampleFrames * streamFormat->fNumChannels; while (numSamplesLeft > 0) { if (*inputBuf8 >= 0) { *floatDestBuf = *inputBuf8 / 127.0; } else { *floatDestBuf = *inputBuf8 / 128.0; } ++inputBuf8; ++floatDestBuf; --numSamplesLeft; } break; case 16: inputBuf16 = &(((SInt16 *)sampleBuf)[firstSampleFrame * streamFormat->fNumChannels]); numSamplesLeft = numSampleFrames * streamFormat->fNumChannels; while (numSamplesLeft > 0) { SInt16 inputSample; inputSample = USBToHostWord (*inputBuf16); if (inputSample >= 0) { *floatDestBuf = inputSample / 32767.0; } else { *floatDestBuf = inputSample / 32768.0; } ++inputBuf16; ++floatDestBuf; --numSamplesLeft; } break; case 20: case 24: // Multiply by 3 because 20 and 24 bit samples are packed into only three bytes, so we have to index bytes, not shorts or longs inputBuf24 = &(((SInt8 *)sampleBuf)[firstSampleFrame * streamFormat->fNumChannels * 3]); numSamplesLeft = numSampleFrames * streamFormat->fNumChannels; while (numSamplesLeft > 0) { SInt32 inputSample; inputSample = *inputBuf24++ & 0x000000FF; inputSample |= (*inputBuf24++ << 8) & 0x0000FF00; inputSample |= (*inputBuf24++ << 16) & 0x00FF0000; if (inputSample & 0x00800000) { inputSample |= 0xFF000000; // have to sign extend because it won't happen automatically } if (inputSample >= 0) { *floatDestBuf = inputSample / 8388607.0; } else { *floatDestBuf = inputSample / 8388608.0; } ++floatDestBuf; --numSamplesLeft; } break; case 32: inputBuf32 = &(((SInt32 *)sampleBuf)[firstSampleFrame * streamFormat->fNumChannels]); numSamplesLeft = numSampleFrames * streamFormat->fNumChannels; while (numSamplesLeft > 0) { SInt32 inputSample; inputSample = OSSwapLittleToHostInt32 (*inputBuf32); if (inputSample >= 0) { *floatDestBuf = inputSample / 2147483647.0; } else { *floatDestBuf = inputSample / 2147483648.0; } ++inputBuf32; ++floatDestBuf; --numSamplesLeft; } break; } return kIOReturnSuccess; } #if FLOATLIB /* ***CoeffsFilterOrder2*** This function fills in the order2 filter coefficients table used in the MonoFilter & StereoFilter functions Float32 *Coeff : is a pointer to the coefficients table Float32 CutOffFreq : is the cut off frequency of the filter (in Hertz) Float32 AttAtCutOffFreq: is the attenuation at the cut off frequency (in linear) Float64 SamplingRate : is the sampling rate frequency (in Hertz) */ void CoeffsFilterOrder2 (Float32 *Coeff, Float32 CutOffFreq, Float32 AttAtCutOffFreq , Float64 SamplingRate) { Float32 k, nu0, pi=3.14159, Att, norm; nu0 = (Float32) (CutOffFreq / SamplingRate); Att = 1 / AttAtCutOffFreq; k = 1/(tan(pi*nu0)); norm = k*(k+Att)+1; /* the first 3 coefficients are Num[0], Num[1] & Num[2] in that order the last 2 coeffients are Den[1] & Den[2] where [.] is the z exposant */ Coeff[0] = 1.0 / norm; Coeff[1] = 2.0 / norm; Coeff[2] = 1.0 / norm; Coeff[3] = 2*(1-k*k) / norm; Coeff[4] = (k*(k-Att)+1) / norm; return; } #else /* ***CoeffsFilterOrder2Table*** This function choose an order2 filter coefficients table used in the MonoFilter & StereoFilter functions The coefficients table depend on the sampling rate Float32 *Coeff : is a pointer on the coefficients table Float64 SamplingRate : is the sampling rate frequency (in Hertz) return: - FALSE if the sampling rate frequency doesn't exist - TRUE otherwise... */ Boolean CoeffsFilterOrder2Table (Float32 *Coeff, UInt32 samplingRate) { Boolean success = TRUE; switch ( samplingRate ) { case 8000: Coeff[0] = 0.00208054389804601669; Coeff[1] = 0.00416108779609203339; Coeff[2] = 0.00208054389804601669; Coeff[3] = -1.86687481403350830078; Coeff[4] = 0.87519699335098266602; break; case 11025: Coeff[0] = 0.00111490569543093443; Coeff[1] = 0.00222981139086186886; Coeff[2] = 0.00111490569543093443; Coeff[3] = -1.90334117412567138672; Coeff[4] = 0.90780085325241088867; break; case 22050: Coeff[0] = 0.00028538206242956221; Coeff[1] = 0.00057076412485912442; Coeff[2] = 0.00028538206242956221; Coeff[3] = -1.95164430141448974609; Coeff[4] = 0.95278578996658325195; break; case 44100: Coeff[0] = 0.00007220284896902740; Coeff[1] = 0.00014440569793805480; Coeff[2] = 0.00007220284896902740; Coeff[3] = -1.97581851482391357422; Coeff[4] = 0.97610741853713989258; break; case 48000: Coeff[0] = 0.00006100598693592474; Coeff[1] = 0.00012201197387184948; Coeff[2] = 0.00006100598693592474; Coeff[3] = -1.97778332233428955078; Coeff[4] = 0.97802722454071044922; break; case 96000: Coeff[0] = 0.00001533597242087126; Coeff[1] = 0.00003067194484174252; Coeff[2] = 0.00001533597242087126; Coeff[3] = -1.98889136314392089844; Coeff[4] = 0.98895263671875000000; break; default: // IOLog("\nNot a registered frequency...\n"); success = FALSE; break; } return(success); } #endif // aml 2.18.02 adding fourth order coefficient setting functions Boolean Set4thOrderCoefficients (Float32 *b0, Float32 *b1, Float32 *b2, Float32 *a1, Float32 *a2, UInt32 samplingRate) { Boolean success = TRUE; switch ( samplingRate ) { case 8000: *b0 = 0.00782020803350; *b1 = 0.01564041606699; *b2 = 0.00782020803350; *a1 = -1.73472576880928; *a2 = 0.76600660094326; break; case 11025: *b0 = 0.00425905333005; *b1 = 0.00851810666010; *b2 = 0.00425905333005; *a1 = -1.80709136077571; *a2 = 0.82412757409590; break; case 22050: *b0 = 0.00111491512001; *b1 = 0.00222983024003; *b2 = 0.00111491512001; *a1 = -1.90335434048751; *a2 = 0.90781400096756; break; case 44100: *b0 = 0.00028538351548666; *b1 = 0.00057076703097332; *b2 = 0.00028538351548666; *a1 = -1.95165117996464; *a2 = 0.95279271402659; break; case 48000: *b0 = 0.00024135904904198; *b1 = 0.00048271809808396; *b2 = 0.00024135904904198; *a1 = -1.95557824031504; *a2 = 0.95654367651120; break; case 96000: *b0 = 0.00006100617875806425; *b1 = 0.0001220123575161285; *b2 = 0.00006100617875806425; *a1 = -1.977786483776763; *a2 = 0.9780305084917958; break; default: // IOLog("\nNot a registered frequency...\n"); success = FALSE; break; } return(success); } // aml 2.18.02 adding 4th order phase compensator coefficient setting function // this function sets the parameters of a second order all-pass filter that is used to compensate for the phase // shift of the 4th order lowpass IIR filter used in the iSub crossover. Note that a0 and b2 are both 1.0. Boolean Set4thOrderPhaseCompCoefficients (Float32 *b0, Float32 *b1, Float32 *a1, Float32 *a2, UInt32 samplingRate) { Boolean success = TRUE; switch ( samplingRate ) { case 8000: *a1 = -1.734725768809275; *a2 = 0.7660066009432638; *b0 = *a2; *b1 = *a1; break; case 11025: *a1 = -1.807091360775707; *a2 = 0.8241275740958973; *b0 = *a2; *b1 = *a1; break; case 22050: *a1 = -1.903354340487510; *a2 = 0.9078140009675627; *b0 = *a2; *b1 = *a1; break; case 44100: *a1 = -1.951651179964643; *a2 = 0.9527927140265903; *b0 = *a2; *b1 = *a1; break; case 48000: *a1 = -1.955578240315035; *a2 = 0.9565436765112033; *b0 = *a2; *b1 = *a1; break; case 96000: *a1 = -1.977786483776763; *a2 = 0.9780305084917958; *b0 = *a2; *b1 = *a1; break; default: // IOLog("\nNot a registered frequency...\n"); success = FALSE; break; } return(success); } // aml 2.18.02 adding 2nd order phase compensator coefficient setting function // this function sets the parameters of a first order all-pass filter that is used to compensate for the phase // shift when using a 2nd order lowpass IIR filter for the iSub crossover. Note that a0 and b1 are both 1.0. Boolean Set2ndOrderPhaseCompCoefficients (float *b0, float *a1, UInt32 samplingRate) { Boolean success = TRUE; switch ( samplingRate ) { case 8000: *a1 = -0.7324848836653277; *b0 = *a1; break; case 11025: *a1 = -0.7985051758519318; *b0 = *a1; break; case 22050: *a1 = -0.8939157008398341; *b0 = *a1; break; case 44100: *a1 = -0.9455137594199962; *b0 = *a1; break; case 48000: *a1 = -0.9498297607998617; *b0 = *a1; break; case 96000: *a1 = -0.9745963490718829; *b0 = *a1; break; default: // IOLog("\nNot a registered frequency...\n"); success = FALSE; break; } return(success); } /* ***MonoFilter*** Mono Order2 Filter Float32 *in : is a pointer to the entry array -> signal to filter... Float32 *low : is a pointer to the low-pass filtered signal Float32 *high : is a pointer to the high-pass filtered signal UInt32 samples: is the number of samples in each array Float64 SamplingRate : is the sampling rate frequency (in Hertz) At the n instant: x is x[n], xx is x[n-1], xxx is x[n-2] (it's the same for y) */ void MonoFilter (Float32 *in, Float32 *low, Float32 *high, UInt32 frames, UInt32 samplingRate) { UInt32 idx; #if !FLOATLIB Boolean success; #endif Float32 LP_Coeff[5]; Float32 x, xx, xxx, y, yy, yyy; // init #if FLOATLIB CoeffsFilterOrder2 (LP_Coeff, 120, 1/sqrt(2), 44100); #else success = CoeffsFilterOrder2Table (LP_Coeff, samplingRate); if (success == FALSE) goto End; #endif x=xx=xxx=y=yy=yyy=0; // convolution for ( idx = 0 ; idx < frames ; idx++ ) { x = in[idx]; // Low-pass filter y = (LP_Coeff[0]*x + LP_Coeff[1]*xx + LP_Coeff[2]*xxx - LP_Coeff[3]*yy - LP_Coeff[4]*yyy); // Update xxx = xx; xx = x; yyy = yy; yy = y; // Storage low[idx] = y; high[idx] = x-y; } #if !FLOATLIB End: #endif return; } /* ***StereoFilter*** Stereo Order2 Filter Float32 *in : is a pointer on the entry array -> signal to filter... Float32 *low : is a pointer on the low-pass filtered signal Float32 *high : is a pointer on the high-pass filtered signal UInt32 samples : is the number of samples in each array Float64 SamplingRate : is the sampling rate frequency (in Hertz) At the n instant: x is x[n], x_1 is x[n-1], x_2 is x[n-2] (it's the same for y) */ void StereoFilter (Float32 *in, Float32 *low, Float32 *high, UInt32 frames, UInt32 SamplingRate, PreviousValues *theValue) { UInt32 idx; Boolean success; Float32 LP_Coeff[5]; Float32 xl, xr, yl, yr; // Get the filter coefficents //CoeffsFilterOrder2 (&LP_Coeff, 120, 0.707, SamplingRate); //don't used because of the tan() function success = CoeffsFilterOrder2Table (LP_Coeff, SamplingRate); if (success == FALSE) goto End; // convolution for ( idx = 0 ; idx < frames ; idx ++ ) { xl = in[2*idx]; xr = in[2*idx+1]; // Low-pass filter yl = (LP_Coeff[0]*xl + LP_Coeff[1]*theValue->xl_1 + LP_Coeff[2]*theValue->xl_2 - LP_Coeff[3]*theValue->yl_1 - LP_Coeff[4]*theValue->yl_2); yr = (LP_Coeff[0]*xr + LP_Coeff[1]*theValue->xr_1 + LP_Coeff[2]*theValue->xr_2 - LP_Coeff[3]*theValue->yr_1 - LP_Coeff[4]*theValue->yr_2); // Update theValue->xl_2 = theValue->xl_1; theValue->xr_2 = theValue->xr_1; theValue->xl_1 = xl; theValue->xr_1 = xr; theValue->yl_2 = theValue->yl_1; theValue->yr_2 = theValue->yr_1; theValue->yl_1 = yl; theValue->yr_1 = yr; // Storage low[2*idx] = yl; low[2*idx+1] = yr; high[2*idx] = xl-yl; high[2*idx+1] = xr-yr; } End: return; } } // aml 2.15.02, stereo filter that runs twice for double the rolloff // tried to make this more efficient and readable that previous filter // ideally all this will move into a class which will keep it's own storage etc. void StereoFilter4thOrder (Float32 *in, Float32 *low, Float32 *high, UInt32 frames, UInt32 SamplingRate, PreviousValues *theValue, PreviousValues *theValue2) { UInt32 i; // Float32 LP_Coeff[5]; Float32 inL, inR, outL1, outR1, outL, outR; Float32 b0, b1, b2, a1, a2; Float32 inLTap1, inLTap2, inRTap1, inRTap2; Float32 outLTap1, outLTap2, outRTap1, outRTap2; Float32 inLTap1_2, inLTap2_2, inRTap1_2, inRTap2_2; Float32 outLTap1_2, outLTap2_2, outRTap1_2, outRTap2_2; // copy to local variables to avoid structure referencing during inner loop inLTap1 = theValue->xl_1; inLTap2 = theValue->xl_2; inRTap1 = theValue->xr_1; inRTap2 = theValue->xr_2; outLTap1 = theValue->yl_1; outLTap2 = theValue->yl_2; outRTap1 = theValue->yr_1; outRTap2 = theValue->yr_2; inLTap1_2 = theValue2->xl_1; inLTap2_2 = theValue2->xl_2; inRTap1_2 = theValue2->xr_1; inRTap2_2 = theValue2->xr_2; outLTap1_2 = theValue2->yl_1; outLTap2_2 = theValue2->yl_2; outRTap1_2 = theValue2->yr_1; outRTap2_2 = theValue2->yr_2; // set all coefficients if (Set4thOrderCoefficients (&b0, &b1, &b2, &a1, &a2, SamplingRate) == FALSE) return; // old way: // success = CoeffsFilterOrder2Table (LP_Coeff, SamplingRate); // if (success == FALSE) goto End; // b0 = LP_Coeff[0]; // b1 = LP_Coeff[1]; // b2 = LP_Coeff[2]; // a1 = LP_Coeff[3]; // a2 = LP_Coeff[4]; for ( i = 0 ; i < frames ; i ++ ) { inL = in[2*i]; inR = in[2*i+1]; // Low-pass filter first pass outL1 = (b0*inL + b1*inLTap1 + b2*inLTap2 - a1*outLTap1 - a2*outLTap2); outR1 = (b0*inR + b1*inRTap1 + b2*inRTap2 - a1*outRTap1 - a2*outRTap2); // update filter taps inLTap2 = inLTap1; inRTap2 = inRTap1; inLTap1 = inL; inRTap1 = inR; outLTap2 = outLTap1; outRTap2 = outRTap1; outLTap1 = outL1; outRTap1 = outR1; // Low-pass filter second pass outL = (b0*outL1 + b1*inLTap1_2 + b2*inLTap2_2 - a1*outLTap1_2 - a2*outLTap2_2); outR = (b0*outR1 + b1*inRTap1_2 + b2*inRTap2_2 - a1*outRTap1_2 - a2*outRTap2_2); // update filter taps inLTap2_2 = inLTap1_2; inRTap2_2 = inRTap1_2; inLTap1_2 = outL1; inRTap1_2 = outR1; outLTap2_2 = outLTap1_2; outRTap2_2 = outRTap1_2; outLTap1_2 = outL; outRTap1_2 = outR; // Storage low[2*i] = outL; low[2*i+1] = outR; high[2*i] = inL-outL; high[2*i+1] = inR-outR; } // update state structures theValue->xl_1 = inLTap1; theValue->xl_2 = inLTap2; theValue->xr_1 = inRTap1; theValue->xr_2 = inRTap2; theValue->yl_1 = outLTap1; theValue->yl_2 = outLTap2; theValue->yr_1 = outRTap1; theValue->yr_2 = outRTap2; theValue2->xl_1 = inLTap1_2; theValue2->xl_2 = inLTap2_2; theValue2->xr_1 = inRTap1_2; theValue2->xr_2 = inRTap2_2; theValue2->yl_1 = outLTap1_2; theValue2->yl_2 = outLTap2_2; theValue2->yr_1 = outRTap1_2; theValue2->yr_2 = outRTap2_2; return; } // aml 2.15.02, stereo filter that runs twice for double the rolloff // tried to make this more efficient and readable that previous filter // ideally all this will move into a class which will keep it's own storage etc. void StereoFilter4thOrderPhaseComp (Float32 *in, Float32 *low, Float32 *high, UInt32 frames, UInt32 SamplingRate, PreviousValues *section1State, PreviousValues *section2State, PreviousValues *phaseCompState) { UInt32 i; Float32 inL, inR, outL1, outR1, outL, outR, inPhaseCompL, inPhaseCompR; // shared coefficients for second order sections Float32 b0, b1, b2, a1, a2; // coefficients for phase compensator Float32 bp0, bp1, ap1, ap2; // taps for second order section 1 Float32 inLTap1, inLTap2, inRTap1, inRTap2; Float32 outLTap1, outLTap2, outRTap1, outRTap2; // taps for second order section 2 Float32 inLTap1_2, inLTap2_2, inRTap1_2, inRTap2_2; Float32 outLTap1_2, outLTap2_2, outRTap1_2, outRTap2_2; // taps for phase compensator Float32 inLTap1_p, inLTap2_p, inRTap1_p, inRTap2_p; Float32 outLTap1_p, outLTap2_p, outRTap1_p, outRTap2_p; // copy to state local variables to avoid structure referencing during inner loop // section 1 inLTap1 = section1State->xl_1; inLTap2 = section1State->xl_2; inRTap1 = section1State->xr_1; inRTap2 = section1State->xr_2; outLTap1 = section1State->yl_1; outLTap2 = section1State->yl_2; outRTap1 = section1State->yr_1; outRTap2 = section1State->yr_2; // section 2 inLTap1_2 = section2State->xl_1; inLTap2_2 = section2State->xl_2; inRTap1_2 = section2State->xr_1; inRTap2_2 = section2State->xr_2; outLTap1_2 = section2State->yl_1; outLTap2_2 = section2State->yl_2; outRTap1_2 = section2State->yr_1; outRTap2_2 = section2State->yr_2; // phase compensator inLTap1_p = phaseCompState->xl_1; inLTap2_p = phaseCompState->xl_2; inRTap1_p = phaseCompState->xr_1; inRTap2_p = phaseCompState->xr_2; outLTap1_p = phaseCompState->yl_1; outLTap2_p = phaseCompState->yl_2; outRTap1_p = phaseCompState->yr_1; outRTap2_p = phaseCompState->yr_2; // set all coefficients if (Set4thOrderCoefficients (&b0, &b1, &b2, &a1, &a2, SamplingRate) == FALSE) return; if (Set4thOrderPhaseCompCoefficients (&bp0, &bp1, &ap1, &ap2, SamplingRate) == FALSE) return; for ( i = 0 ; i < frames ; i ++ ) { inL = in[2*i]; inR = in[2*i+1]; // Low-pass filter first pass outL1 = b0*inL + b1*inLTap1 + b2*inLTap2 - a1*outLTap1 - a2*outLTap2; outR1 = b0*inR + b1*inRTap1 + b2*inRTap2 - a1*outRTap1 - a2*outRTap2; // update section 1 filter taps inLTap2 = inLTap1; inRTap2 = inRTap1; inLTap1 = inL; inRTap1 = inR; outLTap2 = outLTap1; outRTap2 = outRTap1; outLTap1 = outL1; outRTap1 = outR1; // Low-pass filter second pass outL = b0*outL1 + b1*inLTap1_2 + b2*inLTap2_2 - a1*outLTap1_2 - a2*outLTap2_2; outR = b0*outR1 + b1*inRTap1_2 + b2*inRTap2_2 - a1*outRTap1_2 - a2*outRTap2_2; // update section 2 filter taps inLTap2_2 = inLTap1_2; inRTap2_2 = inRTap1_2; inLTap1_2 = outL1; inRTap1_2 = outR1; outLTap2_2 = outLTap1_2; outRTap2_2 = outRTap1_2; outLTap1_2 = outL; outRTap1_2 = outR; // phase compensate the input, note that b2 is 1.0 inPhaseCompL = bp0*inL + bp1*inLTap1_p + inLTap2_p - ap1*outLTap1_p - ap2*outLTap2_p; inPhaseCompR = bp0*inR + bp1*inRTap1_p + inRTap2_p - ap1*outRTap1_p - ap2*outRTap2_p; // update phase compensate filter taps inLTap2_p = inLTap1_p; inRTap2_p = inRTap1_p; inLTap1_p = inL; inRTap1_p = inR; outLTap2_p = outLTap1_p; outRTap2_p = outRTap1_p; outLTap1_p = inPhaseCompL; outRTap1_p = inPhaseCompR; // Storage low[2*i] = outL; low[2*i+1] = outR; high[2*i] = inPhaseCompL-outL; high[2*i+1] = inPhaseCompR-outR; } // update state structures // section 1 state section1State->xl_1 = inLTap1; section1State->xl_2 = inLTap2; section1State->xr_1 = inRTap1; section1State->xr_2 = inRTap2; section1State->yl_1 = outLTap1; section1State->yl_2 = outLTap2; section1State->yr_1 = outRTap1; section1State->yr_2 = outRTap2; // section 2 state section2State->xl_1 = inLTap1_2; section2State->xl_2 = inLTap2_2; section2State->xr_1 = inRTap1_2; section2State->xr_2 = inRTap2_2; section2State->yl_1 = outLTap1_2; section2State->yl_2 = outLTap2_2; section2State->yr_1 = outRTap1_2; section2State->yr_2 = outRTap2_2; // phase compensator state phaseCompState->xl_1 = inLTap1_p; phaseCompState->xl_2 = inLTap2_p; phaseCompState->xr_1 = inRTap1_p; phaseCompState->xr_2 = inRTap2_p; phaseCompState->yl_1 = outLTap1_p; phaseCompState->yl_2 = outLTap2_p; phaseCompState->yr_1 = outRTap1_p; phaseCompState->yr_2 = outRTap2_p; return; }