From 76ac1b50fca2edce7300f3b51172a0947b60c327 Mon Sep 17 00:00:00 2001
From: Toni Wilen <twilen@winuae.net>
Date: Sun, 14 Jan 2024 15:43:50 +0200
Subject: [PATCH] DraCo Amiga HD disk support, PC disk write/format support,
 keyboard repeat support.

---
 draco.cpp                                    |  74 +--
 include/draco.h                              |   2 +-
 inputdevice.cpp                              |  28 +-
 inputevents.def                              |   2 +-
 od-win32/winuae_msvc15/keyboard_at_draco.cpp |  15 +
 x86.cpp                                      | 469 ++++++++++++-------
 6 files changed, 377 insertions(+), 213 deletions(-)

diff --git a/draco.cpp b/draco.cpp
index 9514dfce..aadb3d7d 100644
--- a/draco.cpp
+++ b/draco.cpp
@@ -20,6 +20,7 @@
 #include "zfile.h"
 #include "keybuf.h"
 #include "rommgr.h"
+#include "disk.h"
 
 static int maxcnt = 100;
 
@@ -650,43 +651,56 @@ static void draco_1wire_reset(void)
 #endif
 }
 
+static uae_u16 draco_floppy_data;
+static int draco_floppy_bits, draco_floppy_rate;
+
 // draco reads amiga disks by polling register
 // that returns time since last flux change.
 static uae_u8 draco_floppy_get_data(void)
 {
-	static uae_u16 data;
-	static int bits;
-	if (bits < 8) {
-		uae_u16 t = floppy_get_raw_data();
-		data |= (t & 0xff) << (8 - bits);
-		bits += 8;
+	if (draco_floppy_bits < 8) {
+		uae_u16 t = floppy_get_raw_data(&draco_floppy_rate);
+		draco_floppy_data |= (t & 0xff) << (8 - draco_floppy_bits);
+		draco_floppy_bits += 8;
 	}
-	int bit1 = (data & 0x8000);
-	int bit2 = (data & 0x4000);
-	int bit3 = (data & 0x2000);
-	int bit4 = (data & 0x1000);
+	int bit1 = (draco_floppy_data & 0x8000);
+	int bit2 = (draco_floppy_data & 0x4000);
+	int bit3 = (draco_floppy_data & 0x2000);
+	int bit4 = (draco_floppy_data & 0x1000);
+	int v = 0;
 
 	if (bit1) {
-		data <<= 1;
-		bits--;
-		return 8;
-	}
-	if (bit2) {
-		data <<= 2;
-		bits -= 2;
-		return 24;
+		draco_floppy_data <<= 1;
+		draco_floppy_bits--;
+		v = 8;
+	} else if (bit2) {
+		draco_floppy_data <<= 2;
+		draco_floppy_bits -= 2;
+		v = 24;
+	} else if (bit3) {
+		draco_floppy_data <<= 3;
+		draco_floppy_bits -= 3;
+		v = 40;
+	} else if (bit4) {
+		draco_floppy_data <<= 4;
+		draco_floppy_bits -= 4;
+		v = 56;
 	}
-	if (bit3) {
-		data <<= 3;
-		bits -= 3;
-		return 40;
-	}
-	if (bit4) {
-		data <<= 4;
-		bits -= 4;
-		return 56;
+	switch (draco_floppy_rate) {
+		case FLOPPY_RATE_250K:
+			// above values are in 250Kbs
+			break;
+		case FLOPPY_RATE_500K:
+			v /= 2;
+			break;
+		case FLOPPY_RATE_300K:
+			v = v * 30 / 25;
+			break;
+		case FLOPPY_RATE_1M:
+			v /= 4;
+			break;
 	}
-	return 0;
+	return v;
 }
 
 static void vmotion_write(uaecptr addr, uae_u8 v)
@@ -1184,7 +1198,7 @@ void draco_ext_interrupt(bool i6)
 	draco_irq();
 }
 
-void draco_keycode(uae_u8 scancode, uae_u8 state)
+void draco_keycode(uae_u16 scancode, uae_u8 state)
 {
 	if (currprefs.cs_compatible == CP_DRACO && (currprefs.cpuboard_settings & 0x10)) {
 		if (draco_kbd_buffer_len == 0 && !(draco_reg[3] & DRSTAT_KBDRECV)) {
@@ -1300,6 +1314,8 @@ void draco_reset(int hardreset)
 	draco_superio_cfg[6] = 0xff;
 	draco_superio_cfg[13] = 0x65;
 	draco_superio_cfg[14] = 1;
+	draco_floppy_data = 0;
+	draco_floppy_bits = 0;
 	memset(draco_reg, 0, sizeof(draco_reg));
 	draco_reg[1] = DRCNTRL_FDCINTENA | DRCNTRL_KBDINTENA;
 	draco_reg[5] = 0xff;
diff --git a/include/draco.h b/include/draco.h
index ce98ffa0..58e709b7 100644
--- a/include/draco.h
+++ b/include/draco.h
@@ -6,4 +6,4 @@ void draco_free(void);
 bool draco_mouse(int port, int x, int y, int z, int b);
 void draco_bustimeout(uaecptr addr);
 void draco_ext_interrupt(bool);
-void draco_keycode(uae_u8 scancode, uae_u8 state);
+void draco_keycode(uae_u16 scancode, uae_u8 state);
diff --git a/inputdevice.cpp b/inputdevice.cpp
index 2a0430cb..7d3a0525 100644
--- a/inputdevice.cpp
+++ b/inputdevice.cpp
@@ -199,6 +199,9 @@ static int temp_uid_cnt[IDTYPE_MAX];
 static int gp_swappeddevices[MAX_INPUT_DEVICES][IDTYPE_MAX];
 static int osk_state;
 
+extern int draco_keyboard_get_rate(void);
+static int draco_keybord_repeat_cnt, draco_keybord_repeat_code;
+
 bool osk_status(void)
 {
 	return osk_state != 0;
@@ -4382,6 +4385,17 @@ void inputdevice_hsync (bool forceread)
 			maybe_read_input();
 		}
 	}
+	if (draco_keybord_repeat_cnt > 0) {
+		draco_keybord_repeat_cnt--;
+		if (draco_keybord_repeat_cnt == 0) {
+			int rate = draco_keyboard_get_rate();
+			int b = (rate >> 3) & 3;
+			int d = (rate >> 0) & 7;
+			float r = ((1 << b) * (d + 8) / 240.0f) * 1000.0f;
+			draco_keybord_repeat_cnt = (int)(vblank_hz * maxvpos * r / 1000);
+			draco_keycode(draco_keybord_repeat_code, 1);
+		}
+	}
 }
 
 static uae_u16 POTDAT (int joy)
@@ -5800,6 +5814,7 @@ void inputdevice_reset (void)
 	lightpen_trigger2 = 0;
 	cubo_flag = 0;
 	alg_flag &= 1;
+	draco_keybord_repeat_cnt = 0;
 }
 
 static int getoldport (struct uae_input_device *id)
@@ -10458,9 +10473,18 @@ void inputdevice_draco_key(int kc)
 		if (events[i].data == kc && events[i].data2 && events[i].allow_mask == AM_K) {
 			int code = events[i].data2;
 			if ((code & 0xff00) == 0xe000) {
-				draco_keycode((code & 0xff) | 0x100, state);
+				code = (code & 0xff) | 0x100;
+			} else {
+				code &= 0xff;
+			}
+			draco_keycode(code, state);
+			if (state) {
+				int rate = draco_keyboard_get_rate();
+				int init = ((rate >> 5) & 3) * 250 + 250;
+				draco_keybord_repeat_cnt = (int)(vblank_hz * maxvpos * init / 1000);
+				draco_keybord_repeat_code = code;
 			} else {
-				draco_keycode(code & 0xff, state);
+				draco_keybord_repeat_code = 0;
 			}
 		}
 	}
