/*
 * floppy.c
 *
 * Copyright 2002, Minoru Murashima. All rights reserved.
 * Distributed under the terms of the BSD License.
 *
 * 1.44Mフロッピーのみ対応
 */


#include<types.h>
#include<sys/param.h>
#include<lib.h>
#include<errno.h>
#include<time.h>
#include<proc.h>
#include<interrupt.h>
#include<lock.h>
#include<mp.h>
#include<sp.h>
#include<fs.h>
#include<device.h>
#include<debug.h>


enum{
	/* 1.44Mspec */
	SECTOR_SIZE	= 512,
	CYLINDERS	= 80,
	SECTORS		= 18,
	GAP3		= 27,

	/* IO address */
	FD_DOR	= 0x3f2,	/* Digital output register */
	FD_STAT	= 0x3f4,	/* Main status register */
	FD_DATA	= 0x3f5,	/* Data register */
	FD_CCR	= 0x3f7,	/* Configuration control register */
	FD_DIR	= 0x3f7,	/* Digital input register */

	IRQ_NO = 6,			/* IRQ number */

	/* Command */
	CALIBRATE_COMMAND		= 0x7,	/* Seek head to start */
	SEEK_COMMAND			= 0x8f,	/* Seek head */
	READ_COMMAND			= 0xe6, /* Read 1 sector */
	WRITE_COMMAND			= 0xc5,	/* Write 1 sector */
	SECTOR_IDENTIFI_COMMAND	= 0x4a,	/* Read position of current sector */
	SPECIFY_FD_COMMAND		= 0x3,	/* Set drive specify */
    FORMAT_COMMAND			= 0x4d,	/* Format discket */

	/* Number of command parameters */
	CALIBRATE_PARAM			= 2,	/* Seek head to start parameters */
	SEEK_PARAM				= 3,	/* Seek head parameters */
	READ_PARAM				= 9,	/* Read 1 sector parameters */
	WRITE_PARAM				= 9,	/* Write 1 sector parameters */
	SECTOR_IDENTIFI_PARAM	= 2,	/* Read position of current sector parameters */
	SPECIFY_FD_PARAM		= 3,	/* Set drive specify parameters */
    FORMAT_PARAM			= 6,	/* Format discket parameters */

    /* Number of command results */
	CALIBRATE_RESULT		= 0,	/* Seek head to start results */
	SEEK_RESULT				= 0,	/* Seek head results */
    READ_RESULT				= 7,	/* Read 1 sector results */
	WRITE_RESULT			= 7,	/* Write 1 sector results */
	SECTOR_IDENTIFI_RESULT	= 7,	/* Read position of current sector results */
	SPECIFY_FD_RESULT		= 0,	/* Set drive parameters results */
    FORMAT_RESULT			= 7,	/* Format discket results */

	TIME_OUT = 500,				/* time out(ms) */

	/* DMA mode values */
	READ_DMA	= 0,		/* Read from fd */
	WRITE_DMA	= 1		/* Write to fd */
};


static uchar param[9];					/* Queue for Command parameters */
static uchar result[7];					/* Queue for results after command */
static char *fdbuf=(char*)0;			/* Buffer for data transfer */
static WAIT_INTR wait_intr_queue;		/* 割り込み待ち用 */
static WAIT_QUEUE wait_queue;			/* 処理待ち用Wait queue */
static uint64 time_out;					/* Time out value */
static int current_cyl;					/* Current cylinder position */


/*
 * Intrrupt floppy drive handler
 * return : Task switch on
 */
static int fd_intr_handler()
{
	return wake_intr(&wait_intr_queue,TASK_DEVICE_WAIT);
}

static int fd_intr_nowork()
{
	return 0;
}


static int motorRef = 0;	// Motor on refreance count.
static int motorGate = 0;	// refreance count lock gate.


// FD motor on
static void motorOn()
{
	enter_spinlock(&motorGate);
	{
		if (motorRef == 0){
			outb(FD_DOR, 0x1c);		// MOTA |DMA |Enable | Drive0
			irq_entry[IRQ6] = fd_intr_handler;
		}
		++motorRef;
	}
	exit_spinlock(&motorGate);
}


