Made both the assembly language monitor, and the

emulator smaller, and faster.

I am also starting to make SuBEditor's source code
more readable.

1 files changed, 266 insertions, 288 deletions

diff --git a/sux.c b/sux.c
index 6c9442f..4dffc37 100644
--- a/sux.c
+++ b/sux.c
@@ -6,6 +6,7 @@
 #define bench 0
 #define debug 0
 #define IO 1
+#define getclk 0
 #define keypoll 0
 #if bench
 #include <sys/time.h>
@@ -14,7 +15,7 @@
 #endif
 
 #define THREADS	1
-#define BENCH_INST 100000000 << THREADS-1
+#define BENCH_INST 100000000 << (THREADS-1)
 #define CTRL_ADDR 0xC000
 #define TX_ADDR 0xC001
 #define RX_ADDR 0xC002
@@ -24,13 +25,16 @@
 #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. */
 uint64_t tclk;		/* Total Clock cycles. */
+#endif
+
+const uint16_t tv = 0xFF50; /* Starting address of the Thread Vectors. */
 uint64_t inst[THREADS];
 uint64_t inss;
 uint8_t threads_done = 0;
 uint8_t kbd_rdy = 0;
-uint8_t wai = 0;
 uint8_t step = 0;
 uint8_t irq = 0;
 
