From 02af31d730828d0bf1d8ce2da291d1b844fa7345 Mon Sep 17 00:00:00 2001
From: sorgelig <pour.garbage@gmail.com>
Date: Mon, 28 Jun 2021 01:28:20 +0800
Subject: [PATCH] Update IDE code. Use standard IDE for Minimig.

---
 menu.cpp                           |  61 +++-
 support/minimig/minimig_config.cpp |  25 +-
 support/minimig/minimig_config.h   |   2 +-
 support/minimig/minimig_hdd.cpp    |  31 +-
 support/x86/x86.cpp                |  71 +---
 support/x86/x86_cdrom.cpp          |  27 +-
 support/x86/x86_ide.cpp            | 569 ++++++++++++++++++++---------
 support/x86/x86_ide.h              |  34 +-
 user_io.cpp                        |  13 +-
 9 files changed, 533 insertions(+), 300 deletions(-)

diff --git a/menu.cpp b/menu.cpp
index 08a7a21..b97aadd 100644
--- a/menu.cpp
+++ b/menu.cpp
@@ -59,6 +59,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #include "recent.h"
 #include "support.h"
 #include "bootcore.h"
+#include "support/x86/x86_ide.h"
 
 /*menu states*/
 enum MENU
@@ -5118,10 +5119,10 @@ void HandleUI(void)
 				config_cpu_msg[minimig_config.cpu & 0x03] + 2,
 				config_chipset_msg[(minimig_config.chipset >> 2) & 7],
 				minimig_config.chipset & CONFIG_NTSC ? "N" : "P",
-				(minimig_config.enable_ide && (minimig_config.hardfile[0].enabled ||
-					minimig_config.hardfile[1].enabled ||
-					minimig_config.hardfile[2].enabled ||
-					minimig_config.hardfile[3].enabled)) ? "/HD" : "",
+				(minimig_config.enable_ide && (minimig_config.hardfile[0].cfg ||
+					minimig_config.hardfile[1].cfg ||
+					minimig_config.hardfile[2].cfg ||
+					minimig_config.hardfile[3].cfg)) ? "/HD" : "",
 				config_memory_chip_msg[minimig_config.memory & 0x03],
 				fastcfg ? "+" : "",
 				fastcfg ? config_memory_fast_msg[(minimig_config.cpu>>1) & 1][fastcfg] : "",
