tools/vag-bap-dump.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 "vag-bap.h"
  22
  23
  24
  25
  26 static int net_init(char *ifname)
  27 {
  28         int s;
  29         int recv_own_msgs;
  30         struct sockaddr_can addr;
  31         struct ifreq ifr;
  32
  33         s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
  34         if (s < 0) {
  35                 perror("socket");
  36                 exit(1);
  37         }
  38
  39         /* Convert interface name to index */
  40         memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
  41         strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
  42         if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
  43                 perror("SIOCGIFINDEX");
  44                 exit(1);
  45         }
  46
  47         /* Open the CAN interface */
  48         memset(&addr, 0, sizeof(addr));
  49         addr.can_family = AF_CAN;
  50         addr.can_ifindex = ifr.ifr_ifindex;
  51         if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
  52                 perror("bind");
  53                 return 0;
  54         }
  55
  56         recv_own_msgs = 1; /* 0 = disabled (default), 1 = enabled */
  57         setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,
  58                         &recv_own_msgs, sizeof(recv_own_msgs));
  59
  60         return s;
  61 }
  62
  63
  64
  65
  66
  67 int main(int argc, char **argv)
  68 {
  69         fd_set rfds;
  70         int s;
  71         int i;
  72         int can_id_count;
  73         unsigned *can_ids;
  74         struct BAP_RXer **baps;
  75
  76         if (argc < 3) {
  77                 printf("syntax: %s IFNAME CAN_ID [CAN_ID CAN_ID ...]\n", argv[0]);
  78                 return 1;
  79         }
  80
  81         can_id_count = argc - 2;
  82
  83         can_ids = calloc(can_id_count, sizeof(*can_ids));
  84         baps = calloc(can_id_count, sizeof(*baps));
  85         if (!can_ids || !baps) {
  86                 printf("Out of memory allocating meta structs.\n");
  87                 return 1;
  88         }
  89
  90         for (i = 0; i < can_id_count; i++) {
  91                 can_ids[i] = strtoul(argv[2 + i], NULL, 0);
  92                 baps[i] = vag_bap_rxer_alloc();
  93                 if (!baps[i]) {
  94                         printf("Out of memory allocating BAP struct.\n");
  95                         return 1;
  96                 }
  97
  98                 printf("Listening for CAN ID: %x\n", can_ids[i]);
  99         }
 100
 101
 102         s = net_init(argv[1]);
 103
 104         while (1) {
 105                 int retval;
 106
 107                 FD_ZERO(&rfds);
 108                 FD_SET(s, &rfds);
 109
 110                 retval = select(s+1, &rfds, NULL, NULL, NULL);
 111                 /* We currently rely on Linux timeout behavior here,
 112                  * i.e. the timeout now reflects the remaining time */
 113                 if (retval < 0) {
 114                         perror("select");
 115                         return 1;
 116                 } else if (!retval) {
 117                         /* Timeout, we NEED to check this first */
 118                 } else if (FD_ISSET(s, &rfds)) {
 119                         struct can_frame frame;
 120                         ssize_t ret;
 121
 122                         ret = read(s, &frame, sizeof(frame));
 123                         if (ret < 0) {
 124                                 perror("read from CAN socket");
 125                                 return 1;
 126                         }
 127
 128                         for (i = 0; i < can_id_count; i++) {
 129                                 if (can_ids[i] == frame.can_id) {
 130                                         struct BAP_Frame *bap_frame;
 131                                         struct BAP_RXer *bap = baps[i];
 132
 133                                         bap_frame = vag_bap_handle_can_frame(bap, &frame);
 134                                         if (bap_frame) {
 135                                                 printf("%03x:  ", frame.can_id);
 136                                                 vag_bap_frame_dump(bap_frame);
 137                                                 vag_bap_frame_free(bap_frame);
 138                                         }
 139
 140                                         break;
 141                                 }
 142                         }
 143
 144                         continue;
 145                 }
 146         }
 147
 148         /* TODO: Free */
 149
 150         close(s);
 151
 152         return 0;
 153 }