Index: soft/giet_vm/giet_common/utils.c
===================================================================
--- soft/giet_vm/giet_common/utils.c	(revision 408)
+++ soft/giet_vm/giet_common/utils.c	(revision 430)
@@ -20,5 +20,5 @@
 
 // This global variable is allocated in the boot.c file or in kernel_init.c file
-extern static_scheduler_t* _schedulers[NB_PROCS_MAX<<(X_WIDTH+Y_WIDTH)];
+extern static_scheduler_t* _schedulers[X_SIZE][Y_SIZE][NB_PROCS_MAX];
 
 ///////////////////////////////////////////////////////////////////////////////////
@@ -72,5 +72,5 @@
     asm volatile ( "mfc0     %0,     $15, 1  \n"
                    :"=r" (ret) );
-    return (ret & 0x3FF);
+    return (ret & 0xFFF);
 }
 ////////////////////////////
@@ -173,7 +173,4 @@
 
 ////////////////////////////////////////////////////////////////////////////
-// This function makes a physical read access to a 32 bits word in memory, 
-// after a temporary DTLB de-activation and paddr extension.
-////////////////////////////////////////////////////////////////////////////
 unsigned int _physical_read( unsigned long long paddr ) 
 {
@@ -202,9 +199,6 @@
 }
 ////////////////////////////////////////////////////////////////////////////
