From e6b4c1f5b16c9af3223a7f963afc771114d1c701 Mon Sep 17 00:00:00 2001 From: Janez Date: Thu, 12 Mar 2020 08:38:58 +0100 Subject: [PATCH] socketCAN now compiles and runs. --- Makefile | 2 +- doc/gettingStarted.md | 2 +- socketCAN/CO_Linux_threads.c | 172 +++++++++++++++++++++- socketCAN/CO_Linux_threads.h | 66 +++++++-- socketCAN/CO_OD_storage.c | 17 +-- socketCAN/CO_OD_storage.h | 16 +- socketCAN/CO_error.h | 4 +- socketCAN/CO_error_msgs.h | 12 ++ socketCAN/CO_main_basic.c | 273 ++++++++++++++--------------------- 9 files changed, 356 insertions(+), 208 deletions(-) diff --git a/Makefile b/Makefile index 0b3353b..600aab1 100644 --- a/Makefile +++ b/Makefile @@ -42,7 +42,7 @@ SOURCES = \ OBJS = $(SOURCES:%.c=%.o) CC ?= gcc -OPT = -Og +OPT = -g CFLAGS = -Wall $(OPT) $(INCLUDE_DIRS) LDFLAGS = -pthread diff --git a/doc/gettingStarted.md b/doc/gettingStarted.md index bbb8a30..16dcf38 100644 --- a/doc/gettingStarted.md +++ b/doc/gettingStarted.md @@ -192,7 +192,7 @@ Some tested USB to CAN interfaces, which are natively integrated into Linux are: - Simple serial [USBtin](http://www.fischl.de/usbtin/) - Start with: `sudo slcand -f -o -c -s8 /dev/ttyACM0 can0; sudo ip link set up can0` - [EMS CPC-USB](http://www.ems-wuensche.com/product/datasheet/html/can-usb-adapter-converter-interface-cpcusb.html) - Start with: `sudo ip link set up can0 type can bitrate 250000` - [PCAN-USB FD](http://www.peak-system.com/PCAN-USB-FD.365.0.html?&L=1) - Needs newer Linux kernel, supports CAN flexible data rate. - - You can get the idea of other supported CAN interfaces in [Linux kernel source](https://git.kernel.org/cgit/linux/kernel/git/torvalds/linux.git/tree/drivers/net/can). + - You can get the idea of other supported CAN interfaces in [Linux kernel source](https://git.kernel.org/cgit/linux/kernel/git/torvalds/linux.git/tree/drivers/net/can) (Kconfig files). - Beaglebone or Paspberry PI or similar has CAN capes available. On RPI worked also the above USB interfaces, but it was necessary to compile the kernel. diff --git a/socketCAN/CO_Linux_threads.c b/socketCAN/CO_Linux_threads.c index 8051a34..dcc1185 100644 --- a/socketCAN/CO_Linux_threads.c +++ b/socketCAN/CO_Linux_threads.c @@ -25,13 +25,17 @@ * limitations under the License. */ -#include -#include +#include #include #include +#include +#include +#include +#include #include "CANopen.h" + /* Helper function - get monotonic clock time in microseconds */ static uint64_t CO_LinuxThreads_clock_gettime_us(void) { @@ -41,7 +45,8 @@ static uint64_t CO_LinuxThreads_clock_gettime_us(void) return ts.tv_sec * 1000000 + ts.tv_nsec / 1000; } -/* Mainline thread (threadMain) ***********************************************/ + +/* Mainline thread - basic (threadMain) ***************************************/ static struct { uint64_t start; /* time value CO_process() was called last time in us */ @@ -69,6 +74,7 @@ void threadMain_process(CO_NMT_reset_cmd_t *reset) now = CO_LinuxThreads_clock_gettime_us(); diff = (uint32_t)(now - threadMain.start); + threadMain.start = now; /* we use timerNext_us in CO_process() as indication if processing is * finished. We ignore any calculated values for maximum delay times. */ @@ -78,10 +84,160 @@ void threadMain_process(CO_NMT_reset_cmd_t *reset) diff = 0; } while ((*reset == CO_RESET_NOT) && (finished == 0)); - /* prepare next call */ - threadMain.start = now; } + +/* 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 */ +} threadMainWait; + +static void threadMainWait_callback(void *object) +{ + /* send event to wake threadMainWait_process() */ + uint64_t u = 1; + ssize_t s; + s = write(threadMainWait.event_fd, &u, sizeof(uint64_t)); + if (s != sizeof(uint64_t)) { + log_printf(LOG_DEBUG, DBG_ERRNO, "write()"); + } +} + +void threadMainWait_init(uint32_t interval_us) +{ + int32_t ret; + struct epoll_event ev; + + /* Configure callback functions */ + CO_SDO_initCallback(CO->SDO[0], NULL, threadMainWait_callback); + CO_EM_initCallback(CO->em, NULL, threadMainWait_callback); + + /* Initial value for time calculation */ + threadMainWait.start = CO_LinuxThreads_clock_gettime_us(); + + /* Configure epoll for mainline */ + threadMainWait.epoll_fd = epoll_create(1); + if (threadMainWait.epoll_fd < 0) { + log_printf(LOG_CRIT, DBG_ERRNO, "epoll_create()"); + exit(EXIT_FAILURE); + } + + /* Configure eventfd for notifications and add it to epoll */ + threadMainWait.event_fd = eventfd(0, 0); + if (threadMainWait.event_fd < 0) { + log_printf(LOG_CRIT, DBG_ERRNO, "eventfd()"); + exit(EXIT_FAILURE); + } + ev.events = EPOLLIN; + ev.data.fd = threadMainWait.event_fd; + ret = epoll_ctl(threadMainWait.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 */ + threadMainWait.timer_fd = timerfd_create(CLOCK_MONOTONIC, TFD_NONBLOCK); + if (threadMainWait.timer_fd < 0) { + log_printf(LOG_CRIT, DBG_ERRNO, "timerfd_create()"); + exit(EXIT_FAILURE); + } + threadMainWait.interval_us = interval_us; + threadMainWait.tm.it_interval.tv_sec = interval_us / 1000000; + threadMainWait.tm.it_interval.tv_nsec = (interval_us % 1000000) * 1000; + threadMainWait.tm.it_value.tv_sec = 0; + threadMainWait.tm.it_value.tv_nsec = 1; + ret = timerfd_settime(threadMainWait.timer_fd, 0, &threadMainWait.tm, NULL); + if (ret < 0){ + log_printf(LOG_CRIT, DBG_ERRNO, "timerfd_settime"); + exit(EXIT_FAILURE); + } + ev.events = EPOLLIN; + ev.data.fd = threadMainWait.timer_fd; + ret = epoll_ctl(threadMainWait.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); + } +} + +void threadMainWait_close(void) +{ + CO_SDO_initCallback(CO->SDO[0], NULL, NULL); + CO_EM_initCallback(CO->em, NULL, NULL); + + close(threadMainWait.epoll_fd); + close(threadMainWait.event_fd); + close(threadMainWait.timer_fd); + threadMainWait.epoll_fd = -1; + threadMainWait.event_fd = -1; + threadMainWait.timer_fd = -1; +} + +uint32_t threadMainWait_process(CO_NMT_reset_cmd_t *reset) +{ + int ready, ret; + 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(threadMainWait.epoll_fd, &ev, 1, -1); + if (ready != 1 && errno != EINTR) { + log_printf(LOG_DEBUG, DBG_ERRNO, "epoll_wait"); + } + else if (ev.data.fd == threadMainWait.event_fd) { + s = read(threadMainWait.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 == threadMainWait.timer_fd) { + s = read(threadMainWait.timer_fd, &ull, sizeof(uint64_t)); + if (s != sizeof(uint64_t) && errno != EAGAIN) { + log_printf(LOG_DEBUG, DBG_ERRNO, "read(timer_fd)"); + } + } + + /* calculate time difference since last call */ + ull = CO_LinuxThreads_clock_gettime_us(); + diff = (uint32_t)(ull - threadMainWait.start); + threadMainWait.start = ull; + + /* stack will lower this, if necessary */ + timerNext_us = threadMainWait.interval_us; + + /* process CANopen objects */ + *reset = CO_process(CO, diff, &timerNext_us); + + /* lower next timer interval if necessary */ + if (timerNext_us < threadMainWait.interval_us) { + /* add one microsecond extra delay and make sure it is not zero */ + timerNext_us += 1; + if (threadMainWait.interval_us < 1000000) { + threadMainWait.tm.it_value.tv_nsec = timerNext_us * 1000; + } else { + threadMainWait.tm.it_value.tv_sec = timerNext_us / 1000000; + threadMainWait.tm.it_value.tv_nsec = (timerNext_us % 1000000) * 1000; + } + ret = timerfd_settime(threadMainWait.timer_fd, 0, + &threadMainWait.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 */ @@ -108,12 +264,12 @@ void CANrx_threadTmr_close(void) threadRT.interval_fd = -1; } -void CANrx_threadTmr_process(void) +uint32_t CANrx_threadTmr_process(void) { int32_t result; uint64_t i; bool_t syncWas; - uint64_t missed; + uint64_t missed = 0; result = CO_CANrxWait(CO->CANmodule[0], threadRT.interval_fd, NULL); if (result < 0) { @@ -143,4 +299,6 @@ void CANrx_threadTmr_process(void) CO_UNLOCK_OD(); } } + + return (uint32_t) missed; } diff --git a/socketCAN/CO_Linux_threads.h b/socketCAN/CO_Linux_threads.h index f903c2c..e1e2795 100644 --- a/socketCAN/CO_Linux_threads.h +++ b/socketCAN/CO_Linux_threads.h @@ -38,12 +38,17 @@ extern "C" { * @defgroup CO_socketCAN socketCAN * @{ * - * Linux specific interface to CANopenNode + * 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 threadMain_process() + * - mainline thread for other processing, see threadMain_process() or + * threadMainWait_process() * * The "threads" specified here do not fork threads themselves, but require * that two threads are provided by the calling application. @@ -51,28 +56,29 @@ extern "C" { /** - * Initialize mainline thread. + * Initialize mainline thread - basic. * * @param callback this function is called to indicate #threadMain_process() has * work to do * @param object this pointer is given to _callback()_ */ -extern void threadMain_init(void (*callback)(void*), void *object); +void threadMain_init(void (*callback)(void*), void *object); /** - * Cleanup mainline thread. + * Cleanup mainline thread - basic. */ -extern void threadMain_close(void); +void threadMain_close(void); /** - * Process mainline thread. + * Process mainline thread - basic. * * threadMain is non-realtime thread for CANopenNode processing. It is * initialized by threadMain_init(). There is no configuration for CANopen - * objects. There is also no configuration for epool or interval timer or notify - * pipe. These must be specified externally. + * objects. There is also no configuration for epool or interval timer or + * eventfd. These must be specified externally. For more complete function see + * threadMainWait_process(), they are included. * * threadMain_process() calls CO_process() function for processing mainline * CANopen objects. It is non-blocking and should be called cyclically in 50 ms @@ -81,7 +87,39 @@ extern void threadMain_close(void); * * @param reset return value from CO_process() function. */ -extern void threadMain_process(CO_NMT_reset_cmd_t *reset); +void threadMain_process(CO_NMT_reset_cmd_t *reset); + + +/** + * Initialize mainline thread - blocking. + * + * @param interval_us interval of the threadMainWait_process() + */ +void threadMainWait_init(uint32_t interval_us); + + +/** + * 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); /** @@ -90,13 +128,13 @@ extern void threadMain_process(CO_NMT_reset_cmd_t *reset); * @param interval_us Interval of periodic timer in microseconds, recommended * value for realtime response: 1000 us */ -extern void CANrx_threadTmr_init(uint32_t interval_us); +void CANrx_threadTmr_init(uint32_t interval_us); /** * Terminate realtime thread. */ -extern void CANrx_threadTmr_close(void); +void CANrx_threadTmr_close(void); /** @@ -117,8 +155,10 @@ extern void CANrx_threadTmr_close(void); * * @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. */ -extern void CANrx_threadTmr_process(); +void CANrx_threadTmr_process(void); /** @} */ diff --git a/socketCAN/CO_OD_storage.c b/socketCAN/CO_OD_storage.c index be71bfa..dc6fee6 100644 --- a/socketCAN/CO_OD_storage.c +++ b/socketCAN/CO_OD_storage.c @@ -241,8 +241,7 @@ CO_ReturnError_t CO_OD_storage_init( odStor->odSize = odSize; odStor->filename = filename; odStor->fp = NULL; - odStor->tmr1msPrev = 0; - odStor->lastSavedMs = 0; + odStor->lastSavedUs = 0; buf = malloc(odStor->odSize); if(buf == NULL) { @@ -292,8 +291,8 @@ CO_ReturnError_t CO_OD_storage_init( /******************************************************************************/ CO_ReturnError_t CO_OD_storage_autoSave( CO_OD_storage_t *odStor, - uint16_t timer1ms, - uint16_t delay) + uint32_t timer1usDiff, + uint32_t delay_us) { CO_ReturnError_t ret = CO_ERROR_NO; @@ -303,10 +302,8 @@ CO_ReturnError_t CO_OD_storage_autoSave( } /* don't save file more often than delay */ - if(odStor->lastSavedMs < delay) { - odStor->lastSavedMs += timer1ms - odStor->tmr1msPrev; - } - else { + odStor->lastSavedUs += timer1usDiff; + if (odStor->lastSavedUs > delay_us) { void *buf = NULL; bool_t saveData = false; @@ -360,14 +357,12 @@ CO_ReturnError_t CO_OD_storage_autoSave( fflush(odStor->fp); - odStor->lastSavedMs = 0; + odStor->lastSavedUs = 0; } free(buf); } - odStor->tmr1msPrev = timer1ms; - return ret; } diff --git a/socketCAN/CO_OD_storage.h b/socketCAN/CO_OD_storage.h index 24b4f2a..c0212e8 100644 --- a/socketCAN/CO_OD_storage.h +++ b/socketCAN/CO_OD_storage.h @@ -108,8 +108,7 @@ typedef struct { char *filename; /**< From CO_OD_storage_init() */ /** If CO_OD_storage_autoSave() is used, file stays opened and fp is stored here. */ FILE *fp; - uint16_t tmr1msPrev; /**< used with CO_OD_storage_autoSave. */ - uint32_t lastSavedMs; /**< used with CO_OD_storage_autoSave. */ + uint32_t lastSavedUs; /**< used with CO_OD_storage_autoSave. */ } CO_OD_storage_t; @@ -143,16 +142,17 @@ CO_ReturnError_t CO_OD_storage_init( * CRC bytes. File remains opened. * * @param odStor OD storage object. - * @param timer1ms Variable, which must increment each millisecond. - * @param delay Delay (inhibit) time between writes to disk in milliseconds (60000 for example). + * @param timer1usDiff Time difference in microseconds since last call. + * @param delay_us Delay (inhibit) time between writes to disk in microseconds + * (60000 for example). * - * @return #CO_ReturnError_t: CO_ERROR_NO, CO_ERROR_DATA_CORRUPT (Data in file corrupt), - * CO_ERROR_ILLEGAL_ARGUMENT or CO_ERROR_OUT_OF_MEMORY (malloc failed). + * @return #CO_ReturnError_t: CO_ERROR_NO, CO_ERROR_DATA_CORRUPT (Data in file + * corrupt), CO_ERROR_ILLEGAL_ARGUMENT or CO_ERROR_OUT_OF_MEMORY (malloc failed). */ CO_ReturnError_t CO_OD_storage_autoSave( CO_OD_storage_t *odStor, - uint16_t timer1ms, - uint16_t delay); + uint32_t timer1usDiff, + uint32_t delay_us); /** diff --git a/socketCAN/CO_error.h b/socketCAN/CO_error.h index 41008a4..7f9fb43 100644 --- a/socketCAN/CO_error.h +++ b/socketCAN/CO_error.h @@ -59,10 +59,10 @@ extern "C" { * 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, use: + * and print also to standard error, set 'user' type of program, use: * @code * setlogmask (LOG_UPTO (LOG_NOTICE)); - * openlog ("exampleprog", LOG_PID | LOG_PERROR); + * openlog ("exampleprog", LOG_PID | LOG_PERROR, LOG_USER); * @endcode * * @param priority one of LOG_EMERG, LOG_ALERT, LOG_CRIT, LOG_ERR, LOG_WARNING, diff --git a/socketCAN/CO_error_msgs.h b/socketCAN/CO_error_msgs.h index c15a55c..d8c7ae6 100644 --- a/socketCAN/CO_error_msgs.h +++ b/socketCAN/CO_error_msgs.h @@ -77,6 +77,18 @@ extern "C" { #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_NOT_TCP_PORT "(%s) -t 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_OBJECT_DICTIONARY "(%s) Error in Object Dictionary \"%s\"", __func__ +#define DBG_CAN_OPEN "(%s) CANopen error in %s, err=%d", __func__ +#define DBG_CAN_OPEN_INFO "(%s) CANopen device, Node ID = %d(0x%02X), %s", __func__ +#define DBG_COMMAND_LOCAL_INFO "(%s) Command interface on socket \"%s\" started", __func__ +#define DBG_COMMAND_TCP_INFO "(%s) Command interface on tcp port \"%hu\" started", __func__ + #ifdef __cplusplus } diff --git a/socketCAN/CO_main_basic.c b/socketCAN/CO_main_basic.c index 57a7497..dc9d30c 100644 --- a/socketCAN/CO_main_basic.c +++ b/socketCAN/CO_main_basic.c @@ -55,16 +55,20 @@ /* Use DS309-3 standard - ASCII command interface to CANopen: NMT master, * LSS master and SDO client */ -#ifdef CO_309 +#if CO_CONFIG_309 > 0 #include "CO_command.h" #endif -/* Interval of real-time thread in microseconds */ +/* 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 -#ifdef CO_309 + +#if CO_CONFIG_309 > 0 /* Mutex is locked, when CAN is not valid (configuration state). May be used * from command interface. RT threads may use CO->CANmodule[0]->CANnormal instead. */ pthread_mutex_t CO_CAN_VALID_mtx = PTHREAD_MUTEX_INITIALIZER; @@ -72,25 +76,21 @@ pthread_mutex_t CO_CAN_VALID_mtx = PTHREAD_MUTEX_INITIALIZER; /* Other variables and objects */ static int rtPriority = -1; /* Real time priority, configurable by arguments. (-1=RT disabled) */ -static int mainline_epoll_fd; /* epoll file descriptor for mainline */ +static int nodeId = -1; /* Use value from Object Dictionary or set to 1..127 by arguments */ static CO_OD_storage_t odStor; /* Object Dictionary storage object for CO_OD_ROM */ static CO_OD_storage_t odStorAuto; /* Object Dictionary storage object for CO_OD_EEPROM */ static char *odStorFile_rom = "od_storage"; /* Name of the file */ static char *odStorFile_eeprom = "od_storage_auto"; /* Name of the file */ -#ifdef CO_309 +#if CO_CONFIG_309 > 0 static in_port_t CO_command_socket_tcp_port = 60000; /* default port when used in tcp gateway mode */ #endif #if CO_NO_TRACE > 0 static CO_time_t CO_time; /* Object for current time */ #endif +/* Helper functions ***********************************************************/ /* Realtime thread */ -#ifdef CO_MULTI_THREAD -static void* rt_thread(void* arg); -static pthread_t rt_thread_id; -static int rt_thread_epoll_fd; -#endif - +static void* rt_thread(void* arg); /* Signal handler */ volatile sig_atomic_t CO_endProgram = 0; @@ -98,24 +98,25 @@ static void sigHandler(int sig) { CO_endProgram = 1; } +/* 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) : nodeId; -/* Helper functions ***********************************************************/ -void CO_errExit(char* msg) { - perror(msg); - exit(EXIT_FAILURE); -} - -/* send CANopen generic emergency message */ -void CO_error(const uint32_t info) { - CO_errorReport(CO->em, CO_EM_GENERIC_SOFTWARE_ERROR, CO_EMC_SOFTWARE_INTERNAL, info); - fprintf(stderr, "canopend generic error: 0x%X\n", info); + log_printf(LOG_NOTICE, DBG_EMERGENCY_RX, nodeIdRx, errorCode, + errorRegister, errorBit, infoCode); } +/* Print usage */ static void printUsage(char *progName) { -fprintf(stderr, +printf( "Usage: %s [options]\n", progName); -fprintf(stderr, +printf( "\n" "Options:\n" " -i CANopen Node-id (1..127). If not specified, value from\n" @@ -126,31 +127,30 @@ fprintf(stderr, " -s Set Filename for OD storage ('od_storage' is default).\n" " -a Set Filename for automatic storage variables from\n" " Object dictionary. ('od_storage_auto' is default).\n"); -#ifdef CO_309 -fprintf(stderr, +#if CO_CONFIG_309 > 0 +printf( " -c Enable command interface for master functionality. \n" " If socket path is specified as empty string \"\",\n" " default '%s' will be used.\n" " Note that location of socket path may affect security.\n" " See 'canopencomm/canopencomm --help' for more info.\n" , CO_command_socketPath); -fprintf(stderr, -" -t Enable command interface for master functionality over tcp, \n" -" listen to .\n" +printf( +" -t Enable command interface for master functionality over\n" +" tcp, listen to .\n" " Note that using this mode may affect security.\n" ); #endif -fprintf(stderr, +printf( "\n" "See also: https://github.com/CANopenNode/CANopenNode\n" "\n"); } -/******************************************************************************/ -/** Mainline and RT thread **/ -/******************************************************************************/ +/*Mainline thread *************************************************************/ int main (int argc, char *argv[]) { + pthread_t rt_thread_id; CO_NMT_reset_cmd_t reset = CO_RESET_NOT; CO_ReturnError_t err; CO_ReturnError_t odStorStatus_rom, odStorStatus_eeprom; @@ -160,20 +160,21 @@ int main (int argc, char *argv[]) { char* CANdevice = NULL; /* CAN device, configurable by arguments. */ bool_t nodeIdFromArgs = false; /* True, if program arguments are used for CANopen Node Id */ - int nodeId = -1; /* Use value from Object Dictionary or set to 1..127 by arguments */ bool_t rebootEnable = false; /* Configurable by arguments */ -#ifdef CO_309 +#if CO_CONFIG_309 > 0 typedef enum CMD_MODE {CMD_NONE, CMD_LOCAL, CMD_REMOTE} cmdMode_t; cmdMode_t commandEnable = CMD_NONE; /* Configurable by arguments */ #endif + /* configure system log */ + setlogmask(LOG_UPTO (LOG_DEBUG)); /* LOG_DEBUG - log all meessages */ + 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); } - - - /* Get program options */ while((opt = getopt(argc, argv, "i:p:rc:t:s:a:")) != -1) { switch (opt) { case 'i': @@ -182,7 +183,7 @@ int main (int argc, char *argv[]) { break; case 'p': rtPriority = strtol(optarg, NULL, 0); break; case 'r': rebootEnable = true; break; -#ifdef CO_309 +#if CO_CONFIG_309 > 0 case 'c': /* In case of empty string keep default name, just enable interface. */ if(strlen(optarg) != 0) { @@ -196,8 +197,8 @@ int main (int argc, char *argv[]) { //CO_command_socket_tcp_port = optarg; int scanResult = sscanf(optarg, "%hu", &CO_command_socket_tcp_port); if(scanResult != 1){ //expect one argument to be extracted - printf("ERROR: -t argument \'%s\' is not a valid tcp port\n", optarg); - exit(EXIT_FAILURE); + log_printf(LOG_CRIT, DBG_NOT_TCP_PORT, optarg); + exit(EXIT_FAILURE); } } commandEnable = CMD_REMOTE; @@ -217,47 +218,46 @@ int main (int argc, char *argv[]) { } if(nodeIdFromArgs && (nodeId < 1 || nodeId > 127)) { - fprintf(stderr, "Wrong node ID (%d)\n", nodeId); + log_printf(LOG_CRIT, DBG_WRONG_NODE_ID, nodeId); printUsage(argv[0]); exit(EXIT_FAILURE); } if(rtPriority != -1 && (rtPriority < sched_get_priority_min(SCHED_FIFO) || rtPriority > sched_get_priority_max(SCHED_FIFO))) { - fprintf(stderr, "Wrong RT priority (%d)\n", rtPriority); + log_printf(LOG_CRIT, DBG_WRONG_PRIORITY, rtPriority); printUsage(argv[0]); exit(EXIT_FAILURE); } if(CANdevice0Index == 0) { - char s[120]; - snprintf(s, 120, "Can't find CAN device \"%s\"", CANdevice); - CO_errExit(s); + log_printf(LOG_CRIT, DBG_NO_CAN_DEVICE, CANdevice); + exit(EXIT_FAILURE); } - printf("%s - starting CANopen device with Node ID %d(0x%02X)", argv[0], nodeId, nodeId); + log_printf(LOG_INFO, DBG_CAN_OPEN_INFO, nodeId, nodeId, "starting"); /* Allocate memory for CANopen objects */ - err = CO_new(); + err = CO_new(NULL); if (err != CO_ERROR_NO) { - fprintf(stderr, "Program init - %s - CO_new() failed.\n", argv[0]); + log_printf(LOG_CRIT, DBG_CAN_OPEN, "CO_new()", err); exit(EXIT_FAILURE); } /* Verify, if OD structures have proper alignment of initial values */ if(CO_OD_RAM.FirstWord != CO_OD_RAM.LastWord) { - fprintf(stderr, "Program init - %s - Error in CO_OD_RAM.\n", argv[0]); + log_printf(LOG_CRIT, DBG_OBJECT_DICTIONARY, "CO_OD_RAM"); exit(EXIT_FAILURE); } if(CO_OD_EEPROM.FirstWord != CO_OD_EEPROM.LastWord) { - fprintf(stderr, "Program init - %s - Error in CO_OD_EEPROM.\n", argv[0]); + log_printf(LOG_CRIT, DBG_OBJECT_DICTIONARY, "CO_OD_EEPROM"); exit(EXIT_FAILURE); } if(CO_OD_ROM.FirstWord != CO_OD_ROM.LastWord) { - fprintf(stderr, "Program init - %s - Error in CO_OD_ROM.\n", argv[0]); + log_printf(LOG_CRIT, DBG_OBJECT_DICTIONARY, "CO_OD_ROM"); exit(EXIT_FAILURE); } @@ -268,22 +268,23 @@ int main (int argc, char *argv[]) { /* Catch signals SIGINT and SIGTERM */ - if(signal(SIGINT, sigHandler) == SIG_ERR) - CO_errExit("Program init - SIGINIT handler creation failed"); - if(signal(SIGTERM, sigHandler) == SIG_ERR) - CO_errExit("Program init - SIGTERM handler creation failed"); - - /* increase variable each startup. Variable is automatically stored in non-volatile memory. */ - printf(", count=%u ...\n", ++OD_powerOnCounter); + 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); + } while(reset != CO_RESET_APP && reset != CO_RESET_QUIT && CO_endProgram == 0) { /* CANopen communication reset - initialize CANopen objects *******************/ - printf("%s - communication reset ...\n", argv[0]); + log_printf(LOG_INFO, DBG_CAN_OPEN_INFO, nodeId, nodeId, "communication reset"); -#ifdef CO_309 +#if CO_CONFIG_309 > 0 /* Wait other threads (command interface). */ pthread_mutex_lock(&CO_CAN_VALID_mtx); #endif @@ -307,16 +308,19 @@ int main (int argc, char *argv[]) { } err = CO_CANinit((void *)CANdevice0Index, 0 /* bit rate not used */); - if (err == CO_ERROR_NO) { - err = CO_CANopenInit(nodeId); - } - if(err != CO_ERROR_NO) { - char s[120]; - snprintf(s, 120, "Communication reset - CANopen initialization failed, err=%d", err); - CO_errExit(s); + log_printf(LOG_CRIT, DBG_CAN_OPEN, "CO_CANinit()", err); + exit(EXIT_FAILURE); } + err = CO_CANopenInit(nodeId); + if(err != CO_ERROR_NO) { + log_printf(LOG_CRIT, DBG_CAN_OPEN, "CO_CANopenInit()", err); + exit(EXIT_FAILURE); + } + + /* initialize callbacks */ + CO_EM_initCallbackRx(CO->em, EmergencyRxCallback); /* initialize OD objects 1010 and 1011 and verify errors. */ CO_OD_configure(CO->SDO[0], OD_H1010_STORE_PARAM_FUNC, CO_ODF_1010, (void*)&odStor, 0, 0U); @@ -329,13 +333,6 @@ int main (int argc, char *argv[]) { } - /* Configure callback functions for thread control */ -// void CO_CANopenInitCallback(void *object, void (*pFunctSignal)(void *object)); -// CO_EM_initCallback(CO->em, threadMain_cbSignal); -// CO_SDO_initCallback(CO->SDO[0], threadMain_cbSignal); -// CO_SDOclient_initCallback(CO->SDOclient, threadMain_cbSignal); - - #if CO_NO_TRACE > 0 /* Initialize time */ CO_time_init(&CO_time, CO->SDO[0], &OD_time.epochTimeBaseMs, &OD_time.epochTimeOffsetMs, 0x2130); @@ -345,52 +342,43 @@ int main (int argc, char *argv[]) { if(firstRun) { firstRun = false; - /* Configure epoll for mainline */ - mainline_epoll_fd = epoll_create(4); - if(mainline_epoll_fd == -1) - CO_errExit("Program init - epoll_create mainline failed"); - /* Init mainline */ - threadMain_init(mainline_epoll_fd, &OD_performance[ODA_performance_mainCycleMaxTime]); + /* Init threadMainWait structure and file descriptors */ + threadMainWait_init(MAIN_THREAD_INTERVAL_US); -#ifdef CO_MULTI_THREAD - /* Configure epoll for rt_thread */ - rt_thread_epoll_fd = epoll_create(2); - if(rt_thread_epoll_fd == -1) - CO_errExit("Program init - epoll_create rt_thread failed"); + /* Init threadRT structure and file descriptors */ + CANrx_threadTmr_init(TMR_THREAD_INTERVAL_US); - /* Init threadRT */ - CANrx_threadTmr_init(rt_thread_epoll_fd, TMR_THREAD_INTERVAL_NS, &OD_performance[ODA_performance_timerCycleMaxTime]); - - /* Create rt_thread */ - if(pthread_create(&rt_thread_id, NULL, rt_thread, NULL) != 0) - CO_errExit("Program init - rt_thread creation failed"); - - /* Set priority for rt_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)"); + exit(EXIT_FAILURE); + } if(rtPriority > 0) { struct sched_param param; param.sched_priority = rtPriority; - if(pthread_setschedparam(rt_thread_id, SCHED_FIFO, ¶m) != 0) - CO_errExit("Program init - rt_thread set scheduler failed"); + if (pthread_setschedparam(rt_thread_id, SCHED_FIFO, ¶m) != 0) { + log_printf(LOG_CRIT, DBG_ERRNO, "pthread_setschedparam()"); + exit(EXIT_FAILURE); + } } -#endif -#ifdef CO_309 +#if CO_CONFIG_309 > 0 /* Initialize socket command interface */ switch(commandEnable) { case CMD_LOCAL: if(CO_command_init() != 0) { CO_errExit("Socket command interface initialization failed"); } - printf("%s - Command interface on socket '%s' started ...\n", argv[0], CO_command_socketPath); + log_printf(LOG_INFO, DBG_COMMAND_LOCAL_INFO, CO_command_socketPath); break; case CMD_REMOTE: if(CO_command_init_tcp(CO_command_socket_tcp_port) != 0) { CO_errExit("Socket command interface initialization failed"); } - printf("%s - Command interface on tcp port '%hu' started ...\n", argv[0], CO_command_socket_tcp_port); + log_printf(LOG_INFO, DBG_COMMAND_TCP_INFO, CO_command_socket_tcp_port); break; default: break; @@ -413,58 +401,32 @@ int main (int argc, char *argv[]) { /* start CAN */ CO_CANsetNormalMode(CO->CANmodule[0]); -#ifdef CO_309 +#if CO_CONFIG_309 > 0 pthread_mutex_unlock(&CO_CAN_VALID_mtx); #endif reset = CO_RESET_NOT; - printf("%s - running ...\n", argv[0]); + log_printf(LOG_INFO, DBG_CAN_OPEN_INFO, nodeId, nodeId, "running ..."); while(reset == CO_RESET_NOT && CO_endProgram == 0) { /* loop for normal program execution ******************************************/ - int ready; - struct epoll_event ev; - - ready = epoll_wait(mainline_epoll_fd, &ev, 1, -1); - - if(ready != 1) { - if(errno != EINTR) { - CO_error(0x11100000L + errno); - } - } - - else if(threadMain_process(ev.data.fd, &reset, CO_timer1ms)) { - uint16_t timer1msDiff; - static uint16_t tmr1msPrev = 0; - - /* Calculate time difference */ - timer1msDiff = CO_timer1ms - tmr1msPrev; - tmr1msPrev = CO_timer1ms; - - /* code was processed in the above function. Additional code process below */ + uint32_t timer1usDiff = threadMainWait_process(&reset); #ifdef CO_USE_APPLICATION - /* Execute optional additional application code */ - app_programAsync(timer1msDiff); + app_programAsync(timer1usDiff); #endif - CO_OD_storage_autoSave(&odStorAuto, CO_timer1ms, 60000); - } - - else { - /* No file descriptor was processed. */ - CO_error(0x11200000L); - } + CO_OD_storage_autoSave(&odStorAuto, timer1usDiff, 60000000); } } /* program exit ***************************************************************/ /* join threads */ -#ifdef CO_309 +#if CO_CONFIG_309 > 0 switch (commandEnable) { case CMD_LOCAL: @@ -481,11 +443,10 @@ int main (int argc, char *argv[]) { #endif CO_endProgram = 1; -#ifdef CO_MULTI_THREAD - if(pthread_join(rt_thread_id, NULL) != 0) { - CO_errExit("Program end - pthread_join failed"); + 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 */ @@ -498,16 +459,17 @@ int main (int argc, char *argv[]) { /* delete objects from memory */ CANrx_threadTmr_close(); - threadMain_close(); + threadMainWait_close(); CO_delete((void *)CANdevice0Index); - printf("%s on %s (nodeId=0x%02X) - finished.\n\n", argv[0], CANdevice, nodeId); + log_printf(LOG_INFO, DBG_CAN_OPEN_INFO, nodeId, nodeId, "finished"); /* Flush all buffers (and reboot) */ if(rebootEnable && reset == CO_RESET_APP) { sync(); if(reboot(LINUX_REBOOT_CMD_RESTART) != 0) { - CO_errExit("Program end - reboot failed"); + log_printf(LOG_CRIT, DBG_ERRNO, "reboot()"); + exit(EXIT_FAILURE); } } @@ -515,47 +477,28 @@ int main (int argc, char *argv[]) { } -#ifdef CO_MULTI_THREAD /* Realtime thread for CAN receive and threadTmr ******************************/ static void* rt_thread(void* arg) { /* Endless loop */ while(CO_endProgram == 0) { - int ready; - struct epoll_event ev; - ready = epoll_wait(rt_thread_epoll_fd, &ev, 1, -1); - - if(ready != 1) { - if(errno != EINTR) { - CO_error(0x12100000L + errno); - } - } - - else if(CANrx_threadTmr_process(ev.data.fd)) { - int i; + CANrx_threadTmr_process(); #if CO_NO_TRACE > 0 - /* Monitor variables with trace objects */ - CO_time_process(&CO_time); - for(i=0; itrace[i], *CO_time.epochTimeOffsetMs); - } + /* Monitor variables with trace objects */ + CO_time_process(&CO_time); + for(i=0; itrace[i], *CO_time.epochTimeOffsetMs); + } #endif #ifdef CO_USE_APPLICATION - /* Execute optional additional application code */ - app_program1ms(); + /* Execute optional additional application code */ + app_program1ms(); #endif - } - - else { - /* No file descriptor was processed. */ - CO_error(0x12200000L); - } } return NULL; } -#endif /* CO_MULTI_THREAD */