bafa3a8eb3
currently only read okay, write is not supported, this is enough for
me to study ELF, but the floppy is really small, now only about 400kb
space free showed by df("/").
Signed-off-by: parai <parai@foxmail.com>
361 lines
9.0 KiB
C
361 lines
9.0 KiB
C
|
|
#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,§or);
|
|
|
|
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);
|
|
|
|
}
|