1
2
3
4
5
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
17enum vimc_sen_osd_mode {
18 VIMC_SEN_OSD_SHOW_ALL = 0,
19 VIMC_SEN_OSD_SHOW_COUNTERS = 1,
20 VIMC_SEN_OSD_SHOW_NONE = 2
21};
22
23struct vimc_sen_device {
24 struct vimc_ent_device ved;
25 struct v4l2_subdev sd;
26 struct tpg_data tpg;
27 u8 *frame;
28 enum vimc_sen_osd_mode osd_value;
29 u64 start_stream_ts;
30
31 struct v4l2_mbus_framefmt mbus_format;
32 struct v4l2_ctrl_handler hdl;
33 struct media_pad pad;
34};
35
36static 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
44static int vimc_sen_init_cfg(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_get_try_format(sd, sd_state, i);
53 *mf = fmt_default;
54 }
55
56 return 0;
57}
58
59static int vimc_sen_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(code->index);
64
65 if (!mbus_code)
66 return -EINVAL;
67
68 code->code = mbus_code;
69
70 return 0;
71}
72
73static int vimc_sen_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
83 vpix = vimc_pix_map_by_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
95static int vimc_sen_get_fmt(struct v4l2_subdev *sd,
96 struct v4l2_subdev_state *sd_state,
97 struct v4l2_subdev_format *fmt)
98{
99 struct vimc_sen_device *vsen =
100 container_of(sd, struct vimc_sen_device, sd);
101
102 fmt->format = fmt->which == V4L2_SUBDEV_FORMAT_TRY ?
103 *v4l2_subdev_get_try_format(sd, sd_state, fmt->pad) :
104 vsen->mbus_format;
105
106 return 0;
107}
108
109static void vimc_sen_tpg_s_format(struct vimc_sen_device *vsen)
110{
111 const struct vimc_pix_map *vpix =
112 vimc_pix_map_by_code(vsen->mbus_format.code);
113
114 tpg_reset_source(&vsen->tpg, vsen->mbus_format.width,
115 vsen->mbus_format.height, vsen->mbus_format.field);
116 tpg_s_bytesperline(&vsen->tpg, 0, vsen->mbus_format.width * vpix->bpp);
117 tpg_s_buf_height(&vsen->tpg, vsen->mbus_format.height);
118 tpg_s_fourcc(&vsen->tpg, vpix->pixelformat);
119
120 tpg_s_field(&vsen->tpg, vsen->mbus_format.field, false);
121 tpg_s_colorspace(&vsen->tpg, vsen->mbus_format.colorspace);
122 tpg_s_ycbcr_enc(&vsen->tpg, vsen->mbus_format.ycbcr_enc);
123 tpg_s_quantization(&vsen->tpg, vsen->mbus_format.quantization);
124 tpg_s_xfer_func(&vsen->tpg, vsen->mbus_format.xfer_func);
125}
126
127static void vimc_sen_adjust_fmt(struct v4l2_mbus_framefmt *fmt)
128{
129 const struct vimc_pix_map *vpix;
130
131
132 vpix = vimc_pix_map_by_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
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
148static int vimc_sen_set_fmt(struct v4l2_subdev *sd,
149 struct v4l2_subdev_state *sd_state,
150 struct v4l2_subdev_format *fmt)
151{
152 struct vimc_sen_device *vsen = v4l2_get_subdevdata(sd);
153 struct v4l2_mbus_framefmt *mf;
154
155 if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
156
157 if (vsen->frame)
158 return -EBUSY;
159
160 mf = &vsen->mbus_format;
161 } else {
162 mf = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
163 }
164
165
166 vimc_sen_adjust_fmt(&fmt->format);
167
168 dev_dbg(vsen->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", vsen->sd.name,
171
172 mf->width, mf->height, mf->code,
173 mf->colorspace, mf->quantization,
174 mf->xfer_func, mf->ycbcr_enc,
175
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
185static const struct v4l2_subdev_pad_ops vimc_sen_pad_ops = {
186 .init_cfg = vimc_sen_init_cfg,
187 .enum_mbus_code = vimc_sen_enum_mbus_code,
188 .enum_frame_size = vimc_sen_enum_frame_size,
189 .get_fmt = vimc_sen_get_fmt,
190 .set_fmt = vimc_sen_set_fmt,
191};
192
193static void *vimc_sen_process_frame(struct vimc_ent_device *ved,
194 const void *sink_frame)
195{
196 struct vimc_sen_device *vsen = container_of(ved, struct vimc_sen_device,
197 ved);
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(&vsen->tpg, 0, 0, vsen->frame);
204 tpg_calc_text_basep(&vsen->tpg, basep, 0, vsen->frame);
205 switch (vsen->osd_value) {
206 case VIMC_SEN_OSD_SHOW_ALL: {
207 const char *order = tpg_g_color_order(&vsen->tpg);
208
209 tpg_gen_text(&vsen->tpg, basep, line++ * line_height,
210 16, order);
211 snprintf(str, sizeof(str),
212 "brightness %3d, contrast %3d, saturation %3d, hue %d ",
213 vsen->tpg.brightness,
214 vsen->tpg.contrast,
215 vsen->tpg.saturation,
216 vsen->tpg.hue);
217 tpg_gen_text(&vsen->tpg, basep, line++ * line_height, 16, str);
218 snprintf(str, sizeof(str), "sensor size: %dx%d",
219 vsen->mbus_format.width,
220 vsen->mbus_format.height);
221 tpg_gen_text(&vsen->tpg, basep, line++ * line_height, 16, str);
222 fallthrough;
223 }
224 case VIMC_SEN_OSD_SHOW_COUNTERS: {
225 unsigned int ms;
226
227 ms = div_u64(ktime_get_ns() - vsen->start_stream_ts, 1000000);
228 snprintf(str, sizeof(str), "%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(&vsen->tpg, basep, line++ * line_height, 16, str);
234 break;
235 }
236 case VIMC_SEN_OSD_SHOW_NONE:
237 default:
238 break;
239 }
240
241 return vsen->frame;
242}
243
244static int vimc_sen_s_stream(struct v4l2_subdev *sd, int enable)
245{
246 struct vimc_sen_device *vsen =
247 container_of(sd, struct vimc_sen_device, sd);
248
249 if (enable) {
250 const struct vimc_pix_map *vpix;
251 unsigned int frame_size;
252
253 vsen->start_stream_ts = ktime_get_ns();
254
255
256 vpix = vimc_pix_map_by_code(vsen->mbus_format.code);
257 frame_size = vsen->mbus_format.width * vpix->bpp *
258 vsen->mbus_format.height;
259
260
261
262
263
264 vsen->frame = vmalloc(frame_size);
265 if (!vsen->frame)
266 return -ENOMEM;
267
268
269 vimc_sen_tpg_s_format(vsen);
270
271 } else {
272
273 vfree(vsen->frame);
274 vsen->frame = NULL;
275 }
276
277 return 0;
278}
279
280static const struct v4l2_subdev_core_ops vimc_sen_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
286static const struct v4l2_subdev_video_ops vimc_sen_video_ops = {
287 .s_stream = vimc_sen_s_stream,
288};
289
290static const struct v4l2_subdev_ops vimc_sen_ops = {
291 .core = &vimc_sen_core_ops,
292 .pad = &vimc_sen_pad_ops,
293 .video = &vimc_sen_video_ops,
294};
295
296static int vimc_sen_s_ctrl(struct v4l2_ctrl *ctrl)
297{
298 struct vimc_sen_device *vsen =
299 container_of(ctrl->handler, struct vimc_sen_device, hdl);
300
301 switch (ctrl->id) {
302 case VIMC_CID_TEST_PATTERN:
303 tpg_s_pattern(&vsen->tpg, ctrl->val);
304 break;
305 case V4L2_CID_HFLIP:
306 tpg_s_hflip(&vsen->tpg, ctrl->val);
307 break;
308 case V4L2_CID_VFLIP:
309 tpg_s_vflip(&vsen->tpg, ctrl->val);
310 break;
311 case V4L2_CID_BRIGHTNESS:
312 tpg_s_brightness(&vsen->tpg, ctrl->val);
313 break;
314 case V4L2_CID_CONTRAST:
315 tpg_s_contrast(&vsen->tpg, ctrl->val);
316 break;
317 case V4L2_CID_HUE:
318 tpg_s_hue(&vsen->tpg, ctrl->val);
319 break;
320 case V4L2_CID_SATURATION:
321 tpg_s_saturation(&vsen->tpg, ctrl->val);
322 break;
323 case VIMC_CID_OSD_TEXT_MODE:
324 vsen->osd_value = ctrl->val;
325 break;
326 default:
327 return -EINVAL;
328 }
329 return 0;
330}
331
332static const struct v4l2_ctrl_ops vimc_sen_ctrl_ops = {
333 .s_ctrl = vimc_sen_s_ctrl,
334};
335
336static void vimc_sen_release(struct vimc_ent_device *ved)
337{
338 struct vimc_sen_device *vsen =
339 container_of(ved, struct vimc_sen_device, ved);
340
341 v4l2_ctrl_handler_free(&vsen->hdl);
342 tpg_free(&vsen->tpg);
343 media_entity_cleanup(vsen->ved.ent);
344 kfree(vsen);
345}
346
347
348static const struct v4l2_ctrl_config vimc_sen_ctrl_class = {
349 .flags = V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_WRITE_ONLY,
350 .id = VIMC_CID_VIMC_CLASS,
351 .name = "VIMC Controls",
352 .type = V4L2_CTRL_TYPE_CTRL_CLASS,
353};
354
355static const struct v4l2_ctrl_config vimc_sen_ctrl_test_pattern = {
356 .ops = &vimc_sen_ctrl_ops,
357 .id = VIMC_CID_TEST_PATTERN,
358 .name = "Test Pattern",
359 .type = V4L2_CTRL_TYPE_MENU,
360 .max = TPG_PAT_NOISE,
361 .qmenu = tpg_pattern_strings,
362};
363
364static const char * const vimc_ctrl_osd_mode_strings[] = {
365 "All",
366 "Counters Only",
367 "None",
368 NULL,
369};
370
371static const struct v4l2_ctrl_config vimc_sen_ctrl_osd_mode = {
372 .ops = &vimc_sen_ctrl_ops,
373 .id = VIMC_CID_OSD_TEXT_MODE,
374 .name = "Show Information",
375 .type = V4L2_CTRL_TYPE_MENU,
376 .max = ARRAY_SIZE(vimc_ctrl_osd_mode_strings) - 2,
377 .qmenu = vimc_ctrl_osd_mode_strings,
378};
379
380static struct vimc_ent_device *vimc_sen_add(struct vimc_device *vimc,
381 const char *vcfg_name)
382{
383 struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;
384 struct vimc_sen_device *vsen;
385 int ret;
386
387
388 vsen = kzalloc(sizeof(*vsen), GFP_KERNEL);
389 if (!vsen)
390 return ERR_PTR(-ENOMEM);
391
392 v4l2_ctrl_handler_init(&vsen->hdl, 4);
393
394 v4l2_ctrl_new_custom(&vsen->hdl, &vimc_sen_ctrl_class, NULL);
395 v4l2_ctrl_new_custom(&vsen->hdl, &vimc_sen_ctrl_test_pattern, NULL);
396 v4l2_ctrl_new_custom(&vsen->hdl, &vimc_sen_ctrl_osd_mode, NULL);
397 v4l2_ctrl_new_std(&vsen->hdl, &vimc_sen_ctrl_ops,
398 V4L2_CID_VFLIP, 0, 1, 1, 0);
399 v4l2_ctrl_new_std(&vsen->hdl, &vimc_sen_ctrl_ops,
400 V4L2_CID_HFLIP, 0, 1, 1, 0);
401 v4l2_ctrl_new_std(&vsen->hdl, &vimc_sen_ctrl_ops,
402 V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
403 v4l2_ctrl_new_std(&vsen->hdl, &vimc_sen_ctrl_ops,
404 V4L2_CID_CONTRAST, 0, 255, 1, 128);
405 v4l2_ctrl_new_std(&vsen->hdl, &vimc_sen_ctrl_ops,
406 V4L2_CID_HUE, -128, 127, 1, 0);
407 v4l2_ctrl_new_std(&vsen->hdl, &vimc_sen_ctrl_ops,
408 V4L2_CID_SATURATION, 0, 255, 1, 128);
409 vsen->sd.ctrl_handler = &vsen->hdl;
410 if (vsen->hdl.error) {
411 ret = vsen->hdl.error;
412 goto err_free_vsen;
413 }
414
415
416 tpg_init(&vsen->tpg, vsen->mbus_format.width,
417 vsen->mbus_format.height);
418 ret = tpg_alloc(&vsen->tpg, VIMC_FRAME_MAX_WIDTH);
419 if (ret)
420 goto err_free_hdl;
421
422
423 vsen->pad.flags = MEDIA_PAD_FL_SOURCE;
424 ret = vimc_ent_sd_register(&vsen->ved, &vsen->sd, v4l2_dev,
425 vcfg_name,
426 MEDIA_ENT_F_CAM_SENSOR, 1, &vsen->pad,
427 &vimc_sen_ops);
428 if (ret)
429 goto err_free_tpg;
430
431 vsen->ved.process_frame = vimc_sen_process_frame;
432 vsen->ved.dev = vimc->mdev.dev;
433
434
435 vsen->mbus_format = fmt_default;
436
437 return &vsen->ved;
438
439err_free_tpg:
440 tpg_free(&vsen->tpg);
441err_free_hdl:
442 v4l2_ctrl_handler_free(&vsen->hdl);
443err_free_vsen:
444 kfree(vsen);
445
446 return ERR_PTR(ret);
447}
448
449struct vimc_ent_type vimc_sen_type = {
450 .add = vimc_sen_add,
451 .release = vimc_sen_release
452};
453
