/* started August 28, 2006 */
/* TODO:
 * encode all opcodes as little endian
 */
/* HISTORY
 * September 1, 2006 - rewrote huge parts */
#include <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include "byte_order.h"

#ifdef NDEBUG
#define DEBUG(...) 
#define DEBUG_ONLY(x) 
#else
#define DEBUG(...) fprintf(stderr, __VA_ARGS__)
#define DEBUG_ONLY(x) x
#endif

/* command format:
 *
 * +-----------------------------------------------------------------+
 * | 3 3 2 2 2 2 2 2 2 2 2 2 1 1 1 1 1 1 1 1 1 1                     |
 * | 1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0 |
 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 * | C PARAM-8      | B PARAM-8     | A PARAM-8     | COMMAND 3 arg  | OPC3
 * +----------------+---------------+---------------+----------------+
 * | BC PARAM-16 (immediate)        | A PARAM-8     | COMMAND        | OPC3
 * +----------------+---------------+---------------+----------------+
 * | B PARAM-12             | A PARAM-12            | COMMAND 2 arg  | OPC2
 * +----------------+---------------+---------------+----------------+
 * | A PARAM-24                                     | COMMAND 1 arg  | OPC1
 * +----------------+---------------+---------------+----------------+
 * | UNUSED                                         | COMMAND 0 arg  | OPC0
 * +----------------+---------------+---------------+----------------+
 *
 */

#define MKMASK(n)	(~((~0)<<(n)))
/* an alternate method */
/*
#define MKMASK(n)	(1<<(n)-1)
*/
#define VM_FPU_STACK_SZ	8	/* fpu stack */

#define VM_CMD_BITS	8	/* number of bits used for commands */
#define VM_OPC1_BITS	24	/* number of bits used for param on OPC1 */
#define VM_OPC2_BITS	12	/* number of bits used for param on OPC2 */
#define VM_OPC3_BITS	8 	/* number of bits used for param on OPC3 */

#define VM_CMD_MASK		MKMASK(VM_CMD_BITS)
#define VM_OPC1_MASK		MKMASK(VM_OPC1_BITS)
#define VM_OPC2_MASK		MKMASK(VM_OPC2_BITS)
#define VM_OPC3_MASK		MKMASK(VM_OPC3_BITS)

/* Opcode - 3 args */
#define VM_OPC3(cmd,a,b,c)	((((((((c)&VM_OPC3_MASK)<<VM_OPC3_BITS)|((b)&VM_OPC3_MASK))<<VM_OPC3_BITS)|((a)&VM_OPC3_MASK))<<VM_CMD_BITS)|((cmd)&255))
/* Opcode - 2 args */
#define VM_OPC2(cmd,a,b)	((((((b)&VM_OPC2_MASK)<<VM_OPC2_BITS)|((a)&VM_OPC2_MASK))<<VM_CMD_BITS)|((cmd)&255))
/* Opcode - 1 arg */
#define VM_OPC1(cmd,a)		((((a)&VM_OPC1_MASK)<<VM_CMD_BITS)|((cmd)&255))
/* Opcode - 0 args */
#define VM_OPC0(cmd)		((cmd)&255)

#define VM_CMD(op)		(vmcommand_t)((op)&VM_CMD_MASK)
#define VM_OPC1_A(op)		((op)>>VM_CMD_BITS)
#define VM_OPC2_A(op)		(((op)>>VM_CMD_BITS)&VM_OPC2_MASK)
#define VM_OPC2_B(op)		(((op)>>(VM_CMD_BITS+VM_OPC2_BITS))&VM_OPC2_MASK)
#define VM_OPC3_A(op)		(((op)>>(VM_CMD_BITS))&VM_OPC3_MASK)
#define VM_OPC3_B(op)		(((op)>>(VM_CMD_BITS+VM_OPC3_BITS))&VM_OPC3_MASK)
#define VM_OPC3_BC(op)		(((op)>>(VM_CMD_BITS+VM_OPC3_BITS))&(MKMASK(VM_OPC3_BITS*2)))
#define VM_OPC3_C(op)		(((op)>>(VM_CMD_BITS+VM_OPC3_BITS*2))&VM_OPC3_MASK)

/* turns the param into a signed int
 * DANGER: assumes 2s complement */
