/**************************************************************************************
 Copyright (c) 2002 The UbixOS Project
 All rights reserved.

 Redistribution and use in source and binary forms, with or without modification,
 are prohibited.

 $Id$

**************************************************************************************/

#include <drivers/fdc.h>
#include <drivers/8259.h>
#include <drivers/video.h>
#include <ubixos/gdt.h>
#include <ubixos/idt.h>
#include <ubixos/types.h>
#include <ubixos/io.h>
#include <ubixos/dma.h>

static volatile bool done = FALSE;
static drvGeom geometry = { dg144Heads,dg144Tracks,dg144Spt };
static bool diskChange = FALSE;
static bool motor = FALSE;
static int mTick = 0;
static byte fdcTrack = 0xff;
static byte sr0 = 0;
static volatile int timeOut = 0;
static byte statSize = 0;
static byte status[7] = { 0 };

unsigned long tbaddr = 0x80000L;

void initFloppy() {
  setVector(floppyIsr, mVec+6, (dInt+dPresent+dDpl1));
  enableIrq(6);
  reset();
  }

asm(
  ".globl floppyIsr      \n"
  "floppyIsr:            \n"
  "  pusha               \n"
  "  pushw %ds           \n"
  "  pushw %es           \n"
  "  pushw %ss           \n"
  "  pushw %ss           \n"
  "  popw %ds            \n"
  "  popw %es            \n"
  "  call floppyIsrhndlr \n"
  "  popw %es            \n"
  "  popw %ds            \n"
  "  popa                \n"
  "  iret                \n"
  );

void floppyIsrhndlr() {
  done = TRUE;
  outportByte(0x20,0x20);
  }

void sendByte(int byte) {
  volatile int msr;
  int tmo;
  for (tmo=0;tmo<128;tmo++) {
    msr = inportByte(fdcMsr);
    if ((msr & 0xc0) == 0x80) {
      outportByte(fdcData,byte);
      return;
      }
    inportByte(0x80);
    }
  }

int getByte() {
  volatile int msr;
  int tmo;
  for (tmo=0;tmo<128;tmo++) {
    msr = inportByte(fdcMsr);
    if ((msr & 0xd0) == 0xd0) {
      return inportByte(fdcData);
      }
    inportByte(0x80);
    }
  return(-1);
  }

bool fdcRw(int block,byte *blockBuffer,bool read,unsigned long numSectors) {
  int head,track,sector,tries, copyCount = 0;
  unsigned char *p_tbaddr = (char *)0x80000;
  unsigned char *p_blockbuff = blockBuffer;
  block2Hts(block,&head,&track,&sector);
  motorOn();
  if (!read && blockBuffer) {
    /* copy data from data buffer into track buffer */
    for (copyCount=0; copyCount<(numSectors*512); copyCount++) {
      *p_tbaddr = *p_blockbuff;
      p_blockbuff++;
      p_tbaddr++;
      }
    }
  for (tries = 0;tries < 3;tries++) {
    if (inportByte(fdcDir) & 0x80) {
      diskChange = TRUE;
      seek(1);  /* clear "disk change" status */
      recalibrate();
      motorOff();
      kprint("FDC: Disk change detected. Trying again.\n");
      return fdcRw(block, blockBuffer, read, numSectors);
      }
    if (!seek(track)) {
      motorOff();
      kprint("FDC: Error seeking to track\n");
      return FALSE;
      }
    outportByte(fdcCcr,0);
    if (read) {
      dmaXfer(2,tbaddr,numSectors*512,FALSE);
      sendByte(cmdRead);
      }
    else {
      dmaXfer(2,tbaddr,numSectors*512,TRUE);
      sendByte(cmdWrite);
      }
    sendByte(head << 2);
    sendByte(track);
    sendByte(head);
    sendByte(sector);
    sendByte(2);               /* 512 bytes/sector */
    sendByte(geometry.spt);
    if (geometry.spt == dg144Spt) {
      sendByte(dg144Gap3rw);  /* gap 3 size for 1.44M read/write */
      }
    else {
      sendByte(dg168Gap3rw);  /* gap 3 size for 1.68M read/write */
      }
    sendByte(0xff);            /* DTL = unused */
    if (!waitFdc(TRUE)) {
      kprint("Timed out, trying operation again after reset()\n");
      reset();
      return fdcRw(block, blockBuffer, read, numSectors);
      }
    if ((status[0] & 0xc0) == 0) break;   /* worked! outta here! */
    recalibrate();  /* oops, try again... */
    }
  motorOff();
  if (read && blockBuffer) {
    p_blockbuff = blockBuffer;
    p_tbaddr = (char *) 0x80000;
    for (copyCount=0; copyCount<(numSectors*512); copyCount++) {
      *p_blockbuff = *p_tbaddr;
      p_blockbuff++;
      p_tbaddr++;
      }
    }
  return (tries != 3);
  }

void block2Hts(int block,int *head,int *track,int *sector) {
  *head = (block % (geometry.spt * geometry.heads)) / (geometry.spt);
  *track = block / (geometry.spt * geometry.heads);
  *sector = block % geometry.spt + 1;
  }

void motorOn(void) {
  if (!motor) {
    mTick = -1;     /* stop motor kill countdown */
    outportByte(fdcDor,0x1c);
    motor = TRUE;
    }
  }

void motorOff(void) {
  if (motor) {
    mTick = 13500;
    }
  }

bool seek(int track) {
  if (fdcTrack == track) {
    return(TRUE);
    }
  sendByte(cmdSeek);
  sendByte(0);
  sendByte(track);
  if (!waitFdc(TRUE)) {
    return(FALSE);
    }
  if ((sr0 != 0x20) || (fdcTrack != track)) {
    return(FALSE);
    }
  else {
    return(TRUE);
    }
  }

bool readBlock(int block,byte *blockBuffer, unsigned long numSectors) {
  int track=0, sector=0, head=0, track2=0, result=0, loop=0;
  block2Hts(block, &head, &track, &sector);
  block2Hts(block+numSectors, &head, &track2, &sector);
  if (track!=track2) {
    for (loop=0; loop<numSectors; loop++) {
      result = fdcRw(block+loop, blockBuffer+(loop*512), TRUE, 1);
      }
    return result;
    }
  return fdcRw(block,blockBuffer,TRUE,numSectors);
  }

bool writeBlock(int block,byte *blockBuffer, unsigned long numSectors) {
  return fdcRw(block,blockBuffer,FALSE, numSectors);
  }

bool waitFdc(bool sensei) {
  timeOut = 50000;
  while (!done && timeOut);
  statSize = 0;
  while ((statSize < 7) && (inportByte(fdcMsr) & (1<<4))) {
    status[statSize++] = getByte();
    }
  if (sensei) {
    sendByte(cmdSensei);
    sr0 = getByte();
    fdcTrack = getByte();
    }
  done = FALSE;
  if (!timeOut) {
    if (inportByte(fdcDir) & 0x80) {
      diskChange = TRUE;
      }
    return(FALSE);
    }
  else {
    return(TRUE);
    }
  }

void recalibrate(void) {
  motorOn();
  sendByte(cmdRecal);
  sendByte(0);
  waitFdc(TRUE);
  motorOff();
  }

void reset(void) {
  outportByte(fdcDor,0);
  mTick = 0;
  motor = FALSE;
  outportByte(fdcDrs,0);
  outportByte(fdcDor,0x0c);
  done = TRUE;
  waitFdc(TRUE);
  sendByte(cmdSpecify);
  sendByte(0xdf);
  sendByte(0x02);
  seek(1);
  recalibrate();
  diskChange = FALSE;
  }