Implement full-blown NCO using DMA sniffer

src/main.c

@@ -24,45 +24,51 @@
 #define VREG_VOLTAGE VREG_VOLTAGE_1_20
 #define CLK_SYS_HZ (288 * MHZ)
 
+#define INIT_SAMPLE_RATE 100000
+#define INIT_FREQ 94600000
+#define INIT_GAIN 127
+
 #define LO_PIN 9
 #define RX_PIN 13
 #define FB_PIN 5
 #define PSU_PIN 23
 
 #define PIO pio1
-#define LO_SM 0
-#define FB_SM 1
-#define RX_SM 2
-#define AD_SM 3
+#define SM_LO 0
+#define SM_FB 1
+#define SM_RX 2
+#define SM_AD 3
 
 #define IQ_SAMPLES 32
 #define IQ_BLOCK_LEN (2 * IQ_SAMPLES)
 #define IQ_QUEUE_LEN 8
 
-#define LO_NUM_PHASES 64
-#define LO_PHASE_BITS 10
+/*
+ * 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 LO_COS_PHASES 4096
-#define LO_EFFECTIVE_BITS (32 * LO_PHASE_WORDS * LO_COS_PHASES)
+#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_null = 0;
 
 static uint32_t lo_phase[LO_NUM_PHASES][LO_PHASE_WORDS]
 	__attribute__((__aligned__(LO_NUM_PHASES * 4 * LO_PHASE_WORDS)));
 
-static const uint32_t *lo_cos_phases[LO_COS_PHASES]
-	__attribute__((__aligned__(1 << LO_PHASE_BITS)));
+static uint32_t nco_addr = (uint32_t)lo_phase;
 
 #define DECIMATE 16
-#define RX_STRIDE (2 * IQ_SAMPLES * DECIMATE)
-#define RX_BITS_DEPTH 14
+#define RX_BITS_DEPTH 10
 #define RX_WORDS (1 << (RX_BITS_DEPTH - 2))
 
-static_assert(RX_STRIDE * 4 <= RX_WORDS, "RX_STRIDE * 4 <= RX_WORDS");
-
 static uint32_t rx_cos[RX_WORDS] __attribute__((__aligned__(1 << RX_BITS_DEPTH)));
 
-#define INIT_SAMPLE_RATE 100000
-#define INIT_FREQ 94600000
-#define INIT_GAIN 127
+static const uint32_t *rx_start = rx_cos;
+static const uint32_t *rx_end = rx_cos + RX_WORDS - 1;
 
 #define NUM_GAINS 29
 static int gains[NUM_GAINS] = { 0,   9,	  14,  27,  37,	 77,  87,  125, 144, 157,
@@ -76,8 +82,10 @@ static int frequency = INIT_FREQ;
 static int dma_ch_rx1 = -1;
 static int dma_ch_rx2 = -1;
 
-static int dma_ch_mix1 = -1;
-static int dma_ch_mix2 = -1;
+static int dma_ch_nco1 = -1;
+static int dma_ch_nco2 = -1;
+static int dma_ch_nco3 = -1;
+static int dma_ch_mix = -1;
 
 static int dma_ch_samp_cos = -1;
 
@@ -91,6 +99,11 @@ static size_t iq_queue_pos = 0;
 
 static uint32_t rnd = 0;
 
+static int origin_lo = -1;
+static int origin_rx = -1;
+static int origin_fb = -1;
+static int origin_ad = 0;
+
 inline static __unused uint32_t rnd_next()
 {
 	rnd = rnd * 0x41c64e6d + 12345;
@@ -120,26 +133,26 @@ static void init_lo()
 	pio_program_t prog = {
 		.instructions = insn,
 		.length = sizeof(insn) / sizeof(*insn),
-		.origin = -1,
+		.origin = origin_lo,
 	};
 
-	pio_sm_restart(PIO, LO_SM);
-	pio_sm_clear_fifos(PIO, LO_SM);
+	pio_sm_restart(PIO, SM_LO);
+	pio_sm_clear_fifos(PIO, SM_LO);
 
 	if (pio_can_add_program(PIO, &prog))
-		prog.origin = pio_add_program(PIO, &prog);
+		origin_lo = pio_add_program(PIO, &prog);
 
 	pio_sm_config pc = pio_get_default_sm_config();
 	sm_config_set_out_pins(&pc, LO_PIN, 1);
 	sm_config_set_set_pins(&pc, LO_PIN, 1);
-	sm_config_set_wrap(&pc, prog.origin, prog.origin + prog.length - 1);
+	sm_config_set_wrap(&pc, origin_lo, origin_lo + prog.length - 1);
 	sm_config_set_clkdiv_int_frac(&pc, 1, 0);
 	sm_config_set_fifo_join(&pc, PIO_FIFO_JOIN_TX);
 	sm_config_set_out_shift(&pc, false, true, 32);
-	pio_sm_init(PIO, LO_SM, prog.origin, &pc);
+	pio_sm_init(PIO, SM_LO, origin_lo, &pc);
 
-	pio_sm_set_consecutive_pindirs(PIO, LO_SM, LO_PIN, 1, GPIO_IN);
-	pio_sm_exec_wait_blocking(PIO, LO_SM, pio_encode_set(pio_pins, 0));
+	pio_sm_set_consecutive_pindirs(PIO, SM_LO, LO_PIN, 1, GPIO_IN);
+	pio_sm_exec_wait_blocking(PIO, SM_LO, pio_encode_set(pio_pins, 0));
 }
 
 static void init_fb()
@@ -163,14 +176,14 @@ static void init_fb()
 	pio_program_t prog = {
 		.instructions = insn,
 		.length = sizeof(insn) / sizeof(*insn),
-		.origin = -1,
+		.origin = origin_fb,
 	};
 
-	pio_sm_restart(PIO, FB_SM);
-	pio_sm_clear_fifos(PIO, FB_SM);
+	pio_sm_restart(PIO, SM_FB);
+	pio_sm_clear_fifos(PIO, SM_FB);
 
 	if (pio_can_add_program(PIO, &prog))
-		prog.origin = pio_add_program(PIO, &prog);
+		origin_fb = pio_add_program(PIO, &prog);
 
 	pio_sm_config pc = pio_get_default_sm_config();
 	sm_config_set_sideset(&pc, 1, false, true);
@@ -178,11 +191,11 @@ static void init_fb()
 	sm_config_set_out_pins(&pc, FB_PIN, 1);
 	sm_config_set_set_pins(&pc, FB_PIN, 1);
 	sm_config_set_sideset_pins(&pc, FB_PIN);
-	sm_config_set_wrap(&pc, prog.origin, prog.origin + prog.length - 1);
+	sm_config_set_wrap(&pc, origin_fb, origin_fb + prog.length - 1);
 	sm_config_set_clkdiv_int_frac(&pc, 1, 0);
-	pio_sm_init(PIO, FB_SM, prog.origin, &pc);
+	pio_sm_init(PIO, SM_FB, origin_fb, &pc);
 
-	pio_sm_set_consecutive_pindirs(PIO, FB_SM, FB_PIN, 1, GPIO_OUT);
+	pio_sm_set_consecutive_pindirs(PIO, SM_FB, FB_PIN, 1, GPIO_OUT);
 }
 
 static void init_rx()
@@ -197,26 +210,28 @@ static void init_rx()
 	pio_program_t prog = {
 		.instructions = insn,
 		.length = sizeof(insn) / sizeof(*insn),
-		.origin = -1,
+		.origin = origin_rx,
 	};
 
-	pio_sm_restart(PIO, RX_SM);
-	pio_sm_clear_fifos(PIO, RX_SM);
+	pio_sm_restart(PIO, SM_RX);
+	pio_sm_clear_fifos(PIO, SM_RX);
 
 	if (pio_can_add_program(PIO, &prog))
-		prog.origin = pio_add_program(PIO, &prog);
+		origin_rx = pio_add_program(PIO, &prog);
 
 	pio_sm_config pc = pio_get_default_sm_config();
 	sm_config_set_in_pins(&pc, RX_PIN);
-	sm_config_set_wrap(&pc, prog.origin, prog.origin + prog.length - 1);
+	sm_config_set_wrap(&pc, origin_rx, origin_rx + prog.length - 1);
 	sm_config_set_clkdiv_int_frac(&pc, 1, 0);
 	sm_config_set_fifo_join(&pc, PIO_FIFO_JOIN_RX);
 	sm_config_set_in_shift(&pc, false, true, 32);
-	pio_sm_init(PIO, RX_SM, prog.origin, &pc);
+	pio_sm_init(PIO, SM_RX, origin_rx, &pc);
 
-	pio_sm_set_consecutive_pindirs(PIO, RX_SM, RX_PIN, 1, GPIO_IN);
+	pio_sm_set_consecutive_pindirs(PIO, SM_RX, RX_PIN, 1, GPIO_IN);
 }
 
+static const uint32_t samp_insn = 16;
+
 static void init_ad()
 {
 	const uint16_t insn[] = {
@@ -253,25 +268,23 @@ static void init_ad()
 	pio_program_t prog = {
 		.instructions = insn,
 		.length = sizeof(insn) / sizeof(*insn),
-		.origin = 0,
+		.origin = origin_ad,
 	};
 
-	pio_sm_restart(PIO, AD_SM);
-	pio_sm_clear_fifos(PIO, AD_SM);
+	pio_sm_restart(PIO, SM_AD);
+	pio_sm_clear_fifos(PIO, SM_AD);
 
 	if (pio_can_add_program(PIO, &prog))
 		pio_add_program(PIO, &prog);
 
 	pio_sm_config pc = pio_get_default_sm_config();
-	sm_config_set_wrap(&pc, prog.origin, prog.origin + 15);
+	sm_config_set_wrap(&pc, origin_ad, origin_ad + 15);
 	sm_config_set_clkdiv_int_frac(&pc, 1, 0);
 	sm_config_set_in_shift(&pc, false, true, 32);
 	sm_config_set_out_shift(&pc, false, true, 32);
-	pio_sm_init(PIO, AD_SM, prog.origin, &pc);
+	pio_sm_init(PIO, SM_AD, origin_ad, &pc);
 }
 
-#define STEP_BASE ((UINT_MAX + 1.0) / CLK_SYS_HZ)
-
 static void lo_generate_phase(uint32_t *buf, size_t len, uint32_t step, uint32_t phase)
 {
 	for (size_t i = 0; i < len; i++) {
@@ -287,43 +300,28 @@ static void lo_generate_phase(uint32_t *buf, size_t len, uint32_t step, uint32_t
 	}
 }
 
-static void rx_lo_init(double req_freq, bool align)
+static void rx_lo_init(double freq)
 {
-	const double step_hz = (double)CLK_SYS_HZ / (LO_EFFECTIVE_BITS / 2.0);
-	double freq = req_freq;
-
-	if (align)
-		freq = round(freq / step_hz) * step_hz;
-
 	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 << (__builtin_clz(LO_NUM_PHASES) + 1));
+		lo_generate_phase(lo_phase[i], LO_PHASE_WORDS, step, i << 24);
 
-	uint32_t phase_step = step * 32 * LO_PHASE_WORDS;
-	uint32_t phase = 0;
-
-	for (uint32_t i = 0; i < LO_COS_PHASES; i++) {
-		lo_cos_phases[i] = lo_phase[phase >> (__builtin_clz(LO_NUM_PHASES) + 1)];
-		phase += phase_step;
-	}
+	nco_step = step * 32 * LO_PHASE_WORDS;
 }
 
-static const uint32_t samp_insn = 16;
-
 static void rf_rx_start()
 {
 	dma_ch_rx1 = dma_claim_unused_channel(true);
 	dma_ch_rx2 = dma_claim_unused_channel(true);
 
-	dma_ch_mix1 = dma_claim_unused_channel(true);
-	dma_ch_mix2 = dma_claim_unused_channel(true);
+	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_mix = dma_claim_unused_channel(true);
 
 	dma_ch_samp_cos = dma_claim_unused_channel(true);
 
-	dma_t_samp = dma_claim_unused_timer(true);
-
 	dma_channel_config dma_conf;
 
 	/* Copy PDM bitstream into decimator. */
@@ -331,41 +329,58 @@ static void rf_rx_start()
 	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);
-	channel_config_set_dreq(&dma_conf, pio_get_dreq(PIO, RX_SM, GPIO_IN));
+	channel_config_set_dreq(&dma_conf, pio_get_dreq(PIO, SM_RX, GPIO_IN));
 	channel_config_set_chain_to(&dma_conf, dma_ch_rx2);
-	dma_channel_configure(dma_ch_rx1, &dma_conf, &PIO->txf[AD_SM], &PIO->rxf[RX_SM], UINT_MAX,
+	dma_channel_configure(dma_ch_rx1, &dma_conf, &PIO->txf[SM_AD], &PIO->rxf[SM_RX], UINT_MAX,
 			      false);
 
 	dma_conf = dma_channel_get_default_config(dma_ch_rx2);
 	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);
-	channel_config_set_dreq(&dma_conf, pio_get_dreq(PIO, RX_SM, GPIO_IN));
+	channel_config_set_dreq(&dma_conf, pio_get_dreq(PIO, SM_RX, GPIO_IN));
 	channel_config_set_chain_to(&dma_conf, dma_ch_rx1);
-	dma_channel_configure(dma_ch_rx2, &dma_conf, &PIO->txf[AD_SM], &PIO->rxf[RX_SM], UINT_MAX,
+	dma_channel_configure(dma_ch_rx2, &dma_conf, &PIO->txf[SM_AD], &PIO->rxf[SM_RX], UINT_MAX,
 			      false);
 
+	/* Step the NCO. */
+	dma_conf = dma_channel_get_default_config(dma_ch_nco1);
+	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);
+	channel_config_set_chain_to(&dma_conf, dma_ch_nco2);
+	dma_channel_configure(dma_ch_nco1, &dma_conf, &nco_null, &nco_step, 1, false);
+
+	/* DMA above will increment the phase accumulator. */
+	dma_sniffer_enable(dma_ch_nco1, DMA_SNIFF_CTRL_CALC_VALUE_SUM, true);
+
+	/* 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_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);
+
+	/* Trigger LO using the address. */
+	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);
+
 	/* Drive the LO capacitor. */
