Skip to content

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

softpll: 2nd gen, working helper

parent 0f4fb1ca
Show whitespace changes
Inline Side-by-side
Showing with 220 additions and 200 deletions
dev/softpll.c
View file @ 4783fba2
#include <stdio.h>
#include <stdlib.h>
#include "board.h" #include "board.h"
#include "irq.h" #include "irq.h"
...@@ -24,6 +27,108 @@ ...@@ -24,6 +27,108 @@
static volatile struct SPLL_WB *SPLL = (volatile struct SPLL_WB *) BASE_SOFTPLL; static volatile struct SPLL_WB *SPLL = (volatile struct SPLL_WB *) BASE_SOFTPLL;
typedef struct {
int ki, kp;
int integrator;
int bias;
int anti_windup;
int y_min;
int y_max;
int x,y;
} spll_pi_t;
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;
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;
}
static inline void pi_init(spll_pi_t *pi)
{
pi->integrator = 0;
}
typedef struct {
int lock_cnt;
int lock_samples;
int delock_samples;
int threshold;
int locked;
} spll_lock_det_t;
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;
}
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 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;
};
struct softpll_config { struct softpll_config {
int hpll_f_kp; int hpll_f_kp;
int hpll_f_ki; int hpll_f_ki;
...@@ -55,139 +160,115 @@ struct softpll_config { ...@@ -55,139 +160,115 @@ struct softpll_config {
}; };
const struct softpll_config pll_cfg =
void helper_init(struct spll_helper_state *s)
{ {
/* Helper PLL */
28*32*16, // f_kp
50*32*16, // f_ki
16, // setpoint
2000, // lock detect freq samples
2, // lock detect freq threshold
2.0*32*16, // p_kp
0.05*32*16, // p_ki
1000, // lock detect phase samples
1000, // lock detect phase threshold
2000, // delock threshold
32000, // HPLL dac bias
/* DMTD PLL */
100, // f_kp
600, // f_ki
1304, // p_kp
10, // p_ki
1000, // lock detect samples
500, // lock detect threshold
500, // delock threshold
32000, // DPLL dac bias
2000 // deglitcher threshold
};
struct softpll_state { /* Frequency branch PI controller */
int h_lock_counter; s->pi_freq.y_min = 5;
int h_i; s->pi_freq.y_max = 65530;
int h_dac_bias; s->pi_freq.anti_windup = 0;
int h_p_setpoint; s->pi_freq.kp = 28*32*16;
int h_freq_mode; s->pi_freq.ki = 50*32*16;
int h_locked; s->pi_freq.bias = 32000;
int h_dac_val;
int h_freq_err; /* Freqency branch lock detection */
int h_tag; s->ld_freq.threshold = 2;
s->ld_freq.lock_samples = 1000;
s->ld_freq.delock_samples = 990;
int d_dac_bias;
int d_tag_ref_d0; /* Phase branch PI controller */
int d_tag_fb_d0; s->pi_phase.y_min = 5;
int d_tag_ref_ready; s->pi_phase.y_max = 65530;
int d_tag_fb_ready; s->pi_phase.kp = (int)(2.0 * 32.0 * 4.0);
s->pi_phase.ki = (int)(0.05 * 32.0 * 0.1);
int d_period_ref; s->pi_phase.anti_windup = 0;
int d_period_fb; s->pi_phase.bias = 32000;
int d_freq_mode;
int d_lock_counter; /* Phase branch lock detection */
int d_freq_error; s->ld_phase.threshold = 500;
s->ld_phase.lock_samples = 1000;
int d_i; s->ld_phase.delock_samples = 990;
int d_p_setpoint; s->freq_mode = 1;
int d_phase_shift; s->f_setpoint = 16;
int d_locked;
}; pi_init(&s->pi_freq);
ld_init(&s->ld_freq);
pi_init(&s->pi_phase);
ld_init(&s->ld_phase);
}
int eee, dve; static struct spll_helper_state helper;
static volatile int irq_cnt = 0;
static volatile struct softpll_state pstate; #define HELPER_PERIOD_BITS 8
volatile int irq_cnt = 0; static inline void helper_irq(struct spll_helper_state *hpll, int tag_ref)
volatile int sp_limit = 0xffff;
void _irq_entry()
{ {
int dv; int err, y;
int tag_ref_ready = 0;
int tag_fb_ready = 0;
int tag_ref;
int tag_fb;
// int sp = _get_sp();
irq_cnt ++; /* HPLL: active frequency branch */
if(hpll->freq_mode)
gpio_out(GPIO_PIN_LED_STATUS, 1);
if(SPLL->CSR & READY_REF)
{ {
tag_ref = SPLL->TAG_REF; err = SPLL->PER_HPLL;
tag_ref_ready = 1;
} 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);
if(SPLL->CSR & READY_FB) SPLL->DAC_HPLL = y;
if(ld_update(&hpll->ld_freq, err))
{ {
tag_fb = SPLL->TAG_FB; hpll->freq_mode = 0;
tag_fb_ready = 1; 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 { /* HPLL: active phase branch */
/* HPLL: active frequency branch */ if(hpll->p_setpoint < 0)
if(pstate.h_freq_mode)
{ {
int freq_err = SPLL->PER_HPLL; hpll->p_setpoint = tag_ref;
if(freq_err & 0x100) freq_err |= 0xffffff00; /* sign-extend */ return;
freq_err += pll_cfg.hpll_f_setpoint; }
err = tag_ref - hpll->p_setpoint;
hpll->p_setpoint += 16384;
hpll->p_setpoint &= ((1<<TAG_BITS)-1);
pstate.h_freq_err = freq_err; y = pi_update(&hpll->pi_phase, err);
SPLL->DAC_HPLL = y;
/* PI control */ ld_update(&hpll->ld_phase, err);
if(pstate.h_dac_val > 0 && pstate.h_dac_val < 65530) }
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;
if(dv >= 65530) dv = 65530; void _irq_entry()
if(dv < 0) dv = 0; {
volatile uint32_t csr;
int tag_ref = -1;
int tag_fb = -1;
pstate.h_dac_val = dv; irq_cnt ++;
csr = SPLL->CSR;
SPLL->DAC_HPLL = dv; /* update DAC */ if(csr & READY_REF)
tag_ref = SPLL->TAG_REF;
pstate.h_dac_val = dv; if(csr & READY_FB)
tag_fb = SPLL->TAG_FB;
/* lock detection */ helper_irq(&helper, tag_ref);
if(freq_err >= -pll_cfg.hpll_ld_f_threshold && freq_err <= pll_cfg.hpll_ld_f_threshold)
pstate.h_lock_counter++;
else
pstate.h_lock_counter=0;
/* frequency has been stable for quite a while? switch to phase branch */
if(pstate.h_lock_counter == pll_cfg.hpll_ld_f_samples)
{
pstate.h_freq_mode = 0;
pstate.h_dac_bias = dv;
pstate.h_i = 0;
pstate.h_lock_counter = 0;
pstate.h_p_setpoint = -1;
SPLL->CSR = SPLL_CSR_TAG_EN_W(CHAN_REF);
}
#if 0
/* HPLL: active phase branch */ /* HPLL: active phase branch */
} else if (tag_ref_ready) { } else if (tag_ref_ready) {
if(pstate.h_p_setpoint < 0) /* we don't have yet any phase samples? */ if(pstate.h_p_setpoint < 0) /* we don't have yet any phase samples? */
...@@ -345,6 +426,7 @@ void _irq_entry() ...@@ -345,6 +426,7 @@ void _irq_entry()
} }
gpio_out(GPIO_PIN_LED_STATUS, 0); gpio_out(GPIO_PIN_LED_STATUS, 0);
#endif
clear_irq(); clear_irq();
} }
...@@ -356,57 +438,44 @@ void softpll_enable() ...@@ -356,57 +438,44 @@ void softpll_enable()
SPLL->CSR = 0; SPLL->CSR = 0;
disable_irq(); disable_irq();
SPLL->DAC_HPLL = pll_cfg.hpll_dac_bias; helper_init(&helper);
SPLL->DAC_DMPLL = pll_cfg.dpll_dac_bias;
SPLL->DEGLITCH_THR = pll_cfg.dpll_deglitcher_threshold;
pstate.h_p_setpoint = -1;
pstate.h_i = 0;
pstate.h_freq_mode = 1;
pstate.h_lock_counter = 0;
pstate.h_locked = 0;
pstate.d_p_setpoint = 0;
pstate.d_phase_shift = 0;
pstate.h_dac_val = pll_cfg.hpll_dac_bias;
SPLL->DAC_HPLL = 0;
SPLL->DEGLITCH_THR = 2000;
SPLL->CSR = SPLL_CSR_TAG_EN_W(CHAN_PERIOD);// | SPLL_CSR_TAG_EN_W(CHAN_REF); SPLL->CSR = SPLL_CSR_TAG_EN_W(CHAN_PERIOD);
SPLL->EIC_IER = 1; SPLL->EIC_IER = 1;
enable_irq(); enable_irq();
// softpll_test();
prev_lck = 0;
TRACE_DEV("[softpll]: enabled\n"); TRACE_DEV("[softpll]: enabled\n");
prev_lck = 0;
} }
int softpll_check_lock() int softpll_check_lock()
{ {
TRACE_DEV("[softpll] Helper: lock %d freqmode %d, Main: lock %d freqmode %d\n", TRACE_DEV("[softpll] Helper: lock %d freqmode %d x %d y %d\n", helper.ld_phase.locked, helper.freq_mode, helper.pi_freq.x, helper.pi_phase.y) ;
pstate.h_locked, pstate.h_freq_mode,
pstate.d_locked,pstate.d_freq_mode);
int lck = pstate.h_locked && pstate.d_locked; /*nt lck = pstate.h_locked && pstate.d_locked;
if(lck && !prev_lck) if(lck && !prev_lck)
TRACE_DEV("[softpll]: got lock\n"); TRACE_DEV("[softpll]: got lock\n");
else if (!lck && prev_lck) else if (!lck && prev_lck)
TRACE_DEV("[softpll]: lost lock\n"); TRACE_DEV("[softpll]: lost lock\n");*/
prev_lck = lck; // prev_lck = lck;
return lck; // return lck;
} }
int softpll_busy() int softpll_busy()
{ {
return pstate.d_p_setpoint != pstate.d_phase_shift; // return pstate.d_p_setpoint != pstate.d_phase_shift;
} }
void softpll_set_phase(int ps) void softpll_set_phase(int ps)
{ {
pstate.d_phase_shift = -(int32_t) ((int64_t)ps * 16384LL / 8000LL); // pstate.d_phase_shift = -(int32_t) ((int64_t)ps * 16384LL / 8000LL);
TRACE_DEV("ADJdelta: phase %d [ps], %d units\n", ps, pstate.d_phase_shift); // TRACE_DEV("ADJdelta: phase %d [ps], %d units\n", ps, pstate.d_phase_shift);
} }
void softpll_disable() void softpll_disable()
...@@ -417,5 +486,6 @@ void softpll_disable() ...@@ -417,5 +486,6 @@ void softpll_disable()
int softpll_get_setpoint() int softpll_get_setpoint()
{ {
return pstate.d_p_setpoint; // return pstate.d_p_setpoint;
} }
target/lm32/crt0.S
View file @ 4783fba2
...@@ -89,11 +89,9 @@ ...@@ -89,11 +89,9 @@
.global _start .global _start
_start: _start:
.weak _debug_unit
.global _reset_handler .global _reset_handler
.type _reset_handler, @function .type _reset_handler, @function
.align 256
_debug_unit:
_reset_handler: _reset_handler:
xor r0, r0, r0 xor r0, r0, r0
wcsr IE, r0 wcsr IE, r0
...@@ -105,45 +103,10 @@ _reset_handler: ...@@ -105,45 +103,10 @@ _reset_handler:
nop nop
.size _reset_handler, .-_reset_handler .size _reset_handler, .-_reset_handler
.extern _breakpoint_handler
.global _breakpoint_handler
.type _breakpoint_handler, @function
.align 32
_breakpoint_handler:
bret
.extern _instruction_bus_error_handler
.global _instruction_bus_error_handler
.type _instruction_bus_error_handler, @function
.align 32
_instruction_bus_error_handler:
eret
.extern _watchpoint_handler
.global _watchpoint_handler
.type _watchpoint_handler, @function
.align 32
_watchpoint_handler:
bret
.extern _data_bus_error_handler
.global _data_bus_error_handler
.type _data_bus_error_handler, @function
.align 32
_data_bus_error_handler:
eret
.extern _divide_by_zero_handler
.global _divide_by_zero_handler
.type _divide_by_zero_handler, @function
.align 32
_divide_by_zero_handler:
eret
.extern _irq_entry .extern _irq_entry
.org 0xc0
.global _interrupt_handler .global _interrupt_handler
.type _interrupt_handler, @function .type _interrupt_handler, @function
.align 32
_interrupt_handler: _interrupt_handler:
sw (sp+0), ra sw (sp+0), ra
calli _save_all calli _save_all
...@@ -158,25 +121,12 @@ _interrupt_handler: ...@@ -158,25 +121,12 @@ _interrupt_handler:
nop nop
nop nop
.extern _system_call_handler
.global _system_call_handler
.type _system_call_handler, @function
.align 32
_system_call_handler:
eret
.org 0x100 .org 0x100
.global _crt0 .global _crt0
.type _crt0, @function .type _crt0, @function
_crt0: _crt0:
/* Clear r0 */ /* Clear r0 */
xor r0, r0, r0 xor r0, r0, r0
/* Setup the debug ROM if it's linked in */
mvhi r1, hi(_debug_unit)
ori r1, r1, lo(_debug_unit)
wcsr DEBA, r1
/* Setup stack and global pointer */ /* Setup stack and global pointer */
mvhi sp, hi(_fstack) mvhi sp, hi(_fstack)
ori sp, sp, lo(_fstack) ori sp, sp, lo(_fstack)
......
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