Merge pull request #271 from MerryMage/channel-filter

ndsp: Monopole and biquad channel filter params

libctru/include/3ds/ndsp/channel.h

@@ -167,12 +167,69 @@ void ndspChnWaveBufAdd(int id, ndspWaveBuf* buf);
  * @param enable Whether to enable the IIR monopole filter.
  */
 void ndspChnIirMonoSetEnable(int id, bool enable);
-//   ndspChnIirMonoSetParams
+/**
+ * @brief Manually sets up the parameters on monopole filter
+ * @param id ID of the channel (0..23).
+ * @param enable Whether to enable the IIR monopole filter.
+ */
+bool ndspChnIirMonoSetParamsCustomFilter(int id, float a0, float a1, float b0);
+/**
+ * @brief Sets the monopole to be a low pass filter. (Note: This is a lower-quality filter than the biquad one.)
+ * @param id ID of the channel (0..23).
+ * @param f0 Low pass cut-off frequency.
+ */
+bool ndspChnIirMonoSetParamsLowPassFilter(int id, float f0);
+/**
+ * @brief Sets the monopole to be a high pass filter. (Note: This is a lower-quality filter than the biquad one.)
+ * @param id ID of the channel (0..23).
+ * @param f0 High pass cut-off frequency.
+ */
+bool ndspChnIirMonoSetParamsHighPassFilter(int id, float f0);
 /**
  * @brief Configures whether the IIR biquad filter of a channel is enabled.
  * @param id ID of the channel (0..23).
  * @param enable Whether to enable the IIR biquad filter.
  */
 void ndspChnIirBiquadSetEnable(int id, bool enable);
-//   ndspChnIirBiquadSetParams
+/**
+ * @brief Manually sets up the parameters of the biquad filter
+ * @param id ID of the channel (0..23).
+ */
+bool ndspChnIirBiquadSetParamsCustomFilter(int id, float a0, float a1, float a2, float b0, float b1, float b2);
+/**
+ * @brief Sets the biquad to be a low pass filter.
+ * @param id ID of the channel (0..23).
+ * @param f0 Low pass cut-off frequency.
+ * @param Q "Quality factor", typically should be sqrt(2)/2 (i.e. 0.7071).
+ */
+bool ndspChnIirBiquadSetParamsLowPassFilter(int id, float f0, float Q);
+/**
+ * @brief Sets the biquad to be a high pass filter.
+ * @param id ID of the channel (0..23).
+ * @param f0 High pass cut-off frequency.
+ * @param Q "Quality factor", typically should be sqrt(2)/2 (i.e. 0.7071).
+ */
+bool ndspChnIirBiquadSetParamsHighPassFilter(int id, float f0, float Q);
+/**
+ * @brief Sets the biquad to be a band pass filter.
+ * @param id ID of the channel (0..23).
+ * @param f0 Mid-frequency.
+ * @param Q "Quality factor", typically should be sqrt(2)/2 (i.e. 0.7071).
+ */
+bool ndspChnIirBiquadSetParamsBandPassFilter(int id, float f0, float Q);
+/**
+ * @brief Sets the biquad to be a notch filter.
+ * @param id ID of the channel (0..23).
+ * @param f0 Notch frequency.
+ * @param Q "Quality factor", typically should be sqrt(2)/2 (i.e. 0.7071).
+ */
+bool ndspChnIirBiquadSetParamsNotchFilter(int id, float f0, float Q);
+/**
+ * @brief Sets the biquad to be a peaking equalizer.
+ * @param id ID of the channel (0..23).
+ * @param f0 Central frequency.
+ * @param Q "Quality factor", typically should be sqrt(2)/2 (i.e. 0.7071).
+ * @param gain Amount of gain (raw value = 10 ^ dB/40)
+ */
+bool ndspChnIirBiquadSetParamsPeakingEqualizer(int id, float f0, float Q, float gain);
 ///@}

libctru/source/ndsp/ndsp-channel.c

@@ -11,6 +11,8 @@ enum
 	CFLAG_RATE          = BIT(5),
 	CFLAG_MIX           = BIT(6),
 	CFLAG_ADPCMCOEFS    = BIT(7),
