/***** floppy.c ******************************************************
 * (c) 2010 CTA Systems Inc. All rights reserved. Glory To God       *
 *                                                                   *
 *         Floppy Drive I/O Routines                                 *
 *         =========================                                 *
 *                                                                   *
 ************************************************************ cta os */
#include <system.h>
#include <conio.h>
#include <time.h>
#include "dma.h"
#include "storage.h"

unsigned char CmosReadFloppyData ()
{
      outportb (0x70, 0x10);
      return inportb(0x71);
}

unsigned char FloppyDrivesInstalled;
volatile unsigned char FloppyNewInterrupt;


typedef struct {
     unsigned char Type;
     unsigned Size;
     unsigned char SectorsPerTrack;
     unsigned char Heads;
     unsigned char Tracks;
     unsigned char DataRate;
     unsigned char StepRateTime;
     unsigned char HeadLoadTime;
     unsigned char HeadUnloadTime;
     unsigned char Gap3;
     unsigned char Gap3Format;
} FloppyType;

FloppyType fd[2];

void FloppyReset();
enum FloppyRegisters {
    FloppyRegDOR = 0x3f2,
    FloppyRegMSR = 0x3f4,
    FloppyRegFIFO = 0x3f5,
    FloppyRegCTRL = 0x3f7
};

enum FloppyCommands {
    FloppyCommandReadTrack = 2,
    FloppyCommandSpecify = 3,
    FloppyCommandCheckStatus = 4,
    FloppyCommandWriteSector = 5,
    FloppyCommandReadSector = 6,
    FloppyCommandCalibrate = 7,
    FloppyCommandCheckInt = 8,
    FloppyCommandFormatTrack = 0xd,
    FloppyCommandSeek = 0xf,

    FloppyCommandExtSkip = 0x20,
    FloppyCommandExtDensity = 0x40,
    FloppyCommandExtMultiTrack = 0x80
};

// Initialize DMA
unsigned char FloppyInitializeDMA(unsigned char* buffer, unsigned length)
{
    union { unsigned char byt[4]; // Low[0], Mid[1], Hi[2]
            unsigned long l;
    } a, c;

    a.l = (unsigned) buffer;
    c.l = (unsigned) length-1;

    // Check for buffer issues
    if ((a.l >> 24) || (c.l >> 16) || (((a.l & 0xffff)+c.l) >> 16)) return 0;

    DMAReset();
    DMAMaskChannel(2);                          // Mask channel 2
    DMAResetFlipFlop(1);                        // FlipFlop reset on DMA1

    DMASetAddress(2, a.byt[0], a.byt[1]); // Buffer address
    DMAResetFlipFlop(1);                  // FlipFlop reset on DMA2

    DMASetCount(2, c.byt[0], c.byt[1]);   // Set count
    DMASetRead(2);
    DMAUnmaskAll();

    return 1;
}




inline void FloppyDisableController() {
    outportb (FloppyRegDOR, 0);
}

inline void FloppyEnableController() {
    outportb (FloppyRegDOR, 4 | 8);
}


inline unsigned char FloppySendCommand (unsigned char command)
{
    int i;
    for (i = 0; i < 750; i++)
        if (inportb(FloppyRegMSR) & 128) {
            outportb(FloppyRegFIFO, command); return 1;
        }
    return 0;
}


inline unsigned char FloppyReadData ()
{
    int i;
    for (i = 0; i < 750; i++)
        if (inportb(FloppyRegMSR) & 0x80)
            return inportb(FloppyRegFIFO);
    return 0;
}


inline void FloppyCheckInt(unsigned* st0, unsigned* cyl)
{
    int t;
    for (t=3; t>=0; t--) {
        if (!FloppySendCommand(0x8)) FloppyReset();
        else break;
    }
    for (t=50; !(inportb(FloppyRegMSR) & 0x80) && t>0; --t);

    *st0 = FloppyReadData();
    *cyl = FloppyReadData();
}


