[mirror_ubuntu-artful-kernel.git] / drivers / media / platform / vimc / vimc-sensor.c

/*
 * 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/component.h>
#include <linux/freezer.h>
#include <linux/kthread.h>
#include <linux/module.h>
#include <linux/platform_device.h>
#include <linux/v4l2-mediabus.h>
#include <linux/vmalloc.h>
#include <media/v4l2-subdev.h>
#include <media/v4l2-tpg.h>

#include "vimc-common.h"

#define VIMC_SEN_DRV_NAME "vimc-sensor"

struct vimc_sen_device {
	struct vimc_ent_device ved;
	struct v4l2_subdev sd;
	struct device *dev;
	struct tpg_data tpg;
	struct task_struct *kthread_sen;
	u8 *frame;
	/* The active format */
	struct v4l2_mbus_framefmt mbus_format;
};

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_DEFAULT,
};

static int vimc_sen_init_cfg(struct v4l2_subdev *sd,
			     struct v4l2_subdev_pad_config *cfg)
{
	unsigned int i;

	for (i = 0; i < sd->entity.num_pads; i++) {
		struct v4l2_mbus_framefmt *mf;

		mf = v4l2_subdev_get_try_format(sd, cfg, i);
		*mf = fmt_default;
	}

	return 0;
}

static int vimc_sen_enum_mbus_code(struct v4l2_subdev *sd,
				   struct v4l2_subdev_pad_config *cfg,
				   struct v4l2_subdev_mbus_code_enum *code)
{
	const struct vimc_pix_map *vpix = vimc_pix_map_by_index(code->index);

	if (!vpix)
		return -EINVAL;

