metal-cos/sys/kern/loader.c

#include <stdbool.h>
#include <stdint.h>
#include <string.h>

#include <sys/kassert.h>
#include <sys/sysctl.h>
#include <sys/kmem.h>
#include <sys/queue.h>
#include <sys/disk.h>
#include <sys/elf64.h>

#include <machine/amd64.h>
#include <machine/trap.h>
#include <machine/pmap.h>
#include <sys/thread.h>
#include <sys/spinlock.h>
#include <sys/loader.h>

#include <sys/vfs.h>

bool
Loader_CheckHeader(const Elf64_Ehdr *ehdr)
{
    if (ehdr->e_ident[EI_MAG0] != ELFMAG0 ||
	ehdr->e_ident[EI_MAG1] != ELFMAG1 ||
	ehdr->e_ident[EI_MAG2] != ELFMAG2 ||
	ehdr->e_ident[EI_MAG3] != ELFMAG3)
	return false;

    if (ehdr->e_ident[EI_CLASS] != ELFCLASS64) {
	return false;
    }

    if (ehdr->e_machine != EM_AMD64) {
	return false;
    }

    return true;
}

bool
Loader_LoadFirst(Thread *thr, VNode *vn, void *buf, uint64_t len)
{
    int i;
    const Elf64_Ehdr *ehdr;
    const Elf64_Phdr *phdr;
    AS *as = thr->space;

    ehdr = (const Elf64_Ehdr *)(buf);
    phdr = (const Elf64_Phdr *)(buf + ehdr->e_phoff);

    if (!Loader_CheckHeader(ehdr)) {
	kprintf("Not a valid executable!\n");
	return false;
    }

    kprintf("%8s %16s %8s %8s\n", "Offset", "VAddr", "FileSize", "MemSize");
    for (i = 0; i < ehdr->e_phnum; i++)
    {
	ASSERT(phdr[i].p_type != PT_DYNAMIC);
	if (phdr[i].p_type == PT_LOAD) {
	    uint64_t va = phdr[i].p_vaddr;
	    uint64_t memsz = phdr[i].p_memsz;
	    kprintf("%08llx %016llx %08llx %08llx\n", phdr[i].p_offset,
		    phdr[i].p_vaddr, phdr[i].p_filesz, phdr[i].p_memsz);

	    // Make sure it is page aligned
	    va = va & ~(uint64_t)PGMASK;
	    memsz += phdr[i].p_vaddr - va;

	    kprintf("%016llx %08llx\n", va, memsz);
	    if (!PMap_AllocMap(as, va, phdr[i].p_memsz, PTE_W)) {
		// XXX: Cleanup!
		ASSERT(false);
		return false;
	    }
	}
    }

    PMap_AllocMap(as, MEM_USERSPACE_STKBASE, MEM_USERSPACE_STKLEN, PTE_W);
    PMap_LoadAS(as); // Reload CR3

    for (i = 0; i < ehdr->e_phnum; i++)
    {
	ASSERT(phdr[i].p_type != PT_DYNAMIC);
	if (phdr[i].p_type == PT_LOAD) {
	    if (phdr[i].p_filesz != 0) {
		VFS_Read(vn, (void *)phdr[i].p_vaddr,
			 phdr[i].p_offset, phdr[i].p_filesz);
	    }
	    memset((void *)(phdr[i].p_vaddr + phdr[i].p_filesz),
		   0,
		   (uintptr_t)(phdr[i].p_memsz - phdr[i].p_filesz));
	}
    }

    TrapFrame tf;
    memset(&tf, 0, sizeof(tf));
    tf.ds = SEL_UDS | 3;
    tf.rip = ehdr->e_entry;
    tf.cs = SEL_UCS | 3;
    tf.rsp = MEM_USERSPACE_STKBASE + MEM_USERSPACE_STKLEN;
    tf.ss = SEL_UDS | 3;
    tf.rflags = RFLAGS_IF;
    tf.rdi = 0;
    Trap_Pop(&tf);

    return true;
}

Handle *Console_OpenHandle();

void
Loader_LoadInit()
{
    int status;
    void *pg;
    VNode *init;

    pg = PAlloc_AllocPage();
    if (!pg)
	Panic("Not enough memory!");

    init = VFS_Lookup("/sbin/init");
    status = VFS_Open(init);
    if (status < 0)
	Panic("Loading init process failed!");
    status = VFS_Read(init, pg, 0, 1024);
    if (status < 0)
	Panic("Reading init process failed!");

    Thread *thr = Sched_Current();

    // Open stdin/out/err
    Handle *handle = Console_OpenHandle();
    Handle_Add(thr->proc, handle);
    handle = Console_OpenHandle();
    Handle_Add(thr->proc, handle);
    handle = Console_OpenHandle();
    Handle_Add(thr->proc, handle);

    Loader_LoadFirst(thr, init, pg, 1024);

    VFS_Close(init);
}

