PR #10261 from Samer: add debug_protocol::build_command()

include/librealsense2/h/rs_device.h

@@ -80,6 +80,23 @@ int rs2_supports_device_info(const rs2_device* device, rs2_camera_info info, rs2
  */
 void rs2_hardware_reset(const rs2_device * device, rs2_error ** error);
 
+/**
+* Build debug_protocol raw data command from opcode, parameters and data.
+* The result can be used as raw_data_to_send parameter in send_and_receive_raw_data
+* \param[in]  device                    RealSense device to send data to
+* \param[in]  opcode                    Commad opcode
+* \param[in]  param1                    First input parameter
+* \param[in]  param2                    Second parameter
+* \param[in]  param3                    Third parameter
+* \param[in]  param4                    Fourth parameter
+* \param[in]  data                      Input Data (up to 1024 bytes)
+* \param[in]  size_of_data              Size of input data in bytes
+* \param[out] error                     If non-null, receives any error that occurs during this call, otherwise, errors are ignored
+* \return                               rs2_raw_data_buffer which includes raw command
+*/
+const rs2_raw_data_buffer* rs2_build_debug_protocol_command(rs2_device* device, unsigned opcode, unsigned param1, unsigned param2,
+    unsigned param3, unsigned param4, void* data, unsigned size_of_data, rs2_error** error);
+
 /**
 * Send raw data to device
 * \param[in]  device                    RealSense device to send data to

include/librealsense2/hpp/rs_device.hpp

@@ -830,6 +830,32 @@ namespace rs2
             error::handle(e);
         }
 
+        std::vector<uint8_t> build_command(uint32_t opcode,
+            uint32_t param1 = 0,
+            uint32_t param2 = 0,
+            uint32_t param3 = 0,
+            uint32_t param4 = 0,
+            std::vector<uint8_t> const & data = {}) const
+        {
+            std::vector<uint8_t> results;
+
+            rs2_error* e = nullptr;
+            auto buffer = rs2_build_debug_protocol_command(_dev.get(), opcode, param1, param2, param3, param4,
+                (void*)data.data(), (uint32_t)data.size(), &e);
+            std::shared_ptr<const rs2_raw_data_buffer> list(buffer, rs2_delete_raw_data);
+            error::handle(e);
+
+            auto size = rs2_get_raw_data_size(list.get(), &e);
+            error::handle(e);
+
+            auto start = rs2_get_raw_data(list.get(), &e);
+            error::handle(e);
+
+            results.insert(results.begin(), start, start + size);
+
+            return results;
+        }
+
         std::vector<uint8_t> send_and_receive_raw_data(const std::vector<uint8_t>& input) const
         {
             std::vector<uint8_t> results;
@@ -844,6 +870,7 @@ namespace rs2
             error::handle(e);
 
             auto start = rs2_get_raw_data(list.get(), &e);
+            error::handle(e);
 
             results.insert(results.begin(), start, start + size);
 

src/core/debug.h

@@ -12,6 +12,14 @@ namespace librealsense
     {
     public:
         virtual std::vector<uint8_t> send_receive_raw_data(const std::vector<uint8_t>& input) = 0;
+        virtual std::vector<uint8_t> build_command(uint32_t opcode,
+            uint32_t param1 = 0,
+            uint32_t param2 = 0,
+            uint32_t param3 = 0,
+            uint32_t param4 = 0,
+            uint8_t const * data = nullptr,
+            size_t dataLength = 0) const = 0;
+
     };
 
     MAP_EXTENSION(RS2_EXTENSION_DEBUG, librealsense::debug_interface);

src/ds5/ds5-device.cpp

@@ -122,6 +122,17 @@ namespace librealsense
     {
         return _hw_monitor->send(input);
     }
+
+    std::vector<uint8_t> ds5_device::build_command(uint32_t opcode,
+        uint32_t param1,
+        uint32_t param2,
+        uint32_t param3,
+        uint32_t param4,
+        uint8_t const * data,
+        size_t dataLength) const
+    {
+        return _hw_monitor->build_command(opcode, param1, param2, param3, param4, data, dataLength);
+    }
 
     void ds5_device::hardware_reset()
     {

src/ds5/ds5-device.h

@@ -55,6 +55,14 @@ namespace librealsense
 
         std::vector<uint8_t> send_receive_raw_data(const std::vector<uint8_t>& input) override;
 
+        std::vector<uint8_t> build_command(uint32_t opcode,
+            uint32_t param1 = 0,
+            uint32_t param2 = 0,
+            uint32_t param3 = 0,
+            uint32_t param4 = 0,
+            uint8_t const * data = nullptr,
+            size_t dataLength = 0) const override;
+
         void hardware_reset() override;
 
         void create_snapshot(std::shared_ptr<debug_interface>& snapshot) const override;

src/hw-monitor.cpp

@@ -174,6 +174,22 @@ namespace librealsense
             newCommand.receivedCommandData + newCommand.receivedCommandDataLength);
     }
 
+    std::vector<uint8_t> hw_monitor::build_command(uint32_t opcode,
+        uint32_t param1,
+        uint32_t param2,
+        uint32_t param3,
+        uint32_t param4,
+        uint8_t const * data,
+        size_t dataLength) const
+    {
+        int length;
+        std::vector<uint8_t> result;
+        result.resize(IVCAM_MONITOR_MAX_BUFFER_SIZE);
+        fill_usb_buffer(opcode, param1, param2, param3, param4, data, static_cast<int>(dataLength), result.data(), length);
+        result.resize(length);
+        return result;
+    }
+
     std::string hwmon_error_string( command const & cmd, hwmon_response e )
     {
         auto str = hwmon_error2str( e );

src/hw-monitor.h

@@ -332,6 +332,14 @@ namespace librealsense
 
         std::vector< uint8_t > send( std::vector< uint8_t > const & data ) const;
         std::vector<uint8_t> send( command cmd, hwmon_response * = nullptr, bool locked_transfer = false ) const;
+        std::vector<uint8_t> build_command(uint32_t opcode,
+            uint32_t param1 = 0,
+            uint32_t param2 = 0,
+            uint32_t param3 = 0,
+            uint32_t param4 = 0,
+            uint8_t const * data = nullptr,
+            size_t dataLength = 0) const;
+
         void get_gvd(size_t sz, unsigned char* gvd, uint8_t gvd_cmd) const;
         static std::string get_firmware_version_string(const std::vector<uint8_t>& buff, size_t index, size_t length = 4);
         static std::string get_module_serial_string(const std::vector<uint8_t>& buff, size_t index, size_t length = 6);

src/ivcam/sr300.h

@@ -341,6 +341,18 @@ namespace librealsense
             return _hw_monitor->send(input);
         }
 
+
+        std::vector<uint8_t> build_command(uint32_t opcode,
+            uint32_t param1 = 0,
+            uint32_t param2 = 0,
+            uint32_t param3 = 0,
+            uint32_t param4 = 0,
+            uint8_t const * data = nullptr,
+            size_t dataLength = 0) const override
+        {
+            return _hw_monitor->build_command(opcode, param1, param2, param3, param4, data, dataLength);
+        }
+
         void hardware_reset() override
         {
             force_hardware_reset();

src/l500/l500-device.cpp

@@ -587,6 +587,16 @@ namespace librealsense
         return _hw_monitor->send(input);
     }
 
+    std::vector<uint8_t> l500_device::build_command(uint32_t opcode,
+        uint32_t param1,
+        uint32_t param2,
+        uint32_t param3,
+        uint32_t param4,
+        uint8_t const * data,
+        size_t dataLength) const
+    {
+        return _hw_monitor->build_command(opcode, param1, param2, param3, param4, data, dataLength);
+    }
 
     ivcam2::extended_temperatures l500_device::get_temperatures() const
     {

src/l500/l500-device.h

@@ -48,6 +48,13 @@ namespace librealsense
         }
 
         std::vector< uint8_t > send_receive_raw_data(const std::vector< uint8_t > & input) override;
+        std::vector<uint8_t> build_command(uint32_t opcode,
+            uint32_t param1 = 0,
+            uint32_t param2 = 0,
+            uint32_t param3 = 0,
+            uint32_t param4 = 0,
+            uint8_t const * data = nullptr,
+            size_t dataLength = 0) const override;
 
         void hardware_reset() override
         {

src/realsense.def

@@ -101,6 +101,7 @@ EXPORTS
     rs2_set_region_of_interest
     rs2_get_region_of_interest
 
+    rs2_build_debug_protocol_command
     rs2_send_and_receive_raw_data
     rs2_get_raw_data_size
     rs2_delete_raw_data

src/rs.cpp

@@ -557,6 +557,19 @@ rs2_stream_profile* rs2_clone_video_stream_profile(const rs2_stream_profile* mod
 }
 HANDLE_EXCEPTIONS_AND_RETURN(nullptr, mode, stream, index, format, width, height, intr)
 
+const rs2_raw_data_buffer* rs2_build_debug_protocol_command(rs2_device* device, unsigned opcode, unsigned param1, unsigned param2,
+    unsigned param3, unsigned param4, void* data, unsigned size_of_data, rs2_error** error) BEGIN_API_CALL
+{
+    VALIDATE_NOT_NULL(device);
+
+    auto debug_interface = VALIDATE_INTERFACE(device->device, librealsense::debug_interface);
+    auto ret_data = debug_interface->build_command(opcode, param1, param2, param3, param4, static_cast<uint8_t*>(data), size_of_data);
+
+    return new rs2_raw_data_buffer{ std::move(ret_data) };
+
+}
+HANDLE_EXCEPTIONS_AND_RETURN(nullptr, device)
+
 const rs2_raw_data_buffer* rs2_send_and_receive_raw_data(rs2_device* device, void* raw_data_to_send, unsigned size_of_raw_data_to_send, rs2_error** error) BEGIN_API_CALL
 {
     VALIDATE_NOT_NULL(device);
@@ -566,7 +579,7 @@ const rs2_raw_data_buffer* rs2_send_and_receive_raw_data(rs2_device* device, voi
     auto raw_data_buffer = static_cast<uint8_t*>(raw_data_to_send);
     std::vector<uint8_t> buffer_to_send(raw_data_buffer, raw_data_buffer + size_of_raw_data_to_send);
     auto ret_data = debug_interface->send_receive_raw_data(buffer_to_send);
-    return new rs2_raw_data_buffer{ ret_data };
+    return new rs2_raw_data_buffer{ std::move(ret_data) };
 }
 HANDLE_EXCEPTIONS_AND_RETURN(nullptr, device)
 

unit-tests/debug_protocol/test-build-command.py

@@ -0,0 +1,88 @@
+import pyrealsense2 as rs
+from rspy import devices, log, test, file, repo
+
+
+#############################################################################################
+# Help Functions
+#############################################################################################
+
+def convert_bytes_string_to_decimal_list(command):
+    command_input = []  # array of uint_8t
+
+    # Parsing the command to array of unsigned integers(size should be < 8bits)
+    # threw out spaces
+    command = command.lower()
+    command = command.split()
+
+    for byte in command:
+        command_input.append(int('0x' + byte, 0))
+
+    return command_input
+
+
+def send_hardware_monitor_command(device, command):
+    raw_result = rs.debug_protocol(device).send_and_receive_raw_data(command)
+    status = raw_result[:4]
+    result = raw_result[4:]
+    return status, result
+
+
+#############################################################################################
+# Tests
+#############################################################################################
+
+test.start("Init")
+try:
+    ctx = rs.context()
+    dev = ctx.query_devices()[0]
+except:
+    test.unexpected_exception()
+test.finish()
+
+#############################################################################################
+
+test.start("Old Scenario Test")
+try:
+    # creating a raw data command
+    # [msg_length, magic_number, opcode, params, data]
+    # all values are in hex - little endian
+    msg_length = "14 00"
+    magic_number = "ab cd"
+    gvd_opcode_as_string = "10 00 00 00"  # gvd opcode = 0x10
+    params_and_data = "00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00"  # empty params and data
+    gvd_command = msg_length + " " + magic_number + " " + gvd_opcode_as_string + " " + params_and_data
+    raw_command = convert_bytes_string_to_decimal_list(gvd_command)
+
+    status, old_scenario_result = send_hardware_monitor_command(dev, raw_command)
+
+    # expected status in case of success of "send_hardware_monitor_command" is the same as opcode
+    expected_status = convert_bytes_string_to_decimal_list(gvd_opcode_as_string)
+
+    test.check_equal_lists(status, expected_status)
+except:
+    test.unexpected_exception()
+test.finish()
+
+#############################################################################################
+
+test.start("New Scenario Test")
+try:
+    gvd_opcode_as_int = 0x10
+    gvd_opcode_as_string = "10 00 00 00"  # little endian
+
+    raw_command = rs.debug_protocol(dev).build_command(gvd_opcode_as_int)
+    status, new_scenario_result = send_hardware_monitor_command(dev, raw_command)
+
+    # expected status in case of success of "send_hardware_monitor_command" is the same as opcode
+    expected_status = convert_bytes_string_to_decimal_list(gvd_opcode_as_string)
+
+    test.check_equal_lists(status, expected_status)
+    test.check_equal_lists(new_scenario_result, old_scenario_result)
+except:
+    test.unexpected_exception()
+test.finish()
+
+
+#############################################################################################
+
+test.print_results_and_exit()

unit-tests/live/hw-errors/l500/l500-error-common.h

@@ -35,17 +35,11 @@ std::map< uint8_t, std::pair< std::string, rs2_log_severity > > build_log_errors
 
 void trigger_error_or_exit( const rs2::device & dev, uint8_t num )
 {
-    std::vector< uint8_t > raw_data( 24, 0 );
-    raw_data[0] = 0x14;
-    raw_data[2] = 0xab;
-    raw_data[3] = 0xcd;
-    raw_data[4] = l500_trigger_error_opcode;
-    raw_data[12] = num;
-
     if( auto debug = dev.as< debug_protocol >() )
     {
         try
         {
+            auto raw_data = debug.build_command(l500_trigger_error_opcode, 0, num);
             debug.send_and_receive_raw_data( raw_data );
         }
         catch(std::exception const& e)