path: root/drivers/media/platform/amlogic/c3/isp/c3-isp-core.c

// SPDX-License-Identifier: (GPL-2.0-only OR MIT)
/*
 * Copyright (C) 2024 Amlogic, Inc. All rights reserved
 */

#include <linux/media/amlogic/c3-isp-config.h>
#include <linux/pm_runtime.h>

#include <media/v4l2-event.h>

#include "c3-isp-common.h"
#include "c3-isp-regs.h"

#define C3_ISP_CORE_SUBDEV_NAME		"c3-isp-core"

#define C3_ISP_PHASE_OFFSET_0		0
#define C3_ISP_PHASE_OFFSET_1		1
#define C3_ISP_PHASE_OFFSET_NONE	0xff

#define C3_ISP_CORE_DEF_SINK_PAD_FMT	MEDIA_BUS_FMT_SRGGB10_1X10
#define C3_ISP_CORE_DEF_SRC_PAD_FMT	MEDIA_BUS_FMT_YUV10_1X30

/*
 * struct c3_isp_core_format_info - ISP core format information
 *
 * @mbus_code: the mbus code
 * @pads: bitmask detailing valid pads for this mbus_code
 * @xofst: horizontal phase offset of hardware
 * @yofst: vertical phase offset of hardware
 * @is_raw: the raw format flag of mbus code
 */
struct c3_isp_core_format_info {
	u32 mbus_code;
	u32 pads;
	u8 xofst;
	u8 yofst;
	bool is_raw;
};

static const struct c3_isp_core_format_info c3_isp_core_fmts[] = {
	/* RAW formats */
	{
		.mbus_code	= MEDIA_BUS_FMT_SBGGR10_1X10,
		.pads		= BIT(C3_ISP_CORE_PAD_SINK_VIDEO),
		.xofst		= C3_ISP_PHASE_OFFSET_0,
		.yofst		= C3_ISP_PHASE_OFFSET_1,
		.is_raw		= true,
	}, {
		.mbus_code	= MEDIA_BUS_FMT_SGBRG10_1X10,
		.pads		= BIT(C3_ISP_CORE_PAD_SINK_VIDEO),
		.xofst		= C3_ISP_PHASE_OFFSET_1,
		.yofst		= C3_ISP_PHASE_OFFSET_1,
		.is_raw		= true,
	}, {
		.mbus_code	= MEDIA_BUS_FMT_SGRBG10_1X10,
		.pads		= BIT(C3_ISP_CORE_PAD_SINK_VIDEO),
		.xofst		= C3_ISP_PHASE_OFFSET_0,
		.yofst		= C3_ISP_PHASE_OFFSET_0,
		.is_raw		= true,
	}, {
		.mbus_code	= MEDIA_BUS_FMT_SRGGB10_1X10,
		.pads		= BIT(C3_ISP_CORE_PAD_SINK_VIDEO),
		.xofst		= C3_ISP_PHASE_OFFSET_1,
		.yofst		= C3_ISP_PHASE_OFFSET_0,
		.is_raw		= true,
	}, {
		.mbus_code	= MEDIA_BUS_FMT_SBGGR12_1X12,
		.pads		= BIT(C3_ISP_CORE_PAD_SINK_VIDEO),
		.xofst		= C3_ISP_PHASE_OFFSET_0,
		.yofst		= C3_ISP_PHASE_OFFSET_1,
		.is_raw		= true,
	}, {
		.mbus_code	= MEDIA_BUS_FMT_SGBRG12_1X12,
		.pads		= BIT(C3_ISP_CORE_PAD_SINK_VIDEO),
		.xofst		= C3_ISP_PHASE_OFFSET_1,
		.yofst		= C3_ISP_PHASE_OFFSET_1,
		.is_raw		= true,
	}, {
		.mbus_code	= MEDIA_BUS_FMT_SGRBG12_1X12,
		.pads		= BIT(C3_ISP_CORE_PAD_SINK_VIDEO),
		.xofst		= C3_ISP_PHASE_OFFSET_0,
		.yofst		= C3_ISP_PHASE_OFFSET_0,
		.is_raw		= true,
	}, {
		.mbus_code	= MEDIA_BUS_FMT_SRGGB12_1X12,
		.pads		= BIT(C3_ISP_CORE_PAD_SINK_VIDEO),
		.xofst		= C3_ISP_PHASE_OFFSET_1,
		.yofst		= C3_ISP_PHASE_OFFSET_0,
		.is_raw		= true,
	}, {
		.mbus_code	= MEDIA_BUS_FMT_SRGGB16_1X16,
		.pads		= BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_0)
				| BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_1)
				| BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_2),
		.xofst		= C3_ISP_PHASE_OFFSET_NONE,
		.yofst		= C3_ISP_PHASE_OFFSET_NONE,
		.is_raw		= true,
	}, {
		.mbus_code	= MEDIA_BUS_FMT_SBGGR16_1X16,
		.pads		= BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_0)
				| BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_1)
				| BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_2),
		.xofst		= C3_ISP_PHASE_OFFSET_NONE,
		.yofst		= C3_ISP_PHASE_OFFSET_NONE,
		.is_raw		= true,
	}, {
		.mbus_code	= MEDIA_BUS_FMT_SGRBG16_1X16,
		.pads		= BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_0)
				| BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_1)
				| BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_2),
		.xofst		= C3_ISP_PHASE_OFFSET_NONE,
		.yofst		= C3_ISP_PHASE_OFFSET_NONE,
		.is_raw		= true,
	}, {
		.mbus_code	= MEDIA_BUS_FMT_SGBRG16_1X16,
		.pads		= BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_0)
				| BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_1)
				| BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_2),
		.xofst		= C3_ISP_PHASE_OFFSET_NONE,
		.yofst		= C3_ISP_PHASE_OFFSET_NONE,
		.is_raw		= true,
	},
	/* YUV formats */
	{
		.mbus_code	= MEDIA_BUS_FMT_YUV10_1X30,
		.pads		= BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_0) |
				  BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_1) |
				  BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_2),
		.xofst		= C3_ISP_PHASE_OFFSET_NONE,
		.yofst		= C3_ISP_PHASE_OFFSET_NONE,
		.is_raw		= false,
	},
};

