source: trunk/tools/bootloader_tsar/boot_utils.c @ 218

/*
 * 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>

#include <boot_tty_driver.h>
#include <hal_types.h>
#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;  
    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. 
    asm volatile("mfc2  $15,    $24\n"  /* $15 <= CP2_DATA_PADDR_EXT        */
                 "mtc2  %2,     $24\n"  /* CP2_DATA_PADDR_EXT <= cxy        */
                 "lw    %0,   0(%1)\n"  /* *ptr <= data                     */
                 "mtc2  $15,    $24\n"  /* CP2_DATA_PADDR_EXT <= $15        */
                 "sync             \n"
                 : "=&r"(res)           /* Temporary register so that it    
                                           doesn't overlap the other inputs
                                           or outputs.                      */
                 : "r"(ptr), "r"(cxy)
                 : "$15"
                );

    return res;

} // boot_remote_lw()

/////////////////////////////////////////////
void boot_remote_sw(xptr_t xp, uint32_t data)
{
    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 pointers. */
    ptr = (uint32_t)GET_PTR(xp);                                 
    cxy = (uint32_t)GET_CXY(xp);

    /* 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        */
                 "sw    %0,   0(%1)\n"  /* *ptr <= data                     */
                 "mtc2  $15,    $24\n"  /* CP2_DATA_PADDR_EXT <= $15        */
                 "sync             \n"
                 :
                 : "r"(data), "r"(ptr), "r"(cxy)
                 : "$15", "memory"
                );

} // boot_remote_sw()

//////////////////////////////////////////////////////
int32_t boot_remote_atomic_add(xptr_t xp, int32_t val)
{
    int32_t  res;   /* Value stored at the distant memory location before
                       the atomic operation.                                */
    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 pointers. */
    ptr = (uint32_t)GET_PTR(xp);
    cxy = (uint32_t)GET_CXY(xp);

    /* Assembly instructions to get the work done. */
    asm volatile("mfc2  $15,    $24       \n" /* $15 <= CP2_DATA_PADDR_EXT  */ 
                 "mtc2  %3,     $24       \n" /* CP2_DATA_PADDR_EXT <= cxy  */
                 "1:                      \n"
                 "ll    %0,   0(%1)       \n" /* res <= *ptr                */
                 "addu  $3,     %0,     %2\n" /* $3 <= res + val            */
                 "sc    $3,   0(%1)       \n" /* *ptr <= $3                 */
                 "beq   $3,     $0,     1b\n" /* Retry until success.       */
                 "nop                     \n" /* Delayed slot.              */
                 "mtc2  $15,    $24       \n" /* CP2_DATA_PADDR_EXT <= $15  */
                 "sync                    \n"
                 : "=&r"(res)               /* Temporary register so that
                                               it doesn't overlap the other
                                               inputs or outputs.           */
                 : "r"(ptr), "r"(val), "r"(cxy)
                 : "$3", "$15", "memory"
                );

    return res;

} // boot_remote_atomic_add()

///////////////////////////////////////////////////////////////
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.             */
    uint32_t dptr;      /* Classic pointer to the destination buffer.       */
    uint32_t dcxy;      /* Identifier of the cluster containing the 
                           destination buffer.                              */
    uint32_t sptr;      /* Classic pointer to the source buffer.            */
    uint32_t scxy;      /* Identifier of the cluster containing the 
                           source buffer.                                   */
    uint32_t i;         /* Iterator for memory copying loop.                */

    /* Extracting information from the extended pointers. */
    dptr = (uint32_t)GET_PTR(dest);
    dcxy = (uint32_t)GET_CXY(dest);
    sptr = (uint32_t)GET_PTR(src);
    scxy = (uint32_t)GET_CXY(src);

    /* 
     * Testing if we could perform word-by-word copy (if both addresses are
     * word-aligned).
     */
    if ((dptr & 0x3) || (sptr & 0x3)) 
        words_nr = 0;
    else
        words_nr = size >> 2;

    /* Copying word-by-word. */
    for (i = 0; i < words_nr; i++)
    {
        asm volatile("mfc2  $15,    $24\n"  /* $15 <= CP2_DATA_PADDR_EXT    */
                     "mtc2  %0,     $24\n"  /* CP2_DATA_PADDR_EXT <= scxy   */
                     "lw    $3,   0(%1)\n"  /* $3 <= *(sptr + 4*i)          */ 
                     "mtc2  %2,     $24\n"  /* CP2_DATA_PADDR_EXT <= dcxy   */
                     "sw    $3,   0(%3)\n"  /* *(dptr + 4*i) <= $3          */
                     "mtc2  $15,    $24\n"  /* CP2_DATA_PADDR_EXT <= $15    */
                     "sync             \n"
                     :
                     : "r"(scxy), "r"(sptr + (i << 2)), 
                       "r"(dcxy), "r"(dptr + (i << 2))
                     : "$3", "$15", "memory"
                    );
    }

    /* Copying byte-by-byte if there is any left. */
    for (i = words_nr << 2; i < size; i++)
    {
        asm volatile("mfc2  $15,    $24\n"  /* $15 <= CP2_DATA_PADDR_EXT    */
                     "mtc2  %0,     $24\n"  /* CP2_DATA_PADDR_EXT <= scxy   */
                     "lb    $3,   0(%1)\n"  /* $3 <= *(sptr + i)            */ 
                     "mtc2  %2,     $24\n"  /* CP2_DATA_PADDR_EXT <= dcxy   */
                     "sb    $3,   0(%3)\n"  /* *(dptr + i) <= $3            */
                     "mtc2  $15,    $24\n"  /* CP2_DATA_PADDR_EXT <= $15    */
                     "sync             \n"
                     :
                     : "r"(scxy), "r"(sptr + i), 
                       "r"(dcxy), "r"(dptr + i)
                     : "$3", "$15", "memory"
                    );
    }

} // boot_remote_memcpy()

/****************************************************************************
 *                             Atomic operations.                           *
 ****************************************************************************/

int32_t boot_atomic_add(int32_t* ptr, int32_t val)
{
    int32_t res;    /* Value of the variable before the atomic operation.   */

    asm volatile(".set noreorder          \n" 
                 "1:                      \n"
                 "ll    %0,   0(%1)       \n" /* res <= *ptr                */
                 "addu  $3,     %0,     %2\n" /* $3 <= res + val            */
                 "sc    $3,   0(%1)       \n" /* $ptr <= $3                 */
                 "beq   $3,     $0,     1b\n" /* Retry until success.       */
                 "nop                     \n"
                 "sync                    \n"
                 ".set reorder            \n"
                 : "=&r"(res)               /* Temporary register so that it
                                               doesn't overlap the other
                                               inputs or outputs.           */
                 : "r"(ptr), "r"(val)
                 : "$3", "memory"
                );

    return res;

} // boot_atomic_add()

/****************************************************************************
 *                             Memory functions.                            *
 ****************************************************************************/

///////////////////////////////////////////////////////
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)
        {
            *wdst++ = *wsrc++;
            size -= 4;
        }
    }

    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)
    {
        *cdst++ = *csrc++;
    }
} // boot_memcpy()

////////////////////////////////////////////////////
void boot_memset(void * dst, int val, uint32_t size)
{
    val &= 0xFF;

    // 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)
        {
            *wdst++ = wval;
            size -= 4;
        }
    }
    
    char * cdst = (char *)wdst;

    // byte per byte
    while (size--)
    {
        *cdst++ = (char)val;
    }
} // boot_memset()

/****************************************************************************
 *                              String functions.                           *
 ****************************************************************************/

///////////////////////////////////////
void boot_strcpy(char* dest, char* src)
{
    /* Checking if the arguments are correct. */
    if ((dest == NULL) || (src == NULL))
        return;

    /* Copying the string. */
    while ((*dest++ = *src++) != '\0');

} // boot_strcpy()

/////////////////////////////
uint32_t boot_strlen(char* s)
{
    uint32_t res = 0;   /* Length of the string (in bytes).             */

    if (s != NULL)
    {
        while (*s++ != '\0')
            res++;
    }

    return res;

} // boot_strlen()

///////////////////////////////////
int boot_strcmp(char* s1, char* s2)
{
    if ((s1 == NULL) || (s2 == NULL))
        return 0;

    while (1)
    {
        if (*s1 != *s2)
            return 1;
        if (*s1 == '\0')
            break;
        s1++;
        s2++;
    }

    return 0;

} // boot_strcmp()

/****************************************************************************
 *                             Display functions.                           *
 ****************************************************************************/

/////////////////////////
void boot_puts(char* str)
{
    boot_tty_write(str, boot_strlen(str));

} // boot_puts()
    
///////////////////////////////////////
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) 
        {
            boot_tty_write( format , i );
            format += i;
        }
        if (*format == '%') 
        {
            format++;
            goto printf_arguments;
        }
    }

    // release the lock
    boot_remote_unlock( XPTR( 0 , &tty0_lock ) );

    va_end( args );
    return;

printf_arguments:

    {
        char      buf[20];
        char    * pbuf = NULL;
        uint32_t  len  = 0;
        static const char HexaTab[] = "0123456789ABCDEF";
        uint32_t i;

        switch (*format++) 
        {
            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 );
            }
        }

        if( pbuf != NULL ) boot_tty_write( pbuf, len );
        
        goto printf_text;
    }
}  // boot_printf()






/****************************************************************************
 *                              Misc. functions.                            *
 ****************************************************************************/

////////////////
void boot_exit()
{
    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()

////////////////////////////
uint32_t boot_get_proctime()
{
    uint32_t res;       /* Value stored in the CP0_COUNT register.      */

    asm volatile("mfc0 %0, $9" : "=r"(res));

    return res;

} // boot_get_proctime()


//////////////////////////
uint32_t boot_get_procid()
{
    uint32_t res;       /* Value stored in the CP0_PROCID register.     */

    asm volatile("mfc0 %0, $15, 1" : "=r"(res));

    return (res & 0xFFF);

} // boot_get_procid()


////////////////////////////////////////////////
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;
    
    // Atomically increment counter
    current = boot_remote_atomic_add(XPTR(cxy, &ptr->current), 1);
    
    // The processor arrived last resets the barrier and toggles its sense
    if (current == (count - 1))
    {
        boot_remote_sw(XPTR(cxy, &ptr->current), 0);
        boot_remote_sw(XPTR(cxy, &ptr->sense), expected);
    }
    // Other processors poll the sense
    else
    {
        while (boot_remote_lw(XPTR(cxy, &ptr->sense)) != expected);
    }

} // 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()