Improve band-pass filter

src/main.c

@@ -23,7 +23,7 @@
 #include <stdlib.h>
 
 #define VREG_VOLTAGE VREG_VOLTAGE_1_20
-#define CLK_SYS_HZ (300 * MHZ)
+#define CLK_SYS_HZ (288 * MHZ)
 
 #define INIT_SAMPLE_RATE 200000
 #define INIT_FREQ 94600000
@@ -43,23 +43,21 @@
 #define ADC_RATE (2 * MHZ)
 #define DECIMATE 4
 
-/*
- * NOTE: Must have 256 phases with 256 bytes each.
- *       Otherwise the DMA 1-byte write trick wouldn't work.
- */
-
-#define LO_NUM_PHASES 256
-#define LO_PHASE_BITS 8
-#define LO_PHASE_WORDS (1 << (LO_PHASE_BITS - 2))
+#define NCO_NUM_PHASES (1 << 8)
+#define NCO_PHASE_BITS 8
+#define NCO_PHASE_WORDS (1 << (NCO_PHASE_BITS - 2))
 #define STEP_BASE ((UINT_MAX + 1.0) / CLK_SYS_HZ)
 
-static uint32_t nco_step = (uint32_t)(STEP_BASE * INIT_FREQ) * 32 * LO_PHASE_WORDS;
+static uint32_t nco_step = (uint32_t)(STEP_BASE * INIT_FREQ) * 32 * NCO_PHASE_WORDS;
 static uint32_t nco_null = 0;
+static uint32_t nco_mask = (1 << NCO_PHASE_BITS) - 1;
 
-static uint32_t lo_phase[LO_NUM_PHASES][LO_PHASE_WORDS]
-	__attribute__((__aligned__(LO_NUM_PHASES * 4 * LO_PHASE_WORDS)));
+static uint32_t nco_phase[NCO_NUM_PHASES][NCO_PHASE_WORDS]
+	__attribute__((__aligned__(NCO_NUM_PHASES * 4 * NCO_PHASE_WORDS)));
 
-static uint32_t nco_addr = (uint32_t)lo_phase;
+static_assert(sizeof(nco_phase) == 65536, "sizeof(nco_phase) == 65536");
+
+static uint32_t *nco_addr = &nco_phase[0][0];
 
 #define NUM_GAINS 29
 static int gains[NUM_GAINS] = { 0,   9,	  14,  27,  37,	 77,  87,  125, 144, 157,
@@ -72,6 +70,7 @@ static int frequency = INIT_FREQ;
 static int dma_ch_nco1 = -1;
 static int dma_ch_nco2 = -1;
 static int dma_ch_nco3 = -1;
+static int dma_ch_nco4 = -1;
 static int dma_ch_mix = -1;
 
 static queue_t iq_queue;
@@ -133,7 +132,7 @@ static void init_lo()
 	pio_sm_exec_wait_blocking(PIO, SM_LO, pio_encode_set(pio_pins, 0));
 }
 
