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 "On/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("Current system state:\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("Active node %02x:\n", id);
 112                         printf("  Next:  %02x\n", node->next);
 113                         printf("  Main:  %s\n", nm_main_to_string(node->state));
 114                         printf("  Sleep: %s\n", nm_sleep_to_string(node->state));
 115                         printf("\n");
 116                 }
 117         }
 118 }
 119
 120
 121
 122 static void nm_handle_can_frame(struct NM_Main *nm, struct can_frame *frame)
 123 {
 124         NM_ID id;
 125         NM_ID next;
 126         NM_State state;
 127
 128         //printf("Received CAN frame from CAN ID %03x\n", frame->can_id);
 129
 130         if (frame->can_dlc < 2) {
 131                 printf("Skipping short frame from CAN ID %03x\n", frame->can_id);
 132                 return;
 133         }
 134
 135
 136         if ((frame->can_id & ~0x1f) != 0x420) {
 137                 printf("Skipping non-NM from CAN ID %03x\n", frame->can_id);
 138                 return;
 139         }
 140
 141         printf("Received NM frame from CAN ID %03x\n", frame->can_id);
 142
 143         id = frame->can_id & 0x1f;
 144         next = frame->data[0];
 145         state = frame->data[1];
 146
 147         nm->nodes[id].next = next;
 148         nm->nodes[id].state = state;
 149
 150         nm_dump_all(nm);
 151
 152         /*
 153         switch (state) {
 154                 case 01:
 155                         if (frame.data[0] == my_id) {
 156                                 struct can_frame txframe = {.can_id = base_id + next_id,
 157                                                             .can_dlc = 8,
 158                                                             .data = {next_id, 01, 00, 00, 00, 00, 00, 00},
 159                                                            };
 160                                 can_tx(socket, &txframe);
 161                         }
 162                 break;
 163                 case 02:
 164                         if (ignore_counter > 0) {
 165                                 ignore_counter--;
 166                                 break;
 167                         }
 168                         if (next_id <= my_id
 169                                   ? frame.can_id - base_id < next_id
 170                                   : next_id == my_id || frame.can_id - base_id < next_id) {
 171                                 next_id = frame.can_id - base_id;
 172
 173                                 struct can_frame txframe = {.can_id = base_id + my_id,
 174                                                             .can_dlc = 8,
 175                                                             .data = {my_id, 02, 01, 04, 00, 04, 00, 00},
 176                                                            };
 177                                 can_tx(socket, &txframe);
 178                         }
 179                 break;
 180         }
 181         */
 182 }
 183
 184
 185
 186
 187
 188 static int net_init(char *ifname)
 189 {
 190         int s;
 191         int recv_own_msgs;
 192         struct sockaddr_can addr;
 193         struct ifreq ifr;
 194         struct can_filter fi;
 195
 196         s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
 197         if (s < 0) {
 198                 perror("socket");
 199                 exit(1);
 200         }
 201
 202         /* Convert interface name to index */
 203         memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
 204         strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
 205         if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
 206                 perror("SIOCGIFINDEX");
 207                 exit(1);
 208         }
 209
 210         /* Open the CAN interface */
 211         memset(&addr, 0, sizeof(addr));
 212         addr.can_family = AF_CAN;
 213         addr.can_ifindex = ifr.ifr_ifindex;
 214         if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
 215                 perror("bind");
 216                 return 0;
 217         }
 218
 219         recv_own_msgs = 1; /* 0 = disabled (default), 1 = enabled */
 220         setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,
 221                         &recv_own_msgs, sizeof(recv_own_msgs));
 222
 223         /* Handle only 32 NM IDs at CAN base ID 0x420 */
 224         fi.can_id   = 0x420;
 225         fi.can_mask = 0x7E0;
 226
 227         setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, &fi, sizeof(struct can_filter));
 228
 229         return s;
 230 }
 231
 232 int main(int argc, char **argv)
 233 {
 234         struct NM_Node nodes[32] = {{0}};
 235         struct NM_Main nm = {.max_nodes = 32, .nodes = nodes};
 236         fd_set rdfs;
 237         int s;
 238
 239         if (argc != 2) {
 240                 printf("syntax: %s IFNAME\n", argv[0]);
 241                 exit(1);
 242         }
 243
 244         s = net_init(argv[1]);
 245
 246         while (1) {
 247
 248                 FD_ZERO(&rdfs);
 249
 250                 FD_SET(s, &rdfs);
 251
 252                 if (select(s+1, &rdfs, NULL, NULL, NULL) < 0) {
 253                         perror("select");
 254                         return 1;
 255                 }
 256
 257                 if (FD_ISSET(s, &rdfs)) {
 258                         struct can_frame frame;
 259                         ssize_t ret;
 260
 261                         ret = read(s, &frame, sizeof(frame));
 262                         if (ret < 0) {
 263                                 perror("recvfrom CAN socket");
 264                                 exit(1);
 265                         }
 266
 267                         nm_handle_can_frame(&nm, &frame);
 268                         continue;
 269                 }
 270         }
 271
 272         return 0;
 273 }