CFLAGS += -Wall -g
-vw-nm:
+revag-nm:
--- /dev/null
+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 "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)
+{
+ unsigned id;
+
+ printf("\n");
+ printf(" Node | next | Main | Sleep\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(" %02x %02x %9s %s\n",
+ id,
+ node->next,
+ nm_main_to_string(node->state),
+ nm_sleep_to_string(node->state));
+
+ }
+ }
+
+ printf("\n");
+}
+
+
+
+
+
+
+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 int nm_is_rx_frame_valid(struct NM_Main *nm, struct can_frame *frame)
+{
+ if (frame->can_dlc < 2) {
+ printf("Skipping short frame from CAN ID %03x\n", frame->can_id);
+ return 0;
+ }
+
+ 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;
+ }
+
+ return 1;
+}
+
+
+
+
+
+
+
+
+
+
+static void nm_set_timer_now(struct NM_Main *nm)
+{
+ nm->tv.tv_sec = 0;
+ nm->tv.tv_usec = 0;
+ nm->timer_reason = NM_TIMER_NOW;
+}
+
+static void nm_set_timer_normal(struct NM_Main *nm)
+{
+ nm->tv.tv_sec = 0;
+ nm->tv.tv_usec = NM_USECS_NORMAL_TURN;
+ nm->timer_reason = NM_TIMER_NORMAL;
+}
+
+static void nm_set_timer_awol(struct NM_Main *nm)
+{
+ nm->tv.tv_sec = 0;
+ nm->tv.tv_usec = NM_USECS_NODE_AWOL;
+ nm->timer_reason = NM_TIMER_AWOL;
+}
+
+static void nm_set_timer_limphome(struct NM_Main *nm)
+{
+ nm->tv.tv_sec = 0;
+ nm->tv.tv_usec = NM_USECS_LIMPHOME;
+ nm->timer_reason = NM_TIMER_LIMPHOME;
+}
+
+
+
+
+
+
+static void nm_reset(struct NM_Main *nm)
+{
+ unsigned id;
+
+ if (nm->nodes[nm->my_id].next == nm->my_id) {
+ nm->lonely_resets++;
+ }
+
+ for (id = 0; id < nm->max_nodes; id++) {
+ nm->nodes[id].next = 0xff;
+ nm->nodes[id].state = NM_MAIN_OFF;
+ }
+
+ nm->nodes[nm->my_id].next = nm->my_id;
+ if (nm->lonely_resets >= 5) {
+ printf("Limp home detected :(\n");
+
+ nm->nodes[nm->my_id].state = NM_MAIN_LIMPHOME;
+ nm_set_timer_limphome(nm);
+ } else {
+ nm->nodes[nm->my_id].state = NM_MAIN_LOGIN;
+ nm_set_timer_now(nm);
+ }
+}
+
+
+static void nm_initreset(struct NM_Main *nm)
+{
+ nm_reset(nm);
+
+ nm->lonely_resets = 0;
+}
+
+
+
+
+static struct NM_Main* nm_alloc(unsigned node_bits, NM_ID my_id, canid_t can_base)
+{
+ struct NM_Main *nm;
+
+ if (node_bits < 1 || node_bits > 6) {
+ return NULL;
+ }
+
+ nm = malloc(sizeof(*nm));
+ if (!nm) {
+ return NULL;
+ }
+
+ nm->max_nodes = 1 << node_bits;
+ nm->nodes = malloc(nm->max_nodes * sizeof(*nm->nodes));
+ if (!nm->nodes) {
+ free(nm);
+ return NULL;
+ }
+
+ nm->my_id = my_id;
+ nm->can_base = can_base;
+
+ nm_initreset(nm);
+
+ return nm;
+}
+
+
+
+
+static void nm_free(struct NM_Main *nm)
+{
+ free(nm->nodes);
+ free(nm);
+}
--- /dev/null
+/*
+ * 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 <assert.h>
+
+#include <stdio.h>
+#include <stdint.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <linux/can.h>
+#include <linux/can/raw.h>
+#include <net/if.h>
+#include <sys/ioctl.h>
+#include <endian.h>
+#include <sys/time.h>
+
+
+#include "revag-nm.h"
+#include "revag-nm-tools.h"
+
+
+
+
+
+static void nm_update_my_next_id(struct NM_Main *nm)
+{
+ unsigned id = nm->my_id;
+
+ 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 unsigned nm_num_active_nodes(struct NM_Main *nm)
+{
+ unsigned id = 0;
+ unsigned active = 0;
+
+ for (id = 0; id < nm->max_nodes; id++) {
+ NM_State state;
+
+ state = nm->nodes[id].state & NM_MAIN_MASK;
+
+ if (state == NM_MAIN_ON || state == NM_MAIN_LOGIN) {
+ /* We skip limp home nodes */
+ active++;
+ }
+ }
+
+ return active;
+}
+
+
+
+static void nm_handle_can_frame(struct NM_Main *nm, struct can_frame *frame)
+{
+ 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;
+ }
+
+ 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;
+ }
+
+ 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.
+ */
+
+ /* HACK:
+ * Special case: The Media-In's NM implementation
+ * doesn't auto-switch to NM_ON. Let's say hello,
+ * even if it ends up being a little late.
+ */
+ if (nm_num_active_nodes(nm) >= 2) {
+ nm_set_timer_normal(nm);
+ }
+
+ /* 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)
+{
+ int s;
+ 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) {
+ perror("socket");
+ 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) {
+ perror("SIOCGIFINDEX");
+ exit(1);
+ }
+
+ /* Open the CAN interface */
+ memset(&addr, 0, sizeof(addr));
+ addr.can_family = AF_CAN;
+ addr.can_ifindex = ifr.ifr_ifindex;
+ if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
+ perror("bind");
+ return 0;
+ }
+
+ 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 != 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);
+
+ 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;
+
+ 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;
+}
--- /dev/null
+#ifndef __REVAG_NM_H__
+#define __REVAG_NM_H__
+
+#include <sys/time.h>
+
+
+enum {
+ /* OSEK/VDX NM Level 0 */
+
+ 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;
+};
+
+
+enum timer_reason {
+ NM_TIMER_NOW,
+ NM_TIMER_NORMAL,
+ NM_TIMER_AWOL,
+ NM_TIMER_LIMPHOME,
+};
+
+struct NM_Main {
+ unsigned max_nodes;
+ struct NM_Node *nodes;
+
+ NM_ID my_id;
+ canid_t can_base;
+
+ struct timeval tv;
+ enum timer_reason timer_reason;
+
+ /* How many times have we been alone when we reset? */
+ int lonely_resets;
+};
+
+
+
+
+/* OSEK/VDX NM: T_Typ
+ *
+ * This timeout is ~50 ms in:
+ * - 0x19 (RCD 310, Bosch)
+ *
+ * and ~45ms in:
+ * - 0x0b Instrument cluster?
+ * - 0x15 MDI
+ * - 0x1A Phone
+ */
+#define NM_USECS_NORMAL_TURN 50000
+
+
+/* OSEK/VDX NM: T_Max
+ *
+ * This timeout is 140 ms in:
+ * - 0x19 (RCD 310, Bosch)
+ */
+#define NM_USECS_NODE_AWOL 140000
+
+
+/* OSEK/VDX NM: T_Error
+ *
+ * This timeout is 500 ms in:
+ * - 0x19 (RCD 310, Bosch)
+ */
+#define NM_USECS_LIMPHOME 500000
+
+
+#endif /* __REVAG_NM_H__ */
+++ /dev/null
-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 "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)
-{
- unsigned id;
-
- printf("\n");
- printf(" Node | next | Main | Sleep\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(" %02x %02x %9s %s\n",
- id,
- node->next,
- nm_main_to_string(node->state),
- nm_sleep_to_string(node->state));
-
- }
- }
-
- printf("\n");
-}
-
-
-
-
-
-
-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 int nm_is_rx_frame_valid(struct NM_Main *nm, struct can_frame *frame)
-{
- if (frame->can_dlc < 2) {
- printf("Skipping short frame from CAN ID %03x\n", frame->can_id);
- return 0;
- }
-
- 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;
- }
-
- return 1;
-}
-
-
-
-
-
-
-
-
-
-
-static void nm_set_timer_now(struct NM_Main *nm)
-{
- nm->tv.tv_sec = 0;
- nm->tv.tv_usec = 0;
- nm->timer_reason = NM_TIMER_NOW;
-}
-
-static void nm_set_timer_normal(struct NM_Main *nm)
-{
- nm->tv.tv_sec = 0;
- nm->tv.tv_usec = NM_USECS_NORMAL_TURN;
- nm->timer_reason = NM_TIMER_NORMAL;
-}
-
-static void nm_set_timer_awol(struct NM_Main *nm)
-{
- nm->tv.tv_sec = 0;
- nm->tv.tv_usec = NM_USECS_NODE_AWOL;
- nm->timer_reason = NM_TIMER_AWOL;
-}
-
-static void nm_set_timer_limphome(struct NM_Main *nm)
-{
- nm->tv.tv_sec = 0;
- nm->tv.tv_usec = NM_USECS_LIMPHOME;
- nm->timer_reason = NM_TIMER_LIMPHOME;
-}
-
-
-
-
-
-
-static void nm_reset(struct NM_Main *nm)
-{
- unsigned id;
-
- if (nm->nodes[nm->my_id].next == nm->my_id) {
- nm->lonely_resets++;
- }
-
- for (id = 0; id < nm->max_nodes; id++) {
- nm->nodes[id].next = 0xff;
- nm->nodes[id].state = NM_MAIN_OFF;
- }
-
- nm->nodes[nm->my_id].next = nm->my_id;
- if (nm->lonely_resets >= 5) {
- printf("Limp home detected :(\n");
-
- nm->nodes[nm->my_id].state = NM_MAIN_LIMPHOME;
- nm_set_timer_limphome(nm);
- } else {
- nm->nodes[nm->my_id].state = NM_MAIN_LOGIN;
- nm_set_timer_now(nm);
- }
-}
-
-
-static void nm_initreset(struct NM_Main *nm)
-{
- nm_reset(nm);
-
- nm->lonely_resets = 0;
-}
-
-
-
-
-static struct NM_Main* nm_alloc(unsigned node_bits, NM_ID my_id, canid_t can_base)
-{
- struct NM_Main *nm;
-
- if (node_bits < 1 || node_bits > 6) {
- return NULL;
- }
-
- nm = malloc(sizeof(*nm));
- if (!nm) {
- return NULL;
- }
-
- nm->max_nodes = 1 << node_bits;
- nm->nodes = malloc(nm->max_nodes * sizeof(*nm->nodes));
- if (!nm->nodes) {
- free(nm);
- return NULL;
- }
-
- nm->my_id = my_id;
- nm->can_base = can_base;
-
- nm_initreset(nm);
-
- return nm;
-}
-
-
-
-
-static void nm_free(struct NM_Main *nm)
-{
- free(nm->nodes);
- free(nm);
-}
+++ /dev/null
-/*
- * 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 <assert.h>
-
-#include <stdio.h>
-#include <stdint.h>
-#include <unistd.h>
-#include <stdlib.h>
-#include <string.h>
-#include <sys/types.h>
-#include <sys/socket.h>
-#include <linux/can.h>
-#include <linux/can/raw.h>
-#include <net/if.h>
-#include <sys/ioctl.h>
-#include <endian.h>
-#include <sys/time.h>
-
-
-#include "vw-nm.h"
-#include "vw-nm-tools.h"
-
-
-
-
-
-static void nm_update_my_next_id(struct NM_Main *nm)
-{
- unsigned id = nm->my_id;
-
- 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 unsigned nm_num_active_nodes(struct NM_Main *nm)
-{
- unsigned id = 0;
- unsigned active = 0;
-
- for (id = 0; id < nm->max_nodes; id++) {
- NM_State state;
-
- state = nm->nodes[id].state & NM_MAIN_MASK;
-
- if (state == NM_MAIN_ON || state == NM_MAIN_LOGIN) {
- /* We skip limp home nodes */
- active++;
- }
- }
-
- return active;
-}
-
-
-
-static void nm_handle_can_frame(struct NM_Main *nm, struct can_frame *frame)
-{
- 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;
- }
-
- 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;
- }
-
- 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.
- */
-
- /* HACK:
- * Special case: The Media-In's NM implementation
- * doesn't auto-switch to NM_ON. Let's say hello,
- * even if it ends up being a little late.
- */
- if (nm_num_active_nodes(nm) >= 2) {
- nm_set_timer_normal(nm);
- }
-
- /* 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)
-{
- int s;
- 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) {
- perror("socket");
- 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) {
- perror("SIOCGIFINDEX");
- exit(1);
- }
-
- /* Open the CAN interface */
- memset(&addr, 0, sizeof(addr));
- addr.can_family = AF_CAN;
- addr.can_ifindex = ifr.ifr_ifindex;
- if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
- perror("bind");
- return 0;
- }
-
- 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 != 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);
-
- 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;
-
- 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;
-}
+++ /dev/null
-#ifndef __VW_NM_H__
-#define __VW_NM_H__
-
-#include <sys/time.h>
-
-
-enum {
- /* OSEK/VDX NM Level 0 */
-
- 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;
-};
-
-
-enum timer_reason {
- NM_TIMER_NOW,
- NM_TIMER_NORMAL,
- NM_TIMER_AWOL,
- NM_TIMER_LIMPHOME,
-};
-
-struct NM_Main {
- unsigned max_nodes;
- struct NM_Node *nodes;
-
- NM_ID my_id;
- canid_t can_base;
-
- struct timeval tv;
- enum timer_reason timer_reason;
-
- /* How many times have we been alone when we reset? */
- int lonely_resets;
-};
-
-
-
-
-/* OSEK/VDX NM: T_Typ
- *
- * This timeout is ~50 ms in:
- * - 0x19 (RCD 310, Bosch)
- *
- * and ~45ms in:
- * - 0x0b Instrument cluster?
- * - 0x15 MDI
- * - 0x1A Phone
- */
-#define NM_USECS_NORMAL_TURN 50000
-
-
-/* OSEK/VDX NM: T_Max
- *
- * This timeout is 140 ms in:
- * - 0x19 (RCD 310, Bosch)
- */
-#define NM_USECS_NODE_AWOL 140000
-
-
-/* OSEK/VDX NM: T_Error
- *
- * This timeout is 500 ms in:
- * - 0x19 (RCD 310, Bosch)
- */
-#define NM_USECS_LIMPHOME 500000
-
-
-#endif /* __VW_NM_H__ */
projects / revag-nm.git / commitdiff