#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <signal.h>
#include <stdarg.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ptrace.h>
#include <fcntl.h>
#include <dirent.h>
#include <byteswap.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <errno.h>
#include <ucontext.h>

#include "ndrver.h"

#define IOTRACE	log_printf

#define IOSPY_MAX_MAPS	8

struct iospy_map
{
	unsigned long	fake_start;
	unsigned long	real_start;
	unsigned long	size;
	unsigned long	offset;
	const char *	(*name_hook)(unsigned long offset);
};

static struct iospy_map iospy_maps[IOSPY_MAX_MAPS];
static int iospy_mcount;
static unsigned long last_addr;
static unsigned long last_value;
static unsigned long repeat_count;
static int last_size;
static struct iospy_map *last_map;

static inline const char *name_hook(unsigned long addr)
{
	unsigned long off = addr - last_map->fake_start;

	if (last_map == NULL || last_map->name_hook == NULL)
		return "";
	return last_map->name_hook(off);
}

static inline unsigned long translate_addr(unsigned long addr, int silent)
{
	int i;

	last_map = NULL;
	for (i = 0; i < iospy_mcount; i++)
		if (iospy_maps[i].fake_start <= addr &&
		    (iospy_maps[i].fake_start + iospy_maps[i].size) > addr) {
			last_map = &iospy_maps[i]; // XXX CRAP !
			unsigned long off = addr - iospy_maps[i].fake_start;
			if (!silent)
				IOTRACE("0x%08lx ", off + iospy_maps[i].offset);
			return off + iospy_maps[i].real_start;
		}
	IOTRACE("<unmapped> ");
	return addr;
}

static int check_repeat(int size, unsigned long addr, unsigned long value,
			int force_complete)
{	
	if (!force_complete && last_size == size &&
	    addr == (last_addr + size) && value == last_value) {
		if (repeat_count == 0)
			IOTRACE(" ... ");
		repeat_count++;
		last_addr += size;
		return 1;
	}
	if (repeat_count) {
		IOTRACE("fill 0x%x more times\n", repeat_count);
		repeat_count = 0;
	}
	last_addr = addr;
	last_value = value;
	last_size = size;

	return 0;
}

static unsigned char do_read_byte(unsigned long addr)
{
	unsigned char b;

	check_repeat(0,0,0,1);

	IOTRACE("[IOR] B ");
	b = *((unsigned char *)translate_addr(addr, 0),
	      name_hook(addr));
	IOTRACE(": 0x%02x\n", b);
	return b;
}

static unsigned short do_read_word(unsigned long addr)
{
	unsigned short w;

	check_repeat(0,0,0,1);

	IOTRACE("[IOR] W ");
	w = *((unsigned short *)translate_addr(addr, 0));
	IOTRACE(": 0x%04x (sw: 0x%04x) %s\n", w, bswap_16(w),
		name_hook(addr));
	return w;
}

static unsigned long do_read_long(unsigned long addr)
{
	unsigned long l;

	check_repeat(0,0,0,1);

	IOTRACE("[IOR] L ");
	l = *((unsigned long *)translate_addr(addr, 0));
	IOTRACE(": 0x%08lx (sw: 0x%08x) %s\n", l, bswap_32(l),
		name_hook(addr));
	return l;
}

static double do_read_double(unsigned long addr)
{
	double d;

	check_repeat(0,0,0,1);

	IOTRACE("[IOR] D ");
	d = *((double *)translate_addr(addr, 0));
	IOTRACE(": %f %s\n", (float)d, name_hook(addr));
	return d;
}

static void do_write_byte(unsigned long addr, unsigned char b)
{
	int silent = check_repeat(1, addr, b, 0);
	if (!silent)
		IOTRACE("[IOW] B ");
	*((unsigned char *)translate_addr(addr, silent)) = b;
	if (!silent)
		IOTRACE(": 0x%02x %s\n", b, name_hook(addr));
}

static void do_write_word(unsigned long addr, unsigned short w)
{
	int silent = check_repeat(2, addr, w, 0);
	if (!silent)
		IOTRACE("[IOW] W ");
	*((unsigned short *)translate_addr(addr, silent)) = w;
	if (!silent)
		IOTRACE(": 0x%04x (sw: 0x%04x) %s\n", w, bswap_16(w),
			name_hook(addr));
}

