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_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 | |
26 | struct 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 | |
38 | static 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 | |
46 | static 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 | |
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 |
61 | vimc_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 | |
73 | static 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 | |
91 | static 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 | |
112 | static 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 | |
135 | static struct v4l2_mbus_framefmt * |
136 | vimc_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 | |
146 | static struct v4l2_rect * |
147 | vimc_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 | |
157 | static 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 | |
168 | static 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 | |
233 | static 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 | |
259 | static 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 | |
270 | static 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 | |
294 | static 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 | |
303 | static 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 | |
340 | static const struct v4l2_subdev_video_ops vimc_scaler_video_ops = { |
341 | .s_stream = vimc_scaler_s_stream, |
342 | }; |
343 | |
344 | static const struct v4l2_subdev_ops vimc_scaler_ops = { |
345 | .pad = &vimc_scaler_pad_ops, |
346 | .video = &vimc_scaler_video_ops, |
347 | }; |
348 | |
349 | static const struct v4l2_subdev_internal_ops vimc_scaler_internal_ops = { |
350 | .init_state = vimc_scaler_init_state, |
351 | }; |
352 | |
353 | static 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 | |
381 | static 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 | |
396 | static 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 | |
405 | static 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 | |
445 | struct vimc_ent_type vimc_scaler_type = { |
446 | .add = vimc_scaler_add, |
447 | .release = vimc_scaler_release |
448 | }; |
449 | |