diff options
| -rw-r--r-- | firmware/src/start/Cstartup.S | 37 | ||||
| -rw-r--r-- | firmware/src/start/Cstartup_app.S | 142 |
2 files changed, 148 insertions, 31 deletions
diff --git a/firmware/src/start/Cstartup.S b/firmware/src/start/Cstartup.S index 990dbcb..83e7696 100644 --- a/firmware/src/start/Cstartup.S +++ b/firmware/src/start/Cstartup.S @@ -43,13 +43,14 @@ //#define DEBUG_LL -#ifdef DEBUG_LL -/* Debugging macros for switching on/off LED1 (green) */ #define PIOA_PER 0xFFFFF400 #define PIOA_OER 0xFFFFF410 #define PIOA_SODR 0xFFFFF430 #define PIOA_CODR 0xFFFFF434 #define LED1 25 /* this only works on OpenPICC, not Olimex */ + +#ifdef DEBUG_LL +/* Debugging macros for switching on/off LED1 (green) */ .macro led1on ldr r2, =PIOA_CODR mov r1, #(1 << LED1) @@ -78,6 +79,7 @@ #endif .equ IRQ_Stack_Size, 0x00000400 + .equ FIQ_Stack_Size, 0x00000400 .equ AIC_IVR, (256) .equ AIC_FVR, (260) @@ -167,10 +169,13 @@ rsvdvec: irqvec: b IRQ_Handler_Entry /* 0x18 IRQ */ fiqvec: - b FIQ_Handler_Entry - /* 0x1c FIQ */ + ldr pc, [pc, #-0xF20] /* 0x1c FIQ */ + dfu_state_dummy: .word 0 + + .global IRQ_Handler_Entry + .func IRQ_Handler_Entry FIQ_Handler_Entry: @@ -201,9 +206,6 @@ FIQ_Handler_Entry: /*- Restore the Program Counter using the LR_fiq directly in the PC */ subs pc, lr, #4 - .global IRQ_Handler_Entry - .func IRQ_Handler_Entry - IRQ_Handler_Entry: /*- Manage Exception Entry */ @@ -277,17 +279,6 @@ InitReset: mov lr, pc bx r0 ledinit -/*------------------------------------------------------------------------------ -//*- Stack Sizes Definition -//*------------------------ -//*- Interrupt Stack requires 2 words x 8 priority level x 4 bytes when using -//*- the vectoring. This assume that the IRQ management. -//*- The Interrupt Stack must be adjusted depending on the interrupt handlers. -//*- Fast Interrupt not requires stack If in your application it required you must -//*- be definehere. -//*- The System stack size is not defined and is limited by the free internal -//*- SRAM. -//*------------------------------------------------------------------------------*/ /*------------------------------------------------------------------------------ //*- Top of Stack Definition @@ -297,7 +288,6 @@ InitReset: //*- ARM_MODE_SVC (Application, C) Stack is located at the top of the external memory. //*------------------------------------------------------------------------------*/ - .EQU IRQ_STACK_SIZE, (3*8*4) .EQU ARM_MODE_FIQ, 0x11 .EQU ARM_MODE_IRQ, 0x12 .EQU ARM_MODE_SVC, 0x13 @@ -318,6 +308,9 @@ InitReset: /*- Set up Fast Interrupt Mode and set FIQ Mode Stack*/ msr CPSR_c, #ARM_MODE_FIQ | I_BIT | F_BIT + mov r13, r0 + sub r0, r0, #FIQ_Stack_Size + /*- Init the FIQ register*/ ldr r8, =AT91C_BASE_AIC @@ -325,10 +318,14 @@ InitReset: msr CPSR_c, #ARM_MODE_IRQ | I_BIT | F_BIT mov r13, r0 /* Init stack IRQ */ sub r0, r0, #IRQ_Stack_Size + /*- Set up Supervisor Mode and set Supervisor Mode Stack*/ - msr CPSR_c, #ARM_MODE_SVC + msr CPSR_c, #ARM_MODE_SVC | I_BIT | F_BIT mov r13, r0 /* Init stack Sup */ +/* - Enable Interrupts and FIQ */ + msr CPSR_c, #ARM_MODE_SVC + #ifdef CONFIG_DFU_MAGIC ldr r1, =AT91C_RSTC_RSR ldr r2, [r1] diff --git a/firmware/src/start/Cstartup_app.S b/firmware/src/start/Cstartup_app.S index 0fc2bd5..fb70272 100644 --- a/firmware/src/start/Cstartup_app.S +++ b/firmware/src/start/Cstartup_app.S @@ -20,26 +20,48 @@ //#define DEBUG_LL + .equ AIC_FVR, (260) + .equ AIC_EOICR, (304) + .equ AT91C_BASE_AIC, (0xFFFFF000) + .equ ARM_MODE_FIQ, 0x11 + .equ ARM_MODE_IRQ, 0x12 + .equ ARM_MODE_SVC, 0x13 + + .equ I_BIT, 0x80 + .equ F_BIT, 0x40 + + +#define AT91C_BASE_PIOA 0xFFFFF400 +#define AT91C_BASE_TC0 0xFFFA0000 +#define AT91C_TC_SWTRG (1 << 2) +#define PIOA_SODR 0x30 +#define PIOA_CODR 0x34 +#define PIOA_PDSR 0x3c +#define PIOA_ISR 0x4c +#define PIOA_IDR 0x44 +#define PIO_DATA (1 << 27) +#define TC_CCR 0x00 + + +#define PIO_LED1 (1 << 25) /* this only works on OpenPICC, not Olimex */ + #ifdef DEBUG_LL /* Debugging macros for switching on/off LED1 (green) */ #define PIOA_PER 0xFFFFF400 #define PIOA_OER 0xFFFFF410 -#define PIOA_SODR 0xFFFFF430 -#define PIOA_CODR 0xFFFFF434 -#define LED1 25 /* this only works on OpenPICC, not Olimex */ .macro led1on - ldr r2, =PIOA_CODR - mov r1, #(1 << LED1) - str r1, [r2] + ldr r2, =AT91C_BASE_PIOA + mov r1, #PIO_LED1 + str r1, [r2, #PIOA_CODR] .endm .macro led1off - ldr r2, =PIOA_SODR - mov r1, #(1 << LED1) - str r1, [r2] + ldr r2, =AT91C_BASE_PIOA + mov r1, #PIO_LED1 + str r1, [r2, #PIOA_SODR] .endm .macro ledinit ldr r2, =PIOA_PER - mov r1, #(1 << LED1) + mov r1, #PIO_LED1 str r1, [r2] ldr r2, =PIOA_OER str r1, [r2] @@ -74,6 +96,13 @@ loop_z: cmp r1, r2 strlo r0, [r1], #4 blo loop_z + /* initialize FIQ mode registers */ + msr CPSR_c, #ARM_MODE_FIQ | I_BIT | F_BIT + ldr r10, =AT91C_BASE_PIOA + ldr r12, =AT91C_BASE_TC0 + mov r9, #AT91C_TC_SWTRG + msr CPSR_c, #ARM_MODE_SVC + led1on /* prepare C function call to main */ @@ -91,8 +120,99 @@ loop_z: cmp r1, r2 .func exit exit: b . - .size exit, . - exit + .size exit, . - exit .endfunc + + .text + .arm + .section .fastrun, "ax" + + .global fiq_handler + .func fiq_handler +fiq_handler: + /* code that uses pre-initialized FIQ reg */ + /* r8 AT91C_BASE_AIC (dfu init) + r9 AT91C_TC_SWTRG + r10 AT91C_BASE_PIOA + r11 tmp + r12 AT91C_BASE_TC0 + r13 stack + r14 lr + */ +#if 0 + mov r11, #PIO_LED1 + str r11, [r10, #PIOA_CODR] /* enable LED */ + str r11, [r10, #PIOA_SODR] /* disable LED */ +#endif + + ldr r8, [r10, #PIOA_ISR] + tst r8, #PIO_DATA /* check for PIO_DATA change */ + ldrne r11, [r10, #PIOA_PDSR] + tstne r11, #PIO_DATA /* check for PIO_DATA == 1 */ + strne r9, [r12, #TC_CCR] /* software trigger */ + + movne r11, #PIO_DATA + strne r11, [r10, #PIOA_IDR] /* disable further PIO_DATA FIQ */ + + /*- Mark the End of Interrupt on the AIC */ + ldr r11, =AT91C_BASE_AIC + str r11, [r11, #AIC_EOICR] + + /* push r0, r1-r3, r12, r14 onto FIQ stack */ + stmfd sp!, { r0-r3, r12, lr} + mov r0, r8 + + /* enable interrupts while handling demux */ + /* msr CPSR_c, #F_BIT | ARM_MODE_SVC */ + + /* Call C function, give PIOA_ISR as argument */ + ldr r11, =__pio_irq_demux + mov r14, pc + bx r11 + + /* msr CPSR_c, #I_BIT | F_BIT | ARM_MODE_FIQ */ + + ldmia sp!, { r0-r3, r12, lr } + +#if 0 +/*- Save and r0 in FIQ_Register */ + mov r9, r0 + ldr r0, [r11, #AIC_FVR] + str r8, [r11, #AIC_FVR] + /* msr CPSR_c, #I_BIT | F_BIT | ARM_MODE_SVC */ + + /*- Save scratch/used registers and LR in User Stack */ + stmfd sp!, { r1-r3, r12, lr} + + led1on + + /*- Branch to the routine pointed by the AIC_FVR */ + mov r14, pc + bx r0 + + /*- Restore scratch/used registers and LR from User Stack */ + ldmia sp!, { r1-r3, r12, lr} + + /*- Leave Interrupts disabled and switch back in FIQ mode */ + /* msr CPSR_c, #I_BIT | F_BIT | ARM_MODE_FIQ */ + + /*- Mark the End of Interrupt on the AIC */ + ldr r0, =AT91C_BASE_AIC + str r0, [r0, #AIC_EOICR] + + /*- Restore the R0 ARM_MODE_SVC register */ + mov r0,r9 + + ldr r10, =PIOA_SODR + mov r11, #(1 << LED1) + str r11, [r10] + +#endif + /*- Restore the Program Counter using the LR_fiq directly in the PC */ + subs pc, lr, #4 + + .size fiq_handler, . - fiq_handler + .endfunc .end |