+	CFLAG_IIRMONO       = BIT(8),
+	CFLAG_IIRBIQUAD     = BIT(9),
 };
 
 typedef struct
@@ -25,7 +27,11 @@ typedef struct
 	u16 wavBufCount, wavBufIdNext;
 
 	bool playing, paused;
-	u8 interpType, iirFilterType;
+	u8 interpType;
+
+	u8 iirFilterType;
+	s16 iirMono[2];
+	s16 iirBiquad[5];
 
 	u16 format;
 	u16 wavBufSeq;
@@ -202,6 +208,66 @@ void ndspChnIirBiquadSetEnable(int id, bool enable)
 	LightLock_Unlock(&chn->lock);
 }
 
+static s16 iirParamClamp(float param, float scale_factor, bool* success)
+{
+	float scaled = param * scale_factor;
+	s16 result = (s16) scaled;
+	if (scaled > 0x7FFF)
+	{
+		result = 0x7FFF;
+		*success = false;
+	}
+	else if (scaled < -0x8000)
+	{
+		result = -0x8000;
+		*success = false;
+	}
+	return result;
+}
+
+bool ndspChnIirMonoSetParamsCustomFilter(int id, float a0, float a1, float b0)
+{
+	bool success = true;
+	s16 params[2];
+	params[0] = iirParamClamp(+b0 / a0, (float)(1 << 15), &success);
+	params[1] = iirParamClamp(-a1 / a0, (float)(1 << 15), &success);
+
+	ndspChnSt* chn = &ndspChn[id];
+	LightLock_Lock(&chn->lock);
+
+	memcpy(chn->iirMono, params, sizeof(chn->iirMono));
+	chn->iirFilterType |= BIT(0);
+
+	chn->flags |= CFLAG_IIRMONO | CFLAG_IIRFILTERTYPE;
+
+	LightLock_Unlock(&chn->lock);
+
+	return success;
+}
+
+bool ndspChnIirBiquadSetParamsCustomFilter(int id, float a0, float a1, float a2, float b0, float b1, float b2)
+{
+	bool success = true;
+	s16 params[5];
+	params[0] = iirParamClamp(-a2 / a0, (float)(1 << 14), &success);
+	params[1] = iirParamClamp(-a1 / a0, (float)(1 << 14), &success);
+	params[2] = iirParamClamp(+b2 / a0, (float)(1 << 14), &success);
+	params[3] = iirParamClamp(+b1 / a0, (float)(1 << 14), &success);
+	params[4] = iirParamClamp(+b0 / a0, (float)(1 << 14), &success);
+
+	ndspChnSt* chn = &ndspChn[id];
+	LightLock_Lock(&chn->lock);
+
+	memcpy(chn->iirBiquad, params, sizeof(chn->iirBiquad));
+	chn->iirFilterType |= BIT(1);
+
+	chn->flags |= CFLAG_IIRBIQUAD | CFLAG_IIRFILTERTYPE;
+
+	LightLock_Unlock(&chn->lock);
+
+	return success;
+}
+
 void ndspiInitChn(void)
 {
 	int i;
@@ -278,6 +344,18 @@ void ndspiUpdateChn(void)
 			stflags |= 4;
 		}
 
+		if (flags & CFLAG_IIRBIQUAD)
+		{
+			memcpy(st->iirFilter_biquad, chn->iirBiquad, sizeof(chn->iirBiquad));
+			stflags |= 0x1000000;
+		}
+
+		if (flags & CFLAG_IIRMONO)
+		{
+			memcpy(st->iirFilter_mono, chn->iirMono, sizeof(chn->iirMono));
+			stflags |= 0x800000;
+		}
+
 		// Do wavebuf stuff
 		int wvcount = chn->wavBufCount;
 		ndspWaveBuf* wb = chn->waveBuf;

libctru/source/ndsp/ndsp-filter.c

