1 /*
2 * Copyright (c) 2021 Vestas Wind Systems A/S
3 * Copyright (c) 2018 Alexander Wachter
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7
8 #define DT_DRV_COMPAT zephyr_can_loopback
9
10 #include <stdbool.h>
11 #include <string.h>
12
13 #include <zephyr/drivers/can.h>
14 #include <zephyr/kernel.h>
15 #include <zephyr/logging/log.h>
16
17 LOG_MODULE_REGISTER(can_loopback, CONFIG_CAN_LOG_LEVEL);
18
19 struct can_loopback_frame {
20 struct can_frame frame;
21 can_tx_callback_t cb;
22 void *cb_arg;
23 };
24
25 struct can_loopback_filter {
26 can_rx_callback_t rx_cb;
27 void *cb_arg;
28 struct can_filter filter;
29 };
30
31 struct can_loopback_data {
32 struct can_loopback_filter filters[CONFIG_CAN_MAX_FILTER];
33 struct k_mutex mtx;
34 struct k_msgq tx_msgq;
35 char msgq_buffer[CONFIG_CAN_LOOPBACK_TX_MSGQ_SIZE * sizeof(struct can_loopback_frame)];
36 struct k_thread tx_thread_data;
37 bool started;
38 bool loopback;
39 #ifdef CONFIG_CAN_FD_MODE
40 bool fd;
41 #endif /* CONFIG_CAN_FD_MODE */
42
43 K_KERNEL_STACK_MEMBER(tx_thread_stack,
44 CONFIG_CAN_LOOPBACK_TX_THREAD_STACK_SIZE);
45 };
46
receive_frame(const struct device * dev,const struct can_frame * frame,struct can_loopback_filter * filter)47 static void receive_frame(const struct device *dev,
48 const struct can_frame *frame,
49 struct can_loopback_filter *filter)
50 {
51 struct can_frame frame_tmp = *frame;
52
53 LOG_DBG("Receiving %d bytes. Id: 0x%x, ID type: %s %s",
54 frame->dlc, frame->id,
55 (frame->flags & CAN_FRAME_IDE) != 0 ? "extended" : "standard",
56 (frame->flags & CAN_FRAME_RTR) != 0 ? ", RTR frame" : "");
57
58 filter->rx_cb(dev, &frame_tmp, filter->cb_arg);
59 }
60
tx_thread(void * arg1,void * arg2,void * arg3)61 static void tx_thread(void *arg1, void *arg2, void *arg3)
62 {
63 const struct device *dev = arg1;
64 struct can_loopback_data *data = dev->data;
65 struct can_loopback_frame frame;
66 struct can_loopback_filter *filter;
67 int ret;
68
69 ARG_UNUSED(arg2);
70 ARG_UNUSED(arg3);
71
72 while (1) {
73 ret = k_msgq_get(&data->tx_msgq, &frame, K_FOREVER);
74 if (ret < 0) {
75 LOG_DBG("Pend on TX queue returned without valid frame (err %d)", ret);
76 continue;
77 }
78 frame.cb(dev, 0, frame.cb_arg);
79
80 if (!data->loopback) {
81 continue;
82 }
83
84 k_mutex_lock(&data->mtx, K_FOREVER);
85
86 for (int i = 0; i < CONFIG_CAN_MAX_FILTER; i++) {
87 filter = &data->filters[i];
88 if (filter->rx_cb != NULL &&
89 can_frame_matches_filter(&frame.frame, &filter->filter)) {
90 receive_frame(dev, &frame.frame, filter);
91 }
92 }
93
94 k_mutex_unlock(&data->mtx);
95 }
96 }
97
can_loopback_send(const struct device * dev,const struct can_frame * frame,k_timeout_t timeout,can_tx_callback_t callback,void * user_data)98 static int can_loopback_send(const struct device *dev,
99 const struct can_frame *frame,
100 k_timeout_t timeout, can_tx_callback_t callback,
101 void *user_data)
102 {
103 struct can_loopback_data *data = dev->data;
104 struct can_loopback_frame loopback_frame;
105 uint8_t max_dlc = CAN_MAX_DLC;
106 int ret;
107
108 __ASSERT_NO_MSG(callback != NULL);
109
110 LOG_DBG("Sending %d bytes on %s. Id: 0x%x, ID type: %s %s",
111 frame->dlc, dev->name, frame->id,
112 (frame->flags & CAN_FRAME_IDE) != 0 ? "extended" : "standard",
113 (frame->flags & CAN_FRAME_RTR) != 0 ? ", RTR frame" : "");
114
115 #ifdef CONFIG_CAN_FD_MODE
116 if ((frame->flags & ~(CAN_FRAME_IDE | CAN_FRAME_RTR |
117 CAN_FRAME_FDF | CAN_FRAME_BRS)) != 0) {
118 LOG_ERR("unsupported CAN frame flags 0x%02x", frame->flags);
119 return -ENOTSUP;
120 }
121
122 if ((frame->flags & CAN_FRAME_FDF) != 0) {
123 if (!data->fd) {
124 return -ENOTSUP;
125 }
126
127 max_dlc = CANFD_MAX_DLC;
128 }
129 #else /* CONFIG_CAN_FD_MODE */
130 if ((frame->flags & ~(CAN_FRAME_IDE | CAN_FRAME_RTR)) != 0) {
131 LOG_ERR("unsupported CAN frame flags 0x%02x", frame->flags);
132 return -ENOTSUP;
133 }
134 #endif /* !CONFIG_CAN_FD_MODE */
135
136 if (frame->dlc > max_dlc) {
137 LOG_ERR("DLC of %d exceeds maximum (%d)", frame->dlc, max_dlc);
138 return -EINVAL;
139 }
140
141 if (!data->started) {
142 return -ENETDOWN;
143 }
144
145 loopback_frame.frame = *frame;
146 loopback_frame.cb = callback;
147 loopback_frame.cb_arg = user_data;
148
149 ret = k_msgq_put(&data->tx_msgq, &loopback_frame, timeout);
150 if (ret < 0) {
151 LOG_DBG("TX queue full (err %d)", ret);
152 return -EAGAIN;
153 }
154
155 return 0;
156 }
157
158
get_free_filter(struct can_loopback_filter * filters)159 static inline int get_free_filter(struct can_loopback_filter *filters)
160 {
161 for (int i = 0; i < CONFIG_CAN_MAX_FILTER; i++) {
162 if (filters[i].rx_cb == NULL) {
163 return i;
164 }
165 }
166
167 return -ENOSPC;
168 }
169
can_loopback_add_rx_filter(const struct device * dev,can_rx_callback_t cb,void * cb_arg,const struct can_filter * filter)170 static int can_loopback_add_rx_filter(const struct device *dev, can_rx_callback_t cb,
171 void *cb_arg, const struct can_filter *filter)
172 {
173 struct can_loopback_data *data = dev->data;
174 struct can_loopback_filter *loopback_filter;
175 int filter_id;
176
177 LOG_DBG("Setting filter ID: 0x%x, mask: 0x%x", filter->id, filter->mask);
178
179 #ifdef CONFIG_CAN_FD_MODE
180 if ((filter->flags & ~(CAN_FILTER_IDE | CAN_FILTER_DATA |
181 CAN_FILTER_RTR | CAN_FILTER_FDF)) != 0) {
182 #else
183 if ((filter->flags & ~(CAN_FILTER_IDE | CAN_FILTER_DATA | CAN_FILTER_RTR)) != 0) {
184 #endif
185 LOG_ERR("unsupported CAN filter flags 0x%02x", filter->flags);
186 return -ENOTSUP;
187 }
188
189 k_mutex_lock(&data->mtx, K_FOREVER);
190 filter_id = get_free_filter(data->filters);
191
192 if (filter_id < 0) {
193 LOG_ERR("No free filter left");
194 k_mutex_unlock(&data->mtx);
195 return filter_id;
196 }
197
198 loopback_filter = &data->filters[filter_id];
199
200 loopback_filter->rx_cb = cb;
201 loopback_filter->cb_arg = cb_arg;
202 loopback_filter->filter = *filter;
203 k_mutex_unlock(&data->mtx);
204
205 LOG_DBG("Filter added. ID: %d", filter_id);
206
207 return filter_id;
208 }
209
210 static void can_loopback_remove_rx_filter(const struct device *dev, int filter_id)
211 {
212 struct can_loopback_data *data = dev->data;
213
214 if (filter_id >= ARRAY_SIZE(data->filters)) {
215 LOG_ERR("filter ID %d out-of-bounds", filter_id);
216 return;
217 }
218
219 LOG_DBG("Remove filter ID: %d", filter_id);
220 k_mutex_lock(&data->mtx, K_FOREVER);
221 data->filters[filter_id].rx_cb = NULL;
222 k_mutex_unlock(&data->mtx);
223 }
224
225 static int can_loopback_get_capabilities(const struct device *dev, can_mode_t *cap)
226 {
227 ARG_UNUSED(dev);
228
229 *cap = CAN_MODE_NORMAL | CAN_MODE_LOOPBACK;
230
231 #if CONFIG_CAN_FD_MODE
232 *cap |= CAN_MODE_FD;
233 #endif /* CONFIG_CAN_FD_MODE */
234
235 return 0;
236 }
237
238 static int can_loopback_start(const struct device *dev)
239 {
240 struct can_loopback_data *data = dev->data;
241
242 if (data->started) {
243 return -EALREADY;
244 }
245
246 data->started = true;
247
248 return 0;
249 }
250
251 static int can_loopback_stop(const struct device *dev)
252 {
253 struct can_loopback_data *data = dev->data;
254
255 if (!data->started) {
256 return -EALREADY;
257 }
258
259 data->started = false;
260
261 k_msgq_purge(&data->tx_msgq);
262
263 return 0;
264 }
265
266 static int can_loopback_set_mode(const struct device *dev, can_mode_t mode)
267 {
268 struct can_loopback_data *data = dev->data;
269
270 if (data->started) {
271 return -EBUSY;
272 }
273
274 #ifdef CONFIG_CAN_FD_MODE
275 if ((mode & ~(CAN_MODE_LOOPBACK | CAN_MODE_FD)) != 0) {
276 LOG_ERR("unsupported mode: 0x%08x", mode);
277 return -ENOTSUP;
278 }
279
280 data->fd = (mode & CAN_MODE_FD) != 0;
281 #else
282 if ((mode & ~(CAN_MODE_LOOPBACK)) != 0) {
283 LOG_ERR("unsupported mode: 0x%08x", mode);
284 return -ENOTSUP;
285 }
286 #endif /* CONFIG_CAN_FD_MODE */
287
288 data->loopback = (mode & CAN_MODE_LOOPBACK) != 0;
289
290 return 0;
291 }
292
293 static int can_loopback_set_timing(const struct device *dev,
294 const struct can_timing *timing)
295 {
296 struct can_loopback_data *data = dev->data;
297
298 ARG_UNUSED(timing);
299
300 if (data->started) {
301 return -EBUSY;
302 }
303
304 return 0;
305 }
306
307 #ifdef CONFIG_CAN_FD_MODE
308 static int can_loopback_set_timing_data(const struct device *dev,
309 const struct can_timing *timing)
310 {
311 struct can_loopback_data *data = dev->data;
312
313 ARG_UNUSED(timing);
314
315 if (data->started) {
316 return -EBUSY;
317 }
318
319 return 0;
320 }
321 #endif /* CONFIG_CAN_FD_MODE */
322
323 static int can_loopback_get_state(const struct device *dev, enum can_state *state,
324 struct can_bus_err_cnt *err_cnt)
325 {
326 struct can_loopback_data *data = dev->data;
327
328 if (state != NULL) {
329 if (data->started) {
330 *state = CAN_STATE_ERROR_ACTIVE;
331 } else {
332 *state = CAN_STATE_STOPPED;
333 }
334 }
335
336 if (err_cnt) {
337 err_cnt->tx_err_cnt = 0;
338 err_cnt->rx_err_cnt = 0;
339 }
340
341 return 0;
342 }
343
344 #ifndef CONFIG_CAN_AUTO_BUS_OFF_RECOVERY
345 static int can_loopback_recover(const struct device *dev, k_timeout_t timeout)
346 {
347 struct can_loopback_data *data = dev->data;
348
349 ARG_UNUSED(timeout);
350
351 if (!data->started) {
352 return -ENETDOWN;
353 }
354
355 return 0;
356 }
357 #endif /* CONFIG_CAN_AUTO_BUS_OFF_RECOVERY */
358
359 static void can_loopback_set_state_change_callback(const struct device *dev,
360 can_state_change_callback_t cb,
361 void *user_data)
362 {
363 ARG_UNUSED(dev);
364 ARG_UNUSED(cb);
365 ARG_UNUSED(user_data);
366 }
367
368 static int can_loopback_get_core_clock(const struct device *dev, uint32_t *rate)
369 {
370 /* Return 16MHz as an realistic value for the testcases */
371 *rate = 16000000;
372 return 0;
373 }
374
375 static int can_loopback_get_max_filters(const struct device *dev, bool ide)
376 {
377 ARG_UNUSED(ide);
378
379 return CONFIG_CAN_MAX_FILTER;
380 }
381
382 static const struct can_driver_api can_loopback_driver_api = {
383 .get_capabilities = can_loopback_get_capabilities,
384 .start = can_loopback_start,
385 .stop = can_loopback_stop,
386 .set_mode = can_loopback_set_mode,
387 .set_timing = can_loopback_set_timing,
388 .send = can_loopback_send,
389 .add_rx_filter = can_loopback_add_rx_filter,
390 .remove_rx_filter = can_loopback_remove_rx_filter,
391 .get_state = can_loopback_get_state,
392 #ifndef CONFIG_CAN_AUTO_BUS_OFF_RECOVERY
393 .recover = can_loopback_recover,
394 #endif
395 .set_state_change_callback = can_loopback_set_state_change_callback,
396 .get_core_clock = can_loopback_get_core_clock,
397 .get_max_filters = can_loopback_get_max_filters,
398 .timing_min = {
399 .sjw = 0x1,
400 .prop_seg = 0x01,
401 .phase_seg1 = 0x01,
402 .phase_seg2 = 0x01,
403 .prescaler = 0x01
404 },
405 .timing_max = {
406 .sjw = 0x0F,
407 .prop_seg = 0x0F,
408 .phase_seg1 = 0x0F,
409 .phase_seg2 = 0x0F,
410 .prescaler = 0xFFFF
411 },
412 #ifdef CONFIG_CAN_FD_MODE
413 .set_timing_data = can_loopback_set_timing_data,
414 .timing_data_min = {
415 .sjw = 0x1,
416 .prop_seg = 0x01,
417 .phase_seg1 = 0x01,
418 .phase_seg2 = 0x01,
419 .prescaler = 0x01
420 },
421 .timing_data_max = {
422 .sjw = 0x0F,
423 .prop_seg = 0x0F,
424 .phase_seg1 = 0x0F,
425 .phase_seg2 = 0x0F,
426 .prescaler = 0xFFFF
427 },
428 #endif /* CONFIG_CAN_FD_MODE */
429 };
430
431 static int can_loopback_init(const struct device *dev)
432 {
433 struct can_loopback_data *data = dev->data;
434 k_tid_t tx_tid;
435
436 k_mutex_init(&data->mtx);
437
438 for (int i = 0; i < CONFIG_CAN_MAX_FILTER; i++) {
439 data->filters[i].rx_cb = NULL;
440 }
441
442 k_msgq_init(&data->tx_msgq, data->msgq_buffer, sizeof(struct can_loopback_frame),
443 CONFIG_CAN_LOOPBACK_TX_MSGQ_SIZE);
444
445 tx_tid = k_thread_create(&data->tx_thread_data, data->tx_thread_stack,
446 K_KERNEL_STACK_SIZEOF(data->tx_thread_stack),
447 tx_thread, (void *)dev, NULL, NULL,
448 CONFIG_CAN_LOOPBACK_TX_THREAD_PRIORITY,
449 0, K_NO_WAIT);
450 if (!tx_tid) {
451 LOG_ERR("ERROR spawning tx thread");
452 return -1;
453 }
454
455 LOG_INF("Init of %s done", dev->name);
456
457 return 0;
458 }
459
460 #define CAN_LOOPBACK_INIT(inst) \
461 static struct can_loopback_data can_loopback_dev_data_##inst; \
462 \
463 CAN_DEVICE_DT_INST_DEFINE(inst, can_loopback_init, NULL, \
464 &can_loopback_dev_data_##inst, NULL, \
465 POST_KERNEL, CONFIG_CAN_INIT_PRIORITY,\
466 &can_loopback_driver_api);
467
468 DT_INST_FOREACH_STATUS_OKAY(CAN_LOOPBACK_INIT)
469
