cpu: get blarg CPU instruction test 6 passing

Its been too long since a checkin, but here's some of the
improvements:

- Support for diffing with other emulators
- Better disassmbed output
- New CPU instructions implemented
- Lots of CPU fixes

src/apps/gbdb.c

@@ -29,6 +29,7 @@ typedef void (gbdb_cmd)(struct tokens **tokens);
 static void breakpoint_addr_hit(uint16_t id);
 static void breakpoint_cb(struct lr35902_state *lr);
 static void reset_breakpoints(void);
+static int64_t parse_val(const char *str);
 
 static const char prompt[] = "gbdb >";
 static const char usage[] =
@@ -38,7 +39,7 @@ static const char usage[] =
 	"break <addr>: Adds a breakpoint for the given addess\n"
 	"step <cycles>: executes \"cycle\" instructions (default 1)\n"
 	"regs: dumps the state of the registers\n"
-	"peek: view the next instruction to run\n"
+	"disas: view the next instruction to run\n"
 	"exit: quit the program\n";
 
 static struct lr35902_ops cpu_ops;
@@ -53,6 +54,7 @@ static volatile sig_atomic_t paused = 0;
 static struct {
 	bool quiet;
 	int log_fd;
+	int trace_fd;
 } config;
 
 void gb_log(const char *fmt, ...)
@@ -112,12 +114,34 @@ static void init(void)
 	gb_mem_init(memory, &video);
 
 	config.log_fd = -1;
+	config.trace_fd = -1;
+}
+
+static void print_trace(void)
+{
+	if (config.trace_fd > -1) {
+		uint8_t instr = gb_mem_read(memory, cpu.pc);
+
+		dprintf(config.trace_fd,
+				"A:%02X F:%c%c%c%c BC:%04X DE:%04x HL:%04x SP:%04x PC:%04x 0x%02x",
+				cpu.a,
+				(cpu.zf) ? 'Z' : '-', (cpu.nf) ? 'N' : '-',
+				(cpu.hf) ? 'H' : '-', (cpu.cf) ? 'C' : '-',
+				cpu.bc, cpu.de, cpu.hl, cpu.sp, cpu.pc, instr);
+
+		if (instr == 0xcb) {
+			dprintf(config.trace_fd, " 0x%02x", gb_mem_read(memory, cpu.pc + 1));
+		}
+
+		dprintf(config.trace_fd, "\n");
+	}
 }
 
 static void cycle()
 {
 	int cycles;
 
+	print_trace();
 	cycles = lr35902_cycle(&cpu);
 	gb_video_cycle(&video, cycles);
 }
@@ -159,9 +183,6 @@ static void set_logfile(struct tokens **tokens)
 	char *token = token_next(tokens);
 	int fd;
 
