libhidpp: Add support for returning error data

Some HID++ 2.0 features return error data as part of an error report. Make this available to applications.
cvuchener · Aug 27, 2021 · 9aa200f · 9aa200f
1 parent fa8fab7
commit 9aa200f
Show file tree

Hide file tree

Showing 6 changed files with 30 additions and 6 deletions.
diff --git a/src/libhidpp/hidpp/DispatcherThread.cpp b/src/libhidpp/hidpp/DispatcherThread.cpp
@@ -202,6 +202,7 @@ void DispatcherThread::processReport (std::vector<uint8_t> &&raw_report)
 
 	uint8_t sub_id, address, feature, error_code;
 	unsigned int function, sw_id;
+	std::vector<uint8_t> error_data;
 
 	if (report.checkErrorMessage10 (&sub_id, &address, &error_code)) {
 		std::unique_lock<std::mutex> lock (_command_mutex);
@@ -218,7 +219,7 @@ void DispatcherThread::processReport (std::vector<uint8_t> &&raw_report)
 		else
 			Log::warning () << "HID++1.0 error message was not matched with any command." << std::endl;
 	}
-	else if (report.checkErrorMessage20 (&feature, &function, &sw_id, &error_code)) {
+	else if (report.checkErrorMessage20 (&feature, &function, &sw_id, &error_code, &error_data)) {
 		std::unique_lock<std::mutex> lock (_command_mutex);
 		auto it = std::find_if (_commands.begin (), _commands.end (),
 			[index, feature, function, sw_id] (const Command &cmd) {
@@ -228,7 +229,7 @@ void DispatcherThread::processReport (std::vector<uint8_t> &&raw_report)
 					sw_id == cmd.request.softwareID ();
 			});
 		if (it != _commands.end ()) {
-			it->response.set_exception (std::make_exception_ptr (HIDPP20::Error (error_code)));
+			it->response.set_exception (std::make_exception_ptr (HIDPP20::Error (error_code, std::move(error_data))));
 			_commands.erase (it);
 		}
 		else

diff --git a/src/libhidpp/hidpp/Report.cpp b/src/libhidpp/hidpp/Report.cpp
@@ -255,7 +255,8 @@ bool Report::checkErrorMessage10 (uint8_t *sub_id,
 bool Report::checkErrorMessage20 (uint8_t *feature_index,
 				  unsigned int *function,
 				  unsigned int *sw_id,
-				  uint8_t *error_code) const
+				  uint8_t *error_code,
+				  std::vector<uint8_t> *error_data) const
 {
 	if (static_cast<Type> (_data[Offset::Type]) != Long ||
 	    _data[Offset::SubID] != HIDPP20::ErrorMessage)
@@ -269,6 +270,15 @@ bool Report::checkErrorMessage20 (uint8_t *feature_index,
 		*sw_id = _data[4] & 0x0F;
 	if (error_code)
 		*error_code = _data[5];
+
+	if (error_data)
+	{
+		size_t offset = _data.size() - 1;
+		while(offset >= 6 && _data[offset] == 0x00)		// Look for the last non-zero byte
+			--offset;
+		*error_data = { _data.data() + 6, _data.data() + offset + 1 };	// Copy the error data
+	}
+
 	return true;
 }
 
diff --git a/src/libhidpp/hidpp/Report.h b/src/libhidpp/hidpp/Report.h
@@ -257,12 +257,13 @@ class Report
 	 * \param function	The function of the report responsible for the error.
 	 * \param sw_id		The software ID of the report responsible for the error.
 	 * \param error_code	The error code of the error.
+	 * \param error_data	Additional error data sent by the device.
 	 *
 	 * \return \c true if the report is a HID++ 2.0 error, \c false otherwise.
 	 *
 	 * \sa HIDPP20::Error
 	 */
-	bool checkErrorMessage20 (uint8_t *feature_index, unsigned int *function, unsigned int *sw_id, uint8_t *error_code) const;
+	bool checkErrorMessage20 (uint8_t *feature_index, unsigned int *function, unsigned int *sw_id, uint8_t *error_code, std::vector<uint8_t> *error_data = nullptr) const;
 
 	/**\}*/
 

diff --git a/src/libhidpp/hidpp/SimpleDispatcher.cpp b/src/libhidpp/hidpp/SimpleDispatcher.cpp
@@ -138,6 +138,7 @@ Report SimpleDispatcher::CommandResponse::get (int timeout)
 		}
 		unsigned int function, swid;
 		uint8_t sub_id, address, feature, error_code;
+		std::vector<uint8_t> error_data;
 		if (response.checkErrorMessage10 (&sub_id, &address, &error_code)) {
 			if (sub_id == report.subID () && address == report.address ())
 				throw HIDPP10::Error (error_code);
@@ -146,9 +147,9 @@ Report SimpleDispatcher::CommandResponse::get (int timeout)
 				continue;
 			}
 		}
-		if (response.checkErrorMessage20 (&feature, &function, &swid, &error_code)) {
+		if (response.checkErrorMessage20 (&feature, &function, &swid, &error_code, &error_data)) {
 			if (feature == report.featureIndex () && function == report.function () && swid == report.softwareID ())
-				throw HIDPP20::Error (error_code);
+				throw HIDPP20::Error (error_code, std::move(error_data));
 			else {
 				debug << "Ignored HID++2.0 error response." << std::endl;
 				continue;

diff --git a/src/libhidpp/hidpp20/Error.cpp b/src/libhidpp/hidpp20/Error.cpp
@@ -25,6 +25,13 @@ Error::Error (uint8_t error_code):
 {
 }
 
+Error::Error (uint8_t error_code, std::vector<uint8_t> error_data):
+	_error_code (error_code),
+	_error_data (std::move(error_data))
+{
+}
+
+
 const char *Error::what () const noexcept
 {
 	switch (_error_code) {

diff --git a/src/libhidpp/hidpp20/Error.h b/src/libhidpp/hidpp20/Error.h
@@ -20,6 +20,7 @@
 #define LIBHIDPP_HIDPP20_ERROR_H
 
 #include <stdexcept>
+#include <vector>
 
 #include <hidpp20/defs.h>
 
@@ -44,12 +45,15 @@ class Error: public std::exception
 	};
 
 	Error (uint8_t error_code);
+	Error (uint8_t error_code, std::vector<uint8_t> error_data);
 
 	virtual const char *what () const noexcept;
 	uint8_t errorCode () const;
+	const std::vector<uint8_t>& errorData () const { return _error_data; }
 
 private:
 	uint8_t _error_code;
+	std::vector<uint8_t> _error_data;
 };
 
 }