1.1 --- a/tools/firmware/vmxassist/machine.h	Thu Oct 25 14:45:47 2007 +0100
     1.2 +++ b/tools/firmware/vmxassist/machine.h	Thu Oct 25 14:55:37 2007 +0100
     1.3 @@ -38,10 +38,15 @@
     1.4  #define CR4_PSE		(1 << 4)
     1.5  #define CR4_PAE		(1 << 5)
     1.6  
     1.7 +#define EFLAGS_CF	(1 << 0)
     1.8 +#define EFLAGS_PF	(1 << 2)
     1.9 +#define EFLAGS_AF	(1 << 4)
    1.10  #define EFLAGS_ZF	(1 << 6)
    1.11 +#define EFLAGS_SF	(1 << 7)
    1.12  #define EFLAGS_TF	(1 << 8)
    1.13  #define EFLAGS_IF	(1 << 9)
    1.14  #define EFLAGS_DF	(1 << 10)
    1.15 +#define EFLAGS_OF	(1 << 11)
    1.16  #define EFLAGS_IOPL	(3 << 12)
    1.17  #define EFLAGS_VM	((1 << 17) | EFLAGS_IOPL)
    1.18  #define EFLAGS_VIF	(1 << 19)

     2.1 --- a/tools/firmware/vmxassist/vm86.c	Thu Oct 25 14:45:47 2007 +0100
     2.2 +++ b/tools/firmware/vmxassist/vm86.c	Thu Oct 25 14:55:37 2007 +0100
     2.3 @@ -33,6 +33,7 @@
     2.4  #define	SEG_SS		0x0020
     2.5  #define	SEG_FS		0x0040
     2.6  #define	SEG_GS		0x0080
     2.7 +#define REP		0x0100
     2.8  
     2.9  static unsigned prev_eip = 0;
    2.10  enum vm86_mode mode = 0;
    2.11 @@ -656,6 +657,108 @@ movr(struct regs *regs, unsigned prefix,
    2.12  }
    2.13  
    2.14  /*
    2.15 + * We need to handle string moves that address memory beyond the 64KB segment
    2.16 + * limit that VM8086 mode enforces.
    2.17 + */
    2.18 +static inline int
    2.19 +movs(struct regs *regs, unsigned prefix, unsigned opc)
    2.20 +{
    2.21 +	unsigned eip = regs->eip - 1;
    2.22 +	unsigned sseg = segment(prefix, regs, regs->vds);
    2.23 +	unsigned dseg = regs->ves;
    2.24 +	unsigned saddr, daddr;
    2.25 +	unsigned count = 1;
    2.26 +	int incr = ((regs->eflags & EFLAGS_DF) == 0) ? 1 : -1;
    2.27 +
    2.28 +	saddr = address(regs, sseg, regs->esi);
    2.29 +	daddr = address(regs, dseg, regs->edi);
    2.30 +
    2.31 +	if ((prefix & REP) != 0) {
    2.32 +		count = regs->ecx;
    2.33 +		regs->ecx = 0;
    2.34 +	}
    2.35 +
    2.36 +	switch (opc) {
    2.37 +	case 0xA4: /* movsb */
    2.38 +		regs->esi += (incr * count);
    2.39 +		regs->edi += (incr * count);
    2.40 +
    2.41 +		while (count-- != 0) {
    2.42 +			write8(daddr, read8(saddr));
    2.43 +			daddr += incr;
    2.44 +			saddr += incr;
    2.45 +		}
    2.46 +		TRACE((regs, regs->eip - eip, "movsb (%%esi),%%es:(%%edi)"));
    2.47 +		break;
    2.48 +
    2.49 +	case 0xA5: /* movsw */
    2.50 +		if ((prefix & DATA32) == 0) {
    2.51 +			incr = 2 * incr;
    2.52 +			regs->esi += (incr * count);
    2.53 +			regs->edi += (incr * count);
    2.54 +
    2.55 +			while (count-- != 0) {
    2.56 +				write16(daddr, read16(saddr));
    2.57 +				daddr += incr;
    2.58 +				saddr += incr;
    2.59 +			}
    2.60 +		} else {
    2.61 +			incr = 4 * incr;
    2.62 +			regs->esi += (incr * count);
    2.63 +			regs->edi += (incr * count);
    2.64 +
    2.65 +			while (count-- != 0) {
    2.66 +				write32(daddr, read32(saddr));
    2.67 +				daddr += incr;
    2.68 +				saddr += incr;
    2.69 +			}
    2.70 +		}			
    2.71 +		TRACE((regs, regs->eip - eip, "movsw %s(%%esi),%%es:(%%edi)"));
    2.72 +		break;
    2.73 +	}
    2.74 +
    2.75 +	return 1;
    2.76 +}
    2.77 +
    2.78 +static inline int
    2.79 +lods(struct regs *regs, unsigned prefix, unsigned opc)
    2.80 +{
    2.81 +	unsigned eip = regs->eip - 1;
    2.82 +	unsigned seg = segment(prefix, regs, regs->vds);
    2.83 +	unsigned addr = address(regs, seg, regs->esi);
    2.84 +	unsigned count = 1;
    2.85 +	int incr = ((regs->eflags & EFLAGS_DF) == 0) ? 1 : -1;
    2.86 +
    2.87 +	if ((prefix & REP) != 0) {
    2.88 +		count = regs->ecx;
    2.89 +		regs->ecx = 0;
    2.90 +	}
    2.91 +
    2.92 +	switch (opc) {
    2.93 +	case 0xAD: /* lodsw */
    2.94 +		if ((prefix & DATA32) == 0) {
    2.95 +			incr = 2 * incr;
    2.96 +			regs->esi += (incr * count);
    2.97 +			while (count-- != 0) {
    2.98 +				setreg16(regs, 0, read16(addr));
    2.99 +				addr += incr;
   2.100 +			}
   2.101 +
   2.102 +			TRACE((regs, regs->eip - eip, "lodsw (%%esi),%%ax"));
   2.103 +		} else {
   2.104 +			incr = 4 * incr;
   2.105 +			regs->esi += (incr * count);
   2.106 +			while (count-- != 0) {
   2.107 +				setreg32(regs, 0, read32(addr));
   2.108 +				addr += incr;
   2.109 +			}
   2.110 +			TRACE((regs, regs->eip - eip, "lodsw (%%esi),%%eax"));
   2.111 +		}
   2.112 +		break;
   2.113 +	}
   2.114 +	return 1;
   2.115 +}
   2.116 +/*
   2.117   * Move to and from a control register.
   2.118   */
   2.119  static int
   2.120 @@ -718,6 +821,55 @@ static inline void set_eflags_ZF(unsigne
   2.121  		regs->eflags &= ~EFLAGS_ZF;
   2.122  }
   2.123  
   2.124 +static void set_eflags_add(unsigned hi_bit_mask, unsigned v1, unsigned v2,
   2.125 +				unsigned result, struct regs *regs)
   2.126 +{
   2.127 +	int bit_count;
   2.128 +	unsigned tmp;
   2.129 +	unsigned full_mask;
   2.130 +	unsigned nonsign_mask;
   2.131 +
   2.132 +	/* Carry out of high order bit? */
   2.133 +	if ( v1 & v2 & hi_bit_mask )
   2.134 +		regs->eflags |= EFLAGS_CF;
   2.135 +	else
   2.136 +		regs->eflags &= ~EFLAGS_CF;
   2.137 +
   2.138 +	/* Even parity in least significant byte? */
   2.139 +	tmp = result & 0xff;
   2.140 +	for (bit_count = 0; tmp != 0; bit_count++)
   2.141 +		tmp &= (tmp - 1);
   2.142 +
   2.143 +	if (bit_count & 1)
   2.144 +		regs->eflags &= ~EFLAGS_PF;
   2.145 +	else
   2.146 +		regs->eflags |= EFLAGS_PF;
   2.147 +
   2.148 +	/* Carry out of least significant BCD digit? */
   2.149 +	if ( v1 & v2 & (1<<3) )
   2.150 +		regs->eflags |= EFLAGS_AF;
   2.151 +	else
   2.152 +		regs->eflags &= ~EFLAGS_AF;
   2.153 +
   2.154 +	/* Result is zero? */
   2.155 +	full_mask = (hi_bit_mask - 1) | hi_bit_mask;
   2.156 +	set_eflags_ZF(full_mask, result, regs);
   2.157 +
   2.158 +	/* Sign of result? */
   2.159 +	if ( result & hi_bit_mask )
   2.160 +		regs->eflags |= EFLAGS_SF;
   2.161 +	else
   2.162 +		regs->eflags &= ~EFLAGS_SF;
   2.163 +
   2.164 +	/* Carry out of highest non-sign bit? */
   2.165 +	nonsign_mask = (hi_bit_mask >> 1) & ~hi_bit_mask;
   2.166 +	if ( v1 & v2 & hi_bit_mask )
   2.167 +		regs->eflags |= EFLAGS_OF;
   2.168 +	else
   2.169 +		regs->eflags &= ~EFLAGS_OF;
   2.170 +
   2.171 +}
   2.172 +
   2.173  /*
   2.174   * We need to handle cmp opcodes that address memory beyond the 64KB
   2.175   * segment limit that VM8086 mode enforces.
   2.176 @@ -792,6 +944,82 @@ test(struct regs *regs, unsigned prefix,
   2.177  }
   2.178  
   2.179  /*
   2.180 + * We need to handle add opcodes that address memory beyond the 64KB
   2.181 + * segment limit that VM8086 mode enforces.
   2.182 + */
   2.183 +static int
   2.184 +add(struct regs *regs, unsigned prefix, unsigned opc)
   2.185 +{
   2.186 +	unsigned eip = regs->eip - 1;
   2.187 +	unsigned modrm = fetch8(regs);
   2.188 +	unsigned addr = operand(prefix, regs, modrm);
   2.189 +	unsigned r = (modrm >> 3) & 7;
   2.190 +
   2.191 +	unsigned val1 = 0;
   2.192 +	unsigned val2 = 0;
   2.193 +	unsigned result = 0;
   2.194 +	unsigned hi_bit;
   2.195 +
   2.196 +	if ((modrm & 0xC0) == 0xC0) /* no registers */
   2.197 +		return 0;
   2.198 +
   2.199 +	switch (opc) {
   2.200 +	case 0x00: /* addr32 add r8, r/m8 */
   2.201 +		val1 = getreg8(regs, r);
   2.202 +		val2 = read8(addr);
   2.203 +		result = val1 + val2;
   2.204 +		write8(addr, result);
   2.205 +		TRACE((regs, regs->eip - eip,
   2.206 +			"addb %%e%s, *0x%x", rnames[r], addr));
   2.207 +		break;
   2.208 +		
   2.209 +	case 0x01: /* addr32 add r16, r/m16 */
   2.210 +		if (prefix & DATA32) {
   2.211 +			val1 = getreg32(regs, r);
   2.212 +			val2 = read32(addr);
   2.213 +			result = val1 + val2;
   2.214 +			write32(addr, result);
   2.215 +			TRACE((regs, regs->eip - eip,
   2.216 +				"addl %%e%s, *0x%x", rnames[r], addr));
   2.217 +		} else {
   2.218 +			val1 = getreg16(regs, r);
   2.219 +			val2 = read16(addr);
   2.220 +			result = val1 + val2;
   2.221 +			write16(addr, result);
   2.222 +			TRACE((regs, regs->eip - eip,
   2.223 +				"addw %%e%s, *0x%x", rnames[r], addr));
   2.224 +		}
   2.225 +		break;
   2.226 +		
   2.227 +	case 0x03: /* addr32 add r/m16, r16 */
   2.228 +		if (prefix & DATA32) {
   2.229 +			val1 = getreg32(regs, r);
   2.230 +			val2 = read32(addr);
   2.231 +			result = val1 + val2;
   2.232 +			setreg32(regs, r, result);
   2.233 +			TRACE((regs, regs->eip - eip,
   2.234 +				"addl *0x%x, %%e%s", addr, rnames[r]));
   2.235 +		} else {
   2.236 +			val1 = getreg16(regs, r);
   2.237 +			val2 = read16(addr);
   2.238 +			result = val1 + val2;
   2.239 +			setreg16(regs, r, result);
   2.240 +			TRACE((regs, regs->eip - eip,
   2.241 +				"addw *0x%x, %%%s", addr, rnames[r]));
   2.242 +		}
   2.243 +		break;
   2.244 +	}
   2.245 +
   2.246 +	if (opc == 0x00)
   2.247 +		hi_bit = (1<<7);
   2.248 +	else
   2.249 +		hi_bit = (prefix & DATA32) ? (1<<31) : (1<<15);
   2.250 +	set_eflags_add(hi_bit, val1, val2, result, regs);
   2.251 +
   2.252 +	return 1;
   2.253 +}
   2.254 +
   2.255 +/*
   2.256   * We need to handle pop opcodes that address memory beyond the 64KB
   2.257   * segment limit that VM8086 mode enforces.
   2.258   */
   2.259 @@ -1314,6 +1542,18 @@ opcode(struct regs *regs)
   2.260  
   2.261  	for (;;) {
   2.262  		switch ((opc = fetch8(regs))) {
   2.263 +
   2.264 +		case 0x00: /* addr32 add r8, r/m8 */
   2.265 +		case 0x01: /* addr32 add r16, r/m16 */
   2.266 +		case 0x03: /* addr32 add r/m16, r16 */
   2.267 +			if (mode != VM86_REAL && mode != VM86_REAL_TO_PROTECTED)
   2.268 +				goto invalid;
   2.269 +			if ((prefix & ADDR32) == 0)
   2.270 +				goto invalid;
   2.271 +			if (!add(regs, prefix, opc))
   2.272 +				goto invalid;
   2.273 +			return OPC_EMULATED;
   2.274 +			
   2.275  		case 0x07: /* pop %es */
   2.276  			regs->ves = (prefix & DATA32) ?
   2.277  				pop32(regs) : pop16(regs);
   2.278 @@ -1510,6 +1750,21 @@ opcode(struct regs *regs)
   2.279  			return OPC_EMULATED;
   2.280  		}
   2.281  
   2.282 +		case 0xA4: /* movsb */
   2.283 +		case 0xA5: /* movsw */
   2.284 +			if ((prefix & ADDR32) == 0)
   2.285 +				goto invalid;
   2.286 +			if (!movs(regs, prefix, opc))
   2.287 +				goto invalid;
   2.288 +			return OPC_EMULATED;
   2.289 +
   2.290 +		case 0xAD: /* lodsw */
   2.291 +			if ((prefix & ADDR32) == 0)
   2.292 +				goto invalid;
   2.293 +			if (!lods(regs, prefix, opc))
   2.294 +				goto invalid;
   2.295 +			return OPC_EMULATED;
   2.296 +			
   2.297  		case 0xBB: /* mov bx, imm16 */
   2.298  		{
   2.299  			int data;
   2.300 @@ -1627,6 +1882,11 @@ opcode(struct regs *regs)
   2.301  			/* Do something power-saving here! */
   2.302  			return OPC_EMULATED;
   2.303  
   2.304 +		case 0xF3: /* rep/repe/repz */
   2.305 +			TRACE((regs, regs->eip - eip, "rep"));
   2.306 +			prefix |= REP;
   2.307 +			continue;
   2.308 +
   2.309  		case 0xF6: /* addr32 testb $imm, r/m8 */
   2.310  			if (!(prefix & ADDR32))
   2.311  				goto invalid;
   2.312 @@ -1660,6 +1920,7 @@ emulate(struct regs *regs)
   2.313  {
   2.314  	unsigned flteip;
   2.315  	int nemul = 0;
   2.316 +	unsigned ip;
   2.317  
   2.318  	/* emulate as many instructions as possible */
   2.319  	while (opcode(regs) != OPC_INVALID)
   2.320 @@ -1668,6 +1929,12 @@ emulate(struct regs *regs)
   2.321  	/* detect the case where we are not making progress */
   2.322  	if (nemul == 0 && prev_eip == regs->eip) {
   2.323  		flteip = address(regs, MASK16(regs->cs), regs->eip);
   2.324 +
   2.325 +		printf("Undecoded sequence: \n");
   2.326 +		for (ip=flteip; ip < flteip+16; ip++)
   2.327 +			printf("0x%02x ", read8(ip));
   2.328 +		printf("\n");
   2.329 +
   2.330  		panic("Unknown opcode at %04x:%04x=0x%x",
   2.331  			MASK16(regs->cs), regs->eip, flteip);
   2.332  	} else