@@ -5355,20 +5356,24 @@ void HandleUI(void)
 			uint n = 1, t = 4;
 			for (uint i = 0; i < 4; i++)
 			{
-				strcpy(s, (i & 2) ? " Secondary " : " Primary ");
-				strcat(s, (i & 1) ? "Slave: " : "Master: ");
-				strcat(s, minimig_config.hardfile[i].enabled ? "Enabled" : "Disabled");
-				OsdWrite(m++, s, minimig_config.enable_ide ? (menusub == n++) : 0, minimig_config.enable_ide == 0);
+				strcpy(s, (i & 2) ? " Sec. " : " Pri. ");
+				strcat(s, (i & 1) ? " Slave: " : "Master: ");
+				strcat(s, (minimig_config.hardfile[i].cfg == 2) ? "Removable/CD" : minimig_config.hardfile[i].cfg ? "Fixed/HDD" : "Disabled");
+				OsdWrite(m++, s, minimig_config.enable_ide ? (menusub == n++) : 0, minimig_config.enable_ide ==0);
 				if (minimig_config.hardfile[i].filename[0])
 				{
 					strcpy(s, "                                ");
-					strncpy(&s[3], minimig_config.hardfile[i].filename, 25);
+					char *path = user_io_get_core_path();
+					int len = strlen(path);
+					char *name = minimig_config.hardfile[i].filename;
+					if (!strncasecmp(name, path, len))  name += len + 1;
+					strncpy(&s[3], name, 25);
 				}
 				else
 				{
 					strcpy(s, "   ** not selected **");
 				}
-				enable = minimig_config.enable_ide && minimig_config.hardfile[i].enabled;
+				enable = minimig_config.enable_ide && minimig_config.hardfile[i].cfg;
 				if (enable) menumask |= t;	// Make hardfile selectable
 				OsdWrite(m++, s, menusub == n++, enable == 0);
 				t <<= 2;
@@ -5388,7 +5393,7 @@ void HandleUI(void)
 	case MENU_MINIMIG_DISK2:
 		saved_menustate = MENU_MINIMIG_DISK1;
 
-		if (select || recent)
+		if (select || recent || minus || plus)
 		{
 			if (menusub == 0)
 			{
@@ -5402,24 +5407,33 @@ void HandleUI(void)
 			{
 				if(menusub&1)
 				{
-					if (select)
+					if (select || minus || plus)
 					{
-						int num = (menusub - 1) / 2;
-						minimig_config.hardfile[num].enabled = minimig_config.hardfile[num].enabled ? 0 : 1;
+						int idx = (menusub - 1) / 2;
+						if (minus)
+						{
+							if (!minimig_config.hardfile[idx].cfg) minimig_config.hardfile[idx].cfg = 2;
+							else minimig_config.hardfile[idx].cfg--;
+						}
+						else
+						{
+							minimig_config.hardfile[idx].cfg++;
+							if (minimig_config.hardfile[idx].cfg > 2) minimig_config.hardfile[idx].cfg = 0;
+						}
 						menustate = MENU_MINIMIG_DISK1;
 					}
 				}
-				else
+				else if(select || recent)
 				{
 					fs_Options = SCANO_DIR | SCANO_UMOUNT;
 					fs_MenuSelect = MENU_MINIMIG_HDFFILE_SELECTED;
 					fs_MenuCancel = MENU_MINIMIG_DISK1;
-					strcpy(fs_pFileExt, "HDFVHDIMGDSK");
+					int idx = (menusub - 2) / 2;
+					strcpy(fs_pFileExt, (minimig_config.hardfile[idx].cfg == 2) ? "ISOCUECHDIMG" : "HDFVHDIMGDSK");
 					if (select)
 					{
-						int idx = (menusub - 2) / 2;
 						if (!Selected_S[idx][0]) memcpy(Selected_S[idx], minimig_config.hardfile[idx].filename, sizeof(Selected_S[idx]));
-						SelectFile(Selected_S[idx], "HDFVHDIMGDSK", fs_Options, fs_MenuSelect, fs_MenuCancel);
+						SelectFile(Selected_S[idx], fs_pFileExt, fs_Options, fs_MenuSelect, fs_MenuCancel);
 					}
 					else if (recent_init(500)) menustate = MENU_RECENT1;
 				}
@@ -5457,7 +5471,16 @@ void HandleUI(void)
 			if (len > sizeof(minimig_config.hardfile[num].filename) - 1) len = sizeof(minimig_config.hardfile[num].filename) - 1;
 			if(len) memcpy(minimig_config.hardfile[num].filename, selPath, len);
 			minimig_config.hardfile[num].filename[len] = 0;
-			menustate = checkHDF(minimig_config.hardfile[num].filename, &rdb) ? MENU_MINIMIG_DISK1 : MENU_MINIMIG_HDFFILE_SELECTED2;
+
+			if (ide_is_placeholder(num))
+			{
+				OpenHardfile(num);
+				menustate = MENU_MINIMIG_DISK1;
+			}
+			else
+			{
+				menustate = checkHDF(minimig_config.hardfile[num].filename, &rdb) ? MENU_MINIMIG_DISK1 : MENU_MINIMIG_HDFFILE_SELECTED2;
+			}
 		}
 		break;
 
diff --git a/support/minimig/minimig_config.cpp b/support/minimig/minimig_config.cpp
index b898403..17e8ffa 100644
--- a/support/minimig/minimig_config.cpp
+++ b/support/minimig/minimig_config.cpp
@@ -17,6 +17,7 @@
 #include "minimig_hdd.h"
 #include "minimig_config.h"
 #include "minimig_share.h"
+#include "../x86/x86_ide.h"
 
 const char *config_memory_chip_msg[] = { "512K", "1M",   "1.5M", "2M" };
 const char *config_memory_slow_msg[] = { "none", "512K", "1M",   "1.5M" };
@@ -325,12 +326,20 @@ static void ApplyConfiguration(char reloadkickstart)
 	printf("\nIDE state: %s.\n", minimig_config.enable_ide ? "enabled" : "disabled");
 	if (minimig_config.enable_ide)
 	{
-		printf("Primary Master HDD is %s.\n", minimig_config.hardfile[0].enabled ? "enabled" : "disabled");
-		printf("Primary Slave HDD is %s.\n", minimig_config.hardfile[1].enabled ? "enabled" : "disabled");
-		printf("Secondary Master HDD is %s.\n", minimig_config.hardfile[2].enabled ? "enabled" : "disabled");
-		printf("Secondary Slave HDD is %s.\n", minimig_config.hardfile[3].enabled ? "enabled" : "disabled");
+		printf("Primary Master HDD is %s.\n", (minimig_config.hardfile[0].cfg == 2) ? "CD" : minimig_config.hardfile[0].cfg ? "HDD" : "disabled");
+		printf("Primary Slave HDD is %s.\n", (minimig_config.hardfile[1].cfg == 2) ? "CD" : minimig_config.hardfile[1].cfg ? "HDD" : "disabled");
+		printf("Secondary Master HDD is %s.\n", (minimig_config.hardfile[2].cfg == 2) ? "CD" : minimig_config.hardfile[2].cfg ? "HDD" : "disabled");
+		printf("Secondary Slave HDD is %s.\n", (minimig_config.hardfile[3].cfg == 2) ? "CD" : minimig_config.hardfile[3].cfg ? "HDD" : "disabled");
 	}
 
+	uint8_t hotswap[4] = {
+		minimig_config.hardfile[0].cfg == 2,
+		minimig_config.hardfile[1].cfg == 2,
+		minimig_config.hardfile[2].cfg == 2,
+		minimig_config.hardfile[3].cfg == 2
+	};
+	ide_reset(hotswap);
+
 	rstval = SPI_CPU_HLT;
 	spi_uio_cmd8(UIO_MM2_RST, rstval);
 	spi_uio_cmd8(UIO_MM2_HDD, (minimig_config.enable_ide ? 1 : 0) | (OpenHardfile(0) ? 2 : 0) | (OpenHardfile(1) ? 4 : 0) | (OpenHardfile(2) ? 8 : 0) | (OpenHardfile(3) ? 16 : 0));
@@ -461,10 +470,14 @@ int minimig_cfg_load(int num)
 		minimig_config.floppy.speed = CONFIG_FLOPPY2X;
 		minimig_config.floppy.drives = 1;
 		minimig_config.enable_ide = 0;
-		minimig_config.hardfile[0].enabled = 1;
+		minimig_config.hardfile[0].cfg = 1;
 		minimig_config.hardfile[0].filename[0] = 0;
-		minimig_config.hardfile[1].enabled = 1;
+		minimig_config.hardfile[1].cfg = 1;
 		minimig_config.hardfile[1].filename[0] = 0;
+		minimig_config.hardfile[2].cfg = 0;
+		minimig_config.hardfile[2].filename[0] = 0;
+		minimig_config.hardfile[3].cfg = 0;
+		minimig_config.hardfile[3].filename[0] = 0;
 		updatekickstart = true;
 		BootPrintEx(">>> No config found. Using defaults. <<<");
 	}
diff --git a/support/minimig/minimig_config.h b/support/minimig/minimig_config.h
index 6406472..a5cabdb 100644
--- a/support/minimig/minimig_config.h
+++ b/support/minimig/minimig_config.h
@@ -33,7 +33,7 @@ typedef struct
 
 typedef struct
 {
-	unsigned char enabled;
+	unsigned char cfg;
 	unsigned char reserved;
 	char filename[1024];
 } mm_hardfileTYPE;
diff --git a/support/minimig/minimig_hdd.cpp b/support/minimig/minimig_hdd.cpp
index 9fbe836..9604a64 100644
--- a/support/minimig/minimig_hdd.cpp
+++ b/support/minimig/minimig_hdd.cpp
@@ -32,6 +32,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #include "minimig_config.h"
 #include "../../debug.h"
 #include "../../user_io.h"
+#include "../x86/x86_ide.h"
 
 #define CMD_IDECMD                 0x04
 #define CMD_IDEDAT                 0x08
@@ -728,7 +729,7 @@ uint8_t OpenHardfile(uint8_t unit, const char* filename)
 
 	if (is_minimig())
 	{
-		if (minimig_config.enable_ide && minimig_config.hardfile[unit].enabled)
+		if (minimig_config.enable_ide && minimig_config.hardfile[unit].cfg)
 		{
 			printf("\nChecking HDD %d\n", unit);
 			if (minimig_config.hardfile[unit].filename[0])
@@ -742,7 +743,32 @@ uint8_t OpenHardfile(uint8_t unit, const char* filename)
 					printf("CHS: %u/%u/%u", hdf->cylinders, hdf->heads, hdf->sectors);
 					printf(" (%llu MB), ", ((((uint64_t)hdf->cylinders) * hdf->heads * hdf->sectors) >> 11));
 					printf("Offset: %d\n", hdf->offset);
-					return 1;
+
+					if (ide_check() & 0x8000)
+					{
+						int present = 0;
+						int cd = 0;
+
+						int len = strlen(minimig_config.hardfile[unit].filename);
+						char *ext = minimig_config.hardfile[unit].filename + len - 4;
+						int vhd = (len > 4 && (!strcasecmp(ext, ".hdf") || (!strcasecmp(ext, ".vhd"))));
+
+						if (!vhd)
+						{
+							const char *img_name = cdrom_parse(unit, minimig_config.hardfile[unit].filename);
+							if (img_name) present = ide_img_mount(&hdf->file, img_name, 0);
+							if (present) cd = 1;
+							else vhd = 1;
+						}
+
+						if (!present && vhd) present = ide_img_mount(&hdf->file, minimig_config.hardfile[unit].filename, 1);
+						ide_set(unit, (unit & 2) ? 0xF100 : 0xF000, present ? &hdf->file : 0, 3, cd, hdf->sectors, hdf->heads);
+						if (present) return 1;
+					}
+					else
+					{
+						return 1;
+					}
 				}
 			}
 			printf("HDD %d: not present\n", unit);
@@ -768,6 +794,7 @@ uint8_t OpenHardfile(uint8_t unit, const char* filename)
 	}
 
 	// close if opened earlier.
+	if (is_minimig() && (ide_check() & 0x8000)) ide_set(unit, (unit & 2) ? 0xF100 : 0xF000, 0, 3, 0);
 	FileClose(&hdf->file);
 	return 0;
 }
