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 #include <sys/ioctl.h>
  19 #include <linux/soundcard.h>
  20 #include <linux/fb.h>
  21
  22 #include "header.h"
  23 #include "../common/host_fb.h"
  24 #include "../common/cmn.h"
  25 #include "sys_cacheflush.h"
  26 #include "realfuncs.h"
  27
  28 #if (dbg & 2)
  29 #define LOG_IO_UNK
  30 #endif
  31 #if (dbg & 4)
  32 #define LOG_IO
  33 #endif
  34 //#define LOG_SEGV
  35
  36 #ifdef LOG_IO
  37 #define iolog log_io
  38 #else
  39 #define iolog(...)
  40 #endif
  41
  42 #ifdef LOG_IO_UNK
  43 #define iolog_unh log_io
  44 #else
  45 #define iolog_unh(...)
  46 #endif
  47
  48 #ifdef LOG_SEGV
  49 #define segvlog printf
  50 #else
  51 #define segvlog(...)
  52 #endif
  53
  54 #if defined(LOG_IO) || defined(LOG_IO_UNK)
  55 #include "mmsp2-regs.h"
  56 #endif
  57
  58 typedef unsigned int   u32;
  59 typedef unsigned short u16;
  60 typedef unsigned char  u8;
  61
  62 static pthread_mutex_t fb_mutex = PTHREAD_MUTEX_INITIALIZER;
  63 static pthread_cond_t fb_cond = PTHREAD_COND_INITIALIZER;
  64
  65 struct uppermem_block {
  66   u32 addr; // physical
  67   u32 size;
  68   void *mem;
  69   struct uppermem_block *next;
  70 };
  71
  72 static struct uppermem_block *upper_mem;
  73
  74 static struct {
  75   u32 dstctrl;
  76   u32 dstaddr;
  77   u32 dststride;
  78   u32 srcctrl;
  79   u32 srcaddr;          //
  80   u32 srcstride;
  81   u32 srcforcolor;
  82   u32 srcbackcolor;
  83   u32 patctrl;          //
  84   u32 patforcolor;
  85   u32 patbackcolor;
  86   u32 size;
  87   u32 ctrl;             //
  88   u32 run;
  89   u32 intc;
  90   u32 srcfifo;
  91 } blitter;
  92
  93 #define SRCCTRL_INVIDEO         (1 << 8)
  94 #define SRCCTRL_SRCENB          (1 << 7)
  95 #define CTRL_TRANSPARENCYENB    (1 << 11)
  96
  97 static struct {
  98   u16 mlc_stl_cntl;
  99   union {
 100     u32 mlc_stl_adr;
 101     struct {
 102       u16 mlc_stl_adrl;
 103       u16 mlc_stl_adrh;
 104     };
 105   };
 106   u16 mlc_stl_pallt_a;
 107   union {
 108     u16 mlc_stl_pallt_d[256*2];
 109     u32 mlc_stl_pallt_d32[256];
 110   };
 111
 112   // state
 113   u16 host_pal[256];
 114   u32 old_mlc_stl_adr;
 115   u32 btn_state; // as seen through /dev/GPIO
 116   u32 dirty_pal:1;
 117 } mmsp2;
 118
 119 static u16 *host_screen;
 120 static int host_stride;
 121
 122
 123 #if defined(LOG_IO) || defined(LOG_IO_UNK)
 124 static void log_io(const char *pfx, u32 a, u32 d, int size)
 125 {
 126   const char *fmt, *reg = "";
 127   switch (size) {
 128   case  8: fmt = "%s %08x       %02x %s\n"; d &= 0xff; break;
 129   case 32: fmt = "%s %08x %08x %s\n";       break;
 130   default: fmt = "%s %08x     %04x %s\n";   d &= 0xffff; break;
 131   }
 132
 133   if ((a & ~0xffff) == 0x7f000000)
 134     reg = regnames[a & 0xffff];
 135
 136   printf(fmt, pfx, a, d, reg);
 137 }
 138 #endif
 139
 140 static void memset16(void *dst, u32 pattern, int count)
 141 {
 142   u32 *dl;
 143   u16 *d;
 144
 145   d = (u16 *)((long)dst & ~1);
 146   if ((long)d & 2) {
 147     *d++ = pattern;
 148     count--;
 149   }
 150   dl = (void *)d;
 151   pattern |= pattern << 16;
 152
 153   while (count >= 2) {
 154     *dl++ = pattern;
 155     count -= 2;
 156   }
 157   if (count)
 158     *(u16 *)dl = pattern;
 159 }
 160
 161 static void blt_tr(void *dst, void *src, u32 trc, int w)
 162 {
 163   u16 *d = (u16 *)((long)dst & ~1);
 164   u16 *s = (u16 *)((long)src & ~1);
 165
 166   // XXX: optimize
 167   for (; w > 0; d++, s++, w--)
 168     if (*s != trc)
 169       *d = *s;
 170 }
 171
 172 #define dump_blitter() \
 173 { \
 174   u32 *r = &blitter.dstctrl; \
 175   int i; \
 176   for (i = 0; i < 4*4; i++, r++) { \
 177     printf("%08x ", *r); \
 178     if ((i & 3) == 3) \
 179       printf("\n"); \
 180   } \
 181 }
 182
 183 static void *uppermem_lookup(u32 addr, u8 **mem_end)
 184 {
 185   struct uppermem_block *ub;
 186
 187   for (ub = upper_mem; ub != NULL; ub = ub->next) {
 188     if (ub->addr <= addr && addr < ub->addr + ub->size) {
 189       *mem_end = (u8 *)ub->mem + ub->size;
 190       return (u8 *)ub->mem + addr - ub->addr;
 191     }
 192   }
 193
 194   return NULL;
 195 }
 196
 197 static void blitter_do(void)
 198 {
 199   u8 *dst, *dste, *src = NULL, *srce = NULL;
 200   int w, h, sstrd, dstrd;
 201   int to_screen = 0;
 202   u32 bpp, addr;
 203
 204   w = blitter.size & 0x7ff;
 205   h = (blitter.size >> 16) & 0x7ff;
 206   sstrd = blitter.srcstride;
 207   dstrd = blitter.dststride;
 208
 209   // XXX: need to confirm this..
 210   addr = (blitter.dstaddr & ~3) | ((blitter.dstctrl & 0x1f) >> 3);
 211
 212   // use dst bpp.. How does it do blits with different src bpp?
 213   bpp = (blitter.dstctrl & 0x20) ? 16 : 8;
 214
 215   // maybe the screen?
 216   if (((w == 320 && h == 240) || // blit whole screen
 217        (w * h >= 320*240/2)) &&  // ..or at least half of the area
 218        mmsp2.mlc_stl_adr <= addr && addr < mmsp2.mlc_stl_adr + 320*240*2)
 219     to_screen = 1;
 220
 221   dst = uppermem_lookup(addr, &dste);
 222
 223   // XXX: assume fill if no SRCENB, but it could be pattern blit..
 224   if (blitter.srcctrl & SRCCTRL_SRCENB) {
 225     if (!(blitter.srcctrl & SRCCTRL_INVIDEO))
 226       goto bad_blit;
 227
 228     addr = (blitter.srcaddr & ~3) | ((blitter.srcctrl & 0x1f) >> 3);
 229     src = uppermem_lookup(addr, &srce);
 230     if (src == NULL)
 231       goto bad_blit;
 232
 233     if (src + sstrd * h > srce) {
 234       err("blit %08x->%08x %dx%d did not fit src\n",
 235         blitter.srcaddr, blitter.dstaddr, w, h);
 236       h = (srce - src) / sstrd;
 237     }
 238   }
 239
 240   if (dst == NULL)
 241     goto bad_blit;
 242
 243   if (dst + dstrd * h > dste) {
 244     err("blit %08x->%08x %dx%d did not fit dst\n",
 245       blitter.srcaddr, blitter.dstaddr, w, h);
 246     h = (dste - dst) / dstrd;
 247   }
 248
 249   if (src != NULL) {
 250     // copy
 251     if (bpp == 16 && (blitter.ctrl & CTRL_TRANSPARENCYENB)) {
 252       u32 trc = blitter.ctrl >> 16;
 253       for (; h > 0; h--, dst += dstrd, src += sstrd)
 254         blt_tr(dst, src, trc, w);
 255     }
 256     else {
 257       for (; h > 0; h--, dst += dstrd, src += sstrd)
 258         memcpy(dst, src, w * bpp / 8);
 259     }
 260   }
 261   else {
 262     // fill. Assume the pattern is cleared and bg color is used
 263     u32 bgc = blitter.patbackcolor & 0xffff;
 264     if (bpp == 16) {
 265       for (; h > 0; h--, dst += dstrd)
 266         memset16(dst, bgc, w);
 267     }
 268     else {
 269       for (; h > 0; h--, dst += dstrd)
 270         memset(dst, bgc, w); // bgc?
 271     }
 272   }
 273
 274   if (to_screen)
 275     pthread_cond_signal(&fb_cond);
 276   return;
 277
 278 bad_blit:
 279   err("blit %08x->%08x %dx%d translated to %p->%p\n",
 280     blitter.srcaddr, blitter.dstaddr, w, h, src, dst);
 281   dump_blitter();
 282 }
 283
 284 // TODO: hw scaler stuff
 285 static void mlc_flip(u8 *src, int bpp)
 286 {
 287   u16 *dst = host_screen;
 288   u16 *hpal = mmsp2.host_pal;
 289   int i, u;
 290
 291   if (bpp <= 8 && mmsp2.dirty_pal) {
 292     u32 *srcp = mmsp2.mlc_stl_pallt_d32;
 293     u16 *dstp = hpal;
 294
 295     for (i = 0; i < 256; i++, srcp++, dstp++) {
 296       u32 t = *srcp;
 297       *dstp = ((t >> 8) & 0xf800) | ((t >> 5) & 0x07e0) | ((t >> 3) & 0x001f);
 298     }
 299     mmsp2.dirty_pal = 0;
 300   }
 301
 302   switch (bpp) {
 303   case  4:
 304     for (i = 0; i < 240; i++, dst += host_stride / 2 - 320) {
 305       for (u = 320 / 2; u > 0; u--, src++) {
 306         *dst++ = hpal[*src >> 4];
 307         *dst++ = hpal[*src & 0x0f];
 308       }
 309     }
 310     break;
 311
 312   case  8:
 313     for (i = 0; i < 240; i++, dst += host_stride / 2 - 320) {
 314       for (u = 320 / 4; u > 0; u--) {
 315         *dst++ = hpal[*src++];
 316         *dst++ = hpal[*src++];
 317         *dst++ = hpal[*src++];
 318         *dst++ = hpal[*src++];
 319       }
 320     }
 321     break;
 322
 323   case 16:
 324     for (i = 0; i < 240; i++, dst += host_stride / 2, src += 320*2)
 325       memcpy(dst, src, 320*2);
 326     break;
 327
 328   case 24:
 329     // TODO
 330     break;
 331   }
 332
 333   host_screen = host_video_flip();
 334 }
 335
 336 #define ts_add_nsec(ts, ns) { \
 337   ts.tv_nsec += ns; \
 338   if (ts.tv_nsec >= 1000000000) { \
 339     ts.tv_sec++; \
 340     ts.tv_nsec -= 1000000000; \
 341   } \
 342 }
 343
 344 static void *fb_sync_thread(void *arg)
 345 {
 346   int invalid_fb_addr = 1;
 347   int manual_refresh = 0;
 348   struct timespec ts;
 349   int ret, wait_ret;
 350
 351   //ret = pthread_setschedprio(pthread_self(), -1);
 352   //log("pthread_setschedprio %d\n", ret);
 353   //ret = setpriority(PRIO_PROCESS, 0, -1);
 354   //log("setpriority %d\n", ret);
 355
 356   ret = clock_gettime(CLOCK_REALTIME, &ts);
 357   if (ret != 0) {
 358     perror(PFX "clock_gettime");
 359     exit(1);
 360   }
 361
 362   while (1) {
 363     u8 *gp2x_fb, *gp2x_fb_end;
 364     int mode, bpp;
 365
 366     ret =  pthread_mutex_lock(&fb_mutex);
 367     wait_ret = pthread_cond_timedwait(&fb_cond, &fb_mutex, &ts);
 368     ret |= pthread_mutex_unlock(&fb_mutex);
 369
 370     if (ret != 0) {
 371       err("fb_thread: mutex error: %d\n", ret);
 372       sleep(1);
 373       continue;
 374     }
 375     if (wait_ret != 0 && wait_ret != ETIMEDOUT) {
 376       err("fb_thread: cond error: %d\n", wait_ret);
 377       sleep(1);
 378       continue;
 379     }
 380
 381     if (wait_ret != ETIMEDOUT) {
 382       clock_gettime(CLOCK_REALTIME, &ts);
 383       ts_add_nsec(ts, 50000000);
 384       manual_refresh++;
 385       if (manual_refresh == 2)
 386         log("fb_thread: switch to manual refresh\n");
 387     } else {
 388       ts_add_nsec(ts, 16666667);
 389       if (manual_refresh > 1)
 390         log("fb_thread: switch to auto refresh\n");
 391       manual_refresh = 0;
 392     }
 393
 394     mode = (mmsp2.mlc_stl_cntl >> 9) & 3;
 395     bpp = mode ? mode * 8 : 4;
 396
 397     gp2x_fb = uppermem_lookup(mmsp2.mlc_stl_adr, &gp2x_fb_end);
 398     if (gp2x_fb == NULL || gp2x_fb + 320*240 * bpp / 8 > gp2x_fb_end) {
 399       if (!invalid_fb_addr) {
 400         err("fb_thread: %08x is out of range\n", mmsp2.mlc_stl_adr);
 401         invalid_fb_addr = 1;
 402       }
 403       continue;
 404     }
 405
 406     mlc_flip(gp2x_fb, bpp);
 407   }
 408 }
 409
 410 static u32 xread8(u32 a)
 411 {
 412   iolog("r8 ", a, 0, 8);
 413   iolog_unh("r8 ", a, 0, 8);
 414   return 0;
 415 }
 416
 417 static u32 xread16(u32 a)
 418 {
 419   static u32 fudge, old_a;
 420   u32 d = 0, t;
 421
 422   if ((a & 0xffff0000) == 0x7f000000) {
 423     u32 a_ = a & 0xffff;
 424     switch (a_) {
 425     case 0x0910: // FPLL
 426     case 0x0912:
 427       d = 0x9407;
 428       break;
 429     // minilib reads as:
 430     //  0000 P000 VuVd00 0000 YXBA RLSeSt 0R0D 0L0U
 431     // |        GPIOD        |GPIOC[8:15]|GPIOM[0:7]|
 432     // /dev/GPIO:
 433     //             ... 0PVdVu ...
 434     case 0x1184: // GPIOC
 435       d = ~mmsp2.btn_state & 0xff00;
 436       d |= 0x00ff;
 437       break;
 438     case 0x1186: // GPIOD
 439       t = ~mmsp2.btn_state;
 440       d  = (t >> 9)  & 0x0080;
 441       d |= (t >> 11) & 0x0040;
 442       d |= (t >> 7)  & 0x0800;
 443       d |= 0x373b;
 444       break;
 445     case 0x1198: // GPIOM
 446       mmsp2.btn_state = host_read_btns();
 447       d = ~mmsp2.btn_state & 0xff;
 448       d |= 0x01aa;
 449       break;
 450     case 0x1836: // reserved
 451       d = 0x2330;
 452       break;
 453     case 0x2816: // DPC_X_MAX
 454       d = 319;
 455       break;
 456     case 0x2818: // DPC_Y_MAX
 457       d = 239;
 458       break;
 459     case 0x28da:
 460       d = mmsp2.mlc_stl_cntl;
 461       break;
 462     case 0x290e:
 463     case 0x2912:
 464       d = mmsp2.mlc_stl_adrl;
 465       break;
 466     case 0x2910:
 467     case 0x2914:
 468       d = mmsp2.mlc_stl_adrh;
 469       break;
 470     case 0x2958:
 471       d = mmsp2.mlc_stl_pallt_a;
 472       break;
 473     default:
 474       goto unh;
 475     }
 476     goto out;
 477   }
 478
 479 unh:
 480   if (a == old_a) {
 481     d = fudge;
 482     fudge = ~fudge;
 483   }
 484   old_a = a;
 485   iolog_unh("r16", a, d & 0xffff, 16);
 486
 487 out:
 488   d &= 0xffff;
 489   iolog("r16", a, d, 16);
 490   return d;
 491 }
 492
 493 static u32 xread32(u32 a)
 494 {
 495   u32 d = 0;
 496   if ((a & 0xfff00000) == 0x7f000000) {
 497     u32 a_ = a & 0xffff;
 498     switch (a_) {
 499     case 0x0a00: // TCOUNT, 1/7372800s
 500       // TODO
 501       break;
 502     }
 503   }
 504   if ((a & 0xfff00000) == 0x7f100000) {
 505     u32 *bl = &blitter.dstctrl;
 506     u32 a_ = a & 0xfff;
 507     if (a_ < 0x40) {
 508       d = bl[a_ / 4];
 509       if (a_ == 0x34)
 510         d = 0; // not busy
 511       goto out;
 512     }
 513   }
 514   iolog_unh("r32", a, d, 32);
 515
 516 out:
 517   iolog("r32", a, d, 32);
 518   return d;
 519 }
 520
 521 static void xwrite8(u32 a, u32 d)
 522 {
 523   iolog("w8 ", a, d, 8);
 524   iolog_unh("w8 ", a, d, 8);
 525 }
 526
 527 static void xwrite16(u32 a, u32 d)
 528 {
 529   iolog("w16", a, d, 16);
 530   if ((a & 0xfff00000) == 0x7f000000) {
 531     u32 a_ = a & 0xffff;
 532     switch (a_) {
 533     case 0x28da:
 534       mmsp2.mlc_stl_cntl = d | 0xaa;
 535       break;
 536     case 0x290e:
 537     case 0x2910:
 538       // odd addresses don't affect LCD. What about TV?
 539       return;
 540     case 0x2912:
 541       mmsp2.mlc_stl_adrl = d;
 542       return;
 543     case 0x2914:
 544       mmsp2.mlc_stl_adrh = d;
 545       if (mmsp2.mlc_stl_adr != mmsp2.old_mlc_stl_adr)
 546         // ask for refresh
 547         pthread_cond_signal(&fb_cond);
 548       mmsp2.old_mlc_stl_adr = mmsp2.mlc_stl_adr;
 549       return;
 550     case 0x2958:
 551       mmsp2.mlc_stl_pallt_a = d & 0x1ff;
 552       return;
 553     case 0x295a:
 554       mmsp2.mlc_stl_pallt_d[mmsp2.mlc_stl_pallt_a++] = d;
 555       mmsp2.mlc_stl_pallt_a &= 0x1ff;
 556       mmsp2.dirty_pal = 1;
 557       return;
 558     }
 559   }
 560   iolog_unh("w16", a, d, 16);
 561 }
 562
 563 static void xwrite32(u32 a, u32 d)
 564 {
 565   iolog("w32", a, d, 32);
 566
 567   if ((a & 0xfff00000) == 0x7f100000) {
 568     u32 *bl = &blitter.dstctrl;
 569     u32 a_ = a & 0xfff;
 570     if (a_ < 0x40) {
 571       bl[a_ / 4] = d;
 572       if (a_ == 0x34 && (d & 1))
 573         blitter_do();
 574       return;
 575     }
 576   }
 577   iolog_unh("w32", a, d, 32);
 578 }
 579
 580 #define LINKPAGE_SIZE 0x1000
 581 #define LINKPAGE_COUNT 4
 582 #define LINKPAGE_ALLOC (LINKPAGE_SIZE * LINKPAGE_COUNT)
 583
 584 struct op_context {
 585   u32 pc;
 586   u32 op;
 587   u32 code[0];
 588 };
 589
 590 struct linkpage {
 591   u32 saved_regs[15];
 592   u32 cpsr;
 593   u32 *handler_stack;
 594   void (*handler)(struct op_context *op_ctx);
 595   u32 code[0];
 596 };
 597
 598 static struct linkpage *g_linkpage;
 599 static u32 *g_code_ptr;
 600 static int g_linkpage_count;
 601
 602 static void *g_handler_stack_end;
 603
 604 #define BIT_SET(v, b) (v & (1 << (b)))
 605
 606 static void handle_op(struct op_context *op_ctx)
 607 {
 608   u32 *regs = g_linkpage->saved_regs;
 609   u32 op = op_ctx->op;
 610   u32 t, shift, ret, addr;
 611   int rn, rd;
 612
 613   rd = (op & 0x0000f000) >> 12;
 614   rn = (op & 0x000f0000) >> 16;
 615
 616   if ((op & 0x0f200090) == 0x01000090) { // AM3: LDRH, STRH
 617     if (!BIT_SET(op, 5)) // !H
 618       goto unhandled;
 619     if (BIT_SET(op, 6) && !BIT_SET(op, 20)) // S && !L
 620       goto unhandled;
 621
 622     if (BIT_SET(op, 22))                // imm offset
 623       t = ((op & 0xf00) >> 4) | (op & 0x0f);
 624     else                                // reg offset
 625       t = regs[op & 0x000f];
 626
 627     if (!BIT_SET(op, 23))
 628       t = -t;
 629     addr = regs[rn] + t;
 630
 631     if (BIT_SET(op, 20)) { // Load
 632       ret = xread16(addr);
 633       if (BIT_SET(op, 6)) { // S
 634         ret <<= 16;
 635         ret = (signed int)ret >> 16;
 636       }
 637       regs[rd] = ret;
 638     }
 639     else
 640       xwrite16(addr, regs[rd]);
 641   }
 642   else if ((op & 0x0d200000) == 0x05000000) { // AM2: LDR[B], STR[B]
 643     if (BIT_SET(op, 25)) {              // reg offs
 644       if (BIT_SET(op, 4))
 645         goto unhandled;
 646
 647       t = regs[op & 0x000f];
 648       shift = (op & 0x0f80) >> 7;
 649       switch ((op & 0x0060) >> 5) {
 650         case 0: t = t << shift; break;
 651         case 1: t = t >> (shift + 1); break;
 652         case 2: t = (signed int)t >> (shift + 1); break;
 653         case 3: goto unhandled; // I'm just lazy
 654       }
 655     }
 656     else                                // imm offs
 657       t = op & 0x0fff;
 658
 659     if (!BIT_SET(op, 23))
 660       t = -t;
 661     addr = regs[rn] + t;
 662
 663     if (BIT_SET(op, 20)) { // Load
 664       if (BIT_SET(op, 22)) // Byte
 665         ret = xread8(addr);
 666       else
 667         ret = xread32(addr);
 668       regs[rd] = ret;
 669     }
 670     else {
 671       if (BIT_SET(op, 22)) // Byte
 672         xwrite8(addr, regs[rd]);
 673       else
 674         xwrite32(addr, regs[rd]);
 675     }
 676   }
 677   else
 678     goto unhandled;
 679
 680 #if 0
 681   if (addr != addr_check) {
 682     fprintf(stderr, "bad calculated addr: %08x vs %08x\n", addr, addr_check);
 683     abort();
 684   }
 685 #endif
 686   return;
 687
 688 unhandled:
 689   err("unhandled IO op %08x @ %08x\n", op, op_ctx->pc);
 690 }
 691
 692 static u32 make_offset12(u32 *pc, u32 *target)
 693 {
 694   int lp_offs, u = 1;
 695
 696   lp_offs = (char *)target - (char *)pc - 2*4;
 697   if (lp_offs < 0) {
 698     lp_offs = -lp_offs;
 699     u = 0;
 700   }
 701   if (lp_offs >= LINKPAGE_SIZE) {
 702     err("linkpage too far: %d\n", lp_offs);
 703     abort();
 704   }
 705
 706   return (u << 23) | lp_offs;
 707 }
 708
 709 static u32 make_jmp(u32 *pc, u32 *target, int bl)
 710 {
 711   int jmp_val;
 712
 713   jmp_val = target - pc - 2;
 714   if (jmp_val < (int)0xff000000 || jmp_val > 0x00ffffff) {
 715     err("jump out of range (%p -> %p)\n", pc, target);
 716     abort();
 717   }
 718
 719   return 0xea000000 | (bl << 24) | (jmp_val & 0x00ffffff);
 720 }
 721
 722 static void emit_op(u32 op)
 723 {
 724   *g_code_ptr++ = op;
 725 }
 726
 727 static void emit_op_io(u32 op, u32 *target)
 728 {
 729   op |= make_offset12(g_code_ptr, target);
 730   emit_op(op);
 731 }
 732
 733 static void init_linkpage(void)
 734 {
 735   g_linkpage->handler = handle_op;
 736   g_linkpage->handler_stack = g_handler_stack_end;
 737   g_code_ptr = g_linkpage->code;
 738
 739   // common_code.
 740   // r0 and r14 must be saved by caller, r0 is arg for handle_op
 741   // on return everything is restored except lr, which is used to return
 742   emit_op_io(0xe50f1000, &g_linkpage->saved_regs[1]);  // str r1, [->saved_regs[1]] @ save r1
 743   emit_op   (0xe24f1000 +                              // sub r1, pc, =offs(saved_regs[2])
 744     (g_code_ptr - &g_linkpage->saved_regs[2] + 2) * 4);
 745   emit_op   (0xe8813ffc);                              // stmia r1, {r2-r13}
 746   emit_op_io(0xe51fd000,                               // ldr sp, [->handler_stack]
 747     (u32 *)&g_linkpage->handler_stack);
 748   emit_op   (0xe2414008);                              // sub r4, r1, #4*2
 749   emit_op   (0xe10f1000);                              // mrs r1, cpsr
 750   emit_op_io(0xe50f1000, &g_linkpage->cpsr);           // str r1, [->cpsr]
 751   emit_op   (0xe1a0500e);                              // mov r5, lr
 752   emit_op   (0xe1a0e00f);                              // mov lr, pc
 753   emit_op_io(0xe51ff000, (u32 *)&g_linkpage->handler); // ldr pc, =handle_op
 754   emit_op_io(0xe51f1000, &g_linkpage->cpsr);           // ldr r1, [->cpsr]
 755   emit_op   (0xe128f001);                              // msr cpsr_f, r1
 756   emit_op   (0xe1a0e005);                              // mov lr, r5
 757   emit_op   (0xe8943fff);                              // ldmia r4, {r0-r13}
 758   emit_op   (0xe12fff1e);                              // bx lr @ return
 759 }
 760
 761 static void segv_sigaction(int num, siginfo_t *info, void *ctx)
 762 {
 763   struct ucontext *context = ctx;
 764   u32 *regs = (u32 *)&context->uc_mcontext.arm_r0;
 765   u32 *pc = (u32 *)regs[15];
 766   struct op_context *op_ctx;
 767   int i, lp_size;
 768
 769   if (((regs[15] ^ (u32)&segv_sigaction) & 0xff000000) == 0 ||         // PC is in our segment or
 770       (((regs[15] ^ (u32)g_linkpage) & ~(LINKPAGE_ALLOC - 1)) == 0) || // .. in linkpage
 771       ((long)info->si_addr & 0xffe00000) != 0x7f000000)                // faulting not where expected
 772   {
 773     // real crash - time to die
 774     err("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]);
 775     for (i = 0; i < 8; i++)
 776       err(" r%d=%08x r%2d=%08x\n", i, regs[i], i+8, regs[i+8]);
 777     signal(num, SIG_DFL);
 778     raise(num);
 779     return;
 780   }
 781   segvlog("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]);
 782
 783   // spit PC and op
 784   op_ctx = (void *)g_code_ptr;
 785   op_ctx->pc = (u32)pc;
 786   op_ctx->op = *pc;
 787   g_code_ptr = &op_ctx->code[0];
 788
 789   // emit jump to code ptr
 790   *pc = make_jmp(pc, g_code_ptr, 0);
 791
 792   // generate code:
 793   // TODO: multithreading
 794   emit_op_io(0xe50f0000, &g_linkpage->saved_regs[0]);            // str r0,  [->saved_regs[0]] @ save r0
 795   emit_op_io(0xe50fe000, &g_linkpage->saved_regs[14]);           // str r14, [->saved_regs[14]]
 796   emit_op   (0xe24f0000 + (g_code_ptr - (u32 *)op_ctx + 2) * 4); // sub r0, pc, #op_ctx
 797   emit_op   (make_jmp(g_code_ptr, &g_linkpage->code[0], 1));     // bl common_code
 798   emit_op_io(0xe51fe000, &g_linkpage->saved_regs[14]);           // ldr r14, [->saved_regs[14]]
 799   emit_op   (make_jmp(g_code_ptr, pc + 1, 0));                   // jmp <back>
 800
 801   // sync caches
 802   sys_cacheflush(pc, pc + 1);
 803   sys_cacheflush(g_linkpage, g_code_ptr);
 804
 805   lp_size = (char *)g_code_ptr - (char *)g_linkpage;
 806   segvlog("code #%d %d/%d\n", g_linkpage_count, lp_size, LINKPAGE_SIZE);
 807
 808   if (lp_size + 13*4 > LINKPAGE_SIZE) {
 809     g_linkpage_count++;
 810     if (g_linkpage_count >= LINKPAGE_COUNT) {
 811       err("too many linkpages needed\n");
 812       abort();
 813     }
 814     g_linkpage = (void *)((char *)g_linkpage + LINKPAGE_SIZE);
 815     init_linkpage();
 816   }
 817   //handle_op(regs[15], op, regs, (u32)info->si_addr);
 818   //regs[15] += 4;
 819 }
 820
 821 void emu_init(void *map_bottom)
 822 {
 823   sigaction_t segv_action = {
 824     .sa_sigaction = segv_sigaction,
 825     .sa_flags = SA_SIGINFO,
 826   };
 827   pthread_t tid;
 828   void *pret;
 829   int ret;
 830
 831   g_handler_stack_end = (void *)((long)alloca(1536 * 1024) & ~0xffff);
 832   log("handler stack @ %p (current %p)\n", g_handler_stack_end, &ret);
 833   // touch it now. If we crash now we'll know why
 834   *((char *)g_handler_stack_end - 4096) = 1;
 835
 836   g_linkpage = (void *)(((u32)map_bottom - LINKPAGE_ALLOC) & ~0xfff);
 837   pret = mmap(g_linkpage, LINKPAGE_ALLOC, PROT_READ|PROT_WRITE,
 838               MAP_PRIVATE|MAP_ANONYMOUS, -1, 0);
 839   if (pret != g_linkpage) {
 840     perror(PFX "mmap linkpage");
 841     exit(1);
 842   }
 843   log("linkpages @ %p\n", g_linkpage);
 844   init_linkpage();
 845
 846   // host stuff
 847   ret = host_video_init(&host_stride, 0);
 848   if (ret != 0) {
 849     err("can't init video\n");
 850     exit(1);
 851   }
 852   host_screen = host_video_flip();
 853
 854   ret = pthread_create(&tid, NULL, fb_sync_thread, NULL);
 855   if (ret != 0) {
 856     err("failed to create fb_sync_thread: %d\n", ret);
 857     exit(1);
 858   }
 859   pthread_detach(tid);
 860
 861   // mmsp2 defaults
 862   mmsp2.mlc_stl_adr = 0x03101000; // fb2 is at 0x03381000
 863   mmsp2.mlc_stl_cntl = 0x4ab; // 16bpp, region 1 active
 864
 865   sigemptyset(&segv_action.sa_mask);
 866   sigaction(SIGSEGV, &segv_action, NULL);
 867 }
 868
 869 int emu_read_gpiodev(void *buf, int count)
 870 {
 871   unsigned int btns;
 872
 873   if (count < 4) {
 874     err("gpiodev read %d?\n", count);
 875     return -1;
 876   }
 877
 878   btns = host_read_btns();
 879   memcpy(buf, &btns, 4);
 880   return 4;
 881 }
 882
 883 struct dev_fd_t emu_interesting_fds[] = {
 884   [IFD_SOUND] = { "/dev/dsp", -1 },
 885   { NULL, 0 },
 886 };
 887
 888 static void *emu_mmap_dev(unsigned int length, int prot, int flags, unsigned int offset)
 889 {
 890   struct uppermem_block *umem;
 891   char name[32];
 892   int fd;
 893
 894   // SoC regs
 895   if ((offset & ~0xffff) == 0xc0000000) {
 896     return mmap((void *)0x7f000000, length, PROT_NONE,
 897       MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0);
 898   }
 899   // blitter
 900   if ((offset & ~0xffff) == 0xe0020000) {
 901     return mmap((void *)0x7f100000, length, PROT_NONE,
 902       MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0);
 903   }
 904   // upper mem
 905   if ((offset & 0xfe000000) != 0x02000000)
 906     err("unexpected devmem mmap @ %08x\n", offset);
 907
 908   umem = calloc(1, sizeof(*umem));
 909   if (umem == NULL) {
 910     err("OOM\n");
 911     return MAP_FAILED;
 912   }
 913
 914   umem->addr = offset;
 915   umem->size = length;
 916   umem->mem = mmap(NULL, length, prot, MAP_PRIVATE|MAP_ANONYMOUS, -1, 0);
 917   if (umem->mem != MAP_FAILED)
 918     goto done;
 919
 920   log("upper mem @ %08x %d mmap fail, trying backing file\n", offset, length);
 921   sprintf(name, "m%08x", offset);
 922   fd = open(name, O_CREAT|O_RDWR, 0644);
 923   lseek(fd, length - 1, SEEK_SET);
 924   name[0] = 0;
 925   write(fd, name, 1);
 926
 927   umem->mem = mmap(NULL, length, prot, MAP_SHARED, fd, 0);
 928   if (umem->mem == MAP_FAILED) {
 929     err("failed, giving up\n");
 930     close(fd);
 931     free(umem);
 932     errno = EINVAL;
 933     return MAP_FAILED;
 934   }
 935
 936 done:
 937   log("upper mem @ %08x %d\n", offset, length);
 938   umem->next = upper_mem;
 939   upper_mem = umem;
 940   return umem->mem;
 941 }
 942
 943 void *emu_do_mmap(unsigned int length, int prot, int flags, int fd, unsigned int offset)
 944 {
 945   if (fd == FAKEDEV_MEM)
 946     return emu_mmap_dev(length, prot, flags, offset);
 947
 948   if (fd == FAKEDEV_FB0)
 949     return emu_mmap_dev(length, prot, flags, offset + 0x03101000);
 950
 951   if (fd == FAKEDEV_FB1)
 952     return emu_mmap_dev(length, prot, flags, offset + 0x03381000);
 953
 954   err("bad/ni mmap(?, %d, %x, %x, %d, %08x)\n", length, prot, flags, fd, offset);
 955   errno = EINVAL;
 956   return MAP_FAILED;
 957 }
 958
 959 static int emu_sound_ioctl(int fd, int request, void *argp)
 960 {
 961   int *arg = argp;
 962
 963 #if 0
 964   dbg("snd ioctl(%d, %08x, %p)", fd, request, argp);
 965   if (arg != NULL)
 966     dbg_c(" [%d]", *arg);
 967   dbg_c("\n");
 968 #endif
 969
 970   /* People set strange frag settings on GP2X, which even manage
 971    * to break audio on pandora (causes writes to fail).
 972    * Catch this and set to something that works. */
 973   if (request == SNDCTL_DSP_SPEED) {
 974     int ret, bsize, frag;
 975
 976     // ~4ms. gpSP wants small buffers or else it stutters
 977     // because of it's audio thread sync stuff
 978     bsize = *arg / 250 * 4;
 979     for (frag = 0; bsize; bsize >>= 1, frag++)
 980       ;
 981
 982     frag |= 16 << 16;       // fragment count
 983     ret = ioctl(fd, SNDCTL_DSP_SETFRAGMENT, &frag);
 984     if (ret != 0) {
 985       err("snd ioctl SETFRAGMENT %08x: ", frag);
 986       perror(NULL);
 987     }
 988   }
 989   else if (request == SNDCTL_DSP_SETFRAGMENT)
 990     return 0;
 991
 992   return ioctl(fd, request, argp);
 993 }
 994
 995 int emu_do_ioctl(int fd, int request, void *argp)
 996 {
 997   if (fd == emu_interesting_fds[IFD_SOUND].fd)
 998     return emu_sound_ioctl(fd, request, argp);
 999
1000   switch (fd) {
1001   /* *********************** */
1002   case FAKEDEV_FB0:
1003   case FAKEDEV_FB1:
1004     if (argp == NULL)
1005       goto fail;
1006
1007     switch (request) {
1008       case FBIOGET_FSCREENINFO: {
1009         struct fb_fix_screeninfo *fix = argp;
1010
1011         memset(fix, 0, sizeof(*fix));
1012         strcpy(fix->id, "mmsp2_RGB0");
1013         fix->type         = FB_TYPE_PACKED_PIXELS;
1014         fix->accel        = FB_ACCEL_NONE;
1015         fix->visual       = FB_VISUAL_TRUECOLOR;
1016         fix->line_length  = 320*2;
1017         fix->smem_start   = (fd == FAKEDEV_FB0) ? 0x03101000 : 0x03381000;
1018         fix->smem_len     = 320*240*2;
1019         return 0;
1020       }
1021       case FBIOGET_VSCREENINFO: {
1022         struct fb_var_screeninfo *var = argp;
1023         static const struct fb_bitfield fbb_red   = { offset: 11, length: 5, };
1024         static const struct fb_bitfield fbb_green = { offset:  5, length: 6, };
1025         static const struct fb_bitfield fbb_blue  = { offset:  0, length: 5, };
1026
1027         memset(var, 0, sizeof(*var));
1028         var->activate     = FB_ACTIVATE_NOW;
1029         var->xres         =
1030         var->xres_virtual = 320;
1031         var->yres         =
1032         var->yres_virtual = 240;
1033         var->width        =
1034         var->height       = -1;
1035         var->vmode        = FB_VMODE_NONINTERLACED;
1036         var->bits_per_pixel = 16;
1037         var->red          = fbb_red;
1038         var->green        = fbb_green;
1039         var->blue         = fbb_blue;
1040         return 0;
1041       }
1042       case FBIOPUT_VSCREENINFO: {
1043         struct fb_var_screeninfo *var = argp;
1044         dbg(" put vscreen: %dx%d@%d\n", var->xres, var->yres, var->bits_per_pixel);
1045         if (var->xres != 320 || var->yres != 240 || var->bits_per_pixel != 16)
1046           return -1;
1047         return 0;
1048       }
1049     }
1050
1051   /* *********************** */
1052   case FAKEDEV_TTY0:
1053     // fake tty0 to make GPH SDL happy
1054     if (request == 0x4b46) // KDGKBENT
1055       return -1;
1056     return 0;
1057   }
1058
1059 fail:
1060   err("bad/ni ioctl(%d, %08x, %p)\n", fd, request, argp);
1061   errno = EINVAL;
1062   return -1;
1063 }
1064
1065 static const struct {
1066   const char *from;
1067   const char *to;
1068 } path_map[] = {
1069   { "/mnt/tmp/", "/tmp/" },
1070 };
1071
1072 // FIXME: threads..
1073 static const char *wrap_path(const char *path)
1074 {
1075   static char tmp_path[512];
1076   int i, len;
1077
1078   // do only path mapping for now
1079   for (i = 0; i < ARRAY_SIZE(path_map); i++) {
1080     len = strlen(path_map[i].from);
1081     if (strncmp(path, path_map[i].from, len) == 0) {
1082       snprintf(tmp_path, sizeof(tmp_path), "%s%s", path_map[i].to, path + len);
1083       dbg("mapped path \"%s\" -> \"%s\"\n", path, tmp_path);
1084       return tmp_path;
1085     }
1086   }
1087
1088   return path;
1089 }
1090
1091 void *emu_do_fopen(const char *path, const char *mode)
1092 {
1093   return fopen(wrap_path(path), mode);
1094 }
1095
1096 int emu_do_system(const char *command)
1097 {
1098   static char tmp_path[512];
1099   char *p, *p2;
1100
1101   if (command == NULL)
1102     return -1;
1103
1104   // pass through stuff in PATH
1105   p = strchr(command, ' ');
1106   p2 = strchr(command, '/');
1107   if (p2 == NULL || (p != NULL && p2 > p))
1108     return system(command);
1109
1110   make_local_path(tmp_path, sizeof(tmp_path), "ginge_prep");
1111   p = tmp_path + strlen(tmp_path);
1112
1113   snprintf(p, sizeof(tmp_path) - (p - tmp_path), " %s", wrap_path(command));
1114   dbg("system: \"%s\"\n", tmp_path);
1115   return system(tmp_path);
1116 }
1117