Added systemd watchdog support for gptp daemon

5 files changed, 118 insertions, 6 deletions

diff --git a/daemons/gptp/linux/build/Makefile b/daemons/gptp/linux/build/Makefile
index c270847f..e4126464 100644
--- a/daemons/gptp/linux/build/Makefile
+++ b/daemons/gptp/linux/build/Makefile
@@ -63,7 +63,7 @@ OBJ_FILES = $(OBJ_DIR)/ptp_message.o\
 		 $(OBJ_DIR)/gptp_log.o\
 		 $(OBJ_DIR)/platform.o \
 		 $(OBJ_DIR)/ini.o \
-		 $(OBJ_DIR)/gptp_cfg.o
+		 $(OBJ_DIR)/gptp_cfg.o 
 
 HEADER_FILES = $(COMMON_DIR)/avbts_port.hpp\
 		$(COMMON_DIR)/avbts_ostimerq.hpp\
@@ -85,7 +85,7 @@ HEADER_FILES = $(COMMON_DIR)/avbts_port.hpp\
 		$(SRC_DIR)/linux_ipc.hpp\
 		$(SRC_DIR)/linux_hal_common.hpp\
 		$(SRC_DIR)/linux_hal_persist_file.hpp\
-		$(SRC_DIR)/platform.hpp
+		$(SRC_DIR)/platform.hpp 
 
 ifeq ($(ARCH),I210)
 	IGB_LIB_INCPATH=../../../../lib/igb/
@@ -112,12 +112,19 @@ else
 endif
 
 ifeq ($(GENIVI_DLT),1)
-	GENIVI_DLT_INCLUDE_PATH=/usr/local/include/dlt/
+	GENIVI_DLT_INCLUDE_PATH=/usr/include/dlt/
 	GENIVI_DLT_LIB_PATH=/usr/local/lib/x86_64-linux-gnu/static/
 	CFLAGS_G += -I$(GENIVI_DLT_INCLUDE_PATH) -DGENIVI_DLT
 	LDFLAGS_G += -ldlt -L$(GENIVI_DLT_LIB_PATH)
 endif
 
+ifeq ($(SYSTEMD_WATCHDOG),1)
+	CFLAGS_G += -DSYSTEMD_WATCHDOG
+	LDFLAGS_G += -lsystemd
+	OBJ_FILES += $(OBJ_DIR)/watchdog.o
+	HEADER_FILES += $(SRC_DIR)/watchdog.hpp
+endif
+
 LDFLAGS_G += -lpthread -lrt
 
 CFLAGS = $(CFLAGS_G)
@@ -176,6 +183,8 @@ $(OBJ_DIR)/gptp_cfg.o: $(COMMON_DIR)/gptp_cfg.cpp $(HEADER_FILES)
 $(OBJ_DIR)/ini.o: $(COMMON_DIR)/ini.c $(HEADER_FILES)
 	$(CC) $(CFLAGS) -c $(COMMON_DIR)/ini.c -o $(OBJ_DIR)/ini.o
 
