;******************************************************************************
;
; CP15.S - CP15 coprocesser configuration APIs
;
;******************************************************************************
;
; copyright (c) 2010 Texas InSTRuments Incorporated - http://www.ti.com/
;
;
;  RediSTRibution and use in source and binary forms, with or without
;  modification, are permitted provided that the following conditions
;  are met:
;
;    RediSTRibutions of source code must retain the above copyright
;    notice, this list of conditions and the following disclaimer.
;
;    RediSTRibutions in binary form must reproduce the above copyright
;    notice, this list of conditions and the following disclaimer in the
;    documentation and/or other materials provided with the
;    diSTRibution.
;
;    Neither the name of Texas InSTRuments Incorporated nor the names of
;    its contributors may be used to endorse or promote products derived
;    from this software without specific prior written permission.
;
;  THIS SOFTWARE IS PROVIDED BY THE cOPYRIGHT HOLDERS AND cONTRIBUTORS
;  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INcLUDING, BUT NOT
;  LIMITED TO, THE IMPLIED WARRANTIES OF MERcHANTABILITY AND FITNESS FOR
;  A PARTIcULAR PURPOSE ARE DIScLAIMED. IN NO EVENT SHALL THE cOPYRIGHT
;  OWNER OR cONTRIBUTORS BE LIABLE FOR ANY DIREcT, INDIREcT, INcIDENTAL,
;  SPEcIAL, EXEMPLARY, OR cONSEQUENTIAL DAMAGES (INcLUDING, BUT NOT
;  LIMITED TO, PROcUREMENT OF SUBSTITUTE GOODS OR SERVIcES; LOSS OF USE,
;  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER cAUSED AND ON ANY
;  THEORY OF LIABILITY, WHETHER IN cONTRAcT, STRIcT LIABILITY, OR TORT
;  (INcLUDING NEGLIGENcE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
;  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUcH DAMAGE.
;
;******************************************************************************

;****************************** Global Symbols*********************************

	
_cLIENTD EQU  0x55555555
_FLD_MAX_WAY EQU  0x3ff
_FLD_MAX_IDX EQU  0x7ff
;**************************** code section ************************************
    PRESERVE8
    AREA   cp15, cODE, READONLY      ; name this block of code	; Use to match original sample code.  This section is in vectors_armcc.s (entry point) then that calls crt0_armcc.s

   ENTRY
   
        EXPORT CP15IcacheDisable
        EXPORT CP15DcacheDisable
        EXPORT CP15IcacheEnable
        EXPORT CP15DcacheEnable
        EXPORT CP15IcacheFlush
        EXPORT CP15DcachecleanFlush
        EXPORT CP15Dcacheclean
        EXPORT CP15DcacheFlush
        EXPORT CP15DcachecleanBuff
        EXPORT CP15DcachecleanFlushBuff
        EXPORT CP15DcacheFlushBuff
        EXPORT CP15IcacheFlushBuff
        EXPORT CP15Ttb0Set
        EXPORT CP15TlbInvalidate
        EXPORT CP15MMUDisable
        EXPORT CP15MMUEnable
        EXPORT CP15VectorBaseAddrSet
        EXPORT CP15BranchPredictorInvalidate
        EXPORT CP15BranchPredictionEnable
        EXPORT CP15BranchPredictionDisable
        EXPORT CP15DomainAccessClientSet
        EXPORT CP15ControlFeatureDisable
        EXPORT CP15controlFeatureEnable
        EXPORT CP15TtbCtlTtb0Config
        EXPORT CP15AuxcontrolFeatureEnable
        EXPORT CP15AuxcontrolFeatureDisable
        EXPORT CP15MainIdPrimPartNumGet
	EXPORT CP15NeonEnable
	EXPORT CP15FpuEnable
	EXPORT CP15Preload
	EXPORT CP15IPreload
	
;*****************************************************************************
; This API disable the InSTRuction cache.
;*****************************************************************************
CP15IcacheDisable
    MRC     p15, #0, r0, c1, c0, #0
    BIC     r0,  r0, #0x00001000
    MCR     p15, #0, r0, c1, c0, #0
    BX      lr

;*****************************************************************************
; This API disable the Data cache.
;*****************************************************************************
CP15DcacheDisable
    MRC     p15, #0, r0, c1, c0, #0
    BIC     r0,  r0, #0x00000004
    MCR     p15, #0, r0, c1, c0, #0
    BX      lr

;*****************************************************************************
; This API enables I-cache
;*****************************************************************************
CP15IcacheEnable
    MRC     p15, #0, r0, c1, c0, #0
    ORR     r0,  r0, #0x00001000
    MCR     p15, #0, r0, c1, c0, #0 
    BX      lr

;*****************************************************************************
; This API enable the Data cache.
;*****************************************************************************
CP15DcacheEnable
    MRC     p15, #0, r3, c1, c0, #0 
    ORR     r3,  r3, #0x0000003c
    MCR     p15, #0, r3, c1, c0, #0
    BX      lr

;*****************************************************************************
; This API Invalidates the entire Data/Unified cache
;*****************************************************************************
CP15DcacheFlush
    PUSH    {r0-r11}
    DMB
    MRC     p15, #1, r0, c0, c0, #1  ; Read cLID register
    ANDS    r3, r0, #0x7000000       ; Get Level of coherency
    MOV     r3, r3, lsr #23
    BEQ     ffinished
    MOV     r10, #0
floop1
    ADD     r2, r10, r10, lsr #1
    MOV     r1, r0, lsr r2
    AND     r1, r1, #7
    CMP     r1, #2
    BLT     fskip
    MCR     p15, #2, r10, c0, c0, #0
    DSB
    MRC     p15, #1, r1, c0, c0, #0
    AND     r2, r1, #7
    ADD     r2, r2, #4
    LDR     r4,=_FLD_MAX_WAY
    ANDS    r4, r4, r1, lsr #3
    CLZ     r5, r4
    LDR     r7,=_FLD_MAX_IDX
    ANDS    r7, r7, r1, lsr #13
floop2
    MOV     r9, r4
floop3
    ORR     r11, r10, r9, lsl r5
    ORR     r11, r11, r7, lsl r2
    MCR     p15, #0, r11, c7, c6, #2
    DSB
    SUBS    r9, r9, #1
    BGE     floop3
    SUBS    r7, r7, #1
    BGE     floop2
fskip
    ADD     r10, r10, #2
    CMP     r3, r10
    BGT     floop1

ffinished
    DSB
    ISB
    POP     {r0-r11}
    BX      lr


;*****************************************************************************
; This API cleans the entire D cache to Poc
;*****************************************************************************
CP15Dcacheclean
    PUSH    {r4-r11}
    DMB
    MRC     p15, #1, r0, c0, c0, #1  ; Read cLID register
    ANDS    r3, r0, #0x7000000       ; Get Level of coherency
    MOV     r3, r3, lsr #23
    BEQ     cfinished
    MOV     r10, #0
cloop1
    ADD     r2, r10, r10, lsr #1
    MOV     r1, r0, lsr r2
    AND     r1, r1, #7
    CMP     r1, #2
    BLT     cskip
    MCR     p15, #2, r10, c0, c0, #0
    ISB
    MRC     p15, #1, r1, c0, c0, #0
    AND     r2, r1, #7
    ADD     r2, r2, #4
    LDR     r4,=_FLD_MAX_WAY
    ANDS    r4, r4, r1, lsr #3
    CLZ     r5, r4
    LDR     r7,=_FLD_MAX_IDX
    ANDS    r7, r7, r1, lsr #13
cloop2
    MOV     r9, r4
cloop3
    ORR     r11, r10, r9, lsl r5
    ORR     r11, r11, r7, lsl r2
    MCR     p15, #0, r11, c7, c10, #2
    DSB
    SUBS    r9, r9, #1
    BGE     cloop3
    SUBS    r7, r7, #1
    BGE     cloop2
cskip
    ADD     r10, r10, #2
    CMP     r3, r10
    BGT     cloop1

cfinished
    ISB
    POP     {r4-r11}
    BX      lr


;*****************************************************************************
; This API cleans and invalidates the entire D cache to Poc
;*****************************************************************************
CP15DcachecleanFlush
    PUSH    {r0-r11} 
    DMB                   
    MRC     p15, #1, r0, c0, c0, #1  ; Read cLID register
    ANDS    r3, r0, #0x7000000       ; Get Level of coherency
    MOV     r3, r3, lsr #23  
    BEQ     finished  
    MOV     r10, #0 
loop1
    ADD     r2, r10, r10, lsr #1 
    MOV     r1, r0, lsr r2
    AND     r1, r1, #7 
    CMP     r1, #2  
    BLT     skip 
    MCR     p15, #2, r10, c0, c0, #0 
    ISB 
    MRC     p15, #1, r1, c0, c0, #0 
    AND     r2, r1, #7 
    ADD     r2, r2, #4   
    LDR     r4,=_FLD_MAX_WAY 
    ANDS    r4, r4, r1, lsr #3
    CLZ     r5, r4
    LDR     r7,=_FLD_MAX_IDX
    ANDS    r7, r7, r1, lsr #13 
loop2
    MOV     r9, r4   
loop3
    ORR     r11, r10, r9, lsl r5  
    ORR     r11, r11, r7, lsl r2 
    MCR     p15, #0, r11, c7, c14, #2 
    DMB
    SUBS    r9, r9, #1 
    BGE     loop3 
    SUBS    r7, r7, #1 
    BGE     loop2 
skip
    ADD     r10, r10, #2 
    CMP     r3, r10 
    BGT     loop1 

finished
    ISB                         
    POP     {r0-r11} 
    BX      lr

;*****************************************************************************
; This API invalidates entire I cache
;*****************************************************************************
CP15IcacheFlush
    MOV     r0, #0
    MCR     p15, #0, r0, c7, c5, #0
    DSB
    BX      lr

;*****************************************************************************
; This API cleans the D-cache/Unified  lines corresponding to the buffer 
; pointer upto the specified length to Poc.
; r6 - Start Address 
; r3 - Number of bytes to be cleaned
;*****************************************************************************
CP15DcachecleanBuff
    PUSH    {r0-r12,r14}
    ADD     r14, r6, r3               ; calculate the end address
    DMB
    MRC     p15, #0, r2, c0, c0, #1   ; Read cache Type Register
    UBFX    r2, r2, #16, #4           ; Extract the DMinLine
    MOV     r3, #2
    ADD     r3, r3, r2
    MOV     r2, #1
    LSL     r2, r2, r3                ; calculate the line size
   
    SUB     r3, r2, #1                ; calculate the mask
    BIC     r6, r6, r3                ; Align to cache line boundary   
    BIC     r14, r14, r3              

cleanloop    
    MCR     p15, #0, r6 , c7, c10, #1 ; clean D/Unified to Poc by MVA
    ADDS    r6, r6, r2                ; Go to next line
    CMP     r6, r14 
    BLE     cleanloop    
 
    POP     {r0-r12,r14}
    DSB
    BX      lr

;*****************************************************************************
; This API cleans and invalidates the D-cache/Unified  lines corresponding to 
; the buffer pointer upto the specified length to Poc.
; r0 - Start Address 
; r1 - Number of bytes to be cleaned and flushed
;*****************************************************************************
CP15DcachecleanFlushBuff
    PUSH    {r14}
    ADD     r14, r0, r1               ; calculate the end address
    DMB
    MRC     p15, #0, r2, c0, c0, #1   ; Read cache Type Register
    UBFX    r2, r2, #16, #4           ; Extract the DMinLine
    MOV     r3, #2
    ADD     r3, r3, r2
    MOV     r2, #1
    LSL     r2, r2, r3                ; calculate the line size
   
    SUB     r3, r2, #1                ; calculate the mask
    BIC     r0, r0, r3                ; Align to cache line boundary   
    BIC     r14, r14, r3              

cleanflushloop    
    MCR     p15, #0, r0 , c7, c14, #1 ; clean and Flush D/U line to Poc 
    ADDS    r0, r0, r2                ; Go to next line
    CMP     r0, r14 
    BLE     cleanflushloop    
 
    POP     {r14}
    DSB
    BX      lr

;*****************************************************************************
; This API invalidates the D-cache/Unified  lines corresponding to 
; the buffer pointer upto the specified length to Poc.
; r0 - Start Address 
; r1 - Number of bytes to be flushed
;*****************************************************************************
CP15DcacheFlushBuff
    PUSH    {r14}
    ADD     r14, r0, r1               ; calculate the end address
    DMB
    MRC     p15, #0, r2, c0, c0, #1   ; Read cache Type Register
    UBFX    r2, r2, #16, #4           ; Extract the DMinLine
    MOV     r3, #2
    ADD     r3, r3, r2
    MOV     r2, #1
    LSL     r2, r2, r3                ; calculate the line size
   
    SUB     r3, r2, #1                ; calculate the mask
    TST     r3, r0 
    BIC     r0, r0, r3                ; Align to cache line boundary   
    MCRNE   p15, #0, r0, c7, c14, #1  ; clean and Flush D/U line to Poc 
    ADDNE   r0, r0, r2
    TST     r3, r14 
    BIC     r14, r14, r3              
    MCRNE   p15, #0, r14, c7, c14, #1 ; clean and Flush D/U line to Poc 
    SUBNE   r14, r14, r2
    B       dflushCMP

dflushloop
    MCR     p15, #0, r0 , c7, c6, #1  ; Flush D/U line to Poc    
    ADDS    r0, r0, r2                ; Go to next line

dflushCMP
    CMP     r0, r14
    BLE     dflushloop
    POP     {r14}
    DSB
    BX      lr

;*****************************************************************************
; This API invlidates I-cache lines from the star address till the length   
; specified to PoU.
; r0 - Start Address 
; r1 - Number of bytes to be cleaned
;*****************************************************************************
CP15IcacheFlushBuff
    PUSH    {r14}
    ADD     r14, r0, r1               ; calculate the end address
    DMB
    MRC     p15, #0, r2, c0, c0, #1   ; Read cache Type Register
    UBFX    r2, r2, #0, #4            ; Extract the DMinLine
    MOV     r3, #2
    ADD     r3, r3, r2
    MOV     r2, #1
    LSL     r2, r2, r3                ; calculate the line size
   
    SUB     r3, r2, #1                ; calculate the mask
    BIC     r0, r0, r3                ; Align to cache line boundary   
    BIC     r14, r14, r3              

iflushloop    
    MCR     p15, #0, r0, c7, c5, #1   ; Invalidate by MVA to PoU
    ADDS    r0, r0, r2                ; Go to next line
    CMP     r0, r14 
    BLE     iflushloop    
 
    POP     {r14}
    DSB
    BX      lr

;*****************************************************************************
; Sets TTB0 Register
; r0 - Translation Table Base Address
;*****************************************************************************
CP15Ttb0Set
    MCR     p15, #0, r0, c2, c0, #0
    DMB
    BX      lr

;*****************************************************************************
; This API Invalidates the TLB
;*****************************************************************************
CP15TlbInvalidate
    MCR     p15, #0, r0, c8, c7, #0    ; r0 value will be ignored
    DSB
    BX      lr

;*****************************************************************************
; This API Disables MMU. 
;*****************************************************************************
CP15MMUDisable
    MCR     p15, #0, r0, c8, c7, #0    ; Invalidate TLB  
    MRC     p15, #0, r0, c1, c0, #0    
    BIC     r0, r0, #1      
    MCR     p15, #0, r0, c1, c0, #0    ; clear MMU bit
    DSB  
    BX      lr

;*****************************************************************************
; This API Enables MMU. 
;*****************************************************************************
CP15MMUEnable
    MRC     p15, #0, r0, c1, c0, #0
    ORR     r0, r0, #0x001
    MCR     p15, #0, r0, c1, c0, #0    ; Set MMU Enable bit
    DSB
    BX      lr

;*****************************************************************************
; This API sets the interrupt vector table base address 
; r0 - Interrput Vector Base Address
;*****************************************************************************
CP15VectorBaseAddrSet
    MCR     p15, #0, r0, c12, c0, #0
    DSB
    BX      lr

;*****************************************************************************
; This API invalidates the branch predictor 
;*****************************************************************************
CP15BranchPredictorInvalidate
    MCR     p15, #0, r0, c7, c5, #6
    ISB
    MCR     p15, #0, r0, c7, c5, #7
    ISB
    BX      lr

;*****************************************************************************
; This API enables the branch predictor 
;*****************************************************************************
CP15BranchPredictionEnable
    MRC     p15, #0, r0, c1, c0, #0
    ORR     r0, r0, #0x00000800
    MCR     p15, #0, r0, c1, c0, #0
    ISB
    BX      lr

;*****************************************************************************
; This API disables the branch predictor 
;*****************************************************************************
CP15BranchPredictionDisable
    MRC     p15, #0, r0, c1, c0, #0
    BIC     r0, r0, #0x00000800
    MCR     p15, #0, r0, c1, c0, #0
    BX      lr

;*****************************************************************************
; This API sets the domain access to 'client' 
;*****************************************************************************
CP15DomainAccessClientSet
    LDR     r0,=_cLIENTD 
    MCR     p15, #0, r0, c3, c0, #0
    DSB
    BX      lr


;*****************************************************************************
; This API Disables specified features in CP15 control register
;  r0 -  features   Features to disable in coprocessor 15 control
;                       register. 
;               'features' can take any OR a combination of the
;               below  values. 
;                CP15_cONTROL_TEXREMAP - TEX remap flag 
;                CP15_cONTROL_AccESSFLAG - Access flag control 
;                CP15_cONTROL_ALIGN_cHcK - Alignment Fault checking 
;                CP15_cONTROL_MMU - To enable MMU 
; 
; Note: Other fields of the CP15 c1 control register are not given here
;       as they are not of importance for StarterWare. However, optionally
;       they can also be ADDed.
;
;*****************************************************************************
CP15ControlFeatureDisable
    MRC     p15, #0, r1, c1, c0, #0 
    BIC     r0, r1, r0  
    MCR     p15, #0, r0, c1, c0, #0
    DSB
    BX      lr

;*****************************************************************************
; This API Enables specified features in CP15 control register
;  r0 -  features   Features to disable in coprocessor 15 control
;                       register. 
;               'features' can take any OR a combination of the
;               below  values. 
;                CP15_cONTROL_TEXREMAP - TEX remap flag 
;                CP15_cONTROL_AccESSFLAG - Access flag control 
;                CP15_cONTROL_ALIGN_cHcK - Alignment Fault checking 
;                CP15_cONTROL_MMU - To enable MMU 
; 
; Note: Other fields of the CP15 c1 control register are not given here
;       as they are not of importance for StarterWare. However, optionally
;       they can also be ADDed.
;
;*****************************************************************************
CP15controlFeatureEnable
    MRC     p15, #0, r1, c1, c0, #0 
    ORR     r0, r1, r0  
    MCR     p15, #0, r0, c1, c0, #0
    DSB
    BX      lr

;*****************************************************************************
; This API configures the TTB control register to use only TTB0
;*****************************************************************************
CP15TtbCtlTtb0Config
    MOV     r0, #0x0
    MCR     p15, #0, r0, c2, c0, #2
    DSB
    BX      lr

;*****************************************************************************
; This API Sets the specified fields in Auxiliary control Register
; r0 - Bit Mask for the bits to be set in Auxiliary control Register
;*****************************************************************************
CP15AuxcontrolFeatureEnable
    MRC     p15, #0, r1, c1, c0, #1 
    ORR     r0,  r0, r1
    MCR     p15, #0, r0, c1, c0, #1
    DSB
    BX      lr

;*****************************************************************************
; This API clears the specified fields in Auxiliary control Register
; r0 - Bit Mask for the bits to be cleared in Auxiliary control Register
;*****************************************************************************
CP15AuxcontrolFeatureDisable
    MRC     p15, #0, r1, c1, c0, #1 
    BIC     r0,  r1, r0
    MCR     p15, #0, r0, c1, c0, #1
    DSB
    BX      lr

;*****************************************************************************
; This API returns the main ID register in r0
;*****************************************************************************
CP15MainIdPrimPartNumGet
    MRC     p15, #0, r0, c0, c0, #0
    UBFX    r0, r0, #4, #12
    BX      lr
    
;*****************************************************************************
; This API enable the NEON core 
;*****************************************************************************
CP15FpuEnable
     MCR p15, 0 , r0 , c1 ,c1,2
     ORR r0,r0,#3<<10
     MCR p15,0,r0,c1,c1,2
     MRC p15, 0, r1, c1, c0, 2  ;r1 = Access control Register 
     ORR r1, r1, #(0xf << 20)   ;enable full access for p10,11 
     MCR p15, 0, r1, c1, c0, 2  ;Access control Register = r1
     MOV r1, #0                
     MCR p15, 0, r1, c7, c5, 4  ;flush prefetch buffer because of FMXR belo and cP 10 & 11 were only just enabled
     ISB			;instruction de synchro 
     MOV r0,#0x40000000
     FMXR FPEXc,r0
     BX      lr

;*****************************************************************************
; This API enable the NEON core 
;*****************************************************************************
CP15NeonEnable

     MRC p15, 0, r1, c1, c0, 2  ;r1 = Access control Register 
     ORR r1, r1, #(0xf << 20)   ;enable full access for p10,11 
     MCR p15, 0, r1, c1, c0, 2  ;Access control Register = r1
     MOV r1, #0                
     MCR p15, 0, r1, c7, c5, 4  ;flush prefetch buffer because of FMXR belo and cP 10 & 11 were only just enabled
     ISB			;instruction de synchro 
     MOV r0,#0x40000000
;     FMXR FPEXc,r0
;     mov r1,#0
;     movt r1,#0x0300
;     FMRX r0,FPScR
;     ORR r0,r1,r0
;     FMXR FPScR,r0
     BX      lr

;*****************************************************************************
; This function fill L2 cache and lock L2
;*****************************************************************************
CP15Preload 
 
; PRELOAD and lock WAY 0  
     mov r0,#0XFE 
     MCR p15, 1, r0, c9, c0, 0   ; unlock way0
     mov r4,#0x0000
     movt r4,#0x3400
     MCR p15,0,r4,c11,c4,0       ;DT MEMORY-cAcHE, et way 0 et interrupt 
     mov r2,#0x2000
     movt r2,#0x4030
     MCR p15,0,r2,c11,c5,0       ; base addr stack
     mov r3,#0x4F00
     add r3,r3,#0xc0 
     MCR p15,0,r3,c11,c7,0       ; 512 lines = 32 kbyte by way 
     MCR p15,0,r1,c11,c3,1       ; start  PLE enable 
loop00
     MRC p15,0,r5,c11,c8,0
     AND r5,r5,#3
     CMP r5,#3
     bne loop00 
     MCR p15,0,r1,c11,c3,2 	 ; clear PLE enable
     
     mov r2,#0xD000
     movt r2,#0x8074
     MCR p15,0,r2,c11,c5,0       ; base addr stack
     mov r3,#0x2F00
     add r3,r3,#0xc0 
     MCR p15,0,r3,c11,c7,0       ; 512 lines = 32 kbyte by way 
     MCR p15,0,r1,c11,c3,1       ; start  PLE enable 
loop01
     MRC p15,0,r5,c11,c8,0
     AND r5,r5,#3
     CMP r5,#3
    bne loop01 
     MCR p15,0,r1,c11,c3,2 	 ; clear PLE enable
     
     mov r0,#0XFD
     MCR p15, 1, r0, c9, c0, 0   ; unlock way1
; PRELOAD and lock WAY 2
     mov r2,#0		 	 ;bss zone
     movt r2,#0x80B0
     MCR p15,0,r2,c11,c5,0
     ADD r4,R4,#1	 	;select way 2
     MCR p15,0,r4,c11,c4,0
     mov r3,#0x7F00
     add r3,r3,#0xc0 
     MCR p15,0,r3,c11,c7,0       ; 512 lines = 32 kbyte by way
     MCR p15,0,r1,c11,c3,1       ; start  PLE enable 
loop02
     MRC p15,0,r5,c11,c8,0
     AND r5,r5,#3
     CMP r5,#3
     bne loop02  
     MCR p15,0,r1,c11,c3,2 	; clear PLE enable 
     
      mov r0,#0XFB 
     MCR p15, 1, r0, c9, c0, 0   ; unlock way2
; PRELOAD and lock WAY 3
     ADD r4,R4,#1	 	;select way 3
     MCR p15,0,r4,c11,c4,0
     MCR p15,0,r3,c11,c7,0       ; 512 lines = 32 kbyte by way
     MCR p15,0,r1,c11,c3,1       ; start  PLE enable 
loop03
     MRC p15,0,r5,c11,c8,0
     AND r5,r5,#3
     CMP r5,#3
     bne loop03  
     MCR p15,0,r1,c11,c3,2 	; clear PLE enable 
     
     mov r0,#0XF7 
     MCR p15, 1, r0, c9, c0, 0   ; unlock way2
     
; PRELOAD and lock WAY 4
     ADD r4,R4,#1	 	;select way 4
     MCR p15,0,r4,c11,c4,0
     MCR p15,0,r3,c11,c7,0       ; 512 lines = 32 kbyte by way
     MCR p15,0,r1,c11,c3,1       ; start  PLE enable 
loop04
     MRC p15,0,r5,c11,c8,0
     AND r5,r5,#3
     CMP r5,#3
     bne loop04  
     MCR p15,0,r1,c11,c3,2 	; clear PLE enable   
     
      mov r0,#0XEF 
     MCR p15, 1, r0, c9, c0, 0   ; unlock way2
; PRELOAD and lock WAY 5
     ADD r4,R4,#1	 	;select way 5
     MCR p15,0,r4,c11,c4,0
     MCR p15,0,r3,c11,c7,0       ; 512 lines = 32 kbyte by way
     MCR p15,0,r1,c11,c3,1       ; start  PLE enable 
loop05
     MRC p15,0,r5,c11,c8,0
     AND r5,r5,#3
     CMP r5,#3
     bne loop05  
     MCR p15,0,r1,c11,c3,2 	; clear PLE enable   

     mov r0,#0XDF 
     MCR p15, 1, r0, c9, c0, 0   ; unlock way2
; PRELOAD and lock WAY 6
     ADD r4,R4,#1	 	;select way 6
     MCR p15,0,r4,c11,c4,0
     MCR p15,0,r3,c11,c7,0       ; 512 lines = 32 kbyte by way
     MCR p15,0,r1,c11,c3,1       ; start  PLE enable 
loop06
     MRC p15,0,r5,c11,c8,0
     AND r5,r5,#3
     CMP r5,#3
     bne loop06  
     MCR p15,0,r1,c11,c3,2 	; clear PLE enable    

    mov r0,#0XBF 
     MCR p15, 1, r0, c9, c0, 0   ; unlock way2
; PRELOAD and lock WAY 7
     ADD r4,R4,#1	 	;select way 6
     MCR p15,0,r4,c11,c4,0
     MCR p15,0,r3,c11,c7,0       ; 512 lines = 32 kbyte by way
     MCR p15,0,r1,c11,c3,1       ; start  PLE enable 
loop07
     MRC p15,0,r5,c11,c8,0
     AND r5,r5,#3
     CMP r5,#3
     bne loop07  
     MCR p15,0,r1,c11,c3,2 	; clear PLE enable  

; mov r0,#0X7F
;     MCR p15, 1, r0, c9, c0, 0   ; unlock way1
; PRELOAD and lock WAY 2
;     mov r2,#0x8000		 	 ;bss zone
 ;    movt r2,#0x8074
;     MCR p15,0,r2,c11,c5,0
;     ADD r4,R4,#1	 	;select way 2
;     MCR p15,0,r4,c11,c4,0
;     mov r3,#0x7F00
;     add r3,r3,#0xc0 
;     MCR p15,0,r3,c11,c7,0       ; 512 lines = 32 kbyte by way
;     MCR p15,0,r1,c11,c3,1       ; start  PLE enable 
;loop08:
;     MRC p15,0,r5,c11,c8,0
;     AND r5,r5,#3
 ;    CMP r5,#3
;     bne loop08  
;     MCR p15,0,r1,c11,c3,2 	; clear PLE enable     
   
     mov r1,#0x7F
     MCR p15, 1, r1, c9, c0, 0   ;lock way 2
     MCR p15,0,r1,c11,c3,2 	; clear PLE enable 
 		
     BX      lr



;*****************************************************************************
; This function fill L2 cache and lock L2
;*****************************************************************************
CP15IPreload 
 	PUSH    {r4-r11}
	
	SUB r1,r1,r1
testpli	
	PLI [r0,r1]
	ISB
	add  r1,r1,#0x20
	CMP  r1,#0x1800
	bne  testpli
	POP 	{r4-r11}
	bx 	lr
     
 


;
; End of the file
;
    END

