+static int g_linkpage_count;
+
+static void init_linkpage(void)
+{
+ g_linkpage->lp_r1 = &g_linkpage->saved_regs[1];
+ g_linkpage->handler = handle_op;
+ g_code_ptr = g_linkpage->code;
+}
+
+static u32 make_offset12(u32 *pc, u32 *target)
+{
+ int lp_offs, u = 1;
+
+ lp_offs = (char *)target - (char *)pc - 2*4;
+ if (lp_offs < 0) {
+ lp_offs = -lp_offs;
+ u = 0;
+ }
+ if (lp_offs >= LINKPAGE_SIZE) {
+ fprintf(stderr, "linkpage too far: %d\n", lp_offs);
+ abort();
+ }
+
+ return (u << 23) | lp_offs;
+}
+
+static u32 make_jmp(u32 *pc, u32 *target)
+{
+ int jmp_val;
+
+ jmp_val = target - pc - 2;
+ if (jmp_val < (int)0xff000000 || jmp_val > 0x00ffffff) {
+ fprintf(stderr, "jump out of range (%p -> %p)\n", pc, target);
+ abort();
+ }
+
+ return 0xea000000 | (jmp_val & 0x00ffffff);
+}
+
+static void emit_op(u32 op)
+{
+ *g_code_ptr++ = op;
+}
+
+static void emit_op_io(u32 op, u32 *target)
+{
+ op |= make_offset12(g_code_ptr, target);
+ emit_op(op);
+}
+
+static void segv_sigaction(int num, siginfo_t *info, void *ctx)
+{
+ struct ucontext *context = ctx;
+ u32 *regs = (u32 *)&context->uc_mcontext.arm_r0;
+ u32 *pc = (u32 *)regs[15];
+ u32 old_op = *pc;
+ u32 *pc_ptr, *old_op_ptr;
+ int lp_size;
+
+ if (((regs[15] ^ (u32)&segv_sigaction) & 0xff000000) == 0 || // PC is in our segment or
+ (((regs[15] ^ (u32)g_linkpage) & ~(LINKPAGE_ALLOC - 1)) == 0)) // .. in linkpage
+ {
+ // real crash - time to die
+ printf("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]);
+ signal(num, SIG_DFL);
+ raise(num);
+ }
+ printf("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]);
+
+ // spit PC and op
+ pc_ptr = g_code_ptr++;
+ old_op_ptr = g_code_ptr++;
+ *pc_ptr = (u32)pc;
+ *old_op_ptr = old_op;
+
+ // emit jump to code ptr
+ *pc = make_jmp(pc, g_code_ptr);
+
+ // generate code:
+ // TODO: our own stack
+ emit_op_io(0xe50f0000, &g_linkpage->saved_regs[0]); // str r0, [saved_regs[0]] @ save r0
+ emit_op_io(0xe51f0000, (u32 *)&g_linkpage->lp_r1); // ldr r0, =lp_r1
+ emit_op (0xe8807ffe); // stmia r0, {r1-r14}
+ emit_op (0xe2402004); // sub r2, r0, #4
+ emit_op_io(0xe51f0000, pc_ptr); // ldr r0, =pc
+ emit_op_io(0xe51f1000, old_op_ptr); // ldr r1, =old_op
+ emit_op (0xe1a04002); // mov r4, r2
+ emit_op (0xe1a0e00f); // mov lr, pc
+ emit_op_io(0xe51ff000, (u32 *)&g_linkpage->handler); // ldr pc, =handle_op
+ emit_op (0xe8947fff); // ldmia r4, {r0-r14}
+ emit_op (make_jmp(g_code_ptr, pc + 1)); // jmp <back>
+
+ // sync caches
+ sys_cacheflush(pc, pc + 1);
+ sys_cacheflush(g_linkpage, g_code_ptr);
+
+ lp_size = (char *)g_code_ptr - (char *)g_linkpage;
+ printf("code #%d %d/%d\n", g_linkpage_count, lp_size, LINKPAGE_SIZE);
+
+ if (lp_size + 13*4 > LINKPAGE_SIZE) {
+ g_linkpage_count++;
+ if (g_linkpage_count >= LINKPAGE_COUNT) {
+ fprintf(stderr, "too many linkpages needed\n");
+ abort();
+ }
+ g_linkpage = (void *)((char *)g_linkpage + LINKPAGE_SIZE);
+ init_linkpage();
+ }
+ //handle_op(regs[15], op, regs, (u32)info->si_addr);
+ //regs[15] += 4;
+}