IO op patching to memhandler calls
[ginge.git] / loader / emu.c
CommitLineData
11913091 1// vim:shiftwidth=2:expandtab
2#include <stdio.h>
3#include <stdlib.h>
4#include <sys/types.h>
5#include <sys/stat.h>
6#include <fcntl.h>
7#include <sys/mman.h>
8#include <sys/types.h>
9#include <unistd.h>
10#include <signal.h>
11#include <asm/ucontext.h>
12
13#include "header.h"
86418a84 14#include "sys_cacheflush.h"
11913091 15
86418a84 16//#define iolog printf
17#define iolog(...)
11913091 18
19typedef unsigned int u32;
20typedef unsigned short u16;
21typedef unsigned char u8;
22
23static int memdev;
24static volatile u16 *memregs, *blitter;
25
26
27static volatile void *translate_addr(u32 a, u32 *mask)
28{
29 if ((a & 0xfff00000) == 0x7f000000) {
30 *mask = 0xffff;
31 return memregs;
32 }
33 if ((a & 0xfff00000) == 0x7f100000) {
34 *mask = 0xff;
35 return blitter;
36 }
37 fprintf(stderr, "bad IO @ %08x\n", a);
38 abort();
39}
40
41static u32 xread8(u32 a)
42{
43 volatile u8 *mem;
44 u32 mask;
45
46 iolog("r8 %08x\n", a);
47 mem = translate_addr(a, &mask);
48 return mem[a & mask];
49}
50
51static u32 xread16(u32 a)
52{
53 volatile u16 *mem;
54 u32 mask;
55
86418a84 56 //if ((a & 0xfff00000) == 0x7f100000) { static int a; a ^= ~1; return a & 0xffff; }
11913091 57 iolog("r16 %08x\n", a);
58 mem = translate_addr(a, &mask);
59 return mem[(a & mask) / 2];
60}
61
62static u32 xread32(u32 a)
63{
64 volatile u32 *mem;
65 u32 mask;
66
86418a84 67 //if ((a & 0xfff00000) == 0x7f100000) { static int a; a ^= ~1; return a; }
11913091 68 iolog("r32 %08x\n", a);
69 mem = translate_addr(a, &mask);
70 return mem[(a & mask) / 4];
71}
72
73static void xwrite8(u32 a, u32 d)
74{
75 volatile u8 *mem;
76 u32 mask;
77
78 iolog("w8 %08x %08x\n", a, d);
79 mem = translate_addr(a, &mask);
80 mem[a & mask] = d;
81}
82
83static void xwrite16(u32 a, u32 d)
84{
85 volatile u16 *mem;
86 u32 mask;
87
88 iolog("w16 %08x %08x\n", a, d);
89 mem = translate_addr(a, &mask);
90 mem[(a & mask) / 2] = d;
91}
92
93static void xwrite32(u32 a, u32 d)
94{
95 volatile u32 *mem;
96 u32 mask;
97
98 iolog("w32 %08x %08x\n", a, d);
99 mem = translate_addr(a, &mask);
100 mem[(a & mask) / 4] = d;
101}
102
103#define BIT_SET(v, b) (v & (1 << (b)))
104
86418a84 105static void handle_op(u32 pc, u32 op, u32 *regs, u32 addr_check)
11913091 106{
107 u32 t, shift, ret, addr;
108 int rn, rd;
109
110 rd = (op & 0x0000f000) >> 12;
111 rn = (op & 0x000f0000) >> 16;
112
113 if ((op & 0x0f200090) == 0x01000090) { // AM3: LDRH, STRH
86418a84 114 if (!BIT_SET(op, 5)) // !H
115 goto unhandled;
116 if (BIT_SET(op, 6) && !BIT_SET(op, 20)) // S && !L
11913091 117 goto unhandled;
118
119 if (BIT_SET(op, 22)) // imm offset
120 t = ((op & 0xf00) >> 4) | (op & 0x0f);
121 else // reg offset
122 t = regs[op & 0x000f];
123
124 if (!BIT_SET(op, 23))
125 t = -t;
126 addr = regs[rn] + t;
127
128 if (BIT_SET(op, 20)) { // Load
129 ret = xread16(addr);
86418a84 130 if (BIT_SET(op, 6)) { // S
131 ret <<= 16;
132 ret = (signed int)ret >> 16;
133 }
11913091 134 regs[rd] = ret;
135 }
136 else
137 xwrite16(addr, regs[rd]);
138 }
139 else if ((op & 0x0d200000) == 0x05000000) { // AM2: LDR[B], STR[B]
140 if (BIT_SET(op, 25)) { // reg offs
141 if (BIT_SET(op, 4))
142 goto unhandled;
143
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
151 }
152 }
153 else // imm offs
154 t = op & 0x0fff;
155
156 if (!BIT_SET(op, 23))
157 t = -t;
158 addr = regs[rn] + t;
159
160 if (BIT_SET(op, 20)) { // Load
161 if (BIT_SET(op, 22)) // Byte
162 ret = xread8(addr);
163 else
164 ret = xread32(addr);
165 regs[rd] = ret;
166 }
167 else {
168 if (BIT_SET(op, 22)) // Byte
169 xwrite8(addr, regs[rd]);
170 else
171 xwrite32(addr, regs[rd]);
172 }
173 }
174 else
175 goto unhandled;
176
86418a84 177#if 0
11913091 178 if (addr != addr_check) {
179 fprintf(stderr, "bad calculated addr: %08x vs %08x\n", addr, addr_check);
180 abort();
181 }
86418a84 182#endif
11913091 183 return;
184
185unhandled:
86418a84 186 fprintf(stderr, "unhandled IO op %08x @ %08x\n", op, pc);
11913091 187}
188
189#define LINKPAGE_SIZE 0x1000
86418a84 190#define LINKPAGE_COUNT 4
191#define LINKPAGE_ALLOC (LINKPAGE_SIZE * LINKPAGE_COUNT)
11913091 192
193struct linkpage {
86418a84 194 u32 saved_regs[15];
195 u32 *lp_r1;
196 void (*handler)(u32 addr_pc, u32 op, u32 *regs, u32 addr_check);
11913091 197 u32 code[0];
198};
199
200static struct linkpage *g_linkpage;
201static u32 *g_code_ptr;
86418a84 202static int g_linkpage_count;
203
204static void init_linkpage(void)
205{
206 g_linkpage->lp_r1 = &g_linkpage->saved_regs[1];
207 g_linkpage->handler = handle_op;
208 g_code_ptr = g_linkpage->code;
209}
210
211static u32 make_offset12(u32 *pc, u32 *target)
212{
213 int lp_offs, u = 1;
214
215 lp_offs = (char *)target - (char *)pc - 2*4;
216 if (lp_offs < 0) {
217 lp_offs = -lp_offs;
218 u = 0;
219 }
220 if (lp_offs >= LINKPAGE_SIZE) {
221 fprintf(stderr, "linkpage too far: %d\n", lp_offs);
222 abort();
223 }
224
225 return (u << 23) | lp_offs;
226}
227
228static u32 make_jmp(u32 *pc, u32 *target)
229{
230 int jmp_val;
231
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);
235 abort();
236 }
237
238 return 0xea000000 | (jmp_val & 0x00ffffff);
239}
240
241static void emit_op(u32 op)
242{
243 *g_code_ptr++ = op;
244}
245
246static void emit_op_io(u32 op, u32 *target)
247{
248 op |= make_offset12(g_code_ptr, target);
249 emit_op(op);
250}
251
252static void segv_sigaction(int num, siginfo_t *info, void *ctx)
253{
254 struct ucontext *context = ctx;
255 u32 *regs = (u32 *)&context->uc_mcontext.arm_r0;
256 u32 *pc = (u32 *)regs[15];
257 u32 old_op = *pc;
258 u32 *pc_ptr, *old_op_ptr;
259 int lp_size;
260
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
263 {
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);
267 raise(num);
268 }
269 printf("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]);
270
271 // spit PC and op
272 pc_ptr = g_code_ptr++;
273 old_op_ptr = g_code_ptr++;
274 *pc_ptr = (u32)pc;
275 *old_op_ptr = old_op;
276
277 // emit jump to code ptr
278 *pc = make_jmp(pc, g_code_ptr);
279
280 // generate code:
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>
293
294 // sync caches
295 sys_cacheflush(pc, pc + 1);
296 sys_cacheflush(g_linkpage, g_code_ptr);
297
298 lp_size = (char *)g_code_ptr - (char *)g_linkpage;
299 printf("code #%d %d/%d\n", g_linkpage_count, lp_size, LINKPAGE_SIZE);
300
301 if (lp_size + 13*4 > LINKPAGE_SIZE) {
302 g_linkpage_count++;
303 if (g_linkpage_count >= LINKPAGE_COUNT) {
304 fprintf(stderr, "too many linkpages needed\n");
305 abort();
306 }
307 g_linkpage = (void *)((char *)g_linkpage + LINKPAGE_SIZE);
308 init_linkpage();
309 }
310 //handle_op(regs[15], op, regs, (u32)info->si_addr);
311 //regs[15] += 4;
312}
11913091 313
314void emu_init(void *map_bottom)
315{
316 struct sigaction segv_action = {
317 .sa_sigaction = segv_sigaction,
318 .sa_flags = SA_SIGINFO,
319 };
11913091 320 void *ret;
321
322 sigemptyset(&segv_action.sa_mask);
323 sigaction(SIGSEGV, &segv_action, NULL);
324
86418a84 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);
11913091 327 if (ret != g_linkpage) {
328 perror("mmap linkpage");
329 exit(1);
330 }
86418a84 331 printf("linkpages @ %p\n", g_linkpage);
332 init_linkpage();
11913091 333
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);
86418a84 337 //blitter = mmap(NULL, 0x100, PROT_READ|PROT_WRITE, MAP_PRIVATE|MAP_ANONYMOUS, -1, 0);
11913091 338 printf("mapped %d %p %p\n", memdev, memregs, blitter);
339}
340
341void *emu_mmap_dev(unsigned int length, int prot, int flags, unsigned int offset)
342{
343 char name[32];
344 int fd;
345
346 if ((offset & ~0xffff) == 0xc0000000) {
347 return mmap((void *)0x7f000000, length, PROT_NONE,
348 MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0);
349 }
350 if ((offset & ~0xffff) == 0xe0020000) {
351 return mmap((void *)0x7f100000, length, PROT_NONE,
352 MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0);
353 }
354 // pass through
355 if ((offset & 0xfe000000) == 0x02000000)
356 return mmap(NULL, length, prot, flags, memdev, offset);
357
358 sprintf(name, "m%08x", offset);
359 fd = open(name, O_CREAT|O_RDWR, 0644);
360 lseek(fd, length - 1, SEEK_SET);
361 name[0] = 0;
362 write(fd, name, 1);
363
364 return mmap(NULL, length, prot, MAP_SHARED, fd, 0);
365}
366