fdc.c.svn-base

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

            fdctrl->fifo[0] = 0x41; /* Stepping 1 */
            fdctrl_set_fifo(fdctrl, 1, 0);
            return;
        case 0x2C:
            /* SAVE */
            FLOPPY_DPRINTF("SAVE command\n");
            /* No parameters cmd */
            fdctrl->fifo[0] = 0;
            fdctrl->fifo[1] = 0;
            /* Drives position */
            fdctrl->fifo[2] = drv0(fdctrl)->track;
            fdctrl->fifo[3] = drv1(fdctrl)->track;
            fdctrl->fifo[4] = 0;
            fdctrl->fifo[5] = 0;
            /* timers */
            fdctrl->fifo[6] = fdctrl->timer0;
            fdctrl->fifo[7] = fdctrl->timer1;
            fdctrl->fifo[8] = cur_drv->last_sect;
            fdctrl->fifo[9] = (fdctrl->lock << 7) |
                (cur_drv->perpendicular << 2);
            fdctrl->fifo[10] = fdctrl->config;
            fdctrl->fifo[11] = fdctrl->precomp_trk;
            fdctrl->fifo[12] = fdctrl->pwrd;
            fdctrl->fifo[13] = 0;
            fdctrl->fifo[14] = 0;
            fdctrl_set_fifo(fdctrl, 15, 1);
            return;
        case 0x33:
            /* OPTION */
            FLOPPY_DPRINTF("OPTION command\n");
            /* 1 parameter cmd */
            fdctrl->data_len = 2;
            goto enqueue;
        case 0x42:
            /* READ_TRACK */
            FLOPPY_DPRINTF("READ_TRACK command\n");
            /* 8 parameters cmd */
            fdctrl->data_len = 9;
            goto enqueue;
        case 0x4A:
            /* READ_ID */
            FLOPPY_DPRINTF("READ_ID command\n");
            /* 1 parameter cmd */
            fdctrl->data_len = 2;
            goto enqueue;
        case 0x4C:
            /* RESTORE */
            FLOPPY_DPRINTF("RESTORE command\n");
            /* 17 parameters cmd */
            fdctrl->data_len = 18;
            goto enqueue;
        case 0x4D:
            /* FORMAT_TRACK */
            FLOPPY_DPRINTF("FORMAT_TRACK command\n");
            /* 5 parameters cmd */
            fdctrl->data_len = 6;
            goto enqueue;
        case 0x8E:
            /* DRIVE_SPECIFICATION_COMMAND */
            FLOPPY_DPRINTF("DRIVE_SPECIFICATION_COMMAND command\n");
            /* 5 parameters cmd */
            fdctrl->data_len = 6;
            goto enqueue;
        case 0x8F:
            /* RELATIVE_SEEK_OUT */
            FLOPPY_DPRINTF("RELATIVE_SEEK_OUT command\n");
            /* 2 parameters cmd */
            fdctrl->data_len = 3;
            goto enqueue;
        case 0x94:
            /* LOCK */
            FLOPPY_DPRINTF("LOCK command\n");
            /* No parameters cmd */
            fdctrl->lock = 1;
            fdctrl->fifo[0] = 0x10;
            fdctrl_set_fifo(fdctrl, 1, 1);
            return;
        case 0xCD:
            /* FORMAT_AND_WRITE */
            FLOPPY_DPRINTF("FORMAT_AND_WRITE command\n");
            /* 10 parameters cmd */
            fdctrl->data_len = 11;
            goto enqueue;
        case 0xCF:
            /* RELATIVE_SEEK_IN */
            FLOPPY_DPRINTF("RELATIVE_SEEK_IN command\n");
            /* 2 parameters cmd */
            fdctrl->data_len = 3;
            goto enqueue;
        default:
            /* Unknown command */
            FLOPPY_ERROR("unknown command: 0x%02x\n", value);
            fdctrl_unimplemented(fdctrl);
            return;
        }
    }
 enqueue:
    FLOPPY_DPRINTF("%s: %02x\n", __func__, value);
    fdctrl->fifo[fdctrl->data_pos] = value;
    if (++fdctrl->data_pos == fdctrl->data_len) {
        /* We now have all parameters
         * and will be able to treat the command
         */
        if (fdctrl->data_state & FD_STATE_FORMAT) {
            fdctrl_format_sector(fdctrl);
            return;
        }
        switch (fdctrl->fifo[0] & 0x1F) {
        case 0x06:
            {
                /* READ variants */
                FLOPPY_DPRINTF("treat READ command\n");
                fdctrl_start_transfer(fdctrl, FD_DIR_READ);
                return;
            }
        case 0x0C:
            /* READ_DELETED variants */
//            FLOPPY_DPRINTF("treat READ_DELETED command\n");
            FLOPPY_ERROR("treat READ_DELETED command\n");
            fdctrl_start_transfer_del(fdctrl, FD_DIR_READ);
            return;
        case 0x16:
            /* VERIFY variants */
//            FLOPPY_DPRINTF("treat VERIFY command\n");
            FLOPPY_ERROR("treat VERIFY command\n");
            fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
            return;
        case 0x10:
            /* SCAN_EQUAL variants */
//            FLOPPY_DPRINTF("treat SCAN_EQUAL command\n");
            FLOPPY_ERROR("treat SCAN_EQUAL command\n");
            fdctrl_start_transfer(fdctrl, FD_DIR_SCANE);
            return;
        case 0x19:
            /* SCAN_LOW_OR_EQUAL variants */
//            FLOPPY_DPRINTF("treat SCAN_LOW_OR_EQUAL command\n");
            FLOPPY_ERROR("treat SCAN_LOW_OR_EQUAL command\n");
            fdctrl_start_transfer(fdctrl, FD_DIR_SCANL);
            return;
        case 0x1D:
            /* SCAN_HIGH_OR_EQUAL variants */
//            FLOPPY_DPRINTF("treat SCAN_HIGH_OR_EQUAL command\n");
            FLOPPY_ERROR("treat SCAN_HIGH_OR_EQUAL command\n");
            fdctrl_start_transfer(fdctrl, FD_DIR_SCANH);
            return;
        default:
            break;
        }
        switch (fdctrl->fifo[0] & 0x3F) {
        case 0x05:
            /* WRITE variants */
            FLOPPY_DPRINTF("treat WRITE command (%02x)\n", fdctrl->fifo[0]);
            fdctrl_start_transfer(fdctrl, FD_DIR_WRITE);
            return;
        case 0x09:
            /* WRITE_DELETED variants */
//            FLOPPY_DPRINTF("treat WRITE_DELETED command\n");
            FLOPPY_ERROR("treat WRITE_DELETED command\n");
            fdctrl_start_transfer_del(fdctrl, FD_DIR_WRITE);
            return;
        default:
            break;
        }
        switch (fdctrl->fifo[0]) {
        case 0x03:
            /* SPECIFY */
            FLOPPY_DPRINTF("treat SPECIFY command\n");
            fdctrl->timer0 = (fdctrl->fifo[1] >> 4) & 0xF;
            fdctrl->timer1 = fdctrl->fifo[2] >> 1;
            fdctrl->dma_en = 1 - (fdctrl->fifo[2] & 1) ;
            /* No result back */
            fdctrl_reset_fifo(fdctrl);
            break;
        case 0x04:
            /* SENSE_DRIVE_STATUS */
            FLOPPY_DPRINTF("treat SENSE_DRIVE_STATUS command\n");
            fdctrl->cur_drv = fdctrl->fifo[1] & 1;
            cur_drv = get_cur_drv(fdctrl);
            cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
            /* 1 Byte status back */
            fdctrl->fifo[0] = (cur_drv->ro << 6) |
                (cur_drv->track == 0 ? 0x10 : 0x00) |
                (cur_drv->head << 2) |
                fdctrl->cur_drv |
                0x28;
            fdctrl_set_fifo(fdctrl, 1, 0);
            break;
        case 0x07:
            /* RECALIBRATE */
            FLOPPY_DPRINTF("treat RECALIBRATE command\n");
            fdctrl->cur_drv = fdctrl->fifo[1] & 1;
            cur_drv = get_cur_drv(fdctrl);
            fd_recalibrate(cur_drv);
            fdctrl_reset_fifo(fdctrl);
            /* Raise Interrupt */
            fdctrl_raise_irq(fdctrl, 0x20);
            break;
        case 0x0F:
            /* SEEK */
            FLOPPY_DPRINTF("treat SEEK command\n");
            fdctrl->cur_drv = fdctrl->fifo[1] & 1;
            cur_drv = get_cur_drv(fdctrl);
            fd_start(cur_drv);
            if (fdctrl->fifo[2] <= cur_drv->track)
                cur_drv->dir = 1;
            else
                cur_drv->dir = 0;
            fdctrl_reset_fifo(fdctrl);
            if (fdctrl->fifo[2] > cur_drv->max_track) {
                fdctrl_raise_irq(fdctrl, 0x60);
            } else {
                cur_drv->track = fdctrl->fifo[2];
                /* Raise Interrupt */
                fdctrl_raise_irq(fdctrl, 0x20);
            }
            break;
        case 0x12:
            /* PERPENDICULAR_MODE */
            FLOPPY_DPRINTF("treat PERPENDICULAR_MODE command\n");
            if (fdctrl->fifo[1] & 0x80)
                cur_drv->perpendicular = fdctrl->fifo[1] & 0x7;
            /* No result back */
            fdctrl_reset_fifo(fdctrl);
            break;
        case 0x13:
            /* CONFIGURE */
            FLOPPY_DPRINTF("treat CONFIGURE command\n");
            fdctrl->config = fdctrl->fifo[2];
            fdctrl->precomp_trk =  fdctrl->fifo[3];
            /* No result back */
            fdctrl_reset_fifo(fdctrl);
            break;
        case 0x17:
            /* POWERDOWN_MODE */
            FLOPPY_DPRINTF("treat POWERDOWN_MODE command\n");
            fdctrl->pwrd = fdctrl->fifo[1];
            fdctrl->fifo[0] = fdctrl->fifo[1];
            fdctrl_set_fifo(fdctrl, 1, 1);
            break;
        case 0x33:
            /* OPTION */
            FLOPPY_DPRINTF("treat OPTION command\n");
            /* No result back */
            fdctrl_reset_fifo(fdctrl);
            break;
        case 0x42:
            /* READ_TRACK */
//            FLOPPY_DPRINTF("treat READ_TRACK command\n");
            FLOPPY_ERROR("treat READ_TRACK command\n");
            fdctrl_start_transfer(fdctrl, FD_DIR_READ);
            break;
        case 0x4A:
            /* READ_ID */
            FLOPPY_DPRINTF("treat READ_ID command\n");
            /* XXX: should set main status register to busy */
            cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
            qemu_mod_timer(fdctrl->result_timer,
                           qemu_get_clock(vm_clock) + (ticks_per_sec / 50));
            break;
        case 0x4C:
            /* RESTORE */
            FLOPPY_DPRINTF("treat RESTORE command\n");
            /* Drives position */
            drv0(fdctrl)->track = fdctrl->fifo[3];
            drv1(fdctrl)->track = fdctrl->fifo[4];
            /* timers */
            fdctrl->timer0 = fdctrl->fifo[7];
            fdctrl->timer1 = fdctrl->fifo[8];
            cur_drv->last_sect = fdctrl->fifo[9];
            fdctrl->lock = fdctrl->fifo[10] >> 7;
            cur_drv->perpendicular = (fdctrl->fifo[10] >> 2) & 0xF;
            fdctrl->config = fdctrl->fifo[11];
            fdctrl->precomp_trk = fdctrl->fifo[12];
            fdctrl->pwrd = fdctrl->fifo[13];
            fdctrl_reset_fifo(fdctrl);
            break;
        case 0x4D:
            /* FORMAT_TRACK */
            FLOPPY_DPRINTF("treat FORMAT_TRACK command\n");
            fdctrl->cur_drv = fdctrl->fifo[1] & 1;
            cur_drv = get_cur_drv(fdctrl);
            fdctrl->data_state |= FD_STATE_FORMAT;
            if (fdctrl->fifo[0] & 0x80)
                fdctrl->data_state |= FD_STATE_MULTI;
            else
                fdctrl->data_state &= ~FD_STATE_MULTI;
            fdctrl->data_state &= ~FD_STATE_SEEK;
            cur_drv->bps =
                fdctrl->fifo[2] > 7 ? 16384 : 128 << fdctrl->fifo[2];
#if 0
            cur_drv->last_sect =
                cur_drv->flags & FDISK_DBL_SIDES ? fdctrl->fifo[3] :
                fdctrl->fifo[3] / 2;
#else
            cur_drv->last_sect = fdctrl->fifo[3];
#endif
            /* TODO: implement format using DMA expected by the Bochs BIOS
             * and Linux fdformat (read 3 bytes per sector via DMA and fill
             * the sector with the specified fill byte
             */
            fdctrl->data_state &= ~FD_STATE_FORMAT;
            fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
            break;
        case 0x8E:
            /* DRIVE_SPECIFICATION_COMMAND */
            FLOPPY_DPRINTF("treat DRIVE_SPECIFICATION_COMMAND command\n");
            if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x80) {
                /* Command parameters done */
                if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x40) {
                    fdctrl->fifo[0] = fdctrl->fifo[1];
                    fdctrl->fifo[2] = 0;
                    fdctrl->fifo[3] = 0;
                    fdctrl_set_fifo(fdctrl, 4, 1);
                } else {
                    fdctrl_reset_fifo(fdctrl);
                }
            } else if (fdctrl->data_len > 7) {
                /* ERROR */
                fdctrl->fifo[0] = 0x80 |
                    (cur_drv->head << 2) | fdctrl->cur_drv;
                fdctrl_set_fifo(fdctrl, 1, 1);
            }
            break;
        case 0x8F:
            /* RELATIVE_SEEK_OUT */
            FLOPPY_DPRINTF("treat RELATIVE_SEEK_OUT command\n");
            fdctrl->cur_drv = fdctrl->fifo[1] & 1;
            cur_drv = get_cur_drv(fdctrl);
            fd_start(cur_drv);
            cur_drv->dir = 0;
            if (fdctrl->fifo[2] + cur_drv->track >= cur_drv->max_track) {
                cur_drv->track = cur_drv->max_track - 1;
            } else {
                cur_drv->track += fdctrl->fifo[2];
            }
            fdctrl_reset_fifo(fdctrl);
            fdctrl_raise_irq(fdctrl, 0x20);
            break;
        case 0xCD:
            /* FORMAT_AND_WRITE */
