11913091 |
1 | // vim:shiftwidth=2:expandtab |
2 | #include <stdio.h> |
3 | #include <stdlib.h> |
3d295a9f |
4 | #include <string.h> |
11913091 |
5 | #include <sys/types.h> |
6 | #include <sys/stat.h> |
7 | #include <fcntl.h> |
8 | #include <sys/mman.h> |
9 | #include <sys/types.h> |
10 | #include <unistd.h> |
11 | #include <signal.h> |
12 | #include <asm/ucontext.h> |
13 | |
14 | #include "header.h" |
86418a84 |
15 | #include "sys_cacheflush.h" |
11913091 |
16 | |
86418a84 |
17 | //#define iolog printf |
18 | #define iolog(...) |
11913091 |
19 | |
20 | typedef unsigned int u32; |
21 | typedef unsigned short u16; |
22 | typedef unsigned char u8; |
23 | |
3d295a9f |
24 | struct uppermem_block { |
25 | u32 addr; // physical |
26 | u32 size; |
27 | void *mem; |
28 | struct uppermem_block *next; |
29 | }; |
30 | |
31 | static struct uppermem_block *upper_mem; |
32 | |
33 | static struct { |
34 | u32 dstctrl; |
35 | u32 dstaddr; |
36 | u32 dststride; |
37 | u32 srcctrl; |
38 | u32 srcaddr; // |
39 | u32 srcstride; |
40 | u32 srcforcolor; |
41 | u32 srcbackcolor; |
42 | u32 patctrl; // |
43 | u32 patforcolor; |
44 | u32 patbackcolor; |
45 | u32 size; |
46 | u32 ctrl; // |
47 | u32 run; |
48 | u32 intc; |
49 | u32 srcfifo; |
50 | } blitter; |
51 | |
52 | static struct { |
53 | union { |
54 | u32 mlc_stl_eadr; |
55 | struct { |
56 | u16 mlc_stl_eadrl; |
57 | u16 mlc_stl_eadrh; |
58 | }; |
59 | }; |
60 | } mmsp2; |
61 | |
62 | static u16 *host_screen; |
63 | static int host_stride; |
11913091 |
64 | |
65 | |
3d295a9f |
66 | static void *upper_lookup(u32 addr, u8 **mem_end, int *stride_override) |
11913091 |
67 | { |
3d295a9f |
68 | struct uppermem_block *ub; |
69 | |
70 | // maybe the screen? |
71 | if (mmsp2.mlc_stl_eadr <= addr && addr < mmsp2.mlc_stl_eadr + 320*240*2) { |
72 | host_screen = host_video_flip(); // HACK |
73 | *mem_end = (u8 *)host_screen + host_stride * 240; |
74 | *stride_override = host_stride; |
75 | return (u8 *)host_screen + addr - mmsp2.mlc_stl_eadr; |
11913091 |
76 | } |
3d295a9f |
77 | |
78 | for (ub = upper_mem; ub != NULL; ub = ub->next) { |
79 | if (ub->addr <= addr && addr < ub->addr + ub->size) { |
80 | *mem_end = (u8 *)ub->mem + ub->size; |
81 | return (u8 *)ub->mem + addr - ub->addr; |
82 | } |
11913091 |
83 | } |
3d295a9f |
84 | |
85 | return NULL; |
11913091 |
86 | } |
87 | |
3d295a9f |
88 | static void blitter_do(void) |
11913091 |
89 | { |
3d295a9f |
90 | u8 *dst, *dste, *src, *srce; |
91 | int w, h, sstrd, dstrd; |
92 | |
93 | if (blitter.srcaddr == blitter.dstaddr) |
94 | return; // dummy blit? |
95 | |
96 | w = blitter.size & 0x7ff; |
97 | h = (blitter.size >> 16) & 0x7ff; |
98 | sstrd = blitter.srcstride; |
99 | dstrd = blitter.dststride; |
100 | |
101 | dst = upper_lookup(blitter.dstaddr, &dste, &dstrd); |
102 | src = upper_lookup(blitter.srcaddr, &srce, &sstrd); |
11913091 |
103 | |
3d295a9f |
104 | if (dst == NULL || src == NULL) { |
105 | printf("blit %08x->%08x %dx%d translated to %p->%p\n", |
106 | blitter.srcaddr, blitter.dstaddr, w, h, src, dst); |
107 | return; |
108 | } |
109 | |
110 | if (dst + dstrd * h > dste) { |
111 | printf("blit %08x->%08x %dx%d did not fit dst\n", |
112 | blitter.srcaddr, blitter.dstaddr, w, h); |
113 | h = (dste - dst) / dstrd; |
114 | } |
115 | |
116 | if (src + sstrd * h > srce) { |
117 | printf("blit %08x->%08x %dx%d did not fit src\n", |
118 | blitter.srcaddr, blitter.dstaddr, w, h); |
119 | h = (srce - src) / sstrd; |
120 | } |
121 | |
122 | for (; h > 0; h--, dst += dstrd, src += sstrd) |
123 | memcpy(dst, src, w * 2); |
124 | } |
125 | |
126 | static u32 xread8(u32 a) |
127 | { |
11913091 |
128 | iolog("r8 %08x\n", a); |
3d295a9f |
129 | return 0; |
11913091 |
130 | } |
131 | |
132 | static u32 xread16(u32 a) |
133 | { |
3d295a9f |
134 | // if ((a & 0xfff00000) == 0x7f100000) { static int a; a ^= ~1; return a & 0xffff; } |
11913091 |
135 | iolog("r16 %08x\n", a); |
3d295a9f |
136 | return 0; |
11913091 |
137 | } |
138 | |
139 | static u32 xread32(u32 a) |
140 | { |
3d295a9f |
141 | u32 d = 0; |
142 | if ((a & 0xfff00000) == 0x7f100000) { |
143 | u32 *bl = &blitter.dstctrl; |
144 | a &= 0xfff; |
145 | if (a < 0x40) |
146 | d = bl[a / 4]; |
147 | if (a == 0x34) |
148 | d = 0; // not busy |
149 | } |
11913091 |
150 | iolog("r32 %08x\n", a); |
3d295a9f |
151 | return d; |
11913091 |
152 | } |
153 | |
154 | static void xwrite8(u32 a, u32 d) |
155 | { |
11913091 |
156 | iolog("w8 %08x %08x\n", a, d); |
11913091 |
157 | } |
158 | |
159 | static void xwrite16(u32 a, u32 d) |
160 | { |
11913091 |
161 | iolog("w16 %08x %08x\n", a, d); |
3d295a9f |
162 | if ((a & 0xfff00000) == 0x7f000000) { |
163 | a &= 0xffff; |
164 | switch (a) { |
165 | case 0x2912: mmsp2.mlc_stl_eadrl = d; break; |
166 | case 0x2914: mmsp2.mlc_stl_eadrh = d; break; |
167 | } |
168 | printf("w16 %08x %08x\n", a, d); |
169 | } |
11913091 |
170 | } |
171 | |
172 | static void xwrite32(u32 a, u32 d) |
173 | { |
11913091 |
174 | iolog("w32 %08x %08x\n", a, d); |
3d295a9f |
175 | if ((a & 0xfff00000) == 0x7f000000) { |
176 | printf("w32 %08x %08x\n", a, d); |
177 | return; |
178 | } |
179 | if ((a & 0xfff00000) == 0x7f100000) { |
180 | u32 *bl = &blitter.dstctrl; |
181 | a &= 0xfff; |
182 | if (a < 0x40) |
183 | bl[a / 4] = d; |
184 | if (a == 0x34 && (d & 1)) |
185 | blitter_do(); |
186 | return; |
187 | } |
11913091 |
188 | } |
189 | |
190 | #define BIT_SET(v, b) (v & (1 << (b))) |
191 | |
86418a84 |
192 | static void handle_op(u32 pc, u32 op, u32 *regs, u32 addr_check) |
11913091 |
193 | { |
194 | u32 t, shift, ret, addr; |
195 | int rn, rd; |
196 | |
197 | rd = (op & 0x0000f000) >> 12; |
198 | rn = (op & 0x000f0000) >> 16; |
199 | |
200 | if ((op & 0x0f200090) == 0x01000090) { // AM3: LDRH, STRH |
86418a84 |
201 | if (!BIT_SET(op, 5)) // !H |
202 | goto unhandled; |
203 | if (BIT_SET(op, 6) && !BIT_SET(op, 20)) // S && !L |
11913091 |
204 | goto unhandled; |
205 | |
206 | if (BIT_SET(op, 22)) // imm offset |
207 | t = ((op & 0xf00) >> 4) | (op & 0x0f); |
208 | else // reg offset |
209 | t = regs[op & 0x000f]; |
210 | |
211 | if (!BIT_SET(op, 23)) |
212 | t = -t; |
213 | addr = regs[rn] + t; |
214 | |
215 | if (BIT_SET(op, 20)) { // Load |
216 | ret = xread16(addr); |
86418a84 |
217 | if (BIT_SET(op, 6)) { // S |
218 | ret <<= 16; |
219 | ret = (signed int)ret >> 16; |
220 | } |
11913091 |
221 | regs[rd] = ret; |
222 | } |
223 | else |
224 | xwrite16(addr, regs[rd]); |
225 | } |
226 | else if ((op & 0x0d200000) == 0x05000000) { // AM2: LDR[B], STR[B] |
227 | if (BIT_SET(op, 25)) { // reg offs |
228 | if (BIT_SET(op, 4)) |
229 | goto unhandled; |
230 | |
231 | t = regs[op & 0x000f]; |
232 | shift = (op & 0x0f80) >> 7; |
233 | switch ((op & 0x0060) >> 5) { |
234 | case 0: t = t << shift; break; |
235 | case 1: t = t >> (shift + 1); break; |
236 | case 2: t = (signed int)t >> (shift + 1); break; |
237 | case 3: goto unhandled; // I'm just lazy |
238 | } |
239 | } |
240 | else // imm offs |
241 | t = op & 0x0fff; |
242 | |
243 | if (!BIT_SET(op, 23)) |
244 | t = -t; |
245 | addr = regs[rn] + t; |
246 | |
247 | if (BIT_SET(op, 20)) { // Load |
248 | if (BIT_SET(op, 22)) // Byte |
249 | ret = xread8(addr); |
250 | else |
251 | ret = xread32(addr); |
252 | regs[rd] = ret; |
253 | } |
254 | else { |
255 | if (BIT_SET(op, 22)) // Byte |
256 | xwrite8(addr, regs[rd]); |
257 | else |
258 | xwrite32(addr, regs[rd]); |
259 | } |
260 | } |
261 | else |
262 | goto unhandled; |
263 | |
86418a84 |
264 | #if 0 |
11913091 |
265 | if (addr != addr_check) { |
266 | fprintf(stderr, "bad calculated addr: %08x vs %08x\n", addr, addr_check); |
267 | abort(); |
268 | } |
86418a84 |
269 | #endif |
11913091 |
270 | return; |
271 | |
272 | unhandled: |
86418a84 |
273 | fprintf(stderr, "unhandled IO op %08x @ %08x\n", op, pc); |
11913091 |
274 | } |
275 | |
276 | #define LINKPAGE_SIZE 0x1000 |
86418a84 |
277 | #define LINKPAGE_COUNT 4 |
278 | #define LINKPAGE_ALLOC (LINKPAGE_SIZE * LINKPAGE_COUNT) |
11913091 |
279 | |
280 | struct linkpage { |
86418a84 |
281 | u32 saved_regs[15]; |
282 | u32 *lp_r1; |
283 | void (*handler)(u32 addr_pc, u32 op, u32 *regs, u32 addr_check); |
11913091 |
284 | u32 code[0]; |
285 | }; |
286 | |
287 | static struct linkpage *g_linkpage; |
288 | static u32 *g_code_ptr; |
86418a84 |
289 | static int g_linkpage_count; |
290 | |
291 | static void init_linkpage(void) |
292 | { |
293 | g_linkpage->lp_r1 = &g_linkpage->saved_regs[1]; |
294 | g_linkpage->handler = handle_op; |
295 | g_code_ptr = g_linkpage->code; |
296 | } |
297 | |
298 | static u32 make_offset12(u32 *pc, u32 *target) |
299 | { |
300 | int lp_offs, u = 1; |
301 | |
302 | lp_offs = (char *)target - (char *)pc - 2*4; |
303 | if (lp_offs < 0) { |
304 | lp_offs = -lp_offs; |
305 | u = 0; |
306 | } |
307 | if (lp_offs >= LINKPAGE_SIZE) { |
308 | fprintf(stderr, "linkpage too far: %d\n", lp_offs); |
309 | abort(); |
310 | } |
311 | |
312 | return (u << 23) | lp_offs; |
313 | } |
314 | |
315 | static u32 make_jmp(u32 *pc, u32 *target) |
316 | { |
317 | int jmp_val; |
318 | |
319 | jmp_val = target - pc - 2; |
320 | if (jmp_val < (int)0xff000000 || jmp_val > 0x00ffffff) { |
321 | fprintf(stderr, "jump out of range (%p -> %p)\n", pc, target); |
322 | abort(); |
323 | } |
324 | |
325 | return 0xea000000 | (jmp_val & 0x00ffffff); |
326 | } |
327 | |
328 | static void emit_op(u32 op) |
329 | { |
330 | *g_code_ptr++ = op; |
331 | } |
332 | |
333 | static void emit_op_io(u32 op, u32 *target) |
334 | { |
335 | op |= make_offset12(g_code_ptr, target); |
336 | emit_op(op); |
337 | } |
338 | |
339 | static void segv_sigaction(int num, siginfo_t *info, void *ctx) |
340 | { |
341 | struct ucontext *context = ctx; |
342 | u32 *regs = (u32 *)&context->uc_mcontext.arm_r0; |
343 | u32 *pc = (u32 *)regs[15]; |
344 | u32 old_op = *pc; |
345 | u32 *pc_ptr, *old_op_ptr; |
346 | int lp_size; |
347 | |
348 | if (((regs[15] ^ (u32)&segv_sigaction) & 0xff000000) == 0 || // PC is in our segment or |
349 | (((regs[15] ^ (u32)g_linkpage) & ~(LINKPAGE_ALLOC - 1)) == 0)) // .. in linkpage |
350 | { |
351 | // real crash - time to die |
352 | printf("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]); |
353 | signal(num, SIG_DFL); |
354 | raise(num); |
355 | } |
356 | printf("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]); |
357 | |
358 | // spit PC and op |
359 | pc_ptr = g_code_ptr++; |
360 | old_op_ptr = g_code_ptr++; |
361 | *pc_ptr = (u32)pc; |
362 | *old_op_ptr = old_op; |
363 | |
364 | // emit jump to code ptr |
365 | *pc = make_jmp(pc, g_code_ptr); |
366 | |
367 | // generate code: |
368 | // TODO: our own stack |
369 | emit_op_io(0xe50f0000, &g_linkpage->saved_regs[0]); // str r0, [saved_regs[0]] @ save r0 |
370 | emit_op_io(0xe51f0000, (u32 *)&g_linkpage->lp_r1); // ldr r0, =lp_r1 |
371 | emit_op (0xe8807ffe); // stmia r0, {r1-r14} |
372 | emit_op (0xe2402004); // sub r2, r0, #4 |
373 | emit_op_io(0xe51f0000, pc_ptr); // ldr r0, =pc |
374 | emit_op_io(0xe51f1000, old_op_ptr); // ldr r1, =old_op |
375 | emit_op (0xe1a04002); // mov r4, r2 |
376 | emit_op (0xe1a0e00f); // mov lr, pc |
377 | emit_op_io(0xe51ff000, (u32 *)&g_linkpage->handler); // ldr pc, =handle_op |
378 | emit_op (0xe8947fff); // ldmia r4, {r0-r14} |
379 | emit_op (make_jmp(g_code_ptr, pc + 1)); // jmp <back> |
380 | |
381 | // sync caches |
382 | sys_cacheflush(pc, pc + 1); |
383 | sys_cacheflush(g_linkpage, g_code_ptr); |
384 | |
385 | lp_size = (char *)g_code_ptr - (char *)g_linkpage; |
386 | printf("code #%d %d/%d\n", g_linkpage_count, lp_size, LINKPAGE_SIZE); |
387 | |
388 | if (lp_size + 13*4 > LINKPAGE_SIZE) { |
389 | g_linkpage_count++; |
390 | if (g_linkpage_count >= LINKPAGE_COUNT) { |
391 | fprintf(stderr, "too many linkpages needed\n"); |
392 | abort(); |
393 | } |
394 | g_linkpage = (void *)((char *)g_linkpage + LINKPAGE_SIZE); |
395 | init_linkpage(); |
396 | } |
397 | //handle_op(regs[15], op, regs, (u32)info->si_addr); |
398 | //regs[15] += 4; |
399 | } |
11913091 |
400 | |
401 | void emu_init(void *map_bottom) |
402 | { |
403 | struct sigaction segv_action = { |
404 | .sa_sigaction = segv_sigaction, |
405 | .sa_flags = SA_SIGINFO, |
406 | }; |
11913091 |
407 | void *ret; |
408 | |
409 | sigemptyset(&segv_action.sa_mask); |
410 | sigaction(SIGSEGV, &segv_action, NULL); |
411 | |
86418a84 |
412 | g_linkpage = (void *)(((u32)map_bottom - LINKPAGE_ALLOC) & ~0xfff); |
413 | ret = mmap(g_linkpage, LINKPAGE_ALLOC, PROT_READ|PROT_WRITE, MAP_PRIVATE|MAP_ANONYMOUS, -1, 0); |
11913091 |
414 | if (ret != g_linkpage) { |
415 | perror("mmap linkpage"); |
416 | exit(1); |
417 | } |
86418a84 |
418 | printf("linkpages @ %p\n", g_linkpage); |
419 | init_linkpage(); |
11913091 |
420 | |
3d295a9f |
421 | // host stuff |
422 | host_screen = host_video_init(&host_stride); |
423 | if (host_screen == NULL) { |
424 | printf("can't alloc screen\n"); |
425 | exit(1); |
426 | } |
427 | } |
428 | |
429 | int emu_read_gpiodev(void *buf, int count) |
430 | { |
431 | unsigned int btns; |
432 | |
433 | if (count < 4) { |
434 | printf("gpiodev read %d?\n", count); |
435 | return -1; |
436 | } |
437 | |
438 | btns = host_read_btns(); |
439 | memcpy(buf, &btns, 4); |
440 | return 4; |
11913091 |
441 | } |
442 | |
443 | void *emu_mmap_dev(unsigned int length, int prot, int flags, unsigned int offset) |
444 | { |
3d295a9f |
445 | struct uppermem_block *umem; |
11913091 |
446 | char name[32]; |
447 | int fd; |
448 | |
3d295a9f |
449 | // SoC regs |
11913091 |
450 | if ((offset & ~0xffff) == 0xc0000000) { |
451 | return mmap((void *)0x7f000000, length, PROT_NONE, |
452 | MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0); |
453 | } |
3d295a9f |
454 | // blitter |
11913091 |
455 | if ((offset & ~0xffff) == 0xe0020000) { |
456 | return mmap((void *)0x7f100000, length, PROT_NONE, |
457 | MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0); |
458 | } |
3d295a9f |
459 | // upper mem |
460 | if ((offset & 0xfe000000) != 0x02000000) |
461 | printf("unexpected devmem mmap @ %08x\n", offset); |
11913091 |
462 | |
3d295a9f |
463 | // return mmap(NULL, length, prot, flags, memdev, offset); |
464 | |
465 | umem = calloc(1, sizeof(*umem)); |
466 | if (umem == NULL) { |
467 | printf("OOM\n"); |
468 | return MAP_FAILED; |
469 | } |
470 | |
471 | umem->addr = offset; |
472 | umem->size = length; |
473 | umem->mem = mmap(NULL, length, prot, MAP_PRIVATE|MAP_ANONYMOUS, -1, 0); |
474 | if (umem->mem != MAP_FAILED) |
475 | goto done; |
476 | |
477 | printf("upper mem @ %08x %d mmap fail, trying backing file\n", offset, length); |
11913091 |
478 | sprintf(name, "m%08x", offset); |
479 | fd = open(name, O_CREAT|O_RDWR, 0644); |
480 | lseek(fd, length - 1, SEEK_SET); |
481 | name[0] = 0; |
482 | write(fd, name, 1); |
483 | |
3d295a9f |
484 | umem->mem = mmap(NULL, length, prot, MAP_SHARED, fd, 0); |
485 | if (umem->mem == MAP_FAILED) { |
486 | printf("failed, giving up\n"); |
487 | close(fd); |
488 | free(umem); |
489 | return MAP_FAILED; |
490 | } |
491 | |
492 | done: |
493 | printf("upper mem @ %08x %d\n", offset, length); |
494 | umem->next = upper_mem; |
495 | upper_mem = umem; |
496 | return umem->mem; |
11913091 |
497 | } |
498 | |