;------------------------------------------------------------------------------
;
;  Copyright (c) Microsoft Corporation.  All rights reserved.
;
;
;  Use of this source code is subject to the terms of the Microsoft end-user
;  license agreement (EULA) under which you licensed this SOFTWARE PRODUCT.
;  If you did not accept the terms of the EULA, you are not authorized to use
;  this source code. For a copy of the EULA, please see the LICENSE.RTF on your
;  install media.
;
;------------------------------------------------------------------------------
;
;   File:  power.s
;
;   CPU Power down mode for S5PV210.
;
;------------------------------------------------------------------------------

        INCLUDE    kxarm.h
        INCLUDE    register_map.inc
        INCLUDE    image_cfg.inc
        
        IMPORT      OALClearITLB
        IMPORT      OALClearDTLB
        IMPORT      OALFlushICache
        IMPORT      OALFlushDCache
        IMPORT      OALFlushL1DCache
        IMPORT      OALPAtoVA
        
        TEXTAREA
;------------------------------------------------------------------------------
;
;    Macro for LED on SMDK Board (GPH2[7:4])
;
;    VLED_ON for virtual address domain
;
;    Port initialized for using LED por with GPH2[7:4] is executed in boot time. 
;------------------------------------------------------------------------------

    	MACRO

        vLED_ON    $data
        ; 0x1 [LED 6] / 0x2 [LED 7] / 0x4 [LED 8] / 0x8 [LED 9]
        ;
       ; ldr        r10, =vGPH2DAT
       ; ldr        r11, [r10]
       ; bic        r11, r11, #0xf0
       ; ldr        r12, =$data
       ; mov        r12, r12, lsl #4
       ; orr        r11, r11, r12
       ; str        r11, [r10]

  
        MEND
    	MACRO

        LED_ON    $data
        ; 0x1 [LED 6] / 0x2 [LED 7] / 0x4 [LED 8] / 0x8 [LED 9]
        ;
       ; ldr        r10, =GPH2DAT
       ; ldr        r11, [r10]
       ; bic        r11, r11, #0xf0
       ; ldr        r12, =$data
       ; mov        r12, r12, lsl #4
       ; orr        r11, r11, r12
       ; str        r11, [r10]

  
        MEND        
;------------------------------------------------------------------------------
;
;    CPUWakeupFromDeepIdle Function
;
;    S5PV210 Wakeup from Deep Idle Mode
;
;------------------------------------------------------------------------------
  

        LEAF_ENTRY CPUWakeupFromDeepIdle
        
        ;-------------------------------
        ; Calculate CheckSum of Sleep Data
        
        ldr     r3, =IMAGE_DEEPIDLE_DATA_PA_START          ; Base of Sleep Data Area
        ldr     r2, =0x0                                ; CheckSum is in r2
        ldr     r0, =(SLEEPDATA_SIZE-1)                 ; Size of Sleep Data Area (in words)

DIdle_ReCheckSum_Loop

        ldr     r1, [r3], #4
        and     r1, r1, #0x1
        mov     r1, r1, LSL #31
        orr     r1, r1, r1, LSR #1
        add     r2, r2, r1                              ; CheckSum is in r2
        subs    r0, r0, #1
        bne     DIdle_ReCheckSum_Loop

        ldr     r0, =INFORM1                            ; Still MMU is OFF.
        ldr     r1, [r0]
        cmp     r1, r2                                  ; Compare CheckSum Recalculated and Value in DRAM
        bne     DIdle_CheckSum_Corrupted

