Patchwork [V2,7/7] arm/imx6q: add cpufreq support

login
register
mail settings
Submitter Richard Zhao
Date Dec. 13, 2011, 6:25 a.m.
Message ID <1323757530-19402-8-git-send-email-richard.zhao@linaro.org>
Download mbox | patch
Permalink /patch/131001/
State New
Headers show

Comments

Richard Zhao - Dec. 13, 2011, 6:25 a.m.
use cpu0 node property clock-frequency to check max frequency.

Signed-off-by: Richard Zhao <richard.zhao@linaro.org>
---
 arch/arm/mach-imx/Kconfig        |    1 +
 arch/arm/mach-imx/Makefile       |    2 +-
 arch/arm/mach-imx/cpu_op-imx6q.c |   69 ++++++++++++++++++++++++++++++++++++++
 3 files changed, 71 insertions(+), 1 deletions(-)
 create mode 100644 arch/arm/mach-imx/cpu_op-imx6q.c
Sascha Hauer - Dec. 13, 2011, 10:23 a.m.
On Tue, Dec 13, 2011 at 02:25:30PM +0800, Richard Zhao wrote:
> use cpu0 node property clock-frequency to check max frequency.
> 
> Signed-off-by: Richard Zhao <richard.zhao@linaro.org>
> ---
>  arch/arm/mach-imx/Kconfig        |    1 +
>  arch/arm/mach-imx/Makefile       |    2 +-
>  arch/arm/mach-imx/cpu_op-imx6q.c |   69 ++++++++++++++++++++++++++++++++++++++
>  3 files changed, 71 insertions(+), 1 deletions(-)
>  create mode 100644 arch/arm/mach-imx/cpu_op-imx6q.c
> 
> diff --git a/arch/arm/mach-imx/Kconfig b/arch/arm/mach-imx/Kconfig
> index fbd414b..21e472f 100644
> --- a/arch/arm/mach-imx/Kconfig
> +++ b/arch/arm/mach-imx/Kconfig
> @@ -834,6 +834,7 @@ comment "i.MX6 family:"
>  
>  config SOC_IMX6Q
>  	bool "i.MX6 Quad support"
> +	select ARCH_HAS_CPUFREQ
>  	select ARM_GIC
>  	select CACHE_L2X0
>  	select CPU_V7
> diff --git a/arch/arm/mach-imx/Makefile b/arch/arm/mach-imx/Makefile
> index 9cf630a..2dfe4a7 100644
> --- a/arch/arm/mach-imx/Makefile
> +++ b/arch/arm/mach-imx/Makefile
> @@ -72,7 +72,7 @@ AFLAGS_head-v7.o :=-Wa,-march=armv7-a
>  obj-$(CONFIG_SMP) += platsmp.o
>  obj-$(CONFIG_HOTPLUG_CPU) += hotplug.o
>  obj-$(CONFIG_LOCAL_TIMERS) += localtimer.o
> -obj-$(CONFIG_SOC_IMX6Q) += clock-imx6q.o mach-imx6q.o pm-imx6q.o
> +obj-$(CONFIG_SOC_IMX6Q) += clock-imx6q.o mach-imx6q.o pm-imx6q.o cpu_op-imx6q.o
>  
>  # i.MX5 based machines
>  obj-$(CONFIG_MACH_MX51_BABBAGE) += mach-mx51_babbage.o
> diff --git a/arch/arm/mach-imx/cpu_op-imx6q.c b/arch/arm/mach-imx/cpu_op-imx6q.c
> new file mode 100644
> index 0000000..d24e081
> --- /dev/null
> +++ b/arch/arm/mach-imx/cpu_op-imx6q.c
> @@ -0,0 +1,69 @@
> +/*
> + * Copyright (C) 2011 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 <linux/kernel.h>
> +#include <linux/string.h>
> +#include <linux/of.h>
> +#include <mach/hardware.h>
> +
> +static int arm_max_freq_idx;

This is only used in mx6q_get_cpu_op()

Also shouldn't this be initialized with 1? From what you
said 800MHz is safe for all boards whereas 1GHz is not.

Sascha

> +
> +/* operating point(op): 0 - 1GHz; 1 - 800MHz, 3 - 400MHz, 4  - 160MHz */
> +static struct cpu_op mx6q_cpu_op[] = {
> +	{
> +		.cpu_rate = 996000000,
> +		.cpu_voltage = 1225000,
> +	},
> +	{
> +		.cpu_rate = 792000000,
> +		.cpu_voltage = 1100000,
> +	},
> +	{
> +		.cpu_rate = 396000000,
> +		.cpu_voltage = 950000,
> +	},
> +	{
> +		.cpu_rate = 198000000,
> +		.cpu_voltage = 850000,
> +	},
> +};
> +
> +struct cpu_op *mx6q_get_cpu_op(int *op)
> +{
> +	struct device_node *cpu0;
> +	u32 val;
> +	int i;
> +
> +	cpu0 = of_find_node_by_path("/cpus/cpu@0");
> +	if (cpu0 && !of_property_read_u32(cpu0, "clock-frequency", &val)) {
> +		for (i = 0; i < ARRAY_SIZE(mx6q_cpu_op); i++) {
> +			if (val >= mx6q_cpu_op[i].cpu_rate) {
> +				arm_max_freq_idx = i;
> +				break;
> +			}
> +		}
> +	}
> +	if (cpu0)
> +		of_node_put(cpu0);
> +	*op = ARRAY_SIZE(mx6q_cpu_op) - arm_max_freq_idx;
> +	return mx6q_cpu_op + arm_max_freq_idx;
> +}
> +
> +static int __init mx6q_cpu_op_init(void)
> +{
> +	get_cpu_op = mx6q_get_cpu_op;
> +	return 0;
> +}
> +
> +core_initcall(mx6q_cpu_op_init);
> -- 
> 1.7.5.4
> 
> 
>
Richard Zhao - Dec. 13, 2011, 10:38 a.m.
On Tue, Dec 13, 2011 at 11:23:15AM +0100, Sascha Hauer wrote:
> On Tue, Dec 13, 2011 at 02:25:30PM +0800, Richard Zhao wrote:
> > use cpu0 node property clock-frequency to check max frequency.
> > 
> > Signed-off-by: Richard Zhao <richard.zhao@linaro.org>
> > ---
> >  arch/arm/mach-imx/Kconfig        |    1 +
> >  arch/arm/mach-imx/Makefile       |    2 +-
> >  arch/arm/mach-imx/cpu_op-imx6q.c |   69 ++++++++++++++++++++++++++++++++++++++
> >  3 files changed, 71 insertions(+), 1 deletions(-)
> >  create mode 100644 arch/arm/mach-imx/cpu_op-imx6q.c
> > 
> > diff --git a/arch/arm/mach-imx/Kconfig b/arch/arm/mach-imx/Kconfig
> > index fbd414b..21e472f 100644
> > --- a/arch/arm/mach-imx/Kconfig
> > +++ b/arch/arm/mach-imx/Kconfig
> > @@ -834,6 +834,7 @@ comment "i.MX6 family:"
> >  
> >  config SOC_IMX6Q
> >  	bool "i.MX6 Quad support"
> > +	select ARCH_HAS_CPUFREQ
> >  	select ARM_GIC
> >  	select CACHE_L2X0
> >  	select CPU_V7
> > diff --git a/arch/arm/mach-imx/Makefile b/arch/arm/mach-imx/Makefile
> > index 9cf630a..2dfe4a7 100644
> > --- a/arch/arm/mach-imx/Makefile
> > +++ b/arch/arm/mach-imx/Makefile
> > @@ -72,7 +72,7 @@ AFLAGS_head-v7.o :=-Wa,-march=armv7-a
> >  obj-$(CONFIG_SMP) += platsmp.o
> >  obj-$(CONFIG_HOTPLUG_CPU) += hotplug.o
> >  obj-$(CONFIG_LOCAL_TIMERS) += localtimer.o
> > -obj-$(CONFIG_SOC_IMX6Q) += clock-imx6q.o mach-imx6q.o pm-imx6q.o
> > +obj-$(CONFIG_SOC_IMX6Q) += clock-imx6q.o mach-imx6q.o pm-imx6q.o cpu_op-imx6q.o
> >  
> >  # i.MX5 based machines
> >  obj-$(CONFIG_MACH_MX51_BABBAGE) += mach-mx51_babbage.o
> > diff --git a/arch/arm/mach-imx/cpu_op-imx6q.c b/arch/arm/mach-imx/cpu_op-imx6q.c
> > new file mode 100644
> > index 0000000..d24e081
> > --- /dev/null
> > +++ b/arch/arm/mach-imx/cpu_op-imx6q.c
> > @@ -0,0 +1,69 @@
> > +/*
> > + * Copyright (C) 2011 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 <linux/kernel.h>
> > +#include <linux/string.h>
> > +#include <linux/of.h>
> > +#include <mach/hardware.h>
> > +
> > +static int arm_max_freq_idx;
> 
> This is only used in mx6q_get_cpu_op()
I'll move to local var
> 
> Also shouldn't this be initialized with 1? From what you
> said 800MHz is safe for all boards whereas 1GHz is not.
I'll fix it in next version.

Thanks
Richard
> 
> Sascha
> 
> > +
> > +/* operating point(op): 0 - 1GHz; 1 - 800MHz, 3 - 400MHz, 4  - 160MHz */
> > +static struct cpu_op mx6q_cpu_op[] = {
> > +	{
> > +		.cpu_rate = 996000000,
> > +		.cpu_voltage = 1225000,
> > +	},
> > +	{
> > +		.cpu_rate = 792000000,
> > +		.cpu_voltage = 1100000,
> > +	},
> > +	{
> > +		.cpu_rate = 396000000,
> > +		.cpu_voltage = 950000,
> > +	},
> > +	{
> > +		.cpu_rate = 198000000,
> > +		.cpu_voltage = 850000,
> > +	},
> > +};
> > +
> > +struct cpu_op *mx6q_get_cpu_op(int *op)
> > +{
> > +	struct device_node *cpu0;
> > +	u32 val;
> > +	int i;
> > +
> > +	cpu0 = of_find_node_by_path("/cpus/cpu@0");
> > +	if (cpu0 && !of_property_read_u32(cpu0, "clock-frequency", &val)) {
> > +		for (i = 0; i < ARRAY_SIZE(mx6q_cpu_op); i++) {
> > +			if (val >= mx6q_cpu_op[i].cpu_rate) {
> > +				arm_max_freq_idx = i;
> > +				break;
> > +			}
> > +		}
> > +	}
> > +	if (cpu0)
> > +		of_node_put(cpu0);
> > +	*op = ARRAY_SIZE(mx6q_cpu_op) - arm_max_freq_idx;
> > +	return mx6q_cpu_op + arm_max_freq_idx;
> > +}
> > +
> > +static int __init mx6q_cpu_op_init(void)
> > +{
> > +	get_cpu_op = mx6q_get_cpu_op;
> > +	return 0;
> > +}
> > +
> > +core_initcall(mx6q_cpu_op_init);
> > -- 
> > 1.7.5.4
> > 
> > 
> > 
> 
> -- 
> Pengutronix e.K.                           |                             |
> Industrial Linux Solutions                 | http://www.pengutronix.de/  |
> Peiner Str. 6-8, 31137 Hildesheim, Germany | Phone: +49-5121-206917-0    |
> Amtsgericht Hildesheim, HRA 2686           | Fax:   +49-5121-206917-5555 |
> 
> _______________________________________________
> linux-arm-kernel mailing list
> linux-arm-kernel@lists.infradead.org
> http://lists.infradead.org/mailman/listinfo/linux-arm-kernel
>
Shawn Guo - Dec. 13, 2011, 12:19 p.m.
On Tue, Dec 13, 2011 at 02:25:30PM +0800, Richard Zhao wrote:
> use cpu0 node property clock-frequency to check max frequency.
> 
> Signed-off-by: Richard Zhao <richard.zhao@linaro.org>
> ---
>  arch/arm/mach-imx/Kconfig        |    1 +
>  arch/arm/mach-imx/Makefile       |    2 +-
>  arch/arm/mach-imx/cpu_op-imx6q.c |   69 ++++++++++++++++++++++++++++++++++++++
>  3 files changed, 71 insertions(+), 1 deletions(-)
>  create mode 100644 arch/arm/mach-imx/cpu_op-imx6q.c
> 
NAK.

