source: branches/v4/softs/tsar_boot/src/boot_elf_loader.c @ 613

/**
 * \file    : boot_loader_entry.c
 * \date    : August 2012
 * \author  : Cesar Fuguet
 *
 * This file defines an elf file loader which reads an executable elf file
 * starting at a sector passed as argument of a disk and copy the different
 * ELF program segments in the appropriate memory address using as information
 * the virtual address read from the elf file.
 */

#include <boot_ioc.h>
#include <elf-types.h>
#include <boot_tty.h>
#include <boot_memcpy.h>
#include <defs.h>

void * boot_elf_loader(unsigned int lba)
{
    /**
     * Temporary variables used by the boot loader
     */
    unsigned char   boot_elf_loader_buffer[512];
    Elf32_Ehdr      elf_header;
    Elf32_Phdr      elf_pht[PHDR_ARRAY_SIZE];

    unsigned char * buffer_ptr;
    Elf32_Ehdr    * elf_header_ptr;
    Elf32_Phdr    * elf_pht_ptr;

    unsigned int nb_available;
    unsigned int nb_rest;
    unsigned int nb_read;
    unsigned int nb_block;
    unsigned int offset;
    unsigned int pseg;
    unsigned int i;
    unsigned int segment_req;
   
    /*
     * Loader state machine definition
     */  
    enum
    {
        ELF_HEADER_STATE,
        ELF_PROGRAM_HEADER_STATE,
        ELF_OFFSET_STATE,
        ELF_SEGMENT_STATE,
        ELF_END_STATE
    } init_state;

    boot_puts("Starting boot_elf_loader function...\n\r");

    nb_block           = lba;

    pseg               = 0;
    nb_available       = 0;
    nb_rest            = sizeof(Elf32_Ehdr);
    offset             = 0;

    elf_header_ptr     = (Elf32_Ehdr *) &elf_header;
    elf_pht_ptr        = (Elf32_Phdr *) &elf_pht[0];

    init_state         = ELF_HEADER_STATE;

    while(init_state != ELF_END_STATE)
    {
        if (nb_available == 0)
        {
            buffer_ptr = &boot_elf_loader_buffer[0];

            if ( boot_ioc_read(nb_block , buffer_ptr, 1) )
            {
                boot_puts (
                                        "ERROR: "
                                        "IOC_FAILED"
                                        "\n"
                                );

                boot_exit();
            }
           
            nb_block    += 1;
            nb_available = 512;
        }

        nb_read  = (nb_rest <= nb_available) ? nb_rest : nb_available;
        offset  +=  nb_read;

        switch(init_state)
        {
            /**
             * Reading ELF executable header
             */
            case ELF_HEADER_STATE:
                boot_memcpy(elf_header_ptr, buffer_ptr, nb_read);

                nb_rest -= nb_read;

                if(nb_rest == 0)
                {
                    nb_rest = elf_header_ptr->e_phnum * elf_header_ptr->e_phentsize;

                                        /* Verification of ELF Magic Number */
                                        if (
                                                (elf_header_ptr->e_ident[EI_MAG0] != ELFMAG0) ||
                                                (elf_header_ptr->e_ident[EI_MAG1] != ELFMAG1) ||
                                                (elf_header_ptr->e_ident[EI_MAG2] != ELFMAG2) ||
                                                (elf_header_ptr->e_ident[EI_MAG3] != ELFMAG3) )
                                        {
                                                boot_puts(
                                                        "ERROR: "
                                                        "Input file does not use ELF format"
                                                        "\n"
                                                );      

                                                boot_exit();
                                        }

                                        /* 
                                         * Verification of Program Headers table size. It must be
                                         * smaller than the work size allocated for the 
                                         * elf_pht[PHDR_ARRAY_SIZE] array
                                         **/
                    if (elf_header_ptr->e_phnum > PHDR_ARRAY_SIZE)
                    {
                        boot_puts(
                                                        "ERROR: "
                                                        "ELF PHDR table size is bigger than "
                                                        "the allocated work space"
                                                        "\n"
                                                );

                        boot_exit();
                    }

                    init_state = ELF_PROGRAM_HEADER_STATE;
                }

                break;

            /**
             * Reading ELF program headers
             */  
            case ELF_PROGRAM_HEADER_STATE:
                boot_memcpy(elf_pht_ptr, buffer_ptr, nb_read);

                elf_pht_ptr = (Elf32_Phdr *)((unsigned char *) elf_pht_ptr + nb_read);
                nb_rest    -= nb_read;

                if(nb_rest == 0)
                {
                    elf_pht_ptr = (Elf32_Phdr *) &elf_pht[0];

                    /*
                     * Search the first not NULL segment in the ELF file
                     */
                    for (pseg = 0; pseg < elf_header_ptr->e_phnum; pseg++)
                    {
                        if(elf_pht_ptr[pseg].p_type == PT_LOAD)
                        {
                            nb_rest = elf_pht_ptr[pseg].p_offset - offset;
                            break;
                        }
                    }

                    init_state = ELF_OFFSET_STATE;
                }

                break;

            /**
             * Go to the offset of the first not null program segment in the ELF file
             */
            case ELF_OFFSET_STATE:
                nb_rest -= nb_read;

                if (nb_rest == 0)
                {
                    nb_rest    = elf_pht_ptr[pseg].p_filesz;
                    init_state = ELF_SEGMENT_STATE;
                }

                break;

            /**
             * Reading ELF segments
             */
            case ELF_SEGMENT_STATE:
                /**
                 * Copying ELF segment data in memory segments using the virtual
                 * address got from the ELF file
                 */ 
                segment_req = ((elf_pht_ptr[pseg].p_vaddr & 0xBFC00000) != 0xBFC00000);

                if ( segment_req )
                {
                    boot_memcpy((unsigned char *) elf_pht_ptr[pseg].p_vaddr +
                                (elf_pht_ptr[pseg].p_filesz - nb_rest),
                                buffer_ptr,
                                nb_read);
                }
    
                nb_rest -= nb_read;

                if ( nb_rest == 0 )
                {
                    if ( segment_req )
                    {
                        boot_puts("Copied segment at address ");
                        boot_putx(elf_pht_ptr[pseg].p_vaddr);
                        boot_puts("\n");

                        /*
                         * Fill remaining bytes with zeros (filesz < memsz)
                         */
                        for ( i = 0                                                        ;
                              i < (elf_pht_ptr[pseg].p_memsz - elf_pht_ptr[pseg].p_filesz) ;
                              i--                                                          )
                        {
                            *(unsigned char *)
                            (elf_pht_ptr[pseg].p_vaddr + elf_pht_ptr[pseg].p_filesz + i) = 0;
                        }
                    }

                    /*
                     * Search the first not NULL segment in the ELF file
                     */
                    for ( pseg = pseg + 1; pseg < elf_header_ptr->e_phnum; pseg++) {
                        if(elf_pht_ptr[pseg].p_type == PT_LOAD)
                        {
                            nb_rest = elf_pht_ptr[pseg].p_offset - offset;
                            break;
                        }
                    }

                    /*
                     * Program loading finished
                     */ 
                    if(pseg == elf_header_ptr->e_phnum)
                    {
                        init_state = ELF_END_STATE;
                        break;
                    }

                    init_state = ELF_OFFSET_STATE;
                }
                break;

            default:
                break;
        }

        buffer_ptr              += nb_read;
        nb_available            -= nb_read;
    }

    boot_puts (
                "Finishing boot_elf_loader function.\n"
                "Entry point address: "
        );
    boot_putx(elf_header_ptr->e_entry);
    boot_puts("\n");

    return ((void *) elf_header_ptr->e_entry);
}