From: norly Date: Sat, 17 Dec 2016 02:19:25 +0000 (+0100) Subject: First useful code X-Git-Url: https://git.enpas.org/?p=revag-nm.git;a=commitdiff_plain;h=f9dceee770164d03b51270e567978624282d5996 First useful code --- diff --git a/vw-nm.c b/vw-nm.c index c3bf537..0802ede 100644 --- a/vw-nm.c +++ b/vw-nm.c @@ -1,5 +1,5 @@ /* - * 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 @@ -20,71 +20,165 @@ #include -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; + } + */ } @@ -97,6 +191,7 @@ static int net_init(char *ifname) 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) { @@ -104,6 +199,7 @@ static int net_init(char *ifname) 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) { @@ -111,6 +207,7 @@ static int net_init(char *ifname) exit(1); } + /* Open the CAN interface */ memset(&addr, 0, sizeof(addr)); addr.can_family = AF_CAN; addr.can_ifindex = ifr.ifr_ifindex; @@ -119,15 +216,23 @@ static int net_init(char *ifname) 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; @@ -150,7 +255,16 @@ int main(int argc, char **argv) } 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; } }