fdc.c.svn-base

我们自己开发的一个OSEK操作系统！不知道可不可以？

            DMA_write_memory (nchan, fdctrl->fifo + rel_pos,
                              fdctrl->data_pos, len);
            break;
        case FD_DIR_WRITE:
            /* WRITE commands */
            DMA_read_memory (nchan, fdctrl->fifo + rel_pos,
                             fdctrl->data_pos, len);
            if (bdrv_write(cur_drv->bs, fd_sector(cur_drv),
                           fdctrl->fifo, 1) < 0) {
                FLOPPY_ERROR("writting sector %d\n", fd_sector(cur_drv));
                fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
                goto transfer_error;
            }
            break;
        default:
            /* SCAN commands */
            {
                uint8_t tmpbuf[FD_SECTOR_LEN];
                int ret;
                DMA_read_memory (nchan, tmpbuf, fdctrl->data_pos, len);
                ret = memcmp(tmpbuf, fdctrl->fifo + rel_pos, len);
                if (ret == 0) {
                    status2 = 0x08;
                    goto end_transfer;
                }
                if ((ret < 0 && fdctrl->data_dir == FD_DIR_SCANL) ||
                    (ret > 0 && fdctrl->data_dir == FD_DIR_SCANH)) {
                    status2 = 0x00;
                    goto end_transfer;
                }
            }
            break;
        }
        fdctrl->data_pos += len;
        rel_pos = fdctrl->data_pos % FD_SECTOR_LEN;
        if (rel_pos == 0) {
            /* Seek to next sector */
            FLOPPY_DPRINTF("seek to next sector (%d %02x %02x => %d) (%d)\n",
                           cur_drv->head, cur_drv->track, cur_drv->sect,
                           fd_sector(cur_drv),
                           fdctrl->data_pos - len);
            /* XXX: cur_drv->sect >= cur_drv->last_sect should be an
               error in fact */
            if (cur_drv->sect >= cur_drv->last_sect ||
                cur_drv->sect == fdctrl->eot) {
                cur_drv->sect = 1;
                if (FD_MULTI_TRACK(fdctrl->data_state)) {
                    if (cur_drv->head == 0 &&
                        (cur_drv->flags & FDISK_DBL_SIDES) != 0) {
                        cur_drv->head = 1;
                    } else {
                        cur_drv->head = 0;
                        cur_drv->track++;
                        if ((cur_drv->flags & FDISK_DBL_SIDES) == 0)
                            break;
                    }
                } else {
                    cur_drv->track++;
                    break;
                }
                FLOPPY_DPRINTF("seek to next track (%d %02x %02x => %d)\n",
                               cur_drv->head, cur_drv->track,
                               cur_drv->sect, fd_sector(cur_drv));
            } else {
                cur_drv->sect++;
            }
        }
    }
 end_transfer:
    len = fdctrl->data_pos - start_pos;
    FLOPPY_DPRINTF("end transfer %d %d %d\n",
                   fdctrl->data_pos, len, fdctrl->data_len);
    if (fdctrl->data_dir == FD_DIR_SCANE ||
        fdctrl->data_dir == FD_DIR_SCANL ||
        fdctrl->data_dir == FD_DIR_SCANH)
        status2 = 0x08;
    if (FD_DID_SEEK(fdctrl->data_state))
        status0 |= 0x20;
    fdctrl->data_len -= len;
    //    if (fdctrl->data_len == 0)
    fdctrl_stop_transfer(fdctrl, status0, status1, status2);
 transfer_error:

    return len;
}

/* Data register : 0x05 */
static uint32_t fdctrl_read_data (fdctrl_t *fdctrl)
{
    fdrive_t *cur_drv;
    uint32_t retval = 0;
    int pos, len;

    cur_drv = get_cur_drv(fdctrl);
    fdctrl->state &= ~FD_CTRL_SLEEP;
    if (FD_STATE(fdctrl->data_state) == FD_STATE_CMD) {
        FLOPPY_ERROR("can't read data in CMD state\n");
        return 0;
    }
    pos = fdctrl->data_pos;
    if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
        pos %= FD_SECTOR_LEN;
        if (pos == 0) {
            len = fdctrl->data_len - fdctrl->data_pos;
            if (len > FD_SECTOR_LEN)
                len = FD_SECTOR_LEN;
            bdrv_read(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1);
        }
    }
    retval = fdctrl->fifo[pos];
    if (++fdctrl->data_pos == fdctrl->data_len) {
        fdctrl->data_pos = 0;
        /* Switch from transfer mode to status mode
         * then from status mode to command mode
         */
        if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
            fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
        } else {
            fdctrl_reset_fifo(fdctrl);
            fdctrl_reset_irq(fdctrl);
        }
    }
    FLOPPY_DPRINTF("data register: 0x%02x\n", retval);

    return retval;
}

