Index: trunk/tools/bootloader_tsar/boot_utils.c
===================================================================
--- trunk/tools/bootloader_tsar/boot_utils.c	(revision 1)
+++ trunk/tools/bootloader_tsar/boot_utils.c	(revision 6)
@@ -1,2 +1,25 @@
+/*
+ * boot_utils.c - TSAR bootloader utilities 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
+ */
+
 #include <stdarg.h>
 
@@ -5,20 +28,27 @@
 #include <boot_utils.h>
 
+
+/****************************************************************************
+ *                        Global variables                                  *
+ ****************************************************************************/
+
+extern boot_remote_spinlock_t  tty0_lock;   // allocated in boot.c
+
 /****************************************************************************
  *                              Remote accesses.                            *
  ****************************************************************************/
 
+//////////////////////////////////
 uint32_t boot_remote_lw(xptr_t xp)
 {
-    uint32_t res;   /* Value to be read, stored at the remote address.      */
-    uint32_t ptr;   /* Classic pointer to the distant memory location.      */
-    uint32_t cxy;   /* Identifier of the cluster containing the distant
-                       memory location.                                     */
-
-    /* Extracting information from the extended pointer. */
+    uint32_t res;  
+    uint32_t ptr; 
+    uint32_t cxy;
+
+    // Extracting information from the extended pointer
     ptr = (uint32_t)GET_PTR(xp);
     cxy = (uint32_t)GET_CXY(xp);
 
-    /* Assembly instructions to get the work done. */
+    // Assembly instructions to get the work done. 
     asm volatile("mfc2  $15,    $24\n"  /* $15 <= CP2_DATA_PADDR_EXT        */
                  "mtc2  %2,     $24\n"  /* CP2_DATA_PADDR_EXT <= cxy        */
@@ -37,6 +67,5 @@
 } // boot_remote_lw()
 
-/****************************************************************************/
-
+/////////////////////////////////////////////
 void boot_remote_sw(xptr_t xp, uint32_t data)
 {
@@ -62,6 +91,5 @@
 } // boot_remote_sw()
 
-/****************************************************************************/
-
+//////////////////////////////////////////////////////
 int32_t boot_remote_atomic_add(xptr_t xp, int32_t val)
 {
@@ -98,7 +126,6 @@
 } // boot_remote_atomic_add()
 
-/****************************************************************************/
-
-void boot_remote_memcpy(xptr_t dest, xptr_t src, unsigned int size)
+///////////////////////////////////////////////////////////////
+void boot_remote_memcpy(xptr_t dest, xptr_t src, uint32_t size)
 {
     uint32_t words_nr;  /* Number of 32-bit words to be copied.             */
@@ -194,60 +221,58 @@
  ****************************************************************************/
 
-void boot_memcpy(void* dest, void* src, unsigned int size)
-{
-    /* Word-by-word copy if both addresses are word-aligned. */
-    if ((((unsigned int)dest & 0x3) == 0) && 
-        (((unsigned int)src & 0x3)  == 0))
-    {
-        // 'size' might not be a multiple of 4 bytes, we have to copy a few 
-        // bytes left (at most 3) byte-by-byte later.
+///////////////////////////////////////////////////////
+void boot_memcpy(void * dst, void * src, uint32_t size)
+{
+    uint32_t * wdst = dst;
+    uint32_t * wsrc = src;
+
+    // word-by-word copy if both addresses are word-aligned
+    if ( (((uint32_t)dst & 0x3) == 0) && (((uint32_t)src & 0x3)  == 0) )
+    {
         while (size > 3)
         {
-            *(unsigned int*)dest++ = *(unsigned int*)src++;
+            *wdst++ = *wsrc++;
             size -= 4;
         }
     }
 
-    /*
-     * Byte-by-byte copy if:
-     * - At least 1 of the 2 addresses is not word-aligned,
-     * - 'size' value is not a multiple of 4 bytes.
-     */
+    unsigned char * cdst = (unsigned char *)wdst;
+    unsigned char * csrc = (unsigned char *)wsrc;
+
+    // byte-by-byte copy if:
+    // - At least 1 of the 2 addresses is not word-aligned,
+    // - 'size' value is not a multiple of 4 bytes.
     while (size)
-        *(unsigned char*)dest++ = *(unsigned char*)src++;
-
+    {
+        *cdst++ = *csrc++;
+    }
 } // boot_memcpy()
 
-/****************************************************************************/
-
-void boot_memset(void* base, int val, unsigned int size)
-{
-    unsigned int wval;      /* Word-sized value to word-by-word filling.        */
-
-    /* Word-by-word filling if the base address is word-aligned. */
-    // Extracting the first 2 bytes of 'val'.
+////////////////////////////////////////////////////
+void boot_memset(void * dst, int val, uint32_t size)
+{
     val &= 0xFF;
-    // Making it word-sized. 
-    wval = (val << 24) | (val << 16) | (val << 8) | val;
-
-    if (((unsigned int)base & 0x3) == 0)
-    {
-        // 'size' might not be a multiple of 4 bytes, we have to fill a 
-        // few bytes left (at most 3) byte-by-byte later.
+
+    // build a word-sized value
+    uint32_t   wval = (val << 24) | (val << 16) | (val << 8) | val;
+    uint32_t * wdst = (uint32_t *)dst; 
+
+    // word per word if address aligned
+    if (((uint32_t)dst & 0x3) == 0)
+    {
         while (size > 3)
         {
-            *(unsigned int*)base++ = wval;
+            *wdst++ = wval;
             size -= 4;
         }
     }
     
-    /*
-     * Byte-by-byte filling if:
-     * - The base address is not word-aligned,
-     * - 'size' value is not a multiple of 4 bytes.
-     */
+    char * cdst = (char *)wdst;
+
+    // byte per byte
     while (size--)
-        *(unsigned char*)base++ = val;
-
+    {
+        *cdst++ = (char)val;
+    }
 } // boot_memset()
 
@@ -256,4 +281,5 @@
  ****************************************************************************/
 
+///////////////////////////////////////
 void boot_strcpy(char* dest, char* src)
 {
@@ -267,9 +293,8 @@
 } // boot_strcpy()
 
-/****************************************************************************/
-
-unsigned int boot_strlen(char* s)
-{
-    unsigned int res = 0;   /* Length of the string (in bytes).             */
+/////////////////////////////
+uint32_t boot_strlen(char* s)
+{
+    uint32_t res = 0;   /* Length of the string (in bytes).             */
 
     if (s != NULL)
@@ -283,6 +308,5 @@
 } // boot_strlen()
 
-/****************************************************************************/
-
+///////////////////////////////////
 int boot_strcmp(char* s1, char* s2)
 {
@@ -308,4 +332,5 @@
  ****************************************************************************/
 
+/////////////////////////
 void boot_puts(char* str)
 {
@@ -314,221 +339,163 @@
 } // boot_puts()
     
-/****************************************************************************/
-
-void boot_printf(char* format, ...)
-{
-    va_list      arg;           /* Used to iterate arguments list.          */
-    char         buf[16];       /* Buffer for argument conversion.          */      
-    char*        print_pt;      /* String pointer for argument printing.    */
-    int          arg_val;       /* Raw value of the argument.               */
-    unsigned int arg_len;       /* Length of a argument (in bytes).         */
-    unsigned int nb_printed;    /* Iterator for text printing loop.         */
-    unsigned int conv_index;    /* Index for argument conversion loop.      */
-
-    const char conv_tab[] = "0123456789ABCDEF";
-
-    /* Starting the arguments iterating process with a va_list. */
-    va_start(arg, format);
-
-print_text:
-    
-    while (*format)
-    {
-        /* Counting the number of ordinary characters. */
-        for (nb_printed = 0; 
-             (format[nb_printed] != '\0') && (format[nb_printed] != '%');
-             nb_printed++);
-
-        /* Copying them unchanged to the boot TTY terminal. */
-        if (nb_printed > 0)
+///////////////////////////////////////
+void boot_printf( char * format , ... )
+{
+    va_list args;
+    va_start( args , format );
+
+    // take the lock protecting TTY0
+    boot_remote_lock( XPTR( 0 , &tty0_lock ) );
+
+printf_text:
+
+    while ( *format ) 
+    {
+        uint32_t i;
+        for (i = 0 ; format[i] && (format[i] != '%') ; i++);
+        if (i) 
         {
-            if (boot_tty_write(format, nb_printed))
-                goto error;
-            format += nb_printed;
+            boot_tty_write( format , i );
+            format += i;
         }
-
-        /* Skipping the '%' character. */
-        if (*format == '%')
+        if (*format == '%') 
         {
             format++;
-            goto print_argument;
+            goto printf_arguments;
         }
     }
 
-    /* Freeing the va_list. */
-    va_end(arg);
-
+    // release the lock
+    boot_remote_unlock( XPTR( 0 , &tty0_lock ) );
+
+    va_end( args );
     return;
 
-print_argument:
-
-    /* Analyzing the conversion specifier. */
-    switch (*format++)
-    {
-        // A character.
-        case ('c'):
+printf_arguments:
+
+    {
+        char      buf[20];
+        char    * pbuf = NULL;
+        uint32_t  len  = 0;
+        static const char HexaTab[] = "0123456789ABCDEF";
+        uint32_t i;
+
+        switch (*format++) 
         {
-            // Retrieving the argument.
-            arg_val  = va_arg(arg, int);
-
-            // Preparing for the printing. 
-            arg_len  = 1;
-            buf[0]   = arg_val;
-            print_pt = &buf[0];
-            break;
+            case ('c'):             /* char conversion */
+            {
+                int val = va_arg( args , int );
+                len = 1;
+                buf[0] = val;
+                pbuf = &buf[0];
+                break;
+            }
+            case ('d'):             /* 32 bits decimal signed  */
+            {
+                int val = va_arg( args , int );
+                if (val < 0) 
+                {
+                    val = -val;
+                    boot_tty_write( "-" , 1 );
+                }
+                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  */
+            {
+                uint32_t val = va_arg( args , uint32_t );
+                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 */
+            {
+                uint32_t val = va_arg( args , uint32_t );
+                boot_tty_write( "0x" , 2 );
+                for(i = 0; i < 8; i++) 
+                {
+                    buf[7 - i] = HexaTab[val & 0xF];
+                    if (!(val = (val>>4)))  break;
+                }
+                len =  i + 1;
+                pbuf = &buf[7 - i];
+                break;
+            }
+            case ('X'):             /* 32 bits hexadecimal unsigned  on 10 char */
+            {
+                uint32_t val = va_arg( args , uint32_t );
+                boot_tty_write( "0x" , 2 );
+                for(i = 0; i < 8; i++) 
+                {
+                    buf[7 - i] = HexaTab[val & 0xF];
+                    val = (val>>4);
+                }
+                len  = 8;
+                pbuf = buf;
+                break;
+            }
+            case ('l'):            /* 64 bits hexadecimal unsigned */
+            {
+                uint64_t val = va_arg( args , uint64_t );
+                boot_tty_write( "0x" , 2 );
+                for(i = 0; i < 16; i++) 
+                {
+                    buf[15 - i] = HexaTab[val & 0xF];
+                    if (!(val = (val>>4)))  break;
+                }
+                len  = i + 1;
+                pbuf = &buf[15 - i];
+                break;
+            }
+            case ('L'):           /* 64 bits hexadecimal unsigned on 18 char */ 
+            {
+                uint64_t val = va_arg( args , uint64_t );
+                boot_tty_write( "0x" , 2 );
+                for(i = 0; i < 16; i++) 
+                {
+                    buf[15 - i] = HexaTab[val & 0xF];
+                    val = (val>>4);
+                }
+                len  = 16;
+                pbuf = buf;
+                break;
+            }
+            case ('s'):             /* string */
+            {
+                char* str = va_arg( args , char* );
+                while (str[len]) 
+                {
+                    len++;
+                }
+                pbuf = str;
+                break;
+            }
+            default:
+            {
+                boot_tty_write( "\n[PANIC] in boot_printf() : illegal format\n", 43 );
+            }
         }
 
-        // A 32-bit signed decimal notation of an integer.
-        case ('d'):
-        {
-            // Retrieving the argument.
-            arg_val  = va_arg(arg, int);
-
-            // Printing the minus sign if needed.
-            if (arg_val < 0)
-            {
-                arg_val = -arg_val;
-                if (boot_tty_write("-", 1))
-                    goto error;
-            }
-
-            // Converting the argument raw value to a character string.
-            // Note that the maximum value for this type is 2.147.483.647 
-            // (2^31 - 1), a 10-digit number.
-            for (conv_index = 0; conv_index < 10; conv_index++)
-            {
-                // Writing to the buffer, starting from the least significant
-                // digit.
-                buf[9 - conv_index] = conv_tab[arg_val % 10];
-
-                // Getting to the next digit, stop when no more digit.
-                if ((arg_val /= 10) == 0)
-                    break;
-            }
-
-            // Preparing for the printing.
-            arg_len  = conv_index + 1;
-            print_pt = &buf[9 - conv_index];
-            break;
-        }
-
-        // A 32-bit unsigned decimal notation of an integer.
-        case ('u'):
-        {
-            // Retrieving the argument.
-            arg_val  = va_arg(arg, unsigned int);
-
-            // Converting the argument raw value to a character string.
-            // Note that the maximum value for this type is 4.294.967.295 
-            // (2^32 - 1), also a 10-digit number.
-            for (conv_index = 0; conv_index < 10; conv_index++)
-            {
-                // Writing to the buffer, starting from the least significant
-                // digit.
-                buf[9 - conv_index] = conv_tab[arg_val % 10];
-
-                // Getting to the next digit, stop when no more digit.
-                if ((arg_val /= 10) == 0)
-                    break;
-            }
-
-            // Preparing for the printing.
-            arg_len  = conv_index + 1;
-            print_pt = &buf[9 - conv_index];
-            break;
-        }
-
-        // A 32-bit unsigned hexadecimal notation of an integer.
-        case ('x'):
-        {
-            // Retrieving the argument.
-            arg_val  = va_arg(arg, unsigned int);
-
-            // Printing the hexadecimal prefix.
-            if (boot_tty_write("0x", 2))
-                goto error;
-
-            // Converting the argument raw value to a character string.
-            // Note that the maximum value for this type is 0xFFFFFFFF
-            // (2^32 - 1), a 8-digit hexadecimal number.
-            for (conv_index = 0; conv_index < 8; conv_index++)
-            {
-                // Writing to the buffer, starting from the least significant
-                // digit.
-                buf[7 - conv_index] = conv_tab[arg_val % 16];
-
-                // Getting to the next digit, stop when no more digit.
-                if ((arg_val >>= 4) == 0)
-                    break;
-            }
-
-            // Preparing for the printing.
-            arg_len  = conv_index + 1;
-            print_pt = &buf[7 - conv_index];
-            break;
-        }
-
-        // A 64-bit unsigned hexadecimal notation of an integer.
-        case ('l'):
-        {
-            // Retrieving the argument.
-            arg_val  = va_arg(arg, unsigned long long);
-
-            // Printing the hexadecimal prefix.
-            if (boot_tty_write("0x", 2))
-                goto error;
-
-            // Converting the argument raw value to a character string.
-            // Note that the maximum value for this type is 0xFFFFFFFFFFFFFFFF
-            // (2^64 - 1), a 16-digit hexadecimal number.
-            for (conv_index = 0; conv_index < 16; conv_index++)
-            {
-                // Writing to the buffer, starting from the least significant
-                // digit.
-                buf[15 - conv_index] = conv_tab[arg_val % 16];
-
-                // Getting to the next digit, stop when no more digit.
-                if ((arg_val >>= 4) == 0)
-                    break;
-            }
-
-            // Preparing for the printing.
-            arg_len  = conv_index + 1;
-            print_pt = &buf[15 - conv_index];
-            break;
-        }
-
-        // A NUL terminated string.
-        case ('s'):
-        {
-            // Retrieving the argument.
-            print_pt = va_arg(arg, char*);
-
-            // Preparing for the printing. 
-            arg_len  = boot_strlen(print_pt);
-            break;
-        }
-
-        default:
-            goto error;
-
-    }
-
-    /* Printing the converted argument. */
-    if (boot_tty_write(print_pt, arg_len))
-        goto error;
-
-    goto print_text;
-
-error:
-
-    /* Trying to print an error message then exit. */
-    boot_puts("\n[BOOT ERROR] boot_printf(): "
-              "Cannot print the whole message\n"
-             );
-
-    boot_exit();
-
-} // boot_printf()
+        if( pbuf != NULL ) boot_tty_write( pbuf, len );
+        
+        goto printf_text;
+    }
+}  // boot_printf()
+
+
+
+
+
 
 /****************************************************************************
@@ -536,20 +503,18 @@
  ****************************************************************************/
 
+////////////////
 void boot_exit()
 {
-    boot_printf("\n[BOOT PANIC] Suiciding at cycle %d...\n", 
-                boot_get_proctime()
-               );
-
-    while (1)
-        asm volatile ("nop");
+    boot_printf("\n[BOOT PANIC] core %x suicide at cycle %d...\n", 
+                boot_get_procid() , boot_get_proctime() );
+
+    while (1) asm volatile ("nop");
 
 } // boot_exit()
 
-/****************************************************************************/
-
-unsigned int boot_get_proctime()
-{
-    unsigned int res;       /* Value stored in the CP0_COUNT register.      */
+////////////////////////////
+uint32_t boot_get_proctime()
+{
+    uint32_t res;       /* Value stored in the CP0_COUNT register.      */
 
     asm volatile("mfc0 %0, $9" : "=r"(res));
@@ -559,9 +524,9 @@
 } // boot_get_proctime()
 
-/****************************************************************************/
-
-unsigned int boot_get_procid()
-{
-    unsigned int res;       /* Value stored in the CP0_PROCID register.     */
+
+//////////////////////////
+uint32_t boot_get_procid()
+{
+    uint32_t res;       /* Value stored in the CP0_PROCID register.     */
 
     asm volatile("mfc0 %0, $15, 1" : "=r"(res));
@@ -571,33 +536,26 @@
 } // boot_get_procid()
 
-/****************************************************************************/
-
-void boot_barrier(xptr_t xp_barrier, uint32_t count)
-{
-    boot_barrier_t* ptr;        /* Classic pointer to the toggling 
-                                   barrier.                                 */
-    uint32_t        cxy;        /* Identifier of the cluster containing
-                                   the toggling barrier.                    */
-    uint32_t        expected;   /* Expected barrier state after reset.      */
-    uint32_t        current;    /* Number of processors reached the 
-                                   barrier.                                 */
-
-    /* Extracting information from the extended pointer. */
-    ptr = (boot_barrier_t*)GET_PTR(xp_barrier);
-    cxy = (uint32_t)       GET_CXY(xp_barrier);
-
-    /* 
-     * Explicitly testing the barrier sense value because no initialization
-     * has been previously done.
-     */
-    if (boot_remote_lw(XPTR(cxy, &ptr->sense)) == 0)
-        expected = 1;
-    else
-        expected = 0;
+
+////////////////////////////////////////////////
+void boot_remote_barrier( xptr_t     xp_barrier, 
+                          uint32_t   count)
+{
+    boot_remote_barrier_t * ptr;
+    uint32_t                cxy;
+    uint32_t                expected;
+    uint32_t                current;
+
+    // Extract information from the extended pointer
+    ptr = (boot_remote_barrier_t*)GET_PTR(xp_barrier);
+    cxy = (uint32_t)              GET_CXY(xp_barrier);
+
+    // Explicitly test the barrier sense value because no initialization
+    if (boot_remote_lw(XPTR(cxy, &ptr->sense)) == 0) expected = 1;
+    else                                             expected = 0;
     
-    /* Incrementing the counter. */
+    // Atomically increment counter
     current = boot_remote_atomic_add(XPTR(cxy, &ptr->current), 1);
     
-    /* The processor arrived last resets the barrier and toggles its sense. */
+    // The processor arrived last resets the barrier and toggles its sense
     if (current == (count - 1))
     {
@@ -605,5 +563,5 @@
         boot_remote_sw(XPTR(cxy, &ptr->sense), expected);
     }
-    /* Other processors poll the sense. */
+    // Other processors poll the sense
     else
     {
@@ -613,2 +571,40 @@
 } // boot_barrier()
 
+////////////////////////////////////////
+void boot_remote_lock( xptr_t  lock_xp )
+
+{
+    // Extract information from the extended pointer
+    boot_remote_spinlock_t * ptr = (boot_remote_spinlock_t *)GET_PTR( lock_xp );
+    uint32_t                 cxy = GET_CXY( lock_xp );
+
+    // get next free ticket
+    uint32_t ticket = boot_remote_atomic_add( XPTR( cxy , &ptr->ticket ) , 1 );
+
+    // poll the current slot index
+    while ( boot_remote_lw( XPTR( cxy , &ptr->current ) ) != ticket )
+    {
+        asm volatile ("nop");
+    }
+
+}  // boot_remote_lock()
+
+/////////////////////////////////////////
+void boot_remote_unlock( xptr_t lock_xp )
+{
+    asm volatile ( "sync" );   // for consistency
+
+    // Extract information from the extended pointer
+    boot_remote_spinlock_t * ptr = (boot_remote_spinlock_t *)GET_PTR( lock_xp );
+    uint32_t                 cxy = GET_CXY( lock_xp );
+    xptr_t            current_xp = XPTR( cxy , &ptr->current );
+
+    // get current index value
+    uint32_t current = boot_remote_lw( current_xp );
+
+    // increment current index
+    boot_remote_sw( current_xp , current + 1 );
+
+}  // boot_remote_unlock()
+
+
