blob: e9e26c8d6e5d29e0fed5e8db8d8f94f2e7eabeec (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
|
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 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->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_OTHER_TURN;
return nm;
}
static void nm_free(struct NM_Main *nm)
{
free(nm->nodes);
free(nm);
}
|
