diff --git a/pstop_c/CMakeLists.txt b/pstop_c/CMakeLists.txt index 14eb931..9ac3f99 100644 --- a/pstop_c/CMakeLists.txt +++ b/pstop_c/CMakeLists.txt @@ -4,3 +4,19 @@ cmake_minimum_required(VERSION 3.12) project(PSTOP C) add_subdirectory(pstop) +add_subdirectory(transport) + +add_executable(machine_app + examples/machine/machine_app.c +) + +target_include_directories(machine_app PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/pstop/include + ${CMAKE_CURRENT_SOURCE_DIR}/transport/include + include +) + +target_link_libraries(machine_app PUBLIC + pstop + transport_udp +) diff --git a/pstop_c/examples/machine/machine_app.c b/pstop_c/examples/machine/machine_app.c new file mode 100644 index 0000000..39f2f7f --- /dev/null +++ b/pstop_c/examples/machine/machine_app.c @@ -0,0 +1,70 @@ + +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + +#include + +#include "transport/udp/udp_transport.h" +#include "pstop/machine.h" +#include "pstop/pstop_msg.h" + +pstop_application_t pstop_app; + +#define MAX_CLIENTS 3U + +pstop_client_data_t pstop_clients[MAX_CLIENTS]; + +pstop_machine_t machine; + +udp_transport_data_t udp_transport; + +int is_operator_allowed(const device_id_t *device_id) +{ + return 1; +} + +int +main(int argc, char *argv[]) +{ + transport_init_udp(&udp_transport); + pstop_application_init(&pstop_app); + + pstop_app.app_config.default_timeout_ms = 1000U; + pstop_app.operator_allowed_cb = is_operator_allowed; + + machine_init(&machine, &pstop_app, pstop_clients, MAX_CLIENTS); + + int result = transport_udp_open(&udp_transport, "localhost", 8890); + 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_msg_t req_msg; + pstop_msg_t resp_msg; + pstop_msg_t *resp_msg_ptr = &resp_msg; + + fprintf(stderr, "Connected to localhost:8890\n"); + 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_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); + } + } + machine.check_heartbeats_cb(&machine); + } + + transport_udp_close(&udp_transport); + + return 0; +} diff --git a/pstop_c/pstop/CMakeLists.txt b/pstop_c/pstop/CMakeLists.txt index bc854e6..c718d68 100644 --- a/pstop_c/pstop/CMakeLists.txt +++ b/pstop_c/pstop/CMakeLists.txt @@ -25,6 +25,7 @@ add_executable(pstop_test test/src/pstop/machine_test.c test/src/pstop/machine_timeout_test.c test/src/pstop/pstop_client_test.c + test/src/pstop/pstop_msg_test.c test/src/pstop/test_utils.c diff --git a/pstop_c/pstop/include/pstop/device_id.h b/pstop_c/pstop/include/pstop/device_id.h index 69bb7ce..48956cd 100644 --- a/pstop_c/pstop/include/pstop/device_id.h +++ b/pstop_c/pstop/include/pstop/device_id.h @@ -17,6 +17,8 @@ typedef struct { */ void device_id_init(device_id_t *device_id); +void device_id_set_bytes(device_id_t *device_id, const uint8_t *data); + /** * Copies the device ID from id to device_id. */ diff --git a/pstop_c/pstop/include/pstop/pstop_msg.h b/pstop_c/pstop/include/pstop/pstop_msg.h index 7deee46..d0d51ff 100644 --- a/pstop_c/pstop/include/pstop/pstop_msg.h +++ b/pstop_c/pstop/include/pstop/pstop_msg.h @@ -15,13 +15,25 @@ typedef uint8_t message_type_t; #define PSTOP_MESSAGE_STOP 1U #define PSTOP_MESSAGE_BOND 2U #define PSTOP_MESSAGE_UNBOND 3U -#define PSTOP_MESSAGE_UNKNOWN 0xFFU +#define PSTOP_MESSAGE_UNKNOWN 0x0FU + +#define PSTOP_MESSAGE_SIZE 64U /** * A PSTOP message object. With enough information * to provide basic black channel support. */ typedef struct { + /** + * Version of this message. Currently just 0x0 + */ + uint8_t version; + + /** + * Type of message. + */ + message_type_t message; + /** * The timestamp (in milliseconds) of this message. */ @@ -45,7 +57,7 @@ typedef struct { /** * A timeout in milliseconds that we expect to receive heartbeats from - * this machine. Only valiud when sent from machine to operator. + * this machine. Only valid when sent from machine to operator. */ uint32_t heartbeat_timeout; @@ -60,11 +72,6 @@ typedef struct { */ uint32_t received_counter; - /** - * The type of message. - */ - message_type_t message; - /** * a CRC-16 checksum of the above values. */ @@ -78,4 +85,10 @@ uint16_t pstop_calculate_checksum(const pstop_msg_t *msg); pstop_error_t pstop_is_message_valid(const pstop_msg_t *msg); +/** + * Network encoding/decoding functions + */ +void pstop_message_decode(pstop_msg_t *msg, const uint8_t *data); +void pstop_message_encode(const pstop_msg_t *msg, uint8_t *data); + #endif /* PSTOP_PSTOP_MSG_H */ diff --git a/pstop_c/pstop/src/pstop/device_id.c b/pstop_c/pstop/src/pstop/device_id.c index 3a231c8..ca52925 100644 --- a/pstop_c/pstop/src/pstop/device_id.c +++ b/pstop_c/pstop/src/pstop/device_id.c @@ -12,6 +12,12 @@ device_id_init(device_id_t *device_id) memset(device_id->data, 0, sizeof(device_id_t)); } +void +device_id_set_bytes(device_id_t *device_id, const uint8_t *data) +{ + memcpy(device_id->data, data, DEVICE_ID_LENGTH); +} + void device_id_copy(device_id_t *device_id, const device_id_t *id) { diff --git a/pstop_c/pstop/src/pstop/pstop_application.c b/pstop_c/pstop/src/pstop/pstop_application.c index 9274e2a..0813572 100644 --- a/pstop_c/pstop/src/pstop/pstop_application.c +++ b/pstop_c/pstop/src/pstop/pstop_application.c @@ -19,7 +19,7 @@ pstop_application_config_init(pstop_application_config_t *config) { config->default_timeout_ms = 100U; config->max_lost_messages = 1U; - config->max_missed_heartbeats = 0U; + config->max_missed_heartbeats = 1U; } void diff --git a/pstop_c/pstop/src/pstop/pstop_msg.c b/pstop_c/pstop/src/pstop/pstop_msg.c index 45a87b9..683c5ae 100644 --- a/pstop_c/pstop/src/pstop/pstop_msg.c +++ b/pstop_c/pstop/src/pstop/pstop_msg.c @@ -2,6 +2,8 @@ // SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. // SPDX-License-Identifier: Apache-2.0 +#include + #include "pstop/pstop_msg.h" #include "pstop/checksum.h" #include "pstop/constants.h" @@ -9,6 +11,7 @@ void pstop_message_init(pstop_msg_t *msg) { + msg->version = 0x0U; msg->stamp = 0U; msg->received_stamp = 0U; device_id_init(&(msg->id)); @@ -38,6 +41,162 @@ is_message_type_valid(uint8_t message) return (message >= PSTOP_MESSAGE_OK) && (message <= PSTOP_MESSAGE_UNBOND); } +static +void +read_device_uuid(device_id_t *id, const uint8_t *data, size_t *pos) +{ + memcpy(&(id->data), data + *pos, DEVICE_ID_LENGTH); + *pos = *pos + 16U; +} + +static +void +write_device_uuid(const device_id_t *id, uint8_t *data, size_t *pos) +{ + memcpy(data + *pos, &(id->data), DEVICE_ID_LENGTH); + *pos = *pos + 16U; +} + +static +uint8_t +read_uint8(const uint8_t *data, size_t *pos) +{ + uint8_t b = data[*pos]; + *pos = *pos + 1U; + return b; +} + +static +void +write_uint8(uint8_t value, uint8_t *data, size_t *pos) +{ + data[*pos] = value; + *pos = *pos + 1U; +} + +static +uint16_t +read_uint16(const uint8_t *data, size_t *pos) +{ +#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ + const uint16_t *bytes = (const uint16_t *)(data + *pos); + *pos = *pos + 2U; + return *bytes; +#else + const uint8_t *bytes = data + *pos; + *pos = *pos + 2U; + uint16_t b0 = (uint16_t)bytes[0]; + uint16_t b1 = (uint16_t)bytes[1]; + uint16_t val = (b0 << 8) | b1; + return val; +#endif +} + +static +void +write_uint16(uint16_t value, uint8_t *data, size_t *pos) +{ + uint8_t *bytes = data + *pos; + *pos = *pos + 2U; +#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ + bytes[0] = (uint8_t)(value & 0xFFU); + bytes[1] = (uint8_t)((value >> 8U)& 0xFFU); +#else + bytes[1] = (uint8_t)(value & 0xFFU); + bytes[0] = (uint8_t)((value >> 8U)& 0xFFU); +#endif +} + +static +uint32_t +read_uint32(const uint8_t *data, size_t *pos) +{ +#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ + const uint32_t *bytes = (const uint32_t *)(data + *pos); + *pos = *pos + 4U; + return *bytes; +#else + const uint8_t *bytes = data + *pos; + *pos = *pos + 4U; + uint32_t b0 = (uint32_t)bytes[0]; + uint32_t b1 = (uint32_t)bytes[1]; + uint32_t b2 = (uint32_t)bytes[2]; + uint32_t b3 = (uint32_t)bytes[3]; + uint32_t val = (b0 << 24) | (b1 << 16U) | (b2 << 8U) | b3; + return val; +#endif +} + +static +void +write_uint32(uint32_t value, uint8_t *data, size_t *pos) +{ + uint8_t *bytes = data + *pos; + *pos = *pos + 4U; +#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ + bytes[0] = (uint8_t)(value & 0xFFU); + bytes[1] = (uint8_t)((value >> 8U) & 0xFFU); + bytes[2] = (uint8_t)((value >> 16U) & 0xFFU); + bytes[3] = (uint8_t)((value >> 24U) & 0xFFU); +#else + bytes[3] = (uint8_t)(value & 0xFFU); + bytes[2] = (uint8_t)((value >> 8U) & 0xFFU); + bytes[1] = (uint8_t)((value >> 16U) & 0xFFU); + bytes[0] = (uint8_t)((value >> 24U) & 0xFFU); +#endif +} + +static +uint64_t +read_uint64(const uint8_t *data, size_t *pos) +{ +#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ + const uint64_t *bytes = (const uint64_t *)(data + *pos); + *pos = *pos + 8U; + return *bytes; +#else + const uint8_t *bytes = data + *pos; + *pos = *pos + 8U; + uint64_t b0 = (uint64_t)bytes[0]; + uint64_t b1 = (uint64_t)bytes[1]; + uint64_t b2 = (uint64_t)bytes[2]; + uint64_t b3 = (uint64_t)bytes[3]; + uint64_t b4 = (uint64_t)bytes[4]; + uint64_t b5 = (uint64_t)bytes[5]; + uint64_t b6 = (uint64_t)bytes[6]; + uint64_t b7 = (uint64_t)bytes[7]; + uint64_t val = (b0 << 56U) | (b1 << 48U) | (b2 << 40U) | (b3 << 32U) | (b4 << 24U) | (b5 << 16U) | (b6 << 8U) | b7; + return val; +#endif +} + +static +void +write_uint64(uint64_t value, uint8_t *data, size_t *pos) +{ + uint8_t *bytes = data + *pos; + *pos = *pos + 4U; +#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ + bytes[0] = (uint8_t)(value & 0xFFU); + bytes[1] = (uint8_t)((value >> 8U) & 0xFFU); + bytes[2] = (uint8_t)((value >> 16U) & 0xFFU); + bytes[3] = (uint8_t)((value >> 24U) & 0xFFU); + bytes[4] = (uint8_t)((value >> 32U) & 0xFFU); + bytes[5] = (uint8_t)((value >> 40U) & 0xFFU); + bytes[6] = (uint8_t)((value >> 48U) & 0xFFU); + bytes[7] = (uint8_t)((value >> 56U) & 0xFFU); +#else + bytes[7] = (uint8_t)(value & 0xFFU); + bytes[6] = (uint8_t)((value >> 8U) & 0xFFU); + bytes[5] = (uint8_t)((value >> 16U) & 0xFFU); + bytes[4] = (uint8_t)((value >> 24U) & 0xFFU); + bytes[3] = (uint8_t)((value >> 32U) & 0xFFU); + bytes[2] = (uint8_t)((value >> 40U) & 0xFFU); + bytes[1] = (uint8_t)((value >> 48U) & 0xFFU); + bytes[0] = (uint8_t)((value >> 56U) & 0xFFU); +#endif +} + pstop_error_t pstop_is_message_valid(const pstop_msg_t *msg) { @@ -47,3 +206,36 @@ pstop_is_message_valid(const pstop_msg_t *msg) return PSTOP_OK; } + +void +pstop_message_decode(pstop_msg_t *msg, const uint8_t *data) +{ + size_t pos = 0U; + uint8_t b = read_uint8(data, &pos); + msg->version = (b & 0xF0) >> 4U; + msg->message = (b & 0x0F); + msg->stamp = read_uint64(data, &pos); + msg->received_stamp = read_uint64(data, &pos); + read_device_uuid(&msg->id, data, &pos); + read_device_uuid(&msg->receiver_id, data, &pos); + msg->heartbeat_timeout = read_uint32(data, &pos); + msg->counter = read_uint32(data, &pos); + msg->received_counter = read_uint32(data, &pos); + msg->checksum = read_uint16(data, &pos); +} + +void +pstop_message_encode(const pstop_msg_t *msg, uint8_t *data) +{ + size_t pos = 0U; + uint8_t b = (msg->version << 4) | msg->message; + write_uint8(b, data, &pos); + write_uint64(msg->stamp, data, &pos); + write_uint64(msg->received_stamp, data, &pos); + write_device_uuid(&msg->id, data, &pos); + write_device_uuid(&msg->receiver_id, data, &pos); + write_uint32(msg->heartbeat_timeout, data, &pos); + write_uint32(msg->counter, data, &pos); + write_uint32(msg->received_counter, data, &pos); + write_uint16(msg->checksum, data, &pos); +} diff --git a/pstop_c/pstop/test/include/pstop/test_utils.h b/pstop_c/pstop/test/include/pstop/test_utils.h index 9497a1c..62de7e7 100644 --- a/pstop_c/pstop/test/include/pstop/test_utils.h +++ b/pstop_c/pstop/test/include/pstop/test_utils.h @@ -6,6 +6,6 @@ #include "pstop/device_id.h" -void device_id_set(device_id_t *device_id, const char *id); +void device_id_set_str(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 index d7eb487..74c5712 100644 --- a/pstop_c/pstop/test/src/pstop/device_id_test.c +++ b/pstop_c/pstop/test/src/pstop/device_id_test.c @@ -28,13 +28,13 @@ void test_device_cmp(void) { device_id_t lhs; - device_id_set(&lhs, "testing"); + device_id_set_str(&lhs, "testing"); device_id_t rhs; - device_id_set(&rhs, "testing"); + device_id_set_str(&rhs, "testing"); TEST_ASSERT_EQUAL(0, device_id_cmp(&lhs, &rhs)); - device_id_set(&rhs, "test"); + device_id_set_str(&rhs, "test"); TEST_ASSERT_NOT_EQUAL(0, device_id_cmp(&lhs, &rhs)); } diff --git a/pstop_c/pstop/test/src/pstop/machine_test.c b/pstop_c/pstop/test/src/pstop/machine_test.c index 6aa16e0..54d6015 100644 --- a/pstop_c/pstop/test/src/pstop/machine_test.c +++ b/pstop_c/pstop/test/src/pstop/machine_test.c @@ -68,7 +68,7 @@ static void init_client(device_id_t *id1, pstop_msg_t *msg, const char *id) { - device_id_set(id1, id); + device_id_set_str(id1, id); device_id_copy(&(msg->id), id1); } @@ -104,7 +104,7 @@ test_new_client_operator_allowed(void) machine_init(&machine, &pstop_app, pstop_clients, MAX_CLIENTS); device_id_t id; - device_id_set(&id, "test"); + device_id_set_str(&id, "test"); pstop_msg_t msg; msg.message = PSTOP_MESSAGE_BOND; @@ -128,7 +128,7 @@ 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); + device_id_set_str(&id, device_id); pstop_msg_t msg; msg.message = PSTOP_MESSAGE_OK; @@ -304,7 +304,7 @@ test_bond_ok_stop(void) machine_init(&machine, &pstop_app, pstop_clients, MAX_CLIENTS); device_id_t id; - device_id_set(&id, "test"); + device_id_set_str(&id, "test"); pstop_msg_t msg; msg.message = PSTOP_MESSAGE_BOND; diff --git a/pstop_c/pstop/test/src/pstop/machine_timeout_test.c b/pstop_c/pstop/test/src/pstop/machine_timeout_test.c index caa0beb..1ed0ffb 100644 --- a/pstop_c/pstop/test/src/pstop/machine_timeout_test.c +++ b/pstop_c/pstop/test/src/pstop/machine_timeout_test.c @@ -68,7 +68,7 @@ static void init_client(device_id_t *id1, pstop_msg_t *msg, const char *id) { - device_id_set(id1, id); + device_id_set_str(id1, id); device_id_copy(&(msg->id), id1); } diff --git a/pstop_c/pstop/test/src/pstop/main.c b/pstop_c/pstop/test/src/pstop/main.c index 88fc424..fd8c396 100644 --- a/pstop_c/pstop/test/src/pstop/main.c +++ b/pstop_c/pstop/test/src/pstop/main.c @@ -6,6 +6,7 @@ extern void main_device_id_test(void); extern void main_pstop_client_test(void); +extern void main_pstop_msg_test(void); extern void main_machine_test(void); extern void main_machine_timeout_test(void); @@ -20,6 +21,7 @@ main(void) main_device_id_test(); main_pstop_client_test(); + main_pstop_msg_test(); main_machine_test(); main_machine_timeout_test(); diff --git a/pstop_c/pstop/test/src/pstop/pstop_client_test.c b/pstop_c/pstop/test/src/pstop/pstop_client_test.c index 60e17b9..9d5b078 100644 --- a/pstop_c/pstop/test/src/pstop/pstop_client_test.c +++ b/pstop_c/pstop/test/src/pstop/pstop_client_test.c @@ -94,10 +94,10 @@ test_remove_client(void) TEST_ASSERT_EQUAL(0U, pstop_client_num_active(&clients)); device_id_t c1_id; - device_id_set(&c1_id, "test"); + device_id_set_str(&c1_id, "test"); device_id_t c2_id; - device_id_set(&c2_id, "test2"); + device_id_set_str(&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); @@ -125,12 +125,10 @@ test_remove_client(void) void main_pstop_client_test(void) { - //UnityBegin("pstop_client_test.c"); UnitySetTestFile("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); - } diff --git a/pstop_c/pstop/test/src/pstop/pstop_msg_test.c b/pstop_c/pstop/test/src/pstop/pstop_msg_test.c new file mode 100644 index 0000000..39940c3 --- /dev/null +++ b/pstop_c/pstop/test/src/pstop/pstop_msg_test.c @@ -0,0 +1,88 @@ + +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + +#include "pstop/pstop_msg.h" + +#include + +static +uint8_t +PSTOP_MSG_BYTES[] = { + 0x02U, // version/message + 0x01U, 0x02U, 0x03U, 0x04U, 0x05U, 0x06U, 0x07U, 0x08U, // timestamp + 0x11U, 0x12U, 0x13U, 0x14U, 0x15U, 0x16U, 0x17U, 0x18U, // received timestamp + + 0x20U, 0x21U, 0x22U, 0x23U, 0x24U, 0x25U, 0x26U, 0x27U, + 0x28U, 0x29U, 0x2AU, 0x2BU, 0x2CU, 0x2DU, 0x2EU, 0x2FU, // device UUID + + 0x30U, 0x31U, 0x32U, 0x33U, 0x34U, 0x35U, 0x36U, 0x37U, + 0x38U, 0x39U, 0x3AU, 0x3BU, 0x3CU, 0x3DU, 0x3EU, 0x3FU, // receiver UUID + + 0x40U, 0x41U, 0x42U, 0x43U, // heartbeat timeout, + + 0x50U, 0x51U, 0x52U, 0x53U, // counter + 0x60U, 0x61U, 0x62U, 0x63U, // received counter + + 0x70U, 0x71U // checksum +}; + +static +void +decode_pstop_msg() +{ + pstop_msg_t msg; + pstop_message_decode(&msg, PSTOP_MSG_BYTES); + TEST_ASSERT_EQUAL(0x00U, msg.version); + TEST_ASSERT_EQUAL(0x02U, msg.message); + TEST_ASSERT_EQUAL(0x0807060504030201U, msg.stamp); + TEST_ASSERT_EQUAL(0x1817161514131211U, msg.received_stamp); + + uint8_t ID_BYTES[] = { + 0x20U, 0x21U, 0x22U, 0x23U, 0x24U, 0x25U, 0x26U, 0x27U, + 0x28U, 0x29U, 0x2AU, 0x2BU, 0x2CU, 0x2DU, 0x2EU, 0x2FU // device UUID + }; + device_id_t id; + device_id_set_bytes(&id, ID_BYTES); + + TEST_ASSERT_EQUAL(0, device_id_cmp(&id, &msg.id)); + + uint8_t RECEIVER_ID_BYTES[] = { + 0x30U, 0x31U, 0x32U, 0x33U, 0x34U, 0x35U, 0x36U, 0x37U, + 0x38U, 0x39U, 0x3AU, 0x3BU, 0x3CU, 0x3DU, 0x3EU, 0x3FU // receiver UUID + }; + device_id_set_bytes(&id, RECEIVER_ID_BYTES); + TEST_ASSERT_EQUAL(0, device_id_cmp(&id, &msg.receiver_id)); + TEST_ASSERT_EQUAL(0x43424140U, msg.heartbeat_timeout); + TEST_ASSERT_EQUAL(0x53525150U, msg.counter); + TEST_ASSERT_EQUAL(0x63626160U, msg.received_counter); + TEST_ASSERT_EQUAL(0x7170U, msg.checksum); +} + +static +void +encode_pstop_msg() +{ + pstop_msg_t msg; + msg.version = 0x0U; + msg.message = 0x3U; + msg.stamp = 0x0807060504030201U; + msg.received_stamp = 0x1817161514131211U; + + msg.heartbeat_timeout = 0x43424140U; + msg.counter = 0x53525150U; + msg.received_counter = 0x63626160U; + msg.checksum = 0x7170U; + + uint8_t bytes[PSTOP_MESSAGE_SIZE]; + pstop_message_encode(&msg, bytes); +} + +void +main_pstop_msg_test(void) +{ + UnitySetTestFile("pstop_msg_test.c"); + + RUN_TEST(decode_pstop_msg); + RUN_TEST(encode_pstop_msg); +} diff --git a/pstop_c/pstop/test/src/pstop/test_utils.c b/pstop_c/pstop/test/src/pstop/test_utils.c index d564953..45a02df 100644 --- a/pstop_c/pstop/test/src/pstop/test_utils.c +++ b/pstop_c/pstop/test/src/pstop/test_utils.c @@ -7,7 +7,7 @@ #include "pstop/test_utils.h" void -device_id_set(device_id_t *device_id, const char *id) +device_id_set_str(device_id_t *device_id, const char *id) { device_id_init(device_id); diff --git a/pstop_c/transport/CMakeLists.txt b/pstop_c/transport/CMakeLists.txt new file mode 100644 index 0000000..b13b751 --- /dev/null +++ b/pstop_c/transport/CMakeLists.txt @@ -0,0 +1,16 @@ +# SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.12) +project(transport_udp C) + +add_library(transport_udp + src/transport/udp/udp_transport.c +) + +target_compile_options(transport_udp PRIVATE -Wall) + +target_include_directories(transport_udp PUBLIC + $ + include +) diff --git a/pstop_c/transport/include/transport/transport.h b/pstop_c/transport/include/transport/transport.h new file mode 100644 index 0000000..6ca050c --- /dev/null +++ b/pstop_c/transport/include/transport/transport.h @@ -0,0 +1,27 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + +#ifndef PSTOP_TRANSPORT_TRANSPORT_H +#define PSTOP_TRANSPORT_TRANSPORT_H + +#include +#include + +typedef int (*transport_open_t)(const char *host, int port); +typedef int (*transport_close_t)(); + +typedef int (*transport_read_t)(uint8_t *dest, size_t length, struct sockaddr_storage *clientAddr); + +typedef int (*transport_write_t)(const uint8_t *data, size_t length, struct sockaddr_in *target); + +typedef struct { + + transport_open_t open_cb; + transport_close_t close_cb; + + transport_read_t read_cb; + transport_write_t write_cb; + +} transport_t; + +#endif /* PSTOP_TRANSPORT_TRANSPORT_H */ diff --git a/pstop_c/transport/include/transport/udp/udp_transport.h b/pstop_c/transport/include/transport/udp/udp_transport.h new file mode 100755 index 0000000..77490fb --- /dev/null +++ b/pstop_c/transport/include/transport/udp/udp_transport.h @@ -0,0 +1,31 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + +#ifndef PSTOP_TRANSPORT_UDP_UDPTRANSPORT_H +#define PSTOP_TRANSPORT_UDP_UDPTRANSPORT_H + +#include +#include + +#include +#include + +typedef struct { + + struct sockaddr_in addr; + int fd; + + struct pollfd poll_fds; + +} udp_transport_data_t; + +int transport_udp_open(udp_transport_data_t *transport, const char *host, int port); +int transport_udp_close(udp_transport_data_t *transport); + +int transport_udp_read(udp_transport_data_t *transport, uint8_t *dest, size_t length, struct sockaddr_storage *clientAddr); + +int transport_udp_write(udp_transport_data_t *transport, const uint8_t *data, size_t length, struct sockaddr_in *target); + +void transport_init_udp(udp_transport_data_t *transport); + +#endif /* PSTOP_TRANSPORT_UDP_UDPTRANSPORT_H */ diff --git a/pstop_c/transport/src/transport/udp/udp_transport.c b/pstop_c/transport/src/transport/udp/udp_transport.c new file mode 100755 index 0000000..9358636 --- /dev/null +++ b/pstop_c/transport/src/transport/udp/udp_transport.c @@ -0,0 +1,93 @@ + +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include +#include +#include + +#include "transport/udp/udp_transport.h" + +int +transport_udp_open(udp_transport_data_t *transport, const char *host, int port) +{ + memset(&transport->addr, 0, sizeof(transport->addr)); + transport->addr.sin_addr.s_addr = inet_addr(host); + transport->addr.sin_port = htons(port); + transport->addr.sin_family = AF_INET; + + // create datagram socket + transport->fd = socket(AF_INET, SOCK_DGRAM, 0); + if(transport->fd == -1) { + return -1; + } + + int result = bind(transport->fd, (struct sockaddr*)&transport->addr, sizeof(transport->addr)); + + transport->poll_fds.fd = transport->fd; + transport->poll_fds.events = POLLIN; + + return result; +} + +int +transport_udp_close(udp_transport_data_t *transport) +{ + if(transport->fd != -1) { + close(transport->fd); + } + + transport->fd = -1; + transport->poll_fds.fd = -1; + + return 0; +} + +int +transport_udp_read(udp_transport_data_t *transport, uint8_t *dest, size_t length, struct sockaddr_storage *clientAddr) +{ + int numEvents = poll(&(transport->poll_fds), 1, 10); + + if(numEvents == 0) { + return 0; + } + + if(transport->poll_fds.revents & POLLIN) { + socklen_t addrSize = sizeof(struct sockaddr_storage); + int bytesRead = recvfrom(transport->fd, dest, length, 0, (struct sockaddr *)clientAddr, &addrSize); + + return bytesRead; + } + + return 0; +} + +int +transport_udp_write(udp_transport_data_t *transport, const uint8_t *data, size_t length, struct sockaddr_in *target) +{ + struct sockaddr *addr = (struct sockaddr *)target; + int addrSize = sizeof(struct sockaddr_in); + + if(target == NULL) { + addr = (struct sockaddr *)&transport->addr; + } + ssize_t numbytes = sendto(transport->fd, data, length, 0, addr, addrSize); + + //fprintf(stderr, "Wrote bytes: %d\n", (int)numbytes); + if(numbytes == -1) { + // fprintf(stderr, "errono=%d\n", errno); + return -1; + } + + return numbytes; +} + +void +transport_init_udp(udp_transport_data_t *transport) +{ + transport->fd = -1; + memset(&transport->addr, 0, sizeof(transport->addr)); +}