[BACK]Return to fdd.c CVS log [TXT][DIR] Up to [local] / prex / dev / i386 / pc

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