From d431a67f31097d8349d180465a11aa414d92c205 Mon Sep 17 00:00:00 2001 From: MerryMage Date: Fri, 25 Mar 2016 18:58:00 +0000 Subject: [PATCH] ndsp: Monopole and biquad channel filter params --- libctru/include/3ds/ndsp/channel.h | 61 ++++++++++++++++- libctru/source/ndsp/ndsp-channel.c | 80 +++++++++++++++++++++- libctru/source/ndsp/ndsp-filter.c | 103 +++++++++++++++++++++++++++++ 3 files changed, 241 insertions(+), 3 deletions(-) create mode 100644 libctru/source/ndsp/ndsp-filter.c diff --git a/libctru/include/3ds/ndsp/channel.h b/libctru/include/3ds/ndsp/channel.h index 443725c..d3c81b4 100644 --- a/libctru/include/3ds/ndsp/channel.h +++ b/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); ///@} diff --git a/libctru/source/ndsp/ndsp-channel.c b/libctru/source/ndsp/ndsp-channel.c index d9437ee..3a491e9 100644 --- a/libctru/source/ndsp/ndsp-channel.c +++ b/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; diff --git a/libctru/source/ndsp/ndsp-filter.c b/libctru/source/ndsp/ndsp-filter.c new file mode 100644 index 0000000..81a5608 --- /dev/null +++ b/libctru/source/ndsp/ndsp-filter.c @@ -0,0 +1,103 @@ +#include +#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); +}