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