Update FDCChk

FDCChk now turns the motor on and off, and tests for the presence of a disk by performing two seeks and checking the disk change flag.

1 files changed, 42 insertions, 5 deletions

diff --git a/Kernel/BlkDev/DskFDC.HC b/Kernel/BlkDev/DskFDC.HC
index 07020ee..3b2cac8 100644
--- a/Kernel/BlkDev/DskFDC.HC
+++ b/Kernel/BlkDev/DskFDC.HC
@@ -444,22 +444,59 @@ U8 FDCReadId(CBlkDev *bd,Bool hd)
 
 U8 FDCChk(CBlkDev *bd)
 {
-  // Check if there is a disk of the selected density in the drive
-
+  // Check if there is a disk in the drive
+  U64 status,trk=1,dir;
   Bool unlock=BlkDevLock(bd),h0,h1;
 
   FDCSelDrv(bd);
+  FDCMotor(bd,ON);
+
+  // Seek/Disk Change test
+  // The "read ID" cmd will wait forever for a disk to be inserted before it
+  // attempts to read a sector ID. This is stupid, so I decided to do a test
+  // seek and then check the Disk Change bit of the DIR. If set, we will bail
+  // out and return 0. Otherwise, we continue with the ID read.
+  seek:
+  fdc_int_semaphore=FALSE;
+
+  FDCSendByte(bd,FDC_SEEK);
+  FDCSendByte(bd,bd->unit);
+  FDCSendByte(bd,trk);
+
+  while (!fdc_int_semaphore) Yield;
+
+  FDCSendByte(bd,FDC_SENSE_INTR);
+  FDCReadByte(bd);
+  FDCReadByte(bd);
 
+  if (trk) {
+    trk--;
+    goto seek;
+  }
+
+  dir=InU8(bd->base0+FDC_CCR_DIR);
+
+  if (dir&0x80) {
+    status=FDC_CHK_ERR;
+    goto done;
+  }
+
+  // If we got this far, do the ID reads.
   h0=!FDCReadId(bd,0);
   h1=!FDCReadId(bd,1);
 
   if (h0) {
     if (h1) {
-      return FDC_CHK_DS;
+      status=FDC_CHK_DS;
     } else {
-      return FDC_CHK_SS;
+      status=FDC_CHK_SS;
     }
   } else {
-    return FDC_CHK_ERR;
+    status=FDC_CHK_ERR;
   }
+
+  done:
+  FDCMotor(bd,OFF);
+  if (unlock) BlkDevUnlock(bd);
+  return status;
 }
\ No newline at end of file