src/bios.c

   1 // VisualBoyAdvance - Nintendo Gameboy/GameboyAdvance (TM) emulator.
   2 // Copyright (C) 1999-2003 Forgotten
   3 // Copyright (C) 2005-2006 Forgotten and the VBA development team
   4
   5 // This program is free software; you can redistribute it and/or modify
   6 // it under the terms of the GNU General Public License as published by
   7 // the Free Software Foundation; either version 2, or(at your option)
   8 // any later version.
   9 //
  10 // This program is distributed in the hope that it will be useful,
  11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13 // GNU General Public License for more details.
  14 //
  15 // You should have received a copy of the GNU General Public License
  16 // along with this program; if not, write to the Free Software Foundation,
  17 // Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  18
  19 #include <math.h>
  20 #include <memory.h>
  21 #include <stdlib.h>
  22
  23 #include "GBA.h"
  24 #include "bios.h"
  25 #include "GBAinline.h"
  26 #include "Globals.h"
  27
  28 s16 sineTable[256] = {
  29   (s16)0x0000, (s16)0x0192, (s16)0x0323, (s16)0x04B5, (s16)0x0645, (s16)0x07D5, (s16)0x0964, (s16)0x0AF1,
  30   (s16)0x0C7C, (s16)0x0E05, (s16)0x0F8C, (s16)0x1111, (s16)0x1294, (s16)0x1413, (s16)0x158F, (s16)0x1708,
  31   (s16)0x187D, (s16)0x19EF, (s16)0x1B5D, (s16)0x1CC6, (s16)0x1E2B, (s16)0x1F8B, (s16)0x20E7, (s16)0x223D,
  32   (s16)0x238E, (s16)0x24DA, (s16)0x261F, (s16)0x275F, (s16)0x2899, (s16)0x29CD, (s16)0x2AFA, (s16)0x2C21,
  33   (s16)0x2D41, (s16)0x2E5A, (s16)0x2F6B, (s16)0x3076, (s16)0x3179, (s16)0x3274, (s16)0x3367, (s16)0x3453,
  34   (s16)0x3536, (s16)0x3612, (s16)0x36E5, (s16)0x37AF, (s16)0x3871, (s16)0x392A, (s16)0x39DA, (s16)0x3A82,
  35   (s16)0x3B20, (s16)0x3BB6, (s16)0x3C42, (s16)0x3CC5, (s16)0x3D3E, (s16)0x3DAE, (s16)0x3E14, (s16)0x3E71,
  36   (s16)0x3EC5, (s16)0x3F0E, (s16)0x3F4E, (s16)0x3F84, (s16)0x3FB1, (s16)0x3FD3, (s16)0x3FEC, (s16)0x3FFB,
  37   (s16)0x4000, (s16)0x3FFB, (s16)0x3FEC, (s16)0x3FD3, (s16)0x3FB1, (s16)0x3F84, (s16)0x3F4E, (s16)0x3F0E,
  38   (s16)0x3EC5, (s16)0x3E71, (s16)0x3E14, (s16)0x3DAE, (s16)0x3D3E, (s16)0x3CC5, (s16)0x3C42, (s16)0x3BB6,
  39   (s16)0x3B20, (s16)0x3A82, (s16)0x39DA, (s16)0x392A, (s16)0x3871, (s16)0x37AF, (s16)0x36E5, (s16)0x3612,
  40   (s16)0x3536, (s16)0x3453, (s16)0x3367, (s16)0x3274, (s16)0x3179, (s16)0x3076, (s16)0x2F6B, (s16)0x2E5A,
  41   (s16)0x2D41, (s16)0x2C21, (s16)0x2AFA, (s16)0x29CD, (s16)0x2899, (s16)0x275F, (s16)0x261F, (s16)0x24DA,
  42   (s16)0x238E, (s16)0x223D, (s16)0x20E7, (s16)0x1F8B, (s16)0x1E2B, (s16)0x1CC6, (s16)0x1B5D, (s16)0x19EF,
  43   (s16)0x187D, (s16)0x1708, (s16)0x158F, (s16)0x1413, (s16)0x1294, (s16)0x1111, (s16)0x0F8C, (s16)0x0E05,
  44   (s16)0x0C7C, (s16)0x0AF1, (s16)0x0964, (s16)0x07D5, (s16)0x0645, (s16)0x04B5, (s16)0x0323, (s16)0x0192,
  45   (s16)0x0000, (s16)0xFE6E, (s16)0xFCDD, (s16)0xFB4B, (s16)0xF9BB, (s16)0xF82B, (s16)0xF69C, (s16)0xF50F,
  46   (s16)0xF384, (s16)0xF1FB, (s16)0xF074, (s16)0xEEEF, (s16)0xED6C, (s16)0xEBED, (s16)0xEA71, (s16)0xE8F8,
  47   (s16)0xE783, (s16)0xE611, (s16)0xE4A3, (s16)0xE33A, (s16)0xE1D5, (s16)0xE075, (s16)0xDF19, (s16)0xDDC3,
  48   (s16)0xDC72, (s16)0xDB26, (s16)0xD9E1, (s16)0xD8A1, (s16)0xD767, (s16)0xD633, (s16)0xD506, (s16)0xD3DF,
  49   (s16)0xD2BF, (s16)0xD1A6, (s16)0xD095, (s16)0xCF8A, (s16)0xCE87, (s16)0xCD8C, (s16)0xCC99, (s16)0xCBAD,
  50   (s16)0xCACA, (s16)0xC9EE, (s16)0xC91B, (s16)0xC851, (s16)0xC78F, (s16)0xC6D6, (s16)0xC626, (s16)0xC57E,
  51   (s16)0xC4E0, (s16)0xC44A, (s16)0xC3BE, (s16)0xC33B, (s16)0xC2C2, (s16)0xC252, (s16)0xC1EC, (s16)0xC18F,
  52   (s16)0xC13B, (s16)0xC0F2, (s16)0xC0B2, (s16)0xC07C, (s16)0xC04F, (s16)0xC02D, (s16)0xC014, (s16)0xC005,
  53   (s16)0xC000, (s16)0xC005, (s16)0xC014, (s16)0xC02D, (s16)0xC04F, (s16)0xC07C, (s16)0xC0B2, (s16)0xC0F2,
  54   (s16)0xC13B, (s16)0xC18F, (s16)0xC1EC, (s16)0xC252, (s16)0xC2C2, (s16)0xC33B, (s16)0xC3BE, (s16)0xC44A,
  55   (s16)0xC4E0, (s16)0xC57E, (s16)0xC626, (s16)0xC6D6, (s16)0xC78F, (s16)0xC851, (s16)0xC91B, (s16)0xC9EE,
  56   (s16)0xCACA, (s16)0xCBAD, (s16)0xCC99, (s16)0xCD8C, (s16)0xCE87, (s16)0xCF8A, (s16)0xD095, (s16)0xD1A6,
  57   (s16)0xD2BF, (s16)0xD3DF, (s16)0xD506, (s16)0xD633, (s16)0xD767, (s16)0xD8A1, (s16)0xD9E1, (s16)0xDB26,
  58   (s16)0xDC72, (s16)0xDDC3, (s16)0xDF19, (s16)0xE075, (s16)0xE1D5, (s16)0xE33A, (s16)0xE4A3, (s16)0xE611,
  59   (s16)0xE783, (s16)0xE8F8, (s16)0xEA71, (s16)0xEBED, (s16)0xED6C, (s16)0xEEEF, (s16)0xF074, (s16)0xF1FB,
  60   (s16)0xF384, (s16)0xF50F, (s16)0xF69C, (s16)0xF82B, (s16)0xF9BB, (s16)0xFB4B, (s16)0xFCDD, (s16)0xFE6E
  61 };
  62
  63 void BIOS_ArcTan()
  64 {
  65 #ifdef DEV_VERSION
  66   if(systemVerbose & VERBOSE_SWI) {
  67     log("ArcTan: %08x (VCOUNT=%2d)\n",
  68         reg[0].I,
  69         VCOUNT);
  70   }
  71 #endif
  72
  73   s32 a =  -(((s32)(reg[0].I*reg[0].I)) >> 14);
  74   s32 b = ((0xA9 * a) >> 14) + 0x390;
  75   b = ((b * a) >> 14) + 0x91C;
  76   b = ((b * a) >> 14) + 0xFB6;
  77   b = ((b * a) >> 14) + 0x16AA;
  78   b = ((b * a) >> 14) + 0x2081;
  79   b = ((b * a) >> 14) + 0x3651;
  80   b = ((b * a) >> 14) + 0xA2F9;
  81   a = ((s32)reg[0].I * b) >> 16;
  82   reg[0].I = a;
  83
  84 #ifdef DEV_VERSION
  85   if(systemVerbose & VERBOSE_SWI) {
  86     log("ArcTan: return=%08x\n",
  87         reg[0].I);
  88   }
  89 #endif
  90 }
  91
  92 void BIOS_ArcTan2()
  93 {
  94 #ifdef DEV_VERSION
  95   if(systemVerbose & VERBOSE_SWI) {
  96     log("ArcTan2: %08x,%08x (VCOUNT=%2d)\n",
  97         reg[0].I,
  98         reg[1].I,
  99         VCOUNT);
 100   }
 101 #endif
 102
 103   s32 x = reg[0].I;
 104   s32 y = reg[1].I;
 105   u32 res = 0;
 106   if (y == 0) {
 107     res = ((x>>16) & 0x8000);
 108   } else {
 109     if (x == 0) {
 110       res = ((y>>16) & 0x8000) + 0x4000;
 111     } else {
 112                 if ((abs(x) > abs(y)) || ((abs(x) == abs(y)) && (!((x<0) && (y<0))))) {
 113         reg[1].I = x;
 114         reg[0].I = y << 14;
 115         BIOS_Div();
 116         BIOS_ArcTan();
 117         if (x < 0)
 118           res = 0x8000 + reg[0].I;
 119         else
 120           res = (((y>>16) & 0x8000)<<1) + reg[0].I;
 121       } else {
 122         reg[0].I = x << 14;
 123         BIOS_Div();
 124         BIOS_ArcTan();
 125         res = (0x4000 + ((y>>16) & 0x8000)) - reg[0].I;
 126       }
 127     }
 128   }
 129   reg[0].I = res;
 130
 131 #ifdef DEV_VERSION
 132   if(systemVerbose & VERBOSE_SWI) {
 133     log("ArcTan2: return=%08x\n",
 134         reg[0].I);
 135   }
 136 #endif
 137 }
 138
 139 void BIOS_BitUnPack()
 140 {
 141 #ifdef DEV_VERSION
 142   if(systemVerbose & VERBOSE_SWI) {
 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   if(((source & 0xe000000) == 0) ||
 158      ((source + len) & 0xe000000) == 0)
 159     return;
 160
 161   int bits = CPUReadByte(header+2);
 162   int revbits = 8 - bits;
 163   // u32 value = 0;
 164   u32 base = CPUReadMemory(header+4);
 165   bool addBase = (base & 0x80000000) ? true : false;
 166   base &= 0x7fffffff;
 167   int dataSize = CPUReadByte(header+3);
 168
 169   int data = 0;
 170   int bitwritecount = 0;
 171   while(1) {
 172     len -= 1;
 173     if(len < 0)
 174       break;
 175     int mask = 0xff >> revbits;
 176     u8 b = CPUReadByte(source);
 177     source++;
 178     int bitcount = 0;
 179     while(1) {
 180       if(bitcount >= 8)
 181         break;
 182       u32 d = b & mask;
 183       u32 temp = d >> bitcount;
 184       if(d || addBase) {
 185         temp += base;
 186       }
 187       data |= temp << bitwritecount;
 188       bitwritecount += dataSize;
 189       if(bitwritecount >= 32) {
 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_GetBiosChecksum()
 202 {
 203   reg[0].I=0xBAAE187F;
 204 }
 205
 206 void BIOS_BgAffineSet()
 207 {
 208 #ifdef DEV_VERSION
 209   if(systemVerbose & VERBOSE_SWI) {
 210     log("BgAffineSet: %08x,%08x,%08x (VCOUNT=%2d)\n",
 211         reg[0].I,
 212         reg[1].I,
 213         reg[2].I,
 214         VCOUNT);
 215   }
 216 #endif
 217
 218   u32 src = reg[0].I;
 219   u32 dest = reg[1].I;
 220   int num = reg[2].I;
 221
 222   for(int i = 0; i < num; i++) {
 223     s32 cx = CPUReadMemory(src);
 224     src+=4;
 225     s32 cy = CPUReadMemory(src);
 226     src+=4;
 227     s16 dispx = CPUReadHalfWord(src);
 228     src+=2;
 229     s16 dispy = CPUReadHalfWord(src);
 230     src+=2;
 231     s16 rx = CPUReadHalfWord(src);
 232     src+=2;
 233     s16 ry = CPUReadHalfWord(src);
 234     src+=2;
 235     u16 theta = CPUReadHalfWord(src)>>8;
 236     src+=4; // keep structure alignment
 237     s32 a = sineTable[(theta+0x40)&255];
 238     s32 b = sineTable[theta];
 239
 240     s16 dx =  (rx * a)>>14;
 241     s16 dmx = (rx * b)>>14;
 242     s16 dy =  (ry * b)>>14;
 243     s16 dmy = (ry * a)>>14;
 244
 245     CPUWriteHalfWord(dest, dx);
 246     dest += 2;
 247     CPUWriteHalfWord(dest, -dmx);
 248     dest += 2;
 249     CPUWriteHalfWord(dest, dy);
 250     dest += 2;
 251     CPUWriteHalfWord(dest, dmy);
 252     dest += 2;
 253
 254     s32 startx = cx - dx * dispx + dmx * dispy;
 255     s32 starty = cy - dy * dispx - dmy * dispy;
 256
 257     CPUWriteMemory(dest, startx);
 258     dest += 4;
 259     CPUWriteMemory(dest, starty);
 260     dest += 4;
 261   }
 262 }
 263
 264 void BIOS_CpuSet()
 265 {
 266 #ifdef DEV_VERSION
 267   if(systemVerbose & VERBOSE_SWI) {
 268     log("CpuSet: 0x%08x,0x%08x,0x%08x (VCOUNT=%d)\n", reg[0].I, reg[1].I,
 269         reg[2].I, VCOUNT);
 270   }
 271 #endif
 272
 273   u32 source = reg[0].I;
 274   u32 dest = reg[1].I;
 275   u32 cnt = reg[2].I;
 276
 277   if(((source & 0xe000000) == 0) ||
 278      ((source + (((cnt << 11)>>9) & 0x1fffff)) & 0xe000000) == 0)
 279     return;
 280
 281   int count = cnt & 0x1FFFFF;
 282
 283   // 32-bit ?
 284   if((cnt >> 26) & 1) {
 285     // needed for 32-bit mode!
 286     source &= 0xFFFFFFFC;
 287     dest &= 0xFFFFFFFC;
 288     // fill ?
 289     if((cnt >> 24) & 1) {
 290         u32 value = (source>0x0EFFFFFF ? 0x1CAD1CAD : CPUReadMemory(source));
 291       while(count) {
 292         CPUWriteMemory(dest, value);
 293         dest += 4;
 294         count--;
 295       }
 296     } else {
 297       // copy
 298       while(count) {
 299         CPUWriteMemory(dest, (source>0x0EFFFFFF ? 0x1CAD1CAD : CPUReadMemory(source)));
 300         source += 4;
 301         dest += 4;
 302         count--;
 303       }
 304     }
 305   } else {
 306     // 16-bit fill?
 307     if((cnt >> 24) & 1) {
 308       u16 value = (source>0x0EFFFFFF ? 0x1CAD : CPUReadHalfWord(source));
 309       while(count) {
 310         CPUWriteHalfWord(dest, value);
 311         dest += 2;
 312         count--;
 313       }
 314     } else {
 315       // copy
 316       while(count) {
 317         CPUWriteHalfWord(dest, (source>0x0EFFFFFF ? 0x1CAD : CPUReadHalfWord(source)));
 318         source += 2;
 319         dest += 2;
 320         count--;
 321       }
 322     }
 323   }
 324 }
 325
 326 void BIOS_CpuFastSet()
 327 {
 328 #ifdef DEV_VERSION
 329   if(systemVerbose & VERBOSE_SWI) {
 330     log("CpuFastSet: 0x%08x,0x%08x,0x%08x (VCOUNT=%d)\n", reg[0].I, reg[1].I,
 331         reg[2].I, VCOUNT);
 332   }
 333 #endif
 334
 335   u32 source = reg[0].I;
 336   u32 dest = reg[1].I;
 337   u32 cnt = reg[2].I;
 338
 339   if(((source & 0xe000000) == 0) ||
 340      ((source + (((cnt << 11)>>9) & 0x1fffff)) & 0xe000000) == 0)
 341     return;
 342
 343   // needed for 32-bit mode!
 344   source &= 0xFFFFFFFC;
 345   dest &= 0xFFFFFFFC;
 346
 347   int count = cnt & 0x1FFFFF;
 348
 349   // fill?
 350   if((cnt >> 24) & 1) {
 351     while(count > 0) {
 352       // BIOS always transfers 32 bytes at a time
 353       u32 value = (source>0x0EFFFFFF ? 0xBAFFFFFB : CPUReadMemory(source));
 354       for(int i = 0; i < 8; i++) {
 355         CPUWriteMemory(dest, value);
 356         dest += 4;
 357       }
 358       count -= 8;
 359     }
 360   } else {
 361     // copy
 362     while(count > 0) {
 363       // BIOS always transfers 32 bytes at a time
 364       for(int i = 0; i < 8; i++) {
 365         CPUWriteMemory(dest, (source>0x0EFFFFFF ? 0xBAFFFFFB :CPUReadMemory(source)));
 366         source += 4;
 367         dest += 4;
 368       }
 369       count -= 8;
 370     }
 371   }
 372 }
 373
 374 void BIOS_Diff8bitUnFilterWram()
 375 {
 376 #ifdef DEV_VERSION
 377   if(systemVerbose & VERBOSE_SWI) {
 378     log("Diff8bitUnFilterWram: 0x%08x,0x%08x (VCOUNT=%d)\n", reg[0].I,
 379         reg[1].I, VCOUNT);
 380   }
 381 #endif
 382
 383   u32 source = reg[0].I;
 384   u32 dest = reg[1].I;
 385
 386   u32 header = CPUReadMemory(source);
 387   source += 4;
 388
 389   if(((source & 0xe000000) == 0) ||
 390      ((source + ((header >> 8) & 0x1fffff) & 0xe000000) == 0))
 391     return;
 392
 393   int len = header >> 8;
 394
 395   u8 data = CPUReadByte(source++);
 396   CPUWriteByte(dest++, data);
 397   len--;
 398
 399   while(len > 0) {
 400     u8 diff = CPUReadByte(source++);
 401     data += diff;
 402     CPUWriteByte(dest++, data);
 403     len--;
 404   }
 405 }
 406
 407 void BIOS_Diff8bitUnFilterVram()
 408 {
 409 #ifdef DEV_VERSION
 410   if(systemVerbose & VERBOSE_SWI) {
 411     log("Diff8bitUnFilterVram: 0x%08x,0x%08x (VCOUNT=%d)\n", reg[0].I,
 412         reg[1].I, VCOUNT);
 413   }
 414 #endif
 415
 416   u32 source = reg[0].I;
 417   u32 dest = reg[1].I;
 418
 419   u32 header = CPUReadMemory(source);
 420   source += 4;
 421
 422   if(((source & 0xe000000) == 0) ||
 423      ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)
 424     return;
 425
 426   int len = header >> 8;
 427
 428   u8 data = CPUReadByte(source++);
 429   u16 writeData = data;
 430   int shift = 8;
 431   int bytes = 1;
 432
 433   while(len >= 2) {
 434     u8 diff = CPUReadByte(source++);
 435     data += diff;
 436     writeData |= (data << shift);
 437     bytes++;
 438     shift += 8;
 439     if(bytes == 2) {
 440       CPUWriteHalfWord(dest, writeData);
 441       dest += 2;
 442       len -= 2;
 443       bytes = 0;
 444       writeData = 0;
 445       shift = 0;
 446     }
 447   }
 448 }
 449
 450 void BIOS_Diff16bitUnFilter()
 451 {
 452 #ifdef DEV_VERSION
 453   if(systemVerbose & VERBOSE_SWI) {
 454     log("Diff16bitUnFilter: 0x%08x,0x%08x (VCOUNT=%d)\n", reg[0].I,
 455         reg[1].I, VCOUNT);
 456   }
 457 #endif
 458
 459   u32 source = reg[0].I;
 460   u32 dest = reg[1].I;
 461
 462   u32 header = CPUReadMemory(source);
 463   source += 4;
 464
 465   if(((source & 0xe000000) == 0) ||
 466      ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)
 467     return;
 468
 469   int len = header >> 8;
 470
 471   u16 data = CPUReadHalfWord(source);
 472   source += 2;
 473   CPUWriteHalfWord(dest, data);
 474   dest += 2;
 475   len -= 2;
 476
 477   while(len >= 2) {
 478     u16 diff = CPUReadHalfWord(source);
 479     source += 2;
 480     data += diff;
 481     CPUWriteHalfWord(dest, data);
 482     dest += 2;
 483     len -= 2;
 484   }
 485 }
 486
 487 void BIOS_Div()
 488 {
 489 #ifdef DEV_VERSION
 490   if(systemVerbose & VERBOSE_SWI) {
 491     log("Div: 0x%08x,0x%08x (VCOUNT=%d)\n",
 492         reg[0].I,
 493         reg[1].I,
 494         VCOUNT);
 495   }
 496 #endif
 497
 498   int number = reg[0].I;
 499   int denom = reg[1].I;
 500
 501   if(denom != 0) {
 502     reg[0].I = number / denom;
 503     reg[1].I = number % denom;
 504     s32 temp = (s32)reg[0].I;
 505     reg[3].I = temp < 0 ? (u32)-temp : (u32)temp;
 506   }
 507 #ifdef DEV_VERSION
 508   if(systemVerbose & VERBOSE_SWI) {
 509     log("Div: return=0x%08x,0x%08x,0x%08x\n",
 510         reg[0].I,
 511         reg[1].I,
 512         reg[3].I);
 513   }
 514 #endif
 515 }
 516
 517 void BIOS_DivARM()
 518 {
 519 #ifdef DEV_VERSION
 520   if(systemVerbose & VERBOSE_SWI) {
 521     log("DivARM: 0x%08x, (VCOUNT=%d)\n",
 522         reg[0].I,
 523         VCOUNT);
 524   }
 525 #endif
 526
 527   u32 temp = reg[0].I;
 528   reg[0].I = reg[1].I;
 529   reg[1].I = temp;
 530   BIOS_Div();
 531 }
 532
 533 void BIOS_HuffUnComp()
 534 {
 535 #ifdef DEV_VERSION
 536   if(systemVerbose & VERBOSE_SWI) {
 537     log("HuffUnComp: 0x%08x,0x%08x (VCOUNT=%d)\n",
 538         reg[0].I,
 539         reg[1].I,
 540         VCOUNT);
 541   }
 542 #endif
 543
 544   u32 source = reg[0].I;
 545   u32 dest = reg[1].I;
 546
 547   u32 header = CPUReadMemory(source);
 548   source += 4;
 549
 550   if(((source & 0xe000000) == 0) ||
 551      ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)
 552     return;
 553
 554   u8 treeSize = CPUReadByte(source++);
 555
 556   u32 treeStart = source;
 557
 558   source += ((treeSize+1)<<1)-1; // minus because we already skipped one byte
 559
 560   int len = header >> 8;
 561
 562   u32 mask = 0x80000000;
 563   u32 data = CPUReadMemory(source);
 564   source += 4;
 565
 566   int pos = 0;
 567   u8 rootNode = CPUReadByte(treeStart);
 568   u8 currentNode = rootNode;
 569   bool writeData = false;
 570   int byteShift = 0;
 571   int byteCount = 0;
 572   u32 writeValue = 0;
 573
 574   if((header & 0x0F) == 8) {
 575     while(len > 0) {
 576       // take left
 577       if(pos == 0)
 578         pos++;
 579       else
 580         pos += (((currentNode & 0x3F)+1)<<1);
 581
 582       if(data & mask) {
 583         // right
 584         if(currentNode & 0x40)
 585           writeData = true;
 586         currentNode = CPUReadByte(treeStart+pos+1);
 587       } else {
 588         // left
 589         if(currentNode & 0x80)
 590           writeData = true;
 591         currentNode = CPUReadByte(treeStart+pos);
 592       }
 593
 594       if(writeData) {
 595         writeValue |= (currentNode << byteShift);
 596         byteCount++;
 597         byteShift += 8;
 598
 599         pos = 0;
 600         currentNode = rootNode;
 601         writeData = false;
 602
 603         if(byteCount == 4) {
 604           byteCount = 0;
 605           byteShift = 0;
 606           CPUWriteMemory(dest, writeValue);
 607           writeValue = 0;
 608           dest += 4;
 609           len -= 4;
 610         }
 611       }
 612       mask >>= 1;
 613       if(mask == 0) {
 614         mask = 0x80000000;
 615         data = CPUReadMemory(source);
 616         source += 4;
 617       }
 618     }
 619   } else {
 620     int halfLen = 0;
 621     int value = 0;
 622     while(len > 0) {
 623       // take left
 624       if(pos == 0)
 625         pos++;
 626       else
 627         pos += (((currentNode & 0x3F)+1)<<1);
 628
 629       if((data & mask)) {
 630         // right
 631         if(currentNode & 0x40)
 632           writeData = true;
 633         currentNode = CPUReadByte(treeStart+pos+1);
 634       } else {
 635         // left
 636         if(currentNode & 0x80)
 637           writeData = true;
 638         currentNode = CPUReadByte(treeStart+pos);
 639       }
 640
 641       if(writeData) {
 642         if(halfLen == 0)
 643           value |= currentNode;
 644         else
 645           value |= (currentNode<<4);
 646
 647         halfLen += 4;
 648         if(halfLen == 8) {
 649           writeValue |= (value << byteShift);
 650           byteCount++;
 651           byteShift += 8;
 652
 653           halfLen = 0;
 654           value = 0;
 655
 656           if(byteCount == 4) {
 657             byteCount = 0;
 658             byteShift = 0;
 659             CPUWriteMemory(dest, writeValue);
 660             dest += 4;
 661             writeValue = 0;
 662             len -= 4;
 663           }
 664         }
 665         pos = 0;
 666         currentNode = rootNode;
 667         writeData = false;
 668       }
 669       mask >>= 1;
 670       if(mask == 0) {
 671         mask = 0x80000000;
 672         data = CPUReadMemory(source);
 673         source += 4;
 674       }
 675     }
 676   }
 677 }
 678
 679 void BIOS_LZ77UnCompVram()
 680 {
 681 #ifdef DEV_VERSION
 682   if(systemVerbose & VERBOSE_SWI) {
 683     log("LZ77UnCompVram: 0x%08x,0x%08x (VCOUNT=%d)\n",
 684         reg[0].I,
 685         reg[1].I,
 686         VCOUNT);
 687   }
 688 #endif
 689
 690   u32 source = reg[0].I;
 691   u32 dest = reg[1].I;
 692
 693   u32 header = CPUReadMemory(source);
 694   source += 4;
 695
 696   if(((source & 0xe000000) == 0) ||
 697      ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)
 698     return;
 699
 700   int byteCount = 0;
 701   int byteShift = 0;
 702   u32 writeValue = 0;
 703
 704   int len = header >> 8;
 705
 706   while(len > 0) {
 707     u8 d = CPUReadByte(source++);
 708
 709     if(d) {
 710       for(int i = 0; i < 8; i++) {
 711         if(d & 0x80) {
 712           u16 data = CPUReadByte(source++) << 8;
 713           data |= CPUReadByte(source++);
 714           int length = (data >> 12) + 3;
 715           int offset = (data & 0x0FFF);
 716           u32 windowOffset = dest + byteCount - offset - 1;
 717           for(int i = 0; i < length; i++) {
 718             writeValue |= (CPUReadByte(windowOffset++) << byteShift);
 719             byteShift += 8;
 720             byteCount++;
 721
 722             if(byteCount == 2) {
 723               CPUWriteHalfWord(dest, writeValue);
 724               dest += 2;
 725               byteCount = 0;
 726               byteShift = 0;
 727               writeValue = 0;
 728             }
 729             len--;
 730             if(len == 0)
 731               return;
 732           }
 733         } else {
 734           writeValue |= (CPUReadByte(source++) << byteShift);
 735           byteShift += 8;
 736           byteCount++;
 737           if(byteCount == 2) {
 738             CPUWriteHalfWord(dest, writeValue);
 739             dest += 2;
 740             byteCount = 0;
 741             byteShift = 0;
 742             writeValue = 0;
 743           }
 744           len--;
 745           if(len == 0)
 746             return;
 747         }
 748         d <<= 1;
 749       }
 750     } else {
 751       for(int i = 0; i < 8; i++) {
 752         writeValue |= (CPUReadByte(source++) << byteShift);
 753         byteShift += 8;
 754         byteCount++;
 755         if(byteCount == 2) {
 756           CPUWriteHalfWord(dest, writeValue);
 757           dest += 2;
 758           byteShift = 0;
 759           byteCount = 0;
 760           writeValue = 0;
 761         }
 762         len--;
 763         if(len == 0)
 764           return;
 765       }
 766     }
 767   }
 768 }
 769
 770 void BIOS_LZ77UnCompWram()
 771 {
 772 #ifdef DEV_VERSION
 773   if(systemVerbose & VERBOSE_SWI) {
 774     log("LZ77UnCompWram: 0x%08x,0x%08x (VCOUNT=%d)\n", reg[0].I, reg[1].I,
 775         VCOUNT);
 776   }
 777 #endif
 778
 779   u32 source = reg[0].I;
 780   u32 dest = reg[1].I;
 781
 782   u32 header = CPUReadMemory(source);
 783   source += 4;
 784
 785   if(((source & 0xe000000) == 0) ||
 786      ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)
 787     return;
 788
 789   int len = header >> 8;
 790
 791   while(len > 0) {
 792     u8 d = CPUReadByte(source++);
 793
 794     if(d) {
 795       for(int i = 0; i < 8; i++) {
 796         if(d & 0x80) {
 797           u16 data = CPUReadByte(source++) << 8;
 798           data |= CPUReadByte(source++);
 799           int length = (data >> 12) + 3;
 800           int offset = (data & 0x0FFF);
 801           u32 windowOffset = dest - offset - 1;
 802           for(int i = 0; i < length; i++) {
 803             CPUWriteByte(dest++, CPUReadByte(windowOffset++));
 804             len--;
 805             if(len == 0)
 806               return;
 807           }
 808         } else {
 809           CPUWriteByte(dest++, CPUReadByte(source++));
 810           len--;
 811           if(len == 0)
 812             return;
 813         }
 814         d <<= 1;
 815       }
 816     } else {
 817       for(int i = 0; i < 8; i++) {
 818         CPUWriteByte(dest++, CPUReadByte(source++));
 819         len--;
 820         if(len == 0)
 821           return;
 822       }
 823     }
 824   }
 825 }
 826
 827 void BIOS_ObjAffineSet()
 828 {
 829 #ifdef DEV_VERSION
 830   if(systemVerbose & VERBOSE_SWI) {
 831     log("ObjAffineSet: 0x%08x,0x%08x,0x%08x,0x%08x (VCOUNT=%d)\n",
 832         reg[0].I,
 833         reg[1].I,
 834         reg[2].I,
 835         reg[3].I,
 836         VCOUNT);
 837   }
 838 #endif
 839
 840   u32 src = reg[0].I;
 841   u32 dest = reg[1].I;
 842   int num = reg[2].I;
 843   int offset = reg[3].I;
 844
 845   for(int i = 0; i < num; i++) {
 846     s16 rx = CPUReadHalfWord(src);
 847     src+=2;
 848     s16 ry = CPUReadHalfWord(src);
 849     src+=2;
 850     u16 theta = CPUReadHalfWord(src)>>8;
 851     src+=4; // keep structure alignment
 852
 853     s32 a = (s32)sineTable[(theta+0x40)&255];
 854     s32 b = (s32)sineTable[theta];
 855
 856     s16 dx =  ((s32)rx * a)>>14;
 857     s16 dmx = ((s32)rx * b)>>14;
 858     s16 dy =  ((s32)ry * b)>>14;
 859     s16 dmy = ((s32)ry * a)>>14;
 860
 861     CPUWriteHalfWord(dest, dx);
 862     dest += offset;
 863     CPUWriteHalfWord(dest, -dmx);
 864     dest += offset;
 865     CPUWriteHalfWord(dest, dy);
 866     dest += offset;
 867     CPUWriteHalfWord(dest, dmy);
 868     dest += offset;
 869   }
 870 }
 871
 872 void BIOS_RegisterRamResetU(u32 flags)
 873 {
 874   // no need to trace here. this is only called directly from GBA.cpp
 875   // to emulate bios initialization
 876
 877   CPUUpdateRegister(0x0, 0x80);
 878
 879   if(flags) {
 880     if(flags & 0x01) {
 881       // clear work RAM
 882       memset(workRAM, 0, 0x40000);
 883     }
 884     if(flags & 0x02) {
 885       // clear internal RAM
 886       memset(internalRAM, 0, 0x7e00); // don't clear 0x7e00-0x7fff
 887     }
 888     if(flags & 0x04) {
 889       // clear palette RAM
 890       memset(paletteRAM, 0, 0x400);
 891     }
 892     if(flags & 0x08) {
 893       // clear VRAM
 894       memset(vram, 0, 0x18000);
 895     }
 896     if(flags & 0x10) {
 897       // clean OAM
 898       memset(oam, 0, 0x400);
 899     }
 900
 901     if(flags & 0x80) {
 902       int i;
 903       for(i = 0; i < 0x10; i++)
 904         CPUUpdateRegister(0x200+i*2, 0);
 905
 906       for(i = 0; i < 0xF; i++)
 907         CPUUpdateRegister(0x4+i*2, 0);
 908
 909       for(i = 0; i < 0x20; i++)
 910         CPUUpdateRegister(0x20+i*2, 0);
 911
 912       for(i = 0; i < 0x18; i++)
 913         CPUUpdateRegister(0xb0+i*2, 0);
 914
 915       CPUUpdateRegister(0x130, 0);
 916       CPUUpdateRegister(0x20, 0x100);
 917       CPUUpdateRegister(0x30, 0x100);
 918       CPUUpdateRegister(0x26, 0x100);
 919       CPUUpdateRegister(0x36, 0x100);
 920     }
 921
 922     if(flags & 0x20) {
 923       int i;
 924       for(i = 0; i < 8; i++)
 925         CPUUpdateRegister(0x110+i*2, 0);
 926       CPUUpdateRegister(0x134, 0x8000);
 927       for(i = 0; i < 7; i++)
 928         CPUUpdateRegister(0x140+i*2, 0);
 929     }
 930
 931     if(flags & 0x40) {
 932       int i;
 933       CPUWriteByte(0x4000084, 0);
 934       CPUWriteByte(0x4000084, 0x80);
 935       CPUWriteMemory(0x4000080, 0x880e0000);
 936       CPUUpdateRegister(0x88, CPUReadHalfWord(0x4000088)&0x3ff);
 937       CPUWriteByte(0x4000070, 0x70);
 938       for(i = 0; i < 8; i++)
 939         CPUUpdateRegister(0x90+i*2, 0);
 940       CPUWriteByte(0x4000070, 0);
 941       for(i = 0; i < 8; i++)
 942         CPUUpdateRegister(0x90+i*2, 0);
 943       CPUWriteByte(0x4000084, 0);
 944     }
 945   }
 946 }
 947
 948 void BIOS_RegisterRamReset(void)
 949 {
 950 #ifdef DEV_VERSION
 951   if(systemVerbose & VERBOSE_SWI) {
 952     log("RegisterRamReset: 0x%08x (VCOUNT=%d)\n",
 953         reg[0].I,
 954         VCOUNT);
 955   }
 956 #endif
 957
 958   BIOS_RegisterRamResetU(reg[0].I);
 959 }
 960
 961 void BIOS_RLUnCompVram()
 962 {
 963 #ifdef DEV_VERSION
 964   if(systemVerbose & VERBOSE_SWI) {
 965     log("RLUnCompVram: 0x%08x,0x%08x (VCOUNT=%d)\n",
 966         reg[0].I,
 967         reg[1].I,
 968         VCOUNT);
 969   }
 970 #endif
 971
 972   u32 source = reg[0].I;
 973   u32 dest = reg[1].I;
 974
 975   u32 header = CPUReadMemory(source & 0xFFFFFFFC);
 976   source += 4;
 977
 978   if(((source & 0xe000000) == 0) ||
 979      ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)
 980     return;
 981
 982   int len = header >> 8;
 983   int byteCount = 0;
 984   int byteShift = 0;
 985   u32 writeValue = 0;
 986
 987   while(len > 0) {
 988     u8 d = CPUReadByte(source++);
 989     int l = d & 0x7F;
 990     if(d & 0x80) {
 991       u8 data = CPUReadByte(source++);
 992       l += 3;
 993       for(int i = 0;i < l; i++) {
 994         writeValue |= (data << byteShift);
 995         byteShift += 8;
 996         byteCount++;
 997
 998         if(byteCount == 2) {
 999           CPUWriteHalfWord(dest, writeValue);
1000           dest += 2;
1001           byteCount = 0;
1002           byteShift = 0;
1003           writeValue = 0;
1004         }
1005         len--;
1006         if(len == 0)
1007           return;
1008       }
1009     } else {
1010       l++;
1011       for(int i = 0; i < l; i++) {
1012         writeValue |= (CPUReadByte(source++) << byteShift);
1013         byteShift += 8;
1014         byteCount++;
1015         if(byteCount == 2) {
1016           CPUWriteHalfWord(dest, writeValue);
1017           dest += 2;
1018           byteCount = 0;
1019           byteShift = 0;
1020           writeValue = 0;
1021         }
1022         len--;
1023         if(len == 0)
1024           return;
1025       }
1026     }
1027   }
1028 }
1029
1030 void BIOS_RLUnCompWram()
1031 {
1032 #ifdef DEV_VERSION
1033   if(systemVerbose & VERBOSE_SWI) {
1034     log("RLUnCompWram: 0x%08x,0x%08x (VCOUNT=%d)\n",
1035         reg[0].I,
1036         reg[1].I,
1037         VCOUNT);
1038   }
1039 #endif
1040
1041   u32 source = reg[0].I;
1042   u32 dest = reg[1].I;
1043
1044   u32 header = CPUReadMemory(source & 0xFFFFFFFC);
1045   source += 4;
1046
1047   if(((source & 0xe000000) == 0) ||
1048      ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)
1049     return;
1050
1051   int len = header >> 8;
1052
1053   while(len > 0) {
1054     u8 d = CPUReadByte(source++);
1055     int l = d & 0x7F;
1056     if(d & 0x80) {
1057       u8 data = CPUReadByte(source++);
1058       l += 3;
1059       for(int i = 0;i < l; i++) {
1060         CPUWriteByte(dest++, data);
1061         len--;
1062         if(len == 0)
1063           return;
1064       }
1065     } else {
1066       l++;
1067       for(int i = 0; i < l; i++) {
1068         CPUWriteByte(dest++,  CPUReadByte(source++));
1069         len--;
1070         if(len == 0)
1071           return;
1072       }
1073     }
1074   }
1075 }
1076
1077 void BIOS_SoftReset()
1078 {
1079 #ifdef DEV_VERSION
1080   if(systemVerbose & VERBOSE_SWI) {
1081     log("SoftReset: (VCOUNT=%d)\n", VCOUNT);
1082   }
1083 #endif
1084
1085   armState = true;
1086   armMode = 0x1F;
1087   armIrqEnable = false;
1088   C_FLAG = V_FLAG = N_FLAG = Z_FLAG = false;
1089   reg[13].I = 0x03007F00;
1090   reg[14].I = 0x00000000;
1091   reg[16].I = 0x00000000;
1092   reg[R13_IRQ].I = 0x03007FA0;
1093   reg[R14_IRQ].I = 0x00000000;
1094   reg[SPSR_IRQ].I = 0x00000000;
1095   reg[R13_SVC].I = 0x03007FE0;
1096   reg[R14_SVC].I = 0x00000000;
1097   reg[SPSR_SVC].I = 0x00000000;
1098   u8 b = internalRAM[0x7ffa];
1099
1100   memset(&internalRAM[0x7e00], 0, 0x200);
1101
1102   if(b) {
1103     armNextPC = 0x02000000;
1104     reg[15].I = 0x02000004;
1105   } else {
1106     armNextPC = 0x08000000;
1107     reg[15].I = 0x08000004;
1108   }
1109 }
1110
1111 void BIOS_Sqrt()
1112 {
1113 #ifdef DEV_VERSION
1114   if(systemVerbose & VERBOSE_SWI) {
1115     log("Sqrt: %08x (VCOUNT=%2d)\n",
1116         reg[0].I,
1117         VCOUNT);
1118   }
1119 #endif
1120   reg[0].I = (u32)sqrt((double)reg[0].I);
1121 #ifdef DEV_VERSION
1122   if(systemVerbose & VERBOSE_SWI) {
1123     log("Sqrt: return=%08x\n",
1124         reg[0].I);
1125   }
1126 #endif
1127 }
1128
1129 void BIOS_MidiKey2Freq()
1130 {
1131 #ifdef DEV_VERSION
1132   if(systemVerbose & VERBOSE_SWI) {
1133     log("MidiKey2Freq: WaveData=%08x mk=%08x fp=%08x\n",
1134         reg[0].I,
1135         reg[1].I,
1136         reg[2].I);
1137   }
1138 #endif
1139   int freq = CPUReadMemory(reg[0].I+4);
1140   double tmp;
1141   tmp = ((double)(180 - reg[1].I)) - ((double)reg[2].I / 256.f);
1142   tmp = pow((double)2.f, tmp / 12.f);
1143   reg[0].I = (int)((double)freq / tmp);
1144
1145 #ifdef DEV_VERSION
1146   if(systemVerbose & VERBOSE_SWI) {
1147     log("MidiKey2Freq: return %08x\n",
1148         reg[0].I);
1149   }
1150 #endif
1151 }
1152
1153 void BIOS_SndDriverJmpTableCopy()
1154 {
1155 #ifdef DEV_VERSION
1156   if(systemVerbose & VERBOSE_SWI) {
1157     log("SndDriverJmpTableCopy: dest=%08x\n",
1158         reg[0].I);
1159   }
1160 #endif
1161   for(int i = 0; i < 0x24; i++) {
1162     CPUWriteMemory(reg[0].I, 0x9c);
1163     reg[0].I += 4;
1164   }
1165 }