-// This function makes a physical write access to a 32 bits word in memory, 
-// after a temporary DTLB de-activation and paddr extension.
-////////////////////////////////////////////////////////////////////////////
 void _physical_write( unsigned long long paddr, 
-                               unsigned int       value ) 
+                      unsigned int       value ) 
 {
     unsigned int lsb = (unsigned int)paddr;
@@ -212,5 +206,5 @@
     unsigned int sr;
 
-    _it_disable(&sr);
+   _it_disable(&sr);
 
     asm volatile( "mfc2   $2,     $1                 \n"     /* $2 <= MMU_MODE   */
@@ -232,7 +226,4 @@
 
 ////////////////////////////////////////////////////////////////////////////
-// This function makes a physical read access to a 64 bits word in memory, 
-// after a temporary DTLB de-activation and paddr extension.
-////////////////////////////////////////////////////////////////////////////
 unsigned long long _physical_read_ull( unsigned long long paddr ) 
 {
@@ -266,7 +257,4 @@
 
 ////////////////////////////////////////////////////////////////////////////
-// This function makes a physical write access to a 64 bits word in memory, 
-// after a temporary DTLB de-activation and paddr extension.
-////////////////////////////////////////////////////////////////////////////
 void _physical_write_ull( unsigned long long paddr, 
                           unsigned long long value ) 
@@ -304,5 +292,5 @@
 // multiple of 4 bytes. 
 ///////////////////////////////////////////////////////////////////////////////////
-void _physical_memcpy( unsigned long long dst_paddr,  // dest buffer paddr
+void _physical_memcpy( unsigned long long dst_paddr,  // destination buffer paddr
                        unsigned long long src_paddr,  // source buffer paddr
                        unsigned int size )            // bytes
@@ -352,10 +340,49 @@
 
     _it_restore(&sr);
-}
-
-///////////////////////////////////////////////////////////////////////////////////
-// This function is used by several drivers (_xxx_set_register() function)
-// If the MMU is not activated, the virtual address is extended using
-// X_IO and Y_IO to reach the cluster_io.
+} // end _physical_memcpy()
+
+/////////////////////////////////////////////////////////////////////////////////
+void _physical_memset( unsigned long long paddr,     // destination buffer paddr
+                       unsigned int       size,      // bytes
+                       unsigned int       data )     // written value
+{
+    // check alignment constraints
+    if ( (paddr & 3) || (size & 3) ) 
+    {
+        _printf("\n[GIET ERROR] in _physical_memset() : buffer unaligned\n");
+        _exit();
+    }
+
+    unsigned int lsb  = (unsigned int)paddr;
+    unsigned int msb  = (unsigned int)(paddr >> 32);
+    unsigned int sr;
+
+    _it_disable(&sr);
+
+    asm volatile( "move   $4,     %0                 \n"     /* $4 < lsb               */
+                  "move   $5,     %1                 \n"     /* $5 < msb               */
+                  "move   $6,     %2                 \n"     /* $6 < size              */
+                  "move   $7,     %3                 \n"     /* $7 < data              */
+
+                  "mfc2   $2,     $1                 \n"     /* $2 <= current MMU_MODE */
+                  "andi   $3,     $2,        0xb     \n"     /* $3 <= new MMU_MODE     */
+                  "mtc2   $3,     $1                 \n"     /* DTLB off               */    
+                  "mtc2   $5,     $24                \n"     /* PADDR_EXT <= msb       */   
+          
+                  "ph_memset_loop:                   \n"
+                  "sw     $7,     0($4)              \n"     /* data <= *src_paddr     */
+                  "addi   $4,     $4,        4       \n"     /* iter = iter - 1        */
+                  "bne    $4,     $6, ph_memcpy_loop \n"
+                  "nop                               \n"
+
+                  "mtc2   $0,     $24                \n"     /* PADDR_EXT <= 0         */   
+                  "mtc2   $2,     $1                 \n"     /* restore MMU_MODE       */
+                  : "=r" (data)
+                  : "r" (lsb), "r" (msb), "r" (size), "r"(data)
+                  : "$2", "$3", "$4", "$5", "$6", "$7" );
+
+    _it_restore(&sr);
+}  // _pysical_memset()
+
 ///////////////////////////////////////////////////////////////////////////////////
 void _io_extended_write( unsigned int*  vaddr,
@@ -378,8 +405,4 @@
 
 ///////////////////////////////////////////////////////////////////////////////////
-// This function is used by all drivers (_xxx_get_register() function)
-// If the MMU is not activated, the virtual address is extended using
-// X_IO and Y_IO to reach the cluster_io.
-///////////////////////////////////////////////////////////////////////////////////
 unsigned int _io_extended_read( unsigned int*  vaddr )
 {
@@ -402,13 +425,4 @@
 ///////////////////////////////////////////////////////////////////////////////////
 
-///////////////////////////////////////////////////////////////////////////////////
-// Takes a lock with a blocking ll/sc atomic access.
-// When the cache coherence is granted by the hardware,
-// the first read is a standard (cacheable) lw, as the local copy
-// can be polled when the lock is already taken by another task, reducing 
-// trafic on the interconnect. When the lock is released by the owner task,
-// the local copy is updated or invalidated by the coherence protocol.
-// If there is no hardware cache coherence a random delay is introduced
-// betwween two successive retry.
 ///////////////////////////////////////////////////////////////////////////////////
 void _get_lock(giet_lock_t* lock) 
@@ -460,6 +474,4 @@
 }
 
-///////////////////////////////////////////////////////////////////////////////////
-// Release a previouly taken lock.
 ///////////////////////////////////////////////////////////////////////////////////
 void _release_lock(giet_lock_t* lock) 
@@ -711,8 +723,7 @@
     {
         unsigned int procid     = _get_procid();
-        unsigned int lpid       = procid % NB_PROCS_MAX;
-        unsigned int cluster_xy = procid / NB_PROCS_MAX;
-        unsigned int x          = cluster_xy >> Y_WIDTH;
-        unsigned int y          = cluster_xy & ((1<<Y_WIDTH)-1);
+        unsigned int x          = (procid >> (Y_WIDTH + P_WIDTH)) & ((1<<X_WIDTH)-1);
+        unsigned int y          = (procid >> P_WIDTH) & ((1<<Y_WIDTH)-1);
+        unsigned int lpid       = procid & ((1<<P_WIDTH)-1);
 
         _puts("\n\n[GIET ERROR] in _printf() for processor[");
@@ -744,11 +755,10 @@
 
 
-//////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////
 //           Scheduler and tasks context access functions
-//////////////////////////////////////////////////////////////////////////////
-
-//////////////////////////////////////////////////////////////////////////////
-// Returns index of the currently running task from the processor scheduler.
-//////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////
+
+
+////////////////////////////////////////////////////////////////////////////////////
 unsigned int _get_current_task_id() 
 {
@@ -757,31 +767,24 @@
 }
 ////////////////////////////////////////////////////////////////////////////////////
-// This function returns the content of a context slot 
-// for a task identified by the ltid argument (local task index),
-// and the gpid argument (global processor index)
-////////////////////////////////////////////////////////////////////////////////////
-unsigned int _get_task_slot( unsigned int gpid,
+unsigned int _get_task_slot( unsigned int x,
+                             unsigned int y,
+                             unsigned int p,
                              unsigned int ltid,
                              unsigned int slot )
 {
-    static_scheduler_t* psched  = (static_scheduler_t*)_schedulers[gpid];
+    static_scheduler_t* psched  = (static_scheduler_t*)_schedulers[x][y][p];
     return psched->context[ltid][slot];
 }
 ////////////////////////////////////////////////////////////////////////////////////
-// This function updates the content of a context slot 
-// for any task identified by the ltid argument (local task index),
-// and the gpid argument (global processor index)
-////////////////////////////////////////////////////////////////////////////////////
-void _set_task_slot( unsigned int gpid,
+void _set_task_slot( unsigned int x,
+                     unsigned int y,
+                     unsigned int p,
                      unsigned int ltid,
                      unsigned int slot,
                      unsigned int value )
 {
-    static_scheduler_t* psched  = (static_scheduler_t*)_schedulers[gpid];
+    static_scheduler_t* psched  = (static_scheduler_t*)_schedulers[x][y][p];
     psched->context[ltid][slot] = value;
 }
-////////////////////////////////////////////////////////////////////////////////////
-// This function returns the content of a context slot 
-// for the running task (defined by the scheduler current field).
 ////////////////////////////////////////////////////////////////////////////////////
 unsigned int _get_context_slot( unsigned int slot )
@@ -791,6 +794,4 @@
     return psched->context[task_id][slot];
 }
-////////////////////////////////////////////////////////////////////////////////////
-// This function updates the content of a context slot for the running task.
 ////////////////////////////////////////////////////////////////////////////////////
 void _set_context_slot( unsigned int slot,
@@ -968,8 +969,7 @@
 {
     unsigned int procid     = _get_procid();
-    unsigned int lpid       = procid % NB_PROCS_MAX;
-    unsigned int cluster_xy = procid / NB_PROCS_MAX;
-    unsigned int x          = cluster_xy >> Y_WIDTH;
-    unsigned int y          = cluster_xy & ((1<<Y_WIDTH)-1);
+    unsigned int x          = (procid >> (Y_WIDTH + P_WIDTH)) & ((1<<X_WIDTH)-1);
+    unsigned int y          = (procid >> P_WIDTH) & ((1<<Y_WIDTH)-1);
+    unsigned int lpid       = procid & ((1<<P_WIDTH)-1);
 
 