diff --git a/inputevents.def b/inputevents.def
index 2efefe5e..6d82f849 100644
--- a/inputevents.def
+++ b/inputevents.def
@@ -196,7 +196,7 @@ DEFEVENTKB(KEY_ALT_LEFT,_T("Left Alt"),AM_K,AK_LALT,0x38)
 DEFEVENTKB(KEY_AMIGA_LEFT,_T("Left Amiga"),AM_K,AK_LAMI,0xe05b)
 DEFEVENTKB(KEY_AMIGA_RIGHT,_T("Right Amiga"),AM_K,AK_RAMI,0xe05c)
 DEFEVENTKB(KEY_ALT_RIGHT,_T("Right Alt"),AM_K,AK_RALT,0xe038)
-DEFEVENTKB(KEY_SHIFT_RIGHT,_T("Right Shift"),AM_K,AK_RSH,0x59)
+DEFEVENTKB(KEY_SHIFT_RIGHT,_T("Right Shift"),AM_K,AK_RSH,0x36)
 DEFEVENTKB(KEY_SPACE,_T("Space"),AM_K,AK_SPC,0x39)
 DEFEVENTKB(KEY_CURSOR_UP,_T("Cursor Up"),AM_K,AK_UP,0xe048)
 DEFEVENTKB(KEY_CURSOR_DOWN,_T("Cursor Down"),AM_K,AK_DN,0xe050)