static const struct c3_isp_core_format_info
*core_find_format_by_code(u32 code, u32 pad)
{
	for (unsigned int i = 0; i < ARRAY_SIZE(c3_isp_core_fmts); i++) {
		const struct c3_isp_core_format_info *info =
			&c3_isp_core_fmts[i];

		if (info->mbus_code == code && info->pads & BIT(pad))
			return info;
	}

	return NULL;
}

static const struct c3_isp_core_format_info
*core_find_format_by_index(u32 index, u32 pad)
{
	for (unsigned int i = 0; i < ARRAY_SIZE(c3_isp_core_fmts); i++) {
		const struct c3_isp_core_format_info *info =
			&c3_isp_core_fmts[i];

		if (!(info->pads & BIT(pad)))
			continue;

		if (!index)
			return info;

		index--;
	}

	return NULL;
}

static void c3_isp_core_enable(struct c3_isp_device *isp)
{
	c3_isp_update_bits(isp, ISP_TOP_IRQ_EN, ISP_TOP_IRQ_EN_FRM_END_MASK,
			   ISP_TOP_IRQ_EN_FRM_END_EN);
	c3_isp_update_bits(isp, ISP_TOP_IRQ_EN, ISP_TOP_IRQ_EN_FRM_RST_MASK,
			   ISP_TOP_IRQ_EN_FRM_RST_EN);

	/* Enable image data to ISP core */
	c3_isp_update_bits(isp, ISP_TOP_PATH_SEL, ISP_TOP_PATH_SEL_CORE_MASK,
			   ISP_TOP_PATH_SEL_CORE_MIPI_CORE);
}

static void c3_isp_core_disable(struct c3_isp_device *isp)
{
	/* Disable image data to ISP core */
	c3_isp_update_bits(isp, ISP_TOP_PATH_SEL, ISP_TOP_PATH_SEL_CORE_MASK,
			   ISP_TOP_PATH_SEL_CORE_CORE_DIS);

	c3_isp_update_bits(isp, ISP_TOP_IRQ_EN, ISP_TOP_IRQ_EN_FRM_END_MASK,
			   ISP_TOP_IRQ_EN_FRM_END_DIS);
	c3_isp_update_bits(isp, ISP_TOP_IRQ_EN, ISP_TOP_IRQ_EN_FRM_RST_MASK,
			   ISP_TOP_IRQ_EN_FRM_RST_DIS);
}

