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