#include <rtthread.h>
#include <rthw.h>

#include <bsp.h>

typedef rt_uint8_t  u8;
typedef rt_uint16_t u16;
typedef rt_uint32_t u32;

typedef rt_int8_t  s8;
typedef rt_int16_t s16;
typedef rt_int32_t s32;

#define OUTB(v,p) outb(p,v)

#include "floppy.h"
#include "dma.h"

#define NULL RT_NULL
#define SECTOR_SIZE 512
#define panic(str,...) do { rt_kprintf("panic::" str,##__VA_ARGS__); while(1); } while(0)

#define _local_irq_save(level) level = rt_hw_interrupt_disable()
#define _local_irq_restore(level) rt_hw_interrupt_enable(level)

static u8 floppy_buffer[512];                       /* 软盘高速缓冲区地址指针 */

#define MAX_REPLIES 7                                                             
static u8 floppy_reply_buffer[MAX_REPLIES];         /* 软驱回应缓冲区 */
#define ST0 (floppy_reply_buffer[0])                /* 软驱回应0号字节 */
#define ST1 (floppy_reply_buffer[1])                /* 软驱回应1号字节 */
#define ST2 (floppy_reply_buffer[2])                /* 软驱回应2号字节 */
#define ST3 (floppy_reply_buffer[3])                /* 软驱回应3号字节 */


static char *floppy_inc_name;                       /* 软驱型号名 */
static char *floppy_type;
static u32  floppy_motor=0;                         /* 软驱马达状态字节 */
static u32  floppy_size =0;
/**********************功能函数***************************/
static void floppy_result(void);                    /* 获得软驱响应状态  */
static u32  floppy_sendbyte(u32);                   /* 向软驱控制寄存器发送一个控制字节  */
static u32  floppy_getbyte(void);                   /* 从软驱数据寄存器得到一个数据字节  */
static u32  floppy_get_info(void);                  /* 得到软驱信息  */
static void floppy_motorOn(void);                   /* 打开软驱马达  */
static void floppy_motorOff(void);                  /* 关闭软驱马达  */
static void floppy_setmode(void);                   /* 软驱模式设置  */
static void block_to_hts(u32, u32*, u32*, u32*);    /* 逻辑块转为磁盘头、磁道号和扇区号  */
static void floppy_setupDMA(void);                  /* 设置软驱DMA通道  */
static void floppy_read_cmd(u32 blk);               /* 从软盘上读取指定的逻辑块到缓冲区  */


void floppy_result(void)
{
    u8 stat, i,count;
    i=0;
    for(count=0; count<0xFF; count++)
    {
        stat = inb( FD_STATUS ) & (STATUS_READY|STATUS_DIR|STATUS_BUSY); //读取状态寄存器
        if (stat == STATUS_READY)
            return;
        if (stat == (STATUS_READY|STATUS_DIR|STATUS_BUSY))
        {
            if(i>7) break;
            floppy_reply_buffer[i++]=inb_p(FD_DATA);
        }
    }

    panic("Get floppy status times out !\n");
}

u32 floppy_sendbyte( u32 value )
{
    u8 stat, i;

    for ( i = 0; i < 128; i++ ) {
        stat = inb( FD_STATUS ) & (STATUS_READY|STATUS_DIR);    //读取状态寄存器
        if  ( stat  == STATUS_READY )
        {
            OUTB( value ,FD_DATA);                              //将参数写入数据寄存器
            return 1;
        }
        io_delay();                                             // 作一些延迟
    }
    return 0;
}


u32 floppy_getbyte(void)
{
    u8 stat, i;

    for ( i = 0; i < 128; i++ ) {
        stat = inb( FD_STATUS ) & (STATUS_READY|STATUS_DIR|STATUS_BUSY); //读取状态寄存器
        if (stat == STATUS_READY)
            return -1;
        if ( stat  == 0xD0 )
            return inb(FD_DATA);
        io_delay();
    }
    return 0;
}


u32 floppy_get_info(void)
{
    u32 i;
    u8 CmType, FdType;

    floppy_sendbyte(0x10);
    i = floppy_getbyte(); 

    switch (i)
    {
        case 0x80:   floppy_inc_name = "NEC765A controller"; break;
        case 0x90:   floppy_inc_name = "NEC765B controller"; break;
        default:     floppy_inc_name = "Enhanced controller"; break;
    }

    CmType = readcmos(0x10);        //read floppy type from cmos
    FdType   = (CmType>>4) & 0x07;

    if ( FdType == 0 )
        panic("Floppy driver not found!");

    switch( FdType )
    {
    case 0x02: // 1.2MB
        floppy_type = "1.2MB";
		floppy_size = 2458*512;
    break;

    case 0x04: // 1.44MB       标准软盘
        floppy_type = "1.44MB";
		floppy_size = 2880*512;
        break;

    case 0x05: // 2.88MB
        floppy_type = "2.88MB";
		floppy_size = 2*2880*512;
        break;
    }
    return 1;
}


void floppy_motorOn( void )
{
    u32 eflags;
    if (!floppy_motor)
    {
        _local_irq_save(eflags);
        OUTB(28,FD_DOR);
        floppy_motor = 1;
        _local_irq_restore(eflags);
    }
    return;
}


void floppy_motorOff( void )
{
    u32 eflags;
    if (floppy_motor)
    {
        _local_irq_save(eflags);
        OUTB(12,FD_DOR);
        floppy_motor = 0;
        _local_irq_restore(eflags);

    }
    return;
}


