Made the emulator less bloated, and faster.

1 files changed, 361 insertions, 517 deletions

diff --git a/sux.c b/sux.c
index 9afff16..164ca4a 100644
--- a/sux.c
+++ b/sux.c
@@ -4,8 +4,8 @@
 #include <string.h>
 #include <pthread.h>
 #define bench 0
-#define debug 0
-#define IO 1
+#define debug 1
+#define IO 0
 #define keypoll 0
 #if bench
 #include <sys/time.h>
@@ -21,15 +21,8 @@
 #define STEP_ADDR 0xC010
 #define CURSES_BACKSPACE 0x7F
 
-#define setflag(flag, bit) (flag) ? (cpu->ps |= (bit << (thread << 3))) : (cpu->ps &= ~(bit << (thread << 3)))
-#define clearflags(thread) {\
-			cpu->c[thread] = 0;\
-			cpu->z[thread] = 0;\
-			cpu->i[thread] = 0;\
-			cpu->s[thread] = 0;\
-			cpu->v[thread] = 0;\
-			cpu->n[thread] = 0;\
-}
+#define setflag(flag, bit) ((flag)) ? (cpu->ps |= (bit << (thread << 3))) : (cpu->ps &= ~(bit << (thread << 3)))
+#define getflag(bit) (cpu->ps & (bit << (thread << 3)))
 
 uint64_t clk[THREADS];	/* Per Thread Clock cycles. */
 uint64_t tclk;		/* Total Clock cycles. */
