Switch to hardcoded fiq handler implementation directly in the vector table (milosch's idea)

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

2 files changed, 19 insertions, 105 deletions

diff --git a/openpicc/application/pio_irq.c b/openpicc/application/pio_irq.c
index 04763a3..5359bbf 100644
--- a/openpicc/application/pio_irq.c
+++ b/openpicc/application/pio_irq.c
@@ -41,7 +41,7 @@ struct pioirq_state {
 static struct pioirq_state pirqs;
 static unsigned long count = 0;
 
-/* This FIQ implementation of pio data change works in close cooperation with function my_fiq_handler  
+/* This FIQ implementation of pio data change works in close cooperation with function fiq_handler  
  * in os/boot/boot.s
  * This code uses fast forcing for the PIOA irq so that each PIOA data change triggers
  * a FIQ. The FreeRTOS code has been modified to not mask FIQ ever. This means that the FIQ
@@ -64,7 +64,6 @@ static unsigned long count = 0;
  */
 #define USE_FIQ
 #define PIO_SECONDARY_IRQ 31
-extern void my_fiq_handler(void);
 extern void fiq_handler(void);
 
 /* Will be used in pio_irq_demux_secondary below and contains the PIO_ISR value 
@@ -89,29 +88,6 @@ void __ramfunc __pio_irq_demux(u_int32_t pio)
 			send_usb = 1;
 	}
 
-//	if (send_usb && !pirqs.usb_throttled) {
-//		struct req_ctx *irq_rctx;
-//		irq_rctx = req_ctx_find_get(0, RCTX_STATE_FREE,
-//					    RCTX_STATE_PIOIRQ_BUSY);
-//		if (!irq_rctx) {
-//			/* we cannot disable the interrupt, since we have
-//			 * non-usb listeners */
-//			pirqs.usb_throttled = 1;
-//		} else {
-//			struct openpcd_hdr *opcdh;
-//			u_int32_t *regmask;
-//			opcdh = (struct openpcd_hdr *) irq_rctx->data;
-//			regmask = (u_int32_t *) (irq_rctx->data + sizeof(*opcdh));
-//			opcdh->cmd = OPENPCD_CMD_PIO_IRQ;
-//			opcdh->reg = 0x00;
-//			opcdh->flags = 0x00;
-//			opcdh->val = 0x00;
-//
-//			irq_rctx->tot_len = sizeof(*opcdh) + sizeof(u_int32_t);
-//			req_ctx_set_state(irq_rctx, RCTX_STATE_UDP_EP3_PENDING);
-//		}
-//	}
-
 	AT91F_AIC_AcknowledgeIt();
 	//AT91F_AIC_ClearIt(AT91C_ID_PIOA);
 }
@@ -195,9 +171,12 @@ void pio_irq_init(void)
 	initialized = 1;
 	AT91F_PIOA_CfgPMC();
 #ifdef USE_FIQ
+/*	This code is not necessary anymore, because fiq_handler is directly part of
+	the vector table, so no jump will happen. 
         AT91F_AIC_ConfigureIt(AT91C_ID_FIQ,
                               //0, AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL, &cdsync_cb);
-                              0, AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL, &my_fiq_handler);
+                              0, AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL, &fiq_handler);
+*/
         /* enable fast forcing for PIOA interrupt */
         *AT91C_AIC_FFER = (1 << AT91C_ID_PIOA);
         
diff --git a/openpicc/os/boot/boot.s b/openpicc/os/boot/boot.s
index a7e9c32..cedd3ee 100644
--- a/openpicc/os/boot/boot.s
+++ b/openpicc/os/boot/boot.s
@@ -182,90 +182,14 @@ endless_loop:
 	ldr   pc, _dabt						/* data abort - _dabt		*/
 	nop									/* reserved					*/
 	ldr   pc, [pc,#-0xF20]				/* IRQ - read the AIC		*/
-	ldr   pc, [pc,#-0xF20]				/* FIQ - read the AIC		*/
+/*	ldr   pc, [pc,#-0xF20]				/* FIQ - fall through to fiq_handler		*/
 
-_undf:  .word __undf                    /* undefined				*/
-_swi:   .word swi_handler				/* SWI						*/
-_pabt:  .word __pabt                    /* program abort			*/
-_dabt:  .word __dabt                    /* data abort				*/
-_fiq:   .word __fiq                     /* FIQ						*/
-
-__undf: b     .                         /* undefined				*/
-__pabt: b     .                         /* program abort			*/
-__dabt: b     .                         /* data abort				*/
-__fiq:  b     .                         /* FIQ						*/
 
-/* Following is from openpcd/firmware/src/start/Cstartup_app.S */
+/* Following is modified from openpcd/firmware/src/start/Cstartup_app.S */
 #define LED_TRIGGER
-/*#define CALL_PIO_IRQ_DEMUX*/
-        
-        .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
-                 */
-
-                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 */
-#ifdef LED_TRIGGER
-                movne   r11, #PIO_LED1
-                strne   r11, [r10, #PIOA_CODR] /* enable LED */
-#endif
-
-#if 1
-                movne   r11, #PIO_DATA
-                strne   r11, [r10, #PIOA_IDR]   /* disable further PIO_DATA FIQ */
-#endif
-
-                /*- Mark the End of Interrupt on the AIC */
-                ldr     r11, =AT91C_BASE_AIC
-                str     r11, [r11, #AIC_EOICR]
-
-#ifdef LED_TRIGGER
-                mov     r11, #PIO_LED1
-                str     r11, [r10, #PIOA_SODR] /* disable LED */
-#endif
-
-#ifdef CALL_PIO_IRQ_DEMUX
-                /* push r0, r1-r3, r12, r14 onto FIQ stack */
-                /*stmfd   sp!, { r0-r3, r12, lr}
-                mov     r0, r8*/
-
-                /* enable interrupts while handling demux */
-                /* 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 }*/
-#endif
-
-                /*- Restore the Program Counter using the LR_fiq directly in the PC */
-                subs        pc, lr, #4
-
-        .size   fiq_handler, . - fiq_handler
-        .endfunc
-
-        .global my_fiq_handler
-        .func my_fiq_handler
 my_fiq_handler:
                 /* code that uses pre-initialized FIQ reg */
                 /* r8   tmp
@@ -315,5 +239,16 @@ my_fiq_handler:
         .size   my_fiq_handler, . - my_fiq_handler
         .endfunc
 
+_undf:  .word __undf                    /* undefined				*/
+_swi:   .word swi_handler				/* SWI						*/
+_pabt:  .word __pabt                    /* program abort			*/
+_dabt:  .word __dabt                    /* data abort				*/
+_fiq:   .word __fiq                     /* FIQ						*/
+
+__undf: b     .                         /* undefined				*/
+__pabt: b     .                         /* program abort			*/
+__dabt: b     .                         /* data abort				*/
+__fiq:  b     .                         /* FIQ						*/
+
         .end