Commit 4783fba2 authored by Tomasz Wlostowski's avatar Tomasz Wlostowski

softpll: 2nd gen, working helper

parent 0f4fb1ca
#include <stdio.h>
#include <stdlib.h>
#include "board.h"
#include "irq.h"
......@@ -24,6 +27,108 @@
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 {
int hpll_f_kp;
int hpll_f_ki;
......@@ -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 {
int h_lock_counter;
int h_i;
int h_dac_bias;
int h_p_setpoint;
int h_freq_mode;
int h_locked;
int h_dac_val;
int h_freq_err;
int h_tag;
int d_dac_bias;
int d_tag_ref_d0;
int d_tag_fb_d0;
int d_tag_ref_ready;
int d_tag_fb_ready;
int d_period_ref;
int d_period_fb;
int d_freq_mode;
int d_lock_counter;
int d_freq_error;
int d_i;
int d_p_setpoint;
int d_phase_shift;
int d_locked;
};
/* 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 * 4.0);
s->pi_phase.ki = (int)(0.05 * 32.0 * 0.1);
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->f_setpoint = 16;
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;
volatile int sp_limit = 0xffff;
void _irq_entry()
static inline void helper_irq(struct spll_helper_state *hpll, int tag_ref)
{
int dv;
int tag_ref_ready = 0;
int tag_fb_ready = 0;
int tag_ref;
int tag_fb;
// int sp = _get_sp();
int err, y;
irq_cnt ++;
gpio_out(GPIO_PIN_LED_STATUS, 1);
if(SPLL->CSR & READY_REF)
/* HPLL: active frequency branch */
if(hpll->freq_mode)
{
tag_ref = SPLL->TAG_REF;
tag_ref_ready = 1;
}
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);
if(SPLL->CSR & READY_FB)
SPLL->DAC_HPLL = y;
if(ld_update(&hpll->ld_freq, err))
{
tag_fb = SPLL->TAG_FB;
tag_fb_ready = 1;
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);
}
/* HPLL: active frequency branch */
if(pstate.h_freq_mode)
} else { /* HPLL: active phase branch */
if(hpll->p_setpoint < 0)
{
int freq_err = SPLL->PER_HPLL;
if(freq_err & 0x100) freq_err |= 0xffffff00; /* sign-extend */
freq_err += pll_cfg.hpll_f_setpoint;
hpll->p_setpoint = tag_ref;
return;
}
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 */
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;
ld_update(&hpll->ld_phase, err);
}
}
if(dv >= 65530) dv = 65530;
if(dv < 0) dv = 0;
void _irq_entry()
{
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 */
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;
helper_irq(&helper, tag_ref);
/* 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 */
} else if (tag_ref_ready) {
if(pstate.h_p_setpoint < 0) /* we don't have yet any phase samples? */
......@@ -345,6 +426,7 @@ void _irq_entry()
}
gpio_out(GPIO_PIN_LED_STATUS, 0);
#endif
clear_irq();
}
......@@ -356,57 +438,44 @@ void softpll_enable()
SPLL->CSR = 0;
disable_irq();
SPLL->DAC_HPLL = pll_cfg.hpll_dac_bias;
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;
helper_init(&helper);
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;
enable_irq();
// softpll_test();
prev_lck = 0;
TRACE_DEV("[softpll]: enabled\n");
prev_lck = 0;
}
int softpll_check_lock()
{
TRACE_DEV("[softpll] Helper: lock %d freqmode %d, Main: lock %d freqmode %d\n",
pstate.h_locked, pstate.h_freq_mode,
pstate.d_locked,pstate.d_freq_mode);
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) ;
int lck = pstate.h_locked && pstate.d_locked;
/*nt lck = pstate.h_locked && pstate.d_locked;
if(lck && !prev_lck)
TRACE_DEV("[softpll]: got lock\n");
else if (!lck && prev_lck)
TRACE_DEV("[softpll]: lost lock\n");
TRACE_DEV("[softpll]: lost lock\n");*/
prev_lck = lck;
return lck;
// prev_lck = lck;
// return lck;
}
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)
{
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);
// 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);
}
void softpll_disable()
......@@ -417,5 +486,6 @@ void softpll_disable()
int softpll_get_setpoint()
{
return pstate.d_p_setpoint;
// return pstate.d_p_setpoint;
}
......@@ -89,11 +89,9 @@
.global _start
_start:
.weak _debug_unit
.global _reset_handler
.type _reset_handler, @function
.align 256
_debug_unit:
_reset_handler:
xor r0, r0, r0
wcsr IE, r0
......@@ -105,45 +103,10 @@ _reset_handler:
nop
.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
.org 0xc0
.global _interrupt_handler
.type _interrupt_handler, @function
.align 32
_interrupt_handler:
sw (sp+0), ra
calli _save_all
......@@ -158,25 +121,12 @@ _interrupt_handler:
nop
nop
.extern _system_call_handler
.global _system_call_handler
.type _system_call_handler, @function
.align 32
_system_call_handler:
eret
.org 0x100
.global _crt0
.type _crt0, @function
_crt0:
/* Clear 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 */
mvhi sp, hi(_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