1 // vim:shiftwidth=2:expandtab
11 #include <asm/ucontext.h>
14 #include "sys_cacheflush.h"
16 //#define iolog printf
19 typedef unsigned int u32;
20 typedef unsigned short u16;
21 typedef unsigned char u8;
24 static volatile u16 *memregs, *blitter;
27 static volatile void *translate_addr(u32 a, u32 *mask)
29 if ((a & 0xfff00000) == 0x7f000000) {
33 if ((a & 0xfff00000) == 0x7f100000) {
37 fprintf(stderr, "bad IO @ %08x\n", a);
41 static u32 xread8(u32 a)
46 iolog("r8 %08x\n", a);
47 mem = translate_addr(a, &mask);
51 static u32 xread16(u32 a)
56 //if ((a & 0xfff00000) == 0x7f100000) { static int a; a ^= ~1; return a & 0xffff; }
57 iolog("r16 %08x\n", a);
58 mem = translate_addr(a, &mask);
59 return mem[(a & mask) / 2];
62 static u32 xread32(u32 a)
67 //if ((a & 0xfff00000) == 0x7f100000) { static int a; a ^= ~1; return a; }
68 iolog("r32 %08x\n", a);
69 mem = translate_addr(a, &mask);
70 return mem[(a & mask) / 4];
73 static void xwrite8(u32 a, u32 d)
78 iolog("w8 %08x %08x\n", a, d);
79 mem = translate_addr(a, &mask);
83 static void xwrite16(u32 a, u32 d)
88 iolog("w16 %08x %08x\n", a, d);
89 mem = translate_addr(a, &mask);
90 mem[(a & mask) / 2] = d;
93 static void xwrite32(u32 a, u32 d)
98 iolog("w32 %08x %08x\n", a, d);
99 mem = translate_addr(a, &mask);
100 mem[(a & mask) / 4] = d;
103 #define BIT_SET(v, b) (v & (1 << (b)))
105 static void handle_op(u32 pc, u32 op, u32 *regs, u32 addr_check)
107 u32 t, shift, ret, addr;
110 rd = (op & 0x0000f000) >> 12;
111 rn = (op & 0x000f0000) >> 16;
113 if ((op & 0x0f200090) == 0x01000090) { // AM3: LDRH, STRH
114 if (!BIT_SET(op, 5)) // !H
116 if (BIT_SET(op, 6) && !BIT_SET(op, 20)) // S && !L
119 if (BIT_SET(op, 22)) // imm offset
120 t = ((op & 0xf00) >> 4) | (op & 0x0f);
122 t = regs[op & 0x000f];
124 if (!BIT_SET(op, 23))
128 if (BIT_SET(op, 20)) { // Load
130 if (BIT_SET(op, 6)) { // S
132 ret = (signed int)ret >> 16;
137 xwrite16(addr, regs[rd]);
139 else if ((op & 0x0d200000) == 0x05000000) { // AM2: LDR[B], STR[B]
140 if (BIT_SET(op, 25)) { // reg offs
144 t = regs[op & 0x000f];
145 shift = (op & 0x0f80) >> 7;
146 switch ((op & 0x0060) >> 5) {
147 case 0: t = t << shift; break;
148 case 1: t = t >> (shift + 1); break;
149 case 2: t = (signed int)t >> (shift + 1); break;
150 case 3: goto unhandled; // I'm just lazy
156 if (!BIT_SET(op, 23))
160 if (BIT_SET(op, 20)) { // Load
161 if (BIT_SET(op, 22)) // Byte
168 if (BIT_SET(op, 22)) // Byte
169 xwrite8(addr, regs[rd]);
171 xwrite32(addr, regs[rd]);
178 if (addr != addr_check) {
179 fprintf(stderr, "bad calculated addr: %08x vs %08x\n", addr, addr_check);
186 fprintf(stderr, "unhandled IO op %08x @ %08x\n", op, pc);
189 #define LINKPAGE_SIZE 0x1000
190 #define LINKPAGE_COUNT 4
191 #define LINKPAGE_ALLOC (LINKPAGE_SIZE * LINKPAGE_COUNT)
196 void (*handler)(u32 addr_pc, u32 op, u32 *regs, u32 addr_check);
200 static struct linkpage *g_linkpage;
201 static u32 *g_code_ptr;
202 static int g_linkpage_count;
204 static void init_linkpage(void)
206 g_linkpage->lp_r1 = &g_linkpage->saved_regs[1];
207 g_linkpage->handler = handle_op;
208 g_code_ptr = g_linkpage->code;
211 static u32 make_offset12(u32 *pc, u32 *target)
215 lp_offs = (char *)target - (char *)pc - 2*4;
220 if (lp_offs >= LINKPAGE_SIZE) {
221 fprintf(stderr, "linkpage too far: %d\n", lp_offs);
225 return (u << 23) | lp_offs;
228 static u32 make_jmp(u32 *pc, u32 *target)
232 jmp_val = target - pc - 2;
233 if (jmp_val < (int)0xff000000 || jmp_val > 0x00ffffff) {
234 fprintf(stderr, "jump out of range (%p -> %p)\n", pc, target);
238 return 0xea000000 | (jmp_val & 0x00ffffff);
241 static void emit_op(u32 op)
246 static void emit_op_io(u32 op, u32 *target)
248 op |= make_offset12(g_code_ptr, target);
252 static void segv_sigaction(int num, siginfo_t *info, void *ctx)
254 struct ucontext *context = ctx;
255 u32 *regs = (u32 *)&context->uc_mcontext.arm_r0;
256 u32 *pc = (u32 *)regs[15];
258 u32 *pc_ptr, *old_op_ptr;
261 if (((regs[15] ^ (u32)&segv_sigaction) & 0xff000000) == 0 || // PC is in our segment or
262 (((regs[15] ^ (u32)g_linkpage) & ~(LINKPAGE_ALLOC - 1)) == 0)) // .. in linkpage
264 // real crash - time to die
265 printf("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]);
266 signal(num, SIG_DFL);
269 printf("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]);
272 pc_ptr = g_code_ptr++;
273 old_op_ptr = g_code_ptr++;
275 *old_op_ptr = old_op;
277 // emit jump to code ptr
278 *pc = make_jmp(pc, g_code_ptr);
281 // TODO: our own stack
282 emit_op_io(0xe50f0000, &g_linkpage->saved_regs[0]); // str r0, [saved_regs[0]] @ save r0
283 emit_op_io(0xe51f0000, (u32 *)&g_linkpage->lp_r1); // ldr r0, =lp_r1
284 emit_op (0xe8807ffe); // stmia r0, {r1-r14}
285 emit_op (0xe2402004); // sub r2, r0, #4
286 emit_op_io(0xe51f0000, pc_ptr); // ldr r0, =pc
287 emit_op_io(0xe51f1000, old_op_ptr); // ldr r1, =old_op
288 emit_op (0xe1a04002); // mov r4, r2
289 emit_op (0xe1a0e00f); // mov lr, pc
290 emit_op_io(0xe51ff000, (u32 *)&g_linkpage->handler); // ldr pc, =handle_op
291 emit_op (0xe8947fff); // ldmia r4, {r0-r14}
292 emit_op (make_jmp(g_code_ptr, pc + 1)); // jmp <back>
295 sys_cacheflush(pc, pc + 1);
296 sys_cacheflush(g_linkpage, g_code_ptr);
298 lp_size = (char *)g_code_ptr - (char *)g_linkpage;
299 printf("code #%d %d/%d\n", g_linkpage_count, lp_size, LINKPAGE_SIZE);
301 if (lp_size + 13*4 > LINKPAGE_SIZE) {
303 if (g_linkpage_count >= LINKPAGE_COUNT) {
304 fprintf(stderr, "too many linkpages needed\n");
307 g_linkpage = (void *)((char *)g_linkpage + LINKPAGE_SIZE);
310 //handle_op(regs[15], op, regs, (u32)info->si_addr);
314 void emu_init(void *map_bottom)
316 struct sigaction segv_action = {
317 .sa_sigaction = segv_sigaction,
318 .sa_flags = SA_SIGINFO,
322 sigemptyset(&segv_action.sa_mask);
323 sigaction(SIGSEGV, &segv_action, NULL);
325 g_linkpage = (void *)(((u32)map_bottom - LINKPAGE_ALLOC) & ~0xfff);
326 ret = mmap(g_linkpage, LINKPAGE_ALLOC, PROT_READ|PROT_WRITE, MAP_PRIVATE|MAP_ANONYMOUS, -1, 0);
327 if (ret != g_linkpage) {
328 perror("mmap linkpage");
331 printf("linkpages @ %p\n", g_linkpage);
334 memdev = open("/dev/mem", O_RDWR);
335 memregs = mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, memdev, 0xc0000000);
336 blitter = mmap(NULL, 0x100, PROT_READ|PROT_WRITE, MAP_SHARED, memdev, 0xe0020000);
337 //blitter = mmap(NULL, 0x100, PROT_READ|PROT_WRITE, MAP_PRIVATE|MAP_ANONYMOUS, -1, 0);
338 printf("mapped %d %p %p\n", memdev, memregs, blitter);
341 void *emu_mmap_dev(unsigned int length, int prot, int flags, unsigned int offset)
346 if ((offset & ~0xffff) == 0xc0000000) {
347 return mmap((void *)0x7f000000, length, PROT_NONE,
348 MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0);
350 if ((offset & ~0xffff) == 0xe0020000) {
351 return mmap((void *)0x7f100000, length, PROT_NONE,
352 MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0);
355 if ((offset & 0xfe000000) == 0x02000000)
356 return mmap(NULL, length, prot, flags, memdev, offset);
358 sprintf(name, "m%08x", offset);
359 fd = open(name, O_CREAT|O_RDWR, 0644);
360 lseek(fd, length - 1, SEEK_SET);
364 return mmap(NULL, length, prot, MAP_SHARED, fd, 0);