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 #include <zephyr/sys/util.h>
17
18 LOG_MODULE_REGISTER(can_loopback, CONFIG_CAN_LOG_LEVEL);
19
20 struct can_loopback_frame {
21 struct can_frame frame;
22 can_tx_callback_t cb;
23 void *cb_arg;
24 };
25
26 struct can_loopback_filter {
27 can_rx_callback_t rx_cb;
28 void *cb_arg;
29 struct can_filter filter;
30 };
31
32 struct can_loopback_config {
33 const struct can_driver_config common;
34 };
35
36 struct can_loopback_data {
37 struct can_driver_data common;
38 struct can_loopback_filter filters[CONFIG_CAN_MAX_FILTER];
39 struct k_mutex mtx;
40 struct k_msgq tx_msgq;
41 char msgq_buffer[CONFIG_CAN_LOOPBACK_TX_MSGQ_SIZE * sizeof(struct can_loopback_frame)];
42 struct k_thread tx_thread_data;
43
44 K_KERNEL_STACK_MEMBER(tx_thread_stack,
45 CONFIG_CAN_LOOPBACK_TX_THREAD_STACK_SIZE);
46 };
47
receive_frame(const struct device * dev,const struct can_frame * frame,struct can_loopback_filter * filter)48 static void receive_frame(const struct device *dev,
49 const struct can_frame *frame,
50 struct can_loopback_filter *filter)
51 {
52 struct can_frame frame_tmp = *frame;
53
54 LOG_DBG("Receiving %d bytes. Id: 0x%x, ID type: %s %s",
55 frame->dlc, frame->id,
56 (frame->flags & CAN_FRAME_IDE) != 0 ? "extended" : "standard",
57 (frame->flags & CAN_FRAME_RTR) != 0 ? ", RTR frame" : "");
58
59 filter->rx_cb(dev, &frame_tmp, filter->cb_arg);
60 }
61
tx_thread(void * arg1,void * arg2,void * arg3)62 static void tx_thread(void *arg1, void *arg2, void *arg3)
63 {
64 const struct device *dev = arg1;
65 struct can_loopback_data *data = dev->data;
66 struct can_loopback_frame frame;
67 struct can_loopback_filter *filter;
68 int ret;
69
70 ARG_UNUSED(arg2);
71 ARG_UNUSED(arg3);
72
73 while (1) {
74 ret = k_msgq_get(&data->tx_msgq, &frame, K_FOREVER);
75 if (ret < 0) {
76 LOG_DBG("Pend on TX queue returned without valid frame (err %d)", ret);
77 continue;
78 }
79 frame.cb(dev, 0, frame.cb_arg);
80
81 if ((data->common.mode & CAN_MODE_LOOPBACK) == 0U) {
82 continue;
83 }
84
85 #ifndef CONFIG_CAN_ACCEPT_RTR
86 if ((frame.frame.flags & CAN_FRAME_RTR) != 0U) {
87 continue;
88 }
89 #endif /* !CONFIG_CAN_ACCEPT_RTR */
90
91 k_mutex_lock(&data->mtx, K_FOREVER);
92
93 for (int i = 0; i < CONFIG_CAN_MAX_FILTER; i++) {
94 filter = &data->filters[i];
95 if (filter->rx_cb != NULL &&
96 can_frame_matches_filter(&frame.frame, &filter->filter)) {
97 receive_frame(dev, &frame.frame, filter);
98 }
99 }
100
101 k_mutex_unlock(&data->mtx);
102 }
103 }
104
can_loopback_send(const struct device * dev,const struct can_frame * frame,k_timeout_t timeout,can_tx_callback_t callback,void * user_data)105 static int can_loopback_send(const struct device *dev,
106 const struct can_frame *frame,
107 k_timeout_t timeout, can_tx_callback_t callback,
108 void *user_data)
109 {
110 struct can_loopback_data *data = dev->data;
111 struct can_loopback_frame loopback_frame;
112 uint8_t max_dlc = CAN_MAX_DLC;
113 int ret;
114
115 LOG_DBG("Sending %d bytes on %s. Id: 0x%x, ID type: %s %s",
116 frame->dlc, dev->name, frame->id,
117 (frame->flags & CAN_FRAME_IDE) != 0 ? "extended" : "standard",
118 (frame->flags & CAN_FRAME_RTR) != 0 ? ", RTR frame" : "");
119
120 #ifdef CONFIG_CAN_FD_MODE
121 if ((frame->flags & ~(CAN_FRAME_IDE | CAN_FRAME_RTR |
122 CAN_FRAME_FDF | CAN_FRAME_BRS)) != 0) {
123 LOG_ERR("unsupported CAN frame flags 0x%02x", frame->flags);
124 return -ENOTSUP;
125 }
126
127 if ((frame->flags & CAN_FRAME_FDF) != 0) {
128 if ((data->common.mode & CAN_MODE_FD) == 0U) {
129 return -ENOTSUP;
130 }
131
132 max_dlc = CANFD_MAX_DLC;
133 }
134 #else /* CONFIG_CAN_FD_MODE */
135 if ((frame->flags & ~(CAN_FRAME_IDE | CAN_FRAME_RTR)) != 0) {
136 LOG_ERR("unsupported CAN frame flags 0x%02x", frame->flags);
137 return -ENOTSUP;
138 }
139 #endif /* !CONFIG_CAN_FD_MODE */
140
141 if (frame->dlc > max_dlc) {
142 LOG_ERR("DLC of %d exceeds maximum (%d)", frame->dlc, max_dlc);
143 return -EINVAL;
144 }
145
146 if (!data->common.started) {
147 return -ENETDOWN;
148 }
149
150 loopback_frame.frame = *frame;
151 loopback_frame.cb = callback;
152 loopback_frame.cb_arg = user_data;
153
154 ret = k_msgq_put(&data->tx_msgq, &loopback_frame, timeout);
155 if (ret < 0) {
156 LOG_DBG("TX queue full (err %d)", ret);
157 return -EAGAIN;
158 }
159
160 return 0;
161 }
162
163
get_free_filter(struct can_loopback_filter * filters)164 static inline int get_free_filter(struct can_loopback_filter *filters)
165 {
166 for (int i = 0; i < CONFIG_CAN_MAX_FILTER; i++) {
167 if (filters[i].rx_cb == NULL) {
168 return i;
169 }
170 }
171
172 return -ENOSPC;
173 }
174
can_loopback_add_rx_filter(const struct device * dev,can_rx_callback_t cb,void * cb_arg,const struct can_filter * filter)175 static int can_loopback_add_rx_filter(const struct device *dev, can_rx_callback_t cb,
176 void *cb_arg, const struct can_filter *filter)
177 {
178 struct can_loopback_data *data = dev->data;
179 struct can_loopback_filter *loopback_filter;
180 int filter_id;
181
182 LOG_DBG("Setting filter ID: 0x%x, mask: 0x%x", filter->id, filter->mask);
183
184 if ((filter->flags & ~(CAN_FILTER_IDE)) != 0) {
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
can_loopback_remove_rx_filter(const struct device * dev,int filter_id)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 < 0 || 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
can_loopback_get_capabilities(const struct device * dev,can_mode_t * cap)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
can_loopback_start(const struct device * dev)238 static int can_loopback_start(const struct device *dev)
239 {
240 struct can_loopback_data *data = dev->data;
241
242 if (data->common.started) {
243 return -EALREADY;
244 }
245
246 data->common.started = true;
247
248 return 0;
249 }
250
can_loopback_stop(const struct device * dev)251 static int can_loopback_stop(const struct device *dev)
252 {
253 struct can_loopback_data *data = dev->data;
254
255 if (!data->common.started) {
256 return -EALREADY;
257 }
258
259 data->common.started = false;
260
261 k_msgq_purge(&data->tx_msgq);
262
263 return 0;
264 }
265
can_loopback_set_mode(const struct device * dev,can_mode_t mode)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->common.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 #else
280 if ((mode & ~(CAN_MODE_LOOPBACK)) != 0) {
281 LOG_ERR("unsupported mode: 0x%08x", mode);
282 return -ENOTSUP;
283 }
284 #endif /* CONFIG_CAN_FD_MODE */
285
286 data->common.mode = mode;
287
288 return 0;
289 }
290
can_loopback_set_timing(const struct device * dev,const struct can_timing * timing)291 static int can_loopback_set_timing(const struct device *dev,
292 const struct can_timing *timing)
293 {
294 struct can_loopback_data *data = dev->data;
295
296 ARG_UNUSED(timing);
297
298 if (data->common.started) {
299 return -EBUSY;
300 }
301
302 return 0;
303 }
304
305 #ifdef CONFIG_CAN_FD_MODE
can_loopback_set_timing_data(const struct device * dev,const struct can_timing * timing)306 static int can_loopback_set_timing_data(const struct device *dev,
307 const struct can_timing *timing)
308 {
309 struct can_loopback_data *data = dev->data;
310
311 ARG_UNUSED(timing);
312
313 if (data->common.started) {
314 return -EBUSY;
315 }
316
317 return 0;
318 }
319 #endif /* CONFIG_CAN_FD_MODE */
320
can_loopback_get_state(const struct device * dev,enum can_state * state,struct can_bus_err_cnt * err_cnt)321 static int can_loopback_get_state(const struct device *dev, enum can_state *state,
322 struct can_bus_err_cnt *err_cnt)
323 {
324 struct can_loopback_data *data = dev->data;
325
326 if (state != NULL) {
327 if (data->common.started) {
328 *state = CAN_STATE_ERROR_ACTIVE;
329 } else {
330 *state = CAN_STATE_STOPPED;
331 }
332 }
333
334 if (err_cnt) {
335 err_cnt->tx_err_cnt = 0;
336 err_cnt->rx_err_cnt = 0;
337 }
338
339 return 0;
340 }
341
can_loopback_set_state_change_callback(const struct device * dev,can_state_change_callback_t cb,void * user_data)342 static void can_loopback_set_state_change_callback(const struct device *dev,
343 can_state_change_callback_t cb,
344 void *user_data)
345 {
346 ARG_UNUSED(dev);
347 ARG_UNUSED(cb);
348 ARG_UNUSED(user_data);
349 }
350
can_loopback_get_core_clock(const struct device * dev,uint32_t * rate)351 static int can_loopback_get_core_clock(const struct device *dev, uint32_t *rate)
352 {
353 ARG_UNUSED(dev);
354
355 /* Recommended CAN clock from CiA 601-3 */
356 *rate = MHZ(80);
357
358 return 0;
359 }
360
can_loopback_get_max_filters(const struct device * dev,bool ide)361 static int can_loopback_get_max_filters(const struct device *dev, bool ide)
362 {
363 ARG_UNUSED(ide);
364
365 return CONFIG_CAN_MAX_FILTER;
366 }
367
368 static DEVICE_API(can, can_loopback_driver_api) = {
369 .get_capabilities = can_loopback_get_capabilities,
370 .start = can_loopback_start,
371 .stop = can_loopback_stop,
372 .set_mode = can_loopback_set_mode,
373 .set_timing = can_loopback_set_timing,
374 .send = can_loopback_send,
375 .add_rx_filter = can_loopback_add_rx_filter,
376 .remove_rx_filter = can_loopback_remove_rx_filter,
377 .get_state = can_loopback_get_state,
378 .set_state_change_callback = can_loopback_set_state_change_callback,
379 .get_core_clock = can_loopback_get_core_clock,
380 .get_max_filters = can_loopback_get_max_filters,
381 /* Recommended configuration ranges from CiA 601-2 */
382 .timing_min = {
383 .sjw = 1,
384 .prop_seg = 0,
385 .phase_seg1 = 2,
386 .phase_seg2 = 2,
387 .prescaler = 1
388 },
389 .timing_max = {
390 .sjw = 128,
391 .prop_seg = 0,
392 .phase_seg1 = 256,
393 .phase_seg2 = 128,
394 .prescaler = 32
395 },
396 #ifdef CONFIG_CAN_FD_MODE
397 .set_timing_data = can_loopback_set_timing_data,
398 /* Recommended configuration ranges from CiA 601-2 */
399 .timing_data_min = {
400 .sjw = 1,
401 .prop_seg = 0,
402 .phase_seg1 = 1,
403 .phase_seg2 = 1,
404 .prescaler = 1
405 },
406 .timing_data_max = {
407 .sjw = 16,
408 .prop_seg = 0,
409 .phase_seg1 = 32,
410 .phase_seg2 = 16,
411 .prescaler = 32
412 },
413 #endif /* CONFIG_CAN_FD_MODE */
414 };
415
can_loopback_init(const struct device * dev)416 static int can_loopback_init(const struct device *dev)
417 {
418 struct can_loopback_data *data = dev->data;
419 k_tid_t tx_tid;
420
421 k_mutex_init(&data->mtx);
422
423 for (int i = 0; i < CONFIG_CAN_MAX_FILTER; i++) {
424 data->filters[i].rx_cb = NULL;
425 }
426
427 k_msgq_init(&data->tx_msgq, data->msgq_buffer, sizeof(struct can_loopback_frame),
428 CONFIG_CAN_LOOPBACK_TX_MSGQ_SIZE);
429
430 tx_tid = k_thread_create(&data->tx_thread_data, data->tx_thread_stack,
431 K_KERNEL_STACK_SIZEOF(data->tx_thread_stack),
432 tx_thread, (void *)dev, NULL, NULL,
433 CONFIG_CAN_LOOPBACK_TX_THREAD_PRIORITY,
434 0, K_NO_WAIT);
435 if (!tx_tid) {
436 LOG_ERR("ERROR spawning tx thread");
437 return -1;
438 }
439
440 k_thread_name_set(tx_tid, dev->name);
441
442 return 0;
443 }
444
445 #ifdef CONFIG_CAN_FD_MODE
446 #define CAN_LOOPBACK_MAX_BITRATE 8000000
447 #else /* CONFIG_CAN_FD_MODE */
448 #define CAN_LOOPBACK_MAX_BITRATE 1000000
449 #endif /* !CONFIG_CAN_FD_MODE */
450
451 #define CAN_LOOPBACK_INIT(inst) \
452 static const struct can_loopback_config can_loopback_config_##inst = { \
453 .common = CAN_DT_DRIVER_CONFIG_INST_GET(inst, 0, CAN_LOOPBACK_MAX_BITRATE), \
454 }; \
455 \
456 static struct can_loopback_data can_loopback_data_##inst; \
457 \
458 CAN_DEVICE_DT_INST_DEFINE(inst, can_loopback_init, NULL, \
459 &can_loopback_data_##inst, \
460 &can_loopback_config_##inst, \
461 POST_KERNEL, CONFIG_CAN_INIT_PRIORITY, \
462 &can_loopback_driver_api);
463
464 DT_INST_FOREACH_STATUS_OKAY(CAN_LOOPBACK_INIT)
465
