1#include "gba-bios.h"
 2
 3#include "gba.h"
 4#include "gba-memory.h"
 5
 6static void _CpuSet(struct GBA* gba) {
 7	uint32_t source = gba->cpu.gprs[0];
 8	uint32_t dest = gba->cpu.gprs[1];
 9	uint32_t mode = gba->cpu.gprs[2];
10	int count = mode & 0x000FFFFF;
11	int fill = mode & 0x01000000;
12	int wordsize = (mode & 0x04000000) ? 4 : 2;
13	int i;
14	if (fill) {
15		if (wordsize == 4) {
16			source &= 0xFFFFFFFC;
17			dest &= 0xFFFFFFFC;
18			int32_t word = GBALoad32(&gba->memory.d, source);
19			for (i = 0; i < count; ++i) {
20				GBAStore32(&gba->memory.d, dest + (i << 2), word);
21			}
22		} else {
23			source &= 0xFFFFFFFE;
24			dest &= 0xFFFFFFFE;
25			uint16_t word = GBALoad16(&gba->memory.d, source);
26			for (i = 0; i < count; ++i) {
27				GBAStore16(&gba->memory.d, dest + (i << 1), word);
28			}
29		}
30	} else {
31		if (wordsize == 4) {
32			source &= 0xFFFFFFFC;
33			dest &= 0xFFFFFFFC;
34			for (i = 0; i < count; ++i) {
35				int32_t word = GBALoad32(&gba->memory.d, source + (i << 2));
36				GBAStore32(&gba->memory.d, dest + (i << 2), word);
37			}
38		} else {
39			source &= 0xFFFFFFFE;
40			dest &= 0xFFFFFFFE;
41			for (i = 0; i < count; ++i) {
42				uint16_t word = GBALoad16(&gba->memory.d, source + (i << 1));
43				GBAStore16(&gba->memory.d, dest + (i << 1), word);
44			}
45		}
46	}
47}
48
49void GBASwi16(struct ARMBoard* board, int immediate) {
50	struct GBA* gba = ((struct GBABoard*) board)->p;
51	switch (immediate) {
52	case 0x2:
53		GBAHalt(gba);
54		break;
55	case 0xB:
56		_CpuSet(gba);
57		break;
58	default:
59		GBALog(GBA_LOG_STUB, "Stub software interrupt: %02x", immediate);
60	}
61}
62
63void GBASwi32(struct ARMBoard* board, int immediate) {
64	GBASwi32(board, immediate >> 16);
65}