#include"fdc.h"
#include"dma.h"
#include"pcdevice.h"
#include"hwio.h"
#include"kstring.h"
#include"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 MASTER 0 //Of puppets
#define SLAVE 1
#define _DR0_DOR_F 0x10
#define _DR1_DOR_F 0x20
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 = lba / (2 * f->SectorsPerTrack);
	cv->head = ((lba % (2 * f->SectorsPerTrack)) / f->SectorsPerTrack);
	cv->sector = ((lba % f->SectorsPerTrack) + 1);
}
const byte sptFT[5] = { 9, 15, 9, 18, 36 };
const word sizeFT[6] = { 0, 360, 1200, 720, 1440, 2880 };
int confDrive(int n, byte fT)
{
	Floppy[n].kbSize = sizeFT[fT];
	if (fT)
	{
		Floppy[n].SectorsPerTrack = sptFT[fT - 1];
		if (fT > 2) Floppy[n].physFloppySize = false; //3.5 drives
		else Floppy[n].physFloppySize = true; //5.25 drives
		switch (sizeFT[fT])
		{
		case 2880:
			Floppy[n].data_rate = 1000000; break;
		case 1440:
			Floppy[n].data_rate = 500000; 
			Floppy[n].Gap = 0x1B; break;
		default:; //Unimplemented
		}
	}
	else return 1;
	return 0;
}

extern bool _fdc_fire;
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 _ccr_out(val) outb(FDC_R+FDC_CCR, val)
#define FIFO_HOLD 0x400
#define RQM 0x80
#define _SPC_CF 0x03
#define _RCB_CF 0x07
#define _ICHK_CF 8
#define _SEEK_CF 0x0F
word _fifo_wait()
{
	word i = 0;
	while (!((++i)&FIFO_HOLD))
	{
		if (_msr_in()&RQM)
		{
			return 0;
		}
	}
	return 1;
}
word _fifo_out(byte val)
{
	if(_fifo_wait())return 1;
	outb(FDC_R + FDC_FIFO, val);
	return 0;
}
word _fifo_in()
{
	if (_fifo_wait())return 0xFFFF;
	return inb(FDC_R + FDC_FIFO);
}
struct senseInt* _int_check()
{
	_fifo_out(_ICHK_CF);
	ret_sen.st0 = _fifo_in();
	ret_sen.cyl = _fifo_in();
	kprintf("ST0=%u, CYL=%u\n", ret_sen.st0, ret_sen.cyl);
	return &ret_sen;
}
#define _DMA_DOR_F 0x08
#define _RST_DOR_F 0x04
#define _DR0_PPD_MF 0x04
#define _DR1_PPD_MF 0x08
#define _PPD_MF 0x12
#define _SRT_VF 6
#define _HLT_VF 15
#define _HUT_VF 0 //Maximum
word recalDrive(unsigned 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;
	byte mask;
	if (!Floppy[n].kbSize)return 2;
	switch (Floppy[MASTER].data_rate)
	{
	case 1000000:
		mask = 3;
		break;
	case 500000:
		mask = 0;
		break;
	default: return 1;//Unimplemented
	}
	_ccr_out(mask);//Set data rate
	const dword SRT = 16 - (Floppy[n].data_rate * _SRT_VF / 500000);
	const dword HLT = _HLT_VF*Floppy[n].data_rate / 1000000;
	const dword HUT = _HUT_VF*Floppy[n].data_rate / 8000000;
	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) | n);
	_usleep(4);
	return 0;
}
word seekDrive(byte head, byte _cyl, byte drive)
{
	byte i = 0xFF;
	while (--i)
	{
		_fifo_out(_SEEK_CF);
		_fifo_out((head << 2) | drive);
		_fifo_out(_cyl);
		fdc_IrqWait();
		struct senseInt * _senRet = _int_check();
		if (_senRet->cyl == _cyl&&_senRet->st0&0x20)return 0;
	}
	return 1;
}
#define MT 0x80
#define MFM 0x40
#define READ 0x06
#define WRITE 0x05

