Reverted C traps back in

2 files changed, 79 insertions, 23 deletions

diff --git a/init.asm b/init.asm
index 489722c..cdece4a 100644
--- a/init.asm
+++ b/init.asm
@@ -1,28 +1,10 @@
 	.ORIG x0000
 ; traps
-	.BLKW #31
-	.FILL t_getc
-	.FILL t_out
-	.FILL t_puts
-	.FILL t_in
-	.FILL t_putsp
-	.FILL t_halt
-	.BLKW #219
+	.BLKW #256
 ; interrupts
-	.FILL thalt
-	.FILL thalt
+	.FILL _halt
+	.FILL _halt
 	.BLKW #254
-; supervisor code ; TODO: implement traps and find working assembler
-t_getc
-	.BLKW #1
-t_out
-	.BLKW #1
-t_puts
-	.BLKW #1
-t_in
-	.BLKW #1
-t_putsp
-	.BLKW #1
-t_halt
-	.BLKW #1
+; supervisor code
+_halt	HALT
 	.END
diff --git a/lc3.c b/lc3.c
index 1b08f79..6740b48 100644
--- a/lc3.c
+++ b/lc3.c
@@ -215,6 +215,79 @@ void update_cond(const uint16_t reg)
 	}
 }
 
+void trap(const uint16_t instruction)
+{
+	switch (instruction & 0xff)
+	{
+		case TRAP_GETC:
+		{
+			uint8_t c;
+			ssize_t r = read(0, &c, 1);
+			die(r, "TRAP_GETC read()");
+			registers[REGISTER_R0] = (uint16_t) c;
+			break;
+		}
+		case TRAP_OUT:
+		{
+			uint8_t c = (uint8_t) registers[REGISTER_R0];
+			ssize_t r = write(1, &c, 1);
+			die(r, "TRAP_OUT write()");
+			break;
+		}
+		case TRAP_PUTS:
+		{
+			uint16_t * w = memory + registers[REGISTER_R0];
+			uint8_t c;
+			while (*w)
+			{
+				c = (uint8_t) *w;
+				ssize_t r = write(1, &c, 1);
+				die(r, "TRAP_PUTS write()");
+				++w;
+			}
+			break;
+		}
+		case TRAP_IN:
+		{
+			printf("> ");
+			uint8_t c;
+			ssize_t r = read(0, &c, 1);
+			die(r, "TRAP_IN read()");
+			r = write(1, &c, 1);
+			die(r, "TRAP_IN write()");
+			registers[REGISTER_R0] = (uint16_t) c;
+			break;
+		}
+		case TRAP_PUTSP:
+		{
+			uint16_t * w = memory + registers[REGISTER_R0];
+			uint8_t c;
+			while (*w)
+			{
+				c = (uint8_t) (*w & 0xff);
+				ssize_t r = write(1, &c, 1);
+				die(r, "TRAP_PUTSP write()");
+				c = (uint8_t) (*w >> 8);
+				if (c)
+				{
+					r = write(1, &c, 1);
+					die(r, "TRAP_PUTSP write()");
+				}
+			}
+			break;
+		}
+		case TRAP_HALT:
+		{
+			ssize_t r = write(1, "Halt\n", 5);
+			die(r, "TRAP_HALT write()");
+			exit(EXIT_SUCCESS);
+		}
+		default:
+			return;
+	}
+	registers[REGISTER_PC] = registers[REGISTER_R7];
+}
+
 void step(const uint16_t instruction)
 {
 	const uint16_t opcode = instruction >> 12;
@@ -375,6 +448,7 @@ void step(const uint16_t instruction)
 		{
 			registers[REGISTER_R7] = registers[REGISTER_PC];
 			registers[REGISTER_PC] = read_memory(0 | (instruction & 0xff));
+			trap(instruction);
 			break;
 		}
 		case OP_RES: