/*
 * CHU - driver for CH375 in disk mode
 * Copyright (C) 2024 Javier S. Pedro
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.

 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */


#include <i86.h>
#include <string.h>

#include "inc/dosdrv.h"
#include "inc/bda.h"
#include "inc/mbr.h"

#include "chudisk.h"
#include "ch375.h"
#include "dlog.h"
#include "utils.h"

// Pre-declarations for the two driver routines, note there is a pragma aux for them
DOS_DD_STRATEGY strategy_routine;
DOS_DD_INTERRUPT interrupt_routine;

/** This is the actual .SYS header! */
const DOS_DD_HEADER __based(__segname("_CODE")) header = {
        MK_FP(-1, -1),
        DOS_DD_BLOCK | DOS_BLOCK_SUPPORT_32BIT_ADDRESSES
		| DOS_BLOCK_FORMAT_NON_IBM | DOS_BLOCK_SUPPORT_REMOVABLE_MEDIA
        | DOS_DD_SUPPORT_IOCTL_RW
        | DOS_BLOCK_SUPPORT_IOCTL_GENERIC | DOS_DD_SUPPORT_IOCTL_QUERY,
        strategy_routine,
        interrupt_routine,
        "\1CHUDISK"
};

/** Strategy routine saves DRS here for interrupt routine to use */
DOS_DRS far * saved_drs;

// Some settings
/** The ch375 io port base. */
unsigned short ch375_port;
/** Number of units handled by the driver (i.e. number of drive letters reserved. ) */
unsigned char num_units;
/** Read-only mode (as if write-protected floppy) */
bool read_only;
/** Process FAT32 partitions too. */
bool show_fat32;

/* Runtime data for the driver. */
struct chudisk_data data;

/** Buffer used to load boot sectors from disk and partitions. */
char buffer[512];

/** Waits for the CH375 to generate an interrupt, returns result value.
 *  @param max_ticks max BIOS ticks to wait for interrupt. */
uint8_t wait_for_ch375_result(unsigned int max_ticks)
{
	uint16_t last = bda_get_tick_count_lo();
	while (!ch375_poll_interrupt() && max_ticks > 0) {
		uint16_t now = bda_get_tick_count_lo();
		if (now != last) {
			last = now;
			max_ticks--;
		}
	}
	if (ch375_poll_interrupt()) {
		uint8_t status = ch375_get_int_status();
		//("CH375 INT status=0x%x\n", status);
		return status;
	} else {
		dprintf("CH375 int timeout\n");
		return 0;
	}
}

/** Gets the disk sector from RW command DRS. */
static inline uint32_t get_start_sector(const DOS_DRS far * drs)
{
	return (drs->cmd.rw.start_sector == 0xFFFF
			? drs->cmd.rw.start_sector_huge
			: drs->cmd.rw.start_sector);
}

static inline uint32_t chs_to_lba(const DOS_BPB * bpb, uint16_t cylinder, uint16_t head, uint16_t sector)
{
	uint16_t track = cylinder * bpb->numberOfHeads + head;
	return mul(track, bpb->sectorsPerTrack) + (sector - 1);
}

/** Read N sectors from the disk
 *  @return number of sectors read
 */
static unsigned disk_read_sectors(uint8_t far *buffer, uint32_t lba, uint8_t num)
{
	uint8_t result;
	unsigned total = 0; // Total bytes written

	dprintf("disk read  buf=%Fp sector=%lu num=%u\n", buffer, lba, num);

	// Initiate read operation
	ch375_disk_read(lba, num);

	// Now keep going as long as ch375 says to keep going
	while ((result = wait_for_ch375_result(10)) == CH375_INT_USB_DISK_READ) {
		unsigned len;

		// Sanity check
		if (total / SECTOR_SIZE >= num) {
			dputs("read overflow");
			break;
		}

		ch375_send_command(CH375_RD_USB_DATA);
		len = ch375_read_data_block(&buffer[total]);

		//dprintf(" disk read data block len = %d\n", len);

		if (len == 0) {
			// Done?
			break;
		}

		total += len;

		// Continue
		ch375_send_command(CH375_DISK_RD_GO);
	}

	dprintf(" disk read  total bytes = %u, sectors = %u, final status = 0x%hx\n",
	        total, total / SECTOR_SIZE, result);

	return total / SECTOR_SIZE;
}

/** Write N sectors to the disk
 *  @return number of sectors written
 */
static unsigned disk_write_sectors(uint8_t far *buffer, uint32_t lba, uint8_t num)
{
	uint8_t result;
	unsigned total = 0; // Total bytes written

	dprintf("disk write buf=%Fp sector=%lu num=%u\n", buffer, lba, num);

	// Initiate operation
	ch375_disk_write(lba, num);

	// Now keep going as long as ch375 says to keep going
	while ((result = wait_for_ch375_result(10)) == CH375_INT_USB_DISK_WRITE) {
		const unsigned len = 64;

		// Sanity check
		if (total / SECTOR_SIZE >= num) {
			dputs("write overflow");
			break;
		}

		ch375_send_command(CH375_WR_USB_DATA7);
		ch375_write_data_block(&buffer[total], len);

		total += len;

		// Continue
		ch375_send_command(CH375_DISK_WR_GO);
	}

	dprintf(" disk write total bytes = %u, sectors = %u, final status = 0x%hx\n",
	        total, total / SECTOR_SIZE, result);

	return total / SECTOR_SIZE;
}

