#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 && identNew!=identCurrent) {
+ if (identCurrent < CO_CAN_MSG_SFF_MAX_COB_ID && identNew != identCurrent) {
lookup[identCurrent] = CO_INVALID_COB_ID;
}
if (index == 0) {
lookup[0] = 0;
}
- }
- else {
+ } else {
lookup[identNew] = index;
}
}
-
-static uint32_t CO_CANgetIndexFromIdent(
- uint32_t *lookup,
- uint32_t ident)
-{
+static uint32_t
+CO_CANgetIndexFromIdent(uint32_t* lookup, uint32_t ident) {
/* check if this COB ID is part of the table */
if (ident > CO_CAN_MSG_SFF_MAX_COB_ID) {
return CO_INVALID_COB_ID;
#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;
}
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;
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++;
}
}
}
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) {
}
}
-
-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;
}
#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; i<rxSize; i++){
+ for (i = 0U; i < rxSize; i++) {
rxArray[i].ident = 0U;
rxArray[i].mask = 0xFFFFFFFFU;
rxArray[i].object = NULL;
#if CO_DRIVER_MULTI_INTERFACE == 0
/* add one interface */
- ret = CO_CANmodule_addInterface(CANmodule,
- CANptrReal->can_ifindex);
+ ret = CO_CANmodule_addInterface(CANmodule, CANptrReal->can_ifindex);
if (ret != CO_ERROR_NO) {
CO_CANmodule_disable(CANmodule);
return ret;
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
}
/* 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;
/* 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;
}
/* 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 */
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;
#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;
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;
}
return ret;
}
-
-void CO_CANmodule_disable(CO_CANmodule_t *CANmodule)
-{
+void
+CO_CANmodule_disable(CO_CANmodule_t* CANmodule) {
uint32_t i;
if (CANmodule == NULL) {
/* 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);
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 */
/* 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;
/* 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;
}
#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;
#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;
#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;
}
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
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
return err;
}
-
/*
* The same as #CO_CANsend(), but ensures that there is enough space remaining
* in the driver for more important messages.
* @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) {
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;
#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
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
#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
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;
#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);
}
/* 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) {
/* 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 */
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) */
#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};
return CO_ERROR_NO;
}
-void CO_epoll_close(CO_epoll_t *ep) {
+void
+CO_epoll_close(CO_epoll_t* ep) {
if (ep == NULL) {
return;
}
ep->timer_fd = -1;
}
-
-void CO_epoll_wait(CO_epoll_t *ep) {
+void
+CO_epoll_wait(CO_epoll_t* ep) {
if (ep == NULL) {
return;
}
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) {
}
}
-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;
}
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) {
}
}
-
/* 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));
}
#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;
}
#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) {
}
}
-
/* 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;
}
}
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);
}
/* 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;
}
}
-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};
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;
}
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;
}
}
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;
}
}
log_printf(LOG_INFO, DBG_COMMAND_TCP_INFO, commandInterface);
- }
- else {
+ } else {
epGtw->commandInterface = CO_COMMAND_IF_DISABLED;
}
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;
}
}
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);
}
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)");
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);
}
}
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)");
}
} /* 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;
}
}
#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);
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;
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) {
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
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;
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) {
/* 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);
* 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;
}
CANerrorhandler->CANerrorStatus = 0;
}
-
-void CO_CANerror_disable(
- CO_CANinterfaceErrorhandler_t *CANerrorhandler)
-{
+void
+CO_CANerror_disable(CO_CANinterfaceErrorhandler_t* CANerrorhandler) {
if (CANerrorhandler == NULL) {
return;
}
CANerrorhandler->fd = -1;
}
-
-void CO_CANerror_rxMsg(
- CO_CANinterfaceErrorhandler_t *CANerrorhandler)
-{
+void
+CO_CANerror_rxMsg(CO_CANinterfaceErrorhandler_t* CANerrorhandler) {
if (CANerrorhandler == NULL) {
return;
}
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) {
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;
}
/* 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 */
#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
/* 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
#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;
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 */
/* 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);
#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 <CAN device name> [options]\n", progName);
-printf(
-"\n"
-"Options:\n"
-" -i <Node ID> CANopen Node-id (1..127) or 0xFF (LSS unconfigured).\n");
+static void
+printUsage(char* progName) {
+ printf("Usage: %s <CAN device name> [options]\n", progName);
+ printf("\n"
+ "Options:\n"
+ " -i <Node ID> CANopen Node-id (1..127) or 0xFF (LSS unconfigured).\n");
#ifndef CO_SINGLE_THREAD
-printf(
-" -p <RT priority> 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 <RT priority> 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 <storage path> Path and filename prefix for data storage files.\n"
-" By default files are stored in current dictionary.\n");
+ printf(" -s <storage path> 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 <interface> 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-<file path>\" - Local socket interface on file\n"
-" path, for example \"local-/tmp/CO_command_socket\".\n"
-" 3. \"tcp-<port>\" - Tcp socket interface on specified \n"
-" port, for example \"tcp-60000\".\n"
-" Note that this option may affect security of the CAN.\n"
-" -T <timeout_time> 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 <interface> 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-<file path>\" - Local socket interface on file\n"
+ " path, for example \"local-/tmp/CO_command_socket\".\n"
+ " 3. \"tcp-<port>\" - Tcp socket interface on specified \n"
+ " port, for example \"tcp-60000\".\n"
+ " Note that this option may affect security of the CAN.\n"
+ " -T <timeout_time> 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
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;
/* 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': {
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};
#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);
}
#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);
}
/* 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);
}
/* 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;
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;
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;
#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);
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
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;
#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;
continue;
}
-
/* start CAN */
CO_CANsetNormalMode(CO->CANmodule);
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);
/* 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);
}
}
}
} /* while(reset != CO_RESET_APP */
-
-/* program exit ***************************************************************/
+ /* program exit ***************************************************************/
/* join threads */
CO_endProgram = 1;
#ifndef CO_SINGLE_THREAD
#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);
}
/*******************************************************************************
* 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);
#if (CO_CONFIG_TRACE) & CO_CONFIG_TRACE_ENABLE
/* Monitor variables with trace objects */
CO_time_process(&CO_time);
- for(i=0; i<OD_traceEnable && i<co->CNT_TRACE; i++) {
+ for (i = 0; i < OD_traceEnable && i < co->CNT_TRACE; i++) {
CO_trace_process(CO->trace[i], *CO_time.epochTimeOffsetMs);
}
#endif
/* Execute optional external application code */
app_programRt(CO, epRT.timeDifference_us);
#endif
-
}
return NULL;
#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");
/* 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);
/* 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;
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;
}
}
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 */
}
/* 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);
/* 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);
}
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;
}
/* 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);
memcpy(entry->addr, buf, entry->len);
entry->crc = crc1;
writeFileAccess = "r+";
- }
- else {
+ } else {
dataCorrupt = true;
ret = CO_ERROR_DATA_CORRUPT;
}
/* 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 */
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 */
/* 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);
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;
}
}
/* colors and stream for printing status */
char *greenC, *redC, *resetC;
-FILE *errStream;
-
-
-static void printUsage(char *progName) {
-fprintf(errStream,
-"Usage: %s [options] ['<command string>' ['<command string>'] ...]\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 <input file> Path to the input file.\n"
-" -s <socket path> Path to the unix socket. If not specified, path is obtained\n"
-" from environmental variable, configured with:\n"
-" 'export cocomm_socket=<socket path>'. If latter is not\n"
-" specified, default value is used: '/tmp/CO_command_socket'.\n"
-" -t <host> Connect via tcp to remote <host>. Set also with\n"
-" 'export cocomm_host=<host>'. Unix socket is used by default.\n"
-" -p <port> Tcp port to connect to when using -t. Set also with\n"
-" 'export cocomm_port=<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 <can device> If specified, then candump of specified CAN device will be\n"
-" printed after the command response. Set also with\n"
-" 'export cocomm_candump=<can device>'. Not used by default.\n"
-" -n <count> Print <count> of candump messages, then exit. Set also with\n"
-" 'export cocomm_candump_count=<count>'. Default is 10.\n"
-" -T <msec> Exit candump after <msec> without reception. Set also with\n"
-" 'export cocomm_candump_timeout=<msec>'. 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] ['<command string>' ['<command string>'] ...]\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 <input file> Path to the input file.\n"
+ " -s <socket path> Path to the unix socket. If not specified, path is obtained\n"
+ " from environmental variable, configured with:\n"
+ " 'export cocomm_socket=<socket path>'. If latter is not\n"
+ " specified, default value is used: '/tmp/CO_command_socket'.\n"
+ " -t <host> Connect via tcp to remote <host>. Set also with\n"
+ " 'export cocomm_host=<host>'. Unix socket is used by default.\n"
+ " -p <port> Tcp port to connect to when using -t. Set also with\n"
+ " 'export cocomm_port=<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 <can device> If specified, then candump of specified CAN device will be\n"
+ " printed after the command response. Set also with\n"
+ " 'export cocomm_candump=<can device>'. Not used by default.\n"
+ " -n <count> Print <count> of candump messages, then exit. Set also with\n"
+ " 'export cocomm_candump_count=<count>'. Default is 10.\n"
+ " -T <msec> Exit candump after <msec> without reception. Set also with\n"
+ " 'export cocomm_candump_timeout=<msec>'. 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);
}
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;
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) {
}
}
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);
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);
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;
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;
}
/* 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;
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);
}
}
}
/* Create and connect client socket */
- if(addrFamily == AF_INET) {
+ if (addrFamily == AF_INET) {
struct addrinfo hints, *res, *rp;
int errcode;
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);
}
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);
perror(NULL);
exit(EXIT_FAILURE);
}
- }
- else {
+ } else {
candumpCount = 0;
}
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 */
}
/* 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;
}
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. */
/* 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 */
}
*pbuf = 0;
- printf ("%03X#%s\n", canFrame.can_id, buf);
+ printf("%03X#%s\n", canFrame.can_id, buf);
}
close(fd_candump);
}
projects / 0xmirror / CANopenLinux.git / commitdiff