/*
 * BlargCPU2
 *
 * A dummy CPU emulated on a microcode level, suitable for implementation in hardware.
 *
 * Copyright (c) 2005 Joshua Wise <joshua@joshuawise.com>.
 * Copyright (c) 2005 Matthew Maurer <Fallen.Azrael@gmail.com>.
 *
 * All rights reserved.
 */

/*
four registers + flag + pc + stack pointer

Insns:
 0000 mov reg, lit16
 0001 ldr reg, [reg]
 0010 sto [reg], reg
 0011 mov reg, reg
 0100 add reg, reg
 0101 tst reg, reg
 0110 and reg, reg
 0111 not reg
 1000 push reg, reg
 1001 pop reg, reg
 1010 call reg, reg
 1011 shr reg, reg
 1100 shl reg, reg
 
Predicates:
 000 Never
 001 Not-Equal
 010 Equal
 011 Less than
 100 Greater than
 111 Always

Opcode format:
15 14 13 12 11 10 09 08 07 06 05 04 03 02 01 00
|--INSN---| |-PRED-|    |TREGISTER| |SREGISTER|

Vectors:
  0 - start
  4 - ISR
*/

#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>

#define STACK_LOC 0x8000

#define PRED_NV 0x0
#define PRED_NE 0x1
#define PRED_EQ 0x2
#define PRED_LT 0x3
#define PRED_GT 0x4
#define PRED_AL 0x7

#define FLAG_LT 0x1
#define FLAG_GT 0x2
#define FLAG_EQ 0x4

#define REG_R0 0x0
#define REG_R1 0x1
#define REG_R2 0x2
#define REG_R3 0x3
#define REG_FR 0x4
#define REG_PC 0x5
#define REG_SP 0x6

#define INSN_MOV_REG_LIT16 0x0
#define INSN_LDR_REG_ADR_REG 0x1
#define INSN_STO_ADR_REG_REG 0x2
#define INSN_MOV_REG_REG 0x3
#define INSN_ADD_REG_REG 0x4
#define INSN_TST_REG_REG 0x5
#define INSN_AND_REG_REG 0x6
#define INSN_NOT_REG 0x7
#define INSN_PUSH_REG_REG 0x8
#define INSN_POP_REG_REG 0x9
#define INSN_CALL_REG_REG 0xA
#define INSN_SHR_REG_REG 0xB
#define INSN_SHL_REG_REG 0xC

#define INSN(insn, pred, treg, sreg) (((insn) << 12) | ((pred) << 9) | ((treg) << 4) | ((sreg)))

/*******************************************************************/
/*******************************************************************/

struct peripheral {
	struct peripheral *next;
	
	char *name;
	unsigned short start;
	unsigned short length;
	unsigned short (*read)(struct peripheral*, unsigned short);
	void (*write)(struct peripheral*, unsigned short, unsigned short);
	void *priv;
};

struct peripheral *plist = NULL;

unsigned short null_read(struct peripheral *device, unsigned short address)
{
	printf("%s: read to address %04x disallowed\n", device->name, address);
	abort();
}

void null_write(struct peripheral *device, unsigned short address, unsigned short data)
{
	printf("%s: write to address %04x disallowed\n", device->name, address);
	abort();
}

unsigned short nodev_read(struct peripheral *device, unsigned short address)
{
	printf("peripherals: no device to read from at address %04x\n", address);
	abort();
}

void nodev_write(struct peripheral *device, unsigned short address, unsigned short data)
{
	printf("peripherals: no device to write %04x to at address %04x\n", data, address);
	abort();
}

struct peripheral noperiph = { NULL, "noperiph", 0x0, 0x0, nodev_read, nodev_write, NULL };

struct peripheral* peripheral_cache[65536 /* ouch! */];

/* this could be done in a more optimized manner by stepping through each periph and just setting the bits where it's at, but ... */
void peripheral_rehash()
{
	int address;
	
	for (address = 0; address < 65536; address++)
	{
		struct peripheral *p;
		
		peripheral_cache[address] = &noperiph;	/* No'bdy has harmed me!, quoth the Cyclops */
		
		for (p=plist; p; p = p->next)
			if ((p->start <= address) && ((p->start + p->length) > address))
			{
				peripheral_cache[address] = p;
				break;
			}
	}
}