@@ -91,6 +84,8 @@ void *run(void *args) {
 	gettimeofday(&str[thread], 0);
 #endif
 #if debug && !bench
+	uint8_t scr_row = 0xFF, scr_col = 0xFF;
+	uint8_t updt = 0;
 	#if keypoll
 	pthread_mutex_lock(&mutex);
 	#endif
@@ -111,8 +106,7 @@ void *run(void *args) {
 			}
 			addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->ps >> 8*thread;
 			cpu->sp[thread]--;
-			cpu->i[thread] = 1;
-			setflag(cpu->i[thread], I);
+			setflag(1, I);
 			cpu->pc[thread] = (uint64_t)addr[0xFFA0]
 					| (uint64_t)addr[0xFFA1] << 8
 					| (uint64_t)addr[0xFFA2] << 16
@@ -139,14 +133,29 @@ void *run(void *args) {
 		#if keypoll
 		pthread_mutex_lock(&mutex);
 		#endif
-		/*mvwprintw(scr, lines, 0, "pc: $%08llx, a: $%016llx, b: $%016llx, x: $%016llx, y: $%016llx"
-			", sp: $%04lx, ps: $%016llx, prefix: $%02x, opcode: $%02x, thread: %u, inst: "
-			, cpu->pc[thread], cpu->a[thread], cpu->b[thread], cpu->x[thread], cpu->y[thread]
-			, cpu->sp[thread], cpu->ps, prefix, opcode, thread);*/
-		mvwprintw(scr, lines, 0, "pc: $%08llx, a: $%016llx, b: $%016llx, x: $%016llx, y: $%016llx"
-			", sp: $%04lx, ps: $%016llx, prefix: $%02x, opcode: $%02x, inst: "
-			, cpu->pc[thread], cpu->a[thread], cpu->b[thread], cpu->x[thread], cpu->y[thread]
-			, cpu->sp[thread], cpu->ps, prefix, opcode);
+		wmove(scr, lines, 0);
+		wclrtoeol(scr);
+		wprintw(scr,
+			"pc: $%08llX"
+			", a: $%016llX"
+			", b: $%016llX"
+			", x: $%016llX"
+			", y: $%016llX"
+			, cpu->pc[thread]
+			, cpu->a[thread]
+			, cpu->b[thread]
+			, cpu->x[thread]
+			, cpu->y[thread]);
+		wprintw(scr,
+			", sp: $%04X"
+			", ps: $%04X"
+			", prefix: $%02x"
+			", opcode: $%02x"
+			", inst: "
+			, cpu->sp[thread]
+			, cpu->ps
+			, prefix
+			, opcode);
 		#if keypoll
 		pthread_mutex_unlock(&mutex);
 		#endif
@@ -200,28 +209,19 @@ void *run(void *args) {
 			case ZMY:
 				address = addr[cpu->pc[thread]];
 				switch (addrsize) {
-					case 3:
-						address |= addr[cpu->pc[thread]+1] << 8;
-						address |= addr[cpu->pc[thread]+2] << 16;
-						address |= addr[cpu->pc[thread]+3] << 24;
-						cpu->pc[thread]+=4;
-						break;
 					case 2:
-						address |= addr[cpu->pc[thread]+1] << 8;
-						address |= addr[cpu->pc[thread]+2] << 16;
-						address |= addr[cpu->pc[thread]+3] << 24;
-						address |= (uint64_t)addr[cpu->pc[thread]+4] << 32;
 						address |= (uint64_t)addr[cpu->pc[thread]+5] << 40;
-						cpu->pc[thread]+=6;
-						break;
+						address |= (uint64_t)addr[cpu->pc[thread]+4] << 32;
+						cpu->pc[thread]+=2;
+					case 3:
+						address |= addr[cpu->pc[thread]+3] << 24;
+						cpu->pc[thread]+=1;
 					case 1:
-						address |= addr[cpu->pc[thread]+1] << 8;
 						address |= addr[cpu->pc[thread]+2] << 16;
-						cpu->pc[thread]+=3;
-						break;
+						address |= addr[cpu->pc[thread]+1] << 8;
+						cpu->pc[thread]+=2;
 					case 0:
 						cpu->pc[thread]+=1;
-						break;
 				}
 				tmpaddr = address;
 				switch (optype[opcode]) {
@@ -241,28 +241,19 @@ void *run(void *args) {
 			case INDY:
 				address = addr[cpu->pc[thread]];
 				switch (addrsize) {
-					case 3:
-						address |= addr[cpu->pc[thread]+1] << 8;
-						address |= addr[cpu->pc[thread]+2] << 16;
-						address |= addr[cpu->pc[thread]+3] << 24;
-						cpu->pc[thread]+=4;
-						break;
 					case 2:
-						address |= addr[cpu->pc[thread]+1] << 8;
-						address |= addr[cpu->pc[thread]+2] << 16;
-						address |= addr[cpu->pc[thread]+3] << 24;
-						address |= (uint64_t)addr[cpu->pc[thread]+4] << 32;
 						address |= (uint64_t)addr[cpu->pc[thread]+5] << 40;
-						cpu->pc[thread]+=6;
-						break;
+						address |= (uint64_t)addr[cpu->pc[thread]+4] << 32;
+						cpu->pc[thread]+=2;
+					case 3:
+						address |= addr[cpu->pc[thread]+3] << 24;
+						cpu->pc[thread]+=1;
 					case 1:
-						address |= addr[cpu->pc[thread]+1] << 8;
 						address |= addr[cpu->pc[thread]+2] << 16;
-						cpu->pc[thread]+=3;
-						break;
+						address |= addr[cpu->pc[thread]+1] << 8;
+						cpu->pc[thread]+=2;
 					case 0:
 						cpu->pc[thread]+=1;
-						break;
 				}
 				iclk++;
 				tmpaddr = address;
@@ -289,36 +280,23 @@ void *run(void *args) {
 				break;
 			case ABS:
 				address = addr[cpu->pc[thread]];
-				address |= addr[cpu->pc[thread]+1] << 8;
 				switch (addrsize) {
 					case 3:
-						address |= addr[cpu->pc[thread]+2] << 16;
-						address |= addr[cpu->pc[thread]+3] << 24;
-						address |= (uint64_t)addr[cpu->pc[thread]+4] << 32;
-						address |= (uint64_t)addr[cpu->pc[thread]+5] << 40;
-						address |= (uint64_t)addr[cpu->pc[thread]+6] << 48;
 						address |= (uint64_t)addr[cpu->pc[thread]+7] << 56;
-						cpu->pc[thread]+=8;
-						iclk++;
-						break;
+						cpu->pc[thread]+=1;
 					case 2:
-						address |= addr[cpu->pc[thread]+2] << 16;
-						address |= addr[cpu->pc[thread]+3] << 24;
-						address |= (uint64_t)addr[cpu->pc[thread]+4] << 32;
-						address |= (uint64_t)addr[cpu->pc[thread]+5] << 40;
 						address |= (uint64_t)addr[cpu->pc[thread]+6] << 48;
-						cpu->pc[thread]+=7;
+						address |= (uint64_t)addr[cpu->pc[thread]+5] << 40;
+						cpu->pc[thread]+=2;
 						iclk++;
-						break;
 					case 1:
-						address |= addr[cpu->pc[thread]+2] << 16;
-						address |= addr[cpu->pc[thread]+3] << 24;
 						address |= (uint64_t)addr[cpu->pc[thread]+4] << 32;
-						cpu->pc[thread]+=5;
-						break;
+						address |= addr[cpu->pc[thread]+3] << 24;
+						address |= addr[cpu->pc[thread]+2] << 16;
+						cpu->pc[thread]+=3;
 					case 0:
+						address |= addr[cpu->pc[thread]+1] << 8;
 						cpu->pc[thread]+=2;
-						break;
 				}
 				break;
 
@@ -337,11 +315,9 @@ void *run(void *args) {
 				value |= addr[address+1] << 8;
 		}
 	#if debug && !bench
-		int row, col;
 		#if keypoll
 		pthread_mutex_lock(&mutex);
 		#endif
-		getyx(scr,row, col);
 		char postfix[3];
 		char op[4];
 		op[0] = opname[opcode][0];
@@ -363,18 +339,13 @@ void *run(void *args) {
 			postfix[2] = '\0';
 		}
 		switch (optype[opcode]) {
-			case IMPL:
-				mvwprintw(scr, lines, col, "%s                      \r" , opname[opcode]);
-				break;
+			case IMPL: wprintw(scr, "%s\r" , opname[opcode]); break;
 			case IMM:
-				if (regsize == 1) {
-					mvwprintw(scr, lines, col, "%s #$%02x                \r" , op, value);
-				} else if (regsize == 2) {
-					mvwprintw(scr, lines, col, "%s%s #$%04x            \r" , op, postfix, value);
-				} else if (regsize == 4) {
-					mvwprintw(scr, lines, col, "%s%s #$%08x        \r" , op, postfix, value);
-				} else if (regsize == 8) {
-					mvwprintw(scr, lines, col, "%s%s #$%016llx\r" , op, postfix, value);
+				switch(regsize) {
+					case 1: wprintw(scr, "%s #$%02X\r" , op, value); break;
+					case 2: wprintw(scr, "%s%s #$%04X\r" , op, postfix, value); break;
+					case 4: wprintw(scr, "%s%s #$%08X\r" , op, postfix, value); break;
+					case 8: wprintw(scr, "%s%s #$%016llX\r" , op, postfix, value); break;
 				}
 				break;
 			case ZM:
@@ -385,114 +356,85 @@ void *run(void *args) {
 				else if (optype[opcode] == ZMY)
 					tmpaddr = address - cpu->y[thread];
 				switch (addrsize) {
-					case 3:
-						mvwprintw(scr, lines, col, "%s%s $%08x%s        \r" , op, postfix, tmpaddr, (optype[opcode] == ZM) ? "" : ((optype[opcode] == ZMX) ? ", x" : ", y"));
-						break;
-					case 2:
-						mvwprintw(scr, lines, col, "%s%s $%014llx%s     \r" , op, postfix, tmpaddr, (optype[opcode] == ZM) ? "" : ((optype[opcode] == ZMX) ? ", x" : ", y"));
-						break;
-					case 1:
-						mvwprintw(scr, lines, col, "%s%s $%06x%s        \r" , op, postfix, tmpaddr, (optype[opcode] == ZM) ? "" : ((optype[opcode] == ZMX) ? ", x" : ", y"));
-						break;
-					case 0:
-						mvwprintw(scr, lines, col, "%s%s $%02x%s        \r" , op, postfix, tmpaddr, (optype[opcode] == ZM) ? "" : ((optype[opcode] == ZMX) ? ", x" : ", y"));
-						break;
+					case 3: wprintw(scr, "%s%s $%08X%s\r" , op, postfix, tmpaddr, (optype[opcode] == ZM) ? "" : ((optype[opcode] == ZMX) ? ", x" : ", y")); break;
+					case 2: wprintw(scr, "%s%s $%014llX%s\r" , op, postfix, tmpaddr, (optype[opcode] == ZM) ? "" : ((optype[opcode] == ZMX) ? ", x" : ", y")); break;
+					case 1: wprintw(scr, "%s%s $%06X%s\r" , op, postfix, tmpaddr, (optype[opcode] == ZM) ? "" : ((optype[opcode] == ZMX) ? ", x" : ", y")); break;
+					case 0: wprintw(scr, "%s%s $%02X%s\r" , op, postfix, tmpaddr, (optype[opcode] == ZM) ? "" : ((optype[opcode] == ZMX) ? ", x" : ", y")); break;
 				}
 				break;
 			case IND:
 			case INDX:
 			case INDY:
 				switch (addrsize) {
-					case 3:
-						mvwprintw(scr, lines, col, "%s%s ($%08x%s        \r" , op, postfix, tmpaddr, (optype[opcode] == IND) ? "" : ((optype[opcode] == INDX) ? ", x" : ", y"));
-						break;
-					case 2:
-						mvwprintw(scr, lines, col, "%s%s ($%012llx%s     \r" , op, postfix, tmpaddr, (optype[opcode] == IND) ? "" : ((optype[opcode] == INDX) ? ", x" : ", y"));
-						break;
-					case 1:
-						mvwprintw(scr, lines, col, "%s%s ($%06x%s        \r" , op, postfix, tmpaddr, (optype[opcode] == IND) ? "" : ((optype[opcode] == INDX) ? ", x" : ", y"));
-						break;
-					case 0:
-						mvwprintw(scr, lines, col, "%s%s ($%02x%s        \r" , op, postfix, tmpaddr, (optype[opcode] == IND) ? "" : ((optype[opcode] == INDX) ? ", x" : ", y"));
-						break;
+					case 3: wprintw(scr, "%s%s ($%08X%s\r" , op, postfix, tmpaddr, (optype[opcode] == IND) ? ")" : ((optype[opcode] == INDX) ? ", x)" : "), y")); break;
+					case 2: wprintw(scr, "%s%s ($%012llX%s\r" , op, postfix, tmpaddr, (optype[opcode] == IND) ? ")" : ((optype[opcode] == INDX) ? ", x)" : "), y")); break;
+					case 1: wprintw(scr, "%s%s ($%06X%s\r" , op, postfix, tmpaddr, (optype[opcode] == IND) ? ")" : ((optype[opcode] == INDX) ? ", x)" : "), y")); break;
+					case 0: wprintw(scr, "%s%s ($%02X%s\r" , op, postfix, tmpaddr, (optype[opcode] == IND) ? ")" : ((optype[opcode] == INDX) ? ", x)" : "), y")); break;
 				}
 				break;
 			case ABS:
 				tmpaddr = address;
 				switch (addrsize) {
-					case 3:
-						mvwprintw(scr, lines, col, "%s%s $%016x\r" , op, postfix, tmpaddr);
-						break;
-					case 2:
-						mvwprintw(scr, lines, col, "%s%s $%014llx  \r" , op, postfix, tmpaddr);
-						break;
-					case 1:
-						mvwprintw(scr, lines, col, "%s%s $%010llx    \r" , op, postfix, tmpaddr);
-						break;
-					case 0:
-						mvwprintw(scr, lines, col, "%s%s $%04x       \r" , op, postfix, tmpaddr);
-						break;
+					case 3: wprintw(scr, "%s%s $%016llX\r" , op, postfix, tmpaddr);	break;
+					case 2: wprintw(scr, "%s%s $%014llX\r" , op, postfix, tmpaddr);	break;
+					case 1: wprintw(scr, "%s%s $%010llX\r" , op, postfix, tmpaddr);	break;
+					case 0: wprintw(scr, "%s%s $%04X\r" , op, postfix, tmpaddr   );	break;
 				}
 				break;
-				if (addrsize)
-					mvwprintw(scr, lines, col, "%s%s $%016llx\r" , op, postfix, value);
-				else
-					mvwprintw(scr, lines, col, "%s%s $%04x       \r" , op, postfix, tmpaddr);
-				break;
-
 		}
-		mvwprintw(scr, 28, 0, "TX_ADDR: $%02X, RX_ADDR: $%02X\r", addr[TX_ADDR], addr[RX_ADDR]);
-		mvwprintw(scr, 29, 0, "address: $%016llx, scr_row: %02u, scr_col: %02u, scr_str: %02u, scr_end: %02u\r", address, addr[0], addr[1], addr[0x26], addr[0x27]);
+		mvwprintw(scr, 27, 0, "TX_ADDR: $%02X, RX_ADDR: $%02X\r", addr[TX_ADDR], addr[RX_ADDR]);
+		mvwprintw(scr, 28, 0, "scr_ptr3: %04X\r", (addr[0x5C] << 8) | addr[0x5B]);
+		mvwprintw(scr, 29, 0, "address: $%016llx, scr_row: %02u, scr_col: %02u, scr_str: %02u, scr_end: %02u\r", address, addr[0], addr[1], addr[0x28], addr[0x29]);
 		mvwprintw(scr, 32, 0, "bitabl: %02x %02x %02x %02x %02x %02x %02x %02x "
 				      "%02x %02x %02x %02x %02x %02x %02x %02x\r"
-				       , addr[0x16], addr[0x17], addr[0x18], addr[0x19], addr[0x1A], addr[0x1B], addr[0x1C], addr[0x1D]
-				       , addr[0x1E], addr[0x1F], addr[0x20], addr[0x21], addr[0x22], addr[0x23], addr[0x24], addr[0x25]);
-		mvwprintw(scr, 33, 0, "buffer:\r");
-		uint8_t ln = 34;
+				       , addr[0x18], addr[0x19], addr[0x1A], addr[0x1B], addr[0x1C], addr[0x1D], addr[0x1E], addr[0x1F]
+				       , addr[0x20], addr[0x21], addr[0x22], addr[0x23], addr[0x24], addr[0x25], addr[0x26], addr[0x27]);
+		uint8_t ln = 33;
+		uint16_t line_idx = 0;
 		uint16_t tmpad = 0x2000;
-		for (uint16_t i = 0; i < 20; i+=2) {
-			mvwprintw(scr, ln++, 0, "%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x "
-						"%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x, i: %02x\r"
-						, addr[tmpad+(0x00)+(i*0x10)], addr[tmpad+(0x01)+(i*0x10)], addr[tmpad+(0x02)+(i*0x10)], addr[tmpad+(0x03)+(i*0x10)]
-						, addr[tmpad+(0x04)+(i*0x10)], addr[tmpad+(0x05)+(i*0x10)], addr[tmpad+(0x06)+(i*0x10)], addr[tmpad+(0x07)+(i*0x10)]
-						, addr[tmpad+(0x08)+(i*0x10)], addr[tmpad+(0x09)+(i*0x10)], addr[tmpad+(0x0A)+(i*0x10)], addr[tmpad+(0x0B)+(i*0x10)]
-						, addr[tmpad+(0x0C)+(i*0x10)], addr[tmpad+(0x0D)+(i*0x10)], addr[tmpad+(0x0E)+(i*0x10)], addr[tmpad+(0x0F)+(i*0x10)]
-						, addr[tmpad+(0x10)+(i*0x10)], addr[tmpad+(0x11)+(i*0x10)], addr[tmpad+(0x12)+(i*0x10)], addr[tmpad+(0x13)+(i*0x10)]
-						, addr[tmpad+(0x14)+(i*0x10)], addr[tmpad+(0x15)+(i*0x10)], addr[tmpad+(0x16)+(i*0x10)], addr[tmpad+(0x17)+(i*0x10)]
-						, addr[tmpad+(0x18)+(i*0x10)], addr[tmpad+(0x19)+(i*0x10)], addr[tmpad+(0x1A)+(i*0x10)], addr[tmpad+(0x1B)+(i*0x10)]
-						, addr[tmpad+(0x1C)+(i*0x10)], addr[tmpad+(0x1D)+(i*0x10)], addr[tmpad+(0x1E)+(i*0x10)], addr[tmpad+(0x1F)+(i*0x10)]
-						, i);
-
+		int row, col;
+		uint8_t iscursor = 0;
+		mvwprintw(scr, ln++, 0, "buffer:\r");
+		if (updt) {
+			scr_row = addr[0];
+			scr_col = addr[1];
+			wmove(scr, ln++, 0);
+			for (uint8_t i = 0; i < 10; i++) {
+				line_idx = (i << 6) + (i << 4);
+				for (uint8_t j = 0; j < 0x50; j++) {
+					wprintw(scr, "%02X", addr[tmpad+j+line_idx]);
+						if ((addr[0]+addr[0x28]) == i && addr[1] == j) {
+							iscursor=1;
+							getyx(scr,row, col);
+							wmove(scr, ln++, 0);
+							wclrtoeol(scr);
+							wmove(scr, row+1, col-2);
+							wprintw(scr, "/\\\r");
+							wmove(scr, row, col);
+						}
+				}
+				wprintw(scr, ", i: %02X\r", i);
+				if (!iscursor) {
+					wmove(scr, ln, 0);
+					wclrtoeol(scr);
+				}
+				iscursor = 0;
+				wmove(scr, ln++, 0);
+			}
+			updt = 0;
 		}
-		tmpad = 0x4000;
+		/*tmpad = 0x4000;
+		line_idx = 0;
 		mvwprintw(scr, ln++, 0, "cmd_buf:\r");
-		for (uint16_t i = 0; i < 20; i+=2) {
-			mvwprintw(scr, ln++, 0, "%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x "
-						"%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x, i: %02x\r"
-						, addr[0x4000+(0x00)+(i*0x10)], addr[0x4000+(0x01)+(i*0x10)], addr[0x4000+(0x02)+(i*0x10)], addr[0x4000+(0x03)+(i*0x10)]
-						, addr[0x4000+(0x04)+(i*0x10)], addr[0x4000+(0x05)+(i*0x10)], addr[0x4000+(0x06)+(i*0x10)], addr[0x4000+(0x07)+(i*0x10)]
-						, addr[0x4000+(0x08)+(i*0x10)], addr[0x4000+(0x09)+(i*0x10)], addr[0x4000+(0x0A)+(i*0x10)], addr[0x4000+(0x0B)+(i*0x10)]
-						, addr[0x4000+(0x0C)+(i*0x10)], addr[0x4000+(0x0D)+(i*0x10)], addr[0x4000+(0x0E)+(i*0x10)], addr[0x4000+(0x0F)+(i*0x10)]
-						, addr[0x4000+(0x10)+(i*0x10)], addr[0x4000+(0x11)+(i*0x10)], addr[0x4000+(0x12)+(i*0x10)], addr[0x4000+(0x13)+(i*0x10)]
-						, addr[0x4000+(0x14)+(i*0x10)], addr[0x4000+(0x15)+(i*0x10)], addr[0x4000+(0x16)+(i*0x10)], addr[0x4000+(0x17)+(i*0x10)]
-						, addr[0x4000+(0x18)+(i*0x10)], addr[0x4000+(0x19)+(i*0x10)], addr[0x4000+(0x1A)+(i*0x10)], addr[0x4000+(0x1B)+(i*0x10)]
-						, addr[0x4000+(0x1C)+(i*0x10)], addr[0x4000+(0x1D)+(i*0x10)], addr[0x4000+(0x1E)+(i*0x10)], addr[0x4000+(0x1F)+(i*0x10)]
-						, i);
-
-		}
-		mvwprintw(scr, ln++, 0, "ptr: $%02x%02x%02x%02x%02x%02x%02x%02x\r"
-					, addr[0x30], addr[0x2F], addr[0x2E], addr[0x2D], addr[0x2C], addr[0x2B], addr[0x2A], addr[0x29]);
-		mvwprintw(scr, ln++, 0, "ptr2: $%02x%02x%02x%02x%02x%02x%02x%02x\r"
-					, addr[0x38], addr[0x37], addr[0x36], addr[0x35], addr[0x34], addr[0x33], addr[0x32], addr[0x31]);
-		mvwprintw(scr, ln++, 0, "ptr3: $%02x%02x%02x%02x%02x%02x%02x%02x\r"
-					, addr[0x40], addr[0x3F], addr[0x3E], addr[0x3D], addr[0x3C], addr[0x3B], addr[0x3A], addr[0x39]);
-		mvwprintw(scr, ln++, 0, "ptr4: $%02x%02x%02x%02x%02x%02x%02x%02x\r"
-					, addr[0x48], addr[0x47], addr[0x46], addr[0x45], addr[0x44], addr[0x43], addr[0x42], addr[0x41]);
-		mvwprintw(scr, ln++, 0, "ptr5: $%02x%02x%02x%02x%02x%02x%02x%02x\r"
-					, addr[0x50], addr[0x4F], addr[0x4E], addr[0x4D], addr[0x4C], addr[0x4B], addr[0x4A], addr[0x49]);
-		mvwprintw(scr, ln++, 0, "ptr6: $%02x%02x%02x%02x%02x%02x%02x%02x\r"
-					, addr[0x58], addr[0x57], addr[0x56], addr[0x55], addr[0x54], addr[0x53], addr[0x52], addr[0x51]);
-		wrefresh(scr);
+		for (uint8_t i = 0; i < 20; i++) {
+			wmove(scr, ln);
+			line_idx = (i << 4)+(i << 6);
+			for (uint8_t j = 0; j < 0x10; j++) {
+				wprintw(scr, "%02X ", addr[tmpad+j+line_idx]);
+			}
+			wprintw(scr, ", i: %02X\r", i);
+			ln++;
+		}*/
 		#if keypoll
 		pthread_mutex_unlock(&mutex);
 		#endif
@@ -500,14 +442,6 @@ void *run(void *args) {
 	#endif
 		switch(opcode) {
 			case CPS: /* Clear Processor Status. */
-				clearflags(0);
-				clearflags(1);
-				clearflags(2);
-				clearflags(3);
-				clearflags(4);
-				clearflags(5);
-				clearflags(6);
-				clearflags(7);
 				cpu->ps &= 0;
 				break;
 			case ADC:  /* ADC Immediate. */
@@ -516,17 +450,12 @@ void *run(void *args) {
 			case ADC_Z: /* ADC Zero Matrix. */
 				if (opcode == AAB)
 					value = cpu->b[thread];
-				sum = cpu->a[thread]+value+cpu->c[thread];
+				sum = cpu->a[thread]+value+getflag(C);
 				cpu->a[thread] = sum;
-				cpu->z[thread] = (sum == 0);
-				cpu->n[thread] = (sum >> 63);
-				cpu->v[thread] = !((cpu->a[thread]^value) & 0x8000000000000000) && ((cpu->a[thread]^sum) & 0x8000000000000000);
-				cpu->c[thread] = (sum < value);
-				setflag(cpu->z[thread], Z);
-				setflag(cpu->n[thread], N);
-				setflag(cpu->v[thread], V);
-				setflag(cpu->c[thread], C);
-			setflag(cpu->i[thread], I);
+				setflag(sum == 0, Z);
+				setflag((sum >> 63), N);
+				setflag(((cpu->a[thread]^value) & 0x8000000000000000) && ((cpu->a[thread]^sum) & 0x8000000000000000), V);
+				setflag((sum < value), C);
 				break;
 			case PHB: /* PusH B register to stack. */
 			case PHP: /* PusH Processor status to stack. */
@@ -534,28 +463,23 @@ void *run(void *args) {
 			case PHY: /* PusH Y register to stack. */
 			case PHX: /* PusH X register to stack. */
 				tmp = value;
-				if (opcode == PHB) {
-					reg = cpu->b[thread];
-				} else if (opcode == PHP) {
-					reg = cpu->ps;
-				} else if (opcode == PHA) {
-					reg = cpu->a[thread];
-				} else if (opcode == PHY) {
-					reg = cpu->y[thread];
-				} else if (opcode == PHX) {
-					reg = cpu->x[thread];
+				switch (opcode) {
+					case PHA: reg = cpu->a[thread]; break;
+					case PHB: reg = cpu->b[thread]; break;
+					case PHX: reg = cpu->x[thread]; break;
+					case PHY: reg = cpu->y[thread]; break;
+					case PHP: reg = cpu->ps;	break;
 				}
-
 				if (tmp > 7)
 					tmp = 7;
 				switch (tmp) {
-					case 7:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]--] = reg >> (tmp--<<3);
-					case 6:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]--] = reg >> (tmp--<<3);
-					case 5:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]--] = reg >> (tmp--<<3);
-					case 4:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]--] = reg >> (tmp--<<3);
-					case 3:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]--] = reg >> (tmp--<<3);
-					case 2:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]--] = reg >> (tmp--<<3);
-					case 1:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]--] = reg >> (tmp--<<3);
+					case 7:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]--] = reg >> (7<<3);
+					case 6:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]--] = reg >> (6<<3);
+					case 5:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]--] = reg >> (5<<3);
+					case 4:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]--] = reg >> (4<<3);
+					case 3:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]--] = reg >> (3<<3);
+					case 2:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]--] = reg >> (2<<3);
+					case 1:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]--] = reg >> (1<<3);
 					case 0:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]--] = reg & 0xFF;
 				}
 				break;
