Fix frequency rounding

- Use less DMAs
- Improve bit counting
- Fix sample_rate changes and other fixes

README.md

@@ -32,4 +32,4 @@ See the [blog post](https://blog.porucha.net/2024/pico-sdr/) for more informatio
 
 4. Open `grc/PicoSDR-WBFM.grc` in GNU Radio Companion, adjust carrier frequency to match your favorite FM radio station and press `F6`.
 
-5. Alternatively [gqrx](https://www.gqrx.dk/) works fine with `rtl_tcp` input mode. Maximum sample rate seem to be 400 ksps, above that the samples are dropped.
+5. Alternatively [gqrx](https://www.gqrx.dk/) works fine with `rtl_tcp` input mode. Maximum sample rate seem to be 400 ksps, above that the samples are dropped. Make sure to adjust LNA gain to +30 dB. It's not accurate, but it does control bias strength which in turn does affect analog gain.

grc/PicoSDR-WBFM.grc

@@ -37,7 +37,7 @@ blocks:
   id: variable
   parameters:
     comment: ''
-    value: '88_200_000'
+    value: '94_600_000'
   states:
     bus_sink: false
     bus_source: false
@@ -205,7 +205,7 @@ blocks:
     ant8: ''
     ant9: ''
     args: '"rtl_tcp"'
-    bb_gain0: '20'
+    bb_gain0: '0'
     bb_gain1: '20'
     bb_gain10: '20'
     bb_gain11: '20'
@@ -374,7 +374,7 @@ blocks:
     freq7: 100e6
     freq8: 100e6
     freq9: 100e6
-    gain0: '10'
+    gain0: '0'
     gain1: '10'
     gain10: '10'
     gain11: '10'
@@ -438,7 +438,7 @@ blocks:
     gain_mode7: 'False'
     gain_mode8: 'False'
     gain_mode9: 'False'
-    if_gain0: '20'
+    if_gain0: '0'
     if_gain1: '20'
     if_gain10: '20'
     if_gain11: '20'

grc/PicoSDR.grc

@@ -45,11 +45,23 @@ blocks:
     coordinate: [168, 8.0]
     rotation: 0
     state: enabled
-- name: samp_rate
+- name: decimation
   id: variable
   parameters:
     comment: ''
-    value: '50_000'
+    value: '4'
+  states:
+    bus_sink: false
+    bus_source: false
+    bus_structure: null
+    coordinate: [352, 8.0]
+    rotation: 0
+    state: enabled
+- name: rf_rate
+  id: variable
+  parameters:
+    comment: ''
+    value: '200_000'
   states:
     bus_sink: false
     bus_source: false
@@ -57,6 +69,38 @@ blocks:
     coordinate: [264, 8.0]
     rotation: 0
     state: enabled
+- name: samp_rate
+  id: variable
+  parameters:
+    comment: ''
+    value: rf_rate // decimation
+  states:
+    bus_sink: false
+    bus_source: false
+    bus_structure: null
+    coordinate: [456, 8.0]
+    rotation: 0
+    state: enabled
+- name: analog_agc_xx_0
+  id: analog_agc_xx
+  parameters:
+    affinity: ''
+    alias: ''
+    comment: ''
+    gain: '1.0'
+    max_gain: '65536'
+    maxoutbuf: '0'
+    minoutbuf: '0'
+    rate: 1e-4
+    reference: '0.7'
+    type: complex
+  states:
+    bus_sink: false
+    bus_source: false
+    bus_structure: null
+    coordinate: [360, 312.0]
+    rotation: 0
+    state: enabled
 - name: analog_quadrature_demod_cf_0
   id: analog_quadrature_demod_cf
   parameters:
@@ -70,7 +114,7 @@ blocks:
     bus_sink: false
     bus_source: false
     bus_structure: null
-    coordinate: [640, 536.0]
+    coordinate: [768, 616.0]
     rotation: 0
     state: true
 - name: blocks_message_debug_0
@@ -85,7 +129,7 @@ blocks:
     bus_sink: false
     bus_source: false
     bus_structure: null
-    coordinate: [928, 32.0]
+    coordinate: [1056, 112.0]
     rotation: 0
     state: true
 - name: blocks_probe_rate_0
@@ -105,7 +149,7 @@ blocks:
     bus_sink: false
     bus_source: false
     bus_structure: null
-    coordinate: [640, 40.0]
+    coordinate: [768, 120.0]
     rotation: 0
     state: true
 - name: digital_costas_loop_cc_0
@@ -123,9 +167,33 @@ blocks:
     bus_sink: false
     bus_source: false
     bus_structure: null
-    coordinate: [640, 344.0]
+    coordinate: [768, 424.0]
     rotation: 0
     state: true
+- name: low_pass_filter_0
+  id: low_pass_filter
+  parameters:
+    affinity: ''
+    alias: ''
+    beta: '6.76'
+    comment: ''
+    cutoff_freq: samp_rate / 8
+    decim: decimation
+    gain: '1'
+    interp: '1'
+    maxoutbuf: '0'
+    minoutbuf: '0'
+    samp_rate: rf_rate
+    type: fir_filter_ccf
+    width: samp_rate / 8
+    win: window.WIN_HAMMING
+  states:
+    bus_sink: false
+    bus_source: false
+    bus_structure: null
+    coordinate: [520, 284.0]
+    rotation: 0
+    state: enabled
 - name: osmosdr_source_0
   id: osmosdr_source
   parameters:
@@ -164,7 +232,7 @@ blocks:
     ant8: ''
     ant9: ''
     args: '"rtl_tcp"'
-    bb_gain0: '20'
+    bb_gain0: '0'
     bb_gain1: '20'
     bb_gain10: '20'
     bb_gain11: '20'
@@ -333,7 +401,7 @@ blocks:
     freq7: 100e6
     freq8: 100e6
     freq9: 100e6
-    gain0: '10'
+    gain0: '0'
     gain1: '10'
     gain10: '10'
     gain11: '10'
@@ -397,7 +465,7 @@ blocks:
     gain_mode7: 'False'
     gain_mode8: 'False'
     gain_mode9: 'False'
-    if_gain0: '20'
+    if_gain0: '0'
     if_gain1: '20'
     if_gain10: '20'
     if_gain11: '20'
@@ -465,7 +533,7 @@ blocks:
     minoutbuf: '0'
     nchan: '1'
     num_mboards: '1'
-    sample_rate: samp_rate
+    sample_rate: rf_rate
     sync: sync
     time_source0: ''
     time_source1: ''
@@ -480,7 +548,7 @@ blocks:
     bus_sink: false
     bus_source: false
     bus_structure: null
-    coordinate: [152, 164.0]
+    coordinate: [104, 244.0]
     rotation: 0
     state: enabled
 - name: qtgui_const_sink_x_0
@@ -572,7 +640,7 @@ blocks:
     bus_sink: false
     bus_source: false
     bus_structure: null
-    coordinate: [928, 328.0]
+    coordinate: [1056, 408.0]
     rotation: 0
     state: true
 - name: qtgui_time_sink_x_0
@@ -669,7 +737,7 @@ blocks:
     bus_sink: false
     bus_source: false
     bus_structure: null
-    coordinate: [640, 232.0]
+    coordinate: [768, 312.0]
     rotation: 0
     state: true
 - name: qtgui_time_sink_x_0_0
@@ -766,7 +834,7 @@ blocks:
     bus_sink: false
     bus_source: false
     bus_structure: null
-    coordinate: [928, 512.0]
+    coordinate: [1056, 592.0]
     rotation: 0
     state: true
 - name: qtgui_time_sink_x_0_1
@@ -863,7 +931,7 @@ blocks:
     bus_sink: false
     bus_source: false
     bus_structure: null
-    coordinate: [928, 400.0]
+    coordinate: [1056, 480.0]
     rotation: 0
     state: true
 - name: qtgui_waterfall_sink_x_0_0
@@ -924,20 +992,22 @@ blocks:
     bus_sink: false
     bus_source: false
     bus_structure: null
-    coordinate: [640, 128.0]
+    coordinate: [768, 208.0]
     rotation: 0
     state: true
 
 connections:
+- [analog_agc_xx_0, '0', low_pass_filter_0, '0']
 - [analog_quadrature_demod_cf_0, '0', qtgui_time_sink_x_0_0, '0']
 - [blocks_probe_rate_0, rate, blocks_message_debug_0, print]
 - [digital_costas_loop_cc_0, '0', qtgui_const_sink_x_0, '0']
 - [digital_costas_loop_cc_0, '0', qtgui_time_sink_x_0_1, '0']
-- [osmosdr_source_0, '0', analog_quadrature_demod_cf_0, '0']
+- [low_pass_filter_0, '0', analog_quadrature_demod_cf_0, '0']
-- [osmosdr_source_0, '0', blocks_probe_rate_0, '0']
+- [low_pass_filter_0, '0', blocks_probe_rate_0, '0']
-- [osmosdr_source_0, '0', digital_costas_loop_cc_0, '0']
+- [low_pass_filter_0, '0', digital_costas_loop_cc_0, '0']
-- [osmosdr_source_0, '0', qtgui_time_sink_x_0, '0']
+- [low_pass_filter_0, '0', qtgui_time_sink_x_0, '0']
-- [osmosdr_source_0, '0', qtgui_waterfall_sink_x_0_0, '0']
+- [low_pass_filter_0, '0', qtgui_waterfall_sink_x_0_0, '0']
+- [osmosdr_source_0, '0', analog_agc_xx_0, '0']
 
 metadata:
   file_format: 1

src/cordic.h

@@ -1,50 +0,0 @@
-#pragma once
-
-/*
- * Copyright (c) 2020 STMicroelectronics.
- * All rights reserved.
- *
- * This software component is licensed by ST under BSD 3-Clause license,
- * the "License"; You may not use this file except in compliance with the
- * License. You may obtain a copy of the License at:
- *
- * https://opensource.org/licenses/BSD-3-Clause
- */
-
-#define CORDIC_MAXITER 9
-#define CORDIC_PI 0x10000000
-
-static int CORDIC_ZTBL[] = { 0x04000000, 0x025C80A4, 0x013F670B, 0x00A2223B, 0x005161A8, 0x0028BAFC,
-			     0x00145EC4, 0x000A2F8B, 0x000517CA, 0x00028BE6, 0x000145F3, 0x0000A2FA,
-			     0x0000517D, 0x000028BE, 0x0000145F, 0x00000A30 };
-
-inline static __attribute__((__unused__)) int fast_atan2(int y, int x)
-{
-	int k, tx, z = 0, fl = 0;
-
-	if (x < 0) {
-		fl = ((y > 0) ? +1 : -1);
-		x = -x;
-		y = -y;
-	}
-
-	for (k = 0; k < CORDIC_MAXITER; k++) {
-		tx = x;
-
-		if (y <= 0) {
-			x -= (y >> k);
-			y += (tx >> k);
-			z -= CORDIC_ZTBL[k];
-		} else {
-			x += (y >> k);
-			y -= (tx >> k);
-			z += CORDIC_ZTBL[k];
-		}
-	}
-
-	if (fl != 0) {
-		z += fl * CORDIC_PI;
-	}
-
-	return z;
-}

src/main.c

@@ -21,54 +21,68 @@
 #include <limits.h>
 #include <stdlib.h>
 
-#include "cordic.h"
-
 #define VREG_VOLTAGE VREG_VOLTAGE_1_20
-#define CLK_SYS_HZ (300 * MHZ)
+#define CLK_SYS_HZ (306 * MHZ)
+
+#define RX_PIN 10
+#define FB_PIN 11
 #define PSU_PIN 23
 
+#define PIO pio0
+#define SM_RX 0
+#define SM_BIAS 1
+#define SM_COS 2
+#define SM_SIN 3
+
 #define IQ_SAMPLES 32
 #define IQ_BLOCK_LEN (2 * IQ_SAMPLES)
-#define IQ_QUEUE_LEN 64
+#define IQ_QUEUE_LEN 8
 
 #define XOR_ADDR 0x1000
-#define LO_COS_ACCUMULATOR (&pio1->sm[2].pinctrl)
-#define LO_SIN_ACCUMULATOR (&pio1->sm[3].pinctrl)
-
 #define LO_BITS_DEPTH 15
 #define LO_WORDS (1 << (LO_BITS_DEPTH - 2))
+#define LO_COS_ACCUMULATOR (&PIO->sm[SM_COS].pinctrl)
+#define LO_SIN_ACCUMULATOR (&PIO->sm[SM_SIN].pinctrl)
+#define SIN_PHASE (0u)
+#define COS_PHASE (3u << 30)
+
 static uint32_t lo_cos[LO_WORDS] __attribute__((__aligned__(1 << LO_BITS_DEPTH)));
 static uint32_t lo_sin[LO_WORDS] __attribute__((__aligned__(1 << LO_BITS_DEPTH)));
 
 #define DECIMATE 4
-#define RX_STRIDE (2 * IQ_SAMPLES * DECIMATE)
-#define RX_BITS_DEPTH 13
-#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)));
-static uint32_t rx_sin[RX_WORDS] __attribute__((__aligned__(1 << RX_BITS_DEPTH)));
-
-#define AOUT_PIN 0
-#define AOUT_BITS_DEPTH 9
-#define AOUT_WORDS (1 << (AOUT_BITS_DEPTH - 1))
-
-static uint16_t aout_buf[AOUT_WORDS] __attribute__((__aligned__(1 << AOUT_BITS_DEPTH)));
-
-#define INIT_GAIN 120
 #define INIT_SAMPLE_RATE 100000
 #define INIT_FREQ 94600000
+#define INIT_GAIN 127
 
 #define NUM_GAINS 29
 static int gains[NUM_GAINS] = { 0,   9,	  14,  27,  37,	 77,  87,  125, 144, 157,
 				166, 197, 207, 229, 254, 280, 297, 328, 338, 364,
 				372, 386, 402, 421, 434, 439, 445, 480, 496 };
-static int gain = INIT_GAIN;
 static int sample_rate = INIT_SAMPLE_RATE;
+static int max_amplitude = CLK_SYS_HZ / INIT_SAMPLE_RATE / 2;
+static int gain = INIT_GAIN;
+static int frequency = INIT_FREQ;
 
-#define SIN_PHASE (0u)
+static queue_t iq_queue;
-#define COS_PHASE (3u << 30)
+static uint8_t iq_queue_buffer[IQ_QUEUE_LEN][IQ_BLOCK_LEN];
+static size_t iq_queue_pos = 0;
+
+static uint32_t rnd = 0;
+
+inline static __unused uint32_t rnd_next()
+{
+	rnd = rnd * 0x41c64e6d + 12345;
+	return rnd;
+}
+
+static void dma_channel_clear_chain_to(int ch)
+{
+	uint32_t ctrl = dma_hw->ch[ch].al1_ctrl;
+	ctrl &= ~DMA_CH0_CTRL_TRIG_CHAIN_TO_BITS;
+	ctrl |= ch << DMA_CH0_CTRL_TRIG_CHAIN_TO_LSB;
+	dma_hw->ch[ch].al1_ctrl = ctrl;
+}
 
 /* rx -> cp -> cos -> sin -> pio_cos -> pio_sin -> rx ... */
 static int dma_ch_rx = -1;
@@ -78,186 +92,156 @@ static int dma_ch_sin = -1;
 static int dma_ch_pio_cos = -1;
 static int dma_ch_pio_sin = -1;
 
-static int dma_ch_samp_trig = -1;
 static int dma_ch_samp_cos = -1;
 static int dma_ch_samp_sin = -1;
 
 static int dma_t_samp = -1;
 
-static int dma_ch_in_cos = -1;
+static int origin_rx = -1;
-static int dma_ch_in_sin = -1;
+static int origin_bias = -1;
+static int origin_adder = 0;
 
-static int dma_ch_aout = -1;
+static void init_rx()
-
-static int dma_t_aout = -1;
-
-static queue_t iq_queue;
-static int8_t iq_queue_buffer[IQ_QUEUE_LEN][IQ_BLOCK_LEN];
-static size_t iq_queue_pos = 0;
-
-static int slice, chan;
-
-static void bias_set_delay(int delay)
 {
-	delay += 200;
+	gpio_disable_pulls(RX_PIN);
-
+	pio_gpio_init(PIO, RX_PIN);
-	if (delay < 0)
-		delay = 0;
-
-	if (delay >= 200)
-		delay = 512;
-
-	int bulk = delay / 16;
-	int rest = delay % 16;
-
-	if (0 == rest) {
-		bulk -= 1;
-		rest = 16;
-	}
-
-	if (delay > 0) {
-		if (bulk) {
-			pio1->instr_mem[11] = pio_encode_set(pio_x, bulk) |
-					      pio_encode_sideset(1, 0) | pio_encode_delay(rest - 1);
-		} else {
-			pio1->instr_mem[11] = pio_encode_jmp(10) | pio_encode_sideset(1, 0) |
-					      pio_encode_delay(rest - 1);
-		}
-
-		pio_sm_set_wrap(pio1, 0, 10, 12);
-	} else {
-		pio_sm_set_wrap(pio1, 0, 10, 10);
-		pio_sm_exec(pio1, 0, pio_encode_jmp(10));
-	}
-}
-
-static void bias_init(int in_pin, int out_pin)
-{
-	gpio_disable_pulls(in_pin);
-	gpio_disable_pulls(out_pin);
-
-	pio_gpio_init(pio1, out_pin);
-
-	gpio_set_input_hysteresis_enabled(in_pin, false);
-	gpio_set_drive_strength(out_pin, GPIO_DRIVE_STRENGTH_2MA);
-	gpio_set_slew_rate(out_pin, GPIO_SLEW_RATE_SLOW);
 
 	const uint16_t insn[] = {
-		pio_encode_mov_not(pio_pins, pio_pins) | pio_encode_sideset(1, 1),
+		pio_encode_in(pio_pins, 1) | pio_encode_delay(0),
-		pio_encode_set(pio_x, 31) | pio_encode_sideset(1, 0) | pio_encode_delay(15),
-		pio_encode_jmp_x_dec(2) | pio_encode_sideset(1, 0) | pio_encode_delay(15),
 	};
 
 	pio_program_t prog = {
 		.instructions = insn,
 		.length = sizeof(insn) / sizeof(*insn),
-		.origin = 10,
+		.origin = origin_rx,
 	};
 
-	pio_sm_set_enabled(pio1, 0, false);
+	if (pio_can_add_program(PIO, &prog))
-	pio_sm_restart(pio1, 0);
+		origin_rx = pio_add_program(PIO, &prog);
-	pio_sm_clear_fifos(pio1, 0);
-
-	if (pio_can_add_program(pio1, &prog))
-		pio_add_program(pio1, &prog);
 
 	pio_sm_config pc = pio_get_default_sm_config();
-	sm_config_set_sideset(&pc, 1, false, true);
+	sm_config_set_in_pins(&pc, RX_PIN);
-	sm_config_set_sideset_pins(&pc, out_pin);
+	sm_config_set_wrap(&pc, origin_rx, origin_rx + prog.length - 1);
-	sm_config_set_in_pins(&pc, in_pin);
-	sm_config_set_out_pins(&pc, out_pin, 1);
-	sm_config_set_set_pins(&pc, out_pin, 1);
-
-	sm_config_set_wrap(&pc, prog.origin, prog.origin + prog.length - 1);
-
-	sm_config_set_clkdiv_int_frac(&pc, 1, 0);
-	pio_sm_init(pio1, 0, prog.origin, &pc);
-
-	pio_sm_set_consecutive_pindirs(pio1, 0, out_pin, 1, GPIO_OUT);
-
-	pio_sm_set_enabled(pio1, 0, true);
-}
-
-static void watch_init(int in_pin)
-{
-	const uint16_t insn[] = {
-		pio_encode_in(pio_pins, 1),
-	};
-
-	pio_program_t prog = {
-		.instructions = insn,
-		.length = 1,
-		.origin = 6,
-	};
-
-	pio_sm_set_enabled(pio1, 1, false);
-	pio_sm_restart(pio1, 1);
-	pio_sm_clear_fifos(pio1, 1);
-
-	if (pio_can_add_program(pio1, &prog))
-		pio_add_program(pio1, &prog);
-
-	pio_sm_config pc = pio_get_default_sm_config();
-	sm_config_set_in_pins(&pc, in_pin);
-	sm_config_set_wrap(&pc, prog.origin, prog.origin + 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(pio1, 1, prog.origin, &pc);
+	pio_sm_init(PIO, SM_RX, origin_rx, &pc);
 
-	pio_sm_set_enabled(pio1, 1, true);
+	pio_sm_set_consecutive_pindirs(PIO, SM_RX, RX_PIN, 1, GPIO_IN);
 }
 
-static void adder_init()
+static void init_bias()
 {
-	const uint16_t insn[] = {
+	gpio_disable_pulls(RX_PIN);
-		pio_encode_jmp_y_dec(1),
+	gpio_disable_pulls(FB_PIN);
-		pio_encode_out(pio_pc, 2),
-		pio_encode_out(pio_pc, 2),
-		pio_encode_jmp_x_dec(2),
 
-		/* Avoid Y-- on wrap. */
+	pio_gpio_init(PIO, FB_PIN);
-		pio_encode_out(pio_pc, 2),
+
+	gpio_set_input_hysteresis_enabled(RX_PIN, false);
+	gpio_set_drive_strength(FB_PIN, GPIO_DRIVE_STRENGTH_2MA);
+	gpio_set_slew_rate(FB_PIN, GPIO_SLEW_RATE_SLOW);
+
+	PIO->input_sync_bypass = 1u << RX_PIN;
+
+	const uint16_t insn[] = {
+		pio_encode_mov(pio_isr, pio_null),
+		pio_encode_in(pio_y, 4),
+		pio_encode_in(pio_pins, 1) | pio_encode_delay(15),
+		pio_encode_in(pio_pins, 1) | pio_encode_delay(15),
+
+		pio_encode_mov(pio_y, pio_isr),
+		pio_encode_mov(pio_x, pio_isr),
+
+		pio_encode_jmp_x_dec(6),
+		pio_encode_mov_not(pio_pins, pio_pins) | pio_encode_sideset(1, 1),
 	};
 
 	pio_program_t prog = {
 		.instructions = insn,
 		.length = sizeof(insn) / sizeof(*insn),
-		.origin = 0,
+		.origin = origin_bias,
 	};
 
-	pio_sm_set_enabled(pio1, 2, false);
+	if (pio_can_add_program(PIO, &prog))
-	pio_sm_set_enabled(pio1, 3, false);
+		origin_bias = pio_add_program(PIO, &prog);
-
-	pio_sm_restart(pio1, 2);
-	pio_sm_restart(pio1, 3);
-
-	pio_sm_clear_fifos(pio1, 2);
-	pio_sm_clear_fifos(pio1, 3);
-
-	if (pio_can_add_program(pio1, &prog))
-		pio_add_program(pio1, &prog);
 
 	pio_sm_config pc = pio_get_default_sm_config();
-	sm_config_set_wrap(&pc, prog.origin, prog.origin + prog.length - 1);
+	sm_config_set_in_shift(&pc, false, false, 32);
+	sm_config_set_sideset(&pc, 1, false, true);
+	sm_config_set_sideset_pins(&pc, FB_PIN);
+	sm_config_set_in_pins(&pc, RX_PIN);
+	sm_config_set_out_pins(&pc, FB_PIN, 1);
+	sm_config_set_set_pins(&pc, RX_PIN, 1);
+
+	sm_config_set_wrap(&pc, origin_bias, origin_bias + prog.length - 1);
+
+	sm_config_set_clkdiv_int_frac(&pc, 1, 0);
+	pio_sm_init(PIO, SM_BIAS, origin_bias, &pc);
+
+	pio_sm_exec_wait_blocking(PIO, SM_BIAS, pio_encode_set(pio_y, 31));
+	pio_sm_set_consecutive_pindirs(PIO, SM_BIAS, FB_PIN, 1, GPIO_OUT);
+}
+
+static const uint32_t samp_insn = 16;
+
+static void init_adder()
+{
+	const uint16_t insn[] = {
+		pio_encode_out(pio_pc, 4), // 0000 +0
+		pio_encode_jmp_x_dec(0),   // 0001 +1
+		pio_encode_jmp_x_dec(0),   // 0010 +1
+		pio_encode_jmp_y_dec(0),   // 0011 +2
+		pio_encode_jmp_x_dec(0),   // 0100 +1
+		pio_encode_jmp_y_dec(0),   // 0101 +2
+		pio_encode_jmp_y_dec(0),   // 0110 +2
+		pio_encode_jmp_y_dec(1),   // 0111 +2 +1
+		pio_encode_jmp_x_dec(0),   // 1000 +1
+		pio_encode_jmp_y_dec(0),   // 1001 +2
+		pio_encode_jmp_y_dec(0),   // 1010 +2
+		pio_encode_jmp_y_dec(1),   // 1011 +2 +1
+		pio_encode_jmp_y_dec(0),   // 1100 +2
+		pio_encode_jmp_y_dec(1),   // 1101 +2 +1
+		pio_encode_jmp_y_dec(1),   // 1110 +2 +1
+		pio_encode_jmp_y_dec(3),   // 1111 +2 +2
+
+		/*
+		 * Should wrap here.
+		 * Jump to this portion must be inserted from the outside.
+		 */
+		pio_encode_in(pio_y, 32),
+		pio_encode_in(pio_x, 32),
+		pio_encode_set(pio_y, 0),
+		pio_encode_set(pio_x, 0),
+		//pio_encode_jmp_y_dec(21),
+		//pio_encode_jmp_x_dec(22),
+		pio_encode_out(pio_pc, 4),
+	};
+
+	pio_program_t prog = {
+		.instructions = insn,
+		.length = sizeof(insn) / sizeof(*insn),
+		.origin = origin_adder,
+	};
+
+	if (pio_can_add_program(PIO, &prog))
+		origin_adder = pio_add_program(PIO, &prog);
+
+	pio_sm_config pc = pio_get_default_sm_config();
+	sm_config_set_wrap(&pc, origin_adder, origin_adder + 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(pio1, 2, prog.origin + prog.length - 1, &pc);
-	pio_sm_init(pio1, 3, prog.origin + prog.length - 1, &pc);
 
-	pio_sm_set_enabled(pio1, 2, true);
+	pio_sm_init(PIO, SM_COS, origin_adder, &pc);
-	pio_sm_set_enabled(pio1, 3, true);
+	pio_sm_init(PIO, SM_SIN, origin_adder, &pc);
 }
 
-static void lo_generate(uint32_t *buf, double freq, uint32_t phase)
+#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)
 {
-	static const double base = (UINT_MAX + 1.0) / CLK_SYS_HZ;
+	for (size_t i = 0; i < len; i++) {
-
+		uint32_t bits = 0;
-	uint32_t step = base * freq;
-
-	for (size_t i = 0; i < LO_WORDS; i++) {
-		unsigned bits = 0;
 
 		for (int j = 0; j < 32; j++) {
 			bits |= phase >> 31;
@@ -271,27 +255,19 @@ static void lo_generate(uint32_t *buf, double freq, uint32_t phase)
 
 static void rx_lo_init(double req_freq, bool align)
 {
-	const double step_hz = (double)CLK_SYS_HZ / (4 << LO_BITS_DEPTH);
+	const double step_hz = (double)CLK_SYS_HZ / ((8 << LO_BITS_DEPTH) / 2.0);
 	double freq = req_freq;
 
 	if (align)
 		freq = round(freq / step_hz) * step_hz;
 
-	lo_generate(lo_cos, freq, COS_PHASE);
+	uint32_t step = STEP_BASE * freq;
-	lo_generate(lo_sin, freq, SIN_PHASE);
+
+	lo_generate_phase(lo_cos, LO_WORDS, step, COS_PHASE);
+	lo_generate_phase(lo_sin, LO_WORDS, step, SIN_PHASE);
 }
 
-static const uint32_t samp_insn[4] __attribute__((__aligned__(16)));
+static void rf_rx_start()
-static const uint32_t samp_insn[4] = {
-	0x4040, /* IN Y, 32 */
-	0x4020, /* IN X, 32 */
-	0xe040, /* SET Y, 0 */
-	0xe020, /* SET X, 0 */
-};
-
-static uint32_t null, one = 1;
-
-static void rf_rx_start(int rx_pin, int bias_pin)
 {
 	dma_ch_rx = dma_claim_unused_channel(true);
 	dma_ch_cp = dma_claim_unused_channel(true);
@@ -302,12 +278,6 @@ static void rf_rx_start(int rx_pin, int bias_pin)
 
 	dma_ch_samp_cos = dma_claim_unused_channel(true);
 	dma_ch_samp_sin = dma_claim_unused_channel(true);
-	dma_ch_samp_trig = dma_claim_unused_channel(true);
-
-	dma_ch_aout = dma_claim_unused_channel(true);
-
-	dma_t_samp = dma_claim_unused_timer(true);
-	dma_t_aout = dma_claim_unused_timer(true);
 
 	dma_channel_config dma_conf;
 
@@ -316,9 +286,9 @@ static void rf_rx_start(int rx_pin, int bias_pin)
 	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(pio1, 1, false));
+	channel_config_set_dreq(&dma_conf, pio_get_dreq(PIO, SM_RX, false));
 	channel_config_set_chain_to(&dma_conf, dma_ch_cp);
-	dma_channel_configure(dma_ch_rx, &dma_conf, LO_COS_ACCUMULATOR, &pio1->rxf[1], 1, false);
+	dma_channel_configure(dma_ch_rx, &dma_conf, LO_COS_ACCUMULATOR, &PIO->rxf[SM_RX], 1, false);
 
 	/* Copy accumulator I to accumulator Q. */
 	dma_conf = dma_channel_get_default_config(dma_ch_cp);
@@ -354,9 +324,9 @@ static void rf_rx_start(int rx_pin, int bias_pin)
 	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(pio1, 2, true));
+	channel_config_set_dreq(&dma_conf, pio_get_dreq(PIO, SM_COS, true));
 	channel_config_set_chain_to(&dma_conf, dma_ch_pio_sin);
-	dma_channel_configure(dma_ch_pio_cos, &dma_conf, &pio1->txf[2], LO_COS_ACCUMULATOR, 1,
+	dma_channel_configure(dma_ch_pio_cos, &dma_conf, &PIO->txf[SM_COS], LO_COS_ACCUMULATOR, 1,
 			      false);
 
 	/* Copy mixed Q accumulator to PIO adder Q. */
@@ -364,84 +334,67 @@ static void rf_rx_start(int rx_pin, int bias_pin)
 	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(pio1, 3, true));
+	channel_config_set_dreq(&dma_conf, pio_get_dreq(PIO, SM_SIN, true));
 	channel_config_set_chain_to(&dma_conf, dma_ch_rx);
-	dma_channel_configure(dma_ch_pio_sin, &dma_conf, &pio1->txf[3], LO_SIN_ACCUMULATOR, 1,
+	dma_channel_configure(dma_ch_pio_sin, &dma_conf, &PIO->txf[SM_SIN], LO_SIN_ACCUMULATOR, 1,
 			      false);
 
-	/* Pacing timer for the sampling script trigger channel. */
+	/* Trigger I accumulator values push. */
-	dma_timer_set_fraction(dma_t_samp, 1, CLK_SYS_HZ / (sample_rate * DECIMATE));
+	dma_conf = dma_channel_get_default_config(dma_ch_samp_cos);
-
-	/* Sampling trigger channel. */
-	dma_conf = dma_channel_get_default_config(dma_ch_samp_trig);
 	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, dma_get_timer_dreq(dma_t_samp));
 	channel_config_set_high_priority(&dma_conf, true);
-	channel_config_set_chain_to(&dma_conf, dma_ch_samp_cos);
-	dma_channel_configure(dma_ch_samp_trig, &dma_conf, &null, &one, 1, false);
-
-	/* Trigger I 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);
-	channel_config_set_read_increment(&dma_conf, true);
-	channel_config_set_write_increment(&dma_conf, false);
-	channel_config_set_ring(&dma_conf, false, 4);
-	channel_config_set_high_priority(&dma_conf, true);
 	channel_config_set_chain_to(&dma_conf, dma_ch_samp_sin);
-	dma_channel_configure(dma_ch_samp_cos, &dma_conf, &pio1->sm[2].instr, samp_insn, 4, false);
+	dma_channel_configure(dma_ch_samp_cos, &dma_conf, &PIO->sm[SM_COS].instr, &samp_insn, 1,
+			      false);
 
 	/* Trigger Q accumulator values push. */
 	dma_conf = dma_channel_get_default_config(dma_ch_samp_sin);
 	channel_config_set_transfer_data_size(&dma_conf, DMA_SIZE_32);
-	channel_config_set_read_increment(&dma_conf, true);
+	channel_config_set_read_increment(&dma_conf, false);
 	channel_config_set_write_increment(&dma_conf, false);
-	channel_config_set_ring(&dma_conf, false, 4);
 	channel_config_set_high_priority(&dma_conf, true);
-	channel_config_set_chain_to(&dma_conf, dma_ch_samp_trig);
+	channel_config_set_chain_to(&dma_conf, dma_ch_samp_cos);
-	dma_channel_configure(dma_ch_samp_sin, &dma_conf, &pio1->sm[3].instr, samp_insn, 4, false);
+	dma_channel_configure(dma_ch_samp_sin, &dma_conf, &PIO->sm[SM_SIN].instr, &samp_insn, 1,
-
-	/* Pacing timer for the audio output. */
-	dma_timer_set_fraction(dma_t_aout, 1, CLK_SYS_HZ / 200000);
-
-	dma_conf = dma_channel_get_default_config(dma_ch_aout);
-	channel_config_set_transfer_data_size(&dma_conf, DMA_SIZE_16);
-	channel_config_set_read_increment(&dma_conf, true);
-	channel_config_set_write_increment(&dma_conf, false);
-	channel_config_set_ring(&dma_conf, false, AOUT_BITS_DEPTH);
-	channel_config_set_dreq(&dma_conf, dma_get_timer_dreq(dma_t_aout));
-	dma_channel_configure(dma_ch_aout, &dma_conf, &pwm_hw->slice[slice].cc, aout_buf, UINT_MAX,
 			      false);
 
-	bias_init(rx_pin, bias_pin);
+	init_bias();
-	adder_init();
+	init_adder();
+	init_rx();
 
 	dma_channel_start(dma_ch_rx);
-	dma_channel_start(dma_ch_samp_trig);
+	dma_channel_start(dma_ch_samp_cos);
-	dma_channel_start(dma_ch_aout);
+
-	watch_init(rx_pin);
+	pio_set_sm_mask_enabled(PIO, 0x0f, true);
 }
 
 static void rf_rx_stop(void)
 {
-	pio_sm_set_enabled(pio1, 0, false);
+	pio_set_sm_mask_enabled(PIO, 0x0f, false);
-	pio_sm_set_enabled(pio1, 1, false);
-	pio_sm_set_enabled(pio1, 2, false);
-	pio_sm_set_enabled(pio1, 3, false);
 
-	pio_sm_restart(pio1, 0);
+	pio_sm_restart(PIO, 0);
-	pio_sm_restart(pio1, 1);
+	pio_sm_restart(PIO, 1);
-	pio_sm_restart(pio1, 2);
+	pio_sm_restart(PIO, 2);
-	pio_sm_restart(pio1, 3);
+	pio_sm_restart(PIO, 3);
 
-	pio_sm_clear_fifos(pio1, 0);
+	pio_sm_clear_fifos(PIO, 0);
-	pio_sm_clear_fifos(pio1, 1);
+	pio_sm_clear_fifos(PIO, 1);
-	pio_sm_clear_fifos(pio1, 2);
+	pio_sm_clear_fifos(PIO, 2);
-	pio_sm_clear_fifos(pio1, 3);
+	pio_sm_clear_fifos(PIO, 3);
 
 	sleep_us(10);
 
+	dma_channel_clear_chain_to(dma_ch_rx);
+	dma_channel_clear_chain_to(dma_ch_cp);
+	dma_channel_clear_chain_to(dma_ch_cos);
+	dma_channel_clear_chain_to(dma_ch_sin);
+	dma_channel_clear_chain_to(dma_ch_pio_cos);
+	dma_channel_clear_chain_to(dma_ch_pio_sin);
+	dma_channel_clear_chain_to(dma_ch_samp_cos);
+	dma_channel_clear_chain_to(dma_ch_samp_sin);
+
 	dma_channel_abort(dma_ch_rx);
 	dma_channel_abort(dma_ch_cp);
 	dma_channel_abort(dma_ch_cos);
@@ -450,8 +403,6 @@ static void rf_rx_stop(void)
 	dma_channel_abort(dma_ch_pio_sin);
 	dma_channel_abort(dma_ch_samp_cos);
 	dma_channel_abort(dma_ch_samp_sin);
-	dma_channel_abort(dma_ch_samp_trig);
-	dma_channel_abort(dma_ch_aout);
 
 	dma_channel_cleanup(dma_ch_rx);
 	dma_channel_cleanup(dma_ch_cp);
@@ -461,8 +412,6 @@ static void rf_rx_stop(void)
 	dma_channel_cleanup(dma_ch_pio_sin);
 	dma_channel_cleanup(dma_ch_samp_cos);
 	dma_channel_cleanup(dma_ch_samp_sin);
-	dma_channel_cleanup(dma_ch_samp_trig);
-	dma_channel_cleanup(dma_ch_aout);
 
 	dma_channel_unclaim(dma_ch_rx);
 	dma_channel_unclaim(dma_ch_cp);
@@ -472,11 +421,6 @@ static void rf_rx_stop(void)
 	dma_channel_unclaim(dma_ch_pio_sin);
 	dma_channel_unclaim(dma_ch_samp_cos);
 	dma_channel_unclaim(dma_ch_samp_sin);
-	dma_channel_unclaim(dma_ch_samp_trig);
-	dma_channel_unclaim(dma_ch_aout);
-
-	dma_timer_unclaim(dma_t_samp);
-	dma_timer_unclaim(dma_t_aout);
 
 	dma_ch_rx = -1;
 	dma_ch_cp = -1;
@@ -486,18 +430,46 @@ static void rf_rx_stop(void)
 	dma_ch_pio_sin = -1;
 	dma_ch_samp_cos = -1;
 	dma_ch_samp_sin = -1;
-	dma_ch_samp_trig = -1;
+}
-	dma_ch_aout = -1;
 
-	dma_t_samp = -1;
+inline static uint32_t pio_sm_get_blocking_unsafe(pio_hw_t *pio, int sm)
-	dma_t_aout = -1;
+{
+	while (pio->fstat & (1u << (PIO_FSTAT_RXEMPTY_LSB + sm)))
+		asm volatile("nop");
+
+	return pio->rxf[sm];
+}
+
+inline static int nextI()
+{
+	static int prevI = 0;
+
+	int sI = 0;
+	sI -= 2 * pio_sm_get_blocking_unsafe(PIO, SM_COS);
+	sI -= pio_sm_get_blocking_unsafe(PIO, SM_COS);
+
+	int I = sI - prevI;
+	prevI = sI;
+
+	return I;
+}
+
+inline static int nextQ()
+{
+	static int prevQ = 0;
+
+	int sQ = 0;
+	sQ -= 2 * pio_sm_get_blocking_unsafe(PIO, SM_SIN);
+	sQ -= pio_sm_get_blocking_unsafe(PIO, SM_SIN);
+
+	int Q = sQ - prevQ;
+	prevQ = sQ;
+
+	return Q;
 }
 
 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();
@@ -505,81 +477,25 @@ static void rf_rx(void)
 			return;
 		}
 
-		int head = (dma_hw->ch[dma_ch_in_cos].write_addr - base) / 4;
+		uint8_t *block = iq_queue_buffer[iq_queue_pos];
-		int delta = (head < pos ? head + RX_WORDS : head) - pos;
+		uint8_t *blockptr = block;
-
-		while (delta < RX_STRIDE) {
-			sleep_us(1);
-			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;
-		const uint32_t *sin_ptr = rx_sin + pos;
-
-		pos = (pos + RX_STRIDE) & (RX_WORDS - 1);
-
-		int8_t *block = iq_queue_buffer[iq_queue_pos];
-		int8_t *blockptr = block;
-
-		/*
-		 * Since every 2 samples add to either +1 or -1,
-		 * the maximum amplitude in one direction is 1/2.
-		 */
-		int64_t max_amplitude = CLK_SYS_HZ / 2;
-
-		/*
-		 * Since the waveform is normally half of the time
-		 * above zero, we can halve once more.
-		 *
-		 * This is not perfect, so we do not max out the base
-		 * gain but keep it slightly below the maximum to make
-		 * sure we do not overshoot often.
-		 */
-		max_amplitude /= 2;
-
-		/*
-		 * We are allowing the counters to only go as high
-		 * as sampling rate.
-		 */
-		max_amplitude /= sample_rate;
 
 		for (int i = 0; i < IQ_SAMPLES; i++) {
-			int sI = 0, sQ = 0;
+			int I = 0, Q = 0;
 
-			/*
+			Q += nextQ();
-			 * I: +I1 -I3 +Q2 -Q4
+			I += nextI();
-			 * Q: +Q1 -Q3 -I2 +I4
-			 */
-			sI += *cos_ptr++;
-			sI -= *cos_ptr++;
 
-			sQ -= *cos_ptr++;
+			I -= nextQ();
-			sQ += *cos_ptr++;
+			Q += nextI();
 
-			sI -= *cos_ptr++;
+			Q -= nextQ();
-			sI += *cos_ptr++;
+			I -= nextI();
 
-			sQ += *cos_ptr++;
+			I += nextQ();
-			sQ -= *cos_ptr++;
+			Q -= nextI();
-
-			sQ += *sin_ptr++;
-			sQ -= *sin_ptr++;
-
-			sI += *sin_ptr++;
-			sI -= *sin_ptr++;
-
-			sQ -= *sin_ptr++;
-			sQ += *sin_ptr++;
-
-			sI -= *sin_ptr++;
-			sI += *sin_ptr++;
-
-			int64_t I = sI;
-			int64_t Q = sQ;
 
 			I *= gain;
-			I -= (max_amplitude * 181) / 256;
 			I /= max_amplitude;
 
 			if (I > 127)
@@ -587,10 +503,9 @@ static void rf_rx(void)
 			else if (I < -128)
 				I = -128;
 
-			*blockptr++ = I;
+			*blockptr++ = (uint8_t)I + 128;
 
 			Q *= gain;
-			Q -= (max_amplitude * 181) / 256;
 			Q /= max_amplitude;
 
 			if (Q > 127)
@@ -598,7 +513,7 @@ static void rf_rx(void)
 			else if (Q < -128)
 				Q = -128;
 
-			*blockptr++ = Q;
+			*blockptr++ = (uint8_t)Q + 128;
 		}
 
 		if (queue_try_add(&iq_queue, &block)) {
@@ -611,23 +526,21 @@ static void run_command(uint8_t cmd, uint32_t arg)
 {
 	if (0x01 == cmd) {
 		/* Tune to a new center frequency */
-		rx_lo_init(arg - sample_rate, true);
+		frequency = arg;
+		rx_lo_init(frequency + sample_rate, true);
 	} else if (0x02 == cmd) {
 		/* Set the rate at which IQ sample pairs are sent */
 		sample_rate = arg;
+		max_amplitude = CLK_SYS_HZ / sample_rate / 2;
 		dma_timer_set_fraction(dma_t_samp, 1, CLK_SYS_HZ / (sample_rate * DECIMATE));
-		rx_lo_init(arg - sample_rate, true);
+		rx_lo_init(frequency + sample_rate, true);
 	} else if (0x04 == cmd) {
 		/* Set the tuner gain level */
-		gain = INIT_GAIN * powf(10.0f, 0.005f * arg);
+		gain = INIT_GAIN * pow(10.0, arg / 200.0);
-	} else if (0x05 == cmd) {
-		/* Set PPM error - hack to tweak bias strength */
-		bias_set_delay(arg);
 	} else if (0x0d == cmd) {
 		/* Set tuner gain by the tuner's gain index */
-		if (arg < NUM_GAINS) {
+		if (arg <= NUM_GAINS)
-			gain = INIT_GAIN * powf(10.0f, 0.005f * gains[arg]);
+			gain = INIT_GAIN * pow(10.0, gains[arg] / 200.0);
-		}
 	}
 }
 
@@ -655,47 +568,17 @@ static int check_command(void)
 	return -1;
 }
 
-static void do_rx(int rx_pin, int bias_pin)
+static void do_rx()
 {
-	rf_rx_start(rx_pin, bias_pin);
+	const uint8_t *block;
-	sleep_us(100);
-
-	dma_ch_in_cos = dma_claim_unused_channel(true);
-	dma_ch_in_sin = dma_claim_unused_channel(true);
-
-	dma_channel_config dma_conf;
-
-	dma_conf = dma_channel_get_default_config(dma_ch_in_cos);
-	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, true);
-	channel_config_set_ring(&dma_conf, true, RX_BITS_DEPTH);
-	channel_config_set_dreq(&dma_conf, pio_get_dreq(pio1, 2, false));
-	channel_config_set_chain_to(&dma_conf, dma_ch_in_sin);
-	dma_channel_configure(dma_ch_in_cos, &dma_conf, rx_cos, &pio1->rxf[2], 2, false);
-
-	dma_conf = dma_channel_get_default_config(dma_ch_in_sin);
-	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, true);
-	channel_config_set_ring(&dma_conf, true, RX_BITS_DEPTH);
-	channel_config_set_dreq(&dma_conf, pio_get_dreq(pio1, 3, false));
-	channel_config_set_chain_to(&dma_conf, dma_ch_in_cos);
-	dma_channel_configure(dma_ch_in_sin, &dma_conf, rx_sin, &pio1->rxf[3], 2, true);
-
-	multicore_launch_core1(rf_rx);
-
-	int8_t *block;
 
 	while (queue_try_remove(&iq_queue, &block))
 		/* Flush the queue */;
 
-	pwm_set_enabled(slice, true);
+	rf_rx_start();
+	sleep_us(100);
 
-	unsigned addr = 0;
+	multicore_launch_core1(rf_rx);
-	int prev = 0;
-	int h0 = 0, h1 = 0, h2 = 0, h3 = 0, h4 = 0, h5 = 0, h6 = 0, h7 = 0;
-	int k0 = 0, k1 = 0, k2 = 0, k3 = 0, k4 = 0, k5 = 0, k6 = 0, k7 = 0;
 
 	while (true) {
 		int cmd;
@@ -705,62 +588,23 @@ static void do_rx(int rx_pin, int bias_pin)
 				goto done;
 
 		if (queue_try_remove(&iq_queue, &block)) {
-			for (int i = 0; i < IQ_BLOCK_LEN; i += 2) {
+			fwrite(block, IQ_BLOCK_LEN, 1, stdout);
-				while (addr == dma_hw->ch[dma_ch_aout].read_addr)
+			fflush(stdout);
-					sleep_us(1);
+		} else {
+			int wait = rnd_next() & 0x1fff;
 
-				addr += 2;
+			for (int i = 0; i < wait; i++)
-
+				asm volatile("nop");
-				if (addr >= (unsigned)aout_buf + AOUT_WORDS * 2)
-					addr = (unsigned)aout_buf;
-
-				unsigned pos = addr - (unsigned)aout_buf;
-				pos /= 2;
-				pos += AOUT_WORDS - 2;
-				pos &= AOUT_WORDS - 1;
-
-				//int sample = (127 / M_PI) * atan2f(block[i + 1], block[i]);
-				int sample = fast_atan2(block[i + 1], block[i]) / (CORDIC_PI / 128);
-				int phase = sample - prev;
-				prev = sample;
-
-				uint8_t audio = 128 + (uint8_t)phase;
-
-				h7 = h6, h6 = h5, h5 = h4, h4 = h3, h3 = h2, h2 = h1, h1 = h0,
-				h0 = audio;
-				audio = (h0 + h1 + h2 + h3 + h4 + h5 + h6 + h7) / 8;
-
-				k7 = k6, k6 = k5, k5 = k4, k4 = k3, k3 = k2, k2 = k1, k1 = k0,
-				k0 = audio;
-				audio = (k0 + k1 + k2 + k3 + k4 + k5 + k6 + k7) / 8;
-
-				aout_buf[pos] = audio;
-				block[i] = audio;
-				block[i + 1] = audio;
-			}
-
-			//fwrite(block, IQ_BLOCK_LEN, 1, stdout);
-			//fflush(stdout);
 		}
 	}
 
 done:
-	pwm_set_enabled(slice, false);
 	multicore_fifo_push_blocking(0);
 	multicore_fifo_pop_blocking();
-	sleep_us(10);
+	sleep_us(100);
 	multicore_reset_core1();
 
 	rf_rx_stop();
-
-	dma_channel_abort(dma_ch_in_cos);
-	dma_channel_abort(dma_ch_in_sin);
-	dma_channel_cleanup(dma_ch_in_cos);
-	dma_channel_cleanup(dma_ch_in_sin);
-	dma_channel_unclaim(dma_ch_in_cos);
-	dma_channel_unclaim(dma_ch_in_sin);
-	dma_ch_in_cos = -1;
-	dma_ch_in_sin = -1;
 }
 
 int main()
@@ -771,24 +615,11 @@ int main()
 			CLK_SYS_HZ);
 
 	/* Enable PSU PWM mode. */
-	gpio_disable_pulls(PSU_PIN);
+	gpio_init(PSU_PIN);
-	gpio_set_function(PSU_PIN, GPIO_FUNC_SIO);
 	gpio_set_dir(PSU_PIN, GPIO_OUT);
 	gpio_put(PSU_PIN, 1);
 
-	/* Configure audio output. */
+	/* Prioritize DMA over CPU. */
-	slice = pwm_gpio_to_slice_num(AOUT_PIN);
-	chan = pwm_gpio_to_channel(AOUT_PIN);
-	gpio_set_drive_strength(AOUT_PIN, GPIO_DRIVE_STRENGTH_2MA);
-	gpio_set_slew_rate(AOUT_PIN, GPIO_SLEW_RATE_SLOW);
-	pwm_set_clkdiv_int_frac(slice, 1, 0);
-	pwm_set_enabled(slice, false);
-	pwm_set_wrap(slice, 2048);
-	pwm_set_chan_level(slice, chan, 0);
-
-	gpio_disable_pulls(AOUT_PIN);
-	gpio_set_function(AOUT_PIN, GPIO_FUNC_PWM);
-
 	bus_ctrl_hw->priority |= BUSCTRL_BUS_PRIORITY_DMA_W_BITS | BUSCTRL_BUS_PRIORITY_DMA_R_BITS;
 
 	stdio_usb_init();
@@ -796,11 +627,11 @@ int main()
 
 	queue_init(&iq_queue, sizeof(uint8_t *), IQ_QUEUE_LEN);
 
-	rx_lo_init(INIT_FREQ - INIT_SAMPLE_RATE, true);
+	rx_lo_init(frequency + sample_rate, true);
 
-	run_command(0x02, 200000);
+	/* We need to have the sampling timer ready. */
-	run_command(0x01, 88200000);
+	dma_t_samp = dma_claim_unused_timer(true);
-	do_rx(10, 11);
+	dma_timer_set_fraction(dma_t_samp, 1, CLK_SYS_HZ / (sample_rate * DECIMATE));
 
 	while (true) {
 		if (check_command() > 0) {
@@ -810,7 +641,7 @@ int main()
 			fwrite(header, sizeof header, 1, stdout);
 			fflush(stdout);
 
-			do_rx(10, 11);
+			do_rx();
 		}
 
 		sleep_ms(10);

src/vendor/pico-stdio-usb-simple

@@ -1 +1 @@
-Subproject commit b6b09e34b844326a156bc6734146a88111138473
+Subproject commit ed4858dda407ec66626aade9b7c6ad6016539f4c