some broken state

diff --git a/vw-nm.c b/vw-nm.c
index 975956c1421ee2a6adf73e2cb28bc0b0eb96d1b2..6bdf20e29bebfc399b6a1c63040105fa0cf563a4 100644 (file)
--- 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;
 }