1 files changed, 462 insertions, 0 deletions
diff --git a/src/kernel/blk_drv/floppy.c b/src/kernel/blk_drv/floppy.c
new file mode 100644
index 0000000..f497906
--- /dev/null
+++ b/src/kernel/blk_drv/floppy.c
@@ -0,0 +1,462 @@
+/*
+ *  linux/kernel/floppy.c
+ *
+ *  (C) 1991  Linus Torvalds
+ */
+/*
+ * 02.12.91 - Changed to static variables to indicate need for reset
+ * and recalibrate. This makes some things easier (output_byte reset
+ * checking etc), and means less interrupt jumping in case of errors,
+ * so the code is hopefully easier to understand.
+ */
+/*
+ * This file is certainly a mess. I've tried my best to get it working,
+ * but I don't like programming floppies, and I have only one anyway.
+ * Urgel. I should check for more errors, and do more graceful error
+ * recovery. Seems there are problems with several drives. I've tried to
+ * correct them. No promises. 
+ */
+/*
+ * As with hd.c, all routines within this file can (and will) be called
+ * by interrupts, so extreme caution is needed. A hardware interrupt
+ * handler may not sleep, or a kernel panic will happen. Thus I cannot
+ * call "floppy-on" directly, but have to set a special timer interrupt
+ * etc.
+ *
+ * Also, I'm not certain this works on more than 1 floppy. Bugs may
+ * abund.
+ */
+#include <linux/sched.h>
+#include <linux/fs.h>
+#include <linux/kernel.h>
+#include <linux/fdreg.h>
+#include <asm/system.h>
+#include <asm/io.h>
+#include <asm/segment.h>
+#define MAJOR_NR 2
+#include "blk.h"
+static int recalibrate = 0;
+static int reset = 0;
+static int seek = 0;
+extern unsigned char current_DOR;
+#define immoutb_p(val,port) \
+__asm__("outb %0,%1\n\tjmp 1f\n1:\tjmp 1f\n1:"::"a" ((char) (val)),"i" (port))
+#define TYPE(x) ((x)>>2)
+#define DRIVE(x) ((x)&0x03)
+/*
+ * Note that MAX_ERRORS=8 doesn't imply that we retry every bad read
+ * max 8 times - some types of errors increase the errorcount by 2,
+ * so we might actually retry only 5-6 times before giving up.
+ */
+#define MAX_ERRORS 8
+/*
+ * globals used by 'result()'
+ */
+#define MAX_REPLIES 7
+static unsigned char reply_buffer[MAX_REPLIES];
+#define ST0 (reply_buffer[0])
+#define ST1 (reply_buffer[1])
+#define ST2 (reply_buffer[2])
+#define ST3 (reply_buffer[3])
+/*
+ * This struct defines the different floppy types. Unlike minix
+ * linux doesn't have a "search for right type"-type, as the code
+ * for that is convoluted and weird. I've got enough problems with
+ * this driver as it is.
+ *
+ * The 'stretch' tells if the tracks need to be boubled for some
+ * types (ie 360kB diskette in 1.2MB drive etc). Others should
+ * be self-explanatory.
+ */
+static struct floppy_struct {
+        unsigned int size, sect, head, track, stretch;
+        unsigned char gap,rate,spec1;
+} floppy_type[] = {
+        {    0, 0,0, 0,0,0x00,0x00,0x00 },      /* no testing */
+        {  720, 9,2,40,0,0x2A,0x02,0xDF },      /* 360kB PC diskettes */
+        { 2400,15,2,80,0,0x1B,0x00,0xDF },      /* 1.2 MB AT-diskettes */
+        {  720, 9,2,40,1,0x2A,0x02,0xDF },      /* 360kB in 720kB drive */
+        { 1440, 9,2,80,0,0x2A,0x02,0xDF },      /* 3.5" 720kB diskette */
+        {  720, 9,2,40,1,0x23,0x01,0xDF },      /* 360kB in 1.2MB drive */
+        { 1440, 9,2,80,0,0x23,0x01,0xDF },      /* 720kB in 1.2MB drive */
+        { 2880,18,2,80,0,0x1B,0x00,0xCF },      /* 1.44MB diskette */
+};
+/*
+ * Rate is 0 for 500kb/s, 2 for 300kbps, 1 for 250kbps
+ * Spec1 is 0xSH, where S is stepping rate (F=1ms, E=2ms, D=3ms etc),
+ * H is head unload time (1=16ms, 2=32ms, etc)
+ *
+ * Spec2 is (HLD<<1 | ND), where HLD is head load time (1=2ms, 2=4 ms etc)
+ * and ND is set means no DMA. Hardcoded to 6 (HLD=6ms, use DMA).
+ */
+extern void floppy_interrupt(void);
+extern char tmp_floppy_area[1024];
+/*
+ * These are global variables, as that's the easiest way to give
+ * information to interrupts. They are the data used for the current
+ * request.
+ */
+static int cur_spec1 = -1;
+static int cur_rate = -1;
+static struct floppy_struct * floppy = floppy_type;
+static unsigned char current_drive = 0;
+static unsigned char sector = 0;
+static unsigned char head = 0;
+static unsigned char track = 0;
+static unsigned char seek_track = 0;
+static unsigned char current_track = 255;
+static unsigned char command = 0;
+unsigned char selected = 0;
+struct task_struct * wait_on_floppy_select = NULL;
+void floppy_deselect(unsigned int nr)
+{
+        if (nr != (current_DOR & 3))
+                printk("floppy_deselect: drive not selected\n\r");
+        selected = 0;
+        wake_up(&wait_on_floppy_select);
+}
+/*
+ * floppy-change is never called from an interrupt, so we can relax a bit
+ * here, sleep etc. Note that floppy-on tries to set current_DOR to point
+ * to the desired drive, but it will probably not survive the sleep if
+ * several floppies are used at the same time: thus the loop.
+ */
+int floppy_change(unsigned int nr)
+{
+repeat:
+        floppy_on(nr);
+        while ((current_DOR & 3) != nr && selected)
+                interruptible_sleep_on(&wait_on_floppy_select);
+        if ((current_DOR & 3) != nr)
+                goto repeat;
+        if (inb(FD_DIR) & 0x80) {
+                floppy_off(nr);
+                return 1;
+        }
+        floppy_off(nr);
+        return 0;
+}
+#define copy_buffer(from,to) \
+__asm__("cld ; rep ; movsl" \
+        ::"c" (BLOCK_SIZE/4),"S" ((long)(from)),"D" ((long)(to)) \
+        )
+static void setup_DMA(void)
+{
+        long addr = (long) CURRENT->buffer;
+        cli();
+        if (addr >= 0x100000) {
+                addr = (long) tmp_floppy_area;
+                if (command == FD_WRITE)
+                        copy_buffer(CURRENT->buffer,tmp_floppy_area);
+        }
+/* mask DMA 2 */
+        immoutb_p(4|2,10);
+/* output command byte. I don't know why, but everyone (minix, */
+/* sanches & canton) output this twice, first to 12 then to 11 */
+        __asm__("outb %%al,$12\n\tjmp 1f\n1:\tjmp 1f\n1:\t"
+        "outb %%al,$11\n\tjmp 1f\n1:\tjmp 1f\n1:"::
+        "a" ((char) ((command == FD_READ)?DMA_READ:DMA_WRITE)));
+/* 8 low bits of addr */
+        immoutb_p(addr,4);
+        addr >>= 8;
+/* bits 8-15 of addr */
+        immoutb_p(addr,4);
+        addr >>= 8;
+/* bits 16-19 of addr */
+        immoutb_p(addr,0x81);
+/* low 8 bits of count-1 (1024-1=0x3ff) */
+        immoutb_p(0xff,5);
+/* high 8 bits of count-1 */
+        immoutb_p(3,5);
+/* activate DMA 2 */
+        immoutb_p(0|2,10);
+        sti();
+}
+static void output_byte(char byte)
+{
+        int counter;
+        unsigned char status;
+        if (reset)
+                return;
+        for(counter = 0 ; counter < 10000 ; counter++) {
+                status = inb_p(FD_STATUS) & (STATUS_READY | STATUS_DIR);
+                if (status == STATUS_READY) {
+                        outb(byte,FD_DATA);
+                        return;
+                }
+        }
+        reset = 1;
+        printk("Unable to send byte to FDC\n\r");
+}
+static int result(void)
+{
+        int i = 0, counter, status;
+        if (reset)
+                return -1;
+        for (counter = 0 ; counter < 10000 ; counter++) {
+                status = inb_p(FD_STATUS)&(STATUS_DIR|STATUS_READY|STATUS_BUSY);
+                if (status == STATUS_READY)
+                        return i;
+                if (status == (STATUS_DIR|STATUS_READY|STATUS_BUSY)) {
+                        if (i >= MAX_REPLIES)
+                                break;
+                        reply_buffer[i++] = inb_p(FD_DATA);
+                }
+        }
+        reset = 1;
+        printk("Getstatus times out\n\r");
+        return -1;
+}
+static void bad_flp_intr(void)
+{
+        CURRENT->errors++;
+        if (CURRENT->errors > MAX_ERRORS) {
+                floppy_deselect(current_drive);
+                end_request(0);
+        }
+        if (CURRENT->errors > MAX_ERRORS/2)
+                reset = 1;
+        else
+                recalibrate = 1;
+}       
+/*
+ * Ok, this interrupt is called after a DMA read/write has succeeded,
+ * so we check the results, and copy any buffers.
+ */
+static void rw_interrupt(void)
+{
+        if (result() != 7 || (ST0 & 0xf8) || (ST1 & 0xbf) || (ST2 & 0x73)) {
+                if (ST1 & 0x02) {
+                        printk("Drive %d is write protected\n\r",current_drive);
+                        floppy_deselect(current_drive);
+                        end_request(0);
+                } else
+                        bad_flp_intr();
+                do_fd_request();
+                return;
+        }
+        if (command == FD_READ && (unsigned long)(CURRENT->buffer) >= 0x100000)
+                copy_buffer(tmp_floppy_area,CURRENT->buffer);
+        floppy_deselect(current_drive);
+        end_request(1);
+        do_fd_request();
+}
+static inline void setup_rw_floppy(void)
+{
+        setup_DMA();
+        do_floppy = rw_interrupt;
+        output_byte(command);
+        output_byte(head<<2 | current_drive);
+        output_byte(track);
+        output_byte(head);
+        output_byte(sector);
+        output_byte(2);         /* sector size = 512 */
+        output_byte(floppy->sect);
+        output_byte(floppy->gap);
+        output_byte(0xFF);      /* sector size (0xff when n!=0 ?) */
+        if (reset)
+                do_fd_request();
+}
+/*
+ * This is the routine called after every seek (or recalibrate) interrupt
+ * from the floppy controller. Note that the "unexpected interrupt" routine
+ * also does a recalibrate, but doesn't come here.
+ */
+static void seek_interrupt(void)
+{
+/* sense drive status */
+        output_byte(FD_SENSEI);
+        if (result() != 2 || (ST0 & 0xF8) != 0x20 || ST1 != seek_track) {
+                bad_flp_intr();
+                do_fd_request();
+                return;
+        }
+        current_track = ST1;
+        setup_rw_floppy();
+}
+/*
+ * This routine is called when everything should be correctly set up
+ * for the transfer (ie floppy motor is on and the correct floppy is
+ * selected).
+ */
+static void transfer(void)
+{
+        if (cur_spec1 != floppy->spec1) {
+                cur_spec1 = floppy->spec1;
+                output_byte(FD_SPECIFY);
+                output_byte(cur_spec1);         /* hut etc */
+                output_byte(6);                 /* Head load time =6ms, DMA */
+        }
+        if (cur_rate != floppy->rate)
+                outb_p(cur_rate = floppy->rate,FD_DCR);
+        if (reset) {
+                do_fd_request();
+                return;
+        }
+        if (!seek) {
+                setup_rw_floppy();
+                return;
+        }
+        do_floppy = seek_interrupt;
+        if (seek_track) {
+                output_byte(FD_SEEK);
+                output_byte(head<<2 | current_drive);
+                output_byte(seek_track);
+        } else {
+                output_byte(FD_RECALIBRATE);
+                output_byte(head<<2 | current_drive);
+        }
+        if (reset)
+                do_fd_request();
+}
+/*
+ * Special case - used after a unexpected interrupt (or reset)
+ */
+static void recal_interrupt(void)
+{
+        output_byte(FD_SENSEI);
+        if (result()!=2 || (ST0 & 0xE0) == 0x60)
+                reset = 1;
+        else
+                recalibrate = 0;
+        do_fd_request();
+}
+void unexpected_floppy_interrupt(void)
+{
+        output_byte(FD_SENSEI);
+        if (result()!=2 || (ST0 & 0xE0) == 0x60)
+                reset = 1;
+        else
+                recalibrate = 1;
+}
+static void recalibrate_floppy(void)
+{
+        recalibrate = 0;
+        current_track = 0;
+        do_floppy = recal_interrupt;
+        output_byte(FD_RECALIBRATE);
+        output_byte(head<<2 | current_drive);
+        if (reset)
+                do_fd_request();
+}
+static void reset_interrupt(void)
+{
+        output_byte(FD_SENSEI);
+        (void) result();
+        output_byte(FD_SPECIFY);
+        output_byte(cur_spec1);         /* hut etc */
+        output_byte(6);                 /* Head load time =6ms, DMA */
+        do_fd_request();
+}
+/*
+ * reset is done by pulling bit 2 of DOR low for a while.
+ */
+static void reset_floppy(void)
+{
+        int i;
+        reset = 0;
+        cur_spec1 = -1;
+        cur_rate = -1;
+        recalibrate = 1;
+        printk("Reset-floppy called\n\r");
+        cli();
+        do_floppy = reset_interrupt;
+        outb_p(current_DOR & ~0x04,FD_DOR);
+        for (i=0 ; i<100 ; i++)
+                __asm__("nop");
+        outb(current_DOR,FD_DOR);
+        sti();
+}
+static void floppy_on_interrupt(void)
+{
+/* We cannot do a floppy-select, as that might sleep. We just force it */
+        selected = 1;
+        if (current_drive != (current_DOR & 3)) {
+                current_DOR &= 0xFC;
+                current_DOR |= current_drive;
+                outb(current_DOR,FD_DOR);
+                add_timer(2,&transfer);
+        } else
+                transfer();
+}
+void do_fd_request(void)
+{
+        unsigned int block;
+        seek = 0;
+        if (reset) {
+                reset_floppy();
+                return;
+        }
+        if (recalibrate) {
+                recalibrate_floppy();
+                return;
+        }
+        INIT_REQUEST;
+        floppy = (MINOR(CURRENT->dev)>>2) + floppy_type;
+        if (current_drive != CURRENT_DEV)
+                seek = 1;
+        current_drive = CURRENT_DEV;
+        block = CURRENT->sector;
+        if (block+2 > floppy->size) {
+                end_request(0);
+                goto repeat;
+        }
+        sector = block % floppy->sect;
+        block /= floppy->sect;
+        head = block % floppy->head;
+        track = block / floppy->head;
+        seek_track = track << floppy->stretch;
+        if (seek_track != current_track)
+                seek = 1;
+        sector++;
+        if (CURRENT->cmd == READ)
+                command = FD_READ;
+        else if (CURRENT->cmd == WRITE)
+                command = FD_WRITE;
+        else
+                panic("do_fd_request: unknown command");
+        add_timer(ticks_to_floppy_on(current_drive),&floppy_on_interrupt);
+}
+void floppy_init(void)
+{
+        blk_dev[MAJOR_NR].request_fn = DEVICE_REQUEST;
+        set_trap_gate(0x26,&floppy_interrupt);
+        outb(inb_p(0x21)&~0x40,0x21);
+}

