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
  34
  35 static int nm_handle_can_frame(struct NM_Main *nm, struct can_frame *frame)
  36 {
  37         NM_ID id;
  38         NM_ID next;
  39         NM_State state;
  40         int its_my_turn = 0;
  41
  42         if (frame->can_dlc < 2) {
  43                 printf("Skipping short frame from CAN ID %03x\n", frame->can_id);
  44                 return 0;
  45         }
  46
  47
  48         if ((frame->can_id & ~(nm->max_nodes - 1)) != nm->can_base) {
  49                 printf("Skipping non-NM from CAN ID %03x\n", frame->can_id);
  50                 return 0;
  51         }
  52
  53         printf("Received NM frame from CAN ID %03x\n", frame->can_id);
  54
  55         id = frame->can_id & (nm->max_nodes - 1);
  56         next = frame->data[0];
  57         state = frame->data[1];
  58
  59         nm->nodes[id].next = (state & NM_MAIN_MASK) == NM_MAIN_ON
  60                                 ? next
  61                                 : 0xff;
  62         nm->nodes[id].state = state;
  63
  64         switch (state & NM_MAIN_MASK) {
  65                 case NM_MAIN_ON:
  66                         if (next == nm->my_id) {
  67                                 its_my_turn = 1;
  68                         }
  69                         break;
  70                 case NM_MAIN_LOGIN:
  71                         if (id == nm->my_id) {
  72                                 break;
  73                         }
  74                         printf("Handling LOGIN\n");
  75                         printf("Testing %x < %x\n", id, nm->nodes[nm->my_id].next);
  76                         if (id < nm->nodes[nm->my_id].next) {
  77                                 nm->nodes[nm->my_id].next = id;
  78                         }
  79                         break;
  80         }
  81
  82         nm_dump_all(nm);
  83
  84         return its_my_turn;
  85 }
  86
  87
  88
  89
  90 static NM_ID nm_my_next_id(struct NM_Main *nm) {
  91         unsigned id;
  92
  93         if (nm->max_nodes < 2
  94                 || (nm->nodes[nm->my_id].state & NM_MAIN_MASK) != NM_MAIN_ON) {
  95                 assert(0);
  96         }
  97
  98         id = nm->my_id;
  99         do {
 100                 struct NM_Node *node;
 101
 102                 id++;
 103                 if (id >= nm->max_nodes) {
 104                         id = 0;
 105                 }
 106                 node = &nm->nodes[id];
 107
 108                 if ((node->state & NM_MAIN_MASK) == NM_MAIN_ON) {
 109                         return id;
 110                 }
 111         } while (id != nm->my_id);
 112
 113         /* This is never reached */
 114         assert(0);
 115         return -1;
 116 }
 117
 118
 119
 120
 121 static void nm_timeout_callback(struct NM_Main *nm, struct can_frame *frame) {
 122         nm->nodes[nm->my_id].next = nm_my_next_id(nm);
 123
 124         frame->can_id = nm->can_base + nm->my_id;
 125         frame->can_dlc = 2;
 126         frame->data[0] = nm->nodes[nm->my_id].next;
 127         frame->data[1] = NM_MAIN_ON;
 128 }
 129
 130
 131
 132
 133 static int net_init(char *ifname)
 134 {
 135         int s;
 136         int recv_own_msgs;
 137         struct sockaddr_can addr;
 138         struct ifreq ifr;
 139         struct can_filter fi;
 140
 141         s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
 142         if (s < 0) {
 143                 perror("socket");
 144                 exit(1);
 145         }
 146
 147         /* Convert interface name to index */
 148         memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
 149         strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
 150         if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
 151                 perror("SIOCGIFINDEX");
 152                 exit(1);
 153         }
 154
 155         /* Open the CAN interface */
 156         memset(&addr, 0, sizeof(addr));
 157         addr.can_family = AF_CAN;
 158         addr.can_ifindex = ifr.ifr_ifindex;
 159         if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
 160                 perror("bind");
 161                 return 0;
 162         }
 163
 164         recv_own_msgs = 1; /* 0 = disabled (default), 1 = enabled */
 165         setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,
 166                         &recv_own_msgs, sizeof(recv_own_msgs));
 167
 168         /* Handle only 32 NM IDs at CAN base ID 0x420 */
 169         fi.can_id   = 0x420;
 170         fi.can_mask = 0x7E0;
 171
 172         setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, &fi, sizeof(struct can_filter));
 173
 174         return s;
 175 }
 176
 177
 178 int main(int argc, char **argv)
 179 {
 180         struct NM_Main *nm;
 181         struct timeval tv, *next_tv = NULL;
 182         fd_set rdfs;
 183         int s;
 184
 185         if (argc != 2) {
 186                 printf("syntax: %s IFNAME\n", argv[0]);
 187                 return 1;
 188         }
 189
 190         nm = nm_alloc(5, 0x0b, 0x420);
 191         if (!nm) {
 192                 printf("Out of memory allocating NM struct.\n");
 193                 return 1;
 194         }
 195
 196         s = net_init(argv[1]);
 197
 198         while (1) {
 199                 int retval;
 200
 201                 FD_ZERO(&rdfs);
 202                 FD_SET(s, &rdfs);
 203
 204                 retval = select(s+1, &rdfs, NULL, NULL, next_tv);
 205                 /* We currently rely on Linux timeout behavior here,
 206                  * i.e. the timeout now reflects the remaining time */
 207                 if (retval < 0) {
 208                         perror("select");
 209                         return 1;
 210                 } else if (!retval) {
 211                         /* Timeout */
 212                         struct can_frame frame;
 213
 214                         nm_timeout_callback(nm, &frame);
 215                         can_tx(s, &frame);
 216
 217                         next_tv = NULL;
 218                 } else if (FD_ISSET(s, &rdfs)) {
 219                         struct can_frame frame;
 220                         ssize_t ret;
 221
 222                         ret = read(s, &frame, sizeof(frame));
 223                         if (ret < 0) {
 224                                 perror("recvfrom CAN socket");
 225                                 return 1;
 226                         }
 227
 228                         if (nm_handle_can_frame(nm, &frame)) {
 229                                 tv.tv_sec = 0;
 230                                 tv.tv_usec = 400000;
 231                                 next_tv = &tv;
 232                         }
 233                         continue;
 234                 }
 235         }
 236
 237         nm_free(nm);
 238
 239         return 0;
 240 }