From c94912887e0c403dc2116162e3f43e32f9412e59 Mon Sep 17 00:00:00 2001
From: paoda <rekai@musuka.dev>
Date: Thu, 7 Sep 2023 19:56:54 -0500
Subject: [PATCH] feat(v5te): implement SMLA<x><y>

---
 src/arm/cpu/arm/psr_transfer.zig | 29 +++++++++++++++++++++++------
 src/arm/v4t.zig                  |  1 -
 2 files changed, 23 insertions(+), 7 deletions(-)

diff --git a/src/arm/cpu/arm/psr_transfer.zig b/src/arm/cpu/arm/psr_transfer.zig
index 66bc95e..75ce507 100644
--- a/src/arm/cpu/arm/psr_transfer.zig
+++ b/src/arm/cpu/arm/psr_transfer.zig
@@ -1,6 +1,7 @@
 const std = @import("std");
-const rotr = @import("zba-util").rotr;
+
 const PSR = @import("../../../arm.zig").PSR;
+const rotr = @import("zba-util").rotr;
 
 const log = std.log.scoped(.ctrl_ext_space);
 
@@ -12,9 +13,7 @@ pub fn control(comptime InstrFn: type, comptime I: bool, comptime op: u6) InstrF
             if (I) {
                 // MSR (register)
                 const R = op >> 5 & 1 == 1;
-                msr(R, I, cpu, opcode);
-
-                return;
+                return msr(R, I, cpu, opcode);
             }
 
             switch (op) {
@@ -41,7 +40,7 @@ pub fn control(comptime InstrFn: type, comptime I: bool, comptime op: u6) InstrF
                 },
                 0b01_0011 => cpu.panic("TODO: implement v5TE BLX", .{}),
                 0b00_0101, 0b01_0101 => {
-                    const U = op >> 4 & 1 == 1; // A for Add? is there a convention?
+                    const U = op >> 4 & 1 == 1;
 
                     const rm = opcode & 0xF;
                     const rd = opcode >> 12 & 0xF;
@@ -57,8 +56,26 @@ pub fn control(comptime InstrFn: type, comptime I: bool, comptime op: u6) InstrF
                 0b10_0101 => cpu.panic("TODO: implement QDADD", .{}),
                 0b11_0101 => cpu.panic("TODO: implement QDSUB", .{}),
                 0b01_0111 => cpu.panic("TODO: handle BKPT", .{}),
+                0b00_1000, 0b00_1010, 0b00_1100, 0b00_1110 => { // SMLA<x><y>
+                    const X = op >> 1 & 1;
+                    const Y = op >> 2 & 1;
 
-                else => std.log.debug("unhandled instruction: 0x{X:0>8}", .{opcode}),
+                    const rm = opcode & 0xF;
+                    const rs = opcode >> 8 & 0xF;
+                    const rn = opcode >> 12 & 0xF;
+                    const rd = opcode >> 16 & 0xF;
+
+                    const left: i16 = @intCast((cpu.r[rm] >> 16 * X) & 0xFFFF);
+                    const right: i16 = @intCast((cpu.r[rs] >> 16 * Y) & 0xFFFF);
+                    const accumulate: i32 = @bitCast(cpu.r[rn]);
+
+                    const result = @addWithOverflow(@as(i32, left) * @as(i32, right), accumulate);
+                    cpu.r[rd] = @bitCast(result[0]);
+
+                    if (result[1] == 0b1) cpu.cpsr.q.set();
+                },
+
+                else => cpu.panic("unhandled instruction: 0x{X:0>8}", .{opcode}),
             }
         }
 
diff --git a/src/arm/v4t.zig b/src/arm/v4t.zig
index acce81e..b169730 100644
--- a/src/arm/v4t.zig
+++ b/src/arm/v4t.zig
@@ -59,7 +59,6 @@ pub const arm = struct {
                     } else if (i & 0xD90 == 0x100) blk: {
                         const I = i >> 9 & 1 == 1;
                         const op = ((i >> 5) & 0x3) << 4 | (i & 0xF);
-
                         break :blk control(InstrFn, I, op);
                     } else blk: {
                         const I = i >> 9 & 1 == 1;