1 /*
2 * Copyright (c) 2022 Martin Jäger <martin@libre.solar>
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #define DT_DRV_COMPAT zephyr_native_posix_linux_can
8
9 #include <stdbool.h>
10 #include <stdio.h>
11 #include <string.h>
12
13 #include <zephyr/drivers/can.h>
14 #include <zephyr/kernel.h>
15 #include <zephyr/logging/log.h>
16 #include <zephyr/net/net_pkt.h>
17 #include <zephyr/net/socketcan.h>
18 #include <zephyr/net/socketcan_utils.h>
19
20 #include "can_native_posix_linux_socketcan.h"
21
22 LOG_MODULE_REGISTER(can_npl, CONFIG_CAN_LOG_LEVEL);
23
24 struct can_filter_context {
25 can_rx_callback_t rx_cb;
26 void *cb_arg;
27 struct can_filter filter;
28 };
29
30 struct can_npl_data {
31 struct can_filter_context filters[CONFIG_CAN_MAX_FILTER];
32 struct k_mutex filter_mutex;
33 struct k_sem tx_idle;
34 can_tx_callback_t tx_callback;
35 void *tx_user_data;
36 bool loopback;
37 bool mode_fd;
38 int dev_fd; /* Linux socket file descriptor */
39 struct k_thread rx_thread;
40 bool started;
41
42 K_KERNEL_STACK_MEMBER(rx_thread_stack, CONFIG_ARCH_POSIX_RECOMMENDED_STACK_SIZE);
43 };
44
45 struct can_npl_config {
46 const char *if_name;
47 };
48
dispatch_frame(const struct device * dev,struct can_frame * frame)49 static void dispatch_frame(const struct device *dev, struct can_frame *frame)
50 {
51 struct can_npl_data *data = dev->data;
52 can_rx_callback_t callback;
53 struct can_frame tmp_frame;
54
55 k_mutex_lock(&data->filter_mutex, K_FOREVER);
56
57 for (int filter_id = 0; filter_id < ARRAY_SIZE(data->filters); filter_id++) {
58 if (data->filters[filter_id].rx_cb == NULL) {
59 continue;
60 }
61
62 if (!can_frame_matches_filter(frame, &data->filters[filter_id].filter)) {
63 continue;
64 }
65
66 /* Make a temporary copy in case the user modifies the message */
67 tmp_frame = *frame;
68
69 callback = data->filters[filter_id].rx_cb;
70 callback(dev, &tmp_frame, data->filters[filter_id].cb_arg);
71 }
72
73 k_mutex_unlock(&data->filter_mutex);
74 }
75
rx_thread(void * arg1,void * arg2,void * arg3)76 static void rx_thread(void *arg1, void *arg2, void *arg3)
77 {
78 const struct device *dev = arg1;
79 struct can_npl_data *data = dev->data;
80 struct socketcan_frame sframe;
81 struct can_frame frame;
82 bool msg_confirm;
83 int count;
84
85 ARG_UNUSED(arg2);
86 ARG_UNUSED(arg3);
87
88 LOG_DBG("Starting Linux SocketCAN RX thread");
89
90 while (true) {
91 while (linux_socketcan_poll_data(data->dev_fd) == 0) {
92 count = linux_socketcan_read_data(data->dev_fd, (void *)(&sframe),
93 sizeof(sframe), &msg_confirm);
94 if (msg_confirm) {
95 data->tx_callback(dev, 0, data->tx_user_data);
96 k_sem_give(&data->tx_idle);
97
98 if (!data->loopback) {
99 continue;
100 }
101 }
102 if ((count <= 0) || !data->started) {
103 break;
104 }
105
106 socketcan_to_can_frame(&sframe, &frame);
107
108 LOG_DBG("Received %d bytes. Id: 0x%x, ID type: %s %s",
109 frame.dlc, frame.id,
110 (frame.flags & CAN_FRAME_IDE) != 0 ? "extended" : "standard",
111 (frame.flags & CAN_FRAME_RTR) != 0 ? ", RTR frame" : "");
112
113 dispatch_frame(dev, &frame);
114 }
115
116 /* short sleep required to avoid blocking the whole native_posix process */
117 k_sleep(K_MSEC(1));
118 }
119 }
120
can_npl_send(const struct device * dev,const struct can_frame * frame,k_timeout_t timeout,can_tx_callback_t callback,void * user_data)121 static int can_npl_send(const struct device *dev, const struct can_frame *frame,
122 k_timeout_t timeout, can_tx_callback_t callback, void *user_data)
123 {
124 struct can_npl_data *data = dev->data;
125 struct socketcan_frame sframe;
126 uint8_t max_dlc = CAN_MAX_DLC;
127 size_t mtu = CAN_MTU;
128 int ret = -EIO;
129
130 LOG_DBG("Sending %d bytes on %s. Id: 0x%x, ID type: %s %s",
131 frame->dlc, dev->name, frame->id,
132 (frame->flags & CAN_FRAME_IDE) != 0 ? "extended" : "standard",
133 (frame->flags & CAN_FRAME_RTR) != 0 ? ", RTR frame" : "");
134
135 __ASSERT_NO_MSG(callback != NULL);
136
137 #ifdef CONFIG_CAN_FD_MODE
138 if ((frame->flags & ~(CAN_FRAME_IDE | CAN_FRAME_RTR |
139 CAN_FRAME_FDF | CAN_FRAME_BRS)) != 0) {
140 LOG_ERR("unsupported CAN frame flags 0x%02x", frame->flags);
141 return -ENOTSUP;
142 }
143
144 if ((frame->flags & CAN_FRAME_FDF) != 0) {
145 if (!data->mode_fd) {
146 return -ENOTSUP;
147 }
148
149 max_dlc = CANFD_MAX_DLC;
150 mtu = CANFD_MTU;
151 }
152 #else /* CONFIG_CAN_FD_MODE */
153 if ((frame->flags & ~(CAN_FRAME_IDE | CAN_FRAME_RTR)) != 0) {
154 LOG_ERR("unsupported CAN frame flags 0x%02x", frame->flags);
155 return -ENOTSUP;
156 }
157 #endif /* !CONFIG_CAN_FD_MODE */
158
159 if (frame->dlc > max_dlc) {
160 LOG_ERR("DLC of %d exceeds maximum (%d)", frame->dlc, max_dlc);
161 return -EINVAL;
162 }
163
164 if (data->dev_fd <= 0) {
165 LOG_ERR("No file descriptor: %d", data->dev_fd);
166 return -EIO;
167 }
168
169 if (!data->started) {
170 return -ENETDOWN;
171 }
172
173 socketcan_from_can_frame(frame, &sframe);
174
175 if (k_sem_take(&data->tx_idle, timeout) != 0) {
176 return -EAGAIN;
177 }
178
179 data->tx_callback = callback;
180 data->tx_user_data = user_data;
181
182 ret = linux_socketcan_write_data(data->dev_fd, &sframe, mtu);
183 if (ret < 0) {
184 LOG_ERR("Cannot send CAN data len %d (%d)", sframe.len, -errno);
185 }
186
187 return 0;
188 }
189
can_npl_add_rx_filter(const struct device * dev,can_rx_callback_t cb,void * cb_arg,const struct can_filter * filter)190 static int can_npl_add_rx_filter(const struct device *dev, can_rx_callback_t cb,
191 void *cb_arg, const struct can_filter *filter)
192 {
193 struct can_npl_data *data = dev->data;
194 struct can_filter_context *filter_ctx;
195 int filter_id = -ENOSPC;
196
197 LOG_DBG("Setting filter ID: 0x%x, mask: 0x%x", filter->id,
198 filter->mask);
199
200 #ifdef CONFIG_CAN_FD_MODE
201 if ((filter->flags & ~(CAN_FILTER_IDE | CAN_FILTER_DATA |
202 CAN_FILTER_RTR | CAN_FILTER_FDF)) != 0) {
203 #else
204 if ((filter->flags & ~(CAN_FILTER_IDE | CAN_FILTER_DATA | CAN_FILTER_RTR)) != 0) {
205 #endif
206 LOG_ERR("unsupported CAN filter flags 0x%02x", filter->flags);
207 return -ENOTSUP;
208 }
209
210 k_mutex_lock(&data->filter_mutex, K_FOREVER);
211
212 for (int i = 0; i < ARRAY_SIZE(data->filters); i++) {
213 if (data->filters[i].rx_cb == NULL) {
214 filter_id = i;
215 break;
216 }
217 }
218
219 if (filter_id < 0) {
220 LOG_ERR("No free filter left");
221 k_mutex_unlock(&data->filter_mutex);
222 return filter_id;
223 }
224
225 filter_ctx = &data->filters[filter_id];
226 filter_ctx->rx_cb = cb;
227 filter_ctx->cb_arg = cb_arg;
228 filter_ctx->filter = *filter;
229
230 k_mutex_unlock(&data->filter_mutex);
231
232 LOG_DBG("Filter added. ID: %d", filter_id);
233
234 return filter_id;
235 }
236
237 static void can_npl_remove_rx_filter(const struct device *dev, int filter_id)
238 {
239 struct can_npl_data *data = dev->data;
240
241 if (filter_id < 0 || filter_id >= ARRAY_SIZE(data->filters)) {
242 return;
243 }
244
245 k_mutex_lock(&data->filter_mutex, K_FOREVER);
246 data->filters[filter_id].rx_cb = NULL;
247 k_mutex_unlock(&data->filter_mutex);
248
249 LOG_DBG("Filter removed. ID: %d", filter_id);
250 }
251
252 static int can_npl_get_capabilities(const struct device *dev, can_mode_t *cap)
253 {
254 ARG_UNUSED(dev);
255
256 *cap = CAN_MODE_NORMAL | CAN_MODE_LOOPBACK;
257
258 #if CONFIG_CAN_FD_MODE
259 *cap |= CAN_MODE_FD;
260 #endif /* CONFIG_CAN_FD_MODE */
261
262 return 0;
263 }
264
265 static int can_npl_start(const struct device *dev)
266 {
267 struct can_npl_data *data = dev->data;
268
269 if (data->started) {
270 return -EALREADY;
271 }
272
273 data->started = true;
274
275 return 0;
276 }
277
278 static int can_npl_stop(const struct device *dev)
279 {
280 struct can_npl_data *data = dev->data;
281
282 if (!data->started) {
283 return -EALREADY;
284 }
285
286 data->started = false;
287
288 return 0;
289 }
290
291 static int can_npl_set_mode(const struct device *dev, can_mode_t mode)
292 {
293 struct can_npl_data *data = dev->data;
294
295 #ifdef CONFIG_CAN_FD_MODE
296 if ((mode & ~(CAN_MODE_LOOPBACK | CAN_MODE_FD)) != 0) {
297 LOG_ERR("unsupported mode: 0x%08x", mode);
298 return -ENOTSUP;
299 }
300 #else
301 if ((mode & ~(CAN_MODE_LOOPBACK)) != 0) {
302 LOG_ERR("unsupported mode: 0x%08x", mode);
303 return -ENOTSUP;
304 }
305 #endif /* CONFIG_CAN_FD_MODE */
306
307 if (data->started) {
308 return -EBUSY;
309 }
310
311 /* loopback is handled internally in rx_thread */
312 data->loopback = (mode & CAN_MODE_LOOPBACK) != 0;
313
314 data->mode_fd = (mode & CAN_MODE_FD) != 0;
315 linux_socketcan_set_mode_fd(data->dev_fd, data->mode_fd);
316
317 return 0;
318 }
319
320 static int can_npl_set_timing(const struct device *dev, const struct can_timing *timing)
321 {
322 struct can_npl_data *data = dev->data;
323
324 ARG_UNUSED(timing);
325
326 if (data->started) {
327 return -EBUSY;
328 }
329
330 return 0;
331 }
332
333 #ifdef CONFIG_CAN_FD_MODE
334 static int can_npl_set_timing_data(const struct device *dev, const struct can_timing *timing)
335 {
336 struct can_npl_data *data = dev->data;
337
338 ARG_UNUSED(timing);
339
340 if (data->started) {
341 return -EBUSY;
342 }
343
344 return 0;
345 }
346 #endif /* CONFIG_CAN_FD_MODE */
347
348 static int can_npl_get_state(const struct device *dev, enum can_state *state,
349 struct can_bus_err_cnt *err_cnt)
350 {
351 struct can_npl_data *data = dev->data;
352
353 if (state != NULL) {
354 if (!data->started) {
355 *state = CAN_STATE_STOPPED;
356 } else {
357 /* SocketCAN does not forward error frames by default */
358 *state = CAN_STATE_ERROR_ACTIVE;
359 }
360 }
361
362 if (err_cnt) {
363 err_cnt->tx_err_cnt = 0;
364 err_cnt->rx_err_cnt = 0;
365 }
366
367 return 0;
368 }
369
370 #ifndef CONFIG_CAN_AUTO_BUS_OFF_RECOVERY
371 static int can_npl_recover(const struct device *dev, k_timeout_t timeout)
372 {
373 struct can_npl_data *data = dev->data;
374
375 ARG_UNUSED(timeout);
376
377 if (!data->started) {
378 return -ENETDOWN;
379 }
380
381 return 0;
382 }
383 #endif /* CONFIG_CAN_AUTO_BUS_OFF_RECOVERY */
384
385 static void can_npl_set_state_change_callback(const struct device *dev,
386 can_state_change_callback_t cb,
387 void *user_data)
388 {
389 ARG_UNUSED(dev);
390 ARG_UNUSED(cb);
391 ARG_UNUSED(user_data);
392 }
393
394 static int can_npl_get_core_clock(const struct device *dev, uint32_t *rate)
395 {
396 /* Return 16MHz as an realistic value for the testcases */
397 *rate = 16000000;
398
399 return 0;
400 }
401
402 static int can_npl_get_max_filters(const struct device *dev, bool ide)
403 {
404 ARG_UNUSED(ide);
405
406 return CONFIG_CAN_MAX_FILTER;
407 }
408
409 static const struct can_driver_api can_npl_driver_api = {
410 .start = can_npl_start,
411 .stop = can_npl_stop,
412 .get_capabilities = can_npl_get_capabilities,
413 .set_mode = can_npl_set_mode,
414 .set_timing = can_npl_set_timing,
415 .send = can_npl_send,
416 .add_rx_filter = can_npl_add_rx_filter,
417 .remove_rx_filter = can_npl_remove_rx_filter,
418 .get_state = can_npl_get_state,
419 #ifndef CONFIG_CAN_AUTO_BUS_OFF_RECOVERY
420 .recover = can_npl_recover,
421 #endif
422 .set_state_change_callback = can_npl_set_state_change_callback,
423 .get_core_clock = can_npl_get_core_clock,
424 .get_max_filters = can_npl_get_max_filters,
425 .timing_min = {
426 .sjw = 0x1,
427 .prop_seg = 0x01,
428 .phase_seg1 = 0x01,
429 .phase_seg2 = 0x01,
430 .prescaler = 0x01
431 },
432 .timing_max = {
433 .sjw = 0x0F,
434 .prop_seg = 0x0F,
435 .phase_seg1 = 0x0F,
436 .phase_seg2 = 0x0F,
437 .prescaler = 0xFFFF
438 },
439 #ifdef CONFIG_CAN_FD_MODE
440 .set_timing_data = can_npl_set_timing_data,
441 .timing_data_min = {
442 .sjw = 0x1,
443 .prop_seg = 0x01,
444 .phase_seg1 = 0x01,
445 .phase_seg2 = 0x01,
446 .prescaler = 0x01
447 },
448 .timing_data_max = {
449 .sjw = 0x0F,
450 .prop_seg = 0x0F,
451 .phase_seg1 = 0x0F,
452 .phase_seg2 = 0x0F,
453 .prescaler = 0xFFFF
454 },
455 #endif /* CONFIG_CAN_FD_MODE */
456 };
457
458 static int can_npl_init(const struct device *dev)
459 {
460 const struct can_npl_config *cfg = dev->config;
461 struct can_npl_data *data = dev->data;
462
463 k_mutex_init(&data->filter_mutex);
464 k_sem_init(&data->tx_idle, 1, 1);
465
466 data->dev_fd = linux_socketcan_iface_open(cfg->if_name);
467 if (data->dev_fd < 0) {
468 LOG_ERR("Cannot open %s (%d)", cfg->if_name, data->dev_fd);
469 return -ENODEV;
470 }
471
472 k_thread_create(&data->rx_thread, data->rx_thread_stack,
473 K_KERNEL_STACK_SIZEOF(data->rx_thread_stack),
474 rx_thread, (void *)dev, NULL, NULL,
475 CONFIG_CAN_NATIVE_POSIX_LINUX_RX_THREAD_PRIORITY,
476 0, K_NO_WAIT);
477
478 LOG_DBG("Init of %s done", dev->name);
479
480 return 0;
481 }
482
483 #define CAN_NATIVE_POSIX_LINUX_INIT(inst) \
484 \
485 static const struct can_npl_config can_npl_cfg_##inst = { \
486 .if_name = DT_INST_PROP(inst, host_interface), \
487 }; \
488 \
489 static struct can_npl_data can_npl_data_##inst; \
490 \
491 CAN_DEVICE_DT_INST_DEFINE(inst, can_npl_init, NULL, \
492 &can_npl_data_##inst, &can_npl_cfg_##inst, \
493 POST_KERNEL, CONFIG_CAN_INIT_PRIORITY, \
494 &can_npl_driver_api);
495
496 DT_INST_FOREACH_STATUS_OKAY(CAN_NATIVE_POSIX_LINUX_INIT)
497
