diff --git a/src/disc.h b/src/disc.h
index 808f64294..358d7c2b3 100644
--- a/src/disc.h
+++ b/src/disc.h
@@ -74,3 +74,36 @@ extern int drive_type[2];
 /*Used in the Read A Track command. Only valid for disc_readsector(). */
 #define SECTOR_FIRST -2
 #define SECTOR_NEXT  -1
+
+/* Bits 0-3 define byte type, bit 5 defines whether it is a per-track (0) or per-sector (1) byte, if bit 7 is set, the byte is the index hole. */
+#define BYTE_GAP0		0x00
+#define BYTE_GAP1		0x10
+#define BYTE_GAP4		0x20
+#define BYTE_GAP2		0x40
+#define BYTE_GAP3		0x50
+#define BYTE_I_SYNC		0x01
+#define BYTE_ID_SYNC		0x41
+#define BYTE_DATA_SYNC		0x51
+#define BYTE_IAM_SYNC		0x02
+#define BYTE_IDAM_SYNC		0x42
+#define BYTE_DATAAM_SYNC	0x52
+#define BYTE_IAM		0x03
+#define BYTE_IDAM		0x43
+#define BYTE_DATAAM		0x53
+#define BYTE_ID			0x44
+#define BYTE_DATA		0x54
+#define BYTE_ID_CRC		0x45
+#define BYTE_DATA_CRC		0x55
+
+#define BYTE_INDEX_HOLE		0x80	/* 1 = index hole, 0 = regular byte */
+#define BYTE_IS_SECTOR		0x40	/* 1 = per-sector, 0 = per-track */
+#define BYTE_IS_POST_TRACK	0x20	/* 1 = after all sectors, 0 = before or during all sectors */
+#define BYTE_IS_DATA		0x10	/* 1 = data, 0 = id */
+#define BYTE_TYPE		0x0F	/* 5 = crc, 4 = data, 3 = address mark, 2 = address mark sync, 1 = sync, 0 = gap */
+
+#define BYTE_TYPE_GAP		0x00
+#define BYTE_TYPE_SYNC		0x01
+#define BYTE_TYPE_AM_SYNC	0x02
+#define BYTE_TYPE_AM		0x03
+#define BYTE_TYPE_DATA		0x04
+#define BYTE_TYPE_CRC		0x05
diff --git a/src/disc_86f.c b/src/disc_86f.c
index 8f15bd570..26fed85fc 100644
--- a/src/disc_86f.c
+++ b/src/disc_86f.c
@@ -4,41 +4,61 @@
 #include "ibm.h"
 #include "disc.h"
 #include "disc_86f.h"
+#include "fdc.h"
 #include "fdd.h"
 
 #define MAX_SECTORS 256
 
-int d86f_cur_track_pos[2] = {0, 0};
-
 enum
 {
         STATE_IDLE,
         STATE_READ_FIND_SECTOR,
         STATE_READ_SECTOR,
+	STATE_READ_SECTOR_CRC,
+	STATE_READ_SECTOR_GAP3,
         STATE_READ_FIND_FIRST_SECTOR,
         STATE_READ_FIRST_SECTOR,
+	STATE_READ_FIRST_SECTOR_CRC,
+	STATE_READ_FIRST_SECTOR_GAP3,
         STATE_READ_FIND_NEXT_SECTOR,
         STATE_READ_NEXT_SECTOR,
+	STATE_READ_NEXT_SECTOR_CRC,
+	STATE_READ_NEXT_SECTOR_GAP3,
         STATE_WRITE_FIND_SECTOR,
         STATE_WRITE_SECTOR,
+	STATE_WRITE_SECTOR_CRC,
+	STATE_WRITE_SECTOR_GAP3,
         STATE_READ_FIND_ADDRESS,
         STATE_READ_ADDRESS,
         STATE_FORMAT_FIND,
         STATE_FORMAT,
-	STATE_SEEK
+	STATE_SEEK,
 };
 
-static int d86f_state[2] = {0, 0};
-static int d86f_track[2] = {0, 0};
-static int d86f_side[2] = {0, 0};
+static uint16_t CRCTable[256];
+
 static int d86f_drive;
-static int d86f_sector[2] = {0, 0};
-static int d86f_n[2] = {0, 0};
-static uint8_t d86f_fill[2] = {0, 0};
-static int cur_sector[2];
-static int index_count[2];
-static int format_sector_count[2] = {0, 0};
         
+typedef union {
+	uint16_t word;
+	uint8_t bytes[2];
+} crc_t;
+
+typedef struct
+{
+	uint8_t c;
+	uint8_t h;
+	uint8_t r;
+	uint8_t n;
+} sector_id_fields_t;
+
+typedef union
+{
+	uint32_t dword;
+	uint8_t byte_array[4];
+	sector_id_fields_t id;
+} sector_id_t;
+
 static struct
 {
         FILE *f;
@@ -47,8 +67,25 @@ static struct
         uint8_t track_layout[2][25000];
         uint8_t track_flags;
 	uint8_t track_in_file;
-        uint32_t track_pos[256];
+        uint32_t track_offset[256];
 	uint32_t file_size;
+	sector_id_t format_sector_id;
+	sector_id_t rw_sector_id;
+	sector_id_t last_sector;
+	sector_id_t req_sector;
+	uint32_t index_count;
+	uint8_t state;
+	uint8_t fill;
+	uint16_t track_pos;
+	uint16_t datac;
+	uint16_t id_pos;
+	uint16_t section_pos;
+	crc_t calc_crc;
+	crc_t track_crc;
+	uint8_t track_byte;
+	uint8_t track_index;
+	uint8_t old_track_byte;
+	uint8_t old_track_index;
 } d86f[2];
 
 /* Needed for formatting! */
@@ -62,45 +99,6 @@ int d86f_realtrack(int drive, int track)
 
 void d86f_writeback(int drive, int track);
 
-static uint16_t CRCTable[256];
-
-/* 0 = MFM, 1 = FM, 2 = MFM perpendicular, 3 = reserved */
-/* 4 = ISO, 0 = IBM */
-int d86f_media_type = 0;
-
-/* Bits 0-3 define byte type, bit 5 defines whether it is a per-track (0) or per-sector (1) byte, if bit 7 is set, the byte is the index hole. */
-#define BYTE_GAP0		0x00
-#define BYTE_GAP1		0x10
-#define BYTE_GAP4		0x20
-#define BYTE_GAP2		0x40
-#define BYTE_GAP3		0x50
-#define BYTE_I_SYNC		0x01
-#define BYTE_ID_SYNC		0x41
-#define BYTE_DATA_SYNC		0x51
-#define BYTE_IAM_SYNC		0x02
-#define BYTE_IDAM_SYNC		0x42
-#define BYTE_DATAAM_SYNC	0x52
-#define BYTE_IAM		0x03
-#define BYTE_IDAM		0x43
-#define BYTE_DATAAM		0x53
-#define BYTE_ID			0x44
-#define BYTE_DATA		0x54
-#define BYTE_ID_CRC		0x45
-#define BYTE_DATA_CRC		0x55
-
-#define BYTE_INDEX_HOLE		0x80	/* 1 = index hole, 0 = regular byte */
-#define BYTE_IS_SECTOR		0x40	/* 1 = per-sector, 0 = per-track */
-#define BYTE_IS_POST_TRACK	0x20	/* 1 = after all sectors, 0 = before or during all sectors */
-#define BYTE_IS_DATA		0x10	/* 1 = data, 0 = id */
-#define BYTE_TYPE		0x0F	/* 5 = crc, 4 = data, 3 = address mark, 2 = address mark sync, 1 = sync, 0 = gap */
-
-#define BYTE_TYPE_GAP		0x00
-#define BYTE_TYPE_SYNC		0x01
-#define BYTE_TYPE_AM_SYNC	0x02
-#define BYTE_TYPE_AM		0x03
-#define BYTE_TYPE_DATA		0x04
-#define BYTE_TYPE_CRC		0x05
-
 static void d86f_setupcrc(uint16_t poly, uint16_t rvalue)
 {
 	int c = 256, bc;
@@ -127,16 +125,9 @@ static void d86f_setupcrc(uint16_t poly, uint16_t rvalue)
 	}
 }
 
-typedef union {
-	uint16_t word;
-	uint8_t bytes[2];
-} crc_t;
-
-static crc_t crc[2];
-
 static void d86f_calccrc(int drive, uint8_t byte)
 {
-	crc[drive].word = (crc[drive].word << 8) ^ CRCTable[(crc[drive].word >> 8)^byte];
+	d86f[drive].calc_crc.word = (d86f[drive].calc_crc.word << 8) ^ CRCTable[(d86f[drive].calc_crc.word >> 8)^byte];
 }
 
 void d86f_init()
@@ -212,9 +203,9 @@ void d86f_load(int drive, char *fn)
 	        fwriteprot[drive] = writeprot[drive];
 	}
 
-	fread(d86f[drive].track_pos, 1, 1024, d86f[drive].f);
+	fread(d86f[drive].track_offset, 1, 1024, d86f[drive].f);
 
