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
49static void _FastCpuSet(struct GBA* gba) {
50	uint32_t source = gba->cpu.gprs[0] & 0xFFFFFFFC;
51	uint32_t dest = gba->cpu.gprs[1] & 0xFFFFFFFC;
52	uint32_t mode = gba->cpu.gprs[2];
53	int count = mode & 0x000FFFFF;
54	count = ((count + 7) >> 3) << 3;
55	int i;
56	if (mode & 0x01000000) {
57		int32_t word = GBALoad32(&gba->memory.d, source);
58		for (i = 0; i < count; ++i) {
59			GBAStore32(&gba->memory.d, dest + (i << 2), word);
60		}
61	} else {
62		for (i = 0; i < count; ++i) {
63			int32_t word = GBALoad32(&gba->memory.d, source + (i << 2));
64			GBAStore32(&gba->memory.d, dest + (i << 2), word);
65		}
66	}
67}
68
69void GBASwi16(struct ARMBoard* board, int immediate) {
70	struct GBA* gba = ((struct GBABoard*) board)->p;
71	switch (immediate) {
72	case 0x2:
73		GBAHalt(gba);
74		break;
75	case 0xB:
76		_CpuSet(gba);
77		break;
78	case 0xC:
79		_FastCpuSet(gba);
80		break;
81	default:
82		GBALog(GBA_LOG_STUB, "Stub software interrupt: %02x", immediate);
83	}
84}
85
86void GBASwi32(struct ARMBoard* board, int immediate) {
87	GBASwi32(board, immediate >> 16);
88}