From ae0719d0b8480fb377be50ed82f0f31ee7eb7131 Mon Sep 17 00:00:00 2001 From: Nir Azkiel Date: Mon, 25 Mar 2024 15:55:13 +0200 Subject: [PATCH 1/2] retry GVD command on not ready error (cherry picked from commit 8ff27ee0806a2110a771cc40215c05b224a41f03) --- src/ds/d500/d500-device.cpp | 9 ++++++- src/hw-monitor.cpp | 44 +++++++++++++++++++++++++------ src/hw-monitor.h | 52 ++++++++++++------------------------- 3 files changed, 60 insertions(+), 45 deletions(-) diff --git a/src/ds/d500/d500-device.cpp b/src/ds/d500/d500-device.cpp index 3613d0a31b..a25de9149e 100644 --- a/src/ds/d500/d500-device.cpp +++ b/src/ds/d500/d500-device.cpp @@ -435,8 +435,15 @@ namespace librealsense bool advanced_mode = false; bool usb_modality = true; group_multiple_fw_calls(depth_sensor, [&]() { + + const int HW_NOT_READY_ERR_CODE = -3; + const std::set< int32_t > gvd_retry_errors{ HW_NOT_READY_ERR_CODE }; + + _hw_monitor->get_gvd( gvd_buff.size(), + gvd_buff.data(), + ds::fw_cmd::GVD, + &gvd_retry_errors ); - _hw_monitor->get_gvd(gvd_buff.size(), gvd_buff.data(), ds::fw_cmd::GVD); get_gvd_details(gvd_buff, &gvd_parsed_fields); _device_capabilities = ds_caps::CAP_ACTIVE_PROJECTOR | ds_caps::CAP_RGB_SENSOR | ds_caps::CAP_IMU_SENSOR | diff --git a/src/hw-monitor.cpp b/src/hw-monitor.cpp index 8ce92fe40d..4e3ccd3830 100644 --- a/src/hw-monitor.cpp +++ b/src/hw-monitor.cpp @@ -1,10 +1,9 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2015 Intel Corporation. All Rights Reserved. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + #include "hw-monitor.h" #include "types.h" -#include -#include -#include +#include static inline uint32_t pack( uint8_t c0, uint8_t c1, uint8_t c2, uint8_t c3 ) @@ -28,7 +27,7 @@ namespace librealsense void hw_monitor::fill_usb_buffer(int opCodeNumber, int p1, int p2, int p3, int p4, uint8_t const * data, int dataLength, uint8_t* bufferToSend, int& length) { - auto preHeaderData = IVCAM_MONITOR_MAGIC_NUMBER; + auto preHeaderData = HW_MONITOR_MAGIC_NUMBER; uint8_t* writePtr = bufferToSend; auto header_size = 4; @@ -205,7 +204,7 @@ namespace librealsense int length; std::vector result; size_t length_of_command_with_data = dataLength + size_of_command_without_data; - auto init_size = (length_of_command_with_data > IVCAM_MONITOR_MAX_BUFFER_SIZE) ? length_of_command_with_data : IVCAM_MONITOR_MAX_BUFFER_SIZE; + auto init_size = (length_of_command_with_data > HW_MONITOR_MAX_BUFFER_SIZE) ? length_of_command_with_data : HW_MONITOR_MAX_BUFFER_SIZE; result.resize(init_size); fill_usb_buffer(opcode, param1, param2, param3, param4, data, static_cast(dataLength), result.data(), length); result.resize(length); @@ -226,10 +225,39 @@ namespace librealsense } - void hw_monitor::get_gvd(size_t sz, unsigned char* gvd, uint8_t gvd_cmd) const + void hw_monitor::get_gvd( size_t sz, + unsigned char * gvd, + uint8_t gvd_cmd, + const std::set< int32_t > * retry_error_codes ) const { command command(gvd_cmd); - auto data = send(command); + hwmon_response p_response = hwmon_response::hwm_Unknown; + auto data = send( command, &p_response ); + if( p_response != hwm_Success ) + { + // If we get an error code that match to the error code defined as require retry, + // we will retry the command until it succeed or we reach a timeout + bool should_retry = retry_error_codes && ! retry_error_codes->empty() + && retry_error_codes->find( p_response ) != retry_error_codes->end(); + if( should_retry ) + { + static constexpr size_t RETRIES = 50; + for( int i = 0; i < RETRIES; ++i ) + { + data = send( command, &p_response ); + if( p_response == hwm_Success ) + break; + // If we failed after 'RETRIES' retries or it is less `RETRIES` and the error + // code is not in the retry list than , raise an exception + if( i >= ( RETRIES - 1 ) || retry_error_codes->find( p_response ) == retry_error_codes->end() ) + throw io_exception( rsutils::string::from() + << "error in querying GVD, error:" + << hwmon_error2str( p_response ) ); + LOG_WARNING( "GVD not ready - retrying GET_GVD command" ); + std::this_thread::sleep_for( std::chrono::milliseconds( 100 ) ); + } + } + } auto minSize = std::min(sz, data.size()); std::memcpy( gvd, data.data(), minSize ); } diff --git a/src/hw-monitor.h b/src/hw-monitor.h index a83e74b700..0e496c3f6b 100644 --- a/src/hw-monitor.h +++ b/src/hw-monitor.h @@ -1,50 +1,27 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2015 Intel Corporation. All Rights Reserved. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. #pragma once -#include "uvc-sensor.h" -#include #include "platform/command-transfer.h" +#include "uvc-sensor.h" #include #include #include +#include +#include namespace librealsense { - const uint8_t IV_COMMAND_FIRMWARE_UPDATE_MODE = 0x01; - const uint8_t IV_COMMAND_GET_CALIBRATION_DATA = 0x02; - const uint8_t IV_COMMAND_LASER_POWER = 0x03; - const uint8_t IV_COMMAND_DEPTH_ACCURACY = 0x04; - const uint8_t IV_COMMAND_ZUNIT = 0x05; - const uint8_t IV_COMMAND_LOW_CONFIDENCE_LEVEL = 0x06; - const uint8_t IV_COMMAND_INTENSITY_IMAGE_TYPE = 0x07; - const uint8_t IV_COMMAND_MOTION_VS_RANGE_TRADE= 0x08; - const uint8_t IV_COMMAND_POWER_GEAR = 0x09; - const uint8_t IV_COMMAND_FILTER_OPTION = 0x0A; - const uint8_t IV_COMMAND_VERSION = 0x0B; - const uint8_t IV_COMMAND_CONFIDENCE_THRESHHOLD= 0x0C; - - const uint8_t IVCAM_MONITOR_INTERFACE = 0x4; - const uint8_t IVCAM_MONITOR_ENDPOINT_OUT = 0x1; - const uint8_t IVCAM_MONITOR_ENDPOINT_IN = 0x81; - const uint8_t IVCAM_MIN_SUPPORTED_VERSION = 13; - const uint8_t IVCAM_MONITOR_HEADER_SIZE = (sizeof(uint32_t) * 6); - const uint8_t NUM_OF_CALIBRATION_PARAMS = 100; - const uint8_t PARAMETERS2_BUFFER_SIZE = 50; - const uint8_t SIZE_OF_CALIB_HEADER_BYTES = 4; - const uint8_t NUM_OF_CALIBRATION_COEFFS = 64; - - const uint16_t MAX_SIZE_OF_CALIB_PARAM_BYTES = 800; - const uint16_t SIZE_OF_CALIB_PARAM_BYTES = 512; - const uint16_t IVCAM_MONITOR_MAGIC_NUMBER = 0xcdab; - const uint16_t IVCAM_MONITOR_MAX_BUFFER_SIZE = 1024; - const uint16_t IVCAM_MONITOR_MUTEX_TIMEOUT = 3000; - const uint16_t HW_MONITOR_COMMAND_SIZE = 1000; - const uint16_t HW_MONITOR_BUFFER_SIZE = 1024; - const uint16_t HW_MONITOR_DATA_SIZE_OFFSET = 1020; - const uint16_t SIZE_OF_HW_MONITOR_HEADER = 4; + + const uint16_t HW_MONITOR_MAGIC_NUMBER = 0xcdab; + const uint16_t HW_MONITOR_MAX_BUFFER_SIZE = 1024; + const uint16_t HW_MONITOR_MUTEX_TIMEOUT = 3000; + const uint16_t HW_MONITOR_COMMAND_SIZE = 1000; + const uint16_t HW_MONITOR_BUFFER_SIZE = 1024; + const uint16_t HW_MONITOR_DATA_SIZE_OFFSET = 1020; + const uint16_t SIZE_OF_HW_MONITOR_HEADER = 4; class uvc_sensor; @@ -318,7 +295,10 @@ namespace librealsense uint8_t const * data = nullptr, size_t dataLength = 0); - void get_gvd(size_t sz, unsigned char* gvd, uint8_t gvd_cmd) const; + void get_gvd( size_t sz, + unsigned char * gvd, + uint8_t gvd_cmd, + const std::set< int32_t > * retry_error_codes = nullptr ) const; template std::string get_firmware_version_string( const std::vector< uint8_t > & buff, From 17bb2f21c677a6795988cb55a53b65788e96eb23 Mon Sep 17 00:00:00 2001 From: Nir Azkiel Date: Thu, 28 Mar 2024 11:54:20 +0200 Subject: [PATCH 2/2] CR updates (cherry picked from commit cd3fd76fbdbc87c8da3f9678df307f67f585ed01) --- src/ds/d500/d500-device.cpp | 7 +++++++ src/hw-monitor.cpp | 39 +++++++++++++++++-------------------- src/hw-monitor.h | 2 +- 3 files changed, 26 insertions(+), 22 deletions(-) diff --git a/src/ds/d500/d500-device.cpp b/src/ds/d500/d500-device.cpp index a25de9149e..25e0da2d68 100644 --- a/src/ds/d500/d500-device.cpp +++ b/src/ds/d500/d500-device.cpp @@ -436,6 +436,13 @@ namespace librealsense bool usb_modality = true; group_multiple_fw_calls(depth_sensor, [&]() { + // D500 device can get enumerated before the whole HW in the camera is ready. + // Since GVD gather all information from all the HW, it might need some more time to finish all hand shakes. + // on this case it will return HW_NOT_READY error code. + // Note: D500 error codes list is different than D400. + // This will need a redactor on hw_monitor class to except the error code list from outside. + // Currently, we hard code the HW not ready error code + const int HW_NOT_READY_ERR_CODE = -3; const std::set< int32_t > gvd_retry_errors{ HW_NOT_READY_ERR_CODE }; diff --git a/src/hw-monitor.cpp b/src/hw-monitor.cpp index 4e3ccd3830..5af00fe39c 100644 --- a/src/hw-monitor.cpp +++ b/src/hw-monitor.cpp @@ -233,29 +233,26 @@ namespace librealsense command command(gvd_cmd); hwmon_response p_response = hwmon_response::hwm_Unknown; auto data = send( command, &p_response ); - if( p_response != hwm_Success ) + // If we get an error code that match to the error code defined as require retry, + // we will retry the command until it succeed or we reach a timeout + bool should_retry = retry_error_codes && retry_error_codes->find( p_response ) != retry_error_codes->end(); + if( should_retry ) { - // If we get an error code that match to the error code defined as require retry, - // we will retry the command until it succeed or we reach a timeout - bool should_retry = retry_error_codes && ! retry_error_codes->empty() - && retry_error_codes->find( p_response ) != retry_error_codes->end(); - if( should_retry ) + constexpr size_t RETRIES = 50; + for( int i = 0; i < RETRIES; ++i ) { - static constexpr size_t RETRIES = 50; - for( int i = 0; i < RETRIES; ++i ) - { - data = send( command, &p_response ); - if( p_response == hwm_Success ) - break; - // If we failed after 'RETRIES' retries or it is less `RETRIES` and the error - // code is not in the retry list than , raise an exception - if( i >= ( RETRIES - 1 ) || retry_error_codes->find( p_response ) == retry_error_codes->end() ) - throw io_exception( rsutils::string::from() - << "error in querying GVD, error:" - << hwmon_error2str( p_response ) ); - LOG_WARNING( "GVD not ready - retrying GET_GVD command" ); - std::this_thread::sleep_for( std::chrono::milliseconds( 100 ) ); - } + LOG_WARNING( "GVD not ready - retrying GET_GVD command" ); + std::this_thread::sleep_for( std::chrono::milliseconds( 100 ) ); + data = send( command, &p_response ); + if( p_response == hwm_Success ) + break; + // If we failed after 'RETRIES' retries or it is less `RETRIES` and the error + // code is not in the retry list than , raise an exception + if( i >= ( RETRIES - 1 ) || retry_error_codes->find( p_response ) == retry_error_codes->end() ) + throw io_exception( rsutils::string::from() + << "error in querying GVD, error:" + << hwmon_error2str( p_response ) ); + } } auto minSize = std::min(sz, data.size()); diff --git a/src/hw-monitor.h b/src/hw-monitor.h index 0e496c3f6b..f98f323669 100644 --- a/src/hw-monitor.h +++ b/src/hw-monitor.h @@ -1,5 +1,5 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2024 Intel Corporation. All Rights Reserved. +// Copyright(c) 2015-2024 Intel Corporation. All Rights Reserved. #pragma once