[mirror_edk2.git] / ArmPkg / Library / ArmLib / ArmV7 / ArmV7Support.asm

//------------------------------------------------------------------------------\r
//\r
// Copyright (c) 2008 - 2010, Apple Inc. All rights reserved.<BR>\r
// Copyright (c) 2011 - 2014, ARM Limited. All rights reserved.\r
//\r
// This program and the accompanying materials\r
// are licensed and made available under the terms and conditions of the BSD License\r
// which accompanies this distribution.  The full text of the license may be found at\r
// http://opensource.org/licenses/bsd-license.php\r
//\r
// THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,\r
// WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.\r
//\r
//------------------------------------------------------------------------------\r
\r
    EXPORT  ArmInvalidateInstructionCache\r
    EXPORT  ArmInvalidateDataCacheEntryByMVA\r
    EXPORT  ArmCleanDataCacheEntryByMVA\r
    EXPORT  ArmCleanInvalidateDataCacheEntryByMVA\r
    EXPORT  ArmInvalidateDataCacheEntryBySetWay\r
    EXPORT  ArmCleanDataCacheEntryBySetWay\r
    EXPORT  ArmCleanInvalidateDataCacheEntryBySetWay\r
    EXPORT  ArmDrainWriteBuffer\r
    EXPORT  ArmEnableMmu\r
    EXPORT  ArmDisableMmu\r
    EXPORT  ArmDisableCachesAndMmu\r
    EXPORT  ArmMmuEnabled\r
    EXPORT  ArmEnableDataCache\r
    EXPORT  ArmDisableDataCache\r
    EXPORT  ArmEnableInstructionCache\r
    EXPORT  ArmDisableInstructionCache\r
    EXPORT  ArmEnableSWPInstruction\r
    EXPORT  ArmEnableBranchPrediction\r
    EXPORT  ArmDisableBranchPrediction\r
    EXPORT  ArmSetLowVectors\r
    EXPORT  ArmSetHighVectors\r
    EXPORT  ArmV7AllDataCachesOperation\r
    EXPORT  ArmV7PerformPoUDataCacheOperation\r
    EXPORT  ArmDataMemoryBarrier\r
    EXPORT  ArmDataSyncronizationBarrier\r
    EXPORT  ArmInstructionSynchronizationBarrier\r
    EXPORT  ArmReadVBar\r
    EXPORT  ArmWriteVBar\r
    EXPORT  ArmEnableVFP\r
    EXPORT  ArmCallWFI\r
    EXPORT  ArmReadCbar\r
    EXPORT  ArmReadMpidr\r
    EXPORT  ArmReadTpidrurw\r
    EXPORT  ArmWriteTpidrurw\r
    EXPORT  ArmIsArchTimerImplemented\r
    EXPORT  ArmReadIdPfr1\r
\r
    AREA    ArmV7Support, CODE, READONLY\r
    PRESERVE8\r
\r
DC_ON           EQU     ( 0x1:SHL:2 )\r
IC_ON           EQU     ( 0x1:SHL:12 )\r
CTRL_M_BIT      EQU     (1 << 0)\r
CTRL_C_BIT      EQU     (1 << 2)\r
CTRL_B_BIT      EQU     (1 << 7)\r
CTRL_I_BIT      EQU     (1 << 12)\r
\r
\r
ArmInvalidateDataCacheEntryByMVA\r
  mcr     p15, 0, r0, c7, c6, 1   ; invalidate single data cache line\r
  dsb\r
  isb\r
  bx      lr\r
\r
ArmCleanDataCacheEntryByMVA\r
  mcr     p15, 0, r0, c7, c10, 1  ; clean single data cache line\r
  dsb\r
  isb\r
  bx      lr\r
\r
\r
ArmCleanInvalidateDataCacheEntryByMVA\r
  mcr     p15, 0, r0, c7, c14, 1  ; clean and invalidate single data cache line\r
  dsb\r
  isb\r
  bx      lr\r
\r
\r
ArmInvalidateDataCacheEntryBySetWay\r
  mcr     p15, 0, r0, c7, c6, 2        ; Invalidate this line\r
  dsb\r
  isb\r
  bx      lr\r
\r
\r
ArmCleanInvalidateDataCacheEntryBySetWay\r
  mcr     p15, 0, r0, c7, c14, 2       ; Clean and Invalidate this line\r
  dsb\r
  isb\r
  bx      lr\r
\r
\r
ArmCleanDataCacheEntryBySetWay\r
  mcr     p15, 0, r0, c7, c10, 2       ; Clean this line\r
  dsb\r
  isb\r
  bx      lr\r
\r
\r
ArmInvalidateInstructionCache\r
  mcr     p15,0,R0,c7,c5,0      ;Invalidate entire instruction cache\r
  isb\r
  bx      LR\r