diff --git a/support/x86/x86.cpp b/support/x86/x86.cpp
index 818398f..c10a857 100644
--- a/support/x86/x86.cpp
+++ b/support/x86/x86.cpp
@@ -95,16 +95,6 @@ static uint32_t cpu_get_clock()
 	return clock ? clock : 90500000;
 }
 
-static uint16_t dma_sdio(int status)
-{
-	uint16_t res;
-	EnableIO();
-	res = spi_w(UIO_DMA_SDIO);
-	if(status || !res) res = (uint8_t)spi_w((uint16_t)status);
-	DisableIO();
-	return res;
-}
-
 /*
 static uint32_t dma_get(uint32_t address)
 {
@@ -223,35 +213,6 @@ static fileTYPE fdd1_image = {};
 static fileTYPE ide_image[4] = {};
 static bool boot_from_floppy = 1;
 
-static int img_mount(fileTYPE *f, const char *name, int rw)
-{
-	FileClose(f);
-	int writable = 0, ret = 0;
-
-	int len = strlen(name);
-	if (len)
-	{
-		const char *ext = name+len-4;
-		if (!strncasecmp(".chd", ext, 4))
-		{
-			ret = 1;
-		} else {
-			writable = rw && FileCanWrite(name);
-			ret = FileOpenEx(f, name, writable ? (O_RDWR | O_SYNC) : O_RDONLY);
-			if (!ret) printf("Failed to open file %s\n", name);
-		}
-	}
-
-	if (!ret)
-	{
-		f->size = 0;
-		return 0;
-	}
-
-	printf("Mount %s as %s\n", name, writable ? "read-write" : "read-only");
-	return 1;
-}
-
 static int img_read(fileTYPE *f, uint32_t lba, void *buf, uint32_t cnt)
 {
 	if (!FileSeekLBA(f, lba)) return 0;
@@ -323,7 +284,7 @@ static void fdd_set(int num, char* filename)
 	uint32_t base = newcore ? FDD0_BASE_NEW : FLOPPY0_BASE_OLD;
 	fileTYPE *fdd_image = num ? &fdd1_image : &fdd0_image;
 
-	int floppy = img_mount(fdd_image, filename, 1);
+	int floppy = ide_img_mount(fdd_image, filename, 1);
 	uint32_t size = fdd_image->size/512;
 	printf("floppy size: %d blks\n", size);
 	if (floppy && size)
@@ -418,12 +379,12 @@ static void hdd_set(int num, char* filename)
 	if (num > 1 && !vhd)
 	{
 		const char *img_name = cdrom_parse(num, filename);
-		if (img_name) present = img_mount(&ide_image[num], img_name, 0);
+		if (img_name) present = ide_img_mount(&ide_image[num], img_name, 0);
 		if (present) cd = 1;
 	}
 
-	if(!present && vhd) present = img_mount(&ide_image[num], filename, 1);
-	x86_ide_set(num, base, present ? &ide_image[num] : 0, v3 ? 3 : newcore ? 2 : 0, cd);
+	if(!present && vhd) present = ide_img_mount(&ide_image[num], filename, 1);
+	ide_set(num, base, present ? &ide_image[num] : 0, v3 ? 3 : newcore ? 2 : 0, cd);
 }
 
 static uint8_t bin2bcd(unsigned val)
@@ -433,7 +394,7 @@ static uint8_t bin2bcd(unsigned val)
 
 static void check_ver()
 {
-	uint16_t flg = dma_sdio(0);
+	uint16_t flg = ide_check();
 	newcore = ((flg & 0xC000) == 0xC000);
 	v3 = ((flg & 0xF000) == 0xE000);
 }
@@ -455,7 +416,15 @@ void x86_init()
 		IOWR(PC_BUS_BASE_OLD, 1, 0x000000F0);
 	}
 
-	x86_ide_reset(((dma_sdio(0)>>8) & 3) ^ 1);
+	uint8_t cfg = ide_check();
+	uint8_t hotswap[4] = {
+		0,
+		0,
+		((cfg >> 8) & 1) ^ 1,
+		(cfg >> 9) & 1,
+	};
+	ide_reset(hotswap);
+
 	fdd_set(0, config.img_name[0]);
 	fdd_set(1, config.img_name[1]);
 	for (int i = 0; i < 4; i++) hdd_set(i, config.img_name[i + 2]);
@@ -699,7 +668,7 @@ void img_io_old(uint8_t sd_req)
 			printf("Error: image is not ready.\n");
 		}
 
-		dma_sdio(res ? 1 : 2);
+		ide_check(res ? 1 : 2);
 	}
 	else if (sd_req == 2)
 	{
@@ -732,7 +701,7 @@ void img_io_old(uint8_t sd_req)
 			printf("Error: image is not ready.\n");
 		}
 
-		dma_sdio(res ? 1 : 2);
+		ide_check(res ? 1 : 2);
 	}
 }
 
@@ -746,16 +715,16 @@ void x86_poll()
 
 	x86_share_poll();
 
-	uint16_t sd_req = dma_sdio(0);
+	uint16_t sd_req = ide_check();
 	if (sd_req)
 	{
 		if (sd_req & 0x8000)
 		{
-			if (v3) x86_ide_io(0, sd_req & 7);
+			if (v3) ide_io(0, sd_req & 7);
 			else if (sd_req & 3) img_io(&ide_image[0], HDD0_BASE_NEW, sd_req & 1, 16);
 
 			sd_req >>= 3;
-			if (v3) x86_ide_io(1, sd_req & 7);
+			if (v3) ide_io(1, sd_req & 7);
 			else if (sd_req & 3) img_io(&ide_image[1], HDD1_BASE_NEW, sd_req & 1, 16);
 
 			sd_req >>= 3;
@@ -773,7 +742,7 @@ void x86_set_image(int num, char *filename)
 	memset(config.img_name[num], 0, sizeof(config.img_name[0]));
 	strcpy(config.img_name[num], filename);
 	if (num < 2) fdd_set(num, filename);
-	else if (v3 && x86_ide_is_placeholder(num - 2)) hdd_set(num - 2, filename);
+	else if (v3 && ide_is_placeholder(num - 2)) hdd_set(num - 2, filename);
 }
 
 void x86_config_save()
diff --git a/support/x86/x86_cdrom.cpp b/support/x86/x86_cdrom.cpp
index 684c6c2..a567d65 100644
--- a/support/x86/x86_cdrom.cpp
+++ b/support/x86/x86_cdrom.cpp
@@ -1324,24 +1324,6 @@ void cdrom_handle_pkt(ide_config *ide)
 	}
 }
 
-static void prep_FA_cmd(ide_config *ide)
-{
-	ide->prepcnt = ide->regs.sector_count;
-	if (!ide->prepcnt || ide->prepcnt > ide_io_max_size) ide->prepcnt = ide_io_max_size;
-
-	ide->regs.status = ATA_STATUS_RDY | ATA_STATUS_DRQ | ATA_STATUS_IRQ;
-
-	if (ide->state == IDE_STATE_INIT_RW)
-	{
-		ide->regs.status &= ~ATA_STATUS_IRQ;
-		ide->null = 1;
-	}
-
-	ide->state = IDE_STATE_WAIT_WR;
-	ide->regs.pkt_io_size = ide->prepcnt * 256;
-	ide_set_regs(ide);
-}
-
 int cdrom_handle_cmd(ide_config *ide)
 {
 	uint8_t drv;
@@ -1356,14 +1338,9 @@ int cdrom_handle_cmd(ide_config *ide)
 		memset(&ide->regs, 0, sizeof(ide->regs));
 		ide->regs.drv = drv;
 		ide->regs.pkt_io_size = 256;
-		ide->regs.status = ATA_STATUS_RDY | ATA_STATUS_DRQ | ATA_STATUS_IRQ;
+		ide->regs.status = ATA_STATUS_RDY | ATA_STATUS_DRQ | ATA_STATUS_IRQ | ATA_STATUS_END;
 		ide_set_regs(ide);
-		ide->state = IDE_STATE_WAIT_END;
-		break;
-
-	case 0xFA: // mount image
-		ide->state = IDE_STATE_INIT_RW;
-		prep_FA_cmd(ide);
+		ide->state = IDE_STATE_IDLE;
 		break;
 
 	case 0xEC: // identify (fail)
diff --git a/support/x86/x86_ide.cpp b/support/x86/x86_ide.cpp
index a591672..e3da433 100644
--- a/support/x86/x86_ide.cpp
+++ b/support/x86/x86_ide.cpp
@@ -30,7 +30,13 @@
 	#define dbg_hexdump(...)  void()
 #endif
 
-#define IOWR(base, reg, value, ver) x86_dma_set((base) + ((ver) ? (reg) : ((reg)<<2)), value)
+#if 0
+	#define dbg2_printf     printf
+#else
+	#define dbg2_printf(...)   void()
+#endif
+
+#define IOWR(base, reg, value, ver) x86_dma_set((base) + ((ver) ? (reg) : ((reg)<<2)), (value))
 
 #define ide_send_data(databuf, size) x86_dma_sendbuf(ide->base + 255, (size), (uint32_t*)(databuf))
 #define ide_recv_data(databuf, size) x86_dma_recvbuf(ide->base + 255, (size), (uint32_t*)(databuf))
@@ -40,6 +46,46 @@ uint8_t ide_buf[ide_io_max_size * 512];
 
 ide_config ide_inst[2] = {};
 
+uint16_t ide_check(int status)
+{
+	uint16_t res;
+	EnableIO();
+	res = spi_w(UIO_DMA_SDIO);
+	if (status || !res) res = (uint8_t)spi_w((uint16_t)status);
+	DisableIO();
+	return res;
+}
+
+int ide_img_mount(fileTYPE *f, const char *name, int rw)
+{
+	FileClose(f);
+	int writable = 0, ret = 0;
+
+	int len = strlen(name);
+	if (len)
+	{
+		const char *ext = name + len - 4;
+		if (!strncasecmp(".chd", ext, 4))
+		{
+			ret = 1;
+		}
+		else {
+			writable = rw && FileCanWrite(name);
+			ret = FileOpenEx(f, name, writable ? (O_RDWR | O_SYNC) : O_RDONLY);
+			if (!ret) printf("Failed to open file %s\n", name);
+		}
+	}
+
+	if (!ret)
+	{
+		f->size = 0;
+		return 0;
+	}
+
+	printf("Mount %s as %s\n", name, writable ? "read-write" : "read-only");
+	return 1;
+}
+
 void ide_print_regs(regs_t *regs)
 {
 	printf("\nIDE regs:\n");
@@ -78,27 +124,45 @@ void ide_get_regs(ide_config *ide)
 
 void ide_set_regs(ide_config *ide)
 {
-	uint32_t data[3];
+	if (!(ide->regs.status & (ATA_STATUS_BSY | ATA_STATUS_ERR))) ide->regs.status |= ATA_STATUS_DSC;
 
-	if(!(ide->regs.status & (ATA_STATUS_BSY | ATA_STATUS_ERR))) ide->regs.status |= ATA_STATUS_SKC;
+	uint8_t data[12] =
+	{
+		(uint8_t)((ide->drive[ide->regs.drv].cd) ? 0x80 : ide->regs.io_size),
+		(uint8_t)(ide->regs.error),
+		(uint8_t)(ide->regs.sector_count),
+		(uint8_t)(ide->regs.sector),
 
-	data[0] = (ide->drive[ide->regs.drv].cd) ? 0x80 : ide->regs.io_size;
-	data[0] |= ide->regs.error << 8;
-	data[0] |= ide->regs.sector_count << 16;
-	data[0] |= ide->regs.sector << 24;
+		(uint8_t)(ide->regs.cylinder),
+		(uint8_t)(ide->regs.cylinder >> 8),
+		(uint8_t)(ide->regs.cylinder >> 16),
+		(uint8_t)(ide->regs.cylinder >> 24),
 
-	data[1] = ide->regs.cylinder;
+		(uint8_t)(ide->drive[ide->regs.drv].cd ? ide->regs.pkt_io_size : 0),
+		(uint8_t)(ide->drive[ide->regs.drv].cd ? ide->regs.pkt_io_size >> 8 : 0),
+		(uint8_t)((ide->regs.lba ? 0xE0 : 0xA0) | (ide->regs.drv ? 0x10 : 0x00) | ide->regs.head),
+		(uint8_t)(ide->regs.status)
+	};
 
-	data[2] = (ide->drive[ide->regs.drv].cd) ? ide->regs.pkt_io_size : 0;
-	data[2] |= ide->regs.head << 16;
-	data[2] |= ide->regs.drv << 20;
-	data[2] |= (ide->regs.lba ? 7 : 5) << 21;
-	data[2] |= ide->regs.status << 24;
-
-	x86_dma_sendbuf(ide->base, 3, data);
+	//hexdump(data, 12, ide->base);
+	x86_dma_sendbuf(ide->base, 3, (uint32_t*)data);
 }
 
-void x86_ide_set(uint32_t num, uint32_t baseaddr, fileTYPE *f, int ver, int cd)
+static void ide_set_geometry(drive_t *drive, uint16_t sectors, uint16_t heads)
+{
+	printf("New SPT=%d, Heads=%d\n", sectors, heads);
+
+	drive->heads = heads ? heads : 16;
+	drive->spt = sectors ? sectors : 256;
+
+	uint32_t cylinders = drive->f->size / (drive->heads * drive->spt * 512);
+	if (cylinders > 65535) cylinders = 65535;
+
+	//Maximum 137GB images are supported.
+	drive->cylinders = cylinders;
+}
+
+void ide_set(uint32_t num, uint32_t baseaddr, fileTYPE *f, int ver, int cd, int sectors, int heads)
 {
 	int drvnum = num;
 	int drv = (ver == 3) ? (num & 1) : 0;
@@ -114,12 +178,14 @@ void x86_ide_set(uint32_t num, uint32_t baseaddr, fileTYPE *f, int ver, int cd)
 	drive->cylinders = 0;
 	drive->heads = 0;
 	drive->spt = 0;
+	drive->spb = 0;
 	//drive->total_sectors = 0;
 
 	drive->present = f ? 1 : 0;
 	ide_inst[num].state = IDE_STATE_RESET;
+	ide_inst[num].bitoff = num * 3;
 
-	drive->cd = drive->present && (ver == 3) && num && cd;
+	drive->cd = drive->present && (ver == 3) && cd;
 
 	if (ver == 3)
 	{
@@ -153,17 +219,102 @@ void x86_ide_set(uint32_t num, uint32_t baseaddr, fileTYPE *f, int ver, int cd)
 
 	if (!drive->cd)
 	{
-		if (drive->present)
+		if (drive->present) ide_set_geometry(drive, sectors, heads);
+
+		/*uint16_t identify[256] =
 		{
-			drive->heads = 16;
-			drive->spt = (ver == 3) ? 256 : 63;
+			0x0040, 											//word 0
+			drive->cylinders,									//word 1
+			0x0000,												//word 2 reserved
+			drive->heads,										//word 3
+			0x0000,												//word 4 obsolete
+			0x0000,												//word 5 obsolete
+			drive->spt,											//word 6
+			0x0000,												//word 7 vendor specific
+			0x0000,												//word 8 vendor specific
+			0x0000,												//word 9 vendor specific
+			('A' << 8) | 'O',									//word 10
+			('H' << 8) | 'D',									//word 11
+			('0' << 8) | '0',									//word 12
+			('0' << 8) | '0',									//word 13
+			('0' << 8) | ' ',									//word 14
+			(' ' << 8) | ' ',									//word 15
+			(' ' << 8) | ' ',									//word 16
+			(' ' << 8) | ' ',									//word 17
+			(' ' << 8) | ' ',									//word 18
+			(' ' << 8) | ' ',									//word 19
+			0,   												//word 20 buffer type
+			0,													//word 21 cache size
+			0,													//word 22 number of ecc bytes
+			0,0,0,0,											//words 23..26 firmware revision
+			(' ' << 8) | ' ',									//words 27..46 model number
+			(' ' << 8) | ' ',
+			(' ' << 8) | ' ',
+			(' ' << 8) | ' ',
+			(' ' << 8) | ' ',
+			(' ' << 8) | ' ',
+			(' ' << 8) | ' ',
+			(' ' << 8) | ' ',
+			(' ' << 8) | ' ',
+			(' ' << 8) | ' ',
+			(' ' << 8) | ' ',
+			(' ' << 8) | ' ',
+			(' ' << 8) | ' ',
+			(' ' << 8) | ' ',
+			(' ' << 8) | ' ',
+			(' ' << 8) | ' ',
+			(' ' << 8) | ' ',
+			(' ' << 8) | ' ',
+			(' ' << 8) | ' ',
+			(' ' << 8) | ' ',
+			0x8010,												//word 47 max multiple sectors
+			0,													//word 48 dword io
+			1 << 9,												//word 49 lba supported
+			0,													//word 50 reserved
+			0,													//word 51 pio timing
+			0,													//word 52 pio timing
+			1,													//word 53 valid fields
+			drive->cylinders, 									//word 54
+			drive->heads,										//word 55
+			drive->spt,											//word 56
+			(uint16_t)(drive->total_sectors & 0xFFFF),			//word 57
+			(uint16_t)(drive->total_sectors >> 16),				//word 58
+			0,													//word 59 multiple sectors
+			(uint16_t)(drive->total_sectors & 0xFFFF),			//word 60 LBA-28
+			(uint16_t)(drive->total_sectors >> 16),				//word 61 LBA-28
+			0,													//word 62 single word dma modes
+			0,													//word 63 multiple word dma modes
+			0,													//word 64 pio modes
+			0,0,0,0,											//word 65..68
+			0,0,0,0,0,0,0,0,0,0,0,								//word 69..79
+			0,													//word 80 ata modes
+			0,													//word 81 minor version number
+			0, 													//word 82 supported commands
+			0,													//word 83
+			0,	    											//word 84
+			0,  												//word 85
+			0,													//word 86
+			0,			    									//word 87
+			0,													//word 88
+			0,0,0,0,											//word 89..92
+			0,													//word 93
+			0,0,0,0,0,0,										//word 94..99
+			(uint16_t)(drive->total_sectors & 0xFFFF),			//word 100 LBA-48
+			(uint16_t)(drive->total_sectors >> 16),				//word 101 LBA-48
+			0,													//word 102 LBA-48
+			0,													//word 103 LBA-48
 
-			uint32_t cylinders = drive->f->size / (drive->heads * drive->spt * 512);
-			if (cylinders > 65535) cylinders = 65535;
+			0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,	//word 104..127
 
-			//Maximum 137GB images are supported.
-			drive->cylinders = cylinders;
-		}
+			0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,					//word 128..255
+			0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+			0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+			0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+			0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+			0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+			0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+			0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+		};*/
 
 		uint16_t identify[256] =
 		{
@@ -234,14 +385,14 @@ void x86_ide_set(uint32_t num, uint32_t baseaddr, fileTYPE *f, int ver, int cd)
 			0x007E,												//word 80 ata modes
 			0x0000,												//word 81 minor version number
 			(1 << 14) | (1 << 9), 								//word 82 supported commands
-			(1 << 14) | (1 << 13) | (1 << 12) | (1 << 10),		//word 83
+			(1 << 14) | (1 << 13) | (1 << 12),					//word 83
 			1 << 14,	    									//word 84
 			(1 << 14) | (1 << 9),  								//word 85
-			(1 << 14) | (1 << 13) | (1 << 12) | (1 << 10),		//word 86
+			(1 << 14) | (1 << 13) | (1 << 12),					//word 86
 			1 << 14,	    									//word 87
 			0x0000,												//word 88
 			0,0,0,0,											//word 89..92
-			1 | (1 << 14) | (1 << 13) | (1 << 9) | (1 << 8) | (1 << 3) | (1 << 1) | (1 << 0), //word 93
+			(1 << 14) | (1 << 13) | (1 << 9) | (1 << 8) | (1 << 3) | (1 << 1) | (1 << 0), //word 93
 			0,0,0,0,0,0,										//word 94..99
 			(uint16_t)(drive->total_sectors & 0xFFFF),			//word 100 LBA-48
 			(uint16_t)(drive->total_sectors >> 16),				//word 101 LBA-48
@@ -370,6 +521,7 @@ void x86_ide_set(uint32_t num, uint32_t baseaddr, fileTYPE *f, int ver, int cd)
 			if (*name) drive->id[27 + i] = ((*name++) << 8) | 0x20;
 			if (*name) drive->id[27 + i] = (drive->id[27 + i] & 0xFF00) | (*name++);
 		}
