source: trunk/softs/tsar_boot/boot_loader_entry.c @ 276

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

#include <ioc.h>
#include <elf-types.h>
#include <boot_tty.h>

#define in_reset       __attribute__((section (".reset")))
#define in_reset_data __attribute__((section (".reset_data")))

struct base_addresses;
extern struct base_addresses seg_reset_heap_base;

in_reset static void *boot_memcpy(void *_dst, const void *_src, unsigned int size)
{
    unsigned int *dst = _dst;
    const unsigned int *src = _src;
    if ( ! ((unsigned int)dst & 3) && ! ((unsigned int)src & 3) )
        while (size > 3) {
            *dst++ = *src++;
            size -= 4;
        }

    unsigned char *cdst = (unsigned char*)dst;
    unsigned char *csrc = (unsigned char*)src;

    while (size--) {
        *cdst++ = *csrc++;
    }
    return _dst;
}

in_reset void * _boot_loader_entry()
{
    in_reset_data static const char start_boot_str[] = "Starting bootstrap stage...\n\r";
    in_reset_data static const char start_boot_str_err[] = "ioc failed\n";
    in_reset_data static const char copied_str[]     = "Copied segment at address ";
    in_reset_data static const char finishing_str[]  = "Finishing bootstrap stage, entry point ";
    in_reset_data static const char end_line[]         = "\n\r";
    
    const unsigned int boot_loader_buffer_base = (unsigned int) &seg_reset_heap_base;
    const unsigned int elf_header_base         = boot_loader_buffer_base + 512;
    const unsigned int elf_pht_base            = elf_header_base + sizeof(Elf32_Ehdr);

    /**
     * Temporary variables used by the boot loader
     */
    unsigned char * boot_loader_buffer;
    Elf32_Ehdr    * elf_header;
    Elf32_Phdr    * elf_pht;

    unsigned int nb_available;
    unsigned int nb_rest;
    unsigned int nb_read;
    unsigned int nb_block;
    unsigned int offset;
    unsigned int pseg;
    unsigned int init_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(start_boot_str);

    /* Initialize the block device */
    if (_ioc_init() != 0) {
        boot_puts(start_boot_str_err);
        while (1);
    }

    boot_loader_buffer = 0;
    pseg               = 0;
    init_pseg          = 0;
    nb_block           = 2;
    nb_available       = 0;
    nb_rest            = sizeof(Elf32_Ehdr);
    offset                  = 0;

    elf_header         = (Elf32_Ehdr *) elf_header_base;
    elf_pht            = (Elf32_Phdr *) elf_pht_base;

    init_state         = ELF_HEADER_STATE;

    while(init_state != ELF_END_STATE)
    {
        if (nb_available == 0)
        {
            boot_loader_buffer = (unsigned char *) boot_loader_buffer_base;

            if ( _ioc_read(nb_block , boot_loader_buffer, 1) )
            {
                boot_puts(start_boot_str_err);
                while (1);
            }
           
            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, boot_loader_buffer, nb_read);

                nb_rest -= nb_read;

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

                    init_state = ELF_PROGRAM_HEADER_STATE;
                }

                break;

            /**
             * Reading ELF program headers
             */  
            case ELF_PROGRAM_HEADER_STATE:
                boot_memcpy(elf_pht, boot_loader_buffer, nb_read);

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

                if(nb_rest == 0)
                {
                    elf_pht = (Elf32_Phdr *) elf_pht_base;

                    /*
                     * Search the first not NULL segment in the ELF file
                     */
                    for (pseg = 0; pseg < elf_header->e_phnum; pseg++)
                    {
                        if(elf_pht[pseg].p_type == PT_LOAD)
                        {
                            nb_rest = elf_pht[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[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[pseg].p_vaddr & 0xBFC00000) != 0xBFC00000);

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

                if ( nb_rest == 0 )
                {
                    if ( segment_req )
                    {
                        boot_puts(copied_str);
                        boot_putx(elf_pht[pseg].p_vaddr);
                        boot_puts(end_line);

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

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

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

                    init_state = ELF_OFFSET_STATE;
                }
                break;

            default:
                break;
        }

        boot_loader_buffer += nb_read;
        nb_available       -= nb_read;
    }
    boot_puts(finishing_str);
    boot_putx(elf_header->e_entry);
    boot_puts(end_line);
    return (void*)elf_header->e_entry;
}