1 /*
2 * Copyright (c) 2018 Intel Corporation.
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/logging/log.h>
8 LOG_MODULE_REGISTER(net_socket_can_sample, LOG_LEVEL_DBG);
9
10 #include <zephyr/kernel.h>
11
12 #include <zephyr/drivers/can.h>
13 #include <zephyr/net/socket.h>
14 #include <zephyr/net/socketcan.h>
15 #include <zephyr/net/socketcan_utils.h>
16
17 #define PRIORITY k_thread_priority_get(k_current_get())
18 #define STACKSIZE 1024
19 #define SLEEP_PERIOD K_SECONDS(1)
20
21 static k_tid_t tx_tid;
22 static K_THREAD_STACK_DEFINE(tx_stack, STACKSIZE);
23 static struct k_thread tx_data;
24
25 /* For testing purposes, we create another RX receiver if configured so */
26 #if CONFIG_NET_SOCKETS_CAN_RECEIVERS == 2
27 static k_tid_t rx_tid;
28 static K_THREAD_STACK_DEFINE(rx_stack, STACKSIZE);
29 static struct k_thread rx_data;
30 #endif
31
32 #define CLOSE_PERIOD 15
33
34 static const struct can_filter zfilter = {
35 .flags = 0U,
36 .id = 0x1,
37 .mask = CAN_STD_ID_MASK
38 };
39
40 static struct socketcan_filter sock_filter;
41
tx(void * p1,void * p2,void * p3)42 static void tx(void *p1, void *p2, void *p3)
43 {
44 ARG_UNUSED(p2);
45 ARG_UNUSED(p3);
46
47 int *can_fd = p1;
48 int fd = POINTER_TO_INT(can_fd);
49 struct can_frame zframe = {0};
50 struct socketcan_frame sframe = {0};
51 int ret, i;
52
53 zframe.id = 0x1;
54 zframe.dlc = 8U;
55
56 for (i = 0; i < zframe.dlc; i++) {
57 zframe.data[i] = 0xF0 | i;
58 }
59
60 socketcan_from_can_frame(&zframe, &sframe);
61
62 LOG_DBG("Sending CAN data...");
63
64 while (1) {
65 ret = send(fd, &sframe, sizeof(sframe), 0);
66 if (ret < 0) {
67 LOG_ERR("Cannot send CAN frame (%d)", -errno);
68 }
69
70 k_sleep(SLEEP_PERIOD);
71 }
72 }
73
create_socket(const struct socketcan_filter * sfilter)74 static int create_socket(const struct socketcan_filter *sfilter)
75 {
76 struct sockaddr_can can_addr;
77 int fd, ret;
78
79 fd = socket(AF_CAN, SOCK_RAW, CAN_RAW);
80 if (fd < 0) {
81 LOG_ERR("Cannot create %s CAN socket (%d)", "2nd", fd);
82 return fd;
83 }
84
85 can_addr.can_ifindex = net_if_get_by_iface(
86 net_if_get_first_by_type(&NET_L2_GET_NAME(CANBUS_RAW)));
87 can_addr.can_family = PF_CAN;
88
89 ret = bind(fd, (struct sockaddr *)&can_addr, sizeof(can_addr));
90 if (ret < 0) {
91 LOG_ERR("Cannot bind %s CAN socket (%d)", "2nd", -errno);
92 (void)close(fd);
93 return ret;
94 }
95
96 (void)setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FILTER, sfilter,
97 sizeof(*sfilter));
98
99 return fd;
100 }
101
rx(void * p1,void * p2,void * p3)102 static void rx(void *p1, void *p2, void *p3)
103 {
104 int *can_fd = p1;
105 int *do_close_period = p2;
106 const struct socketcan_filter *sfilter = p3;
107 int close_period = POINTER_TO_INT(do_close_period);
108 int fd = POINTER_TO_INT(can_fd);
109 struct sockaddr_can can_addr;
110 socklen_t addr_len;
111 struct can_frame zframe;
112 struct socketcan_frame sframe;
113 int ret;
114
115 LOG_DBG("[%d] Waiting CAN data...", fd);
116
117 while (1) {
118 uint8_t *data;
119
120 memset(&sframe, 0, sizeof(sframe));
121 addr_len = sizeof(can_addr);
122
123 ret = recvfrom(fd, &sframe, sizeof(struct socketcan_frame),
124 0, (struct sockaddr *)&can_addr, &addr_len);
125 if (ret < 0) {
126 LOG_ERR("[%d] Cannot receive CAN frame (%d)", fd,
127 -errno);
128 continue;
129 }
130
131 socketcan_to_can_frame(&sframe, &zframe);
132
133 LOG_INF("[%d] CAN frame: IDE 0x%x RTR 0x%x ID 0x%x DLC 0x%x",
134 fd,
135 (zframe.flags & CAN_FRAME_IDE) != 0 ? 1 : 0,
136 (zframe.flags & CAN_FRAME_RTR) != 0 ? 1 : 0,
137 zframe.id, zframe.dlc);
138
139 if ((zframe.flags & CAN_FRAME_RTR) != 0) {
140 LOG_INF("[%d] EXT Remote frame received", fd);
141 } else {
142 if (zframe.dlc > 8) {
143 data = (uint8_t *)zframe.data_32;
144 } else {
145 data = zframe.data;
146 }
147
148 LOG_HEXDUMP_INF(data, zframe.dlc, "Data");
149 }
150
151 if (POINTER_TO_INT(do_close_period) > 0) {
152 close_period--;
153 if (close_period <= 0) {
154 (void)close(fd);
155
156 k_sleep(K_SECONDS(1));
157
158 fd = create_socket(sfilter);
159 if (fd < 0) {
160 LOG_ERR("Cannot get socket (%d)",
161 -errno);
162 return;
163 }
164
165 close_period = POINTER_TO_INT(do_close_period);
166 }
167 }
168 }
169 }
170
setup_socket(void)171 static int setup_socket(void)
172 {
173 struct sockaddr_can can_addr;
174 struct net_if *iface;
175 int fd, rx_fd;
176 int ret;
177
178 socketcan_from_can_filter(&zfilter, &sock_filter);
179
180 iface = net_if_get_first_by_type(&NET_L2_GET_NAME(CANBUS_RAW));
181 if (!iface) {
182 LOG_ERR("No CANBUS network interface found!");
183 return -ENOENT;
184 }
185
186 fd = socket(AF_CAN, SOCK_RAW, CAN_RAW);
187 if (fd < 0) {
188 ret = -errno;
189 LOG_ERR("Cannot create %s CAN socket (%d)", "1st", ret);
190 return ret;
191 }
192
193 can_addr.can_ifindex = net_if_get_by_iface(iface);
194 can_addr.can_family = PF_CAN;
195
196 ret = bind(fd, (struct sockaddr *)&can_addr, sizeof(can_addr));
197 if (ret < 0) {
198 ret = -errno;
199 LOG_ERR("Cannot bind %s CAN socket (%d)", "1st", ret);
200 goto cleanup;
201 }
202
203 ret = setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FILTER, &sock_filter,
204 sizeof(sock_filter));
205 if (ret < 0) {
206 ret = -errno;
207 LOG_ERR("Cannot set CAN sockopt (%d)", ret);
208 goto cleanup;
209 }
210
211 /* Delay TX startup so that RX is ready to receive */
212 tx_tid = k_thread_create(&tx_data, tx_stack,
213 K_THREAD_STACK_SIZEOF(tx_stack),
214 tx, INT_TO_POINTER(fd),
215 NULL, NULL, PRIORITY, 0, K_SECONDS(1));
216 if (!tx_tid) {
217 ret = -ENOENT;
218 errno = -ret;
219 LOG_ERR("Cannot create TX thread!");
220 goto cleanup;
221 }
222
223 LOG_DBG("Started socket CAN TX thread");
224
225 LOG_INF("1st RX fd %d", fd);
226
227 rx_fd = fd;
228
229 #if CONFIG_NET_SOCKETS_CAN_RECEIVERS == 2
230 fd = create_socket(&sock_filter);
231 if (fd >= 0) {
232 rx_tid = k_thread_create(&rx_data, rx_stack,
233 K_THREAD_STACK_SIZEOF(rx_stack),
234 rx,
235 INT_TO_POINTER(fd),
236 INT_TO_POINTER(CLOSE_PERIOD),
237 &sock_filter, PRIORITY, 0, K_NO_WAIT);
238 if (!rx_tid) {
239 ret = -ENOENT;
240 errno = -ret;
241 LOG_ERR("Cannot create 2nd RX thread!");
242 goto cleanup2;
243 }
244
245 LOG_INF("2nd RX fd %d", fd);
246 } else {
247 LOG_ERR("2nd RX not created (%d)", fd);
248 }
249 #endif
250
251 return rx_fd;
252
253 #if CONFIG_NET_SOCKETS_CAN_RECEIVERS == 2
254 cleanup2:
255 (void)close(rx_fd);
256 #endif
257
258 cleanup:
259 (void)close(fd);
260 return ret;
261 }
262
main(void)263 int main(void)
264 {
265 const struct device *dev = DEVICE_DT_GET(DT_CHOSEN(zephyr_canbus));
266 int ret;
267 int fd;
268
269 #ifdef CONFIG_SAMPLE_SOCKETCAN_LOOPBACK_MODE
270 ret = can_set_mode(dev, CAN_MODE_LOOPBACK);
271 if (ret != 0) {
272 LOG_ERR("Cannot set CAN loopback mode (%d)", ret);
273 return 0;
274 }
275 #endif
276
277 ret = can_start(dev);
278 if (ret != 0) {
279 LOG_ERR("Cannot start CAN controller (%d)", ret);
280 return 0;
281 }
282
283 /* Let the device start before doing anything */
284 k_sleep(K_SECONDS(2));
285
286 fd = setup_socket();
287 if (fd < 0) {
288 LOG_ERR("Cannot start CAN application (%d)", fd);
289 return 0;
290 }
291
292 rx(INT_TO_POINTER(fd), NULL, NULL);
293 return 0;
294 }
295
