M src/gba/gba-bios.c → src/gba/gba-bios.c

@@ -1,9 +1,57 @@
 #include "gba-bios.h"
 
 #include "gba.h"
+#include "gba-memory.h"
+
+static void _CpuSet(struct GBA* gba) {
+	uint32_t source = gba->cpu.gprs[0];
+	uint32_t dest = gba->cpu.gprs[1];
+	uint32_t mode = gba->cpu.gprs[2];
+	int count = mode & 0x000FFFFF;
+	int fill = mode & 0x01000000;
+	int wordsize = (mode & 0x04000000) ? 4 : 2;
+	int i;
+	if (fill) {
+		if (wordsize == 4) {
+			source &= 0xFFFFFFFC;
+			dest &= 0xFFFFFFFC;
+			int32_t word = GBALoad32(&gba->memory.d, source);
+			for (i = 0; i < count; ++i) {
+				GBAStore32(&gba->memory.d, dest + (i << 2), word);
+			}
+		} else {
+			source &= 0xFFFFFFFE;
+			dest &= 0xFFFFFFFE;
+			uint16_t word = GBALoad16(&gba->memory.d, source);
+			for (i = 0; i < count; ++i) {
+				GBAStore16(&gba->memory.d, dest + (i << 1), word);
+			}
+		}
+	} else {
+		if (wordsize == 4) {
+			source &= 0xFFFFFFFC;
+			dest &= 0xFFFFFFFC;
+			for (i = 0; i < count; ++i) {
+				int32_t word = GBALoad32(&gba->memory.d, source + (i << 2));
+				GBAStore32(&gba->memory.d, dest + (i << 2), word);
+			}
+		} else {
+			source &= 0xFFFFFFFE;
+			dest &= 0xFFFFFFFE;
+			for (i = 0; i < count; ++i) {
+				uint16_t word = GBALoad16(&gba->memory.d, source + (i << 1));
+				GBAStore16(&gba->memory.d, dest + (i << 1), word);
+			}
+		}
+	}
+}
 
 void GBASwi16(struct ARMBoard* board, int immediate) {
+	struct GBA* gba = ((struct GBABoard*) board)->p;
 	switch (immediate) {
+	case 0xB:
+		_CpuSet(gba);
+		break;
 	default:
 		GBALog(GBA_LOG_STUB, "Stub software interrupt: %02x", immediate);
 	}

M src/gba/gba-memory.c → src/gba/gba-memory.c

@@ -7,16 +7,6 @@ #include <sys/mman.h>
 
 static const char* GBA_CANNOT_MMAP = "Could not map memory";
 
-static int32_t GBALoad32(struct ARMMemory* memory, uint32_t address);
-static int16_t GBALoad16(struct ARMMemory* memory, uint32_t address);
-static uint16_t GBALoadU16(struct ARMMemory* memory, uint32_t address);
-static int8_t GBALoad8(struct ARMMemory* memory, uint32_t address);
-static uint8_t GBALoadU8(struct ARMMemory* memory, uint32_t address);
-
-static void GBAStore32(struct ARMMemory* memory, uint32_t address, int32_t value);
-static void GBAStore16(struct ARMMemory* memory, uint32_t address, int16_t value);
-static void GBAStore8(struct ARMMemory* memory, uint32_t address, int8_t value);
-
 static void GBASetActiveRegion(struct ARMMemory* memory, uint32_t region);
 
 void GBAMemoryInit(struct GBAMemory* memory) {

M src/gba/gba-memory.h → src/gba/gba-memory.h

@@ -69,4 +69,14 @@ uint32_t* rom;
 	uint16_t io[SIZE_IO >> 1];
 };
 
+int32_t GBALoad32(struct ARMMemory* memory, uint32_t address);
+int16_t GBALoad16(struct ARMMemory* memory, uint32_t address);
+uint16_t GBALoadU16(struct ARMMemory* memory, uint32_t address);
+int8_t GBALoad8(struct ARMMemory* memory, uint32_t address);
+uint8_t GBALoadU8(struct ARMMemory* memory, uint32_t address);
+
+void GBAStore32(struct ARMMemory* memory, uint32_t address, int32_t value);
+void GBAStore16(struct ARMMemory* memory, uint32_t address, int16_t value);
+void GBAStore8(struct ARMMemory* memory, uint32_t address, int8_t value);
+
 #endif