Skip to content

S
Software for White Rabbit PTP Core
Commit 55e4128a authored by Mattia Rizzi's avatar Mattia Rizzi
Browse files
Options

SyncE compliant Soft PLL

parent fc20ab5f
Hide whitespace changes
Inline Side-by-side
Showing with 176 additions and 21 deletions
include/spll_defs.h
View file @ 55e4128a
......@@ -31,7 +31,7 @@ WARNING: These parameters must be in sync with the generics of the HDL instantia
#define HPLL_N 14
/* Fractional bits in PI controller coefficients */
#define PI_FRACBITS 12
#define PI_FRACBITS 16
/* Max. allowed number of reference channels. Can be used to tweak memory usage. */
#define MAX_CHAN_REF BOARD_MAX_CHAN_REF /* Depends on wrc/wrs */
......
softpll/softpll_ng.c
View file @ 55e4128a
......@@ -12,7 +12,7 @@
#include "irq.h"
volatile int irq_count = 0;
volatile unsigned int irq_count = 0;
volatile struct SPLL_WB *SPLL;
volatile struct PPSG_WB *PPSG;
......@@ -471,13 +471,16 @@ void spll_get_num_channels(int *n_ref, int *n_out)
void spll_show_stats()
{
if (softpll.mode > 0)
if (softpll.mode > 0) {
TRACE_DEV("softpll: irqs %d seq %s mode %d "
"alignment_state %d HL%d ML%d HY=%d MY=%d DelCnt=%d\n",
"alignment_state %d HL%d ML%d HY=%d e_h:%d MY=%d DelCnt=%d ",
irq_count, stringlist_lookup(seq_states, softpll.seq_state), softpll.mode,
softpll.ext.align_state, softpll.helper.ld.locked, softpll.mpll.ld.locked,
softpll.helper.pi.y, softpll.mpll.pi.y,
softpll.helper.pi.y, softpll.helper.err,softpll.mpll.pi.y,
softpll.delock_count);
TRACE_DEV("h_avg:%d m_avg:%d ref_size:%d out_size:%d\n", softpll.helper.ld.avg_value, softpll.mpll.ld.avg_value, softpll.mpll.ref_data, softpll.mpll.out_data);
}
}
int spll_shifter_busy(int channel)
......
softpll/spll_common.c
View file @ 55e4128a
......@@ -21,7 +21,7 @@ int pi_update(spll_pi_t *pi, int x)
pi->x = x;
i_new = pi->integrator + x;
y = ((i_new * pi->ki + x * pi->kp) >> PI_FRACBITS) + pi->bias;
y = (((int64_t)i_new * (int64_t)pi->ki + (int64_t)x * (int64_t)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
......@@ -50,6 +50,9 @@ void pi_init(spll_pi_t *pi)
pi->y = pi->bias;
}
/* 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
......@@ -62,13 +65,22 @@ void pi_init(spll_pi_t *pi)
*/
int ld_update(spll_lock_det_t *ld, int y)
{
ld->call_count++;
ld->avg_acc += y;
if (ld->call_count == (1 << LOG2_AVG_PERIOD)-1) {
ld->call_count = 0;
ld->avg_ready = 1;
ld->avg_value = ld->avg_acc >> LOG2_AVG_PERIOD;
ld->avg_acc = 0;
}
ld->lock_changed = 0;
if (abs(y) <= ld->threshold) {
if (ld->lock_cnt < ld->lock_samples)
ld->lock_cnt++;
if (ld->lock_cnt == ld->lock_samples) {
if (ld->lock_cnt == ld->lock_samples || (ld->avg_ready == 1 && abs(ld->avg_value) < ld->threshold)) {
ld->lock_changed = 1;
ld->locked = 1;
return 1;
......@@ -77,7 +89,7 @@ int ld_update(spll_lock_det_t *ld, int y)
if (ld->lock_cnt > ld->delock_samples)
ld->lock_cnt--;
if (ld->lock_cnt == ld->delock_samples) {
if (ld->lock_cnt == ld->delock_samples && (ld->avg_ready == 1 && abs(ld->avg_value) >= ld->threshold)){
ld->lock_cnt = 0;
ld->lock_changed = 1;
ld->locked = 0;
......@@ -92,6 +104,10 @@ void ld_init(spll_lock_det_t *ld)
ld->locked = 0;
ld->lock_cnt = 0;
ld->lock_changed = 0;
ld->avg_acc = 0;
ld->call_count = 0;
ld->avg_ready = 0;
}
void lowpass_init(spll_lowpass_t *lp, int alpha)
......
softpll/spll_common.h
View file @ 55e4128a
......@@ -24,6 +24,9 @@
#define SPLL_LOCKED 1
#define SPLL_LOCKING 0
#define LOG2_AVG_PERIOD 13
/* Number of reference/output channels. We don't plan to have more than one
SoftPLL instantiation per project, so these can remain global. */
extern int spll_n_chan_ref, spll_n_chan_out;
......@@ -43,6 +46,8 @@ typedef struct {
int x, y; /* Current input (x) and output value (y) */
} spll_pi_t;
/* lock detector state */
typedef struct {
int lock_cnt; /* Lock sample counter */
......@@ -52,8 +57,16 @@ typedef struct {
int threshold; /* Error threshold */
int locked; /* Non-zero: we are locked */
int lock_changed;
int call_count;
int avg_ready;
int avg_value;
int64_t avg_acc;
} spll_lock_det_t;
/* simple, 1st-order lowpass filter */
typedef struct {
int alpha;
......
softpll/spll_debug.h
View file @ 55e4128a
......@@ -27,7 +27,7 @@ integral/proportional gains on the response of the system.
#define DBG_EVENT 4
#define DBG_SAMPLE_ID 6
#define DBG_HELPER 0x20 /* Sample source: Helper PLL */
#define DBG_HELPER 0x10 /* Sample source: Helper PLL */
#define DBG_EXT 0x40 /* Sample source: External Reference PLL */
#define DBG_MAIN 0x0 /* ... : Main PLL */
......
softpll/spll_helper.c
View file @ 55e4128a
......@@ -25,16 +25,21 @@ 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 = 150;//(int)(0.3 * 32.0 * 16.0); // / 2;
s->pi.ki = 2;//(int)(0.03 * 32.0 * 3.0); // / 2;
s->pi.kp = 150*(1 << 4);//(int)(0.3 * 32.0 * 16.0); // / 2;
s->pi.ki = 2*(1 << 4);;//(int)(0.03 * 32.0 * 3.0); // / 2;
s->pi.anti_windup = 1;
/* Phase branch lock detection */
s->ld.threshold = 200;
s->ld.threshold = 2000;
s->ld.lock_samples = 10000;
s->ld.delock_samples = 100;
s->ref_src = ref_channel;
s->delock_count = 0;
s->ld.avg_acc = 0;
s->ld.call_count = 0;
s->ld.avg_ready = 0;
s->ld.avg_value = 0;
}
int helper_update(struct spll_helper_state *s, int tag,
......@@ -57,6 +62,7 @@ int helper_update(struct spll_helper_state *s, int tag,
s->p_adder += (1 << TAG_BITS);
err = (tag + s->p_adder) - s->p_setpoint;
s->err = err;
if (HELPER_ERROR_CLAMP) {
if (err < -HELPER_ERROR_CLAMP)
......@@ -74,11 +80,12 @@ int helper_update(struct spll_helper_state *s, int tag,
s->p_setpoint += (1 << HPLL_N);
s->tag_d0 = tag;
y = pi_update((spll_pi_t *)&s->pi, err);
SPLL->DAC_HPLL = y;
spll_debug(DBG_SAMPLE_ID | DBG_HELPER, s->sample_n++, 0);
spll_debug(DBG_SAMPLE_ID | DBG_HELPER, s->ld.lock_cnt, 0);//s->sample_n++, 0);
spll_debug(DBG_Y | DBG_HELPER, y, 0);
spll_debug(DBG_ERR | DBG_HELPER, err, 1);
......
softpll/spll_helper.h
View file @ 55e4128a
......@@ -31,6 +31,7 @@ struct spll_helper_state {
int ref_src;
int sample_n;
int delock_count;
int err;
spll_pi_t pi;
spll_lock_det_t ld;
spll_biquad_t precomp;
......
softpll/spll_main.c
View file @ 55e4128a
......@@ -22,6 +22,18 @@
#undef WITH_SEQUENCING
void mpll_init_tag_history (struct spll_main_state *s) {
s->ref_data = 0; s->out_data = 0;
s->read_i = 0; s->ref_write_i = 0; s->out_write_i = 0;
s->skip_initial = 1;
// Just init the last one
s->ref_tags[TAG_HISTORY-1].adder = 0;
s->ref_tags[TAG_HISTORY-1].tag = 0;
s->out_tags[TAG_HISTORY-1].adder = 0;
s->out_tags[TAG_HISTORY-1].tag = 0;
}
void mpll_init(struct spll_main_state *s, int id_ref,
int id_out)
{
......@@ -31,8 +43,8 @@ void mpll_init(struct spll_main_state *s, int id_ref,
s->pi.anti_windup = 1;
s->pi.bias = 30000;
#if defined(CONFIG_WR_SWITCH)
s->pi.kp = 1100; // / 2;
s->pi.ki = 30; // / 2;
s->pi.kp = 5000; // / 2;
s->pi.ki = 1; // / 2;
#elif defined(CONFIG_WR_NODE)
s->pi.kp = 1100; // / 2;
s->pi.ki = 30; // / 2;
......@@ -43,15 +55,23 @@ void mpll_init(struct spll_main_state *s, int id_ref,
s->enabled = 0;
/* Freqency branch lock detection */
s->ld.threshold = 1200;
s->ld.lock_samples = 1000;
s->ld.threshold = 4000;
s->ld.lock_samples = 3814*3;
s->ld.delock_samples = 100;
s->ld.avg_acc = 0;
s->ld.call_count = 0;
s->ld.avg_ready = 0;
s->ld.avg_value = 0;
s->id_ref = id_ref;
s->id_out = id_out;
s->dac_index = id_out - spll_n_chan_ref;
TRACE_DEV("ref %d out %d idx %x \n", s->id_ref, s->id_out, s->dac_index);
mpll_init_tag_history(s);
pi_init((spll_pi_t *)&s->pi);
ld_init((spll_lock_det_t *)&s->ld);
}
......@@ -76,6 +96,13 @@ void mpll_start(struct spll_main_state *s)
pi_init((spll_pi_t *)&s->pi);
ld_init((spll_lock_det_t *)&s->ld);
mpll_init_tag_history(s);
s->ld.avg_acc = 0;
s->ld.call_count = 0;
s->ld.avg_ready = 0;
s->ld.avg_value = 0;
spll_enable_tagger(s->id_ref, 1);
spll_enable_tagger(s->id_out, 1);
spll_debug(DBG_EVENT | DBG_MAIN, DBG_EVT_START, 1);
......@@ -87,15 +114,43 @@ void mpll_stop(struct spll_main_state *s)
s->enabled = 0;
}
//Define multi Unit Interval Tracking
#define MULTIUI
int mpll_update(struct spll_main_state *s, int tag, int source)
{
if(!s->enabled)
return SPLL_LOCKED;
int err, y;
struct spll_tag *prev_tag;
if (source == s->id_ref) {
prev_tag = &s->ref_tags[WRAP(s->ref_write_i-1,TAG_HISTORY)];
s->ref_tags[s->ref_write_i].tag = tag;
if (prev_tag->tag > tag) {
s->ref_tags[s->ref_write_i].adder = prev_tag->adder + (1 << TAG_BITS);
} else s->ref_tags[s->ref_write_i].adder = prev_tag->adder;
s->ref_write_i = WRAP(s->ref_write_i+1, TAG_HISTORY);
s->ref_data++;
}
if (source == s->id_out) {
prev_tag = &s->out_tags[WRAP(s->out_write_i-1,TAG_HISTORY)];
s->out_tags[s->out_write_i].tag = tag;
if (prev_tag->tag > tag) {
s->out_tags[s->out_write_i].adder = prev_tag->adder + (1 << TAG_BITS);
} else s->out_tags[s->out_write_i].adder = prev_tag->adder;
s->out_write_i = WRAP(s->out_write_i+1, TAG_HISTORY);
s->out_data++;
}
if (source == s->id_ref)
if (source == s->id_ref) {
s->tag_ref = tag;
}
if (source == s->id_out)
s->tag_out = tag;
......@@ -105,6 +160,9 @@ int mpll_update(struct spll_main_state *s, int tag, int source)
s->adder_ref += (1 << TAG_BITS);
s->tag_ref_d = s->tag_ref;
}
......@@ -113,10 +171,24 @@ int mpll_update(struct spll_main_state *s, int tag, int source)
s->adder_out += (1 << TAG_BITS);
s->tag_out_d = s->tag_out;
}
#ifndef MULTIUI
if (s->tag_ref >= 0 && s->tag_out >= 0) {
err = s->adder_ref + s->tag_ref - s->adder_out - s->tag_out;
#else
if (s->ref_data > 0 && s->out_data > 0 && s->skip_initial==0) {
#endif
uint8_t read_p = WRAP(s->read_i, TAG_HISTORY);
#ifdef MULTIUI
err = s->ref_tags[read_p].adder + s->ref_tags[read_p].tag + s->phase_shift_current -
s->out_tags[read_p].adder - s->out_tags[read_p].tag;
#else
err = s->adder_ref + s->tag_ref - s->adder_out - s->tag_out;
#ifndef WITH_SEQUENCING
......@@ -130,12 +202,22 @@ int mpll_update(struct spll_main_state *s, int tag, int source)
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);
}
#endif
#endif
#define MAIN_ERROR_CLAMP 1000000
if (MAIN_ERROR_CLAMP) {
if (err < -MAIN_ERROR_CLAMP)
err = -MAIN_ERROR_CLAMP;
if (err > MAIN_ERROR_CLAMP)
err = MAIN_ERROR_CLAMP;
}
y = pi_update((spll_pi_t *)&s->pi, err);
SPLL->DAC_MAIN = SPLL_DAC_MAIN_VALUE_W(y)
......@@ -150,12 +232,21 @@ int mpll_update(struct spll_main_state *s, int tag, int source)
s->tag_out = -1;
s->tag_ref = -1;
if (s->adder_ref > 2 * MPLL_TAG_WRAPAROUND
&& s->adder_out > 2 * MPLL_TAG_WRAPAROUND) {
if (s->ref_tags[read_p].adder > 2 * MPLL_TAG_WRAPAROUND
&& s->out_tags[read_p].adder > 2 * MPLL_TAG_WRAPAROUND) {
int i;
s->adder_ref -= MPLL_TAG_WRAPAROUND;
s->adder_out -= MPLL_TAG_WRAPAROUND;
for (i=0; i < TAG_HISTORY; i++) {
s->ref_tags[i].adder -= MPLL_TAG_WRAPAROUND;
s->out_tags[i].adder -= MPLL_TAG_WRAPAROUND;
}
}
s->read_i = WRAP(s->read_i+1, TAG_HISTORY);
s->ref_data--; s->out_data--;
if (s->ld.locked) {
if (s->phase_shift_current < s->phase_shift_target) {
s->phase_shift_current++;
......@@ -168,6 +259,9 @@ int mpll_update(struct spll_main_state *s, int tag, int source)
}
if (ld_update((spll_lock_det_t *)&s->ld, err))
return SPLL_LOCKED;
} else if (s->skip_initial ==1 && s->ref_data > 0 && s->out_data > 0 ) {
mpll_init_tag_history(s);
s->skip_initial = 0;
}
return SPLL_LOCKING;
......@@ -201,6 +295,7 @@ int mpll_set_phase_shift(struct spll_main_state *s,
{
int div = (DIVIDE_DMTD_CLOCKS_BY_2 ? 2 : 1);
s->phase_shift_target = from_picos(desired_shift_ps) / div;
TRACE_DEV("Phase shift %d \n", s->phase_shift_target);
return 0;
}
......
softpll/spll_main.h
View file @ 55e4128a
......@@ -15,6 +15,18 @@
#include "spll_common.h"
// Strictily power of 2
#define TAG_HISTORY 64
#define WRAP(value,buffer_size) (value)&(buffer_size-1)
#define BUFFER_MASK TAG_HISTORY-1
struct spll_tag {
int tag;
int adder;
};
/* State of the Main PLL */
struct spll_main_state {
int state;
......@@ -36,6 +48,14 @@ struct spll_main_state {
int delock_count;
int dac_index;
int enabled;
struct spll_tag ref_tags[TAG_HISTORY];
struct spll_tag out_tags[TAG_HISTORY];
uint8_t ref_write_i; uint8_t out_write_i;
uint8_t ref_data, out_data;
int read_i;
int skip_initial;
};
void mpll_init(struct spll_main_state *s, int id_ref,
......
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