-	dma_conf = dma_channel_get_default_config(dma_ch_mix1);
+	dma_conf = dma_channel_get_default_config(dma_ch_mix);
 	channel_config_set_transfer_data_size(&dma_conf, DMA_SIZE_32);
 	channel_config_set_read_increment(&dma_conf, true);
 	channel_config_set_write_increment(&dma_conf, false);
-	channel_config_set_ring(&dma_conf, GPIO_IN, LO_PHASE_BITS);
-	dma_channel_configure(dma_ch_mix1, &dma_conf, &dma_hw->ch[dma_ch_mix2].al3_read_addr_trig,
-			      lo_cos_phases, 1, false);
-
-	dma_conf = dma_channel_get_default_config(dma_ch_mix2);
-	channel_config_set_transfer_data_size(&dma_conf, DMA_SIZE_32);
-	channel_config_set_read_increment(&dma_conf, true);
-	channel_config_set_write_increment(&dma_conf, false);
-	channel_config_set_dreq(&dma_conf, pio_get_dreq(PIO, LO_SM, GPIO_OUT));
-	channel_config_set_chain_to(&dma_conf, dma_ch_mix1);
-	dma_channel_configure(dma_ch_mix2, &dma_conf, &PIO->txf[LO_SM], NULL, LO_PHASE_WORDS,
+	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);
 
