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		_unLz77(&gba->memory, gba->cpu.gprs[0], &((uint8_t*) gba->memory.wram)[(gba->cpu.gprs[1] & (SIZE_WORKING_RAM - 1))]);
 92		break;
 93	case 0x12:
 94		_unLz77(&gba->memory, gba->cpu.gprs[0], &((uint8_t*) gba->video.vram)[(gba->cpu.gprs[1] & 0x0001FFFF)]);
 95		break;
 96	case 0x1F:
 97		_MidiKey2Freq(gba);
 98		break;
 99	default:
100		GBALog(GBA_LOG_STUB, "Stub software interrupt: %02x", immediate);
101	}
102}
103
104void GBASwi32(struct ARMBoard* board, int immediate) {
105	GBASwi32(board, immediate >> 16);
106}
107
108static void _unLz77(struct GBAMemory* memory, uint32_t source, uint8_t* dest) {
109	int remaining = (GBALoad32(&memory->d, source) & 0xFFFFFF00) >> 8;
110	// We assume the signature byte (0x10) is correct
111	int blockheader;
112	uint32_t sPointer = source + 4;
113	uint8_t* dPointer = dest;
114	int blocksRemaining = 0;
115	int block;
116	uint8_t* disp;
117	int bytes;
118	while (remaining > 0) {
119		if (blocksRemaining) {
120			if (blockheader & 0x80) {
121				// Compressed
122				block = GBALoadU8(&memory->d, sPointer) | (GBALoadU8(&memory->d, sPointer + 1) << 8);
123				sPointer += 2;
124				disp = dPointer - (((block & 0x000F) << 8) | ((block & 0xFF00) >> 8)) - 1;
125				bytes = ((block & 0x00F0) >> 4) + 3;
126				while (bytes-- && remaining) {
127					--remaining;
128					*dPointer = *disp;
129					++disp;
130					++dPointer;
131				}
132			} else {
133				// Uncompressed
134				*dPointer = GBALoadU8(&memory->d, sPointer++);
135				++dPointer;
136				--remaining;
137			}
138			blockheader <<= 1;
139			--blocksRemaining;
140		} else {
141			blockheader = GBALoadU8(&memory->d, sPointer++);
142			blocksRemaining = 8;
143		}
144	}
145}