#include <math.h>
#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 <stdio.h>
#include <limits.h>
#include <stdlib.h>

#define VREG_VOLTAGE VREG_VOLTAGE_1_20
#define CLK_SYS_HZ (320 * 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 16

#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 INIT_SAMPLE_RATE 100000
#define INIT_FREQ 94600000

static int frequency = INIT_FREQ;
static int sample_rate = INIT_SAMPLE_RATE;

#define BASE_GAIN (1 << 15)
#define ATTN_BITS 10
#define DECIMATE 4

static int gain = BASE_GAIN / (CLK_SYS_HZ / INIT_SAMPLE_RATE);

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);

	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_x, 16),
		pio_encode_in(pio_y, 16),
		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(PIO, SM_COS, origin_adder, &pc);
	pio_sm_init(PIO, SM_SIN, origin_adder, &pc);
}

static void lo_generate_phase(uint32_t *buf, size_t len, uint32_t step, uint32_t phase)
{
	uint32_t bits = 0;

	for (size_t i = 0; i < len; i++) {
		for (int j = 0; j < 32; j++) {
			bits <<= 1;
			bits |= phase >> 31;
			phase += step;
		}

		buf[i] = bits;
	}
}

static void rx_lo_init(double freq)
{
	double frac = (double)CLK_SYS_HZ / (8 << LO_BITS_DEPTH);
	freq = roundf(freq / frac) * frac;
	uint32_t step = freq * 4294967296.0 / CLK_SYS_HZ;
	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];
}

static int Ia1, Ia2, Ia3;
static int Ib1, Ib2, Ib3;

static int Qa1, Qa2, Qa3;
static int Qb1, Qb2, Qb3;

inline static void accI()
{
	static uint16_t py, px;

	uint32_t yx = pio_sm_get_blocking_unsafe(PIO, SM_COS);
	uint16_t y = yx >> 16;
	uint16_t x = yx;

	uint16_t ny = py - y;
	uint16_t nx = px - x;

	py = y;
	px = x;

	uint32_t s = (ny << 1) + nx;

	Ia1 += s;
	Ia2 += Ia1;
	Ia3 += Ia2;
}

inline static void accQ()
{
	static uint16_t py, px;

	uint32_t yx = pio_sm_get_blocking_unsafe(PIO, SM_SIN);
	uint16_t y = yx >> 16;
	uint16_t x = yx;

	uint16_t ny = py - y;
	uint16_t nx = px - x;

	py = y;
	px = x;

	uint32_t s = (ny << 1) + nx;

	Qa1 += s;
	Qa2 += Qa1;
	Qa3 += Qa2;
}

inline static int getI()
{
	uint32_t c1, c2, c3;

	c3 = Ia3 - Ib3;
	Ib3 = Ia3;

	c2 = c3 - Ib2;
	Ib2 = c3;

	c1 = c2 - Ib1;
	Ib1 = c2;

	return (int)c1;
}

inline static int getQ()
{
	uint32_t c1, c2, c3;

	c3 = Qa3 - Qb3;
	Qb3 = Qa3;

	c2 = c3 - Qb2;
	Qb2 = c3;

	c1 = c2 - Qb1;
	Qb1 = c2;

	return (int)c1;
}

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++) {
			for (int d = 0; d < DECIMATE; d++) {
				accI();
				accQ();
			}

			int I = getI() * gain;
			int Q = getQ() * gain;

			I >>= ATTN_BITS;
			*blockptr++ = I + 128;

			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);
	} 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);
		dma_timer_set_fraction(dma_t_samp, 1, CLK_SYS_HZ / (sample_rate * DECIMATE));
		rx_lo_init(frequency);
	}
}

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);

	/* 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);
	}
}