|
|
@ -31,33 +31,33 @@ void at91_init1(void) |
|
|
|
{ |
|
|
|
/* disable watchdog */ |
|
|
|
*AT91C_WDTC_WDMR = AT91C_WDTC_WDDIS; |
|
|
|
|
|
|
|
|
|
|
|
/* enable user reset */ |
|
|
|
*AT91C_RSTC_RMR = (AT91C_RSTC_KEY & 0xA5 << 24) | AT91C_RSTC_URSTEN; |
|
|
|
|
|
|
|
/* Set Flash Waitstates */ |
|
|
|
*AT91C_MC_FMR = AT91C_MC_FWS_1FWS; |
|
|
|
|
|
|
|
/* |
|
|
|
|
|
|
|
/* |
|
|
|
* Enable main oscillator (MAINCK) |
|
|
|
* startup time: 8*6/32768 -> 1.46ms |
|
|
|
*/ |
|
|
|
AT91S_PMC *pmc = AT91C_BASE_PMC; |
|
|
|
pmc->PMC_MOR = (AT91C_CKGR_OSCOUNT & (6<<8)) | |
|
|
|
pmc->PMC_MOR = (AT91C_CKGR_OSCOUNT & (6<<8)) | |
|
|
|
AT91C_CKGR_MOSCEN; |
|
|
|
while (!(pmc->PMC_SR & AT91C_PMC_MOSCS)); |
|
|
|
|
|
|
|
/* |
|
|
|
/* |
|
|
|
* PLLCK = 18.432MHz / 24 * 125 = 96MHz -> div:24, mul:124 |
|
|
|
* startup time: 32/32768 -> 976us |
|
|
|
*/ |
|
|
|
pmc->PMC_PLLR = (AT91C_CKGR_DIV & 24) | |
|
|
|
(AT91C_CKGR_MUL & (124<<16)) | |
|
|
|
*/ |
|
|
|
pmc->PMC_PLLR = (AT91C_CKGR_DIV & 24) | |
|
|
|
(AT91C_CKGR_MUL & (124<<16)) | |
|
|
|
(AT91C_CKGR_PLLCOUNT & (32<<8)) ; |
|
|
|
while (!(pmc->PMC_SR & AT91C_PMC_LOCK)); |
|
|
|
|
|
|
|
/* MCK = PLLCK / 2 = 48MHz */ |
|
|
|
pmc->PMC_MCKR = AT91C_PMC_CSS_PLL_CLK | |
|
|
|
pmc->PMC_MCKR = AT91C_PMC_CSS_PLL_CLK | |
|
|
|
AT91C_PMC_PRES_CLK_2; |
|
|
|
while (!(pmc->PMC_SR & AT91C_PMC_MCKRDY)); |
|
|
|
|
|
|
@ -79,3 +79,96 @@ void at91_init1(void) |
|
|
|
} |
|
|
|
aic->AIC_SPU = (uint32_t)empty_isr; |
|
|
|
} |
|
|
|
|
|
|
|
__attribute__((naked)) void IRQ_Handler(void) |
|
|
|
{ |
|
|
|
asm volatile ( |
|
|
|
".equ ARM_MODE_IRQ, 0x12 \n\t" |
|
|
|
".equ ARM_MODE_SVC, 0x13 \n\t" |
|
|
|
".equ I_BIT, 0x80 \n\t" |
|
|
|
|
|
|
|
".equ AIC_IVR, (256) \n\t" |
|
|
|
".equ AIC_EOICR, (304) \n\t" |
|
|
|
".equ AT91C_BASE_AIC, (0xFFFFF000) \n\t" |
|
|
|
|
|
|
|
/* Adjust and save lr_irq on IRQ stack */ |
|
|
|
"sub lr, lr, #4 \n\t" |
|
|
|
"stmfd sp!, { lr } \n\t" |
|
|
|
|
|
|
|
/* Save SPSR (for nested interrupts) */ |
|
|
|
"mrs r14, SPSR \n\t" |
|
|
|
"stmfd sp!, { r14 } \n\t" |
|
|
|
|
|
|
|
/* Save r0 on IRQ stack */ |
|
|
|
"stmfd sp!, { r0 } \n\t" |
|
|
|
|
|
|
|
/* Write in the IVR to support Protect Mode */ |
|
|
|
"ldr r14, =AT91C_BASE_AIC \n\t" |
|
|
|
"ldr r0, [r14, #AIC_IVR] \n\t" |
|
|
|
"str r14, [r14, #AIC_IVR] \n\t" |
|
|
|
|
|
|
|
/* Enable Interrupt and switch to SVC mode */ |
|
|
|
"msr CPSR_c, #ARM_MODE_SVC \n\t" |
|
|
|
|
|
|
|
/* Save scratch/used registers and lr on SVC Stack */ |
|
|
|
"stmfd sp!, { r1-r3, r12, r14 } \n\t" |
|
|
|
|
|
|
|
/* Branch to the routine pointed by the AIC_IVR */ |
|
|
|
"mov r14, pc \n\t" |
|
|
|
"bx r0 \n\t" |
|
|
|
|
|
|
|
/* Restore scratch/used registers and lr from SVC Stack */ |
|
|
|
"ldmia sp!, { r1-r3, r12, r14 } \n\t" |
|
|
|
|
|
|
|
/* Disable Interrupt and switch back to IRQ mode */ |
|
|
|
"msr CPSR_c, #ARM_MODE_IRQ | I_BIT \n\t" |
|
|
|
|
|
|
|
/* Mark the End of Interrupt on the AIC */ |
|
|
|
"ldr r14, =AT91C_BASE_AIC \n\t" |
|
|
|
"str r14, [r14, #AIC_EOICR] \n\t" |
|
|
|
|
|
|
|
/* Restore SPSR_irq and r0 from IRQ stack */ |
|
|
|
"ldmia sp!, { r0 } \n\t" |
|
|
|
"ldmia sp!, { r14 } \n\t" |
|
|
|
"msr SPSR_cxsf, r14 \n\t" |
|
|
|
|
|
|
|
/* Restore adjusted lr_irq from IRQ stack */ |
|
|
|
"ldmia sp!, { pc }^ \n\t" |
|
|
|
); |
|
|
|
} |
|
|
|
|
|
|
|
__attribute__((naked)) void FIQ_Handler(void) |
|
|
|
{ |
|
|
|
asm volatile ( |
|
|
|
".equ ARM_MODE_FIQ, 0x11 \n\t" |
|
|
|
".equ ARM_MODE_SVC, 0x13 \n\t" |
|
|
|
".equ I_BIT, 0x80 \n\t" |
|
|
|
".equ F_BIT, 0x40 \n\t" |
|
|
|
|
|
|
|
".equ AIC_FVR, (260) \n\t" |
|
|
|
|
|
|
|
/* Save r0 to r9_fiq */ |
|
|
|
"mov r9, r0 \n\t" |
|
|
|
|
|
|
|
/* get FIQ Vector from AIC and thus clear FIQ */ |
|
|
|
"ldr r0, [r8, #AIC_FVR] \n\t" |
|
|
|
|
|
|
|
/* Switch to SVC and save registers there */ |
|
|
|
"msr CPSR_c, #ARM_MODE_SVC | I_BIT | F_BIT \n\t" |
|
|
|
"stmfd sp!, { r1-r3, r12, lr } \n\t" |
|
|
|
|
|
|
|
/* execute FIQ in SVC_MODE */ |
|
|
|
"mov r14, pc \n\t" |
|
|
|
"bx r0 \n\t" |
|
|
|
|
|
|
|
/* restore registers and switch back to FIQ */ |
|
|
|
"ldmia sp!, { r1-r3, r12, lr } \n\t" |
|
|
|
"msr CPSR_c, #ARM_MODE_FIQ | I_BIT | F_BIT \n\t" |
|
|
|
|
|
|
|
/* restore the r0 from r9_fiq */ |
|
|
|
"mov r0, r9 \n\t" |
|
|
|
|
|
|
|
/* restore PC using the lr_fiq directly */ |
|
|
|
"subs pc, lr, #4 \n\t" |
|
|
|
); |
|
|
|
} |