linux/drivers/media/platform/qcom/camss/camss-video.c
<<
>>
Prefs
   1// SPDX-License-Identifier: GPL-2.0
   2/*
   3 * camss-video.c
   4 *
   5 * Qualcomm MSM Camera Subsystem - V4L2 device node
   6 *
   7 * Copyright (c) 2013-2015, The Linux Foundation. All rights reserved.
   8 * Copyright (C) 2015-2018 Linaro Ltd.
   9 */
  10#include <linux/slab.h>
  11#include <media/media-entity.h>
  12#include <media/v4l2-dev.h>
  13#include <media/v4l2-device.h>
  14#include <media/v4l2-ioctl.h>
  15#include <media/v4l2-mc.h>
  16#include <media/videobuf2-dma-sg.h>
  17
  18#include "camss-video.h"
  19#include "camss.h"
  20
  21struct fract {
  22        u8 numerator;
  23        u8 denominator;
  24};
  25
  26/*
  27 * struct camss_format_info - ISP media bus format information
  28 * @code: V4L2 media bus format code
  29 * @pixelformat: V4L2 pixel format FCC identifier
  30 * @planes: Number of planes
  31 * @hsub: Horizontal subsampling (for each plane)
  32 * @vsub: Vertical subsampling (for each plane)
  33 * @bpp: Bits per pixel when stored in memory (for each plane)
  34 */
  35struct camss_format_info {
  36        u32 code;
  37        u32 pixelformat;
  38        u8 planes;
  39        struct fract hsub[3];
  40        struct fract vsub[3];
  41        unsigned int bpp[3];
  42};
  43
  44static const struct camss_format_info formats_rdi_8x16[] = {
  45        { MEDIA_BUS_FMT_UYVY8_2X8, V4L2_PIX_FMT_UYVY, 1,
  46          { { 1, 1 } }, { { 1, 1 } }, { 16 } },
  47        { MEDIA_BUS_FMT_VYUY8_2X8, V4L2_PIX_FMT_VYUY, 1,
  48          { { 1, 1 } }, { { 1, 1 } }, { 16 } },
  49        { MEDIA_BUS_FMT_YUYV8_2X8, V4L2_PIX_FMT_YUYV, 1,
  50          { { 1, 1 } }, { { 1, 1 } }, { 16 } },
  51        { MEDIA_BUS_FMT_YVYU8_2X8, V4L2_PIX_FMT_YVYU, 1,
  52          { { 1, 1 } }, { { 1, 1 } }, { 16 } },
  53        { MEDIA_BUS_FMT_SBGGR8_1X8, V4L2_PIX_FMT_SBGGR8, 1,
  54          { { 1, 1 } }, { { 1, 1 } }, { 8 } },
  55        { MEDIA_BUS_FMT_SGBRG8_1X8, V4L2_PIX_FMT_SGBRG8, 1,
  56          { { 1, 1 } }, { { 1, 1 } }, { 8 } },
  57        { MEDIA_BUS_FMT_SGRBG8_1X8, V4L2_PIX_FMT_SGRBG8, 1,
  58          { { 1, 1 } }, { { 1, 1 } }, { 8 } },
  59        { MEDIA_BUS_FMT_SRGGB8_1X8, V4L2_PIX_FMT_SRGGB8, 1,
  60          { { 1, 1 } }, { { 1, 1 } }, { 8 } },
  61        { MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_PIX_FMT_SBGGR10P, 1,
  62          { { 1, 1 } }, { { 1, 1 } }, { 10 } },
  63        { MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_PIX_FMT_SGBRG10P, 1,
  64          { { 1, 1 } }, { { 1, 1 } }, { 10 } },
  65        { MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_PIX_FMT_SGRBG10P, 1,
  66          { { 1, 1 } }, { { 1, 1 } }, { 10 } },
  67        { MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_PIX_FMT_SRGGB10P, 1,
  68          { { 1, 1 } }, { { 1, 1 } }, { 10 } },
  69        { MEDIA_BUS_FMT_SBGGR12_1X12, V4L2_PIX_FMT_SBGGR12P, 1,
  70          { { 1, 1 } }, { { 1, 1 } }, { 12 } },
  71        { MEDIA_BUS_FMT_SGBRG12_1X12, V4L2_PIX_FMT_SGBRG12P, 1,
  72          { { 1, 1 } }, { { 1, 1 } }, { 12 } },
  73        { MEDIA_BUS_FMT_SGRBG12_1X12, V4L2_PIX_FMT_SGRBG12P, 1,
  74          { { 1, 1 } }, { { 1, 1 } }, { 12 } },
  75        { MEDIA_BUS_FMT_SRGGB12_1X12, V4L2_PIX_FMT_SRGGB12P, 1,
  76          { { 1, 1 } }, { { 1, 1 } }, { 12 } },
  77        { MEDIA_BUS_FMT_Y10_1X10, V4L2_PIX_FMT_Y10P, 1,
  78          { { 1, 1 } }, { { 1, 1 } }, { 10 } },
  79};
  80
  81static const struct camss_format_info formats_rdi_8x96[] = {
  82        { MEDIA_BUS_FMT_UYVY8_2X8, V4L2_PIX_FMT_UYVY, 1,
  83          { { 1, 1 } }, { { 1, 1 } }, { 16 } },
  84        { MEDIA_BUS_FMT_VYUY8_2X8, V4L2_PIX_FMT_VYUY, 1,
  85          { { 1, 1 } }, { { 1, 1 } }, { 16 } },
  86        { MEDIA_BUS_FMT_YUYV8_2X8, V4L2_PIX_FMT_YUYV, 1,
  87          { { 1, 1 } }, { { 1, 1 } }, { 16 } },
  88        { MEDIA_BUS_FMT_YVYU8_2X8, V4L2_PIX_FMT_YVYU, 1,
  89          { { 1, 1 } }, { { 1, 1 } }, { 16 } },
  90        { MEDIA_BUS_FMT_SBGGR8_1X8, V4L2_PIX_FMT_SBGGR8, 1,
  91          { { 1, 1 } }, { { 1, 1 } }, { 8 } },
  92        { MEDIA_BUS_FMT_SGBRG8_1X8, V4L2_PIX_FMT_SGBRG8, 1,
  93          { { 1, 1 } }, { { 1, 1 } }, { 8 } },
  94        { MEDIA_BUS_FMT_SGRBG8_1X8, V4L2_PIX_FMT_SGRBG8, 1,
  95          { { 1, 1 } }, { { 1, 1 } }, { 8 } },
  96        { MEDIA_BUS_FMT_SRGGB8_1X8, V4L2_PIX_FMT_SRGGB8, 1,
  97          { { 1, 1 } }, { { 1, 1 } }, { 8 } },
  98        { MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_PIX_FMT_SBGGR10P, 1,
  99          { { 1, 1 } }, { { 1, 1 } }, { 10 } },
 100        { MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_PIX_FMT_SGBRG10P, 1,
 101          { { 1, 1 } }, { { 1, 1 } }, { 10 } },
 102        { MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_PIX_FMT_SGRBG10P, 1,
 103          { { 1, 1 } }, { { 1, 1 } }, { 10 } },
 104        { MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_PIX_FMT_SRGGB10P, 1,
 105          { { 1, 1 } }, { { 1, 1 } }, { 10 } },
 106        { MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE, V4L2_PIX_FMT_SBGGR10, 1,
 107          { { 1, 1 } }, { { 1, 1 } }, { 16 } },
 108        { MEDIA_BUS_FMT_SBGGR12_1X12, V4L2_PIX_FMT_SBGGR12P, 1,
 109          { { 1, 1 } }, { { 1, 1 } }, { 12 } },
 110        { MEDIA_BUS_FMT_SGBRG12_1X12, V4L2_PIX_FMT_SGBRG12P, 1,
 111          { { 1, 1 } }, { { 1, 1 } }, { 12 } },
 112        { MEDIA_BUS_FMT_SGRBG12_1X12, V4L2_PIX_FMT_SGRBG12P, 1,
 113          { { 1, 1 } }, { { 1, 1 } }, { 12 } },
 114        { MEDIA_BUS_FMT_SRGGB12_1X12, V4L2_PIX_FMT_SRGGB12P, 1,
 115          { { 1, 1 } }, { { 1, 1 } }, { 12 } },
 116        { MEDIA_BUS_FMT_SBGGR14_1X14, V4L2_PIX_FMT_SBGGR14P, 1,
 117          { { 1, 1 } }, { { 1, 1 } }, { 14 } },
 118        { MEDIA_BUS_FMT_SGBRG14_1X14, V4L2_PIX_FMT_SGBRG14P, 1,
 119          { { 1, 1 } }, { { 1, 1 } }, { 14 } },
 120        { MEDIA_BUS_FMT_SGRBG14_1X14, V4L2_PIX_FMT_SGRBG14P, 1,
 121          { { 1, 1 } }, { { 1, 1 } }, { 14 } },
 122        { MEDIA_BUS_FMT_SRGGB14_1X14, V4L2_PIX_FMT_SRGGB14P, 1,
 123          { { 1, 1 } }, { { 1, 1 } }, { 14 } },
 124        { MEDIA_BUS_FMT_Y10_1X10, V4L2_PIX_FMT_Y10P, 1,
 125          { { 1, 1 } }, { { 1, 1 } }, { 10 } },
 126        { MEDIA_BUS_FMT_Y10_2X8_PADHI_LE, V4L2_PIX_FMT_Y10, 1,
 127          { { 1, 1 } }, { { 1, 1 } }, { 16 } },
 128};
 129
 130static const struct camss_format_info formats_pix_8x16[] = {
 131        { MEDIA_BUS_FMT_YUYV8_1_5X8, V4L2_PIX_FMT_NV12, 1,
 132          { { 1, 1 } }, { { 2, 3 } }, { 8 } },
 133        { MEDIA_BUS_FMT_YVYU8_1_5X8, V4L2_PIX_FMT_NV12, 1,
 134          { { 1, 1 } }, { { 2, 3 } }, { 8 } },
 135        { MEDIA_BUS_FMT_UYVY8_1_5X8, V4L2_PIX_FMT_NV12, 1,
 136          { { 1, 1 } }, { { 2, 3 } }, { 8 } },
 137        { MEDIA_BUS_FMT_VYUY8_1_5X8, V4L2_PIX_FMT_NV12, 1,
 138          { { 1, 1 } }, { { 2, 3 } }, { 8 } },
 139        { MEDIA_BUS_FMT_YUYV8_1_5X8, V4L2_PIX_FMT_NV21, 1,
 140          { { 1, 1 } }, { { 2, 3 } }, { 8 } },
 141        { MEDIA_BUS_FMT_YVYU8_1_5X8, V4L2_PIX_FMT_NV21, 1,
 142          { { 1, 1 } }, { { 2, 3 } }, { 8 } },
 143        { MEDIA_BUS_FMT_UYVY8_1_5X8, V4L2_PIX_FMT_NV21, 1,
 144          { { 1, 1 } }, { { 2, 3 } }, { 8 } },
 145        { MEDIA_BUS_FMT_VYUY8_1_5X8, V4L2_PIX_FMT_NV21, 1,
 146          { { 1, 1 } }, { { 2, 3 } }, { 8 } },
 147        { MEDIA_BUS_FMT_YUYV8_2X8, V4L2_PIX_FMT_NV16, 1,
 148          { { 1, 1 } }, { { 1, 2 } }, { 8 } },
 149        { MEDIA_BUS_FMT_YVYU8_2X8, V4L2_PIX_FMT_NV16, 1,
 150          { { 1, 1 } }, { { 1, 2 } }, { 8 } },
 151        { MEDIA_BUS_FMT_UYVY8_2X8, V4L2_PIX_FMT_NV16, 1,
 152          { { 1, 1 } }, { { 1, 2 } }, { 8 } },
 153        { MEDIA_BUS_FMT_VYUY8_2X8, V4L2_PIX_FMT_NV16, 1,
 154          { { 1, 1 } }, { { 1, 2 } }, { 8 } },
 155        { MEDIA_BUS_FMT_YUYV8_2X8, V4L2_PIX_FMT_NV61, 1,
 156          { { 1, 1 } }, { { 1, 2 } }, { 8 } },
 157        { MEDIA_BUS_FMT_YVYU8_2X8, V4L2_PIX_FMT_NV61, 1,
 158          { { 1, 1 } }, { { 1, 2 } }, { 8 } },
 159        { MEDIA_BUS_FMT_UYVY8_2X8, V4L2_PIX_FMT_NV61, 1,
 160          { { 1, 1 } }, { { 1, 2 } }, { 8 } },
 161        { MEDIA_BUS_FMT_VYUY8_2X8, V4L2_PIX_FMT_NV61, 1,
 162          { { 1, 1 } }, { { 1, 2 } }, { 8 } },
 163};
 164
 165static const struct camss_format_info formats_pix_8x96[] = {
 166        { MEDIA_BUS_FMT_YUYV8_1_5X8, V4L2_PIX_FMT_NV12, 1,
 167          { { 1, 1 } }, { { 2, 3 } }, { 8 } },
 168        { MEDIA_BUS_FMT_YVYU8_1_5X8, V4L2_PIX_FMT_NV12, 1,
 169          { { 1, 1 } }, { { 2, 3 } }, { 8 } },
 170        { MEDIA_BUS_FMT_UYVY8_1_5X8, V4L2_PIX_FMT_NV12, 1,
 171          { { 1, 1 } }, { { 2, 3 } }, { 8 } },
 172        { MEDIA_BUS_FMT_VYUY8_1_5X8, V4L2_PIX_FMT_NV12, 1,
 173          { { 1, 1 } }, { { 2, 3 } }, { 8 } },
 174        { MEDIA_BUS_FMT_YUYV8_1_5X8, V4L2_PIX_FMT_NV21, 1,
 175          { { 1, 1 } }, { { 2, 3 } }, { 8 } },
 176        { MEDIA_BUS_FMT_YVYU8_1_5X8, V4L2_PIX_FMT_NV21, 1,
 177          { { 1, 1 } }, { { 2, 3 } }, { 8 } },
 178        { MEDIA_BUS_FMT_UYVY8_1_5X8, V4L2_PIX_FMT_NV21, 1,
 179          { { 1, 1 } }, { { 2, 3 } }, { 8 } },
 180        { MEDIA_BUS_FMT_VYUY8_1_5X8, V4L2_PIX_FMT_NV21, 1,
 181          { { 1, 1 } }, { { 2, 3 } }, { 8 } },
 182        { MEDIA_BUS_FMT_YUYV8_2X8, V4L2_PIX_FMT_NV16, 1,
 183          { { 1, 1 } }, { { 1, 2 } }, { 8 } },
 184        { MEDIA_BUS_FMT_YVYU8_2X8, V4L2_PIX_FMT_NV16, 1,
 185          { { 1, 1 } }, { { 1, 2 } }, { 8 } },
 186        { MEDIA_BUS_FMT_UYVY8_2X8, V4L2_PIX_FMT_NV16, 1,
 187          { { 1, 1 } }, { { 1, 2 } }, { 8 } },
 188        { MEDIA_BUS_FMT_VYUY8_2X8, V4L2_PIX_FMT_NV16, 1,
 189          { { 1, 1 } }, { { 1, 2 } }, { 8 } },
 190        { MEDIA_BUS_FMT_YUYV8_2X8, V4L2_PIX_FMT_NV61, 1,
 191          { { 1, 1 } }, { { 1, 2 } }, { 8 } },
 192        { MEDIA_BUS_FMT_YVYU8_2X8, V4L2_PIX_FMT_NV61, 1,
 193          { { 1, 1 } }, { { 1, 2 } }, { 8 } },
 194        { MEDIA_BUS_FMT_UYVY8_2X8, V4L2_PIX_FMT_NV61, 1,
 195          { { 1, 1 } }, { { 1, 2 } }, { 8 } },
 196        { MEDIA_BUS_FMT_VYUY8_2X8, V4L2_PIX_FMT_NV61, 1,
 197          { { 1, 1 } }, { { 1, 2 } }, { 8 } },
 198        { MEDIA_BUS_FMT_UYVY8_2X8, V4L2_PIX_FMT_UYVY, 1,
 199          { { 1, 1 } }, { { 1, 1 } }, { 16 } },
 200        { MEDIA_BUS_FMT_VYUY8_2X8, V4L2_PIX_FMT_VYUY, 1,
 201          { { 1, 1 } }, { { 1, 1 } }, { 16 } },
 202        { MEDIA_BUS_FMT_YUYV8_2X8, V4L2_PIX_FMT_YUYV, 1,
 203          { { 1, 1 } }, { { 1, 1 } }, { 16 } },
 204        { MEDIA_BUS_FMT_YVYU8_2X8, V4L2_PIX_FMT_YVYU, 1,
 205          { { 1, 1 } }, { { 1, 1 } }, { 16 } },
 206};
 207
 208/* -----------------------------------------------------------------------------
 209 * Helper functions
 210 */
 211
 212static int video_find_format(u32 code, u32 pixelformat,
 213                             const struct camss_format_info *formats,
 214                             unsigned int nformats)
 215{
 216        int i;
 217
 218        for (i = 0; i < nformats; i++) {
 219                if (formats[i].code == code &&
 220                    formats[i].pixelformat == pixelformat)
 221                        return i;
 222        }
 223
 224        for (i = 0; i < nformats; i++)
 225                if (formats[i].code == code)
 226                        return i;
 227
 228        WARN_ON(1);
 229
 230        return -EINVAL;
 231}
 232
 233/*
 234 * video_mbus_to_pix_mp - Convert v4l2_mbus_framefmt to v4l2_pix_format_mplane
 235 * @mbus: v4l2_mbus_framefmt format (input)
 236 * @pix: v4l2_pix_format_mplane format (output)
 237 * @f: a pointer to formats array element to be used for the conversion
 238 * @alignment: bytesperline alignment value
 239 *
 240 * Fill the output pix structure with information from the input mbus format.
 241 *
 242 * Return 0 on success or a negative error code otherwise
 243 */
 244static int video_mbus_to_pix_mp(const struct v4l2_mbus_framefmt *mbus,
 245                                struct v4l2_pix_format_mplane *pix,
 246                                const struct camss_format_info *f,
 247                                unsigned int alignment)
 248{
 249        unsigned int i;
 250        u32 bytesperline;
 251
 252        memset(pix, 0, sizeof(*pix));
 253        v4l2_fill_pix_format_mplane(pix, mbus);
 254        pix->pixelformat = f->pixelformat;
 255        pix->num_planes = f->planes;
 256        for (i = 0; i < pix->num_planes; i++) {
 257                bytesperline = pix->width / f->hsub[i].numerator *
 258                        f->hsub[i].denominator * f->bpp[i] / 8;
 259                bytesperline = ALIGN(bytesperline, alignment);
 260                pix->plane_fmt[i].bytesperline = bytesperline;
 261                pix->plane_fmt[i].sizeimage = pix->height /
 262                                f->vsub[i].numerator * f->vsub[i].denominator *
 263                                bytesperline;
 264        }
 265
 266        return 0;
 267}
 268
 269static struct v4l2_subdev *video_remote_subdev(struct camss_video *video,
 270                                               u32 *pad)
 271{
 272        struct media_pad *remote;
 273
 274        remote = media_entity_remote_pad(&video->pad);
 275
 276        if (!remote || !is_media_entity_v4l2_subdev(remote->entity))
 277                return NULL;
 278
 279        if (pad)
 280                *pad = remote->index;
 281
 282        return media_entity_to_v4l2_subdev(remote->entity);
 283}
 284
 285static int video_get_subdev_format(struct camss_video *video,
 286                                   struct v4l2_format *format)
 287{
 288        struct v4l2_subdev_format fmt;
 289        struct v4l2_subdev *subdev;
 290        u32 pad;
 291        int ret;
 292
 293        subdev = video_remote_subdev(video, &pad);
 294        if (subdev == NULL)
 295                return -EPIPE;
 296
 297        fmt.pad = pad;
 298        fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 299
 300        ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
 301        if (ret)
 302                return ret;
 303
 304        ret = video_find_format(fmt.format.code,
 305                                format->fmt.pix_mp.pixelformat,
 306                                video->formats, video->nformats);
 307        if (ret < 0)
 308                return ret;
 309
 310        format->type = video->type;
 311
 312        return video_mbus_to_pix_mp(&fmt.format, &format->fmt.pix_mp,
 313                                    &video->formats[ret], video->bpl_alignment);
 314}
 315
 316/* -----------------------------------------------------------------------------
 317 * Video queue operations
 318 */
 319
 320static int video_queue_setup(struct vb2_queue *q,
 321        unsigned int *num_buffers, unsigned int *num_planes,
 322        unsigned int sizes[], struct device *alloc_devs[])
 323{
 324        struct camss_video *video = vb2_get_drv_priv(q);
 325        const struct v4l2_pix_format_mplane *format =
 326                                                &video->active_fmt.fmt.pix_mp;
 327        unsigned int i;
 328
 329        if (*num_planes) {
 330                if (*num_planes != format->num_planes)
 331                        return -EINVAL;
 332
 333                for (i = 0; i < *num_planes; i++)
 334                        if (sizes[i] < format->plane_fmt[i].sizeimage)
 335                                return -EINVAL;
 336
 337                return 0;
 338        }
 339
 340        *num_planes = format->num_planes;
 341
 342        for (i = 0; i < *num_planes; i++)
 343                sizes[i] = format->plane_fmt[i].sizeimage;
 344
 345        return 0;
 346}
 347
 348static int video_buf_init(struct vb2_buffer *vb)
 349{
 350        struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 351        struct camss_video *video = vb2_get_drv_priv(vb->vb2_queue);
 352        struct camss_buffer *buffer = container_of(vbuf, struct camss_buffer,
 353                                                   vb);
 354        const struct v4l2_pix_format_mplane *format =
 355                                                &video->active_fmt.fmt.pix_mp;
 356        struct sg_table *sgt;
 357        unsigned int i;
 358
 359        for (i = 0; i < format->num_planes; i++) {
 360                sgt = vb2_dma_sg_plane_desc(vb, i);
 361                if (!sgt)
 362                        return -EFAULT;
 363
 364                buffer->addr[i] = sg_dma_address(sgt->sgl);
 365        }
 366
 367        if (format->pixelformat == V4L2_PIX_FMT_NV12 ||
 368                        format->pixelformat == V4L2_PIX_FMT_NV21 ||
 369                        format->pixelformat == V4L2_PIX_FMT_NV16 ||
 370                        format->pixelformat == V4L2_PIX_FMT_NV61)
 371                buffer->addr[1] = buffer->addr[0] +
 372                                format->plane_fmt[0].bytesperline *
 373                                format->height;
 374
 375        return 0;
 376}
 377
 378static int video_buf_prepare(struct vb2_buffer *vb)
 379{
 380        struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 381        struct camss_video *video = vb2_get_drv_priv(vb->vb2_queue);
 382        const struct v4l2_pix_format_mplane *format =
 383                                                &video->active_fmt.fmt.pix_mp;
 384        unsigned int i;
 385
 386        for (i = 0; i < format->num_planes; i++) {
 387                if (format->plane_fmt[i].sizeimage > vb2_plane_size(vb, i))
 388                        return -EINVAL;
 389
 390                vb2_set_plane_payload(vb, i, format->plane_fmt[i].sizeimage);
 391        }
 392
 393        vbuf->field = V4L2_FIELD_NONE;
 394
 395        return 0;
 396}
 397
 398static void video_buf_queue(struct vb2_buffer *vb)
 399{
 400        struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 401        struct camss_video *video = vb2_get_drv_priv(vb->vb2_queue);
 402        struct camss_buffer *buffer = container_of(vbuf, struct camss_buffer,
 403                                                   vb);
 404
 405        video->ops->queue_buffer(video, buffer);
 406}
 407
 408static int video_check_format(struct camss_video *video)
 409{
 410        struct v4l2_pix_format_mplane *pix = &video->active_fmt.fmt.pix_mp;
 411        struct v4l2_format format;
 412        struct v4l2_pix_format_mplane *sd_pix = &format.fmt.pix_mp;
 413        int ret;
 414
 415        sd_pix->pixelformat = pix->pixelformat;
 416        ret = video_get_subdev_format(video, &format);
 417        if (ret < 0)
 418                return ret;
 419
 420        if (pix->pixelformat != sd_pix->pixelformat ||
 421            pix->height != sd_pix->height ||
 422            pix->width != sd_pix->width ||
 423            pix->num_planes != sd_pix->num_planes ||
 424            pix->field != format.fmt.pix_mp.field)
 425                return -EPIPE;
 426
 427        return 0;
 428}
 429
 430static int video_start_streaming(struct vb2_queue *q, unsigned int count)
 431{
 432        struct camss_video *video = vb2_get_drv_priv(q);
 433        struct video_device *vdev = &video->vdev;
 434        struct media_entity *entity;
 435        struct media_pad *pad;
 436        struct v4l2_subdev *subdev;
 437        int ret;
 438
 439        ret = media_pipeline_start(&vdev->entity, &video->pipe);
 440        if (ret < 0)
 441                return ret;
 442
 443        ret = video_check_format(video);
 444        if (ret < 0)
 445                goto error;
 446
 447        entity = &vdev->entity;
 448        while (1) {
 449                pad = &entity->pads[0];
 450                if (!(pad->flags & MEDIA_PAD_FL_SINK))
 451                        break;
 452
 453                pad = media_entity_remote_pad(pad);
 454                if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 455                        break;
 456
 457                entity = pad->entity;
 458                subdev = media_entity_to_v4l2_subdev(entity);
 459
 460                ret = v4l2_subdev_call(subdev, video, s_stream, 1);
 461                if (ret < 0 && ret != -ENOIOCTLCMD)
 462                        goto error;
 463        }
 464
 465        return 0;
 466
 467error:
 468        media_pipeline_stop(&vdev->entity);
 469
 470        video->ops->flush_buffers(video, VB2_BUF_STATE_QUEUED);
 471
 472        return ret;
 473}
 474
 475static void video_stop_streaming(struct vb2_queue *q)
 476{
 477        struct camss_video *video = vb2_get_drv_priv(q);
 478        struct video_device *vdev = &video->vdev;
 479        struct media_entity *entity;
 480        struct media_pad *pad;
 481        struct v4l2_subdev *subdev;
 482
 483        entity = &vdev->entity;
 484        while (1) {
 485                pad = &entity->pads[0];
 486                if (!(pad->flags & MEDIA_PAD_FL_SINK))
 487                        break;
 488
 489                pad = media_entity_remote_pad(pad);
 490                if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 491                        break;
 492
 493                entity = pad->entity;
 494                subdev = media_entity_to_v4l2_subdev(entity);
 495
 496                v4l2_subdev_call(subdev, video, s_stream, 0);
 497        }
 498
 499        media_pipeline_stop(&vdev->entity);
 500
 501        video->ops->flush_buffers(video, VB2_BUF_STATE_ERROR);
 502}
 503
 504static const struct vb2_ops msm_video_vb2_q_ops = {
 505        .queue_setup     = video_queue_setup,
 506        .wait_prepare    = vb2_ops_wait_prepare,
 507        .wait_finish     = vb2_ops_wait_finish,
 508        .buf_init        = video_buf_init,
 509        .buf_prepare     = video_buf_prepare,
 510        .buf_queue       = video_buf_queue,
 511        .start_streaming = video_start_streaming,
 512        .stop_streaming  = video_stop_streaming,
 513};
 514
 515/* -----------------------------------------------------------------------------
 516 * V4L2 ioctls
 517 */
 518
 519static int video_querycap(struct file *file, void *fh,
 520                          struct v4l2_capability *cap)
 521{
 522        struct camss_video *video = video_drvdata(file);
 523
 524        strscpy(cap->driver, "qcom-camss", sizeof(cap->driver));
 525        strscpy(cap->card, "Qualcomm Camera Subsystem", sizeof(cap->card));
 526        snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
 527                 dev_name(video->camss->dev));
 528
 529        return 0;
 530}
 531
 532static int video_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f)
 533{
 534        struct camss_video *video = video_drvdata(file);
 535        int i, j, k;
 536
 537        if (f->type != video->type)
 538                return -EINVAL;
 539
 540        if (f->index >= video->nformats)
 541                return -EINVAL;
 542
 543        /* find index "i" of "k"th unique pixelformat in formats array */
 544        k = -1;
 545        for (i = 0; i < video->nformats; i++) {
 546                for (j = 0; j < i; j++) {
 547                        if (video->formats[i].pixelformat ==
 548                                        video->formats[j].pixelformat)
 549                                break;
 550                }
 551
 552                if (j == i)
 553                        k++;
 554
 555                if (k == f->index)
 556                        break;
 557        }
 558
 559        if (k < f->index)
 560                return -EINVAL;
 561
 562        f->pixelformat = video->formats[i].pixelformat;
 563
 564        return 0;
 565}
 566
 567static int video_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
 568{
 569        struct camss_video *video = video_drvdata(file);
 570
 571        *f = video->active_fmt;
 572
 573        return 0;
 574}
 575
 576static int __video_try_fmt(struct camss_video *video, struct v4l2_format *f)
 577{
 578        struct v4l2_pix_format_mplane *pix_mp;
 579        const struct camss_format_info *fi;
 580        struct v4l2_plane_pix_format *p;
 581        u32 bytesperline[3] = { 0 };
 582        u32 sizeimage[3] = { 0 };
 583        u32 width, height;
 584        u32 bpl, lines;
 585        int i, j;
 586
 587        pix_mp = &f->fmt.pix_mp;
 588
 589        if (video->line_based)
 590                for (i = 0; i < pix_mp->num_planes && i < 3; i++) {
 591                        p = &pix_mp->plane_fmt[i];
 592                        bytesperline[i] = clamp_t(u32, p->bytesperline,
 593                                                  1, 65528);
 594                        sizeimage[i] = clamp_t(u32, p->sizeimage,
 595                                               bytesperline[i],
 596                                               bytesperline[i] * 4096);
 597                }
 598
 599        for (j = 0; j < video->nformats; j++)
 600                if (pix_mp->pixelformat == video->formats[j].pixelformat)
 601                        break;
 602
 603        if (j == video->nformats)
 604                j = 0; /* default format */
 605
 606        fi = &video->formats[j];
 607        width = pix_mp->width;
 608        height = pix_mp->height;
 609
 610        memset(pix_mp, 0, sizeof(*pix_mp));
 611
 612        pix_mp->pixelformat = fi->pixelformat;
 613        pix_mp->width = clamp_t(u32, width, 1, 8191);
 614        pix_mp->height = clamp_t(u32, height, 1, 8191);
 615        pix_mp->num_planes = fi->planes;
 616        for (i = 0; i < pix_mp->num_planes; i++) {
 617                bpl = pix_mp->width / fi->hsub[i].numerator *
 618                        fi->hsub[i].denominator * fi->bpp[i] / 8;
 619                bpl = ALIGN(bpl, video->bpl_alignment);
 620                pix_mp->plane_fmt[i].bytesperline = bpl;
 621                pix_mp->plane_fmt[i].sizeimage = pix_mp->height /
 622                        fi->vsub[i].numerator * fi->vsub[i].denominator * bpl;
 623        }
 624
 625        pix_mp->field = V4L2_FIELD_NONE;
 626        pix_mp->colorspace = V4L2_COLORSPACE_SRGB;
 627        pix_mp->flags = 0;
 628        pix_mp->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(pix_mp->colorspace);
 629        pix_mp->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
 630                                        pix_mp->colorspace, pix_mp->ycbcr_enc);
 631        pix_mp->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(pix_mp->colorspace);
 632
 633        if (video->line_based)
 634                for (i = 0; i < pix_mp->num_planes; i++) {
 635                        p = &pix_mp->plane_fmt[i];
 636                        p->bytesperline = clamp_t(u32, p->bytesperline,
 637                                                  1, 65528);
 638                        p->sizeimage = clamp_t(u32, p->sizeimage,
 639                                               p->bytesperline,
 640                                               p->bytesperline * 4096);
 641                        lines = p->sizeimage / p->bytesperline;
 642
 643                        if (p->bytesperline < bytesperline[i])
 644                                p->bytesperline = ALIGN(bytesperline[i], 8);
 645
 646                        if (p->sizeimage < p->bytesperline * lines)
 647                                p->sizeimage = p->bytesperline * lines;
 648
 649                        if (p->sizeimage < sizeimage[i])
 650                                p->sizeimage = sizeimage[i];
 651                }
 652
 653        return 0;
 654}
 655
 656static int video_try_fmt(struct file *file, void *fh, struct v4l2_format *f)
 657{
 658        struct camss_video *video = video_drvdata(file);
 659
 660        return __video_try_fmt(video, f);
 661}
 662
 663static int video_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
 664{
 665        struct camss_video *video = video_drvdata(file);
 666        int ret;
 667
 668        if (vb2_is_busy(&video->vb2_q))
 669                return -EBUSY;
 670
 671        ret = __video_try_fmt(video, f);
 672        if (ret < 0)
 673                return ret;
 674
 675        video->active_fmt = *f;
 676
 677        return 0;
 678}
 679
 680static int video_enum_input(struct file *file, void *fh,
 681                            struct v4l2_input *input)
 682{
 683        if (input->index > 0)
 684                return -EINVAL;
 685
 686        strscpy(input->name, "camera", sizeof(input->name));
 687        input->type = V4L2_INPUT_TYPE_CAMERA;
 688
 689        return 0;
 690}
 691
 692static int video_g_input(struct file *file, void *fh, unsigned int *input)
 693{
 694        *input = 0;
 695
 696        return 0;
 697}
 698
 699static int video_s_input(struct file *file, void *fh, unsigned int input)
 700{
 701        return input == 0 ? 0 : -EINVAL;
 702}
 703
 704static const struct v4l2_ioctl_ops msm_vid_ioctl_ops = {
 705        .vidioc_querycap                = video_querycap,
 706        .vidioc_enum_fmt_vid_cap_mplane = video_enum_fmt,
 707        .vidioc_g_fmt_vid_cap_mplane    = video_g_fmt,
 708        .vidioc_s_fmt_vid_cap_mplane    = video_s_fmt,
 709        .vidioc_try_fmt_vid_cap_mplane  = video_try_fmt,
 710        .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
 711        .vidioc_querybuf                = vb2_ioctl_querybuf,
 712        .vidioc_qbuf                    = vb2_ioctl_qbuf,
 713        .vidioc_expbuf                  = vb2_ioctl_expbuf,
 714        .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
 715        .vidioc_create_bufs             = vb2_ioctl_create_bufs,
 716        .vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
 717        .vidioc_streamon                = vb2_ioctl_streamon,
 718        .vidioc_streamoff               = vb2_ioctl_streamoff,
 719        .vidioc_enum_input              = video_enum_input,
 720        .vidioc_g_input                 = video_g_input,
 721        .vidioc_s_input                 = video_s_input,
 722};
 723
 724/* -----------------------------------------------------------------------------
 725 * V4L2 file operations
 726 */
 727
 728static int video_open(struct file *file)
 729{
 730        struct video_device *vdev = video_devdata(file);
 731        struct camss_video *video = video_drvdata(file);
 732        struct v4l2_fh *vfh;
 733        int ret;
 734
 735        mutex_lock(&video->lock);
 736
 737        vfh = kzalloc(sizeof(*vfh), GFP_KERNEL);
 738        if (vfh == NULL) {
 739                ret = -ENOMEM;
 740                goto error_alloc;
 741        }
 742
 743        v4l2_fh_init(vfh, vdev);
 744        v4l2_fh_add(vfh);
 745
 746        file->private_data = vfh;
 747
 748        ret = v4l2_pipeline_pm_use(&vdev->entity, 1);
 749        if (ret < 0) {
 750                dev_err(video->camss->dev, "Failed to power up pipeline: %d\n",
 751                        ret);
 752                goto error_pm_use;
 753        }
 754
 755        mutex_unlock(&video->lock);
 756
 757        return 0;
 758
 759error_pm_use:
 760        v4l2_fh_release(file);
 761
 762error_alloc:
 763        mutex_unlock(&video->lock);
 764
 765        return ret;
 766}
 767
 768static int video_release(struct file *file)
 769{
 770        struct video_device *vdev = video_devdata(file);
 771
 772        vb2_fop_release(file);
 773
 774        v4l2_pipeline_pm_use(&vdev->entity, 0);
 775
 776        file->private_data = NULL;
 777
 778        return 0;
 779}
 780
 781static const struct v4l2_file_operations msm_vid_fops = {
 782        .owner          = THIS_MODULE,
 783        .unlocked_ioctl = video_ioctl2,
 784        .open           = video_open,
 785        .release        = video_release,
 786        .poll           = vb2_fop_poll,
 787        .mmap           = vb2_fop_mmap,
 788        .read           = vb2_fop_read,
 789};
 790
 791/* -----------------------------------------------------------------------------
 792 * CAMSS video core
 793 */
 794
 795static void msm_video_release(struct video_device *vdev)
 796{
 797        struct camss_video *video = video_get_drvdata(vdev);
 798
 799        media_entity_cleanup(&vdev->entity);
 800
 801        mutex_destroy(&video->q_lock);
 802        mutex_destroy(&video->lock);
 803
 804        if (atomic_dec_and_test(&video->camss->ref_count))
 805                camss_delete(video->camss);
 806}
 807
 808/*
 809 * msm_video_init_format - Helper function to initialize format
 810 * @video: struct camss_video
 811 *
 812 * Initialize pad format with default value.
 813 *
 814 * Return 0 on success or a negative error code otherwise
 815 */
 816static int msm_video_init_format(struct camss_video *video)
 817{
 818        int ret;
 819        struct v4l2_format format = {
 820                .type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
 821                .fmt.pix_mp = {
 822                        .width = 1920,
 823                        .height = 1080,
 824                        .pixelformat = video->formats[0].pixelformat,
 825                },
 826        };
 827
 828        ret = __video_try_fmt(video, &format);
 829        if (ret < 0)
 830                return ret;
 831
 832        video->active_fmt = format;
 833
 834        return 0;
 835}
 836
 837/*
 838 * msm_video_register - Register a video device node
 839 * @video: struct camss_video
 840 * @v4l2_dev: V4L2 device
 841 * @name: name to be used for the video device node
 842 *
 843 * Initialize and register a video device node to a V4L2 device. Also
 844 * initialize the vb2 queue.
 845 *
 846 * Return 0 on success or a negative error code otherwise
 847 */
 848
 849int msm_video_register(struct camss_video *video, struct v4l2_device *v4l2_dev,
 850                       const char *name, int is_pix)
 851{
 852        struct media_pad *pad = &video->pad;
 853        struct video_device *vdev;
 854        struct vb2_queue *q;
 855        int ret;
 856
 857        vdev = &video->vdev;
 858
 859        mutex_init(&video->q_lock);
 860
 861        q = &video->vb2_q;
 862        q->drv_priv = video;
 863        q->mem_ops = &vb2_dma_sg_memops;
 864        q->ops = &msm_video_vb2_q_ops;
 865        q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
 866        q->io_modes = VB2_DMABUF | VB2_MMAP | VB2_READ;
 867        q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 868        q->buf_struct_size = sizeof(struct camss_buffer);
 869        q->dev = video->camss->dev;
 870        q->lock = &video->q_lock;
 871        ret = vb2_queue_init(q);
 872        if (ret < 0) {
 873                dev_err(v4l2_dev->dev, "Failed to init vb2 queue: %d\n", ret);
 874                goto error_vb2_init;
 875        }
 876
 877        pad->flags = MEDIA_PAD_FL_SINK;
 878        ret = media_entity_pads_init(&vdev->entity, 1, pad);
 879        if (ret < 0) {
 880                dev_err(v4l2_dev->dev, "Failed to init video entity: %d\n",
 881                        ret);
 882                goto error_media_init;
 883        }
 884
 885        mutex_init(&video->lock);
 886
 887        if (video->camss->version == CAMSS_8x16) {
 888                if (is_pix) {
 889                        video->formats = formats_pix_8x16;
 890                        video->nformats = ARRAY_SIZE(formats_pix_8x16);
 891                } else {
 892                        video->formats = formats_rdi_8x16;
 893                        video->nformats = ARRAY_SIZE(formats_rdi_8x16);
 894                }
 895        } else if (video->camss->version == CAMSS_8x96) {
 896                if (is_pix) {
 897                        video->formats = formats_pix_8x96;
 898                        video->nformats = ARRAY_SIZE(formats_pix_8x96);
 899                } else {
 900                        video->formats = formats_rdi_8x96;
 901                        video->nformats = ARRAY_SIZE(formats_rdi_8x96);
 902                }
 903        } else {
 904                goto error_video_register;
 905        }
 906
 907        ret = msm_video_init_format(video);
 908        if (ret < 0) {
 909                dev_err(v4l2_dev->dev, "Failed to init format: %d\n", ret);
 910                goto error_video_register;
 911        }
 912
 913        vdev->fops = &msm_vid_fops;
 914        vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_STREAMING |
 915                                                        V4L2_CAP_READWRITE;
 916        vdev->ioctl_ops = &msm_vid_ioctl_ops;
 917        vdev->release = msm_video_release;
 918        vdev->v4l2_dev = v4l2_dev;
 919        vdev->vfl_dir = VFL_DIR_RX;
 920        vdev->queue = &video->vb2_q;
 921        vdev->lock = &video->lock;
 922        strscpy(vdev->name, name, sizeof(vdev->name));
 923
 924        ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
 925        if (ret < 0) {
 926                dev_err(v4l2_dev->dev, "Failed to register video device: %d\n",
 927                        ret);
 928                goto error_video_register;
 929        }
 930
 931        video_set_drvdata(vdev, video);
 932        atomic_inc(&video->camss->ref_count);
 933
 934        return 0;
 935
 936error_video_register:
 937        media_entity_cleanup(&vdev->entity);
 938        mutex_destroy(&video->lock);
 939error_media_init:
 940        vb2_queue_release(&video->vb2_q);
 941error_vb2_init:
 942        mutex_destroy(&video->q_lock);
 943
 944        return ret;
 945}
 946
 947void msm_video_stop_streaming(struct camss_video *video)
 948{
 949        if (vb2_is_streaming(&video->vb2_q))
 950                vb2_queue_release(&video->vb2_q);
 951}
 952
 953void msm_video_unregister(struct camss_video *video)
 954{
 955        atomic_inc(&video->camss->ref_count);
 956        video_unregister_device(&video->vdev);
 957        atomic_dec(&video->camss->ref_count);
 958}
 959