ksOS/Bootloader/Source/main.c

#include "modules/uefi/uefi.h"
#include "modules/elf/elf.h"
#include "../../Common/bootinfo.h"

#define PAGE_SIZE 0x1000
#define WSTR(str) ((wchar_t*)L##str)

static wchar_t* kernel_path = WSTR("ksOSKernel.elf");
static efi_guid_t dtb_guid = {
    0xb1b621d5, 0xf19c, 0x41a5, {0x83, 0x0b, 0xd9, 0x15, 0x2c, 0x69, 0xaa, 0xe0}
};

static void print(const wchar_t* msg) {
    ST->ConOut->OutputString(ST->ConOut, (wchar_t*)msg);
}

void* memset(void* destination, int value, uint64_t count) {
    uint8_t* ptr = (uint8_t*)destination;
    while (count--) {
        *ptr++ = (uint8_t)value;
    }
    return destination;
}

static efi_status_t die(void) {
    while (1) {
        __asm__ volatile("wfi");
    }

    return EFI_ABORTED;
}

static efi_status_t fail(const wchar_t* msg) {
    print(msg);
    return die();
}

static int guid_equal(const efi_guid_t* lhs, const efi_guid_t* rhs) {
    const uint8_t* lhs_bytes = (const uint8_t*)lhs;
    const uint8_t* rhs_bytes = (const uint8_t*)rhs;

    for (int i = 0; i < 16; ++i) {
        if (lhs_bytes[i] != rhs_bytes[i]) {
            return 0;
        }
    }

    return 1;
}

static void* find_configuration_table(const efi_guid_t* guid) {
    for (uintn_t i = 0; i < ST->NumberOfTableEntries; ++i) {
        if (guid_equal(&ST->ConfigurationTable[i].VendorGuid, guid)) {
            return ST->ConfigurationTable[i].VendorTable;
        }
    }

    return NULL;
}

static efi_status_t open_root_volume(efi_file_handle_t** root) {
    efi_loaded_image_protocol_t* loaded_image = NULL;
    efi_guid_t loaded_image_guid = EFI_LOADED_IMAGE_PROTOCOL_GUID;
    efi_status_t status = gBS->HandleProtocol(IM, &loaded_image_guid, (void**)&loaded_image);
    if (EFI_ERROR(status)) {
        return status;
    }

    efi_simple_file_system_protocol_t* fs = NULL;
    efi_guid_t filesystem_guid = EFI_SIMPLE_FILE_SYSTEM_PROTOCOL_GUID;
    status = gBS->HandleProtocol(loaded_image->DeviceHandle, &filesystem_guid, (void**)&fs);
    if (EFI_ERROR(status)) {
        return status;
    }

    return fs->OpenVolume(fs, root);
}

static efi_status_t read_file_info(efi_file_handle_t* file, efi_file_info_t** file_info) {
    efi_guid_t file_info_guid = EFI_FILE_INFO_GUID;
    uintn_t file_info_size = 0;
    efi_status_t status = file->GetInfo(file, &file_info_guid, &file_info_size, NULL);
    if (status != EFI_BUFFER_TOO_SMALL) {
        return status;
    }

    status = gBS->AllocatePool(EfiLoaderData, file_info_size, (void**)file_info);
    if (EFI_ERROR(status)) {
        return status;
    }

    return file->GetInfo(file, &file_info_guid, &file_info_size, *file_info);
}

