Yifan-Liu1 commented on code in PR #21446:
URL: https://github.com/apache/nuttx/pull/21446#discussion_r4520285345


##########
drivers/usbhost/usbhost_uvc.c:
##########
@@ -0,0 +1,3610 @@
+/****************************************************************************
+ * drivers/usbhost/usbhost_uvc.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.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <debug.h>
+#include <errno.h>
+#include <inttypes.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/param.h>
+#include <sys/time.h>
+#include <sys/videoio.h>
+#include <time.h>
+#include <unistd.h>
+#include <nuttx/arch.h>
+#include <nuttx/atomic.h>
+#include <nuttx/kthread.h>
+#include <nuttx/nuttx.h>
+#include <nuttx/kmalloc.h>
+#include <nuttx/mutex.h>
+#include <nuttx/semaphore.h>
+#include <nuttx/spinlock.h>
+#include <nuttx/video/v4l2_cap.h>
+
+#include "usbhost_uvc.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define imgsensor_to_uvc_dev(s) container_of(s, struct uvc_dev_s, imgsensor)
+
+#define UVC_MAX_DEVNO                  32
+#define UVC_DEFAULT_FPS                30
+#define UVC_FRAME_COMPLETE_THRESHOLD   95  /* percent */
+#define UVC_ISO_WAIT_TIMEOUT_SEC       1
+#define UVC_SLOT_WAIT_TIMEOUT_SEC      2
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+struct uvc_ctrl_map_s
+{
+  uint32_t id;
+  uint8_t  unit_type;
+  uint8_t  selector;
+  uint8_t  size;
+  bool     is_signed;
+  uint8_t  ctrl_bit;
+};
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int uvc_imgdata_init(FAR struct imgdata_s *data);
+static int uvc_imgdata_uninit(FAR struct imgdata_s *data);
+static int uvc_imgdata_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
+uvc_imgdata_validate_frame_setting(FAR struct imgdata_s *data,
+                                   uint8_t nr_datafmts,
+                                   FAR imgdata_format_t *datafmts,
+                                   FAR imgdata_interval_t *interval);
+static int uvc_imgdata_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 uvc_imgdata_stop_capture(FAR struct imgdata_s *data);
+static void uvc_iso_restart(FAR struct uvc_dev_s *dev);
+static bool uvc_imgsensor_is_available(FAR struct imgsensor_s *sensor);
+static int uvc_imgsensor_init(FAR struct imgsensor_s *sensor);
+static int uvc_imgsensor_uninit(FAR struct imgsensor_s *sensor);
+static const char *
+uvc_imgsensor_get_driver_name(FAR struct imgsensor_s *sensor);
+static int
+uvc_imgsensor_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 *interval);
+static int uvc_imgsensor_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 uvc_imgsensor_stop_capture(FAR struct imgsensor_s *sensor,
+                                      imgsensor_stream_type_t type);
+static int
+uvc_imgsensor_get_frame_interval(FAR struct imgsensor_s *sensor,
+                                 imgsensor_stream_type_t type,
+                                 FAR imgsensor_interval_t *interval);
+static int
+uvc_imgsensor_get_supported_value(FAR struct imgsensor_s *sensor,
+                                  uint32_t id,
+                                  FAR imgsensor_supported_value_t *value);
+static int uvc_imgsensor_get_value(FAR struct imgsensor_s *sensor,
+                                   uint32_t id, uint32_t size,
+                                   FAR imgsensor_value_t *value);
+static int uvc_imgsensor_set_value(FAR struct imgsensor_s *sensor,
+                                   uint32_t id, uint32_t size,
+                                   imgsensor_value_t value);
+static FAR struct usbhost_class_s *
+uvc_create(FAR struct usbhost_hubport_s *hport,
+           FAR const struct usbhost_id_s *id);
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+static const struct usbhost_id_s g_uvc_id[1] =
+{
+  {
+    UVC_DEVICE_CLASS,                      /* base     */
+    UVC_SUBCLASS_INTERFACE_COLLECTION,     /* subclass */
+    0,                                     /* protocol */
+    0,                                     /* vid      */
+    0                                      /* pid      */
+  },
+};
+
+static struct usbhost_registry_s g_uvc_registry =
+{
+  NULL,               /* flink  */
+  uvc_create,         /* create */
+  1,                  /* nids   */
+  g_uvc_id            /* id[]   */
+};
+
+static atomic_t g_uvc_devinuse;
+
+static const struct uvc_ctrl_map_s g_uvc_ctrl_map[] =
+{
+  { IMGSENSOR_ID_BRIGHTNESS,
+    UVC_CTRL_UNIT_PU, UVC_PU_BRIGHTNESS_CONTROL,
+    2, true, UVC_PU_BRIGHTNESS_BIT },
+  { IMGSENSOR_ID_CONTRAST,
+    UVC_CTRL_UNIT_PU, UVC_PU_CONTRAST_CONTROL,
+    2, false, UVC_PU_CONTRAST_BIT },
+  { IMGSENSOR_ID_SATURATION,
+    UVC_CTRL_UNIT_PU, UVC_PU_SATURATION_CONTROL,
+    2, false, UVC_PU_SATURATION_BIT },
+  { IMGSENSOR_ID_HUE,
+    UVC_CTRL_UNIT_PU, UVC_PU_HUE_CONTROL,
+    2, true, UVC_PU_HUE_BIT },
+  { IMGSENSOR_ID_AUTO_WHITE_BALANCE,
+    UVC_CTRL_UNIT_PU,
+    UVC_PU_WHITE_BALANCE_TEMPERATURE_AUTO_CONTROL,
+    1, false, UVC_PU_WHITE_BALANCE_TEMPERATURE_AUTO_BIT },
+  { IMGSENSOR_ID_GAMMA,
+    UVC_CTRL_UNIT_PU, UVC_PU_GAMMA_CONTROL,
+    2, false, UVC_PU_GAMMA_BIT },
+  { IMGSENSOR_ID_SHARPNESS,
+    UVC_CTRL_UNIT_PU, UVC_PU_SHARPNESS_CONTROL,
+    2, false, UVC_PU_SHARPNESS_BIT },
+  { IMGSENSOR_ID_EXPOSURE_AUTO,
+    UVC_CTRL_UNIT_CT, UVC_CT_AE_MODE_CONTROL,
+    1, false, UVC_CT_AE_MODE_BIT },
+  { IMGSENSOR_ID_EXPOSURE_ABSOLUTE,
+    UVC_CTRL_UNIT_CT,
+    UVC_CT_EXPOSURE_TIME_ABSOLUTE_CONTROL,
+    4, false, UVC_CT_EXPOSURE_TIME_ABSOLUTE_BIT },
+  { IMGSENSOR_ID_FOCUS_ABSOLUTE,
+    UVC_CTRL_UNIT_CT, UVC_CT_FOCUS_ABSOLUTE_CONTROL,
+    2, false, UVC_CT_FOCUS_ABSOLUTE_BIT },
+  { IMGSENSOR_ID_FOCUS_AUTO,
+    UVC_CTRL_UNIT_CT, UVC_CT_FOCUS_AUTO_CONTROL,
+    1, false, UVC_CT_FOCUS_AUTO_BIT },
+  { IMGSENSOR_ID_ZOOM_ABSOLUTE,
+    UVC_CTRL_UNIT_CT, UVC_CT_ZOOM_ABSOLUTE_CONTROL,
+    2, false, UVC_CT_ZOOM_ABSOLUTE_BIT },
+};
+
+static const struct imgdata_ops_s g_uvc_imgdata_ops =
+{
+  .init                     = uvc_imgdata_init,
+  .uninit                   = uvc_imgdata_uninit,
+  .set_buf                  = uvc_imgdata_set_buf,
+  .validate_frame_setting   = uvc_imgdata_validate_frame_setting,
+  .start_capture            = uvc_imgdata_start_capture,
+  .stop_capture             = uvc_imgdata_stop_capture,
+};
+
+static const struct imgsensor_ops_s g_uvc_imgsensor_ops =
+{
+  .is_available             = uvc_imgsensor_is_available,
+  .init                     = uvc_imgsensor_init,
+  .uninit                   = uvc_imgsensor_uninit,
+  .get_driver_name          = uvc_imgsensor_get_driver_name,
+  .validate_frame_setting   = uvc_imgsensor_validate_frame_setting,
+  .start_capture            = uvc_imgsensor_start_capture,
+  .stop_capture             = uvc_imgsensor_stop_capture,
+  .get_frame_interval       = uvc_imgsensor_get_frame_interval,
+  .get_supported_value      = uvc_imgsensor_get_supported_value,
+  .get_value                = uvc_imgsensor_get_value,
+  .set_value                = uvc_imgsensor_set_value,
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+static inline uint16_t uvc_getle16(FAR const uint8_t *val)
+{
+  return (uint16_t)val[1] << 8 | (uint16_t)val[0];
+}
+
+static inline uint32_t uvc_getle32(FAR const uint8_t *val)
+{
+  return (uint32_t)uvc_getle16(&val[2]) << 16 |
+         (uint32_t)uvc_getle16(val);
+}
+
+static void uvc_putle16(FAR uint8_t *dest, uint16_t val)
+{
+  dest[0] = val & 0xff;
+  dest[1] = val >> 8;
+}
+
+static void uvc_putle32(FAR uint8_t *dest, uint32_t val)
+{
+  dest[0] = val & 0xff;
+  dest[1] = (val >> 8) & 0xff;
+  dest[2] = (val >> 16) & 0xff;
+  dest[3] = (val >> 24) & 0xff;
+}
+
+static FAR const char *uvc_fourcc_to_string(uint32_t fourcc)
+{
+  switch (fourcc)
+    {
+      case V4L2_PIX_FMT_YUYV:
+        return "YUYV";
+
+      case V4L2_PIX_FMT_UYVY:
+        return "UYVY";
+
+      case V4L2_PIX_FMT_NV12:
+        return "NV12";
+
+      case V4L2_PIX_FMT_YVU420:
+        return "YV12";
+
+      case V4L2_PIX_FMT_YUV420:
+        return "I420";
+
+      case V4L2_PIX_FMT_M420:
+        return "M420";
+
+      case V4L2_PIX_FMT_GREY:
+        return "GREY";
+
+      case V4L2_PIX_FMT_RGB565:
+        return "RGB565";
+
+      case V4L2_PIX_FMT_BGR24:
+        return "BGR24";
+
+      case V4L2_PIX_FMT_MJPEG:
+        return "MJPEG";
+
+      case V4L2_PIX_FMT_JPEG:
+        return "JPEG";
+
+      case V4L2_PIX_FMT_H264:
+        return "H264";
+
+      case V4L2_PIX_FMT_HEVC:
+        return "HEVC";
+
+      default:
+        return "UNKNOWN";
+    }
+}
+
+static uint32_t uvc_convert_pixelformat(uint32_t imgdata_format)
+{
+  switch (imgdata_format)
+    {
+      case IMGDATA_PIX_FMT_YUYV:
+        return V4L2_PIX_FMT_YUYV;
+
+      case IMGDATA_PIX_FMT_UYVY:
+        return V4L2_PIX_FMT_UYVY;
+
+      case IMGDATA_PIX_FMT_JPEG:
+        return V4L2_PIX_FMT_JPEG;
+
+      case IMGDATA_PIX_FMT_NV12:
+        return V4L2_PIX_FMT_NV12;
+
+      case IMGDATA_PIX_FMT_YUV420P:
+        return V4L2_PIX_FMT_YUV420;
+
+      case IMGDATA_PIX_FMT_RGB565:
+        return V4L2_PIX_FMT_RGB565;
+
+      default:
+        return imgdata_format;
+    }
+}
+
+static bool uvc_is_compressed_format(uint32_t fourcc)
+{
+  return fourcc == V4L2_PIX_FMT_JPEG ||
+         fourcc == V4L2_PIX_FMT_MJPEG ||
+         fourcc == V4L2_PIX_FMT_H264 ||
+         fourcc == V4L2_PIX_FMT_HEVC;
+}
+
+static int uvc_alloc_devno(void)
+{
+  int devno;
+
+  for (devno = 0; devno < UVC_MAX_DEVNO; devno++)
+    {
+      uint32_t bitno = 1 << devno;
+
+      if (!(atomic_or(&g_uvc_devinuse, bitno) & bitno))
+        {
+          return devno;
+        }
+    }
+
+  return -ENOMEM;
+}
+
+static void uvc_free_devno(int devno)
+{
+  if (devno >= 0 && devno < UVC_MAX_DEVNO)
+    {
+      atomic_and(&g_uvc_devinuse, ~(1 << devno));
+    }
+}
+
+/****************************************************************************
+ * Name: uvc_priv_put
+ *
+ * Description:
+ *   Drop one reference to the class instance, freeing it and the device it
+ *   owns once the last holder is gone.
+ *
+ * Input Parameters:
+ *   priv - The class instance to release a reference on
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+static void uvc_priv_put(FAR struct uvc_priv_s *priv)
+{
+  if (atomic_sub(&priv->crefs, 1) != 1)
+    {
+      return;
+    }
+
+  if (priv->dev != NULL)
+    {
+      kmm_free(priv->dev);
+    }
+
+  kmm_free(priv);
+}
+
+static void uvc_init_ctrlreq(FAR struct usb_ctrlreq_s *req,
+                             uint8_t type, uint8_t req_code,
+                             uint16_t value, uint16_t index,
+                             uint16_t len)
+{
+  memset(req, 0, sizeof(*req));
+  req->type = type;
+  req->req  = req_code;
+  uvc_putle16(req->value, value);
+  uvc_putle16(req->index, index);
+  uvc_putle16(req->len, len);
+}
+
+/* Only the low 4 bytes of bmControls are kept; every control in
+ * g_uvc_ctrl_map lives within the first 32 bits. avail bounds the read
+ * against the remaining descriptor length.
+ */
+
+static uint32_t uvc_read_bmcontrols(FAR const uint8_t *p, int n, int avail)
+{
+  uint32_t bm = 0;
+  int      i;
+
+  if (n > avail)
+    {
+      n = avail;
+    }
+
+  for (i = 0; i < n && i < 4; i++)
+    {
+      bm |= (uint32_t)p[i] << (8 * i);
+    }
+
+  return bm;
+}
+
+static bool uvc_ctrl_present(FAR struct uvc_priv_s *priv,
+                             FAR const struct uvc_ctrl_map_s *map)
+{
+  uint32_t bm = map->unit_type == UVC_CTRL_UNIT_CT ?
+                priv->ct_bmcontrols : priv->pu_bmcontrols;
+
+  return (bm >> map->ctrl_bit) & 1;
+}
+
+static FAR const struct uvc_ctrl_map_s *
+uvc_find_ctrl_map(uint32_t id, FAR int *idx)
+{
+  uint8_t i;
+
+  for (i = 0; i < nitems(g_uvc_ctrl_map); i++)
+    {
+      if (g_uvc_ctrl_map[i].id == id)
+        {
+          if (idx)
+            {
+              *idx = i;
+            }
+
+          return &g_uvc_ctrl_map[i];
+        }
+    }
+
+  return NULL;
+}
+
+static int uvc_ctrl_query(FAR struct uvc_dev_s *dev,
+                          FAR const struct uvc_ctrl_map_s *map,
+                          uint8_t request,
+                          FAR void *data)
+{
+  FAR struct uvc_priv_s        *priv = dev->device;
+  FAR struct usbhost_hubport_s *hport = priv->usbclass.hport;
+  struct usb_ctrlreq_s          req;
+  uint8_t                       unit_id;
+  uint8_t                       reqtype;
+
+  /* The cleanup worker hands the pipes back as soon as the device is gone,
+   * so a control transfer from here on can only fail in the lower half.
+   */
+
+  if (atomic_read(&priv->disconnected))
+    {
+      return -ENODEV;
+    }
+
+  unit_id = map->unit_type == UVC_CTRL_UNIT_CT ?
+    priv->ct_id : priv->pu_id;
+
+  if (request & USB_DIR_IN)
+    {
+      reqtype = USB_REQ_DIR_IN |
+                USB_REQ_TYPE_CLASS |
+                USB_REQ_RECIPIENT_INTERFACE;
+    }
+  else
+    {
+      reqtype = USB_REQ_DIR_OUT |
+                USB_REQ_TYPE_CLASS |
+                USB_REQ_RECIPIENT_INTERFACE;
+    }
+
+  uvc_init_ctrlreq(&req, reqtype, request,
+                   map->selector << 8,
+                   unit_id << 8 | priv->ctrl_iface,
+                   map->size);
+
+  if (request & USB_DIR_IN)
+    {
+      return DRVR_CTRLIN(hport->drvr, hport->ep0, &req, data);
+    }
+
+  return DRVR_CTRLOUT(hport->drvr, hport->ep0, &req, data);
+}
+
+static int32_t uvc_ctrl_read_value(FAR const struct uvc_ctrl_map_s *map,
+                                   FAR const uint8_t *data)
+{
+  int32_t val = 0;
+
+  switch (map->size)
+    {
+      case 1:
+        val = data[0];
+        if (map->is_signed && (val & 0x80))
+          {
+            val |= 0xffffff00;
+          }
+
+        break;
+
+      case 2:
+        val = uvc_getle16(data);
+        if (map->is_signed && (val & 0x8000))
+          {
+            val |= 0xffff0000;
+          }
+
+        break;
+
+      case 4:
+        val = uvc_getle32(data);
+        break;
+    }
+
+  return val;
+}
+
+static void uvc_ctrl_write_value(FAR const struct uvc_ctrl_map_s *map,
+                                 FAR uint8_t *data, int32_t val)
+{
+  switch (map->size)
+    {
+      case 1:
+        data[0] = (uint8_t)(val & 0xff);
+        break;
+
+      case 2:
+        uvc_putle16(data, (uint16_t)(val & 0xffff));
+        break;
+
+      case 4:
+        uvc_putle32(data, (uint32_t)val);
+        break;
+    }
+}
+
+static uint32_t uvc_calculate_fps(FAR const imgdata_interval_t *interval,
+                                  uint32_t default_interval)
+{
+  if (interval && interval->denominator > 0 && interval->numerator > 0)
+    {
+      return interval->denominator / interval->numerator;
+    }
+
+  return default_interval > 0 ? (10000000 / default_interval) : 30;
+}
+
+/* Clamp requested interval to nearest supported value from
+ * frame descriptor's discrete interval list.
+ * Returns the clamped interval in 100ns units.
+ */
+
+static uint32_t
+uvc_try_frame_interval(FAR const struct uvc_frame_info_s *frame,
+                       uint32_t requested_interval)
+{
+  uint32_t best;
+  uint32_t best_diff;
+  uint8_t  i;
+
+  if (frame->num_interval == 0)
+    {
+      /* Continuous: return requested interval clamped to descriptor range */
+
+      return requested_interval;
+    }
+
+  /* Find nearest supported interval (minimum
+   * absolute difference). Works regardless of
+   * whether the list is sorted.
+   */
+
+  best = frame->intervals[0];
+  best_diff = abs((int32_t)(requested_interval - best));
+
+  if (best_diff == 0)
+    {
+      return best;
+    }
+
+  for (i = 1; i < frame->num_interval; i++)
+    {
+      uint32_t diff = abs((int32_t)(requested_interval -
+                                    frame->intervals[i]));
+
+      if (diff == 0)
+        {
+          best = frame->intervals[i];
+          break;
+        }
+
+      if (diff < best_diff)
+        {
+          best_diff = diff;
+          best = frame->intervals[i];
+        }
+    }
+
+  uinfo("UVC: clamp interval %" PRIu32 " -> %" PRIu32 "\n",
+        requested_interval, best);
+  return best;
+}
+
+static int uvc_execute_probe_commit(FAR struct usbhost_hubport_s *hport,
+                                    uint8_t vs_intf, uint8_t format_idx,
+                                    uint8_t frame_idx, uint32_t fps,
+                                    FAR uint8_t *ctrlreq)
+{
+  struct usb_ctrlreq_s req;
+  uint32_t             interval_100ns = fps > 0 ? (10000000u / fps) : 0;
+  int                  ret;
+
+  uinfo("UVC: probe fmt=%d frm=%d fps=%" PRIu32
+        " intv=%" PRIu32 "\n",
+        format_idx, frame_idx, fps, interval_100ns);
+
+  /* Build probe data from scratch instead of GET_CUR.
+   * Some devices STALL GET_CUR when no prior SET_CUR has been issued
+   * or when the VS interface is at alt-setting 0.
+   * Per UVC spec, the host may SET_CUR with desired fields and zeros
+   * elsewhere; the device negotiates and returns the result via GET_CUR.
+   */
+
+  memset(ctrlreq, 0, UVC_PROBE_COMMIT_SIZE);
+
+  /* Byte 2: bFormatIndex, Byte 3: bFrameIndex */
+
+  ctrlreq[2] = format_idx;
+  ctrlreq[3] = frame_idx;
+
+  /* Bytes 4-7: dwFrameInterval (100ns units) */
+
+  if (interval_100ns > 0)
+    {
+      uvc_putle32(&ctrlreq[4], interval_100ns);
+    }
+
+  /* VS_PROBE_CONTROL SET_CUR */
+
+  uvc_init_ctrlreq(&req,
+                   USB_REQ_DIR_OUT |
+                   USB_REQ_TYPE_CLASS |
+                   USB_REQ_RECIPIENT_INTERFACE,
+                   UVC_REQUEST_SET_CUR,
+                   0x0100, vs_intf, UVC_PROBE_COMMIT_SIZE);
+
+  ret = DRVR_CTRLOUT(hport->drvr, hport->ep0,
+                     (FAR const struct usb_ctrlreq_s *)&req, ctrlreq);
+  if (ret < 0)
+    {
+      uerr("ERROR: VS_PROBE_CONTROL SET_CUR failed: %d\n", ret);
+      return ret;
+    }
+
+  /* VS_PROBE_CONTROL GET_CUR -- read back negotiated parameters */
+
+  uvc_init_ctrlreq(&req,
+                   USB_REQ_DIR_IN |
+                   USB_REQ_TYPE_CLASS |
+                   USB_REQ_RECIPIENT_INTERFACE,
+                   UVC_REQUEST_GET_CUR,
+                   0x0100, vs_intf, UVC_PROBE_COMMIT_SIZE);
+
+  ret = DRVR_CTRLIN(hport->drvr, hport->ep0,
+                    (FAR const struct usb_ctrlreq_s *)&req,
+                    ctrlreq);
+  if (ret < 0)
+    {
+      uerr("ERROR: VS_PROBE_CONTROL GET_CUR failed: %d\n", ret);
+      return ret;
+    }
+
+  if (ctrlreq[2] != format_idx || ctrlreq[3] != frame_idx)
+    {
+      uerr("ERROR: Probe mismatch: want %d/%d got %d/%d\n",
+           format_idx, frame_idx, ctrlreq[2], ctrlreq[3]);
+      return -EINVAL;
+    }
+
+  uinfo("UVC: probe verified OK\n");
+
+  /* VS_COMMIT_CONTROL SET_CUR -- commit the probed parameters.
+   * The probe GET_CUR result already contains the negotiated
+   * format/frame/interval, use it directly as commit data.
+   */
+
+  uvc_init_ctrlreq(&req,
+                   USB_REQ_DIR_OUT |
+                   USB_REQ_TYPE_CLASS |
+                   USB_REQ_RECIPIENT_INTERFACE,
+                   UVC_REQUEST_SET_CUR,
+                   0x0200, vs_intf, UVC_PROBE_COMMIT_SIZE);
+
+  ret = DRVR_CTRLOUT(hport->drvr, hport->ep0,
+                     (FAR const struct usb_ctrlreq_s *)&req, ctrlreq);
+  if (ret < 0)
+    {
+      uerr("ERROR: VS_COMMIT_CONTROL SET_CUR failed: %d\n", ret);
+      return ret;
+    }
+
+  uinfo("UVC: commit OK\n");
+
+  return OK;
+}
+
+static int uvc_set_interface_alt(FAR struct usbhost_hubport_s *hport,
+                                 FAR struct uvc_priv_s *priv,
+                                 FAR uint8_t *ctrlreq)
+{
+  struct usbhost_epdesc_s ep_desc;
+  struct usb_ctrlreq_s    req;
+  uint32_t                required_payload = uvc_getle32(&ctrlreq[22]);
+  uint16_t                best_payload;
+  uint16_t                maxp;
+  uint8_t                 desired_alt      = 1;
+  uint8_t                 i;
+  int                     ret;
+
+  uinfo("UVC: required payload=%" PRIu32 "\n", required_payload);
+
+  /* Find best alternate setting */
+
+  if (priv->stream_alt_count > 0)
+    {
+      FAR struct uvc_stream_alt_info_s *ai;
+      uint16_t payload;
+
+      best_payload = 0xffff;
+      for (i = 0; i < priv->stream_alt_count; i++)
+        {
+          ai = &priv->stream_alts[i];
+          payload = ai->max_payload;
+
+          if (payload >= required_payload && payload < best_payload)
+            {
+              best_payload = payload;
+              desired_alt  = ai->alt;
+            }
+        }
+
+      if (best_payload == 0xffff)
+        {
+          /* Fallback to maximum payload */
+
+          maxp = 0;
+          for (i = 0; i < priv->stream_alt_count; i++)
+            {
+              if (priv->stream_alts[i].max_payload > maxp)
+                {
+                  maxp        = priv->stream_alts[i].max_payload;
+                  desired_alt = priv->stream_alts[i].alt;
+                }
+            }
+
+          uwarn("UVC: no alt fits, using max alt=%d payload=%u\n",
+                desired_alt, maxp);
+        }
+      else
+        {
+          uinfo("UVC: selected alt=%d payload=%u\n",
+                desired_alt, best_payload);
+        }
+    }
+
+  /* Set interface alternate setting */
+
+  uvc_init_ctrlreq(&req,
+                   USB_REQ_DIR_OUT |
+                   USB_REQ_TYPE_STANDARD |
+                   USB_REQ_RECIPIENT_INTERFACE,
+                   USB_REQ_SETINTERFACE,
+                   desired_alt,
+                   priv->stream_iface, 0);
+
+  ret = DRVR_CTRLOUT(hport->drvr, hport->ep0,
+                     (FAR const struct usb_ctrlreq_s *)&req, NULL);
+  if (ret < 0)
+    {
+      uerr("ERROR: SET_INTERFACE failed: %d\n", ret);
+      return ret;
+    }
+
+  /* Update device state */
+
+  priv->stream_altsetting = desired_alt;
+
+  if (priv->stream_alt_count > 0)
+    {
+      for (i = 0; i < priv->stream_alt_count; i++)
+        {
+          if (priv->stream_alts[i].alt == desired_alt)
+            {
+              priv->max_pkt_size = priv->stream_alts[i].max_payload;
+              uinfo("UVC: max_pkt=%u alt=%u\n",
+                    priv->max_pkt_size, desired_alt);
+
+              /* Re-allocate ISO endpoint with the correct max packet
+               * size for this alt setting. epinfo->maxpacket was set
+               * during initial epalloc and may not match the current alt.
+               */
+
+              if (priv->isoin)
+                {
+                  ep_desc.hport         = hport;
+                  ep_desc.addr          = priv->isoc_ep_addr;
+                  ep_desc.in            = 1;
+                  ep_desc.xfrtype       = USB_EP_ATTR_XFER_ISOC;
+                  ep_desc.interval      = priv->stream_alts[i].interval;
+                  ep_desc.mxpacketsize  =
+                    priv->stream_alts[i].raw_mxpacketsize;
+
+                  DRVR_EPFREE(hport->drvr, priv->isoin);
+                  priv->isoin = NULL;
+
+                  ret = DRVR_EPALLOC(hport->drvr, &ep_desc, &priv->isoin);
+                  if (ret < 0)
+                    {
+                      uerr("ERROR: ISO ep realloc: %d\n", ret);
+                      return ret;
+                    }
+                }
+
+              break;
+            }
+        }
+    }
+
+  /* Verify interface setting */
+
+  uvc_init_ctrlreq(&req,
+                   USB_REQ_DIR_IN |
+                   USB_REQ_TYPE_STANDARD |
+                   USB_REQ_RECIPIENT_INTERFACE,
+                   USB_REQ_GETINTERFACE,
+                   0, priv->stream_iface, 1);
+
+  ret = DRVR_CTRLIN(hport->drvr, hport->ep0,
+                    (FAR const struct usb_ctrlreq_s *)&req, ctrlreq);
+  if (ret < 0)
+    {
+      uerr("ERROR: GET_INTERFACE failed: %d\n", ret);
+      return ret;
+    }
+
+  if (ctrlreq[0] != desired_alt)
+    {
+      uerr("ERROR: alt mismatch: want %d got %d\n", desired_alt, ctrlreq[0]);
+      return -EIO;
+    }
+
+  return OK;
+}
+
+static int uvc_set_format(FAR struct uvc_priv_s *priv,
+                          uint8_t vs_intf, uint8_t format_idx,
+                          uint8_t frame_idx,
+                          FAR const imgdata_interval_t *interval)
+{
+  FAR struct uvc_frame_info_s  *frame            = NULL;
+  FAR struct usbhost_hubport_s *hport            = priv->usbclass.hport;
+  FAR uint8_t                  *ctrlreq;
+  uint32_t                      default_interval = 0;
+  uint32_t                      requested_intv;
+  uint32_t                      clamped_intv;
+  uint32_t                      fps;
+  size_t                        maxlen;
+  uint8_t                       array_idx;
+  int                           ret;
+
+  uinfo("UVC: set_format vs=%d fmt=%d frm=%d\n",
+        vs_intf, format_idx, frame_idx);
+
+  /* Allocate control request buffer */
+
+  ret = DRVR_ALLOC(hport->drvr, &ctrlreq, &maxlen);
+  if (ret < 0)
+    {
+      uerr("ERROR: DRVR_ALLOC ctrlreq: %d\n", ret);
+      return ret;
+    }
+
+  if (maxlen < UVC_PROBE_COMMIT_SIZE)
+    {
+      uerr("ERROR: DRVR_ALLOC buffer too small: %zu < %d\n",
+           maxlen, UVC_PROBE_COMMIT_SIZE);
+      DRVR_FREE(hport->drvr, ctrlreq);
+      return -ENOMEM;
+    }
+
+  /* Calculate frame rate with interval clamping.
+   * format_idx is 1-based UVC index; find matching
+   * 0-based array entry for frame descriptor lookup.
+   */
+
+  for (array_idx = 0; array_idx < priv->format_list.num_formats; array_idx++)
+    {
+      if (priv->format_list.formats[array_idx].format_index == format_idx)
+        {
+          if (frame_idx > 0 &&
+              frame_idx <= priv->format_list.formats[array_idx].num_frames)
+            {
+              frame =
+                &priv->format_list.formats[array_idx].frames[frame_idx - 1];
+              default_interval = frame->interval;
+            }
+
+          break;
+        }
+    }
+
+  fps = uvc_calculate_fps(interval, default_interval);
+
+  /* Clamp interval to device-supported discrete values */
+
+  if (frame)
+    {
+      uinfo("UVC: frame num_interval=%d intv=%" PRIu32 "\n",
+            frame->num_interval, frame->interval);
+    }
+
+  if (frame && frame->num_interval > 0 && fps > 0)
+    {
+      requested_intv = 10000000u / fps;
+      clamped_intv = uvc_try_frame_interval(frame, requested_intv);
+
+      uinfo("UVC: requested_intv=%" PRIu32 " clamped=%" PRIu32 "\n",
+            requested_intv, clamped_intv);
+
+      if (clamped_intv != requested_intv)
+        {
+          fps = 10000000u / clamped_intv;
+          uinfo("UVC: clamped fps to %" PRIu32 "\n", fps);
+        }
+    }
+
+  /* Execute probe and commit sequence */
+
+  ret = uvc_execute_probe_commit(hport, vs_intf, format_idx, frame_idx, fps,
+                                 ctrlreq);
+  if (ret == OK)
+    {
+      /* Set interface alternate setting */
+
+      ret = uvc_set_interface_alt(hport, priv, ctrlreq);
+    }
+
+  DRVR_FREE(hport->drvr, ctrlreq);
+  return ret;
+}
+
+static int uvc_find_format_match(FAR struct uvc_dev_s *dev,
+                                 uint32_t width, uint32_t height,
+                                 uint32_t pixelformat, int *frame_idx)
+{
+  FAR struct uvc_priv_s *priv = dev->device;
+  uint8_t                i;
+  uint8_t                j;
+
+  if (atomic_read(&priv->disconnected))
+    {
+      return -ENODEV;
+    }
+
+  uinfo("UVC: find format %" PRIu32 "x%" PRIu32
+        " pix=0x%08" PRIx32 "\n",
+        width, height, pixelformat);
+
+  /* Only exact match (resolution + pixel format) - no fallback to avoid
+   * corruption
+   */
+
+  for (i = 0; i < priv->format_list.num_formats; i++)
+    {
+      struct uvc_format_info_s *fmt = &priv->format_list.formats[i];
+
+      if (fmt->pixel_format_fourcc == pixelformat)
+        {
+          for (j = 0; j < fmt->num_frames; j++)
+            {
+              struct uvc_frame_info_s *frame = &fmt->frames[j];
+
+              if (frame->width == width && frame->height == height)
+                {
+                  *frame_idx = j;   /* Set frame index */
+
+                  uinfo("UVC: match fmt=%d frm=%d\n", i, j);
+                  return i;   /* Return format index */
+                }
+            }
+        }
+    }
+
+  uerr("UVC: no match for %" PRIu32 "x%" PRIu32
+       " 0x%08" PRIx32 " (%s)\n",
+       width, height, pixelformat, uvc_fourcc_to_string(pixelformat));
+  for (i = 0; i < priv->format_list.num_formats; i++)
+    {
+      struct uvc_format_info_s *fmt = &priv->format_list.formats[i];
+
+      for (j = 0; j < fmt->num_frames; j++)
+        {
+          struct uvc_frame_info_s *frame = &fmt->frames[j];
+
+          uerr("  fmt%d: %dx%d %s\n", i, frame->width, frame->height,
+               uvc_fourcc_to_string(fmt->pixel_format_fourcc));
+        }
+    }
+
+  return -ENOENT;
+}
+
+static int uvc_validate_format_params(FAR struct uvc_priv_s *priv,
+                                      uint8_t format_idx, uint8_t frame_idx)
+{
+  if (format_idx >= priv->format_list.num_formats)
+    {
+      uerr("UVC: bad format_idx %d >= %d\n",
+           format_idx, priv->format_list.num_formats);
+      return -EINVAL;
+    }
+
+  struct uvc_format_info_s *fmt = &priv->format_list.formats[format_idx];
+
+  if (frame_idx >= fmt->num_frames)
+    {
+      uerr("UVC: bad frame_idx %d >= %d\n", frame_idx, fmt->num_frames);
+      return -EINVAL;
+    }
+
+  return OK;
+}
+
+static int uvc_applyformat(FAR struct uvc_dev_s *dev,
+                           int format_idx, int frame_idx,
+                           FAR const imgdata_interval_t *req_inter)
+{
+  FAR struct uvc_priv_s        *priv = dev->device;
+  FAR struct uvc_format_info_s *fmt;
+  FAR struct uvc_frame_info_s  *frame;
+  int                           ret;
+
+  /* Validate format parameters */
+
+  ret = uvc_validate_format_params(priv, format_idx, frame_idx);
+  if (ret != OK)
+    {
+      return ret;
+    }
+
+  fmt   = &priv->format_list.formats[format_idx];
+  frame = &fmt->frames[frame_idx];
+
+  /* Apply the format with requested frame rate */
+
+  ret = uvc_set_format(priv, priv->stream_iface, fmt->format_index,
+                       frame->frame_index, req_inter);
+  if (ret != OK)
+    {
+      uerr("UVC: set format failed: %d\n", ret);
+
+      /* Fallback: Try with format default frame rate */
+
+      if (req_inter && req_inter->denominator > 0)
+        {
+          uint32_t format_fps = uvc_calculate_fps(NULL, frame->interval);
+
+          uinfo("UVC: fallback to default %" PRIu32 " fps\n",
+                format_fps);
+
+          ret = uvc_set_format(priv, priv->stream_iface,
+                               fmt->format_index, frame->frame_index, NULL);
+          if (ret != OK)
+            {
+              uerr("UVC: default fps also failed: %d\n", ret);
+              return ret;
+            }
+        }
+      else
+        {
+          return ret;
+        }
+    }
+
+  dev->current_format_idx = format_idx;
+  dev->current_frame_idx  = frame_idx;
+
+  uinfo("UVC: applied fmt=%d frm=%d\n",
+        fmt->format_index, frame->frame_index);
+
+  return OK;
+}
+
+static void uvc_free_formats(FAR struct uvc_dev_s *dev)
+{
+  if (dev->fmtdescs)
+    {
+      kmm_free(dev->fmtdescs);
+      dev->fmtdescs = NULL;
+    }
+
+  if (dev->frmsizes)
+    {
+      kmm_free(dev->frmsizes);
+      dev->frmsizes = NULL;
+    }
+
+  if (dev->frmintervals)
+    {
+      kmm_free(dev->frmintervals);
+      dev->frmintervals = NULL;
+    }
+
+  dev->imgsensor.fmtdescs      = NULL;
+  dev->imgsensor.frmsizes      = NULL;
+  dev->imgsensor.frmintervals  = NULL;
+}
+
+static int uvc_imgsensor_init(FAR struct imgsensor_s *sensor)
+{
+  FAR struct uvc_dev_s   *dev  = imgsensor_to_uvc_dev(sensor);
+  FAR struct uvc_priv_s  *priv = dev->device;
+  FAR const char         *desc;
+  uint8_t                 num_formats;
+  int                     total_frames = 0;
+  uint8_t                 fmt_idx      = 0;
+  uint8_t                 size_idx     = 0;
+  uint8_t                 interval_idx = 0;
+  uint8_t                 i;
+  uint8_t                 j;
+
+  if (atomic_read(&priv->disconnected))
+    {
+      return -ENODEV;
+    }
+
+  num_formats = priv->format_list.num_formats;
+
+  uinfo("UVC: setup imgsensor from %d formats\n", num_formats);
+
+  /* Free any previous allocation */
+
+  uvc_free_formats(dev);
+
+  /* Count total frames across all formats */
+
+  for (i = 0; i < num_formats; i++)
+    {
+      total_frames += priv->format_list.formats[i].num_frames;
+    }
+
+  /* Allocate per-device format arrays */
+
+  dev->fmtdescs = kmm_zalloc(num_formats * sizeof(struct v4l2_fmtdesc));
+  dev->frmsizes = kmm_zalloc(total_frames * sizeof(struct v4l2_frmsizeenum));
+  dev->frmintervals =
+    kmm_zalloc(total_frames * sizeof(struct v4l2_frmivalenum));
+
+  if (!dev->fmtdescs || !dev->frmsizes || !dev->frmintervals)
+    {
+      uerr("UVC: format alloc failed\n");
+      uvc_free_formats(dev);
+      return -ENOMEM;
+    }
+
+  for (i = 0; i < num_formats; i++)
+    {
+      FAR struct uvc_format_info_s *fmt = &priv->format_list.formats[i];
+
+      dev->fmtdescs[fmt_idx].index       = fmt_idx;
+      dev->fmtdescs[fmt_idx].type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+      dev->fmtdescs[fmt_idx].flags       = 0;
+      dev->fmtdescs[fmt_idx].pixelformat = fmt->pixel_format_fourcc;
+
+      desc = uvc_fourcc_to_string(fmt->pixel_format_fourcc);
+      strlcpy(dev->fmtdescs[fmt_idx].description, desc,
+              sizeof(dev->fmtdescs[fmt_idx].description));
+
+      for (j = 0; j < fmt->num_frames; j++)
+        {
+          FAR struct uvc_frame_info_s *frame = &fmt->frames[j];
+
+          dev->frmsizes[size_idx].index = size_idx;
+          dev->frmsizes[size_idx].type = V4L2_FRMSIZE_TYPE_DISCRETE;
+          dev->frmsizes[size_idx].pixel_format =
+            fmt->pixel_format_fourcc;
+          dev->frmsizes[size_idx].discrete.width = frame->width;
+          dev->frmsizes[size_idx].discrete.height = frame->height;
+
+          dev->frmintervals[interval_idx].index = interval_idx;
+          dev->frmintervals[interval_idx].pixel_format =
+            fmt->pixel_format_fourcc;
+          dev->frmintervals[interval_idx].width = frame->width;
+          dev->frmintervals[interval_idx].height = frame->height;
+          dev->frmintervals[interval_idx].type = V4L2_FRMIVAL_TYPE_DISCRETE;
+
+          if (frame->interval > 0)
+            {
+              dev->frmintervals[interval_idx].discrete.numerator = 1;
+              dev->frmintervals[interval_idx].discrete.denominator =
+                10000000 / frame->interval;
+            }
+          else
+            {
+              dev->frmintervals[interval_idx].discrete.numerator = 1;
+              dev->frmintervals[interval_idx].discrete.denominator =
+                UVC_DEFAULT_FPS;
+            }
+
+          size_idx++;
+          interval_idx++;
+        }
+
+      fmt_idx++;
+    }
+
+  sensor->fmtdescs_num     = num_formats;
+  sensor->fmtdescs         = dev->fmtdescs;
+  sensor->frmsizes_num     = size_idx;
+  sensor->frmsizes         = dev->frmsizes;
+  sensor->frmintervals_num = interval_idx;
+  sensor->frmintervals     = dev->frmintervals;
+
+  uinfo("UVC: %d fmts, %d sizes, %d intervals\n",
+        sensor->fmtdescs_num,

Review Comment:
   Right, three size_t values were printed with %d. Changed to %zu (0fd929f); 
it builds without the -Wformat warning on the 64-bit simulator now.



-- 
This is an automated message from the Apache Git Service.
To respond to the message, please log on to GitHub and use the
URL above to go to the specific comment.

To unsubscribe, e-mail: [email protected]

For queries about this service, please contact Infrastructure at:
[email protected]

Reply via email to