/* Set the phase offset of blc, wb and lns */
static void c3_isp_core_lswb_ofst(struct c3_isp_device *isp,
				  u8 xofst, u8 yofst)
{
	c3_isp_update_bits(isp, ISP_LSWB_BLC_PHSOFST,
			   ISP_LSWB_BLC_PHSOFST_HORIZ_OFST_MASK,
			   ISP_LSWB_BLC_PHSOFST_HORIZ_OFST(xofst));
	c3_isp_update_bits(isp, ISP_LSWB_BLC_PHSOFST,
			   ISP_LSWB_BLC_PHSOFST_VERT_OFST_MASK,
			   ISP_LSWB_BLC_PHSOFST_VERT_OFST(yofst));

	c3_isp_update_bits(isp, ISP_LSWB_WB_PHSOFST,
			   ISP_LSWB_WB_PHSOFST_HORIZ_OFST_MASK,
			   ISP_LSWB_WB_PHSOFST_HORIZ_OFST(xofst));
	c3_isp_update_bits(isp, ISP_LSWB_WB_PHSOFST,
			   ISP_LSWB_WB_PHSOFST_VERT_OFST_MASK,
			   ISP_LSWB_WB_PHSOFST_VERT_OFST(yofst));

	c3_isp_update_bits(isp, ISP_LSWB_LNS_PHSOFST,
			   ISP_LSWB_LNS_PHSOFST_HORIZ_OFST_MASK,
			   ISP_LSWB_LNS_PHSOFST_HORIZ_OFST(xofst));
	c3_isp_update_bits(isp, ISP_LSWB_LNS_PHSOFST,
			   ISP_LSWB_LNS_PHSOFST_VERT_OFST_MASK,
			   ISP_LSWB_LNS_PHSOFST_VERT_OFST(yofst));
}

/* Set the phase offset of af, ae and awb */
static void c3_isp_core_3a_ofst(struct c3_isp_device *isp,
				u8 xofst, u8 yofst)
{
	c3_isp_update_bits(isp, ISP_AF_CTRL, ISP_AF_CTRL_HORIZ_OFST_MASK,
			   ISP_AF_CTRL_HORIZ_OFST(xofst));
	c3_isp_update_bits(isp, ISP_AF_CTRL, ISP_AF_CTRL_VERT_OFST_MASK,
			   ISP_AF_CTRL_VERT_OFST(yofst));

	c3_isp_update_bits(isp, ISP_AE_CTRL, ISP_AE_CTRL_HORIZ_OFST_MASK,
			   ISP_AE_CTRL_HORIZ_OFST(xofst));
	c3_isp_update_bits(isp, ISP_AE_CTRL, ISP_AE_CTRL_VERT_OFST_MASK,
			   ISP_AE_CTRL_VERT_OFST(yofst));

	c3_isp_update_bits(isp, ISP_AWB_CTRL, ISP_AWB_CTRL_HORIZ_OFST_MASK,
			   ISP_AWB_CTRL_HORIZ_OFST(xofst));
	c3_isp_update_bits(isp, ISP_AWB_CTRL, ISP_AWB_CTRL_VERT_OFST_MASK,
			   ISP_AWB_CTRL_VERT_OFST(yofst));
}

/* Set the phase offset of demosaic */
static void c3_isp_core_dms_ofst(struct c3_isp_device *isp,
				 u8 xofst, u8 yofst)
{
	c3_isp_update_bits(isp, ISP_DMS_COMMON_PARAM0,
			   ISP_DMS_COMMON_PARAM0_HORIZ_PHS_OFST_MASK,
			   ISP_DMS_COMMON_PARAM0_HORIZ_PHS_OFST(xofst));
	c3_isp_update_bits(isp, ISP_DMS_COMMON_PARAM0,
			   ISP_DMS_COMMON_PARAM0_VERT_PHS_OFST_MASK,
			   ISP_DMS_COMMON_PARAM0_VERT_PHS_OFST(yofst));
}

