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/platform_device.h>
21 #include <linux/vmalloc.h>
22 #include <linux/v4l2-mediabus.h>
23 #include <media/v4l2-subdev.h>
25 #include "vimc-common.h"
27 #define VIMC_DEB_DRV_NAME "vimc-debayer"
29 static unsigned int deb_mean_win_size
= 3;
30 module_param(deb_mean_win_size
, uint
, 0000);
31 MODULE_PARM_DESC(deb_mean_win_size
, " the window size to calculate the mean.\n"
32 "NOTE: the window size need to be an odd number, as the main pixel "
33 "stays in the center of the window, otherwise the next odd number "
36 #define IS_SINK(pad) (!pad)
37 #define IS_SRC(pad) (pad)
39 enum vimc_deb_rgb_colors
{
45 struct vimc_deb_pix_map
{
47 enum vimc_deb_rgb_colors order
[2][2];
50 struct vimc_deb_device
{
51 struct vimc_ent_device ved
;
52 struct v4l2_subdev sd
;
54 /* The active format */
55 struct v4l2_mbus_framefmt sink_fmt
;
57 void (*set_rgb_src
)(struct vimc_deb_device
*vdeb
, unsigned int lin
,
58 unsigned int col
, unsigned int rgb
[3]);
59 /* Values calculated when the stream starts */
61 const struct vimc_deb_pix_map
*sink_pix_map
;
62 unsigned int sink_bpp
;
65 static const struct v4l2_mbus_framefmt sink_fmt_default
= {
68 .code
= MEDIA_BUS_FMT_RGB888_1X24
,
69 .field
= V4L2_FIELD_NONE
,
70 .colorspace
= V4L2_COLORSPACE_DEFAULT
,
73 static const struct vimc_deb_pix_map vimc_deb_pix_map_list
[] = {
75 .code
= MEDIA_BUS_FMT_SBGGR8_1X8
,
76 .order
= { { VIMC_DEB_BLUE
, VIMC_DEB_GREEN
},
77 { VIMC_DEB_GREEN
, VIMC_DEB_RED
} }
80 .code
= MEDIA_BUS_FMT_SGBRG8_1X8
,
81 .order
= { { VIMC_DEB_GREEN
, VIMC_DEB_BLUE
},
82 { VIMC_DEB_RED
, VIMC_DEB_GREEN
} }
85 .code
= MEDIA_BUS_FMT_SGRBG8_1X8
,
86 .order
= { { VIMC_DEB_GREEN
, VIMC_DEB_RED
},
87 { VIMC_DEB_BLUE
, VIMC_DEB_GREEN
} }
90 .code
= MEDIA_BUS_FMT_SRGGB8_1X8
,
91 .order
= { { VIMC_DEB_RED
, VIMC_DEB_GREEN
},
92 { VIMC_DEB_GREEN
, VIMC_DEB_BLUE
} }
95 .code
= MEDIA_BUS_FMT_SBGGR10_1X10
,
96 .order
= { { VIMC_DEB_BLUE
, VIMC_DEB_GREEN
},
97 { VIMC_DEB_GREEN
, VIMC_DEB_RED
} }
100 .code
= MEDIA_BUS_FMT_SGBRG10_1X10
,
101 .order
= { { VIMC_DEB_GREEN
, VIMC_DEB_BLUE
},
102 { VIMC_DEB_RED
, VIMC_DEB_GREEN
} }
105 .code
= MEDIA_BUS_FMT_SGRBG10_1X10
,
106 .order
= { { VIMC_DEB_GREEN
, VIMC_DEB_RED
},
107 { VIMC_DEB_BLUE
, VIMC_DEB_GREEN
} }
110 .code
= MEDIA_BUS_FMT_SRGGB10_1X10
,
111 .order
= { { VIMC_DEB_RED
, VIMC_DEB_GREEN
},
112 { VIMC_DEB_GREEN
, VIMC_DEB_BLUE
} }
115 .code
= MEDIA_BUS_FMT_SBGGR12_1X12
,
116 .order
= { { VIMC_DEB_BLUE
, VIMC_DEB_GREEN
},
117 { VIMC_DEB_GREEN
, VIMC_DEB_RED
} }
120 .code
= MEDIA_BUS_FMT_SGBRG12_1X12
,
121 .order
= { { VIMC_DEB_GREEN
, VIMC_DEB_BLUE
},
122 { VIMC_DEB_RED
, VIMC_DEB_GREEN
} }
125 .code
= MEDIA_BUS_FMT_SGRBG12_1X12
,
126 .order
= { { VIMC_DEB_GREEN
, VIMC_DEB_RED
},
127 { VIMC_DEB_BLUE
, VIMC_DEB_GREEN
} }
130 .code
= MEDIA_BUS_FMT_SRGGB12_1X12
,
131 .order
= { { VIMC_DEB_RED
, VIMC_DEB_GREEN
},
132 { VIMC_DEB_GREEN
, VIMC_DEB_BLUE
} }
136 static const struct vimc_deb_pix_map
*vimc_deb_pix_map_by_code(u32 code
)
140 for (i
= 0; i
< ARRAY_SIZE(vimc_deb_pix_map_list
); i
++)
141 if (vimc_deb_pix_map_list
[i
].code
== code
)
142 return &vimc_deb_pix_map_list
[i
];
147 static int vimc_deb_init_cfg(struct v4l2_subdev
*sd
,
148 struct v4l2_subdev_pad_config
*cfg
)
150 struct vimc_deb_device
*vdeb
= v4l2_get_subdevdata(sd
);
151 struct v4l2_mbus_framefmt
*mf
;
154 mf
= v4l2_subdev_get_try_format(sd
, cfg
, 0);
155 *mf
= sink_fmt_default
;
157 for (i
= 1; i
< sd
->entity
.num_pads
; i
++) {
158 mf
= v4l2_subdev_get_try_format(sd
, cfg
, i
);
159 *mf
= sink_fmt_default
;
160 mf
->code
= vdeb
->src_code
;
166 static int vimc_deb_enum_mbus_code(struct v4l2_subdev
*sd
,
167 struct v4l2_subdev_pad_config
*cfg
,
168 struct v4l2_subdev_mbus_code_enum
*code
)
170 /* We only support one format for source pads */
171 if (IS_SRC(code
->pad
)) {
172 struct vimc_deb_device
*vdeb
= v4l2_get_subdevdata(sd
);
177 code
->code
= vdeb
->src_code
;
179 if (code
->index
>= ARRAY_SIZE(vimc_deb_pix_map_list
))
182 code
->code
= vimc_deb_pix_map_list
[code
->index
].code
;
188 static int vimc_deb_enum_frame_size(struct v4l2_subdev
*sd
,
189 struct v4l2_subdev_pad_config
*cfg
,
190 struct v4l2_subdev_frame_size_enum
*fse
)
192 struct vimc_deb_device
*vdeb
= v4l2_get_subdevdata(sd
);
197 if (IS_SINK(fse
->pad
)) {
198 const struct vimc_deb_pix_map
*vpix
=
199 vimc_deb_pix_map_by_code(fse
->code
);
203 } else if (fse
->code
!= vdeb
->src_code
) {
207 fse
->min_width
= VIMC_FRAME_MIN_WIDTH
;
208 fse
->max_width
= VIMC_FRAME_MAX_WIDTH
;
209 fse
->min_height
= VIMC_FRAME_MIN_HEIGHT
;
210 fse
->max_height
= VIMC_FRAME_MAX_HEIGHT
;
215 static int vimc_deb_get_fmt(struct v4l2_subdev
*sd
,
216 struct v4l2_subdev_pad_config
*cfg
,
217 struct v4l2_subdev_format
*fmt
)
219 struct vimc_deb_device
*vdeb
= v4l2_get_subdevdata(sd
);
221 /* Get the current sink format */
222 fmt
->format
= fmt
->which
== V4L2_SUBDEV_FORMAT_TRY
?
223 *v4l2_subdev_get_try_format(sd
, cfg
, 0) :
226 /* Set the right code for the source pad */
227 if (IS_SRC(fmt
->pad
))
228 fmt
->format
.code
= vdeb
->src_code
;
233 static void vimc_deb_adjust_sink_fmt(struct v4l2_mbus_framefmt
*fmt
)
235 const struct vimc_deb_pix_map
*vpix
;
237 /* Don't accept a code that is not on the debayer table */
238 vpix
= vimc_deb_pix_map_by_code(fmt
->code
);
240 fmt
->code
= sink_fmt_default
.code
;
242 fmt
->width
= clamp_t(u32
, fmt
->width
, VIMC_FRAME_MIN_WIDTH
,
243 VIMC_FRAME_MAX_WIDTH
) & ~1;
244 fmt
->height
= clamp_t(u32
, fmt
->height
, VIMC_FRAME_MIN_HEIGHT
,
245 VIMC_FRAME_MAX_HEIGHT
) & ~1;
247 if (fmt
->field
== V4L2_FIELD_ANY
)
248 fmt
->field
= sink_fmt_default
.field
;
250 vimc_colorimetry_clamp(fmt
);
253 static int vimc_deb_set_fmt(struct v4l2_subdev
*sd
,
254 struct v4l2_subdev_pad_config
*cfg
,
255 struct v4l2_subdev_format
*fmt
)
257 struct vimc_deb_device
*vdeb
= v4l2_get_subdevdata(sd
);
258 struct v4l2_mbus_framefmt
*sink_fmt
;
260 if (fmt
->which
== V4L2_SUBDEV_FORMAT_ACTIVE
) {
261 /* Do not change the format while stream is on */
265 sink_fmt
= &vdeb
->sink_fmt
;
267 sink_fmt
= v4l2_subdev_get_try_format(sd
, cfg
, 0);
271 * Do not change the format of the source pad,
272 * it is propagated from the sink
274 if (IS_SRC(fmt
->pad
)) {
275 fmt
->format
= *sink_fmt
;
276 /* TODO: Add support for other formats */
277 fmt
->format
.code
= vdeb
->src_code
;
279 /* Set the new format in the sink pad */
280 vimc_deb_adjust_sink_fmt(&fmt
->format
);
282 dev_dbg(vdeb
->dev
, "%s: sink format update: "
283 "old:%dx%d (0x%x, %d, %d, %d, %d) "
284 "new:%dx%d (0x%x, %d, %d, %d, %d)\n", vdeb
->sd
.name
,
286 sink_fmt
->width
, sink_fmt
->height
, sink_fmt
->code
,
287 sink_fmt
->colorspace
, sink_fmt
->quantization
,
288 sink_fmt
->xfer_func
, sink_fmt
->ycbcr_enc
,
290 fmt
->format
.width
, fmt
->format
.height
, fmt
->format
.code
,
291 fmt
->format
.colorspace
, fmt
->format
.quantization
,
292 fmt
->format
.xfer_func
, fmt
->format
.ycbcr_enc
);
294 *sink_fmt
= fmt
->format
;
300 static const struct v4l2_subdev_pad_ops vimc_deb_pad_ops
= {
301 .init_cfg
= vimc_deb_init_cfg
,
302 .enum_mbus_code
= vimc_deb_enum_mbus_code
,
303 .enum_frame_size
= vimc_deb_enum_frame_size
,
304 .get_fmt
= vimc_deb_get_fmt
,
305 .set_fmt
= vimc_deb_set_fmt
,
308 static void vimc_deb_set_rgb_mbus_fmt_rgb888_1x24(struct vimc_deb_device
*vdeb
,
313 unsigned int i
, index
;
315 index
= VIMC_FRAME_INDEX(lin
, col
, vdeb
->sink_fmt
.width
, 3);
316 for (i
= 0; i
< 3; i
++)
317 vdeb
->src_frame
[index
+ i
] = rgb
[i
];
320 static int vimc_deb_s_stream(struct v4l2_subdev
*sd
, int enable
)
322 struct vimc_deb_device
*vdeb
= v4l2_get_subdevdata(sd
);
326 const struct vimc_pix_map
*vpix
;
327 unsigned int frame_size
;
332 /* Calculate the frame size of the source pad */
333 vpix
= vimc_pix_map_by_code(vdeb
->src_code
);
334 frame_size
= vdeb
->sink_fmt
.width
* vdeb
->sink_fmt
.height
*
337 /* Save the bytes per pixel of the sink */
338 vpix
= vimc_pix_map_by_code(vdeb
->sink_fmt
.code
);
339 vdeb
->sink_bpp
= vpix
->bpp
;
341 /* Get the corresponding pixel map from the table */
343 vimc_deb_pix_map_by_code(vdeb
->sink_fmt
.code
);
346 * Allocate the frame buffer. Use vmalloc to be able to
347 * allocate a large amount of memory
349 vdeb
->src_frame
= vmalloc(frame_size
);
350 if (!vdeb
->src_frame
)
353 /* Turn the stream on in the subdevices directly connected */
354 ret
= vimc_pipeline_s_stream(&vdeb
->sd
.entity
, 1);
356 vfree(vdeb
->src_frame
);
357 vdeb
->src_frame
= NULL
;
361 if (!vdeb
->src_frame
)
364 /* Disable streaming from the pipe */
365 ret
= vimc_pipeline_s_stream(&vdeb
->sd
.entity
, 0);
369 vfree(vdeb
->src_frame
);
370 vdeb
->src_frame
= NULL
;
376 static const struct v4l2_subdev_video_ops vimc_deb_video_ops
= {
377 .s_stream
= vimc_deb_s_stream
,
380 static const struct v4l2_subdev_ops vimc_deb_ops
= {
381 .pad
= &vimc_deb_pad_ops
,
382 .video
= &vimc_deb_video_ops
,
385 static unsigned int vimc_deb_get_val(const u8
*bytes
,
386 const unsigned int n_bytes
)
389 unsigned int acc
= 0;
391 for (i
= 0; i
< n_bytes
; i
++)
392 acc
= acc
+ (bytes
[i
] << (8 * i
));
397 static void vimc_deb_calc_rgb_sink(struct vimc_deb_device
*vdeb
,
399 const unsigned int lin
,
400 const unsigned int col
,
403 unsigned int i
, seek
, wlin
, wcol
;
404 unsigned int n_rgb
[3] = {0, 0, 0};
406 for (i
= 0; i
< 3; i
++)
410 * Calculate how many we need to subtract to get to the pixel in
411 * the top left corner of the mean window (considering the current
412 * pixel as the center)
414 seek
= deb_mean_win_size
/ 2;
416 /* Sum the values of the colors in the mean window */
419 "deb: %s: --- Calc pixel %dx%d, window mean %d, seek %d ---\n",
420 vdeb
->sd
.name
, lin
, col
, vdeb
->sink_fmt
.height
, seek
);
423 * Iterate through all the lines in the mean window, start
424 * with zero if the pixel is outside the frame and don't pass
425 * the height when the pixel is in the bottom border of the
428 for (wlin
= seek
> lin
? 0 : lin
- seek
;
429 wlin
< lin
+ seek
+ 1 && wlin
< vdeb
->sink_fmt
.height
;
433 * Iterate through all the columns in the mean window, start
434 * with zero if the pixel is outside the frame and don't pass
435 * the width when the pixel is in the right border of the
438 for (wcol
= seek
> col
? 0 : col
- seek
;
439 wcol
< col
+ seek
+ 1 && wcol
< vdeb
->sink_fmt
.width
;
441 enum vimc_deb_rgb_colors color
;
444 /* Check which color this pixel is */
445 color
= vdeb
->sink_pix_map
->order
[wlin
% 2][wcol
% 2];
447 index
= VIMC_FRAME_INDEX(wlin
, wcol
,
448 vdeb
->sink_fmt
.width
,
452 "deb: %s: RGB CALC: frame index %d, win pos %dx%d, color %d\n",
453 vdeb
->sd
.name
, index
, wlin
, wcol
, color
);
456 rgb
[color
] = rgb
[color
] +
457 vimc_deb_get_val(&frame
[index
], vdeb
->sink_bpp
);
459 /* Save how many values we already added */
462 dev_dbg(vdeb
->dev
, "deb: %s: RGB CALC: val %d, n %d\n",
463 vdeb
->sd
.name
, rgb
[color
], n_rgb
[color
]);
467 /* Calculate the mean */
468 for (i
= 0; i
< 3; i
++) {
470 "deb: %s: PRE CALC: %dx%d Color %d, val %d, n %d\n",
471 vdeb
->sd
.name
, lin
, col
, i
, rgb
[i
], n_rgb
[i
]);
474 rgb
[i
] = rgb
[i
] / n_rgb
[i
];
477 "deb: %s: FINAL CALC: %dx%d Color %d, val %d\n",
478 vdeb
->sd
.name
, lin
, col
, i
, rgb
[i
]);
482 static void vimc_deb_process_frame(struct vimc_ent_device
*ved
,
483 struct media_pad
*sink
,
484 const void *sink_frame
)
486 struct vimc_deb_device
*vdeb
= container_of(ved
, struct vimc_deb_device
,
491 /* If the stream in this node is not active, just return */
492 if (!vdeb
->src_frame
)
495 for (i
= 0; i
< vdeb
->sink_fmt
.height
; i
++)
496 for (j
= 0; j
< vdeb
->sink_fmt
.width
; j
++) {
497 vimc_deb_calc_rgb_sink(vdeb
, sink_frame
, i
, j
, rgb
);
498 vdeb
->set_rgb_src(vdeb
, i
, j
, rgb
);
501 /* Propagate the frame through all source pads */
502 for (i
= 1; i
< vdeb
->sd
.entity
.num_pads
; i
++) {
503 struct media_pad
*pad
= &vdeb
->sd
.entity
.pads
[i
];
505 vimc_propagate_frame(pad
, vdeb
->src_frame
);
509 static void vimc_deb_comp_unbind(struct device
*comp
, struct device
*master
,
512 struct vimc_ent_device
*ved
= dev_get_drvdata(comp
);
513 struct vimc_deb_device
*vdeb
= container_of(ved
, struct vimc_deb_device
,
516 vimc_ent_sd_unregister(ved
, &vdeb
->sd
);
520 static int vimc_deb_comp_bind(struct device
*comp
, struct device
*master
,
523 struct v4l2_device
*v4l2_dev
= master_data
;
524 struct vimc_platform_data
*pdata
= comp
->platform_data
;
525 struct vimc_deb_device
*vdeb
;
528 /* Allocate the vdeb struct */
529 vdeb
= kzalloc(sizeof(*vdeb
), GFP_KERNEL
);
533 /* Initialize ved and sd */
534 ret
= vimc_ent_sd_register(&vdeb
->ved
, &vdeb
->sd
, v4l2_dev
,
536 MEDIA_ENT_F_ATV_DECODER
, 2,
537 (const unsigned long[2]) {MEDIA_PAD_FL_SINK
,
538 MEDIA_PAD_FL_SOURCE
},
545 vdeb
->ved
.process_frame
= vimc_deb_process_frame
;
546 dev_set_drvdata(comp
, &vdeb
->ved
);
549 /* Initialize the frame format */
550 vdeb
->sink_fmt
= sink_fmt_default
;
552 * TODO: Add support for more output formats, we only support
554 * NOTE: the src format is always the same as the sink, except
557 vdeb
->src_code
= MEDIA_BUS_FMT_RGB888_1X24
;
558 vdeb
->set_rgb_src
= vimc_deb_set_rgb_mbus_fmt_rgb888_1x24
;
563 static const struct component_ops vimc_deb_comp_ops
= {
564 .bind
= vimc_deb_comp_bind
,
565 .unbind
= vimc_deb_comp_unbind
,
568 static int vimc_deb_probe(struct platform_device
*pdev
)
570 return component_add(&pdev
->dev
, &vimc_deb_comp_ops
);
573 static int vimc_deb_remove(struct platform_device
*pdev
)
575 component_del(&pdev
->dev
, &vimc_deb_comp_ops
);
580 static const struct platform_device_id vimc_deb_driver_ids
[] = {
582 .name
= VIMC_DEB_DRV_NAME
,
587 static struct platform_driver vimc_deb_pdrv
= {
588 .probe
= vimc_deb_probe
,
589 .remove
= vimc_deb_remove
,
590 .id_table
= vimc_deb_driver_ids
,
592 .name
= VIMC_DEB_DRV_NAME
,
596 module_platform_driver(vimc_deb_pdrv
);
598 MODULE_DEVICE_TABLE(platform
, vimc_deb_driver_ids
);
600 MODULE_DESCRIPTION("Virtual Media Controller Driver (VIMC) Debayer");
601 MODULE_AUTHOR("Helen Mae Koike Fornazier <helen.fornazier@gmail.com>");
602 MODULE_LICENSE("GPL");