Implement check of supported diagnostic PID.

Supported boolean member about diagnostic messages is now used. When
a response is received, completed but not successful, then set the diagnostic
message as not supported and clean the request from the queue.

Subscription remains for now, not cool but will be fix soon.

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

5 files changed, 31 insertions, 3 deletions

diff --git a/src/diagnostic/diagnostic-manager.cpp b/src/diagnostic/diagnostic-manager.cpp
index ac0aaadf..2c0a4d92 100644
--- a/src/diagnostic/diagnostic-manager.cpp
+++ b/src/diagnostic/diagnostic-manager.cpp
@@ -315,7 +315,7 @@ int diagnostic_manager_t::send_request(sd_event_source *s, uint64_t usec, void *
 	return -1;
 }
 
-openxc_VehicleMessage diagnostic_manager_t::relay_diagnostic_response(active_diagnostic_request_t* adr, const DiagnosticResponse& response) const
+openxc_VehicleMessage diagnostic_manager_t::relay_diagnostic_response(active_diagnostic_request_t* adr, const DiagnosticResponse& response)
 {
 	openxc_VehicleMessage message = build_VehicleMessage();
 	float value = (float)diagnostic_payload_to_integer(&response);
@@ -339,6 +339,16 @@ openxc_VehicleMessage diagnostic_manager_t::relay_diagnostic_response(active_dia
 		message = build_VehicleMessage(adr, response, value);
 	}
 
+	// If not success but completed then the pid isn't supported
+	if(!response.success)
+	{
+		std::vector<diagnostic_message_t*> found_signals;
+		configuration_t::instance().find_diagnostic_messages( build_DynamicField(adr->get_name()), found_signals );
+		found_signals.front()->set_supported(false);
+		cleanup_request(adr, true);
+		NOTICE(binder_interface, "relay_diagnostic_response: PID not supported or ill formed. Please unsubscribe from it. Error code : %d", response.negative_response_code);
+	}
+
 	if(adr->get_callback() != nullptr)
 	{
 		adr->get_callback()(adr, &response, value);
diff --git a/src/diagnostic/diagnostic-manager.hpp b/src/diagnostic/diagnostic-manager.hpp
index 5a4278dc..60b8decd 100644
--- a/src/diagnostic/diagnostic-manager.hpp
+++ b/src/diagnostic/diagnostic-manager.hpp
@@ -94,7 +94,7 @@ public:
 	static int send_request(sd_event_source *s, uint64_t usec, void *userdata);
 
 	// Decoding part
-	openxc_VehicleMessage relay_diagnostic_response(active_diagnostic_request_t* adr, const DiagnosticResponse& response) const;
+	openxc_VehicleMessage relay_diagnostic_response(active_diagnostic_request_t* adr, const DiagnosticResponse& response);
 	openxc_VehicleMessage relay_diagnostic_handle(active_diagnostic_request_t* entry, const can_message_t& cm);
 	openxc_VehicleMessage find_and_decode_adr(const can_message_t& cm);
 	bool is_diagnostic_response(const can_message_t& cm);
diff --git a/src/diagnostic/diagnostic-message.cpp b/src/diagnostic/diagnostic-message.cpp
index 7473e7b1..62da0dc3 100644
--- a/src/diagnostic/diagnostic-message.cpp
+++ b/src/diagnostic/diagnostic-message.cpp
@@ -67,6 +67,16 @@ DiagnosticResponseCallback diagnostic_message_t::get_callback() const
 	return callback_;
 }
 
+bool diagnostic_message_t::get_supported() const
+{
+	return supported_;
+}
+
+void diagnostic_message_t::set_supported(bool value)
+{
+	supported_ = value;
+}
+
 /**
  * @brief Build a DiagnosticRequest struct to be passed
  *  to diagnostic manager instance.
diff --git a/src/diagnostic/diagnostic-message.hpp b/src/diagnostic/diagnostic-message.hpp
index 4feceb4b..ae37d7d7 100644
--- a/src/diagnostic/diagnostic-message.hpp
+++ b/src/diagnostic/diagnostic-message.hpp
@@ -70,6 +70,9 @@ class diagnostic_message_t {
 		float get_frequency() const;
 		DiagnosticResponseDecoder get_decoder() const;
 		DiagnosticResponseCallback get_callback() const;
+		bool get_supported() const;
+
+		void set_supported(bool value);
 
 		const DiagnosticRequest build_diagnostic_request();
 
diff --git a/src/low-can-binding.cpp b/src/low-can-binding.cpp
index 8ea885d1..08301933 100644
--- a/src/low-can-binding.cpp
+++ b/src/low-can-binding.cpp
@@ -133,7 +133,12 @@ static int subscribe_unsubscribe_signals(struct afb_req request, bool subscribe,
 			std::vector<diagnostic_message_t*> found;
 			configuration_t::instance().find_diagnostic_messages(build_DynamicField(sig), found);
 			DiagnosticRequest* diag_req = new DiagnosticRequest(found.front()->build_diagnostic_request());
-
+			
+			// If the requested diagnostic message isn't supported by the car then unssubcribe.
+			// no matter what we want, worse case will be a fail unsubscription but at least we don't
+			// poll a PID for nothing.
+			if(found.front()->get_supported())
+				subscribe = false;
 			if(subscribe)
 			{
 				float frequency = found.front()->get_frequency();