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 <stdio.h>
  10 #include <stdint.h>
  11 #include <unistd.h>
  12 #include <stdlib.h>
  13 #include <string.h>
  14 #include <sys/types.h>
  15 #include <sys/socket.h>
  16 #include <linux/can.h>
  17 #include <linux/can/raw.h>
  18 #include <net/if.h>
  19 #include <sys/ioctl.h>
  20 #include <endian.h>
  21
  22
  23 enum {
  24         /* OSEK/VDX NM Level 0 */
  25
  26         NM_MAIN_OFF      = 0x00,
  27         NM_MAIN_ON       = 0x01,
  28         NM_MAIN_LOGIN    = 0x02,
  29         NM_MAIN_LIMPHOME = 0x04,
  30         NM_MAIN_MASK     = 0x0F,
  31
  32         NM_SLEEP_CANCEL  = 0x00,
  33         NM_SLEEP_REQUEST = 0x10,
  34         NM_SLEEP_ACK     = 0x20,
  35         NM_SLEEP_MASK    = 0xF0,
  36 };
  37
  38 typedef unsigned char NM_ID;
  39 typedef unsigned char NM_State;
  40
  41 struct NM_Node {
  42         NM_ID next;
  43         NM_State state;
  44 };
  45
  46 struct NM_Main {
  47         unsigned max_nodes;
  48         struct NM_Node *nodes;
  49 };
  50
  51
  52
  53
  54 static void can_tx(int socket, struct can_frame *frame)
  55 {
  56         ssize_t ret;
  57
  58         ret = write(socket, frame, sizeof(*frame));
  59         if (ret != sizeof(*frame)) {
  60                 perror("write to CAN socket");
  61                 exit(1);
  62         }
  63 }
  64
  65
  66
  67 static char* nm_main_to_string(NM_State state)
  68 {
  69         switch(state & NM_MAIN_MASK) {
  70                 case NM_MAIN_OFF:
  71                         return "Off";
  72                 case NM_MAIN_ON:
  73                         return "Ready";
  74                 case NM_MAIN_LOGIN:
  75                         return "Login";
  76                 case NM_MAIN_LIMPHOME:
  77                         return "Limp home";
  78                 default:
  79                         return "Unknown?";
  80         }
  81 }
  82
  83 static char* nm_sleep_to_string(NM_State state)
  84 {
  85         switch(state & NM_SLEEP_MASK) {
  86                 case NM_SLEEP_CANCEL:
  87                         return "No";
  88                 case NM_SLEEP_REQUEST:
  89                         return "Request";
  90                 case NM_SLEEP_ACK:
  91                         return "Acknowledged";
  92                 default:
  93                         return "Unknown?";
  94         }
  95 }
  96
  97
  98
  99 static void nm_dump_all(struct NM_Main *nm)
 100 {
 101         unsigned id;
 102
 103         printf("\n");
 104         printf(" Node | next | Main      | Sleep\n");
 105         printf("----------------------------------------\n");
 106
 107         for (id = 0; id < nm->max_nodes; id++) {
 108                 struct NM_Node *node = &nm->nodes[id];
 109
 110                 if (node->state & NM_MAIN_MASK) {
 111                         printf("  %02x     %02x    % 9s   %s\n",
 112                                 id,
 113                                 node->next,
 114                                 nm_main_to_string(node->state),
 115                                 nm_sleep_to_string(node->state));
 116
 117                 }
 118         }
 119
 120         printf("\n");
 121 }
 122
 123
 124
 125 static void nm_handle_can_frame(struct NM_Main *nm, struct can_frame *frame)
 126 {
 127         NM_ID id;
 128         NM_ID next;
 129         NM_State state;
 130
 131         //printf("Received CAN frame from CAN ID %03x\n", frame->can_id);
 132
 133         if (frame->can_dlc < 2) {
 134                 printf("Skipping short frame from CAN ID %03x\n", frame->can_id);
 135                 return;
 136         }
 137
 138
 139         if ((frame->can_id & ~0x1f) != 0x420) {
 140                 printf("Skipping non-NM from CAN ID %03x\n", frame->can_id);
 141                 return;
 142         }
 143
 144         printf("Received NM frame from CAN ID %03x\n", frame->can_id);
 145
 146         id = frame->can_id & 0x1f;
 147         next = frame->data[0];
 148         state = frame->data[1];
 149
 150         nm->nodes[id].next = next;
 151         nm->nodes[id].state = state;
 152
 153         nm_dump_all(nm);
 154
 155         /*
 156         switch (state) {
 157                 case 01:
 158                         if (frame.data[0] == my_id) {
 159                                 struct can_frame txframe = {.can_id = base_id + next_id,
 160                                                             .can_dlc = 8,
 161                                                             .data = {next_id, 01, 00, 00, 00, 00, 00, 00},
 162                                                            };
 163                                 can_tx(socket, &txframe);
 164                         }
 165                 break;
 166                 case 02:
 167                         if (ignore_counter > 0) {
 168                                 ignore_counter--;
 169                                 break;
 170                         }
 171                         if (next_id <= my_id
 172                                   ? frame.can_id - base_id < next_id
 173                                   : next_id == my_id || frame.can_id - base_id < next_id) {
 174                                 next_id = frame.can_id - base_id;
 175
 176                                 struct can_frame txframe = {.can_id = base_id + my_id,
 177                                                             .can_dlc = 8,
 178                                                             .data = {my_id, 02, 01, 04, 00, 04, 00, 00},
 179                                                            };
 180                                 can_tx(socket, &txframe);
 181                         }
 182                 break;
 183         }
 184         */
 185 }
 186
 187
 188
 189
 190
 191 static int net_init(char *ifname)
 192 {
 193         int s;
 194         int recv_own_msgs;
 195         struct sockaddr_can addr;
 196         struct ifreq ifr;
 197         struct can_filter fi;
 198
 199         s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
 200         if (s < 0) {
 201                 perror("socket");
 202                 exit(1);
 203         }
 204
 205         /* Convert interface name to index */
 206         memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
 207         strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
 208         if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
 209                 perror("SIOCGIFINDEX");
 210                 exit(1);
 211         }
 212
 213         /* Open the CAN interface */
 214         memset(&addr, 0, sizeof(addr));
 215         addr.can_family = AF_CAN;
 216         addr.can_ifindex = ifr.ifr_ifindex;
 217         if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
 218                 perror("bind");
 219                 return 0;
 220         }
 221
 222         recv_own_msgs = 1; /* 0 = disabled (default), 1 = enabled */
 223         setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,
 224                         &recv_own_msgs, sizeof(recv_own_msgs));
 225
 226         /* Handle only 32 NM IDs at CAN base ID 0x420 */
 227         fi.can_id   = 0x420;
 228         fi.can_mask = 0x7E0;
 229
 230         setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, &fi, sizeof(struct can_filter));
 231
 232         return s;
 233 }
 234
 235 int main(int argc, char **argv)
 236 {
 237         struct NM_Node nodes[32] = {{0}};
 238         struct NM_Main nm = {.max_nodes = 32, .nodes = nodes};
 239         fd_set rdfs;
 240         int s;
 241
 242         if (argc != 2) {
 243                 printf("syntax: %s IFNAME\n", argv[0]);
 244                 exit(1);
 245         }
 246
 247         s = net_init(argv[1]);
 248
 249         while (1) {
 250
 251                 FD_ZERO(&rdfs);
 252
 253                 FD_SET(s, &rdfs);
 254
 255                 if (select(s+1, &rdfs, NULL, NULL, NULL) < 0) {
 256                         perror("select");
 257                         return 1;
 258                 }
 259
 260                 if (FD_ISSET(s, &rdfs)) {
 261                         struct can_frame frame;
 262                         ssize_t ret;
 263
 264                         ret = read(s, &frame, sizeof(frame));
 265                         if (ret < 0) {
 266                                 perror("recvfrom CAN socket");
 267                                 exit(1);
 268                         }
 269
 270                         nm_handle_can_frame(&nm, &frame);
 271                         continue;
 272                 }
 273         }
 274
 275         return 0;
 276 }