Orange Pi5 kernel

^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   1) // SPDX-License-Identifier: GPL-2.0-only
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   2) /*
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   3)  * vivid-meta-cap.c - meta capture support functions.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   4)  */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   5) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   6) #include <linux/errno.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   7) #include <linux/kernel.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   8) #include <linux/videodev2.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   9) #include <media/v4l2-common.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  10) #include <linux/usb/video.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  11) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  12) #include "vivid-core.h"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  13) #include "vivid-kthread-cap.h"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  14) #include "vivid-meta-cap.h"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  15) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  16) static int meta_cap_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  17) 				unsigned int *nplanes, unsigned int sizes[],
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  18) 				struct device *alloc_devs[])
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  19) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  20) 	struct vivid_dev *dev = vb2_get_drv_priv(vq);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  21) 	unsigned int size =  sizeof(struct vivid_uvc_meta_buf);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  22) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  23) 	if (!vivid_is_webcam(dev))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  24) 		return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  25) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  26) 	if (*nplanes) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  27) 		if (sizes[0] < size)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  28) 			return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  29) 	} else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  30) 		sizes[0] = size;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  31) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  32) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  33) 	if (vq->num_buffers + *nbuffers < 2)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  34) 		*nbuffers = 2 - vq->num_buffers;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  35) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  36) 	*nplanes = 1;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  37) 	return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  38) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  39) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  40) static int meta_cap_buf_prepare(struct vb2_buffer *vb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  41) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  42) 	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  43) 	unsigned int size = sizeof(struct vivid_uvc_meta_buf);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  44) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  45) 	dprintk(dev, 1, "%s\n", __func__);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  46) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  47) 	if (dev->buf_prepare_error) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  48) 		/*
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  49) 		 * Error injection: test what happens if buf_prepare() returns
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  50) 		 * an error.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  51) 		 */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  52) 		dev->buf_prepare_error = false;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  53) 		return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  54) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  55) 	if (vb2_plane_size(vb, 0) < size) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  56) 		dprintk(dev, 1, "%s data will not fit into plane (%lu < %u)\n",
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  57) 			__func__, vb2_plane_size(vb, 0), size);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  58) 		return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  59) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  60) 	vb2_set_plane_payload(vb, 0, size);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  61) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  62) 	return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  63) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  64) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  65) static void meta_cap_buf_queue(struct vb2_buffer *vb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  66) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  67) 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  68) 	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  69) 	struct vivid_buffer *buf = container_of(vbuf, struct vivid_buffer, vb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  70) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  71) 	dprintk(dev, 1, "%s\n", __func__);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  72) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  73) 	spin_lock(&dev->slock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  74) 	list_add_tail(&buf->list, &dev->meta_cap_active);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  75) 	spin_unlock(&dev->slock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  76) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  77) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  78) static int meta_cap_start_streaming(struct vb2_queue *vq, unsigned int count)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  79) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  80) 	struct vivid_dev *dev = vb2_get_drv_priv(vq);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  81) 	int err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  82) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  83) 	dprintk(dev, 1, "%s\n", __func__);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  84) 	dev->meta_cap_seq_count = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  85) 	if (dev->start_streaming_error) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  86) 		dev->start_streaming_error = false;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  87) 		err = -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  88) 	} else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  89) 		err = vivid_start_generating_vid_cap(dev,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  90) 						     &dev->meta_cap_streaming);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  91) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  92) 	if (err) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  93) 		struct vivid_buffer *buf, *tmp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  94) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  95) 		list_for_each_entry_safe(buf, tmp,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  96) 					 &dev->meta_cap_active, list) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  97) 			list_del(&buf->list);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  98) 			vb2_buffer_done(&buf->vb.vb2_buf,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  99) 					VB2_BUF_STATE_QUEUED);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 100) 		}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 101) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 102) 	return err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 103) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 104) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 105) /* abort streaming and wait for last buffer */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 106) static void meta_cap_stop_streaming(struct vb2_queue *vq)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 107) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 108) 	struct vivid_dev *dev = vb2_get_drv_priv(vq);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 109) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 110) 	dprintk(dev, 1, "%s\n", __func__);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 111) 	vivid_stop_generating_vid_cap(dev, &dev->meta_cap_streaming);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 112) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 113) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 114) static void meta_cap_buf_request_complete(struct vb2_buffer *vb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 115) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 116) 	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 117) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 118) 	v4l2_ctrl_request_complete(vb->req_obj.req, &dev->ctrl_hdl_meta_cap);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 119) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 120) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 121) const struct vb2_ops vivid_meta_cap_qops = {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 122) 	.queue_setup		= meta_cap_queue_setup,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 123) 	.buf_prepare		= meta_cap_buf_prepare,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 124) 	.buf_queue		= meta_cap_buf_queue,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 125) 	.start_streaming	= meta_cap_start_streaming,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 126) 	.stop_streaming		= meta_cap_stop_streaming,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 127) 	.buf_request_complete	= meta_cap_buf_request_complete,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 128) 	.wait_prepare		= vb2_ops_wait_prepare,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 129) 	.wait_finish		= vb2_ops_wait_finish,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 130) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 131) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 132) int vidioc_enum_fmt_meta_cap(struct file *file, void  *priv,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 133) 			     struct v4l2_fmtdesc *f)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 134) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 135) 	struct vivid_dev *dev = video_drvdata(file);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 136) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 137) 	if (!vivid_is_webcam(dev))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 138) 		return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 139) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 140) 	if (f->index > 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 141) 		return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 142) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 143) 	f->type = V4L2_BUF_TYPE_META_CAPTURE;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 144) 	f->pixelformat = V4L2_META_FMT_UVC;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 145) 	return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 146) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 147) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 148) int vidioc_g_fmt_meta_cap(struct file *file, void *priv,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 149) 			  struct v4l2_format *f)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 150) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 151) 	struct vivid_dev *dev = video_drvdata(file);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 152) 	struct v4l2_meta_format *meta = &f->fmt.meta;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 153) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 154) 	if (!vivid_is_webcam(dev) || !dev->has_meta_cap)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 155) 		return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 156) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 157) 	meta->dataformat = V4L2_META_FMT_UVC;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 158) 	meta->buffersize = sizeof(struct vivid_uvc_meta_buf);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 159) 	return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 160) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 161) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 162) void vivid_meta_cap_fillbuff(struct vivid_dev *dev,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 163) 			     struct vivid_buffer *buf, u64 soe)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 164) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 165) 	struct vivid_uvc_meta_buf *meta = vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 166) 	int buf_off = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 167) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 168) 	buf->vb.sequence = dev->meta_cap_seq_count;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 169) 	if (dev->field_cap == V4L2_FIELD_ALTERNATE)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 170) 		buf->vb.sequence /= 2;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 171) 	memset(meta, 1, vb2_plane_size(&buf->vb.vb2_buf, 0));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 172) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 173) 	meta->ns = ktime_get_ns();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 174) 	meta->sof = buf->vb.sequence * 30;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 175) 	meta->length = sizeof(*meta) - offsetof(struct vivid_uvc_meta_buf, length);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 176) 	meta->flags = UVC_STREAM_EOH | UVC_STREAM_EOF;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 177) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 178) 	if ((buf->vb.sequence % 2) == 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 179) 		meta->flags |= UVC_STREAM_FID;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 180) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 181) 	dprintk(dev, 2, "%s ns:%llu sof:%4d len:%u flags: 0x%02x",
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 182) 		__func__, meta->ns, meta->sof, meta->length, meta->flags);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 183) 	if (dev->meta_pts) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 184) 		meta->flags |= UVC_STREAM_PTS;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 185) 		meta->buf[0] = div_u64(soe, VIVID_META_CLOCK_UNIT);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 186) 		buf_off = 4;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 187) 		dprintk(dev, 2, " pts: %u\n", *(__u32 *)(meta->buf));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 188) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 189) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 190) 	if (dev->meta_scr) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 191) 		meta->flags |= UVC_STREAM_SCR;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 192) 		meta->buf[buf_off] = div_u64((soe + dev->cap_frame_eof_offset),
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 193) 					     VIVID_META_CLOCK_UNIT);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 194) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 195) 		meta->buf[buf_off + 4] = (buf->vb.sequence * 30) % 1000;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 196) 		dprintk(dev, 2, " stc: %u, sof counter: %u\n",
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 197) 			*(__u32 *)(meta->buf + buf_off),
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 198) 			*(__u16 *)(meta->buf + buf_off + 4));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 199) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 200) 	dprintk(dev, 2, "\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 201) }