diff --git a/tests/psa-client-server/psasim/src/Makefile b/tests/psa-client-server/psasim/src/Makefile
new file mode 100644
index 0000000..fc6ba25
--- /dev/null
+++ b/tests/psa-client-server/psasim/src/Makefile
@@ -0,0 +1,17 @@
+INCLUDE = -I../include/
+PSA_LIB = libpsaff.a
+
+PSA_LIB_OBJS = client.o service.o
+
+.PHONY: all lib
+
+all: $(PSA_LIB)
+
+%.o: %.c
+	$(CC) $(INCLUDE) $(CFLAGS) -c $< -o $@
+
+$(PSA_LIB): $(PSA_LIB_OBJS)
+	$(AR) rcs $(PSA_LIB) client.o service.o
+
+clean:
+	rm -f $(PSA_LIB) $(PSA_LIB_OBJS)
diff --git a/tests/psa-client-server/psasim/src/client.c b/tests/psa-client-server/psasim/src/client.c
new file mode 100644
index 0000000..5a3986e
--- /dev/null
+++ b/tests/psa-client-server/psasim/src/client.c
@@ -0,0 +1,392 @@
+/* PSA firmware framework client API */
+
+/*
+ *  Copyright The Mbed TLS Contributors
+ *  SPDX-License-Identifier: Apache-2.0 OR GPL-2.0-or-later
+ */
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <stddef.h>
+#include <assert.h>
+#include <stdio.h>
+#include <string.h>
+#include <strings.h>
+#include <inttypes.h>
+#include <sys/types.h>
+#include <sys/ipc.h>
+#include <sys/msg.h>
+
+#include "psa/client.h"
+#include "psa/common.h"
+#include "psa/error.h"
+#include "psa/util.h"
+
+typedef struct internal_handle {
+    int server_qid;
+    int client_qid;
+    int internal_server_qid;
+    int valid;
+} internal_handle_t;
+
+typedef struct vectors {
+    const psa_invec *in_vec;
+    size_t in_len;
+    psa_outvec *out_vec;
+    size_t out_len;
+} vectors_t;
+
+/* Note that this implementation is functional and not secure */
+int __psa_ff_client_security_state = NON_SECURE;
+
+/* Access to this global is not thread safe */
+#define MAX_HANDLES 32
+static internal_handle_t handles[MAX_HANDLES] = { { 0 } };
+
+static int get_next_free_handle()
+{
+    /* Never return handle 0 as it's a special null handle */
+    for (int i = 1; i < MAX_HANDLES; i++) {
+        if (handles[i].valid == 0) {
+            return i;
+        }
+    }
+    return -1;
+}
+
+static int handle_is_valid(psa_handle_t handle)
+{
+    if (handle > 0 && handle < MAX_HANDLES) {
+        if (handles[handle].valid == 1) {
+            return 1;
+        }
+    }
+    ERROR("ERROR: Invalid handle");
+    return 0;
+}
+
+static int get_queue_info(char *path, int *cqid, int *sqid)
+{
+
+    key_t server_queue_key;
+    int rx_qid, server_qid;
+
+    INFO("Attempting to contact a RoT service queue");
+
+    if ((rx_qid = msgget(IPC_PRIVATE, 0660)) == -1) {
+        ERROR("msgget: rx_qid");
+        return -1;
+    }
+
+    if ((server_queue_key = ftok(path, PROJECT_ID)) == -1) {
+        ERROR("ftok");
+        return -2;
+    }
+
+    if ((server_qid = msgget(server_queue_key, 0)) == -1) {
+        ERROR("msgget: server_qid");
+        return -3;
+    }
+
+    *cqid = rx_qid;
+    *sqid = server_qid;
+
+    return 0;
+}
+
+static psa_status_t process_response(int rx_qid, vectors_t *vecs, int type,
+                                     int *internal_server_qid)
+{
+
+    struct message response, request;
+    psa_status_t ret = PSA_ERROR_CONNECTION_REFUSED;
+    size_t invec_seek[4] = { 0 };
+    size_t data_size;
+    psa_status_t invec, outvec; /* TODO: Should these be size_t ? */
+
+    assert(internal_server_qid > 0);
+
+    while (1) {
+        data_size = 0;
+        invec = 0;
+        outvec = 0;
+
+        // read response from server
+        if (msgrcv(rx_qid, &response, sizeof(struct message_text), 0, 0) == -1) {
+            ERROR("   msgrcv failed");
+            return ret;
+        }
+
+        // process return message from server
+        switch (response.message_type) {
+            case PSA_REPLY:
+                memcpy(&ret, response.message_text.buf, sizeof(psa_status_t));
+                INFO("   Message received from server: %d", ret);
+                if (type == PSA_IPC_CONNECT && ret > 0) {
+                    *internal_server_qid = ret;
+                    INFO("   ASSSIGNED q ID %d", *internal_server_qid);
+                    ret = PSA_SUCCESS;
+                }
+                return ret;
+                break;
+            case READ_REQUEST:
+                /* read data request */
+                request.message_type = READ_RESPONSE;
+
+                assert(vecs != 0);
+
+                memcpy(&invec, response.message_text.buf, sizeof(psa_status_t));
+                memcpy(&data_size, response.message_text.buf+sizeof(size_t), sizeof(size_t));
+                INFO("   Partition asked for %lu bytes from invec %d", data_size, invec);
+
+                /* need to add more checks here */
+                assert(invec >= 0 && invec < PSA_MAX_IOVEC);
+
+                if (data_size > MAX_FRAGMENT_SIZE) {
+                    data_size = MAX_FRAGMENT_SIZE;
+                }
+
+                /* send response */
+                INFO("   invec_seek[invec] is %lu", invec_seek[invec]);
+                INFO("   Reading from offset %p", vecs->in_vec[invec].base + invec_seek[invec]);
+                memcpy(request.message_text.buf,
+                       (vecs->in_vec[invec].base + invec_seek[invec]),
+                       data_size);
+
+                /* update invec base TODO: check me */
+                invec_seek[invec] = invec_seek[invec] + data_size;
+
+                INFO("   Sending message of type %li", request.message_type);
+                INFO("       with content %s", request.message_text.buf);
+
+                if (msgsnd(*internal_server_qid, &request,
+                           sizeof(int) + sizeof(uint32_t) + data_size, 0) == -1) {
+                    ERROR("Internal error: failed to respond to read request");
+                }
+                break;
+            case WRITE_REQUEST:
+                assert(vecs != 0);
+
+                request.message_type = WRITE_RESPONSE;
+
+                memcpy(&outvec, response.message_text.buf, sizeof(psa_status_t));
+                memcpy(&data_size, response.message_text.buf + sizeof(size_t), sizeof(size_t));
+                INFO("   Partition wants to write %lu bytes to outvec %d", data_size, outvec);
+
+                assert(outvec >= 0 && outvec < PSA_MAX_IOVEC);
+
+                /* copy memory into message and send back amount written */
+                size_t sofar = vecs->out_vec[outvec].len;
+                memcpy(vecs->out_vec[outvec].base + sofar,
+                       response.message_text.buf+(sizeof(size_t)*2), data_size);
+                INFO("   Data size is %lu", data_size);
+                vecs->out_vec[outvec].len += data_size;
+
+                INFO("   Sending message of type %li", request.message_type);
+
+                /* send response */
+                if (msgsnd(*internal_server_qid, &request, sizeof(int) + data_size, 0) == -1) {
+                    ERROR("Internal error: failed to respond to write request");
+                }
+                break;
+            case SKIP_REQUEST:
+                memcpy(&invec, response.message_text.buf, sizeof(psa_status_t));
+                memcpy(&data_size, response.message_text.buf+sizeof(size_t), sizeof(size_t));
+                INFO("   Partition asked to skip %lu bytes in invec %d", data_size, invec);
+                assert(invec >= 0 && invec < PSA_MAX_IOVEC);
+                /* update invec base TODO: check me */
+                invec_seek[invec] = invec_seek[invec] + data_size;
+                break;
+
+            default:
+                FATAL("   ERROR: unknown internal message type: %ld",
+                      response.message_type);
+                return ret;
+        }
+    }
+}
+
+static psa_status_t send(int rx_qid, int server_qid, int *internal_server_qid,
+                         int32_t type, uint32_t minor_version, vectors_t *vecs)
+{
+    {
+        psa_status_t ret = PSA_ERROR_CONNECTION_REFUSED;
+        size_t request_msg_size = (sizeof(int) + sizeof(long)); /* msg type plus queue id */
+        struct message request;
+        request.message_type = 1; /* TODO: change this */
+        request.message_text.psa_type = type;
+        vector_sizes_t vec_sizes;
+
+        /* If the client is non-secure then set the NS bit */
+        if (__psa_ff_client_security_state != 0) {
+            request.message_type |= NON_SECURE;
+        }
+
+        assert(request.message_type >= 0);
+
+        INFO("SEND: Sending message of type %ld with psa_type %d", request.message_type, type);
+        INFO("     internal_server_qid = %i", *internal_server_qid);
+
+        request.message_text.qid = rx_qid;
+
+        if (type == PSA_IPC_CONNECT) {
+            memcpy(request.message_text.buf, &minor_version, sizeof(minor_version));
+            request_msg_size = request_msg_size + sizeof(minor_version);
+            INFO("   Request msg size is %lu", request_msg_size);
+        } else {
+            assert(internal_server_qid > 0);
+        }
+
+        if (vecs != NULL && type >= PSA_IPC_CALL) {
+
+            memset(&vec_sizes, 0, sizeof(vec_sizes));
+
+            /* Copy invec sizes */
+            for (size_t i = 0; i < (vecs->in_len); i++) {
+                vec_sizes.invec_sizes[i] = vecs->in_vec[i].len;
+                INFO("   Client sending vector %lu: %lu", i, vec_sizes.invec_sizes[i]);
+            }
+
+            /* Copy outvec sizes */
+            for (size_t i = 0; i < (vecs->out_len); i++) {
+                vec_sizes.outvec_sizes[i] = vecs->out_vec[i].len;
+
+                /* Reset to 0 since we need to eventually fill in with bytes written */
+                vecs->out_vec[i].len = 0;
+            }
+
+            memcpy(request.message_text.buf, &vec_sizes, sizeof(vec_sizes));
+            request_msg_size = request_msg_size + sizeof(vec_sizes);
+        }
+
+        INFO("   Sending and then waiting");
+
+        // send message to server
+        if (msgsnd(server_qid, &request, request_msg_size, 0) == -1) {
+            ERROR("   msgsnd failed");
+            return ret;
+        }
+
+        return process_response(rx_qid, vecs, type, internal_server_qid);
+    }
+}
+
+
+uint32_t psa_framework_version(void)
+{
+    return PSA_FRAMEWORK_VERSION;
+}
+
+psa_handle_t psa_connect(uint32_t sid, uint32_t minor_version)
+{
+
+    int idx;
+    psa_status_t ret;
+    char pathname[PATHNAMESIZE] = { 0 };
+
+    idx = get_next_free_handle();
+
+    /* if there's a free handle available */
+    if (idx >= 0) {
+        snprintf(pathname, PATHNAMESIZE - 1, TMP_FILE_BASE_PATH "psa_service_%u", sid);
+        INFO("Attempting to contact RoT service at %s", pathname);
+
+        /* if communication is possible */
+        if (get_queue_info(pathname, &handles[idx].client_qid, &handles[idx].server_qid) >= 0) {
+
+            ret = send(handles[idx].client_qid,
+                       handles[idx].server_qid,
+                       &handles[idx].internal_server_qid,
+                       PSA_IPC_CONNECT,
+                       minor_version,
+                       NULL);
+
+            /* if connection accepted by RoT service */
+            if (ret >= 0) {
+                handles[idx].valid = 1;
+                return idx;
+            } else {
+                INFO("Server didn't like you");
+            }
+        } else {
+            INFO("Couldn't contact RoT service. Does it exist?");
+
+            if (__psa_ff_client_security_state == 0) {
+                ERROR("Invalid SID");
+            }
+        }
+    }
+
+    INFO("Couldn't obtain a free handle");
+    return PSA_ERROR_CONNECTION_REFUSED;
+}
+
+uint32_t psa_version(uint32_t sid)
+{
+    int idx;
+    psa_status_t ret;
+    char pathname[PATHNAMESIZE] = { 0 };
+
+    idx = get_next_free_handle();
+
+    if (idx >= 0) {
+        snprintf(pathname, PATHNAMESIZE, TMP_FILE_BASE_PATH "psa_service_%u", sid);
+        if (get_queue_info(pathname, &handles[idx].client_qid, &handles[idx].server_qid) >= 0) {
+            ret = send(handles[idx].client_qid,
+                       handles[idx].server_qid,
+                       &handles[idx].internal_server_qid,
+                       VERSION_REQUEST,
+                       0,
+                       NULL);
+            INFO("psa_version: Recieved from server %d", ret);
+            if (ret > 0) {
+                return ret;
+            }
+        }
+    }
+    INFO("psa_version failed: does the service exist?");
+    return PSA_VERSION_NONE;
+}
+
+psa_status_t psa_call(psa_handle_t handle,
+                      int32_t type,
+                      const psa_invec *in_vec,
+                      size_t in_len,
+                      psa_outvec *out_vec,
+                      size_t out_len)
+{
+
+    handle_is_valid(handle);
+
+    if ((in_len + out_len) > PSA_MAX_IOVEC) {
+        ERROR("Too many iovecs: %lu + %lu", in_len, out_len);
+    }
+
+    vectors_t vecs = { 0 };
+    vecs.in_vec = in_vec;
+    vecs.in_len = in_len;
+    vecs.out_vec = out_vec;
+    vecs.out_len = out_len;
+
+    return send(handles[handle].client_qid,
+                handles[handle].server_qid,
+                &handles[handle].internal_server_qid,
+                type,
+                0,
+                &vecs);
+}
+
+void psa_close(psa_handle_t handle)
+{
+    handle_is_valid(handle);
+    if (send(handles[handle].client_qid, handles[handle].server_qid,
+             &handles[handle].internal_server_qid, PSA_IPC_DISCONNECT, 0, NULL)) {
+        ERROR("ERROR: Couldn't send disconnect msg");
+    } else {
+        if (msgctl(handles[handle].client_qid, IPC_RMID, NULL) != 0) {
+            ERROR("ERROR: Failed to delete msg queue");
+        }
+    }
+    INFO("Closing handle %u", handle);
+    handles[handle].valid = 0;
+}
diff --git a/tests/psa-client-server/psasim/src/common.c b/tests/psa-client-server/psasim/src/common.c
new file mode 100644
index 0000000..287bb50
--- /dev/null
+++ b/tests/psa-client-server/psasim/src/common.c
@@ -0,0 +1,8 @@
+/* Common code between clients and services */
+
+/*
+ *  Copyright The Mbed TLS Contributors
+ *  SPDX-License-Identifier: Apache-2.0 OR GPL-2.0-or-later
+ */
+
+#include "psa/common.h"
diff --git a/tests/psa-client-server/psasim/src/service.c b/tests/psa-client-server/psasim/src/service.c
new file mode 100644
index 0000000..b2b6a08
--- /dev/null
+++ b/tests/psa-client-server/psasim/src/service.c
@@ -0,0 +1,668 @@
+/* PSA Firmware Framework service API */
+
+/*
+ *  Copyright The Mbed TLS Contributors
+ *  SPDX-License-Identifier: Apache-2.0 OR GPL-2.0-or-later
+ */
+
+#include <sys/types.h>
+#include <sys/ipc.h>
+#include <sys/msg.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <strings.h>
+#include <unistd.h>
+#include <time.h>
+#include <assert.h>
+
+#include "psa/service.h"
+#include "psasim/init.h"
+#include "psa/error.h"
+#include "psa/common.h"
+#include "psa/util.h"
+
+#define MAX_CLIENTS 128
+#define MAX_MESSAGES 32
+
+#define SLEEP_MS        50
+
+struct connection {
+    uint32_t client;
+    void *rhandle;
+    int client_to_server_q;
+};
+
+/* Note that this implementation is functional and not secure. */
+extern int __psa_ff_client_security_state;
+
+static psa_msg_t messages[MAX_MESSAGES]; /* Message slots */
+static uint8_t pending_message[MAX_MESSAGES] = { 0 }; /* Booleans indicating active message slots */
+static uint32_t message_client[MAX_MESSAGES] = { 0 }; /* Each client's response queue  */
+static int nsacl[32];
+static int strict_policy[32] = { 0 };
+static uint32_t rot_svc_versions[32];
+static int rot_svc_incoming_queue[32] = { -1 };
+static struct connection connections[MAX_CLIENTS] = { { 0 } };
+
+static uint32_t exposed_signals = 0;
+
+void print_vectors(vector_sizes_t *sizes)
+{
+    INFO("Printing iovec sizes");
+    for (int j = 0; j < PSA_MAX_IOVEC; j++) {
+        INFO("Invec  %d: %lu", j, sizes->invec_sizes[j]);
+    }
+
+    for (int j = 0; j < PSA_MAX_IOVEC; j++) {
+        INFO("Outvec %d: %lu", j, sizes->outvec_sizes[j]);
+    }
+}
+
+int find_connection(uint32_t client)
+{
+    for (int i = 1; i < MAX_CLIENTS; i++) {
+        if (client == connections[i].client) {
+            return i;
+        }
+    }
+    return -1;
+}
+
+void destroy_connection(uint32_t client)
+{
+    int idx = find_connection(client);
+    if (idx >= 0) {
+        connections[idx].client = 0;
+        connections[idx].rhandle = 0;
+        INFO("Destroying connection");
+    } else {
+        ERROR("Couldn't destroy connection for %u", client);
+    }
+}
+
+int find_free_connection()
+{
+    INFO("Allocating connection");
+    return find_connection(0);
+}
+
+static void reply(psa_handle_t msg_handle, psa_status_t status)
+{
+    pending_message[msg_handle] = 1;
+    psa_reply(msg_handle, status);
+    pending_message[msg_handle] = 0;
+}
+
+psa_signal_t psa_wait(psa_signal_t signal_mask, uint32_t timeout)
+{
+    psa_signal_t mask;
+    struct message msg;
+    vector_sizes_t sizes;
+    struct msqid_ds qinfo;
+    uint32_t requested_version;
+    ssize_t len;
+    int idx;
+#if !defined(PSASIM_USE_USLEEP)
+    const struct timespec ts_delay = { .tv_sec = 0, .tv_nsec = SLEEP_MS * 1000000 };
+#endif
+
+    if (timeout == PSA_POLL) {
+        INFO("psa_wait: Called in polling mode");
+    }
+
+    do {
+        mask = signal_mask;
+
+        /* Check the status of each queue */
+        for (int i = 0; i < 32; i++) {
+            if (mask & 0x1) {
+                if (i < 3) {
+                    // do nothing (reserved)
+                } else if (i == 3) {
+                    // this must be psa doorbell
+                } else {
+                    /* Check if this signal corresponds to a queue */
+                    if (rot_svc_incoming_queue[i] >= 0 && (pending_message[i] == 0)) {
+
+                        /* AFAIK there is no "peek" method in SysV, so try to get a message */
+                        len = msgrcv(rot_svc_incoming_queue[i],
+                                     &msg,
+                                     sizeof(struct message_text),
+                                     0,
+                                     IPC_NOWAIT);
+                        if (len > 0) {
+
+                            INFO("Storing that QID in message_client[%d]", i);
+                            INFO("The message handle will be %d", i);
+
+                            msgctl(rot_svc_incoming_queue[i], IPC_STAT, &qinfo);
+                            messages[i].client_id = qinfo.msg_lspid; /* PID of last msgsnd(2) call */
+                            message_client[i] = msg.message_text.qid;
+                            idx = find_connection(msg.message_text.qid);
+
+                            if (msg.message_type & NON_SECURE) {
+                                /* This is a non-secure message */
+
+                                /* Check if NS client is allowed for this RoT service */
+                                if (nsacl[i] <= 0) {
+#if 0
+                                    INFO(
+                                        "Rejecting non-secure client due to manifest security policy");
+                                    reply(i, PSA_ERROR_CONNECTION_REFUSED);
+                                    continue; /* Skip to next signal */
+#endif
+                                }
+
+                                msg.message_type &= ~(NON_SECURE); /* clear */
+                                messages[i].client_id = messages[i].client_id * -1;
+                            }
+
+                            INFO("Got a message from client ID %d", messages[i].client_id);
+                            INFO("Message type is %lu", msg.message_type);
+                            INFO("PSA message type is %d", msg.message_text.psa_type);
+
+                            messages[i].handle = i;
+
+                            switch (msg.message_text.psa_type) {
+                                case PSA_IPC_CONNECT:
+
+                                    if (len >= 16) {
+                                        memcpy(&requested_version, msg.message_text.buf,
+                                               sizeof(requested_version));
+                                        INFO("Requesting version %u", requested_version);
+                                        INFO("Implemented version %u", rot_svc_versions[i]);
+                                        /* TODO: need to check whether the policy is strict,
+                                         * and if so, then reject the client if the number doesn't match */
+
+                                        if (requested_version > rot_svc_versions[i]) {
+                                            INFO(
+                                                "Rejecting client because requested version that was too high");
+                                            reply(i, PSA_ERROR_CONNECTION_REFUSED);
+                                            continue; /* Skip to next signal */
+                                        }
+
+                                        if (strict_policy[i] == 1 &&
+                                            (requested_version != rot_svc_versions[i])) {
+                                            INFO(
+                                                "Rejecting client because enforcing a STRICT version policy");
+                                            reply(i, PSA_ERROR_CONNECTION_REFUSED);
+                                            continue; /* Skip to next signal */
+                                        } else {
+                                            INFO("Not rejecting client");
+                                        }
+                                    }
+
+                                    messages[i].type = PSA_IPC_CONNECT;
+
+                                    if (idx < 0) {
+                                        idx = find_free_connection();
+                                    }
+
+                                    if (idx >= 0) {
+                                        connections[idx].client = msg.message_text.qid;
+                                    } else {
+                                        /* We've run out of system wide connections */
+                                        reply(i, PSA_ERROR_CONNECTION_BUSY);
+                                        ERROR("Ran out of free connections");
+                                        continue;
+                                    }
+
+                                    break;
+                                case PSA_IPC_DISCONNECT:
+                                    messages[i].type = PSA_IPC_DISCONNECT;
+                                    break;
+                                case VERSION_REQUEST:
+                                    INFO("Got a version request");
+                                    reply(i, rot_svc_versions[i]);
+                                    continue; /* Skip to next signal */
+                                    break;
+
+                                default:
+
+                                    /* PSA CALL */
+                                    if (msg.message_text.psa_type >= 0) {
+                                        messages[i].type = msg.message_text.psa_type;
+                                        memcpy(&sizes, msg.message_text.buf, sizeof(sizes));
+                                        print_vectors(&sizes);
+                                        memcpy(&messages[i].in_size, &sizes.invec_sizes,
+                                               (sizeof(size_t) * PSA_MAX_IOVEC));
+                                        memcpy(&messages[i].out_size, &sizes.outvec_sizes,
+                                               (sizeof(size_t) * PSA_MAX_IOVEC));
+                                    } else {
+                                        FATAL("UNKNOWN MESSAGE TYPE RECEIVED %li",
+                                              msg.message_type);
+                                    }
+                                    break;
+                            }
+                            messages[i].handle = i;
+
+                            /* Check if the client has a connection */
+                            if (idx >= 0) {
+                                messages[i].rhandle = connections[idx].rhandle;
+                            } else {
+                                /* Client is begging for a programmer error */
+                                reply(i, PSA_ERROR_PROGRAMMER_ERROR);
+                                continue;
+                            }
+
+                            /* House keeping */
+                            pending_message[i] = 1; /* set message as pending */
+                            exposed_signals |= (0x1 << i); /* assert the signal */
+                        }
+                    }
+                }
+                mask = mask >> 1;
+            }
+        }
+
+        if ((timeout == PSA_BLOCK) && (exposed_signals > 0)) {
+            break;
+        } else {
+            /* There is no 'select' function in SysV to block on multiple queues, so busy-wait :( */
+#if defined(PSASIM_USE_USLEEP)
+            usleep(SLEEP_MS * 1000);
+#else /* PSASIM_USE_USLEEP */
+            nanosleep(&ts_delay, NULL);
+#endif /* PSASIM_USE_USLEEP */
+        }
+    } while (timeout == PSA_BLOCK);
+
+    /* Assert signals */
+    return signal_mask & exposed_signals;
+}
+
+static int signal_to_index(psa_signal_t signal)
+{
+    int i;
+    int count = 0;
+    int ret = -1;
+
+    for (i = 0; i < 32; i++) {
+        if (signal & 0x1) {
+            ret = i;
+            count++;
+        }
+        signal = signal >> 1;
+    }
+
+    if (count > 1) {
+        ERROR("ERROR: Too many signals");
+        return -1; /* Too many signals */
+    }
+    return ret;
+}
+
+static void clear_signal(psa_signal_t signal)
+{
+    exposed_signals = exposed_signals & ~signal;
+}
+
+void raise_signal(psa_signal_t signal)
+{
+    exposed_signals |= signal;
+}
+
+psa_status_t psa_get(psa_signal_t signal, psa_msg_t *msg)
+{
+    int index = signal_to_index(signal);
+    if (index < 0) {
+        ERROR("Bad signal");
+    }
+
+    clear_signal(signal);
+
+    assert(messages[index].handle != 0);
+
+    if (pending_message[index] == 1) {
+        INFO("There is a pending message!");
+        memcpy(msg, &messages[index], sizeof(struct psa_msg_t));
+        assert(msg->handle != 0);
+        return PSA_SUCCESS;
+    } else {
+        INFO("no pending message");
+    }
+
+    return PSA_ERROR_DOES_NOT_EXIST;
+}
+
+static inline int is_valid_msg_handle(psa_handle_t h)
+{
+    if (h > 0 && h < MAX_MESSAGES) {
+        return 1;
+    }
+    ERROR("Not a valid message handle");
+    return 0;
+}
+
+static inline int is_call_msg(psa_handle_t h)
+{
+    assert(messages[h].type >= PSA_IPC_CALL);
+    return 1;
+}
+
+void psa_set_rhandle(psa_handle_t msg_handle, void *rhandle)
+{
+    is_valid_msg_handle(msg_handle);
+    int idx = find_connection(message_client[msg_handle]);
+    INFO("Setting rhandle to %p", rhandle);
+    assert(idx >= 0);
+    connections[idx].rhandle = rhandle;
+}
+
+/* Sends a message from the server to the client. Does not wait for a response */
+static void send_msg(psa_handle_t msg_handle,
+                     int ctrl_msg,
+                     psa_status_t status,
+                     size_t amount,
+                     const void *data,
+                     size_t data_amount)
+{
+    struct message response;
+    int flags = 0;
+
+    assert(ctrl_msg > 0); /* According to System V, it must be greater than 0 */
+
+    response.message_type = ctrl_msg;
+    if (ctrl_msg == PSA_REPLY) {
+        memcpy(response.message_text.buf, &status, sizeof(psa_status_t));
+    } else if (ctrl_msg == READ_REQUEST || ctrl_msg == WRITE_REQUEST || ctrl_msg == SKIP_REQUEST) {
+        memcpy(response.message_text.buf, &status, sizeof(psa_status_t));
+        memcpy(response.message_text.buf+sizeof(size_t), &amount, sizeof(size_t));
+        if (ctrl_msg == WRITE_REQUEST) {
+            /* TODO: Check if too big */
+            memcpy(response.message_text.buf + (sizeof(size_t) * 2), data, data_amount);
+        }
+    }
+
+    /* TODO: sizeof doesn't need to be so big here for small responses */
+    if (msgsnd(message_client[msg_handle], &response, sizeof(response.message_text), flags) == -1) {
+        ERROR("Failed to reply");
+    }
+}
+
+static size_t skip(psa_handle_t msg_handle, uint32_t invec_idx, size_t num_bytes)
+{
+    if (num_bytes < (messages[msg_handle].in_size[invec_idx] - num_bytes)) {
+        messages[msg_handle].in_size[invec_idx] = messages[msg_handle].in_size[invec_idx] -
+                                                  num_bytes;
+        return num_bytes;
+    } else {
+        if (num_bytes >= messages[msg_handle].in_size[invec_idx]) {
+            size_t ret = messages[msg_handle].in_size[invec_idx];
+            messages[msg_handle].in_size[invec_idx] = 0;
+            return ret;
+        } else {
+            return num_bytes;
+        }
+    }
+}
+
+size_t psa_read(psa_handle_t msg_handle, uint32_t invec_idx,
+                void *buffer, size_t num_bytes)
+{
+    size_t sofar = 0;
+    struct message msg = { 0 };
+    int idx;
+    ssize_t len;
+
+    is_valid_msg_handle(msg_handle);
+    is_call_msg(msg_handle);
+
+    if (invec_idx >= PSA_MAX_IOVEC) {
+        ERROR("Invalid iovec number");
+    }
+
+    /* If user wants more data than what's available, truncate their request */
+    if (num_bytes > messages[msg_handle].in_size[invec_idx]) {
+        num_bytes = messages[msg_handle].in_size[invec_idx];
+    }
+
+    while (sofar < num_bytes) {
+        INFO("Server: requesting %lu bytes from client", (num_bytes - sofar));
+        send_msg(msg_handle, READ_REQUEST, invec_idx, (num_bytes - sofar), NULL, 0);
+
+        idx = find_connection(message_client[msg_handle]);
+        assert(idx >= 0);
+
+        len = msgrcv(connections[idx].client_to_server_q, &msg, sizeof(struct message_text), 0, 0);
+        len = (len - sizeof(msg.message_text.qid));
+
+        if (len < 0) {
+            FATAL("Internal error: failed to dispatch read request to the client");
+        }
+
+        if (len > (num_bytes - sofar)) {
+            if ((num_bytes - sofar) > 0) {
+                memcpy(buffer+sofar, msg.message_text.buf, (num_bytes - sofar));
+            }
+        } else {
+            memcpy(buffer + sofar, msg.message_text.buf, len);
+        }
+
+        INFO("Printing what i got so far: %s", msg.message_text.buf);
+
+        sofar = sofar + len;
+    }
+
+    /* Update the seek count */
+    skip(msg_handle, invec_idx, num_bytes);
+    INFO("Finished psa_read");
+    return sofar;
+}
+
+void psa_write(psa_handle_t msg_handle, uint32_t outvec_idx,
+               const void *buffer, size_t num_bytes)
+{
+
+    size_t sofar = 0;
+    struct message msg = { 0 };
+    int idx;
+    ssize_t len;
+
+    is_valid_msg_handle(msg_handle);
+    is_call_msg(msg_handle);
+
+    if (outvec_idx >= PSA_MAX_IOVEC) {
+        ERROR("Invalid iovec number");
+    }
+
+    if (num_bytes > messages[msg_handle].out_size[outvec_idx]) {
+        ERROR("Program tried to write too much data %lu/%lu", num_bytes,
+              messages[msg_handle].out_size[outvec_idx]);
+    }
+
+    while (sofar < num_bytes) {
+        size_t sending = (num_bytes - sofar);
+        if (sending >= MAX_FRAGMENT_SIZE) {
+            sending = MAX_FRAGMENT_SIZE - (sizeof(size_t) * 2);
+        }
+
+        INFO("Server: sending %lu bytes to client", sending);
+
+        send_msg(msg_handle, WRITE_REQUEST, outvec_idx, sending, buffer, sending);
+
+        idx = find_connection(message_client[msg_handle]);
+        assert(idx >= 0);
+
+        len = msgrcv(connections[idx].client_to_server_q, &msg, sizeof(struct message_text), 0, 0);
+        if (len < 1) {
+            FATAL("Client didn't give me a full response");
+        }
+        sofar = sofar + len;
+    }
+
+    /* Update the seek count */
+    messages[msg_handle].out_size[outvec_idx] -= num_bytes;
+}
+
+size_t psa_skip(psa_handle_t msg_handle, uint32_t invec_idx, size_t num_bytes)
+{
+
+    is_valid_msg_handle(msg_handle);
+    is_call_msg(msg_handle);
+
+    size_t ret = skip(msg_handle, invec_idx, num_bytes);
+
+    /* notify client to skip */
+    send_msg(msg_handle, SKIP_REQUEST, invec_idx, num_bytes, NULL, 0);
+    return ret;
+}
+
+static void destroy_temporary_queue(int myqid)
+{
+
+    if (msgctl(myqid, IPC_RMID, NULL) != 0) {
+        INFO("ERROR: Failed to delete msg queue %d", myqid);
+    }
+}
+
+static int make_temporary_queue()
+{
+    int myqid;
+    if ((myqid = msgget(IPC_PRIVATE, 0660)) == -1) {
+        INFO("msgget: myqid");
+        return -1;
+    }
+    return myqid;
+}
+
+/**
+ * Assumes msg_handle is the index into the message array
+ */
+void psa_reply(psa_handle_t msg_handle, psa_status_t status)
+{
+    int idx, q;
+    is_valid_msg_handle(msg_handle);
+
+    if (pending_message[msg_handle] != 1) {
+        ERROR("Not a valid message handle");
+    }
+
+    if (messages[msg_handle].type == PSA_IPC_CONNECT) {
+        switch (status) {
+            case PSA_SUCCESS:
+                idx = find_connection(message_client[msg_handle]);
+                q = make_temporary_queue();
+                if (q > 0 && idx >= 0) {
+                    connections[idx].client_to_server_q = q;
+                    status = q;
+                } else {
+                    FATAL("What happened?");
+                }
+                break;
+            case PSA_ERROR_CONNECTION_REFUSED:
+                destroy_connection(message_client[msg_handle]);
+                break;
+            case PSA_ERROR_CONNECTION_BUSY:
+                destroy_connection(message_client[msg_handle]);
+                break;
+            case PSA_ERROR_PROGRAMMER_ERROR:
+                destroy_connection(message_client[msg_handle]);
+                break;
+            default:
+                ERROR("Not a valid reply %d", status);
+        }
+    } else if (messages[msg_handle].type == PSA_IPC_DISCONNECT) {
+        idx = find_connection(message_client[msg_handle]);
+        if (idx >= 0) {
+            destroy_temporary_queue(connections[idx].client_to_server_q);
+        }
+        destroy_connection(message_client[msg_handle]);
+    }
+
+    send_msg(msg_handle, PSA_REPLY, status, 0, NULL, 0);
+
+    pending_message[msg_handle] = 0;
+    message_client[msg_handle] = 0;
+}
+
+/* TODO: make sure you only clear interrupt signals, and not others */
+void psa_eoi(psa_signal_t signal)
+{
+    int index = signal_to_index(signal);
+    if (index >= 0 && (rot_svc_incoming_queue[index] >= 0)) {
+        clear_signal(signal);
+    } else {
+        ERROR("Tried to EOI a signal that isn't an interrupt");
+    }
+}
+
+void psa_notify(int32_t partition_id)
+{
+    char pathname[PATHNAMESIZE] = { 0 };
+
+    if (partition_id < 0) {
+        ERROR("Not a valid secure partition");
+    }
+
+    snprintf(pathname, PATHNAMESIZE, "/tmp/psa_notify_%u", partition_id);
+    INFO("psa_notify: notifying partition %u using %s",
+         partition_id, pathname);
+    INFO("psa_notify is unimplemented");
+}
+
+void psa_clear(void)
+{
+    clear_signal(PSA_DOORBELL);
+}
+
+void __init_psasim(const char **array,
+                   int size,
+                   const int allow_ns_clients_array[32],
+                   const uint32_t versions[32],
+                   const int strict_policy_array[32])
+{
+
+    static uint8_t library_initialised = 0;
+    key_t key;
+    int qid;
+    FILE *fp;
+    char doorbell_path[PATHNAMESIZE] = { 0 };
+    char queue_path[PATHNAMESIZE];
+    snprintf(doorbell_path, PATHNAMESIZE, TMP_FILE_BASE_PATH "psa_notify_%u", getpid());
+
+    if (library_initialised > 0) {
+        return;
+    } else {
+        library_initialised = 1;
+    }
+
+    if (size != 32) {
+        FATAL("Unsupported value. Aborting.");
+    }
+
+    array[3] = doorbell_path;
+
+    for (int i = 0; i < 32; i++) {
+        if (strncmp(array[i], "", 1) != 0) {
+            INFO("Setting up %s", array[i]);
+            memset(queue_path, 0, sizeof(queue_path));
+            sprintf(queue_path, "%s%s", TMP_FILE_BASE_PATH, array[i]);
+
+            /* Create file if doesn't exist */
+            fp = fopen(queue_path, "ab+");
+            if (fp) {
+                fclose(fp);
+            }
+
+            if ((key = ftok(queue_path, PROJECT_ID)) == -1) {
+                FATAL("Error finding message queue during initialisation");
+            }
+
+            /* TODO: Investigate. Permissions are likely to be too relaxed */
+            if ((qid = msgget(key, IPC_CREAT | 0660)) == -1) {
+                FATAL("Error opening message queue during initialisation");
+            } else {
+                rot_svc_incoming_queue[i] = qid;
+            }
+        }
+    }
+
+    memcpy(nsacl, allow_ns_clients_array, sizeof(int) * 32);
+    memcpy(strict_policy, strict_policy_array, sizeof(int) * 32);
+    memcpy(rot_svc_versions, versions, sizeof(uint32_t) * 32);
+    memset(&connections, 0, sizeof(struct connection) * MAX_CLIENTS);
+
+    __psa_ff_client_security_state = 0; /* Set the client status to SECURE */
+}