Skip to content

S
Software for White Rabbit PTP Core
Commit 5330073e authored by Tomasz Wlostowski's avatar Tomasz Wlostowski Committed by root
Browse files
Options

softpll: working HPLL

parent 9c9e24fe
Hide whitespace changes
Inline Side-by-side
Showing with 79 additions and 53 deletions
dev/softpll.c
View file @ 5330073e
...@@ -5,6 +5,8 @@ ...@@ -5,6 +5,8 @@
#define TAG_BITS 17 #define TAG_BITS 17
#define HPLL_N 14
#define HPLL_DAC_BIAS 30000 #define HPLL_DAC_BIAS 30000
#define PI_FRACBITS 12 #define PI_FRACBITS 12
...@@ -27,6 +29,10 @@ struct softpll_config { ...@@ -27,6 +29,10 @@ struct softpll_config {
int hpll_ld_p_samples; int hpll_ld_p_samples;
int hpll_ld_p_threshold; int hpll_ld_p_threshold;
int hpll_delock_threshold;
int hpll_dac_bias;
int dpll_f_kp; int dpll_f_kp;
int dpll_f_ki; int dpll_f_ki;
...@@ -35,6 +41,7 @@ struct softpll_config { ...@@ -35,6 +41,7 @@ struct softpll_config {
int dpll_ld_samples; int dpll_ld_samples;
int dpll_ld_threshold; int dpll_ld_threshold;
int dpll_delock_threshold;
}; };
...@@ -51,6 +58,8 @@ const struct softpll_config pll_cfg = ...@@ -51,6 +58,8 @@ const struct softpll_config pll_cfg =
0.03*32*16, // p_ki 0.03*32*16, // p_ki
1000, // lock detect phase samples 1000, // lock detect phase samples
220, // lock detect phase threshold 220, // lock detect phase threshold
500, // delock threshold
32000, // HPLL dac bias
/* DMTD PLL */ /* DMTD PLL */
...@@ -70,85 +79,102 @@ struct softpll_state { ...@@ -70,85 +79,102 @@ struct softpll_state {
int h_dac_bias; int h_dac_bias;
int h_p_setpoint; int h_p_setpoint;
int h_freq_mode; int h_freq_mode;
} int h_locked;
};
static int hpll_lock_count = 0;
static int hpll_i = 0;
static int hpll_dac_bias = 0;
static int hpll_perr_setpoint;
int hpll_freq_mode = 1;
volatile int freq_err, eee;
volatile int phase_err;
volatile int hpll_tag;
int hpll_prev_tag;
volatile int dv; static volatile struct softpll_state pstate;
void _irq_entry() void _irq_entry()
{ {
int dv;
if(hpll_freq_mode) /* HPLL: active frequency branch */
if(pstate.h_freq_mode)
{ {
freq_err = SPLL->PER_HPLL;// >> 1; int freq_err = SPLL->PER_HPLL;
if(freq_err & 0x100) freq_err |= 0xffffff00; /* sign-extend */
if(freq_err & 0x100) freq_err |= 0xffffff00; freq_err += pll_cfg.hpll_f_setpoint;
// freq_err = - freq_err;
freq_err += hpll_ferr_setpoint;
hpll_i += freq_err; /* PI control */
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;
eee=freq_err; SPLL->DAC_HPLL = dv; /* update DAC */
dv = ((hpll_i * hpll_ki + freq_err * hpll_kp) >> PI_FRACBITS) + hpll_dac_bias; /* lock detection */
SPLL->DAC_HPLL = dv; if(freq_err >= -pll_cfg.hpll_ld_f_threshold && freq_err <= pll_cfg.hpll_ld_f_threshold)
pstate.h_lock_counter++;
if(freq_err >= -hpll_ld_threshold && freq_err <= hpll_ld_threshold)
hpll_lock_count++;
else else
hpll_lock_count=0; pstate.h_lock_counter=0;
if(hpll_lock_count == hpll_ld_samples) /* frequency has been stable for quite a while? switch to phase branch */
if(pstate.h_lock_counter == pll_cfg.hpll_ld_f_samples)
{ {
hpll_freq_mode = 0; pstate.h_freq_mode = 0;
hpll_dac_bias = dv; pstate.h_dac_bias = dv;
hpll_i = 0; pstate.h_i = 0;
pstate.h_lock_counter = 0;
SPLL->CSR = SPLL_CSR_TAG_EN_W(CHAN_REF); SPLL->CSR = SPLL_CSR_TAG_EN_W(CHAN_REF);
} }
/* HPLL: active phase branch */
} else { } else {
if(hpll_perr_setpoint < 0) if(pstate.h_p_setpoint < 0) /* we don't have yet any phase samples? */
{ pstate.h_p_setpoint = SPLL->TAG_REF & 0x3fff;
hpll_perr_setpoint = SPLL->TAG_REF & 0x3fff; else {
} else { int phase_err;
phase_err = (SPLL->TAG_REF & 0x3fff) - hpll_perr_setpoint;
hpll_i += phase_err; phase_err = (SPLL->TAG_REF & 0x3fff) - pstate.h_p_setpoint;
dv = ((hpll_i * hpll_pki + phase_err * hpll_pkp) >> PI_FRACBITS) + hpll_dac_bias; pstate.h_i += phase_err;
SPLL->DAC_HPLL = dv; dv = ((pstate.h_i * pll_cfg.hpll_p_ki + phase_err * pll_cfg.hpll_p_kp) >> PI_FRACBITS) + pstate.h_dac_bias;
SPLL->DAC_HPLL = dv; /* Update DAC */
if(abs(phase_err) >= pll_cfg.hpll_delock_threshold && pstate.h_locked)
{
pstate.h_locked = 0;
pstate.h_freq_mode = 1;
}
if(abs(phase_err) <= pll_cfg.hpll_ld_p_threshold && !pstate.h_locked)
pstate.h_lock_counter++;
else
pstate.h_lock_counter = 0;
if(pstate.h_lock_counter == pll_cfg.hpll_ld_p_samples)
pstate.h_locked = 1;
} }
} }
clear_irq(); clear_irq();
} }
void softpll_init() void softpll_enable()
{ {
hpll_dac_bias = HPLL_DAC_BIAS; pstate.h_dac_bias = HPLL_DAC_BIAS;
SPLL->DAC_HPLL = HPLL_DAC_BIAS; SPLL->DAC_HPLL = pstate.h_dac_bias;
hpll_perr_setpoint = -1;
hpll_i = 0; pstate.h_p_setpoint = -1;
hpll_freq_mode = 1; pstate.h_i = 0;
hpll_lock_count = 0; pstate.h_freq_mode = 1;
pstate.h_lock_counter = 0;
pstate.h_locked = 0;
SPLL->CSR = SPLL_CSR_TAG_EN_W(CHAN_PERIOD); SPLL->CSR = SPLL_CSR_TAG_EN_W(CHAN_PERIOD);
SPLL->EIC_IER = 1; SPLL->EIC_IER = 1;
enable_irq(); enable_irq();
}
int softpll_check_lock()
for(;;) {
{ return pstate.h_locked;
mprintf("err %07d ldc %07d tag %07d dv %07d\n", eee, hpll_lock_count, phase_err, dv); }
}
} void softpll_disable()
\ No newline at end of file {
SPLL->CSR = 0;
disable_irq();
}
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