inline unsigned char FloppyWaitIRQ()
{
    unsigned tick = ClockGetTickCount();
    unsigned freq = PitGetFrequency();
    tick = tick + (freq * 3);               // Wait 3 seconds
    
    while (FloppyNewInterrupt==0)
           if (tick <= ClockGetTickCount()) return 0;  // timeout

    FloppyNewInterrupt = 0;
    return 1;
}


void FloppyMotor (unsigned char drive, unsigned char on)
{
    if (drive >= FloppyDrivesInstalled) return;

    // Read DOR register
    unsigned char dor = inportb(FloppyRegDOR);

    // Un/set selected drive motor
    if (on) dor |= 1 << (4+drive);
    else dor &= ~(1 << (4+drive));

    // Write DOR
    outportb (FloppyRegDOR, dor);

    // Wait a fifth of a second for motor to turn on
    unsigned temp = ClockGetTickCount();
    unsigned freq = PitGetFrequency();
    while (temp + (freq/5) > ClockGetTickCount());
}


void i86_FloppyHandler(ISR_stack_regs *r)
{
    FloppyNewInterrupt = 1;
}


void FloppyDriveData (unsigned char drv, unsigned char dma)
{
    unsigned data = 0;
    if (drv >= FloppyDrivesInstalled) return;

    outportb(FloppyRegCTRL, fd[drv].DataRate);

    FloppySendCommand (0x3);

    data = ((fd[drv].StepRateTime & 0xf) << 4) | (fd[drv].HeadUnloadTime & 0xf);
    FloppySendCommand (data);

    data = (fd[drv].HeadLoadTime <<1 ) | (dma) ? 1 : 0;
    FloppySendCommand (data);
}


inline void FloppySelect(unsigned char drive)
{
      if (drive >= FloppyDrivesInstalled) return;

      unsigned char dor = inportb(FloppyRegDOR) & 0xf0;

      // Send mechanical drive data
      FloppyDriveData(drive, 1);

      // Select drive in DOR register
      outportb (FloppyRegDOR, dor | 4 | 8 | drive);

}


unsigned char FloppyCalibrate(unsigned drive)
{
    unsigned st0, cyl;

    if (drive >= FloppyDrivesInstalled) return 0;

    FloppyMotor (drive, 1);

    int i;
    for (i = 0; i < 15; i++) {
        FloppyNewInterrupt = 0;
        FloppySendCommand(FloppyCommandCalibrate);
        FloppySendCommand(drive);
        FloppyWaitIRQ();
        FloppyCheckInt(&st0, &cyl);

        if (!cyl) {
            FloppyMotor(drive, 0);
            return 1;
        }
    }
    FloppyMotor(drive, 0);
    return 0;
}


void FloppyReset()
{
    unsigned st0, cyl;

    FloppyNewInterrupt = 0;
    FloppyDisableController();
    FloppyEnableController();
    FloppyWaitIRQ();
   
    int i;
    for (i = 0; i < 4; i++)
        FloppyCheckInt(&st0, &cyl);

    unsigned char drive;
    for (drive = 0; drive < FloppyDrivesInstalled; drive++) {
      FloppyDriveData(drive, 1);
      FloppyCalibrate(drive);
    }
}


