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