static void c3_isp_core_cfg_format(struct c3_isp_device *isp,
				   struct v4l2_subdev_state *state)
{
	struct v4l2_mbus_framefmt *fmt;
	const struct c3_isp_core_format_info *isp_fmt;

	fmt = v4l2_subdev_state_get_format(state, C3_ISP_CORE_PAD_SINK_VIDEO);
	isp_fmt = core_find_format_by_code(fmt->code,
					   C3_ISP_CORE_PAD_SINK_VIDEO);

	c3_isp_write(isp, ISP_TOP_INPUT_SIZE,
		     ISP_TOP_INPUT_SIZE_HORIZ_SIZE(fmt->width) |
		     ISP_TOP_INPUT_SIZE_VERT_SIZE(fmt->height));
	c3_isp_write(isp, ISP_TOP_FRM_SIZE,
		     ISP_TOP_FRM_SIZE_CORE_HORIZ_SIZE(fmt->width) |
		     ISP_TOP_FRM_SIZE_CORE_VERT_SIZE(fmt->height));

	c3_isp_update_bits(isp, ISP_TOP_HOLD_SIZE,
			   ISP_TOP_HOLD_SIZE_CORE_HORIZ_SIZE_MASK,
			   ISP_TOP_HOLD_SIZE_CORE_HORIZ_SIZE(fmt->width));

	c3_isp_write(isp, ISP_AF_HV_SIZE,
		     ISP_AF_HV_SIZE_GLB_WIN_XSIZE(fmt->width) |
		     ISP_AF_HV_SIZE_GLB_WIN_YSIZE(fmt->height));
	c3_isp_write(isp, ISP_AE_HV_SIZE,
		     ISP_AE_HV_SIZE_HORIZ_SIZE(fmt->width) |
		     ISP_AE_HV_SIZE_VERT_SIZE(fmt->height));
	c3_isp_write(isp, ISP_AWB_HV_SIZE,
		     ISP_AWB_HV_SIZE_HORIZ_SIZE(fmt->width) |
		     ISP_AWB_HV_SIZE_VERT_SIZE(fmt->height));

	c3_isp_core_lswb_ofst(isp, isp_fmt->xofst, isp_fmt->yofst);
	c3_isp_core_3a_ofst(isp, isp_fmt->xofst, isp_fmt->yofst);
	c3_isp_core_dms_ofst(isp, isp_fmt->xofst, isp_fmt->yofst);
}

static bool c3_isp_core_streams_ready(struct c3_isp_core *core)
{
	unsigned int n_links = 0;
	struct media_link *link;

	for_each_media_entity_data_link(&core->sd.entity, link) {
		if ((link->source->index == C3_ISP_CORE_PAD_SOURCE_VIDEO_0 ||
		     link->source->index == C3_ISP_CORE_PAD_SOURCE_VIDEO_1 ||
		     link->source->index == C3_ISP_CORE_PAD_SOURCE_VIDEO_2) &&
		    link->flags == MEDIA_LNK_FL_ENABLED)
			n_links++;
	}

	return n_links == core->isp->pipe.start_count;
}

static int c3_isp_core_enable_streams(struct v4l2_subdev *sd,
				      struct v4l2_subdev_state *state,
				      u32 pad, u64 streams_mask)
{
	struct c3_isp_core *core = v4l2_get_subdevdata(sd);
	struct media_pad *sink_pad;
	struct v4l2_subdev *src_sd;
	int ret;

	if (!c3_isp_core_streams_ready(core))
		return 0;

	core->isp->frm_sequence = 0;
	c3_isp_core_cfg_format(core->isp, state);
	c3_isp_core_enable(core->isp);

	sink_pad = &core->pads[C3_ISP_CORE_PAD_SINK_VIDEO];
	core->src_pad = media_pad_remote_pad_unique(sink_pad);
	if (IS_ERR(core->src_pad)) {
		dev_dbg(core->isp->dev,
			"Failed to get source pad for ISP core\n");
		return -EPIPE;
	}

	src_sd = media_entity_to_v4l2_subdev(core->src_pad->entity);

	ret = v4l2_subdev_enable_streams(src_sd, core->src_pad->index, BIT(0));
	if (ret) {
		c3_isp_core_disable(core->isp);
		return ret;
	}

	return 0;
}

