/*
- * Copyright 2015 Max Staudt
+ * Copyright 2015-2016 Max Staudt
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License 2 as published
#include <endian.h>
-static int base_id = 0x420;
-static int my_id = 0x1a;
-static int next_id = 0x19; //myid
+enum {
+ /* OSEK/VDX NM Level 0 */
-static int ignore_counter = 10;
+ NM_MAIN_OFF = 0x00,
+ NM_MAIN_ON = 0x01,
+ NM_MAIN_LOGIN = 0x02,
+ NM_MAIN_LIMPHOME = 0x04,
+ NM_MAIN_MASK = 0x0F,
-static void on_nm_frame(int socket)
+ NM_SLEEP_CANCEL = 0x00,
+ NM_SLEEP_REQUEST = 0x10,
+ NM_SLEEP_ACK = 0x20,
+ NM_SLEEP_MASK = 0xF0,
+};
+
+typedef unsigned char NM_ID;
+typedef unsigned char NM_State;
+
+struct NM_Node {
+ NM_ID next;
+ NM_State state;
+};
+
+struct NM_Main {
+ unsigned max_nodes;
+ struct NM_Node *nodes;
+};
+
+
+
+
+static void can_tx(int socket, struct can_frame *frame)
+{
+ ssize_t ret;
+
+ ret = write(socket, frame, sizeof(*frame));
+ if (ret != sizeof(*frame)) {
+ perror("write to CAN socket");
+ exit(1);
+ }
+}
+
+
+
+static char* nm_main_to_string(NM_State state)
+{
+ switch(state & NM_MAIN_MASK) {
+ case NM_MAIN_OFF:
+ return "Off";
+ case NM_MAIN_ON:
+ return "On/Ready";
+ case NM_MAIN_LOGIN:
+ return "Login";
+ case NM_MAIN_LIMPHOME:
+ return "Limp home";
+ default:
+ return "Unknown?";
+ }
+}
+
+static char* nm_sleep_to_string(NM_State state)
+{
+ switch(state & NM_SLEEP_MASK) {
+ case NM_SLEEP_CANCEL:
+ return "No";
+ case NM_SLEEP_REQUEST:
+ return "Request";
+ case NM_SLEEP_ACK:
+ return "Acknowledged";
+ default:
+ return "Unknown?";
+ }
+}
+
+
+
+static void nm_dump_all(struct NM_Main *nm)
{
- struct can_frame frame;
- struct sockaddr_can addr;
- int ret;
- socklen_t len;
-
- ret = recvfrom(socket, &frame, sizeof(struct can_frame), 0,
- (struct sockaddr *)&addr, &len);
- if (ret < 0) {
- perror("recvfrom");
- exit(1);
- }
-
- if (frame.can_id >> 5 == 0x420 >> 5) {
- printf("Received NM frame from %03x\n", frame.can_id);
- }
-
- if (frame.can_id == base_id + my_id) {
- printf("Skipping my own NM frame.\n");
- return;
- }
-
- switch (frame.data[1]) {
- case 01:
- if (frame.data[0] == my_id) {
- struct can_frame txframe = {.can_id = base_id + next_id,
- .can_dlc = 8,
- .data = {next_id, 01, 00, 00, 00, 00, 00, 00},
- };
- ssize_t tmp = write(socket, &txframe, sizeof(txframe));
- if (tmp != sizeof(txframe)) {
- perror("write socket");
- //return 1;
+ unsigned id;
+
+ printf("\n");
+ printf("Current system state:\n");
+ printf("\n");
+
+ for (id = 0; id < nm->max_nodes; id++) {
+ struct NM_Node *node = &nm->nodes[id];
+
+ if (node->state & NM_MAIN_MASK) {
+ printf("Active node %02x:\n", id);
+ printf(" Next: %02x\n", node->next);
+ printf(" Main: %s\n", nm_main_to_string(node->state));
+ printf(" Sleep: %s\n", nm_sleep_to_string(node->state));
+ printf("\n");
+ }
}
- }
- break;
- case 02:
- if (ignore_counter > 0) {
- ignore_counter--;
- break;
- }
- if (next_id <= my_id
- ? frame.can_id - base_id < next_id
- : next_id == my_id || frame.can_id - base_id < next_id) {
- next_id = frame.can_id - base_id;
-
- struct can_frame txframe = {.can_id = base_id + my_id,
- .can_dlc = 8,
- .data = {my_id, 02, 01, 04, 00, 04, 00, 00},
- };
- ssize_t tmp = write(socket, &txframe, sizeof(txframe));
- if (tmp != sizeof(txframe)) {
- perror("write socket");
- //return 1;
+}
+
+
+
+static void nm_handle_can_frame(struct NM_Main *nm, struct can_frame *frame)
+{
+ NM_ID id;
+ NM_ID next;
+ NM_State state;
+
+ //printf("Received CAN frame from CAN ID %03x\n", frame->can_id);
+
+ if (frame->can_dlc < 2) {
+ printf("Skipping short frame from CAN ID %03x\n", frame->can_id);
+ return;
}
- }
- break;
- }
+
+
+ if ((frame->can_id & ~0x1f) != 0x420) {
+ printf("Skipping non-NM from CAN ID %03x\n", frame->can_id);
+ return;
+ }
+
+ printf("Received NM frame from CAN ID %03x\n", frame->can_id);
+
+ id = frame->can_id & 0x1f;
+ next = frame->data[0];
+ state = frame->data[1];
+
+ nm->nodes[id].next = next;
+ nm->nodes[id].state = state;
+
+ nm_dump_all(nm);
+
+ /*
+ switch (state) {
+ case 01:
+ if (frame.data[0] == my_id) {
+ struct can_frame txframe = {.can_id = base_id + next_id,
+ .can_dlc = 8,
+ .data = {next_id, 01, 00, 00, 00, 00, 00, 00},
+ };
+ can_tx(socket, &txframe);
+ }
+ break;
+ case 02:
+ if (ignore_counter > 0) {
+ ignore_counter--;
+ break;
+ }
+ if (next_id <= my_id
+ ? frame.can_id - base_id < next_id
+ : next_id == my_id || frame.can_id - base_id < next_id) {
+ next_id = frame.can_id - base_id;
+
+ struct can_frame txframe = {.can_id = base_id + my_id,
+ .can_dlc = 8,
+ .data = {my_id, 02, 01, 04, 00, 04, 00, 00},
+ };
+ can_tx(socket, &txframe);
+ }
+ break;
+ }
+ */
}
int recv_own_msgs;
struct sockaddr_can addr;
struct ifreq ifr;
+ struct can_filter fi;
s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (s < 0) {
exit(1);
}
+ /* Convert interface name to index */
memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
exit(1);
}
+ /* Open the CAN interface */
memset(&addr, 0, sizeof(addr));
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
return 0;
}
- recv_own_msgs = 0; /* 0 = disabled (default), 1 = enabled */
+ recv_own_msgs = 1; /* 0 = disabled (default), 1 = enabled */
setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,
&recv_own_msgs, sizeof(recv_own_msgs));
+ /* Handle only 32 NM IDs at CAN base ID 0x420 */
+ fi.can_id = 0x420;
+ fi.can_mask = 0x7E0;
+
+ setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, &fi, sizeof(struct can_filter));
+
return s;
}
int main(int argc, char **argv)
{
+ struct NM_Node nodes[32] = {{0}};
+ struct NM_Main nm = {.max_nodes = 32, .nodes = nodes};
fd_set rdfs;
int s;
}
if (FD_ISSET(s, &rdfs)) {
- on_nm_frame(s);
+ struct can_frame frame;
+ ssize_t ret;
+
+ ret = read(s, &frame, sizeof(frame));
+ if (ret < 0) {
+ perror("recvfrom CAN socket");
+ exit(1);
+ }
+
+ nm_handle_can_frame(&nm, &frame);
continue;
}
}
projects / revag-nm.git / commitdiff