Index: soft/giet_vm/giet_common/utils.c
===================================================================
--- soft/giet_vm/giet_common/utils.c	(revision 293)
+++ soft/giet_vm/giet_common/utils.c	(revision 295)
@@ -47,19 +47,19 @@
                       unsigned int size )    // bytes
 {
-    unsigned int*       dst = dest;
-    const unsigned int* src = source;
+    unsigned int* idst = (unsigned int*)dest;
+    unsigned int* isrc = (unsigned int*)source;
 
     // word-by-word copy
-    if (!((unsigned int) dst & 3) && !((unsigned int) src & 3)) 
+    if (!((unsigned int) idst & 3) && !((unsigned int) isrc & 3)) 
     {
         while (size > 3) 
         {
-            *dst++ = *src++;
+            *idst++ = *isrc++;
             size -= 4;
         }
     }
 
-    unsigned char * cdst = (unsigned char *) dst;
-    unsigned char * csrc = (unsigned char *) src;
+    unsigned char* cdst = (unsigned char*)dest;
+    unsigned char* csrc = (unsigned char*)source;
 
     /* byte-by-byte copy */
@@ -73,14 +73,44 @@
 // Fill a byte string with a byte value.
 //////////////////////////////////////////////////////////////////////////////////
-inline void * _memset( void*        dst, 
+inline void * _memset( void*        dest, 
                        int          value, 
                        unsigned int count ) 
 {
-    char * a = (char *) dst;
+    // word-by-word copy
+    unsigned int* idst = dest;
+    unsigned int  data = (((unsigned char)value)      ) |
+                         (((unsigned char)value) <<  8) |
+                         (((unsigned char)value) << 16) |
+                         (((unsigned char)value) << 24) ;
+
+    if ( ! ((unsigned int)idst & 3) )
+    {
+        while ( count > 3 )
+        {
+            *idst++ = data;
+            count -= 4;
+        }
+    }
+   
+    // byte-by-byte copy
+    unsigned char* cdst = dest;
     while (count--) 
     {
-        *a++ = (char)value;
-    }
-    return dst;
+        *cdst++ = (unsigned char)value;
+    }
+    return dest;
+}
+
+//////////////////////////////////////////////////////////////////////////////////
+// This function implements an interactive break for debug.
+// Execution continue when typing any character on TTY0. 
+// The "str" argument is supposed to indicate the break location.
+//////////////////////////////////////////////////////////////////////////////////
+inline void _break( char* string ) 
+{
+    char byte;
+
+    _printf("\n[GIET DEBUG] break from %s / continue ?\n", string );
+    _getc( &byte );
 }
 
@@ -90,4 +120,13 @@
 inline void _exit() 
 {
+    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);
+
+
+    _printf("\n[GIET PANIC] processor[%d,%d,%d] suicide...\n", x, y, lpid );
+
     while (1) { asm volatile ("nop"); }
 }
@@ -103,5 +142,6 @@
 {
     unsigned int ret;
-    asm volatile("mfc0      %0,     $4, 2\n" : "=r"(ret) );
+    asm volatile( "mfc0      %0,     $4,2    \n" 
+                  : "=r"(ret) );
     return ret;
 }
@@ -112,8 +152,19 @@
 {
     unsigned int ret;
-    asm volatile("mfc2      %0,     $0" : "=r"(ret));
+    asm volatile( "mfc2      %0,     $0      \n"
+                  : "=r"(ret) );
     return ret;
 }
 ///////////////////////////////////////////////////////////////////////////////////
+// Returns MODE register content.
+///////////////////////////////////////////////////////////////////////////////////
+inline unsigned int _get_mmu_mode() 
+{
+    unsigned int ret;
+    asm volatile( "mfc2      %0,     $1      \n"
+                  : "=r"(ret) );
+    return ret;
+}
+///////////////////////////////////////////////////////////////////////////////////
 // Returns EPC register content.
 ///////////////////////////////////////////////////////////////////////////////////
@@ -121,5 +172,6 @@
 {
     unsigned int ret;
-    asm volatile("mfc0      %0,     $14" : "=r"(ret));
+    asm volatile( "mfc0      %0,    $14     \n"
+                  : "=r"(ret) );
     return ret;
 }
@@ -130,5 +182,6 @@
 {
     unsigned int ret;
-    asm volatile("mfc0      %0,     $8" : "=r"(ret));
+    asm volatile( "mfc0      %0,    $8     \n"
+                  : "=r"(ret));
     return ret;
 }
