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 ...@@ -7,7 +7,7 @@ PTPD_CFLAGS = -ffreestanding -DPTPD_FREESTANDING -DWRPC_EXTRA_SLIM -DPTPD_MSBF
PTPD_CFLAGS += -Wall -ggdb -I$D/wrsw_hal \ PTPD_CFLAGS += -Wall -ggdb -I$D/wrsw_hal \
-I$D/libptpnetif -I$D/PTPWRd \ -I$D/libptpnetif -I$D/PTPWRd \
-include $D/compat.h -include $D/PTPWRd/dep/trace.h -include $D/libposix/ptpd-wrappers.h -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/arith.o
OBJS_PTPD += $D/PTPWRd/bmc.o OBJS_PTPD += $D/PTPWRd/bmc.o
......
dev/endpoint.c
View file @ 3c7a7479
...@@ -114,6 +114,7 @@ int ep_link_up() ...@@ -114,6 +114,7 @@ int ep_link_up()
int ep_get_deltas(uint32_t *delta_tx, uint32_t *delta_rx) int ep_get_deltas(uint32_t *delta_tx, uint32_t *delta_rx)
{ {
// mprintf("called ep_get_deltas()\n");
*delta_tx = 0; *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; *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 @@ ...@@ -10,7 +10,7 @@
#define MINIC_DMA_TX_BUF_SIZE 1024 #define MINIC_DMA_TX_BUF_SIZE 1024
#define MINIC_DMA_RX_BUF_SIZE 2048 #define MINIC_DMA_RX_BUF_SIZE 2048
#define MINIC_MTU 1540 #define MINIC_MTU 256
#define F_COUNTER_BITS 4 #define F_COUNTER_BITS 4
#define F_COUNTER_MASK ((1<<F_COUNTER_BITS)-1) #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_ ...@@ -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. */ 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(); minic_new_rx_buffer();
return 0; return 0;
} }
...@@ -162,9 +162,9 @@ int minic_rx_frame(uint8_t *hdr, uint8_t *payload, uint32_t buf_size, struct hw_ ...@@ -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); 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--; utc--;
hwts->utc = utc & 0x7fffffff ; hwts->utc = utc & 0x7fffffff ;
cntr_diff = (counter_r & F_COUNTER_MASK) - counter_f; 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_ ...@@ -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; 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); 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 ...@@ -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); raw_ts = minic_readl(MINIC_REG_TSFIFO_R0);
fid = (minic_readl(MINIC_REG_TSFIFO_R1) >> 5) & 0xffff; 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); EXPLODE_WR_TIMESTAMP(raw_ts, counter_r, counter_f);
pps_gen_get_time(&utc, &nsec); pps_gen_get_time(&utc, &nsec);
if(counter_r > (3*REFCLK_FREQ/4) && nsec < REFCLK_FREQ/4) if(counter_r > 3*125000000/4 && nsec < 125000000/4)
utc--; utc--;
hwts->utc = utc; hwts->utc = utc;
hwts->ahead = 0; hwts->ahead = 0;
hwts->nsec = counter_r * 8; 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) ...@@ -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; cyc_after = ppsg_readl(PPSG_REG_CNTR_NSEC) & 0xfffffff;
} while (cyc_after < cyc_before); } while (cyc_after < cyc_before);
delay(100000);
if(utc) *utc = utc_lo; if(utc) *utc = utc_lo;
if(cntr_nsec) *cntr_nsec = cyc_after; if(cntr_nsec) *cntr_nsec = cyc_after;
} }
dev/softpll.c
View file @ 3c7a7479
...@@ -57,15 +57,15 @@ const struct softpll_config pll_cfg = ...@@ -57,15 +57,15 @@ const struct softpll_config pll_cfg =
{ {
/* Helper PLL */ /* Helper PLL */
28*32*16, // f_kp 28*32*16, // f_kp
20*32*16, // f_ki 50*32*16, // f_ki
16, // setpoint 16, // setpoint
1000, // lock detect freq samples 2000, // lock detect freq samples
2, // lock detect freq threshold 2, // lock detect freq threshold
2.0*32*16, // p_kp 2.0*32*16, // p_kp
0.05*32*16, // p_ki 0.05*32*16, // p_ki
1000, // lock detect phase samples 1000, // lock detect phase samples
300, // lock detect phase threshold 1000, // lock detect phase threshold
500, // delock threshold 2000, // delock threshold
32000, // HPLL dac bias 32000, // HPLL dac bias
/* DMTD PLL */ /* DMTD PLL */
...@@ -80,7 +80,7 @@ const struct softpll_config pll_cfg = ...@@ -80,7 +80,7 @@ const struct softpll_config pll_cfg =
500, // lock detect threshold 500, // lock detect threshold
500, // delock threshold 500, // delock threshold
32000, // DPLL dac bias 32000, // DPLL dac bias
1000 // deglitcher threshold 2000 // deglitcher threshold
}; };
struct softpll_state { struct softpll_state {
...@@ -90,6 +90,9 @@ struct softpll_state { ...@@ -90,6 +90,9 @@ struct softpll_state {
int h_p_setpoint; int h_p_setpoint;
int h_freq_mode; int h_freq_mode;
int h_locked; int h_locked;
int h_dac_val;
int h_freq_err;
int h_tag;
int d_dac_bias; int d_dac_bias;
...@@ -142,12 +145,23 @@ void _irq_entry() ...@@ -142,12 +145,23 @@ void _irq_entry()
int freq_err = SPLL->PER_HPLL; int freq_err = SPLL->PER_HPLL;
if(freq_err & 0x100) freq_err |= 0xffffff00; /* sign-extend */ if(freq_err & 0x100) freq_err |= 0xffffff00; /* sign-extend */
freq_err += pll_cfg.hpll_f_setpoint; freq_err += pll_cfg.hpll_f_setpoint;
pstate.h_freq_err = freq_err;
/* PI control */ /* PI control */
if(pstate.h_dac_val > 0 && pstate.h_dac_val < 65530)
pstate.h_i += freq_err; 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; 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 */ SPLL->DAC_HPLL = dv; /* update DAC */
pstate.h_dac_val = dv;
/* lock detection */ /* lock detection */
if(freq_err >= -pll_cfg.hpll_ld_f_threshold && freq_err <= pll_cfg.hpll_ld_f_threshold) if(freq_err >= -pll_cfg.hpll_ld_f_threshold && freq_err <= pll_cfg.hpll_ld_f_threshold)
...@@ -162,21 +176,37 @@ void _irq_entry() ...@@ -162,21 +176,37 @@ void _irq_entry()
pstate.h_dac_bias = dv; pstate.h_dac_bias = dv;
pstate.h_i = 0; pstate.h_i = 0;
pstate.h_lock_counter = 0; pstate.h_lock_counter = 0;
pstate.h_p_setpoint = -1;
SPLL->CSR = SPLL_CSR_TAG_EN_W(CHAN_REF); SPLL->CSR = SPLL_CSR_TAG_EN_W(CHAN_REF);
} }
/* HPLL: active phase branch */ /* HPLL: active phase branch */
} else if (tag_ref_ready) { } else if (tag_ref_ready) {
if(pstate.h_p_setpoint < 0) /* we don't have yet any phase samples? */ 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 { else {
int phase_err; 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_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; 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 */ SPLL->DAC_HPLL = dv; /* Update DAC */
pstate.h_tag = tag_ref;
if(abs(phase_err) >= pll_cfg.hpll_delock_threshold && pstate.h_locked) if(abs(phase_err) >= pll_cfg.hpll_delock_threshold && pstate.h_locked)
{ {
...@@ -192,6 +222,7 @@ void _irq_entry() ...@@ -192,6 +222,7 @@ void _irq_entry()
if(pstate.h_lock_counter == pll_cfg.hpll_ld_p_samples) 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 */ SPLL->CSR |= SPLL_CSR_TAG_EN_W(CHAN_FB); /* enable feedback channel and start DMPLL */
pstate.h_locked = 1; pstate.h_locked = 1;
pstate.d_tag_ref_d0 = -1; pstate.d_tag_ref_d0 = -1;
...@@ -200,7 +231,8 @@ void _irq_entry() ...@@ -200,7 +231,8 @@ void _irq_entry()
pstate.d_p_setpoint = 0; pstate.d_p_setpoint = 0;
pstate.d_lock_counter = 0; pstate.d_lock_counter = 0;
pstate.d_i = 0; pstate.d_i = 0;
pstate.d_locked = 0; pstate.d_locked = 0;
#endif
} }
} }
...@@ -307,6 +339,8 @@ void _irq_entry() ...@@ -307,6 +339,8 @@ void _irq_entry()
clear_irq(); clear_irq();
} }
static int prev_lck = 0;
void softpll_enable() void softpll_enable()
{ {
SPLL->CSR = 0; SPLL->CSR = 0;
...@@ -323,23 +357,37 @@ void softpll_enable() ...@@ -323,23 +357,37 @@ void softpll_enable()
pstate.h_locked = 0; pstate.h_locked = 0;
pstate.d_p_setpoint = 0; pstate.d_p_setpoint = 0;
pstate.d_phase_shift = 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; SPLL->EIC_IER = 1;
enable_irq(); enable_irq();
// softpll_test(); // softpll_test();
prev_lck = 0;
TRACE_DEV("[softpll]: enabled\n");
} }
int softpll_check_lock() 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.h_freq_mode ,pstate.h_locked,
pstate.d_freq_mode, pstate.d_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() int softpll_busy()
......
dev/uart.c
View file @ 3c7a7479
...@@ -18,4 +18,6 @@ void uart_write_byte(unsigned char x) ...@@ -18,4 +18,6 @@ void uart_write_byte(unsigned char x)
{ {
while(uart->SR & UART_SR_TX_BUSY); while(uart->SR & UART_SR_TX_BUSY);
uart->TDR = x; uart->TDR = x;
if(x == '\n')
uart_write_byte('\r');
} }
include/board.h
View file @ 3c7a7479
...@@ -11,14 +11,15 @@ ...@@ -11,14 +11,15 @@
#define CPU_CLOCK 62500000ULL #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_LED1 0
#define GPIO_PIN_SCL_OUT 1 #define GPIO_PIN_LED2 1
#define GPIO_PIN_SDA_OUT 2 #define GPIO_PIN_SCL_OUT 2
#define GPIO_PIN_SDA_IN 3 #define GPIO_PIN_SDA_OUT 3
#define GPIO_PIN_BTN1 4 #define GPIO_PIN_SDA_IN 4
#define GPIO_PIN_BTN2 5 #define GPIO_PIN_BTN1 5
#define GPIO_PIN_BTN2 6
static inline int delay(int x) static inline int delay(int x)
{ {
......
include/trace.h
View file @ 3c7a7479
...@@ -2,6 +2,6 @@ ...@@ -2,6 +2,6 @@
#define __FREESTANDING_TRACE_H__ #define __FREESTANDING_TRACE_H__
#define TRACE_WRAP(...) #define TRACE_WRAP(...)
#define TRACE_DEV(...) #define TRACE_DEV(...) mprintf(__VA_ARGS__)
#endif #endif
wrc_main.c
View file @ 3c7a7479
...@@ -41,9 +41,9 @@ RunTimeOpts rtOpts = { ...@@ -41,9 +41,9 @@ RunTimeOpts rtOpts = {
.E2E_mode = TRUE, .E2E_mode = TRUE,
.wrStateRetry = WR_DEFAULT_STATE_REPEAT, .wrStateRetry = WR_DEFAULT_STATE_REPEAT,
.wrStateTimeout= WR_DEFAULT_STATE_TIMEOUT_MS, .wrStateTimeout= WR_DEFAULT_STATE_TIMEOUT_MS,
.deltasKnown = WR_DEFAULT_DELTAS_KNOWN, .deltasKnown = TRUE, //WR_DEFAULT_DELTAS_KNOWN,
.knownDeltaTx = WR_DEFAULT_DELTA_TX, .knownDeltaTx = 0,//WR_DEFAULT_DELTA_TX,
.knownDeltaRx = WR_DEFAULT_DELTA_RX, .knownDeltaRx = 0,//WR_DEFAULT_DELTA_RX,
/*SLAVE only or MASTER only*/ /*SLAVE only or MASTER only*/
#ifdef WRPC_SLAVE #ifdef WRPC_SLAVE
.primarySource = FALSE, .primarySource = FALSE,
...@@ -128,14 +128,15 @@ int enable_tracking = 1; ...@@ -128,14 +128,15 @@ int enable_tracking = 1;
int main(void) int main(void)
{ {
int rx, tx; int rx, tx;
int link_went_up, link_went_down;
int prev_link_state= 0, link_state;
int16_t ret; int16_t ret;
uart_init(); uart_init();
gpio_dir(GPIO_PIN_LED, 1);
ep_init(mac_addr); ep_init(mac_addr);
ep_enable(1, 0); ep_enable(1, 1);
minic_init(); minic_init();
...@@ -148,30 +149,43 @@ int main(void) ...@@ -148,30 +149,43 @@ int main(void)
gpio_dir(GPIO_PIN_BTN1, 0); gpio_dir(GPIO_PIN_BTN1, 0);
ptpPortDS = ptpdStartup(0, NULL, &ret, &rtOpts, &ptpClockDS); // softpll_enable();
// for(;;) softpll_check_lock();
initDataClock(&rtOpts, &ptpClockDS);
toState(PTP_INITIALIZING, &rtOpts, ptpPortDS);
wr_servo_man_adjust_phase(-11600 + 1700); wr_servo_man_adjust_phase(-11600 + 1700);
displayConfigINFO(&rtOpts); displayConfigINFO(&rtOpts);
ptpPortDS = ptpdStartup(0, NULL, &ret, &rtOpts, &ptpClockDS);
initDataClock(&rtOpts, &ptpClockDS);
for(;;) for(;;)
{ {
//wr_mon_debug(); link_state = ep_link_up();
if(button_pressed())
link_went_up = !prev_link_state && link_state;
prev_link_state = link_state;
if(link_went_up)
{ {
mprintf("button pressed\n"); uint32_t dtxm, drxm;
enable_tracking = 1-enable_tracking; TRACE_DEV("LINK UP\n");
wr_servo_enable_tracking(enable_tracking); // 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); //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); //mprintf("after state=%d, wrState=%d\n", ptpPortDS->portState, ptpPortDS->wrPortState);
update_rx_queues(); update_rx_queues();
timer_delay(10); 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