[mirror_ubuntu-jammy-kernel.git] / drivers / media / pci / intel / ipu-isys-csi2-be-soc.c

// SPDX-License-Identifier: GPL-2.0
// Copyright (C) 2014 - 2021 Intel Corporation

#include <linux/device.h>
#include <linux/module.h>

#include <media/ipu-isys.h>
#include <media/media-entity.h>
#include <media/v4l2-device.h>

#include "ipu.h"
#include "ipu-bus.h"
#include "ipu-isys.h"
#include "ipu-isys-csi2-be.h"
#include "ipu-isys-subdev.h"
#include "ipu-isys-video.h"

/*
 * Raw bayer format pixel order MUST BE MAINTAINED in groups of four codes.
 * Otherwise pixel order calculation below WILL BREAK!
 */
static const u32 csi2_be_soc_supported_codes_pad[] = {
	MEDIA_BUS_FMT_Y10_1X10,
	MEDIA_BUS_FMT_RGB565_1X16,
	MEDIA_BUS_FMT_RGB888_1X24,
	MEDIA_BUS_FMT_UYVY8_1X16,
	MEDIA_BUS_FMT_YUYV8_1X16,
	MEDIA_BUS_FMT_SBGGR12_1X12,
	MEDIA_BUS_FMT_SGBRG12_1X12,
	MEDIA_BUS_FMT_SGRBG12_1X12,
	MEDIA_BUS_FMT_SRGGB12_1X12,
	MEDIA_BUS_FMT_SBGGR10_1X10,
	MEDIA_BUS_FMT_SGBRG10_1X10,
	MEDIA_BUS_FMT_SGRBG10_1X10,
	MEDIA_BUS_FMT_SRGGB10_1X10,
	MEDIA_BUS_FMT_SBGGR8_1X8,
	MEDIA_BUS_FMT_SGBRG8_1X8,
	MEDIA_BUS_FMT_SGRBG8_1X8,
	MEDIA_BUS_FMT_SRGGB8_1X8,
	0,
};

/*
 * Raw bayer format pixel order MUST BE MAINTAINED in groups of four codes.
 * Otherwise pixel order calculation below WILL BREAK!
 */
static const u32 csi2_be_soc_supported_raw_bayer_codes_pad[] = {
	MEDIA_BUS_FMT_SBGGR12_1X12,
	MEDIA_BUS_FMT_SGBRG12_1X12,
	MEDIA_BUS_FMT_SGRBG12_1X12,
	MEDIA_BUS_FMT_SRGGB12_1X12,
	MEDIA_BUS_FMT_SBGGR10_1X10,
	MEDIA_BUS_FMT_SGBRG10_1X10,
	MEDIA_BUS_FMT_SGRBG10_1X10,
	MEDIA_BUS_FMT_SRGGB10_1X10,
	MEDIA_BUS_FMT_SBGGR8_1X8,
	MEDIA_BUS_FMT_SGBRG8_1X8,
	MEDIA_BUS_FMT_SGRBG8_1X8,
	MEDIA_BUS_FMT_SRGGB8_1X8,
	0,
};

static int get_supported_code_index(u32 code)
{
	int i;

	for (i = 0; csi2_be_soc_supported_raw_bayer_codes_pad[i]; i++) {
		if (csi2_be_soc_supported_raw_bayer_codes_pad[i] == code)
			return i;
	}
	return -EINVAL;
}

static const u32 *csi2_be_soc_supported_codes[NR_OF_CSI2_BE_SOC_PADS];

static struct v4l2_subdev_internal_ops csi2_be_soc_sd_internal_ops = {
	.open = ipu_isys_subdev_open,
	.close = ipu_isys_subdev_close,
};

static const struct v4l2_subdev_core_ops csi2_be_soc_sd_core_ops = {
};

static const struct v4l2_ctrl_config compression_ctrl_cfg = {
	.ops = NULL,
	.id = V4L2_CID_IPU_ISYS_COMPRESSION,
	.name = "ISYS BE-SOC compression",
	.type = V4L2_CTRL_TYPE_BOOLEAN,
	.min = 0,
	.max = 1,
	.step = 1,
	.def = 0,
};

static int set_stream(struct v4l2_subdev *sd, int enable)
{
	return 0;
}

