stdvga.c

来自「xen 3.2.2 源码」· C语言 代码 · 共 640 行 · 第 1/2 页

    addr = stdvga_mem_offset(s, addr);
    if ( addr == ~0u )
        return;

    if ( s->sr[4] & 0x08 )
    {
        /* chain 4 mode : simplest access */
        plane = addr & 3;
        mask = (1 << plane);
        if ( s->sr[2] & mask )
        {
            vram_b = vram_getb(s, addr);
            *vram_b = val;
            vram_put(s, vram_b);
        }
    }
    else if ( s->gr[5] & 0x10 )
    {
        /* odd/even mode (aka text mode mapping) */
        plane = (s->gr[4] & 2) | (addr & 1);
        mask = (1 << plane);
        if ( s->sr[2] & mask )
        {
            addr = ((addr & ~1) << 1) | plane;
            vram_b = vram_getb(s, addr);
            *vram_b = val;
            vram_put(s, vram_b);
        }
    }
    else
    {
        write_mode = s->gr[5] & 3;
        switch ( write_mode )
        {
        default:
        case 0:
            /* rotate */
            b = s->gr[3] & 7;
            val = ((val >> b) | (val << (8 - b))) & 0xff;
            val |= val << 8;
            val |= val << 16;

            /* apply set/reset mask */
            set_mask = mask16[s->gr[1]];
            val = (val & ~set_mask) | (mask16[s->gr[0]] & set_mask);
            bit_mask = s->gr[8];
            break;
        case 1:
            val = s->latch;
            goto do_write;
        case 2:
            val = mask16[val & 0x0f];
            bit_mask = s->gr[8];
            break;
        case 3:
            /* rotate */
            b = s->gr[3] & 7;
            val = (val >> b) | (val << (8 - b));

            bit_mask = s->gr[8] & val;
            val = mask16[s->gr[0]];
            break;
        }

        /* apply logical operation */
        func_select = s->gr[3] >> 3;
        switch ( func_select )
        {
        case 0:
        default:
            /* nothing to do */
            break;
        case 1:
            /* and */
            val &= s->latch;
            break;
        case 2:
            /* or */
            val |= s->latch;
            break;
        case 3:
            /* xor */
            val ^= s->latch;
            break;
        }

        /* apply bit mask */
        bit_mask |= bit_mask << 8;
        bit_mask |= bit_mask << 16;
        val = (val & bit_mask) | (s->latch & ~bit_mask);

    do_write:
        /* mask data according to sr[2] */
        mask = s->sr[2];
        write_mask = mask16[mask];
        vram_l = vram_getl(s, addr);
        *vram_l = (*vram_l & ~write_mask) | (val & write_mask);
        vram_put(s, vram_l);
    }
}

static void stdvga_mem_write(uint64_t addr, uint64_t data, uint64_t size)
{
    /* Intercept mmio write */
    switch ( size )
    {
    case 1:
        stdvga_mem_writeb(addr, (data >>  0) & 0xff);
        break;

    case 2:
        stdvga_mem_writeb(addr+0, (data >>  0) & 0xff);
        stdvga_mem_writeb(addr+1, (data >>  8) & 0xff);
        break;

    case 4:
        stdvga_mem_writeb(addr+0, (data >>  0) & 0xff);
        stdvga_mem_writeb(addr+1, (data >>  8) & 0xff);
        stdvga_mem_writeb(addr+2, (data >> 16) & 0xff);
        stdvga_mem_writeb(addr+3, (data >> 24) & 0xff);
        break;

    case 8:
        stdvga_mem_writeb(addr+0, (data >>  0) & 0xff);
        stdvga_mem_writeb(addr+1, (data >>  8) & 0xff);
        stdvga_mem_writeb(addr+2, (data >> 16) & 0xff);
        stdvga_mem_writeb(addr+3, (data >> 24) & 0xff);
        stdvga_mem_writeb(addr+4, (data >> 32) & 0xff);
        stdvga_mem_writeb(addr+5, (data >> 40) & 0xff);
        stdvga_mem_writeb(addr+6, (data >> 48) & 0xff);
        stdvga_mem_writeb(addr+7, (data >> 56) & 0xff);
        break;

    default:
        gdprintk(XENLOG_WARNING, "invalid io size: %"PRId64"\n", size);
        break;
    }
}

static uint32_t read_data;

