/*
 * floppy.c
 *
 *  Created on: Aug 20, 2011
 *      Author: Tiberiu
 */

#include <debugio.h>
#include <stdio.h>
#include <types.h>
#include <time.h>
#include <storage.h>
#include "floppy.h"
#include "../dma/dma.h"
#include "../cmos/cmos.h"

FloppyType fdTypes[] = {
/*    Sectors
 *       | Sectors per track
 *       |  | Heads
 *       |  | | Tracks
 *       |  | |  |   Gap1
 *       |  | |  |    |  Data rate
 *       |  | |  |    |    |  Spec1
 *       |  | |  |    |    |    |  SRT  HLT  HUT   Motor Spinup time
 *       |  | |  |    |    |    |    |    |    |    |  Motor Spindown time
 *       |  | |  |    |    |    |    |    |    |    |    |  Interrupt timeout
 *       |  | |  |    |    |    |    |    |    |    |    |    |   Disk type name string*/
	{    0, 0,0, 0,0x00,0x00,0x00,0x00,0x00,0x00,   0,   0,   0, "none"},
	{  720, 9,2,40,0x2A,0x01,0xDF,0x0C,0x04,0x00,1000,1000,3000, "5.25\" 360k"},
	{ 2400,15,2,80,0x1B,0x00,0xDF,0x0A,0x08,0x00, 400,1000,3000, "5.25\" 1.2M"},
	{ 1440, 9,2,80,0x2A,0x02,0xDF,0x0F,0x04,0x00,1000,1000,3000, "3.5\" 720k"},
	{ 2880,18,2,80,0x1B,0x00,0xCF,0x0C,0x08,0x00, 400,1000,3000, "3.5\" 1.44M"},
	//{ 2880,18,2,80,0x1B,0x00,0xCF,0x0C,0x08,0x00,2000,3000,3000, "3.5\" 1.44M"},
	{ 5760,36,2,80,0x1B,0x03,0xAF,0x0A,0x0F,0x00, 400,1000,3000, "3.5\" 2.88M AMI BIOS"},
	{ 5760,36,2,80,0x1B,0x03,0xAF,0x0A,0x0F,0x00, 400,1000,3000, "3.5\" 2.88M"},
};

int8 fd0, fd1;

/**************************************
 * IRQ handler etc                    *
 **************************************/
volatile uint8 FloppyIrqFired;
void FloppyIrqHandler(_RegsStack32* UNUSED(r))
{
	FloppyIrqFired = 1;
}

void FloppyWaitIrq()
{
	TimerStart(fdTypes[4].InterruptTimeout);

	while (!FloppyIrqFired && !TimerIsDone());

	if (!FloppyIrqFired) {
		Error("%#[Floppy] %#Irq timeout [%ums] !\n", ColorBrown, ColorLightRed, fdTypes[4].InterruptTimeout);
	}
}


/**************************************
 * Installation                       *
 **************************************/
void FloppyInitialize()
{
	// Detect drives
	uint8 fd = CmosRead(0x10);
	fd0 = fd >> 4;
	fd1 = fd & 0xf;

	if (fd0 > 6) fd0 = 0;
	if (fd1 > 6) fd1 = 0;

	if (!fd0 && !fd1) {
		Error("%#[Floppy] %#No supported floppy drives found.", ColorBrown, ColorLightRed);
		outportb(FloppyRegisterDigitalOutput, 0);
		return;
	}

	Log("%#[Floppy] %#Detected floppy drives:", ColorBrown, ColorLightGray);
	if (fd0) Log(" %#fd0=%#%s", ColorLightCyan, Color(ColorCyan, ColorWhite), fdTypes[fd0].Name);
	if (fd1) Log(" %#fd1=%#%s", ColorLightCyan, Color(ColorCyan, ColorWhite), fdTypes[fd1].Name);
	Log("\n");


	// Reset floppy controller
	FloppyReset();

	// Configure and lock
	FloppyConfigure();
	FloppySendCommand(FloppyCommandLock | 0x80);
	FloppyReadData();

	// Enable perpendicular mode for 3.5" ED floppies
	if (fd0 > 4)
	{
		FloppySendCommand(FloppyCommandPerpendicularMode);
		FloppySendCommand(1);
	}
	if (fd1 > 4)
	{
		FloppySendCommand(FloppyCommandPerpendicularMode);
		FloppySendCommand(2);
	}

	// Initialize DMA
	FloppyInitDma();
}