-	if (token != NULL) {
-		strip_newline(token);
-	}
 
 	if (token == NULL || strcmp(token, "") == 0) {
 		if (config.log_fd != -1) {
@@ -186,6 +207,36 @@ static void set_logfile(struct tokens **tokens)
 	config.log_fd = fd;
 }
 
+static void set_tracefile(struct tokens **tokens)
+{
+	char *token = token_next(tokens);
+	int fd;
+
+
+	if (token == NULL || strcmp(token, "") == 0) {
+		if (config.trace_fd >= -1) {
+			close(config.trace_fd);
+		}
+		config.trace_fd = -1;
+		return;
+	}
+
+	fd = open(token,
+			  O_WRONLY | O_TRUNC | O_CREAT,
+			  S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
+	if (fd < 0) {
+		gb_error("Failed to open tracefile %s, %s\n", token, strerror(errno));
+		return;
+	}
+
+	if (config.trace_fd != -1) {
+		close(config.trace_fd);
+	}
+
+	config.trace_fd = fd;
+	gb_log("Trace fd=%d\n", config.trace_fd);
+}
+
 static void set_quiet_on(struct tokens **tokens)
 {
 	config.quiet = true;
@@ -227,6 +278,7 @@ static void set(struct tokens **tokens)
 		tri_init(&commands);
 		tri_add_string(&commands, "quiet", set_quiet);
 		tri_add_string(&commands, "logfile",  set_logfile);
+		tri_add_string(&commands, "trace",  set_tracefile);
 		tri_add_string(&commands, "video_debug",  set_video_debug);
 		init = true;
 	}
@@ -259,11 +311,11 @@ static void step(struct tokens **tokens)
 {
 	uint64_t steps, end_steps;
 	char *token;
+
 	token = token_next(tokens);
 	if (token == NULL) {
 		steps = 1;
 	} else {
-		strip_newline(token);
 		steps = strtol(token, NULL, 0);
 	}
 
@@ -305,16 +357,42 @@ static void regs(struct tokens **tokens)
 	gb_log("SP: 0x%04x\n", cpu.sp);
 }
 
-static void peek(struct tokens **tokens)
+static uint8_t decode_fetch_mem(void *opaque, uint16_t addr)
+{
+	return gb_mem_read(memory, addr);
+}
+
+static void disas(struct tokens **tokens)
 {
 	uint16_t pc = lr35902_get_reg_16(&cpu, LR35902_REG_PC);
-	uint8_t byte = gb_mem_read(memory, pc);
+	uint16_t old_pc = pc;
-	gb_log("0x%04x:%s\n", pc, gb_byte_to_opcode(byte));
+	const char *disas_cnt_str;
+	int64_t num_disas, i;
+
+
+	if (tokens) {
+		disas_cnt_str = token_next(tokens);
+		if (disas_cnt_str == NULL) {
+			num_disas = 1;
+		} else {
+			num_disas = parse_val(disas_cnt_str);
+		}
+	} else {
+		num_disas = 1;
+	}
+
+	for (i = 0; i < num_disas; i++) {
+		const char *opcode;
+		old_pc = pc;
+		opcode = gb_byte_to_opcode(&pc, decode_fetch_mem, &memory);
+		gb_log("0x%04x:%s\n", old_pc, opcode);
+		free((void *) opcode);
+	}
 }
 
 static void vstep(struct tokens **tokens)
 {
-	peek(NULL);
+	disas(NULL);
 	step(tokens);
 	regs(NULL);
 }
@@ -359,7 +437,7 @@ static int64_t parse_reg_str(const char *str)
 		{ "c", cpu.c },
 		{ "d", cpu.d },
 		{ "e", cpu.e },
-		{ "f", (cpu.zf << 3) | (cpu.nf << 2) | (cpu.hf << 1) | (cpu.cf << 0) },
+		{ "f", f },
 	};
 
 
@@ -387,7 +465,7 @@ static void assert(struct tokens **tokens)
 	const char *usage = "usage: assert <value> <operator> <value>\n";
 	int64_t val0, val1;
 
-	val0_str = token_next(tokens);
+	val0_str = strdup(token_next(tokens));
 	if (val0_str == NULL) {
 		gb_error("%s", usage);
 		return;
@@ -400,7 +478,7 @@ static void assert(struct tokens **tokens)
 		return;
 	}
 
-	val1_str = token_next(tokens);
+	val1_str = strdup(token_next(tokens));
 	if (val1_str == NULL) {
 		gb_error("%s", usage);
 		return;
@@ -438,7 +516,7 @@ static void assert(struct tokens **tokens)
 	return;
 fail:
 	gb_error("ASSERT: %s %s %s\n", val0_str, operator_str, val1_str);
-	gb_error("%s=%" PRId64 ", %s=%" PRId64 "\n", val0_str, val0, val1_str, val1);
+	gb_error("%s=0x%" PRIx64 ", %s=0x%" PRIx64 "\n", val0_str, val0, val1_str, val1);
 	free(val0_str);
 	free(val1_str);
 	free(operator_str);
@@ -449,10 +527,9 @@ fail:
 
 static void mem(struct tokens **tokens)
 {
-	uint16_t addr, bytes, i;
+	uint16_t addr, bytes, addr_end;
 	char *token = token_next(tokens);
-
+	int i;
-	gb_log("mem_addr=%s\n", token);
 
 	if (token == NULL) {
 		gb_error("usage: mem <addr> (num bytes) (format)\n");
@@ -461,29 +538,16 @@ static void mem(struct tokens **tokens)
 
 	addr = parse_val(token);
 	token = token_next(tokens);
-	gb_log("mem_bytes=%s\n", token);
 	if (token == NULL) {
 		bytes = 1;
 	} else {
 		bytes = parse_val(token);
 	}
 
-	token = token_next(tokens);
-	gb_log("mem_format=%s\n", token);
-
 	for (i = 0; i < bytes; i++) {
 		//TODO: Make sure this has no side effects
-		uint8_t val = gb_mem_read(memory, addr + i);
+		gb_log("0x%04x: 0x%02x\n", addr + i, gb_mem_read(memory, addr + i));
-
-		if (token != NULL && token[0] == 'i') {
-			gb_log("0x%04x:%s\n", addr + i, gb_byte_to_opcode(val));
-		} else if (token != NULL && token[0] == 'b') {
-			gb_log("0x%04x:0x%02x %s\n", addr + i, val, gb_byte_to_opcode(val));
-		} else {
-			gb_log("0x%04x:0x%02x\n", addr + i, val);
-		}
 	}
-
 }
 
 static void load(struct tokens **tokens)
@@ -749,6 +813,9 @@ static void process_cmd(const char *cmd_str, struct tri *commands)
 		}
 
 		call_next_cmd(&tokens, commands, "");
+
+		/* Ensure there are no leftover tokens to leak */
+		do {} while (token_next(&tokens));
 	}
 
 }
@@ -795,7 +862,7 @@ int main(int argc, char **argv)
 	tri_add_string(&commands, "mem", mem);
 	/* FIXME */
 	/* tri_add_string(&commands, "dump", mem_dump); */
-	tri_add_string(&commands, "peek", peek);
+	tri_add_string(&commands, "disassemble", disas);
 	tri_add_string(&commands, "help", help);
 	tri_add_string(&commands, "?", help);
 	tri_add_string(&commands, "breakpoint", breakpoint);
@@ -839,5 +906,9 @@ int main(int argc, char **argv)
 		close(config.log_fd);
 	}
 
+	if (config.trace_fd != -1) {
+		close(config.log_fd);
+	}
+
 	return 0;
 }

src/apps/test-tokenize.c

@@ -0,0 +1,55 @@
+#include "common/common.h"
+#include <string.h>
+
+
+static void test_basic(void)
+{
+	struct tokens *tokens;
+	const char *str = "abcd xyz";
+	const char *token;
+
+	tokens = tokenize(str, " ");
+	ASSERT(tokens);
+
+	token = token_next(&tokens);
+	ASSERT(strcmp(token, "abcd") == 0);
+	ASSERT(tokens);
+
+	token = token_next(&tokens);
+	ASSERT(strcmp(token, "xyz") == 0);
+	ASSERT(tokens);
+
+	token = token_next(&tokens);
+	ASSERT(token == NULL);
+	ASSERT(tokens == NULL);
+}
+
+static void test_delims(void)
+{
+	struct tokens *tokens;
+	const char *str = "  abcd \t  xyz  ";
+	const char *token;
+
+	tokens = tokenize(str, "\t ");
+	ASSERT(tokens);
+
+	token = token_next(&tokens);
+	ASSERT(strcmp(token, "abcd") == 0);
+	ASSERT(tokens);
+
+	token = token_next(&tokens);
+	ASSERT(strcmp(token, "xyz") == 0);
+	ASSERT(tokens);
+
+	token = token_next(&tokens);
+	ASSERT(token == NULL);
+	ASSERT(tokens == NULL);
+}
+
+int main(int argc, const char** argv)
+{
+	test_basic();
+	test_delims();
+
+	return 0;
+}

src/common/common.h

