/*
 * $Id: disk.c,v 1.230 2009-03-30 14:10:25 vrsieh Exp $ 
 *
 * Copyright (C) 2004-2009 FAUmachine Team <info@faumachine.org>.
 * This program is free software. You can redistribute it and/or modify it
 * under the terms of the GNU General Public License, either version 2 of
 * the License, or (at your option) any later version. See COPYING.
 */

#include "build_config.h"
#include "compiler.h"

/* ===================== RUNTIME || INIT ======================== */
#if defined(RUNTIME_RM) || defined(INIT_RM)

#define FD_CHANGELINE_SUPPORT	1

#define IDE_ERR_STAT	0x01
#define IDE_INDEX_STAT	0x02
#define IDE_ECC_STAT	0x04
#define IDE_DRQ_STAT	0x08
#define IDE_SEEK_STAT	0x10
#define IDE_WRERR_STAT	0x20
#define IDE_READY_STAT	0x40
#define IDE_BUSY_STAT	0x80

#define WIN_RESTORE	0x10 /* Recalibrate Device */
#define WIN_IDENTIFY    0xec /* Identify ATA Device */
#define WIN_PIDENTIFY   0xa1 /* Identify ATAPI Device */

struct diskette_param_table_t {
	/* FIXME VOSSI */
	uint8_t x0, x1, x2, x3, x4, x5, x6, x7, x8, x9, x10;
};

extern const uint16_t ide_port_table[];
extern const struct diskette_param_table_t diskette_param_table;

#endif /* RUNTIME_RM || INIT_RM */
/* ==================== RUNTIME_RM ==================== */
#ifdef RUNTIME_RM

CODE16;

#include "assert.h"
#include "fixme.h"
#include "stdio.h"
#include "string.h"
#include "in.h"

#include "io.h"
#include "const.h"
#include "cmos.h"
#include "var.h"
#include "el_torito.h"
#include "ptrace.h"
#include "disk.h"

#define ATAPI_TEST_UNIT_READY	0x00
#define ATAPI_REQUEST_SENSE	0x03
#define ATAPI_READ_SECTOR	0x28

#define ATAPI_NO_SENSE		0
#define ATAPI_NOT_READY		2
#define ATAPI_UNIT_ATTENTION	6

struct atapi_compacket {
	unsigned char opcode;
	unsigned char reserved0;
	uint32_t lba;
	unsigned char reserved1;
	union {
		unsigned short transfer_length;
		unsigned short paramlist_length;
		unsigned short allocation_length;
	} misc;
	unsigned char reserved2;
	unsigned short reserved3;
} __attribute__((__packed__));

struct atapi_request_sense {
	/* Byte 0: */
	/*	Bit 0-6: error code */
	/*	Bit 7: valid */
	unsigned char error_code;
	/* Byte 1: */
	unsigned char segment_number;
	/* Byte 2: */
	/*	Bit 0-3: sense_key */
	/*	Bit 4: reserved */
	/*	Bit 5: ili */
	/*	Bit 6-7: reserved */
	unsigned char sense_key;
	/* Byte 3-6: */
	unsigned char information[4];
	/* Byte 7: */
	unsigned char add_sense_len;
	/* Byte 8-11: */
	unsigned char command_info[4];
	/* Byte 12: */
	unsigned char asc;
	/* Byte 13: */
	unsigned char ascq;
	/* Byte 14: */
	unsigned char fruc;
	/* Byte 15-17: */
	unsigned char sks[3];
};

struct Int13DPT {
	unsigned short size;
	unsigned short flags;
	uint32_t cyls;
	uint32_t heads;
	uint32_t secs;
	uint32_t total_low;	/* Should be long long */
	uint32_t total_high;
	unsigned short bytes_per_sec;
	/* ... */
};


struct Int13Ext {
	unsigned char size;
	unsigned char reserved;
	unsigned short count;
	unsigned short offset;
	unsigned short segment;
#if 0	/* FIXME VOSSI */
	unsigned long long lba;
#else
	uint32_t lba;
	uint32_t lba2;
#endif
};

/*
 * Since no provisions are made for multiple drive types, most
 * values in this table are ignored.  I set parameters for 1.44M
 * floppy here
 */
/* Should be located at 0xefc7 in ROM. FIXME VOSSI */

const struct diskette_param_table_t diskette_param_table = {
	0xAF,
	0x02, /* Head load time 0000001, DMA used. */
	0x25,
	0x02,
	18,
	0x1B,
	0xFF,
	0x6C,
	0xF6,
	0x0F,
	0x08
};

CONST unsigned short ide_port_table[] = {
	/* hda/hdb */ 0x01f0,
	/* hdc/hdd */ 0x0170,
	/* hde/hdf */ 0x01e8,
	/* hdg/hdh */ 0x0168,
	/* hdi/hdj */ 0x01e0,
	/* hdk/hdl */ 0x0160,
};

static void
int13_fail(struct regs *regs, unsigned char status)
{
	if (DL < 2) {
		var_put(fd_res, status);
	} else {
		var_put(hd_res, status);
	}

	AH = status;
	F |= 1 << 0;	/* Set carry. */
}

static void
int13_success(struct regs *regs, unsigned char status)
{
	if (DL < 2) {
		var_put(fd_res, status);
	} else {
		var_put(hd_res, status);
	}

	AH = status;
	F &= ~(1 << 0);	/* Clear carry. */
}

unsigned char
ide_harddisk_read_chs(
	unsigned char device,
	unsigned char num, 
	unsigned short cylinder, 
	unsigned char head,
	unsigned char sector,
	unsigned short buffer_seg,
	unsigned short buffer_off)
{
	uint16_t port;
	uint8_t status;

	assert(/* 0 <= device / 2 && */ device / 2 < 6);
	assert(/* 0 <= device % 2 && */ device % 2 < 2);

	/* Get port addresses. */
	port = const_get(ide_port_table[device / 2]);

	/* Wait while busy. */
	do {
		status = inb(port + 0x206);
	} while (status & IDE_BUSY_STAT);

	/*
	 * Send read command.
	 */
	outb(num, port + 2);		/* number of sectors */
	outb(sector & 0x3f, port + 3);	/* sector */
	outb(cylinder & 0xff, port + 4);/* cylinder (low bits) */
	outb(cylinder >> 8, port + 5);	/* cylinder (high bits) */
	outb(0xa0 | ((device % 2) << 4) | head, port + 6);
					/* unit / head */
	outb(0x20, port + 7);		/* command: read */

	/*
	 * Get all sectors.
	 */
	for ( ; 0 < num; num--) {
		unsigned int count;
		unsigned short data;

		/* Wait while busy. */
		do {
			status = inb(port + 0x206);
		} while (status & IDE_BUSY_STAT);

		if (status & IDE_ERR_STAT) {
			/* ERR bit set */
			assert(0);
			return 0xff;	/* FIXME VOSSI */
		}

		if (! (status & IDE_DRQ_STAT)) {
			/* FIXME VOSSI */
			assert(0);
			return 0xff;
		}

		buffer_seg += buffer_off / 16;
		buffer_off %= 16;

		for (count = 0; count < 512 / 2; count++) {
			data = inw(port);
			put_word(buffer_seg, buffer_off + count * 2, data);
		}

		buffer_seg += 512 / 16;
	}

	return 0;
}

static unsigned char
ide_harddisk_write_chs(
	unsigned char device,
	unsigned char num, 
	unsigned short cylinder, 
	unsigned char head,
	unsigned char sector,
	unsigned short buffer_seg,
	unsigned short buffer_off
)
{
	uint16_t port;
	uint8_t status;

	assert(/* 0 <= device / 2 && */ device / 2 < 6);
	assert(/* 0 <= device % 2 && */ device % 2 < 2);

	/* Get port addresses. */
	port = const_get(ide_port_table[device / 2]);

	/* Wait while busy. */
	do {
		status = inb(port + 0x206);
	} while (status & IDE_BUSY_STAT);

	/*
	 * Send write command.
	 */
	outb(num, port + 2);		/* number of sectors */
	outb(sector & 0x3f, port + 3);	/* sector */
	outb(cylinder & 0xff, port + 4);/* cylinder (low bits) */
	outb(cylinder >> 8, port + 5);	/* cylinder (high bits) */
	outb(0xa0 | ((device % 2) << 4) | head, port + 6);
					/* unit / head */
	outb(0x30, port + 7);		/* command: write */

	/* Wait while busy. */
	do {
		status = inb(port + 0x206);
	} while (status & IDE_BUSY_STAT);

	/*
	 * Send all sectors.
	 */
	for ( ; 0 < num; num--) {
		unsigned int count;
		unsigned short data;

		if (! (status & IDE_DRQ_STAT)) {
			/* FIXME VOSSI */
			assert(0);
			return 0xff;
		}

		/* Send sector. */
		buffer_seg += buffer_off / 16;
		buffer_off %= 16;

		for (count = 0; count < 512 / 2; count++) {
			data = get_word(buffer_seg, buffer_off + count * 2);
			outw(data, port);
		}

		buffer_seg += 512 / 16;

		/* Wait while busy. */
		do {
			status = inb(port + 0x206);
		} while (status & IDE_BUSY_STAT);

		if (status & IDE_ERR_STAT) {
			/* ERR bit set */
			assert(0);
			return 0xff;	/* FIXME VOSSI */
		}
	}

	return 0;
}

static unsigned char
ide_harddisk_read_lba(
	unsigned char controller,
	unsigned char unit,
	unsigned char num, 
	uint32_t /* long long */ start,
	unsigned short buffer_seg,
	unsigned short buffer_off
) {
	uint16_t port;
	uint8_t status;

	assert(/* 0 <= controller && */ controller < 6);
	assert(/* 0 <= unit && */ unit < 2);

	/* Get port addresses. */
	port = const_get(ide_port_table[controller]);

	/* Wait while busy. */
	do {
		status = inb(port + 0x206);
	} while (status & IDE_BUSY_STAT);

	/*
	 * Send read command.
	 */
	outb(num, port + 2);			/* number of sectors */
	outb((start >> 0) & 0xff, port + 3);	/* sector */
	outb((start >> 8) & 0xff, port + 4);	/* cylinder (low bits) */
	outb((start >> 16) & 0xff, port + 5);	/* cylinder (high bits) */
	outb(0xe0 | (unit << 4) | ((start >> 24) & 0xf), port + 6);
						/* unit / head */
	outb(0x20, port + 7);			/* command: read */

	/*
	 * Get all sectors.
	 */
	for ( ; 0 < num; num--) {
		unsigned int count;
		unsigned short data;

		/* Wait while busy. */
		do {
			status = inb(port + 0x206);
		} while (status & IDE_BUSY_STAT);

		if (status & IDE_ERR_STAT) {
			/* ERR bit set */
			return 0xff;    /* FIXME VOSSI */
		}

		/* Read result. */
		if (! (status & IDE_DRQ_STAT)) {
			/* FIXME VOSSI */
			assert(0);
			return 0xff;
		}

		buffer_seg += buffer_off / 16;
		buffer_off %= 16;

		for (count = 0; count < 512 / 2; count++) {
			data = inw(port);
			put_word(buffer_seg, buffer_off + count * 2, data);
		}

		buffer_seg += 512 / 16;
	}

	return 0;
}

