+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <time.h>
+#include <math.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <net/if.h>
+#include <arpa/inet.h>
+#include <linux/can.h>
+#include <pthread.h>
+
+/**
+ * ToDo:
+ * [ ] consider can timestamp
+ */
+
+struct optdata
+{
+ int n;
+};
+
+struct optdata d;
+pthread_barrier_t barrier;
+struct timespec* tx_time, * rx_time;
+
+void* udp_recv( void* arg )
+{
+ struct sockaddr_in sa_udp;
+ int udp_sock;
+ struct can_frame cf;
+ int i;
+
+ sa_udp.sin_family = AF_INET;
+ sa_udp.sin_port = htons( 10502 );
+ inet_aton( "127.0.0.1", &sa_udp.sin_addr );
+
+ udp_sock = socket( PF_INET, SOCK_DGRAM, IPPROTO_UDP );
+ if( udp_sock < 0 )
+ {
+ fprintf( stderr, "error: udp socket creation failed\n" );
+ return NULL;
+ }
+
+ if( bind(udp_sock, (struct sockaddr*)&sa_udp, sizeof(sa_udp)) != 0 )
+ {
+ fprintf( stderr, "error: udp socket binding failed\n" );
+ return NULL;
+ }
+
+ pthread_barrier_wait( &barrier );
+
+ /* recv */
+ for( i=0; i<d.n; i++ )
+ {
+ recvfrom( udp_sock, &cf, sizeof(cf), 0, NULL, 0 );
+ clock_gettime( CLOCK_REALTIME, &rx_time[cf.data[0]] );
+ printf( "recv: (id=%d)%u\n", cf.can_id, cf.data[0] );
+ }
+
+ close( udp_sock );
+ return NULL;
+}
+
+int timespec_subtract( struct timespec *result, struct timespec *x, struct timespec *yy )
+{
+ struct timespec ylocal = *yy, *y = &ylocal;
+ /* Perform the carry for the later subtraction by updating Y. */
+ if( x->tv_nsec < y->tv_nsec )
+ {
+ int nsec = (y->tv_nsec - x->tv_nsec) / 1000000000 + 1;
+ y->tv_nsec -= 1000000000 * nsec;
+ y->tv_sec += nsec;
+ }
+ if (x->tv_nsec - y->tv_nsec > 1000000000) {
+ int nsec = (x->tv_nsec - y->tv_nsec) / 1000000000;
+ y->tv_nsec += 1000000000 * nsec;
+ y->tv_sec -= nsec;
+ }
+
+ /* Compute the time remaining to wait.
+ * `tv_nsec' is certainly positive. */
+ result->tv_sec = x->tv_sec - y->tv_sec;
+ result->tv_nsec = x->tv_nsec - y->tv_nsec;
+ /* Return 1 if result is negative. */
+ return x->tv_sec < y->tv_sec;
+}
+
+int main( int argc, char* argv[] )
+{
+ char opt;
+ struct sockaddr_can sa_can;
+ struct can_frame cf;
+ int can_sock;
+ pthread_t thr;
+ float mean = 0;
+ float time;
+ struct timespec res;
+ int i;
+
+ while( 1 )
+ {
+ opt = getopt( argc, argv, "n:" );
+ if( opt == -1 )
+ break;
+ switch( opt )
+ {
+ case 'n':
+ d.n = atoi( optarg );
+ break;
+ case '?':
+ return -1;
+ break;
+ }
+ }
+
+ tx_time = malloc( d.n*sizeof(struct timespec) );
+ rx_time = malloc( d.n*sizeof(struct timespec) );
+
+ pthread_barrier_init( &barrier, NULL, 2 );
+ pthread_create( &thr, NULL, udp_recv, NULL );
+
+ sa_can.can_family = AF_CAN;
+ sa_can.can_ifindex = if_nametoindex( "vcan0" );
+
+ can_sock = socket( PF_CAN, SOCK_RAW, CAN_RAW );
+ if( can_sock < 0 )
+ {
+ fprintf( stderr, "error: can socket creation failed\n" );
+ return -1;
+ }
+
+ if( bind(can_sock, (struct sockaddr*)&sa_can, sizeof(sa_can)) != 0 )
+ {
+ fprintf( stderr, "error: can socket binding failed\n" );
+ return -1;
+ }
+
+ pthread_barrier_wait( &barrier );
+
+ /* send */
+ for( i=0; i<d.n; i++ )
+ {
+ //memset( &cf, 0, sizeof(cf) );
+ cf.can_id = i;
+ cf.can_dlc = 8;
+ cf.data[0] = i;
+ int ret = sendto( can_sock, &cf, sizeof(cf), 0, (struct sockaddr*)&sa_can, sizeof(sa_can) );
+ clock_gettime( CLOCK_REALTIME, &tx_time[cf.data[0]] );
+ printf( "sending(%d)\n", ret );
+ }
+
+ close( can_sock ); /* shutdown? */
+ pthread_join( thr, NULL );
+
+ /* results */
+ for( i=0; i<d.n; i++ )
+ {
+ if( timespec_subtract( &res, &rx_time[i], &tx_time[i] ) == 0 )
+ {
+ time = res.tv_sec*1000000000.0f + (float)res.tv_nsec;
+ mean += time;
+ printf( "transfer time:%fns\n", time );
+ } else
+ {
+ printf( "transfer time: irrelevant\n" );
+ }
+ }
+ mean /= ++i;
+ printf( "average: %fns\n", mean );
+
+ free( tx_time );
+ free( rx_time );
+// clock_gettime( CLOCK_REALTIME, &tx_time );
+
+ return 0;
+}
+