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 }