From 2315544b36f411b03748693acadbce0b3ea05a2a Mon Sep 17 00:00:00 2001
From: Sebastien Bourdeauducq <sebastien@milkymist.org>
Date: Thu, 21 Mar 2013 15:32:26 +0100
Subject: [PATCH] software/videomixer: quick hack for phase detection

---
 software/include/hw/dvisampler.h | 35 +++++++++++++
 software/videomixer/main.c       | 84 +++++++++++++++++++++++++++++++-
 2 files changed, 118 insertions(+), 1 deletion(-)
 create mode 100644 software/include/hw/dvisampler.h

diff --git a/software/include/hw/dvisampler.h b/software/include/hw/dvisampler.h
new file mode 100644
index 000000000..30b60fa60
--- /dev/null
+++ b/software/include/hw/dvisampler.h
@@ -0,0 +1,35 @@
+#ifndef __HW_DVISAMPLER_H
+#define __HW_DVISAMPLER_H
+
+#include <hw/common.h>
+#include <csrbase.h>
+
+#define DVISAMPLER0_CSR(x)		MMPTR(DVISAMPLER0_BASE+(x))
+
+#define CSR_DVISAMPLER0_PLL_RESET	DVISAMPLER0_CSR(0x00)
+#define CSR_DVISAMPLER0_PLL_LOCKED	DVISAMPLER0_CSR(0x04)
+
+#define CSR_DVISAMPLER0_D0_DELAY_CTL	DVISAMPLER0_CSR(0x08)
+#define CSR_DVISAMPLER0_D0_DELAY_BUSY	DVISAMPLER0_CSR(0x0C)
+#define CSR_DVISAMPLER0_D0_PHASE	DVISAMPLER0_CSR(0x10)
+#define CSR_DVISAMPLER0_D0_PHASE_RESET	DVISAMPLER0_CSR(0x14)
+
+#define CSR_DVISAMPLER0_D1_DELAY_CTL	DVISAMPLER0_CSR(0x18)
+#define CSR_DVISAMPLER0_D1_DELAY_BUSY	DVISAMPLER0_CSR(0x1C)
+#define CSR_DVISAMPLER0_D1_PHASE	DVISAMPLER0_CSR(0x20)
+#define CSR_DVISAMPLER0_D1_PHASE_RESET	DVISAMPLER0_CSR(0x24)
+
+#define CSR_DVISAMPLER0_D2_DELAY_CTL	DVISAMPLER0_CSR(0x28)
+#define CSR_DVISAMPLER0_D2_DELAY_BUSY	DVISAMPLER0_CSR(0x2C)
+#define CSR_DVISAMPLER0_D2_PHASE	DVISAMPLER0_CSR(0x30)
+#define CSR_DVISAMPLER0_D2_PHASE_RESET	DVISAMPLER0_CSR(0x34)
+
+#define DVISAMPLER_DELAY_CAL		0x01
+#define DVISAMPLER_DELAY_RST		0x02
+#define DVISAMPLER_DELAY_INC		0x04
+#define DVISAMPLER_DELAY_DEC		0x08
+
+#define DVISAMPLER_TOO_LATE		0x01
+#define DVISAMPLER_TOO_EARLY		0x02
+
+#endif /* __HW_DVISAMPLER_H */
diff --git a/software/videomixer/main.c b/software/videomixer/main.c
index e00e497f5..e549ae7d1 100644
--- a/software/videomixer/main.c
+++ b/software/videomixer/main.c
@@ -2,6 +2,88 @@
 
 #include <irq.h>
 #include <uart.h>