#define FDC_CHANNEL 2
#define _NOT_READY 8
#define _FAULT_SIG 16
/*
Bare bones read sector function
No error checking/retry
*/
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)
	{
		if (selectDrive(drive))return 5;
		if (seekDrive(__chs->head, __chs->cylinder, drive))return 6;
		dmaReset(Slave_DMA);
		dmaMask(FDC_CHANNEL);
		dmaClearFlipFlop(Slave_DMA);
		dmaAddr(FDC_CHANNEL, buf);
		dmaClearFlipFlop(Slave_DMA);
		dmaCount(FDC_CHANNEL, 512 - 1);
		dmaRead(FDC_CHANNEL);
		dmaUnmask(FDC_CHANNEL);
		kprintf("CYLINDER = %u, HEAD = %u, SECTOR = %u", __chs->cylinder, __chs->head, __chs->sector);
		if (_fifo_out(READ | MT | MFM))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;
		const byte st0 = _fifo_in();
		const byte st1 = _fifo_in();
		const byte st2 = _fifo_in();
		const byte cyl = _fifo_in();
		const byte end_head = _fifo_in();
		const byte end_sector = _fifo_in();
		const byte last_result = _fifo_in();
		kprintf("ST0 = %u, ST1 = %u, ST2 = %u\nCYL = %u, END_HEAD = %u, END_SECTOR = %u, LAST_RESULT = %u", st0, st1, st2, cyl, end_head, end_sector, last_result);
		if (last_result != 2)return 7; //7th result byte must be 2
		if (st0 & _NOT_READY)
		{
			kprintf("Not ready");
			returnval |= _NOT_READY;
			_usleep(2);
			continue;
		}
		if (st0 & _FAULT_SIG)
		{
			kprintf("Fault signal");
			returnval |= _FAULT_SIG;
			continue;
		}
		if (st1)
		{
			kprintf("ST1 = %u", st1);
			returnval |= 20;
			continue;
		}
		_int_check();//Locks up drive until reset
		return 0;
	}
	return 8;
}
word fdc_readSector(byte drive, dword sector, byte * buf)
{
	if (drive >= 2)return 31;
	chs __chs;
	lbachs(sector, &__chs, &Floppy[drive]);
	word ret = fdc_readSectorEx(drive, &__chs, buf);
	if (ret)return ret;
	else return 0;
}
#include"dma.h"

const char * fdcDevNm = "FDC";
int initFdc(extendDevInfo * _ex)
{
	strcpy(_ex->devName, fdcDevNm);
	byte RegisterFloppy = getCmosRegister(0x10);
	byte mask = _DMA_DOR_F | _RST_DOR_F;
	if (!confDrive(MASTER, (RegisterFloppy & 0xF0) >> 4))
		mask |= _DR0_DOR_F;
	else return 0x501;
	if (!confDrive(SLAVE, RegisterFloppy & 0x0F))
		mask |= _DR1_DOR_F;
	_dor_out(0);
	_dor_out(mask);
	if (!fdc_IrqWait())return 0x502;
	for (int i = 0; i < 4; i++)_int_check();
	mask = 0; //Perpendicular mode
	if (Floppy[MASTER].kbSize == 2880)
		mask |= _DR0_PPD_MF;
	if (Floppy[SLAVE].kbSize == 2880)
		mask |= _DR1_PPD_MF;
	if(_fifo_out(_PPD_MF))return 0x503;
	_fifo_out(mask);
	int res = recalDrive(0);
	if(res)return 0x503+res;
	if (Floppy[SLAVE].kbSize)
	{
		res = recalDrive(1);
		if (res)return 0x505+res;
	}
	res = selectDrive(0);
	if(res)return 0x507+res;
	return 0;
}
int deinitFdc()
{
	_dor_out(0);
	return 0;
}