\r
ArmEnableMmu\r
  mrc     p15,0,R0,c1,c0,0      ; Read SCTLR into R0 (Read control register configuration data)\r
  orr     R0,R0,#1              ; Set SCTLR.M bit : Enable MMU\r
  mcr     p15,0,R0,c1,c0,0      ; Write R0 into SCTLR (Write control register configuration data)\r
  dsb\r
  isb\r
  bx      LR\r
\r
ArmDisableMmu\r
  mrc     p15,0,R0,c1,c0,0      ; Read SCTLR into R0 (Read control register configuration data)\r
  bic     R0,R0,#1              ; Clear SCTLR.M bit : Disable MMU\r
  mcr     p15,0,R0,c1,c0,0      ; Write R0 into SCTLR (Write control register configuration data)\r
\r
  mcr     p15,0,R0,c8,c7,0      ; TLBIALL : Invalidate unified TLB\r
  mcr     p15,0,R0,c7,c5,6      ; BPIALL  : Invalidate entire branch predictor array\r
  dsb\r
  isb\r
  bx      LR\r
\r
ArmDisableCachesAndMmu\r
  mrc   p15, 0, r0, c1, c0, 0           ; Get control register\r
  bic   r0, r0, #CTRL_M_BIT             ; Disable MMU\r
  bic   r0, r0, #CTRL_C_BIT             ; Disable D Cache\r
  bic   r0, r0, #CTRL_I_BIT             ; Disable I Cache\r
  mcr   p15, 0, r0, c1, c0, 0           ; Write control register\r
  dsb\r
  isb\r
  bx      LR\r
\r
ArmMmuEnabled\r
  mrc     p15,0,R0,c1,c0,0      ; Read SCTLR into R0 (Read control register configuration data)\r
  and     R0,R0,#1\r
  bx      LR\r
\r
ArmEnableDataCache\r
  ldr     R1,=DC_ON             ; Specify SCTLR.C bit : (Data) Cache enable bit\r
  mrc     p15,0,R0,c1,c0,0      ; Read SCTLR into R0 (Read control register configuration data)\r
  orr     R0,R0,R1              ; Set SCTLR.C bit : Data and unified caches enabled\r
  mcr     p15,0,R0,c1,c0,0      ; Write R0 into SCTLR (Write control register configuration data)\r
  dsb\r
  isb\r
  bx      LR\r
\r
ArmDisableDataCache\r
  ldr     R1,=DC_ON             ; Specify SCTLR.C bit : (Data) Cache enable bit\r
  mrc     p15,0,R0,c1,c0,0      ; Read SCTLR into R0 (Read control register configuration data)\r
  bic     R0,R0,R1              ; Clear SCTLR.C bit : Data and unified caches disabled\r
  mcr     p15,0,R0,c1,c0,0      ; Write R0 into SCTLR (Write control register configuration data)\r
  dsb\r
  isb\r
  bx      LR\r
\r
ArmEnableInstructionCache\r
  ldr     R1,=IC_ON             ; Specify SCTLR.I bit : Instruction cache enable bit\r
  mrc     p15,0,R0,c1,c0,0      ; Read SCTLR into R0 (Read control register configuration data)\r
  orr     R0,R0,R1              ; Set SCTLR.I bit : Instruction caches enabled\r
  mcr     p15,0,R0,c1,c0,0      ; Write R0 into SCTLR (Write control register configuration data)\r
  dsb\r
  isb\r
  bx      LR\r
\r
ArmDisableInstructionCache\r
  ldr     R1,=IC_ON             ; Specify SCTLR.I bit : Instruction cache enable bit\r
  mrc     p15,0,R0,c1,c0,0      ; Read SCTLR into R0 (Read control register configuration data)\r
  BIC     R0,R0,R1              ; Clear SCTLR.I bit : Instruction caches disabled\r
  mcr     p15,0,R0,c1,c0,0      ; Write R0 into SCTLR (Write control register configuration data)\r
  isb\r
  bx      LR\r
\r
ArmEnableSWPInstruction\r
  mrc     p15, 0, r0, c1, c0, 0\r
  orr     r0, r0, #0x00000400\r
  mcr     p15, 0, r0, c1, c0, 0\r
  isb\r
  bx      LR\r
\r
ArmEnableBranchPrediction\r
  mrc     p15, 0, r0, c1, c0, 0 ; Read SCTLR into R0 (Read control register configuration data)\r
  orr     r0, r0, #0x00000800   ;\r
  mcr     p15, 0, r0, c1, c0, 0 ; Write R0 into SCTLR (Write control register configuration data)\r
  dsb\r
  isb\r
  bx      LR\r
\r
ArmDisableBranchPrediction\r
  mrc     p15, 0, r0, c1, c0, 0 ; Read SCTLR into R0 (Read control register configuration data)\r
  bic     r0, r0, #0x00000800   ;\r
  mcr     p15, 0, r0, c1, c0, 0 ; Write R0 into SCTLR (Write control register configuration data)\r
  dsb\r
  isb\r
  bx      LR\r
