1 // vim:shiftwidth=2:expandtab
12 #include <asm/ucontext.h>
15 #include "sys_cacheflush.h"
28 #define iolog_unh log_io
30 #define iolog_unh(...)
34 #define segvlog printf
39 #if defined(LOG_IO) || defined(LOG_IO_UNH)
40 #include "mmsp2-regs.h"
43 typedef unsigned int u32;
44 typedef unsigned short u16;
45 typedef unsigned char u8;
47 struct uppermem_block {
51 struct uppermem_block *next;
54 static struct uppermem_block *upper_mem;
75 #define SRCCTRL_INVIDEO (1 << 8)
76 #define SRCCTRL_SRCENB (1 << 7)
77 #define CTRL_TRANSPARENCYENB (1 << 11)
90 u16 mlc_stl_pallt_d[256*2];
91 u32 mlc_stl_pallt_d32[256];
97 u32 btn_state; // as seen through /dev/GPIO
101 static u16 *host_screen;
102 static int host_stride;
105 #if defined(LOG_IO) || defined(LOG_IO_UNH)
106 static void log_io(const char *pfx, u32 a, u32 d, int size)
108 const char *fmt, *reg = "";
110 case 8: fmt = "%s %08x %02x %s\n"; break;
111 case 32: fmt = "%s %08x %08x %s\n"; break;
112 default: fmt = "%s %08x %04x %s\n"; break;
115 if ((a & ~0xffff) == 0x7f000000)
116 reg = regnames[a & 0xffff];
118 printf(fmt, pfx, a, d, reg);
122 static void memset16(void *dst, u32 pattern, int count)
127 d = (u16 *)((long)dst & ~1);
133 pattern |= pattern << 16;
140 *(u16 *)dl = pattern;
143 static void blt_tr(void *dst, void *src, u32 trc, int w)
145 u16 *d = (u16 *)((long)dst & ~1);
146 u16 *s = (u16 *)((long)src & ~1);
149 for (; w > 0; d++, s++, w--)
154 #define dump_blitter() \
156 u32 *r = &blitter.dstctrl; \
158 for (i = 0; i < 4*4; i++, r++) { \
159 printf("%08x ", *r); \
165 static void *uppermem_lookup(u32 addr, u8 **mem_end)
167 struct uppermem_block *ub;
169 for (ub = upper_mem; ub != NULL; ub = ub->next) {
170 if (ub->addr <= addr && addr < ub->addr + ub->size) {
171 *mem_end = (u8 *)ub->mem + ub->size;
172 return (u8 *)ub->mem + addr - ub->addr;
179 static void *blitter_mem_lookup(u32 addr, u8 **mem_end, int *stride_override, int *to_screen)
182 if (mmsp2.mlc_stl_adr <= addr && addr < mmsp2.mlc_stl_adr + 320*240*2) {
183 *mem_end = (u8 *)host_screen + host_stride * 240;
184 *stride_override = host_stride;
186 return (u8 *)host_screen + addr - mmsp2.mlc_stl_adr;
189 return uppermem_lookup(addr, mem_end);
192 static void blitter_do(void)
194 u8 *dst, *dste, *src = NULL, *srce = NULL;
195 int w, h, sstrd, dstrd;
199 w = blitter.size & 0x7ff;
200 h = (blitter.size >> 16) & 0x7ff;
201 sstrd = blitter.srcstride;
202 dstrd = blitter.dststride;
204 // XXX: need to confirm this..
205 addr = (blitter.dstaddr & ~3) | ((blitter.dstctrl & 0x1f) >> 3);
206 dst = blitter_mem_lookup(addr, &dste, &dstrd, &to_screen);
210 // XXX: assume fill if no SRCENB, but it could be pattern blit..
211 if (blitter.srcctrl & SRCCTRL_SRCENB) {
212 if (!(blitter.srcctrl & SRCCTRL_INVIDEO))
215 addr = (blitter.srcaddr & ~3) | ((blitter.srcctrl & 0x1f) >> 3);
216 src = blitter_mem_lookup(addr, &srce, &sstrd, &to_screen);
220 if (src + sstrd * h > srce) {
221 err("blit %08x->%08x %dx%d did not fit src\n",
222 blitter.srcaddr, blitter.dstaddr, w, h);
223 h = (srce - src) / sstrd;
227 if (dst + dstrd * h > dste) {
228 err("blit %08x->%08x %dx%d did not fit dst\n",
229 blitter.srcaddr, blitter.dstaddr, w, h);
230 h = (dste - dst) / dstrd;
235 if (blitter.ctrl & CTRL_TRANSPARENCYENB) {
236 u32 trc = blitter.ctrl >> 16;
237 for (; h > 0; h--, dst += dstrd, src += sstrd)
238 blt_tr(dst, src, trc, w);
241 for (; h > 0; h--, dst += dstrd, src += sstrd)
242 memcpy(dst, src, w * 2);
246 // fill. Assume the pattern is cleared and bg color is used
247 u32 bgc = blitter.patbackcolor & 0xffff;
248 for (; h > 0; h--, dst += dstrd)
249 memset16(dst, bgc, w);
253 host_screen = host_video_flip();
257 err("blit %08x->%08x %dx%d translated to %p->%p\n",
258 blitter.srcaddr, blitter.dstaddr, w, h, src, dst);
262 // TODO: hw scaler stuff
263 static void mlc_flip(u32 addr)
265 int mode = (mmsp2.mlc_stl_cntl >> 9) & 3;
266 int bpp = mode ? mode * 8 : 4;
267 u16 *dst = host_screen;
268 u16 *hpal = mmsp2.host_pal;
272 src = uppermem_lookup(addr, &src_end);
273 if (src == NULL || src + 320*240 * bpp / 8 > src_end) {
274 err("mlc_flip: %08x is out of range\n", addr);
278 if (bpp <= 8 && mmsp2.dirty_pal) {
279 u32 *srcp = mmsp2.mlc_stl_pallt_d32;
282 for (i = 0; i < 256; i++, srcp++, dstp++) {
284 *dstp = ((t >> 8) & 0xf800) | ((t >> 5) & 0x07e0) | ((t >> 3) & 0x001f);
291 for (i = 0; i < 240; i++, dst += host_stride / 2 - 320) {
292 for (u = 320 / 2; u > 0; u--, src++) {
293 *dst++ = hpal[*src >> 4];
294 *dst++ = hpal[*src & 0x0f];
300 for (i = 0; i < 240; i++, dst += host_stride / 2 - 320) {
301 for (u = 320 / 4; u > 0; u--) {
302 *dst++ = hpal[*src++];
303 *dst++ = hpal[*src++];
304 *dst++ = hpal[*src++];
305 *dst++ = hpal[*src++];
311 for (i = 0; i < 240; i++, dst += host_stride / 2, src += 320*2)
312 memcpy(dst, src, 320*2);
320 host_screen = host_video_flip();
323 static u32 xread8(u32 a)
325 iolog("r8 ", a, 0, 8);
326 iolog_unh("r8 ", a, 0, 8);
330 static u32 xread16(u32 a)
332 static u32 fudge, old_a;
335 if ((a & 0xffff0000) == 0x7f000000) {
343 // 0000 P000 VuVd00 0000 YXBA RLSeSt 0R0D 0L0U
344 // | GPIOD |GPIOC[8:15]|GPIOM[0:7]|
347 case 0x1184: // GPIOC
348 d = ~mmsp2.btn_state & 0xff00;
351 case 0x1186: // GPIOD
352 t = ~mmsp2.btn_state;
353 d = (t >> 9) & 0x0080;
354 d |= (t >> 11) & 0x0040;
355 d |= (t >> 7) & 0x0800;
358 case 0x1198: // GPIOM
359 mmsp2.btn_state = host_read_btns();
360 d = ~mmsp2.btn_state & 0xff;
364 d = mmsp2.mlc_stl_cntl;
367 d = mmsp2.mlc_stl_pallt_a;
381 iolog_unh("r16", a, d & 0xffff, 16);
385 iolog("r16", a, d, 16);
389 static u32 xread32(u32 a)
392 if ((a & 0xfff00000) == 0x7f100000) {
393 u32 *bl = &blitter.dstctrl;
402 iolog_unh("r32", a, d, 32);
405 iolog("r32", a, d, 32);
409 static void xwrite8(u32 a, u32 d)
411 iolog("w8 ", a, d, 8);
412 iolog_unh("w8 ", a, d, 8);
415 static void xwrite16(u32 a, u32 d)
417 iolog("w16", a, d, 16);
418 if ((a & 0xfff00000) == 0x7f000000) {
422 mmsp2.mlc_stl_cntl = d | 0xaa;
426 // odd addresses don't affect LCD. What about TV?
429 mmsp2.mlc_stl_adrl = d;
432 mmsp2.mlc_stl_adrh = d;
433 if (mmsp2.mlc_stl_adr != mmsp2.old_mlc_stl_adr)
434 mlc_flip(mmsp2.mlc_stl_adr);
435 mmsp2.old_mlc_stl_adr = mmsp2.mlc_stl_adr;
438 mmsp2.mlc_stl_pallt_a = d & 0x1ff;
441 mmsp2.mlc_stl_pallt_d[mmsp2.mlc_stl_pallt_a++] = d;
442 mmsp2.mlc_stl_pallt_a &= 0x1ff;
447 iolog_unh("w16", a, d, 16);
450 static void xwrite32(u32 a, u32 d)
452 iolog("w32", a, d, 32);
454 if ((a & 0xfff00000) == 0x7f100000) {
455 u32 *bl = &blitter.dstctrl;
459 if (a_ == 0x34 && (d & 1))
464 iolog_unh("w32", a, d, 32);
467 #define LINKPAGE_SIZE 0x1000
468 #define LINKPAGE_COUNT 4
469 #define LINKPAGE_ALLOC (LINKPAGE_SIZE * LINKPAGE_COUNT)
481 void (*handler)(struct op_context *op_ctx);
485 static struct linkpage *g_linkpage;
486 static u32 *g_code_ptr;
487 static int g_linkpage_count;
489 #define HANDLER_STACK_SIZE 4096
490 static void *g_handler_stack_end;
492 #define BIT_SET(v, b) (v & (1 << (b)))
494 static void handle_op(struct op_context *op_ctx)
496 u32 *regs = g_linkpage->saved_regs;
498 u32 t, shift, ret, addr;
501 rd = (op & 0x0000f000) >> 12;
502 rn = (op & 0x000f0000) >> 16;
504 if ((op & 0x0f200090) == 0x01000090) { // AM3: LDRH, STRH
505 if (!BIT_SET(op, 5)) // !H
507 if (BIT_SET(op, 6) && !BIT_SET(op, 20)) // S && !L
510 if (BIT_SET(op, 22)) // imm offset
511 t = ((op & 0xf00) >> 4) | (op & 0x0f);
513 t = regs[op & 0x000f];
515 if (!BIT_SET(op, 23))
519 if (BIT_SET(op, 20)) { // Load
521 if (BIT_SET(op, 6)) { // S
523 ret = (signed int)ret >> 16;
528 xwrite16(addr, regs[rd]);
530 else if ((op & 0x0d200000) == 0x05000000) { // AM2: LDR[B], STR[B]
531 if (BIT_SET(op, 25)) { // reg offs
535 t = regs[op & 0x000f];
536 shift = (op & 0x0f80) >> 7;
537 switch ((op & 0x0060) >> 5) {
538 case 0: t = t << shift; break;
539 case 1: t = t >> (shift + 1); break;
540 case 2: t = (signed int)t >> (shift + 1); break;
541 case 3: goto unhandled; // I'm just lazy
547 if (!BIT_SET(op, 23))
551 if (BIT_SET(op, 20)) { // Load
552 if (BIT_SET(op, 22)) // Byte
559 if (BIT_SET(op, 22)) // Byte
560 xwrite8(addr, regs[rd]);
562 xwrite32(addr, regs[rd]);
569 if (addr != addr_check) {
570 fprintf(stderr, "bad calculated addr: %08x vs %08x\n", addr, addr_check);
577 err("unhandled IO op %08x @ %08x\n", op, op_ctx->pc);
580 static u32 make_offset12(u32 *pc, u32 *target)
584 lp_offs = (char *)target - (char *)pc - 2*4;
589 if (lp_offs >= LINKPAGE_SIZE) {
590 err("linkpage too far: %d\n", lp_offs);
594 return (u << 23) | lp_offs;
597 static u32 make_jmp(u32 *pc, u32 *target, int bl)
601 jmp_val = target - pc - 2;
602 if (jmp_val < (int)0xff000000 || jmp_val > 0x00ffffff) {
603 err("jump out of range (%p -> %p)\n", pc, target);
607 return 0xea000000 | (bl << 24) | (jmp_val & 0x00ffffff);
610 static void emit_op(u32 op)
615 static void emit_op_io(u32 op, u32 *target)
617 op |= make_offset12(g_code_ptr, target);
621 static void init_linkpage(void)
623 g_linkpage->handler = handle_op;
624 g_linkpage->handler_stack = g_handler_stack_end;
625 g_code_ptr = g_linkpage->code;
628 // r0 and r14 must be saved by caller, r0 is arg for handle_op
629 // on return everything is restored except lr, which is used to return
630 emit_op_io(0xe50f1000, &g_linkpage->saved_regs[1]); // str r1, [->saved_regs[1]] @ save r1
631 emit_op (0xe24f1000 + // sub r1, pc, =offs(saved_regs[2])
632 (g_code_ptr - &g_linkpage->saved_regs[2] + 2) * 4);
633 emit_op (0xe8813ffc); // stmia r1, {r2-r13}
634 emit_op_io(0xe51fd000, // ldr sp, [->handler_stack]
635 (u32 *)&g_linkpage->handler_stack);
636 emit_op (0xe2414008); // sub r4, r1, #4*2
637 emit_op (0xe10f1000); // mrs r1, cpsr
638 emit_op_io(0xe50f1000, &g_linkpage->cpsr); // str r1, [->cpsr]
639 emit_op (0xe1a0500e); // mov r5, lr
640 emit_op (0xe1a0e00f); // mov lr, pc
641 emit_op_io(0xe51ff000, (u32 *)&g_linkpage->handler); // ldr pc, =handle_op
642 emit_op_io(0xe51f1000, &g_linkpage->cpsr); // ldr r1, [->cpsr]
643 emit_op (0xe128f001); // msr cpsr_f, r1
644 emit_op (0xe1a0e005); // mov lr, r5
645 emit_op (0xe8943fff); // ldmia r4, {r0-r13}
646 emit_op (0xe12fff1e); // bx lr @ return
649 static void segv_sigaction(int num, siginfo_t *info, void *ctx)
651 struct ucontext *context = ctx;
652 u32 *regs = (u32 *)&context->uc_mcontext.arm_r0;
653 u32 *pc = (u32 *)regs[15];
654 struct op_context *op_ctx;
657 if (((regs[15] ^ (u32)&segv_sigaction) & 0xff000000) == 0 || // PC is in our segment or
658 (((regs[15] ^ (u32)g_linkpage) & ~(LINKPAGE_ALLOC - 1)) == 0) || // .. in linkpage
659 ((long)info->si_addr & 0xffe00000) != 0x7f000000) // faulting not where expected
661 // real crash - time to die
662 err("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]);
663 signal(num, SIG_DFL);
666 segvlog("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]);
669 op_ctx = (void *)g_code_ptr;
670 op_ctx->pc = (u32)pc;
672 g_code_ptr = &op_ctx->code[0];
674 // emit jump to code ptr
675 *pc = make_jmp(pc, g_code_ptr, 0);
678 // TODO: multithreading
679 emit_op_io(0xe50f0000, &g_linkpage->saved_regs[0]); // str r0, [->saved_regs[0]] @ save r0
680 emit_op_io(0xe50fe000, &g_linkpage->saved_regs[14]); // str r14, [->saved_regs[14]]
681 emit_op (0xe24f0000 + (g_code_ptr - (u32 *)op_ctx + 2) * 4); // sub r0, pc, #op_ctx
682 emit_op (make_jmp(g_code_ptr, &g_linkpage->code[0], 1)); // bl common_code
683 emit_op_io(0xe51fe000, &g_linkpage->saved_regs[14]); // ldr r14, [->saved_regs[14]]
684 emit_op (make_jmp(g_code_ptr, pc + 1, 0)); // jmp <back>
687 sys_cacheflush(pc, pc + 1);
688 sys_cacheflush(g_linkpage, g_code_ptr);
690 lp_size = (char *)g_code_ptr - (char *)g_linkpage;
691 segvlog("code #%d %d/%d\n", g_linkpage_count, lp_size, LINKPAGE_SIZE);
693 if (lp_size + 13*4 > LINKPAGE_SIZE) {
695 if (g_linkpage_count >= LINKPAGE_COUNT) {
696 err("too many linkpages needed\n");
699 g_linkpage = (void *)((char *)g_linkpage + LINKPAGE_SIZE);
702 //handle_op(regs[15], op, regs, (u32)info->si_addr);
706 void emu_init(void *map_bottom)
708 struct sigaction segv_action = {
709 .sa_sigaction = segv_sigaction,
710 .sa_flags = SA_SIGINFO,
715 sigemptyset(&segv_action.sa_mask);
716 sigaction(SIGSEGV, &segv_action, NULL);
718 pret = mmap(NULL, HANDLER_STACK_SIZE + 4096, PROT_NONE, MAP_PRIVATE|MAP_ANONYMOUS|MAP_NORESERVE, -1, 0);
719 if (pret == MAP_FAILED) {
720 perror(PFX "mmap handler_stack");
723 ret = mprotect((char *)pret + 4096, HANDLER_STACK_SIZE, PROT_READ | PROT_WRITE);
725 perror(PFX "mprotect handler_stack");
728 g_handler_stack_end = (char *)pret + HANDLER_STACK_SIZE + 4096;
730 g_linkpage = (void *)(((u32)map_bottom - LINKPAGE_ALLOC) & ~0xfff);
731 pret = mmap(g_linkpage, LINKPAGE_ALLOC, PROT_READ|PROT_WRITE, MAP_PRIVATE|MAP_ANONYMOUS, -1, 0);
732 if (pret != g_linkpage) {
733 perror(PFX "mmap linkpage");
736 printf("linkpages @ %p\n", g_linkpage);
740 ret = host_video_init(&host_stride, 0);
742 err("can't alloc screen\n");
745 host_screen = host_video_flip();
748 int emu_read_gpiodev(void *buf, int count)
753 err("gpiodev read %d?\n", count);
757 btns = host_read_btns();
758 memcpy(buf, &btns, 4);
762 void *emu_mmap_dev(unsigned int length, int prot, int flags, unsigned int offset)
764 struct uppermem_block *umem;
769 if ((offset & ~0xffff) == 0xc0000000) {
770 return mmap((void *)0x7f000000, length, PROT_NONE,
771 MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0);
774 if ((offset & ~0xffff) == 0xe0020000) {
775 return mmap((void *)0x7f100000, length, PROT_NONE,
776 MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0);
779 if ((offset & 0xfe000000) != 0x02000000)
780 err("unexpected devmem mmap @ %08x\n", offset);
782 // return mmap(NULL, length, prot, flags, memdev, offset);
784 umem = calloc(1, sizeof(*umem));
792 umem->mem = mmap(NULL, length, prot, MAP_PRIVATE|MAP_ANONYMOUS, -1, 0);
793 if (umem->mem != MAP_FAILED)
796 printf("upper mem @ %08x %d mmap fail, trying backing file\n", offset, length);
797 sprintf(name, "m%08x", offset);
798 fd = open(name, O_CREAT|O_RDWR, 0644);
799 lseek(fd, length - 1, SEEK_SET);
803 umem->mem = mmap(NULL, length, prot, MAP_SHARED, fd, 0);
804 if (umem->mem == MAP_FAILED) {
805 err("failed, giving up\n");
812 printf("upper mem @ %08x %d\n", offset, length);
813 umem->next = upper_mem;