Initial community commit

Src/Winamp/benskiQ/Biquad.cpp

@@ -0,0 +1,204 @@
+#include "Biquad.h"
+#include <assert.h>
+#include <math.h>
+
+Biquad::Biquad()
+		: sampleRate(44100)
+		, _f0(1000)
+{
+	_z_eq_b[0] = 1;
+	_z_eq_b[1] = 0;
+	_z_eq_b[2] = 0;
+	_z_eq_a[0] = 1;
+	_z_eq_a[1] = 0;
+	_z_eq_a[2] = 0;
+
+	clear_buffers();
+
+	_s_eq_a[0] = 1;
+	_s_eq_a[1] = 2;
+	_s_eq_a[2] = 1;
+
+	_s_eq_b[0] = _s_eq_a[0];
+	_s_eq_b[1] = _s_eq_a[1];
+	_s_eq_b[2] = _s_eq_a[2];
+}
+
+
+#define M_PI 3.14159265358979323846
+// warp to the z-plane
+void Biquad::transform_s_to_z()
+{
+	// s to z bilinear transform
+	const double inv_k = tan(_f0 * M_PI / sampleRate);
+	const double k = 1 / inv_k;
+	const double kk = k*k;
+
+	const double b1k = _s_eq_b[1] * k;
+	const double b2kk = _s_eq_b[2] * kk;
+	const double b2kk_plus_b0 = b2kk + _s_eq_b[0];
+	const double b0z = b2kk_plus_b0 + b1k;
+	const double b2z = b2kk_plus_b0 - b1k;
+	const double b1z = 2 * (_s_eq_b[0] - b2kk);
+
+	const double a1k = _s_eq_a[1] * k;
+	const double a2kk = _s_eq_a[2] * kk;
+	const double a2kk_plus_a0 = a2kk + _s_eq_a[0];
+	const double a0z = a2kk_plus_a0 + a1k;
+	const double a2z = a2kk_plus_a0 - a1k;
+	const double a1z = 2 * (_s_eq_a[0] - a2kk);
+
+	// IIR coefficients
+	const double mult = 1 / a0z;
+
+	_z_eq_b[0] = float(b0z * mult);
+	_z_eq_b[1] = float(b1z * mult);
+	_z_eq_b[2] = float(b2z * mult);
+
+	_z_eq_a[0] = 1;
+	_z_eq_a[1] = float(a1z * mult);
+	_z_eq_a[2] = float(a2z * mult);
+}
+
+void Biquad::process_block(float *dest_ptr, const float *src_ptr, long nbr_spl)
+{
+	assert(nbr_spl >= 0);
+
+	if (nbr_spl == 0)
+	{
+		return;
+	}
+
+// If we're not on a pair boudary, we process a single sample.
+	if (_mem_pos != 0)
+	{
+		*dest_ptr++ = (float)process_sample(*src_ptr++);
+		nbr_spl--;
+	}
+
+	if (nbr_spl == 0)
+	{
+		return;
+	}
+
+	long    half_nbr_spl = nbr_spl >> 1;
+	long    index = 0;
+	if (half_nbr_spl > 0)
+	{
+		double    mem_x[2];
+		double    mem_y[2];
+		mem_x[0] = xn[0];
+		mem_x[1] = xn[1];
+		mem_y[0] = yn[0];
+		mem_y[1] = yn[1];
+
+		do
+		{
+
+			float    x = src_ptr[index];
+			mem_y[1] = _z_eq_b[0] * x
+			           + (_z_eq_b[1] * mem_x[0]
+			              + _z_eq_b[2] * mem_x[1])
+			           - (_z_eq_a[1] * mem_y[0]
+			              + _z_eq_a[2] * mem_y[1]);
+
+			mem_x[1] = x;
+			dest_ptr[index] = (float)mem_y[1];
+
+			x = src_ptr[index + 1];
+			mem_y[0] = _z_eq_b[0] * x
+			           + (_z_eq_b[1] * mem_x[1]
+			              + _z_eq_b[2] * mem_x[0])
+			           - (_z_eq_a[1] * mem_y[1]
+			              + _z_eq_a[2] * mem_y[0]);
+
+			mem_x[0] = x;
+			dest_ptr[index + 1] = (float)mem_y[0];
+			index += 2;
+
+			-- half_nbr_spl;
+		}
+		while (half_nbr_spl > 0);
+
+		xn[0] = mem_x[0];
+		xn[1] = mem_x[1];
+		yn[0] = mem_y[0];
+		yn[1] = mem_y[1];
+	}
+
+// If number of samples was odd, there is one more to process.
+	if ((nbr_spl & 1) > 0)
+	{
+		dest_ptr[index] = (float)process_sample(src_ptr[index]);
+	}
+}
+
+void Biquad::clear_buffers()
+{
+	xn[0] = 0;
+	xn[1] = 0;
+	yn[0] = 0;
+	yn[1] = 0;
+	_mem_pos = 0;
+}
+
+double Biquad::process_sample(double x)
+{
+	const int  alt_pos = 1 - _mem_pos;
+	const double  y = _z_eq_b[0] * x
+	                  + (_z_eq_b[1] * xn[_mem_pos]
+	                     + _z_eq_b[2] * xn[alt_pos])
+	                  - (_z_eq_a[1] * yn[_mem_pos]
+	                     + _z_eq_a[2] * yn[alt_pos]);
+
+	xn[alt_pos] = x;
+	yn[alt_pos] = y;
+	_mem_pos = alt_pos;
+
+	return (y);
+}
+
+void Biquad::copy_filter(const Biquad &other)
+{
+	_z_eq_b[0] = other._z_eq_b[0];
+	_z_eq_b[1] = other._z_eq_b[1];
+	_z_eq_b[2] = other._z_eq_b[2];
+	_z_eq_a[1] = other._z_eq_a[1];
+	_z_eq_a[2] = other._z_eq_a[2];
+
+	sampleRate = other.sampleRate;
+	_f0 = other._f0;
+	set_s_eq(other._s_eq_b, other._s_eq_a);
+}
+
+void Biquad::SetSampleRate(double fs)
+{
+	assert(fs > 0);
+
+	sampleRate = fs;
+	transform_s_to_z();
+}
+
+void Biquad::set_freq(double f0)
+{
+	assert(f0 > 0);
+
+	_f0 = f0;
+}
+
+void Biquad::set_s_eq(const double b[3], const double a[3])
+{
+	assert(a != 0);
+	assert(a[2] != 0);
+	assert(b != 0);
+
+	_s_eq_b[0] = float(b[0]);
+	_s_eq_b[1] = float(b[1]);
+	_s_eq_b[2] = float(b[2]);
+
+	_s_eq_a[0] = float(a[0]);
+	_s_eq_a[1] = float(a[1]);
+	_s_eq_a[2] = float(a[2]);
+}
+
+