static efi_status_t load_kernel(efi_file_handle_t* root, efi_physical_address_t* kernel_addr, uint64_t* kernel_size, efi_file_handle_t** out_handle) {
    efi_file_handle_t* kernel_file = NULL;
    efi_status_t status = root->Open(root, &kernel_file, kernel_path, EFI_FILE_MODE_READ, 0);
    if (EFI_ERROR(status)) return status;

    efi_file_info_t* kernel_info = NULL;
    status = read_file_info(kernel_file, &kernel_info);
    if (EFI_ERROR(status)) return status;

    *kernel_size = kernel_info->FileSize;
    uintn_t kernel_pages = (*kernel_size + PAGE_SIZE - 1) / PAGE_SIZE;

    status = gBS->AllocatePages(AllocateAnyPages, EfiLoaderData, kernel_pages, kernel_addr);
    if (EFI_ERROR(status)) return status;

    uintn_t bytes_to_read = (uintn_t)*kernel_size;
    status = kernel_file->Read(kernel_file, &bytes_to_read, (void*)*kernel_addr);
    if (EFI_ERROR(status)) return status;

    *out_handle = kernel_file;
    return EFI_SUCCESS;
}

static efi_status_t parse_elf_headers(efi_physical_address_t kernel_addr) {
    Elf64_Ehdr* elf_header = (Elf64_Ehdr*)kernel_addr;

    if (elf_header->e_ident[0] != 0x7f || elf_header->e_ident[1] != 'E' ||
        elf_header->e_ident[2] != 'L'  || elf_header->e_ident[3] != 'F') {
        return fail(WSTR("Invalid ELF header\r\n"));
    }
    if (elf_header->e_machine != 0xB7) { // AArch64
        return fail(WSTR("Invalid ELF machine\r\n"));
    }

    return EFI_SUCCESS;
}

static efi_status_t load_elf_segments(efi_physical_address_t kernel_addr, efi_file_handle_t* kernel_file, efi_physical_address_t* out_phys_entry) {
    Elf64_Ehdr* elf_header = (Elf64_Ehdr*)kernel_addr;
    *out_phys_entry = 0;

    for (int i = 0; i < elf_header->e_phnum; i++) {
        Elf64_Phdr* phdr = (Elf64_Phdr*)(kernel_addr + elf_header->e_phoff + i * elf_header->e_phentsize);

        if (phdr->p_type != PT_LOAD) continue;

        uintn_t pages = (phdr->p_memsz + 0xFFF) / 0x1000;
        
        efi_physical_address_t segment_addr = phdr->p_paddr; 

        efi_status_t status = gBS->AllocatePages(AllocateAddress, EfiLoaderData, pages, &segment_addr);
        if (EFI_ERROR(status)) {
            if (status == EFI_NOT_FOUND) print(WSTR(" (Range not in RAM) "));
            if (status == EFI_OUT_OF_RESOURCES) print(WSTR(" (Occupied) "));
            return fail(WSTR("Address conflict!\r\n"));
        }

        memset((void*)segment_addr, 0, phdr->p_memsz);
        kernel_file->SetPosition(kernel_file, phdr->p_offset);

        uintn_t size_to_read = phdr->p_filesz;
        status = kernel_file->Read(kernel_file, &size_to_read, (void*)segment_addr);
        if (EFI_ERROR(status) || size_to_read != phdr->p_filesz) {
            return fail(WSTR("File read error\r\n"));
        }

        if (elf_header->e_entry >= phdr->p_vaddr && elf_header->e_entry < phdr->p_vaddr + phdr->p_memsz) {
            uint64_t entry_offset = elf_header->e_entry - phdr->p_vaddr;
            *out_phys_entry = segment_addr + entry_offset;
        }
    }

    if (*out_phys_entry == 0) {
        return fail(WSTR("Entry point not found in PT_LOAD segments\r\n"));
    }

    return EFI_SUCCESS;
}

