+ LOG_ERROR(Core_ARM11, "Failed to decode thumb opcode %04X at %08X", instr, pc);

+ tmp_rd = tmp_rn = dst = 0;

+ case 0x04: /* SUB reg */

+ // Signifies UMAAL

+ if (state->is_v6 && BITS(4, 7) == 0x09) {

+ if (handle_v6_insn(state, instr))

+ break;

+ }

+

+ SETQ;

+ SETQ;

+ DEST = ARMul_GetCPSR(state);

+ ARMul_CPSRAltered (state);

+ SETQ;

+ SETQ;

+ ARMul_CPSRAltered (state);

+ SETQ;

+ SETQ;

+ ARMul_CPSRAltered (state);

+ SETQ;

+ SETQ;

+ ARMul_CPSRAltered (state);

+ if (BITS(4, 11) == 0xF9) { //strexd

+ u32 l = LHSReg;

+

+ bool enter = false;

+

+ if (state->currentexval == (u32)ARMul_ReadWord(state, state->currentexaddr)&&

+ state->currentexvald == (u32)ARMul_ReadWord(state, state->currentexaddr + 4))

+ enter = true;

+

+

+ //todo bug this and STREXD and LDREXD http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.ddi0360e/CHDGJGGC.html

+

+

+ if (enter) {

+ ARMul_StoreWordN(state, LHS, state->Reg[RHSReg]);

+ ARMul_StoreWordN(state,LHS + 4 , state->Reg[RHSReg + 1]);

+ state->Reg[DESTReg] = 0;

+ } else {

+ state->Reg[DESTReg] = 1;

+ }

+

+ break;

+ }

+ case 0x1B: /* MOVS reg */

+ /* ldrexd ichfly */

+ if (BITS(0, 11) == 0xF9F) { //strexd

+ lhs = LHS;

+

+ state->currentexaddr = lhs;

+ state->currentexval = (u32)ARMul_ReadWord(state, lhs);

+ state->currentexvald = (u32)ARMul_ReadWord(state, lhs + 4);

+

+ state->Reg[DESTReg] = ARMul_LoadWordN(state, lhs);

+ state->Reg[DESTReg] = ARMul_LoadWordN(state, lhs + 4);

+ break;

+ }

+

+

+

+

+

+ ARMul_StoreHalfWord(state, lhs, RHS);

+ ARMul_CPSRAltered (state);

+ ARMul_CPSRAltered (state);

+ ARMul_CPSRAltered (state);

+ ARMul_CPSRAltered (state);

