From 134278ae8694b089d1d3df030cd7355cebc44e87 Mon Sep 17 00:00:00 2001 From: guoping wen Date: Mon, 6 Jan 2025 18:32:19 -0800 Subject: [PATCH 1/8] support d430 GMSL --- src/ds/advanced_mode/advanced_mode.cpp | 1 + src/ds/d400/d400-device.cpp | 6 +- src/ds/d400/d400-factory.cpp | 100 +++++++++++++++++++++++++ src/ds/d400/d400-private.h | 14 +++- src/linux/backend-v4l2.cpp | 85 +++++++++++++++++++-- src/linux/backend-v4l2.h | 2 +- 6 files changed, 194 insertions(+), 14 deletions(-) diff --git a/src/ds/advanced_mode/advanced_mode.cpp b/src/ds/advanced_mode/advanced_mode.cpp index d91d234b55..1d2b12ccb4 100644 --- a/src/ds/advanced_mode/advanced_mode.cpp +++ b/src/ds/advanced_mode/advanced_mode.cpp @@ -112,6 +112,7 @@ namespace librealsense case ds::RS421_PID: case ds::RS430_PID: case ds::RS430I_PID: + case ds::RS430_GMSL_PID: case ds::RS435_RGB_PID: case ds::RS435I_PID: default_430(p); diff --git a/src/ds/d400/d400-device.cpp b/src/ds/d400/d400-device.cpp index 2a5cd76174..5a210d76df 100644 --- a/src/ds/d400/d400-device.cpp +++ b/src/ds/d400/d400-device.cpp @@ -501,7 +501,7 @@ namespace librealsense std::unique_ptr< frame_timestamp_reader > timestamp_reader_backup( new ds_timestamp_reader() ); frame_timestamp_reader* timestamp_reader_from_metadata; - if (all_device_infos.front().pid != RS457_PID) + if (all_device_infos.front().pid != RS457_PID && all_device_infos.front().pid != RS430_GMSL_PID) timestamp_reader_from_metadata = new ds_timestamp_reader_from_metadata(std::move(timestamp_reader_backup)); else timestamp_reader_from_metadata = new ds_timestamp_reader_from_metadata_mipi(std::move(timestamp_reader_backup)); @@ -550,7 +550,7 @@ namespace librealsense auto raw_sensor = get_raw_depth_sensor(); _pid = group.uvc_devices.front().pid; // to be changed for D457 - bool mipi_sensor = (RS457_PID == _pid); + bool mipi_sensor = (RS457_PID == _pid || RS430_GMSL_PID == _pid); _color_calib_table_raw = [this]() { @@ -688,7 +688,7 @@ namespace librealsense //if hw_monitor was created by usb replace it with xu // D400_IMU will remain using USB interface due to HW limitations { - if (_pid == ds::RS457_PID) + if (_pid == ds::RS457_PID || _pid == ds::RS430_GMSL_PID) { depth_sensor.register_option(RS2_OPTION_ASIC_TEMPERATURE, std::make_shared(_hw_monitor, diff --git a/src/ds/d400/d400-factory.cpp b/src/ds/d400/d400-factory.cpp index 44867a2e97..ccf64082ab 100644 --- a/src/ds/d400/d400-factory.cpp +++ b/src/ds/d400/d400-factory.cpp @@ -499,6 +499,7 @@ namespace librealsense , firmware_logger_device( dev_info, d400_device::_hw_monitor, get_firmware_logs_command(), get_flash_logs_command() ) { + std::cout << "rs430i_device ..." << std::endl; } std::vector get_profiles_tags() const override @@ -686,6 +687,69 @@ namespace librealsense }; + + class rs430_gmsl_device : public d400_active, + public d400_motion_uvc, + public ds_advanced_mode_base, + public firmware_logger_device + { + public: + rs430_gmsl_device( std::shared_ptr< const d400_info > const & dev_info, bool register_device_notifications ) + : device( dev_info, register_device_notifications ) + , backend_device( dev_info, register_device_notifications ) + , d400_device( dev_info ) + , d400_active( dev_info ) + , d400_motion_uvc( dev_info ) + , ds_advanced_mode_base( d400_device::_hw_monitor, get_depth_sensor() ) + , firmware_logger_device( + dev_info, d400_device::_hw_monitor, get_firmware_logs_command(), get_flash_logs_command() ) + { + } + + std::shared_ptr create_matcher(const frame_holder& frame) const override; + + std::vector get_profiles_tags() const override + { + std::vector tags; + + tags.push_back({ RS2_STREAM_DEPTH, -1, 640, 480, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + + return tags; + }; + + void hardware_reset() override + { + d400_device::hardware_reset(); + //limitation: the user must hold the context from which the device was created + //creating fake notification to trigger invoke_devices_changed_callbacks, causing disconnection and connection + auto dev_info = this->get_device_info(); + auto non_const_device_info = std::const_pointer_cast< librealsense::device_info >( dev_info ); + std::vector< std::shared_ptr< device_info > > devices{ non_const_device_info }; + auto ctx = std::weak_ptr< context >( get_context() ); + std::thread fake_notification( + [ ctx, devs = std::move( devices ) ]() + { + try + { + if( auto strong = ctx.lock() ) + { + strong->invoke_devices_changed_callbacks( devs, {} ); + // MIPI devices do not re-enumerate so we need to give them some time to restart + std::this_thread::sleep_for( std::chrono::milliseconds( 3000 ) ); + } + if( auto strong = ctx.lock() ) + strong->invoke_devices_changed_callbacks( {}, devs ); + } + catch( const std::exception & e ) + { + LOG_ERROR( e.what() ); + return; + } + } ); + fake_notification.detach(); + } + }; + // AWGCT class rs430_rgb_mm_device : public d400_active, public d400_color, @@ -1128,6 +1192,26 @@ namespace librealsense bool const register_device_notifications = true; auto pid = _group.uvc_devices.front().pid; + auto mi = _group.uvc_devices.front().mi; + std::string unique_id = _group.uvc_devices.front().unique_id; + std::string device_path = _group.uvc_devices.front().device_path; + std::string serial = _group.uvc_devices.front().serial; + + uint32_t uvc_capabilities = _group.uvc_devices.front().uvc_capabilities; + bool has_metadata_node = _group.uvc_devices.front().has_metadata_node; + std::string metadata_node_id = _group.uvc_devices.front().metadata_node_id;; + +/* + std::cout << "d400_info::create_device: " << std::endl; + std::cout << std::hex << pid << std::endl; + std::cout << std::dec << mi << std::endl; + std::cout << unique_id << std::endl; + std::cout << device_path << std::endl; + std::cout << serial << std::endl; + std::cout << std::hex << uvc_capabilities << std::endl; + std::cout << metadata_node_id << std::endl; +*/ + switch(pid) { case RS400_PID: @@ -1171,6 +1255,8 @@ namespace librealsense return std::make_shared< rs455_device >( dev_info, register_device_notifications ); case RS457_PID: return std::make_shared< rs457_device >( dev_info, register_device_notifications ); + case RS430_GMSL_PID: + return std::make_shared< rs430_gmsl_device >( dev_info, register_device_notifications ); default: throw std::runtime_error( rsutils::string::from() << "Unsupported RS400 model! 0x" << std::hex << std::setw( 4 ) << std::setfill( '0' ) << (int)pid ); @@ -1369,6 +1455,20 @@ namespace librealsense return matcher_factory::create(RS2_MATCHER_DEFAULT, streams); } + + std::shared_ptr rs430_gmsl_device::create_matcher(const frame_holder& frame) const + { + std::vector streams = { _depth_stream.get() , _left_ir_stream.get() , _right_ir_stream.get() }; + std::vector mm_streams = { _accel_stream.get(), _gyro_stream.get()}; + streams.insert(streams.end(), mm_streams.begin(), mm_streams.end()); +// if( frame.frame->find_metadata( RS2_FRAME_METADATA_FRAME_COUNTER, nullptr ) ) +// { +// return matcher_factory::create(RS2_MATCHER_DLR_C, streams); +// } + return matcher_factory::create(RS2_MATCHER_DEFAULT, streams); + } + + std::shared_ptr rs400_imu_device::create_matcher(const frame_holder& frame) const { // TODO - A proper matcher for High-FPS sensor is required diff --git a/src/ds/d400/d400-private.h b/src/ds/d400/d400-private.h index c7ea54c5f6..bc4d31403c 100644 --- a/src/ds/d400/d400-private.h +++ b/src/ds/d400/d400-private.h @@ -39,6 +39,7 @@ namespace librealsense const uint16_t RS455_PID = 0x0B5C; // D455 const uint16_t RS457_PID = 0xabcd; // D457 const uint16_t RS457_RECOVERY_PID = 0xbbcd; // D457 DFU Recovery + const uint16_t RS430_GMSL_PID = 0xabce; // D450 GMSL // d400 Devices supported by the current version static const std::set rs400_sku_pid = { @@ -64,7 +65,8 @@ namespace librealsense ds::RS416_PID, ds::RS405_PID, ds::RS455_PID, - ds::RS457_PID + ds::RS457_PID, + ds::RS430_GMSL_PID }; static const std::set d400_multi_sensors_pid = { @@ -77,11 +79,13 @@ namespace librealsense ds::RS435_RGB_PID, ds::RS435I_PID, ds::RS455_PID, - ds::RS457_PID + ds::RS457_PID, + ds::RS430_GMSL_PID }; static const std::set d400_mipi_device_pid = { - ds::RS457_PID + ds::RS457_PID, + ds::RS430_GMSL_PID }; static const std::set d400_hid_sensors_pid = { @@ -133,6 +137,7 @@ namespace librealsense { RS455_PID, "Intel RealSense D455" }, { RS457_PID, "Intel RealSense D457" }, { RS457_RECOVERY_PID, "Intel RealSense D457 Recovery"}, + { RS430_GMSL_PID, "Intel RealSense D430" }, }; static std::map d400_device_to_fw_min_version = { @@ -161,7 +166,8 @@ namespace librealsense {RS405_PID, "5.12.11.8" }, {RS455_PID, "5.13.0.50" }, {RS457_PID, "5.13.1.1" }, - {RS457_RECOVERY_PID, "5.13.1.1" } + {RS457_RECOVERY_PID, "5.13.1.1" }, + {RS430_GMSL_PID, "5.16.1.111" }, }; std::vector filter_d400_device_by_capability( diff --git a/src/linux/backend-v4l2.cpp b/src/linux/backend-v4l2.cpp index 49c2686248..774d241fa2 100644 --- a/src/linux/backend-v4l2.cpp +++ b/src/linux/backend-v4l2.cpp @@ -547,6 +547,34 @@ namespace librealsense throw linux_backend_exception(rsutils::string::from() <<__FUNCTION__ << " xioctl(VIDIOC_QUERYCAP) failed"); } + +/* + uint8_t gvd[276]; + struct v4l2_ext_control ctrl; + + memset(gvd,0,276); + + ctrl.id = RS_CAMERA_CID_GVD; + ctrl.size = sizeof(gvd); + ctrl.p_u8 = gvd; + + struct v4l2_ext_controls ext; + + ext.ctrl_class = V4L2_CTRL_CLASS_CAMERA; + ext.controls = &ctrl; + ext.count = 1; + + if (ioctl(*fd, VIDIOC_G_EXT_CTRLS, &ext) == 0) + { +// memcpy(buffer, gvd + DS5_CMD_OPCODE_SIZE, 0x110); + + for (int i = 0; i < 16; i++) + { + std::cout << std::hex << (int) gvd[i] << std::endl; + } + } +*/ + return cap; } @@ -801,7 +829,7 @@ namespace librealsense } void v4l_uvc_device::get_mipi_device_info(const std::string& dev_name, - std::string& bus_info, std::string& card) + std::string& bus_info, std::string& card, uint16_t& device_id) { struct v4l2_capability vcap; int fd = open(dev_name.c_str(), O_RDWR); @@ -831,6 +859,48 @@ namespace librealsense bus_info = reinterpret_cast(vcap.bus_info); card = reinterpret_cast(vcap.card); } + + + uint8_t gvd[276]; + struct v4l2_ext_control ctrl; + + memset(gvd,0,276); + + ctrl.id = RS_CAMERA_CID_GVD; + ctrl.size = sizeof(gvd); + ctrl.p_u8 = gvd; + + struct v4l2_ext_controls ext; + + ext.ctrl_class = V4L2_CTRL_CLASS_CAMERA; + ext.controls = &ctrl; + ext.count = 1; + + if (ioctl(fd, VIDIOC_G_EXT_CTRLS, &ext) == 0) + { +// memcpy(buffer, gvd + DS5_CMD_OPCODE_SIZE, 0x110); + +// for (int i = 0; i < 16; i++) +// { +// std::cout << std::hex << (int) gvd[i] << std::endl; +// } + + uint8_t product_id = 0; + + product_id = gvd[4 + 4]; +// std::cout << "product_id:" << (int) product_id << std::endl; + + switch(product_id) + { + case(0x0F): + device_id = 0xABCE; + break; + default: + device_id = 0xABCD; + break; + } + } + ::close(fd); } @@ -839,15 +909,18 @@ namespace librealsense uint16_t vid{}, pid{}, mi{}; usb_spec usb_specification(usb_undefined); std::string bus_info, card; + uint16_t device_pid = 0; auto dev_name = "/dev/" + name; - get_mipi_device_info(dev_name, bus_info, card); + get_mipi_device_info(dev_name, bus_info, card, device_pid); // the following 2 lines need to be changed in order to enable multiple mipi devices support // or maybe another field in the info structure - TBD vid = 0x8086; - pid = 0xABCD; // D457 dev + pid = device_pid; + +// std::cout << "video_path:" << video_path << ", name:" << dev_name << ", pid=" << std::hex << (int) pid << std::endl; static std::regex video_dev_index("\\d+$"); std::smatch match; @@ -871,9 +944,9 @@ namespace librealsense // further development is needed to permit use of several mipi devices static int first_video_index = ind; // Use camera_video_nodes as number of /dev/video[%d] for each camera sensor subset - const int camera_video_nodes = 6; + const int camera_video_nodes = 5; int cam_id = ind / camera_video_nodes; - ind = (ind - first_video_index) % camera_video_nodes; // offset from first mipi video node and assume 6 nodes per mipi camera + ind = (ind - first_video_index) % camera_video_nodes; // offset from first mipi video node and assume 5 nodes per mipi camera if (ind == 0 || ind == 2 || ind == 4) mi = 0; // video node indicator else if (ind == 1 | ind == 3) @@ -2835,7 +2908,7 @@ namespace librealsense std::shared_ptr v4l_backend::create_uvc_device(uvc_device_info info) const { - bool mipi_device = 0xABCD == info.pid; // D457 development. Not for upstream + bool mipi_device = (0xABCD == info.pid || 0xABCE == info.pid); // D457 development. Not for upstream auto v4l_uvc_dev = mipi_device ? std::make_shared(info) : ((!info.has_metadata_node) ? std::make_shared(info) : std::make_shared(info)); diff --git a/src/linux/backend-v4l2.h b/src/linux/backend-v4l2.h index bc24df144e..52836b0499 100644 --- a/src/linux/backend-v4l2.h +++ b/src/linux/backend-v4l2.h @@ -330,7 +330,7 @@ namespace librealsense static uvc_device_info get_info_from_mipi_device_path(const std::string& video_path, const std::string& name); static void get_mipi_device_info(const std::string& dev_name, - std::string& bus_info, std::string& card); + std::string& bus_info, std::string& card, uint16_t& product_id); v4l_uvc_device(const uvc_device_info& info, bool use_memory_map = false); From cb09fe1e59c1113bcfc30f49dfccc5ed54ca6485 Mon Sep 17 00:00:00 2001 From: gwen2018 Date: Tue, 7 Jan 2025 11:03:03 -0800 Subject: [PATCH 2/8] restore mipi video nodes to 6 --- src/linux/backend-v4l2.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/linux/backend-v4l2.cpp b/src/linux/backend-v4l2.cpp index 774d241fa2..b2d253697f 100644 --- a/src/linux/backend-v4l2.cpp +++ b/src/linux/backend-v4l2.cpp @@ -944,7 +944,7 @@ namespace librealsense // further development is needed to permit use of several mipi devices static int first_video_index = ind; // Use camera_video_nodes as number of /dev/video[%d] for each camera sensor subset - const int camera_video_nodes = 5; + const int camera_video_nodes = 6; int cam_id = ind / camera_video_nodes; ind = (ind - first_video_index) % camera_video_nodes; // offset from first mipi video node and assume 5 nodes per mipi camera if (ind == 0 || ind == 2 || ind == 4) From f4e65f95d452d1c8d9b16945c5531e477cb15b08 Mon Sep 17 00:00:00 2001 From: gwen2018 Date: Tue, 7 Jan 2025 14:41:12 -0800 Subject: [PATCH 3/8] fix IMU --- src/linux/backend-v4l2.cpp | 112 ++++++++++++++++++++++--------------- src/linux/backend-v4l2.h | 3 +- 2 files changed, 69 insertions(+), 46 deletions(-) diff --git a/src/linux/backend-v4l2.cpp b/src/linux/backend-v4l2.cpp index b2d253697f..f1cf34bc1a 100644 --- a/src/linux/backend-v4l2.cpp +++ b/src/linux/backend-v4l2.cpp @@ -828,38 +828,14 @@ namespace librealsense return info; } - void v4l_uvc_device::get_mipi_device_info(const std::string& dev_name, - std::string& bus_info, std::string& card, uint16_t& device_id) + uint16_t v4l_uvc_device::get_mipi_device_pid(const std::string& dev_name) { + uint16_t device_pid = 0; + struct v4l2_capability vcap; int fd = open(dev_name.c_str(), O_RDWR); if (fd < 0) throw linux_backend_exception("Mipi device capability could not be grabbed"); - int err = ioctl(fd, VIDIOC_QUERYCAP, &vcap); - if (err) - { - struct media_device_info mdi; - - err = ioctl(fd, MEDIA_IOC_DEVICE_INFO, &mdi); - if (!err) - { - if (mdi.bus_info[0]) - bus_info = mdi.bus_info; - else - bus_info = std::string("platform:") + mdi.driver; - - if (mdi.model[0]) - card = mdi.model; - else - card = mdi.driver; - } - } - else - { - bus_info = reinterpret_cast(vcap.bus_info); - card = reinterpret_cast(vcap.card); - } - uint8_t gvd[276]; struct v4l2_ext_control ctrl; @@ -878,30 +854,65 @@ namespace librealsense if (ioctl(fd, VIDIOC_G_EXT_CTRLS, &ext) == 0) { -// memcpy(buffer, gvd + DS5_CMD_OPCODE_SIZE, 0x110); - // for (int i = 0; i < 16; i++) // { // std::cout << std::hex << (int) gvd[i] << std::endl; // } - uint8_t product_id = 0; + uint8_t product_pid = 0; - product_id = gvd[4 + 4]; -// std::cout << "product_id:" << (int) product_id << std::endl; + product_pid = gvd[4 + 4]; +// std::cout << "product_id:" << (int) product_pid << std::endl; - switch(product_id) + switch(product_pid) { case(0x0F): - device_id = 0xABCE; + device_pid = 0xABCE; break; default: - device_id = 0xABCD; + device_pid = 0xABCD; break; } } ::close(fd); + + return device_pid; + } + + void v4l_uvc_device::get_mipi_device_info(const std::string& dev_name, + std::string& bus_info, std::string& card) + { + struct v4l2_capability vcap; + int fd = open(dev_name.c_str(), O_RDWR); + if (fd < 0) + throw linux_backend_exception("Mipi device capability could not be grabbed"); + int err = ioctl(fd, VIDIOC_QUERYCAP, &vcap); + if (err) + { + struct media_device_info mdi; + + err = ioctl(fd, MEDIA_IOC_DEVICE_INFO, &mdi); + if (!err) + { + if (mdi.bus_info[0]) + bus_info = mdi.bus_info; + else + bus_info = std::string("platform:") + mdi.driver; + + if (mdi.model[0]) + card = mdi.model; + else + card = mdi.driver; + } + } + else + { + bus_info = reinterpret_cast(vcap.bus_info); + card = reinterpret_cast(vcap.card); + } + + ::close(fd); } uvc_device_info v4l_uvc_device::get_info_from_mipi_device_path(const std::string& video_path, const std::string& name) @@ -909,18 +920,10 @@ namespace librealsense uint16_t vid{}, pid{}, mi{}; usb_spec usb_specification(usb_undefined); std::string bus_info, card; - uint16_t device_pid = 0; auto dev_name = "/dev/" + name; - get_mipi_device_info(dev_name, bus_info, card, device_pid); - - // the following 2 lines need to be changed in order to enable multiple mipi devices support - // or maybe another field in the info structure - TBD - vid = 0x8086; - pid = device_pid; - -// std::cout << "video_path:" << video_path << ", name:" << dev_name << ", pid=" << std::hex << (int) pid << std::endl; + get_mipi_device_info(dev_name, bus_info, card); static std::regex video_dev_index("\\d+$"); std::smatch match; @@ -935,6 +938,24 @@ namespace librealsense throw linux_backend_exception("Unresolved Video4Linux device, device is skipped"); } + + // find device PID from depth video node + static uint16_t device_pid = 0; + static std::regex video_dev_depth("video-rs-depth-\\d+$"); + if (std::regex_search(name, match, video_dev_depth)) + { + device_pid = get_mipi_device_pid(dev_name); +// std::cout << "depth video node name=" << name << ", device_pid=" << device_pid << std::endl; + } + + // the following 2 lines need to be changed in order to enable multiple mipi devices support + // or maybe another field in the info structure - TBD + vid = 0x8086; + pid = device_pid; + +// std::cout << "video_path:" << video_path << ", name:" << dev_name << ", pid=" << std::hex << (int) pid << std::endl; + + // D457 exposes (assuming first_video_index = 0): // - video0 for Depth and video1 for Depth's md. // - video2 for RGB and video3 for RGB's md. @@ -943,10 +964,11 @@ namespace librealsense // next several lines permit to use D457 even if a usb device has already "taken" the video0,1,2 (for example) // further development is needed to permit use of several mipi devices static int first_video_index = ind; + // Use camera_video_nodes as number of /dev/video[%d] for each camera sensor subset const int camera_video_nodes = 6; int cam_id = ind / camera_video_nodes; - ind = (ind - first_video_index) % camera_video_nodes; // offset from first mipi video node and assume 5 nodes per mipi camera + ind = (ind - first_video_index) % camera_video_nodes; // offset from first mipi video node and assume 6 nodes per mipi camera if (ind == 0 || ind == 2 || ind == 4) mi = 0; // video node indicator else if (ind == 1 | ind == 3) diff --git a/src/linux/backend-v4l2.h b/src/linux/backend-v4l2.h index 52836b0499..777cb2ac0f 100644 --- a/src/linux/backend-v4l2.h +++ b/src/linux/backend-v4l2.h @@ -329,8 +329,9 @@ namespace librealsense static uvc_device_info get_info_from_mipi_device_path(const std::string& video_path, const std::string& name); + static uint16_t get_mipi_device_pid(const std::string& dev_name); static void get_mipi_device_info(const std::string& dev_name, - std::string& bus_info, std::string& card, uint16_t& product_id); + std::string& bus_info, std::string& card); v4l_uvc_device(const uvc_device_info& info, bool use_memory_map = false); From 7e0cea7a7e2e1f9d565a4df04e0196e5bb0df6c1 Mon Sep 17 00:00:00 2001 From: guoping wen Date: Tue, 7 Jan 2025 16:10:28 -0800 Subject: [PATCH 4/8] more general fix to IMU --- src/linux/backend-v4l2.cpp | 57 +++++++++++++++++++++++++++----------- src/linux/backend-v4l2.h | 1 + 2 files changed, 42 insertions(+), 16 deletions(-) diff --git a/src/linux/backend-v4l2.cpp b/src/linux/backend-v4l2.cpp index f1cf34bc1a..026027810c 100644 --- a/src/linux/backend-v4l2.cpp +++ b/src/linux/backend-v4l2.cpp @@ -828,6 +828,32 @@ namespace librealsense return info; } + bool v4l_uvc_device::is_format_supported_on_node(const std::string& dev_name, std::string v4l_4cc_fmt) + { + int fd = open(dev_name.c_str(), O_RDWR); + if (fd < 0) + throw linux_backend_exception("Mipi device capability could not be grabbed"); + + struct v4l2_fmtdesc fmtdesc; + memset(&fmtdesc,0,sizeof(fmtdesc)); + fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + + uint32_t format; + memcpy(&format, v4l_4cc_fmt.c_str(), sizeof(format)); + + while (ioctl(fd,VIDIOC_ENUM_FMT,&fmtdesc) == 0) + { + if (fmtdesc.pixelformat == format) + { + return true; + } + + fmtdesc.index++; + } + + return false; + } + uint16_t v4l_uvc_device::get_mipi_device_pid(const std::string& dev_name) { uint16_t device_pid = 0; @@ -925,24 +951,9 @@ namespace librealsense get_mipi_device_info(dev_name, bus_info, card); - static std::regex video_dev_index("\\d+$"); - std::smatch match; - uint8_t ind{}; - if (std::regex_search(name, match, video_dev_index)) - { - ind = static_cast(std::stoi(match[0])); - } - else - { - LOG_WARNING("Unresolved Video4Linux device pattern: " << name << ", device is skipped"); - throw linux_backend_exception("Unresolved Video4Linux device, device is skipped"); - } - - // find device PID from depth video node static uint16_t device_pid = 0; - static std::regex video_dev_depth("video-rs-depth-\\d+$"); - if (std::regex_search(name, match, video_dev_depth)) + if (is_format_supported_on_node(dev_name, "Z16 ")) { device_pid = get_mipi_device_pid(dev_name); // std::cout << "depth video node name=" << name << ", device_pid=" << device_pid << std::endl; @@ -956,6 +967,20 @@ namespace librealsense // std::cout << "video_path:" << video_path << ", name:" << dev_name << ", pid=" << std::hex << (int) pid << std::endl; + static std::regex video_dev_index("\\d+$"); + std::smatch match; + uint8_t ind{}; + if (std::regex_search(name, match, video_dev_index)) + { + ind = static_cast(std::stoi(match[0])); + } + else + { + LOG_WARNING("Unresolved Video4Linux device pattern: " << name << ", device is skipped"); + throw linux_backend_exception("Unresolved Video4Linux device, device is skipped"); + } + + // D457 exposes (assuming first_video_index = 0): // - video0 for Depth and video1 for Depth's md. // - video2 for RGB and video3 for RGB's md. diff --git a/src/linux/backend-v4l2.h b/src/linux/backend-v4l2.h index 777cb2ac0f..2773f9fa42 100644 --- a/src/linux/backend-v4l2.h +++ b/src/linux/backend-v4l2.h @@ -329,6 +329,7 @@ namespace librealsense static uvc_device_info get_info_from_mipi_device_path(const std::string& video_path, const std::string& name); + static bool is_format_supported_on_node(const std::string& dev_name, std::string v4l_4cc_fmt); static uint16_t get_mipi_device_pid(const std::string& dev_name); static void get_mipi_device_info(const std::string& dev_name, std::string& bus_info, std::string& card); From eb43b2c57265bf8b9964379b38e29a247b723f6c Mon Sep 17 00:00:00 2001 From: gwen2018 Date: Mon, 27 Jan 2025 14:13:59 -0800 Subject: [PATCH 5/8] fix fw update in viewer --- common/fw-update-helper.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/common/fw-update-helper.cpp b/common/fw-update-helper.cpp index 68cf8a1d2e..a83ffb21fb 100644 --- a/common/fw-update-helper.cpp +++ b/common/fw-update-helper.cpp @@ -190,7 +190,8 @@ namespace rs2 // if device is D457, and fw is signed - using mipi specific procedure if (_is_signed && (!strcmp(_dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID), "ABCD") - || !strcmp(_dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID), "BBCD")) + || !strcmp(_dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID), "BBCD") + || !strcmp(_dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID), "ABCE")) ) { process_mipi(); From 91f4472f02c201a48b7727166eadd3194570fc63 Mon Sep 17 00:00:00 2001 From: gwen2018 Date: Tue, 28 Jan 2025 13:31:24 -0800 Subject: [PATCH 6/8] more d430 gmsl mipi specific changes --- include/librealsense2/rs.h | 4 ++-- src/ds/d400/d400-active.cpp | 2 +- src/ds/d400/d400-device.cpp | 6 +++--- src/fw-update/fw-update-device.cpp | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/include/librealsense2/rs.h b/include/librealsense2/rs.h index f93ed10c10..6a1fa3b981 100644 --- a/include/librealsense2/rs.h +++ b/include/librealsense2/rs.h @@ -24,8 +24,8 @@ extern "C" { #include "h/rs_sensor.h" #define RS2_API_MAJOR_VERSION 2 -#define RS2_API_MINOR_VERSION 56 -#define RS2_API_PATCH_VERSION 0 +#define RS2_API_MINOR_VERSION 57 +#define RS2_API_PATCH_VERSION 52 #define RS2_API_BUILD_VERSION 0 #ifndef STRINGIFY diff --git a/src/ds/d400/d400-active.cpp b/src/ds/d400/d400-active.cpp index 7fb88d26a9..2c4499d64f 100644 --- a/src/ds/d400/d400-active.cpp +++ b/src/ds/d400/d400-active.cpp @@ -34,7 +34,7 @@ namespace librealsense auto pid = this->_pid; auto& depth_ep = get_depth_sensor(); - if( pid == ds::RS457_PID ) + if( pid == ds::RS457_PID || pid == ds::RS430_GMSL_PID) { depth_ep.register_option( RS2_OPTION_PROJECTOR_TEMPERATURE, std::make_shared< projector_temperature_option_mipi >( diff --git a/src/ds/d400/d400-device.cpp b/src/ds/d400/d400-device.cpp index 5a210d76df..9200bc27f5 100644 --- a/src/ds/d400/d400-device.cpp +++ b/src/ds/d400/d400-device.cpp @@ -909,7 +909,7 @@ namespace librealsense } } - if (!val_in_range(_pid, { ds::RS457_PID })) + if (!val_in_range(_pid, { ds::RS457_PID, RS430_GMSL_PID })) { depth_sensor.register_option( RS2_OPTION_STEREO_BASELINE, std::make_shared< const_value_option >( @@ -992,7 +992,7 @@ namespace librealsense firmware_version fw_ver = firmware_version( get_info( RS2_CAMERA_INFO_FIRMWARE_VERSION ) ); auto pid = get_pid(); - if( ( pid == ds::RS457_PID || pid == ds::RS455_PID ) && fw_ver >= firmware_version( 5, 14, 0, 0 ) ) + if( ( pid == ds::RS457_PID || pid == ds::RS455_PID || pid == ds::RS430_GMSL_PID ) && fw_ver >= firmware_version( 5, 14, 0, 0 ) ) register_feature( std::make_shared< emitter_frequency_feature >( get_depth_sensor() ) ); if( fw_ver >= firmware_version( 5, 11, 9, 0 ) ) @@ -1003,7 +1003,7 @@ namespace librealsense register_feature( std::make_shared< auto_exposure_roi_feature >( get_depth_sensor(), _hw_monitor ) ); - if( pid != ds::RS457_PID && pid != ds::RS415_PID && fw_ver >= firmware_version( 5, 12, 10, 11 ) ) + if( pid != ds::RS457_PID && pid != ds::RS415_PID && pid != ds::RS430_GMSL_PID && fw_ver >= firmware_version( 5, 12, 10, 11 ) ) { register_feature( std::make_shared< auto_exposure_limit_feature >( get_depth_sensor(), d400_device::_hw_monitor ) ); diff --git a/src/fw-update/fw-update-device.cpp b/src/fw-update/fw-update-device.cpp index 7d3e10e1ad..ef171bc36f 100644 --- a/src/fw-update/fw-update-device.cpp +++ b/src/fw-update/fw-update-device.cpp @@ -341,7 +341,7 @@ namespace librealsense void update_device::update(const void* fw_image, int fw_image_size, rs2_update_progress_callback_sptr update_progress_callback) const { LOG_INFO( "Uploading FW image ..." ); - if(_pid == "ABCD" || _pid == "BBCD") + if(_pid == "ABCD" || _pid == "BBCD" || _pid == "ABCE") { update_mipi(fw_image, fw_image_size, update_progress_callback); } From a53e1e4ceb23baa9011096369d1c2382b995f561 Mon Sep 17 00:00:00 2001 From: gwen2018 Date: Fri, 31 Jan 2025 14:33:21 -0800 Subject: [PATCH 7/8] fix UCAL host assistance mode for d430 mipi skus --- common/on-chip-calib.cpp | 7 +++++-- common/on-chip-calib.h | 1 + include/librealsense2/rs.h | 2 +- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/common/on-chip-calib.cpp b/common/on-chip-calib.cpp index 5693a73f86..ad971ffe7e 100644 --- a/common/on-chip-calib.cpp +++ b/common/on-chip-calib.cpp @@ -31,6 +31,9 @@ namespace rs2 device_name_string = _dev.get_info( RS2_CAMERA_INFO_NAME ); if( val_in_range( device_name_string, { std::string( "Intel RealSense D415" ) } ) ) speed = 4; + + if ( strcmp(_dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID), "ABCD") || strcmp(_dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID), "ABCE")) + is_mipi_sku = true; } if (dev.supports(RS2_CAMERA_INFO_FIRMWARE_VERSION)) { @@ -2088,7 +2091,7 @@ namespace rs2 ImGui::SetTooltip("%s", "Calculate ground truth for the specific target"); ImGui::SetCursorScreenPos({ float(x + 9), float(y + height - ImGui::GetTextLineHeightWithSpacing() - 30) }); - get_manager().host_assistance = (get_manager().device_name_string == std::string("Intel RealSense D457") ); // To be used for MIPI SKU only + get_manager().host_assistance = get_manager().is_mipi_sku; // To be used for MIPI SKU only bool assistance = (get_manager().host_assistance != 0); if (ImGui::Checkbox("Host Assistance", &assistance)) get_manager().host_assistance = (assistance ? 1 : 0); @@ -2184,7 +2187,7 @@ namespace rs2 // ImGui::SetTooltip("%s", "On-Chip Calibration Extended"); ImGui::SetCursorScreenPos({ float(x + 9), float(y + height - ImGui::GetTextLineHeightWithSpacing() - 31) }); - get_manager().host_assistance = (get_manager().device_name_string == std::string("Intel RealSense D457") ); // To be used for MIPI SKU only + get_manager().host_assistance = get_manager().is_mipi_sku; // To be used for MIPI SKU only bool assistance = (get_manager().host_assistance != 0); ImGui::Checkbox("Host Assistance", &assistance); if (ImGui::IsItemHovered()) diff --git a/common/on-chip-calib.h b/common/on-chip-calib.h index 2b9af69386..47fab8c45d 100644 --- a/common/on-chip-calib.h +++ b/common/on-chip-calib.h @@ -102,6 +102,7 @@ namespace rs2 const std::string Z16_FORMAT = "Z16"; const std::string RGB8_FORMAT = "RGB8"; std::string device_name_string; + bool is_mipi_sku = false; void calibrate(); void calibrate_fl(); diff --git a/include/librealsense2/rs.h b/include/librealsense2/rs.h index 6a1fa3b981..e9edd35186 100644 --- a/include/librealsense2/rs.h +++ b/include/librealsense2/rs.h @@ -25,7 +25,7 @@ extern "C" { #define RS2_API_MAJOR_VERSION 2 #define RS2_API_MINOR_VERSION 57 -#define RS2_API_PATCH_VERSION 52 +#define RS2_API_PATCH_VERSION 53 #define RS2_API_BUILD_VERSION 0 #ifndef STRINGIFY From 59eebbe23899238e0e41ccff1fdc59c0bfe92d95 Mon Sep 17 00:00:00 2001 From: gwen2018 Date: Fri, 31 Jan 2025 17:41:40 -0800 Subject: [PATCH 8/8] improve device detection error handling --- include/librealsense2/rs.h | 2 +- src/linux/backend-v4l2.cpp | 88 +++++++++++++++++++++----------------- 2 files changed, 50 insertions(+), 40 deletions(-) diff --git a/include/librealsense2/rs.h b/include/librealsense2/rs.h index e9edd35186..0503d5d621 100644 --- a/include/librealsense2/rs.h +++ b/include/librealsense2/rs.h @@ -25,7 +25,7 @@ extern "C" { #define RS2_API_MAJOR_VERSION 2 #define RS2_API_MINOR_VERSION 57 -#define RS2_API_PATCH_VERSION 53 +#define RS2_API_PATCH_VERSION 54 #define RS2_API_BUILD_VERSION 0 #ifndef STRINGIFY diff --git a/src/linux/backend-v4l2.cpp b/src/linux/backend-v4l2.cpp index 026027810c..1a8b390750 100644 --- a/src/linux/backend-v4l2.cpp +++ b/src/linux/backend-v4l2.cpp @@ -832,7 +832,7 @@ namespace librealsense { int fd = open(dev_name.c_str(), O_RDWR); if (fd < 0) - throw linux_backend_exception("Mipi device capability could not be grabbed"); + throw linux_backend_exception("Mipi device format could not be grabbed"); struct v4l2_fmtdesc fmtdesc; memset(&fmtdesc,0,sizeof(fmtdesc)); @@ -856,54 +856,60 @@ namespace librealsense uint16_t v4l_uvc_device::get_mipi_device_pid(const std::string& dev_name) { - uint16_t device_pid = 0; + // GVD product ID + const uint8_t GVD_PID_OFFSET = 4; + + const uint8_t GVD_PID_D430_GMSL = 0x0F; + const uint8_t GVD_PID_D457 = 0x12; + + // device PID + uint16_t device_pid = 0; struct v4l2_capability vcap; int fd = open(dev_name.c_str(), O_RDWR); if (fd < 0) - throw linux_backend_exception("Mipi device capability could not be grabbed"); + throw linux_backend_exception("Mipi device PID could not be found"); - uint8_t gvd[276]; - struct v4l2_ext_control ctrl; + uint8_t gvd[276]; + struct v4l2_ext_control ctrl; - memset(gvd,0,276); + memset(gvd,0,276); - ctrl.id = RS_CAMERA_CID_GVD; - ctrl.size = sizeof(gvd); - ctrl.p_u8 = gvd; + ctrl.id = RS_CAMERA_CID_GVD; + ctrl.size = sizeof(gvd); + ctrl.p_u8 = gvd; - struct v4l2_ext_controls ext; + struct v4l2_ext_controls ext; - ext.ctrl_class = V4L2_CTRL_CLASS_CAMERA; - ext.controls = &ctrl; - ext.count = 1; + ext.ctrl_class = V4L2_CTRL_CLASS_CAMERA; + ext.controls = &ctrl; + ext.count = 1; - if (ioctl(fd, VIDIOC_G_EXT_CTRLS, &ext) == 0) - { -// for (int i = 0; i < 16; i++) -// { -// std::cout << std::hex << (int) gvd[i] << std::endl; -// } + if (ioctl(fd, VIDIOC_G_EXT_CTRLS, &ext) == 0) + { + uint8_t product_pid = 0; - uint8_t product_pid = 0; + product_pid = gvd[4 + GVD_PID_OFFSET]; - product_pid = gvd[4 + 4]; -// std::cout << "product_id:" << (int) product_pid << std::endl; + switch(product_pid) + { + case(GVD_PID_D457): + device_pid = 0xABCD; + break; - switch(product_pid) - { - case(0x0F): - device_pid = 0xABCE; - break; - default: - device_pid = 0xABCD; - break; - } - } + case(GVD_PID_D430_GMSL): + device_pid = 0xABCE; + break; + + default: + device_pid = 0x0000; + break; + } + } ::close(fd); - return device_pid; + return device_pid; } void v4l_uvc_device::get_mipi_device_info(const std::string& dev_name, @@ -953,10 +959,17 @@ namespace librealsense // find device PID from depth video node static uint16_t device_pid = 0; - if (is_format_supported_on_node(dev_name, "Z16 ")) + try + { + if (is_format_supported_on_node(dev_name, "Z16 ")) + { + device_pid = get_mipi_device_pid(dev_name); + } + } + catch(const std::exception & e) { - device_pid = get_mipi_device_pid(dev_name); -// std::cout << "depth video node name=" << name << ", device_pid=" << device_pid << std::endl; + LOG_WARNING("MIPI device product id detection issue, device will be skipped: " << e.what()); + device_pid = 0; } // the following 2 lines need to be changed in order to enable multiple mipi devices support @@ -964,9 +977,6 @@ namespace librealsense vid = 0x8086; pid = device_pid; -// std::cout << "video_path:" << video_path << ", name:" << dev_name << ", pid=" << std::hex << (int) pid << std::endl; - - static std::regex video_dev_index("\\d+$"); std::smatch match; uint8_t ind{};