source: soft/giet_vm/giet_boot/boot.c @ 344

//////////////////////////////////////////////////////////////////////////////////////////
// File     : boot.c
// Date     : 01/11/2013
// Author   : alain greiner
// Copyright (c) UPMC-LIP6
//////////////////////////////////////////////////////////////////////////////////////////
// The boot.c file is part of the GIET-VM nano-kernel.
//
// This nano-kernel has been written for the MIPS32 processor.
// The virtual adresses are on 32 bits and use the (unsigned int) type, but the 
// physicals addresses can have up to 40 bits, and use the  (unsigned long long) type.
// It natively supports clusterised shared mmemory multi-processors architectures, 
// where each processor is identified by a composite index (cluster_xy, local_id),
// and where there is one physical memory bank per cluster.
//
// This code is executed in the boot phase by proc[0] and performs the following tasks:
// - load into memory the giet_vm binary files, contained in a FAT32 file system,
// - build the various page tables (one page table per vspace) 
// - initialize the shedulers (one scheduler per processor)
//
// 1) The binary files to be loaded are:
//    - the "map.bin" file contains the hardware architecture description and the
//      mapping directives. It must be stored in the the seg_boot_mapping segment 
//      (at address SEG_BOOT_MAPPING_BASE defined in hard_config.h file).
//    - the "sys.elf" file contains the kernel binary code and data.
//    - the various "application.elf" files.
//
// 2) The map.bin file contains the binary representation of the map.xml file defining: 
//    - the hardware architecture: number of clusters, number or processors, 
//      size of the memory segments, and peripherals in each cluster.
//    - The structure of the various multi-threaded software applications:
//      number of tasks, communication channels.
//    - The mapping: grouping of virtual objects (vobj) in the virtual segments (vseg),
//      placement of virtual segments (vseg) in the physical segments (pseg), placement 
//      of software tasks on the processors, 
//
// 3) The GIET-VM uses the paged virtual memory to provides two services:
//    - classical memory protection, when several independant applications compiled
//      in different virtual spaces are executing on the same hardware platform.
//    - data placement in NUMA architectures, when we want to control the placement 
//      of the software objects (virtual segments) on the physical memory banks.
//
//    The page table are statically build in the boot phase, and they do not 
//    change during execution. The GIET uses only 4 Kbytes pages.
//    As most applications use only a limited number of segments, the number of PT2s 
//    actually used by a given virtual space is generally smaller than 2048, and is
//    computed during the boot phase.
//    The max number of virtual spaces (GIET_NB_VSPACE_MAX) is a configuration parameter.
//
//    Each page table (one page table per virtual space) is monolithic, and contains 
//    one PT1 and up to (GIET_NB_PT2_MAX) PT2s. The PT1 is addressed using the ix1 field
//    (11 bits) of the VPN, and the selected PT2 is addressed using the ix2 field (9 bits).
//    - PT1[2048] : a first 8K aligned array of unsigned int, indexed by (ix1) field of VPN. 
//    Each entry in the PT1 contains a 32 bits PTD. The MSB bit PTD[31] is 
//    the PTD valid bit, and LSB bits PTD[19:0] are the 20 MSB bits of the physical base
//    address of the selected PT2.
//    The PT1 contains 2048 PTD of 4 bytes => 8K bytes.
//    - PT2[1024][GIET_NB_PT2_MAX] : an array of array of unsigned int. 
//    Each PT2[1024] must be 4K aligned, each entry in a PT2 contains two unsigned int: 
//    the first word contains the protection flags, and the second word contains the PPN.
//    Each PT2 contains 512 PTE2 of 8bytes => 4K bytes.
//    The total size of a page table is finally = 8K + (GIET_NB_PT2_MAX)*4K bytes.
///////////////////////////////////////////////////////////////////////////////////////
// Implementation Notes:
//
// 1) The cluster_id variable is a linear index in the mapping_info array of clusters. 
//    We use the cluster_xy variable for the tological index = x << Y_WIDTH + y
///////////////////////////////////////////////////////////////////////////////////////

#include <giet_config.h>
#include <mwmr_channel.h>
#include <barrier.h>
#include <memspace.h>
#include <tty_driver.h>
#include <xcu_driver.h>
#include <ioc_driver.h>
#include <dma_driver.h>
#include <cma_driver.h>
#include <nic_driver.h>
#include <ioc_driver.h>
#include <iob_driver.h>
#include <pic_driver.h>
#include <mwr_driver.h>
#include <ctx_handler.h>
#include <irq_handler.h>
#include <vmem.h>
#include <utils.h>
#include <elf-types.h>

// for boot FAT initialisation 
#include <fat32.h>

#include <mips32_registers.h>
#include <stdarg.h>

#if !defined(X_SIZE)
# error The X_SIZE value must be defined in the 'hard_config.h' file !
#endif

#if !defined(Y_SIZE)
# error The Y_SIZE value must be defined in the 'hard_config.h' file !
#endif

#if !defined(X_WIDTH)
# error The X_WIDTH value must be defined in the 'hard_config.h' file !
#endif

#if !defined(Y_WIDTH)
# error The Y_WIDTH value must be defined in the 'hard_config.h' file !
#endif

#if !defined(SEG_BOOT_MAPPING_BASE) 
# error: You must define SEG_BOOT_MAPPING_BASE in the hard_config.h file
#endif

#if !defined(SEG_BOOT_BUFFER_BASE) 
# error: You must define SEG_BOOT_BUFFER_BASE in the hard_config.h file
#endif

#if !defined(SEG_BOOT_BUFFER_SIZE) 
# error: You must define SEG_BOOT_BUFFER_SIZE in the hard_config.h file
#endif

#if !defined(NB_PROCS_MAX)
# error The NB_PROCS_MAX value must be defined in the 'hard_config.h' file !
#endif

#if !defined(GIET_NB_VSPACE_MAX)
# error The GIET_NB_VSPACE_MAX value must be defined in the 'giet_config.h' file !
#endif

////////////////////////////////////////////////////////////////////////////
//      Global variables for boot code
// Both the page tables for the various virtual spaces, and the schedulers
// for the processors are physically distributed on the clusters.
// These global variables are just arrays of pointers. 
////////////////////////////////////////////////////////////////////////////

// This global variable is allocated in "fat32.c" file
extern fat32_fs_t fat;

// Page table addresses arrays
__attribute__((section (".bootdata"))) 
volatile paddr_t      _ptabs_paddr[GIET_NB_VSPACE_MAX];

__attribute__((section (".bootdata"))) 
volatile unsigned int _ptabs_vaddr[GIET_NB_VSPACE_MAX];

// Next free PT2 index array
__attribute__((section (".bootdata"))) 
volatile unsigned int _next_free_pt2[GIET_NB_VSPACE_MAX] =
{ [0 ... GIET_NB_VSPACE_MAX - 1] = 0 };

// Max PT2 index 
__attribute__((section (".bootdata"))) 
volatile unsigned int _max_pt2[GIET_NB_VSPACE_MAX] =
{ [0 ... GIET_NB_VSPACE_MAX - 1] = 0 };

// Scheduler pointers array (virtual addresses)
// indexed by (x,y,lpid) : ((x << Y_WIDTH) + y)*NB_PROCS_MAX + lpid
__attribute__((section (".bootdata"))) 
static_scheduler_t* _schedulers[NB_PROCS_MAX<<(X_WIDTH+Y_WIDTH)];


/////////////////////////////////////////////////////////////////////
// This function checks consistence beween the  mapping_info data 
// structure (soft), and the giet_config file (hard).
/////////////////////////////////////////////////////////////////////
void boot_mapping_check() 
{
    mapping_header_t * header = (mapping_header_t *)SEG_BOOT_MAPPING_BASE;

    // checking mapping availability
    if (header->signature != IN_MAPPING_SIGNATURE) 
    {
        _puts("\n[BOOT ERROR] Illegal mapping signature: ");
        _putx(header->signature);
        _puts("\n");
        _exit();
    }

    // checking number of clusters
    if ( (header->x_size  != X_SIZE)  || 
         (header->y_size  != Y_SIZE)  ||
         (header->x_width != X_WIDTH) ||
         (header->y_width != Y_WIDTH) )
    {
        _puts("\n[BOOT ERROR] Incoherent X_SIZE or Y_SIZE ");
        _puts("\n             - In hard_config:  X_SIZE = ");
        _putd( X_SIZE );
        _puts(" / Y_SIZE = ");
        _putd( Y_SIZE );
        _puts(" / X_WIDTH = ");
        _putd( X_WIDTH );
        _puts(" / Y_WIDTH = ");
        _putd( Y_WIDTH );
        _puts("\n             - In mapping_info: x_size = ");
        _putd( header->x_size );
        _puts(" / y_size = ");
        _putd( header->y_size );
        _puts(" / x_width = ");
        _putd( header->x_width );
        _puts(" / y_width = ");
        _putd( header->y_width );
        _puts("\n");
        _exit();
    }
    // checking number of virtual spaces
    if (header->vspaces > GIET_NB_VSPACE_MAX) 
    {
        _puts("\n[BOOT ERROR] : number of vspaces > GIET_NB_VSPACE_MAX\n");
        _puts("\n");
        _exit();
    }

#if BOOT_DEBUG_MAPPING
_puts("\n - x_size    = ");
_putd( header->x_size );
_puts("\n - y_size    = ");
_putd( header->y_size );
_puts("\n - procs     = ");
_putd( header->procs );
_puts("\n - periphs   = ");
_putd( header->periphs );
_puts("\n - vspaces   = ");
_putd( header->vspaces );
_puts("\n - tasks     = ");
_putd( header->tasks );
_puts("\n");
_puts("\n - size of header  = ");
_putd( MAPPING_HEADER_SIZE );
_puts("\n - size of cluster = ");
_putd( MAPPING_CLUSTER_SIZE );
_puts("\n - size of pseg    = ");
_putd( MAPPING_PSEG_SIZE );
_puts("\n - size of proc    = ");
_putd( MAPPING_PROC_SIZE );
_puts("\n - size of vspace  = ");
_putd( MAPPING_VSPACE_SIZE );
_puts("\n - size of vseg    = ");
_putd( MAPPING_VSEG_SIZE );
_puts("\n - size of vobj    = ");
_putd( MAPPING_VOBJ_SIZE );
_puts("\n - size of task    = ");
_putd( MAPPING_TASK_SIZE );
_puts("\n");

unsigned int cluster_id;
mapping_cluster_t * cluster = _get_cluster_base(header);
for( cluster_id = 0; cluster_id < X_SIZE*Y_SIZE ; cluster_id++) 
{
    _puts("\n - cluster[");
    _putd( cluster[cluster_id].x );
    _puts(",");
    _putd( cluster[cluster_id].y );
    _puts("]\n   procs   = ");
    _putd( cluster[cluster_id].procs );
    _puts("\n   psegs   = ");
    _putd( cluster[cluster_id].psegs );
    _puts("\n   periphs = ");
    _putd( cluster[cluster_id].periphs );
    _puts("\n");
}
#endif

} // end boot_mapping_check()