diff --git a/od-win32/winuae_msvc15/keyboard_at_draco.cpp b/od-win32/winuae_msvc15/keyboard_at_draco.cpp
index 176ef762..f9db91e6 100644
--- a/od-win32/winuae_msvc15/keyboard_at_draco.cpp
+++ b/od-win32/winuae_msvc15/keyboard_at_draco.cpp
@@ -25,6 +25,8 @@
     Bits 0 - 1 = scan code set. */
 uint8_t keyboard_mode = 0x02;
 
+extern void write_log(const char *, ...);
+
 void draco_kdb_queue_add(void *d, uint8_t val, int state);
 
 void kbc_at_dev_reset(void *d, int r)
@@ -32,6 +34,12 @@ void kbc_at_dev_reset(void *d, int r)
 }
 void keyboard_at_log(const char *txt, ...)
 {
+    char buffer[256];
+    va_list parms;
+    va_start(parms, txt);
+    vsprintf(buffer, txt, parms);
+    write_log(buffer);
+    va_end(parms);
 }
 void fatalx(const char *txt, ...)
 {
@@ -1155,6 +1163,13 @@ draco_key_process(uint16_t scan, int down)
 void *draco_keyboard_init(void)
 {
     draco_kbd.type = 10;
+    draco_kbd.name = "draco";
     memset(oldkey, 0, sizeof(oldkey));
+    keyboard_at_set_defaults(&draco_kbd);
     return &draco_kbd;
 }
+
+int draco_keyboard_get_rate(void)
+{
+    return draco_kbd.rate;
+}
diff --git a/x86.cpp b/x86.cpp
index 92d94598..49cdaa54 100644
--- a/x86.cpp
+++ b/x86.cpp
@@ -573,7 +573,7 @@ static bool floppy_did_reset;
 static bool floppy_irq;
 static bool floppy_specify_pio;
 static uae_u8 *floppy_pio_buffer;
-static int floppy_pio_len, floppy_pio_cnt;
+static int floppy_pio_len, floppy_pio_cnt, floppy_pio_active;
 static uae_s8 floppy_rate;
 
 #define PC_SEEK_DELAY 50
@@ -750,6 +750,201 @@ static bool floppy_valid_rate(struct floppy_reserved *fr)
 	return fr->rate == floppy_rate || floppy_rate < 0;
 }
 
