1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * virtual camera driver
4 *
5 * Copyright (C) 2018 Fuzhou Rockchip Electronics Co., Ltd.
6 */
7
8 #include <linux/device.h>
9 #include <linux/i2c.h>
10 #include <linux/module.h>
11 #include <linux/of_graph.h>
12 #include <media/media-entity.h>
13 #include <media/v4l2-async.h>
14 #include <media/v4l2-ctrls.h>
15 #include <media/v4l2-fwnode.h>
16 #include <media/v4l2-subdev.h>
17
18 #define PROP_WIDTH "width"
19 #define PROP_HEIGHT "height"
20 #define PROP_BUSFMT "bus-format"
21 #define VCAM_VTS_MAX 0x7fff
22
23 struct output_mode {
24 u32 width;
25 u32 height;
26 u32 hts_def;
27 u32 vts_def;
28 };
29
30 struct output_pixfmt {
31 u32 code;
32 u32 reserved;
33 };
34
35 struct virtual_camera {
36 struct i2c_client *client;
37 bool streaming;
38 struct mutex mutex; /* lock for updating format protection */
39 struct v4l2_subdev subdev;
40 struct media_pad pad;
41 struct v4l2_ctrl *hblank;
42 struct v4l2_ctrl *vblank;
43 struct v4l2_ctrl *link_freq;
44 struct v4l2_ctrl_handler ctrl_handler;
45 struct v4l2_mbus_framefmt def_fmt;
46
47 const struct output_mode *cur_mode;
48 int fmt_code;
49 s64 link_frequency;
50 };
51
52 #define to_virtual_camera(sd) container_of(sd, struct virtual_camera, subdev)
53
54 static const s64 link_freq_menu_items[] = {
55 40000000, /* minimum support frequency */
56 55000000,
57 75000000,
58 100000000,
59 125000000,
60 150000000,
61 200000000,
62 250000000,
63 300000000,
64 350000000,
65 400000000,
66 500000000,
67 600000000,
68 700000000,
69 752000000,
70 800000000,
71 900000000,
72 1000000000,
73 1100000000,
74 1200000000,
75 1250000000 /* maximum support frequency */
76 };
77
78 static const struct output_pixfmt supported_formats[] = {
79 {
80 .code = MEDIA_BUS_FMT_SBGGR8_1X8,
81 }, {
82 .code = MEDIA_BUS_FMT_SGBRG8_1X8,
83 }, {
84 .code = MEDIA_BUS_FMT_SGRBG8_1X8,
85 }, {
86 .code = MEDIA_BUS_FMT_SRGGB8_1X8,
87 }, {
88 .code = MEDIA_BUS_FMT_SBGGR10_1X10,
89 }, {
90 .code = MEDIA_BUS_FMT_SGBRG10_1X10,
91 }, {
92 .code = MEDIA_BUS_FMT_SGRBG10_1X10,
93 }, {
94 .code = MEDIA_BUS_FMT_SRGGB10_1X10,
95 }, {
96 .code = MEDIA_BUS_FMT_RGB888_1X24,
97 },
98 };
99
100 static const struct output_mode supported_modes[] = {
101 {
102 .width = 1280,
103 .height = 720,
104 .hts_def = 1500,
105 .vts_def = 900,
106 }, {
107 .width = 1920,
108 .height = 1080,
109 .hts_def = 2400,
110 .vts_def = 1200,
111 }, {
112 .width = 3840,
113 .height = 720,
114 .hts_def = 4300,
115 .vts_def = 900,
116 }, {
117 .width = 3840,
118 .height = 2160,
119 .hts_def = 4300,
120 .vts_def = 2400,
121 }, {
122 .width = 4096,
123 .height = 2048,
124 .hts_def = 4300,
125 .vts_def = 2400,
126 }, {
127 .width = 5120,
128 .height = 2880,
129 .hts_def = 5800,
130 .vts_def = 3100,
131 }, {
132 .width = 5760,
133 .height = 1080,
134 .hts_def = 6400,
135 .vts_def = 1300,
136 },
137 };
138
vcamera_get_reso_dist(const struct output_mode * mode,struct v4l2_mbus_framefmt * fmt)139 static int vcamera_get_reso_dist(const struct output_mode *mode,
140 struct v4l2_mbus_framefmt *fmt)
141 {
142 return abs(mode->width - fmt->width) +
143 abs(mode->height - fmt->height);
144 }
145
146 static const struct output_mode *
vcamera_get_best_mode(struct v4l2_mbus_framefmt * fmt)147 vcamera_get_best_mode(struct v4l2_mbus_framefmt *fmt)
148 {
149 int dist = 0, min_dist = -1;
150 int best = 0;
151 unsigned int i = 0;
152
153 for (i = 0; i < ARRAY_SIZE(supported_modes); i++) {
154 dist = vcamera_get_reso_dist(&supported_modes[i], fmt);
155 if (min_dist == -1 || dist < min_dist) {
156 min_dist = dist;
157 best = i;
158 }
159 }
160
161 return &supported_modes[best];
162 }
163
vcamera_fill_fmt(struct virtual_camera * vcam,struct v4l2_mbus_framefmt * fmt)164 static void vcamera_fill_fmt(struct virtual_camera *vcam,
165 struct v4l2_mbus_framefmt *fmt)
166 {
167 fmt->code = vcam->fmt_code;
168 fmt->width = vcam->cur_mode->width;
169 fmt->height = vcam->cur_mode->height;
170 fmt->field = V4L2_FIELD_NONE;
171 }
172
vcamera_get_default_fmt(struct virtual_camera * vcam)173 static void vcamera_get_default_fmt(struct virtual_camera *vcam)
174 {
175 struct device *dev = &vcam->client->dev;
176 struct v4l2_mbus_framefmt *def_fmt = &vcam->def_fmt;
177 int index = ARRAY_SIZE(supported_formats);
178
179 vcam->cur_mode = vcamera_get_best_mode(def_fmt);
180 if (vcam->cur_mode->width != def_fmt->width ||
181 vcam->cur_mode->height != def_fmt->height)
182 dev_warn(dev, "get dts res: %dx%d, select best res: %dx%d\n",
183 def_fmt->width, def_fmt->height,
184 vcam->cur_mode->width, vcam->cur_mode->height);
185
186 while (--index >= 0)
187 if (supported_formats[index].code == def_fmt->code)
188 break;
189
190 if (index < 0) {
191 vcam->fmt_code = MEDIA_BUS_FMT_SBGGR8_1X8;
192 dev_warn(dev, "get dts fmt: 0x%x, select default fmt: 0x%x\n",
193 def_fmt->code, vcam->fmt_code);
194 } else {
195 vcam->fmt_code = def_fmt->code;
196 }
197
198 /* if not found link-frequencies in dts, set a default value */
199 if (!vcam->link_frequency)
200 vcam->link_frequency = 500000000;
201
202 vcamera_fill_fmt(vcam, def_fmt);
203 }
204
vcamera_get_fmt(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_format * fmt)205 static int vcamera_get_fmt(struct v4l2_subdev *sd,
206 struct v4l2_subdev_pad_config *cfg,
207 struct v4l2_subdev_format *fmt)
208 {
209 struct virtual_camera *vcam = to_virtual_camera(sd);
210 struct v4l2_mbus_framefmt *mbus_fmt = &fmt->format;
211
212 if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
213 #ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
214 struct v4l2_mbus_framefmt *mf;
215
216 mf = v4l2_subdev_get_try_format(sd, cfg, 0);
217 mutex_lock(&vcam->mutex);
218 fmt->format = *mf;
219 mutex_unlock(&vcam->mutex);
220 return 0;
221 #else
222 return -ENOTTY;
223 #endif
224 }
225
226 mutex_lock(&vcam->mutex);
227 vcamera_fill_fmt(vcam, mbus_fmt);
228 mutex_unlock(&vcam->mutex);
229
230 return 0;
231 }
232
vcamera_set_fmt(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_format * fmt)233 static int vcamera_set_fmt(struct v4l2_subdev *sd,
234 struct v4l2_subdev_pad_config *cfg,
235 struct v4l2_subdev_format *fmt)
236 {
237 struct virtual_camera *vcam = to_virtual_camera(sd);
238 struct v4l2_mbus_framefmt *mf = &fmt->format;
239 int index = ARRAY_SIZE(supported_formats);
240
241 vcam->cur_mode = vcamera_get_best_mode(mf);
242
243 while (--index >= 0)
244 if (supported_formats[index].code == mf->code)
245 break;
246
247 if (index < 0)
248 return -EINVAL;
249
250 vcam->fmt_code = supported_formats[index].code;
251
252 mutex_lock(&vcam->mutex);
253
254 if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
255 #ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
256 mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
257 *mf = fmt->format;
258 #else
259 mutex_unlock(&vcam->mutex);
260 return -ENOTTY;
261 #endif
262 } else {
263 if (vcam->streaming) {
264 mutex_unlock(&vcam->mutex);
265 return -EBUSY;
266 }
267
268 vcamera_fill_fmt(vcam, mf);
269 }
270
271 mutex_unlock(&vcam->mutex);
272
273 return 0;
274 }
275
vcamera_enum_mbus_code(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_mbus_code_enum * code)276 static int vcamera_enum_mbus_code(struct v4l2_subdev *sd,
277 struct v4l2_subdev_pad_config *cfg,
278 struct v4l2_subdev_mbus_code_enum *code)
279 {
280 if (code->index >= ARRAY_SIZE(supported_formats))
281 return -EINVAL;
282
283 code->code = supported_formats[code->index].code;
284
285 return 0;
286 }
287
vcamera_enum_frame_sizes(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_frame_size_enum * fse)288 static int vcamera_enum_frame_sizes(struct v4l2_subdev *sd,
289 struct v4l2_subdev_pad_config *cfg,
290 struct v4l2_subdev_frame_size_enum *fse)
291 {
292 unsigned int index = fse->index;
293 int i = ARRAY_SIZE(supported_formats);
294
295 if (index >= ARRAY_SIZE(supported_modes))
296 return -EINVAL;
297
298 while (--i >= 0)
299 if (fse->code == supported_formats[i].code)
300 break;
301
302 fse->code = supported_formats[i].code;
303
304 fse->min_width = supported_modes[index].width;
305 fse->max_width = fse->min_width;
306 fse->max_height = supported_modes[index].height;
307 fse->min_height = fse->max_height;
308
309 return 0;
310 }
311
vcamera_set_vts(struct virtual_camera * vcam,s32 val)312 static inline void vcamera_set_vts(struct virtual_camera *vcam, s32 val)
313 {
314 /* TODO */
315 }
316
vcamera_s_stream(struct v4l2_subdev * sd,int on)317 static int vcamera_s_stream(struct v4l2_subdev *sd, int on)
318 {
319 struct virtual_camera *vcam = to_virtual_camera(sd);
320
321 mutex_lock(&vcam->mutex);
322
323 on = !!on;
324 if (on == vcam->streaming)
325 goto unlock_and_return;
326
327 /* TODO */
328 vcam->streaming = on;
329
330 unlock_and_return:
331 mutex_unlock(&vcam->mutex);
332 return 0;
333 }
334
335 #ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
vcamera_open(struct v4l2_subdev * sd,struct v4l2_subdev_fh * fh)336 static int vcamera_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
337 {
338 struct virtual_camera *vcam = to_virtual_camera(sd);
339 struct v4l2_mbus_framefmt *try_fmt;
340
341 mutex_lock(&vcam->mutex);
342
343 try_fmt = v4l2_subdev_get_try_format(sd, fh->pad, 0);
344 /* Initialize try_fmt */
345 vcamera_fill_fmt(vcam, try_fmt);
346
347 mutex_unlock(&vcam->mutex);
348
349 return 0;
350 }
351 #endif
352
vcamera_s_ctrl(struct v4l2_ctrl * ctrl)353 static int vcamera_s_ctrl(struct v4l2_ctrl *ctrl)
354 {
355 struct virtual_camera *vcam = container_of(ctrl->handler,
356 struct virtual_camera,
357 ctrl_handler);
358 struct i2c_client *client = vcam->client;
359
360 /* Propagate change of current control to all related controls */
361 switch (ctrl->id) {
362 case V4L2_CID_VBLANK:
363 vcamera_set_vts(vcam, ctrl->val);
364 break;
365 case V4L2_CID_LINK_FREQ:
366 vcam->link_frequency = link_freq_menu_items[ctrl->val];
367 dev_info(&client->dev, "link freq ctrl->val: %d freq: %lld\n",
368 ctrl->val, vcam->link_frequency);
369 break;
370 default:
371 dev_warn(&client->dev, "%s Unhandled id:0x%x, val:0x%x\n",
372 __func__, ctrl->id, ctrl->val);
373 break;
374 }
375
376 return 0;
377 }
378
379 static struct v4l2_subdev_core_ops vcamera_core_ops = {
380 .log_status = v4l2_ctrl_subdev_log_status,
381 };
382
383 static struct v4l2_subdev_video_ops vcamera_video_ops = {
384 .s_stream = vcamera_s_stream,
385 };
386
387 static struct v4l2_subdev_pad_ops vcamera_pad_ops = {
388 .enum_mbus_code = vcamera_enum_mbus_code,
389 .enum_frame_size = vcamera_enum_frame_sizes,
390 .get_fmt = vcamera_get_fmt,
391 .set_fmt = vcamera_set_fmt,
392 };
393
394 static struct v4l2_subdev_ops vcamera_subdev_ops = {
395 .core = &vcamera_core_ops,
396 .video = &vcamera_video_ops,
397 .pad = &vcamera_pad_ops,
398 };
399
400 #ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
401 static const struct v4l2_subdev_internal_ops vcamera_internal_ops = {
402 .open = vcamera_open,
403 };
404 #endif
405
406 static const struct v4l2_ctrl_ops vcamera_ctrl_ops = {
407 .s_ctrl = vcamera_s_ctrl,
408 };
409
vcamera_initialize_controls(struct virtual_camera * vcam)410 static int vcamera_initialize_controls(struct virtual_camera *vcam)
411 {
412 const struct output_mode *mode;
413 struct v4l2_ctrl_handler *handler;
414 u32 h_blank, i;
415 int ret;
416
417 handler = &vcam->ctrl_handler;
418 mode = vcam->cur_mode;
419 ret = v4l2_ctrl_handler_init(handler, 3);
420 if (ret)
421 return ret;
422
423 handler->lock = &vcam->mutex;
424
425 vcam->link_freq = v4l2_ctrl_new_int_menu(handler, &vcamera_ctrl_ops,
426 V4L2_CID_LINK_FREQ,
427 ARRAY_SIZE(link_freq_menu_items) - 1, 0,
428 link_freq_menu_items);
429
430 h_blank = mode->hts_def - mode->width;
431 vcam->hblank = v4l2_ctrl_new_std(handler, NULL, V4L2_CID_HBLANK,
432 h_blank, h_blank, 1, h_blank);
433 if (vcam->hblank)
434 vcam->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
435
436 vcam->vblank = v4l2_ctrl_new_std(handler, &vcamera_ctrl_ops,
437 V4L2_CID_VBLANK, mode->vts_def - mode->height,
438 VCAM_VTS_MAX - mode->height, 1,
439 mode->vts_def - mode->height);
440
441 if (handler->error) {
442 v4l2_ctrl_handler_free(handler);
443 return handler->error;
444 }
445
446 vcam->subdev.ctrl_handler = handler;
447
448 for (i = 0; i < ARRAY_SIZE(link_freq_menu_items); i++) {
449 if (link_freq_menu_items[i] > vcam->link_frequency && i >= 1) {
450 v4l2_ctrl_s_ctrl(vcam->link_freq, i - 1);
451 break;
452 }
453 }
454
455 if (i == ARRAY_SIZE(link_freq_menu_items)) {
456 dev_warn(&vcam->client->dev,
457 "vcam->link_frequency: %lld, max support clock: %lld\n",
458 vcam->link_frequency, link_freq_menu_items[i - 1]);
459 v4l2_ctrl_s_ctrl(vcam->link_freq, i - 1);
460 }
461
462 return 0;
463 }
464
vcamera_check_sensor_id(struct virtual_camera * vcam,struct i2c_client * client)465 static int vcamera_check_sensor_id(struct virtual_camera *vcam,
466 struct i2c_client *client)
467 {
468 /* TODO */
469 return 0;
470 }
471
vcamera_get_pdata(struct i2c_client * client,struct virtual_camera * vcam)472 static int vcamera_get_pdata(struct i2c_client *client,
473 struct virtual_camera *vcam)
474 {
475 struct v4l2_fwnode_endpoint *bus_cfg;
476 struct device_node *np = client->dev.of_node;
477 struct device_node *endpoint;
478 u32 val;
479
480 if (!IS_ENABLED(CONFIG_OF) || !np)
481 return 0;
482
483 if (!of_property_read_u32(np, PROP_WIDTH, &val))
484 vcam->def_fmt.width = val;
485
486 if (!of_property_read_u32(np, PROP_HEIGHT, &val))
487 vcam->def_fmt.height = val;
488
489 if (!of_property_read_u32(np, PROP_BUSFMT, &val))
490 vcam->def_fmt.code = val;
491
492 endpoint = of_graph_get_next_endpoint(np, NULL);
493 if (!endpoint)
494 return -ENODEV;
495
496 bus_cfg = v4l2_fwnode_endpoint_alloc_parse(of_fwnode_handle(endpoint));
497 if (IS_ERR(bus_cfg))
498 goto done;
499
500 if (!bus_cfg->nr_of_link_frequencies) {
501 dev_info(&client->dev,
502 "link-frequencies property not found or too many\n");
503 goto done;
504 }
505
506 vcam->link_frequency = bus_cfg->link_frequencies[0];
507
508 done:
509 v4l2_fwnode_endpoint_free(bus_cfg);
510 of_node_put(endpoint);
511 return 0;
512 }
513
vcamera_probe(struct i2c_client * client,const struct i2c_device_id * id)514 static int vcamera_probe(struct i2c_client *client,
515 const struct i2c_device_id *id)
516 {
517 struct device *dev = &client->dev;
518 struct virtual_camera *vcam;
519 int ret;
520
521 vcam = devm_kzalloc(dev, sizeof(*vcam), GFP_KERNEL);
522 if (!vcam)
523 return -ENOMEM;
524
525 vcam->client = client;
526 vcamera_get_pdata(client, vcam);
527 vcamera_get_default_fmt(vcam);
528
529 mutex_init(&vcam->mutex);
530 v4l2_i2c_subdev_init(&vcam->subdev, client, &vcamera_subdev_ops);
531 ret = vcamera_initialize_controls(vcam);
532 if (ret)
533 goto destroy_mutex;
534
535 ret = vcamera_check_sensor_id(vcam, client);
536 if (ret)
537 return ret;
538
539 #ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
540 vcam->subdev.internal_ops = &vcamera_internal_ops;
541 #endif
542 #if defined(CONFIG_MEDIA_CONTROLLER)
543 vcam->pad.flags = MEDIA_PAD_FL_SOURCE;
544 vcam->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
545 vcam->subdev.entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
546 ret = media_entity_init(&vcam->subdev.entity, 1, &vcam->pad, 0);
547 if (ret < 0)
548 goto free_ctrl_handler;
549 #endif
550
551 ret = v4l2_async_register_subdev(&vcam->subdev);
552 if (ret) {
553 dev_err(dev, "v4l2 async register subdev failed\n");
554 goto clean_entity;
555 }
556
557 dev_info(dev, "virtual camera register success\n");
558
559 return 0;
560
561 clean_entity:
562 #if defined(CONFIG_MEDIA_CONTROLLER)
563 media_entity_cleanup(&vcam->subdev.entity);
564 #endif
565 free_ctrl_handler:
566 v4l2_ctrl_handler_free(&vcam->ctrl_handler);
567 destroy_mutex:
568 mutex_destroy(&vcam->mutex);
569
570 return ret;
571 }
572
vcamera_remove(struct i2c_client * client)573 static int vcamera_remove(struct i2c_client *client)
574 {
575 struct virtual_camera *vcam = i2c_get_clientdata(client);
576
577 v4l2_async_unregister_subdev(&vcam->subdev);
578 media_entity_cleanup(&vcam->subdev.entity);
579 v4l2_ctrl_handler_free(&vcam->ctrl_handler);
580 mutex_destroy(&vcam->mutex);
581
582 return 0;
583 }
584
585 static const struct i2c_device_id vcamera_id[] = {
586 { "virtual-camera", 0 },
587 { /* sentinel */ },
588 };
589 MODULE_DEVICE_TABLE(i2c, vcamera_id);
590
591 static const struct of_device_id vcamera_of_match[] = {
592 { .compatible = "rockchip,virtual-camera" },
593 {},
594 };
595
596 static struct i2c_driver vcamera_i2c_driver = {
597 .driver = {
598 .name = "virtual-camera",
599 .of_match_table = vcamera_of_match
600 },
601 .probe = vcamera_probe,
602 .remove = vcamera_remove,
603 .id_table = vcamera_id,
604 };
605
606 module_i2c_driver(vcamera_i2c_driver);
607
608 MODULE_AUTHOR("Rockchip Camera/ISP team");
609 MODULE_DESCRIPTION("Rockchip virtual camera sensor driver");
610 MODULE_LICENSE("GPL v2");
611