00001 #include <stdio.h>
00002 #include <stdlib.h>
00003 #include <signal.h>
00004 #include <unistd.h>
00005 #include <time.h>
00006 #include <errno.h>
00007 #include <getopt.h>
00008 #include <sys/mman.h>
00009
00010 #include <native/task.h>
00011 #include <native/timer.h>
00012 #include <native/pipe.h>
00013
00014 #include <rtdm/rtcan.h>
00015
00016 extern int optind, opterr, optopt;
00017
00018 static void print_usage(char *prg)
00019 {
00020 fprintf(stderr,
00021 "Usage: %s <can-interface> [Options] <can-msg>\n"
00022 "<can-msg> can consist of up to 8 bytes given as a space separated list\n"
00023 "Options:\n"
00024 " -i, --identifier=ID CAN Identifier (default = 1)\n"
00025 " -r --rtr send remote request\n"
00026 " -e --extended send extended frame\n"
00027 " -l --loop=COUNT send message COUNT times\n"
00028 " -c, --count message count in data[0-3]\n"
00029 " -d, --delay=MS delay in ms (default = 1ms)\n"
00030 " -s, --send use send instead of sendto\n"
00031 " -t, --timeout=MS timeout in ms\n"
00032 " -L, --loopback=0|1 switch local loopback off or on\n"
00033 " -v, --verbose be verbose\n"
00034 " -p, --print=MODULO print every MODULO message\n"
00035 " -h, --help this help\n",
00036 prg);
00037 }
00038
00039
00040 RT_TASK rt_task_desc;
00041
00042 static int s=-1, dlc=0, rtr=0, extended=0, verbose=0, loops=1;
00043 static SRTIME delay=1000000;
00044 static int count=0, print=1, use_send=0, loopback=-1;
00045 static nanosecs_rel_t timeout = 0;
00046 static struct can_frame frame;
00047 static struct sockaddr_can to_addr;
00048
00049
00050 void cleanup(void)
00051 {
00052 int ret;
00053
00054 if (verbose)
00055 printf("Cleaning up...\n");
00056
00057 usleep(100000);
00058
00059 if (s >= 0) {
00060 ret = rt_dev_close(s);
00061 s = -1;
00062 if (ret) {
00063 fprintf(stderr, "rt_dev_close: %s\n", strerror(-ret));
00064 }
00065 rt_task_delete(&rt_task_desc);
00066 }
00067 }
00068
00069 void cleanup_and_exit(int sig)
00070 {
00071 if (verbose)
00072 printf("Signal %d received\n", sig);
00073 cleanup();
00074 exit(0);
00075 }
00076
00077 void rt_task(void)
00078 {
00079 int i, j, ret;
00080
00081 for (i = 0; i < loops; i++) {
00082 rt_task_sleep(rt_timer_ns2ticks(delay));
00083 if (count)
00084 memcpy(&frame.data[0], &i, sizeof(i));
00085
00086 if (use_send)
00087 ret = rt_dev_send(s, (void *)&frame, sizeof(can_frame_t), 0);
00088 else
00089 ret = rt_dev_sendto(s, (void *)&frame, sizeof(can_frame_t), 0,
00090 (struct sockaddr *)&to_addr, sizeof(to_addr));
00091 if (ret < 0) {
00092 switch (ret) {
00093 case -ETIMEDOUT:
00094 if (verbose)
00095 printf("rt_dev_send(to): timed out");
00096 break;
00097 case -EBADF:
00098 if (verbose)
00099 printf("rt_dev_send(to): aborted because socket was closed");
00100 break;
00101 default:
00102 fprintf(stderr, "rt_dev_send: %s\n", strerror(-ret));
00103 break;
00104 }
00105 i = loops;
00106 break;
00107 }
00108 if (verbose && (i % print) == 0) {
00109 if (frame.can_id & CAN_EFF_FLAG)
00110 printf("<0x%08x>", frame.can_id & CAN_EFF_MASK);
00111 else
00112 printf("<0x%03x>", frame.can_id & CAN_SFF_MASK);
00113 printf(" [%d]", frame.can_dlc);
00114 for (j = 0; j < frame.can_dlc; j++) {
00115 printf(" %02x", frame.data[j]);
00116 }
00117 printf("\n");
00118 }
00119 }
00120 }
00121
00122 int main(int argc, char **argv)
00123 {
00124 int i, opt, ret;
00125 struct ifreq ifr;
00126 char name[32];
00127
00128 struct option long_options[] = {
00129 { "help", no_argument, 0, 'h' },
00130 { "identifier", required_argument, 0, 'i'},
00131 { "rtr", no_argument, 0, 'r'},
00132 { "extended", no_argument, 0, 'e'},
00133 { "verbose", no_argument, 0, 'v'},
00134 { "count", no_argument, 0, 'c'},
00135 { "print", required_argument, 0, 'p'},
00136 { "loop", required_argument, 0, 'l'},
00137 { "delay", required_argument, 0, 'd'},
00138 { "send", no_argument, 0, 's'},
00139 { "timeout", required_argument, 0, 't'},
00140 { "loopback", required_argument, 0, 'L'},
00141 { 0, 0, 0, 0},
00142 };
00143
00144 mlockall(MCL_CURRENT | MCL_FUTURE);
00145
00146 signal(SIGTERM, cleanup_and_exit);
00147 signal(SIGINT, cleanup_and_exit);
00148
00149 frame.can_id = 1;
00150
00151 while ((opt = getopt_long(argc, argv, "hvi:l:red:t:cp:sL:",
00152 long_options, NULL)) != -1) {
00153 switch (opt) {
00154 case 'h':
00155 print_usage(argv[0]);
00156 exit(0);
00157
00158 case 'p':
00159 print = strtoul(optarg, NULL, 0);
00160
00161 case 'v':
00162 verbose = 1;
00163 break;
00164
00165 case 'c':
00166 count = 1;
00167 break;
00168
00169 case 'l':
00170 loops = strtoul(optarg, NULL, 0);
00171 break;
00172
00173 case 'i':
00174 frame.can_id = strtoul(optarg, NULL, 0);
00175 break;
00176
00177 case 'r':
00178 rtr = 1;
00179 break;
00180
00181 case 'e':
00182 extended = 1;
00183 break;
00184
00185 case 'd':
00186 delay = strtoul(optarg, NULL, 0) * 1000000LL;
00187 break;
00188
00189 case 's':
00190 use_send = 1;
00191 break;
00192
00193 case 't':
00194 timeout = strtoul(optarg, NULL, 0) * 1000000LL;
00195 break;
00196
00197 case 'L':
00198 loopback = strtoul(optarg, NULL, 0);
00199 break;
00200
00201 default:
00202 fprintf(stderr, "Unknown option %c\n", opt);
00203 break;
00204 }
00205 }
00206
00207 if (optind == argc) {
00208 print_usage(argv[0]);
00209 exit(0);
00210 }
00211
00212 if (argv[optind] == NULL) {
00213 fprintf(stderr, "No Interface supplied\n");
00214 exit(-1);
00215 }
00216
00217 if (verbose)
00218 printf("interface %s\n", argv[optind]);
00219
00220 ret = rt_dev_socket(PF_CAN, SOCK_RAW, CAN_RAW);
00221 if (ret < 0) {
00222 fprintf(stderr, "rt_dev_socket: %s\n", strerror(-ret));
00223 return -1;
00224 }
00225 s = ret;
00226
00227 if (loopback >= 0) {
00228 ret = rt_dev_setsockopt(s, SOL_CAN_RAW, CAN_RAW_LOOPBACK,
00229 &loopback, sizeof(loopback));
00230 if (ret < 0) {
00231 fprintf(stderr, "rt_dev_setsockopt: %s\n", strerror(-ret));
00232 goto failure;
00233 }
00234 if (verbose)
00235 printf("Using loopback=%d\n", loopback);
00236 }
00237
00238 strncpy(ifr.ifr_name, argv[optind], IFNAMSIZ);
00239 if (verbose)
00240 printf("s=%d, ifr_name=%s\n", s, ifr.ifr_name);
00241
00242 ret = rt_dev_ioctl(s, SIOCGIFINDEX, &ifr);
00243 if (ret < 0) {
00244 fprintf(stderr, "rt_dev_ioctl: %s\n", strerror(-ret));
00245 goto failure;
00246 }
00247
00248 memset(&to_addr, 0, sizeof(to_addr));
00249 to_addr.can_ifindex = ifr.ifr_ifindex;
00250 to_addr.can_family = AF_CAN;
00251 if (use_send) {
00252
00253 ret = rt_dev_setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0);
00254 if (ret < 0) {
00255 fprintf(stderr, "rt_dev_setsockopt: %s\n", strerror(-ret));
00256 goto failure;
00257 }
00258
00259 ret = rt_dev_bind(s, (struct sockaddr *)&to_addr, sizeof(to_addr));
00260 if (ret < 0) {
00261 fprintf(stderr, "rt_dev_bind: %s\n", strerror(-ret));
00262 goto failure;
00263 }
00264 }
00265
00266 if (count)
00267 frame.can_dlc = sizeof(int);
00268 else {
00269 for (i = optind + 1; i < argc; i++) {
00270 frame.data[dlc] = strtoul(argv[i], NULL, 0);
00271 dlc++;
00272 if( dlc == 8 )
00273 break;
00274 }
00275 frame.can_dlc = dlc;
00276 }
00277
00278 if (rtr)
00279 frame.can_id |= CAN_RTR_FLAG;
00280
00281 if (extended)
00282 frame.can_id |= CAN_EFF_FLAG;
00283
00284 if (timeout) {
00285 if (verbose)
00286 printf("Timeout: %lld ns\n", (long long)timeout);
00287 ret = rt_dev_ioctl(s, RTCAN_RTIOC_SND_TIMEOUT, &timeout);
00288 if (ret) {
00289 fprintf(stderr, "rt_dev_ioctl SND_TIMEOUT: %s\n", strerror(-ret));
00290 goto failure;
00291 }
00292 }
00293
00294 snprintf(name, sizeof(name), "rtcansend-%d", getpid());
00295 ret = rt_task_shadow(&rt_task_desc, name, 1, 0);
00296 if (ret) {
00297 fprintf(stderr, "rt_task_shadow: %s\n", strerror(-ret));
00298 goto failure;
00299 }
00300
00301 rt_task();
00302
00303 cleanup();
00304 return 0;
00305
00306 failure:
00307 cleanup();
00308 return -1;
00309 }