[PATCH] Make vapic.S into optional rom

[Gleb Natapov <gleb@redhat.com>]

Signed-off-by: Gleb Natapov <gleb@redhat.com>
diff --git a/Makefile b/Makefile
index ea568f5..acd9108 100644
--- a/Makefile
+++ b/Makefile
@@ -259,6 +259,7 @@ pxe-ne2k_pci.bin pxe-rtl8139.bin pxe-pcnet.bin pxe-e1000.bin \
 bamboo.dtb petalogix-s3adsp1800.dtb \
 multiboot.bin
 BLOBS += extboot.bin
+BLOBS += vapic.bin
 else
 BLOBS=
 endif
diff --git a/hw/pc.c b/hw/pc.c
index 83012a9..819b78a 100644
--- a/hw/pc.c
+++ b/hw/pc.c
@@ -53,6 +53,7 @@
 #define VGABIOS_FILENAME "vgabios.bin"
 #define VGABIOS_CIRRUS_FILENAME "vgabios-cirrus.bin"
 #define EXTBOOT_FILENAME "extboot.bin"
+#define VAPIC_FILENAME "vapic.bin"
 
 #define PC_MAX_BIOS_SIZE (4 * 1024 * 1024)
 
@@ -1149,6 +1150,7 @@ static void pc_init1(ram_addr_t ram_size,
     if (extboot_drive) {
         option_rom[nb_option_roms++] = qemu_strdup(EXTBOOT_FILENAME);
     }
+    option_rom[nb_option_roms++] = qemu_strdup(VAPIC_FILENAME);
 
     option_rom_offset = qemu_ram_alloc(PC_ROM_SIZE);
     cpu_register_physical_memory(PC_ROM_MIN_VGA, PC_ROM_SIZE, option_rom_offset);
diff --git a/kvm-tpr-opt.c b/kvm-tpr-opt.c
index 932b49b..2565d79 100644
--- a/kvm-tpr-opt.c
+++ b/kvm-tpr-opt.c
@@ -114,6 +114,7 @@ static uint32_t bios_addr;
 static uint32_t vapic_phys;
 static uint32_t bios_enabled;
 static uint32_t vbios_desc_phys;
+static uint32_t vapic_bios_addr;
 
 static void update_vbios_real_tpr(void)
 {
@@ -187,16 +188,16 @@ static int bios_is_mapped(CPUState *env, uint64_t rip)
     struct kvm_sregs sregs;
     unsigned perms;
     uint32_t i;
-    uint32_t offset, fixup;
+    uint32_t offset, fixup, start = vapic_bios_addr ? : 0xe0000;
 
     if (bios_enabled)
 	return 1;
 
     kvm_get_sregs(env, &sregs);
 
-    probe = (rip & 0xf0000000) + 0xe0000;
+    probe = (rip & 0xf0000000) + start;
     phys = map_addr(&sregs, probe, &perms);
-    if (phys != 0xe0000)
+    if (phys != start)
 	return 0;
     bios_addr = probe;
     for (i = 0; i < 64; ++i) {
@@ -356,6 +357,17 @@ static int tpr_load(QEMUFile *f, void *s, int version_id)
     return 0;
 }
 
+static void vtpr_ioport_write16(void *opaque, uint32_t addr, uint32_t val)
+{
+    struct kvm_regs regs;
+    CPUState *env = cpu_single_env;
+    struct kvm_sregs sregs;
+    kvm_get_regs(env, &regs);
+    kvm_get_sregs(env, &sregs);
+    vapic_bios_addr = ((sregs.cs.base + regs.rip) & ~(512 - 1)) + val;
+    bios_enabled = 0;
+}
+
 static void vtpr_ioport_write(void *opaque, uint32_t addr, uint32_t val)
 {
     CPUState *env = cpu_single_env;
@@ -386,5 +398,6 @@ void kvm_tpr_opt_setup(void)
 {
     register_savevm("kvm-tpr-opt", 0, 1, tpr_save, tpr_load, NULL);
     register_ioport_write(0x7e, 1, 1, vtpr_ioport_write, NULL);
+    register_ioport_write(0x7e, 2, 2, vtpr_ioport_write16, NULL);
 }
 
diff --git a/pc-bios/optionrom/Makefile b/pc-bios/optionrom/Makefile
index 73e74d8..67ecc63 100644
--- a/pc-bios/optionrom/Makefile
+++ b/pc-bios/optionrom/Makefile
@@ -13,7 +13,7 @@ CFLAGS += -I$(SRC_PATH)
 CFLAGS += $(call cc-option, $(CFLAGS), -fno-stack-protector)
 QEMU_CFLAGS = $(CFLAGS)
 
-build-all: multiboot.bin extboot.bin
+build-all: multiboot.bin extboot.bin vapic.bin
 
 %.img: %.o
 	$(call quiet-command,$(LD) -Ttext 0 -e _start -s -o $@ $<,"  Building $(TARGET_DIR)$@")
diff --git a/pc-bios/optionrom/vapic.S b/pc-bios/optionrom/vapic.S
new file mode 100644
index 0000000..1924eeb
--- /dev/null
+++ b/pc-bios/optionrom/vapic.S
@@ -0,0 +1,311 @@
+	.text 0
+	.code16
+.global _start
+_start:
+	.short 0xaa55
+	.byte (_end - _start) / 512
+	mov $vapic_base, %ax
+	out %ax, $0x7e
+	lret
+
+	.code32
+vapic_size = 2*4096
+
+.macro fixup delta=-4
+777:
+	.text 1
+	.long 777b + \delta  - vapic_base
+	.text 0
+.endm
+
+.macro reenable_vtpr
+	out %al, $0x7e
+.endm
+
+.text 1
+	fixup_start = .
+.text 0
+
+vapic_base:
+	.ascii "kvm aPiC"
+
+	/* relocation data */
+	.long vapic_base	; fixup
+	.long fixup_start	; fixup
+	.long fixup_end		; fixup
+
+	.long vapic		; fixup
+	.long vapic_size
+vcpu_shift:
+	.long 0
+real_tpr:
+	.long 0
+	.long up_set_tpr	; fixup
+	.long up_set_tpr_eax	; fixup
+	.long up_get_tpr_eax	; fixup
+	.long up_get_tpr_ecx	; fixup
+	.long up_get_tpr_edx	; fixup
+	.long up_get_tpr_ebx	; fixup
+	.long 0 /* esp. won't work. */
+	.long up_get_tpr_ebp	; fixup
+	.long up_get_tpr_esi	; fixup
+	.long up_get_tpr_edi	; fixup
+	.long up_get_tpr_stack  ; fixup
+	.long mp_set_tpr	; fixup
+	.long mp_set_tpr_eax	; fixup
+	.long mp_get_tpr_eax	; fixup
+	.long mp_get_tpr_ecx	; fixup
+	.long mp_get_tpr_edx	; fixup
+	.long mp_get_tpr_ebx	; fixup
+	.long 0 /* esp. won't work. */
+	.long mp_get_tpr_ebp	; fixup
+	.long mp_get_tpr_esi	; fixup
+	.long mp_get_tpr_edi	; fixup
+	.long mp_get_tpr_stack  ; fixup
+
+.macro kvm_hypercall
+	.byte 0x0f, 0x01, 0xc1
+.endm
+
+kvm_hypercall_vapic_poll_irq = 1
+
+pcr_cpu = 0x51
+
+.align 64
+
+mp_get_tpr_eax:
+	pushf
+	cli
+	reenable_vtpr
+	push %ecx
+
+	fs/movzbl pcr_cpu, %eax
+
+	mov vcpu_shift, %ecx	; fixup
+	shl %cl, %eax
+	testb $1, vapic+4(%eax)	; fixup delta=-5
+	jz mp_get_tpr_bad
+	movzbl vapic(%eax), %eax ; fixup
+
+mp_get_tpr_out:
+	pop %ecx
+	popf
+	ret
+
+mp_get_tpr_bad:
+	mov real_tpr, %eax	; fixup
+	mov (%eax), %eax
+	jmp mp_get_tpr_out
+
+mp_get_tpr_ebx:
+	mov %eax, %ebx
+	call mp_get_tpr_eax
+	xchg %eax, %ebx
+	ret
+
+mp_get_tpr_ecx:
+	mov %eax, %ecx
+	call mp_get_tpr_eax
+	xchg %eax, %ecx
+	ret
+
+mp_get_tpr_edx:
+	mov %eax, %edx
+	call mp_get_tpr_eax
+	xchg %eax, %edx
+	ret
+
+mp_get_tpr_esi:
+	mov %eax, %esi
+	call mp_get_tpr_eax
+	xchg %eax, %esi
+	ret
+
+mp_get_tpr_edi:
+	mov %eax, %edi
+	call mp_get_tpr_edi
+	xchg %eax, %edi
+	ret
+
+mp_get_tpr_ebp:
+	mov %eax, %ebp
+	call mp_get_tpr_eax
+	xchg %eax, %ebp
+	ret
+
+mp_get_tpr_stack:
+	call mp_get_tpr_eax
+	xchg %eax, 4(%esp)
+	ret
+
+mp_set_tpr_eax:
+	push %eax
+	call mp_set_tpr
+	ret
+
+mp_set_tpr:
+	pushf
+	push %eax
+	push %ecx
+	push %edx
+	push %ebx
+	cli
+	reenable_vtpr
+
+mp_set_tpr_failed:
+	fs/movzbl pcr_cpu, %edx
+
+	mov vcpu_shift, %ecx	; fixup
+	shl %cl, %edx
+
+	testb $1, vapic+4(%edx)	; fixup delta=-5
+	jz mp_set_tpr_bad
+
+	mov vapic(%edx), %eax	; fixup
+
+	mov %eax, %ebx
+	mov 24(%esp), %bl
+
+	/* %ebx = new vapic (%bl = tpr, %bh = isr, %b3 = irr) */
+
+	lock cmpxchg %ebx, vapic(%edx) ; fixup
+	jnz mp_set_tpr_failed
+
+	/* compute ppr */
+	cmp %bh, %bl
+	jae mp_tpr_is_bigger
+mp_isr_is_bigger:
+	mov %bh, %bl
+mp_tpr_is_bigger:
+	/* %bl = ppr */
+	mov %bl, %ch   /* ch = ppr */
+	rol $8, %ebx
+	/* now: %bl = irr, %bh = ppr */
+	cmp %bh, %bl
+	ja mp_set_tpr_poll_irq
+
+mp_set_tpr_out:
+	pop %ebx
+	pop %edx
+	pop %ecx
+	pop %eax
+	popf
+	ret $4
+
+mp_set_tpr_poll_irq:
+	mov $kvm_hypercall_vapic_poll_irq, %eax
+	kvm_hypercall
+	jmp mp_set_tpr_out
+
+mp_set_tpr_bad:
+	mov 24(%esp), %ecx
+	mov real_tpr, %eax	; fixup
+	mov %ecx, (%eax)
+	jmp mp_set_tpr_out
+
+up_get_tpr_eax:
+	reenable_vtpr
+	movzbl vapic, %eax ; fixup
+	ret
+
+up_get_tpr_ebx:
+	reenable_vtpr
+	movzbl vapic, %ebx ; fixup
+	ret
+
+up_get_tpr_ecx:
+	reenable_vtpr
+	movzbl vapic, %ecx ; fixup
+	ret
+
+up_get_tpr_edx:
+	reenable_vtpr
+	movzbl vapic, %edx ; fixup
+	ret
+
+up_get_tpr_esi:
+	reenable_vtpr
+	movzbl vapic, %esi ; fixup
+	ret
+
+up_get_tpr_edi:
+	reenable_vtpr
+	movzbl vapic, %edi ; fixup
+	ret
+
+up_get_tpr_ebp:
+	reenable_vtpr
+	movzbl vapic, %ebp ; fixup
+	ret
+
+up_get_tpr_stack:
+	reenable_vtpr
+	movzbl vapic, %eax ; fixup
+	xchg %eax, 4(%esp)
+	ret
+
+up_set_tpr_eax:
+	push %eax
+	call up_set_tpr
+	ret
+
+up_set_tpr:
+	pushf
+	push %eax
+	push %ecx
+	push %ebx
+	reenable_vtpr
+
+up_set_tpr_failed:
+	mov vapic, %eax	; fixup
+
+	mov %eax, %ebx
+	mov 20(%esp), %bl
+
+	/* %ebx = new vapic (%bl = tpr, %bh = isr, %b3 = irr) */
+
+	lock cmpxchg %ebx, vapic ; fixup
+	jnz up_set_tpr_failed
+
+	/* compute ppr */
+	cmp %bh, %bl
+	jae up_tpr_is_bigger
+up_isr_is_bigger:
+	mov %bh, %bl
+up_tpr_is_bigger:
+	/* %bl = ppr */
+	mov %bl, %ch   /* ch = ppr */
+	rol $8, %ebx
+	/* now: %bl = irr, %bh = ppr */
+	cmp %bh, %bl
+	ja up_set_tpr_poll_irq
+
+up_set_tpr_out:
+	pop %ebx
+	pop %ecx
+	pop %eax
+	popf
+	ret $4
+
+up_set_tpr_poll_irq:
+	mov $kvm_hypercall_vapic_poll_irq, %eax
+	kvm_hypercall
+	jmp up_set_tpr_out
+
+.text 1
+	fixup_end = .
+.text 0
+
+.align 128
+/*
+ * vapic format:
+ *  per-vcpu records of size 2^vcpu shift.
+ *     byte 0: tpr (r/w)
+ *     byte 1: highest in-service interrupt (isr) (r/o); bits 3:0 are zero
+ *     byte 2: zero (r/o)
+ *     byte 3: highest pending interrupt (irr) (r/o)
+ */
+.text 2
+vapic:
+. = . + vapic_size
+_end:
--
			Gleb.