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 iolog printf
  18 #define iolog(...)
  19 //#define segvlog printf
  20 #define segvlog(...)
  21
  22 typedef unsigned int   u32;
  23 typedef unsigned short u16;
  24 typedef unsigned char  u8;
  25
  26 struct uppermem_block {
  27   u32 addr; // physical
  28   u32 size;
  29   void *mem;
  30   struct uppermem_block *next;
  31 };
  32
  33 static struct uppermem_block *upper_mem;
  34
  35 static struct {
  36   u32 dstctrl;
  37   u32 dstaddr;
  38   u32 dststride;
  39   u32 srcctrl;
  40   u32 srcaddr;          //
  41   u32 srcstride;
  42   u32 srcforcolor;
  43   u32 srcbackcolor;
  44   u32 patctrl;          //
  45   u32 patforcolor;
  46   u32 patbackcolor;
  47   u32 size;
  48   u32 ctrl;             //
  49   u32 run;
  50   u32 intc;
  51   u32 srcfifo;
  52 } blitter;
  53
  54 #define SRCCTRL_INVIDEO         (1 << 8)
  55 #define SRCCTRL_SRCENB          (1 << 7)
  56 #define CTRL_TRANSPARENCYENB    (1 << 11)
  57
  58 static struct {
  59   union {
  60     u32 mlc_stl_eadr;
  61     struct {
  62       u16 mlc_stl_eadrl;
  63       u16 mlc_stl_eadrh;
  64     };
  65   };
  66 } mmsp2;
  67
  68 static u16 *host_screen;
  69 static int host_stride;
  70
  71
  72 static void memset16(void *dst, u32 pattern, int count)
  73 {
  74   u32 *dl;
  75   u16 *d;
  76
  77   d = (u16 *)((long)dst & ~1);
  78   if ((long)d & 2) {
  79     *d++ = pattern;
  80     count--;
  81   }
  82   dl = (void *)d;
  83   pattern |= pattern << 16;
  84
  85   while (count >= 2) {
  86     *dl++ = pattern;
  87     count -= 2;
  88   }
  89   if (count)
  90     *(u16 *)dl = pattern;
  91 }
  92
  93 static void blt_tr(void *dst, void *src, u32 trc, int w)
  94 {
  95   u16 *d = (u16 *)((long)dst & ~1);
  96   u16 *s = (u16 *)((long)src & ~1);
  97
  98   // XXX: optimize
  99   for (; w > 0; d++, s++, w--)
 100     if (*s != trc)
 101       *d = *s;
 102 }
 103
 104 #define dump_blitter() \
 105 { \
 106   u32 *r = &blitter.dstctrl; \
 107   int i; \
 108   for (i = 0; i < 4*4; i++, r++) { \
 109     printf("%08x ", *r); \
 110     if ((i & 3) == 3) \
 111       printf("\n"); \
 112   } \
 113 }
 114
 115 static void *upper_lookup(u32 addr, u8 **mem_end, int *stride_override)
 116 {
 117   struct uppermem_block *ub;
 118
 119   // maybe the screen?
 120   if (mmsp2.mlc_stl_eadr <= addr && addr < mmsp2.mlc_stl_eadr + 320*240*2) {
 121     host_screen = host_video_flip(); // HACK
 122     *mem_end = (u8 *)host_screen + host_stride * 240;
 123     *stride_override = host_stride;
 124     return (u8 *)host_screen + addr - mmsp2.mlc_stl_eadr;
 125   }
 126
 127   for (ub = upper_mem; ub != NULL; ub = ub->next) {
 128     if (ub->addr <= addr && addr < ub->addr + ub->size) {
 129       *mem_end = (u8 *)ub->mem + ub->size;
 130       return (u8 *)ub->mem + addr - ub->addr;
 131     }
 132   }
 133
 134   return NULL;
 135 }
 136
 137 static void blitter_do(void)
 138 {
 139   u8 *dst, *dste, *src = NULL, *srce = NULL;
 140   int w, h, sstrd, dstrd;
 141   u32 addr;
 142
 143   w = blitter.size & 0x7ff;
 144   h = (blitter.size >> 16) & 0x7ff;
 145   sstrd = blitter.srcstride;
 146   dstrd = blitter.dststride;
 147
 148   // XXX: need to confirm this..
 149   addr = (blitter.dstaddr & ~3) | ((blitter.dstctrl & 0x1f) >> 3);
 150   dst = upper_lookup(addr, &dste, &dstrd);
 151   if (dst == NULL)
 152     goto bad_blit;
 153
 154   // XXX: assume fill if no SRCENB, but it could be pattern blit..
 155   if (blitter.srcctrl & SRCCTRL_SRCENB) {
 156     if (!(blitter.srcctrl & SRCCTRL_INVIDEO))
 157       goto bad_blit;
 158
 159     addr = (blitter.srcaddr & ~3) | ((blitter.srcctrl & 0x1f) >> 3);
 160     src = upper_lookup(addr, &srce, &sstrd);
 161     if (src == NULL)
 162       goto bad_blit;
 163
 164     if (src + sstrd * h > srce) {
 165       printf("blit %08x->%08x %dx%d did not fit src\n",
 166         blitter.srcaddr, blitter.dstaddr, w, h);
 167       h = (srce - src) / sstrd;
 168     }
 169   }
 170
 171   if (dst + dstrd * h > dste) {
 172     printf("blit %08x->%08x %dx%d did not fit dst\n",
 173       blitter.srcaddr, blitter.dstaddr, w, h);
 174     h = (dste - dst) / dstrd;
 175   }
 176
 177   if (src != NULL) {
 178     // copy
 179     if (blitter.ctrl & CTRL_TRANSPARENCYENB) {
 180       u32 trc = blitter.ctrl >> 16;
 181       for (; h > 0; h--, dst += dstrd, src += sstrd)
 182         blt_tr(dst, src, trc, w);
 183     }
 184     else {
 185       for (; h > 0; h--, dst += dstrd, src += sstrd)
 186         memcpy(dst, src, w * 2);
 187     }
 188   }
 189   else {
 190     // fill. Assume the pattern is cleared and bg color is used
 191     u32 bgc = blitter.patbackcolor & 0xffff;
 192     for (; h > 0; h--, dst += dstrd)
 193       memset16(dst, bgc, w);
 194   }
 195   return;
 196
 197 bad_blit:
 198   printf("blit %08x->%08x %dx%d translated to %p->%p\n",
 199     blitter.srcaddr, blitter.dstaddr, w, h, src, dst);
 200   dump_blitter();
 201 }
 202
 203 static u32 xread8(u32 a)
 204 {
 205   iolog("r8  %08x\n", a);
 206   return 0;
 207 }
 208
 209 static u32 xread16(u32 a)
 210 {
 211 // if ((a & 0xfff00000) == 0x7f100000) { static int a; a ^= ~1; return a & 0xffff; }
 212   iolog("r16 %08x\n", a);
 213   return 0;
 214 }
 215
 216 static u32 xread32(u32 a)
 217 {
 218   u32 d = 0;
 219   if ((a & 0xfff00000) == 0x7f100000) {
 220     u32 *bl = &blitter.dstctrl;
 221     a &= 0xfff;
 222     if (a < 0x40)
 223       d = bl[a / 4];
 224     if (a == 0x34)
 225       d = 0; // not busy
 226   }
 227   iolog("r32 %08x\n", a);
 228   return d;
 229 }
 230
 231 static void xwrite8(u32 a, u32 d)
 232 {
 233   iolog("w8  %08x %08x\n", a, d);
 234 }
 235
 236 static void xwrite16(u32 a, u32 d)
 237 {
 238   iolog("w16 %08x %08x\n", a, d);
 239   if ((a & 0xfff00000) == 0x7f000000) {
 240     a &= 0xffff;
 241     switch (a) {
 242       case 0x2912: mmsp2.mlc_stl_eadrl = d; break;
 243       case 0x2914: mmsp2.mlc_stl_eadrh = d; break;
 244     }
 245     //printf("w16 %08x %08x\n", a, d);
 246   }
 247 }
 248
 249 static void xwrite32(u32 a, u32 d)
 250 {
 251   iolog("w32 %08x %08x\n", a, d);
 252   if ((a & 0xfff00000) == 0x7f000000) {
 253     printf("w32 %08x %08x\n", a, d);
 254     return;
 255   }
 256   if ((a & 0xfff00000) == 0x7f100000) {
 257     u32 *bl = &blitter.dstctrl;
 258     a &= 0xfff;
 259     if (a < 0x40)
 260       bl[a / 4] = d;
 261     if (a == 0x34 && (d & 1))
 262       blitter_do();
 263     return;
 264   }
 265 }
 266
 267 #define BIT_SET(v, b) (v & (1 << (b)))
 268
 269 static void handle_op(u32 pc, u32 op, u32 *regs, u32 addr_check)
 270 {
 271   u32 t, shift, ret, addr;
 272   int rn, rd;
 273
 274   rd = (op & 0x0000f000) >> 12;
 275   rn = (op & 0x000f0000) >> 16;
 276
 277   if ((op & 0x0f200090) == 0x01000090) { // AM3: LDRH, STRH
 278     if (!BIT_SET(op, 5)) // !H
 279       goto unhandled;
 280     if (BIT_SET(op, 6) && !BIT_SET(op, 20)) // S && !L
 281       goto unhandled;
 282
 283     if (BIT_SET(op, 22))                // imm offset
 284       t = ((op & 0xf00) >> 4) | (op & 0x0f);
 285     else                                // reg offset
 286       t = regs[op & 0x000f];
 287
 288     if (!BIT_SET(op, 23))
 289       t = -t;
 290     addr = regs[rn] + t;
 291
 292     if (BIT_SET(op, 20)) { // Load
 293       ret = xread16(addr);
 294       if (BIT_SET(op, 6)) { // S
 295         ret <<= 16;
 296         ret = (signed int)ret >> 16;
 297       }
 298       regs[rd] = ret;
 299     }
 300     else
 301       xwrite16(addr, regs[rd]);
 302   }
 303   else if ((op & 0x0d200000) == 0x05000000) { // AM2: LDR[B], STR[B]
 304     if (BIT_SET(op, 25)) {              // reg offs
 305       if (BIT_SET(op, 4))
 306         goto unhandled;
 307
 308       t = regs[op & 0x000f];
 309       shift = (op & 0x0f80) >> 7;
 310       switch ((op & 0x0060) >> 5) {
 311         case 0: t = t << shift; break;
 312         case 1: t = t >> (shift + 1); break;
 313         case 2: t = (signed int)t >> (shift + 1); break;
 314         case 3: goto unhandled; // I'm just lazy
 315       }
 316     }
 317     else                                // imm offs
 318       t = op & 0x0fff;
 319
 320     if (!BIT_SET(op, 23))
 321       t = -t;
 322     addr = regs[rn] + t;
 323
 324     if (BIT_SET(op, 20)) { // Load
 325       if (BIT_SET(op, 22)) // Byte
 326         ret = xread8(addr);
 327       else
 328         ret = xread32(addr);
 329       regs[rd] = ret;
 330     }
 331     else {
 332       if (BIT_SET(op, 22)) // Byte
 333         xwrite8(addr, regs[rd]);
 334       else
 335         xwrite32(addr, regs[rd]);
 336     }
 337   }
 338   else
 339     goto unhandled;
 340
 341 #if 0
 342   if (addr != addr_check) {
 343     fprintf(stderr, "bad calculated addr: %08x vs %08x\n", addr, addr_check);
 344     abort();
 345   }
 346 #endif
 347   return;
 348
 349 unhandled:
 350   fprintf(stderr, "unhandled IO op %08x @ %08x\n", op, pc);
 351 }
 352
 353 #define LINKPAGE_SIZE 0x1000
 354 #define LINKPAGE_COUNT 4
 355 #define LINKPAGE_ALLOC (LINKPAGE_SIZE * LINKPAGE_COUNT)
 356
 357 struct linkpage {
 358   u32 saved_regs[15];
 359   u32 *lp_r1;
 360   void (*handler)(u32 addr_pc, u32 op, u32 *regs, u32 addr_check);
 361   u32 code[0];
 362 };
 363
 364 static struct linkpage *g_linkpage;
 365 static u32 *g_code_ptr;
 366 static int g_linkpage_count;
 367
 368 static void init_linkpage(void)
 369 {
 370   g_linkpage->lp_r1 = &g_linkpage->saved_regs[1];
 371   g_linkpage->handler = handle_op;
 372   g_code_ptr = g_linkpage->code;
 373 }
 374
 375 static u32 make_offset12(u32 *pc, u32 *target)
 376 {
 377   int lp_offs, u = 1;
 378
 379   lp_offs = (char *)target - (char *)pc - 2*4;
 380   if (lp_offs < 0) {
 381     lp_offs = -lp_offs;
 382     u = 0;
 383   }
 384   if (lp_offs >= LINKPAGE_SIZE) {
 385     fprintf(stderr, "linkpage too far: %d\n", lp_offs);
 386     abort();
 387   }
 388
 389   return (u << 23) | lp_offs;
 390 }
 391
 392 static u32 make_jmp(u32 *pc, u32 *target)
 393 {
 394   int jmp_val;
 395
 396   jmp_val = target - pc - 2;
 397   if (jmp_val < (int)0xff000000 || jmp_val > 0x00ffffff) {
 398     fprintf(stderr, "jump out of range (%p -> %p)\n", pc, target);
 399     abort();
 400   }
 401
 402   return 0xea000000 | (jmp_val & 0x00ffffff);
 403 }
 404
 405 static void emit_op(u32 op)
 406 {
 407   *g_code_ptr++ = op;
 408 }
 409
 410 static void emit_op_io(u32 op, u32 *target)
 411 {
 412   op |= make_offset12(g_code_ptr, target);
 413   emit_op(op);
 414 }
 415
 416 static void segv_sigaction(int num, siginfo_t *info, void *ctx)
 417 {
 418   struct ucontext *context = ctx;
 419   u32 *regs = (u32 *)&context->uc_mcontext.arm_r0;
 420   u32 *pc = (u32 *)regs[15];
 421   u32 old_op = *pc;
 422   u32 *pc_ptr, *old_op_ptr;
 423   int lp_size;
 424
 425   if (((regs[15] ^ (u32)&segv_sigaction) & 0xff000000) == 0 ||       // PC is in our segment or
 426       (((regs[15] ^ (u32)g_linkpage) & ~(LINKPAGE_ALLOC - 1)) == 0)) // .. in linkpage
 427   {
 428     // real crash - time to die
 429     printf("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]);
 430     signal(num, SIG_DFL);
 431     raise(num);
 432   }
 433   segvlog("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]);
 434
 435   // spit PC and op
 436   pc_ptr = g_code_ptr++;
 437   old_op_ptr = g_code_ptr++;
 438   *pc_ptr = (u32)pc;
 439   *old_op_ptr = old_op;
 440
 441   // emit jump to code ptr
 442   *pc = make_jmp(pc, g_code_ptr);
 443
 444   // generate code:
 445   // TODO: our own stack
 446   emit_op_io(0xe50f0000, &g_linkpage->saved_regs[0]);  // str r0, [saved_regs[0]] @ save r0
 447   emit_op_io(0xe51f0000, (u32 *)&g_linkpage->lp_r1);   // ldr r0, =lp_r1
 448   emit_op   (0xe8807ffe);                              // stmia r0, {r1-r14}
 449   emit_op   (0xe2402004);                              // sub r2, r0, #4
 450   emit_op_io(0xe51f0000, pc_ptr);                      // ldr r0, =pc
 451   emit_op_io(0xe51f1000, old_op_ptr);                  // ldr r1, =old_op
 452   emit_op   (0xe1a04002);                              // mov r4, r2
 453   emit_op   (0xe1a0e00f);                              // mov lr, pc
 454   emit_op_io(0xe51ff000, (u32 *)&g_linkpage->handler); // ldr pc, =handle_op
 455   emit_op   (0xe8947fff);                              // ldmia r4, {r0-r14}
 456   emit_op   (make_jmp(g_code_ptr, pc + 1));            // jmp <back>
 457
 458   // sync caches
 459   sys_cacheflush(pc, pc + 1);
 460   sys_cacheflush(g_linkpage, g_code_ptr);
 461
 462   lp_size = (char *)g_code_ptr - (char *)g_linkpage;
 463   segvlog("code #%d %d/%d\n", g_linkpage_count, lp_size, LINKPAGE_SIZE);
 464
 465   if (lp_size + 13*4 > LINKPAGE_SIZE) {
 466     g_linkpage_count++;
 467     if (g_linkpage_count >= LINKPAGE_COUNT) {
 468       fprintf(stderr, "too many linkpages needed\n");
 469       abort();
 470     }
 471     g_linkpage = (void *)((char *)g_linkpage + LINKPAGE_SIZE);
 472     init_linkpage();
 473   }
 474   //handle_op(regs[15], op, regs, (u32)info->si_addr);
 475   //regs[15] += 4;
 476 }
 477
 478 void emu_init(void *map_bottom)
 479 {
 480   struct sigaction segv_action = {
 481     .sa_sigaction = segv_sigaction,
 482     .sa_flags = SA_SIGINFO,
 483   };
 484   void *ret;
 485
 486   sigemptyset(&segv_action.sa_mask);
 487   sigaction(SIGSEGV, &segv_action, NULL);
 488
 489   g_linkpage = (void *)(((u32)map_bottom - LINKPAGE_ALLOC) & ~0xfff);
 490   ret = mmap(g_linkpage, LINKPAGE_ALLOC, PROT_READ|PROT_WRITE, MAP_PRIVATE|MAP_ANONYMOUS, -1, 0);
 491   if (ret != g_linkpage) {
 492     perror("mmap linkpage");
 493     exit(1);
 494   }
 495   printf("linkpages @ %p\n", g_linkpage);
 496   init_linkpage();
 497
 498   // host stuff
 499   ret = host_video_init(&host_stride, 0);
 500   if (ret != 0) {
 501     printf("can't alloc screen\n");
 502     exit(1);
 503   }
 504   host_screen = host_video_flip();
 505 }
 506
 507 int emu_read_gpiodev(void *buf, int count)
 508 {
 509   unsigned int btns;
 510
 511   if (count < 4) {
 512     printf("gpiodev read %d?\n", count);
 513     return -1;
 514   }
 515
 516   btns = host_read_btns();
 517   memcpy(buf, &btns, 4);
 518   return 4;
 519 }
 520
 521 void *emu_mmap_dev(unsigned int length, int prot, int flags, unsigned int offset)
 522 {
 523   struct uppermem_block *umem;
 524   char name[32];
 525   int fd;
 526
 527   // SoC regs
 528   if ((offset & ~0xffff) == 0xc0000000) {
 529     return mmap((void *)0x7f000000, length, PROT_NONE,
 530       MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0);
 531   }
 532   // blitter
 533   if ((offset & ~0xffff) == 0xe0020000) {
 534     return mmap((void *)0x7f100000, length, PROT_NONE,
 535       MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0);
 536   }
 537   // upper mem
 538   if ((offset & 0xfe000000) != 0x02000000)
 539     printf("unexpected devmem mmap @ %08x\n", offset);
 540
 541   // return mmap(NULL, length, prot, flags, memdev, offset);
 542
 543   umem = calloc(1, sizeof(*umem));
 544   if (umem == NULL) {
 545     printf("OOM\n");
 546     return MAP_FAILED;
 547   }
 548
 549   umem->addr = offset;
 550   umem->size = length;
 551   umem->mem = mmap(NULL, length, prot, MAP_PRIVATE|MAP_ANONYMOUS, -1, 0);
 552   if (umem->mem != MAP_FAILED)
 553     goto done;
 554
 555   printf("upper mem @ %08x %d mmap fail, trying backing file\n", offset, length);
 556   sprintf(name, "m%08x", offset);
 557   fd = open(name, O_CREAT|O_RDWR, 0644);
 558   lseek(fd, length - 1, SEEK_SET);
 559   name[0] = 0;
 560   write(fd, name, 1);
 561
 562   umem->mem = mmap(NULL, length, prot, MAP_SHARED, fd, 0);
 563   if (umem->mem == MAP_FAILED) {
 564     printf("failed, giving up\n");
 565     close(fd);
 566     free(umem);
 567     return MAP_FAILED;
 568   }
 569
 570 done:
 571   printf("upper mem @ %08x %d\n", offset, length);
 572   umem->next = upper_mem;
 573   upper_mem = umem;
 574   return umem->mem;
 575 }
 576