static int c3_isp_core_disable_streams(struct v4l2_subdev *sd,
				       struct v4l2_subdev_state *state,
				       u32 pad, u64 streams_mask)
{
	struct c3_isp_core *core = v4l2_get_subdevdata(sd);
	struct v4l2_subdev *src_sd;

	if (core->isp->pipe.start_count != 1)
		return 0;

	if (core->src_pad) {
		src_sd = media_entity_to_v4l2_subdev(core->src_pad->entity);
		v4l2_subdev_disable_streams(src_sd, core->src_pad->index,
					    BIT(0));
	}
	core->src_pad = NULL;

	c3_isp_core_disable(core->isp);

	return 0;
}

static int c3_isp_core_enum_mbus_code(struct v4l2_subdev *sd,
				      struct v4l2_subdev_state *state,
				      struct v4l2_subdev_mbus_code_enum *code)
{
	const struct c3_isp_core_format_info *info;

	switch (code->pad) {
	case C3_ISP_CORE_PAD_SINK_VIDEO:
	case C3_ISP_CORE_PAD_SOURCE_VIDEO_0:
	case C3_ISP_CORE_PAD_SOURCE_VIDEO_1:
	case C3_ISP_CORE_PAD_SOURCE_VIDEO_2:
		info = core_find_format_by_index(code->index, code->pad);
		if (!info)
			return -EINVAL;

		code->code = info->mbus_code;

		break;
	case C3_ISP_CORE_PAD_SINK_PARAMS:
	case C3_ISP_CORE_PAD_SOURCE_STATS:
		if (code->index)
			return -EINVAL;

		code->code = MEDIA_BUS_FMT_METADATA_FIXED;

		break;
	default:
		return -EINVAL;
	}

	return 0;
}

static void c3_isp_core_set_sink_fmt(struct v4l2_subdev_state *state,
				     struct v4l2_subdev_format *format)
{
	struct v4l2_mbus_framefmt *sink_fmt;
	struct v4l2_mbus_framefmt *src_fmt;
	const struct c3_isp_core_format_info *isp_fmt;

	sink_fmt = v4l2_subdev_state_get_format(state, format->pad);

	isp_fmt = core_find_format_by_code(format->format.code, format->pad);
	if (!isp_fmt)
		sink_fmt->code = C3_ISP_CORE_DEF_SINK_PAD_FMT;
	else
		sink_fmt->code = format->format.code;

	sink_fmt->width = clamp_t(u32, format->format.width,
				  C3_ISP_MIN_WIDTH, C3_ISP_MAX_WIDTH);
	sink_fmt->height = clamp_t(u32, format->format.height,
				   C3_ISP_MIN_HEIGHT, C3_ISP_MAX_HEIGHT);
	sink_fmt->field = V4L2_FIELD_NONE;
	sink_fmt->colorspace = V4L2_COLORSPACE_RAW;
	sink_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
	sink_fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
	sink_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;

	for (unsigned int i = C3_ISP_CORE_PAD_SOURCE_VIDEO_0;
	     i < C3_ISP_CORE_PAD_MAX; i++) {
		src_fmt = v4l2_subdev_state_get_format(state, i);

		src_fmt->width  = sink_fmt->width;
		src_fmt->height = sink_fmt->height;
	}

	format->format = *sink_fmt;
}

static void c3_isp_core_set_source_fmt(struct v4l2_subdev_state *state,
				       struct v4l2_subdev_format *format)
{
	const struct c3_isp_core_format_info *isp_fmt;
	struct v4l2_mbus_framefmt *src_fmt;
	struct v4l2_mbus_framefmt *sink_fmt;

	sink_fmt = v4l2_subdev_state_get_format(state,
						C3_ISP_CORE_PAD_SINK_VIDEO);
	src_fmt = v4l2_subdev_state_get_format(state, format->pad);

	isp_fmt = core_find_format_by_code(format->format.code, format->pad);
	if (!isp_fmt)
		src_fmt->code = C3_ISP_CORE_DEF_SRC_PAD_FMT;
	else
		src_fmt->code = format->format.code;

