Phase 2: GDT/IDT, allocator, self-test

#include "gdt.h"

#include <stdint.h>

/* The kernel owns its GDT rather than relying on the one stage2 built on
 * the real-mode stack - stage2's copy happens to still be valid at this
 * point, but the kernel should not depend on bootloader memory. */

struct gdt_entry {
    uint16_t limit_low;
    uint16_t base_low;
    uint8_t  base_mid;
    uint8_t  access;
    uint8_t  granularity;
    uint8_t  base_high;
} __attribute__((packed));

struct gdt_ptr {
    uint16_t limit;
    uint32_t base;
} __attribute__((packed));

static struct gdt_entry gdt[3];
static struct gdt_ptr gdtp;

static void gdt_set_gate(int n, uint32_t base, uint32_t limit, uint8_t access, uint8_t gran) {
    gdt[n].base_low = base & 0xFFFF;
    gdt[n].base_mid = (base >> 16) & 0xFF;
    gdt[n].base_high = (base >> 24) & 0xFF;
    gdt[n].limit_low = limit & 0xFFFF;
    gdt[n].granularity = (uint8_t)(((limit >> 16) & 0x0F) | (gran & 0xF0));
    gdt[n].access = access;
}

void gdt_init(void) {
    gdtp.limit = sizeof(gdt) - 1;
    gdtp.base = (uint32_t)&gdt;

    gdt_set_gate(0, 0, 0, 0, 0);            /* null descriptor */
    gdt_set_gate(1, 0, 0xFFFFF, 0x9A, 0xC0); /* 0x08: code, ring0, 4GB flat */
    gdt_set_gate(2, 0, 0xFFFFF, 0x92, 0xC0); /* 0x10: data, ring0, 4GB flat */

    __asm__ volatile (
        "lgdt %0\n"
        "mov $0x10, %%ax\n"
        "mov %%ax, %%ds\n"
        "mov %%ax, %%es\n"
        "mov %%ax, %%fs\n"
        "mov %%ax, %%gs\n"
        "mov %%ax, %%ss\n"
        "ljmp $0x08, $1f\n"
        "1:\n"
        :
        : "m"(gdtp)
        : "eax"
    );
}

; Exception-only ISR stubs (vectors 0-31). This kernel is single-tasking and
; polling-only - interrupts are never enabled (no `sti`, no PIC remap, no
; IRQ handlers) - so the only things that can land here are synchronous CPU
; exceptions. There is no recovery path: each stub reports the vector/error
; code to the C handler and then halts forever, so no iret/register
; restore is needed.

BITS 32
section .text

extern isr_handler

%macro ISR_NOERR 1
global isr_stub_%1
isr_stub_%1:
    push dword 0
    push dword %1
    jmp isr_common
%endmacro

%macro ISR_ERR 1
global isr_stub_%1
isr_stub_%1:
    push dword %1
    jmp isr_common
%endmacro

isr_common:
    ; stack: [vector] [error_code] ...  (both macros leave this layout)
    mov eax, [esp]
    mov ebx, [esp+4]
    push ebx
    push eax
    call isr_handler
.hang:
    hlt
    jmp .hang

ISR_NOERR 0
ISR_NOERR 1
ISR_NOERR 2
ISR_NOERR 3
ISR_NOERR 4
ISR_NOERR 5
ISR_NOERR 6
ISR_NOERR 7
ISR_ERR   8
ISR_NOERR 9
ISR_ERR   10
ISR_ERR   11
ISR_ERR   12
ISR_ERR   13
ISR_ERR   14
ISR_NOERR 15
ISR_NOERR 16
ISR_ERR   17
ISR_NOERR 18
ISR_NOERR 19
ISR_NOERR 20
ISR_NOERR 21
ISR_NOERR 22
ISR_NOERR 23
ISR_NOERR 24
ISR_NOERR 25
ISR_NOERR 26
ISR_NOERR 27
ISR_NOERR 28
ISR_NOERR 29
ISR_NOERR 30
ISR_NOERR 31

section .data
global isr_stub_table
isr_stub_table:
%assign i 0
%rep 32
    dd isr_stub_%[i]
%assign i i+1
%endrep

#include "string.h"

void *memcpy(void *dst, const void *src, size_t n) {
    uint8_t *d = dst;
    const uint8_t *s = src;
    for (size_t i = 0; i < n; i++) {
        d[i] = s[i];
    }
    return dst;
}

void *memset(void *dst, int val, size_t n) {
    uint8_t *d = dst;
    for (size_t i = 0; i < n; i++) {
        d[i] = (uint8_t)val;
    }
    return dst;
}

size_t strlen(const char *s) {
    size_t n = 0;
    while (s[n]) {
        n++;
    }
    return n;
}

int strcmp(const char *a, const char *b) {
    while (*a && (*a == *b)) {
        a++;
        b++;
    }
    return (unsigned char)*a - (unsigned char)*b;
}

char *itoa(int value, char *buf, int base) {
    char tmp[34];
    int i = 0;
    int negative = (base == 10 && value < 0);
    unsigned int uval = negative ? (unsigned int)(-value) : (unsigned int)value;

    if (uval == 0) {
        tmp[i++] = '0';
    }
    while (uval) {
        int digit = (int)(uval % (unsigned int)base);
        tmp[i++] = (char)(digit < 10 ? '0' + digit : 'a' + (digit - 10));
        uval /= (unsigned int)base;
    }
    if (negative) {
        tmp[i++] = '-';
    }

    int j = 0;
    while (i > 0) {
        buf[j++] = tmp[--i];
    }
    buf[j] = '\0';
    return buf;
}

#include "idt.h"

#include <stdint.h>

#include "console.h"
#include "serial.h"
#include "string.h"

struct idt_entry {
    uint16_t offset_low;
    uint16_t selector;
    uint8_t  zero;
    uint8_t  type_attr;
    uint16_t offset_high;
} __attribute__((packed));

struct idt_ptr {
    uint16_t limit;
    uint32_t base;
} __attribute__((packed));

extern uint32_t isr_stub_table[32];

static struct idt_entry idt[256];
static struct idt_ptr idtp;

static void idt_set_gate(int n, uint32_t handler) {
    idt[n].offset_low = handler & 0xFFFF;
    idt[n].selector = 0x08; /* kernel code segment, see gdt.c */
    idt[n].zero = 0;
    idt[n].type_attr = 0x8E; /* present, ring0, 32-bit interrupt gate */
    idt[n].offset_high = (handler >> 16) & 0xFFFF;
}

void idt_init(void) {
    idtp.limit = sizeof(idt) - 1;
    idtp.base = (uint32_t)&idt;

    for (int i = 0; i < 32; i++) {
        idt_set_gate(i, isr_stub_table[i]);
    }

    __asm__ volatile ("lidt %0" : : "m"(idtp));
}

/* Called from idt_stubs.S. Never returns - the stub halts right after. */
void isr_handler(uint32_t vector, uint32_t error_code) {
    char numbuf[16];

    console_write("\nEXCEPTION vector=");
    console_write(itoa((int)vector, numbuf, 10));
    console_write(" error_code=");
    console_write(itoa((int)error_code, numbuf, 10));
    console_write(" - halted\n");

    serial_write("\nEXCEPTION vector=");
    serial_write(itoa((int)vector, numbuf, 10));
    serial_write(" error_code=");
    serial_write(itoa((int)error_code, numbuf, 10));
    serial_write(" - halted\n");
}

#include "memory.h"

#include <stdint.h>

/* Fixed heap region. Starts at 1MB to stay clear of the kernel image
 * (loaded at 0x10000), the boot stack (0x90000), and the legacy BIOS/VGA
 * memory holes below 1MB. QEMU's default RAM is well above this. */
#define HEAP_START 0x100000u
#define HEAP_SIZE  (1u * 1024u * 1024u)

