nikon/CANopenNode
1
0
Fork 0
Code Activity Actions

Reorganize socketCAN/CO_Linux_threads

Browse source 
- Rename files to CO_epoll_interface
 - Object oriented, more flexible, separate functions
 - move epoll, timerfd and eventfd system calls from CO_driver.c to CO_epoll_interface.c
This commit is contained in:
Janez 2020-09-29 09:00:31 +02:00
parent ac4ca2893a
commit 23bc5d38a7
11 changed files with 1091 additions and 966 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
Makefile
View file

@ -20,7 +20,7 @@ INCLUDE_DIRS = \
SOURCES = \
$(DRV_SRC)/CO_driver.c \
$(DRV_SRC)/CO_error.c \
$(DRV_SRC)/CO_Linux_threads.c \
$(DRV_SRC)/CO_epoll_interface.c \
$(DRV_SRC)/CO_OD_storage.c \
$(CANOPEN_SRC)/301/CO_ODinterface.c \
$(CANOPEN_SRC)/301/CO_SDOserver.c \

3
README.md
View file

@ -179,13 +179,14 @@ File structure
- **CO_driver.c** - Interface between Linux socketCAN and CANopenNode.
- **CO_error.h/.c** - Linux socketCAN Error handling object.
- **CO_error_msgs.h** - Error definition strings and logging function.
- **CO_Linux_threads.h/.c** - Helper functions for implementing CANopen threads in Linux.
- **CO_epoll_interface.h/.c** - Helper functions for Linux epoll interface to CANopenNode.
- **CO_OD_storage.h/.c** - Object Dictionary storage object for Linux SocketCAN.
- **CO_main_basic.c** - Mainline for socketCAN (basic usage).
- **doc/** - Directory with documentation
- **CHANGELOG.md** - Change Log file.
- **deviceSupport.md** - Information about supported devices.
- **gettingStarted.md, LSSusage.md, traceUsage.md** - Getting started and usage.
- **objectDictionary.md** - Description of CANopen object dictionary interface.
- **index.html** - Soft link to html/md_README.html.
- **html** - Directory with documentation - must be generated by Doxygen.
- **CANopen.h/.c** - Initialization and processing of CANopen objects.

1
doc/CHANGELOG.md
View file

@ -30,6 +30,7 @@ Change Log
- CO_Linux_threads.h, function `void CANrx_threadTmr_init(uint16_t interval_in_milliseconds (changed to) uint32_t interval_in_microseconds)`
- CO_CANrxBufferInit(): remove check COB ID already used.
- change macros CO_DRIVER_MULTI_INTERFACE and CO_DRIVER_ERROR_REPORTING. To enable(disable), set to 1(0).
- Rename CO_Linux_threads.h/.c to CO_epoll_interface.h/.c and reorganize them. Move epoll, timerfd and eventfd system calls from CO_driver.c to here.
### Fixed
- Bugfix in `CO_HBconsumer_process()`: argument `timeDifference_us` was set to 0 inside for loop, fixed now.
- BUG in CO_HBconsumer.c #168

587
socketCAN/CO_Linux_threads.c
View file

@ -1,587 +0,0 @@
/*
* Helper functions for implementing CANopen threads in Linux
*
* @file Linux_threads.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 "CANopen.h"
#include "CO_Linux_threads.h"
#include <stdlib.h>
#include <unistd.h>
#include <syslog.h>
#include <time.h>
#include <sys/epoll.h>
#include <sys/eventfd.h>
#include <sys/timerfd.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>
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII */
#ifndef LISTEN_BACKLOG
#define LISTEN_BACKLOG 50
#endif
/* Helper function - get monotonic clock time in microseconds */
static inline uint64_t CO_LinuxThreads_clock_gettime_us(void)
{
struct timespec ts;
(void)clock_gettime(CLOCK_MONOTONIC, &ts);
return ts.tv_sec * 1000000 + ts.tv_nsec / 1000;
}
#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) {
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 {
log_printf(LOG_DEBUG, DBG_ERRNO, "write(gtwa_response)");
}
}
return nWritten;
}
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII */
/* Mainline thread - Blocking (threadMainWait) ********************************/
static struct {
uint64_t start; /* time value CO_process() was called last time in us */
int epoll_fd; /* epoll file descriptor */
int event_fd; /* notification event file descriptor */
int timer_fd; /* interval timer file descriptor */
uint32_t interval_us; /* interval for threadMainWait_process */
struct itimerspec tm; /* structure for timer configuration */
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
int32_t commandInterface; /* command interface type or tcp port */
uint32_t socketTimeout_us;
uint32_t socketTimeoutTmr_us;
char *localSocketPath;/* path in case of local socket */
int gtwa_fdSocket; /* gateway socket file descriptor */
int gtwa_fd; /* gateway io stream file descriptor */
bool_t freshCommand;
#endif
} tmw;
static void threadMainWait_callback(void *object)
{
(void)object;
/* send event to wake threadMainWait_process() */
uint64_t u = 1;
ssize_t s;
s = write(tmw.event_fd, &u, sizeof(uint64_t));
if (s != sizeof(uint64_t)) {
log_printf(LOG_DEBUG, DBG_ERRNO, "write()");
}
}
void threadMainWait_init(bool_t CANopenConfiguredOK)
{
/* Configure LSS slave callback function */
#if (CO_CONFIG_LSS) & CO_CONFIG_LSS_SLAVE
CO_LSSslave_initCallbackPre(CO->LSSslave, NULL, threadMainWait_callback);
#endif
/* Initial value for time calculation */
tmw.start = CO_LinuxThreads_clock_gettime_us();
if (!CANopenConfiguredOK)
return;
/* Configure callback functions */
CO_NMT_initCallbackPre(CO->NMT, NULL, threadMainWait_callback);
CO_SDO_initCallbackPre(CO->SDO[0], NULL, threadMainWait_callback);
CO_EM_initCallbackPre(CO->em, NULL, threadMainWait_callback);
CO_HBconsumer_initCallbackPre(CO->HBcons, NULL, threadMainWait_callback);
#if CO_NO_SDO_CLIENT != 0
CO_SDOclient_initCallbackPre(CO->SDOclient[0], NULL,
threadMainWait_callback);
#endif
#if (CO_CONFIG_TIME) & CO_CONFIG_FLAG_CALLBACK_PRE
CO_TIME_initCallbackPre(CO->TIME, NULL, threadMainWait_callback);
#endif
#if (CO_CONFIG_LSS) & CO_CONFIG_FLAG_CALLBACK_PRE
#if (CO_CONFIG_LSS) & CO_CONFIG_LSS_MASTER
CO_LSSmaster_initCallbackPre(CO->LSSmaster, NULL, threadMainWait_callback);
#endif
#endif
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
CO_GTWA_initRead(CO->gtwa, gtwa_write_response, (void *)&tmw.gtwa_fd);
tmw.freshCommand = true;
#endif
}
void threadMainWait_initOnce(uint32_t interval_us,
int32_t commandInterface,
uint32_t socketTimeout_ms,
char *localSocketPath)
{
int ret;
struct epoll_event ev;
/* Configure epoll for mainline */
tmw.epoll_fd = epoll_create(1);
if (tmw.epoll_fd < 0) {
log_printf(LOG_CRIT, DBG_ERRNO, "epoll_create()");
exit(EXIT_FAILURE);
}
/* Configure eventfd for notifications and add it to epoll */
tmw.event_fd = eventfd(0, EFD_NONBLOCK);
if (tmw.event_fd < 0) {
log_printf(LOG_CRIT, DBG_ERRNO, "eventfd()");
exit(EXIT_FAILURE);
}
ev.events = EPOLLIN;
ev.data.fd = tmw.event_fd;
ret = epoll_ctl(tmw.epoll_fd, EPOLL_CTL_ADD, ev.data.fd, &ev);
if (ret < 0) {
log_printf(LOG_CRIT, DBG_ERRNO, "epoll_ctl(event_fd)");
exit(EXIT_FAILURE);
}
/* Configure timer for interval_us and add it to epoll */
tmw.timer_fd = timerfd_create(CLOCK_MONOTONIC, TFD_NONBLOCK);
if (tmw.timer_fd < 0) {
log_printf(LOG_CRIT, DBG_ERRNO, "timerfd_create()");
exit(EXIT_FAILURE);
}
tmw.interval_us = interval_us;
tmw.tm.it_interval.tv_sec = interval_us / 1000000;
tmw.tm.it_interval.tv_nsec = (interval_us % 1000000) * 1000;
tmw.tm.it_value.tv_sec = 0;
tmw.tm.it_value.tv_nsec = 1;
ret = timerfd_settime(tmw.timer_fd, 0, &tmw.tm, NULL);
if (ret < 0) {
log_printf(LOG_CRIT, DBG_ERRNO, "timerfd_settime");
exit(EXIT_FAILURE);
}
ev.events = EPOLLIN;
ev.data.fd = tmw.timer_fd;
ret = epoll_ctl(tmw.epoll_fd, EPOLL_CTL_ADD, ev.data.fd, &ev);
if (ret < 0) {
log_printf(LOG_CRIT, DBG_ERRNO, "epoll_ctl(timer_fd)");
exit(EXIT_FAILURE);
}
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
/* Configure gateway-ascii command interface (CiA309-3) */
tmw.commandInterface = commandInterface;
tmw.socketTimeout_us = (socketTimeout_ms < (UINT_MAX / 1000 - 1000000)) ?
socketTimeout_ms * 1000 : (UINT_MAX - 1000000);
tmw.gtwa_fdSocket = -1;
tmw.gtwa_fd = -1;
if (commandInterface == CO_COMMAND_IF_STDIO) {
tmw.gtwa_fd = STDIN_FILENO;
log_printf(LOG_INFO, DBG_COMMAND_STDIO_INFO);
}
else if (commandInterface == CO_COMMAND_IF_LOCAL_SOCKET) {
struct sockaddr_un addr;
tmw.localSocketPath = localSocketPath;
/* Create, bind and listen local socket */
tmw.gtwa_fdSocket = socket(AF_UNIX, SOCK_STREAM | SOCK_NONBLOCK, 0);
if(tmw.gtwa_fdSocket < 0) {
log_printf(LOG_CRIT, DBG_ERRNO, "socket(local)");
exit(EXIT_FAILURE);
}
memset(&addr, 0, sizeof(struct sockaddr_un));
addr.sun_family = AF_UNIX;
strncpy(addr.sun_path, localSocketPath, sizeof(addr.sun_path) - 1);
ret = bind(tmw.gtwa_fdSocket, (struct sockaddr *) &addr,
sizeof(struct sockaddr_un));
if(ret < 0) {
log_printf(LOG_CRIT, DBG_COMMAND_LOCAL_BIND, localSocketPath);
exit(EXIT_FAILURE);
}
ret = listen(tmw.gtwa_fdSocket, LISTEN_BACKLOG);
if(ret < 0) {
log_printf(LOG_CRIT, DBG_ERRNO, "listen(local)");
exit(EXIT_FAILURE);
}
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 */
tmw.gtwa_fdSocket = socket(AF_INET, SOCK_STREAM | SOCK_NONBLOCK, 0);
if(tmw.gtwa_fdSocket < 0) {
log_printf(LOG_CRIT, DBG_ERRNO, "socket(tcp)");
exit(EXIT_FAILURE);
}
setsockopt(tmw.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(tmw.gtwa_fdSocket, (struct sockaddr *) &addr,
sizeof(struct sockaddr_in));
if(ret < 0) {
log_printf(LOG_CRIT, DBG_COMMAND_TCP_BIND, commandInterface);
exit(EXIT_FAILURE);
}
ret = listen(tmw.gtwa_fdSocket, LISTEN_BACKLOG);
if(ret < 0) {
log_printf(LOG_CRIT, DBG_ERRNO, "listen(tcp)");
exit(EXIT_FAILURE);
}
log_printf(LOG_INFO, DBG_COMMAND_TCP_INFO, commandInterface);
}
else {
tmw.commandInterface = CO_COMMAND_IF_DISABLED;
}
if (tmw.gtwa_fd >= 0) {
ev.events = EPOLLIN;
ev.data.fd = tmw.gtwa_fd;
ret = epoll_ctl(tmw.epoll_fd, EPOLL_CTL_ADD, ev.data.fd, &ev);
if (ret < 0) {
log_printf(LOG_CRIT, DBG_ERRNO, "epoll_ctl(gtwa_fd)");
exit(EXIT_FAILURE);
}
}
if (tmw.gtwa_fdSocket >= 0) {
/* prepare epool 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 = tmw.gtwa_fdSocket;
ret = epoll_ctl(tmw.epoll_fd, EPOLL_CTL_ADD, ev.data.fd, &ev);
if (ret < 0) {
log_printf(LOG_CRIT, DBG_ERRNO, "epoll_ctl(gtwa_fdSocket)");
exit(EXIT_FAILURE);
}
}
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII */
}
void threadMainWait_close(void)
{
close(tmw.epoll_fd);
tmw.epoll_fd = -1;
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
if (tmw.commandInterface == CO_COMMAND_IF_LOCAL_SOCKET) {
if (tmw.gtwa_fd > 0) {
close(tmw.gtwa_fd);
}
close(tmw.gtwa_fdSocket);
/* Remove local socket file from filesystem. */
if(remove(tmw.localSocketPath) < 0) {
log_printf(LOG_CRIT, DBG_ERRNO, "remove(local)");
}
}
else if (tmw.commandInterface >= CO_COMMAND_IF_TCP_SOCKET_MIN) {
if (tmw.gtwa_fd > 0) {
close(tmw.gtwa_fd);
}
close(tmw.gtwa_fdSocket);
}
tmw.gtwa_fd = -1;
tmw.gtwa_fdSocket = -1;
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII */
close(tmw.event_fd);
tmw.event_fd = -1;
close(tmw.timer_fd);
tmw.timer_fd = -1;
}
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
static inline void socetAcceptEnableForEpoll(void) {
struct epoll_event ev;
int ret;
ev.events = EPOLLIN | EPOLLONESHOT;
ev.data.fd = tmw.gtwa_fdSocket;
ret = epoll_ctl(tmw.epoll_fd, EPOLL_CTL_MOD, ev.data.fd, &ev);
if (ret < 0) {
log_printf(LOG_CRIT, DBG_ERRNO, "epoll_ctl(gtwa_fdSocket)");
}
}
#endif
uint32_t threadMainWait_process(CO_NMT_reset_cmd_t *reset)
{
int ready;
struct epoll_event ev;
uint64_t ull;
ssize_t s;
uint32_t diff, timerNext_us;
/* wait for event or timer expiration and read data from file descriptors */
ready = epoll_wait(tmw.epoll_fd, &ev, 1, -1);
if (ready != 1 && errno != EINTR) {
log_printf(LOG_DEBUG, DBG_ERRNO, "epoll_wait");
}
else if (ev.data.fd == tmw.event_fd) {
s = read(tmw.event_fd, &ull, sizeof(uint64_t));
if (s != sizeof(uint64_t)) {
log_printf(LOG_DEBUG, DBG_ERRNO, "read(event_fd)");
}
}
else if (ev.data.fd == tmw.timer_fd) {
s = read(tmw.timer_fd, &ull, sizeof(uint64_t));
if (s != sizeof(uint64_t) && errno != EAGAIN) {
log_printf(LOG_DEBUG, DBG_ERRNO, "read(timer_fd)");
}
}
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
else if (ev.data.fd == tmw.gtwa_fdSocket) {
bool_t fail = false;
tmw.gtwa_fd = accept4(tmw.gtwa_fdSocket, NULL, NULL, SOCK_NONBLOCK);
if (tmw.gtwa_fd < 0) {
fail = true;
if (errno != EAGAIN && errno != EWOULDBLOCK) {
log_printf(LOG_CRIT, DBG_ERRNO, "accept(gtwa_fdSocket)");
}
}
else {
int ret;
/* add fd to epoll */
struct epoll_event ev2;
ev2.events = EPOLLIN;
ev2.data.fd = tmw.gtwa_fd;
ret = epoll_ctl(tmw.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)");
}
tmw.socketTimeoutTmr_us = 0;
}
if (fail) {
socetAcceptEnableForEpoll();
}
}
else if (ev.data.fd == tmw.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);
s = read(tmw.gtwa_fd, buf, space);
if (CO->nodeIdUnconfigured) {
/* purge data */
}
else if (s < 0 && errno != EAGAIN) {
log_printf(LOG_DEBUG, DBG_ERRNO, "read(gtwa_fd)");
}
else if (s >= 0) {
if (tmw.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 && tmw.freshCommand
) {
CO_GTWA_write(CO->gtwa, sequence, strlen(sequence));
}
tmw.freshCommand = closed;
CO_GTWA_write(CO->gtwa, buf, s);
}
else { /* socket, local or tcp */
if (s == 0) {
int ret;
/* EOF received, close connection and enable socket accept*/
ret = epoll_ctl(tmw.epoll_fd, EPOLL_CTL_DEL,
tmw.gtwa_fd, NULL);
if (ret < 0) {
log_printf(LOG_CRIT, DBG_ERRNO,
"epoll_ctl(del, gtwa_fd)");
}
if (close(tmw.gtwa_fd) < 0) {
log_printf(LOG_CRIT, DBG_ERRNO, "close(gtwa_fd)");
}
tmw.gtwa_fd = -1;
socetAcceptEnableForEpoll();
}
else {
CO_GTWA_write(CO->gtwa, buf, s);
}
}
}
tmw.socketTimeoutTmr_us = 0;
}
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII */
/* calculate time difference since last call */
ull = CO_LinuxThreads_clock_gettime_us();
diff = (uint32_t)(ull - tmw.start);
tmw.start = ull;
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
/* if socket connection is established, verify timeout */
if (tmw.socketTimeout_us > 0 && tmw.gtwa_fdSocket > 0 && tmw.gtwa_fd > 0) {
if (tmw.socketTimeoutTmr_us > tmw.socketTimeout_us) {
int ret;
/* timout expired, close current connection and accept next */
ret = epoll_ctl(tmw.epoll_fd, EPOLL_CTL_DEL, tmw.gtwa_fd, NULL);
if (ret < 0) {
log_printf(LOG_CRIT, DBG_ERRNO, "epoll_ctl(del, gtwa_fd), tmo");
}
if (close(tmw.gtwa_fd) < 0) {
log_printf(LOG_CRIT, DBG_ERRNO, "close(gtwa_fd), tmo");
}
tmw.gtwa_fd = -1;
socetAcceptEnableForEpoll();
}
else {
tmw.socketTimeoutTmr_us += diff;
}
}
#endif
/* stack will lower this, if necessary */
timerNext_us = tmw.interval_us;
/* process CANopen objects */
*reset = CO_process(CO, diff, &timerNext_us);
/* lower next timer interval if necessary */
if (timerNext_us < tmw.interval_us) {
int ret;
/* add one microsecond extra delay and make sure it is not zero */
timerNext_us += 1;
if (tmw.interval_us < 1000000) {
tmw.tm.it_value.tv_nsec = timerNext_us * 1000;
}
else {
tmw.tm.it_value.tv_sec = timerNext_us / 1000000;
tmw.tm.it_value.tv_nsec = (timerNext_us % 1000000) * 1000;
}
ret = timerfd_settime(tmw.timer_fd, 0, &tmw.tm, NULL);
if (ret < 0) {
log_printf(LOG_DEBUG, DBG_ERRNO, "timerfd_settime");
}
}
return diff;
}
/* Realtime thread (threadRT) *************************************************/
static struct {
uint32_t us_interval; /* configured interval in us */
int interval_fd; /* timer fd */
} threadRT;
void CANrx_threadTmr_init(uint32_t interval_us)
{
struct itimerspec itval;
threadRT.us_interval = interval_us;
/* set up non-blocking interval timer */
threadRT.interval_fd = timerfd_create(CLOCK_MONOTONIC, 0);
(void)fcntl(threadRT.interval_fd, F_SETFL, O_NONBLOCK);
itval.it_interval.tv_sec = 0;
itval.it_interval.tv_nsec = interval_us * 1000;
itval.it_value = itval.it_interval;
(void)timerfd_settime(threadRT.interval_fd, 0, &itval, NULL);
}
void CANrx_threadTmr_close(void)
{
(void)close(threadRT.interval_fd);
threadRT.interval_fd = -1;
}
uint32_t CANrx_threadTmr_process(void)
{
int32_t result;
uint64_t i;
bool_t syncWas;
uint64_t missed = 0;
result = CO_CANrxWait(CO->CANmodule[0], threadRT.interval_fd, NULL);
if (result < 0) {
result = read(threadRT.interval_fd, &missed, sizeof(missed));
if (result > 0) {
/* at least one timer interval occurred */
CO_LOCK_OD();
if(CO->CANmodule[0]->CANnormal) {
for (i = 0; i <= missed; i++) {
#if CO_NO_SYNC == 1
/* Process Sync */
syncWas = CO_process_SYNC(CO, threadRT.us_interval, NULL);
#else
syncWas = false;
#endif
/* Read inputs */
CO_process_RPDO(CO, syncWas);
/* Write outputs */
CO_process_TPDO(CO, syncWas, threadRT.us_interval, NULL);
}
}
CO_UNLOCK_OD();
}
}
return (uint32_t) missed;
}

