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         status = rkisp1_read(rkisp1, RKISP1_CIF_MI_MIS);
 729         if (!status)
 730                 return IRQ_NONE;
 731
 732         rkisp1_write(rkisp1, RKISP1_CIF_MI_ICR, status);
 733
 734         for (i = 0; i < ARRAY_SIZE(rkisp1->capture_devs); ++i) {
 735                 struct rkisp1_capture *cap = &rkisp1->capture_devs[i];
 736
 737                 if (!(status & RKISP1_CIF_MI_FRAME(cap)))
 738                         continue;
 739                 if (!cap->is_stopping) {
 740                         rkisp1_handle_buffer(cap);
 741                         continue;
 742                 }
 743                 /*
 744                  * Make sure stream is actually stopped, whose state
 745                  * can be read from the shadow register, before
 746                  * wake_up() thread which would immediately free all
 747                  * frame buffers. stop() takes effect at the next
 748                  * frame end that sync the configurations to shadow
 749                  * regs.
 750                  */
 751                 if (!cap->ops->is_stopped(cap)) {
 752                         cap->ops->stop(cap);
 753                         continue;
 754                 }
 755                 cap->is_stopping = false;
 756                 cap->is_streaming = false;
 757                 wake_up(&cap->done);
 758         }
 759
 760         return IRQ_HANDLED;
 761 }
 762
 763 /* ----------------------------------------------------------------------------
 764  * Vb2 operations
 765  */
 766
 767 static int rkisp1_vb2_queue_setup(struct vb2_queue *queue,
 768                                   unsigned int *num_buffers,
 769                                   unsigned int *num_planes,
 770                                   unsigned int sizes[],
 771                                   struct device *alloc_devs[])
 772 {
 773         struct rkisp1_capture *cap = queue->drv_priv;
 774         const struct v4l2_pix_format_mplane *pixm = &cap->pix.fmt;
 775         unsigned int i;
 776
 777         if (*num_planes) {
 778                 if (*num_planes != pixm->num_planes)
 779                         return -EINVAL;
 780
 781                 for (i = 0; i < pixm->num_planes; i++)
 782                         if (sizes[i] < pixm->plane_fmt[i].sizeimage)
 783                                 return -EINVAL;
 784         } else {
 785                 *num_planes = pixm->num_planes;
 786                 for (i = 0; i < pixm->num_planes; i++)
 787                         sizes[i] = pixm->plane_fmt[i].sizeimage;
 788         }
 789
 790         return 0;
 791 }
 792
 793 static int rkisp1_vb2_buf_init(struct vb2_buffer *vb)
 794 {
 795         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 796         struct rkisp1_buffer *ispbuf =
 797                 container_of(vbuf, struct rkisp1_buffer, vb);
 798         struct rkisp1_capture *cap = vb->vb2_queue->drv_priv;
 799         const struct v4l2_pix_format_mplane *pixm = &cap->pix.fmt;
 800         unsigned int i;
 801
 802         memset(ispbuf->buff_addr, 0, sizeof(ispbuf->buff_addr));
 803         for (i = 0; i < pixm->num_planes; i++)
 804                 ispbuf->buff_addr[i] = vb2_dma_contig_plane_dma_addr(vb, i);
 805
 806         /* Convert to non-MPLANE */
 807         if (pixm->num_planes == 1) {
 808                 ispbuf->buff_addr[RKISP1_PLANE_CB] =
 809                         ispbuf->buff_addr[RKISP1_PLANE_Y] +
 810                         rkisp1_pixfmt_comp_size(pixm, RKISP1_PLANE_Y);
 811                 ispbuf->buff_addr[RKISP1_PLANE_CR] =
 812                         ispbuf->buff_addr[RKISP1_PLANE_CB] +
 813                         rkisp1_pixfmt_comp_size(pixm, RKISP1_PLANE_CB);
 814         }
 815
 816         /*
 817          * uv swap can be supported for planar formats by switching
 818          * the address of cb and cr
 819          */
 820         if (cap->pix.info->comp_planes == 3 && cap->pix.cfg->uv_swap)
 821                 swap(ispbuf->buff_addr[RKISP1_PLANE_CR],
 822                      ispbuf->buff_addr[RKISP1_PLANE_CB]);
 823         return 0;
 824 }
 825
 826 static void rkisp1_vb2_buf_queue(struct vb2_buffer *vb)
 827 {
 828         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 829         struct rkisp1_buffer *ispbuf =
 830                 container_of(vbuf, struct rkisp1_buffer, vb);
 831         struct rkisp1_capture *cap = vb->vb2_queue->drv_priv;
 832
 833         spin_lock_irq(&cap->buf.lock);
 834         list_add_tail(&ispbuf->queue, &cap->buf.queue);
 835         spin_unlock_irq(&cap->buf.lock);
 836 }
 837
 838 static int rkisp1_vb2_buf_prepare(struct vb2_buffer *vb)
 839 {
 840         struct rkisp1_capture *cap = vb->vb2_queue->drv_priv;
 841         unsigned int i;
 842
 843         for (i = 0; i < cap->pix.fmt.num_planes; i++) {
 844                 unsigned long size = cap->pix.fmt.plane_fmt[i].sizeimage;
 845
 846                 if (vb2_plane_size(vb, i) < size) {
 847                         dev_err(cap->rkisp1->dev,
 848                                 "User buffer too small (%ld < %ld)\n",
 849                                 vb2_plane_size(vb, i), size);
 850                         return -EINVAL;
 851                 }
 852                 vb2_set_plane_payload(vb, i, size);
 853         }
 854
 855         return 0;
 856 }
 857
 858 static void rkisp1_return_all_buffers(struct rkisp1_capture *cap,
 859                                       enum vb2_buffer_state state)
 860 {
 861         struct rkisp1_buffer *buf;
 862
 863         spin_lock_irq(&cap->buf.lock);
 864         if (cap->buf.curr) {
 865                 vb2_buffer_done(&cap->buf.curr->vb.vb2_buf, state);
 866                 cap->buf.curr = NULL;
 867         }
 868         if (cap->buf.next) {
 869                 vb2_buffer_done(&cap->buf.next->vb.vb2_buf, state);
 870                 cap->buf.next = NULL;
 871         }
 872         while (!list_empty(&cap->buf.queue)) {
 873                 buf = list_first_entry(&cap->buf.queue,
 874                                        struct rkisp1_buffer, queue);
 875                 list_del(&buf->queue);
 876                 vb2_buffer_done(&buf->vb.vb2_buf, state);
 877         }
 878         spin_unlock_irq(&cap->buf.lock);
 879 }
 880
 881 /*
 882  * Most registers inside the rockchip ISP1 have shadow register since
 883  * they must not be changed while processing a frame.
 884  * Usually, each sub-module updates its shadow register after
 885  * processing the last pixel of a frame.
 886  */
 887 static void rkisp1_cap_stream_enable(struct rkisp1_capture *cap)
 888 {
 889         struct rkisp1_device *rkisp1 = cap->rkisp1;
 890         struct rkisp1_capture *other = &rkisp1->capture_devs[cap->id ^ 1];
 891
 892         cap->ops->set_data_path(cap);
 893         cap->ops->config(cap);
 894
 895         /* Setup a buffer for the next frame */
 896         spin_lock_irq(&cap->buf.lock);
 897         rkisp1_set_next_buf(cap);
 898         cap->ops->enable(cap);
 899         /* It's safe to configure ACTIVE and SHADOW registers for the
 900          * first stream. While when the second is starting, do NOT
 901          * force update because it also updates the first one.
 902          *
 903          * The latter case would drop one more buffer(that is 2) since
 904          * there's no buffer in a shadow register when the second FE received.
 905          * This's also required because the second FE maybe corrupt
 906          * especially when run at 120fps.
 907          */
 908         if (!other->is_streaming) {
 909                 /* force cfg update */
 910                 rkisp1_write(rkisp1, RKISP1_CIF_MI_INIT,
 911                              RKISP1_CIF_MI_INIT_SOFT_UPD);
 912                 rkisp1_set_next_buf(cap);
 913         }
 914         spin_unlock_irq(&cap->buf.lock);
 915         cap->is_streaming = true;
 916 }
 917
 918 static void rkisp1_cap_stream_disable(struct rkisp1_capture *cap)
 919 {
 920         int ret;
 921
 922         /* Stream should stop in interrupt. If it doesn't, stop it by force. */
 923         cap->is_stopping = true;
 924         ret = wait_event_timeout(cap->done,
 925                                  !cap->is_streaming,
 926                                  msecs_to_jiffies(1000));
 927         if (!ret) {
 928                 cap->rkisp1->debug.stop_timeout[cap->id]++;
 929                 cap->ops->stop(cap);
 930                 cap->is_stopping = false;
 931                 cap->is_streaming = false;
 932         }
 933 }
 934
 935 /*
 936  * rkisp1_pipeline_stream_disable - disable nodes in the pipeline
 937  *
 938  * Call s_stream(false) in the reverse order from
 939  * rkisp1_pipeline_stream_enable() and disable the DMA engine.
 940  * Should be called before video_device_pipeline_stop()
 941  */
 942 static void rkisp1_pipeline_stream_disable(struct rkisp1_capture *cap)
 943         __must_hold(&cap->rkisp1->stream_lock)
 944 {
 945         struct rkisp1_device *rkisp1 = cap->rkisp1;
 946
 947         rkisp1_cap_stream_disable(cap);
 948
 949         /*
 950          * If the other capture is streaming, isp and sensor nodes shouldn't
 951          * be disabled, skip them.
 952          */
 953         if (rkisp1->pipe.start_count < 2)
 954                 v4l2_subdev_call(&rkisp1->isp.sd, video, s_stream, false);
 955
 956         v4l2_subdev_call(&rkisp1->resizer_devs[cap->id].sd, video, s_stream,
 957                          false);
 958 }
 959
 960 /*
 961  * rkisp1_pipeline_stream_enable - enable nodes in the pipeline
 962  *
 963  * Enable the DMA Engine and call s_stream(true) through the pipeline.
 964  * Should be called after video_device_pipeline_start()
 965  */
 966 static int rkisp1_pipeline_stream_enable(struct rkisp1_capture *cap)
 967         __must_hold(&cap->rkisp1->stream_lock)
 968 {
 969         struct rkisp1_device *rkisp1 = cap->rkisp1;
 970         int ret;
 971
 972         rkisp1_cap_stream_enable(cap);
 973
 974         ret = v4l2_subdev_call(&rkisp1->resizer_devs[cap->id].sd, video,
 975                                s_stream, true);
 976         if (ret)
 977                 goto err_disable_cap;
 978
 979         /*
 980          * If the other capture is streaming, isp and sensor nodes are already
 981          * enabled, skip them.
 982          */
 983         if (rkisp1->pipe.start_count > 1)
 984                 return 0;
 985
 986         ret = v4l2_subdev_call(&rkisp1->isp.sd, video, s_stream, true);
 987         if (ret)
 988                 goto err_disable_rsz;
 989
 990         return 0;
 991
 992 err_disable_rsz:
 993         v4l2_subdev_call(&rkisp1->resizer_devs[cap->id].sd, video, s_stream,
 994                          false);
 995 err_disable_cap:
 996         rkisp1_cap_stream_disable(cap);
 997
 998         return ret;
 999 }
