This is an automated email from the ASF dual-hosted git repository.

xiaoxiang781216 pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/nuttx.git

commit 83e5388623a323acea10d52cd11db2e7b81cad40
Author: raiden00pl <[email protected]>
AuthorDate: Wed Nov 18 14:15:06 2026 +0100

    drivers/video: Add the ST VL53L5CX multizone ToF driver.
    
    The VL53L5CX is a V4L2 depth capture device with its own image sensor
    and image data interfaces: V4L2_PIX_FMT_Z16 in millimetres, 8x8 at
    1-15 Hz or 4x4 at 1-60 Hz.  The sensor INT output starts each frame
    read on the low priority work queue.  The sensor keeps its firmware in
    RAM: the build downloads it from ST's VL53L5CX ULD (X-CUBE-TOF1), which
    is under the BSD-3-Clause license, so the driver depends on
    ALLOW_BSD_COMPONENTS.
    
    Assisted-by: Claude:claude-opus-5-5
    Signed-off-by: raiden00pl <[email protected]>
---
 Documentation/os/drivers/special/video.rst |   33 +
 LICENSE                                    |   30 +
 drivers/video/CMakeLists.txt               |   27 +
 drivers/video/Kconfig                      |   21 +
 drivers/video/Make.defs                    |   23 +
 drivers/video/vl53l5cx.c                   | 3068 ++++++++++++++++++++++++++++
 drivers/video/vl53l5cx/.gitignore          |    2 +
 drivers/video/vl53l5cx/platform.h          |   44 +
 include/nuttx/video/vl53l5cx.h             |   94 +
 9 files changed, 3342 insertions(+)

diff --git a/Documentation/os/drivers/special/video.rst 
b/Documentation/os/drivers/special/video.rst
index e5fd16057f7..9618645fdff 100644
--- a/Documentation/os/drivers/special/video.rst
+++ b/Documentation/os/drivers/special/video.rst
@@ -82,3 +82,36 @@ sensor framework (``SENSOR_TYPE_PROXIMITY``).  The frame is
 per pixel, 0 when the pixel has no valid target.  The resolutions and
 the frame rates are V4L2 frame sizes and frame intervals; an interval
 entry that sets a size or a format only applies to it.
+
+vl53l5cx
+--------
+
+ST VL53L5CX multizone time-of-flight sensor (``CONFIG_VIDEO_VL53L5CX``) on
+I2C, address 0x29.  The driver provides both the ``imgsensor`` and
+``imgdata`` ops; ``vl53l5cx_register()`` checks the device ID and
+registers its own capture device with ``capture_register()``.  The one
+format is ``V4L2_PIX_FMT_Z16`` in the sensor's zone order:
+
+=====  ==========  ==============
+Size   Frame rate  Default
+=====  ==========  ==============
+8x8    1-15 Hz     yes, at 15 Hz
+4x4    1-60 Hz
+=====  ==========  ==============
+
+A zone is 0 when its target status is not 5 or 9; in the first frame
+after a start all zones have status 6 (no wrap-around check yet).  A
+frame that fails the integrity check is returned zeroed with
+``V4L2_BUF_FLAG_ERROR``; a frame with no queued buffer is dropped and the
+sensor keeps ranging.  Frame intervals are rounded to whole frames per
+second.  The first ``VIDIOC_STREAMON`` boots the sensor and writes its
+firmware (about 1.1 s at 1 MHz I2C); the last close puts it to sleep.
+The sensor INT output starts each frame read on the low priority work
+queue; the board passes the INT handling and the I2C address in
+``struct vl53l5cx_config_s``.
+
+The sensor keeps its firmware in RAM.  The build downloads the firmware
+and default data, ``vl53l5cx_buffers.h`` from ST's VL53L5CX Ultra Lite
+Driver (X-CUBE-TOF1 v3.4.3), to ``drivers/video/vl53l5cx``.  It is under
+the BSD-3-Clause license, so the driver depends on
+``CONFIG_ALLOW_BSD_COMPONENTS``.
diff --git a/LICENSE b/LICENSE
index 1ee3d0b35ed..ccd23d6e6a7 100644
--- a/LICENSE
+++ b/LICENSE
@@ -6495,6 +6495,36 @@ drivers/wireless/spirit/
  NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
+drivers/video/vl53l5cx
+======================
+
+   Copyright (c) 2021 STMicroelectronics
+
+ Redistribution and use in source and binary forms, with or without
+ modification, are permitted provided that the following conditions
+ are met:
+
+   1. Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+   2. Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+   3. Neither the name of the copyright holder nor the names of its
+      contributors may be used to endorse or promote products derived from
+      this software without specific prior written permission.
+
+ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+ TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
 arch/arm/src/nrf52/sdk-nrfxlib
 arch/arm/src/nrf53/sdk-nrfxlib
 arch/arm/src/nrf91/sdk-nrfxlib
diff --git a/drivers/video/CMakeLists.txt b/drivers/video/CMakeLists.txt
index 5fb88b1b8cd..e74c41e379a 100644
--- a/drivers/video/CMakeLists.txt
+++ b/drivers/video/CMakeLists.txt
@@ -54,6 +54,33 @@ if(CONFIG_DRIVERS_VIDEO)
       list(APPEND SRCS gc0308.c)
     endif()
 
+    if(CONFIG_VIDEO_VL53L5CX)
+      list(APPEND SRCS vl53l5cx.c)
+
+      # ST firmware and default data (BSD-3-Clause) from the VL53L5CX ULD in
+      # X-CUBE-TOF1.  Files already present are not downloaded again.
+
+      set(VL53L5CX_ULD_URL
+          
https://raw.githubusercontent.com/STMicroelectronics/x-cube-tof1/v3.4.3/Drivers/BSP/Components/vl53l5cx
+      )
+      set(VL53L5CX_DIR ${CMAKE_CURRENT_LIST_DIR}/vl53l5cx)
+      foreach(file modules/vl53l5cx_buffers.h LICENSE.md)
+        get_filename_component(name ${file} NAME)
+        if(NOT EXISTS ${VL53L5CX_DIR}/${name})
+          message(STATUS "Downloading: VL53L5CX ULD ${name}")
+          file(DOWNLOAD ${VL53L5CX_ULD_URL}/${file} ${VL53L5CX_DIR}/${name}.tmp
+               STATUS VL53L5CX_DL_STATUS)
+          list(GET VL53L5CX_DL_STATUS 0 VL53L5CX_DL_ERROR)
+          if(VL53L5CX_DL_ERROR)
+            file(REMOVE ${VL53L5CX_DIR}/${name}.tmp)
+            message(
+              FATAL_ERROR "Failed to download ${VL53L5CX_ULD_URL}/${file}")
+          endif()
+          file(RENAME ${VL53L5CX_DIR}/${name}.tmp ${VL53L5CX_DIR}/${name})
+        endif()
+      endforeach()
+    endif()
+
   endif()
 
   if(CONFIG_GOLDFISH_FB)
diff --git a/drivers/video/Kconfig b/drivers/video/Kconfig
index bf0389d8cb1..65825996f21 100644
--- a/drivers/video/Kconfig
+++ b/drivers/video/Kconfig
@@ -519,6 +519,27 @@ config VIDEO_GC0308
        ---help---
                GC0308 VGA CMOS image sensor with DVP interface.
 
+config VIDEO_VL53L5CX
+       bool "VL53L5CX multizone ToF sensor"
+       default n
+       depends on VIDEO_STREAM && I2C && SCHED_LPWORK && ALLOW_BSD_COMPONENTS
+       ---help---
+               ST VL53L5CX 8x8 multizone time-of-flight sensor as a V4L2 depth
+               capture device (V4L2_PIX_FMT_Z16).  The sensor firmware is ST
+               code under the BSD-3-Clause license, downloaded at build time.
+
+if VIDEO_VL53L5CX
+
+config VIDEO_VL53L5CX_I2C_FREQUENCY
+       int "I2C frequency"
+       default 1000000
+       range 100000 1000000
+       ---help---
+               I2C clock in Hz, up to 1 MHz (Fast-mode Plus).  Most of the
+               first stream start is the 86 KB firmware write at this rate.
+
+endif # VIDEO_VL53L5CX
+
 config GOLDFISH_CAMERA
        bool "Goldfish camera support"
        depends on VIDEO
diff --git a/drivers/video/Make.defs b/drivers/video/Make.defs
index 07b3f1c3f15..5ba2a516751 100644
--- a/drivers/video/Make.defs
+++ b/drivers/video/Make.defs
@@ -64,6 +64,29 @@ ifeq ($(CONFIG_VIDEO_GC0308),y)
   CSRCS += gc0308.c
 endif
 
+ifeq ($(CONFIG_VIDEO_VL53L5CX),y)
+  CSRCS += vl53l5cx.c
+
+# ST firmware and default data (BSD-3-Clause) from the VL53L5CX ULD in
+# X-CUBE-TOF1.  Files already present are not downloaded again.
+
+VL53L5CX_ULD_URL = 
https://raw.githubusercontent.com/STMicroelectronics/x-cube-tof1/v3.4.3/Drivers/BSP/Components/vl53l5cx
+VL53L5CX_DIR     = $(TOPDIR)$(DELIM)drivers$(DELIM)video$(DELIM)vl53l5cx
+VL53L5CX_FILES   = $(VL53L5CX_DIR)$(DELIM)vl53l5cx_buffers.h
+VL53L5CX_FILES  += $(VL53L5CX_DIR)$(DELIM)LICENSE.md
+
+$(VL53L5CX_DIR)$(DELIM)vl53l5cx_buffers.h:
+       $(call DOWNLOAD,$(VL53L5CX_ULD_URL)/modules,vl53l5cx_buffers.h,$@)
+
+$(VL53L5CX_DIR)$(DELIM)LICENSE.md:
+       $(call DOWNLOAD,$(VL53L5CX_ULD_URL),LICENSE.md,$@)
+
+context:: $(VL53L5CX_FILES)
+
+distclean::
+       $(call DELFILE, $(VL53L5CX_FILES))
+endif
+
 endif
 
 ifeq ($(CONFIG_GOLDFISH_FB),y)
