]> rtime.felk.cvut.cz Git - sojka/can-utils.git/blob - can-sniffer.c
Remove remnants of the projects VW history (Email addresses, Makefile targets).
[sojka/can-utils.git] / can-sniffer.c
1 /*
2  *  $Id$
3  */
4
5 /*
6  * can-sniffer.c
7  *
8  * Copyright (c) 2002-2005 Volkswagen Group Electronic Research
9  * All rights reserved.
10  *
11  * Redistribution and use in source and binary forms, with or without
12  * modification, are permitted provided that the following conditions
13  * are met:
14  * 1. Redistributions of source code must retain the above copyright
15  *    notice, this list of conditions, the following disclaimer and
16  *    the referenced file 'COPYING'.
17  * 2. Redistributions in binary form must reproduce the above copyright
18  *    notice, this list of conditions and the following disclaimer in the
19  *    documentation and/or other materials provided with the distribution.
20  * 3. Neither the name of Volkswagen nor the names of its contributors
21  *    may be used to endorse or promote products derived from this software
22  *    without specific prior written permission.
23  *
24  * Alternatively, provided that this notice is retained in full, this
25  * software may be distributed under the terms of the GNU General
26  * Public License ("GPL") version 2 as distributed in the 'COPYING'
27  * file from the main directory of the linux kernel source.
28  *
29  * The provided data structures and external interfaces from this code
30  * are not restricted to be used by modules with a GPL compatible license.
31  *
32  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
33  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
34  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
35  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
36  * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
37  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
38  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
39  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
40  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
41  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
42  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
43  * DAMAGE.
44  *
45  * Send feedback to <socketcan-users@lists.berlios.de>
46  *
47  */
48
49 #include <stdio.h>
50 #include <stdlib.h>
51 #include <unistd.h>
52 #include <string.h>
53 #include <fcntl.h>
54 #include <signal.h>
55 #include <ctype.h>
56 #include <libgen.h>
57 #include <time.h>
58
59 #include <sys/time.h>
60 #include <sys/types.h>
61 #include <sys/stat.h>
62 #include <sys/socket.h>
63 #include <sys/ioctl.h>
64 #include <sys/uio.h>
65
66 #include "af_can.h"
67 #include "bcm.h"
68 #include "terminal.h"
69
70 #define U64_DATA(p) (*(unsigned long long*)(p)->data)
71
72 #define SETFNAME "sniffset."
73
74 /* flags */
75
76 #define ENABLE  1 /* by filter or user */
77 #define DISPLAY 2 /* is on the screen */
78 #define UPDATE  4 /* needs to be printed on the screen */
79 #define CLRSCR  8 /* clear screen in next loop */
80
81 /* flags testing & setting */
82
83 #define is_set(id, flag) (sniftab[id].flags & flag)
84 #define is_clr(id, flag) (!(sniftab[id].flags & flag))
85
86 #define do_set(id, flag) (sniftab[id].flags |= flag)
87 #define do_clr(id, flag) (sniftab[id].flags &= ~flag)
88
89 /* time defaults */
90
91 #define TIMEOUT 50 /* in 100ms */
92 #define HOLD    10 /* in 100ms */
93 #define LOOP     2 /* in 100ms */
94
95 #define MAXANI 8
96 const char anichar[MAXANI] = {'|', '/', '-', '\\', '|', '/', '-', '\\'};
97
98 #define ATTCOLOR ATTBOLD FGRED
99
100 #define STARTLINESTR "X  time    ID  data ... "
101
102 struct snif {
103     int flags;
104     long hold;
105     long timeout;
106     struct timeval laststamp;
107     struct timeval currstamp;
108     struct can_frame last;
109     struct can_frame current;
110     struct can_frame marker;
111     struct can_frame notch;
112 } sniftab[2048];
113
114
115 extern int optind, opterr, optopt;
116
117 static int running = 1;
118 static int clearscreen = 1;
119 static int notch = 0;
120 static long timeout = TIMEOUT;
121 static long hold = HOLD;
122 static long loop = LOOP;
123 static unsigned char binary = 0;
124 static unsigned char color  = 0;
125
126 void rx_setup (int fd, int id);
127 void rx_delete (int fd, int id);
128 void print_snifline(int id);
129 int handle_keyb(int fd);
130 int handle_bcm(int fd, long currcms);
131 int handle_timeo(int fd, long currcms);
132 void writesettings(char* name);
133 void readsettings(char* name, int sockfd);
134
135 void print_usage(char *prg)
136 {
137     const char manual [] = {
138         "commands that can be entered at runtime:\n"
139         "\n"
140         "q<ENTER>       - quit\n"
141         "b<ENTER>       - toggle binary / HEX-ASCII output\n"
142         "c<ENTER>       - toggle color mode\n"
143         "#<ENTER>       - notch currently marked/changed bits (can be used repeatedly)\n"
144         "*<ENTER>       - clear notched marked\n"
145         "rMYNAME<ENTER> - read settings file (filter/notch)\n"
146         "wMYNAME<ENTER> - write settings file (filter/notch)\n"
147         "+FILTER<ENTER> - add CAN-IDs to sniff\n"
148         "-FILTER<ENTER> - remove CAN-IDs to sniff\n"
149         "\n"
150         "FILTER can be a single CAN-ID or a CAN-ID/Bitmask:\n"
151         "+1F5<ENTER>    - add CAN-ID 0x1F5\n"
152         "-42E<ENTER>    - remove CAN-ID 0x42E\n"
153         "-42E7FF<ENTER> - remove CAN-ID 0x42E (using Bitmask)\n"
154         "-500700<ENTER> - remove CAN-IDs 0x500 - 0x5FF\n"
155         "+400600<ENTER> - add CAN-IDs 0x400 - 0x5FF\n"
156         "+000000<ENTER> - add all CAN-IDs\n"
157         "-000000<ENTER> - remove all CAN-IDs\n"
158         "\n"
159         "if (id & filter) == (sniff-id & filter) the action (+/-) is performed,\n"
160         "which is quite easy when the filter is 000\n"
161         "\n"
162     };
163
164     fprintf(stderr, "\nUsage: %s [can-interface]\n", prg);
165     fprintf(stderr, "Options: -m <mask>  (initial FILTER default 0x00000000)\n");
166     fprintf(stderr, "         -v <value> (initial FILTER default 0x00000000)\n");
167     fprintf(stderr, "         -q         (quiet - all IDs deactivated)\n");
168     fprintf(stderr, "         -r <name>  (read %sname from file)\n", SETFNAME);
169     fprintf(stderr, "         -b         (start with binary mode)\n");
170     fprintf(stderr, "         -c         (color changes)\n");
171     fprintf(stderr, "         -t <time>  (timeout for ID display [x100ms] default: %d)\n", TIMEOUT);
172     fprintf(stderr, "         -h <time>  (hold marker on changes [x100ms] default: %d)\n", HOLD);
173     fprintf(stderr, "         -l <time>  (loop time (display) [x100ms] default: %d)\n", LOOP);
174     fprintf(stderr, "\n");
175     fprintf(stderr, "%s", manual);
176 }
177
178 void sigterm(int signo)
179 {
180     running = 0;
181 }
182
183 int main(int argc, char **argv)
184 {
185     fd_set rdfs;
186     int s;
187     canid_t mask = 0;
188     canid_t value = 0;
189     long currcms = 0;
190     long lastcms = 0;
191     unsigned char quiet = 0;
192
193     int opt, ret;
194     struct timeval timeo, start_tv, tv;
195     struct sockaddr_can addr;
196     struct ifreq ifr;
197     int i;
198
199
200     signal(SIGTERM, sigterm);
201     signal(SIGHUP, sigterm);
202     signal(SIGINT, sigterm);
203
204     for (i=0; i < 2048 ;i++) /* default: check all CAN-IDs */
205         do_set(i, ENABLE);
206
207     while ((opt = getopt(argc, argv, "m:v:r:t:h:l:qbc")) != -1) {
208         switch (opt) {
209         case 'm':
210             sscanf(optarg, "%x", &mask);
211             break;
212
213         case 'v':
214             sscanf(optarg, "%x", &value);
215             break;
216
217         case 'r':
218             readsettings(optarg, 0); /* no BCM-setting here */
219             break;
220
221         case 't':
222             sscanf(optarg, "%ld", &timeout);
223             break;
224
225         case 'h':
226             sscanf(optarg, "%ld", &hold);
227             break;
228
229         case 'l':
230             sscanf(optarg, "%ld", &loop);
231             break;
232
233         case 'q':
234             quiet = 1;
235             break;
236
237         case 'b':
238             binary = 1;
239             break;
240
241         case 'c':
242             color = 1;
243             break;
244
245         case '?':
246             break;
247
248         default:
249             fprintf(stderr, "Unknown option %c\n", opt);
250             break;
251         }
252     }
253
254     if (optind == argc) {
255         print_usage(basename(argv[0]));
256         exit(0);
257     }
258         
259     if (mask || value) {
260         for (i=0; i < 2048 ;i++) {
261             if ((i & mask) ==  (value & mask))
262                 do_set(i, ENABLE);
263             else
264                 do_clr(i, ENABLE);
265         }
266     }
267
268     if (quiet)
269         for (i=0; i < 2048 ;i++)
270             do_clr(i, ENABLE);
271
272     if (strlen(argv[optind]) >= IFNAMSIZ) {
273         printf("name of CAN device '%s' is too long!\n", argv[optind]);
274         return 1;
275     }
276
277     if ((s = socket(PF_CAN, SOCK_DGRAM, CAN_BCM)) < 0) {
278         perror("socket");
279         return 1;
280     }
281
282     addr.can_family = AF_CAN;
283     strcpy(ifr.ifr_name, argv[optind]);
284     ioctl(s, SIOCGIFINDEX, &ifr);
285     addr.can_ifindex = ifr.ifr_ifindex;
286
287     if (connect(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
288         perror("bind");
289         return 1;
290     }
291
292     for (i=0; i < 2048 ;i++) /* initial BCM setup */
293         if (is_set(i, ENABLE))
294             rx_setup(s, i);
295
296     gettimeofday(&start_tv, NULL);
297     tv.tv_sec = tv.tv_usec = 0;
298
299     printf("%s", CSR_HIDE); /* hide cursor */
300
301     while (running) {
302
303         FD_ZERO(&rdfs);
304         FD_SET(0, &rdfs);
305         FD_SET(s, &rdfs);
306
307         timeo.tv_sec  = 0;
308         timeo.tv_usec = 100000 * loop;
309
310         if ((ret = select(s+1, &rdfs, NULL, NULL, &timeo)) < 0) {
311             //perror("select");
312             running = 0;
313             continue;
314         }
315
316         gettimeofday(&tv, NULL);
317         currcms = (tv.tv_sec - start_tv.tv_sec) * 10 + (tv.tv_usec / 100000);
318
319         if (FD_ISSET(0, &rdfs))
320             running &= handle_keyb(s);
321
322         if (FD_ISSET(s, &rdfs))
323             running &= handle_bcm(s, currcms);
324
325         if (currcms - lastcms >= loop) {
326                 running &= handle_timeo(s, currcms);
327                 lastcms = currcms;
328         }
329     }
330
331     printf("%s", CSR_SHOW); /* show cursor */
332
333     close(s);
334     return 0;
335 }
336
337 void rx_setup (int fd, int id){
338
339     struct {
340         struct bcm_msg_head msg_head;
341         struct can_frame frame;
342     } txmsg;
343
344     txmsg.msg_head.opcode  = RX_SETUP;
345     txmsg.msg_head.can_id  = id;
346     txmsg.msg_head.flags   = RX_CHECK_DLC;
347     txmsg.msg_head.ival1.tv_sec  = 0;
348     txmsg.msg_head.ival1.tv_usec = 0;
349     txmsg.msg_head.ival2.tv_sec  = 0;
350     txmsg.msg_head.ival2.tv_usec = 0;
351     txmsg.msg_head.nframes = 1;
352     U64_DATA(&txmsg.frame) = (__u64) 0xFFFFFFFFFFFFFFFFULL;
353
354     if (write(fd, &txmsg, sizeof(txmsg)) < 0)
355       perror("write");
356 };
357
358 void rx_delete (int fd, int id){
359
360     struct bcm_msg_head msg_head;
361
362     msg_head.opcode  = RX_DELETE;
363     msg_head.can_id  = id;
364     msg_head.nframes = 0;
365
366     if (write(fd, &msg_head, sizeof(msg_head)) < 0)
367       perror("write");
368 }
369
370 int handle_keyb(int fd){
371
372     char cmd [20] = {0};
373     int i;
374     unsigned int mask;
375     unsigned int value;
376
377     if (read(0, cmd, 19) > strlen("+123456\n"))
378         return 1; /* ignore */
379
380     if (strlen(cmd) > 0)
381         cmd[strlen(cmd)-1] = 0; /* chop off trailing newline */
382
383     switch (cmd[0]) {
384
385     case '+':
386     case '-':
387         sscanf(&cmd[1], "%x", &value);
388         if (strlen(&cmd[1]) > 3) {
389             mask = value & 0xFFF;
390             value >>= 12;
391         }
392         else
393             mask = 0x7FF;
394
395         if (cmd[0] == '+') {
396             for (i=0; i < 2048 ;i++) {
397                 if (((i & mask) == (value & mask)) && (is_clr(i, ENABLE))) {
398                     do_set(i, ENABLE);
399                     rx_setup(fd, i);
400                 }
401             }
402         }
403         else { /* '-' */
404             for (i=0; i < 2048 ;i++) {
405                 if (((i & mask) == (value & mask)) && (is_set(i, ENABLE))) {
406                     do_clr(i, ENABLE);
407                     rx_delete(fd, i);
408                 }
409             }
410         }
411         break;
412
413     case 'w' :
414         writesettings(&cmd[1]);
415         break;
416
417     case 'r' :
418         readsettings(&cmd[1], fd);
419         break;
420
421     case 'q' :
422         running = 0;
423         break;
424
425     case 'b' :
426         if (binary)
427             binary = 0;
428         else
429             binary = 1;
430
431         break;
432
433     case 'c' :
434         if (color)
435             color = 0;
436         else
437             color = 1;
438
439         break;
440
441     case '#' :
442         notch = 1;
443         break;
444
445     case '*' :
446         for (i=0; i < 2048; i++)
447             U64_DATA(&sniftab[i].notch) = (__u64) 0;
448         break;
449
450     default:
451         break;
452     }
453
454     clearscreen = 1;
455
456     return 1; /* ok */
457 };
458
459 int handle_bcm(int fd, long currcms){
460
461     int nbytes, id;
462
463     struct {
464         struct bcm_msg_head msg_head;
465         struct can_frame frame;
466     } bmsg;
467
468     if ((nbytes = read(fd, &bmsg, sizeof(bmsg))) < 0) {
469         perror("bcm read");
470         return 0; /* quit */
471     }
472
473     id = bmsg.msg_head.can_id;
474     ioctl(fd, SIOCGSTAMP, &sniftab[id].currstamp);
475
476     if (bmsg.msg_head.opcode != RX_CHANGED) {
477         printf("received strange BCM opcode %d!\n", bmsg.msg_head.opcode);
478         return 0; /* quit */
479     }
480
481     if (nbytes != sizeof(bmsg)) {
482         printf("received strange BCM data length %d!\n", nbytes);
483         return 0; /* quit */
484     }
485
486     sniftab[id].current = bmsg.frame;
487     U64_DATA(&sniftab[id].marker) |= 
488         U64_DATA(&sniftab[id].current) ^ U64_DATA(&sniftab[id].last);
489     sniftab[id].timeout = currcms + timeout;
490
491     if (is_clr(id, DISPLAY))
492         clearscreen = 1; /* new entry -> new drawing */
493
494     do_set(id, DISPLAY);
495     do_set(id, UPDATE);
496         
497     return 1; /* ok */
498 };
499
500 int handle_timeo(int fd, long currcms){
501
502     int i;
503     int force_redraw = 0;
504
505     if (clearscreen) {
506         char startline[80];
507         printf("%s%s", CLR_SCREEN, CSR_HOME);
508         snprintf(startline, 79, "< can-sniffer parameters: l=%ld h=%ld t=%ld >", loop, hold, timeout);
509         printf("%s%*s",STARTLINESTR, 79-strlen(STARTLINESTR), startline); 
510         force_redraw = 1;
511         clearscreen = 0;
512     }
513
514     if (notch) {
515         for (i=0; i < 2048; i++)
516             U64_DATA(&sniftab[i].notch) |= U64_DATA(&sniftab[i].marker);
517         notch = 0;
518     }
519
520     printf("%s", CSR_HOME);
521     printf("%c\n", anichar[currcms % MAXANI]); /* funny animation */
522
523     for (i=0; i < 2048; i++) {
524
525         if is_set(i, ENABLE) {
526
527             if is_set(i, DISPLAY) {
528
529                 if (is_set(i, UPDATE) || (force_redraw)){
530                     print_snifline(i);
531                     sniftab[i].hold = currcms + hold;
532                     do_clr(i, UPDATE);
533                 }
534                 else
535                     if ((sniftab[i].hold) && (sniftab[i].hold < currcms)) {
536                         U64_DATA(&sniftab[i].marker) = (__u64) 0;
537                         print_snifline(i);
538                         sniftab[i].hold = 0; /* disable update by hold */
539                     }
540                     else
541                         printf("%s", CSR_DOWN); /* skip my line */
542
543                 if (sniftab[i].timeout < currcms) {
544                     do_clr(i, DISPLAY);
545                     do_clr(i, UPDATE);
546                     clearscreen = 1; /* removed entry -> new drawing next time */
547                 }
548             }
549             sniftab[i].last      = sniftab[i].current;
550             sniftab[i].laststamp = sniftab[i].currstamp;
551         }
552     }
553
554     return 1; /* ok */
555
556 };
557
558 void print_snifline(int id){
559
560     long diffsec  = sniftab[id].currstamp.tv_sec  - sniftab[id].laststamp.tv_sec;
561     long diffusec = sniftab[id].currstamp.tv_usec - sniftab[id].laststamp.tv_usec;
562     int i,j;
563
564     if (diffusec < 0)
565         diffsec--, diffusec += 1000000;
566
567     if (diffsec < 0)
568         diffsec = diffusec = 0;
569
570     if (diffsec > 10)
571         diffsec = 9, diffusec = 999999;
572
573     printf("%ld.%06ld  %3x  ", diffsec, diffusec, id);
574
575     if (binary) {
576
577         for (i=0; i<sniftab[id].current.can_dlc; i++)
578             for (j=7; j>=0; j--)
579                 if ((color) && (sniftab[id].marker.data[i] & 1<<j) &&
580                     (!(sniftab[id].notch.data[i] & 1<<j)))
581                     if (sniftab[id].current.data[i] & 1<<j)
582                         printf("%s1%s", ATTCOLOR, ATTRESET);
583                     else
584                         printf("%s0%s", ATTCOLOR, ATTRESET);
585                 else
586                     if (sniftab[id].current.data[i] & 1<<j)
587                         putchar('1');
588                     else
589                         putchar('0');
590     }
591     else {
592
593         for (i=0; i<sniftab[id].current.can_dlc; i++)
594             if ((color) && (sniftab[id].marker.data[i]) && (!(sniftab[id].notch.data[i])))
595                 printf("%s%02X%s ", ATTCOLOR, sniftab[id].current.data[i], ATTRESET);
596             else
597                 printf("%02X ", sniftab[id].current.data[i]);
598
599         if (sniftab[id].current.can_dlc < 8)
600             printf("%*s", (8 - sniftab[id].current.can_dlc) * 3, "");
601
602         for (i=0; i<sniftab[id].current.can_dlc; i++)
603             if ((sniftab[id].current.data[i] > 0x1F) && 
604                 (sniftab[id].current.data[i] < 0x7F))
605                 if ((color) && (sniftab[id].marker.data[i]) && (!(sniftab[id].notch.data[i])))
606                     printf("%s%c%s", ATTCOLOR, sniftab[id].current.data[i], ATTRESET);
607                 else
608                     putchar(sniftab[id].current.data[i]);
609             else
610                 putchar('.');
611     }
612
613     putchar('\n');
614
615     U64_DATA(&sniftab[id].marker) = (__u64) 0;
616
617 };
618
619
620 void writesettings(char* name){
621
622     int fd;
623     char fname[30] = SETFNAME;
624     int i,j;
625     char buf[8]= {0};
626
627     strncat(fname, name, 29 - strlen(fname)); 
628     fd = open(fname,  O_WRONLY|O_CREAT, S_IRUSR|S_IWUSR|S_IRGRP|S_IROTH);
629     
630     if (fd > 0) {
631
632         for (i=0; i < 2048 ;i++) {
633             sprintf(buf, "<%03X>%c.", i, (is_set(i, ENABLE))?'1':'0');
634             write(fd, buf, 7);
635             for (j=0; j<8 ; j++){
636                 sprintf(buf, "%02X", sniftab[i].notch.data[j]);
637                 write(fd, buf, 2);
638             }
639             write(fd, "\n", 1);
640             /* 7 + 16 + 1 = 24 bytes per entry */ 
641         }
642         close(fd);
643     }
644     else
645         printf("unable to write setting file '%s'!\n", fname);
646 };
647
648 void readsettings(char* name, int sockfd){
649
650     int fd;
651     char fname[30] = SETFNAME;
652     char buf[25] = {0};
653     int i,j;
654
655     strncat(fname, name, 29 - strlen(fname)); 
656     fd = open(fname, O_RDONLY);
657     
658     if (fd > 0) {
659         if (!sockfd)
660             printf("reading setting file '%s' ... ", fname);
661
662         for (i=0; i < 2048 ;i++) {
663             if (read(fd, &buf, 24) == 24) {
664                 if (buf[5] & 1) {
665                     if (is_clr(i, ENABLE)) {
666                         do_set(i, ENABLE);
667                         if (sockfd)
668                             rx_setup(sockfd, i);
669                     }
670                 }
671                 else
672                     if (is_set(i, ENABLE)) {
673                         do_clr(i, ENABLE);
674                         if (sockfd)
675                             rx_delete(sockfd, i);
676                     }
677                 for (j=7; j>=0 ; j--){
678                     sniftab[i].notch.data[j] =
679                         (__u8) strtoul(&buf[2*j+7], (char **)NULL, 16) & 0xFF;
680                     buf[2*j+7] = 0; /* cut off each time */
681                 }
682             }
683             else {
684                 if (!sockfd)
685                     printf("was only able to read until index %d from setting file '%s'!\n",
686                            i, fname);
687             }
688         }
689     
690         if (!sockfd)
691             printf("done\n");
692
693         close(fd);
694     }
695     else
696         printf("unable to read setting file '%s'!\n", fname);
697 };