-	/* Pacing timer for the sampling script trigger channel. */
-	dma_timer_set_fraction(dma_t_samp, 1, CLK_SYS_HZ / (sample_rate * DECIMATE));
-
 	/* Trigger accumulator values push. */
 	dma_conf = dma_channel_get_default_config(dma_ch_samp_cos);
 	channel_config_set_transfer_data_size(&dma_conf, DMA_SIZE_32);
@@ -373,7 +388,7 @@ static void rf_rx_start()
 	channel_config_set_write_increment(&dma_conf, false);
 	channel_config_set_high_priority(&dma_conf, true);
 	channel_config_set_dreq(&dma_conf, dma_get_timer_dreq(dma_t_samp));
-	dma_channel_configure(dma_ch_samp_cos, &dma_conf, &PIO->sm[AD_SM].instr, &samp_insn,
+	dma_channel_configure(dma_ch_samp_cos, &dma_conf, &PIO->sm[SM_AD].instr, &samp_insn,
 			      UINT_MAX, false);
 
 	init_ad();
@@ -382,7 +397,7 @@ static void rf_rx_start()
 	init_rx();
 
 	dma_channel_start(dma_ch_rx1);
-	dma_channel_start(dma_ch_mix1);
+	dma_channel_start(dma_ch_nco1);
 	dma_channel_start(dma_ch_samp_cos);
 
 	pio_set_sm_mask_enabled(PIO, 0x0f, true);
@@ -396,96 +411,136 @@ static void rf_rx_stop(void)
 
 	dma_channel_clear_chain_to(dma_ch_rx1);
 	dma_channel_clear_chain_to(dma_ch_rx2);
-	dma_channel_clear_chain_to(dma_ch_mix1);
-	dma_channel_clear_chain_to(dma_ch_mix2);
+	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_mix);
 	dma_channel_clear_chain_to(dma_ch_samp_cos);
 
 	dma_channel_abort(dma_ch_rx1);
 	dma_channel_abort(dma_ch_rx2);
