X-Git-Url: https://git.enpas.org/?a=blobdiff_plain;f=vw-nm.c;h=e5872a7860c4168a0902d498c8ec0b172e10c61c;hb=ca26ded5703d8e5f7e1a3ddd4949b05ed32a66e8;hp=c3bf537750cf1ad067e702d262bda8429eb2b247;hpb=97f941a4a561180e79859da40a9a8babca89ccea;p=revag-nm.git diff --git a/vw-nm.c b/vw-nm.c index c3bf537..e5872a7 100644 --- a/vw-nm.c +++ b/vw-nm.c @@ -1,11 +1,13 @@ /* - * 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 * by the Free Software Foundation. */ +#include + #include #include #include @@ -18,78 +20,217 @@ #include #include #include +#include + + +#include "vw-nm.h" +#include "vw-nm-tools.h" + + -static int base_id = 0x420; -static int my_id = 0x1a; -static int next_id = 0x19; //myid -static int ignore_counter = 10; +static void nm_update_my_next_id(struct NM_Main *nm) { + unsigned id = nm->my_id; -static void on_nm_frame(int socket) + do { + NM_State state; + + id++; + if (id >= nm->max_nodes) { + id = 0; + } + + state = nm->nodes[id].state & NM_MAIN_MASK; + + if (state == NM_MAIN_ON || state == NM_MAIN_LOGIN) { + /* We skip limp home nodes */ + nm->nodes[nm->my_id].next = id; + break; + } + } while (id != nm->my_id); +} + + + +static void nm_handle_can_frame(struct NM_Main *nm, struct can_frame *frame) { - 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; + NM_ID sender, next; + NM_State state; + + /* Is this a valid frame within our logical network? */ + if (!nm_is_rx_frame_valid(nm, frame)) { + return; } - } - 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; + + printf("Received NM frame from CAN ID %03x\n", frame->can_id); + + + /* Parse sender, its perceived successor, and its state */ + sender = frame->can_id & (nm->max_nodes - 1); + next = frame->data[0]; + state = frame->data[1]; + + /* TODO: Validate state, it needs to be within the enum */ + + /* Skip our own frames */ + if (sender == nm->my_id) { + return; + } + + /* If we're currently stuck in Limp Home mode, and we can see + * someone else's messages: reset counters, reset NM, re-login. + */ + if ((nm->nodes[nm->my_id].state & NM_MAIN_MASK) + == NM_MAIN_LIMPHOME) { + nm_initreset(nm); + return; } - } - break; - } + + nm->nodes[sender].next = next; + nm->nodes[sender].state = state; + + /* Update our view of the world */ + nm_update_my_next_id(nm); + + switch (state & NM_MAIN_MASK) { + case NM_MAIN_ON: + /* We're not alone, so let's transition to ON for now. + */ + nm->nodes[nm->my_id].state = NM_MAIN_ON; + + /* The AWOL timeout is ONLY reset on + * NM_MAIN_ON messages. + */ + nm_set_timer_awol(nm); + + if (next == nm->nodes[nm->my_id].next + && nm->nodes[nm->my_id].next != nm->my_id) { + /* Sender doesn't know we exist */ + + nm->nodes[nm->my_id].state = NM_MAIN_LOGIN; + + nm_set_timer_now(nm); + + /* IMPORTANT: The caller needs to check for + * timeouts first, i.e. no other NM frames + * are received until our correcting login + * has been sent. + */ + } else if (next == nm->nodes[nm->my_id].next) { + /* where (nm->nodes[nm->my_id].next == nm->my_id) */ + + /* It can happen when: + * - our sent frames don't go anywhere + * - we just logged in and immediately + * afterwards another ECU sent a regular + * NM frame. + */ + + /* Nothing to do. */ + } else if (next == nm->my_id) { + /* It's our turn, do a normal timeout. + * This is a period in which anyone we missed + * can send its re-login frame to correct us. + */ + + nm_set_timer_normal(nm); + } else { + /* We just received a random ON message. */ + + /* Nothing to do. */ + } + break; + case NM_MAIN_LOGIN: + /* Note: sender != nm->my_id */ + + /* We're not alone anymore, so let's change state. */ + nm->nodes[nm->my_id].state = NM_MAIN_ON; + + /* We don't reset the timeout when somebody logs in. + * Instead, we'll simply include them in the next + * round. + */ + + /* Nothing else to do. */ + break; + case NM_MAIN_LIMPHOME: + /* Nothing we can do. Poor guy. */ + break; + } + + nm_dump_all(nm); +} + + + + + + +static void nm_buildframe(struct NM_Main *nm, struct can_frame *frame) +{ + frame->can_id = nm->can_base + nm->my_id; + frame->can_dlc = 2; + frame->data[0] = nm->nodes[nm->my_id].next; + frame->data[1] = nm->nodes[nm->my_id].state; } +static void nm_timeout_callback(struct NM_Main *nm, struct can_frame *frame) +{ + switch(nm->timer_reason) { + case NM_TIMER_NOW: + /* We're due to log in */ + nm_buildframe(nm, frame); + + if ((nm->nodes[nm->my_id].state & NM_MAIN_MASK) + != NM_MAIN_LOGIN) { + + printf("BUG: TIMER_NOW expired in non-ON state %u\n", + nm->nodes[nm->my_id].state & NM_MAIN_MASK); + } + + /* We're going to be ready, let's + * change state (RCD 310 behavior) + */ + nm->nodes[nm->my_id].state = NM_MAIN_ON; + + nm_set_timer_normal(nm); + break; + case NM_TIMER_NORMAL: + /* We're due to send our own ring message */ + nm_buildframe(nm, frame); + + if ((nm->nodes[nm->my_id].state & NM_MAIN_MASK) + != NM_MAIN_ON) { + + printf("BUG: TIMER_NORMAL expired in non-ON state %u\n", + nm->nodes[nm->my_id].state & NM_MAIN_MASK); + } + + nm_set_timer_awol(nm); + break; + case NM_TIMER_AWOL: + /* The network is silent because a node disappeared + * or something bad happened. + * Reset everything and start over. + */ + nm_reset(nm); + nm_buildframe(nm, frame); + break; + case NM_TIMER_LIMPHOME: + printf("Limp home timer expired again :(\n"); + + nm_buildframe(nm, frame); + nm_set_timer_limphome(nm); + break; + } +} + + + static int net_init(char *ifname) { @@ -97,6 +238,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 +246,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 +254,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,41 +263,77 @@ 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_Main *nm; fd_set rdfs; int s; + NM_ID my_id; - if (argc != 2) { - printf("syntax: %s IFNAME\n", argv[0]); - exit(1); + if (argc != 3) { + printf("syntax: %s IFNAME MY_ID\n", argv[0]); + return 1; + } + + my_id = strtoul(argv[2], NULL, 0); + + nm = nm_alloc(5, my_id, 0x420); + if (!nm) { + printf("Out of memory allocating NM struct.\n"); + return 1; } s = net_init(argv[1]); while (1) { + int retval; FD_ZERO(&rdfs); - FD_SET(s, &rdfs); - if (select(s+1, &rdfs, NULL, NULL, NULL) < 0) { + retval = select(s+1, &rdfs, NULL, NULL, &nm->tv); + /* We currently rely on Linux timeout behavior here, + * i.e. the timeout now reflects the remaining time */ + if (retval < 0) { perror("select"); return 1; - } + } else if (!retval) { + /* Timeout, we NEED to check this first */ + struct can_frame frame; + + nm_timeout_callback(nm, &frame); + can_tx(s, &frame); + } else if (FD_ISSET(s, &rdfs)) { + struct can_frame frame; + ssize_t ret; - if (FD_ISSET(s, &rdfs)) { - on_nm_frame(s); + ret = read(s, &frame, sizeof(frame)); + if (ret < 0) { + perror("recvfrom CAN socket"); + return 1; + } + + nm_handle_can_frame(nm, &frame); continue; } } + nm_free(nm); + close(s); + return 0; }