[U-Boot] [PATCH 04/10] mx5 clocks: Fix get_lp_apm()

[Benoît Thébaudeau]

Signed-off-by: Benoît Thébaudeau <benoit.thebaudeau at advansee.com>
Cc: Stefano Babic <sbabic at denx.de>
---
 .../arch/arm/cpu/armv7/mx5/clock.c                 |   24 +++++++++++++++++++-
 .../arch/arm/include/asm/arch-mx5/crm_regs.h       |   13 +++++++++++
 2 files changed, 36 insertions(+), 1 deletion(-)

diff --git u-boot-4d3c95f.orig/arch/arm/cpu/armv7/mx5/clock.c u-boot-4d3c95f/arch/arm/cpu/armv7/mx5/clock.c
index 10843a4..408fb54 100644
--- u-boot-4d3c95f.orig/arch/arm/cpu/armv7/mx5/clock.c
+++ u-boot-4d3c95f/arch/arm/cpu/armv7/mx5/clock.c
@@ -262,6 +262,24 @@ static uint32_t decode_pll(struct mxc_pll_reg *pll, uint32_t infreq)
 	return ret;
 }
 
+#ifdef CONFIG_MX51
+/*
+ * This function returns the Frequency Pre-Multiplier clock.
+ */
+static u32 get_fpm(void)
+{
+	u32 mult;
+	u32 ccr = __raw_readl(&mxc_ccm->ccr);
+
+	if (ccr & MXC_CCM_CCR_FPM_MULT)
+		mult = 1024;
+	else
+		mult = 512;
+
+	return CONFIG_SYS_MX5_CLK32 * mult;
+}
+#endif
+
 /*
  * Get mcu main rate
  */
@@ -382,7 +400,11 @@ static u32 get_lp_apm(void)
 	u32 ccsr = __raw_readl(&mxc_ccm->ccsr);
 
 	if (ccsr & MXC_CCM_CCSR_LP_APM)
-		ret_val = 32768 * 1024;
+#if defined(CONFIG_MX51)
+		ret_val = get_fpm();
+#elif defined(CONFIG_MX53)
+		ret_val = decode_pll(mxc_plls[PLL4_CLOCK], CONFIG_SYS_MX5_HCLK);
+#endif
 	else
 		ret_val = CONFIG_SYS_MX5_HCLK;
 
diff --git u-boot-4d3c95f.orig/arch/arm/include/asm/arch-mx5/crm_regs.h u-boot-4d3c95f/arch/arm/include/asm/arch-mx5/crm_regs.h
index 7c21351..c208de9 100644
--- u-boot-4d3c95f.orig/arch/arm/include/asm/arch-mx5/crm_regs.h
+++ u-boot-4d3c95f/arch/arm/include/asm/arch-mx5/crm_regs.h
@@ -82,6 +82,19 @@ struct mxc_ccm_reg {
 	u32 cmeor;
 };
 
+/* Define the bits in register CCR */
+#define MXC_CCM_CCR_COSC_EN			(0x1 << 12)
+#if defined(CONFIG_MX51)
+#define MXC_CCM_CCR_FPM_MULT			(0x1 << 11)
+#endif
+#define MXC_CCM_CCR_CAMP2_EN			(0x1 << 10)
+#define MXC_CCM_CCR_CAMP1_EN			(0x1 << 9)
+#if defined(CONFIG_MX51)
+#define MXC_CCM_CCR_FPM_EN			(0x1 << 8)
+#endif
+#define MXC_CCM_CCR_OSCNT_OFFSET		0
+#define MXC_CCM_CCR_OSCNT_MASK			0xFF
+
 /* Define the bits in register CCSR */
 #if defined(CONFIG_MX51)
 #define MXC_CCM_CCSR_LP_APM			(0x1 << 9)