Update FDCMotorTask and FDCMotor

FDCMotorTask was patched to use 64-bit variables, preserve DOR contents when setting the motor bits, and properly set the block device's motor state. FDCMotor now properly checks the current motor status before writing to the DOR or spawning the power-off task, and also preserves the DOR bits.

1 files changed, 20 insertions, 10 deletions

diff --git a/Kernel/BlkDev/DskFDC.HC b/Kernel/BlkDev/DskFDC.HC
index 8a68b21..7517469 100644
--- a/Kernel/BlkDev/DskFDC.HC
+++ b/Kernel/BlkDev/DskFDC.HC
@@ -144,29 +144,39 @@ U0 FDCMotorTask(CBlkDev *bd)
 {
   // Motor timeout task
   F64 timeout = tS()+3.0; // 3 sec timeout
-  U8 dor;
-  while(tS() != timeout) {
+  U64 dor;
+  while(tS()<timeout) {
     if (bd->mtr==FDC_MOTOR_ON)
       return;
     Yield;
   }
   dor=InU8(bd->base0+FDC_DOR);
-  OutU8(bd->base0+FDC_DOR,dor^(1<<4+bd->unit));
+  dor&=~(1<<(4+bd->unit));
+  OutU8(bd->base0+FDC_DOR,dor);
+  bd->mtr=FDC_MOTOR_OFF;
 }
 
 U0 FDCMotor(CBlkDev *bd, Bool onoff)
 {
   // Motor control
   F64 timeout;
-  U8 dor=InU8(bd->base0+FDC_DOR);
+  U64 dor;
   if (onoff) {
-    OutU8(bd->base0+FDC_DOR,dor^(1<<4+bd->unit)|(1<<4+bd->unit));
-    timeout=tS()+0.5;
-    while (tS()<timeout) Yield;
-    bd->mtr=FDC_MOTOR_ON;
+    if (bd->mtr!=FDC_MOTOR_ON) {
+      if (!bd->mtr) {
+        dor=InU8(bd->base0+FDC_DOR);
+        dor|=1<<(4+bd->unit);
+	OutU8(bd->base0+FDC_DOR,dor);
+	timeout=tS()+0.5;
+	while (tS()<timeout) Yield;
+      }
+      bd->mtr=FDC_MOTOR_ON;
+    }
   } else {
-    bd->mtr == FDC_MOTOR_WAIT;
-    Spawn(&FDCMotorTask,bd,"FDC Motor");
+    if (bd->mtr) {
+      bd->mtr == FDC_MOTOR_WAIT;
+      Spawn(&FDCMotorTask,bd,"FDC Motor");
+    }
   }
 }