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