173
socketCAN/CO_Linux_threads.h
View file

@ -1,173 +0,0 @@
/**
* Helper functions for implementing CANopen threads in Linux.
*
* @file CO_Linux_threads.h
* @ingroup CO_socketCAN
* @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.
*/
#ifndef CO_LINUX_THREADS_H
#define CO_LINUX_THREADS_H
#include "309/CO_gateway_ascii.h"
#ifdef __cplusplus
extern "C" {
#endif
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
/**
* 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;
#endif
/**
* @defgroup CO_socketCAN socketCAN
* @{
*
* Linux specific interface to CANopenNode.
*
* CANopenNode runs on top of SocketCAN interface, which is part of the Linux
* kernel. For more info on Linux SocketCAN see
* https://www.kernel.org/doc/html/latest/networking/can.html
*
* CANopenNode runs in two threads:
* - timer based real-time thread for CAN receive, SYNC and PDO, see
* CANrx_threadTmr_process()
* - mainline thread for other processing, see threadMainWait_process()
*
* The "threads" specified here do not fork threads themselves, but require
* that two threads are provided by the calling application.
*
* Main references for Linux functions used here are Linux man pages and the
* book: The Linux Programming Interface by Michael Kerrisk.
*/
/**
* Initialize mainline thread - blocking.
*
* Function must be called always in communication reset section, after
* CO_CANopenInit().
*
* @param CANopenConfiguredOK True, if node has successfully passed NMT
* initialization - it has a valid CANopen node-id, all CANopen objects
* are configured and CANopen runs normally.
*/
void threadMainWait_init(bool_t CANopenConfiguredOK);
/**
* Initialize once mainline thread - blocking.
*
* Function must be called only once, before node starts operating.
*
* @param interval_us Interval of the threadMainWait_process()
* @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
*/
void threadMainWait_initOnce(uint32_t interval_us,
int32_t commandInterface,
uint32_t socketTimeout_ms,
char *localSocketPath);
/**
* Cleanup mainline thread - blocking.
*/
void threadMainWait_close(void);
/**
* Process mainline thread - blocking.
*
* threadMainWait is non-realtime thread for CANopenNode processing. It is
* initialized by threadMainWait_init(). There is no configuration for CANopen
* objects. But there is configuration for epool, interval timer and eventfd.
* Function must be called inside loop. It blocks for correct time and unblocks
* automatically in case of event. It calls CO_process() function for processing
* mainline CANopen objects.
* For more basic function see threadMain_process() alternative.
*
* @param reset return value from CO_process() function.
*
* @return time difference since last call in microseconds
*/
uint32_t threadMainWait_process(CO_NMT_reset_cmd_t *reset);
/**
* Initialize realtime thread.
*
* @param interval_us Interval of periodic timer in microseconds, recommended
* value for realtime response: 1000 us
*/
void CANrx_threadTmr_init(uint32_t interval_us);
/**
* Terminate realtime thread.
*/
void CANrx_threadTmr_close(void);
/**
* Process real-time thread.
*
* CANrx_threadTmr is realtime thread for CANopenNode processing. It is
* initialized by CANrx_threadTmr_init(). There is no configuration for CANopen
* objects. But configuration for epool event notification facility is included
* in CO_CANmodule_init() from CO_driver.c. Epool is configured to monitor the
* following file descriptors: notify pipe, CANrx sockets from all interfaces
* and interval timer.
*
* CANrx_threadTmr_process() blocks on epoll_wait(). This is implemented inside
* CO_CANrxWait() from CO_driver.c. New CAN message is processed in
* CANrx_threadTmr_process() function, which calls CO_process_SYNC(),
* CO_process_RPDO() and CO_process_TPDO() functions for each expired timer
* interval. This function must be called inside an infinite loop.
*
* @remark If realtime is required, this thread must be registered as such in
* the Linux kernel.
*
* @return Number of interval timer passes since last call.
*/
uint32_t CANrx_threadTmr_process(void);
/** @} */
#ifdef __cplusplus
}
#endif /*__cplusplus*/
#endif /* CO_LINUX_THREADS_H */

