/*
    Copyright (C) 2025-2026 Harley Travis <yoshi128k@gmail.com>.
    This software (including source code) is licensed under the BSD Zero Clause
    License. See the Copying.TXT file for details.
*/

interrupt U0 FDCInt()
{
  // On IRQ6, set a semaphore for anything waiting for an FDC int
  fdc_int_semaphore = TRUE;
  OutU8(0x20,0x20); // Send EOI to PIC
}

U0 FDCDMAInit(U16 len)
{
  U64 buf_lo, buf_hi, page, cnt_lo, cnt_hi;

  buf_lo = &FDC_DMA & 0xFF;
  buf_hi = &FDC_DMA >> 8;
  page = &FDC_DMA >> 16;
  cnt_lo = (len - 1) & 0xFF;
  cnt_hi = (len - 1) >> 8;

  OutU8(0x0A, 6); // mask ch 0 and 2
  Sleep(1);
  OutU8(0x0C, -1); // reset flip flop
  Sleep(1);
  OutU8(0x04, buf_lo); // buf low byte
  Sleep(1);
  OutU8(0x04, buf_hi); // buf high byte
  Sleep(1);
  OutU8(0x0C, -1); // reset flip flop again
  Sleep(1);
  OutU8(0x05, cnt_lo); // cntr low byte
  Sleep(1);
  OutU8(0x05, cnt_hi); // cntr high byte
  Sleep(1);
  OutU8(0x81, 0); // page
  Sleep(1);
  OutU8(0x0A, 2); // unmask ch 0 and 2
  Sleep(1);
}

U0 FDCDMAPrepWrite()
{
  OutU8(0x0A, 6); // mask ch 0 and 2
  Sleep(1);
  OutU8(0x0B, 0x5A); // single xfer, addr inc, auto init, write, ch 2
  Sleep(1);
  OutU8(0x0A, 2); // unmask ch 0 and 2
  Sleep(1);
}

U0 FDCDMAPrepRead()
{
  OutU8(0x0A, 6); // mask ch 0 and 2
  Sleep(1);
  OutU8(0x0B, 0x56); // single xfer, addr inc, auto init, read, ch 2
  Sleep(1);
  OutU8(0x0A, 2); // unmask ch 0 and 2
  Sleep(1);
}

U8 FDCReadByte(CBlkDev *bd)
{
  // Read byte from FDC.

  U8 byte; // The byte read
  F64 timeout; // Timeout variable
  timeout = tS()+3.0; // 3 second timeout

  while (TRUE) {
    if (InU8(bd->base0+FDC_MSR_DSR)>>6==3) {
      byte=InU8(bd->base0+FDC_DATA);
      return byte;
    }
    if (tS()>timeout) {
      throw('BlkDev');
    }
  }
}

U0 FDCSendByte(CBlkDev *bd,U8 byte)
{
  // Send byte to FDC.

  F64 timeout; // Timeout variable
  timeout=tS()+3.0; // 3 second timeout

  while (TRUE) {
    if (InU8(bd->base0+FDC_MSR_DSR)>>6==2) {
      OutU8(bd->base0+FDC_DATA,byte);
      return;
    }
    if (tS()>timeout) {
      throw('BlkDev');
    }
  }
}

U0 FDCReset(CBlkDev *bd)
{
  // Reset the FDC

  fdc_int_semaphore=FALSE;

  // Twiddle the reset bit in the DSR
  OutU8(bd->base0+FDC_MSR_DSR,0x80|bd->bps);

  // Wait for the int
  while (!fdc_int_semaphore)
    Yield;

  // Send 4 SISes
  for (i=0,i<4,i++) {
    FDCSendByte(bd,FDC_SENSE_INTR)
    FDCReadByte(bd);
    FDCReadByte(bd);
  }

  // Send "CONFIGURE" to fix FDC cfg
  FDCSendByte(bd,FDC_CONFIGURE);
  FDCSendByte(bd,0); // Null byte
  FDCSendByte(bd,0b01010111); // Implied Seek, FIFO, No Polling, Threshold 8
  FDCSendByte(bd,0); // No Write Precomp

  // Select our drive
  FDCSelDrv(bd);
}