#define peripheral_get(address) (peripheral_cache[address])

void peripheral_add(struct peripheral *device)
{
	device->next = plist;
	plist = device;
	if (!device->read)
		device->read = null_read;
	if (!device->write)
		device->write = null_write;
	peripheral_rehash();
}

void peripheral_remove(struct peripheral *device)
{
	struct peripheral *temp, *ptemp;
	
	if (plist == device)
	{
		plist = device->next;
		return;
	}
	
	ptemp = plist;
	for (temp = plist; temp; temp->next)
		if (temp == device)
			ptemp->next = device->next;
	peripheral_rehash();
}

/*******************************************************************/

#define ROMSIZE 0x4000
unsigned short rom_data[ROMSIZE]= {
	INSN(INSN_MOV_REG_LIT16,    PRED_AL, REG_R3,0       ), 9,           /*mov r3, 9*/           
	INSN(INSN_MOV_REG_LIT16,    PRED_AL, REG_R0,0       ), '9',         /*mov r0, '9'*/         
	INSN(INSN_MOV_REG_LIT16,    PRED_AL, REG_R1,0       ), 0x4000,      /*mov r1, 0x4000*/      
        INSN(INSN_MOV_REG_LIT16,    PRED_AL, REG_R2,0       ), 0xFFFF,      /*mov r2, 0xFFFF*/
	INSN(INSN_STO_ADR_REG_REG,  PRED_AL, REG_R1,REG_R0  ),              /* loop 0x0008 *//*mov [r1], r0*/
	INSN(INSN_ADD_REG_REG,      PRED_AL, REG_R3,REG_R2  ),              /*add r3,r2*/
	INSN(INSN_ADD_REG_REG,      PRED_AL, REG_R0,REG_R2  ),              /*add r0,r2*/
	INSN(INSN_TST_REG_REG,      PRED_AL, REG_R3,REG_R2  ),              /* loop 0x0011 *//*test r3, r2*/
	INSN(INSN_MOV_REG_LIT16,    PRED_NE, REG_PC,0       ), 0x0008,      /*jne 0x0008*/
	INSN(INSN_MOV_REG_LIT16,    PRED_AL, REG_R0,0       ), '\n',        /*mov r0,0*/
	INSN(INSN_STO_ADR_REG_REG,  PRED_AL, REG_R1,REG_R0  ),              /*mov [r1], r0*/
	INSN(INSN_MOV_REG_LIT16,    PRED_AL, REG_PC,0       ), 0x0011,      /*jmp 0x0011*/
};

/*ROM Read*/
unsigned short rom_read(struct peripheral *device, unsigned short address)
{
	return ((unsigned short*)device->priv)[address - device->start];
}

unsigned short rom_optread(struct peripheral *device, unsigned short address)
{
	return ((unsigned short*)device->priv)[address];
}

void rom_add()
{
	struct peripheral *p = (struct peripheral*)malloc(sizeof(struct peripheral));
	p->name = "rom";
	p->start = 0x0;
	p->length = ROMSIZE;
	p->read = (p->start == 0x0) ? &rom_optread : &rom_read;
	p->write = NULL;
	p->priv = &rom_data;	
	peripheral_add(p);
}

/*******************************************************************/

#define RAMSIZE 32768
unsigned short ram_data[RAMSIZE];

unsigned short ram_read(struct peripheral *device, unsigned short address)
{
	return ((unsigned short*)device->priv)[address - device->start];
}

void ram_write(struct peripheral *device, unsigned short address, unsigned short data)
{
	((unsigned short*)device->priv)[address - device->start] = data;
}

void ram_add()
{
	struct peripheral *p = (struct peripheral*)malloc(sizeof(struct peripheral));
	p->name = "ram";
	p->start = 0x8000;
	p->length = RAMSIZE;
	p->read = &ram_read;
	p->write = &ram_write;
	p->priv = &ram_data;
	peripheral_add(p);
}

