* 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>
-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;
+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;
+ }
-struct NM_Node {
- NM_ID next;
- NM_State state;
-};
+ 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;
+ }
-struct NM_Main {
- unsigned max_nodes;
- struct NM_Node *nodes;
-};
+ return 1;
+}
-static void can_tx(int socket, struct can_frame *frame)
-{
- ssize_t ret;
+static void nm_update_my_next_id(struct NM_Main *nm) {
+ unsigned id = nm->my_id;
- ret = write(socket, frame, sizeof(*frame));
- if (ret != sizeof(*frame)) {
- perror("write to CAN socket");
- exit(1);
- }
+ 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) {
+ /* TODO: Check for limp home nodes? */
+ nm->nodes[nm->my_id].next = id;
+ break;
+ }
+ } while (id != nm->my_id);
}
-static char* nm_main_to_string(NM_State state)
+static void nm_handle_can_frame(struct NM_Main *nm, struct can_frame *frame)
{
- 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?";
+ 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;
}
-}
-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?";
+ 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;
}
-}
+ nm->nodes[sender].next = next;
+ nm->nodes[sender].state = state;
+ switch (state & NM_MAIN_MASK) {
+ case NM_MAIN_ON:
+ 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->tv.tv_sec = 0;
+ nm->tv.tv_usec = 0;
+ /* IMPORTANT: The caller needs to check for
+ * timeouts first, so 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 */
+
+ /* TODO: Is this a case we need to handle? */
+
+ /* 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 without knowing that we exist.
+ */
+ } else if (next == nm->my_id) {
+ /* It's our turn.
+ * Reset the timeout so anyone we missed
+ * can send its login frame to correct us.
+ */
+ nm->tv.tv_sec = 0;
+ nm->tv.tv_usec = NM_USECS_MY_TURN;
+ } else {
+ /* We just got some random ON message.
+ * Reset the timer looking out for broken
+ * connectivity.
+ */
+ nm->tv.tv_sec = 0;
+ nm->tv.tv_usec = NM_USECS_OTHER_TURN;
+ }
+ break;
+ case NM_MAIN_LOGIN:
+ /* Note: sender != nm->my_id */
-static void nm_dump_all(struct NM_Main *nm)
-{
- unsigned id;
+ printf("Handling LOGIN\n");
- printf("\n");
- printf(" Node | next | Main | Sleep\n");
- printf("----------------------------------------\n");
+ nm_update_my_next_id(nm);
- for (id = 0; id < nm->max_nodes; id++) {
- struct NM_Node *node = &nm->nodes[id];
+ /* We're not alone anymore, so let's change state. */
+ nm->nodes[nm->my_id].state = NM_MAIN_ON;
- 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));
+ /* We don't reset the timeout when somebody logs in.
+ * Instead, we'll simply include them in the next
+ * round. */
- }
+ /* Actually, when a login is done as a correction,
+ * we do reset the timeout.
+ *
+ * TODO.
+ */
+ break;
}
- printf("\n");
+ nm_dump_all(nm);
}
-static void nm_handle_can_frame(struct NM_Main *nm, struct can_frame *frame)
+
+
+
+static void nm_buildframe(struct NM_Main *nm, struct can_frame *frame)
{
- NM_ID id;
- NM_ID next;
- NM_State state;
+ 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;
+}
- //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;
- }
+static void nm_timeout_callback(struct NM_Main *nm, struct can_frame *frame)
+{
+ nm->tv.tv_sec = 0;
+ nm->tv.tv_usec = NM_USECS_OTHER_TURN;
- printf("Received NM frame from CAN ID %03x\n", frame->can_id);
+ nm_buildframe(nm, frame);
+}
- 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;
- }
- */
-}
+static void nm_start(struct NM_Main *nm, struct can_frame *frame)
+{
+ nm->tv.tv_sec = 0;
+ nm->tv.tv_usec = 50000;
+ nm->nodes[nm->my_id].next = nm->my_id;
+ nm->nodes[nm->my_id].state = NM_MAIN_LOGIN;
+
+ nm_buildframe(nm, frame);
+}
+
+
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;
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]);
+ /* Stir up the hornet's nest */
+ if (1) {
+ struct can_frame frame;
+
+ nm_start(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;
- if (FD_ISSET(s, &rdfs)) {
+ 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");
- exit(1);
+ return 1;
}
- nm_handle_can_frame(&nm, &frame);
+ nm_handle_can_frame(nm, &frame);
continue;
}
}
+ nm_free(nm);
+
return 0;
}
projects / revag-nm.git / blobdiff