#define VM_OPC1_SIGNED(val)	(-((val)&(1<<(VM_OPC1_BITS-1)))|(val))
#define VM_OPC2_SIGNED(val)	(-((val)&(1<<(VM_OPC2_BITS-1)))|(val))
#define VM_OPC3_SIGNED(val)	(-((val)&(1<<(VM_OPC3_BITS-1)))|(val))

/* indicates the alignment of a vmword_t in the core */
#define VM_ALIGNMENT		(sizeof(vmword_t)/sizeof(vmbyte_t))

#if (VM_CMD_BITS+VM_PARAM_BITS*3) <= 32
typedef uint_least32_t vmword_t;
#define VM_WORD_BITS		32
#else
#error Too many bits defined for opcode, use smaller bits or bigger words.
#endif

typedef uint_least8_t vmbyte_t;

typedef enum vmspr {
	VM_SPR_PC,	/* program counter */
	VM_SPR_RB,	/* register base */
	VM_SPR_LR,	/* link register */
	VM_SPR_EXPST,	/* Exception Status */
	VM_SPR_H_IDIV0,	/* fault handler for integer division by 0 */
	VM_SPR_H_MEM,	/* fault handler for invalid memory access */
	VM_SPR_NR
} vmspr_t;

typedef enum vmcommand {
	VM_CMD_FAULT,	/* fault */
	VM_CMD_DOUT,	/* debug output */
	VM_CMD_STSPR,	/* set SPR value */
	VM_CMD_LDSPR,	/* get SPR value */
	VM_CMD_XSPR,	/* exchange/swap SPR value */
	VM_CMD_NOP,	/* no operation */
	VM_CMD_ADD,	/* add */
	VM_CMD_SUB,	/* subtract */
	VM_CMD_MUL,	/* multiply */
	VM_CMD_DIV,	/* divide */
	VM_CMD_XOR,	/* exclusive-OR */
	VM_CMD_OR,	/* OR */
	VM_CMD_AND,	/* AND */
	VM_CMD_NOT,	/* complement */
	VM_CMD_NEG,	/* negate */
	VM_CMD_SETB,	/* set bit */
	VM_CMD_CLRB,	/* clr bit */
	VM_CMD_TSTB,	/* test bit */
	VM_CMD_INVB,	/* invert bit */
	VM_CMD_BRR,	/* branch relative */
	VM_CMD_BRA,	/* branch absolute */
	VM_CMD_BRI,	/* branch immediate */
	VM_CMD_CBRRI,	/* conditional branch relative immediate */
	VM_CMD_CBRA,	/* conditional branch absolute */
	VM_CMD_BRAC,	/* branch absolute compare (A<B), goto C */
	VM_CMD_BRAZ,	/* branch absolute zero (A=B), goto C */
	VM_CMD_SWAP,	/* swap values */
	VM_CMD_LDIH,	/* load word immediate high */
	VM_CMD_LDIL,	/* load word immediate low */
	VM_CMD_LDZL,	/* load word zero low */
	VM_CMD_LDZH,	/* load word zero high */
	VM_CMD_LDIR,	/* load word immediate relative */
	VM_CMD_LDIA,	/* load word immediate absolute */
	VM_CMD_ST,	/* store word *(A)=B */
	VM_CMD_STR,	/* store relative word *(A+B)=C */
	VM_CMD_STRI,	/* store relative immediate word *(A+Bi)=C */
	VM_CMD_STB,	/* store byte */
	VM_CMD_STBR,	/* store byte relative */
	VM_CMD_LDB,	/* load byte */
	VM_CMD_F_PUSH,	/* float : push */
	VM_CMD_F_POP,	/* float : pop */
	VM_CMD_F_ADD,	/* float : addition */
	VM_CMD_F_SUB,	/* float : subtract */
	VM_CMD_F_DIV,	/* float : divide */
	VM_CMD_F_MUL,	/* float : multiply */
	VM_CMD_F_RECIP,	/* float : reciprocal */
	VM_CMD_F_MOD,	/* float : modulus */
	VM_CMD_F_POW,	/* float : raise to power */
	VM_CMD_F_NEG,	/* float : negate */
	VM_CMD_F_SQRT,	/* float : square root */
	VM_CMD_F_CBRT,	/* float : cube root */
	VM_CMD_F_MIN,	/* float : result is which ever is smaller */
	VM_CMD_F_MAX,	/* float : result is which ever is larger */
	VM_CMD_MAX
} vmcommand_t;

