rtcanrecv.c

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 *)&timestamp;
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     /* never returns */
00323 
00324  failure:
00325     cleanup();
00326     return -1;
00327 }

Generated on Mon Mar 24 18:02:40 2008 for Xenomai API by  doxygen 1.5.3