Skip to content

S
Software for White Rabbit PTP Core
Commit 993e6bc2 authored by Grzegorz Daniluk's avatar Grzegorz Daniluk
Browse files
Options

Merge branch 'v3-switch-for-wrpc-tom' into v3-switch_for_wrpc 

Conflicts:
	Makefile
	dev/softpll_ng.c
	wrc_main.c
parents 0f22c9b5 bbd88426
Hide whitespace changes
Inline Side-by-side
Showing with 226 additions and 599 deletions
dev/endpoint.c
View file @ 993e6bc2
......@@ -30,14 +30,14 @@ static int autoneg_enabled;
static volatile struct EP_WB *EP = (volatile struct EP_WB *) BASE_EP;
/* functions for accessing PCS (MDIO) registers */
static uint16_t pcs_read(int location)
uint16_t pcs_read(int location)
{
EP->MDIO_CR = EP_MDIO_CR_ADDR_W(location >> 2);
while ((EP->MDIO_ASR & EP_MDIO_ASR_READY) == 0);
return EP_MDIO_ASR_RDATA_R(EP->MDIO_ASR) & 0xffff;
}
static void pcs_write(int location, int value)
void pcs_write(int location, int value)
{
EP->MDIO_CR = EP_MDIO_CR_ADDR_W(location >> 2)
| EP_MDIO_CR_DATA_W(value)
......
dev/minic.c
View file @ 993e6bc2
......@@ -20,6 +20,8 @@
#define RX_DESC_HAS_OOB(d) ((d) & (1<<29) ? 1 : 0)
#define RX_DESC_SIZE(d) (((d) & (1<<0) ? -1 : 0) + (d & 0xffe))
#define RXOOB_TS_INCORRECT (1<<11)
#define TX_DESC_VALID (1<<31)
#define TX_DESC_WITH_OOB (1<<30)
#define TX_DESC_HAS_OWN_MAC (1<<28)
......@@ -140,10 +142,13 @@ int minic_rx_frame(uint8_t *hdr, uint8_t *payload, uint32_t buf_size, struct hw_
uint32_t raw_ts;
uint32_t rx_addr_cur;
int n_recvd;
// mprintf("erxf\n");
if(! (minic_readl(MINIC_REG_EIC_ISR) & MINIC_EIC_ISR_RX))
return 0;
//TRACE_DEV("minic: got sthx \n");
desc_hdr = *minic.rx_head;
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. */
......@@ -165,11 +170,12 @@ int minic_rx_frame(uint8_t *hdr, uint8_t *payload, uint32_t buf_size, struct hw_
{
uint32_t counter_r, counter_f, counter_ppsg, utc;
int cntr_diff;
uint16_t dhdr;
payload_size -= RX_OOB_SIZE;
memcpy(&raw_ts, (uint8_t *)minic.rx_head + payload_size + 6, 4); /* fixme: ugly way of doing unaligned read */
memcpy(&dhdr, (uint8_t *)minic.rx_head + payload_size + 4, 2);
EXPLODE_WR_TIMESTAMP(raw_ts, counter_r, counter_f);
pps_gen_get_time(&utc, &counter_ppsg);
......@@ -187,6 +193,7 @@ int minic_rx_frame(uint8_t *hdr, uint8_t *payload, uint32_t buf_size, struct hw_
hwts->ahead = 0;
hwts->nsec = counter_r * 8;
hwts->valid = (dhdr & RXOOB_TS_INCORRECT) ? 0 : 1;
}
n_recvd = (buf_size < payload_size ? buf_size : payload_size);
......@@ -211,7 +218,7 @@ int minic_rx_frame(uint8_t *hdr, uint8_t *payload, uint32_t buf_size, struct hw_
minic_writel(MINIC_REG_EIC_ISR, MINIC_EIC_ISR_RX);
}
// mprintf("minic: num_rx %d\n", n_recvd);
// TRACE_DEV("minic: num_rx %d\n", n_recvd);
return n_recvd;
}
......@@ -225,7 +232,7 @@ int minic_tx_frame(uint8_t *hdr, uint8_t *payload, uint32_t size, struct hw_time
minic_new_tx_buffer();
TRACE_DEV("minic_tx_frame: head %x size %d\n", minic.tx_head, size);
// TRACE_DEV("minic_tx_frame: head %x size %d\n", minic.tx_head, size);
memset(minic.tx_head, 0x0, size + 16);
memset((void*)minic.tx_head + 4, 0, size < 60 ? 60 : size);
memcpy((void*)minic.tx_head + 4, hdr, ETH_HEADER_SIZE);
......@@ -281,7 +288,7 @@ int minic_tx_frame(uint8_t *hdr, uint8_t *payload, uint32_t size, struct hw_time
hwts->ahead = 0;
hwts->nsec = counter_r * 8;
TRACE_DEV("minic_tx_frame [%d bytes] TS: %d.%d\n", size, hwts->utc, hwts->nsec);
// TRACE_DEV("minic_tx_frame [%d bytes] TS: %d.%d valid %d\n", size, hwts->utc, hwts->nsec, hwts->valid);
minic.tx_count++;
}
......
dev/softpll.c deleted 100644 → 0
View file @ 0f22c9b5
/*
White Rabbit Softcore PLL (SoftPLL)
*/
#include <stdio.h>
#include <stdlib.h>
#include "board.h"
#include "irq.h"
#include <hw/softpll_regs.h>
#include "softpll.h"
/* Bit size of phase tags generated by the DMTDs. Used to sign-extend the tags. */
#define TAG_BITS 20
/* Helper PLL N divider (1/2**N is the frequency offset) */
#define HPLL_N 14
/* Fractional bits in PI controller coefficients */
#define PI_FRACBITS 12
/* Channel enables in the CSR register: When the corresponding bit is set,
the corresponding channel is enabled */
#define CHAN_FB 8 /* Main clock feedback (clk_ref_i) */
#define CHAN_REF 4 /* Reference clock (clk_rx_i) */
#define CHAN_PERIOD 2 /* (clk_rx_i - clk_dmtd_i) period error */
#define CHAN_AUX 1 /* Auxillary core clock (clk_aux_i) */
/* CSR reg "tag ready" flags: When the corresponding bit is set,
the corresponding TAG_xxx register constains a fresh value of phase/frequency tag */
#define READY_FB (8<<4)
#define READY_REF (4<<4)
#define READY_PERIOD (2<<4)
#define READY_AUX (1<<4)
/* number of fractional cycles in the phase setpoint, used to control the speed of the phase shifter
(which affects the stability of the PLL) */
#define SETPOINT_FRACBITS 1
static volatile struct SPLL_WB *SPLL = (volatile struct SPLL_WB *) BASE_SOFTPLL;
/* PI regulator state */
typedef struct {
int ki, kp; /* integral and proportional gains (1<<PI_FRACBITS == 1.0f) */
int integrator; /* current integrator value */
int bias; /* DC offset always added to the output */
int anti_windup; /* when non-zero, anti-windup is enabled */
int y_min; /* min/max output range, used by claming and antiwindup algorithms */
int y_max;
int x,y; /* Current input and output value */
} spll_pi_t;
/* Processes a single sample (x) using PI controller (pi). Returns the value (y) which should
be used to drive the actuator. */
static inline int pi_update(spll_pi_t *pi, int x)
{
int i_new, y;
pi->x = x;
i_new = pi->integrator + x;
y = ((i_new * pi->ki + x * pi->kp) >> PI_FRACBITS) + pi->bias;
/* clamping (output has to be in <y_min, y_max>) and anti-windup:
stop the integretor if the output is already out of range and the output
is going further away from y_min/y_max. */
if(y < pi->y_min)
{
y = pi->y_min;
if((pi->anti_windup && (i_new > pi->integrator)) || !pi->anti_windup)
pi->integrator = i_new;
} else if (y > pi->y_max) {
y = pi->y_max;
if((pi->anti_windup && (i_new < pi->integrator)) || !pi->anti_windup)
pi->integrator = i_new;
} else
pi->integrator = i_new;
pi->y = y;
return y;
}
/* initializes the PI controller state. Currently almost a stub. */
static inline void pi_init(spll_pi_t *pi)
{
pi->integrator = 0;
}
/* lock detector state */
typedef struct {
int lock_cnt;
int lock_samples;
int delock_samples;
int threshold;
int locked;
} spll_lock_det_t;
/* Lock detector state machine. Takes an error sample (y) and checks if it's withing an acceptable range
(i.e. <-ld.threshold, ld.threshold>. If it has been inside the range for (ld.lock_samples) cyckes, the
FSM assumes the PLL is locked. */
static inline int ld_update(spll_lock_det_t *ld, int y)
{
if (abs(y) <= ld->threshold)
{
if(ld->lock_cnt < ld->lock_samples)
ld->lock_cnt++;
if(ld->lock_cnt == ld->lock_samples)
ld->locked = 1;
} else {
if(ld->lock_cnt > ld->delock_samples)
ld->lock_cnt--;
if(ld->lock_cnt == ld->delock_samples)
{
ld->lock_cnt= 0;
ld->locked = 0;
}
}
return ld->locked;
}
static void ld_init(spll_lock_det_t *ld)
{
ld->locked = 0;
ld->lock_cnt = 0;
}
/* State of the Helper PLL, producing a clock (clk_dmtd_i) which is
slightly offset in frequency from the recovered/reference clock (clk_rx_i or clk_ref_i), so the
Main PLL can use it to perform linear phase measurements */
struct spll_helper_state {
spll_pi_t pi_freq, pi_phase;
spll_lock_det_t ld_freq, ld_phase;
int f_setpoint;
int p_setpoint;
int freq_mode;
};
struct spll_dmpll_state {
spll_pi_t pi_freq, pi_phase;
spll_lock_det_t ld_freq, ld_phase;
int setpoint;
int freq_mode;
int tag_ref_d0;
int tag_fb_d0;
int period_ref;
int period_fb;
int phase_shift;
};
void helper_init(struct spll_helper_state *s)
{
/* Frequency branch PI controller */
s->pi_freq.y_min = 5;
s->pi_freq.y_max = 65530;
s->pi_freq.anti_windup = 0;
s->pi_freq.kp = 28*32*16;
s->pi_freq.ki = 50*32*16;
s->pi_freq.bias = 32000;
/* Freqency branch lock detection */
s->ld_freq.threshold = 2;
s->ld_freq.lock_samples = 1000;
s->ld_freq.delock_samples = 990;
/* Phase branch PI controller */
s->pi_phase.y_min = 5;
s->pi_phase.y_max = 65530;
s->pi_phase.kp = (int)(2.0 * 32.0 * 16.0);
s->pi_phase.ki = (int)(0.05 * 32.0 * 3.0);
s->pi_phase.anti_windup = 0;
s->pi_phase.bias = 32000;
/* Phase branch lock detection */
s->ld_phase.threshold = 500;
s->ld_phase.lock_samples = 10000;
s->ld_phase.delock_samples = 9900;
s->freq_mode = 1;
s->f_setpoint = 16;
pi_init(&s->pi_freq);
ld_init(&s->ld_freq);
pi_init(&s->pi_phase);
ld_init(&s->ld_phase);
}
void main_init(struct spll_dmpll_state *s)
{
/* Frequency branch PI controller */
s->pi_freq.y_min = 5;
s->pi_freq.y_max = 65530;
s->pi_freq.anti_windup = 0;
s->pi_freq.kp = 100;
s->pi_freq.ki = 600;
s->pi_freq.bias = 32000;
/* Freqency branch lock detection */
s->ld_freq.threshold = 500;
s->ld_freq.lock_samples = 1000;
s->ld_freq.delock_samples = 990;
/* Phase branch PI controller */
s->pi_phase.y_min = 5;
s->pi_phase.y_max = 65530;
s->pi_phase.kp = 1304 / 2;
s->pi_phase.ki = 10 * 3;
s->pi_phase.anti_windup = 0;
s->pi_phase.bias = 32000;
/* Phase branch lock detection */
s->ld_phase.threshold = 500;
s->ld_phase.lock_samples = 1000;
s->ld_phase.delock_samples = 990;
s->freq_mode = 1;
s->setpoint=0;
s->phase_shift=0;
s->tag_ref_d0 = -1;
s->tag_fb_d0 = -1;
s->period_ref = 0 ;
s->period_fb = 0 ;
pi_init(&s->pi_freq);
ld_init(&s->ld_freq);
pi_init(&s->pi_phase);
ld_init(&s->ld_phase);
}
static struct spll_helper_state helper;
static struct spll_dmpll_state main, auxpll;
static volatile int irq_cnt = 0;
#define HELPER_PERIOD_BITS 8
static inline void helper_irq(struct spll_helper_state *hpll, int tag_ref)
{
int err, y;
/* HPLL: active frequency branch */
if(hpll->freq_mode)
{
err = SPLL->PER_HPLL;
if (err & (1<<HELPER_PERIOD_BITS))
err |= ~ ((1<<HELPER_PERIOD_BITS) - 1); /* sign-extend the freq error */
err += hpll->f_setpoint;
y = pi_update(&hpll->pi_freq, err);
SPLL->DAC_HPLL = y;
if(ld_update(&hpll->ld_freq, err))
{
hpll->freq_mode = 0;
hpll->p_setpoint = -1;
hpll->pi_phase.bias = y;
pi_init(&hpll->pi_phase);
ld_init(&hpll->ld_phase);
SPLL->CSR = SPLL_CSR_TAG_EN_W(CHAN_REF);
}
} else if (tag_ref >= 0) { /* HPLL: active phase branch */
if(hpll->p_setpoint < 0)
{
hpll->p_setpoint = tag_ref;
return;
}
err = tag_ref - hpll->p_setpoint;
hpll->p_setpoint += 16384;
hpll->p_setpoint &= ((1<<TAG_BITS)-1);
y = pi_update(&hpll->pi_phase, err);
SPLL->DAC_HPLL = y;
if(ld_update(&hpll->ld_phase, err))
{
SPLL->CSR |= SPLL_CSR_TAG_EN_W(CHAN_FB | CHAN_REF);
};
}
}
void dmpll_irq(struct spll_dmpll_state *dmpll, int tag_ref, int tag_fb, volatile uint32_t *dac)
{
int err, tmp, y, fb_ready;
fb_ready = (tag_fb >= 0 ? 1 : 0);
if(dmpll->freq_mode)
{
if(tag_ref >= 0)
{
if(dmpll->tag_ref_d0 >= 0)
{
tmp = tag_ref - dmpll->tag_ref_d0;
if(tmp < 0)
tmp += (1<<TAG_BITS)-1;
dmpll->period_ref = tmp;
}
dmpll->tag_ref_d0 = tag_ref;
}
if(tag_fb >= 0)
{
if(dmpll->tag_fb_d0 > 0)
{
tmp = tag_fb - dmpll->tag_fb_d0;
if(tmp < 0)
tmp += (1<<TAG_BITS)-1;
dmpll->period_fb = tmp;
}
dmpll->tag_fb_d0 = tag_fb;
}
err = dmpll->period_ref - dmpll->period_fb;
y = pi_update(&dmpll->pi_freq, err);
*dac = y;
if(ld_update(&dmpll->ld_freq, err))
{
/* frequency has been stable for quite a while? switch to phase branch */
dmpll->freq_mode = 0;
dmpll->pi_phase.bias = y;//dmpll->pi_freq.bias;
pi_init(&dmpll->pi_phase);
ld_init(&dmpll->ld_phase);
dmpll->setpoint = 0;
dmpll->phase_shift = 0;
}
} else {
if(tag_ref >= 0)
dmpll->tag_ref_d0 = tag_ref;
tag_ref = dmpll->tag_ref_d0 ;
tag_fb += (dmpll->setpoint >> SETPOINT_FRACBITS);
tag_fb &= (1<<TAG_BITS) - 1;
if(fb_ready)
{
tag_ref &= 0x3fff; //while (tag_ref > 16384 ) tag_ref-=16384; /* fixme */
tag_fb &= 0x3fff; //while (tag_fb > 16384 ) tag_fb-=16384;
err = tag_ref - tag_fb;
y = pi_update(&dmpll->pi_phase, err);
*dac = y;
ld_update(&dmpll->ld_phase, err);
if(dmpll->setpoint < dmpll->phase_shift)
dmpll->setpoint++;
else if(dmpll->setpoint > dmpll->phase_shift)
dmpll->setpoint--;
}
}
}
int aux_en = 0;
void _irq_entry()
{
volatile uint32_t csr;
int tag_ref = -1;
int tag_fb = -1;
int tag_aux = -1;
irq_cnt ++;
csr = SPLL->CSR;
if(csr & READY_REF)
tag_ref = SPLL->TAG_REF;
if(csr & READY_FB)
tag_fb = SPLL->TAG_FB;
if(csr & READY_AUX)
tag_aux = SPLL->TAG_AUX;
helper_irq(&helper, tag_ref);
if(helper.ld_phase.locked)
{
dmpll_irq(&main, tag_ref, tag_fb, &SPLL->DAC_DMPLL);
/* Aux channel - i.e. the oscillator on the FMC card */
if((csr & SPLL_CSR_AUX_EN) )
{
int shifting_done = 0;
if(!(csr & CHAN_AUX))
{
auxpll.ld_phase.locked = 0;
auxpll.phase_shift = main.phase_shift;
auxpll.setpoint =0;
auxpll.freq_mode = 1;
pi_init(&auxpll.pi_freq);
ld_init(&auxpll.ld_freq);
//aux_en = 1;
}
dmpll_irq(&auxpll, tag_ref, tag_aux, &SPLL->DAC_AUX);
shifting_done = abs(auxpll.setpoint - auxpll.phase_shift) < 500 ? 1: 0;
SPLL->CSR = SPLL_CSR_TAG_EN_W(CHAN_FB | CHAN_REF | CHAN_AUX) | ((shifting_done && auxpll.ld_phase.locked) ? SPLL_CSR_AUX_LOCK : 0);
} else {
auxpll.ld_phase.locked = 0;
auxpll.phase_shift = main.phase_shift;
auxpll.setpoint =0;
SPLL->CSR = SPLL_CSR_TAG_EN_W(CHAN_FB | CHAN_REF);
// aux_en = 0;
}
}
clear_irq();
}
static int prev_lck = 0;
void softpll_enable()
{
SPLL->CSR = 0;
disable_irq();
helper_init(&helper);
main_init(&main);
main_init(&auxpll);
SPLL->DAC_HPLL = 0;
SPLL->DEGLITCH_THR = 3000;
SPLL->CSR = SPLL_CSR_TAG_EN_W(CHAN_PERIOD);
SPLL->EIC_IER = 1;
enable_irq();
TRACE_DEV("*********************** [softpll]: enabled\n");
// softpll_check_lock();
prev_lck = 0;
}
int softpll_check_lock()
{
static int prev_lck = 0;
int lck = !helper.freq_mode && helper.ld_phase.locked && !main.freq_mode && main.ld_phase.locked;
// if(!lck)
TRACE_DEV("%d %d%d%d%d%d%d %d %d %d\n", irq_cnt, helper.freq_mode, helper.ld_phase.locked, main.freq_mode, main.ld_phase.locked, auxpll.freq_mode, auxpll.ld_phase.locked, main.pi_freq.y, main.pi_phase.y, main.pi_phase.x);
irq_cnt = 0;
if(lck && !prev_lck) {
TRACE_DEV("[softpll]: got lock\n");
}else if (!lck && prev_lck)
TRACE_DEV("[softpll]: lost lock\n");
prev_lck = lck;
return lck;
}
int softpll_get_aux_status()
{
return ((SPLL->CSR & SPLL_CSR_AUX_EN) ? SOFTPLL_AUX_ENABLED: 0) | (auxpll.ld_phase.locked ? SOFTPLL_AUX_LOCKED: 0);
}
int softpll_busy(int channel)
{
// mprintf("busy?");
return main.setpoint != main.phase_shift;
}
void softpll_set_phase( int ps)
{
main.phase_shift = (-(int32_t) ((int64_t)ps * 16384LL / 8000LL)) << SETPOINT_FRACBITS;
auxpll.phase_shift = (-(int32_t) ((int64_t)ps * 16384LL / 8000LL)) << SETPOINT_FRACBITS;
}
void softpll_disable()
{
SPLL->CSR = 0;
disable_irq();
}
int softpll_get_setpoint()
{
// return pstate.d_p_setpoint;
}
dev/softpll_ng.c
View file @ 993e6bc2
......@@ -40,16 +40,30 @@ static volatile struct PPSG_WB *PPSG = (volatile struct PPSG_WB *) BASE_PPS_GEN;
#define SEQ_CLEAR_DACS 9
#define SEQ_WAIT_CLEAR_DACS 10
#define AUX_DISABLED 1
#define AUX_LOCK_PLL 2
#define AUX_ALIGN_PHASE 3
#define AUX_READY 4
struct spll_aux_state {
int state;
int32_t phase_target;
};
struct softpll_state {
int mode;
int seq_state;
int helper_locked;
int dac_timeout;
int default_dac_main;
int delock_count;
int32_t mpll_shift_ps;
struct spll_helper_state helper;
struct spll_external_state ext;
struct spll_main_state mpll;
struct spll_main_state aux[MAX_CHAN_AUX];
struct spll_aux_state aux_fsm[MAX_CHAN_AUX];
struct spll_ptracker_state ptrackers[MAX_PTRACKERS];
};
......@@ -67,7 +81,7 @@ void _irq_entry()
// softpll.seq_state = SEQ_WAIT_CLEAR_DACS;
if(! (SPLL->TRR_CSR & SPLL_TRR_CSR_EMPTY))
while(! (SPLL->TRR_CSR & SPLL_TRR_CSR_EMPTY))
{
trr = SPLL->TRR_R0;
src = SPLL_TRR_R0_CHAN_ID_R(trr);
......@@ -147,15 +161,18 @@ void _irq_entry()
// SPLL->OCER = 0;
// SPLL->RCER = 0;
softpll.seq_state = SEQ_CLEAR_DACS;
softpll.delock_count++;
} else if (softpll.mode == SPLL_MODE_GRAND_MASTER && !external_locked((struct spll_external_state *) &s->ext))
{
// SPLL->OCER = 0;
// SPLL->RCER = 0;
// SPLL->ECCR = 0;
softpll.seq_state = SEQ_START_EXT;
softpll.delock_count++;
} else if (softpll.mode == SPLL_MODE_SLAVE && !softpll.mpll.ld.locked)
{
softpll.seq_state = SEQ_CLEAR_DACS;
softpll.delock_count++;
};
break;
};
......@@ -185,6 +202,9 @@ void _irq_entry()
for(i=0;i<n_chan_ref; i++)
if(ptracker_mask & (1<<i))
ptracker_update((struct spll_ptracker_state *) &s->ptrackers[i], tag, src);
for(i=0;i<n_chan_out-1;i++)
mpll_update(&softpll.aux[i], tag, src);
break;
......@@ -219,6 +239,8 @@ void spll_init(int mode, int slave_ref_channel, int align_pps)
softpll.helper_locked = 0;
softpll.mode = mode;
softpll.default_dac_main = 0;
softpll.delock_count = 0;
// softpll.occr_prev = 0;
SPLL->DAC_HPLL = 0;
SPLL->DAC_MAIN = 0;
......@@ -231,6 +253,8 @@ void spll_init(int mode, int slave_ref_channel, int align_pps)
SPLL->ECCR = 0;
SPLL->RCGER = 0;
SPLL->DCCR = 0;
SPLL->OCCR = 0;
SPLL->DEGLITCH_THR = 1000;
PPSG->ESCR = 0;
......@@ -254,7 +278,10 @@ void spll_init(int mode, int slave_ref_channel, int align_pps)
mpll_init(&softpll.mpll, slave_ref_channel, n_chan_ref);
for(i=0;i<n_chan_out-1;i++)
{
mpll_init(&softpll.aux[i], slave_ref_channel, n_chan_ref + i + 1);
softpll.aux_fsm[i].state = AUX_DISABLED;
}
break;
case SPLL_MODE_FREE_RUNNING_MASTER:
......@@ -267,7 +294,10 @@ void spll_init(int mode, int slave_ref_channel, int align_pps)
mpll_init(&softpll.mpll, slave_ref_channel, n_chan_ref);
for(i=0;i<n_chan_out-1;i++)
{
mpll_init(&softpll.aux[i], slave_ref_channel, n_chan_ref + i + 1);
softpll.aux_fsm[i].state = AUX_DISABLED;
}
PPSG->ESCR = PPSG_ESCR_PPS_VALID | PPSG_ESCR_TM_VALID;
break;
......@@ -280,7 +310,11 @@ void spll_init(int mode, int slave_ref_channel, int align_pps)
mpll_init(&softpll.mpll, slave_ref_channel, n_chan_ref);
for(i=0;i<n_chan_out-1;i++)
{
mpll_init(&softpll.aux[i], slave_ref_channel, n_chan_ref + i + 1);
softpll.aux_fsm[i].state = AUX_DISABLED;
}
// PPSG->ESCR = PPSG_ESCR_PPS_VALID | PPSG_ESCR_TM_VALID;
......@@ -298,17 +332,7 @@ void spll_init(int mode, int slave_ref_channel, int align_pps)
dummy = SPLL->PER_HPLL;
SPLL->EIC_IER = 1;
// _irq_entry();
SPLL->OCER |= 1;
// SPLL->RCER |= 1;
/* for(;;)
{
TRACE_DEV("OCER %x TRR_CSR %x %x\n", SPLL->OCER, SPLL->TRR_CSR, SPLL->TRR_R0);
}*/
SPLL->OCER |= 1;
enable_irq();
......@@ -337,8 +361,10 @@ void spll_shutdown()
void spll_start_channel(int channel)
{
if (softpll.seq_state != SEQ_READY || !channel)
{
TRACE("Can't start channel %d, the PLL is not ready\n", channel);
return;
}
mpll_start(&softpll.aux[channel-1]);
}
......@@ -369,17 +395,33 @@ static int32_t to_picos(int32_t units)
}
/* Channel 0 = local PLL reference, 1...N = aux oscillators */
void spll_set_phase_shift(int channel, int32_t value_picoseconds)
static void set_phase_shift(int channel, int32_t value_picoseconds)
{
volatile struct spll_main_state *st = (!channel ? &softpll.mpll : &softpll.aux[channel-1]);
mpll_set_phase_shift(st, from_picos(value_picoseconds));
int div = (DIVIDE_DMTD_CLOCKS_BY_2 ? 2: 1);
mpll_set_phase_shift(st, from_picos(value_picoseconds) / div);
softpll.mpll_shift_ps = value_picoseconds;
}
void spll_set_phase_shift(int channel, int32_t value_picoseconds)
{
int i;
if(channel == SPLL_ALL_CHANNELS)
{
spll_set_phase_shift(0, value_picoseconds);
for(i=0;i<n_chan_out-1;i++)
if(softpll.aux_fsm[i].state == AUX_READY)
set_phase_shift(i+1, value_picoseconds);
} else
set_phase_shift(channel, value_picoseconds);
}
void spll_get_phase_shift(int channel, int32_t *current, int32_t *target)
{
volatile struct spll_main_state *st = (!channel ? &softpll.mpll : &softpll.aux[channel-1]);
if(current) *current = to_picos(st->phase_shift_current);
if(target) *target = to_picos(st->phase_shift_target);
int div = (DIVIDE_DMTD_CLOCKS_BY_2 ? 2: 1);
if(current) *current = to_picos(st->phase_shift_current * div);
if(target) *target = to_picos(st->phase_shift_target * div);
}
int spll_read_ptracker(int channel, int32_t *phase_ps, int *enabled)
......@@ -388,7 +430,13 @@ int spll_read_ptracker(int channel, int32_t *phase_ps, int *enabled)
int phase = st->phase_val;
if(phase < 0) phase += (1<<HPLL_N);
else if (phase >= (1<<HPLL_N)) phase -= (1<<HPLL_N);
if(DIVIDE_DMTD_CLOCKS_BY_2)
{
phase <<= 1;
phase &= (1<<HPLL_N)-1;
}
*phase_ps = to_picos(phase);
if(enabled)
*enabled = ptracker_mask & (1<<st->id_b) ? 1 : 0;
......@@ -404,10 +452,10 @@ void spll_get_num_channels(int *n_ref, int *n_out)
void spll_show_stats()
{
if(softpll.mode > 0)
TRACE("Irq_count %d Sequencer_state %d mode %d Alignment_state %d HL%d EL%d ML%d HY=%d MY=%d\n",
TRACE("Irq_count %d Sequencer_state %d mode %d Alignment_state %d HL%d EL%d ML%d HY=%d MY=%d DelCnt=%d\n",
irq_count, softpll.seq_state, softpll.mode, softpll.ext.realign_state,
softpll.helper.ld.locked, softpll.ext.ld.locked, softpll.mpll.ld.locked,
softpll.helper.pi.y, softpll.mpll.pi.y);
softpll.helper.pi.y, softpll.mpll.pi.y, softpll.delock_count);
}
......@@ -435,3 +483,77 @@ void spll_enable_ptracker(int ref_channel, int enable)
}
}
int spll_get_delock_count()
{
return softpll.delock_count;
}
int spll_update_aux_clocks()
{
uint32_t occr_aux_en = SPLL_OCCR_OUT_EN_R(SPLL->OCCR);
int i;
for(i=0;i<n_chan_out-1;i++)
{
struct spll_aux_state *s = &softpll.aux_fsm[i];
if((! (occr_aux_en & (1<<(i+1))) && s->state != AUX_DISABLED))
{
TRACE("[aux] disabled channel %d\n", i);
spll_stop_channel(i+1);
SPLL->OCCR &= ~ (SPLL_OCCR_OUT_LOCK_W((1<<(i+1))));
s->state = AUX_DISABLED;
} else switch(s->state)
{
case AUX_DISABLED:
if(occr_aux_en & (1<<(i+1)) && softpll.mpll.ld.locked)
{
TRACE("[aux] enabled channel %d\n", i);
s->state = AUX_LOCK_PLL;
spll_start_channel(i+1);
}
break;
case AUX_LOCK_PLL:
if(softpll.aux[i].ld.locked)
{
TRACE("[aux] channel %d locked [aligning @ %d ps]\n", i, softpll.mpll_shift_ps);
set_phase_shift(i+1, softpll.mpll_shift_ps);
s->state = AUX_ALIGN_PHASE;
}
break;
case AUX_ALIGN_PHASE:
if(!mpll_shifter_busy(&softpll.aux[i]))
{
TRACE("[aux] channel %d phase aligned\n", i);
SPLL->OCCR |= SPLL_OCCR_OUT_LOCK_W((1<<(i+1)));
s->state = AUX_READY;
}
break;
case AUX_READY:
if(!softpll.mpll.ld.locked)
{
SPLL->OCCR &= ~ (SPLL_OCCR_OUT_LOCK_W((1<<(i+1))));
s->state = AUX_DISABLED;
}
break;
}
}
}
int spll_get_aux_status(int channel)
{
int rval = 0;
if(softpll.aux_fsm[channel].state != AUX_DISABLED)
rval |= SPLL_AUX_ENABLED;
if(softpll.aux_fsm[channel].state == AUX_READY)
rval |= SPLL_AUX_LOCKED;
return rval;
}
dev/syscon.c
View file @ 993e6bc2
......@@ -8,10 +8,11 @@ struct s_i2c_if i2c_if[2] = { {SYSC_GPSR_FMC_SCL, SYSC_GPSR_FMC_SDA},
***************************/
void timer_init(uint32_t enable)
{
mprintf("MEMSIZE %x\n", syscon->HWFR & 0xf);
if(enable)
syscon->TCR |= SYSC_TCR_ENABLE;
else
syscon->TCR &= !SYSC_TCR_ENABLE;
syscon->TCR &= ~SYSC_TCR_ENABLE;
}
uint32_t timer_get_tics()
......@@ -23,7 +24,7 @@ void timer_delay(uint32_t how_long)
{
uint32_t t_start;
timer_init(1);
// timer_init(1);
do
{
t_start = timer_get_tics();
......
include/endpoint.h
View file @ 993e6bc2
......@@ -4,6 +4,8 @@
#define DMTD_AVG_SAMPLES 256
#define DMTD_MAX_PHASE 16384
#include <stdint.h>
typedef enum {
AND=0,
NAND=4,
......@@ -18,7 +20,7 @@ NOT=7
void ep_init(uint8_t mac_addr[]);
void get_mac_addr(uint8_t dev_addr[]);
int ep_enable(int enabled, int autoneg);
int ep_link_up();
int ep_link_up(uint16_t *lpa);
int ep_get_deltas(uint32_t *delta_tx, uint32_t *delta_rx);
int ep_get_psval(int32_t *psval);
int ep_cal_pattern_enable();
......@@ -33,4 +35,7 @@ static void pfilter_logic3(int rd, int ra, pfilter_op_t op, int rb, pfilter_op_t
void pfilter_load();
void pfilter_init_default();
uint16_t pcs_read(int location);
void pcs_write(int location, int value);
#endif
include/softpll.h deleted 100644 → 0
View file @ 0f22c9b5
#ifndef __SOFTPLL_H
#define __SOFTPLL_H
#define SOFTPLL_AUX_ENABLED 1
#define SOFTPLL_AUX_LOCKED 2
void softpll_enable();
int softpll_check_lock();
void softpll_disable();
int softpll_busy();
void softpll_set_phase(int ps);
int softpll_get_setpoint();
int softpll_get_aux_status();
#endif
include/softpll_ng.h
View file @ 993e6bc2
......@@ -10,6 +10,10 @@
#define SPLL_MODE_SLAVE 3
#define SPLL_MODE_DISABLED 4
#define SPLL_ALL_CHANNELS 0xffff
#define SPLL_AUX_ENABLED (1<<0)
#define SPLL_AUX_LOCKED (1<<1)
void spll_init(int mode, int slave_ref_channel, int align_pps);
void spll_shutdown();
......@@ -21,6 +25,9 @@ void spll_get_phase_shift(int channel, int32_t *current, int32_t *target);
int spll_read_ptracker(int channel, int32_t *phase_ps, int *enabled);
void spll_get_num_channels(int *n_ref, int *n_out);
int spll_shifter_busy(int channel);
int spll_get_delock_count();
int spll_update_aux_clocks();
int spll_get_aux_status(int channel);
#endif
include/spll_defs.h
View file @ 993e6bc2
......@@ -15,6 +15,11 @@ WARNING: These parameters must be in sync with the generics of the HDL instantia
/* Reference clock period, in picoseconds */
#define CLOCK_PERIOD_PICOSECONDS 8000
/* optional DMTD clock division to improve FPGA timing closure by avoiding
clock nets directly driving FD inputs. Must be consistent with the
g_divide_inputs_by_2 generic. */
#define DIVIDE_DMTD_CLOCKS_BY_2 1
/* Number of bits in phase tags generated by the DMTDs. Used to sign-extend the tags.
Corresponding VHDL generic: g_tag_bits. */
#define TAG_BITS 22
......
include/spll_helper.h
View file @ 993e6bc2
......@@ -17,6 +17,7 @@ struct spll_helper_state {
int p_setpoint, tag_d0;
int ref_src;
int sample_n;
int delock_count;
spll_pi_t pi;
spll_lock_det_t ld;
};
......@@ -27,16 +28,17 @@ static void helper_init(struct spll_helper_state *s, int ref_channel)
/* Phase branch PI controller */
s->pi.y_min = 5;
s->pi.y_max = (1 << DAC_BITS) - 5;
s->pi.kp = (int)(0.3 * 32.0 * 16.0);
s->pi.ki = (int)(0.03 * 32.0 * 3.0);
s->pi.kp = (int)(0.3 * 32.0 * 16.0);// / 2;
s->pi.ki = (int)(0.03 * 32.0 * 3.0);// / 2;
s->pi.anti_windup = 1;
/* Phase branch lock detection */
s->ld.threshold = 200;
s->ld.lock_samples = 1000;
s->ld.delock_samples = 900;
s->ld.lock_samples = 10000;
s->ld.delock_samples = 100;
s->ref_src = ref_channel;
s->delock_count = 0;
}
static int helper_update(struct spll_helper_state *s, int tag, int source)
......
include/spll_main.h
View file @ 993e6bc2
......@@ -25,6 +25,8 @@ struct spll_main_state {
int phase_shift_current;
int id_ref, id_out; /* IDs of the reference and the output channel */
int sample_n;
int delock_count;
int dac_index;
};
......@@ -35,15 +37,19 @@ static void mpll_init(struct spll_main_state *s, int id_ref, int id_out)
s->pi.y_max = 65530;
s->pi.anti_windup = 1;
s->pi.bias = 65000;
s->pi.kp = 1100;
s->pi.ki = 30;
s->pi.kp = 1100;// / 2;
s->pi.ki = 30;// / 2;
s->delock_count = 0;
/* Freqency branch lock detection */
s->ld.threshold = 120;
s->ld.lock_samples = 400;
s->ld.delock_samples = 390;
s->ld.threshold = 1200;
s->ld.lock_samples = 1000;
s->ld.delock_samples = 100;
s->id_ref = id_ref;
s->id_out = id_out;
s->dac_index = id_out - n_chan_ref;
mprintf("DACindex : %d\n", s->dac_index);
pi_init(&s->pi);
ld_init(&s->ld);
......@@ -167,7 +173,7 @@ static int mpll_update(struct spll_main_state *s, int tag, int source)
#endif
y = pi_update(&s->pi, err);
SPLL->DAC_MAIN = SPLL_DAC_MAIN_VALUE_W(y) | SPLL_DAC_MAIN_DAC_SEL_W(s->id_out);
SPLL->DAC_MAIN = SPLL_DAC_MAIN_VALUE_W(y) | SPLL_DAC_MAIN_DAC_SEL_W(s->dac_index);
spll_debug(DBG_MAIN | DBG_REF, s->tag_ref + s->adder_ref, 0);
spll_debug(DBG_MAIN | DBG_TAG, s->tag_out + s->adder_out, 0);
......
monitor/monitor.c
View file @ 993e6bc2
......@@ -76,13 +76,14 @@ int wrc_mon_gui(void)
m_cprintf(C_GREY, "Synchronization source: "); m_cprintf(C_WHITE, "%s\n", cur_servo_state.sync_source);
m_cprintf(C_GREY, "Aux clock status: ");
//aux_stat = spll_check_lock(1); //GGDD softpll_get_aux_status();
//if(aux_stat & SOFTPLL_AUX_ENABLED)
// m_cprintf(C_GREEN,"enabled");
aux_stat = spll_get_aux_status(0);
//if(aux_stat & SOFTPLL_AUX_LOCKED)
// m_cprintf(C_GREEN,", locked");
if(aux_stat & SPLL_AUX_ENABLED)
m_cprintf(C_GREEN,"enabled");
if(aux_stat & SPLL_AUX_LOCKED)
m_cprintf(C_GREEN,", locked");
mprintf("\n");
m_cprintf(C_BLUE, "\nTiming parameters:\n\n");
......
wrc_main.c
View file @ 993e6bc2
......@@ -18,6 +18,7 @@
RunTimeOpts rtOpts = {
.ifaceName = { "wr0" },
.announceInterval = DEFAULT_ANNOUNCE_INTERVAL,
.syncInterval = DEFAULT_SYNC_INTERVAL,
.clockQuality.clockAccuracy = DEFAULT_CLOCK_ACCURACY,
......@@ -43,6 +44,9 @@ RunTimeOpts rtOpts = {
.E2E_mode = TRUE,
.wrStateRetry = WR_DEFAULT_STATE_REPEAT,
.wrStateTimeout= WR_DEFAULT_STATE_TIMEOUT_MS,
.phyCalibrationRequired = FALSE,
.disableFallbackIfWRFails = TRUE,
/*SLAVE only or MASTER only*/
#ifdef WRPC_SLAVE
.primarySource = FALSE,
......@@ -202,8 +206,8 @@ void wrc_initialize()
uart_init();
uart_write_string(__FILE__ " is up (compiled on "
__DATE__ " " __TIME__ ")\n");
// uart_write_string(__FILE__ " is up (compiled on "
// __DATE__ " " __TIME__ ")\n");
mprintf("wr_core: starting up (press G to launch the GUI and D for extra debug messages)....\n");
......@@ -281,9 +285,9 @@ void wrc_initialize()
int wrc_check_link()
{
static int prev_link_state = -1;
int link_state = ep_link_up();
int rv = 0;
static int prev_link_state = -1;
int link_state = ep_link_up(NULL);
int rv = 0;
if(!prev_link_state && link_state)
{
......@@ -342,7 +346,7 @@ void wrc_handle_input()
case 'd':
wrc_extra_debug = 1 - wrc_extra_debug;
wrc_debug_printf(0,"Verbose debug %s.\n", wrc_extra_debug ? "enabled" : "disabled");
// wrc_debug_printf(0,"Verbose debug %s.\n", wrc_extra_debug ? "enabled" : "disabled");
break;
......@@ -359,9 +363,6 @@ void wrc_handle_input()
// wrc_debug_printf(0,"Manual phase adjust: %d\n", wrc_man_phase);
wr_servo_man_adjust_phase(wrc_man_phase);
break;
}
}
}
......@@ -371,32 +372,22 @@ extern volatile int irq_cnt;
int main(void)
{
wrc_initialize();
//for(;;)
//{
// //mprintf("%d\n", timer_get_tics());
// spll_show_stats();
//}
//test_transition();
//wr_servo_rollover_test(1);
for(;;)
{
wrc_handle_input();
if(wrc_gui_mode)
wrc_mon_gui();
#if 1
wrc_initialize();
spll_init(SPLL_MODE_FREE_RUNNING_MASTER, 0, 1);
for(;;)
{
wrc_handle_input();
if(wrc_gui_mode)
wrc_mon_gui();
int l_status = wrc_check_link();
switch (l_status)
{
case LINK_WENT_UP:
break;
case LINK_UP:
// softpll_check_lock();
update_rx_queues();
break;
......@@ -405,12 +396,11 @@ int main(void)
break;
}
singlePortLoop(&rtOpts, ptpPortDS, 0);// RunTimeOpts *rtOpts, PtpPortDS *ptpPortDS, int portIndex)
singlePortLoop(&rtOpts, ptpPortDS, 0);// RunTimeOpts *rtOpts, PtpPortDS *ptpPortDS, int portIndex)
sharedPortsLoop(ptpPortDS);
delay(100000);
#endif
}
spll_update_aux_clocks();
// delay(1000);
}
}
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