Index: system-dm320.c
===================================================================
--- system-dm320.c	(revision 16529)
+++ system-dm320.c	(working copy)
@@ -8,6 +8,7 @@
  * $Id$
  *
  * Copyright (C) 2007 by Karl Kurbjun
+ * Copyright (C) 2008 by Maurus Cuelenaere
  *
  * All files in this archive are subject to the GNU General Public License.
  * See the file COPYING in the source tree root for full license agreement.
@@ -78,7 +79,7 @@
 default_interrupt(ARMCOM_TX);
 default_interrupt(RESERVED);
 
-static void (* const irqvector[])(void) =
+static void (* const irqvector[])(void) __attribute__((aligned(4))) =
 {
     TIMER0,TIMER1,TIMER2,TIMER3,CCD_VD0,CCD_VD1,
     CCD_WEN,VENC,SERIAL0,SERIAL1,EXT_HOST,DSPHINT,
@@ -102,46 +103,57 @@
 
 static void UIRQ(void)
 {
-    unsigned int offset = (IO_INTC_IRQENTRY0>>2)-1;
-    panicf("Unhandled IRQ %02X: %s", offset, irqname[offset]);
+    unsigned int IRQoffset = (IO_INTC_IRQENTRY0 + (IO_INTC_IRQENTRY1 << 16)) - (int)irqvector;
+	 //unsigned int FIQoffset = (IO_INTC_FIQENTRY0 + (IO_INTC_FIQENTRY1 << 16)) - (int)irqvector;
+    panicf("Unhandled IRQ %02X: %s (0x%x and 0x%x)", IRQoffset/4, irqname[(IRQoffset/4)], IO_INTC_IRQENTRY0 + (IO_INTC_IRQENTRY1 << 16), IO_INTC_FIQENTRY0 + (IO_INTC_FIQENTRY1 << 16));
 }
 
 void irq_handler(void)
 {
      * Based on: linux/arch/arm/kernel/entry-armv.S and system-meg-fx.c
      */
 
-    asm volatile(   "stmfd sp!, {r0-r7, ip, lr} \n"   /* Store context */
-                    "sub   sp, sp, #8           \n"); /* Reserve stack */
-    irqvector[(IO_INTC_IRQENTRY0>>2)-1]();
-    asm volatile(   "add   sp, sp, #8           \n"   /* Cleanup stack   */
-                    "ldmfd sp!, {r0-r7, ip, lr} \n"   /* Restore context */
-                    "subs  pc, lr, #4           \n"); /* Return from FIQ */
+    asm volatile("stmfd sp!, {r0-r7, ip, lr}\n"   /* Store context */
+                 "sub   sp, sp, #8          \n" /* Reserve stack */
+                 "LDR   R3, =0x30518        \n"
+                 "LDRH  R2, [R3]            \n"
+                 "MOV   R2, R2,LSL#16       \n"
+                 "MOV   R2, R2,LSR#16       \n"
+                 "ADD   R3, R3, #2          \n"
+                 "LDRH  R3, [R3]            \n"
+                 "MOV   R3, R3,LSL#16       \n"
+                 "LDR   R3, [R2,R3]         \n"
+                 "BLX   R3                  \n"
+                 "add   sp, sp, #8          \n"   /* Cleanup stack   */
+                 "ldmfd sp!, {r0-r7, ip, lr}\n"   /* Restore context */
+                 "subs  pc, lr, #4          \n"); /* Return from IRQ */
 }
 
 void fiq_handler(void)
 {
      * Based on: linux/arch/arm/kernel/entry-armv.S and system-meg-fx.c
      */
 
     asm volatile (
         "sub    lr, lr, #4            \r\n"
         "stmfd  sp!, {r0-r3, ip, lr}  \r\n"
-        "mov    r0, #0x00030000       \r\n"
-        "ldr    r0, [r0, #0x518]       \r\n"
-        "ldr    r1, =irqvector        \r\n"
-        "ldr    r1, [r1, r0, lsl #2]  \r\n"
-        "mov    lr, pc                \r\n"
-        "bx     r1                    \r\n"
+		"LDR   R3, =0x30510		        \n"
+		"LDRH  R2, [R3]			        \n"
+		"MOV   R2, R2,LSL#16		    \n"
+		"MOV   R2, R2,LSR#16		    \n"
+		"ADD   R3, R3, #2			    \n"
+		"LDRH  R3, [R3]			        \n"
+		"MOV   R3, R3,LSL#16		    \n"
+		"LDR   R3, [R2,R3]			    \n"
+		"BLX   R3					    \n"
         "ldmfd  sp!, {r0-r3, ip, pc}^ \r\n"
     );
 }
 
 void system_reboot(void)
 {
-
 }
 
 void system_init(void)
@@ -162,21 +174,28 @@
     IO_INTC_EINT1 = 0;
     IO_INTC_EINT2 = 0;
 
-    /* Setting INTC to all IRQs. */
+    /* Setting all INTC to IRQs. */
     IO_INTC_FISEL0 = 0;
     IO_INTC_FISEL1 = 0;
     IO_INTC_FISEL2 = 0;
 
-    IO_INTC_ENTRY_TBA0 = 0;
-    IO_INTC_ENTRY_TBA1 = 0;
+    /* Setting IRQ base address to 0 and size to 4 bytes */
+    IO_INTC_ENTRY_TBA0 = ((int)irqvector & 0xFFFF) & ~1;
+    IO_INTC_ENTRY_TBA1 = (int)irqvector >> 16;
+	
+    /* IRQENTRY only reflects enabled interrupts */
+    IO_INTC_RAW = 0;
     
-    /* Turn off other timers */
+    /* Turn off all timers */
+    IO_TIMER0_TMMD = CONFIG_TIMER0_TMMD_STOP;
+    IO_TIMER1_TMMD = CONFIG_TIMER1_TMMD_STOP;
     IO_TIMER2_TMMD = CONFIG_TIMER2_TMMD_STOP;
     IO_TIMER3_TMMD = CONFIG_TIMER3_TMMD_STOP;
	 
     /* set GIO26 (reset pin) to output and low */
     IO_GIO_BITCLR1=(1<<10);
     IO_GIO_DIR1&=~(1<<10);
 
     uart_init();
     spi_init();