Add printout of message log from CANopen gateway-ascii (non-standard).

301/CO_SYNC.h

@@ -109,11 +109,14 @@ typedef struct{
     uint16_t            CANdevTxIdx;    /**< From CO_SYNC_init() */
 }CO_SYNC_t;
 
+
+/** Return value for #CO_SYNC_process */
 typedef enum {
     CO_SYNC_NONE            = 0, /**< SYNC not received */
     CO_SYNC_RECEIVED        = 1, /**< SYNC received */
-    CO_SYNC_OUTSIDE_WINDOW  = 2, /**< SYNC received outside SYNC window */
-}CO_SYNC_status_t;
+    CO_SYNC_OUTSIDE_WINDOW  = 2  /**< SYNC received outside SYNC window */
+} CO_SYNC_status_t;
+
 
 /**
  * Initialize SYNC object.

301/CO_config.h

@@ -349,6 +349,14 @@ extern "C" {
 #endif
 
 
+/**
+ * Size of message log buffer in ASCII gateway object.
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_GTWA_LOG_BUF_SIZE 2000
+#endif
+
+
 /** @} */
 
 #ifdef __cplusplus

301/CO_fifo.c

@@ -149,7 +149,7 @@ size_t CO_fifo_write(CO_fifo_t *fifo,
     size_t i;
     char *bufDest;
 
-    if (fifo == NULL || buf == NULL) {
+    if (fifo == NULL || fifo->buf == NULL || buf == NULL) {
         return 0;
     }
 

301/CO_fifo.h

@@ -170,7 +170,7 @@ static inline size_t CO_fifo_getOccupied(CO_fifo_t *fifo) {
  * @return true, if write was successful (enough space in fifo buffer)
  */
 static inline bool_t CO_fifo_putc(CO_fifo_t *fifo, const char c) {
-    if (fifo != NULL) {
+    if (fifo != NULL && fifo->buf != NULL) {
         size_t writePtrNext = fifo->writePtr + 1;
         if (writePtrNext != fifo->readPtr &&
             !(writePtrNext == fifo->bufSize && fifo->readPtr == 0))
@@ -184,6 +184,26 @@ static inline bool_t CO_fifo_putc(CO_fifo_t *fifo, const char c) {
 }
 
 
+/**
+ * Put one character into CO_fifo_t buffer object
+ *
+ * Overwrite old characters, if run out of space
+ *
+ * @param fifo This object
+ * @param c Character to put
+ */
+static inline void CO_fifo_putc_ov(CO_fifo_t *fifo, const char c) {
+    if (fifo != NULL && fifo->buf != NULL) {
+        fifo->buf[fifo->writePtr] = c;
+
+        if (++fifo->writePtr == fifo->bufSize) fifo->writePtr = 0;
+        if (fifo->readPtr == fifo->writePtr) {
+            if (++fifo->readPtr == fifo->bufSize) fifo->readPtr = 0;
+        }
+    }
+}
+
+
 /**
  * Get one character from CO_fifo_t buffer object
  *

309/CO_gateway_ascii.c

@@ -73,6 +73,12 @@ CO_ReturnError_t CO_GTWA_init(CO_GTWA_t* gtwa,
                  &gtwa->commBuf[0],
                  CO_CONFIG_GTWA_COMM_BUF_SIZE + 1);
 
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG
+    CO_fifo_init(&gtwa->logFifo,
+                 &gtwa->logBuf[0],
+                 CO_CONFIG_GTWA_LOG_BUF_SIZE + 1);
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG) */
+
     return CO_ERROR_NO;
 }
 
@@ -91,6 +97,20 @@ void CO_GTWA_initRead(CO_GTWA_t* gtwa,
 }
 
 
+/******************************************************************************/
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG
+void CO_GTWA_log_print(CO_GTWA_t* gtwa, const char *message) {
+    if (gtwa != NULL && message != NULL) {
+        const char *c;
+
+        for (c = &message[0]; *c != 0; c++) {
+            CO_fifo_putc_ov(&gtwa->logFifo, *c);
+        }
+    }
+}
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG */
+
+
 /*******************************************************************************
  * HELPER FUNCTIONS
  ******************************************************************************/
@@ -113,6 +133,7 @@ static const char *CO_GTWA_helpString =
 "[<net>] set sdo_block <value>            # Enable/disable SDO block transfer.\n" \
 "\n" \
 "help                                     # Print this help.\n" \
+"log                                      # Print message log.\n" \
 "\n" \
 "Datatypes:\n" \
 "b                  # Boolean.\n" \
@@ -955,7 +976,16 @@ void CO_GTWA_process(CO_GTWA_t *gtwa,
         }
 #endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_NMT */
 
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LSS
         /* TODO LSS */
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LSS */
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG
+        /* Print message log */
+        else if (strcmp(token, "log") == 0) {
+            gtwa->state = CO_GTWA_ST_LOG;
+        }
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG */
 
 #if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_HELP
         /* Print help */
@@ -963,7 +993,7 @@ void CO_GTWA_process(CO_GTWA_t *gtwa,
             gtwa->helpStringOffset = 0;
             gtwa->state = CO_GTWA_ST_HELP;
         }
-#endif
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_HELP */
 
         /* Unrecognized command */
         else {
@@ -1119,6 +1149,22 @@ void CO_GTWA_process(CO_GTWA_t *gtwa,
     }
 #endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO */
 
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG
+    /* print message log */
+    else if (gtwa->state == CO_GTWA_ST_LOG) {
+        do {
+            gtwa->respBufCount = CO_fifo_read(&gtwa->logFifo, gtwa->respBuf,
+                                              CO_GTWA_RESP_BUF_SIZE, NULL);
+            respBufTransfer(gtwa);
+
+            if (CO_fifo_getOccupied(&gtwa->logFifo) == 0) {
+                gtwa->state = CO_GTWA_ST_IDLE;
+                break;
+            }
+        } while (gtwa->respHold == false);
+    }
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG */
+
 #if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_HELP
     /* Print help string (in multiple segments if necessary) */
     else if (gtwa->state == CO_GTWA_ST_HELP) {

309/CO_gateway_ascii.h

@@ -85,6 +85,7 @@ Command strings start with '"["<sequence>"]"' followed by:
 [<net>] set sdo_block <value>            # Enable/disable SDO block transfer.
 
 help                                     # Print this help.
+log                                      # Print message log.
 
 Datatypes:
 b                  # Boolean.
@@ -208,6 +209,8 @@ typedef enum {
     CO_GTWA_ST_READ = 0x10U,
     /** SDO 'write' (download) */
     CO_GTWA_ST_WRITE = 0x11U,
+    /** print message log */
+    CO_GTWA_ST_LOG = 0x80U,
     /** print 'help' text */
     CO_GTWA_ST_HELP = 0x90U
 } CO_GTWA_state_t;
@@ -305,6 +308,12 @@ typedef struct {
     /** NMT object from CO_GTWA_init() */
     CO_NMT_t *NMT;
 #endif
+#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG) || defined CO_DOXYGEN
+    /** Message log buffer of usable size @ref CO_CONFIG_GTWA_LOG_BUF_SIZE */
+    char logBuf[CO_CONFIG_GTWA_LOG_BUF_SIZE + 1];
+    /** CO_fifo_t object for message log (not pointer) */
+    CO_fifo_t logFifo;
+#endif
 #if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_HELP) || defined CO_DOXYGEN
     /** Offset, when printing help text */
     size_t helpStringOffset;
@@ -386,6 +395,23 @@ static inline size_t CO_GTWA_write(CO_GTWA_t* gtwa,
 }
 
 
+#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG) || defined CO_DOXYGEN
+/**
+ * Print message log string into fifo buffer
+ *
+ * This function enables recording of system log messages including CANopen
+ * events. Function can be called by application for recording any message.
+ * Message is copied to internal fifo buffer. In case fifo is full, old messages
+ * will be owerwritten. Message log fifo can be read with non-standard command
+ * "log". After log is read, it is emptied.
+ *
+ * @param gtwa This object
+ * @param message Null terminated string
+ */
+void CO_GTWA_log_print(CO_GTWA_t* gtwa, const char *message);
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG */
+
+
 /**
  * Process Gateway-ascii object
  *

example/CO_driver_target.h

@@ -120,6 +120,7 @@ extern "C" {
                        CO_CONFIG_GTW_ASCII_PRINT_HELP)
 #define CO_CONFIG_GTW_BLOCK_DL_LOOP 1
 #define CO_CONFIG_GTWA_COMM_BUF_SIZE 2000
+#define CO_CONFIG_GTWA_LOG_BUF_SIZE 2000
 #endif
 
 

socketCAN/CO_Linux_threads.c

@@ -33,6 +33,7 @@
 
 #include <stdlib.h>
 #include <unistd.h>
+#include <syslog.h>
 #include <time.h>
 #include <sys/epoll.h>
 #include <sys/eventfd.h>
@@ -67,11 +68,12 @@ static inline uint64_t CO_LinuxThreads_clock_gettime_us(void)
 /* 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;
-    size_t nWritten = 0;
+    /* 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) {
+        if (n >= 0) {
             nWritten = (size_t)n;
         }
         else {

socketCAN/CO_driver.c

@@ -28,6 +28,7 @@
 #include <stdlib.h>
 #include <unistd.h>
 #include <errno.h>
+#include <syslog.h>
 #include <linux/can/raw.h>
 #include <linux/can/error.h>
 #include <linux/net_tstamp.h>

socketCAN/CO_driver_target.h

@@ -126,6 +126,7 @@ extern "C" {
                        CO_CONFIG_GTW_ASCII_PRINT_HELP)
 #define CO_CONFIG_GTW_BLOCK_DL_LOOP 3
 #define CO_CONFIG_GTWA_COMM_BUF_SIZE 2000
+#define CO_CONFIG_GTWA_LOG_BUF_SIZE 10000
 #endif
 
 

socketCAN/CO_error.c

@@ -27,6 +27,7 @@
 #include <stdlib.h>
 #include <stdio.h>
 #include <stdbool.h>
+#include <syslog.h>
 #include <time.h>
 #include <linux/can/error.h>
 

socketCAN/CO_error.h

@@ -55,8 +55,11 @@ extern "C" {
 /**
  * Message logging function.
  *
- * Default usage is to print messages into system log with syslog() call. By
- * default system stores messages in /var/log/syslog file.
+ * Function must be defined by application. It should record log message to some
+ * place, for example syslog() call in Linux or logging functionality in
+ * CANopen gateway @ref CO_CANopen_309_3.
+ *
+ * By default system stores messages in /var/log/syslog file.
  * Log can optionally be configured before, for example to filter out less
  * critical errors than LOG_NOTICE, specify program name, print also process PID
  * and print also to standard error, set 'user' type of program, use:
@@ -69,9 +72,7 @@ extern "C" {
  *                 LOG_NOTICE, LOG_INFO, LOG_DEBUG
  * @param format format string as in printf
  */
-#ifdef CO_DOXYGEN
 void log_printf(int priority, const char *format, ...);
-#endif
 
 
 /**

socketCAN/CO_error_msgs.h

@@ -32,17 +32,6 @@ extern "C" {
 #endif
 
 
-/*
- * Message logging function.
- */
-#ifndef log_printf
-#include <syslog.h>
-
-#define log_printf(macropar_prio, macropar_message, ...) \
-        syslog(macropar_prio, macropar_message, ##__VA_ARGS__)
-#endif
-
-
 /*
  * Message definitions for Linux CANopen socket driver (notice and errors)
  */

socketCAN/CO_main_basic.c

@@ -31,6 +31,9 @@
 #include <sched.h>
 #include <signal.h>
 #include <errno.h>
+#include <stdarg.h>
+#include <syslog.h>
+#include <time.h>
 #include <sys/epoll.h>
 #include <net/if.h>
 #include <linux/reboot.h>
@@ -89,6 +92,34 @@ static void sigHandler(int sig) {
     CO_endProgram = 1;
 }
 
+/* Message logging function */
+void log_printf(int priority, const char *format, ...) {
+    va_list ap;
+
+    va_start(ap, format);
+    vsyslog(priority, format, ap);
+    va_end(ap);
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG
+    if (CO != NULL) {
+        char buf[200];
+        time_t timer;
+        struct tm* tm_info;
+        size_t len;
+
+        timer = time(NULL);
+        tm_info = localtime(&timer);
+        len = strftime(buf, sizeof(buf), "%Y-%m-%d %H:%M:%S: ", tm_info);
+
+        va_start(ap, format);
+        vsnprintf(buf + len, sizeof(buf) - len - 2, format, ap);
+        va_end(ap);
+        strcat(buf, "\r\n");
+        CO_GTWA_log_print(CO->gtwa, buf);
+    }
+#endif
+}
+
 /* callback for emergency messages */
 static void EmergencyRxCallback(const uint16_t ident,
                                 const uint16_t errorCode,