.program st506_data
.fifo tx
.out 1 left auto 32
.set 1
.side_set 2 opt

// 'out' mapping: READ
// 'set' mapping: READ
// 'side' mapping: INDEX, SERVO_GATE

.wrap_target
	// fetch INDEX marker into X
	out x, 1
	jmp !x index_low

index_high:
	// clear READ, set SERVO_GATE, set INDEX
	set pins, 0 side 0b11
	jmp fetch_index
index_low:
	// clear READ, set SERVO_GATE, clear INDEX
	set pins, 0 side 0b01

fetch_index:
	// fetch pulse length into X
	out x, 15
	// loop during the pulse
loop_index:
	nop [3]
	jmp x-- loop_index [3]

fetch_data:
	// fetch data length into X, clear INDEX and SERVO_GATE
	out x, 16 side 0b00
	// loop while outputting data
loop_data:
	// normal logic:
	out pins, 1 [3]
	jmp x-- loop_data [3]
	// inverted logic:
	//out y, 1 [1]
	//mov y, ~y
	//mov pins, y
	//jmp x-- loop_data [3]

.wrap

% c-sdk {
#include <hardware/clocks.h>

static inline void st506_data_program_init(PIO pio, uint sm, uint offset, uint pin_read, uint pin_servo_gate, uint freq) {
	float clkdiv	= (float)clock_get_hz(clk_sys) / (float)(freq * 8);
	pio_sm_config c = st506_data_program_get_default_config(offset);
	sm_config_set_clkdiv(&c, clkdiv);
	sm_config_set_out_pin_base(&c, pin_read);
	sm_config_set_set_pin_base(&c, pin_read);
	sm_config_set_sideset_pin_base(&c, pin_servo_gate);
	pio_sm_init(pio, sm, offset, &c);
	pio_sm_set_consecutive_pindirs(pio, sm, pin_read, 1, true);
	pio_sm_set_consecutive_pindirs(pio, sm, pin_servo_gate, 2, true);
}

//          +0x0       +0x4         +0x8         +0xC (Trigger)
// Alias 0: READ_ADDR  WRITE_ADDR   TRANS_COUNT  CTRL
// Alias 1: CTRL       READ_ADDR    WRITE_ADDR   TRANS_COUNT
// Alias 2: CTRL       TRANS_COUNT  READ_ADDR    WRITE_ADDR
// Alias 3: CTRL       WRITE_ADDR   TRANS_COUNT  READ_ADDR

static const void *st506_data_read_addr[4];
static int st506_data_ctrl_chan[4];
static int st506_data_data_chan[4];

static inline void st506_data_program_start(PIO pio, uint sm, const void *data, uint data_words) {
	pio_sm_set_enabled(pio, sm, true);

	st506_data_read_addr[sm] = data;
	int ctrl_chan = st506_data_ctrl_chan[sm] = dma_claim_unused_channel(true);
	int data_chan = st506_data_data_chan[sm] = dma_claim_unused_channel(true);

	// Control Channel
	dma_channel_config ctrl_config = dma_channel_get_default_config(ctrl_chan);
	channel_config_set_transfer_data_size(&ctrl_config, DMA_SIZE_32);
	channel_config_set_read_increment(&ctrl_config, false);
	channel_config_set_write_increment(&ctrl_config, false);
	dma_channel_configure(
		ctrl_chan,
		&ctrl_config,
		// write to Data Channel's READ_ADDR (and trigger)
		&dma_hw->ch[data_chan].al3_read_addr_trig,
		// read from 'data_addr'
		&st506_data_read_addr[sm],
		// only transfer one word
		1,
		false
	);

	// Data Channel
	dma_channel_config data_config = dma_channel_get_default_config(data_chan);
	channel_config_set_transfer_data_size(&data_config, DMA_SIZE_32);
	channel_config_set_read_increment(&data_config, true);
	channel_config_set_write_increment(&data_config, false);
	channel_config_set_dreq(&data_config, pio_get_dreq(pio, sm, true));
	channel_config_set_bswap(&data_config, true);
	channel_config_set_chain_to(&data_config, ctrl_chan);
	dma_channel_configure(
		data_chan,
		&data_config,
		// write to PIO's TX FIFO
		&pio->txf[sm],
		// read address is populated by Control Channel
		NULL,
		// transfer the entire data, then trigger the Control Channel
		data_words,
		false
	);

	// start the program by triggering the Control Channel
	dma_channel_start(ctrl_chan);
}
%}