Index: soft/giet_vm/sys/common.c
===================================================================
--- soft/giet_vm/sys/common.c	(revision 166)
+++ soft/giet_vm/sys/common.c	(revision 189)
@@ -11,13 +11,116 @@
 #include <sys_handler.h>
 #include <common.h>
+#include <ctx_handler.h>
 #include <drivers.h>
 #include <stdarg.h>
 
-////////////////////////////////////////////////////////////////////////////
-// _get_lock()
+///////////////////////////////////////////////////////////////////////////////////
+//	Global variables
+///////////////////////////////////////////////////////////////////////////////////
+
+// current context cache TODO
+
+// SR save (used by _it_mask() / it_restore()
+unsigned int	_status_register_save;
+
+///////////////////////////////////////////////////////////////////////////////////
+// 	  _get_sched()
+// Access CP0 and returns scheduler physical address.
+///////////////////////////////////////////////////////////////////////////////////
+inline unsigned int _get_sched()
+{
+    unsigned int ret;
+    asm volatile (	"mfc0	%0,		$22" 
+					: "=r"(ret) );
+    return ret;
+}
+///////////////////////////////////////////////////////////////////////////////////
+// 	  _get_ptpr()
+// Access CP2 and returns PTPR register.
+///////////////////////////////////////////////////////////////////////////////////
+inline unsigned int _get_ptpr()
+{
+    unsigned int ret;
+    asm volatile (	"mfc2	%0,		$0" 
+					: "=r"(ret) );
+    return ret;
+}
+///////////////////////////////////////////////////////////////////////////////////
+// 	  _get_epc()
+// Access CP0 and returns EPC register.
+///////////////////////////////////////////////////////////////////////////////////
+inline unsigned int _get_epc()
+{
+    unsigned int ret;
+    asm volatile (	"mfc0	%0,		$14" 
+					: "=r"(ret) );
+    return ret;
+}
+///////////////////////////////////////////////////////////////////////////////////
+// 	  _get_bar()
+// Access CP0 and returns BAR register.
+///////////////////////////////////////////////////////////////////////////////////
+inline unsigned int _get_bvar()
+{
+    unsigned int ret;
+    asm volatile (	"mfc0	%0,		$8" 
+					: "=r"(ret) );
+    return ret;
+}
+///////////////////////////////////////////////////////////////////////////////////
+// 	  _get_cr()
+// Access CP0 and returns CR register.
+///////////////////////////////////////////////////////////////////////////////////
+inline unsigned int _get_cause()
+{
+    unsigned int ret;
+    asm volatile (	"mfc0	%0,		$13" 
+					: "=r"(ret) );
+    return ret;
+}
+///////////////////////////////////////////////////////////////////////////////////
+// 	  _get_sr()
+// Access CP0 and returns SR register.
+///////////////////////////////////////////////////////////////////////////////////
+inline unsigned int _get_sr()
+{
+    unsigned int ret;
+    asm volatile (	"mfc0	%0,		$12" 
+					: "=r"(ret) );
+    return ret;
+}
+///////////////////////////////////////////////////////////////////////////////////
+//    _it_mask()
+// Access CP0 and mask IRQs
+///////////////////////////////////////////////////////////////////////////////////
+inline void _it_mask()
+{
+    unsigned int	sr_value;
+    asm volatile(	"li		$3,		0xFFFFFFFE	\n"
+					"mfc0	%0,		$12			\n"
+					"and	$3,		$3, %0		\n"
+					"mtc0	$3,		$12			\n"
+					: "=r"(sr_value) : : "$3" );
+    _status_register_save = sr_value;
+}
+///////////////////////////////////////////////////////////////////////////////////
+//    _it_enable()
+// Access CP0 and enable IRQs
+///////////////////////////////////////////////////////////////////////////////////
+inline void _it_restore()
+{
+    unsigned int	sr_value = _status_register_save;
+    asm volatile(	"mtc0  %0,		$12			\n"
+					: : "r"(sr_value) );
+}
+////////////////////////////////////////////////////////////////////////////
+//    _get_lock()
+// Takes a lock with an ll/sc atomic access.
+// A pseudo random delay is introduced before retry in case of miss
+// (delay average value = 100 cycles)
 ////////////////////////////////////////////////////////////////////////////
 inline void _get_lock( unsigned int* plock )
 {
-    register unsigned int  delay = (_proctime() & 0xF) << 4;
+    register unsigned int  delay = ( _proctime() ^ _procid()<<4 ) & 0xFF;
 
     asm volatile (
@@ -49,6 +152,6 @@
 
 ////////////////////////////////////////////////////////////////////////////
-// _puts()
-// used for system code debug / it uses TTY0 
+//    _puts()
+// display a string on TTY0 / used for system code debugand log
 ////////////////////////////////////////////////////////////////////////////
 void _puts(char *buffer) 
@@ -64,6 +167,6 @@
 }
 ////////////////////////////////////////////////////////////////////////////
-// _putw() 
-// used for system code debug / it uses TTY0
+//    _putw() 
+// display an int (hexa) on TTY0 / used for system code debug and log
 ////////////////////////////////////////////////////////////////////////////
 void _putw(unsigned int val)
@@ -85,5 +188,33 @@
 }
 ////////////////////////////////////////////////////////////////////////////
