v6m virtual machine
Import programemu812
mbed LPC812 emulator pre-alpha version
Import programemu1114
mbed LPC1114 emulator pre-alpha version
Diff: BaseV6M.cpp
- Revision:
- 1:5fa0120a6169
- Parent:
- 0:c2ad7f15d6a8
- Child:
- 2:078d2e512ba4
--- a/BaseV6M.cpp Mon Aug 10 13:02:02 2015 +0000 +++ b/BaseV6M.cpp Mon Aug 10 22:39:26 2015 +0900 @@ -0,0 +1,1128 @@ +// BaseV6M.cpp 2015/8/9 +#pragma Otime + +#include "BaseV6M.h" +#include "v6m_log.h" + +const char* RegName[] = { + "r0","r1","r2","r3","r4","r5","r6","r7","r8","r9","r10","r11","r12","sp","lr","pc", + "_xPSR","_IM","_NL"}; + +const uint32_t NZCVlookup[] = { // (d,m,n): (N,Z,C,V) + /* (0,0,0): */ 0UL<<31|0<<30|0<<29|0<<28, + /* (1,0,0): */ 1UL<<31|0<<30|0<<29|1<<28, + /* (0,1,0): */ 0UL<<31|0<<30|1<<29|0<<28, + /* (1,1,0): */ 1UL<<31|0<<30|0<<29|0<<28, + /* (0,0,1): */ 0UL<<31|0<<30|1<<29|0<<28, + /* (1,0,1): */ 1UL<<31|0<<30|0<<29|0<<28, + /* (0,1,1): */ 0UL<<31|0<<30|1<<29|1<<28, + /* (1,1,1): */ 1UL<<31|0<<30|1<<29|0<<28,}; + +static inline uint32_t immed(uint32_t code, int width, int base = 0) { // unsigned immed + return (code>>base) & ((1<<width)-1); +} + +static inline int32_t signed_immed(uint32_t code, int width, int base = 0) { // signed immed + int32_t i = immed(code, width, base); + if (i & (1<<(width-1))) { + return i - (1<<width); + } + return i; +} + +static inline uint32_t add32(uint32_t a, uint32_t b) { + return (a + b) & 0xffffffff; +} + +static inline uint32_t mul32(uint32_t a, uint32_t b) { + return (a * b) & 0xffffffff; +} + +static inline uint32_t not32(uint32_t data) { + return (~data)&0xffffffff; +} + +static inline uint32_t align32(uint32_t addr) { + return addr & 0xfffffffc; +} + +static inline uint32_t align16(uint32_t addr) { + return addr & 0xfffffffe; +} + +BaseV6M::BaseV6M() { + Rd = Rn = Rm = &R[0]; + cycle = 0; + R[8] = R[9] = R[10] = R[11] = R[12] = 0xffffffff; +} + +const char* BaseV6M::GetRegName(uint32_t* pR) { + return RegName[GetRegIndex(pR)]; +} + +int BaseV6M::GetRegIndex(uint32_t* pR) { + if (pR == &R[0]) return 0; + if (pR == &R[1]) return 1; + if (pR == &R[2]) return 2; + if (pR == &R[3]) return 3; + if (pR == &R[4]) return 4; + if (pR == &R[5]) return 5; + if (pR == &R[6]) return 6; + if (pR == &R[7]) return 7; + if (pR == &R[8]) return 8; + if (pR == &R[9]) return 9; + if (pR == &R[10]) return 10; + if (pR == &R[11]) return 11; + if (pR == &R[12]) return 12; + if (pR == &R[13]) return 13; + if (pR == &R[14]) return 14; + if (pR == &R[15]) return 15; + if (pR == &R[16]) return 16; + if (pR == &R[17]) return 17; + if (pR == &R[18]) return 18; + MBED_ASSERT(0); + //return (pR - R) / sizeof(uint32_t); + /* NOTREACHED */ +} + +uint32_t BaseV6M::N() { + return immed(R[_xPSR], 1, 31); +} + +uint32_t BaseV6M::Z() { + return immed(R[_xPSR], 1, 30); +} + +uint32_t BaseV6M::C() { + return immed(R[_xPSR], 1, 29); +} + +uint32_t BaseV6M::V() { + return immed(R[_xPSR], 1, 28); +} + +void BaseV6M::Cin(uint32_t c) { + R[_xPSR] &= ~(1<<29); + R[_xPSR] |= c<<29; +} + +void BaseV6M::NZupdate(uint32_t d) { + uint32_t mask; + if (d & (1UL<<31)) { // N + mask = 1UL<<31|0<<30; + } else if (d == 0) { // Z + mask = 0UL<<31|1<<30; + } else { + mask = 0UL<<31|0<<30; + } + R[_xPSR] &= ~(1UL<<31|1<<30); + R[_xPSR] |= mask; +} + +void BaseV6M::NZCVupdate(uint32_t d, uint32_t n, uint32_t m) { + uint32_t mask = NZCVlookup[d>>31|(n>>31)<<1|(m>>31)<<2]; + if (d == 0) { + mask |= 1<<30; // Z=1 + } + R[_xPSR] &= ~(1UL<<31|1<<30|1<<29|1<<28); + R[_xPSR] |= mask; +} + +void BaseV6M::jump(uint32_t addr) { + R[_PC] = addr; + cache = false; +} + +void BaseV6M::exception_entry(uint32_t num) { + push(R[_xPSR]); + push(R[_PC] - 2); + push(R[_LR]); + push(R[13]); + push(R[3]); + push(R[2]); + push(R[1]); + push(R[0]); + R[_LR] = 0xfffffff9; + uint32_t addr = peek32(num * 4); + V6M_ASSERT(addr&1); + jump(addr); + V6M_INFO("I: Exception entry %d", num); +} + +void BaseV6M::exception_return() { + if ((R[_PC]&0xfffffff0) == 0xfffffff0) { + V6M_ASSERT((R[_PC]&0x0e) == 8); + R[0] = pop(); + R[1] = pop(); + R[2] = pop(); + R[3] = pop(); + R[12] = pop(); + R[_LR] = pop(); + uint32_t addr = pop(); + R[_xPSR] = pop(); + jump(addr); + V6M_INFO("I: Exception return ; =0x%08x", addr); + } +} + +void BaseV6M::push(uint32_t d) { + R[_SP] -= 4; + poke32(R[_SP], d); +} + +uint32_t BaseV6M::pop() { + uint32_t d = peek32(R[_SP]); + R[_SP] += 4; + return d; +} + +void BaseV6M::d_Rn_Rd(uint32_t code) { // decode leaf + Rd = &R[immed(code, 3, 0)]; + Rn = &R[immed(code, 3, 3)]; +} + +void BaseV6M::d_Op_H_Rm(uint32_t code) { // BX Rm / BLX Rm + Rm = &R[immed(code, 4, 3)]; + op = immed(code, 1, 7); +} + +void BaseV6M::d_imm3_Rn_Rd(uint32_t code) { // ADD Rd,Rn,#imm3 + d_Rn_Rd(code); + R[_IM] = immed(code, 3, 6); + Rm = &R[_IM]; +} + +void BaseV6M::d_Rd_imm8(uint32_t code) { // <Op> Rd,#imm8 / LDR rd,[pc,#imm8] + Rd = &R[immed(code, 3, 8)]; + Rn = Rd; + R[_IM] = immed(code, 8, 0); + Rm = &R[_IM]; +} + +void BaseV6M::d_Rn_imm8(uint32_t code) { // <Op> Rn,#imm8 + Rn = &R[immed(code, 3, 8)]; + R[_IM] = immed(code, 8, 0); + Rm = &R[_IM]; +} + +void BaseV6M::d_imm5_Rn_Rd(uint32_t code) { // LSL Rd,Rn,#imm5 / LDR Rd,[Rn,#imm5] + d_Rn_Rd(code); + R[_IM] = immed(code, 5, 6); + Rm = &R[_IM]; +} + +void BaseV6M::d_Op_Rm_Rd(uint32_t code) { // <Op> Rd,Rm + Rd = &R[immed(code, 3, 0)]; + Rn = Rd; + Rm = &R[immed(code, 3, 3)]; + op = immed(code, 4, 6); +} + +void BaseV6M::d_Op_imm7(uint32_t code) { // ADD|SUB sp,sp,#imm7 + R[_IM] = immed(code, 7, 0); + Rm = &R[_IM]; + op = immed(code, 1, 7); +} + +void BaseV6M::d_D_M_Rm_Rd(uint32_t code) { // ADD|CMP|MOV Rd,Rm + Rm = &R[immed(code, 4, 3)]; + Rd = &R[immed(code, 3, 0)]; + if (code & 0x80) { + Rd += 8; + } + Rn = Rd; +} + +void BaseV6M::d_Rm_Rn_Rd(uint32_t code) { // LDR Rd,[Rn,Rm] + d_Rn_Rd(code); + Rm = &R[immed(code, 3, 6)]; +} + +void BaseV6M::d_reg_list(uint32_t code) { // PUSH / POP + reg_list = immed(code, 8, 0); +} + +void BaseV6M::d_Rn_reg_list(uint32_t code) { // LDM / STM + d_reg_list(code); + Rn = &R[immed(code, 3, 8)]; +} + +void BaseV6M::e_adc() { // ADC Rd,Rn,Rm|imm + uint32_t n = *Rn; + uint32_t m = *Rm; + uint32_t d = add32(n, m + C()); + *Rd = d; + NZCVupdate(d, n, m); +} + +void BaseV6M::e_sbc() { // SBC Rd,Rn,Rm|imm + uint32_t n = *Rn; + uint32_t m = not32(*Rm); + uint32_t d = add32(n, m + C()); + *Rd = d; + NZCVupdate(d, n, m); +} + +void BaseV6M::e_add() { // ADD Rd,Rn,Rm|imm + uint32_t n = *Rn; + uint32_t m = *Rm; + uint32_t d = add32(n, m); + *Rd = d; + NZCVupdate(d, n, m); +} + +void BaseV6M::e_sub() { // SUB Rd,Rn,Rm|imm + uint32_t n = *Rn; + uint32_t m = not32(*Rm); + uint32_t d = add32(n, m + 1); + *Rd = d; + NZCVupdate(d, n, m); +} + +void BaseV6M::e_cmp() { // CMP Rn,Rm|imm + Rd = &R[_NL]; + e_sub(); +} + +void BaseV6M::e_cmn() { // CMN Rn,Rm|imm + Rd = &R[_NL]; + e_add(); +} + +void BaseV6M::e_and() { // AND Rd,Rn,Rm|imm + uint32_t data = *Rn & *Rm; + *Rd = data; + NZupdate(data); +} + +void BaseV6M::e_tst() { // TST Rn,Rm|imm + Rd = &R[_NL]; + e_and(); +} + +void BaseV6M::e_lsl() { // LSL Rd,Rn,Rm|imm + uint32_t data = *Rn; + int shift = *Rm & 0xff; + if (shift <= 32) { + Cin((data>>(32-shift))&1); + data <<= shift; + data &= 0xffffffff; + } else { + Cin(0); + data = 0; + } + *Rd = data; + NZupdate(data); +} + +void BaseV6M::e_lsr() { // LSR Rd,Rn,Rm|imm + uint32_t data = *Rn; + int shift = *Rm & 0xff; + if (shift >= 1) { + if (shift <= 32) { + Cin((data>>(shift-1))&1); + data >>= shift; + } else { + Cin(0); + data = 0; + } + } + *Rd = data; + NZupdate(data); +} + +void BaseV6M::e_asr() { // ASR Rd,Rn,Rm|imm + uint32_t data = *Rn; + if ((data & 0x80000000) == 0) { + e_lsr(); + return; + } + int shift = *Rm & 0xff; + if (shift >= 1) { + if (shift <= 31) { + Cin((data>>(shift-1))&1); + data >>= shift; + data |= ~(0x7fffffff>>(shift-1)); + data &= 0xffffffff; + } else { + Cin(1); + data = 0xffffffff; + } + } + *Rd = data; + NZupdate(data); +} + +void BaseV6M::e_ror() { // ROR Rd,Rn,Rm|imm + uint32_t data = *Rn; + int shift = *Rm & 0xff; + if (shift >= 1) { + int k = shift % 32; + if (k == 0) { + Cin(data>>31); + } else { + for (int i = 0; i < k; i++) { + if (data & 1) { + Cin(1); + data >>= 1; + data |= 0x80000000; + } else { + Cin(0); + data >>= 1; + } + } + } + } + *Rd = data; + NZupdate(data); +} + +void BaseV6M::e_orr() { // ORR Rd,Rn,Rm|imm + uint32_t data = *Rn | *Rm; + *Rd = data; + NZupdate(data); +} + +void BaseV6M::e_eor() { // EOR Rd,Rn,Rm|imm + uint32_t data = *Rn ^ *Rm; + *Rd = data; + NZupdate(data); +} + +void BaseV6M::e_mul() { // MUL Rd,Rn,Rm + uint32_t data = mul32(*Rn, *Rm); + *Rd = data; + NZupdate(data); +} + +void BaseV6M::e_bic() { // BIC Rd,Rn,Rm|imm + uint32_t data = *Rn & not32(*Rm); + *Rd = data; + NZupdate(data); +} + +void BaseV6M::e_mov() { // MOV Rd,Rm|imm + uint32_t data = *Rm; + *Rd = data; + NZupdate(data); +} + +void BaseV6M::e_neg() { // NEG + Rn = &R[_IM]; + R[_IM] = 0; + e_sub(); +} + +void BaseV6M::e_mvn() { // MVN + uint32_t data = not32(*Rm); + *Rd = data; + NZupdate(data); +} + +void BaseV6M::c_beq(uint32_t code) { // BEQ #0xd000-0xd0ff + uint32_t addr = R[_PC] + signed_immed(code, 8) * 2; + if (Z() == 1) { + jump(addr); + } + V6M_INFO("I: BEQ 0x%08x", addr); +} + +void BaseV6M::c_bne(uint32_t code) { // BNE 0xd100-0xd1ff + uint32_t addr = R[_PC] + signed_immed(code, 8) * 2; + if (Z() == 0) { + jump(addr); + } + V6M_INFO("I: BNE 0x%08x", addr); +} + +void BaseV6M::c_bcs(uint32_t code) { // BCS #0xd200-0xd2ff + uint32_t addr = R[_PC] + signed_immed(code, 8) * 2; + if (C() == 1) { + jump(addr); + } + V6M_INFO("I: BCS 0x%08x", addr); +} + +void BaseV6M::c_bcc(uint32_t code) { // BCC #0xd300-0xd3ff + uint32_t addr = R[_PC] + signed_immed(code, 8) * 2; + if (C() == 0) { + jump(addr); + } + V6M_INFO("I: BCC 0x%08x", addr); +} + +void BaseV6M::c_bmi(uint32_t code) { // BMI 0xd400-0xd4ff + uint32_t addr = R[_PC] + signed_immed(code, 8) * 2; + if (N() == 1) { + jump(addr); + } + V6M_INFO("I: BMI 0x%08x", addr); +} + +void BaseV6M::c_bpl(uint32_t code) { // BMI 0xd500-0xd5ff + uint32_t addr = R[_PC] + signed_immed(code, 8) * 2; + if (N() == 0) { + jump(addr); + } + V6M_INFO("I: BPL 0x%08x", addr); +} + +void BaseV6M::c_bvs(uint32_t code) { // BVS 0xd600-0xd6ff + uint32_t addr = R[_PC] + signed_immed(code, 8) * 2; + if (V() == 1) { + jump(addr); + } + V6M_INFO("I: BVS 0x%08x", addr); + V6M_ASSERT(0); +} + +void BaseV6M::c_bvc(uint32_t code) { // BVC 0xd700-0xd7ff + uint32_t addr = R[_PC] + signed_immed(code, 8) * 2; + if (V() == 0) { + jump(addr); + } + V6M_INFO("I: BVC 0x%08x", addr); + V6M_ASSERT(0); +} + +void BaseV6M::c_bhi(uint32_t code) { // BHI 0xd800-0xd8ff + uint32_t addr = R[_PC] + signed_immed(code, 8) * 2; + if (C() == 1 && Z() == 0) { + jump(addr); + } + V6M_INFO("I: BHI 0x%08x", addr); +} + +void BaseV6M::c_bls(uint32_t code) { // BLS 0xd900-0xd9ff + uint32_t addr = R[_PC] + signed_immed(code, 8) * 2; + if (C() == 0 || Z() == 1) { + jump(addr); + } + V6M_INFO("I: BLS 0x%08x", addr); +} + +void BaseV6M::c_bge(uint32_t code) { // BGE 0xda00-0xdaff + uint32_t addr = R[_PC] + signed_immed(code, 8) * 2; + if (N() == V()) { + jump(addr); + } + V6M_INFO("I: BGE 0x%08x", addr); +} + +void BaseV6M::c_blt(uint32_t code) { // BLT 0xdb00-0xdbff + uint32_t addr = R[_PC] + signed_immed(code, 8) * 2; + if (N() != V()) { + jump(addr); + } + V6M_INFO("I: BLT 0x%08x", addr); +} + +void BaseV6M::c_bgt(uint32_t code) { // BGT 0xdc00-0xdcff + uint32_t addr = R[_PC] + signed_immed(code, 8) * 2; + if (Z() == 0 && N() == V()) { + jump(addr); + } + V6M_INFO("I: BGT 0x%08x", addr); +} + +void BaseV6M::c_ble(uint32_t code) { // BLE 0xdd00-0xddff + uint32_t addr = R[_PC] + signed_immed(code, 8) * 2; + if (Z() == 1 || N() != V()) { + jump(addr); + } + V6M_INFO("I: BLE 0x%08x", addr); +} + +void BaseV6M::c_b_forward(uint32_t code) { // B 0xe000-0xe3ff + uint32_t addr = R[_PC] + ((code&0x3ff)<<1); + jump(addr); + V6M_INFO("I: B 0x%08x ; forward", addr); +} + +void BaseV6M::c_b_backward(uint32_t code) { // B 0xe400-0xe7ff + uint32_t addr = R[_PC] + ((code|~0x7ff)<<1); + jump(addr); + V6M_INFO("I: B 0x%08x ; backward", addr); +} + +void BaseV6M::c_bl(uint32_t code) { // BL 0xf000-0xf7ff + uint32_t addr = R[_PC] + (signed_immed(code, 11) * 2048*2); + R[_LR] = R[_PC] | 1; + V6M_ASSERT((code2nd&0xe000) == 0xe000); + addr += (immed(code2nd, 11) * 2); + jump(addr); + V6M_INFO("I: BL 0x%08x", addr); +} + +void BaseV6M::c_bx(uint32_t code) { // BX BLX 0x4770 + d_Op_H_Rm(code); + if (op == 1) { + R[_LR] = (R[_PC] - 2) | 1; + } + uint32_t addr = *Rm & 0xfffffffe; + jump(addr); + const char* name[] = {"BX", "BLX"}; + V6M_INFO("I: %s %s ; =0x%x", name[op], GetRegName(Rm), addr); + exception_return(); +} + +void BaseV6M::c_add(uint32_t code) { // ADDS Rd,Rn,Rm 0x1800-0x19ff + d_Rm_Rn_Rd(code); + e_add(); + V6M_INFO("I: ADDS %s,%s,%s", GetRegName(Rd), GetRegName(Rn), GetRegName(Rm)); +} + +void BaseV6M::c_sub(uint32_t code) { // SUBS Rd,Rn,Rm 0x1a00-0x1bff + d_Rm_Rn_Rd(code); + e_sub(); + V6M_INFO("I: SUBS %s,%s,%s", GetRegName(Rd), GetRegName(Rn), GetRegName(Rm)); +} + +void BaseV6M::c_add1(uint32_t code) { // ADD Rd,Rn,#imm3 / MOV Rd,Rn 0x1c00-0x1dff + d_imm3_Rn_Rd(code); + e_add(); + if ((code&0x01c0) == 0x0000) { + V6M_INFO("I: MOV %s,%s", GetRegName(Rd), GetRegName(Rn)); + V6M_ASSERT((code&0xffc0) == 0x1c00); + } else { + V6M_INFO("I: ADD %s,%s,#0x%02x", GetRegName(Rd), GetRegName(Rn), R[_IM]); + } +} + +void BaseV6M::c_sub1(uint32_t code) { // SUB Rd,Rn,#imm3 0x1e00-1fff + d_imm3_Rn_Rd(code); + V6M_ASSERT(R[_IM] != 0); + e_sub(); + V6M_INFO("I: SUB %s,%s,#0x%02x", GetRegName(Rd), GetRegName(Rn), R[_IM]); +} + +void BaseV6M::c_add2(uint32_t code) { // ADD Rd,#imm8 0x3000-0x37ff + d_Rd_imm8(code); + e_add(); + V6M_INFO("I: ADD %s,#0x%02x", GetRegName(Rd), R[_IM]); +} + +void BaseV6M::c_sub2(uint32_t code) { // SUB Rd,#imm8 0x3800-0x3fff + d_Rd_imm8(code); + e_sub(); + V6M_INFO("I: SUB %s,#0x%02x", GetRegName(Rd), R[_IM]); +} + +void BaseV6M::c_mov(uint32_t code) { // MOVS Rd,#imm8 0x2000-0x27ff + d_Rd_imm8(code); + e_mov(); + V6M_INFO("I: MOVS %s,#0x%02x", GetRegName(Rd), R[_IM]); +} + +void BaseV6M::c_cmp(uint32_t code) { // CMP Rn,#imm8 0x2800-0x2fff + d_Rn_imm8(code); + e_cmp(); + V6M_INFO("I: CMP %s,#0x%02x", GetRegName(Rn), R[_IM]); +} + +void BaseV6M::c_lsl(uint32_t code) { // LSLS Rd,Rn,#imm5 0x0000-0x07ff + d_imm5_Rn_Rd(code); + e_lsl(); + V6M_INFO("I: LSLS %s,%s,#%d", GetRegName(Rd), GetRegName(Rn), R[_IM]); +} + +void BaseV6M::c_lsr(uint32_t code) { // LSRS Rd,Rn,#imm5 0x0800-0x0fff + d_imm5_Rn_Rd(code); + if (R[_IM] == 0) { + R[_IM] = 32; + } + e_lsr(); + V6M_INFO("I: LSRS %s,%s,#%d", GetRegName(Rd), GetRegName(Rn), R[_IM]); +} + +void BaseV6M::c_asr(uint32_t code) { // ASRS Rd,Rn,#imm5 0x1000-0x17ff + d_imm5_Rn_Rd(code); + if (R[_IM] == 0) { + R[_IM] = 32; + } + e_asr(); + V6M_INFO("I: ASRS %s,%s,#%d", GetRegName(Rd), GetRegName(Rn), R[_IM]); +} + +void BaseV6M::c_and_eor_lsl_lsr(uint32_t code) { // ANDS|EORS|LSLS|LSRS Rd,Rm 0x4000-0x40ff + d_Op_Rm_Rd(code); + if (op == 0) { + e_and(); + V6M_INFO("I: ANDS %s,%s,%s", GetRegName(Rd), GetRegName(Rn), GetRegName(Rm)); + } else if (op == 1) { + e_eor(); + V6M_INFO("I: EORS %s,%s,%s", GetRegName(Rd), GetRegName(Rn), GetRegName(Rm)); + } else if (op == 2) { + e_lsl(); + V6M_INFO("I: LSLS %s,%s,%s", GetRegName(Rd), GetRegName(Rn), GetRegName(Rm)); + } else { + e_lsr(); + V6M_INFO("I: LSRS %s,%s,%s", GetRegName(Rd), GetRegName(Rn), GetRegName(Rm)); + } +} + +void BaseV6M::c_asr_adc_sbc_ror(uint32_t code) { // ASRS|ADCS|SBCS|RORS Rd,Rm 0x4100-0x41ff + d_Op_Rm_Rd(code); + if (op == 4) { + e_asr(); + V6M_INFO("I: ASRS %s,%s,%s", GetRegName(Rd), GetRegName(Rn), GetRegName(Rm)); + } else if (op == 5) { + e_adc(); + V6M_INFO("I: ADCS %s,%s,%s", GetRegName(Rd), GetRegName(Rn), GetRegName(Rm)); + } else if (op == 6) { + e_sbc(); + V6M_INFO("I: SBCS %s,%s,%s", GetRegName(Rd), GetRegName(Rn), GetRegName(Rm)); + } else { + e_ror(); + V6M_INFO("I: RORS %s,%s,%s", GetRegName(Rd), GetRegName(Rn), GetRegName(Rm)); + } +} + +void BaseV6M::c_tst_neg_cmp_cmn(uint32_t code) { // TST|NEGS|CMP|CMN Rd,Rm 0x4200-0x42ff + d_Op_Rm_Rd(code); + if (op == 8) { + e_tst(); + V6M_INFO("I: TST %s,%s", GetRegName(Rn), GetRegName(Rm)); + } else if (op == 9) { + e_neg(); + V6M_INFO("I: NEG %s,%s", GetRegName(Rn), GetRegName(Rm)); + } else if (op == 10) { + e_cmp(); + V6M_INFO("I: CMP %s,%s", GetRegName(Rn), GetRegName(Rm)); + } else { + e_cmn(); + V6M_INFO("I: CMN %s,%s", GetRegName(Rn), GetRegName(Rm)); + } +} + +void BaseV6M::c_orr_mul_bic_mvn(uint32_t code) { // ORRS|MULS|BICS|MVNS Rd,Rm 0x4300-0x43ff + d_Op_Rm_Rd(code); + if (op == 12) { + e_orr(); + V6M_INFO("I: ORRS %s,%s,%s", GetRegName(Rd), GetRegName(Rn), GetRegName(Rm)); + } else if (op == 13) { + e_mul(); + V6M_INFO("I: MULS %s,%s,%s", GetRegName(Rd), GetRegName(Rn), GetRegName(Rm)); + } else if (op == 14) { + e_bic(); + V6M_INFO("I: BICS %s,%s,%s", GetRegName(Rd), GetRegName(Rn), GetRegName(Rm)); + } else { + e_mvn(); + V6M_INFO("I: MVNS %s,%s,%s", GetRegName(Rd), GetRegName(Rn), GetRegName(Rm)); + } +} + +void BaseV6M::c_add_hr(uint32_t code) { // ADD Rd,Rm (high reg) 0x4400-0x44ff + d_D_M_Rm_Rd(code); + *Rd = add32(*Rn, *Rm); + V6M_INFO("I: ADD %s,%s,%s", GetRegName(Rd), GetRegName(Rn), GetRegName(Rm)); +} + +void BaseV6M::c_cmp_hr(uint32_t code) { // CMP Rd,Rm (high reg) 0x4500-0x45ff + d_D_M_Rm_Rd(code); + V6M_ASSERT(Rd != &R[_PC]); + Rd = &R[_NL]; + e_cmp(); + V6M_INFO("I: CMP %s,%s", GetRegName(Rn), GetRegName(Rm)); +} + +void BaseV6M::c_mov_hr(uint32_t code) { // MOV Rd,Rm (high reg) 0x4600-0x46ff + d_D_M_Rm_Rd(code); + *Rd = *Rm; + if (Rd == &R[_PC]) { + jump(R[_PC]); + V6M_INFO("I: MOV %s,%s", GetRegName(Rd), GetRegName(Rm)); + exception_return(); + } else { + V6M_INFO("I: MOV %s,%s", GetRegName(Rd), GetRegName(Rm)); + } +} + +void BaseV6M::c_add_r_pc(uint32_t code) { // ADD Rd,pc,#imm8 0xa000-0xa7ff + d_Rd_imm8(code); + uint32_t off = R[_IM] * 4; + uint32_t addr = align32(R[_PC]) + off; + *Rd = addr; + V6M_INFO("I: ADD %s,pc,#0x%02x ; @0x%08x", GetRegName(Rd), off, addr); +} + +void BaseV6M::c_add_r_sp(uint32_t code) { // ADD Rd,sp,#imm8 0xa800-0xafff + d_Rd_imm8(code); + uint32_t off = R[_IM] * 4; + uint32_t addr = *Rd = R[_SP] + off; + *Rd = addr; + V6M_INFO("I: ADD %s,sp,#0x%02x ; @0x%08x", GetRegName(Rd), off, addr); + V6M_ASSERT(addr%4 == 0); +} + +void BaseV6M::c_add_sp(uint32_t code) { // ADD|SUB sp,sp,#imm 0xb000-0xb0ff + d_Op_imm7(code); + uint32_t data = R[_IM] * 4; + if (op == 0) { + R[_SP] += data; + } else { + R[_SP] -= data; + } + const char* name[] = {"ADD", "SUB"}; + V6M_INFO("I: %s sp,sp,#0x%x", name[op], data); +} + +void BaseV6M::c_ldr1(uint32_t code) { // LDR Rd,[Rn,#imm5] 0x6800-0x6fff + d_imm5_Rn_Rd(code); + e_ldr<uint32_t>(true); + V6M_INFO("I: LDR %s,[%s,#0x%02x] ; =0x%x @0x%x", GetRegName(Rd), GetRegName(Rn), R[_IM], data, addr); +} + +void BaseV6M::c_str1(uint32_t code) { // STR Rd,[Rn,#imm5] 0x6000-0x67ff + d_imm5_Rn_Rd(code); + e_str<uint32_t>(true); + V6M_INFO("I: STR %s,[%s,#0x%02x] ; =0x%x @0x%x", GetRegName(Rd), GetRegName(Rn), R[_IM], data, addr); +} + +void BaseV6M::c_ldrb(uint32_t code) { // LDRB Rd,[Rn,#imm5] 0x7800-0x7fff + d_imm5_Rn_Rd(code); + e_ldr<uint8_t>(true); + V6M_INFO("I: LDRB %s,[%s,#0x%02x] ; =0x%02x @0x%x", GetRegName(Rd), GetRegName(Rn), R[_IM], data, addr); +} + +void BaseV6M::c_strb(uint32_t code) { // STRB Rd,[Rn,#imm5] 0x7000-0x77ff + d_imm5_Rn_Rd(code); + e_str<uint8_t>(true); + V6M_INFO("I: STRB %s,[%s,#0x%02x] ; =0x%x @0x%x", GetRegName(Rd), GetRegName(Rn), R[_IM], data, addr); +} + +void BaseV6M::c_ldrh(uint32_t code) { // LDRH Rd,[Rn,#imm5] 0x8800-0x8fff + d_imm5_Rn_Rd(code); + e_ldr<uint16_t>(true); + V6M_INFO("I: LDRH %s,[%s,#0x%02x] ; =0x%x @0x%x", GetRegName(Rd), GetRegName(Rn), R[_IM], data, addr); +} + +void BaseV6M::c_strh(uint32_t code) { // STRH Rd,[Rn,#imm5] 0x8000-0x87ff + d_imm5_Rn_Rd(code); + e_str<uint16_t>(true); + V6M_INFO("I: STRH %s,[%s,#0x%02x] ; =0x%x @0x%x", GetRegName(Rd), GetRegName(Rn), R[_IM], data, addr); +} + +void BaseV6M::c_ldr2(uint32_t code) { // LDR Rd,[Rn,Rm] 0x5800-0x59ff + d_Rm_Rn_Rd(code); + e_ldr<uint32_t>(); + V6M_INFO("I: LDR %s,[%s,%s] ; =0x%08x @0x%08x", GetRegName(Rd), GetRegName(Rn), GetRegName(Rm), data, addr); +} + +void BaseV6M::c_str2(uint32_t code) { // STR Rd,[Rn,Rm] 0x5000-0x51ff + d_Rm_Rn_Rd(code); + e_str<uint32_t>(); + V6M_INFO("I: STR %s,[%s,%s] ; =0x%08x @0x%08x", GetRegName(Rd), GetRegName(Rn), GetRegName(Rm), data, addr); +} + +void BaseV6M::c_ldrh2(uint32_t code) { // LDRH Rd,[Rn,Rm] 0x5a00-0x5bff + d_Rm_Rn_Rd(code); + e_ldr<uint16_t>(); + V6M_INFO("I: LDRH %s,[%s,%s] ; =0x%04x @0x%08x", GetRegName(Rd), GetRegName(Rn), GetRegName(Rm), data, addr); +} + +void BaseV6M::c_strh2(uint32_t code) { // STRH Rd,[Rn,Rm] 0x5200-0x53ff + d_Rm_Rn_Rd(code); + e_str<uint16_t>(); + V6M_INFO("I: STRH %s,[%s,%s] ; =0x%04x @0x%08x", GetRegName(Rd), GetRegName(Rn), GetRegName(Rm), data, addr); +} + +void BaseV6M::c_ldrsh(uint32_t code) { // LDRSH Rd,[Rn,Rm] 0x5e00-0x5fff + d_Rm_Rn_Rd(code); + e_ldr<uint16_t>(); + uint32_t data = signed_immed(*Rd, 16) & 0xffffffff; + *Rd = data; + V6M_INFO("I: LDRSH %s,[%s,%s] ; =0x%x @0x%08x", GetRegName(Rd), GetRegName(Rn), GetRegName(Rm), data, addr); +} + +void BaseV6M::c_ldrb2(uint32_t code) { // LDRB Rd,[Rn,Rm] 0x5c00-0x5dff + d_Rm_Rn_Rd(code); + e_ldr<uint8_t>(); + V6M_INFO("I: LDRB %s,[%s,%s] ; =0x%02x @0x%08x", GetRegName(Rd), GetRegName(Rn), GetRegName(Rm), data, addr); +} + +void BaseV6M::c_strb2(uint32_t code) { // STRB Rd,[Rn,Rm] 0x5400-0x55ff + d_Rm_Rn_Rd(code); + e_str<uint8_t>(); + V6M_INFO("I: STRB %s,[%s,%s] ; =0x%02x @0x%08x", GetRegName(Rd), GetRegName(Rn), GetRegName(Rm), data, addr); +} + +void BaseV6M::c_ldrsb(uint32_t code) { // LDRSB Rd,[Rn,Rm] 0x5600-0x57ff + d_Rm_Rn_Rd(code); + e_ldr<uint8_t>(); + uint32_t data = signed_immed(*Rd, 8) & 0xffffffff; + *Rd = data; + V6M_INFO("I: LDRSB %s,[%s,%s] ; =0x%x @0x%08x", GetRegName(Rd), GetRegName(Rn), GetRegName(Rm), data, addr); +} + +void BaseV6M::c_ldr_pc(uint32_t code) { // LDR Rd,[pc,#imm8] 0x4800-0x4fff + d_Rd_imm8(code); + uint32_t off = *Rm * 4; + uint32_t addr = align32(R[_PC]) + off; + uint32_t data = peek32(addr); + *Rd = data; + V6M_INFO("I: LDR %s,[pc,#%d] ; =0x%x @0x%08x", GetRegName(Rd), off, data, addr); +} + +void BaseV6M::c_ldr_sp(uint32_t code) { // LDR Rd,[sp,#imm8] 0x9800-0x9fff + d_Rd_imm8(code); + Rn = &R[_SP]; + e_ldr<uint32_t>(true); + V6M_INFO("I: LDR %s,[sp,#0x%x] ; =0x%08x @0x%08x", GetRegName(Rd), R[_IM], data, addr); +} + +void BaseV6M::c_str_sp(uint32_t code) { // STR Rd,[sp,#imm8] 0x9000-0x97ff + d_Rd_imm8(code); + Rn = &R[_SP]; + e_str<uint32_t>(true); + V6M_INFO("I: STR %s,[sp,#0x%x] ; =0x08%x @0x%08x", GetRegName(Rd), R[_IM], data, addr); +} + +void BaseV6M::e_ldm() { // LDM / POP + uint32_t addr = *Rn; + for (int k = 0; k <= 7; k++) { + if (reg_list & (1<<k)) { + R[k] = peek32(addr); + addr += 4; + } + } + if (! (reg_list & (1<<GetRegIndex(Rn)))) { + *Rn = addr; + } +} + +void BaseV6M::e_stm() { // STM + uint32_t addr = *Rn; + for (int k = 0; k <= 7; k++) { + if (reg_list & (1<<k)) { + poke32(addr, R[k]); + addr += 4; + } + } + *Rn = addr; +} + +void BaseV6M::e_push() { // PUSH + for (int k = 7; k >= 0; k--) { + if (reg_list & (1<<k)) { + push(R[k]); + } + } +} + +void BaseV6M::c_ldm(uint32_t code) { // LDM Rn!,{reg_list} 0xc800-0xcfff + d_Rn_reg_list(code); + e_ldm(); + V6M_INFO("I: LDMIA %s!,{%s}", GetRegName(Rn), StrRegLists()); +} + +void BaseV6M::c_stm(uint32_t code) { // STM Rn!,{reg_list} 0xc000-0xc7ff + d_Rn_reg_list(code); + e_stm(); + V6M_INFO("I: STMIA %s!,{%s}", GetRegName(Rn), StrRegLists()); +} + +void BaseV6M::c_pop(uint32_t code) { // POP {reg_list} 0xbc00-0xbcff + d_reg_list(code); + Rn = &R[_SP]; + e_ldm(); + V6M_INFO("I: POP {%s}", StrRegLists()); +} + +void BaseV6M::c_pop_pc(uint32_t code) { // POP {reg_list,pc} 0xbd00-0xbdff + d_reg_list(code); + Rn = &R[_SP]; + e_ldm(); + jump(pop()); + V6M_INFO("I: POP {%s,pc}", StrRegLists()); +} + +void BaseV6M::c_push(uint32_t code) { // PUSH {reg_list} 0xb400-0xb4ff + d_reg_list(code); + e_push(); + V6M_INFO("I: PUSH {%s}", StrRegLists()); +} + +void BaseV6M::c_push_lr(uint32_t code) { // PUSH {reg_list,lr} 0xb500-0xb5ff + d_reg_list(code); + push(R[_LR]); + e_push(); + V6M_INFO("I: PUSH {%s,lr}", StrRegLists()); +} + +void BaseV6M::c_uxt(uint32_t code) { // UXTH|UXTB Rd,Rm 0xb200-0xb2ff + d_Op_Rm_Rd(code); + if (op == 10) { + *Rd = *Rm & 0xffff; + V6M_INFO("I: UXTH %s,%s", GetRegName(Rd), GetRegName(Rm)); + V6M_ASSERT((code&0xffc0) == 0xb280); + } else if (op == 11) { + *Rd = *Rm & 0xff; + V6M_INFO("I: UXTB %s,%s", GetRegName(Rd), GetRegName(Rm)); + V6M_ASSERT((code&0xffc0) == 0xb2c0); + } else if (op == 8) { + *Rd = signed_immed(*Rm, 16) & 0xffffffff; + V6M_INFO("I: SXTH %s,%s", GetRegName(Rd), GetRegName(Rm)); + V6M_ASSERT((code&0xffc0) == 0xb200); + } else if (op == 9) { + *Rd = signed_immed(*Rm, 8) & 0xffffffff; + V6M_INFO("I: SXTB %s,%s", GetRegName(Rd), GetRegName(Rm)); + V6M_ASSERT((code&0xffc0) == 0xb240); + } else { + V6M_ASSERT(0); + } +} + +void BaseV6M::c_rev(uint32_t code) { // REV|REV16|REVSH Rd,Rm 0xba00-0xba3f + d_Op_Rm_Rd(code); + if (op == 8) { + uint32_t data = *Rm; + *Rd = ((data<<24)&0xff000000)|((data<<8)&0xff0000)|((data>>8)&0xff00)|data>>24; + V6M_INFO("I: REV %s,%s", GetRegName(Rd), GetRegName(Rm)); + } + if (op == 9) { + uint32_t data = *Rm; + *Rd = ((data<<8)&0xff00ff00)|((data>>8)&0x00ff00ff); + V6M_INFO("I: REV16 %s,%s", GetRegName(Rd), GetRegName(Rm)); + } + if (op == 11) { + uint32_t data = *Rm; + *Rd = ((data<<8)&0xff00)|((data>>8)&0x00ff); + if (*Rd & 0x8000) { + *Rd |= 0xffff0000; + } + V6M_INFO("I: REVSH %s,%s", GetRegName(Rd), GetRegName(Rm)); + } else { + V6M_ASSERT(0); + } +} + +void BaseV6M::c_nop(uint32_t code) { // NOP 0xbf00 + V6M_ASSERT(code == 0xbf00); + V6M_INFO("I: NOP"); +} + +void BaseV6M::c_bkpt(uint32_t code) { // BKPT #imm8 0xbe00-0xbeff + V6M_ASSERT(0); +} + +void BaseV6M::c_swi(uint32_t code) { // SWI #imm8 0xdf00-0xdfff + V6M_INFO("I: SWI #%d", immed(code, 8, 0)); + exception_entry(11); +} + +void BaseV6M::c_cps(uint32_t code) { // CPSIE|CPSID A|I|F 0xb6c0-0xb6e7 + uint32_t i = immed(code, 1,4); + uint32_t f = immed(code, 3,0); + V6M_ASSERT(f != 0); + const char* i_name[] = {"CPSIE", "CPSID"}; + const char* f_name[] = {"", "F", "I", "IF", "A", "AF", "AI", "AIF"}; + V6M_INFO("I: %s %s", i_name[i], f_name[f]); +} + +void BaseV6M::c_todo(uint32_t code) { // undefined code + V6M_ERROR("I: ASSERT!!! %04x %04x", code, code2nd); + V6M_ASSERT(0); +} + +void BaseV6M::fetch() { + if (cache) { + code = code2nd; + code2nd = peek16(align16(R[_PC])); + R[_PC] += 2; + } else { + uint32_t d = peek32(align16(R[_PC])); + code = d & 0xffff; + code2nd = d>>16; + R[_PC] += 4; + cache = true; + } +} + +void BaseV6M::execute() { + switch(code>>8) { + case 0xd0: c_beq(code); break; + case 0xd1: c_bne(code); break; + case 0xd2: c_bcs(code); break; + case 0xd3: c_bcc(code); break; + case 0xd4: c_bmi(code); break; + case 0xd5: c_bpl(code); break; + case 0xd6: c_bvs(code); break; + case 0xd7: c_bvc(code); break; + case 0xd8: c_bhi(code); break; + case 0xd9: c_bls(code); break; + case 0xda: c_bge(code); break; + case 0xdb: c_blt(code); break; + case 0xdc: c_bgt(code); break; + case 0xdd: c_ble(code); break; + case 0xe0 ... 0xe3: c_b_forward(code); break; + case 0xe4 ... 0xe7: c_b_backward(code); break; + case 0xf0 ... 0xf7: c_bl(code); break; + case 0x47: c_bx(code); break; + case 0x18 ... 0x19: c_add(code); break; + case 0x1a ... 0x1b: c_sub(code); break; + case 0x1c ... 0x1d: c_add1(code); break; + case 0x1e ... 0x1f: c_sub1(code); break; + case 0x30 ... 0x37: c_add2(code); break; + case 0x38 ... 0x3f: c_sub2(code); break; + case 0x20 ... 0x27: c_mov(code); break; + case 0x28 ... 0x2f: c_cmp(code); break; + case 0x00 ... 0x07: c_lsl(code); break; + case 0x08 ... 0x0f: c_lsr(code); break; + case 0x10 ... 0x17: c_asr(code); break; + case 0x40: c_and_eor_lsl_lsr(code); break; + case 0x41: c_asr_adc_sbc_ror(code); break; + case 0x42: c_tst_neg_cmp_cmn(code); break; + case 0x43: c_orr_mul_bic_mvn(code); break; + case 0x44: c_add_hr(code); break; + case 0x45: c_cmp_hr(code); break; + case 0x46: c_mov_hr(code); break; + case 0xa0 ... 0xa7: c_add_r_pc(code); break; + case 0xa8 ... 0xaf: c_add_r_sp(code); break; + case 0xb0: c_add_sp(code); break; + case 0x68 ... 0x6f: c_ldr1(code); break; + case 0x60 ... 0x67: c_str1(code); break; + case 0x78 ... 0x7f: c_ldrb(code); break; + case 0x70 ... 0x77: c_strb(code); break; + case 0x88 ... 0x8f: c_ldrh(code); break; + case 0x80 ... 0x87: c_strh(code); break; + case 0x58 ... 0x59: c_ldr2(code); break; + case 0x50 ... 0x51: c_str2(code); break; + case 0x5a ... 0x5b: c_ldrh2(code); break; + case 0x52 ... 0x53: c_strh2(code); break; + case 0x5e ... 0x5f: c_ldrsh(code); break; + case 0x5c ... 0x5d: c_ldrb2(code); break; + case 0x54 ... 0x55: c_strb2(code); break; + case 0x56 ... 0x57: c_ldrsb(code); break; + case 0x48 ... 0x4f: c_ldr_pc(code); break; + case 0x98 ... 0x9f: c_ldr_sp(code); break; + case 0x90 ... 0x97: c_str_sp(code); break; + case 0xc8 ... 0xcf: c_ldm(code); break; + case 0xc0 ... 0xc7: c_stm(code); break; + case 0xbc: c_pop(code); break; + case 0xbd: c_pop_pc(code); break; + case 0xb4: c_push(code); break; + case 0xb5: c_push_lr(code); break; + case 0xb2: c_uxt(code); break; + case 0xba: c_rev(code); break; + case 0xbf: c_nop(code); break; + case 0xbe: c_bkpt(code); break; + case 0xdf: c_swi(code); break; + case 0xb6: c_cps(code); break; + default: c_todo(code); break; + } + cycle++; +} + +void BaseV6M::reset() { + R[_SP] = peek32(0x00000000); + R[_PC] = peek32(0x00000004); + cache = false; +} + +void BaseV6M::run(int step) { + while(step-- > 0) { + fetch(); + execute(); + } +}