x86_64: implement TSS initialisation and user/kernel stack pointer switching

arch/x86_64/cpu.c

@@ -1,11 +1,14 @@
-#include <mango/machine/cpu.h>
 #include <arch/msr.h>
+#include <mango/machine/cpu.h>
 
 int ml_cpu_block_init(ml_cpu_block *p)
 {
 	p->c_this = p;
 	gdt_init(&p->c_gdt, &p->c_gdt_ptr);
 	idt_init(&p->c_idt_ptr);
+	tss_init(&p->c_tss, &p->c_tss_ptr);
+
+	gdt_write_tss(&p->c_gdt, &p->c_tss);
 	return 0;
 }
 
@@ -13,6 +16,27 @@ int ml_cpu_block_use(ml_cpu_block *p)
 {
 	gdt_load(&p->c_gdt_ptr);
 	idt_load(&p->c_idt_ptr);
+	tss_load(&p->c_tss);
 	wrmsr(MSR_GS_BASE, (uint64_t)p);
 	return 0;
 }
+
+virt_addr_t ml_cpu_block_get_kstack(ml_cpu_block *p)
+{
+	return tss_get_kstack(&p->c_tss);
+}
+
+virt_addr_t ml_cpu_block_get_ustack(ml_cpu_block *p)
+{
+	return tss_get_ustack(&p->c_tss);
+}
+
+void ml_cpu_block_set_kstack(ml_cpu_block *p, virt_addr_t sp)
+{
+	tss_set_kstack(&p->c_tss, sp);
+}
+
+void ml_cpu_block_set_ustack(ml_cpu_block *p, virt_addr_t sp)
+{
+	tss_set_ustack(&p->c_tss, sp);
+}

arch/x86_64/gdt.c

@@ -1,5 +1,8 @@
-#include <mango/libc/string.h>
 #include <arch/gdt.h>
+#include <arch/tss.h>
+#include <mango/libc/string.h>
+#include <mango/types.h>
+#include <stddef.h>
 
 static void init_entry(struct gdt_entry *entry, int access, int flags)
 {
@@ -15,13 +18,42 @@ int gdt_init(struct gdt *gdt, struct gdt_ptr *gdtp)
 {
 	memset(&gdt->g_entries[0], 0x0, sizeof gdt->g_entries[0]);
 
-	init_entry(&gdt->g_entries[1], GDT_A_PRESENT | GDT_A_CODEREAD | GDT_A_CODE, GDT_F_64BIT);
+	init_entry(
-	init_entry(&gdt->g_entries[2], GDT_A_PRESENT | GDT_A_DATAWRITE | GDT_A_DATA, GDT_F_64BIT);
+		&gdt->g_entries[1],
-	init_entry(&gdt->g_entries[3], GDT_A_PRESENT | GDT_A_USER | GDT_A_CODEREAD | GDT_A_CODE, GDT_F_64BIT);
+		GDT_A_PRESENT | GDT_A_CODEREAD | GDT_A_CODE,
-	init_entry(&gdt->g_entries[4], GDT_A_PRESENT | GDT_A_USER | GDT_A_DATAWRITE | GDT_A_DATA, GDT_F_64BIT);
+		GDT_F_64BIT);
+	init_entry(
+		&gdt->g_entries[2],
+		GDT_A_PRESENT | GDT_A_DATAWRITE | GDT_A_DATA,
+		GDT_F_64BIT);
+	init_entry(
+		&gdt->g_entries[3],
+		GDT_A_PRESENT | GDT_A_USER | GDT_A_CODEREAD | GDT_A_CODE,
+		GDT_F_64BIT);
+	init_entry(
+		&gdt->g_entries[4],
+		GDT_A_PRESENT | GDT_A_USER | GDT_A_DATAWRITE | GDT_A_DATA,
+		GDT_F_64BIT);
 
 	gdtp->g_ptr = (uint64_t)gdt;
 	gdtp->g_limit = sizeof(*gdt) - 1;
 
 	return 0;
 }
+
+void gdt_write_tss(struct gdt *gdt, struct tss *tss)
+{
+	struct tss_gdt_entry *tss_entry = &gdt->g_tss;
+
+	virt_addr_t base = (virt_addr_t)tss;
+	size_t limit = sizeof *tss;
+
+	tss_entry->ge_base_low = (base & 0xFFFF);
+	tss_entry->ge_base_mid = (base >> 16) & 0xFF;
+	tss_entry->ge_base_hi = (base >> 24) & 0xFF;
+	tss_entry->ge_base_ext = (base >> 32) & 0xFFFFFFFF;
+	tss_entry->ge_limit_low = (limit & 0xFFFF);
+	tss_entry->ge_gran = (limit >> 16) & 0xF;
+	tss_entry->ge_access = 0xE9;
+	tss_entry->ge_reserved = 0;
+}

arch/x86_64/include/arch/gdt.h

@@ -1,6 +1,7 @@
 #ifndef ARCH_GDT_H_
 #define ARCH_GDT_H_
 
+#include <arch/tss.h>
 #include <mango/compiler.h>
 #include <stdint.h>
 
@@ -33,6 +34,7 @@ struct gdt_entry {
 
 struct gdt {
 	struct gdt_entry g_entries[NR_GDT_ENTRIES];
+	struct tss_gdt_entry g_tss;
 } __packed;
 
 struct gdt_ptr {
@@ -41,6 +43,7 @@ struct gdt_ptr {
 } __packed;
 
 extern int gdt_init(struct gdt *gdt, struct gdt_ptr *gdtp);
+extern void gdt_write_tss(struct gdt *gdt, struct tss *tss);
 extern int gdt_load(struct gdt_ptr *gdtp);
 
 #ifdef __cplusplus

arch/x86_64/include/arch/tss.h

@@ -0,0 +1,55 @@
+#ifndef ARCH_TSS_H_
+#define ARCH_TSS_H_
+
+#include <mango/compiler.h>
+#include <mango/types.h>
+#include <stdint.h>
+
+#define TSS_GDT_INDEX 5
+
+struct tss {
+	uint32_t res0;
+	uint64_t rsp0;
+	uint64_t rsp1;
+	uint64_t rsp2;
+	uint64_t res1;
+	uint64_t ist1;
+	uint64_t ist2;
+	uint64_t ist3;
+	uint64_t ist4;
+	uint64_t ist5;
+	uint64_t ist6;
+	uint64_t ist7;
+	uint64_t res2;
+	uint16_t res3;
+	uint16_t iomap_offset;
+} __packed;
+
+struct tss_gdt_entry {
+	/* these fields are copied from struct gdt_entry */
+	uint16_t ge_limit_low;
+	uint16_t ge_base_low;
+	uint8_t ge_base_mid;
+	uint8_t ge_access;
+	uint8_t ge_gran;
+	uint8_t ge_base_hi;
+	/* these fields are specific to the TSS entry */
+	uint32_t ge_base_ext;
+	uint32_t ge_reserved;
+} __packed;
+
+struct tss_ptr {
+	uint16_t tss_limit;
+	uint64_t tss_base;
+} __packed;
+
+extern void tss_init(struct tss *tss, struct tss_ptr *ptr);
+extern void tss_load(struct tss *tss);
+
+extern virt_addr_t tss_get_kstack(struct tss *tss);
+extern virt_addr_t tss_get_ustack(struct tss *tss);
+
+extern void tss_set_kstack(struct tss *tss, virt_addr_t sp);
+extern void tss_set_ustack(struct tss *tss, virt_addr_t sp);
+
+#endif

arch/x86_64/include/mango/machine/cpu.h

@@ -21,6 +21,9 @@ typedef struct ml_cpu_block {
 	struct gdt c_gdt;
 	struct gdt_ptr c_gdt_ptr;
 
+	struct tss c_tss;
+	struct tss_ptr c_tss_ptr;
+
 	struct idt_ptr c_idt_ptr;
 	unsigned int c_cpu_id;
 
@@ -45,6 +48,12 @@ extern int ml_init_bootcpu(void);
 extern int ml_cpu_block_init(ml_cpu_block *p);
 extern int ml_cpu_block_use(ml_cpu_block *p);
 
+extern virt_addr_t ml_cpu_block_get_ustack(ml_cpu_block *p);
+extern virt_addr_t ml_cpu_block_get_kstack(ml_cpu_block *p);
+
+extern void ml_cpu_block_set_ustack(ml_cpu_block *p, virt_addr_t sp);
+extern void ml_cpu_block_set_kstack(ml_cpu_block *p, virt_addr_t sp);
+
 /* defined in cpu_ctrl.S */
 extern void ml_halt_cpu(void);
 extern ml_cpu_block *ml_this_cpu(void);

arch/x86_64/include/mango/machine/init.h

@@ -1,6 +1,7 @@
 #ifndef MANGO_X86_64_INIT_H_
 #define MANGO_X86_64_INIT_H_
 
+#include <stddef.h>
 #include <stdint.h>
 
 #ifdef __cplusplus

arch/x86_64/tss.c

@@ -0,0 +1,48 @@
+#include "arch/msr.h"
+
+#include <arch/gdt.h>
+#include <arch/tss.h>
+#include <mango/libc/string.h>
+
+static void tss_flush(int index)
+{
+	index *= sizeof(struct gdt_entry);
+	index |= 3;
+	asm volatile("mov %0, %%eax; ltr %%ax" ::"r"(index));
+}
+
+void tss_init(struct tss *tss, struct tss_ptr *ptr)
+{
+	memset(tss, 0x0, sizeof *tss);
+
+	ptr->tss_base = (uint64_t)tss;
+	ptr->tss_limit = (uint16_t)sizeof *tss;
+}
+
+void tss_load(struct tss *tss)
+{
+	tss_flush(TSS_GDT_INDEX);
+
+	uintptr_t kernel_gs_base_reg = 0xC0000102;
+	wrmsr(kernel_gs_base_reg, (uintptr_t)tss);
+}
+
+virt_addr_t tss_get_kstack(struct tss *tss)
+{
+	return tss->rsp0;
+}
+
+virt_addr_t tss_get_ustack(struct tss *tss)
+{
+	return tss->rsp2;
+}
+
+void tss_set_kstack(struct tss *tss, virt_addr_t sp)
+{
+	tss->rsp0 = sp;
+}
+
+void tss_set_ustack(struct tss *tss, virt_addr_t sp)
+{
+	tss->rsp2 = sp;
+}