Src/Winamp/benskiQ/Biquad.h

@@ -0,0 +1,29 @@
+#pragma once
+
+class Biquad
+{
+public:
+	Biquad();
+	void copy_filter(const Biquad &other);
+	void SetSampleRate(double fs);
+	void set_freq(double f0);
+	void set_s_eq(const double b[3], const double a[3]);
+	void transform_s_to_z();
+
+	void process_block(float *dest_ptr, const float *src_ptr, long nbr_spl);
+	void clear_buffers();
+
+private:
+	double _s_eq_b[3]; // Coefs for numerator (zeros)
+	double _s_eq_a[3]; // Coefs for denominator (poles)
+	double _z_eq_b[3]; // Direct coefficients, order z^(-n)
+	double _z_eq_a[3]; // Recursive coefficients, order z^(-n)
+	double sampleRate; // Hz, > 0
+	double _f0; // Hz, > 0, _f0 % (_sample_freq/2) != 0
+	double xn[2]; // Input memory, order z^(-n)
+	double yn[2]; // Output memory, order z^(-n)
+	int _mem_pos; // 0 or 1
+
+	inline double process_sample(double x);
+};
+

Src/Winamp/benskiQ/EqBand.cpp

@@ -0,0 +1,81 @@
+#include "EqBand.h"
+#include <assert.h>
+#include <math.h>
+
+EqBand::EqBand() : sampleRate(44100), centerFrequency(1000), gain(1), _q(0.5), nch(0), channels(0), bypass(true)
+{
+}
+
+void EqBand::set_num_channels(int num_channels)
+{
+	if (nch < num_channels)
+	{
+		nch = num_channels;
+		delete[]channels;
+		channels = new Biquad[nch];
+		clear_buffers();
+		set_parameters(centerFrequency, gain, _q);
+	}
+}
+
+void EqBand::SetSampleRate(double sample_freq)
+{
+	if (sample_freq != sampleRate)
+	{
+		sampleRate = sample_freq;
+		for (int chn = 0; chn < nch; ++chn)
+		{
+			channels[chn].SetSampleRate(sampleRate);
+		}
+		clear_buffers();
+		set_parameters(centerFrequency, gain, _q);
+	}
+}
+
+void EqBand::set_parameters(double freq, double newGain, double q)
+{
+	centerFrequency = freq;
+	gain = newGain;
+	_q = q;
+
+	if (nch > 0)
+	{
+		Biquad & ref_filter = channels[0];
+
+		ref_filter.set_freq(centerFrequency);
+
+		double a[3] = { 1, 1/_q, 1 };
+		double b[3] = {1, gain / _q, 1};
+		ref_filter.set_s_eq(b, a);
+
+		ref_filter.transform_s_to_z();
+
+		for (int chn = 1; chn < nch; ++chn)
+			channels[chn].copy_filter(ref_filter);
+	}
+
+	bypass = (fabs(gain - 1.0) < 0.02); // About 1/4 dB
+}
+
+void EqBand::process(float ** const out, float ** in, long nbr_spl, int nbr_chn)
+{
+	assert(nbr_chn >= 0);
+	assert(nbr_chn <= nch);
+
+	if (!bypass)
+	{
+		for (int chn = 0; chn < nbr_chn; ++chn)
+		{
+			channels[chn].process_block(out[chn], in[chn], nbr_spl);
+		}
+	}
+}
+
+void EqBand::clear_buffers()
+{
+	for (int chn = 0; chn < nch; ++chn)
+	{
+		channels[chn].clear_buffers();
+	}
+}
+

