revag-nm-tools.h

   1 static char* nm_main_to_string(NM_State state)
   2 {
   3         switch(state & NM_MAIN_MASK) {
   4                 case NM_MAIN_OFF:
   5                         return "Off";
   6                 case NM_MAIN_ON:
   7                         return "Ready";
   8                 case NM_MAIN_LOGIN:
   9                         return "Login";
  10                 case NM_MAIN_LIMPHOME:
  11                         return "Limp home";
  12                 default:
  13                         return "Unknown?";
  14         }
  15 }
  16
  17 static char* nm_sleep_to_string(NM_State state)
  18 {
  19         switch(state & NM_SLEEP_MASK) {
  20                 case NM_SLEEP_CANCEL:
  21                         return "No";
  22                 case NM_SLEEP_REQUEST:
  23                         return "Request";
  24                 case NM_SLEEP_ACK:
  25                         return "Acknowledged";
  26                 default:
  27                         return "Unknown?";
  28         }
  29 }
  30
  31
  32
  33 static void nm_dump_all(struct NM_Main *nm)
  34 {
  35         unsigned id;
  36
  37         printf("\n");
  38         printf(" Node | next | Main      | Sleep\n");
  39         printf("----------------------------------------\n");
  40
  41         for (id = 0; id < nm->max_nodes; id++) {
  42                 struct NM_Node *node = &nm->nodes[id];
  43
  44                 if (node->state & NM_MAIN_MASK) {
  45                         printf("  %02x     %02x    %9s   %s\n",
  46                                 id,
  47                                 node->next,
  48                                 nm_main_to_string(node->state),
  49                                 nm_sleep_to_string(node->state));
  50
  51                 }
  52         }
  53
  54         printf("\n");
  55 }
  56
  57
  58
  59
  60
  61
  62 static void can_tx(int socket, struct can_frame *frame)
  63 {
  64         ssize_t ret;
  65
  66         ret = write(socket, frame, sizeof(*frame));
  67         if (ret != sizeof(*frame)) {
  68                 perror("write to CAN socket");
  69                 exit(1);
  70         }
  71 }
  72
  73
  74
  75
  76
  77
  78
  79 static int nm_is_rx_frame_valid(struct NM_Main *nm, struct can_frame *frame)
  80 {
  81         if (frame->can_dlc < 2) {
  82                 printf("Skipping short frame from CAN ID %03x\n", frame->can_id);
  83                 return 0;
  84         }
  85
  86         if ((frame->can_id & ~(nm->max_nodes - 1)) != nm->can_base) {
  87                 printf("Skipping non-NM from CAN ID %03x\n", frame->can_id);
  88                 return 0;
  89         }
  90
  91         return 1;
  92 }
  93
  94
  95
  96
  97
  98
  99
 100
 101
 102
 103 static void nm_set_timer_now(struct NM_Main *nm)
 104 {
 105         nm->tv.tv_sec = 0;
 106         nm->tv.tv_usec = 0;
 107         nm->timer_reason = NM_TIMER_NOW;
 108 }
 109
 110 static void nm_set_timer_normal(struct NM_Main *nm)
 111 {
 112         nm->tv.tv_sec = 0;
 113         nm->tv.tv_usec = NM_USECS_NORMAL_TURN;
 114         nm->timer_reason = NM_TIMER_NORMAL;
 115 }
 116
 117 static void nm_set_timer_awol(struct NM_Main *nm)
 118 {
 119         nm->tv.tv_sec = 0;
 120         nm->tv.tv_usec = NM_USECS_NODE_AWOL;
 121         nm->timer_reason = NM_TIMER_AWOL;
 122 }
 123
 124 static void nm_set_timer_limphome(struct NM_Main *nm)
 125 {
 126         nm->tv.tv_sec = 0;
 127         nm->tv.tv_usec = NM_USECS_LIMPHOME;
 128         nm->timer_reason = NM_TIMER_LIMPHOME;
 129 }
 130
 131
 132
 133
 134
 135
 136 static void nm_reset(struct NM_Main *nm)
 137 {
 138         unsigned id;
 139
 140         if (nm->nodes[nm->my_id].next == nm->my_id) {
 141                 nm->lonely_resets++;
 142         }
 143
 144         for (id = 0; id < nm->max_nodes; id++) {
 145                 nm->nodes[id].next = 0xff;
 146                 nm->nodes[id].state = NM_MAIN_OFF;
 147         }
 148
 149         nm->nodes[nm->my_id].next = nm->my_id;
 150         if (nm->lonely_resets >= 5) {
 151                 printf("Limp home detected :(\n");
 152
 153                 nm->nodes[nm->my_id].state = NM_MAIN_LIMPHOME;
 154                 nm_set_timer_limphome(nm);
 155         } else {
 156                 nm->nodes[nm->my_id].state = NM_MAIN_LOGIN;
 157                 nm_set_timer_now(nm);
 158         }
 159 }
 160
 161
 162 static void nm_initreset(struct NM_Main *nm)
 163 {
 164         nm_reset(nm);
 165
 166         nm->lonely_resets = 0;
 167 }
 168
 169
 170
 171
 172 static struct NM_Main* nm_alloc(unsigned node_bits, NM_ID my_id, canid_t can_base)
 173 {
 174         struct NM_Main *nm;
 175
 176         if (node_bits < 1 || node_bits > 6) {
 177                 return NULL;
 178         }
 179
 180         nm = malloc(sizeof(*nm));
 181         if (!nm) {
 182                 return NULL;
 183         }
 184
 185         nm->max_nodes = 1 << node_bits;
 186         nm->nodes = malloc(nm->max_nodes * sizeof(*nm->nodes));
 187         if (!nm->nodes) {
 188                 free(nm);
 189                 return NULL;
 190         }
 191
 192         nm->my_id = my_id;
 193         nm->can_base = can_base;
 194
 195         nm_initreset(nm);
 196
 197         return nm;
 198 }
 199
 200
 201
 202
 203 static void nm_free(struct NM_Main *nm)
 204 {
 205         free(nm->nodes);
 206         free(nm);
 207 }