Implemented faults and fault handling.

Added flags FLTSTS, FLTJH, FLTJL.

Added breakpoints and BRK instruction.

Added PUSH and POP instructions for interacting with the stack.

FLAGS.md

@@ -2,9 +2,9 @@
 
 Locations shown below are relative to the start of flags, typically 0xFF00.
 
-Bits marked N/A read as zero and writing values will not have an effect.
+Bits not defined read as zero and writing values will not have an effect.
 
-## Console
+## Console | 0x0*
 
 ### Status | CONSTS | 0x00
 
@@ -12,7 +12,7 @@ Bits marked N/A read as zero and writing values will not have an effect.
 | --------------- | --- | --- | --- | --- | --- | --- | --- | --- |
 | **Read/Write**  |     |     |     |     | R   | R/W | R/W | R   |
 | **Default **    |     |     |     |     | X   | 1   | 1   | X   |
-| **Name**        | N/A | N/A | N/A | N/A | ReadAvailable | ReadBlock | Enabled | Available |
+| **Name**        |     |     |     |     | ReadAvailable | ReadBlock | Enabled | Available |
 
 ### Cursor X | CONPOSX | 0x01
 
@@ -34,17 +34,47 @@ Bits marked N/A read as zero and writing values will not have an effect.
 * 8-bit unsigned number
 * Read/Write
 
-## ALU
+## Interrupts/Faults | 0x1*
+
+Faults can be caught and handled by specifying a jump location in FLTRH and FLTRL. This handler must clear FLTSYS/2, it may call RET to continue execution or jump elsewhere.
+
+### Fault Status | FLTSTS | 0x10
+
+|                 | 8   | 7   | 6               | 5               | 4          | 3            | 2    | 1   |
+| --------------- | --- | --- | --------------- | --------------- | ---------- | ------------ | ---- | --- |
+| **Read/Write**  |     |     | R               | R               | R          | R            | R/W  | R/W |
+| **Default **    |     |     | X               | X               | X          | X            | X    | 0   |
+| **Name**        |     |     | Memory Overflow | Stack  Exceeded | Illegal Op | Undefined Op | Trip | Enabled |
+
+If not enabled, the system will halt should a fault occur. 
+
+In a fault condition, only one type will be indicated.
+
+Clearing **Trip** will clear status bits 3-8, it cannot be set.
+
+If a second fault occurs before trip is cleared, the system will halt; this known as a double-fault.
+
+**Undefined Op** indicates an operation that was unknown. **Illegal Op** indicates that the parameters to the operation were not valid.
+
+**Stack Exceeded** Indicates that the stack either overflowed (PUSH or CALL when full) or underflowed (RET or POP when empty). In this condition the stack pointer is reset to 0.
+
+### Fault Routine High Byte | FLTJH | 0x11
+
+The high byte of the fault handler jump.
+
+### Fault Routine Low Byte | FLTJL | 0x12
+
+The low byte of the fault handler jump.
+
+
+## ALU | 0x2*
 
 Flags here for logic/math results
 
-## Tape
+## Tape | 0x3*
 
 Tape control and status
 
-## Traps
-
-Trap status and settings
 
 
 <!-- 

SPEC.md

@@ -61,11 +61,13 @@
 | 0x33 | XOR   | [R] [@]          |
 | 0x34 | SHL   | [R] [0-7]        | Shift Left 
 | 0x35 | SHR   | [R] [0-7]        | Shift Right