	code->code = vpix->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)
{
	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 int vimc_sen_get_fmt(struct v4l2_subdev *sd,
			    struct v4l2_subdev_pad_config *cfg,
			    struct v4l2_subdev_format *fmt)
{
	struct vimc_sen_device *vsen =
				container_of(sd, struct vimc_sen_device, sd);

	fmt->format = fmt->which == V4L2_SUBDEV_FORMAT_TRY ?
		      *v4l2_subdev_get_try_format(sd, cfg, fmt->pad) :
		      vsen->mbus_format;

	return 0;
}

static void vimc_sen_tpg_s_format(struct vimc_sen_device *vsen)
{
	const struct vimc_pix_map *vpix =
				vimc_pix_map_by_code(vsen->mbus_format.code);

	tpg_reset_source(&vsen->tpg, vsen->mbus_format.width,
			 vsen->mbus_format.height, vsen->mbus_format.field);
	tpg_s_bytesperline(&vsen->tpg, 0, vsen->mbus_format.width * vpix->bpp);
	tpg_s_buf_height(&vsen->tpg, vsen->mbus_format.height);
	tpg_s_fourcc(&vsen->tpg, vpix->pixelformat);
	/* TODO: add support for V4L2_FIELD_ALTERNATE */
	tpg_s_field(&vsen->tpg, vsen->mbus_format.field, false);
	tpg_s_colorspace(&vsen->tpg, vsen->mbus_format.colorspace);
	tpg_s_ycbcr_enc(&vsen->tpg, vsen->mbus_format.ycbcr_enc);
	tpg_s_quantization(&vsen->tpg, vsen->mbus_format.quantization);
	tpg_s_xfer_func(&vsen->tpg, vsen->mbus_format.xfer_func);
}

static void vimc_sen_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 int vimc_sen_set_fmt(struct v4l2_subdev *sd,
			    struct v4l2_subdev_pad_config *cfg,
			    struct v4l2_subdev_format *fmt)
{
	struct vimc_sen_device *vsen = v4l2_get_subdevdata(sd);
	struct v4l2_mbus_framefmt *mf;

	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
		/* Do not change the format while stream is on */
		if (vsen->frame)
			return -EBUSY;

		mf = &vsen->mbus_format;
	} else {
		mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
	}

	/* Set the new format */
	vimc_sen_adjust_fmt(&fmt->format);

	dev_dbg(vsen->dev, "%s: format update: "
		"old:%dx%d (0x%x, %d, %d, %d, %d) "
		"new:%dx%d (0x%x, %d, %d, %d, %d)\n", vsen->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;

	return 0;
}

static const struct v4l2_subdev_pad_ops vimc_sen_pad_ops = {
	.init_cfg		= vimc_sen_init_cfg,
	.enum_mbus_code		= vimc_sen_enum_mbus_code,
	.enum_frame_size	= vimc_sen_enum_frame_size,
	.get_fmt		= vimc_sen_get_fmt,
	.set_fmt		= vimc_sen_set_fmt,
};

static int vimc_sen_tpg_thread(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;

		tpg_fill_plane_buffer(&vsen->tpg, 0, 0, vsen->frame);

		/* 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;
		unsigned int frame_size;

		if (vsen->kthread_sen)
			/* tpg is already executing */
			return 0;

		/* Calculate the frame size */
		vpix = vimc_pix_map_by_code(vsen->mbus_format.code);
		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(frame_size);
		if (!vsen->frame)
			return -ENOMEM;

		/* configure the test pattern generator */
		vimc_sen_tpg_s_format(vsen);

		/* Initialize the image generator thread */
		vsen->kthread_sen = kthread_run(vimc_sen_tpg_thread, vsen,
					"%s-sen", vsen->sd.v4l2_dev->name);
		if (IS_ERR(vsen->kthread_sen)) {
			dev_err(vsen->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 0;

		/* Stop image generator */
		ret = kthread_stop(vsen->kthread_sen);
		if (ret)
			return ret;

		vsen->kthread_sen = NULL;
		vfree(vsen->frame);
		vsen->frame = NULL;
		return 0;
	}

	return 0;
}

static 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_comp_unbind(struct device *comp, struct device *master,
				 void *master_data)
{
	struct vimc_ent_device *ved = dev_get_drvdata(comp);
	struct vimc_sen_device *vsen =
				container_of(ved, struct vimc_sen_device, ved);

	vimc_ent_sd_unregister(ved, &vsen->sd);
	tpg_free(&vsen->tpg);
	kfree(vsen);
}

static int vimc_sen_comp_bind(struct device *comp, struct device *master,
			      void *master_data)
{
	struct v4l2_device *v4l2_dev = master_data;
	struct vimc_platform_data *pdata = comp->platform_data;
	struct vimc_sen_device *vsen;
	int ret;

	/* Allocate the vsen struct */
	vsen = kzalloc(sizeof(*vsen), GFP_KERNEL);
	if (!vsen)
		return -ENOMEM;

	/* Initialize ved and sd */
	ret = vimc_ent_sd_register(&vsen->ved, &vsen->sd, v4l2_dev,
				   pdata->entity_name,
				   MEDIA_ENT_F_ATV_DECODER, 1,
				   (const unsigned long[1]) {MEDIA_PAD_FL_SOURCE},
				   &vimc_sen_ops);
	if (ret)
		goto err_free_vsen;

	dev_set_drvdata(comp, &vsen->ved);
	vsen->dev = comp;

	/* Initialize the frame format */
	vsen->mbus_format = fmt_default;

	/* Initialize the test pattern generator */
	tpg_init(&vsen->tpg, vsen->mbus_format.width,
		 vsen->mbus_format.height);
	ret = tpg_alloc(&vsen->tpg, VIMC_FRAME_MAX_WIDTH);
	if (ret)
		goto err_unregister_ent_sd;

	return 0;

err_unregister_ent_sd:
	vimc_ent_sd_unregister(&vsen->ved,  &vsen->sd);
err_free_vsen:
	kfree(vsen);

	return ret;
}

static const struct component_ops vimc_sen_comp_ops = {
	.bind = vimc_sen_comp_bind,
	.unbind = vimc_sen_comp_unbind,
};

static int vimc_sen_probe(struct platform_device *pdev)
{
	return component_add(&pdev->dev, &vimc_sen_comp_ops);
}

static int vimc_sen_remove(struct platform_device *pdev)
{
	component_del(&pdev->dev, &vimc_sen_comp_ops);

	return 0;
}

static const struct platform_device_id vimc_sen_driver_ids[] = {
	{
		.name           = VIMC_SEN_DRV_NAME,
	},
	{ }
};

static struct platform_driver vimc_sen_pdrv = {
	.probe		= vimc_sen_probe,
	.remove		= vimc_sen_remove,
	.id_table	= vimc_sen_driver_ids,
	.driver		= {
		.name	= VIMC_SEN_DRV_NAME,
	},
};

module_platform_driver(vimc_sen_pdrv);

MODULE_DEVICE_TABLE(platform, vimc_sen_driver_ids);

MODULE_DESCRIPTION("Virtual Media Controller Driver (VIMC) Sensor");
MODULE_AUTHOR("Helen Mae Koike Fornazier <helen.fornazier@gmail.com>");
MODULE_LICENSE("GPL");
Commit	Line	Data
f2fe8906 HK	1	/*
	2	* vimc-sensor.c Virtual Media Controller Driver
	3	*
	4	* Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
	5	*
	6	* This program is free software; you can redistribute it and/or modify
	7	* it under the terms of the GNU General Public License as published by
	8	* the Free Software Foundation; either version 2 of the License, or
	9	* (at your option) any later version.
	10	*
	11	* This program is distributed in the hope that it will be useful,
	12	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	13	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	14	* GNU General Public License for more details.
	15	*
	16	*/
	17
4a29b709	18	#include <linux/component.h>
f2fe8906 HK	19	#include <linux/freezer.h>
f2fe8906 HK	20	#include <linux/kthread.h>
4a29b709 HF	21	#include <linux/module.h>
4a29b709 HF	22	#include <linux/platform_device.h>
f2fe8906 HK	23	#include <linux/v4l2-mediabus.h>
	24	#include <linux/vmalloc.h>
	25	#include <media/v4l2-subdev.h>
554946c3	26	#include <media/v4l2-tpg.h>
f2fe8906	27
4a29b709 HF	28	#include "vimc-common.h"
	29
	30	#define VIMC_SEN_DRV_NAME "vimc-sensor"
f2fe8906 HK	31
	32	struct vimc_sen_device {
	33	struct vimc_ent_device ved;
	34	struct v4l2_subdev sd;
4a29b709	35	struct device *dev;
554946c3	36	struct tpg_data tpg;
f2fe8906 HK	37	struct task_struct *kthread_sen;
	38	u8 *frame;
	39	/* The active format */
	40	struct v4l2_mbus_framefmt mbus_format;
f2fe8906 HK	41	};
f2fe8906 HK	42
88ad71aa HF	43	static const struct v4l2_mbus_framefmt fmt_default = {
	44	.width = 640,
	45	.height = 480,
	46	.code = MEDIA_BUS_FMT_RGB888_1X24,
	47	.field = V4L2_FIELD_NONE,
	48	.colorspace = V4L2_COLORSPACE_DEFAULT,
	49	};
	50
	51	static int vimc_sen_init_cfg(struct v4l2_subdev *sd,
	52	struct v4l2_subdev_pad_config *cfg)
	53	{
	54	unsigned int i;
	55
	56	for (i = 0; i < sd->entity.num_pads; i++) {
	57	struct v4l2_mbus_framefmt *mf;
	58
	59	mf = v4l2_subdev_get_try_format(sd, cfg, i);
	60	*mf = fmt_default;
	61	}
	62
	63	return 0;
	64	}
	65
f2fe8906 HK	66	static int vimc_sen_enum_mbus_code(struct v4l2_subdev *sd,
	67	struct v4l2_subdev_pad_config *cfg,
	68	struct v4l2_subdev_mbus_code_enum *code)
	69	{
88ad71aa	70	const struct vimc_pix_map *vpix = vimc_pix_map_by_index(code->index);
f2fe8906	71
88ad71aa	72	if (!vpix)
f2fe8906 HK	73	return -EINVAL;
f2fe8906 HK	74
88ad71aa	75	code->code = vpix->code;
f2fe8906 HK	76
	77	return 0;
	78	}
	79
	80	static int vimc_sen_enum_frame_size(struct v4l2_subdev *sd,
	81	struct v4l2_subdev_pad_config *cfg,
	82	struct v4l2_subdev_frame_size_enum *fse)
	83	{
88ad71aa	84	const struct vimc_pix_map *vpix;
f2fe8906	85
f2fe8906 HK	86	if (fse->index)
	87	return -EINVAL;
	88
88ad71aa HF	89	/* Only accept code in the pix map table */
	90	vpix = vimc_pix_map_by_code(fse->code);
	91	if (!vpix)
f2fe8906 HK	92	return -EINVAL;
f2fe8906 HK	93
88ad71aa HF	94	fse->min_width = VIMC_FRAME_MIN_WIDTH;
	95	fse->max_width = VIMC_FRAME_MAX_WIDTH;
	96	fse->min_height = VIMC_FRAME_MIN_HEIGHT;
	97	fse->max_height = VIMC_FRAME_MAX_HEIGHT;
f2fe8906 HK	98
	99	return 0;
	100	}
	101
	102	static int vimc_sen_get_fmt(struct v4l2_subdev *sd,
	103	struct v4l2_subdev_pad_config *cfg,
88ad71aa	104	struct v4l2_subdev_format *fmt)
f2fe8906 HK	105	{
	106	struct vimc_sen_device *vsen =
	107	container_of(sd, struct vimc_sen_device, sd);
	108
88ad71aa HF	109	fmt->format = fmt->which == V4L2_SUBDEV_FORMAT_TRY ?
	110	*v4l2_subdev_get_try_format(sd, cfg, fmt->pad) :
	111	vsen->mbus_format;
f2fe8906 HK	112
	113	return 0;
	114	}
	115
554946c3 HF	116	static void vimc_sen_tpg_s_format(struct vimc_sen_device *vsen)
	117	{
	118	const struct vimc_pix_map *vpix =
	119	vimc_pix_map_by_code(vsen->mbus_format.code);
	120
	121	tpg_reset_source(&vsen->tpg, vsen->mbus_format.width,
	122	vsen->mbus_format.height, vsen->mbus_format.field);
	123	tpg_s_bytesperline(&vsen->tpg, 0, vsen->mbus_format.width * vpix->bpp);
	124	tpg_s_buf_height(&vsen->tpg, vsen->mbus_format.height);
	125	tpg_s_fourcc(&vsen->tpg, vpix->pixelformat);
	126	/* TODO: add support for V4L2_FIELD_ALTERNATE */
	127	tpg_s_field(&vsen->tpg, vsen->mbus_format.field, false);
	128	tpg_s_colorspace(&vsen->tpg, vsen->mbus_format.colorspace);
	129	tpg_s_ycbcr_enc(&vsen->tpg, vsen->mbus_format.ycbcr_enc);
	130	tpg_s_quantization(&vsen->tpg, vsen->mbus_format.quantization);
	131	tpg_s_xfer_func(&vsen->tpg, vsen->mbus_format.xfer_func);
	132	}
	133
88ad71aa HF	134	static void vimc_sen_adjust_fmt(struct v4l2_mbus_framefmt *fmt)
	135	{
	136	const struct vimc_pix_map *vpix;
	137
	138	/* Only accept code in the pix map table */
	139	vpix = vimc_pix_map_by_code(fmt->code);
	140	if (!vpix)
	141	fmt->code = fmt_default.code;
	142
	143	fmt->width = clamp_t(u32, fmt->width, VIMC_FRAME_MIN_WIDTH,
	144	VIMC_FRAME_MAX_WIDTH) & ~1;
	145	fmt->height = clamp_t(u32, fmt->height, VIMC_FRAME_MIN_HEIGHT,
	146	VIMC_FRAME_MAX_HEIGHT) & ~1;
	147
	148	/* TODO: add support for V4L2_FIELD_ALTERNATE */
	149	if (fmt->field == V4L2_FIELD_ANY \|\| fmt->field == V4L2_FIELD_ALTERNATE)
	150	fmt->field = fmt_default.field;
	151
	152	vimc_colorimetry_clamp(fmt);
	153	}
	154
	155	static int vimc_sen_set_fmt(struct v4l2_subdev *sd,
	156	struct v4l2_subdev_pad_config *cfg,
	157	struct v4l2_subdev_format *fmt)
	158	{
	159	struct vimc_sen_device *vsen = v4l2_get_subdevdata(sd);
	160	struct v4l2_mbus_framefmt *mf;
	161
	162	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
	163	/* Do not change the format while stream is on */
	164	if (vsen->frame)
	165	return -EBUSY;
	166
	167	mf = &vsen->mbus_format;
	168	} else {
	169	mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
	170	}
	171
	172	/* Set the new format */
	173	vimc_sen_adjust_fmt(&fmt->format);
	174
4a29b709	175	dev_dbg(vsen->dev, "%s: format update: "
88ad71aa HF	176	"old:%dx%d (0x%x, %d, %d, %d, %d) "
	177	"new:%dx%d (0x%x, %d, %d, %d, %d)\n", vsen->sd.name,
	178	/* old */
	179	mf->width, mf->height, mf->code,
	180	mf->colorspace, mf->quantization,
	181	mf->xfer_func, mf->ycbcr_enc,
	182	/* new */
	183	fmt->format.width, fmt->format.height, fmt->format.code,
	184	fmt->format.colorspace, fmt->format.quantization,
	185	fmt->format.xfer_func, fmt->format.ycbcr_enc);
	186
	187	*mf = fmt->format;
	188
	189	return 0;
	190	}
	191
f2fe8906	192	static const struct v4l2_subdev_pad_ops vimc_sen_pad_ops = {
88ad71aa	193	.init_cfg = vimc_sen_init_cfg,
f2fe8906 HK	194	.enum_mbus_code = vimc_sen_enum_mbus_code,
	195	.enum_frame_size = vimc_sen_enum_frame_size,
	196	.get_fmt = vimc_sen_get_fmt,
88ad71aa	197	.set_fmt = vimc_sen_set_fmt,
f2fe8906 HK	198	};
f2fe8906 HK	199
554946c3	200	static int vimc_sen_tpg_thread(void *data)
f2fe8906 HK	201	{
	202	struct vimc_sen_device *vsen = data;
	203	unsigned int i;
	204
	205	set_freezable();
	206	set_current_state(TASK_UNINTERRUPTIBLE);
	207
	208	for (;;) {
	209	try_to_freeze();
	210	if (kthread_should_stop())
	211	break;
	212
554946c3	213	tpg_fill_plane_buffer(&vsen->tpg, 0, 0, vsen->frame);
f2fe8906 HK	214
	215	/* Send the frame to all source pads */
	216	for (i = 0; i < vsen->sd.entity.num_pads; i++)
	217	vimc_propagate_frame(&vsen->sd.entity.pads[i],
	218	vsen->frame);
	219
	220	/* 60 frames per second */
	221	schedule_timeout(HZ/60);
	222	}
	223
	224	return 0;
	225	}
	226
	227	static int vimc_sen_s_stream(struct v4l2_subdev *sd, int enable)
	228	{
	229	struct vimc_sen_device *vsen =
	230	container_of(sd, struct vimc_sen_device, sd);
	231	int ret;
	232
	233	if (enable) {
	234	const struct vimc_pix_map *vpix;
554946c3	235	unsigned int frame_size;
f2fe8906 HK	236
f2fe8906 HK	237	if (vsen->kthread_sen)
554946c3 HF	238	/* tpg is already executing */
554946c3 HF	239	return 0;
f2fe8906 HK	240
	241	/* Calculate the frame size */
	242	vpix = vimc_pix_map_by_code(vsen->mbus_format.code);
554946c3 HF	243	frame_size = vsen->mbus_format.width * vpix->bpp *
554946c3 HF	244	vsen->mbus_format.height;
f2fe8906 HK	245
	246	/*
	247	* Allocate the frame buffer. Use vmalloc to be able to
	248	* allocate a large amount of memory
	249	*/
554946c3	250	vsen->frame = vmalloc(frame_size);
f2fe8906 HK	251	if (!vsen->frame)
	252	return -ENOMEM;
	253
554946c3 HF	254	/* configure the test pattern generator */
	255	vimc_sen_tpg_s_format(vsen);
	256
f2fe8906	257	/* Initialize the image generator thread */
554946c3 HF	258	vsen->kthread_sen = kthread_run(vimc_sen_tpg_thread, vsen,
554946c3 HF	259	"%s-sen", vsen->sd.v4l2_dev->name);
f2fe8906	260	if (IS_ERR(vsen->kthread_sen)) {
4a29b709 HF	261	dev_err(vsen->dev, "%s: kernel_thread() failed\n",
4a29b709 HF	262	vsen->sd.name);
f2fe8906 HK	263	vfree(vsen->frame);
	264	vsen->frame = NULL;
	265	return PTR_ERR(vsen->kthread_sen);
	266	}
	267	} else {
	268	if (!vsen->kthread_sen)
554946c3	269	return 0;
f2fe8906 HK	270
	271	/* Stop image generator */
	272	ret = kthread_stop(vsen->kthread_sen);
554946c3 HF	273	if (ret)
554946c3 HF	274	return ret;
f2fe8906	275
554946c3	276	vsen->kthread_sen = NULL;
f2fe8906 HK	277	vfree(vsen->frame);
f2fe8906 HK	278	vsen->frame = NULL;
554946c3	279	return 0;
f2fe8906 HK	280	}
	281
	282	return 0;
	283	}
	284
0e8298e5	285	static struct v4l2_subdev_video_ops vimc_sen_video_ops = {
f2fe8906 HK	286	.s_stream = vimc_sen_s_stream,
	287	};
	288
	289	static const struct v4l2_subdev_ops vimc_sen_ops = {
	290	.pad = &vimc_sen_pad_ops,
	291	.video = &vimc_sen_video_ops,
	292	};
	293
4a29b709 HF	294	static void vimc_sen_comp_unbind(struct device comp, struct device master,
4a29b709 HF	295	void *master_data)
f2fe8906	296	{
4a29b709	297	struct vimc_ent_device *ved = dev_get_drvdata(comp);
f2fe8906 HK	298	struct vimc_sen_device *vsen =
	299	container_of(ved, struct vimc_sen_device, ved);
	300
c149543e	301	vimc_ent_sd_unregister(ved, &vsen->sd);
554946c3	302	tpg_free(&vsen->tpg);
f2fe8906 HK	303	kfree(vsen);
	304	}
	305
4a29b709 HF	306	static int vimc_sen_comp_bind(struct device comp, struct device master,
4a29b709 HF	307	void *master_data)
f2fe8906	308	{
4a29b709 HF	309	struct v4l2_device *v4l2_dev = master_data;
4a29b709 HF	310	struct vimc_platform_data *pdata = comp->platform_data;
f2fe8906	311	struct vimc_sen_device *vsen;
f2fe8906 HK	312	int ret;
f2fe8906 HK	313
f2fe8906 HK	314	/* Allocate the vsen struct */
	315	vsen = kzalloc(sizeof(*vsen), GFP_KERNEL);
	316	if (!vsen)
4a29b709	317	return -ENOMEM;
f2fe8906	318
c149543e	319	/* Initialize ved and sd */
4a29b709 HF	320	ret = vimc_ent_sd_register(&vsen->ved, &vsen->sd, v4l2_dev,
	321	pdata->entity_name,
	322	MEDIA_ENT_F_ATV_DECODER, 1,
	323	(const unsigned long[1]) {MEDIA_PAD_FL_SOURCE},
	324	&vimc_sen_ops);
f2fe8906	325	if (ret)
c149543e	326	goto err_free_vsen;
f2fe8906	327
4a29b709 HF	328	dev_set_drvdata(comp, &vsen->ved);
	329	vsen->dev = comp;
	330
88ad71aa HF	331	/* Initialize the frame format */
88ad71aa HF	332	vsen->mbus_format = fmt_default;
f2fe8906	333
554946c3 HF	334	/* Initialize the test pattern generator */
	335	tpg_init(&vsen->tpg, vsen->mbus_format.width,
	336	vsen->mbus_format.height);
88ad71aa	337	ret = tpg_alloc(&vsen->tpg, VIMC_FRAME_MAX_WIDTH);
554946c3	338	if (ret)
c149543e	339	goto err_unregister_ent_sd;
f2fe8906	340
4a29b709	341	return 0;
f2fe8906	342
c149543e HF	343	err_unregister_ent_sd:
c149543e HF	344	vimc_ent_sd_unregister(&vsen->ved, &vsen->sd);
f2fe8906 HK	345	err_free_vsen:
	346	kfree(vsen);
	347
4a29b709	348	return ret;
f2fe8906	349	}
4a29b709 HF	350
	351	static const struct component_ops vimc_sen_comp_ops = {
	352	.bind = vimc_sen_comp_bind,
	353	.unbind = vimc_sen_comp_unbind,
	354	};
	355
	356	static int vimc_sen_probe(struct platform_device *pdev)
	357	{
	358	return component_add(&pdev->dev, &vimc_sen_comp_ops);
	359	}
	360
	361	static int vimc_sen_remove(struct platform_device *pdev)
	362	{
	363	component_del(&pdev->dev, &vimc_sen_comp_ops);
	364
	365	return 0;
	366	}
	367
bf183e0f JMC	368	static const struct platform_device_id vimc_sen_driver_ids[] = {
	369	{
	370	.name = VIMC_SEN_DRV_NAME,
	371	},
	372	{ }
	373	};
	374
4a29b709 HF	375	static struct platform_driver vimc_sen_pdrv = {
	376	.probe = vimc_sen_probe,
	377	.remove = vimc_sen_remove,
bf183e0f	378	.id_table = vimc_sen_driver_ids,
4a29b709 HF	379	.driver = {
	380	.name = VIMC_SEN_DRV_NAME,
	381	},
	382	};
	383
4a29b709 HF	384	module_platform_driver(vimc_sen_pdrv);
	385
	386	MODULE_DEVICE_TABLE(platform, vimc_sen_driver_ids);
	387
	388	MODULE_DESCRIPTION("Virtual Media Controller Driver (VIMC) Sensor");
	389	MODULE_AUTHOR("Helen Mae Koike Fornazier <helen.fornazier@gmail.com>");
	390	MODULE_LICENSE("GPL");