Initial prototype client/server app

pstop_c/CMakeLists.txt

@@ -20,3 +20,18 @@ target_link_libraries(machine_app PUBLIC
   pstop
   transport_udp
 )
+
+add_executable(client_app
+  examples/client/client_app.c
+)
+
+target_include_directories(client_app PUBLIC
+  ${CMAKE_CURRENT_SOURCE_DIR}/pstop/include
+  ${CMAKE_CURRENT_SOURCE_DIR}/transport/include
+  include
+)
+
+target_link_libraries(client_app PUBLIC
+  pstop
+  transport_udp
+)

pstop_c/examples/client/client_app.c

@@ -0,0 +1,155 @@
+
+// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc.
+// SPDX-License-Identifier: Apache-2.0
+
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "pstop/pstop_msg.h"
+#include "transport/udp/udp_transport.h"
+#include "pstop/device_id.h"
+#include "pstop/os.h"
+
+udp_transport_data_t udp_transport;
+
+int
+read_msg(udp_transport_data_t *transport, pstop_os_env *env, pstop_msg_t *resp, uint64_t timeout)
+{
+    struct sockaddr_storage client;
+    uint8_t respbytes[PSTOP_MESSAGE_SIZE];
+
+    uint64_t start = env->get_time_cb();
+
+    while(1) {
+        int result = transport_udp_read(&udp_transport, respbytes, PSTOP_MESSAGE_SIZE, &client);
+        if(result == PSTOP_MESSAGE_SIZE) {
+            pstop_message_decode(resp, respbytes);
+            return 1;
+        }
+        uint64_t now = env->get_time_cb();
+        if((now - start) >= timeout) {
+            fprintf(stderr, "Timeout reading response\n");
+            break;
+        }
+    }
+
+    return 0;
+}
+
+void
+send_bond(udp_transport_data_t *transport, pstop_os_env *env, const device_id_t *uuid)
+{
+    uint8_t reqbytes[PSTOP_MESSAGE_SIZE];
+
+    pstop_msg_t req_msg;
+    pstop_msg_t resp_msg;
+
+    req_msg.message = PSTOP_MESSAGE_BOND;
+    device_id_copy(&req_msg.id, uuid);
+
+    pstop_message_encode(&req_msg, reqbytes);
+
+    transport_udp_write(transport, reqbytes, PSTOP_MESSAGE_SIZE, NULL);
+
+    if(read_msg(transport, env, &resp_msg, 2000)) {
+        fprintf(stderr, "Received msg: %d\n", resp_msg.message);
+    }
+}
+
+void
+send_ok(udp_transport_data_t *transport, pstop_os_env *env, const device_id_t *uuid, int is_ok)
+{
+    uint8_t reqbytes[PSTOP_MESSAGE_SIZE];
+
+    pstop_msg_t req_msg;
+    pstop_msg_t resp_msg;
+
+    if(is_ok) {
+        req_msg.message = PSTOP_MESSAGE_OK;
+    }
+    else {
+        req_msg.message = PSTOP_MESSAGE_STOP;
+    }
+
+    device_id_copy(&req_msg.id, uuid);
+
+    pstop_message_encode(&req_msg, reqbytes);
+
+    transport_udp_write(transport, reqbytes, PSTOP_MESSAGE_SIZE, NULL);
+
+    if(read_msg(transport, env, &resp_msg, 2000)) {
+        fprintf(stderr, "Received msg: %d\n", resp_msg.message);
+    }
+}
+
+void
+send_unbond(udp_transport_data_t *transport, pstop_os_env *env, const device_id_t *uuid)
+{
+    uint8_t reqbytes[PSTOP_MESSAGE_SIZE];
+
+    pstop_msg_t req_msg;
+    pstop_msg_t resp_msg;
+
+    req_msg.message = PSTOP_MESSAGE_UNBOND;
+    device_id_copy(&req_msg.id, uuid);
+
+    pstop_message_encode(&req_msg, reqbytes);
+
+    transport_udp_write(transport, reqbytes, PSTOP_MESSAGE_SIZE, NULL);
+
+    if(read_msg(transport, env, &resp_msg, 2000)) {
+        fprintf(stderr, "Received msg: %d\n", resp_msg.message);
+    }
+}
+
+int
+main(int argc, char *argv[])
+{
+    transport_udp_init(&udp_transport);
+
+    if(argc < 3) {
+        printf("USAGE:\n");
+        printf("  %s <port> <uuid-byte>\n\n", argv[0]);
+        printf("Example:\n");
+        printf("  client_app 8890 15\n");
+        return -1;
+    }
+
+    int port = atoi(argv[1]);
+    int uuid_byte = atoi(argv[2]);
+
+    int result = transport_udp_connect(&udp_transport, "127.0.0.1", port);
+    if(result < 0) {
+        fprintf(stderr, "Unable to open UDP: %d\n", result);
+        return -1;
+    }
+
+    uint8_t reqbytes[PSTOP_MESSAGE_SIZE];
+    uint8_t respbytes[PSTOP_MESSAGE_SIZE];
+
+    pstop_os_env env;
+    pstop_os_env_init(&env);
+
+    pstop_msg_t req_msg;
+    pstop_msg_t resp_msg;
+
+    device_id_t uuid = {
+        .data = {
+            0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08,
+            0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F, 0xFF
+        }
+    };
+    uuid.data[15] = (uint8_t)(uuid_byte & 0xFFU);
+
+    fprintf(stderr, "uuid=%d\n", uuid.data[15]);
+    send_bond(&udp_transport, &env, &uuid);
+
+    for(int i = 0; i < 3; ++i) {
+        send_ok(&udp_transport, &env, &uuid, (i % 2));
+    }
+
+    send_unbond(&udp_transport, &env, &uuid);
+
+    return 0;
+}

