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