-| 0x36 | BTS   | [R] [0-7]        | Bit Set
+| 0x36 | BTS   | [R] [1-8]        | Bit Set
-| 0x37 | BTC   | [R] [0-7]        | Bit Clear
+| 0x37 | BTC   | [R] [1-8]        | Bit Clear
 | **Stack**
 | 0x40 | CALL  | [#\|:]           |
 | 0x41 | RET   |                  |
+| 0x42 | PUSH  | [R]              |
+| 0x43 | POP   | [R]              |
 | **Program Flow**
 | 0x50 | HALT  |                  |
 | 0x51 | JMP   | [#\|:]           |
@@ -75,6 +77,7 @@
 | 0x55 | JBC   | [R] [1-8] [#\|:] |
 | **System**
 | 0x60 | SYS   | [0-255]          | Syscall, parameters depend on call
+| 0x61 | BRK   |                  | Pauses program execution if debug is enabled
 
 ### Assembler Only
 

SVM/Assembler.cs

@@ -56,6 +56,12 @@ namespace SVM
                     line = Regex.Replace(line, string.Format(@"(^|\b){0}($|\b)", alias.Key), alias.Value);
                 }
 
+                //Skip empty lines
+                if (string.IsNullOrWhiteSpace(line))
+                {
+                    continue;
+                }
+
                 //Records marker locations
                 if (line.StartsWith(':'))
                 {
@@ -136,6 +142,9 @@ namespace SVM
                 }
             }
 
+            //Read MEMORY section
+            var dataEnd = lastpos;
+
             for (; i < lines.Length; i++)
             {
                 line = lines[i];
@@ -182,6 +191,12 @@ namespace SVM
                         lineData[ldi] = byte.Parse(hex.Substring(di, 2), System.Globalization.NumberStyles.HexNumber);
                     }
                 }
