Implement regular event launching using systemd event loop

Regular events is made launch a timerfd event from binder event loop and then
in the event handler, reschedule next launch in the future based upon the
signal frequency.

Change-Id: I0b1e84eb2135474f4bcc5ee256ba513eea4035a6
Signed-off-by: Romain Forlot <romain.forlot@iot.bzh>

3 files changed, 49 insertions, 21 deletions

diff --git a/src/diagnostic/diagnostic-manager.cpp b/src/diagnostic/diagnostic-manager.cpp
index 482a88fe..e7293ca6 100644
--- a/src/diagnostic/diagnostic-manager.cpp
+++ b/src/diagnostic/diagnostic-manager.cpp
@@ -27,6 +27,7 @@
 #define MAX_RECURRING_DIAGNOSTIC_FREQUENCY_HZ 10
 #define MAX_SIMULTANEOUS_DIAG_REQUESTS 50
 #define MAX_REQUEST_ENTRIES 50
+#define TIMERFD_ACCURACY 0
 #define MICRO 1000000
 
 diagnostic_manager_t::diagnostic_manager_t()
@@ -250,18 +251,11 @@ bool diagnostic_manager_t::add_recurring_request(DiagnosticRequest* request, con
 				DEBUG(binder_interface, "Added recurring diagnostic request (freq: %f) on bus %s: %s",
 						frequencyHz, bus_->get_device_name().c_str(), request_string);
 
+				uint64_t usec;
+				usec = sd_event_now(afb_daemon_get_event_loop(binder_interface->daemon), CLOCK_MONOTONIC, &usec) + (uint64_t)(frequency_clock_t::frequency_to_period(frequencyHz)*MICRO);
 				if(sd_event_add_time(afb_daemon_get_event_loop(binder_interface->daemon), &source,
-					CLOCK_MONOTONIC, (uint64_t)frequencyHz*MICRO, 0,send_request, request) >= 0)
-				{
-					if(sd_event_source_set_enabled(source, SD_EVENT_ON) >= 0)
-						recurring_requests_.push_back(entry);
-					else
-					{
-						ERROR(binder_interface, "add_reccurring_request: Request has not been enabled, it will occurs only one time");
-						free_request_entries_.push_back(entry);
-						added = false;
-					}
-				}
+						CLOCK_MONOTONIC, usec, TIMERFD_ACCURACY, send_request, request) >= 0)
+					recurring_requests_.push_back(entry);
 				else
 				{
 					ERROR(binder_interface, "add_recurring_request: Request fails to be schedule through event loop");
@@ -385,7 +379,18 @@ int diagnostic_manager_t::send_request(sd_event_source *s, uint64_t usec, void *
 		adr->get_timeout_clock().tick();
 		adr->set_in_flight(true);
 		return 1;
+
+		usec = usec + (uint64_t)(frequency_clock_t::frequency_to_period(adr->get_frequency_clock().get_frequency())*MICRO);
+		DEBUG(binder_interface, "send_request: usec: %d", usec);
+		if(sd_event_source_set_time(s, usec) >= 0)
+			if(sd_event_source_set_enabled(s, SD_EVENT_ON) >= 0)
+			{
+				dm.recurring_requests_.push_back(adr);
+				return 1;
+			}
 	}
+	dm.recurring_requests_.push_back(adr);
+	ERROR(binder_interface, "send_request: Something goes wrong when submitting a new request to the CAN bus");
 	return -1;
 }
 
diff --git a/src/utils/timer.cpp b/src/utils/timer.cpp
index e708a7ed..9d385fe5 100644
--- a/src/utils/timer.cpp
+++ b/src/utils/timer.cpp
@@ -15,25 +15,43 @@
  * limitations under the License.
  */
 
+#include <time.h>
 #include <stdlib.h> 
 
 #include "timer.hpp"
 
 #define MS_PER_SECOND 1000
 
+long long int system_time_us()
+{
+	struct timespec t_usec;
+	long long int timestamp_usec;
+	
+	if(!::clock_gettime(CLOCK_MONOTONIC, &t_usec))
+		timestamp_usec = (t_usec.tv_nsec / 1000ll) + (t_usec.tv_sec* 1000000ll);
+	return timestamp_usec;
+}
+
 long long int system_time_ms()
 {
-	struct timeb t_msec;
+	struct timespec t_msec;
 	long long int timestamp_msec;
 	
-	if(!::ftime(&t_msec))
-	{
-		timestamp_msec = (t_msec.time) * 1000ll + 
-			t_msec.millitm;
-	}
+	if(!::clock_gettime(CLOCK_MONOTONIC, &t_msec))
+		timestamp_msec = (t_msec.tv_nsec / 1000000ll) + (t_msec.tv_sec* 1000ll);
 	return timestamp_msec;
 }
 
+long long int system_time_s()
+{
+	struct timespec t_sec;
+	long long int timestamp_sec;
+	
+	if(!::clock_gettime(CLOCK_MONOTONIC, &t_sec))
+		timestamp_sec = t_sec.tv_sec;
+	return timestamp_sec;
+}
+
 frequency_clock_t::frequency_clock_t()
 	: frequency_{10.0}, last_tick_{0}, time_function_{nullptr}
 {}
@@ -66,12 +84,17 @@ bool frequency_clock_t::elapsed(bool stagger)
 	if(!started() && stagger)
 		last_tick_ = get_time_function()() - (rand() % int(period));
 	else
-		// Make sure it ticks the the first call to conditionalTick(...)
+		// Make sure it ticks the the first call
 		elapsed_time = !started() ? period : get_time_function()() - last_tick_;
 
 	return frequency_ == 0 || elapsed_time >= period;
 }
 
+float frequency_clock_t::get_frequency() const
+{
+	return frequency_;
+}
+
 /// @brief Force the clock to tick, regardless of it its time has actually
 /// elapsed.
 void frequency_clock_t::tick()
diff --git a/src/utils/timer.hpp b/src/utils/timer.hpp
index b58397ba..b34fdf64 100644
--- a/src/utils/timer.hpp
+++ b/src/utils/timer.hpp
@@ -17,8 +17,6 @@
 
 #pragma once
 
-#include <sys/timeb.h>
-
 /*
  * @brief return epoch in milliseconds
  *
@@ -26,8 +24,9 @@
  */
 typedef long long int (*time_function_t)();
 
-
+long long int system_time_us();
 long long int system_time_ms();
+long long int system_time_s();
 
 /**
  * @class frequency_clock_t 
@@ -53,6 +52,7 @@ public:
 	frequency_clock_t(float frequency);
 	frequency_clock_t(float frequency, unsigned long last_tick, time_function_t time_function);
 
+	float get_frequency() const;
 	static float frequency_to_period(float frequency);
 	bool started();
 	time_function_t get_time_function();