[ginge.git] / loader / emu.c

// vim:shiftwidth=2:expandtab
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <sys/types.h>
#include <unistd.h>
#include <signal.h>
#include <asm/ucontext.h>

#include "header.h"
#include "sys_cacheflush.h"

//#define iolog printf
#define iolog(...)

typedef unsigned int   u32;
typedef unsigned short u16;
typedef unsigned char  u8;

struct uppermem_block {
  u32 addr; // physical
  u32 size;
  void *mem;
  struct uppermem_block *next;
};

static struct uppermem_block *upper_mem;

static struct {
  u32 dstctrl;
  u32 dstaddr;
  u32 dststride;
  u32 srcctrl;
  u32 srcaddr;          //
  u32 srcstride;
  u32 srcforcolor;
  u32 srcbackcolor;
  u32 patctrl;          //
  u32 patforcolor;
  u32 patbackcolor;
  u32 size;
  u32 ctrl;             //
  u32 run;
  u32 intc;
  u32 srcfifo;
} blitter;

static struct {
  union {
    u32 mlc_stl_eadr;
    struct {
      u16 mlc_stl_eadrl;
      u16 mlc_stl_eadrh;
    };
  };
} mmsp2;

static u16 *host_screen;
static int host_stride;


static void *upper_lookup(u32 addr, u8 **mem_end, int *stride_override)
{
  struct uppermem_block *ub;

  // maybe the screen?
  if (mmsp2.mlc_stl_eadr <= addr && addr < mmsp2.mlc_stl_eadr + 320*240*2) {
    host_screen = host_video_flip(); // HACK
    *mem_end = (u8 *)host_screen + host_stride * 240;
    *stride_override = host_stride;
    return (u8 *)host_screen + addr - mmsp2.mlc_stl_eadr;
  }

  for (ub = upper_mem; ub != NULL; ub = ub->next) {
    if (ub->addr <= addr && addr < ub->addr + ub->size) {
      *mem_end = (u8 *)ub->mem + ub->size;
      return (u8 *)ub->mem + addr - ub->addr;
    }
  }

  return NULL;
}

static void blitter_do(void)
{
  u8 *dst, *dste, *src, *srce;
  int w, h, sstrd, dstrd;

  if (blitter.srcaddr == blitter.dstaddr)
    return; // dummy blit?

  w = blitter.size & 0x7ff;
  h = (blitter.size >> 16) & 0x7ff;
  sstrd = blitter.srcstride;
  dstrd = blitter.dststride;

  dst = upper_lookup(blitter.dstaddr, &dste, &dstrd);
  src = upper_lookup(blitter.srcaddr, &srce, &sstrd);

  if (dst == NULL || src == NULL) {
    printf("blit %08x->%08x %dx%d translated to %p->%p\n",
      blitter.srcaddr, blitter.dstaddr, w, h, src, dst);
    return;
  }

  if (dst + dstrd * h > dste) {
    printf("blit %08x->%08x %dx%d did not fit dst\n",
      blitter.srcaddr, blitter.dstaddr, w, h);
    h = (dste - dst) / dstrd;
  }

  if (src + sstrd * h > srce) {
    printf("blit %08x->%08x %dx%d did not fit src\n",
      blitter.srcaddr, blitter.dstaddr, w, h);
    h = (srce - src) / sstrd;
  }

  for (; h > 0; h--, dst += dstrd, src += sstrd)
    memcpy(dst, src, w * 2);
}

static u32 xread8(u32 a)
{
  iolog("r8  %08x\n", a);
  return 0;
}

static u32 xread16(u32 a)
{
// if ((a & 0xfff00000) == 0x7f100000) { static int a; a ^= ~1; return a & 0xffff; }
  iolog("r16 %08x\n", a);
  return 0;
}

