vimc-scaler.c

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