| /* |
| * 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); |
| } |