Fixed some bugs in some instructions.

Fixed stack based instructions.
Fixed the rotate instructions.
Added carry to the shift instructions.

2 files changed, 346 insertions, 72 deletions

diff --git a/opcode.h b/opcode.h
index 757850e..f634be3 100644
--- a/opcode.h
+++ b/opcode.h
@@ -86,6 +86,8 @@
 #define BRK 0xF8 /* BReaK. */
 #define STP 0xFF /* SToP. */
 
+enum {ALU, THREAD, BRANCH, FLAG, MEMORY, MISC};
+
 static const char *opname[0x100] = {
 	OPNAME(CPS),
 	OPNAME(ADC),
diff --git a/sux.c b/sux.c
index b3e019d..c202d6c 100644
--- a/sux.c
+++ b/sux.c
@@ -13,7 +13,7 @@
 #define STK_STADDR 0x010000 /* Starting address of the stack. */
 uint8_t *addr; /* Address Space. */
 uint8_t update_pc = 1; /* Update the program counter. */
-uint16_t sp; /* Stack pointer. */
+uint16_t sp = 0xFFFF; /* Stack pointer. */
 uint64_t a[8]; /* Accumulator. */
 uint64_t y[8]; /* Y index. */
 uint64_t x[8]; /* X index. */
@@ -21,6 +21,114 @@ uint8_t crt; /* Current Running Threads. */
 uint64_t pc[8]; /* Program counter. */
 uint64_t ps; /* Processor status. */
 
+int optype(uint8_t opcode) {
+	switch (opcode) {
+		/* ALU instructions. */
+		case ADC:
+		case SBC:
+		case MUL:
+		case DIV:
+		case AND:
+		case AAY:
+		case AAX:
+		case ANY:
+		case ANX:
+		case ORA:
+		case OAY:
+		case OAX:
+		case ORY:
+		case ORX:
+		case XOR:
+		case XAY:
+		case XAX:
+		case XRY:
+		case XRX:
+		case SLA:
+		case SRA:
+		case ROL:
+		case ROR:
+		case INC:
+		case IAY:
+		case IAX:
+		case INY:
+		case INX:
+		case DEC:
+		case DAY:
+		case DAX:
+		case DEY:
+		case DEX:
+			return ALU;
+		/* Thread instructions. */
+		case STT:
+		case ENT:
+			return THREAD;
+		/* Branching instructions. */
+		case JMP:
+		case JSR:
+		case RTS:
+		case BPO:
+		case BNG:
+		case BEQ:
+		case BNE:
+		case BCS:
+		case BCC:
+		case BVS:
+		case BVC:
+		case CMP:
+		case CAY:
+		case CAX:
+		case CPY:
+		case CPX:
+			return BRANCH;
+		/* Processor status instructions. */
+		case CPS:
+		case SEI:
+		case CLI:
+		case SEC:
+		case CLC:
+		case SSP:
+		case CSP:
+		case SEV:
+		case CLV:
+			return FLAG;
+		/* Memory based instructions. */
+		case LDA:  /* LDA, imm */
+		case LDY:  /* LDY, imm */
+		case LAY:  /* LAY, imm */
+		case LDX:  /* LDX, imm */
+		case LAX:  /* LAX, imm */
+		case 0x59: /* LDA, abs */
+		case 0x52: /* LDY, abs */
+		case 0x53: /* LAY, abs */
+		case 0x54: /* LDX, abs */
+		case 0x55: /* LAX, abs */
+		case STA:
+		case SAY:
+		case SAX:
+		case STY:
+		case STX:
+		case PHP:
+		case PHA:
+		case PAY:
+		case PAX:
+		case PHY:
+		case PHX:
+		case PLP:
+		case PLA:
+		case PYA:
+		case PXA:
+		case PLY:
+		case PLX:
+			return MEMORY;
+		/* Misc intructions. */
+		case NOP:
+		case BRK:
+			return MISC;
+		default:
+			return -1;
+	}
+}
+
 uint64_t rol(uint64_t a, uint64_t value) {
 	const uint64_t mask = 8 * sizeof(a) - 1;
 	value &= mask;
@@ -33,21 +141,36 @@ uint64_t ror(uint64_t a, uint64_t value) {
 	return (a >> value) | (a << (-value & mask));
 }
 
-int push(uint64_t value, uint8_t size) {
+/*int push(uint64_t value, uint8_t size) {
 	for (uint8_t i = 0; i < size; i++) {
 		addr[STK_STADDR+sp] = value >> 8*i;
 		sp--;
 	}
 	return 1;
-}
+}*/
 
-uint64_t pull(uint8_t size) {
+/*uint64_t pull(uint8_t size) {
 	uint64_t value;
 	for (uint8_t i = 0; i < size; i++) {
-		value = addr[STK_STADDR+sp] << 8*i;
 		sp++;
+		if (i == 0)
+			value = (addr[STK_STADDR+sp]);
+		else
+			value |= (addr[STK_STADDR+sp] << (8*i));
 	}
 	return value;
+}*/
+
+
+
+void push(uint8_t value) {
+	addr[STK_STADDR+sp] = value;
+	sp--;
+}
+
+uint8_t pull() {
+	sp++;
+	return addr[STK_STADDR+sp];
 }
 
 int alu(uint8_t opcode, uint64_t value, uint8_t thread) {
@@ -138,7 +261,7 @@ int alu(uint8_t opcode, uint64_t value, uint8_t thread) {
 		/* Shift accumulator left. */
 		case SLA:
 			sum = (value < 64) ? a[thread] << value : 0;
-			c = a[thread] >> 63;
+			c = a[thread] >> 64-value;
 			a[thread] = sum;
 			break;
 		/* Shift accumulator right. */
@@ -149,11 +272,19 @@ int alu(uint8_t opcode, uint64_t value, uint8_t thread) {
 			break;
 		/* Rotate accumulator left. */
 		case ROL:
-			a[thread] = rol(a[thread], value);
+			c = (ps & (1 << thread)) >> thread;
+			sum = a[thread] << value;
+			sum |= c;
+			c = a[thread] >> (uint64_t)64-value;
+			a[thread] = sum;
 			break;
 		/* Rotate accumulator right. */
 		case ROR:
-			a[thread] = ror(a[thread], value);
+			c = (ps & (1 << thread)) >> thread;
+			sum = a[thread] >> value;
+			sum |= (uint64_t)c << (uint64_t)64-value;
+			c = a[thread] & 1;
+			a[thread] = sum;
 			break;
 		/* Increment accumulator. */
 		case INC:
@@ -236,12 +367,12 @@ int branch(uint8_t opcode, uint64_t value, uint8_t thread) {
 			break;
 		/* Jump to subroutine. */
 		case JSR:
-			push(pc[thread], 8);
+			/*push(pc[thread], 8);*/
 			pc[thread] = value;
 			break;
 		/* Return from subroutine. */
 		case RTS:
-			pc[thread] = pull(8);
+			/*pc[thread] = pull(8);*/
 			break;
 		/* Branch if positive. */
 		case BPO:
@@ -398,6 +529,7 @@ int memory(uint8_t opcode, uint64_t value, uint8_t thread) {
 				x[thread] = value;
 			else if (opcode == 0x55)
 				x[thread] = addr[value];
+			break;
 		/* Store value. */
 		case STA:
 		case SAY:
@@ -416,45 +548,113 @@ int memory(uint8_t opcode, uint64_t value, uint8_t thread) {
 			break;
 		/* Push processor status. */
 		case PHP:
-			push(ps, value);
+			/*push(ps, value);*/
 			break;
 		/* Push accumulator. */
 		case PHA:
 		case PAY:
 		case PAX:
-			push(a[thread], value);
-			if (opcode == PAY)
-				push(y[thread], value);
-			if (opcode == PAX)
-				push(x[thread], value);
+			if (value > 7)
+				value = 7;
+			for (int8_t i = 8*value; i > 0; i-=8) {
+				push(a[thread] >> i);
+			}
+			push(a[thread] & 0xFF);
+			if (opcode == PAY) {
+				if (value > 7)
+					value = 7;
+				for (int8_t i = 8*value; i > 0; i-=8) {
+					push(y[thread] >> i);
+				}
+				push(y[thread] & 0xFF);
+			}
+			if (opcode == PAX) {
+				if (value > 7)
+					value = 7;
+				for (int8_t i = 8*value; i > 0; i-=8) {
+					push(x[thread] >> i);
+				}
+				push(x[thread] & 0xFF);
+			}
 			break;
 		case PHY:
-			push(y[thread], value);
+			if (value > 7)
+				value = 7;
+			for (int8_t i = 8*value; i > 0; i-=8) {
+				push(y[thread] >> i);
+			}
+			push(y[thread] & 0xFF);
+			/*for (uint8_t i = value-1; i > 0; i--)
+				push(y[thread] >> 8*i);*/
 			break;
 		case PHX:
-			push(x[thread], value);
+			if (value > 7)
+				value = 7;
+			for (int8_t i = 8*value; i > 0; i-=8) {
+				push(x[thread] >> i);
+			}
+			push(x[thread] & 0xFF);
+			/*for (uint8_t i = value-1; i > 0; i--)
+				push(x[thread] >> 8*i);*/
 			break;
 		/* Pull processor status. */
 		case PLP:
-			ps = pull(value);
+			/*ps = pull(value);*/
 			break;
 		/* Pull accumulator. */
 		case PLA:
 		case PYA:
 		case PXA:
-			a[thread] = pull(value);
-			if (opcode == PAY)
-				y[thread] = pull(value);
-			if (opcode == PAX)
-				x[thread] = pull(value);
+			if (value > 7)
+				value = 7;
+			for (uint8_t i = 0; i <= 8*value; i+=8) {
+				if (!i)
+					a[thread] = (uint64_t)pull();
+				else
+					a[thread] += (uint64_t)pull() << i;
+			}
+			if (opcode == PAY) {
+				if (value > 7)
+					value = 7;
+				for (uint8_t i = 0; i <= 8*value; i+=8) {
+					if (!i)
+						y[thread] = (uint64_t)pull();
+					else
+						y[thread] += (uint64_t)pull() << i;
+				}
+			}
+			if (opcode == PAX) {
+				if (value > 7)
+					value = 7;
+				for (uint8_t i = 0; i <= 8*value; i+=8) {
+					if (!i)
+						x[thread] = (uint64_t)pull();
+					else
+						x[thread] += (uint64_t)pull() << i;
+				}
+			}
 			break;
 		/* Pull y register. */
 		case PLY:
-			y[thread] = pull(value);
+			if (value > 7)
+				value = 7;
+			for (uint8_t i = 0; i <= 8*value; i+=8) {
+				if (!i)
+					y[thread] = (uint64_t)pull();
+				else
+					y[thread] += (uint64_t)pull() << i;
+			}
 			break;
 		/* Pull x register. */
 		case PLX:
-			x[thread] = pull(value);
+			if (value > 7)
+				value = 7;
+			for (uint8_t i = 0; i <= 8*value; i+=8) {
+				if (!i)
+					x[thread] = (uint64_t)pull();
+				else
+					x[thread] += (uint64_t)pull() << i;
+			}
 			break;
 	}
 	return 1;
@@ -478,65 +678,137 @@ int main(int argc, char **argv) {
 	y[0] = 0;
 	addr = malloc(8*0x04000000);
 	int v = 0;
-	addr[0x8000] = 0x00;	/* CPS */
-	addr[0x8001] = 0xE1;	/* LDA #$E1 */
-	addr[0x8002] = 0x01;
-	addr[0x8003] = 0x51;	/* SLA #$01 */
-	addr[0x8004] = 0x01;
-	addr[0x8005] = 0xF1;	/* STA $4000*/
-	addr[0x8006] = 0x00;
-	addr[0x8007] = 0x40;
-	addr[0x8008] = 0x50;	/* BCS $8001 */
-	addr[0x8009] = 0x01;
+
+	/*addr[0x8000] = 0x00;	/* CPS */
+	/*addr[0x8001] = 0xE1;	/* LDA #$01 */
+	/*addr[0x8002] = 0x01;
+	addr[0x8003] = 0x51;	/* SLA #$04 */
+	/*addr[0x8004] = 0x04;
+	addr[0x8005] = 0x50;	/* BCS $800B */
+	/*addr[0x8006] = 0x0B;
+	addr[0x8007] = 0x80;
+	addr[0x8008] = 0x10;	/* JMP $8003 */
+	/*addr[0x8009] = 0x03;
 	addr[0x800A] = 0x80;
-	addr[0x800B] = 0x10;	/* JMP $1003 */
-	addr[0x800C] = 0x03;
-	addr[0x800D] = 0x80;
-	addr[0x8010] = 0xFF;	/* STP */
+	addr[0x800B] = 0xE1;	/* LDA #$01 */
+	/*addr[0x800C] = 0x01;
+	addr[0x800D] = 0x51;	/* SLA #$38 */
+	/*addr[0x800E] = 0x38;
+	addr[0x800F] = 0x61;	/* SRA #$04 */
+	/*addr[0x8010] = 0x04;
+	addr[0x8011] = 0x50;	/* BCS $8017 */
+	/*addr[0x8012] = 0x17;
+	addr[0x8013] = 0x80;
+	addr[0x8014] = 0x10;	/* JMP $800F */
+	/*addr[0x8015] = 0x0F;
+	addr[0x8016] = 0x80;
+	addr[0x8017] = 0x10;	/* JMP $8001 */
+	/*addr[0x8018] = 0x01;
+	addr[0x8019] = 0x80;
 	addr[0xFFFC] = 0x00;
 	addr[0xFFFD] = 0x80;
+
+	/*addr[0x8000] = 0x00; CPS */
+        /*addr[0x8001] = 0xE1; /* LDA #$01 */
+	/*addr[0x8002] = 0xFF;
+        addr[0x8003] = 0x51; /* SLA #$3C */
+	/*addr[0x8004] = 0x38;
+	addr[0x8005] = 0x01; /* ADC #$02 */
+	/*addr[0x8006] = 0x02;
+        addr[0x8007] = 0x81; /* ROR #$04 */
+	/*addr[0x8008] = 0x04;
+        addr[0x8009] = 0x10; /* JMP $8007 */
+        /*addr[0x800A] = 0x07;
+        addr[0x800B] = 0x80;
+        addr[0x800C] = 0x10; /* JMP $8001 */
+        /*addr[0x800D] = 0x01;
+	addr[0x800E] = 0x80;
+        /*addr[0x8008] = 0x70;  BEQ $800E */
+        /*addr[0x8009] = 0x0E;
+        addr[0x800A] = 0x80;
+        addr[0x800B] = 0x10;  JMP $8001
+        addr[0x800C] = 0x01;
+        addr[0x800D] = 0x80;
+        addr[0x800E] = 0xC1;  INC
+        addr[0x800F] = 0xE8;
+        addr[0x8010] = 0x04;
+        addr[0x8011] = 0x50;
+        addr[0x8012] = 0x17;
+        addr[0x8013] = 0x80;
+        addr[0x8014] = 0x10;
+        addr[0x8015] = 0x0F;
+        addr[0x8016] = 0x80;
+        addr[0x8017] = 0x10;
+        addr[0x8018] = 0x01;
+        addr[0x8019] = 0x80;*/
+
+
+	addr[0x8000] = 0x00; /* CPS */
+	addr[0x8001] = 0xC1; /* INC */
+	addr[0x8002] = 0x09; /* PHA #$08 */
+	addr[0x8003] = 0x08;
+	addr[0x8004] = 0x1C; /* PLX #$08 */
+	addr[0x8005] = 0x08;
+	addr[0x8006] = 0x10; /* JMP $8001 */
+	addr[0x8007] = 0x01;
+	addr[0x8008] = 0x80;
+	addr[0x8009] = 0x00;
+	addr[0x800A] = 0x00;
+	addr[0x800B] = 0x00;
+
+        addr[0xFFFC] = 0x00;
+        addr[0xFFFD] = 0x80;
+
 	pc[0] = addr[0xFFFC] | (addr[0xFFFD] << 8);
 	uint64_t value = 0;
+	uint64_t inst = 0;
+	uint8_t lines = 2;
 	uint8_t end = 0;
 	uint8_t opcode = 0;
 	uint8_t size;
 	printf("\033[2J");
 	while (!end) {
-	printf("pc: 0x%04llx, a: 0x%016llx, carry: %u, inst: %s, value: 0x%04llx, $%04llx: %02x\n", pc[0], a[0], ps & 1, opname[opcode], value, value, addr[value]);
-	opcode = addr[pc[0]];
-	if (opcode == STA || opcode == JMP || opcode == BCS)
-		size = 2;
-	else if (opcode == CPS || opcode == NOP)
-		size = 0;
-	else if (opcode == STP)
-		end = 1, size = 0;
-	else
-		size = 1;
-	if(update_pc)
+		/*printf("pc: 0x%04llx, a: 0x%016llx, carry: %u, inst: %s, value: 0x%04llx, $%04llx: %02x\n", pc[0], a[0], ps & 1, opname[opcode], value, value, addr[value]);*/
+		opcode = addr[pc[0]];
+		if (opcode == STA || opcode == JMP || opcode == BCS)
+			size = 2;
+		else if (opcode == CPS || opcode == NOP || opcode == INC || opcode == IAX || opcode == CAX)
+			size = 0;
+		else if (opcode == STP)
+			end = 1, size = 0;
+		else
+			size = 1;
 		pc[0]++;
-	else
-		update_pc = 1;
-	if (size) {
-		int i = 0;
-		while (i < size) {
-			if (!i)
-				value = (addr[pc[0]]);
-			else
-				value |= (addr[pc[0]] << (8*i));
-			printf("pc: 0x%04llx, a: 0x%016llx, carry: %u, inst: %s, value: 0x%04llx, $%04llx: %02x\n", pc[0], a[0], ps & 1, opname[opcode], value, value, addr[value]);
-			if (update_pc)
+		if (size) {
+			int i = 0;
+			while (i < size) {
+				if (!i)
+					value = (addr[pc[0]]);
+				else
+					value |= (addr[pc[0]] << (8*i));
+				/*printf("pc: 0x%04llx, a: 0x%016llx, carry: %u, inst: %s, value: 0x%04llx, $%04llx: %02x\n", pc[0], a[0], ps & 1, opname[opcode], value, value, addr[value]);*/
 				pc[0]++;
-			else
-				update_pc = 1;
-			i++;
+				i++;
+			}
 		}
-	}
-	if (opcode == SLA || opcode == ADC)
-		alu(opcode, value, 0);
-	if (opcode == JMP || opcode == BCS)
-		branch(opcode, value, 0);
-	memory(opcode, value, 0);
-	printf("pc: 0x%04llx, a: 0x%016llx, carry: %u, inst: %s, value: 0x%04llx, $%04llx: %02x\n", pc[0], a[0], ps & 1, opname[opcode], value, value, addr[value]);
+		if (optype(opcode) == ALU)
+			alu(opcode, value, 0);
+		if (optype(opcode) == THREAD)
+			threads(opcode, value);
+		if (optype(opcode) == BRANCH)
+			branch(opcode, value, 0);
+		if (optype(opcode) == MEMORY)
+			memory(opcode, value, 0);
+		if (optype(opcode) == FLAG)
+			setflags(opcode, 0);
+		if (optype(opcode) == MISC)
+			misc(opcode, 0);
+
+		if (lines > 50)
+			lines = 2;
+		printf("\033[%uHpc: 0x%04llx, a: 0x%016llx, x: 0x%016llx, y: 0x%016llx, sp: 0x%04lx, carry: %u, inst: %s, value: 0x%04llx, $%04llx: %02x\n", lines, pc[0], a[0], x[0], y[0], sp, ps & 1, opname[opcode], value, value, addr[value]);
+		lines++;
+		printf("\033[1HInstructions executed: %llu\n", inst++);
 	}
 	free(addr);
 	return 0;