// FD motor off
static void motorOff()
{
	enter_spinlock(&motorGate);
	{
		--motorRef;
		ASSERT(0 <= motorRef);
		if (motorRef == 0){
			outb(FD_DOR, 0xc);		// DMA | Enable
			irq_entry[IRQ6] = fd_intr_nowork;
		}
	}
	exit_spinlock(&motorGate);
}


/*
 * Issue command
 * parametrs : Number of command operations,parameter array
 * returns : 0 or Error number
 */
static int issue_command(int num)
{
	int i;
	uint64 count;


	for(i=0;i<num;++i)
	{
		if(!(inb(FD_STAT)&0x80))
		{
			count=rdtsc();
			while(!(inb(FD_STAT)&0x80))
				if(rdtsc()-count>time_out)return -EDBUSY;		/* Time out */
		}

		outb (FD_DATA, param[i]);
	}

	return 0;
}


/*
 * Get result after isuuing command
 * parameters : Number of result operation,result array
 * returns : 0 or Error number
 */
static int get_result(int num)
{
	int i;
	uint64 count;


	for (i = 0; i < num; ++i){
		if (!(inb(FD_STAT) & 0x80)){
			count = rdtsc();
			while (!(inb(FD_STAT) & 0x80)){
				if (time_out < rdtsc() - count){
					return -EDBUSY;		/* Time out */
				}
			}
		}

		result[i] = inb(FD_DATA);
	}

	return 0;
}


/*
 * Init DMA
 * 転送バッファアドレスは20bit以内
 * parameters : Mode
 */
void init_dma(int mode)
{
	enum{
		ADDR_REG=		0x4,	/* port for low 16 bits of DMA address */
		PAGE_REG=		0x81,	/* port for top 4 bits of 20-bit DMA addr */
		COUNT_REG=		0x5,	/* port for DMA count (count =  bytes - 1) */
		ENABLE_REG=		0x8,	/* DMA enable port */
		MASK_REG=		0xa,	/* DMA init port */
		MODE_REG=		0xb,	/* DMA mode port */
		FLIPFLOP_REG=	0xc,	/* DMA byte pointer flip-flop */
	};

	static int dma_mode=-1;		/* Current DMA mode */


	if(dma_mode==mode)return;

	dma_mode=mode;
	outb(MASK_REG,0x6);						/* Mask channel2 */
	outb(MODE_REG,(dma_mode==READ_DMA)?0x56:0x5a);	/* transfer mode,single transfer,auto initial,increment */
	outb(FLIPFLOP_REG,0);					/* Reset flip_flop、16bit代入時に必要 */
	outb(ADDR_REG,(uint)fdbuf);				/* Low byteを代入 */
	outb(ADDR_REG,(uint)fdbuf>>8);			/* High byteを代入 */
	outb(PAGE_REG,(uint)fdbuf>>16);			/* Top 4bitを代入 */
	outb(COUNT_REG,(uchar)(SECTOR_SIZE-1));	/* 転送バイト数のLow byteを代入 */
	outb(COUNT_REG,(SECTOR_SIZE-1)>>8);		/* 転送バイト数のHigh byteを代入 */
	outb(MASK_REG,0x2);						/* Mask clear channel2 */
}


/************************************************************************************************
 *
 * FD command
 *
 ************************************************************************************************/


/*
 * Calibrate floppy drive
 * return : 0 or Error number
 */
static int calibrateFd()
{
	int error;

	param[0] = CALIBRATE_COMMAND;
	param[1] = 0;					// Drive 0
	if ((error = issue_command(CALIBRATE_PARAM)) != 0){
		return error;
	}
	wait_intr(&wait_intr_queue,2000,TASK_DEVICE_WAIT);
	
	current_cyl = 0;

	return 0;
}


/*
 * Seek floppy drive
 * On moter
 * return : 0 or Error number
 */
static int seekFd(uchar cyl)
{
	int error;


	if (cyl == current_cyl){
		return 0;
	}

	param[0] = SEEK_COMMAND | ((current_cyl < cyl)? 0x40 : 0);
	param[1] = 0;		/* Drive 0 */
	param[2] = cyl;		/* Cylinder */
	if ((error = issue_command(SEEK_PARAM)) != 0){
		return error;
	}
	wait_intr(&wait_intr_queue, 2000, TASK_DEVICE_WAIT);

	current_cyl = cyl;

	return 0;
}