unsigned FloppyReadSectorImp (unsigned* where, unsigned char drive, unsigned char head, unsigned char track, unsigned char sector)
{
    int t, fail;
    for (t=3; t>=0; t--) {
        fail = 0;
        FloppySelect (drive);
        FloppyInitializeDMA((unsigned char*) where, 512);
        DMASetRead(2);

        FloppyNewInterrupt = 0;

        fail += 1-FloppySendCommand(0x06 | 0x80 | 0x40 );
        fail += 1-FloppySendCommand(head<<2 | drive);
        fail += 1-FloppySendCommand(track);
        fail += 1-FloppySendCommand(head);
        fail += 1-FloppySendCommand(sector);
        fail += 1-FloppySendCommand(2);
        fail += 1-FloppySendCommand( ((sector+1) >= fd[drive].SectorsPerTrack) ? fd[drive].SectorsPerTrack : sector+1);
        fail += 1-FloppySendCommand(fd[drive].Gap3);
        fail += 1-FloppySendCommand(0xff);

        if (fail) {
            FloppyReset(); continue;
        }

        FloppyWaitIRQ();
        while (!(inportb(FloppyRegMSR) & 0x80));

        int i; unsigned ccc=0;
        
        for (i = 0; i < 7; i++) {
            if (i<3) ccc |= FloppyReadData() << (i*4);
            else FloppyReadData();
        }
        
        return ccc;
    }
    return 0xFFFFFFFF;
}


unsigned FloppyWriteSectorImp (unsigned* where, unsigned char drive, unsigned char head, unsigned char track, unsigned char sector)
{
    int t, fail;
    for (t=3; t>=0; t--) {
        fail = 0;
        FloppySelect (drive);
        FloppyInitializeDMA((unsigned char*) where, 512);
        DMASetWrite(2);

        FloppyNewInterrupt = 0;

        fail += 1-FloppySendCommand(0x05 | 0x80 | 0x40 );
        fail += 1-FloppySendCommand(head<<2 | drive);
        fail += 1-FloppySendCommand(track);
        fail += 1-FloppySendCommand(head);
        fail += 1-FloppySendCommand(sector);
        fail += 1-FloppySendCommand(2);
        fail += 1-FloppySendCommand( ((sector+1) >= fd[drive].SectorsPerTrack) ? fd[drive].SectorsPerTrack : sector+1);
        fail += 1-FloppySendCommand(fd[drive].Gap3);
        fail += 1-FloppySendCommand(0xff);

        if (fail) {
            FloppyReset(); continue;
        }
        
        FloppyWaitIRQ();

        int i; unsigned ccc=0;
        for (i = 0; i < 7; i++)
            if (i<3) ccc |= FloppyReadData() << (i*4);
            else FloppyReadData();

        return ccc;
    }
    return 0xFFFFFFFF;
}

unsigned char FloppySeek (unsigned drive, unsigned cyl, unsigned head)
{
    unsigned st0, cyl0;

    if (drive >= FloppyDrivesInstalled) return 0;

    FloppySelect (drive);

    int i;
    for (i = 0; i < 20; i++) {
        FloppyNewInterrupt = 0;
        FloppySendCommand (0xF);
        FloppySendCommand ( (head) << 2 | drive);
        FloppySendCommand (cyl);

        FloppyWaitIRQ();
        FloppyCheckInt(&st0, &cyl0);

        if (cyl0 == cyl) return 1;
    }
    return 0;
}


inline void FloppyLBAtoCHS (int lba, unsigned char drive, unsigned char *head, unsigned char *track, unsigned char *sectors)
{
    *head = (lba % (fd[drive].SectorsPerTrack * 2)) / fd[drive].SectorsPerTrack;
    *track = lba / (fd[drive].SectorsPerTrack * 2);
    *sectors = lba % fd[drive].SectorsPerTrack + 1;
}


/*const char* types[] = {
      "Nonexistant", "5.25\", unsupported.", "5.25\", unsupported.",
      "3.5\", 720kb", "3.5\", 1.44mb", "3.5\", 2.88 mb"};*/

