1 files changed, 60 insertions, 48 deletions

diff --git a/src/core/arm/interpreter/armemu.cpp b/src/core/arm/interpreter/armemu.cpp
index e46b4d15b..45714761a 100644
--- a/src/core/arm/interpreter/armemu.cpp
+++ b/src/core/arm/interpreter/armemu.cpp
@@ -6105,17 +6105,32 @@ L_stm_s_takeabort:
 
 			return 1;
 		}
-		case 0x6c:
-			if ((instr & 0xf03f0) == 0xf0070) { //uxtb16
-				u8 rm_idx = BITS(0, 3);
-				u8 rd_idx = BITS(12, 15);
-				u32 rm_val = state->Reg[rm_idx];
-				u32 rotation = BITS(10, 11) * 8;
-				u32 in = ((rm_val << (32 - rotation)) | (rm_val >> rotation));
-				state->Reg[rd_idx] = in & 0x00FF00FF;
+		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;
-			} else
-				printf ("Unhandled v6 insn: uxtab16\n");
+			}
 			break;
 		case 0x6e: {
 			ARMword Rm;
@@ -6226,45 +6241,42 @@ L_stm_s_takeabort:
 			return 1;
 		}
         case 0x70:
-            if ((instr & 0xf0d0) == 0xf010) { //smuad //ichfly
-                u8 tar = BITS(16, 19);
-                u8 src1 = BITS(0, 3);
-                u8 src2 = BITS(8, 11);
-                u8 swap = BIT(5);
-                s16 a1 = (state->Reg[src1] & 0xFFFF);
-                s16 a2 = ((state->Reg[src1] >> 0x10) & 0xFFFF);
-                s16 b1 = swap ? ((state->Reg[src2] >> 0x10) & 0xFFFF) : (state->Reg[src2] & 0xFFFF);
-                s16 b2 = swap ? (state->Reg[src2] & 0xFFFF) : ((state->Reg[src2] >> 0x10) & 0xFFFF);
-                state->Reg[tar] = a1*a2 + b1*b2;
-                return 1;
-
-            } else if ((instr & 0xf0d0) == 0xf050) { //smusd
-                u8 tar = BITS(16, 19);
-                u8 src1 = BITS(0, 3);
-                u8 src2 = BITS(8, 11);
-                u8 swap = BIT(5);
-                s16 a1 = (state->Reg[src1] & 0xFFFF);
-                s16 a2 = ((state->Reg[src1] >> 0x10) & 0xFFFF);
-                s16 b1 = swap ? ((state->Reg[src2] >> 0x10) & 0xFFFF) : (state->Reg[src2] & 0xFFFF);
-                s16 b2 = swap ? (state->Reg[src2] & 0xFFFF) : ((state->Reg[src2] >> 0x10) & 0xFFFF);
-                state->Reg[tar] = a1*a2 - b1*b2;
-                return 1;
-            } else if ((instr & 0xd0) == 0x10) { //smlad
-                u8 tar = BITS(16, 19);
-                u8 src1 = BITS(0, 3);
-                u8 src2 = BITS(8, 11);
-                u8 src3 = BITS(12, 15);
-                u8 swap = BIT(5);
-
-                u32 a3 = state->Reg[src3];
-
-                s16 a1 = (state->Reg[src1] & 0xFFFF);
-                s16 a2 = ((state->Reg[src1] >> 0x10) & 0xFFFF);
-                s16 b1 = swap ? ((state->Reg[src2] >> 0x10) & 0xFFFF) : (state->Reg[src2] & 0xFFFF);
-                s16 b2 = swap ? (state->Reg[src2] & 0xFFFF) : ((state->Reg[src2] >> 0x10) & 0xFFFF);
-                state->Reg[tar] = a1*a2 + b1*b2 + a3;
+            // ichfly
+            // SMUAD, SMUSD, SMLAD
+            if ((instr & 0xf0d0) == 0xf010 || (instr & 0xf0d0) == 0xf050 || (instr & 0xd0) == 0x10) {
+                const u8 rd_idx = BITS(16, 19);
+                const u8 rn_idx = BITS(0, 3);
+                const u8 rm_idx = BITS(8, 11);
+                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);
+
+                // SMUAD
+                if ((instr & 0xf0d0) == 0xf010) {
+                    state->Reg[rd_idx] = (rn_lo * rm_lo) + (rn_hi * rm_hi);
+                }
+                // SMUSD
+                else if ((instr & 0xf0d0) == 0xf050) {
+                    state->Reg[rd_idx] = (rn_lo * rm_lo) - (rn_hi * rm_hi);
+                }
+                // SMLAD
+                else {
+                    const u8 ra_idx = BITS(12, 15);
+                    state->Reg[rd_idx] = (rn_lo * rm_lo) + (rn_hi * rm_hi) + (s32)state->Reg[ra_idx];
+                }
                 return 1;
-            } else printf ("Unhandled v6 insn: smuad/smusd/smlad/smlsd\n");
+            } else {
+                printf ("Unhandled v6 insn: smlsd\n");
+            }
             break;
         case 0x74:
             printf ("Unhandled v6 insn: smlald/smlsld\n");