#include <pic.h>
#include <common.h>
#include <string_utils.h>
#include <serial.h>

static inline void outb(UINT16 port, UINT8 val) {
    ASM("outb %0, %1" : : "a"(val), "Nd"(port));
}

static inline UINT8 inb(UINT16 port) {
    UINT8 ret;
    ASM("inb %1, %0" : "=a"(ret) : "Nd"(port));
    return ret;
}

static void pic_wait(void) {
    // Small delay for slow PICs
    ASM("jmp 1f\n\t1: jmp 1f\n\t1:");
}

void pic_init(void) {
    serial_write("PIC: remapping 8259 PIC\n");

    // Save masks
    UINT8 mask1 = inb(PIC1_DATA);
    UINT8 mask2 = inb(PIC2_DATA);

    // ICW1: begin initialization, ICW4 needed
    outb(PIC1_CMD, 0x11); pic_wait();
    outb(PIC2_CMD, 0x11); pic_wait();

    // ICW2: vector offset
    outb(PIC1_DATA, PIC_IRQ_BASE);     // IRQ 0-7 → vector 0x20-0x27
    pic_wait();
    outb(PIC2_DATA, PIC_IRQ_BASE + 8); // IRQ 8-15 → vector 0x28-0x2F
    pic_wait();

    // ICW3: cascading
    outb(PIC1_DATA, 0x04); pic_wait();  // slave on IRQ 2
    outb(PIC2_DATA, 0x02); pic_wait();  // cascade identity

    // ICW4: 8086 mode
    outb(PIC1_DATA, 0x01); pic_wait();
    outb(PIC2_DATA, 0x01); pic_wait();

    // Restore masks
    outb(PIC1_DATA, mask1);
    outb(PIC2_DATA, mask2);

    serial_write("PIC: remapped IRQ 0-7 → 0x20-0x27\n");
}

void pic_send_eoi(UINT8 irq) {
    if (irq >= 8) {
        outb(PIC2_CMD, PIC_EOI);
    }
    outb(PIC1_CMD, PIC_EOI);
}

void pic_mask_irq(UINT8 irq) {
    UINT16 port = (irq < 8) ? PIC1_DATA : PIC2_DATA;
    UINT8 shift = irq % 8;
    UINT8 mask = inb(port) | (1 << shift);
    outb(port, mask);
}

void pic_unmask_irq(UINT8 irq) {
    UINT16 port = (irq < 8) ? PIC1_DATA : PIC2_DATA;
    UINT8 shift = irq % 8;
    UINT8 mask = inb(port) & ~(1 << shift);
    outb(port, mask);
}