forked from nuft/serial-can-bridge
-
Notifications
You must be signed in to change notification settings - Fork 0
/
serial_can_bridge.c
116 lines (100 loc) · 2.4 KB
/
serial_can_bridge.c
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
#include <cmp_mem_access/cmp_mem_access.h>
#include <cmp/cmp.h>
#include "can_frame.h"
#include "can_interface.h"
#include "serial_can_bridge.h"
uint32_t can_bridge_filter_id = 0;
uint32_t can_bridge_filter_mask = 0;
static void can_bridge_set_filter(cmp_ctx_t *ctx);
static bool can_send_cmp_frame(cmp_ctx_t *ctx)
{
struct can_frame frame;
if (!can_frame_cmp_read(ctx, &frame)) {
return false;
}
can_interface_send(&frame);
return true;
}
void can_bridge_datagram_rcv_cb(const void *data, size_t len, void *arg)
{
uint32_t cmd;
cmp_mem_access_t cma;
cmp_ctx_t ctx;
(void) arg;
cmp_mem_access_ro_init(&ctx, &cma, data, len);
// read command
if (!cmp_read_uint(&ctx, &cmd)) {
return;
}
switch (cmd) {
case CMD_CAN_FRAME:
can_send_cmp_frame(&ctx);
break;
case CMD_CFG_ID_FILTER:
can_bridge_set_filter(&ctx);
break;
}
}
bool can_bridge_frame_write(struct can_frame *frame, uint8_t *outbuf, size_t *len)
{
cmp_mem_access_t cma;
cmp_ctx_t ctx;
cmp_mem_access_init(&ctx, &cma, outbuf, *len);
// encode with MessagePack
if (!can_frame_cmp_write(&ctx, frame)) {
return false;
}
*len = cmp_mem_access_get_pos(&cma);
return true;
}
static void can_bridge_set_filter(cmp_ctx_t *ctx)
{
uint32_t id, mask, size;
bool rtr, ide;
if (!cmp_read_array(ctx, &size) || size != 6) {
return;
}
if (!cmp_read_bool(ctx, &ide)) {
return;
}
if (!cmp_read_bool(ctx, &rtr)) {
return;
}
if (!cmp_read_uint(ctx, &id)) {
return;
}
if (ide) {
id = (id & CAN_FRAME_EXT_ID_MASK) | CAN_FRAME_EXT_FLAG;
} else {
id = id & CAN_FRAME_STD_ID_MASK;
}
if (rtr) {
id |= CAN_FRAME_RTR_FLAG;
}
if (!cmp_read_bool(ctx, &ide)) {
return;
}
if (!cmp_read_bool(ctx, &rtr)) {
return;
}
if (!cmp_read_uint(ctx, &mask)) {
return;
}
if (ide) {
mask = (mask & CAN_FRAME_EXT_ID_MASK) | CAN_FRAME_EXT_FLAG;
} else {
mask = mask & CAN_FRAME_STD_ID_MASK;
}
if (rtr) {
mask |= CAN_FRAME_RTR_FLAG;
}
can_bridge_filter_id = id;
can_bridge_filter_mask = mask;
}
bool can_bridge_id_passes_filter(uint32_t id)
{
if (((id ^ can_bridge_filter_id) & can_bridge_filter_mask) == 0) {
return true;
}
return false;
}