Commit ea82c87b authored by Alessandro Rubini's avatar Alessandro Rubini

softpll: automatically reindented

Signed-off-by: Alessandro Rubini's avatarAlessandro Rubini <rubini@gnudd.com>
parent c264291f
This diff is collapsed.
......@@ -20,9 +20,9 @@ void spll_start_channel(int channel);
void spll_stop_channel(int channel);
int spll_check_lock(int channel);
void spll_set_phase_shift(int channel, int32_t value_picoseconds);
void spll_get_phase_shift(int channel, int32_t *current, int32_t *target);
void spll_get_phase_shift(int channel, int32_t * current, int32_t * target);
void spll_enable_ptracker(int ref_channel, int enable);
int spll_read_ptracker(int channel, int32_t *phase_ps, int *enabled);
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();
......@@ -32,6 +32,4 @@ int spll_get_aux_status(int channel);
void spll_set_dac(int index, int value);
int spll_get_dac(int index);
#endif
......@@ -9,75 +9,73 @@ spll_common.h - common data structures and functions
*/
/* 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) */
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 */
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;
/* simple, 1st-order lowpass filter */
typedef struct {
int alpha;
int y_d;
int alpha;
int y_d;
} spll_lowpass_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)
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? */
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;
pi->y = y;
return y;
}
/* initializes the PI controller state. Currently almost a stub. */
static inline void pi_init(spll_pi_t *pi)
static inline void pi_init(spll_pi_t * pi)
{
pi->integrator = 0;
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.
......@@ -87,48 +85,44 @@ static inline void pi_init(spll_pi_t *pi)
1: PLL locked
-1: PLL just got out of lock
*/
static inline int ld_update(spll_lock_det_t *ld, int y)
static inline int ld_update(spll_lock_det_t * ld, int y)
{
if (abs(y) <= ld->threshold)
{
if(ld->lock_cnt < ld->lock_samples)
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->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;
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)
static void ld_init(spll_lock_det_t * ld)
{
ld->locked = 0;
ld->lock_cnt = 0;
ld->locked = 0;
ld->lock_cnt = 0;
}
static void lowpass_init(spll_lowpass_t *lp, int alpha)
static void lowpass_init(spll_lowpass_t * lp, int alpha)
{
lp->y_d = 0x80000000;
lp->alpha = alpha;
}
static int lowpass_update(spll_lowpass_t *lp, int x)
static int lowpass_update(spll_lowpass_t * lp, int x)
{
if(lp->y_d == 0x80000000)
{
if (lp->y_d == 0x80000000) {
lp->y_d = x;
return x;
} else {
......@@ -138,7 +132,6 @@ static int lowpass_update(spll_lowpass_t *lp, int x)
}
}
/* Enables/disables DDMTD tag generation on a given (channel).
Channels (0 ... n_chan_ref - 1) are the reference channels (e.g. transceivers' RX clocks
......@@ -153,35 +146,34 @@ Channels (n_chan_ref ... n_chan_out + n_chan_ref-1) are the output channels (loc
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);
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;
SPLL->OCER &= ~(1 << (channel - n_chan_ref));
} else { /* Reference channel */
if (enable)
SPLL->RCER |= 1 << channel;
else
SPLL->RCER &= ~ (1<<channel);
SPLL->RCER &= ~(1 << channel);
}
// TRACE("%s: ch %d, OCER 0x%x, RCER 0x%x\n", __FUNCTION__, channel, SPLL->OCER, SPLL->RCER);
// TRACE("%s: ch %d, OCER 0x%x, RCER 0x%x\n", __FUNCTION__, channel, SPLL->OCER, SPLL->RCER);
}
static void spll_resync_dmtd_counter(int channel)
{
if(channel >= n_chan_ref) /* Output channel? */
SPLL->CRR_OUT = 1<< (channel - n_chan_ref);
else
SPLL->CRR_IN = 1<< channel;
if (channel >= n_chan_ref) /* Output channel? */
SPLL->CRR_OUT = 1 << (channel - n_chan_ref);
else
SPLL->CRR_IN = 1 << channel;
}
static int spll_check_dmtd_resync(int channel)
{
if(channel >= n_chan_ref) /* Output channel? */
return (SPLL->CRR_OUT & (1<< (channel - n_chan_ref))) ? 1 : 0;
else
return (SPLL->CRR_IN & (1<< channel)) ? 1 :0;
}
\ No newline at end of file
if (channel >= n_chan_ref) /* Output channel? */
return (SPLL->CRR_OUT & (1 << (channel - n_chan_ref))) ? 1 : 0;
else
return (SPLL->CRR_IN & (1 << channel)) ? 1 : 0;
}
......@@ -26,13 +26,12 @@ 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_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_HELPER 0x20 /* Sample source: Helper PLL */
#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 */
/* Writes a parameter to the debug FIFO.
......@@ -46,5 +45,6 @@ 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);
SPLL->DFR_SPLL =
(last ? 0x80000000 : 0) | (value & 0xffffff) | (what << 24);
}
......@@ -19,7 +19,6 @@ External PPS ___________|^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
#define REALIGN_STAGE1 1
#define REALIGN_STAGE1_WAIT 2
/* 2nd alignment stage, done after the ext channel PLL has locked. We make sure that the switch's internal PPS signal
is produced exactly on the edge of PLL reference in-phase with 10 MHz clock edge, which has come right after the PPS input
......@@ -42,7 +41,6 @@ Internal PPS __________________________________|^^^^^^^^^|________
/* Realignment done */
#define REALIGN_DONE 7
struct spll_external_state {
int ref_src;
int sample_n;
......@@ -50,17 +48,18 @@ struct spll_external_state {
int realign_clocks;
int realign_state;
int realign_timer;
spll_pi_t pi;
spll_lowpass_t lp_short, lp_long;
spll_lock_det_t ld;
spll_pi_t pi;
spll_lowpass_t lp_short, lp_long;
spll_lock_det_t ld;
};
static void external_init(volatile struct spll_external_state *s, int ext_ref, int realign_clocks)
static void external_init(volatile struct spll_external_state *s, int ext_ref,
int realign_clocks)
{
s->pi.y_min = 5;
s->pi.y_max = (1 << DAC_BITS) - 5;
s->pi.kp = (int)(300);
s->pi.kp = (int)(300);
s->pi.ki = (int)(1);
s->pi.anti_windup = 1;
......@@ -78,88 +77,81 @@ static void external_init(volatile struct spll_external_state *s, int ext_ref, i
s->realign_clocks = realign_clocks;
s->realign_state = (realign_clocks ? REALIGN_STAGE1 : REALIGN_DISABLED);
pi_init((spll_pi_t *) &s->pi);
ld_init((spll_lock_det_t *) &s->ld);
lowpass_init((spll_lowpass_t *) &s->lp_short, 4000);
lowpass_init((spll_lowpass_t *) &s->lp_long, 300);
pi_init((spll_pi_t *) & s->pi);
ld_init((spll_lock_det_t *) & s->ld);
lowpass_init((spll_lowpass_t *) & s->lp_short, 4000);
lowpass_init((spll_lowpass_t *) & s->lp_long, 300);
}
static inline void realign_fsm( struct spll_external_state *s)
static inline void realign_fsm(struct spll_external_state *s)
{
switch(s->realign_state)
{
case REALIGN_STAGE1:
SPLL->ECCR |= SPLL_ECCR_ALIGN_EN;
switch (s->realign_state) {
case REALIGN_STAGE1:
SPLL->ECCR |= SPLL_ECCR_ALIGN_EN;
s->realign_state = REALIGN_STAGE1_WAIT;
s->realign_timer = timer_get_tics();
break;
case REALIGN_STAGE1_WAIT:
if (SPLL->ECCR & SPLL_ECCR_ALIGN_DONE)
s->realign_state = REALIGN_STAGE2;
else if (timer_get_tics() - s->realign_timer >
2 * TICS_PER_SECOND) {
SPLL->ECCR &= ~SPLL_ECCR_ALIGN_EN;
s->realign_state = REALIGN_PPS_INVALID;
}
break;
s->realign_state = REALIGN_STAGE1_WAIT;
case REALIGN_STAGE2:
if (s->ld.locked) {
PPSG->CR = PPSG_CR_CNT_RST | PPSG_CR_CNT_EN;
PPSG->ADJ_UTCLO = 0;
PPSG->ADJ_UTCHI = 0;
PPSG->ADJ_NSEC = 0;
PPSG->ESCR = PPSG_ESCR_SYNC;
s->realign_state = REALIGN_STAGE2_WAIT;
s->realign_timer = timer_get_tics();
break;
case REALIGN_STAGE1_WAIT:
if(SPLL->ECCR & SPLL_ECCR_ALIGN_DONE)
s->realign_state = REALIGN_STAGE2;
else if (timer_get_tics() - s->realign_timer > 2*TICS_PER_SECOND)
{
SPLL->ECCR &= ~SPLL_ECCR_ALIGN_EN;
s->realign_state = REALIGN_PPS_INVALID;
}
break;
case REALIGN_STAGE2:
if(s->ld.locked)
{
PPSG->CR = PPSG_CR_CNT_RST | PPSG_CR_CNT_EN;
PPSG->ADJ_UTCLO = 0;
PPSG->ADJ_UTCHI = 0;
PPSG->ADJ_NSEC = 0;
PPSG->ESCR = PPSG_ESCR_SYNC;
s->realign_state = REALIGN_STAGE2_WAIT;
s->realign_timer = timer_get_tics();
}
break;
case REALIGN_STAGE2_WAIT:
if(PPSG->ESCR & PPSG_ESCR_SYNC)
{
PPSG->ESCR = PPSG_ESCR_PPS_VALID | PPSG_ESCR_TM_VALID;
s->realign_state = REALIGN_DONE;
} else if (timer_get_tics() - s->realign_timer > 2*TICS_PER_SECOND)
{
PPSG->ESCR = 0;
s->realign_state = REALIGN_PPS_INVALID;
}
break;
case REALIGN_PPS_INVALID:
case REALIGN_DISABLED:
case REALIGN_DONE:
return ;
}
break;
case REALIGN_STAGE2_WAIT:
if (PPSG->ESCR & PPSG_ESCR_SYNC) {
PPSG->ESCR = PPSG_ESCR_PPS_VALID | PPSG_ESCR_TM_VALID;
s->realign_state = REALIGN_DONE;
} else if (timer_get_tics() - s->realign_timer >
2 * TICS_PER_SECOND) {
PPSG->ESCR = 0;
s->realign_state = REALIGN_PPS_INVALID;
}
break;
case REALIGN_PPS_INVALID:
case REALIGN_DISABLED:
case REALIGN_DONE:
return;
}
}
static int external_update( struct spll_external_state *s, int tag, int source)
static int external_update(struct spll_external_state *s, int tag, int source)
{
int err, y, y2, ylt;
if(source == s->ref_src)
{
int wrap = tag & (1<<BB_ERROR_BITS) ? 1 : 0;
if (source == s->ref_src) {
int wrap = tag & (1 << BB_ERROR_BITS) ? 1 : 0;
realign_fsm(s);
tag &= ((1<<BB_ERROR_BITS) - 1);
tag &= ((1 << BB_ERROR_BITS) - 1);
// mprintf("err %d\n", tag);
if(wrap)
{
if(tag > s->ph_raw_d0)
s->ph_err_offset -= (1<<BB_ERROR_BITS);
else if(tag <= s->ph_raw_d0)
s->ph_err_offset += (1<<BB_ERROR_BITS);
// mprintf("err %d\n", tag);
if (wrap) {
if (tag > s->ph_raw_d0)
s->ph_err_offset -= (1 << BB_ERROR_BITS);
else if (tag <= s->ph_raw_d0)
s->ph_err_offset += (1 << BB_ERROR_BITS);
}
s->ph_raw_d0 = tag;
......@@ -169,11 +161,10 @@ static int external_update( struct spll_external_state *s, int tag, int source)
y = pi_update(&s->pi, err);
y2 = lowpass_update(&s->lp_short, y);
ylt = lowpass_update(&s->lp_long, y);
y2 = lowpass_update(&s->lp_short, y);
ylt = lowpass_update(&s->lp_long, y);
if(! (SPLL->ECCR & SPLL_ECCR_EXT_REF_PRESENT)) /* no reference? de-lock now */
{
if (!(SPLL->ECCR & SPLL_ECCR_EXT_REF_PRESENT)) { /* no reference? de-lock now */
ld_init(&s->ld);
y2 = 32000;
}
......@@ -184,28 +175,29 @@ static int external_update( struct spll_external_state *s, int tag, int source)
spll_debug(DBG_SAMPLE_ID | DBG_EXT, s->sample_n++, 0);
spll_debug(DBG_Y | DBG_EXT, y2, 1);
if(ld_update(&s->ld, y2 - ylt))
if (ld_update(&s->ld, y2 - ylt))
return SPLL_LOCKED;
}
}
return SPLL_LOCKING;
}
static void external_start( struct spll_external_state *s)
static void external_start(struct spll_external_state *s)
{
// mprintf("ExtStartup\n");
// mprintf("ExtStartup\n");
SPLL->ECCR = 0;
s->sample_n = 0;
s->realign_state = (s->realign_clocks ? REALIGN_STAGE1 : REALIGN_DISABLED);
s->realign_state =
(s->realign_clocks ? REALIGN_STAGE1 : REALIGN_DISABLED);
SPLL->ECCR = SPLL_ECCR_EXT_EN;
spll_debug(DBG_EVENT | DBG_EXT, DBG_EVT_START, 1);
spll_debug(DBG_EVENT | DBG_EXT, DBG_EVT_START, 1);
}
static inline int external_locked( struct spll_external_state *s)
static inline int external_locked(struct spll_external_state *s)
{
return (s->ld.locked && (s->realign_clocks ? s->realign_state == REALIGN_DONE : 1));
return (s->ld.locked
&& (s->realign_clocks ? s->realign_state == REALIGN_DONE : 1));
}
......@@ -3,7 +3,7 @@
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
......@@ -13,26 +13,26 @@
#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;
int delock_count;
spll_pi_t pi;
spll_lock_det_t ld;
int p_adder; /* anti wrap-around adder */
int p_setpoint, tag_d0;
int ref_src;
int sample_n;
int delock_count;
spll_pi_t pi;
spll_lock_det_t ld;
};
static void helper_init(volatile 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);// / 2;
s->pi.ki = (int)(0.03 * 32.0 * 3.0);// / 2;
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 = 10000;
......@@ -41,73 +41,70 @@ static void helper_init(volatile struct spll_helper_state *s, int ref_channel)
s->delock_count = 0;
}
static int helper_update(volatile struct spll_helper_state *s, int tag, int source)
static int helper_update(volatile struct spll_helper_state *s, int tag,
int source)
{
int err, y;
if(source == s->ref_src)
{
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)
{
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);
if (s->tag_d0 > tag)
s->p_adder += (1 << TAG_BITS);
err = (tag + s->p_adder) - s->p_setpoint;
if(HELPER_ERROR_CLAMP)
{
if(err < -HELPER_ERROR_CLAMP) err = -HELPER_ERROR_CLAMP;
if(err > HELPER_ERROR_CLAMP) err = HELPER_ERROR_CLAMP;
}
if((tag + s->p_adder) > HELPER_TAG_WRAPAROUND && s->p_setpoint > HELPER_TAG_WRAPAROUND)
{
if (HELPER_ERROR_CLAMP) {
if (err < -HELPER_ERROR_CLAMP)
err = -HELPER_ERROR_CLAMP;
if (err > HELPER_ERROR_CLAMP)
err = HELPER_ERROR_CLAMP;
}
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->p_setpoint += (1 << HPLL_N);
s->tag_d0 = tag;
y = pi_update((spll_pi_t *) &s->pi, err);
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_Y | DBG_HELPER, y, 0);
spll_debug(DBG_ERR | DBG_HELPER, err, 1);
if(ld_update((spll_lock_det_t *) &s->ld, err))
if (ld_update((spll_lock_det_t *) & s->ld, err))
return SPLL_LOCKED;
}
return SPLL_LOCKING;
}
static void helper_start(volatile struct spll_helper_state *s)
{
/* Set the bias to the upper end of tuning range. This is to ensure that
the HPLL will always lock on positive frequency offset. */
/* 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;
s->p_setpoint = 0;
s->p_setpoint = 0;
s->p_adder = 0;
s->sample_n = 0;
s->tag_d0 = -1;
pi_init((spll_pi_t *) &s->pi);
ld_init((spll_lock_det_t *)&s->ld);
pi_init((spll_pi_t *) & s->pi);
ld_init((spll_lock_det_t *) & s->ld);
spll_enable_tagger(s->ref_src, 1);
spll_debug(DBG_EVENT | DBG_HELPER, DBG_EVT_START, 1);
spll_debug(DBG_EVENT | DBG_HELPER, DBG_EVT_START, 1);
}
......@@ -11,8 +11,8 @@
struct spll_main_state {
int state;
spll_pi_t pi;
spll_lock_det_t ld;
spll_pi_t pi;
spll_lock_det_t ld;
int adder_ref, adder_out, tag_ref, tag_out, tag_ref_d, tag_out_d;
......@@ -21,24 +21,24 @@ struct spll_main_state {
int match_state;
int match_seq;
int phase_shift_target;
int phase_shift_current;
int id_ref, id_out; /* IDs of the reference and the output channel */
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 delock_count;
int dac_index;
};
static void mpll_init(volatile struct spll_main_state *s, int id_ref, int id_out)
static void mpll_init(volatile 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;// / 2;
s->pi.ki = 30;// / 2;
s->pi.kp = 1100; // / 2;
s->pi.ki = 30; // / 2;
s->delock_count = 0;
/* Freqency branch lock detection */
......@@ -48,9 +48,9 @@ static void mpll_init(volatile struct spll_main_state *s, int id_ref, int id_out
s->id_ref = id_ref;
s->id_out = id_out;
s->dac_index = id_out - n_chan_ref;
pi_init((spll_pi_t *) &s->pi);
ld_init((spll_lock_det_t *) &s->ld);
pi_init((spll_pi_t *) & s->pi);
ld_init((spll_lock_det_t *) & s->ld);
}
static void mpll_start(volatile struct spll_main_state *s)
......@@ -66,10 +66,10 @@ static void mpll_start(volatile struct spll_main_state *s)
s->phase_shift_target = 0;
s->phase_shift_current = 0;
s->sample_n= 0;
s->sample_n = 0;
pi_init((spll_pi_t *) &s->pi);
ld_init((spll_lock_det_t *) &s->ld);
pi_init((spll_pi_t *) & s->pi);
ld_init((spll_lock_det_t *) & s->ld);
spll_enable_tagger(s->id_ref, 1);
spll_enable_tagger(s->id_out, 1);
......@@ -81,97 +81,88 @@ static void mpll_stop(volatile struct spll_main_state *s)
spll_enable_tagger(s->id_out, 0);
}
static int mpll_update(volatile struct spll_main_state *s, int tag, int source)
{
int err, y;
#ifdef WITH_SEQUENCING
int new_ref = -1, new_out = -1;
if(source == s->id_ref)
{
new_ref = tag;
s->seq_ref++;
} else if(source == s->id_out) {
new_out = tag;
s->seq_out++;
}
switch(s->match_state)
{
case MATCH_NEXT_TAG:
if(new_ref > 0 && s->seq_out < s->seq_ref)
{
s->tag_ref = new_ref;
s->match_seq = s->seq_ref;
s->match_state = MATCH_WAIT_OUT;
}
if (new_out > 0 && s->seq_out > s->seq_ref)
{
s->tag_out = new_out;
s->match_seq = s->seq_out;
s->match_state = MATCH_WAIT_REF;
}
break;
case MATCH_WAIT_REF:
if(new_ref > 0 && s->seq_ref == s->match_seq)
{
s->match_state = MATCH_NEXT_TAG;
s->tag_ref = new_ref;
}
break;
case MATCH_WAIT_OUT:
if(new_out > 0 && s->seq_out == s->match_seq)
{
s->match_state = MATCH_NEXT_TAG;
s->tag_out = new_out;
}
break;
}
int new_ref = -1, new_out = -1;
if (source == s->id_ref) {
new_ref = tag;
s->seq_ref++;
} else if (source == s->id_out) {
new_out = tag;
s->seq_out++;
}
switch (s->match_state) {
case MATCH_NEXT_TAG:
if (new_ref > 0 && s->seq_out < s->seq_ref) {
s->tag_ref = new_ref;
s->match_seq = s->seq_ref;
s->match_state = MATCH_WAIT_OUT;
}
if (new_out > 0 && s->seq_out > s->seq_ref) {
s->tag_out = new_out;
s->match_seq = s->seq_out;
s->match_state = MATCH_WAIT_REF;
}
break;
case MATCH_WAIT_REF:
if (new_ref > 0 && s->seq_ref == s->match_seq) {
s->match_state = MATCH_NEXT_TAG;
s->tag_ref = new_ref;
}
break;
case MATCH_WAIT_OUT:
if (new_out > 0 && s->seq_out == s->match_seq) {
s->match_state = MATCH_NEXT_TAG;
s->tag_out = new_out;
}
break;
}
#else
if(source == s->id_ref)
if (source == s->id_ref)
s->tag_ref = tag;
if(source == s->id_out)
if (source == s->id_out)
s->tag_out = tag;
#endif
if(s->tag_ref >= 0 && s->tag_out >= 0)
{
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);
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);
s->tag_ref_d = s->tag_ref;
s->tag_out_d = s->tag_out;
err = s->adder_ref + s->tag_ref - s->adder_out - s->tag_out;
err = s->adder_ref + s->tag_ref - s->adder_out - s->tag_out;
#ifndef WITH_SEQUENCING
/* 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.
/* 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);
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
y = pi_update((spll_pi_t *) &s->pi, err);
SPLL->DAC_MAIN = SPLL_DAC_MAIN_VALUE_W(y) | SPLL_DAC_MAIN_DAC_SEL_W(s->dac_index);
y = pi_update((spll_pi_t *) & s->pi, err);
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);
......@@ -182,32 +173,32 @@ static int mpll_update(volatile 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->adder_ref > 2 * MPLL_TAG_WRAPAROUND
&& s->adder_out > 2 * MPLL_TAG_WRAPAROUND) {
s->adder_ref -= MPLL_TAG_WRAPAROUND;
s->adder_out -= MPLL_TAG_WRAPAROUND;
}
if(s->ld.locked)
{
if(s->phase_shift_current < s->phase_shift_target)
{
s->phase_shift_current++;
s->adder_ref++;
} else if(s->phase_shift_current > s->phase_shift_target) {
s->phase_shift_current--;
s->adder_ref--;
}
}
if(ld_update((spll_lock_det_t *) &s->ld, err))
return SPLL_LOCKED;
}
if (s->ld.locked) {
if (s->phase_shift_current < s->phase_shift_target) {
s->phase_shift_current++;
s->adder_ref++;
} else if (s->phase_shift_current >
s->phase_shift_target) {
s->phase_shift_current--;
s->adder_ref--;
}
}
if (ld_update((spll_lock_det_t *) & s->ld, err))
return SPLL_LOCKED;
}
return SPLL_LOCKING;
}
static int mpll_set_phase_shift(volatile struct spll_main_state *s, int desired_shift)
static int mpll_set_phase_shift(volatile struct spll_main_state *s,
int desired_shift)
{
s->phase_shift_target = desired_shift;
return 0;
......
......@@ -10,8 +10,8 @@ struct spll_ptracker_state {
int preserve_sign;
};
static void ptracker_init(volatile struct spll_ptracker_state *s, int id_a, int id_b, int num_avgs)
static void ptracker_init(volatile struct spll_ptracker_state *s, int id_a,
int id_b, int num_avgs)
{
s->tag_a = s->tag_b = -1;
......@@ -21,7 +21,7 @@ static void ptracker_init(volatile struct spll_ptracker_state *s, int id_a, int
s->n_avg = num_avgs;
s->acc = 0;
s->avg_count = 0;
s->sample_n= 0;
s->sample_n = 0;
s->preserve_sign = 0;
}
......@@ -31,35 +31,34 @@ static void ptracker_start(volatile struct spll_ptracker_state *s)
s->ready = 0;
s->acc = 0;
s->avg_count = 0;
s->sample_n= 0;
s->preserve_sign = 0;
s->sample_n = 0;
s->preserve_sign = 0;
spll_resync_dmtd_counter(s->id_b);
spll_enable_tagger(s->id_a, 1);
spll_enable_tagger(s->id_b, 1);
spll_enable_tagger(s->id_a, 1);
spll_enable_tagger(s->id_b, 1);
}
#define PTRACK_WRAP_LO (1<<(HPLL_N-2))
#define PTRACK_WRAP_HI (3*(1<<(HPLL_N-2)))
static int ptracker_update(volatile struct spll_ptracker_state *s, int tag, int source)
static int ptracker_update(volatile struct spll_ptracker_state *s, int tag,
int source)
{
if(source == s->id_a)
if (source == s->id_a)
s->tag_a = tag;
if(source == s->id_b)
if (source == s->id_b)
s->tag_b = tag;
if(s->tag_a >= 0 && s->tag_b >= 0)
{
int delta = (s->tag_a - s->tag_b) & ((1<<HPLL_N) - 1);
if (s->tag_a >= 0 && s->tag_b >= 0) {
int delta = (s->tag_a - s->tag_b) & ((1 << HPLL_N) - 1);
s->sample_n++;
if(s->avg_count == 0)
{
if (s->avg_count == 0) {
if(delta <= PTRACK_WRAP_LO)
if (delta <= PTRACK_WRAP_LO)
s->preserve_sign = -1;
else if (delta >= PTRACK_WRAP_HI)
s->preserve_sign = 1;
......@@ -70,17 +69,17 @@ static int ptracker_update(volatile struct spll_ptracker_state *s, int tag, int
s->acc = delta;
} else {
if(delta <= PTRACK_WRAP_LO && s->preserve_sign > 0)
s->acc += delta + (1<<HPLL_N);
else if (delta >= PTRACK_WRAP_HI && s->preserve_sign < 0)
s->acc += delta - (1<<HPLL_N);
if (delta <= PTRACK_WRAP_LO && s->preserve_sign > 0)
s->acc += delta + (1 << HPLL_N);
else if (delta >= PTRACK_WRAP_HI
&& s->preserve_sign < 0)
s->acc += delta - (1 << HPLL_N);
else
s->acc += delta;
s->avg_count++;
if(s->avg_count == s->n_avg)
{
if (s->avg_count == s->n_avg) {
s->phase_val = s->acc / s->n_avg;
s->ready = 1;
s->acc = 0;
......@@ -90,7 +89,7 @@ static int ptracker_update(volatile struct spll_ptracker_state *s, int tag, int
}
s->tag_b = s->tag_a = -1;
}
}
return SPLL_LOCKING;
}
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