+$(OBJ_DIR)/watchdog.o: $(SRC_DIR)/watchdog.cpp $(HEADER_FILES)
+	$(CXX) $(CPPFLAGS) $(CXXFLAGS) -c $(SRC_DIR)/watchdog.cpp -o $(OBJ_DIR)/watchdog.o
 clean:
 	$(RM) *~ $(OBJ_DIR)/*.o  $(OBJ_DIR)/daemon_cl
 
diff --git a/daemons/gptp/linux/src/daemon_cl.cpp b/daemons/gptp/linux/src/daemon_cl.cpp
index 5d193458..1e4a55cb 100644
--- a/daemons/gptp/linux/src/daemon_cl.cpp
+++ b/daemons/gptp/linux/src/daemon_cl.cpp
@@ -56,6 +56,10 @@
 #include <unistd.h>
 #include <string.h>
 
+#ifdef SYSTEMD_WATCHDOG
+    #include <watchdog.hpp>
+#endif
+
 #define PHY_DELAY_GB_TX_I20 184 //1G delay
 #define PHY_DELAY_GB_RX_I20 382 //1G delay
 #define PHY_DELAY_MB_TX_I20 1044//100M delay
@@ -94,6 +98,33 @@ void print_usage( char *arg0 ) {
 		);
 }
 
+int watchdog_setup(OSThreadFactory *thread_factory)
+{
+#ifdef SYSTEMD_WATCHDOG
+    SystemdWatchdogHandler *watchdog = new SystemdWatchdogHandler();
+    OSThread *watchdog_thread = thread_factory->createThread();
+    int watchdog_result;
+    long unsigned int watchdog_interval;
+    watchdog_interval = watchdog->getSystemdWatchdogInterval(&watchdog_result);
+    if (watchdog_result)
+    {
+        GPTP_LOG_INFO("Watchtog interval read from service file: %lld us", watchdog_interval);
+        watchdog->update_interval = watchdog_interval / 2;
+        GPTP_LOG_STATUS("Starting watchdog handler (Update every: %lld us)", watchdog->update_interval);
+        watchdog_thread->start(watchdogUpdateThreadFunction, watchdog);
+        return 0;
+    } else if (watchdog_result < 0) {
+        GPTP_LOG_ERROR("Watchdog settings read error.");
+        return -1;
+    } else {
+        GPTP_LOG_STATUS("Watchdog disabled");
+        return 0;
+    }
+#else
+    return 0;
+#endif
+}
+
 static IEEE1588Clock *pClock = NULL;
 static IEEE1588Port *pPort = NULL;
 
@@ -123,6 +154,7 @@ int main(int argc, char **argv)
 	memset(config_file_path, 0, 512);
 
 	GPTPPersist *pGPTPPersist = NULL;
+    LinuxThreadFactory *thread_factory = new LinuxThreadFactory();
 
 	// Block SIGUSR1
 	{
@@ -137,7 +169,10 @@ int main(int argc, char **argv)
 
 	GPTP_LOG_REGISTER();
 	GPTP_LOG_INFO("gPTP starting");
-
+    if (watchdog_setup(thread_factory) != 0) {
+        GPTP_LOG_ERROR("Watchdog handler setup error");
+        return -1;
+    }
 	int phy_delay[4]={0,0,0,0};
 	bool input_delay=false;
 
@@ -164,7 +199,6 @@ int main(int argc, char **argv)
 		new LinuxNetworkInterfaceFactory;
 	OSNetworkInterfaceFactory::registerFactory
 		(factory_name_t("default"), default_factory);
-	LinuxThreadFactory *thread_factory = new LinuxThreadFactory();
 	LinuxTimerQueueFactory *timerq_factory = new LinuxTimerQueueFactory();
 	LinuxLockFactory *lock_factory = new LinuxLockFactory();
 	LinuxTimerFactory *timer_factory = new LinuxTimerFactory();
diff --git a/daemons/gptp/linux/src/linux_hal_common.cpp b/daemons/gptp/linux/src/linux_hal_common.cpp
index 19405b15..4879640b 100644
--- a/daemons/gptp/linux/src/linux_hal_common.cpp
+++ b/daemons/gptp/linux/src/linux_hal_common.cpp
@@ -475,8 +475,10 @@ bool LinuxTimestamper::post_init( int ifindex, int sd, TicketingLock *lock ) {
 LinuxTimestamper::~LinuxTimestamper() {}
 
 unsigned long LinuxTimer::sleep(unsigned long micros) {
-	struct timespec req = { 0, (long int)(micros * 1000) };
+    struct timespec req;
 	struct timespec rem;
+    req.tv_sec = micros / 1000000;
+    req.tv_nsec = micros % 1000000 * 1000;
 	int ret = nanosleep( &req, &rem );
 	while( ret == -1 && errno == EINTR ) {
 		req = rem;
diff --git a/daemons/gptp/linux/src/watchdog.cpp b/daemons/gptp/linux/src/watchdog.cpp
new file mode 100644
index 00000000..c433455c
--- /dev/null
+++ b/daemons/gptp/linux/src/watchdog.cpp
@@ -0,0 +1,46 @@
+#include "watchdog.hpp"
+#include "avbts_osthread.hpp"
+#include "gptp_log.hpp"
+#include <systemd/sd-daemon.h>
+
+
+OSThreadExitCode watchdogUpdateThreadFunction(void *arg)
+{
+    SystemdWatchdogHandler *watchdog = (SystemdWatchdogHandler*) arg;
+    watchdog->run_update();
+    return osthread_ok;
+}
+
+
+SystemdWatchdogHandler::SystemdWatchdogHandler()
+{
+    GPTP_LOG_INFO("Creating Systemd watchdog handler.");
+    LinuxTimerFactory timer_factory = LinuxTimerFactory();
+    timer = timer_factory.createTimer();
+}
+
+SystemdWatchdogHandler::~SystemdWatchdogHandler()
+{
+    //Do nothing
+}
+
+long unsigned int
+SystemdWatchdogHandler::getSystemdWatchdogInterval(int *result)
+{
+    long unsigned int watchdog_interval; //in microseconds
+    *result = sd_watchdog_enabled(0, &watchdog_interval);
+    return watchdog_interval;
+}
+
+void SystemdWatchdogHandler::run_update()
+{
+    while(1)
+    {
+        GPTP_LOG_DEBUG("NOTIFYING WATCHDOG.");
+        sd_notify(0, "WATCHDOG=1");
+        GPTP_LOG_DEBUG("GOING TO SLEEP %lld", update_interval);
+        timer->sleep(update_interval);
+        GPTP_LOG_DEBUG("WATCHDOG WAKE UP");
+     }
+}
+
diff --git a/daemons/gptp/linux/src/watchdog.hpp b/daemons/gptp/linux/src/watchdog.hpp
new file mode 100644
index 00000000..0ea76c2b
--- /dev/null
+++ b/daemons/gptp/linux/src/watchdog.hpp
@@ -0,0 +1,21 @@
+#ifndef SYSTEMDWATCHDOGHANDLER_H
+#define SYSTEMDWATCHDOGHANDLER_H
+#include <linux_hal_common.hpp>
+#include <avbts_ostimer.hpp>
+
+
+OSThreadExitCode watchdogUpdateThreadFunction(void *arg);
+
+class SystemdWatchdogHandler
+{
+public:
+    long unsigned int update_interval;
+    long unsigned int getSystemdWatchdogInterval(int *result);
+    void run_update();
+    SystemdWatchdogHandler();
+    virtual ~SystemdWatchdogHandler();
+private:
+    OSTimer *timer;
+};
+
+#endif // SYSTEMDWATCHDOGHANDLER_H