2 * vimc-scaler.c Virtual Media Controller Driver
4 * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
6 * This program is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
11 * This program is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
18 #include <linux/component.h>
19 #include <linux/module.h>
20 #include <linux/mod_devicetable.h>
21 #include <linux/platform_device.h>
22 #include <linux/vmalloc.h>
23 #include <linux/v4l2-mediabus.h>
24 #include <media/v4l2-subdev.h>
26 #include "vimc-common.h"
28 #define VIMC_SCA_DRV_NAME "vimc-scaler"
30 static unsigned int sca_mult
= 3;
31 module_param(sca_mult
, uint
, 0000);
32 MODULE_PARM_DESC(sca_mult
, " the image size multiplier");
34 #define IS_SINK(pad) (!pad)
35 #define IS_SRC(pad) (pad)
38 static const u32 vimc_sca_supported_pixfmt
[] = {
44 struct vimc_sca_device
{
45 struct vimc_ent_device ved
;
46 struct v4l2_subdev sd
;
48 /* NOTE: the source fmt is the same as the sink
49 * with the width and hight multiplied by mult
51 struct v4l2_mbus_framefmt sink_fmt
;
52 /* Values calculated when the stream starts */
54 unsigned int src_line_size
;
58 static const struct v4l2_mbus_framefmt sink_fmt_default
= {
61 .code
= MEDIA_BUS_FMT_RGB888_1X24
,
62 .field
= V4L2_FIELD_NONE
,
63 .colorspace
= V4L2_COLORSPACE_DEFAULT
,
66 static bool vimc_sca_is_pixfmt_supported(u32 pixelformat
)
70 for (i
= 0; i
< ARRAY_SIZE(vimc_sca_supported_pixfmt
); i
++)
71 if (vimc_sca_supported_pixfmt
[i
] == pixelformat
)
76 static int vimc_sca_init_cfg(struct v4l2_subdev
*sd
,
77 struct v4l2_subdev_pad_config
*cfg
)
79 struct v4l2_mbus_framefmt
*mf
;
82 mf
= v4l2_subdev_get_try_format(sd
, cfg
, 0);
83 *mf
= sink_fmt_default
;
85 for (i
= 1; i
< sd
->entity
.num_pads
; i
++) {
86 mf
= v4l2_subdev_get_try_format(sd
, cfg
, i
);
87 *mf
= sink_fmt_default
;
88 mf
->width
= mf
->width
* sca_mult
;
89 mf
->height
= mf
->height
* sca_mult
;
95 static int vimc_sca_enum_frame_size(struct v4l2_subdev
*sd
,
96 struct v4l2_subdev_pad_config
*cfg
,
97 struct v4l2_subdev_frame_size_enum
*fse
)
102 fse
->min_width
= VIMC_FRAME_MIN_WIDTH
;
103 fse
->min_height
= VIMC_FRAME_MIN_HEIGHT
;
105 if (IS_SINK(fse
->pad
)) {
106 fse
->max_width
= VIMC_FRAME_MAX_WIDTH
;
107 fse
->max_height
= VIMC_FRAME_MAX_HEIGHT
;
109 fse
->max_width
= VIMC_FRAME_MAX_WIDTH
* MAX_ZOOM
;
110 fse
->max_height
= VIMC_FRAME_MAX_HEIGHT
* MAX_ZOOM
;
116 static int vimc_sca_get_fmt(struct v4l2_subdev
*sd
,
117 struct v4l2_subdev_pad_config
*cfg
,
118 struct v4l2_subdev_format
*format
)
120 struct vimc_sca_device
*vsca
= v4l2_get_subdevdata(sd
);
122 /* Get the current sink format */
123 format
->format
= (format
->which
== V4L2_SUBDEV_FORMAT_TRY
) ?
124 *v4l2_subdev_get_try_format(sd
, cfg
, 0) :
127 /* Scale the frame size for the source pad */
128 if (IS_SRC(format
->pad
)) {
129 format
->format
.width
= vsca
->sink_fmt
.width
* sca_mult
;
130 format
->format
.height
= vsca
->sink_fmt
.height
* sca_mult
;
136 static void vimc_sca_adjust_sink_fmt(struct v4l2_mbus_framefmt
*fmt
)
138 fmt
->width
= clamp_t(u32
, fmt
->width
, VIMC_FRAME_MIN_WIDTH
,
139 VIMC_FRAME_MAX_WIDTH
) & ~1;
140 fmt
->height
= clamp_t(u32
, fmt
->height
, VIMC_FRAME_MIN_HEIGHT
,
141 VIMC_FRAME_MAX_HEIGHT
) & ~1;
143 if (fmt
->field
== V4L2_FIELD_ANY
)
144 fmt
->field
= sink_fmt_default
.field
;
146 vimc_colorimetry_clamp(fmt
);
149 static int vimc_sca_set_fmt(struct v4l2_subdev
*sd
,
150 struct v4l2_subdev_pad_config
*cfg
,
151 struct v4l2_subdev_format
*fmt
)
153 struct vimc_sca_device
*vsca
= v4l2_get_subdevdata(sd
);
154 struct v4l2_mbus_framefmt
*sink_fmt
;
156 if (!vimc_mbus_code_supported(fmt
->format
.code
))
157 fmt
->format
.code
= sink_fmt_default
.code
;
159 if (fmt
->which
== V4L2_SUBDEV_FORMAT_ACTIVE
) {
160 /* Do not change the format while stream is on */
164 sink_fmt
= &vsca
->sink_fmt
;
166 sink_fmt
= v4l2_subdev_get_try_format(sd
, cfg
, 0);
170 * Do not change the format of the source pad,
171 * it is propagated from the sink
173 if (IS_SRC(fmt
->pad
)) {
174 fmt
->format
= *sink_fmt
;
175 fmt
->format
.width
= sink_fmt
->width
* sca_mult
;
176 fmt
->format
.height
= sink_fmt
->height
* sca_mult
;
178 /* Set the new format in the sink pad */
179 vimc_sca_adjust_sink_fmt(&fmt
->format
);
181 dev_dbg(vsca
->dev
, "%s: sink format update: "
182 "old:%dx%d (0x%x, %d, %d, %d, %d) "
183 "new:%dx%d (0x%x, %d, %d, %d, %d)\n", vsca
->sd
.name
,
185 sink_fmt
->width
, sink_fmt
->height
, sink_fmt
->code
,
186 sink_fmt
->colorspace
, sink_fmt
->quantization
,
187 sink_fmt
->xfer_func
, sink_fmt
->ycbcr_enc
,
189 fmt
->format
.width
, fmt
->format
.height
, fmt
->format
.code
,
190 fmt
->format
.colorspace
, fmt
->format
.quantization
,
191 fmt
->format
.xfer_func
, fmt
->format
.ycbcr_enc
);
193 *sink_fmt
= fmt
->format
;
199 static const struct v4l2_subdev_pad_ops vimc_sca_pad_ops
= {
200 .init_cfg
= vimc_sca_init_cfg
,
201 .enum_mbus_code
= vimc_enum_mbus_code
,
202 .enum_frame_size
= vimc_sca_enum_frame_size
,
203 .get_fmt
= vimc_sca_get_fmt
,
204 .set_fmt
= vimc_sca_set_fmt
,
207 static int vimc_sca_s_stream(struct v4l2_subdev
*sd
, int enable
)
209 struct vimc_sca_device
*vsca
= v4l2_get_subdevdata(sd
);
212 u32 pixelformat
= vsca
->ved
.stream
->producer_pixfmt
;
213 const struct v4l2_format_info
*pix_info
;
214 unsigned int frame_size
;
219 if (!vimc_sca_is_pixfmt_supported(pixelformat
)) {
220 dev_err(vsca
->dev
, "pixfmt (0x%08x) is not supported\n",
225 /* Save the bytes per pixel of the sink */
226 pix_info
= v4l2_format_info(pixelformat
);
227 vsca
->bpp
= pix_info
->bpp
[0];
229 /* Calculate the width in bytes of the src frame */
230 vsca
->src_line_size
= vsca
->sink_fmt
.width
*
231 sca_mult
* vsca
->bpp
;
233 /* Calculate the frame size of the source pad */
234 frame_size
= vsca
->src_line_size
* vsca
->sink_fmt
.height
*
237 /* Allocate the frame buffer. Use vmalloc to be able to
238 * allocate a large amount of memory
240 vsca
->src_frame
= vmalloc(frame_size
);
241 if (!vsca
->src_frame
)
245 if (!vsca
->src_frame
)
248 vfree(vsca
->src_frame
);
249 vsca
->src_frame
= NULL
;
255 static const struct v4l2_subdev_video_ops vimc_sca_video_ops
= {
256 .s_stream
= vimc_sca_s_stream
,
259 static const struct v4l2_subdev_ops vimc_sca_ops
= {
260 .pad
= &vimc_sca_pad_ops
,
261 .video
= &vimc_sca_video_ops
,
264 static void vimc_sca_fill_pix(u8
*const ptr
,
265 const u8
*const pixel
,
266 const unsigned int bpp
)
270 /* copy the pixel to the pointer */
271 for (i
= 0; i
< bpp
; i
++)
275 static void vimc_sca_scale_pix(const struct vimc_sca_device
*const vsca
,
276 const unsigned int lin
, const unsigned int col
,
277 const u8
*const sink_frame
)
279 unsigned int i
, j
, index
;
282 /* Point to the pixel value in position (lin, col) in the sink frame */
283 index
= VIMC_FRAME_INDEX(lin
, col
,
284 vsca
->sink_fmt
.width
,
286 pixel
= &sink_frame
[index
];
289 "sca: %s: --- scale_pix sink pos %dx%d, index %d ---\n",
290 vsca
->sd
.name
, lin
, col
, index
);
292 /* point to the place we are going to put the first pixel
293 * in the scaled src frame
295 index
= VIMC_FRAME_INDEX(lin
* sca_mult
, col
* sca_mult
,
296 vsca
->sink_fmt
.width
* sca_mult
, vsca
->bpp
);
298 dev_dbg(vsca
->dev
, "sca: %s: scale_pix src pos %dx%d, index %d\n",
299 vsca
->sd
.name
, lin
* sca_mult
, col
* sca_mult
, index
);
301 /* Repeat this pixel mult times */
302 for (i
= 0; i
< sca_mult
; i
++) {
303 /* Iterate through each beginning of a
304 * pixel repetition in a line
306 for (j
= 0; j
< sca_mult
* vsca
->bpp
; j
+= vsca
->bpp
) {
308 "sca: %s: sca: scale_pix src pos %d\n",
309 vsca
->sd
.name
, index
+ j
);
311 /* copy the pixel to the position index + j */
312 vimc_sca_fill_pix(&vsca
->src_frame
[index
+ j
],
316 /* move the index to the next line */
317 index
+= vsca
->src_line_size
;
321 static void vimc_sca_fill_src_frame(const struct vimc_sca_device
*const vsca
,
322 const u8
*const sink_frame
)
326 /* Scale each pixel from the original sink frame */
327 /* TODO: implement scale down, only scale up is supported for now */
328 for (i
= 0; i
< vsca
->sink_fmt
.height
; i
++)
329 for (j
= 0; j
< vsca
->sink_fmt
.width
; j
++)
330 vimc_sca_scale_pix(vsca
, i
, j
, sink_frame
);
333 static void *vimc_sca_process_frame(struct vimc_ent_device
*ved
,
334 const void *sink_frame
)
336 struct vimc_sca_device
*vsca
= container_of(ved
, struct vimc_sca_device
,
339 /* If the stream in this node is not active, just return */
340 if (!vsca
->src_frame
)
341 return ERR_PTR(-EINVAL
);
343 vimc_sca_fill_src_frame(vsca
, sink_frame
);
345 return vsca
->src_frame
;
348 static void vimc_sca_release(struct v4l2_subdev
*sd
)
350 struct vimc_sca_device
*vsca
=
351 container_of(sd
, struct vimc_sca_device
, sd
);
356 static const struct v4l2_subdev_internal_ops vimc_sca_int_ops
= {
357 .release
= vimc_sca_release
,
360 static void vimc_sca_comp_unbind(struct device
*comp
, struct device
*master
,
363 struct vimc_ent_device
*ved
= dev_get_drvdata(comp
);
364 struct vimc_sca_device
*vsca
= container_of(ved
, struct vimc_sca_device
,
367 vimc_ent_sd_unregister(ved
, &vsca
->sd
);
371 static int vimc_sca_comp_bind(struct device
*comp
, struct device
*master
,
374 struct v4l2_device
*v4l2_dev
= master_data
;
375 struct vimc_platform_data
*pdata
= comp
->platform_data
;
376 struct vimc_sca_device
*vsca
;
379 /* Allocate the vsca struct */
380 vsca
= kzalloc(sizeof(*vsca
), GFP_KERNEL
);
384 /* Initialize ved and sd */
385 ret
= vimc_ent_sd_register(&vsca
->ved
, &vsca
->sd
, v4l2_dev
,
387 MEDIA_ENT_F_PROC_VIDEO_SCALER
, 2,
388 (const unsigned long[2]) {MEDIA_PAD_FL_SINK
,
389 MEDIA_PAD_FL_SOURCE
},
390 &vimc_sca_int_ops
, &vimc_sca_ops
);
396 vsca
->ved
.process_frame
= vimc_sca_process_frame
;
397 dev_set_drvdata(comp
, &vsca
->ved
);
400 /* Initialize the frame format */
401 vsca
->sink_fmt
= sink_fmt_default
;
406 static const struct component_ops vimc_sca_comp_ops
= {
407 .bind
= vimc_sca_comp_bind
,
408 .unbind
= vimc_sca_comp_unbind
,
411 static int vimc_sca_probe(struct platform_device
*pdev
)
413 return component_add(&pdev
->dev
, &vimc_sca_comp_ops
);
416 static int vimc_sca_remove(struct platform_device
*pdev
)
418 component_del(&pdev
->dev
, &vimc_sca_comp_ops
);
423 static const struct platform_device_id vimc_sca_driver_ids
[] = {
425 .name
= VIMC_SCA_DRV_NAME
,
430 static struct platform_driver vimc_sca_pdrv
= {
431 .probe
= vimc_sca_probe
,
432 .remove
= vimc_sca_remove
,
433 .id_table
= vimc_sca_driver_ids
,
435 .name
= VIMC_SCA_DRV_NAME
,
439 module_platform_driver(vimc_sca_pdrv
);
441 MODULE_DEVICE_TABLE(platform
, vimc_sca_driver_ids
);
443 MODULE_DESCRIPTION("Virtual Media Controller Driver (VIMC) Scaler");
444 MODULE_AUTHOR("Helen Mae Koike Fornazier <helen.fornazier@gmail.com>");
445 MODULE_LICENSE("GPL");