[PATCH 1/2] kvm-unit-tests: Add a func to run instruction in emulator

[Arthur Chunqi Li <yzt356@gmail.com>]

Add a function trap_emulator to run an instruction in emulator.
Set inregs first (%rax is invalid because it is used as return
address), put instruction codec in alt_insn and call func with
alt_insn_length. Get results in outregs.

Signed-off-by: Arthur Chunqi Li <yzt356@gmail.com>
---
 lib/libcflat.h |    1 +
 lib/string.c   |   12 +++++++++
 x86/emulator.c |   78 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 91 insertions(+)

diff --git a/lib/libcflat.h b/lib/libcflat.h
index 0875bd9..fadc33d 100644
--- a/lib/libcflat.h
+++ b/lib/libcflat.h
@@ -50,6 +50,7 @@ extern int vsnprintf(char *buf, int size, const char *fmt, va_list va);
 extern void puts(const char *s);
 
 extern void *memset(void *s, int c, size_t n);
+extern void *memcpy(void *dest, const void *src, size_t n);
 
 extern long atol(const char *ptr);
 #define ARRAY_SIZE(_a)  (sizeof(_a)/sizeof((_a)[0]))
diff --git a/lib/string.c b/lib/string.c
index 9dc94a1..e798f86 100644
--- a/lib/string.c
+++ b/lib/string.c
@@ -42,6 +42,18 @@ void *memset(void *s, int c, size_t n)
     return s;
 }
 
+void *memcpy(void *dest, const void *src, size_t n)
+{
+    size_t i;
+    char *a = dest;
+    char *b = src;
+
+    for (i = 0; i < n; ++i)
+        a[i] = b[i];
+
+    return dest;
+}
+
 long atol(const char *ptr)
 {
     long acc = 0;
diff --git a/x86/emulator.c b/x86/emulator.c
index 96576e5..b3626fa 100644
--- a/x86/emulator.c
+++ b/x86/emulator.c
@@ -11,6 +11,15 @@ int fails, tests;
 
 static int exceptions;
 
+struct regs {
+	u64 rax, rbx, rcx, rdx;
+	u64 rsi, rdi, rsp, rbp;
+	u64 r8, r9, r10, r11;
+	u64 r12, r13, r14, r15;
+	u64 rip, rflags;
+};
+struct regs inregs, outregs, save;
+
 void report(const char *name, int result)
 {
 	++tests;
@@ -685,6 +694,75 @@ static void test_shld_shrd(u32 *mem)
     report("shrd (cl)", *mem == ((0x12345678 >> 3) | (5u << 29)));
 }
 
+#define INSN_XCHG_ALL			\
+	"xchg %rax, 0+save \n\t"		\
+	"xchg %rbx, 8+save \n\t"		\
+	"xchg %rcx, 16+save \n\t"		\
+	"xchg %rdx, 24+save \n\t"		\
+	"xchg %rsi, 32+save \n\t"		\
+	"xchg %rdi, 40+save \n\t"		\
+	"xchg %rsp, 48+save \n\t"		\
+	"xchg %rbp, 56+save \n\t"		\
+	"xchg %r8, 64+save \n\t"		\
+	"xchg %r9, 72+save \n\t"		\
+	"xchg %r10, 80+save \n\t"		\
+	"xchg %r11, 88+save \n\t"		\
+	"xchg %r12, 96+save \n\t"		\
+	"xchg %r13, 104+save \n\t"		\
+	"xchg %r14, 112+save \n\t"		\
+	"xchg %r15, 120+save \n\t"		\
+
+asm(
+	".align 4096\n\t"
+	"insn_page:\n\t"
+	"ret\n\t"
+	"pushf\n\t"
+	"push 136+save \n\t"
+	"popf \n\t"
+	INSN_XCHG_ALL
+	"test_insn:\n\t"
+	"in  (%dx),%al\n\t"
+	".skip 31, 0x90\n\t"
+	"test_insn_end:\n\t"
+	INSN_XCHG_ALL
+	"pushf \n\t"
+	"pop 136+save \n\t"
+	"popf \n\t"
+	"ret \n\t"
+	"insn_page_end:\n\t"
+	".align 4096\n\t"
+
+	"alt_insn_page:\n\t"
+	". = . + 4096\n\t"
+	".align 4096\n\t"
+);
+
+static void trap_emulator(uint64_t *mem, uint8_t* alt_insn, int alt_insn_length)
+{
+	ulong *cr3 = (ulong *)read_cr3();
+	void *insn_ram;
+	extern u8 insn_page[], test_insn[], alt_insn_page[];
+
+	insn_ram = vmap(virt_to_phys(insn_page), 4096);
+	memcpy(alt_insn_page, test_insn, 4096);
+	memcpy(alt_insn_page + (test_insn - insn_page), alt_insn, alt_insn_length);
+	save = inregs;
+
+	/* Load the code TLB with insn_page, but point the page tables at
+	   alt_insn_page (and keep the data TLB clear, for AMD decode assist).
+	   This will make the CPU trap on the insn_page instruction but the
+	   hypervisor will see alt_insn_page. */
+	install_page(cr3, virt_to_phys(insn_page), insn_ram);
+	invlpg(insn_ram);
+	/* Load code TLB */
+	asm volatile("call *%0" : : "r"(insn_ram));
+	install_page(cr3, virt_to_phys(alt_insn_page), insn_ram);
+	/* Trap, let hypervisor emulate at alt_insn_page */
+	asm volatile("call *%0": : "r"(insn_ram+1));
+
+	outregs = save;
+}
+
 static void advance_rip_by_3_and_note_exception(struct ex_regs *regs)
 {
     ++exceptions;
-- 
1.7.9.5