/*******************************************************************/

#include "SDL.h"

struct blargfb {
  SDL_Surface* screen;
  SDL_Color palette[256];
  unsigned short row, col;
};

Uint32 fb_events(Uint32 interval)
{
	SDL_Event event;
	
	while (SDL_PollEvent(&event)) {
		switch (event.type) {
		case SDL_KEYDOWN:
			if (event.key.keysym.sym == SDLK_q)
				exit(0);
			break;
		case SDL_QUIT:
			exit(0);
		}
	}
	return interval;
}

void fb_initsurface(struct blargfb *fb)
{
	if (SDL_Init(SDL_INIT_VIDEO | SDL_INIT_TIMER | SDL_INIT_NOPARACHUTE) < 0)
	{
        	printf("SDL init failed: %s, FB disabled\n", SDL_GetError());
        	fb->screen = NULL;
        	return;
	}
	atexit(SDL_Quit);
	
	fb->screen = SDL_SetVideoMode(320,240,8,SDL_SWSURFACE);
	if (!fb->screen)
	{
		printf("SDL video init failed: %s, FB disabled\n", SDL_GetError());
		return;
	}
	
	SDL_WM_SetCaption("BlargCPU Framebuffer", "BlargFB");
	
	fb->palette[0].r = 0;	fb->palette[0].g = 0;	fb->palette[0].b = 0;
	fb->palette[1].r = 127;	fb->palette[1].g = 0;	fb->palette[1].b = 0;
	fb->palette[2].r = 0;	fb->palette[2].g = 127;	fb->palette[2].b = 0;
	fb->palette[3].r = 0;	fb->palette[3].g = 0;	fb->palette[3].b = 127;
	fb->palette[4].r = 255;	fb->palette[4].g = 0;	fb->palette[4].b = 0;
	fb->palette[5].r = 0;	fb->palette[5].g = 255;	fb->palette[5].b = 0;
	fb->palette[6].r = 0;	fb->palette[6].g = 0;	fb->palette[6].b = 255;
	fb->palette[7].r = 255;	fb->palette[7].g = 255;	fb->palette[7].b = 255;
	SDL_SetColors(fb->screen, fb->palette, 0, 8);
	
	SDL_SetTimer(50, fb_events);
}
  
void fb_init(struct peripheral *device)
{
	struct blargfb *priv = (struct blargfb*)device->priv;
	fb_initsurface(priv);
	priv->row = 0;
	priv->col = 0;
}

unsigned short fb_read(struct peripheral *device, unsigned short address)
{
	struct blargfb *priv = (struct blargfb*)device->priv;
	unsigned char res;
	
	switch (address - device->start)
	{
	case 0:	return priv->row;
	case 1:	return priv->col;
	case 2:	SDL_LockSurface(priv->screen);
		res = *((unsigned char*)(priv->screen->pixels + priv->row * priv->screen->pitch + priv->col));
		SDL_UnlockSurface(priv->screen);
		return res;
	case 3:	return 0;
	default:printf("%s: read: incorrect offset %d\n", device->name, address - device->start);
	};
}

void fb_write(struct peripheral *device, unsigned short address, unsigned short data)
{
	struct blargfb *priv = (struct blargfb*)device->priv;
	
	switch (address - device->start)
	{
	case 0:	priv->row = data; break;
	case 1:	priv->col = data; break;
	case 2:	SDL_LockSurface(priv->screen);
		*((unsigned char*)(priv->screen->pixels + priv->row * priv->screen->pitch + priv->col)) = data;
 		SDL_UnlockSurface(priv->screen);
 		break;
	case 3:	SDL_UpdateRect(priv->screen, 0, 0, 0, 0); break;
	default:printf("%s: write: incorrect offset %d\n", device->name, address - device->start);
	}
}

