@******************************************************************************
@
@ cp15.S - CP15 coprocesser configuration APIs
@
@******************************************************************************
@
@ Copyright (C) 2010-2014 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.
@
@******************************************************************************
@ Updated by Brighton Feng for Cortex A15 on June 15, 2014
@****************************** Global Symbols*********************************
        .global CP15_ICacheDisable
        .global CP15_DCacheDisable
        .global CP15_ICacheEnable
        .global CP15_DCacheEnable
        .global CP15_ICacheInvalidate
        .global CP15_DCacheCleanInvalidate
        .global CP15_DCacheClean
        .global CP15_DCacheInvalidate
        .global CP15_DCacheCleanBuff
        .global CP15_DCacheCleanInvalidateBuff
        .global CP15_DCacheInvalidateBuff
        .global CP15_ICacheInvalidateBuff
        .global CP15_IsICacheEnabled
        .global CP15_IsDCacheEnabled
        .global CP15_Ttb0Set
        .global CP15_TLBInvalidateAll
        .global CP15_MMUDisable
        .global CP15_MMUEnable
        .global CP15_VectorBaseAddrSet
        .global CP15_BranchPredictorInvalidate
        .global CP15_BranchPredictionEnable
        .global CP15_BranchPredictionDisable
        .global CP15_DACRConfig
        .global CP15_SCTLRConfigBitField
        .global CP15_SCTLRConfig
        .global CP15_TTBCRShortConfig
        .global CP15_TTBCRLongConfig
        .global CP15_ACTLRConfigBitField
        .global CP15_MainIdPrimPartNumGet
        .global CP15_64bitTtb0Set
        .global CP15_MAIR0Config
        .global CP15_MAIR1Config
        .global CP15_get_PMU_N
        .global CP15_PMN_config
        .global CP15_CCNT_divider
        .global CP15_enable_PMU
        .global CP15_disable_PMU
        .global CP15_enable_CCNT
        .global CP15_disable_CCNT
        .global CP15_enable_PMN
        .global CP15_disable_PMN
        .global CP15_enable_PMU_user_access
        .global CP15_disable_PMU_user_access
        .global CP15_read_CCNT
        .global CP15_read_PMN
        .global CP15_PMU_software_increment
        .global CP15_PMU_read_flags
        .global CP15_PMU_clear_flags
        .global CP15_enable_CCNT_irq
        .global CP15_disable_CCNT_irq
        .global CP15_enable_PMN_irq
        .global CP15_disable_PMN_irq
        .global CP15_reset_PMN
        .global CP15_reset_CCNT
        .global CP15_get_L2_memory_error_count
        .global CP15_get_CPU_memory_error_count
        .global CP15_clear_AXI_bus_error
        .global CP15_clear_L2_async_error
        .global CP15_get_CPU_ID
        .global CP15_get_interrupt_status
        .global CP14_get_DBGDSCR
        .global CP14_set_DBGDSCR

@**************************** Code section ************************************
        .text

        @ This code is assembled for ARM instructions
        .code 32

@-----------------------------------------------------------------------------
@ Cache operations
@-----------------------------------------------------------------------------

@*****************************************************************************
@ This API disable the InSTRuction cache.
@*****************************************************************************
CP15_ICacheDisable:
    PUSH    {r0, lr}    /*save LR cause there is function call in this function*/
    MRC     p15, #0, r0, c1, c0, #0 	/*SCTLR, System Control Register*/
    BIC     r0,  r0, #0x00001000        /*bit 12: I, Instruction cache enable*/
    MCR     p15, #0, r0, c1, c0, #0 	/*SCTLR, System Control Register*/
    BL      CP15_ICacheInvalidate             /*Invalidate I cache*/
    POP     {r0, lr}
    BX      lr

@*****************************************************************************
@ This API disable the Data cache.
@*****************************************************************************
CP15_DCacheDisable:
    PUSH    {r0-r2, lr} /*save LR cause there is function call in this function*/
    MRC     p15, #0, r0, c1, c0, #0 	/*SCTLR, System Control Register*/
    BIC     r0,  r0, #0x00000004        /*bit 2: C, global enable bit for data and unified caches*/
    MCR     p15, #0, r0, c1, c0, #0 	/*SCTLR, System Control Register*/
    BL      CP15_DCacheCleanInvalidate        /*writeback and invalidate D cache*/
    POP     {r0-r2, lr}
    BX      lr