static unsigned char
ide_harddisk_write_lba(
	unsigned char controller,
	unsigned char unit,
	unsigned char num, 
	uint32_t /* long long */ start,
	unsigned short buffer_seg,
	unsigned short buffer_off)
{
	uint16_t port;
	uint8_t status;

	assert(/* 0 <= controller && */ controller < 6);
	assert(/* 0 <= unit && */ unit < 2);

	/* Get port addresses. */
	port = const_get(ide_port_table[controller]);

	/* Wait while busy. */
	do {
		status = inb(port + 0x206);
	} while (status & IDE_BUSY_STAT);

	/*
	 * Send write command.
	 */
	outb(num, port + 2);			/* number of sectors */
	outb((start >> 0) & 0xff, port + 3);	/* sector */
	outb((start >> 8) & 0xff, port + 4);	/* cylinder (low bits) */
	outb((start >> 16) & 0xff, port + 5);	/* cylinder (high bits) */
	outb(0xe0 | (unit << 4) | ((start >> 24) & 0xf), port + 6);
						/* unit / head */
	outb(0x30, port + 7);			/* command: write */

	/* Wait while busy. */
	do {
		status = inb(port + 0x206);
	} while (status & IDE_BUSY_STAT);

	/*
	 * Send all sectors.
	 */
	for ( ; 0 < num; num--) {
		unsigned int count;
		unsigned short data;

		if (! (status & IDE_DRQ_STAT)) {
			/* FIXME VOSSI */
			assert(0);
			return 0xff;
		}

		/* Write sector. */
		buffer_seg += buffer_off / 16;
		buffer_off %= 16;

		for (count = 0; count < 512 / 2; count++) {
			data = get_word(buffer_seg, buffer_off + count * 2);
			outw(data, port);
		}

		buffer_seg += 512 / 16;

		/* Wait while busy. */
		do {
			status = inb(port + 0x206);
		} while (status & IDE_BUSY_STAT);

		if (status & IDE_ERR_STAT) {
			/* ERR bit set */
			return 0xff;    /* FIXME VOSSI */
		}
	}

	return 0;
}

static void
floppy_poweron(unsigned short drive)
{
	var_put(fmot_tmout, 0x5a); /* About 5sec at 18Hz. */

	if (var_get(fmot_flag) & (1 << drive)) {
		/* Floppy already spinning... */
		return;
	}

	var_put(fmot_flag, var_get(fmot_flag) | (1 << drive));
	outb(0x0c | ((var_get(fmot_flag) & 0x0f) << 4) | drive, 0x3f2);

	/* Wait for floppy spinning up... */
	/* FIXME VOSSI */
}

static ALWAYS_INLINE void
floppy_poweroff(void)
{
	outb(0x0c, 0x3f2);
	var_put(fmot_flag, 0x00);
}

void
floppy_tick(void)
{
	unsigned char timeout;

	timeout = var_get(fmot_tmout);

	if (timeout == 0) {
		/* Nothing to do... */
		return;
	}

	timeout--;
	var_put(fmot_tmout, timeout);

	if (timeout != 0) {
		/* Nothing to do... */
		return;
	}

	/* Power-off motor. */
	floppy_poweroff();
}

static unsigned char
floppy_err(unsigned char st0, unsigned char st1, unsigned char st2)
{
	unsigned char ret;

	if (((st0 >> 6) & 3) == 0) { /* Interrupt code. */
		ret = 0; /* No error. */

	} else {
		/* Error. */
		if ((st1 >> 0) & 1) { /* Missing address mark. */
			ret = 0x02; /* Missing address mark. */
		} else if ((st1 >> 1) & 1) { /* Not writable. */
			ret = 0x03; /* Write protected. */
		} else if ((st1 >> 2) & 1) { /* No data. */
			ret = 0x04; /* Sector not found / read error. */
		} else if ((st1 >> 4) & 1) { /* Overrun/underrun. */
			ret = 0x08; /* DMA overrun/underrun. */
		} else if ((st1 >> 5) & 1) { /* Data error. */
			ret = 0x10; /* CRC checksum error. */
		} else {
			ret = 0x80; /* Unknown error. */ /* FIXME VOSSI */
		}
	}

#if 0
	dprintf("floppy_err: ret=0x%02x\n", ret);
#endif

	return ret;
}

static unsigned char
floppy_read_chs(
	unsigned char drive,
	unsigned char num,
	unsigned char cylinder,
	unsigned char head,
	unsigned char sector,
	unsigned short buffer_seg,
	unsigned short buffer_off
)
{
	unsigned char status;
	unsigned long size;
	unsigned long address;

	if (1 < drive) {
		return 0x01;	/* invalid parameter */
	}

	if (1 < head) {
		return 0x01;	/* invalid parameter */
	}

	size = num * 512 - 1;
	address = ((unsigned long) buffer_seg << 4) + buffer_off;
	if((address & 0xffff) + size > 0xffff) {
		return 0x09;	/* dma boundary */
	}

	/*
	 * Start motor
	 */
	floppy_poweron(drive);
	
	/*
	 * Set up DMA
	 */
	outb(0x06, 0x0a); /* Enable DMA 2. */
	
	outb(0x00, 0x0c); /* Clear byte pointer flip/flop. */
	outb((address >> 0) & 0xff, 0x04); /* Address low byte. */
	outb((address >> 8) & 0xff, 0x04); /* Address high byte. */
	
	outb(0x00, 0x0c); /* Clear byte pointer flip/flop. */
	outb((size >> 0) & 0xff, 0x05); /* Count low byte. */
	outb((size >> 8) & 0xff, 0x05); /* Count high byte. */
	
	/* Mode register: single block, inc addr, no autoinit, read, chan 2 */
	outb((1 << 6) | (0 << 5) | (0 << 4) | (1 << 2) | 2, 0x0b);

	outb((address >> 16) & 0xff, 0x81); /* Page register */

	/*
	 * Wait until DR is ready
	 */
	do {
		status = inb(0x3f4);
	} while ((status & 0xc0) != 0x80);

	/*
	 * Send read command.
	 */
	outb(0xc6, 0x3f5); /* READ with multitrack */
	outb((head << 2) | drive, 0x3f5);
	outb(cylinder, 0x3f5); /* cylinder */
	outb(head, 0x3f5); /* head */
	outb(sector, 0x3f5); /* sector */
	outb(0x02, 0x3f5); /* sectorsize = 0x02 >> 7 */
	outb(18, 0x3f5); /* sector per track/side */
	outb(0x1b, 0x3f5); /* length of GAP 3 */
	outb(0xff, 0x3f5); /* data length (0xff = unused) */
	
	/*
	 * Wait until command is has been finished
	 */
	var_put(f_recal, var_get(f_recal) & ~(1 << 7));
	while (! (var_get(f_recal) & (1 << 7))) {
		asm volatile (
			"sti\n\t"
			"hlt\n\t"
			"cli\n\t"
		);
	}

	/*
	 * Read result
	 */
	var_put(f_stat[0], inb(0x3f5));
	var_put(f_stat[1], inb(0x3f5));
	var_put(f_stat[2], inb(0x3f5));
	var_put(f_stat[3], inb(0x3f5));
	var_put(f_stat[4], inb(0x3f5));
	var_put(f_stat[5], inb(0x3f5));
	var_put(f_stat[6], inb(0x3f5));

	/*
	 * DMA
	 */
	outb(0x02, 0x0a); /* Disable DMA 2. */

	return floppy_err(var_get(f_stat[0]),
			var_get(f_stat[1]), var_get(f_stat[2]));
}

static unsigned char
floppy_write_chs(
	unsigned char drive,
	unsigned char num,
	unsigned char cylinder,
	unsigned char head,
	unsigned char sector,
	unsigned short buffer_seg,
	unsigned short buffer_off
)
{
	unsigned char status;
	unsigned long size;
	unsigned long address;

	if (1 < drive) {
		return 0x01;	/* invalid parameter */
	}

	if (1 < head) {
		return 0x01;	/* invalid parameter */
	}

	size = num * 512 - 1;
	address = ((unsigned long) buffer_seg << 4) + buffer_off;
	if((address & 0xffff) + size > 0xffff) {
		return 0x09;	/* dma boundary */
	}

	/*
	 * Start motor
	 */
	floppy_poweron(drive);
	
	/*
	 * Set up DMA
	 */
	outb(0x06, 0x0a); /* Enable DMA 2. */
	
	outb(0x00, 0x0c); /* Clear byte pointer flip/flop. */
	outb((address >> 0) & 0xff, 0x04); /* Address low byte. */
	outb((address >> 8) & 0xff, 0x04); /* Address high byte. */
	
	outb(0x00, 0x0c); /* Clear byte pointer flip/flop. */
	outb((size >> 0) & 0xff, 0x05); /* Count low byte. */
	outb((size >> 8) & 0xff, 0x05); /* Count high byte. */
	
	/* Mode register: single block, inc addr, no autoinit, write, chan 2 */
	outb((1 << 6) | (0 << 5) | (0 << 4) | (2 << 2) | 2, 0x0b);

	outb((address >> 16) & 0xff, 0x81); /* Page register */

	/*
	 * Wait until DR is ready
	 */
	do {
		status = inb(0x3f4);
	} while ((status & 0xc0) != 0x80);

	/*
	 * Send write command.
	 */
	outb(0xc5, 0x3f5); /* WRITE */
	outb((head << 2) | drive, 0x3f5);
	outb(cylinder, 0x3f5); /* cylinder */
	outb(head, 0x3f5); /* head */
	outb(sector, 0x3f5); /* sector */
	outb(0x02, 0x3f5); /* sectorsize = 0x02 >> 7 */
	outb(18, 0x3f5); /* sectors per track/side */
	outb(0x1b, 0x3f5); /* length of GAP 3 */
	outb(0xff, 0x3f5); /* data length (0xff = unused) */
	
	/*
	 * Wait until command is has been finished
	 */
	var_put(f_recal, var_get(f_recal) & ~(1 << 7));
	while (! (var_get(f_recal) & (1 << 7))) {
		asm volatile (
			"sti\n\t"
			"hlt\n\t"
			"cli\n\t"
		);
	}

	/*
	 * Read result
	 */
	var_put(f_stat[0], inb(0x3f5));
	var_put(f_stat[1], inb(0x3f5));
	var_put(f_stat[2], inb(0x3f5));
	var_put(f_stat[3], inb(0x3f5));
	var_put(f_stat[4], inb(0x3f5));
	var_put(f_stat[5], inb(0x3f5));
	var_put(f_stat[6], inb(0x3f5));
	
	/*
	 * DMA
	 */
	outb(0x02, 0x0a); /* Disable DMA 2. */

	return floppy_err(var_get(f_stat[0]),
			var_get(f_stat[1]), var_get(f_stat[2]));
}