typedef struct block_header {
    size_t size; /* usable size, excluding this header */
    int free;
    struct block_header *next;
} block_header_t;

static block_header_t *heap_head;

void memory_init(void) {
    heap_head = (block_header_t *)HEAP_START;
    heap_head->size = HEAP_SIZE - sizeof(block_header_t);
    heap_head->free = 1;
    heap_head->next = NULL;
}

void *kmalloc(size_t size) {
    block_header_t *cur = heap_head;

    while (cur) {
        if (cur->free && cur->size >= size) {
            size_t remaining = cur->size - size;
            if (remaining >= sizeof(block_header_t) + 16) {
                block_header_t *split =
                    (block_header_t *)((uint8_t *)cur + sizeof(block_header_t) + size);
                split->size = remaining - sizeof(block_header_t);
                split->free = 1;
                split->next = cur->next;
                cur->next = split;
                cur->size = size;
            }
            cur->free = 0;
            return (void *)((uint8_t *)cur + sizeof(block_header_t));
        }
        cur = cur->next;
    }
    return NULL;
}

void kfree(void *ptr) {
    if (!ptr) {
        return;
    }

    block_header_t *blk = (block_header_t *)((uint8_t *)ptr - sizeof(block_header_t));
    blk->free = 1;

    /* Coalesce adjacent free blocks. The list is address-ordered since
     * kmalloc only ever splits forward, so a single forward pass suffices. */
    block_header_t *cur = heap_head;
    while (cur && cur->next) {
        if (cur->free && cur->next->free) {
            cur->size += sizeof(block_header_t) + cur->next->size;
            cur->next = cur->next->next;
        } else {
            cur = cur->next;
        }
    }
}

#include "selftest.h"

#include "console.h"
#include "memory.h"
#include "serial.h"
#include "string.h"

static int ok = 1;

static void check(int cond, const char *what) {
    if (!cond) {
        ok = 0;
        console_write("  FAIL: ");
        console_write(what);
        console_write("\n");
        serial_write("  FAIL: ");
        serial_write(what);
        serial_write("\n");
    }
}

static void test_string(void) {
    char buf[16];

    check(strlen("hello") == 5, "strlen");
    check(strcmp("abc", "abc") == 0, "strcmp equal");
    check(strcmp("abc", "abd") != 0, "strcmp differs");

    itoa(12345, buf, 10);
    check(strcmp(buf, "12345") == 0, "itoa decimal positive");

    itoa(-42, buf, 10);
    check(strcmp(buf, "-42") == 0, "itoa decimal negative");

    itoa(255, buf, 16);
    check(strcmp(buf, "ff") == 0, "itoa hex");
}

static void test_memory(void) {
    memory_init();

    void *a = kmalloc(64);
    void *b = kmalloc(128);
    check(a != NULL && b != NULL, "kmalloc basic");
    check(a != b, "kmalloc distinct pointers");

    memset(a, 0xAB, 64);
    uint8_t *pa = (uint8_t *)a;
    int pattern_ok = 1;
    for (int i = 0; i < 64; i++) {
        if (pa[i] != 0xAB) {
            pattern_ok = 0;
        }
    }
    check(pattern_ok, "kmalloc write/readback");

    kfree(a);
    kfree(b);

    /* After freeing everything, a large allocation should succeed again -
     * proves coalescing merged the freed blocks back together. */
    void *c = kmalloc(HEAP_SIZE_FOR_TEST - 1024);
    check(c != NULL, "kmalloc large after free (coalescing)");
    kfree(c);
}

int selftest_run(void) {
    ok = 1;
    console_write("selftest: running\n");
    serial_write("selftest: running\n");

    test_string();
    test_memory();

    if (ok) {
        console_write("SELFTEST PASS\n");
        serial_write("SELFTEST PASS\n");
    } else {
        console_write("SELFTEST FAIL\n");
        serial_write("SELFTEST FAIL\n");
    }
    return ok;
}