loader/emu.c

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