@@ -570,38 +494,21 @@ void *run(void *args) {
 			case TBA: /* Transfer B to Accumulator. */
 			case TXS: /* Transfer X to Stack pointer. */
 				switch (opcode) {
-					case TAY:
-						cpu->y[thread] = cpu->a[thread];
-						break;
-                                        case TAX:
-						cpu->x[thread] = cpu->a[thread];
-						break;
-                                        case TYX:
-						cpu->x[thread] = cpu->y[thread];
-						break;
-                                        case TYA:
-						cpu->a[thread] = cpu->y[thread];
+					case TAY: cpu->y[thread] = cpu->a[thread]; break;
+                                        case TAX: cpu->x[thread] = cpu->a[thread]; break;
+                                        case TYX: cpu->x[thread] = cpu->y[thread]; break;
+                                        case TYA: cpu->a[thread] = cpu->y[thread]; break;
+                                        case TXA: cpu->a[thread] = cpu->x[thread]; break;
+					case TXY: cpu->y[thread] = cpu->x[thread]; break;
+                                        case TAB: cpu->b[thread] = cpu->a[thread];
+						setflag(cpu->b[thread] == 0, Z);
+						setflag(cpu->b[thread] >> 63, N);
 						break;
-                                        case TXA:
-						cpu->a[thread] = cpu->x[thread];
-						break;
-					case TXY:
-						cpu->y[thread] = cpu->x[thread];
-						break;
-                                        case TAB:
-						cpu->b[thread] = cpu->a[thread];
-						cpu->z[thread] = (cpu->b[thread] == 0);
-						cpu->n[thread] = (cpu->b[thread] >> 63);
-						break;
-                                        case TSX:
-						cpu->x[thread] = cpu->sp[thread] & 0xFFFF;
+                                        case TSX: cpu->x[thread] = cpu->sp[thread] & 0xFFFF;
 						cpu->x[thread] = cpu->stk_st[thread] << 16;
 						break;
-                                        case TBA:
-						cpu->a[thread] = cpu->b[thread];
-						break;
-                                        case TXS:
-						cpu->sp[thread] = cpu->x[thread];
+                                        case TBA: cpu->a[thread] = cpu->b[thread]; break;
+                                        case TXS: cpu->sp[thread] = cpu->x[thread];
 						if (prefix == 0x13 && (value == thread+1 || value > 8)) {
 							cpu->stk_st[thread] = value & 0xFF;
 							cpu->stk_st[thread] += value << 16;
@@ -613,22 +520,20 @@ void *run(void *args) {
 					case TYA:
 					case TXA:
 					case TBA:
-						cpu->z[thread] = (cpu->a[thread] == 0);
-						cpu->n[thread] = (cpu->a[thread] >> 63);
+						setflag(cpu->a[thread] == 0, Z);
+						setflag(cpu->a[thread] >> 63, N);
 						break;
 					case TAY:
 					case TXY:
-						cpu->z[thread] = (cpu->y[thread] == 0);
-						cpu->n[thread] = (cpu->y[thread] >> 63);
+						setflag(cpu->y[thread] == 0, Z);
+						setflag(cpu->y[thread] >> 63, N);
 						break;
 					case TAX:
 					case TYX:
-						cpu->z[thread] = (cpu->x[thread] == 0);
-						cpu->n[thread] = (cpu->x[thread] >> 63);
+						setflag(cpu->x[thread] == 0, Z);
+						setflag(cpu->x[thread] >> 63, N);
 						break;
 				}
-				setflag(cpu->z[thread], Z);
-				setflag(cpu->n[thread], N);
 				break;
 			case JMP: /* JMP Absolute. */
 			case JMP_Z: /* JMP Zero Matrix. */
@@ -642,15 +547,11 @@ void *run(void *args) {
 				if (opcode == SAB) {
 					value = cpu->b[thread];
 				}
-				sum = cpu->a[thread]-value-!cpu->c[thread];
-				cpu->z[thread] = (sum == 0);
-				cpu->n[thread] = (sum >> 63);
-				cpu->v[thread] = ((cpu->a[thread]^value) & 0x8000000000000000) && ((cpu->a[thread]^sum) & 0x8000000000000000);
-				cpu->c[thread] = (sum > value);
-				setflag(cpu->z[thread], Z);
-				setflag(cpu->n[thread], N);
-				setflag(cpu->v[thread], V);
-				setflag(cpu->c[thread], C);
+				sum = cpu->a[thread]-value-!getflag(C);
+				setflag(sum == 0, Z);
+				setflag(sum >> 63, N);
+				setflag(((cpu->a[thread]^value) & 0x8000000000000000) && ((cpu->a[thread]^sum) & 0x8000000000000000), V);
+				setflag((sum > value), C);
 				cpu->a[thread] = sum;
 				break;
 			case PLB: /* PuLl B register from stack. */
@@ -659,29 +560,25 @@ void *run(void *args) {
 			case PLY: /* PuLl Y register from stack. */
 			case PLX: /* PuLl X register from stack. */
 				tmp = value;
-				if (opcode == PLB)
-					reg = cpu->b[thread];
-				else if (opcode == PLP)
-					reg = cpu->ps;
-				else if (opcode == PLA)
-					reg = cpu->a[thread];
-				else if (opcode == PLY)
-					reg = cpu->y[thread];
-				else if (opcode == PLX)
-					reg = cpu->x[thread];
-
+				switch (opcode) {
+					case PLA: reg = cpu->a[thread]; break;
+					case PLB: reg = cpu->b[thread]; break;
+					case PLX: reg = cpu->x[thread]; break;
+					case PLY: reg = cpu->y[thread]; break;
+					case PLP: reg = cpu->ps;	break;
+				}
 				if (tmp > 7)
 					tmp = 7;
 				tmp2 = 0;
-				cpu->sp[thread]++;reg = addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] & 0xFF;tmp2+=8;tmp--;
+				cpu->sp[thread]++;reg = addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] & 0xFF;tmp--;
 				switch (tmp) {
-					case 6:	cpu->sp[thread]++;reg += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << tmp2;tmp2+=8;
-					case 5:	cpu->sp[thread]++;reg += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << tmp2;tmp2+=8;
-					case 4:	cpu->sp[thread]++;reg += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << tmp2;tmp2+=8;
-					case 3:	cpu->sp[thread]++;reg += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << tmp2;tmp2+=8;
-					case 2:	cpu->sp[thread]++;reg += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << tmp2;tmp2+=8;
-					case 1:	cpu->sp[thread]++;reg += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << tmp2;tmp2+=8;
-					case 0:	cpu->sp[thread]++;reg += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << tmp2;tmp2+=8;
+					case 6:	cpu->sp[thread]++;tmp2++;reg += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
+					case 5:	cpu->sp[thread]++;tmp2++;reg += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
+					case 4:	cpu->sp[thread]++;tmp2++;reg += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
+					case 3:	cpu->sp[thread]++;tmp2++;reg += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
+					case 2:	cpu->sp[thread]++;tmp2++;reg += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
+					case 1:	cpu->sp[thread]++;tmp2++;reg += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
+					case 0:	cpu->sp[thread]++;tmp2++;reg += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
 				}
 				if (opcode == PLB) {
 					cpu->b[thread] = reg;
@@ -697,29 +594,22 @@ void *run(void *args) {
 				break;
 			case JSR_IN: /* JSR Indirect. */
 			case JSR: /* Jump to SubRoutine. */
-				/*if (addrsize)
-					stksize = 24;
-				else
-					stksize = 0;*/
 				switch (addrsize) {
-					case 2:
-						addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] >> 40;cpu->sp[thread]--;
-						addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] >> 32;cpu->sp[thread]--;
-					case 3:
-						addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] >> 24;cpu->sp[thread]--;
-					case 1:
-						addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] >> 16;cpu->sp[thread]--;
-						addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] >> 8;cpu->sp[thread]--;
-					case 0:
-						addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] & 0xFF;cpu->sp[thread]--;
+					case 3: stksize = 3; break;
+					case 2: stksize = 5; break;
+					case 1: stksize = 2; break;
+					case 0: stksize = 0; break;
+				}
+				switch (stksize) {
+					case 7:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]--] = (uint64_t)cpu->pc[thread] >> (7<<3);
+					case 6:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]--] = (uint64_t)cpu->pc[thread] >> (6<<3);
+					case 5:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]--] = (uint64_t)cpu->pc[thread] >> (5<<3);
+					case 4:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]--] = (uint64_t)cpu->pc[thread] >> (4<<3);
+					case 3:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]--] = (uint64_t)cpu->pc[thread] >> (3<<3);
+					case 2:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]--] = (uint64_t)cpu->pc[thread] >> (2<<3);
+					case 1:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]--] = (uint64_t)cpu->pc[thread] >> (1<<3);
+					case 0:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]--] = (uint64_t)cpu->pc[thread] & 0xFF;
 				}
