/*
    New, (hopefully) Less Messy Floppy Driver
    Copyright (C) 2025 Yoshi128k.
    Licensed under version 2 of the Do What The Fuck You Want To Public License
    See COPYING for details.
*/

Bool FDCIrq = FALSE;

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

interrupt U0 FDCIrqHandler() {
  OutU8(0x20, 0x20); // EOI
  FDCIrq = TRUE;
}

U16 fdc_base = 0x03F0;

static U8 *fd_types[8] = {
  "none",
  "360kB 5.25\"",
  "1.2MB 5.25\"",
  "720kB 3.5\"",

  "1.44MB 3.5\"",
  "2.88MB 3.5\"",
  "unknown type",
  "unknown type"
};

U0 CMOSGetFloppyDrives()
{
  OutU8(0x70, 0x10);
  Sleep(1);

  U64 drives = InU8(0x71);

  AdamLog("Floppy Drive 0: %s\n", fd_types[drives >> 4]);
  AdamLog("Floppy Drive 1: %s\n", fd_types[drives & 0xF]);
}

U0 FDCSendCmd(U16 base, U8 cmd)
{
  // Send a command to the floppy controller

  // 60 sec timeout
  U64 i;
  for (i = 0; i < 600; i++) {
    Sleep(10);
    if (0x80 & InU8(base + FDC_MSR_DSR)) {
      OutU8(base + FDC_DATA, cmd);
      Sleep(1);
      return;
    }
  }
  AdamErr("FDC Command TimeOut");
}

U8 FDCReadData(U16 base)
{
  // Read data from the floppy controller

  // 60 sec timeout
  U64 i;
  for (i=0;i<600;i++) {
    Sleep(10);
    if (0x80 & InU8(base + FDC_MSR_DSR)) {
      return InU8(base + FDC_DATA);
    }
  }
  AdamErr("FDC Read TimeOut");
}

U0 FDCCheckInt(U16 base, U8 *st0, U8 *cyl)
{
  FDCSendCmd(base, FDC_SENSE_INTR);

  *st0 = FDCReadData(base);
  *cyl = FDCReadData(base);
}

I16 fdc_mtr_ticks = 0;
U8 fdc_mtr_state = FDC_MOTOR_OFF;

U0 FDCMotorOff(U16 base)
{
  OutU8(base + FDC_DOR, 0x0C);
  fdc_mtr_state = FDC_MOTOR_OFF;
}

U0 FDCMotorPwrOffTimer()
{
  // You're supposed to run this function as a separate CTask. It will exit when the motor
  // exits the "wait" state (either because it has turned off or because something needs it to be on).
  // The driver only spawns this when the motor is moved from "on" to "wait".
  while (fdc_mtr_state == FDC_MOTOR_WAIT) {
    // Sanity check for if something wants the motor on at the last second
    if (fdc_mtr_state == FDC_MOTOR_ON) break;

    Sleep(500);

    fdc_mtr_ticks -= 50;
    if (fdc_mtr_ticks <= 0) {
      FDCMotorOff(fdc_base);
    }
    Yield; // Give control back to the scheduler
  }
}

U0 FDCMotorCtrl(U16 base, Bool onoff)
{
  U64 prev_mtr_state;
  if (onoff) {
    if (!fdc_mtr_state) {
      // Turn on motor
      OutU8(base + FDC_DOR, 0x1C);
      Sleep(500); // Wait 500 ms to allow drive to spin up
    }
    fdc_mtr_state = FDC_MOTOR_ON;
  } else {
    prev_mtr_state = fdc_mtr_state;
    if (fdc_mtr_state == FDC_MOTOR_WAIT) {
      AdamLog("FDC: Motor PowerOff Already Pending\n");
    }
    fdc_mtr_ticks = 300; // 3 sec timeout before motor turns off
    fdc_mtr_state = FDC_MOTOR_WAIT;
    // Only spawn the timer task of the motor is currently on; infinitely-looping tasks peg the CPU
    if (prev_mtr_state != FDC_MOTOR_WAIT) Spawn(&FDCMotorPwrOffTimer,,"FDCMotorPwrOffTimer");
  }
}