@@ -58,23 +62,19 @@ void *run(void *args) {
 	struct sux *cpu = &thr->sx;
 	uint8_t thread = thr->th;
 	uint64_t address = 0;
-	uint64_t tmpaddr = 0;
 	uint8_t prefix = 0;
 	uint8_t opcode = 0;
 	uint8_t end = 0;
-	uint8_t stksize;
 	uint64_t sum = 0;
 	uint64_t value = 0;
 	uint64_t reg = 0;
+	#if getclk
 	uint64_t iclk = 0;
+	#endif
 	uint64_t ins = 0;
 	uint64_t sign = 0;
-	uint8_t addrsize;
-	uint8_t rs;
-	uint8_t regsize;
 	uint8_t tmp;
 	uint8_t tmp2;
-	char *s = malloc(2048);
 #if !bench
 	uint8_t lines = (6*thread)+2;
 	uint8_t bcd[4];
@@ -82,13 +82,15 @@ void *run(void *args) {
 	int x = 0, y = 0;
 	uint8_t esc = 0;
 #endif
-	uint16_t tv = 0xFF50; /* Starting address of the Thread Vectors. */
 #if bench
 	gettimeofday(&str[thread], 0);
 #endif
 #if debug && !bench
 	uint8_t scr_row = 0xFF, scr_col = 0xFF;
 	uint8_t updt = 0;
+	uint64_t tmpaddr = 0;
+	addr[STEP_ADDR] = 1;
+	step = 1;
 	#if keypoll
 	pthread_mutex_lock(&mutex);
 	#endif
@@ -99,37 +101,13 @@ void *run(void *args) {
 #endif
 	while (!end) {
 		address = 0;
-		if (wai) {
-			for (int8_t i = 56; i >= 0; i-=8) {
-				if (i)
-					addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread]-1 >> i;
-				else
-					addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->pc[thread]-1 & 0xFF;
-				cpu->sp[thread]--;
-			}
-			addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] = (uint64_t)cpu->ps >> 8*thread;
-			cpu->sp[thread]--;
-			setflag(1, I);
-			cpu->pc[thread] = (uint64_t)addr[0xFFA0]
-					| (uint64_t)addr[0xFFA1] << 8
-					| (uint64_t)addr[0xFFA2] << 16
-					| (uint64_t)addr[0xFFA3] << 24
-					| (uint64_t)addr[0xFFA4] << 32
-					| (uint64_t)addr[0xFFA5] << 40
-					| (uint64_t)addr[0xFFA6] << 48
-					| (uint64_t)addr[0xFFA7] << 56;
-			wai = 0;
-			kbd_rdy &= (uint8_t)~(1 << thread);
-		}
 		prefix = addr[cpu->pc[thread]];
-		if ((prefix & 0x03) == 0x03)
-			cpu->pc[thread]++;
-		else
+		if ((prefix & 0x03) != 0x03) {
 			prefix = 0;
+		}
+		cpu->pc[thread] += ((prefix & 0x03) == 0x03);
 		opcode = addr[cpu->pc[thread]];
-
-
-	#if debug && !bench
+		#if debug && !bench
 		if (lines > 24*(thread+1)) {
 			lines = (24*thread)+2;
 		}
@@ -139,11 +117,11 @@ void *run(void *args) {
 		wmove(scr, lines, 0);
 		wclrtoeol(scr);
 		wprintw(scr,
-			"pc: $%08llX"
-			", a: $%016llX"
-			", b: $%016llX"
-			", x: $%016llX"
-			", y: $%016llX"
+			"pc: $%04"PRIX64
+			", a: $%016"PRIX64
+			", b: $%016"PRIX64
+			", x: $%016"PRIX64
+			", y: $%016"PRIX64
 			, cpu->pc[thread]
 			, cpu->a[thread]
 			, cpu->b[thread]
@@ -151,28 +129,23 @@ void *run(void *args) {
 			, cpu->y[thread]);
 		wprintw(scr,
 			", sp: $%04X"
-			", ps: $%04X"
-			", prefix: $%02x"
-			", opcode: $%02x"
+			", ps: $%02"PRIX64
+			/*", prefix: $%02X"
+			", opcode: $%02X"*/
 			", inst: "
 			, cpu->sp[thread]
-			, cpu->ps
-			, prefix
-			, opcode);
+			, cpu->ps);
+			/*, prefix
+			, opcode);*/
 		#if keypoll
 		pthread_mutex_unlock(&mutex);
 		#endif
-	#endif
-
-		addrsize = (prefix & 0x0C) >> 2;
-		rs = (prefix & 0x30) >> 4;
-		if (rs)
-			regsize = (1 << rs);
-		else
-			regsize = 1;
+		#endif
 		address = cpu->pc[thread];
-		cpu->pc[thread]++;
-		iclk++;
+		++cpu->pc[thread];
+		#if getclk
+		++iclk;
+		#endif
 		switch (optype[opcode]) {
 			case IMPL:
 				break;
@@ -198,11 +171,11 @@ void *run(void *args) {
 					case ASR:
 					case ENT:
 						address = cpu->pc[thread];
-						cpu->pc[thread]+=1;
+						++cpu->pc[thread];
 						break;
 					default:
 						address = cpu->pc[thread];
-						cpu->pc[thread]+=regsize;
+						cpu->pc[thread]+=(1 << (prefix >> 4));
 						break;
 				}
 				break;
@@ -215,37 +188,44 @@ void *run(void *args) {
 				tmp = 0;
 				address = addr[cpu->pc[thread]];
 				/* Unroll Loop by implementing Duff's Device. */
-				switch (addrsize) {
+				switch ((prefix & 0x0C) >> 2) {
 					case 2:
-						address |= (uint64_t)addr[cpu->pc[thread]+5] << 40;
-						address |= (uint64_t)addr[cpu->pc[thread]+4] << 32;
-						tmp+=2;
+						address |= (uint64_t)addr[cpu->pc[thread]+5] << 40;++tmp;
+						address |= (uint64_t)addr[cpu->pc[thread]+4] << 32;++tmp;
 					case 3:
-						address |= addr[cpu->pc[thread]+3] << 24;
-						tmp+=1;
+						address |= (uint64_t)addr[cpu->pc[thread]+3] << 24;++tmp;
 					case 1:
-						address |= addr[cpu->pc[thread]+2] << 16;
-						address |= addr[cpu->pc[thread]+1] << 8;
-						tmp+=2;
+						address |= (uint64_t)addr[cpu->pc[thread]+2] << 16;++tmp;
+						address |= (uint64_t)addr[cpu->pc[thread]+1] <<  8;++tmp;
 					case 0:
-						tmp+=1;
+						++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? */
@@ -253,19 +233,23 @@ void *run(void *args) {
 							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  = addr[address];
-						value += addr[address+1] << 8;
-						value += addr[address+2] << 16;
-						value += addr[address+3] << 24;
+						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;
@@ -275,25 +259,23 @@ void *run(void *args) {
 				break;
 			case ABS:
 				tmp = 0;
-				address = addr[cpu->pc[thread]];
+				address = addr[cpu->pc[thread]];++tmp;
 				/* Unroll Loop by implementing Duff's Device. */
-				switch (addrsize) {
+				switch ((prefix & 0x0C) >> 2) {
 					case 3:
-						address |= (uint64_t)addr[cpu->pc[thread]+7] << 56;
-						tmp+=1;
+						address |= (uint64_t)addr[cpu->pc[thread]+7] << 56;++tmp;
 					case 2:
-						address |= (uint64_t)addr[cpu->pc[thread]+6] << 48;
-						address |= (uint64_t)addr[cpu->pc[thread]+5] << 40;
-						tmp+=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;
-						address |= addr[cpu->pc[thread]+3] << 24;
-						address |= addr[cpu->pc[thread]+2] << 16;
-						tmp+=3;
+						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 |= addr[cpu->pc[thread]+1] << 8;
-						tmp+=2;
+						address |= (uint64_t)addr[cpu->pc[thread]+1] <<  8;++tmp;
 				}
 				cpu->pc[thread]+=tmp;
 				break;
@@ -301,17 +283,17 @@ void *run(void *args) {
 		}
 		value = addr[address];
 		/* Unroll Loop by implementing Duff's Device. */
-		switch (regsize) {
+		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 |= addr[address+3] << 24;
-				value |= addr[address+2] << 16;
+				value |= (uint64_t)addr[address+3] << 24;
+				value |= (uint64_t)addr[address+2] << 16;
 			case 2:
-				value |= addr[address+1] << 8;
+				value |= (uint64_t)addr[address+1] << 8;
 		}
 	#if debug && !bench
 		#if keypoll
@@ -323,7 +305,7 @@ void *run(void *args) {
 		op[1] = opname[opcode][1];
 		op[2] = opname[opcode][2];
 		op[3] = '\0';
-		switch(regsize) {
+		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;
@@ -332,11 +314,11 @@ void *run(void *args) {
 		switch (optype[opcode]) {
 			case IMPL: wprintw(scr, "%s\r" , opname[opcode]); break;
 			case IMM:
-				switch(regsize) {
+				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 #$%016llX\r" , op, postfix, value); break;
+					case 8: wprintw(scr, "%s%s #$%016"PRIX64"\r" , op, postfix, value); break;
 				}
 				break;
 			case ZM:
@@ -346,9 +328,9 @@ void *run(void *args) {
 					case ZMX: tmpaddr = address - cpu->x[thread]; break;
 					case ZMY: tmpaddr = address - cpu->y[thread]; break;
 				}
-				switch (addrsize) {
+				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 $%014llX%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;
 				}
@@ -356,30 +338,33 @@ void *run(void *args) {
 			case IND:
 			case INDX:
 			case INDY:
-				switch (addrsize) {
+				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 ($%012llX%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 (addrsize) {
-					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;
+				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;
 		}
-		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[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]);
+
+		if (updt) {
+			mvwprintw(scr, 27, 0, "TX_ADDR: $%02X, RX_ADDR: $%02X\r", addr[TX_ADDR], addr[RX_ADDR]);
+			mvwprintw(scr, 28, 0, "scr_ptr3: $%04X", (addr[0x5C] << 8) | addr[0x5B]);
+			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[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[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;
@@ -414,17 +399,17 @@ void *run(void *args) {
 			}
 			updt = 0;
 		}
-		/*tmpad = 0x4000;
+		/*ln = 45;
+		tmpad = 0x4000;
 		line_idx = 0;
 		mvwprintw(scr, ln++, 0, "cmd_buf:\r");
-		for (uint8_t i = 0; i < 20; i++) {
-			wmove(scr, ln);
+		for (uint8_t i = 0; i < 5; i++) {
+			wmove(scr, ln++, 0);
 			line_idx = (i << 4)+(i << 6);
-			for (uint8_t j = 0; j < 0x10; j++) {
-				wprintw(scr, "%02X ", addr[tmpad+j+line_idx]);
+			for (uint8_t j = 0; j < 0x50; j++) {
+				wprintw(scr, "%02X", addr[tmpad+j+line_idx]);
 			}
 			wprintw(scr, ", i: %02X\r", i);
-			ln++;
 		}*/
 		#if keypoll
 		pthread_mutex_unlock(&mutex);
@@ -433,7 +418,7 @@ void *run(void *args) {
 	#endif
 		switch(opcode) {
 			case CPS: /* Clear Processor Status. */
-				cpu->ps &= 0;
+				cpu->ps = 0;
 				break;
 			case AAB: /* Add Accumulator with carry by B register. */
 				value = cpu->b[thread]; /* Falls Through. */
@@ -452,7 +437,7 @@ void *run(void *args) {
 			case PHA: /* PusH Accumulator to stack. */
 			case PHY: /* PusH Y register to stack. */
 			case PHX: /* PusH X register to stack. */
-				tmp = value;
+				tmp = (value <= 7) ? value : 7;
 				switch (opcode) {
 					case PHA: reg = cpu->a[thread]; break;
 					case PHB: reg = cpu->b[thread]; break;
@@ -460,18 +445,16 @@ void *run(void *args) {
 					case PHY: reg = cpu->y[thread]; break;
 					case PHP: reg = cpu->ps;	break;
 				}
-				if (tmp > 7)
-					tmp = 7;
 				/* Unroll Loop by implementing Duff's Device. */
 				switch (tmp) {
-					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;
+					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]--;
 				}
 				break;
 			case TAY: /* Transfer Accumulator to Y. */
@@ -527,27 +510,19 @@ void *run(void *args) {
 			case PLA: /* PuLl Accumulator from stack. */
 			case PLY: /* PuLl Y register from stack. */
 			case PLX: /* PuLl X register from stack. */
-				tmp = value;
-				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;
+				tmp = (value <= 7) ? value : 7;
+				reg = 0;
 				tmp2 = 0;
-				cpu->sp[thread]++;reg = addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] & 0xFF;tmp--;
+				cpu->sp[thread]++;reg = addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] & 0xFF;
 				/* Unroll Loop by implementing Duff's Device. */
 				switch (tmp) {
-					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);
+					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;
@@ -557,30 +532,14 @@ void *run(void *args) {
 					case PLP: cpu->ps = reg; 	break;
 				}
 				break;
-			case JSR_IN: /* JSR Indirect. */
-			case JSR: /* Jump to SubRoutine. */
-				stksize = adrsize[addrsize];
-				/* Unroll Loop by implementing Duff's Device. */
-				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;
-				}
-				cpu->pc[thread] = address;
-				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(value == 0, Z);
-				setflag(value >> 63, N);
+				setflag(cpu->a[thread] == 0, Z);
+				setflag(cpu->a[thread] >> 63, N);
 				break;
 			case STT: /* STart Thread. */
 				cpu->crt |= value;
@@ -600,8 +559,9 @@ void *run(void *args) {
 				break;
 			case BPO: /* BPO Absolute. */
 			case BPO_Z: /* BPO Zero Matrix. */
-				if (!getflag(N))
+				if (!getflag(N)) {
 					cpu->pc[thread] = address;
+				}
 				break;
 			case OAB: /* bitwise Or with Accumulator, and B register. */
 				value = cpu->b[thread]; /* Falls Through. */
@@ -609,16 +569,17 @@ void *run(void *args) {
 			case ORA_AB: /* ORA Absolute. */
 			case ORA_Z: /* ORA Zero Matrix. */
 				cpu->a[thread] |= value;
-				setflag(value == 0, Z);
-				setflag(value >> 63, N);
+				setflag(cpu->a[thread] == 0, Z);
+				setflag(cpu->a[thread] >> 63, N);
 				break;
 			case SEI: /* SEt Interrupt. */
 				setflag(1, I);
 				break;
 			case BNG: /* BNG Absolute. */
 			case BNG_Z: /* BNG Zero Matrix. */
-				if (getflag(N))
+				if (getflag(N)) {
 					cpu->pc[thread] = address;
+				}
 				break;
 			case XAB: /* bitwise Xor with Accumulator, and B register. */
 				value = cpu->b[thread]; /* Falls Through. */
@@ -626,16 +587,17 @@ void *run(void *args) {
 			case XOR_AB: /* XOR Absolute. */
 			case XOR_Z: /* XOR Zero Matrix. */
 				cpu->a[thread] ^= value;
-				setflag(value == 0, Z);
-				setflag(value >> 63, N);
+				setflag(cpu->a[thread] == 0, Z);
+				setflag(cpu->a[thread] >> 63, N);
 				break;
 			case CLI: /* CLear Interrupt. */
 				setflag(0, I);
 				break;
 			case BCS: /* BCS Absolute. */
 			case BCS_Z: /* BCS Zero Matrix. */
-				if (getflag(C))
+				if (getflag(C)) {
 					cpu->pc[thread] = address;
+				}
 				break;
 			case LLB: /* Logical shift Left accumulator by B. */
 				value = cpu->b[thread]; /* Falls Through. */
@@ -645,7 +607,7 @@ void *run(void *args) {
 				sum = (value < 64) ? cpu->a[thread] << value : 0;
 				setflag(sum == 0, Z);
 				setflag(sum >> 63, N);
-				setflag(cpu->a[thread] >> 64-value, C);
+				setflag(cpu->a[thread] >> (64-value), C);
 				cpu->a[thread] = sum;
 				break;
 			case SEC: /* SEt Carry flag.*/
@@ -774,12 +736,16 @@ void *run(void *args) {
 							case 'S':
 								#if !debug
 								wscrl(scr, -1);
+								#else
+								updt = (!addr[0x16]);
 								#endif
 								esc = 0;
 								break;
 							case 'T':
 								#if !debug
 								wscrl(scr, 1);
+								#else
+								updt = (!addr[0x16]);
 								#endif
 								esc = 0;
 								break;
@@ -852,7 +818,7 @@ void *run(void *args) {
 				}
 				#endif
 				/* Unroll Loop by implementing Duff's Device. */
-				switch (regsize) {
+				switch (1 << (prefix >> 4)) {
 					case 8:
 						addr[address+7] = value >> 56;
 						addr[address+6] = value >> 48;
@@ -864,11 +830,13 @@ void *run(void *args) {
 					case 2:
 						addr[address+1] = value >> 8;
 				}
+				step = addr[STEP_ADDR] || cpu->pc[thread] == CTRL_ADDR;
 				break;
 			case BCC: /* BCC Absolute. */
 			case BCC_Z: /* BCC Zero Matrix. */
-				if (!getflag(C))
+				if (!getflag(C)) {
 					cpu->pc[thread] = address;
+				}
 				break;
 			case LRB: /* Logical shift Right accumulator by B. */
 				value = cpu->b[thread]; /* Falls Through. */
@@ -936,17 +904,17 @@ void *run(void *args) {
 				}
 				value = addr[address];
 				/* Unroll Loop by implementing Duff's Device. */
-				switch (regsize) {
+				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 |= addr[address+3] << 24;
-						value |= addr[address+2] << 16;
+						value |= (uint64_t)addr[address+3] << 24;
+						value |= (uint64_t)addr[address+2] << 16;
 					case 2:
-						value |= addr[address+1] << 8;
+						value |= (uint64_t)addr[address+1] << 8;
 				}
 				switch (opcode) {
 					case LDB:
@@ -990,8 +958,9 @@ void *run(void *args) {
 				break;
 			case BEQ: /* BEQ Absolute. */
 			case BEQ_Z: /* BEQ Zero Matrix. */
-				if (getflag(Z))
+				if (getflag(Z)) {
 					cpu->pc[thread] = address;
+				}
 				break;
 			case RLB: /* Rotate Left accumulator by B. */
 				value = cpu->b[thread]; /* Falls Through. */
@@ -1002,13 +971,14 @@ void *run(void *args) {
 				sum |= getflag(C);
 				setflag(sum == 0, Z);
 				setflag(sum >> 63, N);
-				setflag(cpu->a[thread] >> (uint64_t)64-value, C);
+				setflag(cpu->a[thread] >> (uint64_t)(64-value), C);
 				cpu->a[thread] = sum;
 				break;
 			case BNE: /* BNE Absolute. */
 			case BNE_Z: /* BNE Zero Matrix. */
-				if (!getflag(Z))
+				if (!getflag(Z)) {
 					cpu->pc[thread] = address;
+				}
 				break;
 			case RRB: /* Rotate Right accumulator by B. */
 				value = cpu->b[thread]; /* Falls Through. */
@@ -1016,7 +986,7 @@ void *run(void *args) {
 			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;
+				sum |= (uint64_t)getflag(C) << (uint64_t)(64-value);
 				setflag(sum == 0, Z);
 				setflag(sum >> 63, N);
 				setflag(cpu->a[thread] & 1, C);
@@ -1024,37 +994,38 @@ void *run(void *args) {
 				break;
 			case BVS: /* BVS Absolute. */
 			case BVS_Z: /* BVS Zero Matrix. */
-				if (getflag(V))
+				if (getflag(V)) {
 					cpu->pc[thread] = address;
+				}
 				break;
 			case MAB: /* Multiply Accumulator by B. */
 				value = cpu->b[thread]; /* Falls Through. */
 			case MUL: /* MUL Immediate. */
 			case MUL_AB: /* MUL Absolute. */
 			case MUL_Z: /* MUL Zero Matrix. */
-				sum = cpu->a[thread]*value+getflag(C);
+				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);
-				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 (!getflag(V))
+				if (!getflag(V)) {
 					cpu->pc[thread] = address;
+				}
 				break;
 			case DIV: /* DIV Immediate. */
 			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;
 				}
-				sum = cpu->a[thread]/value;
 				cpu->a[thread] = sum;
 				setflag(sum == 0, Z);
 				setflag((sum >> 63), N);
@@ -1062,19 +1033,6 @@ void *run(void *args) {
 			case CLV: /* CLear oVerflow flag. */
 				setflag(0, V);
 				break;
-			case RTS: /* ReTurn from Subroutine. */
-				tmp2 = 0;
-				stksize = adrsize[addrsize];
-				cpu->sp[thread]++;reg = addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]] & 0xFF;stksize--;
-				/* Unroll Loop by implementing Duff's Device. */
-				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 CAB: /* Compare Accumulator, and B. */
 				value = cpu->b[thread]; /* Falls Through. */
 			case CPB: /* CPB Immediate. */
@@ -1179,19 +1137,18 @@ void *run(void *args) {
 				setflag(reg == 0, Z);
 				setflag(reg >> 63, N);
 				break;
+			case JSR_IN: /* JSR Indirect. */
+			case JSR: /* Jump to SubRoutine. */
 			case JSL: /* Jump to Subroutine Long. */
-				stksize = adrsize[addrsize+4];
-				/* Unroll Loop by implementing Duff's Device. */
-				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]--;
-				}
+				addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]-0] = (uint64_t)cpu->pc[thread] >> (7<<3);
+				addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]-1] = (uint64_t)cpu->pc[thread] >> (6<<3);
+				addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]-2] = (uint64_t)cpu->pc[thread] >> (5<<3);
+				addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]-3] = (uint64_t)cpu->pc[thread] >> (4<<3);
+				addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]-4] = (uint64_t)cpu->pc[thread] >> (3<<3);
+				addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]-5] = (uint64_t)cpu->pc[thread] >> (2<<3);
+				addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]-6] = (uint64_t)cpu->pc[thread] >> (1<<3);
+				addr[(cpu->stk_st[thread] << 16)+cpu->sp[thread]-7] = (uint64_t)cpu->pc[thread]  & (0xFF);
+				cpu->sp[thread] -= 8;
 				cpu->pc[thread] = address;
 				break;
 			case INC_AB: /* INC Absolute. */
@@ -1199,69 +1156,79 @@ void *run(void *args) {
 				addr[address]++;
 				setflag(addr[address] == 0, Z);
 				setflag(addr[address] >> 7, N);
+				step = addr[STEP_ADDR] || cpu->pc[thread] == CTRL_ADDR;
 				break;
 			case NOP: /* No OPeration. */
 				break;
+			case RTS: /* ReTurn from Subroutine. */
 			case RTL: /* ReTurn from subroutine Long. */
-				tmp2 = 1;
-				stksize = adrsize[addrsize+4];
-				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;
-				/* Unroll Loop by implementing Duff's Device. */
-				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;
+				cpu->sp[thread] += 8;
+				cpu->pc[thread]  = (uint64_t)addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-7)]  & (0xFF);
+				cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-6)] << (1<<3);
+				cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-5)] << (2<<3);
+                                cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-4)] << (3<<3);
+                                cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-3)] << (4<<3);
+                                cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-2)] << (5<<3);
+                                cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-1)] << (6<<3);
+                                cpu->pc[thread] += (uint64_t)addr[(cpu->stk_st[thread] << 16)+(cpu->sp[thread]-0)] << (7<<3);
 				break;
 			case DEC_AB: /* DEC Absolute. */
 			case DEC_Z: /* DEC Zero Matrix. */
 				addr[address]--;
 				setflag(addr[address] == 0, Z);
 				setflag(addr[address] >> 7, N);
