rt firmware: added SoftPLL port (helper branch only)

41c883d5 · Tomasz Wlostowski · 8cfcce52 · 41c883d5 · 41c883d5 · 41c883d5
Commit 41c883d5 authored Mar 07, 2012 by Tomasz Wlostowski
5 changed files
--- a/rt/dev/softpll_ng.c
+++ b/rt/dev/softpll_ng.c
+#include <stdio.h>
+
+#include "board.h"
+#include "hw/softpll_regs.h"
+
+#include "irq.h"
+
+volatile int irq_count = 0,eee,yyy,py;
+
+static volatile struct SPLL_WB *SPLL = (volatile struct SPLL_WB *) BASE_SOFTPLL;
+
+/* The includes below contain code (not only declarations) to enable the compiler
+   to inline functions where necessary and save some CPU cycles */
+#include "spll_defs.h"
+#include "spll_common.h"
+#include "spll_helper.h"
+
+
+struct spll_pmeas_channel {
+	int acc;
+	int n_avgs, remaining;
+	int current;
+	int ready;
+	int n_tags;
+};
+
+static volatile uint32_t spll_pmeas_mask = 0;
+
+
+volatile struct spll_helper_state helper;
+volatile struct spll_pmeas_channel pmeas[32];
+
+static void pmeas_update(struct spll_pmeas_channel *chan, int tag)
+{
+	chan->n_tags++;
+	chan->remaining--;
+	chan->acc += tag & ((1<<HPLL_N)-1);
+	py = tag;
+	if(chan->remaining == 0)
+	{
+		chan->remaining = chan->n_avgs;
+		chan->current = chan->acc / chan->n_avgs;
+		chan->acc = 0;
+		chan->ready = 1;
+	}
+}
+
+static void pmeas_enable(int channel)
+{
+	pmeas[channel].n_avgs = 256;
+	pmeas[channel].remaining = 256;
+	pmeas[channel].current = 0;
+	pmeas[channel].acc = 0;
+	pmeas[channel].ready = 0;
+	pmeas[channel].n_tags = 0;
+	
+	SPLL->RCER |= (1<<channel);
+	
+	spll_pmeas_mask |= (1<<channel);
+}
+
+void _irq_entry()
+{
+	volatile uint32_t trr;
+	int src = -1, tag;
+	if(! (SPLL->CSR & SPLL_TRR_CSR_EMPTY))
+	{
+		trr = SPLL->TRR_R0;
+		src = SPLL_TRR_R0_CHAN_ID_R(trr);
+		tag = SPLL_TRR_R0_VALUE_R(trr);
+		eee = tag;
+
+		helper_update(&helper, tag, src);
+
+/*	if(spll_pmeas_mask & (1<<src))
+		pmeas_update(&pmeas[src], tag);*/
+	}
+
+//		yyy=helper.phase.pi.y;
+		irq_count++;
+		clear_irq();
+}
+
+void spll_init()
+{
+	volatile int dummy;
+	disable_irq();
+
+	SPLL->CSR= 0 ;
+	SPLL->OCER = 0;
+	SPLL->RCER = 0;
+	SPLL->RCGER = 0;
+	SPLL->DCCR = 0;
+	SPLL->DEGLITCH_THR = 1000;
+	while(! (SPLL->TRR_CSR & SPLL_TRR_CSR_EMPTY)) dummy = SPLL->TRR_R0;
+	dummy = SPLL->PER_HPLL;
+	SPLL->EIC_IER = 1;
+}
+
+int spll_check_lock()
+{
+	return helper.phase.ld.locked ? 1 : 0;
+}
+
+void spll_test()
+{
+	int i = 0;
+	volatile	int dummy;
+
+
+	spll_init();
+	helper_start(&helper, 8);
+	enable_irq();
+	
+	while(!spll_check_lock()) { TRACE("%d %d %x %x\n",irq_count, delta, SPLL->TRR_CSR, SPLL->OCER); }
+
+	
+	SPLL->DCCR = SPLL_DCCR_GATE_DIV_W(24);
+	SPLL->RCGER = (1<<7) | (1<<6);
+	pmeas_enable(7);
+	pmeas_enable(6);
+	for(;;) {
+		TRACE("RCER %x Phase %d/%d rdy %d/%d, py %d\n", SPLL->RCER, pmeas[7].current, pmeas[6].current, pmeas[7].ready, pmeas[6].ready, py);
+	}
+
+}
+
+#define CHAN_AUX 7
+#define CHAN_EXT 6
+
+
+/* measures external reference vs local clock phase */
+int spll_gm_measure_ext_phase()
+{
+	SPLL->CSR = 0;
+	SPLL->DCCR = SPLL_DCCR_GATE_DIV_W(25);
+	SPLL->RCGER = (1<<CHAN_AUX);
+	SPLL->RCGER = (1<<CHAN_EXT);
+}
\ No newline at end of file
--- a/rt/dev/spll_common.h
+++ b/rt/dev/spll_common.h
+/*
+
+White Rabbit Softcore PLL (SoftPLL) - common definitions
+
+*/
+
+/* 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;
+}
+
+#define DBG_Y 0
+#define DBG_ERR 1
+#define DBG_TAG 2
+#define DBG_REF 5
+#define DBG_PERIOD 3
+#define DBG_EVENT 4
+#define DBG_SAMPLE_ID 6
+
+#define DBG_HELPER 0x20
+#define DBG_PRELOCK 0x40
+#define DBG_EVT_START 1
+#define DBG_EVT_LOCKED 2
+
+static inline void spll_debug(int what, int value, int last)
+{
+	SPLL->DFR_SPLL = (last ? 0x80000000 : 0) | (value & 0xffffff) | (what << 24);
+}
\ No newline at end of file
--- a/rt/dev/spll_defs.h
+++ b/rt/dev/spll_defs.h
+/*
+
+White Rabbit Softcore PLL (SoftPLL) - common definitions
+
+*/
+
+
+#include <stdio.h>
+
+/* Reference clock frequency */
+#define CLOCK_FREQ 62500000
+
+/* Bit size of phase tags generated by the DMTDs. Used to sign-extend the tags. */
+#define TAG_BITS 22
+
+/* 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
+
+/* Number of reference channels */
+#define N_CHAN_REF 7
+
+/* Local reference input */
+#define ID_LOCAL_REF 6
+
+/* Number of output channels */
+#define N_CHAN_OUT 1
+
--- a/rt/dev/spll_helper.h
+++ b/rt/dev/spll_helper.h
+
+/* 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. This structure keeps the state of the pre-locking
+   stage */
+struct spll_helper_prelock_state {
+ 	spll_pi_t pi; 
+ 	spll_lock_det_t ld;
+ 	int f_setpoint;
+	int ref_select;
+};
+
+
+volatile int serr;
+	
+void helper_prelock_init(struct spll_helper_prelock_state *s)
+{
+	
+	/* Frequency branch PI controller */
+	s->pi.y_min = 5;
+	s->pi.y_max = 65530;
+	s->pi.anti_windup = 0;
+	s->pi.kp = 28*32*16;
+	s->pi.ki = 50*32*16;
+	s->pi.bias = 32000;
+
+	/* Freqency branch lock detection */
+	s->ld.threshold = 2;
+	s->ld.lock_samples = 100;
+	s->ld.delock_samples = 90;
+	
+	s->f_setpoint = -131072 / (1<<HPLL_N);
+
+	pi_init(&s->pi);
+	ld_init(&s->ld);
+}
+
+
+void helper_prelock_enable(int ref_channel, int enable)
+{
+	volatile int dummy;
+	
+	SPLL->CSR = 0;
+
+	dummy = SPLL->PER_HPLL;
+	if(enable)
+		SPLL->CSR = SPLL_CSR_PER_SEL_W(ref_channel) | SPLL_CSR_PER_EN;
+	else
+		SPLL->CSR = 0;
+	
+}
+
+#define SPLL_LOCKED 1
+#define SPLL_LOCKING 0
+
+int helper_prelock_update(struct spll_helper_prelock_state *s)
+{
+	int y;
+	volatile uint32_t per = SPLL->PER_HPLL;
+	
+	if(per & SPLL_PER_HPLL_VALID)
+	{
+		short err = (short) (per & 0xffff);
+
+		err -= s->f_setpoint;
+
+		serr = (int)err;
+
+		y = pi_update(&s->pi, err);
+		SPLL->DAC_HPLL = y;
+
+		spll_debug(DBG_Y | DBG_PRELOCK | DBG_HELPER, y, 0);
+		spll_debug(DBG_ERR | DBG_PRELOCK | DBG_HELPER, err, 1);
+		
+		if(ld_update(&s->ld, err))
+		{
+			spll_debug(DBG_EVENT | DBG_PRELOCK | DBG_HELPER, DBG_EVT_LOCKED, 1);
+			
+			return SPLL_LOCKED;
+		}
+	}
+	
+	return SPLL_LOCKING;
+}
+
+struct spll_helper_phase_state {
+	int p_adder;
+ 	int p_setpoint, tag_d0;
+ 	int ref_src;
+ 	int sample_n;
+ 	spll_pi_t pi; 
+ 	spll_lock_det_t ld;
+};
+	
+void helper_phase_init(struct spll_helper_phase_state *s)
+{
+	
+	/* Phase branch PI controller */
+	s->pi.y_min = 5;
+	s->pi.y_max = 65530;
+ 	s->pi.kp = (int)(0.3 * 32.0 * 16.0);
+	s->pi.ki = (int)(0.03 * 32.0 * 3.0); 
+
+
+	s->pi.anti_windup = 0;
+	s->pi.bias = 32000;
+	
+	/* Phase branch lock detection */
+	s->ld.threshold = 200;
+	s->ld.lock_samples = 1000;
+	s->ld.delock_samples = 900;
+	s->ref_src = 8;
+	s->p_setpoint = 0;	
+	s->p_adder = 0;
+	s->sample_n = 0;
+	s->tag_d0 = 0;
+	pi_init(&s->pi);
+	ld_init(&s->ld);
+}
+
+void helper_phase_enable(int ref_channel, int enable)
+{
+	if(enable) 
+		SPLL->OCER = 1; /* fixme: use ref_channel */ 
+	else
+		SPLL->OCER = 0;
+
+//		spll_debug(DBG_EVENT | DBG_HELPER, DBG_EVT_START, 1);
+}
+
+volatile int delta;
+
+#define TAG_WRAPAROUND 100000000
+
+int helper_phase_update(struct spll_helper_phase_state *s, int tag, int source)
+{
+	int err, y;
+
+	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 > tag)
+			s->p_adder += (1<<TAG_BITS);
+			
+		err = (tag + s->p_adder) - s->p_setpoint;
+
+		s->tag_d0 = tag;
+		s->p_setpoint += (1<<HPLL_N);
+		
+		if(s->p_adder > TAG_WRAPAROUND)
+		{
+			s->p_adder -= TAG_WRAPAROUND;
+			s->p_setpoint -= TAG_WRAPAROUND;
+		}
+		
+		y = pi_update(&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(&s->ld, err))
+			return SPLL_LOCKED;
+	}
+
+	return SPLL_LOCKING;
+}
+
+#define HELPER_PRELOCKING 1
+#define HELPER_PHASE 2
+#define HELPER_LOCKED 3
+
+struct spll_helper_state {
+	struct spll_helper_prelock_state prelock;
+	struct 	spll_helper_phase_state phase;
+	int state;
+	int ref_channel;
+};
+
+void helper_start(struct spll_helper_state *s, int ref_channel)
+{
+	s->state = HELPER_PRELOCKING;	
+	s->ref_channel = ref_channel;
+	
+	helper_prelock_init(&s->prelock);
+	helper_phase_init(&s->phase);
+	helper_prelock_enable(ref_channel, 1);
+	spll_debug(DBG_EVENT | DBG_PRELOCK | DBG_HELPER, DBG_EVT_START, 1);
+	
+}
+
+void helper_update(struct spll_helper_state *s, int tag, int source)
+{
+	switch(s->state)
+	{
+		case HELPER_PRELOCKING:
+			if(helper_prelock_update(&s->prelock) == SPLL_LOCKED)
+			{
+				s->state = HELPER_PHASE;
+				helper_prelock_enable(s->ref_channel, 0);
+				s->phase.pi.bias = s->prelock.pi.y;
+				helper_phase_enable(s->ref_channel, 1);
+			}
+		break;
+		case HELPER_PHASE:
+			if(helper_phase_update(&s->phase, tag, source)==SPLL_LOCKED)
+			{
+//				spll_debug(DBG_EVENT |  DBG_HELPER, DBG_EVT_LOCKED, 1);
+			
+			}
+		break;
+	}
+}
\ No newline at end of file
--- a/rt/include/hw/softpll_regs.h
+++ b/rt/include/hw/softpll_regs.h