source: trunk/softs/tsar_boot/src/reset_elf_loader.c @ 691

/**
 * \file    : reset_elf_loader.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 on 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 <reset_ioc.h>
#include <elf-types.h>
#include <reset_tty.h>
#include <reset_utils.h>
#include <defs.h>

#if (RESET_DEBUG == 1)
static char const * const init_state_str[] = 
{
    "ELF_HEADER_STATE",
    "ELF_PROGRAM_HEADER_STATE",
    "ELF_OFFSET_STATE",
    "ELF_SEGMENT_STATE",
    "ELF_END_STATE"
};
#endif

unsigned char reset_elf_loader_buffer[512] __attribute__((aligned(CACHE_LINE_SIZE)));

/////////////////////////////////////////////////////////////////////////////////////
void * reset_elf_loader(unsigned int lba)
/////////////////////////////////////////////////////////////////////////////////////
{
    /*
     * Temporary variables used by the loader
     */
    Elf32_Ehdr      elf_header;
    Elf32_Phdr      elf_pht[PHDR_ARRAY_SIZE];

    unsigned char * buffer_ptr;
    Elf32_Ehdr    * elf_ehdr_ptr;
    Elf32_Phdr    * elf_phdr_ptr;

    unsigned int nb_available;
    unsigned int nb_rest;
    unsigned int nb_read;
    unsigned int nb_block;
    unsigned int offset;

    unsigned char * pseg_ptr;
    unsigned int pseg_start;
    unsigned int pseg_end;
    unsigned int pseg_remainder;
    unsigned int pseg;

    /*
     * Loader state machine definition
     */
    typedef enum
    {
        ELF_HEADER_STATE,
        ELF_PROGRAM_HEADER_STATE,
        ELF_OFFSET_STATE,
        ELF_SEGMENT_STATE,
        ELF_END_STATE
    } elf_loader_t;

    elf_loader_t init_state;
    init_state = ELF_HEADER_STATE;

#if (RESET_DEBUG == 1)
    elf_loader_t init_state_debug;
    init_state_debug = ELF_END_STATE;
#endif

    reset_puts("\n[RESET] Start reset_elf_loader at cycle ");
    reset_putd( proctime() );
    reset_puts("\n");

    nb_block     = lba;
    nb_available = 0;
    nb_rest      = sizeof(Elf32_Ehdr);
    pseg         = 0;
    offset       = 0;
    elf_ehdr_ptr = (Elf32_Ehdr *) &elf_header;
    elf_phdr_ptr = (Elf32_Phdr *) &elf_pht[0];

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

            if (reset_ioc_read(nb_block , buffer_ptr, 1))
            {
                reset_puts ("[RESET ERROR] reset_ioc_read() failed\n");
                reset_exit();
            }

            nb_block    += 1;
            nb_available = 512;
        }

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

#if (RESET_DEBUG == 1)
        if (init_state != init_state_debug)
        {
            reset_puts("\ninit_state = ");
            reset_puts(init_state_str[init_state]);
            reset_puts("\n");
            init_state_debug = init_state;
        }
