Skip to content

S
Software for White Rabbit PTP Core
Commit 3c7a7479 authored by Tomasz Wlostowski's avatar Tomasz Wlostowski
Browse files
Options

very unstable demo release

parent 73b35616
Hide whitespace changes
Inline Side-by-side
Showing with 122 additions and 49 deletions
Makefile
View file @ 3c7a7479
......@@ -7,7 +7,7 @@ PTPD_CFLAGS = -ffreestanding -DPTPD_FREESTANDING -DWRPC_EXTRA_SLIM -DPTPD_MSBF
PTPD_CFLAGS += -Wall -ggdb -I$D/wrsw_hal \
-I$D/libptpnetif -I$D/PTPWRd \
-include $D/compat.h -include $D/PTPWRd/dep/trace.h -include $D/libposix/ptpd-wrappers.h
PTPD_CFLAGS += -DPTPD_NO_DAEMON -DNEW_SINGLE_WRFSM #-DPTPD_DBGMSG
PTPD_CFLAGS += -DPTPD_NO_DAEMON -DNEW_SINGLE_WRFSM -DPTPD_TRACE_MASK=32
OBJS_PTPD = $D/PTPWRd/arith.o
OBJS_PTPD += $D/PTPWRd/bmc.o
......
dev/endpoint.c
View file @ 3c7a7479
......@@ -114,6 +114,7 @@ int ep_link_up()
int ep_get_deltas(uint32_t *delta_tx, uint32_t *delta_rx)
{
// mprintf("called ep_get_deltas()\n");
*delta_tx = 0;
*delta_rx = 15000 - 7000 + 195000 + 32000 + UIS_PER_SERIAL_BIT * MDIO_WR_SPEC_BSLIDE_R(pcs_read(MDIO_REG_WR_SPEC)) + 2800;
}
......
dev/minic.c
View file @ 3c7a7479
......@@ -10,7 +10,7 @@
#define MINIC_DMA_TX_BUF_SIZE 1024
#define MINIC_DMA_RX_BUF_SIZE 2048
#define MINIC_MTU 1540
#define MINIC_MTU 256
#define F_COUNTER_BITS 4
#define F_COUNTER_MASK ((1<<F_COUNTER_BITS)-1)
......@@ -135,7 +135,7 @@ int minic_rx_frame(uint8_t *hdr, uint8_t *payload, uint32_t buf_size, struct hw_
if(!RX_DESC_VALID(desc_hdr)) /* invalid descriptor? Weird, the RX_ADDR seems to be saying something different. Ignore the packet and purge the RX buffer. */
{
TRACE_DEV("weird, invalid RX descriptor (%x, head %x)\n", desc_hdr, minic.rx_head);
TRACE_DEV("weird, invalid RX descriptor (%x, head %x)\n", desc_hdr, minic.rx_head);
minic_new_rx_buffer();
return 0;
}
......@@ -162,9 +162,9 @@ int minic_rx_frame(uint8_t *hdr, uint8_t *payload, uint32_t buf_size, struct hw_
pps_gen_get_time(&utc, &counter_ppsg);
if(counter_r > (3*REFCLK_FREQ/4) && counter_ppsg < REFCLK_FREQ/4)
if(counter_r > 3*125000000/4 && counter_ppsg < 125000000/4)
utc--;
hwts->utc = utc & 0x7fffffff ;
cntr_diff = (counter_r & F_COUNTER_MASK) - counter_f;
......@@ -178,7 +178,7 @@ int minic_rx_frame(uint8_t *hdr, uint8_t *payload, uint32_t buf_size, struct hw_
hwts->nsec = counter_r * 8;
TRACE_DEV("TS minic_rx_frame: %d.%d\n", hwts->utc, hwts->nsec);
// TRACE_DEV("TS minic_rx_frame: %d.%d\n", hwts->utc, hwts->nsec);
}
n_recvd = (buf_size < payload_size ? buf_size : payload_size);
......@@ -278,17 +278,22 @@ int minic_tx_frame(uint8_t *hdr, uint8_t *payload, uint32_t size, struct hw_time
raw_ts = minic_readl(MINIC_REG_TSFIFO_R0);
fid = (minic_readl(MINIC_REG_TSFIFO_R1) >> 5) & 0xffff;
if(fid != tx_oob_val)
{
TRACE_DEV("minic_tx_frame: unmatched fid %d vs %d\n", fid, tx_oob_val);
}
EXPLODE_WR_TIMESTAMP(raw_ts, counter_r, counter_f);
pps_gen_get_time(&utc, &nsec);
if(counter_r > (3*REFCLK_FREQ/4) && nsec < REFCLK_FREQ/4)
utc--;
if(counter_r > 3*125000000/4 && nsec < 125000000/4)
utc--;
hwts->utc = utc;
hwts->ahead = 0;
hwts->nsec = counter_r * 8;
TRACE_DEV("TS minic_tx_frame: %d.%d\n", hwts->utc, hwts->nsec);
// TRACE_DEV("TS minic_tx_frame: %d.%d\n", hwts->utc, hwts->nsec);
}
......
dev/pps_gen.c
View file @ 3c7a7479
......@@ -78,6 +78,8 @@ void pps_gen_get_time(uint32_t *utc, uint32_t *cntr_nsec)
cyc_after = ppsg_readl(PPSG_REG_CNTR_NSEC) & 0xfffffff;
} while (cyc_after < cyc_before);
delay(100000);
if(utc) *utc = utc_lo;
if(cntr_nsec) *cntr_nsec = cyc_after;
}
dev/softpll.c
View file @ 3c7a7479
......@@ -57,15 +57,15 @@ const struct softpll_config pll_cfg =
{
/* Helper PLL */
28*32*16, // f_kp
20*32*16, // f_ki
50*32*16, // f_ki
16, // setpoint
1000, // lock detect freq samples
2000, // lock detect freq samples
2, // lock detect freq threshold
2.0*32*16, // p_kp
0.05*32*16, // p_ki
1000, // lock detect phase samples
300, // lock detect phase threshold
500, // delock threshold
1000, // lock detect phase threshold
2000, // delock threshold
32000, // HPLL dac bias
/* DMTD PLL */
......@@ -80,7 +80,7 @@ const struct softpll_config pll_cfg =
500, // lock detect threshold
500, // delock threshold
32000, // DPLL dac bias
1000 // deglitcher threshold
2000 // deglitcher threshold
};
struct softpll_state {
......@@ -90,6 +90,9 @@ struct softpll_state {
int h_p_setpoint;
int h_freq_mode;
int h_locked;
int h_dac_val;
int h_freq_err;
int h_tag;
int d_dac_bias;
......@@ -142,12 +145,23 @@ void _irq_entry()
int freq_err = SPLL->PER_HPLL;
if(freq_err & 0x100) freq_err |= 0xffffff00; /* sign-extend */
freq_err += pll_cfg.hpll_f_setpoint;
pstate.h_freq_err = freq_err;
/* PI control */
if(pstate.h_dac_val > 0 && pstate.h_dac_val < 65530)
pstate.h_i += freq_err;
dv = ((pstate.h_i * pll_cfg.hpll_f_ki + freq_err * pll_cfg.hpll_f_kp) >> PI_FRACBITS) + pll_cfg.hpll_dac_bias;
if(dv >= 65530) dv = 65530;
if(dv < 0) dv = 0;
pstate.h_dac_val = dv;
SPLL->DAC_HPLL = dv; /* update DAC */
pstate.h_dac_val = dv;
/* lock detection */
if(freq_err >= -pll_cfg.hpll_ld_f_threshold && freq_err <= pll_cfg.hpll_ld_f_threshold)
......@@ -162,21 +176,37 @@ void _irq_entry()
pstate.h_dac_bias = dv;
pstate.h_i = 0;
pstate.h_lock_counter = 0;
pstate.h_p_setpoint = -1;
SPLL->CSR = SPLL_CSR_TAG_EN_W(CHAN_REF);
}
/* HPLL: active phase branch */
} else if (tag_ref_ready) {
if(pstate.h_p_setpoint < 0) /* we don't have yet any phase samples? */
pstate.h_p_setpoint = tag_ref & 0x3fff;
pstate.h_p_setpoint = tag_ref; // & 0x3fff;
else {
int phase_err;
phase_err = (tag_ref & 0x3fff) - pstate.h_p_setpoint;
// if(tag_ref > 16384) tag_ref -= 16384;
// tag_ref &x7fff;
phase_err = tag_ref - pstate.h_p_setpoint;
if(pstate.h_dac_val > 0 && pstate.h_dac_val < 65530)
pstate.h_i += phase_err;
pstate.h_p_setpoint += 16384;
pstate.h_p_setpoint &= ((1<<TAG_BITS)-1);
dv = ((pstate.h_i * pll_cfg.hpll_p_ki + phase_err * pll_cfg.hpll_p_kp) >> PI_FRACBITS) + pstate.h_dac_bias;
if(dv >= 65530) dv = 65530;
if(dv < 0) dv = 0;
pstate.h_dac_val = dv;
SPLL->DAC_HPLL = dv; /* Update DAC */
pstate.h_tag = tag_ref;
if(abs(phase_err) >= pll_cfg.hpll_delock_threshold && pstate.h_locked)
{
......@@ -192,6 +222,7 @@ void _irq_entry()
if(pstate.h_lock_counter == pll_cfg.hpll_ld_p_samples)
{
#if 1
SPLL->CSR |= SPLL_CSR_TAG_EN_W(CHAN_FB); /* enable feedback channel and start DMPLL */
pstate.h_locked = 1;
pstate.d_tag_ref_d0 = -1;
......@@ -200,7 +231,8 @@ void _irq_entry()
pstate.d_p_setpoint = 0;
pstate.d_lock_counter = 0;
pstate.d_i = 0;
pstate.d_locked = 0;
pstate.d_locked = 0;
#endif
}
}
......@@ -307,6 +339,8 @@ void _irq_entry()
clear_irq();
}
static int prev_lck = 0;
void softpll_enable()
{
SPLL->CSR = 0;
......@@ -323,23 +357,37 @@ void softpll_enable()
pstate.h_locked = 0;
pstate.d_p_setpoint = 0;
pstate.d_phase_shift = 0;
pstate.h_dac_val = pll_cfg.hpll_dac_bias;
SPLL->CSR = SPLL_CSR_TAG_EN_W(CHAN_PERIOD);
SPLL->CSR = SPLL_CSR_TAG_EN_W(CHAN_PERIOD);// | SPLL_CSR_TAG_EN_W(CHAN_REF);
;
SPLL->EIC_IER = 1;
enable_irq();
// softpll_test();
prev_lck = 0;
TRACE_DEV("[softpll]: enabled\n");
}
int softpll_check_lock()
{
TRACE_DEV("LCK h:f%d l%d d: f%d l%d err %d\n",
/* TRACE_DEV("LCK h:f%d l%d d: f%d l%d err %d %d dac %d\n",
pstate.h_freq_mode ,pstate.h_locked,
pstate.d_freq_mode, pstate.d_locked,
pstate.d_freq_error);
pstate.h_tag, pstate.h_tag & 0x3fff, pstate.h_dac_val);*/
int lck = pstate.h_locked && pstate.d_locked;
if(lck && !prev_lck)
TRACE_DEV("[softpll]: got lock\n");
else if (!lck && prev_lck)
TRACE_DEV("[softpll]: lost lock\n");
return pstate.h_locked && pstate.d_locked;
prev_lck = lck;
return lck;
}
int softpll_busy()
......
dev/uart.c
View file @ 3c7a7479
......@@ -18,4 +18,6 @@ void uart_write_byte(unsigned char x)
{
while(uart->SR & UART_SR_TX_BUSY);
uart->TDR = x;
if(x == '\n')
uart_write_byte('\r');
}
include/board.h
View file @ 3c7a7479
......@@ -11,14 +11,15 @@
#define CPU_CLOCK 62500000ULL
#define UART_BAUDRATE 0 /* not a real UART */
#define UART_BAUDRATE 115200ULL /* not a real UART */
#define GPIO_PIN_LED 0
#define GPIO_PIN_SCL_OUT 1
#define GPIO_PIN_SDA_OUT 2
#define GPIO_PIN_SDA_IN 3
#define GPIO_PIN_BTN1 4
#define GPIO_PIN_BTN2 5
#define GPIO_PIN_LED1 0
#define GPIO_PIN_LED2 1
#define GPIO_PIN_SCL_OUT 2
#define GPIO_PIN_SDA_OUT 3
#define GPIO_PIN_SDA_IN 4
#define GPIO_PIN_BTN1 5
#define GPIO_PIN_BTN2 6
static inline int delay(int x)
{
......
include/trace.h
View file @ 3c7a7479
......@@ -2,6 +2,6 @@
#define __FREESTANDING_TRACE_H__
#define TRACE_WRAP(...)
#define TRACE_DEV(...)
#define TRACE_DEV(...) mprintf(__VA_ARGS__)
#endif
wrc_main.c
View file @ 3c7a7479
......@@ -41,9 +41,9 @@ RunTimeOpts rtOpts = {
.E2E_mode = TRUE,
.wrStateRetry = WR_DEFAULT_STATE_REPEAT,
.wrStateTimeout= WR_DEFAULT_STATE_TIMEOUT_MS,
.deltasKnown = WR_DEFAULT_DELTAS_KNOWN,
.knownDeltaTx = WR_DEFAULT_DELTA_TX,
.knownDeltaRx = WR_DEFAULT_DELTA_RX,
.deltasKnown = TRUE, //WR_DEFAULT_DELTAS_KNOWN,
.knownDeltaTx = 0,//WR_DEFAULT_DELTA_TX,
.knownDeltaRx = 0,//WR_DEFAULT_DELTA_RX,
/*SLAVE only or MASTER only*/
#ifdef WRPC_SLAVE
.primarySource = FALSE,
......@@ -128,14 +128,15 @@ int enable_tracking = 1;
int main(void)
{
int rx, tx;
int link_went_up, link_went_down;
int prev_link_state= 0, link_state;
int16_t ret;
uart_init();
gpio_dir(GPIO_PIN_LED, 1);
ep_init(mac_addr);
ep_enable(1, 0);
ep_enable(1, 1);
minic_init();
......@@ -148,30 +149,43 @@ int main(void)
gpio_dir(GPIO_PIN_BTN1, 0);
ptpPortDS = ptpdStartup(0, NULL, &ret, &rtOpts, &ptpClockDS);
initDataClock(&rtOpts, &ptpClockDS);
toState(PTP_INITIALIZING, &rtOpts, ptpPortDS);
// softpll_enable();
// for(;;) softpll_check_lock();
wr_servo_man_adjust_phase(-11600 + 1700);
displayConfigINFO(&rtOpts);
ptpPortDS = ptpdStartup(0, NULL, &ret, &rtOpts, &ptpClockDS);
initDataClock(&rtOpts, &ptpClockDS);
for(;;)
{
//wr_mon_debug();
if(button_pressed())
link_state = ep_link_up();
link_went_up = !prev_link_state && link_state;
prev_link_state = link_state;
if(link_went_up)
{
mprintf("button pressed\n");
enable_tracking = 1-enable_tracking;
wr_servo_enable_tracking(enable_tracking);
}
uint32_t dtxm, drxm;
TRACE_DEV("LINK UP\n");
// toState(PTP_INITIALIZING, &rtOpts, ptpPortDS);
}
//wr_mon_debug();
if(button_pressed())
{
enable_tracking = 1-enable_tracking;
wr_servo_enable_tracking(enable_tracking);
}
//mprintf("before state=%d, wrState=%d\n", ptpPortDS->portState, ptpPortDS->wrPortState);
protocol_nonblock(&rtOpts, ptpPortDS);
protocol_nonblock(&rtOpts, ptpPortDS);
//mprintf("after state=%d, wrState=%d\n", ptpPortDS->portState, ptpPortDS->wrPortState);
update_rx_queues();
timer_delay(10);
update_rx_queues();
timer_delay(10);
}
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment