[PATCH 2/2] x86/boot: set up idt for very early boot stage

[Pingfan Liu <kernelfans@gmail.com>]

The boot code becomes a little complicated, and hits some bugs, e.g.
Commit 3a63f70bf4c3a ("x86/boot: Early parse RSDP and save it in
boot_params") broke kexec boot on EFI systems.

There is few hint when bug happens. Catching the exception and printing
message can give a immediate help, instead of adding more debug_putstr() to
narraw down the problem.

At present, page fault exception handler is added. And the printed out
message looks like:
  early boot page fault:
  ENTRY(startup_64) is at: 000000047f67d200
  nip: 000000047fdeedd3
  fault address: fffffffeef6fde30

Signed-off-by: Pingfan Liu <kernelfans@gmail.com>
Cc: Thomas Gleixner <tglx@linutronix.de>
Cc: Ingo Molnar <mingo@redhat.com>
Cc: Borislav Petkov <bp@alien8.de>
Cc: "H. Peter Anvin" <hpa@zytor.com>
Cc: "Kirill A. Shutemov" <kirill.shutemov@linux.intel.com>
Cc: Cao jin <caoj.fnst@cn.fujitsu.com>
Cc: Wei Huang <wei@redhat.com>
Cc: Chao Fan <fanc.fnst@cn.fujitsu.com>
Cc: Nicolai Stange <nstange@suse.de>
Cc: Dou Liyang <douly.fnst@cn.fujitsu.com>
Cc: linux-kernel@vger.kernel.org
---
 arch/x86/boot/compressed/head_64.S | 11 +++++++
 arch/x86/boot/compressed/misc.c    | 61 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 72 insertions(+)

diff --git a/arch/x86/boot/compressed/head_64.S b/arch/x86/boot/compressed/head_64.S
index e4a25f9..f589aa2 100644
--- a/arch/x86/boot/compressed/head_64.S
+++ b/arch/x86/boot/compressed/head_64.S
@@ -527,6 +527,10 @@ relocated:
 	shrq	$3, %rcx
 	rep	stosq
 
+	pushq	%rsi			/* Save the real mode argument */
+	leaq	startup_64(%rip), %rdi
+	call	setup_early_boot_idt
+	popq	%rsi
 /*
  * Do the extraction, and jump to the new kernel..
  */
@@ -659,6 +663,13 @@ no_longmode:
 
 #include "../../kernel/verify_cpu.S"
 
+	.code64
+.align 8
+ENTRY(boot_page_fault)
+	mov	8(%rsp), %rdi
+	call	do_boot_page_fault
+	iretq
+
 	.data
 gdt64:
 	.word	gdt_end - gdt
diff --git a/arch/x86/boot/compressed/misc.c b/arch/x86/boot/compressed/misc.c
index 475a3c6..8aaa582 100644
--- a/arch/x86/boot/compressed/misc.c
+++ b/arch/x86/boot/compressed/misc.c
@@ -76,6 +76,11 @@ static int lines, cols;
 #ifdef CONFIG_KERNEL_LZ4
 #include "../../../../lib/decompress_unlz4.c"
 #endif
+
+#include "../../include/asm/desc.h"
+#include "../../include/asm/idt.h"
+#include "../../include/asm/traps.h"
+
 /*
  * NOTE: When adding a new decompressor, please update the analysis in
  * ../header.S.
@@ -429,3 +434,59 @@ void fortify_panic(const char *name)
 {
 	error("detected buffer overflow");
 }
+
+static unsigned long rt_startup_64;
+
+void do_boot_page_fault(unsigned long retaddr)
+{
+	struct desc_ptr idt = { .address = 0, .size = 0 };
+	unsigned long fault_address = read_cr2();
+
+	debug_putstr("early boot page fault:\n");
+	debug_putstr("ENTRY(startup_64) is at: ");
+	debug_puthex(rt_startup_64);
+	debug_putstr("\n");
+	debug_putstr("nip: ");
+	debug_puthex(retaddr);
+	debug_putstr("\n");
+	debug_putstr("fault address: ");
+	debug_puthex(fault_address);
+	debug_putstr("\n");
+
+	load_idt(&idt);
+}
+
+asmlinkage void boot_page_fault(void);
+
+static struct idt_data boot_idts[] = {
+	INTG(X86_TRAP_PF, 0),
+};
+
+static gate_desc early_boot_idt_table[IDT_ENTRIES] __page_aligned_bss;
+
+static struct desc_ptr early_boot_idt_descr __ro_after_init = {
+	.size		= (IDT_ENTRIES * 2 * sizeof(unsigned long)) - 1,
+};
+
+static void
+idt_setup_from_table(gate_desc *idt, const struct idt_data *t, int size)
+{
+	gate_desc desc;
+
+	for (; size > 0; t++, size--) {
+		idt_init_desc(&desc, t);
+		write_idt_entry(idt, t->vector, &desc);
+	}
+}
+
+void setup_early_boot_idt(unsigned long rip)
+{
+	rt_startup_64 = rip;
+	/* fill it with runtime address */
+	boot_idts[0].addr = boot_page_fault;
+	early_boot_idt_descr.address = (unsigned long)early_boot_idt_table;
+
+	idt_setup_from_table(early_boot_idt_table, boot_idts,
+		ARRAY_SIZE(boot_idts));
+	load_idt(&early_boot_idt_descr);
+}
-- 
2.7.4