void FloppyInstall()
{
    unsigned char temp = CmosReadFloppyData();
    int i;

    // Set fd0 and fd1 types
    fd[1].Type = temp & 0xf;
    fd[0].Type = temp >> 4;
    
    // SRT = 16 - (ms * datarate / 500000);
    // HLT = ms * datarate / 1000000
    // HUT = ms * datarate / 8000000

    // Set up
    for (i = 0; i < 2; i++) {
        if (fd[i].Type >= 3) FloppyDrivesInstalled++; // 5.25" drives unsupported
        if (fd[i].Type == 3) {  // 720 kb, DD
                fd[i].DataRate = 2;      // speed = 250 kbps
                fd[i].StepRateTime = 12; // 16 - (ms * 250000 / 500000), ms = 8
                fd[i].HeadLoadTime = 7;
                fd[i].HeadUnloadTime = 7;
                fd[i].SectorsPerTrack = 9;
                fd[i].Size = 1440;
                fd[i].Gap3 = 0x2A;
                fd[i].Gap3Format = 0x50;
                fd[i].Heads = 2;
                fd[i].Tracks = 80;
        }
        else if (fd[i].Type == 4) { // 1.44 MB, HD
                fd[i].DataRate = 0;      // speed = 500 kbps
                fd[i].StepRateTime = 8;
                fd[i].HeadLoadTime = 15;
                fd[i].HeadUnloadTime = 15;
                fd[i].SectorsPerTrack = 18;
                fd[i].Size = 2880;
                fd[i].Gap3 = 0x1B;
                fd[i].Gap3Format = 0x6C;
                fd[i].Heads = 2;
                fd[i].Tracks = 80;
        }
        else if (fd[i].Type == 5) { // 2.88 MB, ED
                fd[i].DataRate = 3;      // speed = 1000 kbps;
                fd[i].StepRateTime = 0;
                fd[i].HeadLoadTime = 30;
                fd[i].HeadUnloadTime = 30;
                fd[i].SectorsPerTrack = 36;
                fd[i].Size = 5760;
                fd[i].Gap3 = 0x1B;
                fd[i].Gap3Format = 0x54;
                fd[i].Heads = 2;
                fd[i].Tracks = 80;
        }
    }
    if (FloppyDrivesInstalled == 0) return;       // No drives to set


    FloppyReset();
}


unsigned char FloppyIsDriverEnabled()
{
    return FloppyDrivesInstalled;
}

unsigned* FloppyReadSectors (unsigned* where, unsigned char drive, int sectorLBA, unsigned count)
{
    if (drive >= FloppyDrivesInstalled || !where) return 0;

    unsigned head=0, track=0, sector=1, i, addr = (unsigned)where;

    // start motor, seek to track
    FloppyMotor(drive, 1);

    // read count sectors
    for (i=0; i<count; i++) {

        // Convert lba to chs
        head=0, track=0, sector=1;
        FloppyLBAtoCHS(sectorLBA+i, drive, (unsigned char*)&head, (unsigned char*)&track, (unsigned char*) &sector);

        // Seek
        if (!FloppySeek(drive, track, head)) return 0;
        
        // Read the sector
        FloppyReadSectorImp((unsigned*)0x7e00, drive, head, track, sector);

        memcpy((void*)(addr+(0x200*i)), (void*)0x7e00, 0x200);
    }
    
    FloppyMotor(drive, 0);

    return (unsigned*)where;
}

unsigned* FloppyWriteSectors (unsigned* where, unsigned char drive, int sectorLBA, unsigned count)
{
    if (drive >= FloppyDrivesInstalled || !where) return 0;

    unsigned head=0, track=0, sector=1, i, addr = (unsigned)where;

    // start motor, seek to track
    FloppyMotor(drive, 1);

    // read count sectors
    for (i=0; i<count; i++) {
        memcpy((void*)0x7e00, (void*)(addr+(0x200*i)), 0x200);
        where = where+0x200;
        // Convert lba to chs
        head=0, track=0, sector=1;
        FloppyLBAtoCHS(sectorLBA+i, drive, (unsigned char*)&head, (unsigned char*)&track, (unsigned char*) &sector);

        // Seek
        if (!FloppySeek(drive, track, head)) return 0;

        // Read the sector
        FloppyWriteSectorImp((unsigned*)0x7e00, drive, head, track, sector);
    }

    FloppyMotor(drive, 0);

    return (unsigned*)where;
}