Skip to content

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

softpll: implement PI gain scheduling

parent ccefcbcc
Hide whitespace changes
Inline Side-by-side
Showing with 111 additions and 18 deletions
include/spll_defs.h
View file @ 1e82ca06
......@@ -52,3 +52,6 @@ WARNING: These parameters must be in sync with the generics of the HDL instantia
/* Number of samples in a single ptracker averaging bin */
#define PTRACKER_AVERAGE_SAMPLES 512
/* Maximum MPLL loop gain scheduler levels */
#define SPLL_GAIN_SCHED_MAX 2
\ No newline at end of file
softpll/softpll_ng.c
View file @ 1e82ca06
......@@ -186,7 +186,7 @@ static inline void sequencing_fsm(struct softpll_state *s, int tag_value, int ta
case SEQ_WAIT_MAIN:
{
if (s->mpll.ld.locked)
if (s->mpll.locked)
{
start_ptrackers(s);
s->seq_state = SEQ_READY;
......@@ -205,7 +205,7 @@ static inline void sequencing_fsm(struct softpll_state *s, int tag_value, int ta
s->delock_count++;
s->seq_state = SEQ_CLEAR_DACS;
set_channel_status(s->mpll.id_ref, 0);
} else if (s->mode == SPLL_MODE_SLAVE && !s->mpll.ld.locked) {
} else if (s->mode == SPLL_MODE_SLAVE && !s->mpll.locked) {
s->delock_count++;
s->seq_state = SEQ_CLEAR_DACS;
set_channel_status(s->mpll.id_ref, 0);
......@@ -501,7 +501,7 @@ void spll_show_stats()
"alignment_state %d HL%d ML%d HY=%d MY=%d DelCnt=%d setpoint:%d\n",
s->irq_count, statename,
s->mode, s->ext.align_state,
s->helper.ld.locked, s->mpll.ld.locked,
s->helper.ld.locked, s->mpll.locked,
s->helper.pi.y, s->mpll.pi.y,
s->delock_count, s->mpll.phase_shift_current);
}
......@@ -556,7 +556,7 @@ static int spll_update_aux_clocks(void)
switch (s->seq_state) {
case AUX_DISABLED:
if (softpll.mpll.ld.locked && aux_locking_enabled(ch)) {
if (softpll.mpll.locked && aux_locking_enabled(ch)) {
pll_verbose("softpll: enabled aux channel %d\n", ch);
spll_start_channel(ch);
s->seq_state = AUX_LOCK_PLL;
......@@ -583,7 +583,7 @@ static int spll_update_aux_clocks(void)
break;
case AUX_READY:
if (!softpll.mpll.ld.locked || !s->pll.dmtd.ld.locked) {
if (!softpll.mpll.locked || !s->pll.dmtd.ld.locked) {
pll_verbose("softpll: aux channel %d or mpll lost lock\n", ch);
set_channel_status(ch, 0);
s->seq_state = AUX_DISABLED;
......@@ -653,7 +653,7 @@ int spll_update()
stats.seq_state = softpll.seq_state;
stats.align_state = softpll.ext.align_state;
stats.H_lock = softpll.helper.ld.locked;
stats.M_lock = softpll.mpll.ld.locked;
stats.M_lock = softpll.mpll.locked;
stats.H_y = softpll.helper.pi.y;
stats.M_y = softpll.mpll.pi.y;
stats.del_cnt = softpll.delock_count;
......@@ -662,7 +662,7 @@ int spll_update()
return ret != 0;
}
int spll_measure_frequency(int osc)
static int spll_measure_frequency(int osc)
{
volatile uint32_t *reg;
......@@ -680,7 +680,7 @@ int spll_measure_frequency(int osc)
return 0;
}
// timer_delay_ms(2000);
timer_delay_ms(2000);
return (*reg ) & (0xfffffff);
}
......@@ -732,3 +732,11 @@ void check_vco_frequencies()
f_min = spll_measure_frequency(SPLL_OSC_EXT);
pll_verbose("EXT clock: Freq=%d Hz\n", f_min);
}
void spll_set_gain_schedule( spll_gain_schedule_t* sch )
{
disable_irq();
softpll.mpll.gain_sched = sch;
enable_irq();
}
softpll/softpll_ng.h
View file @ 1e82ca06
......@@ -111,8 +111,9 @@ void spll_set_dac(int out_channel, int value);
/* Returns current DAC sample value for output (out_channel) */
int spll_get_dac(int out_channel);
void spll_set_gain_schedule( spll_gain_schedule_t* sch );
void check_vco_frequencies(void);
int spll_measure_frequency(int osc);
void spll_set_ptracker_average_samples(int channel, int nsamples);
/*
......
softpll/spll_debug.h
View file @ 1e82ca06
......@@ -30,8 +30,10 @@ integral/proportional gains on the response of the system.
#define DBG_EXT 0x40 /* Sample source: External Reference 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 */
#define DBG_EVT_START 1 /* PLL has just started */
#define DBG_EVT_LOCKED 2 /* PLL has just become locked */
#define DBG_EVT_GAIN_SWITCH 3 /* PLL switched the PI gain (scheduling) */
/* Writes a parameter to the debug FIFO.
......
softpll/spll_external.c
View file @ 1e82ca06
......@@ -52,7 +52,7 @@ void external_start(struct spll_external_state *s)
int external_locked(volatile struct spll_external_state *s)
{
if (!s->helper->ld.locked || !s->main->ld.locked ||
if (!s->helper->ld.locked || !s->main->locked ||
!(SPLL->ECCR & SPLL_ECCR_EXT_REF_LOCKED) || // ext PLL became unlocked
(SPLL->ECCR & SPLL_ECCR_EXT_REF_STOPPED)) // 10MHz unplugged (only SPEC)
return 0;
......@@ -152,7 +152,7 @@ int external_align_fsm(volatile struct spll_external_state *s)
case ALIGN_STATE_START_MAIN:
SPLL->AL_CR = 2;
if(s->helper->ld.locked && s->main->ld.locked) {
if(s->helper->ld.locked && s->main->locked) {
PPSG->CR = PPSG_CR_CNT_EN | PPSG_CR_PWIDTH_W(10);
PPSG->ADJ_NSEC = 3;
PPSG->ESCR = PPSG_ESCR_SYNC;
......
softpll/spll_helper.c
View file @ 1e82ca06
......@@ -25,6 +25,7 @@ void helper_init(struct spll_helper_state *s, int ref_channel)
s->pi.ki = 2;
#endif
s->pi.anti_windup = 1;
s->pi.shift = PI_FRACBITS;
/* Phase branch lock detection */
s->ld.threshold = 200;
......
softpll/spll_main.c
View file @ 1e82ca06
......@@ -30,6 +30,7 @@ void mpll_init(struct spll_main_state *s, int id_ref,
s->pi.y_max = 65530;
s->pi.anti_windup = 1;
s->pi.bias = 30000;
s->pi.shift = PI_FRACBITS;
#if defined(CONFIG_WR_SWITCH)
if (spll_ljd_present) {
s->pi.kp = 2000;
......@@ -54,12 +55,65 @@ void mpll_init(struct spll_main_state *s, int id_ref,
s->id_out = id_out;
s->dac_index = id_out - spll_n_chan_ref;
pll_verbose("ref %d out %d idx %x \n", s->id_ref, s->id_out, s->dac_index);
if( s->gain_sched )
{
s->gain_sched->current_stage = 0;
s->gain_sched->locked_d = 0;
}
pi_init((spll_pi_t *)&s->pi);
ld_init((spll_lock_det_t *)&s->ld);
}
static inline void mpll_handle_gain_schedule( struct spll_main_state *s )
{
int do_update = 0;
if (!s->gain_sched)
{
s->locked = s->ld.locked;
return;
}
if( s->gain_sched->locked_d && !s->ld.locked ) // Pll out-of-lock? restart
{
s->gain_sched->current_stage = 0;
s->locked = 0;
do_update = 1;
}
else if ( !s->gain_sched->locked_d && s->ld.locked ) // PLL lock acquired? advance stage
{
spll_debug(DBG_EVENT | DBG_MAIN, DBG_EVT_GAIN_SWITCH, 0);
if ( s->gain_sched->current_stage == s->gain_sched->n_stages - 1 )
{
s->locked = 1;
s->gain_sched->locked_d = 1;
return;
}
else
{
s->gain_sched->current_stage++;
}
do_update = 1;
}
if( do_update )
{
spll_gain_schedule_item_t* stage = &s->gain_sched->stages[ s->gain_sched->current_stage ];
s->pi.kp = stage->kp;
s->pi.ki = stage->ki;
s->pi.shift = stage->shift;
s->ld.lock_samples = stage->lock_samples;
s->ld.lock_cnt = 0;
s->ld.lock_changed = 0;
s->ld.locked = 0;
s->gain_sched->locked_d = 0;
}
s->gain_sched->locked_d = s->ld.locked;
}
void mpll_start(struct spll_main_state *s)
{
pll_verbose("MPLL_Start [dac %d]\n", s->dac_index);
......@@ -74,6 +128,21 @@ void mpll_start(struct spll_main_state *s)
s->phase_shift_current = 0;
s->sample_n = 0;
s->enabled = 1;
s->locked = 0;
if( s->gain_sched )
{
s->gain_sched->current_stage = 0;
s->gain_sched->locked_d = 0;
s->pi.kp = s->gain_sched->stages[0].kp;
s->pi.ki = s->gain_sched->stages[0].ki;
s->pi.shift = s->gain_sched->stages[0].shift;
s->ld.lock_samples = s->gain_sched->stages[0].lock_samples;
s->ld.lock_cnt = 0;
}
pi_init((spll_pi_t *)&s->pi);
ld_init((spll_lock_det_t *)&s->ld);
......@@ -130,7 +199,7 @@ int mpll_update(struct spll_main_state *s, int tag, int source)
2**HPLL_N.
Proper solution: tag sequence numbers */
if (s->ld.locked) {
if (s->locked) {
err &= (1 << HPLL_N) - 1;
if (err & (1 << (HPLL_N - 1)))
err |= ~((1 << HPLL_N) - 1);
......@@ -159,7 +228,7 @@ int mpll_update(struct spll_main_state *s, int tag, int source)
s->adder_out -= MPLL_TAG_WRAPAROUND;
}
if (s->ld.locked) {
if (s->locked) {
if (s->phase_shift_current < s->phase_shift_target) {
s->phase_shift_current++;
#if defined(CONFIG_WR_SWITCH)
......@@ -177,7 +246,14 @@ int mpll_update(struct spll_main_state *s, int tag, int source)
#endif
}
}
if (ld_update((spll_lock_det_t *)&s->ld, err))
ld_update((spll_lock_det_t *)&s->ld, err);
if( s->ld.lock_changed)
spll_debug(DBG_EVENT | DBG_MAIN, DBG_EVT_LOCKED, 1);
mpll_handle_gain_schedule(s);
if(s->locked)
return SPLL_LOCKED;
}
......@@ -219,3 +295,4 @@ int mpll_shifter_busy(struct spll_main_state *s)
{
return s->phase_shift_target != s->phase_shift_current;
}
softpll/spll_main.h
View file @ 1e82ca06
......@@ -20,9 +20,10 @@ struct spll_main_state {
spll_pi_t pi;
spll_lock_det_t ld;
spll_gain_schedule_t* gain_sched;
int adder_ref, adder_out, tag_ref, tag_out, tag_ref_d, tag_out_d;
int locked; // locked flag
int phase_shift_target;
int phase_shift_current;
int id_ref, id_out; /* IDs of the reference and the output channel */
......
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