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