AUDIO: Implement low-pass filtering for Paula

Paula low-pass filtering, as implemented by UAE.

The Amiga has two filtering circuits: a static RC filter
(only) on the A500, and an LED filter that can be enabled
or disabled dynamically.

By default, the Paula now doesn't apply the static RC
filter, but allows for enabling the LED filter (with
setAudioFilter()).

NOTE: At the moment, this code still uses floating point
arithmetics! It also calls tan() three times per
instantiation.

2 files changed, 135 insertions, 8 deletions

diff --git a/audio/mods/paula.cpp b/audio/mods/paula.cpp
index 0ebf3bc32a..283c0cbb7b 100644
--- a/audio/mods/paula.cpp
+++ b/audio/mods/paula.cpp
@@ -20,14 +20,37 @@
  *
  */
 
+/*
+ * The low-pass filter code is based on UAE's audio filter code
+ * found in audio.c. UAE is licensed under the terms of the GPLv2.
+ *
+ * audio.c in UAE states the following:
+ * Copyright 1995, 1996, 1997 Bernd Schmidt
+ * Copyright 1996 Marcus Sundberg
+ * Copyright 1996 Manfred Thole
+ * Copyright 2006 Toni Wilen
+ */
+
+#include <math.h>
+
+#include "common/scummsys.h"
+
 #include "audio/mods/paula.h"
 #include "audio/null.h"
 
 namespace Audio {
 
-Paula::Paula(bool stereo, int rate, uint interruptFreq) :
+Paula::Paula(bool stereo, int rate, uint interruptFreq, FilterMode filterMode) :
 		_stereo(stereo), _rate(rate), _periodScale((double)kPalPaulaClock / rate), _intFreq(interruptFreq) {
 
+	_filterState.mode      = filterMode;
+	_filterState.ledFilter = false;
+	filterResetState();
+
+	_filterState.a0[0] = filterCalculateA0(rate,  6200);
+	_filterState.a0[1] = filterCalculateA0(rate, 20000);
+	_filterState.a0[2] = filterCalculateA0(rate,  7000);
+
 	clearVoices();
 	_voice[0].panning = 191;
 	_voice[1].panning = 63;
@@ -73,12 +96,70 @@ int Paula::readBuffer(int16 *buffer, const int numSamples) {
 		return readBufferIntern<false>(buffer, numSamples);
 }
 
+/* Denormals are very small floating point numbers that force FPUs into slow
+ * mode. All lowpass filters using floats are suspectible to denormals unless
+ * a small offset is added to avoid very small floating point numbers.
+ */
+#define DENORMAL_OFFSET (1E-10)
+
+/* Based on UAE.
+ * Original comment in UAE:
+ *
+ * Amiga has two separate filtering circuits per channel, a static RC filter
+ * on A500 and the LED filter. This code emulates both.
+ *
+ * The Amiga filtering circuitry depends on Amiga model. Older Amigas seem
+ * to have a 6 dB/oct RC filter with cutoff frequency such that the -6 dB
+ * point for filter is reached at 6 kHz, while newer Amigas have no filtering.
+ *
+ * The LED filter is complicated, and we are modelling it with a pair of
+ * RC filters, the other providing a highboost. The LED starts to cut
+ * into signal somewhere around 5-6 kHz, and there's some kind of highboost
+ * in effect above 12 kHz. Better measurements are required.
+ *
+ * The current filtering should be accurate to 2 dB with the filter on,
+ * and to 1 dB with the filter off.
+ */
+inline int32 filter(int32 input, Paula::FilterState &state, int voice) {
+	float normalOutput, ledOutput;
+
+	switch (state.mode) {
+	case Paula::kFilterModeA500:
+		state.rc[voice][0] = state.a0[0] * input + (1 - state.a0[0]) * state.rc[voice][0] + DENORMAL_OFFSET;
+		state.rc[voice][1] = state.a0[1] * state.rc[voice][0] + (1-state.a0[1]) * state.rc[voice][1];
+		normalOutput = state.rc[voice][1];
+
+		state.rc[voice][2] = state.a0[2] * normalOutput        + (1 - state.a0[2]) * state.rc[voice][2];
+		state.rc[voice][3] = state.a0[2] * state.rc[voice][2]  + (1 - state.a0[2]) * state.rc[voice][3];
+		state.rc[voice][4] = state.a0[2] * state.rc[voice][3]  + (1 - state.a0[2]) * state.rc[voice][4];
+
+		ledOutput = state.rc[voice][4];
+		break;
+
+	case Paula::kFilterModeA1200:
+		normalOutput = input;
+
+		state.rc[voice][1] = state.a0[2] * normalOutput        + (1 - state.a0[2]) * state.rc[voice][1] + DENORMAL_OFFSET;
+		state.rc[voice][2] = state.a0[2] * state.rc[voice][1]  + (1 - state.a0[2]) * state.rc[voice][2];
+		state.rc[voice][3] = state.a0[2] * state.rc[voice][2]  + (1 - state.a0[2]) * state.rc[voice][3];
+
+		ledOutput = state.rc[voice][3];
+		break;
+
+	case Paula::kFilterModeNone:
+	default:
+		return input;
+
+	}
+
+	return CLIP<int32>(state.ledFilter ? ledOutput : normalOutput, -32768, 32767);
+}
 
 template<bool stereo>
-inline int mixBuffer(int16 *&buf, const int8 *data, Paula::Offset &offset, frac_t rate, int neededSamples, uint bufSize, byte volume, byte panning) {
+inline int mixBuffer(int16 *&buf, const int8 *data, Paula::Offset &offset, frac_t rate, int neededSamples, uint bufSize, byte volume, byte panning, Paula::FilterState &filterState, int voice) {
 	int samples;
 	for (samples = 0; samples < neededSamples && offset.int_off < bufSize; ++samples) {
-		const int32 tmp = ((int32) data[offset.int_off]) * volume;
+		const int32 tmp = filter(((int32) data[offset.int_off]) * volume, filterState, voice);
 		if (stereo) {
 			*buf++ += (tmp * (255 - panning)) >> 7;
 			*buf++ += (tmp * (panning)) >> 7;
@@ -142,7 +223,7 @@ int Paula::readBufferIntern(int16 *buffer, const int numSamples) {
 			// by the OS/2 version of Hopkins FBI.
 
 			// Mix the generated samples into the output buffer
-			neededSamples -= mixBuffer<stereo>(p, ch.data, ch.offset, rate, neededSamples, ch.length, ch.volume, ch.panning);
+			neededSamples -= mixBuffer<stereo>(p, ch.data, ch.offset, rate, neededSamples, ch.length, ch.volume, ch.panning, _filterState, voice);
 
 			// Wrap around if necessary
 			if (ch.offset.int_off >= ch.length) {
@@ -164,7 +245,7 @@ int Paula::readBufferIntern(int16 *buffer, const int numSamples) {
 				// Repeat as long as necessary.
 				while (neededSamples > 0) {
 					// Mix the generated samples into the output buffer
-					neededSamples -= mixBuffer<stereo>(p, ch.data, ch.offset, rate, neededSamples, ch.length, ch.volume, ch.panning);
+					neededSamples -= mixBuffer<stereo>(p, ch.data, ch.offset, rate, neededSamples, ch.length, ch.volume, ch.panning, _filterState, voice);
 
 					if (ch.offset.int_off >= ch.length) {
 						// Wrap around. See also the note above.
@@ -182,6 +263,32 @@ int Paula::readBufferIntern(int16 *buffer, const int numSamples) {
 	return numSamples;
 }
 
+void Paula::filterResetState() {
+	for (int i = 0; i < NUM_VOICES; i++)
+		for (int j = 0; j < 5; j++)
+			_filterState.rc[i][j] = 0.0f;
+}
+
+/* Based on UAE.
+ * Original comment in UAE:
+ *
+ * This computes the 1st order low-pass filter term b0.
+ * The a1 term is 1.0 - b0. The center frequency marks the -3 dB point.
+ */
+float Paula::filterCalculateA0(int rate, int cutoff) {
+	float omega;
+	/* The BLT correction formula below blows up if the cutoff is above nyquist. */
+	if (cutoff >= rate / 2)
+		return 1.0;
+
+	omega = 2 * M_PI * cutoff / rate;
+	/* Compensate for the bilinear transformation. This allows us to specify the
+	 * stop frequency more exactly, but the filter becomes less steep further
+	 * from stopband. */
+	omega = tan(omega / 2) * 2;
+	return 1 / (1 + 1 / omega);
+}
+
 } // End of namespace Audio
 
 
diff --git a/audio/mods/paula.h b/audio/mods/paula.h
index a1c3182c2c..93d683064a 100644
--- a/audio/mods/paula.h
+++ b/audio/mods/paula.h
@@ -46,6 +46,12 @@ public:
 		kNtscPaulaClock  = kNtscSystemClock / 2
 	};
 
+	enum FilterMode {
+		kFilterModeNone = 0,
+		kFilterModeA500,
+		kFilterModeA1200
+	};
+
 	/* TODO: Document this */
 	struct Offset {
 		uint	int_off;	// integral part of the offset
@@ -54,7 +60,16 @@ public:
 		explicit Offset(int off = 0) : int_off(off), rem_off(0) {}
 	};
 
-	Paula(bool stereo = false, int rate = 44100, uint interruptFreq = 0);
+	struct FilterState {
+		FilterMode mode;
+		bool ledFilter;
+
+		float a0[3];
+		float rc[NUM_VOICES][5];
+	};
+
+	Paula(bool stereo = false, int rate = 44100, uint interruptFreq = 0,
+	      FilterMode filterMode = kFilterModeA1200);
 	~Paula();
 
 	bool playing() const { return _playing; }
@@ -70,7 +85,7 @@ public:
 	}
 	void clearVoice(byte voice);
 	void clearVoices() { for (int i = 0; i < NUM_VOICES; ++i) clearVoice(i); }
-	void startPlay() { _playing = true; }
+	void startPlay() { filterResetState(); _playing = true; }
 	void stopPlay() { _playing = false; }
 	void pausePlay(bool pause) { _playing = !pause; }
 
@@ -184,7 +199,7 @@ protected:
 	}
 
 	void setAudioFilter(bool enable) {
-		// TODO: implement
+		_filterState.ledFilter = enable;
 	}
 
 private:
@@ -198,8 +213,13 @@ private:
 	uint32 _timerBase;
 	bool _playing;
 
+	FilterState _filterState;
+
 	template<bool stereo>
 	int readBufferIntern(int16 *buffer, const int numSamples);
+
+	void filterResetState();
+	float filterCalculateA0(int rate, int cutoff);
 };
 
 } // End of namespace Audio