#include "../../include/sse/fpu.h"
#include "../../include/io/serial.h"

#define COM1 0x3F8

void fpu_init(void) {
    serial_writestring("[FPU] Initializing x87 FPU...\n");

    if (!fpu_detect()) {
        serial_writestring("[FPU] No FPU detected!\n");
        return;
    }

    uint64_t cr0;
    asm volatile("mov %%cr0, %0" : "=r"(cr0));

    cr0 &= ~(1 << 2);
    cr0 |= (1 << 1);
    cr0 &= ~(1 << 3);
    cr0 &= ~(1 << 5);

    asm volatile("mov %0, %%cr0" : : "r"(cr0));

    asm volatile("finit");

    fpu_set_control_word(0x037F);

    uint64_t cr4;
    asm volatile("mov %%cr4, %0" : "=r"(cr4));
    cr4 |= (1ULL << 9);
    cr4 |= (1ULL << 10);
    asm volatile("mov %0, %%cr4" : : "r"(cr4));

    uint32_t mxcsr = 0x1F80;
    asm volatile("ldmxcsr %0" : : "m"(mxcsr));

    serial_writestring("[FPU] SSE/SSE2 enabled (CR4.OSFXSR|OSXMMEXCPT)\n");
    serial_writestring("[FPU] FPU initialized successfully\n");
}

bool fpu_detect(void) {
    uint32_t edx;
    asm volatile(
        "mov $1, %%eax\n"
        "cpuid\n"
        "mov %%edx, %0\n"
        : "=r"(edx)
        :
        : "eax", "ebx", "ecx", "edx"
    );
    return (edx & (1 << 0)) != 0;
}

void fpu_reset(void) {
    asm volatile("finit");
}

void fpu_set_control_word(uint16_t cw) {
    asm volatile("fldcw %0" : : "m"(cw));
}

uint16_t fpu_get_control_word(void) {
    uint16_t cw;
    asm volatile("fnstcw %0" : "=m"(cw));
    return cw;
}

void fpu_set_status_word(uint16_t sw) {
    (void)sw;
}

uint16_t fpu_get_status_word(void) {
    uint16_t sw;
    asm volatile("fnstsw %0" : "=m"(sw));
    return sw;
}

void fpu_set_tag_word(uint16_t tw) {
    (void)tw;
}

uint16_t fpu_get_tag_word(void) {
    struct {
        uint16_t control_word;
        uint16_t status_word;
        uint16_t tag_word;
        uint16_t fpu_ip;
        uint16_t fpu_cs;
        uint16_t fpu_opcode;
        uint16_t fpu_dp;
        uint16_t fpu_ds;
    } __attribute__((packed)) fpu_env;

    asm volatile("fstenv %0" : "=m"(fpu_env));
    return fpu_env.tag_word;
}