@@ -139,5 +192,6 @@
 {
     unsigned int ret;
-    asm volatile("mfc0      %0,     $13" : "=r"(ret));
+    asm volatile( "mfc0      %0,    $13    \n"
+                  : "=r"(ret));
     return ret;
 }
@@ -148,5 +202,6 @@
 {
     unsigned int ret;
-    asm volatile("mfc0      %0,     $12" : "=r"(ret));
+    asm volatile( "mfc0      %0,     $12   \n"
+                  : "=r"(ret));
     return ret;
 }
@@ -156,29 +211,33 @@
 inline void _set_sr(unsigned int val) 
 {
-    asm volatile("mtc0      %0,     $12" ::"r" (val));
-}
-//////////////////////////////////////////////////////////////////////////////////
+    asm volatile( "mtc0      %0,     $12    \n"
+                  :
+                  :"r" (val) );
+}
+//////////////////////////////////////////////////////////////////////////////
 // Returns processor index
-//////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////
 inline unsigned int _get_procid() 
 {
     unsigned int ret;
-    asm volatile ("mfc0     %0,     $15, 1":"=r" (ret));
+    asm volatile ( "mfc0     %0,     $15, 1  \n"
+                   :"=r" (ret) );
     return (ret & 0x3FF);
 }
-///////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////
 // Returns local time (32 bits value)
 // boot_proctime() 
-///////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////
 inline unsigned int _get_proctime() 
 {
     unsigned int ret;
-    asm volatile ("mfc0     %0,     $9":"=r" (ret));
+    asm volatile ( "mfc0     %0,     $9      \n"
+                   :"=r" (ret) );
     return ret;
 }
-///////////////////////////////////////////////////////////////////////////////////
-// Returns index of the currently running task from the sheduler.
-///////////////////////////////////////////////////////////////////////////////////
-unsigned int _get_proc_task_id() 
+//////////////////////////////////////////////////////////////////////////////
+// Returns index of the currently running task from the processor scheduler.
+//////////////////////////////////////////////////////////////////////////////
+unsigned int _get_current_task_id() 
 {
     static_scheduler_t * psched = (static_scheduler_t *) _get_sched();
@@ -186,27 +245,40 @@
 }
 
-///////////////////////////////////////////////////////////////////////////////////
-// Disables IRQs
-///////////////////////////////////////////////////////////////////////////////////
-inline void _it_disable() 
-{
-    asm volatile(
-            "li      $3,        0xFFFFFFFE    \n"
-            "mfc0    $4,        $12           \n"
-            "and     $3,        $3, $4        \n"
-            "mtc0    $3,        $12           \n"
-            ::: "$3", "$4");
-}
-///////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////
+// Save SR value into save_sr_ptr variable and disable IRQs. 
+//////////////////////////////////////////////////////////////////////////////
+inline void _it_disable( unsigned int * save_sr_ptr) 
+{
+    unsigned int sr;
+    asm volatile( "li      $3,        0xFFFFFFFE    \n"
+                  "mfc0    %0,        $12           \n"
+                  "and     $3,        $3,   %0      \n"  
+                  "mtc0    $3,        $12           \n" 
+                  : "=r"(sr)
+                  :
+                  : "$3" );
+    *save_sr_ptr = sr;
+}
+//////////////////////////////////////////////////////////////////////////////
 // Enables IRQs