I8 FDCRecalibrate(U16 base)
{
  FDCIrq = FALSE;

  U64 i;
  U8 st0, cyl;

  FDCMotorCtrl(base, FDC_MOTOR_ON);

  for (i=0;i<10;i++) {
    FDCSendCmd(base, FDC_RECALIBRATE);
    FDCSendCmd(base, 0);

    // Wait for an interrupt, then get the status
    while (!FDCIrq) Yield;
    FDCCheckInt(base, &st0, &cyl);
    AdamLog("ST0: %d Cyl: %d\n",st0,cyl);

    if (st0 & 0xC0) {
      static U8 * status[4] = {0, "error", "invalid", "drive"};
      AdamLog("Calibration Status: %s\n", status[st0 >> 6]);
    }

    if (!cyl) {
      FDCMotorCtrl(base, FDC_MOTOR_OFF);
      return 0;
    }
  }

  AdamErr("FDC ReCalibrate TimeOut");
  FDCMotorCtrl(base, FDC_MOTOR_OFF);
  return -1;
}

I8 FDCReset(U16 base)
{
  FDCIrq = FALSE;

  OutU8(base + FDC_DOR, 0x00);
  Sleep(1);
  OutU8(base + FDC_DOR, 0x0C);
  Sleep(1);

  while (!FDCIrq) Yield;

  // Ignore this
  U64 i, lck;
  U8 st0, cyl;
  for (i=0;i<4;i++) {
    FDCCheckInt(base, &st0, &cyl);
  }

  // Set xfer rate to 500kbps
  OutU8(base + FDC_CCR_DIR, 0x00);
  Sleep(1);

  // Set the mechanical params and disable DMA (Terry didn't use it)
  FDCSendCmd(base, FDC_SPECIFY);
  FDCSendCmd(base, 0b10000000); // SRT = 8 (8 ms), HUT = 0 (256 ms)
  FDCSendCmd(base, 0b00011110); // HLT = 15 (30 ms), NDMA = 0 (DMA enabled)

  // Configure the FIFO
  FDCSendCmd(base, FDC_CONFIGURE);
  FDCSendCmd(base, 0x00); // 1st param is a 0
  FDCSendCmd(base, 0b01011011); // 2nd param: Implied seek on, FIFO on, Drive polling disabled, threshold = 12
  FDCSendCmd(base, 0x00); // 3rd param: write precomp = 0

  // Lock the configuration
  FDCSendCmd(base, 0x94);
  lck = FDCReadData(base); // Result: lock status

  // contingency
  if (FDCRecalibrate(base)) return -1;

  return 0;
}

I8 FDCSeek(U16 base, U8 cyli, U8 head)
{
  U64 i;
  U8 st0, cyl;
  FDCIrq = FALSE;

  FDCMotorCtrl(base, FDC_MOTOR_ON);

  for (i=0;i<10;i++) {
    // Attempt to move to given cyl
    // 1: X X X X X HD D1 D0
    // 2: Cyl No
    FDCSendCmd(base, FDC_SEEK);
    FDCSendCmd(base, head<<2);
    FDCSendCmd(base, cyli);

    while (!FDCIrq) Yield;
    FDCCheckInt(base, &st0, &cyl);
    AdamLog("ST0: %d Cyl: %d\n",st0,cyl);

    if (st0 & 0xC0) {
      static U8 * status[4] = {0, "error", "invalid", "drive"};
      AdamLog("Seek Status: %s\n", status[st0 >> 6]);
    }

    if (cyl == cyli) {
      FDCMotorCtrl(base, FDC_MOTOR_OFF);
      return 0;
    }
  }

  AdamErr("FDC Seek TimeOut");
  FDCMotorCtrl(base, FDC_MOTOR_OFF);
  return -1;
}

