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