- introduce FIQ stack of 1k (way too large, but we might have debug...)

- move FIQ processing out of DFU Flash and into application/RAM


git-svn-id: https://svn.openpcd.org:2342/trunk@239 6dc7ffe9-61d6-0310-9af1-9938baff3ed1

1 files changed, 131 insertions, 11 deletions

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