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

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

#include <reset_utils.h>

/*
 * pread(size_t file_offset, void* buf, size_t nbyte, size_t offset)
 *
 * read from disk into buffer "nbyte" bytes from (file_offset + offset)
 *
 * \param file_offset: Disk relative offset of file
 * \param buf: Destination buffer
 * \param nbyte: Number of bytes to read
 * \param offset: File relative offset
 *
 * \note Absolute disk offset (in bytes) is (file_offset + offset)
 */
int pread(size_t file_offset, void *buf, size_t nbyte, size_t offset) {
    if (nbyte == 0) return 0;

    /*
     * Cache block data buffer and cached block index
     */
    static struct aligned_blk blk_buf;
    static int blk_buf_idx = -1;

    char *dst;
    int offset_blk;
    int unaligned_nbyte;
    int read_nbyte;

    dst = (char*) buf;

    /*
     * Offset parameter is relative to file, therefore compute disk relative
     * offset (in bytes)
     */
    offset += file_offset;

    /*
     * Read unaligned bytes at start of segment passing by block cache
     */
    offset_blk = (offset / BLOCK_SIZE);
    offset = (offset % BLOCK_SIZE);
    unaligned_nbyte = BLOCK_SIZE - offset;
    read_nbyte = 0;
    if (offset) {
        /*
         * Check cache block hit: if miss, read new block else, use cache block
         * data
         */
        if (offset_blk != blk_buf_idx) {
            if (reset_ioc_read(offset_blk, (void*)&blk_buf, 1)) {
                return -1; 
            }
        }
        blk_buf_idx = offset_blk;
        read_nbyte = (nbyte > unaligned_nbyte) ? unaligned_nbyte : nbyte; 
        memcpy((void*)dst, (void*)&blk_buf.b[offset], read_nbyte);
        nbyte -= read_nbyte;
        offset_blk += 1;
    }

    /*
     * Read aligned bytes directly to buffer
     */
    size_t nblk = nbyte / BLOCK_SIZE; 
    if (nblk) {
        if (reset_ioc_read(offset_blk, (void*)&dst[read_nbyte], nblk)) {
            return -1;
        }
        read_nbyte += (nblk * BLOCK_SIZE);
        nbyte -= read_nbyte;
        offset_blk += nblk;
    }

    /*
     * Read unaligned bytes at the end of segment passing by block cache
     */
    if (nbyte) {
        if (reset_ioc_read(offset_blk, (void*)&blk_buf, 1)) {
            return -1;
        }
        blk_buf_idx = offset_blk;
        memcpy((void*)&dst[read_nbyte], (void*)&blk_buf, nbyte);
        read_nbyte += nbyte;
    }
    return read_nbyte; 
}

/********************************************************************
 * 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, size_t n)
{
    unsigned int *dst = _dst;
    const unsigned int *src = _src;
    if ( !((unsigned int)dst & 3) && !((unsigned int)src & 3) )
    {
        while (n > 3) 
        {
            *dst++ = *src++;
            n -= 4;
        }
    }

    unsigned char *cdst = (unsigned char*) dst;
    unsigned char *csrc = (unsigned char*) src;
    while (n--) 
    {
        *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, int c, size_t len)
{
    if (len == 0) return _dst;

    unsigned char val = (unsigned char) c;
    unsigned 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 (len > 3) 
        {
            *dst++ = word;
            len -= 4;
        }
    }

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

    return _dst;
}

/********************************************************************
 * check_elf_header(Elf32_Ehdr*)
 *
 * Verify that ELF file is valid and that the number of program
 * headers does not exceed the defined maximum
 *
 * \param ehdr : ELF header pointer
 *
 ********************************************************************/
void check_elf_header(Elf32_Ehdr *ehdr)
{
    /*
     * Verification of ELF Magic Number
     */
    if ((ehdr->e_ident[EI_MAG0] != ELFMAG0) ||
        (ehdr->e_ident[EI_MAG1] != ELFMAG1) ||
        (ehdr->e_ident[EI_MAG2] != ELFMAG2) ||
        (ehdr->e_ident[EI_MAG3] != ELFMAG3))
    {
        reset_puts("[RESET ERROR] Unrecognized file format (not an ELF format)\n");
        reset_exit();
    }

    /*
     * Verification of Program Headers table size. It must be
     * smaller than the work size allocated for the
     * elf_pht[PHDR_ARRAY_SIZE] array
     */
    if (ehdr->e_phnum > PHDR_ARRAY_SIZE)
    {
        reset_puts("[RESET ERROR] ELF PHDR table size too large\n");
        reset_exit();
    }
}

/********************************************************************
 * 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_puts("\n");
}


/********************************************************************
 * 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, size_t size)
{
    addr_t *mcc_address = (addr_t*)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, size_t line_size, size_t 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