DIdle_CheckSum_Granted
        ;-------------------------------
        ; Restore CP15 Register
        
        ldr     r10, =IMAGE_DEEPIDLE_DATA_PA_START         ; Base of Sleep Data Area
        ldr     r6,  [r10, #DeepIdleState_MMUDOMAIN]       ; Domain Access Control Register
        ldr     r5,  [r10, #DeepIdleState_MMUTTBCTL]       ; TTB Control Register
        ldr     r4,  [r10, #DeepIdleState_MMUTTB1]         ; TTB Register1
        ldr     r3,  [r10, #DeepIdleState_MMUTTB0]         ; TTB Register0
        ldr     r2,  [r10, #DeepIdleState_SYSCTL]          ; System Control Register
        nop
        nop
        nop
        nop
        nop

        ;-------------------------------
        ; Set MMU Table

        ; ------------------------------------------------------------------------------------------------------------------
        ; There are some CPUs having pipeline issues that requires identity mapping before turning on MMU - (ex. Cortex-A8)
        ; We'll create an identity mapping for the address which we'll jump to when turning on MMU on.
        ; And then we'll remove the mapping after we jump to the virtual address.
        ; ------------------------------------------------------------------------------------------------------------------
        ldr     r12, =0xFFF00000                        ; (r12) = mask for section bits
        and     r9, pc, r12                             ; physical address of where we are 
                                                        ; NOTE: we assume that the startup function never spam across 1M boundary.
        orr     r0, r9, #0x2
        orr     r0, r0, #0x400                          ; (r0) = PTE for 1M for current physical address, C=B=0, kernel r/w
        add     r7, r3, r9, LSR #18                     ; (r7) = 1st level PT entry for the identity map

        ldr     r10, [r7]                               ; (r10) = saved content of the 1st-level PT
        str     r0, [r7]                                ; create the identity map

        ;-------------------------------
        ; Clear STANDBYWFI
        
        ldr     r8, =PWR_CFG
        ldr     r9, [r8]
        bic     r9, r9, #(0x3<<8)
        str     r9, [r8]
       
        mcr     p15, 0, r6, c3, c0, 0                   ; Restore Domain Access Control Register
        mcr     p15, 0, r5, c2, c0, 2                   ; Restore TTB Control Register
        mcr     p15, 0, r4, c2, c0, 1                   ; Restore TTB Register1
        mcr     p15, 0, r3, c2, c0, 0                   ; Restore TTB Register0
        mov     r0, #0x0
        mcr     p15, 0, r0, c8, c7, 0                   ; Invalidate I & D TLB

        ldr     r0, =VirtualStart
        cmp     r0, #0                                  ; make sure no stall on "mov pc,r0" below
        mcr     p15, 0, r2, c1, c0, 0                   ; Restore System Control Register (MMU Control)
        mov     pc, r0                                  ; & jump to new virtual address

        nop
        nop
        nop
        nop
        nop

        ;vLED_ON    0x2

        ;-------------------------------
        ; Return to WakeUp_Address

VirtualStart

        ; temporarily set the stack pointer to use OAL functions
        ldr     r3, =IMAGE_DEEPIDLE_DATA_PA_START          ; Sleep Data Area Base Address
        ldr     sp, [r3, #DeepIdleState_SVC_SP]            ; Restore Stack Pointer to use OALPAtoVA
        ldr     r4, [r3, #DeepIdleState_WakeAddr]          ; (r4) = Return Address (VA)

        ; VA of the translation table for identity map
        mov     r0, r7                                  ; (r0) = Physical Address
        mov     r1, #0                                  ; uncached
        bl      OALPAtoVA                               ; Translate PA to VA

        ; restore the identity map
        str     r10, [r0]                               ; (r10) = saved content of the 1st-level PT
                                                        ; (r0) = Virtual address of 1st-level PT where the saved orignal mapping should be restored into
        ; Invalidate I & D TLB
        mov     r0, #0x0
        mcr     p15, 0, r0, c8, c7, 0

        ; go back
        mov     pc, r4                                  ; Jump to Virtual Return Address
InLoop0        
        b       InLoop0

DIdle_CheckSum_Corrupted

        ;--------------------------------
        ; Bad News... CheckSum is Corrupted

        ldr     r0, =DRAM_BASE_PA_START                 ; DRAM Base Physical Address
        add     r0, r0, #IMAGE_NK_OFFSET                ; NK Offset in DRAM
        mov     pc, r0                                  ; Jump to StartUp address
InLoop1        
        b       InLoop1

        ENTRY_END


      [{TRUE}
      
;------------------------------------------------------------------------------
;
;    CPUDeepIdleMode Function
;
;    S5PV210 Deep Idle mode entering function
;
;------------------------------------------------------------------------------

    	LEAF_ENTRY CPUDeepIdleMode

;------------------------------------------------------------------------------
;    1. Push SVC Register into our Stack
;
;------------------------------------------------------------------------------

        stmdb    sp!, {r4-r12}
        stmdb    sp!, {lr}

;------------------------------------------------------------------------------
;    2. Save CP15 Register into DeepIdle Data Area in DRAM
;
;------------------------------------------------------------------------------

        ldr     r3, =IMAGE_DEEPIDLE_DATA_UA_START          ; Deep Idle Data Area Base Address

        ;----------------------------------------------------------------------
        ; WakeUp Routine Address

        ldr     r2, =DIdleWakeUp_Address                     ; Virtual Address of WakeUp Routine
        str     r2, [r3], #4                            ; [DeepIdleState_WakeAddr]

        ;----------------------------------------------------------------------
        ; CP15 System Control Register

        mrc     p15, 0, r2, c1, c0, 0                   ; load r2 with System Control Register
        ldr     r0, =SYSCTL_SBZ_MASK                    ; Should Be Zero Mask for System Control Register
        bic     r2, r2, r0
        ldr     r0, =SYSCTL_SBO_MASK                    ; Should Be One Mask for System Control Register
        orr     r2, r2, r0
        str     r2, [r3], #4                            ; [DeepIdleState_SYSCTL]

        ;----------------------------------------------------------------------
        ; CP15 Translation Table Base Register0

        mrc     p15, 0, r2, c2, c0, 0                   ; load r2 with TTB Register0
        ldr     r0, =MMUTTB_SBZ_MASK                    ; Should Be Zero Mask for TTB Register0
        bic     r2, r2, r0
        str     r2, [r3], #4                            ; [DeepIdleState_MMUTTB0]

        ;----------------------------------------------------------------------
        ; CP15 Translation Table Base Register1

        mrc     p15, 0, r2, c2, c0, 1                   ; load r2 with TTB Register1
        str     r2, [r3], #4                            ; [DeepIdleState_MMUTTB1]

        ;----------------------------------------------------------------------
        ; CP15 Translation Table Base Control Register

        mrc     p15, 0, r2, c2, c0, 2                   ; load r2 with TTB Control Register
        str     r2, [r3], #4                            ; [DeepIdleState_MMUTTBCTL]

        ;----------------------------------------------------------------------
        ; CP15 Domain Access Control Register

        mrc     p15, 0, r2, c3, c0, 0                   ; load r2 with Domain Access Control Register
        str     r2, [r3], #4                            ; [DeepIdleState_MMUDOMAIN]



;------------------------------------------------------------------------------
;    3. Save CPU Register into DeepIdle Data Area in DRAM
;
;------------------------------------------------------------------------------

        ;----------------------------------------------------------------------
        ; Supervisor mode CPU Register

        str     sp, [r3], #4                            ; [DeepIdleState_SVC_SP]
        mrs     r2, spsr                                ; Status Register
        str     r2, [r3], #4                            ; [DeepIdleState_SVC_SPSR]

        ;----------------------------------------------------------------------
        ; FIQ mode CPU Registers

        mov     r1, #Mode_FIQ | NOINT                   ; Enter FIQ mode, no interrupts
        msr     cpsr, r1
        mrs     r2, spsr                                ; Status Register
        stmia   r3!, {r2, r8-r12, sp, lr}               ; Store FIQ mode registers [DeepIdleState_FIQ_SPSR~DeepIdleState_FIQ_LR]

        ;----------------------------------------------------------------------
        ; Abort mode CPU Registers

        mov     r1, #Mode_ABT | NOINT                   ; Enter ABT mode, no interrupts
        msr     cpsr, r1
        mrs     r0, spsr                                ; Status Register
        stmia   r3!, {r0, sp, lr}                       ; Store ABT mode Registers [DeepIdleState_ABT_SPSR~DeepIdleState_ABT_LR]

        ;----------------------------------------------------------------------
        ; IRQ mode CPU Registers

        mov     r1, #Mode_IRQ | NOINT                   ; Enter IRQ mode, no interrupts
        msr     cpsr, r1
        mrs     r0, spsr                                ; Status Register
        stmia   r3!, {r0, sp, lr}                       ; Store the IRQ Mode Registers [DeepIdleState_IRQ_SPSR~DeepIdleState_IRQ_LR]

        ;----------------------------------------------------------------------
        ; Undefined mode CPU Registers

        mov     r1, #Mode_UND | NOINT                   ; Enter UND mode, no interrupts
        msr     cpsr, r1
        mrs     r0, spsr                                ; Status Register
        stmia   r3!, {r0, sp, lr}                       ; Store the UND mode Registers [DeepIdleState_UND_SPSR~DeepIdleState_UND_LR]

        ;----------------------------------------------------------------------
        ; System(User) mode CPU Registers

        mov     r1, #Mode_SYS | NOINT                   ; Enter SYS mode, no interrupts
        msr     cpsr, r1
        stmia   r3!, {sp, lr}                           ; Store the SYS mode Registers [DeepIdleState_SYS_SP, DeepIdleState_SYS_LR]

        ;----------------------------------------------------------------------
        ; Return to SVC mode

        mov     r1, #Mode_SVC | NOINT                   ; Back to SVC mode, no interrupts
        msr     cpsr, r1

;------------------------------------------------------------------------------
;    4. Calculate CheckSum of DeepIdle Data
;------------------------------------------------------------------------------

        ldr     r3, =IMAGE_DEEPIDLE_DATA_UA_START          ; Base of DeepIdle Data Area
        ldr     r2, =0x0
        ldr     r0, =(DEEPIDLEDATA_SIZE-1)                 ; Size of DeepIdle Data Area (in words)

DeepIdle_CheckSum_Loop

        ldr     r1, [r3], #4
        and     r1, r1, #0x1
        mov     r1, r1, LSL #31
        orr     r1, r1, r1, LSR #1
        add     r2, r2, r1
        subs    r0, r0, #1
        bne     DeepIdle_CheckSum_Loop

        ldr     r0, =vINFORM1
        str     r2, [r0]                                ; Store CheckSum in INFORM1 Register (in SysCon)


;------------------------------------------------------------------------------
;    5. Clear TLB, Flush Cache and disable Cache
;------------------------------------------------------------------------------

        bl      OALClearDTLB
        bl      OALClearITLB
        ;bl      OALFlushDCache        
        bl      OALFlushL1DCache
        bl      OALFlushICache

;        mov     r2, #0        
;        mrc     p15, 0, r2, c1, c0, 0
;        bic     r2, r2, #0x4                            ; Disable D-Cache 
;        bic     r2, r2, #0x1000                         ; Disable I-Cache 
;        mcr     p15, 0, r2, c1, c0, 0

;        mrc     p15, 0, r2, c1, c0, 1
;        bic     r2, r2, #0x2
;        mcr     p15, 0, r2, c1, c0, 1                   ; Disable L2-Cache

;------------------------------------------------------------------------------
;    6. Set Oscillation pad and Power Stable Counter
;------------------------------------------------------------------------------
        
        ;ldr     r0, =vOTHERS
        ;ldr     r1, [r0]
        ;ldr     r2, =(0x1<<23) 
        ;orr     r1, r1, r2
        ;str     r1, [r0]
       
        ;ldr     r0, =vPWR_STABLE
        ;ldr     r1, [r0]
        ;ldr     r2, =((PWR_STABLE_CNT_VAL<<4) + 0x1) ; PWR_STABLE_CNT_VAL = (85500) : 7.12ms     
        ;orr     r1, r1, r2
        ;str     r1, [r0]

;------------------------------------------------------------------------------
;    7. Set Power Mode to DeepIdle
;------------------------------------------------------------------------------

;        ldr     r0, =vSLEEP_CFG
;        ldr     r2, [r0]
;        bic     r2, r2, #0x1                           ; Disable OSC_EN (Disable X-tal Osc Pad in Deep Idle mode)
;        bic     r2, r2, #(0x1<<1)                       ; Disable OSCUSB_EN (Disable USB X-tal Osc Pad in DeepIdle mode)
;        str     r2, [r0]


        [{FALSE}
        ldr     r0, =vPWR_CFG
        ldr     r2, [r0]
        bic     r2, r2, #(0x3<<8)                       ; Clear STANDBYWFI
        orr     r2, r2, #(0x1<<8)                       ; Enter DeepIdle mode
        str     r2, [r0]

        ldr     r0, =vOTHERS
        ldr     r1, [r0]
        ldr     r2, =(0x1<<0) 			                ; disable SYSCON_INT_DISABLE
        orr     r1, r1, r2
        str     r1, [r0]
        ]

;------------------------------------------------------------------------------
;    8. Set Power Mode to DeepIdle
;------------------------------------------------------------------------------

        ;mov     r2, #0
        ;mcr     p15, 0, r2, c7, c10, 5                  ; Data Memory Barrier
        ;mcr     p15, 0, r2, c7, c10, 4                  ; Data Synchronization Barrier
        DCD     0xf57ff05f                  ; DMB (Data Memory Barrier)
        DCD     0xf57ff04f                  ; DSB (Data Synchronization Barrier)
    
        DCD     0xe320f003                              ; WFI 

	vLED_ON 0xf
        ldr     r3, =IMAGE_DEEPIDLE_DATA_UA_START      ; Sleep Data Area Base Address
        ldr     r1, [r3, #DeepIdleState_WakeAddr]    ; Return Address
        mov     pc, r1    
InLoop3        
        b InLoop3


        
;******************************************************************************
;    Now CPU is in DeepIdle Mode
;
;******************************************************************************

DIdleWakeUp_Address

;------------------------------------------------------------------------------
;    1. Restore CPU Register from DeepIdle Data Area in DRAM
;------------------------------------------------------------------------------

        ldr     r3, =IMAGE_DEEPIDLE_DATA_UA_START          ; DeepIdle Data Area Base Address

        ;----------------------------------------------------------------------
        ; FIQ mode CPU Registers

        mov     r1, #Mode_FIQ | NOINT                   ; Enter FIQ mode, no interrupts
        msr     cpsr, r1

        ldr     r0,  [r3, #DeepIdleState_FIQ_SPSR]
        msr     spsr, r0
        ldr     r8,  [r3, #DeepIdleState_FIQ_R8]
        ldr     r9,  [r3, #DeepIdleState_FIQ_R9]
        ldr     r10, [r3, #DeepIdleState_FIQ_R10]
        ldr     r11, [r3, #DeepIdleState_FIQ_R11]
        ldr     r12, [r3, #DeepIdleState_FIQ_R12]
        ldr     sp,  [r3, #DeepIdleState_FIQ_SP]
        ldr     lr,  [r3, #DeepIdleState_FIQ_LR]

        ;----------------------------------------------------------------------
        ; Abort mode CPU Registers

        mov     r1, #Mode_ABT | I_Bit                   ; Enter ABT mode, no IRQ - FIQ is available
        msr     cpsr, r1

        ldr     r0,  [r3, #DeepIdleState_ABT_SPSR]
        msr     spsr, r0
        ldr     sp,  [r3, #DeepIdleState_ABT_SP]
        ldr     lr,  [r3, #DeepIdleState_ABT_LR]

        ;----------------------------------------------------------------------
        ; IRQ mode CPU Registers

        mov     r1, #Mode_IRQ | I_Bit                   ; Enter IRQ mode, no IRQ - FIQ is available
        msr     cpsr, r1

        ldr     r0,  [r3, #DeepIdleState_IRQ_SPSR]
        msr     spsr, r0
        ldr     sp,  [r3, #DeepIdleState_IRQ_SP]
        ldr     lr,  [r3, #DeepIdleState_IRQ_LR]

        ;----------------------------------------------------------------------
        ; Undefined mode CPU Registers

        mov     r1, #Mode_UND | I_Bit                   ; Enter UND mode, no IRQ - FIQ is available
        msr     cpsr, r1

        ldr     r0,  [r3, #DeepIdleState_UND_SPSR]
        msr     spsr, r0
        ldr     sp,  [r3, #DeepIdleState_UND_SP]
        ldr     lr,  [r3, #DeepIdleState_UND_LR]

        ;----------------------------------------------------------------------
        ; System(User) mode CPU Registers

        mov     r1, #Mode_SYS | I_Bit                   ; Enter SYS mode, no IRQ - FIQ is available
        msr     cpsr, r1

        ldr     sp,  [r3, #DeepIdleState_SYS_SP]
        ldr     lr,  [r3, #DeepIdleState_SYS_LR]

        ;----------------------------------------------------------------------
        ; Supervisor mode CPU Registers

        mov     r1, #Mode_SVC | I_Bit                   ; Enter SVC mode, no IRQ - FIQ is available
        msr     cpsr, r1

        ldr     r0,  [r3, #DeepIdleState_SVC_SPSR]
        msr     spsr, r0
        ldr     sp,  [r3, #DeepIdleState_SVC_SP]

;------------------------------------------------------------------------------
;    2. Pop SVC Register from our Stack
;------------------------------------------------------------------------------

        ldr     lr,  [sp], #4
        ldmia   sp!, {r4-r12}

;------------------------------------------------------------------------------
;    3. Return to Caller
;------------------------------------------------------------------------------

        mov     pc, lr                                  ; and now back to our sponsors

    ]

 ;       ENTRY_END

        END