pstop_c/examples/machine/machine_app.c

@@ -3,6 +3,7 @@
 // SPDX-License-Identifier: Apache-2.0
 
 #include <stdio.h>
+#include <stdlib.h>
 
 #include "transport/udp/udp_transport.h"
 #include "pstop/machine.h"
@@ -23,18 +24,37 @@ int is_operator_allowed(const device_id_t *device_id)
     return 1;
 }
 
+pstop_status_message_t lastStatus = 0;
+
+static
+int
+robot_status(pstop_status_message_t status)
+{
+    if(lastStatus != status) {
+        fprintf(stderr, "Status = %d\n", (int)status);
+        lastStatus = status;
+    }
+
+    return 0;
+}
+
 int
 main(int argc, char *argv[])
 {
-    transport_init_udp(&udp_transport);
+    transport_udp_init(&udp_transport);
     pstop_application_init(&pstop_app);
 
     pstop_app.app_config.default_timeout_ms = 1000U;
     pstop_app.operator_allowed_cb = is_operator_allowed;
+    pstop_app.status_cb = robot_status;
 
     machine_init(&machine, &pstop_app, pstop_clients, MAX_CLIENTS);
 
-    int result = transport_udp_open(&udp_transport, "localhost", 8890);
+    int port = 8890;
+    if(argc > 1) {
+        port = atoi(argv[1]);
+    }
+    int result = transport_udp_listen(&udp_transport, "127.0.0.1", port);
     if(result < 0) {
         fprintf(stderr, "Unable to open UDP: %d\n", result);
         return -1;
@@ -47,21 +67,25 @@ main(int argc, char *argv[])
     pstop_msg_t resp_msg;
     pstop_msg_t *resp_msg_ptr = &resp_msg;
 
-    fprintf(stderr, "Connected to localhost:8890\n");
+    fprintf(stderr, "Connected to localhost:%d\n", port);
     while(1) {
         struct sockaddr_storage client;
         result = transport_udp_read(&udp_transport, reqbytes, PSTOP_MESSAGE_SIZE, &client);
 
         if(result == PSTOP_MESSAGE_SIZE) {
-            pstop_message_encode(&req_msg, reqbytes);
+            pstop_message_decode(&req_msg, reqbytes);
 
+            fprintf(stderr, "Got message: %d from %d\n", req_msg.message, req_msg.id.data[15]);
             pstop_error_t error = machine.handle_protocol_message_cb(&machine, &req_msg, &resp_msg_ptr);
             if(resp_msg_ptr != NULL) {
                 pstop_message_encode(&resp_msg, respbytes);
                 transport_udp_write(&udp_transport, respbytes, PSTOP_MESSAGE_SIZE, (struct sockaddr_in *)&client);
             }
+            else {
+                fprintf(stderr, "Invalid response: %d\n", (int)error);
+            }
         }
-        machine.check_heartbeats_cb(&machine);
+        //machine.check_heartbeats_cb(&machine);
     }
 
     transport_udp_close(&udp_transport);

pstop_c/pstop/CMakeLists.txt

@@ -9,6 +9,7 @@ add_library(pstop
   src/pstop/checksum.c
   src/pstop/device_id.c
   src/pstop/machine.c
+  src/pstop/os.c
   src/pstop/protocol.c
   src/pstop/pstop_application.c
   src/pstop/pstop_client.c

pstop_c/pstop/include/pstop/os.h

@@ -0,0 +1,22 @@
+// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc.
+// SPDX-License-Identifier: Apache-2.0
+
+#ifndef PSTOP_OS_H
+#define PSTOP_OS_H
+
+#include <stdint.h>
+
+typedef uint64_t (* get_current_time_t)(void);
+
+typedef struct pstop_os_env {
+
+    /**
+     * Callback to return the current time.
+     */
+    get_current_time_t get_time_cb;
+
+} pstop_os_env;
+
+void pstop_os_env_init(pstop_os_env *env);
+
+#endif /* PSTOP_OS_H */

pstop_c/pstop/include/pstop/pstop_application.h

@@ -8,13 +8,13 @@
 
 #include "pstop/error.h"
 #include "pstop/device_id.h"
+#include "pstop/os.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);
 
@@ -34,10 +34,7 @@ typedef struct {
 
 typedef struct pstop_application_t {
 
-    /**
-     * Callback to return the current time.
-     */
-    get_current_time_t get_time_cb;
+    pstop_os_env env;
 
     /**
      * The device ID for this machine

pstop_c/pstop/src/pstop/machine.c

@@ -36,7 +36,7 @@ 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();
+    uint64_t now = application->env.get_time_cb();
 
     device_id_copy(&(client->client_id), &(msg->id));
     client->last_timestamp = now;
@@ -223,7 +223,7 @@ machine_handle_message(pstop_machine_t *machine, const pstop_msg_t *req, pstop_m
         init_new_client(machine->application, client, req);
     }
 
-    uint64_t now = machine->application->get_time_cb();
+    uint64_t now = machine->application->env.get_time_cb();
     client->last_timestamp = now;
 
     switch(req->message) {
@@ -245,7 +245,7 @@ static
 pstop_error_t
 machine_check_heartbeats(pstop_machine_t *machine)
 {
-    uint64_t now = machine->application->get_time_cb();
+    uint64_t now = machine->application->env.get_time_cb();
 
     int needsStop = 0;
 
@@ -314,5 +314,7 @@ machine_stop_robot(pstop_machine_t *machine)
     machine->robot_state.restart_state = ROBOT_RESTART_STATE_NEED_STOP;
     machine->robot_state.client_stop_id = 0U;
 
-    machine->application->status_cb(PSTOP_STATUS_STOP);
+    if(machine->application->status_cb != NULL) {
+        machine->application->status_cb(PSTOP_STATUS_STOP);
+    }
 }

pstop_c/pstop/src/pstop/os.c

@@ -0,0 +1,12 @@
+
+// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc.
+// SPDX-License-Identifier: Apache-2.0
+
+#include "pstop/os.h"
+#include "pstop/time.h"
+
+void
+pstop_os_env_init(pstop_os_env *env)
+{
+    env->get_time_cb = time_get_now;
+}

pstop_c/pstop/src/pstop/protocol.c

@@ -31,6 +31,13 @@ check_message_order(pstop_client_data_t *client, const pstop_msg_t *msg)
     return PSTOP_OK;
 }
 
+static
+int
+is_checksum_valid(const pstop_msg_t *req)
+{
+    return 1;
+}
+
 static
 pstop_error_t
 validate_message(pstop_machine_t *machine, pstop_client_data_t *client, const pstop_msg_t *req, pstop_msg_t **resp)
@@ -65,7 +72,12 @@ validate_message(pstop_machine_t *machine, pstop_client_data_t *client, const ps
 pstop_error_t
 protocol_handle_message(pstop_machine_t *machine, const pstop_msg_t *req, pstop_msg_t **resp)
 {
-    // validate checksum
+    // validate black channel bits
+
+    if(!is_checksum_valid(req)) {
+        *resp = NULL;
+        return PSTOP_MSG_INVALID_CHECKSUM;
+    }
 
     pstop_client_data_t *client = pstop_client_get(&(machine->pstops), &(req->id));
 
@@ -74,10 +86,12 @@ protocol_handle_message(pstop_machine_t *machine, const pstop_msg_t *req, pstop_
     }
     else {
         // validate lost/out of order message
+        #if 0
         pstop_error_t result = validate_message(machine, client, req, resp);
         if(result != PSTOP_OK) {
             return result;
         }
+            #endif
     }
 
     // now send the message to the machine for pstop handling.