#include"fdc.h"
#include"pcdevice.h"
#include"hwio.h"
#include<string.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
struct floppyDrv Floppy[2];
#define MASTER 0 //Of puppets
#define SLAVE 1
struct chs;
struct chs
{
	uint16 head;
	uint16 cylinder;
	uint16 sector;
};
struct senseInt;
struct senseInt
{
	uint16 st0;
	uint16 cyl;
}ret_sen;
void lbachs(unsigned lba, struct chs *buf, struct floppyDrv* f)
{
	buf->cylinder = lba / (2 * f->SectorsPerTrack);
	buf->head = ((lba % (2 * f->SectorsPerTrack)) / f->SectorsPerTrack);
	buf->sector = ((lba % f->SectorsPerTrack) + 1);
}
uint8 sptFT[5] = { 9, 15, 9, 18, 36 };
uint16 sizeFT[6] = { 0, 360, 1200, 720, 1440, 2880 };
#define _DR0_DOR_F 0x10
#define _DR1_DOR_F 0x20
int confDrive(int n, uint8 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; break;
			Floppy[n].Gap = 0x1B;
		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
short _fifo_wait()
{
	uint16 i = 0;
	while (!((++i)&FIFO_HOLD))
	{
		if (_msr_in()&RQM)
		{
			return 0;
		}
	}
	return -1;
}
short _fifo_out(uint8 val)
{
	if(_fifo_wait())return -1;
	outb(FDC_R + FDC_FIFO, val);
	return 0;
}
short _fifo_in()
{
	if (_fifo_wait())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;
}
#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
int recalDrive(unsigned n)
{
	uint8 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;
}
int selectDrive(unsigned n)
{
	uint8 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
	uint32 SRT = 16 - (Floppy[n].data_rate * _SRT_VF / 500000);
	uint32 HLT = _HLT_VF*Floppy[n].data_rate / 1000000;
	uint32 HUT = _HUT_VF*Floppy[n].data_rate / 8000000;
	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;
}
int seekDrive(uint8 head, uint8 _cyl, uint8 drive)
{
	uint8 i = 0xFF;
	while (--i)
	{
		_fifo_out(_SEEK_CF);
		_fifo_out((head << 2) | drive);
		_fifo_out(_cyl);
		fdc_IrqWait();
		if (_int_check()->cyl == _cyl)return 0;
	}
	return 1;
}
#define MT 0x80
#define MFM 0x40
#define READ 0x06
#define WRITE 0x05
/*
	Bare bones read sector function
	No error checking/retry
*/
int fdc_readSector(uint8 drive, uint32 sector, uint8 * buf)
{
	if (drive >= 2)return 1;
	struct chs _chs;
	lbachs(sector, &_chs, &Floppy[drive]);
	int ret = fdc_readSectorEx(drive, &_chs, buf);
	if (ret)return ret;
	else return 0;
}
#include"dma.h"
#define FDC_CHANNEL 2
int fdc_readSectorEx(uint8 drive, struct chs * _chs, uint8 * buf)
{
	if (((unsigned)buf) >> 24)return 3;
	if (selectDrive(drive))return 5;
	if (seekDrive(_chs->head, _chs->cylinder, drive))return 6;
	dmaReset();
	dmaMask(FDC_CHANNEL);
	dmaClearFlipFlop(1);
	dmaAddr(FDC_CHANNEL, buf);
	dmaClearFlipFlop(1);
	dmaCount(FDC_CHANNEL, 512 - 1);
	dmaRead(FDC_CHANNEL);
	dmaUnmask(FDC_CHANNEL);
	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);
	if(!fdc_IrqWait())return 1;
	for (int i = 0; i < 7; i++)
		_fifo_in();
	_int_check();
	return 0;
}
const char * fdcDevNm = "FDC";
int initFdc(extendDevInfo * _ex)
{
	strcpy(_ex->devName, fdcDevNm);
	uint8 RegisterFloppy = getCmosRegister(0x10);
	uint8 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;
}