1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/src/gba/bios.cpp	Sat Mar 03 10:31:27 2012 -0600
     1.3 @@ -0,0 +1,1278 @@
     1.4 +#include <cmath>
     1.5 +#include <cstdlib>
     1.6 +#include <cstring>
     1.7 +
     1.8 +#include "bios.h"
     1.9 +#include "../common/System.h"
    1.10 +#include "GBA.h"
    1.11 +#include "GBACheats.h" // FIXME: SDL build requires this
    1.12 +#include "GBAinline.h"
    1.13 +#include "GBAGlobals.h"
    1.14 +
    1.15 +s16 sineTable[256] = {
    1.16 +	(s16)0x0000, (s16)0x0192, (s16)0x0323, (s16)0x04B5, (s16)0x0645, (s16)0x07D5, (s16)0x0964, (s16)0x0AF1,
    1.17 +	(s16)0x0C7C, (s16)0x0E05, (s16)0x0F8C, (s16)0x1111, (s16)0x1294, (s16)0x1413, (s16)0x158F, (s16)0x1708,
    1.18 +	(s16)0x187D, (s16)0x19EF, (s16)0x1B5D, (s16)0x1CC6, (s16)0x1E2B, (s16)0x1F8B, (s16)0x20E7, (s16)0x223D,
    1.19 +	(s16)0x238E, (s16)0x24DA, (s16)0x261F, (s16)0x275F, (s16)0x2899, (s16)0x29CD, (s16)0x2AFA, (s16)0x2C21,
    1.20 +	(s16)0x2D41, (s16)0x2E5A, (s16)0x2F6B, (s16)0x3076, (s16)0x3179, (s16)0x3274, (s16)0x3367, (s16)0x3453,
    1.21 +	(s16)0x3536, (s16)0x3612, (s16)0x36E5, (s16)0x37AF, (s16)0x3871, (s16)0x392A, (s16)0x39DA, (s16)0x3A82,
    1.22 +	(s16)0x3B20, (s16)0x3BB6, (s16)0x3C42, (s16)0x3CC5, (s16)0x3D3E, (s16)0x3DAE, (s16)0x3E14, (s16)0x3E71,
    1.23 +	(s16)0x3EC5, (s16)0x3F0E, (s16)0x3F4E, (s16)0x3F84, (s16)0x3FB1, (s16)0x3FD3, (s16)0x3FEC, (s16)0x3FFB,
    1.24 +	(s16)0x4000, (s16)0x3FFB, (s16)0x3FEC, (s16)0x3FD3, (s16)0x3FB1, (s16)0x3F84, (s16)0x3F4E, (s16)0x3F0E,
    1.25 +	(s16)0x3EC5, (s16)0x3E71, (s16)0x3E14, (s16)0x3DAE, (s16)0x3D3E, (s16)0x3CC5, (s16)0x3C42, (s16)0x3BB6,
    1.26 +	(s16)0x3B20, (s16)0x3A82, (s16)0x39DA, (s16)0x392A, (s16)0x3871, (s16)0x37AF, (s16)0x36E5, (s16)0x3612,
    1.27 +	(s16)0x3536, (s16)0x3453, (s16)0x3367, (s16)0x3274, (s16)0x3179, (s16)0x3076, (s16)0x2F6B, (s16)0x2E5A,
    1.28 +	(s16)0x2D41, (s16)0x2C21, (s16)0x2AFA, (s16)0x29CD, (s16)0x2899, (s16)0x275F, (s16)0x261F, (s16)0x24DA,
    1.29 +	(s16)0x238E, (s16)0x223D, (s16)0x20E7, (s16)0x1F8B, (s16)0x1E2B, (s16)0x1CC6, (s16)0x1B5D, (s16)0x19EF,
    1.30 +	(s16)0x187D, (s16)0x1708, (s16)0x158F, (s16)0x1413, (s16)0x1294, (s16)0x1111, (s16)0x0F8C, (s16)0x0E05,
    1.31 +	(s16)0x0C7C, (s16)0x0AF1, (s16)0x0964, (s16)0x07D5, (s16)0x0645, (s16)0x04B5, (s16)0x0323, (s16)0x0192,
    1.32 +	(s16)0x0000, (s16)0xFE6E, (s16)0xFCDD, (s16)0xFB4B, (s16)0xF9BB, (s16)0xF82B, (s16)0xF69C, (s16)0xF50F,
    1.33 +	(s16)0xF384, (s16)0xF1FB, (s16)0xF074, (s16)0xEEEF, (s16)0xED6C, (s16)0xEBED, (s16)0xEA71, (s16)0xE8F8,
    1.34 +	(s16)0xE783, (s16)0xE611, (s16)0xE4A3, (s16)0xE33A, (s16)0xE1D5, (s16)0xE075, (s16)0xDF19, (s16)0xDDC3,
    1.35 +	(s16)0xDC72, (s16)0xDB26, (s16)0xD9E1, (s16)0xD8A1, (s16)0xD767, (s16)0xD633, (s16)0xD506, (s16)0xD3DF,
    1.36 +	(s16)0xD2BF, (s16)0xD1A6, (s16)0xD095, (s16)0xCF8A, (s16)0xCE87, (s16)0xCD8C, (s16)0xCC99, (s16)0xCBAD,
    1.37 +	(s16)0xCACA, (s16)0xC9EE, (s16)0xC91B, (s16)0xC851, (s16)0xC78F, (s16)0xC6D6, (s16)0xC626, (s16)0xC57E,
    1.38 +	(s16)0xC4E0, (s16)0xC44A, (s16)0xC3BE, (s16)0xC33B, (s16)0xC2C2, (s16)0xC252, (s16)0xC1EC, (s16)0xC18F,
    1.39 +	(s16)0xC13B, (s16)0xC0F2, (s16)0xC0B2, (s16)0xC07C, (s16)0xC04F, (s16)0xC02D, (s16)0xC014, (s16)0xC005,
    1.40 +	(s16)0xC000, (s16)0xC005, (s16)0xC014, (s16)0xC02D, (s16)0xC04F, (s16)0xC07C, (s16)0xC0B2, (s16)0xC0F2,
    1.41 +	(s16)0xC13B, (s16)0xC18F, (s16)0xC1EC, (s16)0xC252, (s16)0xC2C2, (s16)0xC33B, (s16)0xC3BE, (s16)0xC44A,
    1.42 +	(s16)0xC4E0, (s16)0xC57E, (s16)0xC626, (s16)0xC6D6, (s16)0xC78F, (s16)0xC851, (s16)0xC91B, (s16)0xC9EE,
    1.43 +	(s16)0xCACA, (s16)0xCBAD, (s16)0xCC99, (s16)0xCD8C, (s16)0xCE87, (s16)0xCF8A, (s16)0xD095, (s16)0xD1A6,
    1.44 +	(s16)0xD2BF, (s16)0xD3DF, (s16)0xD506, (s16)0xD633, (s16)0xD767, (s16)0xD8A1, (s16)0xD9E1, (s16)0xDB26,
    1.45 +	(s16)0xDC72, (s16)0xDDC3, (s16)0xDF19, (s16)0xE075, (s16)0xE1D5, (s16)0xE33A, (s16)0xE4A3, (s16)0xE611,
    1.46 +	(s16)0xE783, (s16)0xE8F8, (s16)0xEA71, (s16)0xEBED, (s16)0xED6C, (s16)0xEEEF, (s16)0xF074, (s16)0xF1FB,
    1.47 +	(s16)0xF384, (s16)0xF50F, (s16)0xF69C, (s16)0xF82B, (s16)0xF9BB, (s16)0xFB4B, (s16)0xFCDD, (s16)0xFE6E
    1.48 +};
    1.49 +
    1.50 +void BIOS_ArcTan()
    1.51 +{
    1.52 +#ifdef GBA_LOGGING
    1.53 +	if (systemVerbose & VERBOSE_SWI)
    1.54 +	{
    1.55 +		log("ArcTan: %08x (VCOUNT=%2d)\n",
    1.56 +		    reg[0].I,
    1.57 +		    VCOUNT);
    1.58 +	}
    1.59 +#endif
    1.60 +
    1.61 +	s32 a = -((s32)(reg[0].I * reg[0].I)) >> 14;
    1.62 +	s32 b = ((0xA9 * a) >> 14) + 0x390;
    1.63 +	b        = ((b * a) >> 14) + 0x91C;
    1.64 +	b        = ((b * a) >> 14) + 0xFB6;
    1.65 +	b        = ((b * a) >> 14) + 0x16AA;
    1.66 +	b        = ((b * a) >> 14) + 0x2081;
    1.67 +	b        = ((b * a) >> 14) + 0x3651;
    1.68 +	b        = ((b * a) >> 14) + 0xA2F9;
    1.69 +	reg[0].I = (reg[0].I * b) >> 16;
    1.70 +
    1.71 +#ifdef GBA_LOGGING
    1.72 +	if (systemVerbose & VERBOSE_SWI)
    1.73 +	{
    1.74 +		log("ArcTan: return=%08x\n",
    1.75 +		    reg[0].I);
    1.76 +	}
    1.77 +#endif
    1.78 +}
    1.79 +
    1.80 +void BIOS_ArcTan2()
    1.81 +{
    1.82 +#ifdef GBA_LOGGING
    1.83 +	if (systemVerbose & VERBOSE_SWI)
    1.84 +	{
    1.85 +		log("ArcTan2: %08x,%08x (VCOUNT=%2d)\n",
    1.86 +		    reg[0].I,
    1.87 +		    reg[1].I,
    1.88 +		    VCOUNT);
    1.89 +	}
    1.90 +#endif
    1.91 +
    1.92 +	s16 x = reg[0].I;
    1.93 +	s16 y = reg[1].I;
    1.94 +
    1.95 +	if (y == 0)
    1.96 +	{
    1.97 +		reg[0].I = 0x8000 & x;
    1.98 +		reg[3].I = 0x170;
    1.99 +	}
   1.100 +	else
   1.101 +	{
   1.102 +		if (x == 0)
   1.103 +		{
   1.104 +			reg[0].I = (0x8000 & y) + 0x4000;
   1.105 +			reg[3].I = 0x170;
   1.106 +		}
   1.107 +		else
   1.108 +		{
   1.109 +			if (abs(x) > abs(y))
   1.110 +			{
   1.111 +				reg[1].I = x;
   1.112 +				reg[0].I = y << 14;
   1.113 +				BIOS_Div();
   1.114 +				BIOS_ArcTan();
   1.115 +				if (x < 0)
   1.116 +					reg[0].I = 0x8000 + reg[0].I;
   1.117 +				else
   1.118 +					reg[0].I = ((y & 0x8000) << 1) + reg[0].I;
   1.119 +				reg[3].I = 0x170;
   1.120 +			}
   1.121 +			else
   1.122 +			{
   1.123 +				reg[0].I = x << 14;
   1.124 +				BIOS_Div();
   1.125 +				BIOS_ArcTan();
   1.126 +				reg[0].I = (0x4000 + (y & 0x8000)) - reg[0].I;
   1.127 +				reg[3].I = 0x170;
   1.128 +			}
   1.129 +		}
   1.130 +	}
   1.131 +
   1.132 +#ifdef GBA_LOGGING
   1.133 +	if (systemVerbose & VERBOSE_SWI)
   1.134 +	{
   1.135 +		log("ArcTan2: return=%08x\n",
   1.136 +		    reg[0].I);
   1.137 +	}
   1.138 +#endif
   1.139 +}
   1.140 +
   1.141 +void BIOS_BitUnPack()
   1.142 +{
   1.143 +#ifdef GBA_LOGGING
   1.144 +	if (systemVerbose & VERBOSE_SWI)
   1.145 +	{
   1.146 +		log("BitUnPack: %08x,%08x,%08x (VCOUNT=%2d)\n",
   1.147 +		    reg[0].I,
   1.148 +		    reg[1].I,
   1.149 +		    reg[2].I,
   1.150 +		    VCOUNT);
   1.151 +	}
   1.152 +#endif
   1.153 +
   1.154 +	u32 source = reg[0].I;
   1.155 +	u32 dest   = reg[1].I;
   1.156 +	u32 header = reg[2].I;
   1.157 +
   1.158 +	int len = CPUReadHalfWord(header);
   1.159 +	// check address
   1.160 +	int bits    = CPUReadByte(header+2);
   1.161 +	int revbits = 8 - bits;
   1.162 +	// u32 value = 0;
   1.163 +	u32  base    = CPUReadMemory(header+4);
   1.164 +	bool addBase = (base & 0x80000000) ? true : false;
   1.165 +	base &= 0x7fffffff;
   1.166 +	int dataSize = CPUReadByte(header+3);
   1.167 +
   1.168 +	int data = 0;
   1.169 +	int bitwritecount = 0;
   1.170 +	while (1)
   1.171 +	{
   1.172 +		len -= 1;
   1.173 +		if (len < 0)
   1.174 +			break;
   1.175 +		int mask = 0xff >> revbits;
   1.176 +		u8  b    = CPUReadByte(source);
   1.177 +		source++;
   1.178 +		int bitcount = 0;
   1.179 +		while (1)
   1.180 +		{
   1.181 +			if (bitcount >= 8)
   1.182 +				break;
   1.183 +			u32 d    = b & mask;
   1.184 +			u32 temp = d >> bitcount;
   1.185 +			if (!temp && addBase)
   1.186 +			{
   1.187 +				temp += base;
   1.188 +			}
   1.189 +			data |= temp << bitwritecount;
   1.190 +			bitwritecount += dataSize;
   1.191 +			if (bitwritecount >= 32)
   1.192 +			{
   1.193 +				CPUWriteMemory(dest, data);
   1.194 +				dest += 4;
   1.195 +				data  = 0;
   1.196 +				bitwritecount = 0;
   1.197 +			}
   1.198 +			mask    <<= bits;
   1.199 +			bitcount += bits;
   1.200 +		}
   1.201 +	}
   1.202 +}
   1.203 +
   1.204 +void BIOS_BgAffineSet()
   1.205 +{
   1.206 +#ifdef GBA_LOGGING
   1.207 +	if (systemVerbose & VERBOSE_SWI)
   1.208 +	{
   1.209 +		log("BgAffineSet: %08x,%08x,%08x (VCOUNT=%2d)\n",
   1.210 +		    reg[0].I,
   1.211 +		    reg[1].I,
   1.212 +		    reg[2].I,
   1.213 +		    VCOUNT);
   1.214 +	}
   1.215 +#endif
   1.216 +
   1.217 +	u32 src  = reg[0].I;
   1.218 +	u32 dest = reg[1].I;
   1.219 +	int num  = reg[2].I;
   1.220 +
   1.221 +	for (int i = 0; i < num; i++)
   1.222 +	{
   1.223 +		s32 cx = CPUReadMemory(src);
   1.224 +		src += 4;
   1.225 +		s32 cy = CPUReadMemory(src);
   1.226 +		src += 4;
   1.227 +		s16 dispx = CPUReadHalfWord(src);
   1.228 +		src += 2;
   1.229 +		s16 dispy = CPUReadHalfWord(src);
   1.230 +		src += 2;
   1.231 +		s16 rx = CPUReadHalfWord(src);
   1.232 +		src += 2;
   1.233 +		s16 ry = CPUReadHalfWord(src);
   1.234 +		src += 2;
   1.235 +		u16 theta = CPUReadHalfWord(src)>>8;
   1.236 +		src += 4; // keep structure alignment
   1.237 +		s32 a = (s32)sineTable[(theta+0x40)&255];
   1.238 +		s32 b = (s32)sineTable[theta];
   1.239 +
   1.240 +		s16 dx  =  (s16)((rx * a)>>14);
   1.241 +		s16 dmx = (s16)((rx * b)>>14);
   1.242 +		s16 dy  =  (s16)((ry * b)>>14);
   1.243 +		s16 dmy = (s16)((ry * a)>>14);
   1.244 +
   1.245 +		CPUWriteHalfWord(dest, dx);
   1.246 +		dest += 2;
   1.247 +		CPUWriteHalfWord(dest, -dmx);
   1.248 +		dest += 2;
   1.249 +		CPUWriteHalfWord(dest, dy);
   1.250 +		dest += 2;
   1.251 +		CPUWriteHalfWord(dest, dmy);
   1.252 +		dest += 2;
   1.253 +
   1.254 +		s32 startx = cx - dx * dispx + dmx * dispy;
   1.255 +		s32 starty = cy - dy * dispx - dmy * dispy;
   1.256 +
   1.257 +		CPUWriteMemory(dest, startx);
   1.258 +		dest += 4;
   1.259 +		CPUWriteMemory(dest, starty);
   1.260 +		dest += 4;
   1.261 +	}
   1.262 +}
   1.263 +
   1.264 +void BIOS_CpuSet()
   1.265 +{
   1.266 +#ifdef GBA_LOGGING
   1.267 +	if (systemVerbose & VERBOSE_SWI)
   1.268 +	{
   1.269 +		log("CpuSet: 0x%08x,0x%08x,0x%08x (VCOUNT=%d)\n", reg[0].I, reg[1].I,
   1.270 +		    reg[2].I, VCOUNT);
   1.271 +	}
   1.272 +#endif
   1.273 +
   1.274 +	u32 source = reg[0].I;
   1.275 +	u32 dest   = reg[1].I;
   1.276 +	u32 cnt    = reg[2].I;
   1.277 +
   1.278 +	if (((source & 0xe000000) == 0) ||
   1.279 +	    ((source + (((cnt << 11)>>9) & 0x1fffff)) & 0xe000000) == 0)
   1.280 +		return;
   1.281 +
   1.282 +	int count = cnt & 0x1FFFFF;
   1.283 +
   1.284 +	// 32-bit ?
   1.285 +	if ((cnt >> 26) & 1)
   1.286 +	{
   1.287 +		// needed for 32-bit mode!
   1.288 +		source &= 0xFFFFFFFC;
   1.289 +		dest   &= 0xFFFFFFFC;
   1.290 +		// fill ?
   1.291 +		if ((cnt >> 24) & 1)
   1.292 +		{
   1.293 +			u32 value = CPUReadMemory(source);
   1.294 +			while (count)
   1.295 +			{
   1.296 +				CPUWriteMemory(dest, value);
   1.297 +				dest += 4;
   1.298 +				count--;
   1.299 +			}
   1.300 +		}
   1.301 +		else
   1.302 +		{
   1.303 +			// copy
   1.304 +			while (count)
   1.305 +			{
   1.306 +				CPUWriteMemory(dest, CPUReadMemory(source));
   1.307 +				source += 4;
   1.308 +				dest   += 4;
   1.309 +				count--;
   1.310 +			}
   1.311 +		}
   1.312 +	}
   1.313 +	else
   1.314 +	{
   1.315 +		// 16-bit fill?
   1.316 +		if ((cnt >> 24) & 1)
   1.317 +		{
   1.318 +			u16 value = CPUReadHalfWord(source);
   1.319 +			while (count)
   1.320 +			{
   1.321 +				CPUWriteHalfWord(dest, value);
   1.322 +				dest += 2;
   1.323 +				count--;
   1.324 +			}
   1.325 +		}
   1.326 +		else
   1.327 +		{
   1.328 +			// copy
   1.329 +			while (count)
   1.330 +			{
   1.331 +				CPUWriteHalfWord(dest, CPUReadHalfWord(source));
   1.332 +				source += 2;
   1.333 +				dest   += 2;
   1.334 +				count--;
   1.335 +			}
   1.336 +		}
   1.337 +	}
   1.338 +}
   1.339 +
   1.340 +void BIOS_CpuFastSet()
   1.341 +{
   1.342 +#ifdef GBA_LOGGING
   1.343 +	if (systemVerbose & VERBOSE_SWI)
   1.344 +	{
   1.345 +		log("CpuFastSet: 0x%08x,0x%08x,0x%08x (VCOUNT=%d)\n", reg[0].I, reg[1].I,
   1.346 +		    reg[2].I, VCOUNT);
   1.347 +	}
   1.348 +#endif
   1.349 +
   1.350 +	u32 source = reg[0].I;
   1.351 +	u32 dest   = reg[1].I;
   1.352 +	u32 cnt    = reg[2].I;
   1.353 +
   1.354 +	if (((source & 0xe000000) == 0) ||
   1.355 +	    ((source + (((cnt << 11)>>9) & 0x1fffff)) & 0xe000000) == 0)
   1.356 +		return;
   1.357 +
   1.358 +	// needed for 32-bit mode!
   1.359 +	source &= 0xFFFFFFFC;
   1.360 +	dest   &= 0xFFFFFFFC;
   1.361 +
   1.362 +	int count = cnt & 0x1FFFFF;
   1.363 +
   1.364 +	// fill?
   1.365 +	if ((cnt >> 24) & 1)
   1.366 +	{
   1.367 +		while (count > 0)
   1.368 +		{
   1.369 +			// BIOS always transfers 32 bytes at a time
   1.370 +			u32 value = CPUReadMemory(source);
   1.371 +			for (int i = 0; i < 8; i++)
   1.372 +			{
   1.373 +				CPUWriteMemory(dest, value);
   1.374 +				dest += 4;
   1.375 +			}
   1.376 +			count -= 8;
   1.377 +		}
   1.378 +	}
   1.379 +	else
   1.380 +	{
   1.381 +		// copy
   1.382 +		while (count > 0)
   1.383 +		{
   1.384 +			// BIOS always transfers 32 bytes at a time
   1.385 +			for (int i = 0; i < 8; i++)
   1.386 +			{
   1.387 +				CPUWriteMemory(dest, CPUReadMemory(source));
   1.388 +				source += 4;
   1.389 +				dest   += 4;
   1.390 +			}
   1.391 +			count -= 8;
   1.392 +		}
   1.393 +	}
   1.394 +}
   1.395 +
   1.396 +void BIOS_Diff8bitUnFilterWram()
   1.397 +{
   1.398 +#ifdef GBA_LOGGING
   1.399 +	if (systemVerbose & VERBOSE_SWI)
   1.400 +	{
   1.401 +		log("Diff8bitUnFilterWram: 0x%08x,0x%08x (VCOUNT=%d)\n", reg[0].I,
   1.402 +		    reg[1].I, VCOUNT);
   1.403 +	}
   1.404 +#endif
   1.405 +
   1.406 +	u32 source = reg[0].I;
   1.407 +	u32 dest   = reg[1].I;
   1.408 +
   1.409 +	u32 header = CPUReadMemory(source);
   1.410 +	source += 4;
   1.411 +
   1.412 +	if (((source & 0xe000000) == 0) ||
   1.413 +	    ((source + ((header >> 8) & 0x1fffff) & 0xe000000) == 0))
   1.414 +		return;
   1.415 +
   1.416 +	int len = header >> 8;
   1.417 +
   1.418 +	u8 data = CPUReadByte(source++);
   1.419 +	CPUWriteByte(dest++, data);
   1.420 +	len--;
   1.421 +
   1.422 +	while (len > 0)
   1.423 +	{
   1.424 +		u8 diff = CPUReadByte(source++);
   1.425 +		data += diff;
   1.426 +		CPUWriteByte(dest++, data);
   1.427 +		len--;
   1.428 +	}
   1.429 +}
   1.430 +
   1.431 +void BIOS_Diff8bitUnFilterVram()
   1.432 +{
   1.433 +#ifdef GBA_LOGGING
   1.434 +	if (systemVerbose & VERBOSE_SWI)
   1.435 +	{
   1.436 +		log("Diff8bitUnFilterVram: 0x%08x,0x%08x (VCOUNT=%d)\n", reg[0].I,
   1.437 +		    reg[1].I, VCOUNT);
   1.438 +	}
   1.439 +#endif
   1.440 +
   1.441 +	u32 source = reg[0].I;
   1.442 +	u32 dest   = reg[1].I;
   1.443 +
   1.444 +	u32 header = CPUReadMemory(source);
   1.445 +	source += 4;
   1.446 +
   1.447 +	if (((source & 0xe000000) == 0) ||
   1.448 +	    ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)
   1.449 +		return;
   1.450 +
   1.451 +	int len = header >> 8;
   1.452 +
   1.453 +	u8  data      = CPUReadByte(source++);
   1.454 +	u16 writeData = data;
   1.455 +	int shift     = 8;
   1.456 +	int bytes     = 1;
   1.457 +
   1.458 +	while (len >= 2)
   1.459 +	{
   1.460 +		u8 diff = CPUReadByte(source++);
   1.461 +		data      += diff;
   1.462 +		writeData |= (data << shift);
   1.463 +		bytes++;
   1.464 +		shift += 8;
   1.465 +		if (bytes == 2)
   1.466 +		{
   1.467 +			CPUWriteHalfWord(dest, writeData);
   1.468 +			dest     += 2;
   1.469 +			len      -= 2;
   1.470 +			bytes     = 0;
   1.471 +			writeData = 0;
   1.472 +			shift     = 0;
   1.473 +		}
   1.474 +	}
   1.475 +}
   1.476 +
   1.477 +void BIOS_Diff16bitUnFilter()
   1.478 +{
   1.479 +#ifdef GBA_LOGGING
   1.480 +	if (systemVerbose & VERBOSE_SWI)
   1.481 +	{
   1.482 +		log("Diff16bitUnFilter: 0x%08x,0x%08x (VCOUNT=%d)\n", reg[0].I,
   1.483 +		    reg[1].I, VCOUNT);
   1.484 +	}
   1.485 +#endif
   1.486 +
   1.487 +	u32 source = reg[0].I;
   1.488 +	u32 dest   = reg[1].I;
   1.489 +
   1.490 +	u32 header = CPUReadMemory(source);
   1.491 +	source += 4;
   1.492 +
   1.493 +	if (((source & 0xe000000) == 0) ||
   1.494 +	    ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)
   1.495 +		return;
   1.496 +
   1.497 +	int len = header >> 8;
   1.498 +
   1.499 +	u16 data = CPUReadHalfWord(source);
   1.500 +	source += 2;
   1.501 +	CPUWriteHalfWord(dest, data);
   1.502 +	dest += 2;
   1.503 +	len  -= 2;
   1.504 +
   1.505 +	while (len >= 2)
   1.506 +	{
   1.507 +		u16 diff = CPUReadHalfWord(source);
   1.508 +		source += 2;
   1.509 +		data   += diff;
   1.510 +		CPUWriteHalfWord(dest, data);
   1.511 +		dest += 2;
   1.512 +		len  -= 2;
   1.513 +	}
   1.514 +}
   1.515 +
   1.516 +void BIOS_Div()
   1.517 +{
   1.518 +#ifdef GBA_LOGGING
   1.519 +	if (systemVerbose & VERBOSE_SWI)
   1.520 +	{
   1.521 +		log("Div: 0x%08x,0x%08x (VCOUNT=%d)\n",
   1.522 +		    reg[0].I,
   1.523 +		    reg[1].I,
   1.524 +		    VCOUNT);
   1.525 +	}
   1.526 +#endif
   1.527 +
   1.528 +	int number = reg[0].I;
   1.529 +	int denom  = reg[1].I;
   1.530 +
   1.531 +	if (denom != 0)
   1.532 +	{
   1.533 +		reg[0].I = number / denom;
   1.534 +		reg[1].I = number % denom;
   1.535 +		s32 temp = (s32)reg[0].I;
   1.536 +		reg[3].I = temp < 0 ? (u32)-temp : (u32)temp;
   1.537 +	}
   1.538 +#ifdef GBA_LOGGING
   1.539 +	if (systemVerbose & VERBOSE_SWI)
   1.540 +	{
   1.541 +		log("Div: return=0x%08x,0x%08x,0x%08x\n",
   1.542 +		    reg[0].I,
   1.543 +		    reg[1].I,
   1.544 +		    reg[3].I);
   1.545 +	}
   1.546 +#endif
   1.547 +}
   1.548 +
   1.549 +void BIOS_DivARM()
   1.550 +{
   1.551 +#ifdef GBA_LOGGING
   1.552 +	if (systemVerbose & VERBOSE_SWI)
   1.553 +	{
   1.554 +		log("DivARM: 0x%08x, (VCOUNT=%d)\n",
   1.555 +		    reg[0].I,
   1.556 +		    VCOUNT);
   1.557 +	}
   1.558 +#endif
   1.559 +
   1.560 +	u32 temp = reg[0].I;
   1.561 +	reg[0].I = reg[1].I;
   1.562 +	reg[1].I = temp;
   1.563 +	BIOS_Div();
   1.564 +}
   1.565 +
   1.566 +void BIOS_HuffUnComp()
   1.567 +{
   1.568 +#ifdef GBA_LOGGING
   1.569 +	if (systemVerbose & VERBOSE_SWI)
   1.570 +	{
   1.571 +		log("HuffUnComp: 0x%08x,0x%08x (VCOUNT=%d)\n",
   1.572 +		    reg[0].I,
   1.573 +		    reg[1].I,
   1.574 +		    VCOUNT);
   1.575 +	}
   1.576 +#endif
   1.577 +
   1.578 +	u32 source = reg[0].I;
   1.579 +	u32 dest   = reg[1].I;
   1.580 +
   1.581 +	u32 header = CPUReadMemory(source);
   1.582 +	source += 4;
   1.583 +
   1.584 +	if (((source & 0xe000000) == 0) ||
   1.585 +	    ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)
   1.586 +		return;
   1.587 +
   1.588 +	u8 treeSize = CPUReadByte(source++);
   1.589 +
   1.590 +	u32 treeStart = source;
   1.591 +
   1.592 +	source += (treeSize<<1) + 1;
   1.593 +
   1.594 +	int len = header >> 8;
   1.595 +
   1.596 +	u32 mask = 0x80000000;
   1.597 +	u32 data = CPUReadMemory(source);
   1.598 +	source += 4;
   1.599 +
   1.600 +	int  pos         = 0;
   1.601 +	u8   rootNode    = CPUReadByte(treeStart);
   1.602 +	u8   currentNode = rootNode;
   1.603 +	bool writeData   = false;
   1.604 +	int  byteShift   = 0;
   1.605 +	int  byteCount   = 0;
   1.606 +	u32  writeValue  = 0;
   1.607 +
   1.608 +	if ((header & 0x0F) == 8)
   1.609 +	{
   1.610 +		while (len > 0)
   1.611 +		{
   1.612 +			// take left
   1.613 +			if (pos == 0)
   1.614 +				pos++;
   1.615 +			else
   1.616 +				pos += (((currentNode & 0x3F)+1)<<1);
   1.617 +
   1.618 +			if (data & mask)
   1.619 +			{
   1.620 +				// right
   1.621 +				if (currentNode & 0x40)
   1.622 +					writeData = true;
   1.623 +				currentNode = CPUReadByte(treeStart+pos+1);
   1.624 +			}
   1.625 +			else
   1.626 +			{
   1.627 +				// left
   1.628 +				if (currentNode & 0x80)
   1.629 +					writeData = true;
   1.630 +				currentNode = CPUReadByte(treeStart+pos);
   1.631 +			}
   1.632 +
   1.633 +			if (writeData)
   1.634 +			{
   1.635 +				writeValue |= (currentNode << byteShift);
   1.636 +				byteCount++;
   1.637 +				byteShift += 8;
   1.638 +
   1.639 +				pos         = 0;
   1.640 +				currentNode = rootNode;
   1.641 +				writeData   = false;
   1.642 +
   1.643 +				if (byteCount == 4)
   1.644 +				{
   1.645 +					byteCount = 0;
   1.646 +					byteShift = 0;
   1.647 +					CPUWriteMemory(dest, writeValue);
   1.648 +					writeValue = 0;
   1.649 +					dest      += 4;
   1.650 +					len       -= 4;
   1.651 +				}
   1.652 +			}
   1.653 +			mask >>= 1;
   1.654 +			if (mask == 0)
   1.655 +			{
   1.656 +				mask    = 0x80000000;
   1.657 +				data    = CPUReadMemory(source);
   1.658 +				source += 4;
   1.659 +			}
   1.660 +		}
   1.661 +	}
   1.662 +	else
   1.663 +	{
   1.664 +		int halfLen = 0;
   1.665 +		int value   = 0;
   1.666 +		while (len > 0)
   1.667 +		{
   1.668 +			// take left
   1.669 +			if (pos == 0)
   1.670 +				pos++;
   1.671 +			else
   1.672 +				pos += (((currentNode & 0x3F)+1)<<1);
   1.673 +
   1.674 +			if ((data & mask))
   1.675 +			{
   1.676 +				// right
   1.677 +				if (currentNode & 0x40)
   1.678 +					writeData = true;
   1.679 +				currentNode = CPUReadByte(treeStart+pos+1);
   1.680 +			}
   1.681 +			else
   1.682 +			{
   1.683 +				// left
   1.684 +				if (currentNode & 0x80)
   1.685 +					writeData = true;
   1.686 +				currentNode = CPUReadByte(treeStart+pos);
   1.687 +			}
   1.688 +
   1.689 +			if (writeData)
   1.690 +			{
   1.691 +				if (halfLen == 0)
   1.692 +					value |= currentNode;
   1.693 +				else
   1.694 +					value |= (currentNode<<4);
   1.695 +
   1.696 +				halfLen += 4;
   1.697 +				if (halfLen == 8)
   1.698 +				{
   1.699 +					writeValue |= (value << byteShift);
   1.700 +					byteCount++;
   1.701 +					byteShift += 8;
   1.702 +
   1.703 +					halfLen = 0;
   1.704 +					value   = 0;
   1.705 +
   1.706 +					if (byteCount == 4)
   1.707 +					{
   1.708 +						byteCount = 0;
   1.709 +						byteShift = 0;
   1.710 +						CPUWriteMemory(dest, writeValue);
   1.711 +						dest      += 4;
   1.712 +						writeValue = 0;
   1.713 +						len       -= 4;
   1.714 +					}
   1.715 +				}
   1.716 +				pos         = 0;
   1.717 +				currentNode = rootNode;
   1.718 +				writeData   = false;
   1.719 +			}
   1.720 +			mask >>= 1;
   1.721 +			if (mask == 0)
   1.722 +			{
   1.723 +				mask    = 0x80000000;
   1.724 +				data    = CPUReadMemory(source);
   1.725 +				source += 4;
   1.726 +			}
   1.727 +		}
   1.728 +	}
   1.729 +}
   1.730 +
   1.731 +void BIOS_LZ77UnCompVram()
   1.732 +{
   1.733 +#ifdef GBA_LOGGING
   1.734 +	if (systemVerbose & VERBOSE_SWI)
   1.735 +	{
   1.736 +		log("LZ77UnCompVram: 0x%08x,0x%08x (VCOUNT=%d)\n",
   1.737 +		    reg[0].I,
   1.738 +		    reg[1].I,
   1.739 +		    VCOUNT);
   1.740 +	}
   1.741 +#endif
   1.742 +
   1.743 +	u32 source = reg[0].I;
   1.744 +	u32 dest   = reg[1].I;
   1.745 +
   1.746 +	u32 header = CPUReadMemory(source);
   1.747 +	source += 4;
   1.748 +
   1.749 +	if (((source & 0xe000000) == 0) ||
   1.750 +	    ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)
   1.751 +		return;
   1.752 +
   1.753 +	int byteCount  = 0;
   1.754 +	int byteShift  = 0;
   1.755 +	u32 writeValue = 0;
   1.756 +
   1.757 +	int len = header >> 8;
   1.758 +
   1.759 +	while (len > 0)
   1.760 +	{
   1.761 +		u8 d = CPUReadByte(source++);
   1.762 +
   1.763 +		if (d)
   1.764 +		{
   1.765 +			for (int i = 0; i < 8; i++)
   1.766 +			{
   1.767 +				if (d & 0x80)
   1.768 +				{
   1.769 +					u16 data = CPUReadByte(source++) << 8;
   1.770 +					data |= CPUReadByte(source++);
   1.771 +					int length       = (data >> 12) + 3;
   1.772 +					int offset       = (data & 0x0FFF);
   1.773 +					u32 windowOffset = dest + byteCount - offset - 1;
   1.774 +					for (int i = 0; i < length; i++)
   1.775 +					{
   1.776 +						writeValue |= (CPUReadByte(windowOffset++) << byteShift);
   1.777 +						byteShift  += 8;
   1.778 +						byteCount++;
   1.779 +
   1.780 +						if (byteCount == 2)
   1.781 +						{
   1.782 +							CPUWriteHalfWord(dest, writeValue);
   1.783 +							dest      += 2;
   1.784 +							byteCount  = 0;
   1.785 +							byteShift  = 0;
   1.786 +							writeValue = 0;
   1.787 +						}
   1.788 +						len--;
   1.789 +						if (len == 0)
   1.790 +							return;
   1.791 +					}
   1.792 +				}
   1.793 +				else
   1.794 +				{
   1.795 +					writeValue |= (CPUReadByte(source++) << byteShift);
   1.796 +					byteShift  += 8;
   1.797 +					byteCount++;
   1.798 +					if (byteCount == 2)
   1.799 +					{
   1.800 +						CPUWriteHalfWord(dest, writeValue);
   1.801 +						dest      += 2;
   1.802 +						byteCount  = 0;
   1.803 +						byteShift  = 0;
   1.804 +						writeValue = 0;
   1.805 +					}
   1.806 +					len--;
   1.807 +					if (len == 0)
   1.808 +						return;
   1.809 +				}
   1.810 +				d <<= 1;
   1.811 +			}
   1.812 +		}
   1.813 +		else
   1.814 +		{
   1.815 +			for (int i = 0; i < 8; i++)
   1.816 +			{
   1.817 +				writeValue |= (CPUReadByte(source++) << byteShift);
   1.818 +				byteShift  += 8;
   1.819 +				byteCount++;
   1.820 +				if (byteCount == 2)
   1.821 +				{
   1.822 +					CPUWriteHalfWord(dest, writeValue);
   1.823 +					dest      += 2;
   1.824 +					byteShift  = 0;
   1.825 +					byteCount  = 0;
   1.826 +					writeValue = 0;
   1.827 +				}
   1.828 +				len--;
   1.829 +				if (len == 0)
   1.830 +					return;
   1.831 +			}
   1.832 +		}
   1.833 +	}
   1.834 +}
   1.835 +
   1.836 +void BIOS_LZ77UnCompWram()
   1.837 +{
   1.838 +#ifdef GBA_LOGGING
   1.839 +	if (systemVerbose & VERBOSE_SWI)
   1.840 +	{
   1.841 +		log("LZ77UnCompWram: 0x%08x,0x%08x (VCOUNT=%d)\n", reg[0].I, reg[1].I,
   1.842 +		    VCOUNT);
   1.843 +	}
   1.844 +#endif
   1.845 +
   1.846 +	u32 source = reg[0].I;
   1.847 +	u32 dest   = reg[1].I;
   1.848 +
   1.849 +	u32 header = CPUReadMemory(source);
   1.850 +	source += 4;
   1.851 +
   1.852 +	if (((source & 0xe000000) == 0) ||
   1.853 +	    ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)
   1.854 +		return;
   1.855 +
   1.856 +	int len = header >> 8;
   1.857 +
   1.858 +	while (len > 0)
   1.859 +	{
   1.860 +		u8 d = CPUReadByte(source++);
   1.861 +
   1.862 +		if (d)
   1.863 +		{
   1.864 +			for (int i = 0; i < 8; i++)
   1.865 +			{
   1.866 +				if (d & 0x80)
   1.867 +				{
   1.868 +					u16 data = CPUReadByte(source++) << 8;
   1.869 +					data |= CPUReadByte(source++);
   1.870 +					int length       = (data >> 12) + 3;
   1.871 +					int offset       = (data & 0x0FFF);
   1.872 +					u32 windowOffset = dest - offset - 1;
   1.873 +					for (int i = 0; i < length; i++)
   1.874 +					{
   1.875 +						CPUWriteByte(dest++, CPUReadByte(windowOffset++));
   1.876 +						len--;
   1.877 +						if (len == 0)
   1.878 +							return;
   1.879 +					}
   1.880 +				}
   1.881 +				else
   1.882 +				{
   1.883 +					CPUWriteByte(dest++, CPUReadByte(source++));
   1.884 +					len--;
   1.885 +					if (len == 0)
   1.886 +						return;
   1.887 +				}
   1.888 +				d <<= 1;
   1.889 +			}
   1.890 +		}
   1.891 +		else
   1.892 +		{
   1.893 +			for (int i = 0; i < 8; i++)
   1.894 +			{
   1.895 +				CPUWriteByte(dest++, CPUReadByte(source++));
   1.896 +				len--;
   1.897 +				if (len == 0)
   1.898 +					return;
   1.899 +			}
   1.900 +		}
   1.901 +	}
   1.902 +}
   1.903 +
   1.904 +void BIOS_ObjAffineSet()
   1.905 +{
   1.906 +#ifdef GBA_LOGGING
   1.907 +	if (systemVerbose & VERBOSE_SWI)
   1.908 +	{
   1.909 +		log("ObjAffineSet: 0x%08x,0x%08x,0x%08x,0x%08x (VCOUNT=%d)\n",
   1.910 +		    reg[0].I,
   1.911 +		    reg[1].I,
   1.912 +		    reg[2].I,
   1.913 +		    reg[3].I,
   1.914 +		    VCOUNT);
   1.915 +	}
   1.916 +#endif
   1.917 +
   1.918 +	u32 src    = reg[0].I;
   1.919 +	u32 dest   = reg[1].I;
   1.920 +	int num    = reg[2].I;
   1.921 +	int offset = reg[3].I;
   1.922 +
   1.923 +	for (int i = 0; i < num; i++)
   1.924 +	{
   1.925 +		s16 rx = CPUReadHalfWord(src);
   1.926 +		src += 2;
   1.927 +		s16 ry = CPUReadHalfWord(src);
   1.928 +		src += 2;
   1.929 +		u16 theta = CPUReadHalfWord(src)>>8;
   1.930 +		src += 4; // keep structure alignment
   1.931 +
   1.932 +		s32 a = (s32)sineTable[(theta+0x40)&255];
   1.933 +		s32 b = (s32)sineTable[theta];
   1.934 +
   1.935 +		s16 dx  =  (s16)((rx * a)>>14);
   1.936 +		s16 dmx = (s16)((rx * b)>>14);
   1.937 +		s16 dy  =  (s16)((ry * b)>>14);
   1.938 +		s16 dmy = (s16)((ry * a)>>14);
   1.939 +
   1.940 +		CPUWriteHalfWord(dest, dx);
   1.941 +		dest += offset;
   1.942 +		CPUWriteHalfWord(dest, -dmx);
   1.943 +		dest += offset;
   1.944 +		CPUWriteHalfWord(dest, dy);
   1.945 +		dest += offset;
   1.946 +		CPUWriteHalfWord(dest, dmy);
   1.947 +		dest += offset;
   1.948 +	}
   1.949 +}
   1.950 +
   1.951 +void BIOS_RegisterRamReset(u32 flags)
   1.952 +{
   1.953 +	// no need to trace here. this is only called directly from GBA.cpp
   1.954 +	// to emulate bios initialization
   1.955 +
   1.956 +	if (flags)
   1.957 +	{
   1.958 +		if (flags & 0x01)
   1.959 +		{
   1.960 +			// clear work RAM
   1.961 +			memset(workRAM, 0, 0x40000);
   1.962 +		}
   1.963 +		if (flags & 0x02)
   1.964 +		{
   1.965 +			// clear internal RAM
   1.966 +			memset(internalRAM, 0, 0x7e00); // don't clear 0x7e00-0x7fff
   1.967 +		}
   1.968 +		if (flags & 0x04)
   1.969 +		{
   1.970 +			// clear palette RAM
   1.971 +			memset(paletteRAM, 0, 0x400);
   1.972 +		}
   1.973 +		if (flags & 0x08)
   1.974 +		{
   1.975 +			// clear VRAM
   1.976 +			memset(vram, 0, 0x18000);
   1.977 +		}
   1.978 +		if (flags & 0x10)
   1.979 +		{
   1.980 +			// clean OAM
   1.981 +			memset(oam, 0, 0x400);
   1.982 +		}
   1.983 +
   1.984 +		if (flags & 0x80)
   1.985 +		{
   1.986 +			int i;
   1.987 +			for (i = 0; i < 8; i++)
   1.988 +				CPUUpdateRegister(0x200+i*2, 0);
   1.989 +
   1.990 +			CPUUpdateRegister(0x202, 0xFFFF);
   1.991 +
   1.992 +			for (i = 0; i < 8; i++)
   1.993 +				CPUUpdateRegister(0x4+i*2, 0);
   1.994 +
   1.995 +			for (i = 0; i < 16; i++)
   1.996 +				CPUUpdateRegister(0x20+i*2, 0);
   1.997 +
   1.998 +			for (i = 0; i < 24; i++)
   1.999 +				CPUUpdateRegister(0xb0+i*2, 0);
  1.1000 +
  1.1001 +			CPUUpdateRegister(0x130, 0);
  1.1002 +			CPUUpdateRegister(0x20, 0x100);
  1.1003 +			CPUUpdateRegister(0x30, 0x100);
  1.1004 +			CPUUpdateRegister(0x26, 0x100);
  1.1005 +			CPUUpdateRegister(0x36, 0x100);
  1.1006 +		}
  1.1007 +
  1.1008 +		if (flags & 0x20)
  1.1009 +		{
  1.1010 +			int i;
  1.1011 +			for (i = 0; i < 8; i++)
  1.1012 +				CPUUpdateRegister(0x110+i*2, 0);
  1.1013 +			CPUUpdateRegister(0x134, 0x8000);
  1.1014 +			for (i = 0; i < 7; i++)
  1.1015 +				CPUUpdateRegister(0x140+i*2, 0);
  1.1016 +		}
  1.1017 +
  1.1018 +		if (flags & 0x40)
  1.1019 +		{
  1.1020 +			int i;
  1.1021 +			CPUWriteByte(0x4000084, 0);
  1.1022 +			CPUWriteByte(0x4000084, 0x80);
  1.1023 +			CPUWriteMemory(0x4000080, 0x880e0000);
  1.1024 +			CPUUpdateRegister(0x88, CPUReadHalfWord(0x4000088)&0x3ff);
  1.1025 +			CPUWriteByte(0x4000070, 0x70);
  1.1026 +			for (i = 0; i < 8; i++)
  1.1027 +				CPUUpdateRegister(0x90+i*2, 0);
  1.1028 +			CPUWriteByte(0x4000070, 0);
  1.1029 +			for (i = 0; i < 8; i++)
  1.1030 +				CPUUpdateRegister(0x90+i*2, 0);
  1.1031 +			CPUWriteByte(0x4000084, 0);
  1.1032 +		}
  1.1033 +	}
  1.1034 +}
  1.1035 +
  1.1036 +void BIOS_RegisterRamReset()
  1.1037 +{
  1.1038 +#ifdef GBA_LOGGING
  1.1039 +	if (systemVerbose & VERBOSE_SWI)
  1.1040 +	{
  1.1041 +		log("RegisterRamReset: 0x%08x (VCOUNT=%d)\n",
  1.1042 +		    reg[0].I,
  1.1043 +		    VCOUNT);
  1.1044 +	}
  1.1045 +#endif
  1.1046 +
  1.1047 +	BIOS_RegisterRamReset(reg[0].I);
  1.1048 +}
  1.1049 +
  1.1050 +void BIOS_RLUnCompVram()
  1.1051 +{
  1.1052 +#ifdef GBA_LOGGING
  1.1053 +	if (systemVerbose & VERBOSE_SWI)
  1.1054 +	{
  1.1055 +		log("RLUnCompVram: 0x%08x,0x%08x (VCOUNT=%d)\n",
  1.1056 +		    reg[0].I,
  1.1057 +		    reg[1].I,
  1.1058 +		    VCOUNT);
  1.1059 +	}
  1.1060 +#endif
  1.1061 +
  1.1062 +	u32 source = reg[0].I;
  1.1063 +	u32 dest   = reg[1].I;
  1.1064 +
  1.1065 +	u32 header = CPUReadMemory(source);
  1.1066 +	source += 4;
  1.1067 +
  1.1068 +	if (((source & 0xe000000) == 0) ||
  1.1069 +	    ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)
  1.1070 +		return;
  1.1071 +
  1.1072 +	int len        = header >> 8;
  1.1073 +	int byteCount  = 0;
  1.1074 +	int byteShift  = 0;
  1.1075 +	u32 writeValue = 0;
  1.1076 +
  1.1077 +	while (len > 0)
  1.1078 +	{
  1.1079 +		u8  d = CPUReadByte(source++);
  1.1080 +		int l = d & 0x7F;
  1.1081 +		if (d & 0x80)
  1.1082 +		{
  1.1083 +			u8 data = CPUReadByte(source++);
  1.1084 +			l += 3;
  1.1085 +			for (int i = 0; i < l; i++)
  1.1086 +			{
  1.1087 +				writeValue |= (data << byteShift);
  1.1088 +				byteShift  += 8;
  1.1089 +				byteCount++;
  1.1090 +
  1.1091 +				if (byteCount == 2)
  1.1092 +				{
  1.1093 +					CPUWriteHalfWord(dest, writeValue);
  1.1094 +					dest      += 2;
  1.1095 +					byteCount  = 0;
  1.1096 +					byteShift  = 0;
  1.1097 +					writeValue = 0;
  1.1098 +				}
  1.1099 +				len--;
  1.1100 +				if (len == 0)
  1.1101 +					return;
  1.1102 +			}
  1.1103 +		}
  1.1104 +		else
  1.1105 +		{
  1.1106 +			l++;
  1.1107 +			for (int i = 0; i < l; i++)
  1.1108 +			{
  1.1109 +				writeValue |= (CPUReadByte(source++) << byteShift);
  1.1110 +				byteShift  += 8;
  1.1111 +				byteCount++;
  1.1112 +				if (byteCount == 2)
  1.1113 +				{
  1.1114 +					CPUWriteHalfWord(dest, writeValue);
  1.1115 +					dest      += 2;
  1.1116 +					byteCount  = 0;
  1.1117 +					byteShift  = 0;
  1.1118 +					writeValue = 0;
  1.1119 +				}
  1.1120 +				len--;
  1.1121 +				if (len == 0)
  1.1122 +					return;
  1.1123 +			}
  1.1124 +		}
  1.1125 +	}
  1.1126 +}
  1.1127 +
  1.1128 +void BIOS_RLUnCompWram()
  1.1129 +{
  1.1130 +#ifdef GBA_LOGGING
  1.1131 +	if (systemVerbose & VERBOSE_SWI)
  1.1132 +	{
  1.1133 +		log("RLUnCompWram: 0x%08x,0x%08x (VCOUNT=%d)\n",
  1.1134 +		    reg[0].I,
  1.1135 +		    reg[1].I,
  1.1136 +		    VCOUNT);
  1.1137 +	}
  1.1138 +#endif
  1.1139 +
  1.1140 +	u32 source = reg[0].I;
  1.1141 +	u32 dest   = reg[1].I;
  1.1142 +
  1.1143 +	u32 header = CPUReadMemory(source);
  1.1144 +	source += 4;
  1.1145 +
  1.1146 +	if (((source & 0xe000000) == 0) ||
  1.1147 +	    ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)
  1.1148 +		return;
  1.1149 +
  1.1150 +	int len = header >> 8;
  1.1151 +
  1.1152 +	while (len > 0)
  1.1153 +	{
  1.1154 +		u8  d = CPUReadByte(source++);
  1.1155 +		int l = d & 0x7F;
  1.1156 +		if (d & 0x80)
  1.1157 +		{
  1.1158 +			u8 data = CPUReadByte(source++);
  1.1159 +			l += 3;
  1.1160 +			for (int i = 0; i < l; i++)
  1.1161 +			{
  1.1162 +				CPUWriteByte(dest++, data);
  1.1163 +				len--;
  1.1164 +				if (len == 0)
  1.1165 +					return;
  1.1166 +			}
  1.1167 +		}
  1.1168 +		else
  1.1169 +		{
  1.1170 +			l++;
  1.1171 +			for (int i = 0; i < l; i++)
  1.1172 +			{
  1.1173 +				CPUWriteByte(dest++,  CPUReadByte(source++));
  1.1174 +				len--;
  1.1175 +				if (len == 0)
  1.1176 +					return;
  1.1177 +			}
  1.1178 +		}
  1.1179 +	}
  1.1180 +}
  1.1181 +
  1.1182 +void BIOS_SoftReset()
  1.1183 +{
  1.1184 +#ifdef GBA_LOGGING
  1.1185 +	if (systemVerbose & VERBOSE_SWI)
  1.1186 +	{
  1.1187 +		log("SoftReset: (VCOUNT=%d)\n", VCOUNT);
  1.1188 +	}
  1.1189 +#endif
  1.1190 +
  1.1191 +	armState        = true;
  1.1192 +	armMode         = 0x1F;
  1.1193 +	armIrqEnable    = false;
  1.1194 +	C_FLAG          = V_FLAG = N_FLAG = Z_FLAG = false;
  1.1195 +	reg[13].I       = 0x03007F00;
  1.1196 +	reg[14].I       = 0x00000000;
  1.1197 +	reg[16].I       = 0x00000000;
  1.1198 +	reg[R13_IRQ].I  = 0x03007FA0;
  1.1199 +	reg[R14_IRQ].I  = 0x00000000;
  1.1200 +	reg[SPSR_IRQ].I = 0x00000000;
  1.1201 +	reg[R13_SVC].I  = 0x03007FE0;
  1.1202 +	reg[R14_SVC].I  = 0x00000000;
  1.1203 +	reg[SPSR_SVC].I = 0x00000000;
  1.1204 +	u8 b = internalRAM[0x7ffa];
  1.1205 +
  1.1206 +	memset(&internalRAM[0x7e00], 0, 0x200);
  1.1207 +
  1.1208 +	if (b)
  1.1209 +	{
  1.1210 +		armNextPC = 0x02000000;
  1.1211 +		reg[15].I = 0x02000004;
  1.1212 +	}
  1.1213 +	else
  1.1214 +	{
  1.1215 +		armNextPC = 0x08000000;
  1.1216 +		reg[15].I = 0x08000004;
  1.1217 +	}
  1.1218 +}
  1.1219 +
  1.1220 +void BIOS_Sqrt()
  1.1221 +{
  1.1222 +#ifdef GBA_LOGGING
  1.1223 +	if (systemVerbose & VERBOSE_SWI)
  1.1224 +	{
  1.1225 +		log("Sqrt: %08x (VCOUNT=%2d)\n",
  1.1226 +		    reg[0].I,
  1.1227 +		    VCOUNT);
  1.1228 +	}
  1.1229 +#endif
  1.1230 +	reg[0].I = (u32)sqrt((double)reg[0].I);
  1.1231 +#ifdef GBA_LOGGING
  1.1232 +	if (systemVerbose & VERBOSE_SWI)
  1.1233 +	{
  1.1234 +		log("Sqrt: return=%08x\n",
  1.1235 +		    reg[0].I);
  1.1236 +	}
  1.1237 +#endif
  1.1238 +}
  1.1239 +
  1.1240 +void BIOS_MidiKey2Freq()
  1.1241 +{
  1.1242 +#ifdef GBA_LOGGING
  1.1243 +	if (systemVerbose & VERBOSE_SWI)
  1.1244 +	{
  1.1245 +		log("MidiKey2Freq: WaveData=%08x mk=%08x fp=%08x\n",
  1.1246 +		    reg[0].I,
  1.1247 +		    reg[1].I,
  1.1248 +		    reg[2].I);
  1.1249 +	}
  1.1250 +#endif
  1.1251 +	int    freq = CPUReadMemory(reg[0].I+4);
  1.1252 +	double tmp;
  1.1253 +	tmp      = ((double)(180 - reg[1].I)) - ((double)reg[2].I / 256.f);
  1.1254 +	tmp      = pow((double)2.f, tmp / 12.f);
  1.1255 +	reg[0].I = (int)((double)freq / tmp);
  1.1256 +
  1.1257 +#ifdef GBA_LOGGING
  1.1258 +	if (systemVerbose & VERBOSE_SWI)
  1.1259 +	{
  1.1260 +		log("MidiKey2Freq: return %08x\n",
  1.1261 +		    reg[0].I);
  1.1262 +	}
  1.1263 +#endif
  1.1264 +}
  1.1265 +
  1.1266 +void BIOS_SndDriverJmpTableCopy()
  1.1267 +{
  1.1268 +#ifdef GBA_LOGGING
  1.1269 +	if (systemVerbose & VERBOSE_SWI)
  1.1270 +	{
  1.1271 +		log("SndDriverJmpTableCopy: dest=%08x\n",
  1.1272 +		    reg[0].I);
  1.1273 +	}
  1.1274 +#endif
  1.1275 +	for (int i = 0; i < 0x24; i++)
  1.1276 +	{
  1.1277 +		CPUWriteMemory(reg[0].I, 0x9c);
  1.1278 +		reg[0].I += 4;
  1.1279 +	}
  1.1280 +}
  1.1281 +