//////////////////////////////////////////////////////////////////////////////
//     boot_pseg_get() 
// This function returns the pointer on a physical segment
// identified  by the pseg index.
//////////////////////////////////////////////////////////////////////////////
mapping_pseg_t *boot_pseg_get(unsigned int seg_id) 
{
    mapping_header_t* header = (mapping_header_t*)SEG_BOOT_MAPPING_BASE;
    mapping_pseg_t * pseg    = _get_pseg_base(header);

    // checking argument
    if (seg_id >= header->psegs) 
    {
        _puts("\n[BOOT ERROR] : seg_id argument too large\n");
        _puts("               in function boot_pseg_get()\n");
        _exit();
    }

    return &pseg[seg_id];
}  

//////////////////////////////////////////////////////////////////////////////
// boot_add_pte() 
// This function registers a new PTE in the page table defined
// by the vspace_id argument, and updates both PT1 and PT2.
// A new PT2 is used when required.
// As the set of PT2s is implemented as a fixed size array (no dynamic 
// allocation), this function checks a possible overflow of the PT2 array.
//////////////////////////////////////////////////////////////////////////////
void boot_add_pte(unsigned int vspace_id,
                  unsigned int vpn, 
                  unsigned int flags, 
                  unsigned int ppn,
                  unsigned int verbose) 
{
    unsigned int ix1;
    unsigned int ix2;
    paddr_t      pt1_pbase;     // PT1 physical base address
    paddr_t      pt2_pbase = 0; // PT2 physical base address
    paddr_t      pte_paddr;     // PTE physucal address
    unsigned int pt2_id;        // PT2 index
    unsigned int ptd;           // PTD : entry in PT1
    unsigned int max_pt2;       // max number of PT2s for a given vspace

    ix1 = vpn >> 9;         // 11 bits
    ix2 = vpn & 0x1FF;      //  9 bits

    // check that the _max_pt2[vspace_id] has been set
    max_pt2 = _max_pt2[vspace_id];

    if (max_pt2 == 0) 
    {
        _puts("Undefined page table for vspace ");
        _putd(vspace_id);
        _puts("\n");
        _exit();
    }


    // get page table physical base address
    pt1_pbase = _ptabs_paddr[vspace_id];

    // get ptd in PT1
    ptd = _physical_read(pt1_pbase + 4 * ix1);

    if ((ptd & PTE_V) == 0)    // invalid PTD: compute PT2 base address, 
                               // and set a new PTD in PT1 
    {
        pt2_id = _next_free_pt2[vspace_id];
        if (pt2_id == max_pt2) 
        {
            _puts("\n[BOOT ERROR] in boot_add_pte() function\n");
            _puts("the length of the ptab vobj is too small\n");

            _puts(" max_pt2 = ");
            _putd( max_pt2 );
            _puts("\n");
            _puts(" pt2_id  = ");
            _putd( pt2_id );
            _puts("\n");
            
            _exit();
        }
        else 
        {
            pt2_pbase = pt1_pbase + PT1_SIZE + PT2_SIZE * pt2_id;
            ptd = PTE_V | PTE_T | (unsigned int) (pt2_pbase >> 12);
            _physical_write( pt1_pbase + 4 * ix1, ptd);
            _next_free_pt2[vspace_id] = pt2_id + 1;
        }
    }
    else                       // valid PTD: compute PT2 base address
    {
        pt2_pbase = ((paddr_t)(ptd & 0x0FFFFFFF)) << 12;
    }

    // set PTE in PT2 : flags & PPN in two 32 bits words
    pte_paddr = pt2_pbase + 8 * ix2;
    _physical_write(pte_paddr    , flags);
    _physical_write(pte_paddr + 4, ppn);

    if (verbose)
    {
        _puts(" / vpn = ");
        _putx( vpn );
        _puts(" / ix1 = ");
        _putx( ix1 );
        _puts(" / ix2 = ");
        _putx( ix2 );
        _puts(" / pt1_pbase = ");
        _putl( pt1_pbase );
        _puts(" / ptd = ");
        _putl( ptd );
        _puts(" / pt2_pbase = ");
        _putl( pt2_pbase );
        _puts(" / pte_paddr = ");
        _putl( pte_paddr );
        _puts(" / ppn = ");
        _putx( ppn );
        _puts("/\n");
    }

}   // end boot_add_pte()


/////////////////////////////////////////////////////////////////////
// This function build the page table for a given vspace. 
// The physical base addresses for all vsegs (global and private)
// must have been previously computed and stored in the mapping.
// It initializes the MWMR channels.
/////////////////////////////////////////////////////////////////////
void boot_vspace_pt_build(unsigned int vspace_id) 
{
    unsigned int vseg_id;
    unsigned int npages;
    unsigned int ppn;
    unsigned int vpn;
    unsigned int flags;
    unsigned int page_id;
    unsigned int verbose = 0;   // can be used to activate trace in add_pte()

    mapping_header_t * header = (mapping_header_t *)SEG_BOOT_MAPPING_BASE;
    mapping_vspace_t * vspace = _get_vspace_base(header);
    mapping_vseg_t   * vseg   = _get_vseg_base(header);

    // private segments
    for (vseg_id = vspace[vspace_id].vseg_offset;
         vseg_id < (vspace[vspace_id].vseg_offset + vspace[vspace_id].vsegs);
         vseg_id++) 
    {
        vpn = vseg[vseg_id].vbase >> 12;
        ppn = (unsigned int) (vseg[vseg_id].pbase >> 12);

        npages = vseg[vseg_id].length >> 12;
        if ((vseg[vseg_id].length & 0xFFF) != 0) npages++; 

        flags = PTE_V;
        if (vseg[vseg_id].mode & C_MODE_MASK) flags |= PTE_C;
        if (vseg[vseg_id].mode & X_MODE_MASK) flags |= PTE_X;
        if (vseg[vseg_id].mode & W_MODE_MASK) flags |= PTE_W;
        if (vseg[vseg_id].mode & U_MODE_MASK) flags |= PTE_U;
        
        // These three flags (Local, Remote and Dirty) are set to 1 to reduce
        // latency of TLB miss (L/R) and write (D): Avoid hardware update
        // mechanism for these flags. This optimization can be performed
        // because GIET_VM does nothing with these flags.

        flags |= PTE_L;
        flags |= PTE_R;
        flags |= PTE_D;

#if BOOT_DEBUG_PT
        _puts(vseg[vseg_id].name);
        _puts(" : flags = ");
        _putx(flags);
        _puts(" / npages = ");
        _putd(npages);
        _puts(" / pbase = ");
        _putl(vseg[vseg_id].pbase);
        _puts("\n");
#endif
        // loop on 4K pages
        for (page_id = 0; page_id < npages; page_id++) 
        {
            boot_add_pte(vspace_id, vpn, flags, ppn, verbose);
            vpn++;
            ppn++;
        }
    }

    // global segments
    for (vseg_id = 0; vseg_id < header->globals; vseg_id++) 
    {
        vpn = vseg[vseg_id].vbase >> 12;
        ppn = (unsigned int)(vseg[vseg_id].pbase >> 12);
        npages = vseg[vseg_id].length >> 12;
        if ((vseg[vseg_id].length & 0xFFF) != 0) npages++;

        flags = PTE_V;
        if (vseg[vseg_id].mode & C_MODE_MASK) flags |= PTE_C;
        if (vseg[vseg_id].mode & X_MODE_MASK) flags |= PTE_X;
        if (vseg[vseg_id].mode & W_MODE_MASK) flags |= PTE_W;
        if (vseg[vseg_id].mode & U_MODE_MASK) flags |= PTE_U;

        // Flags set for optimization (as explained above)

        flags |= PTE_L;
        flags |= PTE_R;
        flags |= PTE_D;

#if BOOT_DEBUG_PT
        _puts(vseg[vseg_id].name);
        _puts(" : flags = ");
        _putx(flags);
        _puts(" / npages = ");
        _putd(npages);
        _puts(" / pbase = ");
        _putl(vseg[vseg_id].pbase);
        _puts("\n");
#endif
        // loop on 4K pages
        for (page_id = 0; page_id < npages; page_id++) 
        {
            boot_add_pte(vspace_id, vpn, flags, ppn, verbose);
            vpn++;
            ppn++;
        }
    }
}   // end boot_vspace_pt_build()


///////////////////////////////////////////////////////////////////////////
// Align the value of paddr or vaddr to the required alignement,
// defined by alignPow2 == L2(alignement).
///////////////////////////////////////////////////////////////////////////
paddr_t paddr_align_to(paddr_t paddr, unsigned int alignPow2) 
{
    paddr_t mask = (1 << alignPow2) - 1;
    return ((paddr + mask) & ~mask);
}

unsigned int vaddr_align_to(unsigned int vaddr, unsigned int alignPow2) 
{
    unsigned int mask = (1 << alignPow2) - 1;
    return ((vaddr + mask) & ~mask);
}

