#include <libkern/OSTypes.h>
#include <IOKit/IOReturn.h>
#include <IOKit/IOLib.h>

class IOMemoryDescriptor;
#include <IOKit/usb/USB.h>

#include <IOKit/audio/IOAudioTypes.h>
 
#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;
}