-				/*for (int8_t i = stksize; i >= 0; i-=8) {
-					if (i)
-						addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] >> i;
-					else
-						addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] & 0xFF;
-					cpu->sp[thread]--;
-				}*/
 				cpu->pc[thread] = address;
 				break;
 			case AND: /* AND Immediate. */
@@ -730,10 +620,8 @@ void *run(void *args) {
 					value = cpu->b[thread];
 				}
 				cpu->a[thread] &= value;
-				cpu->z[thread] = (value == 0);
-				cpu->n[thread] = (value >> 63);
-				setflag(cpu->z[thread], Z);
-				setflag(cpu->n[thread], N);
+				setflag(value == 0, Z);
+				setflag(value >> 63, N);
 				break;
 			case STT: /* STart Thread. */
 				cpu->crt |= value;
@@ -753,7 +641,7 @@ void *run(void *args) {
 				break;
 			case BPO: /* BPO Absolute. */
 			case BPO_Z: /* BPO Zero Matrix. */
-				if (!cpu->n[thread])
+				if (!getflag(N))
 					cpu->pc[thread] = address;
 				break;
 			case ORA: /* ORA Immediate. */
@@ -764,18 +652,15 @@ void *run(void *args) {
 					value = cpu->b[thread];
 				}
 				cpu->a[thread] |= value;
-				cpu->z[thread] = (value == 0);
-				cpu->n[thread] = (value >> 63);
-				setflag(cpu->z[thread], Z);
-				setflag(cpu->n[thread], N);
+				setflag(value == 0, Z);
+				setflag(value >> 63, N);
 				break;
 			case SEI: /* SEt Interrupt. */
-				cpu->i[thread] = 1;
-				setflag(cpu->i[thread], I);
+				setflag(1, I);
 				break;
 			case BNG: /* BNG Absolute. */
 			case BNG_Z: /* BNG Zero Matrix. */
-				if (cpu->n[thread])
+				if (getflag(N))
 					cpu->pc[thread] = address;
 				break;
 			case XOR: /* XOR Immediate. */
@@ -786,18 +671,15 @@ void *run(void *args) {
 					value = cpu->b[thread];
 				}
 				cpu->a[thread] ^= value;
-				cpu->z[thread] = (value == 0);
-				cpu->n[thread] = (value >> 63);
-				setflag(cpu->z[thread], Z);
-				setflag(cpu->n[thread], N);
+				setflag(value == 0, Z);
+				setflag(value >> 63, N);
 				break;
 			case CLI: /* CLear Interrupt. */
-				cpu->i[thread] = 0;
-				setflag(cpu->i[thread], I);
+				setflag(0, I);
 				break;
 			case BCS: /* BCS Absolute. */
 			case BCS_Z: /* BCS Zero Matrix. */
-				if (cpu->c[thread])
+				if (getflag(C))
 					cpu->pc[thread] = address;
 				break;
 			case LSL: /* LSL Immediate. */
@@ -808,17 +690,13 @@ void *run(void *args) {
 					value = cpu->b[thread];
 				}
 				sum = (value < 64) ? cpu->a[thread] << value : 0;
-				cpu->z[thread] = (sum == 0);
-				cpu->n[thread] = (sum >> 63);
-				cpu->c[thread] = cpu->a[thread] >> 64-value;
+				setflag(sum == 0, Z);
+				setflag(sum >> 63, N);
+				setflag(cpu->a[thread] >> 64-value, C);
 				cpu->a[thread] = sum;
-				setflag(cpu->z[thread], Z);
-				setflag(cpu->n[thread], N);
-				setflag(cpu->c[thread], C);
 				break;
 			case SEC: /* SEt Carry flag.*/
-				cpu->c[thread] = 1;
-				setflag(cpu->c[thread], C);
+				setflag(1, C);
 				break;
 			case STA: /* STA Absolute. */
 			case STY: /* STY Absolute. */
@@ -842,7 +720,6 @@ void *run(void *args) {
 			case STB_IX: /* STB Indexed Indirect. */
 			case STA_IY: /* STA Indirect Indexed. */
 			case STB_IY: /* STB Indirect Indexed. */
-
 				switch (opcode) {
 					case STB:
 					case STB_Z:
@@ -877,9 +754,6 @@ void *run(void *args) {
 						break;
 				}
 				addr[address] = value & 0xFF;
-				if (address == STEP_ADDR) {
-					step = addr[address];
-				}
 				#if IO
 				if (address == TX_ADDR) {
 					#if keypoll
@@ -912,14 +786,16 @@ void *run(void *args) {
 								esc = 0;
 								break;
 							case 'H':
-								if (!bcd[2] && !bcd[3])
+								if (!bcd[2] && !bcd[3]) {
 									y = 0;
-								else
+								} else {
 									y = ((bcd[3]*10) + bcd[2]);
-								if (!bcd[0] && !bcd[1])
+								}
+								if (!bcd[0] && !bcd[1]) {
 									x = 0;
-								else
+								} else {
 									x = ((bcd[1]*10) + bcd[0]);
+								}
 								idx = 3;
 								wmove(scr, y, x);
 								bcd[0] = 0;
@@ -967,13 +843,11 @@ void *run(void *args) {
 								wmove(scr, y, x);
 							}
 							wdelch(scr);
-							wrefresh(scr);
 						} else if (addr[address] == '\033') {
 							esc = 1;
 						} else {
 							wmove(scr, y, x);
 							waddch(scr, addr[address]);
-							wrefresh(scr);
 							if (addr[address] == '\n') {
 								x = 0;
 								y+=1;
@@ -1022,7 +896,7 @@ void *run(void *args) {
 									x = ((bcd[1]*10) + bcd[0]);
 								mvwprintw(scr, 30, 0, "x: %i, y: %i  ", x, y);
 								mvwprintw(scr, 31, 0, "bcd[3-2]: {%u, %u}, bcd[1-0]: {%u, %u}", bcd[3], bcd[2], bcd[1], bcd[0]);
-								wrefresh(scr);
+								updt = 1;
 								idx = 3;
 								bcd[0] = 0;
 								bcd[1] = 0;
@@ -1052,12 +926,14 @@ void *run(void *args) {
 						}
 					} else {
 						if (addr[address] == CURSES_BACKSPACE || addr[address] == '\b') {
+							updt = 1;
 							if (x > 0) {
 								x--;
 							}
 						} else if (addr[address] == '\033') {
 							esc = 1;
 						} else {
+							updt = (!addr[0x16]);
 							if (addr[address] == '\n') {
 								x = 0;
 								y+=1;
@@ -1069,23 +945,22 @@ void *run(void *args) {
 				}
 				#endif
 				#endif
-				if (regsize >= 2) {
-					addr[address+1] = value >> 8;
-					if (regsize >= 4) {
-						addr[address+2] = value >> 16;
+				switch (regsize) {
+					case 8:
+						addr[address+7] = value >> 56;
+						addr[address+6] = value >> 48;
+						addr[address+5] = value >> 40;
+						addr[address+4] = value >> 32;
+					case 4:
 						addr[address+3] = value >> 24;
-						if (regsize >= 8) {
-							addr[address+4] = value >> 32;
-							addr[address+5] = value >> 40;
-							addr[address+6] = value >> 48;
-							addr[address+7] = value >> 56;
-						}
-					}
+						addr[address+2] = value >> 16;
+					case 2:
+						addr[address+1] = value >> 8;
 				}
 				break;
 			case BCC: /* BCC Absolute. */
 			case BCC_Z: /* BCC Zero Matrix. */
-				if (!cpu->c[thread])
+				if (!getflag(C))
 					cpu->pc[thread] = address;
 				break;
 			case LSR: /* LSR Immediate. */
@@ -1096,13 +971,10 @@ void *run(void *args) {
 					value = cpu->b[thread];
 				}
 				sum = (value < 64) ? cpu->a[thread] >> value : 0;
-				cpu->z[thread] = (sum == 0);
-				cpu->n[thread] = (sum >> 63);
-				cpu->c[thread] = cpu->a[thread] & 1;
+				setflag(sum == 0, Z);
+				setflag(sum >> 63, N);
+				setflag(cpu->a[thread] & 1, C);
 				cpu->a[thread] = sum;
-				setflag(cpu->z[thread], Z);
-				setflag(cpu->n[thread], N);
-				setflag(cpu->c[thread], C);
 				break;
 			case ASR: /* ASR Immediate. */
 			case ARB: /* Arithmetic shift Right accumulator by B. */
@@ -1112,17 +984,13 @@ void *run(void *args) {
 					value = cpu->b[thread];
 				sign = cpu->a[thread] & 0x8000000000000000;
 				sum = (value < 64) ? (cpu->a[thread] >> value) | sign : 0;
-				cpu->z[thread] = (sum == 0);
-				cpu->n[thread] = (sum >> 63);
-				cpu->c[thread] = cpu->a[thread] & 1;
+				setflag(sum == 0, Z);
+				setflag(sum >> 63, N);
+				setflag(cpu->a[thread] & 1, C);
 				cpu->a[thread] = sum;
-				setflag(cpu->z[thread], Z);
-				setflag(cpu->n[thread], N);
-				setflag(cpu->c[thread], C);
 				break;
 			case CLC: /* CLear Carry flag. */
-				cpu->c[thread] = 0;
-				setflag(cpu->c[thread], C);
+				setflag(0, C);
 				break;
 			case LDB: /* LDB Immediate. */
 			case LDA: /* LDA Immediate. */
@@ -1212,14 +1080,12 @@ void *run(void *args) {
 						cpu->x[thread] = value;
 						break;
 				}
-				cpu->z[thread] = (value == 0);
-				cpu->n[thread] = (value >> 63);
-				setflag(cpu->z[thread], Z);
-				setflag(cpu->n[thread], N);
+				setflag(value == 0, Z);
+				setflag(value >> 63, N);
 				break;
 			case BEQ: /* BEQ Absolute. */
 			case BEQ_Z: /* BEQ Zero Matrix. */
-				if (cpu->z[thread])
+				if (getflag(Z))
 					cpu->pc[thread] = address;
 				break;
 			case ROL: /* ROL Immediate. */
@@ -1230,18 +1096,15 @@ void *run(void *args) {
 					value = cpu->b[thread];
 				}
 				sum = cpu->a[thread] << value;
-				sum |= cpu->c[thread];
-				cpu->z[thread] = (sum == 0);
-				cpu->n[thread] = (sum >> 63);
-				cpu->c[thread] = cpu->a[thread] >> (uint64_t)64-value;
+				sum |= getflag(C);
+				setflag(sum == 0, Z);
+				setflag(sum >> 63, N);
+				setflag(cpu->a[thread] >> (uint64_t)64-value, C);
 				cpu->a[thread] = sum;
-				setflag(cpu->z[thread], Z);
-				setflag(cpu->n[thread], N);
-				setflag(cpu->c[thread], C);
 				break;
 			case BNE: /* BNE Absolute. */
 			case BNE_Z: /* BNE Zero Matrix. */
-				if (!cpu->z[thread])
+				if (!getflag(Z))
 					cpu->pc[thread] = address;
 				break;
 			case ROR: /* ROR Immediate. */
@@ -1252,18 +1115,15 @@ void *run(void *args) {
 					value = cpu->b[thread];
 				}
 				sum = cpu->a[thread] >> value;
-				sum |= (uint64_t)cpu->c[thread] << (uint64_t)64-value;
-				cpu->z[thread] = (sum == 0);
-				cpu->n[thread] = (sum >> 63);
-				cpu->c[thread] = cpu->a[thread] & 1;
+				sum |= (uint64_t)getflag(C) << (uint64_t)64-value;
+				setflag(sum == 0, Z);
+				setflag(sum >> 63, N);
+				setflag(cpu->a[thread] & 1, C);
 				cpu->a[thread] = sum;
-				setflag(cpu->z[thread], Z);
-				setflag(cpu->n[thread], N);
-				setflag(cpu->c[thread], C);
 				break;
 			case BVS: /* BVS Absolute. */
 			case BVS_Z: /* BVS Zero Matrix. */
-				if (cpu->v[thread])
+				if (getflag(V))
 					cpu->pc[thread] = address;
 				break;
 			case MUL: /* MUL Immediate. */
@@ -1273,20 +1133,16 @@ void *run(void *args) {
 				if (opcode == MAB) {
 					value = cpu->b[thread];
 				}
-				sum = cpu->a[thread]*value+cpu->c[thread];
+				sum = cpu->a[thread]*value+getflag(C);
 				cpu->a[thread] = sum;
-				cpu->z[thread] = (sum == 0);
-				cpu->n[thread] = (sum >> 63);
-				cpu->v[thread] = !((cpu->a[thread]^value) & 0x8000000000000000) && ((cpu->a[thread]^sum) & 0x8000000000000000);
-				cpu->c[thread] = (!((cpu->a[thread]^sum) && (cpu->a[thread]^value)) && (cpu->a[thread] >= ((uint64_t)1 << 32) && value >= ((uint64_t)1 << 32)));
-				setflag(cpu->z[thread], Z);
-				setflag(cpu->n[thread], N);
-				setflag(cpu->v[thread], V);
-				setflag(cpu->c[thread], C);
+				setflag(sum == 0, Z);
+				setflag(sum >> 63, N);
+				setflag(!((cpu->a[thread]^value) & 0x8000000000000000) && ((cpu->a[thread]^sum) & 0x8000000000000000), V);
+				setflag((!((cpu->a[thread]^sum) && (cpu->a[thread]^value)) && (cpu->a[thread] >= ((uint64_t)1 << 32) && value >= ((uint64_t)1 << 32))), C);
 				break;
 			case BVC: /* BVC Absolute. */
 			case BVC_Z: /* BVC Zero Matrix. */
-				if (!cpu->v[thread])
+				if (!getflag(V))
 					cpu->pc[thread] = address;
 				break;
 			case DIV: /* DIV Immediate. */
@@ -1301,30 +1157,27 @@ void *run(void *args) {
 				}
 				sum = cpu->a[thread]/value;
 				cpu->a[thread] = sum;
-				cpu->z[thread] = (sum == 0);
-				cpu->n[thread] = (sum >> 63);
-				setflag(cpu->z[thread], Z);
-				setflag(cpu->n[thread], N);
+				setflag(sum == 0, Z);
+				setflag((sum >> 63), N);
 				break;
 			case CLV: /* CLear oVerflow flag. */
-				cpu->v[thread] = 0;
-				setflag(cpu->v[thread], V);
+				setflag(0, V);
 				break;
 			case RTS: /* ReTurn from Subroutine. */
-				/*if (addrsize)
-					stksize = 32;
-				else
-					stksize = 8;*/
-				cpu->sp[thread]++;cpu->pc[thread]  = addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]];
+				tmp2 = 0;
 				switch (addrsize) {
-					case 1:
-						cpu->sp[thread]++;cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << 8;
-						cpu->sp[thread]++;cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << 16;
-					case 3:
-						cpu->sp[thread]++;cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << 24;
-					case 2:
-						cpu->sp[thread]++;cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << 32;
-						cpu->sp[thread]++;cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << 40;
+					case 3: stksize = 3; break;
+					case 2: stksize = 5; break;
+					case 1: stksize = 2; break;
+					case 0: stksize = 0; break;
+				}
+				cpu->sp[thread]++;reg = addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] & 0xFF;stksize--;
+				switch (stksize) {
+					case 4:	cpu->sp[thread]++;tmp2++;reg += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
+					case 3:	cpu->sp[thread]++;tmp2++;reg += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
+					case 2:	cpu->sp[thread]++;tmp2++;reg += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
+					case 1:	cpu->sp[thread]++;tmp2++;reg += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
+					case 0:	cpu->sp[thread]++;tmp2++;reg += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
 				}
 				break;
 			case CPB: /* CPB Immediate. */
@@ -1384,14 +1237,10 @@ void *run(void *args) {
 						break;
 				}
 				sum = reg-value;
-				cpu->n[thread] = (sum & 0x8000000000000000);
-				cpu->v[thread] = ((reg^value) & 0x8000000000000000) && ((reg^sum) & 0x8000000000000000);
-				cpu->z[thread] = (sum == 0);
-				cpu->c[thread] = (reg >= value);
-				setflag(cpu->n[thread], N);
-				setflag(cpu->v[thread], V);
-				setflag(cpu->z[thread], Z);
-				setflag(cpu->c[thread], C);
+				setflag(sum >> 63, N);
+				setflag(((reg^value) & 0x8000000000000000) && ((reg^sum) & 0x8000000000000000), V);
+				setflag(sum == 0, Z);
+				setflag(reg >= value, C);
 				break;
 			case ENT: /* ENd Thread. */
 				cpu->crt &= ~value;
@@ -1400,15 +1249,16 @@ void *run(void *args) {
 						cpu->pc[i+1] = cpu->pc[0]+(i+1);
 				break;
 			case RTI: /* ReTurn from Interrupt routine. */
-				cpu->sp[thread]++;
-				cpu->ps = addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << 8*thread;
-				for (uint8_t i = 0; i < 64; i+=8) {
-					cpu->sp[thread]++;
-					if (i)
-						cpu->pc[thread] += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << i;
-					else
-						cpu->pc[thread] = addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]];
-				}
+				cpu->sp[thread] += 9;
+				cpu->ps	= addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-8)] << (thread << 3);
+				cpu->pc[thread]  = (uint64_t)addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-7)];
+				cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-6)] <<  8;
+				cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-5)] << 16;
+				cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-4)] << 24;
+				cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-3)] << 32;
+				cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-2)] << 40;
+				cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-1)] << 48;
+				cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]]     << 56;
 				break;
 			case INC: /* INC Accumulator. */
 			case INB:
@@ -1417,27 +1267,25 @@ void *run(void *args) {
 				switch (opcode) {
 					case INC:
 						cpu->a[thread]+=1;
-						cpu->z[thread] = (cpu->a[thread] == 0);
-						cpu->n[thread] = (cpu->a[thread] >> 63);
+						setflag(cpu->a[thread] == 0, Z);
+						setflag((cpu->a[thread] >> 63), N);
 						break;
 					case INB:
 						cpu->b[thread]+=1;
-						cpu->z[thread] = (cpu->b[thread] == 0);
-						cpu->n[thread] = (cpu->b[thread] >> 63);
+						setflag(cpu->b[thread] == 0, Z);
+						setflag((cpu->b[thread] >> 63), N);
 						break;
 					case INY:
 						cpu->y[thread]+=1;
-						cpu->z[thread] = (cpu->y[thread] == 0);
-						cpu->n[thread] = (cpu->y[thread] >> 63);
+						setflag(cpu->y[thread] == 0, Z);
+						setflag((cpu->y[thread] >> 63), N);
 						break;
 					case INX:
 						cpu->x[thread]+=1;
-						cpu->z[thread] = (cpu->x[thread] == 0);
-						cpu->n[thread] = (cpu->x[thread] >> 63);
+						setflag(cpu->x[thread] == 0, Z);
+						setflag((cpu->x[thread] >> 63), N);
 						break;
 				}
-				setflag(cpu->z[thread], Z);
-				setflag(cpu->n[thread], N);
 				break;
 			case DEC: /* DEC Accumulator. */
 			case DEB:
@@ -1446,105 +1294,101 @@ void *run(void *args) {
 				switch (opcode) {
 					case DEC:
 						cpu->a[thread]-=1;
-						cpu->z[thread] = (cpu->a[thread] == 0);
-						cpu->n[thread] = (cpu->a[thread] >> 63);
+						setflag(cpu->a[thread] == 0, Z);
+						setflag((cpu->a[thread] >> 63), N);
 						break;
 					case DEB:
 						cpu->b[thread]-=1;
-						cpu->z[thread] = (cpu->b[thread] == 0);
-						cpu->n[thread] = (cpu->b[thread] >> 63);
+						setflag(cpu->b[thread] == 0, Z);
+						setflag((cpu->b[thread] >> 63), N);
 						break;
 					case DEY:
 						cpu->y[thread]-=1;
-						cpu->z[thread] = (cpu->y[thread] == 0);
-						cpu->n[thread] = (cpu->y[thread] >> 63);
+						setflag(cpu->y[thread] == 0, Z);
+						setflag((cpu->y[thread] >> 63), N);
 						break;
 					case DEX:
 						cpu->x[thread]-=1;
-						cpu->z[thread] = (cpu->x[thread] == 0);
-						cpu->n[thread] = (cpu->x[thread] >> 63);
+						setflag(cpu->x[thread] == 0, Z);
+						setflag((cpu->x[thread] >> 63), N);
 						break;
 				}
-				setflag(cpu->z[thread], Z);
-				setflag(cpu->n[thread], N);
 				break;
 			case JSL: /* Jump to Subroutine Long. */
 				switch (addrsize) {
-					case 3:
-						addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] >> 56;cpu->sp[thread]--;
-					case 2:
-						addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] >> 48;cpu->sp[thread]--;
-						addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] >> 40;cpu->sp[thread]--;
-					case 1:
-						addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] >> 32;cpu->sp[thread]--;
-						addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] >> 24;cpu->sp[thread]--;
-						addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] >> 16;cpu->sp[thread]--;
-					case 0:
-						addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] >> 8;cpu->sp[thread]--;
-						addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] & 0xFF;cpu->sp[thread]--;
+					case 3: stksize = 7; break;
+					case 2: stksize = 6; break;
+					case 1: stksize = 4; break;
+					case 0: stksize = 1; break;
+				}
+				switch (stksize) {
+					case 7:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] >> (7<<3);cpu->sp[thread]--;
+					case 6:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] >> (6<<3);cpu->sp[thread]--;
+					case 5:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] >> (5<<3);cpu->sp[thread]--;
+					case 4:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] >> (4<<3);cpu->sp[thread]--;
+					case 3:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] >> (3<<3);cpu->sp[thread]--;
+					case 2:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] >> (2<<3);cpu->sp[thread]--;
+					case 1:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] >> (1<<3);cpu->sp[thread]--;
+					case 0:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] & 0xFF;   cpu->sp[thread]--;
 				}
 				cpu->pc[thread] = address;
 				break;
 			case INC_AB: /* INC Absolute. */
 			case INC_Z: /* INC Zero Matrix. */
 				addr[address]++;