-///////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////
 inline void _it_enable() 
 {
-    asm volatile(
-            "li      $3,        0x00000001    \n"
-            "mfc0    $4,        $12           \n"
-            "or      $3,        $3, $4        \n"
-            "mtc0    $3,        $12           \n"
-            ::: "$3", "$4");
+    asm volatile( "li      $3,        0x00000001    \n"
+                  "mfc0    $4,        $12           \n"
+                  "or      $3,        $3, $4        \n"
+                  "mtc0    $3,        $12           \n"  
+                  ::: "$3", "$4" );
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Restores previous SR value.
+//////////////////////////////////////////////////////////////////////////////
+inline void _it_restore( unsigned int * save_sr_ptr ) 
+{
+    unsigned int sr = *save_sr_ptr;
+    asm volatile( "mtc0    %0,        $12           \n" 
+                  :
+                  : "r"(sr) );
 }
 
@@ -216,5 +288,7 @@
 inline void _set_mmu_ptpr(unsigned int val) 
 {
-    asm volatile ("mtc2     %0,     $0"::"r" (val));
+    asm volatile ( "mtc2     %0,     $0            \n"
+                   :
+                   :"r" (val) );
 }
 //////////////////////////////////////////////////////////////////////////////
@@ -223,5 +297,7 @@
 inline void _set_mmu_mode(unsigned int val) 
 {
-    asm volatile ("mtc2     %0,     $1"::"r" (val));
+    asm volatile ( "mtc2     %0,     $1             \n"
+                   :
+                   :"r" (val) );
 }
 //////////////////////////////////////////////////////////////////////////////
@@ -231,5 +307,7 @@
 inline void _set_sched(unsigned int val) 
 {
-    asm volatile ("mtc0     %0,     $4, 2"::"r" (val));
+    asm volatile ( "mtc0     %0,     $4, 2          \n"
+                   :
+                   :"r" (val) );
 }
 
@@ -249,4 +327,9 @@
 
     asm volatile(
+            "li     $3,     0xFFFFFFFE         \n"
+            "mfc0   $2,     $12                \n"
+            "and    $3,     $2,        $3      \n"
+            "mtc0   $3,     $12                \n"     /* IRQ disabled     */
+
             "mfc2   $2,     $1                 \n"     /* $2 <= MMU_MODE   */
             "andi   $3,     $2,        0xb     \n"
@@ -258,4 +341,9 @@
 
             "mtc2   $2,     $1                 \n"     /* restore MMU_MODE */
+
+            "li     $3,     0x00000001         \n"
+            "mfc0   $2,     $12                \n"
+            "or     $3,     $3,        $2      \n"
+            "mtc0   $3,     $12                \n"     /* IRQ enabled      */
             : "=r" (value)
             : "r" (lsb), "r" (msb)
@@ -268,5 +356,5 @@
 ////////////////////////////////////////////////////////////////////////////
 inline void _physical_write( unsigned long long paddr, 
-                      unsigned int       value ) 
+                             unsigned int       value ) 
 {
     unsigned int lsb = (unsigned int)paddr;
@@ -274,4 +362,9 @@
 
     asm volatile(
+            "li     $3,     0xFFFFFFFE         \n"
+            "mfc0   $2,     $12                \n"
+            "and    $3,     $2,        $3      \n"
+            "mtc0   $3,     $12                \n"     /* IRQ disabled     */
+
             "mfc2   $2,     $1                 \n"     /* $2 <= MMU_MODE   */
             "andi   $3,     $2,        0xb     \n"
@@ -283,7 +376,56 @@
 
             "mtc2   $2,     $1                 \n"     /* restore MMU_MODE */
+
+            "li     $3,     0x00000001         \n"
+            "mfc0   $2,     $12                \n"
+            "or     $3,     $3,        $2      \n"
+            "mtc0   $3,     $12                \n"     /* IRQ enabled      */
             :
             : "r" (value), "r" (lsb), "r" (msb)
             : "$2", "$3");
+}
+
+///////////////////////////////////////////////////////////////////////////////////
+// This function is used by all 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.
+///////////////////////////////////////////////////////////////////////////////////
+inline void _io_extended_write( unsigned int*  vaddr,
+                                unsigned int   value )
+{
+    unsigned long long paddr;
+
+    if ( _get_mmu_mode() & 0x4 )  // MMU activated : use virtual address
+    {
+        *vaddr = value;
+    }
+    else                          // use paddr extension for IO
+    {
+        paddr = (unsigned long long)(unsigned int)vaddr +
+                (((unsigned long long)((X_IO<<Y_WIDTH) + Y_IO))<<32); 
+        _physical_write( paddr, value );
+    }
+    asm volatile("sync" ::: "memory");
+}
+
+///////////////////////////////////////////////////////////////////////////////////
+// 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.
+///////////////////////////////////////////////////////////////////////////////////
+inline unsigned int _io_extended_read( unsigned int*  vaddr )
+{
+    unsigned long long paddr;
+
+    if ( _get_mmu_mode() & 0x4 )  // MMU activated : use virtual address
+    {
+        return *(volatile unsigned int*)vaddr;
+    }
+    else                          // use paddr extension for IO
+    {
+        paddr = (unsigned long long)(unsigned int)vaddr +
+                (((unsigned long long)((X_IO<<Y_WIDTH) + Y_IO))<<32); 
+        return _physical_read( paddr );
+    }
 }
 
@@ -323,4 +465,5 @@
             :"$2", "$3", "$4");
 }
+
 ///////////////////////////////////////////////////////////////////////////////////
 // Release a previouly taken lock.
