wa added in frame size assertion, and proc block added for Y16 via mipi

Author: remibettan

src/ds5/ds5-device.cpp

@@ -29,6 +29,7 @@
 #include "proc/temporal-filter.h"
 #include "proc/y8i-to-y8y8.h"
 #include "proc/y12i-to-y16y16.h"
+#include "proc/y12i-to-y16y16-mipi.h"
 #include "proc/color-formats-converter.h"
 #include "proc/syncer-processing-block.h"
 #include "proc/hole-filling-filter.h"
@@ -858,11 +859,23 @@ namespace librealsense
                 []() { return std::make_shared<y8i_to_y8y8>(); }
             ); // L+R
 
-            depth_sensor.register_processing_block(
-                { RS2_FORMAT_Y12I },
-                { {RS2_FORMAT_Y16, RS2_STREAM_INFRARED, 1}, {RS2_FORMAT_Y16, RS2_STREAM_INFRARED, 2} },
-                []() {return std::make_shared<y12i_to_y16y16>(); }
-            );
+            if (!mipi_sensor)
+            {
+                depth_sensor.register_processing_block(
+                    { RS2_FORMAT_Y12I },
+                    { {RS2_FORMAT_Y16, RS2_STREAM_INFRARED, 1}, {RS2_FORMAT_Y16, RS2_STREAM_INFRARED, 2} },
+                    []() {return std::make_shared<y12i_to_y16y16>(); }
+                );
+            }
+            else
+            {
+                depth_sensor.register_processing_block(
+                    { RS2_FORMAT_Y12I },
+                    { {RS2_FORMAT_Y16, RS2_STREAM_INFRARED, 1}, {RS2_FORMAT_Y16, RS2_STREAM_INFRARED, 2} },
+                    []() {return std::make_shared<y12i_to_y16y16_mipi>(); }
+                );
+            }
+            
             pid_hex_str = hexify(_pid);
 
             if ((_pid == RS416_PID || _pid == RS416_RGB_PID) && _fw_version >= firmware_version("5.12.0.1"))

src/proc/CMakeLists.txt

