Index: soft/giet_vm/giet_common/locks.c
===================================================================
--- soft/giet_vm/giet_common/locks.c	(revision 455)
+++ soft/giet_vm/giet_common/locks.c	(revision 466)
@@ -8,5 +8,8 @@
 #include "locks.h"
 #include "giet_config.h"
+#include "hard_config.h"
 #include "utils.h"
+#include "tty0.h"
+#include "kernel_malloc.h"
 
 ///////////////////////////////////////////////////
@@ -32,98 +35,21 @@
 }
 
-////////////////////////////////////
-void _lock_init( spin_lock_t* lock )
-{
-    lock->current = 0;
-    lock->free    = 0;
-
-#if GIET_DEBUG_SYS_LOCK
+///////////////////////////////////////////////////////////////////////////////////
+//      Simple lock access functions
+///////////////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////
+void _simple_lock_acquire( simple_lock_t* lock )
+{
+
+#if GIET_DEBUG_SIMPLE_LOCK
 unsigned int    gpid = _get_procid();
 unsigned int    x    = gpid >> (Y_WIDTH + P_WIDTH);
 unsigned int    y    = (gpid >> P_WIDTH) & ((1<<Y_WIDTH)-1);
 unsigned int    l    = gpid & ((1<<P_WIDTH)-1);
-_printf("\n[SYS_LOCK DEBUG] P[%d,%d,%d] init lock %x"
-                " at cycle %d (current = %d / free = %d)\n",
-                x, y, l, (unsigned int)lock, 
-                _get_proctime(), lock->current, lock->free );
-#endif
-
-}
-
-
-////////////////////////////////////////
-void _lock_acquire( spin_lock_t* lock )
-{
-    // get next free slot index fromlock
-    unsigned int ticket = _atomic_increment( &lock->free, 1 );
-
-#if GIET_DEBUG_SYS_LOCK
-unsigned int    gpid = _get_procid();
-unsigned int    x    = gpid >> (Y_WIDTH + P_WIDTH);
-unsigned int    y    = (gpid >> P_WIDTH) & ((1<<Y_WIDTH)-1);
-unsigned int    l    = gpid & ((1<<P_WIDTH)-1);
-_printf("\n[SYS_LOCK DEBUG] P[%d,%d,%d] get ticket = %d"
-                " for lock %x at cycle %d (current = %d / free = %d)\n",
-                x, y, l, ticket, 
-                (unsigned int)lock, _get_proctime(), lock->current, lock->free );
-#endif
-
-
-    // poll the spin_lock current slot index
-    asm volatile("5678:                   \n"
-                 "lw   $10,  0(%0)        \n"
-                 "move $11,  %1           \n"
-                 "bne  $10,  $11,  5678b  \n"
-                 :
-                 : "r"(lock), "r"(ticket)
-                 : "$10", "$11" );
-
-#if GIET_DEBUG_SYS_LOCK
-_printf("\n[SYS_LOCK DEBUG] P[%d,%d,%d] get lock = %x"
-                " at cycle %d (current = %d / free = %d)\n",
-                x, y, l, (unsigned int)lock, 
-                _get_proctime(), lock->current, lock->free );
-#endif
-
-}
-
-////////////////////////////////////////
-void _lock_release( spin_lock_t* lock )
-{
-    unsigned int current = lock->current;
-
-    if ( current == (GIET_LOCK_MAX_TICKET - 1) ) current = 0;
-    else                                         current = current + 1;
-
-    asm volatile ( "sync                    \n"   /* for consistency                  */
-                   "sw   %1,    0(%0)       \n"   /* release lock                     */
-                   :
-                   : "r"(lock), "r"(current)
-                   : "memory" );
-    
-
-#if GIET_DEBUG_SYS_LOCK
-unsigned int    gpid = _get_procid();
-unsigned int    x    = gpid >> (Y_WIDTH + P_WIDTH);
-unsigned int    y    = (gpid >> P_WIDTH) & ((1<<Y_WIDTH)-1);
-unsigned int    l    = gpid & ((1<<P_WIDTH)-1);
-_printf("\n[SYS_LOCK DEBUG] P[%d,%d,%d] release lock = %x"
-                " at cycle %d (current = %d / free = %d)\n",
-                x, y, l, (unsigned int)lock, 
-                _get_proctime(), lock->current, lock->free );
-#endif
-
-}
-
-
-
-
-
-
-
-
-////////////////////////////////////////////////
-void _simple_lock_acquire( simple_lock_t* lock )
-{
+_nolock_printf("\n[DEBUG SIMPLE_LOCK] P[%d,%d,%d] enters acquire() at cycle %d\n",
+               x , y , l , _get_proctime() );
+#endif
+
     asm volatile ( "1515:                   \n"
 	               "lw   $2,    0(%0)       \n"   /* $2 <= lock current value         */
@@ -137,4 +63,10 @@
                    : "r"(lock)
                    : "$2", "$3", "memory" );
+
+#if GIET_DEBUG_SIMPLE_LOCK
+_nolock_printf("\n[DEBUG SIMPLE_LOCK] P[%d,%d,%d] exit acquire() at cycle %d\n",
+               x , y , l , _get_proctime() );
+#endif
+
 }
 
@@ -147,6 +79,411 @@
                    : "r"(lock)
                    : "memory" );