diff --git a/src/kernel/blk_drv/floppy.c b/src/kernel/blk_drv/floppy.c new file mode 100644 index 0000000..f497906 --- /dev/null +++ b/src/kernel/blk_drv/floppy.c
@@ -0,0 +1,462 @@
	1	/*
	2	* linux/kernel/floppy.c
	3	*
	4	* (C) 1991 Linus Torvalds
	5	*/
	6
	7	/*
	8	* 02.12.91 - Changed to static variables to indicate need for reset
	9	* and recalibrate. This makes some things easier (output_byte reset
	10	* checking etc), and means less interrupt jumping in case of errors,
	11	* so the code is hopefully easier to understand.
	12	*/
	13
	14	/*
	15	* This file is certainly a mess. I've tried my best to get it working,
	16	* but I don't like programming floppies, and I have only one anyway.
	17	* Urgel. I should check for more errors, and do more graceful error
	18	* recovery. Seems there are problems with several drives. I've tried to
	19	* correct them. No promises.
	20	*/
	21
	22	/*
	23	* As with hd.c, all routines within this file can (and will) be called
	24	* by interrupts, so extreme caution is needed. A hardware interrupt
	25	* handler may not sleep, or a kernel panic will happen. Thus I cannot
	26	* call "floppy-on" directly, but have to set a special timer interrupt
	27	* etc.
	28	*
	29	* Also, I'm not certain this works on more than 1 floppy. Bugs may
	30	* abund.
	31	*/
	32
	33	#include <linux/sched.h>
	34	#include <linux/fs.h>
	35	#include <linux/kernel.h>
	36	#include <linux/fdreg.h>
	37	#include <asm/system.h>
	38	#include <asm/io.h>
	39	#include <asm/segment.h>
	40
	41	#define MAJOR_NR 2
	42	#include "blk.h"
	43
	44	static int recalibrate = 0;
	45	static int reset = 0;
	46	static int seek = 0;
	47
	48	extern unsigned char current_DOR;
	49
	50	#define immoutb_p(val,port) \
	51	__asm__("outb %0,%1\n\tjmp 1f\n1:\tjmp 1f\n1:"::"a" ((char) (val)),"i" (port))
	52
	53	#define TYPE(x) ((x)>>2)
	54	#define DRIVE(x) ((x)&0x03)
	55	/*
	56	* Note that MAX_ERRORS=8 doesn't imply that we retry every bad read
	57	* max 8 times - some types of errors increase the errorcount by 2,
	58	* so we might actually retry only 5-6 times before giving up.
	59	*/
	60	#define MAX_ERRORS 8
	61
	62	/*
	63	* globals used by 'result()'
	64	*/
	65	#define MAX_REPLIES 7
	66	static unsigned char reply_buffer[MAX_REPLIES];
	67	#define ST0 (reply_buffer[0])
	68	#define ST1 (reply_buffer[1])
	69	#define ST2 (reply_buffer[2])
	70	#define ST3 (reply_buffer[3])
	71
	72	/*
	73	* This struct defines the different floppy types. Unlike minix
	74	* linux doesn't have a "search for right type"-type, as the code
	75	* for that is convoluted and weird. I've got enough problems with
	76	* this driver as it is.
	77	*
	78	* The 'stretch' tells if the tracks need to be boubled for some
	79	* types (ie 360kB diskette in 1.2MB drive etc). Others should
	80	* be self-explanatory.
	81	*/
	82	static struct floppy_struct {
	83	unsigned int size, sect, head, track, stretch;
	84	unsigned char gap,rate,spec1;
	85	} floppy_type[] = {
	86	{ 0, 0,0, 0,0,0x00,0x00,0x00 }, /* no testing */
	87	{ 720, 9,2,40,0,0x2A,0x02,0xDF }, /* 360kB PC diskettes */
	88	{ 2400,15,2,80,0,0x1B,0x00,0xDF }, /* 1.2 MB AT-diskettes */
	89	{ 720, 9,2,40,1,0x2A,0x02,0xDF }, /* 360kB in 720kB drive */
	90	{ 1440, 9,2,80,0,0x2A,0x02,0xDF }, /* 3.5" 720kB diskette */
	91	{ 720, 9,2,40,1,0x23,0x01,0xDF }, /* 360kB in 1.2MB drive */
	92	{ 1440, 9,2,80,0,0x23,0x01,0xDF }, /* 720kB in 1.2MB drive */
	93	{ 2880,18,2,80,0,0x1B,0x00,0xCF }, /* 1.44MB diskette */
	94	};
	95	/*
	96	* Rate is 0 for 500kb/s, 2 for 300kbps, 1 for 250kbps
	97	* Spec1 is 0xSH, where S is stepping rate (F=1ms, E=2ms, D=3ms etc),
	98	* H is head unload time (1=16ms, 2=32ms, etc)
	99	*
	100	* Spec2 is (HLD<<1 \| ND), where HLD is head load time (1=2ms, 2=4 ms etc)
	101	* and ND is set means no DMA. Hardcoded to 6 (HLD=6ms, use DMA).
	102	*/
	103
	104	extern void floppy_interrupt(void);
	105	extern char tmp_floppy_area[1024];
	106
	107	/*
	108	* These are global variables, as that's the easiest way to give
	109	* information to interrupts. They are the data used for the current
	110	* request.
	111	*/
	112	static int cur_spec1 = -1;
	113	static int cur_rate = -1;
	114	static struct floppy_struct * floppy = floppy_type;
	115	static unsigned char current_drive = 0;
	116	static unsigned char sector = 0;
	117	static unsigned char head = 0;
	118	static unsigned char track = 0;
	119	static unsigned char seek_track = 0;
	120	static unsigned char current_track = 255;
	121	static unsigned char command = 0;
	122	unsigned char selected = 0;
	123	struct task_struct * wait_on_floppy_select = NULL;
	124
	125	void floppy_deselect(unsigned int nr)
	126	{
	127	if (nr != (current_DOR & 3))
	128	printk("floppy_deselect: drive not selected\n\r");
	129	selected = 0;
	130	wake_up(&wait_on_floppy_select);
	131	}
	132
	133	/*
	134	* floppy-change is never called from an interrupt, so we can relax a bit
	135	* here, sleep etc. Note that floppy-on tries to set current_DOR to point
	136	* to the desired drive, but it will probably not survive the sleep if
	137	* several floppies are used at the same time: thus the loop.
	138	*/
	139	int floppy_change(unsigned int nr)
	140	{
	141	repeat:
	142	floppy_on(nr);
	143	while ((current_DOR & 3) != nr && selected)
	144	interruptible_sleep_on(&wait_on_floppy_select);
	145	if ((current_DOR & 3) != nr)
	146	goto repeat;
	147	if (inb(FD_DIR) & 0x80) {
	148	floppy_off(nr);
	149	return 1;
	150	}
	151	floppy_off(nr);
	152	return 0;
	153	}
	154
	155	#define copy_buffer(from,to) \
	156	__asm__("cld ; rep ; movsl" \
	157	::"c" (BLOCK_SIZE/4),"S" ((long)(from)),"D" ((long)(to)) \
	158	)
	159
	160	static void setup_DMA(void)
	161	{
	162	long addr = (long) CURRENT->buffer;
	163
	164	cli();
	165	if (addr >= 0x100000) {
	166	addr = (long) tmp_floppy_area;
	167	if (command == FD_WRITE)
	168	copy_buffer(CURRENT->buffer,tmp_floppy_area);
	169	}
	170	/* mask DMA 2 */
	171	immoutb_p(4\|2,10);
	172	/* output command byte. I don't know why, but everyone (minix, */
	173	/* sanches & canton) output this twice, first to 12 then to 11 */
	174	__asm__("outb %%al,$12\n\tjmp 1f\n1:\tjmp 1f\n1:\t"
	175	"outb %%al,$11\n\tjmp 1f\n1:\tjmp 1f\n1:"::
	176	"a" ((char) ((command == FD_READ)?DMA_READ:DMA_WRITE)));
	177	/* 8 low bits of addr */
	178	immoutb_p(addr,4);
	179	addr >>= 8;
	180	/* bits 8-15 of addr */
	181	immoutb_p(addr,4);
	182	addr >>= 8;
	183	/* bits 16-19 of addr */
	184	immoutb_p(addr,0x81);
	185	/* low 8 bits of count-1 (1024-1=0x3ff) */
	186	immoutb_p(0xff,5);
	187	/* high 8 bits of count-1 */
	188	immoutb_p(3,5);
	189	/* activate DMA 2 */
	190	immoutb_p(0\|2,10);
	191	sti();
	192	}
	193
	194	static void output_byte(char byte)
	195	{
	196	int counter;
	197	unsigned char status;
	198
	199	if (reset)
	200	return;
	201	for(counter = 0 ; counter < 10000 ; counter++) {
	202	status = inb_p(FD_STATUS) & (STATUS_READY \| STATUS_DIR);
	203	if (status == STATUS_READY) {
	204	outb(byte,FD_DATA);
	205	return;
	206	}
	207	}
	208	reset = 1;
	209	printk("Unable to send byte to FDC\n\r");
	210	}
	211
	212	static int result(void)
	213	{
	214	int i = 0, counter, status;
	215
	216	if (reset)
	217	return -1;
	218	for (counter = 0 ; counter < 10000 ; counter++) {
	219	status = inb_p(FD_STATUS)&(STATUS_DIR\|STATUS_READY\|STATUS_BUSY);
	220	if (status == STATUS_READY)
	221	return i;
	222	if (status == (STATUS_DIR\|STATUS_READY\|STATUS_BUSY)) {
	223	if (i >= MAX_REPLIES)
	224	break;
	225	reply_buffer[i++] = inb_p(FD_DATA);
	226	}
	227	}
	228	reset = 1;
	229	printk("Getstatus times out\n\r");
	230	return -1;
	231	}
	232
	233	static void bad_flp_intr(void)
	234	{
	235	CURRENT->errors++;
	236	if (CURRENT->errors > MAX_ERRORS) {
	237	floppy_deselect(current_drive);
	238	end_request(0);
	239	}
	240	if (CURRENT->errors > MAX_ERRORS/2)
	241	reset = 1;
	242	else
	243	recalibrate = 1;
	244	}
	245
	246	/*
	247	* Ok, this interrupt is called after a DMA read/write has succeeded,
	248	* so we check the results, and copy any buffers.
	249	*/
	250	static void rw_interrupt(void)
	251	{
	252	if (result() != 7 \|\| (ST0 & 0xf8) \|\| (ST1 & 0xbf) \|\| (ST2 & 0x73)) {
	253	if (ST1 & 0x02) {
	254	printk("Drive %d is write protected\n\r",current_drive);
	255	floppy_deselect(current_drive);
	256	end_request(0);
	257	} else
	258	bad_flp_intr();
	259	do_fd_request();
	260	return;
	261	}
	262	if (command == FD_READ && (unsigned long)(CURRENT->buffer) >= 0x100000)
	263	copy_buffer(tmp_floppy_area,CURRENT->buffer);
	264	floppy_deselect(current_drive);
	265	end_request(1);
	266	do_fd_request();
	267	}
	268
	269	static inline void setup_rw_floppy(void)
	270	{
	271	setup_DMA();
	272	do_floppy = rw_interrupt;
	273	output_byte(command);
	274	output_byte(head<<2 \| current_drive);
	275	output_byte(track);
	276	output_byte(head);
	277	output_byte(sector);
	278	output_byte(2); /* sector size = 512 */
	279	output_byte(floppy->sect);
	280	output_byte(floppy->gap);
	281	output_byte(0xFF); /* sector size (0xff when n!=0 ?) */
	282	if (reset)
	283	do_fd_request();
	284	}
	285
	286	/*
	287	* This is the routine called after every seek (or recalibrate) interrupt
	288	* from the floppy controller. Note that the "unexpected interrupt" routine
	289	* also does a recalibrate, but doesn't come here.
	290	*/
	291	static void seek_interrupt(void)
	292	{
	293	/* sense drive status */
	294	output_byte(FD_SENSEI);
	295	if (result() != 2 \|\| (ST0 & 0xF8) != 0x20 \|\| ST1 != seek_track) {
	296	bad_flp_intr();
	297	do_fd_request();
	298	return;
	299	}
	300	current_track = ST1;
	301	setup_rw_floppy();
	302	}
	303
	304	/*
	305	* This routine is called when everything should be correctly set up
	306	* for the transfer (ie floppy motor is on and the correct floppy is
	307	* selected).
	308	*/
	309	static void transfer(void)
	310	{
	311	if (cur_spec1 != floppy->spec1) {
	312	cur_spec1 = floppy->spec1;
	313	output_byte(FD_SPECIFY);
	314	output_byte(cur_spec1); /* hut etc */
	315	output_byte(6); /* Head load time =6ms, DMA */
	316	}
	317	if (cur_rate != floppy->rate)
	318	outb_p(cur_rate = floppy->rate,FD_DCR);
	319	if (reset) {
	320	do_fd_request();
	321	return;
	322	}
	323	if (!seek) {
	324	setup_rw_floppy();
	325	return;
	326	}
	327	do_floppy = seek_interrupt;
	328	if (seek_track) {
	329	output_byte(FD_SEEK);
	330	output_byte(head<<2 \| current_drive);
	331	output_byte(seek_track);
	332	} else {
	333	output_byte(FD_RECALIBRATE);
	334	output_byte(head<<2 \| current_drive);
	335	}
	336	if (reset)
	337	do_fd_request();
	338	}
	339
	340	/*
	341	* Special case - used after a unexpected interrupt (or reset)
	342	*/
	343	static void recal_interrupt(void)
	344	{
	345	output_byte(FD_SENSEI);
	346	if (result()!=2 \|\| (ST0 & 0xE0) == 0x60)
	347	reset = 1;
	348	else
	349	recalibrate = 0;
	350	do_fd_request();
	351	}
	352
	353	void unexpected_floppy_interrupt(void)
	354	{
	355	output_byte(FD_SENSEI);
	356	if (result()!=2 \|\| (ST0 & 0xE0) == 0x60)
	357	reset = 1;
	358	else
	359	recalibrate = 1;
	360	}
	361
	362	static void recalibrate_floppy(void)
	363	{
	364	recalibrate = 0;
	365	current_track = 0;
	366	do_floppy = recal_interrupt;
	367	output_byte(FD_RECALIBRATE);
	368	output_byte(head<<2 \| current_drive);
	369	if (reset)
	370	do_fd_request();
	371	}
	372
	373	static void reset_interrupt(void)
	374	{
	375	output_byte(FD_SENSEI);
	376	(void) result();
	377	output_byte(FD_SPECIFY);
	378	output_byte(cur_spec1); /* hut etc */
	379	output_byte(6); /* Head load time =6ms, DMA */
	380	do_fd_request();
	381	}
	382
	383	/*
	384	* reset is done by pulling bit 2 of DOR low for a while.
	385	*/
	386	static void reset_floppy(void)
	387	{
	388	int i;
	389
	390	reset = 0;
	391	cur_spec1 = -1;
	392	cur_rate = -1;
	393	recalibrate = 1;
	394	printk("Reset-floppy called\n\r");
	395	cli();
	396	do_floppy = reset_interrupt;
	397	outb_p(current_DOR & ~0x04,FD_DOR);
	398	for (i=0 ; i<100 ; i++)
	399	__asm__("nop");
	400	outb(current_DOR,FD_DOR);
	401	sti();
	402	}
	403
	404	static void floppy_on_interrupt(void)
	405	{
	406	/* We cannot do a floppy-select, as that might sleep. We just force it */
	407	selected = 1;
	408	if (current_drive != (current_DOR & 3)) {
	409	current_DOR &= 0xFC;
	410	current_DOR \|= current_drive;
	411	outb(current_DOR,FD_DOR);
	412	add_timer(2,&transfer);
	413	} else
	414	transfer();
	415	}
	416
	417	void do_fd_request(void)
	418	{
	419	unsigned int block;
	420
	421	seek = 0;
	422	if (reset) {
	423	reset_floppy();
	424	return;
	425	}
	426	if (recalibrate) {
	427	recalibrate_floppy();
	428	return;
	429	}
	430	INIT_REQUEST;
	431	floppy = (MINOR(CURRENT->dev)>>2) + floppy_type;
	432	if (current_drive != CURRENT_DEV)
	433	seek = 1;
	434	current_drive = CURRENT_DEV;
	435	block = CURRENT->sector;
	436	if (block+2 > floppy->size) {
	437	end_request(0);
	438	goto repeat;
	439	}
	440	sector = block % floppy->sect;
	441	block /= floppy->sect;
	442	head = block % floppy->head;
	443	track = block / floppy->head;
	444	seek_track = track << floppy->stretch;
	445	if (seek_track != current_track)
	446	seek = 1;
	447	sector++;
	448	if (CURRENT->cmd == READ)
	449	command = FD_READ;
	450	else if (CURRENT->cmd == WRITE)
	451	command = FD_WRITE;
	452	else
	453	panic("do_fd_request: unknown command");
	454	add_timer(ticks_to_floppy_on(current_drive),&floppy_on_interrupt);
	455	}
	456
	457	void floppy_init(void)
	458	{
	459	blk_dev[MAJOR_NR].request_fn = DEVICE_REQUEST;
	460	set_trap_gate(0x26,&floppy_interrupt);
	461	outb(inb_p(0x21)&~0x40,0x21);
	462	}