Src/Winamp/benskiQ/EqBand.h

@@ -0,0 +1,23 @@
+#pragma once
+
+#include "Biquad.h"
+
+class EqBand
+{
+public:
+	EqBand();
+	void set_num_channels(int num_channels);
+	void SetSampleRate(double sample_freq);
+	void set_parameters(double freq, double gain, double q);
+	void process(float ** const out, float ** in, long nbr_spl, int nbr_chn);
+
+private:
+	double sampleRate, centerFrequency, gain;
+	double _q;
+	int nch;
+	Biquad *channels;
+	bool bypass;
+
+	void clear_buffers();
+}; 
+

Src/Winamp/benskiQ/benskiQ.cpp

@@ -0,0 +1,231 @@
+#include "main.h"
+
+#include "EqBand.h"
+#include "WinampAttributes.h"
+
+#include <math.h>
+
+extern int filter_srate,  filter_enabled, filter_top, filter_top2;
+extern float preamp_val;
+
+static int filter_nch=0;
+static bool init=false;
+static EqBand benskiQ[10];
+//#define BENSKIQ_Q 0.70710678118654752440084436210485
+#define BENSKIQ_Q 1.41
+static double benskiQ_freqs_iso[10]={31.5, 63, 125, 250, 500, 1000, 2000, 4000, 8000, 16000}; // ISO standard equalizer frequency table
+static double benskiQ_freqs[10]={ 70, 180, 320, 600, 1000, 3000, 6000, 12000, 14000, 16000 }; // winamp style frequency table
+
+static CRITICAL_SECTION benskiQ_cs;
+void benskiQ_init()
+{
+	InitializeCriticalSection(&benskiQ_cs);
+	for (int x=0;x<10;x++)
+	{
+		benskiQ[x].set_parameters((config_eq_frequencies==EQ_FREQUENCIES_WINAMP)?benskiQ_freqs[x]:benskiQ_freqs_iso[x], 1.0, BENSKIQ_Q);
+	}
+}
+
+
+static __inline double VALTODB(int v)
+{
+	v -= 31;
+	if (v < -31) v = -31;
+	if (v > 32) v = 32;
+
+	if (v > 0) return -12.0*(v / 32.0);
+	else if (v < 0)
+	{
+		return -12.0*(v / 31.0);
+	}
+	return 0.0f;
+}
+
+static __inline double VALTOGAIN(int v)
+{
+	return pow(10.0, VALTODB(v)/20.0);
+}
+
+void benskiQ_eq_set(char data[10])
+{
+	if (!init)
+	{
+		init=true; benskiQ_init();
+	}
+	EnterCriticalSection(&benskiQ_cs);
+	for (int x = 0; x < 10; x ++)
+	{
+		benskiQ[x].set_parameters((config_eq_frequencies==EQ_FREQUENCIES_WINAMP)?benskiQ_freqs[x]:benskiQ_freqs_iso[x], VALTOGAIN(data[x]), BENSKIQ_Q);
+	}
+	LeaveCriticalSection(&benskiQ_cs);
+}
+
+static void FillFloat(float **floatBuf, void *samples, size_t bps, size_t numSamples, size_t numChannels, float preamp)
+{
+	switch (bps)
+	{
+	case 8:
+		{
+			preamp /= 256.0f;
+			unsigned __int8 *samples8 = (unsigned __int8 *)samples;
+			for (size_t c=0;c<numChannels;c++)
+				for (size_t x = 0; x != numSamples; x ++)
+				{
+					floatBuf[c][x] = (float)(samples8[c+numChannels*x]-128) * preamp;
+				}
+		}
+		break;
+	case 16:
+		{
+			preamp/=32768.0f;
+			short *samples16 = (short *)samples;
+			for (size_t c=0;c<numChannels;c++)
+				for (size_t x = 0; x != numSamples; x ++)
+				{
+					floatBuf[c][x] = (float)samples16[c+numChannels*x] * preamp;
+				}
+		}
+		break;
+	case 24:
+		{
+			preamp/=2147483648.0f;
+			unsigned __int8 *samples8 = (unsigned __int8 *)samples;
+
+			long temp;
+
+			for (size_t x = 0; x != numSamples; x ++)
+				for (size_t c=0;c<numChannels;c++)
+				{
+					temp = (((long)samples8[0]) << 8);
+					temp = temp | (((long)samples8[1]) << 16);
+					temp = temp | (((long)samples8[2]) << 24);
+					floatBuf[c][x] = (float)temp * preamp;
+					samples8+=3;
+				}
+		}
+		break;
+	case 32:
+		{
+			preamp /= 2147483648.0f;
+			int32_t *samples32 = (int32_t *)samples;
+			for (size_t x = 0; x != numSamples; x ++)
+				for (size_t c=0;c<numChannels;c++)
+				{
+					floatBuf[c][x] = (float)samples32[c+x*numChannels] * preamp;
+				}
+		}
+		break;
+	}
+}
+
+static void FillSamples(void *samples, float **floatBuf, size_t bps, size_t numSamples, size_t numChannels)
+{
+	switch (bps)
+	{
+	case 16:
+		for (size_t i=0;i<numChannels;i++)
+			Float32_To_Int16_Clip((char *)samples+i*(bps/8), (signed int)numChannels, floatBuf[i], 1, (unsigned int) numSamples);
+		break;
+	case 24:
+		for (size_t i=0;i<numChannels;i++)
+			Float32_To_Int24_Clip((char *)samples+i*(bps/8), (signed int)numChannels, floatBuf[i], 1, (unsigned int) numSamples);
+		break;
+
+	}
+}
+
+static int last_nch=0, last_numsamples=0;
+static float **last_sample=0;
+void DeleteSample(float **big, int nch)
+{
+	for (int i=0;i<nch;i++)
+		delete big[i];
+	delete[]big;
+}
+
+float **MakeSample(int numsamples, int nch)
+{
+	if (last_nch < nch || last_numsamples < numsamples)
+	{
+		DeleteSample(last_sample, last_nch);
+		last_nch=max(nch, last_nch);
+		last_numsamples=max(numsamples, last_numsamples);
+		last_sample = new float*[last_nch];
+		for (int i=0;i<last_nch;i++)
+			last_sample [i]=new float[last_numsamples];
+	}
+	return last_sample;
+}
+
+void benskiQ_reset(int srate, int nch)
+{
+	for (int i=0;i<10;i++)
+	{
+		benskiQ[i].SetSampleRate(srate);
+		benskiQ[i].set_num_channels(nch);
+	}
+
+	int x;
+	if (config_eq_frequencies == EQ_FREQUENCIES_WINAMP)
+		for (x = 0; x < 10 && benskiQ_freqs[x]*2 <= srate; x++);
+	else
+		for (x = 0; x < 10 && benskiQ_freqs_iso[x]*2 <= srate; x++);
+	filter_top = min(x, filter_top2);
+	filter_srate=srate;
+	filter_nch=nch;
+}
+
+static float NonReplayGainAdjust()
+{
+	if (!(in_mod->UsesOutputPlug&IN_MODULE_FLAG_REPLAYGAIN) && config_replaygain.GetBool())
+		return pow(10.0f, (float)config_replaygain_non_rg_gain/20.0f);
+	else
+		return 1.0f;
+}
+
+static float ReplayGainPreamp()
+{
+	if (!(in_mod->UsesOutputPlug&IN_MODULE_FLAG_REPLAYGAIN_PREAMP) && config_replaygain.GetBool())
+	   return pow(10.0f, (float)config_replaygain_preamp/20.0f);
+	else
+		return 1.0f; 
+}
+
+int benskiQ_eq_dosamples(short *samples, int numsamples, int bps, int nch, int srate)
+{
+	if (filter_enabled && in_mod && !(in_mod->UsesOutputPlug&IN_MODULE_FLAG_EQ) && bps != 32)
+	{
+		if (srate !=filter_srate || nch != filter_nch)
+			benskiQ_reset(srate, nch);
+
+		if (!init)
+		{
+			init=true; benskiQ_init();
+		}
+		float **in = MakeSample(numsamples, nch);
+
+		FillFloat(in, samples, bps, numsamples, nch, preamp_val*NonReplayGainAdjust()*ReplayGainPreamp());
+		EnterCriticalSection(&benskiQ_cs);
+		for (int x = 0; x < filter_top; x ++)
+		{
+			benskiQ[x].process(in, in, numsamples, nch);
+		}
+		LeaveCriticalSection(&benskiQ_cs);
+		FillSamples(samples, in, bps, numsamples, nch);
+	}
+	else if (!(in_mod->UsesOutputPlug&IN_MODULE_FLAG_REPLAYGAIN) && config_replaygain.GetBool() && (config_replaygain_non_rg_gain.GetFloat() != 0) && bps != 32)
+	{
+		float **in = MakeSample(numsamples, nch);
+		FillFloat(in, samples, bps, numsamples, nch, NonReplayGainAdjust()*ReplayGainPreamp());
+		FillSamples(samples, in, bps, numsamples, nch);
+	}
+	else if (!(in_mod->UsesOutputPlug&IN_MODULE_FLAG_REPLAYGAIN_PREAMP) && config_replaygain.GetBool() && (config_replaygain_preamp.GetFloat() != 0) && bps != 32)
+	{
+		float **in = MakeSample(numsamples, nch);
+		FillFloat(in, samples, bps, numsamples, nch, ReplayGainPreamp());
+		FillSamples(samples, in, bps, numsamples, nch);
+	}
+	else
+		filter_srate = 0;
+	return dsp_dosamples(samples, numsamples, bps, nch, srate);
+}