//                FLOPPY_DPRINTF("treat FORMAT_AND_WRITE command\n");
            FLOPPY_ERROR("treat FORMAT_AND_WRITE command\n");
            fdctrl_unimplemented(fdctrl);
            break;
        case 0xCF:
            /* RELATIVE_SEEK_IN */
            FLOPPY_DPRINTF("treat RELATIVE_SEEK_IN command\n");
            fdctrl->cur_drv = fdctrl->fifo[1] & 1;
            cur_drv = get_cur_drv(fdctrl);
            fd_start(cur_drv);
            cur_drv->dir = 1;
            if (fdctrl->fifo[2] > cur_drv->track) {
                cur_drv->track = 0;
            } else {
                cur_drv->track -= fdctrl->fifo[2];
            }
            fdctrl_reset_fifo(fdctrl);
            /* Raise Interrupt */
            fdctrl_raise_irq(fdctrl, 0x20);
            break;
        }
    }
}

static void fdctrl_result_timer(void *opaque)
{
    fdctrl_t *fdctrl = opaque;
    fdrive_t *cur_drv = get_cur_drv(fdctrl);

    /* Pretend we are spinning.
     * This is needed for Coherent, which uses READ ID to check for
     * sector interleaving.
     */
    if (cur_drv->last_sect != 0) {
        cur_drv->sect = (cur_drv->sect % cur_drv->last_sect) + 1;
    }
    fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
}