/** Enables CH375 USB host mode, initializes "disk" mode on it
 *  and connects to a disk. */
static bool disk_connect()
{
	int tries;

	dputs("switch to usb host");
	ch375_set_usb_mode(CH375_USB_MODE_HOST_AUTO_SOF);
	if (wait_for_ch375_result(4) != CH375_INT_USB_CONNECT) {
		dputs("could not turn on usb host");
		return false;
	}

	dputs("try init disk");
	ch375_disk_init();
	if (wait_for_ch375_result(20) != CH375_INT_USB_SUCCESS) {
		dputs("could not init disk");
		return false;
	}

	// looks like we have to keep polling by asking "is the disk ready?"
	// even if each time we ask we are already polling
	tries = 3;
	while (tries-- > 0) {
		ch375_test_disk_ready();
		if (wait_for_ch375_result(20) == CH375_INT_USB_SUCCESS) {
			dputs("disk now init");
			return true;
		}
		bda_wait_tick();
	}

	dputs("disk failed to init");
	return false;
}

/** Scans the disk for partitions. */
static int disk_scan()
{
	uint32_t next_sector;
	int num_parts, i;

	dprintf("disk scan show_fat32=%d\n", show_fat32);

	// Clear current partitions
	for (i = 0; i < num_units; ++i) {
		data.unit[i].exists = false;
		data.unit[i].ready = false;
		data.unit[i].start_sector = 0;
		data.unit[i].sectors = 0;
	}

	// Refresh disk size
	ch375_send_command(CH375_DISK_SIZE);
	if (wait_for_ch375_result(4) != CH375_INT_USB_SUCCESS) {
		return DOS_DD_ERR_MEDIA_UNAVAILABLE;
	}

	ch375_send_command(CH375_RD_USB_DATA);
	if (ch375_read_data_block(buffer) < 8) {
		dprintf("disk size command return less than 8 bytes\n");
		return DOS_DD_ERR_UNKNOWN_MEDIA;
	}

	data.sectors = SWAPNC_32(*(uint32_t*)&buffer[0]);
	data.bytesPerSector = SWAPNC_32(*(uint32_t*)&buffer[4]);

	dprintf("sectors %lu bytesPerSector %lu\n", data.sectors, data.bytesPerSector);
	if (data.bytesPerSector != SECTOR_SIZE) {
		return DOS_DD_ERR_UNKNOWN_MEDIA;
	}

	// Start creating partitions/units from partition tables
	num_parts = 0;

	// Start with MBR at sector 0
	next_sector = 0;
	do {
		const MBR_PARTENTRY *partentry;
		const uint32_t this_sector = next_sector;

		dprintf("reading boot record at sector %lu\n", this_sector);

		// Try to read sector
		if (disk_read_sectors(buffer, this_sector, 1) != 1) {
			// Fatal for MBR, non-fatal for EBR
			if (this_sector == 0) return DOS_DD_ERR_READ_FAULT;
			break;
		}

		// Check for signature
		if (*(uint16_t*)&buffer[510] != 0xAA55U) {
			dprintf(" invalid MBR signature %x\n", *(uint16_t*)&buffer[510]);
			if (this_sector == 0) return DOS_DD_ERR_UNKNOWN_MEDIA;
			break;
		}

		// Parse the MBR
		partentry = (MBR_PARTENTRY*)&buffer[446];

		next_sector = 0; // If there is another EBR we will store it here
		for (i = 0; i < 4 && num_parts < MAX_UNITS; i++) {
			dprintf(" partition %d type %x start %lu len %lu\n", i, partentry[i].type, partentry[i].lbaFirst, partentry[i].lbaLen);

			if (partentry[i].type == MBR_FAT12 || partentry[i].type == MBR_FAT16_32MB
			        || partentry[i].type == MBR_FAT16 || partentry[i].type == MBR_FAT16_LBA
			        || (show_fat32 && (partentry[i].type == MBR_FAT32 || partentry[i].type == MBR_FAT32_LBA)) ) {
				data.unit[num_parts].exists = true;
				data.unit[num_parts].start_sector = this_sector + partentry[i].lbaFirst;
				data.unit[num_parts].sectors = partentry[i].lbaLen;
				dprintf("  added as unit %u starts at %lu\n", num_parts, data.unit[num_parts].start_sector);
				num_parts++;
			} else if (partentry[i].type == MBR_EXT || partentry[i].type == MBR_EXT_LBA) {
				// Go to the EBR next
				next_sector = this_sector + partentry[i].lbaFirst;
			}
		}
	} while (next_sector != 0 && num_parts < MAX_UNITS);

	// OK!
	dprintf("found %u partitions\n", num_parts);
	data.ready = true;

	return 0;
}

