1
2
3
4
5
6
7
8
9
10
11
12
13
14
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-ctrls.h>
26#include <media/v4l2-event.h>
27#include <media/v4l2-subdev.h>
28#include <media/tpg/v4l2-tpg.h>
29
30#include "vimc-common.h"
31
32#define VIMC_SEN_DRV_NAME "vimc-sensor"
33
34struct vimc_sen_device {
35 struct vimc_ent_device ved;
36 struct v4l2_subdev sd;
37 struct device *dev;
38 struct tpg_data tpg;
39 struct task_struct *kthread_sen;
40 u8 *frame;
41
42 struct v4l2_mbus_framefmt mbus_format;
43 struct v4l2_ctrl_handler hdl;
44};
45
46static const struct v4l2_mbus_framefmt fmt_default = {
47 .width = 640,
48 .height = 480,
49 .code = MEDIA_BUS_FMT_RGB888_1X24,
50 .field = V4L2_FIELD_NONE,
51 .colorspace = V4L2_COLORSPACE_DEFAULT,
52};
53
54static int vimc_sen_init_cfg(struct v4l2_subdev *sd,
55 struct v4l2_subdev_pad_config *cfg)
56{
57 unsigned int i;
58
59 for (i = 0; i < sd->entity.num_pads; i++) {
60 struct v4l2_mbus_framefmt *mf;
61
62 mf = v4l2_subdev_get_try_format(sd, cfg, i);
63 *mf = fmt_default;
64 }
65
66 return 0;
67}
68
69static int vimc_sen_enum_mbus_code(struct v4l2_subdev *sd,
70 struct v4l2_subdev_pad_config *cfg,
71 struct v4l2_subdev_mbus_code_enum *code)
72{
73 const struct vimc_pix_map *vpix = vimc_pix_map_by_index(code->index);
74
75 if (!vpix)
76 return -EINVAL;
77
78 code->code = vpix->code;
79
80 return 0;
81}
82
83static int vimc_sen_enum_frame_size(struct v4l2_subdev *sd,
84 struct v4l2_subdev_pad_config *cfg,
85 struct v4l2_subdev_frame_size_enum *fse)
86{
87 const struct vimc_pix_map *vpix;
88
89 if (fse->index)
90 return -EINVAL;
91
92
93 vpix = vimc_pix_map_by_code(fse->code);
94 if (!vpix)
95 return -EINVAL;
96
97 fse->min_width = VIMC_FRAME_MIN_WIDTH;
98 fse->max_width = VIMC_FRAME_MAX_WIDTH;
99 fse->min_height = VIMC_FRAME_MIN_HEIGHT;
100 fse->max_height = VIMC_FRAME_MAX_HEIGHT;
101
102 return 0;
103}
104
105static int vimc_sen_get_fmt(struct v4l2_subdev *sd,
106 struct v4l2_subdev_pad_config *cfg,
107 struct v4l2_subdev_format *fmt)
108{
109 struct vimc_sen_device *vsen =
110 container_of(sd, struct vimc_sen_device, sd);
111
112 fmt->format = fmt->which == V4L2_SUBDEV_FORMAT_TRY ?
113 *v4l2_subdev_get_try_format(sd, cfg, fmt->pad) :
114 vsen->mbus_format;
115
116 return 0;
117}
118
119static void vimc_sen_tpg_s_format(struct vimc_sen_device *vsen)
120{
121 const struct vimc_pix_map *vpix =
122 vimc_pix_map_by_code(vsen->mbus_format.code);
123
124 tpg_reset_source(&vsen->tpg, vsen->mbus_format.width,
125 vsen->mbus_format.height, vsen->mbus_format.field);
126 tpg_s_bytesperline(&vsen->tpg, 0, vsen->mbus_format.width * vpix->bpp);
127 tpg_s_buf_height(&vsen->tpg, vsen->mbus_format.height);
128 tpg_s_fourcc(&vsen->tpg, vpix->pixelformat);
129
130 tpg_s_field(&vsen->tpg, vsen->mbus_format.field, false);
131 tpg_s_colorspace(&vsen->tpg, vsen->mbus_format.colorspace);
132 tpg_s_ycbcr_enc(&vsen->tpg, vsen->mbus_format.ycbcr_enc);
133 tpg_s_quantization(&vsen->tpg, vsen->mbus_format.quantization);
134 tpg_s_xfer_func(&vsen->tpg, vsen->mbus_format.xfer_func);
135}
136
137static void vimc_sen_adjust_fmt(struct v4l2_mbus_framefmt *fmt)
138{
139 const struct vimc_pix_map *vpix;
140
141
142 vpix = vimc_pix_map_by_code(fmt->code);
143 if (!vpix)
144 fmt->code = fmt_default.code;
145
146 fmt->width = clamp_t(u32, fmt->width, VIMC_FRAME_MIN_WIDTH,
147 VIMC_FRAME_MAX_WIDTH) & ~1;
148 fmt->height = clamp_t(u32, fmt->height, VIMC_FRAME_MIN_HEIGHT,
149 VIMC_FRAME_MAX_HEIGHT) & ~1;
150
151
152 if (fmt->field == V4L2_FIELD_ANY || fmt->field == V4L2_FIELD_ALTERNATE)
153 fmt->field = fmt_default.field;
154
155 vimc_colorimetry_clamp(fmt);
156}
157
158static int vimc_sen_set_fmt(struct v4l2_subdev *sd,
159 struct v4l2_subdev_pad_config *cfg,
160 struct v4l2_subdev_format *fmt)
161{
162 struct vimc_sen_device *vsen = v4l2_get_subdevdata(sd);
163 struct v4l2_mbus_framefmt *mf;
164
165 if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
166
167 if (vsen->frame)
168 return -EBUSY;
169
170 mf = &vsen->mbus_format;
171 } else {
172 mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
173 }
174
175
176 vimc_sen_adjust_fmt(&fmt->format);
177
178 dev_dbg(vsen->dev, "%s: format update: "
179 "old:%dx%d (0x%x, %d, %d, %d, %d) "
180 "new:%dx%d (0x%x, %d, %d, %d, %d)\n", vsen->sd.name,
181
182 mf->width, mf->height, mf->code,
183 mf->colorspace, mf->quantization,
184 mf->xfer_func, mf->ycbcr_enc,
185
186 fmt->format.width, fmt->format.height, fmt->format.code,
187 fmt->format.colorspace, fmt->format.quantization,
188 fmt->format.xfer_func, fmt->format.ycbcr_enc);
189
190 *mf = fmt->format;
191
192 return 0;
193}
194
195static const struct v4l2_subdev_pad_ops vimc_sen_pad_ops = {
196 .init_cfg = vimc_sen_init_cfg,
197 .enum_mbus_code = vimc_sen_enum_mbus_code,
198 .enum_frame_size = vimc_sen_enum_frame_size,
199 .get_fmt = vimc_sen_get_fmt,
200 .set_fmt = vimc_sen_set_fmt,
201};
202
203static int vimc_sen_tpg_thread(void *data)
204{
205 struct vimc_sen_device *vsen = data;
206 unsigned int i;
207
208 set_freezable();
209 set_current_state(TASK_UNINTERRUPTIBLE);
210
211 for (;;) {
212 try_to_freeze();
213 if (kthread_should_stop())
214 break;
215
216 tpg_fill_plane_buffer(&vsen->tpg, 0, 0, vsen->frame);
217
218
219 for (i = 0; i < vsen->sd.entity.num_pads; i++)
220 vimc_propagate_frame(&vsen->sd.entity.pads[i],
221 vsen->frame);
222
223
224 schedule_timeout(HZ/60);
225 }
226
227 return 0;
228}
229
230static int vimc_sen_s_stream(struct v4l2_subdev *sd, int enable)
231{
232 struct vimc_sen_device *vsen =
233 container_of(sd, struct vimc_sen_device, sd);
234 int ret;
235
236 if (enable) {
237 const struct vimc_pix_map *vpix;
238 unsigned int frame_size;
239
240 if (vsen->kthread_sen)
241
242 return 0;
243
244
245 vpix = vimc_pix_map_by_code(vsen->mbus_format.code);
246 frame_size = vsen->mbus_format.width * vpix->bpp *
247 vsen->mbus_format.height;
248
249
250
251
252
253 vsen->frame = vmalloc(frame_size);
254 if (!vsen->frame)
255 return -ENOMEM;
256
257
258 vimc_sen_tpg_s_format(vsen);
259
260
261 vsen->kthread_sen = kthread_run(vimc_sen_tpg_thread, vsen,
262 "%s-sen", vsen->sd.v4l2_dev->name);
263 if (IS_ERR(vsen->kthread_sen)) {
264 dev_err(vsen->dev, "%s: kernel_thread() failed\n",
265 vsen->sd.name);
266 vfree(vsen->frame);
267 vsen->frame = NULL;
268 return PTR_ERR(vsen->kthread_sen);
269 }
270 } else {
271 if (!vsen->kthread_sen)
272 return 0;
273
274
275 ret = kthread_stop(vsen->kthread_sen);
276 if (ret)
277 return ret;
278
279 vsen->kthread_sen = NULL;
280 vfree(vsen->frame);
281 vsen->frame = NULL;
282 return 0;
283 }
284
285 return 0;
286}
287
288static struct v4l2_subdev_core_ops vimc_sen_core_ops = {
289 .log_status = v4l2_ctrl_subdev_log_status,
290 .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
291 .unsubscribe_event = v4l2_event_subdev_unsubscribe,
292};
293
294static const struct v4l2_subdev_video_ops vimc_sen_video_ops = {
295 .s_stream = vimc_sen_s_stream,
296};
297
298static const struct v4l2_subdev_ops vimc_sen_ops = {
299 .core = &vimc_sen_core_ops,
300 .pad = &vimc_sen_pad_ops,
301 .video = &vimc_sen_video_ops,
302};
303
304static int vimc_sen_s_ctrl(struct v4l2_ctrl *ctrl)
305{
306 struct vimc_sen_device *vsen =
307 container_of(ctrl->handler, struct vimc_sen_device, hdl);
308
309 switch (ctrl->id) {
310 case VIMC_CID_TEST_PATTERN:
311 tpg_s_pattern(&vsen->tpg, ctrl->val);
312 break;
313 case V4L2_CID_HFLIP:
314 tpg_s_hflip(&vsen->tpg, ctrl->val);
315 break;
316 case V4L2_CID_VFLIP:
317 tpg_s_vflip(&vsen->tpg, ctrl->val);
318 break;
319 default:
320 return -EINVAL;
321 }
322 return 0;
323}
324
325static const struct v4l2_ctrl_ops vimc_sen_ctrl_ops = {
326 .s_ctrl = vimc_sen_s_ctrl,
327};
328
329static void vimc_sen_comp_unbind(struct device *comp, struct device *master,
330 void *master_data)
331{
332 struct vimc_ent_device *ved = dev_get_drvdata(comp);
333 struct vimc_sen_device *vsen =
334 container_of(ved, struct vimc_sen_device, ved);
335
336 vimc_ent_sd_unregister(ved, &vsen->sd);
337 v4l2_ctrl_handler_free(&vsen->hdl);
338 tpg_free(&vsen->tpg);
339 kfree(vsen);
340}
341
342
343static const struct v4l2_ctrl_config vimc_sen_ctrl_class = {
344 .flags = V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_WRITE_ONLY,
345 .id = VIMC_CID_VIMC_CLASS,
346 .name = "VIMC Controls",
347 .type = V4L2_CTRL_TYPE_CTRL_CLASS,
348};
349
350static const struct v4l2_ctrl_config vimc_sen_ctrl_test_pattern = {
351 .ops = &vimc_sen_ctrl_ops,
352 .id = VIMC_CID_TEST_PATTERN,
353 .name = "Test Pattern",
354 .type = V4L2_CTRL_TYPE_MENU,
355 .max = TPG_PAT_NOISE,
356 .qmenu = tpg_pattern_strings,
357};
358
359static int vimc_sen_comp_bind(struct device *comp, struct device *master,
360 void *master_data)
361{
362 struct v4l2_device *v4l2_dev = master_data;
363 struct vimc_platform_data *pdata = comp->platform_data;
364 struct vimc_sen_device *vsen;
365 int ret;
366
367
368 vsen = kzalloc(sizeof(*vsen), GFP_KERNEL);
369 if (!vsen)
370 return -ENOMEM;
371
372 v4l2_ctrl_handler_init(&vsen->hdl, 4);
373
374 v4l2_ctrl_new_custom(&vsen->hdl, &vimc_sen_ctrl_class, NULL);
375 v4l2_ctrl_new_custom(&vsen->hdl, &vimc_sen_ctrl_test_pattern, NULL);
376 v4l2_ctrl_new_std(&vsen->hdl, &vimc_sen_ctrl_ops,
377 V4L2_CID_VFLIP, 0, 1, 1, 0);
378 v4l2_ctrl_new_std(&vsen->hdl, &vimc_sen_ctrl_ops,
379 V4L2_CID_HFLIP, 0, 1, 1, 0);
380 vsen->sd.ctrl_handler = &vsen->hdl;
381 if (vsen->hdl.error) {
382 ret = vsen->hdl.error;
383 goto err_free_vsen;
384 }
385
386
387 ret = vimc_ent_sd_register(&vsen->ved, &vsen->sd, v4l2_dev,
388 pdata->entity_name,
389 MEDIA_ENT_F_CAM_SENSOR, 1,
390 (const unsigned long[1]) {MEDIA_PAD_FL_SOURCE},
391 &vimc_sen_ops);
392 if (ret)
393 goto err_free_hdl;
394
395 dev_set_drvdata(comp, &vsen->ved);
396 vsen->dev = comp;
397
398
399 vsen->mbus_format = fmt_default;
400
401
402 tpg_init(&vsen->tpg, vsen->mbus_format.width,
403 vsen->mbus_format.height);
404 ret = tpg_alloc(&vsen->tpg, VIMC_FRAME_MAX_WIDTH);
405 if (ret)
406 goto err_unregister_ent_sd;
407
408 return 0;
409
410err_unregister_ent_sd:
411 vimc_ent_sd_unregister(&vsen->ved, &vsen->sd);
412err_free_hdl:
413 v4l2_ctrl_handler_free(&vsen->hdl);
414err_free_vsen:
415 kfree(vsen);
416
417 return ret;
418}
419
420static const struct component_ops vimc_sen_comp_ops = {
421 .bind = vimc_sen_comp_bind,
422 .unbind = vimc_sen_comp_unbind,
423};
424
425static int vimc_sen_probe(struct platform_device *pdev)
426{
427 return component_add(&pdev->dev, &vimc_sen_comp_ops);
428}
429
430static int vimc_sen_remove(struct platform_device *pdev)
431{
432 component_del(&pdev->dev, &vimc_sen_comp_ops);
433
434 return 0;
435}
436
437static const struct platform_device_id vimc_sen_driver_ids[] = {
438 {
439 .name = VIMC_SEN_DRV_NAME,
440 },
441 { }
442};
443
444static struct platform_driver vimc_sen_pdrv = {
445 .probe = vimc_sen_probe,
446 .remove = vimc_sen_remove,
447 .id_table = vimc_sen_driver_ids,
448 .driver = {
449 .name = VIMC_SEN_DRV_NAME,
450 },
451};
452
453module_platform_driver(vimc_sen_pdrv);
454
455MODULE_DEVICE_TABLE(platform, vimc_sen_driver_ids);
456
457MODULE_DESCRIPTION("Virtual Media Controller Driver (VIMC) Sensor");
458MODULE_AUTHOR("Helen Mae Koike Fornazier <helen.fornazier@gmail.com>");
459MODULE_LICENSE("GPL");
460