-	if (!(d86f[drive].track_pos[0]))
+	if (!(d86f[drive].track_offset[0]))
 	{
 		/* File has no track 0, abort. */
 		pclog("86F: No Track 0\n");
@@ -223,7 +214,7 @@ void d86f_load(int drive, char *fn)
 	}
 
 	/* Load track 0 flags as default. */
-	fseek(d86f[drive].f, d86f[drive].track_pos[0], SEEK_SET);
+	fseek(d86f[drive].f, d86f[drive].track_offset[0], SEEK_SET);
 	fread(&(d86f[drive].track_flags), 1, 1, d86f[drive].f);
 
 	fseek(d86f[drive].f, 0, SEEK_END);
@@ -353,16 +344,13 @@ void d86f_seek(int drive, int track)
         if (d86f_is_40_track(drive) && fdd_doublestep_40(drive))
                 track /= 2;
 
-	memset(d86f[drive].track_layout[0], BYTE_GAP4, 25000);
-	memset(d86f[drive].track_data[0], 0xFF, 25000);
-
-	if (d86f_get_sides(drive) == 2)
+	for (side = 0; side < d86f_get_sides(drive); side++)
 	{
-		memset(d86f[drive].track_layout[1], BYTE_GAP4, 25000);
-		memset(d86f[drive].track_data[1], 0xFF, 25000);
+		memset(d86f[drive].track_layout[side], BYTE_GAP4, 25000);
+		memset(d86f[drive].track_data[side], 0xFF, 25000);
 	}
 
-	if (!(d86f[drive].track_pos[track]))
+	if (!(d86f[drive].track_offset[track]))
 	{
 		/* Track does not exist in the image, initialize it as unformatted. */
 		d86f[drive].track_in_file = 0;
@@ -372,56 +360,52 @@ void d86f_seek(int drive, int track)
 
 	d86f[drive].track_in_file = 1;
 
-	fseek(d86f[drive].f, d86f[drive].track_pos[track], SEEK_SET);
+	fseek(d86f[drive].f, d86f[drive].track_offset[track], SEEK_SET);
 
 	fread(&(d86f[drive].track_flags), 1, 1, d86f[drive].f);
 
-	fseek(d86f[drive].f, d86f[drive].track_pos[track] + 2, SEEK_SET);
-	fread(d86f[drive].track_layout[0], 1, d86f_get_raw_size(drive), d86f[drive].f);
-	fseek(d86f[drive].f, d86f[drive].track_pos[track] + 25002, SEEK_SET);
-	fread(d86f[drive].track_data[0], 1, d86f_get_raw_size(drive), d86f[drive].f);
-
-	if (d86f_get_sides(drive) == 2)
+	for (side = 0; side < d86f_get_sides(drive); side++)
 	{
-		fseek(d86f[drive].f, d86f[drive].track_pos[track] + 50002, SEEK_SET);
-		fread(d86f[drive].track_layout[1], 1, d86f_get_raw_size(drive), d86f[drive].f);
-		fseek(d86f[drive].f, d86f[drive].track_pos[track] + 75002, SEEK_SET);
-		fread(d86f[drive].track_data[1], 1, d86f_get_raw_size(drive), d86f[drive].f);
+		fseek(d86f[drive].f, d86f[drive].track_offset[track] + (side * 50000) + 2, SEEK_SET);
+		fread(d86f[drive].track_layout[side], 1, d86f_get_raw_size(drive), d86f[drive].f);
+		fseek(d86f[drive].f, d86f[drive].track_offset[track] + (side * 50000) + 25002, SEEK_SET);
+		fread(d86f[drive].track_data[side], 1, d86f_get_raw_size(drive), d86f[drive].f);
 	}
 }
 
 void d86f_writeback(int drive, int track)
 {
+	int side;
+
         if (!d86f[drive].f)
                 return;
                 
         if (!d86f[drive].track_in_file)
                 return; /*Should never happen*/
 
-	fseek(d86f[drive].f, d86f[drive].track_pos[track], SEEK_SET);
+	fseek(d86f[drive].f, 7, SEEK_SET);
+	fwrite(d86f[drive].track_offset, 1, 1024, d86f[drive].f);
 
+	fseek(d86f[drive].f, d86f[drive].track_offset[track], SEEK_SET);
 	fwrite(&(d86f[drive].track_flags), 1, 1, d86f[drive].f);
 
-	fseek(d86f[drive].f, d86f[drive].track_pos[track] + 2, SEEK_SET);
-	fwrite(d86f[drive].track_layout[0], 1, d86f_get_raw_size(drive), d86f[drive].f);
-	fseek(d86f[drive].f, d86f[drive].track_pos[track] + 25002, SEEK_SET);
-	fwrite(d86f[drive].track_data[0], 1, d86f_get_raw_size(drive), d86f[drive].f);
-
-	if (d86f_get_sides(drive) == 2)
+	for (side = 0; side < d86f_get_sides(drive); side++)
 	{
-		fseek(d86f[drive].f, d86f[drive].track_pos[track] + 50002, SEEK_SET);
-		fwrite(d86f[drive].track_layout[1], 1, d86f_get_raw_size(drive), d86f[drive].f);
-		fseek(d86f[drive].f, d86f[drive].track_pos[track] + 75002, SEEK_SET);
-		fwrite(d86f[drive].track_data[1], 1, d86f_get_raw_size(drive), d86f[drive].f);
+		fseek(d86f[drive].f, d86f[drive].track_offset[track] + (side * 50000) + 2, SEEK_SET);
+		fwrite(d86f[drive].track_layout[side], 1, d86f_get_raw_size(drive), d86f[drive].f);
+		fseek(d86f[drive].f, d86f[drive].track_offset[track] + (side * 50000) + 25002, SEEK_SET);
+		fwrite(d86f[drive].track_data[side], 1, d86f_get_raw_size(drive), d86f[drive].f);
 	}
+
+	// pclog("d86f_writeback(): %08X\n", d86f[drive].track_offset[track]);
 }
 
 void d86f_reset(int drive, int side)
 {
 	if (side == 0)
 	{
-		index_count[drive] = 0;
-		d86f_state[drive] = STATE_SEEK;
+		d86f[drive].index_count = 0;
+		d86f[drive].state = STATE_SEEK;
 	}
 }
 
