1 module gbaid.gba.thumb; 2 3 import core.bitop : popcnt; 4 5 import std..string : format; 6 7 import gbaid.util; 8 9 import gbaid.gba.instable; 10 import gbaid.gba.assembly; 11 12 private enum THUMB_OPCODE_BIT_COUNT = 10; 13 private immutable Executor[1 << THUMB_OPCODE_BIT_COUNT] THUMB_EXECUTORS = createTHUMBTable(); 14 15 public void executeTHUMBInstruction(Registers* registers, MemoryBus* memory, int instruction) { 16 int code = instruction.getBits(6, 15); 17 THUMB_EXECUTORS[code](registers, memory, instruction); 18 } 19 20 private Executor[] createTHUMBTable() { 21 /* 22 23 The instruction encoding, modified from: http://problemkaputt.de/gbatek.htm#thumbinstructionsummary 24 25 Form|_15|_14|_13|_12|_11|_10|_9_|_8_|_7_|_6_|_5_|_4_|_3_|_2_|_1_|_0_| 26 __1_|_0___0___0_|__Op___|_______Offset______|____Rs_____|____Rd_____|Shifted 27 __2_|_0___0___0___1___1_|_I,_Op_|___Rn/nn___|____Rs_____|____Rd_____|ADD/SUB 28 __3_|_0___0___1_|__Op___|____Rd_____|_____________Offset____________|Immedi. 29 __4_|_0___1___0___0___0___0_|______Op_______|____Rs_____|____Rd_____|AluOp 30 __5_|_0___1___0___0___0___1_|__Op___|Hd_|Hs_|____Rs_____|____Rd_____|HiReg/BX 31 __6_|_0___1___0___0___1_|____Rd_____|_____________Word______________|LDR PC 32 __7_|_0___1___0___1_|__Op___|_0_|___Ro______|____Rb_____|____Rd_____|LDR/STR 33 __8_|_0___1___0___1_|__Op___|_1_|___Ro______|____Rb_____|____Rd_____|LDR/STR{H/SB/SH} 34 __9_|_0___1___1_|__Op___|_______Offset______|____Rb_____|____Rd_____|LDR/STR{B} 35 _10_|_1___0___0___0_|Op_|_______Offset______|____Rb_____|____Rd_____|LDRH/STRH 36 _11_|_1___0___0___1_|Op_|____Rd_____|_____________Word______________|LDR/STR SP 37 _12_|_1___0___1___0_|Op_|____Rd_____|_____________Word______________|ADD PC/SP 38 _13_|_1___0___1___1___0___0___0___0_|_S_|___________Word____________|ADD SP,nn 39 _14_|_1___0___1___1_|Op_|_1___0_|_R_|____________Rlist______________|PUSH/POP 40 _15_|_1___1___0___0_|Op_|____Rb_____|____________Rlist______________|STM/LDM 41 _16_|_1___1___0___1_|_____Cond______|_________Signed_Offset_________|B{cond} 42 _17_|_1___1___0___1___1___1___1___1_|___________User_Data___________|SWI 43 _18_|_1___1___1___0___0_|________________Offset_____________________|B 44 _19_|_1___1___1___1_|_H_|______________Offset_Low/High______________|BL,BLX 45 46 The op code is bits 6 to 15 47 For some instructions some of these bits are not used, hence the need for don't cares 48 Anything not covered by the table must raise an UNDEFINED interrupt 49 50 */ 51 52 auto table = createTable!(unsupported)(THUMB_OPCODE_BIT_COUNT); 53 54 // Bits are OpCode(2) 55 addSubTable!("000ttddddd", moveShiftedRegister, unsupported)(table); 56 57 // Bits are I(1),S(1) 58 // where I is immediate and S is subtract 59 addSubTable!("00011ttddd", addAndSubtract, unsupported)(table); 60 61 // Bits are OpCode(2) 62 addSubTable!("001ttddddd", moveCompareAddAndSubtractImmediate, unsupported)(table); 63 64 // Bits are OpCode(4) 65 addSubTable!("010000tttt", aluOperations, unsupported)(table); 66 67 // Bits are OpCode(2),HD(1),HS(1) 68 // where HD is high destination and HS is high source 69 addSubTable!("010001tttt", hiRegisterOperationsAndBranchExchange, unsupported)(table); 70 71 // No bits 72 addSubTable!("01001ddddd", loadPCRelative, unsupported)(table); 73 74 // Bits are OpCode(2) 75 addSubTable!("0101tt0ddd", loadAndStoreWithRegisterOffset, unsupported)(table); 76 77 // Bits are OpCode(2) 78 addSubTable!("0101tt1ddd", loadAndStoreSignExtentedByteAndHalfword, unsupported)(table); 79 80 // Bits are OpCode(2) 81 addSubTable!("011ttddddd", loadAndStoreWithImmediateOffset, unsupported)(table); 82 83 // Bits are OpCode(1) 84 addSubTable!("1000tddddd", loadAndStoreHalfWord, unsupported)(table); 85 86 // Bits are OpCode(1) 87 addSubTable!("1001tddddd", loadAndStoreSPRelative, unsupported)(table); 88 89 // Bits are OpCode(1) 90 addSubTable!("1010tddddd", getRelativeAddresss, unsupported)(table); 91 92 // Bits are S(1) 93 // where S is subtract 94 addSubTable!("10110000td", addOffsetToStackPointer, unsupported)(table); 95 96 // Bits are Pop(1),R(1) 97 // where Pop is pop of the stack and R is include PC or LR 98 addSubTable!("1011t10tdd", pushAndPopRegisters, unsupported)(table); 99 100 // Bits are L(1) 101 // where L is load 102 addSubTable!("1100tddddd", multipleLoadAndStore, unsupported)(table); 103 104 // Bits are C(4) 105 // where C is condition code 106 addSubTable!("1101ttttdd", conditionalBranch, unsupported)(table); 107 108 // No bits 109 addSubTable!("11011111dd", softwareInterrupt, unsupported)(table); 110 111 // No bits 112 addSubTable!("11100ddddd", unconditionalBranch, unsupported)(table); 113 114 // Bits are H(1) 115 // where H is high 116 addSubTable!("1111tddddd", longBranchWithLink, unsupported)(table); 117 118 return table; 119 } 120 121 private void moveShiftedRegister(int code)(Registers* registers, MemoryBus* memory, int instruction) 122 if (code >= 0 && code <= 2) { 123 ubyte shift = cast(ubyte) instruction.getBits(6, 10); 124 int op = registers.get(instruction.getBits(3, 5)); 125 int rd = instruction & 0b111; 126 static if (code == 0) { 127 debug (outputInstructions) registers.logInstruction(instruction, "LSL"); 128 } else static if (code == 1) { 129 debug (outputInstructions) registers.logInstruction(instruction, "LSR"); 130 } else { 131 debug (outputInstructions) registers.logInstruction(instruction, "ASR"); 132 } 133 int carry; 134 op = registers.applyShift!false(code, shift, op, carry); 135 registers.set(rd, op); 136 registers.setApsrFlags(op < 0, op == 0, carry); 137 } 138 139 @("unsupported") 140 private template moveShiftedRegister(int code: 3) { 141 private alias moveShiftedRegister = unsupported; 142 } 143 144 private template addAndSubtract(int code) if (code.getBits(2, 31) == 0) { 145 private alias addAndSubtract = addAndSubtract!(code.checkBit(1), code.checkBit(0)); 146 } 147 148 private void addAndSubtract(bool immediate, bool subtract)(Registers* registers, MemoryBus* memory, int instruction) { 149 int rn = instruction.getBits(6, 8); 150 static if (immediate) { 151 // immediate 152 int op2 = rn; 153 } else { 154 // register 155 int op2 = registers.get(rn); 156 } 157 int op1 = registers.get(instruction.getBits(3, 5)); 158 int rd = instruction & 0b111; 159 static if (subtract) { 160 // SUB 161 debug (outputInstructions) registers.logInstruction(instruction, "SUB"); 162 static if (__traits(compiles, SUB_WITH_FLAGS_ASM)) { 163 int res = void; 164 int flags = void; 165 mixin (SUB_WITH_FLAGS_ASM); 166 registers.setApsrFlagsPacked(flags); 167 } else { 168 int res = op1 - op2; 169 int carry = !borrowedSub(op1, op2, res); 170 int overflow = overflowedSub(op1, op2, res); 171 registers.setApsrFlags(res < 0, res == 0, carry, overflow); 172 } 173 } else { 174 // ADD 175 debug (outputInstructions) registers.logInstruction(instruction, "ADD"); 176 static if (__traits(compiles, ADD_WITH_FLAGS_ASM)) { 177 int res = void; 178 int flags = void; 179 mixin (ADD_WITH_FLAGS_ASM); 180 registers.setApsrFlagsPacked(flags); 181 } else { 182 int res = op1 + op2; 183 int carry = carriedAdd(op1, op2, res); 184 int overflow = overflowedAdd(op1, op2, res); 185 registers.setApsrFlags(res < 0, res == 0, carry, overflow); 186 } 187 } 188 registers.set(rd, res); 189 } 190 191 private mixin template decodeOpMoveCompareAddAndSubtractImmediate(bool op1) { 192 int rd = instruction.getBits(8, 10); 193 int op2 = instruction & 0xFF; 194 static if (op1) { 195 int op1 = registers.get(rd); 196 } 197 } 198 199 private void moveCompareAddAndSubtractImmediate(int code: 0)(Registers* registers, MemoryBus* memory, int instruction) { 200 debug (outputInstructions) registers.logInstruction(instruction, "MOV"); 201 mixin decodeOpMoveCompareAddAndSubtractImmediate!false; 202 registers.set(rd, op2); 203 registers.setApsrFlags(op2 < 0, op2 == 0); 204 } 205 206 private void moveCompareAddAndSubtractImmediate(int code: 1)(Registers* registers, MemoryBus* memory, int instruction) { 207 debug (outputInstructions) registers.logInstruction(instruction, "CMP"); 208 mixin decodeOpMoveCompareAddAndSubtractImmediate!true; 209 static if (__traits(compiles, SUB_WITH_FLAGS_ASM)) { 210 int res = void; 211 int flags = void; 212 mixin (SUB_WITH_FLAGS_ASM); 213 registers.setApsrFlagsPacked(flags); 214 } else { 215 int v = op1 - op2; 216 registers.setApsrFlags(v < 0, v == 0, !borrowedSub(op1, op2, v), overflowedSub(op1, op2, v)); 217 } 218 } 219 220 private void moveCompareAddAndSubtractImmediate(int code: 2)(Registers* registers, MemoryBus* memory, int instruction) { 221 debug (outputInstructions) registers.logInstruction(instruction, "ADD"); 222 mixin decodeOpMoveCompareAddAndSubtractImmediate!true; 223 static if (__traits(compiles, ADD_WITH_FLAGS_ASM)) { 224 int res = void; 225 int flags = void; 226 mixin (ADD_WITH_FLAGS_ASM); 227 registers.setApsrFlagsPacked(flags); 228 } else { 229 int res = op1 + op2; 230 registers.setApsrFlags(res < 0, res == 0, carriedAdd(op1, op2, res), overflowedAdd(op1, op2, res)); 231 } 232 registers.set(rd, res); 233 } 234 235 private void moveCompareAddAndSubtractImmediate(int code: 3)(Registers* registers, MemoryBus* memory, int instruction) { 236 debug (outputInstructions) registers.logInstruction(instruction, "SUB"); 237 mixin decodeOpMoveCompareAddAndSubtractImmediate!true; 238 static if (__traits(compiles, SUB_WITH_FLAGS_ASM)) { 239 int res = void; 240 int flags = void; 241 mixin (SUB_WITH_FLAGS_ASM); 242 registers.setApsrFlagsPacked(flags); 243 } else { 244 int res = op1 - op2; 245 registers.setApsrFlags(res < 0, res == 0, !borrowedSub(op1, op2, res), overflowedSub(op1, op2, res)); 246 } 247 registers.set(rd, res); 248 } 249 250 private mixin template decodeOpAluOperations() { 251 int op2 = registers.get(instruction.getBits(3, 5)); 252 int rd = instruction & 0b111; 253 int op1 = registers.get(rd); 254 } 255 256 private void aluOperations(int code: 0)(Registers* registers, MemoryBus* memory, int instruction) { 257 debug (outputInstructions) registers.logInstruction(instruction, "AND"); 258 mixin decodeOpAluOperations; 259 int res = op1 & op2; 260 registers.set(rd, res); 261 registers.setApsrFlags(res < 0, res == 0); 262 } 263 264 private void aluOperations(int code: 1)(Registers* registers, MemoryBus* memory, int instruction) { 265 debug (outputInstructions) registers.logInstruction(instruction, "EOR"); 266 mixin decodeOpAluOperations; 267 int res = op1 ^ op2; 268 registers.set(rd, res); 269 registers.setApsrFlags(res < 0, res == 0); 270 } 271 272 private void aluOperationsShift(int type)(Registers* registers, MemoryBus* memory, int instruction) { 273 static if (type == 0) { 274 debug (outputInstructions) registers.logInstruction(instruction, "LSL"); 275 } else static if (type == 1) { 276 debug (outputInstructions) registers.logInstruction(instruction, "LSR"); 277 } else static if (type == 2) { 278 debug (outputInstructions) registers.logInstruction(instruction, "ASR"); 279 } else static if (type == 3) { 280 debug (outputInstructions) registers.logInstruction(instruction, "ROR"); 281 } else { 282 static assert (0); 283 } 284 mixin decodeOpAluOperations; 285 int carry; 286 int res = registers.applyShift!true(type, cast(ubyte) op2, op1, carry); 287 registers.set(rd, res); 288 registers.setApsrFlags(res < 0, res == 0, carry); 289 } 290 291 // LSL 292 private alias aluOperations(int code: 2) = aluOperationsShift!0; 293 294 // LSR 295 private alias aluOperations(int code: 3) = aluOperationsShift!1; 296 297 // ASR 298 private alias aluOperations(int code: 4) = aluOperationsShift!2; 299 300 // ROR 301 private alias aluOperations(int code: 7) = aluOperationsShift!3; 302 303 private void aluOperations(int code: 5)(Registers* registers, MemoryBus* memory, int instruction) { 304 debug (outputInstructions) registers.logInstruction(instruction, "ADC"); 305 mixin decodeOpAluOperations; 306 int op3 = registers.getFlag(CPSRFlag.C); 307 int tmp = op1 + op2; 308 int res = tmp + op3; 309 registers.set(rd, res); 310 registers.setApsrFlags(res < 0, res == 0, 311 carriedAdd(op1, op2, tmp) || carriedAdd(tmp, op3, res), 312 overflowedAdd(op1, op2, tmp) || overflowedAdd(tmp, op3, res)); 313 } 314 315 private void aluOperations(int code: 6)(Registers* registers, MemoryBus* memory, int instruction) { 316 debug (outputInstructions) registers.logInstruction(instruction, "SBC"); 317 mixin decodeOpAluOperations; 318 int op3 = !registers.getFlag(CPSRFlag.C); 319 int tmp = op1 - op2; 320 int res = tmp - op3; 321 registers.set(rd, res); 322 registers.setApsrFlags(res < 0, res == 0, 323 !borrowedSub(op1, op2, tmp) && !borrowedSub(tmp, op3, res), 324 overflowedSub(op1, op2, tmp) || overflowedSub(tmp, op3, res)); 325 } 326 327 private void aluOperations(int code: 8)(Registers* registers, MemoryBus* memory, int instruction) { 328 debug (outputInstructions) registers.logInstruction(instruction, "TST"); 329 mixin decodeOpAluOperations; 330 int v = op1 & op2; 331 registers.setApsrFlags(v < 0, v == 0); 332 } 333 334 private void aluOperations(int code: 9)(Registers* registers, MemoryBus* memory, int instruction) { 335 debug (outputInstructions) registers.logInstruction(instruction, "NEG"); 336 mixin decodeOpAluOperations; 337 static if (__traits(compiles, SUB_WITH_FLAGS_ASM)) { 338 int op1 = 0; 339 int res = void; 340 int flags = void; 341 mixin (SUB_WITH_FLAGS_ASM); 342 registers.setApsrFlagsPacked(flags); 343 } else { 344 int res = 0 - op2; 345 registers.setApsrFlags(res < 0, res == 0, !borrowedSub(0, op2, res), overflowedSub(0, op2, res)); 346 } 347 registers.set(rd, res); 348 } 349 350 private void aluOperations(int code: 10)(Registers* registers, MemoryBus* memory, int instruction) { 351 debug (outputInstructions) registers.logInstruction(instruction, "CMP"); 352 mixin decodeOpAluOperations; 353 static if (__traits(compiles, SUB_WITH_FLAGS_ASM)) { 354 int res = void; 355 int flags = void; 356 mixin (SUB_WITH_FLAGS_ASM); 357 registers.setApsrFlagsPacked(flags); 358 } else { 359 int v = op1 - op2; 360 registers.setApsrFlags(v < 0, v == 0, !borrowedSub(op1, op2, v), overflowedSub(op1, op2, v)); 361 } 362 } 363 364 private void aluOperations(int code: 11)(Registers* registers, MemoryBus* memory, int instruction) { 365 debug (outputInstructions) registers.logInstruction(instruction, "CMN"); 366 mixin decodeOpAluOperations; 367 static if (__traits(compiles, ADD_WITH_FLAGS_ASM)) { 368 int res = void; 369 int flags = void; 370 mixin (ADD_WITH_FLAGS_ASM); 371 registers.setApsrFlagsPacked(flags); 372 } else { 373 int v = op1 + op2; 374 registers.setApsrFlags(v < 0, v == 0, carriedAdd(op1, op2, v), overflowedAdd(op1, op2, v)); 375 } 376 } 377 378 private void aluOperations(int code: 12)(Registers* registers, MemoryBus* memory, int instruction) { 379 debug (outputInstructions) registers.logInstruction(instruction, "ORR"); 380 mixin decodeOpAluOperations; 381 int res = op1 | op2; 382 registers.set(rd, res); 383 registers.setApsrFlags(res < 0, res == 0); 384 } 385 386 private void aluOperations(int code: 13)(Registers* registers, MemoryBus* memory, int instruction) { 387 debug (outputInstructions) registers.logInstruction(instruction, "MUL"); 388 mixin decodeOpAluOperations; 389 int res = op1 * op2; 390 registers.set(rd, res); 391 registers.setApsrFlags(res < 0, res == 0); 392 } 393 394 private void aluOperations(int code: 14)(Registers* registers, MemoryBus* memory, int instruction) { 395 debug (outputInstructions) registers.logInstruction(instruction, "BIC"); 396 mixin decodeOpAluOperations; 397 int res = op1 & ~op2; 398 registers.set(rd, res); 399 registers.setApsrFlags(res < 0, res == 0); 400 } 401 402 private void aluOperations(int code: 15)(Registers* registers, MemoryBus* memory, int instruction) { 403 debug (outputInstructions) registers.logInstruction(instruction, "MNV"); 404 mixin decodeOpAluOperations; 405 int res = ~op2; 406 registers.set(rd, res); 407 registers.setApsrFlags(res < 0, res == 0); 408 } 409 410 private mixin template decodeOpHiRegisterOperationsAndBranchExchange(bool highDestination, bool highSource) { 411 static if (highSource) { 412 int rs = instruction.getBits(3, 5) | 0b1000; 413 } else { 414 int rs = instruction.getBits(3, 5); 415 } 416 static if (highDestination) { 417 int rd = instruction & 0b111 | 0b1000; 418 } else { 419 int rd = instruction & 0b111; 420 } 421 } 422 423 private template hiRegisterOperationsAndBranchExchange(int code) if (code.getBits(4, 31) == 0) { 424 private alias hiRegisterOperationsAndBranchExchange = 425 hiRegisterOperationsAndBranchExchange!(code.getBits(2, 3), code.checkBit(1), code.checkBit(0)); 426 } 427 428 private void hiRegisterOperationsAndBranchExchange(int opCode, bool highDestination, bool highSource) 429 (Registers* registers, MemoryBus* memory, int instruction) 430 if (opCode == 0 && (highDestination || highSource)) { 431 debug (outputInstructions) registers.logInstruction(instruction, "ADD"); 432 mixin decodeOpHiRegisterOperationsAndBranchExchange!(highDestination, highSource); 433 registers.set(rd, registers.get(rd) + registers.get(rs)); 434 } 435 436 private void hiRegisterOperationsAndBranchExchange(int opCode, bool highDestination, bool highSource) 437 (Registers* registers, MemoryBus* memory, int instruction) 438 if (opCode == 1 && (highDestination || highSource)) { 439 debug (outputInstructions) registers.logInstruction(instruction, "CMP"); 440 mixin decodeOpHiRegisterOperationsAndBranchExchange!(highDestination, highSource); 441 int op1 = registers.get(rd); 442 int op2 = registers.get(rs); 443 static if (__traits(compiles, SUB_WITH_FLAGS_ASM)) { 444 int res = void; 445 int flags = void; 446 mixin (SUB_WITH_FLAGS_ASM); 447 registers.setApsrFlagsPacked(flags); 448 } else { 449 int v = op1 - op2; 450 registers.setApsrFlags(v < 0, v == 0, !borrowedSub(op1, op2, v), overflowedSub(op1, op2, v)); 451 } 452 } 453 454 private void hiRegisterOperationsAndBranchExchange(int opCode, bool highDestination, bool highSource) 455 (Registers* registers, MemoryBus* memory, int instruction) 456 if (opCode == 2 && (highDestination || highSource)) { 457 debug (outputInstructions) registers.logInstruction(instruction, "MOV"); 458 mixin decodeOpHiRegisterOperationsAndBranchExchange!(highDestination, highSource); 459 registers.set(rd, registers.get(rs)); 460 } 461 462 private void hiRegisterOperationsAndBranchExchange(int opCode, bool highDestination, bool highSource) 463 (Registers* registers, MemoryBus* memory, int instruction) 464 if (opCode == 3 && !highDestination) { 465 debug (outputInstructions) registers.logInstruction(instruction, "BX"); 466 mixin decodeOpHiRegisterOperationsAndBranchExchange!(highDestination, highSource); 467 int address = registers.get(rs); 468 if (!(address & 0b1)) { 469 registers.setFlag(CPSRFlag.T, Set.ARM); 470 } 471 registers.setPC(address & ~1); 472 } 473 474 @("unsupported") 475 private template hiRegisterOperationsAndBranchExchange(int opCode, bool highDestination, bool highSource) 476 if (opCode >= 0 && opCode <= 2 && !highDestination && !highSource || 477 opCode == 3 && highDestination) { 478 private alias hiRegisterOperationsAndBranchExchange = unsupported; 479 } 480 481 private void loadPCRelative()(Registers* registers, MemoryBus* memory, int instruction) { 482 debug (outputInstructions) registers.logInstruction(instruction, "LDR"); 483 int rd = getBits(instruction, 8, 10); 484 int offset = (instruction & 0xFF) * 4; 485 int pc = registers.getPC(); 486 int address = (pc & ~3) + offset; 487 registers.set(rd, address.rotateRead(memory.get!int(address))); 488 } 489 490 private mixin template decodeOpLoadAndStoreWithRegisterOffset() { 491 int offset = registers.get(instruction.getBits(6, 8)); 492 int base = registers.get(instruction.getBits(3, 5)); 493 int rd = instruction & 0b111; 494 int address = base + offset; 495 } 496 497 private void loadAndStoreWithRegisterOffset(int code: 0)(Registers* registers, MemoryBus* memory, int instruction) { 498 debug (outputInstructions) registers.logInstruction(instruction, "STR"); 499 mixin decodeOpLoadAndStoreWithRegisterOffset; 500 memory.set!int(address, registers.get(rd)); 501 } 502 503 private void loadAndStoreWithRegisterOffset(int code: 1)(Registers* registers, MemoryBus* memory, int instruction) { 504 debug (outputInstructions) registers.logInstruction(instruction, "STRB"); 505 mixin decodeOpLoadAndStoreWithRegisterOffset; 506 memory.set!byte(address, cast(byte) registers.get(rd)); 507 } 508 509 private void loadAndStoreWithRegisterOffset(int code: 2)(Registers* registers, MemoryBus* memory, int instruction) { 510 debug (outputInstructions) registers.logInstruction(instruction, "LDR"); 511 mixin decodeOpLoadAndStoreWithRegisterOffset; 512 registers.set(rd, address.rotateRead(memory.get!int(address))); 513 } 514 515 private void loadAndStoreWithRegisterOffset(int code: 3)(Registers* registers, MemoryBus* memory, int instruction) { 516 debug (outputInstructions) registers.logInstruction(instruction, "LDRB"); 517 mixin decodeOpLoadAndStoreWithRegisterOffset; 518 registers.set(rd, memory.get!byte(address) & 0xFF); 519 } 520 521 private void loadAndStoreSignExtentedByteAndHalfword(int code: 0)(Registers* registers, MemoryBus* memory, int instruction) { 522 debug (outputInstructions) registers.logInstruction(instruction, "STRH"); 523 mixin decodeOpLoadAndStoreWithRegisterOffset; 524 memory.set!short(address, cast(short) registers.get(rd)); 525 } 526 527 private void loadAndStoreSignExtentedByteAndHalfword(int code: 1)(Registers* registers, MemoryBus* memory, int instruction) { 528 debug (outputInstructions) registers.logInstruction(instruction, "LDSB"); 529 mixin decodeOpLoadAndStoreWithRegisterOffset; 530 registers.set(rd, memory.get!byte(address)); 531 } 532 533 private void loadAndStoreSignExtentedByteAndHalfword(int code: 2)(Registers* registers, MemoryBus* memory, int instruction) { 534 debug (outputInstructions) registers.logInstruction(instruction, "LDRH"); 535 mixin decodeOpLoadAndStoreWithRegisterOffset; 536 registers.set(rd, address.rotateRead(memory.get!short(address))); 537 } 538 539 private void loadAndStoreSignExtentedByteAndHalfword(int code: 3)(Registers* registers, MemoryBus* memory, int instruction) { 540 debug (outputInstructions) registers.logInstruction(instruction, "LDSH"); 541 mixin decodeOpLoadAndStoreWithRegisterOffset; 542 registers.set(rd, address.rotateReadSigned(memory.get!short(address))); 543 } 544 545 private mixin template decodeOpLoadAndStoreWithImmediateOffset() { 546 int offset = instruction.getBits(6, 10); 547 int base = registers.get(instruction.getBits(3, 5)); 548 int rd = instruction & 0b111; 549 } 550 551 private void loadAndStoreWithImmediateOffset(int code: 0)(Registers* registers, MemoryBus* memory, int instruction) { 552 debug (outputInstructions) registers.logInstruction(instruction, "STR"); 553 mixin decodeOpLoadAndStoreWithImmediateOffset; 554 int address = base + offset * 4; 555 memory.set!int(address, registers.get(rd)); 556 } 557 558 private void loadAndStoreWithImmediateOffset(int code: 1)(Registers* registers, MemoryBus* memory, int instruction) { 559 debug (outputInstructions) registers.logInstruction(instruction, "LDR"); 560 mixin decodeOpLoadAndStoreWithImmediateOffset; 561 int address = base + offset * 4; 562 registers.set(rd, address.rotateRead(memory.get!int(address))); 563 } 564 565 private void loadAndStoreWithImmediateOffset(int code: 2)(Registers* registers, MemoryBus* memory, int instruction) { 566 debug (outputInstructions) registers.logInstruction(instruction, "STRB"); 567 mixin decodeOpLoadAndStoreWithImmediateOffset; 568 int address = base + offset; 569 memory.set!byte(address, cast(byte) registers.get(rd)); 570 } 571 572 private void loadAndStoreWithImmediateOffset(int code: 3)(Registers* registers, MemoryBus* memory, int instruction) { 573 debug (outputInstructions) registers.logInstruction(instruction, "LDRB"); 574 mixin decodeOpLoadAndStoreWithImmediateOffset; 575 int address = base + offset; 576 registers.set(rd, memory.get!byte(address) & 0xFF); 577 } 578 579 private void loadAndStoreHalfWord(int code: 0)(Registers* registers, MemoryBus* memory, int instruction) { 580 debug (outputInstructions) registers.logInstruction(instruction, "STRH"); 581 mixin decodeOpLoadAndStoreWithImmediateOffset; 582 int address = base + offset * 2; 583 memory.set!short(address, cast(short) registers.get(rd)); 584 } 585 586 private void loadAndStoreHalfWord(int code: 1)(Registers* registers, MemoryBus* memory, int instruction) { 587 debug (outputInstructions) registers.logInstruction(instruction, "LDRH"); 588 mixin decodeOpLoadAndStoreWithImmediateOffset; 589 int address = base + offset * 2; 590 registers.set(rd, address.rotateRead(memory.get!short(address))); 591 } 592 593 private template loadAndStoreSPRelative(int code) if (code.getBits(1, 31) == 0) { 594 private alias loadAndStoreSPRelative = loadAndStoreSPRelative!(code.checkBit(0)); 595 } 596 597 private void loadAndStoreSPRelative(bool load)(Registers* registers, MemoryBus* memory, int instruction) { 598 int rd = instruction.getBits(8, 10); 599 int offset = (instruction & 0xFF) * 4; 600 int sp = registers.get(Register.SP); 601 int address = sp + offset; 602 static if (load) { 603 debug (outputInstructions) registers.logInstruction(instruction, "LDR"); 604 registers.set(rd, address.rotateRead(memory.get!int(address))); 605 } else { 606 debug (outputInstructions) registers.logInstruction(instruction, "STR"); 607 memory.set!int(address, registers.get(rd)); 608 } 609 } 610 611 private template getRelativeAddresss(int code) if (code.getBits(1, 31) == 0) { 612 private alias getRelativeAddresss = getRelativeAddresss!(code.checkBit(0)); 613 } 614 615 private void getRelativeAddresss(bool stackPointer)(Registers* registers, MemoryBus* memory, int instruction) { 616 int rd = instruction.getBits(8, 10); 617 int offset = (instruction & 0xFF) * 4; 618 static if (stackPointer) { 619 debug (outputInstructions) registers.logInstruction(instruction, "ADD"); 620 registers.set(rd, registers.get(Register.SP) + offset); 621 } else { 622 debug (outputInstructions) registers.logInstruction(instruction, "ADD"); 623 registers.set(rd, (registers.getPC() & ~3) + offset); 624 } 625 } 626 627 private template addOffsetToStackPointer(int code) if (code.getBits(1, 31) == 0) { 628 private alias addOffsetToStackPointer = addOffsetToStackPointer!(code.checkBit(0)); 629 } 630 631 private void addOffsetToStackPointer(bool subtract)(Registers* registers, MemoryBus* memory, int instruction) { 632 int offset = (instruction & 0x7F) * 4; 633 static if (subtract) { 634 debug (outputInstructions) registers.logInstruction(instruction, "ADD"); 635 registers.set(Register.SP, registers.get(Register.SP) - offset); 636 } else { 637 debug (outputInstructions) registers.logInstruction(instruction, "ADD"); 638 registers.set(Register.SP, registers.get(Register.SP) + offset); 639 } 640 } 641 642 private template pushAndPopRegisters(int code) if (code.getBits(2, 31) == 0) { 643 private alias pushAndPopRegisters = pushAndPopRegisters!(code.checkBit(1), code.checkBit(0)); 644 } 645 646 private void pushAndPopRegisters(bool pop, bool pcAndLR)(Registers* registers, MemoryBus* memory, int instruction) { 647 int registerList = instruction & 0xFF; 648 int sp = registers.get(Register.SP); 649 static if (pop) { 650 debug (outputInstructions) registers.logInstruction(instruction, "POP"); 651 foreach (i; 0 .. 8) { 652 if (registerList.checkBit(i)) { 653 registers.set(i, memory.get!int(sp)); 654 sp += 4; 655 } 656 } 657 static if (pcAndLR) { 658 registers.setPC(memory.get!int(sp) & ~1); 659 sp += 4; 660 } 661 } else { 662 debug (outputInstructions) registers.logInstruction(instruction, "PUSH"); 663 sp -= 4 * (registerList.popcnt() + pcAndLR); 664 int address = sp; 665 foreach (i; 0 .. 8) { 666 if (registerList.checkBit(i)) { 667 memory.set!int(address, registers.get(i)); 668 address += 4; 669 } 670 } 671 static if (pcAndLR) { 672 memory.set!int(address, registers.get(Register.LR)); 673 } 674 } 675 registers.set(Register.SP, sp); 676 } 677 678 private template multipleLoadAndStore(int code) if (code.getBits(1, 31) == 0) { 679 private alias multipleLoadAndStore = multipleLoadAndStore!(code.checkBit(0)); 680 } 681 682 private void multipleLoadAndStore(bool load)(Registers* registers, MemoryBus* memory, int instruction) { 683 int rb = instruction.getBits(8, 10); 684 int registerList = instruction & 0xFF; 685 int address = registers.get(rb); 686 static if (load) { 687 debug (outputInstructions) registers.logInstruction(instruction, "LDMIA"); 688 foreach (i; 0 .. 8) { 689 if (registerList.checkBit(i)) { 690 registers.set(i, memory.get!int(address)); 691 address += 4; 692 } 693 } 694 } else { 695 debug (outputInstructions) registers.logInstruction(instruction, "STMIA"); 696 foreach (i; 0 .. 8) { 697 if (registerList.checkBit(i)) { 698 memory.set!int(address, registers.get(i)); 699 address += 4; 700 } 701 } 702 } 703 // Don't writeback if the address register was loaded 704 static if (load) { 705 if (!registerList.checkBit(rb)) { 706 registers.set(rb, address); 707 } 708 } else { 709 registers.set(rb, address); 710 } 711 } 712 713 private void conditionalBranch(int code)(Registers* registers, MemoryBus* memory, int instruction) 714 if (code >= 0 && code <= 13 ) { 715 if (!registers.checkCondition(code)) { 716 return; 717 } 718 debug (outputInstructions) registers.logInstruction(instruction, "B"); 719 int offset = instruction & 0xFF; 720 // sign extend the offset 721 offset <<= 24; 722 offset >>= 24; 723 registers.setPC(registers.getPC() + offset * 2); 724 } 725 726 @("unsupported") 727 private template conditionalBranch(int code) if (code == 14 || code == 15) { 728 private alias conditionalBranch = unsupported; 729 } 730 731 private void softwareInterrupt()(Registers* registers, MemoryBus* memory, int instruction) { 732 debug (outputInstructions) registers.logInstruction(instruction, "SWI"); 733 registers.setSPSR(Mode.SUPERVISOR, registers.getCPSR()); 734 registers.set(Mode.SUPERVISOR, Register.LR, registers.getPC() - 2); 735 registers.setPC(0x8); 736 registers.setFlag(CPSRFlag.I, 1); 737 registers.setFlag(CPSRFlag.T, Set.ARM); 738 registers.setMode(Mode.SUPERVISOR); 739 } 740 741 private void unconditionalBranch()(Registers* registers, MemoryBus* memory, int instruction) { 742 debug (outputInstructions) registers.logInstruction(instruction, "B"); 743 int offset = instruction & 0x7FF; 744 // sign extend the offset 745 offset <<= 21; 746 offset >>= 21; 747 registers.setPC(registers.getPC() + offset * 2); 748 } 749 750 private template longBranchWithLink(int code) if (code.getBits(1, 31) == 0) { 751 private alias longBranchWithLink = longBranchWithLink!(code.checkBit(0)); 752 } 753 754 private void longBranchWithLink(bool high)(Registers* registers, MemoryBus* memory, int instruction) { 755 int offset = instruction & 0x7FF; 756 static if (high) { 757 debug (outputInstructions) registers.logInstruction(instruction, "BL"); 758 int address = registers.get(Register.LR) + (offset << 1); 759 registers.set(Register.LR, registers.getPC() - 2 | 1); 760 registers.setPC(address); 761 } else { 762 debug (outputInstructions) registers.logInstruction(instruction, "BL_"); 763 // sign extend the offset 764 offset <<= 21; 765 offset >>= 21; 766 registers.set(Register.LR, registers.getPC() + (offset << 12)); 767 } 768 } 769 770 private void unsupported(Registers* registers, MemoryBus* memory, int instruction) { 771 throw new UnsupportedTHUMBInstructionException(registers.getExecutedPC(), instruction); 772 } 773 774 public class UnsupportedTHUMBInstructionException : Exception { 775 private this(int address, int instruction) { 776 super(format("This THUMB instruction is unsupported by the implementation\n%08x: %04x", address, instruction & 0xFFFF)); 777 } 778 }