diff --git a/src/core/Scheduler.zig b/src/core/Scheduler.zig index 4f8ccfc..197b68b 100644 --- a/src/core/Scheduler.zig +++ b/src/core/Scheduler.zig @@ -20,6 +20,15 @@ pub fn push(self: *@This(), kind: Event.Kind, offset: u64) void { self.queue.add(.{ .kind = kind, .tick = self.tick + offset }) catch unreachable; } +pub fn remove(self: *@This(), needle: Event.Kind) void { + for (self.queue.items, 0..) |event, i| { + if (!std.meta.eql(event.kind, needle)) continue; + + _ = self.queue.removeIndex(i); // note: invalidates self.queue.items + return; + } +} + pub fn deinit(self: @This()) void { self.queue.deinit(); } @@ -58,6 +67,8 @@ pub fn handle(self: *@This(), bus_ptr: ?*anyopaque, event: Event, late: u64) voi }, .hblank => bus.ppu.onHblankEnd(self, late), .vblank => bus.ppu.onHblankEnd(self, late), + .sqrt => bus.io.sqrt.onSqrtCalc(), + .div => bus.io.div.onDivCalc(), } }, } @@ -68,7 +79,7 @@ pub const Event = struct { kind: Kind, const Kind7 = enum {}; - const Kind9 = enum { draw, hblank, vblank }; + const Kind9 = enum { draw, hblank, vblank, sqrt, div }; pub const Kind = union(enum) { nds7: Kind7, diff --git a/src/core/io.zig b/src/core/io.zig index 9b7403c..6a4d688 100644 --- a/src/core/io.zig +++ b/src/core/io.zig @@ -211,17 +211,22 @@ pub const masks = struct { pub inline fn ipcFifoSync(bus: anytype, value: anytype) @TypeOf(value) { comptime verifyBusType(@TypeOf(bus)); const T = @TypeOf(value); - const mask: T = 0xF; + const _mask: T = 0xF; - return value & ~mask | @as(T, @intCast(bus.io.shared.ipc_fifo.sync.raw & mask)); + return value & ~_mask | @as(T, @intCast(bus.io.shared.ipc_fifo.sync.raw & _mask)); } pub inline fn ipcFifoCnt(bus: anytype, value: anytype) @TypeOf(value) { comptime verifyBusType(@TypeOf(bus)); const T = @TypeOf(value); - const mask: T = 0x0303; + const _mask: T = 0x0303; - return value & ~mask | @as(T, @intCast(bus.io.shared.ipc_fifo.cnt.raw & mask)); + return value & ~_mask | @as(T, @intCast(bus.io.shared.ipc_fifo.cnt.raw & _mask)); + } + + /// General Mask helper + pub inline fn mask(original: anytype, value: @TypeOf(original), _mask: @TypeOf(original)) @TypeOf(original) { + return (value & _mask) | (original & ~_mask); } fn verifyBusType(comptime BusT: type) void { diff --git a/src/core/nds9/io.zig b/src/core/nds9/io.zig index 1421830..bc2f2ac 100644 --- a/src/core/nds9/io.zig +++ b/src/core/nds9/io.zig @@ -7,6 +7,8 @@ const Bus = @import("Bus.zig"); const SharedIo = @import("../io.zig").Io; const masks = @import("../io.zig").masks; +const sext = @import("../../util.zig").sext; + const log = std.log.scoped(.nds9_io); pub const Io = struct { @@ -19,6 +21,10 @@ pub const Io = struct { // Read Only keyinput: AtomicKeyInput = .{}, + /// DS Maths + div: Divisor = .{}, + sqrt: SquareRootUnit = .{}, + pub fn init(io: *SharedIo) @This() { return .{ .shared = io }; } @@ -31,6 +37,12 @@ pub fn read(bus: *const Bus, comptime T: type, address: u32) T { 0x0400_0210 => bus.io.shared.ie, 0x0400_0214 => bus.io.shared.irq, + 0x0400_02A0 => @truncate(bus.io.div.result), + 0x0400_02A4 => @truncate(bus.io.div.result >> 32), + 0x0400_02A8 => @truncate(bus.io.div.remainder), + 0x0400_02AC => @truncate(bus.io.div.remainder >> 32), + 0x0400_02B4 => @truncate(bus.io.sqrt.result), + 0x0410_0000 => bus.io.shared.ipc_fifo.recv(.arm9), else => warn("unexpected: read(T: {}, addr: 0x{X:0>8}) {} ", .{ T, address, T }), }, @@ -40,6 +52,10 @@ pub fn read(bus: *const Bus, comptime T: type, address: u32) T { 0x0400_0180 => @truncate(bus.io.shared.ipc_fifo.sync.raw), 0x0400_0184 => @truncate(bus.io.shared.ipc_fifo.cnt.raw), + + 0x0400_0280 => @truncate(bus.io.div.cnt.raw), + 0x0400_02B0 => @truncate(bus.io.sqrt.cnt.raw), + else => warn("unexpected: read(T: {}, addr: 0x{X:0>8}) {} ", .{ T, address, T }), }, u8 => switch (address) { @@ -68,6 +84,31 @@ pub fn write(bus: *Bus, comptime T: type, address: u32, value: T) void { 0x0400_0210 => bus.io.shared.ie = value, 0x0400_0214 => bus.io.shared.irq = value, + 0x0400_0290 => { + bus.io.div.numerator = masks.mask(bus.io.div.numerator, value, 0xFFFF_FFFF); + bus.io.div.schedule(bus.scheduler); + }, + 0x0400_0294 => { + bus.io.div.numerator = masks.mask(bus.io.div.numerator, @as(u64, value) << 32, 0xFFFF_FFFF << 32); + bus.io.div.schedule(bus.scheduler); + }, + 0x0400_0298 => { + bus.io.div.denominator = masks.mask(bus.io.div.denominator, value, 0xFFFF_FFFF); + bus.io.div.schedule(bus.scheduler); + }, + 0x0400_029C => { + bus.io.div.denominator = masks.mask(bus.io.div.denominator, @as(u64, value) << 32, 0xFFFF_FFFF << 32); + bus.io.div.schedule(bus.scheduler); + }, + 0x0400_02B8 => { + bus.io.sqrt.param = masks.mask(bus.io.sqrt.param, value, 0xFFFF_FFFF); + bus.io.sqrt.schedule(bus.scheduler); + }, + 0x0400_02BC => { + bus.io.sqrt.param = masks.mask(bus.io.sqrt.param, @as(u64, value) << 32, 0xFFFF_FFFF << 32); + bus.io.sqrt.schedule(bus.scheduler); + }, + 0x0400_0304 => bus.io.powcnt.raw = value, else => log.warn("unexpected: write(T: {}, addr: 0x{X:0>8}, value: 0x{X:0>8})", .{ T, address, value }), @@ -77,6 +118,16 @@ pub fn write(bus: *Bus, comptime T: type, address: u32, value: T) void { 0x0400_0184 => bus.io.shared.ipc_fifo.cnt.raw = masks.ipcFifoCnt(bus, value), 0x0400_0208 => bus.io.shared.ime = value & 1 == 1, + 0x0400_0280 => { + bus.io.div.cnt.raw = value; + bus.io.div.schedule(bus.scheduler); + }, + + 0x0400_02B0 => { + bus.io.sqrt.cnt.raw = value; + bus.io.sqrt.schedule(bus.scheduler); + }, + else => log.warn("unexpected: write(T: {}, addr: 0x{X:0>8}, value: 0x{X:0>8})", .{ T, address, value }), }, u8 => switch (address) { @@ -107,6 +158,135 @@ const PowCnt = extern union { raw: u32, }; +/// Divisor +const Divisor = struct { + const Scheduler = @import("../Scheduler.zig"); + + /// DIVCNT - Division Control (R/W) + cnt: Cnt = .{ .raw = 0x0000_0000 }, + /// DIV_NUMER - division numerator (R/W) + numerator: u64 = 0x0000_0000_0000_0000, + /// DIV_DENOM - division denominator (R/W) + denominator: u64 = 0x0000_0000_0000_0000, + + /// DIV_RESULT - division quotient (R) + result: u64 = 0x0000_0000_0000_0000, + /// DIVREM_RESULT - remainder + remainder: u64 = 0x0000_0000_0000_0000, + + const Cnt = extern union { + mode: Bitfield(u32, 0, 2), + div_by_zero: Bit(u32, 14), + busy: Bit(u32, 15), + raw: u32, + }; + + pub fn schedule(self: *@This(), scheduler: *Scheduler) void { + defer self.cnt.busy.set(); + + const cycle_count: u64 = switch (self.cnt.mode.read()) { + 0b00 => 18, + 0b01, 0b10, 0b11 => 34, + }; + + scheduler.remove(.{ .nds9 = .div }); + scheduler.push(.{ .nds9 = .div }, cycle_count); + } + + pub fn onDivCalc(self: *@This()) void { + defer self.cnt.busy.unset(); + self.cnt.div_by_zero.write(self.denominator == 0); + + switch (self.cnt.mode.read()) { + 0b00 => { + // 32bit / 32bit = 32bit , 32bit + const left = sext(i64, i32, self.numerator); + const right = sext(i64, i32, self.denominator); + + if (right == 0) { + self.remainder = @bitCast(left); + self.result = if (left >> 63 & 1 == 1) @as(u64, 1) else @bitCast(@as(i64, -1)); + + // FIXME(chore): replace `>> 32 ) << 32` with mask + self.result = masks.mask(self.result, (~self.result >> 32) << 32, 0xFFFF_FFFF << 32); + + return; + } + + self.result = @bitCast(@divTrunc(left, right)); + self.remainder = @bitCast(@rem(left, right)); + }, + 0b01, 0b11 => { + // 64bit / 32bit = 64bit , 32bit + const left = sext(i128, i64, self.numerator); + const right = sext(i128, i32, self.denominator); + + if (right == 0) { + self.remainder = @bitCast(@as(i64, @truncate(left))); + self.result = if (left >> 63 & 1 == 1) @as(u64, 1) else @bitCast(@as(i64, -1)); + + return; + } + + self.result = @bitCast(@as(i64, @truncate(@divTrunc(left, right)))); + self.remainder = @bitCast(@as(i64, @truncate(@rem(left, right)))); + }, + 0b10 => { + // 64bit / 64bit = 64bit , 64bit + const left = sext(i128, i64, self.numerator); + const right = sext(i128, i64, self.denominator); + + if (right == 0) { + self.remainder = @bitCast(@as(i64, @truncate(left))); + self.result = if (left >> 63 & 1 == 1) @as(u64, 1) else @bitCast(@as(i64, -1)); + + return; + } + + self.result = @bitCast(@as(i64, @truncate(@divTrunc(left, right)))); + self.remainder = @bitCast(@as(i64, @truncate(@rem(left, right)))); + }, + } + } +}; + +/// Square Root Unit +const SquareRootUnit = struct { + const Scheduler = @import("../Scheduler.zig"); + + /// SQRTCNT - Division Control (R/W) + cnt: Cnt = .{ .raw = 0x0000_0000 }, + + /// SQRT_RESULT - square root result (R) + result: u32 = 0x0000_0000, + /// SQRT_PARAM - square root paramater input (R/W) + param: u64 = 0x0000_0000_0000_0000, + + const Cnt = extern union { + mode: Bit(u32, 0), + busy: Bit(u32, 15), + raw: u32, + }; + + pub fn schedule(self: *@This(), scheduler: *Scheduler) void { + defer self.cnt.busy.set(); + + scheduler.remove(.{ .nds9 = .sqrt }); + scheduler.push(.{ .nds9 = .sqrt }, 13); // always takes 13 cycles + } + + pub fn onSqrtCalc(self: *@This()) void { + defer self.cnt.busy.unset(); + + const mask: u64 = blk: { + const value: u64 = @intFromBool(!self.cnt.mode.read()); + break :blk (value << 32) -% 1; + }; + + self.result = @truncate(std.math.sqrt(self.param & mask)); + } +}; + pub const DispcntA = extern union { bg_mode: Bitfield(u32, 0, 2), diff --git a/src/util.zig b/src/util.zig new file mode 100644 index 0000000..e39d853 --- /dev/null +++ b/src/util.zig @@ -0,0 +1,104 @@ +const std = @import("std"); + +const assert = std.debug.assert; + +/// Sign-Extends a `SrcT` value to a `DestT`. +/// +/// Notes: +/// - `DestT` can be a signed or unsigned integer +/// - `value` is of type `anytype` and may be a signed / unsigned / comptime integer +/// +/// Invariants: +/// - `SrcT` *must* be a signed Integer (the idea being: "why would we sign extend an unsigned value?") +/// - `SrcT` must have equal or less bits than DestT +/// - `SrcT` must have equal or less bits than ValT (`@TypeOf(value)`) +pub fn sext(comptime DestT: type, comptime SrcT: type, value: anytype) DestT { + const ValT = @TypeOf(value); + const dst_info = @typeInfo(DestT); + const src_info = @typeInfo(SrcT); + const val_info = @typeInfo(ValT); + + comptime { + // DestT, SrcT and ValT are expected to be integers + std.debug.assert(dst_info == .Int); + std.debug.assert(src_info == .Int); + std.debug.assert(val_info == .ComptimeInt or val_info == .Int); + + // sexting to a type smaller than SrcT isn't "extension" and therefore doesn't make sense + // sexting also implies a signed value (we may remove this assumption later) + std.debug.assert(src_info.Int.signedness == .signed); + std.debug.assert(src_info.Int.bits <= dst_info.Int.bits); + + // ValT is allowed to have more bits than SrcT, we just truncate + std.debug.assert(val_info == .ComptimeInt or src_info.Int.bits <= val_info.Int.bits); + } + + // signed integers implement Arithmetic Right Shift + const SignedSrcT = @Type(.{ .Int = .{ .signedness = .signed, .bits = src_info.Int.bits } }); + const SignedDestT = @Type(.{ .Int = .{ .signedness = .signed, .bits = dst_info.Int.bits } }); + const SignedValT = switch (val_info) { + .Int => @Type(.{ .Int = .{ .signedness = .signed, .bits = val_info.Int.bits } }), + .ComptimeInt => FittingInt(value), + else => unreachable, + }; + + const signed_value: SignedSrcT = switch (val_info) { + .Int => switch (val_info.Int.signedness) { + .signed => @truncate(value), + .unsigned => @truncate(@as(SignedValT, @bitCast(value))), + }, + .ComptimeInt => @truncate(value), + else => unreachable, + }; + + // maybe no shifts are needed at all? + return @bitCast(@as(SignedDestT, signed_value)); +} + +test "sext" { + const expect = std.testing.expectEqual; + + try expect(@as(u4, 0b1111), sext(u4, i2, 0b11)); + try expect(@as(u4, 0b0001), sext(u4, i2, 0b01)); + + try expect(@as(u4, 0b1111), sext(u4, i2, 0b1111_1111)); + try expect(@as(u4, 0b0001), sext(u4, i2, 0b0000_0001)); + + try expect(@as(i4, -1), sext(i4, i2, 0b11)); + try expect(@as(i4, 1), sext(i4, i2, 0b01)); + + try expect(@as(i4, -1), sext(i4, i2, 0b1111_1111)); + try expect(@as(i4, 1), sext(i4, i2, 0b0000_0001)); + + try expect(@as(u4, 0b1111), sext(u4, i2, @as(u2, 0b11))); + try expect(@as(u4, 0b0001), sext(u4, i2, @as(u2, 0b01))); + + try expect(@as(u4, 0b1111), sext(u4, i2, @as(u8, 0b1111_1111))); + try expect(@as(u4, 0b0001), sext(u4, i2, @as(u8, 0b0000_0001))); + + try expect(@as(i4, -1), sext(i4, i2, @as(i2, -1))); + try expect(@as(i4, 1), sext(i4, i2, @as(i2, 1))); + + try expect(@as(i4, -1), sext(i4, i2, @as(i8, -1))); + try expect(@as(i4, 1), sext(i4, i2, @as(i8, 1))); +} + +fn FittingInt(comptime value: comptime_int) type { + const bits = blk: { + var i: comptime_int = 0; + while (value >> i != 0) : (i += 1) {} + + break :blk i; + }; + + return @Type(.{ .Int = .{ .signedness = .signed, .bits = bits } }); +} + +test "FittingInt" { + try std.testing.expect(FittingInt(0) == i0); + try std.testing.expect(FittingInt(0b1) == i1); + try std.testing.expect(FittingInt(0b1) == i1); + try std.testing.expect(FittingInt(0b11) == i2); + try std.testing.expect(FittingInt(0b101) == i3); + try std.testing.expect(FittingInt(0b1010) == i4); +}