2 files changed, 138 insertions, 0 deletions

diff --git a/src/libcamera/include/v4l2_device.h b/src/libcamera/include/v4l2_device.h
index c70959e6..f68b2b1d 100644
--- a/src/libcamera/include/v4l2_device.h
+++ b/src/libcamera/include/v4l2_device.h
@@ -86,10 +86,20 @@ public:
 	const char *deviceName() const { return caps_.card(); }
 	const char *busName() const { return caps_.bus_info(); }
 
+	int format(V4L2DeviceFormat *fmt);
+	int setFormat(V4L2DeviceFormat *fmt);
+
 private:
+	int getFormatSingleplane(V4L2DeviceFormat *fmt);
+	int setFormatSingleplane(V4L2DeviceFormat *fmt);
+
+	int getFormatMultiplane(V4L2DeviceFormat *fmt);
+	int setFormatMultiplane(V4L2DeviceFormat *fmt);
+
 	std::string deviceNode_;
 	int fd_;
 	V4L2Capability caps_;
+	enum v4l2_buf_type bufferType_;
 };
 
 } /* namespace libcamera */
diff --git a/src/libcamera/v4l2_device.cpp b/src/libcamera/v4l2_device.cpp
index d6143f2b..5c415d0b 100644
--- a/src/libcamera/v4l2_device.cpp
+++ b/src/libcamera/v4l2_device.cpp
@@ -227,6 +227,15 @@ int V4L2Device::open()
 		return -EINVAL;
 	}
 
+	if (caps_.isCapture())
+		bufferType_ = caps_.isMultiplanar()
+			    ? V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE
+			    : V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	else
+		bufferType_ = caps_.isMultiplanar()
+			    ? V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE
+			    : V4L2_BUF_TYPE_VIDEO_OUTPUT;
+
 	return 0;
 }
 
@@ -269,4 +278,123 @@ void V4L2Device::close()
  * \return The string containing the device location
  */
 
+/**
+ * \brief Retrieve the image format set on the V4L2 device
+ * \return 0 for success, a negative error code otherwise
+ */
+int V4L2Device::format(V4L2DeviceFormat *fmt)
+{
+	return caps_.isMultiplanar() ? getFormatMultiplane(fmt) :
+				       getFormatSingleplane(fmt);
+}
+
+/**
+ * \brief Configure an image format on the V4L2 device
+ * \return 0 for success, a negative error code otherwise
+ */
+int V4L2Device::setFormat(V4L2DeviceFormat *fmt)
+{
+	return caps_.isMultiplanar() ? setFormatMultiplane(fmt) :
+				       setFormatSingleplane(fmt);
+}
+
+int V4L2Device::getFormatSingleplane(V4L2DeviceFormat *fmt)
+{
+	struct v4l2_format v4l2Fmt;
+	struct v4l2_pix_format *pix = &v4l2Fmt.fmt.pix;
+	int ret;
+
+	v4l2Fmt.type = bufferType_;
+	ret = ioctl(fd_, VIDIOC_S_FMT, &v4l2Fmt);
+	if (ret) {
+		ret = -errno;
+		LOG(Error) << "Unable to get format: " << strerror(-ret);
+		return ret;
+	}
+
+	fmt->width = pix->width;
+	fmt->height = pix->height;
+	fmt->fourcc = pix->pixelformat;
+	fmt->planes = 1;
+	fmt->planesFmt[0].bpl = pix->bytesperline;
+	fmt->planesFmt[0].size = pix->sizeimage;
+
+	return 0;
+}
+
+int V4L2Device::setFormatSingleplane(V4L2DeviceFormat *fmt)
+{
+	struct v4l2_format v4l2Fmt;
+	struct v4l2_pix_format *pix = &v4l2Fmt.fmt.pix;
+	int ret;
+
+	v4l2Fmt.type = bufferType_;
+	pix->width = fmt->width;
+	pix->height = fmt->height;
+	pix->pixelformat = fmt->fourcc;
+
+	ret = ioctl(fd_, VIDIOC_S_FMT, &v4l2Fmt);
+	if (ret) {
+		ret = -errno;
+		LOG(Error) << "Unable to set format: " << strerror(-ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+int V4L2Device::getFormatMultiplane(V4L2DeviceFormat *fmt)
+{
+	struct v4l2_format v4l2Fmt;
+	struct v4l2_pix_format_mplane *pix = &v4l2Fmt.fmt.pix_mp;
+	int ret;
+
+	v4l2Fmt.type = bufferType_;
+	ret = ioctl(fd_, VIDIOC_G_FMT, &v4l2Fmt);
+	if (ret) {
+		ret = -errno;
+		LOG(Error) << "Unable to get format: " << strerror(-ret);
+		return ret;
+	}
+
+	fmt->width = pix->width;
+	fmt->height = pix->height;
+	fmt->fourcc = pix->pixelformat;
+	fmt->planes = pix->num_planes;
+
+	for (unsigned int i = 0; i < fmt->planes; ++i) {
+		fmt->planesFmt[i].bpl = pix->plane_fmt[i].bytesperline;
+		fmt->planesFmt[i].size = pix->plane_fmt[i].sizeimage;
+	}
+
+	return 0;
+}
+
+int V4L2Device::setFormatMultiplane(V4L2DeviceFormat *fmt)
+{
+	struct v4l2_format v4l2Fmt;
+	struct v4l2_pix_format_mplane *pix = &v4l2Fmt.fmt.pix_mp;
+	int ret;
+
+	v4l2Fmt.type = bufferType_;
+	pix->width = fmt->width;
+	pix->height = fmt->height;
+	pix->pixelformat = fmt->fourcc;
+	pix->num_planes = fmt->planes;
+
+	for (unsigned int i = 0; i < pix->num_planes; ++i) {
+		pix->plane_fmt[i].bytesperline = fmt->planesFmt[i].bpl;
+		pix->plane_fmt[i].sizeimage = fmt->planesFmt[i].size;
+	}
+
+	ret = ioctl(fd_, VIDIOC_S_FMT, &v4l2Fmt);
+	if (ret) {
+		ret = -errno;
+		LOG(Error) << "Unable to set format: " << strerror(-ret);
+		return ret;
+	}
+
+	return 0;
+}
+
 } /* namespace libcamera */