--- /dev/null
+/**************************************************************************/
+/* File: vca_canping.c - utility to test CAN functionality and throughput */
+/* */
+/* LibVCA - Versatile CAN/CANopen API library */
+/* Copyright (C) 2005-2006 Michal Sojka, DCE FEE CTU Prague */
+/* Copyright (C) 2006-2009 Pavel Pisa <pisa@cmp.felk.cvut.cz> */
+/* */
+/* LibVCA is free software; you can redistribute it and/or modify it */
+/* under terms of the GNU General Public License as published by the */
+/* Free Software Foundation; either version 2, or (at your option) any */
+/* later version. LinCAN is distributed in the hope that it will be */
+/* useful, but WITHOUT ANY WARRANTY; without even the implied warranty */
+/* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU */
+/* General Public License for more details. You should have received a */
+/* copy of the GNU General Public License along with LinCAN; see file */
+/* COPYING. If not, write to the Free Software Foundation, 675 Mass Ave, */
+/* Cambridge, MA 02139, USA. */
+/**************************************************************************/
+
+#include <string.h>
+#include <math.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <stdlib.h>
+#include <ctype.h>
+#include <pthread.h>
+#include <signal.h>
+#include <sys/types.h>
+#include <sys/time.h>
+#include <ul_list.h>
+#include <errno.h>
+#include <semaphore.h>
+
+/* TODO: Handle the case where there are more canping slaves running
+ * on one CAN bus. */
+
+#include <can_vca.h>
+
+//#define DEBUG 1
+//#define DEBUG 2
+#define WITH_RTPRIO
+
+#ifdef WITH_RTPRIO
+#include <sched.h>
+
+int sched_policy = SCHED_OTHER;
+int sched_rtprio;
+
+#endif
+
+
+#ifndef DEBUG
+#define dbg(level, fmt, arg...) do {} while (0)
+#else
+#define dbg(level, fmt, arg...) do { if (level <= DEBUG) { printf("candping: " fmt, ## arg); } } while (0)
+#endif
+
+#define NOT_INTERRUPTED_SYSCALL (errno != EINTR && errno != ERESTART)
+#define IS_FINISH_FLAG() (finish_flag)
+
+/* Exit codes */
+#define EXIT_OK 0
+#define EXIT_BAD_PARAM 1
+#define EXIT_CANNOT_OPEN 2
+#define EXIT_FILTER_ERROR 3
+#define EXIT_NO_MEM 4
+#define EXIT_READ_ERROR 5
+#define EXIT_WRITE_ERROR 6
+#define EXIT_SELECT_ERROR 7
+#define EXIT_FLUSH_ERROR 8
+
+/* Global variables */
+sig_atomic_t finish_flag = 0; /* Threads should terminate. */
+sem_t finish_sem; /* Thread signals a termination */
+vca_handle_t global_vcah = VCA_HANDLE_INVALID;
+
+int total_count = 0;
+int total_timeout = 0;
+
+pthread_mutex_t mut_start = PTHREAD_MUTEX_INITIALIZER;
+pthread_cond_t cond_start = PTHREAD_COND_INITIALIZER;
+int start_flag = 0;
+sem_t ready_sem; /* Thread is ready for execution */
+
+
+/* Command line options */
+char *option_device = "/dev/can0";
+int option_masters = 0;
+long int option_first_id = 1000;
+int option_slaves = 0;
+int option_verbose = 0; /* 0 - nothing, 1 - only global
+ * statistics, 2 - simple times, 3 -
+ * verbose times */
+int option_length = 8;
+int option_count = 0;
+int option_wait_ms = 1000;
+int option_timeout = 4;
+int option_open_once = 0;
+int option_synch_start = 0;
+
+/* Lists */
+typedef struct threads {
+ ul_list_head_t head;
+} threads_t;
+
+typedef struct thread_data {
+ pthread_t tid;
+ long int canid;
+
+ int count;
+ double mean; /* mean value of responses */
+ double moment2nd; /* used to compute variance of
+ * responses */
+ int min, max; /* min/max response times */
+ int timeout; /* number of timeouts */
+
+ ul_list_node_t node;
+} thread_data_t;
+
+UL_LIST_CUST_DEC(thread_list, threads_t, thread_data_t, head, node);
+
+threads_t master_threads;
+threads_t slave_threads;
+
+/* Subtract the `struct timeval' values X and Y, storing the result in
+ RESULT. Return 1 if the difference is negative, otherwise 0. */
+
+int timeval_subtract (struct timeval *result, struct timeval *x, struct timeval *y)
+{
+ /* Perform the carry for the later subtraction by updating Y. */
+ if (x->tv_usec < y->tv_usec) {
+ int nsec = (y->tv_usec - x->tv_usec) / 1000000 + 1;
+ y->tv_usec -= 1000000 * nsec;
+ y->tv_sec += nsec;
+ }
+ if (x->tv_usec - y->tv_usec > 1000000) {
+ int nsec = (x->tv_usec - y->tv_usec) / 1000000;
+ y->tv_usec += 1000000 * nsec;
+ y->tv_sec -= nsec;
+ }
+
+ /* Compute the time remaining to wait.
+ `tv_usec' is certainly positive. */
+ result->tv_sec = x->tv_sec - y->tv_sec;
+ result->tv_usec = x->tv_usec - y->tv_usec;
+
+ /* Return 1 if result is negative. */
+ return x->tv_sec < y->tv_sec;
+}
+
+void dbg_print_timeval(char *msg, struct timeval *tv)
+{
+
+ printf("%s sec=%ld usec=%ld\n", msg, tv->tv_sec, tv->tv_usec);
+}
+void kill_all_threads(int signum)
+{
+ thread_data_t *td;
+
+ ul_list_for_each(thread_list, &master_threads, td) {
+ pthread_kill(td->tid, signum);
+ }
+ ul_list_for_each(thread_list, &slave_threads, td) {
+ pthread_kill(td->tid, signum);
+ }
+}
+void term_handler(int signum)
+{
+ dbg(1, "Thread %p got a signal %d\n", (void *)pthread_self(), signum);
+ if (!IS_FINISH_FLAG()) {
+ dbg(1, "Terminating threads\n");
+ finish_flag = 1;
+
+ kill_all_threads(signum);
+ }
+}
+
+void *master_thread(void *arg)
+{
+ thread_data_t *td = (thread_data_t *)arg;
+ int ping_id = td->canid;
+ int pong_id = ping_id + 1;
+ struct canmsg_t pingmsg, pongmsg;
+ int ping_count = 0;
+ int ret = 0;
+ vca_handle_t vcah = VCA_HANDLE_INVALID; /* File descriptor of CAN driver. */
+ int i;
+ fd_set rdset; /* read set for select syscall */
+ struct timeval timeout, pingtime, pongtime;
+ struct canfilt_t canfilt; /* filter for received messages */
+
+ if (!option_open_once) {
+ /* Open can driver */
+ if(vca_open_handle(&vcah, option_device, NULL, 0) < 0) {
+ perror("open");
+ fprintf(stderr, "Error opening %s (for id %d)\n", option_device, ping_id);
+ exit(EXIT_CANNOT_OPEN);
+ }
+ } else {
+ vcah = global_vcah;
+ }
+
+ /* setup filtering of received messages */
+ memset(&canfilt, 0, sizeof(canfilt));
+ canfilt.mask = 0xfffffff;
+ canfilt.id = pong_id; /* pong responces with increased id */
+ ret = vca_set_filt(vcah, &canfilt);
+ if(ret<0) {
+ perror("ioctl CANQUE_FILTER");
+ exit(EXIT_FILTER_ERROR);
+ }
+ ret = vca_queue_flush(vcah, 0);
+ if(ret<0) {
+ perror("ioctl CANQUE_QUEUE_FLUSH");
+ exit(EXIT_FLUSH_ERROR);
+ }
+
+ /* Prepare data structures for the select syscall. */
+ FD_ZERO (&rdset);
+
+ /* Signal that I'm ready */
+ sem_post(&ready_sem);
+
+ if (option_synch_start) {
+ /* Wait for other threads to initialize */
+ pthread_mutex_lock(&mut_start);
+ while (!start_flag) pthread_cond_wait(&cond_start, &mut_start);
+ pthread_mutex_unlock(&mut_start);
+ }
+
+ while (!IS_FINISH_FLAG() && (option_count == 0 || ping_count++ < option_count)) {
+ /* Send a ping message */
+ pingmsg.flags=0;
+ pingmsg.id=ping_id;
+ pingmsg.length = option_length;
+ for (i=0; i < option_length; i++) pingmsg.data[i] = i;
+ gettimeofday(&pingtime, NULL);
+
+ ret = vca_send_msg_seq(vcah, &pingmsg, 1);
+ if (ret < 0) {
+ if (NOT_INTERRUPTED_SYSCALL) {
+ perror("write");
+ exit(EXIT_WRITE_ERROR);
+ }
+ else continue;
+ }
+
+ /* Wait for a pong responce */
+ FD_SET (vca_h2fd(vcah), &rdset);
+
+ timeout.tv_sec = option_timeout;
+ timeout.tv_usec = 0;
+
+ ret = select(FD_SETSIZE, &rdset, NULL, NULL, &timeout);
+ if (ret > 0) {
+ /* Read the message */
+ pongmsg.flags=0;
+ ret = vca_rec_msg_seq(vcah, &pongmsg, 1);
+ if (ret < 0) {
+ if (NOT_INTERRUPTED_SYSCALL) {
+ perror("read");
+ exit(EXIT_READ_ERROR);
+ }
+ else continue;
+ }
+ else { /* A pong message received */
+ long int time;
+ gettimeofday(&pongtime, NULL);
+ if (ret == 0) {
+ fprintf(stderr, "read returned zero\n");
+ exit(EXIT_READ_ERROR);
+ }
+ if (pongmsg.id != pong_id) {
+ fprintf(stderr, "Filter error (expected: %d, received %ld)\n", pong_id, pongmsg.id);
+ exit(EXIT_FILTER_ERROR);
+ }
+ timeval_subtract(&pongtime, &pongtime, &pingtime);
+ time = pongtime.tv_sec*1000000 + pongtime.tv_usec;
+ switch (option_verbose) {
+ case 2:
+ printf("%d:%ld\n", ping_id, time);
+ break;
+ case 3:
+ printf("Pong response for id %d received in %ld us\n", ping_id, time);
+ break;
+ }
+ /* Update statistics */
+ td->count++;
+ td->mean =
+ td->mean * ((double)(td->count - 1) / td->count) +
+ (double)time / td->count;
+ td->moment2nd =
+ td->moment2nd * ((double)(td->count - 1) / td->count) +
+ (double)time*time / td->count;
+ if (time > td->max) td->max = time;
+ if (time < td->min) td->min = time;
+ total_count++;
+ } /* read */
+ }
+ else if (ret == 0) { /* select */
+ if (option_verbose >= 2)
+ printf("Timeout encountered (id %d)\n", ping_id);
+ td->timeout++;
+ total_timeout++;
+ }
+ else {
+ if (NOT_INTERRUPTED_SYSCALL) {
+ perror("select");
+ exit(EXIT_SELECT_ERROR);
+ }
+ else continue;
+ }
+ usleep(option_wait_ms * 1000);
+ }
+
+ if (!option_open_once) {
+ /* Close the can driver */
+ vca_close_handle(vcah);
+ }
+
+ sem_post(&finish_sem);
+ return (void *)ret;
+}
+
+void start_masters(int masters, int first_id)
+{
+ int id = first_id;
+ int i;
+
+ dbg(1, "Starting %d master threads\n", masters);
+
+ for (i = 0; i < masters; i++, id += 2) {
+ thread_data_t *td;
+
+ td = malloc(sizeof(*td));
+ if (!td) {
+ printf("Can't allocate memory");
+ exit(EXIT_NO_MEM);
+ }
+ memset(td, 0, sizeof(*td));
+ td->canid = id;
+ td->min = 0x7fffffff;
+ /* TODO use mutexes and signal blocking */
+ thread_list_ins_tail(&master_threads, td);
+ pthread_create(&td->tid, NULL, master_thread, (void *)td);
+ dbg(2, "Master thread: %p\n", (void *)td->tid);
+ }
+
+
+ /* Wait for all threads beeing ready */
+ for (i = 0; i < masters; i++) sem_wait(&ready_sem);
+
+ /* Start threads */
+ pthread_mutex_lock(&mut_start);
+ start_flag = 1;
+ pthread_cond_broadcast(&cond_start);
+ pthread_mutex_unlock(&mut_start);
+}
+
+void *slave_thread(void *arg)
+{
+ thread_data_t *td = (thread_data_t *)arg;
+ int ping_id = td->canid;
+ int pong_id = ping_id + 1;
+ struct canmsg_t pingmsg, pongmsg;
+ int ret = 0;
+ vca_handle_t vcah = VCA_HANDLE_INVALID; /* File descriptor of CAN driver. */
+ int i;
+ struct canfilt_t canfilt; /* filter for received messages */
+
+ if (!option_open_once) {
+ /* Open the CAN driver */
+ if(vca_open_handle(&vcah, option_device, NULL, 0) < 0) {
+ perror("open");
+ printf("Error opening %s (for id %d)\n", option_device, ping_id);
+ exit(EXIT_CANNOT_OPEN);
+ }
+ } else {
+ vcah = global_vcah;
+ }
+
+ /* setup filtering of received messages */
+ memset(&canfilt, 0, sizeof(canfilt));
+ canfilt.mask = 0xfffffff;
+ canfilt.id = ping_id; /* receive only our ping messages */
+ ret = vca_set_filt(vcah, &canfilt);
+ if (ret < 0) {
+ perror("ioctl CANQUE_FILTER");
+ exit(EXIT_FILTER_ERROR);
+ }
+ /* If there are some messages already, delete them. These may
+ * not processed by our filter and thus may have a wrong
+ * ID. */
+ ret = vca_queue_flush(vcah, 0);
+ if (ret < 0) {
+ perror("ioctl CANQUE_QUEUE_FLUSH");
+ exit(EXIT_FLUSH_ERROR);
+ }
+
+ /* Prepare a pong message */
+ pongmsg.flags = 0;
+ pongmsg.id = pong_id;
+ pongmsg.length = option_length;
+ for (i=0; i < option_length; i++) pongmsg.data[i] = i;
+
+ while (!IS_FINISH_FLAG()) {
+ /* Receive a ping message */
+ pingmsg.flags=0;
+ ret = vca_rec_msg_seq(vcah, &pingmsg, 1);
+ if (ret < 0) {
+ if (NOT_INTERRUPTED_SYSCALL) {
+ printf("%d\n", errno);
+ perror("read");
+ exit(EXIT_READ_ERROR);
+ }
+ }
+ if (NOT_INTERRUPTED_SYSCALL && pingmsg.id != ping_id) {
+ fprintf(stderr, "Filter error (expected: %d, received %ld)\n", ping_id, pingmsg.id);
+ exit(EXIT_FILTER_ERROR);
+ }
+ /* Answer immendiately with a pong message */
+ if (NOT_INTERRUPTED_SYSCALL) {
+ ret = vca_send_msg_seq(vcah, &pongmsg, 1);
+ if (ret < 0) {
+ if (NOT_INTERRUPTED_SYSCALL) {
+ perror("write");
+ exit(EXIT_WRITE_ERROR);
+ }
+ }
+ }
+
+ if (ret >= 0) total_count++;
+
+ if (option_verbose >= 2)
+ /* This drasticly slows down the pong
+ * response. Why??? */
+ printf("Replying to ping id %lu\n", pingmsg.id);
+
+ }
+
+ if (!option_open_once) {
+ /* Close can driver */
+ vca_close_handle(vcah);
+ }
+
+ dbg(2, "Slave thread for id %d is going to finish\n", ping_id);
+
+ sem_post(&finish_sem);
+ return (void *)ret;
+}
+
+void start_slaves(int slaves, int first_id)
+{
+ int id = first_id;
+ int i;
+
+ dbg(1, "Starting %d slave threads\n", slaves);
+
+ for (i = 0; i < slaves; i++, id += 2) {
+ thread_data_t *td;
+
+ td = malloc(sizeof(*td));
+ if (!td) {
+ printf("Can't allocate memory");
+ exit(EXIT_NO_MEM);
+ }
+ memset(td, 0, sizeof(*td));
+ td->canid = id;
+ /* TODO use mutexes and signal blocking */
+ thread_list_ins_tail(&slave_threads, td);
+ pthread_create(&td->tid, NULL, slave_thread, (void *)td);
+ dbg(2, "Slave thread: %p\n", (void *)td->tid);
+ }
+}
+
+#ifdef WITH_RTPRIO
+int set_sched_policy_and_prio(int policy, int rtprio)
+{
+ struct sched_param scheduling_parameters;
+ int maxprio=sched_get_priority_max(policy);
+ int minprio=sched_get_priority_min(policy);
+
+ if((rtprio < minprio) || (rtprio > maxprio)) {
+ fprintf(stderr, "The priority for requested policy is out of <%d, %d> range\n",
+ minprio, maxprio);
+ return -1;
+ }
+
+ scheduling_parameters.sched_priority = rtprio;
+
+ if (0 != pthread_setschedparam(pthread_self(), policy, &scheduling_parameters)) {
+ perror("pthread_setschedparam error");
+ }
+ return 0;
+}
+#endif
+
+void print_help(void)
+{
+ printf("Usage: canping -m <master threads> [other options]\n"
+ " canping -s <slave threads> [other options]\n\n"
+ "Other options:\n"
+ " -c count how many messages each master sends\n"
+ " -d dev device (e.g. /dev/can1)\n"
+ " -h print this help\n"
+ " -i id id of first master message\n"
+ " -l length length of the messages (0..8)\n"
+ " -o open a device only once for all threads (doesn't work)\n" /* due to filters */
+ " -t timeout timeout in seconds (default 4 s)\n"
+ " -v be verbose (use more than once to increase verbosity)\n"
+ " -w ms wait ms miliseconds between sending pings\n"
+ " -y synchronize threads before start (doesn't test race conditions\n"
+ " between open and read/write)\n"
+#ifdef WITH_RTPRIO
+ " -r run at realtime priority\n"
+ " -R P:prio run at realtime priority prio, policy RR or FF\n"
+#endif
+ "\n"
+ "Example: canping -m 10 -vv\n"
+ );
+}
+
+int parse_options(int argc, char *argv[])
+{
+ int c;
+
+ opterr = 0;
+ while ((c = getopt (argc, argv, "c:d:hi:l:m:os:t:vw:yrR:")) != -1)
+ switch (c)
+ {
+ case 'c':
+ option_count = atoi(optarg);
+ break;
+ case 'd':
+ option_device = optarg;
+ break;
+ case 'h':
+ print_help();
+ exit(EXIT_OK);
+ break;
+ case 'i':
+ option_first_id = atoi(optarg);
+ break;
+ case 'l':
+ option_length = atoi(optarg);
+ if (option_length > 8) option_length = 8;
+ if (option_length < 0) option_length = 0;
+ break;
+ case 'm':
+ option_masters = atoi(optarg);
+ break;
+ case 'o':
+ option_open_once = 1;
+ break;
+ case 's':
+ option_slaves = atoi(optarg);
+ break;
+ case 't':
+ option_timeout = atoi(optarg);
+ break;
+ case 'v':
+ option_verbose++;
+ break;
+ case 'w':
+ option_wait_ms = atoi(optarg);
+ break;
+ case 'y':
+ option_synch_start = 1;
+ break;
+#ifdef WITH_RTPRIO
+ case 'r':
+ sched_policy = SCHED_FIFO;
+ sched_rtprio = (sched_get_priority_min(SCHED_FIFO) +
+ sched_get_priority_max(SCHED_FIFO)) / 2;
+ break;
+ case 'R':
+ if(!isalpha(*optarg)) {
+ sched_policy = SCHED_FIFO;
+ } else if(!strncmp(optarg,"FF:",3)) {
+ sched_policy = SCHED_FIFO;
+ optarg += 3;
+ } else if(!strncmp(optarg,"RR:",3)) {
+ sched_policy = SCHED_RR;
+ optarg += 3;
+ } else {
+ fprintf (stderr, "Unknown policy %s\n", optarg);
+ exit(EXIT_BAD_PARAM);
+ }
+ sched_rtprio = atoi(optarg);
+ break;
+#endif
+ case '?':
+ if (isprint (optopt))
+ fprintf (stderr, "Unknown option `-%c'.\n", optopt);
+ else
+ fprintf (stderr,
+ "Unknown option character `\\x%x'.\n",
+ optopt);
+ return 1;
+ default:
+ exit(EXIT_BAD_PARAM);
+ }
+ if (!(option_masters || option_slaves) || (option_masters && option_slaves))
+ exit(EXIT_BAD_PARAM);
+ return 0;
+}
+
+void print_stats(thread_data_t *td)
+{
+ char std[20];
+ int count = td->count + td->timeout;
+
+ if (td->count >= 2) {
+ snprintf(std, 19, "%7.2f", sqrt((td->count/(td->count - 1)) *
+ (td->moment2nd - td->mean*td->mean)));
+ } else {
+ strncpy(std, "N/A", 19);
+ }
+ printf("Id %4ld: count = %5d"
+ " mean = %7.2f stddev = %s"
+ " min = %5d max = %5d [us]"
+ " loss = %d%% (%d)\n",
+ td->canid, count, td->mean,
+ std,
+ td->min, td->max,
+ (count > 0) ? 100 * td->timeout / count : 0, td->timeout
+ );
+}
+
+void wait_for_threads(void)
+{
+ thread_data_t *td;
+ void *thread_ret;
+ int thread_count = option_slaves + option_masters;
+ int ret;
+
+ while (thread_count > 0) {
+ if (option_verbose != 1) {
+ ret = sem_wait(&finish_sem);
+ dbg(2, "Main thread sem_wait() exited with ret=%d.\n", ret);
+
+ /* If the sem_wait is successful i.e. not
+ * interrupted by a signal, decrease the
+ * thread_count*/
+ if (ret == 0) thread_count--;
+ } else {
+ if (!IS_FINISH_FLAG()) {
+ printf("Total count: %6d, Timeouts: %6d\r", total_count, total_timeout);
+ fflush(stdout);
+ usleep(1000000);
+ }
+ while (sem_trywait(&finish_sem) == 0) thread_count--;
+ }
+ }
+ if (option_verbose == 1) {
+ printf("\n");
+ }
+
+ ul_list_for_each(thread_list, &master_threads, td) {
+ pthread_join(td->tid, &thread_ret);
+ dbg(2, "Master thread for id %ld finished; exit code %d\n", td->canid, (int)thread_ret);
+ }
+
+ ul_list_for_each(thread_list, &slave_threads, td) {
+ pthread_join(td->tid, &thread_ret);
+ dbg(2, "Slave thread for id %ld finished; exit code %d\n", td->canid, (int)thread_ret);
+ }
+
+ if (option_masters) printf("Summary statistics:\n");
+
+ ul_list_for_each_cut(thread_list, &master_threads, td) {
+ print_stats(td);
+ free(td);
+ }
+
+
+ ul_list_for_each_cut(thread_list, &slave_threads, td)
+ free(td);
+}
+
+int main(int argc, char *argv[])
+{
+ parse_options(argc, argv);
+
+#ifdef WITH_RTPRIO
+ if(sched_policy != SCHED_OTHER)
+ if(set_sched_policy_and_prio(sched_policy, sched_rtprio) <0)
+ exit(EXIT_BAD_PARAM);
+#endif
+
+
+ thread_list_init_head(&master_threads);
+ thread_list_init_head(&slave_threads);
+
+ sem_init(&finish_sem, 0, 0);
+ sem_init(&ready_sem, 0, 0);
+
+ siginterrupt(SIGINT, 1);
+ signal(SIGINT, term_handler);
+ siginterrupt(SIGTERM, 1);
+ signal(SIGTERM, term_handler);
+
+ dbg(2, "Main thread: %p\n", (void *)pthread_self());
+
+ if (option_open_once) {
+ /* Open the CAN driver */
+ if(vca_open_handle(&global_vcah, option_device, NULL, 0) < 0) {
+ perror("open");
+ printf("Error opening %s\n", option_device);
+ exit(EXIT_CANNOT_OPEN);
+ }
+ }
+
+ if (option_masters) start_masters(option_masters, option_first_id);
+ if (option_slaves) start_slaves(option_slaves, option_first_id);
+
+ wait_for_threads();
+
+ if (option_open_once) {
+ vca_close_handle(global_vcah);
+ }
+
+ return 0;
+}
projects / canping.git / commitdiff