U0 FDCRead(U16 base, U8 cyl, U8 head, U8 sect, U8 trklen)
{
  // Read data from disk (single-track)
  static U8 flags;
  flags = 0x40; // Multi-track off, MFM modulation
  static U64 cmd;
  cmd = FDC_READ_DATA | flags;
  FDCIrq = FALSE;

  U64 st0, st1, st2, rcyl, rhd, rsect, rsectsize;

  FDCMotorCtrl(base, FDC_MOTOR_ON);

  AdamLog("Cmd: %d\nParams: %d %d %d %d %d %d %d %d\n",cmd,head<<2,cyl,head,sect,2,trklen,0x1B,0xFF);

  // Prepare the DMA controller
  AdamLog("Preparing DMA\n");
  FDCDMAInit(512*(trklen - sect + 1));
  FDCDMAPrepRead();

  FDCSendCmd(base, cmd);
  FDCSendCmd(base, head << 2); // Drive 0, specified head
  FDCSendCmd(base, cyl);
  FDCSendCmd(base, head);
  FDCSendCmd(base, sect);
  FDCSendCmd(base, 2); // Sector size = 2 = 512 bytes
  FDCSendCmd(base, trklen); // Track Length/Max Sector No.
  FDCSendCmd(base, 0x1B); // GAP3 Length = 27 (3.5")
  FDCSendCmd(base, 0xFF); // Data Length (irrelevant)

  // Wait for xfer to complete
  while (!FDCIrq) Yield;

  st0 = FDCReadData(base);
  st1 = FDCReadData(base);
  st2 = FDCReadData(base);
  rcyl = FDCReadData(base);
  rhd = FDCReadData(base);
  rsect = FDCReadData(base);
  rsectsize = FDCReadData(base);

  AdamLog("Read Results:\nST0: %d\nST1: %d\nST2: %d\nCyl: %d\nHead: %d\nSect: %d\nSect Size: %d\n",st0,st1,st2,rcyl,rhd,rsect,rsectsize);

  FDCMotorCtrl(base, FDC_MOTOR_OFF);
}


U0 FDCReadMulti(U16 base, U8 cyl, U8 head, U8 sect, U8 trklen)
{
  // Read data from disk (multi-track)
  static U8 flags;
  flags = 0xC0; // Multi-track on, MFM modulation
  static U64 cmd;
  cmd = FDC_READ_DATA | flags;
  FDCIrq = FALSE;

  U64 st0, st1, st2, rcyl, rhd, rsect, rsectsize;

  FDCMotorCtrl(base, FDC_MOTOR_ON);

  AdamLog("Cmd: %d\nParams: %d %d %d %d %d %d %d %d\n",cmd,head<<2,cyl,head,sect,2,trklen,0x1B,0xFF);

  // Prepare the DMA controller
  AdamLog("Preparing DMA\n");
  FDCDMAInit(512*(trklen - sect + 1)*2);
  FDCDMAPrepRead();

  FDCSendCmd(base, cmd);
  FDCSendCmd(base, head << 2); // Drive 0, specified head
  FDCSendCmd(base, cyl);
  FDCSendCmd(base, head);
  FDCSendCmd(base, sect);
  FDCSendCmd(base, 2); // Sector size = 2 = 512 bytes
  FDCSendCmd(base, trklen); // Track Length/Max Sector No.
  FDCSendCmd(base, 0x1B); // GAP3 Length = 27 (3.5")
  FDCSendCmd(base, 0xFF); // Data Length (irrelevant)

  // Wait for the xfer to complete
  while (!FDCIrq) Yield;

  st0 = FDCReadData(base);
  st1 = FDCReadData(base);
  st2 = FDCReadData(base);
  rcyl = FDCReadData(base);
  rhd = FDCReadData(base);
  rsect = FDCReadData(base);
  rsectsize = FDCReadData(base);

  AdamLog("Read Results:\nST0: %d\nST1: %d\nST2: %d\nCyl: %d\nHead: %d\nSect: %d\nSect Size: %d\n",st0,st1,st2,rcyl,rhd,rsect,rsectsize);

  FDCMotorCtrl(base, FDC_MOTOR_OFF);
}

