ENGR00139280: MX6: Add CPUFREQ support

[karo-tx-linux.git] / arch / arm / mach-mx6 / cpu.c

diff --git a/arch/arm/mach-mx6/cpu.c b/arch/arm/mach-mx6/cpu.c
index 8e7b13cb523f203f2b671ef837cd682fe8a6d0e9..dc3f1d16a0216651e2bc99a10a383eb6ffee518e 100644 (file)
--- a/arch/arm/mach-mx6/cpu.c
+++ b/arch/arm/mach-mx6/cpu.c
@@ -25,6 +25,25 @@
 #include <mach/hardware.h>
 #include <asm/io.h>
 
+#include "crm_regs.h"
+
+struct cpu_op *(*get_cpu_op)(int *op);
+
+int mx6_set_cpu_voltage(u32 cpu_volt)
+{
+       u32 reg, val;
+
+       val = (cpu_volt - 725000) / 25000;
+
+       reg = __raw_readl(ANADIG_REG_CORE);
+       reg &= ~(ANADIG_REG_TARGET_MASK << ANADIG_REG0_CORE_TARGET_OFFSET);
+       reg |= ((val + 1) << ANADIG_REG0_CORE_TARGET_OFFSET);
+
+       __raw_writel(reg, ANADIG_REG_CORE);
+
+       return 0;
+}
+
 static int __init post_cpu_init(void)
 {
        unsigned int reg;