usb: gadget: uvc: implement s/g_parm

As the UVC gadget implements ENUM_FRAMEINTERVALS it should also
implement S_PARM and G_PARM to allow to get and set the frame interval.
While the driver doesn't actually do something with the frame interval,
it should still handle and store the interval correctly, if the user
space request it.

Signed-off-by: Michael Tretter <m.tretter@pengutronix.de>

drivers/usb/gadget/function/uvc.h

@@ -96,6 +96,7 @@ struct uvc_video {
 	unsigned int width;
 	unsigned int height;
 	unsigned int imagesize;
+	struct v4l2_fract timeperframe;
 	enum v4l2_colorspace colorspace;
 	enum v4l2_ycbcr_encoding ycbcr_enc;
 	enum v4l2_quantization quantization;

drivers/usb/gadget/function/uvc_v4l2.c

@@ -185,6 +185,9 @@ void uvc_init_default_format(struct uvc_device *uvc)
 		video->xfer_func = V4L2_XFER_FUNC_SRGB;
 		video->ycbcr_enc = V4L2_YCBCR_ENC_601;
 
+		video->timeperframe.numerator = 1;
+		video->timeperframe.denominator = 30;
+
 		return;
 	}
 
@@ -209,6 +212,11 @@ void uvc_init_default_format(struct uvc_device *uvc)
 	video->quantization = V4L2_QUANTIZATION_FULL_RANGE;
 	video->xfer_func = V4L2_XFER_FUNC_SRGB;
 	video->ycbcr_enc = V4L2_YCBCR_ENC_601;
+
+	video->timeperframe.numerator = uframe->frame.dw_default_frame_interval;
+	video->timeperframe.denominator = 10000000;
+	v4l2_simplify_fraction(&video->timeperframe.numerator,
+			       &video->timeperframe.denominator, 8, 333);
 }
 
 static struct uvcg_frame *find_closest_frame_by_size(struct uvc_device *uvc,
@@ -255,6 +263,46 @@ static struct uvcg_frame *find_closest_frame_by_size(struct uvc_device *uvc,
 	return uframe;
 }
 
+static void find_closest_timeperframe(struct uvc_device *uvc,
+				      struct v4l2_fract *timeperframe)
+{
+	struct uvc_video *video = &uvc->video;
+	struct uvcg_format *uformat;
+	struct uvcg_frame *uframe;
+	unsigned long interval;
+	unsigned int best_interval;
+	unsigned int curr;
+	unsigned int dist;
+	unsigned int best_dist = UINT_MAX;
+	int i;
+
+	if (timeperframe->denominator == 0)
+		timeperframe->denominator = video->timeperframe.denominator;
+	if (timeperframe->numerator == 0)
+		timeperframe->numerator = video->timeperframe.numerator;
+
+	uformat = find_format_by_pix(uvc, video->fcc);
+	uframe = find_closest_frame_by_size(uvc, uformat,
+					    video->width, video->height);
+
+	interval = timeperframe->numerator * 10000000;
+	do_div(interval, timeperframe->denominator);
+
+	for (i = 0; i < uframe->frame.b_frame_interval_type; i++) {
+		curr = uframe->dw_frame_interval[i];
+		dist = interval > curr ? interval - curr : curr - interval;
+		if (dist < best_dist) {
+			best_dist = dist;
+			best_interval = curr;
+		}
+	}
+
+	timeperframe->numerator = best_interval;
+	timeperframe->denominator = 10000000;
+	v4l2_simplify_fraction(&timeperframe->numerator,
+			       &timeperframe->denominator, 8, 333);
+}
+
 /* --------------------------------------------------------------------------
  * Requests handling
  */
@@ -456,6 +504,50 @@ uvc_v4l2_enum_framesizes(struct file *file, void *fh,
 	return 0;
 }
 
+static int
+uvc_v4l2_s_parm(struct file *file, void *fh, struct v4l2_streamparm *parm)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct uvc_device *uvc = video_get_drvdata(vdev);
+	struct uvc_video *video = &uvc->video;
+	struct v4l2_outputparm *out;
+
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_OUTPUT &&
+	    parm->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		return -EINVAL;
+
+	out = &parm->parm.output;
+
+	memset(out->reserved, 0, sizeof(out->reserved));
+
+	out->capability = V4L2_CAP_TIMEPERFRAME;
+	find_closest_timeperframe(uvc, &out->timeperframe);
+
+	video->timeperframe = out->timeperframe;
+
+	return 0;
+}
+
+static int
+uvc_v4l2_g_parm(struct file *file, void *fh, struct v4l2_streamparm *parm)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct uvc_device *uvc = video_get_drvdata(vdev);
+	struct uvc_video *video = &uvc->video;
+	struct v4l2_outputparm *out;
+
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_OUTPUT &&
+	    parm->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		return -EINVAL;
+
+	out = &parm->parm.output;
+
+	out->capability |= V4L2_CAP_TIMEPERFRAME;
+	out->timeperframe = video->timeperframe;
+
+	return 0;
+}
+
 static int
 uvc_v4l2_enum_format(struct file *file, void *fh, struct v4l2_fmtdesc *f)
 {
@@ -671,6 +763,8 @@ const struct v4l2_ioctl_ops uvc_v4l2_ioctl_ops = {
 	.vidioc_s_fmt_vid_out = uvc_v4l2_set_format,
 	.vidioc_enum_frameintervals = uvc_v4l2_enum_frameintervals,
 	.vidioc_enum_framesizes = uvc_v4l2_enum_framesizes,
+	.vidioc_g_parm = uvc_v4l2_g_parm,
+	.vidioc_s_parm = uvc_v4l2_s_parm,
 	.vidioc_enum_fmt_vid_out = uvc_v4l2_enum_format,
 	.vidioc_enum_output = uvc_v4l2_enum_output,
 	.vidioc_g_output = uvc_v4l2_g_output,