@*****************************************************************************
@ This API enables I-cache
@*****************************************************************************
CP15_ICacheEnable:
    MRC     p15, #0, r0, c1, c0, #0 	/*SCTLR, System Control Register*/
    ORR     r0,  r0, #0x00001000        /*bit 12: I, Instruction cache enable*/
    MCR     p15, #0, r0, c1, c0, #0 	/*SCTLR, System Control Register*/ 
    BX      lr

@*****************************************************************************
@ This API enable the Data cache.
@*****************************************************************************
CP15_DCacheEnable:
    MRC     p15, #0, r0, c1, c0, #0 	/*SCTLR, System Control Register*/ 
    ORR     r0,  r0, #0x00000004        /*bit 2: C, global enable bit for data and unified caches*/
    MCR     p15, #0, r0, c1, c0, #0 	/*SCTLR, System Control Register*/
    BX      lr

@*****************************************************************************
@ This API Invalidates the entire Data/Unified Cache
@*****************************************************************************
CP15_DCacheInvalidate:
    PUSH    {r4-r11}
    DMB
    MRC     p15, #1, r0, c0, c0, #1  @ Read CLID,  Cache Level ID Register
    ANDS    r3, r0, #0x7000000       @ Get Level of Coherency ([26:24] LoC)
    MOV     r3, r3, lsr #23 		/*read (LOC x 2) */
    BEQ     ffinished
    MOV     r10, #0 			/*start from level 0*/
floop1: 		/*loop for all level of caches*/
    ADD     r2, r10, r10, lsr #1
    MOV     r1, r0, lsr r2
    AND     r1, r1, #7  	/*read Ctype for this level*/
    CMP     r1, #2 	
    BLT     fskip 	/*Ctype < 2, not data/unified cache*/
    MCR     p15, #2, r10, c0, c0, #0 	/* CSSELR, Cache Size Selection Register*/
    ISB
    MRC     p15, #1, r1, c0, c0, #0 	/* CCSIDR, Cache Size ID Register*/
    AND     r2, r1, #7 			/*read line size in  (log2 (number of words in cache line)) - 2*/
    ADD     r2, r2, #4          /*calculate Log2(LINELEN)*/
    LDR     r4, _FLD_MAX_WAY
    ANDS    r4, r4, r1, lsr #3 		/*read number of ways*/
    CLZ     r5, r4
    LDR     r7, _FLD_MAX_IDX
    ANDS    r7, r7, r1, lsr #13 	/*read number of sets (lines)*/
floop2: 		/*loop for all sets*/
    MOV     r9, r4
floop3: 		/*loop for all all ways*/
    ORR     r11, r10, r9, lsl r5        /*level|way<<(32-Log2(ASSOCIATIVITY))*/
    ORR     r11, r11, r7, lsl r2 		/*|SETS<<Log2(LINELEN)*/
    MCR     p15, #0, r11, c7, c6, #2 	/*DCISW, Data cache invalidate by set/way*/
    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     {r4-r11}
    BX      lr

@*****************************************************************************
@ This API cleans/writeback the entire D Cache to PoC
@*****************************************************************************
CP15_DCacheClean:
    PUSH    {r4-r11}
    DMB
    MRC     p15, #1, r0, c0, c0, #1  @ Read CLID,  Cache Level ID Register
    ANDS    r3, r0, #0x7000000       @ Get Level of Coherency ([26:24] LoC)
    MOV     r3, r3, lsr #23 		/*read (LOC x 2) */
    BEQ     cfinished
    MOV     r10, #0 			/*start from level 0*/
cloop1: 		/*loop for all level of caches*/
    ADD     r2, r10, r10, lsr #1
    MOV     r1, r0, lsr r2
    AND     r1, r1, #7  	/*read Ctype for this level*/
    CMP     r1, #2
    BLT     cskip 	/*Ctype < 2, not data/unified cache*/
    MCR     p15, #2, r10, c0, c0, #0 	/* CSSELR, Cache Size Selection Register*/
    ISB
    MRC     p15, #1, r1, c0, c0, #0 	/* CCSIDR, Cache Size ID Register*/
    AND     r2, r1, #7 			/*read line size in  (log2 (number of words in cache line)) - 2*/
    ADD     r2, r2, #4          /*calculate Log2(LINELEN)*/
    LDR     r4, _FLD_MAX_WAY
    ANDS    r4, r4, r1, lsr #3 		/*read number of ways*/
    CLZ     r5, r4
    LDR     r7, _FLD_MAX_IDX
    ANDS    r7, r7, r1, lsr #13 	/*read number of sets (lines)*/
cloop2: 		/*loop for all sets*/
    MOV     r9, r4
