Add Time Sync

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;
+}

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)

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/<os>` and all have default implementations.

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__));
 

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

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

include/csp/interfaces/csp_if_can.h

@@ -188,13 +188,36 @@ 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_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

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;
+}

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;
+}

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;
+}

src/csp_service_handler.c

@@ -11,6 +11,8 @@
 #include <csp/csp_id.h>
 #include <csp/arch/csp_time.h>
 
+#include <stdlib.h>
+
 /**
  * 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;

src/csp_services.c

@@ -6,6 +6,7 @@
 
 #include <csp/csp_cmp.h>
 #include <endian.h>
+#include <string.h>
 #include <csp/arch/csp_time.h>
 
 int csp_ping(uint16_t node, uint32_t timeout, unsigned int size, uint8_t conn_options) {