powerpc/mpc85xx: Fix core cluster PLL calculation for Chassis generation 2

Corenet based SoCs have different core clocks starting from Chassis
generation 2. Cores are organized into clusters. Each cluster has up to
4 cores sharing same clock, which can be chosen from one of three PLLs in
the cluster group with one of the devisors /1, /2 or /4. Two clusters are
put together as a cluster group. These two clusters share the PLLs but may
have different divisor. For example, core 0~3 are in cluster 1. Core 4~7
are in cluster 2. Core 8~11 are in cluster 3 and so on. Cluster 1 and 2
are cluster group A. Cluster 3 and 4 are in cluster group B. Cluster group
A has PLL1, PLL2, PLL3. Cluster group B has PLL4, PLL5. Core 0~3 may have
PLL1/2, core 4~7 may have PLL2/2. Core 8~11 may have PLL4/1.

PME and FMan blocks can take different PLLs, configured by RCW.

Signed-off-by: York Sun <yorksun@freescale.com>
Signed-off-by: Andy Fleming <afleming@freescale.com>

2 files changed, 148 insertions, 26 deletions

diff --git a/arch/powerpc/cpu/mpc85xx/speed.c b/arch/powerpc/cpu/mpc85xx/speed.c
index 6c2bc34714..f07a28b462 100644
--- a/arch/powerpc/cpu/mpc85xx/speed.c
+++ b/arch/powerpc/cpu/mpc85xx/speed.c
@@ -76,8 +76,8 @@ void get_sys_info (sys_info_t * sysInfo)
 		[13] = 2,	/* CC4 PPL / 2 */
 		[14] = 4,	/* CC4 PPL / 4 */
 	};
-	uint lcrr_div, i, freqCC_PLL[4], rcw_tmp;
-	uint ratio[4];
+	uint i, freqCC_PLL[6], rcw_tmp;
+	uint ratio[6];
 	unsigned long sysclk = CONFIG_SYS_CLK_FREQ;
 	uint mem_pll_rat;
 
@@ -97,21 +97,139 @@ void get_sys_info (sys_info_t * sysInfo)
 	ratio[1] = (in_be32(&clk->pllc2gsr) >> 1) & 0x3f;
 	ratio[2] = (in_be32(&clk->pllc3gsr) >> 1) & 0x3f;
 	ratio[3] = (in_be32(&clk->pllc4gsr) >> 1) & 0x3f;
