Mercurial > vba-clojure

     1 #include <cmath>

     2 #include <cstdlib>

     3 #include <cstring>

     4 

     5 #include "bios.h"

     6 #include "../common/System.h"

     7 #include "GBA.h"

     8 #include "GBACheats.h" // FIXME: SDL build requires this

     9 #include "GBAinline.h"

    10 #include "GBAGlobals.h"

    11 

    12 s16 sineTable[256] = {

    13 	(s16)0x0000, (s16)0x0192, (s16)0x0323, (s16)0x04B5, (s16)0x0645, (s16)0x07D5, (s16)0x0964, (s16)0x0AF1,

    14 	(s16)0x0C7C, (s16)0x0E05, (s16)0x0F8C, (s16)0x1111, (s16)0x1294, (s16)0x1413, (s16)0x158F, (s16)0x1708,

    15 	(s16)0x187D, (s16)0x19EF, (s16)0x1B5D, (s16)0x1CC6, (s16)0x1E2B, (s16)0x1F8B, (s16)0x20E7, (s16)0x223D,

    16 	(s16)0x238E, (s16)0x24DA, (s16)0x261F, (s16)0x275F, (s16)0x2899, (s16)0x29CD, (s16)0x2AFA, (s16)0x2C21,

    17 	(s16)0x2D41, (s16)0x2E5A, (s16)0x2F6B, (s16)0x3076, (s16)0x3179, (s16)0x3274, (s16)0x3367, (s16)0x3453,

    18 	(s16)0x3536, (s16)0x3612, (s16)0x36E5, (s16)0x37AF, (s16)0x3871, (s16)0x392A, (s16)0x39DA, (s16)0x3A82,

    19 	(s16)0x3B20, (s16)0x3BB6, (s16)0x3C42, (s16)0x3CC5, (s16)0x3D3E, (s16)0x3DAE, (s16)0x3E14, (s16)0x3E71,

    20 	(s16)0x3EC5, (s16)0x3F0E, (s16)0x3F4E, (s16)0x3F84, (s16)0x3FB1, (s16)0x3FD3, (s16)0x3FEC, (s16)0x3FFB,

    21 	(s16)0x4000, (s16)0x3FFB, (s16)0x3FEC, (s16)0x3FD3, (s16)0x3FB1, (s16)0x3F84, (s16)0x3F4E, (s16)0x3F0E,

    22 	(s16)0x3EC5, (s16)0x3E71, (s16)0x3E14, (s16)0x3DAE, (s16)0x3D3E, (s16)0x3CC5, (s16)0x3C42, (s16)0x3BB6,

    23 	(s16)0x3B20, (s16)0x3A82, (s16)0x39DA, (s16)0x392A, (s16)0x3871, (s16)0x37AF, (s16)0x36E5, (s16)0x3612,

    24 	(s16)0x3536, (s16)0x3453, (s16)0x3367, (s16)0x3274, (s16)0x3179, (s16)0x3076, (s16)0x2F6B, (s16)0x2E5A,

    25 	(s16)0x2D41, (s16)0x2C21, (s16)0x2AFA, (s16)0x29CD, (s16)0x2899, (s16)0x275F, (s16)0x261F, (s16)0x24DA,

    26 	(s16)0x238E, (s16)0x223D, (s16)0x20E7, (s16)0x1F8B, (s16)0x1E2B, (s16)0x1CC6, (s16)0x1B5D, (s16)0x19EF,

    27 	(s16)0x187D, (s16)0x1708, (s16)0x158F, (s16)0x1413, (s16)0x1294, (s16)0x1111, (s16)0x0F8C, (s16)0x0E05,

    28 	(s16)0x0C7C, (s16)0x0AF1, (s16)0x0964, (s16)0x07D5, (s16)0x0645, (s16)0x04B5, (s16)0x0323, (s16)0x0192,

    29 	(s16)0x0000, (s16)0xFE6E, (s16)0xFCDD, (s16)0xFB4B, (s16)0xF9BB, (s16)0xF82B, (s16)0xF69C, (s16)0xF50F,

    30 	(s16)0xF384, (s16)0xF1FB, (s16)0xF074, (s16)0xEEEF, (s16)0xED6C, (s16)0xEBED, (s16)0xEA71, (s16)0xE8F8,

    31 	(s16)0xE783, (s16)0xE611, (s16)0xE4A3, (s16)0xE33A, (s16)0xE1D5, (s16)0xE075, (s16)0xDF19, (s16)0xDDC3,

    32 	(s16)0xDC72, (s16)0xDB26, (s16)0xD9E1, (s16)0xD8A1, (s16)0xD767, (s16)0xD633, (s16)0xD506, (s16)0xD3DF,

    33 	(s16)0xD2BF, (s16)0xD1A6, (s16)0xD095, (s16)0xCF8A, (s16)0xCE87, (s16)0xCD8C, (s16)0xCC99, (s16)0xCBAD,

    34 	(s16)0xCACA, (s16)0xC9EE, (s16)0xC91B, (s16)0xC851, (s16)0xC78F, (s16)0xC6D6, (s16)0xC626, (s16)0xC57E,

    35 	(s16)0xC4E0, (s16)0xC44A, (s16)0xC3BE, (s16)0xC33B, (s16)0xC2C2, (s16)0xC252, (s16)0xC1EC, (s16)0xC18F,

    36 	(s16)0xC13B, (s16)0xC0F2, (s16)0xC0B2, (s16)0xC07C, (s16)0xC04F, (s16)0xC02D, (s16)0xC014, (s16)0xC005,

    37 	(s16)0xC000, (s16)0xC005, (s16)0xC014, (s16)0xC02D, (s16)0xC04F, (s16)0xC07C, (s16)0xC0B2, (s16)0xC0F2,

    38 	(s16)0xC13B, (s16)0xC18F, (s16)0xC1EC, (s16)0xC252, (s16)0xC2C2, (s16)0xC33B, (s16)0xC3BE, (s16)0xC44A,

    39 	(s16)0xC4E0, (s16)0xC57E, (s16)0xC626, (s16)0xC6D6, (s16)0xC78F, (s16)0xC851, (s16)0xC91B, (s16)0xC9EE,

    40 	(s16)0xCACA, (s16)0xCBAD, (s16)0xCC99, (s16)0xCD8C, (s16)0xCE87, (s16)0xCF8A, (s16)0xD095, (s16)0xD1A6,

    41 	(s16)0xD2BF, (s16)0xD3DF, (s16)0xD506, (s16)0xD633, (s16)0xD767, (s16)0xD8A1, (s16)0xD9E1, (s16)0xDB26,

    42 	(s16)0xDC72, (s16)0xDDC3, (s16)0xDF19, (s16)0xE075, (s16)0xE1D5, (s16)0xE33A, (s16)0xE4A3, (s16)0xE611,

    43 	(s16)0xE783, (s16)0xE8F8, (s16)0xEA71, (s16)0xEBED, (s16)0xED6C, (s16)0xEEEF, (s16)0xF074, (s16)0xF1FB,

    44 	(s16)0xF384, (s16)0xF50F, (s16)0xF69C, (s16)0xF82B, (s16)0xF9BB, (s16)0xFB4B, (s16)0xFCDD, (s16)0xFE6E

    45 };

    46 

    47 void BIOS_ArcTan()

    48 {

    49 #ifdef GBA_LOGGING

    50 	if (systemVerbose & VERBOSE_SWI)

    51 	{

    52 		log("ArcTan: %08x (VCOUNT=%2d)\n",

    53 		    reg[0].I,

    54 		    VCOUNT);

    55 	}

    56 #endif

    57 

    58 	s32 a = -((s32)(reg[0].I * reg[0].I)) >> 14;

    59 	s32 b = ((0xA9 * a) >> 14) + 0x390;

    60 	b        = ((b * a) >> 14) + 0x91C;

    61 	b        = ((b * a) >> 14) + 0xFB6;

    62 	b        = ((b * a) >> 14) + 0x16AA;

    63 	b        = ((b * a) >> 14) + 0x2081;

    64 	b        = ((b * a) >> 14) + 0x3651;

    65 	b        = ((b * a) >> 14) + 0xA2F9;

    66 	reg[0].I = (reg[0].I * b) >> 16;

    67 

    68 #ifdef GBA_LOGGING

    69 	if (systemVerbose & VERBOSE_SWI)

    70 	{

    71 		log("ArcTan: return=%08x\n",

    72 		    reg[0].I);

    73 	}

    74 #endif

    75 }

    76 

    77 void BIOS_ArcTan2()

    78 {

    79 #ifdef GBA_LOGGING

    80 	if (systemVerbose & VERBOSE_SWI)

    81 	{

    82 		log("ArcTan2: %08x,%08x (VCOUNT=%2d)\n",

    83 		    reg[0].I,

    84 		    reg[1].I,

    85 		    VCOUNT);

    86 	}

    87 #endif

    88 

    89 	s16 x = reg[0].I;

    90 	s16 y = reg[1].I;

    91 

    92 	if (y == 0)

    93 	{

    94 		reg[0].I = 0x8000 & x;

    95 		reg[3].I = 0x170;

    96 	}

    97 	else

    98 	{

    99 		if (x == 0)

   100 		{

   101 			reg[0].I = (0x8000 & y) + 0x4000;

   102 			reg[3].I = 0x170;

   103 		}

   104 		else

   105 		{

   106 			if (abs(x) > abs(y))

   107 			{

   108 				reg[1].I = x;

   109 				reg[0].I = y << 14;

   110 				BIOS_Div();

   111 				BIOS_ArcTan();

   112 				if (x < 0)

   113 					reg[0].I = 0x8000 + reg[0].I;

   114 				else

   115 					reg[0].I = ((y & 0x8000) << 1) + reg[0].I;

   116 				reg[3].I = 0x170;

   117 			}

   118 			else

   119 			{

   120 				reg[0].I = x << 14;

   121 				BIOS_Div();

   122 				BIOS_ArcTan();

   123 				reg[0].I = (0x4000 + (y & 0x8000)) - reg[0].I;

   124 				reg[3].I = 0x170;

   125 			}

   126 		}

   127 	}

   128 

   129 #ifdef GBA_LOGGING

   130 	if (systemVerbose & VERBOSE_SWI)

   131 	{

   132 		log("ArcTan2: return=%08x\n",

   133 		    reg[0].I);

   134 	}

   135 #endif

   136 }

   137 

   138 void BIOS_BitUnPack()

   139 {

   140 #ifdef GBA_LOGGING

   141 	if (systemVerbose & VERBOSE_SWI)

   142 	{

   143 		log("BitUnPack: %08x,%08x,%08x (VCOUNT=%2d)\n",

   144 		    reg[0].I,

   145 		    reg[1].I,

   146 		    reg[2].I,

   147 		    VCOUNT);

   148 	}

   149 #endif

   150 

   151 	u32 source = reg[0].I;

   152 	u32 dest   = reg[1].I;

   153 	u32 header = reg[2].I;

   154 

   155 	int len = CPUReadHalfWord(header);

   156 	// check address

   157 	int bits    = CPUReadByte(header+2);

   158 	int revbits = 8 - bits;

   159 	// u32 value = 0;

   160 	u32  base    = CPUReadMemory(header+4);

   161 	bool addBase = (base & 0x80000000) ? true : false;

   162 	base &= 0x7fffffff;

   163 	int dataSize = CPUReadByte(header+3);

   164 

   165 	int data = 0;

   166 	int bitwritecount = 0;

   167 	while (1)

   168 	{

   169 		len -= 1;

   170 		if (len < 0)

   171 			break;

   172 		int mask = 0xff >> revbits;

   173 		u8  b    = CPUReadByte(source);

   174 		source++;

   175 		int bitcount = 0;

   176 		while (1)

   177 		{

   178 			if (bitcount >= 8)

   179 				break;

   180 			u32 d    = b & mask;

   181 			u32 temp = d >> bitcount;

   182 			if (!temp && addBase)

   183 			{

   184 				temp += base;

   185 			}

   186 			data |= temp << bitwritecount;

   187 			bitwritecount += dataSize;

   188 			if (bitwritecount >= 32)

   189 			{

   190 				CPUWriteMemory(dest, data);

   191 				dest += 4;

   192 				data  = 0;

   193 				bitwritecount = 0;

   194 			}

   195 			mask    <<= bits;

   196 			bitcount += bits;

   197 		}

   198 	}

   199 }

   200 

   201 void BIOS_BgAffineSet()

   202 {

   203 #ifdef GBA_LOGGING

   204 	if (systemVerbose & VERBOSE_SWI)

   205 	{

   206 		log("BgAffineSet: %08x,%08x,%08x (VCOUNT=%2d)\n",

   207 		    reg[0].I,

   208 		    reg[1].I,

   209 		    reg[2].I,

   210 		    VCOUNT);

   211 	}

   212 #endif

   213 

   214 	u32 src  = reg[0].I;

   215 	u32 dest = reg[1].I;

   216 	int num  = reg[2].I;

   217 

   218 	for (int i = 0; i < num; i++)

   219 	{

   220 		s32 cx = CPUReadMemory(src);

   221 		src += 4;

   222 		s32 cy = CPUReadMemory(src);

   223 		src += 4;

   224 		s16 dispx = CPUReadHalfWord(src);

   225 		src += 2;

   226 		s16 dispy = CPUReadHalfWord(src);

   227 		src += 2;

   228 		s16 rx = CPUReadHalfWord(src);

   229 		src += 2;

   230 		s16 ry = CPUReadHalfWord(src);

   231 		src += 2;

   232 		u16 theta = CPUReadHalfWord(src)>>8;

   233 		src += 4; // keep structure alignment

   234 		s32 a = (s32)sineTable[(theta+0x40)&255];

   235 		s32 b = (s32)sineTable[theta];

   236 

   237 		s16 dx  =  (s16)((rx * a)>>14);

   238 		s16 dmx = (s16)((rx * b)>>14);

   239 		s16 dy  =  (s16)((ry * b)>>14);

   240 		s16 dmy = (s16)((ry * a)>>14);

   241 

   242 		CPUWriteHalfWord(dest, dx);

   243 		dest += 2;

   244 		CPUWriteHalfWord(dest, -dmx);

   245 		dest += 2;

   246 		CPUWriteHalfWord(dest, dy);

   247 		dest += 2;

   248 		CPUWriteHalfWord(dest, dmy);

   249 		dest += 2;

   250 

   251 		s32 startx = cx - dx * dispx + dmx * dispy;

   252 		s32 starty = cy - dy * dispx - dmy * dispy;

   253 

   254 		CPUWriteMemory(dest, startx);

   255 		dest += 4;

   256 		CPUWriteMemory(dest, starty);

   257 		dest += 4;

   258 	}

   259 }

   260 

   261 void BIOS_CpuSet()

   262 {

   263 #ifdef GBA_LOGGING

   264 	if (systemVerbose & VERBOSE_SWI)

   265 	{

   266 		log("CpuSet: 0x%08x,0x%08x,0x%08x (VCOUNT=%d)\n", reg[0].I, reg[1].I,

   267 		    reg[2].I, VCOUNT);

   268 	}

   269 #endif

   270 

   271 	u32 source = reg[0].I;

   272 	u32 dest   = reg[1].I;

   273 	u32 cnt    = reg[2].I;

   274 

   275 	if (((source & 0xe000000) == 0) ||

   276 	    ((source + (((cnt << 11)>>9) & 0x1fffff)) & 0xe000000) == 0)

   277 		return;

   278 

   279 	int count = cnt & 0x1FFFFF;

   280 

   281 	// 32-bit ?

   282 	if ((cnt >> 26) & 1)

   283 	{

   284 		// needed for 32-bit mode!

   285 		source &= 0xFFFFFFFC;

   286 		dest   &= 0xFFFFFFFC;

   287 		// fill ?

   288 		if ((cnt >> 24) & 1)

   289 		{

   290 			u32 value = CPUReadMemory(source);

   291 			while (count)

   292 			{

   293 				CPUWriteMemory(dest, value);

   294 				dest += 4;

   295 				count--;

   296 			}

   297 		}

   298 		else

   299 		{

   300 			// copy

   301 			while (count)

   302 			{

   303 				CPUWriteMemory(dest, CPUReadMemory(source));

   304 				source += 4;

   305 				dest   += 4;

   306 				count--;

   307 			}

   308 		}

   309 	}

   310 	else

   311 	{

   312 		// 16-bit fill?

   313 		if ((cnt >> 24) & 1)

   314 		{

   315 			u16 value = CPUReadHalfWord(source);

   316 			while (count)

   317 			{

   318 				CPUWriteHalfWord(dest, value);

   319 				dest += 2;

   320 				count--;

   321 			}

   322 		}

   323 		else

   324 		{

   325 			// copy

   326 			while (count)

   327 			{

   328 				CPUWriteHalfWord(dest, CPUReadHalfWord(source));

   329 				source += 2;

   330 				dest   += 2;

   331 				count--;

   332 			}

   333 		}

   334 	}

   335 }

   336 

   337 void BIOS_CpuFastSet()

   338 {

   339 #ifdef GBA_LOGGING

   340 	if (systemVerbose & VERBOSE_SWI)

   341 	{

   342 		log("CpuFastSet: 0x%08x,0x%08x,0x%08x (VCOUNT=%d)\n", reg[0].I, reg[1].I,

   343 		    reg[2].I, VCOUNT);

   344 	}

   345 #endif

   346 

   347 	u32 source = reg[0].I;

   348 	u32 dest   = reg[1].I;

   349 	u32 cnt    = reg[2].I;

   350 

   351 	if (((source & 0xe000000) == 0) ||

   352 	    ((source + (((cnt << 11)>>9) & 0x1fffff)) & 0xe000000) == 0)

   353 		return;

   354 

   355 	// needed for 32-bit mode!

   356 	source &= 0xFFFFFFFC;

   357 	dest   &= 0xFFFFFFFC;

   358 

   359 	int count = cnt & 0x1FFFFF;

   360 

   361 	// fill?

   362 	if ((cnt >> 24) & 1)

   363 	{

   364 		while (count > 0)

   365 		{

   366 			// BIOS always transfers 32 bytes at a time

   367 			u32 value = CPUReadMemory(source);

   368 			for (int i = 0; i < 8; i++)

   369 			{

   370 				CPUWriteMemory(dest, value);

   371 				dest += 4;

   372 			}

   373 			count -= 8;

   374 		}

   375 	}

   376 	else

   377 	{

   378 		// copy

   379 		while (count > 0)

   380 		{

   381 			// BIOS always transfers 32 bytes at a time

   382 			for (int i = 0; i < 8; i++)

   383 			{

   384 				CPUWriteMemory(dest, CPUReadMemory(source));

   385 				source += 4;

   386 				dest   += 4;

   387 			}

   388 			count -= 8;

   389 		}

   390 	}

   391 }

   392 

   393 void BIOS_Diff8bitUnFilterWram()

   394 {

   395 #ifdef GBA_LOGGING

   396 	if (systemVerbose & VERBOSE_SWI)

   397 	{

   398 		log("Diff8bitUnFilterWram: 0x%08x,0x%08x (VCOUNT=%d)\n", reg[0].I,

   399 		    reg[1].I, VCOUNT);

   400 	}

   401 #endif

   402 

   403 	u32 source = reg[0].I;

   404 	u32 dest   = reg[1].I;

   405 

   406 	u32 header = CPUReadMemory(source);

   407 	source += 4;

   408 

   409 	if (((source & 0xe000000) == 0) ||

   410 	    ((source + ((header >> 8) & 0x1fffff) & 0xe000000) == 0))

   411 		return;

   412 

   413 	int len = header >> 8;

   414 

   415 	u8 data = CPUReadByte(source++);

   416 	CPUWriteByte(dest++, data);

   417 	len--;

   418 

   419 	while (len > 0)

   420 	{

   421 		u8 diff = CPUReadByte(source++);

   422 		data += diff;

   423 		CPUWriteByte(dest++, data);

   424 		len--;

   425 	}

   426 }

   427 

   428 void BIOS_Diff8bitUnFilterVram()

   429 {

   430 #ifdef GBA_LOGGING

   431 	if (systemVerbose & VERBOSE_SWI)

   432 	{

   433 		log("Diff8bitUnFilterVram: 0x%08x,0x%08x (VCOUNT=%d)\n", reg[0].I,

   434 		    reg[1].I, VCOUNT);

   435 	}

   436 #endif

   437 

   438 	u32 source = reg[0].I;

   439 	u32 dest   = reg[1].I;

   440 

   441 	u32 header = CPUReadMemory(source);

   442 	source += 4;

   443 

   444 	if (((source & 0xe000000) == 0) ||

   445 	    ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)

   446 		return;

   447 

   448 	int len = header >> 8;

   449 

   450 	u8  data      = CPUReadByte(source++);

   451 	u16 writeData = data;

   452 	int shift     = 8;

   453 	int bytes     = 1;

   454 

   455 	while (len >= 2)

   456 	{

   457 		u8 diff = CPUReadByte(source++);

   458 		data      += diff;

   459 		writeData |= (data << shift);

   460 		bytes++;

   461 		shift += 8;

   462 		if (bytes == 2)

   463 		{

   464 			CPUWriteHalfWord(dest, writeData);

   465 			dest     += 2;

   466 			len      -= 2;

   467 			bytes     = 0;

   468 			writeData = 0;

   469 			shift     = 0;

   470 		}

   471 	}

   472 }

   473 

   474 void BIOS_Diff16bitUnFilter()

   475 {

   476 #ifdef GBA_LOGGING

   477 	if (systemVerbose & VERBOSE_SWI)

   478 	{

   479 		log("Diff16bitUnFilter: 0x%08x,0x%08x (VCOUNT=%d)\n", reg[0].I,

   480 		    reg[1].I, VCOUNT);

   481 	}

   482 #endif

   483 

   484 	u32 source = reg[0].I;

   485 	u32 dest   = reg[1].I;

   486 

   487 	u32 header = CPUReadMemory(source);

   488 	source += 4;

   489 

   490 	if (((source & 0xe000000) == 0) ||

   491 	    ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)

   492 		return;

   493 

   494 	int len = header >> 8;

   495 

   496 	u16 data = CPUReadHalfWord(source);

   497 	source += 2;

   498 	CPUWriteHalfWord(dest, data);

   499 	dest += 2;

   500 	len  -= 2;

   501 

   502 	while (len >= 2)

   503 	{

   504 		u16 diff = CPUReadHalfWord(source);

   505 		source += 2;

   506 		data   += diff;

   507 		CPUWriteHalfWord(dest, data);

   508 		dest += 2;

   509 		len  -= 2;

   510 	}

   511 }

   512 

   513 void BIOS_Div()

   514 {

   515 #ifdef GBA_LOGGING

   516 	if (systemVerbose & VERBOSE_SWI)

   517 	{

   518 		log("Div: 0x%08x,0x%08x (VCOUNT=%d)\n",

   519 		    reg[0].I,

   520 		    reg[1].I,

   521 		    VCOUNT);

   522 	}

   523 #endif

   524 

   525 	int number = reg[0].I;

   526 	int denom  = reg[1].I;

   527 

   528 	if (denom != 0)

   529 	{

   530 		reg[0].I = number / denom;

   531 		reg[1].I = number % denom;

   532 		s32 temp = (s32)reg[0].I;

   533 		reg[3].I = temp < 0 ? (u32)-temp : (u32)temp;

   534 	}

   535 #ifdef GBA_LOGGING

   536 	if (systemVerbose & VERBOSE_SWI)

   537 	{

   538 		log("Div: return=0x%08x,0x%08x,0x%08x\n",

   539 		    reg[0].I,

   540 		    reg[1].I,

   541 		    reg[3].I);

   542 	}

   543 #endif

   544 }

   545 

   546 void BIOS_DivARM()

   547 {

   548 #ifdef GBA_LOGGING

   549 	if (systemVerbose & VERBOSE_SWI)

   550 	{

   551 		log("DivARM: 0x%08x, (VCOUNT=%d)\n",

   552 		    reg[0].I,

   553 		    VCOUNT);

   554 	}

   555 #endif

   556 

   557 	u32 temp = reg[0].I;

   558 	reg[0].I = reg[1].I;

   559 	reg[1].I = temp;

   560 	BIOS_Div();

   561 }

   562 

   563 void BIOS_HuffUnComp()

   564 {

   565 #ifdef GBA_LOGGING

   566 	if (systemVerbose & VERBOSE_SWI)

   567 	{

   568 		log("HuffUnComp: 0x%08x,0x%08x (VCOUNT=%d)\n",

   569 		    reg[0].I,

   570 		    reg[1].I,

   571 		    VCOUNT);

   572 	}

   573 #endif

   574 

   575 	u32 source = reg[0].I;

   576 	u32 dest   = reg[1].I;

   577 

   578 	u32 header = CPUReadMemory(source);

   579 	source += 4;

   580 

   581 	if (((source & 0xe000000) == 0) ||

   582 	    ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)

   583 		return;

   584 

   585 	u8 treeSize = CPUReadByte(source++);

   586 

   587 	u32 treeStart = source;

   588 

   589 	source += (treeSize<<1) + 1;

   590 

   591 	int len = header >> 8;

   592 

   593 	u32 mask = 0x80000000;

   594 	u32 data = CPUReadMemory(source);

   595 	source += 4;

   596 

   597 	int  pos         = 0;

   598 	u8   rootNode    = CPUReadByte(treeStart);

   599 	u8   currentNode = rootNode;

   600 	bool writeData   = false;

   601 	int  byteShift   = 0;

   602 	int  byteCount   = 0;

   603 	u32  writeValue  = 0;

   604 

   605 	if ((header & 0x0F) == 8)

   606 	{

   607 		while (len > 0)

   608 		{

   609 			// take left

   610 			if (pos == 0)

   611 				pos++;

   612 			else

   613 				pos += (((currentNode & 0x3F)+1)<<1);

   614 

   615 			if (data & mask)

   616 			{

   617 				// right

   618 				if (currentNode & 0x40)

   619 					writeData = true;

   620 				currentNode = CPUReadByte(treeStart+pos+1);

   621 			}

   622 			else

   623 			{

   624 				// left

   625 				if (currentNode & 0x80)

   626 					writeData = true;

   627 				currentNode = CPUReadByte(treeStart+pos);

   628 			}

   629 

   630 			if (writeData)

   631 			{

   632 				writeValue |= (currentNode << byteShift);

   633 				byteCount++;

   634 				byteShift += 8;

   635 

   636 				pos         = 0;

   637 				currentNode = rootNode;

   638 				writeData   = false;

   639 

   640 				if (byteCount == 4)

   641 				{

   642 					byteCount = 0;

   643 					byteShift = 0;

   644 					CPUWriteMemory(dest, writeValue);

   645 					writeValue = 0;

   646 					dest      += 4;

   647 					len       -= 4;

   648 				}

   649 			}

   650 			mask >>= 1;

   651 			if (mask == 0)

   652 			{

   653 				mask    = 0x80000000;

   654 				data    = CPUReadMemory(source);

   655 				source += 4;

   656 			}

   657 		}

   658 	}

   659 	else

   660 	{

   661 		int halfLen = 0;

   662 		int value   = 0;

   663 		while (len > 0)

   664 		{

   665 			// take left

   666 			if (pos == 0)

   667 				pos++;

   668 			else

   669 				pos += (((currentNode & 0x3F)+1)<<1);

   670 

   671 			if ((data & mask))

   672 			{

   673 				// right

   674 				if (currentNode & 0x40)

   675 					writeData = true;

   676 				currentNode = CPUReadByte(treeStart+pos+1);

   677 			}

   678 			else

   679 			{

   680 				// left

   681 				if (currentNode & 0x80)

   682 					writeData = true;

   683 				currentNode = CPUReadByte(treeStart+pos);

   684 			}

   685 

   686 			if (writeData)

   687 			{

   688 				if (halfLen == 0)

   689 					value |= currentNode;

   690 				else

   691 					value |= (currentNode<<4);

   692 

   693 				halfLen += 4;

   694 				if (halfLen == 8)

   695 				{

   696 					writeValue |= (value << byteShift);

   697 					byteCount++;

   698 					byteShift += 8;

   699 

   700 					halfLen = 0;

   701 					value   = 0;

   702 

   703 					if (byteCount == 4)

   704 					{

   705 						byteCount = 0;

   706 						byteShift = 0;

   707 						CPUWriteMemory(dest, writeValue);

   708 						dest      += 4;

   709 						writeValue = 0;

   710 						len       -= 4;

   711 					}

   712 				}

   713 				pos         = 0;

   714 				currentNode = rootNode;

   715 				writeData   = false;

   716 			}

   717 			mask >>= 1;

   718 			if (mask == 0)

   719 			{

   720 				mask    = 0x80000000;

   721 				data    = CPUReadMemory(source);

   722 				source += 4;

   723 			}

   724 		}

   725 	}

   726 }

   727 

   728 void BIOS_LZ77UnCompVram()

   729 {

   730 #ifdef GBA_LOGGING

   731 	if (systemVerbose & VERBOSE_SWI)

   732 	{

   733 		log("LZ77UnCompVram: 0x%08x,0x%08x (VCOUNT=%d)\n",

   734 		    reg[0].I,

   735 		    reg[1].I,

   736 		    VCOUNT);

   737 	}

   738 #endif

   739 

   740 	u32 source = reg[0].I;

   741 	u32 dest   = reg[1].I;

   742 

   743 	u32 header = CPUReadMemory(source);

   744 	source += 4;

   745 

   746 	if (((source & 0xe000000) == 0) ||

   747 	    ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)

   748 		return;

   749 

   750 	int byteCount  = 0;

   751 	int byteShift  = 0;

   752 	u32 writeValue = 0;

   753 

   754 	int len = header >> 8;

   755 

   756 	while (len > 0)

   757 	{

   758 		u8 d = CPUReadByte(source++);

   759 

   760 		if (d)

   761 		{

   762 			for (int i = 0; i < 8; i++)

   763 			{

   764 				if (d & 0x80)

   765 				{

   766 					u16 data = CPUReadByte(source++) << 8;

   767 					data |= CPUReadByte(source++);

   768 					int length       = (data >> 12) + 3;

   769 					int offset       = (data & 0x0FFF);

   770 					u32 windowOffset = dest + byteCount - offset - 1;

   771 					for (int i = 0; i < length; i++)

   772 					{

   773 						writeValue |= (CPUReadByte(windowOffset++) << byteShift);

   774 						byteShift  += 8;

   775 						byteCount++;

   776 

   777 						if (byteCount == 2)

   778 						{

   779 							CPUWriteHalfWord(dest, writeValue);

   780 							dest      += 2;

   781 							byteCount  = 0;

   782 							byteShift  = 0;

   783 							writeValue = 0;

   784 						}

   785 						len--;

   786 						if (len == 0)

   787 							return;

   788 					}

   789 				}

   790 				else

   791 				{

   792 					writeValue |= (CPUReadByte(source++) << byteShift);

   793 					byteShift  += 8;

   794 					byteCount++;

   795 					if (byteCount == 2)

   796 					{

   797 						CPUWriteHalfWord(dest, writeValue);

   798 						dest      += 2;

   799 						byteCount  = 0;

   800 						byteShift  = 0;

   801 						writeValue = 0;

   802 					}

   803 					len--;

   804 					if (len == 0)

   805 						return;

   806 				}

   807 				d <<= 1;

   808 			}

   809 		}

   810 		else

   811 		{

   812 			for (int i = 0; i < 8; i++)

   813 			{

   814 				writeValue |= (CPUReadByte(source++) << byteShift);

   815 				byteShift  += 8;

   816 				byteCount++;

   817 				if (byteCount == 2)

   818 				{

   819 					CPUWriteHalfWord(dest, writeValue);

   820 					dest      += 2;

   821 					byteShift  = 0;

   822 					byteCount  = 0;

   823 					writeValue = 0;

   824 				}

   825 				len--;

   826 				if (len == 0)

   827 					return;

   828 			}

   829 		}

   830 	}

   831 }

   832 

   833 void BIOS_LZ77UnCompWram()

   834 {

   835 #ifdef GBA_LOGGING

   836 	if (systemVerbose & VERBOSE_SWI)

   837 	{

   838 		log("LZ77UnCompWram: 0x%08x,0x%08x (VCOUNT=%d)\n", reg[0].I, reg[1].I,

   839 		    VCOUNT);

   840 	}

   841 #endif

   842 

   843 	u32 source = reg[0].I;

   844 	u32 dest   = reg[1].I;

   845 

   846 	u32 header = CPUReadMemory(source);

   847 	source += 4;

   848 

   849 	if (((source & 0xe000000) == 0) ||

   850 	    ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)

   851 		return;

   852 

   853 	int len = header >> 8;

   854 

   855 	while (len > 0)

   856 	{

   857 		u8 d = CPUReadByte(source++);

   858 

   859 		if (d)

   860 		{

   861 			for (int i = 0; i < 8; i++)

   862 			{

   863 				if (d & 0x80)

   864 				{

   865 					u16 data = CPUReadByte(source++) << 8;

   866 					data |= CPUReadByte(source++);

   867 					int length       = (data >> 12) + 3;

   868 					int offset       = (data & 0x0FFF);

   869 					u32 windowOffset = dest - offset - 1;

   870 					for (int i = 0; i < length; i++)

   871 					{

   872 						CPUWriteByte(dest++, CPUReadByte(windowOffset++));

   873 						len--;

   874 						if (len == 0)

   875 							return;

   876 					}

   877 				}

   878 				else

   879 				{

   880 					CPUWriteByte(dest++, CPUReadByte(source++));

   881 					len--;

   882 					if (len == 0)

   883 						return;

   884 				}

   885 				d <<= 1;

   886 			}

   887 		}

   888 		else

   889 		{

   890 			for (int i = 0; i < 8; i++)

   891 			{

   892 				CPUWriteByte(dest++, CPUReadByte(source++));

   893 				len--;

   894 				if (len == 0)

   895 					return;

   896 			}

   897 		}

   898 	}

   899 }

   900 

   901 void BIOS_ObjAffineSet()

   902 {

   903 #ifdef GBA_LOGGING

   904 	if (systemVerbose & VERBOSE_SWI)

   905 	{

   906 		log("ObjAffineSet: 0x%08x,0x%08x,0x%08x,0x%08x (VCOUNT=%d)\n",

   907 		    reg[0].I,

   908 		    reg[1].I,

   909 		    reg[2].I,

   910 		    reg[3].I,

   911 		    VCOUNT);

   912 	}

   913 #endif

   914 

   915 	u32 src    = reg[0].I;

   916 	u32 dest   = reg[1].I;

   917 	int num    = reg[2].I;

   918 	int offset = reg[3].I;

   919 

   920 	for (int i = 0; i < num; i++)

   921 	{

   922 		s16 rx = CPUReadHalfWord(src);

   923 		src += 2;

   924 		s16 ry = CPUReadHalfWord(src);

   925 		src += 2;

   926 		u16 theta = CPUReadHalfWord(src)>>8;

   927 		src += 4; // keep structure alignment

   928 

   929 		s32 a = (s32)sineTable[(theta+0x40)&255];

   930 		s32 b = (s32)sineTable[theta];

   931 

   932 		s16 dx  =  (s16)((rx * a)>>14);

   933 		s16 dmx = (s16)((rx * b)>>14);

   934 		s16 dy  =  (s16)((ry * b)>>14);

   935 		s16 dmy = (s16)((ry * a)>>14);

   936 

   937 		CPUWriteHalfWord(dest, dx);

   938 		dest += offset;

   939 		CPUWriteHalfWord(dest, -dmx);

   940 		dest += offset;

   941 		CPUWriteHalfWord(dest, dy);

   942 		dest += offset;

   943 		CPUWriteHalfWord(dest, dmy);

   944 		dest += offset;

   945 	}

   946 }

   947 

   948 void BIOS_RegisterRamReset(u32 flags)

   949 {

   950 	// no need to trace here. this is only called directly from GBA.cpp

   951 	// to emulate bios initialization

   952 

   953 	if (flags)

   954 	{

   955 		if (flags & 0x01)

   956 		{

   957 			// clear work RAM

   958 			memset(workRAM, 0, 0x40000);

   959 		}

   960 		if (flags & 0x02)

   961 		{

   962 			// clear internal RAM

   963 			memset(internalRAM, 0, 0x7e00); // don't clear 0x7e00-0x7fff

   964 		}

   965 		if (flags & 0x04)

   966 		{

   967 			// clear palette RAM

   968 			memset(paletteRAM, 0, 0x400);

   969 		}

   970 		if (flags & 0x08)

   971 		{

   972 			// clear VRAM

   973 			memset(vram, 0, 0x18000);

   974 		}

   975 		if (flags & 0x10)

   976 		{

   977 			// clean OAM

   978 			memset(oam, 0, 0x400);

   979 		}

   980 

   981 		if (flags & 0x80)

   982 		{

   983 			int i;

   984 			for (i = 0; i < 8; i++)

   985 				CPUUpdateRegister(0x200+i*2, 0);

   986 

   987 			CPUUpdateRegister(0x202, 0xFFFF);

   988 

   989 			for (i = 0; i < 8; i++)

   990 				CPUUpdateRegister(0x4+i*2, 0);

   991 

   992 			for (i = 0; i < 16; i++)

   993 				CPUUpdateRegister(0x20+i*2, 0);

   994 

   995 			for (i = 0; i < 24; i++)

   996 				CPUUpdateRegister(0xb0+i*2, 0);

   997 

   998 			CPUUpdateRegister(0x130, 0);

   999 			CPUUpdateRegister(0x20, 0x100);

  1000 			CPUUpdateRegister(0x30, 0x100);

  1001 			CPUUpdateRegister(0x26, 0x100);

  1002 			CPUUpdateRegister(0x36, 0x100);

  1003 		}

  1004 

  1005 		if (flags & 0x20)

  1006 		{

  1007 			int i;

  1008 			for (i = 0; i < 8; i++)

  1009 				CPUUpdateRegister(0x110+i*2, 0);

  1010 			CPUUpdateRegister(0x134, 0x8000);

  1011 			for (i = 0; i < 7; i++)

  1012 				CPUUpdateRegister(0x140+i*2, 0);

  1013 		}

  1014 

  1015 		if (flags & 0x40)

  1016 		{

  1017 			int i;

  1018 			CPUWriteByte(0x4000084, 0);

  1019 			CPUWriteByte(0x4000084, 0x80);

  1020 			CPUWriteMemory(0x4000080, 0x880e0000);

  1021 			CPUUpdateRegister(0x88, CPUReadHalfWord(0x4000088)&0x3ff);

  1022 			CPUWriteByte(0x4000070, 0x70);

  1023 			for (i = 0; i < 8; i++)

  1024 				CPUUpdateRegister(0x90+i*2, 0);

  1025 			CPUWriteByte(0x4000070, 0);

  1026 			for (i = 0; i < 8; i++)

  1027 				CPUUpdateRegister(0x90+i*2, 0);

  1028 			CPUWriteByte(0x4000084, 0);

  1029 		}

  1030 	}

  1031 }

  1032 

  1033 void BIOS_RegisterRamReset()

  1034 {

  1035 #ifdef GBA_LOGGING

  1036 	if (systemVerbose & VERBOSE_SWI)

  1037 	{

  1038 		log("RegisterRamReset: 0x%08x (VCOUNT=%d)\n",

  1039 		    reg[0].I,

  1040 		    VCOUNT);

  1041 	}

  1042 #endif

  1043 

  1044 	BIOS_RegisterRamReset(reg[0].I);

  1045 }

  1046 

  1047 void BIOS_RLUnCompVram()

  1048 {

  1049 #ifdef GBA_LOGGING

  1050 	if (systemVerbose & VERBOSE_SWI)

  1051 	{

  1052 		log("RLUnCompVram: 0x%08x,0x%08x (VCOUNT=%d)\n",

  1053 		    reg[0].I,

  1054 		    reg[1].I,

  1055 		    VCOUNT);

  1056 	}

  1057 #endif

  1058 

  1059 	u32 source = reg[0].I;

  1060 	u32 dest   = reg[1].I;

  1061 

  1062 	u32 header = CPUReadMemory(source);

  1063 	source += 4;

  1064 

  1065 	if (((source & 0xe000000) == 0) ||

  1066 	    ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)

  1067 		return;

  1068 

  1069 	int len        = header >> 8;

  1070 	int byteCount  = 0;

  1071 	int byteShift  = 0;

  1072 	u32 writeValue = 0;

  1073 

  1074 	while (len > 0)

  1075 	{

  1076 		u8  d = CPUReadByte(source++);

  1077 		int l = d & 0x7F;

  1078 		if (d & 0x80)

  1079 		{

  1080 			u8 data = CPUReadByte(source++);

  1081 			l += 3;

  1082 			for (int i = 0; i < l; i++)

  1083 			{

  1084 				writeValue |= (data << byteShift);

  1085 				byteShift  += 8;

  1086 				byteCount++;

  1087 

  1088 				if (byteCount == 2)

  1089 				{

  1090 					CPUWriteHalfWord(dest, writeValue);

  1091 					dest      += 2;

  1092 					byteCount  = 0;

  1093 					byteShift  = 0;

  1094 					writeValue = 0;

  1095 				}

  1096 				len--;

  1097 				if (len == 0)

  1098 					return;

  1099 			}

  1100 		}

  1101 		else

  1102 		{

  1103 			l++;

  1104 			for (int i = 0; i < l; i++)

  1105 			{

  1106 				writeValue |= (CPUReadByte(source++) << byteShift);

  1107 				byteShift  += 8;

  1108 				byteCount++;

  1109 				if (byteCount == 2)

  1110 				{

  1111 					CPUWriteHalfWord(dest, writeValue);

  1112 					dest      += 2;

  1113 					byteCount  = 0;

  1114 					byteShift  = 0;

  1115 					writeValue = 0;

  1116 				}

  1117 				len--;

  1118 				if (len == 0)

  1119 					return;

  1120 			}

  1121 		}

  1122 	}

  1123 }

  1124 

  1125 void BIOS_RLUnCompWram()

  1126 {

  1127 #ifdef GBA_LOGGING

  1128 	if (systemVerbose & VERBOSE_SWI)

  1129 	{

  1130 		log("RLUnCompWram: 0x%08x,0x%08x (VCOUNT=%d)\n",

  1131 		    reg[0].I,

  1132 		    reg[1].I,

  1133 		    VCOUNT);

  1134 	}

  1135 #endif

  1136 

  1137 	u32 source = reg[0].I;

  1138 	u32 dest   = reg[1].I;

  1139 

  1140 	u32 header = CPUReadMemory(source);

  1141 	source += 4;

  1142 

  1143 	if (((source & 0xe000000) == 0) ||

  1144 	    ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)

  1145 		return;

  1146 

  1147 	int len = header >> 8;

  1148 

  1149 	while (len > 0)

  1150 	{

  1151 		u8  d = CPUReadByte(source++);

  1152 		int l = d & 0x7F;

  1153 		if (d & 0x80)

  1154 		{

  1155 			u8 data = CPUReadByte(source++);

  1156 			l += 3;

  1157 			for (int i = 0; i < l; i++)

  1158 			{

  1159 				CPUWriteByte(dest++, data);

  1160 				len--;

  1161 				if (len == 0)

  1162 					return;

  1163 			}

  1164 		}

  1165 		else

  1166 		{

  1167 			l++;

  1168 			for (int i = 0; i < l; i++)

  1169 			{

  1170 				CPUWriteByte(dest++,  CPUReadByte(source++));

  1171 				len--;

  1172 				if (len == 0)

  1173 					return;

  1174 			}

  1175 		}

  1176 	}

  1177 }

  1178 

  1179 void BIOS_SoftReset()

  1180 {

  1181 #ifdef GBA_LOGGING

  1182 	if (systemVerbose & VERBOSE_SWI)

  1183 	{

  1184 		log("SoftReset: (VCOUNT=%d)\n", VCOUNT);

  1185 	}

  1186 #endif

  1187 

  1188 	armState        = true;

  1189 	armMode         = 0x1F;

  1190 	armIrqEnable    = false;

  1191 	C_FLAG          = V_FLAG = N_FLAG = Z_FLAG = false;

  1192 	reg[13].I       = 0x03007F00;

  1193 	reg[14].I       = 0x00000000;

  1194 	reg[16].I       = 0x00000000;

  1195 	reg[R13_IRQ].I  = 0x03007FA0;

  1196 	reg[R14_IRQ].I  = 0x00000000;

  1197 	reg[SPSR_IRQ].I = 0x00000000;

  1198 	reg[R13_SVC].I  = 0x03007FE0;

  1199 	reg[R14_SVC].I  = 0x00000000;

  1200 	reg[SPSR_SVC].I = 0x00000000;

  1201 	u8 b = internalRAM[0x7ffa];

  1202 

  1203 	memset(&internalRAM[0x7e00], 0, 0x200);

  1204 

  1205 	if (b)

  1206 	{

  1207 		armNextPC = 0x02000000;

  1208 		reg[15].I = 0x02000004;

  1209 	}

  1210 	else

  1211 	{

  1212 		armNextPC = 0x08000000;

  1213 		reg[15].I = 0x08000004;

  1214 	}

  1215 }

  1216 

  1217 void BIOS_Sqrt()

  1218 {

  1219 #ifdef GBA_LOGGING

  1220 	if (systemVerbose & VERBOSE_SWI)

  1221 	{

  1222 		log("Sqrt: %08x (VCOUNT=%2d)\n",

  1223 		    reg[0].I,

  1224 		    VCOUNT);

  1225 	}

  1226 #endif

  1227 	reg[0].I = (u32)sqrt((double)reg[0].I);

  1228 #ifdef GBA_LOGGING

  1229 	if (systemVerbose & VERBOSE_SWI)

  1230 	{

  1231 		log("Sqrt: return=%08x\n",

  1232 		    reg[0].I);

  1233 	}

  1234 #endif

  1235 }

  1236 

  1237 void BIOS_MidiKey2Freq()

  1238 {

  1239 #ifdef GBA_LOGGING

  1240 	if (systemVerbose & VERBOSE_SWI)

  1241 	{

  1242 		log("MidiKey2Freq: WaveData=%08x mk=%08x fp=%08x\n",

  1243 		    reg[0].I,

  1244 		    reg[1].I,

  1245 		    reg[2].I);

  1246 	}

  1247 #endif

  1248 	int    freq = CPUReadMemory(reg[0].I+4);

  1249 	double tmp;

  1250 	tmp      = ((double)(180 - reg[1].I)) - ((double)reg[2].I / 256.f);

  1251 	tmp      = pow((double)2.f, tmp / 12.f);

  1252 	reg[0].I = (int)((double)freq / tmp);

  1253 

  1254 #ifdef GBA_LOGGING

  1255 	if (systemVerbose & VERBOSE_SWI)

  1256 	{

  1257 		log("MidiKey2Freq: return %08x\n",

  1258 		    reg[0].I);

  1259 	}

  1260 #endif

  1261 }

  1262 

  1263 void BIOS_SndDriverJmpTableCopy()

  1264 {

  1265 #ifdef GBA_LOGGING

  1266 	if (systemVerbose & VERBOSE_SWI)

  1267 	{

  1268 		log("SndDriverJmpTableCopy: dest=%08x\n",

  1269 		    reg[0].I);

  1270 	}

  1271 #endif

  1272 	for (int i = 0; i < 0x24; i++)

  1273 	{

  1274 		CPUWriteMemory(reg[0].I, 0x9c);

  1275 		reg[0].I += 4;

  1276 	}

  1277 }

  1278
author	Robert McIntyre <rlm@mit.edu>
date	Fri, 04 May 2012 05:17:18 -0500
parents	f9f4f1b99eed
children