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