zx128: Add motor timeout logic for Microdrive

diff --git a/Kernel/platform-zx128/devmdv.c b/Kernel/platform-zx128/devmdv.c
index 6305a70..8b0f5de 100644 (file)
--- a/Kernel/platform-zx128/devmdv.c
+++ b/Kernel/platform-zx128/devmdv.c
@@ -22,12 +22,24 @@ uint8_t *mdv_buf;
 uint8_t mdv_hdr_buf[15];
 uint16_t mdv_len;
 uint8_t mdv_page;
+static uint8_t mdv_tick;
+static uint8_t mdv_minor;
+
+void mdv_timer(void)
+{
+       if (mdv_tick) {
+               mdv_tick--;
+               if (mdv_tick == 0)
+                       mdv_motor_off();
+       }
+}
 
 static int mdv_transfer(uint8_t minor, bool is_read, uint8_t rawflag)
 {
        int err;
        irqflags_t irq;
        uint16_t block, nblock;
+       uint8_t on = 0;
 
        if (rawflag == 2)
                goto bad;
@@ -46,7 +58,17 @@ static int mdv_transfer(uint8_t minor, bool is_read, uint8_t rawflag)
                mdv_page = 1;
        }
 
-       mdv_motor_on(minor + 1);
+       irq = di();
+       if (mdv_minor != minor) {
+               mdv_tick = 0;
+               mdv_motor_off();
+       }
+       if (mdv_tick == 0)
+               on = 1;
+       mdv_tick = 250;
+       irqrestore(irq);
+       if (on)
+               mdv_motor_on(minor + 1);
 
        while(nblock--) {
                mdv_sector = mdvmap[minor][block++];
@@ -59,8 +81,6 @@ static int mdv_transfer(uint8_t minor, bool is_read, uint8_t rawflag)
                irqrestore(irq);
                mdv_buf += 512;
        }
-       /* Should be timer based for the motor */
-       mdv_motor_off();
        return 0;
 bad:
        udata.u_error = EIO;

diff --git a/Kernel/platform-zx128/devmdv.h b/Kernel/platform-zx128/devmdv.h
index 84fcd1f..699b278 100644 (file)
--- a/Kernel/platform-zx128/devmdv.h
+++ b/Kernel/platform-zx128/devmdv.h
@@ -13,5 +13,8 @@ int mdv_motor_off(void);
 int mdv_bread(void);
 int mdv_bwrite(void);
 
+/* tiner */
+void mdv_timer(void);
+
 #endif /* __DEVMDV_DOT_H__ */
 

diff --git a/Kernel/platform-zx128/main.c b/Kernel/platform-zx128/main.c
index bc79fe3..ec0c630 100644 (file)
--- a/Kernel/platform-zx128/main.c
+++ b/Kernel/platform-zx128/main.c
@@ -3,6 +3,7 @@
 #include <kdata.h>
 #include <printf.h>
 #include <devtty.h>
+#include <devmdv.h>
 
 uint16_t ramtop = PROGTOP;
 
@@ -25,6 +26,7 @@ void platform_idle(void)
 void platform_interrupt(void)
 {
  tty_pollirq();
+ mdv_timer();
  timer_interrupt();
 }