struct vmstate {
	vmword_t spr[VM_SPR_NR];	
	double fpu[VM_FPU_STACK_SZ];
	unsigned fpu_ofs;
	char *name;
	unsigned core_sz;
	vmbyte_t *core;
	vmword_t invalid_address; /* return a pointer to this for an invalid address */
};

static inline int vm_isinvalid(struct vmstate *vs, void *p) {
	return (p==&vs->invalid_address);
}

static inline vmword_t *vm_word(struct vmstate *vs, vmword_t ofs) {
	unsigned addr;
	addr=vs->spr[VM_SPR_RB]+ofs*VM_ALIGNMENT;
	if(addr>=vs->core_sz) return &vs->invalid_address;
	return (vmword_t*)&vs->core[addr];
}

static inline vmhalf_t *vm_half(struct vmstate *vs, vmword_t addr) {
	if(addr>=vs->core_sz) return (vmbyte_t*)&vs->invalid_address;
#if __BYTE_ORDER == __LITTLE_ENDIAN
	return (vmhalf_t*)&vs->core[addr^(VM_ALIGNMENT/2-1)];
#else
	return (vmhalf_t*)&vs->core[addr];
#endif
}

static inline vmbyte_t *vm_byte(struct vmstate *vs, vmword_t addr) {
	if(addr>=vs->core_sz) return (vmbyte_t*)&vs->invalid_address;
#if __BYTE_ORDER == __LITTLE_ENDIAN
	return &vs->core[addr^(VM_ALIGNMENT-1)];
#else
	return &vs->core[addr];
#endif
}

void vm_init(struct vmstate *vs, const char *name, unsigned core_sz) {
	assert(vs!=NULL);
	DEBUG("Creating VM '%s'\n", name);
	vs->name=strdup(name);
	vs->core_sz=core_sz;
	vs->core=calloc(core_sz, 1);
	vs->fpu_ofs=0;
	memset(vs->spr, 0, sizeof vs->spr);
	memset(vs->fpu, 0, sizeof vs->fpu);
}

void vm_dump(struct vmstate *vs, unsigned start, unsigned len) {
	int i;
	if((start+len)>vs->core_sz) {
		len=vs->core_sz-start;
	}
	for(i=0;i<len;i++) {
		if(i%16==0) printf("%06X:", start+i);
		if(i%16==8) printf(":"); else printf(" ");
		printf("%02X", vs->core[i]);
		if(i%16==15) printf("\n");
	}
}

void vm_free(struct vmstate *vs) {
	assert(vs!=NULL);
	DEBUG("Freeing VM '%s'\n", vs->name);
	free(vs->name);
	free(vs->core);
	vs->core=0;
	vs->name=0;
	vs->core_sz=0;
}

