drivers/media/platform/vimc/vimc-debayer.c

   1 /*
   2  * vimc-debayer.c Virtual Media Controller Driver
   3  *
   4  * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
   5  *
   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.
  10  *
  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.
  15  *
  16  */
  17
  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>
  25
  26 #include "vimc-common.h"
  27
  28 #define VIMC_DEB_DRV_NAME "vimc-debayer"
  29
  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 "
  35         "is considered");
  36
  37 #define IS_SINK(pad) (!pad)
  38 #define IS_SRC(pad)  (pad)
  39
  40 enum vimc_deb_rgb_colors {
  41         VIMC_DEB_RED = 0,
  42         VIMC_DEB_GREEN = 1,
  43         VIMC_DEB_BLUE = 2,
  44 };
  45
  46 struct vimc_deb_pix_map {
  47         u32 code;
  48         enum vimc_deb_rgb_colors order[2][2];
  49 };
  50
  51 struct vimc_deb_device {
  52         struct vimc_ent_device ved;
  53         struct v4l2_subdev sd;
  54         struct device *dev;
  55         /* The active format */
  56         struct v4l2_mbus_framefmt sink_fmt;
  57         u32 src_code;
  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 */
  61         u8 *src_frame;
  62         const struct vimc_deb_pix_map *sink_pix_map;
  63         unsigned int sink_bpp;
  64 };
  65
  66 static const struct v4l2_mbus_framefmt sink_fmt_default = {
  67         .width = 640,
  68         .height = 480,
  69         .code = MEDIA_BUS_FMT_RGB888_1X24,
  70         .field = V4L2_FIELD_NONE,
  71         .colorspace = V4L2_COLORSPACE_DEFAULT,
  72 };
  73
  74 static const struct vimc_deb_pix_map vimc_deb_pix_map_list[] = {
  75         {
  76                 .code = MEDIA_BUS_FMT_SBGGR8_1X8,
  77                 .order = { { VIMC_DEB_BLUE, VIMC_DEB_GREEN },
  78                            { VIMC_DEB_GREEN, VIMC_DEB_RED } }
  79         },
  80         {
  81                 .code = MEDIA_BUS_FMT_SGBRG8_1X8,
  82                 .order = { { VIMC_DEB_GREEN, VIMC_DEB_BLUE },
  83                            { VIMC_DEB_RED, VIMC_DEB_GREEN } }
  84         },
  85         {
  86                 .code = MEDIA_BUS_FMT_SGRBG8_1X8,
  87                 .order = { { VIMC_DEB_GREEN, VIMC_DEB_RED },
  88                            { VIMC_DEB_BLUE, VIMC_DEB_GREEN } }
  89         },
  90         {
  91                 .code = MEDIA_BUS_FMT_SRGGB8_1X8,
  92                 .order = { { VIMC_DEB_RED, VIMC_DEB_GREEN },
  93                            { VIMC_DEB_GREEN, VIMC_DEB_BLUE } }
  94         },
  95         {
  96                 .code = MEDIA_BUS_FMT_SBGGR10_1X10,
  97                 .order = { { VIMC_DEB_BLUE, VIMC_DEB_GREEN },
  98                            { VIMC_DEB_GREEN, VIMC_DEB_RED } }
  99         },
 100         {
 101                 .code = MEDIA_BUS_FMT_SGBRG10_1X10,
 102                 .order = { { VIMC_DEB_GREEN, VIMC_DEB_BLUE },
 103                            { VIMC_DEB_RED, VIMC_DEB_GREEN } }
 104         },
 105         {
 106                 .code = MEDIA_BUS_FMT_SGRBG10_1X10,
 107                 .order = { { VIMC_DEB_GREEN, VIMC_DEB_RED },
 108                            { VIMC_DEB_BLUE, VIMC_DEB_GREEN } }
 109         },
 110         {
 111                 .code = MEDIA_BUS_FMT_SRGGB10_1X10,
 112                 .order = { { VIMC_DEB_RED, VIMC_DEB_GREEN },
 113                            { VIMC_DEB_GREEN, VIMC_DEB_BLUE } }
 114         },
 115         {
 116                 .code = MEDIA_BUS_FMT_SBGGR12_1X12,
 117                 .order = { { VIMC_DEB_BLUE, VIMC_DEB_GREEN },
 118                            { VIMC_DEB_GREEN, VIMC_DEB_RED } }
 119         },
 120         {
 121                 .code = MEDIA_BUS_FMT_SGBRG12_1X12,
 122                 .order = { { VIMC_DEB_GREEN, VIMC_DEB_BLUE },
 123                            { VIMC_DEB_RED, VIMC_DEB_GREEN } }
 124         },
 125         {
 126                 .code = MEDIA_BUS_FMT_SGRBG12_1X12,
 127                 .order = { { VIMC_DEB_GREEN, VIMC_DEB_RED },
 128                            { VIMC_DEB_BLUE, VIMC_DEB_GREEN } }
 129         },
 130         {
 131                 .code = MEDIA_BUS_FMT_SRGGB12_1X12,
 132                 .order = { { VIMC_DEB_RED, VIMC_DEB_GREEN },
 133                            { VIMC_DEB_GREEN, VIMC_DEB_BLUE } }
 134         },
 135 };
 136
 137 static const struct vimc_deb_pix_map *vimc_deb_pix_map_by_code(u32 code)
 138 {
 139         unsigned int i;
 140
 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];
 144
 145         return NULL;
 146 }
 147
 148 static int vimc_deb_init_cfg(struct v4l2_subdev *sd,
 149                              struct v4l2_subdev_pad_config *cfg)
 150 {
 151         struct vimc_deb_device *vdeb = v4l2_get_subdevdata(sd);
 152         struct v4l2_mbus_framefmt *mf;
 153         unsigned int i;
 154
 155         mf = v4l2_subdev_get_try_format(sd, cfg, 0);
 156         *mf = sink_fmt_default;
 157
 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;
 162         }
 163
 164         return 0;
 165 }
 166
 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)
 170 {
 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);
 174
 175                 if (code->index)
 176                         return -EINVAL;
 177
 178                 code->code = vdeb->src_code;
 179         } else {
 180                 if (code->index >= ARRAY_SIZE(vimc_deb_pix_map_list))
 181                         return -EINVAL;
 182
 183                 code->code = vimc_deb_pix_map_list[code->index].code;
 184         }
 185
 186         return 0;
 187 }
 188
 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)
 192 {
 193         struct vimc_deb_device *vdeb = v4l2_get_subdevdata(sd);
 194
 195         if (fse->index)
 196                 return -EINVAL;
 197
 198         if (IS_SINK(fse->pad)) {
 199                 const struct vimc_deb_pix_map *vpix =
 200                         vimc_deb_pix_map_by_code(fse->code);
 201
 202                 if (!vpix)
 203                         return -EINVAL;
 204         } else if (fse->code != vdeb->src_code) {
 205                 return -EINVAL;
 206         }
 207
 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;
 212
 213         return 0;
 214 }
 215
 216 static int vimc_deb_get_fmt(struct v4l2_subdev *sd,
 217                             struct v4l2_subdev_pad_config *cfg,
 218                             struct v4l2_subdev_format *fmt)
 219 {
 220         struct vimc_deb_device *vdeb = v4l2_get_subdevdata(sd);
 221
 222         /* Get the current sink format */
 223         fmt->format = fmt->which == V4L2_SUBDEV_FORMAT_TRY ?
 224                       *v4l2_subdev_get_try_format(sd, cfg, 0) :
 225                       vdeb->sink_fmt;
 226
 227         /* Set the right code for the source pad */
 228         if (IS_SRC(fmt->pad))
 229                 fmt->format.code = vdeb->src_code;
 230
 231         return 0;
 232 }
 233
 234 static void vimc_deb_adjust_sink_fmt(struct v4l2_mbus_framefmt *fmt)
 235 {
 236         const struct vimc_deb_pix_map *vpix;
 237
 238         /* Don't accept a code that is not on the debayer table */
 239         vpix = vimc_deb_pix_map_by_code(fmt->code);
 240         if (!vpix)
 241                 fmt->code = sink_fmt_default.code;
 242
 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;
 247
 248         if (fmt->field == V4L2_FIELD_ANY)
 249                 fmt->field = sink_fmt_default.field;
 250
 251         vimc_colorimetry_clamp(fmt);
 252 }
 253
 254 static int vimc_deb_set_fmt(struct v4l2_subdev *sd,
 255                             struct v4l2_subdev_pad_config *cfg,
 256                             struct v4l2_subdev_format *fmt)
 257 {
 258         struct vimc_deb_device *vdeb = v4l2_get_subdevdata(sd);
 259         struct v4l2_mbus_framefmt *sink_fmt;
 260
 261         if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
 262                 /* Do not change the format while stream is on */
 263                 if (vdeb->src_frame)
 264                         return -EBUSY;
 265
 266                 sink_fmt = &vdeb->sink_fmt;
 267         } else {
 268                 sink_fmt = v4l2_subdev_get_try_format(sd, cfg, 0);
 269         }
 270
 271         /*
 272          * Do not change the format of the source pad,
 273          * it is propagated from the sink
 274          */
 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;
 279         } else {
 280                 /* Set the new format in the sink pad */
 281                 vimc_deb_adjust_sink_fmt(&fmt->format);
 282
 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,
 286                         /* old */
 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,
 290                         /* new */
 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);
 294
 295                 *sink_fmt = fmt->format;
 296         }
 297
 298         return 0;
 299 }
 300
 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,
 307 };
 308
 309 static void vimc_deb_set_rgb_mbus_fmt_rgb888_1x24(struct vimc_deb_device *vdeb,
 310                                                   unsigned int lin,
 311                                                   unsigned int col,
 312                                                   unsigned int rgb[3])
 313 {
 314         unsigned int i, index;
 315
 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];
 319 }
 320
 321 static int vimc_deb_s_stream(struct v4l2_subdev *sd, int enable)
 322 {
 323         struct vimc_deb_device *vdeb = v4l2_get_subdevdata(sd);
 324
 325         if (enable) {
 326                 const struct vimc_pix_map *vpix;
 327                 unsigned int frame_size;
 328
 329                 if (vdeb->src_frame)
 330                         return 0;
 331
 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 *
 335                                 vpix->bpp;
 336
 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;
 340
 341                 /* Get the corresponding pixel map from the table */
 342                 vdeb->sink_pix_map =
 343                         vimc_deb_pix_map_by_code(vdeb->sink_fmt.code);
 344
 345                 /*
 346                  * Allocate the frame buffer. Use vmalloc to be able to
 347                  * allocate a large amount of memory
 348                  */
 349                 vdeb->src_frame = vmalloc(frame_size);
 350                 if (!vdeb->src_frame)
 351                         return -ENOMEM;
 352
 353         } else {
 354                 if (!vdeb->src_frame)
 355                         return 0;
 356
 357                 vfree(vdeb->src_frame);
 358                 vdeb->src_frame = NULL;
 359         }
 360
 361         return 0;
 362 }
 363
 364 static const struct v4l2_subdev_video_ops vimc_deb_video_ops = {
 365         .s_stream = vimc_deb_s_stream,
 366 };
 367
 368 static const struct v4l2_subdev_ops vimc_deb_ops = {
 369         .pad = &vimc_deb_pad_ops,
 370         .video = &vimc_deb_video_ops,
 371 };
 372
 373 static unsigned int vimc_deb_get_val(const u8 *bytes,
 374                                      const unsigned int n_bytes)
 375 {
 376         unsigned int i;
 377         unsigned int acc = 0;
 378
 379         for (i = 0; i < n_bytes; i++)
 380                 acc = acc + (bytes[i] << (8 * i));
 381
 382         return acc;
 383 }
 384
 385 static void vimc_deb_calc_rgb_sink(struct vimc_deb_device *vdeb,
 386                                    const u8 *frame,
 387                                    const unsigned int lin,
 388                                    const unsigned int col,
 389                                    unsigned int rgb[3])
 390 {
 391         unsigned int i, seek, wlin, wcol;
 392         unsigned int n_rgb[3] = {0, 0, 0};
 393
 394         for (i = 0; i < 3; i++)
 395                 rgb[i] = 0;
 396
 397         /*
 398          * Calculate how many we need to subtract to get to the pixel in
 399          * the top left corner of the mean window (considering the current
 400          * pixel as the center)
 401          */
 402         seek = deb_mean_win_size / 2;
 403
 404         /* Sum the values of the colors in the mean window */
 405
 406         dev_dbg(vdeb->dev,
 407                 "deb: %s: --- Calc pixel %dx%d, window mean %d, seek %d ---\n",
 408                 vdeb->sd.name, lin, col, vdeb->sink_fmt.height, seek);
 409
 410         /*
 411          * Iterate through all the lines in the mean window, start
 412          * with zero if the pixel is outside the frame and don't pass
 413          * the height when the pixel is in the bottom border of the
 414          * frame
 415          */
 416         for (wlin = seek > lin ? 0 : lin - seek;
 417              wlin < lin + seek + 1 && wlin < vdeb->sink_fmt.height;
 418              wlin++) {
 419
 420                 /*
 421                  * Iterate through all the columns in the mean window, start
 422                  * with zero if the pixel is outside the frame and don't pass
 423                  * the width when the pixel is in the right border of the
 424                  * frame
 425                  */
 426                 for (wcol = seek > col ? 0 : col - seek;
 427                      wcol < col + seek + 1 && wcol < vdeb->sink_fmt.width;
 428                      wcol++) {
 429                         enum vimc_deb_rgb_colors color;
 430                         unsigned int index;
 431
 432                         /* Check which color this pixel is */
 433                         color = vdeb->sink_pix_map->order[wlin % 2][wcol % 2];
 434
 435                         index = VIMC_FRAME_INDEX(wlin, wcol,
 436                                                  vdeb->sink_fmt.width,
 437                                                  vdeb->sink_bpp);
 438
 439                         dev_dbg(vdeb->dev,
 440                                 "deb: %s: RGB CALC: frame index %d, win pos %dx%d, color %d\n",
 441                                 vdeb->sd.name, index, wlin, wcol, color);
 442
 443                         /* Get its value */
 444                         rgb[color] = rgb[color] +
 445                                 vimc_deb_get_val(&frame[index], vdeb->sink_bpp);
 446
 447                         /* Save how many values we already added */
 448                         n_rgb[color]++;
 449
 450                         dev_dbg(vdeb->dev, "deb: %s: RGB CALC: val %d, n %d\n",
 451                                 vdeb->sd.name, rgb[color], n_rgb[color]);
 452                 }
 453         }
 454
 455         /* Calculate the mean */
 456         for (i = 0; i < 3; i++) {
 457                 dev_dbg(vdeb->dev,
 458                         "deb: %s: PRE CALC: %dx%d Color %d, val %d, n %d\n",
 459                         vdeb->sd.name, lin, col, i, rgb[i], n_rgb[i]);
 460
 461                 if (n_rgb[i])
 462                         rgb[i] = rgb[i] / n_rgb[i];
 463
 464                 dev_dbg(vdeb->dev,
 465                         "deb: %s: FINAL CALC: %dx%d Color %d, val %d\n",
 466                         vdeb->sd.name, lin, col, i, rgb[i]);
 467         }
 468 }
 469
 470 static void *vimc_deb_process_frame(struct vimc_ent_device *ved,
 471                                     const void *sink_frame)
 472 {
 473         struct vimc_deb_device *vdeb = container_of(ved, struct vimc_deb_device,
 474                                                     ved);
 475         unsigned int rgb[3];
 476         unsigned int i, j;
 477
 478         /* If the stream in this node is not active, just return */
 479         if (!vdeb->src_frame)
 480                 return ERR_PTR(-EINVAL);
 481
 482         for (i = 0; i < vdeb->sink_fmt.height; i++)
 483                 for (j = 0; j < vdeb->sink_fmt.width; j++) {
 484                         vimc_deb_calc_rgb_sink(vdeb, sink_frame, i, j, rgb);
 485                         vdeb->set_rgb_src(vdeb, i, j, rgb);
 486                 }
 487
 488         return vdeb->src_frame;
 489
 490 }
 491
 492 static void vimc_deb_comp_unbind(struct device *comp, struct device *master,
 493                                  void *master_data)
 494 {
 495         struct vimc_ent_device *ved = dev_get_drvdata(comp);
 496         struct vimc_deb_device *vdeb = container_of(ved, struct vimc_deb_device,
 497                                                     ved);
 498
 499         vimc_ent_sd_unregister(ved, &vdeb->sd);
 500         kfree(vdeb);
 501 }
 502
 503 static int vimc_deb_comp_bind(struct device *comp, struct device *master,
 504                               void *master_data)
 505 {
 506         struct v4l2_device *v4l2_dev = master_data;
 507         struct vimc_platform_data *pdata = comp->platform_data;
 508         struct vimc_deb_device *vdeb;
 509         int ret;
 510
 511         /* Allocate the vdeb struct */
 512         vdeb = kzalloc(sizeof(*vdeb), GFP_KERNEL);
 513         if (!vdeb)
 514                 return -ENOMEM;
 515
 516         /* Initialize ved and sd */
 517         ret = vimc_ent_sd_register(&vdeb->ved, &vdeb->sd, v4l2_dev,
 518                                    pdata->entity_name,
 519                                    MEDIA_ENT_F_PROC_VIDEO_PIXEL_ENC_CONV, 2,
 520                                    (const unsigned long[2]) {MEDIA_PAD_FL_SINK,
 521                                    MEDIA_PAD_FL_SOURCE},
 522                                    &vimc_deb_ops);
 523         if (ret) {
 524                 kfree(vdeb);
 525                 return ret;
 526         }
 527
 528         vdeb->ved.process_frame = vimc_deb_process_frame;
 529         dev_set_drvdata(comp, &vdeb->ved);
 530         vdeb->dev = comp;
 531
 532         /* Initialize the frame format */
 533         vdeb->sink_fmt = sink_fmt_default;
 534         /*
 535          * TODO: Add support for more output formats, we only support
 536          * RGB888 for now
 537          * NOTE: the src format is always the same as the sink, except
 538          * for the code
 539          */
 540         vdeb->src_code = MEDIA_BUS_FMT_RGB888_1X24;
 541         vdeb->set_rgb_src = vimc_deb_set_rgb_mbus_fmt_rgb888_1x24;
 542
 543         return 0;
 544 }
 545
 546 static const struct component_ops vimc_deb_comp_ops = {
 547         .bind = vimc_deb_comp_bind,
 548         .unbind = vimc_deb_comp_unbind,
 549 };
 550
 551 static int vimc_deb_probe(struct platform_device *pdev)
 552 {
 553         return component_add(&pdev->dev, &vimc_deb_comp_ops);
 554 }
 555
 556 static int vimc_deb_remove(struct platform_device *pdev)
 557 {
 558         component_del(&pdev->dev, &vimc_deb_comp_ops);
 559
 560         return 0;
 561 }
 562
 563 static const struct platform_device_id vimc_deb_driver_ids[] = {
 564         {
 565                 .name           = VIMC_DEB_DRV_NAME,
 566         },
 567         { }
 568 };
 569
 570 static struct platform_driver vimc_deb_pdrv = {
 571         .probe          = vimc_deb_probe,
 572         .remove         = vimc_deb_remove,
 573         .id_table       = vimc_deb_driver_ids,
 574         .driver         = {
 575                 .name   = VIMC_DEB_DRV_NAME,
 576         },
 577 };
 578
 579 module_platform_driver(vimc_deb_pdrv);
 580
 581 MODULE_DEVICE_TABLE(platform, vimc_deb_driver_ids);
 582
 583 MODULE_DESCRIPTION("Virtual Media Controller Driver (VIMC) Debayer");
 584 MODULE_AUTHOR("Helen Mae Koike Fornazier <helen.fornazier@gmail.com>");
 585 MODULE_LICENSE("GPL");