static void fdctrl_format_sector (fdctrl_t *fdctrl)
{
    fdrive_t *cur_drv;
    uint8_t kh, kt, ks;
    int did_seek;

    fdctrl->cur_drv = fdctrl->fifo[1] & 1;
    cur_drv = get_cur_drv(fdctrl);
    kt = fdctrl->fifo[6];
    kh = fdctrl->fifo[7];
    ks = fdctrl->fifo[8];
    FLOPPY_DPRINTF("format sector at %d %d %02x %02x (%d)\n",
                   fdctrl->cur_drv, kh, kt, ks,
                   _fd_sector(kh, kt, ks, cur_drv->last_sect));
    did_seek = 0;
    switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & 0x40)) {
    case 2:
        /* sect too big */
        fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
        fdctrl->fifo[3] = kt;
        fdctrl->fifo[4] = kh;
        fdctrl->fifo[5] = ks;
        return;
    case 3:
        /* track too big */
        fdctrl_stop_transfer(fdctrl, 0x40, 0x80, 0x00);
        fdctrl->fifo[3] = kt;
        fdctrl->fifo[4] = kh;
        fdctrl->fifo[5] = ks;
        return;
    case 4:
        /* No seek enabled */
        fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
        fdctrl->fifo[3] = kt;
        fdctrl->fifo[4] = kh;
        fdctrl->fifo[5] = ks;
        return;
    case 1:
        did_seek = 1;
        fdctrl->data_state |= FD_STATE_SEEK;
        break;
    default:
        break;
    }
    memset(fdctrl->fifo, 0, FD_SECTOR_LEN);
    if (cur_drv->bs == NULL ||
        bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1) < 0) {
        FLOPPY_ERROR("formatting sector %d\n", fd_sector(cur_drv));
        fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
    } else {
        if (cur_drv->sect == cur_drv->last_sect) {
            fdctrl->data_state &= ~FD_STATE_FORMAT;
            /* Last sector done */
            if (FD_DID_SEEK(fdctrl->data_state))
                fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
            else
                fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
        } else {
            /* More to do */
            fdctrl->data_pos = 0;
            fdctrl->data_len = 4;
        }
    }
}

