Detect limp home state

[revag-nm.git] / vw-nm-tools.h

diff --git a/vw-nm-tools.h b/vw-nm-tools.h
index 426e778848f8894d3c94303b8d03675dc0b57f0a..7d3c32681e0d1435270f1911cb40ce46ba997e7e 100644 (file)
--- a/vw-nm-tools.h
+++ b/vw-nm-tools.h
@@ -76,6 +76,98 @@ static void can_tx(int socket, struct can_frame *frame)
 
 
 
+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)
 {
@@ -100,11 +192,7 @@ static struct NM_Main* nm_alloc(unsigned node_bits, NM_ID my_id, canid_t can_bas
        nm->my_id = my_id;
        nm->can_base = can_base;
 
-       nm->nodes[nm->my_id].next = nm->my_id;
-       nm->nodes[nm->my_id].state = NM_MAIN_LOGIN;
-
-       nm->tv.tv_sec = 0;
-       nm->tv.tv_usec = NM_USECS_NODE_AWOL;
+       nm_initreset(nm);
 
        return nm;
 }