FDC: extract command implementations

Index: fdc.c
===================================================================
@@ -1441,6 +1441,282 @@
     }
 }
 
+static void fdctrl_handle_lock (fdctrl_t *fdctrl, int direction)
+{
+    fdctrl->lock = 1;
+    fdctrl->fifo[0] = 0x10;
+    fdctrl_set_fifo(fdctrl, 1, 1);
+}
+
+static void fdctrl_handle_unlock (fdctrl_t *fdctrl, int direction)
+{
+    fdctrl->lock = 0;
+    fdctrl->fifo[0] = 0;
+    fdctrl_set_fifo(fdctrl, 1, 0);
+}
+
+static void fdctrl_handle_dumpreg (fdctrl_t *fdctrl, int direction)
+{
+    fdrive_t *cur_drv = get_cur_drv(fdctrl);
+
+    /* Drives position */
+    fdctrl->fifo[0] = drv0(fdctrl)->track;
+    fdctrl->fifo[1] = drv1(fdctrl)->track;
+    fdctrl->fifo[2] = 0;
+    fdctrl->fifo[3] = 0;
+    /* timers */
+    fdctrl->fifo[4] = fdctrl->timer0;
+    fdctrl->fifo[5] = (fdctrl->timer1 << 1) | fdctrl->dma_en;
+    fdctrl->fifo[6] = cur_drv->last_sect;
+    fdctrl->fifo[7] = (fdctrl->lock << 7) |
+        (cur_drv->perpendicular << 2);
+    fdctrl->fifo[8] = fdctrl->config;
+    fdctrl->fifo[9] = fdctrl->precomp_trk;
+    fdctrl_set_fifo(fdctrl, 10, 0);
+}
+
+static void fdctrl_handle_version (fdctrl_t *fdctrl, int direction)
+{
+    /* Controller's version */
+    fdctrl->fifo[0] = fdctrl->version;
+    fdctrl_set_fifo(fdctrl, 1, 1);
+}
+
+static void fdctrl_handle_partid (fdctrl_t *fdctrl, int direction)
+{
+    fdctrl->fifo[0] = 0x41; /* Stepping 1 */
+    fdctrl_set_fifo(fdctrl, 1, 0);
+}
+
+static void fdctrl_handle_restore (fdctrl_t *fdctrl, int direction)
+{
+    fdrive_t *cur_drv = get_cur_drv(fdctrl);
+
+    /* Drives position */
+    drv0(fdctrl)->track = fdctrl->fifo[3];
+    drv1(fdctrl)->track = fdctrl->fifo[4];
+    /* timers */
+    fdctrl->timer0 = fdctrl->fifo[7];
+    fdctrl->timer1 = fdctrl->fifo[8];
+    cur_drv->last_sect = fdctrl->fifo[9];
+    fdctrl->lock = fdctrl->fifo[10] >> 7;
+    cur_drv->perpendicular = (fdctrl->fifo[10] >> 2) & 0xF;
+    fdctrl->config = fdctrl->fifo[11];
+    fdctrl->precomp_trk = fdctrl->fifo[12];
+    fdctrl->pwrd = fdctrl->fifo[13];
+    fdctrl_reset_fifo(fdctrl);
+}
+
+static void fdctrl_handle_save (fdctrl_t *fdctrl, int direction)
+{
+    fdrive_t *cur_drv = get_cur_drv(fdctrl);
+
+    fdctrl->fifo[0] = 0;
+    fdctrl->fifo[1] = 0;
+    /* Drives position */
+    fdctrl->fifo[2] = drv0(fdctrl)->track;
+    fdctrl->fifo[3] = drv1(fdctrl)->track;
+    fdctrl->fifo[4] = 0;
+    fdctrl->fifo[5] = 0;
+    /* timers */
+    fdctrl->fifo[6] = fdctrl->timer0;
+    fdctrl->fifo[7] = fdctrl->timer1;
+    fdctrl->fifo[8] = cur_drv->last_sect;
+    fdctrl->fifo[9] = (fdctrl->lock << 7) |
+        (cur_drv->perpendicular << 2);
+    fdctrl->fifo[10] = fdctrl->config;
+    fdctrl->fifo[11] = fdctrl->precomp_trk;
+    fdctrl->fifo[12] = fdctrl->pwrd;
+    fdctrl->fifo[13] = 0;
+    fdctrl->fifo[14] = 0;
+    fdctrl_set_fifo(fdctrl, 15, 1);
+}
+
+static void fdctrl_handle_readid (fdctrl_t *fdctrl, int direction)
+{
+    fdrive_t *cur_drv = get_cur_drv(fdctrl);
+
+    /* XXX: should set main status register to busy */
+    cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
+    qemu_mod_timer(fdctrl->result_timer,
+                   qemu_get_clock(vm_clock) + (ticks_per_sec / 50));
+}
+
+static void fdctrl_handle_format_track (fdctrl_t *fdctrl, int direction)
+{
+    fdrive_t *cur_drv;
+
+    fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+    cur_drv = get_cur_drv(fdctrl);
+    fdctrl->data_state |= FD_STATE_FORMAT;
+    if (fdctrl->fifo[0] & 0x80)
+        fdctrl->data_state |= FD_STATE_MULTI;
+    else
+        fdctrl->data_state &= ~FD_STATE_MULTI;
+    fdctrl->data_state &= ~FD_STATE_SEEK;
+    cur_drv->bps =
+        fdctrl->fifo[2] > 7 ? 16384 : 128 << fdctrl->fifo[2];
+#if 0
+    cur_drv->last_sect =
+        cur_drv->flags & FDISK_DBL_SIDES ? fdctrl->fifo[3] :
+        fdctrl->fifo[3] / 2;
+#else
+    cur_drv->last_sect = fdctrl->fifo[3];
+#endif
+    /* TODO: implement format using DMA expected by the Bochs BIOS
+     * and Linux fdformat (read 3 bytes per sector via DMA and fill
+     * the sector with the specified fill byte
+     */
+    fdctrl->data_state &= ~FD_STATE_FORMAT;
+    fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
+}
+
+static void fdctrl_handle_specify (fdctrl_t *fdctrl, int direction)
+{
+    fdctrl->timer0 = (fdctrl->fifo[1] >> 4) & 0xF;
+    fdctrl->timer1 = fdctrl->fifo[2] >> 1;
+    fdctrl->dma_en = 1 - (fdctrl->fifo[2] & 1) ;
+    /* No result back */
+    fdctrl_reset_fifo(fdctrl);
+}
+
+static void fdctrl_handle_sense_drive_status (fdctrl_t *fdctrl, int direction)
+{
+    fdrive_t *cur_drv;
+
+    fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+    cur_drv = get_cur_drv(fdctrl);
+    cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
+    /* 1 Byte status back */
+    fdctrl->fifo[0] = (cur_drv->ro << 6) |
+        (cur_drv->track == 0 ? 0x10 : 0x00) |
+        (cur_drv->head << 2) |
+        fdctrl->cur_drv |
+        0x28;
+    fdctrl_set_fifo(fdctrl, 1, 0);
+}
+
+static void fdctrl_handle_recalibrate (fdctrl_t *fdctrl, int direction)
+{
+    fdrive_t *cur_drv;
+
+    fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+    cur_drv = get_cur_drv(fdctrl);
+    fd_recalibrate(cur_drv);
+    fdctrl_reset_fifo(fdctrl);
+    /* Raise Interrupt */
+    fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
+}
+
+static void fdctrl_handle_seek (fdctrl_t *fdctrl, int direction)
+{
+    fdrive_t *cur_drv;
+
+    fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+    cur_drv = get_cur_drv(fdctrl);
+    fd_start(cur_drv);
+    if (fdctrl->fifo[2] <= cur_drv->track)
+        cur_drv->dir = 1;
+    else
+        cur_drv->dir = 0;
+    fdctrl_reset_fifo(fdctrl);
+    if (fdctrl->fifo[2] > cur_drv->max_track) {
+        fdctrl_raise_irq(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK);
+    } else {
+        cur_drv->track = fdctrl->fifo[2];
+        /* Raise Interrupt */
+        fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
+    }
+}
+
+static void fdctrl_handle_perpendicular_mode (fdctrl_t *fdctrl, int direction)
+{
+    fdrive_t *cur_drv = get_cur_drv(fdctrl);
+
+    if (fdctrl->fifo[1] & 0x80)
+        cur_drv->perpendicular = fdctrl->fifo[1] & 0x7;
+    /* No result back */
+    fdctrl_reset_fifo(fdctrl);
+}
+
+static void fdctrl_handle_configure (fdctrl_t *fdctrl, int direction)
+{
+    fdctrl->config = fdctrl->fifo[2];
+    fdctrl->precomp_trk =  fdctrl->fifo[3];
+    /* No result back */
+    fdctrl_reset_fifo(fdctrl);
+}
+
+static void fdctrl_handle_powerdown_mode (fdctrl_t *fdctrl, int direction)
+{
+    fdctrl->pwrd = fdctrl->fifo[1];
+    fdctrl->fifo[0] = fdctrl->fifo[1];
+    fdctrl_set_fifo(fdctrl, 1, 1);
+}
+
+static void fdctrl_handle_option (fdctrl_t *fdctrl, int direction)
+{
+    /* No result back */
+    fdctrl_reset_fifo(fdctrl);
+}
+
+static void fdctrl_handle_drive_specification_command (fdctrl_t *fdctrl, int direction)
+{
+    fdrive_t *cur_drv = get_cur_drv(fdctrl);
+
+    if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x80) {
+        /* Command parameters done */
+        if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x40) {
+            fdctrl->fifo[0] = fdctrl->fifo[1];
+            fdctrl->fifo[2] = 0;
+            fdctrl->fifo[3] = 0;
+            fdctrl_set_fifo(fdctrl, 4, 1);
+        } else {
+            fdctrl_reset_fifo(fdctrl);
+        }
+    } else if (fdctrl->data_len > 7) {
+        /* ERROR */
+        fdctrl->fifo[0] = 0x80 |
+            (cur_drv->head << 2) | fdctrl->cur_drv;
+        fdctrl_set_fifo(fdctrl, 1, 1);
+    }
+}
+
+static void fdctrl_handle_relative_seek_out (fdctrl_t *fdctrl, int direction)
+{
+    fdrive_t *cur_drv = get_cur_drv(fdctrl);
+
+    fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+    cur_drv = get_cur_drv(fdctrl);
+    fd_start(cur_drv);
+    cur_drv->dir = 0;
+    if (fdctrl->fifo[2] + cur_drv->track >= cur_drv->max_track) {
+        cur_drv->track = cur_drv->max_track - 1;
+    } else {
+        cur_drv->track += fdctrl->fifo[2];
+    }
+    fdctrl_reset_fifo(fdctrl);
+    fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
+}
+
+static void fdctrl_handle_relative_seek_in (fdctrl_t *fdctrl, int direction)
+{
+    fdrive_t *cur_drv = get_cur_drv(fdctrl);
+
+    fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+    cur_drv = get_cur_drv(fdctrl);
+    fd_start(cur_drv);
+    cur_drv->dir = 1;
+    if (fdctrl->fifo[2] > cur_drv->track) {
+        cur_drv->track = 0;
+    } else {
+        cur_drv->track -= fdctrl->fifo[2];
+    }
+    fdctrl_reset_fifo(fdctrl);
+    /* Raise Interrupt */
+    fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
+}
+
 static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)
 {
     fdrive_t *cur_drv;

@@ -1571,20 +1847,7 @@
         case FD_CMD_DUMPREG:
             /* DUMPREG */
             FLOPPY_DPRINTF("DUMPREG command\n");
-            /* Drives position */
-            fdctrl->fifo[0] = drv0(fdctrl)->track;
-            fdctrl->fifo[1] = drv1(fdctrl)->track;
-            fdctrl->fifo[2] = 0;
-            fdctrl->fifo[3] = 0;
-            /* timers */
-            fdctrl->fifo[4] = fdctrl->timer0;
-            fdctrl->fifo[5] = (fdctrl->timer1 << 1) | fdctrl->dma_en;
-            fdctrl->fifo[6] = cur_drv->last_sect;
-            fdctrl->fifo[7] = (fdctrl->lock << 7) |
-                (cur_drv->perpendicular << 2);
-            fdctrl->fifo[8] = fdctrl->config;
-            fdctrl->fifo[9] = fdctrl->precomp_trk;
-            fdctrl_set_fifo(fdctrl, 10, 0);
+            fdctrl_handle_dumpreg(fdctrl, 0);
             return;
         case FD_CMD_SEEK:
             /* SEEK */

@@ -1595,10 +1858,7 @@
         case FD_CMD_VERSION:
             /* VERSION */
             FLOPPY_DPRINTF("VERSION command\n");
-            /* No parameters cmd */
-            /* Controller's version */
-            fdctrl->fifo[0] = fdctrl->version;
-            fdctrl_set_fifo(fdctrl, 1, 1);
+            fdctrl_handle_version(fdctrl, 0);
             return;
         case FD_CMD_PERPENDICULAR_MODE:
             /* PERPENDICULAR_MODE */

@@ -1615,10 +1875,7 @@
         case FD_CMD_UNLOCK:
             /* UNLOCK */
             FLOPPY_DPRINTF("UNLOCK command\n");
-            /* No parameters cmd */
-            fdctrl->lock = 0;
-            fdctrl->fifo[0] = 0;
-            fdctrl_set_fifo(fdctrl, 1, 0);
+            fdctrl_handle_unlock(fdctrl, 0);
             return;
         case FD_CMD_POWERDOWN_MODE:
             /* POWERDOWN_MODE */

@@ -1629,33 +1886,12 @@
         case FD_CMD_PART_ID:
             /* PART_ID */
             FLOPPY_DPRINTF("PART_ID command\n");
-            /* No parameters cmd */
-            fdctrl->fifo[0] = 0x41; /* Stepping 1 */
-            fdctrl_set_fifo(fdctrl, 1, 0);
+            fdctrl_handle_partid(fdctrl, 0);
             return;
         case FD_CMD_SAVE:
             /* SAVE */
             FLOPPY_DPRINTF("SAVE command\n");
-            /* No parameters cmd */
-            fdctrl->fifo[0] = 0;
-            fdctrl->fifo[1] = 0;
-            /* Drives position */
-            fdctrl->fifo[2] = drv0(fdctrl)->track;
-            fdctrl->fifo[3] = drv1(fdctrl)->track;
-            fdctrl->fifo[4] = 0;
-            fdctrl->fifo[5] = 0;
-            /* timers */
-            fdctrl->fifo[6] = fdctrl->timer0;
-            fdctrl->fifo[7] = fdctrl->timer1;
-            fdctrl->fifo[8] = cur_drv->last_sect;
-            fdctrl->fifo[9] = (fdctrl->lock << 7) |
-                (cur_drv->perpendicular << 2);
-            fdctrl->fifo[10] = fdctrl->config;
-            fdctrl->fifo[11] = fdctrl->precomp_trk;
-            fdctrl->fifo[12] = fdctrl->pwrd;
-            fdctrl->fifo[13] = 0;
-            fdctrl->fifo[14] = 0;
-            fdctrl_set_fifo(fdctrl, 15, 1);
+            fdctrl_handle_save(fdctrl, 0);
             return;
         case FD_CMD_OPTION:
             /* OPTION */

@@ -1702,10 +1938,7 @@
         case FD_CMD_LOCK:
             /* LOCK */
             FLOPPY_DPRINTF("LOCK command\n");
-            /* No parameters cmd */
-            fdctrl->lock = 1;
-            fdctrl->fifo[0] = 0x10;
-            fdctrl_set_fifo(fdctrl, 1, 1);
+            fdctrl_handle_lock(fdctrl, 0);
             return;
         case FD_CMD_FORMAT_AND_WRITE:
             /* FORMAT_AND_WRITE */

@@ -1797,83 +2030,42 @@
         case FD_CMD_SPECIFY:
             /* SPECIFY */
             FLOPPY_DPRINTF("treat SPECIFY command\n");
-            fdctrl->timer0 = (fdctrl->fifo[1] >> 4) & 0xF;
-            fdctrl->timer1 = fdctrl->fifo[2] >> 1;
-            fdctrl->dma_en = 1 - (fdctrl->fifo[2] & 1) ;
-            /* No result back */
-            fdctrl_reset_fifo(fdctrl);
+            fdctrl_handle_specify(fdctrl, 0);
             break;
         case FD_CMD_SENSE_DRIVE_STATUS:
             /* SENSE_DRIVE_STATUS */
             FLOPPY_DPRINTF("treat SENSE_DRIVE_STATUS command\n");
-            fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
-            cur_drv = get_cur_drv(fdctrl);
-            cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
-            /* 1 Byte status back */
-            fdctrl->fifo[0] = (cur_drv->ro << 6) |
-                (cur_drv->track == 0 ? 0x10 : 0x00) |
-                (cur_drv->head << 2) |
-                fdctrl->cur_drv |
-                0x28;
-            fdctrl_set_fifo(fdctrl, 1, 0);
+            fdctrl_handle_sense_drive_status(fdctrl, 0);
             break;
         case FD_CMD_RECALIBRATE:
             /* RECALIBRATE */
             FLOPPY_DPRINTF("treat RECALIBRATE command\n");
-            fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
-            cur_drv = get_cur_drv(fdctrl);
-            fd_recalibrate(cur_drv);
-            fdctrl_reset_fifo(fdctrl);
-            /* Raise Interrupt */
-            fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
+            fdctrl_handle_recalibrate(fdctrl, 0);            
             break;
         case FD_CMD_SEEK:
             /* SEEK */
             FLOPPY_DPRINTF("treat SEEK command\n");
-            fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
-            cur_drv = get_cur_drv(fdctrl);
-            fd_start(cur_drv);
-            if (fdctrl->fifo[2] <= cur_drv->track)
-                cur_drv->dir = 1;
-            else
-                cur_drv->dir = 0;
-            fdctrl_reset_fifo(fdctrl);
-            if (fdctrl->fifo[2] > cur_drv->max_track) {
-                fdctrl_raise_irq(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK);
-            } else {
-                cur_drv->track = fdctrl->fifo[2];
-                /* Raise Interrupt */
-                fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
-            }
+            fdctrl_handle_seek(fdctrl, 0);
             break;
         case FD_CMD_PERPENDICULAR_MODE:
             /* PERPENDICULAR_MODE */
             FLOPPY_DPRINTF("treat PERPENDICULAR_MODE command\n");
-            if (fdctrl->fifo[1] & 0x80)
-                cur_drv->perpendicular = fdctrl->fifo[1] & 0x7;
-            /* No result back */
-            fdctrl_reset_fifo(fdctrl);
+            fdctrl_handle_perpendicular_mode(fdctrl, 0);
             break;
         case FD_CMD_CONFIGURE:
             /* CONFIGURE */
             FLOPPY_DPRINTF("treat CONFIGURE command\n");
-            fdctrl->config = fdctrl->fifo[2];
-            fdctrl->precomp_trk =  fdctrl->fifo[3];
-            /* No result back */
-            fdctrl_reset_fifo(fdctrl);
+            fdctrl_handle_configure(fdctrl, 0);            
             break;
         case FD_CMD_POWERDOWN_MODE:
             /* POWERDOWN_MODE */
             FLOPPY_DPRINTF("treat POWERDOWN_MODE command\n");
-            fdctrl->pwrd = fdctrl->fifo[1];
-            fdctrl->fifo[0] = fdctrl->fifo[1];
-            fdctrl_set_fifo(fdctrl, 1, 1);
+            fdctrl_handle_powerdown_mode(fdctrl, 0);            
             break;
         case FD_CMD_OPTION:
             /* OPTION */
             FLOPPY_DPRINTF("treat OPTION command\n");
-            /* No result back */
-            fdctrl_reset_fifo(fdctrl);
+            fdctrl_handle_option(fdctrl, 0);
             break;
         case FD_CMD_READ_TRACK:
             /* READ_TRACK */

@@ -1884,89 +2076,27 @@
         case FD_CMD_READ_ID:
             /* READ_ID */
             FLOPPY_DPRINTF("treat READ_ID command\n");
-            /* XXX: should set main status register to busy */
-            cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
-            qemu_mod_timer(fdctrl->result_timer,
-                           qemu_get_clock(vm_clock) + (ticks_per_sec / 50));
+            fdctrl_handle_readid(fdctrl, 0);
             break;
         case FD_CMD_RESTORE:
             /* RESTORE */
             FLOPPY_DPRINTF("treat RESTORE command\n");
-            /* Drives position */
-            drv0(fdctrl)->track = fdctrl->fifo[3];
-            drv1(fdctrl)->track = fdctrl->fifo[4];
-            /* timers */
-            fdctrl->timer0 = fdctrl->fifo[7];
-            fdctrl->timer1 = fdctrl->fifo[8];
-            cur_drv->last_sect = fdctrl->fifo[9];
-            fdctrl->lock = fdctrl->fifo[10] >> 7;
-            cur_drv->perpendicular = (fdctrl->fifo[10] >> 2) & 0xF;
-            fdctrl->config = fdctrl->fifo[11];
-            fdctrl->precomp_trk = fdctrl->fifo[12];
-            fdctrl->pwrd = fdctrl->fifo[13];
-            fdctrl_reset_fifo(fdctrl);
+            fdctrl_handle_restore(fdctrl, 0);
             break;
         case FD_CMD_FORMAT_TRACK:
             /* FORMAT_TRACK */
             FLOPPY_DPRINTF("treat FORMAT_TRACK command\n");
-            fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
-            cur_drv = get_cur_drv(fdctrl);
-            fdctrl->data_state |= FD_STATE_FORMAT;
-            if (fdctrl->fifo[0] & 0x80)
-                fdctrl->data_state |= FD_STATE_MULTI;
-            else
-                fdctrl->data_state &= ~FD_STATE_MULTI;
-            fdctrl->data_state &= ~FD_STATE_SEEK;
-            cur_drv->bps =
-                fdctrl->fifo[2] > 7 ? 16384 : 128 << fdctrl->fifo[2];
-#if 0
-            cur_drv->last_sect =
-                cur_drv->flags & FDISK_DBL_SIDES ? fdctrl->fifo[3] :
-                fdctrl->fifo[3] / 2;
-#else
-            cur_drv->last_sect = fdctrl->fifo[3];
-#endif
-            /* TODO: implement format using DMA expected by the Bochs BIOS
-             * and Linux fdformat (read 3 bytes per sector via DMA and fill
-             * the sector with the specified fill byte
-             */
-            fdctrl->data_state &= ~FD_STATE_FORMAT;
-            fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
+            fdctrl_handle_format_track(fdctrl, 0);
             break;
         case FD_CMD_DRIVE_SPECIFICATION_COMMAND:
             /* DRIVE_SPECIFICATION_COMMAND */
             FLOPPY_DPRINTF("treat DRIVE_SPECIFICATION_COMMAND command\n");
-            if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x80) {
-                /* Command parameters done */
-                if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x40) {
-                    fdctrl->fifo[0] = fdctrl->fifo[1];
-                    fdctrl->fifo[2] = 0;
-                    fdctrl->fifo[3] = 0;
-                    fdctrl_set_fifo(fdctrl, 4, 1);
-                } else {
-                    fdctrl_reset_fifo(fdctrl);
-                }
-            } else if (fdctrl->data_len > 7) {
-                /* ERROR */
-                fdctrl->fifo[0] = 0x80 |
-                    (cur_drv->head << 2) | fdctrl->cur_drv;
-                fdctrl_set_fifo(fdctrl, 1, 1);
-            }
+            fdctrl_handle_drive_specification_command(fdctrl, 0);
             break;
         case FD_CMD_RELATIVE_SEEK_OUT:
             /* RELATIVE_SEEK_OUT */
             FLOPPY_DPRINTF("treat RELATIVE_SEEK_OUT command\n");