	src_fmt->width = sink_fmt->width;
	src_fmt->height = sink_fmt->height;
	src_fmt->field = V4L2_FIELD_NONE;
	src_fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;

	if (isp_fmt && isp_fmt->is_raw) {
		src_fmt->colorspace = V4L2_COLORSPACE_RAW;
		src_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
		src_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
	} else {
		src_fmt->colorspace = V4L2_COLORSPACE_SRGB;
		src_fmt->xfer_func = V4L2_XFER_FUNC_SRGB;
		src_fmt->quantization = V4L2_QUANTIZATION_LIM_RANGE;
	}

	format->format = *src_fmt;
}

static int c3_isp_core_set_fmt(struct v4l2_subdev *sd,
			       struct v4l2_subdev_state *state,
			       struct v4l2_subdev_format *format)
{
	if (format->pad == C3_ISP_CORE_PAD_SINK_VIDEO)
		c3_isp_core_set_sink_fmt(state, format);
	else if (format->pad == C3_ISP_CORE_PAD_SOURCE_VIDEO_0 ||
		 format->pad == C3_ISP_CORE_PAD_SOURCE_VIDEO_1 ||
		 format->pad == C3_ISP_CORE_PAD_SOURCE_VIDEO_2)
		c3_isp_core_set_source_fmt(state, format);
	else
		format->format =
			*v4l2_subdev_state_get_format(state, format->pad);

	return 0;
}

static int c3_isp_core_init_state(struct v4l2_subdev *sd,
				  struct v4l2_subdev_state *state)
{
	struct v4l2_mbus_framefmt *fmt;

	/* Video sink pad */
	fmt = v4l2_subdev_state_get_format(state, C3_ISP_CORE_PAD_SINK_VIDEO);
	fmt->width = C3_ISP_DEFAULT_WIDTH;
	fmt->height = C3_ISP_DEFAULT_HEIGHT;
	fmt->field = V4L2_FIELD_NONE;
	fmt->code = C3_ISP_CORE_DEF_SINK_PAD_FMT;
	fmt->colorspace = V4L2_COLORSPACE_RAW;
	fmt->xfer_func = V4L2_XFER_FUNC_NONE;
	fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
	fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;

	/* Video source pad */
	for (unsigned int i = C3_ISP_CORE_PAD_SOURCE_VIDEO_0;
	     i < C3_ISP_CORE_PAD_MAX; i++) {
		fmt = v4l2_subdev_state_get_format(state, i);
		fmt->width = C3_ISP_DEFAULT_WIDTH;
		fmt->height = C3_ISP_DEFAULT_HEIGHT;
		fmt->field = V4L2_FIELD_NONE;
		fmt->code = C3_ISP_CORE_DEF_SRC_PAD_FMT;
		fmt->colorspace = V4L2_COLORSPACE_SRGB;
		fmt->xfer_func = V4L2_XFER_FUNC_SRGB;
		fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
		fmt->quantization = V4L2_QUANTIZATION_LIM_RANGE;
	}

	/* Parameters pad */
	fmt = v4l2_subdev_state_get_format(state, C3_ISP_CORE_PAD_SINK_PARAMS);
	fmt->width = 0;
	fmt->height = 0;
	fmt->field = V4L2_FIELD_NONE;
	fmt->code = MEDIA_BUS_FMT_METADATA_FIXED;

	/* Statistics pad */
	fmt = v4l2_subdev_state_get_format(state, C3_ISP_CORE_PAD_SOURCE_STATS);
	fmt->width = 0;
	fmt->height = 0;
	fmt->field = V4L2_FIELD_NONE;
	fmt->code = MEDIA_BUS_FMT_METADATA_FIXED;

	return 0;
}

static int c3_isp_core_subscribe_event(struct v4l2_subdev *sd,
				       struct v4l2_fh *fh,
				       struct v4l2_event_subscription *sub)
{
	if (sub->type != V4L2_EVENT_FRAME_SYNC)
		return -EINVAL;

	/* V4L2_EVENT_FRAME_SYNC doesn't need id, so should set 0 */
	if (sub->id != 0)
		return -EINVAL;

