Support for XLP4xx and XLP 8xx B0 revision

- Add 4xx processor IDs, add workaround in CPU detection code.
- Update frequency detection code for XLP 8xx.
- Add setting device frequency code.
- Update processor ID checking code.

4 files changed, 159 insertions, 54 deletions

diff --git a/sys/mips/nlm/hal/nlm_hal.c b/sys/mips/nlm/hal/nlm_hal.c
index 049593c..d9d1849 100644
--- a/sys/mips/nlm/hal/nlm_hal.c
+++ b/sys/mips/nlm/hal/nlm_hal.c
@@ -55,33 +55,81 @@ int pic_irt_pcie_lnk2;
 int pic_irt_pcie_lnk3;
 
 uint32_t
-xlp_get_cpu_frequency(int core)
+xlp_get_cpu_frequency(int node, int core)
 {
-	uint64_t sysbase = nlm_get_sys_regbase(nlm_nodeid());
-	uint64_t num;
+	uint64_t sysbase = nlm_get_sys_regbase(node);
 	unsigned int pll_divf, pll_divr, dfs_div, ext_div;
-	unsigned int rstval, dfsval, denom;
+	unsigned int rstval, dfsval;
 
 	rstval = nlm_read_sys_reg(sysbase, SYS_POWER_ON_RESET_CFG);
 	dfsval = nlm_read_sys_reg(sysbase, SYS_CORE_DFS_DIV_VALUE);
 	pll_divf = ((rstval >> 10) & 0x7f) + 1;
 	pll_divr = ((rstval >> 8)  & 0x3) + 1;
-	ext_div  = ((rstval >> 30) & 0x3) + 1;
-	dfs_div  = ((dfsval >> (core * 4)) & 0xf) + 1;
+	if (!nlm_is_xlp8xx_ax())
+		ext_div  = ((rstval >> 30) & 0x3) + 1;
+	else
+		ext_div  = 1;
+	dfs_div  = ((dfsval >> (core << 2)) & 0xf) + 1;
 
-	num = 800000000ULL * pll_divf;
-	denom = 3 * pll_divr * ext_div * dfs_div;
-	num = num/denom;
-	return (num);
+	return ((800000000ULL * pll_divf)/(3 * pll_divr * ext_div * dfs_div));
+}
+
+static u_int
+nlm_get_device_frequency(uint64_t sysbase, int devtype)
+{
+	uint32_t pllctrl, dfsdiv, spf, spr, div_val;
+	int extra_div;
+
+	pllctrl = nlm_read_sys_reg(sysbase, SYS_PLL_CTRL);
+	if (devtype <= 7)
+		div_val = nlm_read_sys_reg(sysbase, SYS_DFS_DIV_VALUE0);
+	else {
+		devtype -= 8;
+		div_val = nlm_read_sys_reg(sysbase, SYS_DFS_DIV_VALUE1);
+	}
+	dfsdiv = ((div_val >> (devtype << 2)) & 0xf) + 1;
+	spf = (pllctrl >> 3 & 0x7f) + 1;
+	spr = (pllctrl >> 1 & 0x03) + 1;
+	extra_div = nlm_is_xlp8xx_ax() ? 1 : 2;
+
+	return ((400 * spf) / (3 * extra_div * spr * dfsdiv));
+}
+
+int
+nlm_set_device_frequency(int node, int devtype, int frequency)
+{
+	uint64_t sysbase;
+	u_int cur_freq;
+	int dec_div;
+
+	sysbase = nlm_get_sys_regbase(node);
+	cur_freq = nlm_get_device_frequency(sysbase, devtype);
+	if (cur_freq < (frequency - 5))
+		dec_div = 1;
+	else
+		dec_div = 0;
+
+	for(;;) {
+		if ((cur_freq >= (frequency - 5)) && (cur_freq <= frequency))
+			break;
+		if (dec_div)
+			nlm_write_sys_reg(sysbase, SYS_DFS_DIV_DEC_CTRL,
+			    (1 << devtype));
+		else
+			nlm_write_sys_reg(sysbase, SYS_DFS_DIV_INC_CTRL,
+			    (1 << devtype));
+		cur_freq = nlm_get_device_frequency(sysbase, devtype);
+	}
+	return (nlm_get_device_frequency(sysbase, devtype));
 }
 
 void
