[ginge.git] / loader / emu.c

// vim:shiftwidth=2:expandtab
#include <stdio.h>
#include <stdlib.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"

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

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

static int memdev;
static volatile u16 *memregs, *blitter;


static volatile void *translate_addr(u32 a, u32 *mask)
{
  if ((a & 0xfff00000) == 0x7f000000) {
    *mask = 0xffff;
    return memregs;
  }
  if ((a & 0xfff00000) == 0x7f100000) {
    *mask = 0xff;
    return blitter;
  }
  fprintf(stderr, "bad IO @ %08x\n", a);
  abort();
}

static u32 xread8(u32 a)
{
  volatile u8 *mem;
  u32 mask;

  iolog("r8  %08x\n", a);
  mem = translate_addr(a, &mask);
  return mem[a & mask];
}

static u32 xread16(u32 a)
{
  volatile u16 *mem;
  u32 mask;

  iolog("r16 %08x\n", a);
  mem = translate_addr(a, &mask);
  return mem[(a & mask) / 2];
}

static u32 xread32(u32 a)
{
  volatile u32 *mem;
  u32 mask;

  iolog("r32 %08x\n", a);
  mem = translate_addr(a, &mask);
  return mem[(a & mask) / 4];
}

static void xwrite8(u32 a, u32 d)
{
  volatile u8 *mem;
  u32 mask;

  iolog("w8  %08x %08x\n", a, d);
  mem = translate_addr(a, &mask);
  mem[a & mask] = d;
}

static void xwrite16(u32 a, u32 d)
{
  volatile u16 *mem;
  u32 mask;

  iolog("w16 %08x %08x\n", a, d);
  mem = translate_addr(a, &mask);
  mem[(a & mask) / 2] = d;
}

static void xwrite32(u32 a, u32 d)
{
  volatile u32 *mem;
  u32 mask;

  iolog("w32 %08x %08x\n", a, d);
  mem = translate_addr(a, &mask);
  mem[(a & mask) / 4] = d;
}

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

static void handle_op(u32 addr_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, 6)) // S
      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);
      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 (addr != addr_check) {
    fprintf(stderr, "bad calculated addr: %08x vs %08x\n", addr, addr_check);
    abort();
  }
  return;

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

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

  //printf("segv %d %p @ %08x\n", info->si_code, info->si_addr, regs[15]);
/*
  static int thissec, sfps;
  struct timeval tv;
  gettimeofday(&tv, NULL);
  sfps++;
  if (tv.tv_sec != thissec) {
    printf("%d\n", sfps);
    sfps = 0;
    thissec = tv.tv_sec;
  }
*/

  handle_op(regs[15], op, regs, (u32)info->si_addr);
  regs[15] += 4;
  return;

  //signal(num, SIG_DFL);
  //raise(num);
}

#define LINKPAGE_SIZE 0x1000

struct linkpage {
  u32 (*xread8)(u32 a);
  u32 (*xread16)(u32 a);
  u32 (*xread32)(u32 a);
  void (*xwrite8)(u32 a, u32 d);
  void (*xwrite16)(u32 a, u32 d);
  void (*xwrite32)(u32 a, u32 d);
  u32 retval;
  u32 *reg_ptr;
  u32 saved_regs[6]; // r0-r3,r12,lr
  u32 code[0];
};

static struct linkpage *g_linkpage;
static u32 *g_code_ptr;

void emu_init(void *map_bottom)
{
  struct sigaction segv_action = {
    .sa_sigaction = segv_sigaction,
    .sa_flags = SA_SIGINFO,
  };
  struct linkpage init_linkpage = {
    .xread8  = xread8,
    .xread16 = xread16,
    .xread32 = xread32,
    .xwrite8  = xwrite8,
    .xwrite16 = xwrite16,
    .xwrite32 = xwrite32,
  };
  void *ret;

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

  g_linkpage = (void *)(((u32)map_bottom - LINKPAGE_SIZE) & ~0xfff);
  ret = mmap(g_linkpage, LINKPAGE_SIZE, PROT_READ|PROT_WRITE, MAP_PRIVATE|MAP_ANONYMOUS, -1, 0);
  if (ret != g_linkpage) {
    perror("mmap linkpage");
    exit(1);
  }
  printf("linkpage @ %p\n", g_linkpage);
  memcpy(g_linkpage, &init_linkpage, sizeof(*g_linkpage));
  g_linkpage->reg_ptr = g_linkpage->saved_regs;
  g_code_ptr = g_linkpage->code;

  memdev = open("/dev/mem", O_RDWR);
  memregs = mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, memdev, 0xc0000000);
  blitter = mmap(NULL, 0x100, PROT_READ|PROT_WRITE, MAP_SHARED, memdev, 0xe0020000);
  printf("mapped %d %p %p\n", memdev, memregs, blitter);
}

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

  if ((offset & ~0xffff) == 0xc0000000) {
    return mmap((void *)0x7f000000, length, PROT_NONE,
      MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0);
  }
  if ((offset & ~0xffff) == 0xe0020000) {
    return mmap((void *)0x7f100000, length, PROT_NONE,
      MAP_PRIVATE|MAP_ANONYMOUS|MAP_FIXED|MAP_NORESERVE, -1, 0);
  }
  // pass through
  if ((offset & 0xfe000000) == 0x02000000)
    return mmap(NULL, length, prot, flags, memdev, offset);

  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);

  return mmap(NULL, length, prot, MAP_SHARED, fd, 0);
}
Commit	Line	Data
11913091	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