cloop3: 		/*loop for all all ways*/
    ORR     r11, r10, r9, lsl r5        /*level|way<<(32-Log2(ASSOCIATIVITY))*/
    ORR     r11, r11, r7, lsl r2 		/*|SETS<<Log2(LINELEN)*/
    MCR     p15, #0, r11, c7, c10, #2 	/*DCCSW, Data cache clean by set/way*/
    SUBS    r9, r9, #1
    BGE     cloop3
    SUBS    r7, r7, #1
    BGE     cloop2
cskip:
    ADD     r10, r10, #2
    CMP     r3, r10
    BGT     cloop1

cfinished:
    DSB
    ISB
    POP     {r4-r11}
    BX      lr

@*****************************************************************************
@ This API cleans/writeback and invalidates the entire D Cache to PoC
@*****************************************************************************
CP15_DCacheCleanInvalidate:
    PUSH    {r4-r11} 
    DMB
    MRC     p15, #1, r0, c0, c0, #1  @ Read CLID,  Cache Level ID Register
    ANDS    r3, r0, #0x7000000       @ Get Level of Coherency ([26:24] LoC)
    MOV     r3, r3, lsr #23 		/*read (LOC x 2) */
    BEQ     finished  
    MOV     r10, #0 			/*start from level 0*/ 
loop1: 		/*loop for all level of caches*/
    ADD     r2, r10, r10, lsr #1 
    MOV     r1, r0, lsr r2
    AND     r1, r1, #7  	/*read Ctype for this level*/ 
    CMP     r1, #2  
    BLT     skip 	/*Ctype < 2, not data/unified cache*/ 
    MCR     p15, #2, r10, c0, c0, #0 	/* CSSELR, Cache Size Selection Register*/ 
    ISB 
    MRC     p15, #1, r1, c0, c0, #0 	/* CCSIDR, Cache Size ID Register*/ 
    AND     r2, r1, #7 			/*read line size in  (log2 (number of words in cache line)) - 2*/
    ADD     r2, r2, #4          /*calculate Log2(LINELEN)*/
    LDR     r4, _FLD_MAX_WAY 
    ANDS    r4, r4, r1, lsr #3 		/*read number of ways*/
    CLZ     r5, r4
    LDR     r7, _FLD_MAX_IDX
    ANDS    r7, r7, r1, lsr #13 	/*read number of sets (lines)*/
loop2: 		/*loop for all sets*/
    MOV     r9, r4   
loop3: 		/*loop for all all ways*/
    ORR     r11, r10, r9, lsl r5        /*level|way<<(32-Log2(ASSOCIATIVITY))*/
    ORR     r11, r11, r7, lsl r2 		/*|SETS<<Log2(LINELEN)*/
    MCR     p15, #0, r11, c7, c14, #2 	/*DCCISW, Data cache clean/writeback and invalidate by set/way*/ 
    SUBS    r9, r9, #1 
    BGE     loop3 
    SUBS    r7, r7, #1 
    BGE     loop2 
skip: 
    ADD     r10, r10, #2 
    CMP     r3, r10 
    BGT     loop1 

finished:
    DSB
    ISB                        
    POP     {r4-r11} 
    BX      lr

_FLD_MAX_WAY:
   .word  0x3ff
_FLD_MAX_IDX:
   .word  0x7fff

@*****************************************************************************
@ This API invalidates entire I Cache
@*****************************************************************************
CP15_ICacheInvalidate:
    MOV     r0, #0
    MCR     p15, #0, r0, c7, c5, #0 	/*ICIALLU, Instruction cache invalidate all to PoU*/
    DSB
    ISB                        
    BX      lr

