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

Last change on this file since 737 was 703, checked in by cfuguet, 10 years ago

tsar_boot:

  • Improvement of memset function. When buffer not word aligned, write first bytes in a byte basis and then, write remaining bytes using 4 byte words.
File size: 8.4 KB
Line 
1/**
2 * \file    : reset_utils.c
3 * \date    : August 2012
4 * \author  : Cesar Fuguet
5 *
6 * Definition of utilities functions used by the TSAR pre-loader
7 */
8
9#include <reset_utils.h>
10
11/*
12 * pread(size_t file_offset, void* buf, size_t nbyte, size_t offset)
13 *
14 * read from disk into buffer "nbyte" bytes from (file_offset + offset)
15 *
16 * \param file_offset: Disk relative offset of file
17 * \param buf: Destination buffer
18 * \param nbyte: Number of bytes to read
19 * \param offset: File relative offset
20 *
21 * \note Absolute disk offset (in bytes) is (file_offset + offset)
22 */
23int pread(size_t file_offset, void *buf, size_t nbyte, size_t offset) {
24    if (nbyte == 0) return 0;
25
26    /*
27     * Cache block data buffer and cached block index
28     */
29    static struct aligned_blk blk_buf;
30    static int blk_buf_idx = -1;
31
32    char *dst;
33    int offset_blk;
34    int unaligned_nbyte;
35    int read_nbyte;
36
37    dst = (char*) buf;
38
39    /*
40     * Offset parameter is relative to file, therefore compute disk relative
41     * offset (in bytes)
42     */
43    offset += file_offset;
44
45    /*
46     * Read unaligned bytes at start of segment passing by block cache
47     */
48    offset_blk = (offset / BLOCK_SIZE);
49    offset = (offset % BLOCK_SIZE);
50    unaligned_nbyte = BLOCK_SIZE - offset;
51    read_nbyte = 0;
52    if (offset) {
53        /*
54         * Check cache block hit: if miss, read new block else, use cache block
55         * data
56         */
57        if (offset_blk != blk_buf_idx) {
58            if (reset_ioc_read(offset_blk, (void*)&blk_buf, 1)) {
59                return -1; 
60            }
61        }
62        blk_buf_idx = offset_blk;
63        read_nbyte = (nbyte > unaligned_nbyte) ? unaligned_nbyte : nbyte; 
64        memcpy((void*)dst, (void*)&blk_buf.b[offset], read_nbyte);
65        nbyte -= read_nbyte;
66        offset_blk += 1;
67    }
68
69    /*
70     * Read aligned bytes directly to buffer
71     */
72    size_t nblk = nbyte / BLOCK_SIZE; 
73    if (nblk) {
74        if (reset_ioc_read(offset_blk, (void*)&dst[read_nbyte], nblk)) {
75            return -1;
76        }
77        read_nbyte += (nblk * BLOCK_SIZE);
78        nbyte -= read_nbyte;
79        offset_blk += nblk;
80    }
81
82    /*
83     * Read unaligned bytes at the end of segment passing by block cache
84     */
85    if (nbyte) {
86        if (reset_ioc_read(offset_blk, (void*)&blk_buf, 1)) {
87            return -1;
88        }
89        blk_buf_idx = offset_blk;
90        memcpy((void*)&dst[read_nbyte], (void*)&blk_buf, nbyte);
91        read_nbyte += nbyte;
92    }
93    return read_nbyte; 
94}
95
96/********************************************************************
97 * proctime()
98 *
99 * Returns processor local time.
100 ********************************************************************/
101inline unsigned int proctime() 
102{
103    unsigned int ret;
104    asm volatile ("mfc0   %0,        $9":"=r" (ret));
105    return ret;
106}
107
108/********************************************************************
109 * memcpy( _dst, _src, size )
110 *
111 * Transfer data between two memory buffers.
112 *
113 * \param _dst   : Destination buffer base address
114 * \param _src   : Source buffer base address
115 * \param size   : Number of bytes to transfer
116 *
117 ********************************************************************/
118void* memcpy(void *_dst, const void *_src, size_t n)
119{
120    unsigned int *dst = _dst;
121    const unsigned int *src = _src;
122    if (!((unsigned int)dst & 3) && !((unsigned int)src & 3)) {
123        while (n > 3) {
124            *dst++ = *src++;
125            n -= 4;
126        }
127    }
128
129    unsigned char *cdst = (unsigned char*) dst;
130    unsigned char *csrc = (unsigned char*) src;
131    while (n--) {
132        *cdst++ = *csrc++;
133    }
134    return _dst;
135}
136
137/********************************************************************
138 * memset( _dst, value, size )
139 *
140 * Initialize memory buffers with predefined value.
141 *
142 * \param _dst   : Destination buffer base address
143 * \param value  : Initialization value
144 * \param size   : Number of bytes to initialize
145 *
146 ********************************************************************/
147void* memset(void *_dst, int c, size_t len)
148{
149    if (len == 0) return _dst;
150
151    unsigned char val = (unsigned char) c;
152
153    /*
154     * Set not word aligned bytes at start of destination buffer
155     */
156    unsigned char* cdst = (unsigned char*) _dst;
157    while (((addr_t)cdst & 3) && len--) {
158        *cdst++ = val;
159    }
160
161    /*
162     * Set 4 bytes words on destination buffer
163     */
164    unsigned int word = (val << 24) | (val << 16) | (val << 8 ) | val;
165    addr_t *wdst = (addr_t*)cdst;
166    while (len > 3) {
167        *wdst++ = word;
168        len -= 4;
169    }
170
171    /*
172     * Set not word aligned bytes at end of destination buffer
173     */
174    cdst = (unsigned char*) wdst;
175    while(len--) {
176        *cdst++ = val;
177    }
178    return _dst;
179}
180
181/********************************************************************
182 * check_elf_header(Elf32_Ehdr*)
183 *
184 * Verify that ELF file is valid and that the number of program
185 * headers does not exceed the defined maximum
186 *
187 * \param ehdr : ELF header pointer
188 *
189 ********************************************************************/
190void check_elf_header(Elf32_Ehdr *ehdr)
191{
192    /*
193     * Verification of ELF Magic Number
194     */
195    if ((ehdr->e_ident[EI_MAG0] != ELFMAG0) ||
196        (ehdr->e_ident[EI_MAG1] != ELFMAG1) ||
197        (ehdr->e_ident[EI_MAG2] != ELFMAG2) ||
198        (ehdr->e_ident[EI_MAG3] != ELFMAG3))
199    {
200        reset_puts("[RESET ERROR] Unrecognized file format (not an ELF format)\n");
201        reset_exit();
202    }
203
204    /*
205     * Verification of Program Headers table size. It must be
206     * smaller than the work size allocated for the
207     * elf_pht[PHDR_ARRAY_SIZE] array
208     */
209    if (ehdr->e_phnum > PHDR_ARRAY_SIZE)
210    {
211        reset_puts("[RESET ERROR] ELF PHDR table size too large\n");
212        reset_exit();
213    }
214}
215
216/********************************************************************
217 * reset_print_elf_phdr( elf_phdr_ptr )
218 *
219 * Print some fields of a ELF program header
220 *
221 * \param elf_phdr_ptr : Pointer to the ELF program header to print
222 *
223 ********************************************************************/
224void reset_print_elf_phdr(Elf32_Phdr * elf_phdr_ptr)
225{
226    reset_puts("- type   : ");
227    reset_putx(elf_phdr_ptr->p_type);
228    reset_puts("\n- offset : ");
229    reset_putx(elf_phdr_ptr->p_offset);
230    reset_puts("\n- vaddr  : ");
231    reset_putx(elf_phdr_ptr->p_vaddr);
232    reset_puts("\n- paddr  : ");
233    reset_putx(elf_phdr_ptr->p_paddr);
234    reset_puts("\n- filesz : ");
235    reset_putx(elf_phdr_ptr->p_filesz);
236    reset_puts("\n- memsz  : ");
237    reset_putx(elf_phdr_ptr->p_memsz);
238    reset_puts("\n- flags  : ");
239    reset_putx(elf_phdr_ptr->p_flags);
240    reset_puts("\n- align  : ");
241    reset_putx(elf_phdr_ptr->p_align);
242    reset_puts("\n");
243}
244
245
246/********************************************************************
247 * reset_mcc_inval()
248 *
249 * Invalidate all data cache lines corresponding to a memory buffer
250 * (identified by an address and a size) in L2 cache.
251 ********************************************************************/
252#if USE_IOB
253void reset_mcc_invalidate (const void * buffer, size_t size)
254{
255    addr_t *mcc_address = (addr_t*)MCC_PADDR_BASE;
256
257    // get the hard lock assuring exclusive access to MEMC
258    while (ioread32(&mcc_address[MCC_LOCK]));
259
260    // write invalidate paremeters on the memory cache this preloader
261    // use only the cluster 0 and then the HI bits are not used
262   
263    iowrite32(&mcc_address[MCC_ADDR_LO], (unsigned int) buffer);
264    iowrite32(&mcc_address[MCC_ADDR_HI], (unsigned int) 0);
265    iowrite32(&mcc_address[MCC_LENGTH] , (unsigned int) size);
266    iowrite32(&mcc_address[MCC_CMD]    , (unsigned int) MCC_CMD_INVAL);
267
268    // release the lock protecting MEMC
269    iowrite32(&mcc_address[MCC_LOCK], (unsigned int) 0);
270}
271#endif
272
273/********************************************************************
274 * reset_dcache_buf_invalidate()
275 *
276 * Invalidate all data cache lines corresponding to a memory buffer
277 * (identified by an address and a size) in L1 cache and L2 cache.
278 ********************************************************************/
279#if (CACHE_COHERENCE == 0) || USE_IOB
280void reset_buf_invalidate (const void * buffer, size_t line_size, size_t size)
281{
282    unsigned int i;
283
284    // iterate on cache lines
285    for (i = 0; i <= size; i += line_size) 
286    {
287        asm volatile(
288            " cache %0, %1"
289            :// no outputs
290            :"i" (0x11), "R" (*((unsigned char *) buffer + i))
291            );
292    }
293
294#if USE_IOB
295    reset_mcc_invalidate(buffer, size);
296#endif
297}
298#endif
299
300// vim: tabstop=4 : softtabstop=4 : shiftwidth=4 : expandtab
Note: See TracBrowser for help on using the repository browser.