We really need to push people to help with cleanup and consolidation
effort rather than accepting patches on top of something that needs
to be cleaned up and consolidated, which will make the effort later
more difficult.
Sascha Hauer - Dec. 13, 2011, 2:20 p.m.
On Tue, Dec 13, 2011 at 08:19:42PM +0800, Shawn Guo wrote:
> On Tue, Dec 13, 2011 at 02:25:30PM +0800, Richard Zhao wrote:
> > use cpu0 node property clock-frequency to check max frequency.
> > 
> > Signed-off-by: Richard Zhao <richard.zhao@linaro.org>
> > ---
> >  arch/arm/mach-imx/Kconfig        |    1 +
> >  arch/arm/mach-imx/Makefile       |    2 +-
> >  arch/arm/mach-imx/cpu_op-imx6q.c |   69 ++++++++++++++++++++++++++++++++++++++
> >  3 files changed, 71 insertions(+), 1 deletions(-)
> >  create mode 100644 arch/arm/mach-imx/cpu_op-imx6q.c
> > 
> NAK.
> 
> We really need to push people to help with cleanup and consolidation
> effort rather than accepting patches on top of something that needs
> to be cleaned up and consolidated, which will make the effort later
> more difficult.

+1

Sascha
Richard Zhao - Dec. 13, 2011, 3:01 p.m.
On Tue, Dec 13, 2011 at 03:20:33PM +0100, Sascha Hauer wrote:
> On Tue, Dec 13, 2011 at 08:19:42PM +0800, Shawn Guo wrote:
> > On Tue, Dec 13, 2011 at 02:25:30PM +0800, Richard Zhao wrote:
> > > use cpu0 node property clock-frequency to check max frequency.
> > > 
> > > Signed-off-by: Richard Zhao <richard.zhao@linaro.org>
> > > ---
> > >  arch/arm/mach-imx/Kconfig        |    1 +
> > >  arch/arm/mach-imx/Makefile       |    2 +-
> > >  arch/arm/mach-imx/cpu_op-imx6q.c |   69 ++++++++++++++++++++++++++++++++++++++
> > >  3 files changed, 71 insertions(+), 1 deletions(-)
> > >  create mode 100644 arch/arm/mach-imx/cpu_op-imx6q.c
> > > 
> > NAK.
> > 
> > We really need to push people to help with cleanup and consolidation
> > effort rather than accepting patches on top of something that needs
> > to be cleaned up and consolidated, which will make the effort later
> > more difficult.
> 
> +1
ok, let's make it clear.
- move arch/arm/plat-mxc/cpufreq.c to drivers/cpufreq/
- consider whether opp help imx, at least for imx cpufreq drive to pass
operating table.
- fall back ways, maybe define in dts /soc like:
cpufreq {
	compatible = "fsl,imx-cpufreq";
	frequency = <100000000 800000000 500000000 200000000>;
	volatage = <1500000 1400000 1300000 1200000>;
};

