Initial community commit

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);
+}