2 * vimc-debayer.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_DEB_DRV_NAME "vimc-debayer"
30 static unsigned int deb_mean_win_size
= 3;
31 module_param(deb_mean_win_size
, uint
, 0000);
32 MODULE_PARM_DESC(deb_mean_win_size
, " the window size to calculate the mean.\n"
33 "NOTE: the window size need to be an odd number, as the main pixel "
34 "stays in the center of the window, otherwise the next odd number "
37 #define IS_SINK(pad) (!pad)
38 #define IS_SRC(pad) (pad)
40 enum vimc_deb_rgb_colors
{
46 struct vimc_deb_pix_map
{
48 enum vimc_deb_rgb_colors order
[2][2];
51 struct vimc_deb_device
{
52 struct vimc_ent_device ved
;
53 struct v4l2_subdev sd
;
55 /* The active format */
56 struct v4l2_mbus_framefmt sink_fmt
;
58 void (*set_rgb_src
)(struct vimc_deb_device
*vdeb
, unsigned int lin
,
59 unsigned int col
, unsigned int rgb
[3]);
60 /* Values calculated when the stream starts */
62 const struct vimc_deb_pix_map
*sink_pix_map
;
63 unsigned int sink_bpp
;
66 static const struct v4l2_mbus_framefmt sink_fmt_default
= {
69 .code
= MEDIA_BUS_FMT_RGB888_1X24
,
70 .field
= V4L2_FIELD_NONE
,
71 .colorspace
= V4L2_COLORSPACE_DEFAULT
,
74 static const struct vimc_deb_pix_map vimc_deb_pix_map_list
[] = {
76 .code
= MEDIA_BUS_FMT_SBGGR8_1X8
,
77 .order
= { { VIMC_DEB_BLUE
, VIMC_DEB_GREEN
},
78 { VIMC_DEB_GREEN
, VIMC_DEB_RED
} }
81 .code
= MEDIA_BUS_FMT_SGBRG8_1X8
,
82 .order
= { { VIMC_DEB_GREEN
, VIMC_DEB_BLUE
},
83 { VIMC_DEB_RED
, VIMC_DEB_GREEN
} }
86 .code
= MEDIA_BUS_FMT_SGRBG8_1X8
,
87 .order
= { { VIMC_DEB_GREEN
, VIMC_DEB_RED
},
88 { VIMC_DEB_BLUE
, VIMC_DEB_GREEN
} }
91 .code
= MEDIA_BUS_FMT_SRGGB8_1X8
,
92 .order
= { { VIMC_DEB_RED
, VIMC_DEB_GREEN
},
93 { VIMC_DEB_GREEN
, VIMC_DEB_BLUE
} }
96 .code
= MEDIA_BUS_FMT_SBGGR10_1X10
,
97 .order
= { { VIMC_DEB_BLUE
, VIMC_DEB_GREEN
},
98 { VIMC_DEB_GREEN
, VIMC_DEB_RED
} }
101 .code
= MEDIA_BUS_FMT_SGBRG10_1X10
,
102 .order
= { { VIMC_DEB_GREEN
, VIMC_DEB_BLUE
},
103 { VIMC_DEB_RED
, VIMC_DEB_GREEN
} }
106 .code
= MEDIA_BUS_FMT_SGRBG10_1X10
,
107 .order
= { { VIMC_DEB_GREEN
, VIMC_DEB_RED
},
108 { VIMC_DEB_BLUE
, VIMC_DEB_GREEN
} }
111 .code
= MEDIA_BUS_FMT_SRGGB10_1X10
,
112 .order
= { { VIMC_DEB_RED
, VIMC_DEB_GREEN
},
113 { VIMC_DEB_GREEN
, VIMC_DEB_BLUE
} }
116 .code
= MEDIA_BUS_FMT_SBGGR12_1X12
,
117 .order
= { { VIMC_DEB_BLUE
, VIMC_DEB_GREEN
},
118 { VIMC_DEB_GREEN
, VIMC_DEB_RED
} }
121 .code
= MEDIA_BUS_FMT_SGBRG12_1X12
,
122 .order
= { { VIMC_DEB_GREEN
, VIMC_DEB_BLUE
},
123 { VIMC_DEB_RED
, VIMC_DEB_GREEN
} }
126 .code
= MEDIA_BUS_FMT_SGRBG12_1X12
,
127 .order
= { { VIMC_DEB_GREEN
, VIMC_DEB_RED
},
128 { VIMC_DEB_BLUE
, VIMC_DEB_GREEN
} }
131 .code
= MEDIA_BUS_FMT_SRGGB12_1X12
,
132 .order
= { { VIMC_DEB_RED
, VIMC_DEB_GREEN
},
133 { VIMC_DEB_GREEN
, VIMC_DEB_BLUE
} }
137 static const struct vimc_deb_pix_map
*vimc_deb_pix_map_by_code(u32 code
)
141 for (i
= 0; i
< ARRAY_SIZE(vimc_deb_pix_map_list
); i
++)
142 if (vimc_deb_pix_map_list
[i
].code
== code
)
143 return &vimc_deb_pix_map_list
[i
];
148 static int vimc_deb_init_cfg(struct v4l2_subdev
*sd
,
149 struct v4l2_subdev_pad_config
*cfg
)
151 struct vimc_deb_device
*vdeb
= v4l2_get_subdevdata(sd
);
152 struct v4l2_mbus_framefmt
*mf
;
155 mf
= v4l2_subdev_get_try_format(sd
, cfg
, 0);
156 *mf
= sink_fmt_default
;
158 for (i
= 1; i
< sd
->entity
.num_pads
; i
++) {
159 mf
= v4l2_subdev_get_try_format(sd
, cfg
, i
);
160 *mf
= sink_fmt_default
;
161 mf
->code
= vdeb
->src_code
;
167 static int vimc_deb_enum_mbus_code(struct v4l2_subdev
*sd
,
168 struct v4l2_subdev_pad_config
*cfg
,
169 struct v4l2_subdev_mbus_code_enum
*code
)
171 /* We only support one format for source pads */
172 if (IS_SRC(code
->pad
)) {
173 struct vimc_deb_device
*vdeb
= v4l2_get_subdevdata(sd
);
178 code
->code
= vdeb
->src_code
;
180 if (code
->index
>= ARRAY_SIZE(vimc_deb_pix_map_list
))
183 code
->code
= vimc_deb_pix_map_list
[code
->index
].code
;
189 static int vimc_deb_enum_frame_size(struct v4l2_subdev
*sd
,
190 struct v4l2_subdev_pad_config
*cfg
,
191 struct v4l2_subdev_frame_size_enum
*fse
)
193 struct vimc_deb_device
*vdeb
= v4l2_get_subdevdata(sd
);
198 if (IS_SINK(fse
->pad
)) {
199 const struct vimc_deb_pix_map
*vpix
=
200 vimc_deb_pix_map_by_code(fse
->code
);
204 } else if (fse
->code
!= vdeb
->src_code
) {
208 fse
->min_width
= VIMC_FRAME_MIN_WIDTH
;
209 fse
->max_width
= VIMC_FRAME_MAX_WIDTH
;
210 fse
->min_height
= VIMC_FRAME_MIN_HEIGHT
;
211 fse
->max_height
= VIMC_FRAME_MAX_HEIGHT
;
216 static int vimc_deb_get_fmt(struct v4l2_subdev
*sd
,
217 struct v4l2_subdev_pad_config
*cfg
,
218 struct v4l2_subdev_format
*fmt
)
220 struct vimc_deb_device
*vdeb
= v4l2_get_subdevdata(sd
);
222 /* Get the current sink format */
223 fmt
->format
= fmt
->which
== V4L2_SUBDEV_FORMAT_TRY
?
224 *v4l2_subdev_get_try_format(sd
, cfg
, 0) :
227 /* Set the right code for the source pad */
228 if (IS_SRC(fmt
->pad
))
229 fmt
->format
.code
= vdeb
->src_code
;
234 static void vimc_deb_adjust_sink_fmt(struct v4l2_mbus_framefmt
*fmt
)
236 const struct vimc_deb_pix_map
*vpix
;
238 /* Don't accept a code that is not on the debayer table */
239 vpix
= vimc_deb_pix_map_by_code(fmt
->code
);
241 fmt
->code
= sink_fmt_default
.code
;
243 fmt
->width
= clamp_t(u32
, fmt
->width
, VIMC_FRAME_MIN_WIDTH
,
244 VIMC_FRAME_MAX_WIDTH
) & ~1;
245 fmt
->height
= clamp_t(u32
, fmt
->height
, VIMC_FRAME_MIN_HEIGHT
,
246 VIMC_FRAME_MAX_HEIGHT
) & ~1;
248 if (fmt
->field
== V4L2_FIELD_ANY
)
249 fmt
->field
= sink_fmt_default
.field
;
251 vimc_colorimetry_clamp(fmt
);
254 static int vimc_deb_set_fmt(struct v4l2_subdev
*sd
,
255 struct v4l2_subdev_pad_config
*cfg
,
256 struct v4l2_subdev_format
*fmt
)
258 struct vimc_deb_device
*vdeb
= v4l2_get_subdevdata(sd
);
259 struct v4l2_mbus_framefmt
*sink_fmt
;
261 if (fmt
->which
== V4L2_SUBDEV_FORMAT_ACTIVE
) {
262 /* Do not change the format while stream is on */
266 sink_fmt
= &vdeb
->sink_fmt
;
268 sink_fmt
= v4l2_subdev_get_try_format(sd
, cfg
, 0);
272 * Do not change the format of the source pad,
273 * it is propagated from the sink
275 if (IS_SRC(fmt
->pad
)) {
276 fmt
->format
= *sink_fmt
;
277 /* TODO: Add support for other formats */
278 fmt
->format
.code
= vdeb
->src_code
;
280 /* Set the new format in the sink pad */
281 vimc_deb_adjust_sink_fmt(&fmt
->format
);
283 dev_dbg(vdeb
->dev
, "%s: sink format update: "
284 "old:%dx%d (0x%x, %d, %d, %d, %d) "
285 "new:%dx%d (0x%x, %d, %d, %d, %d)\n", vdeb
->sd
.name
,
287 sink_fmt
->width
, sink_fmt
->height
, sink_fmt
->code
,
288 sink_fmt
->colorspace
, sink_fmt
->quantization
,
289 sink_fmt
->xfer_func
, sink_fmt
->ycbcr_enc
,
291 fmt
->format
.width
, fmt
->format
.height
, fmt
->format
.code
,
292 fmt
->format
.colorspace
, fmt
->format
.quantization
,
293 fmt
->format
.xfer_func
, fmt
->format
.ycbcr_enc
);
295 *sink_fmt
= fmt
->format
;
301 static const struct v4l2_subdev_pad_ops vimc_deb_pad_ops
= {
302 .init_cfg
= vimc_deb_init_cfg
,
303 .enum_mbus_code
= vimc_deb_enum_mbus_code
,
304 .enum_frame_size
= vimc_deb_enum_frame_size
,
305 .get_fmt
= vimc_deb_get_fmt
,
306 .set_fmt
= vimc_deb_set_fmt
,
309 static void vimc_deb_set_rgb_mbus_fmt_rgb888_1x24(struct vimc_deb_device
*vdeb
,
314 unsigned int i
, index
;
316 index
= VIMC_FRAME_INDEX(lin
, col
, vdeb
->sink_fmt
.width
, 3);
317 for (i
= 0; i
< 3; i
++)
318 vdeb
->src_frame
[index
+ i
] = rgb
[i
];
321 static int vimc_deb_s_stream(struct v4l2_subdev
*sd
, int enable
)
323 struct vimc_deb_device
*vdeb
= v4l2_get_subdevdata(sd
);
327 const struct vimc_pix_map
*vpix
;
328 unsigned int frame_size
;
333 /* Calculate the frame size of the source pad */
334 vpix
= vimc_pix_map_by_code(vdeb
->src_code
);
335 frame_size
= vdeb
->sink_fmt
.width
* vdeb
->sink_fmt
.height
*
338 /* Save the bytes per pixel of the sink */
339 vpix
= vimc_pix_map_by_code(vdeb
->sink_fmt
.code
);
340 vdeb
->sink_bpp
= vpix
->bpp
;
342 /* Get the corresponding pixel map from the table */
344 vimc_deb_pix_map_by_code(vdeb
->sink_fmt
.code
);
347 * Allocate the frame buffer. Use vmalloc to be able to
348 * allocate a large amount of memory
350 vdeb
->src_frame
= vmalloc(frame_size
);
351 if (!vdeb
->src_frame
)
354 /* Turn the stream on in the subdevices directly connected */
355 ret
= vimc_pipeline_s_stream(&vdeb
->sd
.entity
, 1);
357 vfree(vdeb
->src_frame
);
358 vdeb
->src_frame
= NULL
;
362 if (!vdeb
->src_frame
)
365 /* Disable streaming from the pipe */
366 ret
= vimc_pipeline_s_stream(&vdeb
->sd
.entity
, 0);
370 vfree(vdeb
->src_frame
);
371 vdeb
->src_frame
= NULL
;
377 static const struct v4l2_subdev_video_ops vimc_deb_video_ops
= {
378 .s_stream
= vimc_deb_s_stream
,
381 static const struct v4l2_subdev_ops vimc_deb_ops
= {
382 .pad
= &vimc_deb_pad_ops
,
383 .video
= &vimc_deb_video_ops
,
386 static unsigned int vimc_deb_get_val(const u8
*bytes
,
387 const unsigned int n_bytes
)
390 unsigned int acc
= 0;
392 for (i
= 0; i
< n_bytes
; i
++)
393 acc
= acc
+ (bytes
[i
] << (8 * i
));
398 static void vimc_deb_calc_rgb_sink(struct vimc_deb_device
*vdeb
,
400 const unsigned int lin
,
401 const unsigned int col
,
404 unsigned int i
, seek
, wlin
, wcol
;
405 unsigned int n_rgb
[3] = {0, 0, 0};
407 for (i
= 0; i
< 3; i
++)
411 * Calculate how many we need to subtract to get to the pixel in
412 * the top left corner of the mean window (considering the current
413 * pixel as the center)
415 seek
= deb_mean_win_size
/ 2;
417 /* Sum the values of the colors in the mean window */
420 "deb: %s: --- Calc pixel %dx%d, window mean %d, seek %d ---\n",
421 vdeb
->sd
.name
, lin
, col
, vdeb
->sink_fmt
.height
, seek
);
424 * Iterate through all the lines in the mean window, start
425 * with zero if the pixel is outside the frame and don't pass
426 * the height when the pixel is in the bottom border of the
429 for (wlin
= seek
> lin
? 0 : lin
- seek
;
430 wlin
< lin
+ seek
+ 1 && wlin
< vdeb
->sink_fmt
.height
;
434 * Iterate through all the columns in the mean window, start
435 * with zero if the pixel is outside the frame and don't pass
436 * the width when the pixel is in the right border of the
439 for (wcol
= seek
> col
? 0 : col
- seek
;
440 wcol
< col
+ seek
+ 1 && wcol
< vdeb
->sink_fmt
.width
;
442 enum vimc_deb_rgb_colors color
;
445 /* Check which color this pixel is */
446 color
= vdeb
->sink_pix_map
->order
[wlin
% 2][wcol
% 2];
448 index
= VIMC_FRAME_INDEX(wlin
, wcol
,
449 vdeb
->sink_fmt
.width
,
453 "deb: %s: RGB CALC: frame index %d, win pos %dx%d, color %d\n",
454 vdeb
->sd
.name
, index
, wlin
, wcol
, color
);
457 rgb
[color
] = rgb
[color
] +
458 vimc_deb_get_val(&frame
[index
], vdeb
->sink_bpp
);
460 /* Save how many values we already added */
463 dev_dbg(vdeb
->dev
, "deb: %s: RGB CALC: val %d, n %d\n",
464 vdeb
->sd
.name
, rgb
[color
], n_rgb
[color
]);
468 /* Calculate the mean */
469 for (i
= 0; i
< 3; i
++) {
471 "deb: %s: PRE CALC: %dx%d Color %d, val %d, n %d\n",
472 vdeb
->sd
.name
, lin
, col
, i
, rgb
[i
], n_rgb
[i
]);
475 rgb
[i
] = rgb
[i
] / n_rgb
[i
];
478 "deb: %s: FINAL CALC: %dx%d Color %d, val %d\n",
479 vdeb
->sd
.name
, lin
, col
, i
, rgb
[i
]);
483 static void vimc_deb_process_frame(struct vimc_ent_device
*ved
,
484 struct media_pad
*sink
,
485 const void *sink_frame
)
487 struct vimc_deb_device
*vdeb
= container_of(ved
, struct vimc_deb_device
,
492 /* If the stream in this node is not active, just return */
493 if (!vdeb
->src_frame
)
496 for (i
= 0; i
< vdeb
->sink_fmt
.height
; i
++)
497 for (j
= 0; j
< vdeb
->sink_fmt
.width
; j
++) {
498 vimc_deb_calc_rgb_sink(vdeb
, sink_frame
, i
, j
, rgb
);
499 vdeb
->set_rgb_src(vdeb
, i
, j
, rgb
);
502 /* Propagate the frame through all source pads */
503 for (i
= 1; i
< vdeb
->sd
.entity
.num_pads
; i
++) {
504 struct media_pad
*pad
= &vdeb
->sd
.entity
.pads
[i
];
506 vimc_propagate_frame(pad
, vdeb
->src_frame
);
510 static void vimc_deb_comp_unbind(struct device
*comp
, struct device
*master
,
513 struct vimc_ent_device
*ved
= dev_get_drvdata(comp
);
514 struct vimc_deb_device
*vdeb
= container_of(ved
, struct vimc_deb_device
,
517 vimc_ent_sd_unregister(ved
, &vdeb
->sd
);
521 static int vimc_deb_comp_bind(struct device
*comp
, struct device
*master
,
524 struct v4l2_device
*v4l2_dev
= master_data
;
525 struct vimc_platform_data
*pdata
= comp
->platform_data
;
526 struct vimc_deb_device
*vdeb
;
529 /* Allocate the vdeb struct */
530 vdeb
= kzalloc(sizeof(*vdeb
), GFP_KERNEL
);
534 /* Initialize ved and sd */
535 ret
= vimc_ent_sd_register(&vdeb
->ved
, &vdeb
->sd
, v4l2_dev
,
537 MEDIA_ENT_F_PROC_VIDEO_PIXEL_ENC_CONV
, 2,
538 (const unsigned long[2]) {MEDIA_PAD_FL_SINK
,
539 MEDIA_PAD_FL_SOURCE
},
546 vdeb
->ved
.process_frame
= vimc_deb_process_frame
;
547 dev_set_drvdata(comp
, &vdeb
->ved
);
550 /* Initialize the frame format */
551 vdeb
->sink_fmt
= sink_fmt_default
;
553 * TODO: Add support for more output formats, we only support
555 * NOTE: the src format is always the same as the sink, except
558 vdeb
->src_code
= MEDIA_BUS_FMT_RGB888_1X24
;
559 vdeb
->set_rgb_src
= vimc_deb_set_rgb_mbus_fmt_rgb888_1x24
;
564 static const struct component_ops vimc_deb_comp_ops
= {
565 .bind
= vimc_deb_comp_bind
,
566 .unbind
= vimc_deb_comp_unbind
,
569 static int vimc_deb_probe(struct platform_device
*pdev
)
571 return component_add(&pdev
->dev
, &vimc_deb_comp_ops
);
574 static int vimc_deb_remove(struct platform_device
*pdev
)
576 component_del(&pdev
->dev
, &vimc_deb_comp_ops
);
581 static const struct platform_device_id vimc_deb_driver_ids
[] = {
583 .name
= VIMC_DEB_DRV_NAME
,
588 static struct platform_driver vimc_deb_pdrv
= {
589 .probe
= vimc_deb_probe
,
590 .remove
= vimc_deb_remove
,
591 .id_table
= vimc_deb_driver_ids
,
593 .name
= VIMC_DEB_DRV_NAME
,
597 module_platform_driver(vimc_deb_pdrv
);
599 MODULE_DEVICE_TABLE(platform
, vimc_deb_driver_ids
);
601 MODULE_DESCRIPTION("Virtual Media Controller Driver (VIMC) Debayer");
602 MODULE_AUTHOR("Helen Mae Koike Fornazier <helen.fornazier@gmail.com>");
603 MODULE_LICENSE("GPL");