Thanks
Richard
> 
> Sascha
> 
> -- 
> Pengutronix e.K.                           |                             |
> Industrial Linux Solutions                 | http://www.pengutronix.de/  |
> Peiner Str. 6-8, 31137 Hildesheim, Germany | Phone: +49-5121-206917-0    |
> Amtsgericht Hildesheim, HRA 2686           | Fax:   +49-5121-206917-5555 |
Robert Lee - Dec. 13, 2011, 7:04 p.m.
On 13 December 2011 04:38, Richard Zhao <richard.zhao@freescale.com> wrote:
> On Tue, Dec 13, 2011 at 11:23:15AM +0100, Sascha Hauer wrote:
>> On Tue, Dec 13, 2011 at 02:25:30PM +0800, Richard Zhao wrote:
>> > use cpu0 node property clock-frequency to check max frequency.
>> >
>> > Signed-off-by: Richard Zhao <richard.zhao@linaro.org>
>> > ---
>> >  arch/arm/mach-imx/Kconfig        |    1 +
>> >  arch/arm/mach-imx/Makefile       |    2 +-
>> >  arch/arm/mach-imx/cpu_op-imx6q.c |   69 ++++++++++++++++++++++++++++++++++++++
>> >  3 files changed, 71 insertions(+), 1 deletions(-)
>> >  create mode 100644 arch/arm/mach-imx/cpu_op-imx6q.c
>> >
>> > diff --git a/arch/arm/mach-imx/Kconfig b/arch/arm/mach-imx/Kconfig
>> > index fbd414b..21e472f 100644
>> > --- a/arch/arm/mach-imx/Kconfig
>> > +++ b/arch/arm/mach-imx/Kconfig
>> > @@ -834,6 +834,7 @@ comment "i.MX6 family:"
>> >
>> >  config SOC_IMX6Q
>> >     bool "i.MX6 Quad support"
>> > +   select ARCH_HAS_CPUFREQ
>> >     select ARM_GIC
>> >     select CACHE_L2X0
>> >     select CPU_V7
>> > diff --git a/arch/arm/mach-imx/Makefile b/arch/arm/mach-imx/Makefile
>> > index 9cf630a..2dfe4a7 100644
>> > --- a/arch/arm/mach-imx/Makefile
>> > +++ b/arch/arm/mach-imx/Makefile
>> > @@ -72,7 +72,7 @@ AFLAGS_head-v7.o :=-Wa,-march=armv7-a
>> >  obj-$(CONFIG_SMP) += platsmp.o
>> >  obj-$(CONFIG_HOTPLUG_CPU) += hotplug.o
>> >  obj-$(CONFIG_LOCAL_TIMERS) += localtimer.o
>> > -obj-$(CONFIG_SOC_IMX6Q) += clock-imx6q.o mach-imx6q.o pm-imx6q.o
>> > +obj-$(CONFIG_SOC_IMX6Q) += clock-imx6q.o mach-imx6q.o pm-imx6q.o cpu_op-imx6q.o
>> >
>> >  # i.MX5 based machines
>> >  obj-$(CONFIG_MACH_MX51_BABBAGE) += mach-mx51_babbage.o
>> > diff --git a/arch/arm/mach-imx/cpu_op-imx6q.c b/arch/arm/mach-imx/cpu_op-imx6q.c
>> > new file mode 100644
>> > index 0000000..d24e081
>> > --- /dev/null
>> > +++ b/arch/arm/mach-imx/cpu_op-imx6q.c
>> > @@ -0,0 +1,69 @@
>> > +/*
>> > + * Copyright (C) 2011 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 <linux/kernel.h>
>> > +#include <linux/string.h>
>> > +#include <linux/of.h>
>> > +#include <mach/hardware.h>
>> > +
>> > +static int arm_max_freq_idx;
>>
>> This is only used in mx6q_get_cpu_op()
> I'll move to local var
>>
>> Also shouldn't this be initialized with 1? From what you
>> said 800MHz is safe for all boards whereas 1GHz is not.
> I'll fix it in next version.
>

Richard, I communicated with Hui Liu a couple of weeks ago about the
possibility of adding a "max_freq" kernel parameter to the i.MX6q
community u-boot, similiar to what has been done for the BSP u-boot.
If he is or has done this, it may be better to read and use this
kernel parameter from u-boot so that the same kernel can be used
between boards with and without the fix.  I'm not that familiar with
device tree yet but perhaps it provides another way to handle this.
Also keep in

> Thanks
> Richard
>>
>> Sascha
>>
>> > +
>> > +/* operating point(op): 0 - 1GHz; 1 - 800MHz, 3 - 400MHz, 4  - 160MHz */
>> > +static struct cpu_op mx6q_cpu_op[] = {
>> > +   {
>> > +           .cpu_rate = 996000000,
>> > +           .cpu_voltage = 1225000,
>> > +   },
>> > +   {
>> > +           .cpu_rate = 792000000,
>> > +           .cpu_voltage = 1100000,
>> > +   },
>> > +   {
>> > +           .cpu_rate = 396000000,
>> > +           .cpu_voltage = 950000,
>> > +   },
>> > +   {
>> > +           .cpu_rate = 198000000,
>> > +           .cpu_voltage = 850000,
>> > +   },
>> > +};
>> > +
>> > +struct cpu_op *mx6q_get_cpu_op(int *op)
>> > +{
>> > +   struct device_node *cpu0;
>> > +   u32 val;
>> > +   int i;
>> > +
>> > +   cpu0 = of_find_node_by_path("/cpus/cpu@0");
>> > +   if (cpu0 && !of_property_read_u32(cpu0, "clock-frequency", &val)) {
>> > +           for (i = 0; i < ARRAY_SIZE(mx6q_cpu_op); i++) {
>> > +                   if (val >= mx6q_cpu_op[i].cpu_rate) {
>> > +                           arm_max_freq_idx = i;
>> > +                           break;
>> > +                   }
>> > +           }
>> > +   }
>> > +   if (cpu0)
>> > +           of_node_put(cpu0);
>> > +   *op = ARRAY_SIZE(mx6q_cpu_op) - arm_max_freq_idx;
>> > +   return mx6q_cpu_op + arm_max_freq_idx;
>> > +}
>> > +
>> > +static int __init mx6q_cpu_op_init(void)
>> > +{
>> > +   get_cpu_op = mx6q_get_cpu_op;
>> > +   return 0;
>> > +}
>> > +
>> > +core_initcall(mx6q_cpu_op_init);
>> > --
>> > 1.7.5.4
>> >
>> >
>> >
>>
>> --
>> Pengutronix e.K.                           |                             |
>> Industrial Linux Solutions                 | http://www.pengutronix.de/  |
>> Peiner Str. 6-8, 31137 Hildesheim, Germany | Phone: +49-5121-206917-0    |
>> Amtsgericht Hildesheim, HRA 2686           | Fax:   +49-5121-206917-5555 |
>>
>> _______________________________________________
>> linux-arm-kernel mailing list
>> linux-arm-kernel@lists.infradead.org
>> http://lists.infradead.org/mailman/listinfo/linux-arm-kernel
>>
>
Richard Zhao - Dec. 14, 2011, 1:29 a.m.
> >> > +static int arm_max_freq_idx;
> >>
> >> This is only used in mx6q_get_cpu_op()
> > I'll move to local var
> >>
> >> Also shouldn't this be initialized with 1? From what you
> >> said 800MHz is safe for all boards whereas 1GHz is not.
> > I'll fix it in next version.
> >
> 
> Richard, I communicated with Hui Liu a couple of weeks ago about the
> possibility of adding a "max_freq" kernel parameter to the i.MX6q
> community u-boot, similiar to what has been done for the BSP u-boot.
> If he is or has done this, it may be better to read and use this
> kernel parameter from u-boot so that the same kernel can be used
> between boards with and without the fix.  I'm not that familiar with
> device tree yet but perhaps it provides another way to handle this.
> Also keep in
Using bootargs, it's what I did in V1 patch. I prefer that way too.