static const struct v4l2_subdev_video_ops csi2_be_soc_sd_video_ops = {
	.s_stream = set_stream,
};

static int
__subdev_link_validate(struct v4l2_subdev *sd, struct media_link *link,
		       struct v4l2_subdev_format *source_fmt,
		       struct v4l2_subdev_format *sink_fmt)
{
	struct ipu_isys_pipeline *ip = container_of(sd->entity.pipe,
						    struct ipu_isys_pipeline,
						    pipe);

	ip->csi2_be_soc = to_ipu_isys_csi2_be_soc(sd);
	return ipu_isys_subdev_link_validate(sd, link, source_fmt, sink_fmt);
}

static int
ipu_isys_csi2_be_soc_set_sel(struct v4l2_subdev *sd,
			     struct v4l2_subdev_pad_config *cfg,
			     struct v4l2_subdev_selection *sel)
{
	struct ipu_isys_subdev *asd = to_ipu_isys_subdev(sd);
	struct media_pad *pad = &asd->sd.entity.pads[sel->pad];

	if (sel->target == V4L2_SEL_TGT_CROP &&
	    pad->flags & MEDIA_PAD_FL_SOURCE &&
	    asd->valid_tgts[sel->pad].crop) {
		enum isys_subdev_prop_tgt tgt =
		    IPU_ISYS_SUBDEV_PROP_TGT_SOURCE_CROP;
		struct v4l2_mbus_framefmt *ffmt =
			__ipu_isys_get_ffmt(sd, cfg, sel->pad, sel->which);

		if (get_supported_code_index(ffmt->code) < 0) {
			/* Non-bayer formats can't be odd lines cropped */
			sel->r.left &= ~1;
			sel->r.top &= ~1;
		}

		sel->r.width = clamp(sel->r.width, IPU_ISYS_MIN_WIDTH,
				     IPU_ISYS_MAX_WIDTH);

		sel->r.height = clamp(sel->r.height, IPU_ISYS_MIN_HEIGHT,
				      IPU_ISYS_MAX_HEIGHT);

		*__ipu_isys_get_selection(sd, cfg, sel->target, sel->pad,
					  sel->which) = sel->r;
		ipu_isys_subdev_fmt_propagate(sd, cfg, NULL, &sel->r,
					      tgt, sel->pad, sel->which);
		return 0;
	}
	return -EINVAL;
}

static const struct v4l2_subdev_pad_ops csi2_be_soc_sd_pad_ops = {
	.link_validate = __subdev_link_validate,
	.get_fmt = ipu_isys_subdev_get_ffmt,
	.set_fmt = ipu_isys_subdev_set_ffmt,
	.get_selection = ipu_isys_subdev_get_sel,
	.set_selection = ipu_isys_csi2_be_soc_set_sel,
	.enum_mbus_code = ipu_isys_subdev_enum_mbus_code,
};

static struct v4l2_subdev_ops csi2_be_soc_sd_ops = {
	.core = &csi2_be_soc_sd_core_ops,
	.video = &csi2_be_soc_sd_video_ops,
	.pad = &csi2_be_soc_sd_pad_ops,
};

static struct media_entity_operations csi2_be_soc_entity_ops = {
	.link_validate = v4l2_subdev_link_validate,
};

static void csi2_be_soc_set_ffmt(struct v4l2_subdev *sd,
				 struct v4l2_subdev_pad_config *cfg,
				 struct v4l2_subdev_format *fmt)
{
	struct v4l2_mbus_framefmt *ffmt =
		__ipu_isys_get_ffmt(sd, cfg, fmt->pad,
				    fmt->which);

