startup.s

6410BSP3

        bic        r2, r2, r0
        str        r2, [r3], #4                ; [SleepState_MMUTTB0]

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

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

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

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

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

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

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

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

        str        sp, [r3], #4                ; [SleepState_SVC_SP]

        mrs        r2, spsr                    ; Status Register
        str        r2, [r3], #4                ; [SleepState_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 [SleepState_FIQ_SPSR~SleepState_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 [SleepState_ABT_SPSR~SleepState_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 [SleepState_IRQ_SPSR~SleepState_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 [SleepState_UND_SPSR~SleepState_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 [SleepState_SYS_SP, SleepState_SYS_LR]

        ;----------------------------------------------------
        ;Add following : SISO added
        ; 3-1. Save VFP Register into Sleep Data Area in DRAM
        ;----------------------------------------------------

        ;--------------------------------------
        ;    Floating Point Status and Control Register using FMRX
        ;    FMRX{cond} Rd, VFPsysreg     VFPsysreg -> Rd
        ; FMXR{cond} VFPsysreg, Rd         Rd -> VFPsysreg
        ;      FPSCR
        fmrx    r2, fpscr
        str    r2, [r3], #4     ;    [SleepState_VFP_FPSCR]

        ;------------------------------------------
        ;    Floating Point Exception Register
        fmrx    r2, fpexc
        str    r2, [r3], #4       ;    [SleepState_VFP_FPEXC]

        ;-----------------------------------------
        ;VFP Register File (using FLDMX, FSTMX)
        ; FLDM<addressmode><precision>{cond} Rn,{!} VFPregisters
        ;FSTM<addressmode><precision>{cond} Rn,{!} VFPregisters
        ; <addressmode>
        ;IA : Incremental address After each transfer
        ;DB : Decremental address Before each transfer
        ;EA : Empty Ascending stack operation, this is the same as DB for loads, and the same as IA for saves.
        ;FD : Full Descending stack operation, this is the same as IA for loads, and the same as DB for saves.
        ; <precision>
        ;      S : for single-precision
        ;        D : for double-precision
        ;        X : for unspecified precision
        ;    example::
        ;        FLDMIAS    r2, {s1-s5}
        ;        FSTMFDD    r13!, {d3-d6}
        ;        FSTMFDX    r13!, {d0-d3}
        ;        FLDMFDX    r13!, {d0-d3}
        fstmiax    r3!,    {d0-d15}

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

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

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

        ldr        r3, =IMAGE_SLEEP_DATA_UA_START    ; Base of Sleep Data Area
        ldr        r2, =0x0
        ldr        r0, =(SLEEPDATA_SIZE-1)            ; Size of Sleep Data Area (in words)

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        CheckSum_Loop

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

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

        bl        OALClearDTLB
        bl        OALClearITLB
        bl        OALFlushDCache
        bl        OALFlushICache

;-----------------------------------------------------
;    6. Set Oscillation pad and Power Stable Counter
;-----------------------------------------------------

        ldr     r0, =vOSC_STABLE
        ldr     r1, =0x1
        str     r1, [r0]

        ldr     r0, =vPWR_STABLE
        ldr     r1, =0x1
        str     r1, [r0]

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

        VLED_ON 0x6

        ldr        r0, =vPWR_CFG
        ldr        r2, [r0]
        bic        r2, r2, #0x60            ; Clear STANDBYWFI
        orr        r2, r2, #0x60            ; Enter SLEEP mode
        str        r2, [r0]

        ldr        r0, =vSLEEP_CFG
        ldr        r2, [r0]
        bic        r2, r2, #0x61            ; Disable OSC_EN (Disable X-tal Osc Pad in Sleep mode)
        str        r2, [r0]

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

        bl        System_WaitForInterrupt
        b        .

;------------------------------------------------------------------------------
;    Now CPU is in Sleep Mode
;------------------------------------------------------------------------------

WakeUp_Address

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

        VLED_ON    0x3

        ldr        r3, =IMAGE_SLEEP_DATA_UA_START    ; Sleep Data Area Base Address

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

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

        ldr        r0,    [r3, #SleepState_FIQ_SPSR]
        msr        spsr, r0
        ldr        r8,    [r3, #SleepState_FIQ_R8]
        ldr        r9,    [r3, #SleepState_FIQ_R9]
        ldr        r10,    [r3, #SleepState_FIQ_R10]
        ldr        r11,    [r3, #SleepState_FIQ_R11]
        ldr        r12,    [r3, #SleepState_FIQ_R12]
        ldr        sp,    [r3, #SleepState_FIQ_SP]
        ldr        lr,    [r3, #SleepState_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, #SleepState_ABT_SPSR]
        msr        spsr, r0
        ldr        sp,    [r3, #SleepState_ABT_SP]
        ldr        lr,    [r3, #SleepState_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, #SleepState_IRQ_SPSR]
        msr        spsr, r0
        ldr        sp,    [r3, #SleepState_IRQ_SP]
        ldr        lr,    [r3, #SleepState_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, #SleepState_UND_SPSR]
        msr        spsr, r0
        ldr        sp,    [r3, #SleepState_UND_SP]
        ldr        lr,    [r3, #SleepState_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, #SleepState_SYS_SP]
        ldr        lr,    [r3, #SleepState_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, #SleepState_SVC_SPSR]
        msr        spsr, r0
        ldr        sp, [r3, #SleepState_SVC_SP]

        ;----------------------------------------
             ; Add following: SISO added
           ; 1-1 Restore VFP system control registers
           ;----------------------------------------

        ;--------------------------------------
        ;FMRX{cond} Rd, VFPsysreg     VFPsysreg -> Rd
        ; FMXR{cond} VFPsysreg, Rd         Rd -> VFPsysreg
        ;      FPSCR
        ldr        r0, [r3, #SleepState_VFP_FPSCR]
        fmxr    fpscr, r0

        ;------------------------------------------
        ;    Floating Point Exception Register
        ldr        r0, [r3, #SleepState_VFP_FPEXC]
        fmxr    fpexc, r0

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

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

;--------------------------------------
;    3. Return to Caller of OALCPUPowerOff()
;--------------------------------------

        mov     pc, lr                          ; and now back to our sponsors

        ENTRY_END


;------------------------------------------------------------------------------
;    End of OALCPUPowerOff
;------------------------------------------------------------------------------

        END