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