+		//hexdump(drive->id, 256);
 	}
 
 	if (ver < 3)
@@ -388,92 +540,167 @@ void x86_ide_set(uint32_t num, uint32_t baseaddr, fileTYPE *f, int ver, int cd)
 		(ver < 3) ? num : (num * 2 + drv), drive->present, drive->cylinders, drive->heads, drive->spt, drive->total_sectors);
 }
 
-static void process_read(ide_config *ide)
+static uint32_t get_lba(ide_config *ide)
 {
-	uint32_t lba = ide->regs.sector | (ide->regs.cylinder << 8) | (ide->regs.head << 24);
-
-	uint16_t cnt = ide->regs.sector_count;
-	if (!cnt || cnt > ide_io_max_size) cnt = ide_io_max_size;
-
-	if (ide->state == IDE_STATE_INIT_RW)
+	uint32_t lba;
+	if (ide->regs.lba)
 	{
-		//printf("Read from LBA: %d\n", lba);
-		ide->null = !FileSeekLBA(ide->drive[ide->regs.drv].f, lba);
-	}
-
-	if (!ide->null) ide->null = (FileReadAdv(ide->drive[ide->regs.drv].f, ide_buf, cnt * 512, -1) <= 0);
-	if (ide->null) memset(ide_buf, 0, cnt * 512);
-
-	ide_send_data(ide_buf, cnt * 128);
-
-	lba += cnt;
-	ide->regs.sector_count -= cnt;
-
-	ide->regs.sector = lba;
-	lba >>= 8;
-	ide->regs.cylinder = lba;
-	lba >>= 16;
-	ide->regs.head = lba & 0xF;
-
-	ide->state = ide->regs.sector_count ? IDE_STATE_WAIT_RD : IDE_STATE_WAIT_END;
-
-	ide->regs.io_size = cnt;
-	ide->regs.status = ATA_STATUS_RDY | ATA_STATUS_DRQ | ATA_STATUS_IRQ;
-	ide_set_regs(ide);
-}
-
-static void prep_write(ide_config *ide)
-{
-	ide->prepcnt = ide->regs.sector_count;
-	if (!ide->prepcnt || ide->prepcnt > ide_io_max_size) ide->prepcnt = ide_io_max_size;
-
-	ide->regs.status = ATA_STATUS_RDY | ATA_STATUS_DRQ | ATA_STATUS_IRQ;
-
-	if (ide->state == IDE_STATE_INIT_RW)
-	{
-		ide->regs.status &= ~ATA_STATUS_IRQ;
-		ide->null = 1;
-
-		if (ide->regs.cmd != 0xFA)
-		{
-			uint32_t lba = ide->regs.sector | (ide->regs.cylinder << 8) | (ide->regs.head << 24);
-			//printf("Write to LBA: %d\n", lba);
-			ide->null = !FileSeekLBA(ide->drive[ide->regs.drv].f, lba);
-		}
-	}
-
-	ide->state = IDE_STATE_WAIT_WR;
-	ide->regs.io_size = ide->prepcnt;
-	ide_set_regs(ide);
-}
-
-static void process_write(ide_config *ide)
-{
-	ide_recv_data(ide_buf, ide->prepcnt * 128);
-	if (ide->regs.cmd == 0xFA)
-	{
-		ide->regs.sector_count = 0;
-		char* filename = user_io_make_filepath(HomeDir(), (char*)ide_buf);
-		int drvnum = (ide->regs.head == 1) ? 0 : (ide->regs.head == 2) ? 1 : (ide->drive[ide->regs.drv].drvnum + 2);
-
-		static const char* names[6] = { "fdd0", "fdd1", "ide00", "ide01", "ide10", "ide11" };
-		printf("Request for new image for drive %s: %s\n", names[drvnum], filename);
-		x86_set_image(drvnum, filename);
+		lba = ide->regs.sector | (ide->regs.cylinder << 8) | (ide->regs.head << 24);
 	}
 	else
 	{
-		if (!ide->null) ide->null = (FileWriteAdv(ide->drive[ide->regs.drv].f, ide_buf, ide->prepcnt * 512, -1) <= 0);
+		drive_t *drive = &ide->drive[ide->regs.drv];
+		dbg2_printf("  CHS: %d/%d/%d (%d/%d)\n", ide->regs.cylinder, ide->regs.head, ide->regs.sector, drive->heads, drive->spt);
+		lba = ide->regs.cylinder;
+		lba *= drive->heads;
+		lba += ide->regs.head;
+		lba *= drive->spt;
+		lba += ide->regs.sector - 1;
+	}
 
-		uint32_t lba = ide->regs.sector | (ide->regs.cylinder << 8) | (ide->regs.head << 24);
-		lba += ide->prepcnt;
-		ide->regs.sector_count -= ide->prepcnt;
+	dbg2_printf("  LBA: %u\n", lba);
+	return lba;
+}
 