+				step = addr[STEP_ADDR] || cpu->pc[thread] == CTRL_ADDR;
 				break;
 			case BRK: /* BReaK. */
+			case WAI: /* WAit for Interrupt. */
+				if (opcode == WAI) {
+					pthread_mutex_lock(&main_mutex);
+					pthread_cond_signal(&main_cond);
+					pthread_mutex_unlock(&main_mutex);
+					pthread_mutex_lock(&mutex);
+					pthread_cond_wait(&cond, &mutex);
+					pthread_mutex_unlock(&mutex);
+				}
 				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]-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;
-				pthread_mutex_lock(&main_mutex);
-				pthread_cond_signal(&main_cond);
-				pthread_mutex_unlock(&main_mutex);
-				pthread_mutex_lock(&mutex);
-				pthread_cond_wait(&cond, &mutex);
-				pthread_mutex_unlock(&mutex);
+				if (opcode == BRK) {
+					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;
+				} else {
+					cpu->pc[thread] = (uint64_t)addr[0xFFA0]
+							| (uint64_t)addr[0xFFA1] << 8
+							| (uint64_t)addr[0xFFA2] << 16
+							| (uint64_t)addr[0xFFA3] << 24
+							| (uint64_t)addr[0xFFA4] << 32
+							| (uint64_t)addr[0xFFA5] << 40
+							| (uint64_t)addr[0xFFA6] << 48
+							| (uint64_t)addr[0xFFA7] << 56;
+					kbd_rdy &= (uint8_t)~(1 << thread);
+				}
 				break;
 			default:
 				break;
 		}
 		ins++;