@@ -0,0 +1,103 @@
+#include <math.h>
+#include <3ds/types.h>
+#include <3ds/ndsp/ndsp.h>
+#include <3ds/ndsp/channel.h>
+
+#define Fs 32728.0f
+
+bool ndspChnIirMonoSetParamsLowPassFilter(int id, float f0)
+{
+	const float w0 = 2.f * M_PI * f0 / Fs;
+
+	const float a0 = 1.f;
+	const float a1 = 1.f - expf(-w0);
+	const float b0 = expf(-w0);
+
+	return ndspChnIirMonoSetParamsCustomFilter(id, a0, a1, b0);
+}
+
+bool ndspChnIirMonoSetParamsHighPassFilter(int id, float f0)
+{
+	const float w0 = 2.f * M_PI * (0.5f - f0 / Fs);
+
+	const float a0 = 1.f;
+	const float a1 = 1.f - expf(-w0);
+	const float b0 = -expf(-w0);
+
+	return ndspChnIirMonoSetParamsCustomFilter(id, a0, a1, b0);
+}
+
+bool ndspChnIirBiquadSetParamsLowPassFilter(int id, float f0, float Q)
+{
+	const float w0 = 2.f * M_PI * f0 / Fs;
+	const float a = sinf(w0) / (2.f * Q);
+
+	const float a0 = 1.f + a;
+	const float a1 = -2.f * cosf(w0);
+	const float a2 = 1.f - a;
+	const float b0 = 0.5f * (1.f - cosf(w0));
+	const float b1 =        (1.f - cosf(w0));
+	const float b2 = 0.5f * (1.f - cosf(w0));
+
+	return ndspChnIirBiquadSetParamsCustomFilter(id, a0, a1, a2, b0, b1, b2);
+}
+
+bool ndspChnIirBiquadSetParamsHighPassFilter(int id, float f0, float Q)
+{
+	const float w0 = 2.f * M_PI * f0 / Fs;
+	const float a = sinf(w0) / (2.f * Q);
+
+	const float a0 = 1.f + a;
+	const float a1 = -2.f * cosf(w0);
+	const float a2 = 1.f - a;
+	const float b0 = 0.5f * (1.f + cosf(w0));
+	const float b1 =       -(1.f + cosf(w0));
+	const float b2 = 0.5f * (1.f + cosf(w0));
+
+	return ndspChnIirBiquadSetParamsCustomFilter(id, a0, a1, a2, b0, b1, b2);
+}
+
+bool ndspChnIirBiquadSetParamsBandPassFilter(int id, float f0, float Q)
+{
+	const float w0 = 2.f * M_PI * f0 / Fs;
+	const float a = sinf(w0) / (2.f * Q);
+
+	const float a0 = 1.f + a;
+	const float a1 = -2.f * cosf(w0);
+	const float a2 = 1.f - a;
+	const float b0 = a;
+	const float b1 = 0.f;
+	const float b2 = a;
+
+	return ndspChnIirBiquadSetParamsCustomFilter(id, a0, a1, a2, b0, b1, b2);
+}
+
+bool ndspChnIirBiquadSetParamsNotchFilter(int id, float f0, float Q)
+{
+	const float w0 = 2.f * M_PI * f0 / Fs;
+	const float a = sinf(w0) / (2.f * Q);
+
+	const float a0 = 1.f + a;
+	const float a1 = -2.f * cosf(w0);
+	const float a2 = 1.f - a;
+	const float b0 = 1.f;
+	const float b1 = -2.f * cosf(w0);
+	const float b2 = 1.f;
+
+	return ndspChnIirBiquadSetParamsCustomFilter(id, a0, a1, a2, b0, b1, b2);
+}
+
+bool ndspChnIirBiquadSetParamsPeakingEqualizer(int id, float f0, float Q, float gain)
+{
+	const float w0 = 2.f * M_PI * f0 / Fs;
+	const float a = sinf(w0) / (2.f * Q);
+
+	const float a0 = 1.f + a*gain;
+	const float a1 = -2.f * cosf(w0);
+	const float a2 = 1.f - a*gain;
+	const float b0 = 1.f + a*gain;
+	const float b1 = -2.f * cosf(w0);
+	const float b2 = 1.f - a*gain;
+
+	return ndspChnIirBiquadSetParamsCustomFilter(id, a0, a1, a2, b0, b1, b2);
+}