Resguardo inicial

linux-7.2.3/drivers/media/test-drivers/vimc/Kconfig

@@ -0,0 +1,20 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config VIDEO_VIMC
+	tristate "Virtual Media Controller Driver (VIMC)"
+	depends on VIDEO_DEV
+	select FONT_SUPPORT
+	select FONT_8x16
+	select MEDIA_CONTROLLER
+	select VIDEO_V4L2_SUBDEV_API
+	select VIDEOBUF2_VMALLOC
+	select VIDEOBUF2_DMA_CONTIG
+	select VIDEO_V4L2_TPG
+	help
+	  Skeleton driver for Virtual Media Controller
+
+	  This driver can be compared to the vivid driver for emulating
+	  a media node that exposes a complex media topology. The topology
+	  is hard coded for now but is meant to be highly configurable in
+	  the future.
+
+	  When in doubt, say N.

linux-7.2.3/drivers/media/test-drivers/vimc/Makefile

@@ -0,0 +1,6 @@
+# SPDX-License-Identifier: GPL-2.0
+vimc-y := vimc-core.o vimc-common.o vimc-streamer.o vimc-capture.o \
+		vimc-debayer.o vimc-scaler.o vimc-sensor.o vimc-lens.o
+
+obj-$(CONFIG_VIDEO_VIMC) += vimc.o
+

linux-7.2.3/drivers/media/test-drivers/vimc/vimc-capture.c