-		step = addr[STEP_ADDR];
 		#if !bench
+		#if debug
+		updt = (!addr[0x16]);
+		#endif
 		if (step) {
 			pthread_mutex_lock(&main_mutex);
 			pthread_cond_signal(&main_cond);
@@ -1278,7 +1245,11 @@ void *run(void *args) {
 		#if keypoll
 		pthread_mutex_lock(&mutex);
 		#endif
-		mvwprintw(scr, (6*thread)+1, 0, "Instructions executed: %llu, Clock cycles: %llu\r", ins, iclk);
+		wmove(scr, (6*thread)+1, 0);
+		wprintw(scr, "Instructions executed: %"PRIu64, ins);
+		#if getclk
+		wprintw(scr, ", Clock cycles: %"PRIu64, iclk);
+		#endif
 		if (!step) {
 			wrefresh(scr);
 		}
@@ -1291,37 +1262,41 @@ void *run(void *args) {
 			pthread_mutex_lock(&main_mutex);
 			threads_done++;
 			inst[thread] = ins;
+			#if getclk
 			clk[thread] = iclk;
+			#endif
 			pthread_cond_signal(&main_cond);
 			pthread_mutex_unlock(&main_mutex);
 			gettimeofday(&en[thread], 0);
 		}
 		#endif
 	}
-	free(s);
+	return NULL;
 }
 
 int main(int argc, char **argv) {
 	struct suxthr thr[THREADS];
 	char *tmp = malloc(2048);
-	ibcount = 0;
 	addr = malloc(0x04000000);
 	inss = 0;
 	int v = 0;
 
 	if (argc != 2) {
-		if (asmmon("stdin") == 2)
+		if (asmmon("stdin") == 2) {
 			return 0;
+		}
 	} else {
-		if (asmmon(argv[1]) == 2)
+		if (asmmon(argv[1]) == 2) {
 			return 0;
+		}
 	}
 	sprintf(tmp, "\033[2J\033[H");
 	fwrite(tmp, sizeof(char), strlen(tmp), stdout);
 	fflush(stdout);
 	#if !bench
-	if(!scr)
+	if(!scr) {
 		scr = initscr();
+	}
 	nodelay(stdscr, 0);
 	crmode();
 	noecho();
@@ -1338,36 +1313,26 @@ int main(int argc, char **argv) {
 	#endif
 	pthread_t therads[THREADS];
 	int result;
+	uint16_t vec = 0xFFC0;
+	uint8_t offset;
 	for (int i = 0; i < THREADS; i++) {
 		thr[i].sx.sp[i] = 0xFFFF;
 		thr[i].sx.stk_st[i] = i+1;
-		if (i) {
-			thr[i].sx.a[i] = 0;
-			thr[i].sx.b[i] = 0;
-			thr[i].sx.x[i] = 0;
-			thr[i].sx.y[i] = 0;
-			thr[i].sx.pc[i] = (uint64_t)addr[0xFF50+(8*(i-1))]
-					| (uint64_t)addr[0xFF51+(8*(i-1))] << 8
-					| (uint64_t)addr[0xFF52+(8*(i-1))] << 16
-					| (uint64_t)addr[0xFF53+(8*(i-1))] << 24
-					| (uint64_t)addr[0xFF54+(8*(i-1))] << 32
-					| (uint64_t)addr[0xFF55+(8*(i-1))] << 40
-					| (uint64_t)addr[0xFF56+(8*(i-1))] << 48
-					| (uint64_t)addr[0xFF57+(8*(i-1))] << 56;
-		} else {
-			thr[i].sx.a[i] = 0;
-			thr[i].sx.b[i] = 0;
-			thr[i].sx.x[i] = 0;
-			thr[i].sx.y[i] = 0;
-			thr[i].sx.pc[i] = (uint64_t)addr[0xFFC0]
-					| (uint64_t)addr[0xFFC1] << 8
-					| (uint64_t)addr[0xFFC2] << 16
-					| (uint64_t)addr[0xFFC3] << 24
-					| (uint64_t)addr[0xFFC4] << 32
-					| (uint64_t)addr[0xFFC5] << 40
-					| (uint64_t)addr[0xFFC6] << 48
-					| (uint64_t)addr[0xFFC7] << 56;
-		}
+		offset = (i) ? ((i-1) << 3) : 0;
+		vec = (i) ? 0xFF50 : 0xFFC0;
+		thr[i].sx.a[i] = 0;
+		thr[i].sx.b[i] = 0;
+		thr[i].sx.x[i] = 0;
+		thr[i].sx.y[i] = 0;
+		thr[i].sx.pc[i] = (uint64_t)addr[vec+0+offset]
+				| (uint64_t)addr[vec+1+offset] << 8
+				| (uint64_t)addr[vec+2+offset] << 16
+				| (uint64_t)addr[vec+3+offset] << 24
+				| (uint64_t)addr[vec+4+offset] << 32
+				| (uint64_t)addr[vec+5+offset] << 40
+				| (uint64_t)addr[vec+6+offset] << 48
+				| (uint64_t)addr[vec+7+offset] << 56;
+
 		thr[i].th = i;
 		inst[i] = 0;
 		result = pthread_create(&therads[i], NULL, run, &thr[i]);
@@ -1403,8 +1368,9 @@ int main(int argc, char **argv) {
 		c = wgetch(scr);
 		if (c == 19) {
 			step = 1;
-			if (kbd_rdy)
+			if (kbd_rdy) {
 				c = wgetch(scr);
+			}
 		}
 		if (kbd_rdy) {
 			switch (c) {
@@ -1430,11 +1396,9 @@ int main(int argc, char **argv) {
 					break;
 			}
 		} else {
-			if ((c == 19 || c == 18) && step) {
-				if (c == 18)
-					step = 0;
-				else if (c == 19)
-					step_key = 1;
+			if (step) {
+				step = !(c == 18);
+				step_key = (c == 19);
 				#if !keypoll
 				pthread_mutex_lock(&mutex);
 				pthread_cond_signal(&cond);
@@ -1455,8 +1419,10 @@ int main(int argc, char **argv) {
 #if bench
 	if (threads_done == THREADS) {
 		double tm_sec, tm_usec, tm[THREADS], ttm;
+		#if getclk
 		double clkspd;
 		double mhz;
+		#endif
 		double ips[THREADS];
 		double ipst;
 		for (int i = 0; i < THREADS; i++) {
@@ -1468,21 +1434,33 @@ int main(int argc, char **argv) {
 				inss += inst[i];
 				ttm += tm[i];
 				ipst += ips[i];
+				#if getclk
 				tclk += clk[i];
+				#endif
 			} else {
 				inss = inst[i];
 				ttm = tm[i];
 				ipst = ips[i];
+				#if getclk
 				tclk = clk[i];
+				#endif
 			}
+			#if getclk
 			clkspd = (tm[i]/1000000)*1000000/clk[i];
 			mhz = 1000000.0/clkspd/1000000;
-			sprintf(tmp, "Instructions executed for thread %i: %llu, Instructions per Second for thread %i in MIPS: %f, tm: %f\n", i, inst[i], i, ips[i], tm[i]/1000000);
+			#endif
+			sprintf(tmp, "Instructions executed for thread %i: %"PRIu64", Instructions per Second for thread %i in MIPS: %f, tm: %f\n", i, inst[i], i, ips[i], tm[i]/1000000);
 			fwrite(tmp, sizeof(char), strlen(tmp), stdout);
 		}
+		sprintf(tmp, "Total Instructions executed: %"PRIu64", Total Instructions per Second in MIPS: %f", inss, ipst);
+		fwrite(tmp, sizeof(char), strlen(tmp), stdout);
+		#if getclk
 		clkspd = (ttm/1000000)*1000000/tclk;
 		mhz = 1000000.0/clkspd/1000000;
-		sprintf(tmp, "Total Instructions executed: %llu, Total Instructions per Second in MIPS: %f, Clock cycles: %llu, Clock Speed in MHz: %f, tm: %f\n", inss, ipst, tclk, mhz, ttm/1000000);
+		sprintf(tmp, ", Clock cycles: %"PRIu64", Clock Speed in MHz: %f", tclk, mhz);
+		fwrite(tmp, sizeof(char), strlen(tmp), stdout);
+		#endif
+		sprintf(tmp, ", tm: %f\n", ttm/1000000);
 		fwrite(tmp, sizeof(char), strlen(tmp), stdout);
 		fflush(stdout);
 		free(tmp);