static unsigned char
floppy_verify_chs(
	unsigned char drive,
	unsigned char num,
	unsigned char cylinder,
	unsigned char head,
	unsigned char sector,
	unsigned short buffer_seg,
	unsigned short buffer_off)
{
	return 0x01;
}

#if FD_CHANGELINE_SUPPORT
static unsigned char
floppy_check_changeline(unsigned char drive)
{
	unsigned char status;
	unsigned char result;

	if (2 <= drive) {
		return 0x80;	/* drive not ready or present */
	}

	/* FIXME sand:
	 * not sure, whether this method to figure out
	 * disk change is correct...
	 */

	floppy_poweron(drive);

	if (inb(0x3f7) & 0x80) {
		/*
		 * Wait until DR is ready
		 */
		do {
			status = inb(0x3f4);
		} while ((status & 0xc0) != 0x80);

		/*
		 * Send recalibrate command.
		 */
		outb(0x07, 0x3f5); /* RECALIBRATE */
		outb(drive, 0x3f5);

		/*
		 * Wait until command has been finished
		 */
		var_put(f_recal, var_get(f_recal) & ~(1 << 7));
		while (! (var_get(f_recal) & (1 << 7))) {
			asm volatile (
					"sti\n\t"
					"hlt\n\t"
					"cli\n\t"
				     );
		}

		/*
		 * Wait until DR is ready
		 */
		do {
			status = inb(0x3f4);
		} while ((status & 0xc0) != 0x80);

		/*
		 * Send sense interrupt status command.
		 */
		outb(0x08, 0x3f5); /* SENSE INTERRUPT STATUS */
		outb(drive, 0x3f5);

		/*
		 * Wait until DR is ready
		 */
		do {
			status = inb(0x3f4);
		} while ((status & 0xd0) != 0xd0);

		var_put(f_stat[0], inb(0x3f5));
		var_put(f_stat[1], inb(0x3f5));

		if (var_get(f_stat[1]) == 1
		 && (var_get(f_stat[0]) & 0xf0) == 0x20) {
			if (inb(0x37f) & 0x80) {
				result = 0x80;	/* drive not ready or present */
			} else {
				result = 0x06; /* change line active */
			}
		} else {
			result = 0x80; /* drive not ready or present */
		}
	} else {
		result = 0; /* disk not changed */
	}

	floppy_poweroff();

	return result;
}
#endif /* FD_CHANGELINE_SUPPORT */

static void
outsw(unsigned short port, unsigned short *addr, uint32_t count)
{
	for ( ; 0 < count; count--) {
		outw(*addr++, port);
	}
}

static unsigned char
send_atapi_pc(
	unsigned short unit,
	unsigned short port,
	struct atapi_compacket *command
)
{
	uint8_t status;

	/* Select drive. */
	outb(unit << 4, port + 6);

	/* Wait while busy. */
	do {
		status = inb(port + 0x206);
	} while (status & IDE_BUSY_STAT);

	/* Send packet command. */
	outb(0x00, port + 1);		/* No DMA, no overlay */
	outb(2048 % 256, port + 4);	/* 2048 bytes transfer */
	outb(2048 / 256, port + 5);
	outb(0xa0, port + 7);		/* Packet command */

	/* Wait while busy. */
	do {
		status = inb(port + 0x206);
	} while (status & IDE_BUSY_STAT);
	if (status & IDE_ERR_STAT) {
		/* Use sense command - FIXME MARCEL */
		return 1;
	}
	assert(status & IDE_DRQ_STAT);

	assert(((inb(port + 2) >> 0) & 1) == 1); /* Command */
	assert(((inb(port + 2) >> 1) & 1) == 0); /* Out */
	assert(inb(port + 4) == 2048 % 256);
	assert(inb(port + 5) == 2048 / 256);

	/* Send command packet. */
	outsw(port, (void *) command, sizeof(struct atapi_compacket) / 2);

	/* Wait while busy. */
	do {
		status = inb(port + 0x206);
	} while (status & IDE_BUSY_STAT);
	if (status & IDE_ERR_STAT) {
		return 1;
	}

	return 0;
}

static unsigned char
atapi_request_sense(
	unsigned short unit,
	unsigned short port,
	struct atapi_request_sense *rs
)
{
	unsigned short *buf = (unsigned short *) rs;
	unsigned char command[12];
	unsigned char status;
	unsigned short count;

	for (count = 0; count < sizeof(command) / sizeof(command[0]); count++) {
		command[count] = 0;
	}
	command[0] = ATAPI_REQUEST_SENSE;
	command[4] = sizeof(*rs);

	/*
	 * Send ATAPI command.
	 */
	if (send_atapi_pc(unit, port, (struct atapi_compacket *) command)) {
		return 1;
	}

	/*
	 * Wait for result.
	 */
	do {
		status = inb(port + 0x206);
	} while (status & IDE_BUSY_STAT);
	if (status & IDE_ERR_STAT) {
		return 1;
	}

	/*
	 * Get result.
	 */
	for (count = 0; count < sizeof(*rs) / 2; count++) {
		assert(status & IDE_DRQ_STAT);
		*buf++ = inw(port);
	}

	return 0;
}

unsigned char
read_sector_lba_atapi(
	unsigned char device,
	unsigned short num,
	uint32_t /* long long */ block,
	unsigned short buffer_seg,
	unsigned short buffer_off,
	unsigned int sector_size
	)
{
	unsigned short port;
	unsigned short atapi_num;
	unsigned short start_dummy;
	unsigned short end_dummy;
	unsigned int ret;
	unsigned int count;
	unsigned int sector_count;
	unsigned short data;
	union {
		struct atapi_compacket command;
		struct atapi_request_sense rs;
	} u;
	unsigned char status;

again:	;
	assert(/* 0 <= device / 2 && */ device / 2 < 6);
	assert(/* 0 <= device % 2 && */ device % 2 < 2);

	/* Get port addresses. */
	port = const_get(ide_port_table[device / 2]);

	/*
	 * Send read command.
	 */
	u.command.opcode = ATAPI_READ_SECTOR;
	u.command.reserved0 = 0;
	u.command.reserved1 = 0;
	u.command.reserved2 = 0;
	u.command.reserved3 = 0;
	if (sector_size == 2048) {
		/* CDROM mode */
		u.command.lba = htonl(block);
		start_dummy = 0;
		end_dummy = 0;
		atapi_num = num;

	} else {
		/* floppy emulation mode */
		unsigned long slba;
		unsigned long elba;

		assert(sector_size == 512);

		slba = block;
		elba = block + num;

		u.command.lba = htonl(slba / 4);
		start_dummy = slba % 4;
		if (elba % 4 == 0) {
			end_dummy = 0;
		} else {
			end_dummy = 4 - elba % 4;
		}
		atapi_num = elba / 4 - slba / 4;
		if (elba % 4 != 0) {
			atapi_num++;
		}
	}
	u.command.misc.transfer_length = htons(atapi_num);

	ret = send_atapi_pc(device % 2, port, &u.command);
	if (ret != 0) {
		ret = atapi_request_sense(device % 2, port, &u.rs);
		if (ret != 0) {
			return ret;
		}
		if ((u.rs.sense_key & 0xf) == ATAPI_UNIT_ATTENTION) {
			goto again;
		}
		return 1;
	}

	/* read dummy data */
	for (sector_count = 0; sector_count < start_dummy; sector_count++) {
		do {
			status = inb(port + 0x206);
		} while (status & IDE_BUSY_STAT);
		if (status & IDE_ERR_STAT) {
			return 1;
		}
		assert(status & IDE_DRQ_STAT);
		for (count = 0; count < sector_size / 2; count++) {
			data = inw(port);
		}
	}
	/*
	 * Get wanted sectors.
	 */
	for (sector_count = 0; sector_count < num; sector_count++) {
		do {
			status = inb(port + 0x206);
		} while (status & IDE_BUSY_STAT);
		if (status & IDE_ERR_STAT) {
			return 1;
		}
		assert(status & IDE_DRQ_STAT);
		for (count = 0; count < sector_size / 2; count++) {
			data = inw(port);
			put_word(buffer_seg, buffer_off, data);
			buffer_off += 2;
			if (16 <= buffer_off) {
				buffer_seg++;
				buffer_off -= 16;
			}
		}
	}
	/* read dummy data */
	for (sector_count = 0; sector_count < end_dummy; sector_count++) {
		do {
			status = inb(port + 0x206);
		} while (status & IDE_BUSY_STAT);
		if (status & IDE_ERR_STAT) {
			return 1;
		}
		assert(status & IDE_DRQ_STAT);
		for (count = 0; count < sector_size / 2; count++) {
			data = inw(port);
		}
	}

	return 0;
}

