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 */
18enum vic_sca_pad {
19	VIMC_SCALER_SINK = 0,
20	VIMC_SCALER_SRC = 1,
21};
22
23#define VIMC_SCALER_FMT_WIDTH_DEFAULT  640
24#define VIMC_SCALER_FMT_HEIGHT_DEFAULT 480
25
26struct vimc_scaler_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
38static const struct v4l2_mbus_framefmt fmt_default = {
39	.width = VIMC_SCALER_FMT_WIDTH_DEFAULT,
40	.height = VIMC_SCALER_FMT_HEIGHT_DEFAULT,
41	.code = MEDIA_BUS_FMT_RGB888_1X24,
42	.field = V4L2_FIELD_NONE,
43	.colorspace = V4L2_COLORSPACE_SRGB,
44};
45
46static const struct v4l2_rect crop_rect_default = {
47	.width = VIMC_SCALER_FMT_WIDTH_DEFAULT,
48	.height = VIMC_SCALER_FMT_HEIGHT_DEFAULT,
49	.top = 0,
50	.left = 0,
51};
52
53static 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
60static struct v4l2_rect
61vimc_scaler_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
73static int vimc_scaler_init_state(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_state_get_format(sd_state, i);
82		*mf = fmt_default;
83	}
84
85	r = v4l2_subdev_state_get_crop(sd_state, VIMC_SCALER_SINK);
86	*r = crop_rect_default;
87
88	return 0;
89}
90
91static int vimc_scaler_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
112static int vimc_scaler_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
135static struct v4l2_mbus_framefmt *
136vimc_scaler_pad_format(struct vimc_scaler_device *vscaler,
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_state_get_format(sd_state, pad);
142	else
143		return &vscaler->fmt[pad];
144}
145
146static struct v4l2_rect *
147vimc_scaler_pad_crop(struct vimc_scaler_device *vscaler,
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_state_get_crop(sd_state, VIMC_SCALER_SINK);
153	else
154		return &vscaler->crop_rect;
155}
156
157static int vimc_scaler_get_fmt(struct v4l2_subdev *sd,
158			    struct v4l2_subdev_state *sd_state,
159			    struct v4l2_subdev_format *format)
160{
161	struct vimc_scaler_device *vscaler = v4l2_get_subdevdata(sd);
162
163	format->format = *vimc_scaler_pad_format(vscaler, sd_state, format->pad,
164					      format->which);
165	return 0;
166}
167
168static int vimc_scaler_set_fmt(struct v4l2_subdev *sd,
169			    struct v4l2_subdev_state *sd_state,
170			    struct v4l2_subdev_format *format)
171{
172	struct vimc_scaler_device *vscaler = v4l2_get_subdevdata(sd);
173	struct v4l2_mbus_framefmt *fmt;
174
175	/* Do not change the active format while stream is on */
176	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE && vscaler->src_frame)
177		return -EBUSY;
178
179	fmt = vimc_scaler_pad_format(vscaler, sd_state, format->pad, format->which);
180
181	/*
182	 * The media bus code and colorspace can only be changed on the sink
183	 * pad, the source pad only follows.
184	 */
185	if (format->pad == VIMC_SCALER_SINK) {
186		const struct vimc_pix_map *vpix;
187
188		/* Only accept code in the pix map table in non bayer format. */
189		vpix = vimc_pix_map_by_code(format->format.code);
190		if (vpix && !vpix->bayer)
191			fmt->code = format->format.code;
192		else
193			fmt->code = fmt_default.code;
194
195		/* Clamp the colorspace to valid values. */
196		fmt->colorspace = format->format.colorspace;
197		fmt->ycbcr_enc = format->format.ycbcr_enc;
198		fmt->quantization = format->format.quantization;
199		fmt->xfer_func = format->format.xfer_func;
200		vimc_colorimetry_clamp(fmt);
201	}
202
203	/* Clamp and align the width and height */
204	fmt->width = clamp_t(u32, format->format.width, VIMC_FRAME_MIN_WIDTH,
205			     VIMC_FRAME_MAX_WIDTH) & ~1;
206	fmt->height = clamp_t(u32, format->format.height, VIMC_FRAME_MIN_HEIGHT,
207			      VIMC_FRAME_MAX_HEIGHT) & ~1;
208
209	/*
210	 * Propagate the sink pad format to the crop rectangle and the source
211	 * pad.
212	 */
213	if (format->pad == VIMC_SCALER_SINK) {
214		struct v4l2_mbus_framefmt *src_fmt;
215		struct v4l2_rect *crop;
216
217		crop = vimc_scaler_pad_crop(vscaler, sd_state, format->which);
218		crop->width = fmt->width;
219		crop->height = fmt->height;
220		crop->top = 0;
221		crop->left = 0;
222
223		src_fmt = vimc_scaler_pad_format(vscaler, sd_state, VIMC_SCALER_SRC,
224					      format->which);
225		*src_fmt = *fmt;
226	}
227
228	format->format = *fmt;
229
230	return 0;
231}
232
233static int vimc_scaler_get_selection(struct v4l2_subdev *sd,
234				  struct v4l2_subdev_state *sd_state,
235				  struct v4l2_subdev_selection *sel)
236{
237	struct vimc_scaler_device *vscaler = v4l2_get_subdevdata(sd);
238	struct v4l2_mbus_framefmt *sink_fmt;
239
240	if (VIMC_IS_SRC(sel->pad))
241		return -EINVAL;
242
243	switch (sel->target) {
244	case V4L2_SEL_TGT_CROP:
245		sel->r = *vimc_scaler_pad_crop(vscaler, sd_state, sel->which);
246		break;
247	case V4L2_SEL_TGT_CROP_BOUNDS:
248		sink_fmt = vimc_scaler_pad_format(vscaler, sd_state, VIMC_SCALER_SINK,
249					       sel->which);
250		sel->r = vimc_scaler_get_crop_bound_sink(sink_fmt);
251		break;
252	default:
253		return -EINVAL;
254	}
255
256	return 0;
257}
258
259static void vimc_scaler_adjust_sink_crop(struct v4l2_rect *r,
260				      const struct v4l2_mbus_framefmt *sink_fmt)
261{
262	const struct v4l2_rect sink_rect =
263		vimc_scaler_get_crop_bound_sink(sink_fmt);
264
265	/* Disallow rectangles smaller than the minimal one. */
266	v4l2_rect_set_min_size(r, &crop_rect_min);
267	v4l2_rect_map_inside(r, &sink_rect);
268}
269
270static int vimc_scaler_set_selection(struct v4l2_subdev *sd,
271				  struct v4l2_subdev_state *sd_state,
272				  struct v4l2_subdev_selection *sel)
273{
274	struct vimc_scaler_device *vscaler = v4l2_get_subdevdata(sd);
275	struct v4l2_mbus_framefmt *sink_fmt;
276	struct v4l2_rect *crop_rect;
277
278	/* Only support setting the crop of the sink pad */
279	if (VIMC_IS_SRC(sel->pad) || sel->target != V4L2_SEL_TGT_CROP)
280		return -EINVAL;
281
282	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE && vscaler->src_frame)
283		return -EBUSY;
284
285	crop_rect = vimc_scaler_pad_crop(vscaler, sd_state, sel->which);
286	sink_fmt = vimc_scaler_pad_format(vscaler, sd_state, VIMC_SCALER_SINK,
287				       sel->which);
288	vimc_scaler_adjust_sink_crop(&sel->r, sink_fmt);
289	*crop_rect = sel->r;
290
291	return 0;
292}
293
294static const struct v4l2_subdev_pad_ops vimc_scaler_pad_ops = {
295	.enum_mbus_code		= vimc_scaler_enum_mbus_code,
296	.enum_frame_size	= vimc_scaler_enum_frame_size,
297	.get_fmt		= vimc_scaler_get_fmt,
298	.set_fmt		= vimc_scaler_set_fmt,
299	.get_selection		= vimc_scaler_get_selection,
300	.set_selection		= vimc_scaler_set_selection,
301};
302
303static int vimc_scaler_s_stream(struct v4l2_subdev *sd, int enable)
304{
305	struct vimc_scaler_device *vscaler = v4l2_get_subdevdata(sd);
306
307	if (enable) {
308		const struct vimc_pix_map *vpix;
309		unsigned int frame_size;
310
311		if (vscaler->src_frame)
312			return 0;
313
314		/* Save the bytes per pixel of the sink */
315		vpix = vimc_pix_map_by_code(vscaler->fmt[VIMC_SCALER_SINK].code);
316		vscaler->bpp = vpix->bpp;
317
318		/* Calculate the frame size of the source pad */
319		frame_size = vscaler->fmt[VIMC_SCALER_SRC].width
320			   * vscaler->fmt[VIMC_SCALER_SRC].height * vscaler->bpp;
321
322		/* Allocate the frame buffer. Use vmalloc to be able to
323		 * allocate a large amount of memory
324		 */
325		vscaler->src_frame = vmalloc(frame_size);
326		if (!vscaler->src_frame)
327			return -ENOMEM;
328
329	} else {
330		if (!vscaler->src_frame)
331			return 0;
332
333		vfree(vscaler->src_frame);
334		vscaler->src_frame = NULL;
335	}
336
337	return 0;
338}
339
340static const struct v4l2_subdev_video_ops vimc_scaler_video_ops = {
341	.s_stream = vimc_scaler_s_stream,
342};
343
344static const struct v4l2_subdev_ops vimc_scaler_ops = {
345	.pad = &vimc_scaler_pad_ops,
346	.video = &vimc_scaler_video_ops,
347};
348
349static const struct v4l2_subdev_internal_ops vimc_scaler_internal_ops = {
350	.init_state = vimc_scaler_init_state,
351};
352
353static void vimc_scaler_fill_src_frame(const struct vimc_scaler_device *const vscaler,
354				    const u8 *const sink_frame)
355{
356	const struct v4l2_mbus_framefmt *src_fmt = &vscaler->fmt[VIMC_SCALER_SRC];
357	const struct v4l2_rect *r = &vscaler->crop_rect;
358	unsigned int snk_width = vscaler->fmt[VIMC_SCALER_SINK].width;
359	unsigned int src_x, src_y;
360	u8 *walker = vscaler->src_frame;
361
362	/* Set each pixel at the src_frame to its sink_frame equivalent */
363	for (src_y = 0; src_y < src_fmt->height; src_y++) {
364		unsigned int snk_y, y_offset;
365
366		snk_y = (src_y * r->height) / src_fmt->height + r->top;
367		y_offset = snk_y * snk_width * vscaler->bpp;
368
369		for (src_x = 0; src_x < src_fmt->width; src_x++) {
370			unsigned int snk_x, x_offset, index;
371
372			snk_x = (src_x * r->width) / src_fmt->width + r->left;
373			x_offset = snk_x * vscaler->bpp;
374			index = y_offset + x_offset;
375			memcpy(walker, &sink_frame[index], vscaler->bpp);
376			walker += vscaler->bpp;
377		}
378	}
379}
380
381static void *vimc_scaler_process_frame(struct vimc_ent_device *ved,
382				    const void *sink_frame)
383{
384	struct vimc_scaler_device *vscaler = container_of(ved, struct vimc_scaler_device,
385						    ved);
386
387	/* If the stream in this node is not active, just return */
388	if (!vscaler->src_frame)
389		return ERR_PTR(-EINVAL);
390
391	vimc_scaler_fill_src_frame(vscaler, sink_frame);
392
393	return vscaler->src_frame;
394};
395
396static void vimc_scaler_release(struct vimc_ent_device *ved)
397{
398	struct vimc_scaler_device *vscaler =
399		container_of(ved, struct vimc_scaler_device, ved);
400
401	media_entity_cleanup(vscaler->ved.ent);
402	kfree(vscaler);
403}
404
405static struct vimc_ent_device *vimc_scaler_add(struct vimc_device *vimc,
406					    const char *vcfg_name)
407{
408	struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;
409	struct vimc_scaler_device *vscaler;
410	int ret;
411
412	/* Allocate the vscaler struct */
413	vscaler = kzalloc(sizeof(*vscaler), GFP_KERNEL);
414	if (!vscaler)
415		return ERR_PTR(-ENOMEM);
416
417	/* Initialize ved and sd */
418	vscaler->pads[VIMC_SCALER_SINK].flags = MEDIA_PAD_FL_SINK;
419	vscaler->pads[VIMC_SCALER_SRC].flags = MEDIA_PAD_FL_SOURCE;
420
421	ret = vimc_ent_sd_register(&vscaler->ved, &vscaler->sd, v4l2_dev,
422				   vcfg_name,
423				   MEDIA_ENT_F_PROC_VIDEO_SCALER, 2,
424				   vscaler->pads, &vimc_scaler_ops);
425	if (ret) {
426		kfree(vscaler);
427		return ERR_PTR(ret);
428	}
429
430	vscaler->sd.internal_ops = &vimc_scaler_internal_ops;
431
432	vscaler->ved.process_frame = vimc_scaler_process_frame;
433	vscaler->ved.dev = vimc->mdev.dev;
434
435	/* Initialize the frame format */
436	vscaler->fmt[VIMC_SCALER_SINK] = fmt_default;
437	vscaler->fmt[VIMC_SCALER_SRC] = fmt_default;
438
439	/* Initialize the crop selection */
440	vscaler->crop_rect = crop_rect_default;
441
442	return &vscaler->ved;
443}
444
445struct vimc_ent_type vimc_scaler_type = {
446	.add = vimc_scaler_add,
447	.release = vimc_scaler_release
448};
449