-}
-
+
+#if GIET_DEBUG_SIMPLE_LOCK
+unsigned int    gpid = _get_procid();
+unsigned int    x    = gpid >> (Y_WIDTH + P_WIDTH);
+unsigned int    y    = (gpid >> P_WIDTH) & ((1<<Y_WIDTH)-1);
+unsigned int    l    = gpid & ((1<<P_WIDTH)-1);
+_nolock_printf("\n[DEBUG SIMPLE_LOCK] P[%d,%d,%d] release() at cycle %d\n",
+               x , y , l , _get_proctime() );
+#endif
+
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////
+//      Queuing Lock access functions
+///////////////////////////////////////////////////////////////////////////////////
+
+/////////////////////////////////////////
+void _spin_lock_init( spin_lock_t* lock )
+{
+    lock->current = 0;
+    lock->free    = 0;
+
+#if GIET_DEBUG_SPIN_LOCK
+unsigned int    gpid = _get_procid();
+unsigned int    x    = gpid >> (Y_WIDTH + P_WIDTH);
+unsigned int    y    = (gpid >> P_WIDTH) & ((1<<Y_WIDTH)-1);
+unsigned int    l    = gpid & ((1<<P_WIDTH)-1);
+_puts("\n[DEBUG SPIN_LOCK] P[");
+_putd( x );
+_puts(",");
+_putd( y );
+_puts(",");
+_putd( l );
+_puts("] init lock ");
+_putx( (unsigned int)lock );
+_puts(" (current = ");
+_putd( lock->current );
+_puts(" / free = ");
+_putd( lock->free );
+_puts(" )\n");
+#endif
+
+}
+
+
+////////////////////////////////////////////
+void _spin_lock_acquire( spin_lock_t* lock )
+{
+    // get next free slot index fromlock
+    unsigned int ticket = _atomic_increment( &lock->free, 1 );
+
+#if GIET_DEBUG_SPIN_LOCK
+unsigned int    gpid = _get_procid();
+unsigned int    x    = gpid >> (Y_WIDTH + P_WIDTH);
+unsigned int    y    = (gpid >> P_WIDTH) & ((1<<Y_WIDTH)-1);
+unsigned int    l    = gpid & ((1<<P_WIDTH)-1);
+_puts("\n[DEBUG SPIN_LOCK] P[");
+_putd( x );
+_puts(",");
+_putd( y );
+_puts(",");
+_putd( l );
+_puts("] get ticket ");
+_putx( ticket );
+_puts(" for lock ");
+_putx( (unsigned int)lock );
+_puts(" (current = ");
+_putd( lock->current );
+_puts(" / free = ");
+_putd( lock->free );
+_puts(" )\n");
+#endif
+
+
+    // poll the spin_lock current slot index
+    asm volatile("5678:                   \n"
+                 "lw   $10,  0(%0)        \n"
+                 "move $11,  %1           \n"
+                 "bne  $10,  $11,  5678b  \n"
+                 :
+                 : "r"(lock), "r"(ticket)
+                 : "$10", "$11" );
+
+#if GIET_DEBUG_SPIN_LOCK
+_puts("\n[DEBUG SPIN_LOCK] P[");
+_putd( x );
+_puts(",");
+_putd( y );
+_puts(",");
+_putd( l );
+_puts("] get lock ");
+_putx( (unsigned int)lock );
+_puts(" (current = ");
+_putd( lock->current );
+_puts(" / free = ");
+_putd( lock->free );
+_puts(" )\n");
+#endif
+
+}
+
+////////////////////////////////////////////
+void _spin_lock_release( spin_lock_t* lock )
+{
+    unsigned int current = lock->current;
+
+    if ( current == (GIET_LOCK_MAX_TICKET - 1) ) current = 0;
+    else                                         current = current + 1;
+
+    asm volatile ( "sync                    \n"   /* for consistency                  */
+                   "sw   %1,    0(%0)       \n"   /* release lock                     */
+                   :
+                   : "r"(lock), "r"(current)
+                   : "memory" );
+    
+
+#if GIET_DEBUG_SPIN_LOCK
+unsigned int    gpid = _get_procid();
+unsigned int    x    = gpid >> (Y_WIDTH + P_WIDTH);
+unsigned int    y    = (gpid >> P_WIDTH) & ((1<<Y_WIDTH)-1);
+unsigned int    l    = gpid & ((1<<P_WIDTH)-1);
+_puts("\n[DEBUG SPIN_LOCK] P[");
+_putd( x );
+_puts(",");
+_putd( y );
+_puts(",");
+_putd( l );
+_puts("] release lock ");
+_putx( (unsigned int)lock );
+_puts(" (current = ");
+_putd( lock->current );
+_puts(" / free = ");
+_putd( lock->free );
+_puts(" )\n");
+#endif
+
+}
+
+///////////////////////////////////////////////////////////////////////////////////
+//      SBT lock access functions
+///////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////
+// This recursive function is used by the _sbt_lock_init() function
+// to initializes the SBT nodes (mainly the parent and child pointers).
+// It traverses the SBT from top to bottom.
+///////////////////////////////////////////////////////////////////////////////////
+static void _sbt_lock_build( sbt_lock_t*     lock,      // pointer on the SBT lock
+                             unsigned int    x,         // SBT node x coordinate
+                             unsigned int    y,         // SBT node y coordinate
+                             unsigned int    level,     // SBT node level
+                             lock_node_t*    parent )   // pointer on parent node
+{
+
+#if GIET_DEBUG_SBT_LOCK
+unsigned int    gpid = _get_procid();
+unsigned int    px   = gpid >> (Y_WIDTH + P_WIDTH);
+unsigned int    py   = (gpid >> P_WIDTH) & ((1<<Y_WIDTH)-1);
+unsigned int    pl   = gpid & ((1<<P_WIDTH)-1);
+#endif
+
+    // get target node pointer
+    lock_node_t* node = lock->node[x][y][level];
+    
+    if (level == 0 )        // terminal case
+    {
+        // initializes target node
+        node->taken    = 0;   
+        node->level    = level;
+        node->parent   = parent;
+        node->child0   = NULL;
+        node->child1   = NULL;
+        node->x        = x;
+        node->y        = y;
+
+#if GIET_DEBUG_SBT_LOCK
+_nolock_printf("\n[DEBUG SBT_LOCK] P[%d,%d,%d] initialises SBT node[%d,%d,%d] : "
+      "parent = %x / childO = %x / child1 = %x\n",
+      px , py , pl , node->x , node->y , node->level , 
+      (unsigned int)node->parent , (unsigned int)node->child0 , (unsigned int)node->child1 );
+#endif
+
+    }
+    else                   // non terminal case
+    {
+        unsigned int x0;   // x coordinate for child0
+        unsigned int y0;   // y coordinate for child0;
+        unsigned int x1;   // x coordinate for child1;
+        unsigned int y1;   // y coordinate for child1;
+
+        // the child0 coordinates are equal to the parent coordinates
+        // the child1 coordinates are incremented depending on the level value
+        if ( level & 0x1 ) // odd level => X binary tree
+        {
+            x0 = x;
+            y0 = y;
+            x1 = x + (1 << ((level-1)>>1));
+            y1 = y;
+        }    
+        else               // even level => Y binary tree
+        {
+            x0 = x;
+            y0 = y;
+            x1 = x;
+            y1 = y + (1 << ((level-1)>>1));
+        }
+
+        // initializes target node
+        node->taken    = 0;
+        node->level    = level;
+        node->parent   = parent;
+        node->child0   = lock->node[x0][y0][level-1];
+        node->child1   = lock->node[x1][y1][level-1];
+
+#if GIET_DEBUG_SBT_LOCK
+_nolock_printf("\n[DEBUG SBT_LOCK] P[%d,%d,%d] initialises SBT node[%d,%d,%d] : "
+      "parent = %x / childO = %x / child1 = %x\n",
+      px , py , pl , x , y , level , 
+      (unsigned int)node->parent , (unsigned int)node->child0 , (unsigned int)node->child1 );
+#endif
+
+        // recursive calls for children nodes
+        _sbt_lock_build( lock , x0 , y0 , level-1 , node );
+        _sbt_lock_build( lock , x1 , y1 , level-1 , node );
+    }
+
+}  // end _sbt_lock_build()
+
+//////////////////////////////////////////////////////////////////////////////////
+// This recursive function is used by the sbt_lock_acquire() function to
+// get the SBT lock: It tries to get each "partial" lock on the path from bottom
+// to top, using an atomic LL/SC, and starting from bottom.
+// It is blocking : it poll each "partial lock until it can be taken. 
+// The lock is finally obtained when all "partial" locks, at all levels are taken.
+//////////////////////////////////////////////////////////////////////////////////
+static void _sbt_lock_take( lock_node_t* node )
+{
+    // try to take "partial" lock
+    unsigned int* taken = &node->taken;
+
+    asm volatile ( "1945:                   \n"
+	               "lw   $2,    0(%0)       \n"   /* $2 <= lock current value         */
+	               "bnez $2,    1945b       \n"   /* retry if lock already taken      */
+                   "ll   $2,    0(%0)       \n"   /* ll_buffer <= lock current value  */
+                   "bnez $2,    1945b       \n"   /* retry if lock already taken      */
+                   "li   $3,    1           \n"   /* $3 <= argument for sc            */
+                   "sc   $3,    0(%0)       \n"   /* try to set lock                  */
+                   "beqz $3,    1945b       \n"   /* retry if sc failure              */
+                   :
+                   : "r"(taken)
+                   : "$2", "$3", "memory" );
+
+#if GIET_DEBUG_SBT_LOCK
+unsigned int    gpid = _get_procid();
+unsigned int    px   = gpid >> (Y_WIDTH + P_WIDTH);
+unsigned int    py   = (gpid >> P_WIDTH) & ((1<<Y_WIDTH)-1);
+unsigned int    pl   = gpid & ((1<<P_WIDTH)-1);
+_nolock_printf("\n[DEBUG SBT_LOCK] P[%d,%d,%d] get partial SBT lock[%d,%d,%d] : vaddr = %x\n",
+      px , py , pl , node->x , node->y , node->level , (unsigned int)node );
+#endif
+
+    // try to take the parent node lock until top is reached
+    if ( node->parent != NULL ) _sbt_lock_take( node->parent );
+
+} // end _sbt_lock_take()
+    
+
+/////////////////////////////////////////////////////////////////////////////////
+// This recursive function is used by the sbt_lock_release() function to
+// release the SBT lock: It reset all "partial" locks on the path from bottom 
+// to top, using a normal write, and starting from bottom.
+/////////////////////////////////////////////////////////////////////////////////
+static void _sbt_lock_free( lock_node_t* node )
+{
+    // reset "partial" lock
+    node->taken = 0;
+
+#if GIET_DEBUG_SBT_LOCK
+unsigned int    gpid = _get_procid();
+unsigned int    px   = gpid >> (Y_WIDTH + P_WIDTH);
+unsigned int    py   = (gpid >> P_WIDTH) & ((1<<Y_WIDTH)-1);
+unsigned int    pl   = gpid & ((1<<P_WIDTH)-1);
+_nolock_printf("\n[DEBUG SBT_LOCK] P[%d,%d,%d] release partial SBT lock[%d,%d,%d] : vaddr = %x\n",
+      px , py , pl , node->x , node->y , node->level , (unsigned int)node );
+#endif
+
+    // reset parent node until top is reached
+    if ( node->parent != NULL ) _sbt_lock_free( node->parent );
+
+} // end _sbt_lock_free()
+
+//////////////////////////////////////////////////////////////////////////////////
+// This external function initialises the distributed SBT lock.
+//////////////////////////////////////////////////////////////////////////////////
+void _sbt_lock_init( sbt_lock_t*  lock )
+{
+    unsigned int levels = 0;     // depth of the SBT (number of levels)
+
+    // compute SBT levels
+    if      ((X_SIZE == 1 ) && (Y_SIZE == 1 ))  levels = 1;
+    else if ((X_SIZE == 2 ) && (Y_SIZE == 1 ))  levels = 2;
+    else if ((X_SIZE == 2 ) && (Y_SIZE == 2 ))  levels = 3;
+    else if ((X_SIZE == 4 ) && (Y_SIZE == 2 ))  levels = 4;
+    else if ((X_SIZE == 4 ) && (Y_SIZE == 4 ))  levels = 5;
+    else if ((X_SIZE == 8 ) && (Y_SIZE == 4 ))  levels = 6;
+    else if ((X_SIZE == 8 ) && (Y_SIZE == 8 ))  levels = 7;
+    else if ((X_SIZE == 16) && (Y_SIZE == 8 ))  levels = 8;
+    else if ((X_SIZE == 16) && (Y_SIZE == 16))  levels = 9;
+    else
+    {
+        _nolock_printf("\n[GIET ERROR] _sbt_lock_init() :illegal X_SIZE/Y_SIZE \n");
+        _exit();
+    }
+
+#if GIET_DEBUG_SBT_LOCK
+unsigned int    gpid = _get_procid();
+unsigned int    px   = gpid >> (Y_WIDTH + P_WIDTH);
+unsigned int    py   = (gpid >> P_WIDTH) & ((1<<Y_WIDTH)-1);
+unsigned int    pl   = gpid & ((1<<P_WIDTH)-1);
+_nolock_printf("\n[DEBUG SBT_LOCK] P[%d,%d,%d] initialises SBT lock %x : %d levels\n",
+               px , py , pl , (unsigned int)lock , levels );
+#endif
+
+    // allocates memory for the SBT nodes and initializes SBT nodes pointers array
+    // the actual number of SBT nodes in a cluster(x,y) depends on (x,y): 
+    // At least 1 node / at most 9 nodes per cluster.
+    unsigned int x;              // x coordinate for one SBT node
+    unsigned int y;              // y coordinate for one SBT node
+    unsigned int l;              // level for one SBT node
+    for ( x = 0 ; x < X_SIZE ; x++ )
+    {
+        for ( y = 0 ; y < Y_SIZE ; y++ )
+        {
+            for ( l = 0 ; l < levels ; l++ )             // level 0 nodes
+            {
+                
+                if ( ( (l == 0) && ((x&0x00) == 0) && ((y&0x00) == 0) ) ||
+                     ( (l == 1) && ((x&0x01) == 0) && ((y&0x00) == 0) ) ||
+                     ( (l == 2) && ((x&0x01) == 0) && ((y&0x01) == 0) ) ||
+                     ( (l == 3) && ((x&0x03) == 0) && ((y&0x01) == 0) ) ||
+                     ( (l == 4) && ((x&0x03) == 0) && ((y&0x03) == 0) ) ||
+                     ( (l == 5) && ((x&0x07) == 0) && ((y&0x03) == 0) ) ||
+                     ( (l == 6) && ((x&0x07) == 0) && ((y&0x07) == 0) ) ||
+                     ( (l == 7) && ((x&0x0F) == 0) && ((y&0x07) == 0) ) ||
+                     ( (l == 8) && ((x&0x0F) == 0) && ((y&0x0F) == 0) ) )
+                 {
+                     lock->node[x][y][l] = (lock_node_t*)_remote_malloc( sizeof(lock_node_t),
+                                                                         x, y );
+
+#if GIET_DEBUG_SBT_LOCK
+_nolock_printf("\n[DEBUG SBT_LOCK] P[%d,%d,%d] allocates SBT node[%d,%d,%d] : vaddr = %x\n",
+               px , py , pl , x , y , l , (unsigned int)lock->node[x][y][l] );
+#endif
+                 }
+            }
+        }
+    }
+            
+#if GIET_DEBUG_SBT_LOCK
+_nolock_printf("\n[DEBUG SBT_LOCK] SBT nodes initialisation starts\n"); 
+#endif
+
+    // recursively initialize all SBT nodes from root to bottom
+    _sbt_lock_build( lock,       // pointer on the SBT lock descriptor
+                     0,          // x coordinate
+                     0,          // y coordinate
+                     levels-1,   // level in SBT
+                     NULL );     // pointer on the parent node
+
+    asm volatile ("sync" ::: "memory");
+
+#if GIET_DEBUG_SBT_LOCK
+_nolock_printf("\n[DEBUG SBT_LOCK] SBT nodes initialisation completed\n"); 
+#endif
+
+} // end _sbt_lock_init()
+
+//////////////////////////////////////////////////////////////////////////////////
+// This external function get thes SBT lock.
+// Returns only when the lock has been taken. 
+/////////////////////////////////////////////////////////////////////////////////
+void _sbt_lock_acquire( sbt_lock_t*  lock )
+{
+    // get cluster coordinates
+    unsigned int gpid = _get_procid();
+    unsigned int x    = (gpid >> (Y_WIDTH + P_WIDTH)) & ((1<<X_WIDTH)-1);
+    unsigned int y    = (gpid >> P_WIDTH) & ((1<<Y_WIDTH)-1);
+
+    // try to recursively take the "partial" locks (from bottom to top)
+    _sbt_lock_take( lock->node[x][y][0] );
+}
+
+
+/////////////////////////////////////////////////////////////////////////////////
+// This external function releases the SBT lock.
+/////////////////////////////////////////////////////////////////////////////////
+void _sbt_lock_release( sbt_lock_t*  lock )
+{
+    // get cluster coordinates
+    unsigned int gpid = _get_procid();
+    unsigned int x    = (gpid >> (Y_WIDTH + P_WIDTH)) & ((1<<X_WIDTH)-1);
+    unsigned int y    = (gpid >> P_WIDTH) & ((1<<Y_WIDTH)-1);
+
+    // recursively reset the "partial" locks (from bottom to top)
+    _sbt_lock_free( lock->node[x][y][0] );
+}
 
 // Local Variables:
