Get decoding diagnostic request from decoding thread of can_bus_t

Decoding divided in 2 subfunctions dedicated to decode either
pure CAN messages or diagnostic (obd2) message. About now,
a diagnostic request has a name then it will be pushed
on the event_queue as a SimpleMessage. Without name full
details of diagnostic response will be pushed as diagnostic
response. This behavior follows the one from OpenXC.

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

2 files changed, 64 insertions, 0 deletions

diff --git a/src/diagnostic/diagnostic-manager.cpp b/src/diagnostic/diagnostic-manager.cpp
index 6867ec39..c27c2b1f 100644
--- a/src/diagnostic/diagnostic-manager.cpp
+++ b/src/diagnostic/diagnostic-manager.cpp
@@ -20,6 +20,7 @@
 #include "diagnostic-manager.hpp"
 
 #include "uds/uds.h"
+#include "../utils/openxc-utils.hpp"
 #include "../configuration.hpp"
 
 #define MAX_RECURRING_DIAGNOSTIC_FREQUENCY_HZ 10
@@ -259,6 +260,62 @@ bool diagnostic_manager_t::add_recurring_request(DiagnosticRequest* request, con
 	return added;
 }
 
+bool diagnostic_manager_t::is_diagnostic_response(const active_diagnostic_request_t& adr, const can_message_t& cm) const
+{
+	if(cm.get_id() == adr.get_id() + DIAGNOSTIC_RESPONSE_ARBITRATION_ID_OFFSET)
+		return true;
+	DEBUG(binder_interface, "Doesn't find an active diagnostic request that matches.");
+	return false;
+}
+
+active_diagnostic_request_t* diagnostic_manager_t::is_diagnostic_response(const can_message_t& can_message)
+{
+	for (auto& entry : non_recurring_requests_)
+	{
+		if(is_diagnostic_response(*entry, can_message))
+			return entry;
+	}
+
+	for (auto& entry : recurring_requests_)
+	{
+		if(is_diagnostic_response(*entry, can_message))
+			return entry;
+	}
+	return nullptr;
+}
+
+openxc_VehicleMessage diagnostic_manager_t::relay_diagnostic_response(active_diagnostic_request_t* adr, const DiagnosticResponse& response) const
+{
+	openxc_VehicleMessage message;
+	float value = (float)diagnostic_payload_to_integer(&response);
+	if(adr->get_decoder() != nullptr)
+	{
+		value = adr->get_decoder()(&response, value);
+	}
+
+	if((response.success && strnlen(adr->get_name().c_str(), adr->get_name().size())) > 0)
+	{
+		// If name, include 'value' instead of payload, and leave of response
+		// details.
+		message = build_VehicleMessage(build_SimpleMessage(adr->get_name(), build_DynamicField(value)));
+	}
+	else
+	{
+		// If no name, send full details of response but still include 'value'
+		// instead of 'payload' if they provided a decoder. The one case you
+		// can't get is the full detailed response with 'value'. We could add
+		// another parameter for that but it's onerous to carry that around.
+		message = build_VehicleMessage(adr, response, value);
+	}
+
+	if(adr->get_callback() != nullptr)
+	{
+		adr->get_callback()(adr, &response, value);
+	}
+
+	return message;
+}
+
 bool diagnostic_manager_t::conflicting(active_diagnostic_request_t* request, active_diagnostic_request_t* candidate) const
 {
 	return (candidate->get_in_flight() && candidate != request &&
diff --git a/src/diagnostic/diagnostic-manager.hpp b/src/diagnostic/diagnostic-manager.hpp
index 29223fa0..061f5f9c 100644
--- a/src/diagnostic/diagnostic-manager.hpp
+++ b/src/diagnostic/diagnostic-manager.hpp
@@ -22,6 +22,7 @@
 #include <vector>
 
 #include "uds/uds.h"
+#include "openxc.pb.h"
 #include "../can/can-bus-dev.hpp"
 #include "../can/can-message.hpp"
 #include "active-diagnostic-request.hpp"
@@ -32,6 +33,7 @@
  * match the maximum CAN controller count.
  */
 #define MAX_SHIM_COUNT can_bus_t.get_can_devices().size()
+#define DIAGNOSTIC_RESPONSE_ARBITRATION_ID_OFFSET 0x8
 
 class active_diagnostic_request_t;
 
@@ -95,6 +97,11 @@ public:
 		bool waitForMultipleResponses, const DiagnosticResponseDecoder decoder,
 		const DiagnosticResponseCallback callback, float frequencyHz);
 	
+	bool is_diagnostic_response(const active_diagnostic_request_t& adr, const can_message_t& cm) const;
+	active_diagnostic_request_t* is_diagnostic_response(const can_message_t& can_message);
+
+	openxc_VehicleMessage relay_diagnostic_response(active_diagnostic_request_t* adr, const DiagnosticResponse& response) const;
+
 	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);