1 // vim:shiftwidth=2:expandtab
10 #include <sys/types.h>
13 #include <asm/ucontext.h>
17 #include <sys/resource.h>
20 #include "sys_cacheflush.h"
33 #define iolog_unh log_io
35 #define iolog_unh(...)
39 #define segvlog printf
44 #if defined(LOG_IO) || defined(LOG_IO_UNK)
45 #include "mmsp2-regs.h"
48 typedef unsigned int u32;
49 typedef unsigned short u16;
50 typedef unsigned char u8;
52 static pthread_mutex_t fb_mutex = PTHREAD_MUTEX_INITIALIZER;
53 static pthread_cond_t fb_cond = PTHREAD_COND_INITIALIZER;
55 struct uppermem_block {
59 struct uppermem_block *next;
62 static struct uppermem_block *upper_mem;
83 #define SRCCTRL_INVIDEO (1 << 8)
84 #define SRCCTRL_SRCENB (1 << 7)
85 #define CTRL_TRANSPARENCYENB (1 << 11)
98 u16 mlc_stl_pallt_d[256*2];
99 u32 mlc_stl_pallt_d32[256];
105 u32 btn_state; // as seen through /dev/GPIO
109 static u16 *host_screen;
110 static int host_stride;
113 #if defined(LOG_IO) || defined(LOG_IO_UNK)
114 static void log_io(const char *pfx, u32 a, u32 d, int size)
116 const char *fmt, *reg = "";
118 case 8: fmt = "%s %08x %02x %s\n"; d &= 0xff; break;
119 case 32: fmt = "%s %08x %08x %s\n"; break;
120 default: fmt = "%s %08x %04x %s\n"; d &= 0xffff; break;
123 if ((a & ~0xffff) == 0x7f000000)
124 reg = regnames[a & 0xffff];
126 printf(fmt, pfx, a, d, reg);
130 static void memset16(void *dst, u32 pattern, int count)
135 d = (u16 *)((long)dst & ~1);
141 pattern |= pattern << 16;
148 *(u16 *)dl = pattern;
151 static void blt_tr(void *dst, void *src, u32 trc, int w)
153 u16 *d = (u16 *)((long)dst & ~1);
154 u16 *s = (u16 *)((long)src & ~1);
157 for (; w > 0; d++, s++, w--)
162 #define dump_blitter() \
164 u32 *r = &blitter.dstctrl; \
166 for (i = 0; i < 4*4; i++, r++) { \
167 printf("%08x ", *r); \
173 static void *uppermem_lookup(u32 addr, u8 **mem_end)
175 struct uppermem_block *ub;
177 for (ub = upper_mem; ub != NULL; ub = ub->next) {
178 if (ub->addr <= addr && addr < ub->addr + ub->size) {
179 *mem_end = (u8 *)ub->mem + ub->size;
180 return (u8 *)ub->mem + addr - ub->addr;
187 static void blitter_do(void)
189 u8 *dst, *dste, *src = NULL, *srce = NULL;
190 int w, h, sstrd, dstrd;
194 w = blitter.size & 0x7ff;
195 h = (blitter.size >> 16) & 0x7ff;
196 sstrd = blitter.srcstride;
197 dstrd = blitter.dststride;
199 // XXX: need to confirm this..
200 addr = (blitter.dstaddr & ~3) | ((blitter.dstctrl & 0x1f) >> 3);
203 if (w == 320 && h == 240 && mmsp2.mlc_stl_adr <= addr && addr < mmsp2.mlc_stl_adr + 320*240*2)
206 dst = uppermem_lookup(addr, &dste);
208 // XXX: assume fill if no SRCENB, but it could be pattern blit..
209 if (blitter.srcctrl & SRCCTRL_SRCENB) {
210 if (!(blitter.srcctrl & SRCCTRL_INVIDEO))
213 addr = (blitter.srcaddr & ~3) | ((blitter.srcctrl & 0x1f) >> 3);
214 src = uppermem_lookup(addr, &srce);
218 if (src + sstrd * h > srce) {
219 err("blit %08x->%08x %dx%d did not fit src\n",
220 blitter.srcaddr, blitter.dstaddr, w, h);
221 h = (srce - src) / sstrd;
225 if (dst + dstrd * h > dste) {
226 err("blit %08x->%08x %dx%d did not fit dst\n",
227 blitter.srcaddr, blitter.dstaddr, w, h);
228 h = (dste - dst) / dstrd;
233 if (blitter.ctrl & CTRL_TRANSPARENCYENB) {
234 u32 trc = blitter.ctrl >> 16;
235 for (; h > 0; h--, dst += dstrd, src += sstrd)
236 blt_tr(dst, src, trc, w);
239 for (; h > 0; h--, dst += dstrd, src += sstrd)
240 memcpy(dst, src, w * 2);
244 // fill. Assume the pattern is cleared and bg color is used
245 u32 bgc = blitter.patbackcolor & 0xffff;
246 for (; h > 0; h--, dst += dstrd)
247 memset16(dst, bgc, w);
251 pthread_cond_signal(&fb_cond);
255 err("blit %08x->%08x %dx%d translated to %p->%p\n",
256 blitter.srcaddr, blitter.dstaddr, w, h, src, dst);
260 // TODO: hw scaler stuff
261 static void mlc_flip(u8 *src, int bpp)
263 u16 *dst = host_screen;
264 u16 *hpal = mmsp2.host_pal;
267 if (bpp <= 8 && mmsp2.dirty_pal) {
268 u32 *srcp = mmsp2.mlc_stl_pallt_d32;
271 for (i = 0; i < 256; i++, srcp++, dstp++) {
273 *dstp = ((t >> 8) & 0xf800) | ((t >> 5) & 0x07e0) | ((t >> 3) & 0x001f);
280 for (i = 0; i < 240; i++, dst += host_stride / 2 - 320) {
281 for (u = 320 / 2; u > 0; u--, src++) {
282 *dst++ = hpal[*src >> 4];
283 *dst++ = hpal[*src & 0x0f];
289 for (i = 0; i < 240; i++, dst += host_stride / 2 - 320) {
290 for (u = 320 / 4; u > 0; u--) {
291 *dst++ = hpal[*src++];
292 *dst++ = hpal[*src++];
293 *dst++ = hpal[*src++];
294 *dst++ = hpal[*src++];
300 for (i = 0; i < 240; i++, dst += host_stride / 2, src += 320*2)
301 memcpy(dst, src, 320*2);
309 host_screen = host_video_flip();
312 #define ts_add_nsec(ts, ns) { \
314 if (ts.tv_nsec >= 1000000000) { \
316 ts.tv_nsec -= 1000000000; \
320 static void *fb_sync_thread(void *arg)
322 int invalid_fb_addr = 1;
323 int manual_refresh = 0;
327 //ret = pthread_setschedprio(pthread_self(), -1);
328 //log("pthread_setschedprio %d\n", ret);
329 //ret = setpriority(PRIO_PROCESS, 0, -1);
330 //log("setpriority %d\n", ret);
332 ret = clock_gettime(CLOCK_REALTIME, &ts);
334 perror(PFX "clock_gettime");
339 u8 *gp2x_fb, *gp2x_fb_end;
342 ret = pthread_mutex_lock(&fb_mutex);
343 wait_ret = pthread_cond_timedwait(&fb_cond, &fb_mutex, &ts);
344 ret |= pthread_mutex_unlock(&fb_mutex);
346 err("fb_thread: mutex error: %d\n", ret);
350 if (wait_ret != 0 && wait_ret != ETIMEDOUT) {
351 err("fb_thread: cond error: %d\n", wait_ret);
356 if (wait_ret != ETIMEDOUT) {
357 clock_gettime(CLOCK_REALTIME, &ts);
358 ts_add_nsec(ts, 50000000);
360 if (manual_refresh == 2)
361 log("fb_thread: switch to manual refresh\n");
363 ts_add_nsec(ts, 16666667);
364 if (manual_refresh > 1)
365 log("fb_thread: switch to auto refresh\n");
369 mode = (mmsp2.mlc_stl_cntl >> 9) & 3;
370 bpp = mode ? mode * 8 : 4;
372 gp2x_fb = uppermem_lookup(mmsp2.mlc_stl_adr, &gp2x_fb_end);
373 if (gp2x_fb == NULL || gp2x_fb + 320*240 * bpp / 8 > gp2x_fb_end) {
374 if (!invalid_fb_addr) {
375 err("fb_thread: %08x is out of range\n", mmsp2.mlc_stl_adr);
381 mlc_flip(gp2x_fb, bpp);
385 static u32 xread8(u32 a)
387 iolog("r8 ", a, 0, 8);
388 iolog_unh("r8 ", a, 0, 8);
392 static u32 xread16(u32 a)
394 static u32 fudge, old_a;
397 if ((a & 0xffff0000) == 0x7f000000) {
405 // 0000 P000 VuVd00 0000 YXBA RLSeSt 0R0D 0L0U
406 // | GPIOD |GPIOC[8:15]|GPIOM[0:7]|
409 case 0x1184: // GPIOC
410 d = ~mmsp2.btn_state & 0xff00;
413 case 0x1186: // GPIOD
414 t = ~mmsp2.btn_state;
415 d = (t >> 9) & 0x0080;
416 d |= (t >> 11) & 0x0040;
417 d |= (t >> 7) & 0x0800;
420 case 0x1198: // GPIOM
421 mmsp2.btn_state = host_read_btns();
422 d = ~mmsp2.btn_state & 0xff;
425 case 0x1836: // reserved
428 case 0x2816: // DPC_X_MAX
431 case 0x2818: // DPC_Y_MAX
435 d = mmsp2.mlc_stl_cntl;
439 d = mmsp2.mlc_stl_adrl;
443 d = mmsp2.mlc_stl_adrh;
446 d = mmsp2.mlc_stl_pallt_a;
460 iolog_unh("r16", a, d & 0xffff, 16);
464 iolog("r16", a, d, 16);
468 static u32 xread32(u32 a)
471 if ((a & 0xfff00000) == 0x7f100000) {
472 u32 *bl = &blitter.dstctrl;
481 iolog_unh("r32", a, d, 32);
484 iolog("r32", a, d, 32);
488 static void xwrite8(u32 a, u32 d)
490 iolog("w8 ", a, d, 8);
491 iolog_unh("w8 ", a, d, 8);
494 static void xwrite16(u32 a, u32 d)
496 iolog("w16", a, d, 16);
497 if ((a & 0xfff00000) == 0x7f000000) {
501 mmsp2.mlc_stl_cntl = d | 0xaa;
505 // odd addresses don't affect LCD. What about TV?
508 mmsp2.mlc_stl_adrl = d;
511 mmsp2.mlc_stl_adrh = d;
512 if (mmsp2.mlc_stl_adr != mmsp2.old_mlc_stl_adr)
514 pthread_cond_signal(&fb_cond);
515 mmsp2.old_mlc_stl_adr = mmsp2.mlc_stl_adr;
518 mmsp2.mlc_stl_pallt_a = d & 0x1ff;
521 mmsp2.mlc_stl_pallt_d[mmsp2.mlc_stl_pallt_a++] = d;
522 mmsp2.mlc_stl_pallt_a &= 0x1ff;
527 iolog_unh("w16", a, d, 16);
530 static void xwrite32(u32 a, u32 d)
532 iolog("w32", a, d, 32);
534 if ((a & 0xfff00000) == 0x7f100000) {
535 u32 *bl = &blitter.dstctrl;
539 if (a_ == 0x34 && (d & 1))
544 iolog_unh("w32", a, d, 32);
547 #define LINKPAGE_SIZE 0x1000
548 #define LINKPAGE_COUNT 4
549 #define LINKPAGE_ALLOC (LINKPAGE_SIZE * LINKPAGE_COUNT)
561 void (*handler)(struct op_context *op_ctx);
565 static struct linkpage *g_linkpage;
566 static u32 *g_code_ptr;
567 static int g_linkpage_count;
569 static void *g_handler_stack_end;
571 #define BIT_SET(v, b) (v & (1 << (b)))
573 static void handle_op(struct op_context *op_ctx)
575 u32 *regs = g_linkpage->saved_regs;
577 u32 t, shift, ret, addr;
580 rd = (op & 0x0000f000) >> 12;
581 rn = (op & 0x000f0000) >> 16;
583 if ((op & 0x0f200090) == 0x01000090) { // AM3: LDRH, STRH
584 if (!BIT_SET(op, 5)) // !H
586 if (BIT_SET(op, 6) && !BIT_SET(op, 20)) // S && !L
589 if (BIT_SET(op, 22)) // imm offset
590 t = ((op & 0xf00) >> 4) | (op & 0x0f);
592 t = regs[op & 0x000f];
594 if (!BIT_SET(op, 23))
598 if (BIT_SET(op, 20)) { // Load
600 if (BIT_SET(op, 6)) { // S
602 ret = (signed int)ret >> 16;
607 xwrite16(addr, regs[rd]);
609 else if ((op & 0x0d200000) == 0x05000000) { // AM2: LDR[B], STR[B]
610 if (BIT_SET(op, 25)) { // reg offs
614 t = regs[op & 0x000f];
615 shift = (op & 0x0f80) >> 7;
616 switch ((op & 0x0060) >> 5) {
617 case 0: t = t << shift; break;
618 case 1: t = t >> (shift + 1); break;
619 case 2: t = (signed int)t >> (shift + 1); break;
620 case 3: goto unhandled; // I'm just lazy
626 if (!BIT_SET(op, 23))
630 if (BIT_SET(op, 20)) { // Load
631 if (BIT_SET(op, 22)) // Byte
638 if (BIT_SET(op, 22)) // Byte
639 xwrite8(addr, regs[rd]);
641 xwrite32(addr, regs[rd]);
648 if (addr != addr_check) {
649 fprintf(stderr, "bad calculated addr: %08x vs %08x\n", addr, addr_check);
656 err("unhandled IO op %08x @ %08x\n", op, op_ctx->pc);
659 static u32 make_offset12(u32 *pc, u32 *target)
663 lp_offs = (char *)target - (char *)pc - 2*4;
668 if (lp_offs >= LINKPAGE_SIZE) {
669 err("linkpage too far: %d\n", lp_offs);
673 return (u << 23) | lp_offs;
676 static u32 make_jmp(u32 *pc, u32 *target, int bl)
680 jmp_val = target - pc - 2;
681 if (jmp_val < (int)0xff000000 || jmp_val > 0x00ffffff) {
682 err("jump out of range (%p -> %p)\n", pc, target);
686 return 0xea000000 | (bl << 24) | (jmp_val & 0x00ffffff);
689 static void emit_op(u32 op)
694 static void emit_op_io(u32 op, u32 *target)
696 op |= make_offset12(g_code_ptr, target);
700 static void init_linkpage(void)
702 g_linkpage->handler = handle_op;
703 g_linkpage->handler_stack = g_handler_stack_end;
704 g_code_ptr = g_linkpage->code;
707 // r0 and r14 must be saved by caller, r0 is arg for handle_op
708 // on return everything is restored except lr, which is used to return
709 emit_op_io(0xe50f1000, &g_linkpage->saved_regs[1]); // str r1, [->saved_regs[1]] @ save r1
710 emit_op (0xe24f1000 + // sub r1, pc, =offs(saved_regs[2])
711 (g_code_ptr - &g_linkpage->saved_regs[2] + 2) * 4);
712 emit_op (0xe8813ffc); // stmia r1, {r2-r13}
713 emit_op_io(0xe51fd000, // ldr sp, [->handler_stack]
714 (u32 *)&g_linkpage->handler_stack);
715 emit_op (0xe2414008); // sub r4, r1, #4*2
716 emit_op (0xe10f1000); // mrs r1, cpsr
717 emit_op_io(0xe50f1000, &g_linkpage->cpsr); // str r1, [->cpsr]
718 emit_op (0xe1a0500e); // mov r5, lr
719 emit_op (0xe1a0e00f); // mov lr, pc
720 emit_op_io(0xe51ff000, (u32 *)&g_linkpage->handler); // ldr pc, =handle_op
721 emit_op_io(0xe51f1000, &g_linkpage->cpsr); // ldr r1, [->cpsr]
722 emit_op (0xe128f001); // msr cpsr_f, r1
723 emit_op (0xe1a0e005); // mov lr, r5
724 emit_op (0xe8943fff); // ldmia r4, {r0-r13}
725 emit_op (0xe12fff1e); // bx lr @ return
728 static void segv_sigaction(int num, siginfo_t *info, void *ctx)
730 struct ucontext *context = ctx;
731 u32 *regs = (u32 *)&context->uc_mcontext.arm_r0;
732 u32 *pc = (u32 *)regs[15];
733 struct op_context *op_ctx;
736 if (((regs[15] ^ (u32)&segv_sigaction) & 0xff000000) == 0 || // PC is in our segment or
737 (((regs[15] ^ (u32)g_linkpage) & ~(LINKPAGE_ALLOC - 1)) == 0) || // .. in linkpage
738 ((long)info->si_addr & 0xffe00000) != 0x7f000000) // faulting not where expected
740 // real crash - time to die
741 err("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]);
742 signal(num, SIG_DFL);
746 segvlog("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]);
749 op_ctx = (void *)g_code_ptr;
750 op_ctx->pc = (u32)pc;
752 g_code_ptr = &op_ctx->code[0];
754 // emit jump to code ptr
755 *pc = make_jmp(pc, g_code_ptr, 0);
758 // TODO: multithreading
759 emit_op_io(0xe50f0000, &g_linkpage->saved_regs[0]); // str r0, [->saved_regs[0]] @ save r0
760 emit_op_io(0xe50fe000, &g_linkpage->saved_regs[14]); // str r14, [->saved_regs[14]]
761 emit_op (0xe24f0000 + (g_code_ptr - (u32 *)op_ctx + 2) * 4); // sub r0, pc, #op_ctx
762 emit_op (make_jmp(g_code_ptr, &g_linkpage->code[0], 1)); // bl common_code
763 emit_op_io(0xe51fe000, &g_linkpage->saved_regs[14]); // ldr r14, [->saved_regs[14]]
764 emit_op (make_jmp(g_code_ptr, pc + 1, 0)); // jmp <back>
767 sys_cacheflush(pc, pc + 1);
768 sys_cacheflush(g_linkpage, g_code_ptr);
770 lp_size = (char *)g_code_ptr - (char *)g_linkpage;
771 segvlog("code #%d %d/%d\n", g_linkpage_count, lp_size, LINKPAGE_SIZE);
773 if (lp_size + 13*4 > LINKPAGE_SIZE) {
775 if (g_linkpage_count >= LINKPAGE_COUNT) {
776 err("too many linkpages needed\n");
779 g_linkpage = (void *)((char *)g_linkpage + LINKPAGE_SIZE);
782 //handle_op(regs[15], op, regs, (u32)info->si_addr);
786 void emu_init(void *map_bottom)
788 struct sigaction segv_action = {
789 .sa_sigaction = segv_sigaction,
790 .sa_flags = SA_SIGINFO,
796 g_handler_stack_end = (void *)((long)alloca(1536 * 1024) & ~0xffff);
797 log("handler stack @ %p (current %p)\n", g_handler_stack_end, &ret);
798 // touch it now. If we crash now we'll know why
799 *((char *)g_handler_stack_end - 4096) = 1;
801 g_linkpage = (void *)(((u32)map_bottom - LINKPAGE_ALLOC) & ~0xfff);
802 pret = mmap(g_linkpage, LINKPAGE_ALLOC, PROT_READ|PROT_WRITE, MAP_PRIVATE|MAP_ANONYMOUS, -1, 0);
803 if (pret != g_linkpage) {
804 perror(PFX "mmap linkpage");
807 log("linkpages @ %p\n", g_linkpage);
811 ret = host_video_init(&host_stride, 0);
813 err("can't alloc screen\n");
816 host_screen = host_video_flip();
818 ret = pthread_create(&tid, NULL, fb_sync_thread, NULL);
820 err("failed to create fb_sync_thread: %d\n", ret);
826 mmsp2.mlc_stl_adr = 0x03101000; // fb2 is at 0x03381000
828 sigemptyset(&segv_action.sa_mask);
829 sigaction(SIGSEGV, &segv_action, NULL);
832 int emu_read_gpiodev(void *buf, int count)
837 err("gpiodev read %d?\n", count);
841 btns = host_read_btns();
842 memcpy(buf, &btns, 4);
846 void *emu_mmap_dev(unsigned int length, int prot, int flags, unsigned int offset)
848 struct uppermem_block *umem;
853 if ((offset & ~0xffff) == 0xc0000000) {
854 return mmap((void *)0x7f000000, length, PROT_NONE,
855 MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0);
858 if ((offset & ~0xffff) == 0xe0020000) {
859 return mmap((void *)0x7f100000, length, PROT_NONE,
860 MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0);
863 if ((offset & 0xfe000000) != 0x02000000)
864 err("unexpected devmem mmap @ %08x\n", offset);
866 umem = calloc(1, sizeof(*umem));
874 umem->mem = mmap(NULL, length, prot, MAP_PRIVATE|MAP_ANONYMOUS, -1, 0);
875 if (umem->mem != MAP_FAILED)
878 log("upper mem @ %08x %d mmap fail, trying backing file\n", offset, length);
879 sprintf(name, "m%08x", offset);
880 fd = open(name, O_CREAT|O_RDWR, 0644);
881 lseek(fd, length - 1, SEEK_SET);
885 umem->mem = mmap(NULL, length, prot, MAP_SHARED, fd, 0);
886 if (umem->mem == MAP_FAILED) {
887 err("failed, giving up\n");
894 log("upper mem @ %08x %d\n", offset, length);
895 umem->next = upper_mem;