drivers/media/test-drivers/vimc/vimc-scaler.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * vimc-scaler.c Virtual Media Controller Driver
   4  *
   5  * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
   6  */
   7
   8 #include <linux/moduleparam.h>
   9 #include <linux/vmalloc.h>
  10 #include <linux/v4l2-mediabus.h>
  11 #include <media/v4l2-rect.h>
  12 #include <media/v4l2-subdev.h>
  13
  14 #include "vimc-common.h"
  15
  16 static unsigned int sca_mult = 3;
  17 module_param(sca_mult, uint, 0000);
  18 MODULE_PARM_DESC(sca_mult, " the image size multiplier");
  19
  20 #define MAX_ZOOM        8
  21
  22 #define VIMC_SCA_FMT_WIDTH_DEFAULT  640
  23 #define VIMC_SCA_FMT_HEIGHT_DEFAULT 480
  24
  25 struct vimc_sca_device {
  26         struct vimc_ent_device ved;
  27         struct v4l2_subdev sd;
  28         /* NOTE: the source fmt is the same as the sink
  29          * with the width and hight multiplied by mult
  30          */
  31         struct v4l2_mbus_framefmt sink_fmt;
  32         struct v4l2_rect crop_rect;
  33         /* Values calculated when the stream starts */
  34         u8 *src_frame;
  35         unsigned int src_line_size;
  36         unsigned int bpp;
  37         struct media_pad pads[2];
  38 };
  39
  40 static const struct v4l2_mbus_framefmt sink_fmt_default = {
  41         .width = VIMC_SCA_FMT_WIDTH_DEFAULT,
  42         .height = VIMC_SCA_FMT_HEIGHT_DEFAULT,
  43         .code = MEDIA_BUS_FMT_RGB888_1X24,
  44         .field = V4L2_FIELD_NONE,
  45         .colorspace = V4L2_COLORSPACE_SRGB,
  46 };
  47
  48 static const struct v4l2_rect crop_rect_default = {
  49         .width = VIMC_SCA_FMT_WIDTH_DEFAULT,
  50         .height = VIMC_SCA_FMT_HEIGHT_DEFAULT,
  51         .top = 0,
  52         .left = 0,
  53 };
  54
  55 static const struct v4l2_rect crop_rect_min = {
  56         .width = VIMC_FRAME_MIN_WIDTH,
  57         .height = VIMC_FRAME_MIN_HEIGHT,
  58         .top = 0,
  59         .left = 0,
  60 };
  61
  62 static struct v4l2_rect
  63 vimc_sca_get_crop_bound_sink(const struct v4l2_mbus_framefmt *sink_fmt)
  64 {
  65         /* Get the crop bounds to clamp the crop rectangle correctly */
  66         struct v4l2_rect r = {
  67                 .left = 0,
  68                 .top = 0,
  69                 .width = sink_fmt->width,
  70                 .height = sink_fmt->height,
  71         };
  72         return r;
  73 }
  74
  75 static void vimc_sca_adjust_sink_crop(struct v4l2_rect *r,
  76                                       const struct v4l2_mbus_framefmt *sink_fmt)
  77 {
  78         const struct v4l2_rect sink_rect =
  79                 vimc_sca_get_crop_bound_sink(sink_fmt);
  80
  81         /* Disallow rectangles smaller than the minimal one. */
  82         v4l2_rect_set_min_size(r, &crop_rect_min);
  83         v4l2_rect_map_inside(r, &sink_rect);
  84 }
  85
  86 static int vimc_sca_init_cfg(struct v4l2_subdev *sd,
  87                              struct v4l2_subdev_pad_config *cfg)
  88 {
  89         struct v4l2_mbus_framefmt *mf;
  90         struct v4l2_rect *r;
  91         unsigned int i;
  92
  93         mf = v4l2_subdev_get_try_format(sd, cfg, 0);
  94         *mf = sink_fmt_default;
  95
  96         r = v4l2_subdev_get_try_crop(sd, cfg, 0);
  97         *r = crop_rect_default;
  98
  99         for (i = 1; i < sd->entity.num_pads; i++) {
 100                 mf = v4l2_subdev_get_try_format(sd, cfg, i);
 101                 *mf = sink_fmt_default;
 102                 mf->width = mf->width * sca_mult;
 103                 mf->height = mf->height * sca_mult;
 104         }
 105
 106         return 0;
 107 }
 108
 109 static int vimc_sca_enum_mbus_code(struct v4l2_subdev *sd,
 110                                    struct v4l2_subdev_pad_config *cfg,
 111                                    struct v4l2_subdev_mbus_code_enum *code)
 112 {
 113         u32 mbus_code = vimc_mbus_code_by_index(code->index);
 114         const struct vimc_pix_map *vpix;
 115
 116         if (!mbus_code)
 117                 return -EINVAL;
 118
 119         vpix = vimc_pix_map_by_code(mbus_code);
 120
 121         /* We don't support bayer format */
 122         if (!vpix || vpix->bayer)
 123                 return -EINVAL;
 124
 125         code->code = mbus_code;
 126
 127         return 0;
 128 }
 129
 130 static int vimc_sca_enum_frame_size(struct v4l2_subdev *sd,
 131                                     struct v4l2_subdev_pad_config *cfg,
 132                                     struct v4l2_subdev_frame_size_enum *fse)
 133 {
 134         const struct vimc_pix_map *vpix;
 135
 136         if (fse->index)
 137                 return -EINVAL;
 138
 139         /* Only accept code in the pix map table in non bayer format */
 140         vpix = vimc_pix_map_by_code(fse->code);
 141         if (!vpix || vpix->bayer)
 142                 return -EINVAL;
 143
 144         fse->min_width = VIMC_FRAME_MIN_WIDTH;
 145         fse->min_height = VIMC_FRAME_MIN_HEIGHT;
 146
 147         if (VIMC_IS_SINK(fse->pad)) {
 148                 fse->max_width = VIMC_FRAME_MAX_WIDTH;
 149                 fse->max_height = VIMC_FRAME_MAX_HEIGHT;
 150         } else {
 151                 fse->max_width = VIMC_FRAME_MAX_WIDTH * MAX_ZOOM;
 152                 fse->max_height = VIMC_FRAME_MAX_HEIGHT * MAX_ZOOM;
 153         }
 154
 155         return 0;
 156 }
 157
 158 static int vimc_sca_get_fmt(struct v4l2_subdev *sd,
 159                             struct v4l2_subdev_pad_config *cfg,
 160                             struct v4l2_subdev_format *format)
 161 {
 162         struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd);
 163         struct v4l2_rect *crop_rect;
 164
 165         /* Get the current sink format */
 166         if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
 167                 format->format = *v4l2_subdev_get_try_format(sd, cfg, 0);
 168                 crop_rect = v4l2_subdev_get_try_crop(sd, cfg, 0);
 169         } else {
 170                 format->format = vsca->sink_fmt;
 171                 crop_rect = &vsca->crop_rect;
 172         }
 173
 174         /* Scale the frame size for the source pad */
 175         if (VIMC_IS_SRC(format->pad)) {
 176                 format->format.width = crop_rect->width * sca_mult;
 177                 format->format.height = crop_rect->height * sca_mult;
 178         }
 179
 180         return 0;
 181 }
 182
 183 static void vimc_sca_adjust_sink_fmt(struct v4l2_mbus_framefmt *fmt)
 184 {
 185         const struct vimc_pix_map *vpix;
 186
 187         /* Only accept code in the pix map table in non bayer format */
 188         vpix = vimc_pix_map_by_code(fmt->code);
 189         if (!vpix || vpix->bayer)
 190                 fmt->code = sink_fmt_default.code;
 191
 192         fmt->width = clamp_t(u32, fmt->width, VIMC_FRAME_MIN_WIDTH,
 193                              VIMC_FRAME_MAX_WIDTH) & ~1;
 194         fmt->height = clamp_t(u32, fmt->height, VIMC_FRAME_MIN_HEIGHT,
 195                               VIMC_FRAME_MAX_HEIGHT) & ~1;
 196
 197         if (fmt->field == V4L2_FIELD_ANY)
 198                 fmt->field = sink_fmt_default.field;
 199
 200         vimc_colorimetry_clamp(fmt);
 201 }
 202
 203 static int vimc_sca_set_fmt(struct v4l2_subdev *sd,
 204                             struct v4l2_subdev_pad_config *cfg,
 205                             struct v4l2_subdev_format *fmt)
 206 {
 207         struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd);
 208         struct v4l2_mbus_framefmt *sink_fmt;
 209         struct v4l2_rect *crop_rect;
 210
 211         if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
 212                 /* Do not change the format while stream is on */
 213                 if (vsca->src_frame)
 214                         return -EBUSY;
 215
 216                 sink_fmt = &vsca->sink_fmt;
 217                 crop_rect = &vsca->crop_rect;
 218         } else {
 219                 sink_fmt = v4l2_subdev_get_try_format(sd, cfg, 0);
 220                 crop_rect = v4l2_subdev_get_try_crop(sd, cfg, 0);
 221         }
 222
 223         /*
 224          * Do not change the format of the source pad,
 225          * it is propagated from the sink
 226          */
 227         if (VIMC_IS_SRC(fmt->pad)) {
 228                 fmt->format = *sink_fmt;
 229                 fmt->format.width = crop_rect->width * sca_mult;
 230                 fmt->format.height = crop_rect->height * sca_mult;
 231         } else {
 232                 /* Set the new format in the sink pad */
 233                 vimc_sca_adjust_sink_fmt(&fmt->format);
 234
 235                 dev_dbg(vsca->ved.dev, "%s: sink format update: "
 236                         "old:%dx%d (0x%x, %d, %d, %d, %d) "
 237                         "new:%dx%d (0x%x, %d, %d, %d, %d)\n", vsca->sd.name,
 238                         /* old */
 239                         sink_fmt->width, sink_fmt->height, sink_fmt->code,
 240                         sink_fmt->colorspace, sink_fmt->quantization,
 241                         sink_fmt->xfer_func, sink_fmt->ycbcr_enc,
 242                         /* new */
 243                         fmt->format.width, fmt->format.height, fmt->format.code,
 244                         fmt->format.colorspace, fmt->format.quantization,
 245                         fmt->format.xfer_func, fmt->format.ycbcr_enc);
 246
 247                 *sink_fmt = fmt->format;
 248
 249                 /* Do the crop, but respect the current bounds */
 250                 vimc_sca_adjust_sink_crop(crop_rect, sink_fmt);
 251         }
 252
 253         return 0;
 254 }
 255
 256 static int vimc_sca_get_selection(struct v4l2_subdev *sd,
 257                                   struct v4l2_subdev_pad_config *cfg,
 258                                   struct v4l2_subdev_selection *sel)
 259 {
 260         struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd);
 261         struct v4l2_mbus_framefmt *sink_fmt;
 262         struct v4l2_rect *crop_rect;
 263
 264         if (VIMC_IS_SRC(sel->pad))
 265                 return -EINVAL;
 266
 267         if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
 268                 sink_fmt = &vsca->sink_fmt;
 269                 crop_rect = &vsca->crop_rect;
 270         } else {
 271                 sink_fmt = v4l2_subdev_get_try_format(sd, cfg, 0);
 272                 crop_rect = v4l2_subdev_get_try_crop(sd, cfg, 0);
 273         }
 274
 275         switch (sel->target) {
 276         case V4L2_SEL_TGT_CROP:
 277                 sel->r = *crop_rect;
 278                 break;
 279         case V4L2_SEL_TGT_CROP_BOUNDS:
 280                 sel->r = vimc_sca_get_crop_bound_sink(sink_fmt);
 281                 break;
 282         default:
 283                 return -EINVAL;
 284         }
 285
 286         return 0;
 287 }
 288
 289 static int vimc_sca_set_selection(struct v4l2_subdev *sd,
 290                                   struct v4l2_subdev_pad_config *cfg,
 291                                   struct v4l2_subdev_selection *sel)
 292 {
 293         struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd);
 294         struct v4l2_mbus_framefmt *sink_fmt;
 295         struct v4l2_rect *crop_rect;
 296
 297         if (VIMC_IS_SRC(sel->pad))
 298                 return -EINVAL;
 299
 300         if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
 301                 /* Do not change the format while stream is on */
 302                 if (vsca->src_frame)
 303                         return -EBUSY;
 304
 305                 crop_rect = &vsca->crop_rect;
 306                 sink_fmt = &vsca->sink_fmt;
 307         } else {
 308                 crop_rect = v4l2_subdev_get_try_crop(sd, cfg, 0);
 309                 sink_fmt = v4l2_subdev_get_try_format(sd, cfg, 0);
 310         }
 311
 312         switch (sel->target) {
 313         case V4L2_SEL_TGT_CROP:
 314                 /* Do the crop, but respect the current bounds */
 315                 vimc_sca_adjust_sink_crop(&sel->r, sink_fmt);
 316                 *crop_rect = sel->r;
 317                 break;
 318         default:
 319                 return -EINVAL;
 320         }
 321
 322         return 0;
 323 }
 324
 325 static const struct v4l2_subdev_pad_ops vimc_sca_pad_ops = {
 326         .init_cfg               = vimc_sca_init_cfg,
 327         .enum_mbus_code         = vimc_sca_enum_mbus_code,
 328         .enum_frame_size        = vimc_sca_enum_frame_size,
 329         .get_fmt                = vimc_sca_get_fmt,
 330         .set_fmt                = vimc_sca_set_fmt,
 331         .get_selection          = vimc_sca_get_selection,
 332         .set_selection          = vimc_sca_set_selection,
 333 };
 334
 335 static int vimc_sca_s_stream(struct v4l2_subdev *sd, int enable)
 336 {
 337         struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd);
 338
 339         if (enable) {
 340                 const struct vimc_pix_map *vpix;
 341                 unsigned int frame_size;
 342
 343                 if (vsca->src_frame)
 344                         return 0;
 345
 346                 /* Save the bytes per pixel of the sink */
 347                 vpix = vimc_pix_map_by_code(vsca->sink_fmt.code);
 348                 vsca->bpp = vpix->bpp;
 349
 350                 /* Calculate the width in bytes of the src frame */
 351                 vsca->src_line_size = vsca->crop_rect.width *
 352                                       sca_mult * vsca->bpp;
 353
 354                 /* Calculate the frame size of the source pad */
 355                 frame_size = vsca->src_line_size * vsca->crop_rect.height *
 356                              sca_mult;
 357
 358                 /* Allocate the frame buffer. Use vmalloc to be able to
 359                  * allocate a large amount of memory
 360                  */
 361                 vsca->src_frame = vmalloc(frame_size);
 362                 if (!vsca->src_frame)
 363                         return -ENOMEM;
 364
 365         } else {
 366                 if (!vsca->src_frame)
 367                         return 0;
 368
 369                 vfree(vsca->src_frame);
 370                 vsca->src_frame = NULL;
 371         }
 372
 373         return 0;
 374 }
 375
 376 static const struct v4l2_subdev_video_ops vimc_sca_video_ops = {
 377         .s_stream = vimc_sca_s_stream,
 378 };
 379
 380 static const struct v4l2_subdev_ops vimc_sca_ops = {
 381         .pad = &vimc_sca_pad_ops,
 382         .video = &vimc_sca_video_ops,
 383 };
 384
 385 static void vimc_sca_fill_pix(u8 *const ptr,
 386                               const u8 *const pixel,
 387                               const unsigned int bpp)
 388 {
 389         unsigned int i;
 390
 391         /* copy the pixel to the pointer */
 392         for (i = 0; i < bpp; i++)
 393                 ptr[i] = pixel[i];
 394 }
 395
 396 static void vimc_sca_scale_pix(const struct vimc_sca_device *const vsca,
 397                                unsigned int lin, unsigned int col,
 398                                const u8 *const sink_frame)
 399 {
 400         const struct v4l2_rect crop_rect = vsca->crop_rect;
 401         unsigned int i, j, index;
 402         const u8 *pixel;
 403
 404         /* Point to the pixel value in position (lin, col) in the sink frame */
 405         index = VIMC_FRAME_INDEX(lin, col,
 406                                  vsca->sink_fmt.width,
 407                                  vsca->bpp);
 408         pixel = &sink_frame[index];
 409
 410         dev_dbg(vsca->ved.dev,
 411                 "sca: %s: --- scale_pix sink pos %dx%d, index %d ---\n",
 412                 vsca->sd.name, lin, col, index);
 413
 414         /* point to the place we are going to put the first pixel
 415          * in the scaled src frame
 416          */
 417         lin -= crop_rect.top;
 418         col -= crop_rect.left;
 419         index = VIMC_FRAME_INDEX(lin * sca_mult, col * sca_mult,
 420                                  crop_rect.width * sca_mult, vsca->bpp);
 421
 422         dev_dbg(vsca->ved.dev, "sca: %s: scale_pix src pos %dx%d, index %d\n",
 423                 vsca->sd.name, lin * sca_mult, col * sca_mult, index);
 424
 425         /* Repeat this pixel mult times */
 426         for (i = 0; i < sca_mult; i++) {
 427                 /* Iterate through each beginning of a
 428                  * pixel repetition in a line
 429                  */
 430                 for (j = 0; j < sca_mult * vsca->bpp; j += vsca->bpp) {
 431                         dev_dbg(vsca->ved.dev,
 432                                 "sca: %s: sca: scale_pix src pos %d\n",
 433                                 vsca->sd.name, index + j);
 434
 435                         /* copy the pixel to the position index + j */
 436                         vimc_sca_fill_pix(&vsca->src_frame[index + j],
 437                                           pixel, vsca->bpp);
 438                 }
 439
 440                 /* move the index to the next line */
 441                 index += vsca->src_line_size;
 442         }
 443 }
 444
 445 static void vimc_sca_fill_src_frame(const struct vimc_sca_device *const vsca,
 446                                     const u8 *const sink_frame)
 447 {
 448         const struct v4l2_rect r = vsca->crop_rect;
 449         unsigned int i, j;
 450
 451         /* Scale each pixel from the original sink frame */
 452         /* TODO: implement scale down, only scale up is supported for now */
 453         for (i = r.top; i < r.top + r.height; i++)
 454                 for (j = r.left; j < r.left + r.width; j++)
 455                         vimc_sca_scale_pix(vsca, i, j, sink_frame);
 456 }
 457
 458 static void *vimc_sca_process_frame(struct vimc_ent_device *ved,
 459                                     const void *sink_frame)
 460 {
 461         struct vimc_sca_device *vsca = container_of(ved, struct vimc_sca_device,
 462                                                     ved);
 463
 464         /* If the stream in this node is not active, just return */
 465         if (!vsca->src_frame)
 466                 return ERR_PTR(-EINVAL);
 467
 468         vimc_sca_fill_src_frame(vsca, sink_frame);
 469
 470         return vsca->src_frame;
 471 };
 472
 473 static void vimc_sca_release(struct vimc_ent_device *ved)
 474 {
 475         struct vimc_sca_device *vsca =
 476                 container_of(ved, struct vimc_sca_device, ved);
 477
 478         media_entity_cleanup(vsca->ved.ent);
 479         kfree(vsca);
 480 }
 481
 482 static struct vimc_ent_device *vimc_sca_add(struct vimc_device *vimc,
 483                                             const char *vcfg_name)
 484 {
 485         struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;
 486         struct vimc_sca_device *vsca;
 487         int ret;
 488
 489         /* Allocate the vsca struct */
 490         vsca = kzalloc(sizeof(*vsca), GFP_KERNEL);
 491         if (!vsca)
 492                 return ERR_PTR(-ENOMEM);
 493
 494         /* Initialize ved and sd */
 495         vsca->pads[0].flags = MEDIA_PAD_FL_SINK;
 496         vsca->pads[1].flags = MEDIA_PAD_FL_SOURCE;
 497
 498         ret = vimc_ent_sd_register(&vsca->ved, &vsca->sd, v4l2_dev,
 499                                    vcfg_name,
 500                                    MEDIA_ENT_F_PROC_VIDEO_SCALER, 2,
 501                                    vsca->pads, &vimc_sca_ops);
 502         if (ret) {
 503                 kfree(vsca);
 504                 return ERR_PTR(ret);
 505         }
 506
 507         vsca->ved.process_frame = vimc_sca_process_frame;
 508         vsca->ved.dev = vimc->mdev.dev;
 509
 510         /* Initialize the frame format */
 511         vsca->sink_fmt = sink_fmt_default;
 512
 513         /* Initialize the crop selection */
 514         vsca->crop_rect = crop_rect_default;
 515
 516         return &vsca->ved;
 517 }
 518
 519 struct vimc_ent_type vimc_sca_type = {
 520         .add = vimc_sca_add,
 521         .release = vimc_sca_release
 522 };