@*****************************************************************************
@ This API cleans/wirteback 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/writeback
@*****************************************************************************
CP15_DCacheCleanBuff:
    PUSH    {r3, 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   
    TST     r3, r14                   @ is it aligned?
    BIC     r14, r14, r3
    MCRNE   p15, #0, r14, c7, c10, #1 @clean the last unaligned line, DCCMVAC, Clean D/Unified line to PoC by MVA

cleanloop:    
    MCR     p15, #0, r0 , c7, c10, #1 @DCCMVAC, Clean D/Unified to PoC by MVA
    ADDS    r0, r0, r2                @ Go to next line
    CMP     r0, r14 
    BLT     cleanloop
 
    POP     {r3, r14}
    DSB
    BX      lr

@*****************************************************************************
@ This API cleans/writeback 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/writeback and invalidated
@*****************************************************************************
CP15_DCacheCleanInvalidateBuff:
    PUSH    {r3, 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   
    TST     r3, r14                   @ is it aligned?
    BIC     r14, r14, r3
    MCRNE   p15, #0, r14, c7, c14, #1 @DCCIMVAC, Clean and Invalidate D/U line to PoC (operate the last unaligned line)

cleanflushloop:    
    MCR     p15, #0, r0 , c7, c14, #1 @DCCIMVAC, Clean and Invalidate D/U line to PoC 
    ADDS    r0, r0, r2                @ Go to next line
    CMP     r0, r14 
    BLT     cleanflushloop
 
    POP     {r3, 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 invalidated
@*****************************************************************************
CP15_DCacheInvalidateBuff:
    PUSH    {r3, 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                    @ is it aligned?
    BIC     r0, r0, r3                @ Align to cache line boundary   
    MCRNE   p15, #0, r0, c7, c14, #1  @DCCIMVAC, Clean and Invalidate D/U line to PoC (writeback the first unaligned line)
    ADDNE   r0, r0, r2                @ skip unaligned line
    TST     r3, r14                   @ is it aligned?
    BIC     r14, r14, r3              
    MCRNE   p15, #0, r14, c7, c14, #1 @DCCIMVAC, Clean and Invalidate D/U line to PoC (writeback the last unaligned line)
    B       dflushcmp

dflushloop:
    MCR     p15, #0, r0 , c7, c6, #1  @DCIMVAC, Invalidate D/U line to PoC    
    ADDS    r0, r0, r2                @ Go to next line

dflushcmp:
    CMP     r0, r14
    BLT     dflushloop
    POP     {r3, 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 invalidated
@*****************************************************************************
CP15_ICacheInvalidateBuff:
    PUSH    {r3, 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 IMinLine
    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   
    TST     r3, r14                   @ is it aligned?
    BIC     r14, r14, r3
    MCRNE   p15, #0, r14, c7, c5, #1  @ICIMVAU, Invalidate by MVA to PoU (operate the last unaligned line)

iflushloop:    
    MCR     p15, #0, r0, c7, c5, #1   @ICIMVAU, Invalidate by MVA to PoU
    ADDS    r0, r0, r2                @ Go to next line
    CMP     r0, r14 
    BLT     iflushloop
 
    POP     {r3, r14}
    DSB
    ISB                        
    BX      lr

@*****************************************************************************
@ is data and unified cache enabled
CP15_IsDCacheEnabled:
    MRC     p15, #0, r0, c1, c0, #0 	/*SCTLR, System Control Register*/
    LSR     r0,  r0, #2                 /*bit 2: C, data and unified cache*/
	AND     r0,  r0, #1
    BX      lr

@*****************************************************************************
@ is instruction cache enabled*/
CP15_IsICacheEnabled:
    MRC     p15, #0, r0, c1, c0, #0 	/*SCTLR, System Control Register*/
    LSR     r0,  r0, #12                /*bit 12: I, Instruction cache enable*/
	AND     r0,  r0, #1
    BX      lr

@-----------------------------------------------------------------------------
@ branch predictor operations
@-----------------------------------------------------------------------------

@*****************************************************************************
@ This API invalidates the branch predictor 
@*****************************************************************************
CP15_BranchPredictorInvalidate:
    MCR     p15, #0, r0, c7, c5, #6 	/*BPIALL, Invalidate all branch predictors*/
    ISB
    BX      lr

@*****************************************************************************
@ This API enables the branch predictor 
@*****************************************************************************
CP15_BranchPredictionEnable:
    MRC     p15, #0, r0, c1, c0, #0    /*read SCTLR, System Control Register*/
    ORR     r0, r0, #0x00000800 	    /*set Branch prediction bit*/
    MCR     p15, #0, r0, c1, c0, #0   /*write back to SCTLR, System Control Register*/
    ISB
    BX      lr

@*****************************************************************************
@ This API disables the branch predictor 
@*****************************************************************************
CP15_BranchPredictionDisable:
    MRC     p15, #0, r0, c1, c0, #0    /*read SCTLR, System Control Register*/
    BIC     r0, r0, #0x00000800         /*clear Branch prediction bit*/
    MCR     p15, #0, r0, c1, c0, #0   /*write back to SCTLR, System Control Register*/
    ISB
    BX      lr

@-----------------------------------------------------------------------------
@ MMU and Translation table operations
@-----------------------------------------------------------------------------

@*****************************************************************************
@ Sets TTB0 Register
@ r0 - Translation Table Base Address register value
@*****************************************************************************
CP15_Ttb0Set:
    MCR     p15, #0, r0, c2, c0, #0 	/*TTBR0, Translation Table Base Register 0*/
    DMB
    ISB
    BX      lr

@*****************************************************************************
@ Sets 64 bit TTB0 Register
@ r0 - Translation Table Low 32 bit Base Address
@ r1 - Translation Table High 32 bit Base Address
@*****************************************************************************
CP15_64bitTtb0Set:
    MCRR    p15, #0, r0, r1, c2      @ Write 64-bit TTBR0
    DMB
    ISB
    BX      lr

@*****************************************************************************
@ This API Invalidates All the TLB
@*****************************************************************************
CP15_TLBInvalidateAll:
    MCR     p15, #0, r0, c8, c7, #0    @ TLBIALL, TLB Invalidate All. r0 value will be ignored
    DSB
    BX      lr

@*****************************************************************************
@ This API Disables MMU. 
@*****************************************************************************
CP15_MMUDisable:
    MCR     p15, #0, r0, c8, c7, #0    @ TLBIALL, TLB Invalidate All.  
    MRC     p15, #0, r0, c1, c0, #0    /*read SCTLR, System Control Register*/
    BIC     r0, r0, #1       		     @ Clear MMU bit
    MCR     p15, #0, r0, c1, c0, #0    /*write back to SCTLR, System Control Register*/
    MCR     p15, #0, r0, c7, c5, #6 	/*BPIALL, Invalidate all branch predictors*/
    DSB  
    BX      lr

@*****************************************************************************
@ This API Enables MMU. 
@*****************************************************************************
CP15_MMUEnable:
    MRC     p15, #0, r0, c1, c0, #0    /*read SCTLR, System Control Register*/
    ORR     r0, r0, #0x001       	     @ set MMU bit
    MCR     p15, #0, r0, c1, c0, #0    /*write back to SCTLR, System Control Register*/
    MCR     p15, #0, r0, c7, c5, #6 	/*BPIALL, Invalidate all branch predictors*/
    DSB
    BX      lr

@*****************************************************************************
@ This API sets the Domain Access Control Register (DACR) 
@*****************************************************************************
CP15_DACRConfig:
    MCR     p15, #0, r0, c3, c0, #0
    DSB
    BX      lr

@*****************************************************************************
@ This API Configures the Translation Table Base Control Reg (TTBCR) for Short Descriptor
@*****************************************************************************
CP15_TTBCRShortConfig:
@@@@@@@@@@@    MOV     r0, #0x0
    MCR     p15, #0, r0, c2, c0, #2
    DSB
    BX      lr

@*****************************************************************************
@ This API set the Translation Table Base Control Reg (TTBCR) for Long Descriptor
@ 31    - EAE   - 0=Use old TT format, 1=Use new TT format (LPAE)
@ 30    - SBZ (Impl defined as UNK/SBZP on Cortex-A15)
@ 29:28 - SH1   - Shareability memory for translation table walks using TTBR1
@ 27:26 - ORGN1 - Outer cacheability for translation table walks using TTBR1
@ 25:24 - IRGN1 - Inner cacheability for translation table walks using TTBR1
@ 23    - EPD1  - TTBR1 disable (1=disabled, 0=enabled), equivalent to PD1 in old format
@ 22    - A1    - 0=TTBR0.ASID is the ASID  or  1=TTBR1.ASID is the the ASID
@ 21:19 - SBZ
@ 18:16 - T1SZ  - How much of memory is covered by TTRB1
@ 15:14 - SBZ
@ 13:12 - SH0   - Shareability memory for translation table walks using TTBR0
@ 11:10 - ORGN0 - Outer cacheability for translation table walks using TTBR0
@ 9:8   - IRGN0 - Inner cacheability for translation table walks using TTBR0
@ 7     - EPD0  - TTBR0 disable (1=disabled, 0=enabled), equivalent to PD0 in old format
@ 6:3   - SBZ
@ 2:0   - T0SZ  - How much of memory is covered by TTBR0
@*****************************************************************************
CP15_TTBCRLongConfig:     
    MCR     p15, #0, r0, c2, c0, #2     @ Use LPAE format, TTBR0 covers entire address space
    DSB
    BX		lr

@*****************************************************************************
@	Set up MAIR0
@ This API sets the MAIR0 registers
@ These are the settings to be programmed into MAIR0
@ 31:24 - Attr3
@ 23:16 - Attr2
@ 15:8  - Attr1
@ 7:0   - Attr0
@*****************************************************************************
CP15_MAIR0Config:
	MCR		p15, #0, r0, c10, c2, 0
	BX		lr

@*****************************************************************************
@   Set up MAIR1
@ This API sets the MAIR1 registers
@ These are the settings to be programmed into MAIR1
@ 31:24 - Attr7
@ 23:16 - Attr6
@ 15:8  - Attr5
@ 7:0   - Attr4
@*****************************************************************************
CP15_MAIR1Config:
    MCR     p15, #0, r0, c10, c2, 1
    BX      lr

@ ----------------------------------------------------------------------------
@ Performance Monitor Block
@ ----------------------------------------------------------------------------
  @ Returns the number of progammable counters
  @ uint32_t getPMN(void)
CP15_get_PMU_N:
  MRC     p15, #0, r0, c9, c12, #0 @ Read PMCR Register
  MOV     r0, r0, LSR #11        @ Shift N field down to bit 0
  AND     r0, r0, #0x1F          @ Mask to leave just the 5 N bits
  BX      lr
@end_getPMN


  @ Sets the event for a programmable counter to record
  @ void PMN_config(unsigned counter, uint32_t event)
  @ counter (in r0) = Which counter to program (e.g. 0 for PMN0, 1 for PMN1)
  @ event   (in r1) = The event code (from appropriate TRM or ARM Architecture Reference Manual)
CP15_PMN_config:
  AND     r0, r0, #0x1F          @ Mask to leave only bits 4:0
  MCR     p15, #0, r0, c9, c12, #5 @ Write PMSELR Register
  ISB                            @ Synchronize context
  MCR     p15, #0, r1, c9, c13, #1 @ Write PMXEVTYPER Register
  BX      lr
@end_PMN_config


  @ Enables/disables the divider (1/64) on CCNT
  @ void CCNT_divider(int divider)
  @ divider (in r0) = If 0 disable divider, else enable divider
CP15_CCNT_divider:
  MRC     p15, #0, r1, c9, c12, #0  @ Read PMCR

  CMP     r0, #0x0                @ IF (r0 == 0)
  BICEQ   r1, r1, #0x08           @ THEN: Clear the D bit (disables the divisor)
  ORRNE   r1, r1, #0x08           @ ELSE: Set the D bit (enables the divisor)

  MCR     p15, #0, r1, c9, c12, #0  @ Write PMCR
  BX      lr
@end_CCNT_divider


  @ Global PMU enable
  @ void enable_PMU(void)
CP15_enable_PMU:
  MRC     p15, #0, r0, c9, c12, #0  @ Read PMCR
  ORR     r0, r0, #0x01           @ Set E bit
  MCR     p15, #0, r0, c9, c12, #0  @ Write PMCR
  BX      lr
@end_enable_PMU


  @ Global PMU disable
  @ void disable_PMU(void)
CP15_disable_PMU:
  MRC     p15, #0, r0, c9, c12, #0  @ Read PMCR
  BIC     r0, r0, #0x01           @ Clear E bit
  MCR     p15, #0, r0, c9, c12, #0  @ Write PMCR
  BX      lr
@end_disable_PMU


  @ Enable the CCNT
  @ void enable_CCNT(void)
CP15_enable_CCNT:
  MOV     r0, #0x80000000         @ Set C bit
  MCR     p15, #0, r0, c9, c12, #1  @ Write PMCNTENSET Register
  BX      lr
@end_enable_CCNT


  @ Disable the CCNT
  @ void disable_CCNT(void)
CP15_disable_CCNT:
  MOV     r0, #0x80000000         @ Set C bit
  MCR     p15, #0, r0, c9, c12, #2  @ Write PMCNTENCLR Register
  BX      lr
@end_disable_CCNT


  @ Enable PMN{n}
  @ void enable_PMN(uint32_t counter)
  @ counter (in r0) = The counter to enable (e.g. 0 for PMN0, 1 for PMN1)
CP15_enable_PMN:
  MOV     r1, #0x1
  MOV     r1, r1, LSL r0
  MCR     p15, #0, r1, c9, c12, #1  @ Write PMCNTENSET Register
  BX      lr
@end_enable_PMN


  @ Disable PMN{n}
  @ void disable_PMN(uint32_t counter)
  @ counter (in r0) = The counter to disable (e.g. 0 for PMN0, 1 for PMN1)
CP15_disable_PMN:
  MOV     r1, #0x1
  MOV     r1, r1, LSL r0
  MCR     p15, #0, r1, c9, c12, #2  @ Write PMCNTENCLR Register
  BX      lr
@end_disable_PMN


  @ Enables User mode access to the PMU (must be called in a privileged mode)
  @ void enable_PMU_user_access(void)
CP15_enable_PMU_user_access:
  MRC     p15, #0, r0, c9, c14, #0  @ Read PMUSERENR Register
  ORR     r0, r0, #0x01           @ Set EN bit (bit 0)
  MCR     p15, #0, r0, c9, c14, #0  @ Write PMUSERENR Register
  ISB                             @ Synchronize context
  BX      lr
@end_enable_PMU_user_access


  @ Disables User mode access to the PMU (must be called in a privileged mode)
  @ void disable_PMU_user_access(void)
CP15_disable_PMU_user_access:
  MRC     p15, #0, r0, c9, c14, #0  @ Read PMUSERENR Register
  BIC     r0, r0, #0x01           @ Clear EN bit (bit 0)
  MCR     p15, #0, r0, c9, c14, #0  @ Write PMUSERENR Register
  ISB                             @ Synchronize context
  BX      lr
@end_disable_PMU_user_access


  @ Returns the value of CCNT
  @ uint32_t read_CCNT(void)
CP15_read_CCNT:
  MRC     p15, #0, r0, c9, c13, #0 @ Read PMCCNTR Register
  BX      lr
@end_read_CCNT


  @ Returns the value of PMN{n}
  @ uint32_t read_PMN(uint32_t counter)
  @ counter (in r0) = The counter to read (e.g. 0 for PMN0, 1 for PMN1)
CP15_read_PMN:
  AND     r0, r0, #0x1F          @ Mask to leave only bits 4:0
  MCR     p15, #0, r0, c9, c12, #5 @ Write PMSELR Register
  ISB                            @ Synchronize context
  MRC     p15, #0, r0, c9, c13, #2 @ Read current PMXEVCNTR Register
  BX      lr
@end_read_PMN


  @ Writes to software increment register
  @ void PMU_software_increment(uint32_t counter)
  @ counter (in r0) = The counter to increment (e.g. 0 for PMN0, 1 for PMN1)
CP15_PMU_software_increment:
  MOV     r1, #0x01
  MOV     r1, r1, LSL r0
  MCR     p15, #0, r1, c9, c12, #4 @ Write PMSWINCR Register
  BX      lr
@end_PMU_software_increment


  @ Returns the value of the overflow flags
  @ uint32_t PMU_read_flags(void)
CP15_PMU_read_flags:
  MRC     p15, #0, r0, c9, c12, #3 @ Read PMOVSR Register
  BX      lr
@end_read_flags


  @ Writes the overflow flags
  @ void CP15_PMU_clear_flags(uint32_t flags)
CP15_PMU_clear_flags:
  MCR     p15, #0, r0, c9, c12, #3 @ Write PMOVSR Register
  ISB                            @ Synchronize context
  BX      lr
@end_write_flags


  @ Enables interrupt generation on overflow of the CCNT
  @ void enable_CCNT_irq(void)
CP15_enable_CCNT_irq:
  MOV     r0, #0x80000000
  MCR     p15, #0, r0, c9, c14, #1  @ Write PMINTENSET Register
  BX      lr
@end_enable_CCNT_irq


  @ Disables interrupt generation on overflow of the CCNT
  @ void disable_CCNT_irq(void)
CP15_disable_CCNT_irq:
  MOV     r0, #0x80000000
  MCR     p15, #0, r0, c9, c14, #2   @ Write PMINTENCLR Register
  BX      lr
@end_disable_CCNT_irq


  @ Enables interrupt generation on overflow of PMN{x}
  @ void enable_PMN_irq(uint32_t counter)
  @ counter (in r0) = The counter to enable the interrupt for (e.g. 0 for PMN0, 1 for PMN1)
CP15_enable_PMN_irq:
  MOV     r1, #0x1
  MOV     r0, r1, LSL r0
  MCR     p15, #0, r0, c9, c14, #1   @ Write PMINTENSET Register
  BX      lr
@end_enable_PMN_irq


  @ Disables interrupt generation on overflow of PMN{x}
  @ void disable_PMN_irq(uint32_t counter)
  @ counter (in r0) = The counter to disable the interrupt for (e.g. 0 for PMN0, 1 for PMN1)
CP15_disable_PMN_irq:
  MOV     r1, #0x1
  MOV     r0, r1, LSL r0
  MCR     p15, #0, r0, c9, c14, #2  @ Write PMINTENCLR Register
  BX      lr
@end_disable_PMN_irq


  @ Resets all programmable counters to zero
  @ void reset_PMN(void)
CP15_reset_PMN:
  MRC     p15, #0, r0, c9, c12, #0  @ Read PMCR
  ORR     r0, r0, #0x2            @ Set P bit (Event counter reset)
  MCR     p15, #0, r0, c9, c12, #0  @ Write PMCR
  BX      lr
@end_reset_PMN


  @ Resets the CCNT
  @ void reset_CCNT(void)
CP15_reset_CCNT:
  MRC     p15, #0, r0, c9, c12, #0  @ Read PMCR
  ORR     r0, r0, #0x4            @ Set C bit (Clock counter reset)
  MCR     p15, #0, r0, c9, c12, #0  @ Write PMCR
  BX      lr
@end_reset_CCNT

@-----------------------------------------------------------------------------
@ Other utility functions
@-----------------------------------------------------------------------------

@*****************************************************************************
@ This API sets the interrupt vector table base address 
@ r0 - Interrput Vector Base Address
@*****************************************************************************
CP15_VectorBaseAddrSet:
    MCR     p15, #0, r0, c12, c0, #0 	/*VBAR, Vector Base Address Register*/
    DSB
    BX      lr

@*****************************************************************************
@ This API Configs 32-bit System Control Register
@ r0 - Value for the bits to be set in System Control Register
@*****************************************************************************
CP15_SCTLRConfig:
    MCR     p15, #0, r0, c1, c0, #0
    DSB
    BX      lr

@*****************************************************************************
@ This API Configs specified features in System Control Register
@ r0 - Feature for the bits to be set in System Control Register
@ r1 - Bit Mask for the bits to be clear in System Control Register
@*****************************************************************************
CP15_SCTLRConfigBitField:
    MRC     p15, #0, r2, c1, c0, #0 
    BIC     r2,  r2, r1  
    ORR     r2,  r2, r0
    MCR     p15, #0, r2, c1, c0, #0
    DSB
    BX      lr

@*****************************************************************************
@ This API Configs the specified fields in Auxiliary Control Register
@ r0 - Feature for the bits to be set in Auxiliary Control Register
@ r1 - Bit Mask for the bits to be clear in Auxiliary Control Register
@*****************************************************************************
CP15_ACTLRConfigBitField:
    MRC     p15, #0, r2, c1, c0, #1 
    BIC     r2,  r2, r1
    ORR     r2,  r2, r0
    MCR     p15, #0, r2, c1, c0, #1
    DSB
    BX      lr

@*****************************************************************************
@ This API returns the main ID register in r0
@*****************************************************************************
CP15_MainIdPrimPartNumGet:
    MRC     p15, #0, r0, c0, c0, #0
    UBFX    r0, r0, #4, #12
    BX      lr


@*****************************************************************************
@ This API returns the CPU ID in MPcore cluster
@*****************************************************************************
CP15_get_CPU_ID:
	MRC     p15, 0, r0, c0, c0, 5 @read Multiprocessor Affinity Register (MPIDR)
    AND     r0, r0, #3
    BX      lr

@*****************************************************************************
@ This API clear L2 internal asynchronous error in  L2ECTLR register
@*****************************************************************************
CP15_clear_L2_async_error:
    MRC     p15, 1, r0, c9, c0, 3        @read L2ECTLR
    BIC     r0, r0, #(1<<30)             @clear L2 internal asynchronous error bit
    MCR     p15, 1, r0, c9, c0, 3        @write L2ECTLR
    BX      lr

@*****************************************************************************
@ This API clear AXI asynchronous error in  L2ECTLR register
@*****************************************************************************
CP15_clear_AXI_bus_error:
    MRC     p15, 1, r0, c9, c0, 3        @read L2ECTLR
    BIC     r0, r0, #(1<<29)             @clear AXI asynchronous error bit
    MCR     p15, 1, r0, c9, c0, 3        @write L2ECTLR
    BX      lr

@*****************************************************************************
@ This API get CPU memory error count (higher 32-bit of CPU Memory Error Syndrome Register)
@*****************************************************************************
CP15_get_CPU_memory_error_count:
    MRRC    p15, 0, r0, r1, c15          @read 64-bit CPUMERRSR
    MOV     r0, r1                       @get the higher 32-bit including counts
    BX      lr

@*****************************************************************************
@ This API get L2 memory error count (higher 32-bit of L2 Memory Error Syndrome Register)
@*****************************************************************************
CP15_get_L2_memory_error_count:
    MRRC    p15, 1, r0, r1, c15          @read 64-bit L2MERRSR
    MOV     r0, r1                       @get the higher 32-bit including counts
    BX      lr

@*****************************************************************************
@ This API get the Interrupt Status Register
@*****************************************************************************
CP15_get_interrupt_status:
    MRC     p15,0,r0,c12,c1,0          @read ISR
    BX      lr

CP14_get_DBGDSCR:
	MRC 	p14,0,r0,c0,c2,2 		@ Read DBGDSCR
	BX 		lr

CP14_set_DBGDSCR:
	MCR 	p14,0,r0,c0,c2,2 		@ Write DBGDSCR
	BX 		lr

@
@ End of the file
@
    .end