static void
int13_eltorito(struct regs *regs)
{
	struct info {
		unsigned char size;
		unsigned char media;
		unsigned char emulated_drive;
		unsigned char controller_index;
		unsigned long ilba;
		unsigned short device_spec;
		unsigned short buffer_segment;
		unsigned short load_segment;
		unsigned short sector_count;
		unsigned char cylinders;
		unsigned char spt;
		unsigned char heads;
	};

	if (AH == 0x4a		/* ElTorito - Initiate disk emu */
	 || AH == 0x4c		/* ElTorito - Initiate disk emu and boot */
	 || AH == 0x4d) {	/* ElTorito - Return Boot catalog */
		/* FIXME VOSSI */
		assert(0);
		int13_fail(regs, 0x01);

	} else if (AH == 0x4b) {
		/*
		 * ElTorito - Terminate disk emu
		 */
		if (ebda_get(cdemu.media) == 0x00) {
			/* No Emulation */
			/* Nothing to do... */

		} else if (0x01 <= ebda_get(cdemu.media)
			&& ebda_get(cdemu.media) <= 0x03) {
			/* Floppy Emulation */
			/* Decrement number of floppies. */
			uint16_t sys_conf;
			uint8_t num;

			sys_conf = var_get(sys_conf);

			assert((sys_conf >> 0) & 1);
			num = ((sys_conf >> 6) & 0x3) + 1;
			num--;

			if (! num) {
				sys_conf &= ~(1 << 0);
				sys_conf &= ~(3 << 6);
			} else {
				sys_conf |= 1 << 0;
				sys_conf &= ~(3 << 6);
				sys_conf |= (num - 1) << 6;
			}

			var_put(sys_conf, sys_conf);

		} else if (ebda_get(cdemu.media) == 0x04) {
			/* Harddisk */
			uint8_t hd_num;

			hd_num = var_get(hd_num);

			assert(hd_num);
			hd_num--;

			var_put(hd_num, hd_num);

		} else {
			assert(0);
		}

		put_byte(DS, SI + offsetof(struct info, size),
			0x13);
		put_byte(DS, SI + offsetof(struct info, media),
			ebda_get(cdemu.media));
		put_byte(DS, SI + offsetof(struct info, emulated_drive),
			ebda_get(cdemu.emulated_drive));
		put_byte(DS, SI + offsetof(struct info, controller_index),
			ebda_get(cdemu.controller_index));
		put_long(DS, SI + offsetof(struct info, ilba),
			ebda_get(cdemu.ilba));
		put_word(DS, SI + offsetof(struct info, device_spec),
			ebda_get(cdemu.device_spec));
		put_word(DS, SI + offsetof(struct info, buffer_segment),
			ebda_get(cdemu.buffer_segment));
		put_word(DS, SI + offsetof(struct info, load_segment),
			ebda_get(cdemu.load_segment));
		put_word(DS, SI + offsetof(struct info, sector_count),
			ebda_get(cdemu.sector_count));
		put_byte(DS, SI + offsetof(struct info, cylinders),
			ebda_get(cdemu.vdevice.cylinders));
		put_byte(DS, SI + offsetof(struct info, spt),
			ebda_get(cdemu.vdevice.spt));
		put_byte(DS, SI + offsetof(struct info, heads),
			ebda_get(cdemu.vdevice.heads));

		if (AL == 0x00) {
			/* Should be handled accordingly to spec */
			/* FIXME VOSSI */
			ebda_put(cdemu.active, 0);
		}

		var_put(hd_res, 0x00);
		int13_success(regs, 0x00);
	}
}

static void
int13_floppy(struct regs *regs)
{
	unsigned char ret;
#if 0
	assert(/* 0x00 <= DL && */ DL <= 0x01);
#endif

	if (AH == 0x00) {
		/*
		 * Floppy controller reset.
		 */
		unsigned char drive_type;

		if (2 <= DL) {
			int13_fail(regs, 0x01);	/* Invalid param. */
			return;
		}

		drive_type = cmos_get(fd_config);
		if (DL == 0) {
			drive_type >>= 4;
		} else { assert(DL == 1);
			drive_type &= 0xf;
		}

		if (drive_type == 0) {
			int13_fail(regs, 0x80);	/* Not responding. */
			return;
		}

		var_put(f_track[DL], 0);
		int13_success(regs, 0x00);

	} else if (AH == 0x01) {
		/*
		 * Read floppy status.
		 */
		if (var_get(fd_res) == 0) {
			int13_success(regs, 0x00);
		} else {
			int13_fail(regs, var_get(fd_res));
		}

	} else if (AH == 0x02) {
		/*
		 * Read floppy sectors.
		 */
		ret = floppy_read_chs(DL, AL, CH, DH, CL, ES, BX);
		if (ret == 0) {
			int13_success(regs, 0x00);
		} else {
			AL = 0;
			int13_fail(regs, ret);
		}

	} else if (AH == 0x03) {
		/*
		 * Write floppy sectors.
		 */
		ret = floppy_write_chs(DL, AL, CH, DH, CL, ES, BX);
		if (ret == 0) {
			int13_success(regs, 0x00);
		} else {
			AL = 0;
			int13_fail(regs, ret);
		}

	} else if (AH == 0x04) {
		/*
		 * Verify floppy sectors.
		 */
		ret = floppy_verify_chs(DL, AL, CH, DH, CL, ES, BX);
		if (ret == 0) {
			int13_success(regs, 0x00);
		} else {
			AL = 0;
			int13_fail(regs, ret);
		}

	} else if (AH == 0x05) {
		/*
		 * Format disk track.
		 */
		dprintf("int13_floppy: Unknown function %02x\n", AH);
		assert(0);

	} else if (AH == 0x08) {
		/*
		 * Read floppy drive parameters.
		 */
		static CONST struct {
			unsigned char max_track;
			unsigned char sectors;
			unsigned char max_head;

		} floppy_types[] = {
			/* 0 none         */	{ 0, 0, 0 },
			/* 1 360KB, 5.25" */	{ 40-1, 9, 2-1 },
			/* 2 1.2MB, 5.25" */	{ 80-1, 15, 2-1 },
			/* 3 720KB, 3.5"  */	{ 80-1, 9, 2-1 },
			/* 4 1.44MB, 3.5" */	{ 80-1, 18, 2-1 },
			/* 5 2.88MB, 3.5" */	{ 80-1, 36, 2-1 },
			/* 6 160KB, 5.25" */	{ 40-1, 8, 1-1 },
			/* 7 180KB, 5.25" */	{ 40-1, 9, 1-1 },
			/* 8 320KB, 5.25" */	{ 40-1, 8, 2-1 },
			/* 9 unknown      */	{ 0, 0, 0 },
			/* a unknown      */	{ 0, 0, 0 },
			/* b unknown      */	{ 0, 0, 0 },
			/* c unknown      */	{ 0, 0, 0 },
			/* d unknown      */	{ 0, 0, 0 },
			/* e unknown      */	{ 0, 0, 0 },
			/* f unknown      */	{ 0, 0, 0 }
		};

		unsigned char num_floppies;
		unsigned char drive_type;

		num_floppies = 0;
		drive_type = cmos_get(fd_config);
		if ((drive_type >> 0) & 0xf) {
			num_floppies++;
		}
		if ((drive_type >> 4) & 0xf) {
			num_floppies++;
		}

		if (2 <= DL) {
			AL = 0;
			BX = 0;
			CX = 0;
			DX = 0;
			ES = 0;
			DI = 0;
			DL = num_floppies;
			AH = 0x00;	/* no error! */
			F |= 1 << 0;	/* Set carry. */
			return;
		}

		if (DL == 0) {
			drive_type >>= 4;
		} else { assert(DL == 1);
			drive_type &= 0xf;
		}

		BH = 0;
		BL = drive_type;
		AL = 0;
		DL = num_floppies;
		CH = const_get(floppy_types[drive_type].max_track);
		CL = const_get(floppy_types[drive_type].sectors);
		DH = const_get(floppy_types[drive_type].max_head);

		/* Get diskette parameter table stored in INT vector 0x1e. */
		DI = get_word(0x0000, 0x1e * 4 + 0);
		ES = get_word(0x0000, 0x1e * 4 + 2);

		/* Disk status not changed upon success. */
		AH = 0x00;
		F &= ~(1 << 0); /* Clear carry. */

	} else if (AH == 0x15) {
		/*
		 * Read floppy drive type.
		 */
		unsigned char drive_type;

		if (2 <= DL) {
			/* var_put(fmot_stat, ...);	FIXME VOSSI */
			int13_fail(regs, 0x01);
			return;
		}

		drive_type = cmos_get(fd_config);
		if (DL == 0) {
			drive_type >>= 4;
		} else { assert(DL == 1);
			drive_type &= 0xf;
		}
		if (drive_type == 0) {
			int13_success(regs, 0x00);	/* Drive not present. */
		} else {
							/* Drive present. */
#if FD_CHANGELINE_SUPPORT
			int13_success(regs, 0x02);	/* Does support */
							/* change line. */
#else
			int13_success(regs, 0x01);	/* Does not support */
							/* change line. */
#endif
		}
	} else if (AH == 0x16) {
		/*
		 * Get floppy change line status.
		 */
#if FD_CHANGELINE_SUPPORT
		ret = floppy_check_changeline(DL);
		if(ret == 0) {
			/* disk not changed: return successfully */
			int13_success(regs,0x00);
		} else {
			/* other cases:
			 *  - disk changed
			 *  - no disk present,
			 *  - drive not ready,
			 *  - drive not present,
			 *  - ...
			 * => return as if failed
			 */
			int13_fail(regs,ret);
		}
#else
		dprintf("int13_floppy: Unknown function %02x\n", AH);
		assert(0);
#endif
	} else if (AH == 0x17) {
		/*
		 * Set floppy type for format (old).
		 */
		dprintf("int13_floppy: Unknown function %02x\n", AH);
		assert(0);

	} else if (AH == 0x18) {
		/*
		 * Set floppy type for format (new).
		 * AH:			0x18
		 * DL:			drive
		 * CH:			cylinder number (bits 0-7)
		 * CL (bit 6-7):	cylinder number (bits 8-9)
		 * CL (bit 0-5):	sectors per track
		 */
		dprintf("int13_floppy: Set floppy type called (%d,%d).\n",
				(((CL >> 6) & 3) << 8) | CH, (CL >> 0) & 0x3f);
		int13_success(regs, 0x00);	/* FIXME VOSSI */

	} else if (AH == 0x41) {
		/*
		 * BIOS Enhanced Disk Drive call:
		 * "Check Extensions Present".
		 */
		/* Not supported for floppy disks. FIXME VOSSI */
		int13_fail(regs, 0x01);

	} else {
		dprintf("int13_floppy: Drive 0x%02x: Unknown function %02x\n",
				DL, AH);
		int13_fail(regs, 0x01);
	}
}

