visualboyadvance-m/snd_interp.cpp

#include <math.h>

#define WIN32_LEAN_AND_MEAN
#include <windows.h>

#include "../../libresample-0.1.3/include/libresample.h"

//#define LIBSAMPLERATE // buggy
#ifdef LIBSAMPLERATE
#include "../../libsamplerate-0.1.2/src/samplerate.h"
#endif

#include "snd_interp.h"

// this was once borrowed from libmodplug, and was also used to generate the FIR coefficient
// tables that ZSNES uses for its "FIR" interpolation mode

/* 
  ------------------------------------------------------------------------------------------------
   fir interpolation doc,
	(derived from "an engineer's guide to fir digital filters", n.j. loy)

	calculate coefficients for ideal lowpass filter (with cutoff = fc in 0..1 (mapped to 0..nyquist))
	  c[-N..N] = (i==0) ? fc : sin(fc*pi*i)/(pi*i)

	then apply selected window to coefficients
	  c[-N..N] *= w(0..N)
	with n in 2*N and w(n) being a window function (see loy)

	then calculate gain and scale filter coefs to have unity gain.
  ------------------------------------------------------------------------------------------------
*/
// quantizer scale of window coefs
#define WFIR_QUANTBITS		14
#define WFIR_QUANTSCALE		(1L<<WFIR_QUANTBITS)
#define WFIR_8SHIFT			(WFIR_QUANTBITS-8)
#define WFIR_16BITSHIFT		(WFIR_QUANTBITS)
// log2(number)-1 of precalculated taps range is [4..12]
#define WFIR_FRACBITS		12
#define WFIR_LUTLEN			((1L<<(WFIR_FRACBITS+1))+1)
// number of samples in window
#define WFIR_LOG2WIDTH		3
#define WFIR_WIDTH			(1L<<WFIR_LOG2WIDTH)
#define WFIR_SMPSPERWING	((WFIR_WIDTH-1)>>1)
// cutoff (1.0 == pi/2)
#define WFIR_CUTOFF			0.95f
// wfir type
#define WFIR_HANN			0
#define WFIR_HAMMING		1
#define WFIR_BLACKMANEXACT	2
#define WFIR_BLACKMAN3T61	3
#define WFIR_BLACKMAN3T67	4
#define WFIR_BLACKMAN4T92	5
#define WFIR_BLACKMAN4T74	6
#define WFIR_KAISER4T		7
#define WFIR_TYPE			WFIR_KAISER4T
// wfir help
#ifndef M_zPI
#define M_zPI			3.1415926535897932384626433832795
#endif
#define M_zEPS			1e-8
#define M_zBESSELEPS	1e-21