@@ -460,74 +444,69 @@ void d86f_readsector(int drive, int sector, int track, int side, int rate, int s
 {
         // pclog("d86f_readsector: fdc_period=%i img_period=%i rate=%i sector=%i track=%i side=%i\n", fdc_get_bitcell_period(), get_bitcell_period(drive), rate, sector, track, side);
 
-        d86f_track[drive] = track;
-        d86f_side[drive]  = side;
+        d86f[drive].req_sector.id.c = track;
+        d86f[drive].req_sector.id.h = side;
 
 	if (side && (d86f_get_sides(drive) == 1))
 	{
 		fdc_notfound();
-		d86f_state[drive] = STATE_IDLE;
-		index_count[drive] = 0;
+		d86f[drive].state = STATE_IDLE;
+		d86f[drive].index_count = 0;
 		return;
 	}
 
         d86f_drive = drive;
-        d86f_sector[drive] = sector;
-	d86f_n[drive] = sector_size;
-	index_count[drive] = 0;
+        d86f[drive].req_sector.id.r = sector;
+	d86f[drive].req_sector.id.n = sector_size;
+	d86f[drive].index_count = 0;
         if (sector == SECTOR_FIRST)
-                d86f_state[drive] = STATE_READ_FIND_FIRST_SECTOR;
+                d86f[drive].state = STATE_READ_FIND_FIRST_SECTOR;
         else if (sector == SECTOR_NEXT)
-                d86f_state[drive] = STATE_READ_FIND_NEXT_SECTOR;
+                d86f[drive].state = STATE_READ_FIND_NEXT_SECTOR;
         else
-                d86f_state[drive] = STATE_READ_FIND_SECTOR;
+                d86f[drive].state = STATE_READ_FIND_SECTOR;
 }
 
 void d86f_writesector(int drive, int sector, int track, int side, int rate, int sector_size)
 {
-        // pclog("d86f_writesector: fdc_period=%i img_period=%i rate=%i\n", fdc_get_bitcell_period(), get_bitcell_period(drive), rate);
+        d86f[drive].req_sector.id.c = track;
+        d86f[drive].req_sector.id.h = side;
+        d86f[drive].req_sector.id.r = sector;
+	d86f[drive].req_sector.id.n = sector_size;
 
-        d86f_track[drive] = track;
-        d86f_side[drive]  = side;
+        // pclog("d86f_writesector: drive=%c: fdc_period=%i img_period=%i rate=%i chrn=%08X\n", drive + 0x41, fdc_get_bitcell_period(), get_bitcell_period(drive), rate, d86f[drive].req_sector.dword);
 
 	if (side && (d86f_get_sides(drive) == 1))
 	{
 		fdc_notfound();
-		d86f_state[drive] = STATE_IDLE;
-		index_count[drive] = 0;
+		d86f[drive].state = STATE_IDLE;
+		d86f[drive].index_count = 0;
 		return;
 	}
 
         d86f_drive = drive;
-        d86f_sector[drive] = sector;
-	d86f_n[drive] = sector_size;
-	index_count[drive] = 0;
-        d86f_state[drive] = STATE_WRITE_FIND_SECTOR;
+	d86f[drive].index_count = 0;
+        d86f[drive].state = STATE_WRITE_FIND_SECTOR;
 }
 
 void d86f_readaddress(int drive, int track, int side, int rate)
 {
         // pclog("d86f_readaddress: fdc_period=%i img_period=%i rate=%i track=%i side=%i\n", fdc_get_bitcell_period(), get_bitcell_period(drive), rate, track, side);
 
-        d86f_track[drive] = track;
-        d86f_side[drive]  = side;
+        d86f[drive].req_sector.id.c = track;
+        d86f[drive].req_sector.id.h = side;
 
 	if (side && (d86f_get_sides(drive) == 1))
 	{
 		fdc_notfound();
-		d86f_state[drive] = STATE_IDLE;
-		index_count[drive] = 0;
+		d86f[drive].state = STATE_IDLE;
+		d86f[drive].index_count = 0;
 		return;
 	}
 
         d86f_drive = drive;
-	index_count[drive] = 0;
-        d86f_state[drive] = STATE_READ_FIND_ADDRESS;
-}
-
-int d86f_is_iso(int drive)
-{
-	return 0;	/* Currently, we always use the IBM format, not ISO. */
+	d86f[drive].index_count = 0;
+        d86f[drive].state = STATE_READ_FIND_ADDRESS;
 }
 
 void d86f_prepare_track_layout(int drive, int side)
@@ -540,29 +519,24 @@ void d86f_prepare_track_layout(int drive, int side)
 	int real_gap1_len = d86f_is_mfm(drive) ? 50 : 26;
 	int real_gap2_len = (fdc_get_bit_rate() >= 1000) ? 41 : 22;
 	int real_gap3_len = fdc_get_gap();
+
 	memset(d86f[drive].track_layout[side], BYTE_GAP4, d86f_get_raw_size(drive));
 	i = 0;
-	if (!(d86f_is_iso(drive)))
+
+	memset(d86f[drive].track_layout[side] + i, BYTE_GAP0, real_gap0_len);
+	i += real_gap0_len;
+	memset(d86f[drive].track_layout[side] + i, BYTE_I_SYNC, sync_len);
+	i += sync_len;
+	if (d86f_is_mfm(drive))
 	{
-		memset(d86f[drive].track_layout[side] + i, BYTE_GAP0, real_gap0_len);
-		i += real_gap0_len;
-		memset(d86f[drive].track_layout[side] + i, BYTE_I_SYNC, sync_len);
-		i += sync_len;
-		if (d86f_is_mfm(drive))
-		{
-			memset(d86f[drive].track_layout[side] + i, BYTE_IAM_SYNC, 3);
-			i += 3;
-		}
-		memset(d86f[drive].track_layout[side] + i, BYTE_IAM, 1);
-		i++;
-		memset(d86f[drive].track_layout[side] + i, BYTE_GAP1, real_gap1_len);
-		i += real_gap1_len;
-	}
-	else
-	{
-		memset(d86f[drive].track_layout[side] + i, BYTE_GAP1, real_gap1_len);
-		i += real_gap1_len;
+		memset(d86f[drive].track_layout[side] + i, BYTE_IAM_SYNC, 3);
+		i += 3;
 	}
+	memset(d86f[drive].track_layout[side] + i, BYTE_IAM, 1);
+	i++;
+	memset(d86f[drive].track_layout[side] + i, BYTE_GAP1, real_gap1_len);
+	i += real_gap1_len;
+
 	d86f[drive].track_layout[side][0] |= BYTE_INDEX_HOLE;
 	for (j = 0; j < fdc_get_format_sectors(); j++)
 	{
@@ -602,28 +576,28 @@ void d86f_prepare_track_layout(int drive, int side)
 
 void d86f_format(int drive, int track, int side, int rate, uint8_t fill)
 {
-        d86f_track[drive] = track;
-        d86f_side[drive]  = side;
+        d86f[drive].req_sector.id.c = track;
+        d86f[drive].req_sector.id.h  = side;
 
 	if (side && (d86f_get_sides(drive) == 1))
 	{
 		fdc_notfound();
-		d86f_state[drive] = STATE_IDLE;
-		index_count[drive] = 0;
+		d86f[drive].state = STATE_IDLE;
+		d86f[drive].index_count = 0;
 		return;
 	}
 
 	if ((track < 0) || (track > 256))
 	{
 		fdc_writeprotect();
-		d86f_state[drive] = STATE_IDLE;
-		index_count[drive] = 0;
+		d86f[drive].state = STATE_IDLE;
+		d86f[drive].index_count = 0;
 		return;
 	}
 
         d86f_drive = drive;
-        d86f_fill[drive]  = fill;
-	index_count[drive] = 0;
+        d86f[drive].fill  = fill;
+	d86f[drive].index_count = 0;
 	/* Let's prepare the track space and layout before filling. */
 	d86f[drive].track_flags &= 0xc0;
 	d86f[drive].track_flags |= (fdd_getrpm(drive ^ fdd_swap) == 360) ? 0x20 : 0;
@@ -635,8 +609,8 @@ void d86f_format(int drive, int track, int side, int rate, uint8_t fill)
 	if (!d86f[drive].track_in_file)
 	{
 		/* Track is absent from the file, let's add it. */
-		d86f[drive].track_pos[track] = d86f[drive].file_size;
-		d86f[drive].file_size += (2 + 50000);
+		d86f[drive].track_offset[track] = d86f[drive].file_size;
+		d86f[drive].file_size += 50002;
 		if (d86f_get_sides(drive) == 2)
 		{
 			d86f[drive].file_size += 50000;
@@ -644,44 +618,31 @@ void d86f_format(int drive, int track, int side, int rate, uint8_t fill)
 		d86f[drive].track_in_file = 1;
 	}
 
-        d86f_state[drive] = STATE_FORMAT_FIND;
-	format_sector_count[drive] = 0;
+        d86f[drive].state = STATE_FORMAT_FIND;
 }
 
 void d86f_stop(int drive)
 {
-        d86f_state[drive] = STATE_IDLE;
+        d86f[drive].state = STATE_IDLE;
 }
 
 static void index_pulse(int drive)
 {
-	if (d86f_state[drive] != STATE_IDLE)  fdc_indexpulse();
+	if (d86f[drive].state != STATE_IDLE)  fdc_indexpulse();
 }
 
-typedef struct {
-	uint8_t c;
-	uint8_t h;
-	uint8_t r;
-	uint8_t n;
-} sector_data_t;
-
-sector_data_t last_sector[2];
-
 int d86f_match(int drive)
 {
 	int temp;
-	temp = (d86f_track[drive] == last_sector[drive].c);
-	temp = temp && (d86f_side[drive] == last_sector[drive].h);
-	temp = temp && (d86f_sector[drive] == last_sector[drive].r);
-	temp = temp && (d86f_n[drive] == last_sector[drive].n);
+	temp = (d86f[drive].req_sector.dword == d86f[drive].last_sector.dword);
 	return temp;
 }
 
 uint32_t d86f_get_data_len(int drive)
 {
-	if (d86f_n[drive])
+	if (d86f[drive].req_sector.id.n)
 	{
-		return (128 << ((uint32_t) d86f_n[drive]));
+		return (128 << ((uint32_t) d86f[drive].req_sector.id.n));
 	}
 	else
 	{
@@ -691,7 +652,7 @@ uint32_t d86f_get_data_len(int drive)
 		}
 		else
 		{
-			return (128 << ((uint32_t) d86f_n[drive]));
+			return (128 << ((uint32_t) d86f[drive].req_sector.id.n));
 		}
 	}
 }
@@ -714,495 +675,513 @@ int d86f_can_format(int drive)
 	return temp;
 }
 
-int d86f_find_state(int drive)
-{
-	int temp;
-	temp = (d86f_state[drive] == STATE_READ_FIND_SECTOR);
-	temp = temp || (d86f_state[drive] == STATE_READ_FIND_FIRST_SECTOR);
-	temp = temp || (d86f_state[drive] == STATE_READ_FIND_NEXT_SECTOR);
-	temp = temp || (d86f_state[drive] == STATE_WRITE_FIND_SECTOR);
-	temp = temp || (d86f_state[drive] == STATE_READ_FIND_ADDRESS);
-	temp = temp || (d86f_state[drive] == STATE_FORMAT_FIND);
-	return temp;
-}
-
 int d86f_find_state_nf(int drive)
 {
 	int temp;
-	temp = (d86f_state[drive] == STATE_READ_FIND_SECTOR);
-	temp = temp || (d86f_state[drive] == STATE_READ_FIND_FIRST_SECTOR);
-	temp = temp || (d86f_state[drive] == STATE_READ_FIND_NEXT_SECTOR);
-	temp = temp || (d86f_state[drive] == STATE_WRITE_FIND_SECTOR);
-	temp = temp || (d86f_state[drive] == STATE_READ_FIND_ADDRESS);
+	temp = (d86f[drive].state == STATE_READ_FIND_SECTOR);
+	temp = temp || (d86f[drive].state == STATE_READ_FIND_FIRST_SECTOR);
+	temp = temp || (d86f[drive].state == STATE_READ_FIND_NEXT_SECTOR);
+	temp = temp || (d86f[drive].state == STATE_WRITE_FIND_SECTOR);
+	temp = temp || (d86f[drive].state == STATE_READ_FIND_ADDRESS);
+	return temp;
+}
+
+int d86f_find_state(int drive)
+{
+	int temp;
+	temp = d86f_find_state_nf(drive);
+	temp = temp || (d86f[drive].state == STATE_FORMAT_FIND);
+	return temp;
+}
+
+int d86f_read_state_data(int drive)
+{
+	int temp;
+	temp = (d86f[drive].state == STATE_READ_SECTOR);
+	temp = temp || (d86f[drive].state == STATE_READ_FIRST_SECTOR);
+	temp = temp || (d86f[drive].state == STATE_READ_NEXT_SECTOR);
+	return temp;
+}
+
+int d86f_read_state_crc(int drive)
+{
+	int temp;
+	temp = (d86f[drive].state == STATE_READ_SECTOR_CRC);
+	temp = temp || (d86f[drive].state == STATE_READ_FIRST_SECTOR_CRC);
+	temp = temp || (d86f[drive].state == STATE_READ_NEXT_SECTOR_CRC);
+	return temp;
+}
+
+int d86f_read_state_gap3(int drive)
+{
+	int temp;
+	temp = (d86f[drive].state == STATE_READ_SECTOR_GAP3);
+	temp = temp || (d86f[drive].state == STATE_READ_FIRST_SECTOR_GAP3);
+	temp = temp || (d86f[drive].state == STATE_READ_NEXT_SECTOR_GAP3);
+	return temp;
+}
+
+int d86f_write_state(int drive)
+{
+	int temp;
+	temp = (d86f[drive].state == STATE_WRITE_SECTOR);
+	temp = temp || (d86f[drive].state == STATE_WRITE_SECTOR_CRC);
+	temp = temp || (d86f[drive].state == STATE_WRITE_SECTOR_GAP3);
 	return temp;
 }
 
 int d86f_read_state(int drive)
 {
 	int temp;
-	temp = (d86f_state[drive] == STATE_READ_SECTOR);
-	temp = temp || (d86f_state[drive] == STATE_READ_FIRST_SECTOR);
-	temp = temp || (d86f_state[drive] == STATE_READ_NEXT_SECTOR);
+	temp = d86f_read_state_data(drive);
+	temp = temp || d86f_read_state_crc(drive);
+	temp = temp || d86f_read_state_gap3(drive);
 	return temp;
 }
 
-int d86f_section_pos[2] = {0, 0};
-
-int data_pos[2] = {0, 0};
-
 int d86f_data_size(int drive)
 {
 	int temp;
-	temp = d86f_n[drive];
+	temp = d86f[drive].req_sector.id.n;
 	temp = 128 << temp;
 	return temp;
 }
 
-typedef union
+void d86f_poll_write_crc(int drive, int side)
 {
-	uint32_t dword;
-	uint8_t byte_array[4];
-} sector_id;
+	if (d86f[drive].state != STATE_FORMAT)  return;
+	d86f[drive].id_pos = d86f[drive].track_pos - d86f[drive].section_pos;
+	if (d86f[drive].id_pos)
+	{
+		d86f[drive].track_data[side][d86f[drive].track_pos] = d86f[drive].calc_crc.bytes[1];
+	}
+	else
+	{
+		d86f[drive].track_data[side][d86f[drive].track_pos] = d86f[drive].calc_crc.bytes[0];
+	}
+}
 
-sector_id format_sector_id[2];
-sector_id rw_sector_id[2];
+void d86f_poll_advancebyte(int drive, int side)
+{
+	d86f[drive].old_track_byte = d86f[drive].track_byte;
+	d86f[drive].old_track_index = d86f[drive].track_index;
 
-crc_t crc_bytes[2];
+	d86f[drive].track_pos++;
+	d86f[drive].track_pos %= d86f_get_raw_size(drive);
 
-static uint32_t datac[2] = {0, 0};
+	d86f[drive].track_byte = d86f[drive].track_layout[side][d86f[drive].track_pos] & ~BYTE_INDEX_HOLE;
+	d86f[drive].track_index = d86f[drive].track_layout[side][d86f[drive].track_pos] & BYTE_INDEX_HOLE;
+}
 
-void d86f_poll()
+void d86f_poll_reset(int drive, int side)
+{
+	d86f[drive].state = STATE_IDLE;
+	d86f[drive].index_count = 0;
+	d86f_poll_advancebyte(drive, side);
+}
+
+int d86f_poll_check_notfound(int drive)
+{
+	if (d86f[drive].index_count > 1)
+	{
+		/* The index hole has been hit twice and we're still in a find state.
+		   This means sector finding has failed for whatever reason.
+		   Abort with sector not found and set state to idle. */
+		// pclog("d86f_poll(): Sector not found (%i %i %i %i)\n", d86f[drive].req_sector.id.c, d86f[drive].req_sector.id.h, d86f[drive].req_sector.id.r, d86f[drive].req_sector.id.n);
+		fdc_notfound();
+		d86f[drive].state = STATE_IDLE;
+		d86f[drive].index_count = 0;
+		return 1;
+	}
+	else
+	{
+		return 0;
+	}
+}
+
+void d86f_poll_finish(int drive, int side)
+{
+	d86f_poll_reset(drive, side);
+	d86f_poll_advancebyte(drive, side);
+	d86f[drive].last_sector.dword = 0xFFFFFFFF;
+}
+
+void d86f_poll_readwrite(int drive, int side)
 {
         int data;
-	int drive = d86f_drive;
-	int side = d86f_side[drive];
-	int found_sector = 0;
-	int b = 0;
+	uint16_t max_len;
 
-	int cur_id_pos = 0;
-	int cur_gap3_pos = 0;
-
-	uint8_t track_byte = 0;
-	uint8_t track_index = 0;
-	uint8_t track_sector = 0;
-	uint8_t track_byte_type = 0;
-
-	uint8_t old_track_byte = 0;
-	uint8_t old_track_index = 0;
-	uint8_t old_track_sector = 0;
-	uint8_t old_track_byte_type = 0;
-
-	if ((d86f_state[drive] == STATE_SEEK) || (d86f_state[drive] == STATE_IDLE))
+	if (d86f_read_state_data(drive))
 	{
-		d86f_cur_track_pos[drive]++;
-		d86f_cur_track_pos[drive] %= d86f_get_raw_size(drive);
-		return;
-	}
-
-	if (d86f_state[drive] == STATE_FORMAT_FIND)
-	{
-		if (!(d86f_can_format(drive)))
+		max_len = d86f_data_size(drive);
+		if (d86f[drive].datac < d86f_get_data_len(drive))
 		{
-			if (d86f_can_read_address(drive))
+			fdc_data(d86f[drive].track_data[side][d86f[drive].track_pos]);
+		}
+		d86f_calccrc(drive, d86f[drive].track_data[side][d86f[drive].track_pos]);
+	}
+	else if (d86f[drive].state == STATE_WRITE_SECTOR)
+	{
+		max_len = d86f_data_size(drive);
+		if (d86f[drive].datac < d86f_get_data_len(drive))
+		{
+			data = fdc_getdata(d86f[drive].datac == ((128 << ((uint32_t) d86f[drive].last_sector.id.n)) - 1));
+       		        if (data == -1)
 			{
-				// pclog("d86f_poll(): Disk is write protected or attempting to format wrong number of sectors per track\n");
-				fdc_writeprotect();
+				data = 0;
+			}
+		}
+		else
+		{
+			data = 0;
+		}
+		if (!disable_write)
+		{
+			d86f[drive].track_data[side][d86f[drive].track_pos] = data;
+			d86f[drive].track_layout[side][d86f[drive].track_pos] = BYTE_DATA;
+			if (!d86f[drive].track_pos)  d86f[drive].track_layout[side][d86f[drive].track_pos] |= BYTE_INDEX_HOLE;
+		}
+		d86f_calccrc(drive, d86f[drive].track_data[side][d86f[drive].track_pos]);
+	}
+	else if (d86f_read_state_crc(drive))
+	{
+		max_len = 2;
+		d86f[drive].track_crc.bytes[d86f[drive].datac] = d86f[drive].track_data[side][d86f[drive].track_pos];
+	}
+	else if (d86f[drive].state == STATE_WRITE_SECTOR_CRC)
+	{
+		max_len = 2;
+		if (!disable_write)
+		{
+			d86f[drive].track_data[side][d86f[drive].track_pos] = d86f[drive].calc_crc.bytes[d86f[drive].datac];
+			d86f[drive].track_layout[side][d86f[drive].track_pos] = BYTE_DATA_CRC;
+			if (!d86f[drive].track_pos)  d86f[drive].track_layout[side][d86f[drive].track_pos] |= BYTE_INDEX_HOLE;
+		}
+	}
+	else if (d86f_read_state_gap3(drive))
+	{
+		max_len = fdc_get_gap();
+		if (d86f[drive].datac == (fdc_get_gap() - 1))
+		{
+			d86f_poll_finish(drive, side);
+			if (d86f[drive].track_crc.word != d86f[drive].calc_crc.word)
+			{
+				fdc_finishread();
+				fdc_datacrcerror();
 			}
 			else
 			{
-				// pclog("d86f_poll(): Unable to format at the requested density or bitcell period\n");
-				fdc_notfound();
+        	               	fdc_sector_finishread();
 			}
-                        d86f_state[drive] = STATE_IDLE;
-			index_count[drive] = 0;
-			d86f_cur_track_pos[drive]++;
-			d86f_cur_track_pos[drive] %= d86f_get_raw_size(drive);
 			return;
 		}
 	}
-
-	track_byte = d86f[drive].track_layout[side][d86f_cur_track_pos[drive]];
-	track_index = track_byte & BYTE_INDEX_HOLE;
-	track_sector = track_byte & BYTE_IS_SECTOR;
-	track_byte_type = track_byte & BYTE_TYPE;
-
-	if (track_index)
+	else if (d86f[drive].state == STATE_WRITE_SECTOR_GAP3)
 	{
-		if (d86f_state[drive] != STATE_IDLE)
+		max_len = fdc_get_gap();
+		if (d86f[drive].datac == (fdc_get_gap() - 1))
 		{
-			index_pulse(drive);
-			index_count[drive]++;
+    		        if (!disable_write)  d86f_writeback(drive, d86f[drive].req_sector.id.c);
+			d86f_poll_finish(drive, side);
+       	        	fdc_sector_finishread(drive);
+			return;
 		}
-		if (d86f_state[drive] == STATE_FORMAT)
+		else
 		{
-			// pclog("Index hole hit again, format finished\n");
-              		d86f_state[drive] = STATE_IDLE;
-   		        if (!disable_write)  d86f_writeback(drive, d86f_track[drive]);
-                        fdc_sector_finishread(drive);
-		}
-		if ((d86f_state[drive] == STATE_FORMAT_FIND) && d86f_can_read_address(drive))
-		{
-			// pclog("Index hole hit, formatting track...\n");
-			d86f_state[drive] = STATE_FORMAT;
-		}
-	}
-
-	if (d86f_read_state(drive) || (d86f_state[drive] == STATE_WRITE_SECTOR))
-	{
-		/* For the first (data size) bytes, send to the FDC or receive from the FDC and write. */
-		if (d86f_read_state(drive) && (datac[drive] < d86f_data_size(drive)))
-		{
-			if (fdc_data(d86f[drive].track_data[side][d86f_cur_track_pos[drive]]))
-			{
-				/* Data failed to be sent to the FDC, abort. */
-				// pclog("d86f_poll(): Unable to send further data to the FDC\n");
-				d86f_state[drive] = STATE_IDLE;
-				index_count[drive] = 0;
-				d86f_cur_track_pos[drive]++;
-				d86f_cur_track_pos[drive] %= d86f_get_raw_size(drive);
-				return;
-			}
-		}
-
-		if ((d86f_state[drive] == STATE_WRITE_SECTOR) && (datac[drive] < d86f_data_size(drive)))
-		{
-       		       	data = fdc_getdata(datac[drive] == ((128 << ((uint32_t) last_sector[drive].n)) - 1));
-	       	        if (data == -1)
-			{
-				/* Data failed to be sent from the FDC, abort. */
-				// pclog("d86f_poll(): Unable to receive further data from the FDC\n");
-				d86f_state[drive] = STATE_IDLE;
-				index_count[drive] = 0;
-				d86f_cur_track_pos[drive]++;
-				d86f_cur_track_pos[drive] %= d86f_get_raw_size(drive);
-				return;
-			}
 			if (!disable_write)
 			{
-				d86f[drive].track_data[side][d86f_cur_track_pos[drive]] = data;
-				d86f[drive].track_layout[side][d86f_cur_track_pos[drive]] = BYTE_DATA;
-				if (!d86f_cur_track_pos[drive])  d86f[drive].track_layout[side][d86f_cur_track_pos[drive]] |= BYTE_INDEX_HOLE;
+				d86f[drive].track_data[side][d86f[drive].track_pos] = fdc_is_mfm() ? 0x4E : 0xFF;
+				d86f[drive].track_layout[side][d86f[drive].track_pos] = BYTE_GAP3;
+				if (!d86f[drive].track_pos)  d86f[drive].track_layout[side][d86f[drive].track_pos] |= BYTE_INDEX_HOLE;
 			}
 		}
-
-		if (datac[drive] < d86f_data_size(drive))
-		{
-			d86f_calccrc(drive, d86f[drive].track_data[side][d86f_cur_track_pos[drive]]);
-		}
-
-		if (datac[drive] == d86f_data_size(drive))
-		{
-			if (d86f_read_state(drive))
-			{
-				crc_bytes[drive].bytes[0] = d86f[drive].track_data[side][d86f_cur_track_pos[drive]];
-			}
-
-			if (d86f_state[drive] == STATE_WRITE_SECTOR)
-			{
-				if (!disable_write)
-				{
-					d86f[drive].track_data[side][d86f_cur_track_pos[drive]] = crc[drive].bytes[0];
-					d86f[drive].track_layout[side][d86f_cur_track_pos[drive]] = BYTE_DATA_CRC;
-					if (!d86f_cur_track_pos[drive])  d86f[drive].track_layout[side][d86f_cur_track_pos[drive]] |= BYTE_INDEX_HOLE;
-				}
-			}
-		}
-
-		if (datac[drive] == (d86f_data_size(drive) + 1))
-		{
-			if (d86f_read_state(drive))
-			{
-				crc_bytes[drive].bytes[1] = d86f[drive].track_data[side][d86f_cur_track_pos[drive]];
-			}
-
-			if (d86f_state[drive] == STATE_WRITE_SECTOR)
-			{
-				if (!disable_write)
-				{
-					d86f[drive].track_data[side][d86f_cur_track_pos[drive]] = crc[drive].bytes[1];
-					d86f[drive].track_layout[side][d86f_cur_track_pos[drive]] = BYTE_DATA_CRC;
-					if (!d86f_cur_track_pos[drive])  d86f[drive].track_layout[side][d86f_cur_track_pos[drive]] |= BYTE_INDEX_HOLE;
-				}
-			}
-		}
-
-		if ((datac[drive] > (d86f_data_size(drive) + 1)) && (datac[drive] <= (d86f_data_size(drive) + fdc_get_gap() + 1)) && (d86f_state[drive] == STATE_WRITE_SECTOR) && !disable_write)
-		{
-			d86f[drive].track_data[side][d86f_cur_track_pos[drive]] = fdc_is_mfm() ? 0x4E : 0xFF;
-			d86f[drive].track_layout[side][d86f_cur_track_pos[drive]] = BYTE_GAP3;
-			if (!d86f_cur_track_pos[drive])  d86f[drive].track_layout[side][d86f_cur_track_pos[drive]] |= BYTE_INDEX_HOLE;
-		}
-
-		/* At the last data of the FDC-specified gap length, compare the calculated CRC with the one we read and error out if mismatch,  otherwise finish with success. */
-		if (datac[drive] == (d86f_data_size(drive) + fdc_get_gap() + 1))
-		{
-			if (d86f_read_state(drive))
-			{
-	       	                d86f_state[drive] = STATE_IDLE;
-				if (crc_bytes[drive].word != crc[drive].word)
-				{
-					// pclog("d86f_poll(): Data CRC error (%04X, %04X) (%i) (%04X)\n", crc_bytes[drive].word, crc[drive].word, d86f_data_size(drive), d86f_cur_track_pos[drive] - fdc_get_gap() - 1);
-					fdc_finishread();
-					fdc_datacrcerror();
-				}
-				else
-				{
-	        	               	fdc_sector_finishread();
-				}
-				index_count[drive] = 0;
-				d86f_cur_track_pos[drive]++;
-				d86f_cur_track_pos[drive] %= d86f_get_raw_size(drive);
-				return;
-			}
-			if (d86f_state[drive] == STATE_WRITE_SECTOR)
-			{
-               		        d86f_state[drive] = STATE_IDLE;
-	    		        if (!disable_write)  d86f_writeback(drive, d86f_track[drive]);
-        	        	fdc_sector_finishread(drive);
-				index_count[drive] = 0;
-				d86f_cur_track_pos[drive]++;
-				d86f_cur_track_pos[drive] %= d86f_get_raw_size(drive);
-				return;
-			}
-		}
-
-		datac[drive]++;
 	}
 
-	switch(track_byte & ~BYTE_INDEX_HOLE)
+	d86f[drive].datac++;
+	d86f_poll_advancebyte(drive, side);
+
+	if (d86f[drive].datac >= max_len)
+	{
+		d86f[drive].datac = 0;
+		d86f[drive].state++;
+	}
+}
+
+void d86f_poll_find_nf(int drive, int side)
+{
+	if (d86f[drive].track_index)
+	{
+		// pclog("d86f_poll_find_nf(): Index pulse\n");
+		index_pulse(drive);
+		d86f[drive].index_count++;
+	}
+
+	switch(d86f[drive].track_byte)
+	{
+		case BYTE_IDAM:
+			d86f[drive].calc_crc.word = fdc_is_mfm() ? 0xcdb4 : 0xffff;
+			// pclog("CRC reset: %02X\n", d86f[drive].track_byte);
+			d86f_calccrc(drive, d86f[drive].track_data[side][d86f[drive].track_pos]);
+			break;
+
+		case BYTE_DATAAM:
+			d86f[drive].calc_crc.word = fdc_is_mfm() ? 0xcdb4 : 0xffff;
+			// pclog("CRC reset: %02X\n", d86f[drive].track_byte);
+
+			if ((d86f[drive].state &= STATE_WRITE_FIND_SECTOR) && d86f_match(drive) && d86f_can_read_address(drive))
+			{
+				d86f[drive].track_data[side][d86f[drive].track_pos] = 0xFB;
+			}
+			d86f_calccrc(drive, d86f[drive].track_data[side][d86f[drive].track_pos]);
+			break;
+		case BYTE_ID:
+			d86f[drive].id_pos = d86f[drive].track_pos - d86f[drive].section_pos;
+			d86f[drive].rw_sector_id.byte_array[d86f[drive].id_pos] = d86f[drive].track_data[side][d86f[drive].track_pos];
+			d86f_calccrc(drive, d86f[drive].track_data[side][d86f[drive].track_pos]);
+			break;
+		case BYTE_ID_CRC:
+			d86f[drive].id_pos = d86f[drive].track_pos - d86f[drive].section_pos;
+			d86f[drive].track_crc.bytes[d86f[drive].id_pos] = d86f[drive].track_data[side][d86f[drive].track_pos];
+			break;
+	}
+
+	d86f_poll_advancebyte(drive, side);
+
+	if (d86f_poll_check_notfound(drive))  return;
+
+	if (d86f[drive].track_byte != d86f[drive].old_track_byte)
+	{
+		d86f[drive].section_pos = d86f[drive].track_pos;
+
+		switch(d86f[drive].track_byte)
+		{
+			case BYTE_GAP2:
+				if (d86f_can_read_address(drive))
+				{
+					if (d86f_match(drive) || (d86f[drive].state == STATE_READ_FIND_ADDRESS))
+					{
+						if (d86f[drive].track_crc.word != d86f[drive].calc_crc.word)
+						{
+							// pclog("d86f_poll(): Header CRC error (%i %i %i %i)\n", d86f[drive].req_sector.id.c, d86f[drive].req_sector.id.h, d86f[drive].req_sector.id.r, d86f[drive].req_sector.id.n);
+							fdc_finishread();
+							fdc_headercrcerror();
+							d86f[drive].state = STATE_IDLE;
+							d86f[drive].index_count = 0;
+							return;
+						}
+					}
+				}
+
+				d86f[drive].last_sector.dword = d86f[drive].rw_sector_id.dword;
+				// pclog("Read sector ID in find state: %i %i %i %i (sought: %i, %i, %i, %i)\n", d86f[drive].last_sector.id.c, d86f[drive].last_sector.id.h, d86f[drive].last_sector.id.r, d86f[drive].last_sector.id.n, d86f[drive].req_sector.id.c, d86f[drive].req_sector.id.h, d86f[drive].req_sector.id.r, d86f[drive].req_sector.id.n);
+
+				if ((d86f[drive].state == STATE_READ_FIND_ADDRESS) && d86f_can_read_address(drive))
+				{
+					// pclog("Reading sector ID...\n");
+					fdc_sectorid(d86f[drive].last_sector.id.c, d86f[drive].last_sector.id.h, d86f[drive].last_sector.id.r, d86f[drive].last_sector.id.n, 0, 0);
+					d86f[drive].state = STATE_IDLE;
+					d86f[drive].index_count = 0;
+				}
+				break;
+			case BYTE_DATA:
+				d86f[drive].datac = 0;
+				switch (d86f[drive].state)
+				{
+					case STATE_READ_FIND_SECTOR:
+					case STATE_WRITE_FIND_SECTOR:
+						if (d86f_match(drive) && d86f_can_read_address(drive))
+						{
+							d86f[drive].state++;
+						}
+						break;
+					case STATE_READ_FIND_FIRST_SECTOR:
+					case STATE_READ_FIND_NEXT_SECTOR:
+						if (d86f_can_read_address(drive))
+						{
+							d86f[drive].state++;
+						}
+						break;
+				}
+				break;
+		}
+	}
+}
+
+void d86f_poll_find_format(int drive, int side)
+{
+	if (!(d86f_can_format(drive)))
+	{
+		if (d86f_can_read_address(drive))
+		{
+			// pclog("d86f_poll(): Disk is write protected or attempting to format wrong number of sectors per track\n");
+			fdc_writeprotect();
+		}
+		else
+		{
+			// pclog("d86f_poll(): Unable to format at the requested density or bitcell period\n");
+			fdc_notfound();
+		}
+		d86f_poll_reset(drive, side);
+		d86f_poll_advancebyte(drive, side);
+		return;
+	}
+
+	if (d86f[drive].track_index && d86f_can_read_address(drive))
+	{
+		// pclog("Index hole hit, formatting track...\n");
+		d86f[drive].state = STATE_FORMAT;
+		d86f_poll_advancebyte(drive, side);
+		return;
+	}
+
+	d86f_poll_advancebyte(drive, side);
+
+	if (d86f_poll_check_notfound(drive))  return;
+}
+
+void d86f_poll_format(int drive, int side)
+{
+        int data;
+
+	if (d86f[drive].track_index)
+	{
+		// pclog("Index hole hit again, format finished\n");
+		d86f[drive].state = STATE_IDLE;
+		if (!disable_write)  d86f_writeback(drive, d86f[drive].req_sector.id.c);
+		fdc_sector_finishread(drive);
+		d86f[drive].index_count = 0;
+		d86f_poll_advancebyte(drive, side);
+		return;
+	}
+
+	switch(d86f[drive].track_byte)
 	{
 		case BYTE_GAP0:
 		case BYTE_GAP1:
 		case BYTE_GAP2:
 		case BYTE_GAP3:
 		case BYTE_GAP4:
-			if ((d86f_state[drive] == STATE_FORMAT) && !disable_write)  d86f[drive].track_data[side][d86f_cur_track_pos[drive]] = fdc_is_mfm() ? 0x4E : 0xFF;
+			if (!disable_write)  d86f[drive].track_data[side][d86f[drive].track_pos] = fdc_is_mfm() ? 0x4E : 0xFF;
 			break;
 		case BYTE_IAM_SYNC:
-			if ((d86f_state[drive] == STATE_FORMAT) && !disable_write)  d86f[drive].track_data[side][d86f_cur_track_pos[drive]] = 0xC2;
+			if (!disable_write)  d86f[drive].track_data[side][d86f[drive].track_pos] = 0xC2;
 			break;
 		case BYTE_IAM:
-			if ((d86f_state[drive] == STATE_FORMAT) && !disable_write)  d86f[drive].track_data[side][d86f_cur_track_pos[drive]] = 0xFC;
+			if (!disable_write)  d86f[drive].track_data[side][d86f[drive].track_pos] = 0xFC;
 			break;
 		case BYTE_ID_SYNC:
-			if (d86f_state[drive] == STATE_FORMAT)
+			d86f[drive].id_pos = d86f[drive].track_pos - d86f[drive].section_pos;
+			if (!disable_write)  d86f[drive].track_data[side][d86f[drive].track_pos] = 0;
+			if (d86f[drive].id_pos > 3)  break;
+               		data = fdc_getdata(0);
+       	        	if ((data == -1) && (d86f[drive].id_pos < 3))
 			{
-				cur_id_pos = d86f_cur_track_pos[drive] - d86f_section_pos[drive];
-				if (!disable_write)  d86f[drive].track_data[side][d86f_cur_track_pos[drive]] = 0;
-				if (cur_id_pos > 3)  break;
-                		data = fdc_getdata(0);
-        	        	if ((data == -1) && (cur_id_pos < 3))
-				{
-					/* Data failed to be sent from the FDC, abort. */
-					// pclog("d86f_poll(): Unable to receive further data from the FDC\n");
-					d86f_state[drive] = STATE_IDLE;
-					index_count[drive] = 0;
-					d86f_cur_track_pos[drive]++;
-					d86f_cur_track_pos[drive] %= d86f_get_raw_size(drive);
-					return;
-				}
-				format_sector_id[drive].byte_array[cur_id_pos] = data & 0xff;
-				d86f_calccrc(drive, format_sector_id[drive].byte_array[cur_id_pos]);
-				// pclog("format_sector_id[%i] = %i\n", cur_id_pos, format_sector_id[drive].byte_array[cur_id_pos]);
-        	        	if (cur_id_pos == 3)
-				{
-					fdc_stop_id_request();
-					// pclog("Formatting sector: %08X...\n", format_sector_id[drive].dword);
-				}
+				data = 0;
+			}
+			d86f[drive].format_sector_id.byte_array[d86f[drive].id_pos] = data & 0xff;
+			d86f_calccrc(drive, d86f[drive].format_sector_id.byte_array[d86f[drive].id_pos]);
+			// pclog("format_sector_id[%i] = %i\n", cur_id_pos, d86f[drive].format_sector_id.byte_array[d86f[drive].id_pos]);
+       	        	if (d86f[drive].id_pos == 3)
+			{
+				fdc_stop_id_request();
+				// pclog("Formatting sector: %08X...\n", d86f[drive].format_sector_id.dword);
 			}
 			break;
 		case BYTE_I_SYNC:
 		case BYTE_DATA_SYNC:
-			if ((d86f_state[drive] == STATE_FORMAT) && !disable_write)  d86f[drive].track_data[side][d86f_cur_track_pos[drive]] = 0;
+			if (!disable_write)  d86f[drive].track_data[side][d86f[drive].track_pos] = 0;
 			break;
 		case BYTE_IDAM_SYNC:
 		case BYTE_DATAAM_SYNC:
-			if ((d86f_state[drive] == STATE_FORMAT) && !disable_write)  d86f[drive].track_data[side][d86f_cur_track_pos[drive]] = 0xA1;
+			if (!disable_write)  d86f[drive].track_data[side][d86f[drive].track_pos] = 0xA1;
 			break;
 		case BYTE_IDAM:
 		case BYTE_DATAAM:
-			if (d86f_find_state_nf(drive) || (d86f_state[drive] == STATE_FORMAT))  crc[drive].word = fdc_is_mfm() ? 0xcdb4 : 0xffff;
-			if ((d86f_state[drive] == STATE_FORMAT) && !disable_write)  d86f[drive].track_data[side][d86f_cur_track_pos[drive]] = (track_byte == BYTE_IDAM) ? 0xFE : 0xFB;
-			if (d86f_state[drive] == STATE_FORMAT)  d86f_calccrc(drive, d86f[drive].track_data[side][d86f_cur_track_pos[drive]]);
-			if (d86f_find_state_nf(drive) && ((d86f_state[drive] != STATE_WRITE_FIND_SECTOR) || ((track_byte & ~BYTE_INDEX_HOLE) == BYTE_IDAM)))
-			{
-				d86f_calccrc(drive, d86f[drive].track_data[side][d86f_cur_track_pos[drive]]);
-			}
+			d86f[drive].calc_crc.word = fdc_is_mfm() ? 0xcdb4 : 0xffff;
+			// pclog("CRC reset: %02X\n", d86f[drive].track_byte);
+
+			if (!disable_write)  d86f[drive].track_data[side][d86f[drive].track_pos] = (d86f[drive].track_byte == BYTE_IDAM) ? 0xFE : 0xFB;
+			d86f_calccrc(drive, d86f[drive].track_data[side][d86f[drive].track_pos]);
 			break;
 		case BYTE_ID:
-			cur_id_pos = d86f_cur_track_pos[drive] - d86f_section_pos[drive];
-			if ((d86f_state[drive] == STATE_FORMAT) && !disable_write)
+			d86f[drive].id_pos = d86f[drive].track_pos - d86f[drive].section_pos;
+			if (!disable_write)
 			{
-				d86f[drive].track_data[side][d86f_cur_track_pos[drive]] = format_sector_id[drive].byte_array[cur_id_pos];
-			}
-			if (d86f_find_state_nf(drive))
-			{
-				rw_sector_id[drive].byte_array[cur_id_pos] = d86f[drive].track_data[side][d86f_cur_track_pos[drive]];
-			}
-			if ((d86f_state[drive] == STATE_FORMAT) || d86f_find_state_nf(drive))
-			{
-				d86f_calccrc(drive, d86f[drive].track_data[side][d86f_cur_track_pos[drive]]);
+				d86f[drive].track_data[side][d86f[drive].track_pos] = d86f[drive].format_sector_id.byte_array[d86f[drive].id_pos];
 			}
+			d86f_calccrc(drive, d86f[drive].track_data[side][d86f[drive].track_pos]);
 			break;
 		case BYTE_DATA:
-			if ((d86f_state[drive] == STATE_FORMAT) && !disable_write)
+			if (!disable_write)
 			{
-				d86f[drive].track_data[side][d86f_cur_track_pos[drive]] = d86f_fill[drive];
-				d86f_calccrc(drive, d86f_fill[drive]);
+				d86f[drive].track_data[side][d86f[drive].track_pos] = d86f[drive].fill;
+				d86f_calccrc(drive, d86f[drive].fill);
 			}
 			break;
 		case BYTE_ID_CRC:
-			cur_id_pos = d86f_cur_track_pos[drive] - d86f_section_pos[drive];
-			if (d86f_find_state_nf(drive))
-			{
-				crc_bytes[drive].bytes[0] = d86f[drive].track_data[side][(d86f_cur_track_pos[drive] - 1) % d86f_get_raw_size(drive)];
-				crc_bytes[drive].bytes[1] = d86f[drive].track_data[side][d86f_cur_track_pos[drive]];
-			}
-			if ((d86f_state[drive] == STATE_FORMAT) && !disable_write)
-			{
-				if (cur_id_pos)
-				{
-					d86f[drive].track_data[side][d86f_cur_track_pos[drive]] = crc[drive].bytes[1];
-				}
-				else
-				{
-					d86f[drive].track_data[side][d86f_cur_track_pos[drive]] = crc[drive].bytes[0];
-				}
-			}
+			d86f_poll_write_crc(drive, side);
 			break;
 		case BYTE_DATA_CRC:
-			cur_id_pos = d86f_cur_track_pos[drive] - d86f_section_pos[drive];
-			if ((d86f_state[drive] == STATE_FORMAT) && !disable_write)
-			{
-				if (cur_id_pos)
-				{
-					d86f[drive].track_data[side][d86f_cur_track_pos[drive]] = crc[drive].bytes[1];
-				}
-				else
-				{
-					d86f[drive].track_data[side][d86f_cur_track_pos[drive]] = crc[drive].bytes[0];
-				}
-			}
+			d86f[drive].id_pos = d86f[drive].track_pos - d86f[drive].section_pos;
+			d86f_poll_write_crc(drive, side);
 			break;
 	}
 
-	old_track_byte = track_byte;
-	old_track_index = track_index;
-	old_track_sector = track_sector;
-	old_track_byte_type = track_byte_type;
+	d86f_poll_advancebyte(drive, side);
 
-	d86f_cur_track_pos[drive]++;
-	d86f_cur_track_pos[drive] %= d86f_get_raw_size(drive);
-
-	track_byte = d86f[drive].track_layout[side][d86f_cur_track_pos[drive]];
-	track_index = track_byte & BYTE_INDEX_HOLE;
-	track_sector = track_byte & BYTE_IS_SECTOR;
-	track_byte_type = track_byte & BYTE_TYPE;
-
-	if ((d86f_state[drive] != STATE_IDLE) && (d86f_state[drive] != STATE_SEEK))
+	if (d86f[drive].track_byte != d86f[drive].old_track_byte)
 	{
-		if (index_count[drive] > 1)
-		{
-			if (d86f_find_state(drive))
-			{
-				/* The index hole has been hit twice and we're still in a find state.
-				   This means sector finding has failed for whatever reason.
-				   Abort with sector not found and set state to idle. */
-				// pclog("d86f_poll(): Sector not found (%i %i %i %i)\n", d86f_track[drive], d86f_side[drive], d86f_sector[drive], d86f_n[drive]);
-				fdc_notfound();
-				d86f_state[drive] = STATE_IDLE;
-				index_count[drive] = 0;
-				return;
-			}
-		}
-	}
+		// pclog("Track byte: %02X, old: %02X\n", track_byte, old_track_byte);
+		d86f[drive].section_pos = d86f[drive].track_pos;
 
-	if ((d86f_state[drive] == STATE_IDLE) || (d86f_state[drive] == STATE_SEEK))  return;
-
-	if (track_byte != old_track_byte)
-	{
-		// if (d86f_state[drive] == STATE_FORMAT)  pclog("Track byte: %02X, old: %02X\n", track_byte, old_track_byte);
-		d86f_section_pos[drive] = d86f_cur_track_pos[drive];
-
-		switch(track_byte & ~BYTE_INDEX_HOLE)
+		switch(d86f[drive].track_byte & ~BYTE_INDEX_HOLE)
 		{
 			case BYTE_ID_SYNC:
-				if (d86f_state[drive] == STATE_FORMAT)
-				{
-					// pclog("Requesting next sector ID...\n");
-					fdc_request_next_sector_id();
-				}
+				// pclog("Requesting next sector ID...\n");
+				fdc_request_next_sector_id();
 				break;
 			case BYTE_GAP2:
-				if (d86f_find_state_nf(drive))
-				{
-					if (d86f_match(drive) && d86f_can_read_address(drive))
-					{
-						if (crc_bytes[drive].word != crc[drive].word)
-						{
-							// pclog("d86f_poll(): Header CRC error\n");
-							fdc_finishread();
-							fdc_headercrcerror();
-							d86f_state[drive] = STATE_IDLE;
-							index_count[drive] = 0;
-							return;
-						}
-					}
-
-					last_sector[drive].c = rw_sector_id[drive].byte_array[0];
-					last_sector[drive].h = rw_sector_id[drive].byte_array[1];
-					last_sector[drive].r = rw_sector_id[drive].byte_array[2];
-					last_sector[drive].n = rw_sector_id[drive].byte_array[3];
-					// pclog("Read sector ID in find state: %i %i %i %i (sought: %i, %i, %i, %i)\n", last_sector[drive].c, last_sector[drive].h, last_sector[drive].r, last_sector[drive].n, d86f_track[drive], d86f_side[drive], d86f_sector[drive], d86f_n[drive]);
-				}
-
-				if (d86f_state[drive] == STATE_FORMAT)
-				{
-					last_sector[drive].c = format_sector_id[drive].byte_array[0];
-					last_sector[drive].h = format_sector_id[drive].byte_array[1];
-					last_sector[drive].r = format_sector_id[drive].byte_array[2];
-					last_sector[drive].n = format_sector_id[drive].byte_array[3];
-				}
-
-				if ((d86f_state[drive] == STATE_READ_FIND_ADDRESS) && d86f_can_read_address(drive))
-				{
-					// pclog("Reading sector ID...\n");
-					fdc_sectorid(last_sector[drive].c, last_sector[drive].h, last_sector[drive].r, last_sector[drive].n, 0, 0);
-					d86f_state[drive] = STATE_IDLE;
-				}
+				d86f[drive].last_sector.dword = d86f[drive].format_sector_id.dword;
 				break;
 			case BYTE_DATA:
-				if (d86f_read_state(drive) || (d86f_state[drive] == STATE_WRITE_SECTOR))  return;
-				datac[drive] = 0;
-				switch (d86f_state[drive])
-				{
-					case STATE_READ_FIND_SECTOR:
-						if (d86f_match(drive) && d86f_can_read_address(drive))
-						{
-							d86f_state[drive] = STATE_READ_SECTOR;
-						}
-						break;
-					case STATE_READ_FIND_FIRST_SECTOR:
-						if ((cur_sector[drive] == 0) && d86f_can_read_address(drive))  d86f_state[drive] = STATE_READ_FIRST_SECTOR;
-						break;
-					case STATE_READ_FIND_NEXT_SECTOR:
-						if (d86f_can_read_address(drive))  d86f_state[drive] = STATE_READ_NEXT_SECTOR;
-						break;
-					case STATE_WRITE_FIND_SECTOR:
-						if (d86f_match(drive) && d86f_can_read_address(drive))
-						{
-							if (!disable_write)
-							{
-								d86f[drive].track_data[side][(d86f_cur_track_pos[drive] - 1) % d86f_get_raw_size(drive)] = 0xFB;
-							}
-							d86f_calccrc(drive, d86f[drive].track_data[side][(d86f_cur_track_pos[drive] - 1) % d86f_get_raw_size(drive)]);
-
-							d86f_state[drive] = STATE_WRITE_SECTOR;
-							// pclog("Write start: %i %i %i %i, data size: %i (%i)\n", last_sector[drive].c, last_sector[drive].h, last_sector[drive].r, last_sector[drive].n, d86f_data_size(drive), d86f_n[drive]);
-						}
-						break;
-				}
+				d86f[drive].datac = 0;
 				break;
 		}
 	}
 }
+
+void d86f_poll()
+{
+	int drive = d86f_drive;
+	int side = d86f[drive].req_sector.id.h;
+
+	if (d86f[drive].state == STATE_FORMAT)
+	{
+		d86f_poll_format(drive, side);
+		return;
+	}
+
+	if (d86f[drive].state == STATE_FORMAT_FIND)
+	{
+		d86f_poll_find_format(drive, side);
+		return;
+	}
+
+	if (d86f_find_state_nf(drive))
+	{
+		d86f_poll_find_nf(drive, side);
+		return;
+	}
+
+	if (d86f_read_state(drive) || d86f_write_state(drive))
+	{
+		d86f_poll_readwrite(drive, side);
+		return;
+	}
+
+	if ((d86f[drive].state == STATE_SEEK) || (d86f[drive].state == STATE_IDLE))
+	{
+		d86f_poll_advancebyte(drive, side);
+		return;
+	}
+}
diff --git a/src/disc_sector_86box.c b/src/disc_sector_86box.c
index fd577af7d..2e6362689 100644
--- a/src/disc_sector_86box.c
+++ b/src/disc_sector_86box.c
@@ -4,6 +4,7 @@
 #include "ibm.h"
 #include "disc.h"
 #include "disc_sector.h"
+#include "fdc.h"
 #include "fdd.h"
 
 /*Handling for 'sector based' image formats (like .IMG) as opposed to 'stream based' formats (eg .FDI)*/
