#include"fdc.h"
#include"../dma/dma.h"
#include"../x86/kernel/deviceproc.h"
#include"../x86/kernel/hwio.h"
#include"../x86/kernel/pcdevice.h"
#include"../x86/kernel/kstring.h"
#include"../x86/kernel/kdebug.h"
#define FDC_R 0x3F0
#define	FDC_SRA 0 // read-only
#define	FDC_SRB 1 // read-only
#define	FDC_DOR 2
#define	FDC_TAPE 3
#define	FDC_MSR 4 // read-only
#define	FDC_DSR 4 // write-only
#define	FDC_FIFO 5
#define	FDC_DIR 7 // read-only
#define	FDC_CCR 7 // write-only
#define FDC_ENHANCED 0x90
#define MASTER 0 //Of puppets
#define SLAVE 1
#define _DR0_DOR_F 0x10
#define _DR1_DOR_F 0x20
#define MAGIC_DRIVE_NUM 0xAF
#define FIFO_HOLD 0x800
#define RQM 0x80
#define DIO 0x40
#define _SPC_CF 0x03
#define _RCB_CF 0x07
#define _ICHK_CF 0x08
#define _READID_CF 0x0A
#define _DUMPR_CF 0x0E
#define _SEEK_CF 0x0F
#define _VER_CF 0x10
#define _PPD_CF 0x12
#define _CNF_CF 0x13
#define _LCK_CF 0x14
#define MT 0x80
#define MFM 0x40
#define SK 0x20
#define READ 0x06
#define WRITE 0x05
#define FDC_CHANNEL 2
#define _NOT_READY 8
#define _FAULT_SIG 16
#define _DMA_DOR_F 0x08
#define _RST_DOR_F 0x04
#define _RST_DSR_F 0x80
#define _DR0_PPD_MF 0x04
#define _DR1_PPD_MF 0x08
#define _SRT_VF 6
#define _HLT_VF 15
#define _HUT_VF 0 //Maximum
#define _FIFO_SIZE 8
#define DEFAULT_PRECOMPENSATION 0
#define IMPLIED_SEEK 0x40
#define FIFO_DISABLE 0x20
#define DPL_DISABLE 0x10
#define THRESHVAL 7 //8 - 1
struct floppyDrv Floppy[2];

struct senseInt;
struct senseInt
{
	word st0;
	word cyl;
}ret_sen;
void lbachs(unsigned lba, chs * cv, struct floppyDrv* f)
{
	cv->cylinder = (byte)(lba / (2 * f->SectorsPerTrack));
	cv->head = (byte)((lba % (2 * f->SectorsPerTrack)) / f->SectorsPerTrack);
	cv->sector = (byte)((lba % f->SectorsPerTrack) + 1);
}
/*
	Physical Floppy Data of each standard floppy type specified by CMOS registers;
	with each index corresponding to all other values with same index. (from 0 to 4)
	Example : Index 1 >>> 15 SPT, 1200KB medium, 300KBPS (1)
*/
const byte sptFT[5] = { 9, 15, 9, 18, 36 }; //Sectors per track
const word sizeFT[6] = { 360, 1200, 720, 1440, 2880 }; //Floppy medium in kilobytes
const byte spDrFT[5] = { 1, 3, 2, 0, 3 }; //CCR Values
const word drFT[5] = { 300, 500, 250, 500, 1000 }; //Data rate in kilobytes per second
int confDrive(int n, byte fT)
{
	if (fT)
	{
		--fT;
		Floppy[n].kbSize = sizeFT[fT];
		Floppy[n].SectorSize = 512; //TODO
		Floppy[n].SectorsPerTrack = sptFT[fT];
		Floppy[n].magic = MAGIC_DRIVE_NUM;
		if (fT > 1) Floppy[n].physFloppySize = false; //3.5 drives
		else Floppy[n].physFloppySize = true; //5.25 drives
		Floppy[n].data_rate = drFT[fT];
		Floppy[n].REGfT = fT;
		Floppy[n].Gap = 0x1b;
		if (!Floppy[n].data_rate)return 2; //Unimplemented
	}
	else
	{ 
		Floppy[n].kbSize = 0;
		return 1;
	}
	return 0;
}

extern unsigned fdc_IrqWait();
#define _dor_out(val) outb(FDC_R+FDC_DOR, val)
#define _dor_in() inb(FDC_R+FDC_DOR)
#define _msr_in() inb(FDC_R+FDC_MSR)
#define _dsr_out(val) outb(FDC_R+FDC_DSR, val)