	if (sd->entity.pads[fmt->pad].flags & MEDIA_PAD_FL_SINK) {
		if (fmt->format.field != V4L2_FIELD_ALTERNATE)
			fmt->format.field = V4L2_FIELD_NONE;
		*ffmt = fmt->format;

		ipu_isys_subdev_fmt_propagate(sd, cfg, &fmt->format,
					      NULL,
					      IPU_ISYS_SUBDEV_PROP_TGT_SINK_FMT,
					      fmt->pad, fmt->which);
	} else if (sd->entity.pads[fmt->pad].flags & MEDIA_PAD_FL_SOURCE) {
		struct v4l2_mbus_framefmt *sink_ffmt;
		struct v4l2_rect *r = __ipu_isys_get_selection(sd, cfg,
			V4L2_SEL_TGT_CROP, fmt->pad, fmt->which);
		struct ipu_isys_subdev *asd = to_ipu_isys_subdev(sd);
		u32 code;
		int idx;

		sink_ffmt = __ipu_isys_get_ffmt(sd, cfg, 0, fmt->which);
		code = sink_ffmt->code;
		idx = get_supported_code_index(code);

		if (asd->valid_tgts[fmt->pad].crop && idx >= 0) {
			int crop_info = 0;

			/* Only croping odd line at top side. */
			if (r->top & 1)
				crop_info |= CSI2_BE_CROP_VER;

			code = csi2_be_soc_supported_raw_bayer_codes_pad
				[((idx & CSI2_BE_CROP_MASK) ^ crop_info)
				+ (idx & ~CSI2_BE_CROP_MASK)];

		}
		ffmt->code = code;
		ffmt->width = r->width;
		ffmt->height = r->height;
		ffmt->field = sink_ffmt->field;

	}
}

void ipu_isys_csi2_be_soc_cleanup(struct ipu_isys_csi2_be_soc *csi2_be_soc)
{
	v4l2_device_unregister_subdev(&csi2_be_soc->asd.sd);
	ipu_isys_subdev_cleanup(&csi2_be_soc->asd);
	v4l2_ctrl_handler_free(&csi2_be_soc->av.ctrl_handler);
	ipu_isys_video_cleanup(&csi2_be_soc->av);
}

int ipu_isys_csi2_be_soc_init(struct ipu_isys_csi2_be_soc *csi2_be_soc,
			      struct ipu_isys *isys, int index)
{
	struct v4l2_subdev_format fmt = {
		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
		.pad = CSI2_BE_SOC_PAD_SINK,
		.format = {
			   .width = 4096,
			   .height = 3072,
			   },
	};
	int rval, i;

	csi2_be_soc->asd.sd.entity.ops = &csi2_be_soc_entity_ops;
	csi2_be_soc->asd.isys = isys;

	rval = ipu_isys_subdev_init(&csi2_be_soc->asd,
				    &csi2_be_soc_sd_ops, 0,
				    NR_OF_CSI2_BE_SOC_PADS,
				    NR_OF_CSI2_BE_SOC_SOURCE_PADS,
				    NR_OF_CSI2_BE_SOC_SINK_PADS, 0);
	if (rval)
		goto fail;

