mordae/pico-sdr
1
2
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity
pico-sdr/src/main.c
Jan Hamal Dvořák 06695041ba Fix sin/cos
2024-08-14 00:30:02 +02:00

898 lines
25 KiB
C
Raw Permalink Blame History

#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>
#include <string.h>
#define VREG_VOLTAGE VREG_VOLTAGE_1_20
#define CLK_SYS_HZ (300 * MHZ)
/* Pin mapping */
#define PIN_RX0 6
#define PIN_RX1 7
#define PIN_RX2 8
#define PIN_RX3 9
#define PIN_FB0 10
#define PIN_FB1 11
#define PIN_FB2 12
#define PIN_FB3 13
#define PIN_A 14
#define PIN_B 15
#define PSU_PIN 23
#define SM_LO 0
#define SM_FB 1
#define SM_RXI 2
#define SM_RXQ 3
#define SM_ACCI0 0
#define SM_ACCI1 1
#define SM_ACCQ0 2
#define SM_ACCQ1 3
/* PIO code origins */
static int8_t origin_lo = -1;
static int8_t origin_fb = -1;
static int8_t origin_rx = 0;
static int8_t origin_acc = 0;
/*
* NCO (Numerically Controlled Oscillator)
* Must have 256 phases with 256 bytes each for 1-byte DMA writes to work.
*/
#define NCO_NUM_PHASES 256
#define NCO_PHASE_BITS 8
#define NCO_PHASE_WORDS (1 << (NCO_PHASE_BITS - 2))
#define NCO_PHASE_COS 0
#define NCO_PHASE_SIN (3u << 30)
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)nco_phase;
static uint32_t nco_step = 0x80000000;
static uint32_t nco_null = 0;
/* Bit combinations to output for { I+ Q+ I- Q- } */
static const uint32_t nco_quadrature[] = { 3, 2, 0, 1 };
/* Sampling and gain */
#define INIT_SAMPLE_RATE 200000
#define INIT_FREQ 94600000
#define INIT_GAIN 127
#define NUM_GAINS 29
#define DECIMATE 4
#define LPF DECIMATE
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 frequency = INIT_FREQ;
static int sample_rate = INIT_SAMPLE_RATE;
static int max_amplitude = CLK_SYS_HZ / INIT_SAMPLE_RATE;
/* Whenever we need an extra parameter, misuses PPM. */
static int tweak = 0;
/* Output queue */
#define IQ_SAMPLES 32
#define IQ_BLOCK_LEN (2 * IQ_SAMPLES)
#define IQ_QUEUE_LEN 8
static queue_t iq_queue;
static uint8_t iq_queue_buffer[IQ_QUEUE_LEN][IQ_BLOCK_LEN];
static size_t iq_queue_pos = 0;
#define RX_BIT_DEPTH 11
#define RX_WORDS (1 << (RX_BIT_DEPTH - 1))
/* NCO phase accumulation, address preparation, LO triggering */
static int dma_ch_nco1 = -1;
static int dma_ch_nco2 = -1;
static int dma_ch_nco3 = -1;
static int dma_ch_nco4 = -1;
/* Driving multiplexer A, B pins using NCO data */
static int dma_ch_lo = -1;
/* Receiving [I+, I-] and [Q+, Q-] data */
static int dma_ch_rxi = -1;
static int dma_ch_rxq = -1;
/* Sampling the accumulators */
static int dma_ch_samp_i0 = -1;
static int dma_ch_samp_i1 = -1;
static int dma_ch_samp_q0 = -1;
static int dma_ch_samp_q1 = -1;
/* Sampling rate limiter */
static int dma_t_samp = -1;
/* Sampling instruction and DMA script. */
static const uint32_t samp_insn = 16;
/* Random number generator */
static uint32_t rnd = 0;
inline static __unused uint32_t rnd_next()
{
rnd = rnd * 0x41c64e6d + 12345;
return rnd;
}
/*
* Remove chaining on a given DMA channel.
* Handy when one wants to abort a chained DMA channel.
*/
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;
}
/*
* Read next sample from PIO FIFO without any checks.
* Blocks until one is available.
*/
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];
}
static void init_lo()
{
gpio_disable_pulls(PIN_A);
gpio_disable_pulls(PIN_B);
pio_gpio_init(pio0, PIN_A);
pio_gpio_init(pio0, PIN_B);
gpio_set_drive_strength(PIN_A, GPIO_DRIVE_STRENGTH_12MA);
gpio_set_drive_strength(PIN_B, GPIO_DRIVE_STRENGTH_12MA);
gpio_set_slew_rate(PIN_A, GPIO_SLEW_RATE_FAST);
gpio_set_slew_rate(PIN_B, GPIO_SLEW_RATE_FAST);
const uint16_t insn[] = {
pio_encode_out(pio_pins, 2),
};
pio_program_t prog = {
.instructions = insn,
.length = sizeof(insn) / sizeof(*insn),
.origin = origin_lo,
};
pio_sm_restart(pio0, SM_LO);
pio_sm_clear_fifos(pio0, SM_LO);
if (pio_can_add_program(pio0, &prog))
origin_lo = pio_add_program(pio0, &prog);
pio_sm_config pc = pio_get_default_sm_config();
sm_config_set_out_pins(&pc, PIN_A, 2);
sm_config_set_set_pins(&pc, PIN_A, 2);
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(pio0, SM_LO, origin_lo, &pc);
pio_sm_set_consecutive_pindirs(pio0, SM_LO, PIN_A, 2, GPIO_OUT);
pio_sm_exec_wait_blocking(pio0, SM_LO, pio_encode_set(pio_pins, 0));
}
static void init_fb()
{
gpio_disable_pulls(PIN_FB0);
gpio_disable_pulls(PIN_FB1);
gpio_disable_pulls(PIN_FB2);
gpio_disable_pulls(PIN_FB3);
pio_gpio_init(pio0, PIN_FB0);
pio_gpio_init(pio0, PIN_FB1);
pio_gpio_init(pio0, PIN_FB2);
pio_gpio_init(pio0, PIN_FB3);
gpio_set_drive_strength(PIN_FB0, GPIO_DRIVE_STRENGTH_2MA);
gpio_set_drive_strength(PIN_FB1, GPIO_DRIVE_STRENGTH_2MA);
gpio_set_drive_strength(PIN_FB2, GPIO_DRIVE_STRENGTH_2MA);
gpio_set_drive_strength(PIN_FB3, GPIO_DRIVE_STRENGTH_2MA);
gpio_set_slew_rate(PIN_FB0, GPIO_SLEW_RATE_SLOW);
gpio_set_slew_rate(PIN_FB1, GPIO_SLEW_RATE_SLOW);
gpio_set_slew_rate(PIN_FB2, GPIO_SLEW_RATE_SLOW);
gpio_set_slew_rate(PIN_FB3, GPIO_SLEW_RATE_SLOW);
const uint16_t insn[] = {
pio_encode_mov_not(pio_pins, pio_pins) | pio_encode_sideset(4, 0x0f),
};
pio_program_t prog = {
.instructions = insn,
.length = sizeof(insn) / sizeof(*insn),
.origin = origin_fb,
};
pio_sm_restart(pio0, SM_FB);
pio_sm_clear_fifos(pio0, SM_FB);
if (pio_can_add_program(pio0, &prog))
origin_fb = pio_add_program(pio0, &prog);
pio_sm_config pc = pio_get_default_sm_config();
sm_config_set_sideset(&pc, 4, false, true);
sm_config_set_in_pins(&pc, PIN_RX0);
sm_config_set_out_pins(&pc, PIN_FB0, 4);
sm_config_set_set_pins(&pc, PIN_FB0, 4);
sm_config_set_sideset_pins(&pc, PIN_FB0);
sm_config_set_wrap(&pc, origin_fb, origin_fb + prog.length - 1);
sm_config_set_clkdiv_int_frac(&pc, 1, 0);
pio_sm_init(pio0, SM_FB, origin_fb, &pc);
pio_sm_set_consecutive_pindirs(pio0, SM_FB, PIN_FB0, 4, GPIO_OUT);
}
static void init_rx()
{
gpio_disable_pulls(PIN_RX0);
gpio_disable_pulls(PIN_RX1);
gpio_disable_pulls(PIN_RX2);
gpio_disable_pulls(PIN_RX3);
pio_gpio_init(pio0, PIN_RX0);
pio_gpio_init(pio0, PIN_RX1);
pio_gpio_init(pio0, PIN_RX2);
pio_gpio_init(pio0, PIN_RX3);
gpio_set_input_hysteresis_enabled(PIN_RX0, false);
gpio_set_input_hysteresis_enabled(PIN_RX1, false);
gpio_set_input_hysteresis_enabled(PIN_RX2, false);
gpio_set_input_hysteresis_enabled(PIN_RX3, false);
hw_set_bits(&pio0->input_sync_bypass,
(1u << PIN_RX0) | (1u << PIN_RX1) | (1u << PIN_RX2) | (1u << PIN_RX3));
const uint16_t insn[] = {
pio_encode_in(pio_pins, 2) | pio_encode_delay(0),
};
pio_program_t prog = {
.instructions = insn,
.length = sizeof(insn) / sizeof(*insn),
.origin = origin_rx,
};
pio_sm_restart(pio0, SM_RXI);
pio_sm_restart(pio0, SM_RXQ);
pio_sm_clear_fifos(pio0, SM_RXI);
pio_sm_clear_fifos(pio0, SM_RXQ);
if (pio_can_add_program(pio0, &prog))
origin_rx = pio_add_program(pio0, &prog);
pio_sm_config pc = pio_get_default_sm_config();
sm_config_set_in_pins(&pc, PIN_RX0);
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(pio0, SM_RXI, origin_rx, &pc);
sm_config_set_in_pins(&pc, PIN_RX2);
pio_sm_init(pio0, SM_RXQ, origin_rx, &pc);
pio_sm_set_consecutive_pindirs(pio0, SM_RXI, PIN_RX0, 2, GPIO_IN);
pio_sm_set_consecutive_pindirs(pio0, SM_RXQ, PIN_RX2, 2, GPIO_IN);
}
static void init_iq()
{
/*
* Samples arrive interleaved as [+, -, +, -].
*
* That means we need to swap the sign of the negative samples
* inside the lookup table to arrive at correct unsigned total.
*/
const uint16_t insn[] = {
// // nom. swap tot.
pio_encode_jmp_y_dec(5), // 0000 0101 +2
pio_encode_jmp_x_dec(5), // 0001 0100 +1
pio_encode_jmp_y_dec(1), // 0010 0111 +2 +1
pio_encode_jmp_y_dec(5), // 0011 0110 +2
pio_encode_jmp_x_dec(5), // 0100 0001 +1
pio_encode_out(pio_pc, 4), // 0101 0000 --
pio_encode_jmp_y_dec(5), // 0110 0011 +2
pio_encode_jmp_x_dec(5), // 0111 0010 +1
pio_encode_jmp_y_dec(1), // 1000 1101 +2 +1
pio_encode_jmp_y_dec(5), // 1001 1100 +2
pio_encode_jmp_y_dec(0), // 1010 1111 +2 +2
pio_encode_jmp_y_dec(1), // 1011 1110 +2 +1
pio_encode_jmp_y_dec(5), // 1100 1001 +2
pio_encode_jmp_x_dec(5), // 1101 1000 +1
pio_encode_jmp_y_dec(1), // 1110 1011 +2 +1
pio_encode_jmp_y_dec(5), // 1111 1010 +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_acc,
};
pio_sm_restart(pio1, SM_ACCI0);
pio_sm_restart(pio1, SM_ACCI1);
pio_sm_restart(pio1, SM_ACCQ0);
pio_sm_restart(pio1, SM_ACCQ1);
pio_sm_clear_fifos(pio1, SM_ACCI0);
pio_sm_clear_fifos(pio1, SM_ACCI1);
pio_sm_clear_fifos(pio1, SM_ACCQ0);
pio_sm_clear_fifos(pio1, SM_ACCQ1);
if (pio_can_add_program(pio1, &prog))
origin_acc = pio_add_program(pio1, &prog);
pio_sm_config pc = pio_get_default_sm_config();
sm_config_set_wrap(&pc, origin_acc, origin_acc + 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, SM_ACCI0, origin_acc + 5, &pc);
pio_sm_init(pio1, SM_ACCI1, origin_acc + 5, &pc);
pio_sm_init(pio1, SM_ACCQ0, origin_acc + 5, &pc);
pio_sm_init(pio1, SM_ACCQ1, origin_acc + 5, &pc);
}
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 < 16; j++) {
uint32_t s = (((phase + NCO_PHASE_COS) >> 31) << 1) |
((phase + NCO_PHASE_SIN) >> 31);
bits |= nco_quadrature[s];
bits <<= 2;
phase += step;
}
buf[i] = bits;
}
}
inline static uint32_t step_from_freq(uint32_t freq)
{
uint64_t tmp = freq;
tmp <<= 32;
tmp /= CLK_SYS_HZ;
return tmp;
}
static void rx_lo_init(uint32_t freq)
{
uint32_t step = step_from_freq(freq);
for (uint32_t i = 0; i < NCO_NUM_PHASES; i++)
lo_generate_phase(nco_phase[i], NCO_PHASE_WORDS, step, i << 24);
nco_step = step * 16 * NCO_PHASE_WORDS;
}
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_lo = dma_claim_unused_channel(true);
dma_ch_rxi = dma_claim_unused_channel(true);
dma_ch_rxq = dma_claim_unused_channel(true);
dma_ch_samp_i0 = dma_claim_unused_channel(true);
dma_ch_samp_i1 = dma_claim_unused_channel(true);
dma_ch_samp_q0 = dma_claim_unused_channel(true);
dma_ch_samp_q1 = dma_claim_unused_channel(true);
dma_channel_config dma_conf;
/*
* Step the NCO
*
* We are using the DMA sniffer to hold the accumulated phase.
* Since our pregenerated phase data hold both cosine and sine
* bits, we can manage with just one such accumulator.
*/
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 shall increment the phase accumulator. */
dma_sniffer_enable(dma_ch_nco1, DMA_SNIFF_CTRL_CALC_VALUE_SUM, true);
/*
* Prepare the phase address
*
* We cannot use 1-byte write to modify the trigger register directly,
* because the logic would distribute the byte across whole word.
* We can target a single byte inside RAM, though.
*/
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 generated 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_lo].al3_read_addr_trig,
&nco_addr, 1, false);
/*
* Drive the LO bits
*
* We are driving the quadrature mixer, actually, but I guess this
* arrangement counts as LO too. We output 2 bits per tick. Once we
* finish outputing the whole phase buffer (in 1024 ticks), we circle
* back to the NCO.
*/
dma_conf = dma_channel_get_default_config(dma_ch_lo);
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(pio0, SM_LO, GPIO_OUT));
channel_config_set_chain_to(&dma_conf, dma_ch_nco1);
dma_channel_configure(dma_ch_lo, &dma_conf, &pio0->txf[SM_LO], NULL, NCO_PHASE_WORDS,
false);
/*
* Read the incoming bits
*
* We have a single PIO per channel, reading 2 bits per tick.
* Our adder cannot cope with that, because it processes at worst
* 4 bits per 3 ticks. Thus we use two accumulators per channel.
*
* To save on DMA channels, we make use of the fact that the FIFOs are
* arranged sequentially in memory and interleave the accumulators.
* Since we are sure that accumulators are faster than receivers,
* we can safely block on receiver DREQ.
*/
dma_conf = dma_channel_get_default_config(dma_ch_rxi);
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, 3);
channel_config_set_dreq(&dma_conf, pio_get_dreq(pio0, SM_RXI, GPIO_IN));
channel_config_set_chain_to(&dma_conf, dma_ch_rxq);
dma_channel_configure(dma_ch_rxi, &dma_conf, &pio1->txf[SM_ACCI0], &pio0->rxf[SM_RXI], 1,
false);
dma_conf = dma_channel_get_default_config(dma_ch_rxq);
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, 3);
channel_config_set_dreq(&dma_conf, pio_get_dreq(pio0, SM_RXQ, GPIO_IN));
channel_config_set_chain_to(&dma_conf, dma_ch_rxi);
dma_channel_configure(dma_ch_rxq, &dma_conf, &pio1->txf[SM_ACCQ0], &pio0->rxf[SM_RXQ], 1,
false);
/*
* Trigger accumulator values push
*
* We need to inject a jump instruction stored inside samp_insn to all
* four accumulators at the pace given by the timer to make them emit
* current totals and zero the counters.
*/
dma_conf = dma_channel_get_default_config(dma_ch_samp_i0);
channel_config_set_high_priority(&dma_conf, true);
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_chain_to(&dma_conf, dma_ch_samp_i1);
dma_channel_configure(dma_ch_samp_i0, &dma_conf, &pio1->sm[SM_ACCI0].instr, &samp_insn, 1,
false);
dma_conf = dma_channel_get_default_config(dma_ch_samp_i1);
channel_config_set_high_priority(&dma_conf, true);
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_samp_i0);
dma_channel_configure(dma_ch_samp_i1, &dma_conf, &pio1->sm[SM_ACCI1].instr, &samp_insn, 1,
false);
dma_conf = dma_channel_get_default_config(dma_ch_samp_q0);
channel_config_set_high_priority(&dma_conf, true);
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_chain_to(&dma_conf, dma_ch_samp_q1);
dma_channel_configure(dma_ch_samp_q0, &dma_conf, &pio1->sm[SM_ACCQ0].instr, &samp_insn, 1,
false);
dma_conf = dma_channel_get_default_config(dma_ch_samp_q1);
channel_config_set_high_priority(&dma_conf, true);
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_samp_q0);
dma_channel_configure(dma_ch_samp_q1, &dma_conf, &pio1->sm[SM_ACCQ1].instr, &samp_insn, 1,
false);
init_iq();
init_lo();
init_rx();
init_fb();
pio_set_sm_mask_enabled(pio1, 0x0f, true);
pio_set_sm_mask_enabled(pio0, 0x0f, true);
dma_channel_start(dma_ch_nco1);
dma_channel_start(dma_ch_rxi);
dma_channel_start(dma_ch_samp_i0);
dma_channel_start(dma_ch_samp_q0);
}
static void rf_rx_stop(void)
{
pio_set_sm_mask_enabled(pio0, 0x0f, false);
pio_set_sm_mask_enabled(pio1, 0x0f, false);
sleep_us(10);
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_lo);
dma_channel_clear_chain_to(dma_ch_rxi);
dma_channel_clear_chain_to(dma_ch_rxq);
dma_channel_clear_chain_to(dma_ch_samp_i0);
dma_channel_clear_chain_to(dma_ch_samp_i1);
dma_channel_clear_chain_to(dma_ch_samp_q0);
dma_channel_clear_chain_to(dma_ch_samp_q1);
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_lo);
dma_channel_abort(dma_ch_rxi);
dma_channel_abort(dma_ch_rxq);
dma_channel_abort(dma_ch_samp_i0);
dma_channel_abort(dma_ch_samp_i1);
dma_channel_abort(dma_ch_samp_q0);
dma_channel_abort(dma_ch_samp_q1);
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_lo);
dma_channel_cleanup(dma_ch_rxi);
dma_channel_cleanup(dma_ch_rxq);
dma_channel_cleanup(dma_ch_samp_i0);
dma_channel_cleanup(dma_ch_samp_i1);
dma_channel_cleanup(dma_ch_samp_q0);
dma_channel_cleanup(dma_ch_samp_q1);
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_lo);
dma_channel_unclaim(dma_ch_rxi);
dma_channel_unclaim(dma_ch_rxq);
dma_channel_unclaim(dma_ch_samp_i0);
dma_channel_unclaim(dma_ch_samp_i1);
dma_channel_unclaim(dma_ch_samp_q0);
dma_channel_unclaim(dma_ch_samp_q1);
dma_ch_nco1 = -1;
dma_ch_nco2 = -1;
dma_ch_nco3 = -1;
dma_ch_nco4 = -1;
dma_ch_lo = -1;
dma_ch_rxi = -1;
dma_ch_rxq = -1;
dma_ch_samp_i0 = -1;
dma_ch_samp_i1 = -1;
dma_ch_samp_q0 = -1;
dma_ch_samp_q1 = -1;
}
inline static int nextI()
{
int I = 0;
I -= 2 * pio_sm_get_blocking_unsafe(pio1, SM_ACCI0);
I -= pio_sm_get_blocking_unsafe(pio1, SM_ACCI0);
I -= 2 * pio_sm_get_blocking_unsafe(pio1, SM_ACCI1);
I -= pio_sm_get_blocking_unsafe(pio1, SM_ACCI1);
return I;
}
inline static int nextQ()
{
int Q = 0;
Q -= 2 * pio_sm_get_blocking_unsafe(pio1, SM_ACCQ0);
Q -= pio_sm_get_blocking_unsafe(pio1, SM_ACCQ0);
Q -= 2 * pio_sm_get_blocking_unsafe(pio1, SM_ACCQ1);
Q -= pio_sm_get_blocking_unsafe(pio1, SM_ACCQ1);
return Q;
}
static void rf_rx(void)
{
int Ih1[LPF] = { 0 };
int Ia1 = 0;
int Qh1[LPF] = { 0 };
int Qa1 = 0;
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 * DECIMATE / LPF; i++) {
for (int j = 0; j < LPF; j += DECIMATE) {
int I = 0, Q = 0;
for (int d = 0; d < DECIMATE; d++) {
int Is = nextI();
Ia1 += Is - Ih1[j + d];
Ih1[j + d] = Is;
I += Ia1;
int Qs = nextQ();
Qa1 += Qs - Qh1[j + d];
Qh1[j + d] = Qs;
Q += Qa1;
}
static int dcI = 0;
I = ((I << 14) - dcI) / (1 << 14);
dcI += I;
I *= gain;
I /= DECIMATE;
I /= max_amplitude;
I += 128;
if (I < 0)
I = 0;
else if (I > 255)
I = 255;
*blockptr++ = I;
static int dcQ = 0;
Q = ((Q << 14) - dcQ) / (1 << 14);
dcQ += Q;
Q *= gain;
Q /= DECIMATE;
Q /= max_amplitude;
Q += 128;
if (Q < 0)
Q = 0;
else if (Q > 255)
Q = 255;
*blockptr++ = Q;
}
}
if (queue_try_add(&iq_queue, &block)) {
iq_queue_pos = (iq_queue_pos + 1) & (IQ_QUEUE_LEN - 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);
} else if (0x02 == cmd) {
/* Set the rate at which IQ sample pairs are sent */
sample_rate = arg;
max_amplitude = CLK_SYS_HZ / sample_rate;
dma_timer_set_fraction(dma_t_samp, 1, CLK_SYS_HZ / (sample_rate * DECIMATE));
rx_lo_init(frequency);
} else if (0x04 == cmd) {
/* Set the tuner gain level */
gain = INIT_GAIN * powf(10.0f, arg / 200.0f);
} else if (0x05 == cmd) {
/* Normally PPM, but we use it for whatever we need atm. */
tweak = (int)arg;
rx_lo_init(frequency);
} else if (0x0d == cmd) {
/* Set tuner gain by the tuner's gain index */
if (arg >= NUM_GAINS)
arg = NUM_GAINS - 1;
gain = INIT_GAIN * powf(10.0f, gains[arg] / 200.0f);
}
}
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()
{
rf_rx_start();
sleep_us(100);
multicore_launch_core1(rf_rx);
const uint8_t *block;
while (queue_try_remove(&iq_queue, &block))
/* Flush the queue */;
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 = rnd_next() & 0x1fff;
for (int i = 0; i < wait; i++)
asm volatile("nop");
}
}
done:
multicore_fifo_push_blocking(0);
multicore_fifo_pop_blocking();
sleep_us(10);
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);
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);
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(NUM_GAINS) };
fwrite(header, sizeof header, 1, stdout);
fflush(stdout);
do_rx();
gain = INIT_GAIN;
frequency = INIT_FREQ;
sample_rate = INIT_SAMPLE_RATE;
max_amplitude = CLK_SYS_HZ / INIT_SAMPLE_RATE;
tweak = 0;
rx_lo_init(frequency);
}
sleep_ms(10);
}
}
Reference in a new issue View git blame Copy permalink