diff options
author | norly <ny-git@enpas.org> | 2020-07-02 12:22:59 +0200 |
---|---|---|
committer | norly <ny-git@enpas.org> | 2020-07-02 12:51:09 +0200 |
commit | 7cb7ce3686fe0bcedf075bb04d0af2c882988304 (patch) | |
tree | fe820fd59568fa697ae018e2bf373e8f89e3cd49 /revag-nm-tools.h | |
parent | df725c1e1086d45aebe9d78df0d156e618d61986 (diff) |
Rename vw-* to revag-*
Diffstat (limited to 'revag-nm-tools.h')
-rw-r--r-- | revag-nm-tools.h | 207 |
1 files changed, 207 insertions, 0 deletions
diff --git a/revag-nm-tools.h b/revag-nm-tools.h new file mode 100644 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); +} |