Linux kernel mirror (for testing)
git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git
tjh.dev
kernel
os
linux
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
18#include <linux/freezer.h>
19#include <linux/kthread.h>
20#include <linux/v4l2-mediabus.h>
21#include <linux/vmalloc.h>
22#include <media/v4l2-subdev.h>
23
24#include "vimc-sensor.h"
25
26struct vimc_sen_device {
27 struct vimc_ent_device ved;
28 struct v4l2_subdev sd;
29 struct task_struct *kthread_sen;
30 u8 *frame;
31 /* The active format */
32 struct v4l2_mbus_framefmt mbus_format;
33 int frame_size;
34};
35
36static int vimc_sen_enum_mbus_code(struct v4l2_subdev *sd,
37 struct v4l2_subdev_pad_config *cfg,
38 struct v4l2_subdev_mbus_code_enum *code)
39{
40 struct vimc_sen_device *vsen =
41 container_of(sd, struct vimc_sen_device, sd);
42
43 /* TODO: Add support for other codes */
44 if (code->index)
45 return -EINVAL;
46
47 code->code = vsen->mbus_format.code;
48
49 return 0;
50}
51
52static int vimc_sen_enum_frame_size(struct v4l2_subdev *sd,
53 struct v4l2_subdev_pad_config *cfg,
54 struct v4l2_subdev_frame_size_enum *fse)
55{
56 struct vimc_sen_device *vsen =
57 container_of(sd, struct vimc_sen_device, sd);
58
59 /* TODO: Add support to other formats */
60 if (fse->index)
61 return -EINVAL;
62
63 /* TODO: Add support for other codes */
64 if (fse->code != vsen->mbus_format.code)
65 return -EINVAL;
66
67 fse->min_width = vsen->mbus_format.width;
68 fse->max_width = vsen->mbus_format.width;
69 fse->min_height = vsen->mbus_format.height;
70 fse->max_height = vsen->mbus_format.height;
71
72 return 0;
73}
74
75static int vimc_sen_get_fmt(struct v4l2_subdev *sd,
76 struct v4l2_subdev_pad_config *cfg,
77 struct v4l2_subdev_format *format)
78{
79 struct vimc_sen_device *vsen =
80 container_of(sd, struct vimc_sen_device, sd);
81
82 format->format = vsen->mbus_format;
83
84 return 0;
85}
86
87static const struct v4l2_subdev_pad_ops vimc_sen_pad_ops = {
88 .enum_mbus_code = vimc_sen_enum_mbus_code,
89 .enum_frame_size = vimc_sen_enum_frame_size,
90 .get_fmt = vimc_sen_get_fmt,
91 /* TODO: Add support to other formats */
92 .set_fmt = vimc_sen_get_fmt,
93};
94
95/* media operations */
96static const struct media_entity_operations vimc_sen_mops = {
97 .link_validate = v4l2_subdev_link_validate,
98};
99
100static int vimc_thread_sen(void *data)
101{
102 struct vimc_sen_device *vsen = data;
103 unsigned int i;
104
105 set_freezable();
106 set_current_state(TASK_UNINTERRUPTIBLE);
107
108 for (;;) {
109 try_to_freeze();
110 if (kthread_should_stop())
111 break;
112
113 memset(vsen->frame, 100, vsen->frame_size);
114
115 /* Send the frame to all source pads */
116 for (i = 0; i < vsen->sd.entity.num_pads; i++)
117 vimc_propagate_frame(&vsen->sd.entity.pads[i],
118 vsen->frame);
119
120 /* 60 frames per second */
121 schedule_timeout(HZ/60);
122 }
123
124 return 0;
125}
126
127static int vimc_sen_s_stream(struct v4l2_subdev *sd, int enable)
128{
129 struct vimc_sen_device *vsen =
130 container_of(sd, struct vimc_sen_device, sd);
131 int ret;
132
133 if (enable) {
134 const struct vimc_pix_map *vpix;
135
136 if (vsen->kthread_sen)
137 return -EINVAL;
138
139 /* Calculate the frame size */
140 vpix = vimc_pix_map_by_code(vsen->mbus_format.code);
141 vsen->frame_size = vsen->mbus_format.width * vpix->bpp *
142 vsen->mbus_format.height;
143
144 /*
145 * Allocate the frame buffer. Use vmalloc to be able to
146 * allocate a large amount of memory
147 */
148 vsen->frame = vmalloc(vsen->frame_size);
149 if (!vsen->frame)
150 return -ENOMEM;
151
152 /* Initialize the image generator thread */
153 vsen->kthread_sen = kthread_run(vimc_thread_sen, vsen, "%s-sen",
154 vsen->sd.v4l2_dev->name);
155 if (IS_ERR(vsen->kthread_sen)) {
156 dev_err(vsen->sd.v4l2_dev->dev,
157 "%s: kernel_thread() failed\n", vsen->sd.name);
158 vfree(vsen->frame);
159 vsen->frame = NULL;
160 return PTR_ERR(vsen->kthread_sen);
161 }
162 } else {
163 if (!vsen->kthread_sen)
164 return -EINVAL;
165
166 /* Stop image generator */
167 ret = kthread_stop(vsen->kthread_sen);
168 vsen->kthread_sen = NULL;
169
170 vfree(vsen->frame);
171 vsen->frame = NULL;
172 return ret;
173 }
174
175 return 0;
176}
177
178struct v4l2_subdev_video_ops vimc_sen_video_ops = {
179 .s_stream = vimc_sen_s_stream,
180};
181
182static const struct v4l2_subdev_ops vimc_sen_ops = {
183 .pad = &vimc_sen_pad_ops,
184 .video = &vimc_sen_video_ops,
185};
186
187static void vimc_sen_destroy(struct vimc_ent_device *ved)
188{
189 struct vimc_sen_device *vsen =
190 container_of(ved, struct vimc_sen_device, ved);
191
192 v4l2_device_unregister_subdev(&vsen->sd);
193 media_entity_cleanup(ved->ent);
194 kfree(vsen);
195}
196
197struct vimc_ent_device *vimc_sen_create(struct v4l2_device *v4l2_dev,
198 const char *const name,
199 u16 num_pads,
200 const unsigned long *pads_flag)
201{
202 struct vimc_sen_device *vsen;
203 unsigned int i;
204 int ret;
205
206 /* NOTE: a sensor node may be created with more then one pad */
207 if (!name || !num_pads || !pads_flag)
208 return ERR_PTR(-EINVAL);
209
210 /* check if all pads are sources */
211 for (i = 0; i < num_pads; i++)
212 if (!(pads_flag[i] & MEDIA_PAD_FL_SOURCE))
213 return ERR_PTR(-EINVAL);
214
215 /* Allocate the vsen struct */
216 vsen = kzalloc(sizeof(*vsen), GFP_KERNEL);
217 if (!vsen)
218 return ERR_PTR(-ENOMEM);
219
220 /* Allocate the pads */
221 vsen->ved.pads = vimc_pads_init(num_pads, pads_flag);
222 if (IS_ERR(vsen->ved.pads)) {
223 ret = PTR_ERR(vsen->ved.pads);
224 goto err_free_vsen;
225 }
226
227 /* Fill the vimc_ent_device struct */
228 vsen->ved.destroy = vimc_sen_destroy;
229 vsen->ved.ent = &vsen->sd.entity;
230
231 /* Initialize the subdev */
232 v4l2_subdev_init(&vsen->sd, &vimc_sen_ops);
233 vsen->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
234 vsen->sd.entity.ops = &vimc_sen_mops;
235 vsen->sd.owner = THIS_MODULE;
236 strlcpy(vsen->sd.name, name, sizeof(vsen->sd.name));
237 v4l2_set_subdevdata(&vsen->sd, &vsen->ved);
238
239 /* Expose this subdev to user space */
240 vsen->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
241
242 /* Initialize the media entity */
243 ret = media_entity_pads_init(&vsen->sd.entity,
244 num_pads, vsen->ved.pads);
245 if (ret)
246 goto err_clean_pads;
247
248 /* Set the active frame format (this is hardcoded for now) */
249 vsen->mbus_format.width = 640;
250 vsen->mbus_format.height = 480;
251 vsen->mbus_format.code = MEDIA_BUS_FMT_RGB888_1X24;
252 vsen->mbus_format.field = V4L2_FIELD_NONE;
253 vsen->mbus_format.colorspace = V4L2_COLORSPACE_SRGB;
254 vsen->mbus_format.quantization = V4L2_QUANTIZATION_FULL_RANGE;
255 vsen->mbus_format.xfer_func = V4L2_XFER_FUNC_SRGB;
256
257 /* Register the subdev with the v4l2 and the media framework */
258 ret = v4l2_device_register_subdev(v4l2_dev, &vsen->sd);
259 if (ret) {
260 dev_err(vsen->sd.v4l2_dev->dev,
261 "%s: subdev register failed (err=%d)\n",
262 vsen->sd.name, ret);
263 goto err_clean_m_ent;
264 }
265
266 return &vsen->ved;
267
268err_clean_m_ent:
269 media_entity_cleanup(&vsen->sd.entity);
270err_clean_pads:
271 vimc_pads_cleanup(vsen->ved.pads);
272err_free_vsen:
273 kfree(vsen);
274
275 return ERR_PTR(ret);
276}