2 * Copyright 2015-2016 Max Staudt
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.
16 #include <sys/types.h>
17 #include <sys/socket.h>
18 #include <linux/can.h>
19 #include <linux/can/raw.h>
21 #include <sys/ioctl.h>
27 #include "vw-nm-tools.h"
35 static int nm_handle_can_frame(struct NM_Main *nm, struct can_frame *frame)
42 if (frame->can_dlc < 2) {
43 printf("Skipping short frame from CAN ID %03x\n", frame->can_id);
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);
53 printf("Received NM frame from CAN ID %03x\n", frame->can_id);
55 id = frame->can_id & (nm->max_nodes - 1);
56 next = frame->data[0];
57 state = frame->data[1];
59 nm->nodes[id].next = (state & NM_MAIN_MASK) == NM_MAIN_ON
62 nm->nodes[id].state = state;
64 switch (state & NM_MAIN_MASK) {
66 if (next == nm->my_id) {
71 if (id == nm->my_id) {
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;
90 static NM_ID nm_my_next_id(struct NM_Main *nm) {
94 || (nm->nodes[nm->my_id].state & NM_MAIN_MASK) != NM_MAIN_ON) {
100 struct NM_Node *node;
103 if (id >= nm->max_nodes) {
106 node = &nm->nodes[id];
108 if ((node->state & NM_MAIN_MASK) == NM_MAIN_ON) {
111 } while (id != nm->my_id);
113 /* This is never reached */
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);
124 frame->can_id = nm->can_base + nm->my_id;
126 frame->data[0] = nm->nodes[nm->my_id].next;
127 frame->data[1] = NM_MAIN_ON;
133 static int net_init(char *ifname)
137 struct sockaddr_can addr;
139 struct can_filter fi;
141 s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
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");
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) {
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));
168 /* Handle only 32 NM IDs at CAN base ID 0x420 */
172 setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, &fi, sizeof(struct can_filter));
178 int main(int argc, char **argv)
181 struct timeval tv, *next_tv = NULL;
186 printf("syntax: %s IFNAME\n", argv[0]);
190 nm = nm_alloc(5, 0x0b, 0x420);
192 printf("Out of memory allocating NM struct.\n");
196 s = net_init(argv[1]);
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 */
210 } else if (!retval) {
212 struct can_frame frame;
214 nm_timeout_callback(nm, &frame);
218 } else if (FD_ISSET(s, &rdfs)) {
219 struct can_frame frame;
222 ret = read(s, &frame, sizeof(frame));
224 perror("recvfrom CAN socket");
228 if (nm_handle_can_frame(nm, &frame)) {