613 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/*3 * vimc-debayer.c Virtual Media Controller Driver4 *5 * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>6 */7 8#include <linux/moduleparam.h>9#include <linux/platform_device.h>10#include <linux/vmalloc.h>11#include <linux/v4l2-mediabus.h>12#include <media/v4l2-ctrls.h>13#include <media/v4l2-event.h>14#include <media/v4l2-subdev.h>15 16#include "vimc-common.h"17 18/* TODO: Add support for more output formats, we only support RGB888 for now. */19#define VIMC_DEBAYER_SOURCE_MBUS_FMT MEDIA_BUS_FMT_RGB888_1X2420 21enum vimc_debayer_rgb_colors {22 VIMC_DEBAYER_RED = 0,23 VIMC_DEBAYER_GREEN = 1,24 VIMC_DEBAYER_BLUE = 2,25};26 27struct vimc_debayer_pix_map {28 u32 code;29 enum vimc_debayer_rgb_colors order[2][2];30};31 32struct vimc_debayer_device {33 struct vimc_ent_device ved;34 struct v4l2_subdev sd;35 struct v4l2_ctrl_handler hdl;36 struct media_pad pads[2];37 38 u8 *src_frame;39 40 void (*set_rgb_src)(struct vimc_debayer_device *vdebayer,41 unsigned int lin, unsigned int col,42 unsigned int rgb[3]);43 44 /*45 * Virtual "hardware" configuration, filled when the stream starts or46 * when controls are set.47 */48 struct {49 const struct vimc_debayer_pix_map *sink_pix_map;50 unsigned int sink_bpp;51 struct v4l2_area size;52 unsigned int mean_win_size;53 u32 src_code;54 } hw;55};56 57static const struct v4l2_mbus_framefmt sink_fmt_default = {58 .width = 640,59 .height = 480,60 .code = MEDIA_BUS_FMT_SRGGB8_1X8,61 .field = V4L2_FIELD_NONE,62 .colorspace = V4L2_COLORSPACE_SRGB,63};64 65static const u32 vimc_debayer_src_mbus_codes[] = {66 MEDIA_BUS_FMT_GBR888_1X24,67 MEDIA_BUS_FMT_BGR888_1X24,68 MEDIA_BUS_FMT_BGR888_3X8,69 MEDIA_BUS_FMT_RGB888_1X24,70 MEDIA_BUS_FMT_RGB888_2X12_BE,71 MEDIA_BUS_FMT_RGB888_2X12_LE,72 MEDIA_BUS_FMT_RGB888_3X8,73 MEDIA_BUS_FMT_RGB888_1X7X4_SPWG,74 MEDIA_BUS_FMT_RGB888_1X7X4_JEIDA,75 MEDIA_BUS_FMT_RGB888_1X32_PADHI,76};77 78static const struct vimc_debayer_pix_map vimc_debayer_pix_map_list[] = {79 {80 .code = MEDIA_BUS_FMT_SBGGR8_1X8,81 .order = { { VIMC_DEBAYER_BLUE, VIMC_DEBAYER_GREEN },82 { VIMC_DEBAYER_GREEN, VIMC_DEBAYER_RED } }83 },84 {85 .code = MEDIA_BUS_FMT_SGBRG8_1X8,86 .order = { { VIMC_DEBAYER_GREEN, VIMC_DEBAYER_BLUE },87 { VIMC_DEBAYER_RED, VIMC_DEBAYER_GREEN } }88 },89 {90 .code = MEDIA_BUS_FMT_SGRBG8_1X8,91 .order = { { VIMC_DEBAYER_GREEN, VIMC_DEBAYER_RED },92 { VIMC_DEBAYER_BLUE, VIMC_DEBAYER_GREEN } }93 },94 {95 .code = MEDIA_BUS_FMT_SRGGB8_1X8,96 .order = { { VIMC_DEBAYER_RED, VIMC_DEBAYER_GREEN },97 { VIMC_DEBAYER_GREEN, VIMC_DEBAYER_BLUE } }98 },99 {100 .code = MEDIA_BUS_FMT_SBGGR10_1X10,101 .order = { { VIMC_DEBAYER_BLUE, VIMC_DEBAYER_GREEN },102 { VIMC_DEBAYER_GREEN, VIMC_DEBAYER_RED } }103 },104 {105 .code = MEDIA_BUS_FMT_SGBRG10_1X10,106 .order = { { VIMC_DEBAYER_GREEN, VIMC_DEBAYER_BLUE },107 { VIMC_DEBAYER_RED, VIMC_DEBAYER_GREEN } }108 },109 {110 .code = MEDIA_BUS_FMT_SGRBG10_1X10,111 .order = { { VIMC_DEBAYER_GREEN, VIMC_DEBAYER_RED },112 { VIMC_DEBAYER_BLUE, VIMC_DEBAYER_GREEN } }113 },114 {115 .code = MEDIA_BUS_FMT_SRGGB10_1X10,116 .order = { { VIMC_DEBAYER_RED, VIMC_DEBAYER_GREEN },117 { VIMC_DEBAYER_GREEN, VIMC_DEBAYER_BLUE } }118 },119 {120 .code = MEDIA_BUS_FMT_SBGGR12_1X12,121 .order = { { VIMC_DEBAYER_BLUE, VIMC_DEBAYER_GREEN },122 { VIMC_DEBAYER_GREEN, VIMC_DEBAYER_RED } }123 },124 {125 .code = MEDIA_BUS_FMT_SGBRG12_1X12,126 .order = { { VIMC_DEBAYER_GREEN, VIMC_DEBAYER_BLUE },127 { VIMC_DEBAYER_RED, VIMC_DEBAYER_GREEN } }128 },129 {130 .code = MEDIA_BUS_FMT_SGRBG12_1X12,131 .order = { { VIMC_DEBAYER_GREEN, VIMC_DEBAYER_RED },132 { VIMC_DEBAYER_BLUE, VIMC_DEBAYER_GREEN } }133 },134 {135 .code = MEDIA_BUS_FMT_SRGGB12_1X12,136 .order = { { VIMC_DEBAYER_RED, VIMC_DEBAYER_GREEN },137 { VIMC_DEBAYER_GREEN, VIMC_DEBAYER_BLUE } }138 },139};140 141static const struct vimc_debayer_pix_map *vimc_debayer_pix_map_by_code(u32 code)142{143 unsigned int i;144 145 for (i = 0; i < ARRAY_SIZE(vimc_debayer_pix_map_list); i++)146 if (vimc_debayer_pix_map_list[i].code == code)147 return &vimc_debayer_pix_map_list[i];148 149 return NULL;150}151 152static bool vimc_debayer_src_code_is_valid(u32 code)153{154 unsigned int i;155 156 for (i = 0; i < ARRAY_SIZE(vimc_debayer_src_mbus_codes); i++)157 if (vimc_debayer_src_mbus_codes[i] == code)158 return true;159 160 return false;161}162 163static int vimc_debayer_init_state(struct v4l2_subdev *sd,164 struct v4l2_subdev_state *sd_state)165{166 struct v4l2_mbus_framefmt *mf;167 168 mf = v4l2_subdev_state_get_format(sd_state, 0);169 *mf = sink_fmt_default;170 171 mf = v4l2_subdev_state_get_format(sd_state, 1);172 *mf = sink_fmt_default;173 mf->code = VIMC_DEBAYER_SOURCE_MBUS_FMT;174 175 return 0;176}177 178static int vimc_debayer_enum_mbus_code(struct v4l2_subdev *sd,179 struct v4l2_subdev_state *sd_state,180 struct v4l2_subdev_mbus_code_enum *code)181{182 if (VIMC_IS_SRC(code->pad)) {183 if (code->index >= ARRAY_SIZE(vimc_debayer_src_mbus_codes))184 return -EINVAL;185 186 code->code = vimc_debayer_src_mbus_codes[code->index];187 } else {188 if (code->index >= ARRAY_SIZE(vimc_debayer_pix_map_list))189 return -EINVAL;190 191 code->code = vimc_debayer_pix_map_list[code->index].code;192 }193 194 return 0;195}196 197static int vimc_debayer_enum_frame_size(struct v4l2_subdev *sd,198 struct v4l2_subdev_state *sd_state,199 struct v4l2_subdev_frame_size_enum *fse)200{201 if (fse->index)202 return -EINVAL;203 204 if (VIMC_IS_SINK(fse->pad)) {205 const struct vimc_debayer_pix_map *vpix =206 vimc_debayer_pix_map_by_code(fse->code);207 208 if (!vpix)209 return -EINVAL;210 } else if (!vimc_debayer_src_code_is_valid(fse->code)) {211 return -EINVAL;212 }213 214 fse->min_width = VIMC_FRAME_MIN_WIDTH;215 fse->max_width = VIMC_FRAME_MAX_WIDTH;216 fse->min_height = VIMC_FRAME_MIN_HEIGHT;217 fse->max_height = VIMC_FRAME_MAX_HEIGHT;218 219 return 0;220}221 222static void vimc_debayer_adjust_sink_fmt(struct v4l2_mbus_framefmt *fmt)223{224 const struct vimc_debayer_pix_map *vpix;225 226 /* Don't accept a code that is not on the debayer table */227 vpix = vimc_debayer_pix_map_by_code(fmt->code);228 if (!vpix)229 fmt->code = sink_fmt_default.code;230 231 fmt->width = clamp_t(u32, fmt->width, VIMC_FRAME_MIN_WIDTH,232 VIMC_FRAME_MAX_WIDTH) & ~1;233 fmt->height = clamp_t(u32, fmt->height, VIMC_FRAME_MIN_HEIGHT,234 VIMC_FRAME_MAX_HEIGHT) & ~1;235 236 if (fmt->field == V4L2_FIELD_ANY)237 fmt->field = sink_fmt_default.field;238 239 vimc_colorimetry_clamp(fmt);240}241 242static int vimc_debayer_set_fmt(struct v4l2_subdev *sd,243 struct v4l2_subdev_state *sd_state,244 struct v4l2_subdev_format *fmt)245{246 struct vimc_debayer_device *vdebayer = v4l2_get_subdevdata(sd);247 struct v4l2_mbus_framefmt *format;248 249 /* Do not change the format while stream is on. */250 if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE && vdebayer->src_frame)251 return -EBUSY;252 253 /*254 * Do not change the format of the source pad, it is propagated from255 * the sink.256 */257 if (VIMC_IS_SRC(fmt->pad))258 return v4l2_subdev_get_fmt(sd, sd_state, fmt);259 260 /* Set the new format in the sink pad. */261 vimc_debayer_adjust_sink_fmt(&fmt->format);262 263 format = v4l2_subdev_state_get_format(sd_state, 0);264 265 dev_dbg(vdebayer->ved.dev, "%s: sink format update: "266 "old:%dx%d (0x%x, %d, %d, %d, %d) "267 "new:%dx%d (0x%x, %d, %d, %d, %d)\n", vdebayer->sd.name,268 /* old */269 format->width, format->height, format->code,270 format->colorspace, format->quantization,271 format->xfer_func, format->ycbcr_enc,272 /* new */273 fmt->format.width, fmt->format.height, fmt->format.code,274 fmt->format.colorspace, fmt->format.quantization,275 fmt->format.xfer_func, fmt->format.ycbcr_enc);276 277 *format = fmt->format;278 279 /* Propagate the format to the source pad. */280 format = v4l2_subdev_state_get_format(sd_state, 1);281 *format = fmt->format;282 format->code = VIMC_DEBAYER_SOURCE_MBUS_FMT;283 284 return 0;285}286 287static const struct v4l2_subdev_pad_ops vimc_debayer_pad_ops = {288 .enum_mbus_code = vimc_debayer_enum_mbus_code,289 .enum_frame_size = vimc_debayer_enum_frame_size,290 .get_fmt = v4l2_subdev_get_fmt,291 .set_fmt = vimc_debayer_set_fmt,292};293 294static void vimc_debayer_process_rgb_frame(struct vimc_debayer_device *vdebayer,295 unsigned int lin,296 unsigned int col,297 unsigned int rgb[3])298{299 const struct vimc_pix_map *vpix;300 unsigned int i, index;301 302 vpix = vimc_pix_map_by_code(vdebayer->hw.src_code);303 index = VIMC_FRAME_INDEX(lin, col, vdebayer->hw.size.width, 3);304 for (i = 0; i < 3; i++) {305 switch (vpix->pixelformat) {306 case V4L2_PIX_FMT_RGB24:307 vdebayer->src_frame[index + i] = rgb[i];308 break;309 case V4L2_PIX_FMT_BGR24:310 vdebayer->src_frame[index + i] = rgb[2 - i];311 break;312 }313 }314}315 316static int vimc_debayer_s_stream(struct v4l2_subdev *sd, int enable)317{318 struct vimc_debayer_device *vdebayer = v4l2_get_subdevdata(sd);319 320 if (enable) {321 const struct v4l2_mbus_framefmt *sink_fmt;322 const struct v4l2_mbus_framefmt *src_fmt;323 struct v4l2_subdev_state *state;324 const struct vimc_pix_map *vpix;325 unsigned int frame_size;326 327 if (vdebayer->src_frame)328 return 0;329 330 state = v4l2_subdev_lock_and_get_active_state(sd);331 sink_fmt = v4l2_subdev_state_get_format(state, 0);332 src_fmt = v4l2_subdev_state_get_format(state, 1);333 334 /* Calculate the frame size of the source pad */335 vpix = vimc_pix_map_by_code(src_fmt->code);336 frame_size = src_fmt->width * src_fmt->height * vpix->bpp;337 338 /* Save the bytes per pixel of the sink */339 vpix = vimc_pix_map_by_code(sink_fmt->code);340 vdebayer->hw.sink_bpp = vpix->bpp;341 342 /* Get the corresponding pixel map from the table */343 vdebayer->hw.sink_pix_map =344 vimc_debayer_pix_map_by_code(sink_fmt->code);345 346 vdebayer->hw.size.width = sink_fmt->width;347 vdebayer->hw.size.height = sink_fmt->height;348 349 vdebayer->hw.src_code = src_fmt->code;350 351 v4l2_subdev_unlock_state(state);352 353 /*354 * Allocate the frame buffer. Use vmalloc to be able to355 * allocate a large amount of memory356 */357 vdebayer->src_frame = vmalloc(frame_size);358 if (!vdebayer->src_frame)359 return -ENOMEM;360 } else {361 if (!vdebayer->src_frame)362 return 0;363 364 vfree(vdebayer->src_frame);365 vdebayer->src_frame = NULL;366 }367 368 return 0;369}370 371static const struct v4l2_subdev_core_ops vimc_debayer_core_ops = {372 .log_status = v4l2_ctrl_subdev_log_status,373 .subscribe_event = v4l2_ctrl_subdev_subscribe_event,374 .unsubscribe_event = v4l2_event_subdev_unsubscribe,375};376 377static const struct v4l2_subdev_video_ops vimc_debayer_video_ops = {378 .s_stream = vimc_debayer_s_stream,379};380 381static const struct v4l2_subdev_ops vimc_debayer_ops = {382 .core = &vimc_debayer_core_ops,383 .pad = &vimc_debayer_pad_ops,384 .video = &vimc_debayer_video_ops,385};386 387static const struct v4l2_subdev_internal_ops vimc_debayer_internal_ops = {388 .init_state = vimc_debayer_init_state,389};390 391static unsigned int vimc_debayer_get_val(const u8 *bytes,392 const unsigned int n_bytes)393{394 unsigned int i;395 unsigned int acc = 0;396 397 for (i = 0; i < n_bytes; i++)398 acc = acc + (bytes[i] << (8 * i));399 400 return acc;401}402 403static void vimc_debayer_calc_rgb_sink(struct vimc_debayer_device *vdebayer,404 const u8 *frame,405 const unsigned int lin,406 const unsigned int col,407 unsigned int rgb[3])408{409 unsigned int i, seek, wlin, wcol;410 unsigned int n_rgb[3] = {0, 0, 0};411 412 for (i = 0; i < 3; i++)413 rgb[i] = 0;414 415 /*416 * Calculate how many we need to subtract to get to the pixel in417 * the top left corner of the mean window (considering the current418 * pixel as the center)419 */420 seek = vdebayer->hw.mean_win_size / 2;421 422 /* Sum the values of the colors in the mean window */423 424 dev_dbg(vdebayer->ved.dev,425 "deb: %s: --- Calc pixel %dx%d, window mean %d, seek %d ---\n",426 vdebayer->sd.name, lin, col, vdebayer->hw.size.height, seek);427 428 /*429 * Iterate through all the lines in the mean window, start430 * with zero if the pixel is outside the frame and don't pass431 * the height when the pixel is in the bottom border of the432 * frame433 */434 for (wlin = seek > lin ? 0 : lin - seek;435 wlin < lin + seek + 1 && wlin < vdebayer->hw.size.height;436 wlin++) {437 438 /*439 * Iterate through all the columns in the mean window, start440 * with zero if the pixel is outside the frame and don't pass441 * the width when the pixel is in the right border of the442 * frame443 */444 for (wcol = seek > col ? 0 : col - seek;445 wcol < col + seek + 1 && wcol < vdebayer->hw.size.width;446 wcol++) {447 enum vimc_debayer_rgb_colors color;448 unsigned int index;449 450 /* Check which color this pixel is */451 color = vdebayer->hw.sink_pix_map->order[wlin % 2][wcol % 2];452 453 index = VIMC_FRAME_INDEX(wlin, wcol,454 vdebayer->hw.size.width,455 vdebayer->hw.sink_bpp);456 457 dev_dbg(vdebayer->ved.dev,458 "deb: %s: RGB CALC: frame index %d, win pos %dx%d, color %d\n",459 vdebayer->sd.name, index, wlin, wcol, color);460 461 /* Get its value */462 rgb[color] = rgb[color] +463 vimc_debayer_get_val(&frame[index],464 vdebayer->hw.sink_bpp);465 466 /* Save how many values we already added */467 n_rgb[color]++;468 469 dev_dbg(vdebayer->ved.dev, "deb: %s: RGB CALC: val %d, n %d\n",470 vdebayer->sd.name, rgb[color], n_rgb[color]);471 }472 }473 474 /* Calculate the mean */475 for (i = 0; i < 3; i++) {476 dev_dbg(vdebayer->ved.dev,477 "deb: %s: PRE CALC: %dx%d Color %d, val %d, n %d\n",478 vdebayer->sd.name, lin, col, i, rgb[i], n_rgb[i]);479 480 if (n_rgb[i])481 rgb[i] = rgb[i] / n_rgb[i];482 483 dev_dbg(vdebayer->ved.dev,484 "deb: %s: FINAL CALC: %dx%d Color %d, val %d\n",485 vdebayer->sd.name, lin, col, i, rgb[i]);486 }487}488 489static void *vimc_debayer_process_frame(struct vimc_ent_device *ved,490 const void *sink_frame)491{492 struct vimc_debayer_device *vdebayer =493 container_of(ved, struct vimc_debayer_device, ved);494 495 unsigned int rgb[3];496 unsigned int i, j;497 498 /* If the stream in this node is not active, just return */499 if (!vdebayer->src_frame)500 return ERR_PTR(-EINVAL);501 502 for (i = 0; i < vdebayer->hw.size.height; i++)503 for (j = 0; j < vdebayer->hw.size.width; j++) {504 vimc_debayer_calc_rgb_sink(vdebayer, sink_frame, i, j, rgb);505 vdebayer->set_rgb_src(vdebayer, i, j, rgb);506 }507 508 return vdebayer->src_frame;509}510 511static int vimc_debayer_s_ctrl(struct v4l2_ctrl *ctrl)512{513 struct vimc_debayer_device *vdebayer =514 container_of(ctrl->handler, struct vimc_debayer_device, hdl);515 516 switch (ctrl->id) {517 case VIMC_CID_MEAN_WIN_SIZE:518 vdebayer->hw.mean_win_size = ctrl->val;519 break;520 default:521 return -EINVAL;522 }523 return 0;524}525 526static const struct v4l2_ctrl_ops vimc_debayer_ctrl_ops = {527 .s_ctrl = vimc_debayer_s_ctrl,528};529 530static void vimc_debayer_release(struct vimc_ent_device *ved)531{532 struct vimc_debayer_device *vdebayer =533 container_of(ved, struct vimc_debayer_device, ved);534 535 v4l2_ctrl_handler_free(&vdebayer->hdl);536 v4l2_subdev_cleanup(&vdebayer->sd);537 media_entity_cleanup(vdebayer->ved.ent);538 kfree(vdebayer);539}540 541static const struct v4l2_ctrl_config vimc_debayer_ctrl_class = {542 .flags = V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_WRITE_ONLY,543 .id = VIMC_CID_VIMC_CLASS,544 .name = "VIMC Controls",545 .type = V4L2_CTRL_TYPE_CTRL_CLASS,546};547 548static const struct v4l2_ctrl_config vimc_debayer_ctrl_mean_win_size = {549 .ops = &vimc_debayer_ctrl_ops,550 .id = VIMC_CID_MEAN_WIN_SIZE,551 .name = "Debayer Mean Window Size",552 .type = V4L2_CTRL_TYPE_INTEGER,553 .min = 1,554 .max = 25,555 .step = 2,556 .def = 3,557};558 559static struct vimc_ent_device *vimc_debayer_add(struct vimc_device *vimc,560 const char *vcfg_name)561{562 struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;563 struct vimc_debayer_device *vdebayer;564 int ret;565 566 /* Allocate the vdebayer struct */567 vdebayer = kzalloc(sizeof(*vdebayer), GFP_KERNEL);568 if (!vdebayer)569 return ERR_PTR(-ENOMEM);570 571 /* Create controls: */572 v4l2_ctrl_handler_init(&vdebayer->hdl, 2);573 v4l2_ctrl_new_custom(&vdebayer->hdl, &vimc_debayer_ctrl_class, NULL);574 v4l2_ctrl_new_custom(&vdebayer->hdl, &vimc_debayer_ctrl_mean_win_size, NULL);575 vdebayer->sd.ctrl_handler = &vdebayer->hdl;576 if (vdebayer->hdl.error) {577 ret = vdebayer->hdl.error;578 goto err_free_vdebayer;579 }580 581 /* Initialize ved and sd */582 vdebayer->pads[0].flags = MEDIA_PAD_FL_SINK;583 vdebayer->pads[1].flags = MEDIA_PAD_FL_SOURCE;584 585 ret = vimc_ent_sd_register(&vdebayer->ved, &vdebayer->sd, v4l2_dev,586 vcfg_name,587 MEDIA_ENT_F_PROC_VIDEO_PIXEL_ENC_CONV, 2,588 vdebayer->pads, &vimc_debayer_internal_ops,589 &vimc_debayer_ops);590 if (ret)591 goto err_free_hdl;592 593 vdebayer->ved.process_frame = vimc_debayer_process_frame;594 vdebayer->ved.dev = vimc->mdev.dev;595 vdebayer->hw.mean_win_size = vimc_debayer_ctrl_mean_win_size.def;596 597 vdebayer->set_rgb_src = vimc_debayer_process_rgb_frame;598 599 return &vdebayer->ved;600 601err_free_hdl:602 v4l2_ctrl_handler_free(&vdebayer->hdl);603err_free_vdebayer:604 kfree(vdebayer);605 606 return ERR_PTR(ret);607}608 609const struct vimc_ent_type vimc_debayer_type = {610 .add = vimc_debayer_add,611 .release = vimc_debayer_release612};613