From: Janez Date: Mon, 8 Jul 2024 23:02:26 +0000 (+0200) Subject: Format the .c files using clang-format v15. X-Git-Url: https://git.feebdaed.xyz/?a=commitdiff_plain;h=e3b61c3630bd4e1fa9352bae506d1ba8a03ab3fd;p=0xmirror%2FCANopenLinux.git Format the .c files using clang-format v15. --- diff --git a/CO_driver.c b/CO_driver.c index 5d3e54c..b7929b4 100644 --- a/CO_driver.c +++ b/CO_driver.c @@ -41,23 +41,17 @@ pthread_mutex_t CO_OD_mutex = PTHREAD_MUTEX_INITIALIZER; #endif #if CO_DRIVER_MULTI_INTERFACE == 0 -static CO_ReturnError_t CO_CANmodule_addInterface(CO_CANmodule_t *CANmodule, - int can_ifindex); +static CO_ReturnError_t CO_CANmodule_addInterface(CO_CANmodule_t* CANmodule, int can_ifindex); #endif - #if CO_DRIVER_MULTI_INTERFACE > 0 static const uint32_t CO_INVALID_COB_ID = 0xffffffff; -void CO_CANsetIdentToIndex( - uint32_t *lookup, - uint32_t index, - uint32_t identNew, - uint32_t identCurrent) -{ +void +CO_CANsetIdentToIndex(uint32_t* lookup, uint32_t index, uint32_t identNew, uint32_t identCurrent) { /* entry changed, remove old one */ - if (identCurrent CO_CAN_MSG_SFF_MAX_COB_ID) { return CO_INVALID_COB_ID; @@ -93,21 +83,18 @@ static uint32_t CO_CANgetIndexFromIdent( #endif /* CO_DRIVER_MULTI_INTERFACE */ - /* Disable socketCAN rx */ -static CO_ReturnError_t disableRx(CO_CANmodule_t *CANmodule) -{ +static CO_ReturnError_t +disableRx(CO_CANmodule_t* CANmodule) { uint32_t i; CO_ReturnError_t retval; /* insert a filter that doesn't match any messages */ retval = CO_ERROR_NO; - for (i = 0; i < CANmodule->CANinterfaceCount; i ++) { - int ret = setsockopt(CANmodule->CANinterfaces[i].fd, SOL_CAN_RAW, CAN_RAW_FILTER, - NULL, 0); - if(ret < 0){ - log_printf(LOG_ERR, CAN_FILTER_FAILED, - CANmodule->CANinterfaces[i].ifName); + for (i = 0; i < CANmodule->CANinterfaceCount; i++) { + int ret = setsockopt(CANmodule->CANinterfaces[i].fd, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0); + if (ret < 0) { + log_printf(LOG_ERR, CAN_FILTER_FAILED, CANmodule->CANinterfaces[i].ifName); log_printf(LOG_DEBUG, DBG_ERRNO, "setsockopt()"); retval = CO_ERROR_SYSCALL; } @@ -116,10 +103,9 @@ static CO_ReturnError_t disableRx(CO_CANmodule_t *CANmodule) return retval; } - /* Set up or update socketCAN rx filters */ -static CO_ReturnError_t setRxFilters(CO_CANmodule_t *CANmodule) -{ +static CO_ReturnError_t +setRxFilters(CO_CANmodule_t* CANmodule) { size_t i; int count; CO_ReturnError_t retval; @@ -127,15 +113,13 @@ static CO_ReturnError_t setRxFilters(CO_CANmodule_t *CANmodule) struct can_filter rxFiltersCpy[CANmodule->rxSize]; count = 0; - /* remove unused entries ( id == 0 and mask == 0 ) as they would act as - * "pass all" filter */ - for (i = 0; i < CANmodule->rxSize; i ++) { - if ((CANmodule->rxFilter[i].can_id != 0) || - (CANmodule->rxFilter[i].can_mask != 0)) { + /* remove unused entries ( id == 0 and mask == 0 ) as they would act as "pass all" filter */ + for (i = 0; i < CANmodule->rxSize; i++) { + if ((CANmodule->rxFilter[i].can_id != 0) || (CANmodule->rxFilter[i].can_mask != 0)) { rxFiltersCpy[count] = CANmodule->rxFilter[i]; - count ++; + count++; } } @@ -145,33 +129,30 @@ static CO_ReturnError_t setRxFilters(CO_CANmodule_t *CANmodule) } retval = CO_ERROR_NO; - for (i = 0; i < CANmodule->CANinterfaceCount; i ++) { - int ret = setsockopt(CANmodule->CANinterfaces[i].fd, SOL_CAN_RAW, CAN_RAW_FILTER, - rxFiltersCpy, sizeof(struct can_filter) * count); - if(ret < 0){ - log_printf(LOG_ERR, CAN_FILTER_FAILED, - CANmodule->CANinterfaces[i].ifName); - log_printf(LOG_DEBUG, DBG_ERRNO, "setsockopt()"); - retval = CO_ERROR_SYSCALL; - } + for (i = 0; i < CANmodule->CANinterfaceCount; i++) { + int ret = setsockopt(CANmodule->CANinterfaces[i].fd, SOL_CAN_RAW, CAN_RAW_FILTER, rxFiltersCpy, + sizeof(struct can_filter) * count); + if (ret < 0) { + log_printf(LOG_ERR, CAN_FILTER_FAILED, CANmodule->CANinterfaces[i].ifName); + log_printf(LOG_DEBUG, DBG_ERRNO, "setsockopt()"); + retval = CO_ERROR_SYSCALL; + } } return retval; } - -void CO_CANsetConfigurationMode(void *CANptr) -{ +void +CO_CANsetConfigurationMode(void* CANptr) { (void)CANptr; /* Can't do anything because no reference to CANmodule_t is provided */ } - -void CO_CANsetNormalMode(CO_CANmodule_t *CANmodule) -{ +void +CO_CANsetNormalMode(CO_CANmodule_t* CANmodule) { CO_ReturnError_t ret; - if(CANmodule != NULL) { + if (CANmodule != NULL) { CANmodule->CANnormal = false; ret = setRxFilters(CANmodule); if (ret == CO_ERROR_NO) { @@ -181,26 +162,19 @@ void CO_CANsetNormalMode(CO_CANmodule_t *CANmodule) } } - -CO_ReturnError_t CO_CANmodule_init( - CO_CANmodule_t *CANmodule, - void *CANptr, - CO_CANrx_t rxArray[], - uint16_t rxSize, - CO_CANtx_t txArray[], - uint16_t txSize, - uint16_t CANbitRate) -{ +CO_ReturnError_t +CO_CANmodule_init(CO_CANmodule_t* CANmodule, void* CANptr, CO_CANrx_t rxArray[], uint16_t rxSize, CO_CANtx_t txArray[], + uint16_t txSize, uint16_t CANbitRate) { int32_t ret; uint16_t i; (void)CANbitRate; /* verify arguments */ - if(CANmodule==NULL || CANptr == NULL || rxArray==NULL || txArray==NULL) { + if (CANmodule == NULL || CANptr == NULL || rxArray == NULL || txArray == NULL) { return CO_ERROR_ILLEGAL_ARGUMENT; } - CO_CANptrSocketCan_t *CANptrReal = (CO_CANptrSocketCan_t *)CANptr; + CO_CANptrSocketCan_t* CANptrReal = (CO_CANptrSocketCan_t*)CANptr; /* Configure object variables */ CANmodule->epoll_fd = CANptrReal->epoll_fd; @@ -221,16 +195,15 @@ CO_ReturnError_t CO_CANmodule_init( } #endif - /* initialize socketCAN filters - * CAN module filters will be configured with CO_CANrxBufferInit() - * functions, called by separate CANopen init functions */ + /* initialize socketCAN filters. CAN module filters will be configured with + * CO_CANrxBufferInit() functions, called by separate CANopen init functions */ CANmodule->rxFilter = calloc(CANmodule->rxSize, sizeof(struct can_filter)); - if(CANmodule->rxFilter == NULL){ + if (CANmodule->rxFilter == NULL) { log_printf(LOG_DEBUG, DBG_ERRNO, "malloc()"); return CO_ERROR_OUT_OF_MEMORY; } - for(i=0U; ican_ifindex); + ret = CO_CANmodule_addInterface(CANmodule, CANptrReal->can_ifindex); if (ret != CO_ERROR_NO) { CO_CANmodule_disable(CANmodule); return ret; @@ -252,20 +224,18 @@ CO_ReturnError_t CO_CANmodule_init( return CO_ERROR_NO; } - /* enable socketCAN */ #if CO_DRIVER_MULTI_INTERFACE == 0 static #endif -CO_ReturnError_t CO_CANmodule_addInterface(CO_CANmodule_t *CANmodule, - int can_ifindex) -{ + CO_ReturnError_t + CO_CANmodule_addInterface(CO_CANmodule_t* CANmodule, int can_ifindex) { int32_t ret; int32_t tmp; int32_t bytes; - char *ifName; + char* ifName; socklen_t sLen; - CO_CANinterface_t *interface; + CO_CANinterface_t* interface; struct sockaddr_can sockAddr; struct epoll_event ev = {0}; #if CO_DRIVER_ERROR_REPORTING > 0 @@ -278,9 +248,9 @@ CO_ReturnError_t CO_CANmodule_addInterface(CO_CANmodule_t *CANmodule, } /* Add interface to interface list */ - CANmodule->CANinterfaceCount ++; + CANmodule->CANinterfaceCount++; CANmodule->CANinterfaces = realloc(CANmodule->CANinterfaces, - ((CANmodule->CANinterfaceCount) * sizeof(*CANmodule->CANinterfaces))); + ((CANmodule->CANinterfaceCount) * sizeof(*CANmodule->CANinterfaces))); if (CANmodule->CANinterfaces == NULL) { log_printf(LOG_DEBUG, DBG_ERRNO, "malloc()"); return CO_ERROR_OUT_OF_MEMORY; @@ -296,7 +266,7 @@ CO_ReturnError_t CO_CANmodule_addInterface(CO_CANmodule_t *CANmodule, /* Create socket */ interface->fd = socket(PF_CAN, SOCK_RAW, CAN_RAW); - if(interface->fd < 0){ + if (interface->fd < 0) { log_printf(LOG_DEBUG, DBG_ERRNO, "socket(can)"); return CO_ERROR_SYSCALL; } @@ -304,32 +274,29 @@ CO_ReturnError_t CO_CANmodule_addInterface(CO_CANmodule_t *CANmodule, /* enable socket rx queue overflow detection */ tmp = 1; ret = setsockopt(interface->fd, SOL_SOCKET, SO_RXQ_OVFL, &tmp, sizeof(tmp)); - if(ret < 0){ + if (ret < 0) { log_printf(LOG_DEBUG, DBG_ERRNO, "setsockopt(ovfl)"); return CO_ERROR_SYSCALL; } - /* enable software time stamp mode (hardware timestamps do not work properly - * on all devices) */ - tmp = (SOF_TIMESTAMPING_SOFTWARE | - SOF_TIMESTAMPING_RX_SOFTWARE); + /* enable software time stamp mode (hardware timestamps do not work properly on all devices) */ + tmp = (SOF_TIMESTAMPING_SOFTWARE | SOF_TIMESTAMPING_RX_SOFTWARE); ret = setsockopt(interface->fd, SOL_SOCKET, SO_TIMESTAMPING, &tmp, sizeof(tmp)); if (ret < 0) { log_printf(LOG_DEBUG, DBG_ERRNO, "setsockopt(timestamping)"); return CO_ERROR_SYSCALL; } - //todo - modify rx buffer size? first one needs root - //ret = setsockopt(fd, SOL_SOCKET, SO_RCVBUFFORCE, (void *)&bytes, sLen); - //ret = setsockopt(fd, SOL_SOCKET, SO_RCVBUF, (void *)&bytes, sLen); + // todo - modify rx buffer size? first one needs root + // ret = setsockopt(fd, SOL_SOCKET, SO_RCVBUFFORCE, (void *)&bytes, sLen); + // ret = setsockopt(fd, SOL_SOCKET, SO_RCVBUF, (void *)&bytes, sLen); /* print socket rx buffer size in bytes (In my experience, the kernel reserves * around 450 bytes for each CAN message) */ sLen = sizeof(bytes); - getsockopt(interface->fd, SOL_SOCKET, SO_RCVBUF, (void *)&bytes, &sLen); + getsockopt(interface->fd, SOL_SOCKET, SO_RCVBUF, (void*)&bytes, &sLen); if (sLen == sizeof(bytes)) { - log_printf(LOG_INFO, CAN_SOCKET_BUF_SIZE, interface->ifName, - bytes / 446, bytes); + log_printf(LOG_INFO, CAN_SOCKET_BUF_SIZE, interface->ifName, bytes / 446, bytes); } /* bind socket */ @@ -337,7 +304,7 @@ CO_ReturnError_t CO_CANmodule_addInterface(CO_CANmodule_t *CANmodule, sockAddr.can_family = AF_CAN; sockAddr.can_ifindex = can_ifindex; ret = bind(interface->fd, (struct sockaddr*)&sockAddr, sizeof(sockAddr)); - if(ret < 0){ + if (ret < 0) { log_printf(LOG_ERR, CAN_BINDING_FAILED, interface->ifName); log_printf(LOG_DEBUG, DBG_ERRNO, "bind()"); return CO_ERROR_SYSCALL; @@ -345,16 +312,14 @@ CO_ReturnError_t CO_CANmodule_addInterface(CO_CANmodule_t *CANmodule, #if CO_DRIVER_ERROR_REPORTING > 0 CO_CANerror_init(&interface->errorhandler, interface->fd, interface->ifName); - /* set up error frame generation. What actually is available depends on your - * CAN kernel driver */ + /* set up error frame generation. What actually is available depends on your CAN kernel driver */ #ifdef DEBUG - err_mask = CAN_ERR_MASK; //enable ALL error frames + err_mask = CAN_ERR_MASK; // enable ALL error frames #else err_mask = CAN_ERR_ACK | CAN_ERR_CRTL | CAN_ERR_BUSOFF | CAN_ERR_BUSERROR; #endif - ret = setsockopt(interface->fd, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &err_mask, - sizeof(err_mask)); - if(ret < 0){ + ret = setsockopt(interface->fd, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &err_mask, sizeof(err_mask)); + if (ret < 0) { log_printf(LOG_ERR, CAN_ERROR_FILTER_FAILED, interface->ifName); log_printf(LOG_DEBUG, DBG_ERRNO, "setsockopt(can err)"); return CO_ERROR_SYSCALL; @@ -365,7 +330,7 @@ CO_ReturnError_t CO_CANmodule_addInterface(CO_CANmodule_t *CANmodule, ev.events = EPOLLIN; ev.data.fd = interface->fd; ret = epoll_ctl(CANmodule->epoll_fd, EPOLL_CTL_ADD, ev.data.fd, &ev); - if(ret < 0){ + if (ret < 0) { log_printf(LOG_DEBUG, DBG_ERRNO, "epoll_ctl(can)"); return CO_ERROR_SYSCALL; } @@ -376,9 +341,8 @@ CO_ReturnError_t CO_CANmodule_addInterface(CO_CANmodule_t *CANmodule, return ret; } - -void CO_CANmodule_disable(CO_CANmodule_t *CANmodule) -{ +void +CO_CANmodule_disable(CO_CANmodule_t* CANmodule) { uint32_t i; if (CANmodule == NULL) { @@ -389,7 +353,7 @@ void CO_CANmodule_disable(CO_CANmodule_t *CANmodule) /* clear interfaces */ for (i = 0; i < CANmodule->CANinterfaceCount; i++) { - CO_CANinterface_t *interface = &CANmodule->CANinterfaces[i]; + CO_CANinterface_t* interface = &CANmodule->CANinterfaces[i]; #if CO_DRIVER_ERROR_REPORTING > 0 CO_CANerror_disable(&interface->errorhandler); @@ -411,27 +375,19 @@ void CO_CANmodule_disable(CO_CANmodule_t *CANmodule) CANmodule->rxFilter = NULL; } - -CO_ReturnError_t CO_CANrxBufferInit( - CO_CANmodule_t *CANmodule, - uint16_t index, - uint16_t ident, - uint16_t mask, - bool_t rtr, - void *object, - void (*CANrx_callback)(void *object, void *message)) -{ +CO_ReturnError_t +CO_CANrxBufferInit(CO_CANmodule_t* CANmodule, uint16_t index, uint16_t ident, uint16_t mask, bool_t rtr, void* object, + void (*CANrx_callback)(void* object, void* message)) { CO_ReturnError_t ret = CO_ERROR_NO; - if((CANmodule!=NULL) && (index < CANmodule->rxSize)){ - CO_CANrx_t *buffer; + if ((CANmodule != NULL) && (index < CANmodule->rxSize)) { + CO_CANrx_t* buffer; /* buffer, which will be configured */ buffer = &CANmodule->rxArray[index]; #if CO_DRIVER_MULTI_INTERFACE > 0 - CO_CANsetIdentToIndex(CANmodule->rxIdentToIndex, index, ident, - buffer->ident); + CO_CANsetIdentToIndex(CANmodule->rxIdentToIndex, index, ident, buffer->ident); #endif /* Configure object variables */ @@ -443,7 +399,7 @@ CO_ReturnError_t CO_CANrxBufferInit( /* CAN identifier and CAN mask, bit aligned with CAN module */ buffer->ident = ident & CAN_SFF_MASK; - if(rtr){ + if (rtr) { buffer->ident |= CAN_RTR_FLAG; } buffer->mask = (mask & CAN_SFF_MASK) | CAN_EFF_FLAG | CAN_RTR_FLAG; @@ -451,11 +407,10 @@ CO_ReturnError_t CO_CANrxBufferInit( /* Set CAN hardware module filter and mask. */ CANmodule->rxFilter[index].can_id = buffer->ident; CANmodule->rxFilter[index].can_mask = buffer->mask; - if(CANmodule->CANnormal){ + if (CANmodule->CANnormal) { ret = setRxFilters(CANmodule); } - } - else { + } else { log_printf(LOG_DEBUG, DBG_CAN_RX_PARAM_FAILED, "illegal argument"); ret = CO_ERROR_ILLEGAL_ARGUMENT; } @@ -465,65 +420,54 @@ CO_ReturnError_t CO_CANrxBufferInit( #if CO_DRIVER_MULTI_INTERFACE > 0 -bool_t CO_CANrxBuffer_getInterface( - CO_CANmodule_t *CANmodule, - uint16_t ident, - int *can_ifindexRx, - struct timespec *timestamp) -{ - CO_CANrx_t *buffer; +bool_t +CO_CANrxBuffer_getInterface(CO_CANmodule_t* CANmodule, uint16_t ident, int* can_ifindexRx, struct timespec* timestamp) { + CO_CANrx_t* buffer; - if (CANmodule == NULL){ + if (CANmodule == NULL) { return false; } const uint32_t index = CO_CANgetIndexFromIdent(CANmodule->rxIdentToIndex, ident); if ((index == CO_INVALID_COB_ID) || (index > CANmodule->rxSize)) { - return false; + return false; } buffer = &CANmodule->rxArray[index]; /* return values */ if (can_ifindexRx != NULL) { - *can_ifindexRx = buffer->can_ifindex; + *can_ifindexRx = buffer->can_ifindex; } if (timestamp != NULL) { - *timestamp = buffer->timestamp; + *timestamp = buffer->timestamp; } if (buffer->can_ifindex != 0) { - return true; - } - else { - return false; + return true; + } else { + return false; } } #endif /* CO_DRIVER_MULTI_INTERFACE */ +CO_CANtx_t* +CO_CANtxBufferInit(CO_CANmodule_t* CANmodule, uint16_t index, uint16_t ident, bool_t rtr, uint8_t noOfBytes, + bool_t syncFlag) { + CO_CANtx_t* buffer = NULL; -CO_CANtx_t *CO_CANtxBufferInit( - CO_CANmodule_t *CANmodule, - uint16_t index, - uint16_t ident, - bool_t rtr, - uint8_t noOfBytes, - bool_t syncFlag) -{ - CO_CANtx_t *buffer = NULL; - - if((CANmodule != NULL) && (index < CANmodule->txSize)){ + if ((CANmodule != NULL) && (index < CANmodule->txSize)) { /* get specific buffer */ buffer = &CANmodule->txArray[index]; #if CO_DRIVER_MULTI_INTERFACE > 0 - CO_CANsetIdentToIndex(CANmodule->txIdentToIndex, index, ident, buffer->ident); + CO_CANsetIdentToIndex(CANmodule->txIdentToIndex, index, ident, buffer->ident); #endif buffer->can_ifindex = 0; /* CAN identifier and rtr */ buffer->ident = ident & CAN_SFF_MASK; - if(rtr){ + if (rtr) { buffer->ident |= CAN_RTR_FLAG; } buffer->DLC = noOfBytes; @@ -536,11 +480,8 @@ CO_CANtx_t *CO_CANtxBufferInit( #if CO_DRIVER_MULTI_INTERFACE > 0 -CO_ReturnError_t CO_CANtxBuffer_setInterface( - CO_CANmodule_t *CANmodule, - uint16_t ident, - int can_ifindexTx) -{ +CO_ReturnError_t +CO_CANtxBuffer_setInterface(CO_CANmodule_t* CANmodule, uint16_t ident, int can_ifindexTx) { if (CANmodule != NULL) { uint32_t index; @@ -559,18 +500,15 @@ CO_ReturnError_t CO_CANtxBuffer_setInterface( #if CO_DRIVER_MULTI_INTERFACE > 0 /* send CAN message */ -static CO_ReturnError_t CO_CANCheckSendInterface( - CO_CANmodule_t *CANmodule, - CO_CANtx_t *buffer, - CO_CANinterface_t *interface) -{ +static CO_ReturnError_t +CO_CANCheckSendInterface(CO_CANmodule_t* CANmodule, CO_CANtx_t* buffer, CO_CANinterface_t* interface) { CO_ReturnError_t err = CO_ERROR_NO; #if CO_DRIVER_ERROR_REPORTING > 0 CO_CANinterfaceState_t ifState; #endif ssize_t n; - if (CANmodule==NULL || interface==NULL || interface->fd < 0) { + if (CANmodule == NULL || interface == NULL || interface->fd < 0) { return CO_ERROR_ILLEGAL_ARGUMENT; } @@ -583,8 +521,7 @@ static CO_ReturnError_t CO_CANCheckSendInterface( case CO_INTERFACE_LISTEN_ONLY: /* silently drop message */ return CO_ERROR_NO; - default: - return CO_ERROR_INVALID_STATE; + default: return CO_ERROR_INVALID_STATE; } #endif @@ -594,25 +531,22 @@ static CO_ReturnError_t CO_CANCheckSendInterface( if (errno == EINTR) { /* try again */ continue; - } - else if (errno == EAGAIN) { + } else if (errno == EAGAIN) { /* socket queue full */ break; - } - else if (errno == ENOBUFS) { + } else if (errno == ENOBUFS) { /* socketCAN doesn't support blocking write. You can wait here for * a few hundred us and then try again */ #if CO_DRIVER_ERROR_REPORTING > 0 interface->errorhandler.CANerrorStatus |= CO_CAN_ERRTX_OVERFLOW; #endif return CO_ERROR_TX_BUSY; - } - else if (n != CAN_MTU) { + } else if (n != CAN_MTU) { break; } } while (errno != 0); - if(n != CAN_MTU){ + if (n != CAN_MTU) { #if CO_DRIVER_ERROR_REPORTING > 0 interface->errorhandler.CANerrorStatus |= CO_CAN_ERRTX_OVERFLOW; #endif @@ -624,7 +558,6 @@ static CO_ReturnError_t CO_CANCheckSendInterface( return err; } - /* * The same as #CO_CANsend(), but ensures that there is enough space remaining * in the driver for more important messages. @@ -642,11 +575,10 @@ static CO_ReturnError_t CO_CANCheckSendInterface( * @return #CO_ReturnError_t: CO_ERROR_NO, CO_ERROR_TX_OVERFLOW, CO_ERROR_TX_BUSY or * CO_ERROR_TX_PDO_WINDOW (Synchronous TPDO is outside window). */ -CO_ReturnError_t CO_CANCheckSend(CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer); - +CO_ReturnError_t CO_CANCheckSend(CO_CANmodule_t* CANmodule, CO_CANtx_t* buffer); -CO_ReturnError_t CO_CANsend(CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer) -{ +CO_ReturnError_t +CO_CANsend(CO_CANmodule_t* CANmodule, CO_CANtx_t* buffer) { CO_ReturnError_t err; err = CO_CANCheckSend(CANmodule, buffer); if (err == CO_ERROR_TX_BUSY) { @@ -658,18 +590,16 @@ CO_ReturnError_t CO_CANsend(CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer) return err; } - -CO_ReturnError_t CO_CANCheckSend(CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer) -{ +CO_ReturnError_t +CO_CANCheckSend(CO_CANmodule_t* CANmodule, CO_CANtx_t* buffer) { uint32_t i; CO_ReturnError_t err = CO_ERROR_NO; /* check on which interfaces to send this messages */ for (i = 0; i < CANmodule->CANinterfaceCount; i++) { - CO_CANinterface_t *interface = &CANmodule->CANinterfaces[i]; + CO_CANinterface_t* interface = &CANmodule->CANinterfaces[i]; - if ((buffer->can_ifindex == 0) || - buffer->can_ifindex == interface->can_ifindex) { + if ((buffer->can_ifindex == 0) || buffer->can_ifindex == interface->can_ifindex) { CO_ReturnError_t tmp; @@ -689,26 +619,25 @@ CO_ReturnError_t CO_CANCheckSend(CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer) #endif /* CO_DRIVER_MULTI_INTERFACE > 0 */ - #if CO_DRIVER_MULTI_INTERFACE == 0 /* Change handling of tx buffer full in CO_CANsend(). Use CO_CANtx_t->bufferFull * flag. Re-transmit undelivered message inside CO_CANmodule_process(). */ -CO_ReturnError_t CO_CANsend(CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer) -{ +CO_ReturnError_t +CO_CANsend(CO_CANmodule_t* CANmodule, CO_CANtx_t* buffer) { CO_ReturnError_t err = CO_ERROR_NO; - if (CANmodule==NULL || buffer==NULL || CANmodule->CANinterfaceCount==0) { + if (CANmodule == NULL || buffer == NULL || CANmodule->CANinterfaceCount == 0) { return CO_ERROR_ILLEGAL_ARGUMENT; } - CO_CANinterface_t *interface = &CANmodule->CANinterfaces[0]; + CO_CANinterface_t* interface = &CANmodule->CANinterfaces[0]; if (interface == NULL || interface->fd < 0) { return CO_ERROR_ILLEGAL_ARGUMENT; } /* Verify overflow */ - if(buffer->bufferFull){ + if (buffer->bufferFull) { #if CO_DRIVER_ERROR_REPORTING > 0 interface->errorhandler.CANerrorStatus |= CO_CAN_ERRTX_OVERFLOW; #endif @@ -724,16 +653,14 @@ CO_ReturnError_t CO_CANsend(CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer) buffer->bufferFull = false; CANmodule->CANtxCount--; } - } - else if (errno == EINTR || errno == EAGAIN || errno == ENOBUFS) { + } else if (errno == EINTR || errno == EAGAIN || errno == ENOBUFS) { /* Send failed, message will be re-sent by CO_CANmodule_process() */ if (!buffer->bufferFull) { buffer->bufferFull = true; CANmodule->CANtxCount++; } err = CO_ERROR_TX_BUSY; - } - else { + } else { /* Unknown error */ log_printf(LOG_DEBUG, DBG_ERRNO, "send()"); #if CO_DRIVER_ERROR_REPORTING > 0 @@ -747,27 +674,26 @@ CO_ReturnError_t CO_CANsend(CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer) #endif /* CO_DRIVER_MULTI_INTERFACE == 0 */ - -void CO_CANclearPendingSyncPDOs(CO_CANmodule_t *CANmodule) -{ +void +CO_CANclearPendingSyncPDOs(CO_CANmodule_t* CANmodule) { (void)CANmodule; /* Messages are either written to the socket queue or dropped */ } - -void CO_CANmodule_process(CO_CANmodule_t *CANmodule) -{ - if (CANmodule == NULL || CANmodule->CANinterfaceCount == 0) return; +void +CO_CANmodule_process(CO_CANmodule_t* CANmodule) { + if (CANmodule == NULL || CANmodule->CANinterfaceCount == 0) { + return; + } #if CO_DRIVER_ERROR_REPORTING > 0 - /* socketCAN doesn't support microcontroller-like error counters. If an - * error has occured, a special can message is created by the driver and - * received by the application like a regular message. - * Therefore, error counter evaluation is included in rx function. - * Here we just copy evaluated CANerrorStatus from the first CAN interface. */ - - CANmodule->CANerrorStatus = - CANmodule->CANinterfaces[0].errorhandler.CANerrorStatus; + /* socketCAN doesn't support microcontroller-like error counters. If an + * error has occured, a special can message is created by the driver and + * received by the application like a regular message. + * Therefore, error counter evaluation is included in rx function. + * Here we just copy evaluated CANerrorStatus from the first CAN interface. */ + + CANmodule->CANerrorStatus = CANmodule->CANinterfaces[0].errorhandler.CANerrorStatus; #endif #if CO_DRIVER_MULTI_INTERFACE == 0 @@ -776,7 +702,7 @@ void CO_CANmodule_process(CO_CANmodule_t *CANmodule) bool_t found = false; for (uint16_t i = 0; i < CANmodule->txSize; i++) { - CO_CANtx_t *buffer = &CANmodule->txArray[i]; + CO_CANtx_t* buffer = &CANmodule->txArray[i]; if (buffer->bufferFull) { buffer->bufferFull = false; @@ -794,22 +720,19 @@ void CO_CANmodule_process(CO_CANmodule_t *CANmodule) #endif /* CO_DRIVER_MULTI_INTERFACE == 0 */ } - /* Read CAN message from socket and verify some errors */ -static CO_ReturnError_t CO_CANread( - CO_CANmodule_t *CANmodule, - CO_CANinterface_t *interface, - struct can_frame *msg, /* CAN message, return value */ - struct timespec *timestamp) /* timestamp of CAN message, return value */ +static CO_ReturnError_t +CO_CANread(CO_CANmodule_t* CANmodule, CO_CANinterface_t* interface, + struct can_frame* msg, /* CAN message, return value */ + struct timespec* timestamp) /* timestamp of CAN message, return value */ { int32_t n; uint32_t dropped; - /* recvmsg - like read, but generates statistics about the socket - * example in berlios candump.c */ + /* recvmsg - like read, but generates statistics about the socket example in berlios candump.c */ struct iovec iov; struct msghdr msghdr; char ctrlmsg[CMSG_SPACE(sizeof(struct timeval)) + CMSG_SPACE(sizeof(dropped))]; - struct cmsghdr *cmsg; + struct cmsghdr* cmsg; iov.iov_base = msg; iov.iov_len = sizeof(*msg); @@ -833,89 +756,78 @@ static CO_ReturnError_t CO_CANread( } /* check for rx queue overflow, get rx time */ - for (cmsg = CMSG_FIRSTHDR(&msghdr); - cmsg && (cmsg->cmsg_level == SOL_SOCKET); - cmsg = CMSG_NXTHDR(&msghdr, cmsg)) { + for (cmsg = CMSG_FIRSTHDR(&msghdr); cmsg && (cmsg->cmsg_level == SOL_SOCKET); cmsg = CMSG_NXTHDR(&msghdr, cmsg)) { if (cmsg->cmsg_type == SO_TIMESTAMPING) { /* this is system time, not monotonic time! */ *timestamp = ((struct timespec*)CMSG_DATA(cmsg))[0]; - } - else if (cmsg->cmsg_type == SO_RXQ_OVFL) { + } else if (cmsg->cmsg_type == SO_RXQ_OVFL) { dropped = *(uint32_t*)CMSG_DATA(cmsg); if (dropped > CANmodule->rxDropCount) { #if CO_DRIVER_ERROR_REPORTING > 0 interface->errorhandler.CANerrorStatus |= CO_CAN_ERRRX_OVERFLOW; #endif - log_printf(LOG_ERR, CAN_RX_SOCKET_QUEUE_OVERFLOW, - interface->ifName, dropped); + log_printf(LOG_ERR, CAN_RX_SOCKET_QUEUE_OVERFLOW, interface->ifName, dropped); } CANmodule->rxDropCount = dropped; - //todo use this info! + // todo use this info! } } return CO_ERROR_NO; } - /* find msg inside rxArray and call corresponding CANrx_callback */ -static int32_t CO_CANrxMsg( /* return index of received message in rxArray or -1 */ - CO_CANmodule_t *CANmodule, - struct can_frame *msg, /* CAN message input */ - CO_CANrxMsg_t *buffer) /* If not NULL, msg will be copied to buffer */ +static int32_t +CO_CANrxMsg( /* return index of received message in rxArray or -1 */ + CO_CANmodule_t* CANmodule, struct can_frame* msg, /* CAN message input */ + CO_CANrxMsg_t* buffer) /* If not NULL, msg will be copied to buffer */ { int32_t retval; - const CO_CANrxMsg_t *rcvMsg; /* pointer to received message in CAN module */ + const CO_CANrxMsg_t* rcvMsg; /* pointer to received message in CAN module */ uint16_t index; /* index of received message */ - CO_CANrx_t *rcvMsgObj = NULL; /* receive message object from CO_CANmodule_t object. */ + CO_CANrx_t* rcvMsgObj = NULL; /* receive message object from CO_CANmodule_t object. */ bool_t msgMatched = false; - /* CANopenNode can message is binary compatible to the socketCAN one, - * including the extension flags */ + /* CANopenNode can message is binary compatible to the socketCAN one, including the extension flags */ // msg->can_id &= CAN_EFF_MASK; - rcvMsg = (CO_CANrxMsg_t *)msg; + rcvMsg = (CO_CANrxMsg_t*)msg; /* Message has been received. Search rxArray from CANmodule for the * same CAN-ID. */ rcvMsgObj = &CANmodule->rxArray[0]; - for (index = 0; index < CANmodule->rxSize; index ++) { - if(((rcvMsg->ident ^ rcvMsgObj->ident) & rcvMsgObj->mask) == 0U){ + for (index = 0; index < CANmodule->rxSize; index++) { + if (((rcvMsg->ident ^ rcvMsgObj->ident) & rcvMsgObj->mask) == 0U) { msgMatched = true; break; } rcvMsgObj++; } - if(msgMatched) { + if (msgMatched) { /* Call specific function, which will process the message */ - if ((rcvMsgObj != NULL) && (rcvMsgObj->CANrx_callback != NULL)){ - rcvMsgObj->CANrx_callback(rcvMsgObj->object, (void *)rcvMsg); + if ((rcvMsgObj != NULL) && (rcvMsgObj->CANrx_callback != NULL)) { + rcvMsgObj->CANrx_callback(rcvMsgObj->object, (void*)rcvMsg); } /* return message */ if (buffer != NULL) { memcpy(buffer, rcvMsg, sizeof(*buffer)); } retval = index; - } - else { + } else { retval = -1; } return retval; } - -bool_t CO_CANrxFromEpoll(CO_CANmodule_t *CANmodule, - struct epoll_event *ev, - CO_CANrxMsg_t *buffer, - int32_t *msgIndex) -{ +bool_t +CO_CANrxFromEpoll(CO_CANmodule_t* CANmodule, struct epoll_event* ev, CO_CANrxMsg_t* buffer, int32_t* msgIndex) { if (CANmodule == NULL || ev == NULL || CANmodule->CANinterfaceCount == 0) { return false; } /* Verify for epoll events in CAN socket */ - for (uint32_t i = 0; i < CANmodule->CANinterfaceCount; i ++) { - CO_CANinterface_t *interface = &CANmodule->CANinterfaces[i]; + for (uint32_t i = 0; i < CANmodule->CANinterfaceCount; i++) { + CO_CANinterface_t* interface = &CANmodule->CANinterfaces[i]; if (ev->data.fd == interface->fd) { if ((ev->events & (EPOLLERR | EPOLLHUP)) != 0) { @@ -923,26 +835,22 @@ bool_t CO_CANrxFromEpoll(CO_CANmodule_t *CANmodule, /* epoll detected close/error on socket. Try to pull event */ errno = 0; recv(ev->data.fd, &msg, sizeof(msg), MSG_DONTWAIT); - log_printf(LOG_DEBUG, DBG_CAN_RX_EPOLL, - ev->events, strerror(errno)); - } - else if ((ev->events & EPOLLIN) != 0) { + log_printf(LOG_DEBUG, DBG_CAN_RX_EPOLL, ev->events, strerror(errno)); + } else if ((ev->events & EPOLLIN) != 0) { struct can_frame msg; struct timespec timestamp; /* get message */ - CO_ReturnError_t err = CO_CANread(CANmodule, interface, - &msg, ×tamp); + CO_ReturnError_t err = CO_CANread(CANmodule, interface, &msg, ×tamp); - if(err == CO_ERROR_NO && CANmodule->CANnormal) { + if (err == CO_ERROR_NO && CANmodule->CANnormal) { if (msg.can_id & CAN_ERR_FLAG) { /* error msg */ #if CO_DRIVER_ERROR_REPORTING > 0 CO_CANerror_rxMsgError(&interface->errorhandler, &msg); #endif - } - else { + } else { /* data msg */ #if CO_DRIVER_ERROR_REPORTING > 0 /* clear listenOnly and noackCounter if necessary */ @@ -952,18 +860,15 @@ bool_t CO_CANrxFromEpoll(CO_CANmodule_t *CANmodule, if (idx > -1) { /* Store message info */ CANmodule->rxArray[idx].timestamp = timestamp; - CANmodule->rxArray[idx].can_ifindex = - interface->can_ifindex; + CANmodule->rxArray[idx].can_ifindex = interface->can_ifindex; } if (msgIndex != NULL) { *msgIndex = idx; } } } - } - else { - log_printf(LOG_DEBUG, DBG_EPOLL_UNKNOWN, - ev->events, ev->data.fd); + } else { + log_printf(LOG_DEBUG, DBG_EPOLL_UNKNOWN, ev->events, ev->data.fd); } return true; } /* if (ev->data.fd == interface->fd) */ diff --git a/CO_epoll_interface.c b/CO_epoll_interface.c index 7314e3a..995e100 100644 --- a/CO_epoll_interface.c +++ b/CO_epoll_interface.c @@ -50,17 +50,18 @@ #define CANSEND_DELAY_US 100 #endif - /* EPOLL **********************************************************************/ /* Helper function - get monotonic clock time in microseconds */ -static inline uint64_t clock_gettime_us(void) { +static inline uint64_t +clock_gettime_us(void) { struct timespec ts; (void)clock_gettime(CLOCK_MONOTONIC, &ts); return ts.tv_sec * 1000000 + ts.tv_nsec / 1000; } -CO_ReturnError_t CO_epoll_create(CO_epoll_t *ep, uint32_t timerInterval_us) { +CO_ReturnError_t +CO_epoll_create(CO_epoll_t* ep, uint32_t timerInterval_us) { int ret; struct epoll_event ev = {0}; @@ -119,7 +120,8 @@ CO_ReturnError_t CO_epoll_create(CO_epoll_t *ep, uint32_t timerInterval_us) { return CO_ERROR_NO; } -void CO_epoll_close(CO_epoll_t *ep) { +void +CO_epoll_close(CO_epoll_t* ep) { if (ep == NULL) { return; } @@ -134,8 +136,8 @@ void CO_epoll_close(CO_epoll_t *ep) { ep->timer_fd = -1; } - -void CO_epoll_wait(CO_epoll_t *ep) { +void +CO_epoll_wait(CO_epoll_t* ep) { if (ep == NULL) { return; } @@ -156,24 +158,17 @@ void CO_epoll_wait(CO_epoll_t *ep) { if (ready != 1 && errno == EINTR) { /* event from interrupt or signal, nothing to process, continue */ ep->epoll_new = false; - } - else if (ready != 1) { + } else if (ready != 1) { log_printf(LOG_DEBUG, DBG_ERRNO, "epoll_wait"); ep->epoll_new = false; - } - else if ((ep->ev.events & EPOLLIN) != 0 - && ep->ev.data.fd == ep->event_fd - ) { + } else if ((ep->ev.events & EPOLLIN) != 0 && ep->ev.data.fd == ep->event_fd) { uint64_t val; ssize_t s = read(ep->event_fd, &val, sizeof(uint64_t)); if (s != sizeof(uint64_t)) { log_printf(LOG_DEBUG, DBG_ERRNO, "read(event_fd)"); } ep->epoll_new = false; - } - else if ((ep->ev.events & EPOLLIN) != 0 - && ep->ev.data.fd == ep->timer_fd - ) { + } else if ((ep->ev.events & EPOLLIN) != 0 && ep->ev.data.fd == ep->timer_fd) { uint64_t val; ssize_t s = read(ep->timer_fd, &val, sizeof(uint64_t)); if (s != sizeof(uint64_t) && errno != EAGAIN) { @@ -184,14 +179,14 @@ void CO_epoll_wait(CO_epoll_t *ep) { } } -void CO_epoll_processLast(CO_epoll_t *ep) { +void +CO_epoll_processLast(CO_epoll_t* ep) { if (ep == NULL) { return; } if (ep->epoll_new) { - log_printf(LOG_DEBUG, DBG_EPOLL_UNKNOWN, - ep->ev.events, ep->ev.data.fd); + log_printf(LOG_DEBUG, DBG_EPOLL_UNKNOWN, ep->ev.events, ep->ev.data.fd); ep->epoll_new = false; } @@ -201,11 +196,9 @@ void CO_epoll_processLast(CO_epoll_t *ep) { ep->timerNext_us += 1; if (ep->timerInterval_us < 1000000) { ep->tm.it_value.tv_nsec = ep->timerNext_us * 1000; - } - else { + } else { ep->tm.it_value.tv_sec = ep->timerNext_us / 1000000; - ep->tm.it_value.tv_nsec = - (ep->timerNext_us % 1000000) * 1000; + ep->tm.it_value.tv_nsec = (ep->timerNext_us % 1000000) * 1000; } int ret = timerfd_settime(ep->timer_fd, 0, &ep->tm, NULL); if (ret < 0) { @@ -214,12 +207,12 @@ void CO_epoll_processLast(CO_epoll_t *ep) { } } - /* MAINLINE *******************************************************************/ #ifndef CO_SINGLE_THREAD /* Send event to wake CO_epoll_processMain() */ -static void wakeupCallback(void *object) { - CO_epoll_t *ep = (CO_epoll_t *)object; +static void +wakeupCallback(void* object) { + CO_epoll_t* ep = (CO_epoll_t*)object; uint64_t u = 1; ssize_t s; s = write(ep->event_fd, &u, sizeof(uint64_t)); @@ -229,7 +222,8 @@ static void wakeupCallback(void *object) { } #endif -void CO_epoll_initCANopenMain(CO_epoll_t *ep, CO_t *co) { +void +CO_epoll_initCANopenMain(CO_epoll_t* ep, CO_t* co) { if (ep == NULL || co == NULL) { return; } @@ -237,66 +231,52 @@ void CO_epoll_initCANopenMain(CO_epoll_t *ep, CO_t *co) { #ifndef CO_SINGLE_THREAD /* Configure LSS slave callback function */ - #if (CO_CONFIG_LSS) & CO_CONFIG_FLAG_CALLBACK_PRE - #if (CO_CONFIG_LSS) & CO_CONFIG_LSS_SLAVE - CO_LSSslave_initCallbackPre(co->LSSslave, - (void *)ep, wakeupCallback); - #endif - #endif +#if (CO_CONFIG_LSS) & CO_CONFIG_FLAG_CALLBACK_PRE +#if (CO_CONFIG_LSS) & CO_CONFIG_LSS_SLAVE + CO_LSSslave_initCallbackPre(co->LSSslave, (void*)ep, wakeupCallback); +#endif +#endif if (co->nodeIdUnconfigured) { return; } /* Configure callback functions */ - #if (CO_CONFIG_NMT) & CO_CONFIG_FLAG_CALLBACK_PRE - CO_NMT_initCallbackPre(co->NMT, - (void *)ep, wakeupCallback); - #endif - #if (CO_CONFIG_HB_CONS) & CO_CONFIG_FLAG_CALLBACK_PRE - CO_HBconsumer_initCallbackPre(co->HBcons, - (void *)ep, wakeupCallback); - #endif - #if (CO_CONFIG_EM) & CO_CONFIG_FLAG_CALLBACK_PRE - CO_EM_initCallbackPre(co->em, - (void *)ep, wakeupCallback); - #endif - #if (CO_CONFIG_SDO_SRV) & CO_CONFIG_FLAG_CALLBACK_PRE - CO_SDOserver_initCallbackPre(&co->SDOserver[0], - (void *)ep, wakeupCallback); - #endif - #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_CALLBACK_PRE - CO_SDOclient_initCallbackPre(&co->SDOclient[0], - (void *)ep, wakeupCallback); - #endif - #if (CO_CONFIG_TIME) & CO_CONFIG_FLAG_CALLBACK_PRE - CO_TIME_initCallbackPre(co->TIME, - (void *)ep, wakeupCallback); - #endif - #if (CO_CONFIG_LSS) & CO_CONFIG_FLAG_CALLBACK_PRE - #if (CO_CONFIG_LSS) & CO_CONFIG_LSS_MASTER - CO_LSSmaster_initCallbackPre(co->LSSmaster, - (void *)ep, wakeupCallback); - #endif - #endif +#if (CO_CONFIG_NMT) & CO_CONFIG_FLAG_CALLBACK_PRE + CO_NMT_initCallbackPre(co->NMT, (void*)ep, wakeupCallback); +#endif +#if (CO_CONFIG_HB_CONS) & CO_CONFIG_FLAG_CALLBACK_PRE + CO_HBconsumer_initCallbackPre(co->HBcons, (void*)ep, wakeupCallback); +#endif +#if (CO_CONFIG_EM) & CO_CONFIG_FLAG_CALLBACK_PRE + CO_EM_initCallbackPre(co->em, (void*)ep, wakeupCallback); +#endif +#if (CO_CONFIG_SDO_SRV) & CO_CONFIG_FLAG_CALLBACK_PRE + CO_SDOserver_initCallbackPre(&co->SDOserver[0], (void*)ep, wakeupCallback); +#endif +#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_CALLBACK_PRE + CO_SDOclient_initCallbackPre(&co->SDOclient[0], (void*)ep, wakeupCallback); +#endif +#if (CO_CONFIG_TIME) & CO_CONFIG_FLAG_CALLBACK_PRE + CO_TIME_initCallbackPre(co->TIME, (void*)ep, wakeupCallback); +#endif +#if (CO_CONFIG_LSS) & CO_CONFIG_FLAG_CALLBACK_PRE +#if (CO_CONFIG_LSS) & CO_CONFIG_LSS_MASTER + CO_LSSmaster_initCallbackPre(co->LSSmaster, (void*)ep, wakeupCallback); +#endif +#endif #endif /* CO_SINGLE_THREAD */ } -void CO_epoll_processMain(CO_epoll_t *ep, - CO_t *co, - bool_t enableGateway, - CO_NMT_reset_cmd_t *reset) -{ +void +CO_epoll_processMain(CO_epoll_t* ep, CO_t* co, bool_t enableGateway, CO_NMT_reset_cmd_t* reset) { if (ep == NULL || co == NULL || reset == NULL) { return; } /* process CANopen objects */ - *reset = CO_process(co, - enableGateway, - ep->timeDifference_us, - &ep->timerNext_us); + *reset = CO_process(co, enableGateway, ep->timeDifference_us, &ep->timerNext_us); /* If there are unsent CAN messages, call CO_CANmodule_process() earlier */ if (co->CANmodule->CANtxCount > 0 && ep->timerNext_us > CANSEND_DELAY_US) { @@ -304,12 +284,9 @@ void CO_epoll_processMain(CO_epoll_t *ep, } } - /* CANrx and REALTIME *********************************************************/ -void CO_epoll_processRT(CO_epoll_t *ep, - CO_t *co, - bool_t realtime) -{ +void +CO_epoll_processRT(CO_epoll_t* ep, CO_t* co, bool_t realtime) { if (co == NULL || ep == NULL) { return; } @@ -322,25 +299,23 @@ void CO_epoll_processRT(CO_epoll_t *ep, } if (!realtime || ep->timerEvent) { - uint32_t *pTimerNext_us = realtime ? NULL : &ep->timerNext_us; + uint32_t* pTimerNext_us = realtime ? NULL : &ep->timerNext_us; CO_LOCK_OD(co->CANmodule); if (!co->nodeIdUnconfigured && co->CANmodule->CANnormal) { bool_t syncWas = false; #if (CO_CONFIG_SYNC) & CO_CONFIG_SYNC_ENABLE - syncWas = CO_process_SYNC(co, ep->timeDifference_us, - pTimerNext_us); + syncWas = CO_process_SYNC(co, ep->timeDifference_us, pTimerNext_us); #endif #if (CO_CONFIG_PDO) & CO_CONFIG_RPDO_ENABLE - CO_process_RPDO(co, syncWas, ep->timeDifference_us, - pTimerNext_us); + CO_process_RPDO(co, syncWas, ep->timeDifference_us, pTimerNext_us); #endif #if (CO_CONFIG_PDO) & CO_CONFIG_TPDO_ENABLE - CO_process_TPDO(co, syncWas, ep->timeDifference_us, - pTimerNext_us); + CO_process_TPDO(co, syncWas, ep->timeDifference_us, pTimerNext_us); #endif - (void) syncWas; (void) pTimerNext_us; + (void)syncWas; + (void)pTimerNext_us; } CO_UNLOCK_OD(co->CANmodule); } @@ -349,33 +324,29 @@ void CO_epoll_processRT(CO_epoll_t *ep, /* GATEWAY ********************************************************************/ #if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII /* write response string from gateway-ascii object */ -static size_t gtwa_write_response(void *object, - const char *buf, - size_t count, - uint8_t *connectionOK) -{ - int* fd = (int *)object; +static size_t +gtwa_write_response(void* object, const char* buf, size_t count, uint8_t* connectionOK) { + int* fd = (int*)object; /* nWritten = count -> in case of error (non-existing fd) data are purged */ size_t nWritten = count; if (fd != NULL && *fd >= 0) { - ssize_t n = write(*fd, (const void *)buf, count); + ssize_t n = write(*fd, (const void*)buf, count); if (n >= 0) { nWritten = (size_t)n; - } - else { + } else { /* probably EAGAIN - "Resource temporarily unavailable". Retry. */ log_printf(LOG_DEBUG, DBG_ERRNO, "write(gtwa_response)"); nWritten = 0; } - } - else { + } else { *connectionOK = 0; } return nWritten; } -static inline void socetAcceptEnableForEpoll(CO_epoll_gtw_t *epGtw) { +static inline void +socetAcceptEnableForEpoll(CO_epoll_gtw_t* epGtw) { struct epoll_event ev = {0}; int ret; @@ -387,12 +358,9 @@ static inline void socetAcceptEnableForEpoll(CO_epoll_gtw_t *epGtw) { } } -CO_ReturnError_t CO_epoll_createGtw(CO_epoll_gtw_t *epGtw, - int epoll_fd, - int32_t commandInterface, - uint32_t socketTimeout_ms, - char *localSocketPath) -{ +CO_ReturnError_t +CO_epoll_createGtw(CO_epoll_gtw_t* epGtw, int epoll_fd, int32_t commandInterface, uint32_t socketTimeout_ms, + char* localSocketPath) { int ret; struct epoll_event ev = {0}; @@ -400,26 +368,24 @@ CO_ReturnError_t CO_epoll_createGtw(CO_epoll_gtw_t *epGtw, return CO_ERROR_ILLEGAL_ARGUMENT; } - epGtw->epoll_fd = epoll_fd; epGtw->commandInterface = commandInterface; - epGtw->socketTimeout_us = (socketTimeout_ms < (UINT_MAX / 1000 - 1000000)) ? - socketTimeout_ms * 1000 : (UINT_MAX - 1000000); + epGtw->socketTimeout_us = (socketTimeout_ms < (UINT_MAX / 1000 - 1000000)) ? socketTimeout_ms * 1000 + : (UINT_MAX - 1000000); epGtw->gtwa_fdSocket = -1; epGtw->gtwa_fd = -1; if (commandInterface == CO_COMMAND_IF_STDIO) { epGtw->gtwa_fd = STDIN_FILENO; log_printf(LOG_INFO, DBG_COMMAND_STDIO_INFO); - } - else if (commandInterface == CO_COMMAND_IF_LOCAL_SOCKET) { + } else if (commandInterface == CO_COMMAND_IF_LOCAL_SOCKET) { struct sockaddr_un addr; epGtw->localSocketPath = localSocketPath; /* Create, bind and listen local socket */ epGtw->gtwa_fdSocket = socket(AF_UNIX, SOCK_STREAM | SOCK_NONBLOCK, 0); - if(epGtw->gtwa_fdSocket < 0) { + if (epGtw->gtwa_fdSocket < 0) { log_printf(LOG_CRIT, DBG_ERRNO, "socket(local)"); return CO_ERROR_SYSCALL; } @@ -427,15 +393,14 @@ CO_ReturnError_t CO_epoll_createGtw(CO_epoll_gtw_t *epGtw, memset(&addr, 0, sizeof(struct sockaddr_un)); addr.sun_family = AF_UNIX; strncpy(addr.sun_path, localSocketPath, sizeof(addr.sun_path) - 1); - ret = bind(epGtw->gtwa_fdSocket, (struct sockaddr *) &addr, - sizeof(struct sockaddr_un)); - if(ret < 0) { + ret = bind(epGtw->gtwa_fdSocket, (struct sockaddr*)&addr, sizeof(struct sockaddr_un)); + if (ret < 0) { log_printf(LOG_CRIT, DBG_COMMAND_LOCAL_BIND, localSocketPath); return CO_ERROR_SYSCALL; } ret = listen(epGtw->gtwa_fdSocket, LISTEN_BACKLOG); - if(ret < 0) { + if (ret < 0) { log_printf(LOG_CRIT, DBG_ERRNO, "listen(local)"); return CO_ERROR_SYSCALL; } @@ -449,37 +414,32 @@ CO_ReturnError_t CO_epoll_createGtw(CO_epoll_gtw_t *epGtw, } log_printf(LOG_INFO, DBG_COMMAND_LOCAL_INFO, localSocketPath); - } - else if (commandInterface >= CO_COMMAND_IF_TCP_SOCKET_MIN && - commandInterface <= CO_COMMAND_IF_TCP_SOCKET_MAX - ) { + } else if (commandInterface >= CO_COMMAND_IF_TCP_SOCKET_MIN && commandInterface <= CO_COMMAND_IF_TCP_SOCKET_MAX) { struct sockaddr_in addr; const int yes = 1; /* Create, bind and listen socket */ epGtw->gtwa_fdSocket = socket(AF_INET, SOCK_STREAM | SOCK_NONBLOCK, 0); - if(epGtw->gtwa_fdSocket < 0) { + if (epGtw->gtwa_fdSocket < 0) { log_printf(LOG_CRIT, DBG_ERRNO, "socket(tcp)"); return CO_ERROR_SYSCALL; } - setsockopt(epGtw->gtwa_fdSocket, SOL_SOCKET, SO_REUSEADDR, - &yes, sizeof(int)); + setsockopt(epGtw->gtwa_fdSocket, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(int)); memset(&addr, 0, sizeof(struct sockaddr_in)); addr.sin_family = AF_INET; addr.sin_port = htons(commandInterface); addr.sin_addr.s_addr = INADDR_ANY; - ret = bind(epGtw->gtwa_fdSocket, (struct sockaddr *) &addr, - sizeof(struct sockaddr_in)); - if(ret < 0) { + ret = bind(epGtw->gtwa_fdSocket, (struct sockaddr*)&addr, sizeof(struct sockaddr_in)); + if (ret < 0) { log_printf(LOG_CRIT, DBG_COMMAND_TCP_BIND, commandInterface); return CO_ERROR_SYSCALL; } ret = listen(epGtw->gtwa_fdSocket, LISTEN_BACKLOG); - if(ret < 0) { + if (ret < 0) { log_printf(LOG_CRIT, DBG_ERRNO, "listen(tcp)"); return CO_ERROR_SYSCALL; } @@ -493,8 +453,7 @@ CO_ReturnError_t CO_epoll_createGtw(CO_epoll_gtw_t *epGtw, } log_printf(LOG_INFO, DBG_COMMAND_TCP_INFO, commandInterface); - } - else { + } else { epGtw->commandInterface = CO_COMMAND_IF_DISABLED; } @@ -522,7 +481,8 @@ CO_ReturnError_t CO_epoll_createGtw(CO_epoll_gtw_t *epGtw, return CO_ERROR_NO; } -void CO_epoll_closeGtw(CO_epoll_gtw_t *epGtw) { +void +CO_epoll_closeGtw(CO_epoll_gtw_t* epGtw) { if (epGtw == NULL) { return; } @@ -533,11 +493,10 @@ void CO_epoll_closeGtw(CO_epoll_gtw_t *epGtw) { } close(epGtw->gtwa_fdSocket); /* Remove local socket file from filesystem. */ - if(remove(epGtw->localSocketPath) < 0) { + if (remove(epGtw->localSocketPath) < 0) { log_printf(LOG_CRIT, DBG_ERRNO, "remove(local)"); } - } - else if (epGtw->commandInterface >= CO_COMMAND_IF_TCP_SOCKET_MIN) { + } else if (epGtw->commandInterface >= CO_COMMAND_IF_TCP_SOCKET_MIN) { if (epGtw->gtwa_fd > 0) { close(epGtw->gtwa_fd); } @@ -547,48 +506,39 @@ void CO_epoll_closeGtw(CO_epoll_gtw_t *epGtw) { epGtw->gtwa_fdSocket = -1; } -void CO_epoll_initCANopenGtw(CO_epoll_gtw_t *epGtw, CO_t *co) { +void +CO_epoll_initCANopenGtw(CO_epoll_gtw_t* epGtw, CO_t* co) { if (epGtw == NULL || co == NULL || co->nodeIdUnconfigured) { return; } - CO_GTWA_initRead(co->gtwa, gtwa_write_response, (void *)&epGtw->gtwa_fd); + CO_GTWA_initRead(co->gtwa, gtwa_write_response, (void*)&epGtw->gtwa_fd); epGtw->freshCommand = true; } -void CO_epoll_processGtw(CO_epoll_gtw_t *epGtw, - CO_t *co, - CO_epoll_t *ep) -{ +void +CO_epoll_processGtw(CO_epoll_gtw_t* epGtw, CO_t* co, CO_epoll_t* ep) { if (epGtw == NULL || co == NULL || ep == NULL) { return; } /* Verify for epoll events */ - if (ep->epoll_new - && (ep->ev.data.fd == epGtw->gtwa_fdSocket - || ep->ev.data.fd == epGtw->gtwa_fd) - ) { - if ((ep->ev.events & EPOLLIN) != 0 - && ep->ev.data.fd == epGtw->gtwa_fdSocket - ) { + if (ep->epoll_new && (ep->ev.data.fd == epGtw->gtwa_fdSocket || ep->ev.data.fd == epGtw->gtwa_fd)) { + if ((ep->ev.events & EPOLLIN) != 0 && ep->ev.data.fd == epGtw->gtwa_fdSocket) { bool_t fail = false; - epGtw->gtwa_fd = accept4(epGtw->gtwa_fdSocket, - NULL, NULL, SOCK_NONBLOCK); + epGtw->gtwa_fd = accept4(epGtw->gtwa_fdSocket, NULL, NULL, SOCK_NONBLOCK); if (epGtw->gtwa_fd < 0) { fail = true; if (errno != EAGAIN && errno != EWOULDBLOCK) { log_printf(LOG_CRIT, DBG_ERRNO, "accept(gtwa_fdSocket)"); } - } - else { + } else { /* add fd to epoll */ struct epoll_event ev2 = {0}; ev2.events = EPOLLIN; ev2.data.fd = epGtw->gtwa_fd; - int ret = epoll_ctl(ep->epoll_fd, - EPOLL_CTL_ADD, ev2.data.fd, &ev2); + int ret = epoll_ctl(ep->epoll_fd, EPOLL_CTL_ADD, ev2.data.fd, &ev2); if (ret < 0) { fail = true; log_printf(LOG_CRIT, DBG_ERRNO, "epoll_ctl(add, gtwa_fd)"); @@ -600,56 +550,43 @@ void CO_epoll_processGtw(CO_epoll_gtw_t *epGtw, socetAcceptEnableForEpoll(epGtw); } ep->epoll_new = false; - } - else if ((ep->ev.events & EPOLLIN) != 0 - && ep->ev.data.fd == epGtw->gtwa_fd - ) { + } else if ((ep->ev.events & EPOLLIN) != 0 && ep->ev.data.fd == epGtw->gtwa_fd) { char buf[CO_CONFIG_GTWA_COMM_BUF_SIZE]; - size_t space = co->nodeIdUnconfigured ? - CO_CONFIG_GTWA_COMM_BUF_SIZE : - CO_GTWA_write_getSpace(co->gtwa); + size_t space = co->nodeIdUnconfigured ? CO_CONFIG_GTWA_COMM_BUF_SIZE : CO_GTWA_write_getSpace(co->gtwa); ssize_t s = read(epGtw->gtwa_fd, buf, space); if (space == 0 || co->nodeIdUnconfigured) { /* continue or purge data */ - } - else if (s < 0 && errno != EAGAIN) { + } else if (s < 0 && errno != EAGAIN) { log_printf(LOG_DEBUG, DBG_ERRNO, "read(gtwa_fd)"); - } - else if (s >= 0) { + } else if (s >= 0) { if (epGtw->commandInterface == CO_COMMAND_IF_STDIO) { /* simplify command interface on stdio, make hard to type - * sequence optional, prepend "[0] " to string, if missing */ + * sequence optional, prepend "[0] " to string, if missing */ const char sequence[] = "[0] "; - bool_t closed = (buf[s-1] == '\n'); /* is command closed? */ + bool_t closed = (buf[s - 1] == '\n'); /* is command closed? */ - if (buf[0] != '[' && (space - s) >= strlen(sequence) - && isgraph(buf[0]) && buf[0] != '#' - && closed && epGtw->freshCommand - ) { + if (buf[0] != '[' && (space - s) >= strlen(sequence) && isgraph(buf[0]) && buf[0] != '#' && closed + && epGtw->freshCommand) { CO_GTWA_write(co->gtwa, sequence, strlen(sequence)); } epGtw->freshCommand = closed; CO_GTWA_write(co->gtwa, buf, s); - } - else { /* socket, local or tcp */ + } else { /* socket, local or tcp */ if (s == 0) { /* EOF received, close connection and enable socket * accepting */ - int ret = epoll_ctl(ep->epoll_fd, EPOLL_CTL_DEL, - epGtw->gtwa_fd, NULL); + int ret = epoll_ctl(ep->epoll_fd, EPOLL_CTL_DEL, epGtw->gtwa_fd, NULL); if (ret < 0) { - log_printf(LOG_CRIT, DBG_ERRNO, - "epoll_ctl(del, gtwa_fd)"); + log_printf(LOG_CRIT, DBG_ERRNO, "epoll_ctl(del, gtwa_fd)"); } if (close(epGtw->gtwa_fd) < 0) { log_printf(LOG_CRIT, DBG_ERRNO, "close(gtwa_fd)"); } epGtw->gtwa_fd = -1; socetAcceptEnableForEpoll(epGtw); - } - else { + } else { CO_GTWA_write(co->gtwa, buf, s); } } @@ -657,10 +594,8 @@ void CO_epoll_processGtw(CO_epoll_gtw_t *epGtw, epGtw->socketTimeoutTmr_us = 0; ep->epoll_new = false; - } - else if ((ep->ev.events & (EPOLLERR | EPOLLHUP)) != 0) { - log_printf(LOG_DEBUG, DBG_GENERAL, - "socket error or hangup, event=", ep->ev.events); + } else if ((ep->ev.events & (EPOLLERR | EPOLLHUP)) != 0) { + log_printf(LOG_DEBUG, DBG_GENERAL, "socket error or hangup, event=", ep->ev.events); if (close(epGtw->gtwa_fd) < 0) { log_printf(LOG_CRIT, DBG_ERRNO, "close(gtwa_fd, hangup)"); } @@ -668,24 +603,19 @@ void CO_epoll_processGtw(CO_epoll_gtw_t *epGtw, } /* if (ep->epoll_new) */ /* if socket connection is established, verify timeout */ - if (epGtw->socketTimeout_us > 0 - && epGtw->gtwa_fdSocket > 0 && epGtw->gtwa_fd > 0) - { + if (epGtw->socketTimeout_us > 0 && epGtw->gtwa_fdSocket > 0 && epGtw->gtwa_fd > 0) { if (epGtw->socketTimeoutTmr_us > epGtw->socketTimeout_us) { /* timout expired, close current connection and accept next */ - int ret = epoll_ctl(ep->epoll_fd, - EPOLL_CTL_DEL, epGtw->gtwa_fd, NULL); + int ret = epoll_ctl(ep->epoll_fd, EPOLL_CTL_DEL, epGtw->gtwa_fd, NULL); if (ret < 0) { - log_printf(LOG_CRIT, DBG_ERRNO, - "epoll_ctl(del, gtwa_fd), tmo"); + log_printf(LOG_CRIT, DBG_ERRNO, "epoll_ctl(del, gtwa_fd), tmo"); } if (close(epGtw->gtwa_fd) < 0) { log_printf(LOG_CRIT, DBG_ERRNO, "close(gtwa_fd), tmo"); } epGtw->gtwa_fd = -1; socetAcceptEnableForEpoll(epGtw); - } - else { + } else { epGtw->socketTimeoutTmr_us += ep->timeDifference_us; } } diff --git a/CO_error.c b/CO_error.c index cc0bc5f..68865c8 100644 --- a/CO_error.c +++ b/CO_error.c @@ -29,14 +29,11 @@ #include "CO_error.h" #include "301/CO_driver.h" - /* * Reset CAN interface and set to listen only mode */ -static CO_CANinterfaceState_t CO_CANerrorSetListenOnly( - CO_CANinterfaceErrorhandler_t *CANerrorhandler, - bool_t resetIf) -{ +static CO_CANinterfaceState_t +CO_CANerrorSetListenOnly(CO_CANinterfaceErrorhandler_t* CANerrorhandler, bool_t resetIf) { log_printf(LOG_DEBUG, DBG_CAN_SET_LISTEN_ONLY, CANerrorhandler->ifName); clock_gettime(CLOCK_MONOTONIC, &CANerrorhandler->timestamp); @@ -45,28 +42,25 @@ static CO_CANinterfaceState_t CO_CANerrorSetListenOnly( if (resetIf) { int ret; char command[100]; - snprintf(command, sizeof(command), "ip link set %s down && " - "ip link set %s up " - "&", - CANerrorhandler->ifName, - CANerrorhandler->ifName); + snprintf(command, sizeof(command), + "ip link set %s down && " + "ip link set %s up " + "&", + CANerrorhandler->ifName, CANerrorhandler->ifName); ret = system(command); - if(ret < 0){ + if (ret < 0) { log_printf(LOG_DEBUG, DBG_ERRNO, "system()"); } - } return CO_INTERFACE_LISTEN_ONLY; } - /* * Clear listen only */ -static void CO_CANerrorClearListenOnly( - CO_CANinterfaceErrorhandler_t *CANerrorhandler) -{ +static void +CO_CANerrorClearListenOnly(CO_CANinterfaceErrorhandler_t* CANerrorhandler) { log_printf(LOG_DEBUG, DBG_CAN_CLR_LISTEN_ONLY, CANerrorhandler->ifName); CANerrorhandler->listenOnly = false; @@ -74,14 +68,11 @@ static void CO_CANerrorClearListenOnly( CANerrorhandler->timestamp.tv_nsec = 0; } - /* * Check and handle "bus off" state */ -static CO_CANinterfaceState_t CO_CANerrorBusoff( - CO_CANinterfaceErrorhandler_t *CANerrorhandler, - const struct can_frame *msg) -{ +static CO_CANinterfaceState_t +CO_CANerrorBusoff(CO_CANinterfaceErrorhandler_t* CANerrorhandler, const struct can_frame* msg) { CO_CANinterfaceState_t result = CO_INTERFACE_ACTIVE; if ((msg->can_id & CAN_ERR_BUSOFF) != 0) { @@ -98,14 +89,11 @@ static CO_CANinterfaceState_t CO_CANerrorBusoff( return result; } - /* * Check and handle controller problems */ -static CO_CANinterfaceState_t CO_CANerrorCrtl( - CO_CANinterfaceErrorhandler_t *CANerrorhandler, - const struct can_frame *msg) -{ +static CO_CANinterfaceState_t +CO_CANerrorCrtl(CO_CANinterfaceErrorhandler_t* CANerrorhandler, const struct can_frame* msg) { CO_CANinterfaceState_t result = CO_INTERFACE_ACTIVE; /* Control @@ -124,27 +112,22 @@ static CO_CANinterfaceState_t CO_CANerrorCrtl( log_printf(LOG_NOTICE, CAN_RX_PASSIVE, CANerrorhandler->ifName); CANerrorhandler->CANerrorStatus |= CO_CAN_ERRRX_PASSIVE; /* CANerrorhandler->CANerrorStatus |= CO_CAN_ERRRX_WARNING; */ - } - else if ((msg->data[1] & CAN_ERR_CRTL_TX_PASSIVE) != 0) { + } else if ((msg->data[1] & CAN_ERR_CRTL_TX_PASSIVE) != 0) { log_printf(LOG_NOTICE, CAN_TX_PASSIVE, CANerrorhandler->ifName); CANerrorhandler->CANerrorStatus |= CO_CAN_ERRTX_PASSIVE; /* CANerrorhandler->CANerrorStatus |= CO_CAN_ERRTX_WARNING; */ - } - else if ((msg->data[1] & CAN_ERR_CRTL_RX_OVERFLOW) != 0) { + } else if ((msg->data[1] & CAN_ERR_CRTL_RX_OVERFLOW) != 0) { log_printf(LOG_NOTICE, CAN_RX_BUF_OVERFLOW, CANerrorhandler->ifName); CANerrorhandler->CANerrorStatus |= CO_CAN_ERRRX_OVERFLOW; - } - else if ((msg->data[1] & CAN_ERR_CRTL_TX_OVERFLOW) != 0) { + } else if ((msg->data[1] & CAN_ERR_CRTL_TX_OVERFLOW) != 0) { log_printf(LOG_NOTICE, CAN_TX_BUF_OVERFLOW, CANerrorhandler->ifName); CANerrorhandler->CANerrorStatus |= CO_CAN_ERRTX_OVERFLOW; - } - else if ((msg->data[1] & CAN_ERR_CRTL_RX_WARNING) != 0) { + } else if ((msg->data[1] & CAN_ERR_CRTL_RX_WARNING) != 0) { log_printf(LOG_INFO, CAN_RX_LEVEL_WARNING, CANerrorhandler->ifName); /* clear passive flag, set warning */ CANerrorhandler->CANerrorStatus &= 0x7FFF ^ CO_CAN_ERRRX_PASSIVE; /* CANerrorhandler->CANerrorStatus |= CO_CAN_ERRRX_WARNING; */ - } - else if ((msg->data[1] & CAN_ERR_CRTL_TX_WARNING) != 0) { + } else if ((msg->data[1] & CAN_ERR_CRTL_TX_WARNING) != 0) { log_printf(LOG_INFO, CAN_TX_LEVEL_WARNING, CANerrorhandler->ifName); /* clear passive flag, set warning */ CANerrorhandler->CANerrorStatus &= 0x7FFF ^ CO_CAN_ERRTX_PASSIVE; @@ -159,14 +142,11 @@ static CO_CANinterfaceState_t CO_CANerrorCrtl( return result; } - /* * Check and handle controller problems */ -static CO_CANinterfaceState_t CO_CANerrorNoack( - CO_CANinterfaceErrorhandler_t *CANerrorhandler, - const struct can_frame *msg) -{ +static CO_CANinterfaceState_t +CO_CANerrorNoack(CO_CANinterfaceErrorhandler_t* CANerrorhandler, const struct can_frame* msg) { CO_CANinterfaceState_t result = CO_INTERFACE_ACTIVE; if (CANerrorhandler->listenOnly) { @@ -175,7 +155,7 @@ static CO_CANinterfaceState_t CO_CANerrorNoack( /* received no ACK on transmission */ if ((msg->can_id & CAN_ERR_ACK) != 0) { - CANerrorhandler->noackCounter ++; + CANerrorhandler->noackCounter++; if (CANerrorhandler->noackCounter > CO_CANerror_NOACK_MAX) { log_printf(LOG_INFO, CAN_NOACK, CANerrorhandler->ifName); @@ -186,19 +166,14 @@ static CO_CANinterfaceState_t CO_CANerrorNoack( * in here or deleting it within Linux Kernel can driver (set "false"). */ result = CO_CANerrorSetListenOnly(CANerrorhandler, true); } - } - else { + } else { CANerrorhandler->noackCounter = 0; } return result; } - -void CO_CANerror_init( - CO_CANinterfaceErrorhandler_t *CANerrorhandler, - int fd, - const char *ifName) -{ +void +CO_CANerror_init(CO_CANinterfaceErrorhandler_t* CANerrorhandler, int fd, const char* ifName) { if (CANerrorhandler == NULL) { return; } @@ -212,10 +187,8 @@ void CO_CANerror_init( CANerrorhandler->CANerrorStatus = 0; } - -void CO_CANerror_disable( - CO_CANinterfaceErrorhandler_t *CANerrorhandler) -{ +void +CO_CANerror_disable(CO_CANinterfaceErrorhandler_t* CANerrorhandler) { if (CANerrorhandler == NULL) { return; } @@ -224,10 +197,8 @@ void CO_CANerror_disable( CANerrorhandler->fd = -1; } - -void CO_CANerror_rxMsg( - CO_CANinterfaceErrorhandler_t *CANerrorhandler) -{ +void +CO_CANerror_rxMsg(CO_CANinterfaceErrorhandler_t* CANerrorhandler) { if (CANerrorhandler == NULL) { return; } @@ -239,10 +210,8 @@ void CO_CANerror_rxMsg( CANerrorhandler->noackCounter = 0; } - -CO_CANinterfaceState_t CO_CANerror_txMsg( - CO_CANinterfaceErrorhandler_t *CANerrorhandler) -{ +CO_CANinterfaceState_t +CO_CANerror_txMsg(CO_CANinterfaceErrorhandler_t* CANerrorhandler) { struct timespec now; if (CANerrorhandler == NULL) { @@ -262,11 +231,8 @@ CO_CANinterfaceState_t CO_CANerror_txMsg( return CO_INTERFACE_ACTIVE; } - -CO_CANinterfaceState_t CO_CANerror_rxMsgError( - CO_CANinterfaceErrorhandler_t *CANerrorhandler, - const struct can_frame *msg) -{ +CO_CANinterfaceState_t +CO_CANerror_rxMsgError(CO_CANinterfaceErrorhandler_t* CANerrorhandler, const struct can_frame* msg) { if (CANerrorhandler == NULL) { return CO_INTERFACE_BUS_OFF; } @@ -275,10 +241,8 @@ CO_CANinterfaceState_t CO_CANerror_rxMsgError( /* Log all error messages in full to debug log, even if analysis is done * further on. */ - log_printf(LOG_DEBUG, DBG_CAN_ERROR_GENERAL, (int)msg->can_id, - msg->data[0], msg->data[1], msg->data[2], msg->data[3], - msg->data[4], msg->data[5], msg->data[6], msg->data[7], - CANerrorhandler->ifName); + log_printf(LOG_DEBUG, DBG_CAN_ERROR_GENERAL, (int)msg->can_id, msg->data[0], msg->data[1], msg->data[2], + msg->data[3], msg->data[4], msg->data[5], msg->data[6], msg->data[7], CANerrorhandler->ifName); /* Process errors - start with the most unambiguous one */ diff --git a/CO_main_basic.c b/CO_main_basic.c index a914385..56ef0f0 100644 --- a/CO_main_basic.c +++ b/CO_main_basic.c @@ -48,7 +48,6 @@ #include "CO_time_trace.h" #endif - /* Interval of mainline and real-time thread in microseconds */ #ifndef MAIN_THREAD_INTERVAL_US #define MAIN_THREAD_INTERVAL_US 100000 @@ -59,11 +58,9 @@ /* default values for CO_CANopenInit() */ #ifndef NMT_CONTROL -#define NMT_CONTROL \ - CO_NMT_STARTUP_TO_OPERATIONAL \ - | CO_NMT_ERR_ON_ERR_REG \ - | CO_ERR_REG_GENERIC_ERR \ - | CO_ERR_REG_COMMUNICATION +#define NMT_CONTROL \ + CO_NMT_STARTUP_TO_OPERATIONAL \ + | CO_NMT_ERR_ON_ERR_REG | CO_ERR_REG_GENERIC_ERR | CO_ERR_REG_COMMUNICATION #endif #ifndef FIRST_HB_TIME #define FIRST_HB_TIME 500 @@ -99,7 +96,7 @@ #endif /* CANopen object */ -CO_t *CO = NULL; +CO_t* CO = NULL; /* Active node-id, copied from pendingNodeId in the communication reset */ static uint8_t CO_activeNodeId = CO_LSS_NODE_ID_ASSIGNMENT; @@ -115,10 +112,9 @@ typedef struct { mainlineStorage_t mlStorage = {0}; #if (CO_CONFIG_TRACE) & CO_CONFIG_TRACE_ENABLE -static CO_time_t CO_time; /* Object for current time */ +static CO_time_t CO_time; /* Object for current time */ #endif - /* Helper functions ***********************************************************/ #ifndef CO_SINGLE_THREAD /* Realtime thread */ @@ -128,13 +124,16 @@ static void* rt_thread(void* arg); /* Signal handler */ volatile sig_atomic_t CO_endProgram = 0; -static void sigHandler(int sig) { + +static void +sigHandler(int sig) { (void)sig; CO_endProgram = 1; } /* Message logging function */ -void log_printf(int priority, const char *format, ...) { +void +log_printf(int priority, const char* format, ...) { va_list ap; va_start(ap, format); @@ -163,107 +162,94 @@ void log_printf(int priority, const char *format, ...) { #if (CO_CONFIG_EM) & CO_CONFIG_EM_CONSUMER /* callback for emergency messages */ -static void EmergencyRxCallback(const uint16_t ident, - const uint16_t errorCode, - const uint8_t errorRegister, - const uint8_t errorBit, - const uint32_t infoCode) -{ - int16_t nodeIdRx = ident ? (ident&0x7F) : CO_activeNodeId; - - log_printf(LOG_NOTICE, DBG_EMERGENCY_RX, nodeIdRx, errorCode, - errorRegister, errorBit, infoCode); +static void +EmergencyRxCallback(const uint16_t ident, const uint16_t errorCode, const uint8_t errorRegister, const uint8_t errorBit, + const uint32_t infoCode) { + int16_t nodeIdRx = ident ? (ident & 0x7F) : CO_activeNodeId; + + log_printf(LOG_NOTICE, DBG_EMERGENCY_RX, nodeIdRx, errorCode, errorRegister, errorBit, infoCode); } #endif -#if ((CO_CONFIG_NMT) & CO_CONFIG_NMT_CALLBACK_CHANGE) \ - || ((CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_CHANGE) +#if ((CO_CONFIG_NMT)&CO_CONFIG_NMT_CALLBACK_CHANGE) || ((CO_CONFIG_HB_CONS)&CO_CONFIG_HB_CONS_CALLBACK_CHANGE) /* return string description of NMT state. */ -static char *NmtState2Str(CO_NMT_internalState_t state) -{ - switch(state) { - case CO_NMT_INITIALIZING: return "initializing"; +static char* +NmtState2Str(CO_NMT_internalState_t state) { + switch (state) { + case CO_NMT_INITIALIZING: return "initializing"; case CO_NMT_PRE_OPERATIONAL: return "pre-operational"; - case CO_NMT_OPERATIONAL: return "operational"; - case CO_NMT_STOPPED: return "stopped"; - default: return "unknown"; + case CO_NMT_OPERATIONAL: return "operational"; + case CO_NMT_STOPPED: return "stopped"; + default: return "unknown"; } } #endif #if (CO_CONFIG_NMT) & CO_CONFIG_NMT_CALLBACK_CHANGE /* callback for NMT change messages */ -static void NmtChangedCallback(CO_NMT_internalState_t state) -{ +static void +NmtChangedCallback(CO_NMT_internalState_t state) { log_printf(LOG_NOTICE, DBG_NMT_CHANGE, NmtState2Str(state), state); } #endif #if (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_CHANGE /* callback for monitoring Heartbeat remote NMT state change */ -static void HeartbeatNmtChangedCallback(uint8_t nodeId, uint8_t idx, - CO_NMT_internalState_t state, - void *object) -{ +static void +HeartbeatNmtChangedCallback(uint8_t nodeId, uint8_t idx, CO_NMT_internalState_t state, void* object) { (void)object; - log_printf(LOG_NOTICE, DBG_HB_CONS_NMT_CHANGE, - nodeId, idx, NmtState2Str(state), state); + log_printf(LOG_NOTICE, DBG_HB_CONS_NMT_CHANGE, nodeId, idx, NmtState2Str(state), state); } #endif /* callback for storing node id and bitrate */ -static bool_t LSScfgStoreCallback(void *object, uint8_t id, uint16_t bitRate) { - mainlineStorage_t *mainlineStorage = object; +static bool_t +LSScfgStoreCallback(void* object, uint8_t id, uint16_t bitRate) { + mainlineStorage_t* mainlineStorage = object; mainlineStorage->pendingNodeId = id; mainlineStorage->pendingBitRate = bitRate; return true; } /* Print usage */ -static void printUsage(char *progName) { -printf( -"Usage: %s [options]\n", progName); -printf( -"\n" -"Options:\n" -" -i CANopen Node-id (1..127) or 0xFF (LSS unconfigured).\n"); +static void +printUsage(char* progName) { + printf("Usage: %s [options]\n", progName); + printf("\n" + "Options:\n" + " -i CANopen Node-id (1..127) or 0xFF (LSS unconfigured).\n"); #ifndef CO_SINGLE_THREAD -printf( -" -p Real-time priority of RT thread (1 .. 99). If not set or\n" -" set to -1, then normal scheduler is used for RT thread.\n"); + printf(" -p Real-time priority of RT thread (1 .. 99). If not set or\n" + " set to -1, then normal scheduler is used for RT thread.\n"); #endif -printf( -" -r Enable reboot on CANopen NMT reset_node command. \n"); + printf(" -r Enable reboot on CANopen NMT reset_node command. \n"); #if (CO_CONFIG_STORAGE) & CO_CONFIG_STORAGE_ENABLE -printf( -" -s Path and filename prefix for data storage files.\n" -" By default files are stored in current dictionary.\n"); + printf(" -s Path and filename prefix for data storage files.\n" + " By default files are stored in current dictionary.\n"); #endif #if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII -printf( -" -c Enable command interface for master functionality.\n" -" One of three types of interfaces can be specified as:\n" -" 1. \"stdio\" - Standard IO of a program (terminal).\n" -" 2. \"local-\" - Local socket interface on file\n" -" path, for example \"local-/tmp/CO_command_socket\".\n" -" 3. \"tcp-\" - Tcp socket interface on specified \n" -" port, for example \"tcp-60000\".\n" -" Note that this option may affect security of the CAN.\n" -" -T If -c is specified as local or tcp socket, then this\n" -" parameter specifies socket timeout time in milliseconds.\n" -" Default is 0 - no timeout on established connection.\n"); -#endif -printf( -"\n" -"See also: https://github.com/CANopenNode/CANopenNode\n" -"\n"); + printf(" -c Enable command interface for master functionality.\n" + " One of three types of interfaces can be specified as:\n" + " 1. \"stdio\" - Standard IO of a program (terminal).\n" + " 2. \"local-\" - Local socket interface on file\n" + " path, for example \"local-/tmp/CO_command_socket\".\n" + " 3. \"tcp-\" - Tcp socket interface on specified \n" + " port, for example \"tcp-60000\".\n" + " Note that this option may affect security of the CAN.\n" + " -T If -c is specified as local or tcp socket, then this\n" + " parameter specifies socket timeout time in milliseconds.\n" + " Default is 0 - no timeout on established connection.\n"); +#endif + printf("\n" + "See also: https://github.com/CANopenNode/CANopenNode\n" + "\n"); } - /******************************************************************************* * Mainline thread ******************************************************************************/ -int main (int argc, char *argv[]) { +int +main(int argc, char* argv[]) { int programExit = EXIT_SUCCESS; CO_epoll_t epMain; #ifndef CO_SINGLE_THREAD @@ -276,31 +262,24 @@ int main (int argc, char *argv[]) { int opt; bool_t firstRun = true; - char* CANdevice = NULL; /* CAN device, configurable by arguments. */ - int16_t nodeIdFromArgs = -1; /* May be set by arguments */ - bool_t rebootEnable = false; /* Configurable by arguments */ + char* CANdevice = NULL; /* CAN device, configurable by arguments. */ + int16_t nodeIdFromArgs = -1; /* May be set by arguments */ + bool_t rebootEnable = false; /* Configurable by arguments */ #if (CO_CONFIG_STORAGE) & CO_CONFIG_STORAGE_ENABLE CO_storage_t storage; CO_storage_entry_t storageEntries[] = { - { - .addr = &OD_PERSIST_COMM, - .len = sizeof(OD_PERSIST_COMM), - .subIndexOD = 2, - .attr = CO_storage_cmd | CO_storage_restore, - .filename = {'o','d','_','c','o','m','m', - '.','p','e','r','s','i','s','t','\0'} - }, - { - .addr = &mlStorage, - .len = sizeof(mlStorage), - .subIndexOD = 4, - .attr = CO_storage_cmd | CO_storage_auto | CO_storage_restore, - .filename = {'m','a','i','n','l','i','n','e', - '.','p','e','r','s','i','s','t','\0'} - }, - CO_STORAGE_APPLICATION - }; + {.addr = &OD_PERSIST_COMM, + .len = sizeof(OD_PERSIST_COMM), + .subIndexOD = 2, + .attr = CO_storage_cmd | CO_storage_restore, + .filename = {'o', 'd', '_', 'c', 'o', 'm', 'm', '.', 'p', 'e', 'r', 's', 'i', 's', 't', '\0'}}, + {.addr = &mlStorage, + .len = sizeof(mlStorage), + .subIndexOD = 4, + .attr = CO_storage_cmd | CO_storage_auto | CO_storage_restore, + .filename = {'m', 'a', 'i', 'n', 'l', 'i', 'n', 'e', '.', 'p', 'e', 'r', 's', 'i', 's', 't', '\0'}}, + CO_STORAGE_APPLICATION}; uint8_t storageEntriesCount = sizeof(storageEntries) / sizeof(storageEntries[0]); uint32_t storageInitError = 0; uint32_t storageErrorPrev = 0; @@ -312,67 +291,59 @@ int main (int argc, char *argv[]) { /* values from CO_commandInterface_t */ int32_t commandInterface = CO_COMMAND_IF_DISABLED; /* local socket path if commandInterface == CO_COMMAND_IF_LOCAL_SOCKET */ - char *localSocketPath = NULL; + char* localSocketPath = NULL; uint32_t socketTimeout_ms = 0; #else - #define commandInterface 0 - #define localSocketPath NULL +#define commandInterface 0 +#define localSocketPath NULL #endif /* configure system log */ - setlogmask(LOG_UPTO (LOG_DEBUG)); /* LOG_DEBUG - log all messages */ + setlogmask(LOG_UPTO(LOG_DEBUG)); /* LOG_DEBUG - log all messages */ openlog(argv[0], LOG_PID | LOG_PERROR, LOG_USER); /* print also to standard error */ /* Get program options */ - if(argc < 2 || strcmp(argv[1], "--help") == 0){ + if (argc < 2 || strcmp(argv[1], "--help") == 0) { printUsage(argv[0]); exit(EXIT_SUCCESS); } - while((opt = getopt(argc, argv, "i:p:rc:T:s:")) != -1) { + while ((opt = getopt(argc, argv, "i:p:rc:T:s:")) != -1) { switch (opt) { case 'i': { long int nodeIdLong = strtol(optarg, NULL, 0); - nodeIdFromArgs = (nodeIdLong < 0 || nodeIdLong > 0xFF) - ? 0 : (uint8_t)strtol(optarg, NULL, 0); + nodeIdFromArgs = (nodeIdLong < 0 || nodeIdLong > 0xFF) ? 0 : (uint8_t)strtol(optarg, NULL, 0); break; } #ifndef CO_SINGLE_THREAD - case 'p': rtPriority = strtol(optarg, NULL, 0); - break; + case 'p': rtPriority = strtol(optarg, NULL, 0); break; #endif - case 'r': rebootEnable = true; - break; + case 'r': rebootEnable = true; break; #if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII case 'c': { - const char *comm_stdio = "stdio"; - const char *comm_local = "local-"; - const char *comm_tcp = "tcp-"; + const char* comm_stdio = "stdio"; + const char* comm_local = "local-"; + const char* comm_tcp = "tcp-"; if (strcmp(optarg, comm_stdio) == 0) { commandInterface = CO_COMMAND_IF_STDIO; - } - else if (strncmp(optarg, comm_local, strlen(comm_local)) == 0) { + } else if (strncmp(optarg, comm_local, strlen(comm_local)) == 0) { commandInterface = CO_COMMAND_IF_LOCAL_SOCKET; localSocketPath = &optarg[6]; - } - else if (strncmp(optarg, comm_tcp, strlen(comm_tcp)) == 0) { - const char *portStr = &optarg[4]; + } else if (strncmp(optarg, comm_tcp, strlen(comm_tcp)) == 0) { + const char* portStr = &optarg[4]; uint16_t port; int nMatch = sscanf(portStr, "%hu", &port); - if(nMatch != 1) { + if (nMatch != 1) { log_printf(LOG_CRIT, DBG_NOT_TCP_PORT, portStr); exit(EXIT_FAILURE); } commandInterface = port; - } - else { + } else { log_printf(LOG_CRIT, DBG_ARGUMENT_UNKNOWN, "-c", optarg); exit(EXIT_FAILURE); } break; } - case 'T': - socketTimeout_ms = strtoul(optarg, NULL, 0); - break; + case 'T': socketTimeout_ms = strtoul(optarg, NULL, 0); break; #endif #if (CO_CONFIG_STORAGE) & CO_CONFIG_STORAGE_ENABLE case 's': { @@ -390,48 +361,42 @@ int main (int argc, char *argv[]) { break; } #endif - default: - printUsage(argv[0]); - exit(EXIT_FAILURE); + default: printUsage(argv[0]); exit(EXIT_FAILURE); } } - if(optind < argc) { + if (optind < argc) { CANdevice = argv[optind]; CANptr.can_ifindex = if_nametoindex(CANdevice); } /* Valid NodeId is 1..127 or 0xFF(unconfigured) in case of LSSslaveEnabled */ if ((nodeIdFromArgs == 0 || nodeIdFromArgs > 127) - && (!CO_isLSSslaveEnabled(CO) - || nodeIdFromArgs != CO_LSS_NODE_ID_ASSIGNMENT) - ) { + && (!CO_isLSSslaveEnabled(CO) || nodeIdFromArgs != CO_LSS_NODE_ID_ASSIGNMENT)) { log_printf(LOG_CRIT, DBG_WRONG_NODE_ID, nodeIdFromArgs); printUsage(argv[0]); exit(EXIT_FAILURE); } #ifndef CO_SINGLE_THREAD - if(rtPriority != -1 && (rtPriority < sched_get_priority_min(SCHED_FIFO) - || rtPriority > sched_get_priority_max(SCHED_FIFO))) { + if (rtPriority != -1 + && (rtPriority < sched_get_priority_min(SCHED_FIFO) || rtPriority > sched_get_priority_max(SCHED_FIFO))) { log_printf(LOG_CRIT, DBG_WRONG_PRIORITY, rtPriority); printUsage(argv[0]); exit(EXIT_FAILURE); } #endif - if(CANptr.can_ifindex == 0) { + if (CANptr.can_ifindex == 0) { log_printf(LOG_CRIT, DBG_NO_CAN_DEVICE, CANdevice); exit(EXIT_FAILURE); } - - log_printf(LOG_INFO, DBG_CAN_OPEN_INFO, mlStorage.pendingNodeId,"starting"); - + log_printf(LOG_INFO, DBG_CAN_OPEN_INFO, mlStorage.pendingNodeId, "starting"); /* Allocate memory for CANopen objects */ uint32_t heapMemoryUsed = 0; - CO_config_t *config_ptr = NULL; + CO_config_t* config_ptr = NULL; #ifdef CO_MULTIPLE_OD /* example usage of CO_MULTIPLE_OD (but still single OD here) */ CO_config_t co_config = {0}; @@ -455,24 +420,17 @@ int main (int argc, char *argv[]) { #endif /* CO_MULTIPLE_OD */ CO = CO_new(config_ptr, &heapMemoryUsed); if (CO == NULL) { - log_printf(LOG_CRIT, DBG_GENERAL, - "CO_new(), heapMemoryUsed=", heapMemoryUsed); + log_printf(LOG_CRIT, DBG_GENERAL, "CO_new(), heapMemoryUsed=", heapMemoryUsed); exit(EXIT_FAILURE); } - #if (CO_CONFIG_STORAGE) & CO_CONFIG_STORAGE_ENABLE - err = CO_storageLinux_init(&storage, - CO->CANmodule, - OD_ENTRY_H1010_storeParameters, - OD_ENTRY_H1011_restoreDefaultParameters, - storageEntries, - storageEntriesCount, + err = CO_storageLinux_init(&storage, CO->CANmodule, OD_ENTRY_H1010_storeParameters, + OD_ENTRY_H1011_restoreDefaultParameters, storageEntries, storageEntriesCount, &storageInitError); if (err != CO_ERROR_NO && err != CO_ERROR_DATA_CORRUPT) { - char *filename = storageInitError < storageEntriesCount - ? storageEntries[storageInitError].filename : "???"; + char* filename = storageInitError < storageEntriesCount ? storageEntries[storageInitError].filename : "???"; log_printf(LOG_CRIT, DBG_STORAGE, filename); exit(EXIT_FAILURE); } @@ -480,14 +438,11 @@ int main (int argc, char *argv[]) { #ifdef CO_USE_APPLICATION /* Execute optional external application code */ uint32_t errInfo_app_programStart = 0; - err = app_programStart(&mlStorage.pendingBitRate, - &mlStorage.pendingNodeId, - &errInfo_app_programStart); + err = app_programStart(&mlStorage.pendingBitRate, &mlStorage.pendingNodeId, &errInfo_app_programStart); if (err != CO_ERROR_NO) { if (err == CO_ERROR_OD_PARAMETERS) { log_printf(LOG_CRIT, DBG_OD_ENTRY, errInfo_app_programStart); - } - else { + } else { log_printf(LOG_CRIT, DBG_CAN_OPEN, "app_programStart()", err); } exit(EXIT_FAILURE); @@ -504,11 +459,11 @@ int main (int argc, char *argv[]) { } /* Catch signals SIGINT and SIGTERM */ - if(signal(SIGINT, sigHandler) == SIG_ERR) { + if (signal(SIGINT, sigHandler) == SIG_ERR) { log_printf(LOG_CRIT, DBG_ERRNO, "signal(SIGINT, sigHandler)"); exit(EXIT_FAILURE); } - if(signal(SIGTERM, sigHandler) == SIG_ERR) { + if (signal(SIGTERM, sigHandler) == SIG_ERR) { log_printf(LOG_CRIT, DBG_ERRNO, "signal(SIGTERM, sigHandler)"); exit(EXIT_FAILURE); } @@ -526,16 +481,14 @@ int main (int argc, char *argv[]) { /* Create epoll functions */ err = CO_epoll_create(&epMain, MAIN_THREAD_INTERVAL_US); - if(err != CO_ERROR_NO) { - log_printf(LOG_CRIT, DBG_GENERAL, - "CO_epoll_create(main), err=", err); + if (err != CO_ERROR_NO) { + log_printf(LOG_CRIT, DBG_GENERAL, "CO_epoll_create(main), err=", err); exit(EXIT_FAILURE); } #ifndef CO_SINGLE_THREAD err = CO_epoll_create(&epRT, TMR_THREAD_INTERVAL_US); - if(err != CO_ERROR_NO) { - log_printf(LOG_CRIT, DBG_GENERAL, - "CO_epoll_create(RT), err=", err); + if (err != CO_ERROR_NO) { + log_printf(LOG_CRIT, DBG_GENERAL, "CO_epoll_create(RT), err=", err); exit(EXIT_FAILURE); } CANptr.epoll_fd = epRT.epoll_fd; @@ -543,49 +496,43 @@ int main (int argc, char *argv[]) { CANptr.epoll_fd = epMain.epoll_fd; #endif #if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII - err = CO_epoll_createGtw(&epGtw, epMain.epoll_fd, commandInterface, - socketTimeout_ms, localSocketPath); - if(err != CO_ERROR_NO) { + err = CO_epoll_createGtw(&epGtw, epMain.epoll_fd, commandInterface, socketTimeout_ms, localSocketPath); + if (err != CO_ERROR_NO) { log_printf(LOG_CRIT, DBG_GENERAL, "CO_epoll_createGtw(), err=", err); exit(EXIT_FAILURE); } #endif - - while(reset != CO_RESET_APP && reset != CO_RESET_QUIT && CO_endProgram == 0) { -/* CANopen communication reset - initialize CANopen objects *******************/ + while (reset != CO_RESET_APP && reset != CO_RESET_QUIT && CO_endProgram == 0) { + /* CANopen communication reset - initialize CANopen objects *******************/ uint32_t errInfo; /* Wait rt_thread. */ - if(!firstRun) { + if (!firstRun) { CO_LOCK_OD(CO->CANmodule); CO->CANmodule->CANnormal = false; CO_UNLOCK_OD(CO->CANmodule); } /* Enter CAN configuration. */ - CO_CANsetConfigurationMode((void *)&CANptr); + CO_CANsetConfigurationMode((void*)&CANptr); CO_CANmodule_disable(CO->CANmodule); - /* initialize CANopen */ - err = CO_CANinit(CO, (void *)&CANptr, 0 /* bit rate not used */); - if(err != CO_ERROR_NO) { + err = CO_CANinit(CO, (void*)&CANptr, 0 /* bit rate not used */); + if (err != CO_ERROR_NO) { log_printf(LOG_CRIT, DBG_CAN_OPEN, "CO_CANinit()", err); programExit = EXIT_FAILURE; CO_endProgram = 1; continue; } - CO_LSS_address_t lssAddress = {.identity = { - .vendorID = OD_PERSIST_COMM.x1018_identity.vendor_ID, - .productCode = OD_PERSIST_COMM.x1018_identity.productCode, - .revisionNumber = OD_PERSIST_COMM.x1018_identity.revisionNumber, - .serialNumber = OD_PERSIST_COMM.x1018_identity.serialNumber - }}; - err = CO_LSSinit(CO, &lssAddress, - &mlStorage.pendingNodeId, &mlStorage.pendingBitRate); - if(err != CO_ERROR_NO) { + CO_LSS_address_t lssAddress = {.identity = {.vendorID = OD_PERSIST_COMM.x1018_identity.vendor_ID, + .productCode = OD_PERSIST_COMM.x1018_identity.productCode, + .revisionNumber = OD_PERSIST_COMM.x1018_identity.revisionNumber, + .serialNumber = OD_PERSIST_COMM.x1018_identity.serialNumber}}; + err = CO_LSSinit(CO, &lssAddress, &mlStorage.pendingNodeId, &mlStorage.pendingBitRate); + if (err != CO_ERROR_NO) { log_printf(LOG_CRIT, DBG_CAN_OPEN, "CO_LSSinit()", err); programExit = EXIT_FAILURE; CO_endProgram = 1; @@ -595,23 +542,21 @@ int main (int argc, char *argv[]) { CO_activeNodeId = mlStorage.pendingNodeId; errInfo = 0; - err = CO_CANopenInit(CO, /* CANopen object */ - NULL, /* alternate NMT */ - NULL, /* alternate em */ - OD, /* Object dictionary */ - OD_STATUS_BITS, /* Optional OD_statusBits */ - NMT_CONTROL, /* CO_NMT_control_t */ - FIRST_HB_TIME, /* firstHBTime_ms */ + err = CO_CANopenInit(CO, /* CANopen object */ + NULL, /* alternate NMT */ + NULL, /* alternate em */ + OD, /* Object dictionary */ + OD_STATUS_BITS, /* Optional OD_statusBits */ + NMT_CONTROL, /* CO_NMT_control_t */ + FIRST_HB_TIME, /* firstHBTime_ms */ SDO_SRV_TIMEOUT_TIME, /* SDOserverTimeoutTime_ms */ SDO_CLI_TIMEOUT_TIME, /* SDOclientTimeoutTime_ms */ - SDO_CLI_BLOCK, /* SDOclientBlockTransfer */ - CO_activeNodeId, - &errInfo); - if(err != CO_ERROR_NO && err != CO_ERROR_NODE_ID_UNCONFIGURED_LSS) { + SDO_CLI_BLOCK, /* SDOclientBlockTransfer */ + CO_activeNodeId, &errInfo); + if (err != CO_ERROR_NO && err != CO_ERROR_NODE_ID_UNCONFIGURED_LSS) { if (err == CO_ERROR_OD_PARAMETERS) { log_printf(LOG_CRIT, DBG_OD_ENTRY, errInfo); - } - else { + } else { log_printf(LOG_CRIT, DBG_CAN_OPEN, "CO_CANopenInit()", err); } programExit = EXIT_FAILURE; @@ -624,12 +569,10 @@ int main (int argc, char *argv[]) { #if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII CO_epoll_initCANopenGtw(&epGtw, CO); #endif - CO_LSSslave_initCfgStoreCall(CO->LSSslave, &mlStorage, - LSScfgStoreCallback); - if(!CO->nodeIdUnconfigured) { - if(errInfo != 0) { - CO_errorReport(CO->em, CO_EM_INCONSISTENT_OBJECT_DICT, - CO_EMC_DATA_SET, errInfo); + CO_LSSslave_initCfgStoreCall(CO->LSSslave, &mlStorage, LSScfgStoreCallback); + if (!CO->nodeIdUnconfigured) { + if (errInfo != 0) { + CO_errorReport(CO->em, CO_EM_INCONSISTENT_OBJECT_DICT, CO_EMC_DATA_SET, errInfo); } #if (CO_CONFIG_EM) & CO_CONFIG_EM_CONSUMER CO_EM_initCallbackRx(CO->em, EmergencyRxCallback); @@ -638,19 +581,16 @@ int main (int argc, char *argv[]) { CO_NMT_initCallbackChanged(CO->NMT, NmtChangedCallback); #endif #if (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_CHANGE - CO_HBconsumer_initCallbackNmtChanged(CO->HBcons, 0, NULL, - HeartbeatNmtChangedCallback); + CO_HBconsumer_initCallbackNmtChanged(CO->HBcons, 0, NULL, HeartbeatNmtChangedCallback); #endif #if (CO_CONFIG_STORAGE) & CO_CONFIG_STORAGE_ENABLE - if(storageInitError != 0) { - CO_errorReport(CO->em, CO_EM_NON_VOLATILE_MEMORY, - CO_EMC_HARDWARE, storageInitError); + if (storageInitError != 0) { + CO_errorReport(CO->em, CO_EM_NON_VOLATILE_MEMORY, CO_EMC_HARDWARE, storageInitError); } #endif #ifdef CO_USE_APPLICATION if (errInfo_app_programStart != 0) { - CO_errorReport(CO->em, CO_EM_INCONSISTENT_OBJECT_DICT, - CO_EMC_DATA_SET, errInfo_app_programStart); + CO_errorReport(CO->em, CO_EM_INCONSISTENT_OBJECT_DICT, CO_EMC_DATA_SET, errInfo_app_programStart); } #endif @@ -659,24 +599,23 @@ int main (int argc, char *argv[]) { CO_time_init(&CO_time, CO->SDO[0], &OD_time.epochTimeBaseMs, &OD_time.epochTimeOffsetMs, 0x2130); #endif log_printf(LOG_INFO, DBG_CAN_OPEN_INFO, CO_activeNodeId, "communication reset"); - } - else { + } else { log_printf(LOG_INFO, DBG_CAN_OPEN_INFO, CO_activeNodeId, "node-id not initialized"); } /* First time only initialization. */ - if(firstRun) { + if (firstRun) { firstRun = false; CO_TIME_set(CO->TIME, time_ms, time_days, TIME_STAMP_INTERVAL_MS); #ifndef CO_SINGLE_THREAD /* Create rt_thread and set priority */ - if(pthread_create(&rt_thread_id, NULL, rt_thread, NULL) != 0) { + if (pthread_create(&rt_thread_id, NULL, rt_thread, NULL) != 0) { log_printf(LOG_CRIT, DBG_ERRNO, "pthread_create(rt_thread)"); programExit = EXIT_FAILURE; CO_endProgram = 1; continue; } - if(rtPriority > 0) { + if (rtPriority > 0) { struct sched_param param; param.sched_priority = rtPriority; @@ -690,23 +629,20 @@ int main (int argc, char *argv[]) { #endif } /* if(firstRun) */ - #ifdef CO_USE_APPLICATION /* Execute optional external application code */ app_communicationReset(CO); #endif errInfo = 0; - err = CO_CANopenInitPDO(CO, /* CANopen object */ - CO->em, /* emergency object */ - OD, /* Object dictionary */ - CO_activeNodeId, - &errInfo); - if(err != CO_ERROR_NO && err != CO_ERROR_NODE_ID_UNCONFIGURED_LSS) { + err = CO_CANopenInitPDO(CO, /* CANopen object */ + CO->em, /* emergency object */ + OD, /* Object dictionary */ + CO_activeNodeId, &errInfo); + if (err != CO_ERROR_NO && err != CO_ERROR_NODE_ID_UNCONFIGURED_LSS) { if (err == CO_ERROR_OD_PARAMETERS) { log_printf(LOG_CRIT, DBG_OD_ENTRY, errInfo); - } - else { + } else { log_printf(LOG_CRIT, DBG_CAN_OPEN, "CO_CANopenInitPDO()", err); } programExit = EXIT_FAILURE; @@ -714,7 +650,6 @@ int main (int argc, char *argv[]) { continue; } - /* start CAN */ CO_CANsetNormalMode(CO->CANmodule); @@ -722,9 +657,8 @@ int main (int argc, char *argv[]) { log_printf(LOG_INFO, DBG_CAN_OPEN_INFO, CO_activeNodeId, "running ..."); - - while(reset == CO_RESET_NOT && CO_endProgram == 0) { -/* loop for normal program execution ******************************************/ + while (reset == CO_RESET_NOT && CO_endProgram == 0) { + /* loop for normal program execution ******************************************/ CO_epoll_wait(&epMain); #ifdef CO_SINGLE_THREAD CO_epoll_processRT(&epMain, CO, false); @@ -744,15 +678,12 @@ int main (int argc, char *argv[]) { /* don't save more often than interval */ if (storageIntervalTimer < CO_STORAGE_AUTO_INTERVAL) { storageIntervalTimer += epMain.timeDifference_us; - } - else { + } else { uint32_t mask = CO_storageLinux_auto_process(&storage, false); - if(mask != storageErrorPrev && !CO->nodeIdUnconfigured) { - if(mask != 0) { - CO_errorReport(CO->em, CO_EM_NON_VOLATILE_AUTO_SAVE, - CO_EMC_HARDWARE, mask); - } - else { + if (mask != storageErrorPrev && !CO->nodeIdUnconfigured) { + if (mask != 0) { + CO_errorReport(CO->em, CO_EM_NON_VOLATILE_AUTO_SAVE, CO_EMC_HARDWARE, mask); + } else { CO_errorReset(CO->em, CO_EM_NON_VOLATILE_AUTO_SAVE, 0); } } @@ -763,8 +694,7 @@ int main (int argc, char *argv[]) { } } /* while(reset != CO_RESET_APP */ - -/* program exit ***************************************************************/ + /* program exit ***************************************************************/ /* join threads */ CO_endProgram = 1; #ifndef CO_SINGLE_THREAD @@ -790,15 +720,15 @@ int main (int argc, char *argv[]) { #if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII CO_epoll_closeGtw(&epGtw); #endif - CO_CANsetConfigurationMode((void *)&CANptr); + CO_CANsetConfigurationMode((void*)&CANptr); CO_delete(CO); log_printf(LOG_INFO, DBG_CAN_OPEN_INFO, CO_activeNodeId, "finished"); /* Flush all buffers (and reboot) */ - if(rebootEnable && reset == CO_RESET_APP) { + if (rebootEnable && reset == CO_RESET_APP) { sync(); - if(reboot(LINUX_REBOOT_CMD_RESTART) != 0) { + if (reboot(LINUX_REBOOT_CMD_RESTART) != 0) { log_printf(LOG_CRIT, DBG_ERRNO, "reboot()"); exit(EXIT_FAILURE); } @@ -811,10 +741,11 @@ int main (int argc, char *argv[]) { /******************************************************************************* * Realtime thread for CAN receive and threadTmr ******************************************************************************/ -static void* rt_thread(void* arg) { +static void* +rt_thread(void* arg) { (void)arg; /* Endless loop */ - while(CO_endProgram == 0) { + while (CO_endProgram == 0) { CO_epoll_wait(&epRT); CO_epoll_processRT(&epRT, CO, true); @@ -823,7 +754,7 @@ static void* rt_thread(void* arg) { #if (CO_CONFIG_TRACE) & CO_CONFIG_TRACE_ENABLE /* Monitor variables with trace objects */ CO_time_process(&CO_time); - for(i=0; iCNT_TRACE; i++) { + for (i = 0; i < OD_traceEnable && i < co->CNT_TRACE; i++) { CO_trace_process(CO->trace[i], *CO_time.epochTimeOffsetMs); } #endif @@ -832,7 +763,6 @@ static void* rt_thread(void* arg) { /* Execute optional external application code */ app_programRt(CO, epRT.timeDifference_us); #endif - } return NULL; diff --git a/CO_storageLinux.c b/CO_storageLinux.c index dae6929..406d7e3 100644 --- a/CO_storageLinux.c +++ b/CO_storageLinux.c @@ -26,26 +26,29 @@ #if (CO_CONFIG_STORAGE) & CO_CONFIG_STORAGE_ENABLE - /* * Function for writing data on "Store parameters" command - OD object 1010 * * For more information see file CO_storage.h, CO_storage_entry_t. */ -static ODR_t storeLinux(CO_storage_entry_t *entry, CO_CANmodule_t *CANmodule) { +static ODR_t +storeLinux(CO_storage_entry_t* entry, CO_CANmodule_t* CANmodule) { ODR_t ret = ODR_OK; uint16_t crc_store; /* Create names for temporary and old file */ size_t fn_len = strlen(entry->filename) + 5; - char *filename_tmp = malloc(fn_len); - char *filename_old = malloc(fn_len); + char* filename_tmp = malloc(fn_len); + char* filename_old = malloc(fn_len); if (filename_tmp == NULL || filename_old == NULL) { - if (filename_tmp != NULL) free(filename_tmp); - if (filename_old != NULL) free(filename_old); + if (filename_tmp != NULL) { + free(filename_tmp); + } + if (filename_old != NULL) { + free(filename_old); + } ret = ODR_OUT_OF_MEM; - } - else { + } else { strcpy(filename_tmp, entry->filename); strcpy(filename_old, entry->filename); strcat(filename_tmp, ".tmp"); @@ -54,11 +57,10 @@ static ODR_t storeLinux(CO_storage_entry_t *entry, CO_CANmodule_t *CANmodule) { /* Open a temporary file and write data to it */ if (ret == ODR_OK) { - FILE *fp = fopen(filename_tmp, "w"); + FILE* fp = fopen(filename_tmp, "w"); if (fp == NULL) { ret = ODR_HW; - } - else { + } else { CO_LOCK_OD(CANmodule); size_t cnt = fwrite(entry->addr, 1, entry->len, fp); crc_store = crc16_ccitt(entry->addr, entry->len, 0); @@ -73,8 +75,8 @@ static ODR_t storeLinux(CO_storage_entry_t *entry, CO_CANmodule_t *CANmodule) { /* Verify data */ if (ret == ODR_OK) { - uint8_t *buf = NULL; - FILE *fp = NULL; + uint8_t* buf = NULL; + FILE* fp = NULL; size_t cnt = 0; uint16_t crc_verify, crc_read; @@ -90,9 +92,8 @@ static ODR_t storeLinux(CO_storage_entry_t *entry, CO_CANmodule_t *CANmodule) { free(buf); } /* If size or CRC differs, report error */ - if (buf == NULL || fp == NULL || cnt != (entry->len+sizeof(crc_verify)) - || crc_store != crc_verify || crc_store != crc_read - ) { + if (buf == NULL || fp == NULL || cnt != (entry->len + sizeof(crc_verify)) || crc_store != crc_verify + || crc_store != crc_read) { ret = ODR_HW; } } @@ -113,14 +114,14 @@ static ODR_t storeLinux(CO_storage_entry_t *entry, CO_CANmodule_t *CANmodule) { return ret; } - /* * Function for restoring data on "Restore default parameters" command - OD 1011 * * For more information see file CO_storage.h, CO_storage_entry_t. */ -static ODR_t restoreLinux(CO_storage_entry_t *entry, CO_CANmodule_t *CANmodule){ - (void) CANmodule; +static ODR_t +restoreLinux(CO_storage_entry_t* entry, CO_CANmodule_t* CANmodule) { + (void)CANmodule; ODR_t ret = ODR_OK; /* close the file first, if auto storage */ @@ -130,11 +131,10 @@ static ODR_t restoreLinux(CO_storage_entry_t *entry, CO_CANmodule_t *CANmodule){ } /* Rename existing filename to *.old. */ - char *filename_old = malloc(strlen(entry->filename) + 5); + char* filename_old = malloc(strlen(entry->filename) + 5); if (filename_old == NULL) { ret = ODR_OUT_OF_MEM; - } - else { + } else { strcpy(filename_old, entry->filename); strcat(filename_old, ".old"); rename(entry->filename, filename_old); @@ -143,11 +143,10 @@ static ODR_t restoreLinux(CO_storage_entry_t *entry, CO_CANmodule_t *CANmodule){ /* create an empty file and write "-\n" to it. */ if (ret == ODR_OK) { - FILE *fp = fopen(entry->filename, "w"); + FILE* fp = fopen(entry->filename, "w"); if (fp == NULL) { ret = ODR_HW; - } - else { + } else { fputs("-\n", fp); fclose(fp); } @@ -156,35 +155,22 @@ static ODR_t restoreLinux(CO_storage_entry_t *entry, CO_CANmodule_t *CANmodule){ return ret; } - -CO_ReturnError_t CO_storageLinux_init(CO_storage_t *storage, - CO_CANmodule_t *CANmodule, - OD_entry_t *OD_1010_StoreParameters, - OD_entry_t *OD_1011_RestoreDefaultParam, - CO_storage_entry_t *entries, - uint8_t entriesCount, - uint32_t *storageInitError) -{ +CO_ReturnError_t +CO_storageLinux_init(CO_storage_t* storage, CO_CANmodule_t* CANmodule, OD_entry_t* OD_1010_StoreParameters, + OD_entry_t* OD_1011_RestoreDefaultParam, CO_storage_entry_t* entries, uint8_t entriesCount, + uint32_t* storageInitError) { CO_ReturnError_t ret; /* verify arguments */ - if (storage == NULL || entries == NULL || entriesCount == 0 - || storageInitError == NULL - ) { + if (storage == NULL || entries == NULL || entriesCount == 0 || storageInitError == NULL) { return CO_ERROR_ILLEGAL_ARGUMENT; } storage->enabled = false; /* initialize storage and OD extensions */ - ret = CO_storage_init(storage, - CANmodule, - OD_1010_StoreParameters, - OD_1011_RestoreDefaultParam, - storeLinux, - restoreLinux, - entries, - entriesCount); + ret = CO_storage_init(storage, CANmodule, OD_1010_StoreParameters, OD_1011_RestoreDefaultParam, storeLinux, + restoreLinux, entries, entriesCount); if (ret != CO_ERROR_NO) { return ret; } @@ -192,26 +178,23 @@ CO_ReturnError_t CO_storageLinux_init(CO_storage_t *storage, /* initialize entries */ *storageInitError = 0; for (uint8_t i = 0; i < entriesCount; i++) { - CO_storage_entry_t *entry = &entries[i]; + CO_storage_entry_t* entry = &entries[i]; bool_t dataCorrupt = false; - char *writeFileAccess = "w"; + char* writeFileAccess = "w"; /* verify arguments */ - if (entry->addr == NULL || entry->len == 0 || entry->subIndexOD < 2 - || strlen(entry->filename) == 0 - ) { + if (entry->addr == NULL || entry->len == 0 || entry->subIndexOD < 2 || strlen(entry->filename) == 0) { *storageInitError = i; return CO_ERROR_ILLEGAL_ARGUMENT; } /* Open file, check existence and create temporary buffer */ - uint8_t *buf = NULL; - FILE * fp = fopen(entry->filename, "r"); + uint8_t* buf = NULL; + FILE* fp = fopen(entry->filename, "r"); if (fp == NULL) { dataCorrupt = true; ret = CO_ERROR_DATA_CORRUPT; - } - else { + } else { buf = malloc(entry->len + sizeof(uint16_t)); if (buf == NULL) { fclose(fp); @@ -235,8 +218,7 @@ CO_ReturnError_t CO_storageLinux_init(CO_storage_t *storage, memcpy(entry->addr, buf, entry->len); entry->crc = crc1; writeFileAccess = "r+"; - } - else { + } else { dataCorrupt = true; ret = CO_ERROR_DATA_CORRUPT; } @@ -249,8 +231,10 @@ CO_ReturnError_t CO_storageLinux_init(CO_storage_t *storage, /* additional info in case of error */ if (dataCorrupt) { uint32_t errorBit = entry->subIndexOD; - if (errorBit > 31) errorBit = 31; - *storageInitError |= ((uint32_t) 1) << errorBit; + if (errorBit > 31) { + errorBit = 31; + } + *storageInitError |= ((uint32_t)1) << errorBit; } /* open file for auto storage, if set so */ @@ -267,10 +251,8 @@ CO_ReturnError_t CO_storageLinux_init(CO_storage_t *storage, return ret; } - -uint32_t CO_storageLinux_auto_process(CO_storage_t *storage, - bool_t closeFiles) -{ +uint32_t +CO_storageLinux_auto_process(CO_storage_t* storage, bool_t closeFiles) { uint32_t storageError = 0; /* verify arguments */ @@ -280,10 +262,11 @@ uint32_t CO_storageLinux_auto_process(CO_storage_t *storage, /* loop through entries */ for (uint8_t i = 0; i < storage->entriesCount; i++) { - CO_storage_entry_t *entry = &storage->entries[i]; + CO_storage_entry_t* entry = &storage->entries[i]; - if ((entry->attr & CO_storage_auto) == 0 || entry->fp == NULL) + if ((entry->attr & CO_storage_auto) == 0 || entry->fp == NULL) { continue; + } /* If CRC of the current data differs, save the file */ uint16_t crc = crc16_ccitt(entry->addr, entry->len, 0); @@ -297,12 +280,13 @@ uint32_t CO_storageLinux_auto_process(CO_storage_t *storage, fflush(entry->fp); if (cnt == (entry->len + sizeof(crc))) { entry->crc = crc; - } - else { + } else { /* error with save */ uint32_t errorBit = entry->subIndexOD; - if (errorBit > 31) errorBit = 31; - storageError |= ((uint32_t) 1) << errorBit; + if (errorBit > 31) { + errorBit = 31; + } + storageError |= ((uint32_t)1) << errorBit; } } diff --git a/cocomm/cocomm.c b/cocomm/cocomm.c index fed7b21..fb4a554 100644 --- a/cocomm/cocomm.c +++ b/cocomm/cocomm.c @@ -39,58 +39,60 @@ /* colors and stream for printing status */ char *greenC, *redC, *resetC; -FILE *errStream; - - -static void printUsage(char *progName) { -fprintf(errStream, -"Usage: %s [options] ['' [''] ...]\n" -"\n" -"Program reads CANopen gateway command strings from arguments, standard input or\n" -"file. It sends commands to canopend via socket, line after line. Response status\n" -"is printed to standard error and response data (for example, value from write\n" -"command) is printed to standard output. Command strings from arguments must\n" -"be quoted. Socket is either unix domain socket (default) or a remote tcp socket\n" -"(option -t). For more information see http://www.can-cia.org/, CiA 309 standard.\n" -"\n" -"Options:\n" -" -f Path to the input file.\n" -" -s Path to the unix socket. If not specified, path is obtained\n" -" from environmental variable, configured with:\n" -" 'export cocomm_socket='. If latter is not\n" -" specified, default value is used: '/tmp/CO_command_socket'.\n" -" -t Connect via tcp to remote . Set also with\n" -" 'export cocomm_host='. Unix socket is used by default.\n" -" -p Tcp port to connect to when using -t. Set also with\n" -" 'export cocomm_port='. Default is 60000.\n" -" -i If set, then standard input will be read after each command\n" -" string from arguments. Useful with write commands.\n" -" -o all|data|flat By defult (setting 'all') outupt is split to colored stderr\n" -" and stdout. 'data' prints data only to stdout. 'flat' prints\n" -" all to stdout, set also with 'export cocomm_flat=<0|1>'.\n" -" -d If specified, then candump of specified CAN device will be\n" -" printed after the command response. Set also with\n" -" 'export cocomm_candump='. Not used by default.\n" -" -n Print of candump messages, then exit. Set also with\n" -" 'export cocomm_candump_count='. Default is 10.\n" -" -T Exit candump after without reception. Set also with\n" -" 'export cocomm_candump_timeout='. Default is 1000.\n" -" --help Display this help.\n" -"\n" -"For help on command strings type '%s \"help\"'.\n" -"\n" -"See also: https://github.com/CANopenNode/CANopenLinux\n" -"\n", progName, progName); +FILE* errStream; + +static void +printUsage(char* progName) { + fprintf(errStream, + "Usage: %s [options] ['' [''] ...]\n" + "\n" + "Program reads CANopen gateway command strings from arguments, standard input or\n" + "file. It sends commands to canopend via socket, line after line. Response status\n" + "is printed to standard error and response data (for example, value from write\n" + "command) is printed to standard output. Command strings from arguments must\n" + "be quoted. Socket is either unix domain socket (default) or a remote tcp socket\n" + "(option -t). For more information see http://www.can-cia.org/, CiA 309 standard.\n" + "\n" + "Options:\n" + " -f Path to the input file.\n" + " -s Path to the unix socket. If not specified, path is obtained\n" + " from environmental variable, configured with:\n" + " 'export cocomm_socket='. If latter is not\n" + " specified, default value is used: '/tmp/CO_command_socket'.\n" + " -t Connect via tcp to remote . Set also with\n" + " 'export cocomm_host='. Unix socket is used by default.\n" + " -p Tcp port to connect to when using -t. Set also with\n" + " 'export cocomm_port='. Default is 60000.\n" + " -i If set, then standard input will be read after each command\n" + " string from arguments. Useful with write commands.\n" + " -o all|data|flat By defult (setting 'all') outupt is split to colored stderr\n" + " and stdout. 'data' prints data only to stdout. 'flat' prints\n" + " all to stdout, set also with 'export cocomm_flat=<0|1>'.\n" + " -d If specified, then candump of specified CAN device will be\n" + " printed after the command response. Set also with\n" + " 'export cocomm_candump='. Not used by default.\n" + " -n Print of candump messages, then exit. Set also with\n" + " 'export cocomm_candump_count='. Default is 10.\n" + " -T Exit candump after without reception. Set also with\n" + " 'export cocomm_candump_timeout='. Default is 1000.\n" + " --help Display this help.\n" + "\n" + "For help on command strings type '%s \"help\"'.\n" + "\n" + "See also: https://github.com/CANopenNode/CANopenLinux\n" + "\n", + progName, progName); } /* print reply, status to errStream (red or green), value to stdout */ -static int printReply(int fd_gtw) { +static int +printReply(int fd_gtw) { char* replyBuf = malloc(BUF_SIZE + BUF_LAG + 1); size_t count = 0; /* count of bytes in replyBuf */ int firstPass = 1; int ret = EXIT_SUCCESS; - if(replyBuf == NULL) { + if (replyBuf == NULL) { perror("replyBuf malloc"); exit(EXIT_FAILURE); } @@ -98,27 +100,23 @@ static int printReply(int fd_gtw) { for (;;) { ssize_t nRead = read(fd_gtw, &replyBuf[count], BUF_SIZE); /* blocking */ - if(nRead > 0) { + if (nRead > 0) { count += nRead; replyBuf[count] = 0; if (firstPass == 1) { /* check for response type. Only response value goes to stdout */ - if (strstr(replyBuf, "] ERROR:") != NULL - && strstr(replyBuf, "\r\n") != NULL - ) { + if (strstr(replyBuf, "] ERROR:") != NULL && strstr(replyBuf, "\r\n") != NULL) { fprintf(errStream, "%s%s%s", redC, replyBuf, resetC); ret = EXIT_FAILURE; break; - } - else if (strstr(replyBuf, "] OK\r\n") != NULL) { + } else if (strstr(replyBuf, "] OK\r\n") != NULL) { fprintf(errStream, "%s%s%s", greenC, replyBuf, resetC); break; - } - else { - char *replyBufTrimmed = replyBuf; - char *seq = strstr(replyBuf, "] "); - char *end = strstr(replyBuf, "\r\n"); + } else { + char* replyBufTrimmed = replyBuf; + char* seq = strstr(replyBuf, "] "); + char* end = strstr(replyBuf, "\r\n"); if (seq != NULL && (size_t)(seq - replyBuf) < 15) { replyBufTrimmed = seq + 2; seq[1] = 0; @@ -131,8 +129,7 @@ static int printReply(int fd_gtw) { fflush(stdout); fputs("\r\n", errStream); break; - } - else { + } else { /* print replyBuf to stdout, except last BUF_LAG bytes. * move them to the beginning of the replyBuf */ if (count > BUF_LAG) { @@ -145,19 +142,17 @@ static int printReply(int fd_gtw) { } } firstPass = 0; - } - else { - char *end = strstr(replyBuf, "\r\n"); + } else { + char* end = strstr(replyBuf, "\r\n"); if (end != NULL) { - char *errResp = strstr(replyBuf, "\n...ERROR:0x"); + char* errResp = strstr(replyBuf, "\n...ERROR:0x"); if (errResp != NULL) { errResp[0] = 0; fputs(replyBuf, stdout); fflush(stdout); fprintf(errStream, "\n%s%s%s", redC, &errResp[1], resetC); ret = EXIT_FAILURE; - } - else { + } else { end[0] = 0; fputs(replyBuf, stdout); fflush(stdout); @@ -174,13 +169,11 @@ static int printReply(int fd_gtw) { memmove(replyBuf, &replyBuf[nWrite], count); } } - } - else if (nRead == 0) { + } else if (nRead == 0) { fprintf(errStream, "%sError, zero response%s\n", redC, resetC); ret = EXIT_FAILURE; break; - } - else { + } else { perror("Socket read failed"); free(replyBuf); exit(EXIT_FAILURE); @@ -192,16 +185,17 @@ static int printReply(int fd_gtw) { return ret; } - -int main (int argc, char *argv[]) { +int +main(int argc, char* argv[]) { /* configurable options */ - enum {out_all, out_data, out_flat} outputType = out_all; - char *inputFilePath = NULL; - char *socketPath = "/tmp/CO_command_socket"; /* Name of the local domain socket */ - char hostname[HOST_NAME_MAX]; /* name of the remote TCP host */ - char tcpPort[20] = "60000"; /* default port when used in tcp mode */ + enum { out_all, out_data, out_flat } outputType = out_all; + + char* inputFilePath = NULL; + char* socketPath = "/tmp/CO_command_socket"; /* Name of the local domain socket */ + char hostname[HOST_NAME_MAX]; /* name of the remote TCP host */ + char tcpPort[20] = "60000"; /* default port when used in tcp mode */ int additionalReadStdin = 0; - char *candump = NULL; + char* candump = NULL; long candumpCount = 10; long candumpTmo = 1000; @@ -213,13 +207,13 @@ int main (int argc, char *argv[]) { sa_family_t addrFamily = AF_UNIX; errStream = stderr; - if(argc >= 2 && strcmp(argv[1], "--help") == 0) { + if (argc >= 2 && strcmp(argv[1], "--help") == 0) { printUsage(argv[0]); exit(EXIT_SUCCESS); } /* Get program options from environment variables */ - char *env; + char* env; if ((env = getenv("cocomm_host")) != NULL) { strncpy(hostname, env, sizeof(hostname)); addrFamily = AF_INET; @@ -249,11 +243,9 @@ int main (int argc, char *argv[]) { } /* Get program options from arguments */ - while((opt = getopt(argc, argv, "f:s:t:p:io:d:n:T:")) != -1) { + while ((opt = getopt(argc, argv, "f:s:t:p:io:d:n:T:")) != -1) { switch (opt) { - case 'f': - inputFilePath = optarg; - break; + case 'f': inputFilePath = optarg; break; case 's': addrFamily = AF_UNIX; socketPath = optarg; @@ -262,30 +254,19 @@ int main (int argc, char *argv[]) { addrFamily = AF_INET; strncpy(hostname, optarg, sizeof(hostname)); break; - case 'p': - strncpy(tcpPort, optarg, sizeof(tcpPort)); - break; - case 'i': - additionalReadStdin = 1; - break; + case 'p': strncpy(tcpPort, optarg, sizeof(tcpPort)); break; + case 'i': additionalReadStdin = 1; break; case 'o': - if (strcmp(optarg, "data") == 0) + if (strcmp(optarg, "data") == 0) { outputType = out_data; - else if (strcmp(optarg, "flat") == 0) + } else if (strcmp(optarg, "flat") == 0) { outputType = out_flat; + } break; - case 'd': - candump = optarg; - break; - case 'n': - candumpCount = atol(optarg); - break; - case 'T': - candumpTmo = atol(optarg); - break; - default: - printUsage(argv[0]); - exit(EXIT_FAILURE); + case 'd': candump = optarg; break; + case 'n': candumpCount = atol(optarg); break; + case 'T': candumpTmo = atol(optarg); break; + default: printUsage(argv[0]); exit(EXIT_FAILURE); } } @@ -316,7 +297,7 @@ int main (int argc, char *argv[]) { } /* Create and connect client socket */ - if(addrFamily == AF_INET) { + if (addrFamily == AF_INET) { struct addrinfo hints, *res, *rp; int errcode; @@ -350,10 +331,9 @@ int main (int argc, char *argv[]) { perror("Socket connection failed"); exit(EXIT_FAILURE); } - } - else { /* addrFamily == AF_UNIX */ + } else { /* addrFamily == AF_UNIX */ fd_gtw = socket(AF_UNIX, SOCK_STREAM, 0); - if(fd_gtw == -1) { + if (fd_gtw == -1) { perror("Socket creation failed"); exit(EXIT_FAILURE); } @@ -362,7 +342,7 @@ int main (int argc, char *argv[]) { addr_un.sun_family = addrFamily; strncpy(addr_un.sun_path, socketPath, sizeof(addr_un.sun_path) - 1); - if(connect(fd_gtw, (struct sockaddr *)&addr_un, sizeof(struct sockaddr_un)) == -1) { + if (connect(fd_gtw, (struct sockaddr*)&addr_un, sizeof(struct sockaddr_un)) == -1) { fprintf(stderr, "Socket connection failed \"%s\": ", socketPath); perror(NULL); exit(EXIT_FAILURE); @@ -397,8 +377,7 @@ int main (int argc, char *argv[]) { perror(NULL); exit(EXIT_FAILURE); } - } - else { + } else { candumpCount = 0; } @@ -406,22 +385,24 @@ int main (int argc, char *argv[]) { int ret = EXIT_SUCCESS; commBuf = malloc(BUF_SIZE); - if(commBuf == NULL) { + if (commBuf == NULL) { perror("commBuf malloc"); exit(EXIT_FAILURE); } - if(inputFilePath != NULL) { - FILE *fp = fopen(inputFilePath, "r"); - if(fp == NULL) { + if (inputFilePath != NULL) { + FILE* fp = fopen(inputFilePath, "r"); + if (fp == NULL) { perror("Can't open input file"); free(commBuf); exit(EXIT_FAILURE); } - while(fgets(commBuf, BUF_SIZE, fp) != NULL) { + while (fgets(commBuf, BUF_SIZE, fp) != NULL) { size_t len = strlen(commBuf); - if (len < 1) continue; + if (len < 1) { + continue; + } /* send command */ if (write(fd_gtw, commBuf, len) != len) { /* blocking function */ @@ -442,17 +423,17 @@ int main (int argc, char *argv[]) { } /* get command from arguments */ - else if(optind < argc) { - for(int i = optind; i < argc; i++) { - char *comm = argv[i]; + else if (optind < argc) { + for (int i = optind; i < argc; i++) { + char* comm = argv[i]; commBuf[0] = 0; /* Add sequence number if not present on command line argument */ - if(comm[0] != '[' && comm[0] != '#') { + if (comm[0] != '[' && comm[0] != '#') { sprintf(commBuf, "[%d] ", i - optind + 1); } - if((strlen(commBuf) + strlen(comm)) >= (BUF_SIZE - 2)) { + if ((strlen(commBuf) + strlen(comm)) >= (BUF_SIZE - 2)) { fprintf(errStream, "%sCommand string too long!%s\n", redC, greenC); continue; } @@ -467,21 +448,22 @@ int main (int argc, char *argv[]) { free(commBuf); exit(EXIT_FAILURE); } - } - else { + } else { strcat(commBuf, " "); size_t len = strlen(commBuf); char lastChar; do { - if (fgets(commBuf+len, BUF_SIZE-1-len, stdin) == NULL) + if (fgets(commBuf + len, BUF_SIZE - 1 - len, stdin) == NULL) { strcat(commBuf, "\n"); + } len = strlen(commBuf); lastChar = commBuf[len - 1]; - if (len < BUF_SIZE-2 && lastChar != '\n') + if (len < BUF_SIZE - 2 && lastChar != '\n') { continue; + } /* send command */ if (write(fd_gtw, commBuf, len) != len) { /* blocking f. */ @@ -503,9 +485,11 @@ int main (int argc, char *argv[]) { /* get commands from stdin, line after line */ else { - while(fgets(commBuf, BUF_SIZE, stdin) != NULL) { + while (fgets(commBuf, BUF_SIZE, stdin) != NULL) { size_t len = strlen(commBuf); - if (len < 1) continue; + if (len < 1) { + continue; + } /* send command */ if (write(fd_gtw, commBuf, len) != len) { /* blocking function */ @@ -549,7 +533,7 @@ int main (int argc, char *argv[]) { } *pbuf = 0; - printf ("%03X#%s\n", canFrame.can_id, buf); + printf("%03X#%s\n", canFrame.can_id, buf); } close(fd_candump); }