/*
 * Read floppy drive
 * parameters : Head,cylinder,sector
 * return : 0 or Error number
 */
static int read_fd(uchar head,uchar cyl,uchar sector)
{
	int error;

	init_dma(READ_DMA);

	if ((error = seekFd(cyl)) != 0){
		return error;
	}

	param[0] = READ_COMMAND;
	param[1] = head<<2;
	param[2] = cyl;
	param[3] = head;
	param[4] = sector;
	param[5] = 2;			// Secter size=512
	param[6] = SECTORS;		// last sector in track
	param[7] = GAP3;
	param[8] = 0xff;		// If param[5]==0 data length,else 0xff

	error = issue_command(READ_PARAM);
	if (error != 0){
		return error;
	}
	wait_intr(&wait_intr_queue, 2000, TASK_DEVICE_WAIT);
	error = get_result(READ_RESULT);
	if (error != 0){
		return error;
	}

	if (result[0] & 0xc0){
		printk("Read floppy error! ST0=%x,ST1=%x,ST2=%x\n", result[0], result[1], result[2]);
		return -EDERRE;
	}

	return 0;
}


/*
 * Write floppy drive
 * parameters : Head,cylinder,sector
 * return : 0 or Error number
 */
static int write_fd(uchar head,uchar cyl,uchar sector)
{
	int error;


	init_dma(WRITE_DMA);

	if ((error = seekFd(cyl)) != 0){
		return error;
	}

	param[0]=WRITE_COMMAND;
	param[1]=head<<2;
	param[2]=cyl;
	param[3]=head;
	param[4]=sector;
	param[5]=2;			/* Secter size=512 */
	param[6]=SECTORS;	/* last sector in track */
	param[7]=GAP3;
	param[8]=0xff;		/* If param[5]==0 data length,else 0xff */

	error = issue_command(WRITE_PARAM);
	if (error != 0)
		return error;
	wait_intr(&wait_intr_queue,2000,TASK_DEVICE_WAIT);
	error = get_result(WRITE_RESULT);
	if (error != 0)
		return error;
	if(result[0]&0xc0)
	{
		printk("Read floppy error! ST0=%x,ST1=%x,ST2=%x\n",result[0],result[1],result[2]);
		return -EDERRE;
	}

	return 0;
}


/*
 * Format floppy disk
 */
static int format_fd()
{
	return 0;
}


/************************************************************************************************
 *
 * System call interface
 *
 ************************************************************************************************/


/*
 * return : 0
 */
static int open(dev_t dev, int oflags, int devtype, d_thread_t *proc)
{
	return 0;
}


static int close(dev_t dev, int fflag, int devtype, d_thread_t *proc)
{
	return 0;
}


// return : Read blocks or Error number
static int read(dev_t dev, struct uio *uio, int ioflag)
{
	char *readBuf = uio->uio_iov->iov_base;
	uchar head, cyl, sector;
	int rest, error;
	uint i, last;

	if (uio->uio_iov->iov_len == 0){
		return 0;
	}
	if ((SECTORS * CYLINDERS * 2) <= uio->uio_offset + uio->uio_iov->iov_len){
		return -EINVAL;
	}

	motorOn();

	waitSemaphore(&wait_queue);
	{
		/* SMPなら割り込みが同じcpuに発生するようにする */
		if (MFPS_addres){
			set_intr_cpu(IRQ_NO, get_current_cpu());
		}

		/* RWHeadの初期化 */
		if ((rest = calibrateFd()) != 0){
			goto EXIT;
		}

		/* FDの各値の設定 */
		head = (uchar)((uio->uio_offset / SECTORS) & 1);
		cyl = (uchar)(uio->uio_offset / (SECTORS * 2));
		sector = (uchar)(uio->uio_offset % SECTORS);

		rest = uio->uio_iov->iov_len;
		for (i = uio->uio_offset, last = uio->uio_offset + uio->uio_iov->iov_len; i < last; ++i){
			if (SECTORS < ++sector){
				head = (uchar)((i / SECTORS) & 1);
				cyl = (uchar)(i / (SECTORS * 2));
				sector = 1;
			}

			/* Read fd */
			if ((error = read_fd(head, cyl, sector)) != 0){
				rest = error;
				break;
			}

			memcpy(readBuf, fdbuf, SECTOR_SIZE);
			readBuf += SECTOR_SIZE;
		}
	}
EXIT:
	wakeSemaphore(&wait_queue);

	motorOff();

	return rest;
}


