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