ax/adau: introduce programmable lowpass; mixer volumes

ZZ9000_proto.sdk/ZZ9000OS/src/adau.h

@@ -34,19 +34,39 @@ ADI_REG_TYPE Program_Data_IC_1[PROGRAM_SIZE_IC_1] = {
 0x00, 0x00, 0x00, 0x00, 0x01,
 0x00, 0x08, 0x00, 0xE8, 0x01,
 0x00, 0x00, 0x00, 0x00, 0x01,
-0x00, 0x10, 0x00, 0xE8, 0x01,
-0x00, 0x00, 0x00, 0x00, 0x01,
-0x00, 0x18, 0x00, 0xE8, 0x01,
-0x00, 0x02, 0x00, 0x20, 0x01,
-0x00, 0x0A, 0x00, 0x40, 0x01,
-0x00, 0x12, 0x01, 0x22, 0x01,
-0x00, 0x1A, 0x01, 0x44, 0x01,
-0x00, 0x00, 0x00, 0x00, 0x01,
-0x00, 0x20, 0x00, 0xE2, 0x01,
-0x00, 0x28, 0x00, 0xE4, 0x01,
-0x00, 0x21, 0x08, 0x20, 0x01,
+0x00, 0x20, 0x00, 0xE8, 0x01,
+0x00, 0x00, 0x00, 0x00, 0x01,
+0x00, 0x38, 0x00, 0xE8, 0x01,
+0x00, 0x62, 0x03, 0x20, 0x01,
+0x00, 0x5A, 0x04, 0x22, 0x01,
+0x00, 0x4A, 0x03, 0x34, 0x01,
+0x00, 0x42, 0x04, 0x22, 0x01,
+0x00, 0x22, 0x00, 0x22, 0x01,
+0x00, 0x1A, 0x01, 0x22, 0x01,
+0x00, 0x12, 0x02, 0x22, 0x01,
+0x00, 0x00, 0x00, 0x00, 0x01,
+0x00, 0x50, 0x00, 0xE2, 0x01,
+0x00, 0x68, 0x00, 0xF2, 0x01,
+0x00, 0x92, 0x03, 0x20, 0x01,
+0x00, 0x8A, 0x04, 0x22, 0x01,
+0x00, 0x7A, 0x03, 0x34, 0x01,
+0x00, 0x72, 0x04, 0x22, 0x01,
+0x00, 0x3A, 0x00, 0x22, 0x01,
+0x00, 0x32, 0x01, 0x22, 0x01,
+0x00, 0x2A, 0x02, 0x22, 0x01,
+0x00, 0x00, 0x00, 0x00, 0x01,
+0x00, 0x80, 0x00, 0xE2, 0x01,
+0x00, 0x98, 0x00, 0xF2, 0x01,
+0x00, 0x02, 0x05, 0x20, 0x01,
+0x00, 0x0A, 0x05, 0x40, 0x01,
+0x00, 0x52, 0x06, 0x22, 0x01,
+0x00, 0x82, 0x06, 0x44, 0x01,
+0x00, 0x00, 0x00, 0x00, 0x01,
+0x00, 0xA0, 0x00, 0xE2, 0x01,
+0x00, 0xA8, 0x00, 0xE4, 0x01,
+0x00, 0xA1, 0x08, 0x20, 0x01,
 0xFF, 0x68, 0x00, 0x02, 0x01,
-0x00, 0x29, 0x08, 0x20, 0x01,
+0x00, 0xA9, 0x08, 0x20, 0x01,
 0xFF, 0x70, 0x00, 0x02, 0x01,
 0x00, 0x00, 0x00, 0x00, 0x01,
 0x00, 0x00, 0x00, 0x00, 0x01,
@@ -1033,26 +1053,6 @@ ADI_REG_TYPE Program_Data_IC_1[PROGRAM_SIZE_IC_1] = {
 0x00, 0x00, 0x00, 0x00, 0x01,
 0x00, 0x00, 0x00, 0x00, 0x01,
 0x00, 0x00, 0x00, 0x00, 0x01,
-0x00, 0x00, 0x00, 0x00, 0x01,
-0x00, 0x00, 0x00, 0x00, 0x01,
-0x00, 0x00, 0x00, 0x00, 0x01,
-0x00, 0x00, 0x00, 0x00, 0x01,
-0x00, 0x00, 0x00, 0x00, 0x01,
-0x00, 0x00, 0x00, 0x00, 0x01,
-0x00, 0x00, 0x00, 0x00, 0x01,
-0x00, 0x00, 0x00, 0x00, 0x01,
-0x00, 0x00, 0x00, 0x00, 0x01,
-0x00, 0x00, 0x00, 0x00, 0x01,
-0x00, 0x00, 0x00, 0x00, 0x01,
-0x00, 0x00, 0x00, 0x00, 0x01,
-0x00, 0x00, 0x00, 0x00, 0x01,
-0x00, 0x00, 0x00, 0x00, 0x01,
-0x00, 0x00, 0x00, 0x00, 0x01,
-0x00, 0x00, 0x00, 0x00, 0x01,
-0x00, 0x00, 0x00, 0x00, 0x01,
-0x00, 0x00, 0x00, 0x00, 0x01,
-0x00, 0x00, 0x00, 0x00, 0x01,
-0x00, 0x00, 0x00, 0x00, 0x01,
 };
 
 /* DSP Parameter (Coefficient) Data */
@@ -1060,12 +1060,12 @@ ADI_REG_TYPE Program_Data_IC_1[PROGRAM_SIZE_IC_1] = {
 #define PARAM_ADDR_IC_1 0
 ADI_REG_TYPE Param_Data_IC_1[PARAM_SIZE_IC_1] = {
 0x00, 0x80, 0x00, 0x00,
+0x01, 0x00, 0x00, 0x00,
+0x00, 0x80, 0x00, 0x00,
+0x0F, 0x00, 0x00, 0x00,
+0x0F, 0x80, 0x00, 0x00,
+0x00, 0x80, 0x00, 0x00,
 0x00, 0x80, 0x00, 0x00,
-0x00, 0x00, 0x00, 0x00,
-0x00, 0x00, 0x00, 0x00,
-0x00, 0x00, 0x00, 0x00,
-0x00, 0x00, 0x00, 0x00,
-0x00, 0x00, 0x00, 0x00,
 0x00, 0x00, 0x00, 0x00,
 0x00, 0x00, 0x00, 0x00,
 0x00, 0x00, 0x00, 0x00,

ZZ9000_proto.sdk/ZZ9000OS/src/ax.c

@@ -14,6 +14,8 @@
 #include "ax.h"
 #include "memorymap.h"
 #include "xtime_l.h"
+#include "math.h"
+#include "ax.h"
 
 #define IIC2_DEVICE_ID	XPAR_XIICPS_1_DEVICE_ID
 #define IIC2_SCLK_RATE	100000
@@ -187,18 +189,21 @@ int adau_read24(u8 i2c_addr, u16 addr, u8* buffer) {
 	return status1;
 }
 
-void program_adau(u8* program, u32 program_len, u8* params, u32 param_len) {
-	for (u32 i = 0; i < program_len; i+=5) {
-		int res = adau_write40(0x34, 1024+i/5, &program[i]);
-		if (res != 0) printf("[adau_write40] %lx: %d\n", i, res);
-	}
 
+void audio_program_adau_params(u8* params, u32 param_len) {
 	for (u32 i = 0; i < param_len; i+=4) {
 		int res = adau_write32(0x34, 0+i/4, &params[i]);
 		if (res != 0) printf("[adau_write32] %lx: %d\n", i, res);
 	}
 }
 
+void audio_program_adau(u8* program, u32 program_len) {
+	for (u32 i = 0; i < program_len; i+=5) {
+		int res = adau_write40(0x34, 1024+i/5, &program[i]);
+		if (res != 0) printf("[adau_write40] %lx: %d\n", i, res);
+	}
+}
+
 void audio_init_i2s() {
 	XI2stx_Config* i2s_config = XI2s_Tx_LookupConfig(XPAR_XI2STX_0_DEVICE_ID);
 	int status = XI2s_Tx_CfgInitialize(&i2s, i2s_config, i2s_config->BaseAddress);
@@ -271,7 +276,6 @@ void audio_init_i2s() {
 	XI2s_Rx_Enable(&i2srx, 1);
 	XAudioFormatterDMAStart(&audio_formatter_rx);
 
-
 	printf("[adau] XAudioFormatter_InterruptEnable...\n");
 
 	XAudioFormatter_InterruptEnable(&audio_formatter, 1<<13); // IOC
@@ -333,10 +337,6 @@ int audio_adau_init(int program_dsp) {
 	status = adau_read16(i, 2087, rbuf);
 	printf("[adau] read from 2087: %02x%02x (status: %d)\n", rbuf[0], rbuf[1], status);
 
-	if (program_dsp) {
-		program_adau(Program_Data_IC_1, sizeof(Program_Data_IC_1), Param_Data_IC_1, sizeof(Param_Data_IC_1));
-	}
-
 	// TODO: OBP/OLRP
 	u16 MS  = 1<<11; // clock master output
 	//u16 OBF = (0<<10)|(0<<9);    // bclock = 49.152/16 = mclk/4 = 3.072mhz
@@ -372,6 +372,13 @@ int audio_adau_init(int program_dsp) {
 
 	audio_init_i2s();
 
+	if (program_dsp) {
+		audio_program_adau(Program_Data_IC_1, sizeof(Program_Data_IC_1));
+		audio_program_adau_params(Param_Data_IC_1, sizeof(Param_Data_IC_1));
+		audio_adau_set_lpf_params(23900);
+		audio_adau_set_mixer_vol(128, 64);
+	}
+
 	return 1;
 }
 
@@ -553,3 +560,99 @@ void audio_silence() {
 	memset(audio_tx_buffer, 0, AUDIO_TX_BUFFER_SIZE);
 	reset_resampling();
 }
+
+// sources:
+// https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html
+// https://wiki.analog.com/resources/tools-software/sigmastudio/usingsigmastudio/systemimplementation
+// https://ez.analog.com/dsp/sigmadsp/f/q-a/104470/nth-order-filter-coefficient-calculations
+// https://wiki.analog.com/resources/tools-software/sigmastudio/toolbox/filters/general2ndorder
+// https://ez.analog.com/dsp/sigmadsp/f/q-a/65510/parameters-with-adau1701
+
+void adau_to_5_23(double param_dec, uint8_t* param_hex) {
+	long param223;
+	long param227;
+
+	// multiply decimal number by 2^23
+	param223 = param_dec * (1 << 23);
+
+	// convert to positive binary
+	param227 = param223 + (1 << 27);
+
+	param_hex[3] = (uint8_t) param227;
+	param_hex[2] = (uint8_t) (param227 >> 8);
+	param_hex[1] = (uint8_t) (param227 >> 16);
+	param_hex[0] = (uint8_t) (param227 >> 24);
+
+	// invert sign bit to get correct sign
+	param_hex[0] = param_hex[0] ^ 0x08;
+}
+
+double flt_omega(double fs, double f0) {
+	return 2.0 * M_PI * (f0 / fs);
+}
+
+double flt_alpha(double fs, double f0) {
+	double omega = flt_omega(fs, f0);
+	double Q = 1.0 / sqrt(2.0);
+	return sin(omega) / (2.0 * Q);
+}
+
+void audio_adau_set_lpf_params(int f0) {
+	double gain = 1; // FIXME unused
+	int fs = 48000;
+
+	printf("[lpf] f0: %d\n", f0);
+
+	double omega = flt_omega(fs, f0);
+	double alpha = flt_alpha(fs, f0);
+
+	double a0 = 1.0 + alpha;
+	double a1 = -2.0 * cos(omega);
+	double a2 = 1.0 - alpha;
+	double b0 = (1.0 - cos(omega)) / 2.0;
+	double b1 = 1.0 - cos(omega);
+	double b2 = b0;
+
+	a1 /= a0;
+	a2 /= a0;
+	b0 /= a0;
+	b1 /= a0;
+	b2 /= a0;
+
+	a1 = -a1;
+	a2 = -a2;
+
+	uint8_t buf[4];
+
+	adau_to_5_23(b0, buf);
+	adau_write32(0x34, 0, buf);
+	printf("[lpf] b0: %f\t%02x %02x %02x %02x\n", b0, buf[0], buf[1], buf[2], buf[3]);
+	adau_to_5_23(b1, buf);
+	adau_write32(0x34, 1, buf);
+	printf("[lpf] b1: %f\t%02x %02x %02x %02x\n", b1, buf[0], buf[1], buf[2], buf[3]);
+	adau_to_5_23(b2, buf);
+	adau_write32(0x34, 2, buf);
+	printf("[lpf] b2: %f\t%02x %02x %02x %02x\n", b2, buf[0], buf[1], buf[2], buf[3]);
+	adau_to_5_23(a1, buf);
+	adau_write32(0x34, 3, buf);
+	printf("[lpf] a1: %f\t%02x %02x %02x %02x\n", a1, buf[0], buf[1], buf[2], buf[3]);
+	adau_to_5_23(a2, buf);
+	adau_write32(0x34, 4, buf);
+	printf("[lpf] a2: %f\t%02x %02x %02x %02x\n\n", a2, buf[0], buf[1], buf[2], buf[3]);
+}
+
+// vol range: 0-255. 127 = 0db
+// vol1: paula
+// vol2: i2s
+void audio_adau_set_mixer_vol(int vol1, int vol2) {
+	double v1 = ((double)vol1)/127.0;
+	double v2 = ((double)vol2)/127.0;
+
+	printf("[vol] v1: %f v2: %f\n", v1, v2);
+
+	uint8_t buf[4];
+	adau_to_5_23(v1, buf);
+	adau_write32(0x34, 5, buf);
+	adau_to_5_23(v2, buf);
+	adau_write32(0x34, 6, buf);
+}

ZZ9000_proto.sdk/ZZ9000OS/src/ax.h

@@ -13,5 +13,12 @@ void audio_set_tx_buffer(uint8_t* addr);
 void audio_set_rx_buffer(uint8_t* addr);
 void resample_s16(int16_t *input, int16_t *output,
 		int in_sample_rate, int out_sample_rate, int output_samples);
-void audio_set_filter_param(uint32_t zdata);
 void audio_silence();
+void audio_debug_timer(int zdata);
+
+void audio_program_adau(u8* program, u32 program_len);
+void audio_program_adau_params(u8* params, u32 param_len);
+void audio_adau_set_lpf_params(int f0);
+
+// vol range: 0-100. 50 = 0db
+void audio_adau_set_mixer_vol(int vol1, int vol2);