v4l2: v4l2_camera_proxy: Implement VIDIOC_G/S_PRIORITY

Implement VIDIOC_G_PRIORITY and VIDIOC_S_PRIORITY. The behaviour
documented in the V4L2 specification doesn't match the implementation in
the Linux kernel, implement the latter.

Signed-off-by: Paul Elder <paul.elder@ideasonboard.com>
Reviewed-by: Laurent Pinchart <laurent.pinchart@ideasonboard.com>

2 files changed, 66 insertions, 1 deletions

diff --git a/src/v4l2/v4l2_camera_proxy.cpp b/src/v4l2/v4l2_camera_proxy.cpp
index a87a1900..e5b97535 100644
--- a/src/v4l2/v4l2_camera_proxy.cpp
+++ b/src/v4l2/v4l2_camera_proxy.cpp
@@ -45,8 +45,10 @@ int V4L2CameraProxy::open(V4L2CameraFile *file)
 {
 	LOG(V4L2Compat, Debug) << "Servicing open fd = " << file->efd();
 
-	if (refcount_++)
+	if (refcount_++) {
+		files_.insert(file);
 		return 0;
+	}
 
 	/*
 	 * We open the camera here, once, and keep it open until the last
@@ -68,6 +70,8 @@ int V4L2CameraProxy::open(V4L2CameraFile *file)
 	setFmtFromConfig(streamConfig_);
 	sizeimage_ = calculateSizeImage(streamConfig_);
 
+	files_.insert(file);
+
 	return 0;
 }
 
@@ -75,6 +79,8 @@ void V4L2CameraProxy::close(V4L2CameraFile *file)
 {
 	LOG(V4L2Compat, Debug) << "Servicing close fd = " << file->efd();
 
+	files_.erase(file);
+
 	release(file);
 
 	if (--refcount_ > 0)
@@ -304,6 +310,9 @@ int V4L2CameraProxy::vidioc_s_fmt(V4L2CameraFile *file, struct v4l2_format *arg)
 	if (!validateBufferType(arg->type))
 		return -EINVAL;
 
+	if (file->priority() < maxPriority())
+		return -EBUSY;
+
 	int ret = acquire(file);
 	if (ret < 0)
 		return ret;
@@ -340,6 +349,40 @@ int V4L2CameraProxy::vidioc_try_fmt(V4L2CameraFile *file, struct v4l2_format *ar
 	return 0;
 }
 
+enum v4l2_priority V4L2CameraProxy::maxPriority()
+{
+	auto max = std::max_element(files_.begin(), files_.end(),
+				    [](const V4L2CameraFile *a, const V4L2CameraFile *b) {
+					    return a->priority() < b->priority();
+				    });
+	return max != files_.end() ? (*max)->priority() : V4L2_PRIORITY_UNSET;
+}
+
+int V4L2CameraProxy::vidioc_g_priority(V4L2CameraFile *file, enum v4l2_priority *arg)
+{
+	LOG(V4L2Compat, Debug) << "Servicing vidioc_g_priority fd = " << file->efd();
+
+	*arg = maxPriority();
+
+	return 0;
+}
+
+int V4L2CameraProxy::vidioc_s_priority(V4L2CameraFile *file, enum v4l2_priority *arg)
+{
+	LOG(V4L2Compat, Debug)
+		<< "Servicing vidioc_s_priority fd = " << file->efd();
+
+	if (*arg > V4L2_PRIORITY_RECORD)
+		return -EINVAL;
+
+	if (file->priority() < maxPriority())
+		return -EBUSY;
+
+	file->setPriority(*arg);
+
+	return 0;
+}
+
 void V4L2CameraProxy::freeBuffers()
 {
 	LOG(V4L2Compat, Debug) << "Freeing libcamera bufs";
@@ -362,6 +405,9 @@ int V4L2CameraProxy::vidioc_reqbufs(V4L2CameraFile *file, struct v4l2_requestbuf
 
 	LOG(V4L2Compat, Debug) << arg->count << " buffers requested ";
 
+	if (file->priority() < maxPriority())
+		return -EBUSY;
+
 	if (!hasOwnership(file) && owner_)
 		return -EBUSY;
 
@@ -510,6 +556,9 @@ int V4L2CameraProxy::vidioc_streamon(V4L2CameraFile *file, int *arg)
 	if (!validateBufferType(*arg))
 		return -EINVAL;
 
+	if (file->priority() < maxPriority())
+		return -EBUSY;
+
 	if (!hasOwnership(file))
 		return -EBUSY;
 
@@ -525,6 +574,9 @@ int V4L2CameraProxy::vidioc_streamoff(V4L2CameraFile *file, int *arg)
 	if (!validateBufferType(*arg))
 		return -EINVAL;
 
+	if (file->priority() < maxPriority())
+		return -EBUSY;
+
 	if (!hasOwnership(file) && owner_)
 		return -EBUSY;
 
@@ -542,6 +594,8 @@ const std::set<unsigned long> V4L2CameraProxy::supportedIoctls_ = {
 	VIDIOC_G_FMT,
 	VIDIOC_S_FMT,
 	VIDIOC_TRY_FMT,
+	VIDIOC_G_PRIORITY,
+	VIDIOC_S_PRIORITY,
 	VIDIOC_REQBUFS,
 	VIDIOC_QUERYBUF,
 	VIDIOC_QBUF,
@@ -584,6 +638,12 @@ int V4L2CameraProxy::ioctl(V4L2CameraFile *file, unsigned long request, void *ar
 	case VIDIOC_TRY_FMT:
 		ret = vidioc_try_fmt(file, static_cast<struct v4l2_format *>(arg));
 		break;
+	case VIDIOC_G_PRIORITY:
+		ret = vidioc_g_priority(file, static_cast<enum v4l2_priority *>(arg));
+		break;
+	case VIDIOC_S_PRIORITY:
+		ret = vidioc_s_priority(file, static_cast<enum v4l2_priority *>(arg));
+		break;
 	case VIDIOC_REQBUFS:
 		ret = vidioc_reqbufs(file, static_cast<struct v4l2_requestbuffers *>(arg));
 		break;
diff --git a/src/v4l2/v4l2_camera_proxy.h b/src/v4l2/v4l2_camera_proxy.h
index 86c1a7df..8de306b4 100644
--- a/src/v4l2/v4l2_camera_proxy.h
+++ b/src/v4l2/v4l2_camera_proxy.h
@@ -43,6 +43,7 @@ private:
 	unsigned int calculateSizeImage(StreamConfiguration &streamConfig);
 	void querycap(std::shared_ptr<Camera> camera);
 	void tryFormat(struct v4l2_format *arg);
+	enum v4l2_priority maxPriority();
 	void updateBuffers();
 	void freeBuffers();
 
@@ -51,6 +52,8 @@ private:
 	int vidioc_g_fmt(V4L2CameraFile *file, struct v4l2_format *arg);
 	int vidioc_s_fmt(V4L2CameraFile *file, struct v4l2_format *arg);
 	int vidioc_try_fmt(V4L2CameraFile *file, struct v4l2_format *arg);
+	int vidioc_g_priority(V4L2CameraFile *file, enum v4l2_priority *arg);
+	int vidioc_s_priority(V4L2CameraFile *file, enum v4l2_priority *arg);
 	int vidioc_reqbufs(V4L2CameraFile *file, struct v4l2_requestbuffers *arg);
 	int vidioc_querybuf(V4L2CameraFile *file, struct v4l2_buffer *arg);
 	int vidioc_qbuf(V4L2CameraFile *file, struct v4l2_buffer *arg);
@@ -84,6 +87,8 @@ private:
 	std::vector<struct v4l2_buffer> buffers_;
 	std::map<void *, unsigned int> mmaps_;
 
+	std::set<V4L2CameraFile *> files_;
+
 	std::unique_ptr<V4L2Camera> vcam_;
 
 	/*