word _fifo_wait_out()
{
	word i = 0, v;
	while (!((++i)&FIFO_HOLD))
	{
		v = _msr_in();
		if ((v&RQM)&&(!(v&DIO)))
		{
			return 0;
		}
	}
	return 1;
}
word _fifo_wait_in()
{
	word i = 0, v;
	while (!((++i)&FIFO_HOLD))
	{
		v = _msr_in();
		if ((v&RQM) && (v&DIO))
		{
			return 0;
		}
	}
	return 1;
}
short _fifo_out(byte val)
{
	if(_fifo_wait_out())return -1;
	outb(FDC_R + FDC_FIFO, val);
	return 0;
}
short _fifo_in()
{
	if (_fifo_wait_in())return -1;
	return inb(FDC_R + FDC_FIFO);
}
struct senseInt* _int_check()
{
	_fifo_out(_ICHK_CF);
	ret_sen.st0 = _fifo_in();
	ret_sen.cyl = _fifo_in();
	return &ret_sen;
}
word recalDrive(byte n)
{
	byte i = 0xFF;
	while (--i)
	{
		_fifo_out(_RCB_CF);
		_fifo_out(n);
		if (!fdc_IrqWait())return 2;
		struct senseInt * _senRet = _int_check();
		if (!(_senRet->cyl)&&(_senRet->st0&0x20))return 0;
	}
	return 1;
}
word selectDrive(unsigned n)
{
	if (n > 0b11)return 4;
	if (!Floppy[n].kbSize)return 2;
	if(spDrFT[Floppy[n].REGfT]!=0xFF)_dsr_out(spDrFT[Floppy[n].REGfT]);//Set data rate
	else return 1; //Unimplemented
	const byte SRT = (byte)(16 - (Floppy[n].data_rate * _SRT_VF / 500));
	const byte HLT = (byte)(_HLT_VF*Floppy[n].data_rate / 1000);
	const byte HUT = (byte)(_HUT_VF*Floppy[n].data_rate / 8000);
	const bool PIO = false;
	if (_fifo_out(_SPC_CF))return 3; //Specify command
	_fifo_out((SRT << 4) | HUT);
	_fifo_out((HLT << 1) | (PIO & 1));
	_dor_out((_dor_in() & 0b11111100) | (byte)n);
	_pitWait(1); //4 miliseconds
	return 0;
}
word seekDrive(byte head, byte _cyl, byte drive)
{
	byte locH = (head << 2) | drive;
	byte i = 0xFF;
	while (--i)
	{
		_fifo_out(_SEEK_CF);
		_fifo_out(locH);
		_fifo_out(_cyl);
		fdc_IrqWait();
		struct senseInt * _senRet = _int_check();
		if ((_senRet->cyl == _cyl) && (_senRet->st0 & 0x20))
		{
			_fifo_out(_READID_CF | MFM);
			_fifo_out(locH);
			for (byte j = 0; j < 7; j++)_fifo_in();
			if(!fdc_IrqWait())return 1;
			return 0;
		}
	}
	return 1;
}
//CHS Read Floppy Drive Sector Procedure
word fdc_readSectorEx(byte drive, chs * __chs, byte * buf)
{
	word returnval=0;
	if (((unsigned)buf) >> 24)return 3;
	for(byte tries=0;tries<4;++tries)
	{
		word selDrive = selectDrive(drive);
		if (selDrive)return 0x80 | selDrive;
		word seekDrv = seekDrive(__chs->head, __chs->cylinder, drive);
		if (seekDrv)return 0x40 | seekDrv;
		dmaReset(Slave_DMA);
		dmaSetMask(FDC_CHANNEL,true);
		dmaClearFlipFlop(Slave_DMA);
		dmaAddr(FDC_CHANNEL, buf);
		dmaClearFlipFlop(Slave_DMA);
		dmaCount(FDC_CHANNEL, Floppy[drive].SectorSize - 1);
		dmaRead(FDC_CHANNEL);
		dmaSetMask(FDC_CHANNEL,false);
		if (_fifo_out(READ|MT|MFM|SK))return 4;
		_fifo_out((__chs->head << 2) | drive);
		_fifo_out(__chs->cylinder);
		_fifo_out(__chs->head);
		_fifo_out(__chs->sector);
		_fifo_out(2);
		_fifo_out(Floppy[drive].SectorsPerTrack);
		_fifo_out(Floppy[drive].Gap);
		_fifo_out(0xFF);
		returnval = fdc_IrqWait()?0:1;
		word ferr = 512 | returnval;
		const word st0 = _fifo_in(); if (st0 > 255)return ferr;
		const word st1 = _fifo_in(); if (st1 > 255)return ferr;
		const word st2 = _fifo_in(); if (st2 > 255)return ferr;
		const word cyl = _fifo_in(); if (cyl > 255)return ferr;
		const word end_head = _fifo_in(); if (end_head > 255)return ferr;
		const word end_sector = _fifo_in(); if (end_sector > 255)return ferr;
		const word last_result = _fifo_in(); if (last_result > 255)return ferr;
		if (last_result != 2)return 7; //7th result byte must be 2
		if (st0 & _NOT_READY)
		{
			kprintf("Not ready");
			returnval |= _NOT_READY;
			_pitWait(2); //Some tiny period of time before trying again
			continue;
		}
		if (st0 & _FAULT_SIG)
		{
			kprintf("Fault signal");
			returnval |= _FAULT_SIG;
			continue;
		}
		if (st1)
		{
			kprintf("ST1 = %u", st1);
			returnval |= 20;
			continue;
		}
		return 0;
	}
	return returnval;
}
word fdc_readSector(byte drive, dword sector, byte * buf)
{
	if (drive >= 2)return 250;
	if (Floppy[drive].magic != MAGIC_DRIVE_NUM)return 300;
	chs __chs;
	lbachs(sector, &__chs, &Floppy[drive]);
	word ret = fdc_readSectorEx(drive, &__chs, buf);
	if (ret)return ret;
	else return 0;
}
short configureFdc(byte v)
{
	_fifo_out(_CNF_CF);
	_fifo_out(0);
	_fifo_out(v);
	_fifo_out(DEFAULT_PRECOMPENSATION);
	_fifo_out(_LCK_CF);
	return _fifo_in();
}
byte getFdcVer()
{
	_fifo_out(_VER_CF);
	return _fifo_in();
}
extern volatile byte _fdc_fire;
const char * fdcDevNm = "FDC";
/*
	Floppy Drive Controller Initialization Procedure
	0 - Success
	>0x500 - Various problems, see code below
	0x100 - No controller
	0x101 - No master drive
	0x102 - Faulty controller
	0x201 - Unsupported controller
*/
int initFdc(extendDevInfo * _ex)
{
	strcpy(_ex->devName, fdcDevNm); //Copy driver name to the device information block
	if (_msr_in() == 0xFF)return 0x100; //No controller
	if (getFdcVer() != FDC_ENHANCED)return 0x201; //Unsupported
	byte RegisterFloppy = getCmosRegister(0x10); //Get floppy drive information
	byte mask = _DMA_DOR_F | _RST_DOR_F;
	if (!confDrive(MASTER, (RegisterFloppy & 0xF0) >> 4))
		mask |= _DR0_DOR_F;
	else return 0x101;
	if (!confDrive(SLAVE, RegisterFloppy & 0x0F))
		mask |= _DR1_DOR_F;
	enableInterrupt(6);
	_dsr_out(_RST_DSR_F);
	_dsr_out(_RST_DSR_F);
	/*
		ISA I/O is very slow. Processor would execute the next instructions even before the data has started to send.
		So, for safety, we will empty the DOR twice.
	*/
	_dor_out(_RST_DOR_F);
	if (_fifo_wait_out())return 0x102; //Check if faulty controller
	//configureFdc(IMPLIED_SEEK | DPL_DISABLE | THRESHVAL);
	_fdc_fire = 0;
	_dsr_out(_RST_DSR_F);
	_pitWait(1); //100uS
	_dor_out(mask);//Now actually reset the controller 
	_pitWait(2);//20mS
	if (!fdc_IrqWait())return 0x502; //Wait for IRQ, as FDC reset results into an IRQ
	_pitWait(100);//1 second, for the motor to spin
	for (int i = 0; i < 4; i++)_int_check(); //Receive 4 sense interrupts
	mask = 0;
	if (Floppy[MASTER].kbSize == 2880)
		mask |= _DR0_PPD_MF;
	if (Floppy[SLAVE].kbSize == 2880)
		mask |= _DR1_PPD_MF;
	if(_fifo_out(_PPD_CF))return 0x503;
	_fifo_out(mask);//Perpendicular mode
	int res = recalDrive(MASTER); //Recalibrate master drive
	if(res)return 0x503+res;
	if (Floppy[SLAVE].kbSize) //And slave drive
	{
		res = recalDrive(SLAVE);
		if (res)return 0x505+res;
	}
	res = selectDrive(MASTER); //Select the master drive (/dev/fd0)
	if(res)return 0x507+res;
	return 0;
}
int deinitFdc()
{
	_dsr_out(_RST_DSR_F);
	_dor_out(0);
	return 0;
}