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>

10 files changed, 167 insertions, 8 deletions

diff --git a/src/configuration.cpp b/src/configuration.cpp
index e56dc1c..85840f8 100644
--- a/src/configuration.cpp
+++ b/src/configuration.cpp
@@ -89,14 +89,19 @@ std::vector<can_signal_t>& configuration_t::get_can_signals()
 	return can_signals_[active_message_set_];
 }
 
-const std::vector<can_message_definition_t>& configuration_t::get_can_message_definition()
+std::vector<obd2_signal_t>& configuration_t::get_obd2_signals()
 {
-	return can_message_definition_[active_message_set_];
+	return obd2_signals_[active_message_set_];
 }
 
-std::vector<obd2_signal_t>& configuration_t::get_obd2_signals()
+const std::vector<std::string>& configuration_t::get_signals_prefix() const
 {
-	return obd2_signals_[active_message_set_];
+	return signals_prefix_;
+}
+
+const std::vector<can_message_definition_t>& configuration_t::get_can_message_definition()
+{
+	return can_message_definition_[active_message_set_];
 }
 
 uint32_t configuration_t::get_signal_id(obd2_signal_t& sig) const
diff --git a/src/diagnostic/active-diagnostic-request.cpp b/src/diagnostic/active-diagnostic-request.cpp
index 505d816..7a43a0d 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 88d4008..fb87c83 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 5103bf1..6e8a68c 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 39aae32..aa98c0b 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 62e97b8..cb4e691 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 8301917..02c6a9f 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
diff --git a/src/low-can-binding.cpp b/src/low-can-binding.cpp
index 52e893d..7baf147 100644
--- a/src/low-can-binding.cpp
+++ b/src/low-can-binding.cpp
@@ -72,6 +72,44 @@ static int create_event_handle(const std::string& sig_name, std::map<std::string
 	return 1;
 }
 
+static int subscribe_unsubscribe_signal(struct afb_req request, bool subscribe, const std::string& sig, DiagnosticRequest* diag_req, int frequency)
+{
+	int ret;
+	sd_event_source *source;
+
+	std::lock_guard<std::mutex> subscribed_signals_lock(get_subscribed_signals_mutex());
+	std::map<std::string, struct afb_event>& s = get_subscribed_signals();
+	if (s.find(sig) != s.end() && !afb_event_is_valid(s[sig]))
+	{
+		if(!subscribe)
+		{
+			NOTICE(binder_interface, "Event isn't valid, it can't be unsubscribed.");
+			ret = -1;
+		}
+		else
+		{
+			/* Event it isn't valid anymore, recreate it */
+			sd_event_add_time(afb_daemon_get_event_loop(binder_interface->daemon), &source, CLOCK_MONOTONIC, frequency, 0,
+						configuration_t::instance().get_diagnostic_manager().send_request, diag_req);
+			ret = create_event_handle(sig, s);
+		}
+	}
+	else
+	{
+		/* Event doesn't exist , so let's create it */
+		struct afb_event empty_event = {nullptr, nullptr};
+		subscribed_signals[sig] = empty_event;
+		ret = create_event_handle(sig, s);
+	}
+
+	/* Check whether or not the event handler has been correctly created and
+	 * make the subscription/unsubscription operation is so.
+	 */
+	if (ret <= 0)
+		return ret;
+	return make_subscription_unsubscription(request, sig, s, subscribe);
+}
+
 static int subscribe_unsubscribe_signal(struct afb_req request, bool subscribe, const std::string& sig)
 {
 	int ret;
@@ -125,10 +163,25 @@ static int subscribe_unsubscribe_signals(struct afb_req request, bool subscribe,
 	const std::string& can_prefix = configuration_t::instance().get_can_signals().front().get_prefix();
 	const std::string& obd2_prefix = configuration_t::instance().get_obd2_signals().front().get_prefix();
 
-	for(auto& sig : signals)
+	for(const std::string& sig : signals)
 	{
-		//if (sig.find_first_of())
-		int ret = subscribe_unsubscribe_signal(request, subscribe, sig);
+		int ret;
+		if (sig.find_first_of(can_prefix.c_str(), 0, can_prefix.size()))
+			ret = subscribe_unsubscribe_signal(request, subscribe, sig);
+		else if (sig.find_first_of(obd2_prefix.c_str(), 0, obd2_prefix.size()))
+		{
+			std::vector<obd2_signal_t*> found;
+			configuration_t::instance().find_obd2_signals(build_DynamicField(sig), found);
+			int frequency = found.front()->get_frequency();
+			DiagnosticRequest* diag_req = new DiagnosticRequest(found.front()->build_diagnostic_request());
+			configuration_t::instance().get_diagnostic_manager().add_recurring_request(
+				diag_req, sig.c_str(), false, obd2_signal_t::decode_obd2_response, nullptr, (float)frequency);
+				//TODO: Adding callback requesting ignition status:	diag_req, sig.c_str(), false, obd2_signal_t::decode_obd2_response, obd2_signal_t::check_ignition_status, frequency);
+			ret = subscribe_unsubscribe_signal(request, subscribe, sig, diag_req,frequency);
+		}
+		else
+			ret = -1;
+
 		if(ret <= 0)
 			return ret;
 		rets++;
diff --git a/src/utils/timer.cpp b/src/utils/timer.cpp
index 966178b..1461ac4 100644
--- a/src/utils/timer.cpp
+++ b/src/utils/timer.cpp
@@ -71,3 +71,10 @@ bool frequency_clock_t::elapsed(bool stagger)
 
 	return frequency_ == 0 || elapsed_time >= period;
 }
+
+/// @brief Force the clock to tick, regardless of it its time has actually
+/// elapsed.
+void frequency_clock_t::tick()
+{
+	last_tick_ = get_time_function()();
+}
\ No newline at end of file
diff --git a/src/utils/timer.hpp b/src/utils/timer.hpp
index 8037b85..ce6336e 100644
--- a/src/utils/timer.hpp
+++ b/src/utils/timer.hpp
@@ -56,4 +56,6 @@ public:
 	bool started();
 	time_function_t get_time_function();
 	bool elapsed(bool stagger);
+
+	void tick();
 };
\ No newline at end of file