Put the instruction, and I/O routines into separate

functions.

I did this to make it more readable, while still
making it fast, due to inlining it.

1 files changed, 44 insertions, 777 deletions

diff --git a/sux.c b/sux.c
index fe90728..1743fc0 100644
--- a/sux.c
+++ b/sux.c
@@ -1,25 +1,7 @@
-#include "opcode.h"
+#include "sux.h"
 #include <assert.h>
 #include <ctype.h>
 #include <string.h>
-#include <pthread.h>
-
-#if bench
-#include <sys/time.h>
-#else
-#include <curses.h>
-#endif
-
-#define THREADS	1
-#define BENCH_INST 100000000 << (THREADS-1)
-#define CTRL_ADDR 0xC000
-#define TX_ADDR 0xC001
-#define RX_ADDR 0xC002
-#define STEP_ADDR 0xC010
-#define CURSES_BACKSPACE 0x7F
-
-#define setflag(flag, bit) ((flag)) ? (cpu->ps |= (bit << (thread << 3))) : (cpu->ps &= ~(bit << (thread << 3)))
-#define getflag(bit) (cpu->ps & (bit << (thread << 3)))
 
 #if getclk
 uint64_t clk[THREADS];	/* Per Thread Clock cycles. */
@@ -32,28 +14,18 @@ const uint16_t tv = 0xFF50; /* Starting address of the Thread Vectors. */
 uint64_t inst[THREADS];
 #endif
 
-#if debug
-uint8_t subdbg = 0;
-#endif
-
 #if bench
 uint64_t inss;
 #endif
 
-uint8_t threads_done = 0;
-uint8_t kbd_rdy = 0;
-uint8_t step = 0;
-
-
-#if !bench
-WINDOW *scr;
-#endif
-
 pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
 pthread_mutex_t main_mutex = PTHREAD_MUTEX_INITIALIZER;
 pthread_cond_t cond = PTHREAD_COND_INITIALIZER;
 pthread_cond_t main_cond = PTHREAD_COND_INITIALIZER;
 