static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)
{
    fdrive_t *cur_drv;

    cur_drv = get_cur_drv(fdctrl);
    /* Reset mode */
    if (fdctrl->state & FD_CTRL_RESET) {
        FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
        return;
    }
    fdctrl->state &= ~FD_CTRL_SLEEP;
    if (FD_STATE(fdctrl->data_state) == FD_STATE_STATUS) {
        FLOPPY_ERROR("can't write data in status mode\n");
        return;
    }
    /* Is it write command time ? */
    if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
        /* FIFO data write */
        fdctrl->fifo[fdctrl->data_pos++] = value;
        if (fdctrl->data_pos % FD_SECTOR_LEN == (FD_SECTOR_LEN - 1) ||
            fdctrl->data_pos == fdctrl->data_len) {
            bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1);
        }
        /* Switch from transfer mode to status mode
         * then from status mode to command mode
         */
        if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA)
            fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
        return;
    }
    if (fdctrl->data_pos == 0) {
        /* Command */
        switch (value & 0x5F) {
        case 0x46:
            /* READ variants */
            FLOPPY_DPRINTF("READ command\n");
            /* 8 parameters cmd */
            fdctrl->data_len = 9;
            goto enqueue;
        case 0x4C:
            /* READ_DELETED variants */
            FLOPPY_DPRINTF("READ_DELETED command\n");
            /* 8 parameters cmd */
            fdctrl->data_len = 9;
            goto enqueue;
        case 0x50:
            /* SCAN_EQUAL variants */
            FLOPPY_DPRINTF("SCAN_EQUAL command\n");
            /* 8 parameters cmd */
            fdctrl->data_len = 9;
            goto enqueue;
        case 0x56:
            /* VERIFY variants */
            FLOPPY_DPRINTF("VERIFY command\n");
            /* 8 parameters cmd */
            fdctrl->data_len = 9;
            goto enqueue;
        case 0x59:
            /* SCAN_LOW_OR_EQUAL variants */
            FLOPPY_DPRINTF("SCAN_LOW_OR_EQUAL command\n");
            /* 8 parameters cmd */
            fdctrl->data_len = 9;
            goto enqueue;
        case 0x5D:
            /* SCAN_HIGH_OR_EQUAL variants */
            FLOPPY_DPRINTF("SCAN_HIGH_OR_EQUAL command\n");
            /* 8 parameters cmd */
            fdctrl->data_len = 9;
            goto enqueue;
        default:
            break;
        }
        switch (value & 0x7F) {
        case 0x45:
            /* WRITE variants */
            FLOPPY_DPRINTF("WRITE command\n");
            /* 8 parameters cmd */
            fdctrl->data_len = 9;
            goto enqueue;
        case 0x49:
            /* WRITE_DELETED variants */
            FLOPPY_DPRINTF("WRITE_DELETED command\n");
            /* 8 parameters cmd */
            fdctrl->data_len = 9;
            goto enqueue;
        default:
            break;
        }
        switch (value) {
        case 0x03:
            /* SPECIFY */
            FLOPPY_DPRINTF("SPECIFY command\n");
            /* 1 parameter cmd */
            fdctrl->data_len = 3;
            goto enqueue;
        case 0x04:
            /* SENSE_DRIVE_STATUS */
            FLOPPY_DPRINTF("SENSE_DRIVE_STATUS command\n");
            /* 1 parameter cmd */
            fdctrl->data_len = 2;
            goto enqueue;
        case 0x07:
            /* RECALIBRATE */
            FLOPPY_DPRINTF("RECALIBRATE command\n");
            /* 1 parameter cmd */
            fdctrl->data_len = 2;
            goto enqueue;
        case 0x08:
            /* SENSE_INTERRUPT_STATUS */
            FLOPPY_DPRINTF("SENSE_INTERRUPT_STATUS command (%02x)\n",
                           fdctrl->int_status);
            /* No parameters cmd: returns status if no interrupt */
#if 0
            fdctrl->fifo[0] =
                fdctrl->int_status | (cur_drv->head << 2) | fdctrl->cur_drv;
#else
            /* XXX: int_status handling is broken for read/write
               commands, so we do this hack. It should be suppressed
               ASAP */
            fdctrl->fifo[0] =
                0x20 | (cur_drv->head << 2) | fdctrl->cur_drv;
#endif
            fdctrl->fifo[1] = cur_drv->track;
            fdctrl_set_fifo(fdctrl, 2, 0);
            fdctrl_reset_irq(fdctrl);
            fdctrl->int_status = 0xC0;
            return;
        case 0x0E:
            /* DUMPREG */
            FLOPPY_DPRINTF("DUMPREG command\n");
            /* Drives position */
            fdctrl->fifo[0] = drv0(fdctrl)->track;
            fdctrl->fifo[1] = drv1(fdctrl)->track;
            fdctrl->fifo[2] = 0;
            fdctrl->fifo[3] = 0;
            /* timers */
            fdctrl->fifo[4] = fdctrl->timer0;
            fdctrl->fifo[5] = (fdctrl->timer1 << 1) | fdctrl->dma_en;
            fdctrl->fifo[6] = cur_drv->last_sect;
            fdctrl->fifo[7] = (fdctrl->lock << 7) |
                (cur_drv->perpendicular << 2);
            fdctrl->fifo[8] = fdctrl->config;
            fdctrl->fifo[9] = fdctrl->precomp_trk;
            fdctrl_set_fifo(fdctrl, 10, 0);
            return;
        case 0x0F:
            /* SEEK */
            FLOPPY_DPRINTF("SEEK command\n");
            /* 2 parameters cmd */
            fdctrl->data_len = 3;
            goto enqueue;
        case 0x10:
            /* VERSION */
            FLOPPY_DPRINTF("VERSION command\n");
            /* No parameters cmd */
            /* Controller's version */
            fdctrl->fifo[0] = fdctrl->version;
            fdctrl_set_fifo(fdctrl, 1, 1);
            return;
        case 0x12:
            /* PERPENDICULAR_MODE */
            FLOPPY_DPRINTF("PERPENDICULAR_MODE command\n");
            /* 1 parameter cmd */
            fdctrl->data_len = 2;
            goto enqueue;
        case 0x13:
            /* CONFIGURE */
            FLOPPY_DPRINTF("CONFIGURE command\n");
            /* 3 parameters cmd */
            fdctrl->data_len = 4;
            goto enqueue;
        case 0x14:
            /* UNLOCK */
            FLOPPY_DPRINTF("UNLOCK command\n");
            /* No parameters cmd */
            fdctrl->lock = 0;
            fdctrl->fifo[0] = 0;
            fdctrl_set_fifo(fdctrl, 1, 0);
            return;
        case 0x17:
            /* POWERDOWN_MODE */
            FLOPPY_DPRINTF("POWERDOWN_MODE command\n");
            /* 2 parameters cmd */
            fdctrl->data_len = 3;
            goto enqueue;
        case 0x18:
            /* PART_ID */
            FLOPPY_DPRINTF("PART_ID command\n");
            /* No parameters cmd */