+static void put_lba(ide_config *ide, uint32_t lba)
+{
+	if (ide->regs.lba)
+	{
 		ide->regs.sector = lba;
 		lba >>= 8;
 		ide->regs.cylinder = lba;
 		lba >>= 16;
 		ide->regs.head = lba & 0xF;
 	}
+	else
+	{
+		drive_t *drive = &ide->drive[ide->regs.drv];
+		uint32_t hspt = drive->heads * drive->spt;
+		ide->regs.cylinder = lba / hspt;
+		lba = lba % hspt;
+		ide->regs.head = lba / drive->spt;
+		lba = lba % drive->spt;
+		ide->regs.sector = lba + 1;
+	}
+}
+
+inline uint16_t get_cnt(ide_config *ide)
+{
+	drive_t *drive = &ide->drive[ide->regs.drv];
+	dbg2_printf("  Cnt: %d (max = %d)\n", ide->regs.sector_count, drive->spb);
+	uint16_t cnt = ide->regs.sector_count;
+	if (!cnt || cnt > drive->spb)
+	{
+		cnt = drive->spb;
+		dbg2_printf("  New cnt: %d\n", cnt);
+	}
+	return cnt;
+}
+
+static void process_read(ide_config *ide, int multi)
+{
+	uint32_t lba = get_lba(ide);
+	uint16_t ide_req = 0;
+
+	dbg2_printf("  sector_count: %d\n", ide->regs.sector_count);
+
+	uint32_t cnt = multi ? get_cnt(ide) : 1;
+	ide->null = !FileSeekLBA(ide->drive[ide->regs.drv].f, lba);
+	if (!ide->null) ide->null = (FileReadAdv(ide->drive[ide->regs.drv].f, ide_buf, cnt * 512, -1) <= 0);
+	if (ide->null) memset(ide_buf, 0, cnt * 512);
+
+	while (1)
+	{
+		lba += cnt;
+		ide->regs.sector_count -= cnt;
+		put_lba(ide, lba);
+
+		ide->regs.io_size = cnt;
+		ide->regs.status = ATA_STATUS_RDP | ATA_STATUS_RDY | ATA_STATUS_DRQ | ATA_STATUS_IRQ;
+		if (!ide->regs.sector_count) ide->regs.status |= ATA_STATUS_END;
+
+		ide_set_regs(ide);
+		ide_send_data(ide_buf, cnt * 128);
+
+		if (!ide->regs.sector_count)
+		{
+			//ATA_STATUS_END will set ATA_STATUS_RDY at the end
+			ide->state = IDE_STATE_IDLE;
+			break;
+		}
+
+		cnt = multi ? get_cnt(ide) : 1;
+		if (!ide->null) ide->null = (FileReadAdv(ide->drive[ide->regs.drv].f, ide_buf, cnt * 512, -1) <= 0);
+		if (ide->null) memset(ide_buf, 0, cnt * 512);
+
+		ide_req = 0;
+		while (!ide_req) ide_req = (ide_check() >> ide->bitoff) & 7;
+
+		if (ide_req != 5)
+		{
+			ide->state = IDE_STATE_IDLE;
+			break;
+		}
+	}
+
+	dbg2_printf("  finish\n");
+}
+
+static void process_write(ide_config *ide, int multi)
+{
+	uint32_t lba = get_lba(ide);
+	uint32_t cnt = 1;
+	uint16_t ide_req;
+
+	ide->null = (ide->regs.cmd != 0xFA) ? !FileSeekLBA(ide->drive[ide->regs.drv].f, lba) : 1;
+	uint8_t irq = 0;
+
+	while (1)
+	{
+		cnt = multi ? get_cnt(ide) : 1;
+		ide->regs.status = ATA_STATUS_RDY | ATA_STATUS_DRQ | irq;
+		irq = ATA_STATUS_IRQ;
+
+		ide->regs.io_size = cnt;
+		ide_set_regs(ide);
+
+		ide_req = 0;
+		while (!ide_req) ide_req = (ide_check() >> ide->bitoff) & 7;
+
+		if (ide_req != 5)
+		{
+			ide->state = IDE_STATE_IDLE;
+			break;
+		}
+
+		ide_recv_data(ide_buf, cnt * 128);
+
+		if (ide->regs.cmd == 0xFA)
+		{
+			ide->regs.sector_count = 0;
+			char* filename = user_io_make_filepath(HomeDir(), (char*)ide_buf);
+			int drvnum = (ide->regs.head == 1) ? 0 : (ide->regs.head == 2) ? 1 : (ide->drive[ide->regs.drv].drvnum + 2);
+
+			static const char* names[6] = { "fdd0", "fdd1", "ide00", "ide01", "ide10", "ide11" };
+			printf("Request for new image for drive %s: %s\n", names[drvnum], filename);
+			x86_set_image(drvnum, filename);
+		}
+		else
+		{
+			if (!ide->null) ide->null = (FileWriteAdv(ide->drive[ide->regs.drv].f, ide_buf, cnt * 512, -1) <= 0);
+			lba += cnt;
+			ide->regs.sector_count -= cnt;
+			put_lba(ide, lba);
+		}
+
+		if (!ide->regs.sector_count)
+		{
+			ide->state = IDE_STATE_IDLE;
+			ide->regs.status = ATA_STATUS_RDY | ATA_STATUS_IRQ;
+			ide_set_regs(ide);
+			break;
+		}
+	}
 }
 
 static int handle_hdd(ide_config *ide)