///////////////////////////////////////////////////////////////////////////
// Set pbase for a vseg when identity mapping is required.
// The length of the vseg must be known.
// The ordered linked list of vsegs mapped on pseg must be updated,
// and overlap with previously mapped vsegs must be checked.
///////////////////////////////////////////////////////////////////////////
void boot_vseg_set_paddr_ident(mapping_vseg_t * vseg) 
{
    // checking vseg not already mapped
    if (vseg->mapped != 0) 
    {
        _puts("\n[BOOT ERROR] in boot_vseg_set_paddr_ident() : vseg ");
        _puts( vseg->name );
        _puts(" already mapped\n");
        _exit();
    }

    // computes selected pseg pointer 
    mapping_pseg_t* pseg = boot_pseg_get( vseg->psegid );

    // computes vseg alignment constraint
    mapping_header_t* header    = (mapping_header_t*)SEG_BOOT_MAPPING_BASE;
    mapping_vobj_t*   vobj_base = _get_vobj_base( header );
    unsigned int      align     = vobj_base[vseg->vobj_offset].align;
    if ( vobj_base[vseg->vobj_offset].align < 12 ) align = 12;

    // computes required_pbase for identity mapping,
    paddr_t required_pbase = (paddr_t)vseg->vbase;

    // checks identity constraint against alignment constraint
    if ( paddr_align_to( required_pbase, align) != required_pbase )
    {
        _puts("\n[BOOT ERROR] in boot_vseg_set_paddr_ident() : vseg ");
        _puts( vseg->name );
        _puts(" has uncompatible identity and alignment constraints\n");
        _exit();
    }

    // We are looking for a contiguous space in target pseg.
    // If there is vsegs already mapped, we scan the vsegs list to: 
    // - check overlap with already mapped vsegs,
    // - try mapping in holes between already mapped vsegs,
    // - update the ordered linked list if success
    // We don't enter the loop if no vsegs is already mapped.
    // implementation note: The next_vseg field is unsigned int,
    // but we use it to store a MIP32 pointer on a vseg...

    mapping_vseg_t*   curr      = 0;
    mapping_vseg_t*   prev      = 0;
    unsigned int      min_pbase = pseg->base;

    for ( curr = (mapping_vseg_t*)pseg->next_vseg ; 
          (curr != 0) && (vseg->mapped == 0) ; 
          curr = (mapping_vseg_t*)curr->next_vseg )
    {
        // looking before current vseg
        if( (required_pbase >= min_pbase) && 
            (curr->pbase >= (required_pbase + vseg->length)) ) // space found
        {
            vseg->pbase  = required_pbase;
            vseg->mapped = 1;

            // update linked list
            vseg->next_vseg = (unsigned int)curr;
            if( curr == (mapping_vseg_t*)pseg->next_vseg ) 
                pseg->next_vseg = (unsigned int)vseg;
            else
                prev->next_vseg = (unsigned int)vseg;
        }
        else                                         // looking in space after curr
        {
            prev = curr;
            min_pbase = curr->pbase + curr->length;
        }
    }

    // no success in the loop
    if( (vseg->mapped == 0) &&
        (required_pbase >= min_pbase) && 
        ((required_pbase + vseg->length) <= (pseg->base + pseg->length)) )
    {
        vseg->pbase  = required_pbase;
        vseg->mapped = 1;

        // update linked list
        vseg->next_vseg = 0;
        if ((curr == 0) && (prev == 0)) pseg->next_vseg = (unsigned int)vseg;
        else                            prev->next_vseg = (unsigned int)vseg;
    }

    if( vseg->mapped == 0 )
    {
        _puts("\n[BOOT ERROR] in boot_vseg_set_paddr_ident() : vseg ");
        _puts( vseg->name );
        _puts(" cannot be mapped on pseg ");
        _puts( pseg->name );
        _puts("\n");
        _exit();
    }
}  // end boot_vseg_set_paddr_ident()

                
////////////////////////////////////////////////////////////////////////////
// Set pbase for a vseg when there is no identity mapping constraint.
// This is the physical memory allocator (written by Q.Meunier).
// The length of the vseg must be known.
// All identity mapping vsegs must be already mapped.
// We use a linked list of already mapped vsegs, ordered by incresing pbase.
// We try to place the vseg in the "first fit" hole in this list.
////////////////////////////////////////////////////////////////////////////
void boot_vseg_set_paddr(mapping_vseg_t * vseg) 
{
    // checking vseg not already mapped
    if ( vseg->mapped != 0 ) 
    {
        _puts("\n[BOOT ERROR] in boot_vseg_set_paddr() : vseg ");
        _puts( vseg->name );
        _puts(" already mapped\n");
        _exit();
    }

    // computes selected pseg pointer 
    mapping_pseg_t*   pseg      = boot_pseg_get( vseg->psegid );

    // computes vseg alignment constraint
    mapping_header_t* header    = (mapping_header_t*)SEG_BOOT_MAPPING_BASE;
    mapping_vobj_t*   vobj_base = _get_vobj_base( header );
    unsigned int      align     = vobj_base[vseg->vobj_offset].align;
    if ( vobj_base[vseg->vobj_offset].align < 12 ) align = 12;

    // initialise physical base address, with alignment constraint
    paddr_t possible_pbase = paddr_align_to( pseg->base, align );

    // We are looking for a contiguous space in target pseg
    // If there is vsegs already mapped, we scan the vsegs list to: 
    // - try mapping in holes between already mapped vsegs,
    // - update the ordered linked list if success
    // We don't enter the loop if no vsegs is already mapped.
    // implementation note: The next_vseg field is unsigned int,
    // but we use it to store a MIP32 pointer on a vseg...

    mapping_vseg_t*   curr = 0;
    mapping_vseg_t*   prev = 0;

    for( curr = (mapping_vseg_t*)pseg->next_vseg ; 
         (curr != 0) && (vseg->mapped == 0) ; 
         curr = (mapping_vseg_t*)curr->next_vseg )
    {
        // looking for space before current vseg
        if ( (curr->pbase >= possible_pbase + vseg->length) ) // space before curr
        {
            vseg->pbase  = possible_pbase;
            vseg->mapped = 1;

            // update linked list
            vseg->next_vseg = (unsigned int)curr;
            if( curr == (mapping_vseg_t*)pseg->next_vseg ) 
                pseg->next_vseg = (unsigned int)vseg;
            else
                prev->next_vseg = (unsigned int)vseg;
        }
        else                                            // looking for space after curr
        {
            possible_pbase = paddr_align_to( curr->pbase + curr->length, align );
            prev           = curr;
        }
    }
        
    // when no space found, try to allocate space after already mapped vsegs
    if( (vseg->mapped == 0) &&
        ((possible_pbase + vseg->length) <= (pseg->base + pseg->length)) )
    {
        vseg->pbase  = possible_pbase;
        vseg->mapped = 1;

        // update linked list
        vseg->next_vseg = 0;
        if ((curr == 0 ) && (prev == 0)) pseg->next_vseg = (unsigned int)vseg;
        else                             prev->next_vseg = (unsigned int)vseg;
    }

    if( vseg->mapped == 0 )
    {
        _puts("\n[BOOT ERROR] in boot_vseg_set_paddr() : vseg ");
        _puts( vseg->name );
        _puts(" cannot be mapped on pseg ");
        _puts( pseg->name );
        _puts(" in cluster[");
        _putd( pseg->clusterid );
        _puts("]\n");
        _exit();
    }
}  // end boot_vseg_set_paddr()

///////////////////////////////////////////////////////////////////////////
// This function computes the physical base address for a vseg
// as specified in the mapping info data structure.
// It updates the pbase and the length fields of the vseg.
// It updates the pbase and vbase fields of all vobjs in the vseg.
// It updates the _ptabs_paddr[] and _ptabs_vaddr[] arrays. 
// It is a global vseg if vspace_id = (-1). 
///////////////////////////////////////////////////////////////////////////
void boot_vseg_map(mapping_vseg_t * vseg, unsigned int vspace_id) 
{
    unsigned int vobj_id;
    unsigned int cur_vaddr;
    paddr_t      cur_paddr;
    paddr_t      cur_length;
    unsigned int offset;

    mapping_header_t * header = (mapping_header_t *)SEG_BOOT_MAPPING_BASE;
    mapping_vobj_t   * vobj   = _get_vobj_base(header);

    // loop on the vobjs contained in vseg to compute 
    // the vseg length, required for mapping.
    cur_length = 0;
    for ( vobj_id = vseg->vobj_offset; 
          vobj_id < (vseg->vobj_offset + vseg->vobjs); 
          vobj_id++ ) 
    {
        if (vobj[vobj_id].align)
        {
            cur_length = vaddr_align_to(cur_length, vobj[vobj_id].align);
        }
        cur_length += vobj[vobj_id].length;
    }
    vseg->length = paddr_align_to(cur_length, 12);

    // mapping: computes vseg pbase address
    if (vseg->ident != 0)                         // identity mapping 
    {
        boot_vseg_set_paddr_ident( vseg );
    }
    else                                          // unconstrained mapping
    {
        boot_vseg_set_paddr( vseg );
    }

    // second loop on vobjs contained in vseg to :
    // initialize the vaddr and paddr fields of all vobjs,
    // and initialize the page table pointers arrays 

    cur_vaddr = vseg->vbase;
    cur_paddr = vseg->pbase;

    for (vobj_id = vseg->vobj_offset; 
         vobj_id < (vseg->vobj_offset + vseg->vobjs); vobj_id++) 
    {
        if (vobj[vobj_id].align) 
        {
            cur_paddr = paddr_align_to(cur_paddr, vobj[vobj_id].align);
            cur_vaddr = vaddr_align_to(cur_vaddr, vobj[vobj_id].align);
        }
        // set vaddr/paddr for current vobj
        vobj[vobj_id].vaddr = cur_vaddr;
        vobj[vobj_id].paddr = cur_paddr;
        
        // initialize _ptabs_vaddr[] & boot_ptabs-paddr[] if PTAB
        if (vobj[vobj_id].type == VOBJ_TYPE_PTAB) 
        {
            if (vspace_id == ((unsigned int) -1))    // global vseg
            {
                _puts("\n[BOOT ERROR] in boot_vseg_map() function: ");
                _puts("a PTAB vobj cannot be global");
                _exit();
            }
            // we need at least one PT2
            if (vobj[vobj_id].length < (PT1_SIZE + PT2_SIZE)) 
            {
                _puts("\n[BOOT ERROR] in boot_vseg_map() function, ");
                _puts("PTAB too small, minumum size is: ");
                _putx(PT1_SIZE + PT2_SIZE);
                _exit();
            }
            // register both physical and virtual page table address
            _ptabs_vaddr[vspace_id] = vobj[vobj_id].vaddr;
            _ptabs_paddr[vspace_id] = vobj[vobj_id].paddr;
            
            // reset all valid bits in PT1
            for ( offset = 0 ; offset < 8192 ; offset = offset + 4)
            {
                _physical_write(cur_paddr + offset, 0);
            }

            // computing the number of second level pages
            _max_pt2[vspace_id] = (vobj[vobj_id].length - PT1_SIZE) / PT2_SIZE;
        }

        // set next vaddr/paddr
        cur_vaddr = cur_vaddr + vobj[vobj_id].length;
        cur_paddr = cur_paddr + vobj[vobj_id].length;
    } // end for vobjs

}    // end boot_vseg_map()

/////////////////////////////////////////////////////////////////////
// This function builds the page tables for all virtual spaces 
// defined in the mapping_info data structure, in three steps:
// - step 1 : It computes the physical base address for global vsegs
//            and for all associated vobjs.
// - step 2 : It computes the physical base address for all private 
//            vsegs and all vobjs in each virtual space.
// - step 3 : It actually fill the page table for each vspace.
//
// Note: It must exist at least one vspace in the mapping_info...
/////////////////////////////////////////////////////////////////////
void boot_pt_init() 
{
    mapping_header_t * header = (mapping_header_t *)SEG_BOOT_MAPPING_BASE;
    mapping_vspace_t * vspace = _get_vspace_base(header);
    mapping_vseg_t   * vseg   = _get_vseg_base(header);

    unsigned int vspace_id;
    unsigned int vseg_id;

    if (header->vspaces == 0 )
    {
        _puts("\n[BOOT ERROR] in boot_pt_init() : mapping ");
        _puts( header->name );
        _puts(" contains no vspace\n");
        _exit();
    }

#if BOOT_DEBUG_PT
_puts("\n[BOOT DEBUG] ****** mapping global vsegs ******\n");
#endif

    // step 1 : loop on global vsegs 

    // vsegs with identity mapping constraint first
    for (vseg_id = 0; vseg_id < header->globals; vseg_id++) 
    {
        if (vseg[vseg_id].ident == 1) 
            boot_vseg_map(&vseg[vseg_id], ((unsigned int) (-1)));
    }

    // unconstrained vsegs second
    for (vseg_id = 0; vseg_id < header->globals; vseg_id++) 
    {
        if (vseg[vseg_id].ident == 0) 
            boot_vseg_map(&vseg[vseg_id], ((unsigned int) (-1)));
    }

    // step 2 : loop on virtual vspaces to map private vsegs
    for (vspace_id = 0; vspace_id < header->vspaces; vspace_id++) 
    {

#if BOOT_DEBUG_PT
_puts("\n[BOOT DEBUG] ****** mapping private vsegs in vspace ");
_puts(vspace[vspace_id].name);
_puts(" ******\n");
#endif

        // vsegs with identity mapping constraint first
        for (vseg_id = vspace[vspace_id].vseg_offset;
             vseg_id < (vspace[vspace_id].vseg_offset + vspace[vspace_id].vsegs);
             vseg_id++) 
        {
            if (vseg[vseg_id].ident == 1) 
                boot_vseg_map(&vseg[vseg_id], vspace_id);
        }
        // unconstrained vsegs second
        for (vseg_id = vspace[vspace_id].vseg_offset;
             vseg_id < (vspace[vspace_id].vseg_offset + vspace[vspace_id].vsegs);
             vseg_id++) 
        {
            if (vseg[vseg_id].ident == 0)
                boot_vseg_map(&vseg[vseg_id], vspace_id);
        }
    }

#if BOOT_DEBUG_PT
mapping_vseg_t*    curr;
mapping_pseg_t*    pseg    = _get_pseg_base(header);
mapping_cluster_t* cluster = _get_cluster_base(header);
unsigned int       pseg_id;
for( pseg_id = 0 ; pseg_id < header->psegs ; pseg_id++ )
{
    unsigned int cluster_id = pseg[pseg_id].clusterid;
    _puts("\n[BOOT DEBUG] ****** vsegs mapped on pseg ");
    _puts( pseg[pseg_id].name );
    _puts(" in cluster[");
    _putd( cluster[cluster_id].x );
    _puts(",");
    _putd( cluster[cluster_id].y );
    _puts("] ******\n");
    for( curr = (mapping_vseg_t*)pseg[pseg_id].next_vseg ;
         curr != 0 ;
         curr = (mapping_vseg_t*)curr->next_vseg )
    {
        _puts(" - vseg ");
        _puts( curr->name );
        _puts(" : len = ");
        _putx( curr->length );
        _puts(" / vbase ");
        _putx( curr->vbase );
        _puts(" / pbase ");
        _putl( curr->pbase );
        _puts("\n");
    }  
}
#endif

    // step 3 : loop on the vspaces to build the page tables
    for (vspace_id = 0; vspace_id < header->vspaces; vspace_id++) 
    {

#if BOOT_DEBUG_PT
_puts("\n[BOOT DEBUG] ****** building page table for vspace ");
_puts(vspace[vspace_id].name);
_puts(" ******\n");
#endif

        boot_vspace_pt_build(vspace_id);

        _puts("\n[BOOT] Page Table for vspace \"");
        _puts( vspace[vspace_id].name );
        _puts("\" completed at cycle ");
        _putd( _get_proctime() );
        _puts("\n");

#if BOOT_DEBUG_PT
_puts("  vaddr = ");
_putx( _ptabs_vaddr[vspace_id] );
_puts(" / paddr = ");
_putl( _ptabs_paddr[vspace_id] );
_puts(" / PT2 number = ");
_putd( _max_pt2[vspace_id] );
_puts("\n");
#endif
    }
} // end boot_pt_init()

