fdc.c.svn-base

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

    uint8_t cur_drv;
    uint8_t bootsel;
    /* Command FIFO */
    uint8_t *fifo;
    uint32_t data_pos;
    uint32_t data_len;
    uint8_t data_state;
    uint8_t data_dir;
    uint8_t int_status;
    uint8_t eot; /* last wanted sector */
    /* States kept only to be returned back */
    /* Timers state */
    uint8_t timer0;
    uint8_t timer1;
    /* precompensation */
    uint8_t precomp_trk;
    uint8_t config;
    uint8_t lock;
    /* Power down config (also with status regB access mode */
    uint8_t pwrd;
    /* Sun4m quirks? */
    int sun4m;
    /* Floppy drives */
    fdrive_t drives[2];
};

static uint32_t fdctrl_read (void *opaque, uint32_t reg)
{
    fdctrl_t *fdctrl = opaque;
    uint32_t retval;

    switch (reg & 0x07) {
    case 0x00:
        if (fdctrl->sun4m) {
            // Identify to Linux as S82078B
            retval = fdctrl_read_statusB(fdctrl);
        } else {
            retval = (uint32_t)(-1);
        }
        break;
    case 0x01:
        retval = fdctrl_read_statusB(fdctrl);
        break;
    case 0x02:
        retval = fdctrl_read_dor(fdctrl);
        break;
    case 0x03:
        retval = fdctrl_read_tape(fdctrl);
        break;
    case 0x04:
        retval = fdctrl_read_main_status(fdctrl);
        break;
    case 0x05:
        retval = fdctrl_read_data(fdctrl);
        break;
    case 0x07:
        retval = fdctrl_read_dir(fdctrl);
        break;
    default:
        retval = (uint32_t)(-1);
        break;
    }
    FLOPPY_DPRINTF("read reg%d: 0x%02x\n", reg & 7, retval);

    return retval;
}

static void fdctrl_write (void *opaque, uint32_t reg, uint32_t value)
{
    fdctrl_t *fdctrl = opaque;

    FLOPPY_DPRINTF("write reg%d: 0x%02x\n", reg & 7, value);

    switch (reg & 0x07) {
    case 0x02:
        fdctrl_write_dor(fdctrl, value);
        break;
    case 0x03:
        fdctrl_write_tape(fdctrl, value);
        break;
    case 0x04:
        fdctrl_write_rate(fdctrl, value);
        break;
    case 0x05:
        fdctrl_write_data(fdctrl, value);
        break;
    default:
        break;
    }
}

static uint32_t fdctrl_read_mem (void *opaque, target_phys_addr_t reg)
{
    return fdctrl_read(opaque, (uint32_t)reg);
}

static void fdctrl_write_mem (void *opaque,
                              target_phys_addr_t reg, uint32_t value)
{
    fdctrl_write(opaque, (uint32_t)reg, value);
}

static CPUReadMemoryFunc *fdctrl_mem_read[3] = {
    fdctrl_read_mem,
    fdctrl_read_mem,
    fdctrl_read_mem,
};

static CPUWriteMemoryFunc *fdctrl_mem_write[3] = {
    fdctrl_write_mem,
    fdctrl_write_mem,
    fdctrl_write_mem,
};

static CPUReadMemoryFunc *fdctrl_mem_read_strict[3] = {
    fdctrl_read_mem,
    NULL,
    NULL,
};

static CPUWriteMemoryFunc *fdctrl_mem_write_strict[3] = {
    fdctrl_write_mem,
    NULL,
    NULL,
};

static void fd_save (QEMUFile *f, fdrive_t *fd)
{
    uint8_t tmp;

    tmp = fd->drflags;
    qemu_put_8s(f, &tmp);
    qemu_put_8s(f, &fd->head);
    qemu_put_8s(f, &fd->track);
    qemu_put_8s(f, &fd->sect);
    qemu_put_8s(f, &fd->dir);
    qemu_put_8s(f, &fd->rw);
}