-// _strncmp()
+//    _putd() 
+// display an int (decimal) on TTY0 / used for system code debug and log
+////////////////////////////////////////////////////////////////////////////
+void _putd(unsigned int val)
+{
+    static const char   DecTab[] = "0123456789";
+    char				buf[11];
+    unsigned int		i;
+    unsigned int		first;
+
+    buf[10] = 0;
+
+    for (i = 0; i < 10; i++)
+    {
+        if ((val != 0) || (i == 0))
+        {
+            buf[9-i] = DecTab[val % 10];
+            first    = 9-i;
+        }
+        else
+        {
+            break;
+        }
+        val /= 10;
+    }
+    _puts( &buf[first] );
+}
+////////////////////////////////////////////////////////////////////////////
+//    _strncmp()
 // compare two strings s1 & s2 (no more than n characters)
 ////////////////////////////////////////////////////////////////////////////
@@ -101,5 +232,5 @@
 }
 ////////////////////////////////////////////////////////////////////////////
-// 	_dcache_buf_invalidate()
+// 	   _dcache_buf_invalidate()
 // Invalidate all data cache lines corresponding to a memory 
 // buffer (identified by an address and a size).
@@ -126,105 +257,98 @@
     }
 }
-///////////////////////////////////////////////////////////////////////////////////
-// 	_itoa_dec()
-// Convert a 32-bit unsigned integer to a string of ten decimal characters.
-///////////////////////////////////////////////////////////////////////////////////
-void _itoa_dec(unsigned int val, char *buf)
-{
-    const static char dectab[] = "0123456789";
-    unsigned int i;
-
-    for (i = 0; i < 10; i++)
-    {
-        if ((val != 0) || (i == 0))
-            buf[9-i] = dectab[val % 10];
-        else
-            buf[9-i] = 0x20;
-        val /= 10;
-    }
-}
-///////////////////////////////////////////////////////////////////////////////////
-// 	_itoa_hex()
-// Convert a 32-bit unsigned integer to a string of eight hexadecimal characters.
-///////////////////////////////////////////////////////////////////////////////////
-void _itoa_hex(unsigned int val, char *buf)
-{
-    const static char hexatab[] = "0123456789ABCD";
-    unsigned int i;
-
-    for (i = 0; i < 8; i++)
-    {
-        buf[7-i] = hexatab[val % 16];
-        val /= 16;
-    }
-}
-///////////////////////////////////////////////////////////////////////////////////
-// 	_get_ptpr()
-// Access CP2 and returns PTPR register.
-///////////////////////////////////////////////////////////////////////////////////
-inline unsigned int _get_ptpr()
-{
-    unsigned int ret;
-    asm volatile("mfc2 %0, $0" : "=r"(ret));
-    return ret;
-}
-///////////////////////////////////////////////////////////////////////////////////
-// 	_get_epc()
-// Access CP0 and returns EPC register.
-///////////////////////////////////////////////////////////////////////////////////
-inline unsigned int _get_epc()
-{
-    unsigned int ret;
-    asm volatile("mfc0 %0, $14" : "=r"(ret));
-    return ret;
-}
-///////////////////////////////////////////////////////////////////////////////////
-// 	_get_bar()
-// Access CP0 and returns BAR register.
-///////////////////////////////////////////////////////////////////////////////////
-inline unsigned int _get_bar()
-{
-    unsigned int ret;
-    asm volatile("mfc0 %0, $8" : "=r"(ret));
-    return ret;
-}
-///////////////////////////////////////////////////////////////////////////////////
-// 	_get_cr()
-// Access CP0 and returns CR register.
-///////////////////////////////////////////////////////////////////////////////////
-inline unsigned int _get_cause()
-{
-    unsigned int ret;
-    asm volatile("mfc0 %0, $13" : "=r"(ret));
-    return ret;
-}
-
-///////////////////////////////////////////////////////////////////////////////////
-//  	_it_mask()
-// Access CP0 and mask IRQs
-///////////////////////////////////////////////////////////////////////////////////
-inline void _it_mask()
-{
-    asm volatile(
-            "lui   $27,      0xFFFF   \n"
-            "ori   $27, $27, 0xFFFE   \n"
-            "mfc0  $26, $12           \n"
-            "and   $26, $26, $27      \n"
-            "mtc0  $26, $12           \n"
-            ::: "$26", "$27"
-            );
-}
-///////////////////////////////////////////////////////////////////////////////////
-//  	_it_enable()
-// Access CP0 and enable IRQs
-///////////////////////////////////////////////////////////////////////////////////
-inline void _it_enable()
-{
-    asm volatile(
-            "mfc0  $26, $12      \n"
-            "ori   $26, $26, 1   \n"
-            "mtc0  $26, $12      \n"
-            ::: "$26"
-            );
+////////////////////////////////////////////////////////////////////////////
+//    _physical_read_access()
+// This function makes a physical read access to a 32 bits word in memory, 
+// after a temporary DTLB desactivation. 
+////////////////////////////////////////////////////////////////////////////
+unsigned int _physical_read_access(unsigned int* paddr)
+{
+    unsigned int value;
+
+    asm volatile(   "li     $3,     0xFFFFFFFE		\n"
+                    "mfc0   $2,     $12				\n"		/* $2 <= SR        */
+                    "and    $3,     $3,		$2		\n"
+                    "mtc0   $3,     $12				\n"		/* interrupt masked */
+                    "li		$3,		0xB				\n"
+                    "mtc2	$3,		$1				\n"		/* DTLB off			*/	
+
+                    "lw		%0,		0(%1)			\n"		/* entry <= *pslot	*/
+
+                    "li		$3,		0xF				\n"
+                    "mtc2	$3,		$1				\n"		/* DTLB on 			*/	
+                    "mtc0	$2,		$12				\n"		/* restore SR		*/
+                    : "=r"(value)
+                    : "r"(paddr)
+                    : "$2", "$3" );
+    return value;
+}
+////////////////////////////////////////////////////////////////////////////
+//    _physical_write_access()
+// This function makes a physical write access to a 32 bits word in memory, 
+// after a temporary DTLB desactivation. 
+////////////////////////////////////////////////////////////////////////////
+void _physical_write_access(unsigned int* paddr, unsigned int value)
+{
+    asm volatile(   "li     $3,     0xFFFFFFFE		\n"
+                    "mfc0   $2,     $12				\n"		/* $26 <= SR        */
+                    "and    $3,     $3,		$2		\n"
+                    "mtc0   $3,     $12				\n"		/* interrupt masked */
+                    "li		$3,		0xB				\n"
+                    "mtc2	$3,		$1				\n"		/* DTLB off			*/
+	
+                    "sw		%0,		0(%1)			\n"		/* entry <= *pslot	*/
+
+                    "li		$3,		0xF				\n"
+                    "mtc2	$3,		$1				\n"		/* DTLB on 			*/	
+                    "mtc0	$2,		$12				\n"		/* restore SR		*/
+                    :
+                    : "r"(value), "r"(paddr)
+                    : "$2", "$3" );
+}
+////////////////////////////////////////////////////////////////////////////
+//    _get_tasks_number()
+// This function returns the number of tasks allocated to processor.
+////////////////////////////////////////////////////////////////////////////
+unsigned int _get_tasks_number()
+{
+    static_scheduler_t*		psched = (static_scheduler_t*)_get_sched();
+    return _physical_read_access( &(psched->tasks) );
+}
+////////////////////////////////////////////////////////////////////////////
+//    _get_current_task_id()
+// This function returns the index of the currently running task.
+////////////////////////////////////////////////////////////////////////////
+unsigned int _get_current_task_id()
+{
+    static_scheduler_t*		psched = (static_scheduler_t*)_get_sched();
+    return _physical_read_access( &(psched->current) );
+}
+///////////////////////////////////////////////////////////////////////////////
+//    _get_current_context_slot()
+// This function returns the global TTY index for the currently running task.
+///////////////////////////////////////////////////////////////////////////////
+unsigned int _get_current_context_slot(unsigned int slot_id)
+{
+    static_scheduler_t*		psched = (static_scheduler_t*)_get_sched();
+    unsigned int			current = _physical_read_access( &(psched->current) );
+    return _physical_read_access( &(psched->context[current][slot_id]) );
+}
+/////////////////////////////////////////////i//////////////////////////////////
+//    _get_interrupt_vector_entry()
+// This function returns the interrupt_vector entry defined by argument index.
+////////////////////////////////////////////////////////////////////////////////
+unsigned int _get_interrupt_vector_entry( unsigned int index )
+{
+    static_scheduler_t*		psched = (static_scheduler_t*)_get_sched();
+    return _physical_read_access( &(psched->interrupt_vector[index]) );
+}
+////////////////////////////////////////////////////////////////////////////
+//    _set_current_task_id()
+// This function returns the index of the currently running task.
+////////////////////////////////////////////////////////////////////////////
+void _set_current_task_id( unsigned int value )
+{
+    static_scheduler_t*		psched = (static_scheduler_t*)_get_sched();
+    _physical_write_access( &(psched->current), value );
 }
 
@@ -283,3 +407,2 @@
 }
 
-