static u32 xread32(u32 a)
{
  u32 d = 0;
  if ((a & 0xfff00000) == 0x7f100000) {
    u32 *bl = &blitter.dstctrl;
    a &= 0xfff;
    if (a < 0x40)
      d = bl[a / 4];
    if (a == 0x34)
      d = 0; // not busy
  }
  iolog("r32 %08x\n", a);
  return d;
}

static void xwrite8(u32 a, u32 d)
{
  iolog("w8  %08x %08x\n", a, d);
}

static void xwrite16(u32 a, u32 d)
{
  iolog("w16 %08x %08x\n", a, d);
  if ((a & 0xfff00000) == 0x7f000000) {
    a &= 0xffff;
    switch (a) {
      case 0x2912: mmsp2.mlc_stl_eadrl = d; break;
      case 0x2914: mmsp2.mlc_stl_eadrh = d; break;
    }
    printf("w16 %08x %08x\n", a, d);
  }
}

static void xwrite32(u32 a, u32 d)
{
  iolog("w32 %08x %08x\n", a, d);
  if ((a & 0xfff00000) == 0x7f000000) {
    printf("w32 %08x %08x\n", a, d);
    return;
  }
  if ((a & 0xfff00000) == 0x7f100000) {
    u32 *bl = &blitter.dstctrl;
    a &= 0xfff;
    if (a < 0x40)
      bl[a / 4] = d;
    if (a == 0x34 && (d & 1))
      blitter_do();
    return;
  }
}

#define BIT_SET(v, b) (v & (1 << (b)))

static void handle_op(u32 pc, u32 op, u32 *regs, u32 addr_check)
{
  u32 t, shift, ret, addr;
  int rn, rd;

  rd = (op & 0x0000f000) >> 12;
  rn = (op & 0x000f0000) >> 16;

  if ((op & 0x0f200090) == 0x01000090) { // AM3: LDRH, STRH
    if (!BIT_SET(op, 5)) // !H
      goto unhandled;
    if (BIT_SET(op, 6) && !BIT_SET(op, 20)) // S && !L
      goto unhandled;

    if (BIT_SET(op, 22))                // imm offset
      t = ((op & 0xf00) >> 4) | (op & 0x0f);
    else                                // reg offset
      t = regs[op & 0x000f];

    if (!BIT_SET(op, 23))
      t = -t;
    addr = regs[rn] + t;

    if (BIT_SET(op, 20)) { // Load
      ret = xread16(addr);
      if (BIT_SET(op, 6)) { // S
        ret <<= 16;
        ret = (signed int)ret >> 16;
      }
      regs[rd] = ret;
    }
    else
      xwrite16(addr, regs[rd]);
  }
  else if ((op & 0x0d200000) == 0x05000000) { // AM2: LDR[B], STR[B]
    if (BIT_SET(op, 25)) {              // reg offs
      if (BIT_SET(op, 4))
        goto unhandled;

      t = regs[op & 0x000f];
      shift = (op & 0x0f80) >> 7;
      switch ((op & 0x0060) >> 5) {
        case 0: t = t << shift; break;
        case 1: t = t >> (shift + 1); break;
        case 2: t = (signed int)t >> (shift + 1); break;
        case 3: goto unhandled; // I'm just lazy
      }
    }
    else                                // imm offs
      t = op & 0x0fff;

    if (!BIT_SET(op, 23))
      t = -t;
    addr = regs[rn] + t;

    if (BIT_SET(op, 20)) { // Load
      if (BIT_SET(op, 22)) // Byte
        ret = xread8(addr);
      else
        ret = xread32(addr);
      regs[rd] = ret;
    }
    else {
      if (BIT_SET(op, 22)) // Byte
        xwrite8(addr, regs[rd]);
      else
        xwrite32(addr, regs[rd]);
    }
  }
  else
    goto unhandled;

#if 0
  if (addr != addr_check) {
    fprintf(stderr, "bad calculated addr: %08x vs %08x\n", addr, addr_check);
    abort();
  }
#endif
  return;

unhandled:
  fprintf(stderr, "unhandled IO op %08x @ %08x\n", op, pc);
}

