1 /*
2 * Copyright (c) 2019 Vestas Wind Systems A/S
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/kernel.h>
8 #include <zephyr/drivers/can.h>
9 #include <zephyr/init.h>
10 #include <zephyr/sys/util.h>
11
12 #include <canopennode.h>
13
14 #define LOG_LEVEL CONFIG_CANOPEN_LOG_LEVEL
15 #include <zephyr/logging/log.h>
16 LOG_MODULE_REGISTER(canopen_driver);
17
18 K_KERNEL_STACK_DEFINE(canopen_tx_workq_stack,
19 CONFIG_CANOPENNODE_TX_WORKQUEUE_STACK_SIZE);
20
21 struct k_work_q canopen_tx_workq;
22
23 struct canopen_tx_work_container {
24 struct k_work work;
25 CO_CANmodule_t *CANmodule;
26 };
27
28 struct canopen_tx_work_container canopen_tx_queue;
29
30 K_MUTEX_DEFINE(canopen_send_mutex);
31 K_MUTEX_DEFINE(canopen_emcy_mutex);
32 K_MUTEX_DEFINE(canopen_co_mutex);
33
34 static canopen_rxmsg_callback_t rxmsg_callback;
35
canopen_send_lock(void)36 inline void canopen_send_lock(void)
37 {
38 k_mutex_lock(&canopen_send_mutex, K_FOREVER);
39 }
40
canopen_send_unlock(void)41 inline void canopen_send_unlock(void)
42 {
43 k_mutex_unlock(&canopen_send_mutex);
44 }
45
canopen_emcy_lock(void)46 inline void canopen_emcy_lock(void)
47 {
48 k_mutex_lock(&canopen_emcy_mutex, K_FOREVER);
49 }
50
canopen_emcy_unlock(void)51 inline void canopen_emcy_unlock(void)
52 {
53 k_mutex_unlock(&canopen_emcy_mutex);
54 }
55
canopen_od_lock(void)56 inline void canopen_od_lock(void)
57 {
58 k_mutex_lock(&canopen_co_mutex, K_FOREVER);
59 }
60
canopen_od_unlock(void)61 inline void canopen_od_unlock(void)
62 {
63 k_mutex_unlock(&canopen_co_mutex);
64 }
65
canopen_set_rxmsg_callback(canopen_rxmsg_callback_t callback)66 void canopen_set_rxmsg_callback(canopen_rxmsg_callback_t callback)
67 {
68 rxmsg_callback = callback;
69 }
70
canopen_detach_all_rx_filters(CO_CANmodule_t * CANmodule)71 static void canopen_detach_all_rx_filters(CO_CANmodule_t *CANmodule)
72 {
73 uint16_t i;
74
75 if (!CANmodule || !CANmodule->rx_array || !CANmodule->configured) {
76 return;
77 }
78
79 for (i = 0U; i < CANmodule->rx_size; i++) {
80 if (CANmodule->rx_array[i].filter_id != -ENOSPC) {
81 can_remove_rx_filter(CANmodule->dev,
82 CANmodule->rx_array[i].filter_id);
83 CANmodule->rx_array[i].filter_id = -ENOSPC;
84 }
85 }
86 }
87
canopen_rx_callback(const struct device * dev,struct can_frame * frame,void * user_data)88 static void canopen_rx_callback(const struct device *dev, struct can_frame *frame, void *user_data)
89 {
90 CO_CANmodule_t *CANmodule = (CO_CANmodule_t *)user_data;
91 CO_CANrxMsg_t rxMsg;
92 CO_CANrx_t *buffer;
93 canopen_rxmsg_callback_t callback = rxmsg_callback;
94 int i;
95
96 ARG_UNUSED(dev);
97
98 /* Loop through registered rx buffers in priority order */
99 for (i = 0; i < CANmodule->rx_size; i++) {
100 buffer = &CANmodule->rx_array[i];
101
102 if (buffer->filter_id == -ENOSPC || buffer->pFunct == NULL) {
103 continue;
104 }
105
106 if (((frame->id ^ buffer->ident) & buffer->mask) == 0U) {
107 #ifdef CONFIG_CAN_ACCEPT_RTR
108 if (buffer->rtr && ((frame->flags & CAN_FRAME_RTR) == 0U)) {
109 continue;
110 }
111 #endif /* CONFIG_CAN_ACCEPT_RTR */
112 rxMsg.ident = frame->id;
113 rxMsg.DLC = frame->dlc;
114 memcpy(rxMsg.data, frame->data, frame->dlc);
115 buffer->pFunct(buffer->object, &rxMsg);
116 if (callback != NULL) {
117 callback();
118 }
119 break;
120 }
121 }
122 }
123
canopen_tx_callback(const struct device * dev,int error,void * arg)124 static void canopen_tx_callback(const struct device *dev, int error, void *arg)
125 {
126 CO_CANmodule_t *CANmodule = arg;
127
128 ARG_UNUSED(dev);
129
130 if (!CANmodule) {
131 LOG_ERR("failed to process CAN tx callback");
132 return;
133 }
134
135 if (error == 0) {
136 CANmodule->first_tx_msg = false;
137 }
138
139 k_work_submit_to_queue(&canopen_tx_workq, &canopen_tx_queue.work);
140 }
141
canopen_tx_retry(struct k_work * item)142 static void canopen_tx_retry(struct k_work *item)
143 {
144 struct canopen_tx_work_container *container =
145 CONTAINER_OF(item, struct canopen_tx_work_container, work);
146 CO_CANmodule_t *CANmodule = container->CANmodule;
147 struct can_frame frame;
148 CO_CANtx_t *buffer;
149 int err;
150 uint16_t i;
151
152 memset(&frame, 0, sizeof(frame));
153
154 CO_LOCK_CAN_SEND();
155
156 for (i = 0; i < CANmodule->tx_size; i++) {
157 buffer = &CANmodule->tx_array[i];
158 if (buffer->bufferFull) {
159 frame.id = buffer->ident;
160 frame.dlc = buffer->DLC;
161 frame.flags |= (buffer->rtr ? CAN_FRAME_RTR : 0);
162 memcpy(frame.data, buffer->data, buffer->DLC);
163
164 err = can_send(CANmodule->dev, &frame, K_NO_WAIT,
165 canopen_tx_callback, CANmodule);
166 if (err == -EAGAIN) {
167 break;
168 } else if (err != 0) {
169 LOG_ERR("failed to send CAN frame (err %d)",
170 err);
171 CO_errorReport(CANmodule->em,
172 CO_EM_GENERIC_SOFTWARE_ERROR,
173 CO_EMC_COMMUNICATION, 0);
174
175 }
176
177 buffer->bufferFull = false;
178 }
179 }
180
181 CO_UNLOCK_CAN_SEND();
182 }
183
CO_CANsetConfigurationMode(void * CANdriverState)184 void CO_CANsetConfigurationMode(void *CANdriverState)
185 {
186 struct canopen_context *ctx = (struct canopen_context *)CANdriverState;
187 int err;
188
189 err = can_stop(ctx->dev);
190 if (err != 0 && err != -EALREADY) {
191 LOG_ERR("failed to stop CAN interface (err %d)", err);
192 }
193 }
194
CO_CANsetNormalMode(CO_CANmodule_t * CANmodule)195 void CO_CANsetNormalMode(CO_CANmodule_t *CANmodule)
196 {
197 int err;
198
199 err = can_start(CANmodule->dev);
200 if (err != 0 && err != -EALREADY) {
201 LOG_ERR("failed to start CAN interface (err %d)", err);
202 return;
203 }
204
205 CANmodule->CANnormal = true;
206 }
207
CO_CANmodule_init(CO_CANmodule_t * CANmodule,void * CANdriverState,CO_CANrx_t rxArray[],uint16_t rxSize,CO_CANtx_t txArray[],uint16_t txSize,uint16_t CANbitRate)208 CO_ReturnError_t CO_CANmodule_init(CO_CANmodule_t *CANmodule,
209 void *CANdriverState,
210 CO_CANrx_t rxArray[], uint16_t rxSize,
211 CO_CANtx_t txArray[], uint16_t txSize,
212 uint16_t CANbitRate)
213 {
214 struct canopen_context *ctx = (struct canopen_context *)CANdriverState;
215 uint16_t i;
216 int err;
217 int max_filters;
218
219 LOG_DBG("rxSize = %d, txSize = %d", rxSize, txSize);
220
221 if (!CANmodule || !rxArray || !txArray || !CANdriverState) {
222 LOG_ERR("failed to initialize CAN module");
223 return CO_ERROR_ILLEGAL_ARGUMENT;
224 }
225
226 max_filters = can_get_max_filters(ctx->dev, false);
227 if (max_filters != -ENOSYS) {
228 if (max_filters < 0) {
229 LOG_ERR("unable to determine number of CAN RX filters");
230 return CO_ERROR_SYSCALL;
231 }
232
233 if (rxSize > max_filters) {
234 LOG_ERR("insufficient number of concurrent CAN RX filters"
235 " (needs %d, %d available)", rxSize, max_filters);
236 return CO_ERROR_OUT_OF_MEMORY;
237 } else if (rxSize < max_filters) {
238 LOG_DBG("excessive number of concurrent CAN RX filters enabled"
239 " (needs %d, %d available)", rxSize, max_filters);
240 }
241 }
242
243 canopen_detach_all_rx_filters(CANmodule);
244 canopen_tx_queue.CANmodule = CANmodule;
245
246 CANmodule->dev = ctx->dev;
247 CANmodule->rx_array = rxArray;
248 CANmodule->rx_size = rxSize;
249 CANmodule->tx_array = txArray;
250 CANmodule->tx_size = txSize;
251 CANmodule->CANnormal = false;
252 CANmodule->first_tx_msg = true;
253 CANmodule->errors = 0;
254 CANmodule->em = NULL;
255
256 for (i = 0U; i < rxSize; i++) {
257 rxArray[i].ident = 0U;
258 rxArray[i].pFunct = NULL;
259 rxArray[i].filter_id = -ENOSPC;
260 }
261
262 for (i = 0U; i < txSize; i++) {
263 txArray[i].bufferFull = false;
264 }
265
266 err = can_set_bitrate(CANmodule->dev, KHZ(CANbitRate));
267 if (err) {
268 LOG_ERR("failed to configure CAN bitrate (err %d)", err);
269 return CO_ERROR_ILLEGAL_ARGUMENT;
270 }
271
272 err = can_set_mode(CANmodule->dev, CAN_MODE_NORMAL);
273 if (err) {
274 LOG_ERR("failed to configure CAN interface (err %d)", err);
275 return CO_ERROR_ILLEGAL_ARGUMENT;
276 }
277
278 CANmodule->configured = true;
279
280 return CO_ERROR_NO;
281 }
282
CO_CANmodule_disable(CO_CANmodule_t * CANmodule)283 void CO_CANmodule_disable(CO_CANmodule_t *CANmodule)
284 {
285 int err;
286
287 if (!CANmodule || !CANmodule->dev) {
288 return;
289 }
290
291 canopen_detach_all_rx_filters(CANmodule);
292
293 err = can_stop(CANmodule->dev);
294 if (err != 0 && err != -EALREADY) {
295 LOG_ERR("failed to disable CAN interface (err %d)", err);
296 }
297 }
298
CO_CANrxMsg_readIdent(const CO_CANrxMsg_t * rxMsg)299 uint16_t CO_CANrxMsg_readIdent(const CO_CANrxMsg_t *rxMsg)
300 {
301 return rxMsg->ident;
302 }
303
CO_CANrxBufferInit(CO_CANmodule_t * CANmodule,uint16_t index,uint16_t ident,uint16_t mask,bool_t rtr,void * object,CO_CANrxBufferCallback_t pFunct)304 CO_ReturnError_t CO_CANrxBufferInit(CO_CANmodule_t *CANmodule, uint16_t index,
305 uint16_t ident, uint16_t mask, bool_t rtr,
306 void *object,
307 CO_CANrxBufferCallback_t pFunct)
308 {
309 struct can_filter filter;
310 CO_CANrx_t *buffer;
311
312 if (CANmodule == NULL) {
313 return CO_ERROR_ILLEGAL_ARGUMENT;
314 }
315
316 if (!pFunct || (index >= CANmodule->rx_size)) {
317 LOG_ERR("failed to initialize CAN rx buffer, illegal argument");
318 CO_errorReport(CANmodule->em, CO_EM_GENERIC_SOFTWARE_ERROR,
319 CO_EMC_SOFTWARE_INTERNAL, 0);
320 return CO_ERROR_ILLEGAL_ARGUMENT;
321 }
322
323 buffer = &CANmodule->rx_array[index];
324 buffer->object = object;
325 buffer->pFunct = pFunct;
326 buffer->ident = ident;
327 buffer->mask = mask;
328
329 #ifndef CONFIG_CAN_ACCEPT_RTR
330 if (rtr) {
331 LOG_ERR("request for RTR frames, but RTR frames are rejected");
332 CO_errorReport(CANmodule->em, CO_EM_GENERIC_SOFTWARE_ERROR,
333 CO_EMC_SOFTWARE_INTERNAL, 0);
334 return CO_ERROR_ILLEGAL_ARGUMENT;
335 }
336 #else /* !CONFIG_CAN_ACCEPT_RTR */
337 buffer->rtr = rtr;
338 #endif /* CONFIG_CAN_ACCEPT_RTR */
339
340 filter.flags = 0U;
341 filter.id = ident;
342 filter.mask = mask;
343
344 if (buffer->filter_id != -ENOSPC) {
345 can_remove_rx_filter(CANmodule->dev, buffer->filter_id);
346 }
347
348 buffer->filter_id = can_add_rx_filter(CANmodule->dev,
349 canopen_rx_callback,
350 CANmodule, &filter);
351 if (buffer->filter_id == -ENOSPC) {
352 LOG_ERR("failed to add CAN rx callback, no free filter");
353 CO_errorReport(CANmodule->em, CO_EM_MEMORY_ALLOCATION_ERROR,
354 CO_EMC_SOFTWARE_INTERNAL, 0);
355 return CO_ERROR_OUT_OF_MEMORY;
356 }
357
358 return CO_ERROR_NO;
359 }
360
CO_CANtxBufferInit(CO_CANmodule_t * CANmodule,uint16_t index,uint16_t ident,bool_t rtr,uint8_t noOfBytes,bool_t syncFlag)361 CO_CANtx_t *CO_CANtxBufferInit(CO_CANmodule_t *CANmodule, uint16_t index,
362 uint16_t ident, bool_t rtr, uint8_t noOfBytes,
363 bool_t syncFlag)
364 {
365 CO_CANtx_t *buffer;
366
367 if (CANmodule == NULL) {
368 return NULL;
369 }
370
371 if (index >= CANmodule->tx_size) {
372 LOG_ERR("failed to initialize CAN rx buffer, illegal argument");
373 CO_errorReport(CANmodule->em, CO_EM_GENERIC_SOFTWARE_ERROR,
374 CO_EMC_SOFTWARE_INTERNAL, 0);
375 return NULL;
376 }
377
378 buffer = &CANmodule->tx_array[index];
379 buffer->ident = ident;
380 buffer->rtr = rtr;
381 buffer->DLC = noOfBytes;
382 buffer->bufferFull = false;
383 buffer->syncFlag = syncFlag;
384
385 return buffer;
386 }
387
CO_CANsend(CO_CANmodule_t * CANmodule,CO_CANtx_t * buffer)388 CO_ReturnError_t CO_CANsend(CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer)
389 {
390 CO_ReturnError_t ret = CO_ERROR_NO;
391 struct can_frame frame;
392 int err;
393
394 if (!CANmodule || !CANmodule->dev || !buffer) {
395 return CO_ERROR_ILLEGAL_ARGUMENT;
396 }
397
398 memset(&frame, 0, sizeof(frame));
399
400 CO_LOCK_CAN_SEND();
401
402 if (buffer->bufferFull) {
403 if (!CANmodule->first_tx_msg) {
404 CO_errorReport(CANmodule->em, CO_EM_CAN_TX_OVERFLOW,
405 CO_EMC_CAN_OVERRUN, buffer->ident);
406 }
407 buffer->bufferFull = false;
408 ret = CO_ERROR_TX_OVERFLOW;
409 }
410
411 frame.id = buffer->ident;
412 frame.dlc = buffer->DLC;
413 frame.flags = (buffer->rtr ? CAN_FRAME_RTR : 0);
414 memcpy(frame.data, buffer->data, buffer->DLC);
415
416 err = can_send(CANmodule->dev, &frame, K_NO_WAIT, canopen_tx_callback,
417 CANmodule);
418 if (err == -EAGAIN) {
419 buffer->bufferFull = true;
420 } else if (err != 0) {
421 LOG_ERR("failed to send CAN frame (err %d)", err);
422 CO_errorReport(CANmodule->em, CO_EM_GENERIC_SOFTWARE_ERROR,
423 CO_EMC_COMMUNICATION, 0);
424 ret = CO_ERROR_TX_UNCONFIGURED;
425 }
426
427 CO_UNLOCK_CAN_SEND();
428
429 return ret;
430 }
431
CO_CANclearPendingSyncPDOs(CO_CANmodule_t * CANmodule)432 void CO_CANclearPendingSyncPDOs(CO_CANmodule_t *CANmodule)
433 {
434 bool_t tpdoDeleted = false;
435 CO_CANtx_t *buffer;
436 uint16_t i;
437
438 if (!CANmodule) {
439 return;
440 }
441
442 CO_LOCK_CAN_SEND();
443
444 for (i = 0; i < CANmodule->tx_size; i++) {
445 buffer = &CANmodule->tx_array[i];
446 if (buffer->bufferFull && buffer->syncFlag) {
447 buffer->bufferFull = false;
448 tpdoDeleted = true;
449 }
450 }
451
452 CO_UNLOCK_CAN_SEND();
453
454 if (tpdoDeleted) {
455 CO_errorReport(CANmodule->em, CO_EM_TPDO_OUTSIDE_WINDOW,
456 CO_EMC_COMMUNICATION, 0);
457 }
458 }
459
CO_CANverifyErrors(CO_CANmodule_t * CANmodule)460 void CO_CANverifyErrors(CO_CANmodule_t *CANmodule)
461 {
462 CO_EM_t *em = (CO_EM_t *)CANmodule->em;
463 struct can_bus_err_cnt err_cnt;
464 enum can_state state;
465 uint8_t rx_overflows;
466 uint32_t errors;
467 int err;
468
469 /*
470 * TODO: Zephyr lacks an API for reading the rx mailbox
471 * overflow counter.
472 */
473 rx_overflows = 0;
474
475 err = can_get_state(CANmodule->dev, &state, &err_cnt);
476 if (err != 0) {
477 LOG_ERR("failed to get CAN controller state (err %d)", err);
478 return;
479 }
480
481 errors = ((uint32_t)err_cnt.tx_err_cnt << 16) |
482 ((uint32_t)err_cnt.rx_err_cnt << 8) |
483 rx_overflows;
484
485 if (errors != CANmodule->errors) {
486 CANmodule->errors = errors;
487
488 if (state == CAN_STATE_BUS_OFF) {
489 /* Bus off */
490 CO_errorReport(em, CO_EM_CAN_TX_BUS_OFF,
491 CO_EMC_BUS_OFF_RECOVERED, errors);
492 } else {
493 /* Bus not off */
494 CO_errorReset(em, CO_EM_CAN_TX_BUS_OFF, errors);
495
496 if ((err_cnt.rx_err_cnt >= 96U) ||
497 (err_cnt.tx_err_cnt >= 96U)) {
498 /* Bus warning */
499 CO_errorReport(em, CO_EM_CAN_BUS_WARNING,
500 CO_EMC_NO_ERROR, errors);
501 } else {
502 /* Bus not warning */
503 CO_errorReset(em, CO_EM_CAN_BUS_WARNING,
504 errors);
505 }
506
507 if (err_cnt.rx_err_cnt >= 128U) {
508 /* Bus rx passive */
509 CO_errorReport(em, CO_EM_CAN_RX_BUS_PASSIVE,
510 CO_EMC_CAN_PASSIVE, errors);
511 } else {
512 /* Bus not rx passive */
513 CO_errorReset(em, CO_EM_CAN_RX_BUS_PASSIVE,
514 errors);
515 }
516
517 if (err_cnt.tx_err_cnt >= 128U &&
518 !CANmodule->first_tx_msg) {
519 /* Bus tx passive */
520 CO_errorReport(em, CO_EM_CAN_TX_BUS_PASSIVE,
521 CO_EMC_CAN_PASSIVE, errors);
522 } else if (CO_isError(em, CO_EM_CAN_TX_BUS_PASSIVE)) {
523 /* Bus not tx passive */
524 CO_errorReset(em, CO_EM_CAN_TX_BUS_PASSIVE,
525 errors);
526 CO_errorReset(em, CO_EM_CAN_TX_OVERFLOW,
527 errors);
528 }
529 }
530
531 /* This code can be activated if we can read the overflows*/
532 if (false && rx_overflows != 0U) {
533 CO_errorReport(em, CO_EM_CAN_RXB_OVERFLOW,
534 CO_EMC_CAN_OVERRUN, errors);
535 }
536 }
537 }
538
canopen_init(void)539 static int canopen_init(void)
540 {
541
542 k_work_queue_start(&canopen_tx_workq, canopen_tx_workq_stack,
543 K_KERNEL_STACK_SIZEOF(canopen_tx_workq_stack),
544 CONFIG_CANOPENNODE_TX_WORKQUEUE_PRIORITY, NULL);
545
546 k_thread_name_set(&canopen_tx_workq.thread, "canopen_tx_workq");
547
548 k_work_init(&canopen_tx_queue.work, canopen_tx_retry);
549
550 return 0;
551 }
552
553 SYS_INIT(canopen_init, APPLICATION, CONFIG_KERNEL_INIT_PRIORITY_DEFAULT);
554