+#include <hw/dvisampler.h>
+
+static int d0, d1, d2;
+
+static void calibrate_delays(void)
+{
+	CSR_DVISAMPLER0_D0_DELAY_CTL = DVISAMPLER_DELAY_CAL;
+	CSR_DVISAMPLER0_D1_DELAY_CTL = DVISAMPLER_DELAY_CAL;
+	CSR_DVISAMPLER0_D2_DELAY_CTL = DVISAMPLER_DELAY_CAL;
+	while(CSR_DVISAMPLER0_D0_DELAY_BUSY || CSR_DVISAMPLER0_D1_DELAY_BUSY || CSR_DVISAMPLER0_D2_DELAY_BUSY);
+	CSR_DVISAMPLER0_D0_DELAY_CTL = DVISAMPLER_DELAY_RST;
+	CSR_DVISAMPLER0_D1_DELAY_CTL = DVISAMPLER_DELAY_RST;
+	CSR_DVISAMPLER0_D2_DELAY_CTL = DVISAMPLER_DELAY_RST;
+	CSR_DVISAMPLER0_D0_PHASE_RESET = 1;
+	CSR_DVISAMPLER0_D1_PHASE_RESET = 1;
+	CSR_DVISAMPLER0_D2_PHASE_RESET = 1;
+	d0 = d1 = d2 = 0;
+	printf("Delays calibrated\n");
+}
+
+static void adjust_phase(void)
+{
+	switch(CSR_DVISAMPLER0_D0_PHASE) {
+		case DVISAMPLER_TOO_LATE:
+			CSR_DVISAMPLER0_D0_DELAY_CTL = DVISAMPLER_DELAY_DEC;
+			d0--;
+			CSR_DVISAMPLER0_D0_PHASE_RESET = 1;
+			break;
+		case DVISAMPLER_TOO_EARLY:
+			CSR_DVISAMPLER0_D0_DELAY_CTL = DVISAMPLER_DELAY_INC;
+			d0++;
+			CSR_DVISAMPLER0_D0_PHASE_RESET = 1;
+			break;
+	}
+	switch(CSR_DVISAMPLER0_D1_PHASE) {
+		case DVISAMPLER_TOO_LATE:
+			CSR_DVISAMPLER0_D1_DELAY_CTL = DVISAMPLER_DELAY_DEC;
+			d1--;
+			CSR_DVISAMPLER0_D1_PHASE_RESET = 1;
+			break;
+		case DVISAMPLER_TOO_EARLY:
+			CSR_DVISAMPLER0_D1_DELAY_CTL = DVISAMPLER_DELAY_INC;
+			d1++;
+			CSR_DVISAMPLER0_D1_PHASE_RESET = 1;
+			break;
+	}
+	switch(CSR_DVISAMPLER0_D2_PHASE) {
+		case DVISAMPLER_TOO_LATE:
+			CSR_DVISAMPLER0_D2_DELAY_CTL = DVISAMPLER_DELAY_DEC;
+			d2--;
+			CSR_DVISAMPLER0_D2_PHASE_RESET = 1;
+			break;
+		case DVISAMPLER_TOO_EARLY:
+			CSR_DVISAMPLER0_D2_DELAY_CTL = DVISAMPLER_DELAY_INC;
+			d2++;
+			CSR_DVISAMPLER0_D2_PHASE_RESET = 1;
+			break;
+	}
+	//printf("Ph: %4d %4d %4d\n", d0, d1, d2);
+}
+
+static void vmix(void)
+{
+	unsigned int counter;
+
+	while(1) {
+		while(!CSR_DVISAMPLER0_PLL_LOCKED);
+		printf("PLL locked\n");
+		calibrate_delays();
+		adjust_phase();
+
+		counter = 0;
+		while(CSR_DVISAMPLER0_PLL_LOCKED) {
+			counter++;
+			if(counter == 200000) {
+				adjust_phase();
+				counter = 0;
+			}
+		}
+		printf("PLL unlocked\n");
+	}
+}
 
 int main(void)
 {
@@ -11,7 +93,7 @@ int main(void)
 	
 	puts("Minimal video mixer software built "__DATE__" "__TIME__"\n");
 	
-	while(1);
+	vmix();
 	
 	return 0;
 }