ia64/xen-unstable

changeset 4442:19f7234049f8

bitkeeper revision 1.1236.1.198 (424e4a89leX7XO7bU7Dkch4KS5zLhQ)

[PATCH] [PATCH] VMX support for MMIO/PIO in VM8086 mode

Memory mapped and port I/O is currently broken under VMX when the
partition is running in VM8086 mode. The reason is that the instruction
decoding support uses 32-bit opcode/address decodes rather 16-bit
decodes. This patch fixes that. In addition, the patch adds support for
the "stos" instruction decoding because this is a frequently used way
to clear MMIO areas such as the screen.

As an aside, vmx_platform.c should really reuse x86_emulate.c as much
as possible.

Signed-off-by: Leendert van Doorn <leendert@watson.ibm.com>

===== tools/ioemu/iodev/cpu.cc 1.7 vs edited =====
author leendert@watson.ibm.com[kaf24]
date Sat Apr 02 07:32:25 2005 +0000 (2005-04-02)
parents 4cc1f53d83e8
children e7f3366cfafd
files tools/ioemu/iodev/cpu.cc xen/arch/x86/vmx.c xen/arch/x86/vmx_platform.c
line diff
     1.1 --- a/tools/ioemu/iodev/cpu.cc	Sat Apr 02 07:32:19 2005 +0000
     1.2 +++ b/tools/ioemu/iodev/cpu.cc	Sat Apr 02 07:32:25 2005 +0000
     1.3 @@ -51,7 +51,7 @@ ioreq_t* bx_cpu_c::__get_ioreq(void)
     1.4  	if (req->state == STATE_IOREQ_READY) {
     1.5  		req->state = STATE_IOREQ_INPROCESS;
     1.6  	} else {
     1.7 -		BX_INFO(("False I/O requrest ... in-service already: %lx, pvalid: %lx,port: %lx, data: %lx, count: %lx, size: %lx\n", req->state, req->pdata_valid, req->addr, req->u.data, req->count, req->size));
     1.8 +		BX_INFO(("False I/O request ... in-service already: %lx, pvalid: %lx,port: %lx, data: %lx, count: %lx, size: %lx\n", req->state, req->pdata_valid, req->addr, req->u.data, req->count, req->size));
     1.9  		req = NULL;
    1.10  	}
    1.11  
    1.12 @@ -95,6 +95,8 @@ void bx_cpu_c::dispatch_ioreq(ioreq_t *r
    1.13  	}
    1.14  	if (req->port_mm == 0){//port io
    1.15  		if(req->dir == IOREQ_READ){//read
    1.16 +			//BX_INFO(("pio: <READ>addr:%llx, value:%llx, size: %llx, count: %llx\n", req->addr, req->u.data, req->size, req->count));
    1.17 +
    1.18  			if (!req->pdata_valid)
    1.19  				req->u.data = BX_INP(req->addr, req->size);
    1.20  			else {
    1.21 @@ -107,6 +109,8 @@ void bx_cpu_c::dispatch_ioreq(ioreq_t *r
    1.22  				}
    1.23  			}
    1.24  		} else if(req->dir == IOREQ_WRITE) {
    1.25 +			//BX_INFO(("pio: <WRITE>addr:%llx, value:%llx, size: %llx, count: %llx\n", req->addr, req->u.data, req->size, req->count));
    1.26 +
    1.27  			if (!req->pdata_valid) {
    1.28  				BX_OUTP(req->addr, (dma_addr_t) req->u.data, req->size);
    1.29  			} else {
    1.30 @@ -123,20 +127,29 @@ void bx_cpu_c::dispatch_ioreq(ioreq_t *r
    1.31  	} else if (req->port_mm == 1){//memory map io
    1.32  		if (!req->pdata_valid) {
    1.33  			if(req->dir == IOREQ_READ){//read
    1.34 -				BX_MEM_READ_PHYSICAL(req->addr, req->size, &req->u.data);
    1.35 -			} else if(req->dir == IOREQ_WRITE)//write
    1.36 -				BX_MEM_WRITE_PHYSICAL(req->addr, req->size, &req->u.data);
    1.37 +				//BX_INFO(("mmio[value]: <READ> addr:%llx, value:%llx, size: %llx, count: %llx\n", req->addr, req->u.data, req->size, req->count));
    1.38 +
    1.39 +				for (i = 0; i < req->count; i++) {
    1.40 +					BX_MEM_READ_PHYSICAL(req->addr, req->size, &req->u.data);
    1.41 +				}
    1.42 +			} else if(req->dir == IOREQ_WRITE) {//write
    1.43 +				//BX_INFO(("mmio[value]: <WRITE> addr:%llx, value:%llx, size: %llx, count: %llx\n", req->addr, req->u.data, req->size, req->count));
    1.44 +
    1.45 +				for (i = 0; i < req->count; i++) {
    1.46 +					BX_MEM_WRITE_PHYSICAL(req->addr, req->size, &req->u.data);
    1.47 +				}
    1.48 +			}
    1.49  		} else {
    1.50  			//handle movs
    1.51  			unsigned long tmp;
    1.52  			if (req->dir == IOREQ_READ) {
    1.53 -				//BX_INFO(("<READ>addr:%llx, pdata:%llx, size: %x, count: %x\n", req->addr, req->u.pdata, req->size, req->count));
    1.54 +				//BX_INFO(("mmio[pdata]: <READ>addr:%llx, pdata:%llx, size: %x, count: %x\n", req->addr, req->u.pdata, req->size, req->count));
    1.55  				for (i = 0; i < req->count; i++) {
    1.56  					BX_MEM_READ_PHYSICAL(req->addr + (sign * i * req->size), req->size, &tmp);
    1.57  					BX_MEM_WRITE_PHYSICAL((dma_addr_t) req->u.pdata + (sign * i * req->size), req->size, &tmp);
    1.58  				}
    1.59  			} else if (req->dir == IOREQ_WRITE) {
    1.60 -				//BX_INFO(("<WRITE>addr:%llx, pdata:%llx, size: %x, count: %x\n", req->addr, req->u.pdata, req->size, req->count));
    1.61 +				//BX_INFO(("mmio[pdata]: <WRITE>addr:%llx, pdata:%llx, size: %x, count: %x\n", req->addr, req->u.pdata, req->size, req->count));
    1.62  				for (i = 0; i < req->count; i++) {
    1.63  					BX_MEM_READ_PHYSICAL((dma_addr_t)req->u.pdata + (sign * i * req->size), req->size, &tmp);
    1.64  					BX_MEM_WRITE_PHYSICAL(req->addr + (sign * i * req->size), req->size, &tmp);
     2.1 --- a/xen/arch/x86/vmx.c	Sat Apr 02 07:32:19 2005 +0000
     2.2 +++ b/xen/arch/x86/vmx.c	Sat Apr 02 07:32:25 2005 +0000
     2.3 @@ -294,13 +294,17 @@ static void vmx_io_instruction(struct xe
     2.4      vcpu_iodata_t *vio;
     2.5      ioreq_t *p;
     2.6      unsigned long addr;
     2.7 -    unsigned long eip;
     2.8 +    unsigned long eip, cs, eflags;
     2.9 +    int vm86;
    2.10  
    2.11      __vmread(GUEST_EIP, &eip);
    2.12 +    __vmread(GUEST_CS_SELECTOR, &cs);
    2.13 +    __vmread(GUEST_EFLAGS, &eflags);
    2.14 +    vm86 = eflags & X86_EFLAGS_VM ? 1 : 0;
    2.15  
    2.16      VMX_DBG_LOG(DBG_LEVEL_1, 
    2.17 -            "vmx_io_instruction: eip=%p, exit_qualification = %lx",
    2.18 -            eip, exit_qualification);
    2.19 +            "vmx_io_instruction: vm86 %d, eip=%p:%p, exit_qualification = %lx",
    2.20 +            vm86, cs, eip, exit_qualification);
    2.21  
    2.22      if (test_bit(6, &exit_qualification))
    2.23          addr = (exit_qualification >> 16) & (0xffff);
    2.24 @@ -325,17 +329,29 @@ static void vmx_io_instruction(struct xe
    2.25      p->size = (exit_qualification & 7) + 1;
    2.26  
    2.27      if (test_bit(4, &exit_qualification)) {
    2.28 -        unsigned long eflags;
    2.29 -
    2.30 -        __vmread(GUEST_EFLAGS, &eflags);
    2.31          p->df = (eflags & X86_EFLAGS_DF) ? 1 : 0;
    2.32          p->pdata_valid = 1;
    2.33 -        p->u.pdata = (void *) ((p->dir == IOREQ_WRITE) ?
    2.34 -            regs->esi
    2.35 -            : regs->edi);
    2.36 +
    2.37 +        if (vm86) {
    2.38 +            unsigned long seg;
    2.39 +            if (p->dir == IOREQ_WRITE) {
    2.40 +                __vmread(GUEST_DS_SELECTOR, &seg);
    2.41 +                p->u.pdata = (void *)
    2.42 +                        ((seg << 4) | (regs->esi & 0xFFFF));
    2.43 +            } else {
    2.44 +                __vmread(GUEST_ES_SELECTOR, &seg);
    2.45 +                p->u.pdata = (void *)
    2.46 +                        ((seg << 4) | (regs->edi & 0xFFFF));
    2.47 +            }
    2.48 +        } else {
    2.49 +               p->u.pdata = (void *) ((p->dir == IOREQ_WRITE) ?
    2.50 +                   regs->esi : regs->edi);
    2.51 +        }
    2.52          p->u.pdata = (void *) gva_to_gpa(p->u.data);
    2.53 +
    2.54 +
    2.55          if (test_bit(5, &exit_qualification))
    2.56 -            p->count = regs->ecx;
    2.57 +	    p->count = vm86 ? regs->ecx & 0xFFFF : regs->ecx;
    2.58          if ((p->u.data & PAGE_MASK) != 
    2.59              ((p->u.data + p->count * p->size - 1) & PAGE_MASK)) {
    2.60              printk("stringio crosses page boundary!\n");
    2.61 @@ -368,13 +384,20 @@ static void vmx_io_instruction(struct xe
    2.62      do_block();
    2.63  }
    2.64  
    2.65 +static int
    2.66 +vm86assist(struct exec_domain *d)
    2.67 +{
    2.68 +    /* stay tuned ... */
    2.69 +    return 0;
    2.70 +}
    2.71 +
    2.72  #define CASE_GET_REG(REG, reg)  \
    2.73      case REG_ ## REG: value = regs->reg; break
    2.74  
    2.75  /*
    2.76   * Write to control registers
    2.77   */
    2.78 -static void mov_to_cr(int gp, int cr, struct xen_regs *regs)
    2.79 +static int mov_to_cr(int gp, int cr, struct xen_regs *regs)
    2.80  {
    2.81      unsigned long value;
    2.82      unsigned long old_cr;
    2.83 @@ -454,8 +477,21 @@ static void mov_to_cr(int gp, int cr, st
    2.84                      d->arch.arch_vmx.cpu_cr3, mfn);
    2.85              /* undo the get_page done in the para virt case */
    2.86              put_page_and_type(&frame_table[old_base_mfn]);
    2.87 +        } else {
    2.88 +            if ((value & X86_CR0_PE) == 0) {
    2.89 +		unsigned long eip;
    2.90  
    2.91 -        }
    2.92 +	        __vmread(GUEST_EIP, &eip);
    2.93 +                VMX_DBG_LOG(DBG_LEVEL_1,
    2.94 +			"Disabling CR0.PE at %%eip 0x%lx", eip);
    2.95 +		if (vm86assist(d)) {
    2.96 +	            __vmread(GUEST_EIP, &eip);
    2.97 +		    VMX_DBG_LOG(DBG_LEVEL_1,
    2.98 +			"Transfering control to vm86assist %%eip 0x%lx", eip);
    2.99 +		    return 0; /* do not update eip! */
   2.100 +		}
   2.101 +	    }
   2.102 +	}
   2.103          break;
   2.104      }
   2.105      case 3: 
   2.106 @@ -534,7 +570,9 @@ static void mov_to_cr(int gp, int cr, st
   2.107          printk("invalid cr: %d\n", gp);
   2.108          __vmx_bug(regs);
   2.109      }
   2.110 -}   
   2.111 +
   2.112 +    return 1;
   2.113 +}
   2.114  
   2.115  #define CASE_SET_REG(REG, reg)      \
   2.116      case REG_ ## REG:       \
   2.117 @@ -575,7 +613,7 @@ static void mov_from_cr(int cr, int gp, 
   2.118      VMX_DBG_LOG(DBG_LEVEL_VMMU, "mov_from_cr: CR%d, value = %lx,", cr, value);
   2.119  }
   2.120  
   2.121 -static void vmx_cr_access (unsigned long exit_qualification, struct xen_regs *regs)
   2.122 +static int vmx_cr_access(unsigned long exit_qualification, struct xen_regs *regs)
   2.123  {
   2.124      unsigned int gp, cr;
   2.125      unsigned long value;
   2.126 @@ -584,8 +622,7 @@ static void vmx_cr_access (unsigned long
   2.127      case TYPE_MOV_TO_CR:
   2.128          gp = exit_qualification & CONTROL_REG_ACCESS_REG;
   2.129          cr = exit_qualification & CONTROL_REG_ACCESS_NUM;
   2.130 -        mov_to_cr(gp, cr, regs);
   2.131 -        break;
   2.132 +        return mov_to_cr(gp, cr, regs);
   2.133      case TYPE_MOV_FROM_CR:
   2.134          gp = exit_qualification & CONTROL_REG_ACCESS_REG;
   2.135          cr = exit_qualification & CONTROL_REG_ACCESS_NUM;
   2.136 @@ -604,6 +641,7 @@ static void vmx_cr_access (unsigned long
   2.137          __vmx_bug(regs);
   2.138          break;
   2.139      }
   2.140 +    return 1;
   2.141  }
   2.142  
   2.143  static inline void vmx_do_msr_read(struct xen_regs *regs)
   2.144 @@ -619,7 +657,7 @@ static inline void vmx_do_msr_read(struc
   2.145  }
   2.146  
   2.147  /*
   2.148 - * Need to use this exit to rescheule
   2.149 + * Need to use this exit to reschedule
   2.150   */
   2.151  static inline void vmx_vmexit_do_hlt(void)
   2.152  {
   2.153 @@ -891,8 +929,8 @@ asmlinkage void vmx_vmexit_handler(struc
   2.154  
   2.155          VMX_DBG_LOG(DBG_LEVEL_1, "eip = %lx, inst_len =%lx, exit_qualification = %lx", 
   2.156                  eip, inst_len, exit_qualification);
   2.157 -        vmx_cr_access(exit_qualification, &regs);
   2.158 -        __update_guest_eip(inst_len);
   2.159 +        if (vmx_cr_access(exit_qualification, &regs))
   2.160 +	    __update_guest_eip(inst_len);
   2.161          break;
   2.162      }
   2.163      case EXIT_REASON_DR_ACCESS:
     3.1 --- a/xen/arch/x86/vmx_platform.c	Sat Apr 02 07:32:19 2005 +0000
     3.2 +++ b/xen/arch/x86/vmx_platform.c	Sat Apr 02 07:32:25 2005 +0000
     3.3 @@ -55,6 +55,8 @@ static void store_xen_regs(struct xen_re
     3.4      __vmread(GUEST_ESP, &regs->esp);
     3.5      __vmread(GUEST_EFLAGS, &regs->eflags);
     3.6      __vmread(GUEST_CS_SELECTOR, &regs->cs);
     3.7 +    __vmread(GUEST_DS_SELECTOR, &regs->ds);
     3.8 +    __vmread(GUEST_ES_SELECTOR, &regs->es);
     3.9      __vmread(GUEST_EIP, &regs->eip);
    3.10  }
    3.11  
    3.12 @@ -144,19 +146,27 @@ static inline unsigned char *check_prefi
    3.13      while (1) {
    3.14          switch (*inst) {
    3.15              case 0xf3: //REPZ
    3.16 +	    	thread_inst->flags = REPZ;
    3.17 +		break;
    3.18              case 0xf2: //REPNZ
    3.19 +	    	thread_inst->flags = REPNZ;
    3.20 +		break;
    3.21              case 0xf0: //LOCK
    3.22 +	    	break;
    3.23              case 0x2e: //CS
    3.24              case 0x36: //SS
    3.25              case 0x3e: //DS
    3.26              case 0x26: //ES
    3.27              case 0x64: //FS
    3.28              case 0x65: //GS
    3.29 +		thread_inst->seg_sel = *inst;
    3.30                  break;
    3.31              case 0x66: //32bit->16bit
    3.32                  thread_inst->op_size = WORD;
    3.33                  break;
    3.34              case 0x67:
    3.35 +		printf("Not handling 0x67 (yet)\n");
    3.36 +		domain_crash_synchronous(); 
    3.37                  break;
    3.38              default:
    3.39                  return inst;
    3.40 @@ -165,7 +175,7 @@ static inline unsigned char *check_prefi
    3.41      }
    3.42  }
    3.43  
    3.44 -static inline unsigned long get_immediate(const unsigned char *inst, int op_size)
    3.45 +static inline unsigned long get_immediate(int op16, const unsigned char *inst, int op_size)
    3.46  {
    3.47      int mod, reg, rm;
    3.48      unsigned long val = 0;
    3.49 @@ -183,14 +193,21 @@ static inline unsigned long get_immediat
    3.50      switch(mod) {
    3.51          case 0:
    3.52              if (rm == 5) {
    3.53 -                inst = inst + 4; //disp32, skip 4 bytes
    3.54 +		if (op16)
    3.55 +                    inst = inst + 2; //disp16, skip 2 bytes
    3.56 +		else
    3.57 +                    inst = inst + 4; //disp32, skip 4 bytes
    3.58              }
    3.59              break;
    3.60          case 1:
    3.61              inst++; //disp8, skip 1 byte
    3.62              break;
    3.63          case 2:
    3.64 -            inst = inst + 4; //disp32, skip 4 bytes
    3.65 +	    if (op16)
    3.66 +                inst = inst + 2; //disp16, skip 2 bytes
    3.67 +	    else
    3.68 +                inst = inst + 4; //disp32, skip 4 bytes
    3.69 +            break;
    3.70      }
    3.71      for (i = 0; i < op_size; i++) {
    3.72          val |= (*inst++ & 0xff) << (8 * i);
    3.73 @@ -218,7 +235,21 @@ static inline int get_index(const unsign
    3.74  
    3.75  static int vmx_decode(const unsigned char *inst, struct instruction *thread_inst)
    3.76  {
    3.77 -    int index;
    3.78 +    unsigned long eflags;
    3.79 +    int index, vm86 = 0;
    3.80 +
    3.81 +    __vmread(GUEST_EFLAGS, &eflags);
    3.82 +    if (eflags & X86_EFLAGS_VM)
    3.83 +	vm86 = 1;
    3.84 +
    3.85 +    if (vm86) { /* meaning is reversed */
    3.86 +       if (thread_inst->op_size == WORD)
    3.87 +           thread_inst->op_size = LONG;
    3.88 +       else if (thread_inst->op_size == LONG)
    3.89 +           thread_inst->op_size = WORD;
    3.90 +       else if (thread_inst->op_size == 0)
    3.91 +           thread_inst->op_size = WORD;
    3.92 +    }
    3.93  
    3.94      switch(*inst) {
    3.95          case 0x88:
    3.96 @@ -258,7 +289,6 @@ static int vmx_decode(const unsigned cha
    3.97              printk("%x, This opcode hasn't been handled yet!", *inst);
    3.98              return DECODE_failure;
    3.99              /* Not handle it yet. */
   3.100 -
   3.101          case 0xa0:
   3.102              /* mov byte to al */
   3.103              thread_inst->op_size = BYTE;
   3.104 @@ -291,7 +321,6 @@ static int vmx_decode(const unsigned cha
   3.105              /* movsb */
   3.106              thread_inst->op_size = BYTE;
   3.107              strcpy((char *)thread_inst->i_name, "movs");
   3.108 -            
   3.109              return DECODE_success;
   3.110          case 0xa5:
   3.111              /* movsw/movsl */
   3.112 @@ -299,16 +328,28 @@ static int vmx_decode(const unsigned cha
   3.113              } else {
   3.114                  thread_inst->op_size = LONG;
   3.115              }
   3.116 -            
   3.117              strcpy((char *)thread_inst->i_name, "movs");
   3.118 -            
   3.119 +            return DECODE_success;
   3.120 +        case 0xaa:
   3.121 +            /* stosb */
   3.122 +            thread_inst->op_size = BYTE;
   3.123 +            strcpy((char *)thread_inst->i_name, "stosb");
   3.124              return DECODE_success;
   3.125 -
   3.126 +       case 0xab:
   3.127 +            /* stosw/stosl */
   3.128 +            if (thread_inst->op_size == WORD) {
   3.129 +                strcpy((char *)thread_inst->i_name, "stosw");
   3.130 +            } else {
   3.131 +                thread_inst->op_size = LONG;
   3.132 +                strcpy((char *)thread_inst->i_name, "stosl");
   3.133 +            }
   3.134 +            return DECODE_success;
   3.135          case 0xc6:
   3.136              /* mov imm8 to m8 */
   3.137              thread_inst->op_size = BYTE;
   3.138              thread_inst->operand[0] = mk_operand(BYTE, 0, 0, IMMEDIATE);
   3.139 -            thread_inst->immediate = get_immediate((inst+1), thread_inst->op_size);
   3.140 +            thread_inst->immediate = get_immediate(vm86,
   3.141 +					(inst+1), thread_inst->op_size);
   3.142              break;
   3.143          case 0xc7:
   3.144              /* mov imm16/32 to m16/32 */
   3.145 @@ -318,9 +359,9 @@ static int vmx_decode(const unsigned cha
   3.146                  thread_inst->op_size = LONG;
   3.147                  thread_inst->operand[0] = mk_operand(LONG, 0, 0, IMMEDIATE);
   3.148              }
   3.149 -            thread_inst->immediate = get_immediate((inst+1), thread_inst->op_size);
   3.150 +            thread_inst->immediate = get_immediate(vm86,
   3.151 +					(inst+1), thread_inst->op_size);
   3.152              break;
   3.153 -
   3.154          case 0x0f:
   3.155              break;
   3.156          default:
   3.157 @@ -425,6 +466,7 @@ static void send_mmio_req(unsigned long 
   3.158      struct exec_domain *d = current;
   3.159      vcpu_iodata_t *vio;
   3.160      ioreq_t *p;
   3.161 +    int vm86;
   3.162      struct mi_per_cpu_info *mpci_p;
   3.163      struct xen_regs *inst_decoder_regs;
   3.164      extern long evtchn_send(int lport);
   3.165 @@ -432,53 +474,59 @@ static void send_mmio_req(unsigned long 
   3.166  
   3.167      mpci_p = &current->arch.arch_vmx.vmx_platform.mpci;
   3.168      inst_decoder_regs = mpci_p->inst_decoder_regs;
   3.169 +
   3.170      vio = (vcpu_iodata_t *) d->arch.arch_vmx.vmx_platform.shared_page_va;
   3.171 -        
   3.172      if (vio == NULL) {
   3.173          printk("bad shared page\n");
   3.174          domain_crash_synchronous(); 
   3.175      }
   3.176      p = &vio->vp_ioreq;
   3.177 -        
   3.178 +
   3.179 +    vm86 = inst_decoder_regs->eflags & X86_EFLAGS_VM;
   3.180 +
   3.181      set_bit(ARCH_VMX_IO_WAIT, &d->arch.arch_vmx.flags);
   3.182      p->dir = dir;
   3.183      p->pdata_valid = pvalid;
   3.184 -    p->count = 1;
   3.185  
   3.186      p->port_mm = 1;
   3.187      p->size = inst_p->op_size;
   3.188      p->addr = gpa;
   3.189      p->u.data = value;
   3.190  
   3.191 -    // p->state = STATE_UPSTREAM_SENDING;
   3.192      p->state = STATE_IOREQ_READY;
   3.193  
   3.194 -    // Try to use ins/outs' framework
   3.195 -    if (pvalid) {
   3.196 -        // Handle "movs"
   3.197 -        p->u.pdata = (void *) ((p->dir == IOREQ_WRITE) ?
   3.198 -                               inst_decoder_regs->esi
   3.199 -                               : inst_decoder_regs->edi); 
   3.200 +    if (inst_p->flags & REPZ) {
   3.201 +        if (vm86)
   3.202 +            p->count = inst_decoder_regs->ecx & 0xFFFF;
   3.203 +        else
   3.204 +            p->count = inst_decoder_regs->ecx;
   3.205 +        p->df = (inst_decoder_regs->eflags & EF_DF) ? 1 : 0;
   3.206 +    } else
   3.207 +        p->count = 1;
   3.208 +
   3.209 +    if (pvalid)
   3.210          p->u.pdata = (void *) gva_to_gpa(p->u.data);
   3.211 -        p->count = inst_decoder_regs->ecx;
   3.212 -        inst_decoder_regs->ecx = 0;
   3.213 -        p->df = (inst_decoder_regs->eflags & EF_DF) ? 1 : 0;
   3.214 -    }
   3.215 +
   3.216 +#if 0
   3.217 +    printf("send_mmio_req: eip 0x%lx:0x%lx, dir %d, pdata_valid %d, ",
   3.218 +	inst_decoder_regs->cs, inst_decoder_regs->eip, p->dir, p->pdata_valid);
   3.219 +    printf("port_mm %d, size %lld, addr 0x%llx, value 0x%lx, count %lld\n",
   3.220 +	p->port_mm, p->size, p->addr, value, p->count);
   3.221 +#endif
   3.222  
   3.223      evtchn_send(IOPACKET_PORT);
   3.224      do_block(); 
   3.225 -
   3.226  }
   3.227  
   3.228  void handle_mmio(unsigned long va, unsigned long gpa)
   3.229  {
   3.230 -    unsigned long eip;
   3.231 -    unsigned long inst_len;
   3.232 +    unsigned long eip, eflags, cs;
   3.233 +    unsigned long inst_len, inst_addr;
   3.234      struct mi_per_cpu_info *mpci_p;
   3.235      struct xen_regs *inst_decoder_regs;
   3.236      struct instruction mmio_inst;
   3.237      unsigned char inst[MAX_INST_LEN];
   3.238 -    int ret;
   3.239 +    int vm86, ret;
   3.240       
   3.241      mpci_p = &current->arch.arch_vmx.vmx_platform.mpci;
   3.242      inst_decoder_regs = mpci_p->inst_decoder_regs;
   3.243 @@ -486,13 +534,30 @@ void handle_mmio(unsigned long va, unsig
   3.244      __vmread(GUEST_EIP, &eip);
   3.245      __vmread(INSTRUCTION_LEN, &inst_len);
   3.246  
   3.247 +    __vmread(GUEST_EFLAGS, &eflags);
   3.248 +    vm86 = eflags & X86_EFLAGS_VM;
   3.249 +
   3.250 +    if (vm86) {
   3.251 +        __vmread(GUEST_CS_SELECTOR, &cs);
   3.252 +        inst_addr = (cs << 4) | eip;
   3.253 +    } else
   3.254 +        inst_addr = eip; /* XXX should really look at GDT[cs].base too */
   3.255 +
   3.256      memset(inst, '0', MAX_INST_LEN);
   3.257 -    ret = inst_copy_from_guest(inst, eip, inst_len);
   3.258 +    ret = inst_copy_from_guest(inst, inst_addr, inst_len);
   3.259      if (ret != inst_len) {
   3.260          printk("handle_mmio - EXIT: get guest instruction fault\n");
   3.261          domain_crash_synchronous();
   3.262      }
   3.263  
   3.264 +#if 0
   3.265 +    printk("handle_mmio: cs:eip 0x%lx:0x%lx(0x%lx): opcode",
   3.266 +        cs, eip, inst_addr, inst_len);
   3.267 +    for (ret = 0; ret < inst_len; ret++)
   3.268 +        printk(" %02x", inst[ret]);
   3.269 +    printk("\n");
   3.270 +#endif
   3.271 +
   3.272      init_instruction(&mmio_inst);
   3.273      
   3.274      if (vmx_decode(check_prefix(inst, &mmio_inst), &mmio_inst) == DECODE_failure)
   3.275 @@ -506,7 +571,7 @@ void handle_mmio(unsigned long va, unsig
   3.276          if (read_from_mmio(&mmio_inst)) {
   3.277              // Send the request and waiting for return value.
   3.278              mpci_p->mmio_target = mmio_inst.operand[1] | WZEROEXTEND;
   3.279 -            send_mmio_req(gpa, &mmio_inst, 0, 1, 0);
   3.280 +            send_mmio_req(gpa, &mmio_inst, 0, IOREQ_READ, 0);
   3.281              return ;
   3.282          } else {
   3.283              printk("handle_mmio - EXIT: movz error!\n");
   3.284 @@ -515,10 +580,32 @@ void handle_mmio(unsigned long va, unsig
   3.285      }
   3.286  
   3.287      if (!strncmp((char *)mmio_inst.i_name, "movs", 4)) {
   3.288 -        int tmp_dir;
   3.289 +	unsigned long addr = 0;
   3.290 +	int dir;
   3.291 +
   3.292 +	if (vm86) {
   3.293 +	    unsigned long seg;
   3.294  
   3.295 -        tmp_dir = ((va == inst_decoder_regs->edi) ? IOREQ_WRITE : IOREQ_READ);
   3.296 -        send_mmio_req(gpa, &mmio_inst, 0, tmp_dir, 1);
   3.297 +	    __vmread(GUEST_ES_SELECTOR, &seg);
   3.298 +	    if (((seg << 4) | (inst_decoder_regs->edi & 0xFFFF)) == va) {
   3.299 +		dir = IOREQ_WRITE;
   3.300 +		__vmread(GUEST_DS_SELECTOR, &seg);
   3.301 +		addr = (seg << 4) | (inst_decoder_regs->esi & 0xFFFF);
   3.302 +	    } else {
   3.303 +		dir = IOREQ_READ;
   3.304 +		addr = (seg << 4) | (inst_decoder_regs->edi & 0xFFFF);
   3.305 +	    }
   3.306 +	} else { /* XXX should really look at GDT[ds/es].base too */
   3.307 +	    if (va == inst_decoder_regs->edi) {
   3.308 +		dir = IOREQ_WRITE;
   3.309 +		addr = inst_decoder_regs->esi;
   3.310 +	    } else {
   3.311 +		dir = IOREQ_READ;
   3.312 +		addr = inst_decoder_regs->edi;
   3.313 +	    }
   3.314 +	}
   3.315 +
   3.316 +	send_mmio_req(gpa, &mmio_inst, addr, dir, 1);
   3.317          return;
   3.318      }
   3.319  
   3.320 @@ -529,7 +616,7 @@ void handle_mmio(unsigned long va, unsig
   3.321          if (read_from_mmio(&mmio_inst)) {
   3.322              // Send the request and waiting for return value.
   3.323              mpci_p->mmio_target = mmio_inst.operand[1];
   3.324 -            send_mmio_req(gpa, &mmio_inst, value, 1, 0);
   3.325 +            send_mmio_req(gpa, &mmio_inst, value, IOREQ_READ, 0);
   3.326          } else {
   3.327              // Write to MMIO
   3.328              if (mmio_inst.operand[0] & IMMEDIATE) {
   3.329 @@ -541,11 +628,16 @@ void handle_mmio(unsigned long va, unsig
   3.330              } else {
   3.331                  domain_crash_synchronous();
   3.332              }
   3.333 -            send_mmio_req(gpa, &mmio_inst, value, 0, 0);
   3.334 +            send_mmio_req(gpa, &mmio_inst, value, IOREQ_WRITE, 0);
   3.335              return;
   3.336          }
   3.337      }
   3.338  
   3.339 +    if (!strncmp((char *)mmio_inst.i_name, "stos", 4)) {
   3.340 +	send_mmio_req(gpa, &mmio_inst,
   3.341 +		inst_decoder_regs->eax, IOREQ_WRITE, 0);
   3.342 +    }
   3.343 +
   3.344      domain_crash_synchronous();
   3.345  }
   3.346