\r
ArmSetLowVectors\r
  mrc     p15, 0, r0, c1, c0, 0 ; Read SCTLR into R0 (Read control register configuration data)\r
  bic     r0, r0, #0x00002000   ; clear V bit\r
  mcr     p15, 0, r0, c1, c0, 0 ; Write R0 into SCTLR (Write control register configuration data)\r
  isb\r
  bx      LR\r
\r
ArmSetHighVectors\r
  mrc     p15, 0, r0, c1, c0, 0 ; Read SCTLR into R0 (Read control register configuration data)\r
  orr     r0, r0, #0x00002000   ; Set V bit\r
  mcr     p15, 0, r0, c1, c0, 0 ; Write R0 into SCTLR (Write control register configuration data)\r
  isb\r
  bx      LR\r
\r
ArmV7AllDataCachesOperation\r
  stmfd SP!,{r4-r12, LR}\r
  mov   R1, R0                ; Save Function call in R1\r
  mrc   p15, 1, R6, c0, c0, 1 ; Read CLIDR\r
  ands  R3, R6, #&7000000     ; Mask out all but Level of Coherency (LoC)\r
  mov   R3, R3, LSR #23       ; Cache level value (naturally aligned)\r
  beq   Finished\r
  mov   R10, #0\r
\r
Loop1\r
  add   R2, R10, R10, LSR #1    ; Work out 3xcachelevel\r
  mov   R12, R6, LSR R2         ; bottom 3 bits are the Cache type for this level\r
  and   R12, R12, #7            ; get those 3 bits alone\r
  cmp   R12, #2\r
  blt   Skip                    ; no cache or only instruction cache at this level\r
  mcr   p15, 2, R10, c0, c0, 0  ; write the Cache Size selection register (CSSELR) // OR in 1 for Instruction\r
  isb                           ; isb to sync the change to the CacheSizeID reg\r
  mrc   p15, 1, R12, c0, c0, 0  ; reads current Cache Size ID register (CCSIDR)\r
  and   R2, R12, #&7            ; extract the line length field\r
  add   R2, R2, #4              ; add 4 for the line length offset (log2 16 bytes)\r
  ldr   R4, =0x3FF\r
  ands  R4, R4, R12, LSR #3     ; R4 is the max number on the way size (right aligned)\r
  clz   R5, R4                  ; R5 is the bit position of the way size increment\r
  ldr   R7, =0x00007FFF\r
  ands  R7, R7, R12, LSR #13    ; R7 is the max number of the index size (right aligned)\r
\r
Loop2\r
  mov   R9, R4                  ; R9 working copy of the max way size (right aligned)\r
\r
Loop3\r
  orr   R0, R10, R9, LSL R5     ; factor in the way number and cache number into R11\r
  orr   R0, R0, R7, LSL R2      ; factor in the index number\r
\r
  blx   R1\r
\r
  subs  R9, R9, #1              ; decrement the way number\r
  bge   Loop3\r
  subs  R7, R7, #1              ; decrement the index\r
  bge   Loop2\r
Skip\r
  add   R10, R10, #2            ; increment the cache number\r
  cmp   R3, R10\r
  bgt   Loop1\r
\r
Finished\r
  dsb\r
  ldmfd SP!, {r4-r12, lr}\r
  bx    LR\r
\r
ArmV7PerformPoUDataCacheOperation\r
  stmfd SP!,{r4-r12, LR}\r
  mov   R1, R0                ; Save Function call in R1\r
  mrc   p15, 1, R6, c0, c0, 1 ; Read CLIDR\r
  ands  R3, R6, #&38000000    ; Mask out all but Level of Unification (LoU)\r
  mov   R3, R3, LSR #26       ; Cache level value (naturally aligned)\r
  beq   Finished2\r
  mov   R10, #0\r
\r
Loop4\r
  add   R2, R10, R10, LSR #1    ; Work out 3xcachelevel\r
  mov   R12, R6, LSR R2         ; bottom 3 bits are the Cache type for this level\r
  and   R12, R12, #7            ; get those 3 bits alone\r
  cmp   R12, #2\r
  blt   Skip2                   ; no cache or only instruction cache at this level\r
  mcr   p15, 2, R10, c0, c0, 0  ; write the Cache Size selection register (CSSELR) // OR in 1 for Instruction\r
  isb                           ; isb to sync the change to the CacheSizeID reg\r
  mrc   p15, 1, R12, c0, c0, 0  ; reads current Cache Size ID register (CCSIDR)\r
  and   R2, R12, #&7            ; extract the line length field\r
  add   R2, R2, #4              ; add 4 for the line length offset (log2 16 bytes)\r
  ldr   R4, =0x3FF\r
  ands  R4, R4, R12, LSR #3     ; R4 is the max number on the way size (right aligned)\r
  clz   R5, R4                  ; R5 is the bit position of the way size increment\r
  ldr   R7, =0x00007FFF\r
  ands  R7, R7, R12, LSR #13    ; R7 is the max number of the index size (right aligned)\r
