From cf9004e206a2b07dba6d85a5b8708919fd19a102 Mon Sep 17 00:00:00 2001
From: Sam Stevens <sam@xnet.tk>
Date: Sun, 31 Dec 2017 23:18:17 +0000
Subject: [PATCH] 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                      | 46 +++++++++++++---
 SPEC.md                       |  7 ++-
 SVM/Assembler.cs              | 15 ++++++
 SVM/Fault.cs                  | 30 +++++++++++
 SVM/Flag.cs                   | 18 +++++--
 SVM/Flags/CONSTS.cs           |  6 ++-
 SVM/Flags/FLTJH.cs            | 15 ++++++
 SVM/Flags/FLTJL.cs            | 15 ++++++
 SVM/Flags/FLTSTS.cs           | 81 +++++++++++++++++++++++++++++
 SVM/Flags/MemoryBackedFlag.cs | 23 ++++++++
 SVM/Instructions/ADD.cs       |  5 +-
 SVM/Instructions/BRK.cs       | 35 +++++++++++++
 SVM/Instructions/BTC.cs       |  1 +
 SVM/Instructions/BTS.cs       |  1 +
 SVM/Instructions/CALL.cs      |  4 +-
 SVM/Instructions/CLR.cs       |  5 +-
 SVM/Instructions/JBC.cs       |  5 +-
 SVM/Instructions/JZ.cs        |  5 +-
 SVM/Instructions/LOAD.cs      |  5 +-
 SVM/Instructions/POP.cs       | 19 +++++++
 SVM/Instructions/PUSH.cs      | 57 ++++++++++++++++++++
 SVM/Instructions/RET.cs       |  5 +-
 SVM/Instructions/SHL.cs       |  9 ++--
 SVM/PGM/FAULT.txt             | 75 +++++++++++++++++++++++++++
 SVM/Program.cs                |  5 +-
 SVM/SVM.csproj                |  2 +-
 SVM/VM.cs                     | 98 +++++++++++++++++++++++++++++++----
 27 files changed, 547 insertions(+), 45 deletions(-)
 create mode 100644 SVM/Fault.cs
 create mode 100644 SVM/Flags/FLTJH.cs
 create mode 100644 SVM/Flags/FLTJL.cs
 create mode 100644 SVM/Flags/FLTSTS.cs
 create mode 100644 SVM/Flags/MemoryBackedFlag.cs
 create mode 100644 SVM/Instructions/BRK.cs
 create mode 100644 SVM/Instructions/POP.cs
 create mode 100644 SVM/Instructions/PUSH.cs
 create mode 100644 SVM/PGM/FAULT.txt

diff --git a/FLAGS.md b/FLAGS.md
index 7e3e5eb..9663326 100644
--- a/FLAGS.md
+++ b/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
 
 
 <!-- 
diff --git a/SPEC.md b/SPEC.md
index f467eef..bd79b3e 100644
--- a/SPEC.md
+++ b/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
-| 0x37 | BTC   | [R] [0-7]        | Bit Clear
+| 0x36 | BTS   | [R] [1-8]        | Bit Set
+| 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
 
diff --git a/SVM/Assembler.cs b/SVM/Assembler.cs
index 2e3755c..3a83f29 100644
--- a/SVM/Assembler.cs
+++ b/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)
                 {
diff --git a/SVM/Fault.cs b/SVM/Fault.cs
new file mode 100644
index 0000000..8b5a989
--- /dev/null
+++ b/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)
+        {
+
+        }
+    }
+}
diff --git a/SVM/Flag.cs b/SVM/Flag.cs
index 5cbae4b..287a7e0 100644
--- a/SVM/Flag.cs
+++ b/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 void Write(VM vm, byte val);
+        public abstract byte Read();
+        public abstract void Write(byte val);
     }
 }
diff --git a/SVM/Flags/CONSTS.cs b/SVM/Flags/CONSTS.cs
index 5d9f1e9..b6f019c 100644
--- a/SVM/Flags/CONSTS.cs
+++ b/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);
diff --git a/SVM/Flags/FLTJH.cs b/SVM/Flags/FLTJH.cs
new file mode 100644
index 0000000..1f672a4
--- /dev/null
+++ b/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;
+    }
+}
diff --git a/SVM/Flags/FLTJL.cs b/SVM/Flags/FLTJL.cs
new file mode 100644
index 0000000..5f363bd
--- /dev/null
+++ b/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;
+    }
+}
diff --git a/SVM/Flags/FLTSTS.cs b/SVM/Flags/FLTSTS.cs
new file mode 100644
index 0000000..11f4746
--- /dev/null
+++ b/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;
+            }
+        }
+    }
+}
diff --git a/SVM/Flags/MemoryBackedFlag.cs b/SVM/Flags/MemoryBackedFlag.cs
new file mode 100644
index 0000000..1a75f3d
--- /dev/null
+++ b/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;
+        }
+    }
+}
diff --git a/SVM/Instructions/ADD.cs b/SVM/Instructions/ADD.cs
index 12342c7..2014dcb 100644
--- a/SVM/Instructions/ADD.cs
+++ b/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);
diff --git a/SVM/Instructions/BRK.cs b/SVM/Instructions/BRK.cs
new file mode 100644
index 0000000..bee2cd6
--- /dev/null
+++ b/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;
+        }
+    }
+}
diff --git a/SVM/Instructions/BTC.cs b/SVM/Instructions/BTC.cs
index 5c5f62f..744aa03 100644
--- a/SVM/Instructions/BTC.cs
+++ b/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));
         }
     }