+ if ((instr & 0x70) == 0x10) { //pkhbt

+ } else if ((instr & 0x70) == 0x50) { //pkhtb

+ u8 rd_idx = BITS(12, 15);

+ u8 rn_idx = BITS(16, 19);

+ u8 rm_idx = BITS(0, 3);

+ u8 imm5 = BITS(7, 11) ? BITS(7, 11) : 31;

+ state->Reg[rd_idx] = ((static_cast<s32>(state->Reg[rm_idx]) >> imm5) & 0xFFFF) | ((state->Reg[rn_idx]) & 0xFFFF0000);

+ } else if (BIT (4)) {

+ LOG_DEBUG(Core_ARM11, "got unhandled special breakpoint");

+ if ((instr & 0x0FF00FF0) == 0xC400B10) { //vmov BIT(0-3), BIT(12-15), BIT(16-20), vmov d0, r0, r0

+ } else if (state->is_v5) {

+ if ((instr & 0x00000FF0) == 0xB10) { //vmov BIT(12-15), BIT(16-20), BIT(0-3) vmov r0, r0, d0

+ } else if (state->is_v5) {

+ else if (!CP_ACCESS_ALLOWED(state, CPNum)) {

+ } else {

+ ARMul_CPSRAltered (state);

+ ARMul_CPSRAltered (state);

+ static int handle_v6_insn(ARMul_State* state, ARMword instr) {

+ switch (BITS(20, 27)) {

+ case 0x04: // UMAAL

+ {

+ const u8 rm_idx = BITS(8, 11);

+ const u8 rn_idx = BITS(0, 3);

+ const u8 rd_lo_idx = BITS(12, 15);

+ const u8 rd_hi_idx = BITS(16, 19);

+

+ const u32 rm_val = state->Reg[rm_idx];

+ const u32 rn_val = state->Reg[rn_idx];

+ const u32 rd_lo_val = state->Reg[rd_lo_idx];

+ const u32 rd_hi_val = state->Reg[rd_hi_idx];

+

+ const u64 result = (rn_val * rm_val) + rd_lo_val + rd_hi_val;

+

+ state->Reg[rd_lo_idx] = (result & 0xFFFFFFFF);

+ state->Reg[rd_hi_idx] = ((result >> 32) & 0xFFFFFFFF);

+ return 1;

+ }

+ if (BITS(4, 7) == 0x9) {

+ /* strex */

+ u32 l = LHSReg;

+ u32 r = RHSReg;

+ u32 lhs = LHS;

+

+ bool enter = false;

+

+ if (state->currentexval == (u32)ARMul_ReadWord(state, state->currentexaddr))enter = true;

+ //StoreWord(state, lhs, RHS)

+ if (state->Aborted) {

+ TAKEABORT;

+ }

+

+ if (enter) {

+ ARMul_StoreWordS(state, lhs, RHS);

+ state->Reg[DESTReg] = 0;

+ }

+ else {

+ state->Reg[DESTReg] = 1;

+ }

+

+ return 1;

+ }

+ /* ldrex */

+ if (BITS(4, 7) == 0x9) {

+ u32 lhs = LHS;

+

+ state->currentexaddr = lhs;

+ state->currentexval = ARMul_ReadWord(state, lhs);

+

+ LoadWord(state, instr, lhs);

+ return 1;

+ }

+ if (BITS(4, 7) == 0x9) {

+ /* strexb */

+ u32 lhs = LHS;

+

+ bool enter = false;

+

+ if (state->currentexval == (u32)ARMul_ReadByte(state, state->currentexaddr))enter = true;

+

+ BUSUSEDINCPCN;

+ if (state->Aborted) {

+ TAKEABORT;

+ }

+

+

+ if (enter) {

+ ARMul_StoreByte(state, lhs, RHS);

+ state->Reg[DESTReg] = 0;

+ }

+ else {

+ state->Reg[DESTReg] = 1;

+ }

+

+ //printf("In %s, strexb not implemented\n", __FUNCTION__);

+ UNDEF_LSRBPC;

+ /* WRITESDEST (dest); */

+ return 1;

+ }

+ if ((BITS(4, 7)) == 0x9) {

+ /* ldrexb */

+ u32 lhs = LHS;

+ LoadByte(state, instr, lhs, LUNSIGNED);

+

+ state->currentexaddr = lhs;

+ state->currentexval = (u32)ARMul_ReadByte(state, lhs);

+

+ //state->Reg[BITS(12, 15)] = ARMul_LoadByte(state, state->Reg[BITS(16, 19)]);

+ //printf("ldrexb\n");

+ //printf("instr is %x rm is %d\n", instr, BITS(16, 19));

+ //exit(-1);

+

+ //printf("In %s, ldrexb not implemented\n", __FUNCTION__);

+ return 1;

+ }

+ case 0x61: // SADD16, SASX, SSAX, and SSUB16

+ if ((instr & 0xFF0) == 0xf10 || (instr & 0xFF0) == 0xf30 ||

+ (instr & 0xFF0) == 0xf50 || (instr & 0xFF0) == 0xf70)

+ const u8 rd_idx = BITS(12, 15);

+ const u8 rm_idx = BITS(0, 3);

+ const u8 rn_idx = BITS(16, 19);

+ const s16 rn_lo = (state->Reg[rn_idx] & 0xFFFF);

+ const s16 rn_hi = ((state->Reg[rn_idx] >> 16) & 0xFFFF);

+ const s16 rm_lo = (state->Reg[rm_idx] & 0xFFFF);

+ const s16 rm_hi = ((state->Reg[rm_idx] >> 16) & 0xFFFF);

+

+ s32 lo_result;

+ s32 hi_result;

+

+ // SADD16

+ if ((instr & 0xFF0) == 0xf10) {

+ lo_result = (rn_lo + rm_lo);

+ hi_result = (rn_hi + rm_hi);

+ }

+ // SASX

+ else if ((instr & 0xFF0) == 0xf30) {

+ lo_result = (rn_lo - rm_hi);

+ hi_result = (rn_hi + rm_lo);

+ }

+ // SSAX

+ else if ((instr & 0xFF0) == 0xf50) {

+ lo_result = (rn_lo + rm_hi);

+ hi_result = (rn_hi - rm_lo);

+ }

+ // SSUB16

+ else {

+ lo_result = (rn_lo - rm_lo);

+ hi_result = (rn_hi - rm_hi);

+ }

+

+ state->Reg[rd_idx] = (lo_result & 0xFFFF) | ((hi_result & 0xFFFF) << 16);

+

+ if (lo_result >= 0) {

+ state->GEFlag |= (1 << 16);

+ state->GEFlag |= (1 << 17);

+ } else {

+ state->GEFlag &= ~(1 << 16);

+ state->GEFlag &= ~(1 << 17);

+ }

+

+ if (hi_result >= 0) {

+ state->GEFlag |= (1 << 18);

+ state->GEFlag |= (1 << 19);

+ } else {

+ state->GEFlag &= ~(1 << 18);

+ state->GEFlag &= ~(1 << 19);

+ }

+

+ // SADD8/SSUB8

+ else if ((instr & 0xFF0) == 0xf90 || (instr & 0xFF0) == 0xff0)

+ const u8 rd_idx = BITS(12, 15);

+ const u8 rm_idx = BITS(0, 3);

+ const u8 rn_idx = BITS(16, 19);

+ const u32 rm_val = state->Reg[rm_idx];

+ const u32 rn_val = state->Reg[rn_idx];

+

+ u8 lo_val1;

+ u8 lo_val2;

+ u8 hi_val1;

+ u8 hi_val2;

+

+ // SADD8

+ if ((instr & 0xFF0) == 0xf90) {

+ lo_val1 = (u8)((rn_val & 0xFF) + (rm_val & 0xFF));

+ lo_val2 = (u8)(((rn_val >> 8) & 0xFF) + ((rm_val >> 8) & 0xFF));

+ hi_val1 = (u8)(((rn_val >> 16) & 0xFF) + ((rm_val >> 16) & 0xFF));

+ hi_val2 = (u8)(((rn_val >> 24) & 0xFF) + ((rm_val >> 24) & 0xFF));

+

+ if (lo_val1 & 0x80)

+ state->GEFlag |= (1 << 16);

+ else

+ state->GEFlag &= ~(1 << 16);

+

+ if (lo_val2 & 0x80)

+ state->GEFlag |= (1 << 17);

+ else

+ state->GEFlag &= ~(1 << 17);

+

+ if (hi_val1 & 0x80)

+ state->GEFlag |= (1 << 18);

+ else

+ state->GEFlag &= ~(1 << 18);

+

+ if (hi_val2 & 0x80)

+ state->GEFlag |= (1 << 19);

+ else

+ state->GEFlag &= ~(1 << 19);

+ }

+ // SSUB8

+ else {

+ lo_val1 = (u8)((rn_val & 0xFF) - (rm_val & 0xFF));

+ lo_val2 = (u8)(((rn_val >> 8) & 0xFF) - ((rm_val >> 8) & 0xFF));

+ hi_val1 = (u8)(((rn_val >> 16) & 0xFF) - ((rm_val >> 16) & 0xFF));

+ hi_val2 = (u8)(((rn_val >> 24) & 0xFF) - ((rm_val >> 24) & 0xFF));

+

+ if (!(lo_val1 & 0x80))

+ state->GEFlag |= (1 << 16);

+ else

+ state->GEFlag &= ~(1 << 16);

+

+ if (!(lo_val2 & 0x80))

+ state->GEFlag |= (1 << 17);

+ else

+ state->GEFlag &= ~(1 << 17);

+

+ if (!(hi_val1 & 0x80))

+ state->GEFlag |= (1 << 18);

+ else

+ state->GEFlag &= ~(1 << 18);

+

+ if (!(hi_val2 & 0x80))

+ state->GEFlag |= (1 << 19);

+ else

+ state->GEFlag &= ~(1 << 19);

+ }

+

+ state->Reg[rd_idx] = (lo_val1 | lo_val2 << 8 | hi_val1 << 16 | hi_val2 << 24);

+ else {

+ printf("Unhandled v6 insn: %08x", instr);

+ case 0x62: // QADD16, QASX, QSAX, QSUB16, QADD8, and QSUB8

+ const u8 op2 = BITS(5, 7);

+

+ const u8 rd_idx = BITS(12, 15);

+ const u8 rn_idx = BITS(16, 19);

+ const u8 rm_idx = BITS(0, 3);

+ const u16 rm_lo = (state->Reg[rm_idx] & 0xFFFF);

+ const u16 rm_hi = ((state->Reg[rm_idx] >> 0x10) & 0xFFFF);

+ const u16 rn_lo = (state->Reg[rn_idx] & 0xFFFF);

+ const u16 rn_hi = ((state->Reg[rn_idx] >> 0x10) & 0xFFFF);

+

+ u16 lo_result = 0;

+ u16 hi_result = 0;

+

+ // QADD16

+ if (op2 == 0x00) {

+ lo_result = ARMul_SignedSaturatedAdd16(rn_lo, rm_lo);

+ hi_result = ARMul_SignedSaturatedAdd16(rn_hi, rm_hi);

+ }

+ // QASX

+ else if (op2 == 0x01) {

+ lo_result = ARMul_SignedSaturatedSub16(rn_lo, rm_hi);

+ hi_result = ARMul_SignedSaturatedAdd16(rn_hi, rm_lo);

+ }

+ // QSAX

+ else if (op2 == 0x02) {

+ lo_result = ARMul_SignedSaturatedAdd16(rn_lo, rm_hi);

+ hi_result = ARMul_SignedSaturatedSub16(rn_hi, rm_lo);

+ }

+ // QSUB16

+ else if (op2 == 0x03) {

+ lo_result = ARMul_SignedSaturatedSub16(rn_lo, rm_lo);

+ hi_result = ARMul_SignedSaturatedSub16(rn_hi, rm_hi);

+ }

+ // QADD8

+ else if (op2 == 0x04) {

+ lo_result = ARMul_SignedSaturatedAdd8(rn_lo & 0xFF, rm_lo & 0xFF) |

+ ARMul_SignedSaturatedAdd8(rn_lo >> 8, rm_lo >> 8) << 8;

+ hi_result = ARMul_SignedSaturatedAdd8(rn_hi & 0xFF, rm_hi & 0xFF) |

+ ARMul_SignedSaturatedAdd8(rn_hi >> 8, rm_hi >> 8) << 8;

+ }

+ // QSUB8

+ else if (op2 == 0x07) {

+ lo_result = ARMul_SignedSaturatedSub8(rn_lo & 0xFF, rm_lo & 0xFF) |

+ ARMul_SignedSaturatedSub8(rn_lo >> 8, rm_lo >> 8) << 8;

+ hi_result = ARMul_SignedSaturatedSub8(rn_hi & 0xFF, rm_hi & 0xFF) |

+ ARMul_SignedSaturatedSub8(rn_hi >> 8, rm_hi >> 8) << 8;

+ }

+

+ state->Reg[rd_idx] = (lo_result & 0xFFFF) | ((hi_result & 0xFFFF) << 16);

+ {

+ u32 rd = (instr >> 12) & 0xF;

+ u32 rn = (instr >> 16) & 0xF;

+ u32 rm = (instr >> 0) & 0xF;

+ u32 from = state->Reg[rn];

+ u32 to = state->Reg[rm];

+

+ if ((instr & 0xFF0) == 0xF10 || (instr & 0xFF0) == 0xF70) { // UADD16/USUB16

+ u32 h1, h2;

+ state->Cpsr &= 0xfff0ffff;

+ if ((instr & 0x0F0) == 0x070) { // USUB16

+ h1 = ((u16)from - (u16)to);

+ h2 = ((u16)(from >> 16) - (u16)(to >> 16));

+

+ if (!(h1 & 0xffff0000))

+ state->GEFlag |= (3 << 16);

+ else

+ state->GEFlag &= ~(3 << 16);

+ if (!(h2 & 0xffff0000))

+ state->GEFlag |= (3 << 18);

+ else

+ state->GEFlag &= ~(3 << 18);

+ }

+ else { // UADD16

+ h1 = ((u16)from + (u16)to);

+ h2 = ((u16)(from >> 16) + (u16)(to >> 16));

+

+ if (h1 & 0xffff0000)

+ state->GEFlag |= (3 << 16);

+ else

+ state->GEFlag &= ~(3 << 16);

+

+ if (h2 & 0xffff0000)

+ state->GEFlag |= (3 << 18);

+ else

+ state->GEFlag &= ~(3 << 18);

+ }

+

+ state->Reg[rd] = (u32)((h1 & 0xffff) | ((h2 & 0xffff) << 16));

+ else

+ if ((instr & 0xFF0) == 0xF90 || (instr & 0xFF0) == 0xFF0) { // UADD8/USUB8

+ u32 b1, b2, b3, b4;

+ state->Cpsr &= 0xfff0ffff;

+ if ((instr & 0x0F0) == 0x0F0) { // USUB8

+ b1 = ((u8)from - (u8)to);

+ b2 = ((u8)(from >> 8) - (u8)(to >> 8));

+ b3 = ((u8)(from >> 16) - (u8)(to >> 16));

+ b4 = ((u8)(from >> 24) - (u8)(to >> 24));

+

+ if (!(b1 & 0xffffff00))

+ state->GEFlag |= (1 << 16);

+ else

+ state->GEFlag &= ~(1 << 16);

+ if (!(b2 & 0xffffff00))

+ state->GEFlag |= (1 << 17);

+ else

+ state->GEFlag &= ~(1 << 17);

+ if (!(b3 & 0xffffff00))

+ state->GEFlag |= (1 << 18);

+ else

+ state->GEFlag &= ~(1 << 18);

+ if (!(b4 & 0xffffff00))

+ state->GEFlag |= (1 << 19);

+ else

+ state->GEFlag &= ~(1 << 19);

+ }

+ else { // UADD8

+ b1 = ((u8)from + (u8)to);

+ b2 = ((u8)(from >> 8) + (u8)(to >> 8));

+ b3 = ((u8)(from >> 16) + (u8)(to >> 16));

+ b4 = ((u8)(from >> 24) + (u8)(to >> 24));

+ if (b1 & 0xffffff00)

+ state->GEFlag |= (1 << 16);

+ else

+ state->GEFlag &= ~(1 << 16);

+ if (b2 & 0xffffff00)

+ state->GEFlag |= (1 << 17);

+ else

+ state->GEFlag &= ~(1 << 17);

+ if (b3 & 0xffffff00)

+ state->GEFlag |= (1 << 18);

+ else

+ state->GEFlag &= ~(1 << 18);

+ if (b4 & 0xffffff00)

+ state->GEFlag |= (1 << 19);

+ else

+ state->GEFlag &= ~(1 << 19);

+ }

+ state->Reg[rd] = (u32)(b1 | (b2 & 0xff) << 8 | (b3 & 0xff) << 16 | (b4 & 0xff) << 24);

+ return 1;

+ }

+ }

+ printf("Unhandled v6 insn: uasx/usax\n");

+ break;

+ case 0x66: // UQADD16, UQASX, UQSAX, UQSUB16, UQADD8, and UQSUB8

+ {

+ const u8 rd_idx = BITS(12, 15);

+ const u8 rm_idx = BITS(0, 3);

+ const u8 rn_idx = BITS(16, 19);

+ const u8 op2 = BITS(5, 7);

+ const u32 rm_val = state->Reg[rm_idx];

+ const u32 rn_val = state->Reg[rn_idx];

+

+ u16 lo_val = 0;

+ u16 hi_val = 0;

+

+ // UQADD16

+ if (op2 == 0x00) {

+ lo_val = ARMul_UnsignedSaturatedAdd16(rn_val & 0xFFFF, rm_val & 0xFFFF);

+ hi_val = ARMul_UnsignedSaturatedAdd16((rn_val >> 16) & 0xFFFF, (rm_val >> 16) & 0xFFFF);

+ }

+ // UQASX

+ else if (op2 == 0x01) {

+ lo_val = ARMul_UnsignedSaturatedSub16(rn_val & 0xFFFF, (rm_val >> 16) & 0xFFFF);

+ hi_val = ARMul_UnsignedSaturatedAdd16((rn_val >> 16) & 0xFFFF, rm_val & 0xFFFF);

+ }

+ // UQSAX

+ else if (op2 == 0x02) {

+ lo_val = ARMul_UnsignedSaturatedAdd16(rn_val & 0xFFFF, (rm_val >> 16) & 0xFFFF);

+ hi_val = ARMul_UnsignedSaturatedSub16((rn_val >> 16) & 0xFFFF, rm_val & 0xFFFF);

+ }

+ // UQSUB16

+ else if (op2 == 0x03) {

+ lo_val = ARMul_UnsignedSaturatedSub16(rn_val & 0xFFFF, rm_val & 0xFFFF);

+ hi_val = ARMul_UnsignedSaturatedSub16((rn_val >> 16) & 0xFFFF, (rm_val >> 16) & 0xFFFF);

+ }

+ // UQADD8

+ else if (op2 == 0x04) {

+ lo_val = ARMul_UnsignedSaturatedAdd8(rn_val, rm_val) |

+ ARMul_UnsignedSaturatedAdd8(rn_val >> 8, rm_val >> 8) << 8;

+ hi_val = ARMul_UnsignedSaturatedAdd8(rn_val >> 16, rm_val >> 16) |

+ ARMul_UnsignedSaturatedAdd8(rn_val >> 24, rm_val >> 24) << 8;

+ }

+ // UQSUB8

+ else {

+ lo_val = ARMul_UnsignedSaturatedSub8(rn_val, rm_val) |

+ ARMul_UnsignedSaturatedSub8(rn_val >> 8, rm_val >> 8) << 8;

+ hi_val = ARMul_UnsignedSaturatedSub8(rn_val >> 16, rm_val >> 16) |

+ ARMul_UnsignedSaturatedSub8(rn_val >> 24, rm_val >> 24) << 8;

+ }

+ state->Reg[rd_idx] = ((lo_val & 0xFFFF) | hi_val << 16);

+ case 0x67: // UHADD16, UHASX, UHSAX, UHSUB16, UHADD8, and UHSUB8.

+ {

+ const u8 op2 = BITS(5, 7);

+ const u8 rm_idx = BITS(0, 3);

+ const u8 rn_idx = BITS(16, 19);

+ const u8 rd_idx = BITS(12, 15);

+ const u32 rm_val = state->Reg[rm_idx];

+ const u32 rn_val = state->Reg[rn_idx];

+ if (op2 == 0x00 || op2 == 0x01 || op2 == 0x02 || op2 == 0x03)

+ {

+ u32 lo_val = 0;

+ u32 hi_val = 0;

+ // UHADD16

+ if (op2 == 0x00) {

+ lo_val = (rn_val & 0xFFFF) + (rm_val & 0xFFFF);

+ hi_val = ((rn_val >> 16) & 0xFFFF) + ((rm_val >> 16) & 0xFFFF);

+ }

+ // UHASX

+ else if (op2 == 0x01) {

+ lo_val = (rn_val & 0xFFFF) - ((rm_val >> 16) & 0xFFFF);

+ hi_val = ((rn_val >> 16) & 0xFFFF) + (rm_val & 0xFFFF);

+ }

+ // UHSAX

+ else if (op2 == 0x02) {

+ lo_val = (rn_val & 0xFFFF) + ((rm_val >> 16) & 0xFFFF);

+ hi_val = ((rn_val >> 16) & 0xFFFF) - (rm_val & 0xFFFF);

+ }

+ // UHSUB16

+ else if (op2 == 0x03) {

+ lo_val = (rn_val & 0xFFFF) - (rm_val & 0xFFFF);

+ hi_val = ((rn_val >> 16) & 0xFFFF) - ((rm_val >> 16) & 0xFFFF);

+ }

+ lo_val >>= 1;

+ hi_val >>= 1;

+ state->Reg[rd_idx] = (lo_val & 0xFFFF) | ((hi_val & 0xFFFF) << 16);

+ return 1;

+ }

+ else if (op2 == 0x04 || op2 == 0x07) {

+ u32 sum1;

+ u32 sum2;

+ u32 sum3;

+ u32 sum4;

+

+ // UHADD8

+ if (op2 == 0x04) {

+ sum1 = (rn_val & 0xFF) + (rm_val & 0xFF);

+ sum2 = ((rn_val >> 8) & 0xFF) + ((rm_val >> 8) & 0xFF);

+ sum3 = ((rn_val >> 16) & 0xFF) + ((rm_val >> 16) & 0xFF);

+ sum4 = ((rn_val >> 24) & 0xFF) + ((rm_val >> 24) & 0xFF);

+ }

+ // UHSUB8

+ else {

+ sum1 = (rn_val & 0xFF) - (rm_val & 0xFF);

+ sum2 = ((rn_val >> 8) & 0xFF) - ((rm_val >> 8) & 0xFF);

+ sum3 = ((rn_val >> 16) & 0xFF) - ((rm_val >> 16) & 0xFF);

+ sum4 = ((rn_val >> 24) & 0xFF) - ((rm_val >> 24) & 0xFF);

+ }

+

+ sum1 >>= 1;

+ sum2 >>= 1;

+ sum3 >>= 1;

+ sum4 >>= 1;

+

+ state->Reg[rd_idx] = (sum1 & 0xFF) | ((sum2 & 0xFF) << 8) | ((sum3 & 0xFF) << 16) | ((sum4 & 0xFF) << 24);

+ return 1;

+ case 0x68:

+ {

+ u32 rd = (instr >> 12) & 0xF;

+ u32 rn = (instr >> 16) & 0xF;

+ u32 rm = (instr >> 0) & 0xF;

+ u32 from = state->Reg[rn];

+ u32 to = state->Reg[rm];

+ u32 cpsr = ARMul_GetCPSR(state);

+ if ((instr & 0xFF0) == 0xFB0) { // SEL

+ u32 result;

+ if (cpsr & (1 << 16))

+ result = from & 0xff;

+ else

+ result = to & 0xff;

+ if (cpsr & (1 << 17))

+ result |= from & 0x0000ff00;

+ else

+ result |= to & 0x0000ff00;

+ if (cpsr & (1 << 18))

+ result |= from & 0x00ff0000;

+ else

+ result |= to & 0x00ff0000;

+ if (cpsr & (1 << 19))

+ result |= from & 0xff000000;

+ else

+ result |= to & 0xff000000;

+ state->Reg[rd] = result;

+ return 1;

+ }

+ printf("Unhandled v6 insn: pkh/sxtab/selsxtb\n");

+ break;

+

+ case 0x6a: // SSAT, SSAT16, SXTB, and SXTAB

+ {

+ const u8 op2 = BITS(5, 7);

+

+ // SSAT16

+ if (op2 == 0x01) {

+ const u8 rd_idx = BITS(12, 15);

+ const u8 rn_idx = BITS(0, 3);

+ const u8 num_bits = BITS(16, 19) + 1;

+ const s16 min = -(0x8000 >> (16 - num_bits));

+ const s16 max = (0x7FFF >> (16 - num_bits));

+ s16 rn_lo = (state->Reg[rn_idx]);

+ s16 rn_hi = (state->Reg[rn_idx] >> 16);

+

+ if (rn_lo > max) {

+ rn_lo = max;

+ SETQ;

+ } else if (rn_lo < min) {

+ rn_lo = min;

+ SETQ;

+ }

+

+ if (rn_hi > max) {

+ rn_hi = max;

+ SETQ;

+ } else if (rn_hi < min) {

+ rn_hi = min;

+ SETQ;

+ }

+

+ state->Reg[rd_idx] = (rn_lo & 0xFFFF) | ((rn_hi & 0xFFFF) << 16);

+ return 1;

+ }

+ else if (op2 == 0x03) {

+ const u8 rotation = BITS(10, 11) * 8;

+ u32 rm = ((state->Reg[BITS(0, 3)] >> rotation) & 0xFF) | (((state->Reg[BITS(0, 3)] << (32 - rotation)) & 0xFF) & 0xFF);

+ if (rm & 0x80)

+ rm |= 0xffffff00;

+

+ // SXTB, otherwise SXTAB

+ if (BITS(16, 19) == 0xf)

+ state->Reg[BITS(12, 15)] = rm;

+ else

+ state->Reg[BITS(12, 15)] = state->Reg[BITS(16, 19)] + rm;

+

+ return 1;

+ }

+ else {

+ printf("Unimplemented op: SSAT");

+ }

+ }

+ break;

+

+ case 0x6b: // REV, REV16, SXTH, and SXTAH

+ {

+ const u8 op2 = BITS(5, 7);

+

+ // REV

+ if (op2 == 0x01) {

+ DEST = ((RHS & 0xFF) << 24) | ((RHS & 0xFF00)) << 8 | ((RHS & 0xFF0000) >> 8) | ((RHS & 0xFF000000) >> 24);

+ return 1;

+ }

+ // REV16

+ else if (op2 == 0x05) {

+ DEST = ((RHS & 0xFF) << 8) | ((RHS & 0xFF00)) >> 8 | ((RHS & 0xFF0000) << 8) | ((RHS & 0xFF000000) >> 8);

+ return 1;

+ }

+ else if (op2 == 0x03) {

+ const u8 rotate = BITS(10, 11) * 8;

+

+ u32 rm = ((state->Reg[BITS(0, 3)] >> rotate) & 0xFFFF) | (((state->Reg[BITS(0, 3)] << (32 - rotate)) & 0xFFFF) & 0xFFFF);

+ if (rm & 0x8000)

+ rm |= 0xffff0000;

+

+ // SXTH, otherwise SXTAH

+ if (BITS(16, 19) == 15)

+ state->Reg[BITS(12, 15)] = rm;

+ else

+ state->Reg[BITS(12, 15)] = state->Reg[BITS(16, 19)] + rm;

+

+ return 1;

+ }

+ }

+ break;

+

+ case 0x6c: // UXTB16 and UXTAB16

+ {

+ const u8 rm_idx = BITS(0, 3);

+ const u8 rn_idx = BITS(16, 19);

+ const u8 rd_idx = BITS(12, 15);

+ const u32 rm_val = state->Reg[rm_idx];

+ const u32 rn_val = state->Reg[rn_idx];

+ const u32 rotation = BITS(10, 11) * 8;

+ const u32 rotated_rm = ((rm_val << (32 - rotation)) | (rm_val >> rotation));

+

+ // UXTB16

+ if ((instr & 0xf03f0) == 0xf0070) {

+ state->Reg[rd_idx] = rotated_rm & 0x00FF00FF;

+ }

+ else { // UXTAB16

+ const u8 lo_rotated = (rotated_rm & 0xFF);

+ const u16 lo_result = (rn_val & 0xFFFF) + (u16)lo_rotated;

+

+ const u8 hi_rotated = (rotated_rm >> 16) & 0xFF;

+ const u16 hi_result = (rn_val >> 16) + (u16)hi_rotated;

+

+ state->Reg[rd_idx] = ((hi_result << 16) | (lo_result & 0xFFFF));

+ }

+

+ return 1;

+ }

+ break;

+ case 0x6e: // USAT, USAT16, UXTB, and UXTAB

+ {

+ const u8 op2 = BITS(5, 7);

+

+ // USAT16

+ if (op2 == 0x01) {

+ const u8 rd_idx = BITS(12, 15);

+ const u8 rn_idx = BITS(0, 3);

+ const u8 num_bits = BITS(16, 19);

+ const s16 max = 0xFFFF >> (16 - num_bits);

+ s16 rn_lo = (state->Reg[rn_idx]);

+ s16 rn_hi = (state->Reg[rn_idx] >> 16);

+

+ if (max < rn_lo) {

+ rn_lo = max;

+ SETQ;

+ } else if (rn_lo < 0) {

+ rn_lo = 0;

+ SETQ;

+ }

+

+ if (max < rn_hi) {

+ rn_hi = max;

+ SETQ;

+ } else if (rn_hi < 0) {

+ rn_hi = 0;

+ SETQ;

+ }

+

+ state->Reg[rd_idx] = (rn_lo & 0xFFFF) | ((rn_hi << 16) & 0xFFFF);

+ return 1;

+ }

+ else if (op2 == 0x03) {

+ const u8 rotate = BITS(10, 11) * 8;

+ const u32 rm = ((state->Reg[BITS(0, 3)] >> rotate) & 0xFF) | (((state->Reg[BITS(0, 3)] << (32 - rotate)) & 0xFF) & 0xFF);

+

+ if (BITS(16, 19) == 0xf)

+ /* UXTB */

+ state->Reg[BITS(12, 15)] = rm;

+ else

+ /* UXTAB */

+ state->Reg[BITS(12, 15)] = state->Reg[BITS(16, 19)] + rm;

+

+ return 1;

+ }

+ else {

+ printf("Unimplemented op: USAT");

+ }

+ }

+ break;

+

+ case 0x6f: // UXTH, UXTAH, and REVSH.

+ {

+ const u8 op2 = BITS(5, 7);

+

+ // REVSH

+ if (op2 == 0x05) {

+ DEST = ((RHS & 0xFF) << 8) | ((RHS & 0xFF00) >> 8);

+ if (DEST & 0x8000)

+ DEST |= 0xffff0000;

+ return 1;

+ }

+ // UXTH and UXTAH

+ else if (op2 == 0x03) {

+ const u8 rotate = BITS(10, 11) * 8;

+ const ARMword rm = ((state->Reg[BITS(0, 3)] >> rotate) & 0xFFFF) | (((state->Reg[BITS(0, 3)] << (32 - rotate)) & 0xFFFF) & 0xFFFF);

+

+ // UXTH

+ if (BITS(16, 19) == 0xf) {

+ state->Reg[BITS(12, 15)] = rm;

+ }

+ // UXTAH

+ else {

+ state->Reg[BITS(12, 15)] = state->Reg[BITS(16, 19)] + rm;

+ }

+

+ return 1;

+ }

+ }

+ case 0x70:

+ // ichfly

+ // SMUAD, SMUSD, SMLAD, and SMLSD

+ if ((instr & 0xf0d0) == 0xf010 || (instr & 0xf0d0) == 0xf050 ||

+ (instr & 0xd0) == 0x10 || (instr & 0xd0) == 0x50)

+ {

+ const u8 rd_idx = BITS(16, 19);

+ const u8 rn_idx = BITS(0, 3);

+ const u8 rm_idx = BITS(8, 11);

+ const u8 ra_idx = BITS(12, 15);

+ const bool do_swap = (BIT(5) == 1);

+ u32 rm_val = state->Reg[rm_idx];

+ const u32 rn_val = state->Reg[rn_idx];

+ if (do_swap)

+ rm_val = (((rm_val & 0xFFFF) << 16) | (rm_val >> 16));

+ const s16 rm_lo = (rm_val & 0xFFFF);

+ const s16 rm_hi = ((rm_val >> 16) & 0xFFFF);

+ const s16 rn_lo = (rn_val & 0xFFFF);

+ const s16 rn_hi = ((rn_val >> 16) & 0xFFFF);

+ const u32 product1 = (rn_lo * rm_lo);

+ const u32 product2 = (rn_hi * rm_hi);

+ // SMUAD and SMLAD

+ if (BIT(6) == 0) {

+ state->Reg[rd_idx] = product1 + product2;

+ if (BITS(12, 15) != 15) {

+ state->Reg[rd_idx] += state->Reg[ra_idx];

+ ARMul_AddOverflowQ(state, product1 + product2, state->Reg[ra_idx]);

+ }

+ ARMul_AddOverflowQ(state, product1, product2);

+ }

+ // SMUSD and SMLSD

+ else {

+ state->Reg[rd_idx] = product1 - product2;

+

+ if (BITS(12, 15) != 15)

+ state->Reg[rd_idx] += state->Reg[ra_idx];

+ }

+ break;

+ case 0x74: // SMLALD and SMLSLD

+ {

+ const u8 rm_idx = BITS(8, 11);

+ const u8 rn_idx = BITS(0, 3);

+ const u8 rdlo_idx = BITS(12, 15);

+ const u8 rdhi_idx = BITS(16, 19);

+ const bool do_swap = (BIT(5) == 1);

+

+ const u32 rdlo_val = state->Reg[rdlo_idx];

+ const u32 rdhi_val = state->Reg[rdhi_idx];

+ const u32 rn_val = state->Reg[rn_idx];

+ u32 rm_val = state->Reg[rm_idx];

+

+ if (do_swap)

+ rm_val = (((rm_val & 0xFFFF) << 16) | (rm_val >> 16));

+

+ const s32 product1 = (s16)(rn_val & 0xFFFF) * (s16)(rm_val & 0xFFFF);

+ const s32 product2 = (s16)((rn_val >> 16) & 0xFFFF) * (s16)((rm_val >> 16) & 0xFFFF);

+ s64 result;

+

+ // SMLALD

+ if (BIT(6) == 0) {

+ result = (product1 + product2) + (s64)(rdlo_val | ((s64)rdhi_val << 32));

+ }

+ // SMLSLD

+ else {

+ result = (product1 - product2) + (s64)(rdlo_val | ((s64)rdhi_val << 32));

+ state->Reg[rdlo_idx] = (result & 0xFFFFFFFF);

+ state->Reg[rdhi_idx] = ((result >> 32) & 0xFFFFFFFF);

+ break;

+ case 0x75: // SMMLA, SMMUL, and SMMLS

+ {

+ const u8 rm_idx = BITS(8, 11);

+ const u8 rn_idx = BITS(0, 3);

+ const u8 ra_idx = BITS(12, 15);

+ const u8 rd_idx = BITS(16, 19);

+ const bool do_round = (BIT(5) == 1);

+ const u32 rm_val = state->Reg[rm_idx];

+ const u32 rn_val = state->Reg[rn_idx];

+ // Assume SMMUL by default.

+ s64 result = (s64)(s32)rn_val * (s64)(s32)rm_val;

+ if (ra_idx != 15) {

+ const u32 ra_val = state->Reg[ra_idx];

+ // SMMLA, otherwise SMMLS

+ if (BIT(6) == 0)

+ result += ((s64)ra_val << 32);

+ else

+ result = ((s64)ra_val << 32) - result;

+ }

+ if (do_round)

+ result += 0x80000000;

+ state->Reg[rd_idx] = ((result >> 32) & 0xFFFFFFFF);

+ break;

+ case 0x78:

+ if (BITS(20, 24) == 0x18)

+ {

+ const u8 rm_idx = BITS(8, 11);

+ const u8 rn_idx = BITS(0, 3);

+ const u8 rd_idx = BITS(16, 19);

+ const u32 rm_val = state->Reg[rm_idx];

+ const u32 rn_val = state->Reg[rn_idx];

+ const u8 diff1 = ARMul_UnsignedAbsoluteDifference(rn_val & 0xFF, rm_val & 0xFF);

+ const u8 diff2 = ARMul_UnsignedAbsoluteDifference((rn_val >> 8) & 0xFF, (rm_val >> 8) & 0xFF);

+ const u8 diff3 = ARMul_UnsignedAbsoluteDifference((rn_val >> 16) & 0xFF, (rm_val >> 16) & 0xFF);

+ const u8 diff4 = ARMul_UnsignedAbsoluteDifference((rn_val >> 24) & 0xFF, (rm_val >> 24) & 0xFF);

+ u32 finalDif = (diff1 + diff2 + diff3 + diff4);

+ // Op is USADA8 if true.

+ const u8 ra_idx = BITS(12, 15);

+ if (ra_idx != 15)

+ finalDif += state->Reg[ra_idx];

+ state->Reg[rd_idx] = finalDif;

+ return 1;

+ break;

+ case 0x7a:

+ printf ("Unhandled v6 insn: usbfx\n");

+ break;

+ case 0x7c:

+ printf ("Unhandled v6 insn: bfc/bfi\n");

+ break;

+ } \ No newline at end of file