///////////////////////////////////////////////////////////////////////////////
// This function initializes all private vobjs defined in the vspaces,
// such as mwmr channels, barriers and locks, because these vobjs 
// are not known, and not initialized by the compiler.
// The MMU is supposed to be activated...
///////////////////////////////////////////////////////////////////////////////
void boot_vobjs_init() 
{
    mapping_header_t* header = (mapping_header_t *)SEG_BOOT_MAPPING_BASE;
    mapping_vspace_t* vspace = _get_vspace_base(header);
    mapping_vobj_t* vobj     = _get_vobj_base(header);

    unsigned int vspace_id;
    unsigned int vobj_id;

    // loop on the vspaces
    for (vspace_id = 0; vspace_id < header->vspaces; vspace_id++) 
    {

#if BOOT_DEBUG_VOBJS
_puts("\n[BOOT DEBUG] ****** vobjs initialisation in vspace ");
_puts(vspace[vspace_id].name);
_puts(" ******\n");
#endif

        _set_mmu_ptpr( (unsigned int)(_ptabs_paddr[vspace_id] >> 13) );

        unsigned int ptab_found = 0;

        // loop on the vobjs
        for (vobj_id = vspace[vspace_id].vobj_offset;
             vobj_id < (vspace[vspace_id].vobj_offset + vspace[vspace_id].vobjs);
             vobj_id++) 
        {
            switch (vobj[vobj_id].type) 
            {
                case VOBJ_TYPE_MWMR:    // storage capacity is (vobj.length/4 - 5) words
                {
#if BOOT_DEBUG_VOBJS
_puts("MWMR    : ");
_puts(vobj[vobj_id].name);
_puts(" / vaddr = ");
_putx(vobj[vobj_id].vaddr);
_puts(" / paddr = ");
_putl(vobj[vobj_id].paddr);
_puts(" / length = ");
_putx(vobj[vobj_id].length);
_puts("\n");
#endif
                    mwmr_channel_t* mwmr = (mwmr_channel_t *) (vobj[vobj_id].vaddr);
                    mwmr->ptw = 0;
                    mwmr->ptr = 0;
                    mwmr->sts = 0;
                    mwmr->width = vobj[vobj_id].init;
                    mwmr->depth = (vobj[vobj_id].length >> 2) - 6;
                    mwmr->lock = 0;
#if BOOT_DEBUG_VOBJS
_puts("          fifo depth = ");
_putd(mwmr->depth);
_puts(" / width = ");
_putd(mwmr->width);
_puts("\n");
#endif
                    break;
                }
                case VOBJ_TYPE_ELF:    // initialisation done by the loader 
                {
#if BOOT_DEBUG_VOBJS
_puts("ELF     : ");
_puts(vobj[vobj_id].name);
_puts(" / vaddr = ");
_putx(vobj[vobj_id].vaddr);
_puts(" / paddr = ");
_putl(vobj[vobj_id].paddr);
_puts(" / length = ");
_putx(vobj[vobj_id].length);
_puts("\n");
#endif
                    break;
                }
                case VOBJ_TYPE_BLOB:    // initialisation done by the loader 
                {
#if BOOT_DEBUG_VOBJS
_puts("BLOB    : ");
_puts(vobj[vobj_id].name);
_puts(" / vaddr = ");
_putx(vobj[vobj_id].vaddr);
_puts(" / paddr = ");
_putl(vobj[vobj_id].paddr);
_puts(" / length = ");
_putx(vobj[vobj_id].length);
_puts("\n");
#endif
                    break;
                }
                case VOBJ_TYPE_BARRIER:    // init is the number of participants
                {
#if BOOT_DEBUG_VOBJS
_puts("BARRIER : ");
_puts(vobj[vobj_id].name);
_puts(" / vaddr = ");
_putx(vobj[vobj_id].vaddr);
_puts(" / paddr = ");
_putl(vobj[vobj_id].paddr);
_puts(" / length = ");
_putx(vobj[vobj_id].length);
_puts("\n");
#endif
                    giet_barrier_t* barrier = (giet_barrier_t *) (vobj[vobj_id].vaddr);
                    barrier->count = vobj[vobj_id].init;
                    barrier->init = vobj[vobj_id].init;
#if BOOT_DEBUG_VOBJS
_puts("          init_value = ");
_putd(barrier->init);
_puts("\n");
#endif
                    break;
                }
                case VOBJ_TYPE_LOCK:    // init value is "not taken"
                {
#if BOOT_DEBUG_VOBJS
_puts("LOCK    : ");
_puts(vobj[vobj_id].name);
_puts(" / vaddr = ");
_putx(vobj[vobj_id].vaddr);
_puts(" / paddr = ");
_putl(vobj[vobj_id].paddr);
_puts(" / length = ");
_putx(vobj[vobj_id].length);
_puts("\n");
#endif
                    unsigned int* lock = (unsigned int *) (vobj[vobj_id].vaddr);
                    *lock = 0;
                    break;
                }
                case VOBJ_TYPE_BUFFER:    // nothing to initialise
                {
#if BOOT_DEBUG_VOBJS
_puts("BUFFER  : ");
_puts(vobj[vobj_id].name);
_puts(" / vaddr = ");
_putx(vobj[vobj_id].vaddr);
_puts(" / paddr = ");
_putl(vobj[vobj_id].paddr);
_puts(" / length = ");
_putx(vobj[vobj_id].length);
_puts("\n");
#endif
                    break;
                }
                case VOBJ_TYPE_MEMSPACE:
                {
#if BOOT_DEBUG_VOBJS
_puts("MEMSPACE  : ");
_puts(vobj[vobj_id].name);
_puts(" / vaddr = ");
_putx(vobj[vobj_id].vaddr);
_puts(" / paddr = ");
_putl(vobj[vobj_id].paddr);
_puts(" / length = ");
_putx(vobj[vobj_id].length);
_puts("\n");
#endif
                    giet_memspace_t* memspace = (giet_memspace_t *) vobj[vobj_id].vaddr;
                    memspace->buffer = (void *) vobj[vobj_id].vaddr + 8;
                    memspace->size = vobj[vobj_id].length - 8;
#if BOOT_DEBUG_VOBJS
_puts("          buffer vbase = ");
_putx((unsigned int)memspace->buffer);
_puts(" / size = ");
_putx(memspace->size);
_puts("\n");
#endif
                    break;
                }
                case VOBJ_TYPE_PTAB:    // nothing to initialize
                {
#if BOOT_DEBUG_VOBJS
_puts("PTAB    : ");
_puts(vobj[vobj_id].name);
_puts(" / vaddr = ");
_putx(vobj[vobj_id].vaddr);
_puts(" / paddr = ");
_putl(vobj[vobj_id].paddr);
_puts(" / length = ");
_putx(vobj[vobj_id].length);
_puts("\n");
#endif
                    ptab_found = 1;
                    break;
                }
                case VOBJ_TYPE_CONST:
                {
#if BOOT_DEBUG_VOBJS
_puts("CONST   : ");
_puts(vobj[vobj_id].name);
_puts(" / vaddr = ");
_putx(vobj[vobj_id].vaddr);
_puts(" / paddr = ");
_putl(vobj[vobj_id].paddr);
_puts(" / length = ");
_putx(vobj[vobj_id].length);
_puts(" / init = ");
_putx(vobj[vobj_id].init);
_puts("\n");
#endif
                    unsigned int* addr = (unsigned int *) vobj[vobj_id].vaddr;
                    *addr = vobj[vobj_id].init;

#if BOOT_DEBUG_VOBJS
_puts("          init = ");
_putx(*addr);
_puts("\n");
#endif
                    break;
                }
                default:
                {
                    _puts("\n[BOOT ERROR] illegal vobj type: ");
                    _putd(vobj[vobj_id].type);
                    _puts("\n");
                    _exit();
                }
            }            // end switch type
        }            // end loop on vobjs
        if (ptab_found == 0) 
        {
            _puts("\n[BOOT ERROR] Missing PTAB for vspace ");
            _putd(vspace_id);
            _exit();
        }
    } // end loop on vspaces

} // end boot_vobjs_init()

///////////////////////////////////////////////////////////////////////////////
// This function returns in the vbase and length buffers the virtual base 
// address and the length of the  segment allocated to the schedulers array 
// in the cluster defined by the clusterid argument.
///////////////////////////////////////////////////////////////////////////////
void boot_get_sched_vaddr( unsigned int  cluster_id,
                           unsigned int* vbase, 
                           unsigned int* length )
{
    mapping_header_t* header = (mapping_header_t *)SEG_BOOT_MAPPING_BASE;
    mapping_vobj_t*   vobj   = _get_vobj_base(header);
    mapping_vseg_t*   vseg   = _get_vseg_base(header);
    mapping_pseg_t*   pseg   = _get_pseg_base(header);

    unsigned int vseg_id;
    unsigned int found = 0;

    for ( vseg_id = 0 ; (vseg_id < header->vsegs) && (found == 0) ; vseg_id++ )
    {
        if ( (vobj[vseg[vseg_id].vobj_offset].type == VOBJ_TYPE_SCHED) && 
             (pseg[vseg[vseg_id].psegid].clusterid == cluster_id ) )
        {
            *vbase  = vseg[vseg_id].vbase;
            *length = vobj[vseg[vseg_id].vobj_offset].length;
            found = 1;
        }
    }
    if ( found == 0 )
    {
        mapping_cluster_t* cluster = _get_cluster_base(header);
        _puts("\n[BOOT ERROR] No vobj of type SCHED in cluster [");
        _putd( cluster[cluster_id].x );
        _puts(",");
        _putd( cluster[cluster_id].y );
        _puts("]\n");
        _exit();
    }
} // end boot_get_sched_vaddr()

