From 17b33ca5c993db60c20885f11629e43c08e2124c Mon Sep 17 00:00:00 2001 From: norly Date: Sat, 11 Mar 2017 22:51:41 +0100 Subject: [PATCH] some broken state --- vw-nm.c | 238 ++++++++++++++++++++++++-------------------------------- 1 file changed, 101 insertions(+), 137 deletions(-) diff --git a/vw-nm.c b/vw-nm.c index 975956c..6bdf20e 100644 --- a/vw-nm.c +++ b/vw-nm.c @@ -6,6 +6,8 @@ * by the Free Software Foundation. */ +#include + #include #include #include @@ -18,176 +20,116 @@ #include #include #include +#include -enum { - /* OSEK/VDX NM Level 0 */ +#include "vw-nm.h" +#include "vw-nm-tools.h" - NM_MAIN_OFF = 0x00, - NM_MAIN_ON = 0x01, - NM_MAIN_LOGIN = 0x02, - NM_MAIN_LIMPHOME = 0x04, - NM_MAIN_MASK = 0x0F, - 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 int nm_handle_can_frame(struct NM_Main *nm, struct can_frame *frame) +{ + NM_ID id; + NM_ID next; + NM_State state; + int its_my_turn = 0; + if (frame->can_dlc < 2) { + printf("Skipping short frame from CAN ID %03x\n", frame->can_id); + return 0; + } -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); + if ((frame->can_id & ~(nm->max_nodes - 1)) != nm->can_base) { + printf("Skipping non-NM from CAN ID %03x\n", frame->can_id); + return 0; } -} + printf("Received NM frame from CAN ID %03x\n", frame->can_id); + id = frame->can_id & (nm->max_nodes - 1); + next = frame->data[0]; + state = frame->data[1]; -static char* nm_main_to_string(NM_State state) -{ - switch(state & NM_MAIN_MASK) { - case NM_MAIN_OFF: - return "Off"; + nm->nodes[id].next = (state & NM_MAIN_MASK) == NM_MAIN_ON + ? next + : 0xff; + nm->nodes[id].state = state; + + switch (state & NM_MAIN_MASK) { case NM_MAIN_ON: - return "Ready"; + if (next == nm->my_id) { + its_my_turn = 1; + } + break; case NM_MAIN_LOGIN: - return "Login"; - case NM_MAIN_LIMPHOME: - return "Limp home"; - default: - return "Unknown?"; + if (id == nm->my_id) { + break; + } + printf("Handling LOGIN\n"); + printf("Testing %x < %x\n", id, nm->nodes[nm->my_id].next); + if (id < nm->nodes[nm->my_id].next) { + nm->nodes[nm->my_id].next = id; + } + break; } -} -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?"; - } + nm_dump_all(nm); + + return its_my_turn; } -static void nm_dump_all(struct NM_Main *nm) -{ + +static NM_ID nm_my_next_id(struct NM_Main *nm) { unsigned id; - printf("\n"); - printf(" Node | next | Main | Sleep\n"); - printf("----------------------------------------\n"); + if (nm->max_nodes < 2 + || (nm->nodes[nm->my_id].state & NM_MAIN_MASK) != NM_MAIN_ON) { + assert(0); + } - for (id = 0; id < nm->max_nodes; id++) { - struct NM_Node *node = &nm->nodes[id]; + id = nm->my_id; + do { + struct NM_Node *node; - if (node->state & NM_MAIN_MASK) { - printf(" %02x %02x % 9s %s\n", - id, - node->next, - nm_main_to_string(node->state), - nm_sleep_to_string(node->state)); + id++; + if (id >= nm->max_nodes) { + id = 0; + } + node = &nm->nodes[id]; + if ((node->state & NM_MAIN_MASK) == NM_MAIN_ON) { + return id; } - } + } while (id != nm->my_id); - printf("\n"); + /* This is never reached */ + assert(0); + 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; - } - - - 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); +static void nm_timeout_callback(struct NM_Main *nm, struct can_frame *frame) { + nm->nodes[nm->my_id].next = nm_my_next_id(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; - } - */ + 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_MAIN_ON; } - static int net_init(char *ifname) { int s; @@ -232,45 +174,67 @@ static int net_init(char *ifname) return s; } + int main(int argc, char **argv) { - struct NM_Node nodes[32] = {{0}}; - struct NM_Main nm = {.max_nodes = 32, .nodes = nodes}; + struct NM_Main *nm; + struct timeval tv, *next_tv = NULL; fd_set rdfs; int s; if (argc != 2) { printf("syntax: %s IFNAME\n", argv[0]); - exit(1); + return 1; + } + + nm = nm_alloc(5, 0x0b, 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, next_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 */ + struct can_frame frame; + + nm_timeout_callback(nm, &frame); + can_tx(s, &frame); - if (FD_ISSET(s, &rdfs)) { + next_tv = NULL; + } else if (FD_ISSET(s, &rdfs)) { struct can_frame frame; ssize_t ret; ret = read(s, &frame, sizeof(frame)); if (ret < 0) { perror("recvfrom CAN socket"); - exit(1); + return 1; } - nm_handle_can_frame(&nm, &frame); + if (nm_handle_can_frame(nm, &frame)) { + tv.tv_sec = 0; + tv.tv_usec = 400000; + next_tv = &tv; + } continue; } } + nm_free(nm); + return 0; } -- 2.30.2