1 /*
2 * CAN module object for Linux socketCAN.
3 *
4 * This file is a template for other microcontrollers.
5 *
6 * @ingroup CO_driver
7 * @author Janez Paternoster, Martin Wagner
8 * @copyright 2004 - 2015 Janez Paternoster, 2017 - 2020 Neuberger Gebaeudeautomation GmbH
9 *
10 *
11 * This file is part of CANopenNode, an opensource CANopen Stack.
12 * Project home page is <https://github.com/CANopenNode/CANopenNode>.
13 * For more information on CANopen see <http://www.can-cia.org/>.
14 *
15 * Licensed under the Apache License, Version 2.0 (the "License");
16 * you may not use this file except in compliance with the License.
17 * You may obtain a copy of the License at
18 *
19 * http://www.apache.org/licenses/LICENSE-2.0
20 *
21 * Unless required by applicable law or agreed to in writing, software
22 * distributed under the License is distributed on an "AS IS" BASIS,
23 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
24 * See the License for the specific language governing permissions and
25 * limitations under the License.
26 */
27
28 #include <string.h>
29 #include <stdlib.h>
30 #include <unistd.h>
31 #include <errno.h>
32 #include <linux/can/raw.h>
33 #include <linux/can/error.h>
34 #include <linux/net_tstamp.h>
35 #include <sys/socket.h>
36 #include <sys/epoll.h>
37
38 #include "CO_driver.h"
39
40 #if defined CO_DRIVER_ERROR_REPORTING
41 #if __has_include("syslog1/log.h")
42 #include "syslog/log.h"
43 #include "msgs.h"
44 #else
45 #include "CO_msgs.h"
46 #endif
47 #else
48 #define log_printf(macropar_prio, macropar_message, ...)
49 #endif
50
51 #if __has_include("CO_Emergency.h")
52 #include "CO_Emergency.h"
53 #define USE_EMERGENCY_OBJECT
54 #endif
55
56 pthread_mutex_t CO_EMCY_mutex = PTHREAD_MUTEX_INITIALIZER;
57 pthread_mutex_t CO_OD_mutex = PTHREAD_MUTEX_INITIALIZER;
58
59 #ifndef CO_DRIVER_MULTI_INTERFACE
60 static CO_ReturnError_t CO_CANmodule_addInterface(CO_CANmodule_t *CANmodule, const void *CANdriverState);
61 #endif
62
63 #ifdef CO_DRIVER_MULTI_INTERFACE
64
65 static const uint32_t CO_INVALID_COB_ID = 0xffffffff;
66
67 /******************************************************************************/
CO_CANsetIdentToIndex(uint32_t * lookup,uint32_t index,uint32_t identNew,uint32_t identCurrent)68 void CO_CANsetIdentToIndex(
69 uint32_t *lookup,
70 uint32_t index,
71 uint32_t identNew,
72 uint32_t identCurrent)
73 {
74 /* entry changed, remove old one */
75 if (identCurrent<CO_CAN_MSG_SFF_MAX_COB_ID && identNew!=identCurrent) {
76 lookup[identCurrent] = CO_INVALID_COB_ID;
77 }
78
79 /* check if this COB ID is part of the table */
80 if (identNew > CO_CAN_MSG_SFF_MAX_COB_ID) {
81 return;
82 }
83
84 /* Special case COB ID "0" -> valid value in *xArray[0] (CO_*CAN_NMT),
85 * "entry unconfigured" for all others */
86 if (identNew == 0) {
87 if (index == 0) {
88 lookup[0] = 0;
89 }
90 }
91 else {
92 lookup[identNew] = index;
93 }
94 }
95
96
97 /******************************************************************************/
CO_CANgetIndexFromIdent(uint32_t * lookup,uint32_t ident)98 static uint32_t CO_CANgetIndexFromIdent(
99 uint32_t *lookup,
100 uint32_t ident)
101 {
102 /* check if this COB ID is part of the table */
103 if (ident > CO_CAN_MSG_SFF_MAX_COB_ID) {
104 return CO_INVALID_COB_ID;
105 }
106
107 return lookup[ident];
108 }
109
110 #endif
111
112
113 /** Disable socketCAN rx *****************************************************/
disableRx(CO_CANmodule_t * CANmodule)114 static CO_ReturnError_t disableRx(CO_CANmodule_t *CANmodule)
115 {
116 int ret;
117 uint32_t i;
118 CO_ReturnError_t retval;
119
120 /* insert a filter that doesn't match any messages */
121 retval = CO_ERROR_NO;
122 for (i = 0; i < CANmodule->CANinterfaceCount; i ++) {
123 ret = setsockopt(CANmodule->CANinterfaces[i].fd, SOL_CAN_RAW, CAN_RAW_FILTER,
124 NULL, 0);
125 if(ret < 0){
126 log_printf(LOG_ERR, CAN_FILTER_FAILED,
127 CANmodule->CANinterfaces[i].ifName);
128 log_printf(LOG_DEBUG, DBG_ERRNO, "setsockopt()");
129 retval = CO_ERROR_SYSCALL;
130 }
131 }
132
133 return retval;
134 }
135
136 /** Set up or update socketCAN rx filters *************************************/
setRxFilters(CO_CANmodule_t * CANmodule)137 static CO_ReturnError_t setRxFilters(CO_CANmodule_t *CANmodule)
138 {
139 int ret;
140 int i;
141 int count;
142 CO_ReturnError_t retval;
143
144 struct can_filter rxFiltersCpy[CANmodule->rxSize];
145
146 count = 0;
147 /* remove unused entries ( id == 0 and mask == 0 ) as they would act as
148 * "pass all" filter */
149 for (i = 0; i < CANmodule->rxSize; i ++) {
150 if ((CANmodule->rxFilter[i].can_id != 0) ||
151 (CANmodule->rxFilter[i].can_mask != 0)) {
152
153 rxFiltersCpy[count] = CANmodule->rxFilter[i];
154
155 count ++;
156 }
157 }
158
159 if (count == 0) {
160 /* No filter is set, disable RX */
161 return disableRx(CANmodule);
162 }
163
164 retval = CO_ERROR_NO;
165 for (i = 0; i < CANmodule->CANinterfaceCount; i ++) {
166 ret = setsockopt(CANmodule->CANinterfaces[i].fd, SOL_CAN_RAW, CAN_RAW_FILTER,
167 rxFiltersCpy, sizeof(struct can_filter) * count);
168 if(ret < 0){
169 log_printf(LOG_ERR, CAN_FILTER_FAILED,
170 CANmodule->CANinterfaces[i].ifName);
171 log_printf(LOG_DEBUG, DBG_ERRNO, "setsockopt()");
172 retval = CO_ERROR_SYSCALL;
173 }
174 }
175
176 return retval;
177 }
178
179
180 /******************************************************************************/
CO_CANsetConfigurationMode(void * CANdriverState)181 void CO_CANsetConfigurationMode(void *CANdriverState)
182 {
183 /* Can't do anything because no reference to CANmodule_t is provided */
184 }
185
186
187 /******************************************************************************/
CO_CANsetNormalMode(CO_CANmodule_t * CANmodule)188 void CO_CANsetNormalMode(CO_CANmodule_t *CANmodule)
189 {
190 CO_ReturnError_t ret;
191
192 CANmodule->CANnormal = false;
193
194 if(CANmodule != NULL) {
195 ret = setRxFilters(CANmodule);
196 if (ret == CO_ERROR_NO) {
197 /* Put CAN module in normal mode */
198 CANmodule->CANnormal = true;
199 }
200 }
201 }
202
203
204 /******************************************************************************/
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)205 CO_ReturnError_t CO_CANmodule_init(
206 CO_CANmodule_t *CANmodule,
207 void *CANdriverState,
208 CO_CANrx_t rxArray[],
209 uint16_t rxSize,
210 CO_CANtx_t txArray[],
211 uint16_t txSize,
212 uint16_t CANbitRate)
213 {
214 int32_t ret;
215 uint16_t i;
216 struct epoll_event ev;
217
218 /* verify arguments */
219 if(CANmodule==NULL || rxArray==NULL || txArray==NULL){
220 return CO_ERROR_ILLEGAL_ARGUMENT;
221 }
222
223 /* Create epoll FD */
224 CANmodule->fdEpoll = epoll_create(1);
225 if(CANmodule->fdEpoll < 0){
226 log_printf(LOG_DEBUG, DBG_ERRNO, "epoll_create()");
227 CO_CANmodule_disable(CANmodule);
228 return CO_ERROR_SYSCALL;
229 }
230
231 /* Create notification pipe */
232 CANmodule->pipe = CO_NotifyPipeCreate();
233 if (CANmodule->pipe==NULL) {
234 log_printf(LOG_DEBUG, DBG_ERRNO, "pipe");
235 CO_CANmodule_disable(CANmodule);
236 return CO_ERROR_OUT_OF_MEMORY;
237 }
238 /* ...and add it to epoll */
239 ev.events = EPOLLIN;
240 ev.data.fd = CO_NotifyPipeGetFd(CANmodule->pipe);
241 ret = epoll_ctl(CANmodule->fdEpoll, EPOLL_CTL_ADD, ev.data.fd, &ev);
242 if(ret < 0){
243 log_printf(LOG_DEBUG, DBG_ERRNO, "epoll_ctl(pipe)");
244 CO_CANmodule_disable(CANmodule);
245 return CO_ERROR_SYSCALL;
246 }
247
248 /* Configure object variables */
249 CANmodule->CANinterfaces = NULL;
250 CANmodule->CANinterfaceCount = 0;
251 CANmodule->rxArray = rxArray;
252 CANmodule->rxSize = rxSize;
253 CANmodule->txArray = txArray;
254 CANmodule->txSize = txSize;
255 CANmodule->CANnormal = false;
256 CANmodule->em = NULL; //this is set inside CO_Emergency.c init function!
257 CANmodule->fdTimerRead = -1;
258 #ifdef CO_DRIVER_MULTI_INTERFACE
259 for (i = 0; i < CO_CAN_MSG_SFF_MAX_COB_ID; i++) {
260 CANmodule->rxIdentToIndex[i] = CO_INVALID_COB_ID;
261 CANmodule->txIdentToIndex[i] = CO_INVALID_COB_ID;
262 }
263 #endif
264
265 /* initialize socketCAN filters
266 * CAN module filters will be configured with CO_CANrxBufferInit()
267 * functions, called by separate CANopen init functions */
268 CANmodule->rxFilter = calloc(CANmodule->rxSize, sizeof(struct can_filter));
269 if(CANmodule->rxFilter == NULL){
270 log_printf(LOG_DEBUG, DBG_ERRNO, "malloc()");
271 return CO_ERROR_OUT_OF_MEMORY;
272 }
273
274 for(i=0U; i<rxSize; i++){
275 rxArray[i].ident = 0U;
276 rxArray[i].mask = 0xFFFFFFFFU;
277 rxArray[i].object = NULL;
278 rxArray[i].pFunct = NULL;
279 #ifdef CO_DRIVER_MULTI_INTERFACE
280 rxArray[i].CANdriverState = NULL;
281 rxArray[i].timestamp.tv_sec = 0;
282 rxArray[i].timestamp.tv_nsec = 0;
283 #endif
284 }
285
286 #ifndef CO_DRIVER_MULTI_INTERFACE
287 /* add one interface */
288 ret = CO_CANmodule_addInterface(CANmodule, CANdriverState);
289 if (ret != CO_ERROR_NO) {
290 CO_CANmodule_disable(CANmodule);
291 }
292 #else
293 ret = CO_ERROR_NO;
294 #endif
295 return ret;
296 }
297
298
299 /** enable socketCAN *********************************************************/
300 #ifndef CO_DRIVER_MULTI_INTERFACE
301 static
302 #endif
CO_CANmodule_addInterface(CO_CANmodule_t * CANmodule,const void * CANdriverState)303 CO_ReturnError_t CO_CANmodule_addInterface(
304 CO_CANmodule_t *CANmodule,
305 const void *CANdriverState)
306 {
307 int32_t ret;
308 int32_t tmp;
309 int32_t bytes;
310 char *ifName;
311 socklen_t sLen;
312 CO_CANinterface_t *interface;
313 struct sockaddr_can sockAddr;
314 struct epoll_event ev;
315 #ifdef CO_DRIVER_ERROR_REPORTING
316 can_err_mask_t err_mask;
317 #endif
318
319 if (CANmodule->CANnormal != false) {
320 /* can't change config now! */
321 return CO_ERROR_INVALID_STATE;
322 }
323
324 /* Add interface to interface list */
325 CANmodule->CANinterfaceCount ++;
326 CANmodule->CANinterfaces = realloc(CANmodule->CANinterfaces,
327 ((CANmodule->CANinterfaceCount) * sizeof(*CANmodule->CANinterfaces)));
328 if (CANmodule->CANinterfaces == NULL) {
329 log_printf(LOG_DEBUG, DBG_ERRNO, "malloc()");
330 return CO_ERROR_OUT_OF_MEMORY;
331 }
332 interface = &CANmodule->CANinterfaces[CANmodule->CANinterfaceCount - 1];
333
334 interface->CANdriverState = CANdriverState;
335 ifName = if_indextoname((uintptr_t)interface->CANdriverState, interface->ifName);
336 if (ifName == NULL) {
337 log_printf(LOG_DEBUG, DBG_ERRNO, "if_indextoname()");
338 return CO_ERROR_ILLEGAL_ARGUMENT;
339 }
340
341 /* Create socket */
342 interface->fd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
343 if(interface->fd < 0){
344 log_printf(LOG_DEBUG, DBG_ERRNO, "socket(can)");
345 return CO_ERROR_SYSCALL;
346 }
347
348 /* enable socket rx queue overflow detection */
349 tmp = 1;
350 ret = setsockopt(interface->fd, SOL_SOCKET, SO_RXQ_OVFL, &tmp, sizeof(tmp));
351 if(ret < 0){
352 log_printf(LOG_DEBUG, DBG_ERRNO, "setsockopt(ovfl)");
353 return CO_ERROR_SYSCALL;
354 }
355 #ifdef CO_DRIVER_MULTI_INTERFACE
356 /* enable software time stamp mode (hardware timestamps do not work properly
357 * on all devices)*/
358 tmp = (SOF_TIMESTAMPING_SOFTWARE |
359 SOF_TIMESTAMPING_RX_SOFTWARE);
360 ret = setsockopt(interface->fd, SOL_SOCKET, SO_TIMESTAMPING, &tmp, sizeof(tmp));
361 if (ret < 0) {
362 log_printf(LOG_DEBUG, DBG_ERRNO, "setsockopt(timestamping)");
363 return CO_ERROR_SYSCALL;
364 }
365 #endif
366
367 //todo - modify rx buffer size? first one needs root
368 //ret = setsockopt(fd, SOL_SOCKET, SO_RCVBUFFORCE, (void *)&bytes, sLen);
369 //ret = setsockopt(fd, SOL_SOCKET, SO_RCVBUF, (void *)&bytes, sLen);
370
371 /* print socket rx buffer size in bytes (In my experience, the kernel reserves
372 * around 450 bytes for each CAN message) */
373 sLen = sizeof(bytes);
374 getsockopt(interface->fd, SOL_SOCKET, SO_RCVBUF, (void *)&bytes, &sLen);
375 if (sLen == sizeof(bytes)) {
376 log_printf(LOG_INFO, CAN_SOCKET_BUF_SIZE, interface->ifName,
377 bytes / 446, bytes);
378 }
379
380 /* bind socket */
381 memset(&sockAddr, 0, sizeof(sockAddr));
382 sockAddr.can_family = AF_CAN;
383 sockAddr.can_ifindex = (uintptr_t)interface->CANdriverState;
384 ret = bind(interface->fd, (struct sockaddr*)&sockAddr, sizeof(sockAddr));
385 if(ret < 0){
386 log_printf(LOG_ERR, CAN_BINDING_FAILED, interface->ifName);
387 log_printf(LOG_DEBUG, DBG_ERRNO, "bind()");
388 return CO_ERROR_SYSCALL;
389 }
390
391 #ifdef CO_DRIVER_ERROR_REPORTING
392 CO_CANerror_init(&interface->errorhandler, interface->fd, interface->ifName);
393 /* set up error frame generation. What actually is available depends on your
394 * CAN kernel driver */
395 #ifdef DEBUG
396 err_mask = CAN_ERR_MASK; //enable ALL error frames
397 #else
398 err_mask = CAN_ERR_ACK | CAN_ERR_CRTL | CAN_ERR_BUSOFF | CAN_ERR_BUSERROR;
399 #endif
400 ret = setsockopt(interface->fd, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &err_mask,
401 sizeof(err_mask));
402 if(ret < 0){
403 log_printf(LOG_ERR, CAN_ERROR_FILTER_FAILED, interface->ifName);
404 log_printf(LOG_DEBUG, DBG_ERRNO, "setsockopt(can err)");
405 return CO_ERROR_SYSCALL;
406 }
407 #endif
408
409 /* Add socket to epoll */
410 ev.events = EPOLLIN;
411 ev.data.fd = interface->fd;
412 ret = epoll_ctl(CANmodule->fdEpoll, EPOLL_CTL_ADD, ev.data.fd, &ev);
413 if(ret < 0){
414 log_printf(LOG_DEBUG, DBG_ERRNO, "epoll_ctl(can)");
415 return CO_ERROR_SYSCALL;
416 }
417
418 /* rx is started by calling #CO_CANsetNormalMode() */
419 ret = disableRx(CANmodule);
420
421 return ret;
422 }
423
424
425 /******************************************************************************/
CO_CANmodule_disable(CO_CANmodule_t * CANmodule)426 void CO_CANmodule_disable(CO_CANmodule_t *CANmodule)
427 {
428 uint32_t i;
429 struct timespec wait;
430
431 if (CANmodule == NULL) {
432 return;
433 }
434
435 /* clear interfaces */
436 for (i = 0; i < CANmodule->CANinterfaceCount; i++) {
437 CO_CANinterface_t *interface = &CANmodule->CANinterfaces[i];
438
439 #ifdef CO_DRIVER_ERROR_REPORTING
440 CO_CANerror_disable(&interface->errorhandler);
441 #endif
442
443 epoll_ctl(CANmodule->fdEpoll, EPOLL_CTL_DEL, interface->fd, NULL);
444 close(interface->fd);
445 interface->fd = -1;
446 }
447 if (CANmodule->CANinterfaces != NULL) {
448 free(CANmodule->CANinterfaces);
449 }
450 CANmodule->CANinterfaceCount = 0;
451
452 /* cancel rx */
453 if (CANmodule->pipe != NULL) {
454 CO_NotifyPipeSend(CANmodule->pipe);
455 /* give some time for delivery */
456 wait.tv_sec = 0;
457 wait.tv_nsec = 50 /* ms */ * 1000000;
458 nanosleep(&wait, NULL);
459 CO_NotifyPipeFree(CANmodule->pipe);
460 }
461
462 if (CANmodule->fdEpoll >= 0) {
463 close(CANmodule->fdEpoll);
464 }
465 CANmodule->fdEpoll = -1;
466
467 if (CANmodule->rxFilter != NULL) {
468 free(CANmodule->rxFilter);
469 }
470 CANmodule->rxFilter = NULL;
471 }
472
473
474 /******************************************************************************/
CO_CANrxMsg_readIdent(const CO_CANrxMsg_t * rxMsg)475 uint16_t CO_CANrxMsg_readIdent(const CO_CANrxMsg_t *rxMsg)
476 {
477 /* remove socketCAN flags */
478 return (uint16_t) (rxMsg->ident & CAN_SFF_MASK);
479 }
480
481
482 /******************************************************************************/
CO_CANrxBufferInit(CO_CANmodule_t * CANmodule,uint16_t index,uint16_t ident,uint16_t mask,bool_t rtr,void * object,void (* pFunct)(void * object,const CO_CANrxMsg_t * message))483 CO_ReturnError_t CO_CANrxBufferInit(
484 CO_CANmodule_t *CANmodule,
485 uint16_t index,
486 uint16_t ident,
487 uint16_t mask,
488 bool_t rtr,
489 void *object,
490 void (*pFunct)(void *object, const CO_CANrxMsg_t *message))
491 {
492 CO_ReturnError_t ret = CO_ERROR_NO;
493
494 if((CANmodule!=NULL) && (index < CANmodule->rxSize)){
495 uint16_t i;
496 CO_CANrx_t *buffer;
497
498 /* check if COB ID is already used */
499 for (i = 0; i < CANmodule->rxSize; i ++) {
500 buffer = &CANmodule->rxArray[i];
501
502 if (i!=index && ident>0 && ident==buffer->ident) {
503 log_printf(LOG_DEBUG, DBG_CAN_RX_PARAM_FAILED, "duplicate entry");
504 ret = CO_ERROR_ILLEGAL_ARGUMENT;
505 }
506 }
507
508 if (ret == CO_ERROR_NO) {
509 /* buffer, which will be configured */
510 buffer = &CANmodule->rxArray[index];
511
512 #ifdef CO_DRIVER_MULTI_INTERFACE
513 CO_CANsetIdentToIndex(CANmodule->rxIdentToIndex, index, ident,
514 buffer->ident);
515 #endif
516
517 /* Configure object variables */
518 buffer->object = object;
519 buffer->pFunct = pFunct;
520 #ifdef CO_DRIVER_MULTI_INTERFACE
521 buffer->CANdriverState = NULL;
522 buffer->timestamp.tv_nsec = 0;
523 buffer->timestamp.tv_sec = 0;
524 #endif
525
526 /* CAN identifier and CAN mask, bit aligned with CAN module */
527 buffer->ident = ident & CAN_SFF_MASK;
528 if(rtr){
529 buffer->ident |= CAN_RTR_FLAG;
530 }
531 buffer->mask = (mask & CAN_SFF_MASK) | CAN_EFF_FLAG | CAN_RTR_FLAG;
532
533 /* Set CAN hardware module filter and mask. */
534 CANmodule->rxFilter[index].can_id = buffer->ident;
535 CANmodule->rxFilter[index].can_mask = buffer->mask;
536 if(CANmodule->CANnormal){
537 ret = setRxFilters(CANmodule);
538 }
539 }
540 }
541 else {
542 ret = CO_ERROR_ILLEGAL_ARGUMENT;
543 }
544
545 return ret;
546 }
547
548 #ifdef CO_DRIVER_MULTI_INTERFACE
549
550 /******************************************************************************/
CO_CANrxBuffer_getInterface(CO_CANmodule_t * CANmodule,uint16_t ident,const void ** const CANdriverStateRx,struct timespec * timestamp)551 bool_t CO_CANrxBuffer_getInterface(
552 CO_CANmodule_t *CANmodule,
553 uint16_t ident,
554 const void **const CANdriverStateRx,
555 struct timespec *timestamp)
556 {
557 CO_CANrx_t *buffer;
558
559 if (CANmodule == NULL){
560 return false;
561 }
562
563 const uint32_t index = CO_CANgetIndexFromIdent(CANmodule->rxIdentToIndex, ident);
564 if ((index == CO_INVALID_COB_ID) || (index > CANmodule->rxSize)) {
565 return false;
566 }
567 buffer = &CANmodule->rxArray[index];
568
569 /* return values */
570 if (CANdriverStateRx != NULL) {
571 *CANdriverStateRx = buffer->CANdriverState;
572 }
573 if (timestamp != NULL) {
574 *timestamp = buffer->timestamp;
575 }
576 if (buffer->CANdriverState != NULL) {
577 return true;
578 }
579 else {
580 return false;
581 }
582 }
583
584 #endif
585
586
587 /******************************************************************************/
CO_CANtxBufferInit(CO_CANmodule_t * CANmodule,uint16_t index,uint16_t ident,bool_t rtr,uint8_t noOfBytes,bool_t syncFlag)588 CO_CANtx_t *CO_CANtxBufferInit(
589 CO_CANmodule_t *CANmodule,
590 uint16_t index,
591 uint16_t ident,
592 bool_t rtr,
593 uint8_t noOfBytes,
594 bool_t syncFlag)
595 {
596 CO_CANtx_t *buffer = NULL;
597
598 if((CANmodule != NULL) && (index < CANmodule->txSize)){
599 /* get specific buffer */
600 buffer = &CANmodule->txArray[index];
601
602 #ifdef CO_DRIVER_MULTI_INTERFACE
603 CO_CANsetIdentToIndex(CANmodule->txIdentToIndex, index, ident, buffer->ident);
604 #endif
605
606 buffer->CANdriverState = NULL;
607
608 /* CAN identifier and rtr */
609 buffer->ident = ident & CAN_SFF_MASK;
610 if(rtr){
611 buffer->ident |= CAN_RTR_FLAG;
612 }
613 buffer->DLC = noOfBytes;
614 buffer->bufferFull = false;
615 buffer->syncFlag = syncFlag;
616 }
617
618 return buffer;
619 }
620
621 #ifdef CO_DRIVER_MULTI_INTERFACE
622
623 /******************************************************************************/
CO_CANtxBuffer_setInterface(CO_CANmodule_t * CANmodule,uint16_t ident,const void * CANdriverStateTx)624 CO_ReturnError_t CO_CANtxBuffer_setInterface(
625 CO_CANmodule_t *CANmodule,
626 uint16_t ident,
627 const void *CANdriverStateTx)
628 {
629 if (CANmodule != NULL) {
630 uint32_t index;
631
632 index = CO_CANgetIndexFromIdent(CANmodule->txIdentToIndex, ident);
633 if ((index == CO_INVALID_COB_ID) || (index > CANmodule->txSize)) {
634 return CO_ERROR_PARAMETERS;
635 }
636 CANmodule->txArray[index].CANdriverState = CANdriverStateTx;
637
638 return CO_ERROR_NO;
639 }
640 return CO_ERROR_PARAMETERS;
641 }
642
643 #endif
644
645 /******************************************************************************/
CO_CANCheckSendInterface(CO_CANmodule_t * CANmodule,CO_CANtx_t * buffer,CO_CANinterface_t * interface)646 static CO_ReturnError_t CO_CANCheckSendInterface(
647 CO_CANmodule_t *CANmodule,
648 CO_CANtx_t *buffer,
649 CO_CANinterface_t *interface)
650 {
651 CO_ReturnError_t err = CO_ERROR_NO;
652 #ifdef CO_DRIVER_ERROR_REPORTING
653 CO_CANinterfaceState_t ifState;
654 #endif
655 ssize_t n;
656
657 if (CANmodule==NULL || interface==NULL || interface->fd < 0) {
658 return CO_ERROR_PARAMETERS;
659 }
660
661 #ifdef CO_DRIVER_ERROR_REPORTING
662 ifState = CO_CANerror_txMsg(&interface->errorhandler);
663 switch (ifState) {
664 case CO_INTERFACE_ACTIVE:
665 /* continue */
666 break;
667 case CO_INTERFACE_LISTEN_ONLY:
668 /* silently drop message */
669 return CO_ERROR_NO;
670 default:
671 return CO_ERROR_INVALID_STATE;
672 }
673 #endif
674
675 do {
676 errno = 0;
677 n = send(interface->fd, buffer, CAN_MTU, MSG_DONTWAIT);
678 if (errno == EINTR) {
679 /* try again */
680 continue;
681 }
682 else if (errno == EAGAIN) {
683 /* socket queue full */
684 break;
685 }
686 else if (errno == ENOBUFS) {
687 /* socketCAN doesn't support blocking write. You can wait here for
688 * a few hundred us and then try again */
689 return CO_ERROR_TX_BUSY;
690 }
691 else if (n != CAN_MTU) {
692 break;
693 }
694 } while (errno != 0);
695
696 if(n != CAN_MTU){
697 #ifdef USE_EMERGENCY_OBJECT
698 CO_errorReport((CO_EM_t*)CANmodule->em, CO_EM_CAN_TX_OVERFLOW, CO_EMC_CAN_OVERRUN, 0);
699 #endif
700 log_printf(LOG_ERR, DBG_CAN_TX_FAILED, buffer->ident, interface->ifName);
701 log_printf(LOG_DEBUG, DBG_ERRNO, "send()");
702 err = CO_ERROR_TX_OVERFLOW;
703 }
704
705 return err;
706 }
707
708
709 /******************************************************************************/
CO_CANsend(CO_CANmodule_t * CANmodule,CO_CANtx_t * buffer)710 CO_ReturnError_t CO_CANsend(CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer)
711 {
712 CO_ReturnError_t err;
713 err = CO_CANCheckSend(CANmodule, buffer);
714 if (err == CO_ERROR_TX_BUSY) {
715 /* send doesn't have "busy" */
716 #ifdef USE_EMERGENCY_OBJECT
717 CO_errorReport((CO_EM_t*)CANmodule->em, CO_EM_CAN_TX_OVERFLOW, CO_EMC_CAN_OVERRUN, 0);
718 #endif
719 log_printf(LOG_ERR, DBG_CAN_TX_FAILED, buffer->ident, "CANx");
720 log_printf(LOG_DEBUG, DBG_ERRNO, "send()");
721 err = CO_ERROR_TX_OVERFLOW;
722 }
723 return err;
724 }
725
726
727 /******************************************************************************/
CO_CANCheckSend(CO_CANmodule_t * CANmodule,CO_CANtx_t * buffer)728 CO_ReturnError_t CO_CANCheckSend(CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer)
729 {
730 uint32_t i;
731 CO_ReturnError_t err = CO_ERROR_NO;
732
733 /* check on which interfaces to send this messages */
734 for (i = 0; i < CANmodule->CANinterfaceCount; i++) {
735 CO_CANinterface_t *interface = &CANmodule->CANinterfaces[i];
736
737 if ((buffer->CANdriverState == NULL) ||
738 buffer->CANdriverState == interface->CANdriverState) {
739
740 CO_ReturnError_t tmp;
741
742 /* match, use this one */
743 tmp = CO_CANCheckSendInterface(CANmodule, buffer, interface);
744 if (tmp) {
745 /* only last error is returned to callee */
746 err = tmp;
747 }
748 }
749 }
750
751 return err;
752 }
753
754
755 /******************************************************************************/
CO_CANclearPendingSyncPDOs(CO_CANmodule_t * CANmodule)756 void CO_CANclearPendingSyncPDOs(CO_CANmodule_t *CANmodule)
757 {
758 /* Messages are either written to the socket queue or dropped */
759 }
760
761
762 /******************************************************************************/
CO_CANverifyErrors(CO_CANmodule_t * CANmodule)763 void CO_CANverifyErrors(CO_CANmodule_t *CANmodule)
764 {
765 /* socketCAN doesn't support microcontroller-like error counters. If an
766 * error has occured, a special can message is created by the driver and
767 * received by the application like a regular message.
768 * Therefore, error counter evaluation is included in rx function.*/
769 }
770
771 /******************************************************************************/
CO_CANread(CO_CANmodule_t * CANmodule,CO_CANinterface_t * interface,struct can_frame * msg,struct timespec * timestamp)772 static CO_ReturnError_t CO_CANread(
773 CO_CANmodule_t *CANmodule,
774 CO_CANinterface_t *interface,
775 struct can_frame *msg,
776 struct timespec *timestamp)
777 {
778 int32_t n;
779 uint32_t dropped;
780 /* recvmsg - like read, but generates statistics about the socket
781 * example in berlios candump.c */
782 struct iovec iov;
783 struct msghdr msghdr;
784 char ctrlmsg[CMSG_SPACE(sizeof(struct timeval)) + CMSG_SPACE(sizeof(dropped))];
785 struct cmsghdr *cmsg;
786
787 iov.iov_base = msg;
788 iov.iov_len = sizeof(*msg);
789
790 msghdr.msg_name = NULL;
791 msghdr.msg_namelen = 0;
792 msghdr.msg_iov = &iov;
793 msghdr.msg_iovlen = 1;
794 msghdr.msg_control = &ctrlmsg;
795 msghdr.msg_controllen = sizeof(ctrlmsg);
796 msghdr.msg_flags = 0;
797
798 n = recvmsg(interface->fd, &msghdr, 0);
799 if (n != CAN_MTU) {
800 #ifdef USE_EMERGENCY_OBJECT
801 CO_errorReport((CO_EM_t*)CANmodule->em, CO_EM_CAN_RXB_OVERFLOW,
802 CO_EMC_CAN_OVERRUN, n);
803 #endif
804 log_printf(LOG_DEBUG, DBG_CAN_RX_FAILED, interface->ifName);
805 log_printf(LOG_DEBUG, DBG_ERRNO, "recvmsg()");
806 return CO_ERROR_SYSCALL;
807 }
808
809 /* check for rx queue overflow, get rx time */
810 for (cmsg = CMSG_FIRSTHDR(&msghdr);
811 cmsg && (cmsg->cmsg_level == SOL_SOCKET);
812 cmsg = CMSG_NXTHDR(&msghdr, cmsg)) {
813 if (cmsg->cmsg_type == SO_TIMESTAMPING) {
814 /* this is system time, not monotonic time! */
815 *timestamp = ((struct timespec*)CMSG_DATA(cmsg))[0];
816 }
817 else if (cmsg->cmsg_type == SO_RXQ_OVFL) {
818 dropped = *(uint32_t*)CMSG_DATA(cmsg);
819 if (dropped > CANmodule->rxDropCount) {
820 #ifdef USE_EMERGENCY_OBJECT
821 CO_errorReport((CO_EM_t*)CANmodule->em, CO_EM_CAN_RXB_OVERFLOW,
822 CO_EMC_COMMUNICATION, 0);
823 #endif
824 log_printf(LOG_ERR, CAN_RX_SOCKET_QUEUE_OVERFLOW,
825 interface->ifName, dropped);
826 }
827 CANmodule->rxDropCount = dropped;
828 //todo use this info!
829 }
830 }
831
832 return CO_ERROR_NO;
833 }
834
CO_CANrxMsg(CO_CANmodule_t * CANmodule,struct can_frame * msg,CO_CANrxMsg_t * buffer)835 static int32_t CO_CANrxMsg(
836 CO_CANmodule_t *CANmodule,
837 struct can_frame *msg,
838 CO_CANrxMsg_t *buffer)
839 {
840 int32_t retval;
841 const CO_CANrxMsg_t *rcvMsg; /* pointer to received message in CAN module */
842 uint16_t index; /* index of received message */
843 CO_CANrx_t *rcvMsgObj = NULL; /* receive message object from CO_CANmodule_t object. */
844 bool_t msgMatched = false;
845
846 /* CANopenNode can message is binary compatible to the socketCAN one, except
847 * for extension flags */
848 msg->can_id &= CAN_EFF_MASK;
849 rcvMsg = (CO_CANrxMsg_t *)msg;
850
851 /* Message has been received. Search rxArray from CANmodule for the
852 * same CAN-ID. */
853 rcvMsgObj = &CANmodule->rxArray[0];
854 for (index = 0; index < CANmodule->rxSize; index ++) {
855 if(((rcvMsg->ident ^ rcvMsgObj->ident) & rcvMsgObj->mask) == 0U){
856 msgMatched = true;
857 break;
858 }
859 rcvMsgObj++;
860 }
861 if(msgMatched) {
862 /* Call specific function, which will process the message */
863 if ((rcvMsgObj != NULL) && (rcvMsgObj->pFunct != NULL)){
864 rcvMsgObj->pFunct(rcvMsgObj->object, rcvMsg);
865 }
866 /* return message */
867 if (buffer != NULL) {
868 memcpy(buffer, rcvMsg, sizeof(*buffer));
869 }
870 retval = index;
871 }
872 else {
873 retval = -1;
874 }
875
876 return retval;
877 }
878
879 /******************************************************************************/
CO_CANrxWait(CO_CANmodule_t * CANmodule,int fdTimer,CO_CANrxMsg_t * buffer)880 int32_t CO_CANrxWait(CO_CANmodule_t *CANmodule, int fdTimer, CO_CANrxMsg_t *buffer)
881 {
882 int32_t retval;
883 int32_t ret;
884 #ifdef CO_DRIVER_MULTI_INTERFACE
885 const void *CANdriverState;
886 #endif
887 CO_ReturnError_t err;
888 CO_CANinterface_t *interface = NULL;
889 struct epoll_event ev[1];
890 struct can_frame msg;
891 struct timespec timestamp;
892
893 if (CANmodule==NULL || CANmodule->CANinterfaceCount==0) {
894 return -1;
895 }
896
897 if (fdTimer>=0 && fdTimer!=CANmodule->fdTimerRead) {
898 /* new timer, timer changed */
899 epoll_ctl(CANmodule->fdEpoll, EPOLL_CTL_DEL, CANmodule->fdTimerRead, NULL);
900 ev[0].events = EPOLLIN;
901 ev[0].data.fd = fdTimer;
902 ret = epoll_ctl(CANmodule->fdEpoll, EPOLL_CTL_ADD, ev[0].data.fd, &ev[0]);
903 if(ret < 0){
904 return -1;
905 }
906 CANmodule->fdTimerRead = fdTimer;
907 }
908
909 /*
910 * blocking read using epoll
911 */
912 do {
913 errno = 0;
914 ret = epoll_wait(CANmodule->fdEpoll, ev, sizeof(ev) / sizeof(ev[0]), -1);
915 if (errno == EINTR) {
916 /* try again */
917 continue;
918 }
919 else if (ret < 0) {
920 /* epoll failed */
921 return -1;
922 }
923 else if ((ev[0].events & (EPOLLERR | EPOLLHUP)) != 0) {
924 /* epoll detected close/error on socket. Try to pull event */
925 errno = 0;
926 recv(ev[0].data.fd, &msg, sizeof(msg), MSG_DONTWAIT);
927 log_printf(LOG_DEBUG, DBG_CAN_RX_EPOLL, ev[0].events, strerror(errno));
928 continue;
929 }
930 else if ((ev[0].events & EPOLLIN) != 0) {
931 /* one of the sockets is ready */
932 if ((ev[0].data.fd == CO_NotifyPipeGetFd(CANmodule->pipe)) ||
933 (ev[0].data.fd == fdTimer)) {
934 /* timer/pipe socket */
935 return -1;
936 }
937 else {
938 /* CAN socket */
939 uint32_t i;
940
941 for (i = 0; i < CANmodule->CANinterfaceCount; i ++) {
942 interface = &CANmodule->CANinterfaces[i];
943
944 if (ev[0].data.fd == interface->fd) {
945 #ifdef CO_DRIVER_MULTI_INTERFACE
946 /* get interface handle */
947 CANdriverState = interface->CANdriverState;
948 #endif
949 /* get message */
950 err = CO_CANread(CANmodule, interface, &msg, ×tamp);
951 if (err != CO_ERROR_NO) {
952 return -1;
953 }
954 /* no need to continue search */
955 break;
956 }
957 }
958 }
959 }
960 } while (errno != 0);
961
962 /*
963 * evaluate Rx
964 */
965 retval = -1;
966 if(CANmodule->CANnormal){
967
968 if (msg.can_id & CAN_ERR_FLAG) {
969 /* error msg */
970 #ifdef CO_DRIVER_ERROR_REPORTING
971 CO_CANerror_rxMsgError(&interface->errorhandler, &msg);
972 #endif
973 }
974 else {
975 /* data msg */
976 int32_t msgIndex;
977
978 #ifdef CO_DRIVER_ERROR_REPORTING
979 CO_CANerror_rxMsg(&interface->errorhandler);
980 #endif
981
982 msgIndex = CO_CANrxMsg(CANmodule, &msg, buffer);
983 if (msgIndex > -1) {
984 #ifdef CO_DRIVER_MULTI_INTERFACE
985 /* Store message info */
986 CANmodule->rxArray[msgIndex].timestamp = timestamp;
987 CANmodule->rxArray[msgIndex].CANdriverState = CANdriverState;
988 #endif
989 }
990 retval = msgIndex;
991 }
992 }
993 return retval;
994 }
995
