;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; ;; ;; Part one of the system initialization code, ;; contains low-level ;; initialization. ;; ;; Copyright 2007 IAR Systems. All rights reserved. ;; ;; $Revision: 49919 $ ;; MODULE ?cstartup ;; Forward declaration of sections. SECTION SVC_STACK:DATA:NOROOT(3) SECTION IRQ_STACK:DATA:NOROOT(3) SECTION ABT_STACK:DATA:NOROOT(3) SECTION FIQ_STACK:DATA:NOROOT(3) SECTION UND_STACK:DATA:NOROOT(3) SECTION CSTACK:DATA:NOROOT(3) ; ; The module in this file are included in the libraries, and may be ; replaced by any user-defined modules that define the PUBLIC symbol ; __iar_program_start or a user defined start symbol. ; ; To override the cstartup defined in the library, simply add your ; modified version to the workbench project. SECTION .intvec:CODE:NOROOT(2) PUBLIC __vector_core_a9 PUBWEAK __iar_program_start PUBLIC Undefined_Handler EXTERN SWI_Handler PUBLIC Prefetch_Handler PUBLIC Abort_Handler PUBLIC IRQ_Handler PUBLIC FIQ_Handler EXTERN VbarInit EXTERN SetLowVectors EXTERN init_TTB EXTERN enable_mmu EXTERN Peripheral_BasicInit EXTERN initsct EXTERN PowerON_Reset PUBLIC FPUEnable DATA __iar_init$$done: ; The vector table is not needed ; until after copy initialization is done __vector_core_a9: ; Make this a DATA label, so that stack usage ; analysis doesn't consider it an uncalled fun ARM ; All default exception handlers (except reset) are ; defined as weak symbol definitions. ; If a handler is defined by the application it will take precedence. LDR PC,Reset_Addr ; Reset LDR PC,Undefined_Addr ; Undefined instructions LDR PC,SWI_Addr ; Software interrupt (SWI/SVC) LDR PC,Prefetch_Addr ; Prefetch abort LDR PC,Abort_Addr ; Data abort DCD 0 ; RESERVED LDR PC,IRQ_Addr ; IRQ LDR PC,FIQ_Addr ; FIQ DATA Reset_Addr: DCD __iar_program_start Undefined_Addr: DCD Undefined_Handler SWI_Addr: DCD SWI_Handler Prefetch_Addr: DCD Prefetch_Handler Abort_Addr: DCD Abort_Handler IRQ_Addr: DCD IRQ_Handler FIQ_Addr: DCD FIQ_Handler ; -------------------------------------------------- ; ?cstartup -- low-level system initialization code. ; ; After a reset execution starts here, the mode is ARM, supervisor ; with interrupts disabled. ; SECTION .text:CODE:NOROOT(2) EXTERN RZ_A1_SetSramWriteEnable EXTERN create_translation_table EXTERN SystemInit EXTERN InitMemorySubsystem EXTERN __cmain REQUIRE __vector_core_a9 EXTWEAK __iar_init_core EXTWEAK __iar_init_vfp ARM __iar_program_start: ?cstartup: ;;; @ Put any cores other than 0 to sleep mrc p15, 0, r0, c0, c0, 5 ;;; @ Read MPIDR ands r0, r0, #3 goToSleep: wfine bne goToSleep //@ Enable access to NEON/VFP by enabling access to Coprocessors 10 and 11. //@ Enables Full Access i.e. in both privileged and non privileged modes mrc p15, 0, r0, c1, c0, 2 ;@ Read Coprocessor Access Control Register (CPACR) orr r0, r0, #(0xF << 20) ;@ Enable access to CP 10 & 11 mcr p15, 0, r0, c1, c0, 2 ;@ Write Coprocessor Access Control Register (CPACR) isb ;; Switch on the VFP and NEON hardware mov r0, #0x40000000 vmsr fpexc, r0 ;@ Write FPEXC register, EN bit set 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 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 bic r0, r0, #(0x1 << 13) ;@ Clear V bit 13 to disable hivecs mcr p15, 0, r0, c1, c0, 0 ;@ Write value back to CP15 System Control register isb ;; Set Vector Base Address Register (VBAR) to point to this application's vector table ldr r0, =__vector_core_a9 mcr p15, 0, r0, c12, c0, 0 ; ; Add initialization needed before setup of stackpointers here. ; ; ; 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. ; ; -------------------- ; Mode, correspords to bits 0-5 in CPSR #define MODE_MSK 0x1F ; Bit mask for mode bits in CPSR #define USR_MODE 0x10 ; User mode #define FIQ_MODE 0x11 ; Fast Interrupt Request mode #define IRQ_MODE 0x12 ; Interrupt Request mode #define SVC_MODE 0x13 ; Supervisor mode #define ABT_MODE 0x17 ; Abort mode #define UND_MODE 0x1B ; Undefined Instruction mode #define SYS_MODE 0x1F ; System mode #define Mode_SVC 0x13 #define Mode_ABT 0x17 #define Mode_UND 0x1B #define GICI_BASE 0xe8202000 #define ICCIAR_OFFSET 0x0000000C #define ICCEOIR_OFFSET 0x00000010 #define ICCHPIR_OFFSET 0x00000018 #define GICD_BASE 0xe8201000 #define GIC_ERRATA_CHECK_1 0x000003FE #define GIC_ERRATA_CHECK_2 0x000003FF #define ICDABR0_OFFSET 0x00000300 #define ICDIPR0_OFFSET 0x00000400 #define T_Bit 0x20 ; when T bit is set, core is in Thumb state MRS r0, cpsr ; Original PSR value ;; Set up the SVC stack pointer. BIC r0, r0, #MODE_MSK ; Clear the mode bits ORR r0, r0, #SVC_MODE ; Set SVC mode bits MSR cpsr_c, r0 ; Change the mode LDR sp, =SFE(SVC_STACK) ; End of SVC_STACK BIC sp,sp,#0x7 ; Make sure SP is 8 aligned ;; Set up the interrupt stack pointer. 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 sp,sp,#0x7 ; Make sure SP is 8 aligned ;; Set up the fast interrupt stack pointer. 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 sp,sp,#0x7 ; Make sure SP is 8 aligned ;; Set up the ABT stack pointer. BIC r0 ,r0, #MODE_MSK ; Clear the mode bits ORR r0 ,r0, #ABT_MODE ; Set System mode bits MSR cpsr_c, r0 ; Change the mode LDR sp, =SFE(ABT_STACK) ; End of CSTACK BIC sp,sp,#0x7 ; Make sure SP is 8 aligned ;; Set up the UDF stack pointer. BIC r0 ,r0, #MODE_MSK ; Clear the mode bits ORR r0 ,r0, #UND_MODE ; Set System mode bits MSR cpsr_c, r0 ; Change the mode LDR sp, =SFE(UND_STACK) ; End of CSTACK BIC sp,sp,#0x7 ; Make sure SP is 8 aligned ;; Set up the normal stack pointer. 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 BIC sp,sp,#0x7 ; Make sure SP is 8 aligned ;;; isb ldr r0, =RZ_A1_SetSramWriteEnable blx r0 bl create_translation_table ; USR/SYS stack pointer will be set during kernel init ldr r0, =SystemInit blx r0 ldr r0, =InitMemorySubsystem blx r0 ; fp_init mov r0, #0x3000000 vmsr fpscr, r0 ;;; Continue to __cmain for C-level initialization. FUNCALL __iar_program_start, __cmain B __cmain ldr r0, sf_boot ;@ dummy to keep boot loader area loop_here: b loop_here sf_boot: DC32 0x00000001 Undefined_Handler: EXTERN CUndefHandler SRSDB SP!, #Mode_UND PUSH {R0-R4, R12} /* Save APCS corruptible registers to UND mode stack */ MRS R0, SPSR TST R0, #T_Bit /* Check mode */ MOVEQ R1, #4 /* R1 = 4 ARM mode */ MOVNE R1, #2 /* R1 = 2 Thumb mode */ SUB R0, LR, R1 LDREQ R0, [R0] /* ARM mode - R0 points to offending instruction */ BEQ undef_cont /* Thumb instruction */ /* Determine if it is a 32-bit Thumb instruction */ LDRH R0, [R0] MOV R2, #0x1c CMP R2, R0, LSR #11 BHS undef_cont /* 16-bit Thumb instruction */ /* 32-bit Thumb instruction. Unaligned - we need to reconstruct the offending instruction. */ LDRH R2, [LR] ORR R0, R2, R0, LSL #16 undef_cont: MOV R2, LR /* Set LR to third argument */ /* AND R12, SP, #4 */ /* Ensure stack is 8-byte aligned */ MOV R3, SP /* Ensure stack is 8-byte aligned */ AND R12, R3, #4 SUB SP, SP, R12 /* Adjust stack */ PUSH {R12, LR} /* Store stack adjustment and dummy LR */ /* R0 Offending instruction */ /* R1 =2 (Thumb) or =4 (ARM) */ BL CUndefHandler POP {R12, LR} /* Get stack adjustment & discard dummy LR */ ADD SP, SP, R12 /* Unadjust stack */ LDR LR, [SP, #24] /* Restore stacked LR and possibly adjust for retry */ SUB LR, LR, R0 LDR R0, [SP, #28] /* Restore stacked SPSR */ MSR SPSR_cxsf, R0 POP {R0-R4, R12} /* Restore stacked APCS registers */ ADD SP, SP, #8 /* Adjust SP for already-restored banked registers */ MOVS PC, LR Prefetch_Handler: EXTERN CPAbtHandler SUB LR, LR, #4 /* Pre-adjust LR */ SRSDB SP!, #Mode_ABT /* Save LR and SPRS to ABT mode stack */ PUSH {R0-R4, R12} /* Save APCS corruptible registers to ABT mode stack */ MRC p15, 0, R0, c5, c0, 1 /* IFSR */ MRC p15, 0, R1, c6, c0, 2 /* IFAR */ MOV R2, LR /* Set LR to third argument */ /* AND R12, SP, #4 */ /* Ensure stack is 8-byte aligned */ MOV R3, SP /* Ensure stack is 8-byte aligned */ AND R12, R3, #4 SUB SP, SP, R12 /* Adjust stack */ PUSH {R12, LR} /* Store stack adjustment and dummy LR */ BL CPAbtHandler POP {R12, LR} /* Get stack adjustment & discard dummy LR */ ADD SP, SP, R12 /* Unadjust stack */ POP {R0-R4, R12} /* Restore stack APCS registers */ RFEFD SP! /* Return from exception */ Abort_Handler: EXTERN CDAbtHandler SUB LR, LR, #8 /* Pre-adjust LR */ SRSDB SP!, #Mode_ABT /* Save LR and SPRS to ABT mode stack */ PUSH {R0-R4, R12} /* Save APCS corruptible registers to ABT mode stack */ CLREX /* State of exclusive monitors unknown after taken data abort */ MRC p15, 0, R0, c5, c0, 0 /* DFSR */ MRC p15, 0, R1, c6, c0, 0 /* DFAR */ MOV R2, LR /* Set LR to third argument */ /* AND R12, SP, #4 */ /* Ensure stack is 8-byte aligned */ MOV R3, SP /* Ensure stack is 8-byte aligned */ AND R12, R3, #4 SUB SP, SP, R12 /* Adjust stack */ PUSH {R12, LR} /* Store stack adjustment and dummy LR */ BL CDAbtHandler POP {R12, LR} /* Get stack adjustment & discard dummy LR */ ADD SP, SP, R12 /* Unadjust stack */ POP {R0-R4, R12} /* Restore stacked APCS registers */ RFEFD SP! /* Return from exception */ FIQ_Handler: /* An FIQ might occur between the dummy read and the real read of the GIC in IRQ_Handler, * so if a real FIQ Handler is implemented, this will be needed before returning: */ /* LDR R1, =GICI_BASE LDR R0, [R1, #ICCHPIR_OFFSET] ; Dummy Read ICCHPIR (GIC CPU Interface register) to avoid GIC 390 errata 801120 */ B . EXTERN SVC_Handler /* refer RTX function */ IRQ_Handler: EXTERN IRQCount EXTERN IRQTable EXTERN IRQNestLevel /* prologue */ SUB LR, LR, #4 /* Pre-adjust LR */ SRSDB SP!, #Mode_SVC /* Save LR_IRQ and SPRS_IRQ to SVC mode stack */ CPS #Mode_SVC /* Switch to SVC mode, to avoid a nested interrupt corrupting LR on a BL */ PUSH {R0-R3, R12} /* Save remaining APCS corruptible registers to SVC stack */ /* AND R1, SP, #4 */ /* Ensure stack is 8-byte aligned */ MOV R3, SP /* Ensure stack is 8-byte aligned */ AND R1, R3, #4 SUB SP, SP, R1 /* Adjust stack */ PUSH {R1, LR} /* Store stack adjustment and LR_SVC to SVC stack */ LDR R0, =IRQNestLevel /* Get address of nesting counter */ LDR R1, [R0] ADD R1, R1, #1 /* Increment nesting counter */ STR R1, [R0] /* identify and acknowledge interrupt */ LDR R1, =GICI_BASE LDR R0, [R1, #ICCHPIR_OFFSET] /* Dummy Read ICCHPIR (GIC CPU Interface register) to avoid GIC 390 errata 801120 */ LDR R0, [R1, #ICCIAR_OFFSET] /* Read ICCIAR (GIC CPU Interface register) */ DSB /* Ensure that interrupt acknowledge completes before re-enabling interrupts */ /* Workaround GIC 390 errata 733075 * If the ID is not 0, then service the interrupt as normal. * If the ID is 0 and active, then service interrupt ID 0 as normal. * If the ID is 0 but not active, then the GIC CPU interface may be locked-up, so unlock it * with a dummy write to ICDIPR0. This interrupt should be treated as spurious and not serviced. */ LDR R2, =GICD_BASE LDR R3, =GIC_ERRATA_CHECK_1 CMP R0, R3 BEQ unlock_cpu LDR R3, =GIC_ERRATA_CHECK_2 CMP R0, R3 BEQ unlock_cpu CMP R0, #0 BNE int_active /* If the ID is not 0, then service the interrupt */ LDR R3, [R2, #ICDABR0_OFFSET] /* Get the interrupt state */ TST R3, #1 BNE int_active /* If active, then service the interrupt */ unlock_cpu: LDR R3, [R2, #ICDIPR0_OFFSET] /* Not active, so unlock the CPU interface */ STR R3, [R2, #ICDIPR0_OFFSET] /* with a dummy write */ DSB /* Ensure the write completes before continuing */ B ret_irq /* Do not service the spurious interrupt */ /* End workaround */ int_active: LDR R2, =IRQCount /* Read number of IRQs */ LDR R2, [R2] CMP R0, R2 /* Clean up and return if no handler */ BHS ret_irq /* In a single-processor system, spurious interrupt ID 1023 does not need any special handling */ LDR R2, =IRQTable /* Get address of handler */ LDR R2, [R2, R0, LSL #2] CMP R2, #0 /* Clean up and return if handler address is 0 */ BEQ ret_irq PUSH {R0,R1} CPSIE i /* Now safe to re-enable interrupts */ BLX R2 /* Call handler. R0 will be IRQ number */ CPSID i /* Disable interrupts again */ /* write EOIR (GIC CPU Interface register) */ POP {R0,R1} DSB /* Ensure that interrupt source is cleared before we write the EOIR */ ret_irq: /* epilogue */ STR R0, [R1, #ICCEOIR_OFFSET] LDR R0, =IRQNestLevel /* Get address of nesting counter */ LDR R1, [R0] SUB R1, R1, #1 /* Decrement nesting counter */ STR R1, [R0] POP {R1, LR} /* Get stack adjustment and restore LR_SVC */ ADD SP, SP, R1 /* Unadjust stack */ POP {R0-R3,R12} /* Restore stacked APCS registers */ RFEFD SP! /* Return from exception */ ;;; ;;; Add more initialization here ;;; FPUEnable: ARM //Permit access to VFP registers by modifying CPACR MRC p15,0,R1,c1,c0,2 ORR R1,R1,#0x00F00000 MCR p15,0,R1,c1,c0,2 //Enable VFP VMRS R1,FPEXC ORR R1,R1,#0x40000000 VMSR FPEXC,R1 //Initialise VFP registers to 0 MOV R2,#0 VMOV D0, R2,R2 VMOV D1, R2,R2 VMOV D2, R2,R2 VMOV D3, R2,R2 VMOV D4, R2,R2 VMOV D5, R2,R2 VMOV D6, R2,R2 VMOV D7, R2,R2 VMOV D8, R2,R2 VMOV D9, R2,R2 VMOV D10,R2,R2 VMOV D11,R2,R2 VMOV D12,R2,R2 VMOV D13,R2,R2 VMOV D14,R2,R2 VMOV D15,R2,R2 //Initialise FPSCR to a known state VMRS R2,FPSCR LDR R3,=0x00086060 //Mask off all bits that do not have to be preserved. Non-preserved bits can/should be zero. AND R2,R2,R3 VMSR FPSCR,R2 BX LR END