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); } } } }