#define LINKPAGE_SIZE 0x1000
#define LINKPAGE_COUNT 4
#define LINKPAGE_ALLOC (LINKPAGE_SIZE * LINKPAGE_COUNT)

struct linkpage {
  u32 saved_regs[15];
  u32 *lp_r1;
  void (*handler)(u32 addr_pc, u32 op, u32 *regs, u32 addr_check);
  u32 code[0];
};

static struct linkpage *g_linkpage;
static u32 *g_code_ptr;
static int g_linkpage_count;

static void init_linkpage(void)
{
  g_linkpage->lp_r1 = &g_linkpage->saved_regs[1];
  g_linkpage->handler = handle_op;
  g_code_ptr = g_linkpage->code;
}

static u32 make_offset12(u32 *pc, u32 *target)
{
  int lp_offs, u = 1;

  lp_offs = (char *)target - (char *)pc - 2*4;
  if (lp_offs < 0) {
    lp_offs = -lp_offs;
    u = 0;
  }
  if (lp_offs >= LINKPAGE_SIZE) {
    fprintf(stderr, "linkpage too far: %d\n", lp_offs);
    abort();
  }

  return (u << 23) | lp_offs;
}

static u32 make_jmp(u32 *pc, u32 *target)
{
  int jmp_val;

  jmp_val = target - pc - 2;
  if (jmp_val < (int)0xff000000 || jmp_val > 0x00ffffff) {
    fprintf(stderr, "jump out of range (%p -> %p)\n", pc, target);
    abort();
  }

  return 0xea000000 | (jmp_val & 0x00ffffff);
}

static void emit_op(u32 op)
{
  *g_code_ptr++ = op;
}

static void emit_op_io(u32 op, u32 *target)
{
  op |= make_offset12(g_code_ptr, target);
  emit_op(op);
}

