tools/vag-bap-sniffer.c

   1 /*
   2  * Copyright 2017 Max Staudt
   3  *
   4  * This program is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License 2 as published
   6  * by the Free Software Foundation.
   7  */
   8
   9 #include <ctype.h>
  10 #include <stdio.h>
  11 #include <stdint.h>
  12 #include <unistd.h>
  13 #include <stdlib.h>
  14 #include <string.h>
  15 #include <sys/types.h>
  16 #include <sys/socket.h>
  17 #include <linux/can.h>
  18 #include <linux/can/raw.h>
  19 #include <net/if.h>
  20 #include <sys/ioctl.h>
  21
  22 #include <ncurses.h>
  23
  24 #include "vag-bap.h"
  25
  26
  27
  28
  29 static int net_init(char *ifname)
  30 {
  31         int s;
  32         int recv_own_msgs;
  33         struct sockaddr_can addr;
  34         struct ifreq ifr;
  35
  36         s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
  37         if (s < 0) {
  38                 perror("socket");
  39                 exit(1);
  40         }
  41
  42         /* Convert interface name to index */
  43         memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
  44         strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
  45         if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
  46                 perror("SIOCGIFINDEX");
  47                 exit(1);
  48         }
  49
  50         /* Open the CAN interface */
  51         memset(&addr, 0, sizeof(addr));
  52         addr.can_family = AF_CAN;
  53         addr.can_ifindex = ifr.ifr_ifindex;
  54         if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
  55                 perror("bind");
  56                 return 0;
  57         }
  58
  59         recv_own_msgs = 1; /* 0 = disabled (default), 1 = enabled */
  60         setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,
  61                         &recv_own_msgs, sizeof(recv_own_msgs));
  62
  63         return s;
  64 }
  65
  66
  67
  68
  69
  70 int main(int argc, char **argv)
  71 {
  72         fd_set rfds;
  73         int s;
  74         unsigned can_id;
  75         struct BAP_RXer *bap;
  76         unsigned node_id;
  77
  78         if (argc < 4) {
  79                 printf("syntax: %s IFNAME CAN_ID bap_node \n", argv[0]);
  80                 return 1;
  81         }
  82
  83         can_id = strtoul(argv[2], NULL, 0);
  84         node_id = strtoul(argv[3], NULL, 0);
  85
  86         bap = vag_bap_rxer_alloc();
  87         if (!bap) {
  88                 printf("Out of memory allocating BAP struct.\n");
  89                 return 1;
  90         }
  91
  92         printf("Listening for CAN ID: %x and Node %d\n", can_id, node_id);
  93
  94
  95
  96         s = net_init(argv[1]);
  97
  98
  99         initscr();
 100
 101
 102         while (1) {
 103                 int retval;
 104
 105                 FD_ZERO(&rfds);
 106                 FD_SET(s, &rfds);
 107
 108                 retval = select(s+1, &rfds, NULL, NULL, NULL);
 109                 /* We currently rely on Linux timeout behavior here,
 110                  * i.e. the timeout now reflects the remaining time */
 111                 if (retval < 0) {
 112                         perror("select");
 113                         return 1;
 114                 } else if (!retval) {
 115                         /* Timeout, we NEED to check this first */
 116                 } else if (FD_ISSET(s, &rfds)) {
 117                         struct can_frame frame;
 118                         ssize_t ret;
 119
 120                         ret = read(s, &frame, sizeof(frame));
 121                         if (ret < 0) {
 122                                 perror("read from CAN socket");
 123                                 return 1;
 124                         }
 125
 126                         if (can_id == frame.can_id) {
 127                                 struct BAP_Frame *bap_frame;
 128                                 unsigned i;
 129
 130                                 bap_frame = vag_bap_handle_can_frame(bap, &frame);
 131                                 if (bap_frame && bap_frame->node == node_id) {
 132                                         mvprintw(bap_frame->port, 0, "");
 133
 134                                         printw("%u. %2i/%-2i .%c%02i --",
 135                                                 bap_frame->opcode,
 136                                                 bap_frame->node,
 137                                                 bap_frame->port,
 138                                                 bap_frame->is_multiframe ? 'm' : 's',
 139                                                 bap_frame->len);
 140
 141                                         /* Limit huge packets */
 142                                         if (bap_frame->len > 20) {
 143                                                 bap_frame->len = 20;
 144                                         }
 145
 146                                         /* Hex dump */
 147                                         for (i = 0; i < bap_frame->len; i++) {
 148                                                 if (!(i % 4)) {
 149                                                         printw(" ");
 150                                                 }
 151                                                 printw("%02x", (unsigned char)(bap_frame->data[i]));
 152                                         }
 153
 154                                         printw("\n");
 155
 156                                         /* ASCII dump */
 157                                         mvprintw(bap_frame->port, 64, "");
 158                                         for (i = 0; i < bap_frame->len; i++) {
 159                                                 unsigned char c = bap_frame->data[i];
 160                                                 if (!isprint(c) || c == '\n' || c == '\r') {
 161                                                         c = '.';
 162                                                 }
 163                                                 printw("%c", c);
 164                                         }
 165                                         printw("'");
 166                                         printw("\r");
 167
 168                                         refresh();
 169
 170                                         vag_bap_frame_free(bap_frame);
 171                                 }
 172                         }
 173
 174                         continue;
 175                 }
 176         }
 177
 178         endwin();
 179
 180         vag_bap_rxer_free(bap);
 181
 182         close(s);
 183
 184         return 0;
 185 }