static void fdc_save (QEMUFile *f, void *opaque)
{
    fdctrl_t *s = opaque;

    qemu_put_8s(f, &s->state);
    qemu_put_8s(f, &s->dma_en);
    qemu_put_8s(f, &s->cur_drv);
    qemu_put_8s(f, &s->bootsel);
    qemu_put_buffer(f, s->fifo, FD_SECTOR_LEN);
    qemu_put_be32s(f, &s->data_pos);
    qemu_put_be32s(f, &s->data_len);
    qemu_put_8s(f, &s->data_state);
    qemu_put_8s(f, &s->data_dir);
    qemu_put_8s(f, &s->int_status);
    qemu_put_8s(f, &s->eot);
    qemu_put_8s(f, &s->timer0);
    qemu_put_8s(f, &s->timer1);
    qemu_put_8s(f, &s->precomp_trk);
    qemu_put_8s(f, &s->config);
    qemu_put_8s(f, &s->lock);
    qemu_put_8s(f, &s->pwrd);
    fd_save(f, &s->drives[0]);
    fd_save(f, &s->drives[1]);
}

static int fd_load (QEMUFile *f, fdrive_t *fd)
{
    uint8_t tmp;

    qemu_get_8s(f, &tmp);
    fd->drflags = tmp;
    qemu_get_8s(f, &fd->head);
    qemu_get_8s(f, &fd->track);
    qemu_get_8s(f, &fd->sect);
    qemu_get_8s(f, &fd->dir);
    qemu_get_8s(f, &fd->rw);

    return 0;
}

static int fdc_load (QEMUFile *f, void *opaque, int version_id)
{
    fdctrl_t *s = opaque;
    int ret;

    if (version_id != 1)
        return -EINVAL;

    qemu_get_8s(f, &s->state);
    qemu_get_8s(f, &s->dma_en);
    qemu_get_8s(f, &s->cur_drv);
    qemu_get_8s(f, &s->bootsel);
    qemu_get_buffer(f, s->fifo, FD_SECTOR_LEN);
    qemu_get_be32s(f, &s->data_pos);
    qemu_get_be32s(f, &s->data_len);
    qemu_get_8s(f, &s->data_state);
    qemu_get_8s(f, &s->data_dir);
    qemu_get_8s(f, &s->int_status);
    qemu_get_8s(f, &s->eot);
    qemu_get_8s(f, &s->timer0);
    qemu_get_8s(f, &s->timer1);
    qemu_get_8s(f, &s->precomp_trk);
    qemu_get_8s(f, &s->config);
    qemu_get_8s(f, &s->lock);
    qemu_get_8s(f, &s->pwrd);

    ret = fd_load(f, &s->drives[0]);
    if (ret == 0)
        ret = fd_load(f, &s->drives[1]);

    return ret;
}

static void fdctrl_external_reset(void *opaque)
{
    fdctrl_t *s = opaque;

    fdctrl_reset(s, 0);
}

static fdctrl_t *fdctrl_init_common (qemu_irq irq, int dma_chann,
                                     target_phys_addr_t io_base,
                                     BlockDriverState **fds)
{
    fdctrl_t *fdctrl;
    int i;

    FLOPPY_DPRINTF("init controller\n");
    fdctrl = qemu_mallocz(sizeof(fdctrl_t));
    if (!fdctrl)
        return NULL;
    fdctrl->fifo = qemu_memalign(512, FD_SECTOR_LEN);
    if (fdctrl->fifo == NULL) {
        qemu_free(fdctrl);
        return NULL;
    }
    fdctrl->result_timer = qemu_new_timer(vm_clock,
                                          fdctrl_result_timer, fdctrl);

    fdctrl->version = 0x90; /* Intel 82078 controller */
    fdctrl->irq = irq;
    fdctrl->dma_chann = dma_chann;
    fdctrl->io_base = io_base;
    fdctrl->config = 0x60; /* Implicit seek, polling & FIFO enabled */
    if (fdctrl->dma_chann != -1) {
        fdctrl->dma_en = 1;
        DMA_register_channel(dma_chann, &fdctrl_transfer_handler, fdctrl);
    } else {
        fdctrl->dma_en = 0;
    }
    for (i = 0; i < 2; i++) {
        fd_init(&fdctrl->drives[i], fds[i]);
    }
    fdctrl_reset(fdctrl, 0);
    fdctrl->state = FD_CTRL_ACTIVE;
    register_savevm("fdc", io_base, 1, fdc_save, fdc_load, fdctrl);
    qemu_register_reset(fdctrl_external_reset, fdctrl);
    for (i = 0; i < 2; i++) {
        fd_revalidate(&fdctrl->drives[i]);
    }

    return fdctrl;
}

fdctrl_t *fdctrl_init (qemu_irq irq, int dma_chann, int mem_mapped,
                       target_phys_addr_t io_base,
                       BlockDriverState **fds)
{
    fdctrl_t *fdctrl;
    int io_mem;

    fdctrl = fdctrl_init_common(irq, dma_chann, io_base, fds);

    fdctrl->sun4m = 0;
    if (mem_mapped) {
        io_mem = cpu_register_io_memory(0, fdctrl_mem_read, fdctrl_mem_write,
                                        fdctrl);
        cpu_register_physical_memory(io_base, 0x08, io_mem);
    } else {
        register_ioport_read((uint32_t)io_base + 0x01, 5, 1, &fdctrl_read,
                             fdctrl);
        register_ioport_read((uint32_t)io_base + 0x07, 1, 1, &fdctrl_read,
                             fdctrl);
        register_ioport_write((uint32_t)io_base + 0x01, 5, 1, &fdctrl_write,
                              fdctrl);
        register_ioport_write((uint32_t)io_base + 0x07, 1, 1, &fdctrl_write,
                              fdctrl);
    }

    return fdctrl;
}

fdctrl_t *sun4m_fdctrl_init (qemu_irq irq, target_phys_addr_t io_base,
                             BlockDriverState **fds)
{
    fdctrl_t *fdctrl;
    int io_mem;

    fdctrl = fdctrl_init_common(irq, 0, io_base, fds);
    fdctrl->sun4m = 1;
    io_mem = cpu_register_io_memory(0, fdctrl_mem_read_strict,
                                    fdctrl_mem_write_strict,
                                    fdctrl);
    cpu_register_physical_memory(io_base, 0x08, io_mem);

    return fdctrl;
}

/* XXX: may change if moved to bdrv */
int fdctrl_get_drive_type(fdctrl_t *fdctrl, int drive_num)
{
    return fdctrl->drives[drive_num].drive;
}

/* Change IRQ state */
static void fdctrl_reset_irq (fdctrl_t *fdctrl)
{
    FLOPPY_DPRINTF("Reset interrupt\n");
    qemu_set_irq(fdctrl->irq, 0);
    fdctrl->state &= ~FD_CTRL_INTR;
}

static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status)
{
    // Sparc mutation
    if (fdctrl->sun4m && !fdctrl->dma_en) {
        fdctrl->state &= ~FD_CTRL_BUSY;
        fdctrl->int_status = status;
        return;
    }
    if (~(fdctrl->state & FD_CTRL_INTR)) {
        qemu_set_irq(fdctrl->irq, 1);
        fdctrl->state |= FD_CTRL_INTR;
    }
    FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", status);
    fdctrl->int_status = status;
}

/* Reset controller */
static void fdctrl_reset (fdctrl_t *fdctrl, int do_irq)
{
    int i;

    FLOPPY_DPRINTF("reset controller\n");
    fdctrl_reset_irq(fdctrl);
    /* Initialise controller */
    fdctrl->cur_drv = 0;
    /* FIFO state */
    fdctrl->data_pos = 0;
    fdctrl->data_len = 0;
    fdctrl->data_state = FD_STATE_CMD;
    fdctrl->data_dir = FD_DIR_WRITE;
    for (i = 0; i < MAX_FD; i++)
        fd_reset(&fdctrl->drives[i]);
    fdctrl_reset_fifo(fdctrl);
    if (do_irq)
        fdctrl_raise_irq(fdctrl, 0xc0);
}

static inline fdrive_t *drv0 (fdctrl_t *fdctrl)
{
    return &fdctrl->drives[fdctrl->bootsel];
}

static inline fdrive_t *drv1 (fdctrl_t *fdctrl)
{
    return &fdctrl->drives[1 - fdctrl->bootsel];
}

static fdrive_t *get_cur_drv (fdctrl_t *fdctrl)
{
    return fdctrl->cur_drv == 0 ? drv0(fdctrl) : drv1(fdctrl);
}

/* Status B register : 0x01 (read-only) */
static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl)
{
    FLOPPY_DPRINTF("status register: 0x00\n");
    return 0;
}

/* Digital output register : 0x02 */