summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--firmware/src/start/Cstartup.S37
-rw-r--r--firmware/src/start/Cstartup_app.S142
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
personal git repositories of Harald Welte. Your mileage may vary