Implemented interrupts and exceptions

1 files changed, 51 insertions, 1 deletions

diff --git a/lc3.c b/lc3.c
index 0a0ad97..c509946 100644
--- a/lc3.c
+++ b/lc3.c
@@ -124,6 +124,32 @@ uint16_t sign_extend(uint16_t value, const int bits)
 	return (value ^ mask) - mask;
 }
 
+void fire(const uint16_t interrupt, const uint16_t priority)
+{
+	const uint16_t current_priority = (registers[REGISTER_PSR] >> 8) & 0x07;
+	if (current_priority >= priority)
+	{
+		return;
+	}
+	if (0x80 == interrupt && 0 == (0x4000 & memory[MEMORY_KBSR]))
+	{
+		return;
+	}
+	if (FLAG_SUP & registers[REGISTER_PSR])
+	{
+		registers[REGISTER_PSR] &= ~FLAG_SUP;
+		registers[REGISTER_USR] = registers[REGISTER_R6];
+		registers[REGISTER_R6] = registers[REGISTER_SSR];
+	}
+	registers[REGISTER_R6]--;
+	memory[registers[REGISTER_R6]] = registers[REGISTER_PSR];
+	registers[REGISTER_R6]--;
+	memory[registers[REGISTER_R6]] = registers[REGISTER_PC];
+	registers[REGISTER_PC] = 0x0100 | (interrupt & 0x00ff);
+	registers[REGISTER_PSR] &= 0xfeff;
+	registers[REGISTER_PSR] |= priority << 8;
+}
+
 uint16_t check_key()
 {
 	fd_set fds;
@@ -363,6 +389,21 @@ void step(const uint16_t instruction)
 			write_memory(address, registers[src]);
 			break;
 		}
+		case OP_RTI:
+		{
+			if (FLAG_SUP & registers[REGISTER_PSR])
+			{
+				fire(0, (registers[REGISTER_PSR] >> 8) & 0x07);
+			}
+			else
+			{
+				registers[REGISTER_PC] = memory[registers[REGISTER_R6]];
+				registers[REGISTER_R6]++;
+				registers[REGISTER_PSR] = memory[registers[REGISTER_R6]];
+				registers[REGISTER_R6]++;
+			}
+			break;
+		}
 		case OP_NOT:
 		{
 			const uint16_t dest = (instruction >> 9) & 0x07;
@@ -403,13 +444,18 @@ void step(const uint16_t instruction)
 			break;
 		}
 		case OP_TRAP:
+		{
 			registers[REGISTER_R7] = registers[REGISTER_PC];
 			registers[REGISTER_PC] = 0 | (instruction & 0xff);
 			trap(instruction);
 			registers[REGISTER_PC] = registers[REGISTER_R7]; // TODO: Remove this once traps are implemented.
 			break;
-		case OP_RTI:
+		}
 		case OP_RES:
+		{
+			fire(0x01, (registers[REGISTER_PSR] >> 8) & 0x07);
+			break;
+		}
 		default:
 			exit(EXIT_FAILURE);
 	}
@@ -453,5 +499,9 @@ int main(int argc, char ** argv)
 	{
 		const uint16_t instruction = read_memory(registers[REGISTER_PC]++);
 		step(instruction);
+		if (check_key())
+		{
+			fire(0x80, 4);
+		}
 	}
 }