@@ -170,39 +171,6 @@ int id_positions[2][2][MAX_SECTORS];
 /* 4 = ISO, 0 = IBM */
 int media_type = 0;
 
-/* Bits 0-3 define byte type, bit 5 defines whether it is a per-track (0) or per-sector (1) byte, if bit 7 is set, the byte is the index hole. */
-#define BYTE_GAP0		0x00
-#define BYTE_GAP1		0x10
-#define BYTE_GAP4		0x20
-#define BYTE_GAP2		0x40
-#define BYTE_GAP3		0x50
-#define BYTE_I_SYNC		0x01
-#define BYTE_ID_SYNC		0x41
-#define BYTE_DATA_SYNC		0x51
-#define BYTE_IAM_SYNC		0x02
-#define BYTE_IDAM_SYNC		0x42
-#define BYTE_DATAAM_SYNC	0x52
-#define BYTE_IAM		0x03
-#define BYTE_IDAM		0x43
-#define BYTE_DATAAM		0x53
-#define BYTE_ID			0x44
-#define BYTE_DATA		0x54
-#define BYTE_ID_CRC		0x45
-#define BYTE_DATA_CRC		0x55
-
-#define BYTE_INDEX_HOLE		0x80	/* 1 = index hole, 0 = regular byte */
-#define BYTE_IS_SECTOR		0x40	/* 1 = per-sector, 0 = per-track */
-#define BYTE_IS_POST_TRACK	0x20	/* 1 = after all sectors, 0 = before or during all sectors */
-#define BYTE_IS_DATA		0x10	/* 1 = data, 0 = id */
-#define BYTE_TYPE		0x0F	/* 5 = crc, 4 = data, 3 = address mark, 2 = address mark sync, 1 = sync, 0 = gap */
-
-#define BYTE_TYPE_GAP		0x00
-#define BYTE_TYPE_SYNC		0x01
-#define BYTE_TYPE_AM_SYNC	0x02
-#define BYTE_TYPE_AM		0x03
-#define BYTE_TYPE_DATA		0x04
-#define BYTE_TYPE_CRC		0x05
-
 #define GAP3_LEN_VARIABLE	0x1B
 
 int gap3_sizes[3][2] = { {74, 36}, {77, 77}, {60, 60} };
