home *** CD-ROM | disk | FTP | other *** search
/ Chip 2004 July / CMCD0704.ISO / Software / Freeware / Utilitare / VisualBoyAdvance-1.7.2 / src / bios.cpp < prev    next >
Encoding:
C/C++ Source or Header  |  2004-05-13  |  27.0 KB  |  1,159 lines
  1. // VisualBoyAdvance - Nintendo Gameboy/GameboyAdvance (TM) emulator.
  2. // Copyright (C) 1999-2003 Forgotten
  3. // Copyright (C) 2004 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.   reg[0].I = (reg[0].I * b) >> 16;
  82.  
  83. #ifdef DEV_VERSION
  84.   if(systemVerbose & VERBOSE_SWI) {
  85.     log("ArcTan: return=%08x\n",
  86.         reg[0].I);
  87.   }
  88. #endif
  89. }
  90.  
  91. void BIOS_ArcTan2()
  92. {
  93. #ifdef DEV_VERSION
  94.   if(systemVerbose & VERBOSE_SWI) {
  95.     log("ArcTan2: %08x,%08x (VCOUNT=%2d)\n",
  96.         reg[0].I,
  97.         reg[1].I,
  98.         VCOUNT);
  99.   }
  100. #endif
  101.   
  102.   s16 x = reg[0].I;
  103.   s16 y = reg[1].I;
  104.   
  105.   if (y == 0) {
  106.     reg[0].I = 0x8000 & x;
  107.     reg[3].I = 0x170;
  108.   } else {
  109.     if (x == 0) {
  110.       reg[0].I = (0x8000 & y) + 0x4000;
  111.       reg[3].I = 0x170;
  112.     } else {
  113.       if (abs(x) > abs(y)) {
  114.         reg[1].I = x;
  115.         reg[0].I = y << 14;
  116.         BIOS_Div();
  117.         BIOS_ArcTan();
  118.         if (x < 0)
  119.           reg[0].I = 0x8000 + reg[0].I;
  120.         else
  121.           reg[0].I = ((y & 0x8000) << 1 ) + reg[0].I;
  122.         reg[3].I = 0x170;
  123.       } else {
  124.         reg[0].I = x << 14;
  125.         BIOS_Div();
  126.         BIOS_ArcTan();
  127.         reg[0].I = (0x4000 + (y & 0x8000)) - reg[0].I;
  128.         reg[3].I = 0x170;
  129.       }
  130.     }
  131.   }
  132.   
  133. #ifdef DEV_VERSION
  134.   if(systemVerbose & VERBOSE_SWI) {
  135.     log("ArcTan2: return=%08x\n",
  136.         reg[0].I);
  137.   }
  138. #endif
  139. } 
  140.  
  141. void BIOS_BitUnPack()
  142. {
  143. #ifdef DEV_VERSION  
  144.   if(systemVerbose & VERBOSE_SWI) {
  145.     log("BitUnPack: %08x,%08x,%08x (VCOUNT=%2d)\n",
  146.         reg[0].I,
  147.         reg[1].I,
  148.         reg[2].I,
  149.         VCOUNT);
  150.   }
  151. #endif
  152.  
  153.   u32 source = reg[0].I;
  154.   u32 dest = reg[1].I;
  155.   u32 header = reg[2].I;
  156.   
  157.   int len = CPUReadHalfWord(header);
  158.   // check address
  159.   int bits = CPUReadByte(header+2);
  160.   int revbits = 8 - bits; 
  161.   // u32 value = 0;
  162.   u32 base = CPUReadMemory(header+4);
  163.   bool addBase = (base & 0x80000000) ? true : false;
  164.   base &= 0x7fffffff;
  165.   int dataSize = CPUReadByte(header+3);
  166.  
  167.   int data = 0; 
  168.   int bitwritecount = 0; 
  169.   while(1) {
  170.     len -= 1;
  171.     if(len < 0)
  172.       break;
  173.     int mask = 0xff >> revbits; 
  174.     u8 b = CPUReadByte(source); 
  175.     source++;
  176.     int bitcount = 0;
  177.     while(1) {
  178.       if(bitcount >= 8)
  179.         break;
  180.       u32 d = b & mask;
  181.       u32 temp = d >> bitcount;
  182.       if(!temp && addBase) {
  183.         temp += base;
  184.       }
  185.       data |= temp << bitwritecount;
  186.       bitwritecount += dataSize;
  187.       if(bitwritecount >= 32) {
  188.         CPUWriteMemory(dest, data);
  189.         dest += 4;
  190.         data = 0;
  191.         bitwritecount = 0;
  192.       }
  193.       mask <<= bits;
  194.       bitcount += bits;
  195.     }
  196.   }
  197. }
  198.  
  199. void BIOS_BgAffineSet()
  200. {
  201. #ifdef DEV_VERSION  
  202.   if(systemVerbose & VERBOSE_SWI) {
  203.     log("BgAffineSet: %08x,%08x,%08x (VCOUNT=%2d)\n",
  204.         reg[0].I,
  205.         reg[1].I,
  206.         reg[2].I,
  207.         VCOUNT);
  208.   }
  209. #endif
  210.   
  211.   u32 src = reg[0].I;
  212.   u32 dest = reg[1].I;
  213.   int num = reg[2].I;
  214.  
  215.   for(int i = 0; i < num; i++) {
  216.     s32 cx = CPUReadMemory(src);
  217.     src+=4;
  218.     s32 cy = CPUReadMemory(src);
  219.     src+=4;
  220.     s16 dispx = CPUReadHalfWord(src);
  221.     src+=2;
  222.     s16 dispy = CPUReadHalfWord(src);
  223.     src+=2;
  224.     s16 rx = CPUReadHalfWord(src);
  225.     src+=2;
  226.     s16 ry = CPUReadHalfWord(src);
  227.     src+=2;
  228.     u16 theta = CPUReadHalfWord(src)>>8;
  229.     src+=4; // keep structure alignment
  230.     s32 a = sineTable[(theta+0x40)&255];
  231.     s32 b = sineTable[theta];
  232.  
  233.     s16 dx =  (rx * a)>>14;
  234.     s16 dmx = (rx * b)>>14;
  235.     s16 dy =  (ry * b)>>14;
  236.     s16 dmy = (ry * a)>>14;
  237.     
  238.     CPUWriteHalfWord(dest, dx);
  239.     dest += 2;
  240.     CPUWriteHalfWord(dest, -dmx);
  241.     dest += 2;
  242.     CPUWriteHalfWord(dest, dy);
  243.     dest += 2;
  244.     CPUWriteHalfWord(dest, dmy);
  245.     dest += 2;
  246.  
  247.     s32 startx = cx - dx * dispx + dmx * dispy;
  248.     s32 starty = cy - dy * dispx - dmy * dispy;
  249.     
  250.     CPUWriteMemory(dest, startx);
  251.     dest += 4;
  252.     CPUWriteMemory(dest, starty);
  253.     dest += 4;
  254.   }
  255. }  
  256.  
  257. void BIOS_CpuSet()
  258. {
  259. #ifdef DEV_VERSION
  260.   if(systemVerbose & VERBOSE_SWI) {
  261.     log("CpuSet: 0x%08x,0x%08x,0x%08x (VCOUNT=%d)\n", reg[0].I, reg[1].I,
  262.         reg[2].I, VCOUNT);
  263.   }
  264. #endif
  265.   
  266.   u32 source = reg[0].I;
  267.   u32 dest = reg[1].I;
  268.   u32 cnt = reg[2].I;
  269.  
  270.   if(((source & 0xe000000) == 0) ||
  271.      ((source + (((cnt << 11)>>9) & 0x1fffff)) & 0xe000000) == 0)
  272.     return;
  273.  
  274.   int count = cnt & 0x1FFFFF;
  275.  
  276.   // 32-bit ?
  277.   if((cnt >> 26) & 1) {
  278.     // needed for 32-bit mode!
  279.     source &= 0xFFFFFFFC;
  280.     dest &= 0xFFFFFFFC;
  281.     // fill ?
  282.     if((cnt >> 24) & 1) {
  283.       u32 value = CPUReadMemory(source);
  284.       while(count) {
  285.         CPUWriteMemory(dest, value);
  286.         dest += 4;
  287.         count--;
  288.       }
  289.     } else {
  290.       // copy
  291.       while(count) {
  292.         CPUWriteMemory(dest, CPUReadMemory(source));
  293.         source += 4;
  294.         dest += 4;
  295.         count--;
  296.       }
  297.     }
  298.   } else {
  299.     // 16-bit fill?
  300.     if((cnt >> 24) & 1) {
  301.       u16 value = CPUReadHalfWord(source);
  302.       while(count) {
  303.         CPUWriteHalfWord(dest, value);
  304.         dest += 2;
  305.         count--;
  306.       }
  307.     } else {
  308.       // copy
  309.       while(count) {
  310.         CPUWriteHalfWord(dest, CPUReadHalfWord(source));
  311.         source += 2;
  312.         dest += 2;
  313.         count--;
  314.       }
  315.     }
  316.   }
  317. }
  318.  
  319. void BIOS_CpuFastSet()
  320. {
  321. #ifdef DEV_VERSION
  322.   if(systemVerbose & VERBOSE_SWI) {
  323.     log("CpuFastSet: 0x%08x,0x%08x,0x%08x (VCOUNT=%d)\n", reg[0].I, reg[1].I,
  324.         reg[2].I, VCOUNT);
  325.   }
  326. #endif
  327.   
  328.   u32 source = reg[0].I;
  329.   u32 dest = reg[1].I;
  330.   u32 cnt = reg[2].I;
  331.  
  332.   if(((source & 0xe000000) == 0) ||
  333.      ((source + (((cnt << 11)>>9) & 0x1fffff)) & 0xe000000) == 0)
  334.     return;
  335.  
  336.   // needed for 32-bit mode!
  337.   source &= 0xFFFFFFFC;
  338.   dest &= 0xFFFFFFFC;
  339.   
  340.   int count = cnt & 0x1FFFFF;
  341.   
  342.   // fill?
  343.   if((cnt >> 24) & 1) {
  344.     while(count > 0) {
  345.       // BIOS always transfers 32 bytes at a time
  346.       u32 value = CPUReadMemory(source);
  347.       for(int i = 0; i < 8; i++) {
  348.         CPUWriteMemory(dest, value);
  349.         dest += 4;
  350.       }
  351.       count -= 8;
  352.     }
  353.   } else {
  354.     // copy
  355.     while(count > 0) {
  356.       // BIOS always transfers 32 bytes at a time
  357.       for(int i = 0; i < 8; i++) {
  358.         CPUWriteMemory(dest, CPUReadMemory(source));
  359.         source += 4;
  360.         dest += 4;
  361.       }
  362.       count -= 8;
  363.     }
  364.   }
  365. }
  366.  
  367. void BIOS_Diff8bitUnFilterWram()
  368. {
  369. #ifdef DEV_VERSION
  370.   if(systemVerbose & VERBOSE_SWI) {
  371.     log("Diff8bitUnFilterWram: 0x%08x,0x%08x (VCOUNT=%d)\n", reg[0].I,
  372.         reg[1].I, VCOUNT);
  373.   }
  374. #endif
  375.   
  376.   u32 source = reg[0].I;
  377.   u32 dest = reg[1].I;
  378.  
  379.   u32 header = CPUReadMemory(source);
  380.   source += 4;
  381.  
  382.   if(((source & 0xe000000) == 0) ||
  383.      ((source + ((header >> 8) & 0x1fffff) & 0xe000000) == 0))
  384.     return;  
  385.  
  386.   int len = header >> 8;
  387.  
  388.   u8 data = CPUReadByte(source++);
  389.   CPUWriteByte(dest++, data);
  390.   len--;
  391.   
  392.   while(len > 0) {
  393.     u8 diff = CPUReadByte(source++);
  394.     data += diff;
  395.     CPUWriteByte(dest++, data);
  396.     len--;
  397.   }    
  398. }
  399.  
  400. void BIOS_Diff8bitUnFilterVram()
  401. {
  402. #ifdef DEV_VERSION
  403.   if(systemVerbose & VERBOSE_SWI) {
  404.     log("Diff8bitUnFilterVram: 0x%08x,0x%08x (VCOUNT=%d)\n", reg[0].I,
  405.         reg[1].I, VCOUNT);
  406.   }
  407. #endif
  408.   
  409.   u32 source = reg[0].I;
  410.   u32 dest = reg[1].I;
  411.  
  412.   u32 header = CPUReadMemory(source);
  413.   source += 4;
  414.  
  415.   if(((source & 0xe000000) == 0) ||
  416.      ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)
  417.     return;  
  418.   
  419.   int len = header >> 8;
  420.  
  421.   u8 data = CPUReadByte(source++);
  422.   u16 writeData = data;
  423.   int shift = 8;
  424.   int bytes = 1;
  425.   
  426.   while(len >= 2) {
  427.     u8 diff = CPUReadByte(source++);
  428.     data += diff;
  429.     writeData |= (data << shift);
  430.     bytes++;
  431.     shift += 8;
  432.     if(bytes == 2) {
  433.       CPUWriteHalfWord(dest, writeData);
  434.       dest += 2;
  435.       len -= 2;
  436.       bytes = 0;
  437.       writeData = 0;
  438.       shift = 0;
  439.     }
  440.   }  
  441. }
  442.  
  443. void BIOS_Diff16bitUnFilter()
  444. {
  445. #ifdef DEV_VERSION
  446.   if(systemVerbose & VERBOSE_SWI) {
  447.     log("Diff16bitUnFilter: 0x%08x,0x%08x (VCOUNT=%d)\n", reg[0].I,
  448.         reg[1].I, VCOUNT);
  449.   }
  450. #endif
  451.   
  452.   u32 source = reg[0].I;
  453.   u32 dest = reg[1].I;
  454.  
  455.   u32 header = CPUReadMemory(source);
  456.   source += 4;
  457.  
  458.   if(((source & 0xe000000) == 0) ||
  459.      ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)
  460.     return;  
  461.   
  462.   int len = header >> 8;
  463.  
  464.   u16 data = CPUReadHalfWord(source);
  465.   source += 2;
  466.   CPUWriteHalfWord(dest, data);
  467.   dest += 2;
  468.   len -= 2;
  469.   
  470.   while(len >= 2) {
  471.     u16 diff = CPUReadHalfWord(source);
  472.     source += 2;
  473.     data += diff;
  474.     CPUWriteHalfWord(dest, data);
  475.     dest += 2;
  476.     len -= 2;
  477.   }
  478. }
  479.  
  480. void BIOS_Div()
  481. {
  482. #ifdef DEV_VERSION
  483.   if(systemVerbose & VERBOSE_SWI) {
  484.     log("Div: 0x%08x,0x%08x (VCOUNT=%d)\n",
  485.         reg[0].I,
  486.         reg[1].I,
  487.         VCOUNT);
  488.   }
  489. #endif
  490.   
  491.   int number = reg[0].I;
  492.   int denom = reg[1].I;
  493.  
  494.   if(denom != 0) {
  495.     reg[0].I = number / denom;
  496.     reg[1].I = number % denom;
  497.     s32 temp = (s32)reg[0].I;
  498.     reg[3].I = temp < 0 ? (u32)-temp : (u32)temp;
  499.   }
  500. #ifdef DEV_VERSION
  501.   if(systemVerbose & VERBOSE_SWI) {
  502.     log("Div: return=0x%08x,0x%08x,0x%08x\n",
  503.         reg[0].I,
  504.         reg[1].I,
  505.         reg[3].I);
  506.   }
  507. #endif
  508. }
  509.  
  510. void BIOS_DivARM()
  511. {
  512. #ifdef DEV_VERSION
  513.   if(systemVerbose & VERBOSE_SWI) {
  514.     log("DivARM: 0x%08x, (VCOUNT=%d)\n",
  515.         reg[0].I,
  516.         VCOUNT);
  517.   }
  518. #endif
  519.   
  520.   u32 temp = reg[0].I;
  521.   reg[0].I = reg[1].I;
  522.   reg[1].I = temp;
  523.   BIOS_Div();
  524. }
  525.  
  526. void BIOS_HuffUnComp()
  527. {
  528. #ifdef DEV_VERSION
  529.   if(systemVerbose & VERBOSE_SWI) {
  530.     log("HuffUnComp: 0x%08x,0x%08x (VCOUNT=%d)\n",
  531.         reg[0].I,
  532.         reg[1].I,
  533.         VCOUNT);
  534.   }
  535. #endif
  536.   
  537.   u32 source = reg[0].I;
  538.   u32 dest = reg[1].I;
  539.  
  540.   u32 header = CPUReadMemory(source);
  541.   source += 4;
  542.  
  543.   if(((source & 0xe000000) == 0) ||
  544.      ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)
  545.     return;  
  546.   
  547.   u8 treeSize = CPUReadByte(source++);
  548.  
  549.   u32 treeStart = source;
  550.  
  551.   source += (treeSize<<1) + 1;
  552.   
  553.   int len = header >> 8;
  554.  
  555.   u32 mask = 0x80000000;
  556.   u32 data = CPUReadMemory(source);
  557.   source += 4;
  558.  
  559.   int pos = 0;
  560.   u8 rootNode = CPUReadByte(treeStart);
  561.   u8 currentNode = rootNode;
  562.   bool writeData = false;
  563.   int byteShift = 0;
  564.   int byteCount = 0;
  565.   u32 writeValue = 0;
  566.  
  567.   if((header & 0x0F) == 8) {
  568.     while(len > 0) {
  569.       // take left
  570.       if(pos == 0)
  571.         pos++;
  572.       else
  573.         pos += (((currentNode & 0x3F)+1)<<1);
  574.       
  575.       if(data & mask) {
  576.         // right
  577.         if(currentNode & 0x40)
  578.           writeData = true;
  579.         currentNode = CPUReadByte(treeStart+pos+1);
  580.       } else {
  581.         // left
  582.         if(currentNode & 0x80)
  583.           writeData = true;
  584.         currentNode = CPUReadByte(treeStart+pos);
  585.       }
  586.       
  587.       if(writeData) {
  588.         writeValue |= (currentNode << byteShift);
  589.         byteCount++;
  590.         byteShift += 8;
  591.  
  592.         pos = 0;
  593.         currentNode = rootNode;
  594.         writeData = false;
  595.  
  596.         if(byteCount == 4) {
  597.           byteCount = 0;
  598.           byteShift = 0;
  599.           CPUWriteMemory(dest, writeValue);
  600.           writeValue = 0;
  601.           dest += 4;
  602.           len -= 4;
  603.         }
  604.       }
  605.       mask >>= 1;
  606.       if(mask == 0) {
  607.         mask = 0x80000000;
  608.         data = CPUReadMemory(source);
  609.         source += 4;
  610.       }
  611.     }
  612.   } else {
  613.     int halfLen = 0;
  614.     int value = 0;
  615.     while(len > 0) {
  616.       // take left
  617.       if(pos == 0)
  618.         pos++;
  619.       else
  620.         pos += (((currentNode & 0x3F)+1)<<1);
  621.  
  622.       if((data & mask)) {
  623.         // right
  624.         if(currentNode & 0x40)
  625.           writeData = true;
  626.         currentNode = CPUReadByte(treeStart+pos+1);
  627.       } else {
  628.         // left
  629.         if(currentNode & 0x80)
  630.           writeData = true;
  631.         currentNode = CPUReadByte(treeStart+pos);
  632.       }
  633.       
  634.       if(writeData) {
  635.         if(halfLen == 0)
  636.           value |= currentNode;
  637.         else
  638.           value |= (currentNode<<4);
  639.  
  640.         halfLen += 4;
  641.         if(halfLen == 8) {
  642.           writeValue |= (value << byteShift);
  643.           byteCount++;
  644.           byteShift += 8;
  645.           
  646.           halfLen = 0;
  647.           value = 0;
  648.  
  649.           if(byteCount == 4) {
  650.             byteCount = 0;
  651.             byteShift = 0;
  652.             CPUWriteMemory(dest, writeValue);
  653.             dest += 4;
  654.             writeValue = 0;
  655.             len -= 4;
  656.           }
  657.         }
  658.         pos = 0;
  659.         currentNode = rootNode;
  660.         writeData = false;
  661.       }
  662.       mask >>= 1;
  663.       if(mask == 0) {
  664.         mask = 0x80000000;
  665.         data = CPUReadMemory(source);
  666.         source += 4;
  667.       }
  668.     }    
  669.   }
  670. }
  671.  
  672. void BIOS_LZ77UnCompVram()
  673. {
  674. #ifdef DEV_VERSION
  675.   if(systemVerbose & VERBOSE_SWI) {
  676.     log("LZ77UnCompVram: 0x%08x,0x%08x (VCOUNT=%d)\n",
  677.         reg[0].I,
  678.         reg[1].I,
  679.         VCOUNT);
  680.   }
  681. #endif
  682.   
  683.   u32 source = reg[0].I;
  684.   u32 dest = reg[1].I;
  685.  
  686.   u32 header = CPUReadMemory(source);
  687.   source += 4;
  688.  
  689.   if(((source & 0xe000000) == 0) ||
  690.      ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)
  691.     return;    
  692.   
  693.   int byteCount = 0;
  694.   int byteShift = 0;
  695.   u32 writeValue = 0;
  696.   
  697.   int len = header >> 8;
  698.  
  699.   while(len > 0) {
  700.     u8 d = CPUReadByte(source++);
  701.  
  702.     if(d) {
  703.       for(int i = 0; i < 8; i++) {
  704.         if(d & 0x80) {
  705.           u16 data = CPUReadByte(source++) << 8;
  706.           data |= CPUReadByte(source++);
  707.           int length = (data >> 12) + 3;
  708.           int offset = (data & 0x0FFF);
  709.           u32 windowOffset = dest + byteCount - offset - 1;
  710.           for(int i = 0; i < length; i++) {
  711.             writeValue |= (CPUReadByte(windowOffset++) << byteShift);
  712.             byteShift += 8;
  713.             byteCount++;
  714.  
  715.             if(byteCount == 2) {
  716.               CPUWriteHalfWord(dest, writeValue);
  717.               dest += 2;
  718.               byteCount = 0;
  719.               byteShift = 0;
  720.               writeValue = 0;
  721.             }
  722.             len--;
  723.             if(len == 0)
  724.               return;
  725.           }
  726.         } else {
  727.           writeValue |= (CPUReadByte(source++) << byteShift);
  728.           byteShift += 8;
  729.           byteCount++;
  730.           if(byteCount == 2) {
  731.             CPUWriteHalfWord(dest, writeValue);
  732.             dest += 2;
  733.             byteCount = 0;
  734.             byteShift = 0;
  735.             writeValue = 0;
  736.           }
  737.           len--;
  738.           if(len == 0)
  739.             return;
  740.         }
  741.         d <<= 1;
  742.       }
  743.     } else {
  744.       for(int i = 0; i < 8; i++) {
  745.         writeValue |= (CPUReadByte(source++) << byteShift);
  746.         byteShift += 8;
  747.         byteCount++;
  748.         if(byteCount == 2) {
  749.           CPUWriteHalfWord(dest, writeValue);
  750.           dest += 2;      
  751.           byteShift = 0;
  752.           byteCount = 0;
  753.           writeValue = 0;
  754.         }
  755.         len--;
  756.         if(len == 0)
  757.           return;
  758.       }
  759.     }
  760.   }
  761. }
  762.  
  763. void BIOS_LZ77UnCompWram()
  764. {
  765. #ifdef DEV_VERSION
  766.   if(systemVerbose & VERBOSE_SWI) {
  767.     log("LZ77UnCompWram: 0x%08x,0x%08x (VCOUNT=%d)\n", reg[0].I, reg[1].I,
  768.         VCOUNT);
  769.   }
  770. #endif
  771.   
  772.   u32 source = reg[0].I;
  773.   u32 dest = reg[1].I;
  774.  
  775.   u32 header = CPUReadMemory(source);
  776.   source += 4;
  777.  
  778.   if(((source & 0xe000000) == 0) ||
  779.      ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)
  780.     return;  
  781.   
  782.   int len = header >> 8;
  783.  
  784.   while(len > 0) {
  785.     u8 d = CPUReadByte(source++);
  786.  
  787.     if(d) {
  788.       for(int i = 0; i < 8; i++) {
  789.         if(d & 0x80) {
  790.           u16 data = CPUReadByte(source++) << 8;
  791.           data |= CPUReadByte(source++);
  792.           int length = (data >> 12) + 3;
  793.           int offset = (data & 0x0FFF);
  794.           u32 windowOffset = dest - offset - 1;
  795.           for(int i = 0; i < length; i++) {
  796.             CPUWriteByte(dest++, CPUReadByte(windowOffset++));
  797.             len--;
  798.             if(len == 0)
  799.               return;
  800.           }
  801.         } else {
  802.           CPUWriteByte(dest++, CPUReadByte(source++));
  803.           len--;
  804.           if(len == 0)
  805.             return;
  806.         }
  807.         d <<= 1;
  808.       }
  809.     } else {
  810.       for(int i = 0; i < 8; i++) {
  811.         CPUWriteByte(dest++, CPUReadByte(source++));
  812.         len--;
  813.         if(len == 0)
  814.           return;
  815.       }
  816.     }
  817.   }
  818. }
  819.  
  820. void BIOS_ObjAffineSet()
  821. {
  822. #ifdef DEV_VERSION
  823.   if(systemVerbose & VERBOSE_SWI) {
  824.     log("ObjAffineSet: 0x%08x,0x%08x,0x%08x,0x%08x (VCOUNT=%d)\n",
  825.         reg[0].I,
  826.         reg[1].I,
  827.         reg[2].I,
  828.         reg[3].I,
  829.         VCOUNT);
  830.   }
  831. #endif
  832.   
  833.   u32 src = reg[0].I;
  834.   u32 dest = reg[1].I;
  835.   int num = reg[2].I;
  836.   int offset = reg[3].I;
  837.  
  838.   for(int i = 0; i < num; i++) {
  839.     s16 rx = CPUReadHalfWord(src);
  840.     src+=2;
  841.     s16 ry = CPUReadHalfWord(src);
  842.     src+=2;
  843.     u16 theta = CPUReadHalfWord(src)>>8;
  844.     src+=4; // keep structure alignment
  845.  
  846.     s32 a = (s32)sineTable[(theta+0x40)&255];
  847.     s32 b = (s32)sineTable[theta];
  848.  
  849.     s16 dx =  ((s32)rx * a)>>14;
  850.     s16 dmx = ((s32)rx * b)>>14;
  851.     s16 dy =  ((s32)ry * b)>>14;
  852.     s16 dmy = ((s32)ry * a)>>14;
  853.     
  854.     CPUWriteHalfWord(dest, dx);
  855.     dest += offset;
  856.     CPUWriteHalfWord(dest, -dmx);
  857.     dest += offset;
  858.     CPUWriteHalfWord(dest, dy);
  859.     dest += offset;
  860.     CPUWriteHalfWord(dest, dmy);
  861.     dest += offset;
  862.   }
  863. }
  864.  
  865. void BIOS_RegisterRamReset(u32 flags)
  866. {
  867.   // no need to trace here. this is only called directly from GBA.cpp
  868.   // to emulate bios initialization
  869.   
  870.   if(flags) {
  871.     if(flags & 0x01) {
  872.       // clear work RAM
  873.       memset(workRAM, 0, 0x40000);
  874.     }
  875.     if(flags & 0x02) {
  876.       // clear internal RAM
  877.       memset(internalRAM, 0, 0x7e00); // don't clear 0x7e00-0x7fff
  878.     }
  879.     if(flags & 0x04) {
  880.       // clear palette RAM
  881.       memset(paletteRAM, 0, 0x400);
  882.     }
  883.     if(flags & 0x08) {
  884.       // clear VRAM
  885.       memset(vram, 0, 0x18000);
  886.     }
  887.     if(flags & 0x10) {
  888.       // clean OAM
  889.       memset(oam, 0, 0x400);
  890.     }
  891.  
  892.     if(flags & 0x80) {
  893.       int i;
  894.       for(i = 0; i < 8; i++)
  895.         CPUUpdateRegister(0x200+i*2, 0);
  896.  
  897.       CPUUpdateRegister(0x202, 0xFFFF);
  898.  
  899.       for(i = 0; i < 8; i++)
  900.         CPUUpdateRegister(0x4+i*2, 0);
  901.  
  902.       for(i = 0; i < 16; i++)
  903.         CPUUpdateRegister(0x20+i*2, 0);
  904.  
  905.       for(i = 0; i < 24; i++)
  906.         CPUUpdateRegister(0xb0+i*2, 0);
  907.  
  908.       CPUUpdateRegister(0x130, 0);
  909.       CPUUpdateRegister(0x20, 0x100);
  910.       CPUUpdateRegister(0x30, 0x100);
  911.       CPUUpdateRegister(0x26, 0x100);
  912.       CPUUpdateRegister(0x36, 0x100);
  913.     }
  914.     
  915.     if(flags & 0x20) {
  916.       int i;
  917.       for(i = 0; i < 8; i++)
  918.         CPUUpdateRegister(0x110+i*2, 0);
  919.       CPUUpdateRegister(0x134, 0x8000);
  920.       for(i = 0; i < 7; i++)
  921.         CPUUpdateRegister(0x140+i*2, 0);
  922.     }
  923.  
  924.     if(flags & 0x40) {
  925.       int i;
  926.       CPUWriteByte(0x4000084, 0);
  927.       CPUWriteByte(0x4000084, 0x80);
  928.       CPUWriteMemory(0x4000080, 0x880e0000);
  929.       CPUUpdateRegister(0x88, CPUReadHalfWord(0x4000088)&0x3ff);
  930.       CPUWriteByte(0x4000070, 0x70);
  931.       for(i = 0; i < 8; i++)
  932.         CPUUpdateRegister(0x90+i*2, 0);
  933.       CPUWriteByte(0x4000070, 0);
  934.       for(i = 0; i < 8; i++)
  935.         CPUUpdateRegister(0x90+i*2, 0);
  936.       CPUWriteByte(0x4000084, 0);
  937.     }
  938.   }
  939. }
  940.  
  941. void BIOS_RegisterRamReset()
  942. {
  943. #ifdef DEV_VERSION
  944.   if(systemVerbose & VERBOSE_SWI) {
  945.     log("RegisterRamReset: 0x%08x (VCOUNT=%d)\n",
  946.         reg[0].I,
  947.         VCOUNT);
  948.   }
  949. #endif
  950.  
  951.   BIOS_RegisterRamReset(reg[0].I);
  952. }
  953.  
  954. void BIOS_RLUnCompVram()
  955. {
  956. #ifdef DEV_VERSION
  957.   if(systemVerbose & VERBOSE_SWI) {
  958.     log("RLUnCompVram: 0x%08x,0x%08x (VCOUNT=%d)\n",
  959.         reg[0].I,
  960.         reg[1].I,
  961.         VCOUNT);
  962.   }
  963. #endif
  964.   
  965.   u32 source = reg[0].I;
  966.   u32 dest = reg[1].I;
  967.  
  968.   u32 header = CPUReadMemory(source);
  969.   source += 4;
  970.  
  971.   if(((source & 0xe000000) == 0) ||
  972.      ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)
  973.     return;  
  974.   
  975.   int len = header >> 8;
  976.   int byteCount = 0;
  977.   int byteShift = 0;
  978.   u32 writeValue = 0;
  979.  
  980.   while(len > 0) {
  981.     u8 d = CPUReadByte(source++);
  982.     int l = d & 0x7F;
  983.     if(d & 0x80) {
  984.       u8 data = CPUReadByte(source++);
  985.       l += 3;
  986.       for(int i = 0;i < l; i++) {
  987.         writeValue |= (data << byteShift);
  988.         byteShift += 8;
  989.         byteCount++;
  990.  
  991.         if(byteCount == 2) {
  992.           CPUWriteHalfWord(dest, writeValue);
  993.           dest += 2;
  994.           byteCount = 0;
  995.           byteShift = 0;
  996.           writeValue = 0;
  997.         }
  998.         len--;
  999.         if(len == 0)
  1000.           return;
  1001.       }
  1002.     } else {
  1003.       l++;
  1004.       for(int i = 0; i < l; i++) {
  1005.         writeValue |= (CPUReadByte(source++) << byteShift);
  1006.         byteShift += 8;
  1007.         byteCount++;
  1008.         if(byteCount == 2) {
  1009.           CPUWriteHalfWord(dest, writeValue);
  1010.           dest += 2;
  1011.           byteCount = 0;
  1012.           byteShift = 0;
  1013.           writeValue = 0;
  1014.         }
  1015.         len--;
  1016.         if(len == 0)
  1017.           return;
  1018.       }
  1019.     }
  1020.   }
  1021. }
  1022.  
  1023. void BIOS_RLUnCompWram()
  1024. {
  1025. #ifdef DEV_VERSION
  1026.   if(systemVerbose & VERBOSE_SWI) {
  1027.     log("RLUnCompWram: 0x%08x,0x%08x (VCOUNT=%d)\n",
  1028.         reg[0].I,
  1029.         reg[1].I,
  1030.         VCOUNT);
  1031.   }
  1032. #endif
  1033.   
  1034.   u32 source = reg[0].I;
  1035.   u32 dest = reg[1].I;
  1036.  
  1037.   u32 header = CPUReadMemory(source);
  1038.   source += 4;
  1039.  
  1040.   if(((source & 0xe000000) == 0) ||
  1041.      ((source + ((header >> 8) & 0x1fffff)) & 0xe000000) == 0)
  1042.     return;  
  1043.   
  1044.   int len = header >> 8;
  1045.  
  1046.   while(len > 0) {
  1047.     u8 d = CPUReadByte(source++);
  1048.     int l = d & 0x7F;
  1049.     if(d & 0x80) {
  1050.       u8 data = CPUReadByte(source++);
  1051.       l += 3;
  1052.       for(int i = 0;i < l; i++) {
  1053.         CPUWriteByte(dest++, data);
  1054.         len--;
  1055.         if(len == 0)
  1056.           return;
  1057.       }
  1058.     } else {
  1059.       l++;
  1060.       for(int i = 0; i < l; i++) {
  1061.         CPUWriteByte(dest++,  CPUReadByte(source++));
  1062.         len--;
  1063.         if(len == 0)
  1064.           return;
  1065.       }
  1066.     }
  1067.   }
  1068. }
  1069.  
  1070. void BIOS_SoftReset()
  1071. {
  1072. #ifdef DEV_VERSION
  1073.   if(systemVerbose & VERBOSE_SWI) {
  1074.     log("SoftReset: (VCOUNT=%d)\n", VCOUNT);
  1075.   }
  1076. #endif
  1077.  
  1078.   armState = true;
  1079.   armMode = 0x1F;
  1080.   armIrqEnable = false;
  1081.   C_FLAG = V_FLAG = N_FLAG = Z_FLAG = false;
  1082.   reg[13].I = 0x03007F00;
  1083.   reg[14].I = 0x00000000;
  1084.   reg[16].I = 0x00000000;
  1085.   reg[R13_IRQ].I = 0x03007FA0;
  1086.   reg[R14_IRQ].I = 0x00000000;
  1087.   reg[SPSR_IRQ].I = 0x00000000;
  1088.   reg[R13_SVC].I = 0x03007FE0;  
  1089.   reg[R14_SVC].I = 0x00000000;
  1090.   reg[SPSR_SVC].I = 0x00000000;
  1091.   u8 b = internalRAM[0x7ffa];
  1092.  
  1093.   memset(&internalRAM[0x7e00], 0, 0x200);
  1094.  
  1095.   if(b) {
  1096.     armNextPC = 0x02000000;
  1097.     reg[15].I = 0x02000004;
  1098.   } else {
  1099.     armNextPC = 0x08000000;
  1100.     reg[15].I = 0x08000004;
  1101.   }
  1102. }
  1103.  
  1104. void BIOS_Sqrt()
  1105. {
  1106. #ifdef DEV_VERSION
  1107.   if(systemVerbose & VERBOSE_SWI) {
  1108.     log("Sqrt: %08x (VCOUNT=%2d)\n",
  1109.         reg[0].I,
  1110.         VCOUNT);
  1111.   }
  1112. #endif
  1113.   reg[0].I = (u32)sqrt((double)reg[0].I);
  1114. #ifdef DEV_VERSION
  1115.   if(systemVerbose & VERBOSE_SWI) {
  1116.     log("Sqrt: return=%08x\n",
  1117.         reg[0].I);
  1118.   }
  1119. #endif
  1120. }
  1121.  
  1122. void BIOS_MidiKey2Freq()
  1123. {
  1124. #ifdef DEV_VERSION
  1125.   if(systemVerbose & VERBOSE_SWI) {
  1126.     log("MidiKey2Freq: WaveData=%08x mk=%08x fp=%08x\n",
  1127.         reg[0].I,
  1128.         reg[1].I,
  1129.         reg[2].I);
  1130.   }
  1131. #endif
  1132.   int freq = CPUReadMemory(reg[0].I+4);
  1133.   double tmp;
  1134.   tmp = ((double)(180 - reg[1].I)) - ((double)reg[2].I / 256.f);
  1135.   tmp = pow((double)2.f, tmp / 12.f);
  1136.   reg[0].I = (int)((double)freq / tmp);
  1137.  
  1138. #ifdef DEV_VERSION
  1139.   if(systemVerbose & VERBOSE_SWI) {
  1140.     log("MidiKey2Freq: return %08x\n",
  1141.         reg[0].I);
  1142.   }
  1143. #endif
  1144. }
  1145.  
  1146. void BIOS_SndDriverJmpTableCopy()
  1147. {
  1148. #ifdef DEV_VERSION
  1149.   if(systemVerbose & VERBOSE_SWI) {
  1150.     log("SndDriverJmpTableCopy: dest=%08x\n",
  1151.         reg[0].I);
  1152.   }
  1153. #endif
  1154.   for(int i = 0; i < 0x24; i++) {
  1155.     CPUWriteMemory(reg[0].I, 0x9c);
  1156.     reg[0].I += 4;
  1157.   }
  1158. }
  1159.