Annotation of prex/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 <cpufunc.h>
! 52:
! 53: #include "dma.h"
! 54:
! 55: /* #define DEBUG_FDD 1 */
! 56:
! 57: #ifdef DEBUG_FDD
! 58: #define DPRINTF(a) printf a
! 59: #else
! 60: #define DPRINTF(a)
! 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(void *);
! 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: /*
! 146: * Device I/O table
! 147: */
! 148: static struct devio fdd_io = {
! 149: /* open */ fdd_open,
! 150: /* close */ fdd_close,
! 151: /* read */ fdd_read,
! 152: /* write */ fdd_write,
! 153: /* ioctl */ NULL,
! 154: /* event */ NULL,
! 155: };
! 156:
! 157: static device_t fdd_dev; /* Device object */
! 158: static irq_t fdd_irq; /* Interrupt handle */
! 159: static int fdd_dma; /* DMA handle */
! 160: static int nr_open; /* Open count */
! 161: static struct timer fdd_tmr; /* Timer */
! 162:
! 163: static int fdc_stat; /* Current state */
! 164: static struct io_req ioreq; /* I/O request */
! 165: static void *read_buf; /* DMA buffer for read (1 track) */
! 166: static void *write_buf; /* DMA buffer for write (1 sector) */
! 167: static u_char result[7]; /* Result from fdc */
! 168: static struct event fdd_event;
! 169: static int track_cache; /* Current track of read buffer */
! 170:
! 171: /*
! 172: * Send data to FDC
! 173: * Return -1 on failure
! 174: */
! 175: static int
! 176: fdc_out(int dat)
! 177: {
! 178: int i;
! 179:
! 180: for (i = 0; i < 100000; i++) {
! 181: if ((inb_p(FDC_MSR) & 0xc0) == 0x80) {
! 182: outb_p(dat, FDC_DAT);
! 183: return 0;
! 184: }
! 185: }
! 186: DPRINTF(("fdc: timeout msr=%x\n", inb(FDC_MSR)));
! 187: return -1;
! 188: }
! 189:
! 190: /* Return number of result bytes */
! 191: static int
! 192: fdc_result(void)
! 193: {
! 194: int i, msr, index = 0;
! 195:
! 196: for (i = 0; i < 50000; i++) { /* timeout=500msec */
! 197: msr = inb_p(FDC_MSR);
! 198: if ((msr & 0xd0) == 0x80) {
! 199: return index;
! 200: }
! 201: if ((msr & 0xd0) == 0xd0) {
! 202: if (index > 6) {
! 203: DPRINTF(("fdc: overrun\n"));
! 204: return -1;
! 205: }
! 206: result[index++] = inb_p(FDC_DAT);
! 207: /*
! 208: DPRINTF(("result[%d]=%x\n", index - 1,
! 209: result[index - 1]));
! 210: */
! 211: }
! 212: delay_usec(10);
! 213: }
! 214: DPRINTF(("fdc: timeout\n"));
! 215: return -1;
! 216: }
! 217:
! 218: static void
! 219: fdc_error(int errno)
! 220: {
! 221: DPRINTF(("fdc: errno=%d\n", errno));
! 222:
! 223: dma_stop(fdd_dma);
! 224: ioreq.errno = errno;
! 225: sched_wakeup(&fdd_event);
! 226: fdc_off();
! 227: }
! 228:
! 229: /*
! 230: * Stop motor. (No interrupt)
! 231: */
! 232: static void
! 233: fdc_off(void)
! 234: {
! 235: DPRINTF(("fdc: motor off\n"));
! 236:
! 237: fdc_stat = FDS_OFF;
! 238: timer_stop(&fdd_tmr);
! 239: outb_p(0x0c, FDC_DOR);
! 240: }
! 241:
! 242: /*
! 243: * Start motor and wait 250msec. (No interrupt)
! 244: */
! 245: static void
! 246: fdc_on(void)
! 247: {
! 248: DPRINTF(("fdc: motor on\n"));
! 249:
! 250: fdc_stat = FDS_ON;
! 251: outb_p(0x1c, FDC_DOR);
! 252: timer_callout(&fdd_tmr, 250, &fdc_timeout, NULL);
! 253: }
! 254:
! 255: /*
! 256: * Reset FDC and wait an intterupt.
! 257: * Timeout is 500msec.
! 258: */
! 259: static void
! 260: fdc_reset(void)
! 261: {
! 262: DPRINTF(("fdc: reset\n"));
! 263:
! 264: fdc_stat = FDS_RESET;
! 265: timer_callout(&fdd_tmr, 500, &fdc_timeout, NULL);
! 266: outb_p(0x18, FDC_DOR); /* Motor0 enable, DMA enable */
! 267: delay_usec(20); /* Wait 20 usec while reset */
! 268: outb_p(0x1c, FDC_DOR); /* Clear reset */
! 269: }
! 270:
! 271: /*
! 272: * Recalibrate FDC and wait an interrupt.
! 273: * Timeout is 5sec.
! 274: */
! 275: static void
! 276: fdc_recal(void)
! 277: {
! 278: DPRINTF(("fdc: recalibrate\n"));
! 279:
! 280: fdc_stat = FDS_RECAL;
! 281: timer_callout(&fdd_tmr, 5000, &fdc_timeout, NULL);
! 282: fdc_out(CMD_RECAL);
! 283: fdc_out(0); /* Drive 0 */
! 284: }
! 285:
! 286: /*
! 287: * Seek FDC and wait an interrupt.
! 288: * Timeout is 4sec.
! 289: */
! 290: static void
! 291: fdc_seek(void)
! 292: {
! 293: int head, track;
! 294:
! 295: DPRINTF(("fdc: seek\n"));
! 296: fdc_stat = FDS_SEEK;
! 297: head = (ioreq.blkno % (FDG_SECTORS * FDG_HEADS)) / FDG_SECTORS;
! 298: track = ioreq.blkno / (FDG_SECTORS * FDG_HEADS);
! 299:
! 300: timer_callout(&fdd_tmr, 4000, &fdc_timeout, NULL);
! 301:
! 302: fdc_out(CMD_SPECIFY); /* specify command parameter */
! 303: fdc_out(0xd1); /* Step rate = 3msec, Head unload time = 16msec */
! 304: fdc_out(0x02); /* Head load time = 2msec, Dma on (0) */
! 305:
! 306: fdc_out(CMD_SEEK);
! 307: fdc_out(head << 2);
! 308: fdc_out(track);
! 309: }
! 310:
! 311: /*
! 312: * Read/write data and wait an interrupt.
! 313: * Timeout is 2sec.
! 314: */
! 315: static void
! 316: fdc_io(void)
! 317: {
! 318: int head, track, sect;
! 319: u_long io_size;
! 320: int read;
! 321:
! 322: DPRINTF(("fdc: read/write\n"));
! 323: fdc_stat = FDS_IO;
! 324:
! 325: head = (ioreq.blkno % (FDG_SECTORS * FDG_HEADS)) / FDG_SECTORS;
! 326: track = ioreq.blkno / (FDG_SECTORS * FDG_HEADS);
! 327: sect = ioreq.blkno % FDG_SECTORS + 1;
! 328: io_size = ioreq.blksz * SECTOR_SIZE;
! 329: read = (ioreq.cmd == IO_READ) ? 1 : 0;
! 330:
! 331: DPRINTF(("fdc: hd=%x trk=%x sec=%x size=%d read=%d\n",
! 332: head, track, sect, io_size, read));
! 333:
! 334: timer_callout(&fdd_tmr, 2000, &fdc_timeout, NULL);
! 335:
! 336: dma_setup(fdd_dma, ioreq.buf, io_size, read);
! 337:
! 338: /* Send command */
! 339: fdc_out(read ? CMD_READ : CMD_WRITE);
! 340: fdc_out(head << 2);
! 341: fdc_out(track);
! 342: fdc_out(head);
! 343: fdc_out(sect);
! 344: fdc_out(2); /* sector size = 512 bytes */
! 345: fdc_out(FDG_SECTORS);
! 346: fdc_out(FDG_GAP3RW);
! 347: fdc_out(0xff);
! 348: }
! 349:
! 350: /*
! 351: * Wake up iorequester.
! 352: * FDC motor is set to off after 5sec.
! 353: */
! 354: static void
! 355: fdc_ready(void)
! 356: {
! 357: DPRINTF(("fdc: wakeup requester\n"));
! 358:
! 359: fdc_stat = FDS_READY;
! 360: sched_wakeup(&fdd_event);
! 361: timer_callout(&fdd_tmr, 5000, &fdc_timeout, NULL);
! 362: }
! 363:
! 364: /*
! 365: * Timeout handler
! 366: */
! 367: static void
! 368: fdc_timeout(void *arg)
! 369: {
! 370: DPRINTF(("fdc: fdc_stat=%d\n", fdc_stat));
! 371:
! 372: switch (fdc_stat) {
! 373: case FDS_ON:
! 374: fdc_reset();
! 375: break;
! 376: case FDS_RESET:
! 377: case FDS_RECAL:
! 378: DPRINTF(("fdc: reset/recal timeout\n"));
! 379: fdc_error(EIO);
! 380: break;
! 381: case FDS_SEEK:
! 382: case FDS_IO:
! 383: DPRINTF(("fdc: seek/io timeout retry=%d\n", ioreq.nr_retry));
! 384: if (++ioreq.nr_retry <= 3)
! 385: fdc_reset();
! 386: else
! 387: fdc_error(EIO);
! 388: break;
! 389: case FDS_READY:
! 390: fdc_off();
! 391: break;
! 392: default:
! 393: panic("fdc: unknown timeout");
! 394: }
! 395: }
! 396:
! 397: /*
! 398: * Interrupt service routine
! 399: * Do not change the fdc_stat in isr.
! 400: */
! 401: static int
! 402: fdc_isr(int irq)
! 403: {
! 404: DPRINTF(("fdc_stat=%d\n", fdc_stat));
! 405:
! 406: timer_stop(&fdd_tmr);
! 407: switch (fdc_stat) {
! 408: case FDS_IO:
! 409: dma_stop(fdd_dma);
! 410: /* Fall through */
! 411: case FDS_RESET:
! 412: case FDS_RECAL:
! 413: case FDS_SEEK:
! 414: if (ioreq.cmd == IO_NONE) {
! 415: DPRINTF(("fdc: invalid interrupt\n"));
! 416: timer_stop(&fdd_tmr);
! 417: break;
! 418: }
! 419: return INT_CONTINUE;
! 420: case FDS_OFF:
! 421: break;
! 422: default:
! 423: DPRINTF(("fdc: unknown interrupt\n"));
! 424: break;
! 425: }
! 426: return 0;
! 427: }
! 428:
! 429: /*
! 430: * Interrupt service thread
! 431: * This is called when command completion.
! 432: */
! 433: static void
! 434: fdc_ist(int irq)
! 435: {
! 436: int i;
! 437:
! 438: DPRINTF(("fdc_stat=%d\n", fdc_stat));
! 439: if (ioreq.cmd == IO_NONE)
! 440: return;
! 441:
! 442: switch (fdc_stat) {
! 443: case FDS_RESET:
! 444: /* clear output buffer */
! 445: for (i = 0; i < 4; i++) {
! 446: fdc_out(CMD_SENSE);
! 447: fdc_result();
! 448: }
! 449: fdc_recal();
! 450: break;
! 451: case FDS_RECAL:
! 452: fdc_out(CMD_SENSE);
! 453: fdc_result();
! 454: if ((result[0] & 0xf8) != 0x20) {
! 455: DPRINTF(("fdc: recal error\n"));
! 456: fdc_error(EIO);
! 457: break;
! 458: }
! 459: fdc_seek();
! 460: break;
! 461: case FDS_SEEK:
! 462: fdc_out(CMD_SENSE);
! 463: fdc_result();
! 464: if ((result[0] & 0xf8) != 0x20) {
! 465: DPRINTF(("fdc: seek error\n"));
! 466: if (++ioreq.nr_retry <= 3)
! 467: fdc_reset();
! 468: else
! 469: fdc_error(EIO);
! 470: break;
! 471: }
! 472: fdc_io();
! 473: break;
! 474: case FDS_IO:
! 475: fdc_result();
! 476: if ((result[0] & 0xd8) != 0x00) {
! 477: DPRINTF(("fdc: i/o error st0=%x st1=%x st2=%x st3=%x "
! 478: "retry=%d\n",
! 479: result[0], result[1], result[2], result[3],
! 480: ioreq.nr_retry));
! 481: if (++ioreq.nr_retry <= 3)
! 482: fdc_reset();
! 483: else
! 484: fdc_error(EIO);
! 485: break;
! 486: }
! 487: DPRINTF(("fdc: i/o complete\n"));
! 488: fdc_ready();
! 489: break;
! 490: case FDS_OFF:
! 491: /* Ignore */
! 492: break;
! 493: default:
! 494: ASSERT(0);
! 495: }
! 496: return;
! 497: }
! 498:
! 499: /*
! 500: * Open
! 501: */
! 502: static int
! 503: fdd_open(device_t dev, int mode)
! 504: {
! 505:
! 506: nr_open++;
! 507: DPRINTF(("fdd_open: nr_open=%d\n", nr_open));
! 508: return 0;
! 509: }
! 510:
! 511: /*
! 512: * Close
! 513: */
! 514: static int
! 515: fdd_close(device_t dev)
! 516: {
! 517: DPRINTF(("fdd_close: dev=%x\n", dev));
! 518:
! 519: if (nr_open < 1)
! 520: return EINVAL;
! 521: nr_open--;
! 522: if (nr_open == 0) {
! 523: ioreq.cmd = IO_NONE;
! 524: fdc_off();
! 525: }
! 526: return 0;
! 527: }
! 528:
! 529: /*
! 530: * Common routine for read/write
! 531: */
! 532: static int
! 533: fdd_rw(int cmd, char *buf, u_long blksz, int blkno)
! 534: {
! 535: int err;
! 536:
! 537: DPRINTF(("fdd_rw: cmd=%x buf=%x blksz=%d blkno=%x\n",
! 538: cmd, buf, blksz, blkno));
! 539: ioreq.cmd = cmd;
! 540: ioreq.nr_retry = 0;
! 541: ioreq.blkno = blkno;
! 542: ioreq.blksz = blksz;
! 543: ioreq.buf = buf;
! 544: ioreq.errno = 0;
! 545:
! 546: sched_lock();
! 547: if (fdc_stat == FDS_OFF)
! 548: fdc_on();
! 549: else
! 550: fdc_seek();
! 551:
! 552: if (sched_sleep(&fdd_event) == SLP_INTR)
! 553: err = EINTR;
! 554: else
! 555: err = ioreq.errno;
! 556:
! 557: sched_unlock();
! 558: return err;
! 559: }
! 560:
! 561: /*
! 562: * Read
! 563: *
! 564: * Error:
! 565: * EINTR ... Interrupted by signal
! 566: * EIO ... Low level I/O error
! 567: * ENXIO ... Write protected
! 568: * EFAULT ... No physical memory is mapped to buffer
! 569: */
! 570: static int
! 571: fdd_read(device_t dev, char *buf, size_t *nbyte, int blkno)
! 572: {
! 573: char *kbuf;
! 574: int track, sect, err;
! 575: u_int i, nr_sect;
! 576:
! 577: DPRINTF(("fdd_read: buf=%x nbyte=%d blkno=%x\n", buf, *nbyte, blkno));
! 578:
! 579: /* Check overrun */
! 580: if (blkno > FDG_HEADS * FDG_TRACKS * FDG_SECTORS)
! 581: return EIO;
! 582:
! 583: /* Translate buffer address to kernel address */
! 584: kbuf = kmem_map(buf, *nbyte);
! 585: if (kbuf == NULL)
! 586: return EFAULT;
! 587:
! 588: nr_sect = *nbyte / SECTOR_SIZE;
! 589: err = 0;
! 590: for (i = 0; i < nr_sect; i++) {
! 591: /* Translate the logical sector# to logical track#/sector#. */
! 592: track = blkno / FDG_SECTORS;
! 593: sect = blkno % FDG_SECTORS;
! 594: /*
! 595: * If target sector does not exist in buffer,
! 596: * read 1 track (18 sectors) at once.
! 597: */
! 598: if (track != track_cache) {
! 599: err = fdd_rw(IO_READ, read_buf, FDG_SECTORS,
! 600: track * FDG_SECTORS);
! 601: if (err) {
! 602: track_cache = INVALID_TRACK;
! 603: break;
! 604: }
! 605: track_cache = track;
! 606: }
! 607: memcpy(kbuf, (char *)read_buf + sect * SECTOR_SIZE,
! 608: SECTOR_SIZE);
! 609: blkno++;
! 610: kbuf += SECTOR_SIZE;
! 611: }
! 612: *nbyte = i * SECTOR_SIZE;
! 613: return err;
! 614: }
! 615:
! 616: /*
! 617: * Write
! 618: *
! 619: * Error:
! 620: * EINTR ... Interrupted by signal
! 621: * EIO ... Low level I/O error
! 622: * ENXIO ... Write protected
! 623: * EFAULT ... No physical memory is mapped to buffer
! 624: */
! 625: static int
! 626: fdd_write(device_t dev, char *buf, size_t *nbyte, int blkno)
! 627: {
! 628: char *kbuf, *wbuf;
! 629: int track, sect, err;
! 630: u_int i, nr_sect;
! 631:
! 632: DPRINTF(("fdd_write: buf=%x nbyte=%d blkno=%x\n", buf, *nbyte, blkno));
! 633:
! 634: /* Check overrun */
! 635: if (blkno > FDG_HEADS * FDG_TRACKS * FDG_SECTORS)
! 636: return EIO;
! 637:
! 638: /* Translate buffer address to kernel address */
! 639: kbuf = kmem_map(buf, *nbyte);
! 640: if (kbuf == NULL)
! 641: return EFAULT;
! 642:
! 643: nr_sect = *nbyte / SECTOR_SIZE;
! 644: err = 0;
! 645: for (i = 0; i < nr_sect; i++) {
! 646: /* Translate the logical sector# to track#/sector#. */
! 647: track = blkno / FDG_SECTORS;
! 648: sect = blkno % FDG_SECTORS;
! 649: /*
! 650: * If target sector exists in read buffer, use it as
! 651: * write buffer to keep the cache cohrency.
! 652: */
! 653: if (track == track_cache)
! 654: wbuf = (char *)read_buf + sect * SECTOR_SIZE;
! 655: else
! 656: wbuf = write_buf;
! 657:
! 658: memcpy(wbuf, kbuf, SECTOR_SIZE);
! 659: err = fdd_rw(IO_WRITE, wbuf, 1, blkno);
! 660: if (err) {
! 661: track_cache = INVALID_TRACK;
! 662: break;
! 663: }
! 664: blkno++;
! 665: kbuf += SECTOR_SIZE;
! 666: }
! 667: *nbyte = i * SECTOR_SIZE;
! 668:
! 669: DPRINTF(("fdd_write: err=%d\n", err));
! 670: return err;
! 671: }
! 672:
! 673: /*
! 674: * Initialize
! 675: */
! 676: static int
! 677: fdd_init(void)
! 678: {
! 679: char *buf;
! 680: int i;
! 681:
! 682: DPRINTF(("fdd_init\n"));
! 683: if (inb(FDC_MSR) == 0xff) {
! 684: printf("floppy drive not found!\n");
! 685: return -1;
! 686: }
! 687:
! 688: /* Create device object */
! 689: fdd_dev = device_create(&fdd_io, "fd0", DF_BLK);
! 690: ASSERT(fdd_dev);
! 691:
! 692: event_init(&fdd_event, "fdd i/o");
! 693:
! 694: /*
! 695: * Allocate physical pages for DMA buffer.
! 696: * Buffer: 1 track for read, 1 sector for write.
! 697: */
! 698: buf = dma_alloc(TRACK_SIZE + SECTOR_SIZE);
! 699: ASSERT(buf);
! 700: read_buf = buf;
! 701: write_buf = buf + TRACK_SIZE;
! 702:
! 703: /* Allocate DMA */
! 704: fdd_dma = dma_attach(FDD_DMA);
! 705: ASSERT(fdd_dma != -1);
! 706:
! 707: /* Allocate IRQ */
! 708: fdd_irq = irq_attach(FDD_IRQ, IPL_BLOCK, 0, fdc_isr, fdc_ist);
! 709: ASSERT(fdd_irq != IRQ_NULL);
! 710:
! 711: timer_init(&fdd_tmr);
! 712: fdc_stat = FDS_OFF;
! 713: ioreq.cmd = IO_NONE;
! 714: track_cache = INVALID_TRACK;
! 715:
! 716: /* Reset FDC */
! 717: outb_p(0x08, FDC_DOR);
! 718: delay_usec(20);
! 719: outb_p(0x0C, FDC_DOR);
! 720:
! 721: /* Data rate 500kbps */
! 722: outb_p(0x00, FDC_CCR);
! 723:
! 724: /* clear output buffer */
! 725: for (i = 0; i < 4; i++) {
! 726: fdc_out(CMD_SENSE);
! 727: fdc_result();
! 728: }
! 729: return 0;
! 730: }
CVSweb