diff --git a/pstop_c/CMakeLists.txt b/pstop_c/CMakeLists.txt
new file mode 100644
index 0000000..01373a0
--- /dev/null
+++ b/pstop_c/CMakeLists.txt
@@ -0,0 +1,5 @@
+cmake_minimum_required(VERSION 3.12)
+project(PSTOP C)
+
+add_subdirectory(pstop)
+
diff --git a/pstop_c/pstop/CMakeLists.txt b/pstop_c/pstop/CMakeLists.txt
new file mode 100644
index 0000000..ec76f40
--- /dev/null
+++ b/pstop_c/pstop/CMakeLists.txt
@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.12)
+project(PSTOPCore C)
+
+find_package(unity REQUIRED)
+
+add_library(pstop
+  src/pstop/checksum.c
+  src/pstop/device_id.c
+  src/pstop/machine.c
+  src/pstop/protocol.c
+  src/pstop/pstop_application.c
+  src/pstop/pstop_client.c
+  src/pstop/pstop_msg.c
+  src/pstop/time.c
+)
+
+target_compile_options(pstop PRIVATE -Wall -fanalyzer)
+
+target_include_directories(pstop PUBLIC include)
+
+add_executable(pstop_test
+  test/src/pstop/device_id_test.c
+  test/src/pstop/machine_test.c
+  test/src/pstop/pstop_client_test.c
+
+  test/src/pstop/test_utils.c
+
+  test/src/pstop/main.c
+)
+
+target_include_directories(pstop_test PUBLIC
+  include
+  test/include
+)
+
+target_link_libraries(pstop_test PUBLIC
+  pstop
+  unity
+)
diff --git a/pstop_c/pstop/include/pstop/checksum.h b/pstop_c/pstop/include/pstop/checksum.h
new file mode 100644
index 0000000..ab15794
--- /dev/null
+++ b/pstop_c/pstop/include/pstop/checksum.h
@@ -0,0 +1,9 @@
+#ifndef PSTOP_CHECKSUM_H
+#define PSTOP_CHECKSUM_H
+
+#include <stdint.h>
+#include <stddef.h>
+
+uint16_t checksum_crc16(const uint8_t *data, size_t data_length);
+
+#endif /* PSTOP_CHECKSUM_H */
diff --git a/pstop_c/pstop/include/pstop/constants.h b/pstop_c/pstop/include/pstop/constants.h
new file mode 100644
index 0000000..7f92c43
--- /dev/null
+++ b/pstop_c/pstop/include/pstop/constants.h
@@ -0,0 +1,12 @@
+#ifndef PSTOP_CONSTANTS_H
+#define PSTOP_CONSTANTS_H
+
+#define PSTOP_SECOND_MS 1000U
+#define PSTOP_MINUTE_MS (PSTOP_SECOND_MS * 60U)
+#define PSTOP_HOUR_MS (PSTOP_MINUTE_MS * 60U)
+#define PSTOP_DAY_MS (PSTOP_HOUR_MS * 24U)
+
+#define PSTOP_MIN_HEARTBEAT 10U
+#define PSTOP_MAX_HEARTBEAT (PSTOP_HOUR_MS * 1U)
+
+#endif /* PSTOP_CONSTANTS_H */
diff --git a/pstop_c/pstop/include/pstop/device_id.h b/pstop_c/pstop/include/pstop/device_id.h
new file mode 100644
index 0000000..65598f2
--- /dev/null
+++ b/pstop_c/pstop/include/pstop/device_id.h
@@ -0,0 +1,29 @@
+#ifndef PSTOP_DEVICE_ID_H
+#define PSTOP_DEVICE_ID_H
+
+#include <stdint.h>
+
+#define DEVICE_ID_LENGTH 16
+
+typedef struct {
+    uint8_t data[DEVICE_ID_LENGTH];
+} device_id_t;
+
+/**
+ * Initializes the specified device to 0.
+ */
+void device_id_init(device_id_t *device_id);
+
+/**
+ * Copies the device ID from id to device_id.
+ */
+void device_id_copy(device_id_t *device_id, const device_id_t *id);
+
+/**
+ * Compares two device ids.
+ *
+ * @Return 0 if lhs == rhs. Otherwise returns non-zero.
+ */
+int device_id_cmp(const device_id_t *lhs, const device_id_t *rhs);
+
+#endif /* PSTOP_DEVICE_ID_H */
diff --git a/pstop_c/pstop/include/pstop/error.h b/pstop_c/pstop/include/pstop/error.h
new file mode 100644
index 0000000..f90f478
--- /dev/null
+++ b/pstop_c/pstop/include/pstop/error.h
@@ -0,0 +1,24 @@
+#ifndef PSTOP_ERROR_H
+#define PSTOP_ERROR_H
+
+typedef enum {
+
+    PSTOP_OK = 0,
+    PSTOP_OPERATOR_NOT_ALLOWED = 1,
+    PSTOP_OUT_OF_OPERATOR_SPACE = 2,
+
+    PSTOP_MSG_LOST = 10,
+    PSTOP_MSG_REPETITION = 11,
+    PSTOP_MSG_DELAYED = 12,
+    PSTOP_MSG_OUT_OF_ORDER = 13,
+    PSTOP_MSG_INVALID_CHECKSUM = 14,
+
+    PSTOP_HEARTBEAT_INVALID = 20,
+    PSTOP_MESSAGE_TYPE_INVALID = 21,
+    PSTOP_MISSED_HEARTBEATS = 22,
+
+    PSTOP_FATAL = 255
+
+} pstop_error_t;
+
+#endif /* PSTOP_ERROR_H */
diff --git a/pstop_c/pstop/include/pstop/machine.h b/pstop_c/pstop/include/pstop/machine.h
new file mode 100644
index 0000000..60062f7
--- /dev/null
+++ b/pstop_c/pstop/include/pstop/machine.h
@@ -0,0 +1,79 @@
+#ifndef PSTOP_MACHINE_H
+#define PSTOP_MACHINE_H
+
+#include "pstop/pstop_msg.h"
+#include "pstop/error.h"
+#include "pstop/pstop_application.h"
+#include "pstop/pstop_client.h"
+
+typedef struct pstop_machine_t pstop_machine_t;
+
+/**
+ * A function callback to handle protocol (black channel) messages.
+ */
+typedef pstop_error_t (* protocol_handle_message_t)(pstop_machine_t *machine, const pstop_msg_t *req, pstop_msg_t **resp);
+
+/**
+ * A function callback to handle new messages that have arrived.
+ *
+ * @Param machine The machine object
+ * @Param req     The request message
+ * @Param resp    A pointer to a response message. If NULL then no response is sent back.
+ */
+typedef pstop_error_t (* machine_handle_message_t)(pstop_machine_t *machine, const pstop_msg_t *req, pstop_msg_t **resp);
+
+/**
+ * A function callback that is called to check the heartbeats of the connected PSTOPs
+ *
+ * @Param machine The machine object
+ */
+typedef pstop_error_t (* pstop_check_heartbeats_t)(pstop_machine_t *machine);
+
+#define ROBOT_STATE_OK      0
+#define ROBOT_STATE_STOPPED 1
+
+#define ROBOT_RESTART_STATE_OK 0
+#define ROBOT_RESTART_STATE_NEED_STOP 1
+#define ROBOT_RESTART_STATE_STOP_RECEIVED 2
+
+typedef struct {
+
+    /* Is the robot stopped or OK? */
+    int robot_state;
+
+    /**
+     * The client ID that caused the robot to stop if stopped.
+     * 0 if robot is not stopped or if applicaton just started
+     * Or the client ID that has started the stop/ok cycle to get robot moving.
+     */
+    uint32_t client_stop_id;
+
+    /**
+     * 0 = everything is OK
+     * 1 = need stop message
+     * 2 = stop received
+     */
+    int restart_state;
+} robot_state_t;
+
+typedef struct pstop_machine_t {
+
+    protocol_handle_message_t handle_protocol_message_cb;
+
+    machine_handle_message_t handle_machine_message_cb;
+
+    pstop_check_heartbeats_t check_heartbeats_cb;
+
+    pstop_clients_t pstops;
+
+    pstop_application_t application;
+
+    robot_state_t robot_state;
+
+} pstop_machine_t;
+
+void machine_init(pstop_machine_t *machine, const pstop_application_t *app, pstop_client_data_t *clients, uint16_t max_clients);
+
+void machine_stop_robot(pstop_machine_t *machine, pstop_client_data_t *client);
+
+#endif /* PSTOP_MACHINE_H */
diff --git a/pstop_c/pstop/include/pstop/protocol.h b/pstop_c/pstop/include/pstop/protocol.h
new file mode 100644
index 0000000..01d42bc
--- /dev/null
+++ b/pstop_c/pstop/include/pstop/protocol.h
@@ -0,0 +1,10 @@
+#ifndef PSTOP_PROTOCOL_H
+#define PSTOP_PROTOCOL_H
+
+#include "pstop/error.h"
+#include "pstop/pstop_msg.h"
+#include "pstop/machine.h"
+
+pstop_error_t protocol_handle_message(pstop_machine_t *machine, const pstop_msg_t *req, pstop_msg_t **resp);
+
+#endif /* PSTOP_PROTOCOL_H */
diff --git a/pstop_c/pstop/include/pstop/pstop_application.h b/pstop_c/pstop/include/pstop/pstop_application.h
new file mode 100644
index 0000000..ad463ca
--- /dev/null
+++ b/pstop_c/pstop/include/pstop/pstop_application.h
@@ -0,0 +1,68 @@
+#ifndef PSTOP_PSTOP_APPLICATION_H
+#define PSTOP_PSTOP_APPLICATION_H
+
+#include <stdint.h>
+
+#include "pstop/error.h"
+#include "pstop/device_id.h"
+
+typedef enum {
+    PSTOP_STATUS_OK = 0,
+    PSTOP_STATUS_STOP = 1
+} pstop_status_message_t;
+
+typedef uint64_t (* get_current_time_t)(void);
+typedef int      (* is_operator_allowed_t)(const device_id_t *device_id);
+typedef int      (* pstop_status_t)(pstop_status_message_t status);
+
+typedef void     (* log_message_t)(pstop_error_t error, const char *message);
+
+typedef struct {
+    /**
+     * Default timeout for all new clients.
+     */
+    uint64_t default_timeout_ms;
+
+    uint16_t max_lost_messages;
+
+    uint16_t max_missed_heartbeats;
+
+} pstop_application_config_t;
+
+typedef struct pstop_application_t {
+
+    /**
+     * Callback to return the current time.
+     */
+    get_current_time_t get_time_cb;
+
+    /**
+     * The device ID for this machine
+     */
+    device_id_t machine_device_id;
+
+    /**
+     * Simple access control to determine if an operator
+     * is allowed to connect to this machine.
+     *
+     * @Return 1 if operator is allowed, 0 otherwise
+     */
+    is_operator_allowed_t operator_allowed_cb;
+
+    /**
+     * Notifies the underlying hardware about the status of this PSTOP.
+     */
+    pstop_status_t status_cb;
+
+    /**
+     * Callback to handle any errors in the system.
+     */
+    log_message_t log_message_cb;
+
+    pstop_application_config_t app_config;
+
+} pstop_application_t;
+
+void pstop_application_init(pstop_application_t *app);
+
+#endif /* PSTOP_PSTOP_APPLICATION_H */
diff --git a/pstop_c/pstop/include/pstop/pstop_client.h b/pstop_c/pstop/include/pstop/pstop_client.h
new file mode 100644
index 0000000..e5c17b6
--- /dev/null
+++ b/pstop_c/pstop/include/pstop/pstop_client.h
@@ -0,0 +1,91 @@
+#ifndef PSTOP_PSTOP_CLIENT_H
+#define PSTOP_PSTOP_CLIENT_H
+
+#include <stdint.h>
+
+#include "pstop/device_id.h"
+#include "pstop/constants.h"
+
+typedef enum {
+
+    PSTOP_CLIENT_BONDED = 1,
+    PSTOP_CLIENT_UNBONDED = 2,
+    PSTOP_CLIENT_STOPPED = 3,
+    PSTOP_CLIENT_FAILURE = 4,
+    PSTOP_CLIENT_UNKNOWN = 255
+
+} pstop_client_state_t;
+
+typedef struct {
+
+    uint32_t local_client_id;
+
+    device_id_t client_id;
+
+    // last time we've heard from this client
+    uint64_t last_timestamp;
+
+    // last time we've heard from this client
+    uint64_t last_received_heartbeat;
+
+    // how frequently we should hear from this client
+    uint64_t heartbeat_ms;
+
+    // the counter indicating each message we are sending
+    uint32_t msg_counter;
+
+    uint32_t last_counter;
+
+    // approx how far off is the client's clock from this clock
+    // compare the incoming pstop_msg.stamp to this clock.
+    // Ignoring network transit time
+    int32_t clock_drift;
+
+    // how many messages have we lost so far?
+    uint16_t lost_message_counter;
+
+    // how many heartbeats have we missed?
+    uint16_t missed_heartbeats_counter;
+
+    pstop_client_state_t client_state;
+
+} pstop_client_data_t;
+
+typedef struct {
+
+    pstop_client_data_t *clients;
+    uint16_t max_clients;
+    uint16_t num_clients;
+
+} pstop_clients_t;
+
+/**
+ * Initialize the pstop_client_data_t structure to default values.
+ */
+void pstop_client_init(pstop_client_data_t *client);
+
+/**
+ * Initializes a pstop_clients object to default values.
+ */
+void pstop_clients_init(pstop_clients_t *clients);
+
+/**
+ * Returns an empty pstop_client if one is available
+ *
+ * @Return the client data if successful, NULL if no space available.
+ */
+pstop_client_data_t *pstop_client_get_free_client(pstop_clients_t *clients);
+
+/**
+ * Removes the specified pstop client by the device ID.
+ */
+void pstop_client_remove(pstop_clients_t *clients, const device_id_t *client_id);
+
+/**
+ * Finds the specified pstop client by device ID.
+ *
+ * @Return the requsted client if found or NULL if not found.
+ */
+pstop_client_data_t *pstop_client_get(pstop_clients_t *clients, const device_id_t *client_id);
+
+#endif /* PSTOP_PSTOP_CLIENT_H */
diff --git a/pstop_c/pstop/include/pstop/pstop_msg.h b/pstop_c/pstop/include/pstop/pstop_msg.h
new file mode 100644
index 0000000..1592f18
--- /dev/null
+++ b/pstop_c/pstop/include/pstop/pstop_msg.h
@@ -0,0 +1,79 @@
+#ifndef PSTOP_PSTOP_MSG_H
+#define PSTOP_PSTOP_MSG_H
+
+#include <stdint.h>
+
+#include "pstop/device_id.h"
+#include "pstop/error.h"
+
+typedef uint8_t message_type_t;
+
+#define PSTOP_MESSAGE_OK 0U
+#define PSTOP_MESSAGE_STOP 1U
+#define PSTOP_MESSAGE_BOND 2U
+#define PSTOP_MESSAGE_UNBOND 3U
+#define PSTOP_MESSAGE_UNKNOWN 0xFFU
+
+/**
+ * A PSTOP message object. With enough information
+ * to provide basic black channel support.
+ */
+typedef struct {
+    /**
+     * The timestamp (in milliseconds) of this message.
+     */
+    uint64_t stamp;
+
+    /**
+     * The last timestamp that we received from the client.
+     * Or 0 if we are estabilishing a new bonding.
+     */
+    uint64_t received_stamp;
+
+    /**
+     * The UUID of this device.
+     */
+    device_id_t id;
+
+    /**
+     * Unique ID of the device we are sending this message to.
+     */
+    device_id_t receiver_id;
+
+    /**
+     * A timeout in milliseconds that we expect to receive heartbeats from
+     * this machine. Only valiud when sent from machine to operator.
+     */
+    uint32_t heartbeat_timeout;
+
+    /**
+     * An incrementing counter.
+     */
+    uint32_t counter;
+
+    /**
+     * The last counter we received from the client.
+     * Or 0 if we are establishing a new bonding.
+     */
+    uint32_t received_counter;
+
+    /**
+     * The type of message.
+     */
+    message_type_t message;
+
+    /**
+     * a CRC-16 checksum of the above values.
+     */
+    uint16_t checksum;
+
+} pstop_msg_t;
+
+void pstop_message_init(pstop_msg_t *msg);
+
+uint16_t pstop_calculate_checksum(const pstop_msg_t *msg);
+
+pstop_error_t pstop_is_message_valid(const pstop_msg_t *msg);
+
+#endif /* PSTOP_PSTOP_MSG_H */
+
diff --git a/pstop_c/pstop/include/pstop/time.h b/pstop_c/pstop/include/pstop/time.h
new file mode 100644
index 0000000..8c52d38
--- /dev/null
+++ b/pstop_c/pstop/include/pstop/time.h
@@ -0,0 +1,9 @@
+#ifndef PSTOP_TIME_H
+#define PSTOP_TIME_H
+
+#include <stdint.h>
+
+uint64_t time_get_now(void);
+
+#endif /* PSTOP_TIME_H */
+
diff --git a/pstop_c/pstop/src/pstop/checksum.c b/pstop_c/pstop/src/pstop/checksum.c
new file mode 100644
index 0000000..2fa8e49
--- /dev/null
+++ b/pstop_c/pstop/src/pstop/checksum.c
@@ -0,0 +1,10 @@
+
+#include <stddef.h>
+
+#include "pstop/checksum.h"
+
+uint16_t
+checksum_crc16(const uint8_t *data, size_t data_length)
+{
+    return 0U;
+}
diff --git a/pstop_c/pstop/src/pstop/device_id.c b/pstop_c/pstop/src/pstop/device_id.c
new file mode 100644
index 0000000..2ab5e8d
--- /dev/null
+++ b/pstop_c/pstop/src/pstop/device_id.c
@@ -0,0 +1,22 @@
+
+#include <string.h>
+
+#include "pstop/device_id.h"
+
+void
+device_id_init(device_id_t *device_id)
+{
+    memset(device_id->data, 0, sizeof(device_id_t));
+}
+
+void
+device_id_copy(device_id_t *device_id, const device_id_t *id)
+{
+    memcpy(device_id->data, id->data, DEVICE_ID_LENGTH);
+}
+
+int
+device_id_cmp(const device_id_t *lhs, const device_id_t *rhs)
+{
+    return memcmp(lhs->data, rhs->data, DEVICE_ID_LENGTH);
+}
diff --git a/pstop_c/pstop/src/pstop/machine.c b/pstop_c/pstop/src/pstop/machine.c
new file mode 100644
index 0000000..66c3e25
--- /dev/null
+++ b/pstop_c/pstop/src/pstop/machine.c
@@ -0,0 +1,293 @@
+
+#include <stdio.h>
+
+#include <stddef.h>
+
+#include "pstop/machine.h"
+#include "pstop/pstop_client.h"
+#include "pstop/constants.h"
+#include "pstop/protocol.h"
+
+static
+pstop_error_t
+add_new_client(pstop_machine_t *machine, const device_id_t *id, pstop_client_data_t **client)
+{
+    // no client found, can we add it?
+    if(!machine->application.operator_allowed_cb(id)) {
+        // This ID is not allowed
+        *client = NULL;
+        return PSTOP_OPERATOR_NOT_ALLOWED;
+    }
+
+    // we're allowing this ID, do we have room for it?
+    *client = pstop_client_get_free_client(&(machine->pstops));
+
+    if(*client == NULL) {
+        return PSTOP_OUT_OF_OPERATOR_SPACE;
+    }
+
+    return PSTOP_OK;
+}
+
+static
+void
+init_new_client(pstop_application_t *application, pstop_client_data_t *client, const pstop_msg_t *msg)
+{
+    uint64_t now = application->get_time_cb();
+
+    device_id_copy(&(client->client_id), &(msg->id));
+    client->last_timestamp = now;
+    client->last_received_heartbeat = 0U;
+    client->heartbeat_ms = application->app_config.default_timeout_ms;
+    client->msg_counter = 1U;
+    client->last_counter = 0U;
+
+    if(now >= msg->stamp) {
+        client->clock_drift = (int64_t)(now - msg->stamp);
+    }
+    else {
+        client->clock_drift = -(int64_t)(msg->stamp - now);
+    }
+}
+
+static
+pstop_error_t
+handle_bond_msg(pstop_machine_t *machine, pstop_client_data_t *client, const pstop_msg_t *msg, pstop_msg_t *resp)
+{
+    // brand new client, let's initialize it
+    init_new_client(&(machine->application), client, msg);
+
+    client->client_state = PSTOP_CLIENT_BONDED;
+
+    if(machine->pstops.num_clients == 1U) {
+        // first client connecting then we'll need a stop/start sequence from anyone
+        machine->robot_state.restart_state = ROBOT_RESTART_STATE_NEED_STOP;
+        machine->robot_state.client_stop_id = 0U;
+        machine->robot_state.robot_state = ROBOT_STATE_STOPPED;
+    }
+
+    // create BOND response
+    resp->message = PSTOP_MESSAGE_BOND;
+
+    return PSTOP_OK;
+}
+
+static
+pstop_error_t
+handle_ok_msg(pstop_machine_t *machine, pstop_client_data_t *client, const pstop_msg_t *msg, pstop_msg_t *resp)
+{
+    if(client->client_state != PSTOP_CLIENT_BONDED) {
+        resp->message = PSTOP_MESSAGE_UNBOND;
+        return PSTOP_OK;
+    }
+
+    if(machine->robot_state.restart_state == ROBOT_RESTART_STATE_NEED_STOP) {
+        resp->message = PSTOP_MESSAGE_STOP;
+        return PSTOP_OK;
+    }
+
+    // this isn't the client that requested the STOP
+    if(machine->robot_state.client_stop_id != 0U) {
+        if(machine->robot_state.client_stop_id != client->local_client_id) {
+            resp->message = PSTOP_MESSAGE_STOP;
+            return PSTOP_OK;
+        }
+    }
+    // either this is the client that started the STOP/OK cycle or we're in a
+    // normal case where STOP/OK has already finished.
+    machine->robot_state.robot_state = ROBOT_STATE_OK;
+    machine->robot_state.restart_state = ROBOT_RESTART_STATE_OK;
+    machine->robot_state.client_stop_id = 0U;
+
+    resp->message = PSTOP_MESSAGE_OK;
+
+    machine->application.status_cb(PSTOP_STATUS_OK);
+
+    return PSTOP_OK;
+}
+
+static
+void
+handle_need_stop(pstop_machine_t *machine, pstop_client_data_t *client, pstop_msg_t *resp)
+{
+    // if another node already started the stop/ok cycle then we'll expect that
+    // node to send us the OK.
+    if(machine->robot_state.client_stop_id == client->local_client_id) {
+        machine->robot_state.robot_state = ROBOT_STATE_STOPPED;
+        machine->robot_state.restart_state = ROBOT_RESTART_STATE_STOP_RECEIVED;
+    }
+    else {
+        if(machine->robot_state.client_stop_id == 0U) {
+            // we have a new client that sent us a STOP message.
+            machine->robot_state.robot_state = ROBOT_STATE_STOPPED;
+            machine->robot_state.client_stop_id = client->local_client_id;
+            machine->robot_state.restart_state = ROBOT_RESTART_STATE_STOP_RECEIVED;
+        }
+    }
+}
+
+static
+pstop_error_t
+handle_stop_msg(pstop_machine_t *machine, pstop_client_data_t *client, const pstop_msg_t *msg, pstop_msg_t *resp)
+{
+    if(client->client_state != PSTOP_CLIENT_BONDED) {
+        resp->message = PSTOP_MESSAGE_UNBOND;
+        return PSTOP_OK;
+    }
+
+    resp->message = PSTOP_MESSAGE_STOP;
+
+    // do we need a stop/ok cycle?
+    if(machine->robot_state.restart_state == ROBOT_RESTART_STATE_NEED_STOP) {
+        handle_need_stop(machine, client, resp);
+    }
+
+    machine->application.status_cb(PSTOP_STATUS_STOP);
+
+    return PSTOP_OK;
+}
+
+static
+pstop_error_t
+handle_unbond_msg(pstop_machine_t *machine, pstop_client_data_t *client, const pstop_msg_t *msg, pstop_msg_t *resp)
+{
+    resp->message = PSTOP_MESSAGE_UNBOND;
+
+    pstop_client_remove(&(machine->pstops), &(msg->id));
+
+    // if this is the last client then stop the robot
+    if(machine->pstops.num_clients == 0U) {
+        machine_stop_robot(machine, NULL);
+    }
+
+    return PSTOP_OK;
+}
+
+static
+pstop_error_t
+machine_handle_message(pstop_machine_t *machine, const pstop_msg_t *req, pstop_msg_t **resp)
+{
+    // validate we have a response message
+    if(resp == NULL) {
+        return PSTOP_FATAL;
+    }
+
+    if(*resp == NULL) {
+        return PSTOP_FATAL;
+    }
+
+    // validate that the message parameters are correct
+    pstop_error_t result = pstop_is_message_valid(req);
+    if(result != PSTOP_OK) {
+       *resp = NULL;
+       return result;
+    }
+
+    (*resp)->heartbeat_timeout = machine->application.app_config.default_timeout_ms;
+
+    // can we find this client?
+    pstop_client_data_t *client = pstop_client_get(&(machine->pstops), &(req->id));
+    if(client == NULL) {
+        // if not, are they requesting something other than bond?
+        if(req->message != PSTOP_MESSAGE_BOND) {
+            // send back UNBOND message
+            (*resp)->message = PSTOP_MESSAGE_UNBOND;
+            return PSTOP_OK;
+        }
+        // can we add this new client?
+        result = add_new_client(machine, &(req->id), &client);
+        if(result != PSTOP_OK) {
+            // send back UNBOND message
+            (*resp)->message = PSTOP_MESSAGE_UNBOND;
+
+            return result;
+        }
+    }
+
+    switch(req->message) {
+    case PSTOP_MESSAGE_BOND:
+        return handle_bond_msg(machine, client, req, *resp);
+    case PSTOP_MESSAGE_UNBOND:
+        return handle_unbond_msg(machine, client, req, *resp);
+    case PSTOP_MESSAGE_OK:
+        return handle_ok_msg(machine, client, req, *resp);
+    default:
+        break;
+    }
+
+    // by default we'll assume any invalid message means stop
+    return handle_stop_msg(machine, client, req, *resp);
+}
+
+static
+pstop_error_t
+machine_check_heartbeats(pstop_machine_t *machine)
+{
+    //const device_id_t *this_client = &machine->application.machine_device_id;
+
+    uint64_t now = machine->application.get_time_cb();
+
+    for(uint16_t i = 0U; i < machine->pstops.num_clients; ++i) {
+        pstop_client_data_t *client = &(machine->pstops.clients[i]);
+
+        // if for some reason now is in the past compared to the last heart beat
+        // then there's no need to check if we've heard from this client
+        if(now <= client->last_received_heartbeat) {
+            continue;
+        }
+
+        uint64_t diff = client->last_received_heartbeat - now;
+
+        // if we're still within the heartbeat timeout then this client is still
+        // good.
+        if(diff <= client->heartbeat_ms) {
+            continue;
+        }
+
+        client->missed_heartbeats_counter++;
+
+        // problem! this client hasn't talked to us in a while
+        // if we're still within the window of missed heartbeats then we're OK
+
+        uint16_t missed = (uint16_t)(diff / client->heartbeat_ms);
+        if(missed > client->missed_heartbeats_counter) {
+            // trigger a stop!
+            machine_stop_robot(machine, client);
+            return PSTOP_MISSED_HEARTBEATS;
+        }
+    }
+
+    return PSTOP_OK;
+}
+
+void
+machine_init(pstop_machine_t *machine, const pstop_application_t *app, pstop_client_data_t *clients, uint16_t max_clients)
+{
+    machine->handle_machine_message_cb = machine_handle_message;
+    machine->handle_protocol_message_cb = protocol_handle_message;
+    machine->check_heartbeats_cb = machine_check_heartbeats;
+
+    machine->application = *app;
+    machine->pstops.clients = clients;
+    machine->pstops.max_clients = max_clients;
+    machine->pstops.num_clients = 0U;
+
+    machine->robot_state.client_stop_id = 0U;
+    machine->robot_state.restart_state = ROBOT_RESTART_STATE_OK;
+    machine->robot_state.robot_state = ROBOT_STATE_OK;
+
+    pstop_clients_init(&(machine->pstops));
+}
+
+void
+machine_stop_robot(pstop_machine_t *machine, pstop_client_data_t *client)
+{
+    machine->robot_state.robot_state = ROBOT_STATE_STOPPED;
+    machine->robot_state.restart_state = ROBOT_RESTART_STATE_NEED_STOP;
+
+    if(client != NULL) {
+        machine->robot_state.client_stop_id = client->local_client_id;
+    }
+
+    machine->application.status_cb(PSTOP_STATUS_STOP);
+}
diff --git a/pstop_c/pstop/src/pstop/protocol.c b/pstop_c/pstop/src/pstop/protocol.c
new file mode 100644
index 0000000..20b15e9
--- /dev/null
+++ b/pstop_c/pstop/src/pstop/protocol.c
@@ -0,0 +1,105 @@
+
+#include <stdlib.h>
+
+#include "pstop/protocol.h"
+#if 0
+static
+void
+init_response_from_request(const pstop_msg_t *req, pstop_msg_t *resp)
+{
+    device_id_copy(&(resp->receiver_id), &(req->id));
+    device_id_copy(&(resp->id), &(req->receiver_id));
+    resp->received_counter = req->counter;
+    resp->received_stamp = req->stamp;
+}
+#endif
+
+static
+pstop_error_t
+check_message_order(pstop_client_data_t *client, const pstop_msg_t *msg)
+{
+    if(client->last_timestamp >= msg->stamp) {
+        return PSTOP_MSG_OUT_OF_ORDER;
+    }
+    if(client->msg_counter >= msg->counter) {
+        return PSTOP_MSG_REPETITION;
+    }
+
+    return PSTOP_OK;
+}
+
+static
+pstop_error_t
+validate_message(pstop_machine_t *machine, pstop_client_data_t *client, const pstop_msg_t *req, pstop_msg_t **resp)
+{
+    // Check the message and ignore it if it's invalid
+    pstop_error_t result = check_message_order(client, req);
+    if(result != PSTOP_OK) {
+        *resp = NULL;
+        return result;
+    }
+
+    // check for lost messages
+    if(req->counter != (client->msg_counter - 1U)) {
+        client->lost_message_counter++;
+        if(client->lost_message_counter >= machine->application.app_config.max_lost_messages) {
+            // too many lost messages
+            // clean up this client
+            *resp = NULL;
+            machine_stop_robot(machine, NULL);
+            return PSTOP_MSG_LOST;
+        }
+    }
+    else {
+        client->lost_message_counter = 0U;
+    }
+
+    client->missed_heartbeats_counter = 0U;
+
+    return PSTOP_OK;
+}
+
+pstop_error_t
+protocol_handle_message(pstop_machine_t *machine, const pstop_msg_t *req, pstop_msg_t **resp)
+{
+    // validate checksum
+
+    pstop_client_data_t *client = pstop_client_get(&(machine->pstops), &(req->id));
+
+    if(client == NULL) {
+        // validate that this is a new client message
+    }
+    else {
+        // validate lost/out of order message
+        pstop_error_t result = validate_message(machine, client, req, resp);
+        if(result != PSTOP_OK) {
+            return result;
+        }
+    }
+
+    // now send the message to the machine for pstop handling.
+    // This function will create a new client if necessary
+    pstop_error_t result = machine->handle_machine_message_cb(machine, req, resp);
+
+    if(result != PSTOP_OK) {
+        return result;
+    }
+
+    if(resp != NULL) {
+        // add in response black channel values
+        client = pstop_client_get(&(machine->pstops), &(req->id));
+        if(client == NULL) {
+            // problem.
+            return PSTOP_FATAL;
+        }
+
+        // let's add response black channel values
+        client->msg_counter++;
+        client->last_timestamp = req->stamp;
+
+        return PSTOP_OK;
+    }
+
+
+    return PSTOP_FATAL;
+}
diff --git a/pstop_c/pstop/src/pstop/pstop_application.c b/pstop_c/pstop/src/pstop/pstop_application.c
new file mode 100644
index 0000000..3a5a3a5
--- /dev/null
+++ b/pstop_c/pstop/src/pstop/pstop_application.c
@@ -0,0 +1,30 @@
+
+#include <stdlib.h>
+
+#include "pstop/pstop_application.h"
+#include "pstop/time.h"
+
+static
+void
+no_log(pstop_error_t /* error */, const char * /* message */)
+{
+
+}
+
+void
+pstop_application_config_init(pstop_application_config_t *config)
+{
+    config->default_timeout_ms = 100U;
+    config->max_lost_messages = 1U;
+    config->max_missed_heartbeats = 0U;
+}
+
+void
+pstop_application_init(pstop_application_t *app)
+{
+    app->get_time_cb = time_get_now;
+    app->operator_allowed_cb = NULL;
+    app->status_cb = NULL;
+    app->log_message_cb = no_log;
+    pstop_application_config_init(&app->app_config);
+}
diff --git a/pstop_c/pstop/src/pstop/pstop_client.c b/pstop_c/pstop/src/pstop/pstop_client.c
new file mode 100644
index 0000000..0919f9c
--- /dev/null
+++ b/pstop_c/pstop/src/pstop/pstop_client.c
@@ -0,0 +1,81 @@
+
+#include <stdlib.h>
+#include <string.h>
+
+#include "pstop/pstop_client.h"
+
+static uint32_t next_client_id = 0U;
+
+void
+pstop_client_init(pstop_client_data_t *client)
+{
+    device_id_init(&(client->client_id));
+    client->last_timestamp = 0U;
+    client->last_received_heartbeat = 0U;
+    client->heartbeat_ms = 0U;
+    client->msg_counter = 0U;
+    client->last_counter = 0U;
+    client->clock_drift = 0U;
+    client->lost_message_counter = 0U;
+    client->missed_heartbeats_counter = 0U;
+    client->client_state = PSTOP_CLIENT_UNKNOWN;
+    client->local_client_id = next_client_id++;
+}
+
+static
+void
+pstop_client_copy(pstop_client_data_t *dest, pstop_client_data_t *src)
+{
+    memcpy(dest, src, sizeof(pstop_client_data_t));
+}
+
+void
+pstop_clients_init(pstop_clients_t *clients)
+{
+    for(uint16_t i = 0U; i < clients->max_clients; ++i) {
+        pstop_client_init(&(clients->clients[i]));
+    }
+}
+
+pstop_client_data_t *
+pstop_client_get_free_client(pstop_clients_t *clients)
+{
+    if(clients->num_clients == clients->max_clients) {
+        return NULL;
+    }
+
+    pstop_client_data_t *client = &(clients->clients[clients->num_clients]);
+    clients->num_clients++;
+
+    pstop_client_init(client);
+
+    return client;
+}
+
+void
+pstop_client_remove(pstop_clients_t *clients, const device_id_t *client_id)
+{
+    // copies the last position client to replace the one we are removing
+    // then clears out the last one
+    for(uint16_t i = 0U; i < clients->num_clients; ++i) {
+        if(device_id_cmp(&(clients->clients[i].client_id), client_id) == 0) {
+            if(clients->num_clients > 1U) {
+                pstop_client_copy(&(clients->clients[i]), &(clients->clients[clients->num_clients - 1U]));
+            }
+            clients->num_clients--;
+            pstop_client_init(&(clients->clients[clients->num_clients]));
+        }
+    }
+}
+
+pstop_client_data_t *
+pstop_client_get(pstop_clients_t *clients, const device_id_t *client_id)
+{
+    for(uint16_t i = 0U; i < clients->num_clients; ++i) {
+        if(device_id_cmp(&(clients->clients[i].client_id), client_id) == 0) {
+            return &(clients->clients[i]);
+        }
+    }
+
+    return NULL;
+}
diff --git a/pstop_c/pstop/src/pstop/pstop_msg.c b/pstop_c/pstop/src/pstop/pstop_msg.c
new file mode 100644
index 0000000..7225ace
--- /dev/null
+++ b/pstop_c/pstop/src/pstop/pstop_msg.c
@@ -0,0 +1,46 @@
+
+#include "pstop/pstop_msg.h"
+#include "pstop/checksum.h"
+#include "pstop/constants.h"
+
+void
+pstop_message_init(pstop_msg_t *msg)
+{
+    msg->stamp = 0U;
+    msg->received_stamp = 0U;
+    device_id_init(&(msg->id));
+    device_id_init(&(msg->receiver_id));
+    msg->heartbeat_timeout = 0U;
+    msg->counter = 0U;
+    msg->received_counter = 0U;
+    msg->message = PSTOP_MESSAGE_UNKNOWN;
+    msg->checksum = 0U;
+}
+
+// this check belongs in the black channel protocol code
+uint16_t
+pstop_calculate_checksum(const pstop_msg_t *msg)
+{
+    // calculate checksum
+    const uint8_t *start = (const uint8_t *)msg;
+    const uint8_t *end = start + (((const uint8_t *)&(msg->checksum)) - start);
+
+    return checksum_crc16(start, (end - start));
+}
+
+static
+int
+is_message_type_valid(uint8_t message)
+{
+    return (message >= PSTOP_MESSAGE_OK) && (message <= PSTOP_MESSAGE_UNBOND);
+}
+
+pstop_error_t
+pstop_is_message_valid(const pstop_msg_t *msg)
+{
+    if(!is_message_type_valid(msg->message)) {
+        return PSTOP_MESSAGE_TYPE_INVALID;
+    }
+
+    return PSTOP_OK;
+}
diff --git a/pstop_c/pstop/src/pstop/time.c b/pstop_c/pstop/src/pstop/time.c
new file mode 100644
index 0000000..24bb9fd
--- /dev/null
+++ b/pstop_c/pstop/src/pstop/time.c
@@ -0,0 +1,19 @@
+
+#include <time.h>
+
+#include "pstop/time.h"
+
+uint64_t
+time_get_now(void)
+{
+#ifdef __linux__
+    struct timespec ts;
+    timespec_get(&ts, CLOCK_REALTIME);
+
+    // Calculate milliseconds
+    return ((uint64_t)ts.tv_sec) * 1000ULL + ((uint64_t)ts.tv_nsec) / 1000000ULL;
+#else
+    return 0U;
+#endif
+}
+
diff --git a/pstop_c/pstop/test/include/pstop/test_utils.h b/pstop_c/pstop/test/include/pstop/test_utils.h
new file mode 100644
index 0000000..322f99e
--- /dev/null
+++ b/pstop_c/pstop/test/include/pstop/test_utils.h
@@ -0,0 +1,8 @@
+#ifndef PSTOP_TEST_UTILS_H
+#define PSTOP_TEST_UTILS_H
+
+#include "pstop/device_id.h"
+
+void device_id_set(device_id_t *device_id, const char *id);
+
+#endif /* PSTOP_TEST_UTILS_H */
diff --git a/pstop_c/pstop/test/src/pstop/device_id_test.c b/pstop_c/pstop/test/src/pstop/device_id_test.c
new file mode 100644
index 0000000..6746adb
--- /dev/null
+++ b/pstop_c/pstop/test/src/pstop/device_id_test.c
@@ -0,0 +1,47 @@
+
+#include "pstop/device_id.h"
+
+#include <unity/unity.h>
+
+#include "pstop/test_utils.h"
+
+static
+void
+test_device_init(void)
+{
+    device_id_t device;
+    for(int i = 0; i < DEVICE_ID_LENGTH; ++i) {
+        device.data[i] = i;
+    }
+
+    device_id_init(&device);
+    for(int i = 0; i < DEVICE_ID_LENGTH; ++i) {
+        TEST_ASSERT_EQUAL(device.data[i], 0U);
+    }
+}
+
+static
+void
+test_device_cmp(void)
+{
+    device_id_t lhs;
+    device_id_set(&lhs, "testing");
+
+    device_id_t rhs;
+    device_id_set(&rhs, "testing");
+
+    TEST_ASSERT_EQUAL(0, device_id_cmp(&lhs, &rhs));
+    device_id_set(&rhs, "test");
+    TEST_ASSERT_NOT_EQUAL(0, device_id_cmp(&lhs, &rhs));
+}
+
+int
+main_device_id_test(void)
+{
+    UnityBegin("device_id_test.c");
+
+    RUN_TEST(test_device_init);
+    RUN_TEST(test_device_cmp);
+
+    return UNITY_END();
+}
diff --git a/pstop_c/pstop/test/src/pstop/machine_test.c b/pstop_c/pstop/test/src/pstop/machine_test.c
new file mode 100644
index 0000000..cf34b7a
--- /dev/null
+++ b/pstop_c/pstop/test/src/pstop/machine_test.c
@@ -0,0 +1,285 @@
+
+#include "pstop/machine.h"
+
+#include <unity/unity.h>
+
+#include "pstop/time.h"
+#include "pstop/test_utils.h"
+
+static uint64_t current_time;
+
+static
+uint64_t
+get_time(void)
+{
+    return current_time++;
+}
+
+static int operator_allowed_flag;
+
+static
+int
+is_operator_allowed(const device_id_t *id)
+{
+    return operator_allowed_flag;
+}
+
+static pstop_status_message_t lastStatus = PSTOP_STATUS_OK;
+static int robot_status_counter = 0;
+
+static
+int
+robot_status(pstop_status_message_t status)
+{
+    lastStatus = status;
+
+    robot_status_counter++;
+
+    return 0;
+}
+
+static
+void
+log_error(pstop_error_t error, const char *message)
+{
+
+}
+
+pstop_application_t pstop_app = {
+    .get_time_cb = get_time,
+    .machine_device_id.data = "testing",
+    .operator_allowed_cb = is_operator_allowed,
+    .status_cb = robot_status,
+    .log_message_cb = log_error,
+    .app_config.default_timeout_ms = 60U,
+    .app_config.max_lost_messages = 1U,
+    .app_config.max_missed_heartbeats = 1U
+};
+
+#define MAX_CLIENTS 2U
+
+pstop_client_data_t pstop_clients[MAX_CLIENTS];
+
+static
+void
+test_new_client_operator_not_allowed(void)
+{
+    pstop_machine_t machine;
+
+    machine_init(&machine, &pstop_app, pstop_clients, MAX_CLIENTS);
+
+    pstop_msg_t msg;
+    pstop_message_init(&msg);
+
+    pstop_msg_t resp_obj;
+    pstop_message_init(&resp_obj);
+    pstop_msg_t *resp = &resp_obj;
+
+    msg.heartbeat_timeout = 50U;
+    msg.message = PSTOP_MESSAGE_BOND;
+
+    operator_allowed_flag = 0;
+    TEST_ASSERT_EQUAL(PSTOP_OPERATOR_NOT_ALLOWED, machine.handle_machine_message_cb(&machine, &msg, &resp));
+    TEST_ASSERT_NOT_NULL(resp);
+}
+
+static
+void
+test_new_client_operator_allowed(void)
+{
+    pstop_machine_t machine;
+
+    machine_init(&machine, &pstop_app, pstop_clients, MAX_CLIENTS);
+
+    device_id_t id;
+    device_id_set(&id, "test");
+
+    pstop_msg_t msg;
+    msg.message = PSTOP_MESSAGE_BOND;
+    device_id_copy(&(msg.id), &id);
+
+    pstop_msg_t resp_obj;
+    pstop_message_init(&resp_obj);
+    pstop_msg_t *resp = &resp_obj;
+
+    operator_allowed_flag = 1;
+    current_time = 100U;
+
+    TEST_ASSERT_EQUAL(PSTOP_OK, machine.handle_machine_message_cb(&machine, &msg, &resp));
+    TEST_ASSERT_EQUAL(PSTOP_MESSAGE_BOND, resp->message);
+}
+
+static
+pstop_client_data_t *
+client_send_ok(pstop_machine_t *machine, const char *device_id, uint8_t respMsg)
+{
+    device_id_t id;
+    device_id_set(&id, device_id);
+
+    pstop_msg_t msg;
+    msg.message = PSTOP_MESSAGE_OK;
+    device_id_copy(&(msg.id), &id);
+
+    pstop_msg_t resp_obj;
+    pstop_message_init(&resp_obj);
+    pstop_msg_t *resp = &resp_obj;
+
+    operator_allowed_flag = 1;
+    current_time = 100U;
+
+    TEST_ASSERT_EQUAL(PSTOP_OK, machine->handle_machine_message_cb(machine, &msg, &resp));
+
+    TEST_ASSERT_EQUAL(respMsg, resp_obj.message);
+
+    return pstop_client_get(&(machine->pstops), &id);
+}
+
+static
+pstop_client_data_t *
+bond_client(pstop_machine_t *machine, const char *device_id)
+{
+    device_id_t id;
+    device_id_set(&id, device_id);
+
+    pstop_msg_t msg;
+    msg.message = PSTOP_MESSAGE_BOND;
+    device_id_copy(&(msg.id), &id);
+
+    pstop_msg_t resp_obj;
+    pstop_message_init(&resp_obj);
+    pstop_msg_t *resp = &resp_obj;
+
+    operator_allowed_flag = 1;
+    current_time = 100U;
+
+    TEST_ASSERT_EQUAL(PSTOP_OK, machine->handle_machine_message_cb(machine, &msg, &resp));
+
+    TEST_ASSERT_EQUAL(PSTOP_MESSAGE_BOND, resp_obj.message);
+
+    return pstop_client_get(&(machine->pstops), &id);
+}
+
+static
+void
+test_bond_req_bond_resp(void)
+{
+    pstop_machine_t machine;
+
+    machine_init(&machine, &pstop_app, pstop_clients, MAX_CLIENTS);
+
+    // BOND => BOND
+    pstop_client_data_t *client = bond_client(&machine, "test");
+    TEST_ASSERT_NOT_NULL(client);
+}
+
+static
+void
+test_ok_req_unbond_resp(void)
+{
+    pstop_machine_t machine;
+
+    machine_init(&machine, &pstop_app, pstop_clients, MAX_CLIENTS);
+
+    // OK => UNBOND
+    pstop_client_data_t *client = client_send_ok(&machine, "test", PSTOP_MESSAGE_UNBOND);
+}
+
+static
+void
+test_bond_unbond(void)
+{
+    pstop_machine_t machine;
+
+    machine_init(&machine, &pstop_app, pstop_clients, MAX_CLIENTS);
+
+    device_id_t id;
+    device_id_set(&id, "test");
+
+    pstop_msg_t msg;
+    msg.message = PSTOP_MESSAGE_BOND;
+    device_id_copy(&(msg.id), &id);
+
+    pstop_msg_t resp_obj;
+    pstop_message_init(&resp_obj);
+    pstop_msg_t *resp = &resp_obj;
+
+    operator_allowed_flag = 1;
+    current_time = 100U;
+
+    TEST_ASSERT_EQUAL(PSTOP_OK, machine.handle_machine_message_cb(&machine, &msg, &resp));
+    TEST_ASSERT_EQUAL(PSTOP_MESSAGE_BOND, resp_obj.message);
+    TEST_ASSERT_NOT_NULL(pstop_client_get(&(machine.pstops), &id));
+
+    msg.message = PSTOP_MESSAGE_UNBOND;
+    pstop_message_init(&resp_obj);
+
+    TEST_ASSERT_EQUAL(PSTOP_OK, machine.handle_machine_message_cb(&machine, &msg, &resp));
+    TEST_ASSERT_EQUAL(PSTOP_MESSAGE_UNBOND, resp_obj.message);
+    TEST_ASSERT_NULL(pstop_client_get(&(machine.pstops), &id));
+    TEST_ASSERT_EQUAL(0U, machine.pstops.num_clients);
+    TEST_ASSERT_EQUAL(PSTOP_STATUS_STOP, lastStatus);
+}
+
+static
+void
+test_bond_ok_stop(void)
+{
+    pstop_machine_t machine;
+
+    machine_init(&machine, &pstop_app, pstop_clients, MAX_CLIENTS);
+
+    device_id_t id;
+    device_id_set(&id, "test");
+
+    pstop_msg_t msg;
+    msg.message = PSTOP_MESSAGE_BOND;
+    device_id_copy(&(msg.id), &id);
+
+    pstop_msg_t resp_obj;
+    pstop_message_init(&resp_obj);
+    pstop_msg_t *resp = &resp_obj;
+
+    operator_allowed_flag = 1;
+    current_time = 100U;
+
+    robot_status_counter = 0;
+
+    TEST_ASSERT_EQUAL(PSTOP_OK, machine.handle_machine_message_cb(&machine, &msg, &resp));
+    TEST_ASSERT_EQUAL(PSTOP_MESSAGE_BOND, resp_obj.message);
+
+    TEST_ASSERT_EQUAL(0, robot_status_counter);
+
+    msg.message = PSTOP_MESSAGE_STOP;
+    TEST_ASSERT_EQUAL(PSTOP_OK, machine.handle_machine_message_cb(&machine, &msg, &resp));
+    TEST_ASSERT_EQUAL(PSTOP_MESSAGE_STOP, resp_obj.message);
+
+    for(int i = 0; i < 10; ++i) {
+        msg.message = PSTOP_MESSAGE_OK;
+        TEST_ASSERT_EQUAL(PSTOP_OK, machine.handle_machine_message_cb(&machine, &msg, &resp));
+        TEST_ASSERT_EQUAL(PSTOP_MESSAGE_OK, resp_obj.message);
+        TEST_ASSERT_EQUAL(PSTOP_STATUS_OK, lastStatus);
+        TEST_ASSERT_EQUAL(i + 2, robot_status_counter);
+    }
+
+    msg.message = PSTOP_MESSAGE_STOP;
+    TEST_ASSERT_EQUAL(PSTOP_OK, machine.handle_machine_message_cb(&machine, &msg, &resp));
+    TEST_ASSERT_EQUAL(PSTOP_MESSAGE_STOP, resp_obj.message);
+    TEST_ASSERT_EQUAL(PSTOP_STATUS_STOP, lastStatus);
+    TEST_ASSERT_EQUAL(12, robot_status_counter);
+}
+
+int
+main_machine_test(void)
+{
+    UnityBegin("machine_test.c");
+
+    RUN_TEST(test_new_client_operator_not_allowed);
+    RUN_TEST(test_new_client_operator_allowed);
+
+    RUN_TEST(test_bond_req_bond_resp);
+    RUN_TEST(test_ok_req_unbond_resp);
+    RUN_TEST(test_bond_unbond);
+    RUN_TEST(test_bond_ok_stop);
+
+    return UNITY_END();
+}
\ No newline at end of file
diff --git a/pstop_c/pstop/test/src/pstop/main.c b/pstop_c/pstop/test/src/pstop/main.c
new file mode 100644
index 0000000..fc00ed3
--- /dev/null
+++ b/pstop_c/pstop/test/src/pstop/main.c
@@ -0,0 +1,20 @@
+
+#include <unity/unity.h>
+
+extern int main_device_id_test(void);
+extern int main_pstop_client_test(void);
+extern int main_machine_test(void);
+
+void setUp(void) {}
+
+void tearDown(void) {}
+
+int
+main(void)
+{
+    main_device_id_test();
+    main_pstop_client_test();
+    main_machine_test();
+
+    return 0;
+}
\ No newline at end of file
diff --git a/pstop_c/pstop/test/src/pstop/pstop_client_test.c b/pstop_c/pstop/test/src/pstop/pstop_client_test.c
new file mode 100644
index 0000000..178b470
--- /dev/null
+++ b/pstop_c/pstop/test/src/pstop/pstop_client_test.c
@@ -0,0 +1,141 @@
+
+#include "pstop/pstop_client.h"
+
+#include <string.h>
+
+#include <unity/unity.h>
+
+#include "pstop/test_utils.h"
+
+static
+void
+assert_empty_client(pstop_client_data_t *client)
+{
+    for(int i = 0; i < DEVICE_ID_LENGTH; ++i) {
+        TEST_ASSERT_EQUAL(0U, client->client_id.data[i]);
+    }
+    TEST_ASSERT_EQUAL(0U, client->last_timestamp);
+    TEST_ASSERT_EQUAL(0U, client->last_received_heartbeat);
+    TEST_ASSERT_EQUAL(0U, client->heartbeat_ms);
+    TEST_ASSERT_EQUAL(0U, client->msg_counter);
+    TEST_ASSERT_EQUAL(0U, client->clock_drift);
+}
+
+static
+void
+test_client_init(void)
+{
+    pstop_client_data_t client;
+    memset(&client, 0x12, sizeof(pstop_client_data_t));
+
+    pstop_client_init(&client);
+    assert_empty_client(&client);
+}
+
+static
+void
+test_clients_init(void)
+{
+    pstop_client_data_t client_data[5];
+    pstop_clients_t clients = {
+        .clients = client_data,
+        .max_clients = 5,
+        .num_clients = 0
+    };
+
+    memset(client_data, 0x13, sizeof(pstop_client_data_t) * 5);
+
+    pstop_clients_init(&clients);
+    for(int i = 0; i < 5; ++i) {
+        assert_empty_client(&client_data[i]);
+    }
+}
+
+static
+void
+test_get_free_client(void)
+{
+    pstop_client_data_t client_data[2];
+    pstop_clients_t clients = {
+        .clients = client_data,
+        .max_clients = 2,
+        .num_clients = 0
+    };
+
+    pstop_clients_init(&clients);
+    TEST_ASSERT_EQUAL(0U, clients.num_clients);
+
+    memset(client_data, 0x13, sizeof(pstop_client_data_t) * 2);
+
+    pstop_client_data_t *c1 = pstop_client_get_free_client(&clients);
+    TEST_ASSERT_NOT_NULL(c1);
+    TEST_ASSERT_EQUAL(1U, clients.num_clients);
+    assert_empty_client(c1);
+
+    pstop_client_data_t *c2 = pstop_client_get_free_client(&clients);
+    TEST_ASSERT_NOT_NULL(c2);
+    TEST_ASSERT_EQUAL(2U, clients.num_clients);
+    assert_empty_client(c2);
+
+    pstop_client_data_t *c3 = pstop_client_get_free_client(&clients);
+    TEST_ASSERT_NULL(c3);
+    TEST_ASSERT_EQUAL(2U, clients.num_clients);
+}
+
+static
+void
+test_remove_client(void)
+{
+    pstop_client_data_t client_data[2];
+    pstop_clients_t clients = {
+        .clients = client_data,
+        .max_clients = 2,
+        .num_clients = 0
+    };
+
+    pstop_clients_init(&clients);
+    TEST_ASSERT_EQUAL(0U, clients.num_clients);
+
+    device_id_t c1_id;
+    device_id_set(&c1_id, "test");
+
+    device_id_t c2_id;
+    device_id_set(&c2_id, "test2");
+    pstop_client_data_t *c1 = pstop_client_get_free_client(&clients);
+    pstop_client_data_t *c2 = pstop_client_get_free_client(&clients);
+
+    device_id_copy(&(c1->client_id), &c1_id);
+    device_id_copy(&(c2->client_id), &c2_id);
+
+    // remove first client and make sure second client was copied over
+    TEST_ASSERT_EQUAL(2U, clients.num_clients);
+    pstop_client_remove(&clients, &(c1->client_id));
+    TEST_ASSERT_EQUAL(1U, clients.num_clients);
+
+    // validate that the client is no longer in the list
+    TEST_ASSERT_NULL(pstop_client_get(&clients, &c1_id));
+
+    // verify that the other client is still available
+    c2 = pstop_client_get(&clients, &c2_id);
+    TEST_ASSERT_NOT_NULL(c2);
+    TEST_ASSERT_EQUAL(0, device_id_cmp(&c2_id, &(c2->client_id)));
+    assert_empty_client(&(clients.clients[1]));
+
+    // now remove last client
+    pstop_client_remove(&clients, &c2_id);
+    TEST_ASSERT_EQUAL(0U, clients.num_clients);
+    assert_empty_client(&(clients.clients[0]));
+}
+
+int
+main_pstop_client_test(void)
+{
+    UnityBegin("pstop_client_test.c");
+
+    RUN_TEST(test_client_init);
+    RUN_TEST(test_clients_init);
+    RUN_TEST(test_get_free_client);
+    RUN_TEST(test_remove_client);
+
+    return UNITY_END();
+}
\ No newline at end of file
diff --git a/pstop_c/pstop/test/src/pstop/pstop_tests.yml b/pstop_c/pstop/test/src/pstop/pstop_tests.yml
new file mode 100644
index 0000000..717e5ab
--- /dev/null
+++ b/pstop_c/pstop/test/src/pstop/pstop_tests.yml
@@ -0,0 +1,3 @@
+:unity:
+  :main_name: auto
+  :framework: 'unity/unity'
\ No newline at end of file
diff --git a/pstop_c/pstop/test/src/pstop/test_utils.c b/pstop_c/pstop/test/src/pstop/test_utils.c
new file mode 100644
index 0000000..bde457c
--- /dev/null
+++ b/pstop_c/pstop/test/src/pstop/test_utils.c
@@ -0,0 +1,19 @@
+
+#include <string.h>
+
+#include "pstop/test_utils.h"
+
+void
+device_id_set(device_id_t *device_id, const char *id)
+{
+    device_id_init(device_id);
+
+    size_t len = strlen(id);
+    if(len < DEVICE_ID_LENGTH) {
+        memcpy(device_id->data, id, len);
+    }
+    else {
+        memcpy(device_id->data, id, DEVICE_ID_LENGTH);
+    }
+}
+