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