static void do_write_long(unsigned long addr, unsigned long l)
{
	int silent = check_repeat(4, addr, l, 0);
	if (!silent)
		IOTRACE("[IOW] L ");
	*((unsigned long *)translate_addr(addr, silent)) = l;
	if (!silent)
		IOTRACE(": 0x%08lx (sw: 0x%08x) %s\n", l, bswap_32(l),
			name_hook(addr));
}

static void do_write_double(unsigned long addr, double d)
{	
	check_repeat(0,0,0,1);

	IOTRACE("[IOW] D ");
	*((double *)translate_addr(addr, 0)) = d;
	IOTRACE(": %f %s\n", (float)d, name_hook(addr));
}


#define L1_CACHE_BYTES	32	// XXX FIXME

struct aligninfo {
	unsigned char len;
	unsigned char flags;
};

#define	OPCD(inst)	(((inst) & 0xFC000000) >> 26)
#define	RS(inst)	(((inst) & 0x03E00000) >> 21)
#define	RA(inst)	(((inst) & 0x001F0000) >> 16)
#define	IS_XFORM(code)	((code) == 31)

#define INVALID	{ 0, 0 }

#define LD	1	/* load */
#define ST	2	/* store */
#define	SE	4	/* sign-extend value */
#define F	8	/* to/from fp regs */
#define U	0x10	/* update index register */
#define M	0x20	/* multiple load/store */
#define S	0x40	/* single-precision fp, or byte-swap value */
#define HARD	0x80	/* string, stwcx. */

#define DCBZ	0x5f	/* 8xx/82xx dcbz faults when cache not enabled */

/*
 * The PowerPC stores certain bits of the instruction that caused the
 * alignment exception in the DSISR register.  This array maps those
 * bits to information about the operand length and what the
 * instruction would do.
 */