	return v4l2_event_subscribe(fh, sub, 0, NULL);
}

static const struct v4l2_subdev_pad_ops c3_isp_core_pad_ops = {
	.enum_mbus_code = c3_isp_core_enum_mbus_code,
	.get_fmt = v4l2_subdev_get_fmt,
	.set_fmt = c3_isp_core_set_fmt,
	.enable_streams = c3_isp_core_enable_streams,
	.disable_streams = c3_isp_core_disable_streams,
};

static const struct v4l2_subdev_core_ops c3_isp_core_core_ops = {
	.subscribe_event = c3_isp_core_subscribe_event,
	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
};

static const struct v4l2_subdev_ops c3_isp_core_subdev_ops = {
	.core = &c3_isp_core_core_ops,
	.pad = &c3_isp_core_pad_ops,
};

static const struct v4l2_subdev_internal_ops c3_isp_core_internal_ops = {
	.init_state = c3_isp_core_init_state,
};

static int c3_isp_core_link_validate(struct media_link *link)
{
	if (link->sink->index == C3_ISP_CORE_PAD_SINK_PARAMS)
		return 0;

	return v4l2_subdev_link_validate(link);
}

/* Media entity operations */
static const struct media_entity_operations c3_isp_core_entity_ops = {
	.link_validate = c3_isp_core_link_validate,
};

void c3_isp_core_queue_sof(struct c3_isp_device *isp)
{
	struct v4l2_event event = {
		.type = V4L2_EVENT_FRAME_SYNC,
	};

	event.u.frame_sync.frame_sequence = isp->frm_sequence;
	v4l2_event_queue(isp->core.sd.devnode, &event);
}

int c3_isp_core_register(struct c3_isp_device *isp)
{
	struct c3_isp_core *core = &isp->core;
	struct v4l2_subdev *sd = &core->sd;
	int ret;

	v4l2_subdev_init(sd, &c3_isp_core_subdev_ops);
	sd->owner = THIS_MODULE;
	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
	sd->internal_ops = &c3_isp_core_internal_ops;
	snprintf(sd->name, sizeof(sd->name), "%s", C3_ISP_CORE_SUBDEV_NAME);

	sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
	sd->entity.ops = &c3_isp_core_entity_ops;

	core->isp = isp;
	sd->dev = isp->dev;
	v4l2_set_subdevdata(sd, core);

	core->pads[C3_ISP_CORE_PAD_SINK_VIDEO].flags = MEDIA_PAD_FL_SINK;
	core->pads[C3_ISP_CORE_PAD_SINK_PARAMS].flags = MEDIA_PAD_FL_SINK;
	core->pads[C3_ISP_CORE_PAD_SOURCE_STATS].flags = MEDIA_PAD_FL_SOURCE;
	core->pads[C3_ISP_CORE_PAD_SOURCE_VIDEO_0].flags = MEDIA_PAD_FL_SOURCE;
	core->pads[C3_ISP_CORE_PAD_SOURCE_VIDEO_1].flags = MEDIA_PAD_FL_SOURCE;
	core->pads[C3_ISP_CORE_PAD_SOURCE_VIDEO_2].flags = MEDIA_PAD_FL_SOURCE;
	ret = media_entity_pads_init(&sd->entity, C3_ISP_CORE_PAD_MAX,
				     core->pads);
	if (ret)
		return ret;

	ret = v4l2_subdev_init_finalize(sd);
	if (ret)
		goto err_entity_cleanup;

	ret = v4l2_device_register_subdev(&isp->v4l2_dev, sd);
	if (ret)
		goto err_subdev_cleanup;

	return 0;

err_subdev_cleanup:
	v4l2_subdev_cleanup(sd);
err_entity_cleanup:
	media_entity_cleanup(&sd->entity);
	return ret;
}

void c3_isp_core_unregister(struct c3_isp_device *isp)
{
	struct c3_isp_core *core = &isp->core;
	struct v4l2_subdev *sd = &core->sd;

	v4l2_device_unregister_subdev(sd);
	v4l2_subdev_cleanup(sd);
	media_entity_cleanup(&sd->entity);
}