\r
Loop5\r
  mov   R9, R4                  ; R9 working copy of the max way size (right aligned)\r
\r
Loop6\r
  orr   R0, R10, R9, LSL R5     ; factor in the way number and cache number into R11\r
  orr   R0, R0, R7, LSL R2      ; factor in the index number\r
\r
  blx   R1\r
\r
  subs  R9, R9, #1              ; decrement the way number\r
  bge   Loop6\r
  subs  R7, R7, #1              ; decrement the index\r
  bge   Loop5\r
Skip2\r
  add   R10, R10, #2            ; increment the cache number\r
  cmp   R3, R10\r
  bgt   Loop4\r
\r
Finished2\r
  dsb\r
  ldmfd SP!, {r4-r12, lr}\r
  bx    LR\r
\r
ArmDataMemoryBarrier\r
  dmb\r
  bx      LR\r
\r
ArmDataSyncronizationBarrier\r
ArmDrainWriteBuffer\r
  dsb\r
  bx      LR\r
\r
ArmInstructionSynchronizationBarrier\r
  isb\r
  bx      LR\r
\r
ArmReadVBar\r
  // Set the Address of the Vector Table in the VBAR register\r
  mrc     p15, 0, r0, c12, c0, 0\r
  bx      lr\r
\r
ArmWriteVBar\r
  // Set the Address of the Vector Table in the VBAR register\r
  mcr     p15, 0, r0, c12, c0, 0\r
  // Ensure the SCTLR.V bit is clear\r
  mrc     p15, 0, r0, c1, c0, 0 ; Read SCTLR into R0 (Read control register configuration data)\r
  bic     r0, r0, #0x00002000   ; clear V bit\r
  mcr     p15, 0, r0, c1, c0, 0 ; Write R0 into SCTLR (Write control register configuration data)\r
  isb\r
  bx      lr\r
\r
ArmEnableVFP\r
  // Read CPACR (Coprocessor Access Control Register)\r
  mrc     p15, 0, r0, c1, c0, 2\r
  // Enable VPF access (Full Access to CP10, CP11) (V* instructions)\r
  orr     r0, r0, #0x00f00000\r
  // Write back CPACR (Coprocessor Access Control Register)\r
  mcr     p15, 0, r0, c1, c0, 2\r
  isb\r
  // Set EN bit in FPEXC. The Advanced SIMD and VFP extensions are enabled and operate normally.\r
  mov     r0, #0x40000000\r
  mcr     p10,#0x7,r0,c8,c0,#0\r
  bx      lr\r
\r
ArmCallWFI\r
  wfi\r
  bx      lr\r
\r
//Note: Return 0 in Uniprocessor implementation\r
ArmReadCbar\r
  mrc     p15, 4, r0, c15, c0, 0  //Read Configuration Base Address Register\r
  bx      lr\r
\r
ArmReadMpidr\r
  mrc     p15, 0, r0, c0, c0, 5     ; read MPIDR\r
  bx      lr\r
\r
ArmReadTpidrurw\r
  mrc     p15, 0, r0, c13, c0, 2    ; read TPIDRURW\r
  bx      lr\r
\r
ArmWriteTpidrurw\r
  mcr     p15, 0, r0, c13, c0, 2   ; write TPIDRURW\r
  bx      lr\r
\r
ArmIsArchTimerImplemented\r
  mrc    p15, 0, r0, c0, c1, 1     ; Read ID_PFR1\r
  and    r0, r0, #0x000F0000\r
  bx     lr\r
\r
ArmReadIdPfr1\r
  mrc    p15, 0, r0, c0, c1, 1     ; Read ID_PFR1 Register\r
  bx     lr\r
\r
 END\r
Commit	Line	Data
3402aac7	1	//------------------------------------------------------------------------------\r
bd6b9799	2	//\r
bd6b9799	3	// Copyright (c) 2008 - 2010, Apple Inc. All rights reserved.<BR>\r
9401d6f4	4	// Copyright (c) 2011 - 2014, ARM Limited. All rights reserved.\r
bd6b9799	5	//\r
	6	// This program and the accompanying materials\r
	7	// are licensed and made available under the terms and conditions of the BSD License\r
	8	// which accompanies this distribution. The full text of the license may be found at\r
	9	// http://opensource.org/licenses/bsd-license.php\r
	10	//\r
	11	// THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,\r
	12	// WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.\r
	13	//\r
	14	//------------------------------------------------------------------------------\r
	15	\r
	16	EXPORT ArmInvalidateInstructionCache\r
	17	EXPORT ArmInvalidateDataCacheEntryByMVA\r
	18	EXPORT ArmCleanDataCacheEntryByMVA\r
	19	EXPORT ArmCleanInvalidateDataCacheEntryByMVA\r
	20	EXPORT ArmInvalidateDataCacheEntryBySetWay\r
	21	EXPORT ArmCleanDataCacheEntryBySetWay\r
	22	EXPORT ArmCleanInvalidateDataCacheEntryBySetWay\r
	23	EXPORT ArmDrainWriteBuffer\r
	24	EXPORT ArmEnableMmu\r
	25	EXPORT ArmDisableMmu\r
	26	EXPORT ArmDisableCachesAndMmu\r
	27	EXPORT ArmMmuEnabled\r
	28	EXPORT ArmEnableDataCache\r
	29	EXPORT ArmDisableDataCache\r
	30	EXPORT ArmEnableInstructionCache\r
	31	EXPORT ArmDisableInstructionCache\r
	32	EXPORT ArmEnableSWPInstruction\r
	33	EXPORT ArmEnableBranchPrediction\r
	34	EXPORT ArmDisableBranchPrediction\r
	35	EXPORT ArmSetLowVectors\r
	36	EXPORT ArmSetHighVectors\r
	37	EXPORT ArmV7AllDataCachesOperation\r
