1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3 * vimc-sensor.c Virtual Media Controller Driver
4 *
5 * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
6 */
7
8 #include <linux/v4l2-mediabus.h>
9 #include <linux/vmalloc.h>
10 #include <media/v4l2-ctrls.h>
11 #include <media/v4l2-event.h>
12 #include <media/v4l2-subdev.h>
13 #include <media/tpg/v4l2-tpg.h>
14
15 #include "vimc-common.h"
16
17 enum vimc_sensor_osd_mode {
18 VIMC_SENSOR_OSD_SHOW_ALL = 0,
19 VIMC_SENSOR_OSD_SHOW_COUNTERS = 1,
20 VIMC_SENSOR_OSD_SHOW_NONE = 2
21 };
22
23 struct vimc_sensor_device {
24 struct vimc_ent_device ved;
25 struct v4l2_subdev sd;
26 struct tpg_data tpg;
27 struct v4l2_ctrl_handler hdl;
28 struct media_pad pad;
29
30 u8 *frame;
31
32 /*
33 * Virtual "hardware" configuration, filled when the stream starts or
34 * when controls are set.
35 */
36 struct {
37 struct v4l2_area size;
38 enum vimc_sensor_osd_mode osd_value;
39 u64 start_stream_ts;
40 } hw;
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_SRGB,
49 };
50
vimc_sensor_init_state(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state)51 static int vimc_sensor_init_state(struct v4l2_subdev *sd,
52 struct v4l2_subdev_state *sd_state)
53 {
54 struct v4l2_mbus_framefmt *mf;
55
56 mf = v4l2_subdev_state_get_format(sd_state, 0);
57 *mf = fmt_default;
58
59 return 0;
60 }
61
vimc_sensor_enum_mbus_code(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_mbus_code_enum * code)62 static int vimc_sensor_enum_mbus_code(struct v4l2_subdev *sd,
63 struct v4l2_subdev_state *sd_state,
64 struct v4l2_subdev_mbus_code_enum *code)
65 {
66 u32 mbus_code = vimc_mbus_code_by_index(code->index);
67
68 if (!mbus_code)
69 return -EINVAL;
70
71 code->code = mbus_code;
72
73 return 0;
74 }
75
vimc_sensor_enum_frame_size(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_frame_size_enum * fse)76 static int vimc_sensor_enum_frame_size(struct v4l2_subdev *sd,
77 struct v4l2_subdev_state *sd_state,
78 struct v4l2_subdev_frame_size_enum *fse)
79 {
80 const struct vimc_pix_map *vpix;
81
82 if (fse->index)
83 return -EINVAL;
84
85 /* Only accept code in the pix map table */
86 vpix = vimc_pix_map_by_code(fse->code);
87 if (!vpix)
88 return -EINVAL;
89
90 fse->min_width = VIMC_FRAME_MIN_WIDTH;
91 fse->max_width = VIMC_FRAME_MAX_WIDTH;
92 fse->min_height = VIMC_FRAME_MIN_HEIGHT;
93 fse->max_height = VIMC_FRAME_MAX_HEIGHT;
94
95 return 0;
96 }
97
vimc_sensor_tpg_s_format(struct vimc_sensor_device * vsensor,const struct v4l2_mbus_framefmt * format)98 static void vimc_sensor_tpg_s_format(struct vimc_sensor_device *vsensor,
99 const struct v4l2_mbus_framefmt *format)
100 {
101 const struct vimc_pix_map *vpix = vimc_pix_map_by_code(format->code);
102
103 tpg_reset_source(&vsensor->tpg, format->width, format->height,
104 format->field);
105 tpg_s_bytesperline(&vsensor->tpg, 0, format->width * vpix->bpp);
106 tpg_s_buf_height(&vsensor->tpg, format->height);
107 tpg_s_fourcc(&vsensor->tpg, vpix->pixelformat);
108 /* TODO: add support for V4L2_FIELD_ALTERNATE */
109 tpg_s_field(&vsensor->tpg, format->field, false);
110 tpg_s_colorspace(&vsensor->tpg, format->colorspace);
111 tpg_s_ycbcr_enc(&vsensor->tpg, format->ycbcr_enc);
112 tpg_s_quantization(&vsensor->tpg, format->quantization);
113 tpg_s_xfer_func(&vsensor->tpg, format->xfer_func);
114 }
115
vimc_sensor_adjust_fmt(struct v4l2_mbus_framefmt * fmt)116 static void vimc_sensor_adjust_fmt(struct v4l2_mbus_framefmt *fmt)
117 {
118 const struct vimc_pix_map *vpix;
119
120 /* Only accept code in the pix map table */
121 vpix = vimc_pix_map_by_code(fmt->code);
122 if (!vpix)
123 fmt->code = fmt_default.code;
124
125 fmt->width = clamp_t(u32, fmt->width, VIMC_FRAME_MIN_WIDTH,
126 VIMC_FRAME_MAX_WIDTH) & ~1;
127 fmt->height = clamp_t(u32, fmt->height, VIMC_FRAME_MIN_HEIGHT,
128 VIMC_FRAME_MAX_HEIGHT) & ~1;
129
130 /* TODO: add support for V4L2_FIELD_ALTERNATE */
131 if (fmt->field == V4L2_FIELD_ANY || fmt->field == V4L2_FIELD_ALTERNATE)
132 fmt->field = fmt_default.field;
133
134 vimc_colorimetry_clamp(fmt);
135 }
136
vimc_sensor_set_fmt(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * fmt)137 static int vimc_sensor_set_fmt(struct v4l2_subdev *sd,
138 struct v4l2_subdev_state *sd_state,
139 struct v4l2_subdev_format *fmt)
140 {
141 struct vimc_sensor_device *vsensor = v4l2_get_subdevdata(sd);
142 struct v4l2_mbus_framefmt *mf;
143
144 /* Do not change the format while stream is on */
145 if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE && vsensor->frame)
146 return -EBUSY;
147
148 mf = v4l2_subdev_state_get_format(sd_state, fmt->pad);
149
150 /* Set the new format */
151 vimc_sensor_adjust_fmt(&fmt->format);
152
153 dev_dbg(vsensor->ved.dev, "%s: format update: "
154 "old:%dx%d (0x%x, %d, %d, %d, %d) "
155 "new:%dx%d (0x%x, %d, %d, %d, %d)\n", vsensor->sd.name,
156 /* old */
157 mf->width, mf->height, mf->code,
158 mf->colorspace, mf->quantization,
159 mf->xfer_func, mf->ycbcr_enc,
160 /* new */
161 fmt->format.width, fmt->format.height, fmt->format.code,
162 fmt->format.colorspace, fmt->format.quantization,
163 fmt->format.xfer_func, fmt->format.ycbcr_enc);
164
165 *mf = fmt->format;
166
167 return 0;
168 }
169
170 static const struct v4l2_subdev_pad_ops vimc_sensor_pad_ops = {
171 .enum_mbus_code = vimc_sensor_enum_mbus_code,
172 .enum_frame_size = vimc_sensor_enum_frame_size,
173 .get_fmt = v4l2_subdev_get_fmt,
174 .set_fmt = vimc_sensor_set_fmt,
175 };
176
vimc_sensor_process_frame(struct vimc_ent_device * ved,const void * sink_frame)177 static void *vimc_sensor_process_frame(struct vimc_ent_device *ved,
178 const void *sink_frame)
179 {
180 struct vimc_sensor_device *vsensor =
181 container_of(ved, struct vimc_sensor_device, ved);
182
183 const unsigned int line_height = 16;
184 u8 *basep[TPG_MAX_PLANES][2];
185 unsigned int line = 1;
186 char str[100];
187
188 tpg_fill_plane_buffer(&vsensor->tpg, 0, 0, vsensor->frame);
189 tpg_calc_text_basep(&vsensor->tpg, basep, 0, vsensor->frame);
190 switch (vsensor->hw.osd_value) {
191 case VIMC_SENSOR_OSD_SHOW_ALL: {
192 const char *order = tpg_g_color_order(&vsensor->tpg);
193
194 tpg_gen_text(&vsensor->tpg, basep, line++ * line_height,
195 16, order);
196 snprintf(str, sizeof(str),
197 "brightness %3d, contrast %3d, saturation %3d, hue %d ",
198 vsensor->tpg.brightness,
199 vsensor->tpg.contrast,
200 vsensor->tpg.saturation,
201 vsensor->tpg.hue);
202 tpg_gen_text(&vsensor->tpg, basep, line++ * line_height, 16, str);
203 snprintf(str, sizeof(str), "sensor size: %dx%d",
204 vsensor->hw.size.width, vsensor->hw.size.height);
205 tpg_gen_text(&vsensor->tpg, basep, line++ * line_height, 16, str);
206 fallthrough;
207 }
208 case VIMC_SENSOR_OSD_SHOW_COUNTERS: {
209 unsigned int ms;
210
211 ms = div_u64(ktime_get_ns() - vsensor->hw.start_stream_ts, 1000000);
212 snprintf(str, sizeof(str), "%02d:%02d:%02d:%03d",
213 (ms / (60 * 60 * 1000)) % 24,
214 (ms / (60 * 1000)) % 60,
215 (ms / 1000) % 60,
216 ms % 1000);
217 tpg_gen_text(&vsensor->tpg, basep, line++ * line_height, 16, str);
218 break;
219 }
220 case VIMC_SENSOR_OSD_SHOW_NONE:
221 default:
222 break;
223 }
224
225 return vsensor->frame;
226 }
227
vimc_sensor_s_stream(struct v4l2_subdev * sd,int enable)228 static int vimc_sensor_s_stream(struct v4l2_subdev *sd, int enable)
229 {
230 struct vimc_sensor_device *vsensor =
231 container_of(sd, struct vimc_sensor_device, sd);
232
233 if (enable) {
234 const struct v4l2_mbus_framefmt *format;
235 struct v4l2_subdev_state *state;
236 const struct vimc_pix_map *vpix;
237 unsigned int frame_size;
238
239 state = v4l2_subdev_lock_and_get_active_state(sd);
240 format = v4l2_subdev_state_get_format(state, 0);
241
242 /* Configure the test pattern generator. */
243 vimc_sensor_tpg_s_format(vsensor, format);
244
245 /* Calculate the frame size. */
246 vpix = vimc_pix_map_by_code(format->code);
247 frame_size = format->width * vpix->bpp * format->height;
248
249 vsensor->hw.size.width = format->width;
250 vsensor->hw.size.height = format->height;
251
252 v4l2_subdev_unlock_state(state);
253
254 /*
255 * Allocate the frame buffer. Use vmalloc to be able to
256 * allocate a large amount of memory
257 */
258 vsensor->frame = vmalloc(frame_size);
259 if (!vsensor->frame)
260 return -ENOMEM;
261
262 vsensor->hw.start_stream_ts = ktime_get_ns();
263 } else {
264
265 vfree(vsensor->frame);
266 vsensor->frame = NULL;
267 }
268
269 return 0;
270 }
271
272 static const struct v4l2_subdev_core_ops vimc_sensor_core_ops = {
273 .log_status = v4l2_ctrl_subdev_log_status,
274 .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
275 .unsubscribe_event = v4l2_event_subdev_unsubscribe,
276 };
277
278 static const struct v4l2_subdev_video_ops vimc_sensor_video_ops = {
279 .s_stream = vimc_sensor_s_stream,
280 };
281
282 static const struct v4l2_subdev_ops vimc_sensor_ops = {
283 .core = &vimc_sensor_core_ops,
284 .pad = &vimc_sensor_pad_ops,
285 .video = &vimc_sensor_video_ops,
286 };
287
288 static const struct v4l2_subdev_internal_ops vimc_sensor_internal_ops = {
289 .init_state = vimc_sensor_init_state,
290 };
291
vimc_sensor_s_ctrl(struct v4l2_ctrl * ctrl)292 static int vimc_sensor_s_ctrl(struct v4l2_ctrl *ctrl)
293 {
294 struct vimc_sensor_device *vsensor =
295 container_of(ctrl->handler, struct vimc_sensor_device, hdl);
296
297 switch (ctrl->id) {
298 case VIMC_CID_TEST_PATTERN:
299 tpg_s_pattern(&vsensor->tpg, ctrl->val);
300 break;
301 case V4L2_CID_HFLIP:
302 tpg_s_hflip(&vsensor->tpg, ctrl->val);
303 break;
304 case V4L2_CID_VFLIP:
305 tpg_s_vflip(&vsensor->tpg, ctrl->val);
306 break;
307 case V4L2_CID_BRIGHTNESS:
308 tpg_s_brightness(&vsensor->tpg, ctrl->val);
309 break;
310 case V4L2_CID_CONTRAST:
311 tpg_s_contrast(&vsensor->tpg, ctrl->val);
312 break;
313 case V4L2_CID_HUE:
314 tpg_s_hue(&vsensor->tpg, ctrl->val);
315 break;
316 case V4L2_CID_SATURATION:
317 tpg_s_saturation(&vsensor->tpg, ctrl->val);
318 break;
319 case VIMC_CID_OSD_TEXT_MODE:
320 vsensor->hw.osd_value = ctrl->val;
321 break;
322 default:
323 return -EINVAL;
324 }
325 return 0;
326 }
327
328 static const struct v4l2_ctrl_ops vimc_sensor_ctrl_ops = {
329 .s_ctrl = vimc_sensor_s_ctrl,
330 };
331
vimc_sensor_release(struct vimc_ent_device * ved)332 static void vimc_sensor_release(struct vimc_ent_device *ved)
333 {
334 struct vimc_sensor_device *vsensor =
335 container_of(ved, struct vimc_sensor_device, ved);
336
337 v4l2_ctrl_handler_free(&vsensor->hdl);
338 tpg_free(&vsensor->tpg);
339 v4l2_subdev_cleanup(&vsensor->sd);
340 media_entity_cleanup(vsensor->ved.ent);
341 kfree(vsensor);
342 }
343
344 /* Image Processing Controls */
345 static const struct v4l2_ctrl_config vimc_sensor_ctrl_class = {
346 .flags = V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_WRITE_ONLY,
347 .id = VIMC_CID_VIMC_CLASS,
348 .name = "VIMC Controls",
349 .type = V4L2_CTRL_TYPE_CTRL_CLASS,
350 };
351
352 static const struct v4l2_ctrl_config vimc_sensor_ctrl_test_pattern = {
353 .ops = &vimc_sensor_ctrl_ops,
354 .id = VIMC_CID_TEST_PATTERN,
355 .name = "Test Pattern",
356 .type = V4L2_CTRL_TYPE_MENU,
357 .max = TPG_PAT_NOISE,
358 .qmenu = tpg_pattern_strings,
359 };
360
361 static const char * const vimc_ctrl_osd_mode_strings[] = {
362 "All",
363 "Counters Only",
364 "None",
365 NULL,
366 };
367
368 static const struct v4l2_ctrl_config vimc_sensor_ctrl_osd_mode = {
369 .ops = &vimc_sensor_ctrl_ops,
370 .id = VIMC_CID_OSD_TEXT_MODE,
371 .name = "Show Information",
372 .type = V4L2_CTRL_TYPE_MENU,
373 .max = ARRAY_SIZE(vimc_ctrl_osd_mode_strings) - 2,
374 .qmenu = vimc_ctrl_osd_mode_strings,
375 };
376
vimc_sensor_add(struct vimc_device * vimc,const char * vcfg_name)377 static struct vimc_ent_device *vimc_sensor_add(struct vimc_device *vimc,
378 const char *vcfg_name)
379 {
380 struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;
381 struct vimc_sensor_device *vsensor;
382 int ret;
383
384 /* Allocate the vsensor struct */
385 vsensor = kzalloc(sizeof(*vsensor), GFP_KERNEL);
386 if (!vsensor)
387 return ERR_PTR(-ENOMEM);
388
389 v4l2_ctrl_handler_init(&vsensor->hdl, 4);
390
391 v4l2_ctrl_new_custom(&vsensor->hdl, &vimc_sensor_ctrl_class, NULL);
392 v4l2_ctrl_new_custom(&vsensor->hdl, &vimc_sensor_ctrl_test_pattern, NULL);
393 v4l2_ctrl_new_custom(&vsensor->hdl, &vimc_sensor_ctrl_osd_mode, NULL);
394 v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
395 V4L2_CID_VFLIP, 0, 1, 1, 0);
396 v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
397 V4L2_CID_HFLIP, 0, 1, 1, 0);
398 v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
399 V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
400 v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
401 V4L2_CID_CONTRAST, 0, 255, 1, 128);
402 v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
403 V4L2_CID_HUE, -128, 127, 1, 0);
404 v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
405 V4L2_CID_SATURATION, 0, 255, 1, 128);
406 vsensor->sd.ctrl_handler = &vsensor->hdl;
407 if (vsensor->hdl.error) {
408 ret = vsensor->hdl.error;
409 goto err_free_vsensor;
410 }
411
412 /* Initialize the test pattern generator */
413 tpg_init(&vsensor->tpg, fmt_default.width, fmt_default.height);
414 ret = tpg_alloc(&vsensor->tpg, VIMC_FRAME_MAX_WIDTH);
415 if (ret)
416 goto err_free_hdl;
417
418 /* Initialize ved and sd */
419 vsensor->pad.flags = MEDIA_PAD_FL_SOURCE;
420 ret = vimc_ent_sd_register(&vsensor->ved, &vsensor->sd, v4l2_dev,
421 vcfg_name,
422 MEDIA_ENT_F_CAM_SENSOR, 1, &vsensor->pad,
423 &vimc_sensor_internal_ops, &vimc_sensor_ops);
424 if (ret)
425 goto err_free_tpg;
426
427 vsensor->ved.process_frame = vimc_sensor_process_frame;
428 vsensor->ved.dev = vimc->mdev.dev;
429
430 return &vsensor->ved;
431
432 err_free_tpg:
433 tpg_free(&vsensor->tpg);
434 err_free_hdl:
435 v4l2_ctrl_handler_free(&vsensor->hdl);
436 err_free_vsensor:
437 kfree(vsensor);
438
439 return ERR_PTR(ret);
440 }
441
442 const struct vimc_ent_type vimc_sensor_type = {
443 .add = vimc_sensor_add,
444 .release = vimc_sensor_release
445 };
446