/*
- * 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 <assert.h>
+
#include <stdio.h>
#include <stdint.h>
#include <unistd.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);
+
+ if (nm->nodes[nm->my_id].next == nm->my_id) {
+ /* Uh oh, we're the only one left. */
+
+ /* TODO */
+ nm->nodes[nm->my_id].state = NM_MAIN_LOGIN;
+
+ /* TODO: Timeout 140ms (RCD 310) */
+ }
+}
-static int base_id = 0x420;
-static int my_id = 0x1a;
-static int next_id = 0x19; //myid
-static int ignore_counter = 10;
-static void on_nm_frame(int socket)
+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;
}
- }
- 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:
+ 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:
+ case NM_TIMER_NORMAL:
+ /* We're due to send our own ring message */
+ switch(nm->nodes[nm->my_id].state & NM_MAIN_MASK) {
+ case NM_MAIN_ON:
+ break;
+ case NM_MAIN_LOGIN:
+ /* We're going to be ready, let's
+ * change state (RCD 310 behavior)
+ */
+ nm->nodes[nm->my_id].state = NM_MAIN_ON;
+ break;
+ default:
+ printf("BUG: TIMER_NORMAL expired in unknown NM_MAIN state\n");
+ break;
+ }
+ nm_set_timer_awol(nm);
+ nm_buildframe(nm, frame);
+ 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);
+ break;
+ case NM_TIMER_LIMPHOME:
+ /* TODO */
+ break;
+ }
+
+ nm_buildframe(nm, frame);
+}
+
+
+
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) {
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_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]);
+ /* Stir up the hornet's nest */
+ if (1) {
+ struct can_frame frame;
+
+ nm_buildframe(nm, &frame);
+ can_tx(s, &frame);
+ }
+
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;
}
projects / revag-nm.git / blobdiff