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
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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