static void segv_sigaction(int num, siginfo_t *info, void *ctx)
{
  struct ucontext *context = ctx;
  u32 *regs = (u32 *)&context->uc_mcontext.arm_r0;
  u32 *pc = (u32 *)regs[15];
  u32 old_op = *pc;
  u32 *pc_ptr, *old_op_ptr;
  int lp_size;

  if (((regs[15] ^ (u32)&segv_sigaction) & 0xff000000) == 0 ||       // PC is in our segment or
      (((regs[15] ^ (u32)g_linkpage) & ~(LINKPAGE_ALLOC - 1)) == 0)) // .. in linkpage
  {
    // real crash - time to die
    printf("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]);
    signal(num, SIG_DFL);
    raise(num);
  }
  printf("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]);

  // spit PC and op
  pc_ptr = g_code_ptr++;
  old_op_ptr = g_code_ptr++;
  *pc_ptr = (u32)pc;
  *old_op_ptr = old_op;

  // emit jump to code ptr
  *pc = make_jmp(pc, g_code_ptr);

  // generate code:
  // TODO: our own stack
  emit_op_io(0xe50f0000, &g_linkpage->saved_regs[0]);  // str r0, [saved_regs[0]] @ save r0
  emit_op_io(0xe51f0000, (u32 *)&g_linkpage->lp_r1);   // ldr r0, =lp_r1
  emit_op   (0xe8807ffe);                              // stmia r0, {r1-r14}
  emit_op   (0xe2402004);                              // sub r2, r0, #4
  emit_op_io(0xe51f0000, pc_ptr);                      // ldr r0, =pc
  emit_op_io(0xe51f1000, old_op_ptr);                  // ldr r1, =old_op
  emit_op   (0xe1a04002);                              // mov r4, r2
  emit_op   (0xe1a0e00f);                              // mov lr, pc
  emit_op_io(0xe51ff000, (u32 *)&g_linkpage->handler); // ldr pc, =handle_op
  emit_op   (0xe8947fff);                              // ldmia r4, {r0-r14}
  emit_op   (make_jmp(g_code_ptr, pc + 1));            // jmp <back>

  // sync caches
  sys_cacheflush(pc, pc + 1);
  sys_cacheflush(g_linkpage, g_code_ptr);

  lp_size = (char *)g_code_ptr - (char *)g_linkpage;
  printf("code #%d %d/%d\n", g_linkpage_count, lp_size, LINKPAGE_SIZE);

  if (lp_size + 13*4 > LINKPAGE_SIZE) {
    g_linkpage_count++;
    if (g_linkpage_count >= LINKPAGE_COUNT) {
      fprintf(stderr, "too many linkpages needed\n");
      abort();
    }
    g_linkpage = (void *)((char *)g_linkpage + LINKPAGE_SIZE);
    init_linkpage();
  }
  //handle_op(regs[15], op, regs, (u32)info->si_addr);
  //regs[15] += 4;
}

void emu_init(void *map_bottom)
{
  struct sigaction segv_action = {
    .sa_sigaction = segv_sigaction,
    .sa_flags = SA_SIGINFO,
  };
  void *ret;

  sigemptyset(&segv_action.sa_mask);
  sigaction(SIGSEGV, &segv_action, NULL);

  g_linkpage = (void *)(((u32)map_bottom - LINKPAGE_ALLOC) & ~0xfff);
  ret = mmap(g_linkpage, LINKPAGE_ALLOC, PROT_READ|PROT_WRITE, MAP_PRIVATE|MAP_ANONYMOUS, -1, 0);
  if (ret != g_linkpage) {
    perror("mmap linkpage");
    exit(1);
  }
  printf("linkpages @ %p\n", g_linkpage);
  init_linkpage();

  // host stuff
  host_screen = host_video_init(&host_stride);
  if (host_screen == NULL) {
    printf("can't alloc screen\n");
    exit(1);
  }
}

int emu_read_gpiodev(void *buf, int count)
{
  unsigned int btns;

  if (count < 4) {
    printf("gpiodev read %d?\n", count);
    return -1;
  }

  btns = host_read_btns();
  memcpy(buf, &btns, 4);
  return 4;
}

void *emu_mmap_dev(unsigned int length, int prot, int flags, unsigned int offset)
{
  struct uppermem_block *umem;
  char name[32];
  int fd;

  // SoC regs
  if ((offset & ~0xffff) == 0xc0000000) {
    return mmap((void *)0x7f000000, length, PROT_NONE,
      MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0);
  }
  // blitter
  if ((offset & ~0xffff) == 0xe0020000) {
    return mmap((void *)0x7f100000, length, PROT_NONE,
      MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0);
  }
  // upper mem
  if ((offset & 0xfe000000) != 0x02000000)
    printf("unexpected devmem mmap @ %08x\n", offset);

  // return mmap(NULL, length, prot, flags, memdev, offset);

  umem = calloc(1, sizeof(*umem));
  if (umem == NULL) {
    printf("OOM\n");
    return MAP_FAILED;
  }

  umem->addr = offset;
  umem->size = length;
  umem->mem = mmap(NULL, length, prot, MAP_PRIVATE|MAP_ANONYMOUS, -1, 0);
  if (umem->mem != MAP_FAILED)
    goto done;

  printf("upper mem @ %08x %d mmap fail, trying backing file\n", offset, length);
  sprintf(name, "m%08x", offset);
  fd = open(name, O_CREAT|O_RDWR, 0644);
  lseek(fd, length - 1, SEEK_SET);
  name[0] = 0;
  write(fd, name, 1);

  umem->mem = mmap(NULL, length, prot, MAP_SHARED, fd, 0);
  if (umem->mem == MAP_FAILED) {
    printf("failed, giving up\n");
    close(fd);
    free(umem);
    return MAP_FAILED;
  }

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