-nlm_pic_irt_init(void)
+nlm_pic_irt_init(int node)
 {
-	pic_irt_ehci0 = nlm_irtstart(nlm_get_usb_pcibase(nlm_nodeid(), 0));
-	pic_irt_ehci1 = nlm_irtstart(nlm_get_usb_pcibase(nlm_nodeid(), 3));
-	pic_irt_uart0 = nlm_irtstart(nlm_get_uart_pcibase(nlm_nodeid(), 0));
-	pic_irt_uart1 = nlm_irtstart(nlm_get_uart_pcibase(nlm_nodeid(), 1));
+	pic_irt_ehci0 = nlm_irtstart(nlm_get_usb_pcibase(node, 0));
+	pic_irt_ehci1 = nlm_irtstart(nlm_get_usb_pcibase(node, 3));
+	pic_irt_uart0 = nlm_irtstart(nlm_get_uart_pcibase(node, 0));
+	pic_irt_uart1 = nlm_irtstart(nlm_get_uart_pcibase(node, 1));
 
 	/* Hardcoding the PCIE IRT information as PIC doesn't 
 	   understand any value other than 78,79,80,81 for PCIE0/1/2/3 */
@@ -107,24 +155,25 @@ int
 xlp_irt_to_irq(int irt)
 {
 	if (irt == pic_irt_ehci0)
-	    return PIC_EHCI_0_IRQ;
+		return PIC_EHCI_0_IRQ;
 	else if (irt == pic_irt_ehci1)
-	    return PIC_EHCI_1_IRQ;
+		return PIC_EHCI_1_IRQ;
 	else if (irt == pic_irt_uart0)
-	    return PIC_UART_0_IRQ;
+		return PIC_UART_0_IRQ;
 	else if (irt == pic_irt_uart1)
-	    return PIC_UART_1_IRQ;
+		return PIC_UART_1_IRQ;
 	else if (irt == pic_irt_pcie_lnk0)
-	    return PIC_PCIE_0_IRQ;
+		return PIC_PCIE_0_IRQ;
 	else if (irt == pic_irt_pcie_lnk1)
-	    return PIC_PCIE_1_IRQ;
+		return PIC_PCIE_1_IRQ;
 	else if (irt == pic_irt_pcie_lnk2)
-	    return PIC_PCIE_2_IRQ;
+		return PIC_PCIE_2_IRQ;
 	else if (irt == pic_irt_pcie_lnk3)
-	    return PIC_PCIE_3_IRQ;
+		return PIC_PCIE_3_IRQ;
 	else {
-	    printf("Cannot find irq for IRT %d\n", irt);
-	    return 0;
+		if (bootverbose)
+			printf("Cannot find irq for IRT %d\n", irt);
+		return 0;
 	 }
 }
 
@@ -156,15 +205,15 @@ int
 xlp_irq_is_picintr(int irq)
 {
 	switch (irq) {
-		case PIC_MMC_IRQ : return 1;
- 		case PIC_EHCI_0_IRQ : return 1;
- 		case PIC_EHCI_1_IRQ : return 1;
-		case PIC_UART_0_IRQ : return 1;
-		case PIC_UART_1_IRQ : return 1;
-		case PIC_PCIE_0_IRQ : return 1;
-		case PIC_PCIE_1_IRQ : return 1;
-		case PIC_PCIE_2_IRQ : return 1;
-		case PIC_PCIE_3_IRQ : return 1;
-		default: return 0;
+ 		case PIC_EHCI_0_IRQ :
+ 		case PIC_EHCI_1_IRQ :
+		case PIC_UART_0_IRQ :
+		case PIC_UART_1_IRQ :
+		case PIC_PCIE_0_IRQ :
+		case PIC_PCIE_1_IRQ :
+		case PIC_PCIE_2_IRQ :
+		case PIC_PCIE_3_IRQ :
+			return (1);
+		default: return (0);
 	}
 }
diff --git a/sys/mips/nlm/hal/sys.h b/sys/mips/nlm/hal/sys.h
index 89b27fd..adfd8a8 100644
--- a/sys/mips/nlm/hal/sys.h
+++ b/sys/mips/nlm/hal/sys.h
@@ -30,7 +30,7 @@
  */
 
 #ifndef __NLM_HAL_SYS_H__
-#define __NLM_HAL_SYS_H__
+#define	__NLM_HAL_SYS_H__
 
 /**
 * @file_name sys.h
@@ -121,5 +121,24 @@
 #define	nlm_get_sys_pcibase(node) nlm_pcicfg_base(XLP_IO_SYS_OFFSET(node))
 #define	nlm_get_sys_regbase(node) (nlm_get_sys_pcibase(node) + XLP_IO_PCI_HDRSZ)
 
+enum {
+	/* Don't change order and it must start from zero */
+	DFS_DEVICE_NAE = 0,
+	DFS_DEVICE_SAE,
+	DFS_DEVICE_RSA,
+	DFS_DEVICE_DTRE,
+	DFS_DEVICE_CMP,
+	DFS_DEVICE_KBP,
+	DFS_DEVICE_DMC,
+	DFS_DEVICE_NAND,
+	DFS_DEVICE_MMC,
+	DFS_DEVICE_NOR,
+	DFS_DEVICE_CORE,
+	DFS_DEVICE_REGEX_SLOW,
+	DFS_DEVICE_REGEX_FAST,
+	DFS_DEVICE_SATA,
+	INVALID_DFS_DEVICE = 0xFF
+};
+
 #endif
 #endif
