vw-nm.c

   1 /*
   2  * Copyright 2015-2016 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 <assert.h>
  10
  11 #include <stdio.h>
  12 #include <stdint.h>
  13 #include <unistd.h>
  14 #include <stdlib.h>
  15 #include <string.h>
  16 #include <sys/types.h>
  17 #include <sys/socket.h>
  18 #include <linux/can.h>
  19 #include <linux/can/raw.h>
  20 #include <net/if.h>
  21 #include <sys/ioctl.h>
  22 #include <endian.h>
  23 #include <sys/time.h>
  24
  25
  26 #include "vw-nm.h"
  27 #include "vw-nm-tools.h"
  28
  29
  30
  31 static int nm_is_rx_frame_valid(struct NM_Main *nm, struct can_frame *frame)
  32 {
  33         if (frame->can_dlc < 2) {
  34                 printf("Skipping short frame from CAN ID %03x\n", frame->can_id);
  35                 return 0;
  36         }
  37
  38         if ((frame->can_id & ~(nm->max_nodes - 1)) != nm->can_base) {
  39                 printf("Skipping non-NM from CAN ID %03x\n", frame->can_id);
  40                 return 0;
  41         }
  42
  43         return 1;
  44 }
  45
  46
  47
  48
  49 static void nm_update_my_next_id(struct NM_Main *nm) {
  50         unsigned id = nm->my_id;
  51
  52         do {
  53                 NM_State state;
  54
  55                 id++;
  56                 if (id >= nm->max_nodes) {
  57                         id = 0;
  58                 }
  59
  60                 state = nm->nodes[id].state & NM_MAIN_MASK;
  61
  62                 if (state == NM_MAIN_ON || state == NM_MAIN_LOGIN) {
  63                         /* We skip limp home nodes */
  64                         nm->nodes[nm->my_id].next = id;
  65                         break;
  66                 }
  67         } while (id != nm->my_id);
  68
  69         if (nm->nodes[nm->my_id].next == nm->my_id) {
  70                 /* Uh oh, we're the only one left. */
  71
  72                 /* TODO */
  73                 nm->nodes[nm->my_id].state = NM_MAIN_LOGIN;
  74
  75                 /* TODO: Timeout 140ms (RCD 310) */
  76         }
  77 }
  78
  79
  80
  81 static void nm_handle_can_frame(struct NM_Main *nm, struct can_frame *frame)
  82 {
  83         NM_ID sender, next;
  84         NM_State state;
  85
  86         /* Is this a valid frame within our logical network? */
  87         if (!nm_is_rx_frame_valid(nm, frame)) {
  88                 return;
  89         }
  90
  91         printf("Received NM frame from CAN ID %03x\n", frame->can_id);
  92
  93
  94         /* Parse sender, its perceived successor, and its state */
  95         sender = frame->can_id & (nm->max_nodes - 1);
  96         next = frame->data[0];
  97         state = frame->data[1];
  98
  99         /* TODO: Validate state, it needs to be within the enum */
 100
 101         /* Skip our own frames */
 102         if (sender == nm->my_id) {
 103                 return;
 104         }
 105
 106         nm->nodes[sender].next = next;
 107         nm->nodes[sender].state = state;
 108
 109         switch (state & NM_MAIN_MASK) {
 110                 case NM_MAIN_ON:
 111                         if (next == nm->nodes[nm->my_id].next
 112                                 && nm->nodes[nm->my_id].next != nm->my_id) {
 113                                 /* sender doesn't know we exist */
 114
 115                                 nm->nodes[nm->my_id].state = NM_MAIN_LOGIN;
 116
 117                                 nm->tv.tv_sec = 0;
 118                                 nm->tv.tv_usec = 0;
 119                                 /* IMPORTANT: The caller needs to check for
 120                                  * timeouts first, i.e. no other NM frames
 121                                  * are received until our correcting login
 122                                  * has been sent.
 123                                  */
 124                         } else if (next == nm->nodes[nm->my_id].next) {
 125                                 /* where (nm->nodes[nm->my_id].next == nm->my_id) */
 126
 127                                 /* It can happen when:
 128                                  *  - our sent frames don't go anywhere
 129                                  *  - we just logged in and immediately
 130                                  *    afterwards another ECU sent a regular
 131                                  *    NM frame.
 132                                  */
 133
 134                                 /* Let's handle this just like a LOGIN, since
 135                                  * we're learning about a new device.
 136                                  * See case NM_MAIN_LOGIN below for details.
 137                                  */
 138
 139                                 nm_update_my_next_id(nm);
 140                                 nm->nodes[nm->my_id].state = NM_MAIN_ON;
 141                         } else if (next == nm->my_id) {
 142                                 /* It's our turn.
 143                                  * Reset the timeout so anyone we missed
 144                                  * can send its login frame to correct us.
 145                                  */
 146                                 nm->tv.tv_sec = 0;
 147                                 nm->tv.tv_usec = NM_USECS_MY_TURN;
 148                         } else {
 149                                 /* We just got some random ON message.
 150                                  * Reset the timer looking out for broken
 151                                  * connectivity.
 152                                  */
 153                                 nm->tv.tv_sec = 0;
 154                                 nm->tv.tv_usec = NM_USECS_NODE_AWOL;
 155                         }
 156                         break;
 157                 case NM_MAIN_LOGIN:
 158                         /* Note: sender != nm->my_id */
 159
 160                         nm_update_my_next_id(nm);
 161
 162                         /* We're not alone anymore, so let's change state. */
 163                         nm->nodes[nm->my_id].state = NM_MAIN_ON;
 164
 165                         /* We don't reset the timeout when somebody logs in,
 166                          * i.e. (next == sender).
 167                          * Instead, we'll simply include them in the next
 168                          * round. */
 169
 170                         /* Actually, when a login is done as a correction,
 171                          * we do reset the timeout.
 172                          */
 173                         if (next != sender) {
 174                                 nm->tv.tv_sec = 0;
 175                                 nm->tv.tv_usec = NM_USECS_NODE_AWOL;
 176                         }
 177                         break;
 178                 case NM_MAIN_LIMPHOME:
 179                         nm_update_my_next_id(nm);
 180         }
 181
 182         nm_dump_all(nm);
 183 }
 184
 185
 186
 187
 188
 189
 190 static void nm_buildframe(struct NM_Main *nm, struct can_frame *frame)
 191 {
 192         frame->can_id = nm->can_base + nm->my_id;
 193         frame->can_dlc = 2;
 194         frame->data[0] = nm->nodes[nm->my_id].next;
 195         frame->data[1] = nm->nodes[nm->my_id].state;
 196 }
 197
 198
 199
 200
 201 static void nm_timeout_callback(struct NM_Main *nm, struct can_frame *frame)
 202 {
 203         nm->tv.tv_sec = 0;
 204         nm->tv.tv_usec = NM_USECS_NODE_AWOL;
 205
 206         nm_buildframe(nm, frame);
 207 }
 208
 209
 210
 211
 212 static void nm_start(struct NM_Main *nm, struct can_frame *frame)
 213 {
 214         nm->tv.tv_sec = 0;
 215         nm->tv.tv_usec = 50000;
 216
 217
 218
 219         nm->nodes[nm->my_id].next = nm->my_id;
 220         nm->nodes[nm->my_id].state = NM_MAIN_LOGIN;
 221
 222         nm_buildframe(nm, frame);
 223 }
 224
 225
 226
 227
 228 static int net_init(char *ifname)
 229 {
 230         int s;
 231         int recv_own_msgs;
 232         struct sockaddr_can addr;
 233         struct ifreq ifr;
 234         struct can_filter fi;
 235
 236         s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
 237         if (s < 0) {
 238                 perror("socket");
 239                 exit(1);
 240         }
 241
 242         /* Convert interface name to index */
 243         memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
 244         strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
 245         if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
 246                 perror("SIOCGIFINDEX");
 247                 exit(1);
 248         }
 249
 250         /* Open the CAN interface */
 251         memset(&addr, 0, sizeof(addr));
 252         addr.can_family = AF_CAN;
 253         addr.can_ifindex = ifr.ifr_ifindex;
 254         if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
 255                 perror("bind");
 256                 return 0;
 257         }
 258
 259         recv_own_msgs = 1; /* 0 = disabled (default), 1 = enabled */
 260         setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,
 261                         &recv_own_msgs, sizeof(recv_own_msgs));
 262
 263         /* Handle only 32 NM IDs at CAN base ID 0x420 */
 264         fi.can_id   = 0x420;
 265         fi.can_mask = 0x7E0;
 266
 267         setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, &fi, sizeof(struct can_filter));
 268
 269         return s;
 270 }
 271
 272
 273 int main(int argc, char **argv)
 274 {
 275         struct NM_Main *nm;
 276         fd_set rdfs;
 277         int s;
 278
 279         if (argc != 2) {
 280                 printf("syntax: %s IFNAME\n", argv[0]);
 281                 return 1;
 282         }
 283
 284         nm = nm_alloc(5, 0x0b, 0x420);
 285         if (!nm) {
 286                 printf("Out of memory allocating NM struct.\n");
 287                 return 1;
 288         }
 289
 290         s = net_init(argv[1]);
 291
 292         /* Stir up the hornet's nest */
 293         if (1) {
 294                 struct can_frame frame;
 295
 296                 nm_start(nm, &frame);
 297                 can_tx(s, &frame);
 298         }
 299
 300         while (1) {
 301                 int retval;
 302
 303                 FD_ZERO(&rdfs);
 304                 FD_SET(s, &rdfs);
 305
 306                 retval = select(s+1, &rdfs, NULL, NULL, &nm->tv);
 307                 /* We currently rely on Linux timeout behavior here,
 308                  * i.e. the timeout now reflects the remaining time */
 309                 if (retval < 0) {
 310                         perror("select");
 311                         return 1;
 312                 } else if (!retval) {
 313                         /* Timeout, we NEED to check this first */
 314                         struct can_frame frame;
 315
 316                         nm_timeout_callback(nm, &frame);
 317                         can_tx(s, &frame);
 318                 } else if (FD_ISSET(s, &rdfs)) {
 319                         struct can_frame frame;
 320                         ssize_t ret;
 321
 322                         ret = read(s, &frame, sizeof(frame));
 323                         if (ret < 0) {
 324                                 perror("recvfrom CAN socket");
 325                                 return 1;
 326                         }
 327
 328                         nm_handle_can_frame(nm, &frame);
 329                         continue;
 330                 }
 331         }
 332
 333         nm_free(nm);
 334
 335         return 0;
 336 }