@@ -0,0 +1,493 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * vimc-capture.c Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ */
+
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/videobuf2-vmalloc.h>
+
+#include "vimc-common.h"
+#include "vimc-streamer.h"
+
+struct vimc_capture_device {
+	struct vimc_ent_device ved;
+	struct video_device vdev;
+	struct v4l2_pix_format format;
+	struct vb2_queue queue;
+	struct list_head buf_list;
+	/*
+	 * NOTE: in a real driver, a spin lock must be used to access the
+	 * queue because the frames are generated from a hardware interruption
+	 * and the isr is not allowed to sleep.
+	 * Even if it is not necessary a spinlock in the vimc driver, we
+	 * use it here as a code reference
+	 */
+	spinlock_t qlock;
+	struct mutex lock;
+	u32 sequence;
+	struct vimc_stream stream;
+	struct media_pad pad;
+};
+
+static const struct v4l2_pix_format fmt_default = {
+	.width = 640,
+	.height = 480,
+	.pixelformat = V4L2_PIX_FMT_RGB24,
+	.field = V4L2_FIELD_NONE,
+	.colorspace = V4L2_COLORSPACE_SRGB,
+};
+
+struct vimc_capture_buffer {
+	/*
+	 * struct vb2_v4l2_buffer must be the first element
+	 * the videobuf2 framework will allocate this struct based on
+	 * buf_struct_size and use the first sizeof(struct vb2_buffer) bytes of
+	 * memory as a vb2_buffer
+	 */
+	struct vb2_v4l2_buffer vb2;
+	struct list_head list;
+};
+
+static int vimc_capture_querycap(struct file *file, void *priv,
+			     struct v4l2_capability *cap)
+{
+	strscpy(cap->driver, VIMC_PDEV_NAME, sizeof(cap->driver));
+	strscpy(cap->card, KBUILD_MODNAME, sizeof(cap->card));
+
+	return 0;
+}
+
+static void vimc_capture_get_format(struct vimc_ent_device *ved,
+				struct v4l2_pix_format *fmt)
+{
+	struct vimc_capture_device *vcapture = container_of(ved, struct vimc_capture_device,
+						    ved);
+
+	*fmt = vcapture->format;
+}
+
+static int vimc_capture_g_fmt_vid_cap(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct vimc_capture_device *vcapture = video_drvdata(file);
+
+	f->fmt.pix = vcapture->format;
+
+	return 0;
+}
+
+static int vimc_capture_try_fmt_vid_cap(struct file *file, void *priv,
+				    struct v4l2_format *f)
+{
+	struct v4l2_pix_format *format = &f->fmt.pix;
+	const struct vimc_pix_map *vpix;
+
+	format->width = clamp_t(u32, format->width, VIMC_FRAME_MIN_WIDTH,
+				VIMC_FRAME_MAX_WIDTH) & ~1;
+	format->height = clamp_t(u32, format->height, VIMC_FRAME_MIN_HEIGHT,
+				 VIMC_FRAME_MAX_HEIGHT) & ~1;
+
+	/* Don't accept a pixelformat that is not on the table */
+	vpix = vimc_pix_map_by_pixelformat(format->pixelformat);
+	if (!vpix) {
+		format->pixelformat = fmt_default.pixelformat;
+		vpix = vimc_pix_map_by_pixelformat(format->pixelformat);
+	}
+	/* TODO: Add support for custom bytesperline values */
+	format->bytesperline = format->width * vpix->bpp;
+	format->sizeimage = format->bytesperline * format->height;
+
+	if (format->field == V4L2_FIELD_ANY)
+		format->field = fmt_default.field;
+
+	vimc_colorimetry_clamp(format);
+
+	if (format->colorspace == V4L2_COLORSPACE_DEFAULT)
+		format->colorspace = fmt_default.colorspace;
+
+	return 0;
+}
+
+static int vimc_capture_s_fmt_vid_cap(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct vimc_capture_device *vcapture = video_drvdata(file);
+	int ret;
+
+	/* Do not change the format while stream is on */
+	if (vb2_is_busy(&vcapture->queue))
+		return -EBUSY;
+
+	ret = vimc_capture_try_fmt_vid_cap(file, priv, f);
+	if (ret)
+		return ret;
+
+	dev_dbg(vcapture->ved.dev, "%s: format update: "
+		"old:%dx%d (0x%x, %d, %d, %d, %d) "
+		"new:%dx%d (0x%x, %d, %d, %d, %d)\n", vcapture->vdev.name,
+		/* old */
+		vcapture->format.width, vcapture->format.height,
+		vcapture->format.pixelformat, vcapture->format.colorspace,
+		vcapture->format.quantization, vcapture->format.xfer_func,
+		vcapture->format.ycbcr_enc,
+		/* new */
+		f->fmt.pix.width, f->fmt.pix.height,
+		f->fmt.pix.pixelformat,	f->fmt.pix.colorspace,
+		f->fmt.pix.quantization, f->fmt.pix.xfer_func,
+		f->fmt.pix.ycbcr_enc);
+
+	vcapture->format = f->fmt.pix;
+
+	return 0;
+}
+
+static int vimc_capture_enum_fmt_vid_cap(struct file *file, void *priv,
+				     struct v4l2_fmtdesc *f)
+{
+	const struct vimc_pix_map *vpix;
+
+	if (f->mbus_code) {
+		if (f->index > 0)
+			return -EINVAL;
+
+		vpix = vimc_pix_map_by_code(f->mbus_code);
+	} else {
+		vpix = vimc_pix_map_by_index(f->index);
+	}
+
+	if (!vpix)
+		return -EINVAL;
+
+	f->pixelformat = vpix->pixelformat;
+
+	return 0;
+}
+
+static int vimc_capture_enum_framesizes(struct file *file, void *priv,
+				    struct v4l2_frmsizeenum *fsize)
+{
+	const struct vimc_pix_map *vpix;
+
+	if (fsize->index)
+		return -EINVAL;
+
+	/* Only accept code in the pix map table */
+	vpix = vimc_pix_map_by_code(fsize->pixel_format);
+	if (!vpix)
+		return -EINVAL;
+
+	fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
+	fsize->stepwise.min_width = VIMC_FRAME_MIN_WIDTH;
+	fsize->stepwise.max_width = VIMC_FRAME_MAX_WIDTH;
+	fsize->stepwise.min_height = VIMC_FRAME_MIN_HEIGHT;
+	fsize->stepwise.max_height = VIMC_FRAME_MAX_HEIGHT;
+	fsize->stepwise.step_width = 1;
+	fsize->stepwise.step_height = 1;
+
+	return 0;
+}
+
+static const struct v4l2_file_operations vimc_capture_fops = {
+	.owner		= THIS_MODULE,
+	.open		= v4l2_fh_open,
+	.release	= vb2_fop_release,
+	.read           = vb2_fop_read,
+	.poll		= vb2_fop_poll,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap           = vb2_fop_mmap,
+};
+
+static const struct v4l2_ioctl_ops vimc_capture_ioctl_ops = {
+	.vidioc_querycap = vimc_capture_querycap,
+
+	.vidioc_g_fmt_vid_cap = vimc_capture_g_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap = vimc_capture_s_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap = vimc_capture_try_fmt_vid_cap,
+	.vidioc_enum_fmt_vid_cap = vimc_capture_enum_fmt_vid_cap,
+	.vidioc_enum_framesizes = vimc_capture_enum_framesizes,
+
+	.vidioc_reqbufs = vb2_ioctl_reqbufs,
+	.vidioc_create_bufs = vb2_ioctl_create_bufs,
+	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
+	.vidioc_querybuf = vb2_ioctl_querybuf,
+	.vidioc_qbuf = vb2_ioctl_qbuf,
+	.vidioc_dqbuf = vb2_ioctl_dqbuf,
+	.vidioc_expbuf = vb2_ioctl_expbuf,
+	.vidioc_streamon = vb2_ioctl_streamon,
+	.vidioc_streamoff = vb2_ioctl_streamoff,
+	.vidioc_remove_bufs = vb2_ioctl_remove_bufs,
+};
+
+static void vimc_capture_return_all_buffers(struct vimc_capture_device *vcapture,
+					enum vb2_buffer_state state)
+{
+	struct vimc_capture_buffer *vbuf, *node;
+
+	spin_lock(&vcapture->qlock);
+
+	list_for_each_entry_safe(vbuf, node, &vcapture->buf_list, list) {
+		list_del(&vbuf->list);
+		vb2_buffer_done(&vbuf->vb2.vb2_buf, state);
+	}
+
+	spin_unlock(&vcapture->qlock);
+}
+
+static int vimc_capture_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct vimc_capture_device *vcapture = vb2_get_drv_priv(vq);
+	int ret;
+
+	vcapture->sequence = 0;
+
+	/* Start the media pipeline */
+	ret = video_device_pipeline_start(&vcapture->vdev, &vcapture->stream.pipe);
+	if (ret) {
+		vimc_capture_return_all_buffers(vcapture, VB2_BUF_STATE_QUEUED);
+		return ret;
+	}
+
+	ret = vimc_streamer_s_stream(&vcapture->stream, &vcapture->ved, 1);
+	if (ret) {
+		video_device_pipeline_stop(&vcapture->vdev);
+		vimc_capture_return_all_buffers(vcapture, VB2_BUF_STATE_QUEUED);
+		return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * Stop the stream engine. Any remaining buffers in the stream queue are
+ * dequeued and passed on to the vb2 framework marked as STATE_ERROR.
+ */
+static void vimc_capture_stop_streaming(struct vb2_queue *vq)
+{
+	struct vimc_capture_device *vcapture = vb2_get_drv_priv(vq);
+
+	vimc_streamer_s_stream(&vcapture->stream, &vcapture->ved, 0);
+
+	/* Stop the media pipeline */
+	video_device_pipeline_stop(&vcapture->vdev);
+
+	/* Release all active buffers */
+	vimc_capture_return_all_buffers(vcapture, VB2_BUF_STATE_ERROR);
+}
+
+static void vimc_capture_buf_queue(struct vb2_buffer *vb2_buf)
+{
+	struct vimc_capture_device *vcapture = vb2_get_drv_priv(vb2_buf->vb2_queue);
+	struct vimc_capture_buffer *buf = container_of(vb2_buf,
+						   struct vimc_capture_buffer,
+						   vb2.vb2_buf);
+
+	spin_lock(&vcapture->qlock);
+	list_add_tail(&buf->list, &vcapture->buf_list);
+	spin_unlock(&vcapture->qlock);
+}
+
+static int vimc_capture_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
+				unsigned int *nplanes, unsigned int sizes[],
+				struct device *alloc_devs[])
+{
+	struct vimc_capture_device *vcapture = vb2_get_drv_priv(vq);
+
+	if (*nplanes)
+		return sizes[0] < vcapture->format.sizeimage ? -EINVAL : 0;
+	/* We don't support multiplanes for now */
+	*nplanes = 1;
+	sizes[0] = vcapture->format.sizeimage;
+
+	return 0;
+}
+
+static int vimc_capture_buffer_prepare(struct vb2_buffer *vb)
+{
+	struct vimc_capture_device *vcapture = vb2_get_drv_priv(vb->vb2_queue);
+	unsigned long size = vcapture->format.sizeimage;
+
+	if (vb2_plane_size(vb, 0) < size) {
+		dev_err(vcapture->ved.dev, "%s: buffer too small (%lu < %lu)\n",
+			vcapture->vdev.name, vb2_plane_size(vb, 0), size);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static const struct vb2_ops vimc_capture_qops = {
+	.start_streaming	= vimc_capture_start_streaming,
+	.stop_streaming		= vimc_capture_stop_streaming,
+	.buf_queue		= vimc_capture_buf_queue,
+	.queue_setup		= vimc_capture_queue_setup,
+	.buf_prepare		= vimc_capture_buffer_prepare,
+};
+
+static const struct media_entity_operations vimc_capture_mops = {
+	.link_validate		= vimc_vdev_link_validate,
+};
+
+static void vimc_capture_release(struct vimc_ent_device *ved)
+{
+	struct vimc_capture_device *vcapture =
+		container_of(ved, struct vimc_capture_device, ved);
+
+	media_entity_cleanup(vcapture->ved.ent);
+	kfree(vcapture);
+}
+
+static void vimc_capture_unregister(struct vimc_ent_device *ved)
+{
+	struct vimc_capture_device *vcapture =
+		container_of(ved, struct vimc_capture_device, ved);
+
+	vb2_video_unregister_device(&vcapture->vdev);
+}
+
+static void *vimc_capture_process_frame(struct vimc_ent_device *ved,
+				    const void *frame)
+{
+	struct vimc_capture_device *vcapture = container_of(ved, struct vimc_capture_device,
+						    ved);
+	struct vimc_capture_buffer *vimc_buf;
+	void *vbuf;
+
+	spin_lock(&vcapture->qlock);
+
+	/* Get the first entry of the list */
+	vimc_buf = list_first_entry_or_null(&vcapture->buf_list,
+					    typeof(*vimc_buf), list);
+	if (!vimc_buf) {
+		spin_unlock(&vcapture->qlock);
+		return ERR_PTR(-EAGAIN);
+	}
+
+	/* Remove this entry from the list */
+	list_del(&vimc_buf->list);
+
+	spin_unlock(&vcapture->qlock);
+
+	/* Fill the buffer */
+	vimc_buf->vb2.vb2_buf.timestamp = ktime_get_ns();
+	vimc_buf->vb2.sequence = vcapture->sequence++;
+	vimc_buf->vb2.field = vcapture->format.field;
+
+	vbuf = vb2_plane_vaddr(&vimc_buf->vb2.vb2_buf, 0);
+
+	memcpy(vbuf, frame, vcapture->format.sizeimage);
+
+	/* Set it as ready */
+	vb2_set_plane_payload(&vimc_buf->vb2.vb2_buf, 0,
+			      vcapture->format.sizeimage);
+	vb2_buffer_done(&vimc_buf->vb2.vb2_buf, VB2_BUF_STATE_DONE);
+	return NULL;
+}
+
+static struct vimc_ent_device *vimc_capture_add(struct vimc_device *vimc,
+					    const char *vcfg_name)
+{
+	struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;
+	const struct vimc_pix_map *vpix;
+	struct vimc_capture_device *vcapture;
+	struct video_device *vdev;
+	struct vb2_queue *q;
+	int ret;
+
+	/* Allocate the vimc_capture_device struct */
+	vcapture = kzalloc_obj(*vcapture);
+	if (!vcapture)
+		return ERR_PTR(-ENOMEM);
+
+	/* Initialize the media entity */
+	vcapture->vdev.entity.name = vcfg_name;
+	vcapture->vdev.entity.function = MEDIA_ENT_F_IO_V4L;
+	vcapture->pad.flags = MEDIA_PAD_FL_SINK;
+	ret = media_entity_pads_init(&vcapture->vdev.entity,
+				     1, &vcapture->pad);
+	if (ret)
+		goto err_free_vcapture;
+
+	/* Initialize the lock */
+	mutex_init(&vcapture->lock);
+
+	/* Initialize the vb2 queue */
+	q = &vcapture->queue;
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	q->io_modes = VB2_MMAP | VB2_DMABUF;
+	if (vimc_allocator == VIMC_ALLOCATOR_VMALLOC)
+		q->io_modes |= VB2_USERPTR;
+	q->drv_priv = vcapture;
+	q->buf_struct_size = sizeof(struct vimc_capture_buffer);
+	q->ops = &vimc_capture_qops;
+	q->mem_ops = vimc_allocator == VIMC_ALLOCATOR_DMA_CONTIG
+		   ? &vb2_dma_contig_memops : &vb2_vmalloc_memops;
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->min_reqbufs_allocation = 2;
+	q->lock = &vcapture->lock;
+	q->dev = v4l2_dev->dev;
+
+	ret = vb2_queue_init(q);
+	if (ret) {
+		dev_err(vimc->mdev.dev, "%s: vb2 queue init failed (err=%d)\n",
+			vcfg_name, ret);
+		goto err_clean_m_ent;
+	}
+
+	/* Initialize buffer list and its lock */
+	INIT_LIST_HEAD(&vcapture->buf_list);
+	spin_lock_init(&vcapture->qlock);
+
+	/* Set default frame format */
+	vcapture->format = fmt_default;
+	vpix = vimc_pix_map_by_pixelformat(vcapture->format.pixelformat);
+	vcapture->format.bytesperline = vcapture->format.width * vpix->bpp;
+	vcapture->format.sizeimage = vcapture->format.bytesperline *
+				 vcapture->format.height;
+
+	/* Fill the vimc_ent_device struct */
+	vcapture->ved.ent = &vcapture->vdev.entity;
+	vcapture->ved.process_frame = vimc_capture_process_frame;
+	vcapture->ved.vdev_get_format = vimc_capture_get_format;
+	vcapture->ved.dev = vimc->mdev.dev;
+
+	/* Initialize the video_device struct */
+	vdev = &vcapture->vdev;
+	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING
+			  | V4L2_CAP_IO_MC;
+	vdev->entity.ops = &vimc_capture_mops;
+	vdev->release = video_device_release_empty;
+	vdev->fops = &vimc_capture_fops;
+	vdev->ioctl_ops = &vimc_capture_ioctl_ops;
+	vdev->lock = &vcapture->lock;
+	vdev->queue = q;
+	vdev->v4l2_dev = v4l2_dev;
+	vdev->vfl_dir = VFL_DIR_RX;
+	strscpy(vdev->name, vcfg_name, sizeof(vdev->name));
+	video_set_drvdata(vdev, &vcapture->ved);
+
+	/* Register the video_device with the v4l2 and the media framework */
+	ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
+	if (ret) {
+		dev_err(vimc->mdev.dev, "%s: video register failed (err=%d)\n",
+			vcapture->vdev.name, ret);
+		goto err_clean_m_ent;
+	}
+
+	return &vcapture->ved;
+
+err_clean_m_ent:
+	media_entity_cleanup(&vcapture->vdev.entity);
+err_free_vcapture:
+	kfree(vcapture);
+
+	return ERR_PTR(ret);
+}
+
+const struct vimc_ent_type vimc_capture_type = {
+	.add = vimc_capture_add,
+	.unregister = vimc_capture_unregister,
+	.release = vimc_capture_release
+};

linux-7.2.3/drivers/media/test-drivers/vimc/vimc-common.c

@@ -0,0 +1,423 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * vimc-common.c Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+
+#include <media/v4l2-ctrls.h>
+
+#include "vimc-common.h"
+
+/*
+ * NOTE: non-bayer formats need to come first (necessary for enum_mbus_code
+ * in the scaler)
+ */
+static const struct vimc_pix_map vimc_pix_map_list[] = {
+	/* TODO: add all missing formats */
+
+	/* RGB formats */
+	{
+		.code = {
+			MEDIA_BUS_FMT_BGR888_1X24,
+			MEDIA_BUS_FMT_BGR888_3X8
+		},
+		.pixelformat = V4L2_PIX_FMT_BGR24,
+		.bpp = 3,
+		.bayer = false,
+	},
+	{
+		.code = {
+			MEDIA_BUS_FMT_RGB888_1X24,
+			MEDIA_BUS_FMT_RGB888_2X12_BE,
+			MEDIA_BUS_FMT_RGB888_2X12_LE,
+			MEDIA_BUS_FMT_RGB888_3X8,
+			MEDIA_BUS_FMT_RGB888_1X7X4_SPWG,
+			MEDIA_BUS_FMT_RGB888_1X7X4_JEIDA,
+			MEDIA_BUS_FMT_RGB888_1X32_PADHI,
+			MEDIA_BUS_FMT_GBR888_1X24
+		},
+		.pixelformat = V4L2_PIX_FMT_RGB24,
+		.bpp = 3,
+		.bayer = false,
+	},
+	{
+		.code = { MEDIA_BUS_FMT_ARGB8888_1X32 },
+		.pixelformat = V4L2_PIX_FMT_ARGB32,
+		.bpp = 4,
+		.bayer = false,
+	},
+
+	/* Bayer formats */
+	{
+		.code = { MEDIA_BUS_FMT_SBGGR8_1X8 },
+		.pixelformat = V4L2_PIX_FMT_SBGGR8,
+		.bpp = 1,
+		.bayer = true,
+	},
+	{
+		.code = { MEDIA_BUS_FMT_SGBRG8_1X8 },
+		.pixelformat = V4L2_PIX_FMT_SGBRG8,
+		.bpp = 1,
+		.bayer = true,
+	},
+	{
+		.code = { MEDIA_BUS_FMT_SGRBG8_1X8 },
+		.pixelformat = V4L2_PIX_FMT_SGRBG8,
+		.bpp = 1,
+		.bayer = true,
+	},
+	{
+		.code = { MEDIA_BUS_FMT_SRGGB8_1X8 },
+		.pixelformat = V4L2_PIX_FMT_SRGGB8,
+		.bpp = 1,
+		.bayer = true,
+	},
+	{
+		.code = { MEDIA_BUS_FMT_SBGGR10_1X10 },
+		.pixelformat = V4L2_PIX_FMT_SBGGR10,
+		.bpp = 2,
+		.bayer = true,
+	},
+	{
+		.code = { MEDIA_BUS_FMT_SGBRG10_1X10 },
+		.pixelformat = V4L2_PIX_FMT_SGBRG10,
+		.bpp = 2,
+		.bayer = true,
+	},
+	{
+		.code = { MEDIA_BUS_FMT_SGRBG10_1X10 },
+		.pixelformat = V4L2_PIX_FMT_SGRBG10,
+		.bpp = 2,
+		.bayer = true,
+	},
+	{
+		.code = { MEDIA_BUS_FMT_SRGGB10_1X10 },
+		.pixelformat = V4L2_PIX_FMT_SRGGB10,
+		.bpp = 2,
+		.bayer = true,
+	},
+
+	/* 10bit raw bayer a-law compressed to 8 bits */
+	{
+		.code = { MEDIA_BUS_FMT_SBGGR10_ALAW8_1X8 },
+		.pixelformat = V4L2_PIX_FMT_SBGGR10ALAW8,
+		.bpp = 1,
+		.bayer = true,
+	},
+	{
+		.code = { MEDIA_BUS_FMT_SGBRG10_ALAW8_1X8 },
+		.pixelformat = V4L2_PIX_FMT_SGBRG10ALAW8,
+		.bpp = 1,
+		.bayer = true,
+	},
+	{
+		.code = { MEDIA_BUS_FMT_SGRBG10_ALAW8_1X8 },
+		.pixelformat = V4L2_PIX_FMT_SGRBG10ALAW8,
+		.bpp = 1,
+		.bayer = true,
+	},
+	{
+		.code = { MEDIA_BUS_FMT_SRGGB10_ALAW8_1X8 },
+		.pixelformat = V4L2_PIX_FMT_SRGGB10ALAW8,
+		.bpp = 1,
+		.bayer = true,
+	},
+
+	/* 10bit raw bayer DPCM compressed to 8 bits */
+	{
+		.code = { MEDIA_BUS_FMT_SBGGR10_DPCM8_1X8 },
+		.pixelformat = V4L2_PIX_FMT_SBGGR10DPCM8,
+		.bpp = 1,
+		.bayer = true,
+	},
+	{
+		.code = { MEDIA_BUS_FMT_SGBRG10_DPCM8_1X8 },
+		.pixelformat = V4L2_PIX_FMT_SGBRG10DPCM8,
+		.bpp = 1,
+		.bayer = true,
+	},
+	{
+		.code = { MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8 },
+		.pixelformat = V4L2_PIX_FMT_SGRBG10DPCM8,
+		.bpp = 1,
+		.bayer = true,
+	},
+	{
+		.code = { MEDIA_BUS_FMT_SRGGB10_DPCM8_1X8 },
+		.pixelformat = V4L2_PIX_FMT_SRGGB10DPCM8,
+		.bpp = 1,
+		.bayer = true,
+	},
+	{
+		.code = { MEDIA_BUS_FMT_SBGGR12_1X12 },
+		.pixelformat = V4L2_PIX_FMT_SBGGR12,
+		.bpp = 2,
+		.bayer = true,
+	},
+	{
+		.code = { MEDIA_BUS_FMT_SGBRG12_1X12 },
+		.pixelformat = V4L2_PIX_FMT_SGBRG12,
+		.bpp = 2,
+		.bayer = true,
+	},
+	{
+		.code = { MEDIA_BUS_FMT_SGRBG12_1X12 },
+		.pixelformat = V4L2_PIX_FMT_SGRBG12,
+		.bpp = 2,
+		.bayer = true,
+	},
+	{
+		.code = { MEDIA_BUS_FMT_SRGGB12_1X12 },
+		.pixelformat = V4L2_PIX_FMT_SRGGB12,
+		.bpp = 2,
+		.bayer = true,
+	},
+};
+
+bool vimc_is_source(struct media_entity *ent)
+{
+	unsigned int i;
+
+	for (i = 0; i < ent->num_pads; i++)
+		if (ent->pads[i].flags & MEDIA_PAD_FL_SINK)
+			return false;
+	return true;
+}
+
+const struct vimc_pix_map *vimc_pix_map_by_index(unsigned int i)
+{
+	if (i >= ARRAY_SIZE(vimc_pix_map_list))
+		return NULL;
+
+	return &vimc_pix_map_list[i];
+}
+
+u32 vimc_mbus_code_by_index(unsigned int index)
+{
+	unsigned int i, j;
+
+	for (i = 0; i < ARRAY_SIZE(vimc_pix_map_list); i++) {
+		for (j = 0; j < ARRAY_SIZE(vimc_pix_map_list[i].code); j++) {
+			if (!vimc_pix_map_list[i].code[j])
+				break;
+
+			if (!index)
+				return vimc_pix_map_list[i].code[j];
+			index--;
+		}
+	}
+	return 0;
+}
+
+const struct vimc_pix_map *vimc_pix_map_by_code(u32 code)
+{
+	unsigned int i, j;
+
+	for (i = 0; i < ARRAY_SIZE(vimc_pix_map_list); i++) {
+		for (j = 0; j < ARRAY_SIZE(vimc_pix_map_list[i].code); j++) {
+			if (vimc_pix_map_list[i].code[j] == code)
+				return &vimc_pix_map_list[i];
+		}
+	}
+	return NULL;
+}
+
+const struct vimc_pix_map *vimc_pix_map_by_pixelformat(u32 pixelformat)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(vimc_pix_map_list); i++) {
+		if (vimc_pix_map_list[i].pixelformat == pixelformat)
+			return &vimc_pix_map_list[i];
+	}
+	return NULL;
+}
+
+static int vimc_get_pix_format(struct media_pad *pad,
+			       struct v4l2_pix_format *fmt)
+{
+	if (is_media_entity_v4l2_subdev(pad->entity)) {
+		struct v4l2_subdev *sd =
+			media_entity_to_v4l2_subdev(pad->entity);
+		struct v4l2_subdev_format sd_fmt = {
+			.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+			.pad = pad->index,
+		};
+		const struct vimc_pix_map *pix_map;
+		int ret;
+
+		ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &sd_fmt);
+		if (ret)
+			return ret;
+
+		v4l2_fill_pix_format(fmt, &sd_fmt.format);
+		pix_map = vimc_pix_map_by_code(sd_fmt.format.code);
+		fmt->pixelformat = pix_map->pixelformat;
+	} else if (is_media_entity_v4l2_video_device(pad->entity)) {
+		struct video_device *vdev = container_of(pad->entity,
+							 struct video_device,
+							 entity);
+		struct vimc_ent_device *ved = video_get_drvdata(vdev);
+
+		if (!ved->vdev_get_format)
+			return -ENOIOCTLCMD;
+
+		ved->vdev_get_format(ved, fmt);
+	} else {
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int vimc_vdev_link_validate(struct media_link *link)
+{
+	struct v4l2_pix_format source_fmt, sink_fmt;
+	int ret;
+
+	ret = vimc_get_pix_format(link->source, &source_fmt);
+	if (ret)
+		return ret;
+
+	ret = vimc_get_pix_format(link->sink, &sink_fmt);
+	if (ret)
+		return ret;
+
+	pr_info("vimc link validate: "
+		"%s:src:%dx%d (0x%x, %d, %d, %d, %d) "
+		"%s:snk:%dx%d (0x%x, %d, %d, %d, %d)\n",
+		/* src */
+		link->source->entity->name,
+		source_fmt.width, source_fmt.height,
+		source_fmt.pixelformat, source_fmt.colorspace,
+		source_fmt.quantization, source_fmt.xfer_func,
+		source_fmt.ycbcr_enc,
+		/* sink */
+		link->sink->entity->name,
+		sink_fmt.width, sink_fmt.height,
+		sink_fmt.pixelformat, sink_fmt.colorspace,
+		sink_fmt.quantization, sink_fmt.xfer_func,
+		sink_fmt.ycbcr_enc);
+
+	/* The width, height and pixelformat must match. */
+	if (source_fmt.width != sink_fmt.width ||
+	    source_fmt.height != sink_fmt.height ||
+	    source_fmt.pixelformat != sink_fmt.pixelformat)
+		return -EPIPE;
+
+	/*
+	 * The field order must match, or the sink field order must be NONE
+	 * to support interlaced hardware connected to bridges that support
+	 * progressive formats only.
+	 */
+	if (source_fmt.field != sink_fmt.field &&
+	    sink_fmt.field != V4L2_FIELD_NONE)
+		return -EPIPE;
+
+	/*
+	 * If colorspace is DEFAULT, then assume all the colorimetry is also
+	 * DEFAULT, return 0 to skip comparing the other colorimetry parameters
+	 */
+	if (source_fmt.colorspace == V4L2_COLORSPACE_DEFAULT ||
+	    sink_fmt.colorspace == V4L2_COLORSPACE_DEFAULT)
+		return 0;
+
+	/* Colorspace must match. */
+	if (source_fmt.colorspace != sink_fmt.colorspace)
+		return -EPIPE;
+
+	/* Colorimetry must match if they are not set to DEFAULT */
+	if (source_fmt.ycbcr_enc != V4L2_YCBCR_ENC_DEFAULT &&
+	    sink_fmt.ycbcr_enc != V4L2_YCBCR_ENC_DEFAULT &&
+	    source_fmt.ycbcr_enc != sink_fmt.ycbcr_enc)
+		return -EPIPE;
+
+	if (source_fmt.quantization != V4L2_QUANTIZATION_DEFAULT &&
+	    sink_fmt.quantization != V4L2_QUANTIZATION_DEFAULT &&
+	    source_fmt.quantization != sink_fmt.quantization)
+		return -EPIPE;
+
+	if (source_fmt.xfer_func != V4L2_XFER_FUNC_DEFAULT &&
+	    sink_fmt.xfer_func != V4L2_XFER_FUNC_DEFAULT &&
+	    source_fmt.xfer_func != sink_fmt.xfer_func)
+		return -EPIPE;
+
+	return 0;
+}
+
+static const struct media_entity_operations vimc_ent_sd_mops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+int vimc_ent_sd_register(struct vimc_ent_device *ved,
+			 struct v4l2_subdev *sd,
+			 struct v4l2_device *v4l2_dev,
+			 const char *const name,
+			 u32 function,
+			 u16 num_pads,
+			 struct media_pad *pads,
+			 const struct v4l2_subdev_internal_ops *int_ops,
+			 const struct v4l2_subdev_ops *sd_ops)
+{
+	int ret;
+
+	/* Fill the vimc_ent_device struct */
+	ved->ent = &sd->entity;
+
+	/* Initialize the subdev */
+	v4l2_subdev_init(sd, sd_ops);
+	sd->internal_ops = int_ops;
+	sd->entity.function = function;
+	sd->entity.ops = &vimc_ent_sd_mops;
+	sd->owner = THIS_MODULE;
+	strscpy(sd->name, name, sizeof(sd->name));
+	v4l2_set_subdevdata(sd, ved);
+
+	/* Expose this subdev to user space */
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	if (sd->ctrl_handler)
+		sd->flags |= V4L2_SUBDEV_FL_HAS_EVENTS;
+
+	/* Initialize the media entity */
+	ret = media_entity_pads_init(&sd->entity, num_pads, pads);
+	if (ret)
+		return ret;
+
+	/*
+	 * Finalize the subdev initialization if it supports active states. Use
+	 * the control handler lock as the state lock if available.
+	 */
+	if (int_ops && int_ops->init_state) {
+		if (sd->ctrl_handler)
+			sd->state_lock = sd->ctrl_handler->lock;
+
+		ret = v4l2_subdev_init_finalize(sd);
+		if (ret) {
+			dev_err(v4l2_dev->dev,
+				"%s: subdev initialization failed (err=%d)\n",
+				name, ret);
+			goto err_clean_m_ent;
+		}
+	}
+
+	/* Register the subdev with the v4l2 and the media framework */
+	ret = v4l2_device_register_subdev(v4l2_dev, sd);
+	if (ret) {
+		dev_err(v4l2_dev->dev,
+			"%s: subdev register failed (err=%d)\n",
+			name, ret);
+		goto err_clean_sd;
+	}
+
+	return 0;
+
+err_clean_sd:
+	v4l2_subdev_cleanup(sd);
+err_clean_m_ent:
+	media_entity_cleanup(&sd->entity);
+	return ret;
+}

linux-7.2.3/drivers/media/test-drivers/vimc/vimc-common.h

@@ -0,0 +1,284 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * vimc-common.h Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ */
+
+#ifndef _VIMC_COMMON_H_
+#define _VIMC_COMMON_H_
+
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <media/media-device.h>
+#include <media/v4l2-device.h>
+#include <media/tpg/v4l2-tpg.h>
+#include <media/v4l2-ctrls.h>
+
+#define VIMC_PDEV_NAME "vimc"
+
+/* VIMC-specific controls */
+#define VIMC_CID_VIMC_BASE		(0x00f00000 | 0xf000)
+#define VIMC_CID_VIMC_CLASS		(0x00f00000 | 1)
+#define VIMC_CID_TEST_PATTERN		(VIMC_CID_VIMC_BASE + 0)
+#define VIMC_CID_MEAN_WIN_SIZE		(VIMC_CID_VIMC_BASE + 1)
+#define VIMC_CID_OSD_TEXT_MODE		(VIMC_CID_VIMC_BASE + 2)
+
+#define VIMC_FRAME_MAX_WIDTH 4096
+#define VIMC_FRAME_MAX_HEIGHT 2160
+#define VIMC_FRAME_MIN_WIDTH 16
+#define VIMC_FRAME_MIN_HEIGHT 16
+
+#define VIMC_PIXEL_RATE_FIXED		160000000	/* 160 MHz */
+#define VIMC_HBLANK_FIXED		800
+/* VBLANK - vertical blanking (primary FPS control) */
+#define VIMC_VBLANK_MIN			4
+#define VIMC_VBLANK_MAX			65535
+#define VIMC_VBLANK_STEP		1
+#define VIMC_VBLANK_DEFAULT	        3223           /* 30fps vga */
+#define VIMC_PIXELS_THRESHOLD_30FPS	(1920 * 1080) /* 2073600 pixels */
+
+#define VIMC_FRAME_INDEX(lin, col, width, bpp) ((lin * width + col) * bpp)
+
+/* Source and sink pad checks */
+#define VIMC_IS_SRC(pad)	(pad)
+#define VIMC_IS_SINK(pad)	(!(pad))
+
+#define VIMC_PIX_FMT_MAX_CODES 8
+
+extern unsigned int vimc_allocator;
+
+enum vimc_allocator_type {
+	VIMC_ALLOCATOR_VMALLOC = 0,
+	VIMC_ALLOCATOR_DMA_CONTIG = 1,
+};
+
+/**
+ * vimc_colorimetry_clamp - Adjust colorimetry parameters
+ *
+ * @fmt:		the pointer to struct v4l2_pix_format or
+ *			struct v4l2_mbus_framefmt
+ *
+ * Entities must check if colorimetry given by the userspace is valid, if not
+ * then set them as DEFAULT
+ */
+#define vimc_colorimetry_clamp(fmt)					\
+do {									\
+	if ((fmt)->colorspace == V4L2_COLORSPACE_DEFAULT		\
+	    || (fmt)->colorspace > V4L2_COLORSPACE_DCI_P3) {		\
+		(fmt)->colorspace = V4L2_COLORSPACE_DEFAULT;		\
+		(fmt)->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;		\
+		(fmt)->quantization = V4L2_QUANTIZATION_DEFAULT;	\
+		(fmt)->xfer_func = V4L2_XFER_FUNC_DEFAULT;		\
+	}								\
+	if ((fmt)->ycbcr_enc > V4L2_YCBCR_ENC_SMPTE240M)		\
+		(fmt)->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;		\
+	if ((fmt)->quantization > V4L2_QUANTIZATION_LIM_RANGE)		\
+		(fmt)->quantization = V4L2_QUANTIZATION_DEFAULT;	\
+	if ((fmt)->xfer_func > V4L2_XFER_FUNC_SMPTE2084)		\
+		(fmt)->xfer_func = V4L2_XFER_FUNC_DEFAULT;		\
+} while (0)
+
+/**
+ * struct vimc_pix_map - maps media bus code with v4l2 pixel format
+ *
+ * @code:		media bus format code defined by MEDIA_BUS_FMT_* macros
+ * @bpp:		number of bytes each pixel occupies
+ * @pixelformat:	pixel format defined by V4L2_PIX_FMT_* macros
+ * @bayer:		true if this is a bayer format
+ *
+ * Struct which matches the MEDIA_BUS_FMT_* codes with the corresponding
+ * V4L2_PIX_FMT_* fourcc pixelformat and its bytes per pixel (bpp)
+ */
+struct vimc_pix_map {
+	unsigned int code[VIMC_PIX_FMT_MAX_CODES];
+	unsigned int bpp;
+	u32 pixelformat;
+	bool bayer;
+};
+
+/**
+ * struct vimc_ent_device - core struct that represents an entity in the
+ * topology
+ *
+ * @dev:		a pointer of the device struct of the driver
+ * @ent:		the pointer to struct media_entity for the node
+ * @process_frame:	callback send a frame to that node
+ * @vdev_get_format:	callback that returns the current format a pad, used
+ *			only when is_media_entity_v4l2_video_device(ent) returns
+ *			true
+ *
+ * Each node of the topology must create a vimc_ent_device struct. Depending on
+ * the node it will be of an instance of v4l2_subdev or video_device struct
+ * where both contains a struct media_entity.
+ * Those structures should embedded the vimc_ent_device struct through
+ * v4l2_set_subdevdata() and video_set_drvdata() respectively, allowing the
+ * vimc_ent_device struct to be retrieved from the corresponding struct
+ * media_entity
+ */
+struct vimc_ent_device {
+	struct device *dev;
+	struct media_entity *ent;
+	void * (*process_frame)(struct vimc_ent_device *ved,
+				const void *frame);
+	void (*vdev_get_format)(struct vimc_ent_device *ved,
+			      struct v4l2_pix_format *fmt);
+};
+
+/**
+ * struct vimc_device - main device for vimc driver
+ *
+ * @pipe_cfg:	pointer to the vimc pipeline configuration structure
+ * @ent_devs:	array of vimc_ent_device pointers
+ * @mdev:	the associated media_device parent
+ * @v4l2_dev:	Internal v4l2 parent device
+ */
+struct vimc_device {
+	const struct vimc_pipeline_config *pipe_cfg;
+	struct vimc_ent_device **ent_devs;
+	struct media_device mdev;
+	struct v4l2_device v4l2_dev;
+};
+
+/**
+ * struct vimc_ent_type		Structure for the callbacks of the entity types
+ *
+ *
+ * @add:			initializes and registers
+ *				vimc entity - called from vimc-core
+ * @unregister:			unregisters vimc entity - called from vimc-core
+ * @release:			releases vimc entity - called from the v4l2_dev
+ *				release callback
+ */
+struct vimc_ent_type {
+	struct vimc_ent_device *(*add)(struct vimc_device *vimc,
+				       const char *vcfg_name);
+	void (*unregister)(struct vimc_ent_device *ved);
+	void (*release)(struct vimc_ent_device *ved);
+};
+
+/**
+ * struct vimc_ent_config	Structure which describes individual
+ *				configuration for each entity
+ *
+ * @name:			entity name
+ * @type:			contain the callbacks of this entity type
+ *
+ */
+struct vimc_ent_config {
+	const char *name;
+	const struct vimc_ent_type *type;
+};
+
+enum vimc_sensor_osd_mode {
+	VIMC_SENSOR_OSD_SHOW_ALL = 0,
+	VIMC_SENSOR_OSD_SHOW_COUNTERS = 1,
+	VIMC_SENSOR_OSD_SHOW_NONE = 2
+};
+
+struct vimc_sensor_device {
+	struct vimc_ent_device ved;
+	struct v4l2_subdev sd;
+	struct tpg_data tpg;
+	struct v4l2_ctrl_handler hdl;
+	struct media_pad pad;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *vblank;
+
+	u8 *frame;
+
+	/*
+	 * Virtual "hardware" configuration, filled when the stream starts or
+	 * when controls are set.
+	 */
+	struct {
+		struct v4l2_area size;
+		enum vimc_sensor_osd_mode osd_value;
+		u64 start_stream_ts;
+		unsigned long fps_jiffies;
+	} hw;
+};
+
+/**
+ * vimc_is_source - returns true if the entity has only source pads
+ *
+ * @ent: pointer to &struct media_entity
+ *
+ */
+bool vimc_is_source(struct media_entity *ent);
+
+extern const struct vimc_ent_type vimc_sensor_type;
+extern const struct vimc_ent_type vimc_debayer_type;
+extern const struct vimc_ent_type vimc_scaler_type;
+extern const struct vimc_ent_type vimc_capture_type;
+extern const struct vimc_ent_type vimc_lens_type;
+
+/**
+ * vimc_pix_map_by_index - get vimc_pix_map struct by its index
+ *
+ * @i:			index of the vimc_pix_map struct in vimc_pix_map_list
+ */
+const struct vimc_pix_map *vimc_pix_map_by_index(unsigned int i);
+
+/**
+ * vimc_mbus_code_by_index - get mbus code by its index
+ *
+ * @index:		index of the mbus code in vimc_pix_map_list
+ *
+ * Returns 0 if no mbus code is found for the given index.
+ */
+u32 vimc_mbus_code_by_index(unsigned int index);
+
+/**
+ * vimc_pix_map_by_code - get vimc_pix_map struct by media bus code
+ *
+ * @code:		media bus format code defined by MEDIA_BUS_FMT_* macros
+ */
+const struct vimc_pix_map *vimc_pix_map_by_code(u32 code);
+
+/**
+ * vimc_pix_map_by_pixelformat - get vimc_pix_map struct by v4l2 pixel format
+ *
+ * @pixelformat:	pixel format defined by V4L2_PIX_FMT_* macros
+ */
+const struct vimc_pix_map *vimc_pix_map_by_pixelformat(u32 pixelformat);
+
+/**
+ * vimc_ent_sd_register - initialize and register a subdev node
+ *
+ * @ved:	the vimc_ent_device struct to be initialize
+ * @sd:		the v4l2_subdev struct to be initialize and registered
+ * @v4l2_dev:	the v4l2 device to register the v4l2_subdev
+ * @name:	name of the sub-device. Please notice that the name must be
+ *		unique.
+ * @function:	media entity function defined by MEDIA_ENT_F_* macros
+ * @num_pads:	number of pads to initialize
+ * @pads:	the array of pads of the entity, the caller should set the
+ *		flags of the pads
+ * @int_ops:	pointer to &struct v4l2_subdev_internal_ops.
+ * @sd_ops:	pointer to &struct v4l2_subdev_ops.
+ *
+ * Helper function initialize and register the struct vimc_ent_device and struct
+ * v4l2_subdev which represents a subdev node in the topology
+ */
+int vimc_ent_sd_register(struct vimc_ent_device *ved,
+			 struct v4l2_subdev *sd,
+			 struct v4l2_device *v4l2_dev,
+			 const char *const name,
+			 u32 function,
+			 u16 num_pads,
+			 struct media_pad *pads,
+			 const struct v4l2_subdev_internal_ops *int_ops,
+			 const struct v4l2_subdev_ops *sd_ops);
+
+/**
+ * vimc_vdev_link_validate - validates a media link
+ *
+ * @link: pointer to &struct media_link
+ *
+ * This function calls validates if a media link is valid for streaming.
+ */
+int vimc_vdev_link_validate(struct media_link *link);
+
+#endif

linux-7.2.3/drivers/media/test-drivers/vimc/vimc-core.c

@@ -0,0 +1,451 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * vimc-core.c Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ */
+
+#include <linux/dma-mapping.h>
+#include <linux/font.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <media/media-device.h>
+#include <media/tpg/v4l2-tpg.h>
+#include <media/v4l2-device.h>
+
+#include "vimc-common.h"
+
+unsigned int vimc_allocator;
+module_param_named(allocator, vimc_allocator, uint, 0444);
+MODULE_PARM_DESC(allocator, " memory allocator selection, default is 0.\n"
+			     "\t\t    0 == vmalloc\n"
+			     "\t\t    1 == dma-contig");
+
+#define VIMC_MDEV_MODEL_NAME "VIMC MDEV"
+
+#define VIMC_DATA_LINK(src, srcpad, sink, sinkpad, link_flags) {	\
+	.src_ent = src,						\
+	.src_pad = srcpad,					\
+	.sink_ent = sink,					\
+	.sink_pad = sinkpad,					\
+	.flags = link_flags,					\
+}
+
+#define VIMC_ANCILLARY_LINK(primary, ancillary) {	\
+	.primary_ent = primary,			\
+	.ancillary_ent = ancillary		\
+}
+
+/* Structure which describes data links between entities */
+struct vimc_data_link {
+	unsigned int src_ent;
+	u16 src_pad;
+	unsigned int sink_ent;
+	u16 sink_pad;
+	u32 flags;
+};
+
+/* Enum to improve clarity when defining vimc_data_links */
+enum vimc_data_link_ents {
+	SENSOR_A,
+	SENSOR_B,
+	DEBAYER_A,
+	DEBAYER_B,
+	RAW_CAPTURE_0,
+	RAW_CAPTURE_1,
+	RGB_YUV_INPUT,
+	SCALER,
+	RGB_YUV_CAPTURE,
+	LENS_A,
+	LENS_B,
+};
+
+/* Structure which describes ancillary links between entities */
+struct vimc_ancillary_link {
+	unsigned int primary_ent;
+	unsigned int ancillary_ent;
+};
+
+/* Structure which describes the whole topology */
+struct vimc_pipeline_config {
+	const struct vimc_ent_config *ents;
+	size_t num_ents;
+	const struct vimc_data_link *data_links;
+	size_t num_data_links;
+	const struct vimc_ancillary_link *ancillary_links;
+	size_t num_ancillary_links;
+};
+
+/* --------------------------------------------------------------------------
+ * Topology Configuration
+ */
+
+static const struct vimc_ent_config ent_config[] = {
+	[SENSOR_A] = {
+		.name = "Sensor A",
+		.type = &vimc_sensor_type
+	},
+	[SENSOR_B] = {
+		.name = "Sensor B",
+		.type = &vimc_sensor_type
+	},
+	[DEBAYER_A] = {
+		.name = "Debayer A",
+		.type = &vimc_debayer_type
+	},
+	[DEBAYER_B] = {
+		.name = "Debayer B",
+		.type = &vimc_debayer_type
+	},
+	[RAW_CAPTURE_0] = {
+		.name = "Raw Capture 0",
+		.type = &vimc_capture_type
+	},
+	[RAW_CAPTURE_1] = {
+		.name = "Raw Capture 1",
+		.type = &vimc_capture_type
+	},
+	[RGB_YUV_INPUT] = {
+		/* TODO: change this to vimc-input when it is implemented */
+		.name = "RGB/YUV Input",
+		.type = &vimc_sensor_type
+	},
+	[SCALER] = {
+		.name = "Scaler",
+		.type = &vimc_scaler_type
+	},
+	[RGB_YUV_CAPTURE] = {
+		.name = "RGB/YUV Capture",
+		.type = &vimc_capture_type
+	},
+	[LENS_A] = {
+		.name = "Lens A",
+		.type = &vimc_lens_type
+	},
+	[LENS_B] = {
+		.name = "Lens B",
+		.type = &vimc_lens_type
+	},
+};
+
+static const struct vimc_data_link data_links[] = {
+	/* Link: Sensor A (Pad 0)->(Pad 0) Debayer A */
+	VIMC_DATA_LINK(SENSOR_A, 0, DEBAYER_A, 0,
+		       MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE),
+	/* Link: Sensor A (Pad 0)->(Pad 0) Raw Capture 0 */
+	VIMC_DATA_LINK(SENSOR_A, 0, RAW_CAPTURE_0, 0,
+		       MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE),
+	/* Link: Sensor B (Pad 0)->(Pad 0) Debayer B */
+	VIMC_DATA_LINK(SENSOR_B, 0, DEBAYER_B, 0,
+		       MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE),
+	/* Link: Sensor B (Pad 0)->(Pad 0) Raw Capture 1 */
+	VIMC_DATA_LINK(SENSOR_B, 0, RAW_CAPTURE_1, 0,
+		       MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE),
+	/* Link: Debayer A (Pad 1)->(Pad 0) Scaler */
+	VIMC_DATA_LINK(DEBAYER_A, 1, SCALER, 0, MEDIA_LNK_FL_ENABLED),
+	/* Link: Debayer B (Pad 1)->(Pad 0) Scaler */
+	VIMC_DATA_LINK(DEBAYER_B, 1, SCALER, 0, 0),
+	/* Link: RGB/YUV Input (Pad 0)->(Pad 0) Scaler */
+	VIMC_DATA_LINK(RGB_YUV_INPUT, 0, SCALER, 0, 0),
+	/* Link: Scaler (Pad 1)->(Pad 0) RGB/YUV Capture */
+	VIMC_DATA_LINK(SCALER, 1, RGB_YUV_CAPTURE, 0,
+		       MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE),
+};
+
+static const struct vimc_ancillary_link ancillary_links[] = {
+	/* Link: Sensor A -> Lens A */
+	VIMC_ANCILLARY_LINK(0, 9),
+	/* Link: Sensor B -> Lens B */
+	VIMC_ANCILLARY_LINK(1, 10),
+};
+
+static struct vimc_pipeline_config pipe_cfg = {
+	.ents		     = ent_config,
+	.num_ents	     = ARRAY_SIZE(ent_config),
+	.data_links	     = data_links,
+	.num_data_links	     = ARRAY_SIZE(data_links),
+	.ancillary_links     = ancillary_links,
+	.num_ancillary_links = ARRAY_SIZE(ancillary_links),
+};
+
+/* -------------------------------------------------------------------------- */
+
+static void vimc_rm_links(struct vimc_device *vimc)
+{
+	unsigned int i;
+
+	for (i = 0; i < vimc->pipe_cfg->num_ents; i++)
+		media_entity_remove_links(vimc->ent_devs[i]->ent);
+}
+
+static int vimc_create_links(struct vimc_device *vimc)
+{
+	unsigned int i;
+	int ret;
+
+	/* Initialize the links between entities */
+	for (i = 0; i < vimc->pipe_cfg->num_data_links; i++) {
+		const struct vimc_data_link *link = &vimc->pipe_cfg->data_links[i];
+
+		struct vimc_ent_device *ved_src =
+			vimc->ent_devs[link->src_ent];
+		struct vimc_ent_device *ved_sink =
+			vimc->ent_devs[link->sink_ent];
+
+		ret = media_create_pad_link(ved_src->ent, link->src_pad,
+					    ved_sink->ent, link->sink_pad,
+					    link->flags);
+		if (ret)
+			goto err_rm_links;
+	}
+
+	for (i = 0; i < vimc->pipe_cfg->num_ancillary_links; i++) {
+		const struct vimc_ancillary_link *link = &vimc->pipe_cfg->ancillary_links[i];
+
+		struct vimc_ent_device *ved_primary =
+			vimc->ent_devs[link->primary_ent];
+		struct vimc_ent_device *ved_ancillary =
+			vimc->ent_devs[link->ancillary_ent];
+		struct media_link *ret_link =
+			media_create_ancillary_link(ved_primary->ent, ved_ancillary->ent);
+
+		if (IS_ERR(ret_link)) {
+			ret = PTR_ERR(ret_link);
+			goto err_rm_links;
+		}
+	}
+
+	return 0;
+
+err_rm_links:
+	vimc_rm_links(vimc);
+	return ret;
+}
+
+static void vimc_release_subdevs(struct vimc_device *vimc)
+{
+	unsigned int i;
+
+	for (i = 0; i < vimc->pipe_cfg->num_ents; i++)
+		if (vimc->ent_devs[i])
+			vimc->pipe_cfg->ents[i].type->release(vimc->ent_devs[i]);
+}
+
+static void vimc_unregister_subdevs(struct vimc_device *vimc)
+{
+	unsigned int i;
+
+	for (i = 0; i < vimc->pipe_cfg->num_ents; i++)
+		if (vimc->ent_devs[i] && vimc->pipe_cfg->ents[i].type->unregister)
+			vimc->pipe_cfg->ents[i].type->unregister(vimc->ent_devs[i]);
+}
+
+static int vimc_add_subdevs(struct vimc_device *vimc)
+{
+	unsigned int i;
+
+	for (i = 0; i < vimc->pipe_cfg->num_ents; i++) {
+		dev_dbg(vimc->mdev.dev, "new entity for %s\n",
+			vimc->pipe_cfg->ents[i].name);
+		vimc->ent_devs[i] = vimc->pipe_cfg->ents[i].type->add(vimc,
+					vimc->pipe_cfg->ents[i].name);
+		if (IS_ERR(vimc->ent_devs[i])) {
+			int err = PTR_ERR(vimc->ent_devs[i]);
+
+			dev_err(vimc->mdev.dev, "adding entity %s failed (%d)\n",
+				vimc->pipe_cfg->ents[i].name, err);
+			vimc->ent_devs[i] = NULL;
+			vimc_unregister_subdevs(vimc);
+			vimc_release_subdevs(vimc);
+			return err;
+		}
+	}
+	return 0;
+}
+
+static void vimc_v4l2_dev_release(struct v4l2_device *v4l2_dev)
+{
+	struct vimc_device *vimc =
+		container_of(v4l2_dev, struct vimc_device, v4l2_dev);
+
+	vimc_release_subdevs(vimc);
+	media_device_cleanup(&vimc->mdev);
+	kfree(vimc->ent_devs);
+	kfree(vimc);
+}
+
+static int vimc_register_devices(struct vimc_device *vimc)
+{
+	int ret;
+
+	/* Register the v4l2 struct */
+	ret = v4l2_device_register(vimc->mdev.dev, &vimc->v4l2_dev);
+	if (ret) {
+		dev_err(vimc->mdev.dev,
+			"v4l2 device register failed (err=%d)\n", ret);
+		return ret;
+	}
+	/* allocate ent_devs */
+	vimc->ent_devs = kzalloc_objs(*vimc->ent_devs, vimc->pipe_cfg->num_ents);
+	if (!vimc->ent_devs) {
+		ret = -ENOMEM;
+		goto err_v4l2_unregister;
+	}
+
+	/* Invoke entity config hooks to initialize and register subdevs */
+	ret = vimc_add_subdevs(vimc);
+	if (ret)
+		goto err_free_ent_devs;
+
+	/* Initialize links */
+	ret = vimc_create_links(vimc);
+	if (ret)
+		goto err_rm_subdevs;
+
+	/* Register the media device */
+	ret = media_device_register(&vimc->mdev);
+	if (ret) {
+		dev_err(vimc->mdev.dev,
+			"media device register failed (err=%d)\n", ret);
+		goto err_rm_subdevs;
+	}
+
+	/* Expose all subdev's nodes*/
+	ret = v4l2_device_register_subdev_nodes(&vimc->v4l2_dev);
+	if (ret) {
+		dev_err(vimc->mdev.dev,
+			"vimc subdev nodes registration failed (err=%d)\n",
+			ret);
+		goto err_mdev_unregister;
+	}
+
+	return 0;
+
+err_mdev_unregister:
+	media_device_unregister(&vimc->mdev);
+err_rm_subdevs:
+	vimc_unregister_subdevs(vimc);
+	vimc_release_subdevs(vimc);
+err_free_ent_devs:
+	kfree(vimc->ent_devs);
+err_v4l2_unregister:
+	v4l2_device_unregister(&vimc->v4l2_dev);
+
+	return ret;
+}
+
+static int vimc_probe(struct platform_device *pdev)
+{
+	const struct font_desc *font = find_font("VGA8x16");
+	struct vimc_device *vimc;
+	int ret;
+
+	dev_dbg(&pdev->dev, "probe");
+
+	if (!font) {
+		dev_err(&pdev->dev, "could not find font\n");
+		return -ENODEV;
+	}
+
+	tpg_set_font(font->data);
+
+	if (vimc_allocator == VIMC_ALLOCATOR_DMA_CONTIG)
+		dma_coerce_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(32));
+
+	vimc = kzalloc_obj(*vimc);
+	if (!vimc)
+		return -ENOMEM;
+
+	vimc->pipe_cfg = &pipe_cfg;
+
+	/* Link the media device within the v4l2_device */
+	vimc->v4l2_dev.mdev = &vimc->mdev;
+
+	/* Initialize media device */
+	strscpy(vimc->mdev.model, VIMC_MDEV_MODEL_NAME,
+		sizeof(vimc->mdev.model));
+	vimc->mdev.dev = &pdev->dev;
+	media_device_init(&vimc->mdev);
+
+	ret = vimc_register_devices(vimc);
+	if (ret) {
+		media_device_cleanup(&vimc->mdev);
+		kfree(vimc);
+		return ret;
+	}
+	/*
+	 * the release cb is set only after successful registration.
+	 * if the registration fails, we release directly from probe
+	 */
+
+	vimc->v4l2_dev.release = vimc_v4l2_dev_release;
+	platform_set_drvdata(pdev, vimc);
+	return 0;
+}
+
+static void vimc_remove(struct platform_device *pdev)
+{
+	struct vimc_device *vimc = platform_get_drvdata(pdev);
+
+	dev_dbg(&pdev->dev, "remove");
+
+	vimc_unregister_subdevs(vimc);
+	media_device_unregister(&vimc->mdev);
+	v4l2_device_unregister(&vimc->v4l2_dev);
+	v4l2_device_put(&vimc->v4l2_dev);
+}
+
+static void vimc_dev_release(struct device *dev)
+{
+}
+
+static struct platform_device vimc_pdev = {
+	.name = VIMC_PDEV_NAME,
+	.dev.release = vimc_dev_release,
+};
+
+static struct platform_driver vimc_pdrv = {
+	.probe		= vimc_probe,
+	.remove		= vimc_remove,
+	.driver		= {
+		.name	= VIMC_PDEV_NAME,
+	},
+};
+
+static int __init vimc_init(void)
+{
+	int ret;
+
+	ret = platform_device_register(&vimc_pdev);
+	if (ret) {
+		dev_err(&vimc_pdev.dev,
+			"platform device registration failed (err=%d)\n", ret);
+		platform_device_put(&vimc_pdev);
+		return ret;
+	}
+
+	ret = platform_driver_register(&vimc_pdrv);
+	if (ret) {
+		dev_err(&vimc_pdev.dev,
+			"platform driver registration failed (err=%d)\n", ret);
+		platform_device_unregister(&vimc_pdev);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void __exit vimc_exit(void)
+{
+	platform_driver_unregister(&vimc_pdrv);
+
+	platform_device_unregister(&vimc_pdev);
+}
+
+module_init(vimc_init);
+module_exit(vimc_exit);
+
+MODULE_DESCRIPTION("Virtual Media Controller Driver (VIMC)");
+MODULE_AUTHOR("Helen Fornazier <helen.fornazier@gmail.com>");
+MODULE_LICENSE("GPL");

linux-7.2.3/drivers/media/test-drivers/vimc/vimc-debayer.c

@@ -0,0 +1,612 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * vimc-debayer.c Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ */
+
+#include <linux/moduleparam.h>
+#include <linux/platform_device.h>
+#include <linux/vmalloc.h>
+#include <linux/v4l2-mediabus.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-subdev.h>
+
+#include "vimc-common.h"
+
+/* TODO: Add support for more output formats, we only support RGB888 for now. */
+#define VIMC_DEBAYER_SOURCE_MBUS_FMT	MEDIA_BUS_FMT_RGB888_1X24
+
+enum vimc_debayer_rgb_colors {
+	VIMC_DEBAYER_RED = 0,
+	VIMC_DEBAYER_GREEN = 1,
+	VIMC_DEBAYER_BLUE = 2,
+};
+
+struct vimc_debayer_pix_map {
+	u32 code;
+	enum vimc_debayer_rgb_colors order[2][2];
+};
+
+struct vimc_debayer_device {
+	struct vimc_ent_device ved;
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	struct media_pad pads[2];
+
+	u8 *src_frame;
+
+	void (*set_rgb_src)(struct vimc_debayer_device *vdebayer,
+			    unsigned int lin, unsigned int col,
+			    unsigned int rgb[3]);
+
+	/*
+	 * Virtual "hardware" configuration, filled when the stream starts or
+	 * when controls are set.
+	 */
+	struct {
+		const struct vimc_debayer_pix_map *sink_pix_map;
+		unsigned int sink_bpp;
+		struct v4l2_area size;
+		unsigned int mean_win_size;
+		u32 src_code;
+	} hw;
+};
+
+static const struct v4l2_mbus_framefmt sink_fmt_default = {
+	.width = 640,
+	.height = 480,
+	.code = MEDIA_BUS_FMT_SRGGB8_1X8,
+	.field = V4L2_FIELD_NONE,
+	.colorspace = V4L2_COLORSPACE_SRGB,
+};
+
+static const u32 vimc_debayer_src_mbus_codes[] = {
+	MEDIA_BUS_FMT_GBR888_1X24,
+	MEDIA_BUS_FMT_BGR888_1X24,
+	MEDIA_BUS_FMT_BGR888_3X8,
+	MEDIA_BUS_FMT_RGB888_1X24,
+	MEDIA_BUS_FMT_RGB888_2X12_BE,
+	MEDIA_BUS_FMT_RGB888_2X12_LE,
+	MEDIA_BUS_FMT_RGB888_3X8,
+	MEDIA_BUS_FMT_RGB888_1X7X4_SPWG,
+	MEDIA_BUS_FMT_RGB888_1X7X4_JEIDA,
+	MEDIA_BUS_FMT_RGB888_1X32_PADHI,
+};
+
+static const struct vimc_debayer_pix_map vimc_debayer_pix_map_list[] = {
+	{
+		.code = MEDIA_BUS_FMT_SBGGR8_1X8,
+		.order = { { VIMC_DEBAYER_BLUE, VIMC_DEBAYER_GREEN },
+			   { VIMC_DEBAYER_GREEN, VIMC_DEBAYER_RED } }
+	},
+	{
+		.code = MEDIA_BUS_FMT_SGBRG8_1X8,
+		.order = { { VIMC_DEBAYER_GREEN, VIMC_DEBAYER_BLUE },
+			   { VIMC_DEBAYER_RED, VIMC_DEBAYER_GREEN } }
+	},
+	{
+		.code = MEDIA_BUS_FMT_SGRBG8_1X8,
+		.order = { { VIMC_DEBAYER_GREEN, VIMC_DEBAYER_RED },
+			   { VIMC_DEBAYER_BLUE, VIMC_DEBAYER_GREEN } }
+	},
+	{
+		.code = MEDIA_BUS_FMT_SRGGB8_1X8,
+		.order = { { VIMC_DEBAYER_RED, VIMC_DEBAYER_GREEN },
+			   { VIMC_DEBAYER_GREEN, VIMC_DEBAYER_BLUE } }
+	},
+	{
+		.code = MEDIA_BUS_FMT_SBGGR10_1X10,
+		.order = { { VIMC_DEBAYER_BLUE, VIMC_DEBAYER_GREEN },
+			   { VIMC_DEBAYER_GREEN, VIMC_DEBAYER_RED } }
+	},
+	{
+		.code = MEDIA_BUS_FMT_SGBRG10_1X10,
+		.order = { { VIMC_DEBAYER_GREEN, VIMC_DEBAYER_BLUE },
+			   { VIMC_DEBAYER_RED, VIMC_DEBAYER_GREEN } }
+	},
+	{
+		.code = MEDIA_BUS_FMT_SGRBG10_1X10,
+		.order = { { VIMC_DEBAYER_GREEN, VIMC_DEBAYER_RED },
+			   { VIMC_DEBAYER_BLUE, VIMC_DEBAYER_GREEN } }
+	},
+	{
+		.code = MEDIA_BUS_FMT_SRGGB10_1X10,
+		.order = { { VIMC_DEBAYER_RED, VIMC_DEBAYER_GREEN },
+			   { VIMC_DEBAYER_GREEN, VIMC_DEBAYER_BLUE } }
+	},
+	{
+		.code = MEDIA_BUS_FMT_SBGGR12_1X12,
+		.order = { { VIMC_DEBAYER_BLUE, VIMC_DEBAYER_GREEN },
+			   { VIMC_DEBAYER_GREEN, VIMC_DEBAYER_RED } }
+	},
+	{
+		.code = MEDIA_BUS_FMT_SGBRG12_1X12,
+		.order = { { VIMC_DEBAYER_GREEN, VIMC_DEBAYER_BLUE },
+			   { VIMC_DEBAYER_RED, VIMC_DEBAYER_GREEN } }
+	},
+	{
+		.code = MEDIA_BUS_FMT_SGRBG12_1X12,
+		.order = { { VIMC_DEBAYER_GREEN, VIMC_DEBAYER_RED },
+			   { VIMC_DEBAYER_BLUE, VIMC_DEBAYER_GREEN } }
+	},
+	{
+		.code = MEDIA_BUS_FMT_SRGGB12_1X12,
+		.order = { { VIMC_DEBAYER_RED, VIMC_DEBAYER_GREEN },
+			   { VIMC_DEBAYER_GREEN, VIMC_DEBAYER_BLUE } }
+	},
+};
+
+static const struct vimc_debayer_pix_map *vimc_debayer_pix_map_by_code(u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(vimc_debayer_pix_map_list); i++)
+		if (vimc_debayer_pix_map_list[i].code == code)
+			return &vimc_debayer_pix_map_list[i];
+
+	return NULL;
+}
+
+static bool vimc_debayer_src_code_is_valid(u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(vimc_debayer_src_mbus_codes); i++)
+		if (vimc_debayer_src_mbus_codes[i] == code)
+			return true;
+
+	return false;
+}
+
+static int vimc_debayer_init_state(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state)
+{
+	struct v4l2_mbus_framefmt *mf;
+
+	mf = v4l2_subdev_state_get_format(sd_state, 0);
+	*mf = sink_fmt_default;
+
+	mf = v4l2_subdev_state_get_format(sd_state, 1);
+	*mf = sink_fmt_default;
+	mf->code = VIMC_DEBAYER_SOURCE_MBUS_FMT;
+
+	return 0;
+}
+
+static int vimc_debayer_enum_mbus_code(struct v4l2_subdev *sd,
+				       struct v4l2_subdev_state *sd_state,
+				       struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (VIMC_IS_SRC(code->pad)) {
+		if (code->index >= ARRAY_SIZE(vimc_debayer_src_mbus_codes))
+			return -EINVAL;
+
+		code->code = vimc_debayer_src_mbus_codes[code->index];
+	} else {
+		if (code->index >= ARRAY_SIZE(vimc_debayer_pix_map_list))
+			return -EINVAL;
+
+		code->code = vimc_debayer_pix_map_list[code->index].code;
+	}
+
+	return 0;
+}
+
+static int vimc_debayer_enum_frame_size(struct v4l2_subdev *sd,
+					struct v4l2_subdev_state *sd_state,
+					struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index)
+		return -EINVAL;
+
+	if (VIMC_IS_SINK(fse->pad)) {
+		const struct vimc_debayer_pix_map *vpix =
+			vimc_debayer_pix_map_by_code(fse->code);
+
+		if (!vpix)
+			return -EINVAL;
+	} else if (!vimc_debayer_src_code_is_valid(fse->code)) {
+		return -EINVAL;
+	}
+
+	fse->min_width = VIMC_FRAME_MIN_WIDTH;
+	fse->max_width = VIMC_FRAME_MAX_WIDTH;
+	fse->min_height = VIMC_FRAME_MIN_HEIGHT;
+	fse->max_height = VIMC_FRAME_MAX_HEIGHT;
+
+	return 0;
+}
+
+static void vimc_debayer_adjust_sink_fmt(struct v4l2_mbus_framefmt *fmt)
+{
+	const struct vimc_debayer_pix_map *vpix;
+
+	/* Don't accept a code that is not on the debayer table */
+	vpix = vimc_debayer_pix_map_by_code(fmt->code);
+	if (!vpix)
+		fmt->code = sink_fmt_default.code;
+
+	fmt->width = clamp_t(u32, fmt->width, VIMC_FRAME_MIN_WIDTH,
+			     VIMC_FRAME_MAX_WIDTH) & ~1;
+	fmt->height = clamp_t(u32, fmt->height, VIMC_FRAME_MIN_HEIGHT,
+			      VIMC_FRAME_MAX_HEIGHT) & ~1;
+
+	if (fmt->field == V4L2_FIELD_ANY)
+		fmt->field = sink_fmt_default.field;
+
+	vimc_colorimetry_clamp(fmt);
+}
+
+static int vimc_debayer_set_fmt(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_format *fmt)
+{
+	struct vimc_debayer_device *vdebayer = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	/* Do not change the format while stream is on. */
+	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE && vdebayer->src_frame)
+		return -EBUSY;
+
+	/*
+	 * Do not change the format of the source pad, it is propagated from
+	 * the sink.
+	 */
+	if (VIMC_IS_SRC(fmt->pad))
+		return v4l2_subdev_get_fmt(sd, sd_state, fmt);
+
+	/* Set the new format in the sink pad. */
+	vimc_debayer_adjust_sink_fmt(&fmt->format);
+
+	format = v4l2_subdev_state_get_format(sd_state, 0);
+
+	dev_dbg(vdebayer->ved.dev, "%s: sink format update: "
+		"old:%dx%d (0x%x, %d, %d, %d, %d) "
+		"new:%dx%d (0x%x, %d, %d, %d, %d)\n", vdebayer->sd.name,
+		/* old */
+		format->width, format->height, format->code,
+		format->colorspace, format->quantization,
+		format->xfer_func, format->ycbcr_enc,
+		/* new */
+		fmt->format.width, fmt->format.height, fmt->format.code,
+		fmt->format.colorspace,	fmt->format.quantization,
+		fmt->format.xfer_func, fmt->format.ycbcr_enc);
+
+	*format = fmt->format;
+
+	/* Propagate the format to the source pad. */
+	format = v4l2_subdev_state_get_format(sd_state, 1);
+	*format = fmt->format;
+	format->code = VIMC_DEBAYER_SOURCE_MBUS_FMT;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops vimc_debayer_pad_ops = {
+	.enum_mbus_code		= vimc_debayer_enum_mbus_code,
+	.enum_frame_size	= vimc_debayer_enum_frame_size,
+	.get_fmt		= v4l2_subdev_get_fmt,
+	.set_fmt		= vimc_debayer_set_fmt,
+};
+
+static void vimc_debayer_process_rgb_frame(struct vimc_debayer_device *vdebayer,
+					   unsigned int lin,
+					   unsigned int col,
+					   unsigned int rgb[3])
+{
+	const struct vimc_pix_map *vpix;
+	unsigned int i, index;
+
+	vpix = vimc_pix_map_by_code(vdebayer->hw.src_code);
+	index = VIMC_FRAME_INDEX(lin, col, vdebayer->hw.size.width, 3);
+	for (i = 0; i < 3; i++) {
+		switch (vpix->pixelformat) {
+		case V4L2_PIX_FMT_RGB24:
+			vdebayer->src_frame[index + i] = rgb[i];
+			break;
+		case V4L2_PIX_FMT_BGR24:
+			vdebayer->src_frame[index + i] = rgb[2 - i];
+			break;
+		}
+	}
+}
+
+static int vimc_debayer_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct vimc_debayer_device *vdebayer = v4l2_get_subdevdata(sd);
+
+	if (enable) {
+		const struct v4l2_mbus_framefmt *sink_fmt;
+		const struct v4l2_mbus_framefmt *src_fmt;
+		struct v4l2_subdev_state *state;
+		const struct vimc_pix_map *vpix;
+		unsigned int frame_size;
+
+		if (vdebayer->src_frame)
+			return 0;
+
+		state = v4l2_subdev_lock_and_get_active_state(sd);
+		sink_fmt = v4l2_subdev_state_get_format(state, 0);
+		src_fmt = v4l2_subdev_state_get_format(state, 1);
+
+		/* Calculate the frame size of the source pad */
+		vpix = vimc_pix_map_by_code(src_fmt->code);
+		frame_size = src_fmt->width * src_fmt->height * vpix->bpp;
+
+		/* Save the bytes per pixel of the sink */
+		vpix = vimc_pix_map_by_code(sink_fmt->code);
+		vdebayer->hw.sink_bpp = vpix->bpp;
+
+		/* Get the corresponding pixel map from the table */
+		vdebayer->hw.sink_pix_map =
+			vimc_debayer_pix_map_by_code(sink_fmt->code);
+
+		vdebayer->hw.size.width = sink_fmt->width;
+		vdebayer->hw.size.height = sink_fmt->height;
+
+		vdebayer->hw.src_code = src_fmt->code;
+
+		v4l2_subdev_unlock_state(state);
+
+		/*
+		 * Allocate the frame buffer. Use vmalloc to be able to
+		 * allocate a large amount of memory
+		 */
+		vdebayer->src_frame = vmalloc(frame_size);
+		if (!vdebayer->src_frame)
+			return -ENOMEM;
+	} else {
+		if (!vdebayer->src_frame)
+			return 0;
+
+		vfree(vdebayer->src_frame);
+		vdebayer->src_frame = NULL;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops vimc_debayer_core_ops = {
+	.log_status = v4l2_ctrl_subdev_log_status,
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops vimc_debayer_video_ops = {
+	.s_stream = vimc_debayer_s_stream,
+};
+
+static const struct v4l2_subdev_ops vimc_debayer_ops = {
+	.core = &vimc_debayer_core_ops,
+	.pad = &vimc_debayer_pad_ops,
+	.video = &vimc_debayer_video_ops,
+};
+
+static const struct v4l2_subdev_internal_ops vimc_debayer_internal_ops = {
+	.init_state = vimc_debayer_init_state,
+};
+
+static unsigned int vimc_debayer_get_val(const u8 *bytes,
+					 const unsigned int n_bytes)
+{
+	unsigned int i;
+	unsigned int acc = 0;
+
+	for (i = 0; i < n_bytes; i++)
+		acc = acc + (bytes[i] << (8 * i));
+
+	return acc;
+}
+
+static void vimc_debayer_calc_rgb_sink(struct vimc_debayer_device *vdebayer,
+				       const u8 *frame,
+				       const unsigned int lin,
+				       const unsigned int col,
+				       unsigned int rgb[3])
+{
+	unsigned int i, seek, wlin, wcol;
+	unsigned int n_rgb[3] = {0, 0, 0};
+
+	for (i = 0; i < 3; i++)
+		rgb[i] = 0;
+
+	/*
+	 * Calculate how many we need to subtract to get to the pixel in
+	 * the top left corner of the mean window (considering the current
+	 * pixel as the center)
+	 */
+	seek = vdebayer->hw.mean_win_size / 2;
+
+	/* Sum the values of the colors in the mean window */
+
+	dev_dbg(vdebayer->ved.dev,
+		"deb: %s: --- Calc pixel %dx%d, window mean %d, seek %d ---\n",
+		vdebayer->sd.name, lin, col, vdebayer->hw.size.height, seek);
+
+	/*
+	 * Iterate through all the lines in the mean window, start
+	 * with zero if the pixel is outside the frame and don't pass
+	 * the height when the pixel is in the bottom border of the
+	 * frame
+	 */
+	for (wlin = seek > lin ? 0 : lin - seek;
+	     wlin < lin + seek + 1 && wlin < vdebayer->hw.size.height;
+	     wlin++) {
+
+		/*
+		 * Iterate through all the columns in the mean window, start
+		 * with zero if the pixel is outside the frame and don't pass
+		 * the width when the pixel is in the right border of the
+		 * frame
+		 */
+		for (wcol = seek > col ? 0 : col - seek;
+		     wcol < col + seek + 1 && wcol < vdebayer->hw.size.width;
+		     wcol++) {
+			enum vimc_debayer_rgb_colors color;
+			unsigned int index;
+
+			/* Check which color this pixel is */
+			color = vdebayer->hw.sink_pix_map->order[wlin % 2][wcol % 2];
+
+			index = VIMC_FRAME_INDEX(wlin, wcol,
+						 vdebayer->hw.size.width,
+						 vdebayer->hw.sink_bpp);
+
+			dev_dbg(vdebayer->ved.dev,
+				"deb: %s: RGB CALC: frame index %d, win pos %dx%d, color %d\n",
+				vdebayer->sd.name, index, wlin, wcol, color);
+
+			/* Get its value */
+			rgb[color] = rgb[color] +
+				vimc_debayer_get_val(&frame[index],
+						     vdebayer->hw.sink_bpp);
+
+			/* Save how many values we already added */
+			n_rgb[color]++;
+
+			dev_dbg(vdebayer->ved.dev, "deb: %s: RGB CALC: val %d, n %d\n",
+				vdebayer->sd.name, rgb[color], n_rgb[color]);
+		}
+	}
+
+	/* Calculate the mean */
+	for (i = 0; i < 3; i++) {
+		dev_dbg(vdebayer->ved.dev,
+			"deb: %s: PRE CALC: %dx%d Color %d, val %d, n %d\n",
+			vdebayer->sd.name, lin, col, i, rgb[i], n_rgb[i]);
+
+		if (n_rgb[i])
+			rgb[i] = rgb[i] / n_rgb[i];
+
+		dev_dbg(vdebayer->ved.dev,
+			"deb: %s: FINAL CALC: %dx%d Color %d, val %d\n",
+			vdebayer->sd.name, lin, col, i, rgb[i]);
+	}
+}
+
+static void *vimc_debayer_process_frame(struct vimc_ent_device *ved,
+					const void *sink_frame)
+{
+	struct vimc_debayer_device *vdebayer =
+		container_of(ved, struct vimc_debayer_device, ved);
+
+	unsigned int rgb[3];
+	unsigned int i, j;
+
+	/* If the stream in this node is not active, just return */
+	if (!vdebayer->src_frame)
+		return ERR_PTR(-EINVAL);
+
+	for (i = 0; i < vdebayer->hw.size.height; i++)
+		for (j = 0; j < vdebayer->hw.size.width; j++) {
+			vimc_debayer_calc_rgb_sink(vdebayer, sink_frame, i, j, rgb);
+			vdebayer->set_rgb_src(vdebayer, i, j, rgb);
+		}
+
+	return vdebayer->src_frame;
+}
+
+static int vimc_debayer_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vimc_debayer_device *vdebayer =
+		container_of(ctrl->handler, struct vimc_debayer_device, hdl);
+
+	switch (ctrl->id) {
+	case VIMC_CID_MEAN_WIN_SIZE:
+		vdebayer->hw.mean_win_size = ctrl->val;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vimc_debayer_ctrl_ops = {
+	.s_ctrl = vimc_debayer_s_ctrl,
+};
+
+static void vimc_debayer_release(struct vimc_ent_device *ved)
+{
+	struct vimc_debayer_device *vdebayer =
+		container_of(ved, struct vimc_debayer_device, ved);
+
+	v4l2_ctrl_handler_free(&vdebayer->hdl);
+	v4l2_subdev_cleanup(&vdebayer->sd);
+	media_entity_cleanup(vdebayer->ved.ent);
+	kfree(vdebayer);
+}
+
+static const struct v4l2_ctrl_config vimc_debayer_ctrl_class = {
+	.flags = V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_WRITE_ONLY,
+	.id = VIMC_CID_VIMC_CLASS,
+	.name = "VIMC Controls",
+	.type = V4L2_CTRL_TYPE_CTRL_CLASS,
+};
+
+static const struct v4l2_ctrl_config vimc_debayer_ctrl_mean_win_size = {
+	.ops = &vimc_debayer_ctrl_ops,
+	.id = VIMC_CID_MEAN_WIN_SIZE,
+	.name = "Debayer Mean Window Size",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 1,
+	.max = 25,
+	.step = 2,
+	.def = 3,
+};
+
+static struct vimc_ent_device *vimc_debayer_add(struct vimc_device *vimc,
+						const char *vcfg_name)
+{
+	struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;
+	struct vimc_debayer_device *vdebayer;
+	int ret;
+
+	/* Allocate the vdebayer struct */
+	vdebayer = kzalloc_obj(*vdebayer);
+	if (!vdebayer)
+		return ERR_PTR(-ENOMEM);
+
+	/* Create controls: */
+	v4l2_ctrl_handler_init(&vdebayer->hdl, 2);
+	v4l2_ctrl_new_custom(&vdebayer->hdl, &vimc_debayer_ctrl_class, NULL);
+	v4l2_ctrl_new_custom(&vdebayer->hdl, &vimc_debayer_ctrl_mean_win_size, NULL);
+	vdebayer->sd.ctrl_handler = &vdebayer->hdl;
+	if (vdebayer->hdl.error) {
+		ret = vdebayer->hdl.error;
+		goto err_free_vdebayer;
+	}
+
+	/* Initialize ved and sd */
+	vdebayer->pads[0].flags = MEDIA_PAD_FL_SINK;
+	vdebayer->pads[1].flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = vimc_ent_sd_register(&vdebayer->ved, &vdebayer->sd, v4l2_dev,
+				   vcfg_name,
+				   MEDIA_ENT_F_PROC_VIDEO_PIXEL_ENC_CONV, 2,
+				   vdebayer->pads, &vimc_debayer_internal_ops,
+				   &vimc_debayer_ops);
+	if (ret)
+		goto err_free_hdl;
+
+	vdebayer->ved.process_frame = vimc_debayer_process_frame;
+	vdebayer->ved.dev = vimc->mdev.dev;
+	vdebayer->hw.mean_win_size = vimc_debayer_ctrl_mean_win_size.def;
+
+	vdebayer->set_rgb_src = vimc_debayer_process_rgb_frame;
+
+	return &vdebayer->ved;
+
+err_free_hdl:
+	v4l2_ctrl_handler_free(&vdebayer->hdl);
+err_free_vdebayer:
+	kfree(vdebayer);
+
+	return ERR_PTR(ret);
+}
+
+const struct vimc_ent_type vimc_debayer_type = {
+	.add = vimc_debayer_add,
+	.release = vimc_debayer_release
+};

linux-7.2.3/drivers/media/test-drivers/vimc/vimc-lens.c

@@ -0,0 +1,103 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * vimc-lens.c Virtual Media Controller Driver
+ * Copyright (C) 2022 Google, Inc
+ * Author: yunkec@google.com (Yunke Cao)
+ */
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-subdev.h>
+
+#include "vimc-common.h"
+
+#define VIMC_LENS_MAX_FOCUS_POS	1023
+#define VIMC_LENS_MAX_FOCUS_STEP	1
+
+struct vimc_lens_device {
+	struct vimc_ent_device ved;
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	u32 focus_absolute;
+};
+
+static const struct v4l2_subdev_core_ops vimc_lens_core_ops = {
+	.log_status = v4l2_ctrl_subdev_log_status,
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_ops vimc_lens_ops = {
+	.core = &vimc_lens_core_ops
+};
+
+static int vimc_lens_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vimc_lens_device *vlens =
+		container_of(ctrl->handler, struct vimc_lens_device, hdl);
+	if (ctrl->id == V4L2_CID_FOCUS_ABSOLUTE) {
+		vlens->focus_absolute = ctrl->val;
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static const struct v4l2_ctrl_ops vimc_lens_ctrl_ops = {
+	.s_ctrl = vimc_lens_s_ctrl,
+};
+
+static struct vimc_ent_device *vimc_lens_add(struct vimc_device *vimc,
+					     const char *vcfg_name)
+{
+	struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;
+	struct vimc_lens_device *vlens;
+	int ret;
+
+	/* Allocate the vlens struct */
+	vlens = kzalloc_obj(*vlens);
+	if (!vlens)
+		return ERR_PTR(-ENOMEM);
+
+	v4l2_ctrl_handler_init(&vlens->hdl, 1);
+
+	v4l2_ctrl_new_std(&vlens->hdl, &vimc_lens_ctrl_ops,
+			  V4L2_CID_FOCUS_ABSOLUTE, 0,
+			  VIMC_LENS_MAX_FOCUS_POS, VIMC_LENS_MAX_FOCUS_STEP, 0);
+	vlens->sd.ctrl_handler = &vlens->hdl;
+	if (vlens->hdl.error) {
+		ret = vlens->hdl.error;
+		goto err_free_vlens;
+	}
+	vlens->ved.dev = vimc->mdev.dev;
+
+	ret = vimc_ent_sd_register(&vlens->ved, &vlens->sd, v4l2_dev,
+				   vcfg_name, MEDIA_ENT_F_LENS, 0,
+				   NULL, NULL, &vimc_lens_ops);
+	if (ret)
+		goto err_free_hdl;
+
+	return &vlens->ved;
+
+err_free_hdl:
+	v4l2_ctrl_handler_free(&vlens->hdl);
+err_free_vlens:
+	kfree(vlens);
+
+	return ERR_PTR(ret);
+}
+
+static void vimc_lens_release(struct vimc_ent_device *ved)
+{
+	struct vimc_lens_device *vlens =
+		container_of(ved, struct vimc_lens_device, ved);
+
+	v4l2_ctrl_handler_free(&vlens->hdl);
+	v4l2_subdev_cleanup(&vlens->sd);
+	media_entity_cleanup(vlens->ved.ent);
+	kfree(vlens);
+}
+
+const struct vimc_ent_type vimc_lens_type = {
+	.add = vimc_lens_add,
+	.release = vimc_lens_release
+};

linux-7.2.3/drivers/media/test-drivers/vimc/vimc-scaler.c

@@ -0,0 +1,422 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * vimc-scaler.c Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ */
+
+#include <linux/moduleparam.h>
+#include <linux/string.h>
+#include <linux/vmalloc.h>
+#include <linux/v4l2-mediabus.h>
+#include <media/v4l2-rect.h>
+#include <media/v4l2-subdev.h>
+
+#include "vimc-common.h"
+
+/* Pad identifier */
+enum vimc_scaler_pad {
+	VIMC_SCALER_SINK = 0,
+	VIMC_SCALER_SRC = 1,
+};
+
+#define VIMC_SCALER_FMT_WIDTH_DEFAULT  640
+#define VIMC_SCALER_FMT_HEIGHT_DEFAULT 480
+
+struct vimc_scaler_device {
+	struct vimc_ent_device ved;
+	struct v4l2_subdev sd;
+	struct media_pad pads[2];
+
+	u8 *src_frame;
+
+	/*
+	 * Virtual "hardware" configuration, filled when the stream starts or
+	 * when controls are set.
+	 */
+	struct {
+		struct v4l2_mbus_framefmt sink_fmt;
+		struct v4l2_mbus_framefmt src_fmt;
+		struct v4l2_rect sink_crop;
+		unsigned int bpp;
+	} hw;
+};
+
+static const struct v4l2_mbus_framefmt fmt_default = {
+	.width = VIMC_SCALER_FMT_WIDTH_DEFAULT,
+	.height = VIMC_SCALER_FMT_HEIGHT_DEFAULT,
+	.code = MEDIA_BUS_FMT_RGB888_1X24,
+	.field = V4L2_FIELD_NONE,
+	.colorspace = V4L2_COLORSPACE_SRGB,
+};
+
+static const struct v4l2_rect crop_rect_default = {
+	.width = VIMC_SCALER_FMT_WIDTH_DEFAULT,
+	.height = VIMC_SCALER_FMT_HEIGHT_DEFAULT,
+	.top = 0,
+	.left = 0,
+};
+
+static const struct v4l2_rect crop_rect_min = {
+	.width = VIMC_FRAME_MIN_WIDTH,
+	.height = VIMC_FRAME_MIN_HEIGHT,
+	.top = 0,
+	.left = 0,
+};
+
+static struct v4l2_rect
+vimc_scaler_get_crop_bound_sink(const struct v4l2_mbus_framefmt *sink_fmt)
+{
+	/* Get the crop bounds to clamp the crop rectangle correctly */
+	struct v4l2_rect r = {
+		.left = 0,
+		.top = 0,
+		.width = sink_fmt->width,
+		.height = sink_fmt->height,
+	};
+	return r;
+}
+
+static int vimc_scaler_init_state(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state)
+{
+	struct v4l2_mbus_framefmt *mf;
+	struct v4l2_rect *r;
+	unsigned int i;
+
+	for (i = 0; i < sd->entity.num_pads; i++) {
+		mf = v4l2_subdev_state_get_format(sd_state, i);
+		*mf = fmt_default;
+	}
+
+	r = v4l2_subdev_state_get_crop(sd_state, VIMC_SCALER_SINK);
+	*r = crop_rect_default;
+
+	return 0;
+}
+
+static int vimc_scaler_enum_mbus_code(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_mbus_code_enum *code)
+{
+	u32 mbus_code = vimc_mbus_code_by_index(code->index);
+	const struct vimc_pix_map *vpix;
+
+	if (!mbus_code)
+		return -EINVAL;
+
+	vpix = vimc_pix_map_by_code(mbus_code);
+
+	/* We don't support bayer format */
+	if (!vpix || vpix->bayer)
+		return -EINVAL;
+
+	code->code = mbus_code;
+
+	return 0;
+}
+
+static int vimc_scaler_enum_frame_size(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *sd_state,
+				    struct v4l2_subdev_frame_size_enum *fse)
+{
+	const struct vimc_pix_map *vpix;
+
+	if (fse->index)
+		return -EINVAL;
+
+	/* Only accept code in the pix map table in non bayer format */
+	vpix = vimc_pix_map_by_code(fse->code);
+	if (!vpix || vpix->bayer)
+		return -EINVAL;
+
+	fse->min_width = VIMC_FRAME_MIN_WIDTH;
+	fse->min_height = VIMC_FRAME_MIN_HEIGHT;
+
+	fse->max_width = VIMC_FRAME_MAX_WIDTH;
+	fse->max_height = VIMC_FRAME_MAX_HEIGHT;
+
+	return 0;
+}
+
+static int vimc_scaler_set_fmt(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *sd_state,
+			    struct v4l2_subdev_format *format)
+{
+	struct vimc_scaler_device *vscaler = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *fmt;
+
+	/* Do not change the active format while stream is on */
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE && vscaler->src_frame)
+		return -EBUSY;
+
+	fmt = v4l2_subdev_state_get_format(sd_state, format->pad);
+
+	/*
+	 * The media bus code and colorspace can only be changed on the sink
+	 * pad, the source pad only follows.
+	 */
+	if (format->pad == VIMC_SCALER_SINK) {
+		const struct vimc_pix_map *vpix;
+
+		/* Only accept code in the pix map table in non bayer format. */
+		vpix = vimc_pix_map_by_code(format->format.code);
+		if (vpix && !vpix->bayer)
+			fmt->code = format->format.code;
+		else
+			fmt->code = fmt_default.code;
+
+		/* Clamp the colorspace to valid values. */
+		fmt->colorspace = format->format.colorspace;
+		fmt->ycbcr_enc = format->format.ycbcr_enc;
+		fmt->quantization = format->format.quantization;
+		fmt->xfer_func = format->format.xfer_func;
+		vimc_colorimetry_clamp(fmt);
+	}
+
+	/* Clamp and align the width and height */
+	fmt->width = clamp_t(u32, format->format.width, VIMC_FRAME_MIN_WIDTH,
+			     VIMC_FRAME_MAX_WIDTH) & ~1;
+	fmt->height = clamp_t(u32, format->format.height, VIMC_FRAME_MIN_HEIGHT,
+			      VIMC_FRAME_MAX_HEIGHT) & ~1;
+
+	/*
+	 * Propagate the sink pad format to the crop rectangle and the source
+	 * pad.
+	 */
+	if (format->pad == VIMC_SCALER_SINK) {
+		struct v4l2_mbus_framefmt *src_fmt;
+		struct v4l2_rect *crop;
+
+		crop = v4l2_subdev_state_get_crop(sd_state, VIMC_SCALER_SINK);
+		crop->width = fmt->width;
+		crop->height = fmt->height;
+		crop->top = 0;
+		crop->left = 0;
+
+		src_fmt = v4l2_subdev_state_get_format(sd_state, VIMC_SCALER_SRC);
+		*src_fmt = *fmt;
+	}
+
+	format->format = *fmt;
+
+	return 0;
+}
+
+static int vimc_scaler_get_selection(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_selection *sel)
+{
+	struct v4l2_mbus_framefmt *sink_fmt;
+
+	if (VIMC_IS_SRC(sel->pad))
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		sel->r = *v4l2_subdev_state_get_crop(sd_state, VIMC_SCALER_SINK);
+		break;
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sink_fmt = v4l2_subdev_state_get_format(sd_state, VIMC_SCALER_SINK);
+		sel->r = vimc_scaler_get_crop_bound_sink(sink_fmt);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void vimc_scaler_adjust_sink_crop(struct v4l2_rect *r,
+				      const struct v4l2_mbus_framefmt *sink_fmt)
+{
+	const struct v4l2_rect sink_rect =
+		vimc_scaler_get_crop_bound_sink(sink_fmt);
+
+	/* Disallow rectangles smaller than the minimal one. */
+	v4l2_rect_set_min_size(r, &crop_rect_min);
+	v4l2_rect_map_inside(r, &sink_rect);
+}
+
+static int vimc_scaler_set_selection(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_selection *sel)
+{
+	struct vimc_scaler_device *vscaler = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *sink_fmt;
+	struct v4l2_rect *crop_rect;
+
+	/* Only support setting the crop of the sink pad */
+	if (VIMC_IS_SRC(sel->pad) || sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE && vscaler->src_frame)
+		return -EBUSY;
+
+	crop_rect = v4l2_subdev_state_get_crop(sd_state, VIMC_SCALER_SINK);
+	sink_fmt = v4l2_subdev_state_get_format(sd_state, VIMC_SCALER_SINK);
+	vimc_scaler_adjust_sink_crop(&sel->r, sink_fmt);
+	*crop_rect = sel->r;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops vimc_scaler_pad_ops = {
+	.enum_mbus_code		= vimc_scaler_enum_mbus_code,
+	.enum_frame_size	= vimc_scaler_enum_frame_size,
+	.get_fmt		= v4l2_subdev_get_fmt,
+	.set_fmt		= vimc_scaler_set_fmt,
+	.get_selection		= vimc_scaler_get_selection,
+	.set_selection		= vimc_scaler_set_selection,
+};
+
+static int vimc_scaler_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct vimc_scaler_device *vscaler = v4l2_get_subdevdata(sd);
+
+	if (enable) {
+		struct v4l2_subdev_state *state;
+		const struct v4l2_mbus_framefmt *format;
+		const struct v4l2_rect *rect;
+		unsigned int frame_size;
+
+		if (vscaler->src_frame)
+			return 0;
+
+		state = v4l2_subdev_lock_and_get_active_state(sd);
+
+		/* Save the bytes per pixel of the sink. */
+		format = v4l2_subdev_state_get_format(state, VIMC_SCALER_SINK);
+		vscaler->hw.sink_fmt = *format;
+		vscaler->hw.bpp = vimc_pix_map_by_code(format->code)->bpp;
+
+		/* Calculate the frame size of the source pad. */
+		format = v4l2_subdev_state_get_format(state, VIMC_SCALER_SRC);
+		vscaler->hw.src_fmt = *format;
+		frame_size = format->width * format->height * vscaler->hw.bpp;
+
+		rect = v4l2_subdev_state_get_crop(state, VIMC_SCALER_SINK);
+		vscaler->hw.sink_crop = *rect;
+
+		v4l2_subdev_unlock_state(state);
+
+		/*
+		 * Allocate the frame buffer. Use vmalloc to be able to allocate
+		 * a large amount of memory.
+		 */
+		vscaler->src_frame = vmalloc(frame_size);
+		if (!vscaler->src_frame)
+			return -ENOMEM;
+	} else {
+		if (!vscaler->src_frame)
+			return 0;
+
+		vfree(vscaler->src_frame);
+		vscaler->src_frame = NULL;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops vimc_scaler_video_ops = {
+	.s_stream = vimc_scaler_s_stream,
+};
+
+static const struct v4l2_subdev_ops vimc_scaler_ops = {
+	.pad = &vimc_scaler_pad_ops,
+	.video = &vimc_scaler_video_ops,
+};
+
+static const struct v4l2_subdev_internal_ops vimc_scaler_internal_ops = {
+	.init_state = vimc_scaler_init_state,
+};
+
+static void vimc_scaler_fill_src_frame(const struct vimc_scaler_device *const vscaler,
+				    const u8 *const sink_frame)
+{
+	const struct v4l2_mbus_framefmt *sink_fmt = &vscaler->hw.sink_fmt;
+	const struct v4l2_mbus_framefmt *src_fmt = &vscaler->hw.src_fmt;
+	const struct v4l2_rect *r = &vscaler->hw.sink_crop;
+	unsigned int src_x, src_y;
+	u8 *walker = vscaler->src_frame;
+
+	/* Set each pixel at the src_frame to its sink_frame equivalent */
+	for (src_y = 0; src_y < src_fmt->height; src_y++) {
+		unsigned int snk_y, y_offset;
+
+		snk_y = (src_y * r->height) / src_fmt->height + r->top;
+		y_offset = snk_y * sink_fmt->width * vscaler->hw.bpp;
+
+		for (src_x = 0; src_x < src_fmt->width; src_x++) {
+			unsigned int snk_x, x_offset, index;
+
+			snk_x = (src_x * r->width) / src_fmt->width + r->left;
+			x_offset = snk_x * vscaler->hw.bpp;
+			index = y_offset + x_offset;
+			memcpy(walker, &sink_frame[index], vscaler->hw.bpp);
+			walker += vscaler->hw.bpp;
+		}
+	}
+}
+
+static void *vimc_scaler_process_frame(struct vimc_ent_device *ved,
+				    const void *sink_frame)
+{
+	struct vimc_scaler_device *vscaler = container_of(ved, struct vimc_scaler_device,
+						    ved);
+
+	/* If the stream in this node is not active, just return */
+	if (!vscaler->src_frame)
+		return ERR_PTR(-EINVAL);
+
+	vimc_scaler_fill_src_frame(vscaler, sink_frame);
+
+	return vscaler->src_frame;
+};
+
+static void vimc_scaler_release(struct vimc_ent_device *ved)
+{
+	struct vimc_scaler_device *vscaler =
+		container_of(ved, struct vimc_scaler_device, ved);
+
+	v4l2_subdev_cleanup(&vscaler->sd);
+	media_entity_cleanup(vscaler->ved.ent);
+	kfree(vscaler);
+}
+
+static struct vimc_ent_device *vimc_scaler_add(struct vimc_device *vimc,
+					    const char *vcfg_name)
+{
+	struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;
+	struct vimc_scaler_device *vscaler;
+	int ret;
+
+	/* Allocate the vscaler struct */
+	vscaler = kzalloc_obj(*vscaler);
+	if (!vscaler)
+		return ERR_PTR(-ENOMEM);
+
+	/* Initialize ved and sd */
+	vscaler->pads[VIMC_SCALER_SINK].flags = MEDIA_PAD_FL_SINK;
+	vscaler->pads[VIMC_SCALER_SRC].flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = vimc_ent_sd_register(&vscaler->ved, &vscaler->sd, v4l2_dev,
+				   vcfg_name,
+				   MEDIA_ENT_F_PROC_VIDEO_SCALER, 2,
+				   vscaler->pads, &vimc_scaler_internal_ops,
+				   &vimc_scaler_ops);
+	if (ret) {
+		kfree(vscaler);
+		return ERR_PTR(ret);
+	}
+
+	vscaler->ved.process_frame = vimc_scaler_process_frame;
+	vscaler->ved.dev = vimc->mdev.dev;
+
+	return &vscaler->ved;
+}
+
+const struct vimc_ent_type vimc_scaler_type = {
+	.add = vimc_scaler_add,
+	.release = vimc_scaler_release
+};

linux-7.2.3/drivers/media/test-drivers/vimc/vimc-sensor.c

@@ -0,0 +1,507 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * vimc-sensor.c Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ */
+
+#include <linux/v4l2-mediabus.h>
+#include <linux/vmalloc.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-subdev.h>
+#include <media/tpg/v4l2-tpg.h>
+
+#include "vimc-common.h"
+
+static const struct v4l2_mbus_framefmt fmt_default = {
+	.width = 640,
+	.height = 480,
+	.code = MEDIA_BUS_FMT_RGB888_1X24,
+	.field = V4L2_FIELD_NONE,
+	.colorspace = V4L2_COLORSPACE_SRGB,
+};
+
+static int vimc_sensor_init_state(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state)
+{
+	struct vimc_sensor_device *vsensor =
+		container_of(sd, struct vimc_sensor_device, sd);
+
+	struct v4l2_mbus_framefmt *mf;
+
+	mf = v4l2_subdev_state_get_format(sd_state, 0);
+	*mf = fmt_default;
+	vsensor->hw.size.width = fmt_default.width;
+	vsensor->hw.size.height = fmt_default.height;
+
+	return 0;
+}
+
+static int vimc_sensor_enum_mbus_code(struct v4l2_subdev *sd,
+				      struct v4l2_subdev_state *sd_state,
+				      struct v4l2_subdev_mbus_code_enum *code)
+{
+	u32 mbus_code = vimc_mbus_code_by_index(code->index);
+
+	if (!mbus_code)
+		return -EINVAL;
+
+	code->code = mbus_code;
+
+	return 0;
+}
+
+static int vimc_sensor_enum_frame_size(struct v4l2_subdev *sd,
+				       struct v4l2_subdev_state *sd_state,
+				       struct v4l2_subdev_frame_size_enum *fse)
+{
+	const struct vimc_pix_map *vpix;
+
+	if (fse->index)
+		return -EINVAL;
+
+	/* Only accept code in the pix map table */
+	vpix = vimc_pix_map_by_code(fse->code);
+	if (!vpix)
+		return -EINVAL;
+
+	fse->min_width = VIMC_FRAME_MIN_WIDTH;
+	fse->max_width = VIMC_FRAME_MAX_WIDTH;
+	fse->min_height = VIMC_FRAME_MIN_HEIGHT;
+	fse->max_height = VIMC_FRAME_MAX_HEIGHT;
+
+	return 0;
+}
+
+static void vimc_sensor_tpg_s_format(struct vimc_sensor_device *vsensor,
+				     const struct v4l2_mbus_framefmt *format)
+{
+	const struct vimc_pix_map *vpix = vimc_pix_map_by_code(format->code);
+
+	tpg_reset_source(&vsensor->tpg, format->width, format->height,
+			 format->field);
+	tpg_s_bytesperline(&vsensor->tpg, 0, format->width * vpix->bpp);
+	tpg_s_buf_height(&vsensor->tpg, format->height);
+	tpg_s_fourcc(&vsensor->tpg, vpix->pixelformat);
+	/* TODO: add support for V4L2_FIELD_ALTERNATE */
+	tpg_s_field(&vsensor->tpg, format->field, false);
+	tpg_s_colorspace(&vsensor->tpg, format->colorspace);
+	tpg_s_ycbcr_enc(&vsensor->tpg, format->ycbcr_enc);
+	tpg_s_quantization(&vsensor->tpg, format->quantization);
+	tpg_s_xfer_func(&vsensor->tpg, format->xfer_func);
+}
+
+static int vimc_sensor_update_frame_timing(struct v4l2_subdev *sd,
+					   u32 width, u32 height)
+{
+	struct vimc_sensor_device *vsensor =
+		container_of(sd, struct vimc_sensor_device, sd);
+	u64 pixel_rate = vsensor->pixel_rate->val;
+	u32 hts = width + vsensor->hblank->val;
+	u32 vts = height + vsensor->vblank->val;
+	u64 total_pixels = (u64)hts * vts;
+	u64 frame_interval_ns;
+
+	frame_interval_ns = total_pixels * NSEC_PER_SEC;
+	do_div(frame_interval_ns, pixel_rate);
+	vsensor->hw.fps_jiffies = nsecs_to_jiffies(frame_interval_ns);
+	if (vsensor->hw.fps_jiffies == 0)
+		vsensor->hw.fps_jiffies = 1;
+
+	return 0;
+}
+
+static void vimc_sensor_adjust_fmt(struct v4l2_mbus_framefmt *fmt)
+{
+	const struct vimc_pix_map *vpix;
+
+	/* Only accept code in the pix map table */
+	vpix = vimc_pix_map_by_code(fmt->code);
+	if (!vpix)
+		fmt->code = fmt_default.code;
+
+	fmt->width = clamp_t(u32, fmt->width, VIMC_FRAME_MIN_WIDTH,
+			     VIMC_FRAME_MAX_WIDTH) & ~1;
+	fmt->height = clamp_t(u32, fmt->height, VIMC_FRAME_MIN_HEIGHT,
+			      VIMC_FRAME_MAX_HEIGHT) & ~1;
+
+	/* TODO: add support for V4L2_FIELD_ALTERNATE */
+	if (fmt->field == V4L2_FIELD_ANY || fmt->field == V4L2_FIELD_ALTERNATE)
+		fmt->field = fmt_default.field;
+
+	vimc_colorimetry_clamp(fmt);
+}
+
+static u32 vimc_calc_vblank(u32 width, u32 height,
+			    s64 pixel_rate, s32 hblank)
+{
+	u32 hts = width + hblank;
+	u32 target_fps;
+	u64 vts;
+
+	target_fps = (width * height <= VIMC_PIXELS_THRESHOLD_30FPS) ? 30 : 10;
+
+	vts = (u64)pixel_rate;
+	do_div(vts, target_fps * hts);
+
+	if (vts > height)
+		return clamp((u32)(vts - height), VIMC_VBLANK_MIN, VIMC_VBLANK_MAX);
+
+	return VIMC_VBLANK_MIN;
+}
+
+static int vimc_sensor_set_fmt(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *sd_state,
+			       struct v4l2_subdev_format *fmt)
+{
+	struct vimc_sensor_device *vsensor = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *mf;
+
+	/* Do not change the format while stream is on */
+	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE && vsensor->frame)
+		return -EBUSY;
+
+	mf = v4l2_subdev_state_get_format(sd_state, fmt->pad);
+
+	/* Set the new format */
+	vimc_sensor_adjust_fmt(&fmt->format);
+
+	dev_dbg(vsensor->ved.dev, "%s: format update: "
+		"old:%dx%d (0x%x, %d, %d, %d, %d) "
+		"new:%dx%d (0x%x, %d, %d, %d, %d)\n", vsensor->sd.name,
+		/* old */
+		mf->width, mf->height, mf->code,
+		mf->colorspace,	mf->quantization,
+		mf->xfer_func, mf->ycbcr_enc,
+		/* new */
+		fmt->format.width, fmt->format.height, fmt->format.code,
+		fmt->format.colorspace, fmt->format.quantization,
+		fmt->format.xfer_func, fmt->format.ycbcr_enc);
+
+	*mf = fmt->format;
+	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		u32 vblank_def = vimc_calc_vblank(fmt->format.width,
+						  fmt->format.height,
+						  vsensor->pixel_rate->val,
+						  vsensor->hblank->val);
+		vsensor->hw.size.width = fmt->format.width;
+		vsensor->hw.size.height = fmt->format.height;
+		__v4l2_ctrl_modify_range(vsensor->vblank,
+					 VIMC_VBLANK_MIN,
+					 VIMC_VBLANK_MAX,
+					 VIMC_VBLANK_STEP,
+					 vblank_def);
+		__v4l2_ctrl_s_ctrl(vsensor->vblank, vblank_def);
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops vimc_sensor_pad_ops = {
+	.enum_mbus_code		= vimc_sensor_enum_mbus_code,
+	.enum_frame_size	= vimc_sensor_enum_frame_size,
+	.get_fmt		= v4l2_subdev_get_fmt,
+	.set_fmt		= vimc_sensor_set_fmt,
+};
+
+static void *vimc_sensor_process_frame(struct vimc_ent_device *ved,
+				       const void *sink_frame)
+{
+	struct vimc_sensor_device *vsensor =
+		container_of(ved, struct vimc_sensor_device, ved);
+
+	const unsigned int line_height = 16;
+	u8 *basep[TPG_MAX_PLANES][2];
+	unsigned int line = 1;
+	char str[100];
+
+	tpg_fill_plane_buffer(&vsensor->tpg, 0, 0, vsensor->frame);
+	tpg_calc_text_basep(&vsensor->tpg, basep, 0, vsensor->frame);
+	switch (vsensor->hw.osd_value) {
+	case VIMC_SENSOR_OSD_SHOW_ALL: {
+		const char *order = tpg_g_color_order(&vsensor->tpg);
+
+		tpg_gen_text(&vsensor->tpg, basep, line++ * line_height,
+			     16, order);
+		snprintf(str, sizeof(str),
+			 "brightness %3d, contrast %3d, saturation %3d, hue %d ",
+			 vsensor->tpg.brightness,
+			 vsensor->tpg.contrast,
+			 vsensor->tpg.saturation,
+			 vsensor->tpg.hue);
+		tpg_gen_text(&vsensor->tpg, basep, line++ * line_height, 16, str);
+		snprintf(str, sizeof(str), "sensor size: %dx%d",
+			 vsensor->hw.size.width, vsensor->hw.size.height);
+		tpg_gen_text(&vsensor->tpg, basep, line++ * line_height, 16, str);
+		fallthrough;
+	}
+	case VIMC_SENSOR_OSD_SHOW_COUNTERS: {
+		unsigned int ms;
+
+		ms = div_u64(ktime_get_ns() - vsensor->hw.start_stream_ts, 1000000);
+		snprintf(str, sizeof(str), "%02d:%02d:%02d:%03d",
+			 (ms / (60 * 60 * 1000)) % 24,
+			 (ms / (60 * 1000)) % 60,
+			 (ms / 1000) % 60,
+			 ms % 1000);
+		tpg_gen_text(&vsensor->tpg, basep, line++ * line_height, 16, str);
+		break;
+	}
+	case VIMC_SENSOR_OSD_SHOW_NONE:
+	default:
+		break;
+	}
+
+	return vsensor->frame;
+}
+
+static int vimc_sensor_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct vimc_sensor_device *vsensor =
+				container_of(sd, struct vimc_sensor_device, sd);
+
+	if (enable) {
+		const struct v4l2_mbus_framefmt *format;
+		struct v4l2_subdev_state *state;
+		const struct vimc_pix_map *vpix;
+		unsigned int frame_size;
+
+		state = v4l2_subdev_lock_and_get_active_state(sd);
+		format = v4l2_subdev_state_get_format(state, 0);
+
+		/* Configure the test pattern generator. */
+		vimc_sensor_tpg_s_format(vsensor, format);
+
+		/* Calculate the frame size. */
+		vpix = vimc_pix_map_by_code(format->code);
+		frame_size = format->width * vpix->bpp * format->height;
+
+		vsensor->hw.size.width = format->width;
+		vsensor->hw.size.height = format->height;
+		vimc_sensor_update_frame_timing(sd, format->width,
+						format->height);
+
+		v4l2_subdev_unlock_state(state);
+
+		/*
+		 * Allocate the frame buffer. Use vmalloc to be able to
+		 * allocate a large amount of memory
+		 */
+		vsensor->frame = vmalloc(frame_size);
+		if (!vsensor->frame)
+			return -ENOMEM;
+
+		vsensor->hw.start_stream_ts = ktime_get_ns();
+	} else {
+
+		vfree(vsensor->frame);
+		vsensor->frame = NULL;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops vimc_sensor_core_ops = {
+	.log_status = v4l2_ctrl_subdev_log_status,
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops vimc_sensor_video_ops = {
+	.s_stream = vimc_sensor_s_stream,
+};
+
+static const struct v4l2_subdev_ops vimc_sensor_ops = {
+	.core = &vimc_sensor_core_ops,
+	.pad = &vimc_sensor_pad_ops,
+	.video = &vimc_sensor_video_ops,
+};
+
+static const struct v4l2_subdev_internal_ops vimc_sensor_internal_ops = {
+	.init_state = vimc_sensor_init_state,
+};
+
+static int vimc_sensor_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vimc_sensor_device *vsensor =
+		container_of(ctrl->handler, struct vimc_sensor_device, hdl);
+
+	switch (ctrl->id) {
+	case VIMC_CID_TEST_PATTERN:
+		tpg_s_pattern(&vsensor->tpg, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		tpg_s_hflip(&vsensor->tpg, ctrl->val);
+		break;
+	case V4L2_CID_VFLIP:
+		tpg_s_vflip(&vsensor->tpg, ctrl->val);
+		break;
+	case V4L2_CID_BRIGHTNESS:
+		tpg_s_brightness(&vsensor->tpg, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		tpg_s_contrast(&vsensor->tpg, ctrl->val);
+		break;
+	case V4L2_CID_HUE:
+		tpg_s_hue(&vsensor->tpg, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		tpg_s_saturation(&vsensor->tpg, ctrl->val);
+		break;
+	case VIMC_CID_OSD_TEXT_MODE:
+		vsensor->hw.osd_value = ctrl->val;
+		break;
+	case V4L2_CID_PIXEL_RATE:
+		break;
+	case V4L2_CID_HBLANK:
+		break;
+	case V4L2_CID_VBLANK:
+		vimc_sensor_update_frame_timing(&vsensor->sd,
+						vsensor->hw.size.width,
+						vsensor->hw.size.height);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vimc_sensor_ctrl_ops = {
+	.s_ctrl = vimc_sensor_s_ctrl,
+};
+
+static void vimc_sensor_release(struct vimc_ent_device *ved)
+{
+	struct vimc_sensor_device *vsensor =
+		container_of(ved, struct vimc_sensor_device, ved);
+
+	v4l2_ctrl_handler_free(&vsensor->hdl);
+	tpg_free(&vsensor->tpg);
+	v4l2_subdev_cleanup(&vsensor->sd);
+	media_entity_cleanup(vsensor->ved.ent);
+	kfree(vsensor);
+}
+
+/* Image Processing Controls */
+static const struct v4l2_ctrl_config vimc_sensor_ctrl_class = {
+	.flags = V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_WRITE_ONLY,
+	.id = VIMC_CID_VIMC_CLASS,
+	.name = "VIMC Controls",
+	.type = V4L2_CTRL_TYPE_CTRL_CLASS,
+};
+
+static const struct v4l2_ctrl_config vimc_sensor_ctrl_test_pattern = {
+	.ops = &vimc_sensor_ctrl_ops,
+	.id = VIMC_CID_TEST_PATTERN,
+	.name = "Test Pattern",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = TPG_PAT_NOISE,
+	.qmenu = tpg_pattern_strings,
+};
+
+static const char * const vimc_ctrl_osd_mode_strings[] = {
+	"All",
+	"Counters Only",
+	"None",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vimc_sensor_ctrl_osd_mode = {
+	.ops = &vimc_sensor_ctrl_ops,
+	.id = VIMC_CID_OSD_TEXT_MODE,
+	.name = "Show Information",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = ARRAY_SIZE(vimc_ctrl_osd_mode_strings) - 2,
+	.qmenu = vimc_ctrl_osd_mode_strings,
+};
+
+static struct vimc_ent_device *vimc_sensor_add(struct vimc_device *vimc,
+					       const char *vcfg_name)
+{
+	struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;
+	struct vimc_sensor_device *vsensor;
+	int ret;
+
+	/* Allocate the vsensor struct */
+	vsensor = kzalloc_obj(*vsensor);
+	if (!vsensor)
+		return ERR_PTR(-ENOMEM);
+
+	v4l2_ctrl_handler_init(&vsensor->hdl, 4);
+
+	v4l2_ctrl_new_custom(&vsensor->hdl, &vimc_sensor_ctrl_class, NULL);
+	v4l2_ctrl_new_custom(&vsensor->hdl, &vimc_sensor_ctrl_test_pattern, NULL);
+	v4l2_ctrl_new_custom(&vsensor->hdl, &vimc_sensor_ctrl_osd_mode, NULL);
+	v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
+			  V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
+			  V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
+			  V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
+			  V4L2_CID_CONTRAST, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
+			  V4L2_CID_HUE, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
+			  V4L2_CID_SATURATION, 0, 255, 1, 128);
+	/* Timing controls for frame interval configuration */
+	vsensor->pixel_rate = v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
+						V4L2_CID_PIXEL_RATE,
+						VIMC_PIXEL_RATE_FIXED, VIMC_PIXEL_RATE_FIXED,
+						1, VIMC_PIXEL_RATE_FIXED);
+	if (vsensor->pixel_rate)
+		vsensor->pixel_rate->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	vsensor->hblank = v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
+					    V4L2_CID_HBLANK,
+					    VIMC_HBLANK_FIXED, VIMC_HBLANK_FIXED,
+					    1, VIMC_HBLANK_FIXED);
+	if (vsensor->hblank)
+		vsensor->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	vsensor->vblank = v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
+					    V4L2_CID_VBLANK,
+					    VIMC_VBLANK_MIN, VIMC_VBLANK_MAX,
+					    VIMC_VBLANK_STEP, VIMC_VBLANK_DEFAULT);
+
+	vsensor->sd.ctrl_handler = &vsensor->hdl;
+	if (vsensor->hdl.error) {
+		ret = vsensor->hdl.error;
+		goto err_free_vsensor;
+	}
+
+	/* Initialize the test pattern generator */
+	tpg_init(&vsensor->tpg, fmt_default.width, fmt_default.height);
+	ret = tpg_alloc(&vsensor->tpg, VIMC_FRAME_MAX_WIDTH);
+	if (ret)
+		goto err_free_hdl;
+
+	/* Initialize ved and sd */
+	vsensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = vimc_ent_sd_register(&vsensor->ved, &vsensor->sd, v4l2_dev,
+				   vcfg_name,
+				   MEDIA_ENT_F_CAM_SENSOR, 1, &vsensor->pad,
+				   &vimc_sensor_internal_ops, &vimc_sensor_ops);
+	if (ret)
+		goto err_free_tpg;
+
+	vsensor->ved.process_frame = vimc_sensor_process_frame;
+	vsensor->ved.dev = vimc->mdev.dev;
+
+	return &vsensor->ved;
+
+err_free_tpg:
+	tpg_free(&vsensor->tpg);
+err_free_hdl:
+	v4l2_ctrl_handler_free(&vsensor->hdl);
+err_free_vsensor:
+	kfree(vsensor);
+
+	return ERR_PTR(ret);
+}
+
+const struct vimc_ent_type vimc_sensor_type = {
+	.add = vimc_sensor_add,
+	.release = vimc_sensor_release
+};

linux-7.2.3/drivers/media/test-drivers/vimc/vimc-streamer.c

@@ -0,0 +1,273 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * vimc-streamer.c Virtual Media Controller Driver
+ *
+ * Copyright (C) 2018 Lucas A. M. Magalhães <lucmaga@gmail.com>
+ *
+ */
+
+#include <linux/init.h>
+#include <linux/freezer.h>
+#include <linux/kthread.h>
+
+#include "vimc-streamer.h"
+
+/**
+ * vimc_get_source_entity - get the entity connected with the first sink pad
+ *
+ * @ent:	reference media_entity
+ *
+ * Helper function that returns the media entity containing the source pad
+ * linked with the first sink pad from the given media entity pad list.
+ *
+ * Return: The source pad or NULL, if it wasn't found.
+ */
+static struct media_entity *vimc_get_source_entity(struct media_entity *ent)
+{
+	struct media_pad *pad;
+	int i;
+
+	for (i = 0; i < ent->num_pads; i++) {
+		if (ent->pads[i].flags & MEDIA_PAD_FL_SOURCE)
+			continue;
+		pad = media_pad_remote_pad_first(&ent->pads[i]);
+		return pad ? pad->entity : NULL;
+	}
+	return NULL;
+}
+
+/**
+ * vimc_streamer_pipeline_terminate - Disable stream in all ved in stream
+ *
+ * @stream: the pointer to the stream structure with the pipeline to be
+ *	    disabled.
+ *
+ * Calls s_stream to disable the stream in each entity of the pipeline
+ *
+ */
+static void vimc_streamer_pipeline_terminate(struct vimc_stream *stream)
+{
+	struct vimc_ent_device *ved;
+	struct v4l2_subdev *sd;
+
+	while (stream->pipe_size) {
+		stream->pipe_size--;
+		ved = stream->ved_pipeline[stream->pipe_size];
+		stream->ved_pipeline[stream->pipe_size] = NULL;
+
+		if (!is_media_entity_v4l2_subdev(ved->ent))
+			continue;
+
+		sd = media_entity_to_v4l2_subdev(ved->ent);
+		/*
+		 * Do not call .s_stream() to stop an already
+		 * stopped/unstarted subdev.
+		 */
+		if (!v4l2_subdev_is_streaming(sd))
+			continue;
+		v4l2_subdev_call(sd, video, s_stream, 0);
+	}
+}
+
+/**
+ * vimc_streamer_pipeline_init - Initializes the stream structure
+ *
+ * @stream: the pointer to the stream structure to be initialized
+ * @ved:    the pointer to the vimc entity initializing the stream
+ *
+ * Initializes the stream structure. Walks through the entity graph to
+ * construct the pipeline used later on the streamer thread.
+ * Calls vimc_streamer_s_stream() to enable stream in all entities of
+ * the pipeline.
+ *
+ * Return: 0 if success, error code otherwise.
+ */
+static int vimc_streamer_pipeline_init(struct vimc_stream *stream,
+				       struct vimc_ent_device *ved)
+{
+	struct media_entity *entity;
+	struct video_device *vdev;
+	struct v4l2_subdev *sd;
+	int ret = 0;
+
+	stream->pipe_size = 0;
+	while (stream->pipe_size < VIMC_STREAMER_PIPELINE_MAX_SIZE) {
+		if (!ved) {
+			vimc_streamer_pipeline_terminate(stream);
+			return -EINVAL;
+		}
+		stream->ved_pipeline[stream->pipe_size++] = ved;
+
+		if (is_media_entity_v4l2_subdev(ved->ent)) {
+			sd = media_entity_to_v4l2_subdev(ved->ent);
+			ret = v4l2_subdev_call(sd, video, s_stream, 1);
+			if (ret && ret != -ENOIOCTLCMD) {
+				dev_err(ved->dev, "subdev_call error %s\n",
+					ved->ent->name);
+				vimc_streamer_pipeline_terminate(stream);
+				return ret;
+			}
+		}
+
+		entity = vimc_get_source_entity(ved->ent);
+		/* Check if the end of the pipeline was reached */
+		if (!entity) {
+			/* the first entity of the pipe should be source only */
+			if (!vimc_is_source(ved->ent)) {
+				dev_err(ved->dev,
+					"first entity in the pipe '%s' is not a source\n",
+					ved->ent->name);
+				vimc_streamer_pipeline_terminate(stream);
+				return -EPIPE;
+			}
+			return 0;
+		}
+
+		/* Get the next device in the pipeline */
+		if (is_media_entity_v4l2_subdev(entity)) {
+			sd = media_entity_to_v4l2_subdev(entity);
+			ved = v4l2_get_subdevdata(sd);
+		} else {
+			vdev = container_of(entity,
+					    struct video_device,
+					    entity);
+			ved = video_get_drvdata(vdev);
+		}
+	}
+
+	vimc_streamer_pipeline_terminate(stream);
+	return -EINVAL;
+}
+
+/**
+ * vimc_streamer_get_sensor() - Get sensor from pipeline
+ * @stream: the pipeline
+ *
+ * Helper function to find the sensor device in the pipeline.
+ * Returns pointer to sensor device or NULL if not found.
+ */
+static struct vimc_sensor_device *vimc_streamer_get_sensor(struct vimc_stream *stream)
+{
+	int i;
+
+	for (i = 0; i < stream->pipe_size; i++) {
+		struct vimc_ent_device *ved = stream->ved_pipeline[i];
+
+		if (ved && ved->ent &&
+		    ved->ent->function == MEDIA_ENT_F_CAM_SENSOR) {
+			return container_of(ved, struct vimc_sensor_device, ved);
+		}
+	}
+
+	return NULL;
+}
+
+/**
+ * vimc_streamer_thread - Process frames through the pipeline
+ *
+ * @data:	vimc_stream struct of the current stream
+ *
+ * From the source to the sink, gets a frame from each subdevice and send to
+ * the next one of the pipeline at a fixed framerate.
+ *
+ * Return:
+ * Always zero (created as ``int`` instead of ``void`` to comply with
+ * kthread API).
+ */
+static int vimc_streamer_thread(void *data)
+{
+	struct vimc_stream *stream = data;
+	struct vimc_sensor_device *vsensor;
+	u8 *frame = NULL;
+	int i;
+	unsigned long fps_jiffies;
+	const unsigned long default_jiffies = HZ / 30;
+
+	set_freezable();
+	vsensor = vimc_streamer_get_sensor(stream);
+
+	for (;;) {
+		try_to_freeze();
+		if (kthread_should_stop())
+			break;
+
+		/* Read from hardware configuration */
+		fps_jiffies = vsensor ? vsensor->hw.fps_jiffies : default_jiffies;
+
+		for (i = stream->pipe_size - 1; i >= 0; i--) {
+			frame = stream->ved_pipeline[i]->process_frame(
+					stream->ved_pipeline[i], frame);
+			if (!frame || IS_ERR(frame))
+				break;
+		}
+		set_current_state(TASK_UNINTERRUPTIBLE);
+		schedule_timeout(fps_jiffies);
+	}
+
+	return 0;
+}
+
+/**
+ * vimc_streamer_s_stream - Start/stop the streaming on the media pipeline
+ *
+ * @stream:	the pointer to the stream structure of the current stream
+ * @ved:	pointer to the vimc entity of the entity of the stream
+ * @enable:	flag to determine if stream should start/stop
+ *
+ * When starting, check if there is no ``stream->kthread`` allocated. This
+ * should indicate that a stream is already running. Then, it initializes the
+ * pipeline, creates and runs a kthread to consume buffers through the pipeline.
+ * When stopping, analogously check if there is a stream running, stop the
+ * thread and terminates the pipeline.
+ *
+ * Return: 0 if success, error code otherwise.
+ */
+int vimc_streamer_s_stream(struct vimc_stream *stream,
+			   struct vimc_ent_device *ved,
+			   int enable)
+{
+	int ret;
+
+	if (!stream || !ved)
+		return -EINVAL;
+
+	if (enable) {
+		if (stream->kthread)
+			return 0;
+
+		ret = vimc_streamer_pipeline_init(stream, ved);
+		if (ret)
+			return ret;
+
+		stream->kthread = kthread_run(vimc_streamer_thread, stream,
+					      "vimc-streamer thread");
+
+		if (IS_ERR(stream->kthread)) {
+			ret = PTR_ERR(stream->kthread);
+			dev_err(ved->dev, "kthread_run failed with %d\n", ret);
+			vimc_streamer_pipeline_terminate(stream);
+			stream->kthread = NULL;
+			return ret;
+		}
+
+	} else {
+		if (!stream->kthread)
+			return 0;
+
+		ret = kthread_stop(stream->kthread);
+		/*
+		 * kthread_stop returns -EINTR in cases when streamon was
+		 * immediately followed by streamoff, and the thread didn't had
+		 * a chance to run. Ignore errors to stop the stream in the
+		 * pipeline.
+		 */
+		if (ret)
+			dev_dbg(ved->dev, "kthread_stop returned '%d'\n", ret);
+
+		stream->kthread = NULL;
+
+		vimc_streamer_pipeline_terminate(stream);
+	}
+
+	return 0;
+}

linux-7.2.3/drivers/media/test-drivers/vimc/vimc-streamer.h

@@ -0,0 +1,45 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * vimc-streamer.h Virtual Media Controller Driver
+ *
+ * Copyright (C) 2018 Lucas A. M. Magalhães <lucmaga@gmail.com>
+ *
+ */
+
+#ifndef _VIMC_STREAMER_H_
+#define _VIMC_STREAMER_H_
+
+#include <media/media-device.h>
+
+#include "vimc-common.h"
+
+#define VIMC_STREAMER_PIPELINE_MAX_SIZE 16
+
+/**
+ * struct vimc_stream - struct that represents a stream in the pipeline
+ *
+ * @pipe:		the media pipeline object associated with this stream
+ * @ved_pipeline:	array containing all the entities participating in the
+ * 			stream. The order is from a video device (usually a
+ *			capture device) where stream_on was called, to the
+ *			entity generating the first base image to be
+ *			processed in the pipeline.
+ * @pipe_size:		size of @ved_pipeline
+ * @kthread:		thread that generates the frames of the stream.
+ *
+ * When the user call stream_on in a video device, struct vimc_stream is
+ * used to keep track of all entities and subdevices that generates and
+ * process frames for the stream.
+ */
+struct vimc_stream {
+	struct media_pipeline pipe;
+	struct vimc_ent_device *ved_pipeline[VIMC_STREAMER_PIPELINE_MAX_SIZE];
+	unsigned int pipe_size;
+	struct task_struct *kthread;
+};
+
+int vimc_streamer_s_stream(struct vimc_stream *stream,
+			   struct vimc_ent_device *ved,
+			   int enable);
+
+#endif  //_VIMC_STREAMER_H_