cpufreq for freescale mx51

Currently, only two operating points: 160Mhz and 800Mhz.
the operating points are tested on babbage 3.0

Signed-off-by: Yong Shen <yong.shen@linaro.org>
Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>

6 files changed, 70 insertions, 3 deletions

diff --git a/arch/arm/mach-mx5/Kconfig b/arch/arm/mach-mx5/Kconfig
index fe96cd25de83..3ec910a7a182 100644
--- a/arch/arm/mach-mx5/Kconfig
+++ b/arch/arm/mach-mx5/Kconfig
@@ -6,6 +6,7 @@ config ARCH_MX51
         select MXC_TZIC
         select ARCH_MXC_IOMUX_V3
         select ARCH_MXC_AUDMUX_V2
+        select ARCH_HAS_CPUFREQ
 
 comment "MX5 platforms:"
 
diff --git a/arch/arm/mach-mx5/Makefile b/arch/arm/mach-mx5/Makefile
index 1769c161a60d..462f177eddfe 100644
--- a/arch/arm/mach-mx5/Makefile
+++ b/arch/arm/mach-mx5/Makefile
@@ -5,6 +5,7 @@
 # Object file lists.
 obj-y   := cpu.o mm.o clock-mx51.o devices.o
 
+obj-$(CONFIG_CPU_FREQ_IMX)    += cpu_op-mx51.o
 obj-$(CONFIG_MACH_MX51_BABBAGE) += board-mx51_babbage.o
 obj-$(CONFIG_MACH_MX51_3DS) += board-mx51_3ds.o
 obj-$(CONFIG_MACH_EUKREA_CPUIMX51) += board-cpuimx51.o
diff --git a/arch/arm/mach-mx5/board-mx51_babbage.c b/arch/arm/mach-mx5/board-mx51_babbage.c
index 39304b4f0069..cbf07ff34d2b 100644
--- a/arch/arm/mach-mx5/board-mx51_babbage.c
+++ b/arch/arm/mach-mx5/board-mx51_babbage.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2009-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  * Copyright (C) 2009-2010 Amit Kucheria <amit.kucheria@canonical.com>
  *
  * The code contained herein is licensed under the GNU General Public
@@ -32,6 +32,7 @@
 
 #include "devices-imx51.h"
 #include "devices.h"
+#include "cpu_op-mx51.h"
 
 #define BABBAGE_USB_HUB_RESET   (0*32 + 7)      /* GPIO_1_7 */
 #define BABBAGE_USBH1_STP       (0*32 + 27)     /* GPIO_1_27 */
@@ -298,6 +299,9 @@ static void __init mxc_board_init(void)
 {
         struct pad_desc usbh1stp = MX51_PAD_USBH1_STP__USBH1_STP;
 
+#if defined(CONFIG_CPU_FREQ_IMX)
+        get_cpu_op = mx51_get_cpu_op;
+#endif
         mxc_iomux_v3_setup_multiple_pads(mx51babbage_pads,
                                         ARRAY_SIZE(mx51babbage_pads));
         mxc_init_imx_uart();
diff --git a/arch/arm/mach-mx5/clock-mx51.c b/arch/arm/mach-mx5/clock-mx51.c
index f2aae92cf0e2..8ac36d882927 100644
--- a/arch/arm/mach-mx5/clock-mx51.c
+++ b/arch/arm/mach-mx5/clock-mx51.c
@@ -362,7 +362,7 @@ static int _clk_lp_apm_set_parent(struct clk *clk, struct clk *parent)
         return 0;
 }
 
-static unsigned long clk_arm_get_rate(struct clk *clk)
+static unsigned long clk_cpu_get_rate(struct clk *clk)
 {
         u32 cacrr, div;
         unsigned long parent_rate;
@@ -374,6 +374,22 @@ static unsigned long clk_arm_get_rate(struct clk *clk)
         return parent_rate / div;
 }
 
+static int clk_cpu_set_rate(struct clk *clk, unsigned long rate)
+{
+        u32 reg, cpu_podf;
+        unsigned long parent_rate;
+
+        parent_rate = clk_get_rate(clk->parent);
+        cpu_podf = parent_rate / rate - 1;
+        /* use post divider to change freq */
+        reg = __raw_readl(MXC_CCM_CACRR);
+        reg &= ~MXC_CCM_CACRR_ARM_PODF_MASK;
+        reg |= cpu_podf << MXC_CCM_CACRR_ARM_PODF_OFFSET;
+        __raw_writel(reg, MXC_CCM_CACRR);
+
+        return 0;
+}
+
 static int _clk_periph_apm_set_parent(struct clk *clk, struct clk *parent)
 {
         u32 reg, mux;
@@ -736,7 +752,8 @@ static struct clk periph_apm_clk = {
 
 static struct clk cpu_clk = {
         .parent = &pll1_sw_clk,
-        .get_rate = clk_arm_get_rate,
+        .get_rate = clk_cpu_get_rate,
+        .set_rate = clk_cpu_set_rate,
 };
 
 static struct clk ahb_clk = {
@@ -1064,6 +1081,7 @@ static struct clk_lookup lookups[] = {
         _REGISTER_CLOCK("imx51-cspi.0", NULL, cspi_clk)
         _REGISTER_CLOCK("sdhci-esdhc-imx.0", NULL, esdhc1_clk)
         _REGISTER_CLOCK("sdhci-esdhc-imx.1", NULL, esdhc2_clk)
+        _REGISTER_CLOCK(NULL, "cpu_clk", cpu_clk)
 };
 
 static void clk_tree_init(void)
diff --git a/arch/arm/mach-mx5/cpu_op-mx51.c b/arch/arm/mach-mx5/cpu_op-mx51.c
new file mode 100644
index 000000000000..9d34c3d4c024
--- /dev/null
+++ b/arch/arm/mach-mx5/cpu_op-mx51.c
@@ -0,0 +1,29 @@
+/*
+ * Copyright (C) 2010 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/types.h>
+#include <mach/hardware.h>
+#include <linux/kernel.h>
+
+static struct cpu_op mx51_cpu_op[] = {
+        {
+        .cpu_rate = 160000000,},
+        {
+        .cpu_rate = 800000000,},
+};
+
+struct cpu_op *mx51_get_cpu_op(int *op)
+{
+        *op = ARRAY_SIZE(mx51_cpu_op);
+        return mx51_cpu_op;
+}
diff --git a/arch/arm/mach-mx5/cpu_op-mx51.h b/arch/arm/mach-mx5/cpu_op-mx51.h
new file mode 100644
index 000000000000..97477fecb469
--- /dev/null
+++ b/arch/arm/mach-mx5/cpu_op-mx51.h
@@ -0,0 +1,14 @@
+/*
+ * Copyright (C) 2010 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+extern struct cpu_op *mx51_get_cpu_op(int *op);