vw-nm.c

   1 /*
   2  * Copyright 2015 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 #include <endian.h>
  21
  22
  23 static int base_id = 0x420;
  24 static int my_id = 0x1a;
  25 static int next_id = 0x19;  //myid
  26
  27 static int ignore_counter = 10;
  28
  29 static void on_nm_frame(int socket)
  30 {
  31   struct can_frame frame;
  32   struct sockaddr_can addr;
  33   int ret;
  34   socklen_t len;
  35
  36   ret = recvfrom(socket, &frame, sizeof(struct can_frame), 0,
  37                   (struct sockaddr *)&addr, &len);
  38   if (ret < 0) {
  39     perror("recvfrom");
  40     exit(1);
  41   }
  42
  43   if (frame.can_id >> 5 == 0x420 >> 5) {
  44     printf("Received NM frame from %03x\n", frame.can_id);
  45   }
  46
  47   if (frame.can_id == base_id + my_id) {
  48     printf("Skipping my own NM frame.\n");
  49     return;
  50   }
  51
  52   switch (frame.data[1]) {
  53     case 01:
  54       if (frame.data[0] == my_id) {
  55         struct can_frame txframe = {.can_id = base_id + next_id,
  56                                     .can_dlc = 8,
  57                                     .data = {next_id, 01, 00, 00, 00, 00, 00, 00},
  58                                    };
  59         ssize_t tmp = write(socket, &txframe, sizeof(txframe));
  60         if (tmp != sizeof(txframe)) {
  61                 perror("write socket");
  62                 //return 1;
  63         }
  64       }
  65       break;
  66     case 02:
  67       if (ignore_counter > 0) {
  68         ignore_counter--;
  69         break;
  70       }
  71       if (next_id <= my_id
  72           ? frame.can_id - base_id < next_id
  73           : next_id == my_id || frame.can_id - base_id < next_id) {
  74         next_id = frame.can_id - base_id;
  75
  76         struct can_frame txframe = {.can_id = base_id + my_id,
  77                                     .can_dlc = 8,
  78                                     .data = {my_id, 02, 01, 04, 00, 04, 00, 00},
  79                                    };
  80         ssize_t tmp = write(socket, &txframe, sizeof(txframe));
  81         if (tmp != sizeof(txframe)) {
  82                 perror("write socket");
  83                 //return 1;
  84         }
  85       }
  86       break;
  87   }
  88 }
  89
  90
  91
  92
  93
  94 static int net_init(char *ifname)
  95 {
  96         int s;
  97         int recv_own_msgs;
  98         struct sockaddr_can addr;
  99         struct ifreq ifr;
 100
 101         s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
 102         if (s < 0) {
 103                 perror("socket");
 104                 exit(1);
 105         }
 106
 107         memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
 108         strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
 109         if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
 110                 perror("SIOCGIFINDEX");
 111                 exit(1);
 112         }
 113
 114         memset(&addr, 0, sizeof(addr));
 115         addr.can_family = AF_CAN;
 116         addr.can_ifindex = ifr.ifr_ifindex;
 117         if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
 118                 perror("bind");
 119                 return 0;
 120         }
 121
 122         recv_own_msgs = 0; /* 0 = disabled (default), 1 = enabled */
 123         setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,
 124                         &recv_own_msgs, sizeof(recv_own_msgs));
 125
 126         return s;
 127 }
 128
 129 int main(int argc, char **argv)
 130 {
 131         fd_set rdfs;
 132         int s;
 133
 134         if (argc != 2) {
 135                 printf("syntax: %s IFNAME\n", argv[0]);
 136                 exit(1);
 137         }
 138
 139         s = net_init(argv[1]);
 140
 141         while (1) {
 142
 143                 FD_ZERO(&rdfs);
 144
 145                 FD_SET(s, &rdfs);
 146
 147                 if (select(s+1, &rdfs, NULL, NULL, NULL) < 0) {
 148                         perror("select");
 149                         return 1;
 150                 }
 151
 152                 if (FD_ISSET(s, &rdfs)) {
 153                         on_nm_frame(s);
 154                         continue;
 155                 }
 156         }
 157
 158         return 0;
 159 }