@@ -481,75 +708,78 @@ static int handle_hdd(ide_config *ide)
 	switch (ide->regs.cmd)
 	{
 	case 0xEC: // identify
-	{
-		//print_regs(&ide->regs);
-		ide_send_data(ide->drive[ide->regs.drv].id, 128);
-
-		uint8_t drv = ide->regs.drv;
-		memset(&ide->regs, 0, sizeof(ide->regs));
-		ide->regs.drv = drv;
+		{
+			uint8_t drv = ide->regs.drv;
+			memset(&ide->regs, 0, sizeof(ide->regs));
+			ide->regs.drv = drv;
+		}
 		ide->regs.io_size = 1;
-		ide->regs.status = ATA_STATUS_RDY | ATA_STATUS_DRQ | ATA_STATUS_IRQ;
+		ide->regs.status = ATA_STATUS_RDP | ATA_STATUS_RDY | ATA_STATUS_DRQ | ATA_STATUS_IRQ | ATA_STATUS_END;
 		ide_set_regs(ide);
-		ide->state = IDE_STATE_WAIT_END;
-	}
-	break;
+		ide_send_data(ide->drive[ide->regs.drv].id, 128);
+		break;
+
+	case 0xC4: // read multiple
+		if (!ide->drive[ide->regs.drv].spb)
+		{
+			printf("(!) Read multiple is disabled!\n");
+			return 1;
+		}
+		process_read(ide, 1);
+		break;
 
 	case 0x20: // read with retry
 	case 0x21: // read
-	case 0xC4: // read multiple
-	{
-		if (!ide->regs.lba)
+		process_read(ide, 0);
+		break;
+
+	case 0xC5: // write multiple
+		if (!ide->drive[ide->regs.drv].spb)
 		{
-			printf("(!) Unsupported Non-LBA read!\n");
+			printf("(!) Read multiple is disabled!\n");
 			return 1;
 		}
-
-		ide->state = IDE_STATE_INIT_RW;
-		process_read(ide);
-	}
-	break;
+		process_write(ide, 1);
+		break;
 
 	case 0x30: // write with retry
 	case 0x31: // write
-	case 0xC5: // write multiple
-	{
-		if (!ide->regs.lba)
-		{
-			printf("(!) Unsupported Non-LBA write!\n");
-			return 1;
-		}
-
-		ide->state = IDE_STATE_INIT_RW;
-		prep_write(ide);
-	}
-	break;
+		process_write(ide, 0);
+		break;
 
 	case 0xFA: // mount image
-	{
-		ide->state = IDE_STATE_INIT_RW;
-		prep_write(ide);
-	}
-	break;
+		process_write(ide, 0);
+		break;
 
 	case 0xC6: // set multople
-	{
-		if (!ide->regs.sector_count || ide->regs.sector_count > ide_io_max_size)
+		if (ide->regs.sector_count > ide_io_max_size)
 		{
 			return 1;
 		}
-
-		ide->regs.status = ATA_STATUS_RDY;
+		ide->drive[ide->regs.drv].spb = ide->regs.sector_count;
+		printf("New block size: %d\n", ide->drive[ide->regs.drv].spb);
+		ide->regs.status = ATA_STATUS_RDY | ATA_STATUS_IRQ;
 		ide_set_regs(ide);
-	}
-	break;
+		break;
 
 	case 0x08: // reset (fail)
 		printf("Reset command (08h) for HDD not supported\n");
 		return 1;
 
+	case 0x10 ... 0x1F: // recalibrate
+		ide->regs.status = ATA_STATUS_RDY | ATA_STATUS_IRQ;
+		ide->regs.cylinder = 0;
+		ide_set_regs(ide);
+		break;
+
+	case 0x91: // initialize device parameters
+		ide_set_geometry(&ide->drive[ide->regs.drv], ide->regs.sector_count, ide->regs.head + 1);
+		ide->regs.status = ATA_STATUS_RDY | ATA_STATUS_IRQ;
+		ide_set_regs(ide);
+		break;
+
 	default:
-		printf("(!) Unsupported command\n");
+		printf("(!) Unsupported command (%04X)\n", ide->base);
 		ide_print_regs(&ide->regs);
 		return 1;
 	}
