stdvga.c

来自「xen虚拟机源代码安装包」· C语言 代码 · 共 634 行 · 第 1/2 页

static void stdvga_mem_writeb(uint64_t addr, uint32_t val)
{
    struct hvm_hw_stdvga *s = &current->domain->arch.hvm_domain.stdvga;
    int plane, write_mode, b, func_select, mask;
    uint32_t write_mask, bit_mask, set_mask, *vram_l;
    uint8_t *vram_b;

    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;
    p2m_type_t p2mt;

    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);
                if ( hvm_copy_to_guest_phys(data, &tmp, p->size) ==
                     HVMCOPY_bad_gfn_to_mfn )
                {
                    (void)gfn_to_mfn_current(data >> PAGE_SHIFT, &p2mt);
                    /*
                     * The only case we handle is vga_mem <-> vga_mem.
                     * Anything else disables caching and leaves it to qemu-dm.
                     */
                    if ( (p2mt != p2m_mmio_dm) || (data < VGA_MEM_BASE) ||
                         ((data + p->size) > (VGA_MEM_BASE + VGA_MEM_SIZE)) )
                        return 0;
                    stdvga_mem_write(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++ )
            {
                if ( hvm_copy_from_guest_phys(&tmp, data, p->size) ==
                     HVMCOPY_bad_gfn_to_mfn )
                {
                    (void)gfn_to_mfn_current(data >> PAGE_SHIFT, &p2mt);
                    if ( (p2mt != p2m_mmio_dm) || (data < VGA_MEM_BASE) ||
                         ((data + p->size) > (VGA_MEM_BASE + VGA_MEM_SIZE)) )
                        return 0;
                    tmp = stdvga_mem_read(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 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 X86EMUL_UNHANDLEABLE;
    }

    spin_lock(&s->lock);

    if ( s->stdvga && s->cache )
    {
        switch ( p->type )
        {
        case IOREQ_TYPE_COPY:
            buf = mmio_move(s, p);
            if ( buf )
                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 ? X86EMUL_OKAY : X86EMUL_UNHANDLEABLE;
}

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++ )
    {
        pg = alloc_domheap_page(NULL, MEMF_node(domain_to_node(d)));
        if ( pg == 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, VGA_MEM_BASE, VGA_MEM_SIZE, 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;
    }
}