1 // vim:shiftwidth=2:expandtab
12 #include <asm/ucontext.h>
15 #include "sys_cacheflush.h"
17 //#define iolog printf
20 typedef unsigned int u32;
21 typedef unsigned short u16;
22 typedef unsigned char u8;
24 struct uppermem_block {
28 struct uppermem_block *next;
31 static struct uppermem_block *upper_mem;
62 static u16 *host_screen;
63 static int host_stride;
66 static void *upper_lookup(u32 addr, u8 **mem_end, int *stride_override)
68 struct uppermem_block *ub;
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;
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;
88 static void blitter_do(void)
90 u8 *dst, *dste, *src, *srce;
91 int w, h, sstrd, dstrd;
93 if (blitter.srcaddr == blitter.dstaddr)
94 return; // dummy blit?
96 w = blitter.size & 0x7ff;
97 h = (blitter.size >> 16) & 0x7ff;
98 sstrd = blitter.srcstride;
99 dstrd = blitter.dststride;
101 dst = upper_lookup(blitter.dstaddr, &dste, &dstrd);
102 src = upper_lookup(blitter.srcaddr, &srce, &sstrd);
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);
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;
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;
122 for (; h > 0; h--, dst += dstrd, src += sstrd)
123 memcpy(dst, src, w * 2);
126 static u32 xread8(u32 a)
128 iolog("r8 %08x\n", a);
132 static u32 xread16(u32 a)
134 // if ((a & 0xfff00000) == 0x7f100000) { static int a; a ^= ~1; return a & 0xffff; }
135 iolog("r16 %08x\n", a);
139 static u32 xread32(u32 a)
142 if ((a & 0xfff00000) == 0x7f100000) {
143 u32 *bl = &blitter.dstctrl;
150 iolog("r32 %08x\n", a);
154 static void xwrite8(u32 a, u32 d)
156 iolog("w8 %08x %08x\n", a, d);
159 static void xwrite16(u32 a, u32 d)
161 iolog("w16 %08x %08x\n", a, d);
162 if ((a & 0xfff00000) == 0x7f000000) {
165 case 0x2912: mmsp2.mlc_stl_eadrl = d; break;
166 case 0x2914: mmsp2.mlc_stl_eadrh = d; break;
168 printf("w16 %08x %08x\n", a, d);
172 static void xwrite32(u32 a, u32 d)
174 iolog("w32 %08x %08x\n", a, d);
175 if ((a & 0xfff00000) == 0x7f000000) {
176 printf("w32 %08x %08x\n", a, d);
179 if ((a & 0xfff00000) == 0x7f100000) {
180 u32 *bl = &blitter.dstctrl;
184 if (a == 0x34 && (d & 1))
190 #define BIT_SET(v, b) (v & (1 << (b)))
192 static void handle_op(u32 pc, u32 op, u32 *regs, u32 addr_check)
194 u32 t, shift, ret, addr;
197 rd = (op & 0x0000f000) >> 12;
198 rn = (op & 0x000f0000) >> 16;
200 if ((op & 0x0f200090) == 0x01000090) { // AM3: LDRH, STRH
201 if (!BIT_SET(op, 5)) // !H
203 if (BIT_SET(op, 6) && !BIT_SET(op, 20)) // S && !L
206 if (BIT_SET(op, 22)) // imm offset
207 t = ((op & 0xf00) >> 4) | (op & 0x0f);
209 t = regs[op & 0x000f];
211 if (!BIT_SET(op, 23))
215 if (BIT_SET(op, 20)) { // Load
217 if (BIT_SET(op, 6)) { // S
219 ret = (signed int)ret >> 16;
224 xwrite16(addr, regs[rd]);
226 else if ((op & 0x0d200000) == 0x05000000) { // AM2: LDR[B], STR[B]
227 if (BIT_SET(op, 25)) { // reg offs
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
243 if (!BIT_SET(op, 23))
247 if (BIT_SET(op, 20)) { // Load
248 if (BIT_SET(op, 22)) // Byte
255 if (BIT_SET(op, 22)) // Byte
256 xwrite8(addr, regs[rd]);
258 xwrite32(addr, regs[rd]);
265 if (addr != addr_check) {
266 fprintf(stderr, "bad calculated addr: %08x vs %08x\n", addr, addr_check);
273 fprintf(stderr, "unhandled IO op %08x @ %08x\n", op, pc);
276 #define LINKPAGE_SIZE 0x1000
277 #define LINKPAGE_COUNT 4
278 #define LINKPAGE_ALLOC (LINKPAGE_SIZE * LINKPAGE_COUNT)
283 void (*handler)(u32 addr_pc, u32 op, u32 *regs, u32 addr_check);
287 static struct linkpage *g_linkpage;
288 static u32 *g_code_ptr;
289 static int g_linkpage_count;
291 static void init_linkpage(void)
293 g_linkpage->lp_r1 = &g_linkpage->saved_regs[1];
294 g_linkpage->handler = handle_op;
295 g_code_ptr = g_linkpage->code;
298 static u32 make_offset12(u32 *pc, u32 *target)
302 lp_offs = (char *)target - (char *)pc - 2*4;
307 if (lp_offs >= LINKPAGE_SIZE) {
308 fprintf(stderr, "linkpage too far: %d\n", lp_offs);
312 return (u << 23) | lp_offs;
315 static u32 make_jmp(u32 *pc, u32 *target)
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);
325 return 0xea000000 | (jmp_val & 0x00ffffff);
328 static void emit_op(u32 op)
333 static void emit_op_io(u32 op, u32 *target)
335 op |= make_offset12(g_code_ptr, target);
339 static void segv_sigaction(int num, siginfo_t *info, void *ctx)
341 struct ucontext *context = ctx;
342 u32 *regs = (u32 *)&context->uc_mcontext.arm_r0;
343 u32 *pc = (u32 *)regs[15];
345 u32 *pc_ptr, *old_op_ptr;
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
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);
356 printf("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]);
359 pc_ptr = g_code_ptr++;
360 old_op_ptr = g_code_ptr++;
362 *old_op_ptr = old_op;
364 // emit jump to code ptr
365 *pc = make_jmp(pc, g_code_ptr);
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>
382 sys_cacheflush(pc, pc + 1);
383 sys_cacheflush(g_linkpage, g_code_ptr);
385 lp_size = (char *)g_code_ptr - (char *)g_linkpage;
386 printf("code #%d %d/%d\n", g_linkpage_count, lp_size, LINKPAGE_SIZE);
388 if (lp_size + 13*4 > LINKPAGE_SIZE) {
390 if (g_linkpage_count >= LINKPAGE_COUNT) {
391 fprintf(stderr, "too many linkpages needed\n");
394 g_linkpage = (void *)((char *)g_linkpage + LINKPAGE_SIZE);
397 //handle_op(regs[15], op, regs, (u32)info->si_addr);
401 void emu_init(void *map_bottom)
403 struct sigaction segv_action = {
404 .sa_sigaction = segv_sigaction,
405 .sa_flags = SA_SIGINFO,
409 sigemptyset(&segv_action.sa_mask);
410 sigaction(SIGSEGV, &segv_action, NULL);
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);
414 if (ret != g_linkpage) {
415 perror("mmap linkpage");
418 printf("linkpages @ %p\n", g_linkpage);
422 host_screen = host_video_init(&host_stride);
423 if (host_screen == NULL) {
424 printf("can't alloc screen\n");
429 int emu_read_gpiodev(void *buf, int count)
434 printf("gpiodev read %d?\n", count);
438 btns = host_read_btns();
439 memcpy(buf, &btns, 4);
443 void *emu_mmap_dev(unsigned int length, int prot, int flags, unsigned int offset)
445 struct uppermem_block *umem;
450 if ((offset & ~0xffff) == 0xc0000000) {
451 return mmap((void *)0x7f000000, length, PROT_NONE,
452 MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0);
455 if ((offset & ~0xffff) == 0xe0020000) {
456 return mmap((void *)0x7f100000, length, PROT_NONE,
457 MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0);
460 if ((offset & 0xfe000000) != 0x02000000)
461 printf("unexpected devmem mmap @ %08x\n", offset);
463 // return mmap(NULL, length, prot, flags, memdev, offset);
465 umem = calloc(1, sizeof(*umem));
473 umem->mem = mmap(NULL, length, prot, MAP_PRIVATE|MAP_ANONYMOUS, -1, 0);
474 if (umem->mem != MAP_FAILED)
477 printf("upper mem @ %08x %d mmap fail, trying backing file\n", offset, length);
478 sprintf(name, "m%08x", offset);
479 fd = open(name, O_CREAT|O_RDWR, 0644);
480 lseek(fd, length - 1, SEEK_SET);
484 umem->mem = mmap(NULL, length, prot, MAP_SHARED, fd, 0);
485 if (umem->mem == MAP_FAILED) {
486 printf("failed, giving up\n");
493 printf("upper mem @ %08x %d\n", offset, length);
494 umem->next = upper_mem;