Annotation of prex-old/dev/i386/pc/fdd.c, Revision 1.1
1.1 ! nbrk 1: /*-
! 2: * Copyright (c) 2005-2006, Kohsuke Ohtani
! 3: * All rights reserved.
! 4: *
! 5: * Redistribution and use in source and binary forms, with or without
! 6: * modification, are permitted provided that the following conditions
! 7: * are met:
! 8: * 1. Redistributions of source code must retain the above copyright
! 9: * notice, this list of conditions and the following disclaimer.
! 10: * 2. Redistributions in binary form must reproduce the above copyright
! 11: * notice, this list of conditions and the following disclaimer in the
! 12: * documentation and/or other materials provided with the distribution.
! 13: * 3. Neither the name of the author nor the names of any co-contributors
! 14: * may be used to endorse or promote products derived from this software
! 15: * without specific prior written permission.
! 16: *
! 17: * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
! 18: * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
! 19: * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
! 20: * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
! 21: * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
! 22: * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
! 23: * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
! 24: * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
! 25: * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
! 26: * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
! 27: * SUCH DAMAGE.
! 28: */
! 29:
! 30: /*
! 31: * fdd.c - Floppy disk driver
! 32: */
! 33:
! 34: /*
! 35: * State transition table:
! 36: *
! 37: * State Interrupt Timeout Error
! 38: * --------- --------- --------- ---------
! 39: * Off N/A On N/A
! 40: * On N/A Reset N/A
! 41: * Reset Recal Off N/A
! 42: * Recal Seek Off Off
! 43: * Seek IO Reset Off
! 44: * IO Ready Reset Off
! 45: * Ready N/A Off N/A
! 46: *
! 47: */
! 48:
! 49: #include <driver.h>
! 50: #include <delay.h>
! 51: #include <cpu.h>
! 52:
! 53: #include "dma.h"
! 54:
! 55: /* #define DEBUG_FDD */
! 56:
! 57: #ifdef DEBUG_FDD
! 58: #define fdd_printf(fmt, args...) printk("%s: " fmt, ## args)
! 59: #else
! 60: #define fdd_printf(fmt...) do {} while (0)
! 61: #endif
! 62:
! 63: #define FDD_IRQ 6 /* IRQ6 */
! 64: #define FDD_DMA 2 /* DMA2 */
! 65:
! 66: #define SECTOR_SIZE 512
! 67: #define TRACK_SIZE (SECTOR_SIZE * 18)
! 68: #define INVALID_TRACK -1
! 69:
! 70: /* I/O ports */
! 71: #define FDC_DOR 0x3f2 /* Digital output register */
! 72: #define FDC_MSR 0x3f4 /* Main status register (in) */
! 73: #define FDC_DSR 0x3f4 /* Data rate select register (out) */
! 74: #define FDC_DAT 0x3f5 /* Data register */
! 75: #define FDC_DIR 0x3f7 /* Digital input register (in) */
! 76: #define FDC_CCR 0x3f7 /* Configuration control register (out) */
! 77:
! 78: /* Command bytes */
! 79: #define CMD_SPECIFY 0x03 /* Specify drive timing */
! 80: #define CMD_DRVSTS 0x04
! 81: #define CMD_WRITE 0xc5 /* Sector write, multi-track */
! 82: #define CMD_READ 0xe6 /* Sector read */
! 83: #define CMD_RECAL 0x07 /* Recalibrate */
! 84: #define CMD_SENSE 0x08 /* Sense interrupt status */
! 85: #define CMD_FORMAT 0x4d /* Format track */
! 86: #define CMD_SEEK 0x0f /* Seek track */
! 87: #define CMD_VERSION 0x10 /* FDC version */
! 88:
! 89: /* Floppy Drive Geometries */
! 90: #define FDG_HEADS 2
! 91: #define FDG_TRACKS 80
! 92: #define FDG_SECTORS 18
! 93: #define FDG_GAP3FMT 0x54
! 94: #define FDG_GAP3RW 0x1b
! 95:
! 96: /* FDC state */
! 97: #define FDS_OFF 0 /* Motor off */
! 98: #define FDS_ON 1 /* Motor on */
! 99: #define FDS_RESET 2 /* Reset */
! 100: #define FDS_RECAL 3 /* Recalibrate */
! 101: #define FDS_SEEK 4 /* Seek */
! 102: #define FDS_IO 5 /* Read/write */
! 103: #define FDS_READY 6 /* Ready */
! 104:
! 105: static void fdc_timeout(u_long);
! 106: static void fdc_recal(void);
! 107: static void fdc_off(void);
! 108: static void fdc_io(void);
! 109:
! 110: static int fdd_init(void);
! 111: static int fdd_open(device_t, int);
! 112: static int fdd_close(device_t);
! 113: static int fdd_read(device_t, char *, size_t *, int);
! 114: static int fdd_write(device_t, char *, size_t *, int);
! 115:
! 116: /*
! 117: * I/O request
! 118: */
! 119: struct io_req {
! 120: int cmd;
! 121: int nr_retry;
! 122: int blkno;
! 123: u_long blksz;
! 124: void *buf;
! 125: int errno;
! 126: };
! 127: typedef struct io_req *ioreq_t;
! 128:
! 129: /* I/O command */
! 130: #define IO_NONE 0
! 131: #define IO_READ 1
! 132: #define IO_WRITE 2
! 133: #define IO_FORMAT 3 /* not supported */
! 134: #define IO_CANCEL 4
! 135:
! 136: /*
! 137: * Driver structure
! 138: */
! 139: struct driver fdd_drv = {
! 140: /* name */ "Floppy Disk Controller",
! 141: /* order */ 5,
! 142: /* init */ fdd_init,
! 143: };
! 144:
! 145: static struct devio fdd_io = {
! 146: /* open */ fdd_open,
! 147: /* close */ fdd_close,
! 148: /* read */ fdd_read,
! 149: /* write */ fdd_write,
! 150: /* ioctl */ NULL,
! 151: /* event */ NULL,
! 152: };
! 153:
! 154: static device_t fdd_dev; /* Device object */
! 155: static int fdd_irq; /* Interrupt handle */
! 156: static int fdd_dma; /* DMA handle */
! 157: static int nr_open; /* Open count */
! 158: static struct timer fdd_tmr; /* Timer */
! 159:
! 160: static int fdc_stat; /* Current state */
! 161: static struct io_req ioreq; /* I/O request */
! 162: static void *read_buf; /* DMA buffer for read (1 track) */
! 163: static void *write_buf; /* DMA buffer for write (1 sector) */
! 164: static u_char result[7]; /* Result from fdc */
! 165: static struct event io_event = EVENT_INIT(io_event, "fdd");
! 166: static int track_cache; /* Current track of read buffer */
! 167:
! 168: /*
! 169: * Send data to FDC
! 170: * Return -1 on failure
! 171: */
! 172: static int
! 173: fdc_out(u_char dat)
! 174: {
! 175: int i;
! 176:
! 177: for (i = 0; i < 100000; i++) {
! 178: if ((inb_p(FDC_MSR) & 0xc0) == 0x80) {
! 179: outb_p(dat, FDC_DAT);
! 180: return 0;
! 181: }
! 182: }
! 183: printk("fdc: timeout msr=%x\n", inb(FDC_MSR));
! 184: return -1;
! 185: }
! 186:
! 187: /* Return number of result bytes */
! 188: static int
! 189: fdc_result(void)
! 190: {
! 191: int i, msr, index = 0;
! 192:
! 193: for (i = 0; i < 50000; i++) { /* timeout=500msec */
! 194: msr = inb_p(FDC_MSR);
! 195: if ((msr & 0xd0) == 0x80) {
! 196: return index;
! 197: }
! 198: if ((msr & 0xd0) == 0xd0) {
! 199: if (index > 6) {
! 200: printk("fdc: overrun\n");
! 201: return -1;
! 202: }
! 203: result[index++] = inb_p(FDC_DAT);
! 204: /*
! 205: fdd_printf("result[%d]=%x\n", index - 1,
! 206: result[index - 1]);
! 207: */
! 208: }
! 209: delay_usec(10);
! 210: }
! 211: printk("fdc: timeout\n");
! 212: return -1;
! 213: }
! 214:
! 215: static void
! 216: fdc_error(int errno)
! 217: {
! 218: printk("fdc: errno=%d\n", errno);
! 219:
! 220: dma_stop(fdd_dma);
! 221: ioreq.errno = errno;
! 222: sched_wakeup(&io_event);
! 223: fdc_off();
! 224: }
! 225:
! 226: /*
! 227: * Stop motor. (No interrupt)
! 228: */
! 229: static void
! 230: fdc_off(void)
! 231: {
! 232: fdd_printf("motor off\n");
! 233:
! 234: fdc_stat = FDS_OFF;
! 235: timer_stop(&fdd_tmr);
! 236: outb_p(0x0c, FDC_DOR);
! 237: }
! 238:
! 239: /*
! 240: * Start motor and wait 250msec. (No interrupt)
! 241: */
! 242: static void
! 243: fdc_on(void)
! 244: {
! 245: fdd_printf("motor on\n");
! 246:
! 247: fdc_stat = FDS_ON;
! 248: outb_p(0x1c, FDC_DOR);
! 249: timer_callout(&fdd_tmr, fdc_timeout, 0, 250);
! 250: }
! 251:
! 252: /*
! 253: * Reset FDC and wait an intterupt.
! 254: * Timeout is 500msec.
! 255: */
! 256: static void
! 257: fdc_reset(void)
! 258: {
! 259: fdd_printf("reset\n");
! 260:
! 261: fdc_stat = FDS_RESET;
! 262: timer_callout(&fdd_tmr, fdc_timeout, 0, 500);
! 263: outb_p(0x18, FDC_DOR); /* Motor0 enable, DMA enable */
! 264: delay_usec(20); /* Wait 20 usec while reset */
! 265: outb_p(0x1c, FDC_DOR); /* Clear reset */
! 266: }
! 267:
! 268: /*
! 269: * Recalibrate FDC and wait an interrupt.
! 270: * Timeout is 5sec.
! 271: */
! 272: static void
! 273: fdc_recal(void)
! 274: {
! 275: fdd_printf("recalibrate\n");
! 276:
! 277: fdc_stat = FDS_RECAL;
! 278: timer_callout(&fdd_tmr, fdc_timeout, 0, 5000);
! 279: fdc_out(CMD_RECAL);
! 280: fdc_out(0); /* Drive 0 */
! 281: }
! 282:
! 283: /*
! 284: * Seek FDC and wait an interrupt.
! 285: * Timeout is 4sec.
! 286: */
! 287: static void
! 288: fdc_seek(void)
! 289: {
! 290: u_int head, track;
! 291:
! 292: fdd_printf("seek\n");
! 293: fdc_stat = FDS_SEEK;
! 294: head = (ioreq.blkno % (FDG_SECTORS * FDG_HEADS)) / FDG_SECTORS;
! 295: track = ioreq.blkno / (FDG_SECTORS * FDG_HEADS);
! 296:
! 297: timer_callout(&fdd_tmr, fdc_timeout, 0, 4000);
! 298:
! 299: fdc_out(CMD_SPECIFY); /* specify command parameter */
! 300: fdc_out(0xd1); /* Step rate = 3msec, Head unload time = 16msec */
! 301: fdc_out(0x02); /* Head load time = 2msec, Dma on (0) */
! 302:
! 303: fdc_out(CMD_SEEK);
! 304: fdc_out(head << 2);
! 305: fdc_out(track);
! 306: }
! 307:
! 308: /*
! 309: * Read/write data and wait an interrupt.
! 310: * Timeout is 2sec.
! 311: */
! 312: static void
! 313: fdc_io(void)
! 314: {
! 315: u_int head, track, sect;
! 316: u_long io_size;
! 317: int read;
! 318:
! 319: fdd_printf("read/write\n");
! 320: fdc_stat = FDS_IO;
! 321:
! 322: head = (ioreq.blkno % (FDG_SECTORS * FDG_HEADS)) / FDG_SECTORS;
! 323: track = ioreq.blkno / (FDG_SECTORS * FDG_HEADS);
! 324: sect = ioreq.blkno % FDG_SECTORS + 1;
! 325: io_size = ioreq.blksz * SECTOR_SIZE;
! 326: read = (ioreq.cmd == IO_READ) ? 1 : 0;
! 327:
! 328: fdd_printf("hd=%x trk=%x sec=%x size=%d read=%d\n",
! 329: head, track, sect, io_size, read);
! 330:
! 331: timer_callout(&fdd_tmr, fdc_timeout, 0, 2000);
! 332:
! 333: dma_setup(fdd_dma, (u_long) ioreq.buf, io_size, read);
! 334:
! 335: /* Send command */
! 336: fdc_out(read ? CMD_READ : CMD_WRITE);
! 337: fdc_out(head << 2);
! 338: fdc_out(track);
! 339: fdc_out(head);
! 340: fdc_out(sect);
! 341: fdc_out(2); /* sector size = 512 bytes */
! 342: fdc_out(FDG_SECTORS);
! 343: fdc_out(FDG_GAP3RW);
! 344: fdc_out(0xff);
! 345: }
! 346:
! 347: /*
! 348: * Wake up iorequester.
! 349: * FDC motor is set to off after 5sec.
! 350: */
! 351: static void
! 352: fdc_ready(void)
! 353: {
! 354: fdd_printf("wakeup requester\n");
! 355:
! 356: fdc_stat = FDS_READY;
! 357: sched_wakeup(&io_event);
! 358: timer_callout(&fdd_tmr, fdc_timeout, 0, 5000);
! 359: }
! 360:
! 361: /*
! 362: * Timeout handler
! 363: */
! 364: static void
! 365: fdc_timeout(u_long unused)
! 366: {
! 367: fdd_printf("fdc_stat=%d\n", fdc_stat);
! 368:
! 369: switch (fdc_stat) {
! 370: case FDS_ON:
! 371: fdc_reset();
! 372: break;
! 373: case FDS_RESET:
! 374: case FDS_RECAL:
! 375: printk("fdc: reset/recal timeout\n");
! 376: fdc_error(EIO);
! 377: break;
! 378: case FDS_SEEK:
! 379: case FDS_IO:
! 380: printk("fdc: seek/io timeout retry=%d\n", ioreq.nr_retry);
! 381: if (++ioreq.nr_retry <= 3)
! 382: fdc_reset();
! 383: else
! 384: fdc_error(EIO);
! 385: break;
! 386: case FDS_READY:
! 387: fdc_off();
! 388: break;
! 389: default:
! 390: panic("fdc: unknown timeout\n");
! 391: }
! 392: }
! 393:
! 394: /*
! 395: * Interrupt service routine
! 396: * Do not change the fdc_stat in isr.
! 397: */
! 398: static int
! 399: fdc_isr(int irq)
! 400: {
! 401: fdd_printf("fdc_stat=%d\n", fdc_stat);
! 402:
! 403: timer_stop(&fdd_tmr);
! 404: switch (fdc_stat) {
! 405: case FDS_IO:
! 406: dma_stop(fdd_dma);
! 407: /* Fall through */
! 408: case FDS_RESET:
! 409: case FDS_RECAL:
! 410: case FDS_SEEK:
! 411: if (ioreq.cmd == IO_NONE) {
! 412: printk("fdc: invalid interrupt\n");
! 413: timer_stop(&fdd_tmr);
! 414: break;
! 415: }
! 416: return INT_CONTINUE;
! 417: case FDS_OFF:
! 418: break;
! 419: default:
! 420: printk("fdc: unknown interrupt\n");
! 421: break;
! 422: }
! 423: return 0;
! 424: }
! 425:
! 426: /*
! 427: * Interrupt service thread
! 428: * This is called when command completion.
! 429: */
! 430: static void
! 431: fdc_ist(int irq)
! 432: {
! 433: int i;
! 434:
! 435: fdd_printf("fdc_stat=%d\n", fdc_stat);
! 436: if (ioreq.cmd == IO_NONE)
! 437: return;
! 438:
! 439: switch (fdc_stat) {
! 440: case FDS_RESET:
! 441: /* clear output buffer */
! 442: for (i = 0; i < 4; i++) {
! 443: fdc_out(CMD_SENSE);
! 444: fdc_result();
! 445: }
! 446: fdc_recal();
! 447: break;
! 448: case FDS_RECAL:
! 449: fdc_out(CMD_SENSE);
! 450: fdc_result();
! 451: if ((result[0] & 0xf8) != 0x20) {
! 452: printk("fdc: recal error\n");
! 453: fdc_error(EIO);
! 454: break;
! 455: }
! 456: fdc_seek();
! 457: break;
! 458: case FDS_SEEK:
! 459: fdc_out(CMD_SENSE);
! 460: fdc_result();
! 461: if ((result[0] & 0xf8) != 0x20) {
! 462: printk("fdc: seek error\n");
! 463: if (++ioreq.nr_retry <= 3)
! 464: fdc_reset();
! 465: else
! 466: fdc_error(EIO);
! 467: break;
! 468: }
! 469: fdc_io();
! 470: break;
! 471: case FDS_IO:
! 472: fdc_result();
! 473: if ((result[0] & 0xd8) != 0x00) {
! 474: printk("fdc: i/o error st0=%x st1=%x st2=%x st3=%x retry=%d\n",
! 475: result[0], result[1], result[2], result[3],
! 476: ioreq.nr_retry);
! 477: if (++ioreq.nr_retry <= 3)
! 478: fdc_reset();
! 479: else
! 480: fdc_error(EIO);
! 481: break;
! 482: }
! 483: fdd_printf("i/o complete\n");
! 484: fdc_ready();
! 485: break;
! 486: case FDS_OFF:
! 487: /* Ignore */
! 488: break;
! 489: default:
! 490: ASSERT(0);
! 491: }
! 492: return;
! 493: }
! 494:
! 495: /*
! 496: * Open
! 497: */
! 498: static int
! 499: fdd_open(device_t dev, int mode)
! 500: {
! 501:
! 502: nr_open++;
! 503: fdd_printf("nr_open=%d\n", nr_open);
! 504: return 0;
! 505: }
! 506:
! 507: /*
! 508: * Close
! 509: */
! 510: static int
! 511: fdd_close(device_t dev)
! 512: {
! 513: fdd_printf("dev=%x\n", dev);
! 514:
! 515: if (nr_open < 1)
! 516: return EINVAL;
! 517: nr_open--;
! 518: if (nr_open == 0) {
! 519: ioreq.cmd = IO_NONE;
! 520: fdc_off();
! 521: }
! 522: return 0;
! 523: }
! 524:
! 525: /*
! 526: * Common routine for read/write
! 527: */
! 528: static int
! 529: fdd_rw(int cmd, char *buf, u_long blksz, int blkno)
! 530: {
! 531: int err;
! 532:
! 533: fdd_printf("cmd=%x buf=%x blksz=%d blkno=%x\n", cmd, buf, blksz, blkno);
! 534: ioreq.cmd = cmd;
! 535: ioreq.nr_retry = 0;
! 536: ioreq.blkno = blkno;
! 537: ioreq.blksz = blksz;
! 538: ioreq.buf = buf;
! 539: ioreq.errno = 0;
! 540:
! 541: sched_lock();
! 542: if (fdc_stat == FDS_OFF)
! 543: fdc_on();
! 544: else
! 545: fdc_seek();
! 546:
! 547: if (sched_sleep(&io_event) == SLP_INTR)
! 548: err = EINTR;
! 549: else
! 550: err = ioreq.errno;
! 551:
! 552: sched_unlock();
! 553: return err;
! 554: }
! 555:
! 556: /*
! 557: * Read
! 558: *
! 559: * Error:
! 560: * EINTR ... Interrupted by signal
! 561: * EIO ... Low level I/O error
! 562: * ENXIO ... Write protected
! 563: * EFAULT ... No physical memory is mapped to buffer
! 564: */
! 565: static int
! 566: fdd_read(device_t dev, char *buf, size_t *nbyte, int blkno)
! 567: {
! 568: char *kbuf;
! 569: int i, track, sect, nr_sect, err;
! 570:
! 571: fdd_printf("read buf=%x nbyte=%d blkno=%x\n", buf, *nbyte, blkno);
! 572:
! 573: /* Check overrun */
! 574: if (blkno > FDG_HEADS * FDG_TRACKS * FDG_SECTORS)
! 575: return EIO;
! 576:
! 577: /* Translate buffer address to kernel address */
! 578: kbuf = kmem_map(buf, *nbyte);
! 579: if (kbuf == NULL)
! 580: return EFAULT;
! 581:
! 582: nr_sect = *nbyte / SECTOR_SIZE;
! 583: err = 0;
! 584: for (i = 0; i < nr_sect; i++) {
! 585: /* Translate the logical sector# to logical track#/sector#. */
! 586: track = blkno / FDG_SECTORS;
! 587: sect = blkno % FDG_SECTORS;
! 588: /*
! 589: * If target sector does not exist in buffer,
! 590: * read 1 track (18 sectors) at once.
! 591: */
! 592: if (track != track_cache) {
! 593: err = fdd_rw(IO_READ, read_buf, FDG_SECTORS,
! 594: track * FDG_SECTORS);
! 595: if (err) {
! 596: track_cache = INVALID_TRACK;
! 597: break;
! 598: }
! 599: track_cache = track;
! 600: }
! 601: memcpy(kbuf, (char *)read_buf + sect * SECTOR_SIZE,
! 602: SECTOR_SIZE);
! 603: blkno++;
! 604: kbuf += SECTOR_SIZE;
! 605: }
! 606: *nbyte = i * SECTOR_SIZE;
! 607: return err;
! 608: }
! 609:
! 610: /*
! 611: * Write
! 612: *
! 613: * Error:
! 614: * EINTR ... Interrupted by signal
! 615: * EIO ... Low level I/O error
! 616: * ENXIO ... Write protected
! 617: * EFAULT ... No physical memory is mapped to buffer
! 618: */
! 619: static int
! 620: fdd_write(device_t dev, char *buf, size_t *nbyte, int blkno)
! 621: {
! 622: char *kbuf, *wbuf;
! 623: int i, track, sect, nr_sect, err;
! 624:
! 625: fdd_printf("write buf=%x nbyte=%d blkno=%x\n", buf, *nbyte, blkno);
! 626:
! 627: /* Check overrun */
! 628: if (blkno > FDG_HEADS * FDG_TRACKS * FDG_SECTORS)
! 629: return EIO;
! 630:
! 631: /* Translate buffer address to kernel address */
! 632: kbuf = kmem_map(buf, *nbyte);
! 633: if (kbuf == NULL)
! 634: return EFAULT;
! 635:
! 636: nr_sect = *nbyte / SECTOR_SIZE;
! 637: err = 0;
! 638: for (i = 0; i < nr_sect; i++) {
! 639: /* Translate the logical sector# to track#/sector#. */
! 640: track = blkno / FDG_SECTORS;
! 641: sect = blkno % FDG_SECTORS;
! 642: /*
! 643: * If target sector exists in read buffer, use it as
! 644: * write buffer to keep the cache cohrency.
! 645: */
! 646: if (track == track_cache)
! 647: wbuf = (char *)read_buf + sect * SECTOR_SIZE;
! 648: else
! 649: wbuf = write_buf;
! 650:
! 651: memcpy(wbuf, kbuf, SECTOR_SIZE);
! 652: err = fdd_rw(IO_WRITE, wbuf, 1, blkno);
! 653: if (err) {
! 654: track_cache = INVALID_TRACK;
! 655: break;
! 656: }
! 657: blkno++;
! 658: kbuf += SECTOR_SIZE;
! 659: }
! 660: *nbyte = i * SECTOR_SIZE;
! 661:
! 662: fdd_printf("fdd_write err=%d\n", err);
! 663: return err;
! 664: }
! 665:
! 666: /*
! 667: * Initialize
! 668: */
! 669: static int
! 670: fdd_init(void)
! 671: {
! 672: char *buf;
! 673: int i;
! 674:
! 675: fdd_printf("fdd_init\n");
! 676: if (inb(FDC_MSR) == 0xff) {
! 677: printk("Floppy drive not found!\n");
! 678: return -1;
! 679: }
! 680:
! 681: /* Create device object */
! 682: fdd_dev = device_create(&fdd_io, "fd0", DF_BLK);
! 683: ASSERT(fdd_dev);
! 684:
! 685: /*
! 686: * Allocate physical pages for DMA buffer.
! 687: * Buffer: 1 track for read, 1 sector for write.
! 688: */
! 689: buf = dma_alloc(TRACK_SIZE + SECTOR_SIZE);
! 690:
! 691: ASSERT(buf);
! 692: read_buf = buf;
! 693: write_buf = buf + TRACK_SIZE;
! 694:
! 695: /* Allocate DMA */
! 696: fdd_dma = dma_attach(FDD_DMA);
! 697: ASSERT(fdd_dma != -1);
! 698:
! 699: /* Allocate IRQ */
! 700: fdd_irq = irq_attach(FDD_IRQ, IPL_BLOCK, 0, fdc_isr, fdc_ist);
! 701: ASSERT(fdd_irq != -1);
! 702:
! 703: timer_init(&fdd_tmr);
! 704: fdc_stat = FDS_OFF;
! 705: ioreq.cmd = IO_NONE;
! 706: track_cache = INVALID_TRACK;
! 707:
! 708: /* Reset FDC */
! 709: outb_p(0x08, FDC_DOR);
! 710: delay_usec(20);
! 711: outb_p(0x0C, FDC_DOR);
! 712:
! 713: /* Data rate 500kbps */
! 714: outb_p(0x00, FDC_CCR);
! 715:
! 716: /* clear output buffer */
! 717: for (i = 0; i < 4; i++) {
! 718: fdc_out(CMD_SENSE);
! 719: fdc_result();
! 720: }
! 721: return 0;
! 722: }
CVSweb