Thanks
Richard

Patch

diff --git a/arch/arm/mach-imx/Kconfig b/arch/arm/mach-imx/Kconfig
index fbd414b..21e472f 100644
--- a/arch/arm/mach-imx/Kconfig
+++ b/arch/arm/mach-imx/Kconfig
@@ -834,6 +834,7 @@  comment "i.MX6 family:"
 
 config SOC_IMX6Q
 	bool "i.MX6 Quad support"
+	select ARCH_HAS_CPUFREQ
 	select ARM_GIC
 	select CACHE_L2X0
 	select CPU_V7
diff --git a/arch/arm/mach-imx/Makefile b/arch/arm/mach-imx/Makefile
index 9cf630a..2dfe4a7 100644
--- a/arch/arm/mach-imx/Makefile
+++ b/arch/arm/mach-imx/Makefile
@@ -72,7 +72,7 @@  AFLAGS_head-v7.o :=-Wa,-march=armv7-a
 obj-$(CONFIG_SMP) += platsmp.o
 obj-$(CONFIG_HOTPLUG_CPU) += hotplug.o
 obj-$(CONFIG_LOCAL_TIMERS) += localtimer.o
-obj-$(CONFIG_SOC_IMX6Q) += clock-imx6q.o mach-imx6q.o pm-imx6q.o
+obj-$(CONFIG_SOC_IMX6Q) += clock-imx6q.o mach-imx6q.o pm-imx6q.o cpu_op-imx6q.o
 
 # i.MX5 based machines
 obj-$(CONFIG_MACH_MX51_BABBAGE) += mach-mx51_babbage.o
