Rename vw-* to revag-*

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

diff --git a/revag-nm-tools.h b/revag-nm-tools.h
new file mode 100644 (file)
index 0000000..7d3c326
--- /dev/null
+++ b/revag-nm-tools.h
@@ -0,0 +1,207 @@
+static char* nm_main_to_string(NM_State state)
+{
+       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?";
+       }
+}
+
+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?";
+       }
+}
+
+
+
+static void nm_dump_all(struct NM_Main *nm)
+{
+       unsigned id;
+
+       printf("\n");
+       printf(" Node | next | Main      | Sleep\n");
+       printf("----------------------------------------\n");
+
+       for (id = 0; id < nm->max_nodes; id++) {
+               struct NM_Node *node = &nm->nodes[id];
+
+               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));
+
+               }
+       }
+
+       printf("\n");
+}
+
+
+
+
+
+
+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);
+       }
+}
+
+
+
+
+
+
+
+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)
+{
+       struct NM_Main *nm;
+
+       if (node_bits < 1 || node_bits > 6) {
+               return NULL;
+       }
+
+       nm = malloc(sizeof(*nm));
+       if (!nm) {
+               return NULL;
+       }
+
+       nm->max_nodes = 1 << node_bits;
+       nm->nodes = malloc(nm->max_nodes * sizeof(*nm->nodes));
+       if (!nm->nodes) {
+               free(nm);
+               return NULL;
+       }
+
+       nm->my_id = my_id;
+       nm->can_base = can_base;
+
+       nm_initreset(nm);
+
+       return nm;
+}
+
+
+
+
+static void nm_free(struct NM_Main *nm)
+{
+       free(nm->nodes);
+       free(nm);
+}