Implemente way to send diagnostic request when subscribed.

When subscribed, the signal is added to recurring request list
of diagnostic manager and an event is added to the systemd
event loop with timer set using frequency parameter from the
requested signal.

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

6 files changed, 93 insertions, 1 deletions

diff --git a/src/diagnostic/active-diagnostic-request.cpp b/src/diagnostic/active-diagnostic-request.cpp
index 505d816e..7a43a0d4 100644
--- a/src/diagnostic/active-diagnostic-request.cpp
+++ b/src/diagnostic/active-diagnostic-request.cpp
@@ -57,6 +57,11 @@ active_diagnostic_request_t::active_diagnostic_request_t(std::shared_ptr<can_bus
 	  in_flight_{false}, frequency_clock_{frequency_clock_t(frequencyHz)}, timeout_clock_{frequency_clock_t(10)}
 {}
 
+uint32_t active_diagnostic_request_t::get_id() const
+{
+	return id_;
+}
+
 std::shared_ptr<can_bus_dev_t> active_diagnostic_request_t::get_can_bus_dev()
 {
 	return bus_;
@@ -77,6 +82,16 @@ bool active_diagnostic_request_t::get_in_flight() const
 	return in_flight_;
 }
 
+frequency_clock_t& active_diagnostic_request_t::get_frequency_clock()
+{
+	return frequency_clock_;
+}
+
+frequency_clock_t& active_diagnostic_request_t::get_timeout_clock()
+{
+	return timeout_clock_;
+}
+
 void active_diagnostic_request_t::set_handle(DiagnosticShims& shims, DiagnosticRequest* request)
 {
 	handle_ = new DiagnosticRequestHandle(generate_diagnostic_request(&shims, request, nullptr));
@@ -87,6 +102,13 @@ void active_diagnostic_request_t::set_in_flight(bool val)
 	in_flight_ = val;
 }
 
+bool active_diagnostic_request_t::should_send()
+{
+	return !get_in_flight() && (
+			(!get_recurring() && !request_completed()) ||
+			(get_recurring() && get_frequency_clock().elapsed(true)));
+}
+
 bool active_diagnostic_request_t::timed_out()
 {
 	// don't use staggered start with the timeout clock
diff --git a/src/diagnostic/active-diagnostic-request.hpp b/src/diagnostic/active-diagnostic-request.hpp
index 88d40084..fb87c839 100644
--- a/src/diagnostic/active-diagnostic-request.hpp
+++ b/src/diagnostic/active-diagnostic-request.hpp
@@ -88,14 +88,19 @@ public:
 		const DiagnosticResponseDecoder decoder,
 		const DiagnosticResponseCallback callback, float frequencyHz);
 	
+	uint32_t get_id() const;
 	std::shared_ptr<can_bus_dev_t> get_can_bus_dev();
 	DiagnosticRequestHandle* get_handle();
 	bool get_recurring() const;
 	bool get_in_flight() const;
+	frequency_clock_t& get_frequency_clock();
+	frequency_clock_t& get_timeout_clock();
 	
 	void set_handle(DiagnosticShims& shims, DiagnosticRequest* request);
 	void set_in_flight(bool val);
 
+	bool should_send();
+
 	bool timed_out();
 	bool response_received() const;
 	bool request_completed();
diff --git a/src/diagnostic/diagnostic-manager.cpp b/src/diagnostic/diagnostic-manager.cpp
index 5103bf10..6e8a68cf 100644
--- a/src/diagnostic/diagnostic-manager.cpp
+++ b/src/diagnostic/diagnostic-manager.cpp
@@ -261,6 +261,60 @@ bool diagnostic_manager_t::add_recurring_request(DiagnosticRequest* request, con
 	return added;
 }
 
+bool diagnostic_manager_t::conflicting(active_diagnostic_request_t* request, active_diagnostic_request_t* candidate) const
+{
+	return (candidate->get_in_flight() && candidate != request &&
+			candidate->get_can_bus_dev() == request->get_can_bus_dev() &&
+			candidate->get_id() == request->get_id());
+}
+
+
+/// @brief Returns true if there are no other active requests to the same arbitration ID.
+bool diagnostic_manager_t::clear_to_send(active_diagnostic_request_t* request) const
+{
+	for ( auto entry : non_recurring_requests_)
+	{
+		if(conflicting(request, entry))
+			return false;
+	}
+
+	for ( auto entry : recurring_requests_)
+	{
+		if(conflicting(request, entry))
+			return false;
+	}
+	return true;
+}
+
+int diagnostic_manager_t::send_request(sd_event_source *s, uint64_t usec, void *userdata)
+{
+	diagnostic_manager_t& dm = configuration_t::instance().get_diagnostic_manager();
+	DiagnosticRequest* request = (DiagnosticRequest*)userdata;
+	active_diagnostic_request_t* adr = dm.lookup_recurring_request(request);
+
+	if(adr != nullptr && adr->get_can_bus_dev() == dm.get_can_bus_dev() && adr->should_send() &&
+		dm.clear_to_send(adr))
+	{
+		DEBUG(binder_interface, "Got active_diagnostic_request from recurring_requests_ queue.");
+		adr->get_frequency_clock().tick();
+		start_diagnostic_request(&dm.get_shims(), adr->get_handle());
+		if(adr->get_handle()->completed && !adr->get_handle()->success)
+		{
+			DEBUG(binder_interface, "Fatal error sending diagnostic request");
+			return 0;
+		}
+		adr->get_timeout_clock().tick();
+		adr->set_in_flight(true);
+		return 1;
+	}
+	return -1;
+}
+
+DiagnosticShims& diagnostic_manager_t::get_shims()
+{
+	return shims_;
+}
+
 bool diagnostic_manager_t::shims_send(const uint32_t arbitration_id, const uint8_t* data, const uint8_t size)
 {
 	std::shared_ptr<can_bus_dev_t> can_bus_dev = configuration_t::instance().get_diagnostic_manager().get_can_bus_dev();
diff --git a/src/diagnostic/diagnostic-manager.hpp b/src/diagnostic/diagnostic-manager.hpp
index 39aae321..aa98c0b7 100644
--- a/src/diagnostic/diagnostic-manager.hpp
+++ b/src/diagnostic/diagnostic-manager.hpp
@@ -17,6 +17,7 @@
 
 #pragma once
 
+#include <systemd/sd-event.h>
 #include <queue>
 #include <vector>
 
@@ -94,4 +95,8 @@ public:
 	bool add_recurring_request(DiagnosticRequest* request, const char* name,
 		bool waitForMultipleResponses, const DiagnosticResponseDecoder decoder,
 		const DiagnosticResponseCallback callback, float frequencyHz);
+	
+	bool conflicting(active_diagnostic_request_t* request, active_diagnostic_request_t* candidate) const;
+	bool clear_to_send(active_diagnostic_request_t* request) const;
+	static int send_request(sd_event_source *s, uint64_t usec, void *userdata);
 };
diff --git a/src/diagnostic/diagnostic-message.cpp b/src/diagnostic/diagnostic-message.cpp
index 62e97b8f..cb4e691b 100644
--- a/src/diagnostic/diagnostic-message.cpp
+++ b/src/diagnostic/diagnostic-message.cpp
@@ -58,6 +58,11 @@ const std::string& obd2_signal_t::get_prefix() const
 	return prefix_;
 }
 
+int obd2_signal_t::get_frequency() const
+{
+	return frequency_;
+}
+
 void obd2_signal_t::set_prefix(std::string val)
 {
 	prefix_ = val;
diff --git a/src/diagnostic/diagnostic-message.hpp b/src/diagnostic/diagnostic-message.hpp
index 8301917f..02c6a9f6 100644
--- a/src/diagnostic/diagnostic-message.hpp
+++ b/src/diagnostic/diagnostic-message.hpp
@@ -62,6 +62,7 @@ class obd2_signal_t {
 		const std::string& get_generic_name() const;
 		const std::string get_name() const;
 		const std::string& get_prefix() const;
+		int get_frequency() const;
 
 		void set_prefix(std::string val);
 
@@ -71,5 +72,5 @@ class obd2_signal_t {
 		bool is_obd2_request(DiagnosticRequest *request);
 		bool is_obd2_signal(const char *name);
 
-		float decode_obd2_response(const DiagnosticResponse* response, float parsedPayload);
+		static float decode_obd2_response(const DiagnosticResponse* response, float parsedPayload);
 };
\ No newline at end of file