1 files changed, 99 insertions, 42 deletions

diff --git a/Kernel/BlkDev/DskFDC.HC b/Kernel/BlkDev/DskFDC.HC
index 8c08778..c7ea0e3 100644
--- a/Kernel/BlkDev/DskFDC.HC
+++ b/Kernel/BlkDev/DskFDC.HC
@@ -145,9 +145,12 @@ U0 FDCMotorTask(CBlkDev *bd)
 U0 FDCMotor(CBlkDev *bd, Bool onoff)
 {
   // Motor control
+  F64 timeout;
   U8 dor=InU8(bd->base0+FDC_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;
   } else {
     bd->mtr == FDC_MOTOR_WAIT;
@@ -171,21 +174,23 @@ U0 FDCSelDrv(CBlkDev *bd)
   OutU8(bd->base0+FDC_DOR,8|bd->unit);
 }
 
-U0 FDCInit(CBlkDev *bd)
+Bool FDCInit(CBlkDev *bd)
 {
   // Initialize the FDC assigned to this blkdev
 
   U8 ver,st0,pcn;
-  Bool unlock=BlkDevLock(bd);
+  Bool unlock=BlkDevLock(bd),okay=FALSE;
 
   // Check if the controller is 82077AA-compatible
   FDCSendByte(bd,FDC_VERSION);
   ver=FDCReadByte(bd);
 
-  if (ver!=0x90) throw('BlkDev');
-
-  // Reset the controller
-  FDCReset(bd);
+  if (ver==0x90)
+    okay=TRUE;
+  
+  // If this drv is unit 0, do a reset
+  if (!bd->unit)
+    FDCReset(bd);
 
   recalibrate:
   // Recalibrate this drive
@@ -201,48 +206,34 @@ U0 FDCInit(CBlkDev *bd)
   st0=FDCReadByte(bd);
   pcn=FDCReadByte(bd);
 
-  if (st0>>6) throw('BlkDev');
-  if (!(st0&0x20)) goto recalibrate;
+  if (st0>>6) okay=FALSE;
 
-  if (unlock) BlkDevUnlock(bd);
-}
+  // This should only be needed for disks with more than 80 tracks,
+  // in which case we will do another recalibration
+  if (!(st0&0x20)) goto recalibrate;
 
-U0 FDCRBlks(CDrv *dv,U8 *buf,I64 blk,I64 cnt)
-{
-  I64 n;
-  CBlkDev *bd=dv->bd;
-  U8 s,mts;
+  bd->max_blk=(bd->cyls*bd->heads*bd->spt)-1;
 
-  while (cnt>0) {
-    s=(blk%(bd->spt))+1; // sector
-    mts=(blk%(bd->spt*2))+1; // multi-track sector
-    n=cnt;
-    if (bd->heads>1 && n>bd->max_reads-mts+1) {
-      n=bd->max_reads-mts+1;
-    } else if (n>bd->max_reads-s+1) {
-      n=bd->max_reads-s+1;
-    }
-    FDCReadBlks(bd,buf,blk,n);
-    buf+n<<BLK_SIZE_BITS;
-    blk+=n;
-    cnt-=n;
-  }
+  if (unlock) BlkDevUnlock(bd);
+  return okay;
 }
 
-U8 FDCReadCyl(CBlkDev *bd,U8 cyl)
+U8 FDCWriteData(CBlkDev *bd,U8 cyl,Bool head,U8 start,U8 end)
 {
+  U16 length=(end-start+1)*bd->blk_size;
   U8 st0,st1,st2,pcn,hd,sect,size
 
   fdc_irq_semaphore=FALSE;
 
-  FDCDMAInit((bd->spt*bd->heads)*bd->blk_size);
+  FDCDMAInit(length);
   FDCDMAPrepWrite();
 
   FDCSendByte(bd,FDC_WRITE_DATA|(bd->heads>1<<7)|mfm<<6|);
   FDCSendByte(bd,cyl);
-  FDCSendByte(bd,0);
-  FDCSendByte(bd,1);
+  FDCSendByte(bd,head);
+  FDCSendByte(bd,start);
   FDCSendByte(bd,bd->blk_size>>8);
+  FDCSendByte(bd,end);
   FDCSendByte(bd,bd->gpl1);
   FDCSendByte(bd,255);
 
@@ -259,20 +250,22 @@ U8 FDCReadCyl(CBlkDev *bd,U8 cyl)
   return st0;
 }
 
-U8 FDCReadCyl(CBlkDev *bd,U8 cyl)
+U8 FDCReadData(CBlkDev *bd,U8 cyl,Bool head,U8 start,U8 end)
 {
+  U16 length=(end-start+1)*bd->blk_size;
   U8 st0,st1,st2,pcn,hd,sect,size
 
   fdc_irq_semaphore=FALSE;
 
-  FDCDMAInit((bd->spt*bd->heads)*bd->blk_size);
+  FDCDMAInit(length);
   FDCDMAPrepRead();
 
-  FDCSendByte(bd,FDC_READ_DATA|(bd->heads>1<<7)|mfm<<6|);
+  FDCSendByte(bd,FDC_READ_DATA|mfm<<6|);
   FDCSendByte(bd,cyl);
-  FDCSendByte(bd,0);
-  FDCSendByte(bd,1);
+  FDCSendByte(bd,head);
+  FDCSendByte(bd,start); // sector is ignored
   FDCSendByte(bd,bd->blk_size>>8);
+  FDCSendByte(bd,end);
   FDCSendByte(bd,bd->gpl1);
   FDCSendByte(bd,255);
 
@@ -289,10 +282,29 @@ U8 FDCReadCyl(CBlkDev *bd,U8 cyl)
   return st0;
 }
 
+U0 FDCRBlks(CDrv *dv,U8 *buf,I64 blk,I64 cnt)
+{
+  I64 n;
+  CBlkDev *bd=dv->bd;
+  U8 s;
+
+  while (cnt>0) {
+    s=(blk%(bd->spt))+1; // sector
+    n=cnt;
+    if (n>(bd->max_reads-s+1)) {
+      n=bd->max_reads-s+1;
+    }
+    FDCReadBlks(bd,buf,blk,n);
+    buf+n<<BLK_SIZE_BITS;
+    blk+=n;
+    cnt-=n;
+  }
+}
+
 U0 FDCReadBlks(CBlkDev *bd,U8 *buf,I64 blk,I64 cnt)
 {
   I64 retries=3;
-  U8 c,h,s,st0,st1,st2,pcn,hd,sect,size;
+  U8 c,h,s,st0;
   Bool unlock=BlkDevLock(bd);
 
   retry:
@@ -301,14 +313,59 @@ U0 FDCReadBlks(CBlkDev *bd,U8 *buf,I64 blk,I64 cnt)
   FDCSelDrv(bd);
 
   c=blk/(bd->heads*bd->spt);
-  s=(blk%bd->spt)+1;
+  h=blk/bd->spt%heads;
+  s=blk%bd->spt+1;
 
-  FDCReadCyl(bd,c);
+  st0=FDCReadData(bd,c,h,s,s+cnt-1);
 
   if (st0>>6) {
     retries-=1;
     goto retry;
   }
 
-  MemCpy(buf,&FDC_DMA+(c*bd->blk_size),cnt);
+  MemCpy(buf,&FDC_DMA,cnt*bd->blk_size);
+}
+
+U0 FDCWBlks(CDrv *dv,U8 *buf,I64 blk,I64 cnt)
+{
+  I64 n;
+  CBlkDev *bd=dv->bd;
+  U8 s;
+
+  while (cnt>0) {
+    s=(blk%(bd->spt))+1; // sector
+    n=cnt;
+    if (n>(bd->max_reads-s+1)) {
+      n=bd->max_reads-s+1;
+    }
+    FDCWriteBlks(bd,buf,blk,n);
+    buf+n<<BLK_SIZE_BITS;
+    blk+=n;
+    cnt-=n;
+  }
 }
+
+U0 FDCWriteBlks(CBlkDev *bd,U8 *buf,I64 blk,I64 cnt)
+{
+  I64 retries=3;
+  U8 c,h,s,st0;
+  Bool unlock=BlkDevLock(bd);
+
+  retry:
+  if (retries==0) throw('BlkDev');
+
+  MemCpy(&FDC_DMA,buf,cnt);
+
+  FDCSelDrv(bd);
+
+  c=blk/(bd->heads*bd->spt);
+  h=blk/bd->spt%heads;
+  s=blk%bd->spt+1;
+
+  st0=FDCWriteData(bd,c,h,s,s+cnt-1);
+
+  if (st0>>6) {
+    retries-=1;
+    goto retry;
+  }
+}
\ No newline at end of file