esp.c.svn-base

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

            DPRINTF("SCSI command completed unexpectedly\n");
        s->ti_size = 0;
        s->dma_left = 0;
        s->async_len = 0;
        if (arg)
            DPRINTF("Command failed\n");
        s->sense = arg;
        s->rregs[ESP_RSTAT] = STAT_ST;
        esp_dma_done(s);
        s->current_dev = NULL;
    } else {
        DPRINTF("transfer %d/%d\n", s->dma_left, s->ti_size);
        s->async_len = arg;
        s->async_buf = s->current_dev->get_buf(s->current_dev, 0);
        if (s->dma_left) {
            esp_do_dma(s);
        } else if (s->dma_counter != 0 && s->ti_size <= 0) {
            /* If this was the last part of a DMA transfer then the
               completion interrupt is deferred to here.  */
            esp_dma_done(s);
        }
    }
}

static void handle_ti(ESPState *s)
{
    uint32_t dmalen, minlen;

    dmalen = s->rregs[ESP_TCLO] | (s->rregs[ESP_TCMID] << 8);
    if (dmalen==0) {
      dmalen=0x10000;
    }
    s->dma_counter = dmalen;

    if (s->do_cmd)
        minlen = (dmalen < 32) ? dmalen : 32;
    else if (s->ti_size < 0)
        minlen = (dmalen < -s->ti_size) ? dmalen : -s->ti_size;
    else
        minlen = (dmalen < s->ti_size) ? dmalen : s->ti_size;
    DPRINTF("Transfer Information len %d\n", minlen);
    if (s->dma) {
        s->dma_left = minlen;
        s->rregs[ESP_RSTAT] &= ~STAT_TC;
        esp_do_dma(s);
    } else if (s->do_cmd) {
        DPRINTF("command len %d\n", s->cmdlen);
        s->ti_size = 0;
        s->cmdlen = 0;
        s->do_cmd = 0;
        do_cmd(s, s->cmdbuf);
        return;
    }
}

static void esp_reset(void *opaque)
{
    ESPState *s = opaque;

    memset(s->rregs, 0, ESP_REGS);
    memset(s->wregs, 0, ESP_REGS);
    s->rregs[ESP_TCHI] = TCHI_FAS100A; // Indicate fas100a
    s->ti_size = 0;
    s->ti_rptr = 0;
    s->ti_wptr = 0;
    s->dma = 0;
    s->do_cmd = 0;
}

static void parent_esp_reset(void *opaque, int irq, int level)
{
    if (level)
        esp_reset(opaque);
}

static uint32_t esp_mem_readb(void *opaque, target_phys_addr_t addr)
{
    ESPState *s = opaque;
    uint32_t saddr;

    saddr = (addr & ESP_MASK) >> 2;
    DPRINTF("read reg[%d]: 0x%2.2x\n", saddr, s->rregs[saddr]);
    switch (saddr) {
    case ESP_FIFO:
        if (s->ti_size > 0) {
            s->ti_size--;
            if ((s->rregs[ESP_RSTAT] & STAT_PIO_MASK) == 0) {
                /* Data in/out.  */
                fprintf(stderr, "esp: PIO data read not implemented\n");
                s->rregs[ESP_FIFO] = 0;
            } else {
                s->rregs[ESP_FIFO] = s->ti_buf[s->ti_rptr++];
            }
            qemu_irq_raise(s->irq);
        }
        if (s->ti_size == 0) {
            s->ti_rptr = 0;
            s->ti_wptr = 0;
        }
        break;
    case ESP_RINTR:
        // Clear interrupt/error status bits
        s->rregs[ESP_RSTAT] &= ~(STAT_IN | STAT_GE | STAT_PE);
        qemu_irq_lower(s->irq);
        break;
    default:
        break;
    }
    return s->rregs[saddr];
}

static void esp_mem_writeb(void *opaque, target_phys_addr_t addr, uint32_t val)
{
    ESPState *s = opaque;
    uint32_t saddr;

    saddr = (addr & ESP_MASK) >> 2;
    DPRINTF("write reg[%d]: 0x%2.2x -> 0x%2.2x\n", saddr, s->wregs[saddr],
            val);
    switch (saddr) {
    case ESP_TCLO:
    case ESP_TCMID:
        s->rregs[ESP_RSTAT] &= ~STAT_TC;
        break;
    case ESP_FIFO:
        if (s->do_cmd) {
            s->cmdbuf[s->cmdlen++] = val & 0xff;
        } else if ((s->rregs[ESP_RSTAT] & STAT_PIO_MASK) == 0) {
            uint8_t buf;
            buf = val & 0xff;
            s->ti_size--;
            fprintf(stderr, "esp: PIO data write not implemented\n");
        } else {
            s->ti_size++;
            s->ti_buf[s->ti_wptr++] = val & 0xff;
        }
        break;
    case ESP_CMD:
        s->rregs[saddr] = val;
        if (val & CMD_DMA) {
            s->dma = 1;
            /* Reload DMA counter.  */
            s->rregs[ESP_TCLO] = s->wregs[ESP_TCLO];
            s->rregs[ESP_TCMID] = s->wregs[ESP_TCMID];
        } else {
            s->dma = 0;
        }
        switch(val & CMD_CMD) {
        case CMD_NOP:
            DPRINTF("NOP (%2.2x)\n", val);
            break;
        case CMD_FLUSH:
            DPRINTF("Flush FIFO (%2.2x)\n", val);
            //s->ti_size = 0;
            s->rregs[ESP_RINTR] = INTR_FC;
            s->rregs[ESP_RSEQ] = 0;
            break;
        case CMD_RESET:
            DPRINTF("Chip reset (%2.2x)\n", val);
            esp_reset(s);
            break;
        case CMD_BUSRESET:
            DPRINTF("Bus reset (%2.2x)\n", val);
            s->rregs[ESP_RINTR] = INTR_RST;
            if (!(s->wregs[ESP_CFG1] & CFG1_RESREPT)) {
                qemu_irq_raise(s->irq);
            }
            break;
        case CMD_TI:
            handle_ti(s);
            break;
        case CMD_ICCS:
            DPRINTF("Initiator Command Complete Sequence (%2.2x)\n", val);
            write_response(s);
            break;
        case CMD_MSGACC:
            DPRINTF("Message Accepted (%2.2x)\n", val);
            write_response(s);
            s->rregs[ESP_RINTR] = INTR_DC;
            s->rregs[ESP_RSEQ] = 0;
            break;
        case CMD_SATN:
            DPRINTF("Set ATN (%2.2x)\n", val);
            break;
        case CMD_SELATN:
            DPRINTF("Set ATN (%2.2x)\n", val);
            handle_satn(s);
            break;
        case CMD_SELATNS:
            DPRINTF("Set ATN & stop (%2.2x)\n", val);
            handle_satn_stop(s);
            break;
        case CMD_ENSEL:
            DPRINTF("Enable selection (%2.2x)\n", val);
            break;
        default:
            DPRINTF("Unhandled ESP command (%2.2x)\n", val);
            break;
        }
        break;
    case ESP_WBUSID ... ESP_WSYNO:
        break;
    case ESP_CFG1:
        s->rregs[saddr] = val;
        break;
    case ESP_WCCF ... ESP_WTEST:
        break;
    case ESP_CFG2:
        s->rregs[saddr] = val & CFG2_MASK;
        break;
    case ESP_CFG3 ... ESP_RES4:
        s->rregs[saddr] = val;
        break;
    default:
        break;
    }
    s->wregs[saddr] = val;
}

