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 0x6:
100		{
101			div_t result = div(gba->cpu.gprs[0], gba->cpu.gprs[1]);
102			gba->cpu.gprs[0] = result.quot;
103			gba->cpu.gprs[1] = result.rem;
104			gba->cpu.gprs[2] = abs(result.quot);
105		}
106		break;
107	case 0xB:
108		_CpuSet(gba);
109		break;
110	case 0xC:
111		_FastCpuSet(gba);
112		break;
113	case 0x11:
114	case 0x12:
115		switch (gba->cpu.gprs[1] >> BASE_OFFSET) {
116			case REGION_WORKING_RAM:
117				_unLz77(&gba->memory, gba->cpu.gprs[0], &((uint8_t*) gba->memory.wram)[(gba->cpu.gprs[1] & (SIZE_WORKING_RAM - 1))]);
118				break;
119			case REGION_VRAM:
120				_unLz77(&gba->memory, gba->cpu.gprs[0], &((uint8_t*) gba->video.vram)[(gba->cpu.gprs[1] & 0x0001FFFF)]);
121				break;
122			default:
123				GBALog(GBA_LOG_WARN, "Bad LZ77 destination");
124				break;
125		}
126		break;
127	case 0x1F:
128		_MidiKey2Freq(gba);
129		break;
130	default:
131		GBALog(GBA_LOG_STUB, "Stub software interrupt: %02x", immediate);
132	}
133}
134
135void GBASwi32(struct ARMBoard* board, int immediate) {
136	GBASwi32(board, immediate >> 16);
137}
138
139static void _unLz77(struct GBAMemory* memory, uint32_t source, uint8_t* dest) {
140	int remaining = (GBALoad32(&memory->d, source) & 0xFFFFFF00) >> 8;
141	// We assume the signature byte (0x10) is correct
142	int blockheader;
143	uint32_t sPointer = source + 4;
144	uint8_t* dPointer = dest;
145	int blocksRemaining = 0;
146	int block;
147	uint8_t* disp;
148	int bytes;
149	while (remaining > 0) {
150		if (blocksRemaining) {
151			if (blockheader & 0x80) {
152				// Compressed
153				block = GBALoadU8(&memory->d, sPointer) | (GBALoadU8(&memory->d, sPointer + 1) << 8);
154				sPointer += 2;
155				disp = dPointer - (((block & 0x000F) << 8) | ((block & 0xFF00) >> 8)) - 1;
156				bytes = ((block & 0x00F0) >> 4) + 3;
157				while (bytes-- && remaining) {
158					--remaining;
159					*dPointer = *disp;
160					++disp;
161					++dPointer;
162				}
163			} else {
164				// Uncompressed
165				*dPointer = GBALoadU8(&memory->d, sPointer++);
166				++dPointer;
167				--remaining;
168			}
169			blockheader <<= 1;
170			--blocksRemaining;
171		} else {
172			blockheader = GBALoadU8(&memory->d, sPointer++);
173			blocksRemaining = 8;
174		}
175	}
176}