+static void floppy_format(struct x86_bridge *xb, bool real)
+{
+	uae_u8 cmd = floppy_cmd[0];
+	struct pc_floppy *pcf = &floppy_pc[floppy_num];
+	struct floppy_reserved fr = { 0 };
+	bool valid_floppy = disk_reserved_getinfo(floppy_num, &fr);
+
+#if FLOPPY_DEBUG
+	write_log(_T("Floppy%d %s format MF=%d N=%d:SC=%d:GPL=%d:D=%d\n"),
+		floppy_num, real ? _T("real") : _T("sim"), (floppy_cmd[0] & 0x40) ? 1 : 0,
+		floppy_cmd[2], floppy_cmd[3], floppy_cmd[4], floppy_cmd[5]);
+	write_log(_T("DMA addr %08x len %04x\n"), dma[2].page | dma[2].ac, dma[2].cb);
+	write_log(_T("IMG: Secs=%d Heads=%d\n"), fr.secs, fr.heads);
+#endif
+	int secs = floppy_cmd[3];
+	int sector = pcf->sector;
+	int head = pcf->head;
+	int cyl = pcf->cyl;
+	if (valid_floppy && fr.img) {
+		// TODO: CHRN values totally ignored
+		pcf->head = (floppy_cmd[1] & 4) ? 1 : 0;
+		uae_u8 buf[512];
+		memset(buf, floppy_cmd[5], sizeof buf);
+		uae_u8 *pioptr = floppy_pio_buffer;
+		for (int i = 0; i < secs && i < fr.secs && !fr.wrprot; i++) {
+			uae_u8 cx = 0, hx = 0, rx = 0, nx = 0;
+			if (floppy_specify_pio) {
+				if (real) {
+					cx = *pioptr++;
+					hx = *pioptr++;
+					rx = *pioptr++;
+					nx = *pioptr++;
+					floppy_pio_cnt += 4;
+				} else {
+					floppy_pio_len += 4;
+				}
+			} else {
+				if (real) {
+					cx = dma_channel_read(2);
+					hx = dma_channel_read(2);
+					rx = dma_channel_read(2);
+					nx = dma_channel_read(2);
+				}
+			}
+			pcf->sector = rx - 1;
+#if FLOPPY_DEBUG
+			write_log(_T("Floppy%d %d/%d: C=%d H=%d R=%d N=%d\n"), floppy_num, i, fr.secs, cx, hx, rx, nx);
+#endif
+			if (real) {
+				zfile_fseek(fr.img, (pcf->cyl * fr.secs * fr.heads + pcf->head * fr.secs + pcf->sector) * 512, SEEK_SET);
+				zfile_fwrite(buf, 1, 512, fr.img);
+			}
+			pcf->sector++;
+		}
+	} else {
+		floppy_status[0] |= 0x40; // abnormal termination
+		floppy_status[0] |= 0x10; // equipment check
+	}
+	floppy_cmd_len = 7;
+	if (fr.wrprot) {
+		floppy_status[0] |= 0x40; // abnormal termination
+		floppy_status[1] |= 0x02; // not writable
+	}
+	floppy_result[0] = floppy_status[0];
+	floppy_result[1] = floppy_status[1];
+	floppy_result[2] = floppy_status[2];
+	floppy_result[3] = pcf->cyl;
+	floppy_result[4] = pcf->head;
+	floppy_result[5] = pcf->sector + 1;
+	floppy_result[6] = floppy_cmd[2];
+	if (real) {
+		floppy_delay_hsync = 10;
+		disk_reserved_setinfo(floppy_num, pcf->cyl, pcf->head, 1);
+	} else {
+		pcf->cyl = cyl;
+		pcf->sector = sector;
+		pcf->head = head;
+	}
+}
+
+static void floppy_write(struct x86_bridge *xb, bool real)
+{
+	uae_u8 cmd = floppy_cmd[0];
+	struct pc_floppy *pcf = &floppy_pc[floppy_num];
+	struct floppy_reserved fr = { 0 };
+	bool valid_floppy = disk_reserved_getinfo(floppy_num, &fr);
+
+#if FLOPPY_DEBUG
+	write_log(_T("Floppy%d %s write MT=%d MF=%d C=%d:H=%d:R=%d:N=%d:EOT=%d:GPL=%d:DTL=%d\n"),
+		floppy_num, real ? _T("real") : _T("sim"), (floppy_cmd[0] & 0x80) ? 1 : 0, (floppy_cmd[0] & 0x40) ? 1 : 0,
+		floppy_cmd[2], floppy_cmd[3], floppy_cmd[4], floppy_cmd[5],
+		floppy_cmd[6], floppy_cmd[7], floppy_cmd[8]);
+	write_log(_T("DMA addr %08x len %04x\n"), dma[2].page | dma[2].ac, dma[2].ab);
+#endif
+	floppy_delay_hsync = 50;
+	int eot = floppy_cmd[6];
+	bool mt = (floppy_cmd[0] & 0x80) != 0;
+	int sector = pcf->sector;
+	int head = pcf->head;
+	int cyl = pcf->cyl;
+	if (valid_floppy) {
+		if (fr.img && pcf->cyl != floppy_cmd[2]) {
+			floppy_status[0] |= 0x40; // abnormal termination
+			floppy_status[2] |= 0x20; // wrong cylinder
+		} else if (fr.img) {
+			int end = 0;
+			pcf->sector = floppy_cmd[4] - 1;
+			pcf->head = (floppy_cmd[1] & 4) ? 1 : 0;
+			uae_u8 *pioptr = floppy_pio_buffer;
+			while (!end && !fr.wrprot) {
+				int len = 128 << floppy_cmd[5];
+				uae_u8 buf[512] = { 0 };
+				if (floppy_specify_pio) {
+					for (int i = 0; i < 512 && i < len; i++) {
+						if (real) {
+							if (floppy_pio_cnt >= floppy_pio_len) {
+								end = 1;
+								break;
+							}
+							int v = *pioptr++;
+							buf[i] = v;
+							floppy_pio_cnt++;
+						} else {
+							floppy_pio_len++;
+						}
+					}
+				} else {
+					for (int i = 0; i < 512 && i < len; i++) {
+						if (real) {
+							int v = dma_channel_read(2);
+							if (v < 0) {
+								end = -1;
+								break;
+							}
+							buf[i] = v;
+							if (v >= 0x10000) {
+								end = 1;
+								break;
+							}
+						}
+					}
+				}
+#if FLOPPY_DEBUG
+				write_log(_T("LEN=%d END=%d C=%d H=%d S=%d. IMG S=%d H=%d\n"), len, end, pcf->cyl, pcf->head, pcf->sector, fr.secs, fr.heads);
+#endif
+				if (end < 0)
+					break;
+				if (real) {
+					zfile_fseek(fr.img, (pcf->cyl * fr.secs * fr.heads + pcf->head * fr.secs + pcf->sector) * 512, SEEK_SET);
+					zfile_fwrite(buf, 1, 512, fr.img);
+				}
+				pcf->sector++;
+				if (pcf->sector == eot) {
+					if (mt) {
+						pcf->sector = 0;
+						if (pcf->head)
+							pcf->cyl++;
+						pcf->head ^= 1;
+					} else {
+						break;
+					}
+				}
+				if (pcf->sector >= fr.secs) {
+					pcf->sector = 0;
+					pcf->head ^= 1;
+					break;
+				}
+			}
+			floppy_result[3] = cyl;
+			floppy_result[4] = pcf->head;
+			floppy_result[5] = pcf->sector + 1;
+			floppy_result[6] = floppy_cmd[5];
+		} else {
+			floppy_status[0] |= 0x40; // abnormal termination
+			floppy_status[0] |= 0x10; // equipment check
+		}
+	}
+	floppy_cmd_len = 7;
+	if (fr.wrprot) {
+		floppy_status[0] |= 0x40; // abnormal termination
+		floppy_status[1] |= 0x02; // not writable
+	}
+	floppy_result[0] = floppy_status[0];
+	floppy_result[1] = floppy_status[1];
+	floppy_result[2] = floppy_status[2];
+	if (real) {
+		floppy_delay_hsync = 10;
+		disk_reserved_setinfo(floppy_num, pcf->cyl, pcf->head, 1);
+	} else {
+		pcf->cyl = cyl;
+		pcf->sector = sector;
+		pcf->head = head;
+	}
+}
+
 static void floppy_do_cmd(struct x86_bridge *xb)
 {
 	uae_u8 cmd = floppy_cmd[0];
@@ -834,87 +1029,7 @@ static void floppy_do_cmd(struct x86_bridge *xb)
 
 		case 5:
 		{
-#if FLOPPY_DEBUG
-			write_log(_T("Floppy%d write MT=%d MF=%d C=%d:H=%d:R=%d:N=%d:EOT=%d:GPL=%d:DTL=%d\n"),
-				floppy_num, (floppy_cmd[0] & 0x80) ? 1 : 0, (floppy_cmd[0] & 0x40) ? 1 : 0,
-				floppy_cmd[2], floppy_cmd[3], floppy_cmd[4], floppy_cmd[5],
-				floppy_cmd[6], floppy_cmd[7], floppy_cmd[8]);
-			write_log(_T("DMA addr %08x len %04x\n"), dma[2].page | dma[2].ac, dma[2].ab);
-#endif
-			floppy_delay_hsync = 50;
-			int eot = floppy_cmd[6];
-			bool mt = (floppy_cmd[0] & 0x80) != 0;
-			int cyl = pcf->cyl;
-			if (valid_floppy) {
-				if (fr.img && pcf->cyl != floppy_cmd[2]) {
-					floppy_status[0] |= 0x40; // abnormal termination
-					floppy_status[2] |= 0x20; // wrong cylinder
-				} else if (fr.img) {
-					int end = 0;
-					pcf->sector = floppy_cmd[4] - 1;
-					pcf->head = (floppy_cmd[1] & 4) ? 1 : 0;
-					while (!end && !fr.wrprot) {
-						int len = 128 << floppy_cmd[5];
-						uae_u8 buf[512] = { 0 };
-						if (floppy_specify_pio) {
-							end = -1;
-						} else {
-							for (int i = 0; i < 512 && i < len; i++) {
-								int v = dma_channel_read(2);
-								if (v < 0) {
-									end = -1;
-									break;
-								}
-								buf[i] = v;
-								if (v >= 0x10000) {
-									end = 1;
-									break;
-								}
-							}
-						}
-#if FLOPPY_DEBUG
-						write_log(_T("LEN=%d END=%d C=%d H=%d S=%d. IMG S=%d H=%d\n"), len, end, pcf->cyl, pcf->head, pcf->sector, fr.secs, fr.heads);
-#endif
-						if (end < 0)
-							break;
-						zfile_fseek(fr.img, (pcf->cyl * fr.secs * fr.heads + pcf->head * fr.secs + pcf->sector) * 512, SEEK_SET);
-						zfile_fwrite(buf, 1, 512, fr.img);
-						pcf->sector++;
-						if (pcf->sector == eot) {
-							if (mt) {
-								pcf->sector = 0;
-								if (pcf->head)
-									pcf->cyl++;
-								pcf->head ^= 1;
-							} else {
-								break;
-							}
-						}
-						if (pcf->sector >= fr.secs) {
-							pcf->sector = 0;
-							pcf->head ^= 1;
-							break;
-						}
-					}
-					floppy_result[3] = cyl;
-					floppy_result[4] = pcf->head;
-					floppy_result[5] = pcf->sector + 1;
-					floppy_result[6] = floppy_cmd[5];
-				} else {
-					floppy_status[0] |= 0x40; // abnormal termination
-					floppy_status[0] |= 0x10; // equipment check
-				}
-			}
-			floppy_cmd_len = 7;
-			if (fr.wrprot) {
-				floppy_status[0] |= 0x40; // abnormal termination
-				floppy_status[1] |= 0x02; // not writable
-			}
-			floppy_result[0] = floppy_status[0];
-			floppy_result[1] = floppy_status[1];
-			floppy_result[2] = floppy_status[2];
-			floppy_delay_hsync = 10;
-			disk_reserved_setinfo(floppy_num, pcf->cyl, pcf->head, 1);
+			floppy_write(xb, true);
 		}
 		break;
 
@@ -957,6 +1072,7 @@ static void floppy_do_cmd(struct x86_bridge *xb)
 							memcpy(pioptr, buf, 512);
 							pioptr += 512;
 							floppy_pio_len += 512;
+							floppy_pio_active = 1;
 						} else {
 							for (int i = 0; i < 512 && i < len; i++) {
 								int v = dma_channel_write(2, buf[i]);
@@ -1036,56 +1152,13 @@ static void floppy_do_cmd(struct x86_bridge *xb)
 			pcf->sector %= fr.secs;
 		}
 
-		floppy_delay_hsync = 10;
+		floppy_delay_hsync = maxvpos * 20;
 		disk_reserved_setinfo(floppy_num, pcf->cyl, pcf->head, 1);
 		break;
 
 		case 13:
 		{
-#if FLOPPY_DEBUG
-			write_log(_T("Floppy%d format MF=%d N=%d:SC=%d:GPL=%d:D=%d\n"),
-				floppy_num, (floppy_cmd[0] & 0x40) ? 1 : 0,
-				floppy_cmd[2], floppy_cmd[3], floppy_cmd[4], floppy_cmd[5]);
-			write_log(_T("DMA addr %08x len %04x\n"), dma[2].page | dma[2].ac, dma[2].cb);
-			write_log(_T("IMG: Secs=%d Heads=%d\n"), fr.secs, fr.heads);
-#endif
-			int secs = floppy_cmd[3];
-			if (valid_floppy && fr.img) {
-				// TODO: CHRN values totally ignored
-				pcf->head = (floppy_cmd[1] & 4) ? 1 : 0;
-				uae_u8 buf[512];
-				memset(buf, floppy_cmd[5], sizeof buf);
-				for (int i = 0; i < secs && i < fr.secs && !fr.wrprot; i++) {
-					uae_u8 cx = dma_channel_read(2);
-					uae_u8 hx = dma_channel_read(2);
-					uae_u8 rx = dma_channel_read(2);
-					uae_u8 nx = dma_channel_read(2);
-					pcf->sector = rx - 1;
-#if FLOPPY_DEBUG
-					write_log(_T("Floppy%d %d/%d: C=%d H=%d R=%d N=%d\n"), floppy_num, i, fr.secs, cx, hx, rx, nx);
-#endif
-					zfile_fseek(fr.img, (pcf->cyl * fr.secs * fr.heads + pcf->head * fr.secs + pcf->sector) * 512, SEEK_SET);
-					zfile_fwrite(buf, 1, 512, fr.img);
-					pcf->sector++;
-				}
-			} else {
-				floppy_status[0] |= 0x40; // abnormal termination
-				floppy_status[0] |= 0x10; // equipment check
-			}
-			floppy_cmd_len = 7;
-			if (fr.wrprot) {
-				floppy_status[0] |= 0x40; // abnormal termination
-				floppy_status[1] |= 0x02; // not writable
-			}
-			floppy_result[0] = floppy_status[0];
-			floppy_result[1] = floppy_status[1];
-			floppy_result[2] = floppy_status[2];
-			floppy_result[3] = pcf->cyl;
-			floppy_result[4] = pcf->head;
-			floppy_result[5] = pcf->sector + 1;
-			floppy_result[6] = floppy_cmd[2];
-			floppy_delay_hsync = 10;
-			disk_reserved_setinfo(floppy_num, pcf->cyl, pcf->head, 1);
+			floppy_format(xb, true);
 		}
 		break;
 
@@ -1169,63 +1242,97 @@ static void outfloppy(struct x86_bridge *xb, int portnum, uae_u8 v)
 		}
 		break;
 		case 0x3f5: // data reg
