ENGR00315400 ARM: imx: add lpddr2 busfreq support for i.mx6sx

Add freq scaling for lpddr2 of i.MX6SX, support 3 setpoints:

high  -> 400MHz
audio -> 100MHz
low   -> 24MHz

Signed-off-by: Anson Huang <b20788@freescale.com>

6 files changed, 523 insertions, 26 deletions

diff --git a/arch/arm/mach-imx/Makefile b/arch/arm/mach-imx/Makefile
index f4ac8d38efaa..25366995d4df 100644
--- a/arch/arm/mach-imx/Makefile
+++ b/arch/arm/mach-imx/Makefile
@@ -111,7 +111,7 @@ ifeq ($(CONFIG_ARM_IMX6_CPUFREQ),y)
 obj-y += busfreq-imx6.o
 obj-$(CONFIG_SOC_IMX6Q) += ddr3_freq_imx6.o busfreq_ddr3.o
 obj-$(CONFIG_SOC_IMX6SL) += lpddr2_freq_imx6.o busfreq_lpddr2.o imx6sl_wfi.o
-obj-$(CONFIG_SOC_IMX6SX) += ddr3_freq_imx6sx.o
+obj-$(CONFIG_SOC_IMX6SX) += ddr3_freq_imx6sx.o lpddr2_freq_imx6sx.o
 
 endif
 
diff --git a/arch/arm/mach-imx/busfreq-imx6.c b/arch/arm/mach-imx/busfreq-imx6.c
index ecc020855fcc..8e640d39013e 100644
--- a/arch/arm/mach-imx/busfreq-imx6.c
+++ b/arch/arm/mach-imx/busfreq-imx6.c
@@ -46,11 +46,16 @@
 #include <linux/sched.h>
 #include <linux/suspend.h>
 #include "hardware.h"
+#include "common.h"
 
 #define LPAPM_CLK               24000000
 #define DDR3_AUDIO_CLK          50000000
 #define LPDDR2_AUDIO_CLK        100000000
 
+#define MMDC_MDMISC_DDR_TYPE_DDR3       0
+#define MMDC_MDMISC_DDR_TYPE_LPDDR2     1
+
+static int ddr_type;
 int high_bus_freq_mode;
 int med_bus_freq_mode;
 int audio_bus_freq_mode;
@@ -75,7 +80,6 @@ extern int update_ddr_freq_imx6sx(int ddr_rate);
 extern int update_lpddr2_freq(int ddr_rate);
 
 DEFINE_MUTEX(bus_freq_mutex);
-static DEFINE_SPINLOCK(freq_lock);
 
 static struct clk *mmdc_clk;
 static struct clk *pll2_400;
