+ return 0xea000000 | (bl << 24) | (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 init_linkpage(void)
+{
+ g_linkpage->handler = handle_op;
+ g_linkpage->handler_stack = g_handler_stack_end;
+ g_code_ptr = g_linkpage->code;
+
+ // common_code.
+ // r0 and r14 must be saved by caller, r0 is arg for handle_op
+ // on return everything is restored except lr, which is used to return
+ emit_op_io(0xe50f1000, &g_linkpage->saved_regs[1]); // str r1, [->saved_regs[1]] @ save r1
+ emit_op (0xe24f1000 + // sub r1, pc, =offs(saved_regs[2])
+ (g_code_ptr - &g_linkpage->saved_regs[2] + 2) * 4);
+ emit_op (0xe8813ffc); // stmia r1, {r2-r13}
+ emit_op_io(0xe51fd000, // ldr sp, [->handler_stack]
+ (u32 *)&g_linkpage->handler_stack);
+ emit_op (0xe2414008); // sub r4, r1, #4*2
+ emit_op (0xe10f1000); // mrs r1, cpsr
+ emit_op_io(0xe50f1000, &g_linkpage->cpsr); // str r1, [->cpsr]
+ emit_op (0xe1a0500e); // mov r5, lr
+ emit_op (0xe1a0e00f); // mov lr, pc
+ emit_op_io(0xe51ff000, (u32 *)&g_linkpage->handler); // ldr pc, =handle_op
+ emit_op_io(0xe51f1000, &g_linkpage->cpsr); // ldr r1, [->cpsr]
+ emit_op (0xe128f001); // msr cpsr_f, r1
+ emit_op (0xe1a0e005); // mov lr, r5
+ emit_op (0xe8943fff); // ldmia r4, {r0-r13}
+ emit_op (0xe12fff1e); // bx lr @ return
+}
+
+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];
+ struct op_context *op_ctx;
+ 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
+ ((long)info->si_addr & 0xffe00000) != 0x7f000000) // faulting not where expected
+ {
+ // real crash - time to die
+ err("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]);
+ signal(num, SIG_DFL);
+ raise(num);
+ }
+ segvlog("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]);
+
+ // spit PC and op
+ op_ctx = (void *)g_code_ptr;
+ op_ctx->pc = (u32)pc;
+ op_ctx->op = *pc;
+ g_code_ptr = &op_ctx->code[0];
+
+ // emit jump to code ptr
+ *pc = make_jmp(pc, g_code_ptr, 0);
+
+ // generate code:
+ // TODO: multithreading
+ emit_op_io(0xe50f0000, &g_linkpage->saved_regs[0]); // str r0, [->saved_regs[0]] @ save r0
+ emit_op_io(0xe50fe000, &g_linkpage->saved_regs[14]); // str r14, [->saved_regs[14]]
+ emit_op (0xe24f0000 + (g_code_ptr - (u32 *)op_ctx + 2) * 4); // sub r0, pc, #op_ctx
+ emit_op (make_jmp(g_code_ptr, &g_linkpage->code[0], 1)); // bl common_code
+ emit_op_io(0xe51fe000, &g_linkpage->saved_regs[14]); // ldr r14, [->saved_regs[14]]
+ emit_op (make_jmp(g_code_ptr, pc + 1, 0)); // 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;
+ segvlog("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) {
+ err("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;
+}