class CzWINDOWEDFIR
{	public:
		CzWINDOWEDFIR( );
		~CzWINDOWEDFIR( );
		float coef( int _PCnr, float _POfs, float _PCut, int _PWidth, int _PType ) //float _PPos, float _PFc, int _PLen )
		{	double	_LWidthM1		= _PWidth-1;
			double	_LWidthM1Half	= 0.5*_LWidthM1;
			double	_LPosU			= ((double)_PCnr - _POfs);
			double	_LPos			= _LPosU-_LWidthM1Half;
			double	_LPIdl			= 2.0*M_zPI/_LWidthM1;
			double	_LWc,_LSi;
			if( fabs(_LPos)<M_zEPS )
			{	_LWc	= 1.0;
				_LSi	= _PCut;
			}
			else
			{	switch( _PType )
				{	case WFIR_HANN:
						_LWc = 0.50 - 0.50 * cos(_LPIdl*_LPosU);
						break;
					case WFIR_HAMMING:
						_LWc = 0.54 - 0.46 * cos(_LPIdl*_LPosU);
						break;
					case WFIR_BLACKMANEXACT:
						_LWc = 0.42 - 0.50 * cos(_LPIdl*_LPosU) + 0.08 * cos(2.0*_LPIdl*_LPosU);
						break;
					case WFIR_BLACKMAN3T61:
						_LWc = 0.44959 - 0.49364 * cos(_LPIdl*_LPosU) + 0.05677 * cos(2.0*_LPIdl*_LPosU);
						break;
					case WFIR_BLACKMAN3T67:
						_LWc = 0.42323 - 0.49755 * cos(_LPIdl*_LPosU) + 0.07922 * cos(2.0*_LPIdl*_LPosU);
						break;
					case WFIR_BLACKMAN4T92:
						_LWc = 0.35875 - 0.48829 * cos(_LPIdl*_LPosU) + 0.14128 * cos(2.0*_LPIdl*_LPosU) - 0.01168 * cos(3.0*_LPIdl*_LPosU);
						break;
					case WFIR_BLACKMAN4T74:
						_LWc = 0.40217 - 0.49703 * cos(_LPIdl*_LPosU) + 0.09392 * cos(2.0*_LPIdl*_LPosU) - 0.00183 * cos(3.0*_LPIdl*_LPosU);
						break;
					case WFIR_KAISER4T:
						_LWc = 0.40243 - 0.49804 * cos(_LPIdl*_LPosU) + 0.09831 * cos(2.0*_LPIdl*_LPosU) - 0.00122 * cos(3.0*_LPIdl*_LPosU);
						break;
					default:
						_LWc = 1.0;
						break;
				}
				_LPos	 *= M_zPI;
				_LSi	 = sin(_PCut*_LPos)/_LPos;
			}
			return (float)(_LWc*_LSi);
		}
		static signed short lut[WFIR_LUTLEN*WFIR_WIDTH];
};

signed short CzWINDOWEDFIR::lut[WFIR_LUTLEN*WFIR_WIDTH];

CzWINDOWEDFIR::CzWINDOWEDFIR()
{	int _LPcl;
	float _LPcllen	= (float)(1L<<WFIR_FRACBITS);	// number of precalculated lines for 0..1 (-1..0)
	float _LNorm	= 1.0f / (float)(2.0f * _LPcllen);
	float _LCut		= WFIR_CUTOFF;
	float _LScale	= (float)WFIR_QUANTSCALE;
	float _LGain,_LCoefs[WFIR_WIDTH];
	for( _LPcl=0;_LPcl<WFIR_LUTLEN;_LPcl++ )
	{
		float _LOfs		= ((float)_LPcl-_LPcllen)*_LNorm;
		int _LCc,_LIdx	= _LPcl<<WFIR_LOG2WIDTH;
		for( _LCc=0,_LGain=0.0f;_LCc<WFIR_WIDTH;_LCc++ )
		{	_LGain	+= (_LCoefs[_LCc] = coef( _LCc, _LOfs, _LCut, WFIR_WIDTH, WFIR_TYPE ));
		}
		_LGain = 1.0f/_LGain;
		for( _LCc=0;_LCc<WFIR_WIDTH;_LCc++ )
		{	float _LCoef = (float)floor( 0.5 + _LScale*_LCoefs[_LCc]*_LGain );
			lut[_LIdx+_LCc] = (signed short)( (_LCoef<-_LScale)?-_LScale:((_LCoef>_LScale)?_LScale:_LCoef) );
		}
	}
}

CzWINDOWEDFIR::~CzWINDOWEDFIR()
{	// nothing todo
}

CzWINDOWEDFIR sfir;

template <class T, int buffer_size>
class sample_buffer
{
	int ptr, filled;
	T * buffer;

public:
	sample_buffer() : ptr(0), filled(0), buffer(0) {}
	~sample_buffer()
	{
		if (buffer) delete [] buffer;
	}

	void clear()
	{
		if (buffer)
		{
			delete [] buffer;
			buffer = 0;
		}
		ptr = filled = 0;
	}

	inline int size() const
	{
		return filled;
	}