	csi2_be_soc->asd.pad[CSI2_BE_SOC_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
	csi2_be_soc->asd.pad[CSI2_BE_SOC_PAD_SOURCE].flags =
		MEDIA_PAD_FL_SOURCE;
	csi2_be_soc->asd.valid_tgts[CSI2_BE_SOC_PAD_SOURCE].crop = true;

	for (i = 0; i < NR_OF_CSI2_BE_SOC_PADS; i++)
		csi2_be_soc_supported_codes[i] =
			csi2_be_soc_supported_codes_pad;
	csi2_be_soc->asd.supported_codes = csi2_be_soc_supported_codes;
	csi2_be_soc->asd.be_mode = IPU_BE_SOC;
	csi2_be_soc->asd.isl_mode = IPU_ISL_OFF;
	csi2_be_soc->asd.set_ffmt = csi2_be_soc_set_ffmt;

	fmt.pad = CSI2_BE_SOC_PAD_SINK;
	ipu_isys_subdev_set_ffmt(&csi2_be_soc->asd.sd, NULL, &fmt);
	fmt.pad = CSI2_BE_SOC_PAD_SOURCE;
	ipu_isys_subdev_set_ffmt(&csi2_be_soc->asd.sd, NULL, &fmt);
	csi2_be_soc->asd.sd.internal_ops = &csi2_be_soc_sd_internal_ops;

	snprintf(csi2_be_soc->asd.sd.name, sizeof(csi2_be_soc->asd.sd.name),
		 IPU_ISYS_ENTITY_PREFIX " CSI2 BE SOC %d", index);
	v4l2_set_subdevdata(&csi2_be_soc->asd.sd, &csi2_be_soc->asd);

	rval = v4l2_device_register_subdev(&isys->v4l2_dev,
					   &csi2_be_soc->asd.sd);
	if (rval) {
		dev_info(&isys->adev->dev, "can't register v4l2 subdev\n");
		goto fail;
	}

	snprintf(csi2_be_soc->av.vdev.name, sizeof(csi2_be_soc->av.vdev.name),
		 IPU_ISYS_ENTITY_PREFIX " BE SOC capture %d", index);

	/*
	 * Pin type could be overwritten for YUV422 to I420 case, at
	 * set_format phase
	 */
	csi2_be_soc->av.aq.css_pin_type = IPU_FW_ISYS_PIN_TYPE_RAW_SOC;
	csi2_be_soc->av.isys = isys;
	csi2_be_soc->av.pfmts = ipu_isys_pfmts_be_soc;
	csi2_be_soc->av.try_fmt_vid_mplane =
		ipu_isys_video_try_fmt_vid_mplane_default;
	csi2_be_soc->av.prepare_fw_stream =
		ipu_isys_prepare_fw_cfg_default;
	csi2_be_soc->av.aq.buf_prepare = ipu_isys_buf_prepare;
	csi2_be_soc->av.aq.fill_frame_buff_set_pin =
		ipu_isys_buffer_to_fw_frame_buff_pin;
	csi2_be_soc->av.aq.link_fmt_validate = ipu_isys_link_fmt_validate;
	csi2_be_soc->av.aq.vbq.buf_struct_size =
		sizeof(struct ipu_isys_video_buffer);

	/* create v4l2 ctrl for be-soc video node */
	rval = v4l2_ctrl_handler_init(&csi2_be_soc->av.ctrl_handler, 0);
	if (rval) {
		dev_err(&isys->adev->dev,
			"failed to init v4l2 ctrl handler for be_soc\n");
		goto fail;
	}

	csi2_be_soc->av.compression_ctrl =
		v4l2_ctrl_new_custom(&csi2_be_soc->av.ctrl_handler,
				     &compression_ctrl_cfg, NULL);
	if (!csi2_be_soc->av.compression_ctrl) {
		dev_err(&isys->adev->dev,
			"failed to create BE-SOC cmprs ctrl\n");
		goto fail;
	}
	csi2_be_soc->av.compression = 0;
	csi2_be_soc->av.vdev.ctrl_handler =
		&csi2_be_soc->av.ctrl_handler;

	rval = ipu_isys_video_init(&csi2_be_soc->av,
				   &csi2_be_soc->asd.sd.entity,
				   CSI2_BE_SOC_PAD_SOURCE,
				   MEDIA_PAD_FL_SINK,
				   MEDIA_LNK_FL_DYNAMIC);
	if (rval) {
		dev_info(&isys->adev->dev, "can't init video node\n");
		goto fail;
	}

	return 0;

fail:
	ipu_isys_csi2_be_soc_cleanup(csi2_be_soc);

	return rval;
}
Commit	Line	Data
f2efa4ee	1	// SPDX-License-Identifier: GPL-2.0
fad9119b	2	// Copyright (C) 2014 - 2021 Intel Corporation
f2efa4ee WY	3
	4	#include <linux/device.h>
	5	#include <linux/module.h>
	6
	7	#include <media/ipu-isys.h>
	8	#include <media/media-entity.h>
	9	#include <media/v4l2-device.h>
	10
	11	#include "ipu.h"
	12	#include "ipu-bus.h"
	13	#include "ipu-isys.h"
	14	#include "ipu-isys-csi2-be.h"
	15	#include "ipu-isys-subdev.h"
	16	#include "ipu-isys-video.h"
	17
	18	/*
	19	* Raw bayer format pixel order MUST BE MAINTAINED in groups of four codes.
	20	* Otherwise pixel order calculation below WILL BREAK!
	21	*/
	22	static const u32 csi2_be_soc_supported_codes_pad[] = {
	23	MEDIA_BUS_FMT_Y10_1X10,
	24	MEDIA_BUS_FMT_RGB565_1X16,
	25	MEDIA_BUS_FMT_RGB888_1X24,
	26	MEDIA_BUS_FMT_UYVY8_1X16,
	27	MEDIA_BUS_FMT_YUYV8_1X16,
	28	MEDIA_BUS_FMT_SBGGR12_1X12,
	29	MEDIA_BUS_FMT_SGBRG12_1X12,
	30	MEDIA_BUS_FMT_SGRBG12_1X12,
	31	MEDIA_BUS_FMT_SRGGB12_1X12,
	32	MEDIA_BUS_FMT_SBGGR10_1X10,
	33	MEDIA_BUS_FMT_SGBRG10_1X10,
	34	MEDIA_BUS_FMT_SGRBG10_1X10,
	35	MEDIA_BUS_FMT_SRGGB10_1X10,
	36	MEDIA_BUS_FMT_SBGGR8_1X8,
	37	MEDIA_BUS_FMT_SGBRG8_1X8,
	38	MEDIA_BUS_FMT_SGRBG8_1X8,
	39	MEDIA_BUS_FMT_SRGGB8_1X8,
	40	0,
	41	};
	42
fad9119b WY	43	/*
	44	* Raw bayer format pixel order MUST BE MAINTAINED in groups of four codes.
	45	* Otherwise pixel order calculation below WILL BREAK!
	46	*/
	47	static const u32 csi2_be_soc_supported_raw_bayer_codes_pad[] = {
	48	MEDIA_BUS_FMT_SBGGR12_1X12,
	49	MEDIA_BUS_FMT_SGBRG12_1X12,
	50	MEDIA_BUS_FMT_SGRBG12_1X12,
	51	MEDIA_BUS_FMT_SRGGB12_1X12,
	52	MEDIA_BUS_FMT_SBGGR10_1X10,
	53	MEDIA_BUS_FMT_SGBRG10_1X10,
	54	MEDIA_BUS_FMT_SGRBG10_1X10,
	55	MEDIA_BUS_FMT_SRGGB10_1X10,
	56	MEDIA_BUS_FMT_SBGGR8_1X8,
	57	MEDIA_BUS_FMT_SGBRG8_1X8,
	58	MEDIA_BUS_FMT_SGRBG8_1X8,
	59	MEDIA_BUS_FMT_SRGGB8_1X8,
	60	0,
	61	};
	62
	63	static int get_supported_code_index(u32 code)
	64	{
	65	int i;
	66
	67	for (i = 0; csi2_be_soc_supported_raw_bayer_codes_pad[i]; i++) {
	68	if (csi2_be_soc_supported_raw_bayer_codes_pad[i] == code)
	69	return i;
	70	}
	71	return -EINVAL;
	72	}
	73
f2efa4ee WY	74	static const u32 *csi2_be_soc_supported_codes[NR_OF_CSI2_BE_SOC_PADS];
	75
	76	static struct v4l2_subdev_internal_ops csi2_be_soc_sd_internal_ops = {
	77	.open = ipu_isys_subdev_open,
	78	.close = ipu_isys_subdev_close,
	79	};
	80
	81	static const struct v4l2_subdev_core_ops csi2_be_soc_sd_core_ops = {
	82	};
	83
3ebd4441 WY	84	static const struct v4l2_ctrl_config compression_ctrl_cfg = {
	85	.ops = NULL,
	86	.id = V4L2_CID_IPU_ISYS_COMPRESSION,
	87	.name = "ISYS BE-SOC compression",
	88	.type = V4L2_CTRL_TYPE_BOOLEAN,
	89	.min = 0,
	90	.max = 1,
	91	.step = 1,
	92	.def = 0,
	93	};
	94
f2efa4ee WY	95	static int set_stream(struct v4l2_subdev *sd, int enable)
	96	{
	97	return 0;
	98	}
	99
	100	static const struct v4l2_subdev_video_ops csi2_be_soc_sd_video_ops = {
	101	.s_stream = set_stream,
	102	};
	103
	104	static int
	105	__subdev_link_validate(struct v4l2_subdev sd, struct media_link link,
	106	struct v4l2_subdev_format *source_fmt,
	107	struct v4l2_subdev_format *sink_fmt)
	108	{
	109	struct ipu_isys_pipeline *ip = container_of(sd->entity.pipe,
	110	struct ipu_isys_pipeline,
	111	pipe);
	112
	113	ip->csi2_be_soc = to_ipu_isys_csi2_be_soc(sd);
	114	return ipu_isys_subdev_link_validate(sd, link, source_fmt, sink_fmt);
	115	}
	116
	117	static int
	118	ipu_isys_csi2_be_soc_set_sel(struct v4l2_subdev *sd,
	119	struct v4l2_subdev_pad_config *cfg,
	120	struct v4l2_subdev_selection *sel)
	121	{
	122	struct ipu_isys_subdev *asd = to_ipu_isys_subdev(sd);
	123	struct media_pad *pad = &asd->sd.entity.pads[sel->pad];
	124
	125	if (sel->target == V4L2_SEL_TGT_CROP &&
	126	pad->flags & MEDIA_PAD_FL_SOURCE &&
	127	asd->valid_tgts[sel->pad].crop) {
f2efa4ee WY	128	enum isys_subdev_prop_tgt tgt =
f2efa4ee WY	129	IPU_ISYS_SUBDEV_PROP_TGT_SOURCE_CROP;
fad9119b WY	130	struct v4l2_mbus_framefmt *ffmt =
fad9119b WY	131	__ipu_isys_get_ffmt(sd, cfg, sel->pad, sel->which);
f2efa4ee	132
fad9119b WY	133	if (get_supported_code_index(ffmt->code) < 0) {
	134	/* Non-bayer formats can't be odd lines cropped */
	135	sel->r.left &= ~1;
	136	sel->r.top &= ~1;
	137	}
f2efa4ee	138
fad9119b	139	sel->r.width = clamp(sel->r.width, IPU_ISYS_MIN_WIDTH,
f2efa4ee	140	IPU_ISYS_MAX_WIDTH);
fad9119b WY	141
	142	sel->r.height = clamp(sel->r.height, IPU_ISYS_MIN_HEIGHT,
	143	IPU_ISYS_MAX_HEIGHT);
f2efa4ee WY	144
	145	*__ipu_isys_get_selection(sd, cfg, sel->target, sel->pad,
	146	sel->which) = sel->r;
	147	ipu_isys_subdev_fmt_propagate(sd, cfg, NULL, &sel->r,
	148	tgt, sel->pad, sel->which);
	149	return 0;
	150	}
	151	return -EINVAL;
	152	}
	153
	154	static const struct v4l2_subdev_pad_ops csi2_be_soc_sd_pad_ops = {
	155	.link_validate = __subdev_link_validate,
	156	.get_fmt = ipu_isys_subdev_get_ffmt,
	157	.set_fmt = ipu_isys_subdev_set_ffmt,
	158	.get_selection = ipu_isys_subdev_get_sel,
	159	.set_selection = ipu_isys_csi2_be_soc_set_sel,
	160	.enum_mbus_code = ipu_isys_subdev_enum_mbus_code,
	161	};
	162
	163	static struct v4l2_subdev_ops csi2_be_soc_sd_ops = {
	164	.core = &csi2_be_soc_sd_core_ops,
	165	.video = &csi2_be_soc_sd_video_ops,
	166	.pad = &csi2_be_soc_sd_pad_ops,
	167	};
	168
	169	static struct media_entity_operations csi2_be_soc_entity_ops = {
	170	.link_validate = v4l2_subdev_link_validate,
	171	};
	172
	173	static void csi2_be_soc_set_ffmt(struct v4l2_subdev *sd,
	174	struct v4l2_subdev_pad_config *cfg,
	175	struct v4l2_subdev_format *fmt)
	176	{
	177	struct v4l2_mbus_framefmt *ffmt =
	178	__ipu_isys_get_ffmt(sd, cfg, fmt->pad,
	179	fmt->which);
	180
	181	if (sd->entity.pads[fmt->pad].flags & MEDIA_PAD_FL_SINK) {
	182	if (fmt->format.field != V4L2_FIELD_ALTERNATE)
	183	fmt->format.field = V4L2_FIELD_NONE;
	184	*ffmt = fmt->format;
	185
	186	ipu_isys_subdev_fmt_propagate(sd, cfg, &fmt->format,
	187	NULL,
	188	IPU_ISYS_SUBDEV_PROP_TGT_SINK_FMT,
	189	fmt->pad, fmt->which);
	190	} else if (sd->entity.pads[fmt->pad].flags & MEDIA_PAD_FL_SOURCE) {
	191	struct v4l2_mbus_framefmt *sink_ffmt;
fad9119b WY	192	struct v4l2_rect *r = __ipu_isys_get_selection(sd, cfg,
	193	V4L2_SEL_TGT_CROP, fmt->pad, fmt->which);
	194	struct ipu_isys_subdev *asd = to_ipu_isys_subdev(sd);
	195	u32 code;
	196	int idx;
f2efa4ee WY	197
f2efa4ee WY	198	sink_ffmt = __ipu_isys_get_ffmt(sd, cfg, 0, fmt->which);
fad9119b WY	199	code = sink_ffmt->code;
	200	idx = get_supported_code_index(code);
	201
	202	if (asd->valid_tgts[fmt->pad].crop && idx >= 0) {
	203	int crop_info = 0;
	204
	205	/* Only croping odd line at top side. */
	206	if (r->top & 1)
	207	crop_info \|= CSI2_BE_CROP_VER;
	208
	209	code = csi2_be_soc_supported_raw_bayer_codes_pad
	210	[((idx & CSI2_BE_CROP_MASK) ^ crop_info)
	211	+ (idx & ~CSI2_BE_CROP_MASK)];
f2efa4ee	212
fad9119b WY	213	}
fad9119b WY	214	ffmt->code = code;
f2efa4ee WY	215	ffmt->width = r->width;
f2efa4ee WY	216	ffmt->height = r->height;
f2efa4ee WY	217	ffmt->field = sink_ffmt->field;
	218
	219	}
	220	}
	221
	222	void ipu_isys_csi2_be_soc_cleanup(struct ipu_isys_csi2_be_soc *csi2_be_soc)
	223	{
	224	v4l2_device_unregister_subdev(&csi2_be_soc->asd.sd);
	225	ipu_isys_subdev_cleanup(&csi2_be_soc->asd);
3ebd4441	226	v4l2_ctrl_handler_free(&csi2_be_soc->av.ctrl_handler);
f2efa4ee WY	227	ipu_isys_video_cleanup(&csi2_be_soc->av);
	228	}
	229
	230	int ipu_isys_csi2_be_soc_init(struct ipu_isys_csi2_be_soc *csi2_be_soc,
	231	struct ipu_isys *isys, int index)
	232	{
	233	struct v4l2_subdev_format fmt = {
	234	.which = V4L2_SUBDEV_FORMAT_ACTIVE,
	235	.pad = CSI2_BE_SOC_PAD_SINK,
	236	.format = {
	237	.width = 4096,
	238	.height = 3072,
	239	},
	240	};
	241	int rval, i;
	242
	243	csi2_be_soc->asd.sd.entity.ops = &csi2_be_soc_entity_ops;
	244	csi2_be_soc->asd.isys = isys;
	245
	246	rval = ipu_isys_subdev_init(&csi2_be_soc->asd,
	247	&csi2_be_soc_sd_ops, 0,
	248	NR_OF_CSI2_BE_SOC_PADS,
	249	NR_OF_CSI2_BE_SOC_SOURCE_PADS,
	250	NR_OF_CSI2_BE_SOC_SINK_PADS, 0);
	251	if (rval)
	252	goto fail;
	253
	254	csi2_be_soc->asd.pad[CSI2_BE_SOC_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
	255	csi2_be_soc->asd.pad[CSI2_BE_SOC_PAD_SOURCE].flags =
	256	MEDIA_PAD_FL_SOURCE;
	257	csi2_be_soc->asd.valid_tgts[CSI2_BE_SOC_PAD_SOURCE].crop = true;
	258
	259	for (i = 0; i < NR_OF_CSI2_BE_SOC_PADS; i++)
	260	csi2_be_soc_supported_codes[i] =
	261	csi2_be_soc_supported_codes_pad;
	262	csi2_be_soc->asd.supported_codes = csi2_be_soc_supported_codes;
	263	csi2_be_soc->asd.be_mode = IPU_BE_SOC;
	264	csi2_be_soc->asd.isl_mode = IPU_ISL_OFF;
	265	csi2_be_soc->asd.set_ffmt = csi2_be_soc_set_ffmt;
	266
	267	fmt.pad = CSI2_BE_SOC_PAD_SINK;
	268	ipu_isys_subdev_set_ffmt(&csi2_be_soc->asd.sd, NULL, &fmt);
	269	fmt.pad = CSI2_BE_SOC_PAD_SOURCE;
	270	ipu_isys_subdev_set_ffmt(&csi2_be_soc->asd.sd, NULL, &fmt);
	271	csi2_be_soc->asd.sd.internal_ops = &csi2_be_soc_sd_internal_ops;
	272
	273	snprintf(csi2_be_soc->asd.sd.name, sizeof(csi2_be_soc->asd.sd.name),
	274	IPU_ISYS_ENTITY_PREFIX " CSI2 BE SOC %d", index);
	275	v4l2_set_subdevdata(&csi2_be_soc->asd.sd, &csi2_be_soc->asd);
	276
	277	rval = v4l2_device_register_subdev(&isys->v4l2_dev,
	278	&csi2_be_soc->asd.sd);
	279	if (rval) {
	280	dev_info(&isys->adev->dev, "can't register v4l2 subdev\n");
	281	goto fail;
	282	}
	283
	284	snprintf(csi2_be_soc->av.vdev.name, sizeof(csi2_be_soc->av.vdev.name),
	285	IPU_ISYS_ENTITY_PREFIX " BE SOC capture %d", index);
	286
	287	/*
	288	* Pin type could be overwritten for YUV422 to I420 case, at
	289	* set_format phase
	290	*/
291	csi2_be_soc->av.aq.css_pin_type = IPU_FW_ISYS_PIN_TYPE_RAW_SOC;
292	csi2_be_soc->av.isys = isys;
293	csi2_be_soc->av.pfmts = ipu_isys_pfmts_be_soc;
294	csi2_be_soc->av.try_fmt_vid_mplane =
295	ipu_isys_video_try_fmt_vid_mplane_default;
296	csi2_be_soc->av.prepare_fw_stream =
297	ipu_isys_prepare_fw_cfg_default;
298	csi2_be_soc->av.aq.buf_prepare = ipu_isys_buf_prepare;
299	csi2_be_soc->av.aq.fill_frame_buff_set_pin =
300	ipu_isys_buffer_to_fw_frame_buff_pin;
301	csi2_be_soc->av.aq.link_fmt_validate = ipu_isys_link_fmt_validate;
302	csi2_be_soc->av.aq.vbq.buf_struct_size =
303	sizeof(struct ipu_isys_video_buffer);
304
3ebd4441 WY	305	/* create v4l2 ctrl for be-soc video node */
	306	rval = v4l2_ctrl_handler_init(&csi2_be_soc->av.ctrl_handler, 0);
	307	if (rval) {
	308	dev_err(&isys->adev->dev,
	309	"failed to init v4l2 ctrl handler for be_soc\n");
	310	goto fail;
	311	}
	312
	313	csi2_be_soc->av.compression_ctrl =
	314	v4l2_ctrl_new_custom(&csi2_be_soc->av.ctrl_handler,
	315	&compression_ctrl_cfg, NULL);
	316	if (!csi2_be_soc->av.compression_ctrl) {
	317	dev_err(&isys->adev->dev,
	318	"failed to create BE-SOC cmprs ctrl\n");
	319	goto fail;
	320	}
	321	csi2_be_soc->av.compression = 0;
	322	csi2_be_soc->av.vdev.ctrl_handler =
	323	&csi2_be_soc->av.ctrl_handler;
	324
f2efa4ee WY	325	rval = ipu_isys_video_init(&csi2_be_soc->av,
	326	&csi2_be_soc->asd.sd.entity,
	327	CSI2_BE_SOC_PAD_SOURCE,
	328	MEDIA_PAD_FL_SINK,
	329	MEDIA_LNK_FL_DYNAMIC);
	330	if (rval) {
	331	dev_info(&isys->adev->dev, "can't init video node\n");
	332	goto fail;
	333	}
	334
	335	return 0;
	336
	337	fail:
	338	ipu_isys_csi2_be_soc_cleanup(csi2_be_soc);
	339
	340	return rval;
	341	}