Index: firmware/target/arm/tms320dm320/system-dm320.c
===================================================================
--- firmware/target/arm/tms320dm320/system-dm320.c	(revision 17296)
+++ firmware/target/arm/tms320dm320/system-dm320.c	(working copy)
@@ -78,7 +78,7 @@
 default_interrupt(ARMCOM_TX);
 default_interrupt(RESERVED);
 
-static void (* const irqvector[])(void) =
+static void (* const irqvector[])(void) __attribute__((aligned(8))) =
 {
     TIMER0,TIMER1,TIMER2,TIMER3,CCD_VD0,CCD_VD1,
     CCD_WEN,VENC,SERIAL0,SERIAL1,EXT_HOST,DSPHINT,
@@ -111,16 +111,14 @@
     /*
      * 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 */
-    unsigned short addr = IO_INTC_IRQENTRY0>>2;
-    if(addr != 0)
-    {
-        addr--;
-        irqvector[addr]();
-    }
-    asm volatile(   "add   sp, sp, #8           \n"   /* Cleanup stack   */
+                    "sub   sp, sp, #8           \n"   /* Reserve stack */
+                    "mov    r0, #0x00030000     \n"
+                    "ldr    r1, [r0, #0x518]    \n"   /* Fetch low 16-bits (IO_INTC_IRQENTRY0) */
+                    "ldr    r0, [r0, #0x51A]    \n"   /* Fetch high 13-bits (IO_INTC_IRQENTRY1) */
+                    "orr    r0, r1, r0, lsl #16 \n"   /* Combine loaded values */
+                    "blx    r0                  \n"   /* Jump to address */
+                    "add   sp, sp, #8           \n"   /* Cleanup stack   */
                     "ldmfd sp!, {r0-r7, ip, lr} \n"   /* Restore context */
                     "subs  pc, lr, #4           \n"); /* Return from FIQ */
 }
@@ -135,8 +133,8 @@
         "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"
-        "sub    r0, r0, #1             \r\n"
+        "ldr    r0, [r0, #0x510]      \r\n"
+        "sub    r0, r0, #1            \r\n"
         "ldr    r1, =irqvector        \r\n"
         "ldr    r1, [r1, r0, lsl #2]  \r\n"
         "blx    r1                    \r\n"
@@ -191,13 +189,13 @@
     /* IRQENTRY only reflects enabled interrupts */
     IO_INTC_RAW = 0;
 
-    IO_INTC_ENTRY_TBA0 = 0;
-    IO_INTC_ENTRY_TBA1 = 0;
+    IO_INTC_ENTRY_TBA0 = (((int)irqvector & 0xFFFF) >> 2) << 3;
+    IO_INTC_ENTRY_TBA1 = (int)irqvector >> 16;
     
     unsigned short i;
     /* Reset interrupt priorities to default values */
     for(i = 0; i < 23; i++)
-        DM320_REG(0x0540+i*2) = ( (i*2+1) << 8 ) | i*2 ;//IO_INTC_PRIORITYx
+        DM320_REG(0x0540+i*2) = ( (i*2+1) << 8 ) | i*2 ;/* IO_INTC_PRIORITYx */
     
     /* Turn off all timers */
     IO_TIMER0_TMMD = CONFIG_TIMER0_TMMD_STOP;