diff --git a/src/fdc.c b/src/fdc.c
index ced392afc..ca1364f2b 100644
--- a/src/fdc.c
+++ b/src/fdc.c
@@ -777,7 +777,6 @@ bad_command:
 					fdc.dtl = 4000000;
 					fdc.format_sectors = fdc.params[2];
 					fdc.format_n = fdc.params[1];
-					if ((fdc.format_sectors == 18) && (fdc.format_n == 2))  fatal("Deliberate fatal\n");
                                         fdc.format_state = 1;
                                         fdc.pos = 0;
                                         fdc.stat = 0x30;
diff --git a/src/fdc.h b/src/fdc.h
index 46d7e8564..486f02b36 100644
--- a/src/fdc.h
+++ b/src/fdc.h
@@ -14,6 +14,7 @@ void fdc_discchange_clear(int drive);
 void fdc_set_dskchg_activelow();
 void fdc_3f1_enable(int enable);
 void fdc_set_ps1();
+int fdc_get_bit_rate();
 int fdc_get_bitcell_period();
 
 /* A few functions to communicate between Super I/O chips and the FDC. */
@@ -27,3 +28,16 @@ void fdc_update_densel_polarity(int densel_polarity);
 void fdc_update_densel_force(int densel_force);
 void fdc_update_drvrate(int drive, int drvrate);
 void fdc_update_drv2en(int drv2en);
+
+int fdc_get_format_n();
+int fdc_is_mfm();
+double fdc_get_hut();
+double fdc_get_hlt();
+void fdc_request_next_sector_id();
+void fdc_stop_id_request();
+int fdc_get_gap();
+int fdc_get_dtl();
+int fdc_get_format_sectors();
+
+void fdc_finishread();
+void fdc_sector_finishread();
diff --git a/src/vid_svga.c b/src/vid_svga.c
index 14f2df8f8..54aca73c1 100644
--- a/src/vid_svga.c
+++ b/src/vid_svga.c
@@ -612,7 +612,7 @@ void svga_poll(void *p)
 
                 if (svga->displine == svga->hwcursor_latch.y+1 && svga->hwcursor_latch.ena && svga->interlace)
                 {
-                        svga->hwcursor_on = 64 - svga->hwcursor_latch.yoff;
+                        svga->hwcursor_on = 64 - (svga->hwcursor_latch.yoff + 1);
                         svga->hwcursor_oddeven = 1;
                 }