static void
int13_harddisk(struct regs *regs)
{
	unsigned char device;

	if (DL < 0x80 || 0x80 + MAX_BIOS_HDS <= DL) {
		var_put(hd_res, 0x01);
		AH = 0x01;
		F |= 1 << 0; /* Set carry. */
		return;
	}
	device = ebda_get(hdidmap[DL - 0x80]);
	if (MAX_BIOS_HDS <= device) {
		var_put(hd_res, 0x01);
		AH = 0x01;
		F |= 1 << 0; /* Set carry. */
		return;
	}

	/* Clear completion flag. */
	var_put(hd_irq, 0);

	if (AH == 0x00) {
		/*
		 * Reset disk controller.
		 */
		/* ata_reset(device); */	/* FIXME VOSSI */
		var_put(hd_res, 0x00);
		AH = 0x00;
		F &= ~(1 << 0); /* Clear carry. */

	} else if (AH == 0x02
		|| AH == 0x03
		|| AH == 0x04) {
		/*
		 * Read disk sectors.
		 * Write disk sectors.
		 * Verify disk sectors.
		 */
		unsigned char count;
		unsigned short cylinder;
		unsigned char sector;
		unsigned char head;
		unsigned short segment;
		unsigned short offset;
		unsigned char status;

		count = AL;
		cylinder = ((unsigned short) (CL & 0xc0) << 2) | CH;
		sector = CL & 0x3f;
		head = DH;

		segment = ES;
		offset = BX;

		/* Sanity check on count. */
		if (128 < count || count == 0) {
			var_put(hd_res, 0x01);
			AH = 0x01;
			F |= 1 << 0; /* Set carry. */
			return;
		}

		/* Sanity check on C/H/S. */
		if (ebda_get(devices[device].lchs_cylinders) <= cylinder
		 || ebda_get(devices[device].lchs_heads) <= head
		 || ebda_get(devices[device].lchs_spt) < sector) {
			var_put(hd_res, 0x01);
			AH = 0x01;
			F |= 1 << 0; /* Set carry. */
			return;
		}

		if (AH == 0x02) {
			status = ide_harddisk_read_chs(device,
					count, cylinder, head, sector,
					segment, offset);
		} else if (AH == 0x03) {
			status = ide_harddisk_write_chs(device,
					count, cylinder, head, sector,
					segment, offset);
		} else {
			/* Verify */
			status = 0; /* FIXME */
		}
		
		/* Set nb of sector transferred. */
		/* FIXME VOSSI */

		if (status != 0) {
			var_put(hd_res, 0x0c);
			AH = 0x0c;
			F |= 1 << 0; /* Set carry. */
			return;
		}

		var_put(hd_res, 0x00);
		AH = 0x00;
		F &= ~(1 << 0); /* Clear carry. */

	} else if (AH == 0x05) {
		/*
		 * Format disk track.
		 */
		/* FIXME VOSSI */
		var_put(hd_res, 0x00);
		AH = 0x00;
		F &= ~(1 << 0); /* Clear carry. */

	} else if (AH == 0x08) {
		/*
		 * Read disk drive parameters.
		 */
		unsigned short nlc;
		unsigned short nlh;
		unsigned short nlspt;
		unsigned short count;

		nlc = ebda_get(devices[device].lchs_cylinders);
		if (1024 < nlc) {
			nlc = 1024;	/* FIXME VOSSI */
		}
		nlh = ebda_get(devices[device].lchs_heads);
		nlspt = ebda_get(devices[device].lchs_spt);
		count = ebda_get(harddrives);

		nlc = nlc - 2; /* 0 based , last sector not used */

		AL = 0;
		CH = nlc & 0xff;
		CL = ((nlc >> 8) << 6) | (nlspt & 0x3f);
		DH = nlh - 1;
		DL = count;	/* FIXME returns 0, 1, or n hard drives */

		var_put(hd_res, 0x00);
		AH = 0x00;
		F &= ~(1 << 0); /* Clear carry. */

	} else if (AH == 0x10) {
		/*
		 * Check drive ready.
		 */
		/* FIXME */

		var_put(hd_res, 0x00);
		AH = 0x00;
		F &= ~(1 << 0); /* Clear carry. */

	} else if (AH == 0x11) {
		/*
		 * Recalibrate drive.
		 */
		/* FIXME */

		var_put(hd_res, 0x00);
		AH = 0x00;
		F &= ~(1 << 0); /* Clear carry. */

	} else if (AH == 0x15) {
		/*
		 * Read disk drive size.
		 */
		unsigned short npc;
		unsigned short nph;
		unsigned short npspt;
		unsigned long lba;

		/* Get physical geometry from table. */
		/* FIXME VOSSI */
		npc = ebda_get(devices[device].pchs_cylinders);
		nph = ebda_get(devices[device].pchs_heads);
		npspt = ebda_get(devices[device].pchs_spt);

		/* Compute sector count seen by int13. */
		lba = (unsigned long) (npc - 1)
			* (unsigned long) nph
			* (unsigned long) npspt;

		CX = (lba >> 16) & 0xffff;
		DX = (lba >>  0) & 0xffff;

		var_put(hd_res, 0x00);
		AH = 0x03; /* Type: 3 (Harddisk) */
		F &= ~(1 << 0); /* Clear carry. */

	} else if (AH == 0x41) {
		/*
		 * BIOS Enhanced Disk Drive call:
		 * "Check Extensions Present".
		 */
		BX = 0xaa55;	/* Install check. */
		CX = 0x0007;	/* Options supported. */

		var_put(hd_res, 0x00);
		AH = 0x30; /* Version 3.0 */
		F &= ~(1 << 0); /* Clear carry. */

	} else if (AH == 0x42
		|| AH == 0x43
		|| AH == 0x44
		|| AH == 0x47) {
		/*
		 * Extended read.
		 * Extended write.
		 * Extended verify.
		 * Extended seek.
		 */
		unsigned short count;
		unsigned short segment;
		unsigned short offset;
#if 0
		unsigned long long lba;
#else
		unsigned long lba;
#endif
		unsigned char status;


		count = get_word(DS, SI + 2);
		segment = get_word(DS, SI + 6);
		offset = get_word(DS, SI + 4);
#if 0
		lba = get_qword(DS, SI + 8);
#else
		lba = get_long(DS, SI + 8);
#endif

		/* Sanity check on lba. */
		if (ebda_get(devices[device].sectors) <= lba) {
			var_put(hd_res, 0x01);
			AH = 0x01;
			F |= 1 << 0; /* Set carry. */
			return;
		}

		if (AH == 0x44 || AH == 0x47) {
			/* Verify or seek. */
			var_put(hd_res, 0x00);
			AH = 0x00;
			F &= ~(1 << 0); /* Clear carry. */
			return;
		}

		if (AH == 0x42) {
			/* Read */
			status = ide_harddisk_read_lba(device / 2, device % 2,
					count, lba, segment, offset);
		} else { assert(AH == 0x43);
			/* Write */
			status = ide_harddisk_write_lba(device / 2, device % 2,
					count, lba, segment, offset);
		}

		if (status != 0) {
			var_put(hd_res, 0x0c);
			AH = 0x0c;
			F |= 1 << 0; /* Set carry. */
			return;
		}

		/* FIXME VOSSI */
		put_word(DS, SI + 2, count);

		var_put(hd_res, 0x00);
		AH = 0x00;
		F &= ~(1 << 0); /* Clear carry. */

	} else if (AH == 0x45
		|| AH == 0x49) {
		/*
		 * IBM/MS lock/unlock drive.
		 * IBM/MS extended media change.
		 */
		/* All these functions return SUCCESS. */
		var_put(hd_res, 0x00);
		AH = 0x00;
		F &= ~(1 << 0); /* Clear carry. */

	} else if (AH == 0x46) {
		/*
		 * IBM/MS eject media.
		 */
		/* This function returns FAILURE. */
		var_put(hd_res, 0xb2);
		AH = 0xb2;
		F |= 1 << 0; /* Set carry. */

	} else if (AH == 0x48) {
		/*
		 * BIOS Enhanced Disk Drive call:
		 * "Get Device Parameters".
		 */
		struct info {
			unsigned short size;
			unsigned short flags;
			unsigned long cyls;
			unsigned long heads;
			unsigned long secs;
			unsigned long total_low;	/* Should be long long */
			unsigned long total_high;
			unsigned short bytes_per_sec;
			/* ... */
		};
		unsigned short size;

		size = get_word(DS, SI + offsetof(struct info, size));

		if (size < 0x1a) {
			/* Buffer is too small. */
			var_put(hd_res, 0x01);
			AH = 0x01;
			F |= 1 << 0; /* Set carry. */
			return;
		}

		if (0x1a <= size) {
			/* EDD 1.x */
			unsigned long cyls;
			unsigned long heads;
			unsigned long secs;
			unsigned long /* long */ total;	/* FIXME MARCEL */

			cyls = ebda_get(devices[device].pchs_cylinders);
			heads = ebda_get(devices[device].pchs_heads);
			secs = ebda_get(devices[device].pchs_spt);
			total = cyls * heads * secs;

			put_word(DS, SI + offsetof(struct info, size), 0x1a);
			put_word(DS, SI + offsetof(struct info, flags), 0x02);
			put_long(DS, SI + offsetof(struct info, cyls), cyls);
			put_long(DS, SI + offsetof(struct info, heads), heads);
			put_long(DS, SI + offsetof(struct info, secs), secs);
			put_long(DS, SI + offsetof(struct info, total_low), total);
			put_long(DS, SI + offsetof(struct info, total_high), 0L); /* FIXME MARCEL */
			put_word(DS, SI + offsetof(struct info, bytes_per_sec), 512);
		}
		if (0x1e <= size) {
			/* EDD 2.x */
			/* put_word(DS, SI + offsetof(struct info, size), 0x1e); */
			put_word(DS, SI + offsetof(struct info, size), 0x1a);

			/* Not supported yet. FIXME VOSSI */
		}
		if (0x42 <= size) {
			/* EDD 3.x */
			/* Not supported yet. FIXME VOSSI */
		}

		var_put(hd_res, 0x00);
		AH = 0x00;
		F &= ~(1 << 0); /* Clear carry. */

	} else if (AH == 0x4e) {
		/*
		 * Set hardware configuration.
		 */
		assert(0);	/* FIXME VOSSI */

	} else if (AH == 0x09
		|| AH == 0x0c
		|| AH == 0x0d
		|| AH == 0x11
		|| AH == 0x14) {
		/*
		 * Initialize drive parameters.
		 * Seek to specified cylinder.
		 * Alternate disk reset.
		 * Recalibrate.
		 * Controller internal diagnostic.
		 */
		/* All these functions return SUCCESS. */
		var_put(hd_res, 0x00);
		AH = 0x00;
		F &= ~(1 << 0); /* Clear carry. */

	} else if (AH == 0x0a
		|| AH == 0x0b
		|| AH == 0x18
		|| AH == 0x50) {
		/*
		 * Read disk sectors with ECC.
		 * Write disk sectors with ECC.
		 * Set media type for format.
		 * IBM/MS send packet command.
		 */
		/* All these functions return UNSUPPORTED. */
		var_put(hd_res, 0x01);
		AH = 0x01;
		F |= 1 << 0; /* Set carry. */

	} else {
		dprintf("int13_harddisk: AH=0x%02x\n", AH);
		/* Return UNSUPPORTED. */
		var_put(hd_res, 0x01);
		AH = 0x01;
		F |= 1 << 0; /* Set carry. */
	}
}