diff --git a/sys/mips/nlm/xlp.h b/sys/mips/nlm/xlp.h
index c80c80a..d07ef6d 100644
--- a/sys/mips/nlm/xlp.h
+++ b/sys/mips/nlm/xlp.h
@@ -34,21 +34,25 @@
 #include <mips/nlm/hal/mips-extns.h>
 #include <mips/nlm/hal/iomap.h>
 
-#define PIC_UART_0_IRQ	9
-#define PIC_UART_1_IRQ	10
+#define	PIC_UART_0_IRQ	9
+#define	PIC_UART_1_IRQ	10
 
-#define PIC_PCIE_0_IRQ	11
-#define PIC_PCIE_1_IRQ	12
-#define PIC_PCIE_2_IRQ	13
-#define PIC_PCIE_3_IRQ	14
+#define	PIC_PCIE_0_IRQ	11
+#define	PIC_PCIE_1_IRQ	12
+#define	PIC_PCIE_2_IRQ	13
+#define	PIC_PCIE_3_IRQ	14
+
+#define	PIC_EHCI_0_IRQ	39 
+/* 41 used by IRQ_SMP */
+#define	PIC_EHCI_1_IRQ	42 
+#define	PIC_MMC_IRQ	43
 
-#define PIC_EHCI_0_IRQ	39 
-#define PIC_EHCI_1_IRQ	42 
-#define PIC_MMC_IRQ	43
 
 /* XLP 8xx/4xx A0, A1, A2 CPU COP0 PRIDs */
 #define	CHIP_PROCESSOR_ID_XLP_8XX		0x10
 #define	CHIP_PROCESSOR_ID_XLP_3XX		0x11
+#define	CHIP_PROCESSOR_ID_XLP_416		0x94
+#define	CHIP_PROCESSOR_ID_XLP_432		0x14
 
 /* Revision id's */
 #define	XLP_REVISION_A0				0x00
@@ -69,24 +73,51 @@ extern int xlp_hwtid_to_cpuid[];
 #ifdef SMP
 extern void xlp_enable_threads(int code);
 #endif
