mtx: Update the floppy code to turn off the motor when its done

diff --git a/Kernel/platform-mtx/devfd.c b/Kernel/platform-mtx/devfd.c
index 7e124a8..415e60d 100644 (file)
--- a/Kernel/platform-mtx/devfd.c
+++ b/Kernel/platform-mtx/devfd.c
@@ -22,6 +22,17 @@ static uint8_t motorct;
 static uint8_t fd_selected = 0xFF;
 static uint8_t fd_tab[MAX_FD] = { 0xFF, 0xFF };
 
+void fd_motor_timer(void)
+{
+    if (motorct) {
+        motorct--;
+        if (motorct == 0) {
+            fd_motor_off();
+            fd_selected = 0xFF;
+        }
+    }
+}
+
 /*
  *     We only support normal block I/O
  */
@@ -35,15 +46,19 @@ static int fd_transfer(uint8_t minor, bool is_read, uint8_t rawflag)
     uint8_t err = 0;
     uint8_t *driveptr = &fd_tab[minor & 1];
     uint8_t cmd[6];
+    irqflags_t irq;
 
     if(rawflag)
         goto bad2;
 
+    irq = di();
     if (fd_selected != minor) {
         uint8_t err = fd_motor_on(minor|(minor > 1 ? 0: 0x10));
         if (err)
             goto bad;
+        motorct = 150; /* 3 seconds */
     }
+    irqrestore(irq);
 
     dptr = (uint16_t)udata.u_buf->bf_data;
     block = udata.u_buf->bf_blk;

diff --git a/Kernel/platform-mtx/devfd.h b/Kernel/platform-mtx/devfd.h
index 5f94507..dfda83e 100644 (file)
--- a/Kernel/platform-mtx/devfd.h
+++ b/Kernel/platform-mtx/devfd.h
@@ -5,11 +5,12 @@
 int fd_read(uint8_t minor, uint8_t rawflag, uint8_t flag);
 int fd_write(uint8_t minor, uint8_t rawflag, uint8_t flag);
 int fd_open(uint8_t minor, uint16_t flag);
+void fd_motor_timer(void);
 
 /* low level interface */
 uint16_t fd_reset(uint8_t *driveptr);
 uint16_t fd_operation(uint8_t *cmd, uint8_t *driveptr);
 uint16_t fd_motor_on(uint16_t drivesel);
-uint16_t fd_motor_off(uint16_t driveptr);
+uint16_t fd_motor_off(void);
 
 #endif /* __DEVFD_DOT_H__ */

diff --git a/Kernel/platform-mtx/main.c b/Kernel/platform-mtx/main.c
index 6917be8..6d280ce 100644 (file)
--- a/Kernel/platform-mtx/main.c
+++ b/Kernel/platform-mtx/main.c
@@ -3,6 +3,7 @@
 #include <kdata.h>
 #include <printf.h>
 #include <devtty.h>
+#include <devfd.h>
 
 uint16_t ramtop = PROGTOP;
 uint16_t vdpport = 0x02 + 256 * 40;    /* port and width */
@@ -36,6 +37,7 @@ void platform_interrupt(void)
     return;
   }
   kbd_interrupt();
+  fd_motor_timer();
   timer_interrupt();
 }