From 3285bf97a73fe54806e97cc320a61673f8395df2 Mon Sep 17 00:00:00 2001 From: Michal Sojka Date: Thu, 11 Jun 2009 14:25:50 +0200 Subject: [PATCH 1/1] Added vca_canping from OCERA's CVS --- src/Makefile.omk | 10 + src/vca_canping.c | 725 ++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 735 insertions(+) create mode 100644 src/vca_canping.c diff --git a/src/Makefile.omk b/src/Makefile.omk index b902cd4..4a9ab11 100644 --- a/src/Makefile.omk +++ b/src/Makefile.omk @@ -12,3 +12,13 @@ kernel_INCLUDES = -I $(srcdir)/../include #rtlinux_MODULES = canping_rtl canping_rtl_SOURCES = canping_rtl.c + + +ifeq ($(CONFIG_OC_CANVCA),y) + +bin_PROGRAMS = vca_canping +vca_canping_SOURCES = vca_canping.c +vca_canping_LIBS = pthread m vca ulut + +endif #CONFIG_OC_CANVCA + diff --git a/src/vca_canping.c b/src/vca_canping.c new file mode 100644 index 0000000..2123976 --- /dev/null +++ b/src/vca_canping.c @@ -0,0 +1,725 @@ +/**************************************************************************/ +/* 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 */ +/* */ +/* 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* TODO: Handle the case where there are more canping slaves running + * on one CAN bus. */ + +#include + +//#define DEBUG 1 +//#define DEBUG 2 +#define WITH_RTPRIO + +#ifdef WITH_RTPRIO +#include + +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 [other options]\n" + " canping -s [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; +} -- 2.39.2