libcamera: v4l2_videodevice: Match formats supported by the device

Now that V4L2PixelFormat::fromPixelFormat() returns a list of formats
to chose from by checking which ones are actually supported by the video
device.

The first format found to match one of the device supported ones is
returned.

As V4L2VideoDevice::toV4L2PixelFormat() is a const function which uses
other functions of the class, those functions has to be made const as
well.

In particular:

- enumPixelformats() and enumSizes() do not modify the class state and
  can safely be made const.
- formats() uses the above functions and does not itself modify the
  class state and can be made const
- add a const version of V4L2Device::ioctl() to be used by the now const
  functions

Signed-off-by: Jacopo Mondi <jacopo@jmondi.org>

4 files changed, 43 insertions, 10 deletions

diff --git a/include/libcamera/internal/v4l2_device.h b/include/libcamera/internal/v4l2_device.h
index a52a5f2c..f7ec3c70 100644
--- a/include/libcamera/internal/v4l2_device.h
+++ b/include/libcamera/internal/v4l2_device.h
@@ -55,6 +55,7 @@ protected:
 	int setFd(UniqueFD fd);
 
 	int ioctl(unsigned long request, void *argp);
+	int ioctl(unsigned long request, void *argp) const;
 
 	int fd() const { return fd_.get(); }
 
diff --git a/include/libcamera/internal/v4l2_videodevice.h b/include/libcamera/internal/v4l2_videodevice.h
index 29fa0bba..6d8850c9 100644
--- a/include/libcamera/internal/v4l2_videodevice.h
+++ b/include/libcamera/internal/v4l2_videodevice.h
@@ -205,7 +205,7 @@ public:
 	int getFormat(V4L2DeviceFormat *format);
 	int tryFormat(V4L2DeviceFormat *format);
 	int setFormat(V4L2DeviceFormat *format);
-	Formats formats(uint32_t code = 0);
+	Formats formats(uint32_t code = 0) const;
 
 	int setSelection(unsigned int target, Rectangle *rect);
 
@@ -251,8 +251,8 @@ private:
 	int getFormatSingleplane(V4L2DeviceFormat *format);
 	int trySetFormatSingleplane(V4L2DeviceFormat *format, bool set);
 
-	std::vector<V4L2PixelFormat> enumPixelformats(uint32_t code);
-	std::vector<SizeRange> enumSizes(V4L2PixelFormat pixelFormat);
+	std::vector<V4L2PixelFormat> enumPixelformats(uint32_t code) const;
+	std::vector<SizeRange> enumSizes(V4L2PixelFormat pixelFormat) const;
 
 	int requestBuffers(unsigned int count, enum v4l2_memory memoryType);
 	int createBuffers(unsigned int count,
diff --git a/src/libcamera/v4l2_device.cpp b/src/libcamera/v4l2_device.cpp
index 3fc8438f..59f92403 100644
--- a/src/libcamera/v4l2_device.cpp
+++ b/src/libcamera/v4l2_device.cpp
@@ -460,6 +460,21 @@ int V4L2Device::ioctl(unsigned long request, void *argp)
 }
 
 /**
+ * \copydoc ioctl()
+ */
+int V4L2Device::ioctl(unsigned long request, void *argp) const
+{
+	/*
+	 * Printing out an error message is usually better performed
+	 * in the caller, which can provide more context.
+	 */
+	if (::ioctl(fd_.get(), request, argp) < 0)
+		return -errno;
+
+	return 0;
+}
+
+/**
  * \fn V4L2Device::deviceNode()
  * \brief Retrieve the device node path
  * \return The device node path
diff --git a/src/libcamera/v4l2_videodevice.cpp b/src/libcamera/v4l2_videodevice.cpp
index 43c3d0f6..a3242ba7 100644
--- a/src/libcamera/v4l2_videodevice.cpp
+++ b/src/libcamera/v4l2_videodevice.cpp
@@ -1045,7 +1045,7 @@ int V4L2VideoDevice::trySetFormatSingleplane(V4L2DeviceFormat *format, bool set)
  *
  * \return A list of the supported video device formats
  */
-V4L2VideoDevice::Formats V4L2VideoDevice::formats(uint32_t code)
+V4L2VideoDevice::Formats V4L2VideoDevice::formats(uint32_t code) const
 {
 	Formats formats;
 
@@ -1067,7 +1067,7 @@ V4L2VideoDevice::Formats V4L2VideoDevice::formats(uint32_t code)
 	return formats;
 }
 
-std::vector<V4L2PixelFormat> V4L2VideoDevice::enumPixelformats(uint32_t code)
+std::vector<V4L2PixelFormat> V4L2VideoDevice::enumPixelformats(uint32_t code) const
 {
 	std::vector<V4L2PixelFormat> formats;
 	int ret;
@@ -1101,7 +1101,7 @@ std::vector<V4L2PixelFormat> V4L2VideoDevice::enumPixelformats(uint32_t code)
 	return formats;
 }
 
-std::vector<SizeRange> V4L2VideoDevice::enumSizes(V4L2PixelFormat pixelFormat)
+std::vector<SizeRange> V4L2VideoDevice::enumSizes(V4L2PixelFormat pixelFormat) const
 {
 	std::vector<SizeRange> sizes;
 	int ret;
@@ -1990,20 +1990,37 @@ V4L2VideoDevice::fromEntityName(const MediaDevice *media,
 }
 
 /**
- * \brief Convert \a PixelFormat to its corresponding V4L2 FourCC
+ * \brief Convert \a PixelFormat to one of the device supported V4L2 FourCC
  * \param[in] pixelFormat The PixelFormat to convert
  *
+ * Convert a\ pixelformat to a V4L2 FourCC that is known to be supported by
+ * the video device.
+ *
  * For multiplanar formats, the V4L2 format variant (contiguous or
  * non-contiguous planes) is selected automatically based on the capabilities
  * of the video device. If the video device supports the V4L2 multiplanar API,
  * non-contiguous formats are preferred.
  *
- * \return The V4L2_PIX_FMT_* pixel format code corresponding to \a pixelFormat
+ * \return The V4L2PixelFormat corresponding to \a pixelFormat or an invalid
+ * PixelFormat if \a pixelFormat is not supported by the video device
  */
 V4L2PixelFormat V4L2VideoDevice::toV4L2PixelFormat(const PixelFormat &pixelFormat) const
 {
-	return V4L2PixelFormat::fromPixelFormat(pixelFormat,
-						caps_.isMultiplanar())[0];
+	std::vector<V4L2PixelFormat> deviceFormats = enumPixelformats(0);
+	std::vector<V4L2PixelFormat> v4l2PixelFormats =
+		V4L2PixelFormat::fromPixelFormat(pixelFormat, caps_.isMultiplanar());
+
+	for (const V4L2PixelFormat &v4l2Format : v4l2PixelFormats) {
+		auto it = std::find_if(deviceFormats.begin(), deviceFormats.end(),
+				       [&v4l2Format](const V4L2PixelFormat &deviceFormat) {
+					       return v4l2Format == deviceFormat;
+				       });
+
+		if (it != deviceFormats.end())
+			return v4l2Format;
+	}
+
+	return {};
 }
 
 /**