1 files 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 <assert.h>
+
 #include <stdio.h>
 #include <stdint.h>
 #include <unistd.h>
@@ -18,176 +20,116 @@
 #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;
 
-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;
 }