1 files changed, 276 insertions, 0 deletions
diff --git a/drivers/media/platform/vimc/vimc-sensor.c b/drivers/media/platform/vimc/vimc-sensor.c
new file mode 100644
index 000000000000..591f6a4f8bd3
--- /dev/null
+++ b/drivers/media/platform/vimc/vimc-sensor.c
@@ -0,0 +1,276 @@
+/*
+ * vimc-sensor.c Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/freezer.h>
+#include <linux/kthread.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/vmalloc.h>
+#include <media/v4l2-subdev.h>
+
+#include "vimc-sensor.h"
+
+struct vimc_sen_device {
+	struct vimc_ent_device ved;
+	struct v4l2_subdev sd;
+	struct task_struct *kthread_sen;
+	u8 *frame;
+	/* The active format */
+	struct v4l2_mbus_framefmt mbus_format;
+	int frame_size;
+};
+
+static int vimc_sen_enum_mbus_code(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_pad_config *cfg,
+				   struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct vimc_sen_device *vsen =
+				container_of(sd, struct vimc_sen_device, sd);
+
+	/* TODO: Add support for other codes */
+	if (code->index)
+		return -EINVAL;
+
+	code->code = vsen->mbus_format.code;
+
+	return 0;
+}
+
+static int vimc_sen_enum_frame_size(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_pad_config *cfg,
+				    struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct vimc_sen_device *vsen =
+				container_of(sd, struct vimc_sen_device, sd);
+
+	/* TODO: Add support to other formats */
+	if (fse->index)
+		return -EINVAL;
+
+	/* TODO: Add support for other codes */
+	if (fse->code != vsen->mbus_format.code)
+		return -EINVAL;
+
+	fse->min_width = vsen->mbus_format.width;
+	fse->max_width = vsen->mbus_format.width;
+	fse->min_height = vsen->mbus_format.height;
+	fse->max_height = vsen->mbus_format.height;
+
+	return 0;
+}
+
+static int vimc_sen_get_fmt(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_pad_config *cfg,
+			    struct v4l2_subdev_format *format)
+{
+	struct vimc_sen_device *vsen =
+				container_of(sd, struct vimc_sen_device, sd);
+
+	format->format = vsen->mbus_format;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops vimc_sen_pad_ops = {
+	.enum_mbus_code		= vimc_sen_enum_mbus_code,
+	.enum_frame_size	= vimc_sen_enum_frame_size,
+	.get_fmt		= vimc_sen_get_fmt,
+	/* TODO: Add support to other formats */
+	.set_fmt		= vimc_sen_get_fmt,
+};
+
+/* media operations */
+static const struct media_entity_operations vimc_sen_mops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static int vimc_thread_sen(void *data)
+{
+	struct vimc_sen_device *vsen = data;
+	unsigned int i;
+
+	set_freezable();
+	set_current_state(TASK_UNINTERRUPTIBLE);
+
+	for (;;) {
+		try_to_freeze();
+		if (kthread_should_stop())
+			break;
+
+		memset(vsen->frame, 100, vsen->frame_size);
+
+		/* Send the frame to all source pads */
+		for (i = 0; i < vsen->sd.entity.num_pads; i++)
+			vimc_propagate_frame(&vsen->sd.entity.pads[i],
+					     vsen->frame);
+
+		/* 60 frames per second */
+		schedule_timeout(HZ/60);
+	}
+
+	return 0;
+}
+
+static int vimc_sen_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct vimc_sen_device *vsen =
+				container_of(sd, struct vimc_sen_device, sd);
+	int ret;
+
+	if (enable) {
+		const struct vimc_pix_map *vpix;
+
+		if (vsen->kthread_sen)
+			return -EINVAL;
+
+		/* Calculate the frame size */
+		vpix = vimc_pix_map_by_code(vsen->mbus_format.code);
+		vsen->frame_size = vsen->mbus_format.width * vpix->bpp *
+				   vsen->mbus_format.height;
+
+		/*
+		 * Allocate the frame buffer. Use vmalloc to be able to
+		 * allocate a large amount of memory
+		 */
+		vsen->frame = vmalloc(vsen->frame_size);
+		if (!vsen->frame)
+			return -ENOMEM;
+
+		/* Initialize the image generator thread */
+		vsen->kthread_sen = kthread_run(vimc_thread_sen, vsen, "%s-sen",
+						vsen->sd.v4l2_dev->name);
+		if (IS_ERR(vsen->kthread_sen)) {
+			dev_err(vsen->sd.v4l2_dev->dev,
+				"%s: kernel_thread() failed\n",	vsen->sd.name);
+			vfree(vsen->frame);
+			vsen->frame = NULL;
+			return PTR_ERR(vsen->kthread_sen);
+		}
+	} else {
+		if (!vsen->kthread_sen)
+			return -EINVAL;
+
+		/* Stop image generator */
+		ret = kthread_stop(vsen->kthread_sen);
+		vsen->kthread_sen = NULL;
+
+		vfree(vsen->frame);
+		vsen->frame = NULL;
+		return ret;
+	}
+
+	return 0;
+}
+
+struct v4l2_subdev_video_ops vimc_sen_video_ops = {
+	.s_stream = vimc_sen_s_stream,
+};
+
+static const struct v4l2_subdev_ops vimc_sen_ops = {
+	.pad = &vimc_sen_pad_ops,
+	.video = &vimc_sen_video_ops,
+};
+
+static void vimc_sen_destroy(struct vimc_ent_device *ved)
+{
+	struct vimc_sen_device *vsen =
+				container_of(ved, struct vimc_sen_device, ved);
+
+	v4l2_device_unregister_subdev(&vsen->sd);
+	media_entity_cleanup(ved->ent);
+	kfree(vsen);
+}
+
+struct vimc_ent_device *vimc_sen_create(struct v4l2_device *v4l2_dev,
+					const char *const name,
+					u16 num_pads,
+					const unsigned long *pads_flag)
+{
+	struct vimc_sen_device *vsen;
+	unsigned int i;
+	int ret;
+
+	/* NOTE: a sensor node may be created with more then one pad */
+	if (!name || !num_pads || !pads_flag)
+		return ERR_PTR(-EINVAL);
+
+	/* check if all pads are sources */
+	for (i = 0; i < num_pads; i++)
+		if (!(pads_flag[i] & MEDIA_PAD_FL_SOURCE))
+			return ERR_PTR(-EINVAL);
+
+	/* Allocate the vsen struct */
+	vsen = kzalloc(sizeof(*vsen), GFP_KERNEL);
+	if (!vsen)
+		return ERR_PTR(-ENOMEM);
+
+	/* Allocate the pads */
+	vsen->ved.pads = vimc_pads_init(num_pads, pads_flag);
+	if (IS_ERR(vsen->ved.pads)) {
+		ret = PTR_ERR(vsen->ved.pads);
+		goto err_free_vsen;
+	}
+
+	/* Fill the vimc_ent_device struct */
+	vsen->ved.destroy = vimc_sen_destroy;
+	vsen->ved.ent = &vsen->sd.entity;
+
+	/* Initialize the subdev */
+	v4l2_subdev_init(&vsen->sd, &vimc_sen_ops);
+	vsen->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	vsen->sd.entity.ops = &vimc_sen_mops;
+	vsen->sd.owner = THIS_MODULE;
+	strlcpy(vsen->sd.name, name, sizeof(vsen->sd.name));
+	v4l2_set_subdevdata(&vsen->sd, &vsen->ved);
+
+	/* Expose this subdev to user space */
+	vsen->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	/* Initialize the media entity */
+	ret = media_entity_pads_init(&vsen->sd.entity,
+				     num_pads, vsen->ved.pads);
+	if (ret)
+		goto err_clean_pads;
+
+	/* Set the active frame format (this is hardcoded for now) */
+	vsen->mbus_format.width = 640;
+	vsen->mbus_format.height = 480;
+	vsen->mbus_format.code = MEDIA_BUS_FMT_RGB888_1X24;
+	vsen->mbus_format.field = V4L2_FIELD_NONE;
+	vsen->mbus_format.colorspace = V4L2_COLORSPACE_SRGB;
+	vsen->mbus_format.quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	vsen->mbus_format.xfer_func = V4L2_XFER_FUNC_SRGB;
+
+	/* Register the subdev with the v4l2 and the media framework */
+	ret = v4l2_device_register_subdev(v4l2_dev, &vsen->sd);
+	if (ret) {
+		dev_err(vsen->sd.v4l2_dev->dev,
+			"%s: subdev register failed (err=%d)\n",
+			vsen->sd.name, ret);
+		goto err_clean_m_ent;
+	}
+
+	return &vsen->ved;
+
+err_clean_m_ent:
+	media_entity_cleanup(&vsen->sd.entity);
+err_clean_pads:
+	vimc_pads_cleanup(vsen->ved.pads);
+err_free_vsen:
+	kfree(vsen);
+
+	return ERR_PTR(ret);
+}