////////////////////////////////////////////////////////////////////////////////////
// This function initialises all processors schedulers.
// This is done by processor 0, and the MMU must be activated.
// - In Step 1, it initialises the _schedulers[gpid] pointers array, and scan
//              the processors to initialise the schedulers, including the
//              idle_task context (ltid == 14) and HWI / SWI / PTI vectors. 
// - In Step 2, it scan all tasks in all vspaces to complete the tasks contexts, 
//              initialisation as specified in the mapping_info data structure. 
////////////////////////////////////////////////////////////////////////////////////
void boot_schedulers_init() 
{
    mapping_header_t*  header  = (mapping_header_t *)SEG_BOOT_MAPPING_BASE;
    mapping_cluster_t* cluster = _get_cluster_base(header);
    mapping_vspace_t*  vspace  = _get_vspace_base(header);
    mapping_task_t*    task    = _get_task_base(header);
    mapping_vobj_t*    vobj    = _get_vobj_base(header);
    mapping_periph_t*  periph  = _get_periph_base(header);
    mapping_irq_t*     irq     = _get_irq_base(header);

    unsigned int cluster_id;    // cluster index in mapping_info 
    unsigned int periph_id;     // peripheral index in mapping_info
    unsigned int irq_id;        // irq index in mapping_info
    unsigned int vspace_id;     // vspace index in mapping_info
    unsigned int task_id;       // task index in mapping_info
    unsigned int vobj_id;       // vobj index in mapping_info

    unsigned int lpid;          // local processor index (for several loops)

    // TTY, NIC, CMA, HBA, user timer, and WTI channel allocators to user tasks:
    // - TTY[0] is reserved for the kernel
    // - In all clusters the first NB_PROCS_MAX timers 
    //   are reserved for the kernel (context switch)
    unsigned int alloc_tty_channel = 1;              // global
    unsigned int alloc_nic_channel = 0;              // global
    unsigned int alloc_cma_channel = 0;              // global
    unsigned int alloc_hba_channel = 0;              // global
    unsigned int alloc_tim_channel[X_SIZE*Y_SIZE];   // one per cluster 

    // WTI allocators to processors 
    // In all clusters, first NB_PROCS_MAX WTIs are for WAKUP
    unsigned int alloc_wti_channel[X_SIZE*Y_SIZE];   // one per cluster

    // pointers on the XCU and PIC peripherals
    mapping_periph_t*  xcu = NULL;
    mapping_periph_t*  pic = NULL;

    // schedulers array base address in a cluster
    unsigned int          sched_vbase;  
    unsigned int          sched_length;  
    static_scheduler_t*   psched;  

    /////////////////////////////////////////////////////////////////////////
    // Step 1 : loop on the clusters and on the processors 
    //          to initialize the schedulers[] array of pointers,
    //          and the interrupt vectors.
    // Implementation note:
    // We need to use both proc_id to scan the mapping info structure,
    // and lpid to access the schedulers array.
    // - the _schedulers[] array of pointers can contain "holes", because
    //   it is indexed by the global pid = cluster_xy*NB_PROCS_MAX + lpid
    // - the mapping info array of processors is contiguous, it is indexed 
    //   by proc_id, and use an offset specific in each cluster.

    for (cluster_id = 0 ; cluster_id < X_SIZE*Y_SIZE ; cluster_id++) 
    {
        unsigned int x          = cluster[cluster_id].x;
        unsigned int y          = cluster[cluster_id].y;
        unsigned int cluster_xy = (x<<Y_WIDTH) + y;

#if BOOT_DEBUG_SCHED
_puts("\n[BOOT DEBUG] Initialise schedulers in cluster[");
_putd( x );
_puts(",");
_putd( y );
_puts("]\n");
#endif
        alloc_tim_channel[cluster_id] = NB_PROCS_MAX;
        alloc_wti_channel[cluster_id] = NB_PROCS_MAX;

        // checking processors number
        if ( cluster[cluster_id].procs > NB_PROCS_MAX )
        {
            _puts("\n[BOOT ERROR] Too much processors in cluster[");
            _putd( x );
            _puts(",");
            _putd( y );
            _puts("]\n");
            _exit();
        }
 
        // no schedulers initialisation if nprocs == 0
        if ( cluster[cluster_id].procs > 0 )
        {
            // get scheduler array virtual base address from mapping
            boot_get_sched_vaddr( cluster_id, &sched_vbase, &sched_length );

            if ( sched_length < (cluster[cluster_id].procs<<12) ) // 4 Kbytes per scheduler
            {
                _puts("\n[BOOT ERROR] Schedulers segment too small in cluster[");
                _putd( x );
                _puts(",");
                _putd( y );
                _puts("]\n");
                _exit();
            }

            psched = (static_scheduler_t*)sched_vbase;

            // scan peripherals to find the ICU/XCU and the PIC component

            xcu = NULL;  
            for ( periph_id = cluster[cluster_id].periph_offset ;
                  periph_id < cluster[cluster_id].periph_offset + cluster[cluster_id].periphs;
                  periph_id++ )
            {
                if( (periph[periph_id].type == PERIPH_TYPE_XCU) || 
                    (periph[periph_id].type == PERIPH_TYPE_ICU) )
                {
                    xcu = &periph[periph_id];

                    if ( xcu->arg < cluster[cluster_id].procs )
                    {
                        _puts("\n[BOOT ERROR] Not enough inputs for XCU[");
                        _putd( x );
                        _puts(",");
                        _putd( y );
                        _puts("]\n");
                        _exit();
                    }
                }
                if( periph[periph_id].type == PERIPH_TYPE_PIC )   
                {
                    pic = &periph[periph_id];
                }
            } 
            if ( xcu == NULL )
            {         
                _puts("\n[BOOT ERROR] No ICU / XCU component in cluster[");
                _putd( x );
                _puts(",");
                _putd( y );
                _puts("]\n");
                _exit();
            }

            // loop on processors for sechedulers default values
            // initialisation, including WTI and PTI vectors
            for ( lpid = 0 ; lpid < cluster[cluster_id].procs ; lpid++ )
            {
                // set the schedulers pointers array
                _schedulers[cluster_xy * NB_PROCS_MAX + lpid] = 
                      (static_scheduler_t*)&psched[lpid];

#if BOOT_DEBUG_SCHED
_puts("\nProc[");
_putd( x );
_puts(",");
_putd( y );
_puts(",");
_putd( lpid );
_puts("] : scheduler virtual base address = ");
_putx( (unsigned int)&psched[lpid] );
_puts("\n");
#endif
                // initialise the "tasks" and "current" variables default values
                psched[lpid].tasks   = 0;
                psched[lpid].current = IDLE_TASK_INDEX;

                // default values for HWI / PTI / SWI vectors (valid bit = 0)
                unsigned int slot;
                for (slot = 0; slot < 32; slot++)
                {
                    psched[lpid].hwi_vector[slot] = 0;
                    psched[lpid].pti_vector[slot] = 0;
                    psched[lpid].wti_vector[slot] = 0;
                }

                // WTI[lpid] <= ISR_WAKUP / PTI[lpid] <= ISR_TICK 
                psched[lpid].wti_vector[lpid] = ISR_WAKUP | 0x80000000;
                psched[lpid].pti_vector[lpid] = ISR_TICK  | 0x80000000;

                // initializes the idle_task context in scheduler:
                // - the SR slot is 0xFF03 because this task run in kernel mode.
                // - it uses the page table of vspace[0]
                // - it uses the kernel TTY terminal
                // - slots containing addresses (SP, RA, EPC, PTAB, PTPR)
                //   must be re-initialised by kernel_parallel_init()

                psched[lpid].context[IDLE_TASK_INDEX][CTX_CR_ID]    = 0;
                psched[lpid].context[IDLE_TASK_INDEX][CTX_SR_ID]    = 0xFF03;
                psched[lpid].context[IDLE_TASK_INDEX][CTX_PTPR_ID]  = _ptabs_paddr[0]>>13;
                psched[lpid].context[IDLE_TASK_INDEX][CTX_PTAB_ID]  = _ptabs_vaddr[0];
                psched[lpid].context[IDLE_TASK_INDEX][CTX_TTY_ID]   = 0;
                psched[lpid].context[IDLE_TASK_INDEX][CTX_LTID_ID]  = IDLE_TASK_INDEX;
                psched[lpid].context[IDLE_TASK_INDEX][CTX_VSID_ID]  = 0;
                psched[lpid].context[IDLE_TASK_INDEX][CTX_RUN_ID]   = 1;
            }  // end for processors

            // scan HWIs connected to local XCU 
            // for round-robin allocation to processors
            lpid = 0;
            for ( irq_id = xcu->irq_offset ;
                  irq_id < xcu->irq_offset + xcu->irqs ;
                  irq_id++ )
            {
                unsigned int type    = irq[irq_id].srctype;
                unsigned int srcid   = irq[irq_id].srcid;
                unsigned int isr     = irq[irq_id].isr & 0xFFFF;
                unsigned int channel = irq[irq_id].channel << 16;

                if ( (type != IRQ_TYPE_HWI) || (srcid > 31) )
                {
                    _puts("\n[BOOT ERROR] Bad IRQ in XCU of cluster[");
                    _putd( x );
                    _puts(",");
                    _putd( y );
                    _puts("]\n");
                    _exit();
                }

                psched[lpid].hwi_vector[srcid] = isr | channel | 0x80000000;
                lpid = (lpid + 1) % cluster[cluster_id].procs; 

            } // end for irqs
        } // end if nprocs > 0
    } // end for clusters

    // If there is an external PIC component, we scan HWIs connected to PIC
    // for Round Robin allocation (as WTI) to processors.
    // We allocate one WTI per processor, starting from proc[0,0,0], 
    // and we increment (cluster_id, lpid) as required.
    if ( pic != NULL )
    {   
        unsigned int cluster_id = 0;   // index in clusters array
        unsigned int lpid       = 0;   // processor local index

        // scan IRQS defined in PIC
        for ( irq_id = pic->irq_offset ;
              irq_id < pic->irq_offset + pic->irqs ;
              irq_id++ )
        {
            // compute next values for (cluster_id,lpid)
            // if no more procesor available in current cluster
            unsigned int overflow = 0;
            while ( (lpid >= cluster[cluster_id].procs) ||
                    (alloc_wti_channel[cluster_id] >= xcu->arg) )
            {
                overflow++;
                cluster_id = (cluster_id + 1) % (X_SIZE*Y_SIZE);
                lpid       = 0;

                // overflow detection
                if ( overflow > (X_SIZE*Y_SIZE*NB_PROCS_MAX*32) )
                {
                    _puts("\n[BOOT ERROR] Not enough processors for external IRQs\n");
                    _exit();
                }
            }

            unsigned int type    = irq[irq_id].srctype;
            unsigned int srcid   = irq[irq_id].srcid;
            unsigned int isr     = irq[irq_id].isr & 0xFFFF;
            unsigned int channel = irq[irq_id].channel << 16;

            if ( (type != IRQ_TYPE_HWI) || (srcid > 31) )
            {
                _puts("\n[BOOT ERROR] Bad IRQ in PIC component\n");
                _exit();
            }

            // get scheduler[cluster_id] address
            unsigned int x          = cluster[cluster_id].x;
            unsigned int y          = cluster[cluster_id].y;
            unsigned int cluster_xy = (x<<Y_WIDTH) + y;
            psched                  = _schedulers[cluster_xy * NB_PROCS_MAX];

            // update WTI vector for scheduler[cluster_id][lpid]
            unsigned int index = alloc_wti_channel[cluster_id];
            psched[lpid].wti_vector[index] = isr | channel | 0x80000000;
            alloc_wti_channel[cluster_id] = index + 1;
            lpid = lpid + 1;

            // update IRQ fields in mapping for PIC initialisation
            irq[irq_id].dest_id = index;
            irq[irq_id].dest_xy = cluster_xy;

        }  // end for IRQs
    } // end if PIC
                
#if BOOT_DEBUG_SCHED
for ( cluster_id = 0 ; cluster_id < (X_SIZE*Y_SIZE) ; cluster_id++ )
{
    unsigned int x          = cluster[cluster_id].x;
    unsigned int y          = cluster[cluster_id].y;
    unsigned int cluster_xy = (x<<Y_WIDTH) + y;
    psched                  = _schedulers[cluster_xy * NB_PROCS_MAX];
    unsigned int slot;
    unsigned int entry;
    for ( lpid = 0 ; lpid < cluster[cluster_id].procs ; lpid++ )
    {
        _puts("\n*** IRQS for proc[");
        _putd( x );
        _puts(",");
        _putd( y );
        _puts(",");
        _putd( lpid );
        _puts("]\n");
        for ( slot = 0 ; slot < 32 ; slot++ )
        {
            entry = psched[lpid].hwi_vector[slot];
            if ( entry & 0x80000000 ) 
            {
                _puts(" - HWI ");
                _putd( slot );
                _puts(" / isrtype = ");
                _putd( entry & 0xFFFF ); 
                _puts(" / channel = ");
                _putd( (entry >> 16) & 0x7FFF ); 
                _puts("\n");
            }
        }
        for ( slot = 0 ; slot < 32 ; slot++ )
        {
            entry = psched[lpid].wti_vector[slot];
            if ( entry & 0x80000000 ) 
            {
                _puts(" - WTI ");
                _putd( slot );
                _puts(" / isrtype = ");
                _putd( entry & 0xFFFF ); 
                _puts(" / channel = ");
                _putd( (entry >> 16) & 0x7FFF ); 
                _puts("\n");
            }
        }
        for ( slot = 0 ; slot < 32 ; slot++ )
        {
            entry = psched[lpid].pti_vector[slot];
            if ( entry & 0x80000000 ) 
            {
                _puts(" - PTI ");
                _putd( slot );
                _puts(" / isrtype = ");
                _putd( entry & 0xFFFF ); 
                _puts(" / channel = ");
                _putd( (entry >> 16) & 0x7FFF ); 
                _puts("\n");
            }
        }
    }
}
#endif

    ///////////////////////////////////////////////////////////////////
    // Step 2 : loop on the vspaces and the tasks  to complete
    //          the schedulers and task contexts initialisation.

    for (vspace_id = 0; vspace_id < header->vspaces; vspace_id++) 
    {
        // We must set the PTPR depending on the vspace, because the start_vector 
        // and the stack address are defined in virtual space.
        _set_mmu_ptpr( (unsigned int)(_ptabs_paddr[vspace_id] >> 13) );

        // loop on the tasks in vspace (task_id is the global index)
        for (task_id = vspace[vspace_id].task_offset;
             task_id < (vspace[vspace_id].task_offset + vspace[vspace_id].tasks);
             task_id++) 
        {
            // compute the cluster coordinates & local processor index
            unsigned int x          = cluster[task[task_id].clusterid].x;
            unsigned int y          = cluster[task[task_id].clusterid].y;
            unsigned int cluster_xy = (x<<Y_WIDTH) + y;
            unsigned int lpid       = task[task_id].proclocid;                 

#if BOOT_DEBUG_SCHED
_puts("\n[BOOT DEBUG] Initialise context for task ");
_puts( task[task_id].name );
_puts(" in vspace ");
_puts( vspace[vspace_id].name );
_puts("\n");
#endif
            // compute gpid (global processor index) and scheduler base address
            unsigned int gpid = cluster_xy * NB_PROCS_MAX + lpid;
            psched            = _schedulers[gpid];

            // ctx_sr : value required before an eret instruction
            unsigned int ctx_sr = 0x0000FF13;

            // ctx_ptpr : page table physical base address (shifted by 13 bit)
            unsigned int ctx_ptpr = (unsigned int)(_ptabs_paddr[vspace_id] >> 13);

            // ctx_ptab : page_table virtual base address
            unsigned int ctx_ptab = _ptabs_vaddr[vspace_id];

            // ctx_tty : TTY terminal global index provided by the global allocator
            //           Each user terminal is a private ressource: the number of
            //           requested terminal cannot be larger than NB_TTY_CHANNELS.             
            unsigned int ctx_tty = 0xFFFFFFFF;
            if (task[task_id].use_tty) 
            {
                if (alloc_tty_channel >= NB_TTY_CHANNELS) 
                {
                    _puts("\n[BOOT ERROR] TTY channel index too large for task ");
                    _puts(task[task_id].name);
                    _puts(" in vspace ");
                    _puts(vspace[vspace_id].name);
                    _puts("\n");
                    _exit();
                }
                ctx_tty = alloc_tty_channel;
                alloc_tty_channel++;
             }

            // ctx_nic : NIC channel global index provided by the global allocator
            //           Each channel is a private ressource: the number of
            //           requested channels cannot be larger than NB_NIC_CHANNELS.
            unsigned int ctx_nic = 0xFFFFFFFF;
            if (task[task_id].use_nic) 
            {
                if (alloc_nic_channel >= NB_NIC_CHANNELS) 
                {
                    _puts("\n[BOOT ERROR] NIC channel index too large for task ");
                    _puts(task[task_id].name);
                    _puts(" in vspace ");
                    _puts(vspace[vspace_id].name);
                    _puts("\n");
                    _exit();
                }
                ctx_nic = alloc_nic_channel;
                alloc_nic_channel++;
            }

            // ctx_cma : CMA channel global index provided by the global allocator
            //           Each channel is a private ressource: the number of
            //           requested channels cannot be larger than NB_NIC_CHANNELS.
            unsigned int ctx_cma = 0xFFFFFFFF;
            if (task[task_id].use_cma) 
            {
                if (alloc_cma_channel >= NB_CMA_CHANNELS) 
                {
                    _puts("\n[BOOT ERROR] CMA channel index too large for task ");
                    _puts(task[task_id].name);
                    _puts(" in vspace ");
                    _puts(vspace[vspace_id].name);
                    _puts("\n");
                    _exit();
                }
                ctx_cma = alloc_cma_channel;
                alloc_cma_channel++;
            }

            // ctx_hba : HBA channel global index provided by the global allocator
            //           Each channel is a private ressource: the number of
            //           requested channels cannot be larger than NB_NIC_CHANNELS.
            unsigned int ctx_hba = 0xFFFFFFFF;
            if (task[task_id].use_hba) 
            {
                if (alloc_hba_channel >= NB_IOC_CHANNELS) 
                {
                    _puts("\n[BOOT ERROR] IOC channel index too large for task ");
                    _puts(task[task_id].name);
                    _puts(" in vspace ");
                    _puts(vspace[vspace_id].name);
                    _puts("\n");
                    _exit();
                }
                ctx_hba = alloc_hba_channel;
                alloc_hba_channel++;
            }
            // ctx_tim : TIMER local channel index provided by the cluster allocator 
            //           Each timer is a private ressource
            unsigned int ctx_tim = 0xFFFFFFFF;
            if (task[task_id].use_tim) 
            {
                unsigned int cluster_id = task[task_id].clusterid;

                if ( alloc_tim_channel[cluster_id] >= NB_TIM_CHANNELS ) 
                {
                    _puts("\n[BOOT ERROR] local TIMER index too large for task ");
                    _puts(task[task_id].name);
                    _puts(" in vspace ");
                    _puts(vspace[vspace_id].name);
                    _puts("\n");
                    _exit();
                }
                ctx_tim =  alloc_tim_channel[cluster_id];
                alloc_tim_channel[cluster_id]++;
            }
            // ctx_epc : Get the virtual address of the memory location containing
            // the task entry point : the start_vector is stored by GCC in the seg_data 
            // segment and we must wait the .elf loading to get the entry point value...
            vobj_id = vspace[vspace_id].start_vobj_id;     
            unsigned int ctx_epc = vobj[vobj_id].vaddr + (task[task_id].startid)*4;

            // ctx_sp :  Get the vobj containing the stack 
            vobj_id = task[task_id].stack_vobj_id;
            unsigned int ctx_sp = vobj[vobj_id].vaddr + vobj[vobj_id].length;

            // get local task index in scheduler
            unsigned int ltid = psched->tasks;

            // get vspace thread index
            unsigned int thread_id = task[task_id].trdid;

            if (ltid >= IDLE_TASK_INDEX) 
            {
                _puts("\n[BOOT ERROR] in boot_schedulers_init() : ");
                _putd( ltid );
                _puts(" tasks allocated to processor ");
                _putd( gpid );
                _puts(" / max is ");
                _putd( IDLE_TASK_INDEX );
                _puts("\n");
                _exit();
            }

            // update the "tasks" and "current" fields in scheduler:
            // the first task to execute is task 0 as soon as there is at least
            // one task allocated to processor. 
            psched->tasks   = ltid + 1;
            psched->current = 0;

            // initializes the task context in scheduler
            psched->context[ltid][CTX_CR_ID]    = 0;
            psched->context[ltid][CTX_SR_ID]    = ctx_sr;
            psched->context[ltid][CTX_SP_ID]    = ctx_sp;
            psched->context[ltid][CTX_EPC_ID]   = ctx_epc;
            psched->context[ltid][CTX_PTPR_ID]  = ctx_ptpr;
            psched->context[ltid][CTX_TTY_ID]   = ctx_tty;
            psched->context[ltid][CTX_CMA_ID]   = ctx_cma;
            psched->context[ltid][CTX_HBA_ID]   = ctx_hba;
            psched->context[ltid][CTX_NIC_ID]   = ctx_nic;
            psched->context[ltid][CTX_TIM_ID]   = ctx_tim;
            psched->context[ltid][CTX_PTAB_ID]  = ctx_ptab;
            psched->context[ltid][CTX_LTID_ID]  = ltid;
            psched->context[ltid][CTX_GTID_ID]  = task_id;
            psched->context[ltid][CTX_TRDID_ID] = thread_id;
            psched->context[ltid][CTX_VSID_ID]  = vspace_id;
            psched->context[ltid][CTX_RUN_ID]   = 1;

#if BOOT_DEBUG_SCHED
_puts("\nTask ");
_putd( task_id );
_puts(" allocated to processor[");
_putd( x );
_puts(",");
_putd( y );
_puts(",");
_putd( lpid );
_puts("]\n  - ctx[LTID]   = ");
_putd( psched->context[ltid][CTX_LTID_ID] );
_puts("\n  - ctx[SR]     = ");
_putx( psched->context[ltid][CTX_SR_ID] );
_puts("\n  - ctx[SP]     = ");
_putx( psched->context[ltid][CTX_SP_ID] );
_puts("\n  - ctx[EPC]    = ");
_putx( psched->context[ltid][CTX_EPC_ID] );
_puts("\n  - ctx[PTPR]   = ");
_putx( psched->context[ltid][CTX_PTPR_ID] );
_puts("\n  - ctx[TTY]    = ");
_putx( psched->context[ltid][CTX_TTY_ID] );
_puts("\n  - ctx[NIC]    = ");
_putx( psched->context[ltid][CTX_NIC_ID] );
_puts("\n  - ctx[CMA]    = ");
_putx( psched->context[ltid][CTX_CMA_ID] );
_puts("\n  - ctx[IOC]    = ");
_putx( psched->context[ltid][CTX_HBA_ID] );
_puts("\n  - ctx[TIM]    = ");
_putx( psched->context[ltid][CTX_TIM_ID] );
_puts("\n  - ctx[PTAB]   = ");
_putx( psched->context[ltid][CTX_PTAB_ID] );
_puts("\n  - ctx[GTID]   = ");
_putx( psched->context[ltid][CTX_GTID_ID] );
_puts("\n  - ctx[VSID]   = ");
_putx( psched->context[ltid][CTX_VSID_ID] );
_puts("\n  - ctx[TRDID]  = ");
_putx( psched->context[ltid][CTX_TRDID_ID] );
_puts("\n");
#endif

        } // end loop on tasks
    } // end loop on vspaces
} // end _schedulers_init()

