M include/mgba/internal/ds/ds.h → include/mgba/internal/ds/ds.h

@@ -103,6 +103,7 @@ struct mKeyCallback* keyCallback;
 	struct mCoreCallbacks* coreCallbacks;
 
 	struct mTimingEvent divEvent;
+	struct mTimingEvent sqrtEvent;
 };
 
 struct DSCartridge {

M src/ds/ds.c → src/ds/ds.c

@@ -121,6 +121,37 @@ STORE_64LE(result, DS9_REG_DIV_RESULT_0, ds->memory.io9);
 	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;
@@ -179,6 +210,11 @@ ds->divEvent.name = "DS Hardware Divide";
 	ds->divEvent.callback = _divide;
 	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);

M src/ds/io.c → src/ds/io.c

@@ -34,6 +34,12 @@ mTimingSchedule(&ds->ds9.timing, &ds->divEvent, (control & 3) ? 36 : 68);
 	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 @@ case DS9_REG_DIV_DENOM_2:
 	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 @@ case DS_REG_IE_LO:
 	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: