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/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/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_cmp.h b/include/csp/csp_cmp.h index 91d4b0188..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__)); diff --git a/include/csp/csp_hooks.h b/include/csp/csp_hooks.h index 12af6afab..6c2f9494e 100644 --- a/include/csp/csp_hooks.h +++ b/include/csp/csp_hooks.h @@ -99,6 +99,26 @@ void csp_clock_get_time(csp_timestamp_t * time); */ 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 } #endif diff --git a/include/csp/csp_types.h b/include/csp/csp_types.h index 578a4f01c..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 */ @@ -210,6 +210,22 @@ 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 { + 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; +} csp_time_sync_correction_t; + #ifdef __cplusplus } #endif diff --git a/include/csp/interfaces/csp_if_can.h b/include/csp/interfaces/csp_if_can.h index 106515539..e14047f14 100644 --- a/include/csp/interfaces/csp_if_can.h +++ b/include/csp/interfaces/csp_if_can.h @@ -188,6 +188,28 @@ 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). */ @@ -195,6 +217,7 @@ 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_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; /** @@ -244,6 +267,23 @@ int csp_can_tx(csp_iface_t * iface, uint16_t via, csp_packet_t *packet); */ 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 } #endif 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 94c8dc8e6..87afb5494 100644 --- a/src/arch/posix/csp_clock.c +++ b/src/arch/posix/csp_clock.c @@ -23,3 +23,16 @@ __weak int csp_clock_set_time(const csp_timestamp_t * time) { } 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_service_handler.c b/src/csp_service_handler.c index 1947d5631..9ffbe2194 100644 --- a/src/csp_service_handler.c +++ b/src/csp_service_handler.c @@ -11,6 +11,8 @@ #include #include +#include + /** * The CSP CMP mempy function is used to, override the function used to * read/write memory by peek and poke. @@ -195,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) { @@ -246,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_rx); + break; + default: ret = CSP_ERR_INVAL; break; diff --git a/src/csp_services.c b/src/csp_services.c index c1d191c3a..da2a031df 100644 --- a/src/csp_services.c +++ b/src/csp_services.c @@ -6,6 +6,7 @@ #include #include +#include #include int csp_ping(uint16_t node, uint32_t timeout, unsigned int size, uint8_t conn_options) { diff --git a/src/interfaces/csp_if_can.c b/src/interfaces/csp_if_can.c index 6e5649969..74abe4d97 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: @@ -260,7 +262,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 +353,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_rx = can_timestamp; + /* Extract data length */ packet->length = packet->frame_length - csp_id_get_header_size(); @@ -389,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) | @@ -421,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; @@ -449,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; @@ -467,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)) { @@ -474,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; } @@ -502,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) { +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); + 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); +}