//////////////////////////////////////////////////////////////////////////////////
// This function loads the map.bin file from block device.
// The fat global varible is defined in fat32.c file.
//////////////////////////////////////////////////////////////////////////////////
void boot_mapping_init()
{
    _ioc_init( 0 );

    int fd_id = _fat_open( IOC_BOOT_MODE,
                           "map.bin",
                           0 );         // no creation

    if ( fd_id == -1 )
    {
        _puts("\n[BOOT ERROR] : map.bin file not found \n");
        _exit();
    }

#if BOOT_DEBUG_MAPPING
_puts("\n[BOOT] map.bin file successfully open at cycle ");
_putd(_get_proctime());
_puts("\n");
#endif

    unsigned int size    = fat.fd[fd_id].file_size;
    unsigned int nblocks = size >> 9;
    unsigned int offset  = size & 0x1FF;
    if ( offset ) nblocks++;

    unsigned int ok = _fat_read( IOC_BOOT_MODE,
                                 fd_id, 
                                 (unsigned int*)SEG_BOOT_MAPPING_BASE, 
                                 nblocks,       
                                 0 );      // offset
    if ( ok == -1 )
    {
        _puts("\n[BOOT ERROR] : unable to load map.bin file \n");
        _exit();
    }
    _fat_close( fd_id );
    
    boot_mapping_check();
} // end boot_mapping_init()


