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(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(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(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, pad: format->pad,
164 which: 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, pad: format->pad, which: 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(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, which: 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, pad: VIMC_SCALER_SRC,
224 which: 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, which: sel->which);
246 break;
247 case V4L2_SEL_TGT_CROP_BOUNDS:
248 sink_fmt = vimc_scaler_pad_format(vscaler, sd_state, pad: VIMC_SCALER_SINK,
249 which: 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, min_size: &crop_rect_min);
267 v4l2_rect_map_inside(r, boundary: &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, which: sel->which);
286 sink_fmt = vimc_scaler_pad_format(vscaler, sd_state, pad: VIMC_SCALER_SINK,
287 which: sel->which);
288 vimc_scaler_adjust_sink_crop(r: &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(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(size: frame_size);
326 if (!vscaler->src_frame)
327 return -ENOMEM;
328
329 } else {
330 if (!vscaler->src_frame)
331 return 0;
332
333 vfree(addr: 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(error: -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(entity: vscaler->ved.ent);
402 kfree(objp: 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(size: sizeof(*vscaler), GFP_KERNEL);
414 if (!vscaler)
415 return ERR_PTR(error: -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(ved: &vscaler->ved, sd: &vscaler->sd, v4l2_dev,
422 name: vcfg_name,
423 MEDIA_ENT_F_PROC_VIDEO_SCALER, num_pads: 2,
424 pads: vscaler->pads, sd_ops: &vimc_scaler_ops);
425 if (ret) {
426 kfree(objp: vscaler);
427 return ERR_PTR(error: 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

source code of linux/drivers/media/test-drivers/vimc/vimc-scaler.c