1#include "gba-bios.h"
  2
  3#include "gba.h"
  4#include "gba-io.h"
  5#include "gba-memory.h"
  6
  7#include <math.h>
  8
  9static void _unLz77(struct GBAMemory* memory, uint32_t source, uint8_t* dest);
 10
 11static void _CpuSet(struct GBA* gba) {
 12	uint32_t source = gba->cpu.gprs[0];
 13	uint32_t dest = gba->cpu.gprs[1];
 14	uint32_t mode = gba->cpu.gprs[2];
 15	int count = mode & 0x000FFFFF;
 16	int fill = mode & 0x01000000;
 17	int wordsize = (mode & 0x04000000) ? 4 : 2;
 18	int i;
 19	if (fill) {
 20		if (wordsize == 4) {
 21			source &= 0xFFFFFFFC;
 22			dest &= 0xFFFFFFFC;
 23			int32_t word = GBALoad32(&gba->memory.d, source);
 24			for (i = 0; i < count; ++i) {
 25				GBAStore32(&gba->memory.d, dest + (i << 2), word);
 26			}
 27		} else {
 28			source &= 0xFFFFFFFE;
 29			dest &= 0xFFFFFFFE;
 30			uint16_t word = GBALoad16(&gba->memory.d, source);
 31			for (i = 0; i < count; ++i) {
 32				GBAStore16(&gba->memory.d, dest + (i << 1), word);
 33			}
 34		}
 35	} else {
 36		if (wordsize == 4) {
 37			source &= 0xFFFFFFFC;
 38			dest &= 0xFFFFFFFC;
 39			for (i = 0; i < count; ++i) {
 40				int32_t word = GBALoad32(&gba->memory.d, source + (i << 2));
 41				GBAStore32(&gba->memory.d, dest + (i << 2), word);
 42			}
 43		} else {
 44			source &= 0xFFFFFFFE;
 45			dest &= 0xFFFFFFFE;
 46			for (i = 0; i < count; ++i) {
 47				uint16_t word = GBALoad16(&gba->memory.d, source + (i << 1));
 48				GBAStore16(&gba->memory.d, dest + (i << 1), word);
 49			}
 50		}
 51	}
 52}
 53
 54static void _FastCpuSet(struct GBA* gba) {
 55	uint32_t source = gba->cpu.gprs[0] & 0xFFFFFFFC;
 56	uint32_t dest = gba->cpu.gprs[1] & 0xFFFFFFFC;
 57	uint32_t mode = gba->cpu.gprs[2];
 58	int count = mode & 0x000FFFFF;
 59	count = ((count + 7) >> 3) << 3;
 60	int i;
 61	if (mode & 0x01000000) {
 62		int32_t word = GBALoad32(&gba->memory.d, source);
 63		for (i = 0; i < count; ++i) {
 64			GBAStore32(&gba->memory.d, dest + (i << 2), word);
 65		}
 66	} else {
 67		for (i = 0; i < count; ++i) {
 68			int32_t word = GBALoad32(&gba->memory.d, source + (i << 2));
 69			GBAStore32(&gba->memory.d, dest + (i << 2), word);
 70		}
 71	}
 72}
 73
 74static void _MidiKey2Freq(struct GBA* gba) {
 75	uint32_t key = GBALoad32(&gba->memory.d, gba->cpu.gprs[0] + 4);
 76	gba->cpu.gprs[0] = key / powf(2, (180.f - gba->cpu.gprs[1] - gba->cpu.gprs[2] / 256.f) / 12.f);
 77}
 78
 79void GBASwi16(struct ARMBoard* board, int immediate) {
 80	struct GBA* gba = ((struct GBABoard*) board)->p;
 81	switch (immediate) {
 82	case 0x2:
 83		GBAHalt(gba);
 84		break;
 85	case 0x05:
 86		// VBlankIntrWait
 87		gba->cpu.gprs[0] = 1;
 88		gba->cpu.gprs[1] = 1;
 89		// Fall through:
 90	case 0x04:
 91		// IntrWait
 92		gba->memory.io[REG_IME >> 1] = 1;
 93		if (!gba->cpu.gprs[0] && gba->memory.io[REG_IF >> 1] & gba->cpu.gprs[1]) {
 94			break;
 95		}
 96		gba->memory.io[REG_IF >> 1] = 0;
 97		ARMRaiseSWI(&gba->cpu);
 98		break;
 99	case 0xB:
100		_CpuSet(gba);
101		break;
102	case 0xC:
103		_FastCpuSet(gba);
104		break;
105	case 0x11:
106	case 0x12:
107		switch (gba->cpu.gprs[1] >> BASE_OFFSET) {
108			case REGION_WORKING_RAM:
109				_unLz77(&gba->memory, gba->cpu.gprs[0], &((uint8_t*) gba->memory.wram)[(gba->cpu.gprs[1] & (SIZE_WORKING_RAM - 1))]);
110				break;
111			case REGION_VRAM:
112				_unLz77(&gba->memory, gba->cpu.gprs[0], &((uint8_t*) gba->video.vram)[(gba->cpu.gprs[1] & 0x0001FFFF)]);
113				break;
114			default:
115				GBALog(GBA_LOG_WARN, "Bad LZ77 destination");
116				break;
117		}
118		break;
119	case 0x1F:
120		_MidiKey2Freq(gba);
121		break;
122	default:
123		GBALog(GBA_LOG_STUB, "Stub software interrupt: %02x", immediate);
124	}
125}
126
127void GBASwi32(struct ARMBoard* board, int immediate) {
128	GBASwi32(board, immediate >> 16);
129}
130
131static void _unLz77(struct GBAMemory* memory, uint32_t source, uint8_t* dest) {
132	int remaining = (GBALoad32(&memory->d, source) & 0xFFFFFF00) >> 8;
133	// We assume the signature byte (0x10) is correct
134	int blockheader;
135	uint32_t sPointer = source + 4;
136	uint8_t* dPointer = dest;
137	int blocksRemaining = 0;
138	int block;
139	uint8_t* disp;
140	int bytes;
141	while (remaining > 0) {
142		if (blocksRemaining) {
143			if (blockheader & 0x80) {
144				// Compressed
145				block = GBALoadU8(&memory->d, sPointer) | (GBALoadU8(&memory->d, sPointer + 1) << 8);
146				sPointer += 2;
147				disp = dPointer - (((block & 0x000F) << 8) | ((block & 0xFF00) >> 8)) - 1;
148				bytes = ((block & 0x00F0) >> 4) + 3;
149				while (bytes-- && remaining) {
150					--remaining;
151					*dPointer = *disp;
152					++disp;
153					++dPointer;
154				}
155			} else {
156				// Uncompressed
157				*dPointer = GBALoadU8(&memory->d, sPointer++);
158				++dPointer;
159				--remaining;
160			}
161			blockheader <<= 1;
162			--blocksRemaining;
163		} else {
164			blockheader = GBALoadU8(&memory->d, sPointer++);
165			blocksRemaining = 8;
166		}
167	}
168}