U0 FDCMotorTask(CBlkDev *bd)
{
  // Motor timeout task
  F64 timeout = tS()+3.0; // 3 sec timeout
  U8 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));
}

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;
    Spawn(&FDCMotorTask,bd,"FDC Motor");
  }
}

U0 FDCSelDrv(CBlkDev *bd)
{
  // Select the drive assigned to this blkdev

  // Set the CCR appropriately
  OutU8(bd->base0+FDC_CCR_DSR,bd->bps);

  // Send a "SPECIFY" command
  FDCSendByte(bd,FDC_SPECIFY);
  FDCSendByte(bd,bd->srt<<4|bd->hut); // Step Rate Time, Head Unload Time
  FDCSendByte(bd,bd->hlt<<1|!bd->dma); // Head Load Time, DMA

  // Set drv sel in DOR (and enable DMA/IRQs)
  OutU8(bd->base0+FDC_DOR,8|bd->unit);
}

Bool FDCInit(CBlkDev *bd)
{
  // Recalibrate the drive and (on the first drive) initialize the FDC

  U8 ver,st0,pcn;
  Bool unlock=BlkDevLock(bd),okay=FALSE;

  // If this drv is unit 0, do initialization
  if (!bd->unit) {
    // Check if the controller is 82077AA-compatible
    FDCSendByte(bd,FDC_VERSION);
    ver=FDCReadByte(bd);

    if (ver==0x90)
      okay=TRUE;
  
    FDCReset(bd);
  }

  recalibrate:
  // Recalibrate this drive
  fdc_irq_semaphore=FALSE;
  FDCSendByte(bd,FDC_RECALIBRATE);
  FDCSendByte(bd,bd->unit); // Drive number

  // Wait for an IRQ
  while (!fdc_irq_semaphore) Yield;

  // Get the result of the recalibration
  FDCSendByte(bd,FDC_SENSE_INTR);
  st0=FDCReadByte(bd);
  pcn=FDCReadByte(bd);

  if (st0>>6) okay=FALSE;

  // 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;

  bd->max_blk=(bd->cyls*bd->heads*bd->spt)-1;

  if (unlock) BlkDevUnlock(bd);
  return okay;
}

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(length);
  FDCDMAPrepWrite();

  FDCSendByte(bd,FDC_WRITE_DATA|(bd->heads>1<<7)|mfm<<6|);
  FDCSendByte(bd,cyl);
  FDCSendByte(bd,head);
  FDCSendByte(bd,start);
  FDCSendByte(bd,bd->blk_size>>8);
  FDCSendByte(bd,end);
  FDCSendByte(bd,bd->gpl1);
  FDCSendByte(bd,255);

  while(!fdc_int_semaphore) Yield;

  st0=FDCReadByte(bd);
  st1=FDCReadByte(bd);
  st2=FDCReadByte(bd);
  pcn=FDCReadByte(bd);
  hd=FDCReadByte(bd);
  sect=FDCReadByte(bd);
  size=FDCReadByte(bd);

  return st0;
}

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(length);
  FDCDMAPrepRead();

  FDCSendByte(bd,FDC_READ_DATA|mfm<<6|);
  FDCSendByte(bd,cyl);
  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);

  while(!fdc_int_semaphore) Yield;

  st0=FDCReadByte(bd);
  st1=FDCReadByte(bd);
  st2=FDCReadByte(bd);
  pcn=FDCReadByte(bd);
  hd=FDCReadByte(bd);
  sect=FDCReadByte(bd);
  size=FDCReadByte(bd);

  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;
  Bool unlock=BlkDevLock(bd);

  retry:
  if (retries==0) throw('BlkDev');

  FDCSelDrv(bd);

  c=blk/(bd->heads*bd->spt);
  h=blk/bd->spt%heads;
  s=blk%bd->spt+1;

  st0=FDCReadData(bd,c,h,s,s+cnt-1);

  if (st0>>6) {
    retries-=1;
    goto retry;
  }

  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;
  }
}