diff --git a/drivers/video/vl53l5cx.c b/drivers/video/vl53l5cx.c
new file mode 100644
index 00000000000..94b81d2a056
--- /dev/null
+++ b/drivers/video/vl53l5cx.c
@@ -0,0 +1,3068 @@
+/****************************************************************************
+ * drivers/video/vl53l5cx.c
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+/* ST VL53L5CX 8x8 multizone time-of-flight sensor as a V4L2 depth camera.
+ *
+ * The driver provides both the image sensor and the image data interface
+ * and registers its own capture device.  Each frame is V4L2_PIX_FMT_Z16,
+ * 8x8 or 4x4 zones in the sensor's zone order, one little-endian distance
+ * in millimetres per zone, 0 when the zone has no valid target.
+ *
+ * The ST firmware, its default configuration, the default crosstalk
+ * record and the NVM read request (vl53l5cx_buffers.h) are written to the
+ * sensor at the first stream start.  The sensor ranges continuously with
+ * one target per zone; its INT output starts the frame read on the low
+ * priority work queue.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/param.h>
+#include <sys/time.h>
+#include <sys/videoio.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+#include <errno.h>
+
+#include <nuttx/clock.h>
+#include <nuttx/debug.h>
+#include <nuttx/kmalloc.h>
+#include <nuttx/mutex.h>
+#include <nuttx/nuttx.h>
+#include <nuttx/sched.h>
+#include <nuttx/signal.h>
+#include <nuttx/spinlock.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/i2c/i2c_master.h>
+#include <nuttx/video/imgdata.h>
+#include <nuttx/video/imgsensor.h>
+#include <nuttx/video/v4l2_cap.h>
+#include <nuttx/video/vl53l5cx.h>
+
+/* ST firmware and default data, downloaded at build time */
+
+#include "vl53l5cx/vl53l5cx_buffers.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define VL53L5CX_FW_SIZE            sizeof(VL53L5CX_FIRMWARE)
+#define VL53L5CX_CFG_SIZE           sizeof(VL53L5CX_DEFAULT_CONFIGURATION)
+#define VL53L5CX_XTALK_SIZE         sizeof(VL53L5CX_DEFAULT_XTALK)
+#define VL53L5CX_NVMREQ_SIZE        sizeof(VL53L5CX_GET_NVM_CMD)
+
+/* Data bytes per I2C transfer.  Blocks are split into chunks addressed at
+ * block start + offset; 128 stays below the 255 bytes that I2C controllers
+ * move without reloading the byte count.
+ */
+
+#define VL53L5CX_CHUNK              128
+
+/* Page register and pages */
+
+#define VL53L5CX_PAGE_REG           0x7fff
+#define VL53L5CX_PAGE_BOOT          0x00
+#define VL53L5CX_PAGE_STATUS        0x01
+#define VL53L5CX_PAGE_MAIN          0x02
+#define VL53L5CX_PAGE_FW            0x09    /* 0x09, 0x0a and 0x0b */
+
+/* Page 0 registers */
+
+#define VL53L5CX_ID_REG             0x0000
+#define VL53L5CX_REV_REG            0x0001
+#define VL53L5CX_MCU_REG            0x0006
+#define VL53L5CX_MCU_ERR_REG        0x0007
+#define VL53L5CX_POWER_REG          0x0009
+#define VL53L5CX_STOP_CMD_REG       0x0014
+#define VL53L5CX_STOP_ARG_REG       0x0015
+
+#define VL53L5CX_ID                 0xf0
+#define VL53L5CX_REV                0x02
+
+#define VL53L5CX_MCU_READY          0x01    /* Booted, awake */
+#define VL53L5CX_MCU_ALL            0xff
+#define VL53L5CX_MCU_STOPPED        0x80    /* Error code or stop status */
+#define VL53L5CX_MCU_STOP_OK1       0x84
+#define VL53L5CX_MCU_STOP_OK2       0x85
+
+#define VL53L5CX_POWER_SLEEP        0x02
+#define VL53L5CX_POWER_WAKE         0x04
+#define VL53L5CX_POWER_RANGING      0x05    /* Firmware controls the sensor */
+
+#define VL53L5CX_STOP_CMD           0x01
+#define VL53L5CX_STOP_ARG           0x16
+
+/* Page 1 registers */
+
+#define VL53L5CX_FW_REG             0x0021
+#define VL53L5CX_FW_READY           0x10
+
+/* Page 2: result buffer and command window */
+
+#define VL53L5CX_RESULT_REG         0x0000
+#define VL53L5CX_UI_STATUS_REG      0x2c00
+#define VL53L5CX_UI_DATA_REG        0x2c04
+#define VL53L5CX_UI_END             0x3000
+#define VL53L5CX_DCI_READ_REG       0x2ff4
+#define VL53L5CX_CMD_REG            0x2ffc
+
+/* UI status word bytes */
+
+#define VL53L5CX_UI_S0_NVM_DONE     0x02
+#define VL53L5CX_UI_S1_DONE         0x03
+#define VL53L5CX_UI_S2_ERROR        0x7f
+
+/* Firmware download pages */
+
+#define VL53L5CX_FW_PAGE_SIZE       32768   /* Last page holds the rest */
+
+/* Offset (NVM) record */
+
+#define VL53L5CX_NVM_SIZE           492
+#define VL53L5CX_OFS_SIZE           488
+#define VL53L5CX_OFS_DROP           8       /* Dropped at upload */
+#define VL53L5CX_OFS_PATCH          0x10
+#define VL53L5CX_OFS_SIGNAL         0x3c
+#define VL53L5CX_OFS_RANGE          0x140
+
+/* Crosstalk record */
+
+#define VL53L5CX_XT_PATCH1          0x08
+#define VL53L5CX_XT_PATCH2          0x20
+#define VL53L5CX_XT_PATCH3          0x134
+#define VL53L5CX_XT_PATCH4          0x78
+#define VL53L5CX_XT_SIGNAL          0x34
+
+/* Polling: 10 ms between reads, 200 or 500 polls */
+
+#define VL53L5CX_POLL_US            10000
+#define VL53L5CX_POLL_TRIES         200
+#define VL53L5CX_STOP_TRIES         500
+#define VL53L5CX_MCU_BOOT_US        1000
+#define VL53L5CX_MCU_BOOT_TRIES     500
+
+/* DCI items (index, length) */
+
+#define VL53L5CX_DCI_MAX            48
+#define VL53L5CX_DCI_HDR            4
+#define VL53L5CX_DCI_FOOTER         8
+#define VL53L5CX_DCI_FRAME          (VL53L5CX_DCI_HDR + VL53L5CX_DCI_FOOTER)
+
+#define VL53L5CX_DCI_OUTPUT_CHECK   0x5440  /* 12 */
+#define VL53L5CX_DCI_ZONE_CFG       0x5450  /* 8 */
+#define VL53L5CX_DCI_FREQ           0x5458  /* 4 */
+#define VL53L5CX_DCI_MODE           0xad30  /* 8 */
+#define VL53L5CX_DCI_DSS            0xad38  /* 16 */
+#define VL53L5CX_DCI_SINGLE_RANGE   0xd964  /* 4 */
+#define VL53L5CX_DCI_OUTPUT_CFG     0xd968  /* 8 */
+#define VL53L5CX_DCI_OUTPUT_MASK    0xd970  /* 16 */
+#define VL53L5CX_DCI_OUTPUT_LIST    0xd980  /* 48 */
+#define VL53L5CX_DCI_PIPE_CTRL      0xdb80  /* 4 */
+#define VL53L5CX_DCI_GLARE          0xe108  /* 40 */
+
+#define VL53L5CX_ZONE_CFG_LEN       8
+#define VL53L5CX_FREQ_LEN           4
+#define VL53L5CX_FREQ_OFF           1       /* N-offset of the rate */
+#define VL53L5CX_MODE_LEN           8
+#define VL53L5CX_DSS_LEN            16
+#define VL53L5CX_GLARE_LEN          40
+#define VL53L5CX_GLARE_OFF1         0x26
+#define VL53L5CX_GLARE_OFF2         0x25
+#define VL53L5CX_OUTPUT_CHECK_LEN   12
+#define VL53L5CX_OUTPUT_CHECK_SIZE  8
+
+/* Output list: 12 block headers, index << 16 | size << 4 | type */
+
+#define VL53L5CX_NOUTPUTS           12
+#define VL53L5CX_HDR_INDEX(h)       ((uint16_t)((h) >> 16))
+#define VL53L5CX_HDR_SIZE(h)        (((h) >> 4) & 0xfff)
+#define VL53L5CX_HDR_TYPE(h)        ((h) & 0xf)
+#define VL53L5CX_HDR_SIZE_MASK      0x0000fff0
+#define VL53L5CX_HDR_SIZE_SHIFT     4
+#define VL53L5CX_TYPE_ELEM_MAX      12      /* Types 1-12: bytes/element */
+
+#define VL53L5CX_OUT_TARGETS        5
+#define VL53L5CX_OUT_DISTANCE       8
+#define VL53L5CX_OUT_STATUS         10
+#define VL53L5CX_OUT_MANDATORY      0x00000007
+#define VL53L5CX_OUT_MASK3          0xc0000000
+#define VL53L5CX_OUT_COUNT          13      /* List length + 1 */
+
+#define VL53L5CX_BLK_TARGETS        0xdb84
+#define VL53L5CX_BLK_DISTANCE       0xdf44
+#define VL53L5CX_BLK_STATUS         0xe084
+
+/* Result frame */
+
+#define VL53L5CX_FRAME_OVERHEAD     24
+#define VL53L5CX_FRAME_ID           8       /* Header ID, N-offset */
+#define VL53L5CX_FRAME_FOOTER       4       /* Footer ID, from the end */
+#define VL53L5CX_FRAME_BLOCKS       16      /* First block header */
+#define VL53L5CX_STREAM_NONE        0xff
+
+/* Data-ready word at 0x0000 */
+
+#define VL53L5CX_READY_B1           0x05
+#define VL53L5CX_READY_B2           0x05
+#define VL53L5CX_READY_B3           0x10
+#define VL53L5CX_READY_ERROR        0x80
+
+/* Target status codes */
+
+#define VL53L5CX_STATUS_VALID       5
+#define VL53L5CX_STATUS_VALID_LARGE 9
+
+#define VL53L5CX_DISTANCE_SHIFT     2       /* 1/4 mm */
+
+/* Zones and rates */
+
+#define VL53L5CX_WIDTH_8X8          8
+#define VL53L5CX_WIDTH_4X4          4
+#define VL53L5CX_ZONES_MAX          64
+#define VL53L5CX_FPS_8X8            15
+#define VL53L5CX_FPS_4X4            60
+#define VL53L5CX_FPS_MIN            1
+#define VL53L5CX_BYTES_PER_PIXEL    2
+
+/* Scratch buffer for the records and the result frames */
+
+#define VL53L5CX_SCRATCH            VL53L5CX_XTALK_SIZE
+
+/* Register sequence operations */
+
+#define VL53L5CX_OP_WRITE           0
+#define VL53L5CX_OP_READ            1       /* Dummy read */
+#define VL53L5CX_OP_DELAY           2       /* val in ms */
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+struct vl53l5cx_op_s
+{
+  uint16_t reg;
+  uint8_t  val;
+  uint8_t  op;
+};
+
+struct vl53l5cx_dev_s
+{
+  struct imgsensor_s sensor;          /* Image sensor interface */
+  struct imgdata_s data;              /* Image data interface */
+  FAR struct i2c_master_s *i2c;       /* I2C bus */
+  FAR const struct vl53l5cx_config_s *config;
+  mutex_t lock;                       /* Serializes the sensor access */
+  spinlock_t buflock;                 /* Buffer and callback hand-off */
+  struct work_s work;                 /* Frame read */
+
+  /* Device state, under lock */
+
+  bool booted;                        /* Firmware and configuration up */
+  bool asleep;                        /* Low power mode */
+  bool ranging;                       /* Ranging started */
+  uint8_t zones;                      /* Zones configured in the sensor */
+  uint8_t fps;                        /* Ranging frequency */
+  uint16_t framesize;                 /* Result frame size */
+
+  uint8_t lastcount;                  /* Last stream counter, worker */
+
+  /* Under buflock */
+
+  FAR uint8_t *buf;                   /* Next frame buffer */
+  imgdata_capture_t callback;
+  FAR void *arg;
+
+  /* Records and buffers */
+
+  uint8_t ofs[VL53L5CX_OFS_SIZE];     /* Device offset record */
+  uint8_t scratch[VL53L5CX_SCRATCH];
+  uint8_t dci[VL53L5CX_DCI_MAX + VL53L5CX_DCI_FRAME];
+  uint8_t txbuf[2 + VL53L5CX_CHUNK];
+};
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/* Image sensor operations */
+
+static bool vl53l5cx_is_available(FAR struct imgsensor_s *sensor);
+static int vl53l5cx_init(FAR struct imgsensor_s *sensor);
+static int vl53l5cx_uninit(FAR struct imgsensor_s *sensor);
+static FAR const char *
+vl53l5cx_get_driver_name(FAR struct imgsensor_s *sensor);
+static int vl53l5cx_validate_frame_setting(FAR struct imgsensor_s *sensor,
+                                           imgsensor_stream_type_t type,
+                                           uint8_t nr_datafmts,
+                                           FAR imgsensor_format_t *datafmts,
+                                           FAR imgsensor_interval_t *ival);
+static int vl53l5cx_start_capture(FAR struct imgsensor_s *sensor,
+                                  imgsensor_stream_type_t type,
+                                  uint8_t nr_datafmts,
+                                  FAR imgsensor_format_t *datafmts,
+                                  FAR imgsensor_interval_t *interval);
+static int vl53l5cx_stop_capture(FAR struct imgsensor_s *sensor,
+                                 imgsensor_stream_type_t type);
+static int vl53l5cx_get_frame_interval(FAR struct imgsensor_s *sensor,
+                                       imgsensor_stream_type_t type,
+                                       FAR imgsensor_interval_t *interval);
+
+/* Image data operations */
+
+static int vl53l5cx_data_init(FAR struct imgdata_s *data);
+static int vl53l5cx_data_uninit(FAR struct imgdata_s *data);
+static int vl53l5cx_data_set_buf(FAR struct imgdata_s *data,
+                                 uint8_t nr_datafmts,
+                                 FAR imgdata_format_t *datafmts,
+                                 FAR uint8_t *addr, uint32_t size);
+static int vl53l5cx_data_validate(FAR struct imgdata_s *data,
+                                  uint8_t nr_datafmts,
+                                  FAR imgdata_format_t *fmts,
+                                  FAR imgdata_interval_t *ival);
+static int vl53l5cx_data_start_capture(FAR struct imgdata_s *data,
+                                       uint8_t nr_datafmts,
+                                       FAR imgdata_format_t *datafmts,
+                                       FAR imgdata_interval_t *interval,
+                                       FAR imgdata_capture_t callback,
+                                       FAR void *arg);
+static int vl53l5cx_data_stop_capture(FAR struct imgdata_s *data);
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+static const struct imgsensor_ops_s g_vl53l5cx_ops =
+{
+  .is_available           = vl53l5cx_is_available,
+  .init                   = vl53l5cx_init,
+  .uninit                 = vl53l5cx_uninit,
+  .get_driver_name        = vl53l5cx_get_driver_name,
+  .validate_frame_setting = vl53l5cx_validate_frame_setting,
+  .start_capture          = vl53l5cx_start_capture,
+  .stop_capture           = vl53l5cx_stop_capture,
+  .get_frame_interval     = vl53l5cx_get_frame_interval,
+};
+
+static const struct imgdata_ops_s g_vl53l5cx_data_ops =
+{
+  .init                   = vl53l5cx_data_init,
+  .uninit                 = vl53l5cx_data_uninit,
+  .set_buf                = vl53l5cx_data_set_buf,
+  .validate_frame_setting = vl53l5cx_data_validate,
+  .start_capture          = vl53l5cx_data_start_capture,
+  .stop_capture           = vl53l5cx_data_stop_capture,
+};
+
+static const struct v4l2_fmtdesc g_vl53l5cx_fmtdescs[] =
+{
+  {
+    .pixelformat = V4L2_PIX_FMT_Z16,
+    .description = "Depth 16-bit",
+  },
+};
+
+/* 8x8 first: it is the default */
+
+static const struct v4l2_frmsizeenum g_vl53l5cx_frmsizes[] =
+{
+  {
+    .pixel_format = V4L2_PIX_FMT_Z16,
+    .type         = V4L2_FRMSIZE_TYPE_DISCRETE,
+    .discrete     =
+      {
+        .width    = VL53L5CX_WIDTH_8X8,
+        .height   = VL53L5CX_WIDTH_8X8,
+      },
+  },
+  {
+    .pixel_format = V4L2_PIX_FMT_Z16,
+    .type         = V4L2_FRMSIZE_TYPE_DISCRETE,
+    .discrete     =
+      {
+        .width    = VL53L5CX_WIDTH_4X4,
+        .height   = VL53L5CX_WIDTH_4X4,
+      },
+  },
+};
+
+/* Any whole number of frames per second in the range */
+
+static const struct v4l2_frmivalenum g_vl53l5cx_frmintervals[] =
+{
+  {
+    .pixel_format = V4L2_PIX_FMT_Z16,
+    .width        = VL53L5CX_WIDTH_8X8,
+    .height       = VL53L5CX_WIDTH_8X8,
+    .type         = V4L2_FRMIVAL_TYPE_CONTINUOUS,
+    .stepwise     =
+      {
+        .min      =
+          {
+            1, VL53L5CX_FPS_8X8
+          },
+        .max      =
+          {
+            1, VL53L5CX_FPS_MIN
+          },
+        .step     =
+          {
+            1, 1
+          },
+      },
+  },
+  {
+    .pixel_format = V4L2_PIX_FMT_Z16,
+    .width        = VL53L5CX_WIDTH_4X4,
+    .height       = VL53L5CX_WIDTH_4X4,
+    .type         = V4L2_FRMIVAL_TYPE_CONTINUOUS,
+    .stepwise     =
+      {
+        .min      =
+          {
+            1, VL53L5CX_FPS_4X4
+          },
+        .max      =
+          {
+            1, VL53L5CX_FPS_MIN
+          },
+        .step     =
+          {
+            1, 1
+          },
+      },
+  },
+};
+
+/* ULD vl53l5cx_init(): software reboot, up to the boot poll */
+
+static const struct vl53l5cx_op_s g_vl53l5cx_reboot[] =
+{
+  { VL53L5CX_PAGE_REG, VL53L5CX_PAGE_BOOT, VL53L5CX_OP_WRITE },
+  { 0x0009, 0x04, VL53L5CX_OP_WRITE },
+  { 0x000f, 0x40, VL53L5CX_OP_WRITE },
+  { 0x000a, 0x03, VL53L5CX_OP_WRITE },
+  { VL53L5CX_PAGE_REG, 0, VL53L5CX_OP_READ },
+  { 0x000c, 0x01, VL53L5CX_OP_WRITE },
+  { 0x0101, 0x00, VL53L5CX_OP_WRITE },
+  { 0x0102, 0x00, VL53L5CX_OP_WRITE },
+  { 0x010a, 0x01, VL53L5CX_OP_WRITE },
+  { 0x4002, 0x01, VL53L5CX_OP_WRITE },
+  { 0x4002, 0x00, VL53L5CX_OP_WRITE },
+  { 0x010a, 0x03, VL53L5CX_OP_WRITE },
+  { 0x0103, 0x01, VL53L5CX_OP_WRITE },
+  { 0x000c, 0x00, VL53L5CX_OP_WRITE },
+  { 0x000f, 0x43, VL53L5CX_OP_WRITE },
+  { 0, 1, VL53L5CX_OP_DELAY },
+  { 0x000f, 0x40, VL53L5CX_OP_WRITE },
+  { 0x000a, 0x01, VL53L5CX_OP_WRITE },
+  { 0, 100, VL53L5CX_OP_DELAY },
+  { VL53L5CX_PAGE_REG, VL53L5CX_PAGE_BOOT, VL53L5CX_OP_WRITE },
+};
+
+/* ULD vl53l5cx_init(): boot done, up to the firmware status poll */
+
+static const struct vl53l5cx_op_s g_vl53l5cx_booted[] =
+{
+  { 0x000e, 0x01, VL53L5CX_OP_WRITE },
+  { VL53L5CX_PAGE_REG, VL53L5CX_PAGE_MAIN, VL53L5CX_OP_WRITE },
+  { 0x0003, 0x0d, VL53L5CX_OP_WRITE },
+  { VL53L5CX_PAGE_REG, VL53L5CX_PAGE_STATUS, VL53L5CX_OP_WRITE },
+};
+
+/* ULD vl53l5cx_init(): power on and MCU wake */
+
+static const struct vl53l5cx_op_s g_vl53l5cx_poweron[] =
+{
+  { VL53L5CX_PAGE_REG, VL53L5CX_PAGE_BOOT, VL53L5CX_OP_WRITE },
+  { VL53L5CX_PAGE_REG, 0, VL53L5CX_OP_READ },
+  { 0x000c, 0x01, VL53L5CX_OP_WRITE },
+  { VL53L5CX_PAGE_REG, VL53L5CX_PAGE_BOOT, VL53L5CX_OP_WRITE },
+  { 0x0101, 0x00, VL53L5CX_OP_WRITE },
+  { 0x0102, 0x00, VL53L5CX_OP_WRITE },
+  { 0x010a, 0x01, VL53L5CX_OP_WRITE },
+  { 0x4002, 0x01, VL53L5CX_OP_WRITE },
+  { 0x4002, 0x00, VL53L5CX_OP_WRITE },
+  { 0x010a, 0x03, VL53L5CX_OP_WRITE },
+  { 0x0103, 0x01, VL53L5CX_OP_WRITE },
+  { 0x400f, 0x00, VL53L5CX_OP_WRITE },
+  { 0x021a, 0x43, VL53L5CX_OP_WRITE },
+  { 0x021a, 0x03, VL53L5CX_OP_WRITE },
+  { 0x021a, 0x01, VL53L5CX_OP_WRITE },
+  { 0x021a, 0x00, VL53L5CX_OP_WRITE },
+  { 0x0219, 0x00, VL53L5CX_OP_WRITE },
+  { 0x021b, 0x00, VL53L5CX_OP_WRITE },
+  { VL53L5CX_PAGE_REG, VL53L5CX_PAGE_BOOT, VL53L5CX_OP_WRITE },
+  { VL53L5CX_PAGE_REG, 0, VL53L5CX_OP_READ },
+  { 0x000c, 0x00, VL53L5CX_OP_WRITE },
+  { VL53L5CX_PAGE_REG, VL53L5CX_PAGE_STATUS, VL53L5CX_OP_WRITE },
+  { 0x0020, 0x07, VL53L5CX_OP_WRITE },
+  { 0x0020, 0x06, VL53L5CX_OP_WRITE },
+};
+
+/* ULD vl53l5cx_init(): download check, up to the status poll */
+
+static const struct vl53l5cx_op_s g_vl53l5cx_fwcheck[] =
+{
+  { VL53L5CX_PAGE_REG, VL53L5CX_PAGE_STATUS, VL53L5CX_OP_WRITE },
+  { VL53L5CX_PAGE_REG, VL53L5CX_PAGE_MAIN, VL53L5CX_OP_WRITE },
+  { 0x0003, 0x0d, VL53L5CX_OP_WRITE },
+  { VL53L5CX_PAGE_REG, VL53L5CX_PAGE_STATUS, VL53L5CX_OP_WRITE },
+};
+
+/* ULD vl53l5cx_init(): MCU reset and boot */
+
+static const struct vl53l5cx_op_s g_vl53l5cx_mcuboot[] =
+{
+  { VL53L5CX_PAGE_REG, VL53L5CX_PAGE_BOOT, VL53L5CX_OP_WRITE },
+  { VL53L5CX_PAGE_REG, 0, VL53L5CX_OP_READ },
+  { 0x000c, 0x01, VL53L5CX_OP_WRITE },
+  { VL53L5CX_PAGE_REG, VL53L5CX_PAGE_BOOT, VL53L5CX_OP_WRITE },
+  { 0x0114, 0x00, VL53L5CX_OP_WRITE },
+  { 0x0115, 0x00, VL53L5CX_OP_WRITE },
+  { 0x0116, 0x42, VL53L5CX_OP_WRITE },
+  { 0x0117, 0x00, VL53L5CX_OP_WRITE },
+  { 0x000b, 0x00, VL53L5CX_OP_WRITE },
+  { VL53L5CX_PAGE_REG, 0, VL53L5CX_OP_READ },
+  { 0x000c, 0x00, VL53L5CX_OP_WRITE },
+  { 0x000b, 0x01, VL53L5CX_OP_WRITE },
+};
+
+/* Fixed bytes of the command frames (wire order) */
+
+static const uint8_t g_vl53l5cx_dci_rd_footer[VL53L5CX_DCI_FOOTER] =
+{
+  0x00, 0x00, 0x00, 0x0f, 0x00, 0x02, 0x00, 0x08
+};
+
+static const uint8_t g_vl53l5cx_dci_wr_footer[VL53L5CX_DCI_FOOTER - 2] =
+{
+  0x00, 0x00, 0x00, 0x0f, 0x05, 0x01
+};
+
+static const uint8_t g_vl53l5cx_ofs_footer[VL53L5CX_OFS_DROP] =
+{
+  0x00, 0x00, 0x00, 0x0f, 0x03, 0x01, 0x01, 0xe4
+};
+
+static const uint8_t g_vl53l5cx_start_cmd[4] =
+{
+  0x00, 0x03, 0x00, 0x00
+};
+
+static const uint8_t g_vl53l5cx_stopped[4] =
+{
+  0xff, 0x04, 0x00, 0x00
+};
+
+/* 4x4 patches of the offset and the crosstalk records */
+
+static const uint8_t g_vl53l5cx_ofs_patch[8] =
+{
+  0x0f, 0x04, 0x04, 0x00, 0x08, 0x10, 0x10, 0x07
+};
+
+static const uint8_t g_vl53l5cx_xt_patch1[8] =
+{
+  0x0f, 0x04, 0x04, 0x17, 0x08, 0x10, 0x10, 0x07
+};
+
+static const uint8_t g_vl53l5cx_xt_patch2[8] =
+{
+  0x00, 0x78, 0x00, 0x08, 0x00, 0x00, 0x00, 0x08
+};
+
+static const uint8_t g_vl53l5cx_xt_patch3[4] =
+{
+  0xa0, 0xfc, 0x01, 0x00
+};
+
+static const uint8_t g_vl53l5cx_xt_patch4[4] =
+{
+  0x00, 0x00, 0x00, 0x00
+};
+
+/* Output list for one target per zone, see vl53l5cx_start_ranging() */
+
+static const uint32_t g_vl53l5cx_outputs[VL53L5CX_NOUTPUTS] =
+{
+  0x0000000d,                         /* Frame start */
+  0x54b400c0,                         /* Metadata */
+  0x54c00040,                         /* Common data */
+  0x54d00104,                         /* Ambient per SPAD */
+  0x55d00404,                         /* SPADs enabled */
+  0xdb840401,                         /* Targets detected */
+  0xdbc40404,                         /* Signal per SPAD */
+  0xdec40402,                         /* Range sigma */
+  0xdf440402,                         /* Distance */
+  0xe0440401,                         /* Reflectance */
+  0xe0840401,                         /* Target status */
+  0xd85808c0,                         /* Motion indicator */
+};
+
+/* Post-config DCI values: one target in the N-view, the glare filter on
+ * and continuous ranging (mode N1 = 1, N3 = 3).
+ */
+
+static const uint8_t g_vl53l5cx_pipe[4] =
+{
+  1, 0, 1, 0
+};
+
+static const uint8_t g_vl53l5cx_glare_off[2] =
+{
+  VL53L5CX_GLARE_OFF1, VL53L5CX_GLARE_OFF2
+};
+
+static const uint8_t g_vl53l5cx_glare_val[2] =
+{
+  1, 1
+};
+
+static const uint8_t g_vl53l5cx_mode_off[2] =
+{
+  1, 3
+};
+
+static const uint8_t g_vl53l5cx_mode_val[2] =
+{
+  1, 3
+};
+
+/* Resolution: DSS and zone configuration offsets and values */
+
+static const uint8_t g_vl53l5cx_dss_off[3] =
+{
+  4, 6, 9
+};
+
+static const uint8_t g_vl53l5cx_dss_4x4[3] =
+{
+  64, 64, 4
+};
+
+static const uint8_t g_vl53l5cx_dss_8x8[3] =
+{
+  16, 16, 1
+};
+
+static const uint8_t g_vl53l5cx_zone_off[4] =
+{
+  0, 1, 4, 5
+};
+
+static const uint8_t g_vl53l5cx_zone_4x4[4] =
+{
+  4, 4, 8, 8
+};
+
+static const uint8_t g_vl53l5cx_zone_8x8[4] =
+{
+  8, 8, 4, 4
+};
+
+static const uint8_t g_vl53l5cx_freq_off = VL53L5CX_FREQ_OFF;
+
+static const uint32_t g_vl53l5cx_enabled = VL53L5CX_OUT_MANDATORY |
+                                           (1 << VL53L5CX_OUT_TARGETS) |
+                                           (1 << VL53L5CX_OUT_DISTANCE) |
+                                           (1 << VL53L5CX_OUT_STATUS);
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: vl53l5cx_getle16
+ *
+ * Description:
+ *   Read a little-endian 16-bit value.
+ *
+ * Input Parameters:
+ *   buf - The two bytes
+ *
+ * Returned Value:
+ *   The value.
+ *
+ ****************************************************************************/
+
+static uint16_t vl53l5cx_getle16(FAR const uint8_t *buf)
+{
+  return (uint16_t)buf[0] | ((uint16_t)buf[1] << 8);
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_getle32
+ *
+ * Description:
+ *   Read a little-endian 32-bit value.
+ *
+ * Input Parameters:
+ *   buf - The four bytes
+ *
+ * Returned Value:
+ *   The value.
+ *
+ ****************************************************************************/
+
+static uint32_t vl53l5cx_getle32(FAR const uint8_t *buf)
+{
+  return (uint32_t)buf[0] | ((uint32_t)buf[1] << 8) |
+         ((uint32_t)buf[2] << 16) | ((uint32_t)buf[3] << 24);
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_putle32
+ *
+ * Description:
+ *   Write a little-endian 32-bit value.
+ *
+ * Input Parameters:
+ *   buf - The four bytes
+ *   val - The value
+ *
+ * Returned Value:
+ *   None.
+ *
+ ****************************************************************************/
+
+static void vl53l5cx_putle32(FAR uint8_t *buf, uint32_t val)
+{
+  buf[0] = val & 0xff;
+  buf[1] = (val >> 8) & 0xff;
+  buf[2] = (val >> 16) & 0xff;
+  buf[3] = (val >> 24) & 0xff;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_swap
+ *
+ * Description:
+ *   Reverse every 4-byte group: wire order <-> native (N-view) order.
+ *
+ * Input Parameters:
+ *   buf - The data
+ *   len - The data length in bytes
+ *
+ * Returned Value:
+ *   None.
+ *
+ ****************************************************************************/
+
+static void vl53l5cx_swap(FAR uint8_t *buf, size_t len)
+{
+  uint8_t tmp;
+  size_t i;
+
+  for (i = 0; i + 4 <= len; i += 4)
+    {
+      tmp        = buf[i];
+      buf[i]     = buf[i + 3];
+      buf[i + 3] = tmp;
+      tmp        = buf[i + 1];
+      buf[i + 1] = buf[i + 2];
+      buf[i + 2] = tmp;
+    }
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_xfer
+ *
+ * Description:
+ *   One I2C write of the register address and len data bytes from txbuf, or
+ *   a read of len bytes at the register address.
+ *
+ * Input Parameters:
+ *   priv  - The driver state
+ *   reg   - The register address
+ *   rxbuf - The read buffer, NULL for a write
+ *   len   - The number of data bytes
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_xfer(FAR struct vl53l5cx_dev_s *priv, uint16_t reg,
+                         FAR uint8_t *rxbuf, size_t len)
+{
+  struct i2c_msg_s msg[2];
+  int ret;
+
+  priv->txbuf[0] = reg >> 8;
+  priv->txbuf[1] = reg & 0xff;
+
+  msg[0].frequency = CONFIG_VIDEO_VL53L5CX_I2C_FREQUENCY;
+  msg[0].addr      = priv->config->addr;
+  msg[0].flags     = 0;
+  msg[0].buffer    = priv->txbuf;
+  msg[0].length    = rxbuf != NULL ? 2 : 2 + len;
+
+  msg[1].frequency = CONFIG_VIDEO_VL53L5CX_I2C_FREQUENCY;
+  msg[1].addr      = priv->config->addr;
+  msg[1].flags     = I2C_M_READ;
+  msg[1].buffer    = rxbuf;
+  msg[1].length    = len;
+
+  ret = I2C_TRANSFER(priv->i2c, msg, rxbuf != NULL ? 2 : 1);
+  if (ret < 0)
+    {
+      verr("ERROR: I2C at 0x%04x failed: %d\n", reg, ret);
+    }
+
+  return ret;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_write
+ *
+ * Description:
+ *   Write a block to consecutive register addresses in I2C sized chunks.
+ *
+ * Input Parameters:
+ *   priv - The driver state
+ *   reg  - The first register address
+ *   data - The data
+ *   len  - The data length in bytes
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_write(FAR struct vl53l5cx_dev_s *priv, uint16_t reg,
+                          FAR const uint8_t *data, size_t len)
+{
+  size_t off;
+  size_t nbytes;
+  int ret;
+
+  for (off = 0; off < len; off += nbytes)
+    {
+      nbytes = MIN(len - off, VL53L5CX_CHUNK);
+      memcpy(&priv->txbuf[2], &data[off], nbytes);
+      ret = vl53l5cx_xfer(priv, reg + off, NULL, nbytes);
+      if (ret < 0)
+        {
+          return ret;
+        }
+    }
+
+  return OK;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_read
+ *
+ * Description:
+ *   Read a block from consecutive register addresses in I2C sized chunks.
+ *
+ * Input Parameters:
+ *   priv - The driver state
+ *   reg  - The first register address
+ *   data - The read buffer
+ *   len  - The number of bytes to read
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_read(FAR struct vl53l5cx_dev_s *priv, uint16_t reg,
+                         FAR uint8_t *data, size_t len)
+{
+  size_t off;
+  size_t nbytes;
+  int ret;
+
+  for (off = 0; off < len; off += nbytes)
+    {
+      nbytes = MIN(len - off, VL53L5CX_CHUNK);
+      ret = vl53l5cx_xfer(priv, reg + off, &data[off], nbytes);
+      if (ret < 0)
+        {
+          return ret;
+        }
+    }
+
+  return OK;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_putreg
+ *
+ * Description:
+ *   Write one register.
+ *
+ * Input Parameters:
+ *   priv - The driver state
+ *   reg  - The register address
+ *   val  - The value
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_putreg(FAR struct vl53l5cx_dev_s *priv, uint16_t reg,
+                           uint8_t val)
+{
+  return vl53l5cx_write(priv, reg, &val, 1);
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_getreg
+ *
+ * Description:
+ *   Read one register.
+ *
+ * Input Parameters:
+ *   priv - The driver state
+ *   reg  - The register address
+ *   val  - The location of the value read
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_getreg(FAR struct vl53l5cx_dev_s *priv, uint16_t reg,
+                           FAR uint8_t *val)
+{
+  return vl53l5cx_read(priv, reg, val, 1);
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_page
+ *
+ * Description:
+ *   Select a register page.
+ *
+ * Input Parameters:
+ *   priv - The driver state
+ *   page - The page number
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_page(FAR struct vl53l5cx_dev_s *priv, uint8_t page)
+{
+  return vl53l5cx_putreg(priv, VL53L5CX_PAGE_REG, page);
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_runseq
+ *
+ * Description:
+ *   Run a sequence of register writes, dummy reads and delays.
+ *
+ * Input Parameters:
+ *   priv - The driver state
+ *   ops  - The operations
+ *   nops - The number of operations
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_runseq(FAR struct vl53l5cx_dev_s *priv,
+                           FAR const struct vl53l5cx_op_s *ops,
+                           size_t nops)
+{
+  uint8_t val;
+  size_t i;
+  int ret = OK;
+
+  for (i = 0; i < nops && ret >= 0; i++)
+    {
+      switch (ops[i].op)
+        {
+          case VL53L5CX_OP_WRITE:
+            ret = vl53l5cx_putreg(priv, ops[i].reg, ops[i].val);
+            break;
+
+          case VL53L5CX_OP_READ:
+            ret = vl53l5cx_getreg(priv, ops[i].reg, &val);
+            break;
+
+          default:
+            nxsig_usleep(ops[i].val * USEC_PER_MSEC);
+            break;
+        }
+    }
+
+  return ret;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_pollreg
+ *
+ * Description:
+ *   Poll one register of the current page until (value & mask) == val.
+ *
+ * Input Parameters:
+ *   priv  - The driver state
+ *   reg   - The register address
+ *   mask  - The bits to compare
+ *   val   - The expected value of the bits
+ *   tries - The number of reads, 10 ms apart
+ *
+ * Returned Value:
+ *   Zero (OK) on success; -ETIMEDOUT or another negated errno value on
+ *   failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_pollreg(FAR struct vl53l5cx_dev_s *priv, uint16_t reg,
+                            uint8_t mask, uint8_t val, int tries)
+{
+  uint8_t regval = 0;
+  int ret;
+
+  while (tries-- > 0)
+    {
+      ret = vl53l5cx_getreg(priv, reg, &regval);
+      if (ret < 0)
+        {
+          return ret;
+        }
+
+      if ((regval & mask) == val)
+        {
+          return OK;
+        }
+
+      nxsig_usleep(VL53L5CX_POLL_US);
+    }
+
+  verr("ERROR: 0x%04x is 0x%02x\n", reg, regval);
+  return -ETIMEDOUT;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_pollui
+ *
+ * Description:
+ *   Poll the UI status word until a firmware command has completed.
+ *
+ * Input Parameters:
+ *   priv - The driver state
+ *   nvm  - True for the NVM read command
+ *
+ * Returned Value:
+ *   Zero (OK) on success; -EIO on a firmware error, -ETIMEDOUT or another
+ *   negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_pollui(FAR struct vl53l5cx_dev_s *priv, bool nvm)
+{
+  uint8_t status[4];
+  int tries;
+  int ret;
+
+  for (tries = 0; tries < VL53L5CX_POLL_TRIES; tries++)
+    {
+      ret = vl53l5cx_read(priv, VL53L5CX_UI_STATUS_REG, status,
+                          sizeof(status));
+      if (ret < 0)
+        {
+          return ret;
+        }
+
+      if (status[2] >= VL53L5CX_UI_S2_ERROR)
+        {
+          verr("ERROR: firmware status 0x%02x\n", status[2]);
+          return -EIO;
+        }
+
+      if (nvm ? status[0] == VL53L5CX_UI_S0_NVM_DONE :
+                status[1] == VL53L5CX_UI_S1_DONE)
+        {
+          return OK;
+        }
+
+      nxsig_usleep(VL53L5CX_POLL_US);
+    }
+
+  verr("ERROR: command timeout\n");
+  return -ETIMEDOUT;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_dci_header
+ *
+ * Description:
+ *   Fill the header of a DCI command frame.
+ *
+ * Input Parameters:
+ *   hdr   - The four header bytes
+ *   index - The DCI item index
+ *   len   - The item length in bytes
+ *
+ * Returned Value:
+ *   None.
+ *
+ ****************************************************************************/
+
+static void vl53l5cx_dci_header(FAR uint8_t *hdr, uint16_t index,
+                                size_t len)
+{
+  hdr[0] = index >> 8;
+  hdr[1] = index & 0xff;
+  hdr[2] = (len >> 4) & 0xff;
+  hdr[3] = (len & 0x0f) << 4;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_dci_write
+ *
+ * Description:
+ *   Write a DCI item, data in the N-view (ULD vl53l5cx_dci_write_data()).
+ *
+ * Input Parameters:
+ *   priv  - The driver state
+ *   index - The DCI item index
+ *   data  - The item data
+ *   len   - The item length in bytes, a multiple of 4
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_dci_write(FAR struct vl53l5cx_dev_s *priv,
+                              uint16_t index, FAR const uint8_t *data,
+                              size_t len)
+{
+  FAR uint8_t *frame = priv->dci;
+  size_t flen = len + VL53L5CX_DCI_FRAME;
+  int ret;
+
+  DEBUGASSERT(len <= VL53L5CX_DCI_MAX && (len & 3) == 0);
+
+  vl53l5cx_dci_header(frame, index, len);
+  memcpy(&frame[VL53L5CX_DCI_HDR], data, len);
+  vl53l5cx_swap(&frame[VL53L5CX_DCI_HDR], len);
+  memcpy(&frame[VL53L5CX_DCI_HDR + len], g_vl53l5cx_dci_wr_footer,
+         sizeof(g_vl53l5cx_dci_wr_footer));
+  frame[flen - 2] = ((len + VL53L5CX_DCI_FOOTER) >> 8) & 0xff;
+  frame[flen - 1] = (len + VL53L5CX_DCI_FOOTER) & 0xff;
+
+  ret = vl53l5cx_write(priv, VL53L5CX_UI_END - flen, frame, flen);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  return vl53l5cx_pollui(priv, false);
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_dci_read
+ *
+ * Description:
+ *   Read a DCI item into data, in the N-view (ULD vl53l5cx_dci_read_data()).
+ *
+ * Input Parameters:
+ *   priv  - The driver state
+ *   index - The DCI item index
+ *   data  - The item buffer
+ *   len   - The item length in bytes, a multiple of 4
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_dci_read(FAR struct vl53l5cx_dev_s *priv,
+                             uint16_t index, FAR uint8_t *data, size_t len)
+{
+  FAR uint8_t *frame = priv->dci;
+  int ret;
+
+  DEBUGASSERT(len <= VL53L5CX_DCI_MAX && (len & 3) == 0);
+
+  vl53l5cx_dci_header(frame, index, len);
+  memcpy(&frame[VL53L5CX_DCI_HDR], g_vl53l5cx_dci_rd_footer,
+         sizeof(g_vl53l5cx_dci_rd_footer));
+
+  ret = vl53l5cx_write(priv, VL53L5CX_DCI_READ_REG, frame,
+                       VL53L5CX_DCI_FRAME);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_pollui(priv, false);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_read(priv, VL53L5CX_UI_DATA_REG, frame,
+                      len + VL53L5CX_DCI_FRAME);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  memcpy(data, &frame[VL53L5CX_DCI_HDR], len);
+  vl53l5cx_swap(data, len);
+  return OK;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_dci_patch
+ *
+ * Description:
+ *   Read-modify-write the bytes of a DCI item at the N-offsets in off[].
+ *
+ * Input Parameters:
+ *   priv  - The driver state
+ *   index - The DCI item index
+ *   len   - The item length in bytes
+ *   off   - The byte offsets
+ *   val   - The byte values
+ *   count - The number of bytes to change
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_dci_patch(FAR struct vl53l5cx_dev_s *priv,
+                              uint16_t index, size_t len,
+                              FAR const uint8_t *off,
+                              FAR const uint8_t *val, size_t count)
+{
+  uint8_t item[VL53L5CX_DCI_MAX];
+  size_t i;
+  int ret;
+
+  ret = vl53l5cx_dci_read(priv, index, item, len);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  for (i = 0; i < count; i++)
+    {
+      DEBUGASSERT(off[i] < len);
+      item[off[i]] = val[i];
+    }
+
+  return vl53l5cx_dci_write(priv, index, item, len);
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_dci_put32
+ *
+ * Description:
+ *   Write a 32-bit DCI item.
+ *
+ * Input Parameters:
+ *   priv  - The driver state
+ *   index - The DCI item index
+ *   val   - The value
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_dci_put32(FAR struct vl53l5cx_dev_s *priv,
+                              uint16_t index, uint32_t val)
+{
+  uint8_t item[4];
+
+  vl53l5cx_putle32(item, val);
+  return vl53l5cx_dci_write(priv, index, item, sizeof(item));
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_downsample
+ *
+ * Description:
+ *   Average an 8x8 grid of 32-bit unsigned or 16-bit signed N-view values to
+ *   4x4 in elements 0-15, zero elements 16-63.
+ *
+ * Input Parameters:
+ *   grid  - The grid
+ *   range - True for 16-bit signed values
+ *
+ * Returned Value:
+ *   None.
+ *
+ ****************************************************************************/
+
+static void vl53l5cx_downsample(FAR uint8_t *grid, bool range)
+{
+  int32_t out[VL53L5CX_ZONES_MAX / 4];
+  size_t esize = range ? sizeof(int16_t) : sizeof(uint32_t);
+  int64_t sum;
+  int zone;
+  int row;
+  int col;
+  int i;
+
+  for (row = 0; row < VL53L5CX_WIDTH_4X4; row++)
+    {
+      for (col = 0; col < VL53L5CX_WIDTH_4X4; col++)
+        {
+          sum = 0;
+          for (i = 0; i < 4; i++)
+            {
+              zone = (2 * row + i / 2) * VL53L5CX_WIDTH_8X8 + 2 * col +
+                     i % 2;
+              if (range)
+                {
+                  sum += (int16_t)vl53l5cx_getle16(&grid[zone * esize]);
+                }
+              else
+                {
+                  sum += vl53l5cx_getle32(&grid[zone * esize]);
+                }
+            }
+
+          out[row * VL53L5CX_WIDTH_4X4 + col] = (int32_t)(sum / 4);
+        }
+    }
+
+  memset(grid, 0, VL53L5CX_ZONES_MAX * esize);
+  for (i = 0; i < nitems(out); i++)
+    {
+      if (range)
+        {
+          grid[i * esize]     = out[i] & 0xff;
+          grid[i * esize + 1] = (out[i] >> 8) & 0xff;
+        }
+      else
+        {
+          vl53l5cx_putle32(&grid[i * esize], (uint32_t)out[i]);
+        }
+    }
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_upload_offset
+ *
+ * Description:
+ *   Upload the device offset record, patched to 4x4 if needed.
+ *
+ * Input Parameters:
+ *   priv  - The driver state
+ *   is4x4 - True for the 4x4 resolution
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_upload_offset(FAR struct vl53l5cx_dev_s *priv,
+                                  bool is4x4)
+{
+  FAR uint8_t *rec = priv->scratch;
+  size_t ulen = VL53L5CX_OFS_SIZE - VL53L5CX_OFS_DROP;
+  int ret;
+
+  memcpy(rec, priv->ofs, VL53L5CX_OFS_SIZE);
+  if (is4x4)
+    {
+      memcpy(&rec[VL53L5CX_OFS_PATCH], g_vl53l5cx_ofs_patch,
+             sizeof(g_vl53l5cx_ofs_patch));
+      vl53l5cx_swap(rec, VL53L5CX_OFS_SIZE);
+      vl53l5cx_downsample(&rec[VL53L5CX_OFS_SIGNAL], false);
+      vl53l5cx_downsample(&rec[VL53L5CX_OFS_RANGE], true);
+      vl53l5cx_swap(rec, VL53L5CX_OFS_SIZE);
+    }
+
+  memmove(rec, &rec[VL53L5CX_OFS_DROP], ulen);
+  memcpy(&rec[ulen], g_vl53l5cx_ofs_footer, sizeof(g_vl53l5cx_ofs_footer));
+
+  ret = vl53l5cx_write(priv, VL53L5CX_UI_END - VL53L5CX_OFS_SIZE, rec,
+                       VL53L5CX_OFS_SIZE);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  return vl53l5cx_pollui(priv, false);
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_upload_xtalk
+ *
+ * Description:
+ *   Upload the default crosstalk record, patched to 4x4 if needed.
+ *
+ * Input Parameters:
+ *   priv  - The driver state
+ *   is4x4 - True for the 4x4 resolution
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_upload_xtalk(FAR struct vl53l5cx_dev_s *priv,
+                                 bool is4x4)
+{
+  FAR uint8_t *rec = priv->scratch;
+  int ret;
+
+  memcpy(rec, VL53L5CX_DEFAULT_XTALK, VL53L5CX_XTALK_SIZE);
+  if (is4x4)
+    {
+      memcpy(&rec[VL53L5CX_XT_PATCH1], g_vl53l5cx_xt_patch1,
+             sizeof(g_vl53l5cx_xt_patch1));
+      memcpy(&rec[VL53L5CX_XT_PATCH2], g_vl53l5cx_xt_patch2,
+             sizeof(g_vl53l5cx_xt_patch2));
+      vl53l5cx_swap(rec, VL53L5CX_XTALK_SIZE);
+      vl53l5cx_downsample(&rec[VL53L5CX_XT_SIGNAL], false);
+      vl53l5cx_swap(rec, VL53L5CX_XTALK_SIZE);
+      memcpy(&rec[VL53L5CX_XT_PATCH3], g_vl53l5cx_xt_patch3,
+             sizeof(g_vl53l5cx_xt_patch3));
+      memcpy(&rec[VL53L5CX_XT_PATCH4], g_vl53l5cx_xt_patch4,
+             sizeof(g_vl53l5cx_xt_patch4));
+    }
+
+  ret = vl53l5cx_write(priv, VL53L5CX_UI_END - VL53L5CX_XTALK_SIZE, rec,
+                       VL53L5CX_XTALK_SIZE);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  return vl53l5cx_pollui(priv, false);
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_download
+ *
+ * Description:
+ *   Write the firmware into the three firmware pages.
+ *
+ * Input Parameters:
+ *   priv - The driver state
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_download(FAR struct vl53l5cx_dev_s *priv)
+{
+  FAR const uint8_t *fw = VL53L5CX_FIRMWARE;
+  size_t left = VL53L5CX_FW_SIZE;
+  size_t nbytes;
+  uint8_t page;
+  int ret;
+
+  for (page = VL53L5CX_PAGE_FW; left > 0; page++)
+    {
+      nbytes = MIN(left, VL53L5CX_FW_PAGE_SIZE);
+      ret = vl53l5cx_page(priv, page);
+      if (ret < 0)
+        {
+          return ret;
+        }
+
+      ret = vl53l5cx_write(priv, 0, fw, nbytes);
+      if (ret < 0)
+        {
+          return ret;
+        }
+
+      fw   += nbytes;
+      left -= nbytes;
+    }
+
+  return OK;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_mcu_boot
+ *
+ * Description:
+ *   Wait for the MCU to boot after its reset.
+ *
+ * Input Parameters:
+ *   priv - The driver state
+ *
+ * Returned Value:
+ *   Zero (OK) on success; -EIO on an MCU error, -ETIMEDOUT or another
+ *   negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_mcu_boot(FAR struct vl53l5cx_dev_s *priv)
+{
+  uint8_t state;
+  uint8_t err;
+  int tries;
+  int ret;
+
+  for (tries = 0; tries < VL53L5CX_MCU_BOOT_TRIES; tries++)
+    {
+      ret = vl53l5cx_getreg(priv, VL53L5CX_MCU_REG, &state);
+      if (ret < 0)
+        {
+          return ret;
+        }
+
+      if ((state & VL53L5CX_MCU_STOPPED) != 0)
+        {
+          ret = vl53l5cx_getreg(priv, VL53L5CX_MCU_ERR_REG, &err);
+          if (ret >= 0 && err != 0)
+            {
+              verr("ERROR: MCU boot error 0x%02x\n", err);
+              ret = -EIO;
+            }
+
+          return ret;
+        }
+
+      if ((state & VL53L5CX_MCU_READY) != 0)
+        {
+          return OK;
+        }
+
+      nxsig_usleep(VL53L5CX_MCU_BOOT_US);
+    }
+
+  verr("ERROR: MCU boot timeout\n");
+  return -ETIMEDOUT;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_nvm
+ *
+ * Description:
+ *   Read the factory offset record from the NVM.
+ *
+ * Input Parameters:
+ *   priv - The driver state
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_nvm(FAR struct vl53l5cx_dev_s *priv)
+{
+  int ret;
+
+  ret = vl53l5cx_write(priv, VL53L5CX_UI_END - VL53L5CX_NVMREQ_SIZE,
+                       VL53L5CX_GET_NVM_CMD, VL53L5CX_NVMREQ_SIZE);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_pollui(priv, true);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_read(priv, VL53L5CX_UI_DATA_REG, priv->scratch,
+                      VL53L5CX_NVM_SIZE);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  memcpy(priv->ofs, priv->scratch, VL53L5CX_OFS_SIZE);
+  return OK;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_load
+ *
+ * Description:
+ *   Reboot the sensor and write its firmware (ULD vl53l5cx_init()).
+ *
+ * Input Parameters:
+ *   priv - The driver state
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_load(FAR struct vl53l5cx_dev_s *priv)
+{
+  int ret;
+
+  /* Software reboot and boot wait */
+
+  ret = vl53l5cx_runseq(priv, g_vl53l5cx_reboot,
+                        nitems(g_vl53l5cx_reboot));
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_pollreg(priv, VL53L5CX_MCU_REG, VL53L5CX_MCU_ALL,
+                         VL53L5CX_MCU_READY, VL53L5CX_POLL_TRIES);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_runseq(priv, g_vl53l5cx_booted,
+                        nitems(g_vl53l5cx_booted));
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_pollreg(priv, VL53L5CX_FW_REG, VL53L5CX_FW_READY,
+                         VL53L5CX_FW_READY, VL53L5CX_POLL_TRIES);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  /* Power on, MCU wake and firmware download */
+
+  ret = vl53l5cx_runseq(priv, g_vl53l5cx_poweron,
+                        nitems(g_vl53l5cx_poweron));
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_download(priv);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_runseq(priv, g_vl53l5cx_fwcheck,
+                        nitems(g_vl53l5cx_fwcheck));
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_pollreg(priv, VL53L5CX_FW_REG, VL53L5CX_FW_READY,
+                         VL53L5CX_FW_READY, VL53L5CX_POLL_TRIES);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  /* MCU reset and boot */
+
+  ret = vl53l5cx_runseq(priv, g_vl53l5cx_mcuboot,
+                        nitems(g_vl53l5cx_mcuboot));
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_mcu_boot(priv);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  return vl53l5cx_page(priv, VL53L5CX_PAGE_MAIN);
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_configure
+ *
+ * Description:
+ *   Write the offset and crosstalk records, the default configuration and
+ *   the post-config parameters (ULD vl53l5cx_init()).  The sensor is left
+ *   stopped, 4x4, in continuous mode.
+ *
+ * Input Parameters:
+ *   priv - The driver state
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_configure(FAR struct vl53l5cx_dev_s *priv)
+{
+  int ret;
+
+  /* Offset record, default crosstalk and configuration */
+
+  ret = vl53l5cx_nvm(priv);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_upload_offset(priv, true);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_upload_xtalk(priv, true);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_write(priv, VL53L5CX_UI_END - VL53L5CX_CFG_SIZE,
+                       VL53L5CX_DEFAULT_CONFIGURATION, VL53L5CX_CFG_SIZE);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_pollui(priv, false);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  /* Post-config parameters, then continuous mode */
+
+  ret = vl53l5cx_dci_write(priv, VL53L5CX_DCI_PIPE_CTRL, g_vl53l5cx_pipe,
+                           sizeof(g_vl53l5cx_pipe));
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_dci_put32(priv, VL53L5CX_DCI_SINGLE_RANGE, 1);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_dci_patch(priv, VL53L5CX_DCI_GLARE, VL53L5CX_GLARE_LEN,
+                           g_vl53l5cx_glare_off, g_vl53l5cx_glare_val,
+                           nitems(g_vl53l5cx_glare_off));
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_dci_patch(priv, VL53L5CX_DCI_MODE, VL53L5CX_MODE_LEN,
+                           g_vl53l5cx_mode_off, g_vl53l5cx_mode_val,
+                           nitems(g_vl53l5cx_mode_off));
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_dci_put32(priv, VL53L5CX_DCI_SINGLE_RANGE, 0);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  priv->zones = VL53L5CX_WIDTH_4X4 * VL53L5CX_WIDTH_4X4;
+  return OK;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_boot
+ *
+ * Description:
+ *   Load the firmware and configure the sensor.
+ *
+ * Input Parameters:
+ *   priv - The driver state
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_boot(FAR struct vl53l5cx_dev_s *priv)
+{
+  int ret;
+
+  ret = vl53l5cx_load(priv);
+  if (ret >= 0)
+    {
+      ret = vl53l5cx_configure(priv);
+    }
+
+  if (ret < 0)
+    {
+      verr("ERROR: boot failed: %d\n", ret);
+    }
+
+  return ret;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_power
+ *
+ * Description:
+ *   Put the stopped sensor to sleep or wake it up.
+ *
+ * Input Parameters:
+ *   priv  - The driver state
+ *   sleep - True to sleep, false to wake up
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_power(FAR struct vl53l5cx_dev_s *priv, bool sleep)
+{
+  int ret;
+
+  ret = vl53l5cx_page(priv, VL53L5CX_PAGE_BOOT);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_putreg(priv, VL53L5CX_POWER_REG,
+                        sleep ? VL53L5CX_POWER_SLEEP : VL53L5CX_POWER_WAKE);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_pollreg(priv, VL53L5CX_MCU_REG, VL53L5CX_MCU_READY,
+                         sleep ? 0 : VL53L5CX_MCU_READY,
+                         VL53L5CX_POLL_TRIES);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_page(priv, VL53L5CX_PAGE_MAIN);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  priv->asleep = sleep;
+  return OK;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_set_zones
+ *
+ * Description:
+ *   Set the 4x4 or 8x8 resolution and upload the matching offset and
+ *   crosstalk records.
+ *
+ * Input Parameters:
+ *   priv  - The driver state
+ *   is4x4 - True for the 4x4 resolution
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_set_zones(FAR struct vl53l5cx_dev_s *priv, bool is4x4)
+{
+  int ret;
+
+  ret = vl53l5cx_dci_patch(priv, VL53L5CX_DCI_DSS, VL53L5CX_DSS_LEN,
+                           g_vl53l5cx_dss_off,
+                           is4x4 ? g_vl53l5cx_dss_4x4 : g_vl53l5cx_dss_8x8,
+                           nitems(g_vl53l5cx_dss_off));
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_dci_patch(priv, VL53L5CX_DCI_ZONE_CFG,
+                           VL53L5CX_ZONE_CFG_LEN, g_vl53l5cx_zone_off,
+                           is4x4 ? g_vl53l5cx_zone_4x4 : g_vl53l5cx_zone_8x8,
+                           nitems(g_vl53l5cx_zone_off));
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_upload_offset(priv, is4x4);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  return vl53l5cx_upload_xtalk(priv, is4x4);
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_stop_ranging
+ *
+ * Description:
+ *   Stop ranging (ULD vl53l5cx_stop_ranging()).  The registers are restored
+ *   even after an error.
+ *
+ * Input Parameters:
+ *   priv - The driver state
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_stop_ranging(FAR struct vl53l5cx_dev_s *priv)
+{
+  uint8_t cmd[4];
+  uint8_t state = 0;
+  int ret;
+  int err = OK;
+
+  ret = vl53l5cx_read(priv, VL53L5CX_CMD_REG, cmd, sizeof(cmd));
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_page(priv, VL53L5CX_PAGE_BOOT);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  if (memcmp(cmd, g_vl53l5cx_stopped, sizeof(cmd)) != 0)
+    {
+      ret = vl53l5cx_putreg(priv, VL53L5CX_STOP_ARG_REG, VL53L5CX_STOP_ARG);
+      if (ret >= 0)
+        {
+          ret = vl53l5cx_putreg(priv, VL53L5CX_STOP_CMD_REG,
+                                VL53L5CX_STOP_CMD);
+        }
+
+      if (ret >= 0)
+        {
+          err = vl53l5cx_pollreg(priv, VL53L5CX_MCU_REG,
+                                 VL53L5CX_MCU_STOPPED, VL53L5CX_MCU_STOPPED,
+                                 VL53L5CX_STOP_TRIES);
+        }
+    }
+
+  if (ret >= 0)
+    {
+      ret = vl53l5cx_getreg(priv, VL53L5CX_MCU_REG, &state);
+    }
+
+  if (ret >= 0 && (state & VL53L5CX_MCU_STOPPED) != 0)
+    {
+      ret = vl53l5cx_getreg(priv, VL53L5CX_MCU_ERR_REG, &state);
+      if (ret >= 0 && state != VL53L5CX_MCU_STOP_OK1 &&
+          state != VL53L5CX_MCU_STOP_OK2)
+        {
+          verr("ERROR: stop status 0x%02x\n", state);
+          err = -EIO;
+        }
+    }
+
+  /* Clean up even after an error */
+
+  if (ret >= 0)
+    {
+      ret = vl53l5cx_putreg(priv, VL53L5CX_STOP_CMD_REG, 0);
+    }
+
+  if (ret >= 0)
+    {
+      ret = vl53l5cx_putreg(priv, VL53L5CX_STOP_ARG_REG, 0);
+    }
+
+  if (ret >= 0)
+    {
+      ret = vl53l5cx_putreg(priv, VL53L5CX_POWER_REG, VL53L5CX_POWER_WAKE);
+    }
+
+  if (ret >= 0)
+    {
+      ret = vl53l5cx_page(priv, VL53L5CX_PAGE_MAIN);
+    }
+
+  return ret < 0 ? ret : err;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_set_outputs
+ *
+ * Description:
+ *   Write the output list, the frame size and the output mask for one
+ *   target per zone, and keep the frame size.
+ *
+ * Input Parameters:
+ *   priv - The driver state
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_set_outputs(FAR struct vl53l5cx_dev_s *priv)
+{
+  uint8_t list[VL53L5CX_NOUTPUTS * 4];
+  uint8_t cfg[8];
+  uint8_t mask[16];
+  uint32_t hdr;
+  uint32_t type;
+  uint32_t fsize = VL53L5CX_FRAME_OVERHEAD;
+  int i;
+  int ret;
+
+  /* Element counts of the per-zone blocks (one target per zone) */
+
+  for (i = 0; i < VL53L5CX_NOUTPUTS; i++)
+    {
+      hdr  = g_vl53l5cx_outputs[i];
+      type = VL53L5CX_HDR_TYPE(hdr);
+      if ((g_vl53l5cx_enabled & (1 << i)) != 0)
+        {
+          if (type >= 1 && type <= VL53L5CX_TYPE_ELEM_MAX)
+            {
+              hdr    = (hdr & ~VL53L5CX_HDR_SIZE_MASK) |
+                       (priv->zones << VL53L5CX_HDR_SIZE_SHIFT);
+              fsize += 4 + type * priv->zones;
+            }
+          else
+            {
+              fsize += 4 + VL53L5CX_HDR_SIZE(hdr);
+            }
+        }
+
+      vl53l5cx_putle32(&list[i * 4], hdr);
+    }
+
+  if (fsize > sizeof(priv->scratch))
+    {
+      return -EINVAL;
+    }
+
+  priv->framesize = fsize;
+
+  vl53l5cx_putle32(&cfg[0], fsize);
+  vl53l5cx_putle32(&cfg[4], VL53L5CX_OUT_COUNT);
+
+  memset(mask, 0, sizeof(mask));
+  vl53l5cx_putle32(&mask[0], g_vl53l5cx_enabled);
+  vl53l5cx_putle32(&mask[12], VL53L5CX_OUT_MASK3);
+
+  ret = vl53l5cx_dci_write(priv, VL53L5CX_DCI_OUTPUT_LIST, list,
+                           sizeof(list));
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_dci_write(priv, VL53L5CX_DCI_OUTPUT_CFG, cfg, sizeof(cfg));
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  return vl53l5cx_dci_write(priv, VL53L5CX_DCI_OUTPUT_MASK, mask,
+                            sizeof(mask));
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_start_ranging
+ *
+ * Description:
+ *   Configure the outputs, start ranging and check that the firmware took
+ *   the output configuration.
+ *
+ * Input Parameters:
+ *   priv - The driver state
+ *
+ * Returned Value:
+ *   Zero (OK) on success; -EIO if the firmware rejects the outputs, or
+ *   another negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_start_ranging(FAR struct vl53l5cx_dev_s *priv)
+{
+  uint8_t check[VL53L5CX_OUTPUT_CHECK_LEN];
+  int ret;
+
+  ret = vl53l5cx_set_outputs(priv);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_page(priv, VL53L5CX_PAGE_BOOT);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_putreg(priv, VL53L5CX_POWER_REG, VL53L5CX_POWER_RANGING);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_page(priv, VL53L5CX_PAGE_MAIN);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  ret = vl53l5cx_write(priv, VL53L5CX_CMD_REG, g_vl53l5cx_start_cmd,
+                       sizeof(g_vl53l5cx_start_cmd));
+  if (ret < 0)
+    {
+      goto errout;
+    }
+
+  ret = vl53l5cx_pollui(priv, false);
+  if (ret < 0)
+    {
+      goto errout;
+    }
+
+  ret = vl53l5cx_dci_read(priv, VL53L5CX_DCI_OUTPUT_CHECK, check,
+                          sizeof(check));
+  if (ret < 0)
+    {
+      goto errout;
+    }
+
+  if (vl53l5cx_getle16(&check[VL53L5CX_OUTPUT_CHECK_SIZE]) !=
+      priv->framesize)
+    {
+      verr("ERROR: output configuration rejected\n");
+      ret = -EIO;
+      goto errout;
+    }
+
+  priv->lastcount = VL53L5CX_STREAM_NONE;
+  return OK;
+
+errout:
+  vl53l5cx_stop_ranging(priv);
+  return ret;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_parse
+ *
+ * Description:
+ *   Check the result frame in scratch and write the Z16 image.
+ *
+ * Input Parameters:
+ *   priv - The driver state
+ *   out  - The Z16 image buffer
+ *
+ * Returned Value:
+ *   The image size in bytes on success; -EIO for a bad frame.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_parse(FAR struct vl53l5cx_dev_s *priv,
+                          FAR uint8_t *out)
+{
+  FAR uint8_t *frame = priv->scratch;
+  FAR const uint8_t *footer;
+  FAR const uint8_t *targets = NULL;
+  FAR const uint8_t *distance = NULL;
+  FAR const uint8_t *status = NULL;
+  size_t fsize = priv->framesize;
+  size_t pos;
+  size_t len;
+  uint32_t hdr;
+  uint32_t type;
+  uint32_t size;
+  int32_t mm;
+  int i;
+
+  vl53l5cx_swap(frame, fsize);
+
+  /* The header and the footer IDs differ in a torn frame */
+
+  footer = &frame[fsize - VL53L5CX_FRAME_FOOTER];
+  if (frame[VL53L5CX_FRAME_ID] != footer[0] ||
+      frame[VL53L5CX_FRAME_ID + 1] != footer[1])
+    {
+      return -EIO;
+    }
+
+  for (pos = VL53L5CX_FRAME_BLOCKS;
+       pos + 4 <= fsize - VL53L5CX_FRAME_FOOTER; pos += 4 + len)
+    {
+      hdr  = vl53l5cx_getle32(&frame[pos]);
+      type = VL53L5CX_HDR_TYPE(hdr);
+      size = VL53L5CX_HDR_SIZE(hdr);
+      len  = type >= 1 && type <= VL53L5CX_TYPE_ELEM_MAX ? type * size :
+                                                          size;
+      if (pos + 4 + len > fsize - VL53L5CX_FRAME_FOOTER)
+        {
+          break;
+        }
+
+      switch (VL53L5CX_HDR_INDEX(hdr))
+        {
+          case VL53L5CX_BLK_TARGETS:
+            targets = len == priv->zones ? &frame[pos + 4] : NULL;
+            break;
+
+          case VL53L5CX_BLK_DISTANCE:
+            distance = len == priv->zones * sizeof(int16_t) ?
+                       &frame[pos + 4] : NULL;
+            break;
+
+          case VL53L5CX_BLK_STATUS:
+            status = len == priv->zones ? &frame[pos + 4] : NULL;
+            break;
+
+          default:
+            break;
+        }
+    }
+
+  if (targets == NULL || distance == NULL || status == NULL)
+    {
+      return -EIO;
+    }
+
+  for (i = 0; i < priv->zones; i++)
+    {
+      mm = (int16_t)vl53l5cx_getle16(&distance[i * 2]);
+      mm /= 1 << VL53L5CX_DISTANCE_SHIFT;
+      if (targets[i] == 0 || mm < 0 ||
+          (status[i] != VL53L5CX_STATUS_VALID &&
+           status[i] != VL53L5CX_STATUS_VALID_LARGE))
+        {
+          mm = 0;
+        }
+
+      out[i * 2]     = mm & 0xff;
+      out[i * 2 + 1] = (mm >> 8) & 0xff;
+    }
+
+  return priv->zones * VL53L5CX_BYTES_PER_PIXEL;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_worker
+ *
+ * Description:
+ *   Check the data-ready word, read a frame and deliver it to the capture
+ *   callback.
+ *
+ * Input Parameters:
+ *   arg - The driver state
+ *
+ * Returned Value:
+ *   None.
+ *
+ ****************************************************************************/
+
+static void vl53l5cx_worker(FAR void *arg)
+{
+  FAR struct vl53l5cx_dev_s *priv = arg;
+  imgdata_capture_t callback;
+  FAR void *cbarg;
+  FAR uint8_t *buf;
+  struct timespec ts;
+  struct timeval tv;
+  irqstate_t flags;
+  uint8_t status[4];
+  bool frame = false;
+  int ret;
+
+  ret = vl53l5cx_read(priv, VL53L5CX_RESULT_REG, status,
+                      sizeof(status));
+  if (ret >= 0 && status[0] != priv->lastcount &&
+      status[0] != VL53L5CX_STREAM_NONE && status[1] == VL53L5CX_READY_B1 &&
+      (status[2] & VL53L5CX_READY_B2) == VL53L5CX_READY_B2 &&
+      (status[3] & VL53L5CX_READY_B3) == VL53L5CX_READY_B3)
+    {
+      frame = true;
+    }
+  else if (ret >= 0 && (status[3] & VL53L5CX_READY_ERROR) != 0)
+    {
+      verr("ERROR: ranging error 0x%02x\n", status[2]);
+      ret = -EIO;
+    }
+
+  if (frame || ret < 0)
+    {
+      clock_systime_timespec(&ts);
+      TIMESPEC_TO_TIMEVAL(&tv, &ts);
+
+      flags    = spin_lock_irqsave(&priv->buflock);
+      buf      = priv->buf;
+      callback = priv->callback;
+      cbarg    = priv->arg;
+      spin_unlock_irqrestore(&priv->buflock, flags);
+
+      if (frame)
+        {
+          priv->lastcount = status[0];
+        }
+
+      if (callback == NULL)
+        {
+          return;
+        }
+
+      /* No buffer queued: report a dropped frame (size 0) */
+
+      if (buf == NULL)
+        {
+          ret = 0;
+        }
+      else if (frame)
+        {
+          ret = vl53l5cx_read(priv, VL53L5CX_RESULT_REG, priv->scratch,
+                              priv->framesize);
+          if (ret >= 0)
+            {
+              priv->lastcount = priv->scratch[0];
+              ret = vl53l5cx_parse(priv, buf);
+            }
+        }
+
+      /* An error frame is returned zeroed, with V4L2_BUF_FLAG_ERROR */
+
+      if (ret < 0)
+        {
+          memset(buf, 0, priv->zones * VL53L5CX_BYTES_PER_PIXEL);
+        }
+
+      callback(ret < 0 ? EIO : 0,
+               ret < 0 ? priv->zones * VL53L5CX_BYTES_PER_PIXEL : ret,
+               &tv, cbarg);
+    }
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_interrupt
+ *
+ * Description:
+ *   INT falls when a frame is ready: read it on the work queue.
+ *
+ * Input Parameters:
+ *   irq     - The interrupt number
+ *   context - The interrupt context
+ *   arg     - The driver state
+ *
+ * Returned Value:
+ *   Zero (OK).
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_interrupt(int irq, FAR void *context, FAR void *arg)
+{
+  FAR struct vl53l5cx_dev_s *priv = arg;
+
+  work_queue(LPWORK, &priv->work, vl53l5cx_worker, priv, 0);
+  return OK;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_halt
+ *
+ * Description:
+ *   Disable INT and wait for a running frame read to end.
+ *
+ * Input Parameters:
+ *   priv - The driver state
+ *
+ * Returned Value:
+ *   None.
+ *
+ ****************************************************************************/
+
+static void vl53l5cx_halt(FAR struct vl53l5cx_dev_s *priv)
+{
+  priv->config->attach(priv->config, NULL, NULL);
+  work_cancel_sync(LPWORK, &priv->work);
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_check_format
+ *
+ * Description:
+ *   Check a format and a frame interval.
+ *
+ * Input Parameters:
+ *   nr_fmts - The number of formats
+ *   width   - The frame width
+ *   height  - The frame height
+ *   z16     - True for the Z16 pixel format
+ *   num     - The frame interval numerator
+ *   den     - The frame interval denominator
+ *   fps     - The location of the rounded frame rate, or NULL
+ *
+ * Returned Value:
+ *   The zones per row on success; -EINVAL for an unsupported setting.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_check_format(uint8_t nr_fmts, uint16_t width,
+                                 uint16_t height, bool z16,
+                                 uint32_t num, uint32_t den,
+                                 FAR uint8_t *fps)
+{
+  uint32_t rate;
+  uint32_t max;
+
+  if (nr_fmts != 1 || !z16 || width != height)
+    {
+      return -EINVAL;
+    }
+
+  if (width == VL53L5CX_WIDTH_8X8)
+    {
+      max = VL53L5CX_FPS_8X8;
+    }
+  else if (width == VL53L5CX_WIDTH_4X4)
+    {
+      max = VL53L5CX_FPS_4X4;
+    }
+  else
+    {
+      return -EINVAL;
+    }
+
+  /* The sensor ranges at whole frames per second: round the interval */
+
+  if (num == 0)
+    {
+      return -EINVAL;
+    }
+
+  rate = (den + num / 2) / num;
+  if (rate < VL53L5CX_FPS_MIN || rate > max)
+    {
+      return -EINVAL;
+    }
+
+  if (fps != NULL)
+    {
+      *fps = rate;
+    }
+
+  return width;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_is_available
+ *
+ * Description:
+ *   Report the sensor present: it was checked at registration.
+ *
+ * Input Parameters:
+ *   sensor - The image sensor interface of the driver
+ *
+ * Returned Value:
+ *   True.
+ *
+ ****************************************************************************/
+
+static bool vl53l5cx_is_available(FAR struct imgsensor_s *sensor)
+{
+  return true;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_init
+ *
+ * Description:
+ *   Open of the capture device: the sensor boots at the first stream start.
+ *
+ * Input Parameters:
+ *   sensor - The image sensor interface of the driver
+ *
+ * Returned Value:
+ *   Zero (OK).
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_init(FAR struct imgsensor_s *sensor)
+{
+  return OK;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_uninit
+ *
+ * Description:
+ *   Last close: stop ranging and put the sensor to sleep.
+ *
+ * Input Parameters:
+ *   sensor - The image sensor interface of the driver
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_uninit(FAR struct imgsensor_s *sensor)
+{
+  FAR struct vl53l5cx_dev_s *priv =
+    container_of(sensor, struct vl53l5cx_dev_s, sensor);
+  int ret = OK;
+
+  nxmutex_lock(&priv->lock);
+  vl53l5cx_halt(priv);
+  if (priv->ranging)
+    {
+      priv->ranging = false;
+      ret = vl53l5cx_stop_ranging(priv);
+    }
+
+  if (ret >= 0 && priv->booted && !priv->asleep)
+    {
+      ret = vl53l5cx_power(priv, true);
+    }
+
+  nxmutex_unlock(&priv->lock);
+  return ret;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_get_driver_name
+ *
+ * Description:
+ *   Return the driver name.
+ *
+ * Input Parameters:
+ *   sensor - The image sensor interface of the driver
+ *
+ * Returned Value:
+ *   The driver name.
+ *
+ ****************************************************************************/
+
+static FAR const char *
+vl53l5cx_get_driver_name(FAR struct imgsensor_s *sensor)
+{
+  return "VL53L5CX";
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_validate_frame_setting
+ *
+ * Description:
+ *   Check a stream setting.
+ *
+ * Input Parameters:
+ *   sensor      - The image sensor interface of the driver
+ *   type        - The stream type
+ *   nr_datafmts - The number of formats
+ *   datafmts    - The formats
+ *   ival        - The frame interval
+ *
+ * Returned Value:
+ *   Zero (OK) for a supported setting; -EINVAL otherwise.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_validate_frame_setting(FAR struct imgsensor_s *sensor,
+                                           imgsensor_stream_type_t type,
+                                           uint8_t nr_datafmts,
+                                           FAR imgsensor_format_t *datafmts,
+                                           FAR imgsensor_interval_t *ival)
+{
+  int ret;
+
+  ret = vl53l5cx_check_format(nr_datafmts, datafmts->width,
+                              datafmts->height,
+                              datafmts->pixelformat ==
+                              IMGSENSOR_PIX_FMT_Z16,
+                              ival->numerator, ival->denominator, NULL);
+  return ret < 0 ? ret : OK;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_start_capture
+ *
+ * Description:
+ *   Boot the sensor if needed, set the resolution and the rate and start
+ *   ranging.
+ *
+ * Input Parameters:
+ *   sensor      - The image sensor interface of the driver
+ *   type        - The stream type
+ *   nr_datafmts - The number of formats
+ *   datafmts    - The formats
+ *   interval    - The frame interval
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_start_capture(FAR struct imgsensor_s *sensor,
+                                  imgsensor_stream_type_t type,
+                                  uint8_t nr_datafmts,
+                                  FAR imgsensor_format_t *datafmts,
+                                  FAR imgsensor_interval_t *interval)
+{
+  FAR struct vl53l5cx_dev_s *priv =
+    container_of(sensor, struct vl53l5cx_dev_s, sensor);
+  uint8_t fps;
+  uint8_t zones;
+  int width;
+  int ret = OK;
+
+  width = vl53l5cx_check_format(nr_datafmts, datafmts->width,
+                                datafmts->height,
+                                datafmts->pixelformat ==
+                                IMGSENSOR_PIX_FMT_Z16,
+                                interval->numerator,
+                                interval->denominator, &fps);
+  if (width < 0 || type != IMGSENSOR_STREAM_TYPE_VIDEO)
+    {
+      return -EINVAL;
+    }
+
+  zones = width * width;
+
+  nxmutex_lock(&priv->lock);
+  if (!priv->booted)
+    {
+      ret = vl53l5cx_boot(priv);
+      if (ret < 0)
+        {
+          goto out;
+        }
+
+      priv->booted = true;
+    }
+
+  if (priv->asleep)
+    {
+      ret = vl53l5cx_power(priv, false);
+      if (ret < 0)
+        {
+          goto out;
+        }
+    }
+
+  if (priv->zones != zones)
+    {
+      priv->zones = 0;
+      ret = vl53l5cx_set_zones(priv, width == VL53L5CX_WIDTH_4X4);
+      if (ret < 0)
+        {
+          goto out;
+        }
+
+      priv->zones = zones;
+    }
+
+  ret = vl53l5cx_dci_patch(priv, VL53L5CX_DCI_FREQ, VL53L5CX_FREQ_LEN,
+                           &g_vl53l5cx_freq_off, &fps, 1);
+  if (ret < 0)
+    {
+      goto out;
+    }
+
+  priv->fps = fps;
+
+  ret = vl53l5cx_start_ranging(priv);
+  if (ret < 0)
+    {
+      goto out;
+    }
+
+  ret = priv->config->attach(priv->config, vl53l5cx_interrupt, priv);
+  if (ret < 0)
+    {
+      vl53l5cx_stop_ranging(priv);
+      goto out;
+    }
+
+  priv->ranging = true;
+
+out:
+  nxmutex_unlock(&priv->lock);
+  return ret;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_stop_capture
+ *
+ * Description:
+ *   Stop ranging.
+ *
+ * Input Parameters:
+ *   sensor - The image sensor interface of the driver
+ *   type   - The stream type
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_stop_capture(FAR struct imgsensor_s *sensor,
+                                 imgsensor_stream_type_t type)
+{
+  FAR struct vl53l5cx_dev_s *priv =
+    container_of(sensor, struct vl53l5cx_dev_s, sensor);
+  int ret = OK;
+
+  nxmutex_lock(&priv->lock);
+  vl53l5cx_halt(priv);
+  if (priv->ranging)
+    {
+      priv->ranging = false;
+      ret = vl53l5cx_stop_ranging(priv);
+    }
+
+  nxmutex_unlock(&priv->lock);
+  return ret;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_get_frame_interval
+ *
+ * Description:
+ *   Return the frame interval of the running stream.
+ *
+ * Input Parameters:
+ *   sensor   - The image sensor interface of the driver
+ *   type     - The stream type
+ *   interval - The location of the interval
+ *
+ * Returned Value:
+ *   Zero (OK) while ranging; -EINVAL otherwise.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_get_frame_interval(FAR struct imgsensor_s *sensor,
+                                       imgsensor_stream_type_t type,
+                                       FAR imgsensor_interval_t *interval)
+{
+  FAR struct vl53l5cx_dev_s *priv =
+    container_of(sensor, struct vl53l5cx_dev_s, sensor);
+  int ret = -EINVAL;
+
+  nxmutex_lock(&priv->lock);
+  if (priv->ranging)
+    {
+      interval->numerator   = 1;
+      interval->denominator = priv->fps;
+      ret = OK;
+    }
+
+  nxmutex_unlock(&priv->lock);
+  return ret;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_data_init
+ *
+ * Description:
+ *   Open of the image data interface: nothing to do.
+ *
+ * Input Parameters:
+ *   data - The image data interface of the driver
+ *
+ * Returned Value:
+ *   Zero (OK).
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_data_init(FAR struct imgdata_s *data)
+{
+  return OK;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_data_uninit
+ *
+ * Description:
+ *   Close of the image data interface: nothing to do.
+ *
+ * Input Parameters:
+ *   data - The image data interface of the driver
+ *
+ * Returned Value:
+ *   Zero (OK).
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_data_uninit(FAR struct imgdata_s *data)
+{
+  return OK;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_data_set_buf
+ *
+ * Description:
+ *   Set the buffer for the next frame, NULL to drop frames.  Also called
+ *   from the frame callback, so it does not block.
+ *
+ * Input Parameters:
+ *   data        - The image data interface of the driver
+ *   nr_datafmts - The number of formats
+ *   datafmts    - The formats
+ *   addr        - The frame buffer, or NULL
+ *   size        - The buffer size in bytes
+ *
+ * Returned Value:
+ *   Zero (OK) on success; -EINVAL for a buffer too small for the format.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_data_set_buf(FAR struct imgdata_s *data,
+                                 uint8_t nr_datafmts,
+                                 FAR imgdata_format_t *datafmts,
+                                 FAR uint8_t *addr, uint32_t size)
+{
+  FAR struct vl53l5cx_dev_s *priv =
+    container_of(data, struct vl53l5cx_dev_s, data);
+  irqstate_t flags;
+  int ret = OK;
+
+  /* No buffer (addr NULL): the next frames are dropped */
+
+  if (addr != NULL &&
+      (nr_datafmts != 1 ||
+       size < (uint32_t)datafmts->width * datafmts->height *
+              VL53L5CX_BYTES_PER_PIXEL))
+    {
+      addr = NULL;
+      ret  = -EINVAL;
+    }
+
+  flags = spin_lock_irqsave(&priv->buflock);
+  priv->buf = addr;
+  spin_unlock_irqrestore(&priv->buflock, flags);
+  return ret;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_data_validate
+ *
+ * Description:
+ *   Check a stream setting.
+ *
+ * Input Parameters:
+ *   data        - The image data interface of the driver
+ *   nr_datafmts - The number of formats
+ *   fmts        - The formats
+ *   ival        - The frame interval
+ *
+ * Returned Value:
+ *   Zero (OK) for a supported setting; -EINVAL otherwise.
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_data_validate(FAR struct imgdata_s *data,
+                                  uint8_t nr_datafmts,
+                                  FAR imgdata_format_t *fmts,
+                                  FAR imgdata_interval_t *ival)
+{
+  int ret;
+
+  ret = vl53l5cx_check_format(nr_datafmts, fmts->width, fmts->height,
+                              fmts->pixelformat == IMGDATA_PIX_FMT_Z16,
+                              ival->numerator, ival->denominator, NULL);
+  return ret < 0 ? ret : OK;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_data_start_capture
+ *
+ * Description:
+ *   Set the frame callback.
+ *
+ * Input Parameters:
+ *   data        - The image data interface of the driver
+ *   nr_datafmts - The number of formats
+ *   datafmts    - The formats
+ *   interval    - The frame interval
+ *   callback    - The frame callback
+ *   arg         - The callback argument
+ *
+ * Returned Value:
+ *   Zero (OK).
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_data_start_capture(FAR struct imgdata_s *data,
+                                       uint8_t nr_datafmts,
+                                       FAR imgdata_format_t *datafmts,
+                                       FAR imgdata_interval_t *interval,
+                                       FAR imgdata_capture_t callback,
+                                       FAR void *arg)
+{
+  FAR struct vl53l5cx_dev_s *priv =
+    container_of(data, struct vl53l5cx_dev_s, data);
+  irqstate_t flags;
+
+  flags = spin_lock_irqsave(&priv->buflock);
+  priv->callback = callback;
+  priv->arg      = arg;
+  spin_unlock_irqrestore(&priv->buflock, flags);
+  return OK;
+}
+
+/****************************************************************************
+ * Name: vl53l5cx_data_stop_capture
+ *
+ * Description:
+ *   Clear the frame callback and wait for a running frame read to end.
+ *
+ * Input Parameters:
+ *   data - The image data interface of the driver
+ *
+ * Returned Value:
+ *   Zero (OK).
+ *
+ ****************************************************************************/
+
+static int vl53l5cx_data_stop_capture(FAR struct imgdata_s *data)
+{
+  FAR struct vl53l5cx_dev_s *priv =
+    container_of(data, struct vl53l5cx_dev_s, data);
+  irqstate_t flags;
+
+  flags = spin_lock_irqsave(&priv->buflock);
+  priv->buf      = NULL;
+  priv->callback = NULL;
+  spin_unlock_irqrestore(&priv->buflock, flags);
+
+  vl53l5cx_halt(priv);
+  return OK;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: vl53l5cx_register
+ *
+ * Description:
+ *   Check that a VL53L5CX answers on the bus and register it as a V4L2
+ *   depth capture device (V4L2_PIX_FMT_Z16, 8x8 or 4x4).
+ *
+ * Input Parameters:
+ *   devpath - The capture device path, e.g. "/dev/video1"
+ *   i2c     - The I2C bus the sensor is on
+ *   config  - The board configuration, kept by the driver
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+int vl53l5cx_register(FAR const char *devpath,
+                      FAR struct i2c_master_s *i2c,
+                      FAR const struct vl53l5cx_config_s *config)
+{
+  FAR struct vl53l5cx_dev_s *priv;
+  FAR struct imgsensor_s *sensor;
+  uint8_t id = 0;
+  uint8_t rev = 0;
+  int ret;
+
+  DEBUGASSERT(devpath != NULL && i2c != NULL && config != NULL);
+
+  priv = kmm_zalloc(sizeof(struct vl53l5cx_dev_s));
+  if (priv == NULL)
+    {
+      return -ENOMEM;
+    }
+
+  priv->i2c    = i2c;
+  priv->config = config;
+  nxmutex_init(&priv->lock);
+  spin_lock_init(&priv->buflock);
+
+  /* Presence check */
+
+  ret = vl53l5cx_page(priv, VL53L5CX_PAGE_BOOT);
+  if (ret < 0)
+    {
+      goto errout;
+    }
+
+  ret = vl53l5cx_getreg(priv, VL53L5CX_ID_REG, &id);
+  if (ret < 0)
+    {
+      goto errout;
+    }
+
+  ret = vl53l5cx_getreg(priv, VL53L5CX_REV_REG, &rev);
+  if (ret < 0)
+    {
+      goto errout;
+    }
+
+  ret = vl53l5cx_page(priv, VL53L5CX_PAGE_MAIN);
+  if (ret < 0)
+    {
+      goto errout;
+    }
+
+  if (id != VL53L5CX_ID || rev != VL53L5CX_REV)
+    {
+      verr("ERROR: device ID 0x%02x, revision 0x%02x\n", id, rev);
+      ret = -ENODEV;
+      goto errout;
+    }
+
+  priv->sensor.ops              = &g_vl53l5cx_ops;
+  priv->sensor.fmtdescs         = g_vl53l5cx_fmtdescs;
+  priv->sensor.fmtdescs_num     = nitems(g_vl53l5cx_fmtdescs);
+  priv->sensor.frmsizes         = g_vl53l5cx_frmsizes;
+  priv->sensor.frmsizes_num     = nitems(g_vl53l5cx_frmsizes);
+  priv->sensor.frmintervals     = g_vl53l5cx_frmintervals;
+  priv->sensor.frmintervals_num = nitems(g_vl53l5cx_frmintervals);
+  priv->data.ops                = &g_vl53l5cx_data_ops;
+  priv->data.caps               = IMGDATA_CAP_DROP_FRAME;
+
+  sensor = &priv->sensor;
+  ret = capture_register(devpath, &priv->data, &sensor, 1);
+  if (ret < 0)
+    {
+      goto errout;
+    }
+
+  return OK;
+
+errout:
+  nxmutex_destroy(&priv->lock);
+  kmm_free(priv);
+  return ret;
+}
diff --git a/drivers/video/vl53l5cx/.gitignore 
b/drivers/video/vl53l5cx/.gitignore
new file mode 100644
index 00000000000..4693d08153d
--- /dev/null
+++ b/drivers/video/vl53l5cx/.gitignore
@@ -0,0 +1,2 @@
+/vl53l5cx_buffers.h
+/LICENSE.md
diff --git a/drivers/video/vl53l5cx/platform.h 
b/drivers/video/vl53l5cx/platform.h
new file mode 100644
index 00000000000..67b3f1a8979
--- /dev/null
+++ b/drivers/video/vl53l5cx/platform.h
@@ -0,0 +1,44 @@
+/****************************************************************************
+ * drivers/video/vl53l5cx/platform.h
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+/* Replaces the ST ULD platform.h included by the downloaded
+ * vl53l5cx_buffers.h.
+ */
+
+#ifndef __DRIVERS_VIDEO_VL53L5CX_PLATFORM_H
+#define __DRIVERS_VIDEO_VL53L5CX_PLATFORM_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <stdint.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/* One target per zone */
+
+#define VL53L5CX_NB_TARGET_PER_ZONE 1
+
+#endif /* __DRIVERS_VIDEO_VL53L5CX_PLATFORM_H */
diff --git a/include/nuttx/video/vl53l5cx.h b/include/nuttx/video/vl53l5cx.h
new file mode 100644
index 00000000000..d600ff02e36
--- /dev/null
+++ b/include/nuttx/video/vl53l5cx.h
@@ -0,0 +1,94 @@
+/****************************************************************************
+ * include/nuttx/video/vl53l5cx.h
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+#ifndef __INCLUDE_NUTTX_VIDEO_VL53L5CX_H
+#define __INCLUDE_NUTTX_VIDEO_VL53L5CX_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+
+#include <nuttx/irq.h>
+
+#ifdef CONFIG_VIDEO_VL53L5CX
+
+/****************************************************************************
+ * Public Types
+ ****************************************************************************/
+
+struct i2c_master_s;
+
+/* Board configuration.  INT is open drain and falls when a frame is
+ * ready.
+ */
+
+struct vl53l5cx_config_s
+{
+  uint8_t addr;                       /* 7-bit I2C address, 0x29 */
+
+  /* Attach and enable the INT handler, or disable it with isr NULL */
+
+  CODE int (*attach)(FAR const struct vl53l5cx_config_s *config,
+                     xcpt_t isr, FAR void *arg);
+};
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/****************************************************************************
+ * Name: vl53l5cx_register
+ *
+ * Description:
+ *   Check that a VL53L5CX answers on the bus and register it as a V4L2
+ *   depth capture device (V4L2_PIX_FMT_Z16, 8x8 or 4x4).  The firmware
+ *   and the default configuration are written at the first stream start.
+ *
+ * Input Parameters:
+ *   devpath - The capture device path, e.g. "/dev/video1"
+ *   i2c     - The I2C bus the sensor is on
+ *   config  - The board configuration, kept by the driver
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+int vl53l5cx_register(FAR const char *devpath,
+                      FAR struct i2c_master_s *i2c,
+                      FAR const struct vl53l5cx_config_s *config);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CONFIG_VIDEO_VL53L5CX */
+#endif /* __INCLUDE_NUTTX_VIDEO_VL53L5CX_H */

Reply via email to