diff --git a/include/param/param.h b/include/param/param.h index dfc872f..4b31782 100644 --- a/include/param/param.h +++ b/include/param/param.h @@ -86,6 +86,12 @@ typedef enum { #define PM_CSP (5 << 16) //! Known as 5 in elfparse and genparamtable #define PM_KEYCONF (6 << 16) //! Known as 6 in elfparse and genparamtable +/** + * Value to indicate an invalid nsec. + * + * This define should be moved to CSP if CSP will have support for invalid nsec + */ +#define CSP_TIMESTAMP_INVALID_NSEC -1 /** * Parameter description structure @@ -140,7 +146,7 @@ typedef struct param_s { #ifdef PARAM_HAVE_TIMESTAMP #define PARAM_TIMESTAMP_DECL(_name) \ - csp_timestamp_t _timestamp_##_name = { .tv_sec = 0, .tv_nsec = 0 }; + csp_timestamp_t _timestamp_##_name = { .tv_sec = 0, .tv_nsec = CSP_TIMESTAMP_INVALID_NSEC }; #define PARAM_TIMESTAMP_INIT(_name) \ .timestamp = &_timestamp_##_name, diff --git a/include/param/param_client.h b/include/param/param_client.h index ada01fa..025d1f8 100644 --- a/include/param/param_client.h +++ b/include/param/param_client.h @@ -50,6 +50,20 @@ int param_pull_single(param_t *param, int offset, int prio, int verbose, int hos */ int param_pull_all(int prio, int verbose, int host, uint32_t include_mask, uint32_t exclude_mask, int timeout, int version); +/** + * PULL all and set param queue initial timestamp + * @param prio CSP packet priority + * @param verbose printout when received + * @param host remote csp node + * @param include_mask parameter mask + * @param exclude_mask parameter mask + * @param timeout in ms + * @param version 1 or 2 + * @param timestamp if not NULL set param queue initial timestamp to this timestamp + * @return 0 = OK, -1 on network error + */ +int param_pull_all_timestamp(int prio, int verbose, int host, uint32_t include_mask, uint32_t exclude_mask, int timeout, int version, csp_timestamp_t *timestamp); + /** * PUSH single: * diff --git a/include/param/param_queue.h b/include/param/param_queue.h index 0fc4be9..74ecfda 100644 --- a/include/param/param_queue.h +++ b/include/param/param_queue.h @@ -48,6 +48,16 @@ int param_queue_add(param_queue_t *queue, param_t *param, int offset, void *valu */ int param_queue_apply(param_queue_t *queue, int host, int verbose); +/** + * @brief Applies the content of a queue to memory and set param queue initial timestamp to the provided timestamp. + * @param queue[in] Pointer to queue + * @param host[in] If host is set and the node is 0, it will be set to host + * @param verbose[in] 2 prints if packet parse fails, 3 prints each missing param + * @param q_timestamp[in] if not NULL set param queue initial timestamp to this timestamp + * @return 0 OK, -1 ERROR + */ +int param_queue_apply_timestamp(param_queue_t *queue, int host, int verbose, const csp_timestamp_t *q_timestamp); + void param_queue_print(param_queue_t *queue); void param_queue_print_local(param_queue_t *queue); void param_queue_print_params(param_queue_t *queue, uint32_t ref_timestamp); diff --git a/meson.build b/meson.build index 320bb9d..588bd48 100644 --- a/meson.build +++ b/meson.build @@ -24,6 +24,10 @@ if get_option('have_float') == false conf.set('MPACK_FLOAT', 0) endif +if get_option('serialize_extended_timestamp') == true + conf.set('EXTENDED_TIMESTAMP', 0) +endif + if get_option('have_fopen') == true if get_option('list_dynamic') == true conf.set('MPACK_STDIO', 1) diff --git a/meson_options.txt b/meson_options.txt index a1c5765..ff408a3 100644 --- a/meson_options.txt +++ b/meson_options.txt @@ -6,4 +6,6 @@ option('list_dynamic', type: 'boolean', value: false, description: 'Compile supp option('list_pool', type: 'integer', value: 0, description: 'Compile support for pre-allocated param list (requres sys/queue.h)') option('have_float', type: 'boolean', value: true, description: 'Support float/double') option('num_publishqueues', type: 'integer', value: 0, description: 'Number of param publish queues required') +option('serialize_extended_timestamp', type: 'boolean', value: false, description: 'Include ns part of timestamps when serializing parameters (network incompatible with libparam older than June 2025') option('test', type: 'boolean', value: false, description: 'Build GoogleTest based tests (requires gtest)') + diff --git a/src/param/param_client.c b/src/param/param_client.c index c488169..8d68bfa 100644 --- a/src/param/param_client.c +++ b/src/param/param_client.c @@ -26,7 +26,7 @@ static void param_transaction_callback_pull(csp_packet_t *response, int verbose, param_queue_init(&queue, &response->data[2], response->length - 2, response->length - 2, PARAM_QUEUE_TYPE_SET, version); /* Write data to local memory */ - param_queue_apply(&queue, response->id.src, verbose); + param_queue_apply_timestamp(&queue, response->id.src, verbose, context); if (!verbose) { csp_buffer_free(response); @@ -120,7 +120,7 @@ int param_transaction(csp_packet_t *packet, int host, int timeout, param_transac return result; } -int param_pull_all(int prio, int verbose, int host, uint32_t include_mask, uint32_t exclude_mask, int timeout, int version) { +int param_pull_all_timestamp(int prio, int verbose, int host, uint32_t include_mask, uint32_t exclude_mask, int timeout, int version, csp_timestamp_t *timestamp) { csp_packet_t *packet = csp_buffer_get(PARAM_SERVER_MTU); if (packet == NULL) @@ -135,8 +135,12 @@ int param_pull_all(int prio, int verbose, int host, uint32_t include_mask, uint3 packet->data32[2] = htobe32(exclude_mask); packet->length = 12; packet->id.pri = prio; - return param_transaction(packet, host, timeout, param_transaction_callback_pull, verbose, version, NULL); + return param_transaction(packet, host, timeout, param_transaction_callback_pull, verbose, version, timestamp); +} + +int param_pull_all(int prio, int verbose, int host, uint32_t include_mask, uint32_t exclude_mask, int timeout, int version) { + return param_pull_all_timestamp(prio, verbose, host, include_mask, exclude_mask, timeout, version, NULL); } int param_pull_queue(param_queue_t *queue, uint8_t prio, int verbose, int host, int timeout) { diff --git a/src/param/param_queue.c b/src/param/param_queue.c index f4c5158..3d40230 100644 --- a/src/param/param_queue.c +++ b/src/param/param_queue.c @@ -67,14 +67,20 @@ int param_queue_add(param_queue_t *queue, param_t *param, int offset, void *valu return 0; } -int param_queue_apply(param_queue_t *queue, int host, int verbose) { +int param_queue_apply_timestamp(param_queue_t *queue, int host, int verbose, const csp_timestamp_t *q_timestamp) { + int return_code = 0; int atomic_write = 0; + csp_timestamp_t time; + + if (q_timestamp == NULL) { + csp_clock_get_time(&time); + } else { + time = *q_timestamp; + } - csp_timestamp_t time_now; - csp_clock_get_time(&time_now); - queue->last_timestamp = time_now; - queue->client_timestamp = time_now; + queue->last_timestamp = time; + queue->client_timestamp = time; mpack_reader_t reader; mpack_reader_init_data(&reader, queue->buffer, queue->used); @@ -98,13 +104,12 @@ int param_queue_apply(param_queue_t *queue, int host, int verbose) { } #ifdef PARAM_HAVE_TIMESTAMP - /* Only remote paramters use timestamps */ + /* Only remote parameters use timestamps */ if (*param->node != 0) { /* If no timestamp was provided, use client received timestamp */ if (timestamp.tv_sec == 0) { timestamp = queue->client_timestamp; - csp_clock_get_time(×tamp); - } + } *param->timestamp = timestamp; } #endif @@ -173,6 +178,11 @@ int param_queue_apply(param_queue_t *queue, int host, int verbose) { return return_code; } +int param_queue_apply(param_queue_t *queue, int host, int verbose) { + + return param_queue_apply_timestamp(queue, host, verbose, NULL); +} + void param_queue_print(param_queue_t *queue) { if (queue->type == PARAM_QUEUE_TYPE_GET) { printf("cmd new get %s\n", queue->name); @@ -243,4 +253,4 @@ void param_queue_print_params(param_queue_t *queue, uint32_t ref_timestamp) { } outer_count++; } -} \ No newline at end of file +} diff --git a/src/param/param_serializer.c b/src/param/param_serializer.c index 8a42751..24aad67 100644 --- a/src/param/param_serializer.c +++ b/src/param/param_serializer.c @@ -70,14 +70,20 @@ void param_serialize_id(mpack_writer_t *writer, param_t *param, int offset, para int node_flag = (queue->last_node != node) ? 1 : 0; #ifdef PARAM_HAVE_TIMESTAMP int timestamp_flag = (queue->last_timestamp.tv_sec != param->timestamp->tv_sec) ? 1 : 0; + int extendedtimestamp_flag = 0; +#ifdef EXTENDED_TIMESTAMP + extendedtimestamp_flag = (queue->last_timestamp.tv_nsec != param->timestamp->tv_nsec) ? 1 : 0; +#endif /* EXTENDED_TIMESTAMP */ #else int timestamp_flag = 0; + int extendedtimestamp_flag = 0; #endif int extendedid_flag = (param->id > 0x3ff) ? 1 : 0; - uint16_t header = array_flag << PARAM_HEADER_ARRAY_POS + uint16_t header = array_flag << PARAM_HEADER_ARRAY_POS | node_flag << PARAM_HEADER_NODE_POS | timestamp_flag << PARAM_HEADER_TIMESTAMP_POS + | extendedtimestamp_flag << PARAM_HEADER_EXTENDEDTIMESTAMP_POS | extendedid_flag << PARAM_HEADER_EXTENDEDID_POS |(param->id & PARAM_HEADER_ID_MASK); header = htobe16(header); @@ -100,6 +106,12 @@ void param_serialize_id(mpack_writer_t *writer, param_t *param, int offset, para uint32_t _timestamp = htobe32(param->timestamp->tv_sec); mpack_write_bytes(writer, (char*) &_timestamp, 4); } + + if (extendedtimestamp_flag) { + queue->last_timestamp.tv_nsec = param->timestamp->tv_nsec; + uint32_t _timestamp_ns = htobe32(param->timestamp->tv_nsec); + mpack_write_bytes(writer, (char*) &_timestamp_ns, 4); + } #endif if (extendedid_flag) { @@ -168,19 +180,14 @@ void param_deserialize_id(mpack_reader_t *reader, int *id, int *node, csp_timest queue->last_timestamp = queue->client_timestamp; } else { queue->last_timestamp.tv_sec = _timestamp; - if (extendedtimestamp_flag) { - uint32_t _timestamp_ns; - mpack_read_bytes(reader, (char*) &_timestamp_ns, 4); - _timestamp_ns = be32toh(_timestamp_ns); - queue->last_timestamp.tv_nsec = _timestamp_ns; - } else { - queue->last_timestamp.tv_nsec = 0; - } } - } else if (extendedtimestamp_flag) { - /* Invalid header combination, discard header field */ + } + + if (extendedtimestamp_flag) { uint32_t _timestamp_ns; mpack_read_bytes(reader, (char*) &_timestamp_ns, 4); + _timestamp_ns = be32toh(_timestamp_ns); + queue->last_timestamp.tv_nsec = _timestamp_ns; } *timestamp = queue->last_timestamp;