U0 FDCWrite(U16 base, U8 cyl, U8 head, U8 sect, U8 trklen)
{
  // Write data to disk (single-track)
  static U8 flags;
  flags = 0x40; // Multi-track off, MFM modulation
  static U64 cmd;
  cmd = FDC_WRITE_DATA | flags;
  FDCIrq = FALSE;

  U64 st0, st1, st2, rcyl, rhd, rsect, rsectsize;

  FDCMotorCtrl(base, FDC_MOTOR_ON);

  AdamLog("Cmd: %d\nParams: %d %d %d %d %d %d %d %d\n",cmd,head<<2,cyl,head,sect,2,trklen,0x1B,0xFF);

  // Prepare the DMA controller
  AdamLog("Preparing DMA\n");
  FDCDMAInit(512*(trklen - sect + 1));
  FDCDMAPrepWrite();

  FDCSendCmd(base, cmd);
  FDCSendCmd(base, head << 2); // Drive 0, specified head
  FDCSendCmd(base, cyl);
  FDCSendCmd(base, head);
  FDCSendCmd(base, sect);
  FDCSendCmd(base, 2); // Sector size = 2 = 512 bytes
  FDCSendCmd(base, trklen); // Track Length/Max Sector No.
  FDCSendCmd(base, 0x1B); // GAP3 Length = 27 (3.5")
  FDCSendCmd(base, 0xFF); // Data Length (irrelevant)

  // Wait for xfer to complete
  while (!FDCIrq) Yield;

  st0 = FDCReadData(base);
  st1 = FDCReadData(base);
  st2 = FDCReadData(base);
  rcyl = FDCReadData(base);
  rhd = FDCReadData(base);
  rsect = FDCReadData(base);
  rsectsize = FDCReadData(base);

  AdamLog("Read Results:\nST0: %d\nST1: %d\nST2: %d\nCyl: %d\nHead: %d\nSect: %d\nSect Size: %d\n",st0,st1,st2,rcyl,rhd,rsect,rsectsize);

  FDCMotorCtrl(base, FDC_MOTOR_OFF);
}


U0 FDCWriteMulti(U16 base, U8 cyl, U8 head, U8 sect, U8 trklen)
{
  // Write data to disk (multi-track)
  static U8 flags;
  flags = 0xC0; // Multi-track on, MFM modulation
  static U64 cmd;
  cmd = FDC_WRITE_DATA | flags;
  FDCIrq = FALSE;

  U64 st0, st1, st2, rcyl, rhd, rsect, rsectsize;

  FDCMotorCtrl(base, FDC_MOTOR_ON);

  AdamLog("Cmd: %d\nParams: %d %d %d %d %d %d %d %d\n",cmd,head<<2,cyl,head,sect,2,trklen,0x1B,0xFF);

  // Prepare the DMA controller
  AdamLog("Preparing DMA\n");
  FDCDMAInit(512*(trklen - sect + 1)*2);
  FDCDMAPrepRead();

  FDCSendCmd(base, cmd);
  FDCSendCmd(base, head << 2); // Drive 0, specified head
  FDCSendCmd(base, cyl);
  FDCSendCmd(base, head);
  FDCSendCmd(base, sect);
  FDCSendCmd(base, 2); // Sector size = 2 = 512 bytes
  FDCSendCmd(base, trklen); // Track Length/Max Sector No.
  FDCSendCmd(base, 0x1B); // GAP3 Length = 27 (3.5")
  FDCSendCmd(base, 0xFF); // Data Length (irrelevant)

  // Wait for the xfer to complete
  while (!FDCIrq) Yield;

  st0 = FDCReadData(base);
  st1 = FDCReadData(base);
  st2 = FDCReadData(base);
  rcyl = FDCReadData(base);
  rhd = FDCReadData(base);
  rsect = FDCReadData(base);
  rsectsize = FDCReadData(base);

  AdamLog("Read Results:\nST0: %d\nST1: %d\nST2: %d\nCyl: %d\nHead: %d\nSect: %d\nSect Size: %d\n",st0,st1,st2,rcyl,rhd,rsect,rsectsize);

  FDCMotorCtrl(base, FDC_MOTOR_OFF);
}