From: Alan Cox <alan@linux.intel.com>
Date: Tue, 16 Dec 2014 23:34:52 +0000 (+0000)
Subject: mtx: Update the floppy code to turn off the motor when its done
X-Git-Url: https://git.ndcode.org/public/gitweb.cgi?a=commitdiff_plain;h=9421b897dbed0a03213935b903985b9ec5fa5b0d;p=FUZIX.git

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 7e124a84..415e60de 100644
--- 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 5f945073..dfda83e5 100644
--- 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 6917be87..6d280ce1 100644
--- 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();
 }