d60f6af4	38	EXPORT ArmV7PerformPoUDataCacheOperation\r
bd6b9799	39	EXPORT ArmDataMemoryBarrier\r
	40	EXPORT ArmDataSyncronizationBarrier\r
	41	EXPORT ArmInstructionSynchronizationBarrier\r
836c3500	42	EXPORT ArmReadVBar\r
bd6b9799	43	EXPORT ArmWriteVBar\r
	44	EXPORT ArmEnableVFP\r
	45	EXPORT ArmCallWFI\r
	46	EXPORT ArmReadCbar\r
bd6b9799	47	EXPORT ArmReadMpidr\r
	48	EXPORT ArmReadTpidrurw\r
	49	EXPORT ArmWriteTpidrurw\r
	50	EXPORT ArmIsArchTimerImplemented\r
	51	EXPORT ArmReadIdPfr1\r
	52	\r
	53	AREA ArmV7Support, CODE, READONLY\r
	54	PRESERVE8\r
	55	\r
	56	DC_ON EQU ( 0x1:SHL:2 )\r
	57	IC_ON EQU ( 0x1:SHL:12 )\r
	58	CTRL_M_BIT EQU (1 << 0)\r
	59	CTRL_C_BIT EQU (1 << 2)\r
	60	CTRL_B_BIT EQU (1 << 7)\r
	61	CTRL_I_BIT EQU (1 << 12)\r
	62	\r
	63	\r
	64	ArmInvalidateDataCacheEntryByMVA\r
3402aac7	65	mcr p15, 0, r0, c7, c6, 1 ; invalidate single data cache line\r
bd6b9799	66	dsb\r
	67	isb\r
	68	bx lr\r
	69	\r
	70	ArmCleanDataCacheEntryByMVA\r
3402aac7	71	mcr p15, 0, r0, c7, c10, 1 ; clean single data cache line\r
bd6b9799	72	dsb\r
	73	isb\r
	74	bx lr\r
	75	\r
	76	\r
	77	ArmCleanInvalidateDataCacheEntryByMVA\r
	78	mcr p15, 0, r0, c7, c14, 1 ; clean and invalidate single data cache line\r
	79	dsb\r
	80	isb\r
	81	bx lr\r
	82	\r
	83	\r
	84	ArmInvalidateDataCacheEntryBySetWay\r
3402aac7	85	mcr p15, 0, r0, c7, c6, 2 ; Invalidate this line\r
bd6b9799	86	dsb\r
	87	isb\r
	88	bx lr\r
	89	\r
	90	\r
	91	ArmCleanInvalidateDataCacheEntryBySetWay\r
3402aac7	92	mcr p15, 0, r0, c7, c14, 2 ; Clean and Invalidate this line\r
bd6b9799	93	dsb\r
	94	isb\r
	95	bx lr\r
	96	\r
	97	\r
	98	ArmCleanDataCacheEntryBySetWay\r
