From 54fe6f1ea9fa76dbdbad92c9d525367f85aedaf0 Mon Sep 17 00:00:00 2001 From: Jens Henrik Hertz Date: Wed, 5 Nov 2025 16:27:20 +0100 Subject: [PATCH 1/7] Capture time that a message was received for ETH and CAN --- include/csp/csp_hooks.h | 2 +- include/csp/csp_types.h | 1 + include/csp/interfaces/csp_if_can.h | 2 +- include/csp/interfaces/csp_if_eth.h | 2 +- src/csp_service_handler.c | 13 +++++++++---- src/interfaces/csp_if_can.c | 8 +++++--- src/interfaces/csp_if_eth.c | 5 ++++- 7 files changed, 22 insertions(+), 11 deletions(-) diff --git a/include/csp/csp_hooks.h b/include/csp/csp_hooks.h index 12af6afab..207c96345 100644 --- a/include/csp/csp_hooks.h +++ b/include/csp/csp_hooks.h @@ -97,7 +97,7 @@ void csp_clock_get_time(csp_timestamp_t * time); * @param time Structure containing the new time to set * @return 0 on success, -1 on failure */ -int csp_clock_set_time(const csp_timestamp_t * time); +int csp_clock_set_time(const csp_timestamp_t * time, uint64_t rx_timestamp); #ifdef __cplusplus } diff --git a/include/csp/csp_types.h b/include/csp/csp_types.h index 578a4f01c..4e864e665 100644 --- a/include/csp/csp_types.h +++ b/include/csp/csp_types.h @@ -130,6 +130,7 @@ typedef struct csp_packet_s { uint16_t length; /*< Data length */ csp_id_t id; /*< CSP id (unpacked version CPU readable) */ + uint64_t timestamp; /*< Timestamp in ns for (the first received fragment of) the frame */ struct csp_packet_s * next; /*< Used for lists / queues of packets */ diff --git a/include/csp/interfaces/csp_if_can.h b/include/csp/interfaces/csp_if_can.h index 106515539..ddb2917c0 100644 --- a/include/csp/interfaces/csp_if_can.h +++ b/include/csp/interfaces/csp_if_can.h @@ -242,7 +242,7 @@ int csp_can_tx(csp_iface_t * iface, uint16_t via, csp_packet_t *packet); * @param[out] pxTaskWoken Valid reference if called from ISR, otherwise NULL! * @return #CSP_ERR_NONE on success, otherwise an error code. */ -int csp_can_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, uint8_t dlc, int *pxTaskWoken); +int csp_can_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, uint8_t dlc, int *pxTaskWoken, uint64_t timestamp); #ifdef __cplusplus } diff --git a/include/csp/interfaces/csp_if_eth.h b/include/csp/interfaces/csp_if_eth.h index 33c452730..bf8f57369 100644 --- a/include/csp/interfaces/csp_if_eth.h +++ b/include/csp/interfaces/csp_if_eth.h @@ -149,7 +149,7 @@ int csp_eth_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int fro * @param[out] pxTaskWoken Valid reference if called from ISR, otherwise NULL! * @return #CSP_ERR_NONE on success, otherwise an error code. */ -int csp_eth_rx(csp_iface_t * iface, csp_eth_header_t * eth_frame, uint32_t received_len, int * task_woken); +int csp_eth_rx(csp_iface_t * iface, csp_eth_header_t * eth_frame, uint32_t received_len, int * task_woken, uint64_t timestamp); #ifdef __cplusplus } diff --git a/src/csp_service_handler.c b/src/csp_service_handler.c index 1947d5631..2e7691954 100644 --- a/src/csp_service_handler.c +++ b/src/csp_service_handler.c @@ -11,6 +11,9 @@ #include #include +#include +#include + /** * The CSP CMP mempy function is used to, override the function used to * read/write memory by peek and poke. @@ -172,7 +175,7 @@ static int do_cmp_poke_v2(struct csp_cmp_message * cmp) { return CSP_ERR_NONE; } -static int do_cmp_clock(struct csp_cmp_message * cmp) { +static int do_cmp_clock(struct csp_cmp_message * cmp, uint64_t packet_timestamp) { csp_timestamp_t clock; clock.tv_sec = be32toh(cmp->clock.tv_sec); @@ -181,7 +184,7 @@ static int do_cmp_clock(struct csp_cmp_message * cmp) { int res = CSP_ERR_NONE; if (clock.tv_sec != 0) { // set time - res = csp_clock_set_time(&clock); + res = csp_clock_set_time(&clock, packet_timestamp); if (res != CSP_ERR_NONE) { csp_dbg_errno = CSP_DBG_ERR_CLOCK_SET_FAIL; } @@ -243,7 +246,7 @@ static int csp_cmp_handler(csp_packet_t * packet) { break; case CSP_CMP_CLOCK: - ret = do_cmp_clock(cmp); + ret = do_cmp_clock(cmp, packet->timestamp); break; default: @@ -268,9 +271,11 @@ void csp_service_handler(csp_packet_t * packet) { } break; - case CSP_PING: + case CSP_PING: { /* A ping means, just echo the packet, so no changes */ + printf("Packet TS: %"PRIu64"\n", packet->timestamp); break; + } case CSP_PS: { packet->length = csp_ps_hook(packet); diff --git a/src/interfaces/csp_if_can.c b/src/interfaces/csp_if_can.c index 6e5649969..734d6659d 100644 --- a/src/interfaces/csp_if_can.c +++ b/src/interfaces/csp_if_can.c @@ -260,7 +260,7 @@ static int csp_can1_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, return CSP_ERR_NONE; } -static int csp_can2_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, uint8_t dlc, int * task_woken) { +static int csp_can2_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, uint8_t dlc, int * task_woken, uint64_t can_timestamp) { csp_can_interface_data_t * ifdata = iface->interface_data; @@ -351,6 +351,8 @@ static int csp_can2_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u /* END */ if (id & (CFP2_END_MASK << CFP2_END_OFFSET)) { + packet->timestamp = can_timestamp; + /* Extract data length */ packet->length = packet->frame_length - csp_id_get_header_size(); @@ -502,10 +504,10 @@ int csp_can_remove_interface(csp_iface_t * iface) { return CSP_ERR_NONE; } -int csp_can_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, uint8_t dlc, int * task_woken) { +int csp_can_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, uint8_t dlc, int * task_woken, uint64_t timestamp) { if (csp_conf.version == 1) { return csp_can1_rx(iface, id, data, dlc, task_woken); } else { - return csp_can2_rx(iface, id, data, dlc, task_woken); + return csp_can2_rx(iface, id, data, dlc, task_woken, timestamp); } } diff --git a/src/interfaces/csp_if_eth.c b/src/interfaces/csp_if_eth.c index 449e1c9da..7c5acead9 100644 --- a/src/interfaces/csp_if_eth.c +++ b/src/interfaces/csp_if_eth.c @@ -14,6 +14,7 @@ #include #include #include +#include /** @@ -133,7 +134,7 @@ void csp_eth_arp_get_addr(uint8_t * mac_addr, uint16_t csp_addr) memset(mac_addr, 0xff, CSP_ETH_ALEN); } -int csp_eth_rx(csp_iface_t * iface, csp_eth_header_t * eth_frame, uint32_t received_len, int * task_woken) { +int csp_eth_rx(csp_iface_t * iface, csp_eth_header_t * eth_frame, uint32_t received_len, int * task_woken, uint64_t timestamp) { csp_eth_interface_data_t * ifdata = iface->interface_data; @@ -233,6 +234,8 @@ int csp_eth_rx(csp_iface_t * iface, csp_eth_header_t * eth_frame, uint32_t recei return CSP_ERR_NONE; } + packet->timestamp = timestamp; + csp_qfifo_write(packet, iface, task_woken); return CSP_ERR_NONE; From b78d743b9d63b5ac6ebd18480a44a6b29f8450d2 Mon Sep 17 00:00:00 2001 From: Jens Henrik Hertz Date: Thu, 6 Nov 2025 09:52:48 +0100 Subject: [PATCH 2/7] Support CSP_SYNC_TIME packet. --- include/csp/csp.h | 40 +++++++++ include/csp/csp_cmp.h | 11 +++ include/csp/csp_interface.h | 3 +- include/csp/csp_types.h | 11 +++ include/csp/interfaces/csp_if_eth.h | 4 +- include/csp/interfaces/csp_if_i2c.h | 2 +- include/csp/interfaces/csp_if_kiss.h | 2 +- src/csp_bridge.c | 2 +- src/csp_io.c | 54 +++++++++--- src/csp_io.h | 5 +- src/csp_rdp.c | 4 +- src/csp_route.c | 2 +- src/csp_service_handler.c | 39 ++++++++- src/csp_services.c | 59 +++++++++++++ src/drivers/eth/eth_linux.c | 126 +++++++++++++++++++++++++-- src/interfaces/csp_if_can.c | 6 +- src/interfaces/csp_if_eth.c | 7 +- src/interfaces/csp_if_i2c.c | 5 +- src/interfaces/csp_if_kiss.c | 3 +- src/interfaces/csp_if_lo.c | 2 +- src/interfaces/csp_if_tun.c | 2 +- src/interfaces/csp_if_udp.c | 2 +- src/interfaces/csp_if_zmqhub.c | 2 +- 23 files changed, 347 insertions(+), 46 deletions(-) diff --git a/include/csp/csp.h b/include/csp/csp.h index 41f968dfe..fb27a18c4 100644 --- a/include/csp/csp.h +++ b/include/csp/csp.h @@ -100,6 +100,16 @@ csp_packet_t *csp_read(csp_conn_t *conn, uint32_t timeout); */ void csp_send(csp_conn_t *conn, csp_packet_t *packet); +/** + * Send packet on a connection, and get the TX timestamp. + * The packet buffer is automatically freed, and cannot be used after the call to csp_send_with_timestamp() + * + * @param[in] conn connection + * @param[in] packet packet to send + * @param[out] tx_timestamp pointer to variable to receive the TX timestamp in nanoseconds, or NULL if not needed. + */ +void csp_send_and_get_timestamp(csp_conn_t *conn, csp_packet_t *packet, uint64_t *tx_timestamp); + /** * Change the default priority of the connection and send a packet. * @@ -131,6 +141,26 @@ void csp_send_prio(uint8_t prio, csp_conn_t *conn, csp_packet_t *packet); */ int csp_transaction_w_opts(uint8_t prio, uint16_t dst, uint8_t dst_port, uint32_t timeout, const void *outbuf, int outlen, void *inbuf, int inlen, uint32_t opts); + +/** + * Perform an entire request & reply transaction. Also returns the RX timestamp of the reply. + * Creates a connection, send \a outbuf, wait for reply, copy reply to \a inbuf and close the connection. + * + * @param[in] prio priority, see #csp_prio_t + * @param[in] dst destination address + * @param[in] dst_port destination port + * @param[in] timeout timeout in mS to wait for a reply + * @param[in] outbuf outgoing data (request) + * @param[in] outlen length of data in \a outbuf (request) + * @param[out] inbuf user provided buffer for receiving data (reply) + * @param[in] inlen length of expected reply, -1 for unknown size (inbuf MUST be large enough), 0 for no reply. + * @param[in] opts connection options, see @ref CSP_CONNECTION_OPTIONS. + * + * Returns: + * int: 1 or reply size on success, 0 on failure (error, incoming length does not match, timeout) + */ +int csp_transaction_w_opts_timestamped(uint8_t prio, uint16_t dst, uint8_t dst_port, uint32_t timeout, void *outbuf, int outlen, void *inbuf, int inlen, uint32_t opts, uint64_t *timestamp); + /** * Perform an entire request & reply transaction. * Creates a connection, send \a outbuf, wait for reply, copy reply to \a inbuf and close the connection. @@ -415,6 +445,16 @@ void csp_buf_free(uint16_t node, uint32_t timeout); */ void csp_reboot(uint16_t node); +/** + * Synchronize time with the specified node. + * + * @param[in] node address of subsystem. + * @param[in] time_to_set time to set on the remote subsystem. + * + * @return #CSP_ERR_NONE on success, otherwise an error code. + */ +int csp_sync_time(uint16_t node); + /** * Shutdown subsystem. * If handled by the standard CSP service handler, the shutdown handler set by csp_sys_set_shutdown() on the subsystem, will be invoked. diff --git a/include/csp/csp_cmp.h b/include/csp/csp_cmp.h index 91d4b0188..5160d4f11 100644 --- a/include/csp/csp_cmp.h +++ b/include/csp/csp_cmp.h @@ -253,6 +253,17 @@ static inline int csp_cmp_poke_v2(uint16_t node, uint32_t timeout, struct csp_cm return csp_cmp(node, timeout, CSP_CMP_POKE_V2, CMP_SIZE(poke_v2) - sizeof(msg->poke_v2.data) + msg->poke_v2.len, msg); } +/** + * Get clock from remote node. + * + * @param[in] node address of subsystem. + * @param[in] timeout timeout in mS to wait for reply.. + * @param[in,out] msg clock. + * @param[out] rx_timestamp receive timestamp in nanoseconds. + * @return #CSP_ERR_NONE on success, otherwise an error code. + */ +int csp_cmp_clock_with_rx_timestamp(uint16_t node, uint32_t timeout, struct csp_cmp_message * msg, uint64_t *rx_timestamp); + #ifdef __cplusplus } #endif diff --git a/include/csp/csp_interface.h b/include/csp/csp_interface.h index b8daac3b6..48cc51f93 100644 --- a/include/csp/csp_interface.h +++ b/include/csp/csp_interface.h @@ -18,7 +18,7 @@ extern "C" { * * @return #CSP_ERR_NONE on success, otherwise an error code. */ -typedef int (*nexthop_t)(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me); +typedef int (*nexthop_t)(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me, uint64_t *timestamp); typedef int (*csp_alias_add_t)(void * driver_data, uint16_t addr); /** @@ -53,7 +53,6 @@ struct csp_iface_s { struct csp_iface_s * next; }; - /** * Used to represent an alias reception address, bound to a particular interface */ diff --git a/include/csp/csp_types.h b/include/csp/csp_types.h index 4e864e665..b2215e144 100644 --- a/include/csp/csp_types.h +++ b/include/csp/csp_types.h @@ -33,6 +33,7 @@ typedef enum { CSP_REBOOT = 4, /*< Reboot, see #CSP_REBOOT_MAGIC and #CSP_REBOOT_SHUTDOWN_MAGIC */ CSP_BUF_FREE = 5, /*< Free CSP buffers */ CSP_UPTIME = 6, /*< Uptime */ + CSP_TIME_SYNC = 7, /*< Time synchronization */ } csp_service_port_t; /** Listen on all ports, primarily used with csp_bind() */ @@ -211,6 +212,16 @@ typedef csp_memptr64_t (*csp_memwrite64_fnc_t)(csp_memptr64_t, csp_memptr_t, siz */ #define CSP_STATIC_ASSERT(condition, name) typedef char name[(condition) ? 1 : -1] +/** + * Time sync packet format + */ +typedef struct { + uint32_t id; + uint32_t tv_sec; + uint32_t tv_nsec; + uint8_t correction; +} csp_time_sync_t; + #ifdef __cplusplus } #endif diff --git a/include/csp/interfaces/csp_if_eth.h b/include/csp/interfaces/csp_if_eth.h index bf8f57369..c0b39c975 100644 --- a/include/csp/interfaces/csp_if_eth.h +++ b/include/csp/interfaces/csp_if_eth.h @@ -87,7 +87,7 @@ typedef struct csp_eth_header_s * @param[in] eth_frame CSP Ethernet frame to transmit * @return #CSP_ERR_NONE on success, otherwise an error code. */ -typedef int (*csp_eth_driver_tx_t)(void * driver_data, csp_eth_header_t * eth_frame); +typedef int (*csp_eth_driver_tx_t)(void * driver_data, csp_eth_header_t * eth_frame, uint64_t * timestamp); /** * CSP Interface data. @@ -134,7 +134,7 @@ void csp_eth_arp_get_addr(uint8_t * mac_addr, uint16_t csp_addr); * * @return #CSP_ERR_NONE on success, otherwise an error code. */ -int csp_eth_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me); +int csp_eth_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me, uint64_t *timestamp); /** * Process received CSP ethernet frame. diff --git a/include/csp/interfaces/csp_if_i2c.h b/include/csp/interfaces/csp_if_i2c.h index 28909d234..a9faa3707 100644 --- a/include/csp/interfaces/csp_if_i2c.h +++ b/include/csp/interfaces/csp_if_i2c.h @@ -54,7 +54,7 @@ int csp_i2c_add_interface(csp_iface_t * iface); * * @return #CSP_ERR_NONE on success, otherwise an error code. */ -int csp_i2c_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me); +int csp_i2c_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me, uint64_t *timestamp); /** * Process received I2C frame. diff --git a/include/csp/interfaces/csp_if_kiss.h b/include/csp/interfaces/csp_if_kiss.h index a40f6cd7b..67c4f7782 100644 --- a/include/csp/interfaces/csp_if_kiss.h +++ b/include/csp/interfaces/csp_if_kiss.h @@ -62,7 +62,7 @@ int csp_kiss_add_interface(csp_iface_t * iface); * * @return #CSP_ERR_NONE on success, otherwise an error code. */ -int csp_kiss_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me); +int csp_kiss_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me, uint64_t *timestamp); /** * Process received CAN frame. diff --git a/src/csp_bridge.c b/src/csp_bridge.c index 644959944..bbc725c39 100644 --- a/src/csp_bridge.c +++ b/src/csp_bridge.c @@ -64,6 +64,6 @@ void csp_bridge_work(void) { } /* Send to the interface directly, no hassle */ - csp_send_direct_iface(&packet->id, packet, destif, CSP_NO_VIA_ADDRESS, 0); + csp_send_direct_iface(&packet->id, packet, destif, CSP_NO_VIA_ADDRESS, 0, NULL); } diff --git a/src/csp_io.c b/src/csp_io.c index 38c2b6911..0bf874143 100644 --- a/src/csp_io.c +++ b/src/csp_io.c @@ -125,14 +125,14 @@ static inline void send_packet(csp_id_t * idout_copy, csp_packet_t * snd_pkt, cs } } -void csp_send_direct(csp_id_t* idout, csp_packet_t * packet, csp_iface_t * routed_from) { +void csp_send_direct(csp_id_t* idout, csp_packet_t * packet, csp_iface_t * routed_from, uint64_t *tx_timestamp) { int from_me = (routed_from == NULL ? 1 : 0); int via = CSP_NO_VIA_ADDRESS; /* Quickly send on loopback */ if (idout->dst == csp_if_lo.addr) { - csp_send_direct_iface(idout, packet, &csp_if_lo, via, from_me); + csp_send_direct_iface(idout, packet, &csp_if_lo, via, from_me, tx_timestamp); return; } @@ -186,7 +186,7 @@ void csp_send_direct(csp_id_t* idout, csp_packet_t * packet, csp_iface_t * route if (next_iface != NULL) { csp_packet_t * copy = csp_buffer_clone(packet); - send_packet(&idout_copy, copy, next_iface, via, from_me); + send_packet(&idout_copy, copy, next_iface, via, from_me, tx_timestamp); } next_iface = route->iface; via = route->via; @@ -196,7 +196,7 @@ void csp_send_direct(csp_id_t* idout, csp_packet_t * packet, csp_iface_t * route /* If the above worked, we don't want to look at default interfaces */ if (route_found == 1) { if (next_iface != NULL) { - send_packet(&idout_copy, packet, next_iface, via, from_me); + send_packet(&idout_copy, packet, next_iface, via, from_me, tx_timestamp); } else { csp_buffer_free(packet); } @@ -214,13 +214,13 @@ void csp_send_direct(csp_id_t* idout, csp_packet_t * packet, csp_iface_t * route if (next_iface != NULL) { csp_packet_t * copy = csp_buffer_clone(packet); - send_packet(&idout_copy, copy, next_iface, via, from_me); + send_packet(&idout_copy, copy, next_iface, via, from_me, tx_timestamp); } next_iface = iface; } if (next_iface != NULL) { - send_packet(&idout_copy, packet, next_iface, via, from_me); + send_packet(&idout_copy, packet, next_iface, via, from_me, tx_timestamp); return; } @@ -235,7 +235,7 @@ __weak void csp_output_hook(const csp_id_t * idout, csp_packet_t * packet, csp_i return; } -void csp_send_direct_iface(const csp_id_t* idout, csp_packet_t * packet, csp_iface_t * iface, uint16_t via, int from_me) { +void csp_send_direct_iface(csp_id_t* idout, csp_packet_t * packet, csp_iface_t * iface, uint16_t via, int from_me, uint64_t *tx_timestamp) { csp_output_hook(idout, packet, iface, via, from_me); @@ -281,7 +281,7 @@ void csp_send_direct_iface(const csp_id_t* idout, csp_packet_t * packet, csp_ifa /* Store length before passing to interface */ uint16_t bytes = packet->length; - if ((*iface->nexthop)(iface, via, packet, from_me) != CSP_ERR_NONE) + if ((*iface->nexthop)(iface, via, packet, from_me, tx_timestamp) != CSP_ERR_NONE) goto tx_err; iface->tx++; @@ -315,8 +315,21 @@ void csp_send(csp_conn_t * conn, csp_packet_t * packet) { } #endif - csp_send_direct(&conn->idout, packet, NULL); + csp_send_direct(&conn->idout, packet, NULL, NULL); +} + +void csp_send_and_get_timestamp(csp_conn_t * conn, csp_packet_t * packet, uint64_t *tx_timestamp) { + + if (packet == NULL) { + return; + } + + if ((conn == NULL) || (conn->state != CONN_OPEN)) { + csp_buffer_free(packet); + return; + } + csp_send_direct(&conn->idout, packet, NULL, tx_timestamp); } void csp_send_prio(uint8_t prio, csp_conn_t * conn, csp_packet_t * packet) { @@ -324,7 +337,7 @@ void csp_send_prio(uint8_t prio, csp_conn_t * conn, csp_packet_t * packet) { csp_send(conn, packet); } -int csp_transaction_persistent(csp_conn_t * conn, uint32_t timeout, const void * outbuf, int outlen, void * inbuf, int inlen) { +static int csp_transaction_persistent(csp_conn_t * conn, uint32_t timeout, const void * outbuf, int outlen, void * inbuf, int inlen) { if(outlen > CSP_BUFFER_SIZE){ return 0; @@ -355,6 +368,10 @@ int csp_transaction_persistent(csp_conn_t * conn, uint32_t timeout, const void * return 0; } + if (timestamp) { + *timestamp = packet->timestamp; + } + memcpy(inbuf, packet->data, packet->length); int length = packet->length; csp_buffer_free(packet); @@ -367,7 +384,20 @@ int csp_transaction_w_opts(uint8_t prio, uint16_t dest, uint8_t port, uint32_t t if (conn == NULL) return 0; - int status = csp_transaction_persistent(conn, timeout, outbuf, outlen, inbuf, inlen); + int status = csp_transaction_persistent(conn, timeout, outbuf, outlen, inbuf, inlen, NULL); + + csp_close(conn); + + return status; +} + +int csp_transaction_w_opts_timestamped(uint8_t prio, uint16_t dest, uint8_t port, uint32_t timeout, void * outbuf, int outlen, void * inbuf, int inlen, uint32_t opts, uint64_t *timestamp) { + + csp_conn_t * conn = csp_connect(prio, dest, port, 0, opts); + if (conn == NULL) + return 0; + + int status = csp_transaction_persistent(conn, timeout, outbuf, outlen, inbuf, inlen, timestamp); csp_close(conn); @@ -415,7 +445,7 @@ void csp_sendto(uint8_t prio, uint16_t dest, uint8_t dport, uint8_t src_port, ui packet->id.sport = src_port; packet->id.pri = prio; - csp_send_direct(&packet->id, packet, NULL); + csp_send_direct(&packet->id, packet, NULL, NULL); } diff --git a/src/csp_io.h b/src/csp_io.h index 7e829ee4d..1974be373 100644 --- a/src/csp_io.h +++ b/src/csp_io.h @@ -9,5 +9,6 @@ * @return #CSP_ERR_NONE on success, otherwise an error code. * */ -void csp_send_direct(csp_id_t* idout, csp_packet_t * packet, csp_iface_t * routed_from); -void csp_send_direct_iface(const csp_id_t* idout, csp_packet_t * packet, csp_iface_t * iface, uint16_t via, int from_me); + +void csp_send_direct(csp_id_t* idout, csp_packet_t * packet, csp_iface_t * routed_from, uint64_t *tx_timestamp); +void csp_send_direct_iface(csp_id_t* idout, csp_packet_t * packet, csp_iface_t * iface, uint16_t via, int from_me, uint64_t *tx_timestamp); diff --git a/src/csp_rdp.c b/src/csp_rdp.c index 4eaf4ea84..1319e27b7 100644 --- a/src/csp_rdp.c +++ b/src/csp_rdp.c @@ -162,7 +162,7 @@ static int csp_rdp_send_cmp(csp_conn_t * conn, csp_packet_t * packet, int flags, packet->length, (unsigned int)(packet->length - sizeof(rdp_header_t))); /* Send packet to IF */ - csp_send_direct(&idout, packet, NULL); + csp_send_direct(&idout, packet, NULL, NULL); return CSP_ERR_NONE; @@ -398,7 +398,7 @@ void csp_rdp_check_timeouts(csp_conn_t * conn) { /* Send copy to tx_queue */ packet->timestamp_tx = csp_get_ms(); csp_buffer_copy(packet, new_packet); - csp_send_direct(&conn->idout, new_packet, NULL); + csp_send_direct(&conn->idout, new_packet, NULL, NULL); } else { csp_rdp_error("RDP %p: Failed to allocate packet buffer\n", (void *)conn); } diff --git a/src/csp_route.c b/src/csp_route.c index 79cd23d24..d50e97d58 100644 --- a/src/csp_route.c +++ b/src/csp_route.c @@ -164,7 +164,7 @@ int csp_route_work(void) { if (!is_to_me) { /* Otherwise, actually send the message */ - csp_send_direct(&packet->id, packet, input.iface); + csp_send_direct(&packet->id, packet, input.iface, NULL); return CSP_ERR_NONE; } diff --git a/src/csp_service_handler.c b/src/csp_service_handler.c index 2e7691954..3ab4d816b 100644 --- a/src/csp_service_handler.c +++ b/src/csp_service_handler.c @@ -175,7 +175,7 @@ static int do_cmp_poke_v2(struct csp_cmp_message * cmp) { return CSP_ERR_NONE; } -static int do_cmp_clock(struct csp_cmp_message * cmp, uint64_t packet_timestamp) { +static int do_cmp_clock(struct csp_cmp_message * cmp) { csp_timestamp_t clock; clock.tv_sec = be32toh(cmp->clock.tv_sec); @@ -184,7 +184,7 @@ static int do_cmp_clock(struct csp_cmp_message * cmp, uint64_t packet_timestamp) int res = CSP_ERR_NONE; if (clock.tv_sec != 0) { // set time - res = csp_clock_set_time(&clock, packet_timestamp); + res = csp_clock_set_time(&clock, 0); if (res != CSP_ERR_NONE) { csp_dbg_errno = CSP_DBG_ERR_CLOCK_SET_FAIL; } @@ -246,7 +246,7 @@ static int csp_cmp_handler(csp_packet_t * packet) { break; case CSP_CMP_CLOCK: - ret = do_cmp_clock(cmp, packet->timestamp); + ret = do_cmp_clock(cmp); break; default: @@ -273,7 +273,6 @@ void csp_service_handler(csp_packet_t * packet) { case CSP_PING: { /* A ping means, just echo the packet, so no changes */ - printf("Packet TS: %"PRIu64"\n", packet->timestamp); break; } @@ -330,6 +329,38 @@ void csp_service_handler(csp_packet_t * packet) { break; } + case CSP_TIME_SYNC: { + static uint32_t last_sync_id = 0; + static uint64_t last_sync_rx = 0; + + /* Get time from packet */ + csp_time_sync_t time_sync; + memcpy(&time_sync, packet->data, sizeof(csp_time_sync_t)); + + uint32_t id = be32toh(time_sync.id); + + if (id == last_sync_id && last_sync_rx > 0 && time_sync.correction) { + csp_timestamp_t now_ts; + now_ts.tv_sec = be32toh(time_sync.tv_sec); + now_ts.tv_nsec = be32toh(time_sync.tv_nsec); + + csp_clock_set_time(&now_ts, last_sync_rx); + } else { + /* Save sync info */ + last_sync_id = id; + last_sync_rx = packet->timestamp; + } + + printf("Time sync packet received: id=%"PRIu32", sec=%"PRIu32", nsec=%"PRIu32", correction=%d\n", + id, + be32toh(time_sync.tv_sec), + be32toh(time_sync.tv_nsec), + time_sync.correction); + + csp_buffer_free(packet); + break; + } + default: csp_buffer_free(packet); return; diff --git a/src/csp_services.c b/src/csp_services.c index c1d191c3a..8200acefc 100644 --- a/src/csp_services.c +++ b/src/csp_services.c @@ -98,6 +98,54 @@ void csp_shutdown(uint16_t node) { csp_transaction_w_opts(CSP_PRIO_NORM, node, CSP_REBOOT, 0, &magic_word, sizeof(magic_word), NULL, 0, CSP_O_CRC32); } +int csp_sync_time(uint16_t node) { + + /* Prepare data */ + csp_packet_t * packet = csp_buffer_get(0); + if (packet == NULL) + return -1; + + /* Open connection */ + csp_conn_t * conn = csp_connect(CSP_PRIO_NORM, node, CSP_TIME_SYNC, 0, CSP_O_CRC32); + if (conn == NULL) { + csp_buffer_free(packet); + return -1; + } + + csp_time_sync_t time_sync; + static uint32_t sync_id = 1; + time_sync.id = htobe32(sync_id++); + time_sync.tv_sec = 0; + time_sync.tv_nsec = 0; + time_sync.correction = 0; + packet->length = sizeof(time_sync); + memcpy(packet->data, &time_sync, sizeof(time_sync)); + + uint64_t tx_timestamp = 0; + csp_send_and_get_timestamp(conn, packet, &tx_timestamp); + if (tx_timestamp == 0) { + /* No timestamp available */ + return -2; + } + + packet = csp_buffer_get(0); + if (packet == NULL) + return -1; + + time_sync.tv_sec = htobe32(tx_timestamp / (uint64_t)1E9); + time_sync.tv_nsec = htobe32(tx_timestamp % (uint64_t)1E9); + time_sync.correction = 1; + + packet->length = sizeof(time_sync); + memcpy(packet->data, &time_sync, sizeof(time_sync)); + + csp_send(conn, packet); + + csp_close(conn); + + return CSP_ERR_NONE; +} + void csp_ps(uint16_t node, uint32_t timeout) { /* Open connection */ @@ -221,3 +269,14 @@ int csp_cmp(uint16_t node, uint32_t timeout, uint8_t code, int msg_size, struct return CSP_ERR_NONE; } + +int csp_cmp_clock_with_rx_timestamp(uint16_t node, uint32_t timeout, struct csp_cmp_message * msg, uint64_t *rx_timestamp) { + msg->type = CSP_CMP_REQUEST; + msg->code = CSP_CMP_CLOCK; + int status = csp_transaction_w_opts_timestamped(CSP_PRIO_NORM, node, CSP_CMP, timeout, msg, CMP_SIZE(clock), msg, CMP_SIZE(clock), CSP_O_CRC32, rx_timestamp); + if (status == 0) { + return CSP_ERR_TIMEDOUT; + } + + return CSP_ERR_NONE; +} diff --git a/src/drivers/eth/eth_linux.c b/src/drivers/eth/eth_linux.c index fdbf17d6b..0a82c9277 100644 --- a/src/drivers/eth/eth_linux.c +++ b/src/drivers/eth/eth_linux.c @@ -21,6 +21,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -38,7 +41,7 @@ typedef struct { struct ifreq if_idx; } eth_context_t; -int csp_eth_tx_frame(void * driver_data, csp_eth_header_t * eth_frame) { +int csp_eth_tx_frame(void * driver_data, csp_eth_header_t * eth_frame, uint64_t * timestamp) { const eth_context_t * ctx = (eth_context_t*)driver_data; @@ -50,10 +53,69 @@ int csp_eth_tx_frame(void * driver_data, csp_eth_header_t * eth_frame) { uint32_t txsize = sizeof(csp_eth_header_t) + be16toh(eth_frame->seg_size); - if (sendto(ctx->sockfd, (void*)eth_frame, txsize, 0, (struct sockaddr*)&socket_address, sizeof(struct sockaddr_ll)) < 0) { + /* Setup sendmsg for TX timestamping */ + struct iovec iov = { + .iov_base = eth_frame, + .iov_len = txsize + }; + + struct msghdr msg = { + .msg_name = &socket_address, + .msg_namelen = sizeof(struct sockaddr_ll), + .msg_iov = &iov, + .msg_iovlen = 1, + .msg_control = NULL, + .msg_controllen = 0, + .msg_flags = 0 + }; + + if (sendmsg(ctx->sockfd, &msg, 0) < 0) { return CSP_ERR_DRIVER; } + /* Retrieve TX timestamp from error queue */ + char control[CMSG_SPACE(sizeof(struct scm_timestamping))]; + char data[CSP_ETH_BUF_SIZE]; + struct iovec iov_err = { + .iov_base = data, + .iov_len = sizeof(data) + }; + + struct msghdr msg_err = { + .msg_name = NULL, + .msg_namelen = 0, + .msg_iov = &iov_err, + .msg_iovlen = 1, + .msg_control = control, + .msg_controllen = sizeof(control), + .msg_flags = 0 + }; + + /* Try to get TX timestamp (non-blocking) */ + int err_result = recvmsg(ctx->sockfd, &msg_err, MSG_ERRQUEUE | MSG_DONTWAIT); + if (err_result >= 0) { + for (struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg_err); cmsg != NULL; cmsg = CMSG_NXTHDR(&msg_err, cmsg)) { + if (eth_debug) { + csp_print("TX cmsg: level=%d type=%d\n", cmsg->cmsg_level, cmsg->cmsg_type); + } + if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SO_TIMESTAMPING) { + struct scm_timestamping *ts_data = (struct scm_timestamping *)CMSG_DATA(cmsg); + uint64_t tx_timestamp = 0; + /* Try hardware timestamp first (ts[2]), then software (ts[0]) */ + tx_timestamp = (uint64_t)ts_data->ts[0].tv_sec * 1000000000ULL + (uint64_t)ts_data->ts[0].tv_nsec; + if (eth_debug) { + csp_print("TX SW timestamp: %llu ns\n", tx_timestamp); + } + if (timestamp && tx_timestamp) { + *timestamp = tx_timestamp; + } + break; + } + } + } else if (eth_debug) { + csp_print("TX: No timestamp from error queue (errno=%d)\n", errno); + } + return CSP_ERR_NONE; } @@ -64,13 +126,59 @@ void * csp_eth_rx_loop(void * param) { static uint8_t recvbuf[CSP_ETH_BUF_SIZE]; csp_eth_header_t * eth_frame = (csp_eth_header_t *)recvbuf; + uint64_t timestamp = 0; + + /* Setup for recvmsg to receive timestamp */ + struct iovec iov = { + .iov_base = recvbuf, + .iov_len = CSP_ETH_BUF_SIZE + }; + + char control[CMSG_SPACE(sizeof(struct scm_timestamping))]; + + struct msghdr msg = { + .msg_name = NULL, + .msg_namelen = 0, + .msg_iov = &iov, + .msg_iovlen = 1, + .msg_control = control, + .msg_controllen = sizeof(control), + .msg_flags = 0 + }; + while(1) { - /* Receive packet segment */ + /* Receive packet segment with timestamp */ + int32_t received_len = recvmsg(ctx->sockfd, &msg, 0); + + if (received_len < 0) { + continue; + } - uint32_t received_len = recvfrom(ctx->sockfd, recvbuf, CSP_ETH_BUF_SIZE, 0, NULL, NULL); + /* Extract timestamp from control message */ + timestamp = 0; + for (struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; cmsg = CMSG_NXTHDR(&msg, cmsg)) { + if (eth_debug) { + csp_print("RX cmsg: level=%d type=%d\n", cmsg->cmsg_level, cmsg->cmsg_type); + } + if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SO_TIMESTAMPING) { + struct scm_timestamping *ts_data = (struct scm_timestamping *)CMSG_DATA(cmsg); + /* Try hardware timestamp first (ts[2]), then software (ts[0]) */ + if (ts_data->ts[0].tv_sec || ts_data->ts[0].tv_nsec) { + timestamp = (uint64_t)ts_data->ts[0].tv_sec * 1000000000ULL + (uint64_t)ts_data->ts[0].tv_nsec; + if (eth_debug) { + csp_print("RX SW timestamp: %llu ns\n", timestamp); + } + } + break; + } + } + + if (eth_debug && timestamp == 0) { + csp_print("RX: No timestamp received\n"); + } - csp_eth_rx(&ctx->ifdata.iface, eth_frame, received_len, NULL); + csp_eth_rx(&ctx->ifdata.iface, eth_frame, received_len, NULL, timestamp); } return NULL; @@ -157,6 +265,14 @@ int csp_eth_init(const char * device, const char * ifname, int mtu, unsigned int return CSP_ERR_INVAL; } + // Enable TX timestamping + int ts_flags = SOF_TIMESTAMPING_TX_SOFTWARE | SOF_TIMESTAMPING_RX_SOFTWARE | SOF_TIMESTAMPING_SOFTWARE; + if (setsockopt(ctx->sockfd, SOL_SOCKET, SO_TIMESTAMPING, &ts_flags, sizeof(ts_flags)) == -1) { + perror("setsockopt SO_TIMESTAMPING"); + } else { + csp_print("Using hardware timestamping on %s\n", device); + } + /* Bind to device */ if (setsockopt(ctx->sockfd, SOL_SOCKET, SO_BINDTODEVICE, device, IFNAMSIZ-1) == -1) { perror("SO_BINDTODEVICE"); diff --git a/src/interfaces/csp_if_can.c b/src/interfaces/csp_if_can.c index 734d6659d..be592599d 100644 --- a/src/interfaces/csp_if_can.c +++ b/src/interfaces/csp_if_can.c @@ -163,8 +163,9 @@ static int csp_can1_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u return CSP_ERR_NONE; } -static int csp_can1_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me) { +static int csp_can1_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me, uint64_t *timestamp) { (void)from_me; /* Avoid compiler warnings about unused parameter */ + (void)timestamp; /* Avoid compiler warnings about unused parameter */ /* Loopback */ if (packet->id.dst == iface->addr) { @@ -372,10 +373,11 @@ static int csp_can2_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u return CSP_ERR_NONE; } -static int csp_can2_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me) { +static int csp_can2_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me, uint64_t *timestamp) { /* Avoid compiler warnings about unused parameter */ (void)via; (void)from_me; + (void)timestamp; /* Loopback */ if (packet->id.dst == iface->addr || csp_addr_is_alias(packet->id.dst)) { diff --git a/src/interfaces/csp_if_eth.c b/src/interfaces/csp_if_eth.c index 7c5acead9..6ce61bd4e 100644 --- a/src/interfaces/csp_if_eth.c +++ b/src/interfaces/csp_if_eth.c @@ -241,8 +241,7 @@ int csp_eth_rx(csp_iface_t * iface, csp_eth_header_t * eth_frame, uint32_t recei return CSP_ERR_NONE; } -int csp_eth_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me) { - /* Avoid compiler warnings about unused parameter */ +int csp_eth_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me, uint64_t *timestamp) { (void)via; (void)from_me; @@ -259,7 +258,7 @@ int csp_eth_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int fro static atomic_int packet_id = 0; packet_id++; uint16_t offset = 0; - + while (offset < packet->frame_length) { csp_eth_header_t *eth_frame = ifdata->tx_buf; @@ -278,7 +277,7 @@ int csp_eth_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int fro memcpy(eth_frame->frame_begin, packet->frame_begin + offset, seg_size); - if ((ifdata->tx_func)(iface->driver_data, eth_frame) != CSP_ERR_NONE) { + if ((ifdata->tx_func)(iface->driver_data, eth_frame, timestamp) != CSP_ERR_NONE) { iface->tx_error++; /* Does not free on return */ return CSP_ERR_DRIVER; diff --git a/src/interfaces/csp_if_i2c.c b/src/interfaces/csp_if_i2c.c index 68e740e58..9b3ecb893 100644 --- a/src/interfaces/csp_if_i2c.c +++ b/src/interfaces/csp_if_i2c.c @@ -4,8 +4,9 @@ #include #include -int csp_i2c_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me) { - (void)from_me; /* Avoid compiler warnings about unused parameter */ +int csp_i2c_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me, uint64_t *timestamp) { + (void)from_me; + (void)timestamp; /* Loopback */ if (packet->id.dst == iface->addr) { diff --git a/src/interfaces/csp_if_kiss.c b/src/interfaces/csp_if_kiss.c index 5b9dc33b7..2bb78cfa1 100644 --- a/src/interfaces/csp_if_kiss.c +++ b/src/interfaces/csp_if_kiss.c @@ -22,10 +22,11 @@ #define TFESC 0xDD #define TNC_DATA 0x00 -int csp_kiss_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me) { +int csp_kiss_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me, uint64_t *timestamp) { /* Avoid compiler warnings about unused parameter */ (void)via; (void)from_me; + (void)timestamp; csp_kiss_interface_data_t * ifdata = iface->interface_data; void * driver = iface->driver_data; diff --git a/src/interfaces/csp_if_lo.c b/src/interfaces/csp_if_lo.c index f4f17cfba..26121454a 100644 --- a/src/interfaces/csp_if_lo.c +++ b/src/interfaces/csp_if_lo.c @@ -8,7 +8,7 @@ * @param packet Packet to transmit * @return 1 if packet was successfully transmitted, 0 on error */ -static int csp_lo_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me) { +static int csp_lo_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me, uint64_t *timestamp) { /* Avoid compiler warnings about unused parameter */ (void)iface; (void)via; diff --git a/src/interfaces/csp_if_tun.c b/src/interfaces/csp_if_tun.c index 43f5a82ca..77ddbe9e6 100644 --- a/src/interfaces/csp_if_tun.c +++ b/src/interfaces/csp_if_tun.c @@ -22,7 +22,7 @@ __weak int csp_crypto_encrypt(uint8_t * msg_begin, uint8_t msg_len, uint8_t * ci return -1; } -static int csp_if_tun_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me) { +static int csp_if_tun_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me, uint64_t *timestamp) { /* Avoid compiler warnings about unused parameter */ (void)via; (void)from_me; diff --git a/src/interfaces/csp_if_udp.c b/src/interfaces/csp_if_udp.c index ec88b167f..4c9e3d057 100644 --- a/src/interfaces/csp_if_udp.c +++ b/src/interfaces/csp_if_udp.c @@ -17,7 +17,7 @@ #define MSG_CONFIRM (0) #endif -static int csp_if_udp_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me) { +static int csp_if_udp_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me, uint64_t *timestamp) { /* Avoid compiler warnings about unused parameter */ (void)via; (void)from_me; diff --git a/src/interfaces/csp_if_zmqhub.c b/src/interfaces/csp_if_zmqhub.c index 061f72032..223049282 100644 --- a/src/interfaces/csp_if_zmqhub.c +++ b/src/interfaces/csp_if_zmqhub.c @@ -81,7 +81,7 @@ void * csp_zmqhub_fixup_cspv1_del_dest_addr(uint8_t * rx_data, size_t * datalen) * @param packet Packet to transmit * @return 1 if packet was successfully transmitted, 0 on error */ -static int csp_zmqhub_tx(csp_iface_t * iface, uint16_t __maybe_unused via, csp_packet_t * packet, int __maybe_unused from_me) { +static int csp_zmqhub_tx(csp_iface_t * iface, uint16_t __maybe_unused via, csp_packet_t * packet, int __maybe_unused from_me, uint64_t *timestamp) { zmq_driver_t * drv = iface->driver_data; From 5d568f3cd3625e7e072aae64d3e774e2f0e04dc5 Mon Sep 17 00:00:00 2001 From: Jens Henrik Hertz Date: Thu, 6 Nov 2025 12:12:54 +0100 Subject: [PATCH 3/7] Update service handler --- src/arch/posix/csp_clock.c | 2 +- src/csp_service_handler.c | 7 +------ 2 files changed, 2 insertions(+), 7 deletions(-) diff --git a/src/arch/posix/csp_clock.c b/src/arch/posix/csp_clock.c index 94c8dc8e6..ce299b9c8 100644 --- a/src/arch/posix/csp_clock.c +++ b/src/arch/posix/csp_clock.c @@ -15,7 +15,7 @@ __weak void csp_clock_get_time(csp_timestamp_t * time) { } } -__weak int csp_clock_set_time(const csp_timestamp_t * time) { +__weak int csp_clock_set_time(const csp_timestamp_t * time, uint64_t rx_timestamp) { struct timespec ts = {.tv_sec = time->tv_sec, .tv_nsec = time->tv_nsec}; if (clock_settime(CLOCK_REALTIME, &ts) == 0) { diff --git a/src/csp_service_handler.c b/src/csp_service_handler.c index 3ab4d816b..e4b97a2ac 100644 --- a/src/csp_service_handler.c +++ b/src/csp_service_handler.c @@ -273,6 +273,7 @@ void csp_service_handler(csp_packet_t * packet) { case CSP_PING: { /* A ping means, just echo the packet, so no changes */ + // printf("Packet TS: %"PRIu64"\n", packet->timestamp); break; } @@ -351,12 +352,6 @@ void csp_service_handler(csp_packet_t * packet) { last_sync_rx = packet->timestamp; } - printf("Time sync packet received: id=%"PRIu32", sec=%"PRIu32", nsec=%"PRIu32", correction=%d\n", - id, - be32toh(time_sync.tv_sec), - be32toh(time_sync.tv_nsec), - time_sync.correction); - csp_buffer_free(packet); break; } From ca87edc89c78c46d333904517074948d6c6b58eb Mon Sep 17 00:00:00 2001 From: Jens Henrik Hertz Date: Thu, 6 Nov 2025 15:18:56 +0100 Subject: [PATCH 4/7] Add fallback to local time when no hw timestamping available --- src/csp_services.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/csp_services.c b/src/csp_services.c index 8200acefc..129865e89 100644 --- a/src/csp_services.c +++ b/src/csp_services.c @@ -7,6 +7,7 @@ #include #include #include +#include int csp_ping(uint16_t node, uint32_t timeout, unsigned int size, uint8_t conn_options) { @@ -121,11 +122,15 @@ int csp_sync_time(uint16_t node) { packet->length = sizeof(time_sync); memcpy(packet->data, &time_sync, sizeof(time_sync)); + csp_timestamp_t fallback_ts; + csp_clock_get_time(&fallback_ts); + uint64_t tx_timestamp = 0; csp_send_and_get_timestamp(conn, packet, &tx_timestamp); if (tx_timestamp == 0) { /* No timestamp available */ - return -2; + csp_print("Warning: No TX timestamp available when syncing time to node %u, using local clock as fallback\n", node); + tx_timestamp = ((uint64_t)fallback_ts.tv_sec * (uint64_t)1E9) + (uint64_t)fallback_ts.tv_nsec; } packet = csp_buffer_get(0); From 03626046da70efa9a2cbb782a85ba2c70b0713f2 Mon Sep 17 00:00:00 2001 From: Johan de Claville Christiansen Date: Mon, 16 Feb 2026 20:11:14 +0100 Subject: [PATCH 5/7] Fix build after rebase This branch contains a refactoring of the csp core functions that we will not end up accepting, but this branch is left here for reference on the other stuff that was implemented. --- include/csp/csp.h | 2 +- include/csp/drivers/eth_linux.h | 2 +- src/arch/posix/csp_clock.c | 2 ++ src/csp_io.c | 12 ++++++------ src/csp_services.c | 1 + src/drivers/can/can_socketcan.c | 2 +- src/interfaces/csp_if_lo.c | 1 + src/interfaces/csp_if_tun.c | 1 + src/interfaces/csp_if_udp.c | 1 + src/interfaces/csp_if_zmqhub.c | 1 + 10 files changed, 16 insertions(+), 9 deletions(-) diff --git a/include/csp/csp.h b/include/csp/csp.h index fb27a18c4..8c5368f1b 100644 --- a/include/csp/csp.h +++ b/include/csp/csp.h @@ -191,7 +191,7 @@ static inline int csp_transaction(uint8_t prio, uint16_t dest, uint8_t port, uin * @param[in] inlen length of expected reply, -1 for unknown size (inbuf MUST be large enough), 0 for no reply. * @return 1 or reply size on success, 0 on failure (error, incoming length does not match, timeout) */ -int csp_transaction_persistent(csp_conn_t *conn, uint32_t timeout, const void *outbuf, int outlen, void *inbuf, int inlen); +int csp_transaction_persistent(csp_conn_t *conn, uint32_t timeout, const void *outbuf, int outlen, void *inbuf, int inlen, uint64_t *timestamp); /** * Read data from a connection-less server socket. diff --git a/include/csp/drivers/eth_linux.h b/include/csp/drivers/eth_linux.h index e0233926a..3116e9336 100644 --- a/include/csp/drivers/eth_linux.h +++ b/include/csp/drivers/eth_linux.h @@ -36,7 +36,7 @@ int csp_eth_init(const char * device, const char * ifname, int mtu, unsigned int * @param[in] eth_frame The CSP ethernet frame to transmit. * @return #CSP_ERR_NONE on success, otherwise an error code. */ -int csp_eth_tx_frame(void * driver_data, csp_eth_header_t * eth_frame); +int csp_eth_tx_frame(void * driver_data, csp_eth_header_t * eth_frame, uint64_t * timestamp); /** * Posix ethernet RX thread diff --git a/src/arch/posix/csp_clock.c b/src/arch/posix/csp_clock.c index ce299b9c8..173fd859a 100644 --- a/src/arch/posix/csp_clock.c +++ b/src/arch/posix/csp_clock.c @@ -17,6 +17,8 @@ __weak void csp_clock_get_time(csp_timestamp_t * time) { __weak int csp_clock_set_time(const csp_timestamp_t * time, uint64_t rx_timestamp) { + (void)rx_timestamp; + struct timespec ts = {.tv_sec = time->tv_sec, .tv_nsec = time->tv_nsec}; if (clock_settime(CLOCK_REALTIME, &ts) == 0) { return CSP_ERR_NONE; diff --git a/src/csp_io.c b/src/csp_io.c index 0bf874143..388e4cda1 100644 --- a/src/csp_io.c +++ b/src/csp_io.c @@ -121,7 +121,7 @@ static inline void send_packet(csp_id_t * idout_copy, csp_packet_t * snd_pkt, cs } if (snd_pkt != NULL) { - csp_send_direct_iface(idout_copy, snd_pkt, snd_iface, via, from_me); + csp_send_direct_iface(idout_copy, snd_pkt, snd_iface, via, from_me, NULL); } } @@ -186,7 +186,7 @@ void csp_send_direct(csp_id_t* idout, csp_packet_t * packet, csp_iface_t * route if (next_iface != NULL) { csp_packet_t * copy = csp_buffer_clone(packet); - send_packet(&idout_copy, copy, next_iface, via, from_me, tx_timestamp); + send_packet(&idout_copy, copy, next_iface, via, from_me); } next_iface = route->iface; via = route->via; @@ -196,7 +196,7 @@ void csp_send_direct(csp_id_t* idout, csp_packet_t * packet, csp_iface_t * route /* If the above worked, we don't want to look at default interfaces */ if (route_found == 1) { if (next_iface != NULL) { - send_packet(&idout_copy, packet, next_iface, via, from_me, tx_timestamp); + send_packet(&idout_copy, packet, next_iface, via, from_me); } else { csp_buffer_free(packet); } @@ -214,13 +214,13 @@ void csp_send_direct(csp_id_t* idout, csp_packet_t * packet, csp_iface_t * route if (next_iface != NULL) { csp_packet_t * copy = csp_buffer_clone(packet); - send_packet(&idout_copy, copy, next_iface, via, from_me, tx_timestamp); + send_packet(&idout_copy, copy, next_iface, via, from_me); } next_iface = iface; } if (next_iface != NULL) { - send_packet(&idout_copy, packet, next_iface, via, from_me, tx_timestamp); + send_packet(&idout_copy, packet, next_iface, via, from_me); return; } @@ -337,7 +337,7 @@ void csp_send_prio(uint8_t prio, csp_conn_t * conn, csp_packet_t * packet) { csp_send(conn, packet); } -static int csp_transaction_persistent(csp_conn_t * conn, uint32_t timeout, const void * outbuf, int outlen, void * inbuf, int inlen) { +int csp_transaction_persistent(csp_conn_t * conn, uint32_t timeout, const void * outbuf, int outlen, void * inbuf, int inlen, uint64_t *timestamp) { if(outlen > CSP_BUFFER_SIZE){ return 0; diff --git a/src/csp_services.c b/src/csp_services.c index 129865e89..ace5f98b3 100644 --- a/src/csp_services.c +++ b/src/csp_services.c @@ -6,6 +6,7 @@ #include #include +#include #include #include diff --git a/src/drivers/can/can_socketcan.c b/src/drivers/can/can_socketcan.c index 56b228e7a..dd535260e 100644 --- a/src/drivers/can/can_socketcan.c +++ b/src/drivers/can/can_socketcan.c @@ -99,7 +99,7 @@ static void * socketcan_rx_thread(void * arg) { frame.can_id &= CAN_EFF_MASK; /* Call RX callbacsp_can_rx_frameck */ - csp_can_rx(&ctx->iface, frame.can_id, frame.data, frame.can_dlc, NULL); + csp_can_rx(&ctx->iface, frame.can_id, frame.data, frame.can_dlc, NULL, 0); } /* We should never reach this point */ diff --git a/src/interfaces/csp_if_lo.c b/src/interfaces/csp_if_lo.c index 26121454a..d45c1dea1 100644 --- a/src/interfaces/csp_if_lo.c +++ b/src/interfaces/csp_if_lo.c @@ -13,6 +13,7 @@ static int csp_lo_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, i (void)iface; (void)via; (void)from_me; + (void)timestamp; /* Send back into CSP, notice calling from task so last argument must be NULL! */ csp_qfifo_write(packet, &csp_if_lo, NULL); diff --git a/src/interfaces/csp_if_tun.c b/src/interfaces/csp_if_tun.c index 77ddbe9e6..1aab02d1f 100644 --- a/src/interfaces/csp_if_tun.c +++ b/src/interfaces/csp_if_tun.c @@ -26,6 +26,7 @@ static int csp_if_tun_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packe /* Avoid compiler warnings about unused parameter */ (void)via; (void)from_me; + (void)timestamp; csp_if_tun_conf_t * ifconf = iface->driver_data; diff --git a/src/interfaces/csp_if_udp.c b/src/interfaces/csp_if_udp.c index 4c9e3d057..21eadae2a 100644 --- a/src/interfaces/csp_if_udp.c +++ b/src/interfaces/csp_if_udp.c @@ -21,6 +21,7 @@ static int csp_if_udp_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packe /* Avoid compiler warnings about unused parameter */ (void)via; (void)from_me; + (void)timestamp; csp_if_udp_conf_t * ifconf = iface->driver_data; diff --git a/src/interfaces/csp_if_zmqhub.c b/src/interfaces/csp_if_zmqhub.c index 223049282..221da397d 100644 --- a/src/interfaces/csp_if_zmqhub.c +++ b/src/interfaces/csp_if_zmqhub.c @@ -82,6 +82,7 @@ void * csp_zmqhub_fixup_cspv1_del_dest_addr(uint8_t * rx_data, size_t * datalen) * @return 1 if packet was successfully transmitted, 0 on error */ static int csp_zmqhub_tx(csp_iface_t * iface, uint16_t __maybe_unused via, csp_packet_t * packet, int __maybe_unused from_me, uint64_t *timestamp) { + (void) timestamp; zmq_driver_t * drv = iface->driver_data; From c5e5d38d73107f8804d5a6ae4c6812b120467bc7 Mon Sep 17 00:00:00 2001 From: Henrik Davidsen Date: Wed, 25 Feb 2026 10:26:21 +0100 Subject: [PATCH 6/7] Refactor of Time Sync protocol --- contrib/drivers/can/csp_driver_can.c | 4 +- contrib/windows/csp_clock.c | 13 +++ doc/hooks.md | 1 + include/csp/csp.h | 42 +-------- include/csp/csp_cmp.h | 21 +++-- include/csp/csp_hooks.h | 22 ++++- include/csp/csp_interface.h | 3 +- include/csp/csp_types.h | 15 ++-- include/csp/drivers/eth_linux.h | 2 +- include/csp/interfaces/csp_if_can.h | 23 +++++ include/csp/interfaces/csp_if_eth.h | 6 +- include/csp/interfaces/csp_if_i2c.h | 2 +- include/csp/interfaces/csp_if_kiss.h | 2 +- src/arch/freertos/csp_clock.c | 11 +++ src/arch/posix/csp_clock.c | 17 +++- src/arch/zephyr/csp_clock.c | 11 +++ src/csp_bridge.c | 2 +- src/csp_io.c | 48 ++-------- src/csp_io.h | 5 +- src/csp_rdp.c | 4 +- src/csp_route.c | 2 +- src/csp_service_handler.c | 68 ++++++++------- src/csp_services.c | 64 -------------- src/drivers/can/can_zephyr.c | 2 +- src/drivers/eth/eth_linux.c | 126 ++------------------------- src/interfaces/csp_if_can.c | 37 ++++++-- src/interfaces/csp_if_eth.c | 12 ++- src/interfaces/csp_if_i2c.c | 5 +- src/interfaces/csp_if_kiss.c | 3 +- src/interfaces/csp_if_lo.c | 3 +- src/interfaces/csp_if_tun.c | 3 +- src/interfaces/csp_if_udp.c | 3 +- src/interfaces/csp_if_zmqhub.c | 3 +- 33 files changed, 224 insertions(+), 361 deletions(-) diff --git a/contrib/drivers/can/csp_driver_can.c b/contrib/drivers/can/csp_driver_can.c index 9e9f4e106..07c439426 100644 --- a/contrib/drivers/can/csp_driver_can.c +++ b/contrib/drivers/can/csp_driver_can.c @@ -83,7 +83,7 @@ void CAN_0_rx_callback(struct can_async_descriptor *const descr) { /* Process frame within ISR * (This can also be deferred to a task with: csp_can_process_frame_deferred) */ - csp_can_rx(&mcan[0].interface, msg.id, msg.data, msg.len, &xTaskWoken); + csp_can_rx(&mcan[0].interface, msg.id, msg.data, msg.len, &xTaskWoken, 0); } @@ -116,7 +116,7 @@ static void can_task(void * param) { /* Process frame within ISR * (This can also be deferred to a task with: csp_can_process_frame_deferred) */ - csp_can_rx(&mcan[0].interface, msg.id, msg.data, msg.len, &xTaskWoken); + csp_can_rx(&mcan[0].interface, msg.id, msg.data, msg.len, &xTaskWoken, 0); } diff --git a/contrib/windows/csp_clock.c b/contrib/windows/csp_clock.c index 422baf691..97c703cf0 100644 --- a/contrib/windows/csp_clock.c +++ b/contrib/windows/csp_clock.c @@ -21,3 +21,16 @@ void csp_clock_get_time(csp_timestamp_t * time) { int csp_clock_set_time(const csp_timestamp_t * time) { return CSP_ERR_NOTSUP; } + +//__weak int csp_clock_set_time_w_local_time(const csp_timestamp_t * time, uint64_t local_rx_ns) { +int csp_clock_set_time_w_local_time(const csp_timestamp_t * time, uint64_t local_rx_ns) { + (void)time; + (void)local_rx_ns; + return CSP_ERR_NOTSUP; +} + +//__weak void csp_set_packet_tx_time(const void *packet, uint64_t tx_time_ns) { +void csp_set_packet_tx_time(const void *packet, uint64_t tx_time_ns) { + (void)packet; + (void)tx_time_ns; +} diff --git a/doc/hooks.md b/doc/hooks.md index 6c91a7e97..7819190b9 100644 --- a/doc/hooks.md +++ b/doc/hooks.md @@ -44,6 +44,7 @@ application will fail to link if you use csp_if_tun without these two functions. ```c void csp_clock_get_time(csp_timestamp_t * time); int csp_clock_set_time(const csp_timestamp_t * time); +int csp_clock_set_time_w_local_time(const csp_timestamp_t * time, uint64_t local_rx_ns); ``` The get and set time functions rely on `arch/` and all have default implementations. diff --git a/include/csp/csp.h b/include/csp/csp.h index 8c5368f1b..41f968dfe 100644 --- a/include/csp/csp.h +++ b/include/csp/csp.h @@ -100,16 +100,6 @@ csp_packet_t *csp_read(csp_conn_t *conn, uint32_t timeout); */ void csp_send(csp_conn_t *conn, csp_packet_t *packet); -/** - * Send packet on a connection, and get the TX timestamp. - * The packet buffer is automatically freed, and cannot be used after the call to csp_send_with_timestamp() - * - * @param[in] conn connection - * @param[in] packet packet to send - * @param[out] tx_timestamp pointer to variable to receive the TX timestamp in nanoseconds, or NULL if not needed. - */ -void csp_send_and_get_timestamp(csp_conn_t *conn, csp_packet_t *packet, uint64_t *tx_timestamp); - /** * Change the default priority of the connection and send a packet. * @@ -141,26 +131,6 @@ void csp_send_prio(uint8_t prio, csp_conn_t *conn, csp_packet_t *packet); */ int csp_transaction_w_opts(uint8_t prio, uint16_t dst, uint8_t dst_port, uint32_t timeout, const void *outbuf, int outlen, void *inbuf, int inlen, uint32_t opts); - -/** - * Perform an entire request & reply transaction. Also returns the RX timestamp of the reply. - * Creates a connection, send \a outbuf, wait for reply, copy reply to \a inbuf and close the connection. - * - * @param[in] prio priority, see #csp_prio_t - * @param[in] dst destination address - * @param[in] dst_port destination port - * @param[in] timeout timeout in mS to wait for a reply - * @param[in] outbuf outgoing data (request) - * @param[in] outlen length of data in \a outbuf (request) - * @param[out] inbuf user provided buffer for receiving data (reply) - * @param[in] inlen length of expected reply, -1 for unknown size (inbuf MUST be large enough), 0 for no reply. - * @param[in] opts connection options, see @ref CSP_CONNECTION_OPTIONS. - * - * Returns: - * int: 1 or reply size on success, 0 on failure (error, incoming length does not match, timeout) - */ -int csp_transaction_w_opts_timestamped(uint8_t prio, uint16_t dst, uint8_t dst_port, uint32_t timeout, void *outbuf, int outlen, void *inbuf, int inlen, uint32_t opts, uint64_t *timestamp); - /** * Perform an entire request & reply transaction. * Creates a connection, send \a outbuf, wait for reply, copy reply to \a inbuf and close the connection. @@ -191,7 +161,7 @@ static inline int csp_transaction(uint8_t prio, uint16_t dest, uint8_t port, uin * @param[in] inlen length of expected reply, -1 for unknown size (inbuf MUST be large enough), 0 for no reply. * @return 1 or reply size on success, 0 on failure (error, incoming length does not match, timeout) */ -int csp_transaction_persistent(csp_conn_t *conn, uint32_t timeout, const void *outbuf, int outlen, void *inbuf, int inlen, uint64_t *timestamp); +int csp_transaction_persistent(csp_conn_t *conn, uint32_t timeout, const void *outbuf, int outlen, void *inbuf, int inlen); /** * Read data from a connection-less server socket. @@ -445,16 +415,6 @@ void csp_buf_free(uint16_t node, uint32_t timeout); */ void csp_reboot(uint16_t node); -/** - * Synchronize time with the specified node. - * - * @param[in] node address of subsystem. - * @param[in] time_to_set time to set on the remote subsystem. - * - * @return #CSP_ERR_NONE on success, otherwise an error code. - */ -int csp_sync_time(uint16_t node); - /** * Shutdown subsystem. * If handled by the standard CSP service handler, the shutdown handler set by csp_sys_set_shutdown() on the subsystem, will be invoked. diff --git a/include/csp/csp_cmp.h b/include/csp/csp_cmp.h index 5160d4f11..dcdda376d 100644 --- a/include/csp/csp_cmp.h +++ b/include/csp/csp_cmp.h @@ -70,6 +70,14 @@ extern "C" { * Poke/write data from memory - 64-bit version. */ #define CSP_CMP_POKE_V2 9 +/** + * Set clock with Time sync protocol. + */ +#define CSP_CMP_CLOCK_TIME_SYNC 10 +/** + * Time correction with Time sync protocol. + */ +#define CSP_CMP_CLOCK_CORRECTION_TIME_SYNC 11 /**@}*/ /** @@ -171,6 +179,8 @@ struct csp_cmp_message { char data[CSP_CMP_POKE_V2_MAX_LEN]; } poke_v2; csp_timestamp_t clock; + csp_time_sync_t time_sync; + csp_time_sync_correction_t time_sync_correction; }; } __attribute__((__packed__)); @@ -253,17 +263,6 @@ static inline int csp_cmp_poke_v2(uint16_t node, uint32_t timeout, struct csp_cm return csp_cmp(node, timeout, CSP_CMP_POKE_V2, CMP_SIZE(poke_v2) - sizeof(msg->poke_v2.data) + msg->poke_v2.len, msg); } -/** - * Get clock from remote node. - * - * @param[in] node address of subsystem. - * @param[in] timeout timeout in mS to wait for reply.. - * @param[in,out] msg clock. - * @param[out] rx_timestamp receive timestamp in nanoseconds. - * @return #CSP_ERR_NONE on success, otherwise an error code. - */ -int csp_cmp_clock_with_rx_timestamp(uint16_t node, uint32_t timeout, struct csp_cmp_message * msg, uint64_t *rx_timestamp); - #ifdef __cplusplus } #endif diff --git a/include/csp/csp_hooks.h b/include/csp/csp_hooks.h index 207c96345..6c2f9494e 100644 --- a/include/csp/csp_hooks.h +++ b/include/csp/csp_hooks.h @@ -97,7 +97,27 @@ void csp_clock_get_time(csp_timestamp_t * time); * @param time Structure containing the new time to set * @return 0 on success, -1 on failure */ -int csp_clock_set_time(const csp_timestamp_t * time, uint64_t rx_timestamp); +int csp_clock_set_time(const csp_timestamp_t * time); + +/** + * Set the system time with local time + * + * @param time Structure containing the new time to set + * @param local_rx_ns Local time when time occurred + * @return 0 on success, -1 on failure + */ +int csp_clock_set_time_w_local_time(const csp_timestamp_t * time, uint64_t local_rx_ns); + +/** + * Is called with local timestamp of when the packet was send. + * + * The packet has been freed when this function is called and the packet + * pointer can only be used for address comparison of the packet send. + * + * @param packet Pointer to the packet that was send + * @param tx_time_ns The local time the packet was send + */ +void csp_set_packet_tx_time(const void *packet, uint64_t tx_time_ns); #ifdef __cplusplus } diff --git a/include/csp/csp_interface.h b/include/csp/csp_interface.h index 48cc51f93..b8daac3b6 100644 --- a/include/csp/csp_interface.h +++ b/include/csp/csp_interface.h @@ -18,7 +18,7 @@ extern "C" { * * @return #CSP_ERR_NONE on success, otherwise an error code. */ -typedef int (*nexthop_t)(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me, uint64_t *timestamp); +typedef int (*nexthop_t)(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me); typedef int (*csp_alias_add_t)(void * driver_data, uint16_t addr); /** @@ -53,6 +53,7 @@ struct csp_iface_s { struct csp_iface_s * next; }; + /** * Used to represent an alias reception address, bound to a particular interface */ diff --git a/include/csp/csp_types.h b/include/csp/csp_types.h index b2215e144..2fc5944d4 100644 --- a/include/csp/csp_types.h +++ b/include/csp/csp_types.h @@ -33,7 +33,6 @@ typedef enum { CSP_REBOOT = 4, /*< Reboot, see #CSP_REBOOT_MAGIC and #CSP_REBOOT_SHUTDOWN_MAGIC */ CSP_BUF_FREE = 5, /*< Free CSP buffers */ CSP_UPTIME = 6, /*< Uptime */ - CSP_TIME_SYNC = 7, /*< Time synchronization */ } csp_service_port_t; /** Listen on all ports, primarily used with csp_bind() */ @@ -131,7 +130,7 @@ typedef struct csp_packet_s { uint16_t length; /*< Data length */ csp_id_t id; /*< CSP id (unpacked version CPU readable) */ - uint64_t timestamp; /*< Timestamp in ns for (the first received fragment of) the frame */ + uint64_t timestamp; /*< Timestamp in ns for (the last fragment of) the packet */ struct csp_packet_s * next; /*< Used for lists / queues of packets */ @@ -216,11 +215,17 @@ typedef csp_memptr64_t (*csp_memwrite64_fnc_t)(csp_memptr64_t, csp_memptr_t, siz * Time sync packet format */ typedef struct { - uint32_t id; + uint16_t id; +} csp_time_sync_t; + +/** + * Time sync correction packet format + */ +typedef struct { + uint16_t id; uint32_t tv_sec; uint32_t tv_nsec; - uint8_t correction; -} csp_time_sync_t; +} csp_time_sync_correction_t; #ifdef __cplusplus } diff --git a/include/csp/drivers/eth_linux.h b/include/csp/drivers/eth_linux.h index 3116e9336..e0233926a 100644 --- a/include/csp/drivers/eth_linux.h +++ b/include/csp/drivers/eth_linux.h @@ -36,7 +36,7 @@ int csp_eth_init(const char * device, const char * ifname, int mtu, unsigned int * @param[in] eth_frame The CSP ethernet frame to transmit. * @return #CSP_ERR_NONE on success, otherwise an error code. */ -int csp_eth_tx_frame(void * driver_data, csp_eth_header_t * eth_frame, uint64_t * timestamp); +int csp_eth_tx_frame(void * driver_data, csp_eth_header_t * eth_frame); /** * Posix ethernet RX thread diff --git a/include/csp/interfaces/csp_if_can.h b/include/csp/interfaces/csp_if_can.h index ddb2917c0..9262a1f59 100644 --- a/include/csp/interfaces/csp_if_can.h +++ b/include/csp/interfaces/csp_if_can.h @@ -188,12 +188,35 @@ extern "C" { */ typedef int (*csp_can_driver_tx_t)(void * driver_data, uint32_t id, const uint8_t * data, uint8_t dlc); +/** + * Send CAN frame (implemented by driver). + * + * Used by csp_can_tx() to send CAN frames. + * + * @param[in] driver_data driver data from #csp_iface_t + * @param[in] id CAM message id. + * @param[in] data CAN data + * @param[in] dlc data length of \a data. + * @param[in] context this pointer is used in csp_can_set_tx_time when packet has been send and timestamp is available + * @return #CSP_ERR_NONE on success, otherwise an error code. + */ +typedef int (*csp_can_driver_tx_w_context_t)(void * driver_data, uint32_t id, const uint8_t * data, uint8_t dlc, const void * context); + +/** + * Sets the local TX time when the packet indicated with context was send + * + * @param context Same context as used in csp_can_driver_tx_w_context_t + * @param tx_time_ns The local time the packet was send on CAN + */ +void csp_can_set_tx_time(const void * context, uint64_t tx_time_ns); + /** * Interface data (state information). */ typedef struct { atomic_int cfp_packet_counter; /**< CFP Identification number - same number on all fragments from same CSP packet. */ csp_can_driver_tx_t tx_func; /**< Tx function */ + csp_can_driver_tx_w_context_t tx_func_w_context; /**< Tx function, will only be used if tx_func is not set */ csp_packet_t * pbufs; /**< PBUF queue */ } csp_can_interface_data_t; diff --git a/include/csp/interfaces/csp_if_eth.h b/include/csp/interfaces/csp_if_eth.h index c0b39c975..33c452730 100644 --- a/include/csp/interfaces/csp_if_eth.h +++ b/include/csp/interfaces/csp_if_eth.h @@ -87,7 +87,7 @@ typedef struct csp_eth_header_s * @param[in] eth_frame CSP Ethernet frame to transmit * @return #CSP_ERR_NONE on success, otherwise an error code. */ -typedef int (*csp_eth_driver_tx_t)(void * driver_data, csp_eth_header_t * eth_frame, uint64_t * timestamp); +typedef int (*csp_eth_driver_tx_t)(void * driver_data, csp_eth_header_t * eth_frame); /** * CSP Interface data. @@ -134,7 +134,7 @@ void csp_eth_arp_get_addr(uint8_t * mac_addr, uint16_t csp_addr); * * @return #CSP_ERR_NONE on success, otherwise an error code. */ -int csp_eth_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me, uint64_t *timestamp); +int csp_eth_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me); /** * Process received CSP ethernet frame. @@ -149,7 +149,7 @@ int csp_eth_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int fro * @param[out] pxTaskWoken Valid reference if called from ISR, otherwise NULL! * @return #CSP_ERR_NONE on success, otherwise an error code. */ -int csp_eth_rx(csp_iface_t * iface, csp_eth_header_t * eth_frame, uint32_t received_len, int * task_woken, uint64_t timestamp); +int csp_eth_rx(csp_iface_t * iface, csp_eth_header_t * eth_frame, uint32_t received_len, int * task_woken); #ifdef __cplusplus } diff --git a/include/csp/interfaces/csp_if_i2c.h b/include/csp/interfaces/csp_if_i2c.h index a9faa3707..28909d234 100644 --- a/include/csp/interfaces/csp_if_i2c.h +++ b/include/csp/interfaces/csp_if_i2c.h @@ -54,7 +54,7 @@ int csp_i2c_add_interface(csp_iface_t * iface); * * @return #CSP_ERR_NONE on success, otherwise an error code. */ -int csp_i2c_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me, uint64_t *timestamp); +int csp_i2c_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me); /** * Process received I2C frame. diff --git a/include/csp/interfaces/csp_if_kiss.h b/include/csp/interfaces/csp_if_kiss.h index 67c4f7782..a40f6cd7b 100644 --- a/include/csp/interfaces/csp_if_kiss.h +++ b/include/csp/interfaces/csp_if_kiss.h @@ -62,7 +62,7 @@ int csp_kiss_add_interface(csp_iface_t * iface); * * @return #CSP_ERR_NONE on success, otherwise an error code. */ -int csp_kiss_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me, uint64_t *timestamp); +int csp_kiss_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me); /** * Process received CAN frame. diff --git a/src/arch/freertos/csp_clock.c b/src/arch/freertos/csp_clock.c index 11be803b7..8e21fe5ca 100644 --- a/src/arch/freertos/csp_clock.c +++ b/src/arch/freertos/csp_clock.c @@ -12,3 +12,14 @@ __weak int csp_clock_set_time(const csp_timestamp_t * time) { (void)time; /* Avoid compiler warnings about unused parameter */ return CSP_ERR_NOTSUP; } + +__weak int csp_clock_set_time_w_local_time(const csp_timestamp_t * time, uint64_t local_rx_ns) { + (void)time; + (void)local_rx_ns; + return CSP_ERR_NOTSUP; +} + +__weak void csp_set_packet_tx_time(const void *packet, uint64_t tx_time_ns) { + (void)packet; + (void)tx_time_ns; +} diff --git a/src/arch/posix/csp_clock.c b/src/arch/posix/csp_clock.c index 173fd859a..87afb5494 100644 --- a/src/arch/posix/csp_clock.c +++ b/src/arch/posix/csp_clock.c @@ -15,9 +15,7 @@ __weak void csp_clock_get_time(csp_timestamp_t * time) { } } -__weak int csp_clock_set_time(const csp_timestamp_t * time, uint64_t rx_timestamp) { - - (void)rx_timestamp; +__weak int csp_clock_set_time(const csp_timestamp_t * time) { struct timespec ts = {.tv_sec = time->tv_sec, .tv_nsec = time->tv_nsec}; if (clock_settime(CLOCK_REALTIME, &ts) == 0) { @@ -25,3 +23,16 @@ __weak int csp_clock_set_time(const csp_timestamp_t * time, uint64_t rx_timestam } return CSP_ERR_INVAL; // CSP doesn't have any matching error codes } + +__weak int csp_clock_set_time_w_local_time(const csp_timestamp_t * time, uint64_t local_rx_ns) { + + (void)time; + (void)local_rx_ns; + return CSP_ERR_NOTSUP; +} + +__weak void csp_set_packet_tx_time(const void *packet, uint64_t tx_time_ns) { + + (void)packet; + (void)tx_time_ns; +} diff --git a/src/arch/zephyr/csp_clock.c b/src/arch/zephyr/csp_clock.c index 060f8c201..0061b1d5f 100644 --- a/src/arch/zephyr/csp_clock.c +++ b/src/arch/zephyr/csp_clock.c @@ -38,3 +38,14 @@ __weak int csp_clock_set_time(const csp_timestamp_t * time) { return CSP_ERR_NONE; } + +__weak int csp_clock_set_time_w_local_time(const csp_timestamp_t * time, uint64_t local_rx_ns) { + (void)time; + (void)local_rx_ns; + return CSP_ERR_NOTSUP; +} + +__weak void csp_set_packet_tx_time(const void *packet, uint64_t tx_time_ns) { + (void)packet; + (void)tx_time_ns; +} diff --git a/src/csp_bridge.c b/src/csp_bridge.c index bbc725c39..644959944 100644 --- a/src/csp_bridge.c +++ b/src/csp_bridge.c @@ -64,6 +64,6 @@ void csp_bridge_work(void) { } /* Send to the interface directly, no hassle */ - csp_send_direct_iface(&packet->id, packet, destif, CSP_NO_VIA_ADDRESS, 0, NULL); + csp_send_direct_iface(&packet->id, packet, destif, CSP_NO_VIA_ADDRESS, 0); } diff --git a/src/csp_io.c b/src/csp_io.c index 388e4cda1..38c2b6911 100644 --- a/src/csp_io.c +++ b/src/csp_io.c @@ -121,18 +121,18 @@ static inline void send_packet(csp_id_t * idout_copy, csp_packet_t * snd_pkt, cs } if (snd_pkt != NULL) { - csp_send_direct_iface(idout_copy, snd_pkt, snd_iface, via, from_me, NULL); + csp_send_direct_iface(idout_copy, snd_pkt, snd_iface, via, from_me); } } -void csp_send_direct(csp_id_t* idout, csp_packet_t * packet, csp_iface_t * routed_from, uint64_t *tx_timestamp) { +void csp_send_direct(csp_id_t* idout, csp_packet_t * packet, csp_iface_t * routed_from) { int from_me = (routed_from == NULL ? 1 : 0); int via = CSP_NO_VIA_ADDRESS; /* Quickly send on loopback */ if (idout->dst == csp_if_lo.addr) { - csp_send_direct_iface(idout, packet, &csp_if_lo, via, from_me, tx_timestamp); + csp_send_direct_iface(idout, packet, &csp_if_lo, via, from_me); return; } @@ -235,7 +235,7 @@ __weak void csp_output_hook(const csp_id_t * idout, csp_packet_t * packet, csp_i return; } -void csp_send_direct_iface(csp_id_t* idout, csp_packet_t * packet, csp_iface_t * iface, uint16_t via, int from_me, uint64_t *tx_timestamp) { +void csp_send_direct_iface(const csp_id_t* idout, csp_packet_t * packet, csp_iface_t * iface, uint16_t via, int from_me) { csp_output_hook(idout, packet, iface, via, from_me); @@ -281,7 +281,7 @@ void csp_send_direct_iface(csp_id_t* idout, csp_packet_t * packet, csp_iface_t * /* Store length before passing to interface */ uint16_t bytes = packet->length; - if ((*iface->nexthop)(iface, via, packet, from_me, tx_timestamp) != CSP_ERR_NONE) + if ((*iface->nexthop)(iface, via, packet, from_me) != CSP_ERR_NONE) goto tx_err; iface->tx++; @@ -315,21 +315,8 @@ void csp_send(csp_conn_t * conn, csp_packet_t * packet) { } #endif - csp_send_direct(&conn->idout, packet, NULL, NULL); -} - -void csp_send_and_get_timestamp(csp_conn_t * conn, csp_packet_t * packet, uint64_t *tx_timestamp) { - - if (packet == NULL) { - return; - } + csp_send_direct(&conn->idout, packet, NULL); - if ((conn == NULL) || (conn->state != CONN_OPEN)) { - csp_buffer_free(packet); - return; - } - - csp_send_direct(&conn->idout, packet, NULL, tx_timestamp); } void csp_send_prio(uint8_t prio, csp_conn_t * conn, csp_packet_t * packet) { @@ -337,7 +324,7 @@ void csp_send_prio(uint8_t prio, csp_conn_t * conn, csp_packet_t * packet) { csp_send(conn, packet); } -int csp_transaction_persistent(csp_conn_t * conn, uint32_t timeout, const void * outbuf, int outlen, void * inbuf, int inlen, uint64_t *timestamp) { +int csp_transaction_persistent(csp_conn_t * conn, uint32_t timeout, const void * outbuf, int outlen, void * inbuf, int inlen) { if(outlen > CSP_BUFFER_SIZE){ return 0; @@ -368,10 +355,6 @@ int csp_transaction_persistent(csp_conn_t * conn, uint32_t timeout, const void * return 0; } - if (timestamp) { - *timestamp = packet->timestamp; - } - memcpy(inbuf, packet->data, packet->length); int length = packet->length; csp_buffer_free(packet); @@ -384,20 +367,7 @@ int csp_transaction_w_opts(uint8_t prio, uint16_t dest, uint8_t port, uint32_t t if (conn == NULL) return 0; - int status = csp_transaction_persistent(conn, timeout, outbuf, outlen, inbuf, inlen, NULL); - - csp_close(conn); - - return status; -} - -int csp_transaction_w_opts_timestamped(uint8_t prio, uint16_t dest, uint8_t port, uint32_t timeout, void * outbuf, int outlen, void * inbuf, int inlen, uint32_t opts, uint64_t *timestamp) { - - csp_conn_t * conn = csp_connect(prio, dest, port, 0, opts); - if (conn == NULL) - return 0; - - int status = csp_transaction_persistent(conn, timeout, outbuf, outlen, inbuf, inlen, timestamp); + int status = csp_transaction_persistent(conn, timeout, outbuf, outlen, inbuf, inlen); csp_close(conn); @@ -445,7 +415,7 @@ void csp_sendto(uint8_t prio, uint16_t dest, uint8_t dport, uint8_t src_port, ui packet->id.sport = src_port; packet->id.pri = prio; - csp_send_direct(&packet->id, packet, NULL, NULL); + csp_send_direct(&packet->id, packet, NULL); } diff --git a/src/csp_io.h b/src/csp_io.h index 1974be373..7e829ee4d 100644 --- a/src/csp_io.h +++ b/src/csp_io.h @@ -9,6 +9,5 @@ * @return #CSP_ERR_NONE on success, otherwise an error code. * */ - -void csp_send_direct(csp_id_t* idout, csp_packet_t * packet, csp_iface_t * routed_from, uint64_t *tx_timestamp); -void csp_send_direct_iface(csp_id_t* idout, csp_packet_t * packet, csp_iface_t * iface, uint16_t via, int from_me, uint64_t *tx_timestamp); +void csp_send_direct(csp_id_t* idout, csp_packet_t * packet, csp_iface_t * routed_from); +void csp_send_direct_iface(const csp_id_t* idout, csp_packet_t * packet, csp_iface_t * iface, uint16_t via, int from_me); diff --git a/src/csp_rdp.c b/src/csp_rdp.c index 1319e27b7..4eaf4ea84 100644 --- a/src/csp_rdp.c +++ b/src/csp_rdp.c @@ -162,7 +162,7 @@ static int csp_rdp_send_cmp(csp_conn_t * conn, csp_packet_t * packet, int flags, packet->length, (unsigned int)(packet->length - sizeof(rdp_header_t))); /* Send packet to IF */ - csp_send_direct(&idout, packet, NULL, NULL); + csp_send_direct(&idout, packet, NULL); return CSP_ERR_NONE; @@ -398,7 +398,7 @@ void csp_rdp_check_timeouts(csp_conn_t * conn) { /* Send copy to tx_queue */ packet->timestamp_tx = csp_get_ms(); csp_buffer_copy(packet, new_packet); - csp_send_direct(&conn->idout, new_packet, NULL, NULL); + csp_send_direct(&conn->idout, new_packet, NULL); } else { csp_rdp_error("RDP %p: Failed to allocate packet buffer\n", (void *)conn); } diff --git a/src/csp_route.c b/src/csp_route.c index d50e97d58..79cd23d24 100644 --- a/src/csp_route.c +++ b/src/csp_route.c @@ -164,7 +164,7 @@ int csp_route_work(void) { if (!is_to_me) { /* Otherwise, actually send the message */ - csp_send_direct(&packet->id, packet, input.iface, NULL); + csp_send_direct(&packet->id, packet, input.iface); return CSP_ERR_NONE; } diff --git a/src/csp_service_handler.c b/src/csp_service_handler.c index e4b97a2ac..b1ef6155b 100644 --- a/src/csp_service_handler.c +++ b/src/csp_service_handler.c @@ -11,7 +11,6 @@ #include #include -#include #include /** @@ -184,7 +183,7 @@ static int do_cmp_clock(struct csp_cmp_message * cmp) { int res = CSP_ERR_NONE; if (clock.tv_sec != 0) { // set time - res = csp_clock_set_time(&clock, 0); + res = csp_clock_set_time(&clock); if (res != CSP_ERR_NONE) { csp_dbg_errno = CSP_DBG_ERR_CLOCK_SET_FAIL; } @@ -198,6 +197,36 @@ static int do_cmp_clock(struct csp_cmp_message * cmp) { return res; } +#define CSP_TIME_SYNC_NUM_OF 2 +static uint16_t time_sync_last_id[CSP_TIME_SYNC_NUM_OF]; +static uint64_t time_sync_last_rx[CSP_TIME_SYNC_NUM_OF]; +static uint8_t time_sync_next_idx; + +static int do_cmp_time_sync(struct csp_cmp_message *cmp, uint64_t rx_timestamp) { + + uint16_t id = be16toh(cmp->time_sync.id); + + if (cmp->code == CSP_CMP_CLOCK_TIME_SYNC) { + /* Save sync info */ + time_sync_last_id[time_sync_next_idx] = id; + time_sync_last_rx[time_sync_next_idx] = rx_timestamp; + time_sync_next_idx = (time_sync_next_idx + 1) % CSP_TIME_SYNC_NUM_OF; + } else if (cmp->code == CSP_CMP_CLOCK_CORRECTION_TIME_SYNC) { + for (uint8_t idx = 0; idx < CSP_TIME_SYNC_NUM_OF; idx++) { + if (id == time_sync_last_id[idx] && time_sync_last_rx[idx] > 0) { + csp_timestamp_t now_ts; + now_ts.tv_sec = be32toh(cmp->time_sync_correction.tv_sec); + now_ts.tv_nsec = be32toh(cmp->time_sync_correction.tv_nsec); + + csp_clock_set_time_w_local_time(&now_ts, time_sync_last_rx[idx]); + break; + } + } + } + + return CSP_ERR_INVAL; // TODO: How to indicate that a response should not be send? +} + /* CSP Management Protocol handler */ static int csp_cmp_handler(csp_packet_t * packet) { @@ -249,6 +278,11 @@ static int csp_cmp_handler(csp_packet_t * packet) { ret = do_cmp_clock(cmp); break; + case CSP_CMP_CLOCK_TIME_SYNC: + case CSP_CMP_CLOCK_CORRECTION_TIME_SYNC: + ret = do_cmp_time_sync(cmp, packet->timestamp); + break; + default: ret = CSP_ERR_INVAL; break; @@ -271,11 +305,9 @@ void csp_service_handler(csp_packet_t * packet) { } break; - case CSP_PING: { + case CSP_PING: /* A ping means, just echo the packet, so no changes */ - // printf("Packet TS: %"PRIu64"\n", packet->timestamp); break; - } case CSP_PS: { packet->length = csp_ps_hook(packet); @@ -330,32 +362,6 @@ void csp_service_handler(csp_packet_t * packet) { break; } - case CSP_TIME_SYNC: { - static uint32_t last_sync_id = 0; - static uint64_t last_sync_rx = 0; - - /* Get time from packet */ - csp_time_sync_t time_sync; - memcpy(&time_sync, packet->data, sizeof(csp_time_sync_t)); - - uint32_t id = be32toh(time_sync.id); - - if (id == last_sync_id && last_sync_rx > 0 && time_sync.correction) { - csp_timestamp_t now_ts; - now_ts.tv_sec = be32toh(time_sync.tv_sec); - now_ts.tv_nsec = be32toh(time_sync.tv_nsec); - - csp_clock_set_time(&now_ts, last_sync_rx); - } else { - /* Save sync info */ - last_sync_id = id; - last_sync_rx = packet->timestamp; - } - - csp_buffer_free(packet); - break; - } - default: csp_buffer_free(packet); return; diff --git a/src/csp_services.c b/src/csp_services.c index ace5f98b3..da2a031df 100644 --- a/src/csp_services.c +++ b/src/csp_services.c @@ -8,7 +8,6 @@ #include #include #include -#include int csp_ping(uint16_t node, uint32_t timeout, unsigned int size, uint8_t conn_options) { @@ -100,58 +99,6 @@ void csp_shutdown(uint16_t node) { csp_transaction_w_opts(CSP_PRIO_NORM, node, CSP_REBOOT, 0, &magic_word, sizeof(magic_word), NULL, 0, CSP_O_CRC32); } -int csp_sync_time(uint16_t node) { - - /* Prepare data */ - csp_packet_t * packet = csp_buffer_get(0); - if (packet == NULL) - return -1; - - /* Open connection */ - csp_conn_t * conn = csp_connect(CSP_PRIO_NORM, node, CSP_TIME_SYNC, 0, CSP_O_CRC32); - if (conn == NULL) { - csp_buffer_free(packet); - return -1; - } - - csp_time_sync_t time_sync; - static uint32_t sync_id = 1; - time_sync.id = htobe32(sync_id++); - time_sync.tv_sec = 0; - time_sync.tv_nsec = 0; - time_sync.correction = 0; - packet->length = sizeof(time_sync); - memcpy(packet->data, &time_sync, sizeof(time_sync)); - - csp_timestamp_t fallback_ts; - csp_clock_get_time(&fallback_ts); - - uint64_t tx_timestamp = 0; - csp_send_and_get_timestamp(conn, packet, &tx_timestamp); - if (tx_timestamp == 0) { - /* No timestamp available */ - csp_print("Warning: No TX timestamp available when syncing time to node %u, using local clock as fallback\n", node); - tx_timestamp = ((uint64_t)fallback_ts.tv_sec * (uint64_t)1E9) + (uint64_t)fallback_ts.tv_nsec; - } - - packet = csp_buffer_get(0); - if (packet == NULL) - return -1; - - time_sync.tv_sec = htobe32(tx_timestamp / (uint64_t)1E9); - time_sync.tv_nsec = htobe32(tx_timestamp % (uint64_t)1E9); - time_sync.correction = 1; - - packet->length = sizeof(time_sync); - memcpy(packet->data, &time_sync, sizeof(time_sync)); - - csp_send(conn, packet); - - csp_close(conn); - - return CSP_ERR_NONE; -} - void csp_ps(uint16_t node, uint32_t timeout) { /* Open connection */ @@ -275,14 +222,3 @@ int csp_cmp(uint16_t node, uint32_t timeout, uint8_t code, int msg_size, struct return CSP_ERR_NONE; } - -int csp_cmp_clock_with_rx_timestamp(uint16_t node, uint32_t timeout, struct csp_cmp_message * msg, uint64_t *rx_timestamp) { - msg->type = CSP_CMP_REQUEST; - msg->code = CSP_CMP_CLOCK; - int status = csp_transaction_w_opts_timestamped(CSP_PRIO_NORM, node, CSP_CMP, timeout, msg, CMP_SIZE(clock), msg, CMP_SIZE(clock), CSP_O_CRC32, rx_timestamp); - if (status == 0) { - return CSP_ERR_TIMEDOUT; - } - - return CSP_ERR_NONE; -} diff --git a/src/drivers/can/can_zephyr.c b/src/drivers/can/can_zephyr.c index db5de1605..b1af30694 100644 --- a/src/drivers/can/can_zephyr.c +++ b/src/drivers/can/can_zephyr.c @@ -79,7 +79,7 @@ static void csp_can_rx_thread(void * arg1, void * arg2, void * arg3) { } /* Call the common CSP CAN RX function. */ - csp_can_rx(iface, frame.id, frame.data, frame.dlc, NULL); + csp_can_rx(iface, frame.id, frame.data, frame.dlc, NULL, 0); } } diff --git a/src/drivers/eth/eth_linux.c b/src/drivers/eth/eth_linux.c index 0a82c9277..fdbf17d6b 100644 --- a/src/drivers/eth/eth_linux.c +++ b/src/drivers/eth/eth_linux.c @@ -21,9 +21,6 @@ #include #include #include -#include -#include -#include #include #include #include @@ -41,7 +38,7 @@ typedef struct { struct ifreq if_idx; } eth_context_t; -int csp_eth_tx_frame(void * driver_data, csp_eth_header_t * eth_frame, uint64_t * timestamp) { +int csp_eth_tx_frame(void * driver_data, csp_eth_header_t * eth_frame) { const eth_context_t * ctx = (eth_context_t*)driver_data; @@ -53,69 +50,10 @@ int csp_eth_tx_frame(void * driver_data, csp_eth_header_t * eth_frame, uint64_t uint32_t txsize = sizeof(csp_eth_header_t) + be16toh(eth_frame->seg_size); - /* Setup sendmsg for TX timestamping */ - struct iovec iov = { - .iov_base = eth_frame, - .iov_len = txsize - }; - - struct msghdr msg = { - .msg_name = &socket_address, - .msg_namelen = sizeof(struct sockaddr_ll), - .msg_iov = &iov, - .msg_iovlen = 1, - .msg_control = NULL, - .msg_controllen = 0, - .msg_flags = 0 - }; - - if (sendmsg(ctx->sockfd, &msg, 0) < 0) { + if (sendto(ctx->sockfd, (void*)eth_frame, txsize, 0, (struct sockaddr*)&socket_address, sizeof(struct sockaddr_ll)) < 0) { return CSP_ERR_DRIVER; } - /* Retrieve TX timestamp from error queue */ - char control[CMSG_SPACE(sizeof(struct scm_timestamping))]; - char data[CSP_ETH_BUF_SIZE]; - struct iovec iov_err = { - .iov_base = data, - .iov_len = sizeof(data) - }; - - struct msghdr msg_err = { - .msg_name = NULL, - .msg_namelen = 0, - .msg_iov = &iov_err, - .msg_iovlen = 1, - .msg_control = control, - .msg_controllen = sizeof(control), - .msg_flags = 0 - }; - - /* Try to get TX timestamp (non-blocking) */ - int err_result = recvmsg(ctx->sockfd, &msg_err, MSG_ERRQUEUE | MSG_DONTWAIT); - if (err_result >= 0) { - for (struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg_err); cmsg != NULL; cmsg = CMSG_NXTHDR(&msg_err, cmsg)) { - if (eth_debug) { - csp_print("TX cmsg: level=%d type=%d\n", cmsg->cmsg_level, cmsg->cmsg_type); - } - if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SO_TIMESTAMPING) { - struct scm_timestamping *ts_data = (struct scm_timestamping *)CMSG_DATA(cmsg); - uint64_t tx_timestamp = 0; - /* Try hardware timestamp first (ts[2]), then software (ts[0]) */ - tx_timestamp = (uint64_t)ts_data->ts[0].tv_sec * 1000000000ULL + (uint64_t)ts_data->ts[0].tv_nsec; - if (eth_debug) { - csp_print("TX SW timestamp: %llu ns\n", tx_timestamp); - } - if (timestamp && tx_timestamp) { - *timestamp = tx_timestamp; - } - break; - } - } - } else if (eth_debug) { - csp_print("TX: No timestamp from error queue (errno=%d)\n", errno); - } - return CSP_ERR_NONE; } @@ -126,59 +64,13 @@ void * csp_eth_rx_loop(void * param) { static uint8_t recvbuf[CSP_ETH_BUF_SIZE]; csp_eth_header_t * eth_frame = (csp_eth_header_t *)recvbuf; - uint64_t timestamp = 0; - - /* Setup for recvmsg to receive timestamp */ - struct iovec iov = { - .iov_base = recvbuf, - .iov_len = CSP_ETH_BUF_SIZE - }; - - char control[CMSG_SPACE(sizeof(struct scm_timestamping))]; - - struct msghdr msg = { - .msg_name = NULL, - .msg_namelen = 0, - .msg_iov = &iov, - .msg_iovlen = 1, - .msg_control = control, - .msg_controllen = sizeof(control), - .msg_flags = 0 - }; - while(1) { - /* Receive packet segment with timestamp */ - int32_t received_len = recvmsg(ctx->sockfd, &msg, 0); - - if (received_len < 0) { - continue; - } + /* Receive packet segment */ - /* Extract timestamp from control message */ - timestamp = 0; - for (struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; cmsg = CMSG_NXTHDR(&msg, cmsg)) { - if (eth_debug) { - csp_print("RX cmsg: level=%d type=%d\n", cmsg->cmsg_level, cmsg->cmsg_type); - } - if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SO_TIMESTAMPING) { - struct scm_timestamping *ts_data = (struct scm_timestamping *)CMSG_DATA(cmsg); - /* Try hardware timestamp first (ts[2]), then software (ts[0]) */ - if (ts_data->ts[0].tv_sec || ts_data->ts[0].tv_nsec) { - timestamp = (uint64_t)ts_data->ts[0].tv_sec * 1000000000ULL + (uint64_t)ts_data->ts[0].tv_nsec; - if (eth_debug) { - csp_print("RX SW timestamp: %llu ns\n", timestamp); - } - } - break; - } - } - - if (eth_debug && timestamp == 0) { - csp_print("RX: No timestamp received\n"); - } + uint32_t received_len = recvfrom(ctx->sockfd, recvbuf, CSP_ETH_BUF_SIZE, 0, NULL, NULL); - csp_eth_rx(&ctx->ifdata.iface, eth_frame, received_len, NULL, timestamp); + csp_eth_rx(&ctx->ifdata.iface, eth_frame, received_len, NULL); } return NULL; @@ -265,14 +157,6 @@ int csp_eth_init(const char * device, const char * ifname, int mtu, unsigned int return CSP_ERR_INVAL; } - // Enable TX timestamping - int ts_flags = SOF_TIMESTAMPING_TX_SOFTWARE | SOF_TIMESTAMPING_RX_SOFTWARE | SOF_TIMESTAMPING_SOFTWARE; - if (setsockopt(ctx->sockfd, SOL_SOCKET, SO_TIMESTAMPING, &ts_flags, sizeof(ts_flags)) == -1) { - perror("setsockopt SO_TIMESTAMPING"); - } else { - csp_print("Using hardware timestamping on %s\n", device); - } - /* Bind to device */ if (setsockopt(ctx->sockfd, SOL_SOCKET, SO_BINDTODEVICE, device, IFNAMSIZ-1) == -1) { perror("SO_BINDTODEVICE"); diff --git a/src/interfaces/csp_if_can.c b/src/interfaces/csp_if_can.c index be592599d..13e045bcc 100644 --- a/src/interfaces/csp_if_can.c +++ b/src/interfaces/csp_if_can.c @@ -7,7 +7,9 @@ #include #include +#include "csp/csp_types.h" #include "csp_if_can_pbuf.h" +#include /** * TESTING: @@ -163,9 +165,8 @@ static int csp_can1_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u return CSP_ERR_NONE; } -static int csp_can1_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me, uint64_t *timestamp) { +static int csp_can1_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me) { (void)from_me; /* Avoid compiler warnings about unused parameter */ - (void)timestamp; /* Avoid compiler warnings about unused parameter */ /* Loopback */ if (packet->id.dst == iface->addr) { @@ -373,11 +374,10 @@ static int csp_can2_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u return CSP_ERR_NONE; } -static int csp_can2_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me, uint64_t *timestamp) { +static int csp_can2_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me) { /* Avoid compiler warnings about unused parameter */ (void)via; (void)from_me; - (void)timestamp; /* Loopback */ if (packet->id.dst == iface->addr || csp_addr_is_alias(packet->id.dst)) { @@ -393,6 +393,7 @@ static int csp_can2_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, uint32_t can_id = 0; uint8_t frame_buf_inp = 0; + const void *context = NULL; /* Pack mandatory fields of header */ can_id = (((packet->id.pri & CFP2_PRIO_MASK) << CFP2_PRIO_OFFSET) | @@ -425,10 +426,17 @@ static int csp_can2_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, /* Check for end condition */ if (tx_count == packet->length) { can_id |= ((1 & CFP2_END_MASK) << CFP2_END_OFFSET); + context = packet; } /* Send first frame now */ - if ((ifdata->tx_func)(iface->driver_data, can_id, frame_buf, frame_buf_inp) != CSP_ERR_NONE) { + int result; + if (ifdata->tx_func) + result = ifdata->tx_func(iface->driver_data, can_id, frame_buf, frame_buf_inp); + else + result = ifdata->tx_func_w_context(iface->driver_data, can_id, frame_buf, frame_buf_inp, context); + + if (result != CSP_ERR_NONE) { iface->tx_error++; /* Does not free on return */ return CSP_ERR_DRIVER; @@ -453,10 +461,15 @@ static int csp_can2_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, /* Check for end condition */ if (tx_count + data_bytes == packet->length) { can_id |= ((1 & CFP2_END_MASK) << CFP2_END_OFFSET); + context = packet; } - /* Send frame */ - if ((ifdata->tx_func)(iface->driver_data, can_id, packet->data + tx_count, data_bytes) != CSP_ERR_NONE) { + if (ifdata->tx_func) + result = ifdata->tx_func(iface->driver_data, can_id, packet->data + tx_count, data_bytes); + else + result = ifdata->tx_func_w_context(iface->driver_data, can_id, packet->data + tx_count, data_bytes, context); + + if (result != CSP_ERR_NONE) { iface->tx_error++; /* Does not free on return */ return CSP_ERR_DRIVER; @@ -471,6 +484,14 @@ static int csp_can2_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, return CSP_ERR_NONE; } +void csp_can_set_tx_time(const void *context, uint64_t tx_time_ns) { + + if (context == NULL) + return; + + csp_set_packet_tx_time(context, tx_time_ns); +} + int csp_can_add_interface(csp_iface_t * iface) { if ((iface == NULL) || (iface->name == NULL) || (iface->interface_data == NULL)) { @@ -478,7 +499,7 @@ int csp_can_add_interface(csp_iface_t * iface) { } csp_can_interface_data_t * ifdata = iface->interface_data; - if (ifdata->tx_func == NULL) { + if (ifdata->tx_func == NULL && ifdata->tx_func_w_context == NULL) { return CSP_ERR_INVAL; } diff --git a/src/interfaces/csp_if_eth.c b/src/interfaces/csp_if_eth.c index 6ce61bd4e..449e1c9da 100644 --- a/src/interfaces/csp_if_eth.c +++ b/src/interfaces/csp_if_eth.c @@ -14,7 +14,6 @@ #include #include #include -#include /** @@ -134,7 +133,7 @@ void csp_eth_arp_get_addr(uint8_t * mac_addr, uint16_t csp_addr) memset(mac_addr, 0xff, CSP_ETH_ALEN); } -int csp_eth_rx(csp_iface_t * iface, csp_eth_header_t * eth_frame, uint32_t received_len, int * task_woken, uint64_t timestamp) { +int csp_eth_rx(csp_iface_t * iface, csp_eth_header_t * eth_frame, uint32_t received_len, int * task_woken) { csp_eth_interface_data_t * ifdata = iface->interface_data; @@ -234,14 +233,13 @@ int csp_eth_rx(csp_iface_t * iface, csp_eth_header_t * eth_frame, uint32_t recei return CSP_ERR_NONE; } - packet->timestamp = timestamp; - csp_qfifo_write(packet, iface, task_woken); return CSP_ERR_NONE; } -int csp_eth_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me, uint64_t *timestamp) { +int csp_eth_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me) { + /* Avoid compiler warnings about unused parameter */ (void)via; (void)from_me; @@ -258,7 +256,7 @@ int csp_eth_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int fro static atomic_int packet_id = 0; packet_id++; uint16_t offset = 0; - + while (offset < packet->frame_length) { csp_eth_header_t *eth_frame = ifdata->tx_buf; @@ -277,7 +275,7 @@ int csp_eth_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int fro memcpy(eth_frame->frame_begin, packet->frame_begin + offset, seg_size); - if ((ifdata->tx_func)(iface->driver_data, eth_frame, timestamp) != CSP_ERR_NONE) { + if ((ifdata->tx_func)(iface->driver_data, eth_frame) != CSP_ERR_NONE) { iface->tx_error++; /* Does not free on return */ return CSP_ERR_DRIVER; diff --git a/src/interfaces/csp_if_i2c.c b/src/interfaces/csp_if_i2c.c index 9b3ecb893..68e740e58 100644 --- a/src/interfaces/csp_if_i2c.c +++ b/src/interfaces/csp_if_i2c.c @@ -4,9 +4,8 @@ #include #include -int csp_i2c_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me, uint64_t *timestamp) { - (void)from_me; - (void)timestamp; +int csp_i2c_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me) { + (void)from_me; /* Avoid compiler warnings about unused parameter */ /* Loopback */ if (packet->id.dst == iface->addr) { diff --git a/src/interfaces/csp_if_kiss.c b/src/interfaces/csp_if_kiss.c index 2bb78cfa1..5b9dc33b7 100644 --- a/src/interfaces/csp_if_kiss.c +++ b/src/interfaces/csp_if_kiss.c @@ -22,11 +22,10 @@ #define TFESC 0xDD #define TNC_DATA 0x00 -int csp_kiss_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me, uint64_t *timestamp) { +int csp_kiss_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me) { /* Avoid compiler warnings about unused parameter */ (void)via; (void)from_me; - (void)timestamp; csp_kiss_interface_data_t * ifdata = iface->interface_data; void * driver = iface->driver_data; diff --git a/src/interfaces/csp_if_lo.c b/src/interfaces/csp_if_lo.c index d45c1dea1..f4f17cfba 100644 --- a/src/interfaces/csp_if_lo.c +++ b/src/interfaces/csp_if_lo.c @@ -8,12 +8,11 @@ * @param packet Packet to transmit * @return 1 if packet was successfully transmitted, 0 on error */ -static int csp_lo_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me, uint64_t *timestamp) { +static int csp_lo_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me) { /* Avoid compiler warnings about unused parameter */ (void)iface; (void)via; (void)from_me; - (void)timestamp; /* Send back into CSP, notice calling from task so last argument must be NULL! */ csp_qfifo_write(packet, &csp_if_lo, NULL); diff --git a/src/interfaces/csp_if_tun.c b/src/interfaces/csp_if_tun.c index 1aab02d1f..43f5a82ca 100644 --- a/src/interfaces/csp_if_tun.c +++ b/src/interfaces/csp_if_tun.c @@ -22,11 +22,10 @@ __weak int csp_crypto_encrypt(uint8_t * msg_begin, uint8_t msg_len, uint8_t * ci return -1; } -static int csp_if_tun_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me, uint64_t *timestamp) { +static int csp_if_tun_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me) { /* Avoid compiler warnings about unused parameter */ (void)via; (void)from_me; - (void)timestamp; csp_if_tun_conf_t * ifconf = iface->driver_data; diff --git a/src/interfaces/csp_if_udp.c b/src/interfaces/csp_if_udp.c index 21eadae2a..ec88b167f 100644 --- a/src/interfaces/csp_if_udp.c +++ b/src/interfaces/csp_if_udp.c @@ -17,11 +17,10 @@ #define MSG_CONFIRM (0) #endif -static int csp_if_udp_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me, uint64_t *timestamp) { +static int csp_if_udp_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet, int from_me) { /* Avoid compiler warnings about unused parameter */ (void)via; (void)from_me; - (void)timestamp; csp_if_udp_conf_t * ifconf = iface->driver_data; diff --git a/src/interfaces/csp_if_zmqhub.c b/src/interfaces/csp_if_zmqhub.c index 221da397d..061f72032 100644 --- a/src/interfaces/csp_if_zmqhub.c +++ b/src/interfaces/csp_if_zmqhub.c @@ -81,8 +81,7 @@ void * csp_zmqhub_fixup_cspv1_del_dest_addr(uint8_t * rx_data, size_t * datalen) * @param packet Packet to transmit * @return 1 if packet was successfully transmitted, 0 on error */ -static int csp_zmqhub_tx(csp_iface_t * iface, uint16_t __maybe_unused via, csp_packet_t * packet, int __maybe_unused from_me, uint64_t *timestamp) { - (void) timestamp; +static int csp_zmqhub_tx(csp_iface_t * iface, uint16_t __maybe_unused via, csp_packet_t * packet, int __maybe_unused from_me) { zmq_driver_t * drv = iface->driver_data; From c1c2a0fe000848054a8f3191ec9f2bbf098deec5 Mon Sep 17 00:00:00 2001 From: Henrik Davidsen Date: Mon, 4 May 2026 09:15:28 +0200 Subject: [PATCH 7/7] Add new function csp_can_rx_w_timestamp Removed timestamp from csp_can_rx and added a new function csp_can_rx_w_timestamp instead --- contrib/drivers/can/csp_driver_can.c | 4 ++-- doc/basic.md | 2 +- include/csp/csp_types.h | 3 +-- include/csp/interfaces/csp_if_can.h | 21 +++++++++++++++++++-- src/csp_service_handler.c | 2 +- src/drivers/can/can_socketcan.c | 2 +- src/drivers/can/can_zephyr.c | 2 +- src/interfaces/csp_if_can.c | 10 ++++++++-- 8 files changed, 34 insertions(+), 12 deletions(-) diff --git a/contrib/drivers/can/csp_driver_can.c b/contrib/drivers/can/csp_driver_can.c index 07c439426..9e9f4e106 100644 --- a/contrib/drivers/can/csp_driver_can.c +++ b/contrib/drivers/can/csp_driver_can.c @@ -83,7 +83,7 @@ void CAN_0_rx_callback(struct can_async_descriptor *const descr) { /* Process frame within ISR * (This can also be deferred to a task with: csp_can_process_frame_deferred) */ - csp_can_rx(&mcan[0].interface, msg.id, msg.data, msg.len, &xTaskWoken, 0); + csp_can_rx(&mcan[0].interface, msg.id, msg.data, msg.len, &xTaskWoken); } @@ -116,7 +116,7 @@ static void can_task(void * param) { /* Process frame within ISR * (This can also be deferred to a task with: csp_can_process_frame_deferred) */ - csp_can_rx(&mcan[0].interface, msg.id, msg.data, msg.len, &xTaskWoken, 0); + csp_can_rx(&mcan[0].interface, msg.id, msg.data, msg.len, &xTaskWoken); } diff --git a/doc/basic.md b/doc/basic.md index 541212510..3c104c213 100644 --- a/doc/basic.md +++ b/doc/basic.md @@ -67,7 +67,7 @@ Definition of a buffer element `csp_packet_t`: */ typedef struct { uint32_t timestamp_tx; // Time the message was sent - uint32_t timestamp_rx; // Time the message was received + uint64_t timestamp_rx; // Time in ns the message was received (the last fragment) uint16_t length; // Data length csp_id_t id; // CSP id (unpacked version CPU readable) diff --git a/include/csp/csp_types.h b/include/csp/csp_types.h index 2fc5944d4..cf24fc1a2 100644 --- a/include/csp/csp_types.h +++ b/include/csp/csp_types.h @@ -118,7 +118,7 @@ typedef struct { typedef struct csp_packet_s { uint32_t timestamp_tx; /*< Time the message was sent */ - uint32_t timestamp_rx; /*< Time the message was received */ + uint64_t timestamp_rx; /*< Time in ns the message was received (the last fragment) */ struct csp_conn_s * conn; /*< Associated connection (this is used in RDP queue) */ uint16_t rx_count; /*< Received bytes */ @@ -130,7 +130,6 @@ typedef struct csp_packet_s { uint16_t length; /*< Data length */ csp_id_t id; /*< CSP id (unpacked version CPU readable) */ - uint64_t timestamp; /*< Timestamp in ns for (the last fragment of) the packet */ struct csp_packet_s * next; /*< Used for lists / queues of packets */ diff --git a/include/csp/interfaces/csp_if_can.h b/include/csp/interfaces/csp_if_can.h index 9262a1f59..e14047f14 100644 --- a/include/csp/interfaces/csp_if_can.h +++ b/include/csp/interfaces/csp_if_can.h @@ -216,8 +216,8 @@ void csp_can_set_tx_time(const void * context, uint64_t tx_time_ns); typedef struct { atomic_int cfp_packet_counter; /**< CFP Identification number - same number on all fragments from same CSP packet. */ csp_can_driver_tx_t tx_func; /**< Tx function */ - csp_can_driver_tx_w_context_t tx_func_w_context; /**< Tx function, will only be used if tx_func is not set */ csp_packet_t * pbufs; /**< PBUF queue */ + csp_can_driver_tx_w_context_t tx_func_w_context; /**< Tx function, will only be used if tx_func is not set */ } csp_can_interface_data_t; /** @@ -265,7 +265,24 @@ int csp_can_tx(csp_iface_t * iface, uint16_t via, csp_packet_t *packet); * @param[out] pxTaskWoken Valid reference if called from ISR, otherwise NULL! * @return #CSP_ERR_NONE on success, otherwise an error code. */ -int csp_can_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, uint8_t dlc, int *pxTaskWoken, uint64_t timestamp); +int csp_can_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, uint8_t dlc, int *pxTaskWoken); + +/** + * Process received CAN frame. + * + * Called from driver when a single CAN frame (up to 8 bytes) has been received. + * The function will gather the fragments into a single + * CSP packet and route it on when complete. + * + * @param[in] iface incoming interface. + * @param[in] id received CAN message identifier. + * @param[in] data received CAN data. + * @param[in] dlc length of received \a data. + * @param[in] timestamp local timestamp when frame was received. Only set for End frames + * @param[out] pxTaskWoken Valid reference if called from ISR, otherwise NULL! + * @return #CSP_ERR_NONE on success, otherwise an error code. + */ +int csp_can_rx_w_timestamp(csp_iface_t * iface, uint32_t id, const uint8_t * data, uint8_t dlc, uint64_t timestamp, int *pxTaskWoken); #ifdef __cplusplus } diff --git a/src/csp_service_handler.c b/src/csp_service_handler.c index b1ef6155b..9ffbe2194 100644 --- a/src/csp_service_handler.c +++ b/src/csp_service_handler.c @@ -280,7 +280,7 @@ static int csp_cmp_handler(csp_packet_t * packet) { case CSP_CMP_CLOCK_TIME_SYNC: case CSP_CMP_CLOCK_CORRECTION_TIME_SYNC: - ret = do_cmp_time_sync(cmp, packet->timestamp); + ret = do_cmp_time_sync(cmp, packet->timestamp_rx); break; default: diff --git a/src/drivers/can/can_socketcan.c b/src/drivers/can/can_socketcan.c index dd535260e..56b228e7a 100644 --- a/src/drivers/can/can_socketcan.c +++ b/src/drivers/can/can_socketcan.c @@ -99,7 +99,7 @@ static void * socketcan_rx_thread(void * arg) { frame.can_id &= CAN_EFF_MASK; /* Call RX callbacsp_can_rx_frameck */ - csp_can_rx(&ctx->iface, frame.can_id, frame.data, frame.can_dlc, NULL, 0); + csp_can_rx(&ctx->iface, frame.can_id, frame.data, frame.can_dlc, NULL); } /* We should never reach this point */ diff --git a/src/drivers/can/can_zephyr.c b/src/drivers/can/can_zephyr.c index b1af30694..db5de1605 100644 --- a/src/drivers/can/can_zephyr.c +++ b/src/drivers/can/can_zephyr.c @@ -79,7 +79,7 @@ static void csp_can_rx_thread(void * arg1, void * arg2, void * arg3) { } /* Call the common CSP CAN RX function. */ - csp_can_rx(iface, frame.id, frame.data, frame.dlc, NULL, 0); + csp_can_rx(iface, frame.id, frame.data, frame.dlc, NULL); } } diff --git a/src/interfaces/csp_if_can.c b/src/interfaces/csp_if_can.c index 13e045bcc..74abe4d97 100644 --- a/src/interfaces/csp_if_can.c +++ b/src/interfaces/csp_if_can.c @@ -353,7 +353,7 @@ static int csp_can2_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u /* END */ if (id & (CFP2_END_MASK << CFP2_END_OFFSET)) { - packet->timestamp = can_timestamp; + packet->timestamp_rx = can_timestamp; /* Extract data length */ packet->length = packet->frame_length - csp_id_get_header_size(); @@ -527,10 +527,16 @@ int csp_can_remove_interface(csp_iface_t * iface) { return CSP_ERR_NONE; } -int csp_can_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, uint8_t dlc, int * task_woken, uint64_t timestamp) { +int csp_can_rx_w_timestamp(csp_iface_t * iface, uint32_t id, const uint8_t * data, uint8_t dlc, uint64_t timestamp, int * task_woken) { + if (csp_conf.version == 1) { return csp_can1_rx(iface, id, data, dlc, task_woken); } else { return csp_can2_rx(iface, id, data, dlc, task_woken, timestamp); } } + +int csp_can_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, uint8_t dlc, int * task_woken) { + + return csp_can_rx_w_timestamp(iface, id, data, dlc, 0, task_woken); +}