static void
int13_cdrom(struct regs *regs)
{
	unsigned char device;

	if (DL < 0xe0 || 0xe0 + MAX_BIOS_HDS <= DL) {
		var_put(hd_res, 0x01);
		int13_fail(regs, 0x01);
		return;
	}
	device = ebda_get(cdidmap[DL - 0xe0]);
	if (device == MAX_BIOS_HDS) {
		var_put(hd_res, 0x01);
		int13_fail(regs, 0x01);
		return;
	}

	if (AH == 0x00
	 || AH == 0x09
	 || AH == 0x0c
	 || AH == 0x0d
	 || AH == 0x10
	 || AH == 0x11
	 || AH == 0x14
	 || AH == 0x16) {
		/*
		 * Disk controller reset.
		 * Initialize drive parameters.
		 * Seek to specified cylinder.
		 * Alternate disk reset.
		 * Check drive ready.
		 * Recalibrate.
		 * Controller internal diagnostic.
		 * Detect disk change.
		 */
		/* All these functions return SUCCESS. */
		var_put(hd_res, 0x00);
		int13_success(regs, 0x00);

	} else if (AH == 0x03
		|| AH == 0x05
		|| AH == 0x43) {
		/*
		 * Write disk sectors.
		 * Format disk track.
		 * Extended write.
		 */
		/* All these functions return DISK WRITE-PROTECTED. */
		var_put(hd_res, 0x03);
		int13_success(regs, 0x03);

	} else if (AH == 0x01) {
		/*
		 * Read disk status.
		 */
		unsigned char status;

		status = var_get(hd_res);
		var_put(hd_res, 0x00);
		if (status) {
			int13_fail(regs, status);
		} else {
			int13_success(regs, status);
		}

	} else if (AH == 0x015) {
		/*
		 * Read disk drive size.
		 */
		var_put(hd_res, 0x02);
		int13_fail(regs, 0x02);

	} else if (AH == 0x41) {
		/*
		 * BIOS Enhanced Disk Drive call:
		 * "Check Extensions Present".
		 */
		BX = 0xaa55;		/* Install check. */
		CX = 0x0007;		/* Ext disk access, removable and EDD.*/
		var_put(hd_res, 0x00);
		int13_success(regs, 0x01);	/* Version 1.0 */

	} else if (AH == 0x42
		|| AH == 0x44
		|| AH == 0x47) {
		/*
		 * Extended read.
		 * Extended verify sectors.
		 * Extended seek.
		 */
		unsigned short count;
		unsigned short segment;
		unsigned short offset;
#if 0	/* FIXME VOSSI */
		unsigned long long lba;
#else
		unsigned long lba;
#endif
		unsigned char status;

		count = get_word(DS, SI + offsetof(struct Int13Ext, count));
		segment = get_word(DS, SI + offsetof(struct Int13Ext, segment));
		offset = get_word(DS, SI + offsetof(struct Int13Ext, offset));

#if 0	/* FIXME VOSSI */
		lba = get_longlong(DS, SI + offsetof(struct Int13Ext, lba));
#else
		lba = get_long(DS, SI + offsetof(struct Int13Ext, lba));
#endif

		if (AH == 0x44
		 || AH == 0x47) {
			/* Verify or seek. */
			var_put(hd_res, 0x00);
			int13_success(regs, 0x00);
			return;
		}

		status = read_sector_lba_atapi(device,
				count, lba, segment, offset, 2048);
		count = (status == 0) ? count : 0;	/* FIXME VOSSI */
		put_word(DS, SI + offsetof(struct Int13Ext, count), count);
		if (status != 0) {
			var_put(hd_res, 0x0c);
			int13_fail(regs, 0x0c);
			return;
		}

		var_put(hd_res, 0x00);
		int13_success(regs, 0x00);

	} else if (AH == 0x45) {
		/*
		 * Lock/unlock drive.
		 */
		assert(0);	/* FIXME VOSSI */

	} else if (AH == 0x46) {
		/*
		 * Eject media.
		 */
		assert(0);	/* FIXME VOSSI */

	} else if (AH == 0x48) {
		/*
		 * BIOS Enhanced Disk Drive call:
		 * "Get Device Parameters".
		 */
		unsigned short size;

		size = get_word(DS, SI + offsetof(struct Int13DPT, size));

		if (size < 0x1a) {
			/* Buffer is too small. */
			var_put(hd_res, 0x01);
			int13_fail(regs, 0x01);
			return;
		}
		if (0x1a <= size) {
			unsigned short blksize;

			blksize = 2048;	/* FIXME VOSSI */

			put_word(DS, SI + offsetof(struct Int13DPT, size),
					0x1a);

			/* removable, media change, lockable, max values */
			put_word(DS, SI + offsetof(struct Int13DPT, flags),
					0x74);

			put_long(DS, SI + offsetof(struct Int13DPT, cyls),
					0xffffffff);
			put_long(DS, SI + offsetof(struct Int13DPT, heads),
					0xffffffff);
			put_long(DS, SI + offsetof(struct Int13DPT, secs),
					0xffffffff);
			put_long(DS, SI + offsetof(struct Int13DPT, total_low),
					0xffffffff);
			put_long(DS, SI + offsetof(struct Int13DPT, total_high),
					0xffffffff);
			put_word(DS, SI + offsetof(struct Int13DPT, bytes_per_sec),
					blksize);
		}
		if (0x1e <= size) {
			/* EDD 2.x */
			put_word(DS, SI + offsetof(struct Int13DPT, size), 0x1e);

			/* FIXME VOSSI */
		}
		if (0x42 <= size) {
			/* EDD 3.x */
			/* Not supported yet. FIXME VOSSI */
		}

		var_put(hd_res, 0x00);
		int13_success(regs, 0x00);

	} else if (AH == 0x49) {
		/*
		 * Extended media change.
		 */
		/* Always send changed?? */
		var_put(hd_res, 0x06);
		int13_fail(regs, 0x06);

	} else if (AH == 0x4e) {
		/*
		 * Set hardware configuration.
		 */
		/* DMA, prefetch, PIO maximum not supported. */
		if (AL == 0x01
		 || AL == 0x03
		 || AL == 0x04
		 || AL == 0x06) {
			var_put(hd_res, 0x00);
			int13_success(regs, 0x00);
		} else {
			var_put(hd_res, 0x01);
			int13_success(regs, 0x01);
		}

	} else if (AH == 0x02
		|| AH == 0x04
		|| AH == 0x08
		|| AH == 0x0a
		|| AH == 0x0b
		|| AH == 0x18
		|| AH == 0x50) {
		/*
		 * Read sectors.
		 * Verify sectors.
		 * Read disk drive parameters.
		 * Read disk sectors with ECC.
		 * Write disk sectors with ECC.
		 * Set media type for format.
		 * ? - Send packet command.
		 */
		/* All these functions return UNIMPLEMENTED. */
		var_put(hd_res, 0x01);
		int13_fail(regs, 0x01);

	} else {
		dprintf("int13_cdrom: AH=0x%02x\n", AH);
		/* Return UNSUPPORTED. */
		var_put(hd_res, 0x01);
		int13_fail(regs, 0x01);
	}
}

static void
int13_cdemu(struct regs *regs)
{
	unsigned char device;

	/* Recompute the device number. */
	device  = ebda_get(cdemu.controller_index) * 2;
	device += ebda_get(cdemu.device_spec);

	var_put(hd_res, 0x00);

	/*
	 * Basic checks: emulation should be active, DL should equal the
	 * emulated drive.
	 */
	if (ebda_get(cdemu.active) == 0
	 || ebda_get(cdemu.emulated_drive) != DL) {
		assert(0);
	}

	if (AH == 0x00		/* Disk controller reset. */
	 || AH == 0x09		/* Initialize drive parameters. */
	 || AH == 0x0c		/* Seek to specified cylinder. */
	 || AH == 0x0d		/* Alternate disk reset. */
	 || AH == 0x10		/* Check drive ready. */
	 || AH == 0x11		/* Recalibrate. */
	 || AH == 0x14		/* Controller internal diagnostic. */
	 || AH == 0x16) {	/* Detect disk change. */
		/* All these functions return SUCCESS. */
		var_put(hd_res, 0x00);
		int13_success(regs, 0x00);

	} else if (AH == 0x03	/* Write disk sectors. */
		|| AH == 0x05) {/* Format disk track. */
		/* All these functions return DISK WRITE-PROTECTED. */
		var_put(hd_res, 0x03);
		int13_fail(regs, 0x03);
		
	} else if (AH == 0x01) {
		/* Read disk status. */
		unsigned char status;

		status = var_get(hd_res);

		var_put(hd_res, 0x00);
		if (status) {
			int13_fail(regs, status);
		} else {
			int13_success(regs, status);
		}

	} else if (AH == 0x02	/* Read disk sectors. */
		|| AH == 0x04) {/* Verify disk sectors. */
		unsigned short vspt;
		unsigned short vcylinders;
		unsigned short vheads;
		unsigned short sector;
		unsigned short cylinder;
		unsigned short head;
		unsigned short nbsectors;
		unsigned short segment;
		unsigned short offset;
		unsigned long ilba;
		unsigned long vlba;
		unsigned long slba;
		unsigned long before;
		unsigned long elba;
		unsigned char status;

		vspt = ebda_get(cdemu.vdevice.spt);
		vcylinders = ebda_get(cdemu.vdevice.cylinders);
		vheads = ebda_get(cdemu.vdevice.heads);

		ilba = ebda_get(cdemu.ilba);

		sector = CL & 0x3f;
		cylinder = (((unsigned short) CL & 0xc0) << 2) | CH;
		head = DH;
		nbsectors = AL;

		/* No sector to read? */
		if (nbsectors == 0) {
			var_put(hd_res, 0x00);
			int13_success(regs, 0x00);
			return;
		}

		/* Sanity checks. SCO OpenServer needs this! */
		if (vspt < sector
		 || vcylinders <= cylinder
		 || vheads <= head) {
			var_put(hd_res, 0x01);
			int13_fail(regs, 0x01);
			return;
		}

		if (AH == 0x04) {
			/* Verify disk sectors. */
			var_put(hd_res, 0x00);
			int13_success(regs, 0x00);
			return;
		}

		segment = ES + (BX / 16);
		offset = BX % 16;

		/* Calculate the virtual lba inside the image. */
		vlba = ((((unsigned long) cylinder * (unsigned long) vheads)
			+ (unsigned long) head) * (unsigned long) vspt)
			+ (unsigned long) sector - 1;

		/* In advance so we don't loose the count. */
		AL = nbsectors;

		/* Start lba on CD. */
		slba = (unsigned long) vlba / 4;
		before = (unsigned long) vlba % 4;

		/* End lba on CD. */
		elba = (unsigned long) (vlba + nbsectors - 1) / 4;

		/* Read command. */
		status = read_sector_lba_atapi(device,
				nbsectors, ilba * 4 + vlba,
				segment, offset, 512);
		if (status != 0) {
			AL = 0;
			var_put(hd_res, 0x02);
			int13_fail(regs, 0x02);
			return;
		}

		var_put(hd_res, 0x00);
		int13_success(regs, 0x00);

	} else if (AH == 0x08) {
		/* Read disk drive parameters. */
		unsigned short vspt;
		unsigned short vcylinders;
		unsigned short vheads;

		vspt = ebda_get(cdemu.vdevice.spt);
		vcylinders = ebda_get(cdemu.vdevice.cylinders) - 1;
		vheads = ebda_get(cdemu.vdevice.heads) - 1;

		AL = 0x00;
		BL = 0x00;
		CH = vcylinders & 0xff;
		CL = ((vcylinders >> 2) & 0xc0) | (vspt & 0x3f);
		DH = vheads;
		DL = 0x02;	/* FIXME ElTorito Various. should send the real count of drives 1 or 2 */
				/* FIXME ElTorito Harddisk. should send the HD count */
		if (ebda_get(cdemu.media) == 0x01) {
			BL = 0x02;
		} else if (ebda_get(cdemu.media) == 0x02) {
			BL = 0x04;
		} else if (ebda_get(cdemu.media) == 0x03) {
			BL = 0x06;
		}

		/* Get diskette parameter table stored in INT vector 0x1e. */
		DI = get_word(0x0000, 0x1e * 4 + 0);
		ES = get_word(0x0000, 0x1e * 4 + 2);

		var_put(hd_res, 0x00);
		int13_success(regs, 0x00);

	} else if (AH == 0x15) {
		/* Identify drive. */
		/* What to do...? - FIXME VOSSI */
		var_put(hd_res, 0x00);
		int13_success(regs, 0x03);

	} else if (AH == 0x0a	/* Read disk sectors with ECC */
		|| AH == 0x0b	/* Write disk sectors with ECC */
		|| AH == 0x18	/* Set media type for format */
		|| AH == 0x41	/* IBM/MS installation check */
		|| AH == 0x42	/* IBM/MS extended read */
		|| AH == 0x43	/* IBM/MS extended write */
		|| AH == 0x44	/* IBM/MS verify sectors */
		|| AH == 0x45	/* IBM/MS lock/unlock drive */
		|| AH == 0x46	/* IBM/MS eject media */
		|| AH == 0x47	/* IBM/MS extended seek */
		|| AH == 0x48	/* IBM/MS get drive parameters */
		|| AH == 0x49	/* IBM/MS extended media change */
		|| AH == 0x4e	/* ? - set hardware configuration */
		|| AH == 0x50	/* ? - send packet command */
		|| 1) {		/* default */
		dprintf("int13_cdemu: AH=0x%02x\n", AH);
		/* All these functions return UNIMPLEMENTED. */
		var_put(hd_res, 0x01);
		int13_fail(regs, 0x01);
	}
}

