Skip to content

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

Merge branch 'spll_ng' into wrcore_v2 

Conflicts:
	Makefile
parents 4266b6f9 9bb5f281
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 1011 additions and 74 deletions
Makefile
View file @ 709e66d1
PLATFORM = lm32
OBJS_WRC = wrc_main.o dev/uart.o dev/endpoint.o dev/minic.o dev/pps_gen.o dev/syscon.o dev/onewire.o dev/softpll.o lib/mprintf.o monitor/monitor.o dev/ep_pfilter.o dev/dna.o
OBJS_WRC = wrc_main.o dev/uart.o dev/endpoint.o dev/minic.o dev/pps_gen.o dev/syscon.o dev/onewire.o dev/softpll_ng.o lib/mprintf.o monitor/monitor.o dev/ep_pfilter.o dev/dna.o dev/i2c.o dev/eeprom.o
D = ptp-noposix
PTPD_CFLAGS = -ffreestanding -DPTPD_FREESTANDING -DWRPC_EXTRA_SLIM -DPTPD_MSBF -DPTPD_DBG
......
dev/endpoint.c
View file @ 709e66d1
......@@ -151,8 +151,8 @@ int ep_get_deltas(uint32_t *delta_tx, uint32_t *delta_rx)
{
/* fixme: these values should be stored in calibration block in the EEPROM on the FMC. Also, the TX/RX delays of a particular SFP
should be added here */
*delta_tx = 0;
*delta_rx = 15000 - 7000 + 195000 + 32000 + PICOS_PER_SERIAL_BIT * MDIO_WR_SPEC_BSLIDE_R(pcs_read(MDIO_REG_WR_SPEC)) + 2800 - 9000 - 40000 + 2700;
*delta_tx = 46407;
*delta_rx = 273593 + PICOS_PER_SERIAL_BIT * MDIO_WR_SPEC_BSLIDE_R(pcs_read(MDIO_REG_WR_SPEC));
return 0;
}
......
dev/softpll_ng.c 0 → 100644
View file @ 709e66d1
#include <stdio.h>
#include <stdlib.h>
#include "board.h"
#include "timer.h"
#include "trace.h"
#include "hw/softpll_regs.h"
#include "irq.h"
volatile int irq_count = 0,eee,yyy,py;
static volatile struct SPLL_WB *SPLL = (volatile struct SPLL_WB *) BASE_SOFTPLL;
/* The includes below contain code (not only declarations) to enable the compiler
to inline functions where necessary and save some CPU cycles */
#include "spll_defs.h"
#include "spll_common.h"
#include "spll_debug.h"
#include "spll_helper.h"
#include "spll_main.h"
#include "spll_ptracker.h"
static volatile struct spll_helper_state helper;
static volatile struct spll_main_state mpll;
void _irq_entry()
{
volatile uint32_t trr;
int src = -1, tag;
if(! (SPLL->CSR & SPLL_TRR_CSR_EMPTY))
{
trr = SPLL->TRR_R0;
src = SPLL_TRR_R0_CHAN_ID_R(trr);
tag = SPLL_TRR_R0_VALUE_R(trr);
helper_update(&helper, tag, src);
mpll_update(&mpll, tag, src);
}
irq_count++;
//TRACE_DEV("%u\n", irq_count);
clear_irq();
}
void spll_init()
{
volatile int dummy;
disable_irq();
n_chan_ref = SPLL_CSR_N_REF_R(SPLL->CSR);
n_chan_out = SPLL_CSR_N_OUT_R(SPLL->CSR);
TRACE_DEV("SPLL_Init: %d ref channels, %d out channels\n", n_chan_ref, n_chan_out);
SPLL->CSR= 0 ;
SPLL->OCER = 0;
SPLL->RCER = 0;
SPLL->RCGER = 0;
SPLL->DCCR = 0;
SPLL->DEGLITCH_THR = 1000;
SPLL->DAC_MAIN = 32000;
while(! (SPLL->TRR_CSR & SPLL_TRR_CSR_EMPTY)) dummy = SPLL->TRR_R0;
dummy = SPLL->PER_HPLL;
SPLL->EIC_IER = 1;
}
int spll_check_lock()
{
return helper.ld.locked ? 1 : 0;
}
#define CHAN_RX 0
#define CHAN_TCXO 1
void spll_test()
{
int i = 0;
volatile int dummy;
TRACE_DEV("running spll_init\n");
// TRACE_DEV("enable irq\n");
// mpll_init(&mpll, 0, CHAN_TCXO);
while(!helper.ld.locked) ;//TRACE("%d\n", helper.phase.ld.locked);
TRACE_DEV("Helper locked, starting main\n");
// mpll_start(&mpll);
}
static int spll_master = 0;
void softpll_set_mode(int master)
{
spll_master = master;
}
/* Enables SoftPLL in Slave mode.*/
void softpll_enable()
{
if(spll_master)
mprintf("Softpll: running in MASTER mode\n");
else
mprintf("Softpll: running in SLAVE mode\n");
spll_init();
helper_init(&helper, spll_master? CHAN_TCXO : CHAN_RX); /* Change CHAN_RX to CHAN_TCXO if master mode */
helper_start(&helper);
enable_irq();
while(!helper.ld.locked) timer_delay(1);
TRACE_DEV("Helper locked\n");
if(spll_master) return;
/* comment the lines below if Master mode */
mpll_init(&mpll, CHAN_RX, CHAN_TCXO);
mpll_start(&mpll);
while(!mpll.ld.locked) timer_delay(1);
TRACE_DEV("Softpll locked\n");
}
int softpll_check_lock()
{
static int prev_lck = 0;
int lck = mpll.ld.locked && helper.ld.locked;
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 0;
}
int softpll_busy(int channel)
{
return mpll.phase_shift_target != mpll.phase_shift_current;
}
void softpll_set_phase( int ps)
{
mpll_set_phase_shift(&mpll, ((int32_t) ((int64_t)ps * (long long)(1<<HPLL_N) / 8000LL)));
}
void softpll_disable()
{
disable_irq();
}
int softpll_get_setpoint()
{
return mpll.phase_shift_target;
}
void softpll_test()
{
softpll_enable();
for(;;)
{
mprintf("L!\n\n");
softpll_set_phase(1000000);
while(softpll_busy(0)) mprintf("%d %d %d%d %d %d\n", mpll.phase_shift_current, mpll.phase_shift_target, mpll.ld.locked, helper.ld.locked, lerr, ly);
mprintf("R!\n\n");
softpll_set_phase(10000);
while(softpll_busy(0)) mprintf("%d %d %d%d %d\n", mpll.phase_shift_current, mpll.phase_shift_target, mpll.ld.locked, helper.ld.locked, lerr);
}
}
\ No newline at end of file
include/hw/softpll_regs.h
View file @ 709e66d1
This diff is collapsed.
include/inttypes.h
View file @ 709e66d1
......@@ -11,8 +11,6 @@ typedef signed short int16_t;
typedef signed int int32_t;
typedef signed long long int64_t;
typedef uint32_t useconds_t;
#define UINT32_MAX 4294967295U
#endif
include/spll_common.h 0 → 100644
View file @ 709e66d1
/*
White Rabbit Softcore PLL (SoftPLL) - common definitions
Copyright (c) 2010 - 2012 CERN / BE-CO-HT (Tomasz Włostowski)
Licensed under LGPL 2.1.
*/
/* Number of reference/output channels. Currently we support only one SoftPLL instantiation per project,
so these can remain static. */
static int n_chan_ref, n_chan_out;
/* 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 clapming and antiwindup algorithms */
int y_max;
int x, y; /* Current input (x) and output value (y) */
} spll_pi_t;
/* lock detector state */
typedef struct {
int lock_cnt; /* Lock sample counter */
int lock_samples; /* Number of samples below the (threshold) to assume that we are locked */
int delock_samples; /* Accumulated number of samples that causes the PLL go get out of lock.
delock_samples < lock_samples. */
int threshold; /* Error threshold */
int locked; /* Non-zero: we are locked */
} spll_lock_det_t;
/* Processes a single sample (x) with PI control algorithm (pi). Returns the value (y) 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 integrator 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 /* No antiwindup/clamping? */
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 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.
Return value:
0: PLL not locked
1: PLL locked
-1: PLL just got out of lock
*/
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;
return 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 -1;
}
}
return ld->locked;
}
static void ld_init(spll_lock_det_t *ld)
{
ld->locked = 0;
ld->lock_cnt = 0;
}
/* Enables/disables DDMTD tag generation on a given (channel).
Channels (0 ... n_chan_ref - 1) are the reference channels (e.g. transceivers' RX clocks
or a local reference)
Channels (n_chan_ref ... n_chan_out + n_chan_ref-1) are the output channels (local voltage
controlled oscillators). One output (usually the first one) is always used to drive the
oscillator which produces the reference clock for the transceiver. Other outputs can be
used to discipline external oscillators (e.g. on FMCs).
*/
static void spll_enable_tagger(int channel, int enable)
{
if(channel >= n_chan_ref) /* Output channel? */
{
if(enable)
SPLL->OCER |= 1<< (channel - n_chan_ref);
else
SPLL->OCER &= ~ (1<< (channel - n_chan_ref));
} else { /* Reference channel */
if(enable)
SPLL->RCER |= 1<<channel;
else
SPLL->RCER &= ~ (1<<channel);
}
TRACE_DEV("%s: ch %d, OCER 0x%x, RCER 0x%x\n", __FUNCTION__, channel, SPLL->OCER, SPLL->RCER);
}
include/spll_debug.h 0 → 100644
View file @ 709e66d1
/*
White Rabbit Softcore PLL (SoftPLL) - debugging/diagnostic interface
The so-called debug inteface is a large, interrupt-driven FIFO which passes
various realtime parameters (e.g. error value, tags, DAC drive) to an external
application where they can be analyzed. It's very useful for optimizing PI coefficients
and/or lock thresholds.
The data is organized as a stream of samples, where each sample can store a number of parameters.
For example, a stream samples with Y and ERR parameters can be used to evaluate the impact of
integral/proportional gains on the response of the system.
*/
#define DBG_Y 0
#define DBG_ERR 1
#define DBG_TAG 2
#define DBG_REF 5
#define DBG_PERIOD 3
#define DBG_EVENT 4
#define DBG_SAMPLE_ID 6
#define DBG_HELPER 0x20 /* Sample source: Helper PLL */
#define DBG_MAIN 0x0 /* ... : Main PLL */
#define DBG_EVT_START 1 /* PLL has just started */
#define DBG_EVT_LOCKED 2 /* PLL has just become locked */
/* Writes a parameter to the debug FIFO.
value: value of the parameter.
what: type of the parameter and its' source. For example,
- DBG_ERR | DBG_HELPER means that (value) contains the phase error of the helper PLL.
- DBG_EVENT indicates an asynchronous event. (value) must contain the event type (DBG_EVT_xxx)
last: when non-zero, indicates the last parameter in a sample.
*/
static inline void spll_debug(int what, int value, int last)
{
SPLL->DFR_SPLL = (last ? 0x80000000 : 0) | (value & 0xffffff) | (what << 24);
}
include/spll_defs.h 0 → 100644
View file @ 709e66d1
/*
White Rabbit Softcore PLL (SoftPLL) - common definitions
WARNING: These parameters must be in sync with the generics of the HDL instantiation of wr_softpll_ng.
*/
#include <stdio.h>
/* Reference clock frequency, in [Hz] */
#define CLOCK_FREQ 62500000
/* 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
/* Helper PLL N divider (2**(-N) is the frequency offset). Must be big enough
to offer reasonable PLL bandwidth, and small enough so the offset frequency fits
within the tuning range of the helper oscillator. */
#define HPLL_N 14
/* Fractional bits in PI controller coefficients */
#define PI_FRACBITS 12
/* Max. allowed number of reference channels. Can be used to tweak memory usage. */
#define MAX_CHAN_REF 7
/* Max. allowed number of output channels */
#define MAX_CHAN_OUT 1
/* Max. allowed number of phase trackers */
#define MAX_TRACKERS 6
/* Number of bits of the DAC(s) driving the oscillator(s). Must be the same for
all the outputs. */
#define DAC_BITS 16
/* 1.0 / (Speed of the phase shifter) - the higher value, the slower phase shifting.
Used to prevent de-locking PLLs when shifting large offsets. */
#define PHASE_SHIFTER_SPEED 1
\ No newline at end of file
include/spll_helper.h 0 → 100644
View file @ 709e66d1
/* 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.
*/
#define SPLL_LOCKED 1
#define SPLL_LOCKING 0
#define HELPER_TAG_WRAPAROUND 100000000
/* Maximum abs value of the phase error. If the error is bigger, it's clamped to this value. */
#define HELPER_ERROR_CLAMP 150000
struct spll_helper_state {
int p_adder; /* anti wrap-around adder */
int p_setpoint, tag_d0;
int ref_src;
int sample_n;
spll_pi_t pi;
spll_lock_det_t ld;
};
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.anti_windup = 1;
/* Set the bias to the upper end of tuning range. This is to ensure that
the HPLL will always lock on positive frequency offset. */
s->pi.bias = s->pi.y_max;
/* Phase branch lock detection */
s->ld.threshold = 200;
s->ld.lock_samples = 1000;
s->ld.delock_samples = 900;
s->ref_src = ref_channel;
s->p_setpoint = 0;
s->p_adder = 0;
s->sample_n = 0;
s->tag_d0 = -1;
pi_init(&s->pi);
ld_init(&s->ld);
}
static int helper_update(struct spll_helper_state *s, int tag, int source)
{
int err, y;
if(source == s->ref_src)
{
spll_debug(DBG_TAG | DBG_HELPER, tag, 0);
spll_debug(DBG_REF | DBG_HELPER, s->p_setpoint, 0);
if(s->tag_d0 < 0)
{
s->p_setpoint = tag;
s->tag_d0 = tag;
return SPLL_LOCKING;
}
if(s->tag_d0 > tag)
s->p_adder += (1<<TAG_BITS);
err = (tag + s->p_adder) - s->p_setpoint;
// if(s->tag_d0 - tag > -100000)
//spll_debug(DBG_ERR | DBG_HELPER, s->tag_d0-tag, 1);
if(HELPER_ERROR_CLAMP)
{
if(err < -HELPER_ERROR_CLAMP) err = -HELPER_ERROR_CLAMP;
if(err > HELPER_ERROR_CLAMP) err = HELPER_ERROR_CLAMP;
}
//GD
//TRACE_DEV("hpll 2err=%d\n", err);
if((tag + s->p_adder) > HELPER_TAG_WRAPAROUND && s->p_setpoint > HELPER_TAG_WRAPAROUND)
{
s->p_adder -= HELPER_TAG_WRAPAROUND;
s->p_setpoint -= HELPER_TAG_WRAPAROUND;
}
s->p_setpoint += (1<<HPLL_N);
s->tag_d0 = tag;
y = pi_update(&s->pi, err);
SPLL->DAC_HPLL = y;
spll_debug(DBG_SAMPLE_ID | DBG_HELPER, s->sample_n++, 0);
spll_debug(DBG_Y | DBG_HELPER, y, 0);
spll_debug(DBG_ERR | DBG_HELPER, err, 1);
if(ld_update(&s->ld, err))
return SPLL_LOCKED;
}
return SPLL_LOCKING;
}
static void helper_start(struct spll_helper_state *s)
{
spll_enable_tagger(s->ref_src, 1);
spll_debug(DBG_EVENT | DBG_HELPER, DBG_EVT_START, 1);
}
include/spll_main.h 0 → 100644
View file @ 709e66d1
#define MPLL_TAG_WRAPAROUND 1000000000
/* State of the Main PLL */
struct spll_main_state {
int state;
spll_pi_t pi;
spll_lock_det_t ld;
int adder_ref, adder_out, tag_ref, tag_out, tag_ref_d, tag_out_d;
int phase_shift_target;
int phase_shift_current;
int id_ref, id_out; /* IDs of the reference and the output channel */
int sample_n;
int shift_div;
};
volatile int lerr = 0, ly=0;
static void mpll_init(struct spll_main_state *s, int id_ref, int id_out)
{
/* Frequency branch PI controller */
s->pi.y_min = 5;
s->pi.y_max = 65530;
s->pi.anti_windup = 1;
s->pi.bias = 65000;
s->pi.kp = 1100;
s->pi.ki = 30;
/* Freqency branch lock detection */
s->ld.threshold = 120;
s->ld.lock_samples = 400;
s->ld.delock_samples = 390;
s->adder_ref = s->adder_out = 0;
s->tag_ref = -1;
s->tag_out = -1;
s->tag_ref_d = -1;
s->tag_out_d = -1;
s->phase_shift_target = 0;
s->phase_shift_current = 0;
s->id_ref = id_ref;
s->id_out = id_out;
s->sample_n= 0;
s->shift_div = 0;
pi_init(&s->pi);
ld_init(&s->ld);
}
static void mpll_start(struct spll_main_state *s)
{
spll_enable_tagger(s->id_ref, 1);
spll_enable_tagger(s->id_out, 1);
spll_debug(DBG_EVENT | DBG_MAIN, DBG_EVT_START, 1);
}
static int mpll_update(struct spll_main_state *s, int tag, int source)
{
int err, y, tmp;
if(source == s->id_ref)
s->tag_ref = tag;
if(source == s->id_out)
s->tag_out = tag;
if(s->tag_ref >= 0 && s->tag_out >= 0)
{
if(s->tag_ref_d >= 0 && s->tag_ref_d > s->tag_ref)
s->adder_ref += (1<<TAG_BITS);
if(s->tag_out_d >= 0 && s->tag_out_d > s->tag_out)
s->adder_out += (1<<TAG_BITS);
err = s->adder_ref + s->tag_ref - s->adder_out - s->tag_out;
if(s->adder_ref > MPLL_TAG_WRAPAROUND && s->adder_out > MPLL_TAG_WRAPAROUND)
{
s->adder_ref -= MPLL_TAG_WRAPAROUND;
s->adder_out -= MPLL_TAG_WRAPAROUND;
}
/* Hack: the PLL is locked, so the tags are close to each other. But when we start phase shifting, after reaching
full clock period, one of the reference tags will flip before the other, causing a suddent 2**HPLL_N jump in the error.
So, once the PLL is locked, we just mask out everything above 2**HPLL_N.
Proper solution: tag sequence numbers */
if(s->ld.locked)
{
err &= (1<<HPLL_N)-1;
if(err & (1<<(HPLL_N-1)))
err |= ~((1<<HPLL_N)-1);
}
y = pi_update(&s->pi, err);
lerr = err;
ly=y;
SPLL->DAC_MAIN = SPLL_DAC_MAIN_VALUE_W(y) | SPLL_DAC_MAIN_DAC_SEL_W(s->id_out);
spll_debug(DBG_MAIN | DBG_ERR, err, 0);
spll_debug(DBG_MAIN | DBG_SAMPLE_ID, s->sample_n++, 0);
spll_debug(DBG_MAIN | DBG_Y, y, 1);
s->tag_ref_d = s->tag_ref;
s->tag_out_d = s->tag_out;
s->tag_out = -1;
s->tag_ref = -1;
if(s->phase_shift_current < s->phase_shift_target)
{
if(s->shift_div == PHASE_SHIFTER_SPEED-1)
{
s->phase_shift_current++;
s->adder_ref++;
s->shift_div = 0;
} else s->shift_div++;
} else if(s->phase_shift_current > s->phase_shift_target) {
if(s->shift_div == PHASE_SHIFTER_SPEED-1)
{
s->phase_shift_current--;
s->adder_ref--;
s->shift_div = 0;
} else s->shift_div++;
}
if(ld_update(&s->ld, err))
return SPLL_LOCKED;
}
return SPLL_LOCKING;
}
static int mpll_set_phase_shift(struct spll_main_state *s, int desired_shift)
{
s->shift_div = 0;
s->phase_shift_target = desired_shift;
}
static int mpll_shifter_busy(struct spll_main_state *s)
{
return s->phase_shift_target != s->phase_shift_current;
}
\ No newline at end of file
include/spll_ptracker.h 0 → 100644
View file @ 709e66d1
/* State of a Phase Tracker */
struct spll_ptracker_state {
int id_a, id_b;
int n_avg, acc, avg_count;
int phase_val, ready;
int tag_a, tag_b;
int sample_n;
int preserve_sign;
};
static void ptracker_init(struct spll_ptracker_state *s, int id_a, int id_b, int num_avgs)
{
s->tag_a = s->tag_b = -1;
s->id_a = id_a;
s->id_b = id_b;
s->ready = 0;
s->n_avg = num_avgs;
s->acc = 0;
s->avg_count = 0;
s->sample_n= 0;
s->preserve_sign = 0;
}
static void ptracker_start(struct spll_ptracker_state *s)
{