// Meteor - A Nintendo Gameboy Advance emulator // Copyright (C) 2009-2011 Philippe Daouadi // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . #ifndef __INTERPRETER_THUMB_H__ #define __INTERPRETER_THUMB_H__ /* - From GBATEK - THUMB Binary Opcode Format Fo|_15|_14|_13|_12|_11|_10|_9_|_8_|_7_|_6_|_5_|_4_|_3_|_2_|_1_|_0_| _1|_0___0___0_|__Op___|_______Offset______|____Rs_____|____Rd_____|Shifted _2|_0___0___0___1___1_|_I,_Op_|___Rn/nn___|____Rs_____|____Rd_____|ADD/SUB _3|_0___0___1_|__Op___|____Rd_____|_____________Offset____________|Immedi. _4|_0___1___0___0___0___0_|______Op_______|____Rs_____|____Rd_____|AluOp _5|_0___1___0___0___0___1_|__Op___|Hd_|Hs_|____Rs_____|____Rd_____|HiReg/BX _6|_0___1___0___0___1_|____Rd_____|_____________Word______________|LDR PC _7|_0___1___0___1_|__Op___|_0_|___Ro______|____Rb_____|____Rd_____|LDR/STR _8|_0___1___0___1_|__Op___|_1_|___Ro______|____Rb_____|____Rd_____|""H/SB/SH _9|_0___1___1_|__Op___|_______Offset______|____Rb_____|____Rd_____|""{B} 10|_1___0___0___0_|Op_|_______Offset______|____Rb_____|____Rd_____|""H 11|_1___0___0___1_|Op_|____Rd_____|_____________Word______________|"" SP 12|_1___0___1___0_|Op_|____Rd_____|_____________Word______________|ADD PC/SP 13|_1___0___1___1___0___0___0___0_|_S_|___________Word____________|ADD SP,nn 14|_1___0___1___1_|Op_|_1___0_|_R_|____________Rlist______________|PUSH/POP 17|_1___0___1___1___1___1___1___0_|___________User_Data___________|BKPT ARM9 15|_1___1___0___0_|Op_|____Rb_____|____________Rlist______________|STM/LDM 16|_1___1___0___1_|_____Cond______|_________Signed_Offset_________|B{cond} U_|_1___1___0___1___1___1___1___0_|_____________var_______________|UNDEF ARM9 17|_1___1___0___1___1___1___1___1_|___________User_Data___________|SWI 18|_1___1___1___0___0_|________________Offset_____________________|B 19|_1___1___1___0___1_|_________________________var___________|_0_|BLXsuf ARM9 U_|_1___1___1___0___1_|_________________________var___________|_1_|UNDEF ARM9 19|_1___1___1___1_|_H_|______________Offset_Low/High______________|BL (BLX ARM9) */ #include "ameteor/interpreter.hpp" #include "globals.hpp" #include "cpu_globals.hpp" #include "ameteor/memory.hpp" #include "ameteor.hpp" #include "debug.hpp" #define Rb ((code >> 8) & 0x7) #define Ro ((code >> 6) & 0x7) #define Rs ((code >> 3) & 0x7) #define Rd ((code ) & 0x7) #define Imm (code & 0xFF) #define Off ((code >> 6) & 0x1F) //#define HiRs (((code & (0x1 << 6)) >> 3) | Rs) #define HiRs ((code >> 3) & 0xF) #define HiRd (((code & (0x1 << 7)) >> 4) | Rd) #ifdef METDEBUG # define NOT_PC(reg) \ if (reg == 15) \ met_abort("Register is PC") #else # define NOT_PC(reg) {} #endif #define THUMB(name) \ inline void Interpreter::t##name () #define NITHUMB(name) \ void Interpreter::t##name () namespace AMeteor { // move shifted register THUMB(_Shift) { if ((code >> 13) & 0x7) { met_abort("Bits 13-15 must be 000 for shift instructions"); } uint8_t off = Off; switch ((code >> 11) & 0x3) { case 0x0: // LSL if (off) { SETF(C, R(Rs) & (0x1 << (32-off))); R(Rd) = R(Rs) << off; FZ(R(Rd)); FN(R(Rd)); } else { R(Rd) = R(Rs); FZ(R(Rd)); FN(R(Rd)); } break; case 0x1: // LSR if (off) { SETF(C, R(Rs) & (0x1 << (off-1))); R(Rd) = R(Rs) >> off; FZ(R(Rd)); FN(R(Rd)); } else { SETF(C, R(Rs) & (0x1 << 31)); R(Rd) = 0; FLAG_Z = 1; FLAG_N = 0; } break; case 0x2: // ASR if (off) { SETF(C, (((int32_t)R(Rs)) >> (off - 1)) & 0x1); R(Rd) = ((int32_t)R(Rs)) >> off; FZ(R(Rd)); FN(R(Rd)); } else { if (R(Rd) & (0x1 << 31)) { R(Rd) = 0xFFFFFFFF; FLAG_Z = 0; FLAG_N = 1; FLAG_C = 1; } else { R(Rd) = 0; FLAG_Z = 1; FLAG_N = 0; FLAG_C = 0; } } break; case 0x3: // reserved met_abort("Bits 11-12 must not be 11 for shift instructions"); break; } CYCLES16Seq(R(15), 1); } // add/substract THUMB(ADDSUB) { if (((code >> 11) & 0x1F) != 0x3) { met_abort("Bits 11-15 should be 00011 for add/substract"); } uint32_t op2; if ((code >> 10) & 0x1) // imm op2 = Ro; else // reg op2 = R(Ro); if ((code >> 9) & 0x1) // SUB { #ifdef X86_ASM asm("subl %6, %5\n" "setzb %1\n" "setsb %2\n" "setncb %3\n" "setob %4\n" :"=r"(R(Rd)), "=m"(FLAG_Z), "=m"(FLAG_N), "=m"(FLAG_C), "=m"(FLAG_V) :"0"(R(Rs)), "r"(op2)); #else uint32_t op1 = R(Rs), res = R(Rd) = op1 - op2; FLAG_Z = !res; FLAG_N = res >> 31; FLAG_C = SUBCARRY(op1, op2, res); FLAG_V = SUBOVERFLOW(op1, op2, res); #endif } else // ADD { #ifdef X86_ASM asm("addl %6, %5\n" "setzb %1\n" "setsb %2\n" "setcb %3\n" "setob %4\n" :"=r"(R(Rd)), "=m"(FLAG_Z), "=m"(FLAG_N), "=m"(FLAG_C), "=m"(FLAG_V) :"0"(R(Rs)), "r"(op2)); #else uint32_t op1 = R(Rs), res = R(Rd) = op1 + op2; FLAG_Z = !res; FLAG_N = res >> 31; FLAG_C = ADDCARRY(op1, op2, res); FLAG_V = ADDOVERFLOW(op1, op2, res); #endif } CYCLES16Seq(R(15), 1); } // move/compare/add/substact immediate THUMB(_Imm) { if (((code >> 13) & 0x7) != 0x1) { met_abort("Bits 13-15 must be 001 for immediate instructions"); } switch ((code >> 11) & 0x3) { case 0x0: // MOV R(Rb) = Imm; FLAG_Z = !Imm; FLAG_N = 0; break; case 0x1: // CMP #ifdef X86_ASM asm("cmpl %5, %4\n" "setzb %0\n" "setsb %1\n" "setncb %2\n" "setob %3\n" :"=m"(FLAG_Z), "=m"(FLAG_N), "=m"(FLAG_C), "=m"(FLAG_V) :"r"(R(Rb)), "r"(Imm)); #else { uint32_t op1 = R(Rb), op2 = Imm, res = op1 - op2; FLAG_Z = !res; FLAG_N = res >> 31; FLAG_C = SUBCARRY(op1, op2, res); FLAG_V = SUBOVERFLOW(op1, op2, res); } #endif break; case 0x2: // ADD #ifdef X86_ASM asm("addl %6, %5\n" "setzb %1\n" "setsb %2\n" "setcb %3\n" "setob %4\n" :"=r"(R(Rb)), "=m"(FLAG_Z), "=m"(FLAG_N), "=m"(FLAG_C), "=m"(FLAG_V) :"0"(R(Rb)), "r"(Imm)); #else { uint32_t op1 = R(Rb), op2 = Imm, res = R(Rb) = op1 + op2; FLAG_Z = !res; FLAG_N = res >> 31; FLAG_C = ADDCARRY(op1, op2, res); FLAG_V = ADDOVERFLOW(op1, op2, res); } #endif break; case 0x3: // SUB #ifdef X86_ASM asm("subl %6, %5\n" "setzb %1\n" "setsb %2\n" "setncb %3\n" "setob %4\n" :"=r"(R(Rb)), "=m"(FLAG_Z), "=m"(FLAG_N), "=m"(FLAG_C), "=m"(FLAG_V) :"0"(R(Rb)), "r"(Imm)); #else { uint32_t op1 = R(Rb), op2 = Imm, res = R(Rb) = op1 - op2; FLAG_Z = !res; FLAG_N = res >> 31; FLAG_C = SUBCARRY(op1, op2, res); FLAG_V = SUBOVERFLOW(op1, op2, res); } #endif break; } CYCLES16Seq(R(15), 1); } // ALU operations THUMB(_ALU) { if (((code >> 10) & 0x3F) != 0x10) { met_abort("Bits 10-15 must be 010000 for ALU instructions"); } uint8_t opcode = (code >> 6) & 0xF; // TODO put a ifndef X86_ASM here around this declaration uint32_t res = 0; // to avoid a warning uint8_t shift; switch (opcode) { case 0x0: // AND #ifdef X86_ASM asm("andl %4, %3\n" "setzb %1\n" "setsb %2\n" :"=r"(R(Rd)), "=m"(FLAG_Z), "=m"(FLAG_N) :"0"(R(Rd)), "r"(R(Rs))); #else res = R(Rd) &= R(Rs); FLAG_Z = !res; FLAG_N = res >> 31; #endif break; case 0x1: // EOR #ifdef X86_ASM asm("xorl %4, %3\n" "setzb %1\n" "setsb %2\n" :"=r"(R(Rd)), "=m"(FLAG_Z), "=m"(FLAG_N) :"0"(R(Rd)), "r"(R(Rs))); #else res = R(Rd) ^= R(Rs); FLAG_Z = !res; FLAG_N = res >> 31; #endif break; case 0x2: // LSL ICYCLES(1); shift = R(Rs) & 0xFF; if (shift) { if (shift == 32) { SETFB(C, R(Rd) & 0x1); R(Rd) = 0; FLAG_Z = FLAG_N = 0; } else if (shift < 32) { SETFB(C, (R(Rd) >> (32-shift)) & 0x1); res = R(Rd) <<= shift; FLAG_Z = !res; FLAG_N = res >> 31; } else { R(Rd) = 0; FLAG_C = FLAG_Z = FLAG_N = 0; } } else { res = R(Rd); FLAG_Z = !res; FLAG_N = res >> 31; } break; case 0x3: // LSR ICYCLES(1); shift = R(Rs) & 0xFF; if (shift) { if (shift == 32) { SETFB(C, R(Rd) >> 31); R(Rd) = 0; FLAG_Z = FLAG_N = 0; } else if (shift < 32) { SETFB(C, (R(Rd) >> (shift-1)) & 0x1); res = R(Rd) >>= shift; FLAG_Z = !res; FLAG_N = res >> 31; } else { R(Rd) = 0; FLAG_C = FLAG_Z = FLAG_N = 0; } } else { res = R(Rd); FLAG_Z = !res; FLAG_N = res >> 31; } break; case 0x4: // ASR ICYCLES(1); shift = R(Rs) & 0xFF; if (shift) { if (shift >= 32) { R(Rd) = ((int32_t)R(Rs)) >> 31; FLAG_C = FLAG_Z = FLAG_N = R(Rd) & 0x1; } else { SETFB(C, (((int32_t)R(Rd)) >> (shift-1)) & 0x1); res = R(Rd) = ((int32_t)R(Rd)) >> shift; FLAG_Z = !res; FLAG_N = res >> 31; } } else { res = R(Rd); FLAG_Z = !res; FLAG_N = res >> 31; } break; case 0x5: // ADC res = R(Rd) + R(Rs) + FLAG_C; FLAG_C = ADDCARRY(R(Rd), R(Rs), res); FLAG_V = ADDOVERFLOW(R(Rd), R(Rs), res); R(Rd) = res; break; case 0x6: // SBC res = R(Rd) - R(Rs) + FLAG_C - 1; FLAG_C = SUBCARRY(R(Rd), R(Rs), res); FLAG_V = SUBOVERFLOW(R(Rd), R(Rs), res); R(Rd) = res; break; case 0x7: // ROR ICYCLES(1); shift = R(Rs) & 0xFF; if (shift) { shift %= 32; res = R(Rd); SETFB(C, (res >> (shift - 1)) & 0x1); res = R(Rd) = ROR(res, shift); FLAG_Z = !res; FLAG_N = res >> 31; } else { res = R(Rd); FLAG_Z = !res; FLAG_N = res >> 31; } break; case 0x8: // TST #ifdef X86_ASM asm("testl %3, %2\n" "setzb %0\n" "setsb %1\n" :"=m"(FLAG_Z), "=m"(FLAG_N) :"r"(R(Rd)), "r"(R(Rs))); #else res = R(Rd) & R(Rs); FLAG_Z = !res; FLAG_N = res >> 31; #endif break; case 0x9: // NEG #ifdef X86_ASM asm("negl %5\n" "setzb %1\n" "setsb %2\n" "setncb %3\n" "setob %4\n" :"=r"(R(Rd)), "=m"(FLAG_Z), "=m"(FLAG_N), "=m"(FLAG_C), "=m"(FLAG_V) :"0"(R(Rs))); #else { uint32_t op2; op2 = R(Rs); res = R(Rd) = -op2; FLAG_Z = !res; FLAG_N = res >> 31; // we overflow only if op2 == INT_MIN FLAG_V = SUBOVERFLOW(0, op2, res); // we have a carry only if op2 == 0 FLAG_C = SUBCARRY(0, op2, res); } #endif break; case 0xA: // CMP #ifdef X86_ASM asm("cmpl %5, %4\n" "setzb %0\n" "setsb %1\n" "setncb %2\n" "setob %3\n" :"=m"(FLAG_Z), "=m"(FLAG_N), "=m"(FLAG_C), "=m"(FLAG_V) :"r"(R(Rd)), "r"(R(Rs))); #else { uint32_t op1, op2; op1 = R(Rd); op2 = R(Rs); res = op1 - op2; FLAG_Z = !res; FLAG_N = res >> 31; FLAG_C = SUBCARRY(op1, op2, res); FLAG_V = SUBOVERFLOW(op1, op2, res); } #endif break; case 0xB: // CMN #ifdef X86_ASM asm("addl %5, %4\n" "setzb %0\n" "setsb %1\n" "setcb %2\n" "setob %3\n" :"=m"(FLAG_Z), "=m"(FLAG_N), "=m"(FLAG_C), "=m"(FLAG_V) :"r"(R(Rd)), "r"(R(Rs)) :"4"); #else { uint32_t op1, op2; op1 = R(Rd); op2 = R(Rs); res = op1 + op2; FLAG_Z = !res; FLAG_N = res >> 31; FLAG_C = ADDCARRY(op1, op2, res); FLAG_V = ADDOVERFLOW(op1, op2, res); } #endif break; case 0xC: // ORR #ifdef X86_ASM asm("orl %4, %3\n" "setzb %1\n" "setsb %2\n" :"=r"(R(Rd)), "=m"(FLAG_Z), "=m"(FLAG_N) :"0"(R(Rd)), "r"(R(Rs))); #else res = R(Rd) |= R(Rs); FLAG_Z = !res; FLAG_N = res >> 31; #endif break; case 0xD: // MUL MULICYCLES(Rs); res = R(Rd) *= R(Rs); FLAG_Z = !res; FLAG_N = res >> 31; break; case 0xE: // BIC res = R(Rd) &= ~R(Rs); FLAG_Z = !res; FLAG_N = res >> 31; break; case 0xF: // MVN res = R(Rd) = ~R(Rs); FLAG_Z = !res; FLAG_N = res >> 31; break; } CYCLES16Seq(R(15), 1); } // Hi register operations/branch exchange THUMB(_HiRegOp) { if (((code >> 10) & 0x3F) != 0x11) { met_abort("Bits 10-15 must be 010001 for HiReg instructions"); } uint8_t rd = HiRd, rs = HiRs; switch ((code >> 8) & 0x3) { case 0x0: // ADD R(rd) += R(rs); if (rd == 15) { R(15) &= 0xFFFFFFFE; CYCLES16NSeq(R(15), 3); R(15) += 2; } else CYCLES16Seq(R(15), 1); break; case 0x1: // CMP #ifdef X86_ASM asm("cmpl %5, %4\n" "setzb %0\n" "setsb %1\n" "setncb %2\n" "setob %3\n" :"=m"(FLAG_Z), "=m"(FLAG_N), "=m"(FLAG_C), "=m"(FLAG_V) :"r"(R(rd)), "r"(R(rs))); #else { uint32_t op1 = R(rd), op2 = R(rs), res = op1 - op2; FLAG_Z = !res; FLAG_N = res >> 31; FLAG_C = SUBCARRY(op1, op2, res); FLAG_V = SUBOVERFLOW(op1, op2, res); } #endif CYCLES16Seq(R(15), 1); break; case 0x2: if (rd != 8 || rs != 8) // MOV { R(rd) = R(rs); if (rd == 15) { R(15) &= 0xFFFFFFFE; CYCLES16NSeq(R(15), 3); R(15) += 2; } else CYCLES16Seq(R(15), 1); } else // NOP CYCLES16Seq(R(15), 1); break; case 0x3: if (Rd) met_abort("Rd must be 0 for BX/BLX instructions"); if (code & (0x1 << 7)) // BLX { NOT_PC(rs); met_abort("BLX not implemented"); } else // BX { if (R(rs) & 0x1) { R(15) = (R(rs) & 0xFFFFFFFE) + 2; CYCLES16NSeq(R(15), 3); } else { // switch to arm FLAG_T = 0; R(15) = (R(rs) & 0xFFFFFFFC) + 4; CYCLES32NSeq(R(15), 3); } } break; } } // load PC-relative THUMB(LDRimm) { if (((code >> 11) & 0x1F) != 0x9) { met_abort("Bits 11-15 must be 01001 for LDR with imm instructions"); } uint32_t add = (R(15) & 0xFFFFFFFC) + (Imm << 2); R(Rb) = MEM.Read32(add); CYCLES32NSeq(add, 1); ICYCLES(1); CYCLES16Seq(R(15), 1); } // load/store with register offset // and load/store sign-extended byte/halfword THUMB(STRLDRreg) { if (((code >> 12) & 0xF) != 0x5) { met_abort("Bits 12-15 must be 0101 for LDR/STR with reg instructions"); } uint32_t add = R(Ro) + R(Rs); switch ((code >> 9) & 0x7) { // load/store with register offset case 0x0: // STR MEM.Write32(add, R(Rd)); CYCLES32NSeq(add, 1); CYCLES16NSeq(R(15), 1); break; case 0x2: // STRB MEM.Write8(add, R(Rd)); CYCLES16NSeq(add, 1); CYCLES16NSeq(R(15), 1); break; case 0x4: // LDR R(Rd) = MEM.Read32(add); CYCLES32NSeq(add, 1); ICYCLES(1); CYCLES16Seq(R(15), 1); break; case 0x6: // LDRB R(Rd) = MEM.Read8(add); CYCLES16NSeq(add, 1); ICYCLES(1); CYCLES16Seq(R(15), 1); break; // load/store sign-extended byte/halfword case 0x1: // STRH MEM.Write16(add, R(Rd)); CYCLES16NSeq(add, 1); CYCLES16NSeq(R(15), 1); break; case 0x3: // LDSB R(Rd) = (int8_t)MEM.Read8(add); CYCLES16NSeq(add, 1); ICYCLES(1); CYCLES16Seq(R(15), 1); break; case 0x5: // LDRH R(Rd) = MEM.Read16(add); CYCLES16NSeq(add, 1); ICYCLES(1); CYCLES16Seq(R(15), 1); break; case 0x7: // LDSH R(Rd) = (int16_t)MEM.Read16(add); CYCLES16NSeq(add, 1); ICYCLES(1); CYCLES16Seq(R(15), 1); break; } } // load/store with immediate offset THUMB(STRLDRoff) { if (((code >> 13) & 0x7) != 0x3) { met_abort("Bits 13-15 must be 011 for LDR/STR with off instructions"); } uint32_t add; switch ((code >> 11) & 0x3) { case 0x0: // STR add = R(Rs) + (Off << 2); MEM.Write32(add, R(Rd)); CYCLES32NSeq(add, 1); CYCLES16NSeq(R(15), 1); break; case 0x1: // LDR add = R(Rs) + (Off << 2); R(Rd) = MEM.Read32(add); CYCLES32NSeq(add, 1); ICYCLES(1); CYCLES16Seq(R(15), 1); break; case 0x2: // STRB add = R(Rs) + Off; MEM.Write8(add, R(Rd)); CYCLES16NSeq(add, 1); CYCLES16NSeq(R(15), 1); break; case 0x3: // LDRB add = R(Rs) + Off; R(Rd) = MEM.Read8(add); CYCLES16NSeq(add, 1); ICYCLES(1); CYCLES16Seq(R(15), 1); break; } } // load/store halfword THUMB(LDRHSTRHoff) { if (((code >> 12) & 0xF) != 0x8) { met_abort("Bits 12-15 must be 1000 for LDRH/STRH with off instructions"); } uint32_t add = R(Rs) + (Off * 2); if (code & (0x1 << 11)) // LDRH { R(Rd) = MEM.Read16(add); CYCLES16NSeq(add, 1); ICYCLES(1); CYCLES16Seq(R(15), 1); } else // STRH { MEM.Write16(add, R(Rd)); CYCLES16NSeq(add, 1); CYCLES16NSeq(R(15), 1); } } // load/store SP-relative THUMB(STRLDRsp) { if (((code >> 12) & 0xF) != 0x9) { met_abort("Bits 12-15 must be 1001 for LDR/STR SP-relative instructions"); } uint32_t add = R(13) + (Imm << 2); if (code & (0x1 << 11)) // LDR { R(Rb) = MEM.Read32(add); CYCLES32NSeq(add, 1); ICYCLES(1); CYCLES16Seq(R(15), 1); } else // STR { MEM.Write32(add, R(Rb)); CYCLES32NSeq(add, 1); CYCLES16NSeq(R(15), 1); } } // get relative address THUMB(ADDpcsp) { if (((code >> 12) & 0xF) != 0xA) { met_abort("Bits 12-15 must be 1010 for ADD relative instructions"); } if (code & (0x1 << 11)) // with SP { R(Rb) = R(13) + (Imm << 2); } else // with PC { R(Rb) = (R(15) & 0xFFFFFFFC) + (Imm << 2); } CYCLES16Seq(R(15), 1); } // add offset to stack pointer THUMB(ADDsp) { if (((code >> 8) & 0xFF) != 0xB0) { met_abort("Bits 8-15 must be 10110000 for ADD to SP instructions"); } if (code & (0x1 << 7)) // substract { R(13) -= ((code & 0x7F) << 2); } else // add { R(13) += ((code & 0x7F) << 2); } CYCLES16Seq(R(15), 1); } // push/pop registers THUMB(PUSHPOP) { if (((code >> 12) & 0xF) != 0xB) { met_abort("Bits 12-15 must be 1011 for PUSH/POP instructions"); } if (((code >> 9) & 0x3) != 0x2) { met_abort("Bits 9-10 must be 10 for PUSH/POP instructions"); } static const uint8_t NumBits[] = {0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 4}; uint8_t numregs = NumBits[(code >> 4) & 0xF] + NumBits[ code & 0xF]; if (code & (0x1 << 8)) ++numregs; uint32_t add, newsp; if (code & (0x1 << 11)) // POP { newsp = R(13) + 4 * numregs; add = R(13) & 0xFFFFFFFC; CYCLES32NSeq(add, numregs); ICYCLES(1); for (register uint8_t n = 0; n < 8; ++n) if (code & (0x1 << n)) { R(n) = MEM.Read32 (add); add += 4; } if (code & (0x1 << 8)) // POP PC { R(15) = (MEM.Read32(add) + 2) & 0xFFFFFFFE; CYCLES16NSeq(R(15), 3); } else CYCLES16Seq(R(15), 1); } else // PUSH { newsp = R(13) - 4 * numregs; add = newsp & 0xFFFFFFFC; CYCLES32NSeq(add, numregs); CYCLES16NSeq(R(15), 1); for (register uint8_t n = 0; n < 8; ++n) if (code & (0x1 << n)) { MEM.Write32 (add, R(n)); add += 4; } if (code & (0x1 << 8)) // PUSH LR { MEM.Write32 (add, R(14)); } } R(13) = newsp; } // multiple load/store THUMB(STMLDM) { if (((code >> 12) & 0xF) != 0xC) { met_abort("Bits 12-15 must be 1100 for STM/LDM instructions"); } static const uint8_t NumBits[] = {0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 4}; uint8_t numregs = NumBits[(code >> 4) & 0xF] + NumBits[ code & 0xF]; uint32_t add = R(Rb) & 0xFFFFFFFC; if (code & (0x1 << 11)) // LDMIA { CYCLES32NSeq(add, numregs); ICYCLES(1); for (register uint8_t n = 0; n < 8; ++n) if (code & (0x1 << n)) { R(n) = MEM.Read32 (add); add += 4; } CYCLES16Seq(R(15), 1); } else // STMIA { CYCLES32NSeq(add, numregs); CYCLES16NSeq(R(15), 1); for (register uint8_t n = 0; n < 8; ++n) if (code & (0x1 << n)) { MEM.Write32 (add, R(n)); add += 4; } } if (!(code & (0x1 << Rb))) R(Rb) = R(Rb) + 4 * numregs; } // conditional branch THUMB(_CondBranch) { if (((code >> 12) & 0xF) != 0xD) { met_abort("Bits 12-15 must be 1101 for branch on condition instructions"); } bool branch = false; switch ((code >> 8) & 0xF) { case 0x0: // BEQ if (FLAG_Z) branch = true; break; case 0x1: // BNE if (!FLAG_Z) branch = true; break; case 0x2: // BCS if (FLAG_C) branch = true; break; case 0x3: // BCC if (!FLAG_C) branch = true; break; case 0x4: // BMI if (FLAG_N) branch = true; break; case 0x5: // BPL if (!FLAG_N) branch = true; break; case 0x6: // BVS if (FLAG_V) branch = true; break; case 0x7: // BVC if (!FLAG_V) branch = true; break; case 0x8: // BHI if (FLAG_C && !FLAG_Z) branch = true; break; case 0x9: // BLS if (!FLAG_C || FLAG_Z) branch = true; break; case 0xA: // BGE if (FLAG_N == FLAG_V) branch = true; break; case 0xB: // BLT if (FLAG_N != FLAG_V) branch = true; break; case 0xC: // BGT if (!FLAG_Z && FLAG_N == FLAG_V) branch = true; break; case 0xD: // BLE if (FLAG_Z || FLAG_N != FLAG_V) branch = true; break; case 0xE: // undefined met_abort("Undefined branch on condition"); break; case 0xF: // reserved for SWI instruction met_abort("Bits 8-11 must not be 1111 for branch instruction"); break; } if (branch) { R(15) += (((int32_t)(int8_t)Imm) << 1) + 2; CYCLES16NSeq(R(15), 3); } else CYCLES16Seq(R(15), 1); } // software interrupt and breakpoint THUMB(SWI) { CPU.SoftwareInterrupt(code & 0xFF); // FIXME seems wrong ! CYCLES32NSeq(0, 3); } // unconditional branch THUMB(B) { if (((code >> 11) & 0x1F) != 0x1C) { met_abort("Bits 11-15 must be 11100 for branch instructions"); } int32_t off = (code & 0x7FF) << 1; if (off & 0x800) off |= 0xFFFFF000; // extends sign bit R(15) += off + 2; CYCLES16NSeq(R(15), 3); } // long branch with link THUMB(_BL1) { if (((code >> 11) & 0x1F) != 0x1E) { met_abort("Bits 11-15 must be 11110 for long branch instructions"); } if (code & (0x1 << 10)) // negative offset R(14) = R(15) + (((int32_t)((code & 0x7FF) << 12)) | 0xFF800000); else R(14) = R(15) + ((code & 0x7FF) << 12); CYCLES16Seq(R(15), 1); } THUMB(_BL2) { // XXX we dont do check here to be sure this instruction is called after // BL1 uint32_t add = R(14) + ((code & 0x7FF) << 1); if (code & (0x1 << 11)) // BL { R(14) = (R(15)-2) | 0x1; R(15) = (add & 0xFFFFFFFE) + 2; } else // BLX { met_abort("BLX not implemented"); if (code & 0x1) met_abort("BLX with odd address"); FLAG_T = 0; } CYCLES16NSeq(R(15), 3); } NITHUMB(_Code) { switch (code >> 13) { case 0: // 000 if ((code & 0x1800) == 0x1800) // 00011 tADDSUB(); else // 000 t_Shift(); break; case 1: // 001 t_Imm(); break; case 2: // 010 switch ((code >> 10) & 0x7) { case 0: // 010000 t_ALU(); break; case 1: // 010001 t_HiRegOp(); break; case 2: case 3: // 01001x tLDRimm(); break; default: tSTRLDRreg(); break; } break; case 3: // 011 tSTRLDRoff(); break; case 4: // 100 if (code & (0x1 << 12)) // 1001 tSTRLDRsp(); else // 100 tLDRHSTRHoff(); break; case 5: // 101 if (code & (0x1 << 12)) // 1011 { switch (code & 0x0600) { case 0x0000: // 1011x00 tADDsp(); break; case 0x0400: // 1011x10 tPUSHPOP(); break; default: met_abort("not implemented or unknown"); break; } } else // 101 tADDpcsp(); break; case 6: // 110 if (code & (0x1 << 12)) // 1101 { if ((code & 0x0F00) == 0x0F00) // 11011111 tSWI(); else // 1101xxxx t_CondBranch(); } else // 1100 tSTMLDM(); break; case 7: // 111 switch ((code >> 11) & 0x3) { case 0: // 11100 tB(); break; case 2: // 11110 t_BL1(); break; case 3: // 11111 t_BL2(); break; default: met_abort("not implemented or unknown"); break; } break; } } } #undef Rb #undef Ro #undef Rs #undef Rd #undef Imm #undef THUMB #endif