277 lines
6.9 KiB
C
277 lines
6.9 KiB
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/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);
|
|
}
|