source: trunk/packages/xen-3.1/xen-3.1/tools/firmware/vmxassist/vm86.c @ 34

Last change on this file since 34 was 34, checked in by hartmans, 18 years ago

Add xen and xen-common

File size: 39.6 KB
Line 
1/*
2 * vm86.c: A vm86 emulator. The main purpose of this emulator is to do as
3 * little work as possible.
4 *
5 * Leendert van Doorn, leendert@watson.ibm.com
6 * Copyright (c) 2005-2006, International Business Machines Corporation.
7 *
8 * This program is free software; you can redistribute it and/or modify it
9 * under the terms and conditions of the GNU General Public License,
10 * version 2, as published by the Free Software Foundation.
11 *
12 * This program is distributed in the hope it will be useful, but WITHOUT
13 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
14 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
15 * more details.
16 *
17 * You should have received a copy of the GNU General Public License along with
18 * this program; if not, write to the Free Software Foundation, Inc., 59 Temple
19 * Place - Suite 330, Boston, MA 02111-1307 USA.
20 */
21#include "vm86.h"
22#include "util.h"
23#include "machine.h"
24
25#define HIGHMEM         (1 << 20)               /* 1MB */
26#define MASK16(v)       ((v) & 0xFFFF)
27
28#define DATA32          0x0001
29#define ADDR32          0x0002
30#define SEG_CS          0x0004
31#define SEG_DS          0x0008
32#define SEG_ES          0x0010
33#define SEG_SS          0x0020
34#define SEG_FS          0x0040
35#define SEG_GS          0x0080
36
37static unsigned prev_eip = 0;
38enum vm86_mode mode = 0;
39
40static struct regs saved_rm_regs;
41
42#ifdef DEBUG
43int traceset = 0;
44
45char *states[] = {
46        "<VM86_REAL>",
47        "<VM86_REAL_TO_PROTECTED>",
48        "<VM86_PROTECTED_TO_REAL>",
49        "<VM86_PROTECTED>"
50};
51
52static char *rnames[] = { "ax", "cx", "dx", "bx", "sp", "bp", "si", "di" };
53#endif /* DEBUG */
54
55#define PDE_PS                          (1 << 7)
56#define PT_ENTRY_PRESENT        0x1
57
58/* We only support access to <=4G physical memory due to 1:1 mapping */
59static uint64_t
60guest_linear_to_phys(uint32_t base)
61{
62        uint32_t gcr3 = oldctx.cr3;
63        uint64_t l2_mfn;
64        uint64_t l1_mfn;
65        uint64_t l0_mfn;
66
67        if (!(oldctx.cr0 & CR0_PG))
68                return base;
69
70        if (!(oldctx.cr4 & CR4_PAE)) {
71                l1_mfn = ((uint32_t *)(long)gcr3)[(base >> 22) & 0x3ff];
72                if (!(l1_mfn & PT_ENTRY_PRESENT))
73                        panic("l2 entry not present\n");
74
75                if ((oldctx.cr4 & CR4_PSE) && (l1_mfn & PDE_PS)) {
76                        l0_mfn = l1_mfn & 0xffc00000;
77                        return l0_mfn + (base & 0x3fffff);
78                }
79
80                l1_mfn &= 0xfffff000;
81
82                l0_mfn = ((uint32_t *)(long)l1_mfn)[(base >> 12) & 0x3ff];
83                if (!(l0_mfn & PT_ENTRY_PRESENT))
84                        panic("l1 entry not present\n");
85                l0_mfn &= 0xfffff000;
86
87                return l0_mfn + (base & 0xfff);
88        } else {
89                l2_mfn = ((uint64_t *)(long)gcr3)[(base >> 30) & 0x3];
90                if (!(l2_mfn & PT_ENTRY_PRESENT))
91                        panic("l3 entry not present\n");
92                l2_mfn &= 0xffffff000ULL;
93
94                if (l2_mfn & 0xf00000000ULL) {
95                        printf("l2 page above 4G\n");
96                        cpuid_addr_value(l2_mfn + 8 * ((base >> 21) & 0x1ff), &l1_mfn);
97                } else
98                        l1_mfn = ((uint64_t *)(long)l2_mfn)[(base >> 21) & 0x1ff];
99                if (!(l1_mfn & PT_ENTRY_PRESENT))
100                        panic("l2 entry not present\n");
101
102                if (l1_mfn & PDE_PS) { /* CR4.PSE is ignored in PAE mode */
103                        l0_mfn = l1_mfn & 0xfffe00000ULL;
104                        return l0_mfn + (base & 0x1fffff);
105                }
106
107                l1_mfn &= 0xffffff000ULL;
108
109                if (l1_mfn & 0xf00000000ULL) {
110                        printf("l1 page above 4G\n");
111                        cpuid_addr_value(l1_mfn + 8 * ((base >> 12) & 0x1ff), &l0_mfn);
112                } else
113                        l0_mfn = ((uint64_t *)(long)l1_mfn)[(base >> 12) & 0x1ff];
114                if (!(l0_mfn & PT_ENTRY_PRESENT))
115                        panic("l1 entry not present\n");
116
117                l0_mfn &= 0xffffff000ULL;
118
119                return l0_mfn + (base & 0xfff);
120        }
121}
122
123static unsigned
124address(struct regs *regs, unsigned seg, unsigned off)
125{
126        uint64_t gdt_phys_base;
127        unsigned long long entry;
128        unsigned seg_base, seg_limit;
129        unsigned entry_low, entry_high;
130
131        if (seg == 0) {
132                if (mode == VM86_REAL || mode == VM86_REAL_TO_PROTECTED)
133                        return off;
134                else
135                        panic("segment is zero, but not in real mode!\n");
136        }
137
138        if (mode == VM86_REAL || seg > oldctx.gdtr_limit ||
139                (mode == VM86_REAL_TO_PROTECTED && regs->cs == seg))
140                return ((seg & 0xFFFF) << 4) + off;
141
142        gdt_phys_base = guest_linear_to_phys(oldctx.gdtr_base);
143        if (gdt_phys_base != (uint32_t)gdt_phys_base) {
144                printf("gdt base address above 4G\n");
145                cpuid_addr_value(gdt_phys_base + 8 * (seg >> 3), &entry);
146        } else
147                entry = ((unsigned long long *)(long)gdt_phys_base)[seg >> 3];
148
149        entry_high = entry >> 32;
150        entry_low = entry & 0xFFFFFFFF;
151
152        seg_base  = (entry_high & 0xFF000000) | ((entry >> 16) & 0xFFFFFF);
153        seg_limit = (entry_high & 0xF0000) | (entry_low & 0xFFFF);
154
155        if (entry_high & 0x8000 &&
156                ((entry_high & 0x800000 && off >> 12 <= seg_limit) ||
157                (!(entry_high & 0x800000) && off <= seg_limit)))
158                return seg_base + off;
159
160        panic("should never reach here in function address():\n\t"
161                  "entry=0x%08x%08x, mode=%d, seg=0x%08x, offset=0x%08x\n",
162                  entry_high, entry_low, mode, seg, off);
163
164        return 0;
165}
166
167#ifdef DEBUG
168void
169trace(struct regs *regs, int adjust, char *fmt, ...)
170{
171        unsigned off = regs->eip - adjust;
172        va_list ap;
173
174        if ((traceset & (1 << mode)) &&
175                (mode == VM86_REAL_TO_PROTECTED || mode == VM86_REAL)) {
176                /* 16-bit, seg:off addressing */
177                unsigned addr = address(regs, regs->cs, off);
178                printf("0x%08x: 0x%x:0x%04x ", addr, regs->cs, off);
179                printf("(%d) ", mode);
180                va_start(ap, fmt);
181                vprintf(fmt, ap);
182                va_end(ap);
183                printf("\n");
184        }
185        if ((traceset & (1 << mode)) &&
186                (mode == VM86_PROTECTED_TO_REAL || mode == VM86_PROTECTED)) {
187                /* 16-bit, gdt addressing */
188                unsigned addr = address(regs, regs->cs, off);
189                printf("0x%08x: 0x%x:0x%08x ", addr, regs->cs, off);
190                printf("(%d) ", mode);
191                va_start(ap, fmt);
192                vprintf(fmt, ap);
193                va_end(ap);
194                printf("\n");
195        }
196}
197#endif /* DEBUG */
198
199static inline unsigned
200read32(unsigned addr)
201{
202        return *(unsigned long *) addr;
203}
204
205static inline unsigned
206read16(unsigned addr)
207{
208        return *(unsigned short *) addr;
209}
210
211static inline unsigned
212read8(unsigned addr)
213{
214        return *(unsigned char *) addr;
215}
216
217static inline void
218write32(unsigned addr, unsigned value)
219{
220        *(unsigned long *) addr = value;
221}
222
223static inline void
224write16(unsigned addr, unsigned value)
225{
226        *(unsigned short *) addr = value;
227}
228
229static inline void
230write8(unsigned addr, unsigned value)
231{
232        *(unsigned char *) addr = value;
233}
234
235static inline void
236push32(struct regs *regs, unsigned value)
237{
238        regs->uesp -= 4;
239        write32(address(regs, regs->uss, MASK16(regs->uesp)), value);
240}
241
242static inline void
243push16(struct regs *regs, unsigned value)
244{
245        regs->uesp -= 2;
246        write16(address(regs, regs->uss, MASK16(regs->uesp)), value);
247}
248
249static inline unsigned
250pop32(struct regs *regs)
251{
252        unsigned value = read32(address(regs, regs->uss, MASK16(regs->uesp)));
253        regs->uesp += 4;
254        return value;
255}
256
257static inline unsigned
258pop16(struct regs *regs)
259{
260        unsigned value = read16(address(regs, regs->uss, MASK16(regs->uesp)));
261        regs->uesp += 2;
262        return value;
263}
264
265static inline unsigned
266fetch32(struct regs *regs)
267{
268        unsigned addr = address(regs, regs->cs, MASK16(regs->eip));
269
270        regs->eip += 4;
271        return read32(addr);
272}
273
274static inline unsigned
275fetch16(struct regs *regs)
276{
277        unsigned addr = address(regs, regs->cs, MASK16(regs->eip));
278
279        regs->eip += 2;
280        return read16(addr);
281}
282
283static inline unsigned
284fetch8(struct regs *regs)
285{
286        unsigned addr = address(regs, regs->cs, MASK16(regs->eip));
287
288        regs->eip++;
289        return read8(addr);
290}
291
292static unsigned
293getreg32(struct regs *regs, int r)
294{
295        switch (r & 7) {
296        case 0: return regs->eax;
297        case 1: return regs->ecx;
298        case 2: return regs->edx;
299        case 3: return regs->ebx;
300        case 4: return regs->uesp;
301        case 5: return regs->ebp;
302        case 6: return regs->esi;
303        case 7: return regs->edi;
304        }
305        return ~0;
306}
307
308static unsigned
309getreg16(struct regs *regs, int r)
310{
311        return MASK16(getreg32(regs, r));
312}
313
314static unsigned
315getreg8(struct regs *regs, int r)
316{
317        switch (r & 7) {
318        case 0: return regs->eax & 0xFF; /* al */
319        case 1: return regs->ecx & 0xFF; /* cl */
320        case 2: return regs->edx & 0xFF; /* dl */
321        case 3: return regs->ebx & 0xFF; /* bl */
322        case 4: return (regs->eax >> 8) & 0xFF; /* ah */
323        case 5: return (regs->ecx >> 8) & 0xFF; /* ch */
324        case 6: return (regs->edx >> 8) & 0xFF; /* dh */
325        case 7: return (regs->ebx >> 8) & 0xFF; /* bh */
326        }
327        return ~0;
328}
329
330static void
331setreg32(struct regs *regs, int r, unsigned v)
332{
333        switch (r & 7) {
334        case 0: regs->eax = v; break;
335        case 1: regs->ecx = v; break;
336        case 2: regs->edx = v; break;
337        case 3: regs->ebx = v; break;
338        case 4: regs->uesp = v; break;
339        case 5: regs->ebp = v; break;
340        case 6: regs->esi = v; break;
341        case 7: regs->edi = v; break;
342        }
343}
344
345static void
346setreg16(struct regs *regs, int r, unsigned v)
347{
348        setreg32(regs, r, (getreg32(regs, r) & ~0xFFFF) | MASK16(v));
349}
350
351static void
352setreg8(struct regs *regs, int r, unsigned v)
353{
354        v &= 0xFF;
355        switch (r & 7) {
356        case 0: regs->eax = (regs->eax & ~0xFF) | v; break;
357        case 1: regs->ecx = (regs->ecx & ~0xFF) | v; break;
358        case 2: regs->edx = (regs->edx & ~0xFF) | v; break;
359        case 3: regs->ebx = (regs->ebx & ~0xFF) | v; break;
360        case 4: regs->eax = (regs->eax & ~0xFF00) | (v << 8); break;
361        case 5: regs->ecx = (regs->ecx & ~0xFF00) | (v << 8); break;
362        case 6: regs->edx = (regs->edx & ~0xFF00) | (v << 8); break;
363        case 7: regs->ebx = (regs->ebx & ~0xFF00) | (v << 8); break;
364        }
365}
366
367static unsigned
368segment(unsigned prefix, struct regs *regs, unsigned seg)
369{
370        if (prefix & SEG_ES)
371                seg = regs->ves;
372        if (prefix & SEG_DS)
373                seg = regs->vds;
374        if (prefix & SEG_CS)
375                seg = regs->cs;
376        if (prefix & SEG_SS)
377                seg = regs->uss;
378        if (prefix & SEG_FS)
379                seg = regs->vfs;
380        if (prefix & SEG_GS)
381                seg = regs->vgs;
382        return seg;
383}
384
385static unsigned
386sib(struct regs *regs, int mod, unsigned byte)
387{
388        unsigned scale = (byte >> 6) & 3;
389        int index = (byte >> 3) & 7;
390        int base = byte & 7;
391        unsigned addr = 0;
392
393        switch (mod) {
394        case 0:
395                if (base == 5)
396                        addr = fetch32(regs);
397                else
398                        addr = getreg32(regs, base);
399                break;
400        case 1:
401                addr = getreg32(regs, base) + (char) fetch8(regs);
402                break;
403        case 2:
404                addr = getreg32(regs, base) + fetch32(regs);
405                break;
406        }
407
408        if (index != 4)
409                addr += getreg32(regs, index) << scale;
410
411        return addr;
412}
413
414/*
415 * Operand (modrm) decode
416 */
417static unsigned
418operand(unsigned prefix, struct regs *regs, unsigned modrm)
419{
420        int mod, disp = 0, seg;
421
422        seg = segment(prefix, regs, regs->vds);
423
424        if (prefix & ADDR32) { /* 32-bit addressing */
425                switch ((mod = (modrm >> 6) & 3)) {
426                case 0:
427                        switch (modrm & 7) {
428                        case 0: return address(regs, seg, regs->eax);
429                        case 1: return address(regs, seg, regs->ecx);
430                        case 2: return address(regs, seg, regs->edx);
431                        case 3: return address(regs, seg, regs->ebx);
432                        case 4: return address(regs, seg,
433                                                   sib(regs, mod, fetch8(regs)));
434                        case 5: return address(regs, seg, fetch32(regs));
435                        case 6: return address(regs, seg, regs->esi);
436                        case 7: return address(regs, seg, regs->edi);
437                        }
438                        break;
439                case 1:
440                case 2:
441                        if ((modrm & 7) != 4) {
442                                if (mod == 1)
443                                        disp = (char) fetch8(regs);
444                                else
445                                        disp = (int) fetch32(regs);
446                        }
447                        switch (modrm & 7) {
448                        case 0: return address(regs, seg, regs->eax + disp);
449                        case 1: return address(regs, seg, regs->ecx + disp);
450                        case 2: return address(regs, seg, regs->edx + disp);
451                        case 3: return address(regs, seg, regs->ebx + disp);
452                        case 4: return address(regs, seg,
453                                                   sib(regs, mod, fetch8(regs)));
454                        case 5: return address(regs, seg, regs->ebp + disp);
455                        case 6: return address(regs, seg, regs->esi + disp);
456                        case 7: return address(regs, seg, regs->edi + disp);
457                        }
458                        break;
459                case 3:
460                        return getreg32(regs, modrm);
461                }
462        } else { /* 16-bit addressing */
463                switch ((mod = (modrm >> 6) & 3)) {
464                case 0:
465                        switch (modrm & 7) {
466                        case 0: return address(regs, seg, MASK16(regs->ebx) +
467                                        MASK16(regs->esi));
468                        case 1: return address(regs, seg, MASK16(regs->ebx) +
469                                        MASK16(regs->edi));
470                        case 2: return address(regs, seg, MASK16(regs->ebp) +
471                                        MASK16(regs->esi));
472                        case 3: return address(regs, seg, MASK16(regs->ebp) +
473                                        MASK16(regs->edi));
474                        case 4: return address(regs, seg, MASK16(regs->esi));
475                        case 5: return address(regs, seg, MASK16(regs->edi));
476                        case 6: return address(regs, seg, fetch16(regs));
477                        case 7: return address(regs, seg, MASK16(regs->ebx));
478                        }
479                        break;
480                case 1:
481                case 2:
482                        if (mod == 1)
483                                disp = (char) fetch8(regs);
484                        else
485                                disp = (int) fetch16(regs);
486                        switch (modrm & 7) {
487                        case 0: return address(regs, seg, MASK16(regs->ebx) +
488                                        MASK16(regs->esi) + disp);
489                        case 1: return address(regs, seg, MASK16(regs->ebx) +
490                                        MASK16(regs->edi) + disp);
491                        case 2: return address(regs, seg, MASK16(regs->ebp) +
492                                        MASK16(regs->esi) + disp);
493                        case 3: return address(regs, seg, MASK16(regs->ebp) +
494                                        MASK16(regs->edi) + disp);
495                        case 4: return address(regs, seg,
496                                        MASK16(regs->esi) + disp);
497                        case 5: return address(regs, seg,
498                                        MASK16(regs->edi) + disp);
499                        case 6: return address(regs, seg,
500                                        MASK16(regs->ebp) + disp);
501                        case 7: return address(regs, seg,
502                                        MASK16(regs->ebx) + disp);
503                        }
504                        break;
505                case 3:
506                        return getreg16(regs, modrm);
507                }
508        }
509
510        return 0;
511}
512
513/*
514 * Load new IDT
515 */
516static int
517lidt(struct regs *regs, unsigned prefix, unsigned modrm)
518{
519        unsigned eip = regs->eip - 3;
520        unsigned addr = operand(prefix, regs, modrm);
521
522        oldctx.idtr_limit = ((struct dtr *) addr)->size;
523        if ((prefix & DATA32) == 0)
524                oldctx.idtr_base = ((struct dtr *) addr)->base & 0xFFFFFF;
525        else
526                oldctx.idtr_base = ((struct dtr *) addr)->base;
527        TRACE((regs, regs->eip - eip, "lidt 0x%x <%d, 0x%x>",
528                addr, oldctx.idtr_limit, oldctx.idtr_base));
529
530        return 1;
531}
532
533/*
534 * Load new GDT
535 */
536static int
537lgdt(struct regs *regs, unsigned prefix, unsigned modrm)
538{
539        unsigned eip = regs->eip - 3;
540        unsigned addr = operand(prefix, regs, modrm);
541
542        oldctx.gdtr_limit = ((struct dtr *) addr)->size;
543        if ((prefix & DATA32) == 0)
544                oldctx.gdtr_base = ((struct dtr *) addr)->base & 0xFFFFFF;
545        else
546                oldctx.gdtr_base = ((struct dtr *) addr)->base;
547        TRACE((regs, regs->eip - eip, "lgdt 0x%x <%d, 0x%x>",
548                addr, oldctx.gdtr_limit, oldctx.gdtr_base));
549
550        return 1;
551}
552
553/*
554 * Modify CR0 either through an lmsw instruction.
555 */
556static int
557lmsw(struct regs *regs, unsigned prefix, unsigned modrm)
558{
559        unsigned eip = regs->eip - 3;
560        unsigned ax = operand(prefix, regs, modrm) & 0xF;
561        unsigned cr0 = (oldctx.cr0 & 0xFFFFFFF0) | ax;
562
563        TRACE((regs, regs->eip - eip, "lmsw 0x%x", ax));
564#ifndef TEST
565        oldctx.cr0 = cr0 | CR0_PE | CR0_NE;
566#else
567        oldctx.cr0 = cr0 | CR0_PE | CR0_NE | CR0_PG;
568#endif
569        if (cr0 & CR0_PE)
570                set_mode(regs, VM86_REAL_TO_PROTECTED);
571
572        return 1;
573}
574
575/*
576 * We need to handle moves that address memory beyond the 64KB segment
577 * limit that VM8086 mode enforces.
578 */
579static int
580movr(struct regs *regs, unsigned prefix, unsigned opc)
581{
582        unsigned eip = regs->eip - 1;
583        unsigned modrm = fetch8(regs);
584        unsigned addr = operand(prefix, regs, modrm);
585        unsigned val, r = (modrm >> 3) & 7;
586
587        if ((modrm & 0xC0) == 0xC0) /* no registers */
588                return 0;
589
590        switch (opc) {
591        case 0x88: /* addr32 mov r8, r/m8 */
592                val = getreg8(regs, r);
593                TRACE((regs, regs->eip - eip,
594                        "movb %%e%s, *0x%x", rnames[r], addr));
595                write8(addr, val);
596                break;
597
598        case 0x8A: /* addr32 mov r/m8, r8 */
599                TRACE((regs, regs->eip - eip,
600                        "movb *0x%x, %%%s", addr, rnames[r]));
601                setreg8(regs, r, read8(addr));
602                break;
603
604        case 0x89: /* addr32 mov r16, r/m16 */
605                val = getreg32(regs, r);
606                if (prefix & DATA32) {
607                        TRACE((regs, regs->eip - eip,
608                                "movl %%e%s, *0x%x", rnames[r], addr));
609                        write32(addr, val);
610                } else {
611                        TRACE((regs, regs->eip - eip,
612                                "movw %%%s, *0x%x", rnames[r], addr));
613                        write16(addr, MASK16(val));
614                }
615                break;
616
617        case 0x8B: /* addr32 mov r/m16, r16 */
618                if (prefix & DATA32) {
619                        TRACE((regs, regs->eip - eip,
620                                "movl *0x%x, %%e%s", addr, rnames[r]));
621                        setreg32(regs, r, read32(addr));
622                } else {
623                        TRACE((regs, regs->eip - eip,
624                                "movw *0x%x, %%%s", addr, rnames[r]));
625                        setreg16(regs, r, read16(addr));
626                }
627                break;
628
629        case 0xC6: /* addr32 movb $imm, r/m8 */
630                if ((modrm >> 3) & 7)
631                        return 0;
632                val = fetch8(regs);
633                write8(addr, val);
634                TRACE((regs, regs->eip - eip, "movb $0x%x, *0x%x",
635                                                        val, addr));
636                break;
637        }
638        return 1;
639}
640
641/*
642 * Move to and from a control register.
643 */
644static int
645movcr(struct regs *regs, unsigned prefix, unsigned opc)
646{
647        unsigned eip = regs->eip - 2;
648        unsigned modrm = fetch8(regs);
649        unsigned cr = (modrm >> 3) & 7;
650
651        if ((modrm & 0xC0) != 0xC0) /* only registers */
652                return 0;
653
654        switch (opc) {
655        case 0x20: /* mov Rd, Cd */
656                TRACE((regs, regs->eip - eip, "movl %%cr%d, %%eax", cr));
657                switch (cr) {
658                case 0:
659#ifndef TEST
660                        setreg32(regs, modrm,
661                                oldctx.cr0 & ~(CR0_PE | CR0_NE));
662#else
663                        setreg32(regs, modrm,
664                                oldctx.cr0 & ~(CR0_PE | CR0_NE | CR0_PG));
665#endif
666                        break;
667                case 2:
668                        setreg32(regs, modrm, get_cr2());
669                        break;
670                case 3:
671                        setreg32(regs, modrm, oldctx.cr3);
672                        break;
673                case 4:
674                        setreg32(regs, modrm, oldctx.cr4);
675                        break;
676                }
677                break;
678        case 0x22: /* mov Cd, Rd */
679                TRACE((regs, regs->eip - eip, "movl %%eax, %%cr%d", cr));
680                switch (cr) {
681                case 0:
682                        oldctx.cr0 = getreg32(regs, modrm) | (CR0_PE | CR0_NE);
683#ifdef TEST
684                        oldctx.cr0 |= CR0_PG;
685#endif
686                        if (getreg32(regs, modrm) & CR0_PE)
687                                set_mode(regs, VM86_REAL_TO_PROTECTED);
688                        else
689                                set_mode(regs, VM86_REAL);
690                        break;
691                case 3:
692                        oldctx.cr3 = getreg32(regs, modrm);
693                        break;
694                case 4:
695                        oldctx.cr4 = getreg32(regs, modrm);
696                        break;
697                }
698                break;
699        }
700
701        return 1;
702}
703
704static inline void set_eflags_ZF(unsigned mask, unsigned v1, struct regs *regs)
705{
706        if ((v1 & mask) == 0)
707                regs->eflags |= EFLAGS_ZF;
708        else
709                regs->eflags &= ~EFLAGS_ZF;
710}
711
712/*
713 * We need to handle cmp opcodes that address memory beyond the 64KB
714 * segment limit that VM8086 mode enforces.
715 */
716static int
717cmp(struct regs *regs, unsigned prefix, unsigned opc)
718{
719        unsigned eip = regs->eip - 1;
720        unsigned modrm = fetch8(regs);
721        unsigned addr = operand(prefix, regs, modrm);
722        unsigned diff, val, r = (modrm >> 3) & 7;
723
724        if ((modrm & 0xC0) == 0xC0) /* no registers */
725                return 0;
726
727        switch (opc) {
728        case 0x39: /* addr32 cmp r16, r/m16 */
729                val = getreg32(regs, r);
730                if (prefix & DATA32) {
731                        diff = read32(addr) - val;
732                        set_eflags_ZF(~0, diff, regs);
733
734                        TRACE((regs, regs->eip - eip,
735                                "cmp %%e%s, *0x%x (0x%x)",
736                                rnames[r], addr, diff));
737                } else {
738                        diff = read16(addr) - val;
739                        set_eflags_ZF(0xFFFF, diff, regs);
740
741                        TRACE((regs, regs->eip - eip,
742                                "cmp %%%s, *0x%x (0x%x)",
743                                rnames[r], addr, diff));
744                }
745                break;
746
747        /* other cmp opcodes ... */
748        }
749        return 1;
750}
751
752/*
753 * We need to handle test opcodes that address memory beyond the 64KB
754 * segment limit that VM8086 mode enforces.
755 */
756static int
757test(struct regs *regs, unsigned prefix, unsigned opc)
758{
759        unsigned eip = regs->eip - 1;
760        unsigned modrm = fetch8(regs);
761        unsigned addr = operand(prefix, regs, modrm);
762        unsigned val, diff;
763
764        if ((modrm & 0xC0) == 0xC0) /* no registers */
765                return 0;
766
767        switch (opc) {
768        case 0xF6: /* testb $imm, r/m8 */
769                if ((modrm >> 3) & 7)
770                        return 0;
771                val = fetch8(regs);
772                diff = read8(addr) & val;
773                set_eflags_ZF(0xFF, diff, regs);
774
775                TRACE((regs, regs->eip - eip, "testb $0x%x, *0x%x (0x%x)",
776                                                        val, addr, diff));
777                break;
778
779        /* other test opcodes ... */
780        }
781
782        return 1;
783}
784
785/*
786 * We need to handle pop opcodes that address memory beyond the 64KB
787 * segment limit that VM8086 mode enforces.
788 */
789static int
790pop(struct regs *regs, unsigned prefix, unsigned opc)
791{
792        unsigned eip = regs->eip - 1;
793        unsigned modrm = fetch8(regs);
794        unsigned addr = operand(prefix, regs, modrm);
795
796        if ((modrm & 0xC0) == 0xC0) /* no registers */
797                return 0;
798
799        switch (opc) {
800        case 0x8F: /* pop r/m16 */
801                if ((modrm >> 3) & 7)
802                        return 0;
803                if (prefix & DATA32)
804                        write32(addr, pop32(regs));
805                else
806                        write16(addr, pop16(regs));
807                TRACE((regs, regs->eip - eip, "pop *0x%x", addr));
808                break;
809
810        /* other pop opcodes ... */
811        }
812
813        return 1;
814}
815
816static int
817mov_to_seg(struct regs *regs, unsigned prefix, unsigned opc)
818{
819        unsigned modrm = fetch8(regs);
820
821        /* Only need to emulate segment loads in real->protected mode. */
822        if (mode != VM86_REAL_TO_PROTECTED)
823                return 0;
824
825        /* Register source only. */
826        if ((modrm & 0xC0) != 0xC0)
827                goto fail;
828
829        switch ((modrm & 0x38) >> 3) {
830        case 0: /* es */
831                regs->ves = getreg16(regs, modrm);
832                saved_rm_regs.ves = 0;
833                oldctx.es_sel = regs->ves;
834                return 1;
835
836        /* case 1: cs */
837
838        case 2: /* ss */
839                regs->uss = getreg16(regs, modrm);
840                saved_rm_regs.uss = 0;
841                oldctx.ss_sel = regs->uss;
842                return 1;
843        case 3: /* ds */
844                regs->vds = getreg16(regs, modrm);
845                saved_rm_regs.vds = 0;
846                oldctx.ds_sel = regs->vds;
847                return 1;
848        case 4: /* fs */
849                regs->vfs = getreg16(regs, modrm);
850                saved_rm_regs.vfs = 0;
851                oldctx.fs_sel = regs->vfs;
852                return 1;
853        case 5: /* gs */
854                regs->vgs = getreg16(regs, modrm);
855                saved_rm_regs.vgs = 0;
856                oldctx.gs_sel = regs->vgs;
857                return 1;
858        }
859
860 fail:
861        printf("%s:%d: missed opcode %02x %02x\n",
862                   __FUNCTION__, __LINE__, opc, modrm);
863        return 0;
864}
865
866/*
867 * Emulate a segment load in protected mode
868 */
869static int
870load_seg(unsigned long sel, uint32_t *base, uint32_t *limit, union vmcs_arbytes *arbytes)
871{
872        uint64_t gdt_phys_base;
873        unsigned long long entry;
874
875        /* protected mode: use seg as index into gdt */
876        if (sel > oldctx.gdtr_limit)
877                return 0;
878
879        if (sel == 0) {
880                arbytes->fields.null_bit = 1;
881                return 1;
882        }
883
884        gdt_phys_base = guest_linear_to_phys(oldctx.gdtr_base);
885        if (gdt_phys_base != (uint32_t)gdt_phys_base) {
886                printf("gdt base address above 4G\n");
887                cpuid_addr_value(gdt_phys_base + 8 * (sel >> 3), &entry);
888        } else
889                entry = ((unsigned long long *)(long)gdt_phys_base)[sel >> 3];
890
891        /* Check the P bit first */
892        if (!((entry >> (15+32)) & 0x1) && sel != 0)
893                return 0;
894
895        *base =  (((entry >> (56-24)) & 0xFF000000) |
896                  ((entry >> (32-16)) & 0x00FF0000) |
897                  ((entry >> (   16)) & 0x0000FFFF));
898        *limit = (((entry >> (48-16)) & 0x000F0000) |
899                  (entry & 0x0000FFFF));
900
901        arbytes->bytes = 0;
902        arbytes->fields.seg_type = (entry >> (8+32)) & 0xF; /* TYPE */
903        arbytes->fields.s = (entry >> (12+32)) & 0x1; /* S */
904        if (arbytes->fields.s)
905                arbytes->fields.seg_type |= 1; /* accessed */
906        arbytes->fields.dpl = (entry >> (13+32)) & 0x3; /* DPL */
907        arbytes->fields.p = (entry >> (15+32)) & 0x1; /* P */
908        arbytes->fields.avl = (entry >> (20+32)) & 0x1; /* AVL */
909        arbytes->fields.default_ops_size = (entry >> (22+32)) & 0x1; /* D */
910
911        if (entry & (1ULL << (23+32))) { /* G */
912                arbytes->fields.g = 1;
913                *limit = (*limit << 12) | 0xFFF;
914        }
915
916        return 1;
917}
918
919/*
920 * Emulate a protected mode segment load, falling back to clearing it if
921 * the descriptor was invalid.
922 */
923static void
924load_or_clear_seg(unsigned long sel, uint32_t *base, uint32_t *limit, union vmcs_arbytes *arbytes)
925{
926        if (!load_seg(sel, base, limit, arbytes))
927                load_seg(0, base, limit, arbytes);
928}
929
930
931/*
932 * Transition to protected mode
933 */
934static void
935protected_mode(struct regs *regs)
936{
937        extern char stack_top[];
938
939        regs->eflags &= ~(EFLAGS_TF|EFLAGS_VM);
940
941        oldctx.eip = regs->eip;
942        oldctx.esp = regs->uesp;
943        oldctx.eflags = regs->eflags;
944
945        /* reload all segment registers */
946        if (!load_seg(regs->cs, &oldctx.cs_base,
947                                &oldctx.cs_limit, &oldctx.cs_arbytes))
948                panic("Invalid %%cs=0x%x for protected mode\n", regs->cs);
949        oldctx.cs_sel = regs->cs;
950
951        load_or_clear_seg(oldctx.es_sel, &oldctx.es_base,
952                          &oldctx.es_limit, &oldctx.es_arbytes);
953        load_or_clear_seg(oldctx.ss_sel, &oldctx.ss_base,
954                          &oldctx.ss_limit, &oldctx.ss_arbytes);
955        load_or_clear_seg(oldctx.ds_sel, &oldctx.ds_base,
956                          &oldctx.ds_limit, &oldctx.ds_arbytes);
957        load_or_clear_seg(oldctx.fs_sel, &oldctx.fs_base,
958                          &oldctx.fs_limit, &oldctx.fs_arbytes);
959        load_or_clear_seg(oldctx.gs_sel, &oldctx.gs_base,
960                          &oldctx.gs_limit, &oldctx.gs_arbytes);
961
962        /* initialize jump environment to warp back to protected mode */
963        regs->uss = DATA_SELECTOR;
964        regs->uesp = (unsigned long)stack_top;
965        regs->cs = CODE_SELECTOR;
966        regs->eip = (unsigned long)switch_to_protected_mode;
967
968        /* this should get us into 32-bit mode */
969}
970
971/*
972 * Start real-mode emulation
973 */
974static void
975real_mode(struct regs *regs)
976{
977        regs->eflags |= EFLAGS_VM | 0x02;
978
979        /*
980         * When we transition from protected to real-mode and we
981         * have not reloaded the segment descriptors yet, they are
982         * interpreted as if they were in protect mode.
983         * We emulate this behavior by assuming that these memory
984         * reference are below 1MB and set %ss, %ds, %es accordingly.
985         */
986        if (regs->uss != 0) {
987                if (regs->uss >= HIGHMEM)
988                        panic("%%ss 0x%lx higher than 1MB", regs->uss);
989                regs->uss = address(regs, regs->uss, 0) >> 4;
990        } else {
991                regs->uss = saved_rm_regs.uss;
992        }
993        if (regs->vds != 0) {
994                if (regs->vds >= HIGHMEM)
995                        panic("%%ds 0x%lx higher than 1MB", regs->vds);
996                regs->vds = address(regs, regs->vds, 0) >> 4;
997        } else {
998                regs->vds = saved_rm_regs.vds;
999        }
1000        if (regs->ves != 0) {
1001                if (regs->ves >= HIGHMEM)
1002                        panic("%%es 0x%lx higher than 1MB", regs->ves);
1003                regs->ves = address(regs, regs->ves, 0) >> 4;
1004        } else {
1005                regs->ves = saved_rm_regs.ves;
1006        }
1007
1008        /* this should get us into 16-bit mode */
1009}
1010
1011/*
1012 * This is the smarts of the emulator and handles the mode transitions. The
1013 * emulator handles 4 different modes. 1) VM86_REAL: emulated real-mode,
1014 * Just handle those instructions that are not supported under VM8086.
1015 * 2) VM86_REAL_TO_PROTECTED: going from real-mode to protected mode. In
1016 * this we single step through the instructions until we reload the
1017 * new %cs (some OSes do a lot of computations before reloading %cs). 2)
1018 * VM86_PROTECTED_TO_REAL when we are going from protected to real mode. In
1019 * this case we emulate the instructions by hand. Finally, 4) VM86_PROTECTED
1020 * when we transitioned to protected mode and we should abandon the
1021 * emulator. No instructions are emulated when in VM86_PROTECTED mode.
1022 */
1023void
1024set_mode(struct regs *regs, enum vm86_mode newmode)
1025{
1026        switch (newmode) {
1027        case VM86_REAL:
1028                if ((mode == VM86_PROTECTED_TO_REAL) ||
1029                    (mode == VM86_REAL_TO_PROTECTED)) {
1030                        regs->eflags &= ~EFLAGS_TF;
1031                        real_mode(regs);
1032                } else if (mode != VM86_REAL)
1033                        panic("unexpected real mode transition");
1034                break;
1035
1036        case VM86_REAL_TO_PROTECTED:
1037                if (mode == VM86_REAL) {
1038                        regs->eflags |= EFLAGS_TF;
1039                        saved_rm_regs.vds = regs->vds;
1040                        saved_rm_regs.ves = regs->ves;
1041                        saved_rm_regs.vfs = regs->vfs;
1042                        saved_rm_regs.vgs = regs->vgs;
1043                        saved_rm_regs.uss = regs->uss;
1044                        oldctx.ds_sel = 0;
1045                        oldctx.es_sel = 0;
1046                        oldctx.fs_sel = 0;
1047                        oldctx.gs_sel = 0;
1048                        oldctx.ss_sel = 0;
1049                } else if (mode != VM86_REAL_TO_PROTECTED)
1050                        panic("unexpected real-to-protected mode transition");
1051                break;
1052
1053        case VM86_PROTECTED_TO_REAL:
1054                if (mode != VM86_PROTECTED)
1055                        panic("unexpected protected-to-real mode transition");
1056                break;
1057
1058        case VM86_PROTECTED:
1059                if (mode != VM86_REAL_TO_PROTECTED)
1060                        panic("unexpected protected mode transition");
1061                protected_mode(regs);
1062                break;
1063        }
1064
1065        mode = newmode;
1066        TRACE((regs, 0, states[mode]));
1067}
1068
1069static void
1070jmpl(struct regs *regs, int prefix)
1071{
1072        unsigned n = regs->eip;
1073        unsigned cs, eip;
1074
1075        eip = (prefix & DATA32) ? fetch32(regs) : fetch16(regs);
1076        cs = fetch16(regs);
1077
1078        TRACE((regs, (regs->eip - n) + 1, "jmpl 0x%x:0x%x", cs, eip));
1079
1080        regs->cs = cs;
1081        regs->eip = eip;
1082
1083        if (mode == VM86_REAL_TO_PROTECTED)             /* jump to protected mode */
1084                set_mode(regs, VM86_PROTECTED);
1085        else if (mode == VM86_PROTECTED_TO_REAL)/* jump to real mode */
1086                set_mode(regs, VM86_REAL);
1087        else
1088                panic("jmpl");
1089}
1090
1091static void
1092jmpl_indirect(struct regs *regs, int prefix, unsigned modrm)
1093{
1094        unsigned n = regs->eip;
1095        unsigned cs, eip;
1096        unsigned addr;
1097
1098        addr = operand(prefix, regs, modrm);
1099
1100        eip = (prefix & DATA32) ? read32(addr) : read16(addr);
1101        addr += (prefix & DATA32) ? 4 : 2;
1102        cs = read16(addr);
1103
1104        TRACE((regs, (regs->eip - n) + 1, "jmpl 0x%x:0x%x", cs, eip));
1105
1106        regs->cs = cs;
1107        regs->eip = eip;
1108
1109        if (mode == VM86_REAL_TO_PROTECTED)             /* jump to protected mode */
1110                set_mode(regs, VM86_PROTECTED);
1111        else if (mode == VM86_PROTECTED_TO_REAL)/* jump to real mode */
1112                set_mode(regs, VM86_REAL);
1113        else
1114                panic("jmpl");
1115}
1116
1117static void
1118retl(struct regs *regs, int prefix)
1119{
1120        unsigned cs, eip;
1121
1122        if (prefix & DATA32) {
1123                eip = pop32(regs);
1124                cs = MASK16(pop32(regs));
1125        } else {
1126                eip = pop16(regs);
1127                cs = pop16(regs);
1128        }
1129
1130        TRACE((regs, 1, "retl (to 0x%x:0x%x)", cs, eip));
1131
1132        regs->cs = cs;
1133        regs->eip = eip;
1134
1135        if (mode == VM86_REAL_TO_PROTECTED)             /* jump to protected mode */
1136                set_mode(regs, VM86_PROTECTED);
1137        else if (mode == VM86_PROTECTED_TO_REAL)/* jump to real mode */
1138                set_mode(regs, VM86_REAL);
1139        else
1140                panic("retl");
1141}
1142
1143static void
1144interrupt(struct regs *regs, int n)
1145{
1146        TRACE((regs, 0, "external interrupt %d", n));
1147        push16(regs, regs->eflags);
1148        push16(regs, regs->cs);
1149        push16(regs, regs->eip);
1150        regs->eflags &= ~EFLAGS_IF;
1151        regs->eip = read16(address(regs, 0, n * 4));
1152        regs->cs = read16(address(regs, 0, n * 4 + 2));
1153}
1154
1155/*
1156 * Most port I/O operations are passed unmodified. We do have to be
1157 * careful and make sure the emulated program isn't remapping the
1158 * interrupt vectors. The following simple state machine catches
1159 * these attempts and rewrites them.
1160 */
1161static int
1162outbyte(struct regs *regs, unsigned prefix, unsigned opc)
1163{
1164        static char icw2[2] = { 0 };
1165        int al, port;
1166
1167        switch (opc) {
1168        case 0xE6: /* outb port, al */
1169                port = fetch8(regs);
1170                break;
1171        case 0xEE: /* outb (%dx), al */
1172                port = MASK16(regs->edx);
1173                break;
1174        default:
1175                return 0;
1176        }
1177
1178        al = regs->eax & 0xFF;
1179
1180        switch (port) {
1181        case PIC_MASTER + PIC_CMD:
1182                if (al & (1 << 4)) /* A0=0,D4=1 -> ICW1 */
1183                        icw2[0] = 1;
1184                break;
1185        case PIC_MASTER + PIC_IMR:
1186                if (icw2[0]) {
1187                        icw2[0] = 0;
1188                        printf("Remapping master: ICW2 0x%x -> 0x%x\n",
1189                                al, NR_EXCEPTION_HANDLER);
1190                        al = NR_EXCEPTION_HANDLER;
1191                }
1192                break;
1193
1194        case PIC_SLAVE  + PIC_CMD:
1195                if (al & (1 << 4)) /* A0=0,D4=1 -> ICW1 */
1196                        icw2[1] = 1;
1197                break;
1198        case PIC_SLAVE  + PIC_IMR:
1199                if (icw2[1]) {
1200                        icw2[1] = 0;
1201                        printf("Remapping slave: ICW2 0x%x -> 0x%x\n",
1202                                al, NR_EXCEPTION_HANDLER+8);
1203                        al = NR_EXCEPTION_HANDLER+8;
1204                }
1205                break;
1206        }
1207
1208        outb(port, al);
1209        return 1;
1210}
1211
1212static int
1213inbyte(struct regs *regs, unsigned prefix, unsigned opc)
1214{
1215        int port;
1216
1217        switch (opc) {
1218        case 0xE4: /* inb al, port */
1219                port = fetch8(regs);
1220                break;
1221        case 0xEC: /* inb al, (%dx) */
1222                port = MASK16(regs->edx);
1223                break;
1224        default:
1225                return 0;
1226        }
1227
1228        regs->eax = (regs->eax & ~0xFF) | inb(port);
1229        return 1;
1230}
1231
1232static void
1233pushrm(struct regs *regs, int prefix, unsigned modrm)
1234{
1235        unsigned n = regs->eip;
1236        unsigned addr;
1237        unsigned data;
1238
1239        addr = operand(prefix, regs, modrm);
1240
1241        if (prefix & DATA32) {
1242                data = read32(addr);
1243                push32(regs, data);
1244        } else {
1245                data = read16(addr);
1246                push16(regs, data);
1247        }
1248
1249        TRACE((regs, (regs->eip - n) + 1, "push *0x%x", addr));
1250}
1251
1252enum { OPC_INVALID, OPC_EMULATED };
1253
1254#define rdmsr(msr,val1,val2)                            \
1255        __asm__ __volatile__(                           \
1256                "rdmsr"                                 \
1257                : "=a" (val1), "=d" (val2)              \
1258                : "c" (msr))
1259
1260#define wrmsr(msr,val1,val2)                            \
1261        __asm__ __volatile__(                           \
1262                "wrmsr"                                 \
1263                : /* no outputs */                      \
1264                : "c" (msr), "a" (val1), "d" (val2))
1265
1266/*
1267 * Emulate a single instruction, including all its prefixes. We only implement
1268 * a small subset of the opcodes, and not all opcodes are implemented for each
1269 * of the four modes we can operate in.
1270 */
1271static int
1272opcode(struct regs *regs)
1273{
1274        unsigned eip = regs->eip;
1275        unsigned opc, modrm, disp;
1276        unsigned prefix = 0;
1277
1278        for (;;) {
1279                switch ((opc = fetch8(regs))) {
1280                case 0x07: /* pop %es */
1281                        regs->ves = (prefix & DATA32) ?
1282                                pop32(regs) : pop16(regs);
1283                        TRACE((regs, regs->eip - eip, "pop %%es"));
1284                        if (mode == VM86_REAL_TO_PROTECTED) {
1285                                saved_rm_regs.ves = 0;
1286                                oldctx.es_sel = regs->ves;
1287                        }
1288                        return OPC_EMULATED;
1289
1290                case 0x0F: /* two byte opcode */
1291                        if (mode == VM86_PROTECTED)
1292                                goto invalid;
1293                        switch ((opc = fetch8(regs))) {
1294                        case 0x01:
1295                                switch (((modrm = fetch8(regs)) >> 3) & 7) {
1296                                case 0: /* sgdt */
1297                                case 1: /* sidt */
1298                                        goto invalid;
1299                                case 2: /* lgdt */
1300                                        if (!lgdt(regs, prefix, modrm))
1301                                                goto invalid;
1302                                        return OPC_EMULATED;
1303                                case 3: /* lidt */
1304                                        if (!lidt(regs, prefix, modrm))
1305                                                goto invalid;
1306                                        return OPC_EMULATED;
1307                                case 4: /* smsw */
1308                                        goto invalid;
1309                                case 5:
1310                                        goto invalid;
1311                                case 6: /* lmsw */
1312                                        if (!lmsw(regs, prefix, modrm))
1313                                                goto invalid;
1314                                        return OPC_EMULATED;
1315                                case 7: /* invlpg */
1316                                        goto invalid;
1317                                }
1318                                break;
1319                        case 0x09: /* wbinvd */
1320                                return OPC_EMULATED;
1321                        case 0x20: /* mov Rd, Cd (1h) */
1322                        case 0x22:
1323                                if (!movcr(regs, prefix, opc))
1324                                        goto invalid;
1325                                return OPC_EMULATED;
1326                        case 0x30: /* WRMSR */
1327                                wrmsr(regs->ecx, regs->eax, regs->edx);
1328                                return OPC_EMULATED;
1329                        case 0x32: /* RDMSR */
1330                                rdmsr(regs->ecx, regs->eax, regs->edx);
1331                                return OPC_EMULATED;
1332                        default:
1333                                goto invalid;
1334                        }
1335                        goto invalid;
1336
1337                case 0x1F: /* pop %ds */
1338                        regs->vds = (prefix & DATA32) ?
1339                                pop32(regs) : pop16(regs);
1340                        TRACE((regs, regs->eip - eip, "pop %%ds"));
1341                        if (mode == VM86_REAL_TO_PROTECTED) {
1342                                saved_rm_regs.vds = 0;
1343                                oldctx.ds_sel = regs->vds;
1344                        }
1345                        return OPC_EMULATED;
1346
1347                case 0x26:
1348                        TRACE((regs, regs->eip - eip, "%%es:"));
1349                        prefix |= SEG_ES;
1350                        continue;
1351
1352                case 0x2E:
1353                        TRACE((regs, regs->eip - eip, "%%cs:"));
1354                        prefix |= SEG_CS;
1355                        continue;
1356
1357                case 0x36:
1358                        TRACE((regs, regs->eip - eip, "%%ss:"));
1359                        prefix |= SEG_SS;
1360                        continue;
1361
1362                case 0x39: /* addr32 cmp r16, r/m16 */
1363                case 0x3B: /* addr32 cmp r/m16, r16 */
1364                        if (mode != VM86_REAL && mode != VM86_REAL_TO_PROTECTED)
1365                                goto invalid;
1366                        if ((prefix & ADDR32) == 0)
1367                                goto invalid;
1368                        if (!cmp(regs, prefix, opc))
1369                                goto invalid;
1370                        return OPC_EMULATED;
1371
1372                case 0x3E:
1373                        TRACE((regs, regs->eip - eip, "%%ds:"));
1374                        prefix |= SEG_DS;
1375                        continue;
1376
1377                case 0x64:
1378                        TRACE((regs, regs->eip - eip, "%%fs:"));
1379                        prefix |= SEG_FS;
1380                        continue;
1381
1382                case 0x65:
1383                        TRACE((regs, regs->eip - eip, "%%gs:"));
1384                        prefix |= SEG_GS;
1385                        continue;
1386
1387                case 0x66:
1388                        TRACE((regs, regs->eip - eip, "data32"));
1389                        prefix |= DATA32;
1390                        continue;
1391
1392                case 0x67:
1393                        TRACE((regs, regs->eip - eip, "addr32"));
1394                        prefix |= ADDR32;
1395                        continue;
1396
1397                case 0x88: /* addr32 mov r8, r/m8 */
1398                case 0x8A: /* addr32 mov r/m8, r8 */
1399                        if (mode != VM86_REAL && mode != VM86_REAL_TO_PROTECTED)
1400                                goto invalid;
1401                        if ((prefix & ADDR32) == 0)
1402                                goto invalid;
1403                        if (!movr(regs, prefix, opc))
1404                                goto invalid;
1405                        return OPC_EMULATED;
1406
1407                case 0x89: /* addr32 mov r16, r/m16 */
1408                        if (mode == VM86_PROTECTED_TO_REAL) {
1409                                unsigned modrm = fetch8(regs);
1410                                unsigned addr = operand(prefix, regs, modrm);
1411                                unsigned val, r = (modrm >> 3) & 7;
1412
1413                                if (prefix & DATA32) {
1414                                        val = getreg16(regs, r);
1415                                        write32(addr, val);
1416                                } else {
1417                                        val = getreg32(regs, r);
1418                                        write16(addr, MASK16(val));
1419                                }
1420                                TRACE((regs, regs->eip - eip,
1421                                        "mov %%%s, *0x%x", rnames[r], addr));
1422                                return OPC_EMULATED;
1423                        }
1424                case 0x8B: /* addr32 mov r/m16, r16 */
1425                        if (mode != VM86_REAL && mode != VM86_REAL_TO_PROTECTED)
1426                                goto invalid;
1427                        if ((prefix & ADDR32) == 0)
1428                                goto invalid;
1429                        if (!movr(regs, prefix, opc))
1430                                goto invalid;
1431                        return OPC_EMULATED;
1432
1433                case 0x8E: /* mov r16, sreg */
1434                        if (!mov_to_seg(regs, prefix, opc))
1435                                goto invalid;
1436                        return OPC_EMULATED;
1437
1438                case 0x8F: /* addr32 pop r/m16 */
1439                        if ((prefix & ADDR32) == 0)
1440                                goto invalid;
1441                        if (!pop(regs, prefix, opc))
1442                                goto invalid;
1443                        return OPC_EMULATED;
1444
1445                case 0x90: /* nop */
1446                        TRACE((regs, regs->eip - eip, "nop"));
1447                        return OPC_EMULATED;
1448
1449                case 0x9C: /* pushf */
1450                        TRACE((regs, regs->eip - eip, "pushf"));
1451                        if (prefix & DATA32)
1452                                push32(regs, regs->eflags & ~EFLAGS_VM);
1453                        else
1454                                push16(regs, regs->eflags & ~EFLAGS_VM);
1455                        return OPC_EMULATED;
1456
1457                case 0x9D: /* popf */
1458                        TRACE((regs, regs->eip - eip, "popf"));
1459                        if (prefix & DATA32)
1460                                regs->eflags = pop32(regs);
1461                        else
1462                                regs->eflags = (regs->eflags & 0xFFFF0000L) |
1463                                                                pop16(regs);
1464                        regs->eflags |= EFLAGS_VM;
1465                        return OPC_EMULATED;
1466
1467                case 0xA1: /* mov ax, r/m16 */
1468                        {
1469                                int addr, data;
1470                                int seg = segment(prefix, regs, regs->vds);
1471                                int offset = prefix & ADDR32? fetch32(regs) : fetch16(regs);
1472
1473                                if (prefix & DATA32) {
1474                                        addr = address(regs, seg, offset);
1475                                        data = read32(addr);
1476                                        setreg32(regs, 0, data);
1477                                } else {
1478                                        addr = address(regs, seg, offset);
1479                                        data = read16(addr);
1480                                        setreg16(regs, 0, data);
1481                                }
1482                                TRACE((regs, regs->eip - eip, "mov *0x%x, %%ax", addr));
1483                        }
1484                        return OPC_EMULATED;
1485
1486                case 0xBB: /* mov bx, imm16 */
1487                        {
1488                                int data;
1489                                if (prefix & DATA32) {
1490                                        data = fetch32(regs);
1491                                        setreg32(regs, 3, data);
1492                                } else {
1493                                        data = fetch16(regs);
1494                                        setreg16(regs, 3, data);
1495                                }
1496                                TRACE((regs, regs->eip - eip, "mov $0x%x, %%bx", data));
1497                        }
1498                        return OPC_EMULATED;
1499
1500                case 0xC6: /* addr32 movb $imm, r/m8 */
1501                        if ((prefix & ADDR32) == 0)
1502                                goto invalid;
1503                        if (!movr(regs, prefix, opc))
1504                                goto invalid;
1505                        return OPC_EMULATED;
1506
1507                case 0xCB: /* retl */
1508                        if ((mode == VM86_REAL_TO_PROTECTED) ||
1509                                (mode == VM86_PROTECTED_TO_REAL)) {
1510                                retl(regs, prefix);
1511                                return OPC_INVALID;
1512                        }
1513                        goto invalid;
1514
1515                case 0xCD: /* int $n */
1516                        TRACE((regs, regs->eip - eip, "int"));
1517                        interrupt(regs, fetch8(regs));
1518                        return OPC_EMULATED;
1519
1520                case 0xCF: /* iret */
1521                        if (prefix & DATA32) {
1522                                TRACE((regs, regs->eip - eip, "data32 iretd"));
1523                                regs->eip = pop32(regs);
1524                                regs->cs = pop32(regs);
1525                                regs->eflags = pop32(regs);
1526                        } else {
1527                                TRACE((regs, regs->eip - eip, "iret"));
1528                                regs->eip = pop16(regs);
1529                                regs->cs = pop16(regs);
1530                                regs->eflags = (regs->eflags & 0xFFFF0000L) |
1531                                                                pop16(regs);
1532                        }
1533                        return OPC_EMULATED;
1534
1535                case 0xE4: /* inb al, port */
1536                        if (!inbyte(regs, prefix, opc))
1537                                goto invalid;
1538                        return OPC_EMULATED;
1539
1540                case 0xE6: /* outb port, al */
1541                        if (!outbyte(regs, prefix, opc))
1542                                goto invalid;
1543                        return OPC_EMULATED;
1544
1545                case 0xEA: /* jmpl */
1546                        if ((mode == VM86_REAL_TO_PROTECTED) ||
1547                                (mode == VM86_PROTECTED_TO_REAL)) {
1548                                jmpl(regs, prefix);
1549                                return OPC_INVALID;
1550                        }
1551                        goto invalid;
1552
1553                case 0xFF: /* jmpl (indirect) */
1554                        {
1555                                unsigned modrm = fetch8(regs);
1556                                switch((modrm >> 3) & 7) {
1557                                case 5: /* jmpl (indirect) */
1558                                        if ((mode == VM86_REAL_TO_PROTECTED) ||
1559                                                (mode == VM86_PROTECTED_TO_REAL)) {
1560                                                jmpl_indirect(regs, prefix, modrm);
1561                                                return OPC_INVALID;
1562                                        }
1563                                        goto invalid;
1564
1565                                case 6: /* push r/m16 */
1566                                        pushrm(regs, prefix, modrm);
1567                                        return OPC_EMULATED;
1568
1569                                default:
1570                                        goto invalid;
1571                                }
1572                        }
1573
1574                case 0xEB: /* short jump */
1575                        if ((mode == VM86_REAL_TO_PROTECTED) ||
1576                                (mode == VM86_PROTECTED_TO_REAL)) {
1577                                disp = (char) fetch8(regs);
1578                                TRACE((regs, 2, "jmp 0x%x", regs->eip + disp));
1579                                regs->eip += disp;
1580                                return OPC_EMULATED;
1581                        }
1582                        goto invalid;
1583
1584                case 0xEC: /* inb al, (%dx) */
1585                        if (!inbyte(regs, prefix, opc))
1586                                goto invalid;
1587                        return OPC_EMULATED;
1588
1589                case 0xEE: /* outb (%dx), al */
1590                        if (!outbyte(regs, prefix, opc))
1591                                goto invalid;
1592                        return OPC_EMULATED;
1593
1594                case 0xF0: /* lock */
1595                        TRACE((regs, regs->eip - eip, "lock"));
1596                        continue;
1597
1598                case 0xF6: /* addr32 testb $imm, r/m8 */
1599                        if ((prefix & ADDR32) == 0)
1600                                goto invalid;
1601                        if (!test(regs, prefix, opc))
1602                                goto invalid;
1603                        return OPC_EMULATED;
1604
1605                case 0xFA: /* cli */
1606                        TRACE((regs, regs->eip - eip, "cli"));
1607                        regs->eflags &= ~EFLAGS_IF;
1608                        return OPC_EMULATED;
1609
1610                case 0xFB: /* sti */
1611                        TRACE((regs, regs->eip - eip, "sti"));
1612                        regs->eflags |= EFLAGS_IF;
1613                        return OPC_EMULATED;
1614
1615                default:
1616                        goto invalid;
1617                }
1618        }
1619
1620invalid:
1621        regs->eip = eip;
1622        TRACE((regs, regs->eip - eip, "opc 0x%x", opc));
1623        return OPC_INVALID;
1624}
1625
1626void
1627emulate(struct regs *regs)
1628{
1629        unsigned flteip;
1630        int nemul = 0;
1631
1632        /* emulate as many instructions as possible */
1633        while (opcode(regs) != OPC_INVALID)
1634                nemul++;
1635
1636        /* detect the case where we are not making progress */
1637        if (nemul == 0 && prev_eip == regs->eip) {
1638                flteip = address(regs, MASK16(regs->cs), regs->eip);
1639                panic("Unknown opcode at %04x:%04x=0x%x",
1640                        MASK16(regs->cs), regs->eip, flteip);
1641        } else
1642                prev_eip = regs->eip;
1643}
1644
1645void
1646trap(int trapno, int errno, struct regs *regs)
1647{
1648        /* emulate device interrupts */
1649        if (trapno >= NR_EXCEPTION_HANDLER) {
1650                int irq = trapno - NR_EXCEPTION_HANDLER;
1651                if (irq < 8) 
1652                        interrupt(regs, irq + 8);
1653                else
1654                        interrupt(regs, 0x70 + (irq - 8));
1655                return;
1656        }
1657
1658        switch (trapno) {
1659        case 1: /* Debug */
1660                if (regs->eflags & EFLAGS_VM) {
1661                        /* emulate any 8086 instructions  */
1662                        if (mode == VM86_REAL)
1663                                return;
1664                        if (mode != VM86_REAL_TO_PROTECTED)
1665                                panic("not in real-to-protected mode");
1666                        emulate(regs);
1667                        return;
1668                }
1669                goto invalid;
1670
1671        case 13: /* GPF */
1672                if (regs->eflags & EFLAGS_VM) {
1673                        /* emulate any 8086 instructions  */
1674                        if (mode == VM86_PROTECTED)
1675                                panic("unexpected protected mode");
1676                        emulate(regs);
1677                        return;
1678                }
1679                goto invalid;
1680
1681        default:
1682        invalid:
1683                printf("Trap (0x%x) while in %s mode\n",
1684                        trapno, regs->eflags & EFLAGS_VM ? "real" : "protected");
1685                if (trapno == 14)
1686                        printf("Page fault address 0x%x\n", get_cr2());
1687                dump_regs(regs);
1688                halt();
1689        }
1690}
Note: See TracBrowser for help on using the repository browser.