@@ -37,7 +37,7 @@ void gb_error(const char *fmt, ...);
 		gb_error("Assert at: %s:%d <%s> : %s\n", \
 				 __FILE__, __LINE__,			 \
 				 __func__, #x);					 \
-		exit(1); \
+		abort(); \
 	}
 
 #define ASSERT_MSG(x,...)	  \
@@ -46,7 +46,7 @@ void gb_error(const char *fmt, ...);
 				 __FILE__, __LINE__,				\
 				 __func__, #x);						\
 		printf(__VA_ARGS__); \
-		exit(1); \
+		abort(); \
 	}
 
 #define DEBUG_LOG(...)	  \

src/gb_disas.c

@@ -1,6 +1,11 @@
 /*TODO: This is hacky and has some bugs but whatever*/
 
+#include <stdint.h>
+#include <string.h>
 #include <stdio.h>
+#include <stdlib.h>
+
+#include "gb_disas.h"
 
 static const char *opcodes[256] = {
 	//0
@@ -244,7 +249,7 @@ static const char *opcodes[256] = {
 	//14
 	"LDH (a8),A",
 	"POP HL",
-	"LD (C),A",
+	"LD (0xff00 + C),A",
 	"*UNDEF*",
 	"*UNDEF*",
 	"PUSH HL",
@@ -261,7 +266,7 @@ static const char *opcodes[256] = {
 	//15
 	"LDH A,(a8)",
 	"POP AF",
-	"LD A,(C)",
+	"LD A,(0xff00 + C)",
 	"DI",
 	"*UNDEF*",
 	"PUSH AF",
@@ -277,6 +282,151 @@ static const char *opcodes[256] = {
 	"RST 38H"
 };
 
-const char * gb_byte_to_opcode(unsigned char byte) {
+static const char *ioreg[] = {
-	return opcodes[(int) byte];
+	[0] = "P1",
+	[1] = "SB",
+	[2] = "SC",
+	[4] = "DIV",
+	[5] = "TIMA",
+	[6] = "TMA",
+	[7] = "TAC",
+	[0x0f] = "IF",
+	[0x10] = "NR 10",
+	[0x11] = "NR 11",
+	[0x12] = "NR 12",
+	[0x13] = "NR 13",
+	[0x14] = "NR 14",
+	[0x16] = "NR 21",
+	[0x17] = "NR 22",
+	[0x18] = "NR 23",
+	[0x19] = "NR 24",
+	[0x1A] = "NR 30",
+	[0x1B] = "NR 31",
+	[0x1C] = "NR 32",
+	[0x1D] = "NR 33",
+	[0x1E] = "NR 34",
+	[0x20] = "NR 41",
+	[0x21] = "NR 42",
+	[0x22] = "NR 43",
+	[0x23] = "NR 44",
+	[0x24] = "NR 50",
+	[0x25] = "NR 51",
+	[0x26] = "NR 52",
+	[0x30] = "WavePatternRam[0x0]",
+	[0x31] = "WavePatternRam[0x1]",
+	[0x32] = "WavePatternRam[0x2]",
+	[0x33] = "WavePatternRam[0x3]",
+	[0x34] = "WavePatternRam[0x4]",
+	[0x35] = "WavePatternRam[0x5]",
+	[0x36] = "WavePatternRam[0x6]",
+	[0x37] = "WavePatternRam[0x7]",
+	[0x38] = "WavePatternRam[0x8]",
+	[0x39] = "WavePatternRam[0x9]",
+	[0x3A] = "WavePatternRam[0xA]",
+	[0x3B] = "WavePatternRam[0xB]",
+	[0x3C] = "WavePatternRam[0xC]",
+	[0x3D] = "WavePatternRam[0xD]",
+	[0x3E] = "WavePatternRam[0xE]",
+	[0x3F] = "WavePatternRam[0xF]",
+	[0x40] = "LCDC",
+	[0x41] = "STAT",
+	[0x42] = "SCY",
+	[0x43] = "SCX",
+	[0x44] = "LC",
+	[0x45] = "LYC",
+	[0x46] = "DMA",
+	[0x47] = "BGP",
+	[0x48] = "OBP0",
+	[0x49] = "OBP1",
+	[0x4A] = "WY",
+	[0x4B] = "WX",
+	[0xFF] = "IE",
+};
+
+
+static const char *ioreg_str(uint16_t addr)
+{
+	unsigned int offset;
+
+	if (addr > 0xff00) {
+		offset = addr & 0xff;
+		if (ioreg[offset] != NULL) {
+			return ioreg[offset];
+		}
+	}
+
+	return NULL;
+}
+
+const char *gb_byte_to_opcode(uint16_t *addr, gb_byte_fetch_fn fetch, void *opaque)
+{
+	char *buf = malloc(32);
+	uint8_t inst = fetch(opaque, (*addr)++);
+	const char *immediate;
+	uint8_t val;
+	size_t len;
+	const char *opcode = opcodes[inst];
+
+	/* TODO: DRY this code up a bit */
+
+	if (inst == 0xcb) {
+		/* CB-prefix instruction */
+		/* TODO: Right now, this does almost nothing */
+		strncpy(buf, opcode, 32);
+		(*addr)++;
+
+	} else if ((immediate = strstr(opcode, "d8")) != NULL) {
+		uint8_t val = fetch(opaque, (*addr)++);
+		len = immediate - opcode;
+		strncpy(buf, opcode, len);
+		snprintf(buf + len, 32 - len, "0x%02x", val);
+		strncat(buf, opcode + len + strlen("d8"), 32);
+
+	} else if ((immediate = strstr(opcode, "a8")) != NULL) {
+		uint16_t val = ((uint16_t)fetch(opaque, (*addr)++)) + 0xff00;
+		len = immediate - opcode;
+		strncpy(buf, opcode, len);
+		if (ioreg_str(val)) {
+			snprintf(buf + len, 32 - len, "0x%04x=%s", val, ioreg_str(val));
+		} else {
+			snprintf(buf + len, 32 - len, "0x%04x", val);
+		}
+		strncat(buf, opcode + len + strlen("a8"), 32);
+
+	} else if ((immediate = strstr(opcode, "r8")) != NULL) {
+		int16_t val = fetch(opaque, (*addr)++);
+		if (val & (1 << 7)) {
+			val |= 0xFF00;
+		}
+		len = immediate - opcode;
+		strncpy(buf, opcode, len);
+		snprintf(buf + len, 32 - len, "0x%04x", *addr + val);
+		strncat(buf, opcode + len + strlen("r8"), 32);
+
+	} else if ((immediate = strstr(opcode, "d16")) != NULL) {
+		uint16_t val = fetch(opaque, (*addr)++);
+		val |= ((uint16_t) fetch(opaque, (*addr)++)) << 8;
+		len = immediate - opcode;
+		strncpy(buf, opcode, len);
+		if (ioreg_str(val)) {
+			snprintf(buf + len, 32 - len, "0x%04x=%s", val, ioreg_str(val));
+		} else {
+			snprintf(buf + len, 32 - len, "0x%04x", val);
+		}
+		strncat(buf, opcode + len + strlen("a16"), 32);
+
+	} else if ((immediate = strstr(opcode, "a16")) != NULL) {
+		uint16_t val = fetch(opaque, (*addr)++);
+		val |= ((uint16_t) fetch(opaque, (*addr)++)) << 8;
+		len = immediate - opcode;
+		strncpy(buf, opcode, len);
+		snprintf(buf + len, 32 - len, "0x%04x", val);
+		strncat(buf, opcode + len + strlen("a16"), 32);
+
+	} else {
+		strncpy(buf, opcode, 32);
+	}
+
+
+	return buf;
 }

src/gb_disas.h

@@ -2,7 +2,9 @@
 #ifndef _GB_DISAS_H_H
 #define _GB_DISAS_H_H
 
-const char *gb_byte_to_opcode(unsigned char byte);
+typedef uint8_t (* gb_byte_fetch_fn)(void *opaque, uint16_t addr);
+
+const char *gb_byte_to_opcode(uint16_t *addr, gb_byte_fetch_fn fetch, void *opaque);
 
 /* int main(int argc, char **argv) */
 /* { */

src/gbemu/cpu.c

@@ -13,8 +13,11 @@
 #define SET_BIT(x, idx) do {(x) |= (1 << (idx));} while (0)
 #define CLR_BIT(x, idx) do {(x) &= ~(1 << (idx));} while (0)
 #define GET_BIT(x, bit) (((x) >> (bit)) & 1)
-#define WRITE_BIT(x, idx, bit)									\
+#define WRITE_BIT(x, idx, bit)					\
-	do {(x) &= (~(1 << (idx)) | ((bit) << (idx)));} while(0)
+	do {										\
+		(x) &= ~(1 << (idx));					\
+		(x) |= (bit) << (idx);					\
+	} while(0)
 
 #define CALC_H_ADD(a, b) ((((a) & 0xf) + ((b) & 0xf)) > 0xf)
 #define CALC_H_SUB(a, b) (((a) & 0xf) < ((b) & 0xf))
@@ -163,25 +166,25 @@ static const unsigned int cb_extra_cycles[256] = { 0 };
 		(cpu)->zf = (dst) == 0;						\
 	} while (0)
 
-#define ADD_16(cpu, dst, src)						\
+#define ADD_16(cpu, dst, src)							\
-	do {											\
+	do {												\
-		uint8_t _reg = src;							\
+		uint16_t _src = src;							\
-		(cpu)->nf = 0;								\
+		(cpu)->nf = 0;									\
-		(cpu)->hf = CALC_H_ADD((dst), (_reg));		\
+		(cpu)->hf = (dst & 0xfff) + (_src & 0xfff) > 0xfff;	\
-		(cpu)->cf = CALC_C_ADD_16((dst), (_reg));	\
+		(cpu)->cf = CALC_C_ADD_16(dst, _src);			\
-		(dst) += (_reg);							\
+		(dst) = _src + dst;								\
-	} while (0)										\
+	} while (0)											\
 
-#define ADC_8(cpu, dst, src)					\
+#define ADC_8(cpu, dst, src)									\
-	do {										\
+	do {														\
-		/* FIXME flag generation */				\
+		uint16_t _tmp;											\
-		int tmp;								\
+		uint16_t _reg = src;										\
-		uint8_t _reg = src;						\
+		_tmp = _reg + dst + cpu->cf;							\
-		tmp = _reg + dst + cpu->cf;				\
+		cpu->nf = 0;											\
-		cpu->nf = 0;							\
+		cpu->hf = _reg + dst + cpu->cf > 0xff;					\
-		cpu->hf = CALC_H_ADD((_reg), (dst));	\
+		cpu->cf = !(_tmp & 0xff);								\
-		(dst) = tmp;							\
+		(dst) = _tmp;											\
-		cpu->zf = (dst) == 0;					\
+		cpu->zf = !(dst);										\
 	} while (0)
 
 #define SUB_8(cpu, dst, src)					\
@@ -189,22 +192,23 @@ static const unsigned int cb_extra_cycles[256] = { 0 };
 		uint8_t _reg = src;						\
 		int tmp;								\
 		tmp = dst - _reg;						\
-		cpu->nf = 1;							\
+		(cpu)->cf = _reg > dst;					\
-		cpu->hf = CALC_H_SUB(_reg, dst);		\
+		(cpu)->nf = 1;							\
+		(cpu)->hf = CALC_H_SUB(dst, _reg);		\
+		(cpu)->zf = tmp == 0;					\
 		dst = tmp;								\
-		cpu->zf = (dst) == 0;					\
 	} while (0)
 
 #define SBC_8(cpu, dst, src)					\
 	do {										\
-		/* FIXME flag generation */				\
 		int tmp;								\
 		uint8_t _reg = src;						\
-		tmp = (dst) - (_reg) - 1;				\
+		tmp = (dst) - (_reg) - (cpu->cf);		\
+		(cpu)->cf = _reg + (cpu->cf) > dst;		\
 		(cpu)->nf = 1;							\
 		(cpu)->hf = CALC_H_ADD(_reg, dst);		\
+		(cpu)->zf = tmp == 0;					\
 		(dst) = (tmp);							\
-		cpu->zf = (dst) == 0;					\
 	} while (0)
 
 #define AND_8(cpu, dst, src)					\
@@ -257,42 +261,39 @@ static const unsigned int cb_extra_cycles[256] = { 0 };
 		cpu->pc += val_16;								\
 	} while (0)
 
-/* TODO: In general, check that we pop/push things correctly */
+
 #define POP_16(cpu, dst)												\
 	do {																\
-		(dst) = (uint16_t) gb_mem_read((cpu)->memory, (cpu)->sp++) << 8; \
+		(dst) =  gb_mem_read((cpu)->memory, (cpu)->sp++);				\
-		(dst) |= gb_mem_read((cpu)->memory, (cpu)->sp++);				\
+		(dst) |= (uint16_t) gb_mem_read((cpu)->memory, (cpu)->sp++) << 8; \
 	} while (0)
 
-#define PUSH_16(cpu, src)							\
+#define PUSH_16(cpu, src)								\
-	do {											\
+	do {												\
-		gb_mem_write(cpu->memory, --cpu->sp, src);		\
 		gb_mem_write(cpu->memory, --cpu->sp, src >> 8);	\
+		gb_mem_write(cpu->memory, --cpu->sp, src);		\
 	} while (0)
 
-#define CALL(cpu)												\
+#define CALL(cpu)														\
-	do {														\
-		uint16_t pc_base;										\
-		gb_mem_write(cpu->memory, --cpu->sp, cpu->pc);				\
-		gb_mem_write(cpu->memory, --cpu->sp, cpu->pc >> 8);			\
-		pc_base = gb_mem_read(cpu->memory, cpu->pc++);				\
-		pc_base |= (uint16_t) gb_mem_read(cpu->memory, cpu->pc) << 8; \
-		cpu->pc = pc_base;										\
-	} while (0)
-
-#define RET(cpu)														\
 	do {																\
-		(cpu)->pc = 0;													\
+		uint16_t pc_base;												\
-		(cpu)->pc |= (uint16_t) gb_mem_read((cpu)->memory, (cpu)->sp++) << 8; \
+		uint16_t old_pc = cpu->pc;										\
-		(cpu)->pc |= gb_mem_read((cpu)->memory, (cpu)->sp++);				\
+		pc_base = gb_mem_read(cpu->memory, cpu->pc++);					\
-		(cpu)->pc += 2;													\
+		pc_base |= (uint16_t) gb_mem_read(cpu->memory, cpu->pc++) << 8;	\
+		PUSH_16(cpu, cpu->pc);											\
+		cpu->pc = pc_base;												\
 	} while (0)
 
-#define RST(cpu, n)												\
+#define RET(cpu)													\
-	do {														\
+	do {															\
-		gb_mem_write((cpu)->memory, (cpu)->sp++, (cpu)->pc >> 8);	\
+		uint16_t old_pc = cpu->pc;									\
-		gb_mem_write((cpu)->memory, (cpu)->sp++, (cpu)->pc);		\
+		POP_16(cpu, cpu->pc);										\
-		(cpu)->pc = (n);										\
+	} while (0)
+
+#define RST(cpu, n)								\
+	do {										\
+		PUSH_16(cpu, cpu->pc);					\
+		(cpu)->pc = (n);						\
 	} while (0)
 
 #define BIT(cpu, reg, bit)						\
@@ -325,6 +326,38 @@ static const unsigned int cb_extra_cycles[256] = { 0 };
 		reg = tmp;								\
 	} while (0)
 
+#define SRL(cpu, reg)							\
+	do {										\
+		cpu->cf = GET_BIT(reg, 0);				\
+		reg >>= 1;								\
+		cpu->nf = 0;							\
+		cpu->zf = !reg;							\
+		cpu->hf = 0;							\
+	} while (0)
+
+#define RR(cpu, reg)							\
+	do {										\
+		uint8_t ci = cpu->cf;					\
+		cpu->cf = GET_BIT(reg, 0);				\
+		reg >>= 1;								\
+		WRITE_BIT(reg, 7, ci);					\
+		cpu->nf = 0;							\
+		cpu->zf = !reg;							\
+		cpu->hf = 0;							\
+	} while (0)
+
+#define SWAP(cpu, reg)							\
+	do {										\
+		uint8_t tmp = reg << 4;					\
+		tmp |= reg >> 4;						\
+		(cpu)->zf = tmp == 0;					\
+		(cpu)->hf = 0;							\
+		(cpu)->nf = 0;							\
+		(cpu)->cf = 0;							\
+		reg = tmp;								\
+	} while (0)
+
+
 int lr35902_cycle(struct lr35902_state *cpu)
 {
 	uint8_t inst;
@@ -665,7 +698,7 @@ int lr35902_cycle(struct lr35902_state *cpu)
 		cpu->e = cpu->h;
 		break;
 	case 0x5d: /* LD E,L */
-		cpu->e = cpu->h;
+		cpu->e = cpu->l;
 		break;
 	case 0x5e: /* LD E,(HL) */
 		cpu->e = gb_mem_read(cpu->memory, cpu->hl);
@@ -762,7 +795,7 @@ int lr35902_cycle(struct lr35902_state *cpu)
 		cpu->a = cpu->l;
 		break;
 	case 0x7e: /* LD A,(HL) */
-		cpu->a = gb_mem_read(cpu->memory, cpu->hl);;
+		cpu->a = gb_mem_read(cpu->memory, cpu->hl);
 		break;
 	case 0x7f: /* LD A,A */
 		break;
@@ -781,7 +814,7 @@ int lr35902_cycle(struct lr35902_state *cpu)
 	case 0x84:/* ADD A,H */
 		ADD_8(cpu, cpu->a, cpu->h);
 		break;
-	case 0x85:/* ADD A,B */
+	case 0x85:/* ADD A,l */
 		ADD_8(cpu, cpu->a, cpu->l);
 		break;
 	case 0x86: /* ADD A,(HL) */
@@ -966,8 +999,13 @@ int lr35902_cycle(struct lr35902_state *cpu)
 	case 0xc1: /* POP BC */
 		POP_16(cpu, cpu->bc);
 		break;
-	case 0xc2:
+	case 0xc2: /* JP NZ, a16 */
-		ASSERT(0);
+		val_16 = gb_mem_read(cpu->memory, cpu->pc++);
+		val_16 |= ((uint16_t) gb_mem_read(cpu->memory, cpu->pc++) << 8);
+		if (!cpu->zf) {
+			cpu->pc = val_16;
+		}
+		break;
 	case 0xc3: /* JP a16 */
 		val_16 = gb_mem_read(cpu->memory, cpu->pc++);
 		val_16 |= ((uint16_t) gb_mem_read(cpu->memory, cpu->pc++) << 8);
@@ -983,8 +1021,10 @@ int lr35902_cycle(struct lr35902_state *cpu)
 	case 0xc5: /* PUSH BC */
 		PUSH_16(cpu, cpu->bc);
 		break;
-	case 0xc6:
+	case 0xc6: /* ADD A, d8 */
-		ASSERT(0);
+		val = gb_mem_read(cpu->memory, cpu->pc++);
+		ADD_8(cpu, cpu->a, val);
+		break;
 	case 0xc7: /* RST 00 */
 		RST(cpu, 0);
 		break;
@@ -996,7 +1036,13 @@ int lr35902_cycle(struct lr35902_state *cpu)
 	case 0xc9: /* RET */
 		RET(cpu);
 		break;
-	case 0xca:
+	case 0xca: /* JP z, a16 */
+		val_16 = gb_mem_read(cpu->memory, cpu->pc++);
+		val_16 |= ((uint16_t) gb_mem_read(cpu->memory, cpu->pc++) << 8);
+		if (cpu->zf) {
+			cpu->pc = val_16;
+		}
+		break;
 	case 0xcb:
 		inst = gb_mem_read(cpu->memory, cpu->pc++);
 		cycles = cb_extra_cycles[inst];
@@ -1043,13 +1089,29 @@ int lr35902_cycle(struct lr35902_state *cpu)
 			RL(cpu, cpu->a);
 			break;
 		case 0x18:
+			RR(cpu, cpu->b);
+			break;
 		case 0x19:
+			RR(cpu, cpu->c);
+			break;
 		case 0x1a:
+			RR(cpu, cpu->d);
+			break;
 		case 0x1b:
+			RR(cpu, cpu->e);
+			break;
 		case 0x1c:
+			RR(cpu, cpu->h);
+			break;
 		case 0x1d:
+			RR(cpu, cpu->l);
+			break;
 		case 0x1e:
+			ASSERT(0);
+			break;
 		case 0x1f:
+			RR(cpu, cpu->a);
+			break;
 		case 0x20:
 		case 0x21:
 		case 0x22:
@@ -1067,22 +1129,53 @@ int lr35902_cycle(struct lr35902_state *cpu)
 		case 0x2e:
 		case 0x2f:
 		case 0x30:
+			SWAP(cpu, cpu->b);
+			break;
 		case 0x31:
+			SWAP(cpu, cpu->c);
+			break;
 		case 0x32:
+			SWAP(cpu, cpu->d);
+			break;
 		case 0x33:
+			SWAP(cpu, cpu->e);
+			break;
 		case 0x34:
+			SWAP(cpu, cpu->h);
+			break;
 		case 0x35:
+			SWAP(cpu, cpu->l);
+			break;
 		case 0x36:
-		case 0x37:
-		case 0x38:
-		case 0x39:
-		case 0x3a:
-		case 0x3b:
-		case 0x3c:
-		case 0x3d:
-		case 0x3e:
-		case 0x3f:
 			ASSERT(0);
+			break;
+		case 0x37:
+			SWAP(cpu, cpu->a);
+			break;
+		case 0x38:
+			SRL(cpu, cpu->b);
+			break;
+		case 0x39:
+			SRL(cpu, cpu->c);
+			break;
+		case 0x3a:
+			SRL(cpu, cpu->d);
+			break;
+		case 0x3b:
+			SRL(cpu, cpu->e);
+			break;
+		case 0x3c:
+			SRL(cpu, cpu->h);
+			break;
+		case 0x3d:
+			SRL(cpu, cpu->l);
+			break;
+		case 0x3e:
+			ASSERT(0);
+			break;
+		case 0x3f:
+			SRL(cpu, cpu->a);
+			break;
 		case 0x40:
 		case 0x41:
 		case 0x42:
@@ -1344,7 +1437,9 @@ int lr35902_cycle(struct lr35902_state *cpu)
 		CALL(cpu);
 		break;
 	case 0xce:
-		ASSERT(0);
+		val = gb_mem_read(cpu->memory, cpu->pc++);
+		ADC_8(cpu, cpu->a, val);
+		break;
 	case 0xcf: /* RST 08 */
 		RST(cpu, 0x08);
 		break;
@@ -1356,7 +1451,12 @@ int lr35902_cycle(struct lr35902_state *cpu)
 	case 0xd1: /* POP DE */
 		POP_16(cpu, cpu->de);
 		break;
-	case 0xd2:
+	case 0xd2: /* JP NC, a16 */
+		val_16 = gb_mem_read(cpu->memory, cpu->pc++);
+		val_16 |= ((uint16_t) gb_mem_read(cpu->memory, cpu->pc++) << 8);
+		if (!cpu->cf) {
+			cpu->pc = val_16;
+		}
 		break;
 	case 0xd3: /* UNDEF */
 		if (cpu->undef_d3) {
@@ -1378,8 +1478,10 @@ int lr35902_cycle(struct lr35902_state *cpu)
 	case 0xd5:/* PUSH DE */
 		PUSH_16(cpu, cpu->de);
 		break;
-	case 0xd6:
+	case 0xd6: /* SUB A,d8 */
-		ASSERT(0);
+		val = gb_mem_read(cpu->memory, cpu->pc++);
+		SUB_8(cpu, cpu->a, val);
+		break;
 	case 0xd7: /* RST 0x10 */
 		RST(cpu, 0x10);
 		break;
@@ -1389,7 +1491,9 @@ int lr35902_cycle(struct lr35902_state *cpu)
 		}
 		break;
 	case 0xd9: /* RETI */
+		gb_log("Interrupts ON\n");
 		RET(cpu);
+		ASSERT(0);
 		cpu->int_state = LR35902_INT_ON;
 		break;
 	case 0xda:
@@ -1404,6 +1508,7 @@ int lr35902_cycle(struct lr35902_state *cpu)
 		}
 		break;
 	case 0xdd: /* UNDEF */
+		ASSERT(0);
 		break;
 	case 0xde:
 		ASSERT(0);
@@ -1426,59 +1531,86 @@ int lr35902_cycle(struct lr35902_state *cpu)
 	case 0xe5: /* PUSH HL */
 		PUSH_16(cpu, cpu->hl);
 		break;
-	case 0xe6:
+	case 0xe6: /* AND A, d8 */
+		val = gb_mem_read(cpu->memory, cpu->pc++);
+		AND_8(cpu, cpu->a, val);
+		break;
 	case 0xe7: /* RST 0x20 */
 		RST(cpu, 0x20);
 		break;
 	case 0xe8:
-	case 0xe9:
 		ASSERT(0);
+	case 0xe9: /* JP (hl) */
+		cpu->pc = cpu->hl;
+		break;
 	case 0xea: /* LD (a16), A */
 		val_16 = gb_mem_read(cpu->memory, cpu->pc++);
 		val_16 |= gb_mem_read(cpu->memory, cpu->pc++) << 8;
 		gb_mem_write(cpu->memory, val_16, cpu->a);
 		break;
 	case 0xeb: /* UNDEF */
-		break;
 	case 0xec: /* UNDEF */
-		break;
 	case 0xed: /* UNDEF */
+		ASSERT(0);
+	case 0xee: /* XOR d8 */
+		val = gb_mem_read(cpu->memory, cpu->pc++);
+		XOR_8(cpu, cpu->a, val);
 		break;
-	case 0xee:
-			ASSERT(0);
 	case 0xef: /* RST 0x28 */
 		RST(cpu, 0x28);
 		break;
 	case 0xf0: /* LDH A,(a8) / LD A,($FF00+a8) */
 		val = gb_mem_read(cpu->memory, cpu->pc++);
 		cpu->a = gb_mem_read(cpu->memory, 0xff00 + val);
+		if (cpu->pc == 0x66) {
+			break;
+		}
 		break;
 	case 0xf1: /* POP AF */
-		cpu->a = gb_mem_read(cpu->memory, cpu->sp++);
 		val = gb_mem_read(cpu->memory, cpu->sp++);
+		cpu->a = gb_mem_read(cpu->memory, cpu->sp++);
 		cpu->zf = GET_BIT(val, CPU_F_BIT_POS_Z);
 		cpu->nf = GET_BIT(val, CPU_F_BIT_POS_N);
 		cpu->hf = GET_BIT(val, CPU_F_BIT_POS_H);
 		cpu->cf = GET_BIT(val, CPU_F_BIT_POS_C);
 		break;
-	case 0xf2:
+	case 0xf2: /* LD A,(C) / LD A,(C+$FF00) */
-	case 0xf3:
+		cpu->a = gb_mem_read(cpu->memory, 0xFF00 + cpu->c);
-			ASSERT(0);
+		break;
+	case 0xf3: /* DI */
+		gb_log("Interrupts OFF\n");
+		/*TODO: implement me */
 	case 0xf4: /* UNDEF */
 		break;
 	case 0xf5: /* PUSH AF */
-		PUSH_16(cpu, cpu->af);
+		val = cpu->zf << CPU_F_BIT_POS_Z;
+		val |= cpu->nf << CPU_F_BIT_POS_N;
+		val |= cpu->cf << CPU_F_BIT_POS_C;
+		val |= cpu->hf << CPU_F_BIT_POS_H;
+		gb_mem_write(cpu->memory, --cpu->sp, cpu->a);
+		gb_mem_write(cpu->memory, --cpu->sp, val);
+		break;
+	case 0xf6: /* OR d8 */
+		val = gb_mem_read(cpu->memory, cpu->pc++);
+		OR_8(cpu, cpu->a, val);
 		break;
-	case 0xf6:
 	case 0xf7: /* RST 0x30 */
 		RST(cpu, 0x30);
 		break;
 	case 0xf8:
 	case 0xf9:
-	case 0xfa:
-	case 0xfb:
 		ASSERT(0);
+		break;
+	case 0xfa:
+		val_16 = gb_mem_read(cpu->memory, cpu->pc++);
+		val_16 |= gb_mem_read(cpu->memory, cpu->pc++) << 8;
+		cpu->a = gb_mem_read(cpu->memory, val_16);
+		break;
+	case 0xfb: /* EI */
+		gb_log0("Interrupts ON\n");
+		break;
 	case 0xfc: /* UNDEF */
+		ASSERT(0);
 		break;
 	case 0xfd: /* UNDEF */
 		break;

src/gbemu/memory.c

@@ -10,6 +10,10 @@
 #include <string.h>
 #include <stdio.h>
 #include <errno.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
 
 #define MAX_RAM_LEN (1 << 20) /* Up to 8Mb Cartridge */
 #define UNMAP_BOOTROM_ADDR 0xff50
@@ -28,10 +32,52 @@ void gb_mem_init(struct gb_memory *memory, struct gb_video *v)
 	bootrom_mapped = 1;
 }
 
-uint8_t gb_mem_read(struct gb_memory *cpu, uint16_t addr)
+static uint8_t ser_byte = 0;
+static uint8_t ser_ctl = 0;
+
+static void gb_serial_write(struct gb_memory *memory, uint16_t addr, uint8_t val)
+{
+	int fd;
+
+	switch (addr) {
+	case 0xFF01:
+		ser_byte = val;
+		break;
+	case 0xFF02:
+		if (val == 0x81) {
+			fd = open("serial.log", O_WRONLY | O_CREAT | O_APPEND,
+					  S_IRWXU | S_IRGRP | S_IROTH | S_IWUSR);
+			printf("%c", ser_byte);
+			write(fd, &ser_byte, sizeof(ser_byte));
+			close(fd);
+		}
+		break;
+	default:
+		ASSERT(0);
+	}
+}
+
+static uint8_t gb_serial_read(struct gb_memory *memory, uint16_t addr)
+{
+	int fd;
+	static uint8_t ser_byte = 0;
+
+	switch (addr) {
+	case 0xFF01:
+		return ser_byte;
+	case 0xFF02:
+		return ser_ctl;
+	default:
+		ASSERT(0);
+	}
+}
+
+uint8_t gb_mem_read(struct gb_memory *memory, uint16_t addr)
 {
 
 	switch (addr) {
+	case 0xFF01 ... 0xFF02:
+		return gb_serial_read(memory, addr);
 	case 0xFF40 ... 0xFF4B:
 		return gb_video_mem_read(video, addr);
 	default:
@@ -39,8 +85,44 @@ uint8_t gb_mem_read(struct gb_memory *cpu, uint16_t addr)
 	}
 }
 
-void gb_mem_write(struct gb_memory *cpu, uint16_t addr, uint8_t val)
+void gb_mbc3_write(struct gb_memory *memory, uint16_t addr, uint8_t val)
 {
+	switch (addr) {
+	case 0x0000 ... 0x1FFF:
+		/* RAM and Timer Enable */
+		ASSERT(0);
+	case 0xA000 ... 0xBFFF:
+		/* RAM Bank */
+		ASSERT(0);
+	case 0x2000 ... 0x3FFF:
+		/* ROM Bank Number */
+		ASSERT(0);
+	case 0x4000 ... 0x5FFF:
+		/* RAM Bank Number / RTC Reg Select */
+		ASSERT(0);
+	case 0x6000 ... 0x7FFF:
+		/* Latch Clock Data */
+		ASSERT(0);
+	case 0xFF01 ... 0xFF02:
+		gb_serial_write(memory, addr, val);
+		break;
+	case 0xFF40 ... 0xFF4B:
+		gb_video_mem_write(video, addr, val);
+		break;
+	case 0xFFFF:
+		gb_log("Writing to interrupt mask: 0x%02x\n", val);
+	default:
+		ram[addr] = val;
+	}
+}
+
+void gb_mem_write(struct gb_memory *memory, uint16_t addr, uint8_t val)
+{
+
+	if (!bootrom_mapped && ram[0x147] == 0x05) {
+		gb_mbc3_write(memory, addr, val);
+		return;
+	}
 
 	switch (addr) {
 	case UNMAP_BOOTROM_ADDR:
@@ -48,26 +130,30 @@ void gb_mem_write(struct gb_memory *cpu, uint16_t addr, uint8_t val)
 			bootrom_mapped = 0;
 			memcpy(ram, cart_low, sizeof(cart_low));
 		}
-		break;
+		return;
+	case 0xFF01 ... 0xFF02:
+		gb_serial_write(memory, addr, val);
+		return;
 	case 0xFF40 ... 0xFF4B:
 		gb_video_mem_write(video, addr, val);
-		break;
+		return;
 	case 0x8000 ... 0x87FF:
 	case 0x9800 ... 0x9BFF:
-		ram[addr] = val;
 		break;
 	case 0 ... 0x100:
 		if (bootrom_mapped) {
-			break;
+			return;
 		}
-		/* Intentional fall-through */
+		break;
-	default:
+	case 0xFFFF:
-		ram[addr] = val;
+		gb_log("Writing to interrupt mask: 0x%02x\n", val);
+		break;
 	}
 
+	ram[addr] = val;
 }
 
-void gb_mem_force_write(struct gb_memory *cpu, uint16_t addr, uint8_t val)
+void gb_mem_force_write(struct gb_memory *memory, uint16_t addr, uint8_t val)
 {
 
 	switch (addr) {

src/gbemu/video.c

@@ -106,14 +106,19 @@ out:
 
 void gb_video_cycle(struct gb_video *video, int cycles)
 {
+	static int screenshot_count = 0;
+
 	video->line_counter += cycles;
 	if (video->line_counter >= CYCLES_PER_LINE) {
 		video->line_counter -= CYCLES_PER_LINE;
 		video->curline += 1;
 		if (video->curline > LCD_Y_MAX) {
 			video->curline = 0;
-			if (video->lcdcont & 1 << 7) {
+			if (video->lcdcont & (1 << 7)) {
-				gb_video_screenshot(video);
+				screenshot_count++;
+				if (screenshot_count % 30 == 0) {
+					gb_video_screenshot(video);
+				}
 			}
 		}
 	}
@@ -164,7 +169,7 @@ void gb_video_mem_write(struct gb_video *video, uint16_t addr, uint8_t val)
 	uint8_t *write_addr;
 
 	if (video->debug_logging) {
-		gb_log("Video[%x]=%x\n", addr, val);
+		gb_log("Write Video[%x]=%x\n", addr, val);
 	}
 
 	switch (addr) {