diff --git a/src/picoros.c b/src/picoros.c index f1ed6d6..b0ef77b 100644 --- a/src/picoros.c +++ b/src/picoros.c @@ -148,6 +148,10 @@ static void sub_data_handler(z_loaned_sample_t *sample, void *ctx) { return; } uint8_t *raw_data = (uint8_t*)z_malloc(raw_data_len); + if (raw_data == NULL) { + _PR_LOG("Out of memory receiving a sample, dropped\n"); + return; + } _z_bytes_to_buf(b, raw_data, raw_data_len); // Call user callback function if given: @@ -169,6 +173,12 @@ static void queriable_data_handler(z_loaned_query_t *query, void *arg) { // get request data uint8_t* rx_data = (uint8_t*)z_malloc(rx_data_len); + // z_malloc(0) may legitimately return NULL; only a non-empty request is + // an allocation failure. Unanswered, the caller times out. + if (rx_data == NULL && rx_data_len > 0) { + _PR_LOG("Out of memory receiving a service request, not answered\n"); + return; + } _z_bytes_to_buf(b, rx_data, rx_data_len); // process @@ -179,13 +189,34 @@ static void queriable_data_handler(z_loaned_query_t *query, void *arg) { z_owned_bytes_t reply_payload; z_bytes_copy_from_buf(&reply_payload, reply.data, reply.length); - // rmw attachment - srv->attachment.sequence_number = 1; - srv->attachment.time = 1000000000 * z_clock_now().tv_sec + z_clock_now().tv_nsec; + // rmw attachment: echo the request's sequence number and GID so a + // long-lived rmw_zenoh client can match this reply to its pending + // request. A request with no attachment, or one that is not exactly + // sizeof(rmw_attachment_t) bytes, keeps the old bookkeeping values + // (sequence 1, this server's own GID). Built in a stack-local copy: + // srv->attachment carries this server's own GID + // (rmw_zenoh_gen_attachment_gid() sets it once, not per-reply), so + // must NOT be mutated here -- doing so would leak one client's GID + // into every later reply that lacks its own attachment. + // z_query_reply() below sends synchronously and the attachment is + // dropped before it returns, so a stack-local tx is safe to point + // z_bytes_from_static_buf() at. + rmw_attachment_t tx = srv->attachment; + tx.sequence_number = 1; + const z_loaned_bytes_t *req_attachment = z_query_attachment(query); + if (req_attachment != NULL) { + size_t req_attachment_len = _z_bytes_len(req_attachment); + if (req_attachment_len == sizeof(rmw_attachment_t)) { + uint8_t req_attachment_buf[sizeof(rmw_attachment_t)]; + _z_bytes_to_buf(req_attachment, req_attachment_buf, req_attachment_len); + picoros_reply_attachment_from_request(&tx, req_attachment_buf, req_attachment_len); + } + } + tx.time = 1000000000 * z_clock_now().tv_sec + z_clock_now().tv_nsec; z_query_reply_options_t options; z_query_reply_options_default(&options); z_owned_bytes_t tx_attachment; - z_bytes_from_static_buf(&tx_attachment, (uint8_t*)&srv->attachment, sizeof(rmw_attachment_t)); + z_bytes_from_static_buf(&tx_attachment, (uint8_t*)&tx, sizeof(tx)); options.attachment = z_bytes_move(&tx_attachment); // send reply @@ -239,6 +270,10 @@ static void get_data_handler(z_loaned_reply_t *reply, void *ctx){ return; } raw_data = (uint8_t*)z_malloc(raw_data_len); + if (raw_data == NULL) { + _PR_LOG("Out of memory receiving a service reply, dropped\n"); + return; + } _z_bytes_to_buf(payload, raw_data, raw_data_len); picoros_srv_client_t* client = (picoros_srv_client_t*)ctx; @@ -489,6 +524,9 @@ picoros_res_t picoros_service_drop(picoros_srv_server_t* serv) { picoros_res_t picoros_service_client_init(picoros_srv_client_t * client){ if (client->_key_buf == NULL){ client->_key_buf = z_malloc(KEYEXPR_SIZE); + if (client->_key_buf == NULL) { + return PICOROS_ERROR; + } } // Generate key expressions if (client->topic.type != NULL) { diff --git a/src/picoros.h b/src/picoros.h index f3a5a76..befd7d3 100644 --- a/src/picoros.h +++ b/src/picoros.h @@ -31,6 +31,8 @@ #include #include +#include +#include /* Exported constants --------------------------------------------------------*/ /** @brief Maximum size for key expressions used in rmw topic names @ingroup picoros */ @@ -61,6 +63,36 @@ typedef struct __attribute__((__packed__)) { uint8_t rmw_gid[RMW_GID_SIZE]; /**< RMW Global Identifier */ } rmw_attachment_t; +/** + * @brief Copy a request's rmw identity (sequence number and GID) into a + * service reply's attachment. + * + * rmw_zenoh clients match a reply to its pending request by the reply + * attachment's sequence number (and writer GID); a server that always + * replies with its own bookkeeping values instead of the request's makes a + * long-lived client drop every reply after its first. Leaves `reply` + * untouched (including `time`, which the caller sets separately) and + * returns false unless `req` is non-NULL and `req_len` is exactly + * `sizeof(rmw_attachment_t)`. + * + * @param reply Attachment to update in place. + * @param req Raw bytes of the request's attachment, or NULL. + * @param req_len Length of `req` in bytes. + * @return true if `reply` was updated from `req`, false otherwise. + */ +static inline bool picoros_reply_attachment_from_request(rmw_attachment_t *reply, + const uint8_t *req, size_t req_len) { + if (reply == NULL || req == NULL || req_len != sizeof(rmw_attachment_t)) { + return false; + } + rmw_attachment_t incoming; + memcpy(&incoming, req, sizeof(incoming)); + reply->sequence_number = incoming.sequence_number; + reply->rmw_gid_size = incoming.rmw_gid_size; + memcpy(reply->rmw_gid, incoming.rmw_gid, RMW_GID_SIZE); + return true; +} + /** * @brief RMW topic structure required by rmw_zenoh */ diff --git a/src/picoserdes.c b/src/picoserdes.c index 1e20e2f..c1f62af 100644 --- a/src/picoserdes.c +++ b/src/picoserdes.c @@ -72,14 +72,22 @@ BASE_TYPES_LIST(PS_DES_BASE) // Deserialize string without copy bool ucdr_deserialize_rstring(ucdrBuffer* ub, char** pstring){ uint32_t len = 0; - bool ret = ucdr_deserialize_endian_uint32_t(ub, ub->endianness, &len); - if (ret){ - *pstring = (char*)ub->iterator; - ub->iterator += len; - ub->offset += len; - ub->last_data_size = 1; + if (!ucdr_deserialize_endian_uint32_t(ub, ub->endianness, &len)){ + return false; } - return ret; + // The length comes off the wire. It must fit what is left of the buffer + // and include the terminating NUL; otherwise the returned pointer runs + // past the end, and a length near UINT32_MAX wraps the iterator backwards + // on a 32-bit target. Refuse without moving the iterator. + if (len == 0 || len > ucdr_buffer_remaining(ub) || ub->iterator[len - 1] != '\0'){ + ub->error = true; + return false; + } + *pstring = (char*)ub->iterator; + ub->iterator += len; + ub->offset += len; + ub->last_data_size = 1; + return true; } // Wrapper for having consistent names @@ -109,16 +117,22 @@ bool ucdr_deserialize_array_rstring(ucdrBuffer* ub, char** strings, uint32_t num // Deserialize strings sequence to array of string pointers bool ucdr_deserialize_sequence_rstring(ucdrBuffer* ub, char** strings, uint32_t max_number, uint32_t* number){ - ucdr_deserialize_endian_uint32_t(ub, ub->endianness, number); - if (*number <= max_number){ - for (int i = 0; i < *number; i++){ - if (ucdr_deserialize_rstring(ub, &strings[i]) == false){ - return false; - } + uint32_t count = 0; + *number = 0; + if (!ucdr_deserialize_endian_uint32_t(ub, ub->endianness, &count)){ + return false; + } + if (count > max_number){ + ub->error = true; + return false; + } + for (uint32_t i = 0; i < count; i++){ + if (ucdr_deserialize_rstring(ub, &strings[i]) == false){ + return false; } - return true; } - return false; + *number = count; + return true; } // Serialize array of string pointers diff --git a/test/test_picoserdes.c b/test/test_picoserdes.c index d2328b6..d70a546 100644 --- a/test/test_picoserdes.c +++ b/test/test_picoserdes.c @@ -199,6 +199,52 @@ int main() { (void)len; } + print_header("Malformed String Tests:"); + { + // A CDR string is a uint32 length (including the NUL) then the bytes. + // Each case lies about that length; decoding must refuse it and leave + // the output pointer untouched rather than point past the buffer. + struct { + const char* name; + uint32_t len; + const char* body; + size_t body_len; + } cases[] = { + {"string zero length", 0, "", 0}, + {"string length past buffer end", 64, "abc", 4}, + {"string length near UINT32_MAX", 0xFFFFFFF0u, "abc", 4}, + {"string missing NUL", 3, "abc", 3}, + }; + for (size_t i = 0; i < sizeof(cases) / sizeof(cases[0]); i++) { + uint8_t buffer[16] = {0}; + ucdrBuffer w; + ucdr_init_buffer(&w, buffer, sizeof(buffer)); + ucdr_serialize_uint32_t(&w, cases[i].len); + memcpy(buffer + 4, cases[i].body, cases[i].body_len); + + ucdrBuffer r; + ucdr_init_buffer(&r, buffer, 4 + cases[i].body_len); + char* out = NULL; + bool test_passed = !ucdr_deserialize_rstring(&r, &out) && out == NULL; + print_test_result(cases[i].name, test_passed); + if (!test_passed) { some_test_failed = true; } + } + + // More strings on the wire than the caller has room for. + uint8_t buffer[64] = {0}; + ucdrBuffer w; + ucdr_init_buffer(&w, buffer, sizeof(buffer)); + ucdr_serialize_uint32_t(&w, 5); + char* strings[2] = {NULL, NULL}; + uint32_t number = 0xdead; + ucdrBuffer r; + ucdr_init_buffer(&r, buffer, sizeof(buffer)); + bool test_passed = !ucdr_deserialize_sequence_rstring(&r, strings, 2, &number) + && number == 0; + print_test_result("string sequence count past capacity", test_passed); + if (!test_passed) { some_test_failed = true; } + } + if(some_test_failed){ printf("\n%s%s Some tests failed! %s\n\n", BOLD_TEXT, RED_TEXT, RESET_TEXT);