static struct aligninfo aligninfo[128] = {
	{ 4, LD },		/* 00 0 0000: lwz / lwarx */
	{ 1, LD },		/* 00 0 0001: lbz */
	{ 4, ST },		/* 00 0 0010: stw */
       	{ 1, ST },		/* 00 0 0011: stb */
	{ 2, LD },		/* 00 0 0100: lhz */
	{ 2, LD+SE },		/* 00 0 0101: lha */
	{ 2, ST },		/* 00 0 0110: sth */
	{ 4, LD+M },		/* 00 0 0111: lmw */
	{ 4, LD+F+S },		/* 00 0 1000: lfs */
	{ 8, LD+F },		/* 00 0 1001: lfd */
	{ 4, ST+F+S },		/* 00 0 1010: stfs */
	{ 8, ST+F },		/* 00 0 1011: stfd */
	INVALID,		/* 00 0 1100 */
	INVALID,		/* 00 0 1101: ld/ldu/lwa */
	INVALID,		/* 00 0 1110 */
	INVALID,		/* 00 0 1111: std/stdu */
	{ 4, LD+U },		/* 00 1 0000: lwzu */
	{ 1, LD+U },		/* 00 1 0001: lbzu */
	{ 4, ST+U },		/* 00 1 0010: stwu */
	{ 1, ST+U },		/* 00 1 0011: stbu */
	{ 2, LD+U },		/* 00 1 0100: lhzu */
	{ 2, LD+SE+U },		/* 00 1 0101: lhau */
	{ 2, ST+U },		/* 00 1 0110: sthu */
	{ 4, ST+M },		/* 00 1 0111: stmw */
	{ 4, LD+F+S+U },	/* 00 1 1000: lfsu */
	{ 8, LD+F+U },		/* 00 1 1001: lfdu */
	{ 4, ST+F+S+U },	/* 00 1 1010: stfsu */
	{ 8, ST+F+U },		/* 00 1 1011: stfdu */
	INVALID,		/* 00 1 1100 */
	INVALID,		/* 00 1 1101 */
	INVALID,		/* 00 1 1110 */
	INVALID,		/* 00 1 1111 */
	INVALID,		/* 01 0 0000: ldx */
	INVALID,		/* 01 0 0001 */
	INVALID,		/* 01 0 0010: stdx */
	INVALID,		/* 01 0 0011 */
	INVALID,		/* 01 0 0100 */
	INVALID,		/* 01 0 0101: lwax */
	INVALID,		/* 01 0 0110 */
	INVALID,		/* 01 0 0111 */
	{ 0, LD+HARD },		/* 01 0 1000: lswx */
	{ 0, LD+HARD },		/* 01 0 1001: lswi */
	{ 0, ST+HARD },		/* 01 0 1010: stswx */
	{ 0, ST+HARD },		/* 01 0 1011: stswi */
	INVALID,		/* 01 0 1100 */
	INVALID,		/* 01 0 1101 */
	INVALID,		/* 01 0 1110 */
	INVALID,		/* 01 0 1111 */
	INVALID,		/* 01 1 0000: ldux */
	INVALID,		/* 01 1 0001 */
	INVALID,		/* 01 1 0010: stdux */
	INVALID,		/* 01 1 0011 */
	INVALID,		/* 01 1 0100 */
	INVALID,		/* 01 1 0101: lwaux */
	INVALID,		/* 01 1 0110 */
	INVALID,		/* 01 1 0111 */
	INVALID,		/* 01 1 1000 */
	INVALID,		/* 01 1 1001 */
	INVALID,		/* 01 1 1010 */
	INVALID,		/* 01 1 1011 */
	INVALID,		/* 01 1 1100 */
	INVALID,		/* 01 1 1101 */
	INVALID,		/* 01 1 1110 */
	INVALID,		/* 01 1 1111 */
	INVALID,		/* 10 0 0000 */
	INVALID,		/* 10 0 0001 */
	{ 0, ST+HARD },		/* 10 0 0010: stwcx. */
	INVALID,		/* 10 0 0011 */
	INVALID,		/* 10 0 0100 */
	INVALID,		/* 10 0 0101 */
	INVALID,		/* 10 0 0110 */
	INVALID,		/* 10 0 0111 */
	{ 4, LD+S },		/* 10 0 1000: lwbrx */
	INVALID,		/* 10 0 1001 */
	{ 4, ST+S },		/* 10 0 1010: stwbrx */
	INVALID,		/* 10 0 1011 */
	{ 2, LD+S },		/* 10 0 1100: lhbrx */
	INVALID,		/* 10 0 1101 */
	{ 2, ST+S },		/* 10 0 1110: sthbrx */
	INVALID,		/* 10 0 1111 */
	INVALID,		/* 10 1 0000 */
	INVALID,		/* 10 1 0001 */
	INVALID,		/* 10 1 0010 */
	INVALID,		/* 10 1 0011 */
	INVALID,		/* 10 1 0100 */
	INVALID,		/* 10 1 0101 */
	INVALID,		/* 10 1 0110 */
	INVALID,		/* 10 1 0111 */
	INVALID,		/* 10 1 1000 */
	INVALID,		/* 10 1 1001 */
	INVALID,		/* 10 1 1010 */
	INVALID,		/* 10 1 1011 */
	INVALID,		/* 10 1 1100 */
	INVALID,		/* 10 1 1101 */
	INVALID,		/* 10 1 1110 */
	{ 0, ST+HARD },		/* 10 1 1111: dcbz */
	{ 4, LD },		/* 11 0 0000: lwzx */
	{ 1, LD },		/* 11 0 0001: lbzx */
	{ 4, ST },		/* 11 0 0010: stwx */
	{ 1, ST },		/* 11 0 0011: stbx */
	{ 2, LD },		/* 11 0 0100: lhzx */
	{ 2, LD+SE },		/* 11 0 0101: lhax */
	{ 2, ST },		/* 11 0 0110: sthx */
	INVALID,		/* 11 0 0111 */
	{ 4, LD+F+S },		/* 11 0 1000: lfsx */
	{ 8, LD+F },		/* 11 0 1001: lfdx */
	{ 4, ST+F+S },		/* 11 0 1010: stfsx */
	{ 8, ST+F },		/* 11 0 1011: stfdx */
	INVALID,		/* 11 0 1100 */
	INVALID,		/* 11 0 1101: lmd */
	INVALID,		/* 11 0 1110 */
	INVALID,		/* 11 0 1111: stmd */
	{ 4, LD+U },		/* 11 1 0000: lwzux */
	{ 1, LD+U },		/* 11 1 0001: lbzux */
	{ 4, ST+U },		/* 11 1 0010: stwux */
	{ 1, ST+U },		/* 11 1 0011: stbux */
	{ 2, LD+U },		/* 11 1 0100: lhzux */
	{ 2, LD+SE+U },		/* 11 1 0101: lhaux */
	{ 2, ST+U },		/* 11 1 0110: sthux */
	INVALID,		/* 11 1 0111 */
	{ 4, LD+F+S+U },	/* 11 1 1000: lfsux */
	{ 8, LD+F+U },		/* 11 1 1001: lfdux */
	{ 4, ST+F+S+U },	/* 11 1 1010: stfsux */
	{ 8, ST+F+U },		/* 11 1 1011: stfdux */
	INVALID,		/* 11 1 1100 */
	INVALID,		/* 11 1 1101 */
	INVALID,		/* 11 1 1110 */
	INVALID,		/* 11 1 1111 */
};