-uint32_t xlp_get_cpu_frequency(int core);
-void nlm_pic_irt_init(void);
+uint32_t xlp_get_cpu_frequency(int node, int core);
+int nlm_set_device_frequency(int node, int devtype, int frequency);
+void nlm_pic_irt_init(int node);
 int xlp_irt_to_irq(int irt);
 int xlp_irq_to_irt(int irq);
 int xlp_irq_is_picintr(int irq);
 
+static __inline int nlm_processor_id(void)
+{
+	return ((mips_rd_prid() >> 8) & 0xff);
+}
+
 static __inline int nlm_is_xlp3xx(void)
 {
-	int prid = (mips_rd_prid() >> 8) & 0xff;
 
-	return (prid == CHIP_PROCESSOR_ID_XLP_3XX);
+	return (nlm_processor_id() == CHIP_PROCESSOR_ID_XLP_3XX);
+}
+
+static __inline int nlm_is_xlp4xx(void)
+{
+	int prid = nlm_processor_id();
+
+	return (prid == CHIP_PROCESSOR_ID_XLP_432 ||
+	    prid == CHIP_PROCESSOR_ID_XLP_416);
 }
 
 static __inline int nlm_is_xlp8xx(void)
 {
-	int prid = (mips_rd_prid() >> 8) & 0xff;
+	int prid = nlm_processor_id();
+
+	return (prid == CHIP_PROCESSOR_ID_XLP_8XX ||
+	    prid == CHIP_PROCESSOR_ID_XLP_432 ||
+	    prid == CHIP_PROCESSOR_ID_XLP_416);
+}
+
+static __inline int nlm_is_xlp8xx_ax(void)
+{
+	uint32_t procid = mips_rd_prid();
+	int prid = (procid >> 8) & 0xff;
+	int rev = procid & 0xff;
 
-	return (prid == CHIP_PROCESSOR_ID_XLP_8XX);
+	return ((prid == CHIP_PROCESSOR_ID_XLP_8XX ||
+	    prid == CHIP_PROCESSOR_ID_XLP_432 ||
+	    prid == CHIP_PROCESSOR_ID_XLP_416) &&
+	    (rev < XLP_REVISION_B0));
 }
 
 #endif /* LOCORE */
diff --git a/sys/mips/nlm/xlp_machdep.c b/sys/mips/nlm/xlp_machdep.c
index 53ccb0f..2df3f23 100644
--- a/sys/mips/nlm/xlp_machdep.c
+++ b/sys/mips/nlm/xlp_machdep.c
@@ -196,9 +196,15 @@ xlp_parse_mmu_options(void)
 		goto unsupp;
 	}
 
-	/* Take out cores which do not exist on chip */
+	/* Try to find the enabled cores from SYS block */
 	sysbase = nlm_get_sys_regbase(0);
 	cpu_rst_mask = nlm_read_sys_reg(sysbase, SYS_CPU_RESET) & 0xff;
+
+	/* XLP 416 does not report this correctly, fix */
+	if (nlm_processor_id() == CHIP_PROCESSOR_ID_XLP_416)
+		cpu_rst_mask = 0xe;
+
+	/* Take out cores which do not exist on chip */
 	for (i = 1; i < XLP_MAX_CORES; i++) {
 		if ((cpu_rst_mask & (1 << i)) == 0)
 			cpu_map &= ~(0xfu << (4 * i));
@@ -515,12 +521,12 @@ platform_start(__register_t a0 __unused,
 	/* initialize console so that we have printf */
 	boothowto |= (RB_SERIAL | RB_MULTIPLE);	/* Use multiple consoles */
 
-	nlm_pic_irt_init(); /* complete before interrupts or console init */
+	nlm_pic_irt_init(0); /* complete before interrupts or console init */
 	init_static_kenv(boot1_env, sizeof(boot1_env));
 	xlp_bootargs_init(a0);
 
 	/* clockrate used by delay, so initialize it here */
-	xlp_cpu_frequency = xlp_get_cpu_frequency(0);
+	xlp_cpu_frequency = xlp_get_cpu_frequency(0, 0);
 	cpu_clock = xlp_cpu_frequency / 1000000;
 	mips_timer_early_init(xlp_cpu_frequency);