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