arch/arm/mach-imx/imx6sl_lpm_wfi.S

   1 /*
   2  * Copyright (C) 2015 Freescale Semiconductor, Inc. All Rights Reserved.
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; either version 2 of the license, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in teh hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  12  * GNU General Public License for more details.
  13  */
  14
  15 #include <linux/linkage.h>
  16
  17 #define PM_INFO_PM_INFO_SIZE_OFFSET     0x0
  18 #define PM_INFO_TTBR_OFFSET             0x4
  19 #define PM_INFO_MMDC_V_OFFSET           0x8
  20 #define PM_INFO_IOMUXC_V_OFFSET         0xc
  21 #define PM_INFO_CCM_V_OFFSET            0x10
  22 #define PM_INFO_L2_V_OFFSET             0x14
  23 #define PM_INFO_ANATOP_V_OFFSET         0x18
  24 #define PM_INFO_IO_NUM_OFFSET           0x1c
  25 #define PM_INFO_IO_VAL_OFFSET           0x20
  26
  27 #define MX6Q_MMDC_MAPSR                 0x404
  28 #define MX6Q_MMDC_MPDGCTRL0             0x83c
  29
  30 .global mx6sl_lpm_wfi_start
  31 .global mx6sl_lpm_wfi_end
  32
  33         .macro  pll_do_wait_lock
  34 1:
  35         ldr     r7, [r10, r8]
  36         ands    r7, #0x80000000
  37         beq     1b
  38
  39         .endm
  40
  41         .macro  ccm_do_wait
  42 2:
  43         ldr     r7, [r10, #0x48]
  44         cmp     r7, #0x0
  45         bne     2b
  46
  47         .endm
  48
  49         .macro  ccm_enter_idle
  50
  51         ldr     r10, [r0, #PM_INFO_CCM_V_OFFSET]
  52         /*
  53          * if in audio_bus_freq_mode, skip to
  54          * audio_mode low power setting.
  55          */
  56         cmp     r1, #0x1
  57         beq     audio_mode
  58         /*
  59          * Now set DDR rate to 1MHz.
  60          * DDR is from bypassed PLL2 on periph2_clk2 path.
  61          * Set the periph2_clk2_podf to divide by 8.
  62          */
  63         ldr     r6, [r10, #0x14]
  64         orr     r6, r6, #0x07
  65         str     r6, [r10, #0x14]
  66
  67         /* Now set MMDC PODF to divide by 3. */
  68         ldr     r6, [r10, #0x14]
  69         bic     r6, r6, #0x38
  70         orr     r6, r6, #0x10
  71         str     r6, [r10, #0x14]
  72
  73         ccm_do_wait
  74
  75         /* Set the AHB to 3MHz. AXI to 3MHz. */
  76         ldr     r6, [r10, #0x14]
  77         /*r12 stores the origin AHB podf value */
  78         mov     r12, r6
  79         orr     r6, r6, #0x1c00
  80         orr     r6, r6, #0x70000
  81         str     r6, [r10, #0x14]
  82
  83         ccm_do_wait
  84
  85         /* Now set ARM to 24MHz.
  86          * Move ARM to be sourced from step_clk
  87          * after setting step_clk to 24MHz.
  88          */
  89         ldr     r6, [r10, #0x0c]
  90         bic     r6, r6, #0x100
  91         str     r6, [r10, #0xc]
  92         /*Now pll1_sw_clk to step_clk */
  93         ldr     r6, [r10, #0x0c]
  94         orr     r6, r6, #0x4
  95         str     r6, [r10, #0x0c]
  96
  97         /* Bypass PLL1 and power it down */
  98         ldr     r10, [r0, #PM_INFO_ANATOP_V_OFFSET]
  99         ldr     r6, =(1 << 16)
 100         orr     r6, r6, #0x1000
 101         str     r6, [r10, #0x04]
 102
 103         /*
 104          * Set the ARM PODF to divide by 8.
 105          * IPG is at 1.5MHz here, we need ARM to
 106          * run at the 12:5 ratio (WAIT mode issue).
 107          */
 108         ldr     r10, [r0, #PM_INFO_CCM_V_OFFSET]
 109         ldr     r11, [r10, #0x10]
 110         ldr     r6, =0x07
 111         str     r6, [r10, #0x10]
 112
 113         ccm_do_wait
 114
 115         b       ccm_idle_done
 116
 117 audio_mode:
 118         /*
 119          * MMDC is sourced from pll2_200M.
 120          * Set the mmdc_podf to div by 8
 121          */
 122         ldr     r10, [r0, #PM_INFO_CCM_V_OFFSET]
 123         ldr     r6, [r10, #0x14]
 124         orr     r6, r6, #0x38
 125         str     r6, [r10, #0x14]
 126
 127         ccm_do_wait
 128         /*
 129          * Bypass PLL1. the PLL1 output is disabled,
 130          * need to enable its output.
 131          */
 132         ldr     r10, [r0, #PM_INFO_ANATOP_V_OFFSET]
 133         ldr     r6, =(1 << 16)
 134         orr     r6, r6, #0x2000
 135         str     r6, [r10, #0x04]
 136
 137         /*
 138          * ARM is sourced from pll2_pfd2_400M here.
 139          * switch ARM to bypassed PLL1
 140          */
 141         ldr     r10, [r0, #PM_INFO_CCM_V_OFFSET]
 142         ldr     r6, [r10, #0x0c]
 143         bic     r6, r6, #0x4
 144         str     r6, [r10, #0xc]
 145
 146         /*
 147          * set the arm_podf to divide by 3
 148          * as IPG is at 4MHz, we cannot run
 149          * arm clk above 9.6MHz when system
 150          * enter WAIT mode
 151          */
 152         ldr     r11, [r10, #0x10]
 153         ldr     r6, =0x2
 154         str     r6, [r10, #0x10]
 155
 156         ccm_do_wait
 157
 158 ccm_idle_done:
 159
 160         .endm
 161
 162         .macro  ccm_exit_idle
 163
 164         /*
 165          * If in audio_bus_freq_mode, skip to
 166          * audio_mode ccm restore.
 167          */
 168         cmp     r1, #0x1
 169         beq     audio_ccm_restore
 170
 171         ldr     r10, [r0, #PM_INFO_ANATOP_V_OFFSET]
 172         /* Power up PLL1 and un-bypass it. */
 173         ldr     r6, =(1 << 12)
 174         str     r6, [r10, #0x08]
 175
 176         /* Wait for PLL1 to relock */
 177         ldr     r8, =0x0
 178         pll_do_wait_lock
 179
 180         ldr     r6, =(1 << 16)
 181         str     r6, [r10, #0x08]
 182
 183         ldr     r10, [r0, #PM_INFO_CCM_V_OFFSET]
 184         /* Set PLL1_sw_clk back to PLL1 */
 185         ldr     r6, [r10, #0x0c]
 186         bic     r6, r6, #0x4
 187         str     r6, [r10, #0x0c]
 188
 189         /* Restore AHB/AXI back */
 190         str     r12, [r10, #0x14]
 191
 192         ccm_do_wait
 193
 194         /* restore mmdc back to 24MHz*/
 195         ldr     r6, [r10, #0x14]
 196         bic     r6, r6, #0x3f
 197         str     r6, [r10, #0x14]
 198
 199         ccm_do_wait
 200         b       ccm_exit_done
 201
 202 audio_ccm_restore:
 203         /* move arm clk back to pll2_pfd2_400M */
 204         ldr     r6, [r10, #0xc]
 205         orr     r6, r6, #0x4
 206         str     r6, [r10, #0xc]
 207
 208         ldr     r10, [r0, #PM_INFO_ANATOP_V_OFFSET]
 209
 210         ldr     r6, =(1 << 16)
 211         str     r6, [r10, #0x08]
 212         ldr     r6, =(1 << 13)
 213         str     r6, [r10, #0x8]
 214
 215         /* restore mmdc podf */
 216         ldr     r10, [r0, #PM_INFO_CCM_V_OFFSET]
 217         ldr     r6, [r10, #0x14]
 218         bic     r6, r6, #0x38
 219         orr     r6, #0x8
 220         str     r6, [r10, #0x14]
 221
 222         ccm_do_wait
 223
 224 ccm_exit_done:
 225
 226         .endm
 227
 228         .macro check_pll_state
 229
 230         ldr     r10, [r0, #PM_INFO_ANATOP_V_OFFSET]
 231         /*
 232          * Check whether any PLL is enabled, as only when
 233          * there is no PLLs enabled, 2p5 can be off and
 234          * only enable the weak one. PLL1 will be powered
 235          * down late, so no need to check PLL1 state.
 236          */
 237
 238         /* sys PLL2 */
 239         ldr     r6, [r10, #0x30]
 240         ands r6, r6, #(1 << 31)
 241         bne     1f
 242
 243         /* usb PLL3 */
 244         ldr     r6, [r10, #0x10]
 245         ands    r6, r6, #(1 << 31)
 246         bne     1f
 247
 248         /* audio PLL4 */
 249         ldr     r6, [r10, #0x70]
 250         ands    r6, r6, #(1 << 31)
 251         bne     1f
 252
 253         /* video PLL5 */
 254         ldr     r6, [r10, #0xa0]
 255         ands    r6, r6, #(1 << 31)
 256         bne     1f
 257
 258         /* enet PLL6 */
 259         ldr     r6, [r10, #0xe0]
 260         ands    r6, r6, #(1 << 31)
 261         bne     1f
 262
 263         /* usb host PLL7 */
 264         ldr     r6, [r10, #0x20]
 265         ands    r6, r6, #(1 << 31)
 266         bne     1f
 267
 268         ldr     r4, =0x1
 269         b       check_done
 270 1:
 271         ldr     r4, =0x0
 272
 273 check_done:
 274         .endm
 275
 276         .macro  anatop_enter_idle
 277
 278         ldr     r10, [r0, #PM_INFO_ANATOP_V_OFFSET]
 279         cmp     r4, #0x0
 280         beq     anatop_enter_done
 281
 282         /* Disable 1p1 brown out. */
 283         ldr     r10, [r0, #PM_INFO_ANATOP_V_OFFSET]
 284         ldr     r6, [r10, #0x110]
 285         bic     r6, r6, #0x2
 286         str     r6, [r10, #0x110]
 287         /*
 288          * Set the OSC bias current to -37.5%
 289          * to drop the power on VDDHIGH.
 290          */
 291         ldr     r6, [r10, #0x150]
 292         orr     r6, r6, #0xc000
 293         str     r6, [r10, #0x150]
 294
 295         /*
 296          * if the usb VBUS wakeup is enabled, skip
 297          * disable main 2p5.
 298          */
 299         cmp     r2, #0x1
 300         beq     anatop_enter_done
 301
 302         /* Enable the week 2p5 */
 303         ldr     r6, [r10, #0x130]
 304         orr     r6, r6, #0x40000
 305         str     r6, [r10, #0x130]
 306
 307         /* Disable main 2p5. */
 308         ldr     r6, [r10, #0x130]
 309         bic     r6, r6, #0x1
 310         str     r6, [r10, #0x130]
 311
 312         /*
 313          * Cannot diable regular bandgap
 314          * in LDO-enable mode. The bandgap
 315          * is required for ARM-LDO to regulate
 316          * the voltage.
 317          */
 318         ldr     r6, [r10, #0x140]
 319         and     r6, r6, #0x1f
 320         cmp     r6, #0x1f
 321         bne     anatop_enter_done
 322
 323         /* Enable low power bandgap */
 324         ldr     r6, [r10, #0x260]
 325         orr     r6, r6, #0x20
 326         str     r6, [r10, #0x260]
 327
 328         /*
 329          * Turn off the bias current
 330          * from the regular bandgap.
 331          */
 332         ldr     r6, [r10, #0x260]
 333         orr     r6, r6, #0x80
 334         str     r6, [r10, #0x260]
 335
 336         /*
 337          * Clear the REFTTOP+SELFBIASOFF,
 338          * self_bais circuit of the band gap.
 339          * Per RM, should be cleared when
 340          * band gap is powered down.
 341          */
 342         ldr     r6, [r10, #0x150]
 343         bic     r6, r6, #0x8
 344         str     r6, [r10, #0x150]
 345
 346         /* Power down the regular bandgap */
 347         ldr     r6, [r10, #0x150]
 348         orr     r6, r6, #0x1
 349         str     r6, [r10, #0x150]
 350 anatop_enter_done:
 351
 352         .endm
 353
 354         .macro  anatop_exit_idle
 355
 356         ldr     r10, [r0, #PM_INFO_ANATOP_V_OFFSET]
 357         cmp     r4, #0x0
 358         beq     skip_anatop_restore
 359
 360         cmp     r2, #0x1
 361         beq     ldo2p5_not_disabled
 362         /*
 363          * Regular bandgap will not be disabled
 364          * in LDO-enabled mode as it is required
 365          * for ARM-LDO to reguulate the voltage.
 366          */
 367         ldr     r6, [r10, #0x140]
 368         and     r6, r6, #0x1f
 369         cmp     r6, #0x1f
 370         bne     skip_bandgap_restore
 371
 372         /* Power up the regular bandgap */
 373         ldr     r6, [r10, #0x150]
 374         bic     r6, r6, #0x1
 375         str     r6, [r10, #0x150]
 376
 377         /* wait for bandgap stable */
 378 3:
 379         ldr     r6, [r10, #0x150]
 380         and     r6, r6, #0x80
 381         cmp     r6, #0x80
 382         bne     3b
 383
 384         /* now disable bandgap self-bias circuit */
 385         ldr     r6, [r10, #0x150]
 386         orr     r6, r6, #0x8
 387         str     r6, [r10, #0x150]
 388
 389         /* Turn on the bias current
 390          * from the regular bandgap.
 391          */
 392         ldr     r6, [r10, #0x260]
 393         bic     r6, r6, #0x80
 394         str     r6, [r10, #0x260]
 395
 396         /* Disable the low power bandgap */
 397         ldr     r6, [r10, #0x260]
 398         bic     r6, r6, #0x20
 399         str     r6, [r10, #0x260]
 400
 401 skip_bandgap_restore:
 402         /* Enable main 2p5. */
 403         ldr     r6, [r10, #0x130]
 404         orr     r6, r6, #0x1
 405         str     r6, [r10, #0x130]
 406
 407         /* Ensure the 2p5 is up */
 408 5:
 409         ldr     r6, [r10, #0x130]
 410         and     r6, r6, #0x20000
 411         cmp     r6, #0x20000
 412         bne     5b
 413
 414         /* Disable the weak 2p5 */
 415         ldr     r6, [r10, #0x130]
 416         bic     r6, r6, #0x40000
 417         str     r6, [r10, #0x130]
 418
 419 ldo2p5_not_disabled:
 420         /*
 421          * Set the OSC bias current to max
 422          * value for normal operation.
 423          */
 424         ldr     r6, [r10, #0x150]
 425         bic     r6, r6, #0xc000
 426         str     r6, [r10, #0x150]
 427
 428         /* Enable 1p1 brown out, */
 429         ldr     r6, [r10, #0x110]
 430         orr     r6, r6, #0x2
 431         str     r6, [r10, #0x110]
 432
 433 skip_anatop_restore:
 434
 435         .endm
 436
 437         .macro  disable_l1_dcache
 438
 439         /* disable d-cache */
 440         mrc     p15, 0, r7, c1, c0, 0
 441         bic     r7, r7, #(1 << 2)
 442         mcr     p15, 0, r7, c1, c0, 0
 443
 444         dsb
 445         isb
 446
 447         .endm
 448
 449         .macro  mmdc_enter_dvfs_mode
 450
 451         /* disable automatic power saving. */
 452         ldr     r7, [r10, #MX6Q_MMDC_MAPSR]
 453         orr     r7, r7, #0x1
 454         str     r7, [r10, #MX6Q_MMDC_MAPSR]
 455
 456         /* disable power down timer */
 457         ldr     r7, [r10, #0x04]
 458         bic     r7, r7, #0xff00
 459         str     r7, [r10, #0x04]
 460
 461         /* Make the DDR explicitly enter self-refresh. */
 462         ldr     r7, [r10, #MX6Q_MMDC_MAPSR]
 463         orr     r7, r7, #(1 << 21)
 464         str     r7, [r10, #MX6Q_MMDC_MAPSR]
 465
 466 poll_dvfs_set:
 467         ldr     r7, [r10, #MX6Q_MMDC_MAPSR]
 468         ands    r7, r7, #(1 << 25)
 469         beq     poll_dvfs_set
 470
 471         /* set SBS step-by step mode */
 472         ldr     r7, [r10, #0x410]
 473         orr     r7, r7, #0x100
 474         str     r7, [r10, #0x410]
 475
 476         .endm
 477
 478         .macro  resume_mmdc
 479         /* restore MMDC IO */
 480         ldr     r10, [r0, #PM_INFO_IOMUXC_V_OFFSET]
 481
 482         ldr     r6, [r0, #PM_INFO_IO_NUM_OFFSET]
 483         ldr     r7, =PM_INFO_IO_VAL_OFFSET
 484         add     r7, r7, r0
 485 6:
 486         ldr     r8, [r7], #0x4
 487         ldr     r9, [r7], #0x4
 488         str     r9, [r10, r8]
 489         subs    r6, r6, #0x1
 490         bne     6b
 491
 492         /*
 493          * Need to reset the FIFO to avoid MMDC lockup
 494          * caused because of floating/changing the
 495          * configuration of many DDR IO pads.
 496          */
 497         ldr     r10, [r0, #PM_INFO_MMDC_V_OFFSET]
 498         /* reset read FIFO, RST_RD_FIFO */
 499         ldr     r7, =MX6Q_MMDC_MPDGCTRL0
 500         ldr     r6, [r10, r7]
 501         orr     r6, r6, #(1 << 31)
 502         str     r6, [r10, r7]
 503 7:
 504         ldr     r6, [r10, r7]
 505         ands    r6, r6, #(1 << 31)
 506         bne     7b
 507
 508         /* reset FIFO a second time */
 509         ldr     r7, =MX6Q_MMDC_MPDGCTRL0
 510         ldr     r6, [r10, r7]
 511         orr     r6, r6, #(1 << 31)
 512         str     r6, [r10, r7]
 513 8:
 514         ldr     r6, [r10, r7]
 515         ands    r6, r6, #(1 <<31)
 516         bne     8b
 517
 518         ldr     r10, [r0, #PM_INFO_MMDC_V_OFFSET]
 519         /* Let DDR out of self-refresh */
 520         ldr     r7, [r10, #MX6Q_MMDC_MAPSR]
 521         bic     r7, r7, #(1 << 21)
 522         str     r7, [r10, #MX6Q_MMDC_MAPSR]
 523 9:
 524         ldr     r7, [r10, #MX6Q_MMDC_MAPSR]
 525         ands    r7, r7, #(1 << 25)
 526         bne     9b
 527
 528         /* enable power down timer */
 529         ldr     r7, [r10, #0x04]
 530         orr     r7, r7, #0x5500
 531         str     r7, [r10, #0x04]
 532
 533         /* enable DDR auto power saving */
 534         ldr     r7, [r10, #MX6Q_MMDC_MAPSR]
 535         bic     r7, r7, #0x1
 536         str     r7, [r10, #MX6Q_MMDC_MAPSR]
 537
 538         /* Clear SBS - unblock DDR accesses */
 539         ldr     r7, [r10, #0x410]
 540         bic     r7, r7, #0x100
 541         str     r7, [r10, #0x410]
 542
 543         .endm
 544
 545         .macro  tlb_set_to_ocram
 546
 547         /* save ttbr */
 548         mrc     p15, 0, r7, c2, c0, 1
 549         str     r7, [r0, #PM_INFO_TTBR_OFFSET]
 550
 551         /*
 552          * To ensure no page table walks occur in DDR, we
 553          * have a another page table stored in IRAM that only
 554          * contains entries pointing to IRAM, AIPS1 and AIPS2.
 555          * we need to set the TTBR1 to the new IRAM TLB.
 556          * Do the following steps:
 557          * 1. Flush the Branch Target Address Cache (BTAC)
 558          * 2. Set TTBR1 to point to the IRAM page table.
 559          * 3. Disable page table walks in TTBR0 (PD0 = 1)
 560          * 4. Set TTBR0.N=1, implying 0-2G is transslated by TTBR0
 561          *    and 2-4G is translated by TTBR1.
 562          */
 563
 564         ldr     r6, =iram_tlb_phys_addr
 565         ldr     r7, [r6]
 566
 567         /* Disable Branch Prediction, Z bit in SCTLR */
 568         mrc     p15, 0, r6, c1, c0, 0
 569         bic     r6, r6, #0x800
 570         mcr     p15, 0, r6, c1, c0, 0
 571
 572         /* Flush the BTAC. */
 573         ldr     r6, =0x0
 574         mcr     p15, 0, r6, c7, c1, 6
 575
 576         dsb
 577         isb
 578
 579         /* store the IRAM table in TTBR1 */
 580         mcr     p15, 0, r7, c2, c0, 1
 581         /* Read TTBCR and set PD0=1, N=1 */
 582         mrc     p15, 0, r6, c2, c0, 2
 583         orr     r6, r6, #0x11
 584         mcr     p15, 0, r6, c2, c0, 2
 585
 586         dsb
 587         isb
 588
 589         /* Flush the TLB */
 590         ldr     r6, =0x0
 591         mcr     p15, 0, r6, c8, c3, 0
 592
 593         .endm
 594
 595         .macro  tlb_back_to_ddr
 596
 597         /* Restore the TTBCR */
 598         dsb
 599         isb
 600
 601         /* Read TTBCR and set PD0=0, N=0 */
 602         mrc     p15, 0, r6, c2, c0, 2
 603         bic     r6, r6, #0x11
 604         mcr     p15, 0, r6, c2, c0, 2
 605         /* Flush the TLB */
 606         ldr     r6, =0x0
 607         mcr     p15, 0, r6, c8, c3, 0
 608
 609         dsb
 610         isb
 611
 612         /* Enable Branch Prediction, Z bit in SCTLR. */
 613         mrc     p15, 0, r6, c1, c0, 0
 614         orr     r6, r6, #0x800
 615         mcr     p15, 0 ,r6, c1, c0, 0
 616         /* Flush the Branch Target Address Cache (BTAC) */
 617         ldr     r6, =0x0
 618         mcr     p15, 0, r6, c7, c1, 6
 619         /* Restore ttbr */
 620         ldr     r7, [r0, #PM_INFO_TTBR_OFFSET]
 621         mcr     p15, 0, r7, c2, c0, 1
 622
 623         .endm
 624
 625 .extern iram_tlb_phys_addr
 626
 627 /*
 628  * imx6sl_low_power_wfi code
 629  * r0: wfi code base address
 630  * r1: audio_bus_freq mode stat
 631  * r2: vbus_ldo status
 632  * r4: used for store the PLLs state
 633  * r11: used for saving the ARM_PODF origin value
 634  * r12: used for saving AHB_PODF origin value
 635  */
 636         .align 3
 637 ENTRY(imx6sl_low_power_wfi)
 638
 639 mx6sl_lpm_wfi_start:
 640
 641         push {r4-r12}
 642
 643         tlb_set_to_ocram
 644         disable_l1_dcache
 645
 646 #ifdef CONFIG_CACHE_L2X0
 647         /* sync L2 */
 648         ldr     r10, [r0, #PM_INFO_L2_V_OFFSET]
 649         /* Wait for background operations to complete. */
 650 wait_for_l2_idle:
 651         ldr     r6, [r10, #0x730]
 652         cmp     r6, #0x0
 653         bne     wait_for_l2_idle
 654
 655         mov     r6, #0x0
 656         str     r6, [r10, #0x730]
 657         /* disable L2 */
 658         str     r6, [r10, #0x100]
 659
 660         dsb
 661         isb
 662 #endif
 663
 664         /* make sure MMDC in self-refresh */
 665         ldr     r10, [r0, #PM_INFO_MMDC_V_OFFSET]
 666         mmdc_enter_dvfs_mode
 667         /* save DDR IO settings and set to LPM mode*/
 668         ldr     r10, [r0, #PM_INFO_IOMUXC_V_OFFSET]
 669         ldr     r6, =0x0
 670         ldr     r7, [r0, #PM_INFO_IO_NUM_OFFSET]
 671         ldr     r8, =PM_INFO_IO_VAL_OFFSET
 672         add     r8, r8, r0
 673
 674         /* imx6sl's last 3 IOs need special setting */
 675         sub     r7, r7, #0x3
 676 save_and_set_mmdc_io_lpm:
 677         ldr     r9, [r8], #0x4
 678         ldr     r5, [r10, r9]
 679         str     r6, [r10, r9]
 680         str     r5, [r8], #0x4
 681         subs    r7, r7, #0x1
 682         bne     save_and_set_mmdc_io_lpm
 683         ldr     r6, =0x1000
 684         ldr     r9, [r8], #0x4
 685         ldr     r5, [r10, r9]
 686         str     r5, [r8], #0x4
 687         str     r6, [r10, r9]
 688         ldr     r9, [r8], #0x4
 689         ldr     r5, [r10, r9]
 690         str     r6, [r10, r9]
 691         str     r5, [r8], #0x4
 692         ldr     r6, =0x80000
 693         ldr     r9, [r8], #0x4
 694         ldr     r5, [r10, r9]
 695         str     r6, [r10, r9]
 696         str     r5, [r8], #0x4
 697
 698
 699         /* check the PLLs lock state */
 700         check_pll_state
 701
 702         ccm_enter_idle
 703         /* if in audio low power mode, no
 704          * need to do anatop setting.
 705          */
 706         cmp     r1, #0x1
 707         beq     do_wfi
 708         anatop_enter_idle
 709 do_wfi:
 710         wfi
 711         /*
 712          * Add these nops so that the
 713          * prefetcher will not try to get
 714          * any instrutions from DDR.
 715          * The prefetch depth is about 23
 716          * on A9, so adding 25 nops.
 717          */
 718         nop
 719         nop
 720         nop
 721         nop
 722         nop
 723
 724         nop
 725         nop
 726         nop
 727         nop
 728         nop
 729
 730         nop
 731         nop
 732         nop
 733         nop
 734         nop
 735
 736         nop
 737         nop
 738         nop
 739         nop
 740         nop
 741
 742         nop
 743         nop
 744         nop
 745         nop
 746         nop
 747
 748         /*
 749          * restore the ARM PODF first to speed
 750          * up the restore procedure
 751          */
 752         ldr     r10, [r0, #PM_INFO_CCM_V_OFFSET]
 753         /* Restore arm_clk_podf */
 754         str     r11, [r10, #0x10]
 755         ccm_do_wait
 756
 757         /*
 758          * if in audio low power mode, skip
 759          * restore the anatop setting.
 760          */
 761         cmp     r1, #0x1
 762         beq     skip_analog_restore
 763         anatop_exit_idle
 764
 765 skip_analog_restore:
 766         ccm_exit_idle
 767         resume_mmdc
 768
 769         /* enable d-cache */
 770         mrc     p15, 0, r7, c1, c0, 0
 771         orr     r7, r7, #(1 << 2)
 772         mcr     p15, 0, r7, c1, c0, 0
 773
 774 #ifdef CONFIG_CACHE_L2X0
 775         ldr     r10, [r0, #PM_INFO_L2_V_OFFSET]
 776         mov     r7, #0x1
 777         /* enable L2 */
 778         str     r7, [r10, #0x100]
 779 #endif
 780         tlb_back_to_ddr
 781
 782         /* Restore register */
 783         pop     {r4 - r12}
 784         mov     pc, lr
 785
 786         /*
 787          * Add ltorg here to ensure that all
 788          * literals are stored here and are
 789          * within the text space.
 790          */
 791         .ltorg
 792 mx6sl_lpm_wfi_end: