1 | #include <sys/stat.h> |
2 | #include <stdio.h> |
3 | #include <errno.h> |
4 | #include <string.h> |
5 | #include <stdlib.h> |
6 | #include "debug.h" |
7 | #include "elfldr.h" |
8 | #include "elf.h" |
9 | #include "ingenic.h" |
10 | |
11 | #define max(a, b) ((a) > (b) ? (a) : (b)) |
12 | #define ALIGN(a) ((a + 4095) & ~4095) |
13 | |
14 | #define TRAMP_ARGC 0 // command line argument count |
15 | #define TRAMP_ARGV 1 // command line argument array |
16 | #define TRAMP_ARG2 2 |
17 | #define TRAMP_ARG3 3 |
18 | #define TRAMP_ENTRY 4 // Kernel entry point |
19 | |
20 | static const uint32_t trampoline_template[] = { |
21 | 0x3c04ffff, // lui a0, 0xffff |
22 | 0x3484ffff, // ori a0,a0,0xffff |
23 | |
24 | 0x3c05ffff, // lui a1, 0xffff |
25 | 0x34a5ffff, // ori a1,a1,0xffff |
26 | |
27 | 0x3c06ffff, // lui a2, 0xffff |
28 | 0x34c6ffff, // ori a2,a2,0xffff |
29 | |
30 | 0x3c07ffff, // lui a3, 0xffff |
31 | 0x34e7ffff, // ori a3,a3,0xffff |
32 | |
33 | 0x3c08ffff, // lui t0, 0xffff |
34 | 0x3508ffff, // ori t0,t0,0xffff |
35 | 0x01000008, // jr t0 |
36 | 0x00000000, // nop |
37 | }; |
38 | |
39 | static const unsigned char valid_ident[EI_PAD] = { |
40 | [EI_MAG0] = ELFMAG0, |
41 | [EI_MAG1] = ELFMAG1, |
42 | [EI_MAG2] = ELFMAG2, |
43 | [EI_MAG3] = ELFMAG3, |
44 | [EI_CLASS] = ELFCLASS32, |
45 | [EI_DATA] = ELFDATA2LSB, |
46 | [EI_VERSION] = EV_CURRENT |
47 | }; |
48 | |
49 | static int load_segment(void *ingenic, void *data, uint32_t base, uint32_t filesz, uint32_t memsz) { |
50 | uint32_t end = base + memsz, tail = memsz - filesz; |
51 | int ret = 0; |
52 | |
53 | printf("Loading segment: base 0x%08X, file 0x%08X, mem 0x%08X\n", base, filesz, memsz); |
54 | |
55 | if(end > ingenic_sdram_size(ingenic) + SDRAM_BASE - STAGE2_CODESIZE) { |
56 | fputs(" Segment doesn't fit into SDRAM\n", stderr); |
57 | |
58 | return -1; |
59 | } |
60 | |
61 | if(filesz && ingenic_load_sdram(ingenic, data, base, filesz) == -1) |
62 | return -1; |
63 | |
64 | if(tail) { |
65 | char *dummy_data = malloc(tail); |
66 | |
67 | if(dummy_data == NULL) |
68 | return -1; |
69 | |
70 | memset(dummy_data, 0, tail); |
71 | |
72 | ret = ingenic_load_sdram(ingenic, dummy_data, base + filesz, tail); |
73 | |
74 | free(dummy_data); |
75 | } |
76 | |
77 | return ret; |
78 | } |
79 | |
80 | static int load_elf_image(FILE *elf, void *ingenic, uint32_t *entry, uint32_t *end) { |
81 | Elf32_Ehdr ehdr; |
82 | Elf32_Phdr phdr; |
83 | int i, ret; |
84 | char *data; |
85 | |
86 | if(fread(&ehdr, 1, sizeof(Elf32_Ehdr), elf) != sizeof(Elf32_Ehdr)) { |
87 | if(feof(elf)) |
88 | errno = EINVAL; |
89 | |
90 | return -1; |
91 | } |
92 | |
93 | *entry = ehdr.e_entry; |
94 | |
95 | if(memcmp(ehdr.e_ident, valid_ident, EI_PAD) != 0 || ehdr.e_type != ET_EXEC || ehdr.e_machine != EM_MIPS || ehdr.e_version != EV_CURRENT |
96 | || ehdr.e_phoff == 0 || ehdr.e_phentsize != sizeof(Elf32_Phdr)) { |
97 | |
98 | fputs("Bad ELF identification\n", stderr); |
99 | |
100 | errno = EINVAL; |
101 | |
102 | return -1; |
103 | } |
104 | |
105 | fseek(elf, ehdr.e_phoff, SEEK_SET); |
106 | |
107 | *end = 0; |
108 | |
109 | for(i = 0; i < ehdr.e_phnum; i++) { |
110 | if(fread(&phdr, 1, sizeof(Elf32_Phdr), elf) != sizeof(Elf32_Phdr)) { |
111 | if(feof(elf)) |
112 | errno = EINVAL; |
113 | |
114 | return -1; |
115 | } |
116 | |
117 | if(phdr.p_type == PT_LOAD) { |
118 | data = malloc(phdr.p_filesz); |
119 | |
120 | if(data == NULL) |
121 | return -1; |
122 | |
123 | long save = ftell(elf); |
124 | fseek(elf, phdr.p_offset, SEEK_SET); |
125 | ret = fread(data, 1, phdr.p_filesz, elf); |
126 | fseek(elf, save, SEEK_SET); |
127 | |
128 | if(ret != phdr.p_filesz) { |
129 | free(data); |
130 | |
131 | if(feof(elf)) |
132 | errno = EINVAL; |
133 | |
134 | return -1; |
135 | } |
136 | |
137 | ret = load_segment(ingenic, data, phdr.p_paddr, phdr.p_filesz, phdr.p_memsz); |
138 | |
139 | free(data); |
140 | |
141 | if(ret == -1) |
142 | return -1; |
143 | |
144 | *end = max(phdr.p_paddr + phdr.p_memsz, *end); |
145 | } |
146 | } |
147 | |
148 | return 0; |
149 | } |
150 | |
151 | |
152 | static void trampoline_set(uint32_t *trampoline, int index, |
153 | uint32_t value) { |
154 | index *= 2; |
155 | |
156 | trampoline[index] = (trampoline[index] & 0xFFFF0000) | ((value & 0xFFFF0000) >> 16); |
157 | trampoline[index + 1] = (trampoline[index + 1] & 0xFFFF0000) | (value & 0x0000FFFF); |
158 | } |
159 | |
160 | static int load_args(void *ingenic, uint32_t base, const char *filename, |
161 | const char *const *args, int *pargc, uint32_t *end) { |
162 | |
163 | size_t total_len = 0; |
164 | int argc = 0; |
165 | |
166 | for(int i = 0; args[i]; i++) { |
167 | size_t len = strlen(args[i]); |
168 | total_len += len + 1; |
169 | |
170 | for(int j = 0; j < len; j++) |
171 | if(args[i][j] == ' ') |
172 | argc++; |
173 | |
174 | argc++; |
175 | } |
176 | |
177 | *pargc = argc; |
178 | *end = base + total_len + sizeof(uint32_t) * argc; |
179 | |
180 | uint32_t *buf = malloc(sizeof(uint32_t) * argc + total_len); |
181 | if(buf == NULL) |
182 | return -1; |
183 | |
184 | char *cmdline = (char *)(buf + argc); |
185 | size_t off = 0; |
186 | |
187 | for(int i = 0; args[i]; i++) { |
188 | size_t len = strlen(args[i]); |
189 | memcpy(cmdline + off, args[i], len + 1); |
190 | |
191 | if(args[i + 1]) |
192 | cmdline[off + len] = ' '; |
193 | |
194 | off += len + 1; |
195 | } |
196 | |
197 | printf("Compiled cmdline: '%s'\n", cmdline); |
198 | |
199 | uint32_t offset = 0; |
200 | char *ptr = cmdline; |
201 | |
202 | for(int i = 0; i < argc; i++) { |
203 | buf[i] = base + argc * sizeof(uint32_t) + offset; |
204 | |
205 | ptr = strchr(ptr, ' '); |
206 | |
207 | if(ptr == NULL) |
208 | break; |
209 | |
210 | *ptr++ = 0; |
211 | offset = ptr - cmdline; |
212 | } |
213 | |
214 | int ret = load_segment(ingenic, |
215 | buf, |
216 | base, |
217 | sizeof(uint32_t) * argc + total_len, |
218 | sizeof(uint32_t) * argc + total_len); |
219 | free(buf); |
220 | |
221 | return ret; |
222 | } |
223 | |
224 | int load_elf(void *ingenic, |
225 | const char *filename, |
226 | const char *args, |
227 | const char *initrd) { |
228 | |
229 | uint32_t entry, end, trampoline_base, args_base; |
230 | uint32_t initrd_base, initrd_size; |
231 | |
232 | int argc; |
233 | const char *all_args[4] = { filename, args, NULL, NULL }; |
234 | char initrd_args[64]; |
235 | |
236 | printf( |
237 | "Loading kernel %s:\n" |
238 | " Command line: '%s'\n", |
239 | filename, |
240 | args |
241 | ); |
242 | |
243 | FILE *elf = fopen(filename, "rb"); |
244 | |
245 | if(elf == NULL) |
246 | return -1; |
247 | |
248 | int ret = load_elf_image(elf, ingenic, &entry, &end); |
249 | |
250 | fclose(elf); |
251 | |
252 | if(ret == -1) |
253 | return -1; |
254 | |
255 | if(initrd) { |
256 | struct stat statbuf; |
257 | |
258 | initrd_base = ALIGN(end); |
259 | |
260 | if(stat(initrd, &statbuf) == -1) |
261 | return -1; |
262 | |
263 | initrd_size = statbuf.st_size; |
264 | |
265 | printf("Loading initrd to 0x%08X, size 0x%08X\n", |
266 | initrd_base, initrd_size); |
267 | |
268 | |
269 | end = initrd_base + initrd_size; |
270 | |
271 | if(end > ingenic_sdram_size(ingenic) + SDRAM_BASE - STAGE2_CODESIZE) { |
272 | fputs(" Initrd doesn't fit into SDRAM\n", stderr); |
273 | |
274 | return -1; |
275 | } |
276 | |
277 | if(ingenic_load_sdram_file(ingenic, initrd_base, initrd) == -1) |
278 | return -1; |
279 | |
280 | snprintf(initrd_args, sizeof(initrd_args), |
281 | "rd_start=0x%08X rd_size=0x%08X", |
282 | initrd_base, initrd_size); |
283 | |
284 | all_args[2] = initrd_args; |
285 | } |
286 | |
287 | args_base = ALIGN(end); |
288 | |
289 | if(load_args(ingenic, args_base, filename, all_args, &argc, &end) == -1) |
290 | return -1; |
291 | |
292 | trampoline_base = ALIGN(end); |
293 | end = trampoline_base + sizeof(trampoline_template); |
294 | |
295 | uint32_t *trampoline = malloc(sizeof(trampoline_template)); |
296 | if(trampoline == NULL) |
297 | return -1; |
298 | |
299 | memcpy(trampoline, trampoline_template, sizeof(trampoline_template)); |
300 | trampoline_set(trampoline, TRAMP_ARGC, argc); |
301 | trampoline_set(trampoline, TRAMP_ARGV, args_base); |
302 | trampoline_set(trampoline, TRAMP_ARG2, 0); |
303 | trampoline_set(trampoline, TRAMP_ARG3, 0); |
304 | trampoline_set(trampoline, TRAMP_ENTRY, entry); |
305 | |
306 | ret = load_segment(ingenic, trampoline, trampoline_base, sizeof(trampoline_template), sizeof(trampoline_template)); |
307 | |
308 | free(trampoline); |
309 | |
310 | if(ret == -1) |
311 | return -1; |
312 | |
313 | printf("Image end: 0x%08X, entry: 0x%08X\n", end, entry); |
314 | |
315 | //return 0; |
316 | return ingenic_go(ingenic, trampoline_base); |
317 | } |
318 | |
319 | |