void vm_exec(struct vmstate *vs, vmword_t op) {
	vmword_t a, b, c;
/*	DEBUG("%s:executing:0x%08X\n", vs->name, op); */
	switch(VM_CMD(op)) {
		case VM_CMD_NOP: printf("NOP\n"); break;
		case VM_CMD_DOUT:
			a=VM_OPC1_A(op);
			printf("DOUT %u\n", a);
			fprintf(stderr, "DEBUG: (%u) -> %u\n", a, *vm_word(vs, a));
			break;
		case VM_CMD_LDZL: /* Load Immediate Low Zero */
			a=VM_OPC3_A(op);
			b=VM_OPC3_BC(op);
			printf("LDZL %u, %u\n", a, b);
			*vm_word(vs, a)=b;
			break;
		case VM_CMD_LDIL: /* Load Immediate Low */
			a=VM_OPC3_A(op);
			b=VM_OPC3_BC(op);
			printf("LDIL %u, %u\n", a, b);
			*vm_word(vs, a)&=(~0)<<(VM_OPC3_BITS*2);
			*vm_word(vs, a)|=b;
			break;
		case VM_CMD_LDIH: /* Load Immediate High */
			a=VM_OPC3_A(op);
			b=VM_OPC3_BC(op);
			printf("LDIH %u, %u\n", a, b);
			*vm_word(vs, a)&=MKMASK(VM_OPC3_BITS*2);
			*vm_word(vs, a)|=b<<(VM_OPC3_BITS*2);
			break;
		case VM_CMD_LDZH: /* Load Zero High */
			a=VM_OPC3_A(op);
			b=VM_OPC3_BC(op);
			printf("LDZH %u, %u\n", a, b);
			*vm_word(vs, a)=b<<(VM_OPC3_BITS*2);
			break;
		case VM_CMD_ADD: /* Addition */
			a=VM_OPC3_A(op);
			b=VM_OPC3_B(op);
			c=VM_OPC3_C(op);
			printf("ADD %u, %u, %u\n", a, b, c);
			printf("\t= %u + %u\n", *vm_word(vs, b), *vm_word(vs, c));
			*vm_word(vs, a)=*vm_word(vs, b)+*vm_word(vs, c);

			break;
		case VM_CMD_SUB: /* Subtraction */
			printf("SUB %u, %u, %u\n", VM_OPC3_A(op), VM_OPC3_B(op), VM_OPC3_C(op));
			break;
		case VM_CMD_BRR: /* Branch relative */
			printf("BRR %d\n", VM_OPC1_SIGNED(VM_OPC1_A(op)));
			break;
		case VM_CMD_BRA: /* Branch Absolute */
			printf("BRA %u+%d\n", VM_OPC2_B(op), VM_OPC2_SIGNED(VM_OPC2_A(op)));
			break;
		case VM_CMD_BRI: /* Branch [to] Immediate [address] */
			printf("BRI 0x%06X\n", VM_OPC1_A(op));
			break;
		case VM_CMD_CBRRI: /* Conditional Branch Relative Immediate */
			printf("CBRRI c%02X %d\n", VM_OPC2_B(op), VM_OPC2_SIGNED(VM_OPC2_A(op)));
			break;
		case VM_CMD_MAX: /* eliminate warning */
		default:
			DEBUG("%s:unrecognized opcode 0x%08X\n", vs->name, op);
	}
}

/* dumps a list of all the opcodes */
static void dump_ops() {
	int i;
	for(i=0;i<VM_CMD_MAX;i++) {
		switch((vmcommand_t)i) {
			case VM_CMD_NOP: printf("NOP\n"); break;
			case VM_CMD_ADD: printf("ADD\n"); break;
			case VM_CMD_SUB: printf("SUB\n"); break;
			case VM_CMD_BRR: printf("BRR\n"); break;
			case VM_CMD_BRA: printf("BRA\n"); break;
			case VM_CMD_BRI: printf("BRI\n"); break;
			case VM_CMD_MAX: abort();
		}
	}
}

int main() {
	struct vmstate vs;
	vm_init(&vs, "test", 512);

/*
	vm_exec(&vs, VM_OPC2(VM_CMD_BRA, 0x8000, 1));
	vm_exec(&vs, VM_OPC1(VM_CMD_BRI, 0x800000));
	vm_exec(&vs, VM_OPC2(VM_CMD_CBRRI, 0x321, 55));
	vm_exec(&vs, VM_OPC1(VM_CMD_BRR, 0x123456));
	vm_exec(&vs, VM_OPC1(VM_CMD_BRR, 0x404040));
	vm_exec(&vs, VM_OPC1(VM_CMD_BRR, 0x800000));
	vm_exec(&vs, VM_OPC1(VM_CMD_BRR, -55));
*/
	vm_exec(&vs, VM_OPC3(VM_CMD_LDZL, 2, 15, 0));
	vm_exec(&vs, VM_OPC3(VM_CMD_LDZL, 3, 15, 0));
	vm_exec(&vs, VM_OPC3(VM_CMD_LDZL, 4, 15, 0));
	vm_exec(&vs, VM_OPC3(VM_CMD_ADD, 0x02, 0x03, 0x04));
	vm_exec(&vs, VM_OPC1(VM_CMD_DOUT, 2));
	vm_exec(&vs, VM_OPC1(VM_CMD_DOUT, 3));
	vm_exec(&vs, VM_OPC1(VM_CMD_DOUT, 4));

	vm_dump(&vs, 0, 128);

	vm_free(&vs);
	
	/* dump_ops(); */
	return 0;
}