-				cpu->z[thread] = (addr[address] == 0);
-				cpu->n[thread] = (addr[address] >> 7);
-				setflag(cpu->z[thread], Z);
-				setflag(cpu->n[thread], N);
+				setflag(addr[address] == 0, Z);
+				setflag(addr[address] >> 7, N);
 				break;
 			case NOP: /* No OPeration. */
 				break;
 			case RTL: /* ReTurn from subroutine Long. */
-				cpu->sp[thread]++;cpu->pc[thread]  = addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]];
-				cpu->sp[thread]++;cpu->pc[thread] += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << 8;
+				tmp2 = 1;
 				switch (addrsize) {
-					case 1:
-						cpu->sp[thread]++;cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << 16;
-						cpu->sp[thread]++;cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << 24;
-						cpu->sp[thread]++;cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << 32;
-					case 2:
-						cpu->sp[thread]++;cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << 40;
-						cpu->sp[thread]++;cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << 48;
-					case 3:
-						cpu->sp[thread]++;cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << 56;
+					case 3: stksize = 7; break;
+					case 2: stksize = 6; break;
+					case 1: stksize = 4; break;
+					case 0: stksize = 0; break;
 				}
-				/*for (uint8_t i = 0; i < stksize; i+=8) {
-					cpu->sp[thread]++;
-					if (i)
-						cpu->pc[thread] += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << i;
-					else
-						cpu->pc[thread] = addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]];
-				}*/
+				cpu->sp[thread] += 2;
+				stksize -= 2;
+				cpu->pc[thread]  = addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-1)] & 0xFF;
+				cpu->pc[thread] += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << 8;
+				switch (stksize) {
+					case 5:	cpu->sp[thread]++;tmp2++;cpu->pc[thread] += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
+					case 4:	cpu->sp[thread]++;tmp2++;cpu->pc[thread] += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
+					case 3:	cpu->sp[thread]++;tmp2++;cpu->pc[thread] += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
+					case 2:	cpu->sp[thread]++;tmp2++;cpu->pc[thread] += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
+					case 1:	cpu->sp[thread]++;tmp2++;cpu->pc[thread] += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
+					case 0:	cpu->sp[thread]++;tmp2++;cpu->pc[thread] += addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
+				}
+				break;
 				break;
 			case DEC_AB: /* DEC Absolute. */
 			case DEC_Z: /* DEC Zero Matrix. */
 				addr[address]--;
