5 files changed, 63 insertions, 9 deletions
diff --git a/arch/arm/mach-mx6/Kconfig b/arch/arm/mach-mx6/Kconfig
index ccd3a3321882..9fddbdf21907 100644
--- a/arch/arm/mach-mx6/Kconfig
+++ b/arch/arm/mach-mx6/Kconfig
@@ -33,6 +33,7 @@ config MACH_MX6Q_SABREAUTO
 	select IMX_HAVE_PLATFORM_IMX_I2C
 	select IMX_HAVE_PLATFORM_VIV_GPU
 	select IMX_HAVE_PLATFORM_IMX_VPU
+	select IMX_HAVE_PLATFORM_IMX_DVFS
 	select IMX_HAVE_PLATFORM_IMX_ESAI
 	select IMX_HAVE_PLATFORM_IMX_ANATOP_THERMAL
 	select IMX_HAVE_PLATFORM_FSL_USB2_UDC
diff --git a/arch/arm/mach-mx6/board-mx6q_sabreauto.c b/arch/arm/mach-mx6/board-mx6q_sabreauto.c
index 2fc366e9ac1e..1d178512bb47 100644
--- a/arch/arm/mach-mx6/board-mx6q_sabreauto.c
+++ b/arch/arm/mach-mx6/board-mx6q_sabreauto.c
@@ -45,14 +45,12 @@
 #include <linux/pwm_backlight.h>
 #include <linux/fec.h>
 #include <linux/memblock.h>
+#include <linux/gpio.h>
+#include <linux/etherdevice.h>
+
 #include <mach/common.h>
 #include <mach/hardware.h>
-#include <asm/irq.h>
-#include <asm/setup.h>
-#include <asm/mach-types.h>
-#include <asm/mach/arch.h>
-#include <asm/mach/time.h>
-#include <asm/mach/flash.h>
+#include <mach/mxc_dvfs.h>
 #include <mach/memory.h>
 #include <mach/iomux-mx6q.h>
 #include <mach/imx-uart.h>
@@ -61,8 +59,13 @@
 #include <mach/ipu-v3.h>
 #include <mach/mxc_hdmi.h>
 #include <mach/mxc_asrc.h>
-#include <linux/gpio.h>
-#include <linux/etherdevice.h>
+
+#include <asm/irq.h>
+#include <asm/setup.h>
+#include <asm/mach-types.h>
+#include <asm/mach/arch.h>
+#include <asm/mach/time.h>
+#include <asm/mach/flash.h>
 
 #include "usb.h"
 #include "devices-imx6q.h"
@@ -784,6 +787,29 @@ static int __init early_use_esai_record(char *p)
 
 early_param("esai_record", early_use_esai_record);
 
+static struct mxc_dvfs_platform_data sabreauto_dvfscore_data = {
+	.reg_id = "cpu_vddgp",
+	.clk1_id = "cpu_clk",
+	.clk2_id = "gpc_dvfs_clk",
+	.gpc_cntr_offset = MXC_GPC_CNTR_OFFSET,
+	.ccm_cdcr_offset = MXC_CCM_CDCR_OFFSET,
+	.ccm_cacrr_offset = MXC_CCM_CACRR_OFFSET,
+	.ccm_cdhipr_offset = MXC_CCM_CDHIPR_OFFSET,
+	.prediv_mask = 0x1F800,
+	.prediv_offset = 11,
+	.prediv_val = 3,
+	.div3ck_mask = 0xE0000000,
+	.div3ck_offset = 29,
+	.div3ck_val = 2,
+	.emac_val = 0x08,
+	.upthr_val = 25,
+	.dnthr_val = 9,
+	.pncthr_val = 33,
+	.upcnt_val = 10,
+	.dncnt_val = 10,
+	.delay_time = 80,
+};
+
 static int mx6_sabre_set_cpu_voltage(u32 cpu_volt)
 {
 	return mx6_set_cpu_voltage(cpu_volt);
@@ -877,6 +903,8 @@ static void __init mx6_board_init(void)
 	imx6q_add_imx2_wdt(0, NULL);
 	imx6q_add_dma();
 	imx6q_add_gpmi(&mx6q_gpmi_nfc_platform_data);
+
+	imx6q_add_dvfs_core(&sabreauto_dvfscore_data);
 }
 
 extern void __iomem *twd_base;
