From 3786a5fd3b16f5123fbab273519bb938f26dafd5 Mon Sep 17 00:00:00 2001
From: CocoSimone <simococo03@gmail.com>
Date: Fri, 10 Feb 2023 02:13:45 +0100
Subject: [PATCH] new vector loads/stores

---
 src/backend/core/RSP.hpp              |  3 ++
 src/backend/core/rsp/decode.cpp       |  4 +-
 src/backend/core/rsp/instructions.cpp | 68 ++++++++++++++++++++++-----
 3 files changed, 63 insertions(+), 12 deletions(-)

diff --git a/src/backend/core/RSP.hpp b/src/backend/core/RSP.hpp
index c3afa2e1..cd4bcb6a 100644
--- a/src/backend/core/RSP.hpp
+++ b/src/backend/core/RSP.hpp
@@ -258,6 +258,7 @@ struct RSP {
   void llv(u32 instr);
   void lrv(u32 instr);
   void lqv(u32 instr);
+  void lhv(u32 instr);
   void ltv(u32 instr);
   void lpv(u32 instr);
   void j(u32 instr);
@@ -281,7 +282,9 @@ struct RSP {
   void ssv(u32 instr);
   void suv(u32 instr);
   void slv(u32 instr);
+  void shv(u32 instr);
   void sfv(u32 instr);
+  void srv(u32 instr);
   void spv(u32 instr);
   void sllv(u32 instr);
   void srlv(u32 instr);
diff --git a/src/backend/core/rsp/decode.cpp b/src/backend/core/rsp/decode.cpp
index 6fd1bb32..77432313 100644
--- a/src/backend/core/rsp/decode.cpp
+++ b/src/backend/core/rsp/decode.cpp
@@ -69,7 +69,7 @@ inline void lwc2(RSP& rsp, u32 instr) {
     case 0x05: rsp.lrv(instr); break;
     case 0x06: rsp.lpv(instr); break;
     case 0x07: rsp.luv(instr); break;
-    case 0x0A: break;
+    case 0x08: rsp.lhv(instr); break;
     case 0x0B: rsp.ltv(instr); break;
     default: Util::panic("Unhandled RSP LWC2 {:05b}\n", mask);
   }
@@ -84,8 +84,10 @@ inline void swc2(RSP& rsp, u32 instr) {
     case 0x02: rsp.slv(instr); break;
     case 0x03: rsp.sdv(instr); break;
     case 0x04: rsp.sqv(instr); break;
+    case 0x05: rsp.srv(instr); break;
     case 0x06: rsp.spv(instr); break;
     case 0x07: rsp.suv(instr); break;
+    case 0x08: rsp.shv(instr); break;
     case 0x09: rsp.sfv(instr); break;
     case 0x0A: rsp.swv(instr); break;
     case 0x0B: rsp.stv(instr); break;
diff --git a/src/backend/core/rsp/instructions.cpp b/src/backend/core/rsp/instructions.cpp
index ce2eea23..d815db5f 100644
--- a/src/backend/core/rsp/instructions.cpp
+++ b/src/backend/core/rsp/instructions.cpp
@@ -219,17 +219,6 @@ void RSP::lqv(u32 instr) {
   }
 }
 
-void RSP::lrv(u32 instr) {
-  int e = E1(instr);
-  u32 addr = gpr[BASE(instr)] + SignExt7bit(OFFSET(instr), 4);
-  int start = 16 - ((addr & 0xf) - e);
-  addr &= 0xFFFFFFF0;
-
-  for(int i = start; i < 16; i++) {
-    vpr[VT(instr)].byte[BYTE_INDEX(i & 0xf)] = ReadByte(addr++);
-  }
-}
-
 void RSP::lpv(u32 instr) {
   int e = E1(instr);
   u32 addr = gpr[BASE(instr)] + SignExt7bit(OFFSET(instr), 3);
@@ -420,6 +409,63 @@ void RSP::spv(u32 instr) {
   }
 }
 
+void RSP::srv(u32 instr) {
+  u32 address = gpr[BASE(instr)] + SignExt7bit(OFFSET(instr), 4);
+  int start = E1(instr);
+  int end = start + (address & 15);
+  int base = 16 - (address & 15);
+  address &= ~15;
+  for(int i = start; i < end; i++) {
+    WriteByte(address++, vpr[VT(instr)].byte[BYTE_INDEX((i + base) & 0xF)]);
+  }
+}
+
+void RSP::shv(u32 instr) {
+  u32 address = gpr[BASE(instr)] + SignExt7bit(OFFSET(instr), 4);
+
+  u32 in_addr_offset = address & 0x7;
+  address &= ~0x7;
+
+  int e = E1(instr);
+
+  for (int i = 0; i < 8; i++) {
+    int byte_index = (i * 2) + e;
+    u16 val = vpr[VT(instr)].byte[BYTE_INDEX(byte_index & 15)] << 1;
+    val |= vpr[VT(instr)].byte[BYTE_INDEX((byte_index + 1) & 15)] >> 7;
+    u8 b = val & 0xFF;
+
+    int ofs = in_addr_offset + (i * 2);
+    WriteByte(address + (ofs & 0xF), b);
+  }
+}
+
+void RSP::lhv(u32 instr) {
+  u32 address = gpr[BASE(instr)] + SignExt7bit(OFFSET(instr), 4);
+
+  u32 in_addr_offset = address & 0x7;
+  address &= ~0x7;
+
+  int e = E1(instr);
+
+  for (int i = 0; i < 8; i++) {
+    int ofs = ((16 - e) + (i * 2) + in_addr_offset) & 0xF;
+    u16 val = ReadByte(address + ofs);
+    val <<= 7;
+    vpr[VT(instr)].element[ELEMENT_INDEX(i)] = val;
+  }
+}
+
+void RSP::lrv(u32 instr) {
+  u32 address = gpr[BASE(instr)] + SignExt7bit(OFFSET(instr), 4);
+  int e = E1(instr);
+  int start = 16 - ((address & 15) - e);
+  address &= ~15;
+
+  for(int i = start; i < 16; i++) {
+    vpr[VT(instr)].byte[BYTE_INDEX(i & 0xF)] = ReadByte(address++);
+  }
+}
+
 void RSP::sfv(u32 instr) {
   VPR& vt = vpr[VT(instr)];
   u32 address = gpr[BASE(instr)] + SignExt7bit(OFFSET(instr), 4);