void
LoaderFileHelper(AS *as, VNode *vn, uintptr_t vaddr,
		 uintptr_t offset, uintptr_t len)
{
    void *raddr;

    if ((vaddr % PGSIZE) != 0) {
	uintptr_t maxlen = PGSIZE - (vaddr % PGSIZE);
	uintptr_t rlen = maxlen < len ? maxlen : len;

	raddr = (void *)DMPA2VA(PMap_Translate(as, vaddr));
	VFS_Read(vn, raddr, offset, rlen);
	vaddr += rlen;
	offset += rlen;
	len -= rlen;
    }

    while (len > PGSIZE) {
	raddr = (void *)DMPA2VA(PMap_Translate(as, vaddr));
	VFS_Read(vn, raddr, offset, PGSIZE);
	vaddr += PGSIZE;
	offset += PGSIZE;
	len -= PGSIZE;
    }

    if (len > 0) {
	raddr = (void *)DMPA2VA(PMap_Translate(as, vaddr));
	VFS_Read(vn, raddr, offset, len);
    }
}

void
LoaderZeroHelper(AS *as, uintptr_t vaddr, uintptr_t len)
{
    void *raddr;

    if ((vaddr % PGSIZE) != 0) {
	uintptr_t maxlen = PGSIZE - (vaddr % PGSIZE);
	uintptr_t rlen = maxlen < len ? maxlen : len;

	raddr = (void *)DMPA2VA(PMap_Translate(as, vaddr));
	memset(raddr, 0, rlen);
	vaddr += rlen;
	len -= rlen;
    }

    while (len > PGSIZE) {
	raddr = (void *)DMPA2VA(PMap_Translate(as, vaddr));
	memset(raddr, 0, PGSIZE);
	vaddr += PGSIZE;
	len -= PGSIZE;
    }

    if (len > 0) {
	raddr = (void *)DMPA2VA(PMap_Translate(as, vaddr));
	memset(raddr, 0, len);
    }
}

bool
Loader_Load(Thread *thr, VNode *vn, void *buf, uint64_t len)
{
    int i;
    const Elf64_Ehdr *ehdr;
    const Elf64_Phdr *phdr;
    AS *as = thr->space;

    ehdr = (const Elf64_Ehdr *)(buf);
    phdr = (const Elf64_Phdr *)(buf + ehdr->e_phoff);

    if (!Loader_CheckHeader(ehdr)) {
	kprintf("Not a valid executable!\n");
	return false;
    }

    kprintf("%8s %16s %8s %8s\n", "Offset", "VAddr", "FileSize", "MemSize");
    for (i = 0; i < ehdr->e_phnum; i++)
    {
	ASSERT(phdr[i].p_type != PT_DYNAMIC);
	if (phdr[i].p_type == PT_LOAD) {
	    uint64_t va = phdr[i].p_vaddr;
	    uint64_t memsz = phdr[i].p_memsz;
	    kprintf("%08llx %016llx %08llx %08llx\n", phdr[i].p_offset,
		    phdr[i].p_vaddr, phdr[i].p_filesz, phdr[i].p_memsz);

	    // Make sure it is page aligned
	    va = va & ~(uint64_t)PGMASK;
	    memsz += phdr[i].p_vaddr - va;

	    kprintf("%016llx %08llx\n", va, memsz);
	    if (!PMap_AllocMap(as, va, phdr[i].p_memsz, PTE_W)) {
		// XXX: Cleanup!
		ASSERT(false);
		return false;
	    }
	}
    }

    PMap_AllocMap(as, MEM_USERSPACE_STKBASE, MEM_USERSPACE_STKLEN, PTE_W);

    for (i = 0; i < ehdr->e_phnum; i++)
    {
	ASSERT(phdr[i].p_type != PT_DYNAMIC);
	if (phdr[i].p_type == PT_LOAD) {
	    if (phdr[i].p_filesz != 0) {
		LoaderFileHelper(as, vn, phdr[i].p_vaddr,
				 phdr[i].p_offset, phdr[i].p_filesz);
	    }
	    LoaderZeroHelper(as,
			     phdr[i].p_vaddr + phdr[i].p_filesz,
			     phdr[i].p_memsz - phdr[i].p_filesz);
	}
    }

    Thread_SetupUThread(thr, ehdr->e_entry, 0);

    return true;
}