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 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