loader/emu.c

   1 // vim:shiftwidth=2:expandtab
   2 #include <stdio.h>
   3 #include <stdlib.h>
   4 #include <sys/types.h>
   5 #include <sys/stat.h>
   6 #include <fcntl.h>
   7 #include <sys/mman.h>
   8 #include <sys/types.h>
   9 #include <unistd.h>
  10 #include <signal.h>
  11 #include <asm/ucontext.h>
  12
  13 #include "header.h"
  14
  15 #define iolog printf
  16 //#define iolog(...)
  17
  18 typedef unsigned int   u32;
  19 typedef unsigned short u16;
  20 typedef unsigned char  u8;
  21
  22 static int memdev;
  23 static volatile u16 *memregs, *blitter;
  24
  25
  26 static volatile void *translate_addr(u32 a, u32 *mask)
  27 {
  28   if ((a & 0xfff00000) == 0x7f000000) {
  29     *mask = 0xffff;
  30     return memregs;
  31   }
  32   if ((a & 0xfff00000) == 0x7f100000) {
  33     *mask = 0xff;
  34     return blitter;
  35   }
  36   fprintf(stderr, "bad IO @ %08x\n", a);
  37   abort();
  38 }
  39
  40 static u32 xread8(u32 a)
  41 {
  42   volatile u8 *mem;
  43   u32 mask;
  44
  45   iolog("r8  %08x\n", a);
  46   mem = translate_addr(a, &mask);
  47   return mem[a & mask];
  48 }
  49
  50 static u32 xread16(u32 a)
  51 {
  52   volatile u16 *mem;
  53   u32 mask;
  54
  55   iolog("r16 %08x\n", a);
  56   mem = translate_addr(a, &mask);
  57   return mem[(a & mask) / 2];
  58 }
  59
  60 static u32 xread32(u32 a)
  61 {
  62   volatile u32 *mem;
  63   u32 mask;
  64
  65   iolog("r32 %08x\n", a);
  66   mem = translate_addr(a, &mask);
  67   return mem[(a & mask) / 4];
  68 }
  69
  70 static void xwrite8(u32 a, u32 d)
  71 {
  72   volatile u8 *mem;
  73   u32 mask;
  74
  75   iolog("w8  %08x %08x\n", a, d);
  76   mem = translate_addr(a, &mask);
  77   mem[a & mask] = d;
  78 }
  79
  80 static void xwrite16(u32 a, u32 d)
  81 {
  82   volatile u16 *mem;
  83   u32 mask;
  84
  85   iolog("w16 %08x %08x\n", a, d);
  86   mem = translate_addr(a, &mask);
  87   mem[(a & mask) / 2] = d;
  88 }
  89
  90 static void xwrite32(u32 a, u32 d)
  91 {
  92   volatile u32 *mem;
  93   u32 mask;
  94
  95   iolog("w32 %08x %08x\n", a, d);
  96   mem = translate_addr(a, &mask);
  97   mem[(a & mask) / 4] = d;
  98 }
  99
 100 #define BIT_SET(v, b) (v & (1 << (b)))
 101
 102 static void handle_op(u32 addr_pc, u32 op, u32 *regs, u32 addr_check)
 103 {
 104   u32 t, shift, ret, addr;
 105   int rn, rd;
 106
 107   rd = (op & 0x0000f000) >> 12;
 108   rn = (op & 0x000f0000) >> 16;
 109
 110   if ((op & 0x0f200090) == 0x01000090) { // AM3: LDRH, STRH
 111     if (BIT_SET(op, 6)) // S
 112       goto unhandled;
 113
 114     if (BIT_SET(op, 22))                // imm offset
 115       t = ((op & 0xf00) >> 4) | (op & 0x0f);
 116     else                                // reg offset
 117       t = regs[op & 0x000f];
 118
 119     if (!BIT_SET(op, 23))
 120       t = -t;
 121     addr = regs[rn] + t;
 122
 123     if (BIT_SET(op, 20)) { // Load
 124       ret = xread16(addr);
 125       regs[rd] = ret;
 126     }
 127     else
 128       xwrite16(addr, regs[rd]);
 129   }
 130   else if ((op & 0x0d200000) == 0x05000000) { // AM2: LDR[B], STR[B]
 131     if (BIT_SET(op, 25)) {              // reg offs
 132       if (BIT_SET(op, 4))
 133         goto unhandled;
 134
 135       t = regs[op & 0x000f];
 136       shift = (op & 0x0f80) >> 7;
 137       switch ((op & 0x0060) >> 5) {
 138         case 0: t = t << shift; break;
 139         case 1: t = t >> (shift + 1); break;
 140         case 2: t = (signed int)t >> (shift + 1); break;
 141         case 3: goto unhandled; // I'm just lazy
 142       }
 143     }
 144     else                                // imm offs
 145       t = op & 0x0fff;
 146
 147     if (!BIT_SET(op, 23))
 148       t = -t;
 149     addr = regs[rn] + t;
 150
 151     if (BIT_SET(op, 20)) { // Load
 152       if (BIT_SET(op, 22)) // Byte
 153         ret = xread8(addr);
 154       else
 155         ret = xread32(addr);
 156       regs[rd] = ret;
 157     }
 158     else {
 159       if (BIT_SET(op, 22)) // Byte
 160         xwrite8(addr, regs[rd]);
 161       else
 162         xwrite32(addr, regs[rd]);
 163     }
 164   }
 165   else
 166     goto unhandled;
 167
 168   if (addr != addr_check) {
 169     fprintf(stderr, "bad calculated addr: %08x vs %08x\n", addr, addr_check);
 170     abort();
 171   }
 172   return;
 173
 174 unhandled:
 175   fprintf(stderr, "unhandled IO op %08x @ %08x\n", op, addr_pc);
 176 }
 177
 178 static void segv_sigaction(int num, siginfo_t *info, void *ctx)
 179 {
 180   struct ucontext *context = ctx;
 181   u32 *regs = (u32 *)&context->uc_mcontext.arm_r0;
 182   u32 op = *(u32 *)regs[15];
 183
 184   //printf("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]);
 185 /*
 186   static int thissec, sfps;
 187   struct timeval tv;
 188   gettimeofday(&tv, NULL);
 189   sfps++;
 190   if (tv.tv_sec != thissec) {
 191     printf("%d\n", sfps);
 192     sfps = 0;
 193     thissec = tv.tv_sec;
 194   }
 195 */
 196
 197   handle_op(regs[15], op, regs, (u32)info->si_addr);
 198   regs[15] += 4;
 199   return;
 200
 201   //signal(num, SIG_DFL);
 202   //raise(num);
 203 }
 204
 205 #define LINKPAGE_SIZE 0x1000
 206
 207 struct linkpage {
 208   u32 (*xread8)(u32 a);
 209   u32 (*xread16)(u32 a);
 210   u32 (*xread32)(u32 a);
 211   void (*xwrite8)(u32 a, u32 d);
 212   void (*xwrite16)(u32 a, u32 d);
 213   void (*xwrite32)(u32 a, u32 d);
 214   u32 retval;
 215   u32 *reg_ptr;
 216   u32 saved_regs[6]; // r0-r3,r12,lr
 217   u32 code[0];
 218 };
 219
 220 static struct linkpage *g_linkpage;
 221 static u32 *g_code_ptr;
 222
 223 void emu_init(void *map_bottom)
 224 {
 225   struct sigaction segv_action = {
 226     .sa_sigaction = segv_sigaction,
 227     .sa_flags = SA_SIGINFO,
 228   };
 229   struct linkpage init_linkpage = {
 230     .xread8  = xread8,
 231     .xread16 = xread16,
 232     .xread32 = xread32,
 233     .xwrite8  = xwrite8,
 234     .xwrite16 = xwrite16,
 235     .xwrite32 = xwrite32,
 236   };
 237   void *ret;
 238
 239   sigemptyset(&segv_action.sa_mask);
 240   sigaction(SIGSEGV, &segv_action, NULL);
 241
 242   g_linkpage = (void *)(((u32)map_bottom - LINKPAGE_SIZE) & ~0xfff);
 243   ret = mmap(g_linkpage, LINKPAGE_SIZE, PROT_READ|PROT_WRITE, MAP_PRIVATE|MAP_ANONYMOUS, -1, 0);
 244   if (ret != g_linkpage) {
 245     perror("mmap linkpage");
 246     exit(1);
 247   }
 248   printf("linkpage @ %p\n", g_linkpage);
 249   memcpy(g_linkpage, &init_linkpage, sizeof(*g_linkpage));
 250   g_linkpage->reg_ptr = g_linkpage->saved_regs;
 251   g_code_ptr = g_linkpage->code;
 252
 253   memdev = open("/dev/mem", O_RDWR);
 254   memregs = mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, memdev, 0xc0000000);
 255   blitter = mmap(NULL, 0x100, PROT_READ|PROT_WRITE, MAP_SHARED, memdev, 0xe0020000);
 256   printf("mapped %d %p %p\n", memdev, memregs, blitter);
 257 }
 258
 259 void *emu_mmap_dev(unsigned int length, int prot, int flags, unsigned int offset)
 260 {
 261   char name[32];
 262   int fd;
 263
 264   if ((offset & ~0xffff) == 0xc0000000) {
 265     return mmap((void *)0x7f000000, length, PROT_NONE,
 266       MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0);
 267   }
 268   if ((offset & ~0xffff) == 0xe0020000) {
 269     return mmap((void *)0x7f100000, length, PROT_NONE,
 270       MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0);
 271   }
 272   // pass through
 273   if ((offset & 0xfe000000) == 0x02000000)
 274     return mmap(NULL, length, prot, flags, memdev, offset);
 275
 276   sprintf(name, "m%08x", offset);
 277   fd = open(name, O_CREAT|O_RDWR, 0644);
 278   lseek(fd, length - 1, SEEK_SET);
 279   name[0] = 0;
 280   write(fd, name, 1);
 281
 282   return mmap(NULL, length, prot, MAP_SHARED, fd, 0);
 283 }
 284