@@ -557,10 +787,12 @@ static int handle_hdd(ide_config *ide)
 	return 0;
 }
 
-void x86_ide_io(int num, int req)
+void ide_io(int num, int req)
 {
 	ide_config *ide = &ide_inst[num];
 
+	//printf("req: %d, disk: %d\n", req, num);
+
 	if (req == 0) // no request
 	{
 		if (ide->state == IDE_STATE_RESET)
@@ -578,9 +810,12 @@ void x86_ide_io(int num, int req)
 		ide->state = IDE_STATE_IDLE;
 		ide_get_regs(ide);
 
+		dbg2_printf("IDE command: %02X (on %d)\n", ide->regs.cmd, ide->regs.drv);
+
 		int err = 0;
 
-		if (ide->drive[ide->regs.drv].cd) err = cdrom_handle_cmd(ide);
+		if(ide->regs.cmd == 0xFA) err = handle_hdd(ide);
+		else if (ide->drive[ide->regs.drv].cd) err = cdrom_handle_cmd(ide);
 		else if (!ide->drive[ide->regs.drv].present) err = 1;
 		else err = handle_hdd(ide);
 
@@ -593,32 +828,8 @@ void x86_ide_io(int num, int req)
 	}
 	else if (req == 5) // data request
 	{
-		dbg_printf("IDE data request\n");
-		if (ide->state == IDE_STATE_WAIT_END)
-		{
-			ide->state = IDE_STATE_IDLE;
-			ide->regs.status = ATA_STATUS_RDY;
-			ide_set_regs(ide);
-		}
-		else if (ide->state == IDE_STATE_WAIT_RD)
-		{
-			process_read(ide);
-		}
-		else if (ide->state == IDE_STATE_WAIT_WR)
-		{
-			process_write(ide);
-			if (ide->regs.sector_count)
-			{
-				prep_write(ide);
-			}
-			else
-			{
-				ide->state = IDE_STATE_IDLE;
-				ide->regs.status = ATA_STATUS_RDY;
-				ide_set_regs(ide);
-			}
-		}
-		else if (ide->state == IDE_STATE_WAIT_PKT_CMD)
+		dbg2_printf("IDE data request (on %d)\n", ide->regs.drv);
+		if (ide->state == IDE_STATE_WAIT_PKT_CMD)
 		{
 			cdrom_handle_pkt(ide);
 		}
@@ -669,20 +880,20 @@ void x86_ide_io(int num, int req)
 	}
 }
 