	void push_back(T sample)
	{
		if (!buffer) buffer = new T[buffer_size];
		buffer[ptr] = sample;
		if (++ptr >= buffer_size) ptr = 0;
		if (filled < buffer_size) filled++;
	}

	void erase(int count)
	{
		if (count > filled) filled = 0;
		else filled -= count;
	}

	T operator[] (int index) const
	{
		index += ptr - filled;
		if (index < 0) index += buffer_size;
		else if (index > buffer_size) index -= buffer_size;
		return buffer[index];
	}

	// omghax!
	void lock( T * & out1, unsigned & count1, T * & out2, unsigned & count2 )
	{
		if (!buffer) buffer = new T[buffer_size];
		unsigned free = buffer_size - filled;
		out1 = & buffer[ ptr ];
		if ( ptr )
		{
			count1 = buffer_size - ptr;
			out2 = &buffer[ 0 ];
			count2 = ptr;
			if ( count1 > free )
			{
				count1 = free;
				out2 = 0;
				count2 = 0;
			}
			else if ( count1 + count2 > free )
			{
				count2 = free - count1;
				if ( ! count2 ) out2 = 0;
			}
		}
		else
		{
			count1 = free;
			out2 = 0;
			count2 = 0;
		}
	}

	void push_count( unsigned count )
	{
		if ( count + filled > buffer_size )
		{
			count = buffer_size - filled;
		}

		ptr = ( ptr + count ) % buffer_size;
		filled += count;
	}
};

class foo_null : public foo_interpolate
{
	int sample;

public:
	foo_null() : sample(0) {}
	~foo_null() {}

	void reset() {}

	void push( double rate, int psample )
	{
		sample = psample;
	}

	int pop(double rate)
	{
		return sample;
	}
};

class foo_linear : public foo_interpolate
{
	sample_buffer<int,4> samples;

	int position;

	inline int smp(int index)
	{
		return samples[index];
	}

public:
	foo_linear()
	{
		position = 0;
	}

	~foo_linear() {}

	void reset()
	{
		position = 0;
		samples.clear();
	}

	void push(double rate, int sample)
	{
		samples.push_back(sample);
	}

	int pop(double rate)
	{
		int ret, lrate;

		if (position > 0x7fff)
		{
			int howmany = position >> 15;
			position &= 0x7fff;
			samples.erase(howmany);
		}

		if (samples.size() < 2) return 0;

		ret  = smp(0) * (0x8000 - position);
		ret += smp(1) * position;
		ret >>= 15;

		// wahoo, takes care of drifting
		if (samples.size() > 2)
		{
			rate += (.5 / 32768.);
		}

		lrate = (int)(32768. * rate);
		position += lrate;

		return ret;
	}
};

// and this integer cubic interpolation implementation was kind of borrowed from either TiMidity
// or the P.E.Op.S. SPU project, or is in use in both, or something...

class foo_cubic : public foo_interpolate
{
	sample_buffer<int,12> samples;

	int position;

	inline int smp(int index)
	{
		return samples[index];
	}

public:
	foo_cubic()
	{
		position = 0;
	}

	~foo_cubic() {}

	void reset()
	{
		position = 0;
		samples.clear();
	}

	void push(double rate, int sample)
	{
		samples.push_back(sample);
	}

	int pop(double rate)
	{
		int ret, lrate;

		if (position > 0x7fff)
		{
			int howmany = position >> 15;
			position &= 0x7fff;
			samples.erase(howmany);
		}

		if (samples.size() < 4) return 0;

		ret  = smp(3) - 3 * smp(2) + 3 * smp(1) - smp(0);
		ret *= (position - (2 << 15)) / 6;
		ret >>= 15;
		ret += smp(2) - 2 * smp(1) + smp(0);
		ret *= (position - (1 << 15)) >> 1;
		ret >>= 15;
		ret += smp(1) - smp(0);
		ret *= position;
		ret >>= 15;
		ret += smp(0);

		if (ret > 32767) ret = 32767;
		else if (ret < -32768) ret = -32768;

		// wahoo, takes care of drifting
		if (samples.size() > 8)
		{
			rate += (.5 / 32768.);
		}

		lrate = (int)(32768. * rate);
		position += lrate;

		return ret;
	}
};