-	for (i = 0; i < 4; i++) {
+	ratio[4] = (in_be32(&clk->pllc5gsr) >> 1) & 0x3f;
+	ratio[5] = (in_be32(&clk->pllc6gsr) >> 1) & 0x3f;
+	for (i = 0; i < 6; i++) {
 		if (ratio[i] > 4)
 			freqCC_PLL[i] = sysclk * ratio[i];
 		else
 			freqCC_PLL[i] = sysInfo->freqSystemBus * ratio[i];
 	}
-	rcw_tmp = in_be32(&gur->rcwsr[3]);
+#ifdef CONFIG_SYS_FSL_QORIQ_CHASSIS2
+	/*
+	 * Each cluster has up to 4 cores, sharing the same PLL selection.
+	 * The cluster assignment is fixed per SoC. There is no way identify the
+	 * assignment so far, presuming the "first configuration" which is to
+	 * fill the lower cluster group first before moving up to next group.
+	 * PLL1, PLL2, PLL3 are cluster group A, feeding core 0~3 on cluster 1
+	 * and core 4~7 on cluster 2
+	 * PLL4, PLL5, PLL6 are cluster group B, feeding core 8~11 on cluster 3
+	 * and core 12~15 on cluster 4 if existing
+	 */
 	for_each_cpu(i, cpu, cpu_numcores(), cpu_mask()) {
-		u32 c_pll_sel = (in_be32(&clk->clkc0csr + cpu*8) >> 27) & 0xf;
+		u32 c_pll_sel = (in_be32(&clk->clkc0csr + (cpu / 4) * 8) >> 27)
+				& 0xf;
 		u32 cplx_pll = core_cplx_PLL[c_pll_sel];
+		if (cplx_pll > 3)
+			printf("Unsupported architecture configuration"
+				" in function %s\n", __func__);
+		cplx_pll += (cpu / 8) * 3;
 
 		sysInfo->freqProcessor[cpu] =
 			 freqCC_PLL[cplx_pll] / core_cplx_PLL_div[c_pll_sel];
 	}
+#define PME_CLK_SEL	0xe0000000
+#define PME_CLK_SHIFT	29
+#define FM1_CLK_SEL	0x1c000000
+#define FM1_CLK_SHIFT	26
+	rcw_tmp = in_be32(&gur->rcwsr[7]);
+
+#ifdef CONFIG_SYS_DPAA_PME
+	switch ((rcw_tmp & PME_CLK_SEL) >> PME_CLK_SHIFT) {
+	case 1:
+		sysInfo->freqPME = freqCC_PLL[0];
+		break;
+	case 2:
+		sysInfo->freqPME = freqCC_PLL[0] / 2;
+		break;
+	case 3:
+		sysInfo->freqPME = freqCC_PLL[0] / 3;
+		break;
+	case 4:
+		sysInfo->freqPME = freqCC_PLL[0] / 4;
+		break;
+	case 6:
+		sysInfo->freqPME = freqCC_PLL[1] / 2;
+		break;
+	case 7:
+		sysInfo->freqPME = freqCC_PLL[1] / 3;
+		break;
+	default:
+		printf("Error: Unknown PME clock select!\n");
+	case 0:
+		sysInfo->freqPME = sysInfo->freqSystemBus / 2;
+		break;
+
+	}
+#endif
 
+#ifdef CONFIG_SYS_DPAA_FMAN
+	switch ((rcw_tmp & FM1_CLK_SEL) >> FM1_CLK_SHIFT) {
+	case 1:
+		sysInfo->freqFMan[0] = freqCC_PLL[3];
+		break;
+	case 2:
+		sysInfo->freqFMan[0] = freqCC_PLL[3] / 2;
+		break;
+	case 3:
+		sysInfo->freqFMan[0] = freqCC_PLL[3] / 3;
+		break;
+	case 4:
+		sysInfo->freqFMan[0] = freqCC_PLL[3] / 4;
+		break;
+	case 6:
+		sysInfo->freqFMan[0] = freqCC_PLL[4] / 2;
+		break;
+	case 7:
+		sysInfo->freqFMan[0] = freqCC_PLL[4] / 3;
+		break;
+	default:
+		printf("Error: Unknown FMan1 clock select!\n");
+	case 0:
+		sysInfo->freqFMan[0] = sysInfo->freqSystemBus / 2;
+		break;
+	}
+#if (CONFIG_SYS_NUM_FMAN) == 2
+#define FM2_CLK_SEL	0x00000038
+#define FM2_CLK_SHIFT	3
+	rcw_tmp = in_be32(&gur->rcwsr[15]);
+	switch ((rcw_tmp & FM2_CLK_SEL) >> FM2_CLK_SHIFT) {
+	case 1:
+		sysInfo->freqFMan[1] = freqCC_PLL[4];
+		break;
+	case 2:
+		sysInfo->freqFMan[1] = freqCC_PLL[4] / 2;
+		break;
+	case 3:
+		sysInfo->freqFMan[1] = freqCC_PLL[4] / 3;
+		break;
+	case 4:
+		sysInfo->freqFMan[1] = freqCC_PLL[4] / 4;
+		break;
+	case 6:
+		sysInfo->freqFMan[1] = freqCC_PLL[3] / 2;
+		break;
+	case 7:
+		sysInfo->freqFMan[1] = freqCC_PLL[3] / 3;
+		break;
+	default:
+		printf("Error: Unknown FMan2 clock select!\n");
+	case 0:
+		sysInfo->freqFMan[1] = sysInfo->freqSystemBus / 2;
+		break;
+	}
+#endif	/* CONFIG_SYS_NUM_FMAN == 2 */
+#endif	/* CONFIG_SYS_DPAA_FMAN */
+
+#else /* CONFIG_SYS_FSL_QORIQ_CHASSIS2 */
+
+	for_each_cpu(i, cpu, cpu_numcores(), cpu_mask()) {
+		u32 c_pll_sel = (in_be32(&clk->clkc0csr + cpu*8) >> 27) & 0xf;
+		u32 cplx_pll = core_cplx_PLL[c_pll_sel];
+
+		sysInfo->freqProcessor[cpu] =
+			 freqCC_PLL[cplx_pll] / core_cplx_PLL_div[c_pll_sel];
+	}
 #define PME_CLK_SEL	0x80000000
 #define FM1_CLK_SEL	0x40000000
 #define FM2_CLK_SEL	0x20000000
@@ -159,11 +277,10 @@ void get_sys_info (sys_info_t * sysInfo)
 #endif
 #endif
 
-#else
-	uint plat_ratio,e500_ratio,half_freqSystemBus;
-#if defined(CONFIG_FSL_LBC)
-	uint lcrr_div;
-#endif
+#endif /* CONFIG_SYS_FSL_QORIQ_CHASSIS2 */
+
+#else /* CONFIG_FSL_CORENET */
+	uint plat_ratio, e500_ratio, half_freqSystemBus;
 	int i;
 #ifdef CONFIG_QE
 	__maybe_unused u32 qe_ratio;
@@ -210,6 +327,7 @@ void get_sys_info (sys_info_t * sysInfo)
 #endif /* CONFIG_FSL_CORENET */
 
 #if defined(CONFIG_FSL_LBC)
+	uint lcrr_div;
 #if defined(CONFIG_SYS_LBC_LCRR)
 	/* We will program LCRR to this value later */
 	lcrr_div = CONFIG_SYS_LBC_LCRR & LCRR_CLKDIV;
diff --git a/arch/powerpc/include/asm/immap_85xx.h b/arch/powerpc/include/asm/immap_85xx.h
index d0824b36e5..f7240b0f08 100644
--- a/arch/powerpc/include/asm/immap_85xx.h
+++ b/arch/powerpc/include/asm/immap_85xx.h
@@ -1899,34 +1899,38 @@ typedef struct ccsr_gur {
 #define rmuliodnr rio1maintliodnr
 
 typedef struct ccsr_clk {
-	u32	clkc0csr;	/* Core 0 Clock control/status */
+	u32	clkc0csr;	/* 0x000 Core 0 Clock control/status */
 	u8	res1[0x1c];
-	u32	clkc1csr;	/* Core 1 Clock control/status */
+	u32	clkc1csr;	/* 0x020 Core 1 Clock control/status */
 	u8	res2[0x1c];
-	u32	clkc2csr;	/* Core 2 Clock control/status */
+	u32	clkc2csr;	/* 0x040 Core 2 Clock control/status */
 	u8	res3[0x1c];
-	u32	clkc3csr;	/* Core 3 Clock control/status */
+	u32	clkc3csr;	/* 0x060 Core 3 Clock control/status */
 	u8	res4[0x1c];
-	u32	clkc4csr;	/* Core 4 Clock control/status */
+	u32	clkc4csr;	/* 0x080 Core 4 Clock control/status */
 	u8	res5[0x1c];
-	u32	clkc5csr;	/* Core 5 Clock control/status */
+	u32	clkc5csr;	/* 0x0a0 Core 5 Clock control/status */
 	u8	res6[0x1c];
-	u32	clkc6csr;	/* Core 6 Clock control/status */
+	u32	clkc6csr;	/* 0x0c0 Core 6 Clock control/status */
 	u8	res7[0x1c];
-	u32	clkc7csr;	/* Core 7 Clock control/status */
+	u32	clkc7csr;	/* 0x0e0 Core 7 Clock control/status */
 	u8	res8[0x71c];
-	u32	pllc1gsr;	/* Cluster PLL 1 General Status */
+	u32	pllc1gsr;	/* 0x800 Cluster PLL 1 General Status */
 	u8	res10[0x1c];
-	u32	pllc2gsr;	/* Cluster PLL 2 General Status */
+	u32	pllc2gsr;	/* 0x820 Cluster PLL 2 General Status */
 	u8	res11[0x1c];
-	u32	pllc3gsr;	/* Cluster PLL 3 General Status */
+	u32	pllc3gsr;	/* 0x840 Cluster PLL 3 General Status */
 	u8	res12[0x1c];
-	u32	pllc4gsr;	/* Cluster PLL 4 General Status */
-	u8	res13[0x39c];
-	u32	pllpgsr;	/* Platform PLL General Status */
+	u32	pllc4gsr;	/* 0x860 Cluster PLL 4 General Status */
+	u8	res13[0x1c];
+	u32	pllc5gsr;	/* 0x880 Cluster PLL 5 General Status */
 	u8	res14[0x1c];
-	u32	plldgsr;	/* DDR PLL General Status */
-	u8	res15[0x3dc];
+	u32	pllc6gsr;	/* 0x8a0 Cluster PLL 6 General Status */
+	u8	res15[0x35c];
+	u32	pllpgsr;	/* 0xc00 Platform PLL General Status */
+	u8	res16[0x1c];
+	u32	plldgsr;	/* 0xc20 DDR PLL General Status */
+	u8	res17[0x3dc];
 } ccsr_clk_t;
 
 #ifdef CONFIG_SYS_FSL_QORIQ_CHASSIS2