-int x86_ide_is_placeholder(int num)
+int ide_is_placeholder(int num)
 {
 	return ide_inst[num / 2].drive[num & 1].placeholder;
 }
 
-void x86_ide_reset(uint8_t hotswap)
+void ide_reset(uint8_t hotswap[4])
 {
 	ide_inst[0].drive[0].placeholder = 0;
 	ide_inst[0].drive[1].placeholder = 0;
 	ide_inst[1].drive[0].placeholder = 0;
 	ide_inst[1].drive[1].placeholder = 0;
 
-	ide_inst[0].drive[0].allow_placeholder = 0;
-	ide_inst[0].drive[1].allow_placeholder = 0;
-	ide_inst[1].drive[0].allow_placeholder = hotswap & 1;
-	ide_inst[1].drive[1].allow_placeholder = (hotswap >> 1) & 1;
+	ide_inst[0].drive[0].allow_placeholder = hotswap[0];
+	ide_inst[0].drive[1].allow_placeholder = hotswap[1];
+	ide_inst[1].drive[0].allow_placeholder = hotswap[2];
+	ide_inst[1].drive[1].allow_placeholder = hotswap[3];
 }
diff --git a/support/x86/x86_ide.h b/support/x86/x86_ide.h
index 7a91f4a..80a6004 100644
--- a/support/x86/x86_ide.h
+++ b/support/x86/x86_ide.h
@@ -5,13 +5,12 @@
 
 #define ATA_STATUS_BSY  0x80  // busy
 #define ATA_STATUS_RDY  0x40  // ready
-#define ATA_STATUS_DF   0x20  // device fault
-#define ATA_STATUS_WFT  0x20  // write fault (old name)
-#define ATA_STATUS_SKC  0x10  // seek complete
+#define ATA_STATUS_RDP  0x20  // performance read
+#define ATA_STATUS_DSC  0x10  // seek complete
 #define ATA_STATUS_SERV 0x10  // service
 #define ATA_STATUS_DRQ  0x08  // data request
 #define ATA_STATUS_IRQ  0x04  // rise IRQ
-#define ATA_STATUS_IDX  0x02  // index
+#define ATA_STATUS_END  0x02  // last read
 #define ATA_STATUS_ERR  0x01  // error (ATA)
 #define ATA_STATUS_CHK  0x01  // check (ATAPI)
 
@@ -28,13 +27,10 @@
 #define IDE_STATE_IDLE          0
 #define IDE_STATE_RESET         1
 #define IDE_STATE_INIT_RW       2
-#define IDE_STATE_WAIT_RD       3
-#define IDE_STATE_WAIT_WR       4
-#define IDE_STATE_WAIT_END      5
-#define IDE_STATE_WAIT_PKT_CMD  6
-#define IDE_STATE_WAIT_PKT_RD   7
-#define IDE_STATE_WAIT_PKT_END  8
-#define IDE_STATE_WAIT_PKT_MODE 9
+#define IDE_STATE_WAIT_PKT_CMD  3
+#define IDE_STATE_WAIT_PKT_RD   4
+#define IDE_STATE_WAIT_PKT_END  5
+#define IDE_STATE_WAIT_PKT_MODE 6
 
 typedef struct
 {
@@ -82,6 +78,7 @@ typedef struct
 	uint16_t heads;
 	uint16_t spt;
 	uint32_t total_sectors;
+	uint32_t spb;
 
 	uint8_t  placeholder;
 	uint8_t  allow_placeholder;
@@ -98,7 +95,7 @@ typedef struct
 	uint32_t play_end_lba;
 
 	chd_file	*chd_f;
-	int 	chd_hunknum; 	
+	int 	chd_hunknum;
 	uint8_t	*chd_hunkbuf;
 	uint32_t chd_total_size;
 	uint32_t chd_last_partial_lba;
@@ -109,6 +106,7 @@ typedef struct
 typedef struct
 {
 	uint32_t base;
+	uint32_t bitoff;
 	uint32_t state;
 	uint32_t null;
 	uint32_t prepcnt;
@@ -117,6 +115,8 @@ typedef struct
 	drive_t drive[2];
 } ide_config;
 
+#include "x86_cdrom.h"
+
 extern ide_config ide_inst[2];
 extern const uint32_t ide_io_max_size;
 extern uint8_t ide_buf[];
@@ -125,9 +125,11 @@ void ide_print_regs(regs_t *regs);
 void ide_get_regs(ide_config *ide);
 void ide_set_regs(ide_config *ide);
 
-void x86_ide_set(uint32_t num, uint32_t baseaddr, fileTYPE *f, int ver, int cd);
-void x86_ide_io(int num, int req);
-int x86_ide_is_placeholder(int num);
-void x86_ide_reset(uint8_t hotswap);
+uint16_t ide_check(int status = 0);
+void ide_set(uint32_t num, uint32_t baseaddr, fileTYPE *f, int ver, int cd, int sectors = 0, int heads = 0);
+void ide_io(int num, int req);
+int ide_is_placeholder(int num);
+void ide_reset(uint8_t hotswap[4]);
+int ide_img_mount(fileTYPE *f, const char *name, int rw);
 
 #endif
diff --git a/user_io.cpp b/user_io.cpp
index 6e85d9c..843abd2 100644
--- a/user_io.cpp
+++ b/user_io.cpp
@@ -35,6 +35,7 @@
 #include "shmem.h"
 
 #include "support.h"
+#include "support/x86/x86_ide.h"
 
 static char core_path[1024] = {};
 static char rbf_path[1024] = {};
@@ -2500,7 +2501,17 @@ void user_io_poll()
 		spi_w(0);
 		DisableFpga();
 		HandleFDD(c1, c2);
-		HandleHDD(c1, c2);
+
+		uint16_t sd_req = ide_check();
+		if (sd_req & 0x8000)
+		{
+			ide_io(0, sd_req & 7);
+			ide_io(1, (sd_req >> 3) & 7);
+		}
+		else
+		{
+			HandleHDD(c1, c2);
+		}
 		UpdateDriveStatus();
 
 		kbd_fifo_poll();