-static void lo_generate_phase(uint32_t *buf, size_t len, uint32_t step, uint32_t phase)
+static void nco_generate_phase(uint32_t *buf, size_t len, uint32_t step, uint32_t phase)
 {
 	for (size_t i = 0; i < len; i++) {
 		uint32_t bits = 0;
@@ -152,10 +151,11 @@ static void rx_lo_init(double freq)
 {
 	uint32_t step = STEP_BASE * freq;
 
-	for (uint32_t i = 0; i < LO_NUM_PHASES; i++)
-		lo_generate_phase(lo_phase[i], LO_PHASE_WORDS, step, i << 24);
+	for (uint32_t i = 0; i < NCO_NUM_PHASES; i++)
+		nco_generate_phase(nco_phase[i], NCO_PHASE_WORDS, step,
+				   i << (__builtin_clz(NCO_NUM_PHASES) + 1));
 
-	nco_step = step * 32 * LO_PHASE_WORDS;
+	nco_step = step * 32 * NCO_PHASE_WORDS;
 }
 
 static void rf_rx_start()
@@ -163,6 +163,7 @@ static void rf_rx_start()
 	dma_ch_nco1 = dma_claim_unused_channel(true);
 	dma_ch_nco2 = dma_claim_unused_channel(true);
 	dma_ch_nco3 = dma_claim_unused_channel(true);
+	dma_ch_nco4 = dma_claim_unused_channel(true);
 	dma_ch_mix = dma_claim_unused_channel(true);
 
 	dma_channel_config dma_conf;
@@ -180,20 +181,31 @@ static void rf_rx_start()
 
 	/* Prepare the phase address. */
 	dma_conf = dma_channel_get_default_config(dma_ch_nco2);
-	channel_config_set_transfer_data_size(&dma_conf, DMA_SIZE_8);
+	channel_config_set_transfer_data_size(&dma_conf, DMA_SIZE_16);
 	channel_config_set_read_increment(&dma_conf, false);
 	channel_config_set_write_increment(&dma_conf, false);
 	channel_config_set_chain_to(&dma_conf, dma_ch_nco3);
-	dma_channel_configure(dma_ch_nco2, &dma_conf, (uint8_t *)(&nco_addr) + 1,
-			      ((uint8_t *)&dma_hw->sniff_data) + 3, 1, false);
+	dma_channel_configure(dma_ch_nco2, &dma_conf, (void *)(&nco_addr) + 0,
+			      ((void *)&dma_hw->sniff_data) + 2, 1, false);
 
-	/* Trigger LO using the address. */
+	/* Copy it to the DMA. */
 	dma_conf = dma_channel_get_default_config(dma_ch_nco3);
 	channel_config_set_transfer_data_size(&dma_conf, DMA_SIZE_32);
 	channel_config_set_read_increment(&dma_conf, false);
 	channel_config_set_write_increment(&dma_conf, false);
-	dma_channel_configure(dma_ch_nco3, &dma_conf, &dma_hw->ch[dma_ch_mix].al3_read_addr_trig,
-			      &nco_addr, 1, false);
+	channel_config_set_chain_to(&dma_conf, dma_ch_nco4);
+	dma_channel_configure(dma_ch_nco3, &dma_conf, &dma_hw->ch[dma_ch_mix].read_addr, &nco_addr,
+			      1, false);
+
+	/* Trigger LO by clearing the bottom bits. */
+	dma_conf = dma_channel_get_default_config(dma_ch_nco4);
+	channel_config_set_transfer_data_size(&dma_conf, DMA_SIZE_32);
+	channel_config_set_read_increment(&dma_conf, false);
+	channel_config_set_write_increment(&dma_conf, false);
+	dma_channel_configure(dma_ch_nco4, &dma_conf,
+			      (void *)&dma_hw->ch[dma_ch_mix].al3_read_addr_trig +
+				      REG_ALIAS_CLR_BITS,
+			      &nco_mask, 1, false);
 
 	/* Drive the LO capacitor. */
 	dma_conf = dma_channel_get_default_config(dma_ch_mix);
@@ -202,8 +214,8 @@ static void rf_rx_start()
 	channel_config_set_write_increment(&dma_conf, false);
 	channel_config_set_dreq(&dma_conf, pio_get_dreq(PIO, SM_LO, GPIO_OUT));
 	channel_config_set_chain_to(&dma_conf, dma_ch_nco1);
-	dma_channel_configure(dma_ch_mix, &dma_conf, &PIO->txf[SM_LO], lo_phase, LO_PHASE_WORDS,
-			      false);
+	dma_channel_configure(dma_ch_mix, &dma_conf, &PIO->txf[SM_LO], &nco_phase[0][0],
+			      NCO_PHASE_WORDS, false);
 
 	init_lo();
 
@@ -221,26 +233,31 @@ static void rf_rx_stop(void)
 	dma_channel_clear_chain_to(dma_ch_nco1);
 	dma_channel_clear_chain_to(dma_ch_nco2);
 	dma_channel_clear_chain_to(dma_ch_nco3);
+	dma_channel_clear_chain_to(dma_ch_nco4);
 	dma_channel_clear_chain_to(dma_ch_mix);
 
 	dma_channel_abort(dma_ch_nco1);
 	dma_channel_abort(dma_ch_nco2);
 	dma_channel_abort(dma_ch_nco3);
+	dma_channel_abort(dma_ch_nco4);
 	dma_channel_abort(dma_ch_mix);
 
 	dma_channel_cleanup(dma_ch_nco1);
 	dma_channel_cleanup(dma_ch_nco2);
 	dma_channel_cleanup(dma_ch_nco3);
+	dma_channel_cleanup(dma_ch_nco4);
 	dma_channel_cleanup(dma_ch_mix);
 
 	dma_channel_unclaim(dma_ch_nco1);
 	dma_channel_unclaim(dma_ch_nco2);
 	dma_channel_unclaim(dma_ch_nco3);
+	dma_channel_unclaim(dma_ch_nco4);
 	dma_channel_unclaim(dma_ch_mix);
 
 	dma_ch_nco1 = -1;
 	dma_ch_nco2 = -1;
 	dma_ch_nco3 = -1;
+	dma_ch_nco4 = -1;
 	dma_ch_mix = -1;
 }
 
@@ -250,18 +267,17 @@ struct IQ {
 
 inline static int nextQ(void)
 {
-	static int x1;
-	static int y1;
+	static int x4, x3, x2, x1;
 
 	int x0 = gain * adc_fifo_get_blocking();
 
-	int y0 = x0 + x1;
+	int x = x2 + x2 - x4 - x0;
+	x4 = x3;
+	x3 = x2;
+	x2 = x1;
 	x1 = x0;
 
-	int y = y0 - y1;
-	y1 = y0;
-
-	return y;
+	return x;
 }
 
 inline static struct IQ next_sample()
@@ -273,6 +289,14 @@ inline static struct IQ next_sample()
 	I -= nextQ();
 	Q -= nextQ();
 
+	static int dcI, dcQ;
+
+	I = ((I << 12) - dcI) >> 12;
+	dcI += I;
+
+	Q = ((Q << 12) - dcQ) >> 12;
+	dcQ += Q;
+
 	I += 127.4 * 512;
 	I /= 512;