softpll: added phase trackers (replacement of VHDL dmtd_phase_meas)

Conflicts: rt/dev/softpll_ng.c

softpll: added phase trackers (replacement of VHDL dmtd_phase_meas)
Conflicts: rt/dev/softpll_ng.c
c8cd7957 · Tomasz Wlostowski · 54af54de · c8cd7957 · c8cd7957
Commit c8cd7957 authored Mar 11, 2012 by Tomasz Wlostowski
Hide whitespace changes
Inline Side-by-side

Showing with 88 additions and 42 deletions

softpll_ng.c rt/dev/softpll_ng.c +3 -42

spll_ptracker.h rt/dev/spll_ptracker.h +85 -0

No files found.
--- a/rt/dev/softpll_ng.c
+++ b/rt/dev/softpll_ng.c
@@ -21,49 +21,10 @@ static volatile struct SPLL_WB *SPLL = (volatile struct SPLL_WB *) BASE_SOFTPLL;
 #include "spll_debug.h"
 #include "spll_helper.h"
 #include "spll_main.h"
+#include "spll_ptracker.h"

-
-struct spll_pmeas_channel {
-	int acc;
-	int n_avgs, remaining;
-	int current;
-	int ready;
-	int n_tags;
-};
-
-static volatile struct spll_helper_state helper;
-static volatile struct spll_main_state mpll;
-static volatile struct spll_pmeas_channel pmeas[MAX_CHAN_REF + MAX_CHAN_OUT];
-
-
-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);
-}
+static struct spll_helper_state helper;
+static struct spll_main_state mpll;

 void _irq_entry()
 {

--- a/rt/dev/spll_ptracker.h
+++ b/rt/dev/spll_ptracker.h
+
+/* State of a Phase Tracker */
+
+struct spll_ptracker_state {
+	int id_a, id_b;
+	int n_avg, acc, avg_count;
+	int phase_val, ready;
+	int tag_a, tag_b; 	 	
+	int sample_n;
+	int preserve_sign;
+};
+
+
+static void ptracker_init(struct spll_ptracker_state *s, int id_a, int id_b, int num_avgs)
+{
+	s->tag_a = s->tag_b = -1;
+	
+	s->id_a = id_a;
+	s->id_b = id_b;
+	s->ready = 0;
+	s->n_avg = num_avgs;
+	s->acc = 0;
+	s->avg_count = 0;
+	s->sample_n=  0;
+	s->preserve_sign = 0;
+}
+
+static void ptracker_start(struct spll_ptracker_state *s)
+{
+  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(struct spll_ptracker_state *s, int tag, int source)
+{
+
+	if(source == s->id_a)
+		s->tag_a = tag;
+	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->avg_count == 0)
+		{
+		
+			if(delta <= PTRACK_WRAP_LO) 
+				s->preserve_sign = -1;
+			else if (delta >= PTRACK_WRAP_HI)
+				s->preserve_sign = 1;
+			else
+				s->preserve_sign = 0;
+	
+			
+			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);
+			else
+				s->acc += delta;
+
+			s->avg_count++;
+
+			if(s->avg_count == s->n_avg)
+			{
+				s->phase_val = s->acc / s->n_avg;
+				s->ready = 1;
+				s->acc = 0;
+				s->avg_count = 0;
+			}
+		
+		}	
+		s->tag_b = s->tag_a = -1;
+  }
+
+	return SPLL_LOCKING;
+}