pico-sdr/src/main.c
2025-06-08 12:00:40 +02:00

654 lines
17 KiB
C

#include <pico/stdlib.h>
#include <pico/stdio_usb.h>
#include <pico/multicore.h>
#include <pico/util/queue.h>
#include <hardware/clocks.h>
#include <hardware/dma.h>
#include <hardware/gpio.h>
#include <hardware/pll.h>
#include <hardware/vreg.h>
#include <hardware/sync.h>
#include <hardware/pio.h>
#include <hardware/pwm.h>
#include <hardware/interp.h>
#include <hardware/regs/clocks.h>
#include <hardware/structs/bus_ctrl.h>
#include <math.h>
#include <stdio.h>
#include <limits.h>
#include <stdlib.h>
#define VREG_VOLTAGE VREG_VOLTAGE_1_20
#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 8
#define XOR_ADDR 0x1000
#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 INIT_SAMPLE_RATE 100000
#define INIT_FREQ 94600000
#define BASE_GAIN (1 << 15)
#define ATTN_BITS 8
static int sample_rate = INIT_SAMPLE_RATE;
static int gain = BASE_GAIN / (CLK_SYS_HZ / INIT_SAMPLE_RATE / 2);
static int frequency = INIT_FREQ;
static queue_t iq_queue;
static uint8_t iq_queue_buffer[IQ_QUEUE_LEN][IQ_BLOCK_LEN];
static size_t iq_queue_pos = 0;
static uint32_t xorshift_seed;
static inline __unused uint32_t xorshift(void)
{
uint32_t x = xorshift_seed;
x ^= x << 13;
x ^= x >> 17;
x ^= x << 5;
xorshift_seed = x;
return x;
}
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;
static int dma_ch_cp = -1;
static int dma_ch_cos = -1;
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_cos = -1;
static int dma_ch_samp_sin = -1;
static int dma_t_samp = -1;
static int origin_rx = -1;
static int origin_bias = -1;
static int origin_adder = 0;
static void init_rx()
{
gpio_disable_pulls(RX_PIN);
pio_gpio_init(PIO, RX_PIN);
const uint16_t insn[] = {
pio_encode_in(pio_pins, 1) | pio_encode_delay(0),
};
pio_program_t prog = {
.instructions = insn,
.length = sizeof(insn) / sizeof(*insn),
.origin = origin_rx,
};
if (pio_can_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, 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, SM_RX, origin_rx, &pc);
pio_sm_set_consecutive_pindirs(PIO, SM_RX, RX_PIN, 1, GPIO_IN);
}
static void init_bias()
{
gpio_disable_pulls(RX_PIN);
gpio_disable_pulls(FB_PIN);
pio_gpio_init(PIO, FB_PIN);
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 = origin_bias,
};
if (pio_can_add_program(PIO, &prog))
origin_bias = pio_add_program(PIO, &prog);
pio_sm_config pc = pio_get_default_sm_config();
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[] = {
/* y has weight of 2, x has weight of 1 */
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_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 + 13);
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, SM_COS, origin_adder, &pc);
pio_sm_init(PIO, SM_SIN, origin_adder, &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++) {
uint32_t bits = 0;
for (int j = 0; j < 32; j++) {
bits |= phase >> 31;
bits <<= 1;
phase += step;
}
buf[i] = bits;
}
}
static void rx_lo_init(double req_freq, bool align)
{
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;
uint32_t step = STEP_BASE * freq;
lo_generate_phase(lo_cos, LO_WORDS, step, COS_PHASE);
lo_generate_phase(lo_sin, LO_WORDS, step, SIN_PHASE);
}
static void rf_rx_start()
{
dma_ch_rx = dma_claim_unused_channel(true);
dma_ch_cp = dma_claim_unused_channel(true);
dma_ch_cos = dma_claim_unused_channel(true);
dma_ch_sin = dma_claim_unused_channel(true);
dma_ch_pio_cos = dma_claim_unused_channel(true);
dma_ch_pio_sin = dma_claim_unused_channel(true);
dma_ch_samp_cos = dma_claim_unused_channel(true);
dma_ch_samp_sin = dma_claim_unused_channel(true);
dma_channel_config dma_conf;
/* Read received word into accumulator I. */
dma_conf = dma_channel_get_default_config(dma_ch_rx);
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, SM_RX, false));
channel_config_set_chain_to(&dma_conf, dma_ch_cp);
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);
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_cos);
dma_channel_configure(dma_ch_cp, &dma_conf, LO_SIN_ACCUMULATOR, LO_COS_ACCUMULATOR, 1,
false);
/* Read lo_cos into accumulator I with XOR. */
dma_conf = dma_channel_get_default_config(dma_ch_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, LO_BITS_DEPTH);
channel_config_set_chain_to(&dma_conf, dma_ch_sin);
dma_channel_configure(dma_ch_cos, &dma_conf, LO_COS_ACCUMULATOR + XOR_ADDR / 4, lo_cos, 1,
false);
/* Read lo_sin into accumulator Q with XOR. */
dma_conf = dma_channel_get_default_config(dma_ch_sin);
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, LO_BITS_DEPTH);
channel_config_set_chain_to(&dma_conf, dma_ch_pio_cos);
dma_channel_configure(dma_ch_sin, &dma_conf, LO_SIN_ACCUMULATOR + XOR_ADDR / 4, lo_sin, 1,
false);
/* Copy mixed I accumulator to PIO adder I. */
dma_conf = dma_channel_get_default_config(dma_ch_pio_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, false);
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, &PIO->txf[SM_COS], LO_COS_ACCUMULATOR, 1,
false);
/* Copy mixed Q accumulator to PIO adder Q. */
dma_conf = dma_channel_get_default_config(dma_ch_pio_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, false);
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, &PIO->txf[SM_SIN], LO_SIN_ACCUMULATOR, 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, 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_sin);
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, false);
channel_config_set_write_increment(&dma_conf, false);
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_sin, &dma_conf, &PIO->sm[SM_SIN].instr, &samp_insn, 1,
false);
init_bias();
init_adder();
init_rx();
dma_channel_start(dma_ch_rx);
dma_channel_start(dma_ch_samp_cos);
pio_set_sm_mask_enabled(PIO, 0x0f, true);
}
static void rf_rx_stop(void)
{
pio_set_sm_mask_enabled(PIO, 0x0f, false);
pio_sm_restart(PIO, 0);
pio_sm_restart(PIO, 1);
pio_sm_restart(PIO, 2);
pio_sm_restart(PIO, 3);
pio_sm_clear_fifos(PIO, 0);
pio_sm_clear_fifos(PIO, 1);
pio_sm_clear_fifos(PIO, 2);
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);
dma_channel_abort(dma_ch_sin);
dma_channel_abort(dma_ch_pio_cos);
dma_channel_abort(dma_ch_pio_sin);
dma_channel_abort(dma_ch_samp_cos);
dma_channel_abort(dma_ch_samp_sin);
dma_channel_cleanup(dma_ch_rx);
dma_channel_cleanup(dma_ch_cp);
dma_channel_cleanup(dma_ch_cos);
dma_channel_cleanup(dma_ch_sin);
dma_channel_cleanup(dma_ch_pio_cos);
dma_channel_cleanup(dma_ch_pio_sin);
dma_channel_cleanup(dma_ch_samp_cos);
dma_channel_cleanup(dma_ch_samp_sin);
dma_channel_unclaim(dma_ch_rx);
dma_channel_unclaim(dma_ch_cp);
dma_channel_unclaim(dma_ch_cos);
dma_channel_unclaim(dma_ch_sin);
dma_channel_unclaim(dma_ch_pio_cos);
dma_channel_unclaim(dma_ch_pio_sin);
dma_channel_unclaim(dma_ch_samp_cos);
dma_channel_unclaim(dma_ch_samp_sin);
dma_ch_rx = -1;
dma_ch_cp = -1;
dma_ch_cos = -1;
dma_ch_sin = -1;
dma_ch_pio_cos = -1;
dma_ch_pio_sin = -1;
dma_ch_samp_cos = -1;
dma_ch_samp_sin = -1;
}
inline static void led_set(bool on)
{
gpio_put(PICO_DEFAULT_LED_PIN, on);
}
inline static uint32_t pio_sm_get_blocking_unsafe(pio_hw_t *pio, int sm)
{
while (pio->fstat & (1u << (PIO_FSTAT_RXEMPTY_LSB + sm)))
asm volatile("nop");
return pio->rxf[sm];
}
inline static int nextI()
{
static int prevI2, prevI1, prevI;
int sI2 = pio_sm_get_blocking_unsafe(PIO, SM_COS);
int I2 = sI2 - prevI2;
prevI2 = sI2;
int sI1 = pio_sm_get_blocking_unsafe(PIO, SM_COS);
int I1 = sI1 - prevI1;
prevI1 = sI1;
int sI = I2 + I2 + I1;
int I = sI - prevI;
prevI = sI;
return I;
}
inline static int nextQ()
{
static int prevQ2, prevQ1, prevQ;
int sQ2 = pio_sm_get_blocking_unsafe(PIO, SM_SIN);
int Q2 = sQ2 - prevQ2;
prevQ2 = sQ2;
int sQ1 = pio_sm_get_blocking_unsafe(PIO, SM_SIN);
int Q1 = sQ1 - prevQ1;
prevQ1 = sQ1;
int sQ = Q2 + Q2 + Q1;
int Q = sQ - prevQ;
prevQ = sQ;
return Q;
}
static void rf_rx(void)
{
while (true) {
if (multicore_fifo_rvalid()) {
multicore_fifo_pop_blocking();
multicore_fifo_push_blocking(0);
return;
}
uint8_t *block = iq_queue_buffer[iq_queue_pos];
uint8_t *blockptr = block;
for (int i = 0; i < IQ_SAMPLES; i++) {
int I = 0, Q = 0;
Q += nextQ();
I += nextI();
I -= nextQ();
Q += nextI();
Q -= nextQ();
I -= nextI();
I += nextQ();
Q -= nextI();
I *= gain;
I >>= ATTN_BITS;
*blockptr++ = I + 128;
Q *= gain;
Q >>= ATTN_BITS;
*blockptr++ = Q + 128;
}
if (queue_try_add(&iq_queue, &block)) {
iq_queue_pos = (iq_queue_pos + 1) % IQ_QUEUE_LEN;
led_set(0);
} else {
led_set(1);
}
}
}
static void run_command(uint8_t cmd, uint32_t arg)
{
if (0x01 == cmd) {
/* Tune to a new center frequency */
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;
gain = BASE_GAIN / (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);
}
}
static int check_command(void)
{
static uint8_t buf[5];
static int pos = 0;
int c;
while ((c = getchar_timeout_us(0)) >= 0) {
if (0 == pos && 0 == c)
return 0;
buf[pos++] = c;
if (5 == pos) {
uint32_t arg = (buf[1] << 24) | (buf[2] << 16) | (buf[3] << 8) | buf[4];
run_command(buf[0], arg);
pos = 0;
return buf[0];
}
}
return -1;
}
static void do_rx()
{
const uint8_t *block;
while (queue_try_remove(&iq_queue, &block))
/* Flush the queue */;
rf_rx_start();
sleep_us(100);
multicore_launch_core1(rf_rx);
while (true) {
int cmd;
while ((cmd = check_command()) >= 0)
if (0 == cmd)
goto done;
if (queue_try_remove(&iq_queue, &block)) {
fwrite(block, IQ_BLOCK_LEN, 1, stdout);
fflush(stdout);
} else {
int wait = xorshift() >> (32 - 15);
for (int i = 0; i < wait; i++)
asm volatile("nop");
}
}
done:
multicore_fifo_push_blocking(0);
multicore_fifo_pop_blocking();
sleep_us(100);
multicore_reset_core1();
rf_rx_stop();
}
int main()
{
vreg_set_voltage(VREG_VOLTAGE);
set_sys_clock_khz(CLK_SYS_HZ / KHZ, true);
clock_configure(clk_peri, 0, CLOCKS_CLK_PERI_CTRL_AUXSRC_VALUE_CLKSRC_PLL_SYS, CLK_SYS_HZ,
CLK_SYS_HZ);
/* Enable PSU PWM mode. */
gpio_init(PSU_PIN);
gpio_set_dir(PSU_PIN, GPIO_OUT);
gpio_put(PSU_PIN, 1);
gpio_init(PICO_DEFAULT_LED_PIN);
gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
gpio_put(PICO_DEFAULT_LED_PIN, 0);
/* Prioritize DMA over CPU. */
bus_ctrl_hw->priority |= BUSCTRL_BUS_PRIORITY_DMA_W_BITS | BUSCTRL_BUS_PRIORITY_DMA_R_BITS;
stdio_usb_init();
setvbuf(stdout, NULL, _IONBF, 0);
queue_init(&iq_queue, sizeof(uint8_t *), IQ_QUEUE_LEN);
rx_lo_init(frequency + sample_rate, true);
/* We need to have the sampling timer ready. */
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) {
static const uint32_t header[3] = { __builtin_bswap32(0x52544c30),
__builtin_bswap32(5),
__builtin_bswap32(29) };
fwrite(header, sizeof header, 1, stdout);
fflush(stdout);
do_rx();
}
sleep_ms(10);
}
}