]>
Commit | Line | Data |
---|---|---|
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/component.h> | |
19 | #include <linux/freezer.h> | |
20 | #include <linux/kthread.h> | |
21 | #include <linux/module.h> | |
22 | #include <linux/platform_device.h> | |
23 | #include <linux/v4l2-mediabus.h> | |
24 | #include <linux/vmalloc.h> | |
25 | #include <media/v4l2-subdev.h> | |
26 | #include <media/v4l2-tpg.h> | |
27 | ||
28 | #include "vimc-common.h" | |
29 | ||
30 | #define VIMC_SEN_DRV_NAME "vimc-sensor" | |
31 | ||
32 | struct vimc_sen_device { | |
33 | struct vimc_ent_device ved; | |
34 | struct v4l2_subdev sd; | |
35 | struct device *dev; | |
36 | struct tpg_data tpg; | |
37 | struct task_struct *kthread_sen; | |
38 | u8 *frame; | |
39 | /* The active format */ | |
40 | struct v4l2_mbus_framefmt mbus_format; | |
41 | }; | |
42 | ||
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 | ||
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 | { | |
70 | const struct vimc_pix_map *vpix = vimc_pix_map_by_index(code->index); | |
71 | ||
72 | if (!vpix) | |
73 | return -EINVAL; | |
74 | ||
75 | code->code = vpix->code; | |
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 | { | |
84 | const struct vimc_pix_map *vpix; | |
85 | ||
86 | if (fse->index) | |
87 | return -EINVAL; | |
88 | ||
89 | /* Only accept code in the pix map table */ | |
90 | vpix = vimc_pix_map_by_code(fse->code); | |
91 | if (!vpix) | |
92 | return -EINVAL; | |
93 | ||
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; | |
98 | ||
99 | return 0; | |
100 | } | |
101 | ||
102 | static int vimc_sen_get_fmt(struct v4l2_subdev *sd, | |
103 | struct v4l2_subdev_pad_config *cfg, | |
104 | struct v4l2_subdev_format *fmt) | |
105 | { | |
106 | struct vimc_sen_device *vsen = | |
107 | container_of(sd, struct vimc_sen_device, sd); | |
108 | ||
109 | fmt->format = fmt->which == V4L2_SUBDEV_FORMAT_TRY ? | |
110 | *v4l2_subdev_get_try_format(sd, cfg, fmt->pad) : | |
111 | vsen->mbus_format; | |
112 | ||
113 | return 0; | |
114 | } | |
115 | ||
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 | ||
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 | ||
175 | dev_dbg(vsen->dev, "%s: format update: " | |
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 | ||
192 | static const struct v4l2_subdev_pad_ops vimc_sen_pad_ops = { | |
193 | .init_cfg = vimc_sen_init_cfg, | |
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, | |
197 | .set_fmt = vimc_sen_set_fmt, | |
198 | }; | |
199 | ||
200 | static int vimc_sen_tpg_thread(void *data) | |
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 | ||
213 | tpg_fill_plane_buffer(&vsen->tpg, 0, 0, vsen->frame); | |
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; | |
235 | unsigned int frame_size; | |
236 | ||
237 | if (vsen->kthread_sen) | |
238 | /* tpg is already executing */ | |
239 | return 0; | |
240 | ||
241 | /* Calculate the frame size */ | |
242 | vpix = vimc_pix_map_by_code(vsen->mbus_format.code); | |
243 | frame_size = vsen->mbus_format.width * vpix->bpp * | |
244 | vsen->mbus_format.height; | |
245 | ||
246 | /* | |
247 | * Allocate the frame buffer. Use vmalloc to be able to | |
248 | * allocate a large amount of memory | |
249 | */ | |
250 | vsen->frame = vmalloc(frame_size); | |
251 | if (!vsen->frame) | |
252 | return -ENOMEM; | |
253 | ||
254 | /* configure the test pattern generator */ | |
255 | vimc_sen_tpg_s_format(vsen); | |
256 | ||
257 | /* Initialize the image generator thread */ | |
258 | vsen->kthread_sen = kthread_run(vimc_sen_tpg_thread, vsen, | |
259 | "%s-sen", vsen->sd.v4l2_dev->name); | |
260 | if (IS_ERR(vsen->kthread_sen)) { | |
261 | dev_err(vsen->dev, "%s: kernel_thread() failed\n", | |
262 | vsen->sd.name); | |
263 | vfree(vsen->frame); | |
264 | vsen->frame = NULL; | |
265 | return PTR_ERR(vsen->kthread_sen); | |
266 | } | |
267 | } else { | |
268 | if (!vsen->kthread_sen) | |
269 | return 0; | |
270 | ||
271 | /* Stop image generator */ | |
272 | ret = kthread_stop(vsen->kthread_sen); | |
273 | if (ret) | |
274 | return ret; | |
275 | ||
276 | vsen->kthread_sen = NULL; | |
277 | vfree(vsen->frame); | |
278 | vsen->frame = NULL; | |
279 | return 0; | |
280 | } | |
281 | ||
282 | return 0; | |
283 | } | |
284 | ||
285 | static struct v4l2_subdev_video_ops vimc_sen_video_ops = { | |
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 | ||
294 | static void vimc_sen_comp_unbind(struct device *comp, struct device *master, | |
295 | void *master_data) | |
296 | { | |
297 | struct vimc_ent_device *ved = dev_get_drvdata(comp); | |
298 | struct vimc_sen_device *vsen = | |
299 | container_of(ved, struct vimc_sen_device, ved); | |
300 | ||
301 | vimc_ent_sd_unregister(ved, &vsen->sd); | |
302 | tpg_free(&vsen->tpg); | |
303 | kfree(vsen); | |
304 | } | |
305 | ||
306 | static int vimc_sen_comp_bind(struct device *comp, struct device *master, | |
307 | void *master_data) | |
308 | { | |
309 | struct v4l2_device *v4l2_dev = master_data; | |
310 | struct vimc_platform_data *pdata = comp->platform_data; | |
311 | struct vimc_sen_device *vsen; | |
312 | int ret; | |
313 | ||
314 | /* Allocate the vsen struct */ | |
315 | vsen = kzalloc(sizeof(*vsen), GFP_KERNEL); | |
316 | if (!vsen) | |
317 | return -ENOMEM; | |
318 | ||
319 | /* Initialize ved and sd */ | |
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); | |
325 | if (ret) | |
326 | goto err_free_vsen; | |
327 | ||
328 | dev_set_drvdata(comp, &vsen->ved); | |
329 | vsen->dev = comp; | |
330 | ||
331 | /* Initialize the frame format */ | |
332 | vsen->mbus_format = fmt_default; | |
333 | ||
334 | /* Initialize the test pattern generator */ | |
335 | tpg_init(&vsen->tpg, vsen->mbus_format.width, | |
336 | vsen->mbus_format.height); | |
337 | ret = tpg_alloc(&vsen->tpg, VIMC_FRAME_MAX_WIDTH); | |
338 | if (ret) | |
339 | goto err_unregister_ent_sd; | |
340 | ||
341 | return 0; | |
342 | ||
343 | err_unregister_ent_sd: | |
344 | vimc_ent_sd_unregister(&vsen->ved, &vsen->sd); | |
345 | err_free_vsen: | |
346 | kfree(vsen); | |
347 | ||
348 | return ret; | |
349 | } | |
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 | ||
368 | static const struct platform_device_id vimc_sen_driver_ids[] = { | |
369 | { | |
370 | .name = VIMC_SEN_DRV_NAME, | |
371 | }, | |
372 | { } | |
373 | }; | |
374 | ||
375 | static struct platform_driver vimc_sen_pdrv = { | |
376 | .probe = vimc_sen_probe, | |
377 | .remove = vimc_sen_remove, | |
378 | .id_table = vimc_sen_driver_ids, | |
379 | .driver = { | |
380 | .name = VIMC_SEN_DRV_NAME, | |
381 | }, | |
382 | }; | |
383 | ||
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"); |