479 lines
13 KiB
ArmAsm
479 lines
13 KiB
ArmAsm
@-------------------------------------------------------------------------------
|
|
@ sys_core.asm
|
|
@
|
|
@ (c) Texas Instruments 2009-2013, All rights reserved.
|
|
@
|
|
|
|
#include <rtconfig.h>
|
|
|
|
.equ Mode_USR, 0x10
|
|
.equ Mode_FIQ, 0x11
|
|
.equ Mode_IRQ, 0x12
|
|
.equ Mode_SVC, 0x13
|
|
.equ Mode_ABT, 0x17
|
|
.equ Mode_UND, 0x1B
|
|
.equ Mode_SYS, 0x1F
|
|
|
|
.equ I_Bit, 0x80 @ when I bit is set, IRQ is disabled
|
|
.equ F_Bit, 0x40 @ when F bit is set, FIQ is disabled
|
|
|
|
.equ UND_Stack_Size, 0x00000000
|
|
.equ SVC_Stack_Size, 0x00000000
|
|
.equ ABT_Stack_Size, 0x00000000
|
|
.equ FIQ_Stack_Size, 0x00001000
|
|
.equ IRQ_Stack_Size, 0x00001000
|
|
|
|
.section .bss.noinit
|
|
/* stack */
|
|
.globl stack_start
|
|
.globl stack_top
|
|
|
|
stack_start:
|
|
.rept (UND_Stack_Size + SVC_Stack_Size + ABT_Stack_Size + FIQ_Stack_Size + IRQ_Stack_Size)
|
|
.byte 0
|
|
.endr
|
|
stack_top:
|
|
|
|
.section .text, "ax"
|
|
.text
|
|
.arm
|
|
|
|
.globl _c_int00
|
|
|
|
.globl _reset
|
|
_reset:
|
|
@-------------------------------------------------------------------------------
|
|
@ Initialize CPU Registers
|
|
@ After reset, the CPU is in the Supervisor mode (M = 10011)
|
|
cpsid if, #19
|
|
|
|
#if defined (__VFP_FP__) && !defined(__SOFTFP__) && defined(RT_VFP_LAZY_STACKING)
|
|
@ Turn on FPV coprocessor
|
|
mrc p15, #0x00, r2, c1, c0, #0x02
|
|
orr r2, r2, #0xF00000
|
|
mcr p15, #0x00, r2, c1, c0, #0x02
|
|
|
|
fmrx r2, fpexc
|
|
orr r2, r2, #0x40000000
|
|
fmxr fpexc, r2
|
|
#endif
|
|
|
|
@-------------------------------------------------------------------------------
|
|
@ Initialize Stack Pointers
|
|
ldr r0, =stack_top
|
|
|
|
@ Set the startup stack for svc
|
|
mov sp, r0
|
|
|
|
@ Enter Undefined Instruction Mode and set its Stack Pointer
|
|
msr cpsr_c, #Mode_UND|I_Bit|F_Bit
|
|
mov sp, r0
|
|
sub r0, r0, #UND_Stack_Size
|
|
|
|
@ Enter Abort Mode and set its Stack Pointer
|
|
msr cpsr_c, #Mode_ABT|I_Bit|F_Bit
|
|
mov sp, r0
|
|
sub r0, r0, #ABT_Stack_Size
|
|
|
|
@ Enter FIQ Mode and set its Stack Pointer
|
|
msr cpsr_c, #Mode_FIQ|I_Bit|F_Bit
|
|
mov sp, r0
|
|
sub r0, r0, #FIQ_Stack_Size
|
|
|
|
@ Enter IRQ Mode and set its Stack Pointer
|
|
msr cpsr_c, #Mode_IRQ|I_Bit|F_Bit
|
|
mov sp, r0
|
|
sub r0, r0, #IRQ_Stack_Size
|
|
|
|
@ Switch back to SVC
|
|
msr cpsr_c, #Mode_SVC|I_Bit|F_Bit
|
|
|
|
bl next1
|
|
next1:
|
|
bl next2
|
|
next2:
|
|
bl next3
|
|
next3:
|
|
bl next4
|
|
next4:
|
|
ldr lr, =_c_int00
|
|
bx lr
|
|
|
|
.globl data_init
|
|
data_init:
|
|
/* copy .data to SRAM */
|
|
ldr r1, =_sidata /* .data start in image */
|
|
ldr r2, =_edata /* .data end in image */
|
|
ldr r3, =_sdata /* sram data start */
|
|
data_loop:
|
|
ldr r0, [r1, #0]
|
|
str r0, [r3]
|
|
|
|
add r1, r1, #4
|
|
add r3, r3, #4
|
|
|
|
cmp r3, r2 /* check if data to clear */
|
|
blo data_loop /* loop until done */
|
|
|
|
/* clear .bss */
|
|
mov r0,#0 /* get a zero */
|
|
ldr r1,=__bss_start /* bss start */
|
|
ldr r2,=__bss_end /* bss end */
|
|
|
|
bss_loop:
|
|
cmp r1,r2 /* check if data to clear */
|
|
strlo r0,[r1],#4 /* clear 4 bytes */
|
|
blo bss_loop /* loop until done */
|
|
|
|
/* call C++ constructors of global objects */
|
|
ldr r0, =__ctors_start__
|
|
ldr r1, =__ctors_end__
|
|
|
|
ctor_loop:
|
|
cmp r0, r1
|
|
beq ctor_end
|
|
ldr r2, [r0], #4
|
|
stmfd sp!, {r0-r3, ip, lr}
|
|
mov lr, pc
|
|
bx r2
|
|
ldmfd sp!, {r0-r3, ip, lr}
|
|
b ctor_loop
|
|
ctor_end:
|
|
bx lr
|
|
|
|
@-------------------------------------------------------------------------------
|
|
@ Enable RAM ECC Support
|
|
|
|
.globl _coreEnableRamEcc_
|
|
_coreEnableRamEcc_:
|
|
|
|
stmfd sp!, {r0}
|
|
mrc p15, #0x00, r0, c1, c0, #0x01
|
|
orr r0, r0, #0x0C000000
|
|
mcr p15, #0x00, r0, c1, c0, #0x01
|
|
ldmfd sp!, {r0}
|
|
bx lr
|
|
|
|
@-------------------------------------------------------------------------------
|
|
@ Disable RAM ECC Support
|
|
|
|
.globl _coreDisableRamEcc_
|
|
_coreDisableRamEcc_:
|
|
|
|
stmfd sp!, {r0}
|
|
mrc p15, #0x00, r0, c1, c0, #0x01
|
|
bic r0, r0, #0x0C000000
|
|
mcr p15, #0x00, r0, c1, c0, #0x01
|
|
ldmfd sp!, {r0}
|
|
bx lr
|
|
|
|
|
|
@-------------------------------------------------------------------------------
|
|
@ Enable Flash ECC Support
|
|
|
|
.globl _coreEnableFlashEcc_
|
|
_coreEnableFlashEcc_:
|
|
|
|
stmfd sp!, {r0}
|
|
mrc p15, #0x00, r0, c1, c0, #0x01
|
|
orr r0, r0, #0x02000000
|
|
dmb
|
|
mcr p15, #0x00, r0, c1, c0, #0x01
|
|
ldmfd sp!, {r0}
|
|
bx lr
|
|
|
|
@-------------------------------------------------------------------------------
|
|
@ Disable Flash ECC Support
|
|
|
|
.globl _coreDisableFlashEcc_
|
|
_coreDisableFlashEcc_:
|
|
|
|
stmfd sp!, {r0}
|
|
mrc p15, #0x00, r0, c1, c0, #0x01
|
|
bic r0, r0, #0x02000000
|
|
mcr p15, #0x00, r0, c1, c0, #0x01
|
|
ldmfd sp!, {r0}
|
|
bx lr
|
|
|
|
|
|
@-------------------------------------------------------------------------------
|
|
@ Get data fault status register
|
|
|
|
.globl _coreGetDataFault_
|
|
_coreGetDataFault_:
|
|
|
|
mrc p15, #0, r0, c5, c0, #0
|
|
bx lr
|
|
|
|
|
|
|
|
@-------------------------------------------------------------------------------
|
|
@ Clear data fault status register
|
|
|
|
.globl _coreClearDataFault_
|
|
_coreClearDataFault_:
|
|
|
|
stmfd sp!, {r0}
|
|
mov r0, #0
|
|
mcr p15, #0, r0, c5, c0, #0
|
|
ldmfd sp!, {r0}
|
|
bx lr
|
|
|
|
|
|
|
|
@-------------------------------------------------------------------------------
|
|
@ Get instruction fault status register
|
|
|
|
.globl _coreGetInstructionFault_
|
|
_coreGetInstructionFault_:
|
|
|
|
mrc p15, #0, r0, c5, c0, #1
|
|
bx lr
|
|
|
|
|
|
|
|
@-------------------------------------------------------------------------------
|
|
@ Clear instruction fault status register
|
|
|
|
.globl _coreClearInstructionFault_
|
|
_coreClearInstructionFault_:
|
|
|
|
stmfd sp!, {r0}
|
|
mov r0, #0
|
|
mcr p15, #0, r0, c5, c0, #1
|
|
ldmfd sp!, {r0}
|
|
bx lr
|
|
|
|
|
|
|
|
@-------------------------------------------------------------------------------
|
|
@ Get data fault address register
|
|
|
|
.globl _coreGetDataFaultAddress_
|
|
_coreGetDataFaultAddress_:
|
|
|
|
mrc p15, #0, r0, c6, c0, #0
|
|
bx lr
|
|
|
|
|
|
|
|
@-------------------------------------------------------------------------------
|
|
@ Clear data fault address register
|
|
|
|
.globl _coreClearDataFaultAddress_
|
|
_coreClearDataFaultAddress_:
|
|
|
|
stmfd sp!, {r0}
|
|
mov r0, #0
|
|
mcr p15, #0, r0, c6, c0, #0
|
|
ldmfd sp!, {r0}
|
|
bx lr
|
|
|
|
|
|
|
|
@-------------------------------------------------------------------------------
|
|
@ Get instruction fault address register
|
|
|
|
.globl _coreGetInstructionFaultAddress_
|
|
_coreGetInstructionFaultAddress_:
|
|
|
|
mrc p15, #0, r0, c6, c0, #2
|
|
bx lr
|
|
|
|
|
|
|
|
@-------------------------------------------------------------------------------
|
|
@ Clear instruction fault address register
|
|
|
|
.globl _coreClearInstructionFaultAddress_
|
|
_coreClearInstructionFaultAddress_:
|
|
|
|
stmfd sp!, {r0}
|
|
mov r0, #0
|
|
mcr p15, #0, r0, c6, c0, #2
|
|
ldmfd sp!, {r0}
|
|
bx lr
|
|
|
|
|
|
|
|
@-------------------------------------------------------------------------------
|
|
@ Get auxiliary data fault status register
|
|
|
|
.globl _coreGetAuxiliaryDataFault_
|
|
_coreGetAuxiliaryDataFault_:
|
|
|
|
mrc p15, #0, r0, c5, c1, #0
|
|
bx lr
|
|
|
|
|
|
|
|
@-------------------------------------------------------------------------------
|
|
@ Clear auxiliary data fault status register
|
|
|
|
.globl _coreClearAuxiliaryDataFault_
|
|
_coreClearAuxiliaryDataFault_:
|
|
|
|
stmfd sp!, {r0}
|
|
mov r0, #0
|
|
mcr p15, #0, r0, c5, c1, #0
|
|
ldmfd sp!, {r0}
|
|
bx lr
|
|
|
|
|
|
|
|
@-------------------------------------------------------------------------------
|
|
@ Get auxiliary instruction fault status register
|
|
|
|
.globl _coreGetAuxiliaryInstructionFault_
|
|
_coreGetAuxiliaryInstructionFault_:
|
|
|
|
mrc p15, #0, r0, c5, c1, #1
|
|
bx lr
|
|
|
|
|
|
@-------------------------------------------------------------------------------
|
|
@ Clear auxiliary instruction fault status register
|
|
|
|
.globl _coreClearAuxiliaryInstructionFault_
|
|
_coreClearAuxiliaryInstructionFault_:
|
|
|
|
stmfd sp!, {r0}
|
|
mov r0, #0
|
|
mrc p15, #0, r0, c5, c1, #1
|
|
ldmfd sp!, {r0}
|
|
bx lr
|
|
|
|
|
|
@-------------------------------------------------------------------------------
|
|
@ Clear ESM CCM errorss
|
|
|
|
.globl _esmCcmErrorsClear_
|
|
_esmCcmErrorsClear_:
|
|
|
|
stmfd sp!, {r0-r2}
|
|
ldr r0, ESMSR1_REG @ load the ESMSR1 status register address
|
|
ldr r2, ESMSR1_ERR_CLR
|
|
str r2, [r0] @ clear the ESMSR1 register
|
|
|
|
ldr r0, ESMSR2_REG @ load the ESMSR2 status register address
|
|
ldr r2, ESMSR2_ERR_CLR
|
|
str r2, [r0] @ clear the ESMSR2 register
|
|
|
|
ldr r0, ESMSSR2_REG @ load the ESMSSR2 status register address
|
|
ldr r2, ESMSSR2_ERR_CLR
|
|
str r2, [r0] @ clear the ESMSSR2 register
|
|
|
|
ldr r0, ESMKEY_REG @ load the ESMKEY register address
|
|
mov r2, #0x5 @ load R2 with 0x5
|
|
str r2, [r0] @ clear the ESMKEY register
|
|
|
|
ldr r0, VIM_INTREQ @ load the INTREQ register address
|
|
ldr r2, VIM_INT_CLR
|
|
str r2, [r0] @ clear the INTREQ register
|
|
ldr r0, CCMR4_STAT_REG @ load the CCMR4 status register address
|
|
ldr r2, CCMR4_ERR_CLR
|
|
str r2, [r0] @ clear the CCMR4 status register
|
|
ldmfd sp!, {r0-r2}
|
|
bx lr
|
|
|
|
ESMSR1_REG: .word 0xFFFFF518
|
|
ESMSR2_REG: .word 0xFFFFF51C
|
|
ESMSR3_REG: .word 0xFFFFF520
|
|
ESMKEY_REG: .word 0xFFFFF538
|
|
ESMSSR2_REG: .word 0xFFFFF53C
|
|
CCMR4_STAT_REG: .word 0xFFFFF600
|
|
ERR_CLR_WRD: .word 0xFFFFFFFF
|
|
CCMR4_ERR_CLR: .word 0x00010000
|
|
ESMSR1_ERR_CLR: .word 0x80000000
|
|
ESMSR2_ERR_CLR: .word 0x00000004
|
|
ESMSSR2_ERR_CLR: .word 0x00000004
|
|
VIM_INT_CLR: .word 0x00000001
|
|
VIM_INTREQ: .word 0xFFFFFE20
|
|
|
|
|
|
@-------------------------------------------------------------------------------
|
|
@ Work Around for Errata CORTEX-R4#57:
|
|
@
|
|
@ Errata Description:
|
|
@ Conditional VMRS APSR_Nzcv, FPSCR May Evaluate With Incorrect Flags
|
|
@ Workaround:
|
|
@ Disable out-of-order single-precision floating point
|
|
@ multiply-accumulate instruction completion
|
|
|
|
.globl _errata_CORTEXR4_57_
|
|
_errata_CORTEXR4_57_:
|
|
|
|
push {r0}
|
|
mrc p15, #0, r0, c15, c0, #0 @ Read Secondary Auxiliary Control Register
|
|
orr r0, r0, #0x10000 @ Set BIT 16 (Set DOOFMACS)
|
|
mcr p15, #0, r0, c15, c0, #0 @ Write Secondary Auxiliary Control Register
|
|
pop {r0}
|
|
bx lr
|
|
|
|
@-------------------------------------------------------------------------------
|
|
@ Work Around for Errata CORTEX-R4#66:
|
|
@
|
|
@ Errata Description:
|
|
@ Register Corruption During A Load-Multiple Instruction At
|
|
@ an Exception Vector
|
|
@ Workaround:
|
|
@ Disable out-of-order completion for divide instructions in
|
|
@ Auxiliary Control register
|
|
|
|
.globl _errata_CORTEXR4_66_
|
|
_errata_CORTEXR4_66_:
|
|
|
|
push {r0}
|
|
mrc p15, #0, r0, c1, c0, #1 @ Read Auxiliary Control register
|
|
orr r0, r0, #0x80 @ Set BIT 7 (Disable out-of-order completion
|
|
@ for divide instructions.)
|
|
mcr p15, #0, r0, c1, c0, #1 @ Write Auxiliary Control register
|
|
pop {r0}
|
|
bx lr
|
|
|
|
.globl turnon_VFP
|
|
turnon_VFP:
|
|
@ Enable FPV
|
|
STMDB sp!, {r0}
|
|
fmrx r0, fpexc
|
|
orr r0, r0, #0x40000000
|
|
fmxr fpexc, r0
|
|
LDMIA sp!, {r0}
|
|
subs pc, lr, #4
|
|
|
|
.macro push_svc_reg
|
|
sub sp, sp, #17 * 4 @/* Sizeof(struct rt_hw_exp_stack) */
|
|
stmia sp, {r0 - r12} @/* Calling r0-r12 */
|
|
mov r0, sp
|
|
mrs r6, spsr @/* Save CPSR */
|
|
str lr, [r0, #15*4] @/* Push PC */
|
|
str r6, [r0, #16*4] @/* Push CPSR */
|
|
cps #Mode_SVC
|
|
str sp, [r0, #13*4] @/* Save calling SP */
|
|
str lr, [r0, #14*4] @/* Save calling PC */
|
|
.endm
|
|
|
|
.globl vector_svc
|
|
vector_svc:
|
|
push_svc_reg
|
|
bl rt_hw_trap_svc
|
|
b .
|
|
|
|
.globl vector_pabort
|
|
vector_pabort:
|
|
push_svc_reg
|
|
bl rt_hw_trap_pabt
|
|
b .
|
|
|
|
.globl vector_dabort
|
|
vector_dabort:
|
|
push_svc_reg
|
|
bl rt_hw_trap_dabt
|
|
b .
|
|
|
|
.globl vector_resv
|
|
vector_resv:
|
|
push_svc_reg
|
|
bl rt_hw_trap_resv
|
|
b .
|