/***************************************************************************//**
* @brief dds_set_phase
*******************************************************************************/
void dds_set_scale(uint32_t chan, double scale)
{
uint32_t scale_reg;
uint32_t sign_part;
uint32_t int_part;
uint32_t fract_part;
if (PCORE_VERSION_MAJOR(dds_st.pcore_version) > 6)
{
if(scale >= 1.0)
{
sign_part = 0;
int_part = 1;
fract_part = 0;
dds_st.cached_scale[chan] = 1.0;
goto set_scale_reg;
}
if(scale <= -1.0)
{
sign_part = 1;
int_part = 1;
fract_part = 0;
dds_st.cached_scale[chan] = -1.0;
goto set_scale_reg;
}
if(scale < 0)
{
sign_part = 1;
int_part = 0;
dds_st.cached_scale[chan] = scale;
scale *= -1;
goto set_scale_reg;
}
sign_part = 0;
int_part = 0;
dds_st.cached_scale[chan] = scale;
fract_part = (uint32_t)(scale * 0x4000);
set_scale_reg:
scale_reg = (sign_part << 15) | (int_part << 14) | fract_part;
}
else
{
if(scale >= 1.0)
{
scale_reg = 0;
scale = 1.0;
}
if(scale <= 0.0)
{
scale_reg = 0;
scale = 0.0;
}
dds_st.cached_scale[chan] = scale;
fract_part = (uint32_t)(scale * 1000000);
scale_reg = 500000 / fract_part;
}
dac_stop();
dac_write(ADI_REG_CHAN_CNTRL_1_IIOCHAN(chan), ADI_DDS_SCALE(scale_reg));
dac_start_sync(0);
}
/***************************************************************************//**
* @brief dds_set_frequency
*******************************************************************************/
void dds_set_frequency(uint32_t chan, uint32_t freq)
{
uint64_t val64;
uint32_t reg;
dds_st.cached_freq[chan] = freq;
dac_stop();
dac_read(ADI_REG_CHAN_CNTRL_2_IIOCHAN(chan), ®);
reg &= ~ADI_DDS_INCR(~0);
val64 = (uint64_t) freq * 0xFFFFULL;
do_div(&val64, *dds_st.dac_clk);
reg |= ADI_DDS_INCR(val64) | 1;
dac_write(ADI_REG_CHAN_CNTRL_2_IIOCHAN(chan), reg);
dac_start_sync(0);
}
/***************************************************************************//**
* @brief dds_set_phase
*******************************************************************************/
void dds_set_phase(uint32_t chan, uint32_t phase)
{
uint64_t val64;
uint32_t reg;
dds_st.cached_phase[chan] = phase;
dac_stop();
dac_read(ADI_REG_CHAN_CNTRL_2_IIOCHAN(chan), ®);
reg &= ~ADI_DDS_INIT(~0);
val64 = (uint64_t) phase * 0x10000ULL + (360000 / 2);
do_div(&val64, 360000);
reg |= ADI_DDS_INIT(val64);
dac_write(ADI_REG_CHAN_CNTRL_2_IIOCHAN(chan), reg);
dac_start_sync(0);
}
/***************************************************************************//**
* @brief dds_set_frequency
*******************************************************************************/
void dds_set_frequency(struct ad9361_rf_phy *phy, uint32_t chan, uint32_t freq)
{
uint64_t val64;
uint32_t reg;
dds_st[phy->id_no].cached_freq[chan] = freq;
dac_stop(phy);
dac_read(phy, DAC_REG_CHAN_CNTRL_2_IIOCHAN(chan), ®);
reg &= ~DAC_DDS_INCR(~0);
val64 = (uint64_t) freq * 0xFFFFULL;
do_div(&val64, *dds_st[phy->id_no].dac_clk);
reg |= DAC_DDS_INCR(val64) | 1;
dac_write(phy, DAC_REG_CHAN_CNTRL_2_IIOCHAN(chan), reg);
dac_start_sync(phy, 0);
}
/***************************************************************************//**
* @brief dds_set_phase
*******************************************************************************/
void dds_set_phase(struct ad9361_rf_phy *phy, uint32_t chan, uint32_t phase)
{
uint64_t val64;
uint32_t reg;
dds_st[phy->id_no].cached_phase[chan] = phase;
dac_stop(phy);
dac_read(phy, DAC_REG_CHAN_CNTRL_2_IIOCHAN(chan), ®);
reg &= ~DAC_DDS_INIT(~0);
val64 = (uint64_t) phase * 0x10000ULL + (360000 / 2);
do_div(&val64, 360000);
reg |= DAC_DDS_INIT(val64);
dac_write(phy, DAC_REG_CHAN_CNTRL_2_IIOCHAN(chan), reg);
dac_start_sync(phy, 0);
}
/* playback_set_src
*
* Change the playback source. This will be refused if the current source
* is an active network stream.
*/
int playback_set_src(enum playback_source new_src) {
/* Can't switch away from network while playing. */
if (playback_src == SRC_NETWORK && new_src != SRC_NETWORK
&& dac_get_state() != DAC_IDLE) {
return -1;
}
if (playback_src == new_src)
return 0;
/* Stop the DAC and set playback_src_flags to 0, which will prevent
* the abstract generator and ILDA player from producing output. */
dac_stop(DAC_FLAG_STOP_SRCSWITCH);
playback_source_flags = 0;
playback_src = new_src;
return 0;
}
/***************************************************************************//**
* @brief dac_init
*******************************************************************************/
void dac_init(struct ad9361_rf_phy *phy, uint8_t data_sel)
{
uint32_t tx_count;
uint32_t index;
uint32_t index_i1;
uint32_t index_q1;
uint32_t index_i2;
uint32_t index_q2;
uint32_t data_i1;
uint32_t data_q1;
uint32_t data_i2;
uint32_t data_q2;
dac_write(ADI_REG_RSTN, 0x0);
dac_write(ADI_REG_RSTN, ADI_RSTN | ADI_MMCM_RSTN);
dac_write(ADI_REG_RATECNTRL, ADI_RATE(3));
dds_st.dac_clk = &phy->clks[TX_SAMPL_CLK]->rate;
dds_st.num_dds_channels = 8; // FIXME
dac_read(ADI_REG_VERSION, &dds_st.pcore_version);
dac_stop();
switch (data_sel) {
case DATA_SEL_DDS:
dds_default_setup(DDS_CHAN_TX1_I_F1, 90000, 1000000, 0.25);
dds_default_setup(DDS_CHAN_TX1_I_F2, 90000, 1000000, 0.25);
dds_default_setup(DDS_CHAN_TX1_Q_F1, 0, 1000000, 0.25);
dds_default_setup(DDS_CHAN_TX1_Q_F2, 0, 1000000, 0.25);
dds_default_setup(DDS_CHAN_TX2_I_F1, 90000, 1000000, 0.25);
dds_default_setup(DDS_CHAN_TX2_I_F2, 90000, 1000000, 0.25);
dds_default_setup(DDS_CHAN_TX2_Q_F1, 0, 1000000, 0.25);
dds_default_setup(DDS_CHAN_TX2_Q_F2, 0, 1000000, 0.25);
dac_write(ADI_REG_CNTRL_2, 0);
dac_datasel(-1, DATA_SEL_DDS);
break;
case DATA_SEL_DMA:
tx_count = sizeof(sine_lut) / sizeof(uint16_t);
for(index = 0; index < (tx_count * 2); index += 2)
{
index_i1 = index;
index_q1 = index + (tx_count / 2);
if(index_q1 >= (tx_count * 2))
index_q1 -= (tx_count * 2);
data_i1 = (sine_lut[index_i1 / 2] << 20);
data_q1 = (sine_lut[index_q1 / 2] << 4);
// FIXME
index_i2 = index_i1;
index_q2 = index_q1;
if(index_i2 >= (tx_count * 2))
index_i2 -= (tx_count * 2);
if(index_q2 >= (tx_count * 2))
index_q2 -= (tx_count * 2);
data_i2 = (sine_lut[index_i2 / 2] << 20);
data_q2 = (sine_lut[index_q2 / 2] << 4);
// FIXME
}
dac_dma_write(AXI_DMAC_REG_CTRL, 0);
dac_dma_write(AXI_DMAC_REG_CTRL, AXI_DMAC_CTRL_ENABLE);
// FIXME dac_dma_write(AXI_DMAC_REG_SRC_ADDRESS, DAC_DDR_BASEADDR);
dac_dma_write(AXI_DMAC_REG_SRC_STRIDE, 0x0);
dac_dma_write(AXI_DMAC_REG_X_LENGTH, (tx_count * 8) - 1);
dac_dma_write(AXI_DMAC_REG_Y_LENGTH, 0x0);
dac_dma_write(AXI_DMAC_REG_START_TRANSFER, 0x1);
dac_write(ADI_REG_CNTRL_2, 0);
dac_datasel(-1, DATA_SEL_DMA);
break;
default:
break;
}
dds_st.enable = true;
dac_start_sync(0);
}
/***************************************************************************//**
* @brief dds_set_phase
*******************************************************************************/
void dds_set_scale(struct ad9361_rf_phy *phy, uint32_t chan, int32_t scale_micro_units)
{
uint32_t scale_reg;
uint32_t sign_part;
uint32_t int_part;
uint32_t fract_part;
if (PCORE_VERSION_MAJOR(dds_st[phy->id_no].pcore_version) > 6)
{
if(scale_micro_units >= 1000000)
{
sign_part = 0;
int_part = 1;
fract_part = 0;
dds_st[phy->id_no].cached_scale[chan] = 1000000;
goto set_scale_reg;
}
if(scale_micro_units <= -1000000)
{
sign_part = 1;
int_part = 1;
fract_part = 0;
dds_st[phy->id_no].cached_scale[chan] = -1000000;
goto set_scale_reg;
}
dds_st[phy->id_no].cached_scale[chan] = scale_micro_units;
if(scale_micro_units < 0)
{
sign_part = 1;
int_part = 0;
scale_micro_units *= -1;
}
else
{
sign_part = 0;
int_part = 0;
}
fract_part = (uint32_t)(((uint64_t)scale_micro_units * 0x4000) / 1000000);
set_scale_reg:
scale_reg = (sign_part << 15) | (int_part << 14) | fract_part;
}
else
{
if(scale_micro_units >= 1000000)
{
scale_reg = 0;
scale_micro_units = 1000000;
}
if(scale_micro_units <= 0)
{
scale_reg = 0;
scale_micro_units = 0;
}
dds_st[phy->id_no].cached_scale[chan] = scale_micro_units;
fract_part = (uint32_t)(scale_micro_units);
scale_reg = 500000 / fract_part;
}
dac_stop(phy);
dac_write(phy, DAC_REG_CHAN_CNTRL_1_IIOCHAN(chan), DAC_DDS_SCALE(scale_reg));
dac_start_sync(phy, 0);
}
/* recv_fsm
*
* Attempt to process some data from the buffer located at 'data'.
*
* If a command is succcessfully read, then this returns the number of bytes
* consumed from the buffer. If a partial command is present in the buffer,
* but not enough to process yet, then this will return 0; the invoking
* function should call it later when more data is available. If an error
* occurs such that no further data can be handled from the connection, then
* this will call close_conn on the connection and return -1.
*/
static int recv_fsm(struct tcp_pcb *pcb, uint8_t * data, int len) {
uint8_t cmd = *data;
int npoints;
switch (ps_state) {
case MAIN:
switch (cmd) {
case 'p':
/* Prepare stream. */
if (dac_prepare() < 0) {
return send_resp(pcb, RESP_NAK_INVL, cmd, 1);
} else {
return send_resp(pcb, RESP_ACK, cmd, 1);
}
case 'b':
/* Make sure we have all of this packet... */
if (len < sizeof(struct begin_command))
return 0;
struct begin_command *bc = (struct begin_command *)data;
if (bc->point_rate > DAC_MAX_POINT_RATE)
return send_resp(pcb, RESP_NAK_INVL, cmd, 1);
// XXX set_low_water_mark(bc->low_water_mark);
dac_set_rate(bc->point_rate);
dac_start();
return send_resp(pcb, RESP_ACK, cmd,
sizeof(struct begin_command));
case 'u':
/* Update and Begin use the same packet format */
if (len < sizeof(struct begin_command))
return 0;
struct begin_command *uc = (struct begin_command *)data;
if (uc->point_rate > DAC_MAX_POINT_RATE)
return send_resp(pcb, RESP_NAK_INVL, cmd, 1);
dac_set_rate(uc->point_rate);
// XXX set_low_water_mark(uc->low_water_mark);
return send_resp(pcb, RESP_ACK, cmd,
sizeof(struct begin_command));
case 'q':
if (len < sizeof(struct queue_command))
return 0;
struct queue_command *qc = (struct queue_command *)data;
if (qc->point_rate > DAC_MAX_POINT_RATE)
return send_resp(pcb, RESP_NAK_INVL, cmd, 1);
dac_rate_queue(qc->point_rate);
return send_resp(pcb, RESP_ACK, cmd,
sizeof(struct queue_command));
case 'd':
/* Data: switch into the DATA state to start reading
* points into the buffer. */
if (len < sizeof(struct data_command))
return 0;
struct data_command *h = (struct data_command *) data;
if (h->npoints) {
ps_state = DATA;
ps_pointsleft = h->npoints;
/* We'll send a response once we've read all
* of the data. */
return sizeof(struct data_command);
} else {
/* 0-length data packets are legit. */
return send_resp(pcb, RESP_ACK, cmd,
sizeof(struct data_command));
}
case 's':
/* Stop */
if (dac_get_state() == DAC_IDLE) {
return send_resp(pcb, RESP_NAK_INVL, cmd, 1);
} else {
dac_stop(0);
return send_resp(pcb, RESP_ACK, cmd, 1);
}
case 0:
case 0xFF:
/* Emergency-stop. */
le_estop(ESTOP_PACKET);
return send_resp(pcb, RESP_ACK, cmd, 1);
case 'c':
/* Clear e-stop. */
le_estop_clear(ESTOP_CLEAR_ALL);
if (le_get_state() == LIGHTENGINE_READY)
return send_resp(pcb, RESP_ACK, cmd, 1);
else
return send_resp(pcb, RESP_NAK_ESTOP, cmd, 1);
case '?':
/* Ping */
return send_resp(pcb, RESP_ACK, cmd, 1);
default:
outputf("unknown cmd 0x%02x", cmd);
return close_conn(pcb, CONNCLOSED_UNKNOWNCMD, -1);
}
return -1;
case DATA:
ASSERT_NOT_EQUAL(ps_pointsleft, 0);
/* We can only write a complete point at a time. */
if (len < sizeof(struct dac_point))
return 0;
/* How many bytes of data is it our business to write? */
npoints = len / sizeof(struct dac_point);
if (npoints > ps_pointsleft)
npoints = ps_pointsleft;
/* How much do we have room for now? Note that dac_prepare
* is a ring buffer, so it's OK if it returns less than the
* number of points we have ready for it. We'll just have
* the FSM invoke us again. */
int nready = dac_request();
packed_point_t *addr = dac_request_addr();
/* On the other hand, if the DAC isn't ready for *any* data,
* then ignore the rest of this write command and NAK when
* it's over. The FSM will take care of us... */
if (nready <= 0) {
if (nready == 0) {
outputf("overflow: wanted to write %d", npoints);
} else {
outputf("underflow: pl %d np %d r %d", ps_pointsleft, npoints, nready);
}
ps_state = DATA_ABORTING;
/* Danger: goto. This could probably be structured
* better... */
goto handle_aborted_data;
}
if (npoints > nready)
npoints = nready;
dac_point_t *pdata = (dac_point_t *)data;
int i;
for (i = 0; i < npoints; i++) {
dac_pack_point(addr + i, pdata + i);
}
/* Let the DAC know we've given it more data */
dac_advance(npoints);
ps_pointsleft -= npoints;
if (!ps_pointsleft) {
ps_state = MAIN;
return send_resp(pcb, RESP_ACK, 'd',
npoints * sizeof(struct dac_point));
} else {
return (npoints * sizeof(struct dac_point));
}
case DATA_ABORTING:
ASSERT_NOT_EQUAL(ps_pointsleft, 0);
/* We can only consume a complete point at a time. */
if (len < sizeof(struct dac_point))
return 0;
/* How many points do we have? */
npoints = len / sizeof(struct dac_point);
if (npoints > ps_pointsleft)
npoints = ps_pointsleft;
handle_aborted_data:
ps_pointsleft -= npoints;
if (!ps_pointsleft) {
ps_state = MAIN;
return send_resp(pcb, RESP_NAK_INVL, 'd',
npoints * sizeof(struct dac_point));
} else {
return (npoints * sizeof(struct dac_point));
}
default:
break;
}
panic("invalid state in recv_dfa");
return -1;
}
static void ilda_stop_FPV_param(const char *path) {
playback_source_flags &= ~ILDA_PLAYER_PLAYING;
dac_stop(0);
}
/***************************************************************************//**
* @brief dac_init
*******************************************************************************/
void dac_init(struct ad9361_rf_phy *phy, uint8_t data_sel, uint8_t config_dma)
{
uint32_t tx_count;
uint32_t index;
uint32_t index_i1;
uint32_t index_q1;
uint32_t index_i2;
uint32_t index_q2;
uint32_t index_mem;
uint32_t data_i1;
uint32_t data_q1;
uint32_t data_i2;
uint32_t data_q2;
uint32_t length;
dac_write(phy, DAC_REG_RSTN, 0x0);
dac_write(phy, DAC_REG_RSTN, DAC_RSTN | DAC_MMCM_RSTN);
dds_st[phy->id_no].dac_clk = &phy->clks[TX_SAMPL_CLK]->rate;
dds_st[phy->id_no].rx2tx2 = phy->pdata->rx2tx2;
if(dds_st[phy->id_no].rx2tx2)
{
dds_st[phy->id_no].num_buf_channels = 4;
dac_write(phy, DAC_REG_RATECNTRL, DAC_RATE(3));
}
else
{
dds_st[phy->id_no].num_buf_channels = 2;
dac_write(phy, DAC_REG_RATECNTRL, DAC_RATE(1));
}
dac_read(phy, DAC_REG_VERSION, &dds_st[phy->id_no].pcore_version);
dac_stop(phy);
switch (data_sel) {
case DATA_SEL_DDS:
dds_default_setup(phy, DDS_CHAN_TX1_I_F1, 90000, 1000000, 250000);
dds_default_setup(phy, DDS_CHAN_TX1_I_F2, 90000, 1000000, 250000);
dds_default_setup(phy, DDS_CHAN_TX1_Q_F1, 0, 1000000, 250000);
dds_default_setup(phy, DDS_CHAN_TX1_Q_F2, 0, 1000000, 250000);
if(dds_st[phy->id_no].rx2tx2)
{
dds_default_setup(phy, DDS_CHAN_TX2_I_F1, 90000, 1000000, 250000);
dds_default_setup(phy, DDS_CHAN_TX2_I_F2, 90000, 1000000, 250000);
dds_default_setup(phy, DDS_CHAN_TX2_Q_F1, 0, 1000000, 250000);
dds_default_setup(phy, DDS_CHAN_TX2_Q_F2, 0, 1000000, 250000);
}
dac_write(phy, DAC_REG_CNTRL_2, 0);
dac_datasel(phy, -1, DATA_SEL_DDS);
break;
case DATA_SEL_DMA:
if(config_dma)
{
tx_count = sizeof(sine_lut) / sizeof(uint16_t);
if(dds_st[phy->id_no].rx2tx2)
{
#ifdef FMCOMMS5
for(index = 0, index_mem = 0; index < (tx_count * 2); index += 2, index_mem += 4)
#else
for(index = 0, index_mem = 0; index < (tx_count * 2); index += 2, index_mem += 2)
#endif
{
index_i1 = index;
index_q1 = index + (tx_count / 2);
if(index_q1 >= (tx_count * 2))
index_q1 -= (tx_count * 2);
data_i1 = (sine_lut[index_i1 / 2] << 20);
data_q1 = (sine_lut[index_q1 / 2] << 4);
Xil_Out32(DAC_DDR_BASEADDR + index_mem * 4, data_i1 | data_q1);
index_i2 = index_i1;
index_q2 = index_q1;
if(index_i2 >= (tx_count * 2))
index_i2 -= (tx_count * 2);
if(index_q2 >= (tx_count * 2))
index_q2 -= (tx_count * 2);
data_i2 = (sine_lut[index_i2 / 2] << 20);
data_q2 = (sine_lut[index_q2 / 2] << 4);
Xil_Out32(DAC_DDR_BASEADDR + (index_mem + 1) * 4, data_i2 | data_q2);
#ifdef FMCOMMS5
Xil_Out32(DAC_DDR_BASEADDR + (index_mem + 2) * 4, data_i1 | data_q1);
Xil_Out32(DAC_DDR_BASEADDR + (index_mem + 3) * 4, data_i2 | data_q2);
#endif
}
}
else
{
for(index = 0; index < tx_count; index += 1)
{
index_i1 = index;
index_q1 = index + (tx_count / 4);
if(index_q1 >= tx_count)
index_q1 -= tx_count;
data_i1 = (sine_lut[index_i1] << 20);
data_q1 = (sine_lut[index_q1] << 4);
Xil_Out32(DAC_DDR_BASEADDR + index * 4, data_i1 | data_q1);
}
}
Xil_DCacheFlush();
if(dds_st[phy->id_no].rx2tx2)
{
length = (tx_count * 8);
}
else
{
length = (tx_count * 4);
}
#ifdef FMCOMMS5
length = (tx_count * 16);
#endif
dac_dma_write(AXI_DMAC_REG_CTRL, 0);
dac_dma_write(AXI_DMAC_REG_CTRL, AXI_DMAC_CTRL_ENABLE);
dac_dma_write(AXI_DMAC_REG_SRC_ADDRESS, DAC_DDR_BASEADDR);
dac_dma_write(AXI_DMAC_REG_SRC_STRIDE, 0x0);
dac_dma_write(AXI_DMAC_REG_X_LENGTH, length - 1);
dac_dma_write(AXI_DMAC_REG_Y_LENGTH, 0x0);
dac_dma_write(AXI_DMAC_REG_START_TRANSFER, 0x1);
}
dac_write(phy, DAC_REG_CNTRL_2, 0);
dac_datasel(phy, -1, DATA_SEL_DMA);
break;
default:
break;
}
dds_st[phy->id_no].enable = true;
dac_start_sync(phy, 0);
}
int main(int argc, char **argv) {
__disable_irq();
LPC_GPIO2->FIODIR &= ~(1 << 8);
LPC_SC->PCLKSEL0 = PCLKSEL0_INIT_VALUE;
LPC_SC->PCLKSEL1 = PCLKSEL1_INIT_VALUE;
LPC_SC->PCONP = PCONP_INIT_VALUE;
clock_init();
serial_init();
/* Enable bus, usage, and mem faults. */
SCB->SHCSR |= SCB_SHCSR_MEMFAULTENA_Msk | SCB_SHCSR_BUSFAULTENA_Msk
| SCB_SHCSR_USGFAULTENA_Msk;
#if 0
/* Disable write buffers. This is useful for debugging - wild writes
* are imprecise exceptions unless the Cortex's write buffering is
* disabled. However, this has a significant performance hit, so it
* should only be set if necessary. */
*((uint32_t *)0xE000E008) |= (1<<1);
#endif
debugf("\r\n###############\r\n");
debugf("# Ether Dream #\r\n");
debugf("###############\r\n");
debugf("Firmware: %s\r\n", build);
debugf("RSID: %d\r\n", LPC_SC->RSID);
LPC_SC->RSID = 0xF;
hw_get_board_rev();
debugf("Hardware Revision %d\n", hw_board_rev);
debugf("Starting up: led");
led_init();
led_set_frontled(1);
debugf(" skub");
skub_init();
debugf(" lwip\r\n");
lwip_init();
clock.time = 0;
clock.mtime = 0;
SysTick_Config(SystemCoreClock / 10000);
/* Initialize hardware */
FPA_init();
outputf("Entering main loop...");
watchdog_init();
/* Startup might have taken some time, so reset the periodic event
* timers. */
int i;
for (i = 0; i < (sizeof(events) / sizeof(events[0])); i++) {
events_last[i] = events[i].start + clock.time;
}
__enable_irq();
playback_set_src(SRC_NETWORK);
/* Default values */
dac_set_rate(30000);
ilda_set_fps_limit(30);
while (1) {
watchdog_feed();
/* Check the stuff we check on each loop iteration. */
for (i = 0; i < TABLE_LENGTH(poll); i++) {
poll_table[i].f();
}
/* Check for periodic events */
check_periodic_timers();
if (f0ad_flag) {
/* Re-enter the bootloader. */
outputf("Reentering bootloader...");
dac_stop(0);
FORCE_BOOTLOAD_FLAG = FORCE_BOOTLOAD_VALUE;
__disable_irq();
/* The watchdog timer will kick us soon... */
while(1);
}
}
}
