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