3402aac7	99	mcr p15, 0, r0, c7, c10, 2 ; Clean this line\r
bd6b9799	100	dsb\r
	101	isb\r
	102	bx lr\r
	103	\r
	104	\r
	105	ArmInvalidateInstructionCache\r
	106	mcr p15,0,R0,c7,c5,0 ;Invalidate entire instruction cache\r
	107	isb\r
	108	bx LR\r
	109	\r
	110	ArmEnableMmu\r
	111	mrc p15,0,R0,c1,c0,0 ; Read SCTLR into R0 (Read control register configuration data)\r
	112	orr R0,R0,#1 ; Set SCTLR.M bit : Enable MMU\r
	113	mcr p15,0,R0,c1,c0,0 ; Write R0 into SCTLR (Write control register configuration data)\r
	114	dsb\r
	115	isb\r
	116	bx LR\r
	117	\r
	118	ArmDisableMmu\r
	119	mrc p15,0,R0,c1,c0,0 ; Read SCTLR into R0 (Read control register configuration data)\r
	120	bic R0,R0,#1 ; Clear SCTLR.M bit : Disable MMU\r
	121	mcr p15,0,R0,c1,c0,0 ; Write R0 into SCTLR (Write control register configuration data)\r
	122	\r
	123	mcr p15,0,R0,c8,c7,0 ; TLBIALL : Invalidate unified TLB\r
	124	mcr p15,0,R0,c7,c5,6 ; BPIALL : Invalidate entire branch predictor array\r
	125	dsb\r
	126	isb\r
	127	bx LR\r
	128	\r
	129	ArmDisableCachesAndMmu\r
	130	mrc p15, 0, r0, c1, c0, 0 ; Get control register\r
	131	bic r0, r0, #CTRL_M_BIT ; Disable MMU\r
	132	bic r0, r0, #CTRL_C_BIT ; Disable D Cache\r
	133	bic r0, r0, #CTRL_I_BIT ; Disable I Cache\r
	134	mcr p15, 0, r0, c1, c0, 0 ; Write control register\r
	135	dsb\r
	136	isb\r
	137	bx LR\r
	138	\r
	139	ArmMmuEnabled\r
	140	mrc p15,0,R0,c1,c0,0 ; Read SCTLR into R0 (Read control register configuration data)\r
	141	and R0,R0,#1\r
	142	bx LR\r
	143	\r
	144	ArmEnableDataCache\r
	145	ldr R1,=DC_ON ; Specify SCTLR.C bit : (Data) Cache enable bit\r
	146	mrc p15,0,R0,c1,c0,0 ; Read SCTLR into R0 (Read control register configuration data)\r
	147	orr R0,R0,R1 ; Set SCTLR.C bit : Data and unified caches enabled\r
	148	mcr p15,0,R0,c1,c0,0 ; Write R0 into SCTLR (Write control register configuration data)\r
	149	dsb\r
	150	isb\r
	151	bx LR\r
3402aac7	152	\r
bd6b9799	153	ArmDisableDataCache\r
	154	ldr R1,=DC_ON ; Specify SCTLR.C bit : (Data) Cache enable bit\r
	155	mrc p15,0,R0,c1,c0,0 ; Read SCTLR into R0 (Read control register configuration data)\r
	156	bic R0,R0,R1 ; Clear SCTLR.C bit : Data and unified caches disabled\r
	157	mcr p15,0,R0,c1,c0,0 ; Write R0 into SCTLR (Write control register configuration data)\r
	158	dsb\r
	159	isb\r
	160	bx LR\r
	161	\r
	162	ArmEnableInstructionCache\r
	163	ldr R1,=IC_ON ; Specify SCTLR.I bit : Instruction cache enable bit\r
	164	mrc p15,0,R0,c1,c0,0 ; Read SCTLR into R0 (Read control register configuration data)\r
	165	orr R0,R0,R1 ; Set SCTLR.I bit : Instruction caches enabled\r
	166	mcr p15,0,R0,c1,c0,0 ; Write R0 into SCTLR (Write control register configuration data)\r
	167	dsb\r
	168	isb\r
	169	bx LR\r
3402aac7	170	\r
bd6b9799	171	ArmDisableInstructionCache\r
	172	ldr R1,=IC_ON ; Specify SCTLR.I bit : Instruction cache enable bit\r
	173	mrc p15,0,R0,c1,c0,0 ; Read SCTLR into R0 (Read control register configuration data)\r
	174	BIC R0,R0,R1 ; Clear SCTLR.I bit : Instruction caches disabled\r
	175	mcr p15,0,R0,c1,c0,0 ; Write R0 into SCTLR (Write control register configuration data)\r
	176	isb\r
	177	bx LR\r
	178	\r
	179	ArmEnableSWPInstruction\r
	180	mrc p15, 0, r0, c1, c0, 0\r
	181	orr r0, r0, #0x00000400\r
	182	mcr p15, 0, r0, c1, c0, 0\r
	183	isb\r
	184	bx LR\r
	185	\r
	186	ArmEnableBranchPrediction\r
	187	mrc p15, 0, r0, c1, c0, 0 ; Read SCTLR into R0 (Read control register configuration data)\r
	188	orr r0, r0, #0x00000800 ;\r
	189	mcr p15, 0, r0, c1, c0, 0 ; Write R0 into SCTLR (Write control register configuration data)\r
	190	dsb\r
	191	isb\r
	192	bx LR\r
	193	\r
	194	ArmDisableBranchPrediction\r
	195	mrc p15, 0, r0, c1, c0, 0 ; Read SCTLR into R0 (Read control register configuration data)\r
	196	bic r0, r0, #0x00000800 ;\r
	197	mcr p15, 0, r0, c1, c0, 0 ; Write R0 into SCTLR (Write control register configuration data)\r
	198	dsb\r
	199	isb\r
	200	bx LR\r
	201	\r
	202	ArmSetLowVectors\r
	203	mrc p15, 0, r0, c1, c0, 0 ; Read SCTLR into R0 (Read control register configuration data)\r
	204	bic r0, r0, #0x00002000 ; clear V bit\r
	205	mcr p15, 0, r0, c1, c0, 0 ; Write R0 into SCTLR (Write control register configuration data)\r
	206	isb\r
	207	bx LR\r
	208	\r
	209	ArmSetHighVectors\r
	210	mrc p15, 0, r0, c1, c0, 0 ; Read SCTLR into R0 (Read control register configuration data)\r
