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


#include"types.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"


enum{
	/* 1.44Mspec */
	SECTER_SIZE=512,
	CYLINDERS=80,
	SECTERS=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=0xf,				/* Seek head */
	READ_COMMAND=0xe6, 				/* Read 1 sector */
	WRITE_COMMAND=0xc5,				/* Write 1 secter */
	SECTER_IDENTIFI_COMMAND=0x4a,	/* Read position of current secter */
	SPECIFY_FD_COMMAND=3,			/* 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 secter parameters */
	SECTER_IDENTIFI_PARAM=2,	/* Read position of current secter 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 secter results */
	SECTER_IDENTIFI_RESULT=7,	/* Read position of current secter 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=		/* 割り込み待ち用 */
	{NULL,0};
static WAIT_QUEUE wait_queue=			/* 処理待ち用Wait queue */
	{NULL,(PROC*)&wait_queue,0,0};
static uint64 time_out;					/* Time out value */
static int current_cyl;					/* Current cylinder position */
static int motor_ref=0;					/* Motor on refreance count */
static int motor_gate=0;				/* refreance count lock gate */


static void on_motor();
static void off_motor();
static int issue_command(int);
static int get_result(int);
static int fd_intr_handler();
static void init_dma(int);
static int calibrate_fd();
static int seek_fd(uchar);
static int _read_fd(uchar,uchar,uchar);
static int _write_fd(uchar,uchar,uchar);
static int open_fd();
static int read_fd(void*,size_t,size_t);
static int write_fd(void*,size_t,size_t);
static int ioctl_fd(int,void*);


static DEV_INFO fd_info={				/* Device interface */
	"fd",SECTER_SIZE,0,2880-1,open_fd,read_fd,write_fd,ioctl_fd
};


/*
 * FD motor on
 */
void on_motor()
{
	enter_spinlock(&motor_gate);
	{
		if(motor_ref==0)outb(FD_DOR,0x1c);		/* MOTA|DMA|Enable|Drive0 */
		++motor_ref;
	}
	exit_spinlock(&motor_gate);
}


/*
 * FD motor off
 */
void off_motor()
{
	enter_spinlock(&motor_gate);
	{
		--motor_ref;
		if(motor_ref==0)outb(FD_DOR,0xc);		/* DMA|Enable */
	}
	exit_spinlock(&motor_gate);
}


/*
 * Issue command
 * parametrs : Number of command operations,parameter array
 * returns : 0 or Error number
 */
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
 */
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(rdtsc()-count>time_out)return -EDBUSY;		/* Time out */
		}

		result[i]=inb(FD_DATA);
	}

	return 0;
}


/*
 * Intrrupt floppy drive handler
 * return : Task switch on
 */
int fd_intr_handler()
{
/**********************************************
	printk("Interrupt IRQ6\n");
**********************************************/

	return wake_intr(&wait_intr_queue);
}


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


/*
 * 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 */
	if((error=issue_command(SPECIFY_FD_PARAM))!=0)return error;

	if(wait_intr(&wait_intr_queue,2000)==-1)return -ETIMEOUT;

	/* Init DMA */
	init_dma(READ_DMA);

	if((error=regist_device(&fd_info))!=0)return error;

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

	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)(SECTER_SIZE-1));	/* 転送バイト数のLow byteを代入 */
	outb(COUNT_REG,(SECTER_SIZE-1)>>8);		/* 転送バイト数のHigh byteを代入 */
	outb(MASK_REG,0x2);						/* Mask clear channel2 */
}


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


/*
 * Calibrate floppy drive
 * On moter
 * return : 0 or Error number
 */
