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_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_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_get_try_format(&vscaler->sd, 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_get_try_crop(&vscaler->sd, sd_state,
153						VIMC_SCALER_SINK);
154	else
155		return &vscaler->crop_rect;
156}
157
158static int vimc_scaler_get_fmt(struct v4l2_subdev *sd,
159			    struct v4l2_subdev_state *sd_state,
160			    struct v4l2_subdev_format *format)
161{
162	struct vimc_scaler_device *vscaler = v4l2_get_subdevdata(sd);
163
164	format->format = *vimc_scaler_pad_format(vscaler, sd_state, format->pad,
165					      format->which);
166	return 0;
167}
168
169static int vimc_scaler_set_fmt(struct v4l2_subdev *sd,
170			    struct v4l2_subdev_state *sd_state,
171			    struct v4l2_subdev_format *format)
172{
173	struct vimc_scaler_device *vscaler = 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 && vscaler->src_frame)
178		return -EBUSY;
179
180	fmt = vimc_scaler_pad_format(vscaler, 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_SCALER_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_SCALER_SINK) {
215		struct v4l2_mbus_framefmt *src_fmt;
216		struct v4l2_rect *crop;
217
218		crop = vimc_scaler_pad_crop(vscaler, 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_scaler_pad_format(vscaler, sd_state, VIMC_SCALER_SRC,
225					      format->which);
226		*src_fmt = *fmt;
227	}
228
229	format->format = *fmt;
230
231	return 0;
232}
233
234static int vimc_scaler_get_selection(struct v4l2_subdev *sd,
235				  struct v4l2_subdev_state *sd_state,
236				  struct v4l2_subdev_selection *sel)
237{
238	struct vimc_scaler_device *vscaler = 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_scaler_pad_crop(vscaler, sd_state, sel->which);
247		break;
248	case V4L2_SEL_TGT_CROP_BOUNDS:
249		sink_fmt = vimc_scaler_pad_format(vscaler, sd_state, VIMC_SCALER_SINK,
250					       sel->which);
251		sel->r = vimc_scaler_get_crop_bound_sink(sink_fmt);
252		break;
253	default:
254		return -EINVAL;
255	}
256
257	return 0;
258}
259
260static void vimc_scaler_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_scaler_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
271static int vimc_scaler_set_selection(struct v4l2_subdev *sd,
272				  struct v4l2_subdev_state *sd_state,
273				  struct v4l2_subdev_selection *sel)
274{
275	struct vimc_scaler_device *vscaler = 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 && vscaler->src_frame)
284		return -EBUSY;
285
286	crop_rect = vimc_scaler_pad_crop(vscaler, sd_state, sel->which);
287	sink_fmt = vimc_scaler_pad_format(vscaler, sd_state, VIMC_SCALER_SINK,
288				       sel->which);
289	vimc_scaler_adjust_sink_crop(&sel->r, sink_fmt);
290	*crop_rect = sel->r;
291
292	return 0;
293}
294
295static const struct v4l2_subdev_pad_ops vimc_scaler_pad_ops = {
296	.init_cfg		= vimc_scaler_init_cfg,
297	.enum_mbus_code		= vimc_scaler_enum_mbus_code,
298	.enum_frame_size	= vimc_scaler_enum_frame_size,
299	.get_fmt		= vimc_scaler_get_fmt,
300	.set_fmt		= vimc_scaler_set_fmt,
301	.get_selection		= vimc_scaler_get_selection,
302	.set_selection		= vimc_scaler_set_selection,
303};
304
305static int vimc_scaler_s_stream(struct v4l2_subdev *sd, int enable)
306{
307	struct vimc_scaler_device *vscaler = v4l2_get_subdevdata(sd);
308
309	if (enable) {
310		const struct vimc_pix_map *vpix;
311		unsigned int frame_size;
312
313		if (vscaler->src_frame)
314			return 0;
315
316		/* Save the bytes per pixel of the sink */
317		vpix = vimc_pix_map_by_code(vscaler->fmt[VIMC_SCALER_SINK].code);
318		vscaler->bpp = vpix->bpp;
319
320		/* Calculate the frame size of the source pad */
321		frame_size = vscaler->fmt[VIMC_SCALER_SRC].width
322			   * vscaler->fmt[VIMC_SCALER_SRC].height * vscaler->bpp;
323
324		/* Allocate the frame buffer. Use vmalloc to be able to
325		 * allocate a large amount of memory
326		 */
327		vscaler->src_frame = vmalloc(frame_size);
328		if (!vscaler->src_frame)
329			return -ENOMEM;
330
331	} else {
332		if (!vscaler->src_frame)
333			return 0;
334
335		vfree(vscaler->src_frame);
336		vscaler->src_frame = NULL;
337	}
338
339	return 0;
340}
341
342static const struct v4l2_subdev_video_ops vimc_scaler_video_ops = {
343	.s_stream = vimc_scaler_s_stream,
344};
345
346static const struct v4l2_subdev_ops vimc_scaler_ops = {
347	.pad = &vimc_scaler_pad_ops,
348	.video = &vimc_scaler_video_ops,
349};
350
351static void vimc_scaler_fill_src_frame(const struct vimc_scaler_device *const vscaler,
352				    const u8 *const sink_frame)
353{
354	const struct v4l2_mbus_framefmt *src_fmt = &vscaler->fmt[VIMC_SCALER_SRC];
355	const struct v4l2_rect *r = &vscaler->crop_rect;
356	unsigned int snk_width = vscaler->fmt[VIMC_SCALER_SINK].width;
357	unsigned int src_x, src_y;
358	u8 *walker = vscaler->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 * vscaler->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 * vscaler->bpp;
372			index = y_offset + x_offset;
373			memcpy(walker, &sink_frame[index], vscaler->bpp);
374			walker += vscaler->bpp;
375		}
376	}
377}
378
379static void *vimc_scaler_process_frame(struct vimc_ent_device *ved,
380				    const void *sink_frame)
381{
382	struct vimc_scaler_device *vscaler = container_of(ved, struct vimc_scaler_device,
383						    ved);
384
385	/* If the stream in this node is not active, just return */
386	if (!vscaler->src_frame)
387		return ERR_PTR(-EINVAL);
388
389	vimc_scaler_fill_src_frame(vscaler, sink_frame);
390
391	return vscaler->src_frame;
392};
393
394static void vimc_scaler_release(struct vimc_ent_device *ved)
395{
396	struct vimc_scaler_device *vscaler =
397		container_of(ved, struct vimc_scaler_device, ved);
398
399	media_entity_cleanup(vscaler->ved.ent);
400	kfree(vscaler);
401}
402
403static struct vimc_ent_device *vimc_scaler_add(struct vimc_device *vimc,
404					    const char *vcfg_name)
405{
406	struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;
407	struct vimc_scaler_device *vscaler;
408	int ret;
409
410	/* Allocate the vscaler struct */
411	vscaler = kzalloc(sizeof(*vscaler), GFP_KERNEL);
412	if (!vscaler)
413		return ERR_PTR(-ENOMEM);
414
415	/* Initialize ved and sd */
416	vscaler->pads[VIMC_SCALER_SINK].flags = MEDIA_PAD_FL_SINK;
417	vscaler->pads[VIMC_SCALER_SRC].flags = MEDIA_PAD_FL_SOURCE;
418
419	ret = vimc_ent_sd_register(&vscaler->ved, &vscaler->sd, v4l2_dev,
420				   vcfg_name,
421				   MEDIA_ENT_F_PROC_VIDEO_SCALER, 2,
422				   vscaler->pads, &vimc_scaler_ops);
423	if (ret) {
424		kfree(vscaler);
425		return ERR_PTR(ret);
426	}
427
428	vscaler->ved.process_frame = vimc_scaler_process_frame;
429	vscaler->ved.dev = vimc->mdev.dev;
430
431	/* Initialize the frame format */
432	vscaler->fmt[VIMC_SCALER_SINK] = fmt_default;
433	vscaler->fmt[VIMC_SCALER_SRC] = fmt_default;
434
435	/* Initialize the crop selection */
436	vscaler->crop_rect = crop_rect_default;
437
438	return &vscaler->ved;
439}
440
441struct vimc_ent_type vimc_scaler_type = {
442	.add = vimc_scaler_add,
443	.release = vimc_scaler_release
444};
445