c6ba1c12	211	orr r0, r0, #0x00002000 ; Set V bit\r
bd6b9799	212	mcr p15, 0, r0, c1, c0, 0 ; Write R0 into SCTLR (Write control register configuration data)\r
	213	isb\r
	214	bx LR\r
	215	\r
	216	ArmV7AllDataCachesOperation\r
	217	stmfd SP!,{r4-r12, LR}\r
	218	mov R1, R0 ; Save Function call in R1\r
	219	mrc p15, 1, R6, c0, c0, 1 ; Read CLIDR\r
	220	ands R3, R6, #&7000000 ; Mask out all but Level of Coherency (LoC)\r
	221	mov R3, R3, LSR #23 ; Cache level value (naturally aligned)\r
	222	beq Finished\r
	223	mov R10, #0\r
	224	\r
3402aac7	225	Loop1\r
bd6b9799	226	add R2, R10, R10, LSR #1 ; Work out 3xcachelevel\r
	227	mov R12, R6, LSR R2 ; bottom 3 bits are the Cache type for this level\r
	228	and R12, R12, #7 ; get those 3 bits alone\r
	229	cmp R12, #2\r
	230	blt Skip ; no cache or only instruction cache at this level\r
	231	mcr p15, 2, R10, c0, c0, 0 ; write the Cache Size selection register (CSSELR) // OR in 1 for Instruction\r
3402aac7	232	isb ; isb to sync the change to the CacheSizeID reg\r
bd6b9799	233	mrc p15, 1, R12, c0, c0, 0 ; reads current Cache Size ID register (CCSIDR)\r
	234	and R2, R12, #&7 ; extract the line length field\r
	235	add R2, R2, #4 ; add 4 for the line length offset (log2 16 bytes)\r
	236	ldr R4, =0x3FF\r
	237	ands R4, R4, R12, LSR #3 ; R4 is the max number on the way size (right aligned)\r
	238	clz R5, R4 ; R5 is the bit position of the way size increment\r
	239	ldr R7, =0x00007FFF\r
	240	ands R7, R7, R12, LSR #13 ; R7 is the max number of the index size (right aligned)\r
	241	\r
3402aac7	242	Loop2\r
bd6b9799	243	mov R9, R4 ; R9 working copy of the max way size (right aligned)\r
bd6b9799	244	\r
3402aac7	245	Loop3\r
bd6b9799	246	orr R0, R10, R9, LSL R5 ; factor in the way number and cache number into R11\r
	247	orr R0, R0, R7, LSL R2 ; factor in the index number\r
	248	\r
	249	blx R1\r
	250	\r
	251	subs R9, R9, #1 ; decrement the way number\r
	252	bge Loop3\r
	253	subs R7, R7, #1 ; decrement the index\r
	254	bge Loop2\r
3402aac7	255	Skip\r
bd6b9799	256	add R10, R10, #2 ; increment the cache number\r
	257	cmp R3, R10\r
	258	bgt Loop1\r
3402aac7	259	\r
bd6b9799	260	Finished\r
	261	dsb\r
	262	ldmfd SP!, {r4-r12, lr}\r
	263	bx LR\r
	264	\r
d60f6af4	265	ArmV7PerformPoUDataCacheOperation\r
	266	stmfd SP!,{r4-r12, LR}\r
	267	mov R1, R0 ; Save Function call in R1\r
	268	mrc p15, 1, R6, c0, c0, 1 ; Read CLIDR\r
	269	ands R3, R6, #&38000000 ; Mask out all but Level of Unification (LoU)\r
	270	mov R3, R3, LSR #26 ; Cache level value (naturally aligned)\r
	271	beq Finished2\r
	272	mov R10, #0\r
	273	\r
3402aac7	274	Loop4\r
d60f6af4	275	add R2, R10, R10, LSR #1 ; Work out 3xcachelevel\r
	276	mov R12, R6, LSR R2 ; bottom 3 bits are the Cache type for this level\r
	277	and R12, R12, #7 ; get those 3 bits alone\r
	278	cmp R12, #2\r
	279	blt Skip2 ; no cache or only instruction cache at this level\r
	280	mcr p15, 2, R10, c0, c0, 0 ; write the Cache Size selection register (CSSELR) // OR in 1 for Instruction\r