-	dma_channel_abort(dma_ch_mix1);
-	dma_channel_abort(dma_ch_mix2);
+	dma_channel_abort(dma_ch_nco1);
+	dma_channel_abort(dma_ch_nco2);
+	dma_channel_abort(dma_ch_nco3);
+	dma_channel_abort(dma_ch_mix);
 	dma_channel_abort(dma_ch_samp_cos);
 
 	dma_channel_cleanup(dma_ch_rx1);
 	dma_channel_cleanup(dma_ch_rx2);
-	dma_channel_cleanup(dma_ch_mix1);
-	dma_channel_cleanup(dma_ch_mix2);
+	dma_channel_cleanup(dma_ch_nco1);
+	dma_channel_cleanup(dma_ch_nco2);
+	dma_channel_cleanup(dma_ch_nco3);
+	dma_channel_cleanup(dma_ch_mix);
 	dma_channel_cleanup(dma_ch_samp_cos);
 
 	dma_channel_unclaim(dma_ch_rx1);
 	dma_channel_unclaim(dma_ch_rx2);
-	dma_channel_unclaim(dma_ch_mix1);
-	dma_channel_unclaim(dma_ch_mix2);
+	dma_channel_unclaim(dma_ch_nco1);
+	dma_channel_unclaim(dma_ch_nco2);
+	dma_channel_unclaim(dma_ch_nco3);
+	dma_channel_unclaim(dma_ch_mix);
 	dma_channel_unclaim(dma_ch_samp_cos);
 
-	dma_timer_unclaim(dma_t_samp);
-
 	dma_ch_rx1 = -1;
 	dma_ch_rx2 = -1;
-	dma_ch_mix1 = -1;
-	dma_ch_mix2 = -1;
+	dma_ch_nco1 = -1;
+	dma_ch_nco2 = -1;
+	dma_ch_nco3 = -1;
+	dma_ch_mix = -1;
 	dma_ch_samp_cos = -1;
-
-	dma_t_samp = -1;
 }
 
 struct IQ {
 	int I, Q;
 };
 