diff --git a/arch/arm/mach-imx/cpu_op-imx6q.c b/arch/arm/mach-imx/cpu_op-imx6q.c
new file mode 100644
index 0000000..d24e081
--- /dev/null
+++ b/arch/arm/mach-imx/cpu_op-imx6q.c
@@ -0,0 +1,69 @@ 
+/*
+ * Copyright (C) 2011 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 <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/of.h>
+#include <mach/hardware.h>
+
+static int arm_max_freq_idx;
+
+/* operating point(op): 0 - 1GHz; 1 - 800MHz, 3 - 400MHz, 4  - 160MHz */
+static struct cpu_op mx6q_cpu_op[] = {
+	{
+		.cpu_rate = 996000000,
+		.cpu_voltage = 1225000,
+	},
+	{
+		.cpu_rate = 792000000,
+		.cpu_voltage = 1100000,
+	},
+	{
+		.cpu_rate = 396000000,
+		.cpu_voltage = 950000,
+	},
+	{
+		.cpu_rate = 198000000,
+		.cpu_voltage = 850000,
+	},
+};
+
+struct cpu_op *mx6q_get_cpu_op(int *op)
+{
+	struct device_node *cpu0;
+	u32 val;
+	int i;
+
+	cpu0 = of_find_node_by_path("/cpus/cpu@0");
+	if (cpu0 && !of_property_read_u32(cpu0, "clock-frequency", &val)) {
+		for (i = 0; i < ARRAY_SIZE(mx6q_cpu_op); i++) {
+			if (val >= mx6q_cpu_op[i].cpu_rate) {
+				arm_max_freq_idx = i;
+				break;
+			}
+		}
+	}
+	if (cpu0)
+		of_node_put(cpu0);
+	*op = ARRAY_SIZE(mx6q_cpu_op) - arm_max_freq_idx;
+	return mx6q_cpu_op + arm_max_freq_idx;
+}
+
+static int __init mx6q_cpu_op_init(void)
+{
+	get_cpu_op = mx6q_get_cpu_op;
+	return 0;
+}
+
+core_initcall(mx6q_cpu_op_init);