[libcamera-devel,v4,3/8] libcamera: v4l2_subdevice: Implement ENUM_FRAME_SIZES

642 diff mbox series

diff --git a/src/libcamera/include/v4l2_subdevice.h b/src/libcamera/include/v4l2_subdevice.h
index dc311034a8ee..827068ec563c 100644
--- a/src/libcamera/include/v4l2_subdevice.h
+++ b/src/libcamera/include/v4l2_subdevice.h
@@ -7,7 +7,9 @@ 
 #ifndef __LIBCAMERA_V4L2_SUBDEVICE_H__
 #define __LIBCAMERA_V4L2_SUBDEVICE_H__
 
+#include <map>
 #include <string>
+#include <vector>
 
 #include "log.h"
 #include "media_object.h"
@@ -39,6 +41,7 @@  public:
 	int setCrop(unsigned int pad, Rectangle *rect);
 	int setCompose(unsigned int pad, Rectangle *rect);
 
+	std::vector<V4L2SubdeviceFormat> &formats(unsigned int pad);
 	int getFormat(unsigned int pad, V4L2SubdeviceFormat *format);
 	int setFormat(unsigned int pad, V4L2SubdeviceFormat *format);
 
@@ -46,11 +49,17 @@  protected:
 	std::string logPrefix() const { return "'" + deviceName() + "'"; }
 
 private:
+	int listPadSizes(unsigned int pad, unsigned int mbus_code,
+			 std::vector<V4L2SubdeviceFormat> *formats);
+	std::vector<V4L2SubdeviceFormat> listPadFormats(unsigned int pad);
+
 	int setSelection(unsigned int pad, unsigned int target,
 			 Rectangle *rect);
 
 	const MediaEntity *entity_;
 	int fd_;
+
+	std::map<unsigned int, std::vector<V4L2SubdeviceFormat>> formats_;
 };
 
 } /* namespace libcamera */
diff --git a/src/libcamera/v4l2_subdevice.cpp b/src/libcamera/v4l2_subdevice.cpp
index dbb54506ee41..b3a5d7a37413 100644
--- a/src/libcamera/v4l2_subdevice.cpp
+++ b/src/libcamera/v4l2_subdevice.cpp
@@ -5,6 +5,10 @@ 
  * v4l2_subdevice.cpp - V4L2 Subdevice
  */
 
+#include <map>
+#include <string>
+#include <vector>
+
 #include <fcntl.h>
 #include <string.h>
 #include <sys/ioctl.h>
@@ -116,6 +120,9 @@  int V4L2Subdevice::open()
 	}
 	fd_ = ret;
 
+	for (MediaPad *pad : entity_->pads())
+		formats_[pad->index()] = listPadFormats(pad->index());
+
 	return 0;
 }
 
@@ -178,6 +185,25 @@  int V4L2Subdevice::setCompose(unsigned int pad, Rectangle *rect)
 	return setSelection(pad, V4L2_SEL_TGT_COMPOSE, rect);
 }
 
+/**
+ * \brief List the sub-device image resolutions and formats on \a pad
+ * \param[in] pad The 0-indexed pad number to enumerate formats on
+ *
+ * \return A vector of image formats, or an empty vector if the pad does not
+ * exist
+ */
+std::vector<V4L2SubdeviceFormat> &V4L2Subdevice::formats(unsigned int pad)
+{
+	/*
+	 * If pad does not exist, return an empty vector at position
+	 * pads().size()
+	 */
+	if (pad > entity_->pads().size())
+		pad = entity_->pads().size();
+
+	return formats_[pad];
+}
+
 /**
  * \brief Retrieve the image format set on one of the V4L2 subdevice pads
  * \param[in] pad The 0-indexed pad number the format is to be retrieved from
@@ -243,6 +269,102 @@  int V4L2Subdevice::setFormat(unsigned int pad, V4L2SubdeviceFormat *format)
 	return 0;
 }
 
+int V4L2Subdevice::listPadSizes(unsigned int pad, unsigned int mbus_code,
+				std::vector<V4L2SubdeviceFormat> *formats)
+{
+	struct v4l2_subdev_frame_size_enum sizeEnum = {};
+	int ret;
+
+	sizeEnum.index = 0;
+	sizeEnum.pad = pad;
+	sizeEnum.code = mbus_code;
+	sizeEnum.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+
+	while (!(ret = ioctl(fd_, VIDIOC_SUBDEV_ENUM_FRAME_SIZE, &sizeEnum))) {
+		V4L2SubdeviceFormat minFormat = {
+			.mbus_code = mbus_code,
+			.width = sizeEnum.min_width,
+			.height = sizeEnum.min_height,
+		};
+		formats->push_back(minFormat);
+
+		/*
+		 * Most subdevices report discrete frame resolutions, where
+		 * min and max sizes are identical. For continue frame
+		 * resolutions, store the min and max sizes interval.
+		 */
+		if (sizeEnum.min_width == sizeEnum.max_width &&
+		    sizeEnum.min_height == sizeEnum.max_height) {
+			sizeEnum.index++;
+			continue;
+		}
+
+		V4L2SubdeviceFormat maxFormat = {
+			.mbus_code = mbus_code,
+			.width = sizeEnum.max_width,
+			.height = sizeEnum.max_height,
+		};
+		formats->push_back(maxFormat);
+
+		sizeEnum.index++;
+	}
+
+	if (ret && (errno != EINVAL && errno != ENOTTY)) {
+		LOG(V4L2Subdev, Error)
+			<< "Unable to enumerate format on pad " << pad
+			<< ": " << strerror(errno);
+		return ret;
+	}
+
+	return 0;
+}
+
+std::vector<V4L2SubdeviceFormat> V4L2Subdevice::listPadFormats(unsigned int pad)
+{
+	struct v4l2_subdev_mbus_code_enum mbusEnum = {};
+	std::vector<V4L2SubdeviceFormat> formats = {};
+	int ret;
+
+	mbusEnum.pad = pad;
+	mbusEnum.index = 0;
+	mbusEnum.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+
+	while (!(ret = ioctl(fd_, VIDIOC_SUBDEV_ENUM_MBUS_CODE, &mbusEnum))) {
+		ret = listPadSizes(pad, mbusEnum.code, &formats);
+		if (ret)
+			return formats;
+
+		mbusEnum.index++;
+	}
+
+	/*
+	 * The subdevice might not support ENUM_MBUS_CODE but might support
+	 * ENUM_FRAME_SIZES. Try with the currently applied format.
+	 */
+	if (ret && errno == ENOTTY) {
+		struct V4L2SubdeviceFormat subdevFormat;
+		if (getFormat(pad, &subdevFormat)) {
+			LOG(V4L2Subdev, Error)
+				<< "Unable to get format on pad " << pad
+				<< ": " << strerror(errno);
+			return formats;
+		}
+
+		ret = listPadSizes(pad, subdevFormat.mbus_code, &formats);
+		if (ret)
+			return formats;
+	}
+
+	if (ret && (errno != EINVAL && errno != ENOTTY)) {
+		LOG(V4L2Subdev, Error)
+			<< "Unable to enumerate format on pad " << pad
+			<< ": " << strerror(errno);
+		return formats;
+	}
+
+	return formats;
+}
+
 int V4L2Subdevice::setSelection(unsigned int pad, unsigned int target,
 				Rectangle *rect)
 {