3402aac7	281	isb ; isb to sync the change to the CacheSizeID reg\r
d60f6af4	282	mrc p15, 1, R12, c0, c0, 0 ; reads current Cache Size ID register (CCSIDR)\r
	283	and R2, R12, #&7 ; extract the line length field\r
	284	add R2, R2, #4 ; add 4 for the line length offset (log2 16 bytes)\r
	285	ldr R4, =0x3FF\r
	286	ands R4, R4, R12, LSR #3 ; R4 is the max number on the way size (right aligned)\r
	287	clz R5, R4 ; R5 is the bit position of the way size increment\r
	288	ldr R7, =0x00007FFF\r
	289	ands R7, R7, R12, LSR #13 ; R7 is the max number of the index size (right aligned)\r
	290	\r
3402aac7	291	Loop5\r
d60f6af4	292	mov R9, R4 ; R9 working copy of the max way size (right aligned)\r
d60f6af4	293	\r
3402aac7	294	Loop6\r
d60f6af4	295	orr R0, R10, R9, LSL R5 ; factor in the way number and cache number into R11\r
	296	orr R0, R0, R7, LSL R2 ; factor in the index number\r
	297	\r
	298	blx R1\r
	299	\r
	300	subs R9, R9, #1 ; decrement the way number\r
	301	bge Loop6\r
	302	subs R7, R7, #1 ; decrement the index\r
	303	bge Loop5\r
3402aac7	304	Skip2\r
d60f6af4	305	add R10, R10, #2 ; increment the cache number\r
	306	cmp R3, R10\r
	307	bgt Loop4\r
3402aac7	308	\r
d60f6af4	309	Finished2\r
	310	dsb\r
	311	ldmfd SP!, {r4-r12, lr}\r
	312	bx LR\r
	313	\r
bd6b9799	314	ArmDataMemoryBarrier\r
	315	dmb\r
	316	bx LR\r
3402aac7	317	\r
bd6b9799	318	ArmDataSyncronizationBarrier\r
	319	ArmDrainWriteBuffer\r
	320	dsb\r
	321	bx LR\r
3402aac7	322	\r
bd6b9799	323	ArmInstructionSynchronizationBarrier\r
	324	isb\r
	325	bx LR\r
	326	\r
836c3500	327	ArmReadVBar\r
	328	// Set the Address of the Vector Table in the VBAR register\r
	329	mrc p15, 0, r0, c12, c0, 0\r
	330	bx lr\r
	331	\r
bd6b9799	332	ArmWriteVBar\r
bd6b9799	333	// Set the Address of the Vector Table in the VBAR register\r
3402aac7	334	mcr p15, 0, r0, c12, c0, 0\r
bd6b9799	335	// Ensure the SCTLR.V bit is clear\r
	336	mrc p15, 0, r0, c1, c0, 0 ; Read SCTLR into R0 (Read control register configuration data)\r
	337	bic r0, r0, #0x00002000 ; clear V bit\r
	338	mcr p15, 0, r0, c1, c0, 0 ; Write R0 into SCTLR (Write control register configuration data)\r
	339	isb\r
	340	bx lr\r
	341	\r
	342	ArmEnableVFP\r
	343	// Read CPACR (Coprocessor Access Control Register)\r
	344	mrc p15, 0, r0, c1, c0, 2\r
	345	// Enable VPF access (Full Access to CP10, CP11) (V* instructions)\r
	346	orr r0, r0, #0x00f00000\r
	347	// Write back CPACR (Coprocessor Access Control Register)\r
	348	mcr p15, 0, r0, c1, c0, 2\r
18029bb9	349	isb\r
bd6b9799	350	// Set EN bit in FPEXC. The Advanced SIMD and VFP extensions are enabled and operate normally.\r
	351	mov r0, #0x40000000\r
	352	mcr p10,#0x7,r0,c8,c0,#0\r
	353	bx lr\r
	354	\r
	355	ArmCallWFI\r
	356	wfi\r
	357	bx lr\r
	358	\r
	359	//Note: Return 0 in Uniprocessor implementation\r
	360	ArmReadCbar\r
	361	mrc p15, 4, r0, c15, c0, 0 //Read Configuration Base Address Register\r
	362	bx lr\r
	363	\r
bd6b9799	364	ArmReadMpidr\r
	365	mrc p15, 0, r0, c0, c0, 5 ; read MPIDR\r
	366	bx lr\r
	367	\r
	368	ArmReadTpidrurw\r
	369	mrc p15, 0, r0, c13, c0, 2 ; read TPIDRURW\r
	370	bx lr\r
	371	\r
	372	ArmWriteTpidrurw\r
	373	mcr p15, 0, r0, c13, c0, 2 ; write TPIDRURW\r
	374	bx lr\r
	375	\r
	376	ArmIsArchTimerImplemented\r
	377	mrc p15, 0, r0, c0, c1, 1 ; Read ID_PFR1\r
	378	and r0, r0, #0x000F0000\r
	379	bx lr\r
	380	\r
	381	ArmReadIdPfr1\r
	382	mrc p15, 0, r0, c0, c1, 1 ; Read ID_PFR1 Register\r
	383	bx lr\r
	384	\r
	385	END\r