diff --git a/fih_test_tool/Readme.rst b/fih_test_tool/Readme.rst
new file mode 100644
index 0000000..81b3d34
--- /dev/null
+++ b/fih_test_tool/Readme.rst
@@ -0,0 +1,121 @@
+#############
+FIH TEST TOOL
+#############
+
+This directory contains a tool for testing the fault injection mitigations
+implemented by TF-M.
+
+Description
+===========
+
+The tool relies on QEMU (simulating the AN521 target), and GDB.
+
+The tool will:
+   * first compile a version of TF-M with the given parameters.
+   * Then setup some infrastructure used for control of a QEMU session,
+     including a Virtual Hard Drive (VHD) to save test state and some FIFOs for
+     control.
+   * Then run GDB, which then creates a QEMU instance and attaches to it via the
+     GDBserver interface and the FIFOs.
+   * Run through an example execution of TF-M, conducting fault tests.
+   * Output a JSON file with details of which tests where run, and what failed.
+
+The workflow for actually running the test execution is as follows:
+   * Perform setup, including starting QEMU.
+   * Until the end_breakpoint is hit:
+      * Execute through the program until a `test_location` breakpoint is hit.
+      * Save the execution state to the QEMU VHD.
+      * Enable all the `critical point breakpoints`.
+      * Run through until the end_breakpoint to save the "known good" state. The
+         state saving is described below.
+      * For each of the fault tests specified:
+         * Load the test_start state, so that a clean environment is present.
+         * Perform the fault.
+         * Run through, evaluating any `critical memory` at every `critical
+            point breakpoint` and saving this to the test state.
+         * Detect any failure loop states, QEMU crashes, or the end breakpoint,
+            and end the test.
+         * Compare the execution state of the test with the "known good" state.
+      * Load the state at the start of the test.
+      * Disable all the `critical point breakpoints`.
+
+The output file will be inside the created TFM build directory. It is named
+`results.json` The name of the created TFM build dir will be determined by the
+build options. For example `build_GNUARM_debug_OFF_2/results.json`
+
+Dependencies
+============
+
+ * qemu-system-arm
+ * gdb-multiarch (with python3 support)
+ * python3.7+
+ * python packages detailed in requirements.txt
+
+The version of python packaged with gdb-multiarch can differ from the version of
+python that is shipped by the system. The version of python used by
+gdb-multiarch can can be tested by running the command:
+`gdb-multiarch -batch -ex "python import sys; print(sys.version)"`.
+If this version is not greater than or equal to 3.7, then gdb-multiarch may need
+to be upgraded. Under some distributions, this might require upgrading to a
+later version of the distribution.
+
+Usage of the tool
+=================
+
+Options can be determined by using
+
+``./fih_test --help``
+
+In general, executing `fih_test` from a directory inside the TF-M source
+directory (`<TFM_DIR>/build`), will automatically set the SOURCE_DIR and
+BUILD_DIR variables / arguments correctly.
+
+For example:
+```
+cd <TFM_DIR>
+mkdir build
+cd build
+<Path to>/fih_test -f LOW
+```
+
+Fault types
+=====================
+
+The types of faults simulated is controlled by ``faults/__init__.py``. This file
+should be altered to change the fault simulation parameters. To add new fault
+types, new fault files should be added to the ``faults`` directory.
+
+Currently, the implemented fault types are:
+ * Setting a single register (from r0 to r15) to a random uint32
+ * Setting a single register (from r0 to r15) to zero
+ * Skipping between 1 and 7 instructions
+
+All of these will be run at every evaluation point, currently 40 faults per
+evaluation point.
+
+Working with results
+====================
+
+Results are written as a JSON file, and can be large. As such, it can be useful
+to employ dedicated tools to parse the JSON.
+
+The use of `jq <https://stedolan.github.io/jq/>` is highly recommended. Full
+documentation of this program is out of scope of this document, but instructions
+and reference material can be found at the linked webpage.
+
+For example, to find the amount of passes:
+
+``cat results.json | jq 'select(.passed==true) | .passed' | wc -l``
+
+And the amount of fails:
+
+``cat results.json | jq 'select(.passed==false) | .passed' | wc -l``
+
+To find all the faults that caused failures, and the information about where
+they occurred:
+
+``cat results.json | jq 'select(.passed==false) | {pc: .pc, file: .file, line: .line, asm: .asm, fault: .fault}'``
+
+--------------
+
+*Copyright (c) 2021, Arm Limited. All rights reserved.*
diff --git a/fih_test_tool/fih_test b/fih_test_tool/fih_test
new file mode 100755
index 0000000..dafdf53
--- /dev/null
+++ b/fih_test_tool/fih_test
@@ -0,0 +1,64 @@
+#!/usr/bin/env bash
+# Copyright (c) 2021, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+usage()
+{
+    echo "$0 [-s source_dir] [-d build_dir] [-b <build_type>] [-c <compiler>] [-f <fih_profile>] [-l <tfm_level>]"
+}
+
+# Parse arguments
+while test $# -gt 0; do
+    case $1 in
+        -s|--source_dir)
+        SOURCE_DIR="$2"
+        shift
+        shift
+        ;;
+        -d|--build_dir)
+        BUILD_DIR="$2"
+        shift
+        shift
+        ;;
+        -b|--build_type)
+        BUILD_TYPE="$2"
+        shift
+        shift
+        ;;
+        -c|--compiler)
+        COMPILER="$2"
+        shift
+        shift
+        ;;
+        -f|--fih_profile)
+        FIH_PROFILE="$2"
+        shift
+        shift
+        ;;
+        -l|--tfm_level)
+        TFM_LEVEL="$2"
+        shift
+        shift
+        ;;
+        -h|--help)
+        usage
+        exit 0
+        ;;
+        *)
+        echo "Invalid argument"
+        usage
+        exit 1
+        ;;
+    esac
+done
+
+SCRIPT_DIR=$(dirname $(realpath "${BASH_SOURCE[0]}"))
+source ${SCRIPT_DIR}/util.sh
+
+TEST_DIR=$(realpath $(pwd))
+set_default SOURCE_DIR $(realpath ${TEST_DIR}/..)
+
+source ${SCRIPT_DIR}/fih_test_build_tfm.sh
+source ${SCRIPT_DIR}/fih_test_make_manifest.sh
+source ${SCRIPT_DIR}/fih_test_run_gdb.sh
diff --git a/fih_test_tool/fih_test_build_tfm.sh b/fih_test_tool/fih_test_build_tfm.sh
new file mode 100644
index 0000000..1d15deb
--- /dev/null
+++ b/fih_test_tool/fih_test_build_tfm.sh
@@ -0,0 +1,66 @@
+#!/usr/bin/env bash
+# Copyright (c) 2021, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+set_default MCUBOOT_PATH ${TEST_DIR}/mcuboot
+if ! test -f ${MCUBOOT_PATH}/success
+then
+    mkdir -p ${MCUBOOT_PATH}
+    git clone https://github.com/mcu-tools/mcuboot ${MCUBOOT_PATH}
+    pushd ${MCUBOOT_PATH}
+    git checkout 81d19f0
+    popd
+    touch ${MCUBOOT_PATH}/success
+fi
+
+set_default MBEDCRYPTO_PATH ${TEST_DIR}/mbedtls
+if ! test -f ${MBEDCRYPTO_PATH}/success
+then
+    mkdir -p ${MBEDCRYPTO_PATH}
+    git clone https://github.com/ARMmbed/mbedtls -b mbedtls-2.24.0 ${MBEDCRYPTO_PATH}
+    pushd ${MBEDCRYPTO_PATH}
+    git am ${SOURCE_DIR}/lib/ext/mbedcrypto/*.patch
+    popd
+    touch ${MBEDCRYPTO_PATH}/success
+fi
+
+set_default TFM_TEST_REPO_PATH ${TEST_DIR}/tf-m-tests
+if ! test -f ${TFM_TEST_REPO_PATH}/success
+then
+    mkdir -p ${TFM_TEST_REPO_PATH}
+    git clone https://git.trustedfirmware.org/TF-M/tf-m-tests.git ${TFM_TEST_REPO_PATH}
+    touch ${TFM_TEST_REPO_PATH}/success
+fi
+
+set_default BUILD_TYPE debug
+set_default COMPILER GNUARM
+set_default FIH_PROFILE OFF
+set_default TFM_LEVEL 2
+
+set_default BUILD_DIR ${TEST_DIR}/build_${COMPILER}_${BUILD_TYPE}_${FIH_PROFILE}_${TFM_LEVEL}
+
+set -e
+
+mkdir -p ${BUILD_DIR}
+pushd ${SOURCE_DIR}
+cmake -B ${BUILD_DIR} \
+    -DCMAKE_BUILD_TYPE=${BUILD_TYPE} \
+    -DTFM_TOOLCHAIN_FILE=toolchain_${COMPILER}.cmake \
+    -DTFM_PLATFORM=mps2/an521 \
+    -DTFM_PSA_API=ON \
+    -DDEBUG_AUTHENTICATION=FULL \
+    -DTFM_ISOLATION_LEVEL=${TFM_LEVEL} \
+    -DTFM_FIH_PROFILE=${FIH_PROFILE} \
+    -DMCUBOOT_PATH=${MCUBOOT_PATH} \
+    -DMBEDCRYPTO_PATH=${MBEDCRYPTO_PATH} \
+    -DTFM_TEST_REPO_PATH=${TFM_TEST_REPO_PATH} \
+    .
+popd
+
+pushd ${BUILD_DIR}
+make clean
+make -j install
+popd
+
+set +e
diff --git a/fih_test_tool/fih_test_exec_qemu.sh b/fih_test_tool/fih_test_exec_qemu.sh
new file mode 100755
index 0000000..c0d2a00
--- /dev/null
+++ b/fih_test_tool/fih_test_exec_qemu.sh
@@ -0,0 +1,18 @@
+#!/usr/bin/env bash
+# Copyright (c) 2021, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+qemu-system-arm \
+    -M mps2-an521 \
+    -kernel ${BUILD_DIR}/bin/bl2.axf \
+    -device loader,file=${BUILD_DIR}/bin/tfm_s_ns_signed.bin,addr=0x10080000 \
+    -pidfile ${QEMU_PIDFILE} \
+    -drive file=${QEMU_VHD},id=disk,format=qcow2 \
+    -d guest_errors \
+    -D /tmp/qemu.log \
+    -display none \
+    -s -S \
+    -chardev file,id=char0,path=/dev/null \
+    -monitor pipe:${QEMU_MON_FIFO} \
+    -daemonize
diff --git a/fih_test_tool/fih_test_make_manifest.sh b/fih_test_tool/fih_test_make_manifest.sh
new file mode 100644
index 0000000..3b42747
--- /dev/null
+++ b/fih_test_tool/fih_test_make_manifest.sh
@@ -0,0 +1,37 @@
+#!/usr/bin/env bash
+# Copyright (c) 2021, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+
+set_default AXF_FILE ${BUILD_DIR}/bin/tfm_s.axf
+set_default OBJDUMP arm-none-eabi-objdump
+set_default GDB gdb-multiarch
+
+if ! test -f ${AXF_FILE}
+then
+    error "no such file ${AXF_FILE}"
+fi
+
+# Check if the ELF file specified is compatible
+if  ! file ${AXF_FILE} | grep "ELF.*32.*ARM" &>/dev/null
+then
+    error "Incompatible file: ${AXF_FILE}"
+fi
+
+#TODO clean this up, and use less regex
+
+# Dump all objects that have a name containing FIH_LABEL
+ADDRESSES=$($OBJDUMP $AXF_FILE -t | grep "FIH_LABEL")
+# strip all data except "address, label_name"
+ADDRESSES=$(echo "$ADDRESSES" | sed "s/\([[:xdigit:]]*\).*\(FIH_LABEL_.*\)_[0-9]*_[0-9]*/0x\1, \2/g")
+# Sort by address in ascending order
+ADDRESSES=$(echo "$ADDRESSES" | sort)
+# In the case that there is a START followed by another START take the first one
+ADDRESSES=$(echo "$ADDRESSES" | sed "N;s/\(.*START.*\)\n\(.*START.*\)/\1/;P;D")
+# Same for END except take the second one
+ADDRESSES=$(echo "$ADDRESSES" | sed "N;s/\(.*END.*\)\n\(.*END.*\)/\2/;P;D")
+
+# Output in CSV format with a label
+echo "Address, Type" > ${BUILD_DIR}/fih_manifest.csv
+echo "$ADDRESSES"   >> ${BUILD_DIR}/fih_manifest.csv
diff --git a/fih_test_tool/fih_test_run_gdb.sh b/fih_test_tool/fih_test_run_gdb.sh
new file mode 100644
index 0000000..fc0ce27
--- /dev/null
+++ b/fih_test_tool/fih_test_run_gdb.sh
@@ -0,0 +1,37 @@
+#!/usr/bin/env bash
+# Copyright (c) 2021, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+set_default QEMU_UART_FIFO ${BUILD_DIR}/qemu_uart
+set_default QEMU_MON_FIFO  ${BUILD_DIR}/qemu_mon
+set_default QEMU_PIDFILE   ${BUILD_DIR}/qemu_pid
+set_default QEMU_VHD       ${BUILD_DIR}/qemu_vhd
+
+rm ${QEMU_MON_FIFO}.* ${QEMU_UART_FIFO}.* ${QEMU_VHD} ${QEMU_PID}
+
+mkfifo ${QEMU_UART_FIFO}.in ${QEMU_UART_FIFO}.out
+mkfifo ${QEMU_MON_FIFO}.in ${QEMU_MON_FIFO}.out
+
+rm ${BUILD_DIR}/results.json
+
+# The disk image is used to store snapshots, to allow easier recreation of test
+# state
+qemu-img create -f qcow2 ${QEMU_VHD} 50M
+
+pushd ${BUILD_DIR}
+
+gdb-multiarch --ex "set architecture armv8-m.main" \
+              --ex "set confirm off" \
+              --ex "set pagination off" \
+              --ex "set target-async on" \
+              --ex "file ${BUILD_DIR}/bin/bl2.axf" \
+              --ex "add-symbol-file ${BUILD_DIR}/bin/tfm_s.axf  0x00080000" \
+              --ex "add-symbol-file ${BUILD_DIR}/bin/tfm_ns.axf 0x00100400" \
+              --ex "source ${SCRIPT_DIR}/gdb-tool/fih_test_gdb_python_script.py"
+
+popd
+
+kill $(cat ${QEMU_PIDFILE})
+
+rm -f ${QEMU_MON_FIFO}.* ${QEMU_UART_FIFO}.* ${QEMU_VHD} ${QEMU_PIDFILE}
diff --git a/fih_test_tool/gdb-tool/backend/qemu.py b/fih_test_tool/gdb-tool/backend/qemu.py
new file mode 100644
index 0000000..2d2e1f0
--- /dev/null
+++ b/fih_test_tool/gdb-tool/backend/qemu.py
@@ -0,0 +1,73 @@
+# Copyright (c) 2021, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+import subprocess
+import os
+import gdb
+
+backend_has_saveload = True
+
+qemu_mon_in_file  = None
+qemu_mon_out_file = None
+
+qemu_pid = None
+
+def backend_start_and_reset_and_connect():
+    global qemu_mon_in_file
+    global qemu_mon_out_file
+    global qemu_pid
+
+    print("(Re)Starting QEMU")
+    p = subprocess.run(os.path.dirname(os.path.realpath(__file__)) + "/../../fih_test_exec_qemu.sh")
+    p = subprocess.run(["pgrep", "qemu-system-arm"], capture_output=True)
+    qemu_pid = p.stdout.decode('utf-8').rstrip()
+
+    try:
+        gdb.execute('detach')
+    except gdb.error:
+        # We were probably not attached in the first place - this was post-crash
+        # or first run.
+        pass
+    gdb.execute('target extended-remote localhost:1234')
+
+    if qemu_mon_in_file is not None:
+        qemu_mon_in_file.close()
+    if qemu_mon_out_file is not None:
+        qemu_mon_out_file.close()
+
+    qemu_mon_in_file   = open("qemu_mon.in", 'w')
+    qemu_mon_out_file  = open("qemu_mon.out", 'r')
+
+def backend_reset():
+    gdb.execute('detach')
+    _kill_qemu()
+    backend_start_and_reset_and_connect()
+    print("Reset QEMU")
+
+####################### Save / Loading #########################################
+
+def backend_save_state(id):
+    print("Saving state: {} at PC {}".format(str(id),
+                                             hex(gdb.selected_frame().pc())))
+    _write_fifo(qemu_mon_in_file, 'savevm {}'.format(str(id)))
+
+def backend_load_state(id):
+    _write_fifo(qemu_mon_in_file, 'loadvm {}'.format(str(id)))
+    # Disconnect and reconnect to reset gdb's internal state
+    gdb.execute('detach')
+    gdb.execute('target extended-remote localhost:1234')
+    print("Loaded state: " + str(id))
+
+######################## Internal ##############################################
+
+def _write_fifo(fifo, msg):
+    fifo.write(msg + '\n')
+    fifo.flush()
+
+def _kill_qemu():
+    global qemu_pid
+    if qemu_pid is None:
+        return
+    p = subprocess.run(["kill", qemu_pid, "-9"])
+    qemu_pid = None
diff --git a/fih_test_tool/gdb-tool/faults/__init__.py b/fih_test_tool/gdb-tool/faults/__init__.py
new file mode 100644
index 0000000..eeac3c3
--- /dev/null
+++ b/fih_test_tool/gdb-tool/faults/__init__.py
@@ -0,0 +1,15 @@
+# Copyright (c) 2021, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+from .skip import skip_fault
+from .register import register_fault
+
+fault_types = [
+            [register_fault(reg="r" + str(x)) for x in range(16)],
+            [register_fault(reg="r" + str(x), val=0) for x in range(16)],
+            [skip_fault(size=x * 2) for x in range(8)],
+        ]
+
+# Flatten the list
+fault_types = sum(fault_types, [])
diff --git a/fih_test_tool/gdb-tool/faults/register.py b/fih_test_tool/gdb-tool/faults/register.py
new file mode 100644
index 0000000..b3d0ab0
--- /dev/null
+++ b/fih_test_tool/gdb-tool/faults/register.py
@@ -0,0 +1,32 @@
+# Copyright (c) 2021, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+import gdb
+import random
+
+class register_fault():
+    def __init__(self, reg=None,
+                       val=None):
+        if reg is None:
+            self.reg = "r" + str(random.randint(0, 16))
+        else:
+            self.reg = reg
+
+        if val is None:
+            self.val = random.randint(0, 0xFFFFFFFF - 1)
+        else:
+            self.val = val
+
+    def execute(self):
+        gdb.execute('set ${} = {}'.format(self.reg, self.val))
+
+    def __repr__(self):
+        return "Register Fault: set {} to {}".format(self.reg, hex(self.val))
+
+    def as_json(self):
+        return {
+                'type': 'register',
+                'reg': self.reg,
+                'val': hex(self.val),
+                }
diff --git a/fih_test_tool/gdb-tool/faults/skip.py b/fih_test_tool/gdb-tool/faults/skip.py
new file mode 100644
index 0000000..c594a5e
--- /dev/null
+++ b/fih_test_tool/gdb-tool/faults/skip.py
@@ -0,0 +1,25 @@
+# Copyright (c) 2021, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+import gdb
+import random
+
+class skip_fault():
+    def __init__(self, size=None):
+        if size is None:
+            self.size = random.randint(1, 6) * 2
+        else:
+            self.size = size
+
+    def execute(self):
+        gdb.execute('set $pc += {}'.format(self.size))
+
+    def __repr__(self):
+        return "Skip Fault: pc += {}".format(self.size)
+
+    def as_json(self):
+        return {
+                'type': 'skip',
+                'size': self.size,
+                }
diff --git a/fih_test_tool/gdb-tool/fih_test_gdb_python_script.py b/fih_test_tool/gdb-tool/fih_test_gdb_python_script.py
new file mode 100644
index 0000000..ea46f65
--- /dev/null
+++ b/fih_test_tool/gdb-tool/fih_test_gdb_python_script.py
@@ -0,0 +1,395 @@
+# Copyright (c) 2021, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+import gdb
+import os
+import sys
+import csv
+import time
+from threading import Thread
+import json
+import inspect
+# Add modules in subdirs to path
+sys.path.append(os.path.dirname(os.path.realpath(inspect.getfile(inspect.currentframe()))))
+from faults import fault_types
+from backend.qemu import *
+
+####################### Globals ################################################
+
+results_file = open('results.json', "w+")
+
+last_hit_breakpoint = None
+
+####################### Shim Functions #########################################
+
+class async_function_executor:
+    def __init__(self, cmd):
+        self.__cmd = cmd
+
+    def __call__(self):
+        gdb.execute(self.__cmd)
+
+def set_test_mode():
+    for c in critical_point_breakpoints:
+        c.enabled = True
+    for b in test_location_breakpoints:
+        b.enabled = False
+
+def set_runthrough_mode():
+    for c in critical_point_breakpoints:
+        c.enabled = False
+    for b in test_location_breakpoints:
+        b.enabled = True
+
+def stop_handler(event):
+    global last_hit_breakpoint
+    try:
+        last_hit_breakpoint = event.breakpoints[-1]
+    except AttributeError:
+        pass
+
+####################### GDB Functions ##########################################
+
+_wait_halt_required = 0
+
+def _gdb_wait_halt(timeout):
+    global _wait_halt_required
+    wait_time = 0
+    while(wait_time < timeout):
+        if _wait_halt_required != 1:
+            return
+        time.sleep(0.01)
+        wait_time += 0.01
+    print("SENDING TIMEOUT")
+    gdb.post_event(async_function_executor("interrupt"))
+    _wait_halt_required = 0
+    print("TIMEOUT")
+
+def continue_with_timeout(timeout=1):
+    out = False
+    global _wait_halt_required
+    _wait_halt_required = 1;
+    thread = Thread(target = _gdb_wait_halt, args = (timeout, ))
+    thread.start()
+    try:
+        gdb.execute('continue')
+    except gdb.error as e:
+        print(e)
+        # If this errors, the backend has probably crashed.
+        # First stop the sigint being sent
+        _wait_halt_required = 0
+        # Then reboot the backend
+        backend_start_and_reset_and_connect()
+        # Restore the state
+        if backend_has_saveload:
+            backend_load_state('test_start')
+        else:
+            set_runthrough_mode()
+            gdb.execute('continue')
+            set_test_mode()
+        # Notify the test runner that a crash occurred
+        out = True
+    _wait_halt_required = 0;
+    thread.join()
+    return out
+
+def get_function_from_addr(addr):
+    try:
+        output = gdb.execute('disassemble {}'.format(addr), to_string=True)
+    except gdb.error:
+        return None
+    # The function name is on the first line
+    output = output.split('\n')[0]
+    # The function name is the last word
+    output = output.split(' ')[-1]
+    # Remove the colon at the end
+    output = output[:-1]
+    return output
+
+def get_asm_from_addr(addr):
+    try:
+        output = gdb.execute('disassemble {}'.format(addr), to_string=True)
+    except gdb.error:
+        return None
+    # Get the line starting with the => symbol
+    output = output.split('\n')[:-1]
+    try:
+        output = [x for x in output if '=>' == x.lstrip().split(' ')[0]][0]
+    except IndexError:
+        return ''
+    # Remove the leading position info
+    output = "".join(output.split(':')[1:]).rstrip().lstrip().replace('\t', '    ')
+    return output
+
+def get_line_from_addr(addr):
+    lineno = get_lineno_from_addr(addr)
+    try:
+        output = gdb.execute('list *{}'.format(addr), to_string=True)
+    except gdb.error:
+        return None
+    # Get the line starting with the known lineno
+    output = output.split('\n')
+    output = [x for x in output if lineno == x.split('\t')[0]][0]
+    # Remove the lineno and leading whitespace
+    output = output[len(lineno):].lstrip()
+    return output
+
+def get_lineno_from_addr(addr):
+    try:
+        output = gdb.execute('info line *{}'.format(addr), to_string=True)
+    except gdb.error:
+        return None
+    # The line number is the second word
+    output = output.split(' ')[1]
+    return output
+
+def get_file_from_addr(addr):
+    try:
+        output = gdb.execute('info symbol {}'.format(addr), to_string=True)
+    except gdb.error:
+        return None
+    # The file name is the last word
+    output = output.split(' ')[-1].replace('\n', '')
+    return output
+
+def get_addr_from_symbol(symbol):
+    addr = gdb.execute("p &" + symbol, to_string=True)
+    return addr
+
+def read_mem(addr, size=4):
+    try:
+        out = gdb.inferiors()[0].read_memory(addr, size).tobytes()
+    except gdb.MemoryError:
+        out = bytes(size)
+    return out
+
+def read_word_mem(addr):
+    return int.from_bytes(read_mem(addr, 4), byteorder='little')
+
+def write_mem(addr, bytes):
+    out = gdb.inferiors()[0].write_memory(addr, bytes)
+    return out
+
+def write_word_mem(addr, word):
+    return write_mem(addr, word.to_bytes(4, byteorder='little'))
+
+####################### Test Functions #########################################
+
+def read_mpu_config():
+    MPU_TYPE  = 0xE000ED90
+    MPU_RNR   = 0xE000ED98
+    MPU_CTRL  = 0xE000ED94
+    MPU_MAIR0 = 0xE000EDC0
+    MPU_MAIR1 = 0xE000EDC4
+    MPU_RBAR  = 0xE000ED9C
+    MPU_RLAR  = 0xE000EDA0
+
+    out  = read_mem(MPU_CTRL)
+    out += read_mem(MPU_MAIR0)
+    out += read_mem(MPU_MAIR1)
+
+    mpu_rnr_max = (read_word_mem(MPU_TYPE) & (255 << 8)) >> 8
+    mpu_rnr_old = read_word_mem(MPU_RNR)
+    # TODO this doesn't work because of
+    # https://bugs.launchpad.net/qemu/+bug/1625216. It can't alter RNR so will
+    # only read one section
+    for i in range(min(1, mpu_rnr_max)):
+        write_word_mem(MPU_RNR, i)
+        out += read_mem(MPU_RBAR)
+        out += read_mem(MPU_RLAR)
+
+    write_word_mem(MPU_RNR, mpu_rnr_old)
+    return out
+
+def read_ppc_config():
+    SPCTRL = 0x50080000
+    SPCTRL_SIZE = 0x1000
+    NSPCTRL = 0x40080000
+    NSPCTRL_SIZE = 0x1000
+
+    out  = read_mem(SPCTRL, SPCTRL_SIZE)
+    out += read_mem(NSPCTRL, NSPCTRL_SIZE)
+
+    return out
+
+def read_sau_config():
+    SAU_TYPE = 0xE000EDD4
+    SAU_RNR  = 0xE000EDD8
+    SAU_CTRL = 0xE000EDD0
+    SAU_RBAR = 0xE000EDDC
+    SAU_RLAR = 0xE000EDE0
+
+    out  = read_mem(SAU_CTRL)
+
+    sau_rnr_max = read_word_mem(SAU_TYPE) & 255
+    sau_rnr_old = read_word_mem(SAU_RNR)
+    # TODO this doesn't work because of
+    # https://bugs.launchpad.net/qemu/+bug/1625216. It can't alter RNR so will
+    # only read one section
+    for i in range(sau_rnr_max):
+        write_word_mem(SAU_RNR, i)
+        out += read_mem(SAU_RBAR)
+        out += read_mem(SAU_RLAR)
+
+    write_word_mem(SAU_RNR, sau_rnr_old)
+    return out
+
+def read_vulnerable_state():
+    out = {}
+    out['mpc'] = read_mpu_config()
+    out['ppc'] = read_ppc_config()
+    out['sau'] = read_sau_config()
+    # TODO MPC
+
+    for k in out.keys():
+        out[k] = out[k].hex()
+
+    return out;
+
+def evaluate_at_critical_points(test, is_known_good=False):
+    global last_hit_breakpoint
+
+    last_hit_breakpoint = None
+    while last_hit_breakpoint not in end_breakpoints and not test['finished']:
+        last_hit_breakpoint = None
+        ret = continue_with_timeout(0.2)
+        if ret == True:
+            test['finished'] = True
+            test['passed'] = True
+            test['state'] = "CRASHED BACKEND"
+            return test
+        pc = gdb.selected_frame().pc()
+        result = {'pc': hex(pc)}
+        if last_hit_breakpoint in fault_breakpoints and is_known_good:
+            # This is an error. In general, this is caused by some odd behaviour
+            # in GDB interacting with an obscure bug in Qemu. The mechanism for
+            # this bug is roughtly: GDB will issue reads for locations it has
+            # breakpoints at everytime time it hits _any_ breakpoint. If this
+            # happens between the NS SAU being enabled and the NS MPC being
+            # configured, then there will be a translation failure, which causes
+            # the tfm_access_violation_handler to be hit once the IRQs are
+            # re-enabled. GDB reads in Qemu shouldn't ever cause faults, so I
+            # think this is where the actual bug is, but it's easier to
+            # work-around than fix. To avoid this happening, you should not
+            # place any breakpoints that are inside the NS code region at
+            # 0x00100000 to 0x001FFFFF. This is why the end breakpoint is at the
+            # end of secure code, and not an the start of NS code.
+            print("If you've hit this Exception, please see the comment in the"
+                  "code (above the `raise` call) about why it might be"
+                  "happening.")
+            raise(Exception)
+        elif last_hit_breakpoint in fault_breakpoints:
+            test['finished'] = True
+            test['passed'] = True
+            result['state'] = "ERROR_LOOP"
+        elif last_hit_breakpoint not in critical_point_breakpoints + end_breakpoints:
+            test['finished'] = True
+            test['passed'] = True
+            result['state'] = "TIMEOUT"
+        else:
+            result['state'] = "NORMAL"
+        result['vulnerable_mem'] = read_vulnerable_state()
+        test['results'].append(result)
+
+    test['finished'] = True
+    return test
+
+def run_test():
+    global last_hit_breakpoint
+    global test_location_breakpoints
+
+    test_breakpoint = last_hit_breakpoint
+
+    set_test_mode()
+    if backend_has_saveload:
+        backend_save_state('test_start')
+    pc = gdb.selected_frame().pc()
+
+    out = []
+    test = {'results':  [],
+            'finished': False}
+    known_good = evaluate_at_critical_points(test, True)['results']
+
+    for f in fault_types:
+        if backend_has_saveload:
+            backend_load_state('test_start')
+        else:
+            backend_reset()
+            set_runthrough_mode()
+            gdb.execute('continue')
+            set_test_mode()
+        f.execute()
+        test = {'pc': hex(pc),
+                'file': '{}:{}'.format(get_file_from_addr(pc), get_lineno_from_addr(pc)),
+                'line': get_line_from_addr(pc),
+                'asm': get_asm_from_addr(pc),
+                'results': [],
+                'passed': None,
+                'known_good': known_good,
+                'finished': False,
+                'fault': f.as_json()}
+        print("Running {} at addr {}".format(f, hex(pc)))
+        test = evaluate_at_critical_points(test)
+        if test['results'] != known_good and test['passed'] != True:
+            test['passed'] = False
+        else:
+            test['passed'] = True
+        json.dump(test, results_file)
+        results_file.flush()
+
+    set_runthrough_mode()
+    if backend_has_saveload:
+        backend_load_state('test_start')
+    else:
+        backend_reset()
+        gdb.execute('continue')
+        test_location_breakpoints.remove(test_breakpoint)
+        test_breakpoint.delete()
+    return out
+
+####################### Setup ##################################################
+
+backend_start_and_reset_and_connect()
+gdb.events.stop.connect(stop_handler)
+
+with open('fih_manifest.csv', newline='') as csv_file:
+    manifest = list(csv.reader(csv_file, delimiter=','))[1:]
+
+critical_points = ['*' + x[0] for x in manifest if "FIH_CRITICAL_POINT" in x[1]]
+critical_point_breakpoints = [gdb.Breakpoint(c) for c in critical_points]
+
+fault_breakpoint_locations = [
+                              'tfm_access_violation_handler'
+                              ]
+fault_breakpoint_locations += ['*' + x[0] for x in manifest if "FAILURE_LOOP" in x[1]]
+fault_breakpoints = [gdb.Breakpoint(f) for f in fault_breakpoint_locations]
+
+critical_memory = [x[0] for x in manifest if "FIH_CRITICAL_MEMORY" in x[1]]
+critical_memory = []
+
+test_location_starts = [int(x[0], 16) for x in manifest if "START" in x[1]]
+test_location_ends   = [int(x[0], 16) for x in manifest if "END" in x[1]]
+test_location_zones  = zip(test_location_starts, test_location_ends)
+test_locations = sum([list(range(x[0],x[1], 2)) for x in test_location_zones], [])
+test_locations = ["*{}".format(x) for x in test_locations]
+test_location_breakpoints = [gdb.Breakpoint(t) for t in test_locations]
+
+end_breakpoint_locations = ["tfm_core_handler_mode", "jump_to_ns_code"]
+end_breakpoints = [gdb.Breakpoint(t) for t in end_breakpoint_locations]
+
+####################### Runtime ################################################
+
+backend_save_state('start')
+set_runthrough_mode()
+
+out = []
+
+
+gdb.execute('continue')
+while last_hit_breakpoint not in end_breakpoints:
+    run_test()
+    gdb.execute('continue')
+
+results_file.close()
+gdb.execute('quit')
diff --git a/fih_test_tool/requirements.txt b/fih_test_tool/requirements.txt
new file mode 100644
index 0000000..bc9f32d
--- /dev/null
+++ b/fih_test_tool/requirements.txt
@@ -0,0 +1,5 @@
+# Copyright (c) 2021, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+gdb
diff --git a/fih_test_tool/util.sh b/fih_test_tool/util.sh
new file mode 100644
index 0000000..2b7c78e
--- /dev/null
+++ b/fih_test_tool/util.sh
@@ -0,0 +1,30 @@
+# Copyright (c) 2021, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+set_default()
+{
+    if test -z "$(eval echo '$'$1)"
+    then
+        eval export $1='$2'
+    fi
+}
+
+info()
+{
+    printf "[INF] $1\n" 1>&2
+}
+warn()
+{
+    printf "\e[33m[WRN] $1\e[0m\n" 1>&2
+}
+error()
+{
+    printf "\e[31m[ERR] $1\e[0m\n" 1>&2
+    if test -n "$2"
+    then
+        exit $2
+    else
+        exit 1
+    fi
+}