static CPUReadMemoryFunc *esp_mem_read[3] = {
    esp_mem_readb,
    NULL,
    NULL,
};

static CPUWriteMemoryFunc *esp_mem_write[3] = {
    esp_mem_writeb,
    NULL,
    NULL,
};

static void esp_save(QEMUFile *f, void *opaque)
{
    ESPState *s = opaque;

    qemu_put_buffer(f, s->rregs, ESP_REGS);
    qemu_put_buffer(f, s->wregs, ESP_REGS);
    qemu_put_be32s(f, &s->ti_size);
    qemu_put_be32s(f, &s->ti_rptr);
    qemu_put_be32s(f, &s->ti_wptr);
    qemu_put_buffer(f, s->ti_buf, TI_BUFSZ);
    qemu_put_be32s(f, &s->sense);
    qemu_put_be32s(f, &s->dma);
    qemu_put_buffer(f, s->cmdbuf, TI_BUFSZ);
    qemu_put_be32s(f, &s->cmdlen);
    qemu_put_be32s(f, &s->do_cmd);
    qemu_put_be32s(f, &s->dma_left);
    // There should be no transfers in progress, so dma_counter is not saved
}

static int esp_load(QEMUFile *f, void *opaque, int version_id)
{
    ESPState *s = opaque;

    if (version_id != 3)
        return -EINVAL; // Cannot emulate 2

    qemu_get_buffer(f, s->rregs, ESP_REGS);
    qemu_get_buffer(f, s->wregs, ESP_REGS);
    qemu_get_be32s(f, &s->ti_size);
    qemu_get_be32s(f, &s->ti_rptr);
    qemu_get_be32s(f, &s->ti_wptr);
    qemu_get_buffer(f, s->ti_buf, TI_BUFSZ);
    qemu_get_be32s(f, &s->sense);
    qemu_get_be32s(f, &s->dma);
    qemu_get_buffer(f, s->cmdbuf, TI_BUFSZ);
    qemu_get_be32s(f, &s->cmdlen);
    qemu_get_be32s(f, &s->do_cmd);
    qemu_get_be32s(f, &s->dma_left);

    return 0;
}

void esp_scsi_attach(void *opaque, BlockDriverState *bd, int id)
{
    ESPState *s = (ESPState *)opaque;

    if (id < 0) {
        for (id = 0; id < ESP_MAX_DEVS; id++) {
            if (s->scsi_dev[id] == NULL)
                break;
        }
    }
    if (id >= ESP_MAX_DEVS) {
        DPRINTF("Bad Device ID %d\n", id);
        return;
    }
    if (s->scsi_dev[id]) {
        DPRINTF("Destroying device %d\n", id);
        s->scsi_dev[id]->destroy(s->scsi_dev[id]);
    }
    DPRINTF("Attaching block device %d\n", id);
    /* Command queueing is not implemented.  */
    s->scsi_dev[id] = scsi_generic_init(bd, 0, esp_command_complete, s);
    if (s->scsi_dev[id] == NULL)
        s->scsi_dev[id] = scsi_disk_init(bd, 0, esp_command_complete, s);
}

void *esp_init(target_phys_addr_t espaddr,
               void *dma_opaque, qemu_irq irq, qemu_irq *reset)
{
    ESPState *s;
    int esp_io_memory;

    s = qemu_mallocz(sizeof(ESPState));
    if (!s)
        return NULL;

    s->irq = irq;
    s->dma_opaque = dma_opaque;

    esp_io_memory = cpu_register_io_memory(0, esp_mem_read, esp_mem_write, s);
    cpu_register_physical_memory(espaddr, ESP_SIZE, esp_io_memory);

    esp_reset(s);

    register_savevm("esp", espaddr, 3, esp_save, esp_load, s);
    qemu_register_reset(esp_reset, s);

    *reset = *qemu_allocate_irqs(parent_esp_reset, s, 1);

    return s;
}