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.
14 #include <sys/types.h>
15 #include <sys/socket.h>
16 #include <linux/can.h>
17 #include <linux/can/raw.h>
19 #include <sys/ioctl.h>
24 /* OSEK/VDX NM Level 0 */
29 NM_MAIN_LIMPHOME = 0x04,
32 NM_SLEEP_CANCEL = 0x00,
33 NM_SLEEP_REQUEST = 0x10,
38 typedef unsigned char NM_ID;
39 typedef unsigned char NM_State;
48 struct NM_Node *nodes;
54 static void can_tx(int socket, struct can_frame *frame)
58 ret = write(socket, frame, sizeof(*frame));
59 if (ret != sizeof(*frame)) {
60 perror("write to CAN socket");
67 static char* nm_main_to_string(NM_State state)
69 switch(state & NM_MAIN_MASK) {
76 case NM_MAIN_LIMPHOME:
83 static char* nm_sleep_to_string(NM_State state)
85 switch(state & NM_SLEEP_MASK) {
88 case NM_SLEEP_REQUEST:
91 return "Acknowledged";
99 static void nm_dump_all(struct NM_Main *nm)
104 printf(" Node | next | Main | Sleep\n");
105 printf("----------------------------------------\n");
107 for (id = 0; id < nm->max_nodes; id++) {
108 struct NM_Node *node = &nm->nodes[id];
110 if (node->state & NM_MAIN_MASK) {
111 printf(" %02x %02x % 9s %s\n",
114 nm_main_to_string(node->state),
115 nm_sleep_to_string(node->state));
125 static void nm_handle_can_frame(struct NM_Main *nm, struct can_frame *frame)
131 //printf("Received CAN frame from CAN ID %03x\n", frame->can_id);
133 if (frame->can_dlc < 2) {
134 printf("Skipping short frame from CAN ID %03x\n", frame->can_id);
139 if ((frame->can_id & ~0x1f) != 0x420) {
140 printf("Skipping non-NM from CAN ID %03x\n", frame->can_id);
144 printf("Received NM frame from CAN ID %03x\n", frame->can_id);
146 id = frame->can_id & 0x1f;
147 next = frame->data[0];
148 state = frame->data[1];
150 nm->nodes[id].next = next;
151 nm->nodes[id].state = state;
158 if (frame.data[0] == my_id) {
159 struct can_frame txframe = {.can_id = base_id + next_id,
161 .data = {next_id, 01, 00, 00, 00, 00, 00, 00},
163 can_tx(socket, &txframe);
167 if (ignore_counter > 0) {
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;
176 struct can_frame txframe = {.can_id = base_id + my_id,
178 .data = {my_id, 02, 01, 04, 00, 04, 00, 00},
180 can_tx(socket, &txframe);
191 static int net_init(char *ifname)
195 struct sockaddr_can addr;
197 struct can_filter fi;
199 s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
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");
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) {
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));
226 /* Handle only 32 NM IDs at CAN base ID 0x420 */
230 setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, &fi, sizeof(struct can_filter));
235 int main(int argc, char **argv)
237 struct NM_Node nodes[32] = {{0}};
238 struct NM_Main nm = {.max_nodes = 32, .nodes = nodes};
243 printf("syntax: %s IFNAME\n", argv[0]);
247 s = net_init(argv[1]);
255 if (select(s+1, &rdfs, NULL, NULL, NULL) < 0) {
260 if (FD_ISSET(s, &rdfs)) {
261 struct can_frame frame;
264 ret = read(s, &frame, sizeof(frame));
266 perror("recvfrom CAN socket");
270 nm_handle_can_frame(&nm, &frame);