diff --git a/arch/arm/mach-mx6/cpu.c b/arch/arm/mach-mx6/cpu.c
index ddc7b98f9597..46ac7da8cbea 100644
--- a/arch/arm/mach-mx6/cpu.c
+++ b/arch/arm/mach-mx6/cpu.c
@@ -38,6 +38,8 @@ extern void mx6_wait(void);
 struct cpu_op *(*get_cpu_op)(int *op);
 static void __iomem *arm_base = IO_ADDRESS(MX6Q_A9_PLATFRM_BASE);
 
+void __iomem *gpc_base;
+void __iomem *ccm_base;
 
 int mx6_set_cpu_voltage(u32 cpu_volt)
 {
@@ -104,6 +106,9 @@ static int __init post_cpu_init(void)
 	memcpy((void *)cpaddr, mx6_wait, SZ_4K);
 	mx6_wait_in_iram = (void *)mx6_wait_in_iram_base;
 
+	gpc_base = MX6_IO_ADDRESS(GPC_BASE_ADDR);
+	ccm_base = MX6_IO_ADDRESS(CCM_BASE_ADDR);
+
 	return 0;
 }
 
diff --git a/arch/arm/mach-mx6/cpu_op-mx6.c b/arch/arm/mach-mx6/cpu_op-mx6.c
index ae3bff98bdde..51bc917f86cc 100644
--- a/arch/arm/mach-mx6/cpu_op-mx6.c
+++ b/arch/arm/mach-mx6/cpu_op-mx6.c
@@ -12,10 +12,12 @@
  */
 
 #include <linux/types.h>
-#include <mach/hardware.h>
 #include <linux/kernel.h>
+#include <mach/hardware.h>
+#include <mach/mxc_dvfs.h>
 
 extern struct cpu_op *(*get_cpu_op)(int *op);
+extern struct dvfs_op *(*get_dvfs_core_op)(int *wp);
 extern void (*set_num_cpu_op)(int num);
 static int num_cpu_op;
 
@@ -51,6 +53,18 @@ static struct cpu_op mx6_cpu_op[] = {
 	 .cpu_voltage = 900000,},
 };
 
+static struct dvfs_op dvfs_core_setpoint[] = {
+	{33, 14, 33, 10, 10, 0x08}, /* 1GHz*/
+	{30, 12, 33, 10, 10, 0x08},   /* 800MHz */
+	{28, 8, 33, 10, 10, 0x08},   /* 400MHz */
+	{20, 0, 33, 20, 10, 0x08} };   /* 167MHz*/
+
+static struct dvfs_op *mx6_get_dvfs_core_table(int *wp)
+{
+	*wp = ARRAY_SIZE(dvfs_core_setpoint);
+	return dvfs_core_setpoint;
+}
+
 struct cpu_op *mx6_get_cpu_op(int *op)
 {
 	*op = num_cpu_op;
@@ -69,5 +83,6 @@ void mx6_cpu_op_init(void)
 	set_num_cpu_op = mx6_set_num_cpu_op;
 
 	num_cpu_op = ARRAY_SIZE(mx6_cpu_op);
+	get_dvfs_core_op = mx6_get_dvfs_core_table;
 }
 
diff --git a/arch/arm/mach-mx6/devices-imx6q.h b/arch/arm/mach-mx6/devices-imx6q.h
index 2d5069fb41c0..b607758074cd 100644
--- a/arch/arm/mach-mx6/devices-imx6q.h
+++ b/arch/arm/mach-mx6/devices-imx6q.h
@@ -138,3 +138,8 @@ extern const struct imx_pm_imx_data imx6q_pm_imx_data __initconst;
 extern const struct imx_imx_asrc_data imx6q_imx_asrc_data[] __initconst;
 #define imx6q_add_asrc(pdata)	\
 	imx_add_imx_asrc(imx6q_imx_asrc_data, pdata)
+
+extern const struct imx_dvfs_core_data imx6q_dvfs_core_data __initconst;
+#define imx6q_add_dvfs_core(pdata)	\
+	imx_add_dvfs_core(&imx6q_dvfs_core_data, pdata)
+