-				cpu->z[thread] = (addr[address] == 0);
-				cpu->n[thread] = (addr[address] >> 7);
-				setflag(cpu->z[thread], Z);
-				setflag(cpu->n[thread], N);
+				setflag(addr[address] == 0, Z);
+				setflag(addr[address] >> 7, N);
 				break;
 			case BRK: /* BReaK. */
-				for (int8_t i = stksize; i >= 0; i-=8) {
-					if (i)
-						addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] >> i;
-					else
-						addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread] & 0xFF;
-					cpu->sp[thread]--;
-				}
-				addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->ps >> thread << 3;
-				cpu->sp[thread]--;
-				cpu->i[thread] = 1;
-				setflag(cpu->i[thread], I);
-				cpu->pc[thread] = (uint64_t)addr[0xFFE0]
-						| (uint64_t)addr[0xFFE1] << 8
-						| (uint64_t)addr[0xFFE2] << 16
-						| (uint64_t)addr[0xFFE3] << 24
-						| (uint64_t)addr[0xFFE4] << 32
-						| (uint64_t)addr[0xFFE5] << 40
-						| (uint64_t)addr[0xFFE6] << 48
-						| (uint64_t)addr[0xFFE7] << 56;
+				addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]]     = (uint64_t)cpu->pc[thread] >> 56;
+				addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-1)] = (uint64_t)cpu->pc[thread] >> 48;
+				addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-2)] = (uint64_t)cpu->pc[thread] >> 40;
+				addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-3)] = (uint64_t)cpu->pc[thread] >> 32;
+				addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-4)] = (uint64_t)cpu->pc[thread] >> 24;
+				addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-5)] = (uint64_t)cpu->pc[thread] >> 16;
+				addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-6)] = (uint64_t)cpu->pc[thread] >> 8;
+				addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-7)] = (uint64_t)cpu->pc[thread] & 0xFF;
+				addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-8)] = (uint64_t)cpu->ps >> (thread << 3);
+				cpu->sp[thread] -= 9;
+				setflag(1, I);
+				cpu->pc[thread]  = (uint64_t)addr[0xFFE0];
+				cpu->pc[thread]	+= (uint64_t)addr[0xFFE1] << 8;
+				cpu->pc[thread] += (uint64_t)addr[0xFFE2] << 16;
+				cpu->pc[thread] += (uint64_t)addr[0xFFE3] << 24;
+				cpu->pc[thread] += (uint64_t)addr[0xFFE4] << 32;
+				cpu->pc[thread] += (uint64_t)addr[0xFFE5] << 40;
+				cpu->pc[thread] += (uint64_t)addr[0xFFE6] << 48;
+				cpu->pc[thread] += (uint64_t)addr[0xFFE7] << 56;
 				break;
 			case WAI: /* WAit for Interrupt. */
 				wai = 1;