int calibrate_fd()
{
	int error;

	/*
	 * read write動作の後自動でcalibrateが起こるようで、
	 * wait_intr_queue.flagが1ならcalibrateされている。
	 */
	if(wait_intr_queue.flag==1)
	{
		wait_intr_queue.flag=0;
		return 0;
	}

	param[0]=CALIBRATE_COMMAND;
	param[1]=0;	/* Drive 0 */
	if((error=issue_command(CALIBRATE_PARAM))!=0)return error;

	if(wait_intr(&wait_intr_queue,2000)==-1)return -ETIMEOUT;

	current_cyl=0;

	return 0;
}


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


	if(cyl==current_cyl)return 0;

	param[0]=SEEK_COMMAND;
	param[1]=0;			/* Drive 0 */
	param[2]=cyl;		/* Cylinder */
	if((error=issue_command(SEEK_PARAM))!=0)return error;

	if(wait_intr(&wait_intr_queue,2000)==-1)return -ETIMEOUT;

	current_cyl=cyl;

	return 0;
}


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


	init_dma(READ_DMA);

	if((error=seek_fd(cyl))!=0)return error;

	param[0]=READ_COMMAND;
	param[1]=head<<2;
	param[2]=cyl;
	param[3]=head;
	param[4]=secter;
	param[5]=2;			/* Secter size=512 */
	param[6]=SECTERS;	/* last sector in track */
	param[7]=GAP3;
	param[8]=0xff;		/* If param[5]==0 data length,else 0xff */
	if((error=issue_command(READ_PARAM))!=0)return error;

	if(wait_intr(&wait_intr_queue,2000)==-1)return -ETIMEOUT;

	if((error=get_result(READ_RESULT))!=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,secter
 * return : 0 or Error number
 */
int _write_fd(uchar head,uchar cyl,uchar secter)
{
	int error;


	init_dma(WRITE_DMA);

	if((error=seek_fd(cyl))!=0)return error;

	param[0]=WRITE_COMMAND;
	param[1]=head<<2;
	param[2]=cyl;
	param[3]=head;
	param[4]=secter;
	param[5]=2;			/* Secter size=512 */
	param[6]=SECTERS;	/* last sector in track */
	param[7]=GAP3;
	param[8]=0xff;		/* If param[5]==0 data length,else 0xff */
	if((error=issue_command(WRITE_PARAM))!=0)return error;

	if(wait_intr(&wait_intr_queue,2000)==-1)return -ETIMEOUT;

	if((error=get_result(WRITE_RESULT))!=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
 */
int format_fd()
{
	return 0;
}


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


/*
 * return : 0
 */
int open_fd()
{
	return 0;
}


/*
 * parameters : buffer,read blocks,begin block
 * return : Read blocks or Error number
 */
int read_fd(void *buf,size_t blocks,size_t begin)
{
	uchar head,cyl,secter;
	int rest,error;
	uint i,last;


	if(blocks==0)return 0;
	if(begin+blocks>=(SECTERS*CYLINDERS*2))return -EINVAL;

	on_motor();

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

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

		/* FDの各値の設定 */
		head=(uchar)((begin/SECTERS)&1);
		cyl=(uchar)(begin/(SECTERS*2));
		secter=(uchar)(begin%SECTERS);

		rest=blocks;
		for(i=begin,last=begin+blocks;i<last;++i)
		{
			if(++secter>SECTERS)
			{
				head=(uchar)((i/SECTERS)&1);
				cyl=(uchar)(i/(SECTERS*2));
				secter=1;
			}

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

			memcpy(buf,fdbuf,SECTER_SIZE);
			(uint)buf+=SECTER_SIZE;
		}
	}
EXIT:
	wake_proc(&wait_queue);

	off_motor();

	return rest;
}


/*
 * parameters : buffer,Write blocks,begin block
 * return : write number of secters
 */
int write_fd(void *buf,size_t blocks,size_t begin)
{
	uchar head,cyl,secter;
	int rest,error;
	uint i,last;


	if(blocks==0)return 0;
	if(begin+blocks>=(SECTERS*CYLINDERS*2))return EINVAL;

	on_motor();

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

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

		/* FDの各値の設定 */
		head=(uchar)((begin/SECTERS)&1);
		cyl=(uchar)(begin/(SECTERS*2));
		secter=(uchar)(begin%SECTERS);

		rest=blocks;
		for(i=begin,last=begin+blocks;i<last;++i)
		{
			if(++secter>SECTERS)
			{
				head=(uchar)((i/SECTERS)&1);
				cyl=(uchar)(i/(SECTERS*2));
				secter=1;
			}

			memcpy(fdbuf,buf,SECTER_SIZE);
			(uint)buf+=SECTER_SIZE;

			/* Write fd */
			if((error=_write_fd(head,cyl,secter))!=0)
			{
				rest=error;
				break;
			}
		}
	}
EXIT:
	wake_proc(&wait_queue);

	off_motor();

	return rest;
}


int ioctl_fd(int command,void *param)
{
	return 0;
}
/***************************************************************************/
void test_fd()
{
	char *buf1=(char*)0x80000;
	char *buf2=(char*)0x90000;

	buf1[0]=0xaa;
	buf1[511]=0xbb;

	printk("write_fd=%x\n",write_fd(buf1,1,0));
	printk("read_fd=%x\n",read_fd(buf2,1,0));

	printk("buf2[0]=%x,buf2[511]=%x\n",buf2[0],buf2[511]);
}
/***************************************************************************/