// return : write number of sectors
static int write(dev_t dev, struct uio *uio, int ioflag)
{
	char *writeBuf = uio->uio_iov->iov_base;
	uchar head, cyl, sector;
	int rest, error;
	uint i, last;

	if (uio->uio_iov->iov_len == 0){
		return 0;
	}
	if ((SECTORS * CYLINDERS * 2) <= uio->uio_offset + uio->uio_iov->iov_len){
		return EINVAL;
	}

	motorOn();

	waitSemaphore(&wait_queue);
	{
		/* SMPなら割り込みが同じcpuに発生するようにする */
		if (MFPS_addres){
			set_intr_cpu(IRQ_NO, get_current_cpu());
		}

		/* RWHeadの初期化 */
		if ((rest = calibrateFd()) != 0){
			goto EXIT;
		}

		/* FDの各値の設定 */
		head = (uchar)((uio->uio_offset / SECTORS) & 1);
		cyl = (uchar)(uio->uio_offset / (SECTORS * 2));
		sector = (uchar)(uio->uio_offset % SECTORS);

		rest = uio->uio_iov->iov_len;
		for (i = uio->uio_offset, last = uio->uio_offset + uio->uio_iov->iov_len; i < last; ++i)
		{
			if(SECTORS < ++sector){
				head = (uchar)((i / SECTORS) & 1);
				cyl = (uchar)(i / (SECTORS * 2));
				sector = 1;
			}

			memcpy(fdbuf, writeBuf, SECTOR_SIZE);
			writeBuf += SECTOR_SIZE;

			/* Write fd */
			if ((error = write_fd(head, cyl, sector)) != 0){
				rest = error;
				break;
			}
		}
	}
EXIT:
	wakeSemaphore(&wait_queue);

	motorOff();

	return rest;
}


static int ioctl_fd(dev_t dev, u_long cmd, caddr_t data, int fflag, d_thread_t *p)
{
	return 0;
}


static int poll(dev_t dev, int events, d_thread_t *p)
{
	return POLLIN | POLLRDNORM | POLLOUT | POLLWRNORM;
}


/************************************************************************************************
 *
 * Init FD
 *
 ************************************************************************************************/


/* Device interface */
static struct cdevsw fd_info={
	open, 
	close, 
	read, 
	write, 
	ioctl_fd, 
	poll, 
	NULL, 
	NULL, 
	"fd", 
	0, 
	NULL, 
	NULL, 
	D_DISK, 
	0, 
	NULL, 
	SECTOR_SIZE, 
	0, 
	2880 - 1, 
};


/*
 * Init floppy drive
 * returns : 0 or Error=-1
 */
int init_fd()
{
	int error;

	/* Reset Floppy Drive */
	outb(FD_DOR,0);

	/* On Floppy Drive */
	outb(FD_DOR,0xc);

	/* Time out */
	time_out=clock_1m*TIME_OUT;

	/* 1.4MFloppyのRate 0=500Kbit */
	outb(FD_CCR,0);

	/* 割り込みの設定 */
	irq_entry[IRQ6]=fd_intr_handler;
	release_irq_mask(IRQ_NO);

	/* FloppyDriveパラメータのセット */
	param[0]=SPECIFY_FD_COMMAND;
	param[1]=0xd1;				/* StepRate=0xd(3ms),HeadUnloadTime=1(16ms) */
	param[2]=2;					/* HeadLoadTime=1(2ms),DMAMode */

	error = issue_command(SPECIFY_FD_PARAM);
	if (error != 0)
		return error;
	wait_intr(&wait_intr_queue,2000,TASK_DEVICE_WAIT);

	/* Init DMA */
	init_dma(READ_DMA);

	if (make_dev(&fd_info, 0, 0, 0, S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP, "floppy") == NODEV){
		return -ENODEV;
	}

	printk("fd : 1.44M FLOPPY DISK drive\n");

	return 0;
}

/***************************************************************************/
void test_fd()
{
}
/***************************************************************************/
