drivers/media/platform/rockchip/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 swapped, 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 const 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 const 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  * Most of registers inside rockchip ISP1 have shadow register since
 834  * they must be not be changed during processing a frame.
 835  * Usually, each sub-module updates its shadow register after
 836  * processing the last pixel of a frame.
 837  */
 838 static void rkisp1_cap_stream_enable(struct rkisp1_capture *cap)
 839 {
 840         struct rkisp1_device *rkisp1 = cap->rkisp1;
 841         struct rkisp1_capture *other = &rkisp1->capture_devs[cap->id ^ 1];
 842
 843         cap->ops->set_data_path(cap);
 844         cap->ops->config(cap);
 845
 846         /* Setup a buffer for the next frame */
 847         spin_lock_irq(&cap->buf.lock);
 848         rkisp1_set_next_buf(cap);
 849         cap->ops->enable(cap);
 850         /* It's safe to config ACTIVE and SHADOW regs for the
 851          * first stream. While when the second is starting, do NOT
 852          * force update because it also update the first one.
 853          *
 854          * The latter case would drop one more buf(that is 2) since
 855          * there's not buf in shadow when the second FE received. This's
 856          * also required because the second FE maybe corrupt especially
 857          * when run at 120fps.
 858          */
 859         if (!other->is_streaming) {
 860                 /* force cfg update */
 861                 rkisp1_write(rkisp1,
 862                              RKISP1_CIF_MI_INIT_SOFT_UPD, RKISP1_CIF_MI_INIT);
 863                 rkisp1_set_next_buf(cap);
 864         }
 865         spin_unlock_irq(&cap->buf.lock);
 866         cap->is_streaming = true;
 867 }
 868
 869 static void rkisp1_cap_stream_disable(struct rkisp1_capture *cap)
 870 {
 871         int ret;
 872
 873         /* Stream should stop in interrupt. If it doesn't, stop it by force. */
 874         cap->is_stopping = true;
 875         ret = wait_event_timeout(cap->done,
 876                                  !cap->is_streaming,
 877                                  msecs_to_jiffies(1000));
 878         if (!ret) {
 879                 cap->rkisp1->debug.stop_timeout[cap->id]++;
 880                 cap->ops->stop(cap);
 881                 cap->is_stopping = false;
 882                 cap->is_streaming = false;
 883         }
 884 }
 885
 886 /*
 887  * rkisp1_pipeline_stream_disable - disable nodes in the pipeline
 888  *
 889  * Call s_stream(false) in the reverse order from
 890  * rkisp1_pipeline_stream_enable() and disable the DMA engine.
 891  * Should be called before media_pipeline_stop()
 892  */
 893 static void rkisp1_pipeline_stream_disable(struct rkisp1_capture *cap)
 894         __must_hold(&cap->rkisp1->stream_lock)
 895 {
 896         struct rkisp1_device *rkisp1 = cap->rkisp1;
 897
 898         rkisp1_cap_stream_disable(cap);
 899
 900         /*
 901          * If the other capture is streaming, isp and sensor nodes shouldn't
 902          * be disabled, skip them.
 903          */
 904         if (rkisp1->pipe.streaming_count < 2) {
 905                 v4l2_subdev_call(rkisp1->active_sensor->sd, video, s_stream,
 906                                  false);
 907                 v4l2_subdev_call(&rkisp1->isp.sd, video, s_stream, false);
 908         }
 909
 910         v4l2_subdev_call(&rkisp1->resizer_devs[cap->id].sd, video, s_stream,
 911                          false);
 912 }
 913
 914 /*
 915  * rkisp1_pipeline_stream_enable - enable nodes in the pipeline
 916  *
 917  * Enable the DMA Engine and call s_stream(true) through the pipeline.
 918  * Should be called after media_pipeline_start()
 919  */
 920 static int rkisp1_pipeline_stream_enable(struct rkisp1_capture *cap)
 921         __must_hold(&cap->rkisp1->stream_lock)
 922 {
 923         struct rkisp1_device *rkisp1 = cap->rkisp1;
 924         int ret;
 925
 926         rkisp1_cap_stream_enable(cap);
 927
 928         ret = v4l2_subdev_call(&rkisp1->resizer_devs[cap->id].sd, video,
 929                                s_stream, true);
 930         if (ret)
 931                 goto err_disable_cap;
 932
 933         /*
 934          * If the other capture is streaming, isp and sensor nodes are already
 935          * enabled, skip them.
 936          */
 937         if (rkisp1->pipe.streaming_count > 1)
 938                 return 0;
 939
 940         ret = v4l2_subdev_call(&rkisp1->isp.sd, video, s_stream, true);
 941         if (ret)
 942                 goto err_disable_rsz;
 943
 944         ret = v4l2_subdev_call(rkisp1->active_sensor->sd, video, s_stream,
 945                                true);
 946         if (ret)
 947                 goto err_disable_isp;
 948
 949         return 0;
 950
 951 err_disable_isp:
 952         v4l2_subdev_call(&rkisp1->isp.sd, video, s_stream, false);
 953 err_disable_rsz:
 954         v4l2_subdev_call(&rkisp1->resizer_devs[cap->id].sd, video, s_stream,
 955                          false);
 956 err_disable_cap:
 957         rkisp1_cap_stream_disable(cap);
 958
 959         return ret;
 960 }
 961
 962 static void rkisp1_vb2_stop_streaming(struct vb2_queue *queue)
 963 {
 964         struct rkisp1_capture *cap = queue->drv_priv;
 965         struct rkisp1_vdev_node *node = &cap->vnode;
 966         struct rkisp1_device *rkisp1 = cap->rkisp1;
 967         int ret;
 968
 969         mutex_lock(&cap->rkisp1->stream_lock);
 970
 971         rkisp1_pipeline_stream_disable(cap);
 972
 973         rkisp1_return_all_buffers(cap, VB2_BUF_STATE_ERROR);
 974
 975         v4l2_pipeline_pm_put(&node->vdev.entity);
 976         ret = pm_runtime_put(rkisp1->dev);
 977         if (ret < 0)
 978                 dev_err(rkisp1->dev, "power down failed error:%d\n", ret);
 979
 980         rkisp1_dummy_buf_destroy(cap);
 981
 982         media_pipeline_stop(&node->vdev.entity);
 983
 984         mutex_unlock(&cap->rkisp1->stream_lock);
 985 }
 986
 987 static int
 988 rkisp1_vb2_start_streaming(struct vb2_queue *queue, unsigned int count)
 989 {
 990         struct rkisp1_capture *cap = queue->drv_priv;
 991         struct media_entity *entity = &cap->vnode.vdev.entity;
 992         int ret;
 993
 994         mutex_lock(&cap->rkisp1->stream_lock);
 995
 996         ret = media_pipeline_start(entity, &cap->rkisp1->pipe);
 997         if (ret) {
 998                 dev_err(cap->rkisp1->dev, "start pipeline failed %d\n", ret);
 999                 goto err_ret_buffers;
1000         }
1001
1002         ret = rkisp1_dummy_buf_create(cap);
1003         if (ret)
1004                 goto err_pipeline_stop;
1005
1006         ret = pm_runtime_get_sync(cap->rkisp1->dev);
1007         if (ret < 0) {
1008                 pm_runtime_put_noidle(cap->rkisp1->dev);
1009                 dev_err(cap->rkisp1->dev, "power up failed %d\n", ret);
1010                 goto err_destroy_dummy;
1011         }
1012         ret = v4l2_pipeline_pm_get(entity);
1013         if (ret) {
1014                 dev_err(cap->rkisp1->dev, "open cif pipeline failed %d\n", ret);
1015                 goto err_pipe_pm_put;
1016         }
1017
1018         ret = rkisp1_pipeline_stream_enable(cap);
1019         if (ret)
1020                 goto err_v4l2_pm_put;
1021
1022         mutex_unlock(&cap->rkisp1->stream_lock);
1023
1024         return 0;
1025
1026 err_v4l2_pm_put:
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_pipeline_stop:
1033         media_pipeline_stop(entity);
1034 err_ret_buffers:
1035         rkisp1_return_all_buffers(cap, VB2_BUF_STATE_QUEUED);
1036         mutex_unlock(&cap->rkisp1->stream_lock);
1037
1038         return ret;
1039 }
1040
1041 static const struct vb2_ops rkisp1_vb2_ops = {
1042         .queue_setup = rkisp1_vb2_queue_setup,
1043         .buf_queue = rkisp1_vb2_buf_queue,
1044         .buf_prepare = rkisp1_vb2_buf_prepare,
1045         .wait_prepare = vb2_ops_wait_prepare,
1046         .wait_finish = vb2_ops_wait_finish,
1047         .stop_streaming = rkisp1_vb2_stop_streaming,
1048         .start_streaming = rkisp1_vb2_start_streaming,
1049 };
1050
1051 /* ----------------------------------------------------------------------------
1052  * IOCTLs operations
1053  */
1054
1055 static const struct v4l2_format_info *
1056 rkisp1_fill_pixfmt(struct v4l2_pix_format_mplane *pixm,
1057                    enum rkisp1_stream_id id)
1058 {
1059         struct v4l2_plane_pix_format *plane_y = &pixm->plane_fmt[0];
1060         const struct v4l2_format_info *info;
1061         unsigned int i;
1062         u32 stride;
1063
1064         memset(pixm->plane_fmt, 0, sizeof(pixm->plane_fmt));
1065         info = v4l2_format_info(pixm->pixelformat);
1066         pixm->num_planes = info->mem_planes;
1067         stride = info->bpp[0] * pixm->width;
1068         /* Self path supports custom stride but Main path doesn't */
1069         if (id == RKISP1_MAINPATH || plane_y->bytesperline < stride)
1070                 plane_y->bytesperline = stride;
1071         plane_y->sizeimage = plane_y->bytesperline * pixm->height;
1072
1073         /* normalize stride to pixels per line */
1074         stride = DIV_ROUND_UP(plane_y->bytesperline, info->bpp[0]);
1075
1076         for (i = 1; i < info->comp_planes; i++) {
1077                 struct v4l2_plane_pix_format *plane = &pixm->plane_fmt[i];
1078
1079                 /* bytesperline for other components derive from Y component */
1080                 plane->bytesperline = DIV_ROUND_UP(stride, info->hdiv) *
1081                                       info->bpp[i];
1082                 plane->sizeimage = plane->bytesperline *
1083                                    DIV_ROUND_UP(pixm->height, info->vdiv);
1084         }
1085
1086         /*
1087          * If pixfmt is packed, then plane_fmt[0] should contain the total size
1088          * considering all components. plane_fmt[i] for i > 0 should be ignored
1089          * by userspace as mem_planes == 1, but we are keeping information there
1090          * for convenience.
1091          */
1092         if (info->mem_planes == 1)
1093                 for (i = 1; i < info->comp_planes; i++)
1094                         plane_y->sizeimage += pixm->plane_fmt[i].sizeimage;
1095
1096         return info;
1097 }
1098
1099 static const struct rkisp1_capture_fmt_cfg *
1100 rkisp1_find_fmt_cfg(const struct rkisp1_capture *cap, const u32 pixelfmt)
1101 {
1102         unsigned int i;
1103
1104         for (i = 0; i < cap->config->fmt_size; i++) {
1105                 if (cap->config->fmts[i].fourcc == pixelfmt)
1106                         return &cap->config->fmts[i];
1107         }
1108         return NULL;
1109 }
1110
1111 static void rkisp1_try_fmt(const struct rkisp1_capture *cap,
1112                            struct v4l2_pix_format_mplane *pixm,
1113                            const struct rkisp1_capture_fmt_cfg **fmt_cfg,
1114                            const struct v4l2_format_info **fmt_info)
1115 {
1116         const struct rkisp1_capture_config *config = cap->config;
1117         const struct rkisp1_capture_fmt_cfg *fmt;
1118         const struct v4l2_format_info *info;
1119         const unsigned int max_widths[] = { RKISP1_RSZ_MP_SRC_MAX_WIDTH,
1120                                             RKISP1_RSZ_SP_SRC_MAX_WIDTH };
1121         const unsigned int max_heights[] = { RKISP1_RSZ_MP_SRC_MAX_HEIGHT,
1122                                              RKISP1_RSZ_SP_SRC_MAX_HEIGHT};
1123
1124         fmt = rkisp1_find_fmt_cfg(cap, pixm->pixelformat);
1125         if (!fmt) {
1126                 fmt = config->fmts;
1127                 pixm->pixelformat = fmt->fourcc;
1128         }
1129
1130         pixm->width = clamp_t(u32, pixm->width,
1131                               RKISP1_RSZ_SRC_MIN_WIDTH, max_widths[cap->id]);
1132         pixm->height = clamp_t(u32, pixm->height,
1133                                RKISP1_RSZ_SRC_MIN_HEIGHT, max_heights[cap->id]);
1134
1135         pixm->field = V4L2_FIELD_NONE;
1136         pixm->colorspace = V4L2_COLORSPACE_DEFAULT;
1137         pixm->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
1138         pixm->quantization = V4L2_QUANTIZATION_DEFAULT;
1139
1140         info = rkisp1_fill_pixfmt(pixm, cap->id);
1141
1142         if (fmt_cfg)
1143                 *fmt_cfg = fmt;
1144         if (fmt_info)
1145                 *fmt_info = info;
1146 }
1147
1148 static void rkisp1_set_fmt(struct rkisp1_capture *cap,
1149                            struct v4l2_pix_format_mplane *pixm)
1150 {
1151         rkisp1_try_fmt(cap, pixm, &cap->pix.cfg, &cap->pix.info);
1152         cap->pix.fmt = *pixm;
1153
1154         /* SP supports custom stride in number of pixels of the Y plane */
1155         if (cap->id == RKISP1_SELFPATH)
1156                 cap->sp_y_stride = pixm->plane_fmt[0].bytesperline /
1157                                    cap->pix.info->bpp[0];
1158 }
1159
1160 static int rkisp1_try_fmt_vid_cap_mplane(struct file *file, void *fh,
1161                                          struct v4l2_format *f)
1162 {
1163         struct rkisp1_capture *cap = video_drvdata(file);
1164
1165         rkisp1_try_fmt(cap, &f->fmt.pix_mp, NULL, NULL);
1166
1167         return 0;
1168 }
1169
1170 static int rkisp1_enum_fmt_vid_cap_mplane(struct file *file, void *priv,
1171                                           struct v4l2_fmtdesc *f)
1172 {
1173         struct rkisp1_capture *cap = video_drvdata(file);
1174         const struct rkisp1_capture_fmt_cfg *fmt = NULL;
1175         unsigned int i, n = 0;
1176
1177         if (!f->mbus_code) {
1178                 if (f->index >= cap->config->fmt_size)
1179                         return -EINVAL;
1180
1181                 fmt = &cap->config->fmts[f->index];
1182                 f->pixelformat = fmt->fourcc;
1183                 return 0;
1184         }
1185
1186         for (i = 0; i < cap->config->fmt_size; i++) {
1187                 if (cap->config->fmts[i].mbus != f->mbus_code)
1188                         continue;
1189
1190                 if (n++ == f->index) {
1191                         f->pixelformat = cap->config->fmts[i].fourcc;
1192                         return 0;
1193                 }
1194         }
1195         return -EINVAL;
1196 }
1197
1198 static int rkisp1_s_fmt_vid_cap_mplane(struct file *file,
1199                                        void *priv, struct v4l2_format *f)
1200 {
1201         struct rkisp1_capture *cap = video_drvdata(file);
1202         struct rkisp1_vdev_node *node =
1203                                 rkisp1_vdev_to_node(&cap->vnode.vdev);
1204
1205         if (vb2_is_busy(&node->buf_queue))
1206                 return -EBUSY;
1207
1208         rkisp1_set_fmt(cap, &f->fmt.pix_mp);
1209
1210         return 0;
1211 }
1212
1213 static int rkisp1_g_fmt_vid_cap_mplane(struct file *file, void *fh,
1214                                        struct v4l2_format *f)
1215 {
1216         struct rkisp1_capture *cap = video_drvdata(file);
1217
1218         f->fmt.pix_mp = cap->pix.fmt;
1219
1220         return 0;
1221 }
1222
1223 static int
1224 rkisp1_querycap(struct file *file, void *priv, struct v4l2_capability *cap)
1225 {
1226         struct rkisp1_capture *cap_dev = video_drvdata(file);
1227         struct rkisp1_device *rkisp1 = cap_dev->rkisp1;
1228
1229         strscpy(cap->driver, rkisp1->dev->driver->name, sizeof(cap->driver));
1230         strscpy(cap->card, rkisp1->dev->driver->name, sizeof(cap->card));
1231         strscpy(cap->bus_info, RKISP1_BUS_INFO, sizeof(cap->bus_info));
1232
1233         return 0;
1234 }
1235
1236 static const struct v4l2_ioctl_ops rkisp1_v4l2_ioctl_ops = {
1237         .vidioc_reqbufs = vb2_ioctl_reqbufs,
1238         .vidioc_querybuf = vb2_ioctl_querybuf,
1239         .vidioc_create_bufs = vb2_ioctl_create_bufs,
1240         .vidioc_qbuf = vb2_ioctl_qbuf,
1241         .vidioc_expbuf = vb2_ioctl_expbuf,
1242         .vidioc_dqbuf = vb2_ioctl_dqbuf,
1243         .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
1244         .vidioc_streamon = vb2_ioctl_streamon,
1245         .vidioc_streamoff = vb2_ioctl_streamoff,
1246         .vidioc_try_fmt_vid_cap_mplane = rkisp1_try_fmt_vid_cap_mplane,
1247         .vidioc_s_fmt_vid_cap_mplane = rkisp1_s_fmt_vid_cap_mplane,
1248         .vidioc_g_fmt_vid_cap_mplane = rkisp1_g_fmt_vid_cap_mplane,
1249         .vidioc_enum_fmt_vid_cap = rkisp1_enum_fmt_vid_cap_mplane,
1250         .vidioc_querycap = rkisp1_querycap,
1251         .vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
1252         .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
1253 };
1254
1255 static int rkisp1_capture_link_validate(struct media_link *link)
1256 {
1257         struct video_device *vdev =
1258                 media_entity_to_video_device(link->sink->entity);
1259         struct v4l2_subdev *sd =
1260                 media_entity_to_v4l2_subdev(link->source->entity);
1261         struct rkisp1_capture *cap = video_get_drvdata(vdev);
1262         const struct rkisp1_capture_fmt_cfg *fmt =
1263                 rkisp1_find_fmt_cfg(cap, cap->pix.fmt.pixelformat);
1264         struct v4l2_subdev_format sd_fmt;
1265         int ret;
1266
1267         sd_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
1268         sd_fmt.pad = link->source->index;
1269         ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &sd_fmt);
1270         if (ret)
1271                 return ret;
1272
1273         if (sd_fmt.format.height != cap->pix.fmt.height ||
1274             sd_fmt.format.width != cap->pix.fmt.width ||
1275             sd_fmt.format.code != fmt->mbus)
1276                 return -EPIPE;
1277
1278         return 0;
1279 }
1280
1281 /* ----------------------------------------------------------------------------
1282  * core functions
1283  */
1284
1285 static const struct media_entity_operations rkisp1_media_ops = {
1286         .link_validate = rkisp1_capture_link_validate,
1287 };
1288
1289 static const struct v4l2_file_operations rkisp1_fops = {
1290         .open = v4l2_fh_open,
1291         .release = vb2_fop_release,
1292         .unlocked_ioctl = video_ioctl2,
1293         .poll = vb2_fop_poll,
1294         .mmap = vb2_fop_mmap,
1295 };
1296
1297 static void rkisp1_unregister_capture(struct rkisp1_capture *cap)
1298 {
1299         media_entity_cleanup(&cap->vnode.vdev.entity);
1300         vb2_video_unregister_device(&cap->vnode.vdev);
1301 }
1302
1303 void rkisp1_capture_devs_unregister(struct rkisp1_device *rkisp1)
1304 {
1305         struct rkisp1_capture *mp = &rkisp1->capture_devs[RKISP1_MAINPATH];
1306         struct rkisp1_capture *sp = &rkisp1->capture_devs[RKISP1_SELFPATH];
1307
1308         rkisp1_unregister_capture(mp);
1309         rkisp1_unregister_capture(sp);
1310 }
1311
1312 static int rkisp1_register_capture(struct rkisp1_capture *cap)
1313 {
1314         const char * const dev_names[] = {RKISP1_MP_DEV_NAME,
1315                                           RKISP1_SP_DEV_NAME};
1316         struct v4l2_device *v4l2_dev = &cap->rkisp1->v4l2_dev;
1317         struct video_device *vdev = &cap->vnode.vdev;
1318         struct rkisp1_vdev_node *node;
1319         struct vb2_queue *q;
1320         int ret;
1321
1322         strscpy(vdev->name, dev_names[cap->id], sizeof(vdev->name));
1323         node = rkisp1_vdev_to_node(vdev);
1324         mutex_init(&node->vlock);
1325
1326         vdev->ioctl_ops = &rkisp1_v4l2_ioctl_ops;
1327         vdev->release = video_device_release_empty;
1328         vdev->fops = &rkisp1_fops;
1329         vdev->minor = -1;
1330         vdev->v4l2_dev = v4l2_dev;
1331         vdev->lock = &node->vlock;
1332         vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE |
1333                             V4L2_CAP_STREAMING | V4L2_CAP_IO_MC;
1334         vdev->entity.ops = &rkisp1_media_ops;
1335         video_set_drvdata(vdev, cap);
1336         vdev->vfl_dir = VFL_DIR_RX;
1337         node->pad.flags = MEDIA_PAD_FL_SINK;
1338
1339         q = &node->buf_queue;
1340         q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
1341         q->io_modes = VB2_MMAP | VB2_DMABUF;
1342         q->drv_priv = cap;
1343         q->ops = &rkisp1_vb2_ops;
1344         q->mem_ops = &vb2_dma_contig_memops;
1345         q->buf_struct_size = sizeof(struct rkisp1_buffer);
1346         q->min_buffers_needed = RKISP1_MIN_BUFFERS_NEEDED;
1347         q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1348         q->lock = &node->vlock;
1349         q->dev = cap->rkisp1->dev;
1350         ret = vb2_queue_init(q);
1351         if (ret) {
1352                 dev_err(cap->rkisp1->dev,
1353                         "vb2 queue init failed (err=%d)\n", ret);
1354                 return ret;
1355         }
1356
1357         vdev->queue = q;
1358
1359         ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
1360         if (ret) {
1361                 dev_err(cap->rkisp1->dev,
1362                         "failed to register %s, ret=%d\n", vdev->name, ret);
1363                 return ret;
1364         }
1365         v4l2_info(v4l2_dev, "registered %s as /dev/video%d\n", vdev->name,
1366                   vdev->num);
1367
1368         ret = media_entity_pads_init(&vdev->entity, 1, &node->pad);
1369         if (ret) {
1370                 video_unregister_device(vdev);
1371                 return ret;
1372         }
1373
1374         return 0;
1375 }
1376
1377 static void
1378 rkisp1_capture_init(struct rkisp1_device *rkisp1, enum rkisp1_stream_id id)
1379 {
1380         struct rkisp1_capture *cap = &rkisp1->capture_devs[id];
1381         struct v4l2_pix_format_mplane pixm;
1382
1383         memset(cap, 0, sizeof(*cap));
1384         cap->id = id;
1385         cap->rkisp1 = rkisp1;
1386
1387         INIT_LIST_HEAD(&cap->buf.queue);
1388         init_waitqueue_head(&cap->done);
1389         spin_lock_init(&cap->buf.lock);
1390         if (cap->id == RKISP1_SELFPATH) {
1391                 cap->ops = &rkisp1_capture_ops_sp;
1392                 cap->config = &rkisp1_capture_config_sp;
1393         } else {
1394                 cap->ops = &rkisp1_capture_ops_mp;
1395                 cap->config = &rkisp1_capture_config_mp;
1396         }
1397
1398         cap->is_streaming = false;
1399
1400         memset(&pixm, 0, sizeof(pixm));
1401         pixm.pixelformat = V4L2_PIX_FMT_YUYV;
1402         pixm.width = RKISP1_DEFAULT_WIDTH;
1403         pixm.height = RKISP1_DEFAULT_HEIGHT;
1404         rkisp1_set_fmt(cap, &pixm);
1405 }
1406
1407 int rkisp1_capture_devs_register(struct rkisp1_device *rkisp1)
1408 {
1409         struct rkisp1_capture *cap;
1410         unsigned int i, j;
1411         int ret;
1412
1413         for (i = 0; i < ARRAY_SIZE(rkisp1->capture_devs); i++) {
1414                 rkisp1_capture_init(rkisp1, i);
1415                 cap = &rkisp1->capture_devs[i];
1416                 cap->rkisp1 = rkisp1;
1417                 ret = rkisp1_register_capture(cap);
1418                 if (ret)
1419                         goto err_unreg_capture_devs;
1420         }
1421
1422         return 0;
1423
1424 err_unreg_capture_devs:
1425         for (j = 0; j < i; j++) {
1426                 cap = &rkisp1->capture_devs[j];
1427                 rkisp1_unregister_capture(cap);
1428         }
1429
1430         return ret;
1431 }