239 lines
9.6 KiB
ArmAsm
239 lines
9.6 KiB
ArmAsm
|
|
MODULE ?cstartup
|
|
|
|
;; Forward declaration of sections.
|
|
SECTION IRQ_STACK:DATA:NOROOT(3)
|
|
SECTION FIQ_STACK:DATA:NOROOT(3)
|
|
SECTION SVC_STACK:DATA:NOROOT(3)
|
|
SECTION ABT_STACK:DATA:NOROOT(3)
|
|
SECTION UND_STACK:DATA:NOROOT(3)
|
|
SECTION CSTACK:DATA:NOROOT(3)
|
|
|
|
//------------------------------------------------------------------------------
|
|
// Headers
|
|
//------------------------------------------------------------------------------
|
|
|
|
#define __ASSEMBLY__
|
|
|
|
//------------------------------------------------------------------------------
|
|
// Definitions
|
|
//------------------------------------------------------------------------------
|
|
#define IRAM_BASE 0x300000
|
|
#define SYS_CPU_CTL 0xe4900208
|
|
#define AIC 0xFFFFF000
|
|
#define AIC_IVR 0x10
|
|
#define AIC_EOICR 0x38
|
|
|
|
#define ARM_MODE_ABT 0x17
|
|
#define ARM_MODE_FIQ 0x11
|
|
#define ARM_MODE_IRQ 0x12
|
|
#define ARM_MODE_SVC 0x13
|
|
#define ARM_MODE_SYS 0x1F
|
|
|
|
#define I_BIT 0x80
|
|
#define F_BIT 0x40
|
|
|
|
//------------------------------------------------------------------------------
|
|
// Startup routine
|
|
//------------------------------------------------------------------------------
|
|
|
|
/*
|
|
Exception vectors
|
|
*/
|
|
// SECTION .vectors:CODE:NOROOT(2)
|
|
SECTION .intvec:CODE:NOROOT(2)
|
|
PUBLIC __vector
|
|
PUBLIC __iar_program_start
|
|
|
|
ARM ; Always ARM mode after reset
|
|
|
|
__vector:
|
|
ldr pc, Reset
|
|
DCD 0x424b5244
|
|
//LDR PC, Undefined_Addr
|
|
LDR PC, SWI_Addr
|
|
LDR PC, Prefetch_Addr
|
|
LDR PC, Abort_Addr
|
|
NOP ; Reserved vector
|
|
LDR PC, IRQ_Addr
|
|
LDR PC, FIQ_Addr
|
|
|
|
IMPORT undef_handler
|
|
IMPORT swi_handler
|
|
IMPORT prefetch_handler
|
|
IMPORT data_abort_handler
|
|
IMPORT irq_handler
|
|
IMPORT fiq_handler
|
|
|
|
Reset: dc32 __iar_program_start
|
|
Undefined_Addr: dc32 undef_handler ;Undefined_Handler
|
|
SWI_Addr: dc32 swi_handler ;SWI_Handler
|
|
Prefetch_Addr: dc32 prefetch_handler ;ExceptionPAB
|
|
Abort_Addr: dc32 data_abort_handler ;ExceptionDAB
|
|
Reserved_Addr: dc32 0 ;ExceptionREV
|
|
IRQ_Addr: dc32 irq_handler
|
|
FIQ_Addr: dc32 fiq_handler
|
|
|
|
MODE_MSK DEFINE 0x1F ; Bit mask for mode bits in CPSR
|
|
|
|
USR_MODE DEFINE 0x10 ; User mode
|
|
FIQ_MODE DEFINE 0x11 ; Fast Interrupt Request mode
|
|
IRQ_MODE DEFINE 0x12 ; Interrupt Request mode
|
|
SVC_MODE DEFINE 0x13 ; Supervisor mode
|
|
ABT_MODE DEFINE 0x17 ; Abort mode
|
|
UND_MODE DEFINE 0x1B ; Undefined Instruction mode
|
|
SYS_MODE DEFINE 0x1F ; System mode
|
|
|
|
CP_DIS_MASK DEFINE 0xFFFFEFFA
|
|
|
|
SECTION .text:CODE:NOROOT(2)
|
|
EXTERN ?main
|
|
REQUIRE __vector
|
|
|
|
__iar_program_start:
|
|
b reset_handler
|
|
// DCD 0x424b5241
|
|
// DCD 0
|
|
// DCD 0
|
|
|
|
reset_handler:
|
|
;==================================================================
|
|
; Reset registers
|
|
;==================================================================
|
|
MOV r2, #0
|
|
MOV r3, #0
|
|
MOV r4, #0
|
|
MOV r5, #0
|
|
MOV r6, #0
|
|
MOV r7, #0
|
|
MOV r8, #0
|
|
MOV r9, #0
|
|
MOV r10, #0
|
|
MOV r11, #0
|
|
MOV r12, #0
|
|
|
|
;==================================================================
|
|
; Disable caches, MMU and branch prediction in case they were left enabled from an earlier run
|
|
; This does not need to be done from a cold reset
|
|
;==================================================================
|
|
MRC p15, 0, r0, c1, c0, 0 ; Read CP15 System Control register
|
|
BIC r0, r0, #(0x1 << 12) ; Clear I bit 12 to disable I Cache
|
|
;ORR r0, r0, #(0x1 << 12) ; Set I bit 12 to enable I Cache
|
|
BIC r0, r0, #(0x1 << 2) ; Clear C bit 2 to disable D Cache
|
|
BIC r0, r0, #0x1 ; Clear M bit 0 to disable MMU
|
|
BIC r0, r0, #(0x1 << 11) ; Clear Z bit 11 to disable branch prediction
|
|
MCR p15, 0, r0, c1, c0, 0 ; Write value back to CP15 System Control register
|
|
|
|
;==================================================================
|
|
; Cache Invalidation code for Cortex-A7
|
|
; NOTE: Neither Caches, nor MMU, nor BTB need post-reset invalidation on Cortex-A7,
|
|
; but forcing a cache invalidation, makes the code more portable to other CPUs (e.g. Cortex-A9)
|
|
;==================================================================
|
|
; Invalidate L1 Instruction Cache
|
|
MRC p15, 1, r0, c0, c0, 1 ; Read Cache Level ID Register (CLIDR)
|
|
TST r0, #0x3 ; Harvard Cache?
|
|
MOV r0, #0 ; SBZ
|
|
MCRNE p15, 0, r0, c7, c5, 0 ; ICIALLU - Invalidate instruction cache and flush branch target cache
|
|
|
|
; Invalidate Data/Unified Caches
|
|
|
|
MRC p15, 1, r0, c0, c0, 1 ; Read CLIDR
|
|
ANDS r3, r0, #0x07000000 ; Extract coherency level
|
|
MOV r3, r3, LSR #23 ; Total cache levels << 1
|
|
BEQ Finished ; If 0, no need to clean
|
|
|
|
MOV r10, #0 ; R10 holds current cache level << 1
|
|
Loop1 ADD r2, r10, r10, LSR #1 ; R2 holds cache "Set" position
|
|
MOV r1, r0, LSR r2 ; Bottom 3 bits are the Cache-type for this level
|
|
AND r1, r1, #7 ; Isolate those lower 3 bits
|
|
CMP r1, #2
|
|
BLT Skip ; No cache or only instruction cache at this level
|
|
|
|
MCR p15, 2, r10, c0, c0, 0 ; Write the Cache Size selection register
|
|
ISB ; ISB to sync the change to the CacheSizeID reg
|
|
MRC p15, 1, r1, c0, c0, 0 ; Reads current Cache Size ID register
|
|
AND r2, r1, #7 ; Extract the line length field
|
|
ADD r2, r2, #4 ; Add 4 for the line length offset (log2 16 bytes)
|
|
LDR r4, =0x3FF
|
|
ANDS r4, r4, r1, LSR #3 ; R4 is the max number on the way size (right aligned)
|
|
CLZ r5, r4 ; R5 is the bit position of the way size increment
|
|
LDR r7, =0x7FFF
|
|
ANDS r7, r7, r1, LSR #13 ; R7 is the max number of the index size (right aligned)
|
|
|
|
Loop2 MOV r9, r4 ; R9 working copy of the max way size (right aligned)
|
|
|
|
Loop3 ORR r11, r10, r9, LSL r5 ; Factor in the Way number and cache number into R11
|
|
ORR r11, r11, r7, LSL r2 ; Factor in the Set number
|
|
MCR p15, 0, r11, c7, c6, 2 ; Invalidate by Set/Way
|
|
SUBS r9, r9, #1 ; Decrement the Way number
|
|
BGE Loop3
|
|
SUBS r7, r7, #1 ; Decrement the Set number
|
|
BGE Loop2
|
|
Skip ADD r10, r10, #2 ; increment the cache number
|
|
CMP r3, r10
|
|
BGT Loop1
|
|
|
|
Finished
|
|
|
|
;==================================================================
|
|
; Invalidate TLB
|
|
;==================================================================
|
|
MOV r0, #0
|
|
MCR p15, 0, r0, c8, c7, 0
|
|
|
|
;==================================================================
|
|
; Branch Prediction Enable
|
|
;==================================================================
|
|
;MOV r1, #0
|
|
;MRC p15, 0, r1, c1, c0, 0 /* Read Control Register configuration data */
|
|
;ORR r1, r1, #(0x1 << 11) /* Global BP Enable bit */
|
|
;MCR p15, 0, r1, c1, c0, 0 /* Write Control Register configuration data */
|
|
|
|
; Initialize the stack pointers.
|
|
; The pattern below can be used for any of the exception stacks:
|
|
; FIQ, IRQ, SVC, ABT, UND, SYS.
|
|
; The USR mode uses the same stack as SYS.
|
|
; The stack segments must be defined in the linker command file,
|
|
; and be declared above.
|
|
mrs r0,cpsr ; Original PSR value
|
|
bic r0,r0,#MODE_MSK ; Clear the mode bits
|
|
orr r0,r0,#SVC_MODE ; Set Supervisor mode bits
|
|
msr cpsr_c,r0 ; Change the mode
|
|
ldr sp,=SFE(SVC_STACK) ; End of SVC_STACK
|
|
|
|
bic r0,r0,#MODE_MSK ; Clear the mode bits
|
|
orr r0,r0,#ABT_MODE ; Set Abort mode bits
|
|
msr cpsr_c,r0 ; Change the mode
|
|
ldr sp,=SFE(ABT_STACK) ; End of ABT_STACK
|
|
|
|
bic r0,r0,#MODE_MSK ; Clear the mode bits
|
|
orr r0,r0,#UND_MODE ; Set Undefined mode bits
|
|
msr cpsr_c,r0 ; Change the mode
|
|
ldr sp,=SFE(UND_STACK) ; End of UND_STACK
|
|
|
|
bic r0,r0,#MODE_MSK ; Clear the mode bits
|
|
orr r0,r0,#FIQ_MODE ; Set FIR mode bits
|
|
msr cpsr_c,r0 ; Change the mode
|
|
ldr sp,=SFE(FIQ_STACK) ; End of FIQ_STACK
|
|
|
|
bic r0,r0,#MODE_MSK ; Clear the mode bits
|
|
orr r0,r0,#IRQ_MODE ; Set IRQ mode bits
|
|
msr cpsr_c,r0 ; Change the mode
|
|
ldr sp,=SFE(IRQ_STACK) ; End of IRQ_STACK
|
|
|
|
bic r0,r0,#MODE_MSK ; Clear the mode bits
|
|
orr r0,r0,#SYS_MODE ; Set System mode bits
|
|
msr cpsr_c,r0 ; Change the mode
|
|
ldr sp,=SFE(CSTACK) ; End of CSTACK
|
|
|
|
/* Branch to main() */
|
|
LDR r0, =?main
|
|
BLX r0
|
|
|
|
/* Loop indefinitely when program is finished */
|
|
loop4:
|
|
B loop4
|
|
|
|
|
|
END
|
|
|