- Simplified some parts of the codebase.

- Fixed some bugs with the push, and pull functions
- Fixed a bug in the assembler.

3 files changed, 39 insertions, 41 deletions

diff --git a/assemble.c b/assemble.c
index b8b377e..49137c3 100644
--- a/assemble.c
+++ b/assemble.c
@@ -194,7 +194,7 @@ uint64_t handle_opcode(token *t, bytecount *bc, uint8_t isasm, uint64_t address,
 		if (t->next) {
 			rs = get_rs(t->next, inst, dbg);
 			t = (rs != 0xFF) ? t->next : t;
-			if (t->next && t->next->id == TOK_OF) {
+			if (t->next) {
 				of = get_of(t->next, dbg);
 				t = (of != 0xFF) ? t->next : t;
 			}
@@ -228,6 +228,9 @@ uint64_t handle_opcode(token *t, bytecount *bc, uint8_t isasm, uint64_t address,
 			}
 		}
 		uint64_t saveaddr = address;
+		uint64_t max_val = 0;
+		uint8_t i = 0;
+		uint8_t j = 1;
 		switch (type) {
 			case IMPL:
 				if (prefix) {
@@ -290,23 +293,22 @@ uint64_t handle_opcode(token *t, bytecount *bc, uint8_t isasm, uint64_t address,
 				break;
 			default:
 				if (of != 0xFF) {
-					uint64_t shift = 1;
-					uint8_t i = 8;
-					uint8_t j = 1;
+					i = 8;
 					for (; i <= 64; i += 8, j++) {
-						if ((int64_t)val.u64 >= ~(int64_t)(shift << (i-1)) || (int64_t)val.u64 <= (int64_t)(shift << (i-1))) {
+						max_val |= ((uint64_t)1 << (i-1));
+						if ((int64_t)val.u64 >= ~(int64_t)(max_val) || (int64_t)val.u64 <= (int64_t)(max_val)) {
 							opsize = j;
 							break;
 						}
 					}
 				} else {
-					opsize = (val.u64 >  0x00000000000000FF) ? 2 : opsize;
-					opsize = (val.u64 >  0x000000000000FFFF) ? 3 : opsize;
-					opsize = (val.u64 >  0x0000000000FFFFFF) ? 4 : opsize;
-					opsize = (val.u64 >  0x00000000FFFFFFFF) ? 5 : opsize;
-					opsize = (val.u64 >  0x000000FFFFFFFFFF) ? 6 : opsize;
-					opsize = (val.u64 >  0x0000FFFFFFFFFFFF) ? 7 : opsize;
-					opsize = (val.u64 >  0x00FFFFFFFFFFFFFF) ? 8 : opsize;
+					for (; i <= 64; i += 8, j++) {
+						max_val |= (0xFF << i);
+						if (val.u64 <= max_val) {
+							opsize = j;
+							break;
+						}
+					}
 				}
 				if (type == 0xFF) {
 					switch (opsize-1) {
diff --git a/sux.c b/sux.c
index ae62304..ae99a8b 100644
--- a/sux.c
+++ b/sux.c
@@ -172,6 +172,7 @@ void *run(void *args) {
 		pthread_mutex_unlock(&mutex);
 		#endif
 	#endif
+		uint8_t size = (1 << ((prefix >> 4) & 3))-1;
 		switch(opcode) {
 			case CPS_IMP: /* Clear Processor Status. */
 				cpu->ps.u64 = 0;
@@ -183,11 +184,11 @@ void *run(void *args) {
 			case ADC_Z: /* ADC Zero Matrix. */
 				adc(cpu, value.u64, thread);
 				break;
-			case PHP_IMP: push(cpu, cpu->ps.u64,      0, thread); break; /* PusH Processor status to stack. */
-			case PHA_IMP: push(cpu, cpu->a     , prefix, thread); break; /* PusH Accumulator to stack. */
-			case PHB_IMP: push(cpu, cpu->b     , prefix, thread); break; /* PusH B register to stack. */
-			case PHY_IMP: push(cpu, cpu->y     , prefix, thread); break; /* PusH Y register to stack. */
-			case PHX_IMP: push(cpu, cpu->x     , prefix, thread); break; /* PusH X register to stack. */
+			case PHP_IMP: push(cpu, cpu->ps.u8[thread],    0, thread); break; /* PusH Processor status to stack. */
+			case PHA_IMP: push(cpu, cpu->a            , size, thread); break; /* PusH Accumulator to stack. */
+			case PHB_IMP: push(cpu, cpu->b            , size, thread); break; /* PusH B register to stack. */
+			case PHY_IMP: push(cpu, cpu->y            , size, thread); break; /* PusH Y register to stack. */
+			case PHX_IMP: push(cpu, cpu->x            , size, thread); break; /* PusH X register to stack. */
 			case TAY_IMP: /* Transfer Accumulator to Y. */
 			case TAX_IMP: /* Transfer Accumulator to Y. */
 			case TYX_IMP: /* Transfer Y to X. */
@@ -213,11 +214,11 @@ void *run(void *args) {
 			case SBC_Z: /* SBC Zero Matrix. */
 				sbc(cpu, value.u64, thread);
 				break;
-			case PLP_IMP: cpu->ps.u64 = pull(cpu,      0, thread); break; /* PuLl Processor status from stack. */
-			case PLA_IMP: cpu->a      = pull(cpu, prefix, thread); break; /* PuLl Accumulator from stack. */
-			case PLB_IMP: cpu->b      = pull(cpu, prefix, thread); break; /* PuLl B register from stack. */
-			case PLY_IMP: cpu->y      = pull(cpu, prefix, thread); break; /* PuLl Y register from stack. */
-			case PLX_IMP: cpu->x      = pull(cpu, prefix, thread); break; /* PuLl X register from stack. */
+			case PLP_IMP: cpu->ps.u8[thread] = pull(cpu,    0, thread); break; /* PuLl Processor status from stack. */
+			case PLA_IMP: cpu->a             = pull(cpu, size, thread); break; /* PuLl Accumulator from stack. */
+			case PLB_IMP: cpu->b             = pull(cpu, size, thread); break; /* PuLl B register from stack. */
+			case PLY_IMP: cpu->y             = pull(cpu, size, thread); break; /* PuLl Y register from stack. */
+			case PLX_IMP: cpu->x             = pull(cpu, size, thread); break; /* PuLl X register from stack. */
 				break;
 			case ABA_IMP: /* bitwise And with Accumulator, and B register. */
 				value.u64 = cpu->b; /* Falls Through. */
@@ -442,9 +443,7 @@ void *run(void *args) {
 			case JSR_IN: /* JSR Indirect. */
 			case JSR_AB: /* Jump to SubRoutine. */
 			case JSR_Z: /* JSR Zero Matrix. */
-				value.u64 = cpu->pc;
-				setreg(addr, -, (cpu->stk_st << 16)+cpu->sp, value.u8, +, 0, 7);
-				cpu->sp -= 8;
+				push(cpu, cpu->pc, 7, thread);
 				cpu->pc = address.u64;
 				break;
 			case INC_AB: /* INC Absolute. */
@@ -454,12 +453,9 @@ void *run(void *args) {
 			case NOP_IMP: /* No OPeration. */
 				break;
 			case RTI_IMP: /* ReTurn from Interrupt routine. */
-				cpu->sp   += 1;
-				cpu->ps.u8[thread] = addr[(cpu->stk_st << 16)+(cpu->sp)]; /* Falls through. */
+				cpu->ps.u64 = pull(cpu, 0, thread);
 			case RTS_IMP: /* ReTurn from Subroutine. */
-				cpu->sp += 8;
-				setreg(value.u8, +, 0, addr, -, (cpu->stk_st << 16)+cpu->sp, 7);
-				cpu->pc = value.u64;
+				cpu->pc = pull(cpu, 7, thread);
 				break;
 			case DEC_AB: /* DEC Absolute. */
 			case DEC_Z: /* DEC Zero Matrix. */
@@ -475,10 +471,8 @@ void *run(void *args) {
 					pthread_cond_wait(&cond, &mutex);
 					pthread_mutex_unlock(&mutex);
 				}
-				value.u64 = cpu->pc;
-				setreg(addr, -, (cpu->stk_st << 16)+cpu->sp, value.u8, +, 0, 7);
-				addr[(cpu->stk_st << 16)+cpu->sp-8] = cpu->ps.u8[thread];
-				cpu->sp -= 9;
+				push(cpu, cpu->pc, 7, thread);
+				push(cpu, cpu->ps.u8[thread], 0, thread);
 				setflag(1, I);
 				setreg(value.u8, +, 0, addr, +, (opcode == BRK) ? 0xFFE0 : 0xFFA0, 7);
 				if (opcode == WAI_IMP) {
diff --git a/sux.h b/sux.h
index 77736c4..9ac0ffc 100644
--- a/sux.h
+++ b/sux.h
@@ -95,7 +95,7 @@ static inline uint64_t get_addr(struct sux *cpu, uint64_t *tmpaddr, uint8_t opco
 					case 1:
 					case 3: saveaddr.u64 = tmp2 + (int32_t)address.u32[0]; break;
 				}
-				(optype[opcode] != INDY) ? (address.u64 = saveaddr.u64) : (reg = saveaddr.u64);
+				address.u64 = saveaddr.u64;
 			}
 			switch (optype[opcode]) {
 				case ZMX:
@@ -206,18 +206,20 @@ static inline void transfer(struct sux *cpu, uint64_t value, uint8_t opcode, uin
 	setflag(reg >> 63, N);
 }
 
-static inline void push(struct sux *cpu, uint64_t value, uint8_t prefix, uint8_t thread) {
+static inline void push(struct sux *cpu, uint64_t value, uint8_t size, uint8_t thread) {
 	union reg reg;
 	reg.u64 = value;
-	setreg(addr, -, (cpu->stk_st << 16)+cpu->sp, reg.u8, +, 0, (1 << ((prefix >> 4) & 3))-1);
-	cpu->sp -= (1 << ((prefix >> 4) & 3));
+	uint64_t sbr = (cpu->stk_st << 16);
+	setreg(addr, -, (sbr+cpu->sp), reg.u8, -, size, size);
+	cpu->sp -= size+1;
 }
 
-static inline uint64_t pull(struct sux *cpu, uint8_t prefix, uint8_t thread) {
+static inline uint64_t pull(struct sux *cpu, uint8_t size, uint8_t thread) {
 	union reg reg;
 	reg.u64 = 0;
-	cpu->sp += (1 << ((prefix >> 4) & 3));
-	setreg(reg.u8, +, 0, addr, -, (cpu->stk_st << 16)+cpu->sp, (1 << ((prefix >> 4) & 3))-1);
+	uint64_t sbr = (cpu->stk_st << 16);
+	cpu->sp += size+1;
+	setreg(reg.u8, -, size, addr, -, (sbr+cpu->sp), size);
 	return reg.u64;
 }