1 files changed, 278 insertions, 47 deletions

diff --git a/arch/arm/mach-imx/imx8ulp/cgc.c b/arch/arm/mach-imx/imx8ulp/cgc.c
index 7bfc3862cd..38bcbb91e6 100644
--- a/arch/arm/mach-imx/imx8ulp/cgc.c
+++ b/arch/arm/mach-imx/imx8ulp/cgc.c
@@ -189,8 +189,8 @@ void cgc2_pll4_init(void)
 		;
 
 	/* Enable all 4 PFDs */
-	setbits_le32(&cgc2_regs->pll4pfdcfg, 30 << 0); /* 316.8Mhz for NIC_LPAV */
-	setbits_le32(&cgc2_regs->pll4pfdcfg, 18 << 8);
+	setbits_le32(&cgc2_regs->pll4pfdcfg, 18 << 0);
+	setbits_le32(&cgc2_regs->pll4pfdcfg, 30 << 8); /* 316.8Mhz for NIC_LPAV */
 	setbits_le32(&cgc2_regs->pll4pfdcfg, 12 << 16);
 	setbits_le32(&cgc2_regs->pll4pfdcfg, 24 << 24);
 
@@ -205,15 +205,144 @@ void cgc2_pll4_init(void)
 	clrbits_le32(&cgc2_regs->pll4div_pfd1, BIT(7) | BIT(15) | BIT(23) | BIT(31));
 }
 
+void cgc2_pll4_pfd_config(enum cgc_clk pllpfd, u32 pfd)
+{
+	void __iomem *reg = &cgc2_regs->pll4div_pfd0;
+	u32 halt_mask = BIT(7) | BIT(15);
+	u32 pfd_shift = (pllpfd - PLL4_PFD0) * 8;
+	u32 val;
+
+	if (pllpfd < PLL4_PFD0 || pllpfd > PLL4_PFD3)
+		return;
+
+	if ((pllpfd - PLL4_PFD0) >> 1)
+		reg = &cgc2_regs->pll4div_pfd1;
+
+	halt_mask = halt_mask << (((pllpfd - PLL4_PFD0) & 0x1) * 16);
+
+	/* halt pfd div */
+	setbits_le32(reg, halt_mask);
+
+	/* gate pfd */
+	setbits_le32(&cgc2_regs->pll4pfdcfg, BIT(7) << pfd_shift);
+
+	val = readl(&cgc2_regs->pll4pfdcfg);
+	val &= ~(0x3f << pfd_shift);
+	val |= (pfd << pfd_shift);
+	writel(val, &cgc2_regs->pll4pfdcfg);
+
+	/* ungate */
+	clrbits_le32(&cgc2_regs->pll4pfdcfg, BIT(7) << pfd_shift);
+
+	/* Wait stable */
+	while ((readl(&cgc2_regs->pll4pfdcfg) & (BIT(6) << pfd_shift))
+		!= (BIT(6) << pfd_shift))
+		;
+
+	/* enable pfd div */
+	clrbits_le32(reg, halt_mask);
+}
+
+void cgc2_pll4_pfddiv_config(enum cgc_clk pllpfddiv, u32 div)
+{
+	void __iomem *reg = &cgc2_regs->pll4div_pfd0;
+	u32 shift = ((pllpfddiv - PLL4_PFD0_DIV1) & 0x3) * 8;
+
+	if (pllpfddiv < PLL4_PFD0_DIV1 || pllpfddiv > PLL4_PFD3_DIV2)
+		return;
+
+	if ((pllpfddiv - PLL4_PFD0_DIV1) >> 2)
+		reg = &cgc2_regs->pll4div_pfd1;
+
+	/* Halt pfd div */
+	setbits_le32(reg, BIT(7) << shift);
+
+	/* Clear div */
+	clrbits_le32(reg, 0x3f << shift);
+
+	/* Set div*/
+	setbits_le32(reg, div << shift);
+
+	/* Enable pfd div */
+	clrbits_le32(reg, BIT(7) << shift);
+}
+
 void cgc2_ddrclk_config(u32 src, u32 div)
 {
+	/* If reg lock is set, wait until unlock by HW */
+	/* This lock is triggered by div updating and ddrclk halt status change, */
+	while ((readl(&cgc2_regs->ddrclk) & BIT(31)))
+		;
+
 	writel((src << 28) | (div << 21), &cgc2_regs->ddrclk);
 	/* wait for DDRCLK switching done */
 	while (!(readl(&cgc2_regs->ddrclk) & BIT(27)))
 		;
 }
 