@@ -26,6 +26,7 @@ target_sources(${LRS_TARGET}
         "${CMAKE_CURRENT_LIST_DIR}/disparity-transform.cpp"
         "${CMAKE_CURRENT_LIST_DIR}/y8i-to-y8y8.cpp"
         "${CMAKE_CURRENT_LIST_DIR}/y12i-to-y16y16.cpp"
+        "${CMAKE_CURRENT_LIST_DIR}/y12i-to-y16y16-mipi.cpp"
         "${CMAKE_CURRENT_LIST_DIR}/identity-processing-block.cpp"
         "${CMAKE_CURRENT_LIST_DIR}/threshold.cpp"
         "${CMAKE_CURRENT_LIST_DIR}/rates-printer.cpp"
@@ -55,6 +56,7 @@ target_sources(${LRS_TARGET}
         "${CMAKE_CURRENT_LIST_DIR}/disparity-transform.h"
         "${CMAKE_CURRENT_LIST_DIR}/y8i-to-y8y8.h"
         "${CMAKE_CURRENT_LIST_DIR}/y12i-to-y16y16.h"
+        "${CMAKE_CURRENT_LIST_DIR}/y12i-to-y16y16-mipi.h"
         "${CMAKE_CURRENT_LIST_DIR}/identity-processing-block.h"
         "${CMAKE_CURRENT_LIST_DIR}/threshold.h"
         "${CMAKE_CURRENT_LIST_DIR}/rates-printer.h"

src/proc/y12i-to-y16y16-mipi.cpp

@@ -0,0 +1,39 @@
+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright(c) 2022 Intel Corporation. All Rights Reserved.
+
+#include "y12i-to-y16y16-mipi.h"
+#include "stream.h"
+#ifdef RS2_USE_CUDA
+#include "cuda/cuda-conversion.cuh"
+#endif
+
+namespace librealsense
+{
+//D457 dev - padding of 8 bits added after each bits, should be removed after it is corrected in SerDes
+    struct y12i_pixel_mipi { uint8_t rl : 8, rh : 4, ll : 4, lh : 8, padding : 8; int l() const { return lh << 4 | ll; } int r() const { return rh << 8 | rl; } };
+
+    void unpack_y16_y16_from_y12i_10_mipi(byte * const dest[], const byte * source, int width, int height, int actual_size)
+    {
+        auto count = width * height;
+#ifdef RS2_USE_CUDA
+        rscuda::split_frame_y16_y16_from_y12i_cuda(dest, count, reinterpret_cast<const y12i_pixel *>(source));
+#else
+        split_frame(dest, count, reinterpret_cast<const y12i_pixel_mipi*>(source),
+            [](const y12i_pixel_mipi& p) -> uint16_t { return p.l() << 6 | p.l() >> 4; },  // We want to convert 10-bit data to 16-bit data
+            [](const y12i_pixel_mipi& p) -> uint16_t { return p.r() << 6 | p.r() >> 4; }); // Multiply by 64 1/16 to efficiently approximate 65535/1023
+#endif
+    }
+
+    y12i_to_y16y16_mipi::y12i_to_y16y16_mipi(int left_idx, int right_idx)
+        : y12i_to_y16y16_mipi("Y12I to Y16L Y16R Transform", left_idx, right_idx) {}
+
+    y12i_to_y16y16_mipi::y12i_to_y16y16_mipi(const char * name, int left_idx, int right_idx)
+        : interleaved_functional_processing_block(name, RS2_FORMAT_Y12I, RS2_FORMAT_Y16, RS2_STREAM_INFRARED, RS2_EXTENSION_VIDEO_FRAME, 1,
+                                                                         RS2_FORMAT_Y16, RS2_STREAM_INFRARED, RS2_EXTENSION_VIDEO_FRAME, 2)
+    {}
+
+    void y12i_to_y16y16_mipi::process_function(byte * const dest[], const byte * source, int width, int height, int actual_size, int input_size)
+    {
+        unpack_y16_y16_from_y12i_10_mipi(dest, source, width, height, actual_size);
+    }
+}

src/proc/y12i-to-y16y16-mipi.h

@@ -0,0 +1,21 @@
+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright(c) 2022 Intel Corporation. All Rights Reserved.
+
+#pragma once
+
+#include "synthetic-stream.h"
+#include "option.h"
+#include "image.h"
+
+namespace librealsense
+{
+    class y12i_to_y16y16_mipi : public interleaved_functional_processing_block
+    {
+    public:
+        y12i_to_y16y16_mipi(int left_idx = 1, int right_idx = 2);
+
+    protected:
+        y12i_to_y16y16_mipi(const char* name, int left_idx, int right_idx);
+        void process_function(byte * const dest[], const byte * source, int width, int height, int actual_size, int input_size) override;
+    };
+}

src/proc/y12i-to-y16y16.cpp

@@ -7,16 +7,10 @@
 #include "cuda/cuda-conversion.cuh"
 #endif
 
-#define D457
-
 namespace librealsense
 {
-//D457 dev - padding of 8 bits added after each bits, should be removed after it is corrected in SerDes
-#ifndef D457
     struct y12i_pixel { uint8_t rl : 8, rh : 4, ll : 4, lh : 8; int l() const { return lh << 4 | ll; } int r() const { return rh << 8 | rl; } };
-#else
-    struct y12i_pixel { uint8_t rl : 8, rh : 4, ll : 4, lh : 8, padding : 8; int l() const { return lh << 4 | ll; } int r() const { return rh << 8 | rl; } };
-#endif
+
     void unpack_y16_y16_from_y12i_10(byte * const dest[], const byte * source, int width, int height, int actual_size)
     {
         auto count = width * height;

src/sensor.cpp

@@ -476,8 +476,11 @@ void log_callback_end( uint32_t fps,
                         }
                         else
                         {
-                            assert( expected_size == sizeof(byte) * f.frame_size );
-                            memcpy( (void *)fh->get_frame_data(), f.pixels, expected_size );
+                            if (req_profile_base->get_format() == RS2_FORMAT_Y12I)
+                                expected_size *= (expected_size * 3 / 4 == sizeof(byte) * f.frame_size) ? 3 / 4 : 1;
+                            
+                            assert(expected_size == sizeof(byte) * f.frame_size);
+                            memcpy((void*)fh->get_frame_data(), f.pixels, expected_size);
                         }
 
                         auto&& video = dynamic_cast<video_frame*>(fh.frame);