[SeaBIOS] [PATCH 6/7] Introduce floppy_turn_off_motor_delayed() to keep the motor spinning for 2 seconds after the end of a floppy operation

[Nikolay Nikolov]

The new function is called after each floppy operation (except controller
reset) and resets the floppy motor counter in BDA to FLOPPY_MOTOR_TICKS
(about 2 seconds).

Signed-off-by: Nikolay Nikolov 
---
 src/hw/floppy.c | 35 ++++++++++++++++++++++++++++-------
 1 file changed, 28 insertions(+), 7 deletions(-)

diff --git a/src/hw/floppy.c b/src/hw/floppy.c
index b5bc114..acaff3f 100644
--- a/src/hw/floppy.c
+++ b/src/hw/floppy.c
@@ -347,6 +347,13 @@ floppy_turn_on_motor(u8 floppyid)
     floppy_dor_write((floppyid ? FLOPPY_DOR_MOTOR_B : FLOPPY_DOR_MOTOR_A) | FLOPPY_DOR_IRQ | FLOPPY_DOR_RESET | floppyid);
 }

+static inline void
+floppy_turn_off_motor_delayed(void)
+{
+    // reset the disk motor timeout value of INT 08
+    SET_BDA(floppy_motor_counter, FLOPPY_MOTOR_TICKS);
+}
+
 // Activate a drive and send a command to it.
 static int
 floppy_drive_pio(u8 floppyid, int command, u8 *param)
@@ -570,8 +577,10 @@ floppy_read(struct disk_op_s *op)
 {
     struct chs_s chs = lba2chs(op);
     int ret = floppy_prep(op->drive_fl, chs.cylinder);
-    if (ret)
+    if (ret) {
+        floppy_turn_off_motor_delayed();
         return ret;
+    }

// send read-normal-data command to controller
     u8 floppyid = GET_GLOBALFLAT(op->drive_fl->cntl_id);
@@ -584,7 +593,9 @@ floppy_read(struct disk_op_s *op)
     param[5] = chs.sector + op->count - 1; // last sector to read on track
     param[6] = FLOPPY_GAPLEN;
     param[7] = FLOPPY_DATALEN;
-    return floppy_dma_cmd(op, op->count * DISK_SECTOR_SIZE, FC_READ, param);
+    ret = floppy_dma_cmd(op, op->count * DISK_SECTOR_SIZE, FC_READ, param);
+    floppy_turn_off_motor_delayed();
+    return ret;
 }

// Write Diskette Sectors
@@ -593,8 +604,10 @@ floppy_write(struct disk_op_s *op)
 {
     struct chs_s chs = lba2chs(op);
     int ret = floppy_prep(op->drive_fl, chs.cylinder);
-    if (ret)
+    if (ret) {
+        floppy_turn_off_motor_delayed();
         return ret;
+    }

// send write-normal-data command to controller
     u8 floppyid = GET_GLOBALFLAT(op->drive_fl->cntl_id);
@@ -607,7 +620,9 @@ floppy_write(struct disk_op_s *op)
     param[5] = chs.sector + op->count - 1; // last sector to write on track
     param[6] = FLOPPY_GAPLEN;
     param[7] = FLOPPY_DATALEN;
-    return floppy_dma_cmd(op, op->count * DISK_SECTOR_SIZE, FC_WRITE, param);
+    ret = floppy_dma_cmd(op, op->count * DISK_SECTOR_SIZE, FC_WRITE, param);
+    floppy_turn_off_motor_delayed();
+    return ret;
 }

// Verify Diskette Sectors
@@ -616,8 +631,10 @@ floppy_verify(struct disk_op_s *op)
 {
     struct chs_s chs = lba2chs(op);
     int ret = floppy_prep(op->drive_fl, chs.cylinder);
-    if (ret)
+    if (ret) {
+        floppy_turn_off_motor_delayed();
         return ret;
+    }

// This command isn't implemented - just return success.
     return DISK_RET_SUCCESS;
@@ -629,8 +646,10 @@ floppy_format(struct disk_op_s *op)
 {
     struct chs_s chs = lba2chs(op);
     int ret = floppy_prep(op->drive_fl, chs.cylinder);
-    if (ret)
+    if (ret) {
+        floppy_turn_off_motor_delayed();
         return ret;
+    }

// send format-track command to controller
     u8 floppyid = GET_GLOBALFLAT(op->drive_fl->cntl_id);
@@ -640,7 +659,9 @@ floppy_format(struct disk_op_s *op)
     param[2] = op->count; // number of sectors per track
     param[3] = FLOPPY_FORMAT_GAPLEN;
     param[4] = FLOPPY_FILLBYTE;
-    return floppy_dma_cmd(op, op->count * 4, FC_FORMAT, param);
+    ret = floppy_dma_cmd(op, op->count * 4, FC_FORMAT, param);
+    floppy_turn_off_motor_delayed();
+    return ret;
 }

int