void fb_add()
{
	struct peripheral *p = (struct peripheral*)malloc(sizeof(struct peripheral));
	struct blargfb *fb = (struct blargfb*)malloc(sizeof(struct blargfb));
	p->name = "fb";
	p->start = 0x4100;
	p->length = 0x4;
	p->read = &fb_read;
	p->write = &fb_write;
	p->priv = fb;
	fb_init(p);
	peripheral_add(p);
}

/*******************************************************************/

void console_write(struct peripheral *device, unsigned short address, unsigned short data)
{
	write(1, &data, 1);
}

void console_add()
{
	struct peripheral *p = (struct peripheral*)malloc(sizeof(struct peripheral));
	p->name = "console";
	p->start = 0x4000;
	p->length = 0x1;
	p->read = NULL;
	p->write = &console_write;
	p->priv = NULL;
	peripheral_add(p);
}

/*******************************************************************/

unsigned short fetch(unsigned short address)
{
	struct peripheral* temp = peripheral_get(address);
	return temp->read(temp, address);
}

void store(unsigned short address, unsigned short data)
{
	struct peripheral* temp = peripheral_get(address);
	temp->write(temp, address, data);
}

/*******************************************************************/
/*******************************************************************/

unsigned short regs[7];

void reset()
{
  regs[REG_PC] = 0x0;
  regs[REG_FR] = 0x0;
  regs[REG_SP] = STACK_LOC;
}

#if DEBUG
char* insntostr(unsigned short opc)
{
	static char insn[] = "inspr rx,rx";
	switch ((opc >> 12) & 0x7)
	{
	case INSN_MOV_REG_LIT16:	memcpy(insn, "mvc", 3); break;
        case INSN_STO_ADR_REG_REG:	memcpy(insn, "sto", 3); break;
        case INSN_LDR_REG_ADR_REG:	memcpy(insn, "ldr", 3); break;
        case INSN_MOV_REG_REG:		memcpy(insn, "mov", 3); break;
        case INSN_ADD_REG_REG:		memcpy(insn, "add", 3); break;
        case INSN_TST_REG_REG:		memcpy(insn, "tst", 3); break;
        case INSN_AND_REG_REG:		memcpy(insn, "and", 3); break;
        case INSN_NOT_REG:		memcpy(insn, "not", 3); break;
	case INSN_PUSH_REG_REG:		memcpy(insn, "psh", 3); break;
	case INSN_POP_REG_REG:		memcpy(insn, "pop", 3); break;
	case INSN_CALL_REG_REG:		memcpy(insn, "cal", 3); break;
	default:			memcpy(insn, "bad", 3); break;
	}
	switch ((opc >> 9) & 0x7)
        {
        case PRED_NV:	memcpy(insn+3, "nv", 2); break;
        case PRED_NE:	memcpy(insn+3, "ne", 2); break;
        case PRED_EQ:	memcpy(insn+3, "eq", 2); break;
        case PRED_LT:	memcpy(insn+3, "lt", 2); break;
        case PRED_GT:	memcpy(insn+3, "gt", 2); break;
        case PRED_AL:	memcpy(insn+3, "  ", 2); break;
        default:	memcpy(insn+3, "!!", 2); break;
        }
        
        sprintf(insn+6, "r%x,r%x", (opc >> 4) & 0xF, opc & 0xF);
        return insn;
}
#endif