@@ -116,7 +120,10 @@ static void enter_lpm_imx6sx(void)
         if (audio_bus_count) {
                 /* Need to ensure that PLL2_PFD_400M is kept ON. */
                 clk_prepare_enable(pll2_400);
-                update_ddr_freq_imx6sx(DDR3_AUDIO_CLK);
+                if (ddr_type == MMDC_MDMISC_DDR_TYPE_DDR3)
+                        update_ddr_freq_imx6sx(DDR3_AUDIO_CLK);
+                else if (ddr_type == MMDC_MDMISC_DDR_TYPE_LPDDR2)
+                        update_lpddr2_freq(LPDDR2_AUDIO_CLK);
                 clk_set_parent(periph2_clk2_sel, pll3);
                 clk_set_parent(periph2_pre_clk, pll2_400);
                 clk_set_parent(periph2_clk, periph2_pre_clk);
@@ -129,13 +136,19 @@ static void enter_lpm_imx6sx(void)
                  * tree is right, although it will not do any
                  * change to hardware.
                  */
-                if (high_bus_freq_mode)
-                        clk_set_rate(mmdc_clk, DDR3_AUDIO_CLK);
-
+                if (high_bus_freq_mode) {
+                        if (ddr_type == MMDC_MDMISC_DDR_TYPE_DDR3)
+                                clk_set_rate(mmdc_clk, DDR3_AUDIO_CLK);
+                        else if (ddr_type == MMDC_MDMISC_DDR_TYPE_LPDDR2)
+                                clk_set_rate(mmdc_clk, LPDDR2_AUDIO_CLK);
+                }
                 audio_bus_freq_mode = 1;
                 low_bus_freq_mode = 0;
         } else {
-                update_ddr_freq_imx6sx(LPAPM_CLK);
+                if (ddr_type == MMDC_MDMISC_DDR_TYPE_DDR3)
+                        update_ddr_freq_imx6sx(LPAPM_CLK);
+                else if (ddr_type == MMDC_MDMISC_DDR_TYPE_LPDDR2)
+                        update_lpddr2_freq(LPAPM_CLK);
                 clk_set_parent(periph2_clk2_sel, osc_clk);
                 clk_set_parent(periph2_clk, periph2_clk2);
 
@@ -162,7 +175,10 @@ static void exit_lpm_imx6sx(void)
         clk_set_parent(periph_pre_clk, pll2_400);
         clk_set_parent(periph_clk, periph_pre_clk);
 
-        update_ddr_freq_imx6sx(ddr_normal_rate);
+        if (ddr_type == MMDC_MDMISC_DDR_TYPE_DDR3)
+                update_ddr_freq_imx6sx(ddr_normal_rate);
+        else if (ddr_type == MMDC_MDMISC_DDR_TYPE_LPDDR2)
+                update_lpddr2_freq(ddr_normal_rate);
         /* correct parent info after ddr freq change in asm code */
         clk_set_parent(periph2_clk2_sel, pll3);
         clk_set_parent(periph2_pre_clk, pll2_400);
@@ -186,8 +202,6 @@ static void exit_lpm_imx6sx(void)
 
 static void enter_lpm_imx6sl(void)
 {
-        unsigned long flags;
-
         if (high_bus_freq_mode) {
                 pll2_org_rate = clk_get_rate(pll2);
                 /* Set periph_clk to be sourced from OSC_CLK */
@@ -202,9 +216,7 @@ static void enter_lpm_imx6sl(void)
                 clk_set_rate(ahb_clk, LPAPM_CLK / 3);
 
                 /* Set up DDR to 100MHz. */
-                spin_lock_irqsave(&freq_lock, flags);
                 update_lpddr2_freq(LPDDR2_AUDIO_CLK);
-                spin_unlock_irqrestore(&freq_lock, flags);
 
                 /* Fix the clock tree in kernel */
                 clk_set_rate(pll2, pll2_org_rate);
@@ -271,9 +283,7 @@ static void enter_lpm_imx6sl(void)
                                  */
                                 clk_set_parent(step_clk, osc_clk);
                                 /* Now set DDR to 24MHz. */
-                                spin_lock_irqsave(&freq_lock, flags);
                                 update_lpddr2_freq(LPAPM_CLK);
-                                spin_unlock_irqrestore(&freq_lock, flags);
 
                                 /*
                                  * Fix the clock tree in kernel.
@@ -299,12 +309,8 @@ static void enter_lpm_imx6sl(void)
 
 static void exit_lpm_imx6sl(void)
 {
-        unsigned long flags;
-
-        spin_lock_irqsave(&freq_lock, flags);
         /* Change DDR freq in IRAM. */
         update_lpddr2_freq(ddr_normal_rate);
-        spin_unlock_irqrestore(&freq_lock, flags);
 
         /*
          * Fix the clock tree in kernel.
@@ -978,12 +984,19 @@ static int busfreq_probe(struct platform_device *pdev)
         register_pm_notifier(&imx_bus_freq_pm_notifier);
         register_reboot_notifier(&imx_busfreq_reboot_notifier);
 
-        if (cpu_is_imx6sl())
+        if (cpu_is_imx6sl()) {
                 err = init_mmdc_lpddr2_settings(pdev);
-        else if (cpu_is_imx6sx())
-                err = init_mmdc_ddr3_settings_imx6sx(pdev);
-        else
+        } else if (cpu_is_imx6sx()) {
+                ddr_type = imx_mmdc_get_ddr_type();
+                /* check whether it is a DDR3 or LPDDR2 board */
+                if (ddr_type == MMDC_MDMISC_DDR_TYPE_DDR3)
+                        err = init_mmdc_ddr3_settings_imx6sx(pdev);
+                else if (ddr_type == MMDC_MDMISC_DDR_TYPE_LPDDR2)
+                        err = init_mmdc_lpddr2_settings(pdev);
+        } else {
                 err = init_mmdc_ddr3_settings_imx6q(pdev);
+        }
+
         if (err) {
                 dev_err(busfreq_dev, "Busfreq init of MMDC failed\n");
                 return err;
diff --git a/arch/arm/mach-imx/busfreq_lpddr2.c b/arch/arm/mach-imx/busfreq_lpddr2.c
index a3559ca4a106..1e332bf66cbf 100644
--- a/arch/arm/mach-imx/busfreq_lpddr2.c
+++ b/arch/arm/mach-imx/busfreq_lpddr2.c
@@ -46,6 +46,7 @@
 static struct device *busfreq_dev;
 static void *ddr_freq_change_iram_base;
 static int curr_ddr_rate;
+static DEFINE_SPINLOCK(freq_lock);
 
 void (*mx6_change_lpddr2_freq)(u32 ddr_freq, int bus_freq_mode) = NULL;
 
@@ -53,6 +54,7 @@ extern unsigned int ddr_normal_rate;
 extern int low_bus_freq_mode;
 extern int ultra_low_bus_freq_mode;
 extern void mx6_lpddr2_freq_change(u32 freq, int bus_freq_mode);
+extern void imx6sx_lpddr2_freq_change(u32 freq, int bus_freq_mode);
 
 extern unsigned long save_ttbr1(void);
 extern void restore_ttbr1(unsigned long ttbr1);
@@ -61,12 +63,14 @@ extern unsigned long iram_tlb_phys_addr;
 /* change the DDR frequency. */
 int update_lpddr2_freq(int ddr_rate)
 {
-        unsigned long ttbr1;
+        unsigned long ttbr1, flags;
+
         if (ddr_rate == curr_ddr_rate)
                 return 0;
 
         dev_dbg(busfreq_dev, "\nBus freq set to %d start...\n", ddr_rate);
 
+        spin_lock_irqsave(&freq_lock, flags);
         /*
          * Flush the TLB, to ensure no TLB maintenance occurs
          * when DDR is in self-refresh.
@@ -79,6 +83,7 @@ int update_lpddr2_freq(int ddr_rate)
         restore_ttbr1(ttbr1);
 
         curr_ddr_rate = ddr_rate;
+        spin_unlock_irqrestore(&freq_lock, flags);
 
         dev_dbg(busfreq_dev, "\nBus freq set to %d done...\n", ddr_rate);
 
@@ -94,8 +99,14 @@ int init_mmdc_lpddr2_settings(struct platform_device *busfreq_pdev)
                         (void *)IMX_IO_P2V(iram_tlb_phys_addr) +
                         MX6SL_LPDDR2_FREQ_ADDR_OFFSET;
 
-        mx6_change_lpddr2_freq = (void *)fncpy(ddr_freq_change_iram_base,
-                &mx6_lpddr2_freq_change, LPDDR2_FREQ_CODE_SIZE);
+        if (cpu_is_imx6sl())
+                mx6_change_lpddr2_freq = (void *)fncpy(
+                        ddr_freq_change_iram_base,
+                        &mx6_lpddr2_freq_change, LPDDR2_FREQ_CODE_SIZE);
+        else if (cpu_is_imx6sx())
+                mx6_change_lpddr2_freq = (void *)fncpy(
+                        ddr_freq_change_iram_base,
+                        &imx6sx_lpddr2_freq_change, LPDDR2_FREQ_CODE_SIZE);
 
         curr_ddr_rate = ddr_normal_rate;
 
diff --git a/arch/arm/mach-imx/common.h b/arch/arm/mach-imx/common.h
index 80a132961671..b0872d986b7a 100644
--- a/arch/arm/mach-imx/common.h
+++ b/arch/arm/mach-imx/common.h
@@ -153,6 +153,7 @@ extern int imx6_set_lpm(enum mxc_cpu_pwr_mode mode);
 extern void imx6_set_cache_lpm_in_wait(bool enable);
 extern void imx6sl_set_wait_clk(bool enter);
 extern void imx6_enet_mac_init(const char *compatible);
+extern int imx_mmdc_get_ddr_type(void);
 
 extern void imx_cpu_die(unsigned int cpu);
 extern int imx_cpu_kill(unsigned int cpu);
diff --git a/arch/arm/mach-imx/lpddr2_freq_imx6sx.S b/arch/arm/mach-imx/lpddr2_freq_imx6sx.S
new file mode 100644
index 000000000000..94e7db554283
--- /dev/null
+++ b/arch/arm/mach-imx/lpddr2_freq_imx6sx.S
@@ -0,0 +1,454 @@
+/*
+ * Copyright (C) 2014 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#include <linux/linkage.h>
+#include "hardware.h"
+
+#define CCM_CBCDR       0x14
+#define CCM_CBCMR       0x18
+#define CCM_CSCMR1      0x1c
+#define CCM_CDHIPR      0x48
+
+#define L2_CACHE_SYNC   0x730
+
+#define MMDC0_MDPDC     0x4
+#define MMDC0_MAPSR     0x404
+#define MMDC0_MADPCR0   0x410
+
+        .macro  wait_for_ccm_handshake
+
+1:
+        ldr     r8, [r2, #CCM_CDHIPR]
+        cmp     r8, #0
+        bne     1b
+
+        .endm
+
+        .macro  switch_to_24MHz
+
+        /* periph2_clk2 sel to OSC_CLK */
+        ldr     r8, [r2, #CCM_CBCMR]
+        orr     r8, r8, #(1 << 20)
+        str     r8, [r2, #CCM_CBCMR]
+
+        /* periph2_clk2_podf to 0 */
+        ldr     r8, [r2, #CCM_CBCDR]
+        bic     r8, r8, #0x7
+        str     r8, [r2, #CCM_CBCDR]
+
+        /* periph2_clk sel to periph2_clk2 */
+        ldr     r8, [r2, #CCM_CBCDR]
+        orr     r8, r8, #(0x1 << 26)
+        str     r8, [r2, #CCM_CBCDR]
+
+        wait_for_ccm_handshake
+
+        /* fabric_mmdc_podf to 0 */
+        ldr     r8, [r2, #CCM_CBCDR]
+        bic     r8, r8, #(0x7 << 3)
+        str     r8, [r2, #CCM_CBCDR]
+
+        wait_for_ccm_handshake
+
+        .endm
+
+        .macro  switch_to_100MHz
+
+        /* check whether periph2_clk is from top path */
+        ldr     r8, [r2, #CCM_CBCDR]
+        ands    r8, #(1 << 26)
+        beq     skip_periph2_clk2_switch_100m
+
+        /* now switch periph2_clk back. */
+        ldr     r8, [r2, #CCM_CBCDR]
+        bic     r8, r8, #(1 << 26)
+        str     r8, [r2, #CCM_CBCDR]
+
+        wait_for_ccm_handshake
+
+        /*
+         * on i.MX6SX, pre_periph2_clk will be always from
+         * pll2_pfd2, so no need to set pre_periph2_clk
+         * parent, just set the mmdc divider directly.
+         */
+skip_periph2_clk2_switch_100m:
+
+        /* fabric_mmdc_podf to 3 so that mmdc is 400 / 4 = 100MHz */
+        ldr     r8, [r2, #CCM_CBCDR]
+        bic     r8, r8, #(0x7 << 3)
+        orr     r8, r8, #(0x3 << 3)
+        str     r8, [r2, #CCM_CBCDR]
+
+        wait_for_ccm_handshake
+
+        .endm
+
+        .macro  switch_to_400MHz
+
+        /* check whether periph2_clk is from top path */
+        ldr     r8, [r2, #CCM_CBCDR]
+        ands    r8, #(1 << 26)
+        beq     skip_periph2_clk2_switch_400m
+
+        /* now switch periph2_clk back. */
+        ldr     r8, [r2, #CCM_CBCDR]
+        bic     r8, r8, #(1 << 26)
+        str     r8, [r2, #CCM_CBCDR]
+
+        wait_for_ccm_handshake
+
+        /*
+         * on i.MX6SX, pre_periph2_clk will be always from
+         * pll2_pfd2, so no need to set pre_periph2_clk
+         * parent, just set the mmdc divider directly.
+         */
+skip_periph2_clk2_switch_400m:
+
+        /* fabric_mmdc_podf to 0 */
+        ldr     r8, [r2, #CCM_CBCDR]
+        bic     r8, r8, #(0x7 << 3)
+        str     r8, [r2, #CCM_CBCDR]
+
+        wait_for_ccm_handshake
+
+        .endm
+
+        .macro  mmdc_clk_lower_100MHz
+
+        /*
+         * Prior to reducing the DDR frequency (at 528/400 MHz),
+         * read the Measure unit count bits (MU_UNIT_DEL_NUM)
+         */
+        ldr     r8, =0x8B8
+        ldr     r6, [r5, r8]
+        /* Original MU unit count */
+        mov     r6, r6, LSR #16
+        ldr     r4, =0x3FF
+        and     r6, r6, r4
+        /* Original MU unit count * 2 */
+        mov     r7, r6, LSL #1
+        /*
+         * Bypass the automatic measure unit when below 100 MHz
+         * by setting the Measure unit bypass enable bit (MU_BYP_EN)
+         */
+        ldr     r6, [r5, r8]
+        orr     r6, r6, #0x400
+        str     r6, [r5, r8]
+        /*
+         * Double the measure count value read in step 1 and program it in the
+         * measurement bypass bits (MU_BYP_VAL) of the MMDC PHY Measure Unit
+         * Register for the reduced frequency operation below 100 MHz
+         */
+        ldr     r6, [r5, r8]
+        ldr     r4, =0x3FF
+        bic     r6, r6, r4
+        orr     r6, r6, r7
+        str     r6, [r5, r8]
+        /* Now perform a Force Measurement. */
+        ldr     r6, [r5, r8]
+        orr     r6, r6, #0x800
+        str     r6, [r5, r8]
+        /* Wait for FRC_MSR to clear. */
+force_measure:
+        ldr     r6, [r5, r8]
+        and     r6, r6, #0x800
+        cmp     r6, #0x0
+        bne     force_measure
+
+        /* For freq lower than 100MHz, need to set RALAT to 2 */
+        ldr     r6, [r5, #0x18]
+        bic     r6, r6, #(0x7 << 6)
+        orr     r6, r6, #(0x2 << 6)
+        str     r6, [r5, #0x18]
+
+        .endm
+
+        .macro  mmdc_clk_above_100MHz
+
+        /* Make sure that the PHY measurement unit is NOT in bypass mode */
+        ldr     r8, =0x8B8
+        ldr     r6, [r5, r8]
+        bic     r6, r6, #0x400
+        str     r6, [r5, r8]
+        /* Now perform a Force Measurement. */
+        ldr     r6, [r5, r8]
+        orr     r6, r6, #0x800
+        str     r6, [r5, r8]
+        /* Wait for FRC_MSR to clear. */
+force_measure1:
+        ldr     r6, [r5, r8]
+        and     r6, r6, #0x800
+        cmp     r6, #0x0
+        bne     force_measure1
+
+        /* For freq higher than 100MHz, need to set RALAT to 5 */
+        ldr     r6, [r5, #0x18]
+        bic     r6, r6, #(0x7 << 6)
+        orr     r6, r6, #(0x5 << 6)
+        str     r6, [r5, #0x18]
+
+        .endm
+
+        .align 3
+ENTRY(imx6sx_lpddr2_freq_change)
+
+        push    {r2 - r8}
+
+        /*
+         * To ensure no page table walks occur in DDR, we
+         * have a another page table stored in IRAM that only
+         * contains entries pointing to IRAM, AIPS1 and AIPS2.
+         * We need to set the TTBR1 to the new IRAM TLB.
+         * Do the following steps:
+         * 1. Flush the Branch Target Address Cache (BTAC)
+         * 2. Set TTBR1 to point to IRAM page table.
+         * 3. Disable page table walks in TTBR0 (PD0 = 1)
+         * 4. Set TTBR0.N=1, implying 0-2G is translated by TTBR0
+         *     and 2-4G is translated by TTBR1.
+         */
+
+        ldr     r6, =iram_tlb_phys_addr
+        ldr     r7, [r6]
+
+        /* Flush the Branch Target Address Cache (BTAC) */
+        ldr     r6, =0x0
+        mcr     p15, 0, r6, c7, c1, 6
+
+        /* Disable Branch Prediction, Z bit in SCTLR. */
+        mrc     p15, 0, r6, c1, c0, 0
+        bic     r6, r6, #0x800
+        mcr     p15, 0, r6, c1, c0, 0
+
+        dsb
+        isb
+        /* Store the IRAM table in TTBR1 */
+        mcr     p15, 0, r7, c2, c0, 1
+
+        /* Read TTBCR and set PD0=1, N = 1 */
+        mrc     p15, 0, r6, c2, c0, 2
+        orr     r6, r6, #0x11
+        mcr     p15, 0, r6, c2, c0, 2
+
+        dsb
+        isb
+
+        /* flush the TLB */
+        ldr     r6, =0x0
+        mcr     p15, 0, r6, c8, c3, 0
+
+        /* Disable L1 data cache. */
+        mrc     p15, 0, r6, c1, c0, 0
+        bic     r6, r6, #0x4
+        mcr     p15, 0, r6, c1, c0, 0
+
+        dsb
+        isb
+
+#ifdef CONFIG_CACHE_L2X0
+        /*
+         * Need to make sure the buffers in L2 are drained.
+         * Performing a sync operation does this.
+         */
+        ldr     r7, =IMX_IO_P2V(MX6Q_L2_BASE_ADDR)
+        mov     r6, #0x0
+        str     r6, [r7, #L2_CACHE_SYNC]
+
+        /*
+         * The second dsb might be needed to keep cache sync (device write)
+         * ordering with the memory accesses before it.
+         */
+        dsb
+        isb
+
+        /* Disable L2. */
+        str     r6, [r7, #0x100]
+#endif
+
+        ldr     r2, =IMX_IO_P2V(MX6Q_CCM_BASE_ADDR)
+        ldr     r3, =IMX_IO_P2V(MX6Q_ANATOP_BASE_ADDR)
+        ldr     r5, =IMX_IO_P2V(MX6Q_MMDC_P0_BASE_ADDR)
+
+        /* Disable Automatic power savings. */
+        ldr     r6, [r5, #MMDC0_MAPSR]
+        orr     r6, r6, #0x1
+        str     r6, [r5, #MMDC0_MAPSR]
+
+        /* MMDC0_MDPDC disable power down timer */
+        ldr     r6, [r5, #MMDC0_MDPDC]
+        bic     r6, r6, #0xff00
+        str     r6, [r5, #MMDC0_MDPDC]
+
+        /* Delay for a while */
+        ldr     r8, =10
+delay:
+        ldr     r7, =0
+cont:
+        ldr     r6, [r5, r7]
+        add     r7, r7, #4
+        cmp     r7, #16
+        bne     cont
+        sub     r8, r8, #1
+        cmp     r8, #0
+        bgt     delay
+
+        /* Make the DDR explicitly enter self-refresh. */
+        ldr     r6, [r5, #MMDC0_MAPSR]
+        orr     r6, r6, #0x200000
+        str     r6, [r5, #MMDC0_MAPSR]
+
+poll_dvfs_set_1:
+        ldr     r6, [r5, #MMDC0_MAPSR]
+        and     r6, r6, #0x2000000
+        cmp     r6, #0x2000000
+        bne     poll_dvfs_set_1
+
+        /* set SBS step-by-step mode */
+        ldr     r6, [r5, #MMDC0_MADPCR0]
+        orr     r6, r6, #0x100
+        str     r6, [r5, #MMDC0_MADPCR0]
+
+        ldr     r6, =100000000
+        cmp     r0, r6
+        bgt     set_ddr_mu_above_100
+        mmdc_clk_lower_100MHz
+
+set_ddr_mu_above_100:
+        ldr     r6, =24000000
+        cmp     r0, r6
+        beq     set_to_24MHz
+
+        ldr     r6, =100000000
+        cmp     r0, r6
+        beq     set_to_100MHz
+
+        switch_to_400MHz
+
+        mmdc_clk_above_100MHz
+
+        b       done
+
+set_to_24MHz:
+        switch_to_24MHz
+        b       done
+set_to_100MHz:
+        switch_to_100MHz
+done:
+        /* clear DVFS - exit from self refresh mode */
+        ldr     r6, [r5, #MMDC0_MAPSR]
+        bic     r6, r6, #0x200000
+        str     r6, [r5, #MMDC0_MAPSR]
+
+poll_dvfs_clear_1:
+        ldr     r6, [r5, #MMDC0_MAPSR]
+        and     r6, r6, #0x2000000
+        cmp     r6, #0x2000000
+        beq     poll_dvfs_clear_1
+
+        /* Enable Automatic power savings. */
+        ldr     r6, [r5, #MMDC0_MAPSR]
+        bic     r6, r6, #0x1
+        str     r6, [r5, #MMDC0_MAPSR]
+
+        ldr     r6, =24000000
+        cmp     r0, r6
+        beq     skip_power_down
+
+        /* Enable MMDC power down timer. */
+        ldr     r6, [r5, #MMDC0_MDPDC]
+        orr     r6, r6, #0x5500
+        str     r6, [r5, #MMDC0_MDPDC]
+
+skip_power_down:
+        /* clear SBS - unblock DDR accesses */
+        ldr     r6, [r5, #MMDC0_MADPCR0]
+        bic     r6, r6, #0x100
+        str     r6, [r5, #MMDC0_MADPCR0]
+
+
+#ifdef CONFIG_CACHE_L2X0
+        /* Enable L2. */
+        ldr     r7, =IMX_IO_P2V(MX6Q_L2_BASE_ADDR)
+        ldr     r6, =0x1
+        str     r6, [r7, #0x100]
+#endif
+
+        /* Enable L1 data cache. */
+        mrc     p15, 0, r6, c1, c0, 0
+        orr     r6, r6, #0x4
+        mcr     p15, 0, r6, c1, c0, 0
+
+        /* Restore the TTBCR */
+        dsb
+        isb
+
+        /* Read TTBCR and set PD0=0, N = 0 */
+        mrc     p15, 0, r6, c2, c0, 2
+        bic     r6, r6, #0x11
+        mcr     p15, 0, r6, c2, c0, 2
+        dsb
+        isb
+
+        /* flush the TLB */
+        ldr     r6, =0x0
+        mcr     p15, 0, r6, c8, c3, 0
+
+        dsb
+        isb
+
+        /* Enable Branch Prediction, Z bit in SCTLR. */
+        mrc     p15, 0, r6, c1, c0, 0
+        orr     r6, r6, #0x800
+        mcr     p15, 0, r6, c1, c0, 0
+
+        /* Flush the Branch Target Address Cache (BTAC) */
+        ldr     r6, =0x0
+        mcr     p15, 0, r6, c7, c1, 6
+
+        nop
+        nop
+        nop
+        nop
+        nop
+
+        nop
+        nop
+        nop
+        nop
+        nop
+
+        nop
+        nop
+        nop
+        nop
+        nop
+
+        nop
+        nop
+        nop
+        nop
+        nop
+
+        nop
+        nop
+        nop
+        nop
+        nop
+
+        /* Restore registers */
+        pop     {r2 - r8}
+        mov     pc, lr
diff --git a/arch/arm/mach-imx/mmdc.c b/arch/arm/mach-imx/mmdc.c
index 7a9686ad994c..a6f547b3aa33 100644
--- a/arch/arm/mach-imx/mmdc.c
+++ b/arch/arm/mach-imx/mmdc.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2011 Freescale Semiconductor, Inc.
+ * Copyright 2011-2014 Freescale Semiconductor, Inc.
  * Copyright 2011 Linaro Ltd.
  *
  * The code contained herein is licensed under the GNU General Public
@@ -21,6 +21,12 @@
 #define BP_MMDC_MAPSR_PSD       0
 #define BP_MMDC_MAPSR_PSS       4
 
+#define MMDC_MDMISC             0x18
+#define BM_MMDC_MDMISC_DDR_TYPE 0x18
+#define BP_MMDC_MDMISC_DDR_TYPE 0x3
+
+static int ddr_type;
+
 static int imx_mmdc_probe(struct platform_device *pdev)
 {
         struct device_node *np = pdev->dev.of_node;
@@ -31,6 +37,13 @@ static int imx_mmdc_probe(struct platform_device *pdev)
         mmdc_base = of_iomap(np, 0);
         WARN_ON(!mmdc_base);
 
+        reg = mmdc_base + MMDC_MDMISC;
+        /* Get ddr type */
+        val = readl_relaxed(reg);
+        val &= BM_MMDC_MDMISC_DDR_TYPE;
+        val >>= BP_MMDC_MDMISC_DDR_TYPE;
+        ddr_type = val;
+
         reg = mmdc_base + MMDC_MAPSR;
 
         /* Enable automatic power saving */
@@ -51,6 +64,11 @@ static int imx_mmdc_probe(struct platform_device *pdev)
         return 0;
 }
 
+int imx_mmdc_get_ddr_type(void)
+{
+        return ddr_type;
+}
+
 static struct of_device_id imx_mmdc_dt_ids[] = {
         { .compatible = "fsl,imx6q-mmdc", },
         { /* sentinel */ }