@@ -328,30 +471,38 @@
 inline void _release_lock(unsigned int * plock) 
 {
-    asm volatile ( "sync\n" ); // necessary because of the TSAR consistency model 
+    asm volatile ( "sync\n" ::: "memory" ); 
+    // sync is necessary because of the TSAR consistency model 
     *plock = 0;
 }
 
 ///////////////////////////////////////////////////////////////////////////////////
-// Display a string on TTY0 / used for system code debug and log.
-// It does not use the TTY driver, but uses the seg_tty_base variable...
-///////////////////////////////////////////////////////////////////////////////////
-void _puts(char * buffer) 
-{
-    unsigned int n;
-    for (n = 0; n < 1000; n++) 
-    {
-        if (buffer[n] == 0)  break; 
-    }
-    _tty_write( buffer, n, 0 );   // last argument is TTY channel
-}
-
-///////////////////////////////////////////////////////////////////////////////////
 //           Access functions to system terminal TTY0
 ///////////////////////////////////////////////////////////////////////////////////
 
 ///////////////////////////////////////////////////////////////////////////////////
-// Display a 32 bits unsigned int as an hexadecimal string on TTY0 
-///////////////////////////////////////////////////////////////////////////////////
-void _putx(unsigned int val) 
+// Display "string" argument on TTY0.
+// It uses the low level access functions from TTY driver, using a busy waiting
+// policy if TTY buffer is full.
+// The exclusive access lock should be taken by the caller.
+///////////////////////////////////////////////////////////////////////////////////
+void _puts( char* string ) 
+{
+    unsigned int n = 0;
+
+    while ( string[n] > 0 )
+    {
+        // test status register
+        while ( (_tty_get_register( 0, TTY_STATUS ) & 0x2) );
+
+        // write one byte
+        _tty_set_register( 0, TTY_WRITE, (unsigned int)string[n] );
+        n++;
+    }
+}
+
+///////////////////////////////////////////////////////////////////////////////////
+// Display a 32 bits unsigned int as an hexadecimal string on TTY0.
+///////////////////////////////////////////////////////////////////////////////////
+void _putx( unsigned int val )
 {
     static const char HexaTab[] = "0123456789ABCDEF";
@@ -368,11 +519,11 @@
         val = val >> 4;
     }
-    _puts(buf);
-}
-
-///////////////////////////////////////////////////////////////////////////////////
-// Display a 64 bits unsigned long as an hexadecimal string on TTY0 
-///////////////////////////////////////////////////////////////////////////////////
-void _putl(unsigned long long val)
+    _puts( buf );
+}
+
+///////////////////////////////////////////////////////////////////////////////////
+// Display a 64 bits unsigned long as an hexadecimal string on TTY0.
+///////////////////////////////////////////////////////////////////////////////////
+void _putl( unsigned long long val )
 {
     static const char HexaTab[] = "0123456789ABCDEF";
@@ -389,11 +540,11 @@
         val = val >> 4;
     }
-    _puts(buf);
-}
-
-///////////////////////////////////////////////////////////////////////////////////
-// Display a 32 bits unsigned int as a decimal string on TTY0
-///////////////////////////////////////////////////////////////////////////////////
-void _putd(unsigned int val) 
+    _puts( buf );
+}
+
+///////////////////////////////////////////////////////////////////////////////////
+// Display a 32 bits unsigned int as a decimal string on TTY0.
+///////////////////////////////////////////////////////////////////////////////////
+void _putd( unsigned int val ) 
 {
     static const char DecTab[] = "0123456789";
@@ -414,5 +565,180 @@
         val /= 10;
     }
