;------------------------------------------------------------------------------
;
;   Copyright (C) 2004-2007, Freescale Semiconductor, Inc. All Rights Reserved.
;   THIS SOURCE CODE, AND ITS USE AND DISTRIBUTION, IS SUBJECT TO THE TERMS
;   AND CONDITIONS OF THE APPLICABLE LICENSE AGREEMENT
;
;------------------------------------------------------------------------------
;
;  Module: wfi.s
;
;  This module implements the OAL assembly-level support for entering the ARM
;  wait-for-interrupt low-power mode.
;
;------------------------------------------------------------------------------
    INCLUDE kxarm.h
    INCLUDE armmacros.s

;
; ARM constants
;
ARM_CTRL_ICACHE             EQU     (1 << 12)
ARM_CTRL_DCACHE             EQU     (1 << 2)

    TEXTAREA

;------------------------------------------------------------------------------
;
;  Function: OALCPUEnterWFI
;
;  This function provides the instruction sequence for requesting the ARM CPU 
;  to enter the WFI (wait-for-interrupt).  This routine will be called by
;  the OALCPUIdle and OEMPowerOff.  
;
;  Parameters:
;      None.
;
;  Returns:
;      None.
;
;------------------------------------------------------------------------------
    LEAF_ENTRY OALCPUEnterWFI

    ; Apply software workaround for Errata TLSbo65953 (L1 I-cache 
    ; corruption upon exit from WFI).  The only known workaround compatible
    ; with DVFS involves disabling and flushing all internal memories (I-cache,
    ; D-cache, and branch target cache) of the ARM1136 platform.  

    mrc     p15, 0, r0, c1, c0, 0               ; Read system control reg
    bic     r0, r0, #ARM_CTRL_ICACHE            ; Disable I-cache
    bic     r0, r0, #ARM_CTRL_DCACHE            ; Disable D-cache
    mcr     p15, 0, r0, c1, c0, 0               ; Update system control reg

    ; The following block of code is the recommended workaround for
    ; ARM1136 erratum 411920.  This code sequence is also a documented
    ; workaround for other ARM1136 instruction cache errata (328429, 
    ; 371025, 405875).  Note that we skip disabling interrupts during the
    ; 411920 workaround since OALCPUEnterWFI is always called with
    ; interrupts disabled
    
    mov     r0, #0                              ; Invalidate entire I-cache (also
    mcr     p15, 0, r0, c7, c5, 0               ; flushes branch target cache)
    mcr     p15, 0, r0, c7, c5, 0
    mcr     p15, 0, r0, c7, c5, 0
    mcr     p15, 0, r0, c7, c5, 0
    nop
    nop
    nop
    nop
    nop
    nop
    nop
    nop
    nop
    nop
    nop
    nop
    
    mov     r0, #0                              ; Clean and invalidate entire
    mcr     p15, 0, r0, c7, c14, 0              ; D-cache

    mov     r0,#0
    mcr     p15, 0, r0, c7, c0, 4               ; Enter WFI

    mrc     p15, 0, r0, c1, c0, 0               ; Read system control reg
    orr     r0, r0, #ARM_CTRL_ICACHE            ; Enable I-cache
    orr     r0, r0, #ARM_CTRL_DCACHE            ; Enable D-cache
    mcr     p15, 0, r0, c1, c0, 0               ; Update system control reg

    RETURN

    END