1000
1001 static void rkisp1_vb2_stop_streaming(struct vb2_queue *queue)
1002 {
1003         struct rkisp1_capture *cap = queue->drv_priv;
1004         struct rkisp1_vdev_node *node = &cap->vnode;
1005         struct rkisp1_device *rkisp1 = cap->rkisp1;
1006         int ret;
1007
1008         mutex_lock(&cap->rkisp1->stream_lock);
1009
1010         rkisp1_pipeline_stream_disable(cap);
1011
1012         rkisp1_return_all_buffers(cap, VB2_BUF_STATE_ERROR);
1013
1014         v4l2_pipeline_pm_put(&node->vdev.entity);
1015         ret = pm_runtime_put(rkisp1->dev);
1016         if (ret < 0)
1017                 dev_err(rkisp1->dev, "power down failed error:%d\n", ret);
1018
1019         rkisp1_dummy_buf_destroy(cap);
1020
1021         video_device_pipeline_stop(&node->vdev);
1022
1023         mutex_unlock(&cap->rkisp1->stream_lock);
1024 }
1025
1026 static int
1027 rkisp1_vb2_start_streaming(struct vb2_queue *queue, unsigned int count)
1028 {
1029         struct rkisp1_capture *cap = queue->drv_priv;
1030         struct media_entity *entity = &cap->vnode.vdev.entity;
1031         int ret;
1032
1033         mutex_lock(&cap->rkisp1->stream_lock);
1034
1035         ret = video_device_pipeline_start(&cap->vnode.vdev, &cap->rkisp1->pipe);
1036         if (ret) {
1037                 dev_err(cap->rkisp1->dev, "start pipeline failed %d\n", ret);
1038                 goto err_ret_buffers;
1039         }
1040
1041         ret = rkisp1_dummy_buf_create(cap);
1042         if (ret)
1043                 goto err_pipeline_stop;
1044
1045         ret = pm_runtime_resume_and_get(cap->rkisp1->dev);
1046         if (ret < 0) {
1047                 dev_err(cap->rkisp1->dev, "power up failed %d\n", ret);
1048                 goto err_destroy_dummy;
1049         }
1050         ret = v4l2_pipeline_pm_get(entity);
1051         if (ret) {
1052                 dev_err(cap->rkisp1->dev, "open cif pipeline failed %d\n", ret);
1053                 goto err_pipe_pm_put;
1054         }
1055
1056         ret = rkisp1_pipeline_stream_enable(cap);
1057         if (ret)
1058                 goto err_v4l2_pm_put;
1059
1060         mutex_unlock(&cap->rkisp1->stream_lock);
1061
1062         return 0;
1063
1064 err_v4l2_pm_put:
1065         v4l2_pipeline_pm_put(entity);
1066 err_pipe_pm_put:
1067         pm_runtime_put(cap->rkisp1->dev);
1068 err_destroy_dummy:
1069         rkisp1_dummy_buf_destroy(cap);
1070 err_pipeline_stop:
1071         video_device_pipeline_stop(&cap->vnode.vdev);
1072 err_ret_buffers:
1073         rkisp1_return_all_buffers(cap, VB2_BUF_STATE_QUEUED);
1074         mutex_unlock(&cap->rkisp1->stream_lock);
1075
1076         return ret;
1077 }
1078
1079 static const struct vb2_ops rkisp1_vb2_ops = {
1080         .queue_setup = rkisp1_vb2_queue_setup,
1081         .buf_init = rkisp1_vb2_buf_init,
1082         .buf_queue = rkisp1_vb2_buf_queue,
1083         .buf_prepare = rkisp1_vb2_buf_prepare,
1084         .wait_prepare = vb2_ops_wait_prepare,
1085         .wait_finish = vb2_ops_wait_finish,
1086         .stop_streaming = rkisp1_vb2_stop_streaming,
1087         .start_streaming = rkisp1_vb2_start_streaming,
1088 };
1089
1090 /* ----------------------------------------------------------------------------
1091  * IOCTLs operations
1092  */
1093
1094 static const struct v4l2_format_info *
1095 rkisp1_fill_pixfmt(struct v4l2_pix_format_mplane *pixm,
1096                    enum rkisp1_stream_id id)
1097 {
1098         struct v4l2_plane_pix_format *plane_y = &pixm->plane_fmt[0];
1099         const struct v4l2_format_info *info;
1100         unsigned int i;
1101         u32 stride;
1102
1103         memset(pixm->plane_fmt, 0, sizeof(pixm->plane_fmt));
1104         info = v4l2_format_info(pixm->pixelformat);
1105         pixm->num_planes = info->mem_planes;
1106
1107         /*
1108          * The SP supports custom strides, expressed as a number of pixels for
1109          * the Y plane. Clamp the stride to a reasonable value to avoid integer
1110          * overflows when calculating the bytesperline and sizeimage values.
1111          */
1112         if (id == RKISP1_SELFPATH)
1113                 stride = clamp(DIV_ROUND_UP(plane_y->bytesperline, info->bpp[0]),
1114                                pixm->width, 65536U);
1115         else
1116                 stride = pixm->width;
1117
1118         plane_y->bytesperline = stride * info->bpp[0];
1119         plane_y->sizeimage = plane_y->bytesperline * pixm->height;
1120
1121         for (i = 1; i < info->comp_planes; i++) {
1122                 struct v4l2_plane_pix_format *plane = &pixm->plane_fmt[i];
1123
1124                 /* bytesperline for other components derive from Y component */
1125                 plane->bytesperline = DIV_ROUND_UP(stride, info->hdiv) *
1126                                       info->bpp[i];
1127                 plane->sizeimage = plane->bytesperline *
1128                                    DIV_ROUND_UP(pixm->height, info->vdiv);
1129         }
1130
1131         /*
1132          * If pixfmt is packed, then plane_fmt[0] should contain the total size
1133          * considering all components. plane_fmt[i] for i > 0 should be ignored
1134          * by userspace as mem_planes == 1, but we are keeping information there
1135          * for convenience.
1136          */
1137         if (info->mem_planes == 1)
1138                 for (i = 1; i < info->comp_planes; i++)
1139                         plane_y->sizeimage += pixm->plane_fmt[i].sizeimage;
1140
1141         return info;
1142 }
1143
1144 static const struct rkisp1_capture_fmt_cfg *
1145 rkisp1_find_fmt_cfg(const struct rkisp1_capture *cap, const u32 pixelfmt)
1146 {
1147         unsigned int i;
1148
1149         for (i = 0; i < cap->config->fmt_size; i++) {
1150                 if (cap->config->fmts[i].fourcc == pixelfmt)
1151                         return &cap->config->fmts[i];
1152         }
1153         return NULL;
1154 }
1155
1156 static void rkisp1_try_fmt(const struct rkisp1_capture *cap,
1157                            struct v4l2_pix_format_mplane *pixm,
1158                            const struct rkisp1_capture_fmt_cfg **fmt_cfg,
1159                            const struct v4l2_format_info **fmt_info)
1160 {
1161         const struct rkisp1_capture_config *config = cap->config;
1162         const struct rkisp1_capture_fmt_cfg *fmt;
1163         const struct v4l2_format_info *info;
1164         static const unsigned int max_widths[] = {
1165                 RKISP1_RSZ_MP_SRC_MAX_WIDTH, RKISP1_RSZ_SP_SRC_MAX_WIDTH
1166         };
1167         static const unsigned int max_heights[] = {
1168                 RKISP1_RSZ_MP_SRC_MAX_HEIGHT, RKISP1_RSZ_SP_SRC_MAX_HEIGHT
1169         };
1170
1171         fmt = rkisp1_find_fmt_cfg(cap, pixm->pixelformat);
1172         if (!fmt) {
1173                 fmt = config->fmts;
1174                 pixm->pixelformat = fmt->fourcc;
1175         }
1176
1177         pixm->width = clamp_t(u32, pixm->width,
1178                               RKISP1_RSZ_SRC_MIN_WIDTH, max_widths[cap->id]);
1179         pixm->height = clamp_t(u32, pixm->height,
1180                                RKISP1_RSZ_SRC_MIN_HEIGHT, max_heights[cap->id]);
1181
1182         pixm->field = V4L2_FIELD_NONE;
1183         pixm->colorspace = V4L2_COLORSPACE_DEFAULT;
1184         pixm->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
1185         pixm->quantization = V4L2_QUANTIZATION_DEFAULT;
1186
1187         info = rkisp1_fill_pixfmt(pixm, cap->id);
1188
1189         if (fmt_cfg)
1190                 *fmt_cfg = fmt;
1191         if (fmt_info)
1192                 *fmt_info = info;
1193 }
1194
1195 static void rkisp1_set_fmt(struct rkisp1_capture *cap,
1196                            struct v4l2_pix_format_mplane *pixm)
1197 {
1198         rkisp1_try_fmt(cap, pixm, &cap->pix.cfg, &cap->pix.info);
1199         cap->pix.fmt = *pixm;
1200
1201         /* SP supports custom stride in number of pixels of the Y plane */
1202         if (cap->id == RKISP1_SELFPATH)
1203                 cap->sp_y_stride = pixm->plane_fmt[0].bytesperline /
1204                                    cap->pix.info->bpp[0];
1205 }
1206
1207 static int rkisp1_try_fmt_vid_cap_mplane(struct file *file, void *fh,
1208                                          struct v4l2_format *f)
1209 {
1210         struct rkisp1_capture *cap = video_drvdata(file);
1211
1212         rkisp1_try_fmt(cap, &f->fmt.pix_mp, NULL, NULL);
1213
1214         return 0;
1215 }
1216
1217 static int rkisp1_enum_fmt_vid_cap_mplane(struct file *file, void *priv,
1218                                           struct v4l2_fmtdesc *f)
1219 {
1220         struct rkisp1_capture *cap = video_drvdata(file);
1221         const struct rkisp1_capture_fmt_cfg *fmt = NULL;
1222         unsigned int i, n = 0;
1223
1224         if (!f->mbus_code) {
1225                 if (f->index >= cap->config->fmt_size)
1226                         return -EINVAL;
1227
1228                 fmt = &cap->config->fmts[f->index];
1229                 f->pixelformat = fmt->fourcc;
1230                 return 0;
1231         }
1232
1233         for (i = 0; i < cap->config->fmt_size; i++) {
1234                 if (cap->config->fmts[i].mbus != f->mbus_code)
1235                         continue;
1236
1237                 if (n++ == f->index) {
1238                         f->pixelformat = cap->config->fmts[i].fourcc;
1239                         return 0;
1240                 }
1241         }
1242         return -EINVAL;
1243 }
1244
1245 static int rkisp1_enum_framesizes(struct file *file, void *fh,
1246                                   struct v4l2_frmsizeenum *fsize)
1247 {
1248         static const unsigned int max_widths[] = {
1249                 RKISP1_RSZ_MP_SRC_MAX_WIDTH,
1250                 RKISP1_RSZ_SP_SRC_MAX_WIDTH,
1251         };
1252         static const unsigned int max_heights[] = {
1253                 RKISP1_RSZ_MP_SRC_MAX_HEIGHT,
1254                 RKISP1_RSZ_SP_SRC_MAX_HEIGHT,
1255         };
1256         struct rkisp1_capture *cap = video_drvdata(file);
1257
1258         if (fsize->index != 0)
1259                 return -EINVAL;
1260
1261         fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
1262
1263         fsize->stepwise.min_width = RKISP1_RSZ_SRC_MIN_WIDTH;
1264         fsize->stepwise.max_width = max_widths[cap->id];
1265         fsize->stepwise.step_width = 2;
1266
1267         fsize->stepwise.min_height = RKISP1_RSZ_SRC_MIN_HEIGHT;
1268         fsize->stepwise.max_height = max_heights[cap->id];
1269         fsize->stepwise.step_height = 2;
1270
1271         return 0;
1272 }
1273
1274 static int rkisp1_s_fmt_vid_cap_mplane(struct file *file,
1275                                        void *priv, struct v4l2_format *f)
1276 {
1277         struct rkisp1_capture *cap = video_drvdata(file);
1278         struct rkisp1_vdev_node *node =
1279                                 rkisp1_vdev_to_node(&cap->vnode.vdev);
1280
1281         if (vb2_is_busy(&node->buf_queue))
1282                 return -EBUSY;
1283
1284         rkisp1_set_fmt(cap, &f->fmt.pix_mp);
1285
1286         return 0;
1287 }
1288
1289 static int rkisp1_g_fmt_vid_cap_mplane(struct file *file, void *fh,
1290                                        struct v4l2_format *f)
1291 {
1292         struct rkisp1_capture *cap = video_drvdata(file);
1293
1294         f->fmt.pix_mp = cap->pix.fmt;
1295
1296         return 0;
1297 }
1298
1299 static int
1300 rkisp1_querycap(struct file *file, void *priv, struct v4l2_capability *cap)
1301 {
1302         strscpy(cap->driver, RKISP1_DRIVER_NAME, sizeof(cap->driver));
1303         strscpy(cap->card, RKISP1_DRIVER_NAME, sizeof(cap->card));
1304         strscpy(cap->bus_info, RKISP1_BUS_INFO, sizeof(cap->bus_info));
1305
1306         return 0;
1307 }
1308
1309 static const struct v4l2_ioctl_ops rkisp1_v4l2_ioctl_ops = {
1310         .vidioc_reqbufs = vb2_ioctl_reqbufs,
1311         .vidioc_querybuf = vb2_ioctl_querybuf,
1312         .vidioc_create_bufs = vb2_ioctl_create_bufs,
1313         .vidioc_qbuf = vb2_ioctl_qbuf,
1314         .vidioc_expbuf = vb2_ioctl_expbuf,
1315         .vidioc_dqbuf = vb2_ioctl_dqbuf,
1316         .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
1317         .vidioc_streamon = vb2_ioctl_streamon,
1318         .vidioc_streamoff = vb2_ioctl_streamoff,
1319         .vidioc_try_fmt_vid_cap_mplane = rkisp1_try_fmt_vid_cap_mplane,
1320         .vidioc_s_fmt_vid_cap_mplane = rkisp1_s_fmt_vid_cap_mplane,
1321         .vidioc_g_fmt_vid_cap_mplane = rkisp1_g_fmt_vid_cap_mplane,
1322         .vidioc_enum_fmt_vid_cap = rkisp1_enum_fmt_vid_cap_mplane,
1323         .vidioc_enum_framesizes = rkisp1_enum_framesizes,
1324         .vidioc_querycap = rkisp1_querycap,
1325         .vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
1326         .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
1327 };
1328
1329 static int rkisp1_capture_link_validate(struct media_link *link)
1330 {
1331         struct video_device *vdev =
1332                 media_entity_to_video_device(link->sink->entity);
1333         struct v4l2_subdev *sd =
1334                 media_entity_to_v4l2_subdev(link->source->entity);
1335         struct rkisp1_capture *cap = video_get_drvdata(vdev);
1336         const struct rkisp1_capture_fmt_cfg *fmt =
1337                 rkisp1_find_fmt_cfg(cap, cap->pix.fmt.pixelformat);
1338         struct v4l2_subdev_format sd_fmt = {
1339                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1340                 .pad = link->source->index,
1341         };
1342         int ret;
1343
1344         ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &sd_fmt);
1345         if (ret)
1346                 return ret;
1347
1348         if (sd_fmt.format.height != cap->pix.fmt.height ||
1349             sd_fmt.format.width != cap->pix.fmt.width ||
1350             sd_fmt.format.code != fmt->mbus) {
1351                 dev_dbg(cap->rkisp1->dev,
1352                         "link '%s':%u -> '%s':%u not valid: 0x%04x/%ux%u != 0x%04x/%ux%u\n",
1353                         link->source->entity->name, link->source->index,
1354                         link->sink->entity->name, link->sink->index,
1355                         sd_fmt.format.code, sd_fmt.format.width,
1356                         sd_fmt.format.height, fmt->mbus, cap->pix.fmt.width,
1357                         cap->pix.fmt.height);
1358                 return -EPIPE;
1359         }
1360
1361         return 0;
1362 }
1363
1364 /* ----------------------------------------------------------------------------
1365  * core functions
1366  */
1367
1368 static const struct media_entity_operations rkisp1_media_ops = {
1369         .link_validate = rkisp1_capture_link_validate,
1370 };
1371
1372 static const struct v4l2_file_operations rkisp1_fops = {
1373         .open = v4l2_fh_open,
1374         .release = vb2_fop_release,
1375         .unlocked_ioctl = video_ioctl2,
1376         .poll = vb2_fop_poll,
1377         .mmap = vb2_fop_mmap,
1378 };
1379
1380 static void rkisp1_unregister_capture(struct rkisp1_capture *cap)
1381 {
1382         if (!video_is_registered(&cap->vnode.vdev))
1383                 return;
1384
1385         media_entity_cleanup(&cap->vnode.vdev.entity);
1386         vb2_video_unregister_device(&cap->vnode.vdev);
1387         mutex_destroy(&cap->vnode.vlock);
1388 }
1389
1390 void rkisp1_capture_devs_unregister(struct rkisp1_device *rkisp1)
1391 {
1392         struct rkisp1_capture *mp = &rkisp1->capture_devs[RKISP1_MAINPATH];
1393         struct rkisp1_capture *sp = &rkisp1->capture_devs[RKISP1_SELFPATH];
1394
1395         rkisp1_unregister_capture(mp);
1396         rkisp1_unregister_capture(sp);
1397 }
1398
1399 static int rkisp1_register_capture(struct rkisp1_capture *cap)
1400 {
1401         static const char * const dev_names[] = {
1402                 RKISP1_MP_DEV_NAME, RKISP1_SP_DEV_NAME
1403         };
1404         struct v4l2_device *v4l2_dev = &cap->rkisp1->v4l2_dev;
1405         struct video_device *vdev = &cap->vnode.vdev;
1406         struct rkisp1_vdev_node *node;
1407         struct vb2_queue *q;
1408         int ret;
1409
1410         strscpy(vdev->name, dev_names[cap->id], sizeof(vdev->name));
1411         node = rkisp1_vdev_to_node(vdev);
1412         mutex_init(&node->vlock);
1413
1414         vdev->ioctl_ops = &rkisp1_v4l2_ioctl_ops;
1415         vdev->release = video_device_release_empty;
1416         vdev->fops = &rkisp1_fops;
1417         vdev->minor = -1;
1418         vdev->v4l2_dev = v4l2_dev;
1419         vdev->lock = &node->vlock;
1420         vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE |
1421                             V4L2_CAP_STREAMING | V4L2_CAP_IO_MC;
1422         vdev->entity.ops = &rkisp1_media_ops;
1423         video_set_drvdata(vdev, cap);
1424         vdev->vfl_dir = VFL_DIR_RX;
1425         node->pad.flags = MEDIA_PAD_FL_SINK;
1426
1427         q = &node->buf_queue;
1428         q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
1429         q->io_modes = VB2_MMAP | VB2_DMABUF;
1430         q->drv_priv = cap;
1431         q->ops = &rkisp1_vb2_ops;
1432         q->mem_ops = &vb2_dma_contig_memops;
1433         q->buf_struct_size = sizeof(struct rkisp1_buffer);
1434         q->min_queued_buffers = RKISP1_MIN_BUFFERS_NEEDED;
1435         q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1436         q->lock = &node->vlock;
1437         q->dev = cap->rkisp1->dev;
1438         ret = vb2_queue_init(q);
1439         if (ret) {
1440                 dev_err(cap->rkisp1->dev,
1441                         "vb2 queue init failed (err=%d)\n", ret);
1442                 goto error;
1443         }
1444
1445         vdev->queue = q;
1446
1447         ret = media_entity_pads_init(&vdev->entity, 1, &node->pad);
1448         if (ret)
1449                 goto error;
1450
1451         ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
1452         if (ret) {
1453                 dev_err(cap->rkisp1->dev,
1454                         "failed to register %s, ret=%d\n", vdev->name, ret);
1455                 goto error;
1456         }
1457
1458         v4l2_info(v4l2_dev, "registered %s as /dev/video%d\n", vdev->name,
1459                   vdev->num);
1460
1461         return 0;
1462
1463 error:
1464         media_entity_cleanup(&vdev->entity);
1465         mutex_destroy(&node->vlock);
1466         return ret;
1467 }
1468
1469 static void
1470 rkisp1_capture_init(struct rkisp1_device *rkisp1, enum rkisp1_stream_id id)
1471 {
1472         struct rkisp1_capture *cap = &rkisp1->capture_devs[id];
1473         struct v4l2_pix_format_mplane pixm;
1474
1475         memset(cap, 0, sizeof(*cap));
1476         cap->id = id;
1477         cap->rkisp1 = rkisp1;
1478
1479         INIT_LIST_HEAD(&cap->buf.queue);
1480         init_waitqueue_head(&cap->done);
1481         spin_lock_init(&cap->buf.lock);
1482         if (cap->id == RKISP1_SELFPATH) {
1483                 cap->ops = &rkisp1_capture_ops_sp;
1484                 cap->config = &rkisp1_capture_config_sp;
1485         } else {
1486                 cap->ops = &rkisp1_capture_ops_mp;
1487                 cap->config = &rkisp1_capture_config_mp;
1488         }
1489
1490         cap->is_streaming = false;
1491
1492         memset(&pixm, 0, sizeof(pixm));
1493         pixm.pixelformat = V4L2_PIX_FMT_YUYV;
1494         pixm.width = RKISP1_DEFAULT_WIDTH;
1495         pixm.height = RKISP1_DEFAULT_HEIGHT;
1496         rkisp1_set_fmt(cap, &pixm);
1497 }
1498
1499 int rkisp1_capture_devs_register(struct rkisp1_device *rkisp1)
1500 {
1501         unsigned int i;
1502         int ret;
1503
1504         for (i = 0; i < ARRAY_SIZE(rkisp1->capture_devs); i++) {
1505                 struct rkisp1_capture *cap = &rkisp1->capture_devs[i];
1506
1507                 rkisp1_capture_init(rkisp1, i);
1508
1509                 ret = rkisp1_register_capture(cap);
1510                 if (ret) {
1511                         rkisp1_capture_devs_unregister(rkisp1);
1512                         return ret;
1513                 }
1514         }
1515
1516         return 0;
1517
1518 }