--- /dev/null
+/canopend
+/cocomm/cocomm
+*.o
+*.persist
+*.persist.old
+
+#eclipse
+.cproject
+.project
+.settings/
+
+#kdevelop
+*.kdev4
--- /dev/null
+[submodule "CANopenNode"]
+ path = CANopenNode
+ url = https://github.com/CANopenNode/CANopenNode.git
--- /dev/null
+Subproject commit 76b43c88ef6d5490cb2f1518e10646e8dcb45c76
--- /dev/null
+/*
+ * Linux socketCAN interface for CANopenNode.
+ *
+ * @file CO_driver.c
+ * @ingroup CO_driver
+ * @author Janez Paternoster, Martin Wagner
+ * @copyright 2004 - 2015 Janez Paternoster, 2017 - 2020 Neuberger Gebaeudeautomation GmbH
+ *
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <errno.h>
+#include <syslog.h>
+#include <linux/can/raw.h>
+#include <linux/can/error.h>
+#include <linux/net_tstamp.h>
+#include <sys/socket.h>
+#include <asm/socket.h>
+#include <sys/eventfd.h>
+#include <time.h>
+
+#include "301/CO_driver.h"
+#include "CO_error.h"
+
+#ifndef CO_SINGLE_THREAD
+pthread_mutex_t CO_EMCY_mutex = PTHREAD_MUTEX_INITIALIZER;
+pthread_mutex_t CO_OD_mutex = PTHREAD_MUTEX_INITIALIZER;
+#endif
+
+#if CO_DRIVER_MULTI_INTERFACE == 0
+static CO_ReturnError_t CO_CANmodule_addInterface(CO_CANmodule_t *CANmodule,
+ int can_ifindex);
+#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)
+{
+ /* entry changed, remove old one */
+ if (identCurrent<CO_CAN_MSG_SFF_MAX_COB_ID && identNew!=identCurrent) {
+ lookup[identCurrent] = CO_INVALID_COB_ID;
+ }
+
+ /* check if this COB ID is part of the table */
+ if (identNew > CO_CAN_MSG_SFF_MAX_COB_ID) {
+ return;
+ }
+
+ /* Special case COB ID "0" -> valid value in *xArray[0] (CO_*CAN_NMT),
+ * "entry unconfigured" for all others */
+ if (identNew == 0) {
+ if (index == 0) {
+ lookup[0] = 0;
+ }
+ }
+ else {
+ lookup[identNew] = index;
+ }
+}
+
+
+/******************************************************************************/
+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;
+ }
+
+ return lookup[ident];
+}
+
+#endif /* CO_DRIVER_MULTI_INTERFACE */
+
+
+/** Disable socketCAN rx ******************************************************/
+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);
+ 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)
+{
+ 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)) {
+
+ rxFiltersCpy[count] = CANmodule->rxFilter[i];
+
+ count ++;
+ }
+ }
+
+ if (count == 0) {
+ /* No filter is set, disable RX */
+ return disableRx(CANmodule);
+ }
+
+ retval = CO_ERROR_NO;
+ for (i = 0; i < CANmodule->CANinterfaceCount; i ++) {
+ int ret = setsockopt(CANmodule->CANinterfaces[i].fd, SOL_CAN_RAW, CAN_RAW_FILTER,
+ rxFiltersCpy, sizeof(struct can_filter) * count);
+ if(ret < 0){
+ log_printf(LOG_ERR, CAN_FILTER_FAILED,
+ CANmodule->CANinterfaces[i].ifName);
+ log_printf(LOG_DEBUG, DBG_ERRNO, "setsockopt()");
+ retval = CO_ERROR_SYSCALL;
+ }
+ }
+
+ return retval;
+}
+
+
+/******************************************************************************/
+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)
+{
+ CO_ReturnError_t ret;
+
+ if(CANmodule != NULL) {
+ CANmodule->CANnormal = false;
+ ret = setRxFilters(CANmodule);
+ if (ret == CO_ERROR_NO) {
+ /* Put CAN module in normal mode */
+ CANmodule->CANnormal = true;
+ }
+ }
+}
+
+
+/******************************************************************************/
+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) {
+ return CO_ERROR_ILLEGAL_ARGUMENT;
+ }
+
+ CO_CANptrSocketCan_t *CANptrReal = (CO_CANptrSocketCan_t *)CANptr;
+
+ /* Configure object variables */
+ CANmodule->epoll_fd = CANptrReal->epoll_fd;
+ CANmodule->CANinterfaces = NULL;
+ CANmodule->CANinterfaceCount = 0;
+ CANmodule->rxArray = rxArray;
+ CANmodule->rxSize = rxSize;
+ CANmodule->txArray = txArray;
+ CANmodule->txSize = txSize;
+ CANmodule->CANerrorStatus = 0;
+ CANmodule->CANnormal = false;
+ CANmodule->CANtxCount = 0;
+
+#if CO_DRIVER_MULTI_INTERFACE > 0
+ for (i = 0; i < CO_CAN_MSG_SFF_MAX_COB_ID; i++) {
+ CANmodule->rxIdentToIndex[i] = CO_INVALID_COB_ID;
+ CANmodule->txIdentToIndex[i] = CO_INVALID_COB_ID;
+ }
+#endif
+
+ /* 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){
+ log_printf(LOG_DEBUG, DBG_ERRNO, "malloc()");
+ return CO_ERROR_OUT_OF_MEMORY;
+ }
+
+ for(i=0U; i<rxSize; i++){
+ rxArray[i].ident = 0U;
+ rxArray[i].mask = 0xFFFFFFFFU;
+ rxArray[i].object = NULL;
+ rxArray[i].CANrx_callback = NULL;
+ rxArray[i].can_ifindex = 0;
+ rxArray[i].timestamp.tv_sec = 0;
+ rxArray[i].timestamp.tv_nsec = 0;
+ }
+
+#if CO_DRIVER_MULTI_INTERFACE == 0
+ /* add one interface */
+ ret = CO_CANmodule_addInterface(CANmodule,
+ CANptrReal->can_ifindex);
+ if (ret != CO_ERROR_NO) {
+ CO_CANmodule_disable(CANmodule);
+ return ret;
+ }
+#endif
+ 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)
+{
+ int32_t ret;
+ int32_t tmp;
+ int32_t bytes;
+ char *ifName;
+ socklen_t sLen;
+ CO_CANinterface_t *interface;
+ struct sockaddr_can sockAddr;
+ struct epoll_event ev;
+#if CO_DRIVER_ERROR_REPORTING > 0
+ can_err_mask_t err_mask;
+#endif
+
+ if (CANmodule->CANnormal != false) {
+ /* can't change config now! */
+ return CO_ERROR_INVALID_STATE;
+ }
+
+ /* Add interface to interface list */
+ CANmodule->CANinterfaceCount ++;
+ CANmodule->CANinterfaces = realloc(CANmodule->CANinterfaces,
+ ((CANmodule->CANinterfaceCount) * sizeof(*CANmodule->CANinterfaces)));
+ if (CANmodule->CANinterfaces == NULL) {
+ log_printf(LOG_DEBUG, DBG_ERRNO, "malloc()");
+ return CO_ERROR_OUT_OF_MEMORY;
+ }
+ interface = &CANmodule->CANinterfaces[CANmodule->CANinterfaceCount - 1];
+
+ interface->can_ifindex = can_ifindex;
+ ifName = if_indextoname(can_ifindex, interface->ifName);
+ if (ifName == NULL) {
+ log_printf(LOG_DEBUG, DBG_ERRNO, "if_indextoname()");
+ return CO_ERROR_ILLEGAL_ARGUMENT;
+ }
+
+ /* Create socket */
+ interface->fd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
+ 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){
+ 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);
+ 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);
+
+ /* 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);
+ if (sLen == sizeof(bytes)) {
+ log_printf(LOG_INFO, CAN_SOCKET_BUF_SIZE, interface->ifName,
+ bytes / 446, bytes);
+ }
+
+ /* bind socket */
+ memset(&sockAddr, 0, sizeof(sockAddr));
+ sockAddr.can_family = AF_CAN;
+ sockAddr.can_ifindex = can_ifindex;
+ ret = bind(interface->fd, (struct sockaddr*)&sockAddr, sizeof(sockAddr));
+ 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 */
+#ifdef DEBUG
+ 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){
+ log_printf(LOG_ERR, CAN_ERROR_FILTER_FAILED, interface->ifName);
+ log_printf(LOG_DEBUG, DBG_ERRNO, "setsockopt(can err)");
+ return CO_ERROR_SYSCALL;
+ }
+#endif /* CO_DRIVER_ERROR_REPORTING */
+
+ /* Add socket to epoll */
+ ev.events = EPOLLIN;
+ ev.data.fd = interface->fd;
+ ret = epoll_ctl(CANmodule->epoll_fd, EPOLL_CTL_ADD, ev.data.fd, &ev);
+ if(ret < 0){
+ log_printf(LOG_DEBUG, DBG_ERRNO, "epoll_ctl(can)");
+ return CO_ERROR_SYSCALL;
+ }
+
+ /* rx is started by calling #CO_CANsetNormalMode() */
+ ret = disableRx(CANmodule);
+
+ return ret;
+}
+
+
+/******************************************************************************/
+void CO_CANmodule_disable(CO_CANmodule_t *CANmodule)
+{
+ uint32_t i;
+
+ if (CANmodule == NULL) {
+ return;
+ }
+
+ CANmodule->CANnormal = false;
+
+ /* clear interfaces */
+ for (i = 0; i < CANmodule->CANinterfaceCount; i++) {
+ CO_CANinterface_t *interface = &CANmodule->CANinterfaces[i];
+
+#if CO_DRIVER_ERROR_REPORTING > 0
+ CO_CANerror_disable(&interface->errorhandler);
+#endif
+
+ epoll_ctl(CANmodule->epoll_fd, EPOLL_CTL_DEL, interface->fd, NULL);
+ close(interface->fd);
+ interface->fd = -1;
+ }
+ CANmodule->CANinterfaceCount = 0;
+ if (CANmodule->CANinterfaces != NULL) {
+ free(CANmodule->CANinterfaces);
+ }
+ CANmodule->CANinterfaces = NULL;
+
+ if (CANmodule->rxFilter != NULL) {
+ free(CANmodule->rxFilter);
+ }
+ 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 ret = CO_ERROR_NO;
+
+ 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);
+#endif
+
+ /* Configure object variables */
+ buffer->object = object;
+ buffer->CANrx_callback = CANrx_callback;
+ buffer->can_ifindex = 0;
+ buffer->timestamp.tv_nsec = 0;
+ buffer->timestamp.tv_sec = 0;
+
+ /* CAN identifier and CAN mask, bit aligned with CAN module */
+ buffer->ident = ident & CAN_SFF_MASK;
+ 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){
+ ret = setRxFilters(CANmodule);
+ }
+ }
+ else {
+ log_printf(LOG_DEBUG, DBG_CAN_RX_PARAM_FAILED, "illegal argument");
+ ret = CO_ERROR_ILLEGAL_ARGUMENT;
+ }
+
+ return ret;
+}
+
+#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;
+
+ 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;
+ }
+ buffer = &CANmodule->rxArray[index];
+
+ /* return values */
+ if (can_ifindexRx != NULL) {
+ *can_ifindexRx = buffer->can_ifindex;
+ }
+ if (timestamp != NULL) {
+ *timestamp = buffer->timestamp;
+ }
+ if (buffer->can_ifindex != 0) {
+ 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;
+
+ 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);
+#endif
+
+ buffer->can_ifindex = 0;
+
+ /* CAN identifier and rtr */
+ buffer->ident = ident & CAN_SFF_MASK;
+ if(rtr){
+ buffer->ident |= CAN_RTR_FLAG;
+ }
+ buffer->DLC = noOfBytes;
+ buffer->bufferFull = false;
+ buffer->syncFlag = syncFlag;
+ }
+
+ return buffer;
+}
+
+#if CO_DRIVER_MULTI_INTERFACE > 0
+
+/******************************************************************************/
+CO_ReturnError_t CO_CANtxBuffer_setInterface(
+ CO_CANmodule_t *CANmodule,
+ uint16_t ident,
+ int can_ifindexTx)
+{
+ if (CANmodule != NULL) {
+ uint32_t index;
+
+ index = CO_CANgetIndexFromIdent(CANmodule->txIdentToIndex, ident);
+ if ((index == CO_INVALID_COB_ID) || (index > CANmodule->txSize)) {
+ return CO_ERROR_ILLEGAL_ARGUMENT;
+ }
+ CANmodule->txArray[index].can_ifindex = can_ifindexTx;
+
+ return CO_ERROR_NO;
+ }
+ return CO_ERROR_ILLEGAL_ARGUMENT;
+}
+
+#endif /* CO_DRIVER_MULTI_INTERFACE */
+#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)
+{
+ 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) {
+ return CO_ERROR_ILLEGAL_ARGUMENT;
+ }
+
+#if CO_DRIVER_ERROR_REPORTING > 0
+ ifState = CO_CANerror_txMsg(&interface->errorhandler);
+ switch (ifState) {
+ case CO_INTERFACE_ACTIVE:
+ /* continue */
+ break;
+ case CO_INTERFACE_LISTEN_ONLY:
+ /* silently drop message */
+ return CO_ERROR_NO;
+ default:
+ return CO_ERROR_INVALID_STATE;
+ }
+#endif
+
+ do {
+ errno = 0;
+ n = send(interface->fd, buffer, CAN_MTU, MSG_DONTWAIT);
+ if (errno == EINTR) {
+ /* try again */
+ continue;
+ }
+ else if (errno == EAGAIN) {
+ /* socket queue full */
+ break;
+ }
+ 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) {
+ break;
+ }
+ } while (errno != 0);
+
+ if(n != CAN_MTU){
+#if CO_DRIVER_ERROR_REPORTING > 0
+ interface->errorhandler.CANerrorStatus |= CO_CAN_ERRTX_OVERFLOW;
+#endif
+ log_printf(LOG_ERR, DBG_CAN_TX_FAILED, buffer->ident, interface->ifName);
+ log_printf(LOG_DEBUG, DBG_ERRNO, "send()");
+ err = CO_ERROR_TX_OVERFLOW;
+ }
+
+ return err;
+}
+
+
+/*
+ * The same as #CO_CANsend(), but ensures that there is enough space remaining
+ * in the driver for more important messages.
+ *
+ * The default threshold is 50%, or at least 1 message buffer. If sending
+ * would violate those limits, #CO_ERROR_TX_OVERFLOW is returned and the
+ * message will not be sent.
+ *
+ * (It is not in header, because usage of CO_CANCheckSend() is unclear.)
+ *
+ * @param CANmodule This object.
+ * @param buffer Pointer to transmit buffer, returned by CO_CANtxBufferInit().
+ * Data bytes must be written in buffer before function call.
+ *
+ * @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_CANsend(CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer)
+{
+ CO_ReturnError_t err;
+ err = CO_CANCheckSend(CANmodule, buffer);
+ if (err == CO_ERROR_TX_BUSY) {
+ /* send doesn't have "busy" */
+ log_printf(LOG_ERR, DBG_CAN_TX_FAILED, buffer->ident, "CANx");
+ log_printf(LOG_DEBUG, DBG_ERRNO, "send()");
+ err = CO_ERROR_TX_OVERFLOW;
+ }
+ return err;
+}
+
+
+/******************************************************************************/
+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];
+
+ if ((buffer->can_ifindex == 0) ||
+ buffer->can_ifindex == interface->can_ifindex) {
+
+ CO_ReturnError_t tmp;
+
+ /* match, use this one */
+ tmp = CO_CANCheckSendInterface(CANmodule, buffer, interface);
+ if (tmp) {
+ /* only last error is returned to callee */
+ err = tmp;
+ }
+ }
+ }
+
+ return err;
+}
+
+#warning CO_CANsend() is outdated for CO_DRIVER_MULTI_INTERFACE > 0
+
+#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 err = CO_ERROR_NO;
+
+ if (CANmodule==NULL || buffer==NULL || CANmodule->CANinterfaceCount==0) {
+ return CO_ERROR_ILLEGAL_ARGUMENT;
+ }
+
+ CO_CANinterface_t *interface = &CANmodule->CANinterfaces[0];
+ if (interface == NULL || interface->fd < 0) {
+ return CO_ERROR_ILLEGAL_ARGUMENT;
+ }
+
+ /* Verify overflow */
+ if(buffer->bufferFull){
+#if CO_DRIVER_ERROR_REPORTING > 0
+ interface->errorhandler.CANerrorStatus |= CO_CAN_ERRTX_OVERFLOW;
+#endif
+ log_printf(LOG_ERR, DBG_CAN_TX_FAILED, buffer->ident, interface->ifName);
+ err = CO_ERROR_TX_OVERFLOW;
+ }
+
+ errno = 0;
+ ssize_t n = send(interface->fd, buffer, CAN_MTU, MSG_DONTWAIT);
+ if (errno == 0 && n == CAN_MTU) {
+ /* success */
+ if (buffer->bufferFull) {
+ buffer->bufferFull = false;
+ CANmodule->CANtxCount--;
+ }
+ }
+ 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 {
+ /* Unknown error */
+ log_printf(LOG_DEBUG, DBG_ERRNO, "send()");
+#if CO_DRIVER_ERROR_REPORTING > 0
+ interface->errorhandler.CANerrorStatus |= CO_CAN_ERRTX_OVERFLOW;
+#endif
+ err = CO_ERROR_SYSCALL;
+ }
+
+ return err;
+}
+
+#endif /* CO_DRIVER_MULTI_INTERFACE == 0 */
+
+
+/******************************************************************************/
+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;
+
+#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;
+#endif
+
+#if CO_DRIVER_MULTI_INTERFACE == 0
+ /* recall CO_CANsend(), if message was unsent before */
+ if (CANmodule->CANtxCount > 0) {
+ bool_t found = false;
+
+ for (uint16_t i = 0; i < CANmodule->txSize; i++) {
+ CO_CANtx_t *buffer = &CANmodule->txArray[i];
+
+ if (buffer->bufferFull) {
+ buffer->bufferFull = false;
+ CANmodule->CANtxCount--;
+ CO_CANsend(CANmodule, buffer);
+ found = true;
+ break;
+ }
+ }
+
+ if (!found) {
+ CANmodule->CANtxCount = 0;
+ }
+ }
+#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 */
+{
+ int32_t n;
+ uint32_t dropped;
+ /* 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;
+
+ iov.iov_base = msg;
+ iov.iov_len = sizeof(*msg);
+
+ msghdr.msg_name = NULL;
+ msghdr.msg_namelen = 0;
+ msghdr.msg_iov = &iov;
+ msghdr.msg_iovlen = 1;
+ msghdr.msg_control = &ctrlmsg;
+ msghdr.msg_controllen = sizeof(ctrlmsg);
+ msghdr.msg_flags = 0;
+
+ n = recvmsg(interface->fd, &msghdr, 0);
+ if (n != CAN_MTU) {
+#if CO_DRIVER_ERROR_REPORTING > 0
+ interface->errorhandler.CANerrorStatus |= CO_CAN_ERRRX_OVERFLOW;
+#endif
+ log_printf(LOG_DEBUG, DBG_CAN_RX_FAILED, interface->ifName);
+ log_printf(LOG_DEBUG, DBG_ERRNO, "recvmsg()");
+ return CO_ERROR_SYSCALL;
+ }
+
+ /* check for rx queue overflow, get rx time */
+ 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) {
+ 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);
+ }
+ CANmodule->rxDropCount = dropped;
+ //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 */
+{
+ int32_t retval;
+ 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. */
+ bool_t msgMatched = false;
+
+ /* CANopenNode can message is binary compatible to the socketCAN one, except
+ * for extension flags */
+ msg->can_id &= CAN_EFF_MASK;
+ 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){
+ msgMatched = true;
+ break;
+ }
+ rcvMsgObj++;
+ }
+ if(msgMatched) {
+ /* Call specific function, which will process the message */
+ 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 {
+ retval = -1;
+ }
+
+ return retval;
+}
+
+
+/******************************************************************************/
+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];
+
+ if (ev->data.fd == interface->fd) {
+ if ((ev->events & (EPOLLERR | EPOLLHUP)) != 0) {
+ struct can_frame msg;
+ /* 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) {
+ struct can_frame msg;
+ struct timespec timestamp;
+
+ /* get message */
+ CO_ReturnError_t err = CO_CANread(CANmodule, interface,
+ &msg, ×tamp);
+
+ 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 {
+ /* data msg */
+#if CO_DRIVER_ERROR_REPORTING > 0
+ /* clear listenOnly and noackCounter if necessary */
+ CO_CANerror_rxMsg(&interface->errorhandler);
+#endif
+ int32_t idx = CO_CANrxMsg(CANmodule, &msg, buffer);
+ if (idx > -1) {
+ /* Store message info */
+ CANmodule->rxArray[idx].timestamp = timestamp;
+ 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);
+ }
+ return true;
+ } /* if (ev->data.fd == interface->fd) */
+ }
+ return false;
+}
--- /dev/null
+/**
+ * Linux socketCAN specific definitions for CANopenNode.
+ *
+ * @file CO_driver_target.h
+ * @ingroup CO_socketCAN_driver_target
+ * @author Janez Paternoster
+ * @author Martin Wagner
+ * @copyright 2004 - 2020 Janez Paternoster
+ * @copyright 2018 - 2020 Neuberger Gebaeudeautomation GmbH
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+
+#ifndef CO_DRIVER_TARGET_H
+#define CO_DRIVER_TARGET_H
+
+/* This file contains device and application specific definitions.
+ * It is included from CO_driver.h, which contains documentation
+ * for common definitions below. */
+
+#include <stddef.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <endian.h>
+#ifndef CO_SINGLE_THREAD
+#include <pthread.h>
+#endif
+#include <linux/can.h>
+#include <net/if.h>
+#include <sys/epoll.h>
+
+#ifdef CO_DRIVER_CUSTOM
+#include "CO_driver_custom.h"
+#endif
+#include "CO_error.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/* Stack configuration override default values.
+ * For more information see file CO_config.h. */
+#ifdef CO_SINGLE_THREAD
+#define CO_CONFIG_GLOBAL_FLAG_CALLBACK_PRE 0
+#else
+#define CO_CONFIG_GLOBAL_FLAG_CALLBACK_PRE CO_CONFIG_FLAG_CALLBACK_PRE
+#endif
+#define CO_CONFIG_GLOBAL_FLAG_TIMERNEXT CO_CONFIG_FLAG_TIMERNEXT
+
+#ifndef CO_CONFIG_NMT
+#define CO_CONFIG_NMT (CO_CONFIG_NMT_CALLBACK_CHANGE | \
+ CO_CONFIG_NMT_MASTER | \
+ CO_CONFIG_GLOBAL_FLAG_CALLBACK_PRE | \
+ CO_CONFIG_GLOBAL_FLAG_TIMERNEXT)
+#endif
+
+#ifndef CO_CONFIG_HB_CONS
+#define CO_CONFIG_HB_CONS (CO_CONFIG_HB_CONS_ENABLE | \
+ CO_CONFIG_HB_CONS_CALLBACK_CHANGE | \
+ CO_CONFIG_GLOBAL_FLAG_CALLBACK_PRE | \
+ CO_CONFIG_GLOBAL_FLAG_TIMERNEXT | \
+ CO_CONFIG_GLOBAL_FLAG_OD_DYNAMIC)
+#endif
+
+#ifndef CO_CONFIG_EM
+#define CO_CONFIG_EM (CO_CONFIG_EM_PRODUCER | \
+ CO_CONFIG_EM_PROD_CONFIGURABLE | \
+ CO_CONFIG_EM_PROD_INHIBIT | \
+ CO_CONFIG_EM_HISTORY | \
+ CO_CONFIG_EM_STATUS_BITS | \
+ CO_CONFIG_EM_CONSUMER | \
+ CO_CONFIG_GLOBAL_FLAG_CALLBACK_PRE | \
+ CO_CONFIG_GLOBAL_FLAG_TIMERNEXT)
+#endif
+
+#ifndef CO_CONFIG_SDO_SRV
+#define CO_CONFIG_SDO_SRV (CO_CONFIG_SDO_SRV_SEGMENTED | \
+ CO_CONFIG_SDO_SRV_BLOCK | \
+ CO_CONFIG_GLOBAL_FLAG_CALLBACK_PRE | \
+ CO_CONFIG_GLOBAL_FLAG_TIMERNEXT | \
+ CO_CONFIG_GLOBAL_FLAG_OD_DYNAMIC)
+#endif
+
+#ifndef CO_CONFIG_SDO_SRV_BUFFER_SIZE
+#define CO_CONFIG_SDO_SRV_BUFFER_SIZE 900
+#endif
+
+#ifndef CO_CONFIG_SDO_CLI
+#define CO_CONFIG_SDO_CLI (CO_CONFIG_SDO_CLI_ENABLE | \
+ CO_CONFIG_SDO_CLI_SEGMENTED | \
+ CO_CONFIG_SDO_CLI_BLOCK | \
+ CO_CONFIG_SDO_CLI_LOCAL | \
+ CO_CONFIG_GLOBAL_FLAG_CALLBACK_PRE | \
+ CO_CONFIG_GLOBAL_FLAG_TIMERNEXT | \
+ CO_CONFIG_GLOBAL_FLAG_OD_DYNAMIC)
+#endif
+
+#ifndef CO_CONFIG_TIME
+#define CO_CONFIG_TIME (CO_CONFIG_TIME_ENABLE | \
+ CO_CONFIG_TIME_PRODUCER | \
+ CO_CONFIG_GLOBAL_FLAG_CALLBACK_PRE | \
+ CO_CONFIG_GLOBAL_FLAG_OD_DYNAMIC)
+#endif
+
+#ifndef CO_CONFIG_LSS
+#define CO_CONFIG_LSS (CO_CONFIG_LSS_SLAVE | \
+ CO_CONFIG_LSS_SLAVE_FASTSCAN_DIRECT_RESPOND | \
+ CO_CONFIG_LSS_MASTER | \
+ CO_CONFIG_GLOBAL_FLAG_CALLBACK_PRE)
+#endif
+
+#ifndef CO_CONFIG_GTW
+#define CO_CONFIG_GTW (CO_CONFIG_GTW_ASCII | \
+ CO_CONFIG_GTW_ASCII_SDO | \
+ CO_CONFIG_GTW_ASCII_NMT | \
+ CO_CONFIG_GTW_ASCII_LSS | \
+ CO_CONFIG_GTW_ASCII_LOG | \
+ CO_CONFIG_GTW_ASCII_ERROR_DESC | \
+ CO_CONFIG_GTW_ASCII_PRINT_HELP | \
+ CO_CONFIG_GTW_ASCII_PRINT_LEDS)
+#define CO_CONFIG_GTW_BLOCK_DL_LOOP 3
+#define CO_CONFIG_GTWA_COMM_BUF_SIZE 2000
+#define CO_CONFIG_GTWA_LOG_BUF_SIZE 10000
+#endif
+
+#ifndef CO_CONFIG_CRC16
+#define CO_CONFIG_CRC16 (CO_CONFIG_CRC16_ENABLE)
+#endif
+
+#ifndef CO_CONFIG_FIFO
+#define CO_CONFIG_FIFO (CO_CONFIG_FIFO_ENABLE | \
+ CO_CONFIG_FIFO_ALT_READ | \
+ CO_CONFIG_FIFO_CRC16_CCITT | \
+ CO_CONFIG_FIFO_ASCII_COMMANDS | \
+ CO_CONFIG_FIFO_ASCII_DATATYPES)
+#endif
+
+
+/* Print debug info from some internal parts of the stack */
+#if (CO_CONFIG_DEBUG) & CO_CONFIG_DEBUG_COMMON
+#include <stdio.h>
+#include <syslog.h>
+#define CO_DEBUG_COMMON(msg) log_printf(LOG_DEBUG, DBG_CO_DEBUG, msg);
+#endif
+
+
+/**
+ * @defgroup CO_socketCAN_driver_target CO_driver_target.h
+ * @ingroup CO_socketCAN
+ * @{
+ *
+ * Linux socketCAN specific @ref CO_driver definitions for CANopenNode.
+ */
+
+/**
+ * Multi interface support
+ *
+ * Enable this to use interface combining at driver level. This
+ * adds functions to broadcast/selective transmit messages on the
+ * given interfaces as well as combining all received message into
+ * one queue.
+ *
+ * If CO_DRIVER_MULTI_INTERFACE is set to 0, then CO_CANmodule_init()
+ * adds single socketCAN interface specified by CANptr argument. In case of
+ * failure, CO_CANmodule_init() returns CO_ERROR_SYSCALL.
+ *
+ * If CO_DRIVER_MULTI_INTERFACE is set to 1, then CO_CANmodule_init()
+ * ignores CANptr argument. Interfaces must be added by
+ * CO_CANmodule_addInterface() function after CO_CANmodule_init().
+ *
+ * Macro is set to 0 (disabled) by default. It can be overridden.
+ *
+ * This is not intended to realize interface redundancy!!!
+ */
+#ifndef CO_DRIVER_MULTI_INTERFACE
+#define CO_DRIVER_MULTI_INTERFACE 0
+#endif
+
+/**
+ * CAN bus error reporting
+ *
+ * CO_DRIVER_ERROR_REPORTING enabled adds support for socketCAN error detection
+ * and handling functions inside the driver. This is needed when you have
+ * CANopen with "0" connected nodes as a use case, as this is normally
+ * forbidden in CAN.
+ *
+ * Macro is set to 1 (enabled) by default. It can be overridden.
+ *
+ * you need to enable error reporting in your kernel driver using:
+ * @code{.sh}
+ * ip link set canX type can berr-reporting on
+ * @endcode
+ * Of course, the kernel driver for your hardware needs this functionality to be
+ * implemented...
+ */
+#ifndef CO_DRIVER_ERROR_REPORTING
+#define CO_DRIVER_ERROR_REPORTING 1
+#endif
+
+/* skip this section for Doxygen, because it is documented in CO_driver.h */
+#ifndef CO_DOXYGEN
+
+/* Basic definitions */
+#ifdef __BYTE_ORDER
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+ #define CO_LITTLE_ENDIAN
+ #define CO_SWAP_16(x) x
+ #define CO_SWAP_32(x) x
+ #define CO_SWAP_64(x) x
+#else
+ #define CO_BIG_ENDIAN
+ #include <byteswap.h>
+ #define CO_SWAP_16(x) bswap_16(x)
+ #define CO_SWAP_32(x) bswap_32(x)
+ #define CO_SWAP_64(x) bswap_64(x)
+#endif
+#endif
+/* NULL is defined in stddef.h */
+/* true and false are defined in stdbool.h */
+/* int8_t to uint64_t are defined in stdint.h */
+typedef uint_fast8_t bool_t;
+typedef float float32_t;
+typedef double float64_t;
+
+
+/* CAN receive message structure as aligned in socketCAN. */
+typedef struct {
+ uint32_t ident;
+ uint8_t DLC;
+ uint8_t padding[3];
+ uint8_t data[8];
+} CO_CANrxMsg_t;
+
+/* Access to received CAN message */
+static inline uint16_t CO_CANrxMsg_readIdent(void *rxMsg) {
+ CO_CANrxMsg_t *rxMsgCasted = (CO_CANrxMsg_t *)rxMsg;
+ return (uint16_t) (rxMsgCasted->ident & CAN_SFF_MASK);
+}
+static inline uint8_t CO_CANrxMsg_readDLC(void *rxMsg) {
+ CO_CANrxMsg_t *rxMsgCasted = (CO_CANrxMsg_t *)rxMsg;
+ return (uint8_t) (rxMsgCasted->DLC);
+}
+static inline uint8_t *CO_CANrxMsg_readData(void *rxMsg) {
+ CO_CANrxMsg_t *rxMsgCasted = (CO_CANrxMsg_t *)rxMsg;
+ return (uint8_t *) (rxMsgCasted->data);
+}
+
+
+/* Received message object */
+typedef struct {
+ uint32_t ident;
+ uint32_t mask;
+ void *object;
+ void (*CANrx_callback)(void *object, void *message);
+ int can_ifindex; /* CAN Interface index from last message */
+ struct timespec timestamp; /* time of reception of last message */
+} CO_CANrx_t;
+
+/* Transmit message object as aligned in socketCAN. */
+typedef struct {
+ uint32_t ident;
+ uint8_t DLC;
+ uint8_t padding[3]; /* ensure alignment */
+ uint8_t data[8];
+ volatile bool_t bufferFull;
+ volatile bool_t syncFlag; /* info about transmit message */
+ int can_ifindex; /* CAN Interface index to use */
+} CO_CANtx_t;
+
+
+/* Max COB ID for standard frame format */
+#define CO_CAN_MSG_SFF_MAX_COB_ID (1 << CAN_SFF_ID_BITS)
+
+/* CAN interface object (CANptr), passed to CO_CANinit() */
+typedef struct {
+ int can_ifindex; /* CAN Interface index */
+ int epoll_fd; /* File descriptor for epoll, which waits for
+ CAN receive event */
+} CO_CANptrSocketCan_t;
+
+/* socketCAN interface object */
+typedef struct {
+ int can_ifindex; /* CAN Interface index */
+ char ifName[IFNAMSIZ]; /* CAN Interface name */
+ int fd; /* socketCAN file descriptor */
+#if CO_DRIVER_ERROR_REPORTING > 0 || defined CO_DOXYGEN
+ CO_CANinterfaceErrorhandler_t errorhandler;
+#endif
+} CO_CANinterface_t;
+
+/* CAN module object */
+typedef struct {
+ /* List of can interfaces. From CO_CANmodule_init() or one per
+ * CO_CANmodule_addInterface() call */
+ CO_CANinterface_t *CANinterfaces;
+ uint32_t CANinterfaceCount; /* interface count */
+ CO_CANrx_t *rxArray;
+ uint16_t rxSize;
+ struct can_filter *rxFilter;/* socketCAN filter list, one per rx buffer */
+ uint32_t rxDropCount; /* messages dropped on rx socket queue */
+ CO_CANtx_t *txArray;
+ uint16_t txSize;
+ uint16_t CANerrorStatus;
+ volatile bool_t CANnormal;
+ volatile uint16_t CANtxCount;
+ int epoll_fd; /* File descriptor for epoll, which waits for
+ CAN receive event */
+#if CO_DRIVER_MULTI_INTERFACE > 0 || defined CO_DOXYGEN
+ /* Lookup tables Cob ID to rx/tx array index.
+ * Only feasible for SFF Messages. */
+ uint32_t rxIdentToIndex[CO_CAN_MSG_SFF_MAX_COB_ID];
+ uint32_t txIdentToIndex[CO_CAN_MSG_SFF_MAX_COB_ID];
+#endif
+} CO_CANmodule_t;
+
+
+/* Data storage: Maximum file name length including path */
+#ifndef CO_STORAGE_PATH_MAX
+#define CO_STORAGE_PATH_MAX 255
+#endif
+
+/* Data storage object for one entry */
+typedef struct {
+ void *addr;
+ size_t len;
+ uint8_t subIndexOD;
+ uint8_t attr;
+ /* Name of the file, where data block is stored */
+ char filename[CO_STORAGE_PATH_MAX];
+ /* CRC checksum of the data stored previously, for auto storage */
+ uint16_t crc;
+ /* Pointer to opened file, for auto storage */
+ FILE *fp;
+} CO_storage_entry_t;
+
+
+#ifdef CO_SINGLE_THREAD
+#define CO_LOCK_CAN_SEND(CAN_MODULE) {(void) CAN_MODULE;}
+#define CO_UNLOCK_CAN_SEND(CAN_MODULE) {(void) CAN_MODULE;}
+#define CO_LOCK_EMCY(CAN_MODULE) {(void) CAN_MODULE;}
+#define CO_UNLOCK_EMCY(CAN_MODULE) {(void) CAN_MODULE;}
+#define CO_LOCK_OD(CAN_MODULE) {(void) CAN_MODULE;}
+#define CO_UNLOCK_OD(CAN_MODULE) {(void) CAN_MODULE;}
+#define CO_MemoryBarrier()
+#else
+
+/* (un)lock critical section in CO_CANsend() - unused */
+#define CO_LOCK_CAN_SEND(CAN_MODULE)
+#define CO_UNLOCK_CAN_SEND(CAN_MODULE)
+
+/* (un)lock critical section in CO_errorReport() or CO_errorReset() */
+extern pthread_mutex_t CO_EMCY_mutex;
+static inline int CO_LOCK_EMCY(CO_CANmodule_t *CANmodule) {
+ (void)CANmodule;
+ return pthread_mutex_lock(&CO_EMCY_mutex);
+}
+static inline void CO_UNLOCK_EMCY(CO_CANmodule_t *CANmodule) {
+ (void)CANmodule;
+ (void)pthread_mutex_unlock(&CO_EMCY_mutex);
+}
+
+/* (un)lock critical section when accessing Object Dictionary */
+extern pthread_mutex_t CO_OD_mutex;
+static inline int CO_LOCK_OD(CO_CANmodule_t *CANmodule) {
+ (void)CANmodule;
+ return pthread_mutex_lock(&CO_OD_mutex);
+}
+static inline void CO_UNLOCK_OD(CO_CANmodule_t *CANmodule) {
+ (void)CANmodule;
+ (void)pthread_mutex_unlock(&CO_OD_mutex);
+}
+
+/* Synchronization between CAN receive and message processing threads. */
+#define CO_MemoryBarrier() {__sync_synchronize();}
+#endif /* CO_SINGLE_THREAD */
+
+#define CO_FLAG_READ(rxNew) ((rxNew) != NULL)
+#define CO_FLAG_SET(rxNew) {CO_MemoryBarrier(); rxNew = (void*)1L;}
+#define CO_FLAG_CLEAR(rxNew) {CO_MemoryBarrier(); rxNew = NULL;}
+
+#endif /* #ifndef CO_DOXYGEN */
+
+
+#if CO_DRIVER_MULTI_INTERFACE > 0 || defined CO_DOXYGEN
+/**
+ * Add socketCAN interface to can driver
+ *
+ * Function must be called after CO_CANmodule_init.
+ *
+ * @param CANmodule This object will be initialized.
+ * @param can_ifindex CAN Interface index
+ * @return #CO_ReturnError_t: CO_ERROR_NO, CO_ERROR_ILLEGAL_ARGUMENT,
+ * CO_ERROR_SYSCALL or CO_ERROR_INVALID_STATE.
+ */
+CO_ReturnError_t CO_CANmodule_addInterface(CO_CANmodule_t *CANmodule,
+ int can_ifindex);
+
+/**
+ * Check on which interface the last message for one message buffer was received
+ *
+ * It is in the responsibility of the user to check that this information is
+ * useful as some messages can be received at any time on any bus.
+ *
+ * @param CANmodule This object.
+ * @param ident 11-bit standard CAN Identifier.
+ * @param [out] CANptrRx message was received on this interface
+ * @param [out] timestamp message was received at this time (system clock)
+ *
+ * @retval false message has never been received, therefore no base address
+ * and timestamp are available
+ * @retval true base address and timestamp are valid
+ */
+bool_t CO_CANrxBuffer_getInterface(CO_CANmodule_t *CANmodule,
+ uint16_t ident,
+ const void **const CANptrRx,
+ struct timespec *timestamp);
+
+/**
+ * Set which interface should be used for message buffer transmission
+ *
+ * It is in the responsibility of the user to ensure that the correct interface
+ * is used. Some messages need to be transmitted on all interfaces.
+ *
+ * If given interface is unknown or NULL is used, a message is transmitted on
+ * all available interfaces.
+ *
+ * @param CANmodule This object.
+ * @param ident 11-bit standard CAN Identifier.
+ * @param CANptrTx use this interface. NULL = not specified
+ *
+ * @return #CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ */
+CO_ReturnError_t CO_CANtxBuffer_setInterface(CO_CANmodule_t *CANmodule,
+ uint16_t ident,
+ const void *CANptrTx);
+#endif /* CO_DRIVER_MULTI_INTERFACE */
+
+
+/**
+ * Receives CAN messages from matching epoll event
+ *
+ * This function verifies, if epoll event matches event from any CANinterface.
+ * In case of match, message is read from CAN and pre-processed for CANopenNode
+ * objects. CAN error frames are also processed.
+ *
+ * In case of CAN message function searches _rxArray_ from CO_CANmodule_t and
+ * if matched it calls the corresponding CANrx_callback, optionally copies
+ * received CAN message to _buffer_ and returns index of matched _rxArray_.
+ *
+ * This function can be used in two ways, which can be combined:
+ * - automatic mode: If CANrx_callback is specified for matched _rxArray_, then
+ * calls its callback.
+ * - manual mode: evaluate message filters, return received message
+ *
+ * @param CANmodule This object.
+ * @param ev Epoll event, which vill be verified for matches.
+ * @param [out] buffer Storage for received message or _NULL_ if not used.
+ * @param [out] msgIndex Index of received message in array from CO_CANmodule_t
+ * _rxArray_, copy of CAN message is available in _buffer_.
+ *
+ * @return True, if epoll event matches any CAN interface.
+ */
+bool_t CO_CANrxFromEpoll(CO_CANmodule_t *CANmodule,
+ struct epoll_event *ev,
+ CO_CANrxMsg_t *buffer,
+ int32_t *msgIndex);
+
+/** @} */
+
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
+#endif /* CO_DRIVER_TARGET_H */
--- /dev/null
+/*
+ * Helper functions for Linux epoll interface to CANopenNode.
+ *
+ * @file CO_epoll_interface.c
+ * @author Janez Paternoster
+ * @author Martin Wagner
+ * @copyright 2004 - 2015 Janez Paternoster
+ * @copyright 2018 - 2020 Neuberger Gebaeudeautomation GmbH
+ *
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/* following macro is necessary for accept4() function call (sockets) */
+#define _GNU_SOURCE
+
+#include "CO_epoll_interface.h"
+
+#include <stdlib.h>
+#include <unistd.h>
+#include <syslog.h>
+#include <time.h>
+#include <fcntl.h>
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
+#include <stdio.h>
+#include <ctype.h>
+#include <limits.h>
+#include <sys/socket.h>
+#include <sys/un.h>
+#include <netinet/in.h>
+#include <signal.h>
+
+#ifndef LISTEN_BACKLOG
+#define LISTEN_BACKLOG 50
+#endif
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII */
+
+/* delay for recall CANsend(), if CAN TX buffer is full */
+#ifndef CANSEND_DELAY_US
+#define CANSEND_DELAY_US 100
+#endif
+
+
+/* EPOLL **********************************************************************/
+/* Helper function - get monotonic clock time in microseconds */
+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) {
+ int ret;
+ struct epoll_event ev;
+
+ if (ep == NULL) {
+ return CO_ERROR_ILLEGAL_ARGUMENT;
+ }
+
+ /* Configure epoll for mainline */
+ ep->epoll_new = false;
+ ep->epoll_fd = epoll_create(1);
+ if (ep->epoll_fd < 0) {
+ log_printf(LOG_CRIT, DBG_ERRNO, "epoll_create()");
+ return CO_ERROR_SYSCALL;
+ }
+
+ /* Configure eventfd for notifications and add it to epoll */
+ ep->event_fd = eventfd(0, EFD_NONBLOCK);
+ if (ep->event_fd < 0) {
+ log_printf(LOG_CRIT, DBG_ERRNO, "eventfd()");
+ return CO_ERROR_SYSCALL;
+ }
+ ev.events = EPOLLIN;
+ ev.data.fd = ep->event_fd;
+ ret = epoll_ctl(ep->epoll_fd, EPOLL_CTL_ADD, ev.data.fd, &ev);
+ if (ret < 0) {
+ log_printf(LOG_CRIT, DBG_ERRNO, "epoll_ctl(event_fd)");
+ return CO_ERROR_SYSCALL;
+ }
+
+ /* Configure timer for timerInterval_us and add it to epoll */
+ ep->timer_fd = timerfd_create(CLOCK_MONOTONIC, TFD_NONBLOCK);
+ if (ep->timer_fd < 0) {
+ log_printf(LOG_CRIT, DBG_ERRNO, "timerfd_create()");
+ return CO_ERROR_SYSCALL;
+ }
+ ep->tm.it_interval.tv_sec = timerInterval_us / 1000000;
+ ep->tm.it_interval.tv_nsec = (timerInterval_us % 1000000) * 1000;
+ ep->tm.it_value.tv_sec = 0;
+ ep->tm.it_value.tv_nsec = 1;
+ ret = timerfd_settime(ep->timer_fd, 0, &ep->tm, NULL);
+ if (ret < 0) {
+ log_printf(LOG_CRIT, DBG_ERRNO, "timerfd_settime");
+ return CO_ERROR_SYSCALL;
+ }
+ ev.events = EPOLLIN;
+ ev.data.fd = ep->timer_fd;
+ ret = epoll_ctl(ep->epoll_fd, EPOLL_CTL_ADD, ev.data.fd, &ev);
+ if (ret < 0) {
+ log_printf(LOG_CRIT, DBG_ERRNO, "epoll_ctl(timer_fd)");
+ return CO_ERROR_SYSCALL;
+ }
+ ep->timerInterval_us = timerInterval_us;
+ ep->previousTime_us = clock_gettime_us();
+ ep->timeDifference_us = 0;
+
+ return CO_ERROR_NO;
+}
+
+void CO_epoll_close(CO_epoll_t *ep) {
+ if (ep == NULL) {
+ return;
+ }
+
+ close(ep->epoll_fd);
+ ep->epoll_fd = -1;
+
+ close(ep->event_fd);
+ ep->event_fd = -1;
+
+ close(ep->timer_fd);
+ ep->timer_fd = -1;
+}
+
+
+void CO_epoll_wait(CO_epoll_t *ep) {
+ if (ep == NULL) {
+ return;
+ }
+
+ /* wait for an event */
+ int ready = epoll_wait(ep->epoll_fd, &ep->ev, 1, -1);
+ ep->epoll_new = true;
+ ep->timerEvent = false;
+
+ /* calculate time difference since last call */
+ uint64_t now = clock_gettime_us();
+ ep->timeDifference_us = (uint32_t)(now - ep->previousTime_us);
+ ep->previousTime_us = now;
+ /* application may will lower this */
+ ep->timerNext_us = ep->timerInterval_us;
+
+ /* process event */
+ if (ready != 1 && errno == EINTR) {
+ /* event from interrupt or signal, nothing to process, continue */
+ ep->epoll_new = false;
+ }
+ 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
+ ) {
+ 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
+ ) {
+ uint64_t val;
+ ssize_t s = read(ep->timer_fd, &val, sizeof(uint64_t));
+ if (s != sizeof(uint64_t) && errno != EAGAIN) {
+ log_printf(LOG_DEBUG, DBG_ERRNO, "read(timer_fd)");
+ }
+ ep->epoll_new = false;
+ ep->timerEvent = true;
+ }
+}
+
+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);
+ ep->epoll_new = false;
+ }
+
+ /* lower next timer interval if changed by application */
+ if (ep->timerNext_us < ep->timerInterval_us) {
+ /* add one microsecond extra delay and make sure it is not zero */
+ ep->timerNext_us += 1;
+ if (ep->timerInterval_us < 1000000) {
+ ep->tm.it_value.tv_nsec = ep->timerNext_us * 1000;
+ }
+ else {
+ ep->tm.it_value.tv_sec = ep->timerNext_us / 1000000;
+ 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) {
+ log_printf(LOG_DEBUG, DBG_ERRNO, "timerfd_settime");
+ }
+ }
+}
+
+
+/* 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;
+ uint64_t u = 1;
+ ssize_t s;
+ s = write(ep->event_fd, &u, sizeof(uint64_t));
+ if (s != sizeof(uint64_t)) {
+ log_printf(LOG_DEBUG, DBG_ERRNO, "write()");
+ }
+}
+#endif
+
+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->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
+
+#endif /* CO_SINGLE_THREAD */
+}
+
+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);
+
+ /* If there are unsent CAN messages, call CO_CANmodule_process() earlier */
+ if (co->CANmodule->CANtxCount > 0 && ep->timerNext_us > CANSEND_DELAY_US) {
+ ep->timerNext_us = CANSEND_DELAY_US;
+ }
+}
+
+
+/* CANrx and REALTIME *********************************************************/
+void CO_epoll_processRT(CO_epoll_t *ep,
+ CO_t *co,
+ bool_t realtime)
+{
+ if (co == NULL || ep == NULL) {
+ return;
+ }
+
+ /* Verify for epoll events */
+ if (ep->epoll_new) {
+ if (CO_CANrxFromEpoll(co->CANmodule, &ep->ev, NULL, NULL)) {
+ ep->epoll_new = false;
+ }
+ }
+
+ if (!realtime || ep->timerEvent) {
+ 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);
+#endif
+#if (CO_CONFIG_PDO) & CO_CONFIG_RPDO_ENABLE
+ 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);
+#endif
+ (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;
+ /* 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);
+ if (n >= 0) {
+ nWritten = (size_t)n;
+ }
+ else {
+ /* probably EAGAIN - "Resource temporarily unavailable". Retry. */
+ log_printf(LOG_DEBUG, DBG_ERRNO, "write(gtwa_response)");
+ nWritten = 0;
+ }
+ }
+ else {
+ *connectionOK = 0;
+ }
+ return nWritten;
+}
+
+static inline void socetAcceptEnableForEpoll(CO_epoll_gtw_t *epGtw) {
+ struct epoll_event ev;
+ int ret;
+
+ ev.events = EPOLLIN | EPOLLONESHOT;
+ ev.data.fd = epGtw->gtwa_fdSocket;
+ ret = epoll_ctl(epGtw->epoll_fd, EPOLL_CTL_MOD, ev.data.fd, &ev);
+ if (ret < 0) {
+ log_printf(LOG_CRIT, DBG_ERRNO, "epoll_ctl(gtwa_fdSocket)");
+ }
+}
+
+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;
+
+ if (epGtw == NULL || epoll_fd < 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->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) {
+ 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) {
+ 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) {
+ log_printf(LOG_CRIT, DBG_COMMAND_LOCAL_BIND, localSocketPath);
+ return CO_ERROR_SYSCALL;
+ }
+
+ ret = listen(epGtw->gtwa_fdSocket, LISTEN_BACKLOG);
+ if(ret < 0) {
+ log_printf(LOG_CRIT, DBG_ERRNO, "listen(local)");
+ return CO_ERROR_SYSCALL;
+ }
+
+ /* Ignore the SIGPIPE signal, which may happen, if remote client broke
+ * the connection. Signal may be triggered by write call inside
+ * gtwa_write_response() function */
+ if (signal(SIGPIPE, SIG_IGN) == SIG_ERR) {
+ log_printf(LOG_CRIT, DBG_ERRNO, "signal");
+ 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
+ ) {
+ 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) {
+ log_printf(LOG_CRIT, DBG_ERRNO, "socket(tcp)");
+ return CO_ERROR_SYSCALL;
+ }
+
+ 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) {
+ log_printf(LOG_CRIT, DBG_COMMAND_TCP_BIND, commandInterface);
+ return CO_ERROR_SYSCALL;
+ }
+
+ ret = listen(epGtw->gtwa_fdSocket, LISTEN_BACKLOG);
+ if(ret < 0) {
+ log_printf(LOG_CRIT, DBG_ERRNO, "listen(tcp)");
+ return CO_ERROR_SYSCALL;
+ }
+
+ /* Ignore the SIGPIPE signal, which may happen, if remote client broke
+ * the connection. Signal may be triggered by write call inside
+ * gtwa_write_response() function */
+ if (signal(SIGPIPE, SIG_IGN) == SIG_ERR) {
+ log_printf(LOG_CRIT, DBG_ERRNO, "signal");
+ return CO_ERROR_SYSCALL;
+ }
+
+ log_printf(LOG_INFO, DBG_COMMAND_TCP_INFO, commandInterface);
+ }
+ else {
+ epGtw->commandInterface = CO_COMMAND_IF_DISABLED;
+ }
+
+ if (epGtw->gtwa_fd >= 0) {
+ ev.events = EPOLLIN;
+ ev.data.fd = epGtw->gtwa_fd;
+ ret = epoll_ctl(epGtw->epoll_fd, EPOLL_CTL_ADD, ev.data.fd, &ev);
+ if (ret < 0) {
+ log_printf(LOG_CRIT, DBG_ERRNO, "epoll_ctl(gtwa_fd)");
+ return CO_ERROR_SYSCALL;
+ }
+ }
+ if (epGtw->gtwa_fdSocket >= 0) {
+ /* prepare epoll for listening for new socket connection. After
+ * connection will be accepted, fd for io operation will be defined. */
+ ev.events = EPOLLIN | EPOLLONESHOT;
+ ev.data.fd = epGtw->gtwa_fdSocket;
+ ret = epoll_ctl(epGtw->epoll_fd, EPOLL_CTL_ADD, ev.data.fd, &ev);
+ if (ret < 0) {
+ log_printf(LOG_CRIT, DBG_ERRNO, "epoll_ctl(gtwa_fdSocket)");
+ return CO_ERROR_SYSCALL;
+ }
+ }
+
+ return CO_ERROR_NO;
+}
+
+void CO_epoll_closeGtw(CO_epoll_gtw_t *epGtw) {
+ if (epGtw == NULL) {
+ return;
+ }
+
+ if (epGtw->commandInterface == CO_COMMAND_IF_LOCAL_SOCKET) {
+ if (epGtw->gtwa_fd > 0) {
+ close(epGtw->gtwa_fd);
+ }
+ close(epGtw->gtwa_fdSocket);
+ /* Remove local socket file from filesystem. */
+ if(remove(epGtw->localSocketPath) < 0) {
+ log_printf(LOG_CRIT, DBG_ERRNO, "remove(local)");
+ }
+ }
+ else if (epGtw->commandInterface >= CO_COMMAND_IF_TCP_SOCKET_MIN) {
+ if (epGtw->gtwa_fd > 0) {
+ close(epGtw->gtwa_fd);
+ }
+ close(epGtw->gtwa_fdSocket);
+ }
+ epGtw->gtwa_fd = -1;
+ epGtw->gtwa_fdSocket = -1;
+}
+
+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);
+ epGtw->freshCommand = true;
+}
+
+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
+ ) {
+ bool_t fail = false;
+
+ 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 {
+ /* add fd to epoll */
+ struct epoll_event ev2;
+ ev2.events = EPOLLIN;
+ ev2.data.fd = epGtw->gtwa_fd;
+ 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)");
+ }
+ epGtw->socketTimeoutTmr_us = 0;
+ }
+
+ if (fail) {
+ socetAcceptEnableForEpoll(epGtw);
+ }
+ ep->epoll_new = false;
+ }
+ 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);
+
+ ssize_t s = read(epGtw->gtwa_fd, buf, space);
+
+ if (space == 0 || co->nodeIdUnconfigured) {
+ /* continue or purge data */
+ }
+ else if (s < 0 && errno != EAGAIN) {
+ log_printf(LOG_DEBUG, DBG_ERRNO, "read(gtwa_fd)");
+ }
+ 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 */
+ const char sequence[] = "[0] ";
+ 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
+ ) {
+ CO_GTWA_write(co->gtwa, sequence, strlen(sequence));
+ }
+ epGtw->freshCommand = closed;
+ CO_GTWA_write(co->gtwa, buf, s);
+ }
+ 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);
+ if (ret < 0) {
+ 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 {
+ 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);
+ 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->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);
+ if (ret < 0) {
+ 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 {
+ epGtw->socketTimeoutTmr_us += ep->timeDifference_us;
+ }
+ }
+}
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII */
--- /dev/null
+/**
+ * Helper functions for Linux epoll interface to CANopenNode.
+ *
+ * @file CO_epoll_interface.h
+ * @ingroup CO_epoll_interface
+ * @author Janez Paternoster
+ * @author Martin Wagner
+ * @copyright 2004 - 2020 Janez Paternoster
+ * @copyright 2018 - 2020 Neuberger Gebaeudeautomation GmbH
+ *
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CO_EPOLL_INTERFACE_H
+#define CO_EPOLL_INTERFACE_H
+
+#include "CANopen.h"
+
+#include <sys/epoll.h>
+#include <sys/eventfd.h>
+#include <sys/timerfd.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @defgroup CO_socketCAN socketCAN
+ * @{
+ *
+ * Linux specific interface to CANopenNode.
+ *
+ * Linux includes CAN interface inside its kernel, so called SocketCAN. It
+ * operates as a network device. For more information on Linux SocketCAN see
+ * https://www.kernel.org/doc/html/latest/networking/can.html
+ *
+ * Linux specific files for interfacing with Linux SocketCAN are located inside
+ * "CANopenNode/socketCAN" directory.
+ *
+ * CANopenNode runs as a set of non-blocking functions. It can run in single or
+ * multiple threads. Best approach for RT IO device can be with two threads:
+ * - timer based real-time thread for CAN receive, SYNC and PDO, see
+ * @ref CO_epoll_processRT()
+ * - mainline thread for other processing, see @ref CO_epoll_processMain()
+ *
+ * Main references for Linux functions used here are Linux man pages and the
+ * book: The Linux Programming Interface by Michael Kerrisk.
+ * @}
+ */
+
+/**
+ * @defgroup CO_epoll_interface Epoll interface
+ * @ingroup CO_socketCAN
+ * @{
+ *
+ * Linux epoll interface to CANopenNode.
+ *
+ * The Linux epoll API performs a monitoring multiple file descriptors to see
+ * if I/O is possible on any of them.
+ *
+ * CANopenNode uses epoll interface to provide an event based mechanism. Epoll
+ * waits for multiple different events, such as: interval timer event,
+ * notification event, CAN receive event or socket based event for gateway.
+ * CANopenNode non-blocking functions are processed after each event.
+ *
+ * CANopenNode itself offers functionality for calculation of time, when next
+ * interval timer event should trigger the processing. It can also trigger
+ * notification events in case of multi-thread operation.
+ */
+
+/**
+ * Object for epoll, timer and event API.
+ */
+typedef struct {
+ /** Epoll file descriptor */
+ int epoll_fd;
+ /** Notification event file descriptor */
+ int event_fd;
+ /** Interval timer file descriptor */
+ int timer_fd;
+ /** Interval of the timer in microseconds, from @ref CO_epoll_create() */
+ uint32_t timerInterval_us;
+ /** Time difference since last @ref CO_epoll_wait() execution in
+ * microseconds */
+ uint32_t timeDifference_us;
+ /** Timer value in microseconds, which can be changed by application and can
+ * shorten time of next @ref CO_epoll_wait() execution */
+ uint32_t timerNext_us;
+ /** True,if timer event is inside @ref CO_epoll_wait() */
+ bool_t timerEvent;
+ /** time value from the last process call in microseconds */
+ uint64_t previousTime_us;
+ /** Structure for timerfd */
+ struct itimerspec tm;
+ /** Structure for epoll_wait */
+ struct epoll_event ev;
+ /** true, if new epoll event is necessary to process */
+ bool_t epoll_new;
+} CO_epoll_t;
+
+/**
+ * Create Linux epoll, timerfd and eventfd
+ *
+ * Create and configure multiple Linux notification facilities, which trigger
+ * execution of the task. Epoll blocks and monitors multiple file descriptors,
+ * timerfd triggers in constant timer intervals and eventfd triggers on external
+ * signal.
+ *
+ * @param ep This object
+ * @param timerInterval_us Timer interval in microseconds
+ *
+ * @return @ref CO_ReturnError_t CO_ERROR_NO, CO_ERROR_ILLEGAL_ARGUMENT or
+ * CO_ERROR_SYSCALL.
+ */
+CO_ReturnError_t CO_epoll_create(CO_epoll_t *ep, uint32_t timerInterval_us);
+
+/**
+ * Close epoll, timerfd and eventfd
+ *
+ * @param ep This object
+ */
+void CO_epoll_close(CO_epoll_t *ep);
+
+/**
+ * Wait for an epoll event
+ *
+ * This function blocks until event registered on epoll: timerfd, eventfd, or
+ * application specified event. Function also calculates timeDifference_us since
+ * last call and prepares timerNext_us.
+ *
+ * @param ep This object
+ */
+void CO_epoll_wait(CO_epoll_t *ep);
+
+/**
+ * Closing function for an epoll event
+ *
+ * This function must be called after @ref CO_epoll_wait(). Between them
+ * should be application specified processing functions, which can check for
+ * own events and do own processing. Application may also lower timerNext_us
+ * variable. If lowered, then interval timer will be reconfigured and
+ * @ref CO_epoll_wait() will be triggered earlier.
+ *
+ * @param ep This object
+ */
+void CO_epoll_processLast(CO_epoll_t *ep);
+
+/**
+ * Initialization of functions in CANopen reset-communication section
+ *
+ * Configure callbacks for CANopen objects.
+ *
+ * @param ep This object
+ * @param co CANopen object
+ */
+void CO_epoll_initCANopenMain(CO_epoll_t *ep, CO_t *co);
+
+/**
+ * Process CANopen mainline functions
+ *
+ * This function calls @ref CO_process(). It is non-blocking and should execute
+ * cyclically. It should be between @ref CO_epoll_wait() and
+ * @ref CO_epoll_processLast() functions.
+ *
+ * @param ep This object
+ * @param co CANopen object
+ * @param enableGateway If true, gateway to external world will be enabled.
+ * @param [out] reset Return from @ref CO_process().
+ */
+void CO_epoll_processMain(CO_epoll_t *ep,
+ CO_t *co,
+ bool_t enableGateway,
+ CO_NMT_reset_cmd_t *reset);
+
+
+/**
+ * Process CAN receive and realtime functions
+ *
+ * This function checks epoll for CAN receive event and processes CANopen
+ * realtime functions: @ref CO_process_SYNC(), @ref CO_process_RPDO() and
+ * @ref CO_process_TPDO(). It is non-blocking and should execute cyclically.
+ * It should be between @ref CO_epoll_wait() and @ref CO_epoll_processLast()
+ * functions.
+ *
+ * Function can be used in the mainline thread or in own realtime thread.
+ *
+ * Processing of CANopen realtime functions is protected with @ref CO_LOCK_OD.
+ * Also Node-Id must be configured and CANmodule must be in CANnormal for
+ * processing.
+ *
+ * @param ep Pointer to @ref CO_epoll_t object.
+ * @param co CANopen object
+ * @param realtime Set to true, if function is called from the own realtime
+ * thread, and is executed at short constant interval.
+ */
+void CO_epoll_processRT(CO_epoll_t *ep,
+ CO_t *co,
+ bool_t realtime);
+
+
+#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII) || defined CO_DOXYGEN
+/**
+ * Command interface type for gateway-ascii
+ */
+typedef enum {
+ CO_COMMAND_IF_DISABLED = -100,
+ CO_COMMAND_IF_STDIO = -2,
+ CO_COMMAND_IF_LOCAL_SOCKET = -1,
+ CO_COMMAND_IF_TCP_SOCKET_MIN = 0,
+ CO_COMMAND_IF_TCP_SOCKET_MAX = 0xFFFF
+} CO_commandInterface_t;
+
+/**
+ * Object for gateway
+ */
+typedef struct {
+ /** Epoll file descriptor, from @ref CO_epoll_createGtw() */
+ int epoll_fd;
+ /** Command interface type or tcp port number, see
+ * @ref CO_commandInterface_t */
+ int32_t commandInterface;
+ /** Socket timeout in microseconds */
+ uint32_t socketTimeout_us;
+ /** Socket timeout timer in microseconds */
+ uint32_t socketTimeoutTmr_us;
+ /** Path in case of local socket */
+ char *localSocketPath;
+ /** Gateway socket file descriptor */
+ int gtwa_fdSocket;
+ /** Gateway io stream file descriptor */
+ int gtwa_fd;
+ /** Indication of fresh command */
+ bool_t freshCommand;
+} CO_epoll_gtw_t;
+
+/**
+ * Create socket for gateway-ascii command interface and add it to epoll
+ *
+ * Depending on arguments function configures stdio interface or local socket
+ * or IP socket.
+ *
+ * @param epGtw This object
+ * @param epoll_fd Already configured epoll file descriptor
+ * @param commandInterface Command interface type from CO_commandInterface_t
+ * @param socketTimeout_ms Timeout for established socket connection in [ms]
+ * @param localSocketPath File path, if commandInterface is local socket
+ *
+ * @return @ref CO_ReturnError_t CO_ERROR_NO, CO_ERROR_ILLEGAL_ARGUMENT or
+ * CO_ERROR_SYSCALL.
+ */
+CO_ReturnError_t CO_epoll_createGtw(CO_epoll_gtw_t *epGtw,
+ int epoll_fd,
+ int32_t commandInterface,
+ uint32_t socketTimeout_ms,
+ char *localSocketPath);
+
+/**
+ * Close gateway-ascii sockets
+ *
+ * @param epGtw This object
+ */
+void CO_epoll_closeGtw(CO_epoll_gtw_t *epGtw);
+
+/**
+ * Initialization of gateway functions in CANopen reset-communication section
+ *
+ * @param epGtw This object
+ * @param co CANopen object
+ */
+void CO_epoll_initCANopenGtw(CO_epoll_gtw_t *epGtw, CO_t *co);
+
+/**
+ * Process CANopen gateway functions
+ *
+ * This function checks for epoll events and verifies socket connection timeout.
+ * It is non-blocking and should execute cyclically. It should be between
+ * @ref CO_epoll_wait() and @ref CO_epoll_processLast() functions.
+ *
+ * @param epGtw This object
+ * @param co CANopen object
+ * @param ep Pointer to @ref CO_epoll_t object.
+ */
+void CO_epoll_processGtw(CO_epoll_gtw_t *epGtw,
+ CO_t *co,
+ CO_epoll_t *ep);
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII */
+
+/** @} */
+
+#ifdef __cplusplus
+}
+#endif /*__cplusplus*/
+
+#endif /* CO_EPOLL_INTERFACE_H */
--- /dev/null
+/*
+ * CAN module object for Linux socketCAN Error handling.
+ *
+ * @file CO_error.c
+ * @author Martin Wagner
+ * @copyright 2018 - 2020 Neuberger Gebaeudeautomation GmbH
+ *
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <syslog.h>
+#include <time.h>
+#include <linux/can/error.h>
+
+#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)
+{
+ log_printf(LOG_DEBUG, DBG_CAN_SET_LISTEN_ONLY, CANerrorhandler->ifName);
+
+ clock_gettime(CLOCK_MONOTONIC, &CANerrorhandler->timestamp);
+ CANerrorhandler->listenOnly = true;
+
+ 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);
+ ret = system(command);
+ 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)
+{
+ log_printf(LOG_DEBUG, DBG_CAN_CLR_LISTEN_ONLY, CANerrorhandler->ifName);
+
+ CANerrorhandler->listenOnly = false;
+ CANerrorhandler->timestamp.tv_sec = 0;
+ 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)
+{
+ CO_CANinterfaceState_t result = CO_INTERFACE_ACTIVE;
+
+ if ((msg->can_id & CAN_ERR_BUSOFF) != 0) {
+ log_printf(LOG_NOTICE, CAN_BUSOFF, CANerrorhandler->ifName);
+
+ /* The can interface changed it's state to "bus off" (e.g. because of
+ * a short on the can wires). We re-start the interface and mark it
+ * "listen only".
+ * Restarting the interface is the only way to clear kernel and hardware
+ * tx queues */
+ result = CO_CANerrorSetListenOnly(CANerrorhandler, true);
+ CANerrorhandler->CANerrorStatus |= CO_CAN_ERRTX_BUS_OFF;
+ }
+ return result;
+}
+
+
+/**
+ * Check and handle controller problems
+ */
+static CO_CANinterfaceState_t CO_CANerrorCrtl(
+ CO_CANinterfaceErrorhandler_t *CANerrorhandler,
+ const struct can_frame *msg)
+{
+ CO_CANinterfaceState_t result = CO_INTERFACE_ACTIVE;
+
+ /* Control
+ * - error counters (rec/tec) are handled inside CAN hardware, nothing
+ * to do in here
+ * - we can't really do anything about buffer overflows here. Confirmed
+ * CANopen protocols will detect the error, non-confirmed protocols
+ * need to be error tolerant.
+ * - There is no information, when CAN controller leaves warning level,
+ * so we can't clear it. So we also don't set it. */
+ if ((msg->can_id & CAN_ERR_CRTL) != 0) {
+ /* clear bus off here */
+ CANerrorhandler->CANerrorStatus &= 0xFFFF ^ CO_CAN_ERRTX_BUS_OFF;
+
+ if ((msg->data[1] & CAN_ERR_CRTL_RX_PASSIVE) != 0) {
+ 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) {
+ 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) {
+ 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) {
+ 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) {
+ 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) {
+ log_printf(LOG_INFO, CAN_TX_LEVEL_WARNING, CANerrorhandler->ifName);
+ /* clear passive flag, set warning */
+ CANerrorhandler->CANerrorStatus &= 0x7FFF ^ CO_CAN_ERRTX_PASSIVE;
+ /* CANerrorhandler->CANerrorStatus |= CO_CAN_ERRTX_WARNING; */
+ }
+#ifdef CAN_ERR_CRTL_ACTIVE
+ else if ((msg->data[1] & CAN_ERR_CRTL_ACTIVE) != 0) {
+ log_printf(LOG_NOTICE, CAN_TX_LEVEL_ACTIVE, CANerrorhandler->ifName);
+ }
+#endif
+ }
+ return result;
+}
+
+
+/**
+ * Check and handle controller problems
+ */
+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) {
+ return CO_INTERFACE_LISTEN_ONLY;
+ }
+
+ /* received no ACK on transmission */
+ if ((msg->can_id & CAN_ERR_ACK) != 0) {
+ CANerrorhandler->noackCounter ++;
+ if (CANerrorhandler->noackCounter > CO_CANerror_NOACK_MAX) {
+ log_printf(LOG_INFO, CAN_NOACK, CANerrorhandler->ifName);
+
+ /* We get the NO-ACK error continuously when no other CAN node
+ * is active on the bus (Error Counting exception 1 in CAN spec).
+ * todo - you need to pull the message causing no-ack from the CAN
+ * hardware buffer. This can be done by either resetting interface
+ * in here or deleting it within Linux Kernel can driver (set "false"). */
+ result = CO_CANerrorSetListenOnly(CANerrorhandler, true);
+ }
+ }
+ else {
+ CANerrorhandler->noackCounter = 0;
+ }
+ return result;
+}
+
+
+/******************************************************************************/
+void CO_CANerror_init(
+ CO_CANinterfaceErrorhandler_t *CANerrorhandler,
+ int fd,
+ const char *ifName)
+{
+ if (CANerrorhandler == NULL) {
+ return;
+ }
+
+ CANerrorhandler->fd = fd;
+ memcpy(CANerrorhandler->ifName, ifName, sizeof(CANerrorhandler->ifName));
+ CANerrorhandler->noackCounter = 0;
+ CANerrorhandler->listenOnly = false;
+ CANerrorhandler->timestamp.tv_sec = 0;
+ CANerrorhandler->timestamp.tv_nsec = 0;
+ CANerrorhandler->CANerrorStatus = 0;
+}
+
+
+/******************************************************************************/
+void CO_CANerror_disable(
+ CO_CANinterfaceErrorhandler_t *CANerrorhandler)
+{
+ if (CANerrorhandler == NULL) {
+ return;
+ }
+
+ memset(CANerrorhandler, 0, sizeof(*CANerrorhandler));
+ CANerrorhandler->fd = -1;
+}
+
+
+/******************************************************************************/
+void CO_CANerror_rxMsg(
+ CO_CANinterfaceErrorhandler_t *CANerrorhandler)
+{
+ if (CANerrorhandler == NULL) {
+ return;
+ }
+
+ /* someone is active, we can leave listen only immediately */
+ if (CANerrorhandler->listenOnly) {
+ CO_CANerrorClearListenOnly(CANerrorhandler);
+ }
+ CANerrorhandler->noackCounter = 0;
+}
+
+
+/******************************************************************************/
+CO_CANinterfaceState_t CO_CANerror_txMsg(
+ CO_CANinterfaceErrorhandler_t *CANerrorhandler)
+{
+ struct timespec now;
+
+ if (CANerrorhandler == NULL) {
+ return CO_INTERFACE_BUS_OFF;
+ }
+
+ if (CANerrorhandler->listenOnly) {
+ clock_gettime(CLOCK_MONOTONIC, &now);
+ if (CANerrorhandler->timestamp.tv_sec + CO_CANerror_LISTEN_ONLY < now.tv_sec) {
+ /* let's try that again. Maybe someone is waiting for LSS now. It
+ * doesn't matter which message is sent, as all messages are ACKed. */
+ CO_CANerrorClearListenOnly(CANerrorhandler);
+ return CO_INTERFACE_ACTIVE;
+ }
+ return CO_INTERFACE_LISTEN_ONLY;
+ }
+ return CO_INTERFACE_ACTIVE;
+}
+
+
+/******************************************************************************/
+CO_CANinterfaceState_t CO_CANerror_rxMsgError(
+ CO_CANinterfaceErrorhandler_t *CANerrorhandler,
+ const struct can_frame *msg)
+{
+ if (CANerrorhandler == NULL) {
+ return CO_INTERFACE_BUS_OFF;
+ }
+
+ CO_CANinterfaceState_t result;
+
+ /* 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);
+
+ /* Process errors - start with the most unambiguous one */
+
+ result = CO_CANerrorBusoff(CANerrorhandler, msg);
+ if (result != CO_INTERFACE_ACTIVE) {
+ return result;
+ }
+
+ result = CO_CANerrorCrtl(CANerrorhandler, msg);
+ if (result != CO_INTERFACE_ACTIVE) {
+ return result;
+ }
+
+ result = CO_CANerrorNoack(CANerrorhandler, msg);
+ if (result != CO_INTERFACE_ACTIVE) {
+ return result;
+ }
+
+ return CO_INTERFACE_ACTIVE;
+}
--- /dev/null
+/**
+ * CANopenNode Linux socketCAN Error handling.
+ *
+ * @file CO_error.h
+ * @ingroup CO_socketCAN_ERROR
+ * @author Martin Wagner
+ * @copyright 2018 - 2020 Neuberger Gebaeudeautomation GmbH
+ *
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+
+#ifndef CO_ERROR_H
+#define CO_ERROR_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <string.h>
+#include <linux/can.h>
+#include <net/if.h>
+
+#if __has_include("CO_error_custom.h")
+ #include "CO_error_custom.h"
+#else
+ #include "CO_error_msgs.h"
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @defgroup CO_socketCAN_ERROR CAN errors & Log
+ * @ingroup CO_socketCAN
+ * @{
+ *
+ * CANopen Errors and System message log
+ */
+
+/**
+ * Message logging function.
+ *
+ * Function must be defined by application. It should record log message to some
+ * place, for example syslog() call in Linux or logging functionality in
+ * CANopen gateway @ref CO_CANopen_309_3.
+ *
+ * By default system stores messages in /var/log/syslog file.
+ * Log can optionally be configured before, for example to filter out less
+ * critical errors than LOG_NOTICE, specify program name, print also process PID
+ * and print also to standard error, set 'user' type of program, use:
+ * @code
+ * setlogmask (LOG_UPTO (LOG_NOTICE));
+ * openlog ("exampleprog", LOG_PID | LOG_PERROR, LOG_USER);
+ * @endcode
+ *
+ * @param priority one of LOG_EMERG, LOG_ALERT, LOG_CRIT, LOG_ERR, LOG_WARNING,
+ * LOG_NOTICE, LOG_INFO, LOG_DEBUG
+ * @param format format string as in printf
+ */
+void log_printf(int priority, const char *format, ...);
+
+
+/**
+ * driver interface state
+ *
+ * CAN hardware can be in the followning states:
+ * - error active (OK)
+ * - error passive (Can't generate error flags)
+ * - bus off (no influence on bus)
+ */
+typedef enum {
+ CO_INTERFACE_ACTIVE, /**< CAN error passive/active */
+ CO_INTERFACE_LISTEN_ONLY, /**< CAN error passive/active, but currently no other device on bus */
+ CO_INTERFACE_BUS_OFF /**< CAN bus off */
+} CO_CANinterfaceState_t;
+
+
+/**
+ * This is how many NO-ACKs need to be received in a row to assume
+ * that no other nodes are connected to a bus and therefore are
+ * assuming listen-only
+ */
+#define CO_CANerror_NOACK_MAX 16
+
+
+/**
+ * This is how long we are going to block transmission if listen-only
+ * mode is active
+ *
+ * Time is in seconds.
+ */
+#define CO_CANerror_LISTEN_ONLY 10
+
+
+/**
+ * socketCAN interface error handling
+ */
+typedef struct {
+ int fd; /**< interface FD */
+ char ifName[IFNAMSIZ]; /**< interface name as string */
+ uint32_t noackCounter; /**< counts no ACK on CAN transmission */
+ volatile unsigned char listenOnly; /**< set to listen only mode */
+ struct timespec timestamp; /**< listen only mode started at this time */
+ uint16_t CANerrorStatus; /**< CAN error status bitfield, see @ref CO_CAN_ERR_status_t */
+} CO_CANinterfaceErrorhandler_t;
+
+
+/**
+ * Initialize CAN error handler
+ *
+ * We need one error handler per interface
+ *
+ * @param CANerrorhandler This object will be initialized.
+ * @param fd interface file descriptor
+ * @param ifname interface name as string
+ */
+void CO_CANerror_init(
+ CO_CANinterfaceErrorhandler_t *CANerrorhandler,
+ int fd,
+ const char *ifname);
+
+
+/**
+ * Reset CAN error handler
+ *
+ * @param CANerrorhandler CAN error object.
+ */
+void CO_CANerror_disable(
+ CO_CANinterfaceErrorhandler_t *CANerrorhandler);
+
+
+/**
+ * Message received event
+ *
+ * When a message is received at least one other CAN module is connected.
+ * Function clears listenOnly and noackCounter error flags.
+ *
+ * @param CANerrorhandler CAN error object.
+ */
+void CO_CANerror_rxMsg(
+ CO_CANinterfaceErrorhandler_t *CANerrorhandler);
+
+
+/**
+ * Check if interface is ready for message transmission
+ *
+ * Message mustn't be transmitted if not ready.
+ *
+ * @param CANerrorhandler CAN error object.
+ * @return CO_INTERFACE_ACTIVE message transmission ready
+ */
+CO_CANinterfaceState_t CO_CANerror_txMsg(
+ CO_CANinterfaceErrorhandler_t *CANerrorhandler);
+
+
+/**
+ * Error message received event
+ *
+ * This handles all received error messages.
+ *
+ * @param CANerrorhandler CAN error object.
+ * @param msg received error message
+ * @return #CO_CANinterfaceState_t
+ */
+CO_CANinterfaceState_t CO_CANerror_rxMsgError(
+ CO_CANinterfaceErrorhandler_t *CANerrorhandler,
+ const struct can_frame *msg);
+
+
+/** @} */
+
+#ifdef __cplusplus
+}
+#endif /*__cplusplus*/
+
+#endif /* CO_ERROR_H */
--- /dev/null
+/*
+ * Definitions for CANopenNode Linux socketCAN Error handling.
+ *
+ * @file CO_Error_msgs.h
+ * @author Martin Wagner
+ * @copyright 2020 Neuberger Gebaeudeautomation GmbH
+ *
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+
+#ifndef CO_ERROR_MSGS_H
+#define CO_ERROR_MSGS_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/*
+ * Message definitions for Linux CANopen socket driver (notice and errors)
+ */
+#define CAN_NOT_FOUND "(%s) CAN Interface \"%s\" not found", __func__
+#define CAN_INIT_FAILED "(%s) CAN Interface \"%s\" Init failed", __func__
+#define CAN_BINDING_FAILED "(%s) Binding CAN Interface \"%s\" failed", __func__
+#define CAN_ERROR_FILTER_FAILED "(%s) Setting CAN Interface \"%s\" error filter failed", __func__
+#define CAN_FILTER_FAILED "(%s) Setting CAN Interface \"%s\" message filter failed", __func__
+#define CAN_NAMETOINDEX "CAN Interface \"%s\" -> Index %d"
+#define CAN_SOCKET_BUF_SIZE "CAN Interface \"%s\" RX buffer set to %d messages (%d Bytes)"
+#define CAN_RX_SOCKET_QUEUE_OVERFLOW "CAN Interface \"%s\" has lost %d messages"
+#define CAN_BUSOFF "CAN Interface \"%s\" changed to \"Bus Off\". Switching to Listen Only mode..."
+#define CAN_NOACK "CAN Interface \"%s\" no \"ACK\" received. Switching to Listen Only mode..."
+#define CAN_RX_PASSIVE "CAN Interface \"%s\" changed state to \"Rx Passive\""
+#define CAN_TX_PASSIVE "CAN Interface \"%s\" changed state to \"Tx Passive\""
+#define CAN_TX_LEVEL_ACTIVE "CAN Interface \"%s\" changed state to \"Active\""
+#define CAN_RX_BUF_OVERFLOW "CAN Interface \"%s\" Rx buffer overflow. Message dropped"
+#define CAN_TX_BUF_OVERFLOW "CAN Interface \"%s\" Tx buffer overflow. Message dropped"
+#define CAN_RX_LEVEL_WARNING "CAN Interface \"%s\" reached Rx Warning Level"
+#define CAN_TX_LEVEL_WARNING "CAN Interface \"%s\" reached Tx Warning Level"
+
+
+/*
+ * Message definitions for debugging
+ */
+#define DBG_GENERAL "(%s) Error: %s%d", __func__
+#define DBG_ERRNO "(%s) OS error \"%s\" in %s", __func__, strerror(errno)
+#define DBG_CO_DEBUG "(%s) CO_DEBUG: %s", __func__
+#define DBG_CAN_TX_FAILED "(%s) Transmitting CAN msg OID 0x%03x failed(%s)", __func__
+#define DBG_CAN_RX_PARAM_FAILED "(%s) Setting CAN rx buffer failed (%s)", __func__
+#define DBG_CAN_RX_FAILED "(%s) Receiving CAN msg failed (%s)", __func__
+#define DBG_CAN_ERROR_GENERAL "(%s) Socket error msg ID: 0x%08x, Data[0..7]: 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x (%s)", __func__
+#define DBG_CAN_RX_EPOLL "(%s) CAN Epoll error (0x%02x - %s)", __func__
+#define DBG_CAN_SET_LISTEN_ONLY "(%s) %s Set Listen Only", __func__
+#define DBG_CAN_CLR_LISTEN_ONLY "(%s) %s Leave Listen Only", __func__
+
+/* mainline */
+#define DBG_EMERGENCY_RX "CANopen Emergency message from node 0x%02X: errorCode=0x%04X, errorRegister=0x%02X, errorBit=0x%02X, infoCode=0x%08X"
+#define DBG_NMT_CHANGE "CANopen NMT state changed to: \"%s\" (%d)"
+#define DBG_HB_CONS_NMT_CHANGE "CANopen Remote node ID = 0x%02X (index = %d): NMT state changed to: \"%s\" (%d)"
+#define DBG_ARGUMENT_UNKNOWN "(%s) Unknown %s argument: \"%s\"", __func__
+#define DBG_NOT_TCP_PORT "(%s) -c argument \"%s\" is not a valid tcp port", __func__
+#define DBG_WRONG_NODE_ID "(%s) Wrong node ID \"%d\"", __func__
+#define DBG_WRONG_PRIORITY "(%s) Wrong RT priority \"%d\"", __func__
+#define DBG_NO_CAN_DEVICE "(%s) Can't find CAN device \"%s\"", __func__
+#define DBG_STORAGE "(%s) Error with storage \"%s\"", __func__
+#define DBG_OD_ENTRY "(%s) Error in Object Dictionary entry: 0x%X", __func__
+#define DBG_CAN_OPEN "(%s) CANopen error in %s, err=%d", __func__
+#define DBG_CAN_OPEN_INFO "CANopen device, Node ID = 0x%02X, %s"
+
+/* CO_epoll_interface */
+#define DBG_EPOLL_UNKNOWN "(%s) CAN Epoll error, events=0x%02x, fd=%d", __func__
+#define DBG_COMMAND_LOCAL_BIND "(%s) Can't bind local socket to path \"%s\"", __func__
+#define DBG_COMMAND_TCP_BIND "(%s) Can't bind tcp socket to port \"%d\"", __func__
+#define DBG_COMMAND_STDIO_INFO "CANopen command interface on \"standard IO\" started"
+#define DBG_COMMAND_LOCAL_INFO "CANopen command interface on local socket \"%s\" started"
+#define DBG_COMMAND_TCP_INFO "CANopen command interface on tcp port \"%d\" started"
+
+
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
+#endif /* CO_ERROR_MSGS_H */
--- /dev/null
+/*
+ * CANopen main program file for CANopenNode on Linux.
+ *
+ * @file CO_main_basic.c
+ * @author Janez Paternoster
+ * @copyright 2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <sched.h>
+#include <signal.h>
+#include <errno.h>
+#include <stdarg.h>
+#include <syslog.h>
+#include <time.h>
+#include <sys/epoll.h>
+#include <net/if.h>
+#include <linux/reboot.h>
+#include <sys/reboot.h>
+
+#include "CANopen.h"
+#include "OD.h"
+#include "CO_error.h"
+#include "CO_epoll_interface.h"
+#include "CO_storageLinux.h"
+
+/* Call external application functions. */
+#ifdef CO_USE_APPLICATION
+#include "CO_application.h"
+#endif
+
+/* Add trace functionality for recording variables over time */
+#if (CO_CONFIG_TRACE) & CO_CONFIG_TRACE_ENABLE
+#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
+#endif
+#ifndef TMR_THREAD_INTERVAL_US
+#define TMR_THREAD_INTERVAL_US 1000
+#endif
+
+/* 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
+#endif
+#ifndef FIRST_HB_TIME
+#define FIRST_HB_TIME 500
+#endif
+#ifndef SDO_SRV_TIMEOUT_TIME
+#define SDO_SRV_TIMEOUT_TIME 1000
+#endif
+#ifndef SDO_CLI_TIMEOUT_TIME
+#define SDO_CLI_TIMEOUT_TIME 500
+#endif
+#ifndef SDO_CLI_BLOCK
+#define SDO_CLI_BLOCK false
+#endif
+#ifndef OD_STATUS_BITS
+#define OD_STATUS_BITS NULL
+#endif
+/* CANopen gateway enable switch for CO_epoll_processMain() */
+#ifndef GATEWAY_ENABLE
+#define GATEWAY_ENABLE true
+#endif
+/* Interval for time stamp message in milliseconds */
+#ifndef TIME_STAMP_INTERVAL_MS
+#define TIME_STAMP_INTERVAL_MS 10000
+#endif
+
+/* Definitions for application specific data storage objects */
+#ifndef CO_STORAGE_APPLICATION
+#define CO_STORAGE_APPLICATION
+#endif
+/* Interval for automatic data storage in microseconds */
+#ifndef CO_STORAGE_AUTO_INTERVAL
+#define CO_STORAGE_AUTO_INTERVAL 60000000
+#endif
+
+/* CANopen object */
+CO_t *CO = NULL;
+
+/* Active node-id, copied from pendingNodeId in the communication reset */
+static uint8_t CO_activeNodeId = CO_LSS_NODE_ID_ASSIGNMENT;
+
+/* Data block for mainline data, which can be stored to non-volatile memory */
+typedef struct {
+ /* Pending CAN bit rate, can be set by argument or LSS slave. */
+ uint16_t pendingBitRate;
+ /* Pending CANopen NodeId, can be set by argument or LSS slave. */
+ uint8_t pendingNodeId;
+} mainlineStorage_t;
+
+mainlineStorage_t mlStorage = {
+ .pendingBitRate = 0,
+ .pendingNodeId = CO_LSS_NODE_ID_ASSIGNMENT
+};
+
+#if (CO_CONFIG_TRACE) & CO_CONFIG_TRACE_ENABLE
+static CO_time_t CO_time; /* Object for current time */
+#endif
+
+
+/* Helper functions ***********************************************************/
+#ifndef CO_SINGLE_THREAD
+/* Realtime thread */
+CO_epoll_t epRT;
+static void* rt_thread(void* arg);
+#endif
+
+/* Signal handler */
+volatile sig_atomic_t CO_endProgram = 0;
+static void sigHandler(int sig) {
+ (void)sig;
+ CO_endProgram = 1;
+}
+
+/* Message logging function */
+void log_printf(int priority, const char *format, ...) {
+ va_list ap;
+
+ va_start(ap, format);
+ vsyslog(priority, format, ap);
+ va_end(ap);
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG
+ if (CO != NULL) {
+ char buf[200];
+ time_t timer;
+ struct tm* tm_info;
+ size_t len;
+
+ timer = time(NULL);
+ tm_info = localtime(&timer);
+ len = strftime(buf, sizeof(buf), "%Y-%m-%d %H:%M:%S: ", tm_info);
+
+ va_start(ap, format);
+ vsnprintf(buf + len, sizeof(buf) - len - 2, format, ap);
+ va_end(ap);
+ strcat(buf, "\r\n");
+ CO_GTWA_log_print(CO->gtwa, buf);
+ }
+#endif
+}
+
+#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);
+}
+#endif
+
+#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";
+ case CO_NMT_PRE_OPERATIONAL: return "pre-operational";
+ 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)
+{
+ 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)
+{
+ (void)object;
+ 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;
+ 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");
+#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");
+#endif
+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");
+#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");
+}
+
+
+/*******************************************************************************
+ * Mainline thread
+ ******************************************************************************/
+int main (int argc, char *argv[]) {
+ int programExit = EXIT_SUCCESS;
+ CO_epoll_t epMain;
+#ifndef CO_SINGLE_THREAD
+ pthread_t rt_thread_id;
+ int rtPriority = -1;
+#endif
+ CO_NMT_reset_cmd_t reset = CO_RESET_NOT;
+ CO_ReturnError_t err;
+ CO_CANptrSocketCan_t CANptr = {0};
+ 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 */
+
+#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
+ };
+ uint8_t storageEntriesCount = sizeof(storageEntries) / sizeof(storageEntries[0]);
+ uint32_t storageInitError = 0;
+ uint32_t storageErrorPrev = 0;
+ uint32_t storageIntervalTimer = 0;
+#endif
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
+ CO_epoll_gtw_t epGtw;
+ /* 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;
+ uint32_t socketTimeout_ms = 0;
+#else
+ #define commandInterface 0
+ #define localSocketPath NULL
+#endif
+
+ /* configure system log */
+ 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){
+ printUsage(argv[0]);
+ exit(EXIT_SUCCESS);
+ }
+ 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);
+ break;
+ }
+#ifndef CO_SINGLE_THREAD
+ case 'p': rtPriority = strtol(optarg, NULL, 0);
+ break;
+#endif
+ 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-";
+ if (strcmp(optarg, comm_stdio) == 0) {
+ commandInterface = CO_COMMAND_IF_STDIO;
+ }
+ 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];
+ uint16_t port;
+ int nMatch = sscanf(portStr, "%hu", &port);
+ if(nMatch != 1) {
+ log_printf(LOG_CRIT, DBG_NOT_TCP_PORT, portStr);
+ exit(EXIT_FAILURE);
+ }
+ commandInterface = port;
+ }
+ else {
+ log_printf(LOG_CRIT, DBG_ARGUMENT_UNKNOWN, "-c", optarg);
+ exit(EXIT_FAILURE);
+ }
+ break;
+ }
+ case 'T':
+ socketTimeout_ms = strtoul(optarg, NULL, 0);
+ break;
+#endif
+#if (CO_CONFIG_STORAGE) & CO_CONFIG_STORAGE_ENABLE
+ case 's': {
+ /* add prefix to each storageEntries[i].filename */
+ for (uint8_t i = 0; i < storageEntriesCount; i++) {
+ char* filePrefix = optarg;
+ size_t filePrefixLen = strlen(filePrefix);
+ char* file = storageEntries[i].filename;
+ size_t fileLen = strlen(file);
+ if (fileLen + filePrefixLen < CO_STORAGE_PATH_MAX) {
+ memmove(&file[filePrefixLen], &file[0], fileLen + 1);
+ memcpy(&file[0], &filePrefix[0], filePrefixLen);
+ }
+ }
+ break;
+ }
+#endif
+ default:
+ printUsage(argv[0]);
+ exit(EXIT_FAILURE);
+ }
+ }
+
+ 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)
+ ) {
+ 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))) {
+ log_printf(LOG_CRIT, DBG_WRONG_PRIORITY, rtPriority);
+ printUsage(argv[0]);
+ exit(EXIT_FAILURE);
+ }
+#endif
+
+ 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");
+
+
+ /* Allocate memory for CANopen objects */
+ uint32_t heapMemoryUsed = 0;
+ CO = CO_new(NULL, &heapMemoryUsed);
+ if (CO == NULL) {
+ 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,
+ &storageInitError);
+
+ if (err != CO_ERROR_NO && err != CO_ERROR_DATA_CORRUPT) {
+ char *filename = storageInitError < storageEntriesCount
+ ? storageEntries[storageInitError].filename : "???";
+ log_printf(LOG_CRIT, DBG_STORAGE, filename);
+ exit(EXIT_FAILURE);
+ }
+#endif
+
+ /* Overwrite stored node-id, if specified by program arguments */
+ if (nodeIdFromArgs > 0) {
+ mlStorage.pendingNodeId = (uint8_t)nodeIdFromArgs;
+ }
+
+ /* Catch signals SIGINT and SIGTERM */
+ if(signal(SIGINT, sigHandler) == SIG_ERR) {
+ log_printf(LOG_CRIT, DBG_ERRNO, "signal(SIGINT, sigHandler)");
+ exit(EXIT_FAILURE);
+ }
+ if(signal(SIGTERM, sigHandler) == SIG_ERR) {
+ log_printf(LOG_CRIT, DBG_ERRNO, "signal(SIGTERM, sigHandler)");
+ exit(EXIT_FAILURE);
+ }
+
+ /* get current time for CO_TIME_set(), since January 1, 1984, UTC. */
+ struct timespec ts;
+ if (clock_gettime(CLOCK_REALTIME, &ts) == -1) {
+ log_printf(LOG_CRIT, DBG_GENERAL, "clock_gettime(main)", 0);
+ exit(EXIT_FAILURE);
+ }
+ uint16_t time_days = (uint16_t)(ts.tv_sec / (24 * 60 * 60));
+ time_days -= 5113; /* difference between Unix epoch and CANopen Epoch */
+ uint32_t time_ms = (uint32_t)(ts.tv_sec % (24 * 60 * 60)) * 1000;
+ time_ms += ts.tv_nsec / 1000000;
+
+ /* 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);
+ 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);
+ exit(EXIT_FAILURE);
+ }
+ CANptr.epoll_fd = epRT.epoll_fd;
+#else
+ 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) {
+ 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 *******************/
+ uint32_t errInfo;
+
+ /* Wait rt_thread. */
+ if(!firstRun) {
+ CO_LOCK_OD(CO->CANmodule);
+ CO->CANmodule->CANnormal = false;
+ CO_UNLOCK_OD(CO->CANmodule);
+ }
+
+ /* Enter CAN configuration. */
+ 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) {
+ 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) {
+ log_printf(LOG_CRIT, DBG_CAN_OPEN, "CO_LSSinit()", err);
+ programExit = EXIT_FAILURE;
+ CO_endProgram = 1;
+ continue;
+ }
+
+ 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 */
+ 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) {
+ if (err == CO_ERROR_OD_PARAMETERS) {
+ log_printf(LOG_CRIT, DBG_OD_ENTRY, errInfo);
+ }
+ else {
+ log_printf(LOG_CRIT, DBG_CAN_OPEN, "CO_CANopenInit()", err);
+ }
+ programExit = EXIT_FAILURE;
+ CO_endProgram = 1;
+ continue;
+ }
+
+ /* initialize part of threadMain and callbacks */
+ CO_epoll_initCANopenMain(&epMain, CO);
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
+ CO_epoll_initCANopenGtw(&epGtw, CO);
+#endif
+ CO_LSSslave_initCfgStoreCallback(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);
+#endif
+#if (CO_CONFIG_NMT) & CO_CONFIG_NMT_CALLBACK_CHANGE
+ 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);
+#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);
+ }
+#endif
+
+#if (CO_CONFIG_TRACE) & CO_CONFIG_TRACE_ENABLE
+ /* Initialize time */
+ 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 {
+ log_printf(LOG_INFO, DBG_CAN_OPEN_INFO, CO_activeNodeId, "node-id not initialized");
+ }
+
+ /* First time only initialization. */
+ 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) {
+ log_printf(LOG_CRIT, DBG_ERRNO, "pthread_create(rt_thread)");
+ programExit = EXIT_FAILURE;
+ CO_endProgram = 1;
+ continue;
+ }
+ if(rtPriority > 0) {
+ struct sched_param param;
+
+ param.sched_priority = rtPriority;
+ if (pthread_setschedparam(rt_thread_id, SCHED_FIFO, ¶m) != 0) {
+ log_printf(LOG_CRIT, DBG_ERRNO, "pthread_setschedparam()");
+ programExit = EXIT_FAILURE;
+ CO_endProgram = 1;
+ continue;
+ }
+ }
+#endif
+#ifdef CO_USE_APPLICATION
+ /* Execute optional additional application code */
+ errInfo = 0;
+ err = app_programStart(!CO->nodeIdUnconfigured, &errInfo);
+ if(err != CO_ERROR_NO) {
+ if (err == CO_ERROR_OD_PARAMETERS) {
+ log_printf(LOG_CRIT, DBG_OD_ENTRY, errInfo);
+ }
+ else {
+ log_printf(LOG_CRIT, DBG_CAN_OPEN, "app_programStart()", err);
+ }
+ programExit = EXIT_FAILURE;
+ CO_endProgram = 1;
+ continue;
+ }
+ if(errInfo != 0 && !CO->nodeIdUnconfigured) {
+ CO_errorReport(CO->em, CO_EM_INCONSISTENT_OBJECT_DICT,
+ CO_EMC_DATA_SET, errInfo);
+ }
+#endif
+ } /* if(firstRun) */
+
+
+#ifdef CO_USE_APPLICATION
+ /* Execute optional additional application code */
+ app_communicationReset(!CO->nodeIdUnconfigured);
+#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) {
+ if (err == CO_ERROR_OD_PARAMETERS) {
+ log_printf(LOG_CRIT, DBG_OD_ENTRY, errInfo);
+ }
+ else {
+ log_printf(LOG_CRIT, DBG_CAN_OPEN, "CO_CANopenInitPDO()", err);
+ }
+ programExit = EXIT_FAILURE;
+ CO_endProgram = 1;
+ continue;
+ }
+
+
+ /* start CAN */
+ CO_CANsetNormalMode(CO->CANmodule);
+
+ reset = CO_RESET_NOT;
+
+ log_printf(LOG_INFO, DBG_CAN_OPEN_INFO, CO_activeNodeId, "running ...");
+
+
+ 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);
+#endif
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
+ CO_epoll_processGtw(&epGtw, CO, &epMain);
+#endif
+ CO_epoll_processMain(&epMain, CO, GATEWAY_ENABLE, &reset);
+ CO_epoll_processLast(&epMain);
+
+#ifdef CO_USE_APPLICATION
+ app_programAsync(!CO->nodeIdUnconfigured, epMain.timeDifference_us);
+#endif
+
+#if (CO_CONFIG_STORAGE) & CO_CONFIG_STORAGE_ENABLE
+ /* don't save more often than interval */
+ if (storageIntervalTimer < CO_STORAGE_AUTO_INTERVAL) {
+ storageIntervalTimer += epMain.timeDifference_us;
+ }
+ 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 {
+ CO_errorReset(CO->em, CO_EM_NON_VOLATILE_AUTO_SAVE, 0);
+ }
+ }
+ storageErrorPrev = mask;
+ storageIntervalTimer = 0;
+ }
+#endif
+ }
+ } /* while(reset != CO_RESET_APP */
+
+
+/* program exit ***************************************************************/
+ /* join threads */
+ CO_endProgram = 1;
+#ifndef CO_SINGLE_THREAD
+ if (pthread_join(rt_thread_id, NULL) != 0) {
+ log_printf(LOG_CRIT, DBG_ERRNO, "pthread_join()");
+ exit(EXIT_FAILURE);
+ }
+#endif
+#ifdef CO_USE_APPLICATION
+ /* Execute optional additional application code */
+ app_programEnd();
+#endif
+
+#if (CO_CONFIG_STORAGE) & CO_CONFIG_STORAGE_ENABLE
+ CO_storageLinux_auto_process(&storage, true);
+#endif
+
+ /* delete objects from memory */
+#ifndef CO_SINGLE_THREAD
+ CO_epoll_close(&epRT);
+#endif
+ CO_epoll_close(&epMain);
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
+ CO_epoll_closeGtw(&epGtw);
+#endif
+ 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) {
+ sync();
+ if(reboot(LINUX_REBOOT_CMD_RESTART) != 0) {
+ log_printf(LOG_CRIT, DBG_ERRNO, "reboot()");
+ exit(EXIT_FAILURE);
+ }
+ }
+
+ exit(programExit);
+}
+
+#ifndef CO_SINGLE_THREAD
+/*******************************************************************************
+ * Realtime thread for CAN receive and threadTmr
+ ******************************************************************************/
+static void* rt_thread(void* arg) {
+ (void)arg;
+ /* Endless loop */
+ while(CO_endProgram == 0) {
+
+ CO_epoll_wait(&epRT);
+ CO_epoll_processRT(&epRT, CO, true);
+ CO_epoll_processLast(&epRT);
+
+#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++) {
+ CO_trace_process(CO->trace[i], *CO_time.epochTimeOffsetMs);
+ }
+#endif
+
+#ifdef CO_USE_APPLICATION
+ /* Execute optional additional application code */
+ app_programRt(!CO->nodeIdUnconfigured, epRT.timeDifference_us);
+#endif
+
+ }
+
+ return NULL;
+}
+#endif
--- /dev/null
+/*\r
+ * CANopen data storage object for Linux\r
+ *\r
+ * @file CO_storageLinux.c\r
+ * @author Janez Paternoster\r
+ * @copyright 2021 Janez Paternoster\r
+ *\r
+ * This file is part of CANopenNode, an opensource CANopen Stack.\r
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.\r
+ * For more information on CANopen see <http://www.can-cia.org/>.\r
+ *\r
+ * Licensed under the Apache License, Version 2.0 (the "License");\r
+ * you may not use this file except in compliance with the License.\r
+ * You may obtain a copy of the License at\r
+ *\r
+ * http://www.apache.org/licenses/LICENSE-2.0\r
+ *\r
+ * Unless required by applicable law or agreed to in writing, software\r
+ * distributed under the License is distributed on an "AS IS" BASIS,\r
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\r
+ * See the License for the specific language governing permissions and\r
+ * limitations under the License.\r
+ */\r
+\r
+#include "CO_storageLinux.h"\r
+#include "301/crc16-ccitt.h"\r
+\r
+#include <stdio.h>\r
+#include <string.h>\r
+#include <stdlib.h>\r
+\r
+#if (CO_CONFIG_STORAGE) & CO_CONFIG_STORAGE_ENABLE\r
+\r
+\r
+/*\r
+ * Function for writing data on "Store parameters" command - OD object 1010\r
+ *\r
+ * For more information see file CO_storage.h, CO_storage_entry_t.\r
+ */\r
+static ODR_t storeLinux(CO_storage_entry_t *entry, CO_CANmodule_t *CANmodule) {\r
+ ODR_t ret = ODR_OK;\r
+ uint16_t crc_store;\r
+\r
+ /* Create names for temporary and old file */\r
+ size_t fn_len = strlen(entry->filename) + 5;\r
+ char *filename_tmp = malloc(fn_len);\r
+ char *filename_old = malloc(fn_len);\r
+ if (filename_tmp == NULL || filename_old == NULL) {\r
+ if (filename_tmp != NULL) free(filename_tmp);\r
+ if (filename_old != NULL) free(filename_old);\r
+ ret = ODR_OUT_OF_MEM;\r
+ }\r
+ else {\r
+ strcpy(filename_tmp, entry->filename);\r
+ strcpy(filename_old, entry->filename);\r
+ strcat(filename_tmp, ".tmp");\r
+ strcat(filename_old, ".old");\r
+ }\r
+\r
+ /* Open a temporary file and write data to it */\r
+ if (ret == ODR_OK) {\r
+ FILE *fp = fopen(filename_tmp, "w");\r
+ if (fp == NULL) {\r
+ ret = ODR_HW;\r
+ }\r
+ else {\r
+ CO_LOCK_OD(CANmodule);\r
+ size_t cnt = fwrite(entry->addr, 1, entry->len, fp);\r
+ crc_store = crc16_ccitt(entry->addr, entry->len, 0);\r
+ CO_UNLOCK_OD(CANmodule);\r
+ cnt += fwrite(&crc_store, 1, sizeof(crc_store), fp);\r
+ fclose(fp);\r
+ if (cnt != (entry->len + sizeof(crc_store))) {\r
+ ret = ODR_HW;\r
+ }\r
+ }\r
+ }\r
+\r
+ /* Verify data */\r
+ if (ret == ODR_OK) {\r
+ uint8_t *buf = NULL;\r
+ FILE *fp = NULL;\r
+ size_t cnt = 0;\r
+ uint16_t crc_verify, crc_read;\r
+\r
+ buf = malloc(entry->len + 4);\r
+ if (buf != NULL) {\r
+ fp = fopen(filename_tmp, "r");\r
+ if (fp != NULL) {\r
+ cnt = fread(buf, 1, entry->len + 4, fp);\r
+ crc_verify = crc16_ccitt(buf, entry->len, 0);\r
+ fclose(fp);\r
+ memcpy(&crc_read, &buf[entry->len], sizeof(crc_read));\r
+ }\r
+ free(buf);\r
+ }\r
+ /* If size or CRC differs, report error */\r
+ if (buf == NULL || fp == NULL || cnt != (entry->len+sizeof(crc_verify))\r
+ || crc_store != crc_verify || crc_store != crc_read\r
+ ) {\r
+ ret = ODR_HW;\r
+ }\r
+ }\r
+\r
+ /* rename existing file to *.old and *.tmp to existing */\r
+ if (ret == ODR_OK) {\r
+ rename(entry->filename, filename_old);\r
+ if (rename(filename_tmp, entry->filename) != 0) {\r
+ ret = ODR_HW;\r
+ }\r
+ }\r
+\r
+ free(filename_tmp);\r
+ free(filename_old);\r
+\r
+ return ret;\r
+}\r
+\r
+\r
+/*\r
+ * Function for restoring data on "Restore default parameters" command - OD 1011\r
+ *\r
+ * For more information see file CO_storage.h, CO_storage_entry_t.\r
+ */\r
+static ODR_t restoreLinux(CO_storage_entry_t *entry, CO_CANmodule_t *CANmodule){\r
+ (void) CANmodule;\r
+ ODR_t ret = ODR_OK;\r
+\r
+ /* close the file first, if auto storage */\r
+ if ((entry->attr & CO_storage_auto) != 0 && entry->fp != NULL) {\r
+ fclose(entry->fp);\r
+ entry->fp = NULL;\r
+ }\r
+\r
+ /* Rename existing filename to *.old. */\r
+ char *filename_old = malloc(strlen(entry->filename) + 5);\r
+ if (filename_old == NULL) {\r
+ ret = ODR_OUT_OF_MEM;\r
+ }\r
+ else {\r
+ strcpy(filename_old, entry->filename);\r
+ strcat(filename_old, ".old");\r
+ rename(entry->filename, filename_old);\r
+ free(filename_old);\r
+ }\r
+\r
+ /* create an empty file and write "-\n" to it. */\r
+ if (ret == ODR_OK) {\r
+ FILE *fp = fopen(entry->filename, "w");\r
+ if (fp == NULL) {\r
+ ret = ODR_HW;\r
+ }\r
+ else {\r
+ fputs("-\n", fp);\r
+ fclose(fp);\r
+ }\r
+ }\r
+\r
+ return ret;\r
+}\r
+\r
+\r
+CO_ReturnError_t CO_storageLinux_init(CO_storage_t *storage,\r
+ CO_CANmodule_t *CANmodule,\r
+ OD_entry_t *OD_1010_StoreParameters,\r
+ OD_entry_t *OD_1011_RestoreDefaultParam,\r
+ CO_storage_entry_t *entries,\r
+ uint8_t entriesCount,\r
+ uint32_t *storageInitError)\r
+{\r
+ CO_ReturnError_t ret;\r
+\r
+ /* verify arguments */\r
+ if (storage == NULL || entries == NULL || entriesCount == 0\r
+ || storageInitError == NULL\r
+ ) {\r
+ return CO_ERROR_ILLEGAL_ARGUMENT;\r
+ }\r
+\r
+ storage->enabled = false;\r
+\r
+ /* initialize storage and OD extensions */\r
+ ret = CO_storage_init(storage,\r
+ CANmodule,\r
+ OD_1010_StoreParameters,\r
+ OD_1011_RestoreDefaultParam,\r
+ storeLinux,\r
+ restoreLinux,\r
+ entries,\r
+ entriesCount);\r
+ if (ret != CO_ERROR_NO) {\r
+ return ret;\r
+ }\r
+\r
+ /* initialize entries */\r
+ *storageInitError = 0;\r
+ for (uint8_t i = 0; i < entriesCount; i++) {\r
+ CO_storage_entry_t *entry = &entries[i];\r
+ bool_t dataCorrupt = false;\r
+ char *writeFileAccess = "w";\r
+\r
+ /* verify arguments */\r
+ if (entry->addr == NULL || entry->len == 0 || entry->subIndexOD < 2\r
+ || strlen(entry->filename) == 0\r
+ ) {\r
+ *storageInitError = i;\r
+ return CO_ERROR_ILLEGAL_ARGUMENT;\r
+ }\r
+\r
+ /* Open file, check existence and create temporary buffer */\r
+ uint8_t *buf = NULL;\r
+ FILE * fp = fopen(entry->filename, "r");\r
+ if (fp == NULL) {\r
+ dataCorrupt = true;\r
+ ret = CO_ERROR_DATA_CORRUPT;\r
+ }\r
+ else {\r
+ buf = malloc(entry->len + 4);\r
+ if (buf == NULL) {\r
+ fclose(fp);\r
+ *storageInitError = i;\r
+ return CO_ERROR_OUT_OF_MEMORY;\r
+ }\r
+ }\r
+\r
+ /* Read data into temporary buffer first. Then verify and copy to addr*/\r
+ if (!dataCorrupt) {\r
+ size_t cnt = fread(buf, 1, entry->len + 4, fp);\r
+\r
+ /* If file is empty, just skip loading, default values will be used,\r
+ * no error. Otherwise verify length and crc and copy data. */\r
+ if (!(cnt == 2 && buf[0] == '-')) {\r
+ uint16_t crc1, crc2;\r
+ crc1 = crc16_ccitt(buf, entry->len, 0);\r
+ memcpy(&crc2, &buf[entry->len], sizeof(crc2));\r
+\r
+ if (crc1 == crc2 && cnt == (entry->len + sizeof(crc2))) {\r
+ memcpy(entry->addr, buf, entry->len);\r
+ entry->crc = crc1;\r
+ writeFileAccess = "r+";\r
+ }\r
+ else {\r
+ dataCorrupt = true;\r
+ ret = CO_ERROR_DATA_CORRUPT;\r
+ }\r
+ }\r
+\r
+ free(buf);\r
+ fclose(fp);\r
+ }\r
+\r
+ /* additional info in case of error */\r
+ if (dataCorrupt) {\r
+ uint32_t errorBit = entry->subIndexOD;\r
+ if (errorBit > 31) errorBit = 31;\r
+ *storageInitError |= ((uint32_t) 1) << errorBit;\r
+ }\r
+\r
+ /* open file for auto storage, if set so */\r
+ if ((entry->attr & CO_storage_auto) != 0) {\r
+ entry->fp = fopen(entry->filename, writeFileAccess);\r
+ if (entry->fp == NULL) {\r
+ *storageInitError = i;\r
+ return CO_ERROR_ILLEGAL_ARGUMENT;\r
+ }\r
+ }\r
+ } /* for (entries) */\r
+\r
+ storage->enabled = true;\r
+ return ret;\r
+}\r
+\r
+\r
+uint32_t CO_storageLinux_auto_process(CO_storage_t *storage,\r
+ bool_t closeFiles)\r
+{\r
+ uint32_t storageError = 0;\r
+\r
+ /* verify arguments */\r
+ if (storage == NULL) {\r
+ return false;\r
+ }\r
+\r
+ /* loop through entries */\r
+ for (uint8_t i = 0; i < storage->entriesCount; i++) {\r
+ CO_storage_entry_t *entry = &storage->entries[i];\r
+\r
+ if ((entry->attr & CO_storage_auto) == 0 || entry->fp == NULL)\r
+ continue;\r
+\r
+ /* If CRC of the current data differs, save the file */\r
+ uint16_t crc = crc16_ccitt(entry->addr, entry->len, 0);\r
+ if (crc != entry->crc) {\r
+ size_t cnt;\r
+ rewind(entry->fp);\r
+ CO_LOCK_OD(storage->CANmodule);\r
+ cnt = fwrite(entry->addr, 1, entry->len, entry->fp);\r
+ CO_UNLOCK_OD(storage->CANmodule);\r
+ cnt += fwrite(&crc, 1, sizeof(crc), entry->fp);\r
+ fflush(entry->fp);\r
+ if (cnt == (entry->len + sizeof(crc))) {\r
+ entry->crc = crc;\r
+ }\r
+ else {\r
+ /* error with save */\r
+ uint32_t errorBit = entry->subIndexOD;\r
+ if (errorBit > 31) errorBit = 31;\r
+ storageError |= ((uint32_t) 1) << errorBit;\r
+ }\r
+ }\r
+\r
+ if (closeFiles) {\r
+ fclose(entry->fp);\r
+ entry->fp = NULL;\r
+ }\r
+ }\r
+\r
+ return storageError;\r
+}\r
+\r
+#endif /* (CO_CONFIG_STORAGE) & CO_CONFIG_STORAGE_ENABLE */\r
--- /dev/null
+/**\r
+ * CANopen data storage object for Linux\r
+ *\r
+ * @file CO_storageLinux.h\r
+ * @ingroup CO_storageLinux\r
+ * @author Janez Paternoster\r
+ * @copyright 2021 Janez Paternoster\r
+ *\r
+ * This file is part of CANopenNode, an opensource CANopen Stack.\r
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.\r
+ * For more information on CANopen see <http://www.can-cia.org/>.\r
+ *\r
+ * Licensed under the Apache License, Version 2.0 (the "License");\r
+ * you may not use this file except in compliance with the License.\r
+ * You may obtain a copy of the License at\r
+ *\r
+ * http://www.apache.org/licenses/LICENSE-2.0\r
+ *\r
+ * Unless required by applicable law or agreed to in writing, software\r
+ * distributed under the License is distributed on an "AS IS" BASIS,\r
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\r
+ * See the License for the specific language governing permissions and\r
+ * limitations under the License.\r
+ */\r
+\r
+#ifndef CO_STORAGE_LINUX_H\r
+#define CO_STORAGE_LINUX_H\r
+\r
+#include "storage/CO_storage.h"\r
+\r
+#if ((CO_CONFIG_STORAGE) & CO_CONFIG_STORAGE_ENABLE) || defined CO_DOXYGEN\r
+\r
+#ifdef __cplusplus\r
+extern "C" {\r
+#endif\r
+\r
+/**\r
+ * @defgroup CO_storageLinux Data storage with Linux\r
+ * @ingroup CO_socketCAN\r
+ * @{\r
+ *\r
+ * Data initialize, store and restore functions with Linux, see @ref CO_storage\r
+ */\r
+\r
+\r
+/**\r
+ * Initialize data storage object (Linux specific)\r
+ *\r
+ * This function should be called by application after the program startup,\r
+ * before @ref CO_CANopenInit(). This function initializes storage object,\r
+ * OD extensions on objects 1010 and 1011, reads data from file, verifies them\r
+ * and writes data to addresses specified inside entries. This function\r
+ * internally calls @ref CO_storage_init().\r
+ *\r
+ * @param storage This object will be initialized. It must be defined by\r
+ * application and must exist permanently.\r
+ * @param CANmodule CAN device, used for @ref CO_LOCK_OD() macro.\r
+ * @param OD_1010_StoreParameters OD entry for 0x1010 -"Store parameters".\r
+ * Entry is optional, may be NULL.\r
+ * @param OD_1011_RestoreDefaultParam OD entry for 0x1011 -"Restore default\r
+ * parameters". Entry is optional, may be NULL.\r
+ * @param entries Pointer to array of storage entries, see @ref CO_storage_init.\r
+ * @param entriesCount Count of storage entries\r
+ * @param [out] storageInitError If function returns CO_ERROR_DATA_CORRUPT,\r
+ * then this variable contains a bit mask from subIndexOD values, where data\r
+ * was not properly initialized. If other error, then this variable contains\r
+ * index or erroneous entry.\r
+ *\r
+ * @return CO_ERROR_NO, CO_ERROR_DATA_CORRUPT if data can not be initialized,\r
+ * CO_ERROR_ILLEGAL_ARGUMENT or CO_ERROR_OUT_OF_MEMORY.\r
+ */\r
+CO_ReturnError_t CO_storageLinux_init(CO_storage_t *storage,\r
+ CO_CANmodule_t *CANmodule,\r
+ OD_entry_t *OD_1010_StoreParameters,\r
+ OD_entry_t *OD_1011_RestoreDefaultParam,\r
+ CO_storage_entry_t *entries,\r
+ uint8_t entriesCount,\r
+ uint32_t *storageInitError);\r
+\r
+\r
+/**\r
+ * Automatically save data if differs from previous call.\r
+ *\r
+ * Should be called cyclically by program. Each interval it verifies, if crc\r
+ * checksum of data differs from previous checksum. If it does, data are saved\r
+ * into pre-opened file.\r
+ *\r
+ * @param storage This object\r
+ * @param closeFiles If true, then all files will be closed. Use on end of the\r
+ * program.\r
+ *\r
+ * @return 0 on success or bit mask from subIndexOD values, where data was not\r
+ * able to be saved.\r
+ */\r
+uint32_t CO_storageLinux_auto_process(CO_storage_t *storage,\r
+ bool_t closeFiles);\r
+\r
+/** @} */ /* CO_storageLinux */\r
+\r
+#ifdef __cplusplus\r
+}\r
+#endif /* __cplusplus */\r
+\r
+#endif /* (CO_CONFIG_STORAGE) & CO_CONFIG_STORAGE_ENABLE */\r
+\r
+#endif /* CO_STORAGE_LINUX_H */\r
--- /dev/null
+
+ Apache License
+ Version 2.0, January 2004
+ http://www.apache.org/licenses/
+
+ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+ 1. Definitions.
+
+ "License" shall mean the terms and conditions for use, reproduction,
+ and distribution as defined by Sections 1 through 9 of this document.
+
+ "Licensor" shall mean the copyright owner or entity authorized by
+ the copyright owner that is granting the License.
+
+ "Legal Entity" shall mean the union of the acting entity and all
+ other entities that control, are controlled by, or are under common
+ control with that entity. For the purposes of this definition,
+ "control" means (i) the power, direct or indirect, to cause the
+ direction or management of such entity, whether by contract or
+ otherwise, or (ii) ownership of fifty percent (50%) or more of the
+ outstanding shares, or (iii) beneficial ownership of such entity.
+
+ "You" (or "Your") shall mean an individual or Legal Entity
+ exercising permissions granted by this License.
+
+ "Source" form shall mean the preferred form for making modifications,
+ including but not limited to software source code, documentation
+ source, and configuration files.
+
+ "Object" form shall mean any form resulting from mechanical
+ transformation or translation of a Source form, including but
+ not limited to compiled object code, generated documentation,
+ and conversions to other media types.
+
+ "Work" shall mean the work of authorship, whether in Source or
+ Object form, made available under the License, as indicated by a
+ copyright notice that is included in or attached to the work
+ (an example is provided in the Appendix below).
+
+ "Derivative Works" shall mean any work, whether in Source or Object
+ form, that is based on (or derived from) the Work and for which the
+ editorial revisions, annotations, elaborations, or other modifications
+ represent, as a whole, an original work of authorship. For the purposes
+ of this License, Derivative Works shall not include works that remain
+ separable from, or merely link (or bind by name) to the interfaces of,
+ the Work and Derivative Works thereof.
+
+ "Contribution" shall mean any work of authorship, including
+ the original version of the Work and any modifications or additions
+ to that Work or Derivative Works thereof, that is intentionally
+ submitted to Licensor for inclusion in the Work by the copyright owner
+ or by an individual or Legal Entity authorized to submit on behalf of
+ the copyright owner. For the purposes of this definition, "submitted"
+ means any form of electronic, verbal, or written communication sent
+ to the Licensor or its representatives, including but not limited to
+ communication on electronic mailing lists, source code control systems,
+ and issue tracking systems that are managed by, or on behalf of, the
+ Licensor for the purpose of discussing and improving the Work, but
+ excluding communication that is conspicuously marked or otherwise
+ designated in writing by the copyright owner as "Not a Contribution."
+
+ "Contributor" shall mean Licensor and any individual or Legal Entity
+ on behalf of whom a Contribution has been received by Licensor and
+ subsequently incorporated within the Work.
+
+ 2. Grant of Copyright License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ copyright license to reproduce, prepare Derivative Works of,
+ publicly display, publicly perform, sublicense, and distribute the
+ Work and such Derivative Works in Source or Object form.
+
+ 3. Grant of Patent License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ (except as stated in this section) patent license to make, have made,
+ use, offer to sell, sell, import, and otherwise transfer the Work,
+ where such license applies only to those patent claims licensable
+ by such Contributor that are necessarily infringed by their
+ Contribution(s) alone or by combination of their Contribution(s)
+ with the Work to which such Contribution(s) was submitted. If You
+ institute patent litigation against any entity (including a
+ cross-claim or counterclaim in a lawsuit) alleging that the Work
+ or a Contribution incorporated within the Work constitutes direct
+ or contributory patent infringement, then any patent licenses
+ granted to You under this License for that Work shall terminate
+ as of the date such litigation is filed.
+
+ 4. Redistribution. You may reproduce and distribute copies of the
+ Work or Derivative Works thereof in any medium, with or without
+ modifications, and in Source or Object form, provided that You
+ meet the following conditions:
+
+ (a) You must give any other recipients of the Work or
+ Derivative Works a copy of this License; and
+
+ (b) You must cause any modified files to carry prominent notices
+ stating that You changed the files; and
+
+ (c) You must retain, in the Source form of any Derivative Works
+ that You distribute, all copyright, patent, trademark, and
+ attribution notices from the Source form of the Work,
+ excluding those notices that do not pertain to any part of
+ the Derivative Works; and
+
+ (d) If the Work includes a "NOTICE" text file as part of its
+ distribution, then any Derivative Works that You distribute must
+ include a readable copy of the attribution notices contained
+ within such NOTICE file, excluding those notices that do not
+ pertain to any part of the Derivative Works, in at least one
+ of the following places: within a NOTICE text file distributed
+ as part of the Derivative Works; within the Source form or
+ documentation, if provided along with the Derivative Works; or,
+ within a display generated by the Derivative Works, if and
+ wherever such third-party notices normally appear. The contents
+ of the NOTICE file are for informational purposes only and
+ do not modify the License. You may add Your own attribution
+ notices within Derivative Works that You distribute, alongside
+ or as an addendum to the NOTICE text from the Work, provided
+ that such additional attribution notices cannot be construed
+ as modifying the License.
+
+ You may add Your own copyright statement to Your modifications and
+ may provide additional or different license terms and conditions
+ for use, reproduction, or distribution of Your modifications, or
+ for any such Derivative Works as a whole, provided Your use,
+ reproduction, and distribution of the Work otherwise complies with
+ the conditions stated in this License.
+
+ 5. Submission of Contributions. Unless You explicitly state otherwise,
+ any Contribution intentionally submitted for inclusion in the Work
+ by You to the Licensor shall be under the terms and conditions of
+ this License, without any additional terms or conditions.
+ Notwithstanding the above, nothing herein shall supersede or modify
+ the terms of any separate license agreement you may have executed
+ with Licensor regarding such Contributions.
+
+ 6. Trademarks. This License does not grant permission to use the trade
+ names, trademarks, service marks, or product names of the Licensor,
+ except as required for reasonable and customary use in describing the
+ origin of the Work and reproducing the content of the NOTICE file.
+
+ 7. Disclaimer of Warranty. Unless required by applicable law or
+ agreed to in writing, Licensor provides the Work (and each
+ Contributor provides its Contributions) on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+ implied, including, without limitation, any warranties or conditions
+ of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+ PARTICULAR PURPOSE. You are solely responsible for determining the
+ appropriateness of using or redistributing the Work and assume any
+ risks associated with Your exercise of permissions under this License.
+
+ 8. Limitation of Liability. In no event and under no legal theory,
+ whether in tort (including negligence), contract, or otherwise,
+ unless required by applicable law (such as deliberate and grossly
+ negligent acts) or agreed to in writing, shall any Contributor be
+ liable to You for damages, including any direct, indirect, special,
+ incidental, or consequential damages of any character arising as a
+ result of this License or out of the use or inability to use the
+ Work (including but not limited to damages for loss of goodwill,
+ work stoppage, computer failure or malfunction, or any and all
+ other commercial damages or losses), even if such Contributor
+ has been advised of the possibility of such damages.
+
+ 9. Accepting Warranty or Additional Liability. While redistributing
+ the Work or Derivative Works thereof, You may choose to offer,
+ and charge a fee for, acceptance of support, warranty, indemnity,
+ or other liability obligations and/or rights consistent with this
+ License. However, in accepting such obligations, You may act only
+ on Your own behalf and on Your sole responsibility, not on behalf
+ of any other Contributor, and only if You agree to indemnify,
+ defend, and hold each Contributor harmless for any liability
+ incurred by, or claims asserted against, such Contributor by reason
+ of your accepting any such warranty or additional liability.
+
+ END OF TERMS AND CONDITIONS
+
+ APPENDIX: How to apply the Apache License to your work.
+
+ To apply the Apache License to your work, attach the following
+ boilerplate notice, with the fields enclosed by brackets "[]"
+ replaced with your own identifying information. (Don't include
+ the brackets!) The text should be enclosed in the appropriate
+ comment syntax for the file format. We also recommend that a
+ file or class name and description of purpose be included on the
+ same "printed page" as the copyright notice for easier
+ identification within third-party archives.
+
+ Copyright [yyyy] [name of copyright owner]
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
--- /dev/null
+# Makefile for CANopenNode with Linux socketCAN (with commander functionalities)
+
+
+DRV_SRC = .
+CANOPEN_SRC = CANopenNode
+APPL_SRC = CANopenNode/example
+
+
+LINK_TARGET = canopend
+
+
+INCLUDE_DIRS = \
+ -I$(DRV_SRC) \
+ -I$(CANOPEN_SRC) \
+ -I$(APPL_SRC)
+
+
+SOURCES = \
+ $(DRV_SRC)/CO_driver.c \
+ $(DRV_SRC)/CO_error.c \
+ $(DRV_SRC)/CO_epoll_interface.c \
+ $(DRV_SRC)/CO_storageLinux.c \
+ $(CANOPEN_SRC)/301/CO_ODinterface.c \
+ $(CANOPEN_SRC)/301/CO_NMT_Heartbeat.c \
+ $(CANOPEN_SRC)/301/CO_HBconsumer.c \
+ $(CANOPEN_SRC)/301/CO_Emergency.c \
+ $(CANOPEN_SRC)/301/CO_SDOserver.c \
+ $(CANOPEN_SRC)/301/CO_SDOclient.c \
+ $(CANOPEN_SRC)/301/CO_TIME.c \
+ $(CANOPEN_SRC)/301/CO_SYNC.c \
+ $(CANOPEN_SRC)/301/CO_PDO.c \
+ $(CANOPEN_SRC)/301/crc16-ccitt.c \
+ $(CANOPEN_SRC)/301/CO_fifo.c \
+ $(CANOPEN_SRC)/303/CO_LEDs.c \
+ $(CANOPEN_SRC)/304/CO_GFC.c \
+ $(CANOPEN_SRC)/304/CO_SRDO.c \
+ $(CANOPEN_SRC)/305/CO_LSSslave.c \
+ $(CANOPEN_SRC)/305/CO_LSSmaster.c \
+ $(CANOPEN_SRC)/309/CO_gateway_ascii.c \
+ $(CANOPEN_SRC)/storage/CO_storage.c \
+ $(CANOPEN_SRC)/extra/CO_trace.c \
+ $(CANOPEN_SRC)/CANopen.c \
+ $(APPL_SRC)/OD.c \
+ $(DRV_SRC)/CO_main_basic.c
+
+
+OBJS = $(SOURCES:%.c=%.o)
+CC ?= gcc
+OPT =
+OPT += -g
+#OPT += -O2
+OPT += -DCO_SINGLE_THREAD
+#OPT += -DCO_CONFIG_DEBUG=0xFFFF
+#OPT += -Wextra -Wshadow -pedantic -fanalyzer
+#OPT += -DCO_USE_GLOBALS
+#OPT += -DCO_MULTIPLE_OD
+CFLAGS = -Wall $(OPT) $(INCLUDE_DIRS)
+LDFLAGS =
+LDFLAGS += -g
+#LDFLAGS += -pthread
+
+#Options can be also passed via make: 'make OPT="-g" LDFLAGS="-pthread"'
+
+
+.PHONY: all clean
+
+all: clean $(LINK_TARGET)
+
+clean:
+ rm -f $(OBJS) $(LINK_TARGET)
+
+%.o: %.c
+ $(CC) $(CFLAGS) -c $< -o $@
+
+$(LINK_TARGET): $(OBJS)
+ $(CC) $(LDFLAGS) $^ -o $@
--- /dev/null
+CANopenLinux
+============
+
+CANopenLinux is a CANopen stack running on Linux devices.
+
+It is based on [CANopenNode](https://github.com/CANopenNode/CANopenNode), which is free and open source CANopen Stack and is included as a git submodule.
+
+CANopen is the internationally standardized (EN 50325-4) ([CiA301](http://can-cia.org/standardization/technical-documents)) CAN-based higher-layer protocol for embedded control system. For more information on CANopen see http://www.can-cia.org/.
+
+
+Getting or updating the project
+-------------------------------
+Clone the project from git repository and get submodules:
+
+ git clone https://github.com/CANopenNode/CANopenLinux.git
+ cd CANopenLinux
+ git submodule init
+ git submodule update
+
+Update the project:
+
+ cd CANopenLinux
+ git pull # or: git fetch; inspect the changes (gitk); git merge
+ git submodule update
+
+
+Usage
+-----
+Support for CAN interface is part of the Linux kernel, so called [SocketCAN](https://en.wikipedia.org/wiki/SocketCAN). CANopenNode runs on top of SocketCAN, so it should be able to run on any Linux machine, it depends on configuration of the kernel. Examples below was tested on Debian based machines, including Ubuntu and Raspberry PI. It is possible to run tests described below without real CAN interface, because Linux kernel already contains virtual CAN interface.
+
+Windows or Mac users, who don't have Linux installed, can use [VirtualBox](https://www.virtualbox.org/) and install [Ubuntu](https://ubuntu.com/download/desktop) or similar.
+
+
+### CAN interfaces
+#### Virtual CAN interface
+It can connect multiple programs inside Linux machine. It can be activated by the following commands:
+
+ sudo modprobe vcan
+ sudo ip link add dev can0 type vcan
+ sudo ip link set up can0
+
+#### USB, PCI or similar CAN interface
+There are several CAN interfaces on the market which works with Linux SocketCAN. See [Linux kernel source](https://git.kernel.org/cgit/linux/kernel/git/torvalds/linux.git/tree/drivers/net/can), Kconfig files, for supported interfaces by the Linux kernel. For example [EMS CPC-USB](https://www.ems-wuensche.com/?post_type=product&p=746) or [PCAN-USB FD](http://www.peak-system.com/PCAN-USB-FD.365.0.html?&L=1). Usually such interface is started with:
+
+ sudo ip link set up can0 type can bitrate 250000
+
+#### Serial slcan interface
+Most cheap CAN interface, for example [USBtin](http://www.fischl.de/usbtin/). It may not be fast enough and may lose messages. Usually it is started with (-s5=250kbps):
+
+ sudo slcand -f -o -c -s5 /dev/ttyACM0 can0
+ sudo ip link set up can0
+
+#### CAN capes for Raspberry PI or similar
+RPI may work wit some of the USB CAN interfaces or with CAN shield like [this](https://www.sg-electronic-systems.com/ecommerce/12-can-bus-shield). CAN shield may not be fast enough and may lose messages.
+
+
+### CAN utilities
+[SocketCAN userspace utilities and tools](https://github.com/linux-can/can-utils) contains several useful command line tools for CAN in Linux. Install with:
+
+ sudo apt-get install can-utils
+
+In own terminal run candump to display all CAN messages with timestamp:
+
+ candump -td -a can0
+
+
+### Running CANopenLinux device
+Compile:
+
+ cd CANopenNode
+ make
+
+Display options:
+
+ ./canopend --help
+
+Run on can0 device with CANopen NodeID = 4:
+
+ ./canopend can0 -i 4
+
+If NodeID is not specified, then CANopen LSS protocol may be used. Program can be finished by pressing Ctrl+c or with CANopen reset node command.
+
+After connecting the CANopen Linux device into the CAN(open) network, bootup message is visible. By default device uses Object Dictionary from `CANopenNode/example`, which contains only communication parameters. With the external CANopen tool all parameters can be accessed and CANopen Linux device can be configured (For example write heartbeat producer time in object 0x1017,0).
+
+When CANopen Linux device is first connected to the CANopen network it shows bootup message and emergency message, because are missing storage files. To avoid emergency message it is necessary to trigger saveAll command (write correct code into parameter 0x1010,1 with SDO command) and restart the program.
+
+Note also, if there are multiple instances of canopend running from the same directory, storage path should be specified for each.
+
+
+### CANopen ASCII command interface
+CANopenNode includes CANopen ASCII command interface (gateway) specified by standard CiA309-3. It can be used as a commander for other CANopen devices: NMT master, LSS master, SDO client, etc. In CANopen Linux device command interface is available by default.
+
+To use ASCII command interface on canopend directly just run it with `-c "stdio"` and type the commands followed by enter in it.
+
+ ./canopend can0 -i 1 -c "stdio"
+ help
+ 1 write 0x1010 1 vs save
+ 1 reset node
+
+To create CANopen Linux commander device on local socket run:
+
+ ./canopend can0 -i 1 -c "local-/tmp/CO_command_socket"
+
+#### cocomm
+CANopenLinux/cocomm directory contains a small command line program, which establishes socket connection with `canopend` (CANopen Linux commander device). It sends standardized CANopen commands (CiA309-3) to gateway and prints the responses to stdout and stderr. See [cocomm/cocomm.md](cocomm/cocomm.md) for usage.
+
+
+Creating new project
+--------------------
+`canopend` is a basic CANopen Linux device with optional commander functionalities. However, CANopen device is able to be much more, like (simple) input/output (digital or analog) device according to standard CiA401 or anything else as specified by other CANopen device profiles or own idea.
+
+New project can be started in new directory simply by adding customized makefile, custom Object Dictionary OD.h/c files and custom application source files in Arduino style, which are called from CO_main_basic.c file.
+
+See [CANopenSocket](https://github.com/CANopenNode/CANopenSocket) for demo.
+
+
+### Create new project with [KDevelop](https://www.kdevelop.org/)
+- `sudo apt install kdevelop breeze`
+- Run KDevelop, select: Project -> open project
+- Navigate to project directory and click open.
+- KDevelop will recognize `Makefile` and will just use it. Click Finish.
+- Open project settings (right click on project on left panel)
+ - Make: set to 1 thread operation.
+ - Language support, Includes, add paths to directories:
+ - `<path_to_CANopenLinux_driver_files>`
+ - `<path_to_CANopenNode>`
+ - `<path_to_project_files>`
+ - Language support, Defines, add:
+ - `CO_DRIVER_CUSTOM`
+- Run -> Setup launches -> basicDevice:
+ - Add Executable file and name it.
+ - Executable file: `<select_executable>`
+ - Arguments: `can0 -i 4`
+- Build, then Execute or Debug
+
+
+Change Log
+----------
+- **[v4.0](https://github.com/CANopenNode/CANopenLinux/tree/HEAD) - current**: Git repository started on GitHub.
+ - Linux driver files copied from https://github.com/CANopenNode/CANopenNode/tree/76b43c88ef6d5490cb2f1518e10646e8dcb45c76/socketCAN
+ - cocomm copied from https://github.com/CANopenNode/CANopenSocket/tree/71f21e41fd4527718d8b6161938b479386d2d03b/cocomm
+ - Submodule CANopenNode added.
+ - Few adjustments and documentation written.
+
+
+License
+-------
+This file is part of CANopenNode, an opensource CANopen Stack.
+Project home page is <https://github.com/CANopenNode/CANopenNode>.
+For more information on CANopen see <http://www.can-cia.org/>.
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
--- /dev/null
+# Makefile for CANopenCommand.
+
+APPL_SRC = .
+LINK_TARGET = cocomm
+INCLUDE_DIRS = -I$(APPL_SRC)
+SOURCES = $(APPL_SRC)/cocomm.c
+
+OBJS = $(SOURCES:%.c=%.o)
+CC ?= gcc
+OPT = -g
+#OPT = -g -pedantic -Wshadow -fanalyzer
+CFLAGS = -Wall $(OPT) $(INCLUDE_DIRS)
+LDFLAGS =
+
+.PHONY: all clean
+
+all: clean $(LINK_TARGET)
+
+clean:
+ rm -f $(OBJS) $(LINK_TARGET)
+
+install:
+ cp $(LINK_TARGET) /usr/bin/$(LINK_TARGET)
+
+%.o: %.c
+ $(CC) $(CFLAGS) -c $< -o $@
+
+$(LINK_TARGET): $(OBJS)
+ $(CC) $(LDFLAGS) $^ -o $@
--- /dev/null
+/*
+ * Client socket command interface for CANopenSocket.
+ *
+ * @file cocomm.c
+ * @author Janez Paternoster
+ * @copyright 2020 Janez Paternoster
+ *
+ * This file is part of CANopenSocket, a Linux implementation of CANopen
+ * stack with master functionality. Project home page is
+ * <https://github.com/CANopenNode/CANopenSocket>. CANopenSocket is based
+ * on CANopenNode: <https://github.com/CANopenNode/CANopenNode>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <unistd.h>
+#include <bits/getopt_core.h>
+#include <string.h>
+#include <netdb.h>
+#include <limits.h>
+#include <net/if.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/un.h>
+#include <linux/can.h>
+#include <signal.h>
+
+#ifndef BUF_SIZE
+#define BUF_SIZE 1000
+#endif
+#define BUF_LAG 100 /* max size of error response */
+
+/* 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/CANopenSocket\n"
+"\n", progName, progName);
+}
+
+/* print reply, status to errStream (red or green), value to stdout */
+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) {
+ perror("replyBuf malloc");
+ exit(EXIT_FAILURE);
+ }
+
+ for (;;) {
+ ssize_t nRead = read(fd_gtw, &replyBuf[count], BUF_SIZE); /* blocking*/
+
+ 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
+ ) {
+ fprintf(errStream, "%s%s%s", redC, replyBuf, resetC);
+ ret = EXIT_FAILURE;
+ break;
+ }
+ 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");
+ if (seq != NULL && (size_t)(seq - replyBuf) < 15) {
+ replyBufTrimmed = seq + 2;
+ seq[1] = 0;
+ count -= strlen(replyBuf) + 1;
+ fprintf(errStream, "%s%s%s ", greenC, replyBuf, resetC);
+ }
+ if (end != NULL) {
+ end[0] = 0;
+ fputs(replyBufTrimmed, stdout);
+ fflush(stdout);
+ fputs("\r\n", errStream);
+ break;
+ }
+ else {
+ /* print replyBuf to stdout, except last BUF_LAG bytes.
+ * move them to the beginning of the replyBuf */
+ if (count > BUF_LAG) {
+ size_t nWrite = count - BUF_LAG;
+ fwrite(replyBufTrimmed, 1, nWrite, stdout);
+ replyBufTrimmed += nWrite;
+ count = BUF_LAG;
+ }
+ memmove(replyBuf, replyBufTrimmed, count);
+ }
+ }
+ firstPass = 0;
+ }
+ else {
+ char *end = strstr(replyBuf, "\r\n");
+ if (end != NULL) {
+ 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 {
+ end[0] = 0;
+ fputs(replyBuf, stdout);
+ fflush(stdout);
+ fprintf(errStream, "\n%s...success%s\r\n", greenC, resetC);
+ }
+ break;
+ }
+ /* print replyBuf to stdout, except last BUF_LAG bytes.
+ * move them to the beginning of the replyBuf */
+ if (count > BUF_LAG) {
+ size_t nWrite = count - BUF_LAG;
+ fwrite(replyBuf, 1, nWrite, stdout);
+ count = BUF_LAG;
+ memmove(replyBuf, &replyBuf[nWrite], count);
+ }
+ }
+ }
+ else if (nRead == 0) {
+ fprintf(errStream, "%sError, zero response%s\n", redC, resetC);
+ ret = EXIT_FAILURE;
+ break;
+ }
+ else {
+ perror("Socket read failed");
+ free(replyBuf);
+ exit(EXIT_FAILURE);
+ }
+ fflush(stdout);
+ }
+ free(replyBuf);
+
+ return ret;
+}
+
+
+/******************************************************************************/
+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 */
+ int additionalReadStdin = 0;
+ char *candump = NULL;
+ long candumpCount = 10;
+ long candumpTmo = 1000;
+
+ char* commBuf;
+ int fd_gtw;
+ int fd_candump;
+ int opt;
+ struct sockaddr_un addr_un;
+ sa_family_t addrFamily = AF_UNIX;
+ errStream = stderr;
+
+ if(argc >= 2 && strcmp(argv[1], "--help") == 0) {
+ printUsage(argv[0]);
+ exit(EXIT_SUCCESS);
+ }
+
+ /* Get program options from environment variables */
+ char *env;
+ if ((env = getenv("cocomm_host")) != NULL) {
+ strncpy(hostname, env, sizeof(hostname));
+ addrFamily = AF_INET;
+ }
+ if ((env = getenv("cocomm_port")) != NULL) {
+ strncpy(tcpPort, env, sizeof(tcpPort));
+ }
+ if ((env = getenv("cocomm_socket")) != NULL) {
+ socketPath = env;
+ addrFamily = AF_UNIX;
+ }
+ if ((env = getenv("cocomm_flat")) != NULL) {
+ if (strcmp(env, "1") == 0) {
+ outputType = out_flat;
+ }
+ }
+ if ((env = getenv("cocomm_candump")) != NULL) {
+ candump = env;
+ }
+ if ((env = getenv("cocomm_candump_count")) != NULL) {
+ candumpCount = atol(env);
+ }
+ if ((env = getenv("cocomm_candump_timeout")) != NULL) {
+ candumpTmo = atol(env);
+ }
+
+ /* Get program options from arguments */
+ while((opt = getopt(argc, argv, "f:s:t:p:io:d:n:T:")) != -1) {
+ switch (opt) {
+ case 'f':
+ inputFilePath = optarg;
+ break;
+ case 's':
+ addrFamily = AF_UNIX;
+ socketPath = optarg;
+ break;
+ case 't':
+ addrFamily = AF_INET;
+ strncpy(hostname, optarg, sizeof(hostname));
+ break;
+ case 'p':
+ strncpy(tcpPort, optarg, sizeof(tcpPort));
+ break;
+ case 'i':
+ additionalReadStdin = 1;
+ break;
+ case 'o':
+ if (strcmp(optarg, "data") == 0)
+ outputType = out_data;
+ 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);
+ }
+ }
+
+ switch (outputType) {
+ default:
+ case out_all:
+ greenC = "\033[32m";
+ redC = "\033[31m";
+ resetC = "\033[0m";
+ errStream = stderr;
+ break;
+ case out_data:
+ greenC = redC = resetC = "";
+ errStream = fopen("/dev/null", "w+");
+ break;
+ case out_flat:
+ greenC = redC = resetC = "";
+ errStream = stdout;
+ break;
+ }
+
+ /* Ignore the SIGPIPE signal, which may happen, if gateway broke
+ * the connection. Program will exit with error message anyway.
+ * Signal may be triggered by socket write call. */
+ if (signal(SIGPIPE, SIG_IGN) == SIG_ERR) {
+ perror("signal");
+ exit(EXIT_FAILURE);
+ }
+
+ /* Create and connect client socket */
+ if(addrFamily == AF_INET) {
+
+ struct addrinfo hints, *res, *rp;
+ int errcode;
+
+ memset(&hints, 0, sizeof(hints));
+ hints.ai_family = AF_INET;
+ hints.ai_socktype = SOCK_STREAM;
+ hints.ai_flags |= AI_CANONNAME;
+
+ errcode = getaddrinfo(hostname, tcpPort, &hints, &res);
+ if (errcode != 0) {
+ fprintf(stderr, "Error! Getaddrinfo for host %s failed\n", hostname);
+ exit(EXIT_FAILURE);
+ }
+
+ /* getaddrinfo() returns a list of address structures. Try each address
+ * until we successfully connect. If socket (or connect) fails,
+ * we (close the socket and) try the next address. */
+
+ for (rp = res; rp != NULL; rp = rp->ai_next) {
+ fd_gtw = socket(rp->ai_family, rp->ai_socktype, rp->ai_protocol);
+ if (fd_gtw == -1) {
+ continue;
+ }
+
+ if (connect(fd_gtw, rp->ai_addr, rp->ai_addrlen) != -1) {
+ break; /* Success */
+ }
+
+ close(fd_gtw);
+ perror("Socket connection failed");
+ exit(EXIT_FAILURE);
+ }
+ }
+ else { // addrFamily == AF_UNIX
+ fd_gtw = socket(AF_UNIX, SOCK_STREAM, 0);
+ if(fd_gtw == -1) {
+ perror("Socket creation failed");
+ exit(EXIT_FAILURE);
+ }
+
+ memset(&addr_un, 0, sizeof(struct sockaddr_un));
+ 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) {
+ fprintf(stderr, "Socket connection failed \"%s\": ", socketPath);
+ perror(NULL);
+ exit(EXIT_FAILURE);
+ }
+ }
+
+ /* Prepare candump */
+ if (candump != NULL && candumpCount > 0 && candumpTmo > 0) {
+ struct sockaddr_can sockAddr;
+
+ fd_candump = socket(PF_CAN, SOCK_RAW, CAN_RAW);
+ if (fd_candump < 0) {
+ perror("CAN socket creation failed");
+ exit(EXIT_FAILURE);
+ }
+
+ struct timeval tv;
+ tv.tv_sec = candumpTmo / 1000;
+ tv.tv_usec = (candumpTmo % 1000) * 1000;
+ setsockopt(fd_candump, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof tv);
+
+ memset(&sockAddr, 0, sizeof(sockAddr));
+ sockAddr.can_family = AF_CAN;
+ sockAddr.can_ifindex = if_nametoindex(candump);
+ int ret = bind(fd_candump, (struct sockaddr*)&sockAddr, sizeof(sockAddr));
+ if (ret < 0) {
+ fprintf(stderr, "CAN Socket binding failed \"%s\": ", candump);
+ perror(NULL);
+ exit(EXIT_FAILURE);
+ }
+ }
+ else {
+ candumpCount = 0;
+ }
+
+ /* get commands from input file, line after line */
+ int ret = EXIT_SUCCESS;
+
+ commBuf = malloc(BUF_SIZE);
+ if(commBuf == NULL) {
+ perror("commBuf malloc");
+ exit(EXIT_FAILURE);
+ }
+
+ 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) {
+ size_t len = strlen(commBuf);
+ if (len < 1) continue;
+
+ // send command
+ if (write(fd_gtw, commBuf, len) != len) { /* blocking function */
+ perror("Socket write failed");
+ free(commBuf);
+ exit(EXIT_FAILURE);
+ }
+
+ // print reply, if command is complete
+ if (commBuf[len - 1] == '\n') {
+ if (printReply(fd_gtw) == EXIT_FAILURE) {
+ ret = EXIT_FAILURE;
+ }
+ }
+ }
+
+ fclose(fp);
+ }
+
+ /* get command from arguments */
+ 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] != '#') {
+ sprintf(commBuf, "[%d] ", i - optind + 1);
+ }
+
+ if((strlen(commBuf) + strlen(comm)) >= (BUF_SIZE - 2)) {
+ fprintf(errStream, "%sCommand string too long!%s\n", redC, greenC);
+ continue;
+ }
+
+ strcat(commBuf, comm);
+
+ if (additionalReadStdin == 0) {
+ strcat(commBuf, "\n");
+ size_t len = strlen(commBuf);
+ if (write(fd_gtw, commBuf, len) != len) { /* blocking function */
+ perror("Socket write failed");
+ free(commBuf);
+ exit(EXIT_FAILURE);
+ }
+ }
+ else {
+ strcat(commBuf, " ");
+ size_t len = strlen(commBuf);
+ char lastChar;
+
+ do {
+ 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')
+ continue;
+
+ // send command
+ if (write(fd_gtw, commBuf, len) != len) { /* blocking f. */
+ perror("Socket write failed");
+ free(commBuf);
+ exit(EXIT_FAILURE);
+ }
+
+ commBuf[0] = 0;
+ len = 0;
+ } while (lastChar != '\n');
+ }
+
+ if (printReply(fd_gtw) == EXIT_FAILURE) {
+ ret = EXIT_FAILURE;
+ }
+ }
+ }
+
+ /* get commands from stdin, line after line */
+ else {
+ while(fgets(commBuf, BUF_SIZE, stdin) != NULL) {
+ size_t len = strlen(commBuf);
+ if (len < 1) continue;
+
+ // send command
+ if (write(fd_gtw, commBuf, len) != len) { /* blocking function */
+ perror("Socket write failed");
+ free(commBuf);
+ exit(EXIT_FAILURE);
+ }
+
+ // print reply, if command is complete
+ if (commBuf[len - 1] == '\n') {
+ if (printReply(fd_gtw) == EXIT_FAILURE) {
+ ret = EXIT_FAILURE;
+ }
+ }
+ }
+ }
+
+ close(fd_gtw);
+
+ free(commBuf);
+
+ /* candump output */
+ if (candumpCount > 0) {
+ for (int i = 0; i < candumpCount; i++) {
+ const char hex_asc[] = "0123456789ABCDEF";
+ struct can_frame canFrame;
+
+ ssize_t nbytes = read(fd_candump, &canFrame, sizeof(struct can_frame));
+
+ if (nbytes < (ssize_t)sizeof(struct can_frame)) {
+ perror("CAN raw socket read timeout");
+ exit(EXIT_FAILURE);
+ }
+
+ char buf[17];
+ char* pbuf = &buf[0];
+ for (int j = 0; j < canFrame.can_dlc && j < sizeof(buf); j++) {
+ uint8_t byte = canFrame.data[j];
+ *pbuf++ = hex_asc[byte >> 4];
+ *pbuf++ = hex_asc[byte & 0x0F];
+ }
+ *pbuf = 0;
+
+ printf ("%03X#%s\n", canFrame.can_id, buf);
+ }
+ close(fd_candump);
+ }
+
+ exit(ret);
+}
--- /dev/null
+Client socket interface to CANopenNode ASCII command interface
+==============================================================
+
+`cocomm` is a small command line program, which establishes socket connection with `canopend` (CANopen Linux commander device). It sends standardized CANopen commands (CiA309-3) to gateway and prints the responses to stdout and stderr. It is similar to command `nc -U /tmp/CO_command_socket`, but adjusted to CANopen.
+
+
+Compile and install
+-------------------
+ cd cocomm
+ make
+ sudo make install
+
+This will compile the `cocomm` utility and copy it to the /usr/bin/ directory.
+
+
+Example usage
+-------------
+ cocomm --help
+ cocomm "help"
+ cocomm "help datatype"
+ cocomm "help lss"
+ cocomm "1 read 0x1017 0 u16"
+ cocomm "1 write 0x1017 0 u16 1000"
+ cocomm "1 w 0x1010 1 vs save"
+ cocomm "1 reset node"
+
+Example will display usage help, read Heartbeat producer time from CANopen device with NodeId = 1, write 1000 ms to the same variable, store all non-volatile data on the device and reset the device. (Suppose CANopen device with NodeId = 1 is our CANopen Linux commander device. After 'reset node' command, our device will be stopped and cocomm won't work any more. Of course, cocomm can access any CANopen device, just by specifying it's node ID.)
+
+Parameters to program can be set by program arguments, as described in `cocomm --help`, and can also be changed by environmental variables. For example, to change default socket path for all next `cocomm` commands in current terminal, type:
+
+ export cocomm_socket="some other path than /tmp/CO_command_socket"
+
+Commands can be also written into a file, for example create a `commands.txt` file, and for its content enter the commands:
+
+ [1] r 0x1017 0 u16
+ [2] 1 start
+
+Then make `cocomm` use that file:
+
+ $ cocomm -f commands.txt
+ [1] 1000
+ [2] OK
+
+Program writes data to stdout and messages in green or red color to stderr.
+
+For more examples see [CANopenSocket](https://github.com/CANopenNode/CANopenSocket).
+
+
+Background about communication paths, when using cocomm
+-------------------------------------------------------
+
+1. `canopend` serves a socket connection on `/tmp/CO_command_socket` address. This is local Unix socket, TCP socket can be used also. `canopend` is pure CANopen device with commander functionalities and gateway. It listens for socket connections.
+2. When run, `cocomm` tries to connect to `/tmp/CO_command_socket` address (this is default setting). `canopend` accepts the connection.
+3. `cocomm` writes the specified ascii command to established socket connection, for example `[1] 4 read 0x1017 0 u16`.
+4. Gateway in `canopend` receives the command and decodes it. `read` commands goes internally into `CO_SDOclientUploadInitiate()` and then command is processed with multiple `CO_SDOclientUpload()` function calls.
+5. `CO_SDOclientUpload()` now sends a CAN message to targeted CANopen device. (CAN interface in Linux is implemented with CAN sockets. This is the third type of sockets mentioned here.) However, in our example targeted CANopen device receives SDO request, asking the value of the variable, located in Object Dictionary at index 0x1017, subindex 0.
+6. Targeted CANopen device receives CAN message with CAN ID=0x604. It determines SDO request, so `CO_SDOserver_process()` function gets the message. Function gets the value from internal Object Dictionary and sends the CAN response with CAN ID=0x584. Those messages can be seen in candump terminal. And it is not necessary to understand the details of SDO communication, it may be quite complex.
+7. `canopend` receives the CAN message, `CO_SDOclientUpload()` decodes it and sends binary value to the gateway.
+8. Gateway in `canopend` translates binary value to asciiValue, unsigned16 in our example. It prepares the response, in our case `[1] ` + asciiValue + `\r\n`. Then writes the response text back to `/tmp/CO_command_socket`.
+9. `cocomm` reads the response text from local socket and prints it partly to stderr (`[1] \r\n`) and partly to stdout (asciiValue).
+10. If there are more commands, step 3 is repeated. Otherwise `cocomm` closes the socket connection and exits.
projects / 0xmirror / CANopenLinux.git / commitdiff