-inline static struct IQ next_sample(const uint32_t *buf)
+inline static int get_next_sample()
 {
-	int x15 = gain * (-2 * buf[0] - buf[1] - dc_level);
-	int x14 = gain * (-2 * buf[2] - buf[3] - dc_level);
-	int x13 = gain * (-2 * buf[4] - buf[5] - dc_level);
-	int x12 = gain * (-2 * buf[6] - buf[7] - dc_level);
-	int x11 = gain * (-2 * buf[8] - buf[9] - dc_level);
-	int x10 = gain * (-2 * buf[10] - buf[11] - dc_level);
-	int x09 = gain * (-2 * buf[12] - buf[13] - dc_level);
-	int x08 = gain * (-2 * buf[14] - buf[15] - dc_level);
-	int x07 = gain * (-2 * buf[16] - buf[17] - dc_level);
-	int x06 = gain * (-2 * buf[18] - buf[19] - dc_level);
-	int x05 = gain * (-2 * buf[20] - buf[21] - dc_level);
-	int x04 = gain * (-2 * buf[22] - buf[23] - dc_level);
-	int x03 = gain * (-2 * buf[24] - buf[25] - dc_level);
-	int x02 = gain * (-2 * buf[26] - buf[27] - dc_level);
-	int x01 = gain * (-2 * buf[28] - buf[29] - dc_level);
-	int x00 = gain * (-2 * buf[30] - buf[31] - dc_level);
+	static const uint32_t *tail = rx_cos;
 
+	const uint32_t *head = (const uint32_t *)dma_hw->ch[dma_ch_in_cos].write_addr;
+
+	while (head == tail) {
+		asm volatile("nop; nop; nop; nop");
+		head = (const uint32_t *)dma_hw->ch[dma_ch_in_cos].write_addr;
+	}
+
+	int value = -(*tail++);
+	value *= 2;
+	value -= *tail++;
+
+	if (tail > rx_end)
+		tail = rx_start;
+
+	return gain * value - dc_level;
+}
+
+inline static struct IQ next_sample()
+{
 	int I = 0, Q = 0;
 
+	int x15 = get_next_sample();
 	I += 93 * x15;
-	I += 71 * x14;
-	I += 39 * x13;
-	I += 0 * x12;
-	I += -39 * x11;
-	I += -71 * x10;
-	I += -93 * x09;
-	I += -101 * x08;
-	I += -93 * x07;
-	I += -71 * x06;
-	I += -39 * x05;
-	I += 0 * x04;
-	I += 39 * x03;
-	I += 71 * x02;
-	I += 93 * x01;
-	I += 101 * x00;
-
 	Q += 39 * x15;
+
+	int x14 = get_next_sample();
+	I += 71 * x14;
 	Q += 71 * x14;
+
+	int x13 = get_next_sample();
+	I += 39 * x13;
 	Q += 93 * x13;
+
+	int x12 = get_next_sample();
+	I += 0 * x12;
 	Q += 101 * x12;
+
+	int x11 = get_next_sample();
+	I += -39 * x11;
 	Q += 93 * x11;
+
+	int x10 = get_next_sample();
+	I += -71 * x10;
 	Q += 71 * x10;
+
+	int x09 = get_next_sample();
+	I += -93 * x09;
 	Q += 39 * x09;
+
+	int x08 = get_next_sample();
+	I += -101 * x08;
 	Q += 0 * x08;
+
+	int x07 = get_next_sample();
+	I += -93 * x07;
 	Q += -39 * x07;
+
+	int x06 = get_next_sample();
+	I += -71 * x06;
 	Q += -71 * x06;
+
+	int x05 = get_next_sample();
+	I += -39 * x05;
 	Q += -93 * x05;
+
+	int x04 = get_next_sample();
+	I += 0 * x04;
 	Q += -101 * x04;
+
+	int x03 = get_next_sample();
+	I += 39 * x03;
 	Q += -93 * x03;
+
+	int x02 = get_next_sample();
+	I += 71 * x02;
 	Q += -71 * x02;
+
+	int x01 = get_next_sample();
+	I += 93 * x01;
 	Q += -39 * x01;
+
+	int x00 = get_next_sample();
+	I += 101 * x00;
 	Q += 0 * x00;
 
 	I /= 1024;
@@ -496,9 +551,6 @@ inline static struct IQ next_sample(const uint32_t *buf)
 
 static void rf_rx(void)
 {
-	const uint32_t base = (uint32_t)rx_cos;
-	int pos = 0;
-
 	while (true) {
 		if (multicore_fifo_rvalid()) {
 			multicore_fifo_pop_blocking();
@@ -506,31 +558,13 @@ static void rf_rx(void)
 			return;
 		}
 
-		int head = (dma_hw->ch[dma_ch_in_cos].write_addr - base) / 4;
-		int delta = (head < pos ? head + RX_WORDS : head) - pos;
-
-		while (delta < RX_STRIDE) {
-			int wait = rnd_next() & 63;
-
-			for (int i = 0; i < wait; i++)
-				asm volatile("nop");
-
-			head = (dma_hw->ch[dma_ch_in_cos].write_addr - base) / 4;
-			delta = (head < pos ? head + RX_WORDS : head) - pos;
-		}
-
-		const uint32_t *cos_ptr = rx_cos + pos;
-
-		pos = (pos + RX_STRIDE) & (RX_WORDS - 1);
-
 		uint8_t *block = iq_queue_buffer[iq_queue_pos];
 		uint8_t *blockptr = block;
 
 		for (int i = 0; i < IQ_SAMPLES; i++) {
-			struct IQ IQ = next_sample(cos_ptr);
+			struct IQ IQ = next_sample();
 			int64_t I = IQ.I;
 			int64_t Q = IQ.Q;
-			cos_ptr += 2 * DECIMATE;
 
 			I /= dc_level;
 
@@ -562,13 +596,13 @@ static void run_command(uint8_t cmd, uint32_t arg)
 	if (0x01 == cmd) {
 		/* Tune to a new center frequency */
 		frequency = arg;
-		rx_lo_init(arg + sample_rate, true);
+		rx_lo_init(frequency + sample_rate);
 	} else if (0x02 == cmd) {
 		/* Set the rate at which IQ sample pairs are sent */
 		sample_rate = arg;
 		dc_level = CLK_SYS_HZ / sample_rate / 2;
 		dma_timer_set_fraction(dma_t_samp, 1, CLK_SYS_HZ / (sample_rate * DECIMATE));
-		rx_lo_init(frequency + sample_rate, true);
+		rx_lo_init(frequency + sample_rate);
 	} else if (0x04 == cmd) {
 		/* Set the tuner gain level */
 		gain = INIT_GAIN * powf(10.0f, arg / 200.0f);
@@ -620,8 +654,8 @@ static void do_rx()
 	channel_config_set_read_increment(&dma_conf, false);
 	channel_config_set_write_increment(&dma_conf, true);
 	channel_config_set_ring(&dma_conf, GPIO_OUT, RX_BITS_DEPTH);
-	channel_config_set_dreq(&dma_conf, pio_get_dreq(PIO, AD_SM, false));
-	dma_channel_configure(dma_ch_in_cos, &dma_conf, rx_cos, &PIO->rxf[AD_SM], UINT_MAX, true);
+	channel_config_set_dreq(&dma_conf, pio_get_dreq(PIO, SM_AD, false));
+	dma_channel_configure(dma_ch_in_cos, &dma_conf, rx_cos, &PIO->rxf[SM_AD], UINT_MAX, true);
 
 	multicore_launch_core1(rf_rx);
 
@@ -682,7 +716,10 @@ int main()
 
 	queue_init(&iq_queue, sizeof(uint8_t *), IQ_QUEUE_LEN);
 
-	rx_lo_init(frequency + sample_rate, true);
+	rx_lo_init(frequency + sample_rate);
+
+	dma_t_samp = dma_claim_unused_timer(true);
+	dma_timer_set_fraction(dma_t_samp, 1, CLK_SYS_HZ / (sample_rate * DECIMATE));
 
 	while (true) {
 		if (check_command() > 0) {