+uint8_t threads_done = 0;
+uint8_t step = 0;
+
 struct suxthr {
 	struct sux sx;
 	uint8_t th;
@@ -71,31 +43,18 @@ void *run(void *args) {
 	uint64_t address = 0;
 	uint8_t prefix = 0;
 	uint8_t opcode = 0;
-	uint64_t sum = 0;
+	uint8_t esc = 0;
 	uint64_t value = 0;
-	uint64_t reg = 0;
 	#if getclk
 	uint64_t iclk = 0;
 	#endif
 	#if !IO
 	uint64_t ins = 0;
 	#endif
-	uint8_t sign = 0;
-	uint8_t tmp;
-	uint8_t tmp2;
 #if !bench
 	uint8_t lines = (6*thread)+2;
-	uint8_t bcd[4];
-	uint8_t idx = 3, iscol = 0;
-	int x = 0, y = 0;
-	uint8_t esc = 0;
-#endif
-#if bench
-	gettimeofday(&str[thread], 0);
 #endif
 #if debug && !bench
-	uint64_t tmpaddr = 0;
-	uint16_t scr_col = 0;
 	if (!subdbg) {
 		addr[STEP_ADDR] = 1;
 		step = 1;
@@ -108,6 +67,10 @@ void *run(void *args) {
 	pthread_mutex_unlock(&mutex);
 	#endif
 #endif
+	uint64_t tmpaddr = 0;
+#if bench
+	gettimeofday(&str[thread], 0);
+#endif
 	for (;;) {
 		address = 0;
 		prefix = addr[cpu->pc[thread]];
@@ -151,141 +114,7 @@ void *run(void *args) {
 		#if getclk
 		++iclk;
 		#endif
-		switch (optype[opcode]) {
-			case IMPL:
-				break;
-			case IMM:
-				switch (opcode) {
-					case TXS:
-						break;
-					case PHB:
-					case PHP:
-					case PHA:
-					case PHY:
-					case PHX:
-					case PLB:
-					case PLP:
-					case PLA:
-					case PLY:
-					case PLX:
-					case STT:
-					case LSL:
-					case LSR:
-					case ROL:
-					case ROR:
-					case ASR:
-					case ENT:
-						address = cpu->pc[thread];
-						++cpu->pc[thread];
-						break;
-					default:
-						address = cpu->pc[thread];
-						cpu->pc[thread]+=(1 << (prefix >> 4));
-						break;
-				}
-				break;
-			case ZM:
-			case ZMX:
-			case ZMY:
-			case IND:
-			case INDX:
-			case INDY:
-				tmp = 0;
-				address = addr[cpu->pc[thread]];
-				/* Unroll Loop by implementing Duff's Device. */
-				switch ((prefix & 0x0C) >> 2) {
-					case 2:
-						address |= (uint64_t)addr[cpu->pc[thread]+5] << 40;++tmp;
-						address |= (uint64_t)addr[cpu->pc[thread]+4] << 32;++tmp;
-					case 3:
-						address |= (uint64_t)addr[cpu->pc[thread]+3] << 24;++tmp;
-					case 1:
-						address |= (uint64_t)addr[cpu->pc[thread]+2] << 16;++tmp;
-						address |= (uint64_t)addr[cpu->pc[thread]+1] <<  8;++tmp;
-					case 0:
-						++tmp;
-				}
-				cpu->pc[thread]+=tmp;
-				#if debug && !bench
-				tmpaddr = address;
-				#endif
-				#if getclk
-				iclk++;
-				#endif
-				reg = 0;
-				switch (optype[opcode]) {
-					case ZMX:
-						address += cpu->x[thread];
-						#if getclk
-						iclk++;
-						#endif
-						break;
-					case ZMY:
-						address += cpu->y[thread];
-						#if getclk
-						iclk++;
-						#endif
-						break;
-					case INDX:
-						address += cpu->x[thread];
-						#if getclk
-						iclk++;
-						#endif
-						/* Falls Through. */
-					case INDY:
-						/* Did we fall through? */
-						if (optype[opcode] == INDX) {
-							reg = 0; /* Yes, so set reg back to zero. */
-						} else {
-							reg = cpu->y[thread]; /* No, so set reg to Y. */
-							#if getclk
-							iclk++;
-							#endif
-						}
-						/* Falls Through. */
-					case IND:
-						value  = (uint64_t)addr[address];
-						value += (uint64_t)addr[address+1] << 8;
-						value += (uint64_t)addr[address+2] << 16;
-						value += (uint64_t)addr[address+3] << 24;
-						value += (uint64_t)addr[address+4] << 32;
-						value += (uint64_t)addr[address+5] << 40;
-						value += (uint64_t)addr[address+6] << 48;
-						value += (uint64_t)addr[address+7] << 56;
-						#if getclk
-						iclk++;
-						#endif
-						value += reg;
-						address = value;
-						value = 0;
-						reg = 0;
-						break;
-				}
-				break;
-			case ABS:
-				tmp = 0;
-				address = addr[cpu->pc[thread]];++tmp;
-				/* Unroll Loop by implementing Duff's Device. */
-				switch ((prefix & 0x0C) >> 2) {
-					case 3:
-						address |= (uint64_t)addr[cpu->pc[thread]+7] << 56;++tmp;
-					case 2:
-						address |= (uint64_t)addr[cpu->pc[thread]+6] << 48;++tmp;
-						address |= (uint64_t)addr[cpu->pc[thread]+5] << 40;++tmp;
-						#if getclk
-						iclk++;
-						#endif
-					case 1:
-						address |= (uint64_t)addr[cpu->pc[thread]+4] << 32;++tmp;
-						address |= (uint64_t)addr[cpu->pc[thread]+3] << 24;++tmp;
-						address |= (uint64_t)addr[cpu->pc[thread]+2] << 16;++tmp;
-					case 0:
-						address |= (uint64_t)addr[cpu->pc[thread]+1] <<  8;++tmp;
-				}
-				cpu->pc[thread]+=tmp;
-				break;
-
-		}
+		address = get_addr(cpu, &tmpaddr, opcode, prefix, thread);
 		value = addr[address];
 		/* Unroll Loop by implementing Duff's Device. */
 		switch (1 << (prefix >> 4)) {
@@ -304,138 +133,15 @@ void *run(void *args) {
 		#if keypoll
 		pthread_mutex_lock(&mutex);
 		#endif
-		char postfix[3];
-		char op[4];
-		op[0] = opname[opcode][0];
-		op[1] = opname[opcode][1];
-		op[2] = opname[opcode][2];
-		op[3] = '\0';
-		switch(1 << (prefix >> 4)) {
-			case 1: postfix[0] =   0; postfix[1] =   0; postfix[2] = 0; break;
-			case 2: postfix[0] = '.'; postfix[1] = 'W'; postfix[2] = 0; break;
-			case 4: postfix[0] = '.'; postfix[1] = 'D'; postfix[2] = 0; break;
-			case 8: postfix[0] = '.'; postfix[1] = 'Q'; postfix[2] = 0; break;
-		}
-		switch (optype[opcode]) {
-			case IMPL: wprintw(scr, "%s\r" , opname[opcode]); break;
-			case IMM:
-				switch(1 << (prefix >> 4)) {
-					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 #$%016"PRIX64"\r" , op, postfix, value); break;
-				}
-				break;
-			case ZM:
-			case ZMX:
-			case ZMY:
-				switch (optype[opcode]) {
-					case ZMX: tmpaddr = address - cpu->x[thread]; break;
-					case ZMY: tmpaddr = address - cpu->y[thread]; break;
-				}
-				switch ((prefix & 0x0C) >> 2) {
-					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 $%014"PRIX64"%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 ((prefix & 0x0C) >> 2) {
-					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 ($%012"PRIX64"%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 ((prefix & 0x0C) >> 2) {
-					case 3: wprintw(scr, "%s%s $%016"PRIX64"\r" , op, postfix, tmpaddr);	break;
-					case 2: wprintw(scr, "%s%s $%014"PRIX64"\r" , op, postfix, tmpaddr);	break;
-					case 1: wprintw(scr, "%s%s $%010"PRIX64"\r" , op, postfix, tmpaddr);	break;
-					case 0: wprintw(scr, "%s%s $%04" PRIX64"\r" , op, postfix, tmpaddr);	break;
-				}
-				break;
-		}
-
-		if (address == TX_ADDR || address == RX_ADDR) {
-			wmove(scr, 27, 0);
-			wprintw(scr, "TX_ADDR: $%02X, RX_ADDR: $%02X", addr[TX_ADDR], addr[RX_ADDR]);
-		}
-		if (subdbg) {
-			uint8_t ln = 33;
-			uint16_t line_idx = 0;
-			uint16_t tmpad = 0x2000;
-			int row, col;
-			uint8_t iscursor = 0;
-			uint64_t ptr;
-			uint8_t adr;
-			wmove(scr, 30, 0);
-			adr = 0x1F;
-			ptr = (uint64_t)addr[adr+0] << 0x00 | (uint64_t)addr[adr+1] << 0x08 | (uint64_t)addr[adr+2] << 0x10 | (uint64_t)addr[adr+3] << 0x18 |
-			      (uint64_t)addr[adr+4] << 0x20 | (uint64_t)addr[adr+5] << 0x28 | (uint64_t)addr[adr+6] << 0x30 | (uint64_t)addr[adr+7] << 0x38;
-			wprintw(scr, "ptr1: $%04"PRIX64, ptr);
-			adr = 0x27;
-			ptr = (uint64_t)addr[adr+0] << 0x00 | (uint64_t)addr[adr+1] << 0x08 | (uint64_t)addr[adr+2] << 0x10 | (uint64_t)addr[adr+3] << 0x18 |
-			      (uint64_t)addr[adr+4] << 0x20 | (uint64_t)addr[adr+5] << 0x28 | (uint64_t)addr[adr+6] << 0x30 | (uint64_t)addr[adr+7] << 0x38;
-			wprintw(scr, ", ptr2: $%04"PRIX64, ptr);
-			adr = 0x2F;
-			ptr = (uint64_t)addr[adr+0] << 0x00 | (uint64_t)addr[adr+1] << 0x08 | (uint64_t)addr[adr+2] << 0x10 | (uint64_t)addr[adr+3] << 0x18 |
-			      (uint64_t)addr[adr+4] << 0x20 | (uint64_t)addr[adr+5] << 0x28 | (uint64_t)addr[adr+6] << 0x30 | (uint64_t)addr[adr+7] << 0x38;
-			wprintw(scr, ", ptr3: $%04"PRIX64, ptr);
-			if (address == CTRL_ADDR || addr[STEP_ADDR]) {
-				mvwprintw(scr, 29, 0, "address: $%04"PRIX64", scr_row: %02u, scr_col: %02u, scr_str: %02u, scr_end: %02u\r", address, addr[0], addr[1], addr[0x1C], addr[0x1D]);
-				mvwprintw(scr, 32, 0, "bitabl: %02X%02X%02X%02X%02X%02X%02X%02X"
-							"%02X%02X%02X%02X%02X%02X%02X%02X"
-							, addr[0x1000], addr[0x1001], addr[0x1002], addr[0x1003], addr[0x1004], addr[0x1005], addr[0x1006], addr[0x1007]
-							, addr[0x1008], addr[0x1009], addr[0x100A], addr[0x100B], addr[0x100C], addr[0x100D], addr[0x100E], addr[0x100F]);
-				mvwprintw(scr, ln++, 0, "buffer:\r");
-				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[0x1C]) == 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", i);
-					if (!iscursor) {
-						wmove(scr, ln, 0);
-						wclrtoeol(scr);
-					}
-					iscursor = 0;
-					wmove(scr, ln++, 0);
-				}
-			}
-			/*if (address == 0x4000 || tmpaddr == 0x4000 || addr[STEP_ADDR]) {
-				ln = 46;
-				tmpad = 0x4000;
-				line_idx = 0;
-				mvwprintw(scr, ln++, 0, "cmd_buf:");
-				for (uint8_t i = 0; i < 5; i++) {
-					wmove(scr, ln++, 0);
-					line_idx = (i << 4)+(i << 6);
-					for (uint8_t j = 0; j < 0x50; j++) {
-						wprintw(scr, "%02X", addr[tmpad+j+line_idx]);
-					}
-					wprintw(scr, ", i: %02X", i);
-				}
-			}*/
-		}
+		uint64_t operands[3];
+		operands[0] = value;
+		operands[1] = address;
+		operands[2] = tmpaddr;
+		disasm(cpu, operands, lines, opcode, prefix, thread);
+		lines+=1;
 		#if keypoll
 		pthread_mutex_unlock(&mutex);
 		#endif
-		lines+=1;
 	#endif
 		switch(opcode) {
 			case CPS: /* Clear Processor Status. */
@@ -446,37 +152,14 @@ void *run(void *args) {
 			case ADC:  /* ADC Immediate. */
 			case ADC_AB: /* ADC Absolute. */
 			case ADC_Z: /* ADC Zero Matrix. */
-				sum = cpu->a[thread]+value+getflag(C);
-				cpu->a[thread] = sum;
-				setflag(sum == 0, Z);
-				setflag((sum >> 63), N);
-				setflag(((cpu->a[thread]^value) >> 63) && ((cpu->a[thread]^sum) >> 63), V);
-				setflag((sum < value), C);
+				adc(cpu, value, thread);
 				break;
 			case PHB: /* PusH B register to stack. */
 			case PHP: /* PusH Processor status to stack. */
 			case PHA: /* PusH Accumulator to stack. */
 			case PHY: /* PusH Y register to stack. */
 			case PHX: /* PusH X register to stack. */
-				tmp = (value <= 7) ? value : 7;
-				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;
-				}
-				/* Unroll Loop by implementing Duff's Device. */
-				switch (tmp) {
-					case 7:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = reg >> (7<<3);cpu->sp[thread]--;
-					case 6:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = reg >> (6<<3);cpu->sp[thread]--;
-					case 5:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = reg >> (5<<3);cpu->sp[thread]--;
-					case 4:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = reg >> (4<<3);cpu->sp[thread]--;
-					case 3:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = reg >> (3<<3);cpu->sp[thread]--;
-					case 2:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = reg >> (2<<3);cpu->sp[thread]--;
-					case 1:	addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = reg >> (1<<3);cpu->sp[thread]--;
-					case 0: addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = reg  & (0xFF);cpu->sp[thread]--;
-				}
+				push(cpu, value, opcode, thread);
 				break;
 			case TAY: /* Transfer Accumulator to Y. */
 			case TAX: /* Transfer Accumulator to Y. */
@@ -488,26 +171,7 @@ void *run(void *args) {
 			case TSX: /* Transfer Stack pointer to X. */
 			case TBA: /* Transfer B to Accumulator. */
 			case TXS: /* Transfer X to Stack pointer. */
-				switch (opcode) {
-                                        case TBA: cpu->a[thread] = cpu->b[thread]; reg = cpu->a[thread]; break;
-                                        case TXA: cpu->a[thread] = cpu->x[thread]; reg = cpu->a[thread]; break;
-                                        case TYA: cpu->a[thread] = cpu->y[thread]; reg = cpu->a[thread]; break;
-                                        case TAB: cpu->b[thread] = cpu->a[thread]; reg = cpu->b[thread]; break;
-					case TAY: cpu->y[thread] = cpu->a[thread]; reg = cpu->y[thread]; break;
-					case TXY: cpu->y[thread] = cpu->x[thread]; reg = cpu->y[thread]; break;
-                                        case TAX: cpu->x[thread] = cpu->a[thread]; reg = cpu->x[thread]; break;
-                                        case TYX: cpu->x[thread] = cpu->y[thread]; reg = cpu->x[thread]; break;
-                                        case TSX: cpu->x[thread] = cpu->sp[thread] & 0xFFFF; cpu->x[thread] = cpu->stk_st[thread] << 16; 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;
-							cpu->pc[thread]+=2;
-						}
-						break;
-				}
-				setflag(reg == 0, Z);
-				setflag(reg >> 63, N);
+				transfer(cpu, value, opcode, prefix, thread);
 				break;
 			case JMP: /* JMP Absolute. */
 			case JMP_Z: /* JMP Zero Matrix. */
@@ -519,48 +183,21 @@ void *run(void *args) {
 			case SBC: /* SBC Immediate. */
 			case SBC_AB: /* SBC Absolute. */
 			case SBC_Z: /* SBC Zero Matrix. */
-				sum = cpu->a[thread]-value-!getflag(C);
-				setflag(sum == 0, Z);
-				setflag(sum >> 63, N);
-				setflag(((cpu->a[thread]^value) >> 63) && ((cpu->a[thread]^sum) >> 63), V);
-				setflag((sum > value), C);
-				cpu->a[thread] = sum;
+				sbc(cpu, value, thread);
 				break;
 			case PLB: /* PuLl B register from stack. */
 			case PLP: /* PuLl Processor status from stack. */
 			case PLA: /* PuLl Accumulator from stack. */
 			case PLY: /* PuLl Y register from stack. */
 			case PLX: /* PuLl X register from stack. */
-				tmp = (value <= 7) ? value : 7;
-				reg = 0;
-				tmp2 = 0;
-				cpu->sp[thread]++;reg = addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] & 0xFF;
-				/* Unroll Loop by implementing Duff's Device. */
-				switch (tmp) {
-					case 7:	cpu->sp[thread]++;tmp2++;reg += (uint64_t)addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
-					case 6:	cpu->sp[thread]++;tmp2++;reg += (uint64_t)addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
-					case 5:	cpu->sp[thread]++;tmp2++;reg += (uint64_t)addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
-					case 4:	cpu->sp[thread]++;tmp2++;reg += (uint64_t)addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
-					case 3:	cpu->sp[thread]++;tmp2++;reg += (uint64_t)addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
-					case 2:	cpu->sp[thread]++;tmp2++;reg += (uint64_t)addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
-					case 1:	cpu->sp[thread]++;tmp2++;reg += (uint64_t)addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] << (tmp2<<3);
-				}
-				switch (opcode) {
-					case PLA: cpu->a[thread] = reg;	break;
-					case PLB: cpu->b[thread] = reg;	break;
-					case PLX: cpu->x[thread] = reg;	break;
-					case PLY: cpu->y[thread] = reg;	break;
-					case PLP: cpu->ps = reg; 	break;
-				}
+				pull(cpu, value, opcode, thread);
 				break;
 			case ABA: /* bitwise And with Accumulator, and B register. */
 				value = cpu->b[thread]; /* Falls Through. */
 			case AND: /* AND Immediate. */
 			case AND_AB: /* AND Absolute. */
 			case AND_Z: /* AND Zero Matrix. */
-				cpu->a[thread] &= value;
-				setflag(cpu->a[thread] == 0, Z);
-				setflag(cpu->a[thread] >> 63, N);
+				and(cpu, value, thread);
 				break;
 			case STT: /* STart Thread. */
 				cpu->crt |= value;
@@ -589,9 +226,7 @@ void *run(void *args) {
 			case ORA: /* ORA Immediate. */
 			case ORA_AB: /* ORA Absolute. */
 			case ORA_Z: /* ORA Zero Matrix. */
-				cpu->a[thread] |= value;
-				setflag(cpu->a[thread] == 0, Z);
-				setflag(cpu->a[thread] >> 63, N);
+				or(cpu, value, thread);
 				break;
 			case SEI: /* SEt Interrupt. */
 				setflag(1, I);
@@ -607,9 +242,7 @@ void *run(void *args) {
 			case XOR: /* XOR Immediate. */
 			case XOR_AB: /* XOR Absolute. */
 			case XOR_Z: /* XOR Zero Matrix. */
-				cpu->a[thread] ^= value;
-				setflag(cpu->a[thread] == 0, Z);
-				setflag(cpu->a[thread] >> 63, N);
+				xor(cpu, value, thread);
 				break;
 			case CLI: /* CLear Interrupt. */
 				setflag(0, I);
@@ -625,11 +258,7 @@ void *run(void *args) {
 			case LSL: /* LSL Immediate. */
 			case LSL_AB: /* LSL Absolute. */
 			case LSL_Z: /* LSL Zero Matrix. */
-				sum = (value < 64) ? cpu->a[thread] << value : 0;
-				setflag(sum == 0, Z);
-				setflag(sum >> 63, N);
-				setflag(cpu->a[thread] >> (64-value), C);
-				cpu->a[thread] = sum;
+				lsl(cpu, value, thread);
 				break;
 			case SEC: /* SEt Carry flag.*/
 				setflag(1, C);
@@ -656,219 +285,7 @@ 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:
-					case STB_ZX:
-					case STB_ZY:
-					case STB_IN:
-					case STB_IX:
-					case STB_IY:
-						value = cpu->b[thread];
-						break;
-					case STA:
-					case STA_Z:
-					case STA_ZX:
-					case STA_ZY:
-					case STA_IN:
-					case STA_IX:
-					case STA_IY:
-						value = cpu->a[thread];
-						break;
-					case STY:
-					case STY_Z:
-					case STY_ZX:
-					case STY_IN:
-						value = cpu->y[thread];
-						break;
-
-					case STX:
-					case STX_Z:
-					case STX_ZY:
-					case STX_IN:
-						value = cpu->x[thread];
-						break;
-				}
-				addr[address] = value & 0xFF;
-				#if (IO || debug) && !branch
-				if (address == TX_ADDR) {
-					#if keypoll
-					pthread_mutex_lock(&mutex);
-					#endif
-					#if debug
-					if (!subdbg) {
-						scr_col = (addr[TX_ADDR] != 0x0C && addr[TX_ADDR] != '\n' && scr_col < 160) ? (addr[1] << 1)-2 : 0;
-						wmove(scr, 28, scr_col);
-					}
-					#endif
-					if (esc) {
-						#if debug
-						if (!subdbg && addr[RX_ADDR] == '\n') {
-							wclrtoeol(scr);
-						}
-						#endif
-						switch (addr[TX_ADDR]) {
-							case 'A':
-								if (y > 0)
-									y--;
-								#if !debug
-								wmove(scr, y, x);
-								#endif
-								esc = 0;
-								break;
-							case 'B':
-								if (y < getmaxy(scr))
-									y++;
-								#if !debug
-								wmove(scr, y, x);
-								#endif
-								esc = 0;
-								break;
-							case 'C':
-								if (x < getmaxx(scr))
-									x++;
-								#if !debug
-								wmove(scr, y, x);
-								#endif
-								esc = 0;
-								break;
-							case 'D':
-								if (x > 0)
-									x--;
-								#if !debug
-								wmove(scr, y, x);
-								#endif
-								esc = 0;
-								break;
-							case 'H':
-								if (!bcd[2] && !bcd[3]) {
-									y = 0;
-								} else {
-									y = ((bcd[3]*10) + bcd[2]);
-								}
-								if (!bcd[0] && !bcd[1]) {
-									x = 0;
-								} else {
-									x = ((bcd[1]*10) + bcd[0]);
-								}
-								#if !debug
-								wmove(scr, y, x);
-								#else
-								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]);*/
-								#endif
-								idx = 3;
-								bcd[0] = 0;
-								bcd[1] = 0;
-								bcd[2] = 0;
-								bcd[3] = 0;
-								esc = 0;
-								break;
-							case 'S':
-								#if !debug
-								wscrl(scr, -1);
-								#else
-								#endif
-								esc = 0;
-								break;
-							case 'T':
-								#if !debug
-								wscrl(scr, 1);
-								#else
-								#endif
-								esc = 0;
-								break;
-							case '0':
-							case '1':
-							case '2':
-							case '3':
-							case '4':
-							case '5':
-							case '6':
-							case '7':
-							case '8':
-							case '9':
-								bcd[idx--] = (addr[address] - '0');
-								break;
-							default:
-								iscol = (addr[address] == ';');
-								break;
-						}
-					} else {
-						switch (addr[TX_ADDR]) {
-							case 0xC:
-								x=0,y=0;
-								#if !debug
-								wclear(scr);
-								wmove(scr, y, x);
-								#endif
-								break;
-							case CURSES_BACKSPACE:
-							case '\b':
-								if (x > 0) {
-									x--;
-									#if !debug
-									wmove(scr, y, x);
-									#endif
-								}
-								#if !debug
-								wdelch(scr);
-								#else
-								if (!subdbg) {
-									scr_col++;
-									wmove(scr, 28, scr_col--);
-									wdelch(scr);
-									wmove(scr, 28, scr_col);
-									wdelch(scr);
-								}
-								#endif
-								break;
-							case '\033':
-								esc = 1;
-								break;
-							case '\n':
-								#if !debug
-								wmove(scr, y, x);
-								waddch(scr, addr[address]);
-								#endif
-								x = 0;
-								y+=1;
-								break;
-							default:
-								#if !debug
-								wmove(scr, y, x);
-								waddch(scr, addr[address]);
-								#else
-								if (!subdbg && scr_col < 160) {
-									if (addr[address] != ' ') {
-										wprintw(scr, "%02X", addr[address]);
-									} else {
-										wprintw(scr, "  ");
-									}
-								}
-								#endif
-								x+=1;
-								break;
-						}
-					}
-					#if keypoll
-					pthread_mutex_unlock(&mutex);
-					#endif
-				}
-				#endif
-				/* Unroll Loop by implementing Duff's Device. */
-				switch (1 << (prefix >> 4)) {
-					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;
-						addr[address+2] = value >> 16;
-					case 2:
-						addr[address+1] = value >> 8;
-				}
+				store(cpu, address, &esc, opcode, prefix, thread);
 				step = addr[STEP_ADDR] || cpu->pc[thread] == CTRL_ADDR;
 				break;
 			case BCC: /* BCC Absolute. */
@@ -882,23 +299,14 @@ void *run(void *args) {
 			case LSR: /* LSR Immediate. */
 			case LSR_AB: /* LSR Absolute. */
 			case LSR_Z: /* LSR Zero Matrix. */
-				sum = (value < 64) ? cpu->a[thread] >> value : 0;
-				setflag(sum == 0, Z);
-				setflag(sum >> 63, N);
-				setflag(cpu->a[thread] & 1, C);
-				cpu->a[thread] = sum;
+				lsr(cpu, value, thread);
 				break;
 			case ARB: /* Arithmetic shift Right accumulator by B. */
 				value = cpu->b[thread]; /* Falls Through. */
 			case ASR: /* ASR Immediate. */
 			case ASR_AB: /* ASR Absolute. */
 			case ASR_Z: /* ASR Zero Matrix. */
-				sign = cpu->a[thread] >> 63;
-				sum = (value < 64) ? (cpu->a[thread] >> value) | ((uint64_t)sign << 63) : 0;
-				setflag(sum == 0, Z);
-				setflag(sum >> 63, N);
-				setflag(cpu->a[thread] & 1, C);
-				cpu->a[thread] = sum;
+				asr(cpu, value, thread);
 				break;
 			case CLC: /* CLear Carry flag. */
 				setflag(0, C);
@@ -929,71 +337,7 @@ void *run(void *args) {
 			case LDA_IX: /* LDA Indexed Indirect. */
 			case LDB_IY: /* LDB Indirect Indexed. */
 			case LDA_IY: /* LDA Indirect Indexed. */
-				if (address == CTRL_ADDR) {
-					kbd_rdy = 1;
-					pthread_mutex_lock(&main_mutex);
-					pthread_cond_signal(&main_cond);
-					pthread_mutex_unlock(&main_mutex);
-					#if !keypoll
-					pthread_mutex_lock(&mutex);
-					pthread_cond_wait(&cond, &mutex);
-					pthread_mutex_unlock(&mutex);
-					#endif
-					kbd_rdy = 0;
-				}
-				value = addr[address];
-				/* Unroll Loop by implementing Duff's Device. */
-				switch (1 << (prefix >> 4)) {
-					case 8:
-						value |= (uint64_t)addr[address+7] << 56;
-						value |= (uint64_t)addr[address+6] << 48;
-						value |= (uint64_t)addr[address+5] << 40;
-						value |= (uint64_t)addr[address+4] << 32;
-					case 4:
-						value |= (uint64_t)addr[address+3] << 24;
-						value |= (uint64_t)addr[address+2] << 16;
-					case 2:
-						value |= (uint64_t)addr[address+1] << 8;
-				}
-				switch (opcode) {
-					case LDB:
-					case LDB_AB:
-					case LDB_Z:
-					case LDB_ZX:
-					case LDB_ZY:
-					case LDB_IN:
-					case LDB_IX:
-					case LDB_IY:
-						cpu->b[thread] = value;
-						break;
-					case LDA:
-					case LDA_AB:
-					case LDA_Z:
-					case LDA_ZX:
-					case LDA_ZY:
-					case LDA_IN:
-					case LDA_IX:
-					case LDA_IY:
-						cpu->a[thread] = value;
-						break;
-					case LDY:
-					case LDY_AB:
-					case LDY_Z:
-					case LDY_ZX:
-					case LDY_IN:
-						cpu->y[thread] = value;
-						break;
-
-					case LDX:
-					case LDX_AB:
-					case LDX_Z:
-					case LDX_ZY:
-					case LDX_IN:
-						cpu->x[thread] = value;
-						break;
-				}
-				setflag(value == 0, Z);
-				setflag(value >> 63, N);
+				load(cpu, address, &esc, opcode, prefix, thread);
 				break;
 			case BEQ: /* BEQ Absolute. */
 			case BEQ_Z: /* BEQ Zero Matrix. */
@@ -1006,12 +350,7 @@ void *run(void *args) {
 			case ROL: /* ROL Immediate. */
 			case ROL_AB: /* ROL Absolute. */
 			case ROL_Z: /* ROL Zero Matrix. */
-				sum = cpu->a[thread] << 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;
+				rol(cpu, value, thread);
 				break;
 			case BNE: /* BNE Absolute. */
 			case BNE_Z: /* BNE Zero Matrix. */
@@ -1024,12 +363,7 @@ void *run(void *args) {
 			case ROR: /* ROR Immediate. */
 			case ROR_AB: /* ROR Absolute. */
 			case ROR_Z: /* ROR Zero Matrix. */
-				sum = cpu->a[thread] >> value;
-				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;
+				ror(cpu, value, thread);
 				break;
 			case BVS: /* BVS Absolute. */
 			case BVS_Z: /* BVS Zero Matrix. */
@@ -1042,11 +376,7 @@ void *run(void *args) {
 			case MUL: /* MUL Immediate. */
 			case MUL_AB: /* MUL Absolute. */
 			case MUL_Z: /* MUL Zero Matrix. */
-				sum = cpu->a[thread]*value;
-				cpu->a[thread] = sum;
-				setflag(sum == 0, Z);
-				setflag(sum >> 63, N);
-				setflag(!((cpu->a[thread]^value) >> 63) && ((cpu->a[thread]^sum) >> 63), V);
+				mul(cpu, value, thread);
 				break;
 			case BVC: /* BVC Absolute. */
 			case BVC_Z: /* BVC Zero Matrix. */
@@ -1058,16 +388,7 @@ void *run(void *args) {
 			case DAB: /* Divide Accumulator by B. */
 			case DIV_AB: /* DIV Absolute. */
 			case DIV_Z: /* DIV Zero Matrix. */
-				sum = cpu->a[thread]/value;
-				if (opcode != DAB) {
-					cpu->b[thread] = cpu->a[thread] % value;
-				} else {
-					value = cpu->b[thread];
-					cpu->x[thread] = cpu->a[thread] % value;
-				}
-				cpu->a[thread] = sum;
-				setflag(sum == 0, Z);
-				setflag((sum >> 63), N);
+				divd(cpu, value, opcode, thread);
 				break;
 			case CLV: /* CLear oVerflow flag. */
 				setflag(0, V);
@@ -1094,43 +415,7 @@ void *run(void *args) {
 			case CPB_IX: /* CPB Indexed Indirect. */
 			case CMP_IY: /* CMP Indirect Indexed. */
 			case CPB_IY: /* CPB Indirect Indexed. */
-				switch (opcode) {
-					case CPB:
-					case CPB_AB:
-					case CPB_Z:
-					case CPB_IN:
-					case CPB_IX:
-					case CPB_IY:
-						reg = cpu->b[thread];
-						break;
-					case CMP:
-					case CAB:
-					case CMP_AB:
-					case CMP_Z:
-					case CMP_IN:
-					case CMP_IX:
-					case CMP_IY:
-						reg = cpu->a[thread];
-						break;
-					case CPY:
-					case CPY_AB:
-					case CPY_Z:
-					case CPY_IN:
-						reg = cpu->y[thread];
-						break;
-
-					case CPX:
-					case CPX_AB:
-					case CPX_Z:
-					case CPX_IN:
-						reg = cpu->x[thread];
-						break;
-				}
-				sum = reg-value;
-				setflag(sum >> 63, N);
-				setflag(((reg^value) >> 63) && ((reg^sum) >> 63), V);
-				setflag(sum == 0, Z);
-				setflag(reg >= value, C);
+				cmp(cpu, value, opcode, thread);
 				break;
 			case ENT: /* ENd Thread. */
 				cpu->crt &= ~value;
@@ -1154,27 +439,13 @@ void *run(void *args) {
 			case INB:
 			case INY:
 			case INX:
-				switch (opcode) {
-					case INC: cpu->a[thread]+=1; reg = cpu->a[thread]; break;
-					case INB: cpu->b[thread]+=1; reg = cpu->b[thread]; break;
-					case INY: cpu->y[thread]+=1; reg = cpu->y[thread]; break;
-					case INX: cpu->x[thread]+=1; reg = cpu->x[thread]; break;
-				}
-				setflag(reg == 0, Z);
-				setflag(reg >> 63, N);
+				incr(cpu, value, opcode, thread);
 				break;
 			case DEC: /* DEC Accumulator. */
 			case DEB:
 			case DEY:
 			case DEX:
-				switch (opcode) {
-					case DEC: cpu->a[thread]-=1; reg = cpu->a[thread]; break;
-					case DEB: cpu->b[thread]-=1; reg = cpu->b[thread]; break;
-					case DEY: cpu->y[thread]-=1; reg = cpu->y[thread]; break;
-					case DEX: cpu->x[thread]-=1; reg = cpu->x[thread]; break;
-				}
-				setflag(reg == 0, Z);
-				setflag(reg >> 63, N);
+				decr(cpu, value, opcode, thread);
 				break;
 			case JSR_IN: /* JSR Indirect. */
 			case JSR: /* Jump to SubRoutine. */
@@ -1192,9 +463,7 @@ void *run(void *args) {
 				break;
 			case INC_AB: /* INC Absolute. */
 			case INC_Z: /* INC Zero Matrix. */
-				addr[address]++;
-				setflag(addr[address] == 0, Z);
-				setflag(addr[address] >> 7, N);
+				incm(cpu, address, thread);
 				step = addr[STEP_ADDR] || cpu->pc[thread] == CTRL_ADDR;
 				break;
 			case NOP: /* No OPeration. */
@@ -1213,9 +482,7 @@ void *run(void *args) {
 				break;
 			case DEC_AB: /* DEC Absolute. */
 			case DEC_Z: /* DEC Zero Matrix. */
-				addr[address]--;
-				setflag(addr[address] == 0, Z);
-				setflag(addr[address] >> 7, N);
+				decm(cpu, address, thread);
 				step = addr[STEP_ADDR] || cpu->pc[thread] == CTRL_ADDR;
 				break;
 			case BRK: /* BReaK. */
@@ -1335,7 +602,6 @@ int main(int argc, char **argv) {
 	sprintf(tmp, "\033[2J\033[H");
 	fwrite(tmp, sizeof(char), strlen(tmp), stdout);
 	fflush(stdout);
-	#if !bench
 	if(!scr) {
 		scr = initscr();
 	}
@@ -1352,7 +618,6 @@ int main(int argc, char **argv) {
 	attron(COLOR_PAIR(1) | A_BOLD);
 	wmove(scr, 0, 0);
 	wrefresh(scr);
-	#endif
 	pthread_t therads[THREADS];
 	int result;
 	uint16_t vec = 0xFFC0;
@@ -1384,10 +649,9 @@ int main(int argc, char **argv) {
 	}
 	int c = 0;
 	uint8_t step_key = 0;
-#if !bench
+	uint8_t end = 0;
 	werase(scr);
-#endif
-	while (threads_done < THREADS) {
+	while (threads_done < THREADS && !end) {
 		#if !bench
 		int x, y;
 		if ((step_key && step && !kbd_rdy) || !step || kbd_rdy) {
@@ -1411,13 +675,15 @@ int main(int argc, char **argv) {
 		#if keypoll
 		pthread_mutex_lock(&mutex);
 		#endif
-		getyx(scr, y, x);
 		c = wgetch(scr);
 		if (c == 19) {
 			if (kbd_rdy) {
 				c = wgetch(scr);
 			}
 			step = 1;
+		} else if (c == 0x11) {
+			end = 1;
+			continue;
 		}
 		if (kbd_rdy) {
 			switch (c) {
@@ -1453,6 +719,7 @@ int main(int argc, char **argv) {
 		pthread_mutex_unlock(&main_mutex);
 		#endif
 	}
+	endwin();
 #if bench
 	if (threads_done == THREADS) {
 		double tm_sec, tm_usec, tm[THREADS], ttm;