1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3 * vimc-scaler.c Virtual Media Controller Driver
4 *
5 * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
6 */
7
8 #include <linux/moduleparam.h>
9 #include <linux/vmalloc.h>
10 #include <linux/v4l2-mediabus.h>
11 #include <media/v4l2-rect.h>
12 #include <media/v4l2-subdev.h>
13
14 #include "vimc-common.h"
15
16 static unsigned int sca_mult = 3;
17 module_param(sca_mult, uint, 0000);
18 MODULE_PARM_DESC(sca_mult, " the image size multiplier");
19
20 #define MAX_ZOOM 8
21
22 #define VIMC_SCA_FMT_WIDTH_DEFAULT 640
23 #define VIMC_SCA_FMT_HEIGHT_DEFAULT 480
24
25 struct vimc_sca_device {
26 struct vimc_ent_device ved;
27 struct v4l2_subdev sd;
28 /* NOTE: the source fmt is the same as the sink
29 * with the width and hight multiplied by mult
30 */
31 struct v4l2_mbus_framefmt sink_fmt;
32 struct v4l2_rect crop_rect;
33 /* Values calculated when the stream starts */
34 u8 *src_frame;
35 unsigned int src_line_size;
36 unsigned int bpp;
37 struct media_pad pads[2];
38 };
39
40 static const struct v4l2_mbus_framefmt sink_fmt_default = {
41 .width = VIMC_SCA_FMT_WIDTH_DEFAULT,
42 .height = VIMC_SCA_FMT_HEIGHT_DEFAULT,
43 .code = MEDIA_BUS_FMT_RGB888_1X24,
44 .field = V4L2_FIELD_NONE,
45 .colorspace = V4L2_COLORSPACE_SRGB,
46 };
47
48 static const struct v4l2_rect crop_rect_default = {
49 .width = VIMC_SCA_FMT_WIDTH_DEFAULT,
50 .height = VIMC_SCA_FMT_HEIGHT_DEFAULT,
51 .top = 0,
52 .left = 0,
53 };
54
55 static const struct v4l2_rect crop_rect_min = {
56 .width = VIMC_FRAME_MIN_WIDTH,
57 .height = VIMC_FRAME_MIN_HEIGHT,
58 .top = 0,
59 .left = 0,
60 };
61
62 static struct v4l2_rect
vimc_sca_get_crop_bound_sink(const struct v4l2_mbus_framefmt * sink_fmt)63 vimc_sca_get_crop_bound_sink(const struct v4l2_mbus_framefmt *sink_fmt)
64 {
65 /* Get the crop bounds to clamp the crop rectangle correctly */
66 struct v4l2_rect r = {
67 .left = 0,
68 .top = 0,
69 .width = sink_fmt->width,
70 .height = sink_fmt->height,
71 };
72 return r;
73 }
74
vimc_sca_adjust_sink_crop(struct v4l2_rect * r,const struct v4l2_mbus_framefmt * sink_fmt)75 static void vimc_sca_adjust_sink_crop(struct v4l2_rect *r,
76 const struct v4l2_mbus_framefmt *sink_fmt)
77 {
78 const struct v4l2_rect sink_rect =
79 vimc_sca_get_crop_bound_sink(sink_fmt);
80
81 /* Disallow rectangles smaller than the minimal one. */
82 v4l2_rect_set_min_size(r, &crop_rect_min);
83 v4l2_rect_map_inside(r, &sink_rect);
84 }
85
vimc_sca_init_cfg(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg)86 static int vimc_sca_init_cfg(struct v4l2_subdev *sd,
87 struct v4l2_subdev_pad_config *cfg)
88 {
89 struct v4l2_mbus_framefmt *mf;
90 struct v4l2_rect *r;
91 unsigned int i;
92
93 mf = v4l2_subdev_get_try_format(sd, cfg, 0);
94 *mf = sink_fmt_default;
95
96 r = v4l2_subdev_get_try_crop(sd, cfg, 0);
97 *r = crop_rect_default;
98
99 for (i = 1; i < sd->entity.num_pads; i++) {
100 mf = v4l2_subdev_get_try_format(sd, cfg, i);
101 *mf = sink_fmt_default;
102 mf->width = mf->width * sca_mult;
103 mf->height = mf->height * sca_mult;
104 }
105
106 return 0;
107 }
108
vimc_sca_enum_mbus_code(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_mbus_code_enum * code)109 static int vimc_sca_enum_mbus_code(struct v4l2_subdev *sd,
110 struct v4l2_subdev_pad_config *cfg,
111 struct v4l2_subdev_mbus_code_enum *code)
112 {
113 u32 mbus_code = vimc_mbus_code_by_index(code->index);
114 const struct vimc_pix_map *vpix;
115
116 if (!mbus_code)
117 return -EINVAL;
118
119 vpix = vimc_pix_map_by_code(mbus_code);
120
121 /* We don't support bayer format */
122 if (!vpix || vpix->bayer)
123 return -EINVAL;
124
125 code->code = mbus_code;
126
127 return 0;
128 }
129
vimc_sca_enum_frame_size(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_frame_size_enum * fse)130 static int vimc_sca_enum_frame_size(struct v4l2_subdev *sd,
131 struct v4l2_subdev_pad_config *cfg,
132 struct v4l2_subdev_frame_size_enum *fse)
133 {
134 const struct vimc_pix_map *vpix;
135
136 if (fse->index)
137 return -EINVAL;
138
139 /* Only accept code in the pix map table in non bayer format */
140 vpix = vimc_pix_map_by_code(fse->code);
141 if (!vpix || vpix->bayer)
142 return -EINVAL;
143
144 fse->min_width = VIMC_FRAME_MIN_WIDTH;
145 fse->min_height = VIMC_FRAME_MIN_HEIGHT;
146
147 if (VIMC_IS_SINK(fse->pad)) {
148 fse->max_width = VIMC_FRAME_MAX_WIDTH;
149 fse->max_height = VIMC_FRAME_MAX_HEIGHT;
150 } else {
151 fse->max_width = VIMC_FRAME_MAX_WIDTH * MAX_ZOOM;
152 fse->max_height = VIMC_FRAME_MAX_HEIGHT * MAX_ZOOM;
153 }
154
155 return 0;
156 }
157
vimc_sca_get_fmt(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_format * format)158 static int vimc_sca_get_fmt(struct v4l2_subdev *sd,
159 struct v4l2_subdev_pad_config *cfg,
160 struct v4l2_subdev_format *format)
161 {
162 struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd);
163 struct v4l2_rect *crop_rect;
164
165 /* Get the current sink format */
166 if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
167 format->format = *v4l2_subdev_get_try_format(sd, cfg, 0);
168 crop_rect = v4l2_subdev_get_try_crop(sd, cfg, 0);
169 } else {
170 format->format = vsca->sink_fmt;
171 crop_rect = &vsca->crop_rect;
172 }
173
174 /* Scale the frame size for the source pad */
175 if (VIMC_IS_SRC(format->pad)) {
176 format->format.width = crop_rect->width * sca_mult;
177 format->format.height = crop_rect->height * sca_mult;
178 }
179
180 return 0;
181 }
182
vimc_sca_adjust_sink_fmt(struct v4l2_mbus_framefmt * fmt)183 static void vimc_sca_adjust_sink_fmt(struct v4l2_mbus_framefmt *fmt)
184 {
185 const struct vimc_pix_map *vpix;
186
187 /* Only accept code in the pix map table in non bayer format */
188 vpix = vimc_pix_map_by_code(fmt->code);
189 if (!vpix || vpix->bayer)
190 fmt->code = sink_fmt_default.code;
191
192 fmt->width = clamp_t(u32, fmt->width, VIMC_FRAME_MIN_WIDTH,
193 VIMC_FRAME_MAX_WIDTH) & ~1;
194 fmt->height = clamp_t(u32, fmt->height, VIMC_FRAME_MIN_HEIGHT,
195 VIMC_FRAME_MAX_HEIGHT) & ~1;
196
197 if (fmt->field == V4L2_FIELD_ANY)
198 fmt->field = sink_fmt_default.field;
199
200 vimc_colorimetry_clamp(fmt);
201 }
202
vimc_sca_set_fmt(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_format * fmt)203 static int vimc_sca_set_fmt(struct v4l2_subdev *sd,
204 struct v4l2_subdev_pad_config *cfg,
205 struct v4l2_subdev_format *fmt)
206 {
207 struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd);
208 struct v4l2_mbus_framefmt *sink_fmt;
209 struct v4l2_rect *crop_rect;
210
211 if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
212 /* Do not change the format while stream is on */
213 if (vsca->src_frame)
214 return -EBUSY;
215
216 sink_fmt = &vsca->sink_fmt;
217 crop_rect = &vsca->crop_rect;
218 } else {
219 sink_fmt = v4l2_subdev_get_try_format(sd, cfg, 0);
220 crop_rect = v4l2_subdev_get_try_crop(sd, cfg, 0);
221 }
222
223 /*
224 * Do not change the format of the source pad,
225 * it is propagated from the sink
226 */
227 if (VIMC_IS_SRC(fmt->pad)) {
228 fmt->format = *sink_fmt;
229 fmt->format.width = crop_rect->width * sca_mult;
230 fmt->format.height = crop_rect->height * sca_mult;
231 } else {
232 /* Set the new format in the sink pad */
233 vimc_sca_adjust_sink_fmt(&fmt->format);
234
235 dev_dbg(vsca->ved.dev, "%s: sink format update: "
236 "old:%dx%d (0x%x, %d, %d, %d, %d) "
237 "new:%dx%d (0x%x, %d, %d, %d, %d)\n", vsca->sd.name,
238 /* old */
239 sink_fmt->width, sink_fmt->height, sink_fmt->code,
240 sink_fmt->colorspace, sink_fmt->quantization,
241 sink_fmt->xfer_func, sink_fmt->ycbcr_enc,
242 /* new */
243 fmt->format.width, fmt->format.height, fmt->format.code,
244 fmt->format.colorspace, fmt->format.quantization,
245 fmt->format.xfer_func, fmt->format.ycbcr_enc);
246
247 *sink_fmt = fmt->format;
248
249 /* Do the crop, but respect the current bounds */
250 vimc_sca_adjust_sink_crop(crop_rect, sink_fmt);
251 }
252
253 return 0;
254 }
255
vimc_sca_get_selection(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_selection * sel)256 static int vimc_sca_get_selection(struct v4l2_subdev *sd,
257 struct v4l2_subdev_pad_config *cfg,
258 struct v4l2_subdev_selection *sel)
259 {
260 struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd);
261 struct v4l2_mbus_framefmt *sink_fmt;
262 struct v4l2_rect *crop_rect;
263
264 if (VIMC_IS_SRC(sel->pad))
265 return -EINVAL;
266
267 if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
268 sink_fmt = &vsca->sink_fmt;
269 crop_rect = &vsca->crop_rect;
270 } else {
271 sink_fmt = v4l2_subdev_get_try_format(sd, cfg, 0);
272 crop_rect = v4l2_subdev_get_try_crop(sd, cfg, 0);
273 }
274
275 switch (sel->target) {
276 case V4L2_SEL_TGT_CROP:
277 sel->r = *crop_rect;
278 break;
279 case V4L2_SEL_TGT_CROP_BOUNDS:
280 sel->r = vimc_sca_get_crop_bound_sink(sink_fmt);
281 break;
282 default:
283 return -EINVAL;
284 }
285
286 return 0;
287 }
288
vimc_sca_set_selection(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_selection * sel)289 static int vimc_sca_set_selection(struct v4l2_subdev *sd,
290 struct v4l2_subdev_pad_config *cfg,
291 struct v4l2_subdev_selection *sel)
292 {
293 struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd);
294 struct v4l2_mbus_framefmt *sink_fmt;
295 struct v4l2_rect *crop_rect;
296
297 if (VIMC_IS_SRC(sel->pad))
298 return -EINVAL;
299
300 if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
301 /* Do not change the format while stream is on */
302 if (vsca->src_frame)
303 return -EBUSY;
304
305 crop_rect = &vsca->crop_rect;
306 sink_fmt = &vsca->sink_fmt;
307 } else {
308 crop_rect = v4l2_subdev_get_try_crop(sd, cfg, 0);
309 sink_fmt = v4l2_subdev_get_try_format(sd, cfg, 0);
310 }
311
312 switch (sel->target) {
313 case V4L2_SEL_TGT_CROP:
314 /* Do the crop, but respect the current bounds */
315 vimc_sca_adjust_sink_crop(&sel->r, sink_fmt);
316 *crop_rect = sel->r;
317 break;
318 default:
319 return -EINVAL;
320 }
321
322 return 0;
323 }
324
325 static const struct v4l2_subdev_pad_ops vimc_sca_pad_ops = {
326 .init_cfg = vimc_sca_init_cfg,
327 .enum_mbus_code = vimc_sca_enum_mbus_code,
328 .enum_frame_size = vimc_sca_enum_frame_size,
329 .get_fmt = vimc_sca_get_fmt,
330 .set_fmt = vimc_sca_set_fmt,
331 .get_selection = vimc_sca_get_selection,
332 .set_selection = vimc_sca_set_selection,
333 };
334
vimc_sca_s_stream(struct v4l2_subdev * sd,int enable)335 static int vimc_sca_s_stream(struct v4l2_subdev *sd, int enable)
336 {
337 struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd);
338
339 if (enable) {
340 const struct vimc_pix_map *vpix;
341 unsigned int frame_size;
342
343 if (vsca->src_frame)
344 return 0;
345
346 /* Save the bytes per pixel of the sink */
347 vpix = vimc_pix_map_by_code(vsca->sink_fmt.code);
348 vsca->bpp = vpix->bpp;
349
350 /* Calculate the width in bytes of the src frame */
351 vsca->src_line_size = vsca->crop_rect.width *
352 sca_mult * vsca->bpp;
353
354 /* Calculate the frame size of the source pad */
355 frame_size = vsca->src_line_size * vsca->crop_rect.height *
356 sca_mult;
357
358 /* Allocate the frame buffer. Use vmalloc to be able to
359 * allocate a large amount of memory
360 */
361 vsca->src_frame = vmalloc(frame_size);
362 if (!vsca->src_frame)
363 return -ENOMEM;
364
365 } else {
366 if (!vsca->src_frame)
367 return 0;
368
369 vfree(vsca->src_frame);
370 vsca->src_frame = NULL;
371 }
372
373 return 0;
374 }
375
376 static const struct v4l2_subdev_video_ops vimc_sca_video_ops = {
377 .s_stream = vimc_sca_s_stream,
378 };
379
380 static const struct v4l2_subdev_ops vimc_sca_ops = {
381 .pad = &vimc_sca_pad_ops,
382 .video = &vimc_sca_video_ops,
383 };
384
vimc_sca_fill_pix(u8 * const ptr,const u8 * const pixel,const unsigned int bpp)385 static void vimc_sca_fill_pix(u8 *const ptr,
386 const u8 *const pixel,
387 const unsigned int bpp)
388 {
389 unsigned int i;
390
391 /* copy the pixel to the pointer */
392 for (i = 0; i < bpp; i++)
393 ptr[i] = pixel[i];
394 }
395
vimc_sca_scale_pix(const struct vimc_sca_device * const vsca,unsigned int lin,unsigned int col,const u8 * const sink_frame)396 static void vimc_sca_scale_pix(const struct vimc_sca_device *const vsca,
397 unsigned int lin, unsigned int col,
398 const u8 *const sink_frame)
399 {
400 const struct v4l2_rect crop_rect = vsca->crop_rect;
401 unsigned int i, j, index;
402 const u8 *pixel;
403
404 /* Point to the pixel value in position (lin, col) in the sink frame */
405 index = VIMC_FRAME_INDEX(lin, col,
406 vsca->sink_fmt.width,
407 vsca->bpp);
408 pixel = &sink_frame[index];
409
410 dev_dbg(vsca->ved.dev,
411 "sca: %s: --- scale_pix sink pos %dx%d, index %d ---\n",
412 vsca->sd.name, lin, col, index);
413
414 /* point to the place we are going to put the first pixel
415 * in the scaled src frame
416 */
417 lin -= crop_rect.top;
418 col -= crop_rect.left;
419 index = VIMC_FRAME_INDEX(lin * sca_mult, col * sca_mult,
420 crop_rect.width * sca_mult, vsca->bpp);
421
422 dev_dbg(vsca->ved.dev, "sca: %s: scale_pix src pos %dx%d, index %d\n",
423 vsca->sd.name, lin * sca_mult, col * sca_mult, index);
424
425 /* Repeat this pixel mult times */
426 for (i = 0; i < sca_mult; i++) {
427 /* Iterate through each beginning of a
428 * pixel repetition in a line
429 */
430 for (j = 0; j < sca_mult * vsca->bpp; j += vsca->bpp) {
431 dev_dbg(vsca->ved.dev,
432 "sca: %s: sca: scale_pix src pos %d\n",
433 vsca->sd.name, index + j);
434
435 /* copy the pixel to the position index + j */
436 vimc_sca_fill_pix(&vsca->src_frame[index + j],
437 pixel, vsca->bpp);
438 }
439
440 /* move the index to the next line */
441 index += vsca->src_line_size;
442 }
443 }
444
vimc_sca_fill_src_frame(const struct vimc_sca_device * const vsca,const u8 * const sink_frame)445 static void vimc_sca_fill_src_frame(const struct vimc_sca_device *const vsca,
446 const u8 *const sink_frame)
447 {
448 const struct v4l2_rect r = vsca->crop_rect;
449 unsigned int i, j;
450
451 /* Scale each pixel from the original sink frame */
452 /* TODO: implement scale down, only scale up is supported for now */
453 for (i = r.top; i < r.top + r.height; i++)
454 for (j = r.left; j < r.left + r.width; j++)
455 vimc_sca_scale_pix(vsca, i, j, sink_frame);
456 }
457
vimc_sca_process_frame(struct vimc_ent_device * ved,const void * sink_frame)458 static void *vimc_sca_process_frame(struct vimc_ent_device *ved,
459 const void *sink_frame)
460 {
461 struct vimc_sca_device *vsca = container_of(ved, struct vimc_sca_device,
462 ved);
463
464 /* If the stream in this node is not active, just return */
465 if (!vsca->src_frame)
466 return ERR_PTR(-EINVAL);
467
468 vimc_sca_fill_src_frame(vsca, sink_frame);
469
470 return vsca->src_frame;
471 };
472
vimc_sca_release(struct vimc_ent_device * ved)473 static void vimc_sca_release(struct vimc_ent_device *ved)
474 {
475 struct vimc_sca_device *vsca =
476 container_of(ved, struct vimc_sca_device, ved);
477
478 media_entity_cleanup(vsca->ved.ent);
479 kfree(vsca);
480 }
481
vimc_sca_add(struct vimc_device * vimc,const char * vcfg_name)482 static struct vimc_ent_device *vimc_sca_add(struct vimc_device *vimc,
483 const char *vcfg_name)
484 {
485 struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;
486 struct vimc_sca_device *vsca;
487 int ret;
488
489 /* Allocate the vsca struct */
490 vsca = kzalloc(sizeof(*vsca), GFP_KERNEL);
491 if (!vsca)
492 return ERR_PTR(-ENOMEM);
493
494 /* Initialize ved and sd */
495 vsca->pads[0].flags = MEDIA_PAD_FL_SINK;
496 vsca->pads[1].flags = MEDIA_PAD_FL_SOURCE;
497
498 ret = vimc_ent_sd_register(&vsca->ved, &vsca->sd, v4l2_dev,
499 vcfg_name,
500 MEDIA_ENT_F_PROC_VIDEO_SCALER, 2,
501 vsca->pads, &vimc_sca_ops);
502 if (ret) {
503 kfree(vsca);
504 return ERR_PTR(ret);
505 }
506
507 vsca->ved.process_frame = vimc_sca_process_frame;
508 vsca->ved.dev = vimc->mdev.dev;
509
510 /* Initialize the frame format */
511 vsca->sink_fmt = sink_fmt_default;
512
513 /* Initialize the crop selection */
514 vsca->crop_rect = crop_rect_default;
515
516 return &vsca->ved;
517 }
518
519 struct vimc_ent_type vimc_sca_type = {
520 .add = vimc_sca_add,
521 .release = vimc_sca_release
522 };
523