-u32 decode_pll(enum cgc1_clk pll)
+void cgc2_ddrclk_wait_unlock(void)
+{
+	while ((readl(&cgc2_regs->ddrclk) & BIT(31)))
+		;
+}
+
+void cgc2_lpav_init(enum cgc_clk clk)
+{
+	u32 i, scs, reg;
+	const enum cgc_clk src[] = {FRO, PLL4_PFD1, SOSC, LVDS};
+
+	reg = readl(&cgc2_regs->niclpavclk);
+	scs = (reg >> 28) & 0x3;
+
+	for (i = 0; i < 4; i++) {
+		if (clk == src[i]) {
+			if (scs == i)
+				return;
+
+			reg &= ~(0x3 << 28);
+			reg |= (i << 28);
+
+			writel(reg, &cgc2_regs->niclpavclk);
+			break;
+		}
+	}
+
+	if (i == 4)
+		printf("Invalid clock source [%u] for LPAV\n", clk);
+}
+
+u32 cgc2_nic_get_rate(enum cgc_clk clk)
+{
+	u32 reg, rate;
+	u32 scs, lpav_axi_clk, lpav_ahb_clk, lpav_bus_clk;
+	const enum cgc_clk src[] = {FRO, PLL4_PFD1, SOSC, LVDS};
+
+	reg = readl(&cgc2_regs->niclpavclk);
+	scs = (reg >> 28) & 0x3;
+	lpav_axi_clk = ((reg >> 21) & 0x3f) + 1;
+	lpav_ahb_clk = ((reg >> 14) & 0x3f) + 1;
+	lpav_bus_clk = ((reg >> 7) & 0x3f) + 1;
+
+	rate = cgc_clk_get_rate(src[scs]);
+
+	switch (clk) {
+	case LPAV_AXICLK:
+		rate = rate / lpav_axi_clk;
+		break;
+	case LPAV_AHBCLK:
+		rate = rate / (lpav_axi_clk * lpav_ahb_clk);
+		break;
+	case LPAV_BUSCLK:
+		rate = rate / (lpav_axi_clk * lpav_bus_clk);
+		break;
+	default:
+		return 0;
+	}
+
+	return rate;
+}
+
+u32 decode_pll(enum cgc_clk pll)
 {
 	u32 reg, infreq, mult;
 	u32 num, denom;
@@ -247,6 +376,17 @@ u32 decode_pll(enum cgc1_clk pll)
 		num = readl(&cgc1_regs->pll3num) & 0x3FFFFFFF;
 
 		return (u64)infreq * mult + (u64)infreq * num / denom;
+	case PLL4:
+		reg = readl(&cgc2_regs->pll4csr);
+		if (!(reg & BIT(24)))
+			return 0;
+
+		reg = readl(&cgc2_regs->pll4cfg);
+		mult = (reg >> 16) & 0x7F;
+		denom = readl(&cgc2_regs->pll4denom) & 0x3FFFFFFF;
+		num = readl(&cgc2_regs->pll4num) & 0x3FFFFFFF;
+
+		return (u64)infreq * mult + (u64)infreq * num / denom;
 	default:
 		printf("Unsupported pll clocks %d\n", pll);
 		break;
@@ -255,93 +395,117 @@ u32 decode_pll(enum cgc1_clk pll)
 	return 0;
 }
 
-u32 cgc1_pll3_vcodiv_rate(void)
+u32 cgc_pll_vcodiv_rate(enum cgc_clk clk)
 {
 	u32 reg, gate, div;
+	void __iomem *plldiv_vco;
+	enum cgc_clk pll;
+
+	if (clk == PLL3_VCODIV) {
+		plldiv_vco = &cgc1_regs->pll3div_vco;
+		pll = PLL3;
+	} else {
+		plldiv_vco = &cgc2_regs->pll4div_vco;
+		pll = PLL4;
+	}
 
-	reg = readl(&cgc1_regs->pll3div_vco);
+	reg = readl(plldiv_vco);
 	gate = BIT(7) & reg;
 	div = reg & 0x3F;
 
-	return gate ? 0 : decode_pll(PLL3) / (div + 1);
+	return gate ? 0 : decode_pll(pll) / (div + 1);
 }
 
-u32 cgc1_pll3_pfd_rate(enum cgc1_clk clk)
+u32 cgc_pll_pfd_rate(enum cgc_clk clk)
 {
 	u32 index, gate, vld, reg;
+	void __iomem *pllpfdcfg;
+	enum cgc_clk pll;
 
 	switch (clk) {
 	case PLL3_PFD0:
-		index = 0;
-		break;
 	case PLL3_PFD1:
-		index = 1;
-		break;
 	case PLL3_PFD2:
-		index = 2;
-		break;
 	case PLL3_PFD3:
-		index = 3;
+		index = clk - PLL3_PFD0;
+		pllpfdcfg = &cgc1_regs->pll3pfdcfg;
+		pll = PLL3;
+		break;
+	case PLL4_PFD0:
+	case PLL4_PFD1:
+	case PLL4_PFD2:
+	case PLL4_PFD3:
+		index = clk - PLL4_PFD0;
+		pllpfdcfg = &cgc2_regs->pll4pfdcfg;
+		pll = PLL4;
 		break;
 	default:
 		return 0;
 	}
 
-	reg = readl(&cgc1_regs->pll3pfdcfg);
+	reg = readl(pllpfdcfg);
 	gate = reg & (BIT(7) << (index * 8));
 	vld = reg & (BIT(6) << (index * 8));
 
 	if (gate || !vld)
 		return 0;
 
-	return (u64)decode_pll(PLL3) * 18 / ((reg >> (index * 8)) & 0x3F);
+	return (u64)decode_pll(pll) * 18 / ((reg >> (index * 8)) & 0x3F);
 }
 
-u32 cgc1_pll3_pfd_div(enum cgc1_clk clk)
+u32 cgc_pll_pfd_div(enum cgc_clk clk)
 {
 	void __iomem *base;
 	u32 pfd, index, gate, reg;
 
 	switch (clk) {
 	case PLL3_PFD0_DIV1:
-		base = &cgc1_regs->pll3div_pfd0;
-		pfd = PLL3_PFD0;
-		index = 0;
-		break;
 	case PLL3_PFD0_DIV2:
 		base = &cgc1_regs->pll3div_pfd0;
 		pfd = PLL3_PFD0;
-		index = 1;
+		index = clk - PLL3_PFD0_DIV1;
 		break;
 	case PLL3_PFD1_DIV1:
-		base = &cgc1_regs->pll3div_pfd0;
-		pfd = PLL3_PFD1;
-		index = 2;
-		break;
 	case PLL3_PFD1_DIV2:
 		base = &cgc1_regs->pll3div_pfd0;
 		pfd = PLL3_PFD1;
-		index = 3;
+		index = clk - PLL3_PFD0_DIV1;
 		break;
 	case PLL3_PFD2_DIV1:
-		base = &cgc1_regs->pll3div_pfd1;
-		pfd = PLL3_PFD2;
-		index = 0;
-		break;
 	case PLL3_PFD2_DIV2:
 		base = &cgc1_regs->pll3div_pfd1;
 		pfd = PLL3_PFD2;
-		index = 1;
+		index = clk - PLL3_PFD2_DIV1;
 		break;
 	case PLL3_PFD3_DIV1:
-		base = &cgc1_regs->pll3div_pfd1;
-		pfd = PLL3_PFD3;
-		index = 2;
-		break;
 	case PLL3_PFD3_DIV2:
 		base = &cgc1_regs->pll3div_pfd1;
 		pfd = PLL3_PFD3;
-		index = 3;
+		index = clk - PLL3_PFD2_DIV1;
+		break;
+	case PLL4_PFD0_DIV1:
+	case PLL4_PFD0_DIV2:
+		base = &cgc2_regs->pll4div_pfd0;
+		pfd = PLL4_PFD0;
+		index = clk - PLL4_PFD0_DIV1;
+		break;
+	case PLL4_PFD1_DIV1:
+	case PLL4_PFD1_DIV2:
+		base = &cgc2_regs->pll4div_pfd0;
+		pfd = PLL4_PFD1;
+		index = clk - PLL4_PFD0_DIV1;
+		break;
+	case PLL4_PFD2_DIV1:
+	case PLL4_PFD2_DIV2:
+		base = &cgc2_regs->pll4div_pfd1;
+		pfd = PLL4_PFD2;
+		index = clk - PLL4_PFD2_DIV1;
+		break;
+	case PLL4_PFD3_DIV1:
+	case PLL4_PFD3_DIV2:
+		base = &cgc2_regs->pll4div_pfd1;
+		pfd = PLL4_PFD3;
+		index = clk - PLL4_PFD2_DIV1;
 		break;
 	default:
 		return 0;
@@ -353,10 +517,52 @@ u32 cgc1_pll3_pfd_div(enum cgc1_clk clk)
 	if (gate)
 		return 0;
 
-	return cgc1_pll3_pfd_rate(pfd) / (((reg >> (index * 8)) & 0x3F) + 1);
+	return cgc_pll_pfd_rate(pfd) / (((reg >> (index * 8)) & 0x3F) + 1);
+}
+
+u32 cgc1_nic_get_rate(enum cgc_clk clk)
+{
+	u32 reg, rate;
+	u32 scs, nic_ad_divplat, nic_per_divplat;
+	u32 xbar_ad_divplat, xbar_divbus, ad_slow;
+	const enum cgc_clk src[] = {FRO, PLL3_PFD0, SOSC, LVDS};
+
+	reg = readl(&cgc1_regs->nicclk);
+	scs = (reg >> 28) & 0x3;
+	nic_ad_divplat = ((reg >> 21) & 0x3f) + 1;
+	nic_per_divplat = ((reg >> 14) & 0x3f) + 1;
+
+	reg = readl(&cgc1_regs->xbarclk);
+	xbar_ad_divplat = ((reg >> 14) & 0x3f) + 1;
+	xbar_divbus = ((reg >> 7) & 0x3f) + 1;
+	ad_slow = (reg & 0x3f) + 1;
+
+	rate = cgc_clk_get_rate(src[scs]);
+
+	switch (clk) {
+	case NIC_APCLK:
+		rate = rate / nic_ad_divplat;
+		break;
+	case NIC_PERCLK:
+		rate = rate / (nic_ad_divplat * nic_per_divplat);
+		break;
+	case XBAR_APCLK:
+		rate = rate / (nic_ad_divplat * xbar_ad_divplat);
+		break;
+	case XBAR_BUSCLK:
+		rate = rate / (nic_ad_divplat * xbar_ad_divplat * xbar_divbus);
+		break;
+	case AD_SLOWCLK:
+		rate = rate / (nic_ad_divplat * xbar_ad_divplat * ad_slow);
+		break;
+	default:
+		return 0;
+	}
+
+	return rate;
 }
 
-u32 cgc1_sosc_div(enum cgc1_clk clk)
+u32 cgc1_sosc_div(enum cgc_clk clk)
 {
 	u32 reg, gate, index;
 
@@ -385,7 +591,7 @@ u32 cgc1_sosc_div(enum cgc1_clk clk)
 	return 24000000 / (((reg >> (index * 8)) & 0x3F) + 1);
 }
 
-u32 cgc1_fro_div(enum cgc1_clk clk)
+u32 cgc1_fro_div(enum cgc_clk clk)
 {
 	u32 reg, gate, vld, index;
 
@@ -415,9 +621,11 @@ u32 cgc1_fro_div(enum cgc1_clk clk)
 	return 24000000 / (((reg >> (index * 8)) & 0x3F) + 1);
 }
 
-u32 cgc1_clk_get_rate(enum cgc1_clk clk)
+u32 cgc_clk_get_rate(enum cgc_clk clk)
 {
 	switch (clk) {
+	case LVDS:
+		return 0; /* No external LVDS clock used */
 	case SOSC:
 	case SOSC_DIV1:
 	case SOSC_DIV2:
@@ -429,16 +637,21 @@ u32 cgc1_clk_get_rate(enum cgc1_clk clk)
 	case FRO_DIV3:
 		return cgc1_fro_div(clk);
 	case PLL2:
-		return decode_pll(PLL2);
 	case PLL3:
-		return decode_pll(PLL3);
+	case PLL4:
+		return decode_pll(clk);
 	case PLL3_VCODIV:
-		return cgc1_pll3_vcodiv_rate();
+	case PLL4_VCODIV:
+		return cgc_pll_vcodiv_rate(clk);
 	case PLL3_PFD0:
 	case PLL3_PFD1:
 	case PLL3_PFD2:
 	case PLL3_PFD3:
-		return cgc1_pll3_pfd_rate(clk);
+	case PLL4_PFD0:
+	case PLL4_PFD1:
+	case PLL4_PFD2:
+	case PLL4_PFD3:
+		return cgc_pll_pfd_rate(clk);
 	case PLL3_PFD0_DIV1:
 	case PLL3_PFD0_DIV2:
 	case PLL3_PFD1_DIV1:
@@ -447,9 +660,27 @@ u32 cgc1_clk_get_rate(enum cgc1_clk clk)
 	case PLL3_PFD2_DIV2:
 	case PLL3_PFD3_DIV1:
 	case PLL3_PFD3_DIV2:
-		return cgc1_pll3_pfd_div(clk);
+	case PLL4_PFD0_DIV1:
+	case PLL4_PFD0_DIV2:
+	case PLL4_PFD1_DIV1:
+	case PLL4_PFD1_DIV2:
+	case PLL4_PFD2_DIV1:
+	case PLL4_PFD2_DIV2:
+	case PLL4_PFD3_DIV1:
+	case PLL4_PFD3_DIV2:
+		return cgc_pll_pfd_div(clk);
+	case NIC_APCLK:
+	case NIC_PERCLK:
+	case XBAR_APCLK:
+	case XBAR_BUSCLK:
+	case AD_SLOWCLK:
+		return cgc1_nic_get_rate(clk);
+	case LPAV_AXICLK:
+	case LPAV_AHBCLK:
+	case LPAV_BUSCLK:
+		return cgc2_nic_get_rate(clk);
 	default:
-		printf("Unsupported cgc1 clock: %d\n", clk);
+		printf("Unsupported cgc clock: %d\n", clk);
 		return 0;
 	}
 }