ENGR00179574: MX6- Add bus frequency scaling support

Add support for scaling the bus frequency (both DDR
and ahb_clk).
The DDR and AHB_CLK are dropped to 24MHz when all devices
that need high AHB frequency are disabled and the CORE
frequency is at the lowest setpoint.
The DDR is dropped to 400MHz for the video playback usecase.
In this mode the GPU, FEC, SATA etc are disabled.

To scale the bus frequency, its necessary that all cores
except the core that is executing the DDR frequency change
are in WFE. This is achieved by generating interrupts on
un-used interrupts (Int no 139, 144, 145 and 146).

Signed-off-by: Ranjani Vaidyanathan <ra5478@freescale.com>

1 files changed, 873 insertions, 0 deletions

diff --git a/arch/arm/mach-mx6/mx6_ddr_freq.S b/arch/arm/mach-mx6/mx6_ddr_freq.S
new file mode 100644
index 000000000000..766d867ee1c4
--- /dev/null
+++ b/arch/arm/mach-mx6/mx6_ddr_freq.S
@@ -0,0 +1,873 @@
+/*
+ * Copyright (C) 2011-2012 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 <mach/hardware.h>
+
+	.macro   switch_to_528MHz
+
+	/* DDR freq change to 528MHz */
+
+	/* check if periph_clk_sel is already set */
+	ldr     r0, [r6, #0x14]
+	and     r0, r0, #0x2000000
+	cmp     r0, #0x2000000
+	beq	    switch_pre_periph_clk_528
+
+	/* Step 1: Change periph_clk to be sourced from pll3_clk. */
+	/* Ensure PLL3 is the source and set the divider to 1. */
+	ldr     r0, [r6, #0x18]
+	bic     r0, r0, #0x3000
+	str     r0, [r6, #0x18]
+
+	ldr     r0, [r6, #0x14]
+	bic     r0, r0, #0x38000000
+	str     r0, [r6, #0x14]
+
+	/* Now switch periph_clk to pll3_main_clk. */
+	ldr     r0, [r6, #0x14]
+	orr     r0, r0, #0x2000000
+	str     r0, [r6, #0x14]
+
+periph_clk_switch3:
+	ldr      r0, [r6, #0x48]
+	cmp    r0, #0
+	bne     periph_clk_switch3
+
+switch_pre_periph_clk_528:
+
+	/* Now switch pre_periph_clk to PLL2_528MHz. */
+	ldr     r0, [r6, #0x18]
+	bic     r0, r0, #0xC0000
+	str     r0, [r6, #0x18]
+
+	/* Set the MMDC_DIV=1, AXI_DIV = 2, AHB_DIV=4 (need to maintain GPT divider). */
+	ldr     r0, [r6, #0x14]
+	ldr     r2, =0x3F1D00
+	bic     r0, r0, r2
+	orr     r0, r0, #0x10000
+	orr     r0, r0, #0xD00
+	str     r0, [r6, #0x14]
+
+wait_div_update1:
+	ldr     r0, [r6, #0x48]
+	cmp     r0, #0
+	bne     wait_div_update1
+
+	/* Now switch periph_clk back. */
+	ldr     r0, [r6, #0x14]
+	bic     r0, r0, #0x2000000
+	str     r0, [r6, #0x14]
+
+periph_clk_switch4:
+	ldr     r0, [r6, #0x48]
+	cmp     r0, #0
+	bne     periph_clk_switch4
+
+	/* Change the GPT divider so that its at 6MHz. */
+	ldr     r0, [r6, #0x1C]
+	bic     r0, r0, #0x3F
+	orr     r0, r0, #0xB
+	str     r0, [r6, #0x1C]
+
+	.endm
+
+	.macro   switch_to_400MHz
+
+	/* check if periph_clk_sel is already set */
+	ldr     r0, [r6, #0x14]
+	and     r0, r0, #0x2000000
+	cmp 	 r0, #0x2000000
+	beq	 switch_pre_periph_clk_400
+
+	/* Step 1: Change periph_clk to be sourced from pll3_clk. */
+	/* Ensure PLL3 is the source and set the divider to 1. */
+	ldr     r0, [r6, #0x18]
+	bic     r0, r0, #0x3000
+	str     r0, [r6, #0x18]
+
+	ldr     r0, [r6, #0x14]
+	bic     r0, r0, #0x38000000
+	str     r0, [r6, #0x14]
+
+	/* Now switch periph_clk to pll3_main_clk. */
+	ldr     r0, [r6, #0x14]
+	orr     r0, r0, #0x2000000
+	str     r0, [r6, #0x14]
+
+periph_clk_switch5:
+	ldr      r0, [r6, #0x48]
+	cmp    r0, #0
+	bne     periph_clk_switch5
+
+switch_pre_periph_clk_400:
+
+    /* Now switch pre_periph_clk to PFD_400MHz. */
+	ldr     r0, [r6, #0x18]
+	bic     r0, r0, #0xC0000
+	orr     r0, r0, #0x40000
+	str     r0, [r6, #0x18]
+
+	/* Set the MMDC_DIV=1, AXI_DIV = 2, AHB_DIV=3 (need to maintain GPT divider). */
+	ldr     r0, [r6, #0x14]
+	ldr     r2, =0x3F1D00
+	bic     r0, r0, r2
+	orr     r0, r0, #0x10000
+	orr     r0, r0, #0x900
+	str     r0, [r6, #0x14]
+
+wait_div_update400:
+	ldr     r0, [r6, #0x48]
+	cmp     r0, #0
+	bne     wait_div_update400
+
+	/* Now switch periph_clk back. */
+	ldr     r0, [r6, #0x14]
+	bic     r0, r0, #0x2000000
+	str     r0, [r6, #0x14]
+
+periph_clk_switch6:
+	ldr      r0, [r6, #0x48]
+	cmp    r0, #0
+	bne     periph_clk_switch6
+
+	/* Change the GPT divider so that its at 6MHz. */
+	ldr     r0, [r6, #0x1C]
+	bic     r0, r0, #0x3F
+	orr     r0, r0, #0xB
+	str     r0, [r6, #0x1C]
+
+	.endm
+
+	.macro   switch_to_50MHz
+
+	/* Set DDR to 50MHz. */
+	/* check if periph_clk_sel is already set */
+	ldr     r0, [r6, #0x14]
+	and     r0, r0, #0x2000000
+	cmp 	 r0, #0x2000000
+	beq	 switch_pre_periph_clk_50
+
+	/* Set the periph_clk to be sourced from PLL2_PFD_200M */
+	/* Step 1: Change periph_clk to be sourced from pll3_clk. */
+	/* Ensure PLL3 is the source and set the divider to 1. */
+	ldr     r0, [r6, #0x18]
+	bic     r0, r0, #0x3000
+	str     r0, [r6, #0x18]
+
+	ldr     r0, [r6, #0x14]
+	bic     r0, r0, #0x38000000
+	str     r0, [r6, #0x14]
+
+	/* Now switch periph_clk to pll3_main_clk. */
+	ldr     r0, [r6, #0x14]
+	orr     r0, r0, #0x2000000
+	str     r0, [r6, #0x14]
+
+periph_clk_switch_50:
+	ldr      r0, [r6, #0x48]
+	cmp    r0, #0
+	bne     periph_clk_switch_50
+
+switch_pre_periph_clk_50:
+
+	/* Now switch pre_periph_clk to PFD_200MHz. */
+	ldr     r0, [r6, #0x18]
+	orr     r0, r0, #0xC0000
+	str     r0, [r6, #0x18]
+
+	/* Set the MMDC_DIV=4, AXI_DIV = 4, AHB_DIV=6 (need to maintain GPT divider). */
+	ldr     r0, [r6, #0x14]
+	ldr     r2, =0x3F1C00
+	bic     r0, r0, r2
+
+	orr     r0, r0, #0x180000
+	orr     r0, r0, #0x10000
+
+	/* If changing AHB divider remember to change the IPGPER divider too below. */
+	 orr     r0, r0, #0xC00
+	 str     r0, [r6, #0x14]
+
+wait_div_update_50:
+	ldr      r0, [r6, #0x48]
+	cmp    r0, #0
+	bne     wait_div_update_50
+
+	/* Now switch periph_clk back. */
+	ldr     r0, [r6, #0x14]
+	bic     r0, r0, #0x2000000
+	str     r0, [r6, #0x14]
+
+periph_clk_switch2:
+	ldr      r0, [r6, #0x48]
+	cmp    r0, #0
+	bne     periph_clk_switch2
+
+	/* Change the GPT divider so that its at 6MHz. */
+	ldr     r0, [r6, #0x1C]
+	bic     r0, r0, #0x3F
+	orr     r0, r0, #0x3
+	str     r0, [r6, #0x1C]
+
+	.endm
+
+	.macro   switch_to_24MHz
+	/* Change the freq now */
+	/* Try setting DDR to 24MHz. */
+	/* Source it from the periph_clk2 */
+	/* Ensure the periph_clk2 is sourced from 24MHz
+	and the divider is 1. */
+	ldr     r0, [r6, #0x18]
+	bic     r0, r0, #0x3000
+	orr     r0, r0, #0x1000
+	str     r0, [r6, #0x18]
+
+	ldr     r0, [r6, #0x14]
+	bic     r0, r0, #0x38000000
+	str     r0, [r6, #0x14]
+
+	/* Now switch periph_clk to 24MHz. */
+	ldr     r0, [r6, #0x14]
+	orr     r0, r0, #0x2000000
+	str     r0, [r6, #0x14]
+
+periph_clk_switch1:
+	ldr      r0, [r6, #0x48]
+	cmp    r0, #0
+	bne     periph_clk_switch1
+
+	/* Change all the dividers to 1. */
+	ldr     r0, [r6, #0x14]
+	ldr     r2, =0x3F1C00
+	bic     r0, r0, r2
+	str     r0, [r6, #0x14]
+
+	/* Wait for the divider to change. */
+wait_div_update:
+	ldr      r0, [r6, #0x48]
+	cmp    r0, #0
+	bne     wait_div_update
+
+	/* Change the GPT divider so that its at 6MHz. */
+	ldr     r0, [r6, #0x1C]
+	bic     r0, r0, #0x3F
+	orr     r0, r0, #0x1
+	str     r0, [r6, #0x1C]
+
+	.endm
+
+/*
+ *  mx6_ddr_freq_change
+ *
+ *  Idle the processor (eg, wait for interrupt).
+ *  Make sure DDR is in self-refresh.
+ *  IRQs are already disabled.
+ */
+ENTRY(mx6_ddr_freq_change)
+
+	stmfd   sp!, {r4,r5,r6, r7, r8, r9, r10, r11}      @ Save registers
+
+	ldr     r6, =CCM_BASE_ADDR
+	add     r6, r6, #PERIPBASE_VIRT
+	ldr     r5, =MMDC_P0_BASE_ADDR
+	add     r5, r5, #PERIPBASE_VIRT
+	ldr     r7, =MX6Q_IOMUXC_BASE_ADDR
+	add     r7, r7, #PERIPBASE_VIRT
+
+	mov     r4, r0				@save new freq requested
+	mov     r8, r1				@save the ddr settings for the new rate
+	mov     r9, r2				@save the mode DDR is currently in (DLL ON/OFF)
+	mov     r11, r3			@save iomux offsets
+
+ddr_freq_change:
+	/* Make sure no TLB miss will occur when the DDR is in self refresh. */
+	/* Invalidate TLB single entry to ensure that the address is not
+	  * already in the TLB.
+	  */
+
+	adr   r10, ddr_freq_change               @Address in this function.
+
+
+	mcr   p15, 0, r10, c8, c7, 1      @//@ Make sure freq code address
+	                                       @//  @ is not already in TLB.
+	mcr   p15, 0, r6, c8, c7, 1      @//@ Make sure CCM address
+	                                         @//@ is not already in TLB.
+	mcr   p15, 0, r5, c8, c7, 1      @//@ make sure MMDC address
+	                                         @//@ is not already in TLB.
+	mcr   p15, 0, r7, c8, c7, 1      @//@ make sure IOMUX address
+	                                         @//@ is not already in TLB.
+
+	mrc   p15, 0, r0, c10, c0, 0    @//@ Read the TLB lockdown register
+	orr    r0, r0, #1                    @//@ Set the Preserve bit.
+	mcr   p15, 0, r0, c10, c0, 0    @//@ Write to the lockdown register
+
+	ldr    r2, [r6]                        @ TLB will miss,
+	                                         @CCM address will be loaded
+	ldr    r2, [r5]                        @ TLB will miss,
+	                                         @MMDC address will be loaded
+	ldr    r2, [r7]                        @ TLB will miss,
+	                                         @IOMUX will be loaded
+
+	ldr    r2, [r8]                        @ Get the DDR settings.
+
+	ldr    r2, [r10]                        @ TLB will miss
+
+	ldr    r2, [r11]				@Get the IOMUX settings
+
+	mrc  p15, 0, r0, c10, c0, 0      @//@ Read the lockdown register
+	                                             @//@ (victim will be incremented)
+	bic   r0, r0, #1                     @//@ Clear the preserve bit
+	mcr   p15, 0, r0, c10, c0, 0     @//@ Write to the lockdown register
+
+	/* Disable automatic power saving. */
+
+	ldr    r0, [r5, #0x404]
+	orr    r0, r0, #0x01
+	str    r0, [r5, #0x404]
+
+	/* Disable MMDC power down timer. */
+	/*MMDC0_MDPDC disable power down timer */
+	ldr    r0, [r5, #0x4]
+	bic    r0, r0, #0xff00
+	str    r0, [r5, #0x4]
+
+	/* set CON_REG */
+	ldr     r0, =0x8000
+		str     r0, [r5, #0x1C]
+poll_conreq_set_1:
+	ldr     r0, [r5, #0x1C]
+	and     r0, r0, #0x4000
+	cmp	    r0, #0x4000
+	bne     poll_conreq_set_1
+
+	/*setmem /32 0x021b001c = 0x00008010 //Precharge all on cs0  */
+	/*setmem /32 0x021b001c = 0x00008018 //Precharge all on cs1 */
+	ldr     r0, =0x00008010
+	str     r0, [r5, #0x1C]
+	ldr     r0, =0x00008018
+	str     r0, [r5, #0x1C]
+
+	/* if requested frequency is greater than 300MHz go to DLL on mode */
+	ldr     r1, =300000000
+	cmp     r4, r1
+	bge     dll_on_mode
+
+dll_off_mode:
+
+	/* if DLL is currently on, turn it off
+	cmp    	r9, #1
+	beq 	continue_dll_off_1
+
+	/* setmem /32 0x021b001c = 0x00018031 //Rtt_NOM off + set dll off, cs 0 */
+	/* setmem /32 0x021b001c = 0x00018039 //Rtt_NOM off + set dll off, cs 1 */
+	ldr     r0, =0x00018031
+	str     r0, [r5, #0x1C]
+
+	ldr     r0, =0x00018039
+	str     r0, [r5, #0x1C]
+
+	ldr      r1, =10
+delay1a:
+	ldr      r2, =0
+cont1a:
+	ldr      r0, [r5, r2]
+	add     r2, r2, #4
+	cmp    r2, #16
+	bne     cont1a
+	sub     r1, r1, #1
+	cmp    r1, #0
+	bgt     delay1a
+
+continue_dll_off_1:
+
+	/* set DVFS - enter self refresh mode */
+	ldr     r0, [r5, #0x404]
+	orr    r0, r0, #0x200000
+	str     r0, [r5, #0x404]
+
+	/* de-assert con_req */
+	mov    r0, #0x0
+	str    r0, [r5, #0x1C]
+
+poll_dvfs_set_1:
+	ldr     r0, [r5, #0x404]
+	and     r0, r0, #0x2000000
+	cmp	    r0, #0x2000000
+	bne   	poll_dvfs_set_1
+
+	ldr     r1, =24000000
+	cmp     r4, r1
+	beq     switch_freq_24
+
+	switch_to_50MHz
+	b continue_dll_off_2
+
+switch_freq_24:
+	switch_to_24MHz
+
+continue_dll_off_2:
+
+	/* set SBS - block ddr accesses */
+	ldr     r0, [r5, #0x410]
+	orr    	r0, r0, #0x100
+	str     r0, [r5, #0x410]
+
+	/* clear DVFS - exit from self refresh mode */
+	ldr     r0, [r5, #0x404]
+	bic     r0, r0, #0x200000
+	str     r0, [r5, #0x404]
+
+poll_dvfs_clear_1:
+	ldr     r0, [r5, #0x404]
+	and     r0, r0, #0x2000000
+	cmp     r0, #0x2000000
+	beq     poll_dvfs_clear_1
+
+	/* if DLL was previously on, continue DLL off routine
+	cmp    	r9, #1
+	beq 	continue_dll_off_3
+
+	/* setmem /32 0x021b001c = 0x00018031 //Rtt_NOM off + set dll off, cs 0 */
+	/* setmem /32 0x021b001c = 0x00018039 //Rtt_NOM off + set dll off, cs 1 */
+	ldr     r0, =0x00018031
+	str     r0, [r5, #0x1C]
+
+	ldr     r0, =0x00018039
+	str     r0, [r5, #0x1C]
+
+	/* setmem /32 0x021b001c = 0x04208030 //write mode reg MR0: CL=6, wr=6 ,cs 0  */
+	/* setmem /32 0x021b001c = 0x04208038 //write mode reg MR0: CL=6, wr=6 ,cs 1  */
+	ldr     r0, =0x08208030
+	str     r0, [r5, #0x1C]
+
+	ldr     r0, =0x08208038
+	str     r0, [r5, #0x1C]
+
+	/* setmem /32 0x021b001c = 0x02088032 //write mode reg MR2 , CWL=6 ,cs0 */
+	/* setmem /32 0x021b001c = 0x0208803A //write mode reg MR2 , CWL=6 ,cs1 */
+	ldr     r0, =0x00088032
+	str     r0, [r5, #0x1C]
+
+	ldr     r0, =0x0008803A
+	str     r0, [r5, #0x1C]
+
+	/* double refresh ????
+	 ldr     r0, =0x00001800
+	 str     r0, [r5, #0x20]*/
+
+	/* delay for a while. */
+	ldr      r1, =4
+delay_1:
+	ldr      r2, =0
+cont_1:
+	ldr      r0, [r5, r2]
+	add     r2, r2, #4
+	cmp    r2, #16
+	bne     cont_1
+	sub     r1, r1, #1
+	cmp    r1, #0
+	bgt     delay_1
+
+	/* MMDC0_MDCFG0 see spread sheet for timings, CAS=6 */
+	ldr     r0, [r5, #0x0C]
+	bic     r0, r0, #0xf
+	orr     r0, r0, #0x3
+	str     r0, [r5, #0x0C]
+
+	/* MMDC0_MDCFG1 see spread sheet for timings, tCWL=6 */
+	ldr     r0, [r5, #0x10]
+	bic     r0, r0, #0x7
+	orr     r0, r0, #0x4
+	str     r0, [r5, #0x10]
+
+	/* Enable bank interleaving, Address mirror on, WALAT = 0x1, RALAT = 0x2, DDR2_EN = 0 */
+	/*setmem /32 0x021b0018 = 0x00091680 */
+	ldr     r0, =0x00091680
+	str     r0, [r5, #0x18]
+
+	/* enable dqs pull down in the IOMUX. */
+	/*
+	setmem /32 0x020e05a8 = 0x00003030 // IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS0 - DSE=110
+	setmem /32 0x020e05b0 = 0x00003030 // IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS1 - DSE=110
+	setmem /32 0x020e0524 = 0x00003030 // IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS2 - DSE=110
+	setmem /32 0x020e051c = 0x00003030 // IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS3 - DSE=110
+	setmem /32 0x020e0518 = 0x00003030 // IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS4 - DSE=110
+	setmem /32 0x020e050c = 0x00003030 // IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS5 - DSE=110
+	setmem /32 0x020e05b8 = 0x00003030 // IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS6 - DSE=110
+	setmem /32 0x020e05c0 = 0x00003030 // IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS7 - DSE=110
+	*/
+	ldr    r1, [r11]                   @size of array
+	add   r11, r11, #8            @skip first eight bytes in array
+	ldr     r2, =0x3028
+update_iomux:
+	ldr    r0, [r11, #0x0]           @ offset
+	ldr    r3, [r7, r0]
+	bic    r3, r3, r2		      @ Clear the DSE, PUE and PKE bits
+	orr	 r3, r3, #0x3000          @ Enable the Pull downs and lower the drive strength.
+	orr    r3, r3, #0x28
+	str    r3, [r7, r0]
+	add   r11, r11, #8
+	sub   r1, r1, #1
+	cmp  r1, #0
+	bgt   update_iomux
+
+	/*  ODT disabled */
+	/* setmem /32 0x021b0818 = 0x0      // DDR_PHY_P0_MPODTCTRL */
+	/* setmem /32 0x021b4818 = 0x0      // DDR_PHY_P1_MPODTCTRL */
+	ldr    r0, =0x0
+	ldr    r2, =0x818
+	str    r0, [r5, r2]
+	ldr    r2, =0x4818
+	str    r0, [r5, r2]
+
+	/* DQS gating disabled */
+	/* setmem /32 0x021b083c = 0x233f033f */
+	ldr    r2, =0x83c
+	ldr    r0, [r5, r2]
+	orr     r0, r0, #0x20000000
+	str    r0, [r5, r2]
+
+	ldr    r2, =0x483c
+	ldr    r0, [r5, r2]
+	orr     r0, r0, #0x20000000
+	str    r0, [r5, r2]
+
+	/* MMDC0_MAPSR adopt power down enable */
+	/* setmem /32 0x021b0404 = 0x00011006 */
+	ldr    r0, [r5, #0x404]
+	bic    r0, r0, #0x01
+	str    r0, [r5, #0x404]
+
+	/* frc_msr + mu bypass*/
+	ldr       r0, =0x00000060
+	str       r0, [r5, #0x8b8]
+	ldr       r2, =0x48b8
+	str       r0, [r5, r2]
+	ldr       r0, =0x00000460
+	str       r0, [r5, #0x8b8]
+	ldr       r2, =0x48b8
+	str       r0, [r5, r2]
+	ldr       r0, =0x00000c60
+	str       r0, [r5, #0x8b8]
+	ldr       r2, =0x48b8
+	str       r0, [r5, r2]
+
+continue_dll_off_3:
+
+	/* clear SBS - unblock accesses to DDR */
+	ldr     r0, [r5, #0x410]
+	bic     r0, r0, #0x100
+	str     r0, [r5, #0x410]
+
+	mov		r0, #0x0
+	str		r0, [r5, #0x1C]
+poll_conreq_clear_1:
+	ldr		r0, [r5, #0x1C]
+	and	    r0, r0, #0x4000
+	cmp	    r0, #0x4000
+	beq     poll_conreq_clear_1
+
+	b        done
+
+dll_on_mode:
+	/* assert DVFS - enter self refresh mode */
+	ldr     r0, [r5, #0x404]
+	orr    	r0, r0, #0x200000
+	str     r0, [r5, #0x404]
+
+	/* de-assert CON_REQ */
+	mov    r0, #0x0
+	str    r0, [r5, #0x1C]
+
+	/* poll DVFS ack */
+poll_dvfs_set_2:
+	ldr     r0, [r5, #0x404]
+	and     r0, r0, #0x2000000
+	cmp     r0, #0x2000000
+	bne     poll_dvfs_set_2
+
+	ldr    r1, =528000000
+	cmp  r4, r1
+	beq   switch_freq_528
+
+	switch_to_400MHz
+
+	b continue_dll_on
+
+switch_freq_528:
+	switch_to_528MHz
+
+continue_dll_on:
+
+	/* set step-by-step mode */
+	ldr     r0, [r5, #0x410]
+	orr     r0, r0, #0x100
+	str     r0, [r5, #0x410]
+
+	/* clear DVFS - exit self refresh mode */
+	ldr     r0, [r5, #0x404]
+	bic     r0, r0, #0x200000
+	str     r0, [r5, #0x404]
+
+poll_dvfs_clear_2:
+	ldr     r0, [r5, #0x404]
+	and     r0, r0, #0x2000000
+	cmp     r0, #0x2000000
+	beq     poll_dvfs_clear_2
+
+	/* if DLL is currently off, turn it back on */
+	cmp    	r9, #0
+	beq 	update_calibration
+
+	ldr     r0, =0xa5390003
+	str     r0, [r5, #0x800]
+	ldr     r2, =0x4800
+	str     r0, [r5, r2]
+
+	/* enable DQS gating */
+	ldr    r2, =0x83c
+	ldr    r0, [r5, r2]
+	bic    r0, r0, #0x20000000
+	str    r0, [r5, r2]
+
+	ldr    r2, =0x483c
+	ldr    r0, [r5, r2]
+	bic    r0, r0, #0x20000000
+	str    r0, [r5, r2]
+
+	/* force measure */
+	ldr       r0, =0x00000800
+	str       r0, [r5, #0x8b8]
+	ldr       r2, =0x48b8
+	str       r0, [r5, r2]
+
+	/* delay for while */
+	ldr      r1, =4
+delay5:
+	ldr      r2, =0
+cont5:
+	ldr      r0, [r5, r2]
+	add     r2, r2, #4
+	cmp    r2, #16
+	bne     cont5
+	sub     r1, r1, #1
+	cmp    r1, #0
+	bgt     delay5
+
+	/* Disable dqs pull down in the IOMUX. */
+	/*
+	setmem /32 0x020e05a8 = 0x00000030 // IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS0 - DSE=110
+	setmem /32 0x020e05b0 = 0x00000030 // IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS1 - DSE=110
+	setmem /32 0x020e0524 = 0x00000030 // IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS2 - DSE=110
+	setmem /32 0x020e051c = 0x00000030 // IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS3 - DSE=110
+	setmem /32 0x020e0518 = 0x00000030 // IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS4 - DSE=110
+	setmem /32 0x020e050c = 0x00000030 // IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS5 - DSE=110
+	setmem /32 0x020e05b8 = 0x00000030 // IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS6 - DSE=110
+	setmem /32 0x020e05c0 = 0x00000030 // IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS7 - DSE=110
+	*/
+	ldr    r1, [r11]                   @size of array
+	add  r11, r11, #8             @skip first eight bytes in array
+update_iomux1:
+	ldr    r0, [r11, #0x0]       @ offset
+	ldr    r3, [r11, #0x4]
+	str    r3, [r7, r0]		  @Store the original IOMUX value read during boot
+	add   r11, r11, #8
+	sub   r1, r1, #1
+	cmp  r1, #0
+	bgt   update_iomux1
+
+	/* config ESDCTL timings to 528MHz:
+	@// setmem /32 0x021b000c = 0x555A7975		@// MMDC0_MDCFG0 see spread sheet for timings
+	@//setmem /32 0x021b0010 = 0xFF538E64		      @// MMDC0_MDCFG1 see spread sheet for timings
+	@//setmem /32 0x021b0014 = 0x01ff00db               @// MMDC0_MDCFG2 - tRRD - 4ck; tWTR - 4ck; tRTP - 4ck; tDLLK - 512ck
+	@//setmem /32 0x021b0018 = 0x00081740             @// MMDC0_MDMISC, RALAT=0x5 (original value)
+	*/
+
+	ldr     r0, [r5, #0x0C]
+	bic     r0, r0, #0xf
+	orr     r0, r0, #0x5
+	str     r0, [r5, #0x0C]
+
+	ldr     r0, [r5, #0x10]
+	bic     r0, r0, #0x7
+	orr     r0, r0, #0x4
+	str     r0, [r5, #0x10]
+
+	/* update MISC register: WALAT, RALAT */
+	ldr       r0, =0x00081740
+	str       r0, [r5, #0x18]
+
+	/*configure ddr devices to dll on, odt
+	@//setmem /32 0x021b001c = 0x00428031
+	@//setmem /32 0x021b001c = 0x00428039
+	*/
+	ldr      r0, =0x00028031
+	str      r0, [r5, #0x1C]
+
+	ldr      r0, =0x00028039
+	str      r0, [r5, #0x1C]
+
+	/* delay for while  */
+	ldr      r1, =4
+delay7:
+	ldr      r2, =0
+cont7:
+	ldr      r0, [r5, r2]
+	add     r2, r2, #4
+	cmp    r2, #16
+	bne     cont7
+	sub     r1, r1, #1
+	cmp    r1, #0
+	bgt     delay7
+
+	/* reset dll
+	@// setmem /32 0x021b001c = 0x09208030
+	@// setmem /32 0x021b001c = 0x09208038
+	*/
+	ldr       r0, =0x09208030
+	str       r0, [r5, #0x1C]
+
+	ldr       r0, =0x09208038
+	str       r0, [r5, #0x1C]
+
+	/* delay for while  */
+	ldr      r1, =100
+delay8:
+	ldr      r2, =0
+cont8:
+	ldr      r0, [r5, r2]
+	add     r2, r2, #4
+	cmp    r2, #16
+	bne     cont8
+	sub     r1, r1, #1
+	cmp    r1, #0
+	bgt     delay8
+
+	/* tcwl=6:
+	@//setmem /32 0x021b001c = 0x04088032
+	@//setmem /32 0x021b001c = 0x0408803a
+	*/
+	ldr      r0, =0x04088032
+	str      r0, [r5, #0x1C]
+
+	ldr      r0, =0x0408803a
+	str      r0, [r5, #0x1C]
+
+	ldr      r0, =0x00428031
+	str      r0, [r5, #0x1C]
+
+	ldr      r0, =0x00428039
+	str      r0, [r5, #0x1C]
+
+	/* tcl=8
+	@// setmem /32 0x021b001c = 0x08408030
+	@// setmem /32 0x021b001c = 0x08408038
+	*/
+	ldr       r0, =0x08408030
+	str       r0, [r5, #0x1C]
+
+	ldr       r0, =0x08408038
+	str       r0, [r5, #0x1C]
+
+	/* issue a zq command
+	@// setmem /32 0x021b001c = 0x04000040
+	@// setmem /32 0x021b001c = 0x04000048
+	 */
+	ldr       r0, =0x04008040
+	str       r0, [r5, #0x1C]
+
+	ldr       r0, =0x04008048
+	str       r0, [r5, #0x1C]
+
+	/* ESDCTL ODT enable
+	@//setmem /32 0x021b0818 = 0x00022225      @// DDR_PHY_P0_MPODTCTRL
+	@//setmem /32 0x021b4818 = 0x00022225      @// DDR_PHY_P1_MPODTCTRL
+	*/
+	ldr       r0, =0x00022225
+	str       r0, [r5, #0x818]
+	ldr       r2, =0x4818
+	str       r0, [r5, r2]
+
+	/* delay for while */
+	ldr      r1, =40
+delay15:
+	ldr      r2, =0
+cont15:
+	ldr      r0, [r5, r2]
+	add     r2, r2, #4
+	cmp    r2, #16
+	bne     cont15
+	sub     r1, r1, #1
+	cmp    r1, #0
+	bgt     delay15
+
+	/* MMDC0_MAPSR adopt power down enable */
+	/* setmem /32 0x021b0404 = 0x00011006 */
+	ldr    r0, [r5, #0x404]
+	bic    r0, r0, #0x01
+	str    r0, [r5, #0x404]
+
+	/* Enable MMDC power down timer. */
+	ldr    r0, [r5, #0x4]
+	orr    r0, r0, #0x5500
+	str    r0, [r5, #0x4]
+
+update_calibration:
+	/* Write the new calibration values. */
+	ldr     r1, [r8]             @size of array
+	add   r8, r8, #8             @skip first eight bytes in array
+update_calib:
+	ldr    r0, [r8, #0x0]       @ offset
+	ldr    r3, [r8, #0x4]       @ value
+	str    r3, [r5, r0]
+	add   r8, r8, #8
+	sub   r1, r1, #1
+	cmp  r1, #0
+	bgt   update_calib
+
+	/* Perform a force measurement. */
+	ldr     r0, =0x800
+	str     r0, [r5, #0x8B8]
+	ldr      r2, =0x48B8
+	str     r0, [r5, r2]
+
+	/* clear SBS - unblock DDR accesses */
+	ldr     r0, [r5, #0x410]
+	bic     r0, r0, #0x100
+	str     r0, [r5, #0x410]
+
+	mov		r0, #0x0
+	str		r0, [r5, #0x1C]
+poll_conreq_clear_2:
+	ldr		r0, [r5, #0x1C]
+	and	    r0, r0, #0x4000
+	cmp	    r0, #0x4000
+	beq     poll_conreq_clear_2
+
+done:
+
+	/* Restore registers */
+
+	ldmfd sp!, {r4,r5,r6, r7, r8, r9, r10, r11}
+
+	mov     pc, lr
+
+	.type   mx6_do_ddr_freq_change, #object
+ENTRY(mx6_do_ddr_freq_change)
+	.word   mx6_ddr_freq_change
+	.size    mx6_ddr_freq_change, . - mx6_ddr_freq_change