diff --git a/include/mgba/internal/ds/ds.h b/include/mgba/internal/ds/ds.h index 84a7f4348..435f4896e 100644 --- a/include/mgba/internal/ds/ds.h +++ b/include/mgba/internal/ds/ds.h @@ -103,6 +103,7 @@ struct DS { struct mCoreCallbacks* coreCallbacks; struct mTimingEvent divEvent; + struct mTimingEvent sqrtEvent; }; struct DSCartridge { diff --git a/src/ds/ds.c b/src/ds/ds.c index 784242bd5..53a6c359e 100644 --- a/src/ds/ds.c +++ b/src/ds/ds.c @@ -121,6 +121,37 @@ static void _divide(struct mTiming* timing, void* context, uint32_t cyclesLate) STORE_64LE(remainder, DS9_REG_DIVREM_RESULT_0, ds->memory.io9); } + static void _sqrt(struct mTiming* timing, void* context, uint32_t cyclesLate) { + UNUSED(timing); + UNUSED(cyclesLate); + struct DS* ds = context; + ds->memory.io9[DS9_REG_SQRTCNT >> 1] &= ~0x8000; + uint64_t param; + LOAD_64LE(param, DS9_REG_SQRT_PARAM_0, ds->memory.io9); + if (!(ds->memory.io9[DS9_REG_SQRTCNT >> 1] & 1)) { + param &= 0xFFFFFFFFULL; + } + + uint64_t result = 0; + uint64_t bit = 0x4000000000000000ULL; // The second-to-top bit is set: 1 << 30 for 32 bits + + // "bit" starts at the highest power of four <= the argument. + while (bit > param) { + bit >>= 2; + } + + while (bit != 0) { + if (param >= param + bit) { + param -= param + bit; + param = (result >> 1) + bit; + } else { + param >>= 1; + } + bit >>= 2; + } + STORE_32LE(result, DS9_REG_SQRT_RESULT_LO, ds->memory.io9); +} + void DSCreate(struct DS* ds) { ds->d.id = DS_COMPONENT_MAGIC; ds->d.init = DSInit; @@ -180,6 +211,11 @@ static void DSInit(void* cpu, struct mCPUComponent* component) { ds->divEvent.context = ds; ds->divEvent.priority = 0x50; + ds->sqrtEvent.name = "DS Hardware Sqrt"; + ds->sqrtEvent.callback = _sqrt; + ds->sqrtEvent.context = ds; + ds->sqrtEvent.priority = 0x51; + mTimingInit(&ds->ds7.timing, &ds->ds7.cpu->cycles, &ds->ds7.cpu->nextEvent); mTimingInit(&ds->ds9.timing, &ds->ds9.cpu->cycles, &ds->ds9.cpu->nextEvent); } diff --git a/src/ds/io.c b/src/ds/io.c index 69e9f9cf6..6a29e62ca 100644 --- a/src/ds/io.c +++ b/src/ds/io.c @@ -34,6 +34,12 @@ static uint16_t _scheduleDiv(struct DS* ds, uint16_t control) { return control | 0x8000; } +static uint16_t _scheduleSqrt(struct DS* ds, uint16_t control) { + mTimingDeschedule(&ds->ds9.timing, &ds->sqrtEvent); + mTimingSchedule(&ds->ds9.timing, &ds->sqrtEvent, 26); + return control | 0x8000; +} + static uint32_t DSIOWrite(struct DSCommon* dscore, uint32_t address, uint16_t value) { switch (address) { // Video @@ -333,6 +339,15 @@ void DS9IOWrite(struct DS* ds, uint32_t address, uint16_t value) { case DS9_REG_DIV_DENOM_3: ds->memory.io9[DS9_REG_DIVCNT >> 1] = _scheduleDiv(ds, ds->memory.io9[DS9_REG_DIVCNT >> 1]); break; + case DS9_REG_SQRTCNT: + value = _scheduleSqrt(ds, value); + break; + case DS9_REG_SQRT_PARAM_0: + case DS9_REG_SQRT_PARAM_1: + case DS9_REG_SQRT_PARAM_2: + case DS9_REG_SQRT_PARAM_3: + ds->memory.io9[DS9_REG_SQRTCNT >> 1] = _scheduleSqrt(ds, ds->memory.io9[DS9_REG_SQRTCNT >> 1]); + break; default: { @@ -450,6 +465,30 @@ uint16_t DS9IORead(struct DS* ds, uint32_t address) { case DS_REG_IE_HI: case DS_REG_IF_LO: case DS_REG_IF_HI: + case DS9_REG_DIVCNT: + case DS9_REG_DIV_NUMER_0: + case DS9_REG_DIV_NUMER_1: + case DS9_REG_DIV_NUMER_2: + case DS9_REG_DIV_NUMER_3: + case DS9_REG_DIV_DENOM_0: + case DS9_REG_DIV_DENOM_1: + case DS9_REG_DIV_DENOM_2: + case DS9_REG_DIV_DENOM_3: + case DS9_REG_DIV_RESULT_0: + case DS9_REG_DIV_RESULT_1: + case DS9_REG_DIV_RESULT_2: + case DS9_REG_DIV_RESULT_3: + case DS9_REG_DIVREM_RESULT_0: + case DS9_REG_DIVREM_RESULT_1: + case DS9_REG_DIVREM_RESULT_2: + case DS9_REG_DIVREM_RESULT_3: + case DS9_REG_SQRTCNT: + case DS9_REG_SQRT_PARAM_0: + case DS9_REG_SQRT_PARAM_1: + case DS9_REG_SQRT_PARAM_2: + case DS9_REG_SQRT_PARAM_3: + case DS9_REG_SQRT_RESULT_LO: + case DS9_REG_SQRT_RESULT_HI: // Handled transparently by the registers break; default: