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