drivers/staging/media/rkisp1/rkisp1-capture.c

   1 // SPDX-License-Identifier: (GPL-2.0+ OR MIT)
   2 /*
   3  * Rockchip ISP1 Driver - V4l capture device
   4  *
   5  * Copyright (C) 2019 Collabora, Ltd.
   6  *
   7  * Based on Rockchip ISP1 driver by Rockchip Electronics Co., Ltd.
   8  * Copyright (C) 2017 Rockchip Electronics Co., Ltd.
   9  */
  10
  11 #include <linux/delay.h>
  12 #include <linux/pm_runtime.h>
  13 #include <media/v4l2-common.h>
  14 #include <media/v4l2-event.h>
  15 #include <media/v4l2-fh.h>
  16 #include <media/v4l2-ioctl.h>
  17 #include <media/v4l2-mc.h>
  18 #include <media/v4l2-subdev.h>
  19 #include <media/videobuf2-dma-contig.h>
  20
  21 #include "rkisp1-common.h"
  22
  23 /*
  24  * NOTE: There are two capture video devices in rkisp1, selfpath and mainpath.
  25  *
  26  * differences between selfpath and mainpath
  27  * available mp sink input: isp
  28  * available sp sink input : isp, dma(TODO)
  29  * available mp sink pad fmts: yuv422, raw
  30  * available sp sink pad fmts: yuv422, yuv420......
  31  * available mp source fmts: yuv, raw, jpeg(TODO)
  32  * available sp source fmts: yuv, rgb
  33  */
  34
  35 #define RKISP1_SP_DEV_NAME      RKISP1_DRIVER_NAME "_selfpath"
  36 #define RKISP1_MP_DEV_NAME      RKISP1_DRIVER_NAME "_mainpath"
  37
  38 #define RKISP1_MIN_BUFFERS_NEEDED 3
  39
  40 enum rkisp1_plane {
  41         RKISP1_PLANE_Y  = 0,
  42         RKISP1_PLANE_CB = 1,
  43         RKISP1_PLANE_CR = 2
  44 };
  45
  46 /*
  47  * @fourcc: pixel format
  48  * @fmt_type: helper filed for pixel format
  49  * @uv_swap: if cb cr swaped, for yuv
  50  * @write_format: defines how YCbCr self picture data is written to memory
  51  * @output_format: defines sp output format
  52  * @mbus: the mbus code on the src resizer pad that matches the pixel format
  53  */
  54 struct rkisp1_capture_fmt_cfg {
  55         u32 fourcc;
  56         u8 uv_swap;
  57         u32 write_format;
  58         u32 output_format;
  59         u32 mbus;
  60 };
  61
  62 struct rkisp1_capture_ops {
  63         void (*config)(struct rkisp1_capture *cap);
  64         void (*stop)(struct rkisp1_capture *cap);
  65         void (*enable)(struct rkisp1_capture *cap);
  66         void (*disable)(struct rkisp1_capture *cap);
  67         void (*set_data_path)(struct rkisp1_capture *cap);
  68         bool (*is_stopped)(struct rkisp1_capture *cap);
  69 };
  70
  71 struct rkisp1_capture_config {
  72         const struct rkisp1_capture_fmt_cfg *fmts;
  73         int fmt_size;
  74         struct {
  75                 u32 y_size_init;
  76                 u32 cb_size_init;
  77                 u32 cr_size_init;
  78                 u32 y_base_ad_init;
  79                 u32 cb_base_ad_init;
  80                 u32 cr_base_ad_init;
  81                 u32 y_offs_cnt_init;
  82                 u32 cb_offs_cnt_init;
  83                 u32 cr_offs_cnt_init;
  84         } mi;
  85 };
  86
  87 /*
  88  * The supported pixel formats for mainpath. NOTE, pixel formats with identical 'mbus'
  89  * are grouped together. This is assumed and used by the function rkisp1_cap_enum_mbus_codes
  90  */
  91 static const struct rkisp1_capture_fmt_cfg rkisp1_mp_fmts[] = {
  92         /* yuv422 */
  93         {
  94                 .fourcc = V4L2_PIX_FMT_YUYV,
  95                 .uv_swap = 0,
  96                 .write_format = RKISP1_MI_CTRL_MP_WRITE_YUVINT,
  97                 .mbus = MEDIA_BUS_FMT_YUYV8_2X8,
  98         }, {
  99                 .fourcc = V4L2_PIX_FMT_YUV422P,
 100                 .uv_swap = 0,
 101                 .write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_PLA_OR_RAW8,
 102                 .mbus = MEDIA_BUS_FMT_YUYV8_2X8,
 103         }, {
 104                 .fourcc = V4L2_PIX_FMT_NV16,
 105                 .uv_swap = 0,
 106                 .write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_SPLA,
 107                 .mbus = MEDIA_BUS_FMT_YUYV8_2X8,
 108         }, {
 109                 .fourcc = V4L2_PIX_FMT_NV61,
 110                 .uv_swap = 1,
 111                 .write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_SPLA,
 112                 .mbus = MEDIA_BUS_FMT_YUYV8_2X8,
 113         }, {
 114                 .fourcc = V4L2_PIX_FMT_YVU422M,
 115                 .uv_swap = 1,
 116                 .write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_PLA_OR_RAW8,
 117                 .mbus = MEDIA_BUS_FMT_YUYV8_2X8,
 118         },
 119         /* yuv400 */
 120         {
 121                 .fourcc = V4L2_PIX_FMT_GREY,
 122                 .uv_swap = 0,
 123                 .write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_PLA_OR_RAW8,
 124                 .mbus = MEDIA_BUS_FMT_YUYV8_2X8,
 125         },
 126         /* yuv420 */
 127         {
 128                 .fourcc = V4L2_PIX_FMT_NV21,
 129                 .uv_swap = 1,
 130                 .write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_SPLA,
 131                 .mbus = MEDIA_BUS_FMT_YUYV8_1_5X8,
 132         }, {
 133                 .fourcc = V4L2_PIX_FMT_NV12,
 134                 .uv_swap = 0,
 135                 .write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_SPLA,
 136                 .mbus = MEDIA_BUS_FMT_YUYV8_1_5X8,
 137         }, {
 138                 .fourcc = V4L2_PIX_FMT_NV21M,
 139                 .uv_swap = 1,
 140                 .write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_SPLA,
 141                 .mbus = MEDIA_BUS_FMT_YUYV8_1_5X8,
 142         }, {
 143                 .fourcc = V4L2_PIX_FMT_NV12M,
 144                 .uv_swap = 0,
 145                 .write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_SPLA,
 146                 .mbus = MEDIA_BUS_FMT_YUYV8_1_5X8,
 147         }, {
 148                 .fourcc = V4L2_PIX_FMT_YUV420,
 149                 .uv_swap = 0,
 150                 .write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_PLA_OR_RAW8,
 151                 .mbus = MEDIA_BUS_FMT_YUYV8_1_5X8,
 152         }, {
 153                 .fourcc = V4L2_PIX_FMT_YVU420,
 154                 .uv_swap = 1,
 155                 .write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_PLA_OR_RAW8,
 156                 .mbus = MEDIA_BUS_FMT_YUYV8_1_5X8,
 157         },
 158         /* raw */
 159         {
 160                 .fourcc = V4L2_PIX_FMT_SRGGB8,
 161                 .write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_PLA_OR_RAW8,
 162                 .mbus = MEDIA_BUS_FMT_SRGGB8_1X8,
 163         }, {
 164                 .fourcc = V4L2_PIX_FMT_SGRBG8,
 165                 .write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_PLA_OR_RAW8,
 166                 .mbus = MEDIA_BUS_FMT_SGRBG8_1X8,
 167         }, {
 168                 .fourcc = V4L2_PIX_FMT_SGBRG8,
 169                 .write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_PLA_OR_RAW8,
 170                 .mbus = MEDIA_BUS_FMT_SGBRG8_1X8,
 171         }, {
 172                 .fourcc = V4L2_PIX_FMT_SBGGR8,
 173                 .write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_PLA_OR_RAW8,
 174                 .mbus = MEDIA_BUS_FMT_SBGGR8_1X8,
 175         }, {
 176                 .fourcc = V4L2_PIX_FMT_SRGGB10,
 177                 .write_format = RKISP1_MI_CTRL_MP_WRITE_RAW12,
 178                 .mbus = MEDIA_BUS_FMT_SRGGB10_1X10,
 179         }, {
 180                 .fourcc = V4L2_PIX_FMT_SGRBG10,
 181                 .write_format = RKISP1_MI_CTRL_MP_WRITE_RAW12,
 182                 .mbus = MEDIA_BUS_FMT_SGRBG10_1X10,
 183         }, {
 184                 .fourcc = V4L2_PIX_FMT_SGBRG10,
 185                 .write_format = RKISP1_MI_CTRL_MP_WRITE_RAW12,
 186                 .mbus = MEDIA_BUS_FMT_SGBRG10_1X10,
 187         }, {
 188                 .fourcc = V4L2_PIX_FMT_SBGGR10,
 189                 .write_format = RKISP1_MI_CTRL_MP_WRITE_RAW12,
 190                 .mbus = MEDIA_BUS_FMT_SBGGR10_1X10,
 191         }, {
 192                 .fourcc = V4L2_PIX_FMT_SRGGB12,
 193                 .write_format = RKISP1_MI_CTRL_MP_WRITE_RAW12,
 194                 .mbus = MEDIA_BUS_FMT_SRGGB12_1X12,
 195         }, {
 196                 .fourcc = V4L2_PIX_FMT_SGRBG12,
 197                 .write_format = RKISP1_MI_CTRL_MP_WRITE_RAW12,
 198                 .mbus = MEDIA_BUS_FMT_SGRBG12_1X12,
 199         }, {
 200                 .fourcc = V4L2_PIX_FMT_SGBRG12,
 201                 .write_format = RKISP1_MI_CTRL_MP_WRITE_RAW12,
 202                 .mbus = MEDIA_BUS_FMT_SGBRG12_1X12,
 203         }, {
 204                 .fourcc = V4L2_PIX_FMT_SBGGR12,
 205                 .write_format = RKISP1_MI_CTRL_MP_WRITE_RAW12,
 206                 .mbus = MEDIA_BUS_FMT_SBGGR12_1X12,
 207         },
 208 };
 209
 210 /*
 211  * The supported pixel formats for selfpath. NOTE, pixel formats with identical 'mbus'
 212  * are grouped together. This is assumed and used by the function rkisp1_cap_enum_mbus_codes
 213  */
 214 static const struct rkisp1_capture_fmt_cfg rkisp1_sp_fmts[] = {
 215         /* yuv422 */
 216         {
 217                 .fourcc = V4L2_PIX_FMT_YUYV,
 218                 .uv_swap = 0,
 219                 .write_format = RKISP1_MI_CTRL_SP_WRITE_INT,
 220                 .output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV422,
 221                 .mbus = MEDIA_BUS_FMT_YUYV8_2X8,
 222         }, {
 223                 .fourcc = V4L2_PIX_FMT_YUV422P,
 224                 .uv_swap = 0,
 225                 .write_format = RKISP1_MI_CTRL_SP_WRITE_PLA,
 226                 .output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV422,
 227                 .mbus = MEDIA_BUS_FMT_YUYV8_2X8,
 228         }, {
 229                 .fourcc = V4L2_PIX_FMT_NV16,
 230                 .uv_swap = 0,
 231                 .write_format = RKISP1_MI_CTRL_SP_WRITE_SPLA,
 232                 .output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV422,
 233                 .mbus = MEDIA_BUS_FMT_YUYV8_2X8,
 234         }, {
 235                 .fourcc = V4L2_PIX_FMT_NV61,
 236                 .uv_swap = 1,
 237                 .write_format = RKISP1_MI_CTRL_SP_WRITE_SPLA,
 238                 .output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV422,
 239                 .mbus = MEDIA_BUS_FMT_YUYV8_2X8,
 240         }, {
 241                 .fourcc = V4L2_PIX_FMT_YVU422M,
 242                 .uv_swap = 1,
 243                 .write_format = RKISP1_MI_CTRL_SP_WRITE_PLA,
 244                 .output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV422,
 245                 .mbus = MEDIA_BUS_FMT_YUYV8_2X8,
 246         },
 247         /* yuv400 */
 248         {
 249                 .fourcc = V4L2_PIX_FMT_GREY,
 250                 .uv_swap = 0,
 251                 .write_format = RKISP1_MI_CTRL_SP_WRITE_PLA,
 252                 .output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV400,
 253                 .mbus = MEDIA_BUS_FMT_YUYV8_2X8,
 254         },
 255         /* rgb */
 256         {
 257                 .fourcc = V4L2_PIX_FMT_XBGR32,
 258                 .write_format = RKISP1_MI_CTRL_SP_WRITE_PLA,
 259                 .output_format = RKISP1_MI_CTRL_SP_OUTPUT_RGB888,
 260                 .mbus = MEDIA_BUS_FMT_YUYV8_2X8,
 261         }, {
 262                 .fourcc = V4L2_PIX_FMT_RGB565,
 263                 .write_format = RKISP1_MI_CTRL_SP_WRITE_PLA,
 264                 .output_format = RKISP1_MI_CTRL_SP_OUTPUT_RGB565,
 265                 .mbus = MEDIA_BUS_FMT_YUYV8_2X8,
 266         },
 267         /* yuv420 */
 268         {
 269                 .fourcc = V4L2_PIX_FMT_NV21,
 270                 .uv_swap = 1,
 271                 .write_format = RKISP1_MI_CTRL_SP_WRITE_SPLA,
 272                 .output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV420,
 273                 .mbus = MEDIA_BUS_FMT_YUYV8_1_5X8,
 274         }, {
 275                 .fourcc = V4L2_PIX_FMT_NV12,
 276                 .uv_swap = 0,
 277                 .write_format = RKISP1_MI_CTRL_SP_WRITE_SPLA,
 278                 .output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV420,
 279                 .mbus = MEDIA_BUS_FMT_YUYV8_1_5X8,
 280         }, {
 281                 .fourcc = V4L2_PIX_FMT_NV21M,
 282                 .uv_swap = 1,
 283                 .write_format = RKISP1_MI_CTRL_SP_WRITE_SPLA,
 284                 .output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV420,
 285                 .mbus = MEDIA_BUS_FMT_YUYV8_1_5X8,
 286         }, {
 287                 .fourcc = V4L2_PIX_FMT_NV12M,
 288                 .uv_swap = 0,
 289                 .write_format = RKISP1_MI_CTRL_SP_WRITE_SPLA,
 290                 .output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV420,
 291                 .mbus = MEDIA_BUS_FMT_YUYV8_1_5X8,
 292         }, {
 293                 .fourcc = V4L2_PIX_FMT_YUV420,
 294                 .uv_swap = 0,
 295                 .write_format = RKISP1_MI_CTRL_SP_WRITE_PLA,
 296                 .output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV420,
 297                 .mbus = MEDIA_BUS_FMT_YUYV8_1_5X8,
 298         }, {
 299                 .fourcc = V4L2_PIX_FMT_YVU420,
 300                 .uv_swap = 1,
 301                 .write_format = RKISP1_MI_CTRL_SP_WRITE_PLA,
 302                 .output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV420,
 303                 .mbus = MEDIA_BUS_FMT_YUYV8_1_5X8,
 304         },
 305 };
 306
 307 static const struct rkisp1_capture_config rkisp1_capture_config_mp = {
 308         .fmts = rkisp1_mp_fmts,
 309         .fmt_size = ARRAY_SIZE(rkisp1_mp_fmts),
 310         .mi = {
 311                 .y_size_init =          RKISP1_CIF_MI_MP_Y_SIZE_INIT,
 312                 .cb_size_init =         RKISP1_CIF_MI_MP_CB_SIZE_INIT,
 313                 .cr_size_init =         RKISP1_CIF_MI_MP_CR_SIZE_INIT,
 314                 .y_base_ad_init =       RKISP1_CIF_MI_MP_Y_BASE_AD_INIT,
 315                 .cb_base_ad_init =      RKISP1_CIF_MI_MP_CB_BASE_AD_INIT,
 316                 .cr_base_ad_init =      RKISP1_CIF_MI_MP_CR_BASE_AD_INIT,
 317                 .y_offs_cnt_init =      RKISP1_CIF_MI_MP_Y_OFFS_CNT_INIT,
 318                 .cb_offs_cnt_init =     RKISP1_CIF_MI_MP_CB_OFFS_CNT_INIT,
 319                 .cr_offs_cnt_init =     RKISP1_CIF_MI_MP_CR_OFFS_CNT_INIT,
 320         },
 321 };
 322
 323 static const struct rkisp1_capture_config rkisp1_capture_config_sp = {
 324         .fmts = rkisp1_sp_fmts,
 325         .fmt_size = ARRAY_SIZE(rkisp1_sp_fmts),
 326         .mi = {
 327                 .y_size_init =          RKISP1_CIF_MI_SP_Y_SIZE_INIT,
 328                 .cb_size_init =         RKISP1_CIF_MI_SP_CB_SIZE_INIT,
 329                 .cr_size_init =         RKISP1_CIF_MI_SP_CR_SIZE_INIT,
 330                 .y_base_ad_init =       RKISP1_CIF_MI_SP_Y_BASE_AD_INIT,
 331                 .cb_base_ad_init =      RKISP1_CIF_MI_SP_CB_BASE_AD_INIT,
 332                 .cr_base_ad_init =      RKISP1_CIF_MI_SP_CR_BASE_AD_INIT,
 333                 .y_offs_cnt_init =      RKISP1_CIF_MI_SP_Y_OFFS_CNT_INIT,
 334                 .cb_offs_cnt_init =     RKISP1_CIF_MI_SP_CB_OFFS_CNT_INIT,
 335                 .cr_offs_cnt_init =     RKISP1_CIF_MI_SP_CR_OFFS_CNT_INIT,
 336         },
 337 };
 338
 339 static inline struct rkisp1_vdev_node *
 340 rkisp1_vdev_to_node(struct video_device *vdev)
 341 {
 342         return container_of(vdev, struct rkisp1_vdev_node, vdev);
 343 }
 344
 345 int rkisp1_cap_enum_mbus_codes(struct rkisp1_capture *cap,
 346                                struct v4l2_subdev_mbus_code_enum *code)
 347 {
 348         const struct rkisp1_capture_fmt_cfg *fmts = cap->config->fmts;
 349         /*
 350          * initialize curr_mbus to non existing mbus code 0 to ensure it is
 351          * different from fmts[0].mbus
 352          */
 353         u32 curr_mbus = 0;
 354         int i, n = 0;
 355
 356         for (i = 0; i < cap->config->fmt_size; i++) {
 357                 if (fmts[i].mbus == curr_mbus)
 358                         continue;
 359
 360                 curr_mbus = fmts[i].mbus;
 361                 if (n++ == code->index) {
 362                         code->code = curr_mbus;
 363                         return 0;
 364                 }
 365         }
 366         return -EINVAL;
 367 }
 368
 369 /* ----------------------------------------------------------------------------
 370  * Stream operations for self-picture path (sp) and main-picture path (mp)
 371  */
 372
 373 static void rkisp1_mi_config_ctrl(struct rkisp1_capture *cap)
 374 {
 375         u32 mi_ctrl = rkisp1_read(cap->rkisp1, RKISP1_CIF_MI_CTRL);
 376
 377         mi_ctrl &= ~GENMASK(17, 16);
 378         mi_ctrl |= RKISP1_CIF_MI_CTRL_BURST_LEN_LUM_64;
 379
 380         mi_ctrl &= ~GENMASK(19, 18);
 381         mi_ctrl |= RKISP1_CIF_MI_CTRL_BURST_LEN_CHROM_64;
 382
 383         mi_ctrl |= RKISP1_CIF_MI_CTRL_INIT_BASE_EN |
 384                    RKISP1_CIF_MI_CTRL_INIT_OFFSET_EN;
 385
 386         rkisp1_write(cap->rkisp1, mi_ctrl, RKISP1_CIF_MI_CTRL);
 387 }
 388
 389 static u32 rkisp1_pixfmt_comp_size(const struct v4l2_pix_format_mplane *pixm,
 390                                    unsigned int component)
 391 {
 392         /*
 393          * If packed format, then plane_fmt[0].sizeimage is the sum of all
 394          * components, so we need to calculate just the size of Y component.
 395          * See rkisp1_fill_pixfmt().
 396          */
 397         if (!component && pixm->num_planes == 1)
 398                 return pixm->plane_fmt[0].bytesperline * pixm->height;
 399         return pixm->plane_fmt[component].sizeimage;
 400 }
 401
 402 static void rkisp1_irq_frame_end_enable(struct rkisp1_capture *cap)
 403 {
 404         u32 mi_imsc = rkisp1_read(cap->rkisp1, RKISP1_CIF_MI_IMSC);
 405
 406         mi_imsc |= RKISP1_CIF_MI_FRAME(cap);
 407         rkisp1_write(cap->rkisp1, mi_imsc, RKISP1_CIF_MI_IMSC);
 408 }
 409
 410 static void rkisp1_mp_config(struct rkisp1_capture *cap)
 411 {
 412         const struct v4l2_pix_format_mplane *pixm = &cap->pix.fmt;
 413         struct rkisp1_device *rkisp1 = cap->rkisp1;
 414         u32 reg;
 415
 416         rkisp1_write(rkisp1, rkisp1_pixfmt_comp_size(pixm, RKISP1_PLANE_Y),
 417                      cap->config->mi.y_size_init);
 418         rkisp1_write(rkisp1, rkisp1_pixfmt_comp_size(pixm, RKISP1_PLANE_CB),
 419                      cap->config->mi.cb_size_init);
 420         rkisp1_write(rkisp1, rkisp1_pixfmt_comp_size(pixm, RKISP1_PLANE_CR),
 421                      cap->config->mi.cr_size_init);
 422
 423         rkisp1_irq_frame_end_enable(cap);
 424
 425         /* set uv swapping for semiplanar formats */
 426         if (cap->pix.info->comp_planes == 2) {
 427                 reg = rkisp1_read(rkisp1, RKISP1_CIF_MI_XTD_FORMAT_CTRL);
 428                 if (cap->pix.cfg->uv_swap)
 429                         reg |= RKISP1_CIF_MI_XTD_FMT_CTRL_MP_CB_CR_SWAP;
 430                 else
 431                         reg &= ~RKISP1_CIF_MI_XTD_FMT_CTRL_MP_CB_CR_SWAP;
 432                 rkisp1_write(rkisp1, reg, RKISP1_CIF_MI_XTD_FORMAT_CTRL);
 433         }
 434
 435         rkisp1_mi_config_ctrl(cap);
 436
 437         reg = rkisp1_read(rkisp1, RKISP1_CIF_MI_CTRL);
 438         reg &= ~RKISP1_MI_CTRL_MP_FMT_MASK;
 439         reg |= cap->pix.cfg->write_format;
 440         rkisp1_write(rkisp1, reg, RKISP1_CIF_MI_CTRL);
 441
 442         reg = rkisp1_read(rkisp1, RKISP1_CIF_MI_CTRL);
 443         reg |= RKISP1_CIF_MI_MP_AUTOUPDATE_ENABLE;
 444         rkisp1_write(rkisp1, reg, RKISP1_CIF_MI_CTRL);
 445 }
 446
 447 static void rkisp1_sp_config(struct rkisp1_capture *cap)
 448 {
 449         const struct v4l2_pix_format_mplane *pixm = &cap->pix.fmt;
 450         struct rkisp1_device *rkisp1 = cap->rkisp1;
 451         u32 mi_ctrl, reg;
 452
 453         rkisp1_write(rkisp1, rkisp1_pixfmt_comp_size(pixm, RKISP1_PLANE_Y),
 454                      cap->config->mi.y_size_init);
 455         rkisp1_write(rkisp1, rkisp1_pixfmt_comp_size(pixm, RKISP1_PLANE_CB),
 456                      cap->config->mi.cb_size_init);
 457         rkisp1_write(rkisp1, rkisp1_pixfmt_comp_size(pixm, RKISP1_PLANE_CR),
 458                      cap->config->mi.cr_size_init);
 459
 460         rkisp1_write(rkisp1, pixm->width, RKISP1_CIF_MI_SP_Y_PIC_WIDTH);
 461         rkisp1_write(rkisp1, pixm->height, RKISP1_CIF_MI_SP_Y_PIC_HEIGHT);
 462         rkisp1_write(rkisp1, cap->sp_y_stride, RKISP1_CIF_MI_SP_Y_LLENGTH);
 463
 464         rkisp1_irq_frame_end_enable(cap);
 465
 466         /* set uv swapping for semiplanar formats */
 467         if (cap->pix.info->comp_planes == 2) {
 468                 reg = rkisp1_read(rkisp1, RKISP1_CIF_MI_XTD_FORMAT_CTRL);
 469                 if (cap->pix.cfg->uv_swap)
 470                         reg |= RKISP1_CIF_MI_XTD_FMT_CTRL_SP_CB_CR_SWAP;
 471                 else
 472                         reg &= ~RKISP1_CIF_MI_XTD_FMT_CTRL_SP_CB_CR_SWAP;
 473                 rkisp1_write(rkisp1, reg, RKISP1_CIF_MI_XTD_FORMAT_CTRL);
 474         }
 475
 476         rkisp1_mi_config_ctrl(cap);
 477
 478         mi_ctrl = rkisp1_read(rkisp1, RKISP1_CIF_MI_CTRL);
 479         mi_ctrl &= ~RKISP1_MI_CTRL_SP_FMT_MASK;
 480         mi_ctrl |= cap->pix.cfg->write_format |
 481                    RKISP1_MI_CTRL_SP_INPUT_YUV422 |
 482                    cap->pix.cfg->output_format |
 483                    RKISP1_CIF_MI_SP_AUTOUPDATE_ENABLE;
 484         rkisp1_write(rkisp1, mi_ctrl, RKISP1_CIF_MI_CTRL);
 485 }
 486
 487 static void rkisp1_mp_disable(struct rkisp1_capture *cap)
 488 {
 489         u32 mi_ctrl = rkisp1_read(cap->rkisp1, RKISP1_CIF_MI_CTRL);
 490
 491         mi_ctrl &= ~(RKISP1_CIF_MI_CTRL_MP_ENABLE |
 492                      RKISP1_CIF_MI_CTRL_RAW_ENABLE);
 493         rkisp1_write(cap->rkisp1, mi_ctrl, RKISP1_CIF_MI_CTRL);
 494 }
 495
 496 static void rkisp1_sp_disable(struct rkisp1_capture *cap)
 497 {
 498         u32 mi_ctrl = rkisp1_read(cap->rkisp1, RKISP1_CIF_MI_CTRL);
 499
 500         mi_ctrl &= ~RKISP1_CIF_MI_CTRL_SP_ENABLE;
 501         rkisp1_write(cap->rkisp1, mi_ctrl, RKISP1_CIF_MI_CTRL);
 502 }
 503
 504 static void rkisp1_mp_enable(struct rkisp1_capture *cap)
 505 {
 506         u32 mi_ctrl;
 507
 508         rkisp1_mp_disable(cap);
 509
 510         mi_ctrl = rkisp1_read(cap->rkisp1, RKISP1_CIF_MI_CTRL);
 511         if (v4l2_is_format_bayer(cap->pix.info))
 512                 mi_ctrl |= RKISP1_CIF_MI_CTRL_RAW_ENABLE;
 513         /* YUV */
 514         else
 515                 mi_ctrl |= RKISP1_CIF_MI_CTRL_MP_ENABLE;
 516
 517         rkisp1_write(cap->rkisp1, mi_ctrl, RKISP1_CIF_MI_CTRL);
 518 }
 519
 520 static void rkisp1_sp_enable(struct rkisp1_capture *cap)
 521 {
 522         u32 mi_ctrl = rkisp1_read(cap->rkisp1, RKISP1_CIF_MI_CTRL);
 523
 524         mi_ctrl |= RKISP1_CIF_MI_CTRL_SP_ENABLE;
 525         rkisp1_write(cap->rkisp1, mi_ctrl, RKISP1_CIF_MI_CTRL);
 526 }
 527
 528 static void rkisp1_mp_sp_stop(struct rkisp1_capture *cap)
 529 {
 530         if (!cap->is_streaming)
 531                 return;
 532         rkisp1_write(cap->rkisp1,
 533                      RKISP1_CIF_MI_FRAME(cap), RKISP1_CIF_MI_ICR);
 534         cap->ops->disable(cap);
 535 }
 536
 537 static bool rkisp1_mp_is_stopped(struct rkisp1_capture *cap)
 538 {
 539         u32 en = RKISP1_CIF_MI_CTRL_SHD_MP_IN_ENABLED |
 540                  RKISP1_CIF_MI_CTRL_SHD_RAW_OUT_ENABLED;
 541
 542         return !(rkisp1_read(cap->rkisp1, RKISP1_CIF_MI_CTRL_SHD) & en);
 543 }
 544
 545 static bool rkisp1_sp_is_stopped(struct rkisp1_capture *cap)
 546 {
 547         return !(rkisp1_read(cap->rkisp1, RKISP1_CIF_MI_CTRL_SHD) &
 548                  RKISP1_CIF_MI_CTRL_SHD_SP_IN_ENABLED);
 549 }
 550
 551 static void rkisp1_mp_set_data_path(struct rkisp1_capture *cap)
 552 {
 553         u32 dpcl = rkisp1_read(cap->rkisp1, RKISP1_CIF_VI_DPCL);
 554
 555         dpcl = dpcl | RKISP1_CIF_VI_DPCL_CHAN_MODE_MP |
 556                RKISP1_CIF_VI_DPCL_MP_MUX_MRSZ_MI;
 557         rkisp1_write(cap->rkisp1, dpcl, RKISP1_CIF_VI_DPCL);
 558 }
 559
 560 static void rkisp1_sp_set_data_path(struct rkisp1_capture *cap)
 561 {
 562         u32 dpcl = rkisp1_read(cap->rkisp1, RKISP1_CIF_VI_DPCL);
 563
 564         dpcl |= RKISP1_CIF_VI_DPCL_CHAN_MODE_SP;
 565         rkisp1_write(cap->rkisp1, dpcl, RKISP1_CIF_VI_DPCL);
 566 }
 567
 568 static struct rkisp1_capture_ops rkisp1_capture_ops_mp = {
 569         .config = rkisp1_mp_config,
 570         .enable = rkisp1_mp_enable,
 571         .disable = rkisp1_mp_disable,
 572         .stop = rkisp1_mp_sp_stop,
 573         .set_data_path = rkisp1_mp_set_data_path,
 574         .is_stopped = rkisp1_mp_is_stopped,
 575 };
 576
 577 static struct rkisp1_capture_ops rkisp1_capture_ops_sp = {
 578         .config = rkisp1_sp_config,
 579         .enable = rkisp1_sp_enable,
 580         .disable = rkisp1_sp_disable,
 581         .stop = rkisp1_mp_sp_stop,
 582         .set_data_path = rkisp1_sp_set_data_path,
 583         .is_stopped = rkisp1_sp_is_stopped,
 584 };
 585
 586 /* ----------------------------------------------------------------------------
 587  * Frame buffer operations
 588  */
 589
 590 static int rkisp1_dummy_buf_create(struct rkisp1_capture *cap)
 591 {
 592         const struct v4l2_pix_format_mplane *pixm = &cap->pix.fmt;
 593         struct rkisp1_dummy_buffer *dummy_buf = &cap->buf.dummy;
 594
 595         dummy_buf->size = max3(rkisp1_pixfmt_comp_size(pixm, RKISP1_PLANE_Y),
 596                                rkisp1_pixfmt_comp_size(pixm, RKISP1_PLANE_CB),
 597                                rkisp1_pixfmt_comp_size(pixm, RKISP1_PLANE_CR));
 598
 599         /* The driver never access vaddr, no mapping is required */
 600         dummy_buf->vaddr = dma_alloc_attrs(cap->rkisp1->dev,
 601                                            dummy_buf->size,
 602                                            &dummy_buf->dma_addr,
 603                                            GFP_KERNEL,
 604                                            DMA_ATTR_NO_KERNEL_MAPPING);
 605         if (!dummy_buf->vaddr)
 606                 return -ENOMEM;
 607
 608         return 0;
 609 }
 610
 611 static void rkisp1_dummy_buf_destroy(struct rkisp1_capture *cap)
 612 {
 613         dma_free_attrs(cap->rkisp1->dev,
 614                        cap->buf.dummy.size, cap->buf.dummy.vaddr,
 615                        cap->buf.dummy.dma_addr, DMA_ATTR_NO_KERNEL_MAPPING);
 616 }
 617
 618 static void rkisp1_set_next_buf(struct rkisp1_capture *cap)
 619 {
 620         cap->buf.curr = cap->buf.next;
 621         cap->buf.next = NULL;
 622
 623         if (!list_empty(&cap->buf.queue)) {
 624                 u32 *buff_addr;
 625
 626                 cap->buf.next = list_first_entry(&cap->buf.queue, struct rkisp1_buffer, queue);
 627                 list_del(&cap->buf.next->queue);
 628
 629                 buff_addr = cap->buf.next->buff_addr;
 630
 631                 rkisp1_write(cap->rkisp1,
 632                              buff_addr[RKISP1_PLANE_Y],
 633                              cap->config->mi.y_base_ad_init);
 634                 rkisp1_write(cap->rkisp1,
 635                              buff_addr[RKISP1_PLANE_CB],
 636                              cap->config->mi.cb_base_ad_init);
 637                 rkisp1_write(cap->rkisp1,
 638                              buff_addr[RKISP1_PLANE_CR],
 639                              cap->config->mi.cr_base_ad_init);
 640         } else {
 641                 /*
 642                  * Use the dummy space allocated by dma_alloc_coherent to
 643                  * throw data if there is no available buffer.
 644                  */
 645                 rkisp1_write(cap->rkisp1,
 646                              cap->buf.dummy.dma_addr,
 647                              cap->config->mi.y_base_ad_init);
 648                 rkisp1_write(cap->rkisp1,
 649                              cap->buf.dummy.dma_addr,
 650                              cap->config->mi.cb_base_ad_init);
 651                 rkisp1_write(cap->rkisp1,
 652                              cap->buf.dummy.dma_addr,
 653                              cap->config->mi.cr_base_ad_init);
 654         }
 655
 656         /* Set plane offsets */
 657         rkisp1_write(cap->rkisp1, 0, cap->config->mi.y_offs_cnt_init);
 658         rkisp1_write(cap->rkisp1, 0, cap->config->mi.cb_offs_cnt_init);
 659         rkisp1_write(cap->rkisp1, 0, cap->config->mi.cr_offs_cnt_init);
 660 }
 661
 662 /*
 663  * This function is called when a frame end comes. The next frame
 664  * is processing and we should set up buffer for next-next frame,
 665  * otherwise it will overflow.
 666  */
 667 static void rkisp1_handle_buffer(struct rkisp1_capture *cap)
 668 {
 669         struct rkisp1_isp *isp = &cap->rkisp1->isp;
 670         struct rkisp1_buffer *curr_buf;
 671
 672         spin_lock(&cap->buf.lock);
 673         curr_buf = cap->buf.curr;
 674
 675         if (curr_buf) {
 676                 curr_buf->vb.sequence = isp->frame_sequence;
 677                 curr_buf->vb.vb2_buf.timestamp = ktime_get_boottime_ns();
 678                 curr_buf->vb.field = V4L2_FIELD_NONE;
 679                 vb2_buffer_done(&curr_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
 680         } else {
 681                 cap->rkisp1->debug.frame_drop[cap->id]++;
 682         }
 683
 684         rkisp1_set_next_buf(cap);
 685         spin_unlock(&cap->buf.lock);
 686 }
 687
 688 void rkisp1_capture_isr(struct rkisp1_device *rkisp1)
 689 {
 690         unsigned int i;
 691         u32 status;
 692
 693         status = rkisp1_read(rkisp1, RKISP1_CIF_MI_MIS);
 694         rkisp1_write(rkisp1, status, RKISP1_CIF_MI_ICR);
 695
 696         for (i = 0; i < ARRAY_SIZE(rkisp1->capture_devs); ++i) {
 697                 struct rkisp1_capture *cap = &rkisp1->capture_devs[i];
 698
 699                 if (!(status & RKISP1_CIF_MI_FRAME(cap)))
 700                         continue;
 701                 if (!cap->is_stopping) {
 702                         rkisp1_handle_buffer(cap);
 703                         continue;
 704                 }
 705                 /*
 706                  * Make sure stream is actually stopped, whose state
 707                  * can be read from the shadow register, before
 708                  * wake_up() thread which would immediately free all
 709                  * frame buffers. stop() takes effect at the next
 710                  * frame end that sync the configurations to shadow
 711                  * regs.
 712                  */
 713                 if (!cap->ops->is_stopped(cap)) {
 714                         cap->ops->stop(cap);
 715                         continue;
 716                 }
 717                 cap->is_stopping = false;
 718                 cap->is_streaming = false;
 719                 wake_up(&cap->done);
 720         }
 721 }
 722
 723 /* ----------------------------------------------------------------------------
 724  * Vb2 operations
 725  */
 726
 727 static int rkisp1_vb2_queue_setup(struct vb2_queue *queue,
 728                                   unsigned int *num_buffers,
 729                                   unsigned int *num_planes,
 730                                   unsigned int sizes[],
 731                                   struct device *alloc_devs[])
 732 {
 733         struct rkisp1_capture *cap = queue->drv_priv;
 734         const struct v4l2_pix_format_mplane *pixm = &cap->pix.fmt;
 735         unsigned int i;
 736
 737         if (*num_planes) {
 738                 if (*num_planes != pixm->num_planes)
 739                         return -EINVAL;
 740
 741                 for (i = 0; i < pixm->num_planes; i++)
 742                         if (sizes[i] < pixm->plane_fmt[i].sizeimage)
 743                                 return -EINVAL;
 744         } else {
 745                 *num_planes = pixm->num_planes;
 746                 for (i = 0; i < pixm->num_planes; i++)
 747                         sizes[i] = pixm->plane_fmt[i].sizeimage;
 748         }
 749
 750         return 0;
 751 }
 752
 753 static void rkisp1_vb2_buf_queue(struct vb2_buffer *vb)
 754 {
 755         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 756         struct rkisp1_buffer *ispbuf =
 757                 container_of(vbuf, struct rkisp1_buffer, vb);
 758         struct rkisp1_capture *cap = vb->vb2_queue->drv_priv;
 759         const struct v4l2_pix_format_mplane *pixm = &cap->pix.fmt;
 760         unsigned int i;
 761
 762         memset(ispbuf->buff_addr, 0, sizeof(ispbuf->buff_addr));
 763         for (i = 0; i < pixm->num_planes; i++)
 764                 ispbuf->buff_addr[i] = vb2_dma_contig_plane_dma_addr(vb, i);
 765
 766         /* Convert to non-MPLANE */
 767         if (pixm->num_planes == 1) {
 768                 ispbuf->buff_addr[RKISP1_PLANE_CB] =
 769                         ispbuf->buff_addr[RKISP1_PLANE_Y] +
 770                         rkisp1_pixfmt_comp_size(pixm, RKISP1_PLANE_Y);
 771                 ispbuf->buff_addr[RKISP1_PLANE_CR] =
 772                         ispbuf->buff_addr[RKISP1_PLANE_CB] +
 773                         rkisp1_pixfmt_comp_size(pixm, RKISP1_PLANE_CB);
 774         }
 775
 776         /*
 777          * uv swap can be supported for planar formats by switching
 778          * the address of cb and cr
 779          */
 780         if (cap->pix.info->comp_planes == 3 && cap->pix.cfg->uv_swap)
 781                 swap(ispbuf->buff_addr[RKISP1_PLANE_CR],
 782                      ispbuf->buff_addr[RKISP1_PLANE_CB]);
 783
 784         spin_lock_irq(&cap->buf.lock);
 785         list_add_tail(&ispbuf->queue, &cap->buf.queue);
 786         spin_unlock_irq(&cap->buf.lock);
 787 }
 788
 789 static int rkisp1_vb2_buf_prepare(struct vb2_buffer *vb)
 790 {
 791         struct rkisp1_capture *cap = vb->vb2_queue->drv_priv;
 792         unsigned int i;
 793
 794         for (i = 0; i < cap->pix.fmt.num_planes; i++) {
 795                 unsigned long size = cap->pix.fmt.plane_fmt[i].sizeimage;
 796
 797                 if (vb2_plane_size(vb, i) < size) {
 798                         dev_err(cap->rkisp1->dev,
 799                                 "User buffer too small (%ld < %ld)\n",
 800                                 vb2_plane_size(vb, i), size);
 801                         return -EINVAL;
 802                 }
 803                 vb2_set_plane_payload(vb, i, size);
 804         }
 805
 806         return 0;
 807 }
 808
 809 static void rkisp1_return_all_buffers(struct rkisp1_capture *cap,
 810                                       enum vb2_buffer_state state)
 811 {
 812         struct rkisp1_buffer *buf;
 813
 814         spin_lock_irq(&cap->buf.lock);
 815         if (cap->buf.curr) {
 816                 vb2_buffer_done(&cap->buf.curr->vb.vb2_buf, state);
 817                 cap->buf.curr = NULL;
 818         }
 819         if (cap->buf.next) {
 820                 vb2_buffer_done(&cap->buf.next->vb.vb2_buf, state);
 821                 cap->buf.next = NULL;
 822         }
 823         while (!list_empty(&cap->buf.queue)) {
 824                 buf = list_first_entry(&cap->buf.queue,
 825                                        struct rkisp1_buffer, queue);
 826                 list_del(&buf->queue);
 827                 vb2_buffer_done(&buf->vb.vb2_buf, state);
 828         }
 829         spin_unlock_irq(&cap->buf.lock);
 830 }
 831
 832 /*
 833  * rkisp1_pipeline_sink_walk - Walk through the pipeline and call cb
 834  * @from: entity at which to start pipeline walk
 835  * @until: entity at which to stop pipeline walk
 836  *
 837  * Walk the entities chain starting at the pipeline video node and stop
 838  * all subdevices in the chain.
 839  *
 840  * If the until argument isn't NULL, stop the pipeline walk when reaching the
 841  * until entity. This is used to disable a partially started pipeline due to a
 842  * subdev start error.
 843  */
 844 static int rkisp1_pipeline_sink_walk(struct media_entity *from,
 845                                      struct media_entity *until,
 846                                      int (*cb)(struct media_entity *from,
 847                                                struct media_entity *curr))
 848 {
 849         struct media_entity *entity = from;
 850         struct media_pad *pad;
 851         unsigned int i;
 852         int ret;
 853
 854         while (1) {
 855                 pad = NULL;
 856                 /* Find remote source pad */
 857                 for (i = 0; i < entity->num_pads; i++) {
 858                         struct media_pad *spad = &entity->pads[i];
 859
 860                         if (!(spad->flags & MEDIA_PAD_FL_SINK))
 861                                 continue;
 862                         pad = media_entity_remote_pad(spad);
 863                         if (pad && is_media_entity_v4l2_subdev(pad->entity))
 864                                 break;
 865                 }
 866                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 867                         break;
 868
 869                 entity = pad->entity;
 870                 if (entity == until)
 871                         break;
 872
 873                 ret = cb(from, entity);
 874                 if (ret)
 875                         return ret;
 876         }
 877
 878         return 0;
 879 }
 880
 881 static int rkisp1_pipeline_disable_cb(struct media_entity *from,
 882                                       struct media_entity *curr)
 883 {
 884         struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(curr);
 885
 886         return v4l2_subdev_call(sd, video, s_stream, false);
 887 }
 888
 889 static int rkisp1_pipeline_enable_cb(struct media_entity *from,
 890                                      struct media_entity *curr)
 891 {
 892         struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(curr);
 893
 894         return v4l2_subdev_call(sd, video, s_stream, true);
 895 }
 896
 897 static void rkisp1_stream_stop(struct rkisp1_capture *cap)
 898 {
 899         int ret;
 900
 901         /* Stream should stop in interrupt. If it dosn't, stop it by force. */
 902         cap->is_stopping = true;
 903         ret = wait_event_timeout(cap->done,
 904                                  !cap->is_streaming,
 905                                  msecs_to_jiffies(1000));
 906         if (!ret) {
 907                 cap->rkisp1->debug.stop_timeout[cap->id]++;
 908                 cap->ops->stop(cap);
 909                 cap->is_stopping = false;
 910                 cap->is_streaming = false;
 911         }
 912 }
 913
 914 static void rkisp1_vb2_stop_streaming(struct vb2_queue *queue)
 915 {
 916         struct rkisp1_capture *cap = queue->drv_priv;
 917         struct rkisp1_vdev_node *node = &cap->vnode;
 918         struct rkisp1_device *rkisp1 = cap->rkisp1;
 919         int ret;
 920
 921         mutex_lock(&cap->rkisp1->stream_lock);
 922
 923         rkisp1_stream_stop(cap);
 924         media_pipeline_stop(&node->vdev.entity);
 925         ret = rkisp1_pipeline_sink_walk(&node->vdev.entity, NULL,
 926                                         rkisp1_pipeline_disable_cb);
 927         if (ret)
 928                 dev_err(rkisp1->dev,
 929                         "pipeline stream-off failed error:%d\n", ret);
 930
 931         rkisp1_return_all_buffers(cap, VB2_BUF_STATE_ERROR);
 932
 933         v4l2_pipeline_pm_put(&node->vdev.entity);
 934         ret = pm_runtime_put(rkisp1->dev);
 935         if (ret < 0)
 936                 dev_err(rkisp1->dev, "power down failed error:%d\n", ret);
 937
 938         rkisp1_dummy_buf_destroy(cap);
 939
 940         mutex_unlock(&cap->rkisp1->stream_lock);
 941 }
 942
 943 /*
 944  * Most of registers inside rockchip ISP1 have shadow register since
 945  * they must be not be changed during processing a frame.
 946  * Usually, each sub-module updates its shadow register after
 947  * processing the last pixel of a frame.
 948  */
 949 static void rkisp1_stream_start(struct rkisp1_capture *cap)
 950 {
 951         struct rkisp1_device *rkisp1 = cap->rkisp1;
 952         struct rkisp1_capture *other = &rkisp1->capture_devs[cap->id ^ 1];
 953
 954         cap->ops->set_data_path(cap);
 955         cap->ops->config(cap);
 956
 957         /* Setup a buffer for the next frame */
 958         spin_lock_irq(&cap->buf.lock);
 959         rkisp1_set_next_buf(cap);
 960         cap->ops->enable(cap);
 961         /* It's safe to config ACTIVE and SHADOW regs for the
 962          * first stream. While when the second is starting, do NOT
 963          * force update because it also update the first one.
 964          *
 965          * The latter case would drop one more buf(that is 2) since
 966          * there's not buf in shadow when the second FE received. This's
 967          * also required because the second FE maybe corrupt especially
 968          * when run at 120fps.
 969          */
 970         if (!other->is_streaming) {
 971                 /* force cfg update */
 972                 rkisp1_write(rkisp1,
 973                              RKISP1_CIF_MI_INIT_SOFT_UPD, RKISP1_CIF_MI_INIT);
 974                 rkisp1_set_next_buf(cap);
 975         }
 976         spin_unlock_irq(&cap->buf.lock);
 977         cap->is_streaming = true;
 978 }
 979
 980 static int
 981 rkisp1_vb2_start_streaming(struct vb2_queue *queue, unsigned int count)
 982 {
 983         struct rkisp1_capture *cap = queue->drv_priv;
 984         struct media_entity *entity = &cap->vnode.vdev.entity;
 985         int ret;
 986
 987         mutex_lock(&cap->rkisp1->stream_lock);
 988
 989         ret = rkisp1_dummy_buf_create(cap);
 990         if (ret)
 991                 goto err_ret_buffers;
 992
 993         ret = pm_runtime_get_sync(cap->rkisp1->dev);
 994         if (ret < 0) {
 995                 pm_runtime_put_noidle(cap->rkisp1->dev);
 996                 dev_err(cap->rkisp1->dev, "power up failed %d\n", ret);
 997                 goto err_destroy_dummy;
 998         }
 999         ret = v4l2_pipeline_pm_get(entity);
1000         if (ret) {
1001                 dev_err(cap->rkisp1->dev, "open cif pipeline failed %d\n", ret);
1002                 goto err_pipe_pm_put;
1003         }
1004
1005         rkisp1_stream_start(cap);
1006
1007         /* start sub-devices */
1008         ret = rkisp1_pipeline_sink_walk(entity, NULL,
1009                                         rkisp1_pipeline_enable_cb);
1010         if (ret)
1011                 goto err_stop_stream;
1012
1013         ret = media_pipeline_start(entity, &cap->rkisp1->pipe);
1014         if (ret) {
1015                 dev_err(cap->rkisp1->dev, "start pipeline failed %d\n", ret);
1016                 goto err_pipe_disable;
1017         }
1018
1019         mutex_unlock(&cap->rkisp1->stream_lock);
1020
1021         return 0;
1022
1023 err_pipe_disable:
1024         rkisp1_pipeline_sink_walk(entity, NULL, rkisp1_pipeline_disable_cb);
1025 err_stop_stream:
1026         rkisp1_stream_stop(cap);
1027         v4l2_pipeline_pm_put(entity);
1028 err_pipe_pm_put:
1029         pm_runtime_put(cap->rkisp1->dev);
1030 err_destroy_dummy:
1031         rkisp1_dummy_buf_destroy(cap);
1032 err_ret_buffers:
1033         rkisp1_return_all_buffers(cap, VB2_BUF_STATE_QUEUED);
1034         mutex_unlock(&cap->rkisp1->stream_lock);
1035
1036         return ret;
1037 }
1038
1039 static struct vb2_ops rkisp1_vb2_ops = {
1040         .queue_setup = rkisp1_vb2_queue_setup,
1041         .buf_queue = rkisp1_vb2_buf_queue,
1042         .buf_prepare = rkisp1_vb2_buf_prepare,
1043         .wait_prepare = vb2_ops_wait_prepare,
1044         .wait_finish = vb2_ops_wait_finish,
1045         .stop_streaming = rkisp1_vb2_stop_streaming,
1046         .start_streaming = rkisp1_vb2_start_streaming,
1047 };
1048
1049 /* ----------------------------------------------------------------------------
1050  * IOCTLs operations
1051  */
1052
1053 static const struct v4l2_format_info *
1054 rkisp1_fill_pixfmt(struct v4l2_pix_format_mplane *pixm,
1055                    enum rkisp1_stream_id id)
1056 {
1057         struct v4l2_plane_pix_format *plane_y = &pixm->plane_fmt[0];
1058         const struct v4l2_format_info *info;
1059         unsigned int i;
1060         u32 stride;
1061
1062         memset(pixm->plane_fmt, 0, sizeof(pixm->plane_fmt));
1063         info = v4l2_format_info(pixm->pixelformat);
1064         pixm->num_planes = info->mem_planes;
1065         stride = info->bpp[0] * pixm->width;
1066         /* Self path supports custom stride but Main path doesn't */
1067         if (id == RKISP1_MAINPATH || plane_y->bytesperline < stride)
1068                 plane_y->bytesperline = stride;
1069         plane_y->sizeimage = plane_y->bytesperline * pixm->height;
1070
1071         /* normalize stride to pixels per line */
1072         stride = DIV_ROUND_UP(plane_y->bytesperline, info->bpp[0]);
1073
1074         for (i = 1; i < info->comp_planes; i++) {
1075                 struct v4l2_plane_pix_format *plane = &pixm->plane_fmt[i];
1076
1077                 /* bytesperline for other components derive from Y component */
1078                 plane->bytesperline = DIV_ROUND_UP(stride, info->hdiv) *
1079                                       info->bpp[i];
1080                 plane->sizeimage = plane->bytesperline *
1081                                    DIV_ROUND_UP(pixm->height, info->vdiv);
1082         }
1083
1084         /*
1085          * If pixfmt is packed, then plane_fmt[0] should contain the total size
1086          * considering all components. plane_fmt[i] for i > 0 should be ignored
1087          * by userspace as mem_planes == 1, but we are keeping information there
1088          * for convenience.
1089          */
1090         if (info->mem_planes == 1)
1091                 for (i = 1; i < info->comp_planes; i++)
1092                         plane_y->sizeimage += pixm->plane_fmt[i].sizeimage;
1093
1094         return info;
1095 }
1096
1097 static const struct rkisp1_capture_fmt_cfg *
1098 rkisp1_find_fmt_cfg(const struct rkisp1_capture *cap, const u32 pixelfmt)
1099 {
1100         unsigned int i;
1101
1102         for (i = 0; i < cap->config->fmt_size; i++) {
1103                 if (cap->config->fmts[i].fourcc == pixelfmt)
1104                         return &cap->config->fmts[i];
1105         }
1106         return NULL;
1107 }
1108
1109 static void rkisp1_try_fmt(const struct rkisp1_capture *cap,
1110                            struct v4l2_pix_format_mplane *pixm,
1111                            const struct rkisp1_capture_fmt_cfg **fmt_cfg,
1112                            const struct v4l2_format_info **fmt_info)
1113 {
1114         const struct rkisp1_capture_config *config = cap->config;
1115         const struct rkisp1_capture_fmt_cfg *fmt;
1116         const struct v4l2_format_info *info;
1117         const unsigned int max_widths[] = { RKISP1_RSZ_MP_SRC_MAX_WIDTH,
1118                                             RKISP1_RSZ_SP_SRC_MAX_WIDTH };
1119         const unsigned int max_heights[] = { RKISP1_RSZ_MP_SRC_MAX_HEIGHT,
1120                                              RKISP1_RSZ_SP_SRC_MAX_HEIGHT};
1121
1122         fmt = rkisp1_find_fmt_cfg(cap, pixm->pixelformat);
1123         if (!fmt) {
1124                 fmt = config->fmts;
1125                 pixm->pixelformat = fmt->fourcc;
1126         }
1127
1128         pixm->width = clamp_t(u32, pixm->width,
1129                               RKISP1_RSZ_SRC_MIN_WIDTH, max_widths[cap->id]);
1130         pixm->height = clamp_t(u32, pixm->height,
1131                                RKISP1_RSZ_SRC_MIN_HEIGHT, max_heights[cap->id]);
1132
1133         pixm->field = V4L2_FIELD_NONE;
1134         pixm->colorspace = V4L2_COLORSPACE_DEFAULT;
1135         pixm->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
1136
1137         info = rkisp1_fill_pixfmt(pixm, cap->id);
1138
1139         if (fmt_cfg)
1140                 *fmt_cfg = fmt;
1141         if (fmt_info)
1142                 *fmt_info = info;
1143 }
1144
1145 static void rkisp1_set_fmt(struct rkisp1_capture *cap,
1146                            struct v4l2_pix_format_mplane *pixm)
1147 {
1148         rkisp1_try_fmt(cap, pixm, &cap->pix.cfg, &cap->pix.info);
1149         cap->pix.fmt = *pixm;
1150
1151         /* SP supports custom stride in number of pixels of the Y plane */
1152         if (cap->id == RKISP1_SELFPATH)
1153                 cap->sp_y_stride = pixm->plane_fmt[0].bytesperline /
1154                                    cap->pix.info->bpp[0];
1155 }
1156
1157 static int rkisp1_try_fmt_vid_cap_mplane(struct file *file, void *fh,
1158                                          struct v4l2_format *f)
1159 {
1160         struct rkisp1_capture *cap = video_drvdata(file);
1161
1162         rkisp1_try_fmt(cap, &f->fmt.pix_mp, NULL, NULL);
1163
1164         return 0;
1165 }
1166
1167 static int rkisp1_enum_fmt_vid_cap_mplane(struct file *file, void *priv,
1168                                           struct v4l2_fmtdesc *f)
1169 {
1170         struct rkisp1_capture *cap = video_drvdata(file);
1171         const struct rkisp1_capture_fmt_cfg *fmt = NULL;
1172         unsigned int i, n = 0;
1173
1174         if (!f->mbus_code) {
1175                 if (f->index >= cap->config->fmt_size)
1176                         return -EINVAL;
1177
1178                 fmt = &cap->config->fmts[f->index];
1179                 f->pixelformat = fmt->fourcc;
1180                 return 0;
1181         }
1182
1183         for (i = 0; i < cap->config->fmt_size; i++) {
1184                 if (cap->config->fmts[i].mbus != f->mbus_code)
1185                         continue;
1186
1187                 if (n++ == f->index) {
1188                         f->pixelformat = cap->config->fmts[i].fourcc;
1189                         return 0;
1190                 }
1191         }
1192         return -EINVAL;
1193 }
1194
1195 static int rkisp1_s_fmt_vid_cap_mplane(struct file *file,
1196                                        void *priv, struct v4l2_format *f)
1197 {
1198         struct rkisp1_capture *cap = video_drvdata(file);
1199         struct rkisp1_vdev_node *node =
1200                                 rkisp1_vdev_to_node(&cap->vnode.vdev);
1201
1202         if (vb2_is_busy(&node->buf_queue))
1203                 return -EBUSY;
1204
1205         rkisp1_set_fmt(cap, &f->fmt.pix_mp);
1206
1207         return 0;
1208 }
1209
1210 static int rkisp1_g_fmt_vid_cap_mplane(struct file *file, void *fh,
1211                                        struct v4l2_format *f)
1212 {
1213         struct rkisp1_capture *cap = video_drvdata(file);
1214
1215         f->fmt.pix_mp = cap->pix.fmt;
1216
1217         return 0;
1218 }
1219
1220 static int
1221 rkisp1_querycap(struct file *file, void *priv, struct v4l2_capability *cap)
1222 {
1223         struct rkisp1_capture *cap_dev = video_drvdata(file);
1224         struct rkisp1_device *rkisp1 = cap_dev->rkisp1;
1225
1226         strscpy(cap->driver, rkisp1->dev->driver->name, sizeof(cap->driver));
1227         strscpy(cap->card, rkisp1->dev->driver->name, sizeof(cap->card));
1228         strscpy(cap->bus_info, RKISP1_BUS_INFO, sizeof(cap->bus_info));
1229
1230         return 0;
1231 }
1232
1233 static const struct v4l2_ioctl_ops rkisp1_v4l2_ioctl_ops = {
1234         .vidioc_reqbufs = vb2_ioctl_reqbufs,
1235         .vidioc_querybuf = vb2_ioctl_querybuf,
1236         .vidioc_create_bufs = vb2_ioctl_create_bufs,
1237         .vidioc_qbuf = vb2_ioctl_qbuf,
1238         .vidioc_expbuf = vb2_ioctl_expbuf,
1239         .vidioc_dqbuf = vb2_ioctl_dqbuf,
1240         .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
1241         .vidioc_streamon = vb2_ioctl_streamon,
1242         .vidioc_streamoff = vb2_ioctl_streamoff,
1243         .vidioc_try_fmt_vid_cap_mplane = rkisp1_try_fmt_vid_cap_mplane,
1244         .vidioc_s_fmt_vid_cap_mplane = rkisp1_s_fmt_vid_cap_mplane,
1245         .vidioc_g_fmt_vid_cap_mplane = rkisp1_g_fmt_vid_cap_mplane,
1246         .vidioc_enum_fmt_vid_cap = rkisp1_enum_fmt_vid_cap_mplane,
1247         .vidioc_querycap = rkisp1_querycap,
1248         .vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
1249         .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
1250 };
1251
1252 static int rkisp1_capture_link_validate(struct media_link *link)
1253 {
1254         struct video_device *vdev =
1255                 media_entity_to_video_device(link->sink->entity);
1256         struct v4l2_subdev *sd =
1257                 media_entity_to_v4l2_subdev(link->source->entity);
1258         struct rkisp1_capture *cap = video_get_drvdata(vdev);
1259         const struct rkisp1_capture_fmt_cfg *fmt =
1260                 rkisp1_find_fmt_cfg(cap, cap->pix.fmt.pixelformat);
1261         struct v4l2_subdev_format sd_fmt = {
1262                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1263                 .pad = link->source->index,
1264         };
1265         int ret;
1266
1267         ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &sd_fmt);
1268         if (ret)
1269                 return ret;
1270
1271         if (sd_fmt.format.height != cap->pix.fmt.height ||
1272             sd_fmt.format.width != cap->pix.fmt.width ||
1273             sd_fmt.format.code != fmt->mbus)
1274                 return -EPIPE;
1275
1276         return 0;
1277 }
1278
1279 /* ----------------------------------------------------------------------------
1280  * core functions
1281  */
1282
1283 static const struct media_entity_operations rkisp1_media_ops = {
1284         .link_validate = rkisp1_capture_link_validate,
1285 };
1286
1287 static const struct v4l2_file_operations rkisp1_fops = {
1288         .open = v4l2_fh_open,
1289         .release = vb2_fop_release,
1290         .unlocked_ioctl = video_ioctl2,
1291         .poll = vb2_fop_poll,
1292         .mmap = vb2_fop_mmap,
1293 };
1294
1295 static void rkisp1_unregister_capture(struct rkisp1_capture *cap)
1296 {
1297         media_entity_cleanup(&cap->vnode.vdev.entity);
1298         vb2_video_unregister_device(&cap->vnode.vdev);
1299 }
1300
1301 void rkisp1_capture_devs_unregister(struct rkisp1_device *rkisp1)
1302 {
1303         struct rkisp1_capture *mp = &rkisp1->capture_devs[RKISP1_MAINPATH];
1304         struct rkisp1_capture *sp = &rkisp1->capture_devs[RKISP1_SELFPATH];
1305
1306         rkisp1_unregister_capture(mp);
1307         rkisp1_unregister_capture(sp);
1308 }
1309
1310 static int rkisp1_register_capture(struct rkisp1_capture *cap)
1311 {
1312         const char * const dev_names[] = {RKISP1_MP_DEV_NAME,
1313                                           RKISP1_SP_DEV_NAME};
1314         struct v4l2_device *v4l2_dev = &cap->rkisp1->v4l2_dev;
1315         struct video_device *vdev = &cap->vnode.vdev;
1316         struct rkisp1_vdev_node *node;
1317         struct vb2_queue *q;
1318         int ret;
1319
1320         strscpy(vdev->name, dev_names[cap->id], sizeof(vdev->name));
1321         node = rkisp1_vdev_to_node(vdev);
1322         mutex_init(&node->vlock);
1323
1324         vdev->ioctl_ops = &rkisp1_v4l2_ioctl_ops;
1325         vdev->release = video_device_release_empty;
1326         vdev->fops = &rkisp1_fops;
1327         vdev->minor = -1;
1328         vdev->v4l2_dev = v4l2_dev;
1329         vdev->lock = &node->vlock;
1330         vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE |
1331                             V4L2_CAP_STREAMING | V4L2_CAP_IO_MC;
1332         vdev->entity.ops = &rkisp1_media_ops;
1333         video_set_drvdata(vdev, cap);
1334         vdev->vfl_dir = VFL_DIR_RX;
1335         node->pad.flags = MEDIA_PAD_FL_SINK;
1336
1337         q = &node->buf_queue;
1338         q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
1339         q->io_modes = VB2_MMAP | VB2_DMABUF;
1340         q->drv_priv = cap;
1341         q->ops = &rkisp1_vb2_ops;
1342         q->mem_ops = &vb2_dma_contig_memops;
1343         q->buf_struct_size = sizeof(struct rkisp1_buffer);
1344         q->min_buffers_needed = RKISP1_MIN_BUFFERS_NEEDED;
1345         q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1346         q->lock = &node->vlock;
1347         q->dev = cap->rkisp1->dev;
1348         ret = vb2_queue_init(q);
1349         if (ret) {
1350                 dev_err(cap->rkisp1->dev,
1351                         "vb2 queue init failed (err=%d)\n", ret);
1352                 return ret;
1353         }
1354
1355         vdev->queue = q;
1356
1357         ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
1358         if (ret) {
1359                 dev_err(cap->rkisp1->dev,
1360                         "failed to register %s, ret=%d\n", vdev->name, ret);
1361                 return ret;
1362         }
1363         v4l2_info(v4l2_dev, "registered %s as /dev/video%d\n", vdev->name,
1364                   vdev->num);
1365
1366         ret = media_entity_pads_init(&vdev->entity, 1, &node->pad);
1367         if (ret) {
1368                 video_unregister_device(vdev);
1369                 return ret;
1370         }
1371
1372         return 0;
1373 }
1374
1375 static void
1376 rkisp1_capture_init(struct rkisp1_device *rkisp1, enum rkisp1_stream_id id)
1377 {
1378         struct rkisp1_capture *cap = &rkisp1->capture_devs[id];
1379         struct v4l2_pix_format_mplane pixm;
1380
1381         memset(cap, 0, sizeof(*cap));
1382         cap->id = id;
1383         cap->rkisp1 = rkisp1;
1384
1385         INIT_LIST_HEAD(&cap->buf.queue);
1386         init_waitqueue_head(&cap->done);
1387         spin_lock_init(&cap->buf.lock);
1388         if (cap->id == RKISP1_SELFPATH) {
1389                 cap->ops = &rkisp1_capture_ops_sp;
1390                 cap->config = &rkisp1_capture_config_sp;
1391         } else {
1392                 cap->ops = &rkisp1_capture_ops_mp;
1393                 cap->config = &rkisp1_capture_config_mp;
1394         }
1395
1396         cap->is_streaming = false;
1397
1398         memset(&pixm, 0, sizeof(pixm));
1399         pixm.pixelformat = V4L2_PIX_FMT_YUYV;
1400         pixm.width = RKISP1_DEFAULT_WIDTH;
1401         pixm.height = RKISP1_DEFAULT_HEIGHT;
1402         rkisp1_set_fmt(cap, &pixm);
1403 }
1404
1405 int rkisp1_capture_devs_register(struct rkisp1_device *rkisp1)
1406 {
1407         struct rkisp1_capture *cap;
1408         unsigned int i, j;
1409         int ret;
1410
1411         for (i = 0; i < ARRAY_SIZE(rkisp1->capture_devs); i++) {
1412                 rkisp1_capture_init(rkisp1, i);
1413                 cap = &rkisp1->capture_devs[i];
1414                 cap->rkisp1 = rkisp1;
1415                 ret = rkisp1_register_capture(cap);
1416                 if (ret)
1417                         goto err_unreg_capture_devs;
1418         }
1419
1420         return 0;
1421
1422 err_unreg_capture_devs:
1423         for (j = 0; j < i; j++) {
1424                 cap = &rkisp1->capture_devs[j];
1425                 rkisp1_unregister_capture(cap);
1426         }
1427
1428         return ret;
1429 }