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