static int disk_init()
{
	ch375_test_disk_ready();
	if (wait_for_ch375_result(4) != CH375_INT_USB_SUCCESS) {
		// Disk is not even connected, try to connect
		if (!disk_connect()) {
			// Nothing works: fail
			return DOS_DD_ERR_DRIVE_NOT_READY;
		}
	}
	// Disk is now ready: initiate scan for partitions
	return disk_scan();
}

/** Return the volume label from the cached BPB, i.e. from the last time the drive was accessed. */
static inline const char * unit_get_volume_label(uint8_t unit)
{
	const DOS_BPB *bpb = &data.unit[unit].bpb;

	if (bpb->sectorsPerFAT != 0) {
		// FAT16
		return bpb->fat16.volumeLabel;
	} else {
		// FAT32
		return bpb->fat32.volumeLabel;
	}
}

/** Try to initialize logigcal drive 'unit'. This also initializes the entire disk
 *  if it was not done yet (scanning all partitions).
 *  It will read the BPB of the corresponding partition and cache it. */
static int unit_init(uint8_t unit)
{
	DOS_BPB *bpb = &data.unit[unit].bpb;
	int err;

	// Check if disk is ready, and initialize it if not
	if (!data.ready) {
		if (err = disk_init()) {
			return err;
		}
	}

	// Disk should be ready, partitions enumerated
	if (!data.unit[saved_drs->unit].exists) {
		dputs("disk is ready, but partition does not exist");
		return DOS_DD_ERR_MEDIA_UNAVAILABLE;
	}

	// Try to read starting sector of partition into buffer
	if (disk_read_sectors(buffer, data.unit[unit].start_sector, 1) != 1) {
		// Cannot read start sector...
		return DOS_DD_ERR_READ_FAULT;
	}

	// For now, copy largest possible BPB into our internal cache
	memcpy(bpb, &buffer[DOS_BPB_OFFSET_ON_DISK], sizeof(DOS_BPB));

	dprintf("unit %u BPB sectorsPerCluster=%u sectorsPerTrack=%u numberOfHeads=%u\n",
	        unit, bpb->sectorsPerCluster,
	        bpb->sectorsPerTrack, bpb->numberOfHeads);
	dprintf("   bytesPerSector=%u numberOfFats=%u sectorsPerFat=%u \n",
	        bpb->bytesPerSector,bpb->numberOfFATs,
	        bpb->sectorsPerFAT);
	dprintf("   numberOfSectors=%u numberOfSectorsLarge=%lu\n",
	        bpb->numberOfSectors, bpb->numberOfSectorsLarge);

	if (bpb->sectorsPerFAT != 0) {
		// FAT12/16
		dprintf("   %s volumeSerialNumber=%lx volumeLabel='%.11s' fsType='%.8s'\n",
		        "FAT16", bpb->fat16.volumeSerialNumber, bpb->fat16.volumeLabel, bpb->fat16.fsType);
	} else if (bpb->fat32.sectorPerFatLarge != 0) {
		dprintf("   %s volumeSerialNumber=%lx volumeLabel='%.11s' fsType='%.8s'\n",
		        "FAT32", bpb->fat32.volumeSerialNumber, bpb->fat32.volumeLabel, bpb->fat32.fsType);
	} else {
		dprintf("  -> unknown filesystem\n");
		return DOS_DD_ERR_UNKNOWN_MEDIA;
	}

	data.unit[saved_drs->unit].ready = true;
	return 0;
}

static bool unit_check_media_changed(uint8_t unit)
{
	if (data.ready) {
		// Recheck if disk is still ready
		ch375_test_disk_ready();
		if (wait_for_ch375_result(4) == CH375_INT_USB_SUCCESS) {
			// Disk is ready. But is partition ready ?
			if (data.unit[unit].ready) {
				// Disk ready, partition ready. Perfect!
				dputs("mediaquery -> disk ready -> unit ready -> not changed");
				return false;
			} else {
				// Disk ready, but this partition is not.
				// Likely first time access for this partition.
				dputs("mediaquery -> disk ready -> unit !ready -> changed");
				return true;
			}
		} else {
			// CH375 is not ready but we thought it was. USB has been removed!
			dputs("mediaquery -> disk not ready -> changed");
			// Invalidate disk ready flag so that we rescan for partitions
			data.ready = false;
			return true;
		}
	} else {
		// We do not think CH375 is ready. First time we check, or USB not present.
		dputs("mediaquery -> !ready -> changed");
		return true;
	}
}

void strategy_routine(DOS_DRS __far *drs)
{
	load_ds_from_cs();
	//dprintf("strategy routine drs=%Fp len=%u func=%u\n", drs, drs->len, drs->function);
	saved_drs = drs;
}

