test-drivers/vimc/vimc-scaler.c

0001 // SPDX-License-Identifier: GPL-2.0-or-later
0002 /*
0003  * vimc-scaler.c Virtual Media Controller Driver
0004  *
0005  * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
0006  */
0007
0008 #include <linux/moduleparam.h>
0009 #include <linux/string.h>
0010 #include <linux/vmalloc.h>
0011 #include <linux/v4l2-mediabus.h>
0012 #include <media/v4l2-rect.h>
0013 #include <media/v4l2-subdev.h>
0014
0015 #include "vimc-common.h"
0016
0017 /* Pad identifier */
0018 enum vic_sca_pad {
0019     VIMC_SCALER_SINK = 0,
0020     VIMC_SCALER_SRC = 1,
0021 };
0022
0023 #define VIMC_SCALER_FMT_WIDTH_DEFAULT  640
0024 #define VIMC_SCALER_FMT_HEIGHT_DEFAULT 480
0025
0026 struct vimc_scaler_device {
0027     struct vimc_ent_device ved;
0028     struct v4l2_subdev sd;
0029     struct v4l2_rect crop_rect;
0030     /* Frame format for both sink and src pad */
0031     struct v4l2_mbus_framefmt fmt[2];
0032     /* Values calculated when the stream starts */
0033     u8 *src_frame;
0034     unsigned int bpp;
0035     struct media_pad pads[2];
0036 };
0037
0038 static const struct v4l2_mbus_framefmt fmt_default = {
0039     .width = VIMC_SCALER_FMT_WIDTH_DEFAULT,
0040     .height = VIMC_SCALER_FMT_HEIGHT_DEFAULT,
0041     .code = MEDIA_BUS_FMT_RGB888_1X24,
0042     .field = V4L2_FIELD_NONE,
0043     .colorspace = V4L2_COLORSPACE_SRGB,
0044 };
0045
0046 static const struct v4l2_rect crop_rect_default = {
0047     .width = VIMC_SCALER_FMT_WIDTH_DEFAULT,
0048     .height = VIMC_SCALER_FMT_HEIGHT_DEFAULT,
0049     .top = 0,
0050     .left = 0,
0051 };
0052
0053 static const struct v4l2_rect crop_rect_min = {
0054     .width = VIMC_FRAME_MIN_WIDTH,
0055     .height = VIMC_FRAME_MIN_HEIGHT,
0056     .top = 0,
0057     .left = 0,
0058 };
0059
0060 static struct v4l2_rect
0061 vimc_scaler_get_crop_bound_sink(const struct v4l2_mbus_framefmt *sink_fmt)
0062 {
0063     /* Get the crop bounds to clamp the crop rectangle correctly */
0064     struct v4l2_rect r = {
0065         .left = 0,
0066         .top = 0,
0067         .width = sink_fmt->width,
0068         .height = sink_fmt->height,
0069     };
0070     return r;
0071 }
0072
0073 static int vimc_scaler_init_cfg(struct v4l2_subdev *sd,
0074                  struct v4l2_subdev_state *sd_state)
0075 {
0076     struct v4l2_mbus_framefmt *mf;
0077     struct v4l2_rect *r;
0078     unsigned int i;
0079
0080     for (i = 0; i < sd->entity.num_pads; i++) {
0081         mf = v4l2_subdev_get_try_format(sd, sd_state, i);
0082         *mf = fmt_default;
0083     }
0084
0085     r = v4l2_subdev_get_try_crop(sd, sd_state, VIMC_SCALER_SINK);
0086     *r = crop_rect_default;
0087
0088     return 0;
0089 }
0090
0091 static int vimc_scaler_enum_mbus_code(struct v4l2_subdev *sd,
0092                    struct v4l2_subdev_state *sd_state,
0093                    struct v4l2_subdev_mbus_code_enum *code)
0094 {
0095     u32 mbus_code = vimc_mbus_code_by_index(code->index);
0096     const struct vimc_pix_map *vpix;
0097
0098     if (!mbus_code)
0099         return -EINVAL;
0100
0101     vpix = vimc_pix_map_by_code(mbus_code);
0102
0103     /* We don't support bayer format */
0104     if (!vpix || vpix->bayer)
0105         return -EINVAL;
0106
0107     code->code = mbus_code;
0108
0109     return 0;
0110 }
0111
0112 static int vimc_scaler_enum_frame_size(struct v4l2_subdev *sd,
0113                     struct v4l2_subdev_state *sd_state,
0114                     struct v4l2_subdev_frame_size_enum *fse)
0115 {
0116     const struct vimc_pix_map *vpix;
0117
0118     if (fse->index)
0119         return -EINVAL;
0120
0121     /* Only accept code in the pix map table in non bayer format */
0122     vpix = vimc_pix_map_by_code(fse->code);
0123     if (!vpix || vpix->bayer)
0124         return -EINVAL;
0125
0126     fse->min_width = VIMC_FRAME_MIN_WIDTH;
0127     fse->min_height = VIMC_FRAME_MIN_HEIGHT;
0128
0129     fse->max_width = VIMC_FRAME_MAX_WIDTH;
0130     fse->max_height = VIMC_FRAME_MAX_HEIGHT;
0131
0132     return 0;
0133 }
0134
0135 static struct v4l2_mbus_framefmt *
0136 vimc_scaler_pad_format(struct vimc_scaler_device *vscaler,
0137             struct v4l2_subdev_state *sd_state, u32 pad,
0138             enum v4l2_subdev_format_whence which)
0139 {
0140     if (which == V4L2_SUBDEV_FORMAT_TRY)
0141         return v4l2_subdev_get_try_format(&vscaler->sd, sd_state, pad);
0142     else
0143         return &vscaler->fmt[pad];
0144 }
0145
0146 static struct v4l2_rect *
0147 vimc_scaler_pad_crop(struct vimc_scaler_device *vscaler,
0148           struct v4l2_subdev_state *sd_state,
0149           enum v4l2_subdev_format_whence which)
0150 {
0151     if (which == V4L2_SUBDEV_FORMAT_TRY)
0152         return v4l2_subdev_get_try_crop(&vscaler->sd, sd_state,
0153                         VIMC_SCALER_SINK);
0154     else
0155         return &vscaler->crop_rect;
0156 }
0157
0158 static int vimc_scaler_get_fmt(struct v4l2_subdev *sd,
0159                 struct v4l2_subdev_state *sd_state,
0160                 struct v4l2_subdev_format *format)
0161 {
0162     struct vimc_scaler_device *vscaler = v4l2_get_subdevdata(sd);
0163
0164     format->format = *vimc_scaler_pad_format(vscaler, sd_state, format->pad,
0165                           format->which);
0166     return 0;
0167 }
0168
0169 static int vimc_scaler_set_fmt(struct v4l2_subdev *sd,
0170                 struct v4l2_subdev_state *sd_state,
0171                 struct v4l2_subdev_format *format)
0172 {
0173     struct vimc_scaler_device *vscaler = v4l2_get_subdevdata(sd);
0174     struct v4l2_mbus_framefmt *fmt;
0175
0176     /* Do not change the active format while stream is on */
0177     if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE && vscaler->src_frame)
0178         return -EBUSY;
0179
0180     fmt = vimc_scaler_pad_format(vscaler, sd_state, format->pad, format->which);
0181
0182     /*
0183      * The media bus code and colorspace can only be changed on the sink
0184      * pad, the source pad only follows.
0185      */
0186     if (format->pad == VIMC_SCALER_SINK) {
0187         const struct vimc_pix_map *vpix;
0188
0189         /* Only accept code in the pix map table in non bayer format. */
0190         vpix = vimc_pix_map_by_code(format->format.code);
0191         if (vpix && !vpix->bayer)
0192             fmt->code = format->format.code;
0193         else
0194             fmt->code = fmt_default.code;
0195
0196         /* Clamp the colorspace to valid values. */
0197         fmt->colorspace = format->format.colorspace;
0198         fmt->ycbcr_enc = format->format.ycbcr_enc;
0199         fmt->quantization = format->format.quantization;
0200         fmt->xfer_func = format->format.xfer_func;
0201         vimc_colorimetry_clamp(fmt);
0202     }
0203
0204     /* Clamp and align the width and height */
0205     fmt->width = clamp_t(u32, format->format.width, VIMC_FRAME_MIN_WIDTH,
0206                  VIMC_FRAME_MAX_WIDTH) & ~1;
0207     fmt->height = clamp_t(u32, format->format.height, VIMC_FRAME_MIN_HEIGHT,
0208                   VIMC_FRAME_MAX_HEIGHT) & ~1;
0209
0210     /*
0211      * Propagate the sink pad format to the crop rectangle and the source
0212      * pad.
0213      */
0214     if (format->pad == VIMC_SCALER_SINK) {
0215         struct v4l2_mbus_framefmt *src_fmt;
0216         struct v4l2_rect *crop;
0217
0218         crop = vimc_scaler_pad_crop(vscaler, sd_state, format->which);
0219         crop->width = fmt->width;
0220         crop->height = fmt->height;
0221         crop->top = 0;
0222         crop->left = 0;
0223
0224         src_fmt = vimc_scaler_pad_format(vscaler, sd_state, VIMC_SCALER_SRC,
0225                           format->which);
0226         *src_fmt = *fmt;
0227     }
0228
0229     format->format = *fmt;
0230
0231     return 0;
0232 }
0233
0234 static int vimc_scaler_get_selection(struct v4l2_subdev *sd,
0235                   struct v4l2_subdev_state *sd_state,
0236                   struct v4l2_subdev_selection *sel)
0237 {
0238     struct vimc_scaler_device *vscaler = v4l2_get_subdevdata(sd);
0239     struct v4l2_mbus_framefmt *sink_fmt;
0240
0241     if (VIMC_IS_SRC(sel->pad))
0242         return -EINVAL;
0243
0244     switch (sel->target) {
0245     case V4L2_SEL_TGT_CROP:
0246         sel->r = *vimc_scaler_pad_crop(vscaler, sd_state, sel->which);
0247         break;
0248     case V4L2_SEL_TGT_CROP_BOUNDS:
0249         sink_fmt = vimc_scaler_pad_format(vscaler, sd_state, VIMC_SCALER_SINK,
0250                            sel->which);
0251         sel->r = vimc_scaler_get_crop_bound_sink(sink_fmt);
0252         break;
0253     default:
0254         return -EINVAL;
0255     }
0256
0257     return 0;
0258 }
0259
0260 static void vimc_scaler_adjust_sink_crop(struct v4l2_rect *r,
0261                       const struct v4l2_mbus_framefmt *sink_fmt)
0262 {
0263     const struct v4l2_rect sink_rect =
0264         vimc_scaler_get_crop_bound_sink(sink_fmt);
0265
0266     /* Disallow rectangles smaller than the minimal one. */
0267     v4l2_rect_set_min_size(r, &crop_rect_min);
0268     v4l2_rect_map_inside(r, &sink_rect);
0269 }
0270
0271 static int vimc_scaler_set_selection(struct v4l2_subdev *sd,
0272                   struct v4l2_subdev_state *sd_state,
0273                   struct v4l2_subdev_selection *sel)
0274 {
0275     struct vimc_scaler_device *vscaler = v4l2_get_subdevdata(sd);
0276     struct v4l2_mbus_framefmt *sink_fmt;
0277     struct v4l2_rect *crop_rect;
0278
0279     /* Only support setting the crop of the sink pad */
0280     if (VIMC_IS_SRC(sel->pad) || sel->target != V4L2_SEL_TGT_CROP)
0281         return -EINVAL;
0282
0283     if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE && vscaler->src_frame)
0284         return -EBUSY;
0285
0286     crop_rect = vimc_scaler_pad_crop(vscaler, sd_state, sel->which);
0287     sink_fmt = vimc_scaler_pad_format(vscaler, sd_state, VIMC_SCALER_SINK,
0288                        sel->which);
0289     vimc_scaler_adjust_sink_crop(&sel->r, sink_fmt);
0290     *crop_rect = sel->r;
0291
0292     return 0;
0293 }
0294
0295 static const struct v4l2_subdev_pad_ops vimc_scaler_pad_ops = {
0296     .init_cfg       = vimc_scaler_init_cfg,
0297     .enum_mbus_code     = vimc_scaler_enum_mbus_code,
0298     .enum_frame_size    = vimc_scaler_enum_frame_size,
0299     .get_fmt        = vimc_scaler_get_fmt,
0300     .set_fmt        = vimc_scaler_set_fmt,
0301     .get_selection      = vimc_scaler_get_selection,
0302     .set_selection      = vimc_scaler_set_selection,
0303 };
0304
0305 static int vimc_scaler_s_stream(struct v4l2_subdev *sd, int enable)
0306 {
0307     struct vimc_scaler_device *vscaler = v4l2_get_subdevdata(sd);
0308
0309     if (enable) {
0310         const struct vimc_pix_map *vpix;
0311         unsigned int frame_size;
0312
0313         if (vscaler->src_frame)
0314             return 0;
0315
0316         /* Save the bytes per pixel of the sink */
0317         vpix = vimc_pix_map_by_code(vscaler->fmt[VIMC_SCALER_SINK].code);
0318         vscaler->bpp = vpix->bpp;
0319
0320         /* Calculate the frame size of the source pad */
0321         frame_size = vscaler->fmt[VIMC_SCALER_SRC].width
0322                * vscaler->fmt[VIMC_SCALER_SRC].height * vscaler->bpp;
0323
0324         /* Allocate the frame buffer. Use vmalloc to be able to
0325          * allocate a large amount of memory
0326          */
0327         vscaler->src_frame = vmalloc(frame_size);
0328         if (!vscaler->src_frame)
0329             return -ENOMEM;
0330
0331     } else {
0332         if (!vscaler->src_frame)
0333             return 0;
0334
0335         vfree(vscaler->src_frame);
0336         vscaler->src_frame = NULL;
0337     }
0338
0339     return 0;
0340 }
0341
0342 static const struct v4l2_subdev_video_ops vimc_scaler_video_ops = {
0343     .s_stream = vimc_scaler_s_stream,
0344 };
0345
0346 static const struct v4l2_subdev_ops vimc_scaler_ops = {
0347     .pad = &vimc_scaler_pad_ops,
0348     .video = &vimc_scaler_video_ops,
0349 };
0350
0351 static void vimc_scaler_fill_src_frame(const struct vimc_scaler_device *const vscaler,
0352                     const u8 *const sink_frame)
0353 {
0354     const struct v4l2_mbus_framefmt *src_fmt = &vscaler->fmt[VIMC_SCALER_SRC];
0355     const struct v4l2_rect *r = &vscaler->crop_rect;
0356     unsigned int snk_width = vscaler->fmt[VIMC_SCALER_SINK].width;
0357     unsigned int src_x, src_y;
0358     u8 *walker = vscaler->src_frame;
0359
0360     /* Set each pixel at the src_frame to its sink_frame equivalent */
0361     for (src_y = 0; src_y < src_fmt->height; src_y++) {
0362         unsigned int snk_y, y_offset;
0363
0364         snk_y = (src_y * r->height) / src_fmt->height + r->top;
0365         y_offset = snk_y * snk_width * vscaler->bpp;
0366
0367         for (src_x = 0; src_x < src_fmt->width; src_x++) {
0368             unsigned int snk_x, x_offset, index;
0369
0370             snk_x = (src_x * r->width) / src_fmt->width + r->left;
0371             x_offset = snk_x * vscaler->bpp;
0372             index = y_offset + x_offset;
0373             memcpy(walker, &sink_frame[index], vscaler->bpp);
0374             walker += vscaler->bpp;
0375         }
0376     }
0377 }
0378
0379 static void *vimc_scaler_process_frame(struct vimc_ent_device *ved,
0380                     const void *sink_frame)
0381 {
0382     struct vimc_scaler_device *vscaler = container_of(ved, struct vimc_scaler_device,
0383                             ved);
0384
0385     /* If the stream in this node is not active, just return */
0386     if (!vscaler->src_frame)
0387         return ERR_PTR(-EINVAL);
0388
0389     vimc_scaler_fill_src_frame(vscaler, sink_frame);
0390
0391     return vscaler->src_frame;
0392 };
0393
0394 static void vimc_scaler_release(struct vimc_ent_device *ved)
0395 {
0396     struct vimc_scaler_device *vscaler =
0397         container_of(ved, struct vimc_scaler_device, ved);
0398
0399     media_entity_cleanup(vscaler->ved.ent);
0400     kfree(vscaler);
0401 }
0402
0403 static struct vimc_ent_device *vimc_scaler_add(struct vimc_device *vimc,
0404                         const char *vcfg_name)
0405 {
0406     struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;
0407     struct vimc_scaler_device *vscaler;
0408     int ret;
0409
0410     /* Allocate the vscaler struct */
0411     vscaler = kzalloc(sizeof(*vscaler), GFP_KERNEL);
0412     if (!vscaler)
0413         return ERR_PTR(-ENOMEM);
0414
0415     /* Initialize ved and sd */
0416     vscaler->pads[VIMC_SCALER_SINK].flags = MEDIA_PAD_FL_SINK;
0417     vscaler->pads[VIMC_SCALER_SRC].flags = MEDIA_PAD_FL_SOURCE;
0418
0419     ret = vimc_ent_sd_register(&vscaler->ved, &vscaler->sd, v4l2_dev,
0420                    vcfg_name,
0421                    MEDIA_ENT_F_PROC_VIDEO_SCALER, 2,
0422                    vscaler->pads, &vimc_scaler_ops);
0423     if (ret) {
0424         kfree(vscaler);
0425         return ERR_PTR(ret);
0426     }
0427
0428     vscaler->ved.process_frame = vimc_scaler_process_frame;
0429     vscaler->ved.dev = vimc->mdev.dev;
0430
0431     /* Initialize the frame format */
0432     vscaler->fmt[VIMC_SCALER_SINK] = fmt_default;
0433     vscaler->fmt[VIMC_SCALER_SRC] = fmt_default;
0434
0435     /* Initialize the crop selection */
0436     vscaler->crop_rect = crop_rect_default;
0437
0438     return &vscaler->ved;
0439 }
0440
0441 struct vimc_ent_type vimc_scaler_type = {
0442     .add = vimc_scaler_add,
0443     .release = vimc_scaler_release
0444 };