-            fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
-            cur_drv = get_cur_drv(fdctrl);
-            fd_start(cur_drv);
-            cur_drv->dir = 0;
-            if (fdctrl->fifo[2] + cur_drv->track >= cur_drv->max_track) {
-                cur_drv->track = cur_drv->max_track - 1;
-            } else {
-                cur_drv->track += fdctrl->fifo[2];
-            }
-            fdctrl_reset_fifo(fdctrl);
-            fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
+            fdctrl_handle_relative_seek_out(fdctrl, 0);
             break;
         case FD_CMD_FORMAT_AND_WRITE:
             /* FORMAT_AND_WRITE */

@@ -1977,18 +2107,7 @@
         case FD_CMD_RELATIVE_SEEK_IN:
             /* RELATIVE_SEEK_IN */
             FLOPPY_DPRINTF("treat RELATIVE_SEEK_IN command\n");
-            fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
-            cur_drv = get_cur_drv(fdctrl);
-            fd_start(cur_drv);
-            cur_drv->dir = 1;
-            if (fdctrl->fifo[2] > cur_drv->track) {
-                cur_drv->track = 0;
-            } else {
-                cur_drv->track -= fdctrl->fifo[2];
-            }
-            fdctrl_reset_fifo(fdctrl);
-            /* Raise Interrupt */
-            fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
+            fdctrl_handle_relative_seek_in(fdctrl, 0);
             break;
         }
     }