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/string.h>
10 #include <linux/vmalloc.h>
11 #include <linux/v4l2-mediabus.h>
12 #include <media/v4l2-rect.h>
13 #include <media/v4l2-subdev.h>
14 
15 #include "vimc-common.h"
16 
17 /* Pad identifier */
18 enum vic_sca_pad {
19 	VIMC_SCA_SINK = 0,
20 	VIMC_SCA_SRC = 1,
21 };
22 
23 #define VIMC_SCA_FMT_WIDTH_DEFAULT  640
24 #define VIMC_SCA_FMT_HEIGHT_DEFAULT 480
25 
26 struct vimc_sca_device {
27 	struct vimc_ent_device ved;
28 	struct v4l2_subdev sd;
29 	struct v4l2_rect crop_rect;
30 	/* Frame format for both sink and src pad */
31 	struct v4l2_mbus_framefmt fmt[2];
32 	/* Values calculated when the stream starts */
33 	u8 *src_frame;
34 	unsigned int bpp;
35 	struct media_pad pads[2];
36 };
37 
38 static const struct v4l2_mbus_framefmt fmt_default = {
39 	.width = VIMC_SCA_FMT_WIDTH_DEFAULT,
40 	.height = VIMC_SCA_FMT_HEIGHT_DEFAULT,
41 	.code = MEDIA_BUS_FMT_RGB888_1X24,
42 	.field = V4L2_FIELD_NONE,
43 	.colorspace = V4L2_COLORSPACE_SRGB,
44 };
45 
46 static const struct v4l2_rect crop_rect_default = {
47 	.width = VIMC_SCA_FMT_WIDTH_DEFAULT,
48 	.height = VIMC_SCA_FMT_HEIGHT_DEFAULT,
49 	.top = 0,
50 	.left = 0,
51 };
52 
53 static const struct v4l2_rect crop_rect_min = {
54 	.width = VIMC_FRAME_MIN_WIDTH,
55 	.height = VIMC_FRAME_MIN_HEIGHT,
56 	.top = 0,
57 	.left = 0,
58 };
59 
60 static struct v4l2_rect
vimc_sca_get_crop_bound_sink(const struct v4l2_mbus_framefmt * sink_fmt)61 vimc_sca_get_crop_bound_sink(const struct v4l2_mbus_framefmt *sink_fmt)
62 {
63 	/* Get the crop bounds to clamp the crop rectangle correctly */
64 	struct v4l2_rect r = {
65 		.left = 0,
66 		.top = 0,
67 		.width = sink_fmt->width,
68 		.height = sink_fmt->height,
69 	};
70 	return r;
71 }
72 
vimc_sca_init_cfg(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state)73 static int vimc_sca_init_cfg(struct v4l2_subdev *sd,
74 			     struct v4l2_subdev_state *sd_state)
75 {
76 	struct v4l2_mbus_framefmt *mf;
77 	struct v4l2_rect *r;
78 	unsigned int i;
79 
80 	for (i = 0; i < sd->entity.num_pads; i++) {
81 		mf = v4l2_subdev_get_try_format(sd, sd_state, i);
82 		*mf = fmt_default;
83 	}
84 
85 	r = v4l2_subdev_get_try_crop(sd, sd_state, VIMC_SCA_SINK);
86 	*r = crop_rect_default;
87 
88 	return 0;
89 }
90 
vimc_sca_enum_mbus_code(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_mbus_code_enum * code)91 static int vimc_sca_enum_mbus_code(struct v4l2_subdev *sd,
92 				   struct v4l2_subdev_state *sd_state,
93 				   struct v4l2_subdev_mbus_code_enum *code)
94 {
95 	u32 mbus_code = vimc_mbus_code_by_index(code->index);
96 	const struct vimc_pix_map *vpix;
97 
98 	if (!mbus_code)
99 		return -EINVAL;
100 
101 	vpix = vimc_pix_map_by_code(mbus_code);
102 
103 	/* We don't support bayer format */
104 	if (!vpix || vpix->bayer)
105 		return -EINVAL;
106 
107 	code->code = mbus_code;
108 
109 	return 0;
110 }
111 
vimc_sca_enum_frame_size(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_frame_size_enum * fse)112 static int vimc_sca_enum_frame_size(struct v4l2_subdev *sd,
113 				    struct v4l2_subdev_state *sd_state,
114 				    struct v4l2_subdev_frame_size_enum *fse)
115 {
116 	const struct vimc_pix_map *vpix;
117 
118 	if (fse->index)
119 		return -EINVAL;
120 
121 	/* Only accept code in the pix map table in non bayer format */
122 	vpix = vimc_pix_map_by_code(fse->code);
123 	if (!vpix || vpix->bayer)
124 		return -EINVAL;
125 
126 	fse->min_width = VIMC_FRAME_MIN_WIDTH;
127 	fse->min_height = VIMC_FRAME_MIN_HEIGHT;
128 
129 	fse->max_width = VIMC_FRAME_MAX_WIDTH;
130 	fse->max_height = VIMC_FRAME_MAX_HEIGHT;
131 
132 	return 0;
133 }
134 
135 static struct v4l2_mbus_framefmt *
vimc_sca_pad_format(struct vimc_sca_device * vsca,struct v4l2_subdev_state * sd_state,u32 pad,enum v4l2_subdev_format_whence which)136 vimc_sca_pad_format(struct vimc_sca_device *vsca,
137 		    struct v4l2_subdev_state *sd_state, u32 pad,
138 		    enum v4l2_subdev_format_whence which)
139 {
140 	if (which == V4L2_SUBDEV_FORMAT_TRY)
141 		return v4l2_subdev_get_try_format(&vsca->sd, sd_state, pad);
142 	else
143 		return &vsca->fmt[pad];
144 }
145 
146 static struct v4l2_rect *
vimc_sca_pad_crop(struct vimc_sca_device * vsca,struct v4l2_subdev_state * sd_state,enum v4l2_subdev_format_whence which)147 vimc_sca_pad_crop(struct vimc_sca_device *vsca,
148 		  struct v4l2_subdev_state *sd_state,
149 		  enum v4l2_subdev_format_whence which)
150 {
151 	if (which == V4L2_SUBDEV_FORMAT_TRY)
152 		return v4l2_subdev_get_try_crop(&vsca->sd, sd_state,
153 						VIMC_SCA_SINK);
154 	else
155 		return &vsca->crop_rect;
156 }
157 
vimc_sca_get_fmt(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * format)158 static int vimc_sca_get_fmt(struct v4l2_subdev *sd,
159 			    struct v4l2_subdev_state *sd_state,
160 			    struct v4l2_subdev_format *format)
161 {
162 	struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd);
163 
164 	format->format = *vimc_sca_pad_format(vsca, sd_state, format->pad,
165 					      format->which);
166 	return 0;
167 }
168 
vimc_sca_set_fmt(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * format)169 static int vimc_sca_set_fmt(struct v4l2_subdev *sd,
170 			    struct v4l2_subdev_state *sd_state,
171 			    struct v4l2_subdev_format *format)
172 {
173 	struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd);
174 	struct v4l2_mbus_framefmt *fmt;
175 
176 	/* Do not change the active format while stream is on */
177 	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE && vsca->src_frame)
178 		return -EBUSY;
179 
180 	fmt = vimc_sca_pad_format(vsca, sd_state, format->pad, format->which);
181 
182 	/*
183 	 * The media bus code and colorspace can only be changed on the sink
184 	 * pad, the source pad only follows.
185 	 */
186 	if (format->pad == VIMC_SCA_SINK) {
187 		const struct vimc_pix_map *vpix;
188 
189 		/* Only accept code in the pix map table in non bayer format. */
190 		vpix = vimc_pix_map_by_code(format->format.code);
191 		if (vpix && !vpix->bayer)
192 			fmt->code = format->format.code;
193 		else
194 			fmt->code = fmt_default.code;
195 
196 		/* Clamp the colorspace to valid values. */
197 		fmt->colorspace = format->format.colorspace;
198 		fmt->ycbcr_enc = format->format.ycbcr_enc;
199 		fmt->quantization = format->format.quantization;
200 		fmt->xfer_func = format->format.xfer_func;
201 		vimc_colorimetry_clamp(fmt);
202 	}
203 
204 	/* Clamp and align the width and height */
205 	fmt->width = clamp_t(u32, format->format.width, VIMC_FRAME_MIN_WIDTH,
206 			     VIMC_FRAME_MAX_WIDTH) & ~1;
207 	fmt->height = clamp_t(u32, format->format.height, VIMC_FRAME_MIN_HEIGHT,
208 			      VIMC_FRAME_MAX_HEIGHT) & ~1;
209 
210 	/*
211 	 * Propagate the sink pad format to the crop rectangle and the source
212 	 * pad.
213 	 */
214 	if (format->pad == VIMC_SCA_SINK) {
215 		struct v4l2_mbus_framefmt *src_fmt;
216 		struct v4l2_rect *crop;
217 
218 		crop = vimc_sca_pad_crop(vsca, sd_state, format->which);
219 		crop->width = fmt->width;
220 		crop->height = fmt->height;
221 		crop->top = 0;
222 		crop->left = 0;
223 
224 		src_fmt = vimc_sca_pad_format(vsca, sd_state, VIMC_SCA_SRC,
225 					      format->which);
226 		*src_fmt = *fmt;
227 	}
228 
229 	format->format = *fmt;
230 
231 	return 0;
232 }
233 
vimc_sca_get_selection(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_selection * sel)234 static int vimc_sca_get_selection(struct v4l2_subdev *sd,
235 				  struct v4l2_subdev_state *sd_state,
236 				  struct v4l2_subdev_selection *sel)
237 {
238 	struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd);
239 	struct v4l2_mbus_framefmt *sink_fmt;
240 
241 	if (VIMC_IS_SRC(sel->pad))
242 		return -EINVAL;
243 
244 	switch (sel->target) {
245 	case V4L2_SEL_TGT_CROP:
246 		sel->r = *vimc_sca_pad_crop(vsca, sd_state, sel->which);
247 		break;
248 	case V4L2_SEL_TGT_CROP_BOUNDS:
249 		sink_fmt = vimc_sca_pad_format(vsca, sd_state, VIMC_SCA_SINK,
250 					       sel->which);
251 		sel->r = vimc_sca_get_crop_bound_sink(sink_fmt);
252 		break;
253 	default:
254 		return -EINVAL;
255 	}
256 
257 	return 0;
258 }
259 
vimc_sca_adjust_sink_crop(struct v4l2_rect * r,const struct v4l2_mbus_framefmt * sink_fmt)260 static void vimc_sca_adjust_sink_crop(struct v4l2_rect *r,
261 				      const struct v4l2_mbus_framefmt *sink_fmt)
262 {
263 	const struct v4l2_rect sink_rect =
264 		vimc_sca_get_crop_bound_sink(sink_fmt);
265 
266 	/* Disallow rectangles smaller than the minimal one. */
267 	v4l2_rect_set_min_size(r, &crop_rect_min);
268 	v4l2_rect_map_inside(r, &sink_rect);
269 }
270 
vimc_sca_set_selection(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_selection * sel)271 static int vimc_sca_set_selection(struct v4l2_subdev *sd,
272 				  struct v4l2_subdev_state *sd_state,
273 				  struct v4l2_subdev_selection *sel)
274 {
275 	struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd);
276 	struct v4l2_mbus_framefmt *sink_fmt;
277 	struct v4l2_rect *crop_rect;
278 
279 	/* Only support setting the crop of the sink pad */
280 	if (VIMC_IS_SRC(sel->pad) || sel->target != V4L2_SEL_TGT_CROP)
281 		return -EINVAL;
282 
283 	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE && vsca->src_frame)
284 		return -EBUSY;
285 
286 	crop_rect = vimc_sca_pad_crop(vsca, sd_state, sel->which);
287 	sink_fmt = vimc_sca_pad_format(vsca, sd_state, VIMC_SCA_SINK,
288 				       sel->which);
289 	vimc_sca_adjust_sink_crop(&sel->r, sink_fmt);
290 	*crop_rect = sel->r;
291 
292 	return 0;
293 }
294 
295 static const struct v4l2_subdev_pad_ops vimc_sca_pad_ops = {
296 	.init_cfg		= vimc_sca_init_cfg,
297 	.enum_mbus_code		= vimc_sca_enum_mbus_code,
298 	.enum_frame_size	= vimc_sca_enum_frame_size,
299 	.get_fmt		= vimc_sca_get_fmt,
300 	.set_fmt		= vimc_sca_set_fmt,
301 	.get_selection		= vimc_sca_get_selection,
302 	.set_selection		= vimc_sca_set_selection,
303 };
304 
vimc_sca_s_stream(struct v4l2_subdev * sd,int enable)305 static int vimc_sca_s_stream(struct v4l2_subdev *sd, int enable)
306 {
307 	struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd);
308 
309 	if (enable) {
310 		const struct vimc_pix_map *vpix;
311 		unsigned int frame_size;
312 
313 		if (vsca->src_frame)
314 			return 0;
315 
316 		/* Save the bytes per pixel of the sink */
317 		vpix = vimc_pix_map_by_code(vsca->fmt[VIMC_SCA_SINK].code);
318 		vsca->bpp = vpix->bpp;
319 
320 		/* Calculate the frame size of the source pad */
321 		frame_size = vsca->fmt[VIMC_SCA_SRC].width
322 			   * vsca->fmt[VIMC_SCA_SRC].height * vsca->bpp;
323 
324 		/* Allocate the frame buffer. Use vmalloc to be able to
325 		 * allocate a large amount of memory
326 		 */
327 		vsca->src_frame = vmalloc(frame_size);
328 		if (!vsca->src_frame)
329 			return -ENOMEM;
330 
331 	} else {
332 		if (!vsca->src_frame)
333 			return 0;
334 
335 		vfree(vsca->src_frame);
336 		vsca->src_frame = NULL;
337 	}
338 
339 	return 0;
340 }
341 
342 static const struct v4l2_subdev_video_ops vimc_sca_video_ops = {
343 	.s_stream = vimc_sca_s_stream,
344 };
345 
346 static const struct v4l2_subdev_ops vimc_sca_ops = {
347 	.pad = &vimc_sca_pad_ops,
348 	.video = &vimc_sca_video_ops,
349 };
350 
vimc_sca_fill_src_frame(const struct vimc_sca_device * const vsca,const u8 * const sink_frame)351 static void vimc_sca_fill_src_frame(const struct vimc_sca_device *const vsca,
352 				    const u8 *const sink_frame)
353 {
354 	const struct v4l2_mbus_framefmt *src_fmt = &vsca->fmt[VIMC_SCA_SRC];
355 	const struct v4l2_rect *r = &vsca->crop_rect;
356 	unsigned int snk_width = vsca->fmt[VIMC_SCA_SINK].width;
357 	unsigned int src_x, src_y;
358 	u8 *walker = vsca->src_frame;
359 
360 	/* Set each pixel at the src_frame to its sink_frame equivalent */
361 	for (src_y = 0; src_y < src_fmt->height; src_y++) {
362 		unsigned int snk_y, y_offset;
363 
364 		snk_y = (src_y * r->height) / src_fmt->height + r->top;
365 		y_offset = snk_y * snk_width * vsca->bpp;
366 
367 		for (src_x = 0; src_x < src_fmt->width; src_x++) {
368 			unsigned int snk_x, x_offset, index;
369 
370 			snk_x = (src_x * r->width) / src_fmt->width + r->left;
371 			x_offset = snk_x * vsca->bpp;
372 			index = y_offset + x_offset;
373 			memcpy(walker, &sink_frame[index], vsca->bpp);
374 			walker += vsca->bpp;
375 		}
376 	}
377 }
378 
vimc_sca_process_frame(struct vimc_ent_device * ved,const void * sink_frame)379 static void *vimc_sca_process_frame(struct vimc_ent_device *ved,
380 				    const void *sink_frame)
381 {
382 	struct vimc_sca_device *vsca = container_of(ved, struct vimc_sca_device,
383 						    ved);
384 
385 	/* If the stream in this node is not active, just return */
386 	if (!vsca->src_frame)
387 		return ERR_PTR(-EINVAL);
388 
389 	vimc_sca_fill_src_frame(vsca, sink_frame);
390 
391 	return vsca->src_frame;
392 };
393 
vimc_sca_release(struct vimc_ent_device * ved)394 static void vimc_sca_release(struct vimc_ent_device *ved)
395 {
396 	struct vimc_sca_device *vsca =
397 		container_of(ved, struct vimc_sca_device, ved);
398 
399 	media_entity_cleanup(vsca->ved.ent);
400 	kfree(vsca);
401 }
402 
vimc_sca_add(struct vimc_device * vimc,const char * vcfg_name)403 static struct vimc_ent_device *vimc_sca_add(struct vimc_device *vimc,
404 					    const char *vcfg_name)
405 {
406 	struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;
407 	struct vimc_sca_device *vsca;
408 	int ret;
409 
410 	/* Allocate the vsca struct */
411 	vsca = kzalloc(sizeof(*vsca), GFP_KERNEL);
412 	if (!vsca)
413 		return ERR_PTR(-ENOMEM);
414 
415 	/* Initialize ved and sd */
416 	vsca->pads[VIMC_SCA_SINK].flags = MEDIA_PAD_FL_SINK;
417 	vsca->pads[VIMC_SCA_SRC].flags = MEDIA_PAD_FL_SOURCE;
418 
419 	ret = vimc_ent_sd_register(&vsca->ved, &vsca->sd, v4l2_dev,
420 				   vcfg_name,
421 				   MEDIA_ENT_F_PROC_VIDEO_SCALER, 2,
422 				   vsca->pads, &vimc_sca_ops);
423 	if (ret) {
424 		kfree(vsca);
425 		return ERR_PTR(ret);
426 	}
427 
428 	vsca->ved.process_frame = vimc_sca_process_frame;
429 	vsca->ved.dev = vimc->mdev.dev;
430 
431 	/* Initialize the frame format */
432 	vsca->fmt[VIMC_SCA_SINK] = fmt_default;
433 	vsca->fmt[VIMC_SCA_SRC] = fmt_default;
434 
435 	/* Initialize the crop selection */
436 	vsca->crop_rect = crop_rect_default;
437 
438 	return &vsca->ved;
439 }
440 
441 struct vimc_ent_type vimc_sca_type = {
442 	.add = vimc_sca_add,
443 	.release = vimc_sca_release
444 };
445