#define SWAP(a, b)	(t = (a), (a) = (b), (b) = t)

static void iospy_segv(int signum, siginfo_t *info, void *ctxt_v)
{
	ucontext_t *ctxt = (ucontext_t *)ctxt_v;
	struct pt_regs *regs = ctxt->uc_mcontext.regs;
	double *fpregs = ctxt->uc_mcontext.uc_regs->fpregs.fpregs;
	int instr, oinstr, nb, flags;
	int opcode, f1, f2, f3;
	int t, reg, areg;
	unsigned long addr;
	union {
		long l;
		float f;
		double d;
		unsigned char v[8];
	} data;

	instr = oinstr = *((unsigned int *)regs->nip);
	opcode = OPCD(instr);
	reg = RS(instr);
	areg = RA(instr);

	if (!IS_XFORM(opcode)) {
		f1 = 0;
		f2 = (instr & 0x04000000) >> 26;
		f3 = (instr & 0x78000000) >> 27;
	} else {
		f1 = (instr & 0x00000006) >> 1;
		f2 = (instr & 0x00000040) >> 6;
		f3 = (instr & 0x00000780) >> 7;
	}

	instr = ((f1 << 5) | (f2 << 4) | f3);

	nb = aligninfo[instr].len;
	if (nb == 0) {
		unsigned long taddr;
		long *p;
		int i;

		if (instr != DCBZ)
			goto unhandled;	/* too hard or invalid instruction */
		/*
		 * The dcbz (data cache block zero) instruction
		 * gives an alignment fault if used on non-cacheable
		 * memory.  We handle the fault mainly for the
		 * case when we are running with the cache disabled
		 * for debugging.
		 */
		IOTRACE("[IOW] DCBZ ");
		taddr = translate_addr(regs->dar & -L1_CACHE_BYTES, 0);
		p = (long *)taddr;
		for (i = 0; i < L1_CACHE_BYTES / sizeof(long); ++i)
			p[i] = 0;
		IOTRACE("\n");
		goto handled;
	}

	flags = aligninfo[instr].flags;

	/* For the 4xx-family processors, the 'dar' field of the
	 * pt_regs structure is overloaded and is really from the DEAR.
	 */

	addr = regs->dar;

	if (flags & M)
		goto unhandled;		/* too hard for now */

	/* If we read the operand, copy it in */
	if (flags & LD) {
		unsigned short w;
		data.l = 0;
		switch(nb) {
		case 1:
			data.v[3] = do_read_byte(addr);
			break;
		case 2:
			w = do_read_word(addr);
			data.v[2] = w >> 8;
			data.v[3] = w & 0xff;
			break;
		case 4:
			data.l = do_read_long(addr);
			break;
		case 8:
			data.d = do_read_double(addr);
			break;
		default:
			IOTRACE("Unknown load size: %d\n", nb);
			goto unhandled;
		}
	}

	switch (flags & ~U) {
	case LD+SE:
		if (data.v[2] >= 0x80)
			data.v[0] = data.v[1] = -1;
		/* fall through */
	case LD:
		regs->gpr[reg] = data.l;
		break;
	case LD+S:
		if (nb == 2) {
			SWAP(data.v[2], data.v[3]);
		} else {
			SWAP(data.v[0], data.v[3]);
			SWAP(data.v[1], data.v[2]);
		}
		regs->gpr[reg] = data.l;
		break;
	case ST:
		data.l = regs->gpr[reg];
		break;
	case ST+S:
		data.l = regs->gpr[reg];
		if (nb == 2) {
			SWAP(data.v[2], data.v[3]);
		} else {
			SWAP(data.v[0], data.v[3]);
			SWAP(data.v[1], data.v[2]);
		}
		break;
	case LD+F:
		fpregs[reg] = data.d;
		break;
	case ST+F:
		data.d = fpregs[reg];
		break;
	/* these require some floating point conversions... */
	/* we'd like to use the assignment, but we have to compile
	 * the kernel with -msoft-float so it doesn't use the
	 * fp regs for copying 8-byte objects. */
	case LD+F+S:
		fpregs[reg] = data.f;
		break;
	case ST+F+S:
		data.f = fpregs[reg];
		break;
	default:
		IOTRACE("align: can't handle flags=%x\n", flags);
		goto unhandled;
	}

	if (flags & ST) {
		unsigned short w;
		switch(nb) {
		case 1:
			do_write_byte(addr, data.v[3]);
			break;
		case 2:
			w = ((unsigned short)data.v[2]) << 8;
			w |= data.v[3];
			do_write_word(addr, w);
			break;
		case 4:
			do_write_long(addr, data.l);
			break;
		case 8:
			do_write_double(addr, data.d);
			break;
		default:
			IOTRACE("Unknown store size: %d\n", nb);
			goto unhandled;
		}
	}

	if (flags & U) {
		regs->gpr[areg] = regs->dar;
	}
 handled:
	regs->nip += 4;
	return;

 unhandled:
	IOTRACE(" Can't handle instruction 0x%08x ! skipped...\n", oinstr);
	IOTRACE(" TRAP: %lx NIP: 0x%08lx, LR: 0x%08lx, DAR: 0x%08lx\n",
		regs->trap, regs->nip, regs->link, regs->dar);

	regs->nip += 4;
}

