@@ -125,10 +125,54 @@ void setup_multiboot(struct mbi_bootinfo *bi)
/* From x86/efi/efistart64.S */
extern void load_idt(void);
+extern void load_gdt_tss(size_t tss_offset);
+extern phys_addr_t tss_descr;
+extern phys_addr_t ring0stacktop;
+extern gdt_entry_t gdt64[];
+extern size_t ring0stacksize;
+
+static void setup_gdt_tss(void)
+{
+ gdt_entry_t *tss_lo, *tss_hi;
+ tss64_t *curr_tss;
+ phys_addr_t curr_tss_addr;
+ u32 id;
+ size_t tss_offset;
+ size_t pre_tss_entries;
+
+ /* Get APIC ID, see also x86/cstart64.S:load_tss */
+ id = apic_id();
+
+ /* Get number of GDT entries before TSS-related GDT entry */
+ pre_tss_entries = (size_t)((u8 *)&(tss_descr) - (u8 *)gdt64) / sizeof(gdt_entry_t);
+
+ /* Each TSS descriptor takes up 2 GDT entries */
+ tss_offset = (pre_tss_entries + id * 2) * sizeof(gdt_entry_t);
+ tss_lo = &(gdt64[pre_tss_entries + id * 2 + 0]);
+ tss_hi = &(gdt64[pre_tss_entries + id * 2 + 1]);
+
+ /* Runtime address of current TSS */
+ curr_tss_addr = (((phys_addr_t)&tss) + (phys_addr_t)(id * sizeof(tss64_t)));
+
+ /* Use runtime address for ring0stacktop, see also x86/cstart64.S:tss */
+ curr_tss = (tss64_t *)curr_tss_addr;
+ curr_tss->rsp0 = (u64)((u8*)&ring0stacktop - id * ring0stacksize);
+
+ /* Update TSS descriptors */
+ tss_lo->limit_low = sizeof(tss64_t);
+ tss_lo->base_low = (u16)(curr_tss_addr & 0xffff);
+ tss_lo->base_middle = (u8)((curr_tss_addr >> 16) & 0xff);
+ tss_lo->base_high = (u8)((curr_tss_addr >> 24) & 0xff);
+ tss_hi->limit_low = (u16)((curr_tss_addr >> 32) & 0xffff);
+ tss_hi->base_low = (u16)((curr_tss_addr >> 48) & 0xffff);
+
+ load_gdt_tss(tss_offset);
+}
void setup_efi(void)
{
reset_apic();
+ setup_gdt_tss();
setup_idt();
load_idt();
mask_pic_interrupts();
@@ -1,9 +1,24 @@
/* Startup code and pre-defined data structures */
+#include "apic-defs.h"
+#include "asm-generic/page.h"
#include "crt0-efi-x86_64.S"
.globl boot_idt
.globl idt_descr
+.globl tss_descr
+.globl gdt64_desc
+.globl ring0stacksize
+
+max_cpus = MAX_TEST_CPUS
+ring0stacksize = PAGE_SIZE
+
+.bss
+
+.globl ring0stacktop
+ . = . + ring0stacksize * max_cpus
+ .align 16
+ring0stacktop:
.data
@@ -18,6 +33,48 @@ idt_descr:
.word end_boot_idt - boot_idt - 1
.quad 0 /* To be filled with runtime addr of boot_idt(%rip) */
+gdt64_desc:
+ .word gdt64_end - gdt64 - 1
+ .quad 0 /* To be filled with runtime addr of gdt64(%rip) */
+
+.globl gdt64
+gdt64:
+ .quad 0
+ .quad 0x00af9b000000ffff /* 64-bit code segment */
+ .quad 0x00cf93000000ffff /* 32/64-bit data segment */
+ .quad 0x00af1b000000ffff /* 64-bit code segment, not present */
+ .quad 0x00cf9b000000ffff /* 32-bit code segment */
+ .quad 0x008f9b000000FFFF /* 16-bit code segment */
+ .quad 0x008f93000000FFFF /* 16-bit data segment */
+ .quad 0x00cffb000000ffff /* 32-bit code segment (user) */
+ .quad 0x00cff3000000ffff /* 32/64-bit data segment (user) */
+ .quad 0x00affb000000ffff /* 64-bit code segment (user) */
+
+ .quad 0 /* 6 spare selectors */
+ .quad 0
+ .quad 0
+ .quad 0
+ .quad 0
+ .quad 0
+
+tss_descr:
+ .rept max_cpus
+ .quad 0x000089000000ffff /* 64-bit avail tss */
+ .quad 0 /* tss high addr */
+ .endr
+gdt64_end:
+
+.globl tss
+tss:
+ .rept max_cpus
+ .long 0
+ .quad 0
+ .quad 0, 0
+ .quad 0, 0, 0, 0, 0, 0, 0, 0
+ .long 0, 0, 0
+ .endr
+tss_end:
+
.section .init
.code64
.text
@@ -32,3 +89,34 @@ load_idt:
lidtq idt_descr(%rip)
retq
+
+.globl load_gdt_tss
+load_gdt_tss:
+ /* Set GDT runtime address */
+ lea gdt64(%rip), %rax
+ mov %rax, gdt64_desc+2(%rip)
+
+ /* Load GDT */
+ lgdt gdt64_desc(%rip)
+
+ /* Load TSS */
+ mov %rdi, %rax
+ ltr %ax
+
+ /* Update data segments */
+ mov $0x10, %ax /* 3rd entry in gdt64: 32/64-bit data segment */
+ mov %ax, %ds
+ mov %ax, %es
+ mov %ax, %fs
+ mov %ax, %gs
+ mov %ax, %ss
+
+ /*
+ * Update the code segment by putting it on the stack before the return
+ * address, then doing a far return: this will use the new code segment
+ * along with the address.
+ */
+ popq %rdi
+ pushq $0x08 /* 2nd entry in gdt64: 64-bit code segment */
+ pushq %rdi
+ lretq