-		floppy_cmd[floppy_idx] = v;
-		if (floppy_idx == 0) {
-			floppy_cmd_len = -1;
-			switch(v & 31)
-			{
-				case 3: // specify
-				floppy_cmd_len = 3;
-				break;
-				case 4: // sense drive status
-				floppy_cmd_len = 2;
-				break;
-				case 5: // write data
-				floppy_cmd_len = 9;
-				break;
-				case 6: // read data
-				floppy_cmd_len = 9;
-				break;
-				case 7: // recalibrate
-				floppy_cmd_len = 2;
-				break;
-				case 8: // sense interrupt status
-				floppy_cmd_len = 1;
-				break;
-				case 10: // read id
-				floppy_cmd_len = 2;
-				break;
-				case 12: // perpendiculaor mode
-				if (xb->type < 0) {
+		if (floppy_pio_len > 0 && floppy_pio_cnt < floppy_pio_len && floppy_pio_active) {
+			floppy_pio_buffer[floppy_pio_cnt++] = v;
+			if (floppy_pio_cnt >= floppy_pio_len) {
+				floppy_pio_cnt = 0;
+				floppy_pio_active = 0;
+				floppy_clear_irq();
+				floppy_do_cmd(xb);
+				floppy_pio_cnt = 0;
+				floppy_pio_len = 0;
+			}
+		} else {
+			floppy_cmd[floppy_idx] = v;
+			if (floppy_idx == 0) {
+				floppy_cmd_len = -1;
+				switch(v & 31)
+				{
+					case 3: // specify
+					floppy_cmd_len = 3;
+					break;
+					case 4: // sense drive status
 					floppy_cmd_len = 2;
-				}
-				break;
-				case 13: // format track
-				floppy_cmd_len = 6;
-				break;
-				case 15: // seek
-				floppy_cmd_len = 3;
-				break;
-				case 16: // get versionm
-				if (xb->type < 0) {
+					break;
+					case 5: // write data
+					floppy_cmd_len = 9;
+					break;
+					case 6: // read data
+					floppy_cmd_len = 9;
+					break;
+					case 7: // recalibrate
+					floppy_cmd_len = 2;
+					break;
+					case 8: // sense interrupt status
 					floppy_cmd_len = 1;
+					break;
+					case 10: // read id
+					floppy_cmd_len = 2;
+					break;
+					case 12: // perpendiculaor mode
+					if (xb->type < 0) {
+						floppy_cmd_len = 2;
+					}
+					break;
+					case 13: // format track
+					floppy_cmd_len = 6;
+					break;
+					case 15: // seek
+					floppy_cmd_len = 3;
+					break;
+					case 16: // get versionm
+					if (xb->type < 0) {
+						floppy_cmd_len = 1;
+					}
+					break;
+					case 19: // configure
+					if (xb->type < 0) {
+						floppy_cmd_len = 4;
+					}
+					break;
 				}
-				break;
-				case 19: // configure
-				if (xb->type < 0) {
-					floppy_cmd_len = 4;
+				if (floppy_cmd_len < 0) {
+					write_log(_T("Floppy unimplemented command %02x\n"), v);
+					floppy_cmd_len = 1;
 				}
-				break;
 			}
-			if (floppy_cmd_len < 0) {
-				write_log(_T("Floppy unimplemented command %02x\n"), v);
-				floppy_cmd_len = 1;
+			floppy_idx++;
+			if (floppy_idx >= floppy_cmd_len) {
+				if (floppy_specify_pio && (floppy_cmd[0] & 31) == 5) {
+					floppy_write(xb, false);
+					floppy_pio_cnt = 0;
+					floppy_pio_active = 0;
+					if (floppy_pio_len > 0) {
+						floppy_delay_hsync = 10;
+						floppy_pio_active = 1;
+					} else {
+						floppy_write(xb, true);
+					}
+				} else if (floppy_specify_pio && (floppy_cmd[0] & 31) == 13) {
+					floppy_format(xb, false);
+					floppy_pio_cnt = 0;
+					floppy_pio_active = 0;
+					if (floppy_pio_len > 0) {
+						floppy_delay_hsync = 10;
+						floppy_pio_active = 1;
+					} else {
+						floppy_format(xb, true);
+					}
+				} else {
+					floppy_do_cmd(xb);
+				}
 			}
 		}
-		floppy_idx++;
-		if (floppy_idx >= floppy_cmd_len) {
-			floppy_do_cmd(xb);
-		}
 		break;
 		case 0x3f7: // configuration control
 		if (xb->type >= TYPE_2286) {
@@ -1276,9 +1383,9 @@ static uae_u8 infloppy(struct x86_bridge *xb, int portnum)
 		v = 0;
 		if (!floppy_delay_hsync && (floppy_dpc & 4))
 			v |= 0x80;
-		if (floppy_idx || floppy_delay_hsync || floppy_pio_len) {
+		if (floppy_idx || floppy_delay_hsync || floppy_pio_active) {
 			v |= 0x10;
-			if (floppy_pio_len)
+			if (floppy_pio_active)
 				v |= 0x20;
 		}
 		if ((v & 0x80) && floppy_dir)
@@ -1293,6 +1400,7 @@ static uae_u8 infloppy(struct x86_bridge *xb, int portnum)
 			v = floppy_pio_buffer[floppy_pio_cnt++];
 			if (floppy_pio_cnt >= floppy_pio_len) {
 				floppy_pio_len = 0;
+				floppy_pio_active = 0;
 				floppy_clear_irq();
 				floppy_delay_hsync = 50;
 			}
@@ -1335,10 +1443,11 @@ static uae_u8 infloppy(struct x86_bridge *xb, int portnum)
 	return v;
 }
 
-uae_u16 floppy_get_raw_data(void)
+uae_u16 floppy_get_raw_data(int *rate)
 {
 	struct floppy_reserved fr = { 0 };
 	bool valid_floppy = disk_reserved_getinfo(floppy_num, &fr);
+	*rate = fr.rate;
 	if (valid_floppy) {
 		return DSKBYTR_fake(fr.num);
 	}
-- 
2.47.3