class foo_fir : public foo_interpolate
{
	sample_buffer<int,24> samples;

	int position;

	inline int smp(int index)
	{
		return samples[index];
	}

public:
	foo_fir()
	{
		position = 0;
	}

	~foo_fir() {}

	void reset()
	{
		position = 0;
		samples.clear();
	}

	void push(double rate, int sample)
	{
		samples.push_back(sample);
	}

	int pop(double rate)
	{
		int ret, lrate;

		if (position > 0x7fff)
		{
			int howmany = position >> 15;
			position &= 0x7fff;
			samples.erase(howmany);
		}

		if (samples.size() < 8) return 0;

		ret  = smp(0) * CzWINDOWEDFIR::lut[(position & ~7)    ];
		ret += smp(1) * CzWINDOWEDFIR::lut[(position & ~7) + 1];
		ret += smp(2) * CzWINDOWEDFIR::lut[(position & ~7) + 2];
		ret += smp(3) * CzWINDOWEDFIR::lut[(position & ~7) + 3];
		ret += smp(4) * CzWINDOWEDFIR::lut[(position & ~7) + 4];
		ret += smp(5) * CzWINDOWEDFIR::lut[(position & ~7) + 5];
		ret += smp(6) * CzWINDOWEDFIR::lut[(position & ~7) + 6];
		ret += smp(7) * CzWINDOWEDFIR::lut[(position & ~7) + 7];
		ret >>= WFIR_QUANTBITS;

		if (ret > 32767) ret = 32767;
		else if (ret < -32768) ret = -32768;

		// wahoo, takes care of drifting
		if (samples.size() > 16)
		{
			rate += (.5 / 32768.);
		}

		lrate = (int)(32768. * rate);
		position += lrate;

		return ret;
	}
};

class foo_libresample : public foo_interpolate
{
	sample_buffer<float,32> samples;

	void * resampler;

public:
	foo_libresample()
	{
		resampler = 0;
	}

	~foo_libresample()
	{
		reset();
	}

	void reset()
	{
		samples.clear();
		if (resampler)
		{
			resample_close( resampler );
			resampler = 0;
		}
	}

	void push( double rate, int sample )
	{
		if ( ! resampler )
		{
			resampler = resample_open( 0, .25, 44100. / 4000. );
		}

		{
			float in = float( sample );
			float * samples1, * samples2;
			unsigned count1, count2;

			samples.lock( samples1, count1, samples2, count2 );

			int used;
			int processed = resample_process( resampler, 1. / rate, & in, 1, 0, & used, samples1, count1 );

			samples.push_count( processed );

			if ( ! used && count2 )
			{
				processed = resample_process( resampler, 1. / rate, & in, 1, 0, & used, samples2, count2 );

				samples.push_count( processed );
			}
		}
	}

	int pop( double rate )
	{
		int ret;

		if ( samples.size() )
		{
			ret = int( samples[ 0 ] );
			samples.erase( 1 );
		}
		else ret = 0;

		if ( ret > 32767 ) ret = 32767;
		else if ( ret < -32768 ) ret = -32768;

		return ret;
	}
};

#ifdef LIBSAMPLERATE
class foo_src : public foo_interpolate
{
	sample_buffer<float,32> samples;

	SRC_STATE * resampler;

	SRC_DATA resampler_data;

public:
	foo_src()
	{
		resampler = 0;
	}

	~foo_src()
	{
		reset();
	}

	void reset()
	{
		samples.clear();
		if (resampler)
		{
			resampler = src_delete( resampler );
		}
	}

