diff --git a/at91_init0.s b/at91_init0.s index 583bc28..b2ced47 100644 --- a/at91_init0.s +++ b/at91_init0.s @@ -1,244 +1,241 @@ -/*************************************************************************** - * Copyright (C) 01/2008 by Olaf Rempel * - * razzor@kopf-tisch.de * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; version 2 of the License * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, write to the * - * Free Software Foundation, Inc., * - * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * - ***************************************************************************/ - .equ AIC_IVR, (256) - .equ AIC_FVR, (260) - .equ AIC_EOICR, (304) - .equ AT91C_BASE_AIC, (0xFFFFF000) - - .equ IRQ_Stack_Size, (3 * 8 * 4) - .equ FIQ_Stack_Size, (3 * 8 * 4) - .equ ABT_Stack_Size, 192 - - .equ ARM_MODE_FIQ, 0x11 - .equ ARM_MODE_IRQ, 0x12 - .equ ARM_MODE_SVC, 0x13 - .equ ARM_MODE_ABT, 0x17 - - .equ I_BIT, 0x80 - .equ F_BIT, 0x40 - - .section .text - .global _start - .func _start - -_start: ldr pc, [pc, #24] /* 0x00 Reset handler */ -undefvec: ldr pc, [pc, #24] /* 0x04 Undefined Instruction */ -swivec: ldr pc, [pc, #24] /* 0x08 Software Interrupt */ -pabtvec: ldr pc, [pc, #24] /* 0x0C Prefetch Abort */ -dabtvec: ldr pc, [pc, #24] /* 0x10 Data Abort */ -rsvdvec: ldr pc, [pc, #24] /* 0x14 reserved */ -irqvec: ldr pc, [pc, #24] /* 0x18 IRQ */ -fiqvec: ldr pc, [pc, #24] /* 0x1c FIQ */ - - /* 0x80000000 will result in Prefetch Abort */ - .word InitReset - .word 0x80000000 - .word 0x80000000 - .word ABT_Handler_Entry - .word ABT_Handler_Entry - .word 0x80000000 - .word IRQ_Handler_Entry - .word FIQ_Handler_Entry - - .endfunc - - .global InitReset - .func InitReset - -InitReset: - .extern at91_init1 - /* Call Low level init function */ - ldr sp,=__stack_top__ - ldr r0,=at91_init1 - mov lr, pc - bx r0 - mov r0, sp - - /* Setup FIQ Mode Stack */ - msr CPSR_c, #ARM_MODE_FIQ | I_BIT | F_BIT - mov sp, r0 - sub r0, r0, #FIQ_Stack_Size - - /* store AIC Base in ARM_MODE_FIQ:r8 for faster access */ - ldr r8, =AT91C_BASE_AIC - - /* Setup IRQ Mode Stack */ - msr CPSR_c, #ARM_MODE_IRQ | I_BIT | F_BIT - mov sp, r0 - sub r0, r0, #IRQ_Stack_Size - - /* Setup Abort Mode Stack */ - msr CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT - mov sp, r0 - sub r0, r0, #ABT_Stack_Size - - /* Setup Supervisor Mode Stack (IRQ & NMI enabled) */ - msr CPSR_c, #ARM_MODE_SVC - mov sp, r0 - - /* Relocate .data section */ - ldr r1, =__text_end__ - ldr r2, =__data_start__ - ldr r3, =__data_end__ -LoopRel: cmp r2, r3 - ldrlo r0, [r1], #4 - strlo r0, [r2], #4 - blo LoopRel - - /* Clear .bss section */ - mov r0, #0 - ldr r1, =__bss_start__ - ldr r2, =__bss_end__ -LoopZI: cmp r1, r2 - strlo r0, [r1], #4 - BLO LoopZI - - /* Start main() */ - ldr lr,=exit - ldr r0,=main - bx r0 - - .endfunc - - .global exit - .func exit - /* exit dummy for newlib */ -exit: b . - .endfunc - - .global ABT_Handler_Entry - .func ABT_Handler_Entry -ABT_Handler_Entry: - /* disable interrupts (F_BIT not set on entry) */ - msr CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT - - /* store all registers */ - stmfd sp!, { r0-r12 } - - /* saved cpsr (from aborted mode) */ - mrs r0, SPSR - - /* address of abort (pc) */ - mov r3, lr - - /* enter previous mode and get lr(r14), sp(r13) */ - /* TODO: interrupts might be enabled? */ - /* TODO: thumb mode enabled? */ - msr CPSR_c, r0 - mov r1, sp - mov r2, lr - - /* return to abort mode */ - msr CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT - - /* store remaining registers (r1-r3 == r13-r15) */ - stmfd sp!, { r1-r3 } - mov r1, sp - - /* execute C Handler (cpsr, registers) */ - ldr r5,=at91_abt_handler - mov lr, pc - bx r5 - b . - .endfunc - - .global FIQ_Handler_Entry - .func FIQ_Handler_Entry -FIQ_Handler_Entry: - /* Save r0 to ARM_MODE_FIQ:r9 */ - mov r9,r0 - - /* get FIQ Vector from AIC and thus clear FIQ */ - ldr r0 , [r8, #AIC_FVR] - - /* Switch to ARM_MODE_SVC and save registers there */ - msr CPSR_c,#ARM_MODE_SVC | I_BIT | F_BIT - stmfd sp!, { r1-r3, r12, lr} - - /* execute FIQ in SVC_MODE */ - mov r14, pc - bx r0 - - /* restore registers and switch back to ARM_MODE_FIQ */ - ldmia sp!, { r1-r3, r12, lr} - msr CPSR_c, #ARM_MODE_FIQ | I_BIT | F_BIT - - /* restore the ARM_MODE_SVC:r0 */ - mov r0,r9 - - /* restore PC using the LR_fiq directly */ - subs pc,lr,#4 - - .endfunc - - - .global IRQ_Handler_Entry - .func IRQ_Handler_Entry -IRQ_Handler_Entry: -/*- Manage Exception Entry */ -/*- Adjust and save LR_irq in IRQ stack */ - sub lr, lr, #4 - stmfd sp!, {lr} - -/*- Save SPSR need to be saved for nested interrupt */ - mrs r14, SPSR - stmfd sp!, {r14} - -/*- Save and r0 in IRQ stack */ - stmfd sp!, {r0} - -/*- Write in the IVR to support Protect Mode */ -/*- No effect in Normal Mode */ -/*- De-assert the NIRQ and clear the source in Protect Mode */ - ldr r14, =AT91C_BASE_AIC - ldr r0 , [r14, #AIC_IVR] - str r14, [r14, #AIC_IVR] - -/*- Enable Interrupt and Switch in Supervisor Mode */ - msr CPSR_c, #ARM_MODE_SVC - -/*- Save scratch/used registers and LR in User Stack */ - stmfd sp!, { r1-r3, r12, r14} - -/*- Branch to the routine pointed by the AIC_IVR */ - mov r14, pc - bx r0 -/*- Restore scratch/used registers and LR from User Stack*/ - ldmia sp!, { r1-r3, r12, r14} - -/*- Disable Interrupt and switch back in IRQ mode */ - msr CPSR_c, #ARM_MODE_IRQ | I_BIT - -/*- Mark the End of Interrupt on the AIC */ - ldr r14, =AT91C_BASE_AIC - str r14, [r14, #AIC_EOICR] - -/*- Restore SPSR_irq and r0 from IRQ stack */ - ldmia sp!, {r0} - -/*- Restore SPSR_irq and r0 from IRQ stack */ - ldmia sp!, {r14} - msr SPSR_cxsf, r14 - -/*- Restore adjusted LR_irq from IRQ stack directly in the PC */ - ldmia sp!, {pc}^ - - .endfunc - - .end +/*************************************************************************** + * Copyright (C) 01/2008 by Olaf Rempel * + * razzor@kopf-tisch.de * + * * + * This program is free software; you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation; version 2 of the License * + * * + * This program is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU General Public License for more details. * + * * + * You should have received a copy of the GNU General Public License * + * along with this program; if not, write to the * + * Free Software Foundation, Inc., * + * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * + ***************************************************************************/ + .equ AIC_IVR, (256) + .equ AIC_FVR, (260) + .equ AIC_EOICR, (304) + .equ AT91C_BASE_AIC, (0xFFFFF000) + + .equ IRQ_Stack_Size, (3 * 8 * 4) + .equ FIQ_Stack_Size, (3 * 8 * 4) + .equ ABT_Stack_Size, 192 + + .equ ARM_MODE_FIQ, 0x11 + .equ ARM_MODE_IRQ, 0x12 + .equ ARM_MODE_SVC, 0x13 + .equ ARM_MODE_ABT, 0x17 + + .equ I_BIT, 0x80 + .equ F_BIT, 0x40 + + .section .text + .global _start + .func _start + +_start: ldr pc, [pc, #24] /* 0x00 Reset handler */ +undefvec: ldr pc, [pc, #24] /* 0x04 Undefined Instruction */ +swivec: ldr pc, [pc, #24] /* 0x08 Software Interrupt */ +pabtvec: ldr pc, [pc, #24] /* 0x0C Prefetch Abort */ +dabtvec: ldr pc, [pc, #24] /* 0x10 Data Abort */ +rsvdvec: ldr pc, [pc, #24] /* 0x14 reserved */ +irqvec: ldr pc, [pc, #24] /* 0x18 IRQ */ +fiqvec: ldr pc, [pc, #24] /* 0x1c FIQ */ + + /* 0x80000000 will result in Prefetch Abort */ + .word InitReset + .word 0x80000000 + .word 0x80000000 + .word ABT_Handler_Entry + .word ABT_Handler_Entry + .word 0x80000000 + .word IRQ_Handler_Entry + .word FIQ_Handler_Entry + + .endfunc + + .global InitReset + .func InitReset + .extern at91_init1 +InitReset: + /* Call Low level init function */ + ldr sp, =__stack_top__ + ldr r0, =at91_init1 + mov lr, pc + bx r0 + mov r0, sp + + /* Setup FIQ Mode Stack */ + msr CPSR_c, #ARM_MODE_FIQ | I_BIT | F_BIT + mov sp, r0 + sub r0, r0, #FIQ_Stack_Size + + /* store AIC Base in ARM_MODE_FIQ:r8 for faster access */ + ldr r8, =AT91C_BASE_AIC + + /* Setup IRQ Mode Stack */ + msr CPSR_c, #ARM_MODE_IRQ | I_BIT | F_BIT + mov sp, r0 + sub r0, r0, #IRQ_Stack_Size + + /* Setup Abort Mode Stack */ + msr CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT + mov sp, r0 + sub r0, r0, #ABT_Stack_Size + + /* Setup Supervisor Mode Stack (IRQ & NMI enabled) */ + msr CPSR_c, #ARM_MODE_SVC + mov sp, r0 + + /* Relocate .data section */ + ldr r1, =__text_end__ + ldr r2, =__data_start__ + ldr r3, =__data_end__ +LoopRel: cmp r2, r3 + ldrlo r0, [r1], #4 + strlo r0, [r2], #4 + blo LoopRel + + /* Clear .bss section */ + mov r0, #0 + ldr r1, =__bss_start__ + ldr r2, =__bss_end__ +LoopZI: cmp r1, r2 + strlo r0, [r1], #4 + BLO LoopZI + + /* Start main() */ + ldr lr, =exit + ldr r0, =main + bx r0 + + .endfunc + + .global exit + .func exit + /* exit dummy for newlib */ +exit: b . + .endfunc + + .global ABT_Handler_Entry + .func ABT_Handler_Entry +ABT_Handler_Entry: + /* disable interrupts (F_BIT not set on entry) */ + msr CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT + + /* store all registers */ + stmfd sp!, { r0-r12 } + + /* saved cpsr (from aborted mode) */ + mrs r0, SPSR + + /* address of abort (pc) */ + mov r3, lr + + /* enter previous mode and get lr(r14), sp(r13) */ + /* TODO: interrupts might be enabled? */ + /* TODO: thumb mode enabled? */ + msr CPSR_c, r0 + mov r1, sp + mov r2, lr + + /* return to abort mode */ + msr CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT + + /* store remaining registers (r1-r3 == r13-r15) */ + stmfd sp!, { r1-r3 } + mov r1, sp + + /* execute C Handler (cpsr, registers) */ + ldr r5, =at91_abt_handler + mov lr, pc + bx r5 + b . + .endfunc + + .global FIQ_Handler_Entry + .func FIQ_Handler_Entry +FIQ_Handler_Entry: + /* Save r0 to ARM_MODE_FIQ:r9 */ + mov r9, r0 + + /* get FIQ Vector from AIC and thus clear FIQ */ + ldr r0, [r8, #AIC_FVR] + + /* Switch to ARM_MODE_SVC and save registers there */ + msr CPSR_c, #ARM_MODE_SVC | I_BIT | F_BIT + stmfd sp!, { r1-r3, r12, lr } + + /* execute FIQ in SVC_MODE */ + mov r14, pc + bx r0 + + /* restore registers and switch back to ARM_MODE_FIQ */ + ldmia sp!, { r1-r3, r12, lr } + msr CPSR_c, #ARM_MODE_FIQ | I_BIT | F_BIT + + /* restore the ARM_MODE_SVC:r0 */ + mov r0, r9 + + /* restore PC using the LR_fiq directly */ + subs pc, lr, #4 + .endfunc + + .global IRQ_Handler_Entry + .func IRQ_Handler_Entry +IRQ_Handler_Entry: + /* Manage Exception Entry */ + /* Adjust and save LR_irq in IRQ stack */ + sub lr, lr, #4 + stmfd sp!, { lr } + + /* Save SPSR need to be saved for nested interrupt */ + mrs r14, SPSR + stmfd sp!, { r14 } + + /* Save and r0 in IRQ stack */ + stmfd sp!, { r0 } + + /* Write in the IVR to support Protect Mode */ + /* No effect in Normal Mode */ + /* De-assert the NIRQ and clear the source in Protect Mode */ + ldr r14, =AT91C_BASE_AIC + ldr r0, [r14, #AIC_IVR] + str r14, [r14, #AIC_IVR] + + /* Enable Interrupt and Switch in Supervisor Mode */ + msr CPSR_c, #ARM_MODE_SVC + + /* Save scratch/used registers and LR in User Stack */ + stmfd sp!, { r1-r3, r12, r14 } + + /* Branch to the routine pointed by the AIC_IVR */ + mov r14, pc + bx r0 + + /* Restore scratch/used registers and LR from User Stack */ + ldmia sp!, { r1-r3, r12, r14 } + + /* Disable Interrupt and switch back in IRQ mode */ + msr CPSR_c, #ARM_MODE_IRQ | I_BIT + + /* Mark the End of Interrupt on the AIC */ + ldr r14, =AT91C_BASE_AIC + str r14, [r14, #AIC_EOICR] + + /* Restore SPSR_irq and r0 from IRQ stack */ + ldmia sp!, { r0 } + + /* Restore SPSR_irq and r0 from IRQ stack */ + ldmia sp!, { r14 } + msr SPSR_cxsf, r14 + + /* Restore adjusted LR_irq from IRQ stack directly in the PC */ + ldmia sp!, { pc }^ + + .endfunc + .end