File content as of revision 5:65d77b2e6bc7:

// BaseV6M.cpp 2016/3/28
#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 uint32_t immed(uint32_t code, int width, int base = 0) { // unsigned immed
    return (code>>base) & ((1<<width)-1);
}

static 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 uint32_t add32(uint32_t a, uint32_t b) {
    return (a + b) & 0xffffffff;
}

static uint32_t mul32(uint32_t a, uint32_t b) {
    return (a * b) & 0xffffffff;
}

static uint32_t not32(uint32_t data) {
    return (~data)&0xffffffff;
}

static uint32_t align32(uint32_t addr) {
    return addr & 0xfffffffc;
}

static 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] = align16(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::d32_Rd_SYSm(uint32_t code, uint32_t code2nd) { // MRS
    Rd = &R[immed(code2nd, 4, 8)];
    int m = immed(code2nd, 7, 0);
}

void BaseV6M::d32_Rn_SYSm(uint32_t code, uint32_t code2nd) { // MSR
    Rn = &R[immed(code, 4, 0)];
    int m = immed(code2nd, 7, 0);
}

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::c32_bl_forward(uint32_t code) { // BL 0xf000-0xf2ff
    R[_LR] = R[_PC] | 1;
    uint32_t imm32 = immed(code, 10)<<12|immed(code2nd, 11)<<1;
    if ((code2nd & (1<<13)) == 0) { // J1
        imm32 |= (1<<23);
    }
    if ((code2nd & (1<<11)) == 0) { // J2
        imm32 |= (1<<22);
    }
    uint32_t addr = R[_PC] + imm32;
    V6M_ASSERT((code2nd&0xd000) == 0xd000);
    jump(addr);
    V6M_INFO("I: BL 0x%08x ; forward", addr);
}

void BaseV6M::c32_bl_forward3(uint32_t code) { // BL 0xf300-0xf3ff
    if ((code2nd&0xd000) == 0xd000) {
        c32_bl_forward(code);
        return;
    }
    if ((code&0xf0) == 0x80) {
        c32_msr(code, code2nd);
    } else if ((code&0xff) == 0xbf) {
        c32_dsb_dmb_isb(code, code2nd);
    } else if ((code&0xff) == 0xef) {
        c32_mrs(code, code2nd);
    } else {
        V6M_ASSERT(0);
    }
}

void BaseV6M::c32_mrs(uint32_t code, uint32_t code2nd) {
    d32_Rd_SYSm(code, code2nd);
    jump(R[_PC]);
    V6M_INFO("I: MRS %s,%s", GetRegName(Rd), GetSysName(Rm));
}

void BaseV6M::c32_msr(uint32_t code, uint32_t code2nd) {
    d32_Rn_SYSm(code, code2nd);
    jump(R[_PC]);
    V6M_INFO("I: MSR %s,%s", GetSysName(Rm), GetRegName(Rn));
}

void BaseV6M::c32_dsb_dmb_isb(uint32_t code, uint32_t code2nd) {
    if (code2nd == 0x8f4f) {
        jump(R[_PC]);
        V6M_INFO("I: DSB.W");
    } else if (code2nd == 0x8f5f) {
        jump(R[_PC]);
        V6M_INFO("I: DMB.W");
    } else if (code2nd == 0x8f6f) {
        jump(R[_PC]);
        V6M_INFO("I: ISB.W");
    } else {
        V6M_ASSERT(0);
    }
}

void BaseV6M::c32_bl_backward(uint32_t code) { // BL 0xf400-0xf7ff
    R[_LR] = R[_PC] | 1;
    uint32_t imm32 = 0xff000000|immed(code, 10)<<12|immed(code2nd, 11)<<1;
    if (code2nd & (1<<13)) { // J1
        imm32 |= (1<<23);
    }
    if (code2nd & (1<<11)) { // J2
        imm32 |= (1<<22);
    }
    uint32_t addr = R[_PC] + imm32;
    V6M_ASSERT((code2nd&0xd000) == 0xd000);
    jump(addr);
    V6M_INFO("I: BL 0x%08x ; backward", 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);
    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);
    if (Rd == &R[_PC]) {
        jump(R[_PC]);
        V6M_INFO("I: ADD pc,pc,%s ; pc=0x%08x", GetRegName(Rn), R[_PC]);
        exception_return();
    } else {
        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() {
    V6M_ASSERT((R[_PC]&1) == 0);
    if (cache) {
        code = code2nd;
        code2nd = peek16(R[_PC]);
        R[_PC] += 2;
    } else {
        uint32_t d = peek32(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 ... 0xf2: c32_bl_forward(code); break;
        case 0xf3: c32_bl_forward3(code); break;
        case 0xf4 ... 0xf7: c32_bl_backward(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);
    jump(R[_PC]);
}

void BaseV6M::run(int step) {
    while(step-- > 0) {
        fetch();
        execute();
    }
}