int iospy_init(void)
{
	struct sigaction act = {
		.sa_sigaction = iospy_segv,
		.sa_flags = SA_SIGINFO
	};
	sigaction(SIGSEGV, &act, NULL);
	return 0;
}

void * iospy_mmap(void *start, size_t length, int prot , int flags, int fd, off_t offset,
		  const char * (*name_hook)(unsigned long off))
{
	int i = iospy_mcount;
	void *fake, *real;
	int fflags;

	if ( i >= IOSPY_MAX_MAPS ) {
		IOTRACE("iospy: Too many mappings, doing normal mmap\n");
		goto normal_map;
	}
	if (prot == 0)
		goto normal_map;
	fflags = (flags & MAP_FIXED) | MAP_PRIVATE | MAP_ANONYMOUS;
	fake = mmap(start, length, PROT_NONE, fflags, 0, 0);
	if (fake == MAP_FAILED) {
		IOTRACE("iospy: Failed to do the fake mapping\n");
		return mmap(start, length, prot, flags, fd, offset);
	}
	real = mmap(0, length, prot, flags & ~MAP_FIXED, fd, offset);
	if (real == MAP_FAILED) {
		munmap(fake, length);
		return MAP_FAILED;
	}
	iospy_mcount++;
	iospy_maps[i].fake_start = (unsigned long)fake;
	iospy_maps[i].real_start = (unsigned long)real;
	iospy_maps[i].size = length;
	iospy_maps[i].offset = offset;
	iospy_maps[i].name_hook = name_hook;
	return fake;
 normal_map:
	return mmap(start, length, prot, flags, fd, offset);
}

int iospy_munmap(void *start, size_t length)
{
	return munmap(start, length);
}