C_ENTRY void
bios_13_xxxx(struct regs *regs)
{
	if (0x4a <= AH && AH <= 0x4d) {
		/*
		 * El-torito functions.
		 */
		int13_eltorito(regs);

	} else if (ebda_get(cdemu.active)) {
		if (DL == ebda_get(cdemu.emulated_drive)) {
			/*
			 * Emulated drive.
			 */
			int13_cdemu(regs);

		} else if ((DL & 0xe0) == ebda_get(cdemu.emulated_drive)) {
			/*
			 * Drive in same class as emulated drive.
			 * -> change drive number
			 */
			DL--;

			/* int 13h,08h is special because it returns
			 * the number of drives in DL, so we mustn't
			 * change it before returning from this handler */
			if(AH == 0x08) goto int13_legacy;

			if (DL < 0x80) {
				int13_floppy(regs);
			} else if (DL < 0xe0) {
				int13_harddisk(regs);
			} else {
				int13_cdrom(regs);
			}

			/* change back drive number as some software
			 * seems to rely on it */
			DL++;
		} else {
			/*
			 * Unaffected drive.
			 */
			goto int13_legacy;
		}
	} else {
int13_legacy:
		/*
		 * Standard int13 functions.
		 */
		if (DL < 0x80) {
			int13_floppy(regs);
		} else if (DL < 0xe0) {
			int13_harddisk(regs);
		} else {
			int13_cdrom(regs);
		}
	}
}

C_ENTRY void
bios_0e(struct regs *regs)
{
	var_put(f_recal, var_get(f_recal) | (1 << 7));

	eoi();

#if 0
	asm volatile (
		"pushw %ax\n"
		"movb $0x91, %ah\n" /* Signal end-of-operation. */
		"movb $0x01, %al\n" /* Device type is floppy. */
		"int $0x15\n"
		"popw %ax\n"
	);
#endif
}

#endif /* RUNTIME_RM */
/* ==================== REAL-MODE INIT ==================== */
#ifdef INIT_RM

CODE16;

#include "assert.h"
#include "stdio.h"
#include "string.h"
#include "in.h"
#include "io.h"
#include "cmos.h"
#include "var.h"
#include "el_torito.h"
#include "ptrace.h"
#include "disk.h"
#include "const.h"
#include "video.h"

/* cut && paste of umide.h */
/* structure returned by HDIO_GET_IDENTITY, as per ANSI ATA2 rev.2f spec */
struct hd_driveid {
	unsigned short config;	/* lots of obsolete bit flags */
	unsigned short cyls;	/* "physical" cyls */
	unsigned short reserved2;	/* reserved (word 2) */
	unsigned short heads;	/* "physical" heads */
	unsigned short track_bytes;	/* unformatted bytes per track */
	unsigned short sector_bytes;	/* unformatted bytes per sector */
	unsigned short sectors;	/* "physical" sectors per track */
	unsigned short vendor0;	/* vendor unique */
	unsigned short vendor1;	/* vendor unique */
	unsigned short vendor2;	/* vendor unique */
	unsigned char serial_no[20];	/* 0 = not_specified */
	unsigned short buf_type;
	unsigned short buf_size;    /* 512 byte increments; 0 = not_specified */
	unsigned short ecc_bytes;   /* for r/w long cmds; 0 = not_specified */
	unsigned char fw_rev[8];	/* 0 = not_specified */
	unsigned char model[40];	/* 0 = not_specified */
	unsigned char max_multsect;	/* 0=not_implemented */
	unsigned char vendor3;		/* vendor unique */
	unsigned short dword_io;	/* 0=not_implemented; 1=implemented */
	unsigned char vendor4;		/* vendor unique */
	unsigned char capability;    /* bits 0:DMA 1:LBA 2:IORDYsw 3:IORDYsup */
	unsigned short reserved50;	/* reserved (word 50) */
	unsigned char vendor5;		/* vendor unique */
	unsigned char tPIO;		/* 0=slow, 1=medium, 2=fast */
	unsigned char vendor6;		/* vendor unique */
	unsigned char tDMA;		/* 0=slow, 1=medium, 2=fast */
	unsigned short field_valid;	/* bits 0:cur_ok 1:eide_ok */
	unsigned short cur_cyls;	/* logical cylinders */
	unsigned short cur_heads;	/* logical heads */
	unsigned short cur_sectors;	/* logical sectors per track */
	unsigned short cur_capacity0;	/* logical total sectors on drive */
	unsigned short cur_capacity1;	/*  (2 words, misaligned int)     */
	unsigned char multsect;		/* current multiple sector count */
	unsigned char multsect_valid;	/* when (bit0==1) multsect is ok */
	uint32_t lba_capacity;		/* total number of sectors */
	unsigned short dma_1word;	/* single-word dma info */
	unsigned short dma_mword;	/* multiple-word dma info */
	unsigned short eide_pio_modes;	/* bits 0:mode3 1:mode4 */
	unsigned short eide_dma_min;	/* min mword dma cycle time (ns) */
	unsigned short eide_dma_time;/* recommended mword dma cycle time (ns) */
	unsigned short eide_pio;	/* min cycle time (ns), no IORDY  */
	unsigned short eide_pio_iordy;	/* min cycle time (ns), with IORDY */
	unsigned short words69_70[2];	/* reserved words 69-70 */
	/* HDIO_GET_IDENTITY currently returns only words 0 through 70 */
	unsigned short words71_74[4];	/* reserved words 71-74 */
	unsigned short queue_depth;	/*  */
	unsigned short words76_79[4];	/* reserved words 76-79 */
	unsigned short major_rev_num;	/*  */
	unsigned short minor_rev_num;	/*  */
	/* bits 0:Smart 1:Security 2:Removable 3:PM */
	unsigned short command_set_1;
	unsigned short command_set_2;	/* bits 14:Smart Enabled 13:0 zero */
	unsigned short cfsse;	/* command set-feature supported extensions */
	unsigned short cfs_enable_1;	/* command set-feature enabled */
	unsigned short cfs_enable_2;	/* command set-feature enabled */
	unsigned short csf_default;	/* command set-feature default */
	unsigned short dma_ultra;	/*  */
	unsigned short word89;	/* reserved (word 89) */
	unsigned short word90;	/* reserved (word 90) */
	unsigned short CurAPMvalues;	/* current APM values */
	unsigned short word92;	/* reserved (word 92) */
	unsigned short hw_config;	/* hardware config */
	unsigned short words94_125[32];	/* reserved words 94-125 */
	unsigned short last_lun;	/* reserved (word 126) */
	unsigned short word127;	/* reserved (word 127) */
	unsigned short dlf;	/* device lock function
				 * 15:9        reserved
				 * 8   security level 1:max 0:high
				 * 7:6 reserved
				 * 5   enhanced erase
				 * 4   expire
				 * 3   frozen
				 * 2   locked
				 * 1   en/disabled
				 * 0   capability
				 */
	unsigned short csfo;	/* current set features options
				 * 15:4        reserved
				 * 3   auto reassign
				 * 2   reverting
				 * 1   read-look-ahead
				 * 0   write cache
				 */
	unsigned short words130_155[26]; /* reserved vendor words 130-155 */
	unsigned short word156;
	unsigned short words157_159[3];	 /* reserved vendor words 157-159 */
	unsigned short words160_255[95]; /* reserved words 160-255 */
};


static unsigned char
get_identify_ide(
	unsigned short cmd, /* ATA COMMAND */
	unsigned char contr,
	unsigned char drive,
	unsigned short *buf
)
{
	uint16_t port;
	uint8_t status;
	unsigned int i;

	assert(cmd == WIN_IDENTIFY || cmd == WIN_PIDENTIFY);
	assert(/* 0 <= contr && */ contr < 6);
	assert(/* 0 <= drive && */ drive < 2);

	port = const_get(ide_port_table[contr]);

	/* Wait while busy. */
	for (i = 0; ; i++) {
		if (i == 0xffff) {
			return 1;
		}
		status = inb(port + 0x206);
		if (! (status & IDE_BUSY_STAT)) {
			break;
		}
	}

	/* Send command. */
	outb(drive << 4, port + 6);	/* unit */
	outb(cmd, port + 7);		/* command */

	/* Wait while controller busy. */
	do {
		status = inb(port + 0x206);
	} while (status & IDE_BUSY_STAT);

	if (! (status & IDE_DRQ_STAT)
	 || (status & IDE_ERR_STAT)) {
		/* Strange response -> command not recognized. */
		return 1;
	}

	/* Read info. */
	for (i = 0; i < 512 / sizeof(unsigned short); i++) {
		buf[i] = inw(port);
	}

	return 0;
}