static int mmio_move(struct hvm_hw_stdvga *s, ioreq_t *p)
{
    int i;
    int sign = p->df ? -1 : 1;

    if ( p->data_is_ptr )
    {
        if ( p->dir == IOREQ_READ )
        {
            uint64_t addr = p->addr, data = p->data, tmp;
            for ( i = 0; i < p->count; i++ ) 
            {
                tmp = stdvga_mem_read(addr, p->size);
                hvm_copy_to_guest_phys(data, &tmp, p->size);
                data += sign * p->size;
                addr += sign * p->size;
            }
        }
        else
        {
            uint32_t addr = p->addr, data = p->data, tmp;
            for ( i = 0; i < p->count; i++ )
            {
                hvm_copy_from_guest_phys(&tmp, data, p->size);
                stdvga_mem_write(addr, tmp, p->size);
                data += sign * p->size;
                addr += sign * p->size;
            }
        }
    }
    else
    {
        if ( p->dir == IOREQ_READ )
        {
            uint32_t addr = p->addr;
            for ( i = 0; i < p->count; i++ )
            {
                p->data = stdvga_mem_read(addr, p->size);
                addr += sign * p->size;
            }
        }
        else
        {
            uint32_t addr = p->addr;
            for ( i = 0; i < p->count; i++ )
            {
                stdvga_mem_write(addr, p->data, p->size);
                addr += sign * p->size;
            }
        }
    }

    read_data = p->data;
    return 1;
}

static uint32_t op_and(uint32_t a, uint32_t b) { return a & b; }
static uint32_t op_or (uint32_t a, uint32_t b) { return a | b; }
static uint32_t op_xor(uint32_t a, uint32_t b) { return a ^ b; }
static uint32_t op_add(uint32_t a, uint32_t b) { return a + b; }
static uint32_t op_sub(uint32_t a, uint32_t b) { return a - b; }
static uint32_t (*op_array[])(uint32_t, uint32_t) = {
    [IOREQ_TYPE_AND] = op_and,
    [IOREQ_TYPE_OR ] = op_or,
    [IOREQ_TYPE_XOR] = op_xor,
    [IOREQ_TYPE_ADD] = op_add,
    [IOREQ_TYPE_SUB] = op_sub
};

static int mmio_op(struct hvm_hw_stdvga *s, ioreq_t *p)
{
    uint32_t orig, mod = 0;
    orig = stdvga_mem_read(p->addr, p->size);

    if ( p->dir == IOREQ_WRITE )
    {
        mod = (op_array[p->type])(orig, p->data);
        stdvga_mem_write(p->addr, mod, p->size);
    }

    return 0; /* Don't try to buffer these operations */
}

int stdvga_intercept_mmio(ioreq_t *p)
{
    struct domain *d = current->domain;
    struct hvm_hw_stdvga *s = &d->arch.hvm_domain.stdvga;
    int buf = 0, rc;

    if ( p->size > 8 )
    {
        gdprintk(XENLOG_WARNING, "invalid mmio size %d\n", (int)p->size);
        return 0;
    }

    spin_lock(&s->lock);

    if ( s->stdvga && s->cache )
    {
        switch ( p->type )
        {
        case IOREQ_TYPE_COPY:
            buf = mmio_move(s, p);
            break;
        case IOREQ_TYPE_AND:
        case IOREQ_TYPE_OR:
        case IOREQ_TYPE_XOR:
        case IOREQ_TYPE_ADD:
        case IOREQ_TYPE_SUB:
            buf = mmio_op(s, p);
            break;
        default:
            gdprintk(XENLOG_WARNING, "unsupported mmio request type:%d "
                     "addr:0x%04x data:0x%04x size:%d count:%d state:%d "
                     "isptr:%d dir:%d df:%d\n",
                     p->type, (int)p->addr, (int)p->data, (int)p->size,
                     (int)p->count, p->state,
                     p->data_is_ptr, p->dir, p->df);
            s->cache = 0;
        }
    }
    else
    {
        buf = (p->dir == IOREQ_WRITE);
    }

    rc = (buf && hvm_buffered_io_send(p));

    spin_unlock(&s->lock);

    return rc;
}

void stdvga_init(struct domain *d)
{
    struct hvm_hw_stdvga *s = &d->arch.hvm_domain.stdvga;
    struct page_info *pg;
    void *p;
    int i;

    memset(s, 0, sizeof(*s));
    spin_lock_init(&s->lock);
    
    for ( i = 0; i != ARRAY_SIZE(s->vram_page); i++ )
    {
        if ( (pg = alloc_domheap_page(NULL)) == NULL )
            break;
        s->vram_page[i] = pg;
        p = map_domain_page(page_to_mfn(pg));
        clear_page(p);
        unmap_domain_page(p);
    }

    if ( i == ARRAY_SIZE(s->vram_page) )
    {
        /* Sequencer registers. */
        register_portio_handler(d, 0x3c4, 2, stdvga_intercept_pio);
        /* Graphics registers. */
        register_portio_handler(d, 0x3ce, 2, stdvga_intercept_pio);
        /* MMIO. */
        register_buffered_io_handler(
            d, 0xa0000, 0x20000, stdvga_intercept_mmio);
    }
}

void stdvga_deinit(struct domain *d)
{
    struct hvm_hw_stdvga *s = &d->arch.hvm_domain.stdvga;
    int i;

    for ( i = 0; i != ARRAY_SIZE(s->vram_page); i++ )
    {
        if ( s->vram_page[i] == NULL )
            continue;
        free_domheap_page(s->vram_page[i]);
        s->vram_page[i] = NULL;
    }
}