void interrupt_routine()
{
	int err;
	load_ds_from_cs();
	//dprintf("interrupt routine drs=%Fp len=%u func=%u\n", saved_drs, saved_drs->len, saved_drs->function);
	switch (saved_drs->function) {
	case DOS_DD_FUNC_INIT:
		driver_init(saved_drs);
		break;
	case DOS_DD_FUNC_MEDIA_CHECK:
		if (saved_drs->unit >= num_units) {
			saved_drs->status = DOS_DD_ERR_UNKNOWN_UNIT | DOS_DD_STATUS_ERROR_DONE;
			return;
		}
		saved_drs->cmd.mediacheck.out.result = unit_check_media_changed(saved_drs->unit)
		        ? DOS_DD_MEDIAQUERY_CHANGED : DOS_DD_MEDIAQUERY_NOT_CHANGED;
		saved_drs->cmd.mediacheck.out.volume_id = unit_get_volume_label(saved_drs->unit);
		saved_drs->status = DOS_DD_STATUS_DONE;
		break;
	case DOS_DD_FUNC_BUILD_BPB:
		dprintf("build bpb unit=%hu\n", saved_drs->unit);
		if (saved_drs->unit >= num_units) {
			dputs("unit is unknown");
			saved_drs->status = DOS_DD_ERR_UNKNOWN_UNIT | DOS_DD_STATUS_ERROR_DONE;
			return;
		}
		// Try to initialize disk and unit
		if (err = unit_init(saved_drs->unit)) {
			saved_drs->status = err | DOS_DD_STATUS_ERROR_DONE;
			return;
		}
		// Return pointer to our cached BPB
		saved_drs->cmd.buildbpb.out.bpb = &data.unit[saved_drs->unit].bpb;
		saved_drs->status = DOS_DD_STATUS_DONE;
		break;
	case DOS_DD_FUNC_READ:
		dprintf("read unit=%hu sector=%u large_sector=%lu len=%u\n", saved_drs->unit,
		        saved_drs->cmd.rw.start_sector, saved_drs->cmd.rw.start_sector_huge, saved_drs->cmd.rw.len);
		if (saved_drs->unit >= num_units) {
			saved_drs->status = DOS_DD_ERR_UNKNOWN_UNIT | DOS_DD_STATUS_ERROR_DONE;
			return;
		}
		if (!data.ready || !data.unit[saved_drs->unit].ready) {
			dputs("partition not ready");
			// We don't try to initialize the drive here, DOS must do that by calling buildBPB
			saved_drs->status = DOS_DD_ERR_MEDIA_UNAVAILABLE | DOS_DD_STATUS_ERROR_DONE;
			return;
		}
		if (saved_drs->cmd.rw.len > 128) {
			// Too many sectors at once!
			// TODO: Should we allow partial reads? When I tried even "type <file>" crashes.
			// TODO: Should we also check for segment wraparound?
			saved_drs->cmd.rw.len = 0;
			saved_drs->status = DOS_DD_ERR_READ_FAULT | DOS_DD_STATUS_ERROR_DONE;
			return;
		} else if (saved_drs->cmd.rw.len >= 1) {
			uint32_t sector = get_start_sector(saved_drs);
			if (sector > data.unit[saved_drs->unit].sectors) {
				saved_drs->cmd.rw.len = 0;
				saved_drs->status = DOS_DD_ERR_SECTOR_NOT_FOUND | DOS_DD_STATUS_ERROR_DONE;
				return;
			}
			// Make absolute
			sector += data.unit[saved_drs->unit].start_sector;
			if (sector > data.sectors) {
				saved_drs->cmd.rw.len = 0;
				saved_drs->status = DOS_DD_ERR_SEEK | DOS_DD_STATUS_ERROR_DONE;
				return;
			}

			saved_drs->cmd.rw.len = disk_read_sectors(saved_drs->cmd.rw.buffer, sector, saved_drs->cmd.rw.len);
			if (saved_drs->cmd.rw.len == 0) {
				// Not sure if required
				saved_drs->status = DOS_DD_ERR_READ_FAULT | DOS_DD_STATUS_ERROR_DONE;
				return;
			}
		}
		saved_drs->cmd.rw.volume_id = unit_get_volume_label(saved_drs->unit);
		saved_drs->status = DOS_DD_STATUS_DONE;
		break;
	case DOS_DD_FUNC_WRITE:
	case DOS_DD_FUNC_WRITE_VERIFY: // TODO Not doing the verify yet part...
		dprintf("write unit=%hu sector=%u large_sector=%lu len=%u\n", saved_drs->unit,
		        saved_drs->cmd.rw.start_sector, saved_drs->cmd.rw.start_sector_huge, saved_drs->cmd.rw.len);
		if (saved_drs->unit >= num_units) {
			saved_drs->status = DOS_DD_ERR_UNKNOWN_UNIT | DOS_DD_STATUS_ERROR_DONE;
			return;
		}
		if (!data.ready || !data.unit[saved_drs->unit].ready) {
			dputs("partition not ready");
			saved_drs->status = DOS_DD_ERR_MEDIA_UNAVAILABLE | DOS_DD_STATUS_ERROR_DONE;
			return;
		}
		if (read_only) {
			saved_drs->status = DOS_DD_ERR_WRITE_PROTECT | DOS_DD_STATUS_ERROR_DONE;
			return;
		}
		if (saved_drs->cmd.rw.len > 128) {
			// Too much!
			saved_drs->cmd.rw.len = 0;
			saved_drs->status = DOS_DD_ERR_WRITE_FAULT | DOS_DD_STATUS_ERROR_DONE;
			return;
		} else if (saved_drs->cmd.rw.len >= 1) {
			// Check relative sector value against partition limit
			uint32_t sector = get_start_sector(saved_drs);
			if (sector > data.unit[saved_drs->unit].sectors) {
				saved_drs->cmd.rw.len = 0;
				saved_drs->status = DOS_DD_ERR_SECTOR_NOT_FOUND | DOS_DD_STATUS_ERROR_DONE;
				return;
			}
			// Make absolute and check against disk limit
			sector += data.unit[saved_drs->unit].start_sector;
			if (sector > data.sectors) {
				saved_drs->cmd.rw.len = 0;
				saved_drs->status = DOS_DD_ERR_SEEK | DOS_DD_STATUS_ERROR_DONE;
				return;
			}

			saved_drs->cmd.rw.len = disk_write_sectors(saved_drs->cmd.rw.buffer, sector, saved_drs->cmd.rw.len);
			if (saved_drs->cmd.rw.len == 0) {
				// Not sure if required
				saved_drs->status = DOS_DD_ERR_WRITE_FAULT | DOS_DD_STATUS_ERROR_DONE;
				return;
			}

			if (saved_drs->function == DOS_DD_FUNC_WRITE_VERIFY) {
				// Verify one sector at a time. Use our internal buffer.
				uint8_t far * origbuf = saved_drs->cmd.rw.buffer;
				unsigned i;
				for (i = 0; i < saved_drs->cmd.rw.len; i++) {
					dprintf(" verifying sector %u\n", i);
					if (disk_read_sectors(buffer, sector + i, 1) != 1) {
						saved_drs->status = DOS_DD_ERR_READ_FAULT | DOS_DD_STATUS_ERROR_DONE;
						return;
					}

					if (_fmemcmp(&origbuf[i * SECTOR_SIZE], buffer, SECTOR_SIZE) != 0) {
						dprintf("  cmp error for sec %u\n", i);
						saved_drs->status = DOS_DD_ERR_CRC | DOS_DD_STATUS_ERROR_DONE;
						return;
					}
				}
			}
		}
		saved_drs->cmd.rw.volume_id = unit_get_volume_label(saved_drs->unit);
		saved_drs->status = DOS_DD_STATUS_DONE;
		break;
	case DOS_DD_FUNC_DEVICE_OPEN:
		dputs("device open");
		// Do nothing here -- we init the device on build bpb, as with older DOSes
		saved_drs->status = DOS_DD_STATUS_DONE;
		break;
	case DOS_DD_FUNC_DEVICE_CLOSE:
		dputs("device close");
		saved_drs->status = DOS_DD_STATUS_DONE;
		break;
	case DOS_DD_FUNC_REMOVEABLE_MEDIA:
		dputs("removable device");
		// If the disk is fixed, we must set "busy" bit here. We are removable.
		saved_drs->status = DOS_DD_STATUS_DONE;
		break;
	case DOS_DD_FUNC_IOCTL_READ:
		dprintf("ioctl read unit=%hu buf=%Fp len=%u\n", saved_drs->unit,
		        saved_drs->cmd.ioctlrw.buffer, saved_drs->cmd.ioctlrw.len);
		if (saved_drs->unit >= num_units) {
			saved_drs->status = DOS_DD_ERR_UNKNOWN_UNIT | DOS_DD_STATUS_ERROR_DONE;
			return;
		}
		// If there's enough space, return our internal config string.
		if (saved_drs->cmd.ioctlrw.len >= 3 + 2 + 1 + 5 + 1) {
			uint8_t far * buf = saved_drs->cmd.ioctlrw.buffer;
			int len = 0;
			// B = base port (2 byte)
			buf[len++] = 'B';
			_fmemcpy(&buf[len], &ch375_port, sizeof(ch375_port));
			len += sizeof(ch375_port);
			// 3 = showing FAT32 (1 byte)
			buf[len++] = '3';
			buf[len++] = show_fat32;
			// R = read only (1 byte)
			buf[len++] = 'R';
			buf[len++] = read_only;
			// S = partition start (4 bytes)
			buf[len++] = 'S';
			_fmemcpy(&buf[len], &data.unit[saved_drs->unit].start_sector, sizeof(uint32_t));
			len += sizeof(uint32_t);
			buf[len++] = '\0';
			saved_drs->cmd.ioctlrw.len = len;
		} else {
			saved_drs->cmd.ioctlrw.len = 0;
		}
		saved_drs->status = DOS_DD_STATUS_DONE;
		break;
	case DOS_DD_FUNC_IOCTL_WRITE:
		dprintf("ioctl write unit=%hu buf=%Fp len=%u\n", saved_drs->unit,
		        saved_drs->cmd.ioctlrw.buffer, saved_drs->cmd.ioctlrw.len);
		if (saved_drs->unit >= num_units) {
			saved_drs->status = DOS_DD_ERR_UNKNOWN_UNIT | DOS_DD_STATUS_ERROR_DONE;
			return;
		}
		if (saved_drs->cmd.ioctlrw.len >= 2) {
			uint8_t far * buf = saved_drs->cmd.ioctlrw.buffer;
			int i = 0;
			do {
				char c = buf[i++];
				switch (c) {
				case 'S': // Partition start (5 bytes)
					_fmemcpy(&data.unit[saved_drs->unit].start_sector, &buf[i], sizeof(uint32_t));
					i += sizeof(uint32_t);
					dprintf(" unit=%hu new start=%lu\n", saved_drs->unit, data.unit[saved_drs->unit].start_sector);
					// Mark unit as valid but needing refresh
					data.unit[saved_drs->unit].exists = true;
					data.unit[saved_drs->unit].ready = false;
					break;
				case 'L': // Partition len (5 bytes)
					_fmemcpy(&data.unit[saved_drs->unit].sectors, &buf[i], sizeof(uint32_t));
					i += sizeof(uint32_t);
					dprintf(" unit=%hu new len=%lu\n", saved_drs->unit, data.unit[saved_drs->unit].sectors);
					// Mark unit as valid but needing refresh
					data.unit[saved_drs->unit].exists = true;
					data.unit[saved_drs->unit].ready = false;
					break;
				case '\0':
					break;
				default:
					dprintf(" invalid control string char '%c' at %d\n", c, i-1);
					saved_drs->cmd.ioctlrw.len = i;
					saved_drs->status = DOS_DD_ERR_WRITE_FAULT | DOS_DD_STATUS_ERROR_DONE;
					return;
				}
			} while (i < saved_drs->cmd.ioctlrw.len);
		} else {
			saved_drs->cmd.ioctlrw.len = 0;
			saved_drs->status = DOS_DD_ERR_WRITE_FAULT | DOS_DD_STATUS_ERROR_DONE;
			return;
		}
		saved_drs->status = DOS_DD_STATUS_DONE;
		break;
	case DOS_DD_FUNC_IOCTL_GENERIC:
		dprintf("ioctl unit=%hu %x:%x buf=%Fp\n", saved_drs->unit,
		        saved_drs->cmd.ioctl.category, saved_drs->cmd.ioctl.minor, saved_drs->cmd.ioctl.data);
		if (saved_drs->unit >= num_units) {
			saved_drs->status = DOS_DD_ERR_UNKNOWN_UNIT | DOS_DD_STATUS_ERROR_DONE;
			return;
		}
		// DOS can call these functions without doing a mediacheck/buildBPB
		// Do not want to "forget" about a media change, so can't just call buildBPB here.
		// But some functions (like get device parameters?) seem to act like a buildBPB.
		if (unit_check_media_changed(saved_drs->unit)) {
			// Init the disk, but not the partition, until we are sure we want to.
			if (!data.ready) {
				if (err = disk_init()) {
					// Cannot access to disk or scan for partitions? No IOCTLs for you!
					saved_drs->status = err | DOS_DD_STATUS_ERROR_DONE;
					return;
				}
			}
		}
		if (saved_drs->cmd.ioctl.category == DOS_IOCTL_DISK) {
			DOS_IOCTL_DISK_DATA far * iod = saved_drs->cmd.ioctl.data;
			CHUDISK_IOCTL_DISK_DATA far * ciod = saved_drs->cmd.ioctl.data;
			DOS_BPB * bpb = &data.unit[saved_drs->unit].bpb;

			switch (saved_drs->cmd.ioctl.minor) {
			case DOS_IOCTL_DISK_GET_DEVICE_PARAMETERS: // Get CHS values (full disk!) and BPB
				dprintf(" get_device_parameters specialFunctions=0x%x\n", iod->devparams.specialFunctions);
				// If partition's BPB is not cached, read it from the disk
				if (!data.unit[saved_drs->unit].ready) {
					// This acts as if buildBPB was called
					if ((err = unit_init(saved_drs->unit))) {
						// TODO Now would be a good time to talk about a default BPB...
						saved_drs->status = err | DOS_DD_STATUS_ERROR_DONE;
						return;
					}
				}

				iod->devparams.specialFunctions = DOS_DEVICEPARAMS_USE_DEVICE_BPB | DOS_DEVICEPARAMS_REGULARTRACKS;
				iod->devparams.deviceType = DOS_DEVICEPARAMS_HDD;
				iod->devparams.deviceAttributes = DOS_DEVICEPARAMS_REMOVABLE;
				if (bpb->numberOfHeads && bpb->sectorsPerTrack) {
					// Compute number of cylinders of entire disk based on this part's BPB...
					iod->devparams.numberOfCylinders =
					        div(data.sectors, bpb->numberOfHeads * bpb->sectorsPerTrack);
					// TODO Check for overflows...
				} else {
					iod->devparams.numberOfCylinders = 0;
				}
				iod->devparams.mediaType = 0;

				// Return only DOS3.x BPB -- not sure if caller will have space for anything larger.
				_fmemcpy(&iod->devparams.bpb, bpb, sizeof(DOS_BPB_DOS3));

				dprintf(" get_device_parameters -> numberOfCylinders=%u\n", iod->devparams.numberOfCylinders);

				saved_drs->status = DOS_DD_STATUS_DONE;
				return;
			case DOS_IOCTL_DISK_GET_MEDIA_ID: // Get the volume label
				if (!data.unit[saved_drs->unit].ready) {
					// TODO Should we reload BPB here like above?
					dputs(" but partition is not ready");
					saved_drs->status = DOS_DD_ERR_MEDIA_UNAVAILABLE | DOS_DD_STATUS_ERROR_DONE;
					return;
				}
				iod->mid.infoLevel = 0;
				if (bpb->sectorsPerFAT != 0) {
					iod->mid.serialNumber = bpb->fat16.volumeSerialNumber;
					_fmemcpy(iod->mid.volumeLabel, bpb->fat16.volumeLabel, 11);
					_fmemcpy(iod->mid.fsType, bpb->fat16.fsType, 8);
				} else if (bpb->fat32.sectorPerFatLarge != 0) {
					iod->mid.serialNumber = bpb->fat32.volumeSerialNumber;
					_fmemcpy(iod->mid.volumeLabel, bpb->fat32.volumeLabel, 11);
					_fmemcpy(iod->mid.fsType, bpb->fat32.fsType, 8);
				}
				dprintf(" get media id -> volumeSerialNumber=%lx volumeLabel='%.11Fs' fsType='%.8Fs'\n",
				        iod->mid.serialNumber, iod->mid.volumeLabel, iod->mid.fsType);
				saved_drs->status = DOS_DD_STATUS_DONE;
				return;
			case DOS_IOCTL_DISK_SET_MEDIA_ID: // Set the volume label, etc.
				if (!data.unit[saved_drs->unit].exists) {
					dputs(" but partition does not exist!");
					saved_drs->status = DOS_DD_ERR_MEDIA_UNAVAILABLE | DOS_DD_STATUS_ERROR_DONE;
					return;
				}
				dprintf(" set media id to volumeSerialNumber=%lx volumeLabel='%.11Fs' fsType='%.8Fs'\n",
				        iod->mid.serialNumber, iod->mid.volumeLabel, iod->mid.fsType);
				if (read_only) {
					saved_drs->status = DOS_DD_ERR_WRITE_PROTECT | DOS_DD_STATUS_ERROR_DONE;
					return;
				}
				// Here we always reload BPB -- do not want to write back outdated stuff.
				if (disk_read_sectors(buffer, data.unit[saved_drs->unit].start_sector, 1) != 1) {
					// Cannot read start sector...
					saved_drs->status = DOS_DD_ERR_READ_FAULT | DOS_DD_STATUS_ERROR_DONE;
					return;
				}
				bpb = (DOS_BPB*) &buffer[DOS_BPB_OFFSET_ON_DISK];
				if (bpb->sectorsPerFAT != 0) {
					bpb->fat16.volumeSerialNumber = iod->mid.serialNumber;
					_fmemcpy(bpb->fat16.volumeLabel, iod->mid.volumeLabel, 11);
					_fmemcpy(bpb->fat16.fsType, iod->mid.fsType, 8);
				} else if (bpb->fat32.sectorPerFatLarge != 0) {
					bpb->fat32.volumeSerialNumber = iod->mid.serialNumber;
					_fmemcpy(bpb->fat32.volumeLabel, iod->mid.volumeLabel, 11);
					_fmemcpy(bpb->fat32.fsType, iod->mid.fsType, 8);
				} else {
					saved_drs->status = DOS_DD_ERR_UNKNOWN_MEDIA | DOS_DD_STATUS_ERROR_DONE;
					return;
				}
				if (disk_write_sectors(buffer, data.unit[saved_drs->unit].start_sector, 1) != 1) {
					// Cannot re-write start sector...
					saved_drs->status = DOS_DD_ERR_WRITE_FAULT | DOS_DD_STATUS_ERROR_DONE;
					return;
				}
				// Re-sync our internal BPB
				memcpy(&data.unit[saved_drs->unit].bpb, bpb, sizeof(DOS_BPB));
				saved_drs->status = DOS_DD_STATUS_DONE;
				return;
			case DOS_IOCTL_DISK_READ_TRACK_ON_LOGICAL_DEVICE: // Absolute read
				dprintf(" read track cylinder %u head %u sector %u sectors %u buf %Fp -> lba %lu\n",
				        iod->rw.cylinder, iod->rw.head, iod->rw.firstSector, iod->rw.sectors, iod->rw.buffer,
				        chs_to_lba(bpb, iod->rw.cylinder, iod->rw.head, iod->rw.firstSector + 1));
				// Need the partition's BPB to map CHS to LBA...
				if (!data.unit[saved_drs->unit].ready) {
					saved_drs->status = DOS_DD_ERR_MEDIA_UNAVAILABLE | DOS_DD_STATUS_ERROR_DONE;
					return;
				}
				if (disk_read_sectors(iod->rw.buffer, chs_to_lba(bpb, iod->rw.cylinder, iod->rw.head, iod->rw.firstSector + 1), iod->rw.sectors) != iod->rw.sectors) {
					saved_drs->status = DOS_DD_ERR_READ_FAULT | DOS_DD_STATUS_ERROR_DONE;
					return;
				}
				saved_drs->status = DOS_DD_STATUS_DONE;
				return;
			case DOS_IOCTL_DISK_WRITE_TRACK_ON_LOGICAL_DEVICE:
				dprintf(" write track cylinder %u head %u sector %u sectors %u buf %Fp -> lba %lu\n",
				        iod->rw.cylinder, iod->rw.head, iod->rw.firstSector, iod->rw.sectors, iod->rw.buffer,
				        chs_to_lba(bpb, iod->rw.cylinder, iod->rw.head, iod->rw.firstSector + 1));
				// Need the partition's BPB to map CHS to LBA...
				if (!data.unit[saved_drs->unit].ready) {
					saved_drs->status = DOS_DD_ERR_MEDIA_UNAVAILABLE | DOS_DD_STATUS_ERROR_DONE;
					return;
				}
				if (read_only) {
					saved_drs->status = DOS_DD_ERR_WRITE_PROTECT | DOS_DD_STATUS_ERROR_DONE;
					return;
				}
				if (disk_write_sectors(iod->rw.buffer, chs_to_lba(bpb, iod->rw.cylinder, iod->rw.head, iod->rw.firstSector + 1), iod->rw.sectors) != iod->rw.sectors) {
					saved_drs->status = DOS_DD_ERR_WRITE_FAULT | DOS_DD_STATUS_ERROR_DONE;
					return;
				}
				saved_drs->status = DOS_DD_STATUS_DONE;
				return;
			case DOS_IOCTL_DISK_FORMAT_TRACK_ON_LOGICAL_DEVICE:
				dprintf(" formatting track cylinder %u head %u\n",
				        iod->fv.cylinder, iod->fv.head);
				if (!data.unit[saved_drs->unit].exists) {
					dputs(" but partition does not exist!");
					saved_drs->status = DOS_DD_ERR_MEDIA_UNAVAILABLE | DOS_DD_STATUS_ERROR_DONE;
					return;
				}
				if (read_only) {
					saved_drs->status = DOS_DD_ERR_WRITE_PROTECT | DOS_DD_STATUS_ERROR_DONE;
					return;
				}
				// Total nop. Should we zerofill the track or something?
				iod->fv.specialFunctions = 1; // Not supported by BIOS?
				saved_drs->status = DOS_DD_STATUS_DONE;
				return;
			case CHUDISK_IOCTL_DISK_READ_ABS_SECTOR:
				dprintf(" read abs sector %lu sectors %u buf %Fp\n",
				        ciod->rw.sector, ciod->rw.sectors, ciod->rw.buffer);
				if (disk_read_sectors(ciod->rwabs.buffer, ciod->rwabs.sector, ciod->rwabs.sectors) != ciod->rwabs.sectors) {
					saved_drs->status = DOS_DD_ERR_READ_FAULT | DOS_DD_STATUS_ERROR_DONE;
					return;
				}
				saved_drs->status = DOS_DD_STATUS_DONE;
				return;
			}
		}

		dputs(" ioctl not supported\n");
		saved_drs->status = DOS_DD_ERR_UNKNOWN_COMMAND | DOS_DD_STATUS_ERROR_DONE;
		break;
	case DOS_DD_FUNC_GET_LOGICAL_DEVICE:
		dprintf("get logical device unit=%hu\n", saved_drs->unit);
		saved_drs->unit = 0;
		saved_drs->status = DOS_DD_STATUS_DONE;
		break;
	case DOS_DD_FUNC_SET_LOGICAL_DEVICE:
		// We do not support this. Ignore it.
		dprintf("set logical device unit=%hu\n", saved_drs->unit);
		saved_drs->status = DOS_DD_STATUS_DONE;
		break;
	case DOS_DD_FUNC_IOCTL_QUERY:
		dprintf("ioctl query %x:%x buf=%Fp\n", saved_drs->cmd.ioctl.category, saved_drs->cmd.ioctl.minor, saved_drs->cmd.ioctl.data);
		switch (saved_drs->cmd.ioctl.category) {
		case DOS_IOCTL_DISK:
			switch (saved_drs->cmd.ioctl.minor) {
			case DOS_IOCTL_DISK_GET_DEVICE_PARAMETERS:
			case DOS_IOCTL_DISK_GET_MEDIA_ID:
			case DOS_IOCTL_DISK_SET_MEDIA_ID:
			case DOS_IOCTL_DISK_READ_TRACK_ON_LOGICAL_DEVICE:
			case DOS_IOCTL_DISK_WRITE_TRACK_ON_LOGICAL_DEVICE:
			case DOS_IOCTL_DISK_FORMAT_TRACK_ON_LOGICAL_DEVICE:
			case CHUDISK_IOCTL_DISK_READ_ABS_SECTOR:
				// Supported
				saved_drs->status = DOS_DD_STATUS_DONE;
				return;
			}
			break;
		}
		// Unsupported
		saved_drs->status = DOS_DD_ERR_UNKNOWN_COMMAND | DOS_DD_STATUS_ERROR_DONE;
		break;
	default:
		dprintf("unknown function received %u\n", saved_drs->function);
		break;
	}
}

// Required by Watcom ABI -- since we do not link to the runtime
void _small_code() {}

// Symbol to let initialization code know the end of resident data
char resident_end;