static efi_status_t populate_memory_map(Bootinfo* boot_info) {
    uintn_t map_size = 0;
    efi_memory_descriptor_t* map = NULL;
    uintn_t map_key = 0;
    uintn_t descriptor_size = 0;
    uint32_t descriptor_version = 0;

    efi_status_t status = gBS->GetMemoryMap(&map_size, NULL, &map_key, &descriptor_size, &descriptor_version);
    if (status != EFI_BUFFER_TOO_SMALL) {
        return status;
    }

    map_size += PAGE_SIZE;
    status = gBS->AllocatePool(EfiLoaderData, map_size, (void**)&map);
    if (EFI_ERROR(status)) {
        return status;
    }

    while (1) {
        status = gBS->GetMemoryMap(&map_size, map, &map_key, &descriptor_size, &descriptor_version);
        if (EFI_ERROR(status)) {
            return status;
        }

        boot_info->memoryMap.descriptorSize = descriptor_size;
        boot_info->memoryMap.descriptorVersion = descriptor_version;
        boot_info->memoryMap.mapSize = map_size;
        boot_info->memoryMap.mapKey = map_key;
        boot_info->memoryMap.map = map;

        status = gBS->ExitBootServices(IM, map_key);
        if (status == EFI_SUCCESS) {
            return EFI_SUCCESS;
        }

        map_size += 2 * descriptor_size;
    }
}

efi_status_t bootloader_main(void) {
    efi_gop_t* gop = NULL;
    efi_guid_t gop_guid = EFI_GRAPHICS_OUTPUT_PROTOCOL_GUID;
    efi_status_t status = ST->BootServices->LocateProtocol(&gop_guid, 0, (void**)&gop);
    if (EFI_ERROR(status) || gop == NULL) {
        return fail(WSTR("Failed to locate GOP\r\n"));
    }

    void* dtb_address = find_configuration_table(&dtb_guid);
    if (dtb_address == NULL) {
        return fail(WSTR("Failed to find DTB\r\n"));
    }

    efi_file_handle_t* root = NULL;
    status = open_root_volume(&root);
    if (EFI_ERROR(status)) {
        return fail(WSTR("Failed to open boot volume\r\n"));
    }

    efi_physical_address_t kernel_addr = 0;
    uint64_t kernel_size = 0;
    efi_file_handle_t* kernel_file = NULL;
    status = load_kernel(root, &kernel_addr, &kernel_size, &kernel_file);
    if (EFI_ERROR(status)) {
        return fail(WSTR("Failed to load ksOSKernel.bin\r\n"));
    }

    Elf64_Ehdr* elf_header = (Elf64_Ehdr*)kernel_addr;

    status = parse_elf_headers(kernel_addr);
    if (EFI_ERROR(status)) return status;

    efi_physical_address_t phys_entry = 0;
    status = load_elf_segments(kernel_addr, kernel_file, &phys_entry);
    if (EFI_ERROR(status)) return status;

    Bootinfo* boot_info = NULL;
    status = gBS->AllocatePool(EfiLoaderData, sizeof(Bootinfo), (void**)&boot_info);
    if (EFI_ERROR(status) || boot_info == NULL) {
        return fail(WSTR("Failed to allocate boot info\r\n"));
    }

    boot_info->magic = BOOTINFO_MAGIC;
    boot_info->kernelInfo.kernelAddress = (void*)kernel_addr;
    boot_info->kernelInfo.kernelSize = kernel_size;
    boot_info->framebuffer.base = (BIUInt32*)gop->Mode->FrameBufferBase;
    boot_info->framebuffer.baseSize = gop->Mode->FrameBufferSize;
    boot_info->framebuffer.height = gop->Mode->Information->VerticalResolution;
    boot_info->framebuffer.width = gop->Mode->Information->HorizontalResolution;
    boot_info->framebuffer.pitch = gop->Mode->Information->PixelsPerScanLine;
    boot_info->dtb = dtb_address;

    print(WSTR("Almost ready to jump. Reading memory map\r\n"));
    status = populate_memory_map(boot_info);
    if (EFI_ERROR(status)) {
        return fail(WSTR("Failed to exit boot services\r\n"));
    }

    typedef void (*kernel_entry_t)(Bootinfo*);
    kernel_entry_t kernel_main = (kernel_entry_t)phys_entry;
    kernel_main(boot_info);

    return EFI_SUCCESS;
}