music5000: implement output filtering.

See: https://stardot.org.uk/forums/viewtopic.php?p=403375#p403375

src/config.c

@@ -12,6 +12,7 @@
 #include "model.h"
 #include "mouse.h"
 #include "mmccard.h"
+#include "music5000.h"
 #include "ide.h"
 #include "midi.h"
 #include "scsi.h"
@@ -169,6 +170,7 @@ void config_load(void)
     sound_tape       = get_config_bool("sound", "sndtape",       false);
     sound_filter     = get_config_bool("sound", "soundfilter",   true);
     sound_paula      = get_config_bool("sound", "soundpaula",    false);
+    music5000_fno    = get_config_int("sound", "music5000_filter", 0);
 
     curwave          = get_config_int("sound", "soundwave",     0);
     sidmethod        = get_config_int("sound", "sidmethod",     0);
@@ -313,6 +315,7 @@ void config_save(void)
         set_config_bool("sound", "sndtape",     sound_tape);
         set_config_bool("sound", "soundfilter", sound_filter);
         set_config_bool("sound", "soundpaula",  sound_paula);
+        set_config_int("sound", "music5000_filter", music5000_fno);
 
         set_config_int("sound", "soundwave", curwave);
         set_config_int("sound", "sidmethod", sidmethod);

src/gui-allegro.c

@@ -406,6 +406,7 @@ static ALLEGRO_MENU *create_sid_menu(void)
 static const char *wave_names[] = { "Square", "Saw", "Sine", "Triangle", "SID", NULL };
 static const char *dd_type_names[] = { "5.25\"", "3.5\"", NULL };
 static const char *dd_noise_vols[] = { "33%", "66%", "100%", NULL };
+static const char *filt_freq[] = { "Original (3214Hz)", "16kHz", NULL };
 
 static ALLEGRO_MENU *create_sound_menu(void)
 {
@@ -414,6 +415,9 @@ static ALLEGRO_MENU *create_sound_menu(void)
     add_checkbox_item(menu, "Internal sound chip",   IDM_SOUND_INTERNAL,  sound_internal);
     add_checkbox_item(menu, "BeebSID",               IDM_SOUND_BEEBSID,   sound_beebsid);
     add_checkbox_item(menu, "Music 5000",            IDM_SOUND_MUSIC5000, sound_music5000);
+    sub = al_create_menu();
+    add_radio_set(sub, filt_freq, IDM_SOUND_MFILT, music5000_fno);
+    al_append_menu_item(menu, "Music 5000 Filter", 0, 0, NULL, sub);
     add_checkbox_item(menu, "Paula",                 IDM_SOUND_PAULA,     sound_paula);
     add_checkbox_item(menu, "Printer port DAC",      IDM_SOUND_DAC,       sound_dac);
     add_checkbox_item(menu, "Disc drive noise",      IDM_SOUND_DDNOISE,   sound_ddnoise);
@@ -1379,6 +1383,9 @@ void gui_allegro_event(ALLEGRO_EVENT *event)
         case IDM_SOUND_MUSIC5000:
             sound_music5000 = !sound_music5000;
             break;
+        case IDM_SOUND_MFILT:
+            music5000_fno = radio_event_simple(event, music5000_fno);
+            break;
         case IDM_SOUND_PAULA:
             sound_paula = !sound_paula;
             break;

src/gui-allegro.h

@@ -66,6 +66,7 @@ typedef enum {
     IDM_SOUND_INTERNAL,
     IDM_SOUND_BEEBSID,
     IDM_SOUND_MUSIC5000,
+    IDM_SOUND_MFILT,
     IDM_SOUND_PAULA,
     IDM_SOUND_DAC,
     IDM_SOUND_DDNOISE,

src/music5000.c

@@ -351,7 +351,49 @@ static void fput_samples(FILE *fp, int sl, int sr)
     }
 }
 
-static void music5000_get_sample(int16_t *left, int16_t *right)
+typedef struct _m5000_fcoeff {
+    double biquada[3];
+    double biquadb[3];
+    double gain;
+} m5000_fcoeff;
+
+#define NBQ 2
+
+static const m5000_fcoeff m500_filters[2] = {
+    { // original.
+        {0.6545294918791053,-1.503352371060256,-0.640959826975052},
+        {1,2,1},
+        147.38757472209932
+    },
+    { // 16Khz.
+        {0.40854522701892754,0.7646729121849647,0.29507476655875625},
+        {1,2,1},
+        2.8424433902604673
+    }
+};
+
+static double xyv_l[]={0,0,0,0,0,0,0,0,0};
+static double xyv_r[]={0,0,0,0,0,0,0,0,0};
+int music5000_fno;
+
+static double applyfilter(const m5000_fcoeff *fcp, double *xyv, double v)
+{
+	int i,b,xp=0,yp=3,bqp=0;
+	double out=v/fcp->gain;
+	for (i=8; i>0; i--) {xyv[i]=xyv[i-1];}
+	for (b=0; b<NBQ; b++)
+	{
+		int len=(b==NBQ-1)?1:2;
+		xyv[xp]=out;
+		for(i=0; i<len; i++) { out+=xyv[xp+len-i]*fcp->biquadb[bqp+i]-xyv[yp+len-i]*fcp->biquada[bqp+i]; }
+		bqp+=len;
+		xyv[yp]=out;
+		xp=yp; yp+=len+1;
+	}
+	return out;
+}
+
+static void music5000_get_sample(int16_t *left, int16_t *right, const m5000_fcoeff *fcp)
 {
     int clip;
     static int divisor = 1;
@@ -367,8 +409,9 @@ static void music5000_get_sample(int16_t *left, int16_t *right)
     static int window = FREQ_M5 * 30;
 #endif
 
-    int sl = m5000.sleft  + m3000.sleft;
-    int sr = m5000.sright + m3000.sright;
+    int sl = applyfilter(fcp, xyv_l, (double)(m5000.sleft  + m3000.sleft) / 262144.0) * 262144;
+    int sr = applyfilter(fcp, xyv_r, (double)(m5000.sright + m3000.sright) / 262144.0) * 262144;
+
     fput_samples(music5000_fp, sl, sr);
 
 #ifdef LOG_LEVELS
@@ -441,13 +484,13 @@ static void music5000_get_sample(int16_t *left, int16_t *right)
 }
 
 // Music 5000 runs at a sample rate of 6MHz / 128 = 46875
-static void music5000_fillbuf(int16_t *buffer, int len) {
+static void music5000_fillbuf(int16_t *buffer, int len, const m5000_fcoeff *fcp) {
     int sample;
     int16_t *bufptr = buffer;
     for (sample = 0; sample < len; sample++) {
         update_channels(&m5000);
         update_channels(&m3000);
-        music5000_get_sample(bufptr, bufptr + 1);
+        music5000_get_sample(bufptr, bufptr + 1, fcp);
         bufptr += 2;
     }
 }
@@ -463,7 +506,7 @@ void music5000_streamfrag(void)
 
     if (sound_music5000) {
         if ((buf = al_get_audio_stream_fragment(stream))) {
-            music5000_fillbuf(buf, BUFLEN_M5);
+            music5000_fillbuf(buf, BUFLEN_M5, &m500_filters[music5000_fno]);
             al_set_audio_stream_fragment(stream, buf);
             al_set_audio_stream_playing(stream, true);
         }

src/music5000.h

@@ -13,5 +13,6 @@ FILE *music5000_rec_start(const char *fn);
 void music5000_rec_stop(void);
 
 extern FILE *music5000_fp;
+extern int music5000_fno;
 
 #endif