Skip to content

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

removed obsolete 1-st gen softpll files

parent aac466f0
Hide whitespace changes
Inline Side-by-side
Showing with 0 additions and 519 deletions
dev/softpll.c deleted 100644 → 0
View file @ aac466f0
/*
White Rabbit Softcore PLL (SoftPLL)
*/
#include <stdio.h>
#include <stdlib.h>
#include "board.h"
#include "irq.h"
#include <hw/softpll_regs.h>
#include "softpll.h"
/* Bit size of phase tags generated by the DMTDs. Used to sign-extend the tags. */
#define TAG_BITS 20
/* Helper PLL N divider (1/2**N is the frequency offset) */
#define HPLL_N 14
/* Fractional bits in PI controller coefficients */
#define PI_FRACBITS 12
/* Channel enables in the CSR register: When the corresponding bit is set,
the corresponding channel is enabled */
#define CHAN_FB 8 /* Main clock feedback (clk_ref_i) */
#define CHAN_REF 4 /* Reference clock (clk_rx_i) */
#define CHAN_PERIOD 2 /* (clk_rx_i - clk_dmtd_i) period error */
#define CHAN_AUX 1 /* Auxillary core clock (clk_aux_i) */
/* CSR reg "tag ready" flags: When the corresponding bit is set,
the corresponding TAG_xxx register constains a fresh value of phase/frequency tag */
#define READY_FB (8<<4)
#define READY_REF (4<<4)
#define READY_PERIOD (2<<4)
#define READY_AUX (1<<4)
/* number of fractional cycles in the phase setpoint, used to control the speed of the phase shifter
(which affects the stability of the PLL) */
#define SETPOINT_FRACBITS 1
static volatile struct SPLL_WB *SPLL = (volatile struct SPLL_WB *) BASE_SOFTPLL;
/* 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 claming and antiwindup algorithms */
int y_max;
int x,y; /* Current input and output value */
} spll_pi_t;
/* Processes a single sample (x) using PI controller (pi). Returns the value (y) which should
be used 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 integretor 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
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 */
typedef struct {
int lock_cnt;
int lock_samples;
int delock_samples;
int threshold;
int locked;
} spll_lock_det_t;
/* 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. */
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;
} 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 ld->locked;
}
static void ld_init(spll_lock_det_t *ld)
{
ld->locked = 0;
ld->lock_cnt = 0;
}
/* 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 */
struct spll_helper_state {
spll_pi_t pi_freq, pi_phase;
spll_lock_det_t ld_freq, ld_phase;
int f_setpoint;
int p_setpoint;
int freq_mode;
};
struct spll_dmpll_state {
spll_pi_t pi_freq, pi_phase;
spll_lock_det_t ld_freq, ld_phase;
int setpoint;
int freq_mode;
int tag_ref_d0;
int tag_fb_d0;
int period_ref;
int period_fb;
int phase_shift;
};
void helper_init(struct spll_helper_state *s)
{
/* Frequency branch PI controller */
s->pi_freq.y_min = 5;
s->pi_freq.y_max = 65530;
s->pi_freq.anti_windup = 0;
s->pi_freq.kp = 28*32*16;
s->pi_freq.ki = 50*32*16;
s->pi_freq.bias = 32000;
/* Freqency branch lock detection */
s->ld_freq.threshold = 2;
s->ld_freq.lock_samples = 1000;
s->ld_freq.delock_samples = 990;
/* Phase branch PI controller */
s->pi_phase.y_min = 5;
s->pi_phase.y_max = 65530;
s->pi_phase.kp = (int)(2.0 * 32.0 * 16.0);
s->pi_phase.ki = (int)(0.05 * 32.0 * 3.0);
s->pi_phase.anti_windup = 0;
s->pi_phase.bias = 32000;
/* Phase branch lock detection */
s->ld_phase.threshold = 500;
s->ld_phase.lock_samples = 10000;
s->ld_phase.delock_samples = 9900;
s->freq_mode = 1;
s->f_setpoint = 16;
pi_init(&s->pi_freq);
ld_init(&s->ld_freq);
pi_init(&s->pi_phase);
ld_init(&s->ld_phase);
}
void main_init(struct spll_dmpll_state *s)
{
/* Frequency branch PI controller */
s->pi_freq.y_min = 5;
s->pi_freq.y_max = 65530;
s->pi_freq.anti_windup = 0;
s->pi_freq.kp = 100;
s->pi_freq.ki = 600;
s->pi_freq.bias = 32000;
/* Freqency branch lock detection */
s->ld_freq.threshold = 500;
s->ld_freq.lock_samples = 1000;
s->ld_freq.delock_samples = 990;
/* Phase branch PI controller */
s->pi_phase.y_min = 5;
s->pi_phase.y_max = 65530;
s->pi_phase.kp = 1304 / 2;
s->pi_phase.ki = 10 * 3;
s->pi_phase.anti_windup = 0;
s->pi_phase.bias = 32000;
/* Phase branch lock detection */
s->ld_phase.threshold = 500;
s->ld_phase.lock_samples = 1000;
s->ld_phase.delock_samples = 990;
s->freq_mode = 1;
s->setpoint=0;
s->phase_shift=0;
s->tag_ref_d0 = -1;
s->tag_fb_d0 = -1;
s->period_ref = 0 ;
s->period_fb = 0 ;
pi_init(&s->pi_freq);
ld_init(&s->ld_freq);
pi_init(&s->pi_phase);
ld_init(&s->ld_phase);
}
static struct spll_helper_state helper;
static struct spll_dmpll_state main, auxpll;
static volatile int irq_cnt = 0;
#define HELPER_PERIOD_BITS 8
static inline void helper_irq(struct spll_helper_state *hpll, int tag_ref)
{
int err, y;
/* HPLL: active frequency branch */
if(hpll->freq_mode)
{
err = SPLL->PER_HPLL;
if (err & (1<<HELPER_PERIOD_BITS))
err |= ~ ((1<<HELPER_PERIOD_BITS) - 1); /* sign-extend the freq error */
err += hpll->f_setpoint;
y = pi_update(&hpll->pi_freq, err);
SPLL->DAC_HPLL = y;
if(ld_update(&hpll->ld_freq, err))
{
hpll->freq_mode = 0;
hpll->p_setpoint = -1;
hpll->pi_phase.bias = y;
pi_init(&hpll->pi_phase);
ld_init(&hpll->ld_phase);
SPLL->CSR = SPLL_CSR_TAG_EN_W(CHAN_REF);
}
} else if (tag_ref >= 0) { /* HPLL: active phase branch */
if(hpll->p_setpoint < 0)
{
hpll->p_setpoint = tag_ref;
return;
}
err = tag_ref - hpll->p_setpoint;
hpll->p_setpoint += 16384;
hpll->p_setpoint &= ((1<<TAG_BITS)-1);
y = pi_update(&hpll->pi_phase, err);
SPLL->DAC_HPLL = y;
if(ld_update(&hpll->ld_phase, err))
{
SPLL->CSR |= SPLL_CSR_TAG_EN_W(CHAN_FB | CHAN_REF);
};
}
}
void dmpll_irq(struct spll_dmpll_state *dmpll, int tag_ref, int tag_fb, volatile uint32_t *dac)
{
int err, tmp, y, fb_ready;
fb_ready = (tag_fb >= 0 ? 1 : 0);
if(dmpll->freq_mode)
{
if(tag_ref >= 0)
{
if(dmpll->tag_ref_d0 >= 0)
{
tmp = tag_ref - dmpll->tag_ref_d0;
if(tmp < 0)
tmp += (1<<TAG_BITS)-1;
dmpll->period_ref = tmp;
}
dmpll->tag_ref_d0 = tag_ref;
}
if(tag_fb >= 0)
{
if(dmpll->tag_fb_d0 > 0)
{
tmp = tag_fb - dmpll->tag_fb_d0;
if(tmp < 0)
tmp += (1<<TAG_BITS)-1;
dmpll->period_fb = tmp;
}
dmpll->tag_fb_d0 = tag_fb;
}
err = dmpll->period_ref - dmpll->period_fb;
y = pi_update(&dmpll->pi_freq, err);
*dac = y;
if(ld_update(&dmpll->ld_freq, err))
{
/* frequency has been stable for quite a while? switch to phase branch */
dmpll->freq_mode = 0;
dmpll->pi_phase.bias = y;//dmpll->pi_freq.bias;
pi_init(&dmpll->pi_phase);
ld_init(&dmpll->ld_phase);
dmpll->setpoint = 0;
dmpll->phase_shift = 0;
}
} else {
if(tag_ref >= 0)
dmpll->tag_ref_d0 = tag_ref;
tag_ref = dmpll->tag_ref_d0 ;
tag_fb += (dmpll->setpoint >> SETPOINT_FRACBITS);
tag_fb &= (1<<TAG_BITS) - 1;
if(fb_ready)
{
tag_ref &= 0x3fff; //while (tag_ref > 16384 ) tag_ref-=16384; /* fixme */
tag_fb &= 0x3fff; //while (tag_fb > 16384 ) tag_fb-=16384;
err = tag_ref - tag_fb;
y = pi_update(&dmpll->pi_phase, err);
*dac = y;
ld_update(&dmpll->ld_phase, err);
if(dmpll->setpoint < dmpll->phase_shift)
dmpll->setpoint++;
else if(dmpll->setpoint > dmpll->phase_shift)
dmpll->setpoint--;
}
}
}
int aux_en = 0;
void _irq_entry()
{
volatile uint32_t csr;
int tag_ref = -1;
int tag_fb = -1;
int tag_aux = -1;
irq_cnt ++;
csr = SPLL->CSR;
if(csr & READY_REF)
tag_ref = SPLL->TAG_REF;
if(csr & READY_FB)
tag_fb = SPLL->TAG_FB;
if(csr & READY_AUX)
tag_aux = SPLL->TAG_AUX;
helper_irq(&helper, tag_ref);
if(helper.ld_phase.locked)
{
dmpll_irq(&main, tag_ref, tag_fb, &SPLL->DAC_DMPLL);
/* Aux channel - i.e. the oscillator on the FMC card */
if((csr & SPLL_CSR_AUX_EN) )
{
int shifting_done = 0;
if(!(csr & CHAN_AUX))
{
auxpll.ld_phase.locked = 0;
auxpll.phase_shift = main.phase_shift;
auxpll.setpoint =0;
auxpll.freq_mode = 1;
pi_init(&auxpll.pi_freq);
ld_init(&auxpll.ld_freq);
//aux_en = 1;
}
dmpll_irq(&auxpll, tag_ref, tag_aux, &SPLL->DAC_AUX);
shifting_done = abs(auxpll.setpoint - auxpll.phase_shift) < 500 ? 1: 0;
SPLL->CSR = SPLL_CSR_TAG_EN_W(CHAN_FB | CHAN_REF | CHAN_AUX) | ((shifting_done && auxpll.ld_phase.locked) ? SPLL_CSR_AUX_LOCK : 0);
} else {
auxpll.ld_phase.locked = 0;
auxpll.phase_shift = main.phase_shift;
auxpll.setpoint =0;
SPLL->CSR = SPLL_CSR_TAG_EN_W(CHAN_FB | CHAN_REF);
// aux_en = 0;
}
}
clear_irq();
}
static int prev_lck = 0;
void softpll_enable()
{
SPLL->CSR = 0;
disable_irq();
helper_init(&helper);
main_init(&main);
main_init(&auxpll);
SPLL->DAC_HPLL = 0;
SPLL->DEGLITCH_THR = 3000;
SPLL->CSR = SPLL_CSR_TAG_EN_W(CHAN_PERIOD);
SPLL->EIC_IER = 1;
enable_irq();
TRACE_DEV("*********************** [softpll]: enabled\n");
// softpll_check_lock();
prev_lck = 0;
}
int softpll_check_lock()
{
static int prev_lck = 0;
int lck = !helper.freq_mode && helper.ld_phase.locked && !main.freq_mode && main.ld_phase.locked;
// if(!lck)
TRACE_DEV("%d %d%d%d%d%d%d %d %d %d\n", irq_cnt, helper.freq_mode, helper.ld_phase.locked, main.freq_mode, main.ld_phase.locked, auxpll.freq_mode, auxpll.ld_phase.locked, main.pi_freq.y, main.pi_phase.y, main.pi_phase.x);
irq_cnt = 0;
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 ((SPLL->CSR & SPLL_CSR_AUX_EN) ? SOFTPLL_AUX_ENABLED: 0) | (auxpll.ld_phase.locked ? SOFTPLL_AUX_LOCKED: 0);
}
int softpll_busy(int channel)
{
// mprintf("busy?");
return main.setpoint != main.phase_shift;
}
void softpll_set_phase( int ps)
{
main.phase_shift = (-(int32_t) ((int64_t)ps * 16384LL / 8000LL)) << SETPOINT_FRACBITS;
auxpll.phase_shift = (-(int32_t) ((int64_t)ps * 16384LL / 8000LL)) << SETPOINT_FRACBITS;
}
void softpll_disable()
{
SPLL->CSR = 0;
disable_irq();
}
int softpll_get_setpoint()
{
// return pstate.d_p_setpoint;
}
include/softpll.h deleted 100644 → 0
View file @ aac466f0
#ifndef __SOFTPLL_H
#define __SOFTPLL_H
#define SOFTPLL_AUX_ENABLED 1
#define SOFTPLL_AUX_LOCKED 2
void softpll_enable();
int softpll_check_lock();
void softpll_disable();
int softpll_busy();
void softpll_set_phase(int ps);
int softpll_get_setpoint();
int softpll_get_aux_status();
#endif
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