#endif

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

                nb_rest -= nb_read;

                if(nb_rest == 0)
                {
                    nb_rest = elf_ehdr_ptr->e_phnum * elf_ehdr_ptr->e_phentsize;
                    /*
                     * Verification of ELF Magic Number
                     */
                    if ( (elf_ehdr_ptr->e_ident[EI_MAG0] != ELFMAG0) ||
                         (elf_ehdr_ptr->e_ident[EI_MAG1] != ELFMAG1) ||
                         (elf_ehdr_ptr->e_ident[EI_MAG2] != ELFMAG2) ||
                         (elf_ehdr_ptr->e_ident[EI_MAG3] != ELFMAG3) )
                    {
                        reset_puts("[RESET ERROR] boot-loader file is not an ELF format\n");
                        reset_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_ehdr_ptr->e_phnum > PHDR_ARRAY_SIZE)
                    {
                        reset_puts("[RESET ERROR] ELF PHDR table size too large\n");
                        reset_exit();
                    }

                    init_state = ELF_PROGRAM_HEADER_STATE;
                }

                break;

            /*
             * Reading ELF program headers
             */
            case ELF_PROGRAM_HEADER_STATE:
                memcpy(elf_phdr_ptr, buffer_ptr, nb_read);

                elf_phdr_ptr = 
                    (Elf32_Phdr *)((unsigned char *) elf_phdr_ptr + nb_read);

                nb_rest -= nb_read;

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

                    /*
                     * Search the first not NULL segment in the ELF file
                     */
                    for (pseg = 0; pseg < elf_ehdr_ptr->e_phnum; pseg++)
                    {
                        if(elf_phdr_ptr[pseg].p_type == PT_LOAD)
                        {
#if (RESET_DEBUG == 1)
                            reset_puts("loadable segment found:\n");
                            reset_print_elf_phdr(&elf_phdr_ptr[pseg]);
#endif
                            if (elf_phdr_ptr[pseg].p_offset < offset)
                            {
                                /* 
                                 * Case where the segment to load includes the 
                                 * elf and program headers 
                                 */
                                nb_rest = elf_phdr_ptr[pseg].p_filesz - offset;
                                init_state = ELF_SEGMENT_STATE;
                            }
                            else
                            {
                                /* 
                                 * Segment to load is further away in ELF file
                                 */
                                nb_rest = elf_phdr_ptr[pseg].p_offset - offset;
                                init_state = ELF_OFFSET_STATE;
                            }
                            break;
                        }
                    }

                    if (pseg == elf_ehdr_ptr->e_phnum)
                    {
                        reset_puts("[RESET ERROR] No PT_LOAD found\n");
                        reset_exit();
                    }

                }

                break;

            /*
             * Go to the offset of the first not null program segment in the
             * ELF file
             *
             * TODO:
             * No need to read from the disk the useless bytes. Try to compute
             * the next usefull lba
             */
            case ELF_OFFSET_STATE:
                nb_rest -= nb_read;

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

                break;

            /*
             * Reading ELF segments
             *
             * TODO:
             * Do not pass by block buffer but write directly in target memory
             * address
             */
            case ELF_SEGMENT_STATE:
                /*
                 * Verify that loadable segment does not conflict with
                 * pre-loader memory space 
                 */
                pseg_start = elf_phdr_ptr[pseg].p_paddr;

                pseg_end   = elf_phdr_ptr[pseg].p_paddr +
                             elf_phdr_ptr[pseg].p_memsz;

                if ((pseg_start >= 0xBFC00000 && pseg_start <= 0xBFC10000) ||
                    (pseg_end   >= 0xBFC00000 && pseg_end   <= 0xBFC10000) ||
                    (pseg_start <  0xBFC00000 && pseg_end   >  0xBFC10000))
                {
                    reset_puts("[RESET ERROR] conflict with pre-loader memory space\n");
                    reset_exit();
                }

                /*
                 * Copy the ELF segment data in memory using the
                 * virtual address obtained from the ELF file
                 */
                pseg_ptr = (unsigned char *)
                    elf_phdr_ptr[pseg].p_paddr  +
                    elf_phdr_ptr[pseg].p_filesz -
                    nb_rest;

                memcpy(pseg_ptr, buffer_ptr, nb_read);

                nb_rest -= nb_read;

                if (nb_rest == 0)
                {
                    /*
                     * Fill remaining bytes with zeros (filesz < memsz)
                     */
                    pseg_remainder =
                        elf_phdr_ptr[pseg].p_memsz  -
                        elf_phdr_ptr[pseg].p_filesz ;

                    pseg_ptr = (unsigned char *)
                        elf_phdr_ptr[pseg].p_paddr  +
                        elf_phdr_ptr[pseg].p_filesz ;

                    memset(pseg_ptr, 0, pseg_remainder);

                    reset_puts("\n[RESET] Segment loaded : address = ");
                    reset_putx(elf_phdr_ptr[pseg].p_paddr);
                    reset_puts(" / size = ");
                    reset_putx(elf_phdr_ptr[pseg].p_filesz);
                    reset_puts("\n");

                    /*
                     * Search the next first not NULL segment in the ELF file
                     */
                    for (pseg += 1; pseg < elf_ehdr_ptr->e_phnum; pseg++)
                    {
                        if(elf_phdr_ptr[pseg].p_type == PT_LOAD)
                        {
#if (RESET_DEBUG == 1)
                            reset_puts("loadable segment found:\n");
                            reset_print_elf_phdr(&elf_phdr_ptr[pseg]);
#endif
                            nb_rest = elf_phdr_ptr[pseg].p_offset - offset;
                            break;
                        }
                    }

                    /*
                     * Program loading finished
                     */
                    if(pseg == elf_ehdr_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;
    }

    reset_puts("\n[RESET] Complete reset_elf_loader at cycle ");
    reset_putd( proctime() );
    reset_puts(" / boot entry = ");
    reset_putx( (unsigned int)(elf_ehdr_ptr->e_entry) );
    reset_puts("\n");

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

// vim: tabstop=4 : softtabstop=4 : shiftwidth=4 : expandtab