	void push( double rate, int sample )
	{
		if ( ! resampler )
		{
			int err;
			resampler = src_new( SRC_LINEAR, 1, & err );
			if ( err )
			{
				if ( resampler ) resampler = src_delete( resampler );
				return;
			}
		}

		{
			float in = float( sample );
			float * samples1, * samples2;
			unsigned count1, count2;

			samples.lock( samples1, count1, samples2, count2 );

			resampler_data.data_in = & in;
			resampler_data.input_frames = 1;
			resampler_data.data_out = samples1;
			resampler_data.output_frames = count1;
			resampler_data.src_ratio = 1. / rate;

			if ( src_process( resampler, & resampler_data ) )
				return;

			samples.push_count( resampler_data.output_frames_gen );

			if ( ! resampler_data.input_frames_used && count2 )
			{
				resampler_data.data_out = samples2;
				resampler_data.output_frames = count2;

				if ( src_process( resampler, & resampler_data ) )
					return;

				samples.push_count( resampler_data.output_frames_gen );
			}
		}
	}

	int pop(double rate)
	{
		int ret;

		if ( samples.size() )
		{
			ret = int( samples[ 0 ] );
			samples.erase( 1 );
		}
		else ret = 0;

		if ( ret > 32767 ) ret = 32767;
		else if ( ret < -32768 ) ret = -32768;

		return ret;
	}
};
#endif

foo_interpolate * get_filter(int which)
{
	switch (which)
	{
	default:
		return new foo_null;
	case 1:
		return new foo_linear;
	case 2:
		return new foo_cubic;
	case 3:
		return new foo_fir;
	case 4:
		return new foo_libresample;
	}
}

// and here is the implementation specific code, in a messier state than the stuff above

extern bool timer0On;
extern int timer0Reload;
extern int timer0ClockReload;
extern bool timer1On;
extern int timer1Reload;
extern int timer1ClockReload;

extern int SOUND_CLOCK_TICKS;
extern int soundInterpolation;

double calc_rate(int timer)
{
	if (timer ? timer1On : timer0On)
	{
		return double(SOUND_CLOCK_TICKS) /
			double((0x10000 - (timer ? timer1Reload : timer0Reload)) << 
			(timer ? timer1ClockReload : timer0ClockReload));
	}
	else
	{
		return 1.;
	}
}

static foo_interpolate * interp[2];

class foo_interpolate_setup
{
public:
	foo_interpolate_setup()
	{
		for (int i = 0; i < 2; i++)
		{
			interp[i] = get_filter(0);
		}
	}

	~foo_interpolate_setup()
	{
		for (int i = 0; i < 2; i++)
		{
			delete interp[i];
		}
	}
};

static foo_interpolate_setup blah;

class critical_section
{
	CRITICAL_SECTION cs;

public:
	critical_section() { InitializeCriticalSection(&cs); }
	~critical_section() { DeleteCriticalSection(&cs); }

	void enter() { EnterCriticalSection(&cs); }
	void leave() { LeaveCriticalSection(&cs); }
};

static critical_section interp_sync;
static int interpolation = 0;

class scopelock
{
	critical_section * cs;

public:
	scopelock(critical_section & pcs) { cs = &pcs; cs->enter(); }
	~scopelock() { cs->leave(); }
};

void interp_switch(int which)
{
	scopelock sl(interp_sync);

	for (int i = 0; i < 2; i++)
	{
		delete interp[i];
		interp[i] = get_filter(which);
	}

	interpolation = which;
}

void interp_reset(int ch)
{
	scopelock sl(interp_sync);
	if (soundInterpolation != interpolation) interp_switch(soundInterpolation);

	interp[ch]->reset();
}

void interp_push(int ch, double rate, int sample)
{
	scopelock sl(interp_sync);
	if (soundInterpolation != interpolation) interp_switch(soundInterpolation);

	interp[ch]->push(rate, sample);
}

int interp_pop(int ch, double rate)
{
	scopelock sl(interp_sync);
	if (soundInterpolation != interpolation) interp_switch(soundInterpolation);

	return interp[ch]->pop(rate);
}