1 files changed, 74 insertions, 24 deletions

diff --git a/libc/cpu.c b/libc/cpu.c
index 52c0280..2eb6c46 100644
--- a/libc/cpu.c
+++ b/libc/cpu.c
@@ -48,41 +48,91 @@ void outl(u16 port, u32 data)
 	__asm__ volatile("outl %0, %1" ::"a"(data), "Nd"(port));
 }
 
-void cpuid(int code, u32 *a, u32 *b, u32 *c, u32 *d)
+#ifdef kernel
+
+static void cpuid(int code, u32 *a, u32 *b, u32 *c, u32 *d)
 {
 	__asm__ volatile("cpuid" : "=a"(*a), "=b"(*b), "=c"(*c), "=d"(*d) : "a"(code));
 }
 
-char *cpu_string(char buf[13])
-{
-	u32 a, b, c, d;
-	cpuid(CPUID_VENDOR_STRING, &a, &b, &c, &d);
-	char *ebx = (char *)&b;
-	char *ecx = (char *)&c;
-	char *edx = (char *)&d;
-	buf[0] = ebx[0];
-	buf[1] = ebx[1];
-	buf[2] = ebx[2];
-	buf[3] = ebx[3];
-	buf[4] = edx[0];
-	buf[5] = edx[1];
-	buf[6] = edx[2];
-	buf[7] = edx[3];
-	buf[8] = ecx[0];
-	buf[9] = ecx[1];
-	buf[10] = ecx[2];
-	buf[11] = ecx[3];
-	buf[12] = 0;
+static char *cpu_string(char buf[16])
+{
+	// wtf
+	cpuid(CPUID_VENDOR_STRING, (u32 *)(buf + 12), (u32 *)(buf), (u32 *)(buf + 8),
+	      (u32 *)(buf + 4));
+
 	return buf;
 }
 
 void cpu_print(void)
 {
-	char buf[13] = { 0 };
-	printf("%s\n", cpu_string(buf));
+	char buf[16] = { 0 };
+	printf("CPU vendor: %s\n", cpu_string(buf));
+}
+
+static u32 cr0_get(void)
+{
+	u32 cr0;
+	__asm__ volatile("movl %%cr0, %%eax" : "=a"(cr0));
+	return cr0;
+}
+
+static void cr0_set(u32 cr0)
+{
+	__asm__ volatile("movl %%eax, %%cr0" ::"a"(cr0));
+}
+
+static u32 cr4_get(void)
+{
+	u32 cr4;
+	__asm__ volatile("movl %%cr4, %%eax" : "=a"(cr4));
+	return cr4;
+}
+
+static void cr4_set(u32 cr4)
+{
+	__asm__ volatile("movl %%eax, %%cr4" ::"a"(cr4));
+}
+
+static u32 cpu_features = 0;
+u8 cpu_has_feature(u32 feature)
+{
+	return (cpu_features & feature) != 0;
+}
+
+void fpu_handler()
+{
+	__asm__ volatile("clts");
+}
+
+static u8 fpu_state[512] __attribute__((aligned(16)));
+void fpu_restore(void)
+{
+	__asm__ volatile("fxrstor (%0)" ::"r"(fpu_state));
+}
+
+void cpu_enable_features(void)
+{
+	u32 a = 0, b = 0, c = 0, d = 0;
+	cpuid(CPUID_FEATURES, &a, &b, &c, &d);
+	cpu_features = d;
+	if (cpu_has_feature(CPUID_FEAT_EDX_SSE)) {
+		cr0_set(cr0_get() & ~(1 << 2));
+		cr0_set(cr0_get() | (1 << 1));
+		cr4_set(cr4_get() | (3 << 9));
+	} else {
+		panic("No SSE support!\n");
+	}
+
+	if (cpu_has_feature(CPUID_FEAT_EDX_FPU)) {
+		__asm__ volatile("fninit");
+		__asm__ volatile("fxsave %0" : "=m"(fpu_state));
+		irq_install_handler(7, fpu_handler);
+	} else {
+		panic("No FPU support!\n");
+	}
 }
 
-#ifdef kernel
 void cli(void)
 {
 	__asm__ volatile("cli");