M68k ELF loader function

#include <stdint.h>
#include "elf.h"

uint32_t loadELF(uint8_t fd, uint32_t *entry){
    /* ELF headers/structs */
    Elf32_Ehdr ehdr;
    Elf32_Phdr phdr;
    Elf32_Dyn dyn;

    /* Relocation variables/structs */
    Elf32_Rela *rela_ptr;
    uint32_t rela_count;
    uint32_t rela_size = 0;
    Elf32_Sym *sym_ptr;
    uint32_t sym_size = 0;
    uint32_t sym_val;

    uint32_t tmp;

    uint32_t total_mem_size = 0;
    uint32_t virtual_base_addr = 0xFFFFFFFF;
    uint32_t physical_base_addr;


    /* Move the pointer to the start of the File, just in case */
    SYSCALL_seekFile(fd, 0, SEEK_SET);

    if(SYSCALL_readFile(fd, &ehdr, sizeof(Elf32_Ehdr)) != sizeof(Elf32_Ehdr)){
        dbg_printf("Failed to read ELF header\n");
        SYSCALL_closeFile(fd);
        return 0;
    }

    if((memcmp(ehdr.e_ident, ELFMAG, SELFMAG) != 0) ||  /* Check ELF magic number */
        (ehdr.e_ident[EI_CLASS] != ELFCLASS32) ||       /* Check ELF class (32-bit) */
        (ehdr.e_ident[EI_DATA] != ELFDATA2MSB) ||       /* Check data encoding (big-endian) */
        (ehdr.e_ident[EI_VERSION] != EV_CURRENT) ||     /* Check ELF version */
        (ehdr.e_machine != EM_M68K) ||                  /* Check target architecture (Motorola 68000) */
        (ehdr.e_type != ET_EXEC)){                      /* Check file type (executable) */

        dbg_printf("Invalid ELF header\n");
        SYSCALL_closeFile(fd);
        return 0;
    }


    for(uint32_t i = 0; i < ehdr.e_phnum; i++){

        /* Read each program header */
        // dbg_printf("Reading program header %lu from offset %lu in the file\n", i, ehdr.e_phoff + (i * sizeof(Elf32_Phdr)));
        SYSCALL_seekFile(fd, ehdr.e_phoff + (i * sizeof(Elf32_Phdr)), SEEK_SET);
        if(SYSCALL_readFile(fd, &phdr, sizeof(Elf32_Phdr)) != sizeof(Elf32_Phdr)){
            dbg_printf("Failed to read program header %lu\n", i);
            SYSCALL_closeFile(fd);
            return 0;
        }

        /* If the segment is loadable, calculate memory size and find the base address */
        if(phdr.p_type == PT_LOAD){
            dbg_printf("Found loadable segment: vaddr = 0x%08lX, memsz = 0x%08lX\n", phdr.p_vaddr, phdr.p_memsz);
            total_mem_size += phdr.p_memsz;
            if((phdr.p_vaddr) < virtual_base_addr){
                virtual_base_addr = phdr.p_vaddr;
            }
        }
    }

    /* Allocate the required Memory */
    physical_base_addr = (uint32_t)allocMem(total_mem_size, currentTask);

    if(!physical_base_addr){
        dbg_printf("Failed to allocate %luB for executable\n", total_mem_size);
        SYSCALL_closeFile(fd);
        return 0;
    }

    dbg_printf("Total amount of Memory: %luB\nVirtual/Physical Base Addresses: 0x%08lX/0x%08lX\n", total_mem_size, virtual_base_addr, physical_base_addr);

    /* Load each loadable segment into memory */
    for(uint32_t i = 0; i < ehdr.e_phnum; i++){

        /* Read each program header again */
        // dbg_printf("Reading program header %lu from offset %lu in the file\n", i, ehdr.e_phoff + (i * sizeof(Elf32_Phdr)));
        SYSCALL_seekFile(fd, ehdr.e_phoff + (i * sizeof(Elf32_Phdr)), SEEK_SET);
        if(SYSCALL_readFile(fd, &phdr, sizeof(Elf32_Phdr)) != sizeof(Elf32_Phdr)){
            dbg_printf("Failed to read program header %lu\n", i);

            SYSCALL_closeFile(fd);

            freeMem((void*)physical_base_addr);

            return 0;
        }

        /* If the segment is not loadable, skip it */
        if(phdr.p_type != PT_LOAD) continue;

        dbg_printf("Loading segment %lu from offset %lu in the file\n", i, phdr.p_offset);

        SYSCALL_seekFile(fd, phdr.p_offset, SEEK_SET);
        if(SYSCALL_readFile(fd, (uint8_t*)RELOC(phdr.p_vaddr), phdr.p_filesz) != phdr.p_filesz){
            dbg_printf("Failed to load segment %lu\n", i);

            SYSCALL_closeFile(fd);

            freeMem((void*)physical_base_addr);

            return 0;
        }

        dbg_printf("Loaded segment\n");

        // Zero out the remaining memory if p_memsz > p_filesz
        if(phdr.p_memsz > phdr.p_filesz){
            memset((uint8_t*)(RELOC(phdr.p_vaddr) + phdr.p_filesz), 0, phdr.p_memsz - phdr.p_filesz);
        }
    }

    /* Process relocation entries if necessary */
    if(physical_base_addr == virtual_base_addr) goto noreloc;

    for(uint32_t i = 0; i < ehdr.e_phnum; i++){

        /* Read each program header again again */
        dbg_printf("Reading program header %lu from offset %lu in the file\n", i, ehdr.e_phoff + (i * sizeof(Elf32_Phdr)));
        SYSCALL_seekFile(fd, ehdr.e_phoff + (i * sizeof(Elf32_Phdr)), SEEK_SET);
        if(SYSCALL_readFile(fd, &phdr, sizeof(Elf32_Phdr)) != sizeof(Elf32_Phdr)){
            dbg_printf("Failed to read program header %lu\n", i);
            SYSCALL_closeFile(fd);
            freeMem((void*)physical_base_addr);
            return 0;
        }

        /* If the segment is not dynamic, skip it */
        if(phdr.p_type != PT_DYNAMIC) continue;

        for(uint32_t j = 0; j < (phdr.p_memsz / sizeof(Elf32_Dyn)); j++){

            // dbg_printf("reading dynamic header %lu/%lu from offset %lu in the file\n", j, (phdr.p_memsz / sizeof(Elf32_Dyn)), phdr.p_offset + (j * sizeof(Elf32_Dyn)));
            SYSCALL_seekFile(fd, phdr.p_offset + (j * sizeof(Elf32_Dyn)), SEEK_SET);
            if(SYSCALL_readFile(fd, &dyn, sizeof(Elf32_Dyn)) != sizeof(Elf32_Dyn)){
                dbg_printf("Failed to read dynamic section entry %lu\n", j);
                SYSCALL_closeFile(fd);
                freeMem((void*)physical_base_addr);
                return 0;
            }

            /* Process relocation entries */
            switch(dyn.d_tag){
                case DT_NULL:       // End of the list

                goto dyn_exit;

                case DT_SYMTAB:     // Symbol Table Address
                    dbg_printf("SYMTAB, 0x%08lX\n", dyn.d_un.d_ptr);
                    sym_ptr = (Elf32_Sym*)(RELOC(dyn.d_un.d_ptr));
                break;

                case DT_SYMENT:     // Symbol Table Size
                    dbg_printf("SYMENT, %lu\n", dyn.d_un.d_val);
                    sym_size = dyn.d_un.d_val;
                break;

                case DT_RELA:       // Relocation (addend) Table Address
                    dbg_printf("RELA, 0x%08lX\n", dyn.d_un.d_ptr);
                    rela_ptr = (Elf32_Rela*)(RELOC(dyn.d_un.d_ptr));
                break;

                case DT_RELASZ:     // Relocation (addend) Table Size
                    dbg_printf("RELASZ, %lu\n", dyn.d_un.d_val);
                    rela_count = dyn.d_un.d_val;
                break;

                case DT_RELAENT:    // Relocation (addend) Entry Size
                    dbg_printf("RELAENT, %lu\n", dyn.d_un.d_val);
                    rela_size = dyn.d_un.d_val;
                break;

                default:
                    /*
                    if(dyn.d_tag > 33){
                        dbg_printf("Unhandled dynamic entry, d_tag: 0x%08lX\n", dyn.d_tag);
                    }else{
                        dbg_printf("Unhandled dynamic entry, d_tag: %lu\n", dyn.d_tag);
                    }
                    */
                break;
            }
        }
    }
    /* The exit being here assumes there is only ever 1 Dynamic Segment */
    dyn_exit:


    /* Actually apply the relocation (if any were present) */
    if(rela_size == 0) goto noreloc;

    dbg_printf("Handle Relocation Types\n");
    for(uint32_t i = 0; i < (rela_count / rela_size); i++){


        /* Handle the different relocation types */
        switch(ELF32_R_TYPE(rela_ptr[i].r_info)){

            /*
                r_offset is the virtual address of the 32-bit word that needs relocation
                physical address = (r_offset - virtual_base_addr) + physical_base_addr

                r_addend is the value that needs to be relocated and written to the physical address of r_offset
                value = (r_addend - virtual_base_addr) + physical_base_addr
            */
            case R_68K_RELATIVE:
                tmp = mread32(RELOC(rela_ptr[i].r_offset));
                dbg_printf("Address: 0x%08lX (Old/New Values: 0x%08lX/0x%08lX) (R_68K_RELATIVE)\n", RELOC(rela_ptr[i].r_offset), tmp, RELOC(rela_ptr[i].r_addend));
                mwrite32(RELOC(rela_ptr[i].r_offset), RELOC(rela_ptr[i].r_addend));
            break;

            /*
                32 is Similar to RELATIVE, but with a symbol that gets added to r_addend before relocation
            */
            case R_68K_32:
                sym_val = sym_ptr[ELF32_R_SYM(rela_ptr[i].r_info)].st_value;

                tmp = mread32(RELOC(rela_ptr[i].r_offset));
                dbg_printf("Address: 0x%08lX (Old/New Values: 0x%08lX/0x%08lX) (R_68K_32)\n", RELOC(rela_ptr[i].r_offset), tmp, RELOC((rela_ptr[i].r_addend + sym_val)));

                mwrite32(RELOC(rela_ptr[i].r_offset), RELOC((rela_ptr[i].r_addend + sym_val)));
            break;

            /*
                Same as 32, but with a 16-bit word
            */
            case R_68K_16:
                sym_val = sym_ptr[ELF32_R_SYM(rela_ptr[i].r_info)].st_value;

                tmp = mread16(RELOC(rela_ptr[i].r_offset));
                dbg_printf("Address: 0x%08lX (Old/New Values: 0x%08X/0x%08X) (R_68K_16)\n", RELOC(rela_ptr[i].r_offset), (uint16_t)tmp, (uint16_t)(RELOC((rela_ptr[i].r_addend + sym_val)) & 0x0000FFFF));

                mwrite16(RELOC(rela_ptr[i].r_offset), RELOC((rela_ptr[i].r_addend + sym_val)) & 0x0000FFFF);
            break;

            /*
                Same as 32, but with a 8-bit byte
            */
            case R_68K_8:
                sym_val = sym_ptr[ELF32_R_SYM(rela_ptr[i].r_info)].st_value;

                tmp = mread8(RELOC(rela_ptr[i].r_offset));
                dbg_printf("Address: 0x%08lX (Old/New Values: 0x%08X/0x%08X) (R_68K_8)\n", RELOC(rela_ptr[i].r_offset), (uint8_t)tmp, (uint8_t)(RELOC((rela_ptr[i].r_addend + sym_val)) & 0x000000FF));

                mwrite8(RELOC(rela_ptr[i].r_offset), RELOC((rela_ptr[i].r_addend + sym_val)) & 0x000000FF);
            break;

            default:
                dbg_printf("Unhandled relocation type: %lu\n", ELF32_R_TYPE(rela_ptr[i].r_info));
            break;
        }

    }
    noreloc:

    /* Update the entry point address */
    *entry = RELOC((uint32_t)ehdr.e_entry);

    dbg_printf("Entry point set to: 0x%08lX\n", *entry);

    /* Close the file and return */
    SYSCALL_closeFile(fd);
    return physical_base_addr;
}