void FloppyInitDma()
{
	DmaMaskChannel(2);
	DmaResetFlipFlop(2);
	DmaSetAddress(2, 0, 0x10);
	DmaResetFlipFlop(2);
	DmaSetCount(2, 0xff, 0x23);
	DmaSetExternalPageRegisters(2,0);
	DmaUnmaskChannel(2);
}


/**************************************
 * Controller reset                   *
 **************************************/
void FloppyReset()
{
	FloppyIrqFired = 0; int32 i = 0;

	Log("%#[Floppy] %#Resetting...\n", ColorBrown, ColorLightGray);

	// Clear reset bit from DOR
	outportb(FloppyRegisterDigitalOutput, 0);
	for (i = 0; i < 1000; i++);
	outportb(FloppyRegisterDigitalOutput, 4|8);

	// Wait for IRQ6
	FloppyWaitIrq(fd0);

	// Recalibrate every drive
	if (fd0)
	{
		FloppyMotor(0,1);
		FloppySelectDrive(0);
		FloppyRecalibrate(0);
		FloppyMotor(0,0);
	}

	if (fd1)
	{
		FloppyMotor(1,1);
		FloppySelectDrive(1);
		FloppyRecalibrate(1);
		FloppyMotor(1,0);
	}

}

/**************************************
 * Configure floppy controller        *
 **************************************/
void FloppyConfigure()
{
	FloppySendCommand(FloppyCommandConfigure);
	FloppySendCommand(0);
	FloppySendCommand(1<<6 | 7);
	FloppySendCommand(0);
}

/**************************************
 * Base commands                      *
 **************************************/
void FloppySendCommand (uint8 command)
{
	int32 t;
	for (t = 0; t < 5000 && ((inportb(FloppyRegisterMainStatus) & FloppyMsrRQM) == 0); t++) ;

	outportb (FloppyRegisterFIFO, command);

}

uint8 FloppyReadData ()
{
	int32 t;
	for (t = 0; t < 5000 && ((inportb(FloppyRegisterMainStatus) & FloppyMsrRQM) == 0); t++) ;

	return inportb (FloppyRegisterFIFO);
}

/**************************************
 * Sense interrupt                    *
 **************************************/
void FloppySenseInterrupt(uint8 *st0, uint8 *cyl)
{
	FloppySendCommand(FloppyCommandSenseInterrupt);
	*st0 = FloppyReadData();
	*cyl = FloppyReadData();
}

/**************************************
 * Specify                            *
 **************************************/
void FloppySpecify (uint8 fd)
{
	FloppySendCommand(FloppyCommandSpecify);
	FloppySendCommand((fdTypes[fd].SRT << 4) | fdTypes[fd].HUT);
	FloppySendCommand(fdTypes[fd].HLT << 1);
}

/**************************************
 * Motor on/off                       *
 **************************************/
void FloppyMotor (uint8 fd_number, uint8 status)
{
	if (fd_number >= 2) return;

	uint8 fd = (fd_number == 0) ? fd0 : fd1;
	uint8 temp = inportb(FloppyRegisterDigitalOutput);

	// Turn motor on/off
	if (status) temp |= 0x1<<(4+fd_number);
	else temp &= ~(0x1<<(4+fd_number));
	outportb(FloppyRegisterDigitalOutput, temp);

	// Wait for spinup/spindown
	if (status) TimerStart(fdTypes[fd].Spinup);
	else TimerStart(fdTypes[fd].Spindown);

	Log("%#[Floppy] %#Waiting for motor...\n", ColorBrown, ColorLightGray);
	while (!TimerIsDone());
}

/**************************************
 * Select drive                       *
 **************************************/
void FloppySelectDrive(uint8 number)
{
	if (number >= 2) return;
	uint8 fd = (number == 0) ? fd0 : fd1;

	// Set CCR
	outportb(FloppyRegisterConfigurationControl, fdTypes[fd].DataRate);

	// Specify
	FloppySpecify(fd);

	// Select drive
	uint8 dor = inportb(FloppyRegisterDigitalOutput);
	dor = (dor & ~0xFF) | number;
}

/**************************************
 * RECALIBRATE 						  *
 * motor must be on, drive selected   *
 **************************************/