void cpuloop()
{
    while(1)
    {
        unsigned short insn;
        unsigned char pred_match;
        unsigned short tmp;
        insn = fetch(regs[REG_PC]);
        switch ((insn >> 9) & 0x7)
        {
        case PRED_NV:	pred_match = 0;	break;
        case PRED_NE:	pred_match = regs[REG_FR] & (FLAG_LT | FLAG_GT); break;
        case PRED_EQ:	pred_match = regs[REG_FR] & FLAG_EQ; break;
        case PRED_LT:	pred_match = regs[REG_FR] & FLAG_LT; break;
        case PRED_GT:	pred_match = regs[REG_FR] & FLAG_GT; break;
        case PRED_AL:	pred_match = 1; break;
        default:	printf("Invalid predicate %1x, aborting.\n", (insn >> 9) & 0x7);
                        abort();
        }
        
#ifdef DEBUG
        printf("r0:%04x r1:%04x r2:%04x r3:%04x fr:%04x pc:%04x sp:%04x -- %04x[%s]\n",
          regs[0], regs[1], regs[2], regs[3], regs[4], regs[5], regs[6],
          insn, insntostr(insn));
#endif
  
#define UCODE_NEXT_INSN		break;
#define UCODE_INC_PC		regs[REG_PC]++;
#define UCODE_INC_LATCH		tmp++;
#define UCODE_DEC_LATCH		tmp--;
#define UCODE_LATCH_ADR_PC	tmp = fetch(regs[REG_PC]);
#define UCODE_LATCH_PC		tmp = regs[REG_PC];

#define UCODE_STORE_TREG	regs[(insn >> 4) & 0xF] = tmp;
#define UCODE_LATCH_TREG	tmp = regs[(insn >> 4) & 0xF];
#define UCODE_LATCH_ADR_TREG	tmp = fetch(regs[(insn >> 4) & 0xF]);

#define UCODE_STORE_SREG	regs[insn & 0xF] = tmp;
#define UCODE_LATCH_SREG	tmp = regs[insn & 0xF];
#define UCODE_LATCH_ADR_SREG	tmp = fetch(regs[insn & 0xF]);

#define UCODE_STORE_ADR_TREG	store(regs[(insn >> 4) & 0xF], tmp);
#define UCODE_STORE_PC		regs[REG_PC] = tmp;
#define UCODE_ADD_TREG		tmp += regs[(insn >> 4) & 0xF];
#define UCODE_ZERO_FR		regs[REG_FR] = 0;
#define UCODE_SET_FR_EQ		if (tmp == regs[(insn >> 4) & 0xF]) regs[REG_FR] |= FLAG_EQ;
#define UCODE_SET_FR_LT		if (tmp <  regs[(insn >> 4) & 0xF]) regs[REG_FR] |= FLAG_LT;
#define UCODE_SET_FR_GT		if (tmp  > regs[(insn >> 4) & 0xF]) regs[REG_FR] |= FLAG_GT;
#define UCODE_AND_TREG		tmp &= regs[(insn >> 4) & 0xF];
#define UCODE_LATCH_NOT_TREG	tmp = ~regs[(insn >> 4) & 0xF];

#define UCODE_SHIFT_RIGHT	tmp = tmp >> (regs[(insn >> 4) & 0xF]);
#define UCODE_SHIFT_LEFT	tmp = tmp << (regs[(insn << 4) & 0xF]);	

        switch ((insn >> 12) & 0xF)
        {
        case INSN_MOV_REG_LIT16:		    UCODE_INC_PC
                                    if (pred_match) UCODE_LATCH_ADR_PC
                                                    UCODE_INC_PC
                                    if (pred_match) UCODE_STORE_TREG
                                                    UCODE_NEXT_INSN
                                                    
        case INSN_STO_ADR_REG_REG:  if (pred_match) UCODE_LATCH_SREG
                                                    UCODE_INC_PC
                                    if (pred_match) UCODE_STORE_ADR_TREG
                                                    UCODE_NEXT_INSN
                                                    
        case INSN_LDR_REG_ADR_REG:  if (pred_match) UCODE_LATCH_ADR_SREG
                                                    UCODE_INC_PC
                                    if (pred_match) UCODE_STORE_TREG
                                                    UCODE_NEXT_INSN
                                                    
        case INSN_MOV_REG_REG:	    if (pred_match) UCODE_LATCH_SREG
                                                    UCODE_INC_PC
                                    if (pred_match) UCODE_STORE_TREG
                                                    UCODE_NEXT_INSN
                                                    
        case INSN_ADD_REG_REG:	    if (pred_match) UCODE_LATCH_SREG
                                                    UCODE_INC_PC
                                    if (pred_match) UCODE_ADD_TREG
                                    if (pred_match) UCODE_STORE_TREG
                                                    UCODE_NEXT_INSN
                                                    
        case INSN_TST_REG_REG:	    if (pred_match) UCODE_LATCH_SREG
                                                    UCODE_INC_PC
                                    if (pred_match) UCODE_ZERO_FR
                                    if (pred_match) UCODE_SET_FR_EQ
                                    if (pred_match) UCODE_SET_FR_LT
                                    if (pred_match) UCODE_SET_FR_GT
                                                    UCODE_NEXT_INSN
        
        case INSN_AND_REG_REG:	    if (pred_match) UCODE_LATCH_SREG
                                                    UCODE_INC_PC
                                    if (pred_match) UCODE_AND_TREG
                                    if (pred_match) UCODE_STORE_TREG
                                                    UCODE_NEXT_INSN
                                                    
        case INSN_NOT_REG:	    if (pred_match) UCODE_LATCH_NOT_TREG
                                                    UCODE_INC_PC
				    if (pred_match) UCODE_STORE_TREG
				        	    UCODE_NEXT_INSN
                                                    
	case INSN_PUSH_REG_REG:	    if (pred_match) UCODE_LATCH_SREG
				    if (pred_match) UCODE_STORE_ADR_TREG
				    if (pred_match) UCODE_LATCH_TREG
				    if (pred_match) UCODE_INC_LATCH
				    if (pred_match) UCODE_STORE_TREG
					    	    UCODE_INC_PC
						    UCODE_NEXT_INSN
							    
	case INSN_POP_REG_REG:			    UCODE_INC_PC
				    if (pred_match) UCODE_LATCH_TREG
				    if (pred_match) UCODE_DEC_LATCH
				    if (pred_match) UCODE_STORE_TREG
				    if (pred_match) UCODE_LATCH_ADR_TREG
				    if (pred_match) UCODE_STORE_SREG
					   	    UCODE_NEXT_INSN
	
	case INSN_CALL_REG_REG:	    		    UCODE_INC_PC
				    if (pred_match) UCODE_LATCH_PC
				    if (pred_match) UCODE_STORE_ADR_TREG
				    if (pred_match) UCODE_LATCH_TREG
				    if (pred_match) UCODE_INC_LATCH
				    if (pred_match) UCODE_STORE_TREG
				    if (pred_match) UCODE_LATCH_SREG
				    if (pred_match) UCODE_STORE_PC
					    	    UCODE_NEXT_INSN
	
	case INSN_SHR_REG_REG:	    if (pred_match) UCODE_LATCH_SREG
						    UCODE_INC_PC
				    if (pred_match) UCODE_SHIFT_RIGHT
				    if (pred_match) UCODE_STORE_SREG
						    UCODE_NEXT_INSN

	case INSN_SHL_REG_REG:	    if (pred_match) UCODE_LATCH_SREG
						    UCODE_INC_PC
				    if (pred_match) UCODE_SHIFT_LEFT
				    if (pred_match) UCODE_STORE_SREG
						    UCODE_NEXT_INSN
/*
* Two concerns as of right now about this microcode:
* 1.) We latch to some things, but then access the registers directly in others. As far as I can tell, we need a second latch.
* 2.) We increment the program counter before storing to tregs and the like sometimes, but don't others. For example, in
* call, we increment, then proceed to store to the address in treg. However, at other times, such as in tst, we latch to a
* register, first, almost giving the impression that we couldn't afterwards. I'm not sure which way it is, but it should
* be one way throughout the entire system.
* --Matthew Maurer
*/ 
	default:		    printf("Internal emulation error: Out of range opcode\n");
                                    abort();
        }
    }
}

/*******************************************************************/
/*******************************************************************/

int main(int argc, char** argv)
{
  if (argc == 2)
  {
    int fd;
    fd = open(argv[1], O_RDONLY);
    if (fd < 0)
    {
      perror("open");
      exit(0);
    }
    read(fd, rom_data, ROMSIZE*sizeof(short));
    close(fd);
  }
  rom_add();
  ram_add();
  console_add();
  fb_add();
  reset();
  cpuloop();
  return 0;
}