void floppy_setmode(void)
{   
    floppy_sendbyte (FD_SPECIFY);
    floppy_sendbyte (0xcf);
    floppy_sendbyte (0x06);
    OUTB (0,FD_DCR);
}


void block_to_hts(u32 block, u32 *head, u32 *track, u32 *sector )
{
    *head = ( block % ( 18 * 2 ) ) /18;
    *track =  block / ( 18 * 2 );
    *sector = block % 18 + 1;
}


void floppy_setupDMA(void)
{  
    u32 eflags;
    _local_irq_save(eflags);
    DisableDma(2);
    ClearDmaFF(2);
    SetDmaMode(2,DMA_MODE_READ);
    SetDmaAddr(2,(unsigned long)floppy_buffer);
    SetDmaCount(2,512);
    EnableDma(2);
    _local_irq_restore(eflags);
}


void floppy_read_cmd(u32 blk)
{
    u32 head;
    u32 track;
    u32 sector;

    block_to_hts(blk,&head,&track,&sector);

    floppy_motorOn();
    io_delay();

    floppy_setupDMA();
    io_delay();

    floppy_setmode();
    io_delay();
    floppy_sendbyte (FD_READ);                  //send read command
    floppy_sendbyte (head*4 + 0);
    floppy_sendbyte (track);                    /*  Cylinder  */
    floppy_sendbyte (head);                     /*  Head  */
    floppy_sendbyte (sector);                   /*  Sector  */
    floppy_sendbyte (2);                        /*  0=128, 1=256, 2=512, 3=1024, ...  */
    floppy_sendbyte (18);
    //floppy_sendbyte (sector+secs-1);          /*  Last sector in track:here are  sectors count */
    floppy_sendbyte (0x1B);
    floppy_sendbyte (0xff);                      
    return;
}

static struct rt_device devF;
static struct rt_mutex lock;
static struct rt_semaphore sem;

/* RT-Thread device interface */

static rt_err_t rt_floppy_init_internal(rt_device_t dev)
{
    return RT_EOK;
}

static rt_err_t rt_floppy_open(rt_device_t dev, rt_uint16_t oflag)
{
    return RT_EOK;
}

static rt_err_t rt_floppy_close(rt_device_t dev)
{
    return RT_EOK;
}

/* position: block page address, not bytes address
 * buffer:
 * size  : how many blocks
 */
static rt_size_t rt_floppy_read(rt_device_t device, rt_off_t position, void *buffer, rt_size_t size)
{
	rt_size_t doSize = size;

    rt_mutex_take(&lock, RT_WAITING_FOREVER);
	while(size>0)
	{
		floppy_read_cmd(position);

		rt_sem_take(&sem, RT_WAITING_FOREVER); /* waiting isr sem forever */

		floppy_result();
		io_delay();

		if(ST1 != 0 || ST2 != 0)
		{
			panic("ST0 %d ST1 %d ST2 %d\n",ST0,ST1,ST2);
		}
    
		rt_memcpy(buffer, floppy_buffer, 512);

		floppy_motorOff();
		io_delay();
		
		position += 1;
		size     -= 1;
	}
	rt_mutex_release(&lock);

    return doSize;
}

/* position: block page address, not bytes address
 * buffer:
 * size  : how many blocks
 */
static rt_size_t rt_floppy_write(rt_device_t device, rt_off_t position, const void *buffer, rt_size_t size)
{
    rt_mutex_take(&lock, RT_WAITING_FOREVER);
	panic("FIXME:I don't know how!\n");
    rt_mutex_release(&lock);
    return size;
}

static rt_err_t rt_floppy_control(rt_device_t dev, rt_uint8_t cmd, void *args)
{
    RT_ASSERT(dev != RT_NULL);

    if (cmd == RT_DEVICE_CTRL_BLK_GETGEOME)
    {
        struct rt_device_blk_geometry *geometry;

        geometry = (struct rt_device_blk_geometry *)args;
        if (geometry == RT_NULL) return -RT_ERROR;

        geometry->bytes_per_sector = SECTOR_SIZE;
        geometry->block_size = SECTOR_SIZE;

        geometry->sector_count = floppy_size / SECTOR_SIZE;
    }

    return RT_EOK;
}

static void rt_floppy_isr(int vector, void* param)
{
	(void)vector;
	(void)param;
	rt_sem_release(&sem);
}

void rt_floppy_init(void)
{
    struct rt_device *device;

    rt_mutex_init(&lock,"fdlock", RT_IPC_FLAG_FIFO);
	rt_sem_init(&sem, "fdsem", 0, RT_IPC_FLAG_FIFO);

	rt_hw_interrupt_install(FLOPPY_IRQ, rt_floppy_isr, RT_NULL, "floppy");
    rt_hw_interrupt_umask(FLOPPY_IRQ);

    floppy_get_info();
    rt_kprintf("Floppy Inc : %s  Floppy Type : %s\n",floppy_inc_name,floppy_type);

    device = &(devF);

    device->type  = RT_Device_Class_Block;
    device->init = rt_floppy_init_internal;
    device->open = rt_floppy_open;
    device->close = rt_floppy_close;
    device->read = rt_floppy_read;
    device->write = rt_floppy_write;
    device->control = rt_floppy_control;
    device->user_data = NULL;

    rt_device_register(device, "floppy",
                       RT_DEVICE_FLAG_RDWR | RT_DEVICE_FLAG_REMOVABLE | RT_DEVICE_FLAG_STANDALONE);

}