Index: trunk/tools/bootloader_tsar/boot.c
===================================================================
--- trunk/tools/bootloader_tsar/boot.c	(revision 1)
+++ trunk/tools/bootloader_tsar/boot.c	(revision 6)
@@ -1,4 +1,27 @@
+/*
+ * boot.c - TSAR bootloader implementation.
+ * 
+ * Authors :   Alain Greiner / Vu Son  (2016)
+ *
+ * Copyright (c) UPMC Sorbonne Universites
+ *
+ * This file is part of ALMOS-MKH.
+ *
+ * ALMOS-MKH is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2.0 of the License.
+ *
+ * ALMOS-MKH is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with ALMOS-MKH; if not, write to the Free Software Foundation,
+ * Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+
 /****************************************************************************
- * This file contains the ALMOS-MKH. boot-loader for the TSAR architecture.  *
+ * This file contains the ALMOS-MKH. boot-loader for the TSAR architecture. *
  *                                                                          *
  * It supports clusterised shared memory multi-processor architectures,     *
@@ -47,4 +70,5 @@
 
 #include <elf-types.h>
+#include <hal_types.h>
 
 #include <almos_config.h>
@@ -53,6 +77,4 @@
 #include <arch_info.h>
 #include <boot_info.h>
-
-#include <hal_types.h>
 
 #include <boot_utils.h>
@@ -62,6 +84,6 @@
 #include <boot_tty_driver.h>
 
-/****************************************************************************
- *                                 Macros.                                  *
+/*****************************************************************************
+ *                                 Macros.                              
  ****************************************************************************/
 
@@ -70,43 +92,36 @@
                             (~(PPM_PAGE_SIZE-1)))
 
-/****************************************************************************
- *                             Global variables.                            *
+/*****************************************************************************
+ *                             Global variables.                            
  ****************************************************************************/
 
 // synchronization variables. 
-volatile boot_barrier_t global_barrier;     /* Used by bscpu to synchronize 
-                                               with other CP0s cores.       */
-
-volatile uint32_t   global_count;           /* Number of cores expected in 
-                                               global barrier.              */
-
-volatile uint32_t   local_barrier;          /* Used by CP0 to synchronize 
-                                               with local CPi.              */
-
-volatile uint32_t   boot_cluster_ready;     /* Modified by bscpu to report 
-                                               that the boot cluster is 
-                                               ready.                       */
-
-// kernel image memory layout. 
-uint32_t ktext_base;                        /* ktext segment base address.  */
-uint32_t ktext_end;                         /* ktext segment end address.   */
-uint32_t kdata_base;                        /* kdata segment base address.  */
-uint32_t kdata_end;                         /* kdata segment end address.   */
-
-uint32_t kernel_entry;                      /* Kernel entry point.          */
-
-// Extern variables. 
-extern void boot_entry();                   /* boot_loader() function       */
-
-/****************************************************************************
- *                           Internal functions.                            *
- ****************************************************************************/
-
+
+volatile boot_remote_spinlock_t tty0_lock;       // protect TTY0 access
+volatile boot_remote_barrier_t  global_barrier;  // synchronize CP0 cores
+volatile boot_remote_barrier_t  local_barrier;   // synchronize cores in one cluster
+uint32_t                        active_cp0s_nr;  // number of expected CP0s
+ 
+// kernel segments layout variables
+
+uint32_t                        seg_kcode_base;  // kcode segment base address
+uint32_t                        seg_kcode_size;  // kcode segment size (bytes) 
+uint32_t                        seg_kdata_base;  // kdata segment base address
+uint32_t                        seg_kdata_size;  // kdata segment size (bytes) 
+uint32_t                        kernel_entry;    // kernel entry point
+
+// address used by the WTI to activate remote CP0s
+
+extern void                     boot_entry();    // boot_loader entry point
+
+/*********************************************************************************
+ * This function returns the printable string for each device type
+ ********************************************************************************/
 char * device_type_str( uint32_t dev_type )
 {
-    if     ( dev_type == DEV_TYPE_RAM     ) return "RAM";
-    else if( dev_type == DEV_TYPE_DMA     ) return "DMA";
-    else if( dev_type == DEV_TYPE_FBF     ) return "FBF";
-    else if( dev_type == DEV_TYPE_IOB     ) return "IOB";
+    if     ( dev_type == DEV_TYPE_RAM_SCL ) return "RAM_SCL";
+    else if( dev_type == DEV_TYPE_ROM_SCL ) return "ROM_SCL";
+    else if( dev_type == DEV_TYPE_FBF_SCL ) return "FBF_SCL";
+    else if( dev_type == DEV_TYPE_IOB_TSR ) return "IOB_TSR";
     else if( dev_type == DEV_TYPE_IOC_BDV ) return "IOC_BDV";
     else if( dev_type == DEV_TYPE_IOC_HBA ) return "IOC_HBA";
@@ -114,22 +129,17 @@
     else if( dev_type == DEV_TYPE_IOC_SPI ) return "IOC_SPI";
     else if( dev_type == DEV_TYPE_IOC_RDK ) return "IOC_RDK";
-    else if( dev_type == DEV_TYPE_MMC     ) return "MMC";
-    else if( dev_type == DEV_TYPE_MWR_CPY ) return "MWR_CPY";
-    else if( dev_type == DEV_TYPE_MWR_GCD ) return "MWR_GCD";
-    else if( dev_type == DEV_TYPE_MWR_DCT ) return "MWR_DCT";
-    else if( dev_type == DEV_TYPE_NIC     ) return "NIC";
-    else if( dev_type == DEV_TYPE_ROM     ) return "ROM";
-    else if( dev_type == DEV_TYPE_SIM     ) return "SIM";
-    else if( dev_type == DEV_TYPE_TIM     ) return "TIM";
-    else if( dev_type == DEV_TYPE_TTY     ) return "TTY";
-    else if( dev_type == DEV_TYPE_XCU     ) return "XCU";
-    else if( dev_type == DEV_TYPE_PIC     ) return "PIC";
-    else if( dev_type == DEV_TYPE_CMA     ) return "CMA";
-    else                                    return "UNDEFINED";
+    else if( dev_type == DEV_TYPE_MMC_TSR ) return "MMC_TSR";
+    else if( dev_type == DEV_TYPE_DMA_SCL ) return "DMA_SCL";
+    else if( dev_type == DEV_TYPE_NIC_CBF ) return "NIC_CBF";
+    else if( dev_type == DEV_TYPE_TIM_SCL ) return "TIM_SCL";
+    else if( dev_type == DEV_TYPE_TXT_TTY ) return "TXT_TTY";
+    else if( dev_type == DEV_TYPE_ICU_XCU ) return "ICU_XCU";
+    else if( dev_type == DEV_TYPE_PIC_TSR ) return "PIC_TSR";
+    else                                    return "undefined";
 }
 
