GBA.Unity/Assets/emulator/Nds9Math.cs

276 lines
9.9 KiB
C#

using System;
using static OptimeGBA.Bits;
namespace OptimeGBA
{
public unsafe sealed class Nds9Math
{
public Nds Nds;
public Nds9Math(Nds nds)
{
Nds = nds;
}
public long DIV_NUMER;
public long DIV_DENOM;
public long DIV_RESULT;
public long DIVREM_RESULT;
public uint SQRT_RESULT;
public ulong SQRT_PARAM;
// DIVCNT
public uint DivisionMode;
public bool DividedByZero;
public bool DivideBusy;
// SQRTCNT
public bool SqrtUse64BitInput;
public bool SqrtBusy;
public byte ReadHwio8(uint addr)
{
byte val = 0;
switch (addr)
{
case 0x4000280: // DIVCNT B0
val |= (byte)(DivisionMode & 0b11);
break;
case 0x4000281: // DIVCNT B1
if (DividedByZero) val = BitSet(val, 6);
if (DivideBusy) val = BitSet(val, 7);
break;
case 0x4000282: // DIVCNT B2
case 0x4000283: // DIVCNT B3
break;
case 0x4000290: // DIV_NUMER B0
case 0x4000291: // DIV_NUMER B1
case 0x4000292: // DIV_NUMER B2
case 0x4000293: // DIV_NUMER B3
case 0x4000294: // DIV_NUMER B4
case 0x4000295: // DIV_NUMER B5
case 0x4000296: // DIV_NUMER B6
case 0x4000297: // DIV_NUMER B7
return GetByteIn(DIV_NUMER, addr & 7);
case 0x4000298: // DIV_DENOM B0
case 0x4000299: // DIV_DENOM B1
case 0x400029A: // DIV_DENOM B2
case 0x400029B: // DIV_DENOM B3
case 0x400029C: // DIV_DENOM B4
case 0x400029D: // DIV_DENOM B5
case 0x400029E: // DIV_DENOM B6
case 0x400029F: // DIV_DENOM B7
return GetByteIn(DIV_DENOM, addr & 7);
case 0x40002A0: // DIV_RESULT B0
case 0x40002A1: // DIV_RESULT B1
case 0x40002A2: // DIV_RESULT B2
case 0x40002A3: // DIV_RESULT B3
case 0x40002A4: // DIV_RESULT B4
case 0x40002A5: // DIV_RESULT B5
case 0x40002A6: // DIV_RESULT B6
case 0x40002A7: // DIV_RESULT B7
return GetByteIn(DIV_RESULT, addr & 7);
case 0x40002A8: // DIVREM_RESULT B0
case 0x40002A9: // DIVREM_RESULT B1
case 0x40002AA: // DIVREM_RESULT B2
case 0x40002AB: // DIVREM_RESULT B3
case 0x40002AC: // DIVREM_RESULT B4
case 0x40002AD: // DIVREM_RESULT B5
case 0x40002AE: // DIVREM_RESULT B6
case 0x40002AF: // DIVREM_RESULT B7
return GetByteIn(DIVREM_RESULT, addr & 7);
case 0x40002B0: // SQRTCNT B0
if (SqrtUse64BitInput) val = BitSet(val, 0);
break;
case 0x40002B1: // SQRTCNT B0
break;
case 0x40002B4: // SQRT_RESULT B0
case 0x40002B5: // SQRT_RESULT B1
case 0x40002B6: // SQRT_RESULT B2
case 0x40002B7: // SQRT_RESULT B3
return GetByteIn(SQRT_RESULT, addr & 3);
case 0x40002B8: // SQRT_PARAM B0
case 0x40002B9: // SQRT_PARAM B1
case 0x40002BA: // SQRT_PARAM B2
case 0x40002BB: // SQRT_PARAM B3
case 0x40002BC: // SQRT_PARAM B4
case 0x40002BD: // SQRT_PARAM B5
case 0x40002BE: // SQRT_PARAM B6
case 0x40002BF: // SQRT_PARAM B7
return GetByteIn(SQRT_PARAM, addr & 7);
default:
throw new NotImplementedException("Read from DS math @ " + Util.Hex(addr, 8));
}
return val;
}
public void WriteHwio8(uint addr, byte val)
{
switch (addr)
{
case 0x4000280: // DIVCNT B0
DivisionMode = (byte)(val & 0b11);
Divide();
break;
case 0x4000281: // DIVCNT B1
case 0x4000282: // DIVCNT B2
case 0x4000283: // DIVCNT B3
break;
case 0x4000290: // DIV_NUMER B0
case 0x4000291: // DIV_NUMER B1
case 0x4000292: // DIV_NUMER B2
case 0x4000293: // DIV_NUMER B3
case 0x4000294: // DIV_NUMER B4
case 0x4000295: // DIV_NUMER B5
case 0x4000296: // DIV_NUMER B6
case 0x4000297: // DIV_NUMER B7
DIV_NUMER = SetByteIn(DIV_NUMER, val, addr & 7);
Divide();
break;
case 0x4000298: // DIV_DENOM B0
case 0x4000299: // DIV_DENOM B1
case 0x400029A: // DIV_DENOM B2
case 0x400029B: // DIV_DENOM B3
case 0x400029C: // DIV_DENOM B4
case 0x400029D: // DIV_DENOM B5
case 0x400029E: // DIV_DENOM B6
case 0x400029F: // DIV_DENOM B7
DIV_DENOM = SetByteIn(DIV_DENOM, val, addr & 7);
Divide();
break;
case 0x40002B0: // SQRTCNT B0
SqrtUse64BitInput = BitTest(val, 0);
TakeSquareRoot();
break;
case 0x40002B1: // SQRTCNT B0
break;
case 0x40002B8: // SQRT_PARAM B0
case 0x40002B9: // SQRT_PARAM B1
case 0x40002BA: // SQRT_PARAM B2
case 0x40002BB: // SQRT_PARAM B3
case 0x40002BC: // SQRT_PARAM B4
case 0x40002BD: // SQRT_PARAM B5
case 0x40002BE: // SQRT_PARAM B6
case 0x40002BF: // SQRT_PARAM B7
SQRT_PARAM = SetByteIn(SQRT_PARAM, val, addr & 7);
TakeSquareRoot();
return;
// default:
// throw new NotImplementedException("Write to DS math @ " + Util.Hex(addr, 8));
}
}
public void Divide()
{
DividedByZero = DIV_DENOM == 0;
switch (DivisionMode)
{
case 0: // 32bit / 32bit
if ((int)DIV_NUMER == int.MinValue && (int)DIV_DENOM == -1) // Overflow
{
DIV_RESULT = (long)(int)DIV_NUMER ^ (0xFFFFFFFFL << 32);
DIVREM_RESULT = 0;
}
else if ((int)DIV_DENOM != 0)
{
DIV_RESULT = (int)DIV_NUMER / (int)DIV_DENOM;
DIVREM_RESULT = (int)DIV_NUMER % (int)DIV_DENOM;
}
else // Division by 0
{
DIV_RESULT = (((int)DIV_NUMER < 0) ? 1 : -1) ^ (0xFFFFFFFFL << 32);
DIVREM_RESULT = (int)DIV_NUMER;
}
break;
case 3:
case 1: // 64bit / 32bit
if (DIV_NUMER == long.MinValue && (int)DIV_DENOM == -1) // Overflow
{
DIV_RESULT = DIV_NUMER;
DIVREM_RESULT = 0;
}
else if ((int)DIV_DENOM != 0)
{
DIV_RESULT = DIV_NUMER / (int)DIV_DENOM;
DIVREM_RESULT = DIV_NUMER % (int)DIV_DENOM;
}
else // Division by 0
{
DIV_RESULT = (DIV_NUMER < 0) ? 1 : -1;
DIVREM_RESULT = DIV_NUMER;
}
break;
case 2: // 64bit / 64bit
if (DIV_NUMER == long.MinValue && DIV_DENOM == -1) // Overflow
{
DIV_RESULT = DIV_NUMER;
DIVREM_RESULT = 0;
}
else if (DIV_DENOM != 0)
{
DIV_RESULT = DIV_NUMER / DIV_DENOM;
DIVREM_RESULT = DIV_NUMER % DIV_DENOM;
}
else // Division by 0
{
DIV_RESULT = (DIV_NUMER < 0) ? 1 : -1;
DIVREM_RESULT = DIV_NUMER;
}
break;
}
// Console.WriteLine("Divison Mode: " + DivisionMode);
// Console.WriteLine("Numerator : " + DIV_NUMER);
// Console.WriteLine("Demoninator: " + DIV_DENOM);
// Console.WriteLine("Result : " + DIV_RESULT);
// Console.WriteLine("Remainder : " + DIVREM_RESULT);
}
public void TakeSquareRoot()
{
if (SqrtUse64BitInput)
{
ulong val = SQRT_PARAM;
uint final = 0;
ulong rem = 0;
uint prod = 0;
const uint nbits = 32;
const int topShift = 62;
for (int i = 0; i < nbits; i++)
{
rem = (rem << 2) + ((val >> topShift) & 0x3);
val <<= 2;
final <<= 1;
prod = (final << 1) + 1;
if (rem >= prod)
{
rem -= prod;
final++;
}
}
SQRT_RESULT = final;
}
else
{
SQRT_RESULT = (uint)Math.Sqrt((uint)SQRT_PARAM);
}
}
}
}