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

Last change on this file since 922 was 830, checked in by porquet, 10 years ago

tsar_boot: fix the number of bytes to read from disk

File size: 5.7 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#include <reset_tty.h>
11#include <reset_ioc.h>
12#include <io.h>
13
14/**
15 * \param file_offset: Disk relative offset of file
16 * \param buf: Destination buffer
17 * \param nbyte: Number of bytes to read
18 * \param offset: File relative offset
19 *
20 * \brief read from disk into buffer "nbyte" bytes from (file_offset + offset)
21 *
22 * \note Absolute disk offset (in bytes) is (file_offset + offset)
23 */
24int pread(size_t file_offset, void *buf, size_t nbyte, size_t offset) {
25    if (nbyte == 0) return 0;
26
27    /*
28     * Cache block data buffer and cached block index
29     */
30    static struct aligned_blk blk_buf;
31    static int blk_buf_idx = -1;
32
33    char *dst;
34    int offset_blk;
35    int unaligned_nbyte;
36    int read_nbyte;
37
38    dst = (char*) buf;
39
40    /*
41     * Offset parameter is relative to file, therefore compute disk relative
42     * offset (in bytes)
43     */
44    offset += file_offset;
45
46    /*
47     * Read unaligned bytes at start of segment passing by block cache
48     */
49    offset_blk = (offset / BLOCK_SIZE);
50    offset = (offset % BLOCK_SIZE);
51    unaligned_nbyte = BLOCK_SIZE - offset;
52    read_nbyte = 0;
53    if (offset) {
54        /*
55         * Check cache block hit: if miss, read new block else, use cache block
56         * data
57         */
58        if (offset_blk != blk_buf_idx) {
59            if (reset_ioc_read(offset_blk, (void*)&blk_buf, 1)) {
60                return -1;
61            }
62        }
63        blk_buf_idx = offset_blk;
64        read_nbyte = (nbyte > unaligned_nbyte) ? unaligned_nbyte : nbyte;
65        memcpy((void*)dst, (void*)&blk_buf.b[offset], read_nbyte);
66        nbyte -= read_nbyte;
67        offset_blk += 1;
68    }
69
70    /*
71     * Read aligned bytes directly to buffer
72     */
73    size_t nblk = nbyte / BLOCK_SIZE;
74    if (nblk) {
75        if (reset_ioc_read(offset_blk, (void*)&dst[read_nbyte], nblk)) {
76            return -1;
77        }
78        read_nbyte += (nblk * BLOCK_SIZE);
79        nbyte -= (nblk * BLOCK_SIZE);
80        offset_blk += nblk;
81    }
82
83    /*
84     * Read unaligned bytes at the end of segment passing by block cache
85     */
86    if (nbyte) {
87        if (reset_ioc_read(offset_blk, (void*)&blk_buf, 1)) {
88            return -1;
89        }
90        blk_buf_idx = offset_blk;
91        memcpy((void*)&dst[read_nbyte], (void*)&blk_buf, nbyte);
92        read_nbyte += nbyte;
93    }
94    return read_nbyte;
95}
96
97/**
98 * \param _dst   : Destination buffer base address
99 * \param _src   : Source buffer base address
100 * \param size   : Number of bytes to transfer
101 *
102 * \brief Transfer data between two memory buffers.
103 */
104void* memcpy(void *_dst, const void *_src, size_t n)
105{
106    unsigned int *dst = _dst;
107    const unsigned int *src = _src;
108    if (!((unsigned int)dst & 3) && !((unsigned int)src & 3)) {
109        while (n > 3) {
110            *dst++ = *src++;
111            n -= 4;
112        }
113    }
114
115    unsigned char *cdst = (unsigned char*) dst;
116    unsigned char *csrc = (unsigned char*) src;
117    while (n--) {
118        *cdst++ = *csrc++;
119    }
120    return _dst;
121}
122
123/**
124 * \param _dst   : Destination buffer base address
125 * \param value  : Initialization value
126 * \param size   : Number of bytes to initialize
127 *
128 * \brief Initialize memory buffers with predefined value.
129 */
130void* memset(void *_dst, int c, size_t len)
131{
132    if (len == 0) return _dst;
133
134    unsigned char val = (unsigned char) c;
135
136    /*
137     * Set not word aligned bytes at start of destination buffer
138     */
139    unsigned char* cdst = (unsigned char*) _dst;
140    while (((addr_t)cdst & 3) && len--) {
141        *cdst++ = val;
142    }
143
144    /*
145     * Set 4 bytes words on destination buffer
146     */
147    unsigned int word = (val << 24) | (val << 16) | (val << 8 ) | val;
148    addr_t *wdst = (addr_t*)cdst;
149    while (len > 3) {
150        *wdst++ = word;
151        len -= 4;
152    }
153
154    /*
155     * Set not word aligned bytes at end of destination buffer
156     */
157    cdst = (unsigned char*) wdst;
158    while(len--) {
159        *cdst++ = val;
160    }
161    return _dst;
162}
163
164/**
165 * \param ehdr : ELF header pointer
166 *
167 * \brief Verify that ELF file is valid and that the number of program headers
168 *        does not exceed the defined maximum
169 */
170void check_elf_header(Elf32_Ehdr *ehdr)
171{
172    /*
173     * Verification of ELF Magic Number
174     */
175    if ((ehdr->e_ident[EI_MAG0] != ELFMAG0) ||
176        (ehdr->e_ident[EI_MAG1] != ELFMAG1) ||
177        (ehdr->e_ident[EI_MAG2] != ELFMAG2) ||
178        (ehdr->e_ident[EI_MAG3] != ELFMAG3))
179    {
180        reset_puts("[RESET ERROR] Unrecognized file format (not an ELF format)\n");
181        reset_exit();
182    }
183
184    /*
185     * Verification of Program Headers table size. It must be
186     * smaller than the work size allocated for the
187     * elf_pht[RESET_PHDR_ARRAY_SIZE] array
188     */
189    if (ehdr->e_phnum > RESET_PHDR_ARRAY_SIZE)
190    {
191        reset_puts("[RESET ERROR] ELF PHDR table size too large\n");
192        reset_exit();
193    }
194}
195
196/**
197 * \param elf_phdr_ptr : Pointer to the ELF program header to print
198 *
199 * \brief Print some fields of a ELF program header
200 */
201void reset_print_elf_phdr(Elf32_Phdr * elf_phdr_ptr)
202{
203    reset_puts("- type   : ");
204    reset_putx(elf_phdr_ptr->p_type);
205    reset_puts("\n- offset : ");
206    reset_putx(elf_phdr_ptr->p_offset);
207    reset_puts("\n- vaddr  : ");
208    reset_putx(elf_phdr_ptr->p_vaddr);
209    reset_puts("\n- paddr  : ");
210    reset_putx(elf_phdr_ptr->p_paddr);
211    reset_puts("\n- filesz : ");
212    reset_putx(elf_phdr_ptr->p_filesz);
213    reset_puts("\n- memsz  : ");
214    reset_putx(elf_phdr_ptr->p_memsz);
215    reset_puts("\n- flags  : ");
216    reset_putx(elf_phdr_ptr->p_flags);
217    reset_puts("\n- align  : ");
218    reset_putx(elf_phdr_ptr->p_align);
219    reset_puts("\n");
220}
221
222/*
223 * vim: tabstop=4 : softtabstop=4 : shiftwidth=4 : expandtab
224 */
Note: See TracBrowser for help on using the repository browser.