- 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

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