//////////////////////////////////////////////////////////////////////////////////
// This function open the .elf file identified by the "pathname" argument.
// It loads the complete file in a dedicated buffer, it copies all loadable 
// segments  at the memory virtual address defined in the .elf file, 
// and close the file.
// Notes: 
// - The processor PTPR should contain the value corresponding to the
//   vspace containing the .elf file.
// - As this function requires a temporary memory buffer
//   to load the complete .elf file before to copy the various segments
//   to te proper location, it uses the seg_boot_buffer defined in map.xml.
//////////////////////////////////////////////////////////////////////////////////
void load_one_elf_file( unsigned int mode,
                        char*        pathname,
                        unsigned int vspace_id )    // to use the proper page_table
{
    unsigned int seg_id;

    // get boot buffer address and size
    char*             boot_buffer      = (char*)SEG_BOOT_BUFFER_BASE;
    unsigned int      boot_buffer_size = SEG_BOOT_BUFFER_SIZE;

#if BOOT_DEBUG_ELF
_puts("\n[BOOT DEBUG] Start searching file ");
_puts( pathname );
_puts(" at cycle ");
_putd( _get_proctime() );
_puts("\n");
#endif

    // open .elf file
    int fd_id = _fat_open( mode,
                           pathname,
                           0 );      // no creation
    if ( fd_id < 0 )
    {
        _puts("\n[BOOT ERROR] load_one_elf_file() : ");
        _puts( pathname );
        _puts(" not found\n");
        _exit();
    }

    // check boot_buffer size versus file size
    if ( fat.fd[fd_id].file_size > boot_buffer_size )
    {
        _puts("\n[BOOT ERROR] load_one_elf_file() : ");
        _puts( pathname );
        _puts(" exceeds the seg_boot_buffer size\n");
        _exit();
    }

    // compute number of sectors
    unsigned int nbytes   = fat.fd[fd_id].file_size;
    unsigned int nsectors = nbytes>>9;
    if( nbytes & 0x1FF) nsectors++;

    // load file in boot_buffer
    if( _fat_read( mode, 
                   fd_id, 
                   boot_buffer,
                   nsectors,
                   0 ) != nsectors )
    {
        _puts("\n[BOOT ERROR] load_one_elf_file() : unexpected EOF for file ");
        _puts( pathname );
        _puts("\n");   
        _exit();
    }

    // Check ELF Magic Number in ELF header
    Elf32_Ehdr* elf_header_ptr = (Elf32_Ehdr*)boot_buffer;

    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) )
    {
        _puts("\n[BOOT ERROR] load_elf() : file ");
        _puts( pathname );
        _puts(" does not use ELF format\n");   
        _exit();
    }

    // get program header table pointer
    unsigned int pht_index = elf_header_ptr->e_phoff;
    if( pht_index == 0 )
    {
        _puts("\n[BOOT ERROR] load_one_elf_file() : file ");
        _puts( pathname );
        _puts(" does not contain loadable segment\n");   
        _exit();
    }
    Elf32_Phdr* elf_pht_ptr = (Elf32_Phdr*)(boot_buffer + pht_index);

    // get number of segments
    unsigned int nsegments   = elf_header_ptr->e_phnum;

#if BOOT_DEBUG_ELF
_puts("\n[BOOT DEBUG] File ");
_puts( pathname );
_puts(" loaded at cycle ");
_putd( _get_proctime() );
_puts(" / bytes = ");
_putd( nbytes );
_puts(" / sectors = ");
_putd( nsectors );
_puts("\n");
#endif

    // Loop on loadable segments in the ELF file
    for (seg_id = 0 ; seg_id < nsegments ; seg_id++)
    {
        if(elf_pht_ptr[seg_id].p_type == PT_LOAD)
        {
            // Get segment attributes
            unsigned int seg_vaddr  = elf_pht_ptr[seg_id].p_vaddr;
            unsigned int seg_offset = elf_pht_ptr[seg_id].p_offset;
            unsigned int seg_filesz = elf_pht_ptr[seg_id].p_filesz;
            unsigned int seg_memsz  = elf_pht_ptr[seg_id].p_memsz;

            if( seg_memsz < seg_filesz )
            {
                _puts("\n[BOOT ERROR] load_one_elf_file() : segment at vaddr = ");
                _putx( seg_vaddr );
                _puts(" in file ");
                _puts( pathname );
                _puts(" has a wrong size \n");   
                _exit();
            }

            // fill empty space with 0 as required
            if( seg_memsz > seg_filesz )
            {
                unsigned int i; 
                for( i = seg_filesz ; i < seg_memsz ; i++ ) boot_buffer[i+seg_offset] = 0;
            } 

            unsigned int src_vaddr = (unsigned int)boot_buffer + seg_offset;

#if BOOT_DEBUG_ELF
_puts(" - segment ");
_putd( seg_id );
_puts(" / dst_vaddr = ");
_putx( seg_vaddr );
_puts(" / src_vaddr = ");
_putx( src_vaddr );
_puts(" / size = ");
_putx( seg_filesz );
_puts("\n");
#endif

            // copy the segment from boot buffer to destination buffer
            if( NB_DMA_CHANNELS > 0 )
            {
                _dma_copy( 0,              // DMA cluster index
                           0,              // DMA channel index 
                           vspace_id,      
                           seg_vaddr,
                           src_vaddr,
                           seg_filesz );
            }
            else
            {
                _memcpy( (char*)seg_vaddr,
                         (char*)src_vaddr,
                         seg_filesz );
            }
        }
    } // end for segments

    // close .elf file
    _fat_close( fd_id );

} // end load_one_elf_file()


//////////////////////////////////////////////////////////////////////////////////
// This function uses the map.bin data structure to load the "kernel.elf" file
// as well as the various "application.elf" files.
// The "preloader.elf" file is not loaded, because it has been burned in the ROM.
// The "boot.elf" file is not loaded, because it has been loaded by the preloader.
// This function scans all vobjs defined in the map.bin data structure to collect
// all .elf files pathnames, and calls the load_one_elf_file() function to
// load all loadable segments at the virtual address found in the .elf file.
//////////////////////////////////////////////////////////////////////////////////
void boot_elf_load()
{
    mapping_header_t* header = (mapping_header_t *)SEG_BOOT_MAPPING_BASE;
    mapping_vspace_t* vspace = _get_vspace_base( header );
    mapping_vobj_t*   vobj   = _get_vobj_base( header );
    unsigned int      vspace_id;
    unsigned int      vobj_id;
    unsigned int      found;

    // Scan all vobjs corresponding to global vsegs,
    // to find the pathname to the kernel.elf file
    found = 0;
    for( vobj_id = 0 ; vobj_id < header->globals ; vobj_id++ )
    {
        if(vobj[vobj_id].type == VOBJ_TYPE_ELF) 
        {   
            found = 1;
            break;
        }
    }

    // We need one kernel.elf file
    if (found == 0)
    {
        _puts("[BOOT ERROR] boot_elf_load() : kernel.elf file not found\n");
        _exit();
    }

    load_one_elf_file( IOC_BOOT_MODE,
                       vobj[vobj_id].binpath,
                       0 );                         // vspace 0

    _puts("\n[BOOT] File \"");
    _puts( vobj[vobj_id].binpath );
    _puts("\" loaded at cycle ");
    _putd( _get_proctime() );
    _puts("\n");

    // loop on the vspaces, scanning all vobjs in a vspace,
    // to find the pathname of the .elf file associated to the vspace.
    for( vspace_id = 0 ; vspace_id < header->vspaces ; vspace_id++ )
    {
        // Set PTPR depending on the vspace, as seg_data is defined in virtual space.
        _set_mmu_ptpr( (unsigned int)(_ptabs_paddr[vspace_id] >> 13) );

        // loop on the vobjs in vspace (vobj_id is the global index)
        unsigned int found = 0;
        for (vobj_id = vspace[vspace_id].vobj_offset;
             vobj_id < (vspace[vspace_id].vobj_offset + vspace[vspace_id].vobjs);
             vobj_id++) 
        {
            if(vobj[vobj_id].type == VOBJ_TYPE_ELF) 
            {   
                found = 1;
                break;
            }
        }

        // We want one .elf file per vspace
        if (found == 0)
        {
            _puts("[BOOT ERROR] boot_elf_load() : .elf file not found for vspace ");
            _puts( vspace[vspace_id].name );
            _puts("\n");
            _exit();
        }

        load_one_elf_file( IOC_BOOT_MODE,
                           vobj[vobj_id].binpath,
                           vspace_id );

        _puts("\n[BOOT] File \"");
        _puts( vobj[vobj_id].binpath );
        _puts("\" loaded at cycle ");
        _putd( _get_proctime() );
        _puts("\n");

    }  // end for vspaces

    // restaure vspace 0 PTPR
    _set_mmu_ptpr( (unsigned int)(_ptabs_paddr[0] >> 13) );

} // end boot_elf_load()