-    _puts(&buf[first]);
+    _puts( &buf[first] );
+}
+
+///////////////////////////////////////////////////////////////////////////////////
+// Display a format on TTY0.
+// To provide an atomic display, this function takes the lock protecting
+// exclusive access to TTY0, entering a critical section until the lock
+// is released.
+// Only a limited number of formats are supported:
+//   - %d : 32 bits signed   decimal
+//   - %u : 32 bits unsigned decimal
+//   - %x : 32 bits unsigned hexa
+//   - %l : 64 bits unsigned hexa
+//   - %c : char
+//   - %s : string
+///////////////////////////////////////////////////////////////////////////////////
+void _printf( char * format, ... ) 
+{
+    va_list ap;
+    va_start(ap, format);
+    unsigned int save_sr;     // used to save the SR value in critical section
+
+    // get TTY0 lock
+    _tty_get_lock( 0, &save_sr );
+
+printf_text:
+
+    while (*format) 
+    {
+        unsigned int i;
+        for (i = 0 ; format[i] && (format[i] != '%') ; i++);
+        if (i) 
+        {
+            if ( _tty_write( format, i, 0 ) != i ) goto return_error;
+            format += i;
+        }
+        if (*format == '%') 
+        {
+            format++;
+            goto printf_arguments;
+        }
+    }
+
+    // release TTY0 lock
+    _tty_release_lock( 0, &save_sr );
+
+    va_end(ap);
+    return;
+
+printf_arguments:
+
+    {
+        char buf[20];
+        char * pbuf;
+        unsigned int len = 0;
+        static const char HexaTab[] = "0123456789ABCDEF";
+        unsigned int i;
+
+        switch (*format++) 
+        {
+            case ('c'):             /* char conversion */
+            {
+                int val = va_arg( ap, int );
+                len = 1;
+                buf[0] = val;
+                pbuf = &buf[0];
+                break;
+            }
+            case ('d'):             /* 32 bits decimal signed  */
+            {
+                int val = va_arg( ap, int );
+                if (val < 0) 
+                {
+                    val = -val;
+                    if ( _tty_write( "-" , 1, 0 ) != 1 ) goto return_error;
+                }
+                for(i = 0; i < 10; i++) 
+                {
+                    buf[9 - i] = HexaTab[val % 10];
+                    if (!(val /= 10)) break;
+                }
+                len =  i + 1;
+                pbuf = &buf[9 - i];
+                break;
+            }
+            case ('u'):             /* 32 bits decimal unsigned  */
+            {
+                unsigned int val = va_arg( ap, unsigned int );
+                for(i = 0; i < 10; i++) 
+                {
+                    buf[9 - i] = HexaTab[val % 10];
+                    if (!(val /= 10)) break;
+                }
+                len =  i + 1;
+                pbuf = &buf[9 - i];
+                break;
+            }
+            case ('x'):             /* 32 bits hexadecimal unsigned */
+            {
+                unsigned int val = va_arg( ap, unsigned int );
+                if ( _tty_write( "0x" , 2, 0 ) != 2 ) goto return_error;
+                for(i = 0; i < 8; i++) 
+                {
+                    buf[7 - i] = HexaTab[val % 16];
+                    if (!(val /= 16))  break;
+                }
+                len =  i + 1;
+                pbuf = &buf[7 - i];
+                break;
+            }
+            case ('l'):            /* 64 bits hexadecimal unsigned */
+            {
+                unsigned long long val = va_arg( ap, unsigned long long );
+                if ( _tty_write( "0x" , 2, 0 ) != 2 ) goto return_error;
+                for(i = 0; i < 16; i++) 
+                {
+                    buf[15 - i] = HexaTab[val % 16];
+                    if (!(val /= 16))  break;
+                }
+                len =  i + 1;
+                pbuf = &buf[15 - i];
+                break;
+            }
+            case ('s'):             /* string */
+            {
+                char* str = va_arg( ap, char* );
+                while (str[len]) 
+                {
+                    len++;
+                }
+                pbuf = str;
+                break;
+            }
+            default:
+                goto return_error;
+        }
+
+        if ( _tty_write( pbuf, len, 0 ) != len ) goto return_error;
+        
+        goto printf_text;
+    }
+
+return_error:
+
+    {
+        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);
+
+        _puts("\n\n[GIET ERROR] in _printf() for processor[");
+        _putd( x );
+        _puts(",");
+        _putd( y );
+        _puts(",");
+        _putd( lpid );
+        _puts("]\n");
+
+        // release TTY0 lock
+        _tty_release_lock( 0, &save_sr );
+
+        _exit();
+    }
+}
+
+///////////////////////////////////////////////////////////////////////////////////
+// Get a character from TTY0.
+///////////////////////////////////////////////////////////////////////////////////
+void _getc( char*        byte )
+{
+    // test status register
+    while ( _tty_get_register( 0, TTY_STATUS ) == 0 );
+
+    // read one byte
+    *byte = (char)_tty_get_register( 0, TTY_READ );
 }
 
@@ -452,5 +778,5 @@
 // register, to be more processor independant.
 ///////////////////////////////////////////////////////////////////////////////////
-void _dcache_buf_invalidate( const void * buffer, 
+void _dcache_buf_invalidate( void * buffer, 
                              unsigned int size) 
 {
