1 files changed, 67 insertions, 15 deletions

diff --git a/drivers/media/v4l2-core/v4l2-common.c b/drivers/media/v4l2-core/v4l2-common.c
index 8650ad92b64d..b518b92d6d96 100644
--- a/drivers/media/v4l2-core/v4l2-common.c
+++ b/drivers/media/v4l2-core/v4l2-common.c
@@ -357,31 +357,35 @@ void v4l_bound_align_image(u32 *w, unsigned int wmin, unsigned int wmax,
 }
 EXPORT_SYMBOL_GPL(v4l_bound_align_image);
 
-const struct v4l2_frmsize_discrete *
-v4l2_find_nearest_format(const struct v4l2_frmsize_discrete *sizes,
-			  size_t num_sizes,
-			  s32 width, s32 height)
+const void *
+__v4l2_find_nearest_size(const void *array, size_t array_size,
+			 size_t entry_size, size_t width_offset,
+			 size_t height_offset, s32 width, s32 height)
 {
-	int i;
-	u32 error, min_error = UINT_MAX;
-	const struct v4l2_frmsize_discrete *size, *best = NULL;
+	u32 error, min_error = U32_MAX;
+	const void *best = NULL;
+	unsigned int i;
 
-	if (!sizes)
+	if (!array)
 		return NULL;
 
-	for (i = 0, size = sizes; i < num_sizes; i++, size++) {
-		error = abs(size->width - width) + abs(size->height - height);
-		if (error < min_error) {
-			min_error = error;
-			best = size;
-		}
+	for (i = 0; i < array_size; i++, array += entry_size) {
+		const u32 *entry_width = array + width_offset;
+		const u32 *entry_height = array + height_offset;
+
+		error = abs(*entry_width - width) + abs(*entry_height - height);
+		if (error > min_error)
+			continue;
+
+		min_error = error;
+		best = array;
 		if (!error)
 			break;
 	}
 
 	return best;
 }
-EXPORT_SYMBOL_GPL(v4l2_find_nearest_format);
+EXPORT_SYMBOL_GPL(__v4l2_find_nearest_size);
 
 void v4l2_get_timestamp(struct timeval *tv)
 {
@@ -392,3 +396,51 @@ void v4l2_get_timestamp(struct timeval *tv)
 	tv->tv_usec = ts.tv_nsec / NSEC_PER_USEC;
 }
 EXPORT_SYMBOL_GPL(v4l2_get_timestamp);
+
+int v4l2_g_parm_cap(struct video_device *vdev,
+		    struct v4l2_subdev *sd, struct v4l2_streamparm *a)
+{
+	struct v4l2_subdev_frame_interval ival = { 0 };
+	int ret;
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+	    a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		return -EINVAL;
+
+	if (vdev->device_caps & V4L2_CAP_READWRITE)
+		a->parm.capture.readbuffers = 2;
+	if (v4l2_subdev_has_op(sd, video, g_frame_interval))
+		a->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+	ret = v4l2_subdev_call(sd, video, g_frame_interval, &ival);
+	if (!ret)
+		a->parm.capture.timeperframe = ival.interval;
+	return ret;
+}
+EXPORT_SYMBOL_GPL(v4l2_g_parm_cap);
+
+int v4l2_s_parm_cap(struct video_device *vdev,
+		    struct v4l2_subdev *sd, struct v4l2_streamparm *a)
+{
+	struct v4l2_subdev_frame_interval ival = {
+		.interval = a->parm.capture.timeperframe
+	};
+	int ret;
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+	    a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		return -EINVAL;
+
+	memset(&a->parm, 0, sizeof(a->parm));
+	if (vdev->device_caps & V4L2_CAP_READWRITE)
+		a->parm.capture.readbuffers = 2;
+	else
+		a->parm.capture.readbuffers = 0;
+
+	if (v4l2_subdev_has_op(sd, video, g_frame_interval))
+		a->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+	ret = v4l2_subdev_call(sd, video, s_frame_interval, &ival);
+	if (!ret)
+		a->parm.capture.timeperframe = ival.interval;
+	return ret;
+}
+EXPORT_SYMBOL_GPL(v4l2_s_parm_cap);