source: trunk/softs/tsar_boot/src/reset_utils.c @ 673

/**
 * \file    : reset_utils.c
 * \date    : August 2012
 * \author  : Cesar Fuguet
 *
 * Definition of utilities functions used by the TSAR pre-loader
 */

#include <reset_utils.h>

/********************************************************************
 * proctime()
 *
 * Returns processor local time.
 ********************************************************************/
inline unsigned int proctime() 
{
    unsigned int ret;
    asm volatile ("mfc0   %0,        $9":"=r" (ret));
    return ret;
}

/********************************************************************
 * memcpy( _dst, _src, size )
 *
 * Transfer data between two memory buffers.
 *
 * \param _dst   : Destination buffer base address 
 * \param _src   : Source buffer base address
 * \param size   : Number of bytes to transfer 
 *
 ********************************************************************/
void * memcpy(void *_dst, const void *_src, unsigned int size)
{
    unsigned int *dst = _dst;
    const unsigned int *src = _src;
    if ( !((unsigned int)dst & 3) && !((unsigned int)src & 3) )
    {
        while (size > 3) 
        {
            *dst++ = *src++;
            size -= 4;
        }
    }

    unsigned char *cdst = (unsigned char*) dst;
    unsigned char *csrc = (unsigned char*) src;

    while (size--) 
    {
        *cdst++ = *csrc++;
    }
    return _dst;
}

/********************************************************************
 * memset( _dst, value, size )
 *
 * Initialize memory buffers with predefined value.
 *
 * \param _dst   : Destination buffer base address 
 * \param value  : Initialization value 
 * \param size   : Number of bytes to initialize
 *
 ********************************************************************/
void * memset(void *_dst, const int value, unsigned int size)
{
    unsigned char val = (unsigned char) value;
    int word = (val << 24) || (val << 16) ||
               (val << 8 ) || (val      );

    /*
     * Write 4 bytes when destination buffer is aligned to 4 bytes
     * and size is greater or equal to 4 
     */
    unsigned int *dst = _dst;
    if ( !((unsigned int)dst & 3) )
    {
        while (size > 3) 
        {
            *dst++ = word;
            size -= 4;
        }
    }

    /*
     * Write 1 byte when destination buffer is not aligned to 4 bytes
     * or size is smaller than 4 
     */
    char* cdst = (char*) _dst;
    while(size--)
    {
        *cdst++ = (char) value;
    }

    return _dst;
}

/********************************************************************
 * reset_print_elf_phdr( elf_phdr_ptr )
 *
 * Print some fields of a ELF program header
 *
 * \param elf_phdr_ptr : Pointer to the ELF program header to print
 *
 ********************************************************************/
void reset_print_elf_phdr(Elf32_Phdr * elf_phdr_ptr)
{
    reset_puts("- type   : ");
    reset_putx(elf_phdr_ptr->p_type);

    reset_puts("\n- offset : ");
    reset_putx(elf_phdr_ptr->p_offset);

    reset_puts("\n- vaddr  : ");
    reset_putx(elf_phdr_ptr->p_vaddr);

    reset_puts("\n- paddr  : ");
    reset_putx(elf_phdr_ptr->p_paddr);

    reset_puts("\n- filesz : ");
    reset_putx(elf_phdr_ptr->p_filesz);

    reset_puts("\n- memsz  : ");
    reset_putx(elf_phdr_ptr->p_memsz);

    reset_puts("\n- flags  : ");
    reset_putx(elf_phdr_ptr->p_flags);

    reset_puts("\n- align  : ");
    reset_putx(elf_phdr_ptr->p_align);
}


/********************************************************************
 * reset_mcc_inval()
 *
 * Invalidate all data cache lines corresponding to a memory buffer
 * (identified by an address and a size) in L2 cache.
 ********************************************************************/
#if USE_IOB 
void reset_mcc_invalidate ( const void * buffer,
                            unsigned int size)
{
    unsigned int * mcc_address = (unsigned int *)MCC_PADDR_BASE;

    // get the hard lock assuring exclusive access to MEMC
    while (ioread32(&mcc_address[MCC_LOCK]));

    // write invalidate paremeters on the memory cache this preloader
    // use only the cluster 0 and then the HI bits are not used
    
    iowrite32(&mcc_address[MCC_ADDR_LO], (unsigned int) buffer);
    iowrite32(&mcc_address[MCC_ADDR_HI], (unsigned int) 0);
    iowrite32(&mcc_address[MCC_LENGTH] , (unsigned int) size);
    iowrite32(&mcc_address[MCC_CMD]    , (unsigned int) MCC_CMD_INVAL);

    // release the lock protecting MEMC
    iowrite32(&mcc_address[MCC_LOCK], (unsigned int) 0);
}
#endif

/********************************************************************
 * reset_dcache_buf_invalidate()
 *
 * Invalidate all data cache lines corresponding to a memory buffer
 * (identified by an address and a size) in L1 cache and L2 cache.
 ********************************************************************/
#if (CACHE_COHERENCE == 0) || USE_IOB
void reset_buf_invalidate ( const void * buffer,
                            unsigned int line_size,
                            unsigned int size)
{
    unsigned int i;

    // iterate on cache lines
    for (i = 0; i <= size; i += line_size) 
    {
        asm volatile(
            " cache %0, %1"
            :// no outputs
            :"i" (0x11), "R" (*((unsigned char *) buffer + i))
            );
    }

#if USE_IOB
    reset_mcc_invalidate(buffer, size);
#endif
}
#endif

// vim: tabstop=4 : softtabstop=4 : shiftwidth=4 : expandtab