224
socketCAN/CO_driver.c
View file

@ -35,7 +35,6 @@
#include <sys/socket.h>
#include <asm/socket.h>
#include <sys/eventfd.h>
#include <sys/epoll.h>
#include <time.h>
#include "301/CO_driver.h"
@ -47,8 +46,7 @@ pthread_mutex_t CO_OD_mutex = PTHREAD_MUTEX_INITIALIZER;
#if CO_DRIVER_MULTI_INTERFACE == 0
static CO_ReturnError_t CO_CANmodule_addInterface(CO_CANmodule_t *CANmodule,
int can_ifindex,
int epoll_fd);
int can_ifindex);
#endif
@ -207,40 +205,17 @@ CO_ReturnError_t CO_CANmodule_init(
{
int32_t ret;
uint16_t i;
struct epoll_event ev;
(void)CANbitRate;
/* verify arguments */
if(CANmodule==NULL || rxArray==NULL || txArray==NULL){
if(CANmodule==NULL || CANptr == NULL || rxArray==NULL || txArray==NULL) {
return CO_ERROR_ILLEGAL_ARGUMENT;
}
/* Create epoll FD */
CANmodule->fdEpoll = epoll_create(1);
if(CANmodule->fdEpoll < 0){
log_printf(LOG_DEBUG, DBG_ERRNO, "epoll_create()");
CO_CANmodule_disable(CANmodule);
return CO_ERROR_SYSCALL;
}
/* Create notification event */
CANmodule->fdEvent = eventfd(0, EFD_NONBLOCK);
if (CANmodule->fdEvent < 0) {
log_printf(LOG_DEBUG, DBG_ERRNO, "eventfd");
CO_CANmodule_disable(CANmodule);
return CO_ERROR_OUT_OF_MEMORY;
}
/* ...and add it to epoll */
ev.events = EPOLLIN;
ev.data.fd = CANmodule->fdEvent;
ret = epoll_ctl(CANmodule->fdEpoll, EPOLL_CTL_ADD, ev.data.fd, &ev);
if(ret < 0){
log_printf(LOG_DEBUG, DBG_ERRNO, "epoll_ctl(eventfd)");
CO_CANmodule_disable(CANmodule);
return CO_ERROR_SYSCALL;
}
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;
@ -249,7 +224,6 @@ CO_ReturnError_t CO_CANmodule_init(
CANmodule->txSize = txSize;
CANmodule->CANerrorStatus = 0;
CANmodule->CANnormal = false;
CANmodule->fdTimerRead = -1;
#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;
@ -278,14 +252,8 @@ CO_ReturnError_t CO_CANmodule_init(
#if CO_DRIVER_MULTI_INTERFACE == 0
/* add one interface */
if (CANptr == NULL) {
CO_CANmodule_disable(CANmodule);
return CO_ERROR_ILLEGAL_ARGUMENT;
}
CO_CANptrSocketCan_t *CANptrReal = (CO_CANptrSocketCan_t *)CANptr;
ret = CO_CANmodule_addInterface(CANmodule,
CANptrReal->can_ifindex,
CANptrReal->epoll_fd);
CANptrReal->can_ifindex);
if (ret != CO_ERROR_NO) {
CO_CANmodule_disable(CANmodule);
return ret;
@ -300,8 +268,7 @@ CO_ReturnError_t CO_CANmodule_init(
static
#endif
CO_ReturnError_t CO_CANmodule_addInterface(CO_CANmodule_t *CANmodule,
int can_ifindex,
int epoll_fd)
int can_ifindex)
{
int32_t ret;
int32_t tmp;
@ -331,7 +298,6 @@ CO_ReturnError_t CO_CANmodule_addInterface(CO_CANmodule_t *CANmodule,
interface = &CANmodule->CANinterfaces[CANmodule->CANinterfaceCount - 1];
interface->can_ifindex = can_ifindex;
interface->epoll_fd = epoll_fd;
ifName = if_indextoname(can_ifindex, interface->ifName);
if (ifName == NULL) {
log_printf(LOG_DEBUG, DBG_ERRNO, "if_indextoname()");
@ -408,7 +374,7 @@ CO_ReturnError_t CO_CANmodule_addInterface(CO_CANmodule_t *CANmodule,
/* Add socket to epoll */
ev.events = EPOLLIN;
ev.data.fd = interface->fd;
ret = epoll_ctl(CANmodule->fdEpoll, EPOLL_CTL_ADD, ev.data.fd, &ev);
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;
@ -425,12 +391,13 @@ CO_ReturnError_t CO_CANmodule_addInterface(CO_CANmodule_t *CANmodule,
void CO_CANmodule_disable(CO_CANmodule_t *CANmodule)
{
uint32_t i;
struct timespec wait;
if (CANmodule == NULL) {
return;
}
CANmodule->CANnormal = false;
/* clear interfaces */
for (i = 0; i < CANmodule->CANinterfaceCount; i++) {
CO_CANinterface_t *interface = &CANmodule->CANinterfaces[i];
@ -439,34 +406,15 @@ void CO_CANmodule_disable(CO_CANmodule_t *CANmodule)
CO_CANerror_disable(&interface->errorhandler);
#endif
epoll_ctl(CANmodule->fdEpoll, EPOLL_CTL_DEL, interface->fd, NULL);
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->CANinterfaceCount = 0;
/* cancel rx */
if (CANmodule->fdEvent != -1) {
uint64_t u = 1;
ssize_t s;
s = write(CANmodule->fdEvent, &u, sizeof(uint64_t));
if (s != sizeof(uint64_t)) {
log_printf(LOG_DEBUG, DBG_ERRNO, "write()");
}
/* give some time for delivery */
wait.tv_sec = 0;
wait.tv_nsec = 50 /* ms */ * 1000000;
nanosleep(&wait, NULL);
close(CANmodule->fdEvent);
}
if (CANmodule->fdEpoll >= 0) {
close(CANmodule->fdEpoll);
}
CANmodule->fdEpoll = -1;
CANmodule->CANinterfaces = NULL;
if (CANmodule->rxFilter != NULL) {
free(CANmodule->rxFilter);
@ -890,113 +838,69 @@ static int32_t CO_CANrxMsg( /* return index of received message
/******************************************************************************/
int32_t CO_CANrxWait(CO_CANmodule_t *CANmodule, int fdTimer, CO_CANrxMsg_t *buffer)
bool_t CO_CANrxFromEpoll(CO_CANmodule_t *CANmodule,
struct epoll_event *ev,
CO_CANrxMsg_t *buffer,
int32_t *msgIndex)
{
int32_t retval;
int32_t ret;
int can_ifindex = 0;
CO_ReturnError_t err;
CO_CANinterface_t *interface = NULL;
struct epoll_event ev[1];
struct can_frame msg;
struct timespec timestamp;
if (CANmodule==NULL || CANmodule->CANinterfaceCount==0) {
return -1;
if (CANmodule == NULL || ev == NULL || CANmodule->CANinterfaceCount == 0) {
return false;
}
if (fdTimer>=0 && fdTimer!=CANmodule->fdTimerRead) {
/* new timer, timer changed */
epoll_ctl(CANmodule->fdEpoll, EPOLL_CTL_DEL, CANmodule->fdTimerRead, NULL);
ev[0].events = EPOLLIN;
ev[0].data.fd = fdTimer;
ret = epoll_ctl(CANmodule->fdEpoll, EPOLL_CTL_ADD, ev[0].data.fd, &ev[0]);
if(ret < 0){
return -1;
}
CANmodule->fdTimerRead = fdTimer;
}
/* Verify for epoll events in CAN socket */
for (uint32_t i = 0; i < CANmodule->CANinterfaceCount; i ++) {
CO_CANinterface_t *interface = &CANmodule->CANinterfaces[i];
/*
* blocking read using epoll
*/
do {
errno = 0;
ret = epoll_wait(CANmodule->fdEpoll, ev, sizeof(ev) / sizeof(ev[0]), -1);
if (errno == EINTR) {
/* try again */
continue;
}
else if (ret < 0) {
/* epoll failed */
return -1;
}
else if ((ev[0].events & (EPOLLERR | EPOLLHUP)) != 0) {
/* epoll detected close/error on socket. Try to pull event */
errno = 0;
recv(ev[0].data.fd, &msg, sizeof(msg), MSG_DONTWAIT);
log_printf(LOG_DEBUG, DBG_CAN_RX_EPOLL, ev[0].events, strerror(errno));
continue;
}
else if ((ev[0].events & EPOLLIN) != 0) {
/* one of the sockets is ready */
if ((ev[0].data.fd == CANmodule->fdEvent) ||
(ev[0].data.fd == fdTimer)) {
/* timer or notification event */
return -1;
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 {
/* CAN socket */
uint32_t i;
else if ((ev->events & EPOLLIN) != 0) {
struct can_frame msg;
struct timespec timestamp;
for (i = 0; i < CANmodule->CANinterfaceCount; i ++) {
interface = &CANmodule->CANinterfaces[i];
/* get message */
CO_ReturnError_t err = CO_CANread(CANmodule, interface,
&msg, &timestamp);
if (ev[0].data.fd == interface->fd) {
/* get interface handle */
can_ifindex = interface->can_ifindex;
/* get message */
err = CO_CANread(CANmodule, interface, &msg, &timestamp);
if (err != CO_ERROR_NO) {
return -1;
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;
}
/* no need to continue search */
break;
}
}
}
}
} while (errno != 0);
/*
* evaluate Rx
*/
retval = -1;
if(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 */
int32_t msgIndex;
#if CO_DRIVER_ERROR_REPORTING > 0
/* clear listenOnly and noackCounter if necessary */
CO_CANerror_rxMsg(&interface->errorhandler);
#endif
msgIndex = CO_CANrxMsg(CANmodule, &msg, buffer);
if (msgIndex > -1) {
/* Store message info */
CANmodule->rxArray[msgIndex].timestamp = timestamp;
CANmodule->rxArray[msgIndex].can_ifindex = can_ifindex;
else {
log_printf(LOG_DEBUG, DBG_EPOLL_UNKNOWN,
ev->events, ev->data.fd);
}
retval = msgIndex;
}
return true;
} /* if (ev->data.fd == interface->fd) */
}
return retval;
return false;
}

44
socketCAN/CO_driver_target.h
View file

@ -40,6 +40,7 @@
#include <pthread.h>
#include <linux/can.h>
#include <net/if.h>
#include <sys/epoll.h>
#ifdef CO_DRIVER_CUSTOM
#include "CO_driver_custom.h"
@ -309,8 +310,6 @@ typedef struct {
typedef struct {
int can_ifindex; /* CAN Interface index */
char ifName[IFNAMSIZ]; /* CAN Interface name */
int epoll_fd; /* File descriptor for epoll, which waits for
CAN receive event */
int fd; /* socketCAN file descriptor */
#if CO_DRIVER_ERROR_REPORTING > 0 || defined CO_DOXYGEN
CO_CANinterfaceErrorhandler_t errorhandler;
@ -331,10 +330,8 @@ typedef struct {
uint16_t txSize;
uint16_t CANerrorStatus;
volatile bool_t CANnormal;
int fdEvent; /* notification event file descriptor */
int fdEpoll; /* epoll FD for event, CANrx sockets in all
interfaces and fdTimerRead */
int fdTimerRead; /* timer handle from CANrxWait() */
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. */
@ -383,13 +380,11 @@ static inline void CO_UNLOCK_OD() {
*
* @param CANmodule This object will be initialized.
* @param can_ifindex CAN Interface index
* @param epoll_fd File descriptor for epoll, which waits for CAN receive event
* @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,
int epoll_fd);
int can_ifindex);
/**
* Check on which interface the last message for one message buffer was received
@ -433,13 +428,15 @@ CO_ReturnError_t CO_CANtxBuffer_setInterface(CO_CANmodule_t *CANmodule,
/**
* Functions receives CAN messages (blocking)
* Receives CAN messages from matching epoll event
*
* This function waits for received CAN message, CAN error frame, notification
* event or fdTimer expiration. In case of CAN message it 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 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
@ -447,16 +444,17 @@ CO_ReturnError_t CO_CANtxBuffer_setInterface(CO_CANmodule_t *CANmodule,
* - manual mode: evaluate message filters, return received message
*
* @param CANmodule This object.
* @param fdTimer File descriptor with activated timeout. If set to -1, then
* timer will not be used. File descriptor must be read
* externally if retval == -1! Read must be nonblocking and
* provides number of timer expirations since last read.
* @param ev Epoll event, which vill be verified for matches.
* @param [out] buffer Storage for received message or _NULL_ if not used.
* @retval >= 0 index of received message in array from CO_CANmodule_t
* _rxArray_, copy of CAN message is available in _buffer_.
* @retval -1 no message received (timer expired or notification event or error)
* @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.
*/
int32_t CO_CANrxWait(CO_CANmodule_t* CANmodule, int fdTimer, CO_CANrxMsg_t* buffer);
bool_t CO_CANrxFromEpoll(CO_CANmodule_t *CANmodule,
struct epoll_event *ev,
CO_CANrxMsg_t *buffer,
int32_t *msgIndex);
/** @} */

638
socketCAN/CO_epoll_interface.c Normal file
View file

@ -0,0 +1,638 @@
/*
* 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>
#ifndef LISTEN_BACKLOG
#define LISTEN_BACKLOG 50
#endif
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII */
/* 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 *******************************************************************/
/* 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()");
}
}
void CO_epoll_initCANopenMain(CO_epoll_t *ep, CO_t *co) {
if (ep == NULL || co == NULL) {
return;
}
/* Configure LSS slave callback function */
#if (CO_CONFIG_LSS) & CO_CONFIG_LSS_SLAVE
CO_LSSslave_initCallbackPre(co->LSSslave,
(void *)ep, wakeupCallback);
#endif
if (co->nodeIdUnconfigured) {
return;
}
/* Configure callback functions */
CO_NMT_initCallbackPre(co->NMT,
(void *)ep, wakeupCallback);
CO_SDO_initCallbackPre(co->SDO[0],
(void *)ep, wakeupCallback);
CO_EM_initCallbackPre(co->em,
(void *)ep, wakeupCallback);
CO_HBconsumer_initCallbackPre(co->HBcons,
(void *)ep, wakeupCallback);
#if CO_NO_SDO_CLIENT != 0
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
}
void CO_epoll_processMain(CO_epoll_t *ep,
CO_t *co,
CO_NMT_reset_cmd_t *reset)
{
if (ep == NULL || co == NULL || reset == NULL) {
return;
}
/* process CANopen objects */
*reset = CO_process(co, ep->timeDifference_us, &ep->timerNext_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[0], &ep->ev, NULL, NULL)) {
ep->epoll_new = false;
}
}
if (!realtime || ep->timerEvent) {
uint32_t *pTimerNext_us = realtime ? NULL : &ep->timerNext_us;
CO_LOCK_OD();
if (!co->nodeIdUnconfigured && co->CANmodule[0]->CANnormal) {
bool_t syncWas = false;
#if CO_NO_SYNC == 1
/* Process Sync */
syncWas = CO_process_SYNC(co, ep->timeDifference_us,
pTimerNext_us);
#endif
/* Read inputs */
CO_process_RPDO(co, syncWas);
/* Write outputs */
CO_process_TPDO(co, syncWas, ep->timeDifference_us,
pTimerNext_us);
}
CO_UNLOCK_OD();
}
}
/* 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) {
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 {
log_printf(LOG_DEBUG, DBG_ERRNO, "write(gtwa_response)");
}
}
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;
}
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;
}
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 & (EPOLLERR | EPOLLHUP)) != 0) {
log_printf(LOG_DEBUG, DBG_GENERAL,
"socket error or hangup, event=", ep->ev.events);
}
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 (co->nodeIdUnconfigured) {
/* 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;
}
} /* 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 */

307
socketCAN/CO_epoll_interface.h Normal file
View file

@ -0,0 +1,307 @@
/**
* 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 [out] reset Return from @ref CO_process().
*/
void CO_epoll_processMain(CO_epoll_t *ep,
CO_t *co,
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 */

4
socketCAN/CO_error_msgs.h
View file

@ -57,6 +57,7 @@ extern "C" {
/*
* 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_CAN_TX_FAILED "(%s) Transmitting CAN msg OID 0x%08x failed(%s)", __func__
#define DBG_CAN_RX_PARAM_FAILED "(%s) Setting CAN rx buffer failed (%s)", __func__
@ -79,7 +80,8 @@ extern "C" {
#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_Linux_threads */
/* 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"

74
socketCAN/CO_main_basic.c
View file

@ -42,7 +42,7 @@
#include "CANopen.h"
#include "CO_OD_storage.h"
#include "CO_error.h"
#include "CO_Linux_threads.h"
#include "CO_epoll_interface.h"
/* Call external application functions. */
#if __has_include("CO_application.h")
@ -71,6 +71,7 @@
/* Other variables and objects */
CO_epoll_t epRT; /* Epoll-timer object for realtime thread */
static int rtPriority = -1; /* Real time priority, configurable by arguments. (-1=RT disabled) */
static uint8_t CO_pendingNodeId = 0xFF;/* Use value from Object Dictionary or by arguments (set to 1..127
* or unconfigured=0xFF). Can be changed by LSS slave. */
@ -212,6 +213,7 @@ printf(
* Mainline thread
******************************************************************************/
int main (int argc, char *argv[]) {
CO_epoll_t epMain;
pthread_t rt_thread_id;
CO_NMT_reset_cmd_t reset = CO_RESET_NOT;
CO_ReturnError_t err;
@ -224,6 +226,7 @@ int main (int argc, char *argv[]) {
bool_t nodeIdFromArgs = false; /* True, if program arguments are used for CANopen Node Id */
bool_t rebootEnable = false; /* Configurable by arguments */
#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 */
@ -369,6 +372,32 @@ int main (int argc, char *argv[]) {
}
/* Initialize thread functions: epoll, timer, events */
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);
}
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;
#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 *******************/
@ -379,9 +408,9 @@ int main (int argc, char *argv[]) {
CO_UNLOCK_OD();
}
/* Enter CAN configuration. */
CO_CANsetConfigurationMode((void *)&CANptr);
CO_CANmodule_disable(CO->CANmodule[0]);
/* initialize CANopen */
@ -406,9 +435,12 @@ int main (int argc, char *argv[]) {
}
/* initialize part of threadMain and callbacks */
threadMainWait_init(!CO->nodeIdUnconfigured);
CO_epoll_initCANopenMain(&epMain, CO);
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
CO_epoll_initCANopenGtw(&epGtw, CO);
#endif
CO_LSSslave_initCfgStoreCallback(CO->LSSslave, NULL,
LSScfgStoreCallback);
LSScfgStoreCallback);
if(!CO->nodeIdUnconfigured) {
CO_EM_initCallbackRx(CO->em, EmergencyRxCallback);
CO_NMT_initCallbackChanged(CO->NMT, NmtChangedCallback);
@ -438,14 +470,6 @@ int main (int argc, char *argv[]) {
if(firstRun) {
firstRun = false;
/* Init threadMainWait structure and file descriptors */
threadMainWait_initOnce(MAIN_THREAD_INTERVAL_US, commandInterface,
socketTimeout_ms, localSocketPath);
/* Init threadRT structure and file descriptors */
CANrx_threadTmr_init(TMR_THREAD_INTERVAL_US);
/* 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)");
@ -484,15 +508,21 @@ int main (int argc, char *argv[]) {
while(reset == CO_RESET_NOT && CO_endProgram == 0) {
/* loop for normal program execution ******************************************/
uint32_t timer1usDiff = threadMainWait_process(&reset);
CO_epoll_wait(&epMain);
CO_epoll_processMain(&epMain, CO, &reset);
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
CO_epoll_processGtw(&epGtw, CO, &epMain);
#endif
CO_epoll_processLast(&epMain);
#ifdef CO_USE_APPLICATION
app_programAsync(!CO->nodeIdUnconfigured, thrEpTm.timeDifference_us);
app_programAsync(!CO->nodeIdUnconfigured, epMain.timeDifference_us);
#endif
CO_OD_storage_autoSave(&odStorAuto, timer1usDiff, 60000000);
CO_OD_storage_autoSave(&odStorAuto,
epMain.timeDifference_us, 60000000);
}
}
} /* while(reset != CO_RESET_APP */
/* program exit ***************************************************************/
@ -513,8 +543,11 @@ int main (int argc, char *argv[]) {
CO_OD_storage_autoSaveClose(&odStorAuto);
/* delete objects from memory */
CANrx_threadTmr_close();
threadMainWait_close();
CO_epoll_close(&epRT);
CO_epoll_close(&epMain);
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
CO_epoll_closeGtw(&epGtw);
#endif
CO_delete((void *)&CANptr);
log_printf(LOG_INFO, DBG_CAN_OPEN_INFO, CO_activeNodeId, "finished");
@ -540,8 +573,9 @@ static void* rt_thread(void* arg) {
/* Endless loop */
while(CO_endProgram == 0) {
/* function may skip some milliseconds. Number of missed is returned */
CANrx_threadTmr_process();
CO_epoll_wait(&epRT);
CO_epoll_processRT(&epRT, CO, true);
CO_epoll_processLast(&epRT);
#if CO_NO_TRACE > 0
/* Monitor variables with trace objects */