static uint8_t
recalibrate(unsigned char contr, unsigned char drive)
{
	uint16_t port;
	uint8_t status;

	assert(/* 0 <= contr && */ contr < 6);
	assert(/* 0 <= drive && */ drive < 2);

	port = const_get(ide_port_table[contr]);

	/* Send command. */
	outb(0, port + 1);		/* Features */
	outb(1, port + 2);		/* #Sectors */
	outb(0, port + 3);		/* Sector Number */
	outb(0, port + 4);		/* Cylinder Low */
	outb(0, port + 5);		/* Cylinder High */
	outb(drive << 4, port + 6);	/* Unit */
	outb(WIN_RESTORE, port + 7);	/* Command */

	/* Wait while controller busy. */
	do {
		status = inb(port + 0x206);
		if (status == 0xff) {
			/* No drive present. */
			return 1;
		}
	} while (status & IDE_BUSY_STAT);

	return 0;
}

const char *
get_identify_floppy(unsigned short drive)
{
	static const char * const floppy_type[] = {
		"None",
		"360K, 5.25 in.",
		"1.2M, 5.25 in.",
		"720K, 3.5 in.",
		"1.44M, 3.5 in.",
		"2.88M, 3.5 in.",
		"160K, 5.25 in.",
		"180K, 5.25 in.",
		"320K, 5.25 in.",
	};
	unsigned char drive_type;

	drive_type = cmos_get(fd_config);

	if (drive == 0) {
		drive_type >>= 4;
	} else {
		drive_type &= 0xf;
	}
	
	return floppy_type[drive_type];
}

void
floppy_init(void)
{
	uint8_t drive_type;
	uint16_t equipment;

	/* Reset floppy controller. */
	outb(0x08, 0x3f2);
	outb(0x0c, 0x3f2);

	/* Call sense interrupt status. */
	outb(0x08, 0x3f5);

	/* Get result bytes. */
	(void) inb(0x3f5);
	(void) inb(0x3f5);

	/* Call configure to enable implied seeks. */
	outb(0x13, 0x3f5); /* Configure command */
	outb(0x00, 0x3f5); /* Dummy */
	outb(0 << 7 /* ? */
	   | 1 << 6 /* Enable implied seek */
	   | 1 << 5 /* Enable FIFO */
	   | 1 << 4 /* Disable polling */
	   | 0 << 0, /* FIFO threshold = 1 */
		0x3f5);
	outb(0x00, 0x3f5); /* Precompensation beginning at track 0 */

	/* Get result bytes. */
	/* None. */

	/* int $0x1e vector is diskette_param_table. */
	put_word(0x0000, 0x1e * 4 + 0, PTR_OFF(&diskette_param_table));
	put_word(0x0000, 0x1e * 4 + 2, PTR_SEG(&diskette_param_table));

	/* adjust sys_conf settings for floppy drives */
	/* bit   0: drives are present */
	/* bit 7-6: number of drives - 1 */
	drive_type = cmos_get(fd_config);
	equipment = var_get(sys_conf) & ~0xc1;
	if ((drive_type >> 4) & 0xf) {
		equipment |= (1 << 0);
	}
	if ((drive_type >> 0) & 0xf) {
		equipment |= (1 << 6);
	}
	var_put(sys_conf, equipment);
}

static void
detecting(unsigned short contr, unsigned short unit)
{
	bprintf("  Detecting IDE ");
	if (contr == 0) {
		if (unit == 0) {
			bprintf("Primary Master  ");
		} else {
			bprintf("Primary Slave   ");
		}
	} else if (contr == 1) {
		if (unit == 0) {
			bprintf("Secondary Master");
		} else {
			bprintf("Secondary Slave ");
		}
	} else {
		bprintf("%d/%d             ", contr, unit);
	}
	bprintf("... ");
}


void
disk_init(void)
{
	unsigned char hd;
	unsigned char cd;
	unsigned short contr;
	unsigned short unit;
	unsigned short i;

	hd = 0x80;
	cd = 0xe0;
	ebda_put(harddrives, 0);
	ebda_put(cdcount, 0);
	var_put(hd_num, 0);
	cmos_put(hd_config, 0x00);
	cmos_put(hd_extconfig0, 0);
	cmos_put(hd_extconfig1, 0);
	cmos_put(hd_extconfig2, 0);
	cmos_put(hd_extconfig3, 0);

	for (contr = 0; contr < 6; contr++) {
		for (unit = 0; unit < 2; unit++) {
			const unsigned int nr = contr * 2 + unit;
			unsigned short buffer[512 / sizeof(unsigned short)];
			struct hd_driveid *id = (struct hd_driveid *) buffer;
			int type; /* none == -1; disk == 0; cdrom == 1*/

			if (contr < 2) {
				detecting(contr, unit);
			}

			/* First try disk. */
			if (! get_identify_ide(WIN_IDENTIFY,
					contr, unit, buffer)) {
				/* Harddisk */
				type = 0;
				ebda_put(devices[nr].drive, hd);
				ebda_put(devices[nr].pchs_cylinders, id->cyls);
				ebda_put(devices[nr].pchs_heads, id->heads);
				ebda_put(devices[nr].pchs_spt, id->sectors);
				ebda_put(devices[nr].lchs_cylinders, id->cur_cyls);
				ebda_put(devices[nr].lchs_heads, id->cur_heads);
				ebda_put(devices[nr].lchs_spt, id->cur_sectors);

				ebda_put(devices[nr].sectors, id->lba_capacity);
				ebda_put(devices[nr].type, 0);

				if (hd - 0x80 < 2) {
					ebda_put(hdpt[hd - 0x80].cyls, id->cyls);
					ebda_put(hdpt[hd - 0x80].heads, id->heads);
					ebda_put(hdpt[hd - 0x80].reserved0, 0);
					ebda_put(hdpt[hd - 0x80].precomp, -1);
					ebda_put(hdpt[hd - 0x80].reserved1, 0);
					ebda_put(hdpt[hd - 0x80].control, (8 < id->heads) << 3);
					ebda_put(hdpt[hd - 0x80].reserved2, 0);
					ebda_put(hdpt[hd - 0x80].reserved3, 0);
					ebda_put(hdpt[hd - 0x80].reserved4, 0);
					ebda_put(hdpt[hd - 0x80].landing, id->cyls - 1);
					ebda_put(hdpt[hd - 0x80].secs, id->sectors);
					ebda_put(hdpt[hd - 0x80].reserved5, 0);
				}
				if (hd == 0x80) {
					cmos_put(hd_config,
						0xf0 | cmos_get(hd_config));
					cmos_put(hd_extconfig0, 0x2f);
				} else if (hd == 0x81) {
					cmos_put(hd_config,
						0x0f | cmos_get(hd_config));
					cmos_put(hd_extconfig1, 0x2f);
				} else if (hd == 0x82) {
					cmos_put(hd_extconfig2, 0x2f);
				} else if (hd == 0x83) {
					cmos_put(hd_extconfig2, 0x2f);
				}
				if (nr == 0) {
					/* Primary Master */
					cmos_put(hd_config_user0.cyls,
							id->cyls);
					cmos_put(hd_config_user0.heads,
							id->heads);
					cmos_put(hd_config_user0.precomp,
							-1);
					cmos_put(hd_config_user0.landing,
							id->cyls - 1);
					cmos_put(hd_config_user0.secs,
							id->sectors);
				} else if (nr == 1) {
					/* Primary Slave */
					cmos_put(hd_config_user1.cyls,
							id->cyls);
					cmos_put(hd_config_user1.heads,
							id->heads);
					cmos_put(hd_config_user1.precomp,
							-1);
					cmos_put(hd_config_user1.landing,
							id->cyls - 1);
					cmos_put(hd_config_user1.secs,
							id->sectors);
				} else if (nr == 2) {
					/* Secondary Master */
					cmos_put(hd_config_user2.cyls,
							id->cyls);
					cmos_put(hd_config_user2.heads,
							id->heads);
					cmos_put(hd_config_user2.precomp,
							-1);
					cmos_put(hd_config_user2.landing,
							id->cyls - 1);
					cmos_put(hd_config_user2.secs,
							id->sectors);
				} else if (nr == 3) {
					/* Secondary Slave */
					cmos_put(hd_config_user3.cyls,
							id->cyls);
					cmos_put(hd_config_user3.heads,
							id->heads);
					cmos_put(hd_config_user3.precomp,
							-1);
					cmos_put(hd_config_user3.landing,
							id->cyls - 1);
					cmos_put(hd_config_user3.secs,
							id->sectors);
				}

				ebda_put(hdidmap[hd - 0x80], nr);

				hd++;

				ebda_put(harddrives, ebda_get(harddrives) + 1);
				var_put(hd_num, var_get(hd_num) + 1);

			} else if (! get_identify_ide(WIN_PIDENTIFY,
					contr, unit, buffer)) {
				/* CD-ROM */
				type = 1;
				ebda_put(devices[nr].drive, cd);
				ebda_put(devices[nr].type, 1);

				ebda_put(cdidmap[cd - 0xe0], nr);

				cd++;

				ebda_put(cdcount, ebda_get(cdcount) + 1);

			} else {
				/* nothing */
				ebda_put(devices[nr].drive, 0xff);
				ebda_put(devices[nr].type, 0xfe);
				type = -1;
			}

			if (2 <= contr && type != -1) {
				detecting(contr, unit);
			}

			if (contr < 2 && type == -1) {
				bprintf("None\n");
			} else if (type != -1) {
				/* display device name */
				for (i = 0; i < 40; i++) {
					if (i % 2) {
						putchar(id->model[i-1]);
					} else {
						putchar(id->model[i+1]);
					}
				}
				putchar('\n');
			}
		}
	}

	for ( ; hd < 0x80 + MAX_BIOS_HDS; hd++) {
		ebda_put(hdidmap[hd - 0x80], MAX_BIOS_HDS);
	}
	for ( ; cd < 0xe0 + MAX_BIOS_HDS; cd++) {
		ebda_put(cdidmap[cd - 0xe0], MAX_BIOS_HDS);
	}

	/* int $0x41 vector is harddisk0_param_table. */
	put_word(0x0000, 0x41 * 4 + 0, offsetof(struct ebda, hdpt[0]));
	put_word(0x0000, 0x41 * 4 + 2, EBDA_SEG);

	/* int $0x46 vector is harddisk1_param_table. */
	put_word(0x0000, 0x46 * 4 + 0, offsetof(struct ebda, hdpt[1]));
	put_word(0x0000, 0x46 * 4 + 2, EBDA_SEG);

	/*
	 * WARNING: Minix3 needs HDs accessed last.
	 * So just recalibrate HDs.
	 */
	for (hd = 0x80; hd < 0x80 + MAX_BIOS_HDS; hd++) {
		unsigned int nr;

		nr = ebda_get(hdidmap[hd - 0x80]);

		if (nr != MAX_BIOS_HDS) {
			recalibrate(nr / 2, nr % 2);
		}
	}
}

#endif /* INIT_RM */