+
+                if (dataOrigin < dataEnd)
+                {
+                    throw new Exception("Memory section starts within data section");
+                }
+
                 Array.Copy(lineData, 0, mem, dataOrigin, lineData.Length);
                 if (lastpos < dataOrigin + lineData.Length)
                 {

SVM/Fault.cs

@@ -0,0 +1,30 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace SVM
+{
+    public enum FaultType
+    {
+        None,
+        UndefinedOp,
+        IllegalOp,
+        StackExceeded,
+        MemoryOverflow
+    }
+
+    class Fault : Exception
+    {
+        public FaultType Type { get; private set; }
+
+        public Fault(FaultType type) : base()
+        {
+            Type = type;
+        }
+
+        public Fault(string message) : base(message)
+        {
+
+        }
+    }
+}

SVM/Flag.cs

@@ -8,7 +8,7 @@ namespace SVM
 {
     abstract class Flag
     {
-        public static Flag[] GetAllFlags()
+        public static Flag[] GetAllFlags(VM vm)
         {
             var types = from t in Assembly.GetAssembly(typeof(Flag)).GetTypes()
                         where t.IsSubclassOf(typeof(Flag))
@@ -18,16 +18,26 @@ namespace SVM
 
             foreach (var t in types)
             {
-                flags.Add((Flag)Activator.CreateInstance(t));
+                if (!t.IsAbstract)
+                {
+                    flags.Add((Flag)Activator.CreateInstance(t, vm));
+                }
             }
 
             return flags.ToArray();
         }
 
+        protected VM vm;
+
+        public Flag(VM vm)
+        {
+            this.vm = vm;
+        }
+
         public abstract string ASM { get; }
         public abstract byte Address { get; }
 
-        public abstract byte Read(VM vm);
+        public abstract byte Read();
-        public abstract void Write(VM vm, byte val);
+        public abstract void Write(byte val);
     }
 }

SVM/Flags/CONSTS.cs

@@ -17,7 +17,9 @@ namespace SVM.Flags
 
         public override byte Address => 0x00;
 
-        public override byte Read(VM vm)
+        public CONSTS(VM vm) : base(vm) { }
+
+        public override byte Read()
         {
             var port = vm.Ports[0] as ConsolePort;
             Debug.Assert(port != null);
@@ -39,7 +41,7 @@ namespace SVM.Flags
             return result;
         }
 
-        public override void Write(VM vm, byte val)
+        public override void Write(byte val)
         {
             var port = vm.Ports[0] as ConsolePort;
             Debug.Assert(port != null);

SVM/Flags/FLTJH.cs

@@ -0,0 +1,15 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace SVM.Flags
+{
+    class FLTJH : MemoryBackedFlag
+    {
+        public FLTJH(VM vm) : base(vm) { }
+
+        public override string ASM => "FLTJH";
+
+        public override byte Address => 0x11;
+    }
+}

SVM/Flags/FLTJL.cs

@@ -0,0 +1,15 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace SVM.Flags
+{
+    class FLTJL : MemoryBackedFlag
+    {
+        public FLTJL(VM vm) : base(vm) { }
+
+        public override string ASM => "FLTJL";
+
+        public override byte Address => 0x12;
+    }
+}

SVM/Flags/FLTSTS.cs

@@ -0,0 +1,81 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace SVM.Flags
+{
+    class FLTSTS : Flag
+    {
+        public const byte ENABLED_BIT = 1 << 0;
+        public const byte TRIP_BIT = 1 << 1;
+        public const byte UNDEFINEDOP_BIT = 1 << 2;
+        public const byte ILLEGALOP_BIT = 1 << 3;
+        public const byte STACKEXCEEDED_BIT = 1 << 4;
+        public const byte MEMORYOVERFLOW_BIT = 1 << 5;
+
+        public FLTSTS(VM vm) : base(vm)
+        {
+        }
+
+        public override string ASM => "FLTSTS";
+
+        public override byte Address => 0x10;
+
+        public bool Enabled { get; private set; }
+        public bool Tripped { get; private set; }
+
+        private FaultType Type = FaultType.None;
+
+        public bool Trip(Fault flt)
+        {
+            if (Tripped || !Enabled)
+            {
+                return false;
+            }
+            Tripped = true;
+            Type = flt.Type;
+            return true;
+        }
+
+        public override byte Read()
+        {
+            byte result = 0;
+
+            if (Enabled)
+            {
+                result |= ENABLED_BIT;
+            }
+            if (Tripped)
+            {
+                result |= TRIP_BIT;
+                switch(Type)
+                {
+                    case FaultType.IllegalOp:
+                        result |= ILLEGALOP_BIT;
+                        break;
+                    case FaultType.UndefinedOp:
+                        result |= UNDEFINEDOP_BIT;
+                        break;
+                    case FaultType.StackExceeded:
+                        result |= STACKEXCEEDED_BIT;
+                        break;
+                    case FaultType.MemoryOverflow:
+                        result |= MEMORYOVERFLOW_BIT;
+                        break;
+                }
+            }
+
+            return result;
+        }
+
+        public override void Write(byte val)
+        {
+            Enabled = (val & ENABLED_BIT) == ENABLED_BIT;
+            if ((val & TRIP_BIT) == 0)
+            {
+                Tripped = false;
+                Type = FaultType.None;
+            }
+        }
+    }
+}

SVM/Flags/MemoryBackedFlag.cs

@@ -0,0 +1,23 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace SVM.Flags
+{
+    abstract class MemoryBackedFlag : Flag
+    {
+        public MemoryBackedFlag(VM vm) : base(vm) { }
+
+        public override byte Read()
+        {
+            ushort addr = (ushort)(vm.FlagStart + Address);
+            return vm.MEM[addr];
+        }
+
+        public override void Write(byte val)
+        {
+            ushort addr = (ushort)(vm.FlagStart + Address);
+            vm.MEM[addr] = val;
+        }
+    }
+}

SVM/Instructions/ADD.cs

@@ -34,7 +34,10 @@ namespace SVM.Instructions
         {
             Debug.Assert(vars.Length > 1);
             var reg = vars[0];
-            Debug.Assert(reg <= VM.REGISTERS);
+            if (reg <= VM.REGISTERS)
+            {
+                throw new Fault(FaultType.IllegalOp);
+            }
             var loc = Location.FromByteCode(vars, 1);
 
             Run(vm, reg, loc);

SVM/Instructions/BRK.cs

@@ -0,0 +1,35 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Text;
+
+namespace SVM.Instructions
+{
+    class BRK : Instruction
+    {
+        public override string ASM => "BRK";
+
+        public override byte OP => 0x61;
+
+        public override byte[] Encode(string asm, Dictionary<string, ushort> markerRefs)
+        {
+            Debug.Assert(string.IsNullOrWhiteSpace(asm));
+            return new byte[] { OP };
+        }
+
+        public override void Exec(VM vm, byte[] vars)
+        {
+            if (vm.DEBUG)
+            {
+                vm.RUN = false;
+                vm.InvokeBreakpoint();
+            }
+        }
+
+        public override string ToASM(byte[] vars)
+        {
+            Debug.Assert(vars.Length == 0);
+            return ASM;
+        }
+    }
+}

SVM/Instructions/BTC.cs

@@ -12,6 +12,7 @@ namespace SVM.Instructions
 
         protected override void Run(VM vm, byte reg, byte bit)
         {
+            bit--;
             vm.R[reg] = (ushort)(vm.R[reg] & ~(1 << bit));
         }
     }

SVM/Instructions/BTS.cs

@@ -12,6 +12,7 @@ namespace SVM.Instructions
 
         protected override void Run(VM vm, byte reg, byte bit)
         {
+            bit--;
             vm.R[reg] = (ushort)(vm.R[reg] | 1 << bit);
         }
     }

SVM/Instructions/CALL.cs

@@ -41,9 +41,7 @@ namespace SVM.Instructions
         {
             Debug.Assert(vars.Length == 2);
 
-            Debug.Assert(vm.SP < VM.STACKDEPTH);
+            vm.PushStack(vm.PC);
-
-            vm.STACK[vm.SP++] = vm.PC;
             var jmp = (ushort)((vars[0] << 8) + vars[1]);
             vm.PC = jmp;
         }

SVM/Instructions/CLR.cs

@@ -27,7 +27,10 @@ namespace SVM.Instructions
         {
             Debug.Assert(vars.Length == 1);
             byte reg = vars[0];
-            Debug.Assert(reg <= VM.REGISTERS);
+            if (reg <= VM.REGISTERS)
+            {
+                throw new Fault(FaultType.IllegalOp);
+            }
             Run(vm, reg);
         }
 

SVM/Instructions/JBC.cs

@@ -73,7 +73,10 @@ namespace SVM.Instructions
         {
             Debug.Assert(vars.Length == 4);
             byte reg = vars[0];
-            Debug.Assert(reg <= VM.REGISTERS);
+            if (reg <= VM.REGISTERS)
+            {
+                throw new Fault(FaultType.IllegalOp);
+            }
             byte bit = vars[1];
             ushort loc = (ushort)((vars[2] << 8) + vars[3]);
 

SVM/Instructions/JZ.cs

@@ -47,7 +47,10 @@ namespace SVM.Instructions
         {
             Debug.Assert(vars.Length == 3);
             byte reg = vars[0];
-            Debug.Assert(reg <= VM.REGISTERS);
+            if (reg <= VM.REGISTERS)
+            {
+                throw new Fault(FaultType.IllegalOp);
+            }
             ushort loc = (ushort)((vars[1] << 8) + vars[2]);
 
             CheckJump(vm, reg, loc);

SVM/Instructions/LOAD.cs

@@ -47,7 +47,10 @@ namespace SVM.Instructions
         {
             Debug.Assert(vars.Length > 1);
             var reg = vars[0];
-            Debug.Assert(reg <= VM.REGISTERS);
+            if (reg <= VM.REGISTERS)
+            {
+                throw new Fault(FaultType.IllegalOp);
+            }
             var loc = Location.FromByteCode(vars, 1);
 
             return string.Format("{0} {1} {2}", ASM, Register.ToASM(reg), loc.ToASM());

SVM/Instructions/POP.cs

@@ -0,0 +1,19 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Text;
+
+namespace SVM.Instructions
+{
+    class POP : PUSH
+    {
+        public override string ASM => "POP";
+
+        public override byte OP => 0x43;
+
+        protected override void Run(VM vm, byte reg)
+        {
+            vm.R[reg] = vm.PopStack();
+        }
+    }
+}

SVM/Instructions/PUSH.cs

@@ -0,0 +1,57 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Text;
+
+namespace SVM.Instructions
+{
+    class PUSH : Instruction
+    {
+        public override string ASM => "PUSH";
+
+        public override byte OP => 0x42;
+
+        public override byte[] Encode(string asm, Dictionary<string, ushort> markerRefs)
+        {
+            var parts = asm.Split(" ");
+            Debug.Assert(parts.Length == 1);
+            var reg = Register.FromASM(parts[0]);
+
+            return new byte[] { OP, reg };
+        }
+
+        public override byte[] Decode(VM vm)
+        {
+            return vm.MEM.Subset(vm.PC++, 1);
+        }
+
+        public override void Exec(VM vm, byte[] vars)
+        {
+            Debug.Assert(vars.Length == 1);
+            var reg = vars[0];
+            if (reg <= VM.REGISTERS)
+            {
+                throw new Fault(FaultType.IllegalOp);
+            }
+            Run(vm, reg);
+        }
+
+        protected virtual void Run(VM vm, byte reg)
+        {
+            if (vm.SP > VM.STACKDEPTH)
+            {
+                throw new Fault(FaultType.StackExceeded);
+            }
+            vm.PushStack(vm.R[reg]);
+            vm.R[reg] = 0;
+        }
+
+        public override string ToASM(byte[] vars)
+        {
+            Debug.Assert(vars.Length == 1);
+            var reg = vars[0];
+
+            return string.Format("{0} {1}", ASM, Register.ToASM(reg));
+        }
+    }
+}

SVM/Instructions/RET.cs

@@ -19,11 +19,8 @@ namespace SVM.Instructions
         public override void Exec(VM vm, byte[] vars)
         {
             Debug.Assert(vars.Length == 0);
-            Debug.Assert(vm.SP > 0);
 
-            vm.SP--;
+            vm.PC = vm.PopStack();
-            vm.PC = vm.STACK[vm.SP];
-            vm.STACK[vm.SP] = 0;
         }
 
         public override string ToASM(byte[] vars)

SVM/Instructions/SHL.cs

@@ -17,7 +17,6 @@ namespace SVM.Instructions
             Debug.Assert(parts.Length == 2);
             byte reg = Register.FromASM(parts[0]);
             byte bit = byte.Parse(parts[1]);
-            Debug.Assert(bit < 8);
 
             return new byte[] { OP, reg, bit };
         }
@@ -36,15 +35,17 @@ namespace SVM.Instructions
             byte reg = vars[0];
             byte bit = vars[1];
 
-            Debug.Assert(reg <= VM.REGISTERS);
+            if (reg <= VM.REGISTERS)
-            Debug.Assert(bit < 8);
+            {
+                throw new Fault(FaultType.IllegalOp);
+            }
 
             Run(vm, reg, bit);
         }
 
         protected virtual void Run(VM vm, byte reg, byte bit)
         {
-            vm.R[reg] = (ushort)(vm.R[reg] << bit-1);
+            vm.R[reg] = (ushort)(vm.R[reg] << bit);
         }
 
         public override string ToASM(byte[] vars)

SVM/PGM/FAULT.txt

@@ -0,0 +1,75 @@
+ALIAS F_FLTSTS  M0xFF10
+ALIAS F_FLTJH   M0xFF11
+ALIAS F_FLTJL   M0xFF12
+ALIAS STR_FAULT I0x100
+ALIAS STR_OK	I0x120
+
+ORIGIN 0
+			JMP :MAIN			# Jump to program entry point
+
+ORIGIN 0x10
+			LOAD A F_FLTSTS		# Load fault status flag
+			BTC A 2				# Clear trip bit
+			SAVE A F_FLTSTS		# Save flag back
+			PUSH A				# Save the status code on the stack
+			LOAD A STR_FAULT	# Write message to console
+			CALL :WR_MSG
+			POP A				# Get the status code back
+			BTC A 1				# Remove the enabled bit
+			CALL :WRHEX			# Output the hex code for the fault
+			CALL :WRNL
+			RET
+
+:MAIN		LOAD A I0x0			# Set fault handler to 0x10
+			SAVE A F_FLTJH
+			LOAD A I0x10
+			SAVE A F_FLTJL
+			CLR A
+			BTS A 1
+			SAVE A F_FLTSTS		# Enable fault handler
+
+
+			POP A				# Nothing in stack!
+								# This should trigger the fault handler above
+
+			LOAD A STR_OK		# Once handled, we continue from here
+			CALL :WR_MSG
+			CALL :WRNL
+
+			HALT
+
+:WR_MSG		LOAD C RA
+:WR_MSG_L	LOAD A AC
+			JZ A :WR_MSG_R
+			SAVE A P0
+			INC C
+			JMP :WR_MSG_L
+:WR_MSG_R	CLR C
+			RET
+
+:WRNL		PUSH A
+			LOAD A I13
+			SAVE A P0
+			LOAD A I10
+			SAVE A P0
+			POP A
+			RET
+
+:WRHEX		LOAD B RA
+			SHR B 4			# Upper 4 bits
+			ADD B I0x130
+			LOAD B AB
+			SAVE B P0
+			LOAD B RA
+			AND B I0x0F		# Lower 4 bits
+			ADD B I0x130
+			LOAD B AB
+			SAVE B P0
+			CLR B
+			RET
+
+
+MEMORY
+0x100	"Fault Occured. Code: 0x"
+0x120	"All Good Now :)"
+0x130	0x 30 31 32 33 34 35 36 37 38 39 41 42 43 44 45 46	#0-F in ACSII

SVM/Program.cs

@@ -1,4 +1,5 @@
 using System;
+using System.Diagnostics;
 using System.IO;
 
 namespace SVM

SVM/SVM.csproj

@@ -18,7 +18,7 @@
     <Content Include="PGM\CALL.txt">
       <CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>
     </Content>
-    <Content Include="PGM\FlagTest.txt">
+    <Content Include="PGM\FLAGTEST.txt">
       <CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>
     </Content>
     <Content Include="PGM\HELLO.txt">

SVM/VM.cs

@@ -2,6 +2,7 @@
 using System.Collections.Generic;
 using System.Text;
 using System.Threading;
+using SVM.Flags;
 
 namespace SVM
 {
@@ -13,6 +14,7 @@ namespace SVM
         public const int MEMSIZE = 0xFFFF;
 
         public bool RUN;
+        public bool DEBUG;
         public ushort PC;
         public ushort[] R = new ushort[REGISTERS];
         public ushort RI;
@@ -22,10 +24,14 @@ namespace SVM
         public Port[] Ports = new Port[PORTS];
         public ushort FlagStart = 0xFF00;
 
+        public ulong InstructionCount = 0;
         public int CycleDelay = 0;
 
         private Dictionary<byte, Instruction> instructions = new Dictionary<byte, Instruction>();
         private Dictionary<byte, Flag> flags = new Dictionary<byte, Flag>();
+        private Dictionary<Type, byte> flagTypeMap = new Dictionary<Type, byte>();
+
+        public event EventHandler Breakpoint;
         
         public VM()
         {
@@ -33,9 +39,10 @@ namespace SVM
             {
                 instructions.Add(instr.OP, instr);
             }
-            foreach(var flag in Flag.GetAllFlags())
+            foreach(var flag in Flag.GetAllFlags(this))
             {
                 flags.Add(flag.Address, flag);
+                flagTypeMap.Add(flag.GetType(), flag.Address);
             }
 
             Ports[0] = new Ports.ConsolePort(this);
@@ -47,6 +54,7 @@ namespace SVM
             PC = 0;
             SP = 0;
             RI = 0;
+            InstructionCount = 0;
             Array.Fill<ushort>(R, 0);
             Array.Fill<ushort>(STACK, 0);
             Array.Fill<byte>(MEM, 0);
@@ -74,16 +82,50 @@ namespace SVM
         {
             if (RUN)
             {
-                var pc = PC;
+                try
-                var instr = instructions[MEM[PC++]];
+                {
-                byte[] decoded = instr.Decode(this);
+                    InstructionCount++;
-                //Console.Write("{4:X4} | A{0:X3} B{1:X3} C{2:X3} D{3:X3} | ", R[0], R[1], R[2], R[3], pc);
+                    if (PC == 0xFFFF)
-                //Console.WriteLine("{0}", instr.ToASM(decoded));
+                    {
-                instr.Exec(this, decoded);
+                        throw new Fault(FaultType.MemoryOverflow);
-                //Console.ReadKey(true);
+                    }
+                    var pc = PC;
+                    var op = MEM[PC++];
+                    if (!instructions.ContainsKey(op))
+                    {
+                        throw new Fault(FaultType.UndefinedOp);
+                    }
+                    var instr = instructions[op];
+                    byte[] decoded = instr.Decode(this);
+                    //Console.Write("{4:X4} | A{0:X3} B{1:X3} C{2:X3} D{3:X3} | ", R[0], R[1], R[2], R[3], pc);
+                    //Console.WriteLine("{0}", instr.ToASM(decoded));
+                    instr.Exec(this, decoded);
+                    //Console.ReadKey(true);
+                } catch(Fault flt)
+                {
+                    FLTSTS faultStatus = GetFlag<FLTSTS>();
+                    if (!faultStatus.Trip(flt))
+                    {
+                        //Halt system as trip failed (already tripped or not enabled)
+                        Ports[0].Write(Encoding.ASCII.GetBytes(
+                            string.Format("Unhandled Fault [{0}] at 0x{1:X2}", flt.Type, PC)
+                            ));
+                        RUN = false;
+                    } else
+                    {
+                        //Tripped ok. Get and jump to handler
+                        FLTJH faultJMPH = GetFlag<FLTJH>();
+                        FLTJL faultJMPL = GetFlag<FLTJL>();
+                        //Push current PC onto stack
+                        PushStack(PC);
+                        PC = (ushort)((faultJMPH.Read() << 8) + faultJMPL.Read()); 
+                    }
+                }
             }
         }
 
+        #region Memory
+
         public byte Read(ushort location)
         {
             if (location >= FlagStart && location < FlagStart + 0xFF)
@@ -91,7 +133,7 @@ namespace SVM
                 byte flagAddress = (byte)(location - FlagStart);
                 if (flags.ContainsKey(flagAddress))
                 {
-                    return flags[flagAddress].Read(this);
+                    return flags[flagAddress].Read();
                 }
                 return 0;
             }
@@ -104,12 +146,48 @@ namespace SVM
                 byte flagAddress = (byte)(location - FlagStart);
                 if (flags.ContainsKey(flagAddress))
                 {
-                    flags[flagAddress].Write(this, val);
+                    flags[flagAddress].Write(val);
                 }
             } else
             {
                 MEM[location] = val;
             }
         }
+
+        public void PushStack(ushort val)
+        {
+            if (SP > VM.STACKDEPTH)
+            {
+                throw new Fault(FaultType.StackExceeded);
+            }
+
+            STACK[SP++] = val;
+        }
+        public ushort PopStack()
+        {
+            if (SP <= 0)
+            {
+                throw new Fault(FaultType.StackExceeded);
+            }
+            var val = STACK[--SP];
+            STACK[SP] = 0;
+            return val;
+        }
+
+        #endregion
+
+        public void InvokeBreakpoint()
+        {
+            Breakpoint?.Invoke(this, new EventArgs());
+        }
+
+        public T GetFlag<T>() where T : Flag
+        {
+            if (flagTypeMap.ContainsKey(typeof(T)))
+            {
+                return flags[flagTypeMap[typeof(T)]] as T;
+            }
+            return default(T);
+        }
     }
 }