loader/emu.c

   1 // vim:shiftwidth=2:expandtab
   2 #include <stdio.h>
   3 #include <stdlib.h>
   4 #include <string.h>
   5 #include <sys/types.h>
   6 #include <sys/stat.h>
   7 #include <fcntl.h>
   8 #include <sys/mman.h>
   9 #include <sys/types.h>
  10 #include <unistd.h>
  11 #include <signal.h>
  12 #include <asm/ucontext.h>
  13
  14 #include "header.h"
  15 #include "sys_cacheflush.h"
  16
  17 //#define LOG_IO
  18 //#define LOG_IO_UNH
  19 //#define LOG_SEGV
  20
  21 #ifdef LOG_IO
  22 #define iolog log_io
  23 #else
  24 #define iolog(...)
  25 #endif
  26
  27 #ifdef LOG_IO_UNH
  28 #define iolog_unh log_io
  29 #else
  30 #define iolog_unh(...)
  31 #endif
  32
  33 #ifdef LOG_SEGV
  34 #define segvlog printf
  35 #else
  36 #define segvlog(...)
  37 #endif
  38
  39 #if defined(LOG_IO) || defined(LOG_IO_UNH)
  40 #include "mmsp2-regs.h"
  41 #endif
  42
  43 typedef unsigned int   u32;
  44 typedef unsigned short u16;
  45 typedef unsigned char  u8;
  46
  47 struct uppermem_block {
  48   u32 addr; // physical
  49   u32 size;
  50   void *mem;
  51   struct uppermem_block *next;
  52 };
  53
  54 static struct uppermem_block *upper_mem;
  55
  56 static struct {
  57   u32 dstctrl;
  58   u32 dstaddr;
  59   u32 dststride;
  60   u32 srcctrl;
  61   u32 srcaddr;          //
  62   u32 srcstride;
  63   u32 srcforcolor;
  64   u32 srcbackcolor;
  65   u32 patctrl;          //
  66   u32 patforcolor;
  67   u32 patbackcolor;
  68   u32 size;
  69   u32 ctrl;             //
  70   u32 run;
  71   u32 intc;
  72   u32 srcfifo;
  73 } blitter;
  74
  75 #define SRCCTRL_INVIDEO         (1 << 8)
  76 #define SRCCTRL_SRCENB          (1 << 7)
  77 #define CTRL_TRANSPARENCYENB    (1 << 11)
  78
  79 static struct {
  80   u16 mlc_stl_cntl;
  81   union {
  82     u32 mlc_stl_adr;
  83     struct {
  84       u16 mlc_stl_adrl;
  85       u16 mlc_stl_adrh;
  86     };
  87   };
  88   u16 mlc_stl_pallt_a;
  89   union {
  90     u16 mlc_stl_pallt_d[256*2];
  91     u32 mlc_stl_pallt_d32[256];
  92   };
  93
  94   // state
  95   u16 host_pal[256];
  96   u32 old_mlc_stl_adr;
  97   u32 btn_state; // as seen through /dev/GPIO
  98   u16 dirty_pal:1;
  99 } mmsp2;
 100
 101 static u16 *host_screen;
 102 static int host_stride;
 103
 104
 105 #if defined(LOG_IO) || defined(LOG_IO_UNH)
 106 static void log_io(const char *pfx, u32 a, u32 d, int size)
 107 {
 108   const char *fmt, *reg = "";
 109   switch (size) {
 110   case  8: fmt = "%s %08x       %02x %s\n"; break;
 111   case 32: fmt = "%s %08x %08x %s\n"; break;
 112   default: fmt = "%s %08x     %04x %s\n"; break;
 113   }
 114
 115   if ((a & ~0xffff) == 0x7f000000)
 116     reg = regnames[a & 0xffff];
 117
 118   printf(fmt, pfx, a, d, reg);
 119 }
 120 #endif
 121
 122 static void memset16(void *dst, u32 pattern, int count)
 123 {
 124   u32 *dl;
 125   u16 *d;
 126
 127   d = (u16 *)((long)dst & ~1);
 128   if ((long)d & 2) {
 129     *d++ = pattern;
 130     count--;
 131   }
 132   dl = (void *)d;
 133   pattern |= pattern << 16;
 134
 135   while (count >= 2) {
 136     *dl++ = pattern;
 137     count -= 2;
 138   }
 139   if (count)
 140     *(u16 *)dl = pattern;
 141 }
 142
 143 static void blt_tr(void *dst, void *src, u32 trc, int w)
 144 {
 145   u16 *d = (u16 *)((long)dst & ~1);
 146   u16 *s = (u16 *)((long)src & ~1);
 147
 148   // XXX: optimize
 149   for (; w > 0; d++, s++, w--)
 150     if (*s != trc)
 151       *d = *s;
 152 }
 153
 154 #define dump_blitter() \
 155 { \
 156   u32 *r = &blitter.dstctrl; \
 157   int i; \
 158   for (i = 0; i < 4*4; i++, r++) { \
 159     printf("%08x ", *r); \
 160     if ((i & 3) == 3) \
 161       printf("\n"); \
 162   } \
 163 }
 164
 165 static void *uppermem_lookup(u32 addr, u8 **mem_end)
 166 {
 167   struct uppermem_block *ub;
 168
 169   for (ub = upper_mem; ub != NULL; ub = ub->next) {
 170     if (ub->addr <= addr && addr < ub->addr + ub->size) {
 171       *mem_end = (u8 *)ub->mem + ub->size;
 172       return (u8 *)ub->mem + addr - ub->addr;
 173     }
 174   }
 175
 176   return NULL;
 177 }
 178
 179 static void *blitter_mem_lookup(u32 addr, u8 **mem_end, int *stride_override, int *to_screen)
 180 {
 181   // maybe the screen?
 182   if (mmsp2.mlc_stl_adr <= addr && addr < mmsp2.mlc_stl_adr + 320*240*2) {
 183     *mem_end = (u8 *)host_screen + host_stride * 240;
 184     *stride_override = host_stride;
 185     *to_screen = 1;
 186     return (u8 *)host_screen + addr - mmsp2.mlc_stl_adr;
 187   }
 188
 189   return uppermem_lookup(addr, mem_end);
 190 }
 191
 192 static void blitter_do(void)
 193 {
 194   u8 *dst, *dste, *src = NULL, *srce = NULL;
 195   int w, h, sstrd, dstrd;
 196   int to_screen = 0;
 197   u32 addr;
 198
 199   w = blitter.size & 0x7ff;
 200   h = (blitter.size >> 16) & 0x7ff;
 201   sstrd = blitter.srcstride;
 202   dstrd = blitter.dststride;
 203
 204   // XXX: need to confirm this..
 205   addr = (blitter.dstaddr & ~3) | ((blitter.dstctrl & 0x1f) >> 3);
 206   dst = blitter_mem_lookup(addr, &dste, &dstrd, &to_screen);
 207   if (dst == NULL)
 208     goto bad_blit;
 209
 210   // XXX: assume fill if no SRCENB, but it could be pattern blit..
 211   if (blitter.srcctrl & SRCCTRL_SRCENB) {
 212     if (!(blitter.srcctrl & SRCCTRL_INVIDEO))
 213       goto bad_blit;
 214
 215     addr = (blitter.srcaddr & ~3) | ((blitter.srcctrl & 0x1f) >> 3);
 216     src = blitter_mem_lookup(addr, &srce, &sstrd, &to_screen);
 217     if (src == NULL)
 218       goto bad_blit;
 219
 220     if (src + sstrd * h > srce) {
 221       err("blit %08x->%08x %dx%d did not fit src\n",
 222         blitter.srcaddr, blitter.dstaddr, w, h);
 223       h = (srce - src) / sstrd;
 224     }
 225   }
 226
 227   if (dst + dstrd * h > dste) {
 228     err("blit %08x->%08x %dx%d did not fit dst\n",
 229       blitter.srcaddr, blitter.dstaddr, w, h);
 230     h = (dste - dst) / dstrd;
 231   }
 232
 233   if (src != NULL) {
 234     // copy
 235     if (blitter.ctrl & CTRL_TRANSPARENCYENB) {
 236       u32 trc = blitter.ctrl >> 16;
 237       for (; h > 0; h--, dst += dstrd, src += sstrd)
 238         blt_tr(dst, src, trc, w);
 239     }
 240     else {
 241       for (; h > 0; h--, dst += dstrd, src += sstrd)
 242         memcpy(dst, src, w * 2);
 243     }
 244   }
 245   else {
 246     // fill. Assume the pattern is cleared and bg color is used
 247     u32 bgc = blitter.patbackcolor & 0xffff;
 248     for (; h > 0; h--, dst += dstrd)
 249       memset16(dst, bgc, w);
 250   }
 251
 252   if (to_screen)
 253     host_screen = host_video_flip();
 254   return;
 255
 256 bad_blit:
 257   err("blit %08x->%08x %dx%d translated to %p->%p\n",
 258     blitter.srcaddr, blitter.dstaddr, w, h, src, dst);
 259   dump_blitter();
 260 }
 261
 262 // TODO: hw scaler stuff
 263 static void mlc_flip(u32 addr)
 264 {
 265   int mode = (mmsp2.mlc_stl_cntl >> 9) & 3;
 266   int bpp = mode ? mode * 8 : 4;
 267   u16 *dst = host_screen;
 268   u16 *hpal = mmsp2.host_pal;
 269   u8 *src, *src_end;
 270   int i, u;
 271
 272   src = uppermem_lookup(addr, &src_end);
 273   if (src == NULL || src + 320*240 * bpp / 8 > src_end) {
 274     err("mlc_flip: %08x is out of range\n", addr);
 275     return;
 276   }
 277
 278   if (bpp <= 8 && mmsp2.dirty_pal) {
 279     u32 *srcp = mmsp2.mlc_stl_pallt_d32;
 280     u16 *dstp = hpal;
 281
 282     for (i = 0; i < 256; i++, srcp++, dstp++) {
 283       u32 t = *srcp;
 284       *dstp = ((t >> 8) & 0xf800) | ((t >> 5) & 0x07e0) | ((t >> 3) & 0x001f);
 285     }
 286     mmsp2.dirty_pal = 0;
 287   }
 288
 289   switch (bpp) {
 290   case  4:
 291     for (i = 0; i < 240; i++, dst += host_stride / 2 - 320) {
 292       for (u = 320 / 2; u > 0; u--, src++) {
 293         *dst++ = hpal[*src >> 4];
 294         *dst++ = hpal[*src & 0x0f];
 295       }
 296     }
 297     break;
 298
 299   case  8:
 300     for (i = 0; i < 240; i++, dst += host_stride / 2 - 320) {
 301       for (u = 320 / 4; u > 0; u--) {
 302         *dst++ = hpal[*src++];
 303         *dst++ = hpal[*src++];
 304         *dst++ = hpal[*src++];
 305         *dst++ = hpal[*src++];
 306       }
 307     }
 308     break;
 309
 310   case 16:
 311     for (i = 0; i < 240; i++, dst += host_stride / 2, src += 320*2)
 312       memcpy(dst, src, 320*2);
 313     break;
 314
 315   case 24:
 316     // TODO
 317     break;
 318   }
 319
 320   host_screen = host_video_flip();
 321 }
 322
 323 static u32 xread8(u32 a)
 324 {
 325   iolog("r8 ", a, 0, 8);
 326   iolog_unh("r8 ", a, 0, 8);
 327   return 0;
 328 }
 329
 330 static u32 xread16(u32 a)
 331 {
 332   static u32 fudge, old_a;
 333   u32 d = 0, t;
 334
 335   if ((a & 0xffff0000) == 0x7f000000) {
 336     u32 a_ = a & 0xffff;
 337     switch (a_) {
 338     case 0x0910: // FPLL
 339     case 0x0912:
 340       d = 0x9407;
 341       break;
 342     // minilib reads as:
 343     //  0000 P000 VuVd00 0000 YXBA RLSeSt 0R0D 0L0U
 344     // |        GPIOD        |GPIOC[8:15]|GPIOM[0:7]|
 345     // /dev/GPIO:
 346     //             ... 0PVdVu ...
 347     case 0x1184: // GPIOC
 348       d = ~mmsp2.btn_state & 0xff00;
 349       d |= 0x00ff;
 350       break;
 351     case 0x1186: // GPIOD
 352       t = ~mmsp2.btn_state;
 353       d  = (t >> 9)  & 0x0080;
 354       d |= (t >> 11) & 0x0040;
 355       d |= (t >> 7)  & 0x0800;
 356       d |= 0x373b;
 357       break;
 358     case 0x1198: // GPIOM
 359       mmsp2.btn_state = host_read_btns();
 360       d = ~mmsp2.btn_state & 0xff;
 361       d |= 0x01aa;
 362       break;
 363     case 0x28da:
 364       d = mmsp2.mlc_stl_cntl;
 365       break;
 366     case 0x2958:
 367       d = mmsp2.mlc_stl_pallt_a;
 368       break;
 369     default:
 370       goto unh;
 371     }
 372     goto out;
 373   }
 374
 375 unh:
 376   if (a == old_a) {
 377     d = fudge;
 378     fudge = ~fudge;
 379   }
 380   old_a = a;
 381   iolog_unh("r16", a, d & 0xffff, 16);
 382
 383 out:
 384   d &= 0xffff;
 385   iolog("r16", a, d, 16);
 386   return d;
 387 }
 388
 389 static u32 xread32(u32 a)
 390 {
 391   u32 d = 0;
 392   if ((a & 0xfff00000) == 0x7f100000) {
 393     u32 *bl = &blitter.dstctrl;
 394     u32 a_ = a & 0xfff;
 395     if (a_ < 0x40) {
 396       d = bl[a_ / 4];
 397       if (a_ == 0x34)
 398         d = 0; // not busy
 399       goto out;
 400     }
 401   }
 402   iolog_unh("r32", a, d, 32);
 403
 404 out:
 405   iolog("r32", a, d, 32);
 406   return d;
 407 }
 408
 409 static void xwrite8(u32 a, u32 d)
 410 {
 411   iolog("w8 ", a, d, 8);
 412   iolog_unh("w8 ", a, d, 8);
 413 }
 414
 415 static void xwrite16(u32 a, u32 d)
 416 {
 417   iolog("w16", a, d, 16);
 418   if ((a & 0xfff00000) == 0x7f000000) {
 419     u32 a_ = a & 0xffff;
 420     switch (a_) {
 421     case 0x28da:
 422       mmsp2.mlc_stl_cntl = d | 0xaa;
 423       break;
 424     case 0x290e:
 425     case 0x2910:
 426       // odd addresses don't affect LCD. What about TV?
 427       return;
 428     case 0x2912:
 429       mmsp2.mlc_stl_adrl = d;
 430       return;
 431     case 0x2914:
 432       mmsp2.mlc_stl_adrh = d;
 433       if (mmsp2.mlc_stl_adr != mmsp2.old_mlc_stl_adr)
 434         mlc_flip(mmsp2.mlc_stl_adr);
 435       mmsp2.old_mlc_stl_adr = mmsp2.mlc_stl_adr;
 436       return;
 437     case 0x2958:
 438       mmsp2.mlc_stl_pallt_a = d & 0x1ff;
 439       return;
 440     case 0x295a:
 441       mmsp2.mlc_stl_pallt_d[mmsp2.mlc_stl_pallt_a++] = d;
 442       mmsp2.mlc_stl_pallt_a &= 0x1ff;
 443       mmsp2.dirty_pal = 1;
 444       return;
 445     }
 446   }
 447   iolog_unh("w16", a, d, 16);
 448 }
 449
 450 static void xwrite32(u32 a, u32 d)
 451 {
 452   iolog("w32", a, d, 32);
 453
 454   if ((a & 0xfff00000) == 0x7f100000) {
 455     u32 *bl = &blitter.dstctrl;
 456     u32 a_ = a & 0xfff;
 457     if (a_ < 0x40) {
 458       bl[a_ / 4] = d;
 459       if (a_ == 0x34 && (d & 1))
 460         blitter_do();
 461       return;
 462     }
 463   }
 464   iolog_unh("w32", a, d, 32);
 465 }
 466
 467 #define LINKPAGE_SIZE 0x1000
 468 #define LINKPAGE_COUNT 4
 469 #define LINKPAGE_ALLOC (LINKPAGE_SIZE * LINKPAGE_COUNT)
 470
 471 struct op_context {
 472   u32 pc;
 473   u32 op;
 474   u32 code[0];
 475 };
 476
 477 struct linkpage {
 478   u32 saved_regs[15];
 479   u32 cpsr;
 480   u32 *handler_stack;
 481   void (*handler)(struct op_context *op_ctx);
 482   u32 code[0];
 483 };
 484
 485 static struct linkpage *g_linkpage;
 486 static u32 *g_code_ptr;
 487 static int g_linkpage_count;
 488
 489 #define HANDLER_STACK_SIZE 4096
 490 static void *g_handler_stack_end;
 491
 492 #define BIT_SET(v, b) (v & (1 << (b)))
 493
 494 static void handle_op(struct op_context *op_ctx)
 495 {
 496   u32 *regs = g_linkpage->saved_regs;
 497   u32 op = op_ctx->op;
 498   u32 t, shift, ret, addr;
 499   int rn, rd;
 500
 501   rd = (op & 0x0000f000) >> 12;
 502   rn = (op & 0x000f0000) >> 16;
 503
 504   if ((op & 0x0f200090) == 0x01000090) { // AM3: LDRH, STRH
 505     if (!BIT_SET(op, 5)) // !H
 506       goto unhandled;
 507     if (BIT_SET(op, 6) && !BIT_SET(op, 20)) // S && !L
 508       goto unhandled;
 509
 510     if (BIT_SET(op, 22))                // imm offset
 511       t = ((op & 0xf00) >> 4) | (op & 0x0f);
 512     else                                // reg offset
 513       t = regs[op & 0x000f];
 514
 515     if (!BIT_SET(op, 23))
 516       t = -t;
 517     addr = regs[rn] + t;
 518
 519     if (BIT_SET(op, 20)) { // Load
 520       ret = xread16(addr);
 521       if (BIT_SET(op, 6)) { // S
 522         ret <<= 16;
 523         ret = (signed int)ret >> 16;
 524       }
 525       regs[rd] = ret;
 526     }
 527     else
 528       xwrite16(addr, regs[rd]);
 529   }
 530   else if ((op & 0x0d200000) == 0x05000000) { // AM2: LDR[B], STR[B]
 531     if (BIT_SET(op, 25)) {              // reg offs
 532       if (BIT_SET(op, 4))
 533         goto unhandled;
 534
 535       t = regs[op & 0x000f];
 536       shift = (op & 0x0f80) >> 7;
 537       switch ((op & 0x0060) >> 5) {
 538         case 0: t = t << shift; break;
 539         case 1: t = t >> (shift + 1); break;
 540         case 2: t = (signed int)t >> (shift + 1); break;
 541         case 3: goto unhandled; // I'm just lazy
 542       }
 543     }
 544     else                                // imm offs
 545       t = op & 0x0fff;
 546
 547     if (!BIT_SET(op, 23))
 548       t = -t;
 549     addr = regs[rn] + t;
 550
 551     if (BIT_SET(op, 20)) { // Load
 552       if (BIT_SET(op, 22)) // Byte
 553         ret = xread8(addr);
 554       else
 555         ret = xread32(addr);
 556       regs[rd] = ret;
 557     }
 558     else {
 559       if (BIT_SET(op, 22)) // Byte
 560         xwrite8(addr, regs[rd]);
 561       else
 562         xwrite32(addr, regs[rd]);
 563     }
 564   }
 565   else
 566     goto unhandled;
 567
 568 #if 0
 569   if (addr != addr_check) {
 570     fprintf(stderr, "bad calculated addr: %08x vs %08x\n", addr, addr_check);
 571     abort();
 572   }
 573 #endif
 574   return;
 575
 576 unhandled:
 577   err("unhandled IO op %08x @ %08x\n", op, op_ctx->pc);
 578 }
 579
 580 static u32 make_offset12(u32 *pc, u32 *target)
 581 {
 582   int lp_offs, u = 1;
 583
 584   lp_offs = (char *)target - (char *)pc - 2*4;
 585   if (lp_offs < 0) {
 586     lp_offs = -lp_offs;
 587     u = 0;
 588   }
 589   if (lp_offs >= LINKPAGE_SIZE) {
 590     err("linkpage too far: %d\n", lp_offs);
 591     abort();
 592   }
 593
 594   return (u << 23) | lp_offs;
 595 }
 596
 597 static u32 make_jmp(u32 *pc, u32 *target, int bl)
 598 {
 599   int jmp_val;
 600
 601   jmp_val = target - pc - 2;
 602   if (jmp_val < (int)0xff000000 || jmp_val > 0x00ffffff) {
 603     err("jump out of range (%p -> %p)\n", pc, target);
 604     abort();
 605   }
 606
 607   return 0xea000000 | (bl << 24) | (jmp_val & 0x00ffffff);
 608 }
 609
 610 static void emit_op(u32 op)
 611 {
 612   *g_code_ptr++ = op;
 613 }
 614
 615 static void emit_op_io(u32 op, u32 *target)
 616 {
 617   op |= make_offset12(g_code_ptr, target);
 618   emit_op(op);
 619 }
 620
 621 static void init_linkpage(void)
 622 {
 623   g_linkpage->handler = handle_op;
 624   g_linkpage->handler_stack = g_handler_stack_end;
 625   g_code_ptr = g_linkpage->code;
 626
 627   // common_code.
 628   // r0 and r14 must be saved by caller, r0 is arg for handle_op
 629   // on return everything is restored except lr, which is used to return
 630   emit_op_io(0xe50f1000, &g_linkpage->saved_regs[1]);  // str r1, [->saved_regs[1]] @ save r1
 631   emit_op   (0xe24f1000 +                              // sub r1, pc, =offs(saved_regs[2])
 632     (g_code_ptr - &g_linkpage->saved_regs[2] + 2) * 4);
 633   emit_op   (0xe8813ffc);                              // stmia r1, {r2-r13}
 634   emit_op_io(0xe51fd000,                               // ldr sp, [->handler_stack]
 635     (u32 *)&g_linkpage->handler_stack);
 636   emit_op   (0xe2414008);                              // sub r4, r1, #4*2
 637   emit_op   (0xe10f1000);                              // mrs r1, cpsr
 638   emit_op_io(0xe50f1000, &g_linkpage->cpsr);           // str r1, [->cpsr]
 639   emit_op   (0xe1a0500e);                              // mov r5, lr
 640   emit_op   (0xe1a0e00f);                              // mov lr, pc
 641   emit_op_io(0xe51ff000, (u32 *)&g_linkpage->handler); // ldr pc, =handle_op
 642   emit_op_io(0xe51f1000, &g_linkpage->cpsr);           // ldr r1, [->cpsr]
 643   emit_op   (0xe128f001);                              // msr cpsr_f, r1
 644   emit_op   (0xe1a0e005);                              // mov lr, r5
 645   emit_op   (0xe8943fff);                              // ldmia r4, {r0-r13}
 646   emit_op   (0xe12fff1e);                              // bx lr @ return
 647 }
 648
 649 static void segv_sigaction(int num, siginfo_t *info, void *ctx)
 650 {
 651   struct ucontext *context = ctx;
 652   u32 *regs = (u32 *)&context->uc_mcontext.arm_r0;
 653   u32 *pc = (u32 *)regs[15];
 654   struct op_context *op_ctx;
 655   int lp_size;
 656
 657   if (((regs[15] ^ (u32)&segv_sigaction) & 0xff000000) == 0 ||         // PC is in our segment or
 658       (((regs[15] ^ (u32)g_linkpage) & ~(LINKPAGE_ALLOC - 1)) == 0) || // .. in linkpage
 659       ((long)info->si_addr & 0xffe00000) != 0x7f000000)                // faulting not where expected
 660   {
 661     // real crash - time to die
 662     err("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]);
 663     signal(num, SIG_DFL);
 664     raise(num);
 665   }
 666   segvlog("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]);
 667
 668   // spit PC and op
 669   op_ctx = (void *)g_code_ptr;
 670   op_ctx->pc = (u32)pc;
 671   op_ctx->op = *pc;
 672   g_code_ptr = &op_ctx->code[0];
 673
 674   // emit jump to code ptr
 675   *pc = make_jmp(pc, g_code_ptr, 0);
 676
 677   // generate code:
 678   // TODO: multithreading
 679   emit_op_io(0xe50f0000, &g_linkpage->saved_regs[0]);            // str r0,  [->saved_regs[0]] @ save r0
 680   emit_op_io(0xe50fe000, &g_linkpage->saved_regs[14]);           // str r14, [->saved_regs[14]]
 681   emit_op   (0xe24f0000 + (g_code_ptr - (u32 *)op_ctx + 2) * 4); // sub r0, pc, #op_ctx
 682   emit_op   (make_jmp(g_code_ptr, &g_linkpage->code[0], 1));     // bl common_code
 683   emit_op_io(0xe51fe000, &g_linkpage->saved_regs[14]);           // ldr r14, [->saved_regs[14]]
 684   emit_op   (make_jmp(g_code_ptr, pc + 1, 0));                   // jmp <back>
 685
 686   // sync caches
 687   sys_cacheflush(pc, pc + 1);
 688   sys_cacheflush(g_linkpage, g_code_ptr);
 689
 690   lp_size = (char *)g_code_ptr - (char *)g_linkpage;
 691   segvlog("code #%d %d/%d\n", g_linkpage_count, lp_size, LINKPAGE_SIZE);
 692
 693   if (lp_size + 13*4 > LINKPAGE_SIZE) {
 694     g_linkpage_count++;
 695     if (g_linkpage_count >= LINKPAGE_COUNT) {
 696       err("too many linkpages needed\n");
 697       abort();
 698     }
 699     g_linkpage = (void *)((char *)g_linkpage + LINKPAGE_SIZE);
 700     init_linkpage();
 701   }
 702   //handle_op(regs[15], op, regs, (u32)info->si_addr);
 703   //regs[15] += 4;
 704 }
 705
 706 void emu_init(void *map_bottom)
 707 {
 708   struct sigaction segv_action = {
 709     .sa_sigaction = segv_sigaction,
 710     .sa_flags = SA_SIGINFO,
 711   };
 712   void *pret;
 713   int ret;
 714
 715   sigemptyset(&segv_action.sa_mask);
 716   sigaction(SIGSEGV, &segv_action, NULL);
 717
 718   pret = mmap(NULL, HANDLER_STACK_SIZE + 4096, PROT_NONE, MAP_PRIVATE|MAP_ANONYMOUS|MAP_NORESERVE, -1, 0);
 719   if (pret == MAP_FAILED) {
 720     perror(PFX "mmap handler_stack");
 721     exit(1);
 722   }
 723   ret = mprotect((char *)pret + 4096, HANDLER_STACK_SIZE, PROT_READ | PROT_WRITE);
 724   if (ret != 0) {
 725     perror(PFX "mprotect handler_stack");
 726     exit(1);
 727   }
 728   g_handler_stack_end = (char *)pret + HANDLER_STACK_SIZE + 4096;
 729
 730   g_linkpage = (void *)(((u32)map_bottom - LINKPAGE_ALLOC) & ~0xfff);
 731   pret = mmap(g_linkpage, LINKPAGE_ALLOC, PROT_READ|PROT_WRITE, MAP_PRIVATE|MAP_ANONYMOUS, -1, 0);
 732   if (pret != g_linkpage) {
 733     perror(PFX "mmap linkpage");
 734     exit(1);
 735   }
 736   printf("linkpages @ %p\n", g_linkpage);
 737   init_linkpage();
 738
 739   // host stuff
 740   ret = host_video_init(&host_stride, 0);
 741   if (ret != 0) {
 742     err("can't alloc screen\n");
 743     exit(1);
 744   }
 745   host_screen = host_video_flip();
 746 }
 747
 748 int emu_read_gpiodev(void *buf, int count)
 749 {
 750   unsigned int btns;
 751
 752   if (count < 4) {
 753     err("gpiodev read %d?\n", count);
 754     return -1;
 755   }
 756
 757   btns = host_read_btns();
 758   memcpy(buf, &btns, 4);
 759   return 4;
 760 }
 761
 762 void *emu_mmap_dev(unsigned int length, int prot, int flags, unsigned int offset)
 763 {
 764   struct uppermem_block *umem;
 765   char name[32];
 766   int fd;
 767
 768   // SoC regs
 769   if ((offset & ~0xffff) == 0xc0000000) {
 770     return mmap((void *)0x7f000000, length, PROT_NONE,
 771       MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0);
 772   }
 773   // blitter
 774   if ((offset & ~0xffff) == 0xe0020000) {
 775     return mmap((void *)0x7f100000, length, PROT_NONE,
 776       MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0);
 777   }
 778   // upper mem
 779   if ((offset & 0xfe000000) != 0x02000000)
 780     err("unexpected devmem mmap @ %08x\n", offset);
 781
 782   // return mmap(NULL, length, prot, flags, memdev, offset);
 783
 784   umem = calloc(1, sizeof(*umem));
 785   if (umem == NULL) {
 786     printf("OOM\n");
 787     return MAP_FAILED;
 788   }
 789
 790   umem->addr = offset;
 791   umem->size = length;
 792   umem->mem = mmap(NULL, length, prot, MAP_PRIVATE|MAP_ANONYMOUS, -1, 0);
 793   if (umem->mem != MAP_FAILED)
 794     goto done;
 795
 796   printf("upper mem @ %08x %d mmap fail, trying backing file\n", offset, length);
 797   sprintf(name, "m%08x", offset);
 798   fd = open(name, O_CREAT|O_RDWR, 0644);
 799   lseek(fd, length - 1, SEEK_SET);
 800   name[0] = 0;
 801   write(fd, name, 1);
 802
 803   umem->mem = mmap(NULL, length, prot, MAP_SHARED, fd, 0);
 804   if (umem->mem == MAP_FAILED) {
 805     err("failed, giving up\n");
 806     close(fd);
 807     free(umem);
 808     return MAP_FAILED;
 809   }
 810
 811 done:
 812   printf("upper mem @ %08x %d\n", offset, length);
 813   umem->next = upper_mem;
 814   upper_mem = umem;
 815   return umem->mem;
 816 }
 817