-/****************************************************************************
+/************************************************************************************
  * This function loads the arch_info.bin file into the boot cluster memory.
- ****************************************************************************/
+ ***********************************************************************************/
 static void boot_archinfo_load()
 {
@@ -154,59 +164,48 @@
 
 #if DEBUG_BOOT_INFO
-boot_printf("\n[BOOT] file %s loaded at %l\n",
-            ARCHINFO_PATHNAME , ARCHINFO_BASE );
+boot_printf("\n[BOOT INFO] in %s : file %s loaded at address = %x\n",
+            __FUNCTION__ , ARCHINFO_PATHNAME , ARCHINFO_BASE );
 #endif
 
 } // boot_archinfo_load()
 
-/****************************************************************************
- * This function loads the 'kernel.elf' file into the boot cluster memory   *
- * bank, analyzes it then places the kernel image at the temporary physical *
- * memory address KERN_IMG_TMP_BASE since other processors are still        *
- * executing the preloader code (which means that the kernel image cannot   *
- * be placed now at its final memory location starting at address 0x0.      *
- ****************************************************************************/
+/**************************************************************************************
+ * This function loads the 'kernel.elf' file into the boot cluster memory buffer,
+ * analyzes it, and places the the two seg_kcode & seg_kdata segments at their final
+ * physical adresses (just after the preloader zone).        
+ * It set the global variables defining the kernel layout.
+ *************************************************************************************/
 static void boot_kernel_load()
 {
-    Elf32_Ehdr* elf_header;         /* Pointer on 'kernel.elf' header.      */
-    Elf32_Phdr* program_header;     /* Pointer on 'kernel.elf' program 
-                                       header.                              */
-    uint32_t    phdr_offset;        /* Program header offset in 
-                                       'kernel.elf' file.                   */
-    uint32_t    segments_nb;        /* Total number of segments in 
-                                       'kernel.elf' file.                   */
-
-    uint32_t    seg_src_addr;       /* Segment address in 'kernel.elf' 
-                                       file (source).                       */
-    uint32_t    seg_paddr;          /* Physical address at which the 
-                                       first byte of the segment resides 
-                                       in memory.                           */
-    uint32_t    seg_offset;         /* Offset from the beginning of 
-                                       'kernel.elf' file to the segment's
-                                       first byte.                          */ 
-    uint32_t    seg_filesz;         /* Segment's number of bytes in 
-                                       'kernel.elf' file.                   */ 
-    uint32_t    seg_memsz;          /* Segment's number of bytes in the 
-                                       memory image.                        */
-
-    uint32_t    seg_id;             /* Iterator for program header scanning
-                                       loop.                                */
-
-    /* Loading file into memory. */
-    if (boot_fat32_load(KERNEL_PATHNAME, KERN_BASE, KERN_MAX_SIZE))
-    {
-        boot_printf("\n[BOOT ERROR]: boot_kernel_load(): "
-                    "<%s> file not found\n",
+    Elf32_Ehdr * elf_header;      // pointer on kernel.elf header.  
+    Elf32_Phdr * program_header;  // pointer on kernel.elf program header. 
+    uint32_t     phdr_offset;     // program header offset in kernel.elf file. 
+    uint32_t     segments_nb;     // number of segments in kernel.elf file. 
+    uint32_t     seg_src_addr;    // segment address in kernel.elf file (source).
+    uint32_t     seg_paddr;       // segment local physical address of segment 
+    uint32_t     seg_offset;      // segment offset in kernel.elf file
+    uint32_t     seg_filesz;      // segment size (bytes) in kernel.elf file 
+    uint32_t     seg_memsz;       // segment size (bytes) in memory image.
+    bool_t       kcode_found;     // kcode segment found.
+    bool_t       kdata_found;     // kdata segment found.
+    uint32_t     seg_id;          // iterator for segments loop.
+
+#if DEBUG_BOOT_ELF 
+boot_printf("\n[BOOT INFO] %s enters for file %s at cycle %d\n",
+            __FUNCTION__ , KERNEL_PATHNAME , boot_get_proctime() );
+#endif
+
+    // Load kernel.elf file into memory buffer
+    if ( boot_fat32_load(KERNEL_PATHNAME, KERN_BASE, KERN_MAX_SIZE) )
+    {
+        boot_printf("\n[BOOT ERROR] in %s : <%s> file not found\n",
                     KERNEL_PATHNAME);
         boot_exit();
     }
 
-    /* 
-     * Initializing pointer to header which is the first element of the 
-     * .elf file. 
-     */
+    // get pointer to kernel.elf header  
     elf_header = (Elf32_Ehdr*)KERN_BASE;
 
-    /* Signature problem, abort program !!! */
+    // check signature 
     if ((elf_header->e_ident[EI_MAG0] != ELFMAG0)   ||
         (elf_header->e_ident[EI_MAG1] != ELFMAG1)   ||
@@ -220,18 +219,19 @@
     }
 
-    /* Getting the program header table offset and the number of segments. */
+    // Get program header table offset and number of segments
     phdr_offset     = elf_header->e_phoff;
     segments_nb     = elf_header->e_phnum;
 
-    /* Getting the program header table pointer. */
+    // Get program header table pointer
     program_header  = (Elf32_Phdr*)(KERN_BASE + phdr_offset);
 
-    /* Looking for loadable segments. */
+    // loop on segments
+    kcode_found = false;
+    kdata_found = false;
     for (seg_id = 0; seg_id < segments_nb; seg_id++) 
     {
-        // Found one:
-        if (program_header[seg_id].p_type == PT_LOAD)
+        if (program_header[seg_id].p_type == PT_LOAD)   // Found one loadable segment
         {
-            // Getting its attributes.
+            // Get segment attributes.
             seg_paddr    = program_header[seg_id].p_paddr;   
             seg_offset   = program_header[seg_id].p_offset;
@@ -239,53 +239,96 @@
             seg_memsz    = program_header[seg_id].p_memsz;
 
-            // Load it to its appropriate physical memory address.
+            // get segment base address in buffer
             seg_src_addr = (uint32_t)KERN_BASE + seg_offset;
-            boot_memcpy((void*)(KERN_IMG_TMP_BASE + seg_paddr), 
-                        (void*)seg_src_addr, 
-                               seg_filesz);
+
+            // Load segment to its final physical memory address
+            boot_memcpy( (void*)seg_paddr, 
+                         (void*)seg_src_addr, 
+                         seg_filesz );
+
+#if DEBUG_BOOT_ELF 
+boot_printf("\n[BOOT INFO] in %s for file %s : found loadable segment\n"
+            "   base = %x / size = %x\n",
+            __FUNCTION__ , KERNEL_PATHNAME , seg_paddr , seg_memsz );
+#endif
 
             // Fill remaining memory with zero if (filesz < memsz).
-            boot_memset((void*)(KERN_IMG_TMP_BASE + seg_paddr + seg_filesz),
-                                0,
-                                seg_memsz - seg_filesz);
-            
-            /* 
-             * Note: we suppose that the 'kernel.elf' file contains only 2 
-             * loadable segments ktext + kdata and that the main 
-             * difference between these two is the WRITE permission: ktext 
-             * contains read-only instructions and read_only data,
-             * while kdata contains writable data.
-             */
-
-            // Get ktext segment base and end addresses.
-            if ((program_header[seg_id].p_flags & PF_W) == 0)
-            {
-                ktext_base = seg_paddr;
-                ktext_end  = seg_paddr + seg_memsz;
-            }
-
-            // Get kdata segment base and end addresses.
-            else
-            {
-                kdata_base = seg_paddr;
-                kdata_end  = seg_paddr + seg_memsz;
+            if( seg_memsz < seg_filesz )
+            {
+                boot_memset( (void*)(seg_paddr + seg_filesz), 0, seg_memsz - seg_filesz);
+            }
+
+            // Note: we suppose that the 'kernel.elf' file contains only 2 
+            // loadable segments ktext & kdata and that the main 
+            // difference between these two is the WRITE permission: ktext 
+            // contains read-only instructions and read_only data,
+            // while kdata contains writable data.
+
+            if ((program_header[seg_id].p_flags & PF_W) == 0)  // kcode segment
+            {
+                if( kcode_found )
+                {
+                    boot_printf("\n[BOOT_ERROR] in %s for file %s :\n"
+                                "   two loadable kcode segments found\n",
+                                __FUNCTION__ , KERNEL_PATHNAME );
+                    boot_exit();
+                } 
+
+                kcode_found     = true;
+                seg_kcode_base = seg_paddr;
+                seg_kcode_size = seg_memsz;
+            }
+            else                                               // kdata segment
+            {
+                if( kdata_found )
+                {
+                    boot_printf("\n[BOOT_ERROR] in %s for file %s :\n"
+                                "   two loadable kdata segments found\n",
+                                __FUNCTION__ , KERNEL_PATHNAME );
+                    boot_exit();
+                } 
+
+                kdata_found     = true;
+                seg_kdata_base = seg_paddr;
+                seg_kdata_size = seg_memsz;
             }
         }
     }
 
-    // Get the entry point for kernel code.
+    // check kcode & kdata segments found
+    if( kcode_found == false )
+    {
+        boot_printf("\n[BOOT_ERROR] in %s for file %s :\n"
+                    "   kcode segment not found\n",
+                    __FUNCTION__ , KERNEL_PATHNAME );
+        boot_exit();
+    }
+    if( kdata_found == false )
+    {
+        boot_printf("\n[BOOT_ERROR] in %s for file %s :\n"
+                    "   kdata segment not found\n",
+                    __FUNCTION__ , KERNEL_PATHNAME );
+        boot_exit();
+    }
+
+    // set entry point 
     kernel_entry = (uint32_t)elf_header->e_entry;
 
+#if DEBUG_BOOT_ELF 
+boot_printf("\n[BOOT INFO] %s successfully completed for file %s at cycle %d\n",
+            __FUNCTION__ , KERNEL_PATHNAME , boot_get_proctime() );
+#endif
+
 } // boot_kernel_load()
 
-/****************************************************************************
- * This function initializes the local 'boot_info_t' structure.             *
- * @ boot_info  : pointer to local boot_info_t structure                    *
- * @ cxy        : cluster identifier                                        *
- ****************************************************************************/
+/*************************************************************************************
+ * This function initializes the  boot_info_t structure for a given cluster. 
+ * @ boot_info  : pointer to local boot_info_t structure  
+ * @ cxy        : cluster identifier                    
+ ************************************************************************************/
 static void boot_info_init( boot_info_t * boot_info,
                             cxy_t         cxy )
 {
-    archinfo_header_t  * header;        
+    archinfo_header_t  * header;
     archinfo_core_t    * core_base;     
     archinfo_cluster_t * cluster_base; 
@@ -294,4 +337,7 @@
 
     archinfo_cluster_t * cluster; 
+    archinfo_cluster_t * my_cluster = NULL;   // target cluster
+    archinfo_cluster_t * io_cluster = NULL;   // cluster containing ext. peripherals
+
     archinfo_core_t    * core;
     uint32_t             core_id; 
@@ -303,6 +349,15 @@
     boot_device_t      * boot_dev; 
 
-    // get pointer on ARCHINFO header 
-    header = (archinfo_header_t*)ARCHINFO_BASE;
+#if DEBUG_BOOT_INFO
+boot_printf("\n[BOOT INFO] %s : enter for cluster %x at cycle %d\n",
+            __FUNCTION__ , cxy , boot_get_proctime() );
+#endif
+
+    // get pointer on ARCHINFO header  and on the four arch_info arrays
+    header       = (archinfo_header_t*)ARCHINFO_BASE;
+    core_base    = archinfo_get_core_base   (header);
+    cluster_base = archinfo_get_cluster_base(header);
+    device_base  = archinfo_get_device_base (header);
+    irq_base     = archinfo_get_irq_base    (header);
 
     // Initialize global platform parameters
@@ -315,105 +370,87 @@
 
     // Initialize kernel segments
-    boot_info->kernel_code_start = ktext_base;
-    boot_info->kernel_code_end   = ktext_end;
-    boot_info->kernel_data_start = kdata_base;
-    boot_info->kernel_data_end   = kdata_end;
-
-    // Initialize specific cluster parameter
-    core_base    = archinfo_get_core_base   (header);
-    cluster_base = archinfo_get_cluster_base(header);
-    device_base  = archinfo_get_device_base (header);
-    irq_base     = archinfo_get_irq_base    (header);
-
-    // lopp on the clusters to find local cluster descriptor
+    boot_info->kernel_code_start = seg_kcode_base;
+    boot_info->kernel_code_end   = seg_kcode_base + seg_kcode_size;
+    boot_info->kernel_data_start = seg_kdata_base;
+    boot_info->kernel_data_end   = seg_kdata_base + seg_kdata_size;
+
+    // loop on arch_info clusters to get relevant pointers
     for (cluster =  cluster_base;
          cluster < &cluster_base[header->x_size * header->y_size];
          cluster++)
     {
-        if (cluster->cxy != cxy) continue;
-
-        boot_info->cxy          = cluster->cxy;
-        boot_info->cores_nr     = cluster->cores;
-        boot_info->devices_nr   = cluster->devices;
-
-#if DEBUG_BOOT_INFO
-boot_printf("\n[BOOT] build boot_info for cluster %x : %d cores / %d devices\n",
-            cluster->cxy , cluster->cores , cluster->devices );
-#endif
-        // Initialize array of core descriptors
-        for (core = &core_base[cluster->core_offset], core_id = 0;
-             core < &core_base[cluster->core_offset + cluster->cores];
-             core++, core_id++)
+        if( cluster->cxy  == cxy )            my_cluster = cluster;
+        if( cluster->cxy  == header->io_cxy ) io_cluster = cluster;
+    }
+
+    if( my_cluster == NULL ) 
+    {
+        boot_printf("\n[ERROR] in %s : cannot found cluster %x in arch_info\n",
+                    __FUNCTION__ , cxy );
+        boot_exit();
+    }
+
+    if( io_cluster == NULL ) 
+    {
+        boot_printf("\n[ERROR] in %s : cannot found io_cluster %x in arch_info\n",
+                    __FUNCTION__ , header->io_cxy );
+        boot_exit();
+    }
+
+    // loop on all arch-info peripherals in IO_cluster, 
+    // to initialize the boot_info array of external peripherals
+
+#if DEBUG_BOOT_INFO
+boot_printf("\n[BOOT INFO] %s : External peripherals\n", __FUNCTION__ );
+#endif
+
+    device_id = 0;
+    for (device = &device_base[io_cluster->device_offset];
+         device < &device_base[io_cluster->device_offset + io_cluster->devices];
+         device++ )
+    {
+        // initialise one entry for each external peripheral
+        if( (device->type != DEV_TYPE_RAM_SCL) &&
+            (device->type != DEV_TYPE_ICU_XCU) &&
+            (device->type != DEV_TYPE_MMC_TSR) &&
+            (device->type != DEV_TYPE_DMA_SCL) ) 
         {
-            boot_info->core[core_id].gid = (gid_t)core->gid;
-            boot_info->core[core_id].lid = (lid_t)core->lid; 
-            boot_info->core[core_id].cxy = (cxy_t)core->cxy;
-
-#if DEBUG_BOOT_INFO
-boot_printf("  - core %x : cxy = %x / lid = %d\n", 
-            core->gid , core->cxy , core->lid );
-#endif
-
+            boot_dev = &boot_info->ext_dev[device_id];
+
+            boot_dev->type     = device->type;
+            boot_dev->base     = device->base;
+            boot_dev->size     = device->size;
+            boot_dev->channels = device->channels;
+            boot_dev->param0   = device->arg0;    
+            boot_dev->param1   = device->arg1;    
+            boot_dev->param2   = device->arg2;    
+            boot_dev->param3   = device->arg3;    
+            boot_dev->irqs     = device->irqs;    
+
+            device_id++;
         }
 
-        // Initialize array of device descriptors
-        for (device = &device_base[cluster->device_offset], device_id = 0;
-             device < &device_base[cluster->device_offset + cluster->devices];
-             device++, device_id++)
-        {
-            boot_dev = &boot_info->dev[device_id];
-
-            boot_dev->type       =         device->type;
-            boot_dev->base       = (xptr_t)device->base;
-            boot_dev->size       =         device->size;
-            boot_dev->channels   =         device->channels;
-            boot_dev->param0     =         device->arg0;    
-            boot_dev->param1     =         device->arg1;    
-            boot_dev->param2     =         device->arg2;    
-            boot_dev->param3     =         device->arg3;    
-            boot_dev->irqs       =         device->irqs;    
-
-#if DEBUG_BOOT_INFO
-boot_printf("  - device %s : base = %l / size = %d / channels = %d / irqs = %d\n",
+#if DEBUG_BOOT_INFO
+boot_printf("  - %s : base = %l / size = %l / channels = %d / irqs = %d\n",
             device_type_str( device->type ) , device->base , device->size ,
             device->channels , device->irqs );   
 #endif
-
-            // Initialize information about physical memory in cluster
-            if (device->type == DEV_TYPE_RAM)
-            {
-                // Compute total number of physical memory pages in cluster
-                boot_info->pages_nr = device->size >> CONFIG_PPM_PAGE_SHIFT;
-
-                // Get the last address allocated for the kernel segments
-                uint32_t end = (ktext_end < kdata_end) ? kdata_end : ktext_end;
-
-                // Computing the number of pages allocated for the kernel.
-                if( (end & CONFIG_PPM_PAGE_MASK) == 0 )
-                {
-                    boot_info->pages_offset = end >> CONFIG_PPM_PAGE_SHIFT;
-                }
-                else
-                {
-                    boot_info->pages_offset = (end >> CONFIG_PPM_PAGE_SHIFT) + 1;
-                }
-            }
-            
-            // Initialize array of irq descriptors for XCU 
-            if (device->type == DEV_TYPE_XCU) 
-            {
-                for (irq_id = 0; irq_id < CONFIG_MAX_HWIS_PER_ICU; irq_id++)
-                {
-                    boot_dev->irq[irq_id].valid  = 0;
-                }
-
-                for (irq = &irq_base[device->irq_offset];
-                     irq < &irq_base[device->irq_offset + device->irqs];
-                     irq++)
-                {
-                    boot_dev->irq[irq->port].valid    = 1;
-                    boot_dev->irq[irq->port].dev_type = irq->dev_type;
-                    boot_dev->irq[irq->port].channel  = irq->channel;
-                    boot_dev->irq[irq->port].is_rx    = irq->is_rx;
+    
+        // Initialize array of irq descriptors for PIC
+        if (device->type == DEV_TYPE_PIC_TSR) 
+        {
+            for (irq_id = 0; irq_id < CONFIG_MAX_IRQS_PER_PIC; irq_id++)
+            {
+                boot_dev->irq[irq_id].valid  = 0;
+            }
+
+            for (irq = &irq_base[device->irq_offset];
+                 irq < &irq_base[device->irq_offset + device->irqs];
+                 irq++)
+            {
+                boot_dev->irq[irq->port].valid    = 1;
+                boot_dev->irq[irq->port].dev_type = irq->dev_type;
+                boot_dev->irq[irq->port].channel  = irq->channel;
+                boot_dev->irq[irq->port].is_rx    = irq->is_rx;
 
 #if DEBUG_BOOT_INFO
@@ -421,24 +458,126 @@
             irq->port , device_type_str( irq->dev_type ) , irq->channel , irq->is_rx );
 #endif
-
-                }
-            }
-
-            // Initialize array of irq descriptors for PIC
-            if (device->type == DEV_TYPE_PIC) 
-            {
-                for (irq_id = 0; irq_id < CONFIG_MAX_IRQS_PER_PIC; irq_id++)
-                {
-                    boot_dev->irq[irq_id].valid  = 0;
-                }
-
-                for (irq = &irq_base[device->irq_offset];
-                     irq < &irq_base[device->irq_offset + device->irqs];
-                     irq++)
-                {
-                    boot_dev->irq[irq->port].valid    = 1;
-                    boot_dev->irq[irq->port].dev_type = irq->dev_type;
-                    boot_dev->irq[irq->port].channel  = irq->channel;
-                    boot_dev->irq[irq->port].is_rx    = irq->is_rx;
+            }
+        }
+    }
+
+    // initialize number of external peripherals
+    boot_info->ext_dev_nr = device_id;
+
+    // Initialize cluster specific resources
+    boot_info->cxy  = my_cluster->cxy;
+
+#if DEBUG_BOOT_INFO
+boot_printf("\n[BOOT INFO] %s : cores in cluster %x\n", __FUNCTION__ );
+#endif
+
+    // Initialize array of core descriptors
+    core_id = 0;
+    for (core = &core_base[my_cluster->core_offset];
+         core < &core_base[my_cluster->core_offset + my_cluster->cores];
+         core++ )
+    {
+        boot_info->core[core_id].gid = (gid_t)core->gid;
+        boot_info->core[core_id].lid = (lid_t)core->lid; 
+        boot_info->core[core_id].cxy = (cxy_t)core->cxy;
+
+#if DEBUG_BOOT_INFO
+boot_printf("  - core_gid = %x : cxy = %x / lid = %d\n", 
+            core->gid , core->cxy , core->lid );
+#endif
+        core_id++;
+    }
+
+    // Initialize number of cores in my_cluster
+    boot_info->cores_nr = core_id;
+
+    // loop on all peripherals in my_cluster to initialise
+    // boot_info array of internal peripherals in my_cluster
+
+#if DEBUG_BOOT_INFO
+boot_printf("\n[BOOT INFO] %s : internal peripherals in cluster %x\n", __FUNCTION__ );
+#endif
+
+    device_id = 0;
+    for (device = &device_base[my_cluster->device_offset];
+         device < &device_base[my_cluster->device_offset + my_cluster->devices];
+         device++ )
+    {
+        // initialise one entry for each internal peripheral
+        if( (device->type == DEV_TYPE_RAM_SCL) ||
+            (device->type == DEV_TYPE_ICU_XCU) ||
+            (device->type == DEV_TYPE_MMC_TSR) ||
+            (device->type == DEV_TYPE_DMA_SCL) ) 
+        {
+            boot_dev = &boot_info->int_dev[device_id];
+
+            boot_dev->type     = device->type;
+            boot_dev->base     = device->base;
+            boot_dev->size     = device->size;
+            boot_dev->channels = device->channels;
+            boot_dev->param0   = device->arg0;    
+            boot_dev->param1   = device->arg1;    
+            boot_dev->param2   = device->arg2;    
+            boot_dev->param3   = device->arg3;    
+            boot_dev->irqs     = device->irqs; 
+   
+            device_id++;
+        }
+
+#if DEBUG_BOOT_INFO
+boot_printf("  - %s : base = %l / size = %l / channels = %d / irqs = %d\n",
+            device_type_str( device->type ) , device->base , device->size ,
+            device->channels , device->irqs );   
+#endif
+
+        // Initialize information about physical memory in cluster
+        if (device->type == DEV_TYPE_RAM_SCL)
+        {
+            // Compute total number of physical memory pages in cluster
+            boot_info->pages_nr = device->size >> CONFIG_PPM_PAGE_SHIFT;
+
+            // Get the last address allocated for the kernel segments
+            uint32_t end;
+            if( boot_info->kernel_code_end > boot_info->kernel_data_end )
+            {
+                end = boot_info->kernel_code_end;
+            }
+            else
+            {
+                end = boot_info->kernel_data_end;
+            }
+                
+            // Compute the number of pages allocated for the kernel.
+            if( (end & CONFIG_PPM_PAGE_MASK) == 0 )
+            {
+                boot_info->pages_offset = end >> CONFIG_PPM_PAGE_SHIFT;
+            }
+            else
+            {
+                boot_info->pages_offset = (end >> CONFIG_PPM_PAGE_SHIFT) + 1;
+            }
+
+#if DEBUG_BOOT_INFO
+boot_printf("    . physical memory : %x pages / first free page = %x\n",
+            boot_info->pages_nr , boot_info->pages_offset );
+#endif
+        }
+            
+        // Initialize array of irq descriptors for XCU 
+        if (device->type == DEV_TYPE_ICU_XCU) 
+        {
+            for (irq_id = 0; irq_id < CONFIG_MAX_HWIS_PER_ICU; irq_id++)
+            {
+                boot_dev->irq[irq_id].valid  = 0;
+            }
+
+            for (irq = &irq_base[device->irq_offset];
+                 irq < &irq_base[device->irq_offset + device->irqs];
+                 irq++)
+            {
+                boot_dev->irq[irq->port].valid    = 1;
+                boot_dev->irq[irq->port].dev_type = irq->dev_type;
+                boot_dev->irq[irq->port].channel  = irq->channel;
+                boot_dev->irq[irq->port].is_rx    = irq->is_rx;
 
 #if DEBUG_BOOT_INFO
@@ -447,22 +586,21 @@
 #endif
 
-                }
             }
         }
-
-#if DEBUG_BOOT_INFO
-boot_printf("  - ram : number of pages = %x / first free page = %x\n",
-            boot_info->pages_nr , boot_info->pages_offset );
-#endif
-
-    }
+    }
+
+    // initialize number of internal peripherals in my_cluster
+    boot_info->int_dev_nr = device_id;
+
+    // set boot_info signature
+    boot_info->signature = BOOT_INFO_SIGNATURE;
+
 } // boot_info_init()
 
-/****************************************************************************
- * This function is executed by all cores in order to check their           *
- * local boot_info_t structure.                                             *
- * @ boot_info  : pointer to local 'boot_info_t' structure to be checked.   *
- * @ lid        : core local identifier, index the core descriptor table.   *
- ****************************************************************************/
+/***********************************************************************************
+ * This function check the local boot_info_t structure for a given core.
+ * @ boot_info  : pointer to local 'boot_info_t' structure to be checked.
+ * @ lid        : core local identifier, index the core descriptor table.
+ **********************************************************************************/
 static void boot_check_core( boot_info_t * boot_info, 
                              lid_t         lid)
@@ -490,20 +628,16 @@
 } // boot_check_core()
 
-/****************************************************************************
- * This function is called by the bscpu to activate all other CP0s.         *
+/*********************************************************************************
+ * This function is called by CP0 in cluster(0,0) to activate all other CP0s.
  * It returns the number of CP0s actually activated.
- ****************************************************************************/
-static uint32_t boot_wake_cp0()
+ ********************************************************************************/
+static uint32_t boot_wake_all_cp0s()
 {
-    archinfo_header_t*  header;         /* Pointer on ARCHINFO header.      */
-    archinfo_cluster_t* cluster_base;   /* Pointer on ARCHINFO cluster 
-                                           base.                            */ 
-    archinfo_cluster_t* cluster;        /* Iterator for waking CP0 loop.    */
-    archinfo_device_t*  device_base;    /* Pointer on ARCHINFO peripheral 
-                                           device base.                     */
-    archinfo_device_t*  device;         /* Iterator for finding XICU device 
-                                           loop.                            */
-
-    uint32_t            cp0_nb = 0;     /* Number of CP0 woken up.          */
+    archinfo_header_t*  header;         // Pointer on ARCHINFO header
+    archinfo_cluster_t* cluster_base;   // Pointer on ARCHINFO clusters base
+    archinfo_cluster_t* cluster;        // Iterator for loop on clusters
+    archinfo_device_t*  device_base;    // Pointer on ARCHINFO devices base
+    archinfo_device_t*  device;         // Iterator for loop on devices
+    uint32_t            cp0_nb = 0;     // CP0s counter
 
     header       = (archinfo_header_t*)ARCHINFO_BASE;
@@ -528,12 +662,17 @@
             continue;
 
-        // Look for the XICU device associated to the CP0 of this cluster 
-        // then send an WTI to it in order to wake it up. 
+        // search XICU device associated to CP0, and send a WTI to activate it 
         for (device = &device_base[cluster->device_offset];
              device < &device_base[cluster->device_offset + cluster->devices];
              device++)
         {
-            if (device->type == DEV_TYPE_XCU)
-            {
+            if (device->type == DEV_TYPE_ICU_XCU)
+            {
+
+#if DEBUG_BOOT_WAKUP
+boot_printf("\n[BOOT] core[%x][0] activated at cycle %d\n",
+            cluster->cxy , boot_get_proctime );
+#endif
+
                 boot_remote_sw((xptr_t)device->base, (uint32_t)boot_entry);
                 cp0_nb++;
@@ -545,227 +684,182 @@
 } // boot_wake_cp0()
 
-/****************************************************************************
- * This function is called by all CP0 to activate all local CPi cores.      *
- * @ boot_info  : pointer to local 'boot_info_t' structure, used to find    *
- *                the XICU device associated with local CPi base addresses. *
- ****************************************************************************/
-static void boot_wake_local_cores(boot_info_t* boot_info)
+/*********************************************************************************
+ * This function is called by all CP0 to activate all local CPi cores. 
+ * @ boot_info  : pointer to local 'boot_info_t' structure, used to find
+ *                the XICU device associated with local CPi base addresses.
+ *********************************************************************************/
+static void boot_wake_local_cores(boot_info_t * boot_info)
 {
-    boot_device_t*  device;         // Iterator on devices
-    unsigned int    core_id;        // Iterator on cores
+    boot_device_t *  device;         // Iterator on devices
+    unsigned int     core_id;        // Iterator on cores
 
     // loop on devices to find XCU
-    for (device = &boot_info->dev[0];
-         device < &boot_info->dev[boot_info->devices_nr];
+    for (device = &boot_info->int_dev[0];
+         device < &boot_info->int_dev[boot_info->int_dev_nr];
          device++)
     {
-        if (device->type == DEV_TYPE_XCU)
+        if (device->type == DEV_TYPE_ICU_XCU)
         {
             // loop on cores
             for (core_id = 1; core_id < boot_info->cores_nr; core_id++)
+            {
+
+#if DEBUG_BOOT_WAKUP
+boot_printf("\n[BOOT] core[%x][%d] activated at cycle %d\n",
+             boot_info->cxy , core_id , boot_get_proctime() );
+#endif
                 boot_remote_sw((xptr_t) (device->base + (core_id << 2)),
                                (uint32_t)boot_entry); 
+            }
         }
     }
 } // boot_wake_local_cores()
 
-/****************************************************************************
- *                               API functions.                             *
- ****************************************************************************/
-
-/****************************************************************************
+
+/*********************************************************************************
  * This main function of the boot-loader is called by the  boot_entry()  
  * function, and executed by all cores. 
  * The arguments values are computed by the boot_entry code.
- * @ lid    : core local identifier in its cluster,
+ * @ lid    : core local identifier,
  * @ cxy    : cluster identifier,
- ****************************************************************************/
+ *********************************************************************************/
 void boot_loader( lid_t lid, 
                   cxy_t cxy )
 {
-    boot_info_t * boot_info;   // Pointer on local boot_info_t structure
-    uint32_t     local_count;  // Number of cores expected in local barrier                                  */
+    boot_info_t * boot_info;       // pointer on local boot_info_t structure
 
     if (lid == 0) 
     {
-        /**************************************
-         * PHASE ONE: only bscpu executes it. *
-         **************************************/
+        /****************************************************
+         * PHASE A : only CP0 in boot cluster executes it 
+         ***************************************************/
         if (cxy == BOOT_CORE_CXY)
         {
-            boot_printf("\n[BOOT] Starting on core[%d] in cluster %x at cycle %d\n",
-                        lid, cxy, boot_get_proctime());
+            boot_printf("\n[BOOT] core[%x][%d] enters at cycle %d\n",
+                        cxy , lid , boot_get_proctime() );
 
             // Initialize IOC driver 
             if      (USE_IOC_BDV) boot_bdv_init();
             else if (USE_IOC_HBA) boot_hba_init();
-            /*
-            else if (USE_IOC_SDC) boot_sdc_init();
-            else if (USE_IOC_SPI) boot_spi_init();
-            */
+            // else if (USE_IOC_SDC) boot_sdc_init();
+            // else if (USE_IOC_SPI) boot_spi_init();
             else if (!USE_IOC_RDK)
             {
-                boot_printf("\n[BOOT ERROR] boot_loader(): "
-                            "No IOC driver\n"
-                           );
+                boot_printf("\n[BOOT ERROR] in %s : no IOC driver\n");
                 boot_exit();
             }
 
-            // Initialize simplified version of FAT32. 
+            // Initialize FAT32. 
             boot_fat32_init();
+
+            // Load the 'kernel.elf' file into memory from IOC, and set   
+            // the global variables defining the kernel layout      
+            boot_kernel_load();
+
+            boot_printf("\n[BOOT] core[%x][%d] loaded kernel at cycle %d\n",
+                        cxy , lid , boot_get_proctime() );
 
             // Load the arch_info.bin file into memory.
             boot_archinfo_load();
 
-            // Load the 'kernel.elf' file into memory.
-            boot_kernel_load();
-
-            // Get local 'boot_info_t' structure base address.
+            // Get local boot_info_t structure base address.
             // It is the first structure in the .kdata segment. 
-            boot_info = (boot_info_t*)(KERN_IMG_TMP_BASE + kdata_base);
-
-            // Signature problem, abort program !!!
+            boot_info = (boot_info_t *)seg_kdata_base;
+
+            // Initialize local boot_info_t structure.
+            boot_info_init( boot_info , cxy );
+
+            // check boot_info signature
             if (boot_info->signature != BOOT_INFO_SIGNATURE)
             {
-                boot_printf("\n[BOOT] boot_loader(): "
-                            "boot_info signature should be %x\n",
-                            BOOT_INFO_SIGNATURE);
+                boot_printf("\n[BOOT ERROR] in %s reported by core[%x][%d]\n"
+                            "  illegal boot_info signature / should be %x\n",
+                            __FUNCTION__ , cxy , lid , BOOT_INFO_SIGNATURE );
                 boot_exit();
             }
 
-            // Initialize local 'boot_info_t' structure.
-            boot_printf("\n[BOOT] boot_loader(): "
-                        "Initializing the local boot_info_t structure "
-                        "at cycle %d\n",
-                        boot_get_proctime());
-
-            boot_info_init(boot_info, cxy);
+            boot_printf("\n[BOOT] core[%x][%d] loaded boot_info at cycle %d\n",
+                        cxy , lid , boot_get_proctime() );
 
             // Check core information.
             boot_check_core(boot_info, lid);
 
-            // Set the barrier.
-            boot_cluster_ready = 0;
-
-            // Activate other CP0s
-            boot_printf("\n[BOOT] boot_loader(): "
-                        "Waking other CP0 up at cycle %d\n",
-                        boot_get_proctime());
-
-            global_count = boot_wake_cp0();
-
-            // Wait until all CP0s ready to enter kernel.
-            boot_printf("\n[BOOT] boot_loader(): "
-                        "Waiting for other %d CP0 at cycle %d\n",
-                        global_count, boot_get_proctime());
-
-            boot_barrier(XPTR(BOOT_CORE_CXY, &global_barrier), 
-                         global_count);
-
-            // activate other local cores of the boot cluster.
-            local_barrier = 0;
-            boot_printf("\n[BOOT] boot_loader(): "
-                        "Waking other CPi up at cycle %d\n",
-                        boot_get_proctime());
-
-            boot_wake_local_cores(boot_info);
-
-            // Wait until all other local cores are ready 
-            boot_printf("\n[BOOT] boot_loader(): "
-                        "Waiting for other %d CPi at cycle %d\n",
-                        boot_info->cores_nr - 1, boot_get_proctime());
-
-            local_count = boot_info->cores_nr - 1;
-            while (local_barrier != local_count);
-
-            // Move the local kernel image at address 0x0 (erase preloader code).
-            boot_printf("\n[BOOT] boot_loader(): "
-                        "Moving the kernel image and erasing the preloader"
-                        "at cycle %d\n",
-                        boot_get_proctime());
-
-            // ktext segment.
-            boot_memcpy((void*)ktext_base,
-                        (void*)(KERN_IMG_TMP_BASE + ktext_base),
-                        ktext_end - ktext_base);
-
-            // kdata segment.
-            boot_memcpy((void*)kdata_base,
-                        (void*)(KERN_IMG_TMP_BASE + kdata_base),
-                        kdata_end - kdata_base);
-            
-            // activate other local cores.
-            boot_cluster_ready = 1;
-            boot_printf("\n[BOOT] boot_loader(): "
-                        "Everything is set, jumping to the kernel "
-                        "at cycle %d\n",
-                        boot_get_proctime());
+            // Activate other CP0s / get number of active CP0s
+            active_cp0s_nr = boot_wake_all_cp0s() + 1;
+
+            // Wait until all clusters (i.e all CP0s) ready to enter kernel.
+            boot_remote_barrier( XPTR( BOOT_CORE_CXY , &global_barrier ) ,
+                                 active_cp0s_nr );
+
+            // activate other local cores
+            boot_wake_local_cores( boot_info );
+
+            // Wait until all local cores in cluster ready
+            boot_remote_barrier( XPTR( cxy , &local_barrier ) , 
+                                 boot_info->cores_nr );
         }
-        /****************************************************
-         * PHASE TWO: all CP0s other than bscpu execute it. *
-         ****************************************************/
+        /******************************************************************
+         * PHASE B : all CP0s other than CP0 in boot cluster execute it
+         *****************************************************************/
         else
         {
-            /* 
-             * Note: at this point, we cannot access the global variables of
-             * this boot code since all the address extension registers for 
-             * DATA are pointing to their local cluster in order to have 
-             * access to the local stack and execute this C code. 
-             * However, all the address extension registers for INSTRUCTIONS 
-             * are still pointing to the boot cluster, thus we can access 
-             * and call functions defined in the boot code, for example 
-             * boot_remote_memcpy().
-             */
-
-            // Copy the boot-loader binary code 'boot.elf' into the local memory
-            boot_remote_memcpy(XPTR(cxy,           BOOT_BASE),
-                               XPTR(BOOT_CORE_CXY, BOOT_BASE),
-                               (unsigned int)BOOT_MAX_SIZE);
-
-            /*
-             * Note: from now on, it is safe to refer to the boot code global variables 
-             * such as the base address and size of the kernel segments.
-             */
-
-            // switch to the INSTRUCTION local memory space,  
-            // to avoid contention at the boot cluster.
+            // at this point, all INSTRUCTION address extension registers
+            // point on cluster(0,0), but the DATA extension registers point
+            // already on the local cluster to use the local stack. 
+            // To access the bootloader global variables we must first copy
+            // the boot code (data and instructions) in the local cluster.
+            boot_remote_memcpy( XPTR( cxy           , BOOT_BASE ),
+                                XPTR( BOOT_CORE_CXY , BOOT_BASE ),
+                                BOOT_MAX_SIZE );
+
+            // from now, it is safe to refer to the boot code global variables 
+            boot_printf("\n[BOOT] core[%x][%d] replicated boot code at cycle %d\n",
+                        cxy , lid , boot_get_proctime() );
+
+            // switch to the INSTRUCTION local memory space, to avoid contention.
             asm volatile("mtc2  %0, $25" :: "r"(cxy));
 
-            // Copy the 'arch_info.bin' file into the local memory.
+            // Copy the arch_info.bin file into the local memory.
             boot_remote_memcpy(XPTR(cxy,           ARCHINFO_BASE),
                                XPTR(BOOT_CORE_CXY, ARCHINFO_BASE),
-                               (unsigned int)ARCHINFO_MAX_SIZE);
-
-            // Copy the kernel image into local memory at address 0x0.
-            // ktext segment.
-            boot_remote_memcpy(XPTR(cxy, ktext_base),
-                               XPTR(BOOT_CORE_CXY, KERN_IMG_TMP_BASE + ktext_base),
-                               ktext_end - ktext_base);
-            // kdata segment.
-            boot_remote_memcpy(XPTR(cxy, kdata_base),
-                               XPTR(BOOT_CORE_CXY, KERN_IMG_TMP_BASE + kdata_base),
-                               kdata_end - kdata_base);
-
-            // Get local 'boot_info_t' structure base address.
-            // This is the first structure in the kdata segment.
-            boot_info = (boot_info_t*)kdata_base;
+                               ARCHINFO_MAX_SIZE );
+
+            boot_printf("\n[BOOT] core[%x][%d] replicated arch_info at cycle %d\n",
+                        cxy , lid , boot_get_proctime() );
+
+            // Copy the kcode segment into local memory 
+            boot_remote_memcpy( XPTR( cxy           , seg_kcode_base ),
+                                XPTR( BOOT_CORE_CXY , seg_kcode_base ),
+                                seg_kcode_size );
+
+            // Copy the kdata segment into local memory
+            boot_remote_memcpy( XPTR( cxy           , seg_kdata_base ),
+                                XPTR( BOOT_CORE_CXY , seg_kdata_base ),
+                                seg_kdata_size );
+
+            boot_printf("\n[BOOT] core[%x][%d] replicated kernel code at cycle %d\n",
+                        cxy , lid , boot_get_proctime() );
+
+            // Get local boot_info_t structure base address.
+            boot_info = (boot_info_t*)seg_kdata_base;
 
             // Initialize local boot_info_t structure.
-            boot_info_init(boot_info, cxy);
+            boot_info_init( boot_info , cxy );
 
             // Check core information.
-            boot_check_core(boot_info, lid);
-
-            // Activateall other local CPi cores in this cluster.
-            local_barrier = 0;
-            boot_wake_local_cores(boot_info);
-
-            // Waiting until all other local cores ready
-            local_count = boot_info->cores_nr - 1;
-            while (local_barrier != local_count);
-
-            // All cores in this cluster are ready to enter kernel.
-            boot_barrier(XPTR(BOOT_CORE_CXY, &global_barrier), 
-                         global_count);
+            boot_check_core( boot_info , lid );
+
+            // get number of active clusters from BOOT_CORE cluster
+            uint32_t count = boot_remote_lw( XPTR( BOOT_CORE_CXY , &active_cp0s_nr ) );
+
+            // Wait until all clusters (i.e all CP0s) ready to enter kernel
+            boot_remote_barrier( XPTR( BOOT_CORE_CXY , &global_barrier ) , count );
+
+            // activate other local cores
+            boot_wake_local_cores( boot_info );
+
+            // Wait until all local cores in cluster ready
+            boot_remote_barrier( XPTR( cxy , &local_barrier ) , 
+                                 boot_info->cores_nr );
         }
     }
@@ -773,36 +867,38 @@
     {
         /***************************************************************
-         * PHASE THREE: all non CP0 cores in all clusters execute it.  *
+         * PHASE C: all non CP0 cores in all clusters execute it
          **************************************************************/
 
-        if (cxy == BOOT_CORE_CXY)  // boot cluster only
-        {
-            // Report to the local CP0 that CPi is ready
-            boot_atomic_add((int32_t*)&local_barrier, 1);
-
-            // wait completion of kernel image move in  boot cluster
-            while (boot_cluster_ready != 1);
-
-            // Check core information
-            boot_info = (boot_info_t*)kdata_base;
-            boot_check_core(boot_info, lid);
-        }
-        else                      // other clusters
-        {
-            // Switch to the INSTRUCTIONS local memory space 
-            // to avoid contention at the boot cluster.
-            asm volatile("mtc2  %0, $25" :: "r"(cxy));
-
-            // Report to the local CP0 that CPi is ready
-            boot_atomic_add((int32_t*)&local_barrier, 1);
-
-            // Check core information
-            boot_info = (boot_info_t*)kdata_base;
-            boot_check_core(boot_info, lid);
-        }
-    }
-
-    // Jump to the kernel code. 
-    asm volatile("jr   %0" :: "r"(kernel_entry));
+        // Switch to the INSTRUCTIONS local memory space 
+        // to avoid contention at the boot cluster.
+        asm volatile("mtc2  %0, $25" :: "r"(cxy));
+
+        // Get local boot_info_t structure base address.
+        boot_info = (boot_info_t *)seg_kdata_base;
+
+        // Check core information
+        boot_check_core(boot_info, lid);
+
+        // Wait until all local cores in cluster ready
+        boot_remote_barrier( XPTR( cxy , &local_barrier ) , boot_info->cores_nr );
+    }
+
+    // Each core compute address of a temporary kernel stack 
+    // in the upper part of the local cluster memory...
+    uint32_t stack_ptr = ((boot_info->pages_nr - lid) << 12) - 16;
+
+    // All cores initialise stack pointer,
+    // reset the BEV bit in status register,
+    // register "boot_info" argument in a0,
+    // and jump to kernel_entry. 
+    asm volatile( "mfc0  $27,  $12           \n"
+                  "lui   $26,  0xFFBF        \n"
+                  "ori   $26,  $26,  0xFFFF  \n"
+                  "and   $27,  $27,  $26     \n"
+                  "mtc0  $27,  $12           \n" 
+                  "move  $4,   %0            \n"
+                  "move  $29,  %1            \n"
+                  "jr    %2                  \n"
+                  :: "r"(boot_info) , "r"(stack_ptr) , "r"(kernel_entry) );
 
 } // boot_loader()
