abcdisk/abcio: fix handling of command queues

Correct the handling of command queues. Disk I/O now works!

fw/abcdisk.c

@@ -222,6 +222,11 @@ static inline uint8_t *cur_buf(struct ctl_state *state)
 
 static void disk_start_command(struct ctl_state *state)
 {
+#if 0
+    con_printf("%-2s:  start command\n", state->params->name);
+#endif
+    
+    memset(&state->k, 0xee, sizeof state->k);
     abc_setup_out_queue(&state->iodev, state->k, 4, 0x81);
 }
 
@@ -362,7 +367,7 @@ static void unmount_drives(struct ctl_state *state)
     }
 }
 
-#define IDLE_CALLBACK_MASK	(1 << 4)
+#define IDLE_CALLBACK_MASK	(1 << 4) /* C3 = reset */
 
 static void abcdisk_callback_out(struct abc_dev *dev, uint8_t data)
 {
@@ -410,7 +415,7 @@ static void init_drives(struct ctl_state *state)
     for (i = 0; i < 8; i++) {
 	struct drive_state *drv = &state->drv[i];
 	unsigned int filesec;
-	char * const *pfx;
+	const char * const *pfx;
 
 	snprintf(drv->name, sizeof drv->name, "%-.2s%c",
 		 state->params->name, i + '0');
@@ -433,180 +438,178 @@ static void do_next_command(struct ctl_state *state)
     struct drive_state *drv = cur_drv_mutable(state);
     uint8_t *buf = cur_buf(state);
 
-    while (state->k[0]) {
-#if 1
-	con_printf("%-3s: cmd %02x %02x %02x %02x\n",
-		   drv->name, state->k[0], state->k[1],
-		   state->k[2], state->k[3]);
+#if 0
+    con_printf("%-3s: cmd %02x %02x %02x %02x\n",
+	       drv->name, state->k[0], state->k[1],
+	       state->k[2], state->k[3]);
 #endif
 
-	if (state->k[0] & 0x01) {
-	    /* READ SECTOR */
-	    if (!(drv->flags & DF_MOUNTED)) {
-		disk_set_error(state, DISK_NOT_READY);
-	    } else if (!cur_sector_valid(state)) {
-		disk_set_error(state, OUT_OF_RANGE);
-	    } else if (!file_pos_valid(state)) {
-		disk_set_error(state, CRC_ERROR);
-	    } else {
-		UINT rlen = 0;
-		FRESULT rv;
+    if (state->k[0] & 0x01) {
+	/* READ SECTOR */
+	if (!(drv->flags & DF_MOUNTED)) {
+	    disk_set_error(state, DISK_NOT_READY);
+	} else if (!cur_sector_valid(state)) {
+	    disk_set_error(state, OUT_OF_RANGE);
+	} else if (!file_pos_valid(state)) {
+	    disk_set_error(state, CRC_ERROR);
+	} else {
+	    UINT rlen = 0;
+	    FRESULT rv;
 
-		set_led(LED_DISKIO, true);
+	    set_led(LED_DISKIO, true);
 
-		rv = f_lseek(&drv->file, file_pos(state));
-		if (rv == FR_OK)
-		    rv = f_read(&drv->file, buf, 256, &rlen);
-		if (rv != FR_OK || rlen != 256) {
-		    disk_set_error(state, CRC_ERROR);
-		}
+	    rv = f_lseek(&drv->file, file_pos(state));
+	    if (rv == FR_OK)
+		rv = f_read(&drv->file, buf, 256, &rlen);
+	    if (rv != FR_OK || rlen != 256) {
+		disk_set_error(state, CRC_ERROR);
 	    }
-	    state->k[0] &= ~0x01;   /* Command done */
 	}
-	if (state->k[0] & 0x02) {
-	    /* SECTOR TO HOST */
-	    state->k[0] &= ~0x02;   /* Command done */
-	    abc_setup_inp_queue(&state->iodev, buf, 256, 0x01);
-	    return;
-	}
-	if (state->k[0] & 0x04) {
-	    /* SECTOR FROM HOST */
-	    state->k[0] &= ~0x04;   /* Command done */
-	    abc_setup_out_queue(&state->iodev, buf, 256, 0x01);
-	    return;
-	}
-	if (state->k[0] & 0x08) {
-	    /* WRITE SECTOR */
-	    if (!(drv->flags & DF_MOUNTED)) {
-		disk_set_error(state, DISK_NOT_READY);
-	    } else if (drv->flags & DF_READONLY) {
-		disk_set_error(state, WRITE_PROTECT);
-	    } else if (!cur_sector_valid(state)) {
-		disk_set_error(state, OUT_OF_RANGE);
-	    } else if (!file_pos_valid(state)) {
-		disk_set_error(state, CRC_ERROR);
-	    } else {
-		UINT wlen = 0;
-		FRESULT rv;
+	state->k[0] &= ~0x01;   /* Command done */
+    }
+    if (state->k[0] & 0x02) {
+	/* SECTOR TO HOST */
+	state->k[0] &= ~0x02;   /* Command done */
+	abc_setup_inp_queue(&state->iodev, buf, 256, 0x01);
+	return;
+    }
+    if (state->k[0] & 0x04) {
+	/* SECTOR FROM HOST */
+	state->k[0] &= ~0x04;   /* Command done */
+	abc_setup_out_queue(&state->iodev, buf, 256, 0x01);
+	return;
+    }
+    if (state->k[0] & 0x08) {
+	/* WRITE SECTOR */
+	if (!(drv->flags & DF_MOUNTED)) {
+	    disk_set_error(state, DISK_NOT_READY);
+	} else if (drv->flags & DF_READONLY) {
+	    disk_set_error(state, WRITE_PROTECT);
+	} else if (!cur_sector_valid(state)) {
+	    disk_set_error(state, OUT_OF_RANGE);
+	} else if (!file_pos_valid(state)) {
+	    disk_set_error(state, CRC_ERROR);
+	} else {
+	    UINT wlen = 0;
+	    FRESULT rv;
 
-		set_led(LED_DISKIO, true);
+	    set_led(LED_DISKIO, true);
 
-		rv = f_lseek(&drv->file, file_pos(state));
-		if (rv == FR_OK)
-		    rv = f_write(&drv->file, buf, 256, &wlen);
-		if (rv != FR_OK || wlen != 256)
-		    disk_set_error(state, WRITE_FAULT);
-	    }
-	    state->k[0] &= ~0x08;   /* Command done */
+	    rv = f_lseek(&drv->file, file_pos(state));
+	    if (rv == FR_OK)
+		rv = f_write(&drv->file, buf, 256, &wlen);
+	    if (rv != FR_OK || wlen != 256)
+		disk_set_error(state, WRITE_FAULT);
 	}
+	state->k[0] &= ~0x08;   /* Command done */
+    }
 
 #if FORMAT_SUPPORT
-	/* This code needs additional work */
-
-	if (state->k[0] & 0x10 && state->k[1] & 0x08) {
-	    state->out_ptr = 0;
-	    /* FORMAT */
-	    if (!drv->hf) {
-		state->error = 0x80;	/* Not ready */
-	    } else if (!file_wrok(hf)) {
-		state->error = 0x40;	/* Write protect */
-	    } else {
-		unsigned int s, c0, c1, s0, s1;
-		unsigned int cylsec = state->s * state->h;
-		uint8_t data[256];
-		unsigned int fmtsec, filesec;
-
-		/* Sector count produced by format */
-		fmtsec = state->params->maxsectors;
-
-		/* For non-MO-drives, this seems to be internally generated */
-		memset(data, 0x40, 256);
-
-		if (state->fmtdata_in_buf) {
-		    /*
-		     * MO drives put the sector image in the buffers, for
-		     * backwards compatibility and to support single density.
-		     *
-		     * Right before the F7 header CRC opcode is a density byte;
-		     * 00 for single, and 01 for double.  The data begins after
-		     * a byte of FB.
-		     */
-		    bool single = false;
-		    const uint8_t *p, *ep;
-
-		    ep = state->buf[1];
-		    for (p = state->buf[0]+1; p < ep; p++) {
-			if (*p == 0xf7)
-			    single = (p[-1] == 0);
-			if (*p == 0xfb)
-			    break;
-		    }
+    /* This code needs additional work */
+
+    if (state->k[0] & 0x10 && state->k[1] & 0x08) {
+	state->out_ptr = 0;
+	/* FORMAT */
+	if (!drv->hf) {
+	    state->error = 0x80;	/* Not ready */
+	} else if (!file_wrok(hf)) {
+	    state->error = 0x40;	/* Write protect */
+	} else {
+	    unsigned int s, c0, c1, s0, s1;
+	    unsigned int cylsec = state->s * state->h;
+	    uint8_t data[256];
+	    unsigned int fmtsec, filesec;
 
-		    fmtsec >>= single;
+	    /* Sector count produced by format */
+	    fmtsec = state->params->maxsectors;
 
-		    if (*p++ == 0xfb) { /* Data block found */
-			if (single) {
-			    /* Really two 128-byte sectors! */
-			    memcpy(data, p, 128);
-			    memcpy(data+128, p, 128);
-			} else {
-			    memcpy(data, p, 256);
-			}
-		    }
-		}
+	    /* For non-MO-drives, this seems to be internally generated */
+	    memset(data, 0x40, 256);
 
+	    if (state->fmtdata_in_buf) {
 		/*
-		 * Adjust the size of the accessible device to the smallest
-		 * of the physical file and the formatted size
+		 * MO drives put the sector image in the buffers, for
+		 * backwards compatibility and to support single density.
+		 *
+		 * Right before the F7 header CRC opcode is a density byte;
+		 * 00 for single, and 01 for double.  The data begins after
+		 * a byte of FB.
 		 */
-		filesec = drv->hf->filesize >> 8;
-		drv->sectors = (filesec && filesec < fmtsec) ? filesec : fmtsec;
-
-		/*
-		 * k2 and k3 contain the first and last cylinder numbers to
-		 * format, inclusively.  The last cylinder may be partial due
-		 * to virtual remapping, e.g. for sf floppies.
-		 */
-
-		c0 = state->k[2];
-		s0 = c0 * cylsec;
-		c1 = state->k[3] + 1;
-		s1 = c1 * cylsec;
-
-		if (tracing(TRACE_DISK)) {
-		    fprintf(tracef, "%s: formatting cyl %u..%u, sectors %u..%u\n",
-			    drv->name, c0, c1-1, s0, s1-1);
+		bool single = false;
+		const uint8_t *p, *ep;
+
+		ep = state->buf[1];
+		for (p = state->buf[0]+1; p < ep; p++) {
+		    if (*p == 0xf7)
+			single = (p[-1] == 0);
+		    if (*p == 0xfb)
+			break;
 		}
 
-		clearerr(hf->f);
-		state->error = 0;
+		fmtsec >>= single;
 
-		for (s = s0; s < s1; s++) {
-		    unsigned int ps = virt2phys(drv, s);
-		    if (ps >= drv->sectors) {
-			state->error |= 0x02; /* Track 0/Lost data? */
-			break;
-		    } else if (hf->map) {
-			memcpy(hf->map + (ps << 8), data, 256);
+		if (*p++ == 0xfb) { /* Data block found */
+		    if (single) {
+			/* Really two 128-byte sectors! */
+			memcpy(data, p, 128);
+			memcpy(data+128, p, 128);
 		    } else {
-			fseek(hf->f, ps << 8, SEEK_SET);
-			fwrite(data, 1, 256, hf->f);
+			memcpy(data, p, 256);
 		    }
 		}
-		if (ferror(hf->f))
-		    state->error |= 0x20; /* Write fault */
 	    }
-	    state->k[1] &= ~0x08;
+
+	    /*
+	     * Adjust the size of the accessible device to the smallest
+	     * of the physical file and the formatted size
+	     */
+	    filesec = drv->hf->filesize >> 8;
+	    drv->sectors = (filesec && filesec < fmtsec) ? filesec : fmtsec;
+
+	    /*
+	     * k2 and k3 contain the first and last cylinder numbers to
+	     * format, inclusively.  The last cylinder may be partial due
+	     * to virtual remapping, e.g. for sf floppies.
+	     */
+
+	    c0 = state->k[2];
+	    s0 = c0 * cylsec;
+	    c1 = state->k[3] + 1;
+	    s1 = c1 * cylsec;
+
+	    if (tracing(TRACE_DISK)) {
+		fprintf(tracef, "%s: formatting cyl %u..%u, sectors %u..%u\n",
+			drv->name, c0, c1-1, s0, s1-1);
+	    }
+
+	    clearerr(hf->f);
+	    state->error = 0;
+
+	    for (s = s0; s < s1; s++) {
+		unsigned int ps = virt2phys(drv, s);
+		if (ps >= drv->sectors) {
+		    state->error |= 0x02; /* Track 0/Lost data? */
+		    break;
+		} else if (hf->map) {
+		    memcpy(hf->map + (ps << 8), data, 256);
+		} else {
+		    fseek(hf->f, ps << 8, SEEK_SET);
+		    fwrite(data, 1, 256, hf->f);
+		}
+	    }
+	    if (ferror(hf->f))
+		state->error |= 0x20; /* Write fault */
 	}
+	state->k[1] &= ~0x08;
+    }
 
-	if (!(state->k[1] & 0x38))
-	    state->k[0] &= ~0x10;
-#else
-	/* No FORMAT support */
+    if (!(state->k[1] & 0x38))
 	state->k[0] &= ~0x10;
+#else
+    /* No FORMAT support */
+    state->k[0] &= ~0x10;
 #endif
-	state->k[0] &= ~0xe0;	/* Unimplemented commands */
-    }
+    state->k[0] &= ~0xe0;	/* Unimplemented commands */
 
     disk_start_command(state);
 }
@@ -721,9 +724,8 @@ void abcdisk_io_poll(void)
 	if (pending & PEND_RESET)
 	    disk_reset_state(state);
 
-	if (pending & PEND_IO) {
+	if (pending & PEND_IO)
 	    do_next_command(state);
-	}
 
 	if (need_sync)
 	    sync_drives(state);
@@ -756,7 +758,7 @@ void abcdisk_init(void)
 	state->params = &parameters[i];
 	state->iodev  = iodev_template;
 
-	disk_reset_state(state);
 	abc_register(&state->iodev, state->params->devsel);
+	disk_reset_state(state);
     }
 }

fw/abcio.c

@@ -45,7 +45,7 @@ IRQHANDLER(abc)
 		*dev->out_buf++ = data;
 		dev->inp_data[1] &= dev->status_first_out_mask;
 		ABC_INP1_DATA = dev->inp_data[1];
-		if (!dev->out_cnt)
+		if (dev->out_cnt)
 		    break;	/* No callback */
 	    }
 	    if (callback & 1)
@@ -95,7 +95,8 @@ IRQHANDLER(abc)
 	    dev->callback_status(dev);
     }
 
-    ABC_BUSY_STATUS = what;
+    /* callback_mask might have changed */
+    ABC_BUSY = what | ((dev->callback_mask | 0x0082) << 16);
 }
 
 void abc_setup_out_queue(struct abc_dev *dev, void *buf, size_t len,
@@ -107,7 +108,8 @@ void abc_setup_out_queue(struct abc_dev *dev, void *buf, size_t len,
     dev->out_cnt = len;
     dev->inp_data[1] = status;
     dev->callback_mask |= (1 << 0);
-    abc_select(selected_dev);
+    if (selected_dev == dev)
+	abc_select(dev);	/* Update registers */
 
     unmask_irq(ABC_IRQ);
 }
@@ -122,7 +124,8 @@ void abc_setup_inp_queue(struct abc_dev *dev, const void *buf, size_t len,
     dev->inp_data[0] = *dev->inp_buf++;
     dev->inp_data[1] = status;
     dev->callback_mask |= (1 << 8);
-    abc_select(selected_dev);
+    if (selected_dev == dev)
+	abc_select(dev);	/* Update registers */
 
     unmask_irq(ABC_IRQ);
 }