diff --git a/nu/ds.c b/nu/ds.c new file mode 100644 index 0000000..e81ddff --- /dev/null +++ b/nu/ds.c @@ -0,0 +1,221 @@ +#include +#include + +#include +#include +#include +#include +#include + +#include "hook/iobuf.h" +#include "hook/iohook.h" + +#include "nu/ds.h" +#include "nu/nvram.h" + +#include "util/crc.h" +#include "util/dprintf.h" +#include "util/setupapi.h" + +#pragma pack(push, 1) + +enum { + DS_IOCTL_SETUP = 0x80006004, + DS_IOCTL_READ_SECTOR = 0x80006010, +}; + +struct ds_eeprom { + uint32_t crc32; + uint8_t unk_04[5]; + char serial_no[17]; + uint8_t unk_1A[6]; +}; + +static_assert(sizeof(struct ds_eeprom) == 0x20, "DS EEPROM size"); + +#pragma pack(pop) + +static HRESULT ds_handle_irp(struct irp *irp); +static HRESULT ds_handle_open(struct irp *irp); +static HRESULT ds_handle_close(struct irp *irp); +static HRESULT ds_handle_ioctl(struct irp *irp); + +static HRESULT ds_ioctl_get_geometry(struct irp *irp); +static HRESULT ds_ioctl_setup(struct irp *irp); +static HRESULT ds_ioctl_read_sector(struct irp *irp); + +static const char ds_serial_file[] = "DEVICE/ds.txt"; +static struct ds_eeprom ds_eeprom; +static HANDLE ds_fd; + +HRESULT ds_hook_init(void) +{ + HRESULT hr; + FILE *f; + char c; + int i; + + memset(&ds_eeprom, 0, sizeof(ds_eeprom)); + + f = fopen(ds_serial_file, "r"); + + if (f != NULL) { + i = 0; + + for (;;) { + if (feof(f) || i >= sizeof(ds_eeprom.serial_no) - 1) { + break; + } + + c = getc(f); + + if (isspace(c)) { + break; + } + + ds_eeprom.serial_no[i++] = c; + } + + fclose(f); + } else { + dprintf("Failed to open %s\n", ds_serial_file); + } + + ds_eeprom.crc32 = crc32(&ds_eeprom.unk_04, 0x1C, 0); + + hr = iohook_push_handler(ds_handle_irp); + + if (FAILED(hr)) { + return hr; + } + + hr = setupapi_add_phantom_dev(&ds_guid, L"$ds"); + + if (FAILED(hr)) { + return hr; + } + + ds_fd = iohook_open_dummy_fd(); + + return S_OK; +} + +static HRESULT ds_handle_irp(struct irp *irp) +{ + assert(irp != NULL); + + if (irp->op != IRP_OP_OPEN && irp->fd != ds_fd) { + return iohook_invoke_next(irp); + } + + switch (irp->op) { + case IRP_OP_OPEN: return ds_handle_open(irp); + case IRP_OP_CLOSE: return ds_handle_close(irp); + case IRP_OP_IOCTL: return ds_handle_ioctl(irp); + default: return HRESULT_FROM_WIN32(ERROR_INVALID_FUNCTION); + } +} + +static HRESULT ds_handle_open(struct irp *irp) +{ + if (wcscmp(irp->open_filename, L"$ds") != 0) { + return iohook_invoke_next(irp); + } + + dprintf("DS: Open device\n"); + irp->fd = ds_fd; + + return S_OK; +} + +static HRESULT ds_handle_close(struct irp *irp) +{ + dprintf("DS: Close device\n"); + + return S_OK; +} + +static HRESULT ds_handle_ioctl(struct irp *irp) +{ + switch (irp->ioctl) { + case IOCTL_DISK_GET_DRIVE_GEOMETRY: + return ds_ioctl_get_geometry(irp); + + case DS_IOCTL_SETUP: + return ds_ioctl_setup(irp); + + case DS_IOCTL_READ_SECTOR: + return ds_ioctl_read_sector(irp); + + default: + dprintf("DS: Unknown ioctl %x, write %i read %i\n", + irp->ioctl, + irp->write.nbytes, + irp->read.nbytes); + + return HRESULT_FROM_WIN32(ERROR_INVALID_FUNCTION); + } +} + +static HRESULT ds_ioctl_get_geometry(struct irp *irp) +{ + DISK_GEOMETRY *out; + + if (irp->read.nbytes < sizeof(*out)) { + dprintf("DS: Invalid ioctl response buffer size\n"); + + return HRESULT_FROM_WIN32(ERROR_INSUFFICIENT_BUFFER); + } + + dprintf("DS: Get geometry\n"); + + out = (DISK_GEOMETRY *) irp->read.bytes; + out->Cylinders.QuadPart = 1; + out->MediaType = 0; + out->TracksPerCylinder = 1; + out->SectorsPerTrack = 2; + out->BytesPerSector = 32; + + irp->read.pos = sizeof(*out); + + return S_OK; +} + +static HRESULT ds_ioctl_setup(struct irp *irp) +{ + dprintf("DS: Setup IOCTL\n"); + + return S_OK; +} + +static HRESULT ds_ioctl_read_sector(struct irp *irp) +{ + struct const_iobuf src; + uint32_t sector_no; + HRESULT hr; + + hr = iobuf_read_le32(&irp->write, §or_no); + + if (FAILED(hr)) { + return hr; + } + + dprintf("DS: Read sector %08x\n", sector_no); + + src.bytes = (const uint8_t *) &ds_eeprom; + src.nbytes = sizeof(ds_eeprom); + src.pos = 0; + + iobuf_move(&irp->read, &src); + + if (irp->ovl != NULL) { + irp->ovl->Internal = STATUS_SUCCESS; + irp->ovl->InternalHigh = irp->read.pos; + + if (irp->ovl->hEvent != NULL) { + SetEvent(irp->ovl->hEvent); + } + } + + return S_OK; +} diff --git a/nu/ds.h b/nu/ds.h new file mode 100644 index 0000000..6bca367 --- /dev/null +++ b/nu/ds.h @@ -0,0 +1,12 @@ +#pragma once + +#include + +DEFINE_GUID( + ds_guid, + 0x279A9F67, + 0x348F, + 0x41C9, + 0xA4, 0xC4, 0xDF, 0xDB, 0x8A, 0xE8, 0xE5, 0xE0); + +HRESULT ds_hook_init(void); diff --git a/nu/guid.c b/nu/guid.c index 3ea1064..75a8ab6 100644 --- a/nu/guid.c +++ b/nu/guid.c @@ -1,5 +1,6 @@ #include #include +#include "nu/ds.h" #include "nu/eeprom.h" #include "nu/sram.h" diff --git a/nu/meson.build b/nu/meson.build index d4aaeaf..301db40 100644 --- a/nu/meson.build +++ b/nu/meson.build @@ -8,6 +8,8 @@ nu_lib = static_library( capnhook.get_variable('hooklib_dep'), ], sources : [ + 'ds.c', + 'ds.h', 'eeprom.c', 'eeprom.h', 'guid.c',