X-Git-Url: https://git.kernelconcepts.de/?p=karo-tx-uboot.git;a=blobdiff_plain;f=arch%2Farm%2Fcpu%2Farmv7%2Fmx6%2Fsoc.c;h=dd5aaa286a68d708fda6814e6d107adba4d70231;hp=05683a3400cbcb55d436c6128d8d6262bc5db64f;hb=0ce4af99c07acebf4fce9a91f1099d2460629293;hpb=36c1ca4d46ef11ac7b3c0afb5c42dadb4e8773f3

diff --git a/arch/arm/cpu/armv7/mx6/soc.c b/arch/arm/cpu/armv7/mx6/soc.c
index 05683a3400..dd5aaa286a 100644
--- a/arch/arm/cpu/armv7/mx6/soc.c
+++ b/arch/arm/cpu/armv7/mx6/soc.c
@@ -9,6 +9,7 @@
 
 #include <common.h>
 #include <asm/armv7.h>
+#include <asm/bootm.h>
 #include <asm/pl310.h>
 #include <asm/errno.h>
 #include <asm/io.h>
@@ -20,6 +21,7 @@
 #include <stdbool.h>
 #include <asm/arch/mxc_hdmi.h>
 #include <asm/arch/crm_regs.h>
+#include <asm/bootm.h>
 
 enum ldo_reg {
 	LDO_ARM,
@@ -354,10 +356,10 @@ const struct boot_mode soc_boot_modes[] = {
 void s_init(void)
 {
 	struct anatop_regs *anatop = (struct anatop_regs *)ANATOP_BASE_ADDR;
-	int is_6q = is_cpu_type(MXC_CPU_MX6Q);
+	struct mxc_ccm_reg *ccm = (struct mxc_ccm_reg *)CCM_BASE_ADDR;
 	u32 mask480;
 	u32 mask528;
-
+	u32 reg, periph1, periph2;
 
 	if (is_cpu_type(MXC_CPU_MX6SX))
 		return;
@@ -372,15 +374,23 @@ void s_init(void)
 		ANATOP_PFD_CLKGATE_MASK(1) |
 		ANATOP_PFD_CLKGATE_MASK(2) |
 		ANATOP_PFD_CLKGATE_MASK(3);
-	mask528 = ANATOP_PFD_CLKGATE_MASK(0) |
-		ANATOP_PFD_CLKGATE_MASK(1) |
+	mask528 = ANATOP_PFD_CLKGATE_MASK(1) |
 		ANATOP_PFD_CLKGATE_MASK(3);
 
-	/*
-	 * Don't reset PFD2 on DL/S
-	 */
-	if (is_6q)
+	reg = readl(&ccm->cbcmr);
+	periph2 = ((reg & MXC_CCM_CBCMR_PRE_PERIPH2_CLK_SEL_MASK)
+		>> MXC_CCM_CBCMR_PRE_PERIPH2_CLK_SEL_OFFSET);
+	periph1 = ((reg & MXC_CCM_CBCMR_PRE_PERIPH_CLK_SEL_MASK)
+		>> MXC_CCM_CBCMR_PRE_PERIPH_CLK_SEL_OFFSET);
+
+	/* Checking if PLL2 PFD0 or PLL2 PFD2 is using for periph clock */
+	if ((periph2 != 0x2) && (periph1 != 0x2))
+		mask528 |= ANATOP_PFD_CLKGATE_MASK(0);
+
+	if ((periph2 != 0x1) && (periph1 != 0x1) &&
+		(periph2 != 0x3) && (periph1 != 0x3))
 		mask528 |= ANATOP_PFD_CLKGATE_MASK(2);
+
 	writel(mask480, &anatop->pfd_480_set);
 	writel(mask528, &anatop->pfd_528_set);
 	writel(mask480, &anatop->pfd_480_clr);