[GSP::GPU] Implement writeHwRegsWithMask

include/services/gsp_gpu.hpp

@@ -16,6 +16,7 @@ class GPUService {
 	void acquireRight(u32 messagePointer);
 	void registerInterruptRelayQueue(u32 messagePointer);
 	void writeHwRegs(u32 messagePointer);
+	void writeHwRegsWithMask(u32 messagePointer);
 
 public:
 	GPUService(Memory& mem, u32& currentPID) : mem(mem), currentPID(currentPID) {}

src/core/services/gsp_gpu.cpp

@@ -4,7 +4,8 @@ namespace GPUCommands {
 	enum : u32 {
 		AcquireRight = 0x00160042,
 		RegisterInterruptRelayQueue = 0x00130042,
-		WriteHwRegs = 0x00010082
+		WriteHwRegs = 0x00010082,
+		WriteHwRegsWithMask = 0x00020084
 	};
 }
 
@@ -25,6 +26,7 @@ void GPUService::handleSyncRequest(u32 messagePointer) {
 		case GPUCommands::AcquireRight: acquireRight(messagePointer); break;
 		case GPUCommands::RegisterInterruptRelayQueue: registerInterruptRelayQueue(messagePointer); break;
 		case GPUCommands::WriteHwRegs: writeHwRegs(messagePointer); break;
+		case GPUCommands::WriteHwRegsWithMask: writeHwRegsWithMask(messagePointer); break;
 ;		default: Helpers::panic("GPU service requested. Command: %08X\n", command);
 	}
 }
@@ -68,7 +70,7 @@ void GPUService::writeHwRegs(u32 messagePointer) {
 	printf("GSP::GPU::writeHwRegs (GPU address = %08X, size = %X, data address = %08X)\n", ioAddr, size, dataPointer);
 
 	// Check for alignment
-	if ((size & 3) || (ioAddr & 3)) {
+	if ((size & 3) || (ioAddr & 3) || (dataPointer & 3)) {
 		Helpers::panic("GSP::GPU::writeHwRegs misalignment");
 	}
 
@@ -85,6 +87,47 @@ void GPUService::writeHwRegs(u32 messagePointer) {
 		printf("GSP::GPU: Wrote %08X to GPU register %X\n", value, ioAddr);
 		dataPointer += 4;
 		ioAddr += 4;
+		// TODO: Write the value to the register
 	}
+	mem.write32(messagePointer + 4, Result::Success);
+}
+
+// Update sequential GPU registers using an array of data and mask values using this formula
+// GPU register = (register & ~mask) | (data & mask).
+void GPUService::writeHwRegsWithMask(u32 messagePointer) {
+	u32 ioAddr = mem.read32(messagePointer + 4); // GPU address based at 0x1EB00000, word aligned
+	const u32 size = mem.read32(messagePointer + 8); // Size in bytes
+
+	u32 dataPointer = mem.read32(messagePointer + 16); // Data pointer
+	u32 maskPointer = mem.read32(messagePointer + 24); // Mask pointer
+
+	printf("GSP::GPU::writeHwRegsWithMask (GPU address = %08X, size = %X, data address = %08X, mask address = %08X)\n", 
+		ioAddr, size, dataPointer, maskPointer);
+
+	// Check for alignment
+	if ((size & 3) || (ioAddr & 3) || (dataPointer & 3) || (maskPointer & 3)) {
+		Helpers::panic("GSP::GPU::writeHwRegs misalignment");
+	}
+
+	if (size > 0x80) {
+		Helpers::panic("GSP::GPU::writeHwRegs size too big");
+	}
+
+	if (ioAddr >= 0x420000) {
+		Helpers::panic("GSP::GPU::writeHwRegs offset too big");
+	}
+
+	for (u32 i = 0; i < size; i += 4) {
+		const u32 currentValue = 0; // TODO: Read the actual register value
+		const u32 data = mem.read32(dataPointer);
+		const u32 mask = mem.read32(maskPointer);
+
+		u32 newValue = (currentValue & ~mask) | (data & mask);
+		// TODO: write new value
+		maskPointer += 4;
+		dataPointer += 4;
+		ioAddr += 4;
+	}
+
 	mem.write32(messagePointer + 4, Result::Success);
 }