@@ -1559,6 +1403,7 @@ void *run(void *args) {
 				break;
 		}
 		ins++;
+		step = addr[STEP_ADDR];
 		if (step) {
 			pthread_mutex_lock(&main_mutex);
 			pthread_cond_signal(&main_cond);
@@ -1566,27 +1411,23 @@ void *run(void *args) {
 			pthread_mutex_lock(&mutex);
 			pthread_cond_wait(&cond, &mutex);
 			pthread_mutex_unlock(&mutex);
+			#if debug && !bench
+			wrefresh(scr);
+			#endif
 		}
 		if (!addr[CTRL_ADDR])
 			kbd_ln = 0;
 		else
 			kbd_ln = 1;
 
-		/*if (kbd_ln)
-			usleep(16666);
-			usleep(500000);*/
 	#if debug && !bench
 		#if keypoll
 		pthread_mutex_lock(&mutex);
 		#endif
-		/*mvwprintw(scr, getmaxy(scr)-lines, 0, "Operand: $%llx"
-				", $%04llx: $%02x, $%04llx: $%02x"
-				", $1000: $%02x, $1001: $%02x    "
-				, value
-				, RX_ADDR, addr[RX_ADDR], TX_ADDR, addr[TX_ADDR]
-				, addr[0x1000], addr[0x1001]);*/
 		mvwprintw(scr, (6*thread)+1, 0, "Instructions executed: %llu, Clock cycles: %llu\r", ins, iclk);
-		wrefresh(scr);
+		if (!step) {
+			wrefresh(scr);
+		}
 		#if keypoll
 		pthread_mutex_unlock(&mutex);
 		#endif
@@ -1702,6 +1543,9 @@ int main(int argc, char **argv) {
 				step_key = 0;
 				addr[CTRL_ADDR] = 0;
 				kbd_ln = 0;
+				#if !debug
+				wrefresh(scr);
+				#endif
 			}
 		}
 		#if keypoll
@@ -1727,12 +1571,12 @@ int main(int argc, char **argv) {
 					if (kbd_rdy && c < 0x100) {
 						addr[RX_ADDR] = (uint8_t)c;
 						addr[CTRL_ADDR] = 1;
-					#if debug && !bench
+						#if debug && !bench
 						wmove(scr, getmaxy(scr)-1, 0);
 						wprintw(scr, "c: %i, x: %i, y: %i, i: %i.", c, x, y, i++);
 						wmove(scr, y, x);
 						wrefresh(scr);
-					#endif
+						#endif
 					}
 					if (c == '\n')
 						kbd_ln = 1;