////////////////////////////////////////////////////////////////////////////////
// This function intializes the periherals and coprocessors, as specified
// in the mapping_info file.
////////////////////////////////////////////////////////////////////////////////
void boot_peripherals_init() 
{
    mapping_header_t * header   = (mapping_header_t *)SEG_BOOT_MAPPING_BASE;
    mapping_cluster_t * cluster = _get_cluster_base(header);
    mapping_periph_t * periph   = _get_periph_base(header);
    mapping_vobj_t * vobj       = _get_vobj_base(header);
    mapping_coproc_t * coproc   = _get_coproc_base(header);
    mapping_cp_port_t * cp_port = _get_cp_port_base(header);
    mapping_irq_t * irq         = _get_irq_base(header);

    unsigned int cluster_id;
    unsigned int periph_id;
    unsigned int coproc_id;
    unsigned int cp_port_id;
    unsigned int channel_id;

    // loop on all physical clusters
    for (cluster_id = 0; cluster_id < X_SIZE*Y_SIZE; cluster_id++) 
    {
        // computes cluster coordinates
        unsigned int x          = cluster[cluster_id].x;
        unsigned int y          = cluster[cluster_id].y;
        unsigned int cluster_xy = (x<<Y_WIDTH) + y;

#if BOOT_DEBUG_PERI
_puts("\n[BOOT DEBUG] Peripherals initialisation in cluster[");
_putd( x );
_puts(",");
_putd( y );
_puts("]\n");
#endif

        // loop on peripherals
        for (periph_id = cluster[cluster_id].periph_offset;
             periph_id < cluster[cluster_id].periph_offset +
             cluster[cluster_id].periphs; periph_id++) 
        {
            unsigned int type       = periph[periph_id].type;
            unsigned int channels   = periph[periph_id].channels;

            switch (type) 
            {
                case PERIPH_TYPE_IOC:    // vci_block_device component
                {
                    // initialize all channels except channel 0 because it has been
                    // initialized by the preloader.
                    for (channel_id = 1; channel_id < channels; channel_id++) 
                    {
                        _ioc_init( channel_id );
                    }
#if BOOT_DEBUG_PERI
_puts("- IOC : channels = ");
_putd(channels);
_puts("\n");
#endif
                    break;
                }
                case PERIPH_TYPE_CMA:    // vci_chbuf_dma component
                {
                    for (channel_id = 0; channel_id < channels; channel_id++) 
                    {
                        // TODO
                    }
#if BOOT_DEBUG_PERI
_puts("- CMA : channels = ");
_putd(channels);
_puts("\n");
#endif
                    break;
                }
                case PERIPH_TYPE_NIC:    // vci_multi_nic component
                {
                    for (channel_id = 0; channel_id < channels; channel_id++) 
                    {
                        // TODO
                    }
#if BOOT_DEBUG_PERI
_puts("- NIC : channels = ");
_putd(channels);
_puts("\n");
#endif
                    break;
                }
                case PERIPH_TYPE_TTY:    // vci_multi_tty component
                {
                    // nothing to do
#if BOOT_DEBUG_PERI
_puts("- TTY : channels = ");
_putd(channels);
_puts("\n");
#endif
                    break;
                }
                case PERIPH_TYPE_IOB:    // vci_io_bridge component
                {
#if BOOT_DEBUG_PERI
_puts("- IOB : channels = ");
_putd(channels);
_puts("\n");
#endif
                    if (GIET_USE_IOMMU) 
                    {
                        // TODO
                        // get the iommu page table physical address
                        // set IOMMU page table address
                        // pseg_base[IOB_IOMMU_PTPR] = ptab_pbase;    
                        // activate IOMMU
                        // pseg_base[IOB_IOMMU_ACTIVE] = 1;        
                    }
                    break;
                }
                case PERIPH_TYPE_PIC:    // vci_iopic component
                {
                          
#if BOOT_DEBUG_PERI
_puts("- PIC : channels = ");
_putd(channels);
_puts("\n");
#endif
                    // scan all IRQs defined in mapping for PIC component,
                    // and initialises addresses for WTI IRQs
                    for ( channel_id = periph[periph_id].irq_offset ;
                          channel_id < periph[periph_id].irq_offset + periph[periph_id].irqs ;
                          channel_id++ )
                    {
                        unsigned int hwi_id     = irq[channel_id].srcid;   // HWI index in PIC
                        unsigned int wti_id     = irq[channel_id].dest_id; // WTI index in XCU
                        unsigned int cluster_xy = irq[channel_id].dest_xy; // XCU coordinates
                        unsigned int vaddr;

                        _xcu_get_wti_address( wti_id, &vaddr );

                        _pic_init( hwi_id, vaddr, cluster_xy ); 
#if BOOT_DEBUG_PERI
_puts("    hwi_index = ");
_putd( hwi_id );
_puts(" / wti_index = ");
_putd( wti_id );
_puts(" / vaddr = ");
_putx( vaddr );
_puts(" in cluster[");
_putd( cluster_xy >> Y_WIDTH );
_puts(",");
_putd( cluster_xy & ((1<<Y_WIDTH)-1) );
_puts("]\n");
#endif
                    }
                    break;
                }
            }  // end switch periph type
        }  // end for periphs

#if BOOT_DEBUG_PERI
_puts("\n[BOOT DEBUG] Coprocessors initialisation in cluster[");
_putd( x );
_puts(",");
_putd( y );
_puts("]\n");
#endif

        // loop on coprocessors
        for ( coproc_id = cluster[cluster_id].coproc_offset;
              coproc_id < cluster[cluster_id].coproc_offset +
              cluster[cluster_id].coprocs; coproc_id++ ) 
        {

#if BOOT_DEBUG_PERI
_puts("- coprocessor name : ");
_puts(coproc[coproc_id].name);
_puts(" / nb ports = ");
_putd((unsigned int) coproc[coproc_id].ports);
_puts("\n");
#endif
            // loop on the coprocessor ports
            for ( cp_port_id = coproc[coproc_id].port_offset;
                  cp_port_id < coproc[coproc_id].port_offset + coproc[coproc_id].ports;
                  cp_port_id++ ) 
            {
                // Get global index of associted vobj
                unsigned int vobj_id   = cp_port[cp_port_id].mwmr_vobj_id; 

                // Get MWMR channel base address 
                paddr_t mwmr_channel_pbase = vobj[vobj_id].paddr;

                _mwr_hw_init( cluster_xy,
                              cp_port_id, 
                              cp_port[cp_port_id].direction, 
                              mwmr_channel_pbase );
#if BOOT_DEBUG_PERI
_puts("     port direction: ");
_putd( (unsigned int)cp_port[cp_port_id].direction );
_puts(" / mwmr_channel_pbase = ");
_putl( mwmr_channel_pbase );
_puts(" / name = ");
_puts(vobj[vobj_id].name);
_puts("\n"); 
#endif
            } // end for cp_ports
        } // end for coprocs
    } // end for clusters
} // end boot_peripherals_init()

/////////////////////////////////////////////////////////////////////////
// This function is the entry point of the boot code for all processors.
// Most of this code is executed by Processor 0 only.
/////////////////////////////////////////////////////////////////////////
void boot_init()
{
    mapping_header_t*  header     = (mapping_header_t *)SEG_BOOT_MAPPING_BASE;
    mapping_cluster_t* cluster    = _get_cluster_base(header);
    unsigned int       gpid       = _get_procid();
 
    if ( gpid == 0 )    // only Processor 0 does it
    {
        _puts("\n[BOOT] boot_init start at cycle ");
        _putd(_get_proctime());
        _puts("\n");

        // Loading the map.bin file into memory and checking it
        boot_mapping_init();

        _puts("\n[BOOT] Mapping \"");
        _puts( header->name );
        _puts("\" loaded at cycle ");
        _putd(_get_proctime());
        _puts("\n");

        // Building all page tables
        boot_pt_init();

        // Activating proc 0 MMU 
        _set_mmu_ptpr( (unsigned int)(_ptabs_paddr[0]>>13) );
        _set_mmu_mode( 0xF );

        _puts("\n[BOOT] Processor[0,0,0] : MMU activation at cycle ");
        _putd(_get_proctime());
        _puts("\n");

        // Initialising private vobjs in vspaces
        boot_vobjs_init();

        _puts("\n[BOOT] Private vobjs initialised at cycle ");
        _putd(_get_proctime());
        _puts("\n");

        // Initializing schedulers
        boot_schedulers_init();

        _puts("\n[BOOT] Schedulers initialised at cycle ");
        _putd(_get_proctime());
        _puts("\n");
        
        // Setting CP0_SCHED register for proc 0
        _set_sched( (unsigned int)_schedulers[0] );

        // Initializing non replicated peripherals
        boot_peripherals_init();

        _puts("\n[BOOT] Non replicated peripherals initialised at cycle ");
        _putd(_get_proctime());
        _puts("\n");

        // Loading all .elf files
        boot_elf_load();

        // P0 starts all other processors
        unsigned int clusterid, p;

        for ( clusterid = 0 ; clusterid < X_SIZE*Y_SIZE ; clusterid++ ) 
        {
            unsigned int nprocs     = cluster[clusterid].procs;
            unsigned int xdest      = cluster[clusterid].x;
            unsigned int ydest      = cluster[clusterid].y;
            unsigned int cluster_xy = (xdest<<Y_WIDTH) + ydest;

            for ( p = 0 ; p < nprocs; p++ ) 
            {
                if ( (nprocs > 0) && ((clusterid != 0) || (p != 0)) )
                {
                    _xcu_send_wti( cluster_xy, p, (unsigned int)boot_init );
                }
            }
        }
  
    }  // end monoprocessor boot

    // reset BEV bit in the status register to use GIET exception
    // handler instead of the PRELOADER exception handler
    _set_sr( 0 );

    // all processor initialise SCHED register
    _set_sched( (unsigned int)_schedulers[gpid] );

    // all processors (but Proc 0) activate MMU
    if ( gpid != 0 )
    {
        _set_mmu_ptpr( (unsigned int)(_ptabs_paddr[0]>>13) );
        _set_mmu_mode( 0xF );
    }

    // all processors jump to kernel_init (address defined in giet_vsegs.ld)
    unsigned int kernel_entry = (unsigned int)&kernel_init_vbase;
    asm volatile( "jr   %0" ::"r"(kernel_entry) );

} // end boot_init()


// Local Variables:
// tab-width: 4
// c-basic-offset: 4
// c-file-offsets:((innamespace . 0)(inline-open . 0))
// indent-tabs-mode: nil
// End:
// vim: filetype=c:expandtab:shiftwidth=4:tabstop=4:softtabstop=4