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/pipe.h>
00012
00013 #include <rtdm/rtcan.h>
00014
00015 static void print_usage(char *prg)
00016 {
00017 fprintf(stderr,
00018 "Usage: %s [<can-interface>] [Options]\n"
00019 "Options:\n"
00020 " -f --filter=id:mask[:id:mask]... apply filter\n"
00021 " -e --error=mask receive error messages\n"
00022 " -t, --timeout=MS timeout in ms\n"
00023 " -T, --timestamp with absolute timestamp\n"
00024 " -R, --timestamp-rel with relative timestamp\n"
00025 " -v, --verbose be verbose\n"
00026 " -p, --print=MODULO print every MODULO message\n"
00027 " -h, --help this help\n",
00028 prg);
00029 }
00030
00031
00032 extern int optind, opterr, optopt;
00033
00034 static int s = -1, verbose = 0, print = 1;
00035 static nanosecs_rel_t timeout = 0, with_timestamp = 0, timestamp_rel = 0;
00036
00037 RT_TASK rt_task_desc;
00038
00039 #define BUF_SIZ 255
00040 #define MAX_FILTER 16
00041
00042 struct sockaddr_can recv_addr;
00043 struct can_filter recv_filter[MAX_FILTER];
00044 static int filter_count = 0;
00045
00046 int add_filter(u_int32_t id, u_int32_t mask)
00047 {
00048 if (filter_count >= MAX_FILTER)
00049 return -1;
00050 recv_filter[filter_count].can_id = id;
00051 recv_filter[filter_count].can_mask = mask;
00052 printf("Filter #%d: id=0x%08x mask=0x%08x\n", filter_count, id, mask);
00053 filter_count++;
00054 return 0;
00055 }
00056
00057 void cleanup(void)
00058 {
00059 int ret;
00060
00061 if (verbose)
00062 printf("Cleaning up...\n");
00063
00064 if (s >= 0) {
00065 ret = rt_dev_close(s);
00066 s = -1;
00067 if (ret) {
00068 fprintf(stderr, "rt_dev_close: %s\n", strerror(-ret));
00069 }
00070 rt_task_delete(&rt_task_desc);
00071 }
00072 }
00073
00074 void cleanup_and_exit(int sig)
00075 {
00076 if (verbose)
00077 printf("Signal %d received\n", sig);
00078 cleanup();
00079 exit(0);
00080 }
00081
00082 void rt_task(void)
00083 {
00084 int i, ret, count = 0;
00085 struct can_frame frame;
00086 struct sockaddr_can addr;
00087 socklen_t addrlen = sizeof(addr);
00088 struct msghdr msg;
00089 struct iovec iov;
00090 nanosecs_abs_t timestamp, timestamp_prev = 0;
00091
00092 if (with_timestamp) {
00093 msg.msg_iov = &iov;
00094 msg.msg_iovlen = 1;
00095 msg.msg_name = (void *)&addr;
00096 msg.msg_namelen = sizeof(struct sockaddr_can);
00097 msg.msg_control = (void *)×tamp;
00098 msg.msg_controllen = sizeof(nanosecs_abs_t);
00099 }
00100
00101 while (1) {
00102 if (with_timestamp) {
00103 iov.iov_base = (void *)&frame;
00104 iov.iov_len = sizeof(can_frame_t);
00105 ret = rt_dev_recvmsg(s, &msg, 0);
00106 } else
00107 ret = rt_dev_recvfrom(s, (void *)&frame, sizeof(can_frame_t), 0,
00108 (struct sockaddr *)&addr, &addrlen);
00109 if (ret < 0) {
00110 switch (ret) {
00111 case -ETIMEDOUT:
00112 if (verbose)
00113 printf("rt_dev_recv: timed out");
00114 continue;
00115 case -EBADF:
00116 if (verbose)
00117 printf("rt_dev_recv: aborted because socket was closed");
00118 break;
00119 default:
00120 fprintf(stderr, "rt_dev_recv: %s\n", strerror(-ret));
00121 }
00122 break;
00123 }
00124
00125 if (print && (count % print) == 0) {
00126 printf("#%d: (%d) ", count, addr.can_ifindex);
00127 if (with_timestamp && msg.msg_controllen) {
00128 if (timestamp_rel) {
00129 printf("%lldns ", (long long)(timestamp - timestamp_prev));
00130 timestamp_prev = timestamp;
00131 } else
00132 printf("%lldns ", (long long)timestamp);
00133 }
00134 if (frame.can_id & CAN_ERR_FLAG)
00135 printf("!0x%08x!", frame.can_id & CAN_ERR_MASK);
00136 else if (frame.can_id & CAN_EFF_FLAG)
00137 printf("<0x%08x>", frame.can_id & CAN_EFF_MASK);
00138 else
00139 printf("<0x%03x>", frame.can_id & CAN_SFF_MASK);
00140
00141 printf(" [%d]", frame.can_dlc);
00142 if (!(frame.can_id & CAN_RTR_FLAG))
00143 for (i = 0; i < frame.can_dlc; i++) {
00144 printf(" %02x", frame.data[i]);
00145 }
00146 if (frame.can_id & CAN_ERR_FLAG) {
00147 printf(" ERROR ");
00148 if (frame.can_id & CAN_ERR_BUSOFF)
00149 printf("bus-off");
00150 if (frame.can_id & CAN_ERR_CRTL)
00151 printf("controller problem");
00152 } else if (frame.can_id & CAN_RTR_FLAG)
00153 printf(" remote request");
00154 printf("\n");
00155 }
00156 count++;
00157 }
00158 }
00159
00160 int main(int argc, char **argv)
00161 {
00162 int opt, ret;
00163 u_int32_t id, mask;
00164 u_int32_t err_mask = 0;
00165 struct ifreq ifr;
00166 char *ptr;
00167 char name[32];
00168
00169 struct option long_options[] = {
00170 { "help", no_argument, 0, 'h' },
00171 { "verbose", no_argument, 0, 'v'},
00172 { "filter", required_argument, 0, 'f'},
00173 { "error", required_argument, 0, 'e'},
00174 { "timeout", required_argument, 0, 't'},
00175 { "timestamp", no_argument, 0, 'T'},
00176 { "timestamp-rel", no_argument, 0, 'R'},
00177 { 0, 0, 0, 0},
00178 };
00179
00180 mlockall(MCL_CURRENT | MCL_FUTURE);
00181
00182 signal(SIGTERM, cleanup_and_exit);
00183 signal(SIGINT, cleanup_and_exit);
00184
00185 while ((opt = getopt_long(argc, argv, "hve:f:t:p:RT",
00186 long_options, NULL)) != -1) {
00187 switch (opt) {
00188 case 'h':
00189 print_usage(argv[0]);
00190 exit(0);
00191
00192 case 'p':
00193 print = strtoul(optarg, NULL, 0);
00194 break;
00195
00196 case 'v':
00197 verbose = 1;
00198 break;
00199
00200 case 'e':
00201 err_mask = strtoul(optarg, NULL, 0);
00202 break;
00203
00204 case 'f':
00205 ptr = optarg;
00206 while (1) {
00207 id = strtoul(ptr, NULL, 0);
00208 ptr = strchr(ptr, ':');
00209 if (!ptr) {
00210 fprintf(stderr, "filter must be applied in the form id:mask[:id:mask]...\n");
00211 exit(1);
00212 }
00213 ptr++;
00214 mask = strtoul(ptr, NULL, 0);
00215 ptr = strchr(ptr, ':');
00216 add_filter(id, mask);
00217 if (!ptr)
00218 break;
00219 ptr++;
00220 }
00221 break;
00222
00223 case 't':
00224 timeout = (nanosecs_rel_t)strtoul(optarg, NULL, 0) * 1000000;
00225 break;
00226
00227 case 'R':
00228 timestamp_rel = 1;
00229 case 'T':
00230 with_timestamp = 1;
00231 break;
00232
00233 default:
00234 fprintf(stderr, "Unknown option %c\n", opt);
00235 break;
00236 }
00237 }
00238
00239 ret = rt_dev_socket(PF_CAN, SOCK_RAW, CAN_RAW);
00240 if (ret < 0) {
00241 fprintf(stderr, "rt_dev_socket: %s\n", strerror(-ret));
00242 return -1;
00243 }
00244 s = ret;
00245
00246 if (argv[optind] == NULL) {
00247 if (verbose)
00248 printf("interface all\n");
00249
00250 ifr.ifr_ifindex = 0;
00251 } else {
00252 if (verbose)
00253 printf("interface %s\n", argv[optind]);
00254
00255 strncpy(ifr.ifr_name, argv[optind], IFNAMSIZ);
00256 if (verbose)
00257 printf("s=%d, ifr_name=%s\n", s, ifr.ifr_name);
00258
00259 ret = rt_dev_ioctl(s, SIOCGIFINDEX, &ifr);
00260 if (ret < 0) {
00261 fprintf(stderr, "rt_dev_ioctl GET_IFINDEX: %s\n", strerror(-ret));
00262 goto failure;
00263 }
00264 }
00265
00266 if (err_mask) {
00267 ret = rt_dev_setsockopt(s, SOL_CAN_RAW, CAN_RAW_ERR_FILTER,
00268 &err_mask, sizeof(err_mask));
00269 if (ret < 0) {
00270 fprintf(stderr, "rt_dev_setsockopt: %s\n", strerror(-ret));
00271 goto failure;
00272 }
00273 if (verbose)
00274 printf("Using err_mask=%#x\n", err_mask);
00275 }
00276
00277 if (filter_count) {
00278 ret = rt_dev_setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER,
00279 &recv_filter, filter_count *
00280 sizeof(struct can_filter));
00281 if (ret < 0) {
00282 fprintf(stderr, "rt_dev_setsockopt: %s\n", strerror(-ret));
00283 goto failure;
00284 }
00285 }
00286
00287 recv_addr.can_family = AF_CAN;
00288 recv_addr.can_ifindex = ifr.ifr_ifindex;
00289 ret = rt_dev_bind(s, (struct sockaddr *)&recv_addr,
00290 sizeof(struct sockaddr_can));
00291 if (ret < 0) {
00292 fprintf(stderr, "rt_dev_bind: %s\n", strerror(-ret));
00293 goto failure;
00294 }
00295
00296 if (timeout) {
00297 if (verbose)
00298 printf("Timeout: %lld ns\n", (long long)timeout);
00299 ret = rt_dev_ioctl(s, RTCAN_RTIOC_RCV_TIMEOUT, &timeout);
00300 if (ret) {
00301 fprintf(stderr, "rt_dev_ioctl RCV_TIMEOUT: %s\n", strerror(-ret));
00302 goto failure;
00303 }
00304 }
00305
00306 if (with_timestamp) {
00307 ret = rt_dev_ioctl(s, RTCAN_RTIOC_TAKE_TIMESTAMP, RTCAN_TAKE_TIMESTAMPS);
00308 if (ret) {
00309 fprintf(stderr, "rt_dev_ioctl TAKE_TIMESTAMP: %s\n", strerror(-ret));
00310 goto failure;
00311 }
00312 }
00313
00314 snprintf(name, sizeof(name), "rtcanrecv-%d", getpid());
00315 ret = rt_task_shadow(&rt_task_desc, name, 0, 0);
00316 if (ret) {
00317 fprintf(stderr, "rt_task_shadow: %s\n", strerror(-ret));
00318 goto failure;
00319 }
00320
00321 rt_task();
00322
00323
00324 failure:
00325 cleanup();
00326 return -1;
00327 }