diff --git a/SVM/Instructions/BTS.cs b/SVM/Instructions/BTS.cs
index 3c9d4b7..9cdd2d7 100644
--- a/SVM/Instructions/BTS.cs
+++ b/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);
         }
     }
diff --git a/SVM/Instructions/CALL.cs b/SVM/Instructions/CALL.cs
index e0965da..5019349 100644
--- a/SVM/Instructions/CALL.cs
+++ b/SVM/Instructions/CALL.cs
@@ -41,9 +41,7 @@ namespace SVM.Instructions
         {
             Debug.Assert(vars.Length == 2);
 
-            Debug.Assert(vm.SP < VM.STACKDEPTH);
-
-            vm.STACK[vm.SP++] = vm.PC;
+            vm.PushStack(vm.PC);
             var jmp = (ushort)((vars[0] << 8) + vars[1]);
             vm.PC = jmp;
         }
diff --git a/SVM/Instructions/CLR.cs b/SVM/Instructions/CLR.cs
index 693da66..c3867ea 100644
--- a/SVM/Instructions/CLR.cs
+++ b/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);
         }
 
diff --git a/SVM/Instructions/JBC.cs b/SVM/Instructions/JBC.cs
index 457a2e9..b475784 100644
--- a/SVM/Instructions/JBC.cs
+++ b/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]);
 
diff --git a/SVM/Instructions/JZ.cs b/SVM/Instructions/JZ.cs
index 576cd73..2e6acc2 100644
--- a/SVM/Instructions/JZ.cs
+++ b/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);
diff --git a/SVM/Instructions/LOAD.cs b/SVM/Instructions/LOAD.cs
index 8592867..e8d2371 100644
--- a/SVM/Instructions/LOAD.cs
+++ b/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());
diff --git a/SVM/Instructions/POP.cs b/SVM/Instructions/POP.cs
new file mode 100644
index 0000000..2b22086
--- /dev/null
+++ b/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();
+        }
+    }
+}
diff --git a/SVM/Instructions/PUSH.cs b/SVM/Instructions/PUSH.cs
new file mode 100644
index 0000000..722ad4e
--- /dev/null
+++ b/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));
+        }
+    }
+}
diff --git a/SVM/Instructions/RET.cs b/SVM/Instructions/RET.cs
index 8ea8d91..14d49a6 100644
--- a/SVM/Instructions/RET.cs
+++ b/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.STACK[vm.SP];
-            vm.STACK[vm.SP] = 0;
+            vm.PC = vm.PopStack();
         }
 
         public override string ToASM(byte[] vars)
diff --git a/SVM/Instructions/SHL.cs b/SVM/Instructions/SHL.cs
index f02a4bf..8c37eb8 100644
--- a/SVM/Instructions/SHL.cs
+++ b/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);
-            Debug.Assert(bit < 8);
+            if (reg <= VM.REGISTERS)
+            {
+                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)
diff --git a/SVM/PGM/FAULT.txt b/SVM/PGM/FAULT.txt
new file mode 100644
index 0000000..0ae94b3
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/SVM/Program.cs b/SVM/Program.cs
index fcd80d5..c0299d0 100644
--- a/SVM/Program.cs
+++ b/SVM/Program.cs
@@ -1,4 +1,5 @@
 using System;
+using System.Diagnostics;
 using System.IO;
 
 namespace SVM
@@ -27,11 +28,11 @@ namespace SVM
 
             var mem = asm.Compile(content);
             Console.WriteLine("Compiled to {0} bytes", mem.Length);
-            
+
             var vm = new VM();
             vm.CycleDelay = 25;
             vm.Load(mem, 0);
-
+            
             vm.Run();
 
             Console.ReadKey(true);
diff --git a/SVM/SVM.csproj b/SVM/SVM.csproj
index 81fa700..857f26a 100644
--- a/SVM/SVM.csproj
+++ b/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">
diff --git a/SVM/VM.cs b/SVM/VM.cs
index 66756d5..a5c3649 100644
--- a/SVM/VM.cs
+++ b/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,20 +24,25 @@ 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()
         {
             foreach(var instr in Instruction.GetAllInstructions())
             {
                 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;
-                var instr = instructions[MEM[PC++]];
-                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);
+                try
+                {
+                    InstructionCount++;
+                    if (PC == 0xFFFF)
+                    {
+                        throw new Fault(FaultType.MemoryOverflow);
+                    }
+                    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);
+        }
     }
 }