From 2f32db41cb8f8242727f2c4ed2626684752c49dd Mon Sep 17 00:00:00 2001
From: cyberwarriorx <cyberwarriorx@users.sf.net>
Date: Sat, 23 Dec 2006 00:52:11 +0000
Subject: [PATCH] -Added some SWI changes Mighty Max sent me. Relocating SWI
 routines should work now.

---
 desmume/src/arm_instructions.c   | 22 ++++++++++++++++++++--
 desmume/src/thumb_instructions.c | 21 +++++++++++++++++++--
 2 files changed, 39 insertions(+), 4 deletions(-)

diff --git a/desmume/src/arm_instructions.c b/desmume/src/arm_instructions.c
index 2828dc10f..aec1c4624 100644
--- a/desmume/src/arm_instructions.c
+++ b/desmume/src/arm_instructions.c
@@ -7773,8 +7773,26 @@ static u32 FASTCALL  OP_MRC(armcpu_t *cpu)
 //--------------SWI-------------------------------
 static u32 FASTCALL  OP_SWI(armcpu_t *cpu)
 {
-     u32 swinum = (cpu->instruction>>16)&0x1F;
-     return cpu->swi_tab[swinum](cpu) + 3;  
+     if (((cpu->intVector != 0) ^ (cpu->proc_ID == ARMCPU_ARM9)))
+     {
+        /* TODO (#1#): translocated SWI vectors */
+        /* we use an irq thats not in the irq tab, as
+           it was replaced duie to a changed intVector */
+        Status_Reg tmp = cpu->CPSR;
+        armcpu_switchMode(cpu, SVC);            /* enter svc mode */
+        cpu->R[14] = cpu->R[15] - 4;            /* jump to swi Vector */
+        cpu->SPSR = tmp;                        /* save old CPSR as new SPSR */
+        cpu->CPSR.bits.T = 0;                   /* handle as ARM32 code */
+        cpu->CPSR.bits.I = cpu->SPSR.bits.I;    /* keep int disable flag */
+        cpu->R[15] = cpu->intVector + 0x08;
+        cpu->next_instruction = cpu->R[15];
+        return 4;
+     }
+     else
+     {
+        u32 swinum = (cpu->instruction>>16)&0x1F;
+        return cpu->swi_tab[swinum](cpu) + 3;
+     }
 }
 
 //----------------BKPT-------------------------
diff --git a/desmume/src/thumb_instructions.c b/desmume/src/thumb_instructions.c
index 5af2db9b6..2eeaff6c8 100644
--- a/desmume/src/thumb_instructions.c
+++ b/desmume/src/thumb_instructions.c
@@ -861,8 +861,25 @@ static u32 FASTCALL OP_B_COND(armcpu_t *cpu)
 
 static u32 FASTCALL OP_SWI_THUMB(armcpu_t *cpu)
 {
-     u32 swinum = cpu->instruction & 0xFF;
-     return cpu->swi_tab[swinum](cpu) + 3;  
+     if (((cpu->intVector != 0) ^ (cpu->proc_ID == ARMCPU_ARM9)))
+     {
+        /* we use an irq thats not in the irq tab, as
+        it was replaced duie to a changed intVector */
+        Status_Reg tmp = cpu->CPSR;
+        armcpu_switchMode(cpu, SVC);            /* enter svc mode */
+        cpu->R[14] = cpu->R[15] - 4;            /* jump to swi Vector */
+        cpu->SPSR = tmp;                        /* save old CPSR as new SPSR */
+        cpu->CPSR.bits.T = 0;                   /* handle as ARM32 code */
+        cpu->CPSR.bits.I = cpu->SPSR.bits.I;    /* keep int disable flag */
+        cpu->R[15] = cpu->intVector + 0x08;
+        cpu->next_instruction = cpu->R[15];
+        return 3;
+     }
+     else
+     {
+        u32 swinum = cpu->instruction & 0xFF;
+        return cpu->swi_tab[swinum](cpu) + 3;  
+     }
      //return 3;
 }