void FloppyRecalibrate(uint8 fd_number)
{
	if (fd_number >= 2) return;

	uint8 st0, cyl, timeout = 10;
	do {
		Log("%#[Floppy] %#Recalibrating: attempt %u/10\n", ColorBrown, ColorLightGray, 11-timeout);
		FloppyIrqFired = 0;
		FloppySendCommand(FloppyCommandRecalibrate);
		FloppySendCommand(fd_number);
		FloppyWaitIrq();

		FloppySenseInterrupt(&st0, &cyl);

		timeout--;
	} while((st0 & 0x20) == 0 && timeout > 0);
}

/**************************************
 * SEEK        						  *
 * motor must be on, drive selected   *
 **************************************/
void FloppySeek(uint8 fd_number, uint8 cylinder, uint8 head)
{
	if (fd_number >= 2) return;

	uint8 st0, cyl, timeout = 10;
	do {
		Log("%#[Floppy] %#Seeking: attempt %u/10\n", ColorBrown, ColorLightGray, 11-timeout);
		FloppyIrqFired = 0;
		FloppySendCommand(FloppyCommandSeek);
		FloppySendCommand(head<<2 | fd_number);
		FloppySendCommand(cylinder);
		FloppyWaitIrq();

		FloppySenseInterrupt(&st0, &cyl);

		timeout--;
	} while(cyl != cylinder && timeout > 0);

}

/**************************************
 * READ/WRITE  						  *
 * motor must be on, drive selected   *
 **************************************/
void FloppyRW(uint8 isWrite, uint8 fd_number, uint8 head, uint8 cylinder, uint8 sector)
{
	if (fd_number >= 2) return;
	uint8 fd = (fd_number == 0) ? fd0 : fd1;

	uint8 timeout = 10;
	uint8 result[7], i, error;
	do
	{
		error = 0;
		Log("%#[Floppy] %#Read/write operation: attempt %u/10\n", ColorBrown, ColorLightGray, 11-timeout);
		FloppyIrqFired = 0;

		if (isWrite) FloppySendCommand(FloppyCommandWriteData | FloppyModeMultitrack | FloppyModeMagneticEncoding);
		else FloppySendCommand(FloppyCommandReadData | FloppyModeMultitrack | FloppyModeMagneticEncoding);

		FloppySendCommand(head<<2 | fd_number);
		FloppySendCommand(cylinder);
		FloppySendCommand(head);
		FloppySendCommand(sector);
		FloppySendCommand(2);
		FloppySendCommand(fdTypes[fd].SectorsPerTrack);
		FloppySendCommand(fdTypes[fd].Gap);
		FloppySendCommand(0xff);

		FloppyWaitIrq();

		for (i = 0; i < 7; i++)
			result[i] = FloppyReadData();

		// Disk is write protected, don't try again
		if (result[1] & 2)
		{
			Error("%#[Floppy] %#Error: disk is write protected!\n", ColorBrown, ColorLightRed);
			return;
		}

		// Any other error - try again
		if (result[0] & 0xC8) error = 1;
		if (result[1] & 0xB5) error = 1;
		if (result[2] & 0x77) error = 1;
		if (result[6] & 0x02) error = 1;

		timeout--;
	} while (timeout > 0 && !error);
}

uint32 FloppyRead(uint8 drive, uint32 lba)
{
	if (drive >= 2) return 0;
	uint8 fd = (drive == 0) ? fd0 : fd1;

	// Convert LBA to CHS
	uint32 cyl=0, head=0, sect=1;
	ConvertLbaToChs(fdTypes[fd].SectorsPerTrack, lba, &cyl, &head, &sect);
	Log("%#[Floppy] %#Converted LBA=%u to Cyl=%u Head=%u Sect=%u\n", ColorBrown, ColorLightGray, lba, cyl, head, sect);

	FloppyInitDma();

	// Reset drive if necessary
	if ((inportb(FloppyRegisterMainStatus) & 0xC0) != 0x80)
		FloppyReset();

	// Start motor, select drive
	FloppyMotor(drive, 1);
	FloppySelectDrive(drive);

	// Seek to correct location
	FloppySeek(drive, cyl, head);

	// Start DMA read
	DmaMaskChannel(2);
	DmaSetMode(2, 0x46);
	DmaUnmaskChannel(2);

	FloppyRW(0, drive, head, cyl, sect);

	FloppyMotor(drive, 0);

	return 0x1000;
}

// Log("%#[Drivers] %#Initializing blah blah %d...", ColorWhite, ColorLightGray,PIT_FREQUENCY);