configure and activate FUN0

The constant definitions for this have been rationalized for consistency

configure and activate FUN0
The constant definitions for this have been rationalized for consistency
3a4c2a0f · Juan David González Cobas · 3122401f · 3a4c2a0f
Commit 3a4c2a0f authored Nov 20, 2012 by Juan David González Cobas
Hide whitespace changes
Inline Side-by-side

Showing with 60 additions and 18 deletions

svec_drv.c kernel/svec_drv.c +60 -18

No files found.
--- a/kernel/svec_drv.c
+++ b/kernel/svec_drv.c
@@ -120,27 +120,68 @@ int unmap_cs_csr(void) {
 	return 0;
 }

-#define BIT_SETRG (0x7FFFB)
-#define BIT_CLEARRG (0x7FFF7)
-#define WB64 0
+
+#define FUN0ADER  (0x7FF63)
+#define INT_LEVEL (0x7ff5b)
+#define INTVECTOR (0x7ff5f)
+#define WB_32_64 (0x7ff33)
+#define BIT_SET_REG (0x7FFFB)
+#define BIT_CLR_REG (0x7FFF7)
 #define WB32 1
+#define WB64 0
 #define RESET_CORE 0x80
-#define ENABLE 0x10
+#define ENABLE_CORE 0x10

-int setup_csr_fa0 (void)
+static u8 csr_read(void *base, u32 offset)
 {
-	/* Check if CS/CSR window is mapped */
-	if (cs_csr == NULL) {
-		printk(KERN_ERR PFX "CS/CSR window not found\n");
-		return -EINVAL;
-	}
+	char *p = base;
+	u8 value;

-	iowrite8(RESET_CORE, cs_csr->kernel_va + BIT_SETRG);
-	/*
-	iowrite8(ENABLE, cs_cst->kernel_va + BIT_CLEARRG);
-	iowrite
-	*/
-	return 0;
+	value = ioread8(p + offset);
+	pr_debug("read: 0x%08x -> 0x%02x\n", (u32)(p+offset), value);
+	return value;
+}
+
+static void csr_write(u8 value, void *base, u32 offset)
+{
+	char *p = base;
+
+	pr_debug("write: 0x%08x <- 0x%02x\n", (u32)(p+offset), value);
+	iowrite8(value, p + offset);
+}
+
+void setup_csr_fa0(void *base, u32 vme, unsigned vector, unsigned level)
+{
+	u8 fa[4];		/* FUN0 ADER contents */
+
+	/* reset the core */
+	csr_write(RESET_CORE, base, BIT_SET_REG);
+	msleep(10);
+
+	/* disable the core */
+	csr_write(ENABLE_CORE, base, BIT_CLR_REG);
+
+	/* default to 32bit WB interface */
+	csr_write(WB32, base, WB_32_64);
+
+	/* set interrupt vector and level */
+	csr_write(vector, base, INTVECTOR);
+	csr_write(level, base, INT_LEVEL);
+
+	/* do address relocation for FUN0 */
+	fa[0] = (vme >> 24) & 0xFF;
+	fa[1] = (vme >> 16) & 0xFF;
+	fa[2] = (vme >> 8 ) & 0xFF;
+	fa[3] = (VME_A32_USER_DATA_SCT & 0x3F) << 2;
+			/* DFSR and XAM are zero */
+
+	csr_write(fa[0], base, FUN0ADER);
+	csr_write(fa[1], base, FUN0ADER + 4);
+	csr_write(fa[2], base, FUN0ADER + 8);
+	csr_write(fa[3], base, FUN0ADER + 12);
+
+	/* enable module, hence make FUN0 available */
+	csr_write(ENABLE_CORE, base, BIT_SET_REG);
 }

 int svec_load_fpga(struct svec_dev *svec, const void *blob, int size)
@@ -344,8 +385,9 @@ static int __devinit svec_probe(struct device *pdev, unsigned int ndev)
 	if (error)
 		goto failed;

-	error = setup_csr_fa0();
-
+	/* configure and activate function 0 */
+	setup_csr_fa0(cs_csr->kernel_va, vmebase2[ndev],
+				vector[ndev], level[ndev]);
 	
 #if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,29)
 	name = pdev->bus_id;