1 // vim:shiftwidth=2:expandtab
12 #include <asm/ucontext.h>
15 #include "sys_cacheflush.h"
17 //#define iolog printf
19 //#define segvlog printf
22 typedef unsigned int u32;
23 typedef unsigned short u16;
24 typedef unsigned char u8;
26 struct uppermem_block {
30 struct uppermem_block *next;
33 static struct uppermem_block *upper_mem;
54 #define SRCCTRL_INVIDEO (1 << 8)
55 #define SRCCTRL_SRCENB (1 << 7)
56 #define CTRL_TRANSPARENCYENB (1 << 11)
68 static u16 *host_screen;
69 static int host_stride;
72 static void memset16(void *dst, u32 pattern, int count)
77 d = (u16 *)((long)dst & ~1);
83 pattern |= pattern << 16;
93 static void blt_tr(void *dst, void *src, u32 trc, int w)
95 u16 *d = (u16 *)((long)dst & ~1);
96 u16 *s = (u16 *)((long)src & ~1);
99 for (; w > 0; d++, s++, w--)
104 #define dump_blitter() \
106 u32 *r = &blitter.dstctrl; \
108 for (i = 0; i < 4*4; i++, r++) { \
109 printf("%08x ", *r); \
115 static void *upper_lookup(u32 addr, u8 **mem_end, int *stride_override)
117 struct uppermem_block *ub;
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;
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;
137 static void blitter_do(void)
139 u8 *dst, *dste, *src = NULL, *srce = NULL;
140 int w, h, sstrd, dstrd;
143 w = blitter.size & 0x7ff;
144 h = (blitter.size >> 16) & 0x7ff;
145 sstrd = blitter.srcstride;
146 dstrd = blitter.dststride;
148 // XXX: need to confirm this..
149 addr = (blitter.dstaddr & ~3) | ((blitter.dstctrl & 0x1f) >> 3);
150 dst = upper_lookup(addr, &dste, &dstrd);
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))
159 addr = (blitter.srcaddr & ~3) | ((blitter.srcctrl & 0x1f) >> 3);
160 src = upper_lookup(addr, &srce, &sstrd);
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;
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;
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);
185 for (; h > 0; h--, dst += dstrd, src += sstrd)
186 memcpy(dst, src, w * 2);
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);
198 printf("blit %08x->%08x %dx%d translated to %p->%p\n",
199 blitter.srcaddr, blitter.dstaddr, w, h, src, dst);
203 static u32 xread8(u32 a)
205 iolog("r8 %08x\n", a);
209 static u32 xread16(u32 a)
211 // if ((a & 0xfff00000) == 0x7f100000) { static int a; a ^= ~1; return a & 0xffff; }
212 iolog("r16 %08x\n", a);
216 static u32 xread32(u32 a)
219 if ((a & 0xfff00000) == 0x7f100000) {
220 u32 *bl = &blitter.dstctrl;
227 iolog("r32 %08x\n", a);
231 static void xwrite8(u32 a, u32 d)
233 iolog("w8 %08x %08x\n", a, d);
236 static void xwrite16(u32 a, u32 d)
238 iolog("w16 %08x %08x\n", a, d);
239 if ((a & 0xfff00000) == 0x7f000000) {
242 case 0x2912: mmsp2.mlc_stl_eadrl = d; break;
243 case 0x2914: mmsp2.mlc_stl_eadrh = d; break;
245 //printf("w16 %08x %08x\n", a, d);
249 static void xwrite32(u32 a, u32 d)
251 iolog("w32 %08x %08x\n", a, d);
252 if ((a & 0xfff00000) == 0x7f000000) {
253 printf("w32 %08x %08x\n", a, d);
256 if ((a & 0xfff00000) == 0x7f100000) {
257 u32 *bl = &blitter.dstctrl;
261 if (a == 0x34 && (d & 1))
267 #define BIT_SET(v, b) (v & (1 << (b)))
269 static void handle_op(u32 pc, u32 op, u32 *regs, u32 addr_check)
271 u32 t, shift, ret, addr;
274 rd = (op & 0x0000f000) >> 12;
275 rn = (op & 0x000f0000) >> 16;
277 if ((op & 0x0f200090) == 0x01000090) { // AM3: LDRH, STRH
278 if (!BIT_SET(op, 5)) // !H
280 if (BIT_SET(op, 6) && !BIT_SET(op, 20)) // S && !L
283 if (BIT_SET(op, 22)) // imm offset
284 t = ((op & 0xf00) >> 4) | (op & 0x0f);
286 t = regs[op & 0x000f];
288 if (!BIT_SET(op, 23))
292 if (BIT_SET(op, 20)) { // Load
294 if (BIT_SET(op, 6)) { // S
296 ret = (signed int)ret >> 16;
301 xwrite16(addr, regs[rd]);
303 else if ((op & 0x0d200000) == 0x05000000) { // AM2: LDR[B], STR[B]
304 if (BIT_SET(op, 25)) { // reg offs
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
320 if (!BIT_SET(op, 23))
324 if (BIT_SET(op, 20)) { // Load
325 if (BIT_SET(op, 22)) // Byte
332 if (BIT_SET(op, 22)) // Byte
333 xwrite8(addr, regs[rd]);
335 xwrite32(addr, regs[rd]);
342 if (addr != addr_check) {
343 fprintf(stderr, "bad calculated addr: %08x vs %08x\n", addr, addr_check);
350 fprintf(stderr, "unhandled IO op %08x @ %08x\n", op, pc);
353 #define LINKPAGE_SIZE 0x1000
354 #define LINKPAGE_COUNT 4
355 #define LINKPAGE_ALLOC (LINKPAGE_SIZE * LINKPAGE_COUNT)
360 void (*handler)(u32 addr_pc, u32 op, u32 *regs, u32 addr_check);
364 static struct linkpage *g_linkpage;
365 static u32 *g_code_ptr;
366 static int g_linkpage_count;
368 static void init_linkpage(void)
370 g_linkpage->lp_r1 = &g_linkpage->saved_regs[1];
371 g_linkpage->handler = handle_op;
372 g_code_ptr = g_linkpage->code;
375 static u32 make_offset12(u32 *pc, u32 *target)
379 lp_offs = (char *)target - (char *)pc - 2*4;
384 if (lp_offs >= LINKPAGE_SIZE) {
385 fprintf(stderr, "linkpage too far: %d\n", lp_offs);
389 return (u << 23) | lp_offs;
392 static u32 make_jmp(u32 *pc, u32 *target)
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);
402 return 0xea000000 | (jmp_val & 0x00ffffff);
405 static void emit_op(u32 op)
410 static void emit_op_io(u32 op, u32 *target)
412 op |= make_offset12(g_code_ptr, target);
416 static void segv_sigaction(int num, siginfo_t *info, void *ctx)
418 struct ucontext *context = ctx;
419 u32 *regs = (u32 *)&context->uc_mcontext.arm_r0;
420 u32 *pc = (u32 *)regs[15];
422 u32 *pc_ptr, *old_op_ptr;
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
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);
433 segvlog("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]);
436 pc_ptr = g_code_ptr++;
437 old_op_ptr = g_code_ptr++;
439 *old_op_ptr = old_op;
441 // emit jump to code ptr
442 *pc = make_jmp(pc, g_code_ptr);
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>
459 sys_cacheflush(pc, pc + 1);
460 sys_cacheflush(g_linkpage, g_code_ptr);
462 lp_size = (char *)g_code_ptr - (char *)g_linkpage;
463 segvlog("code #%d %d/%d\n", g_linkpage_count, lp_size, LINKPAGE_SIZE);
465 if (lp_size + 13*4 > LINKPAGE_SIZE) {
467 if (g_linkpage_count >= LINKPAGE_COUNT) {
468 fprintf(stderr, "too many linkpages needed\n");
471 g_linkpage = (void *)((char *)g_linkpage + LINKPAGE_SIZE);
474 //handle_op(regs[15], op, regs, (u32)info->si_addr);
478 void emu_init(void *map_bottom)
480 struct sigaction segv_action = {
481 .sa_sigaction = segv_sigaction,
482 .sa_flags = SA_SIGINFO,
486 sigemptyset(&segv_action.sa_mask);
487 sigaction(SIGSEGV, &segv_action, NULL);
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);
491 if (ret != g_linkpage) {
492 perror("mmap linkpage");
495 printf("linkpages @ %p\n", g_linkpage);
499 ret = host_video_init(&host_stride, 0);
501 printf("can't alloc screen\n");
504 host_screen = host_video_flip();
507 int emu_read_gpiodev(void *buf, int count)
512 printf("gpiodev read %d?\n", count);
516 btns = host_read_btns();
517 memcpy(buf, &btns, 4);
521 void *emu_mmap_dev(unsigned int length, int prot, int flags, unsigned int offset)
523 struct uppermem_block *umem;
528 if ((offset & ~0xffff) == 0xc0000000) {
529 return mmap((void *)0x7f000000, length, PROT_NONE,
530 MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0);
533 if ((offset & ~0xffff) == 0xe0020000) {
534 return mmap((void *)0x7f100000, length, PROT_NONE,
535 MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0);
538 if ((offset & 0xfe000000) != 0x02000000)
539 printf("unexpected devmem mmap @ %08x\n", offset);
541 // return mmap(NULL, length, prot, flags, memdev, offset);
543 umem = calloc(1, sizeof(*umem));
551 umem->mem = mmap(NULL, length, prot, MAP_PRIVATE|MAP_ANONYMOUS, -1, 0);
552 if (umem->mem != MAP_FAILED)
555 printf("upper mem @ %08x %d mmap fail, trying backing file\n", offset, length);
556 sprintf(name, "m%08x", offset);
557 fd = open(name, O_CREAT|O_RDWR, 0644);
558 lseek(fd, length - 1, SEEK_SET);
562 umem->mem = mmap(NULL, length, prot, MAP_SHARED, fd, 0);
563 if (umem->mem == MAP_FAILED) {
564 printf("failed, giving up\n");
571 printf("upper mem @ %08x %d\n", offset, length);
572 umem->next = upper_mem;