From 40dca89e4d363c04754b05ed4a26d348a5b03e5f Mon Sep 17 00:00:00 2001
From: Joakim Eriksson <joakime@sics.se>
Date: Sun, 1 May 2011 12:19:02 +0200
Subject: [PATCH] fixed destination handling for indexed mode to work below and
 above 64KB

---
 se/sics/mspsim/core/MSP430Core.java  | 34 ++++++++++++++++++++++++----
 se/sics/mspsim/core/USARTSource.java |  0
 se/sics/mspsim/core/USCI.java        |  0
 3 files changed, 30 insertions(+), 4 deletions(-)
 mode change 100755 => 100644 se/sics/mspsim/core/USARTSource.java
 mode change 100755 => 100644 se/sics/mspsim/core/USCI.java

diff --git a/se/sics/mspsim/core/MSP430Core.java b/se/sics/mspsim/core/MSP430Core.java
index 575ecc2..d121aa5 100644
--- a/se/sics/mspsim/core/MSP430Core.java
+++ b/se/sics/mspsim/core/MSP430Core.java
@@ -988,6 +988,7 @@ public class MSP430Core extends Chip implements MSP430Constants {
     op = instruction >> 12;
     int sp = 0;
     int sr = 0;
+    int rval = 0; /* register value */
     boolean word = (instruction & 0x40) == 0;
 
     // Destination vars
@@ -1059,8 +1060,20 @@ public class MSP430Core extends Chip implements MSP430Constants {
 	  break;
 	case AM_INDEX:
 	  // TODO: needs to handle if SR is used!
-	  dstAddress = readRegisterCG(dstRegister, ad) + read(pc, MODE_WORD);
-//	  memory[pc] + (memory[pc + 1] << 8);
+	  rval = readRegisterCG(dstRegister, ad);
+
+	  /* Support for MSP430X and below / above 64 KB */
+	  /* if register is pointing to <64KB then it needs to be truncated to below 64 */
+	  if (rval < 0xffff) {
+	      dstAddress = (rval + read(pc, MODE_WORD)) & 0xffff;
+	  } else {
+              dstAddress = read(pc, MODE_WORD);
+              if ((dstAddress & 0x8000) > 0) {
+                  dstAddress |= 0xf0000;
+              }
+              dstAddress += rval;
+              dstAddress &= 0xfffff;
+	  }
 
 	  // When is PC incremented - assuming immediately after "read"?
 	  pc += 2;
@@ -1300,6 +1313,8 @@ public class MSP430Core extends Chip implements MSP430Constants {
 	  srcAddress = readRegisterCG(srcRegister, as) + read(pc, MODE_WORD);
 //	    memory[pc] + (memory[pc + 1] << 8);
 	  // When is PC incremented - assuming immediately after "read"?
+	  
+	  
 	  incRegister(PC, 2);
 	  cycles += dstRegMode ? 3 : 6;
 	  break;
@@ -1344,8 +1359,19 @@ public class MSP430Core extends Chip implements MSP430Constants {
           dstAddress = read(pc, MODE_WORD); //memory[pc] + (memory[pc + 1] << 8);
         } else {
           // CG here - probably not!???
-          dstAddress = readRegister(dstRegister) + read(pc, MODE_WORD);
-//          + memory[pc] + (memory[pc + 1] << 8);
+          rval = readRegister(dstRegister);
+          /* Support for MSP430X and below / above 64 KB */
+          /* if register is pointing to <64KB then it needs to be truncated to below 64 */
+          if (rval < 0xffff) {
+              dstAddress = (rval + read(pc, MODE_WORD)) & 0xffff;
+          } else {
+              dstAddress = read(pc, MODE_WORD);
+              if ((dstAddress & 0x8000) > 0) {
+                  dstAddress |= 0xf0000;
+              }
+              dstAddress += rval;
+              dstAddress &= 0xfffff;
+          }
         }
 
         if (op != MOV)
diff --git a/se/sics/mspsim/core/USARTSource.java b/se/sics/mspsim/core/USARTSource.java
old mode 100755
new mode 100644
diff --git a/se/sics/mspsim/core/USCI.java b/se/sics/mspsim/core/USCI.java
old mode 100755
new mode 100644