---

static int mxc_isi_crossbar_enum_mbus_code(struct v4l2_subdev *sd,
					   struct v4l2_subdev_state *state,
					   struct v4l2_subdev_mbus_code_enum *code)
{
	struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
	const struct mxc_isi_bus_format_info *info;

	if (code->pad >= xbar->num_sinks) {
		const struct v4l2_mbus_framefmt *format;

		/*
		 * The media bus code on source pads is identical to the
		 * connected sink pad.
		 */
		if (code->index > 0)
			return -EINVAL;

		format = v4l2_subdev_state_get_opposite_stream_format(state,
								      code->pad,
								      code->stream);
		if (!format)
			return -EINVAL;

		code->code = format->code;

		return 0;
	}

	info = mxc_isi_bus_format_by_index(code->index, MXC_ISI_PIPE_PAD_SINK);
	if (!info)
		return -EINVAL;

	code->code = info->mbus_code;

	return 0;
}

static int mxc_isi_crossbar_set_fmt(struct v4l2_subdev *sd,
				    struct v4l2_subdev_state *state,
				    struct v4l2_subdev_format *fmt)
{
	struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
	struct v4l2_mbus_framefmt *sink_fmt;
	struct v4l2_subdev_route *route;

	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE &&
	    media_pad_is_streaming(&xbar->pads[fmt->pad]))
    {   
        printk("%s(%d) Error EBUSY\n",__FUNCTION__,__LINE__);
		return -EBUSY;
       }

	/*
	 * The source pad format is always identical to the sink pad format and
	 * can't be modified.
	 */
	if (fmt->pad >= xbar->num_sinks)
		return v4l2_subdev_get_fmt(sd, state, fmt);

	/* Validate the requested format. */
	if (!mxc_isi_bus_format_by_code(fmt->format.code, MXC_ISI_PIPE_PAD_SINK))
		fmt->format.code = MXC_ISI_DEF_MBUS_CODE_SINK;

	fmt->format.width = clamp_t(unsigned int, fmt->format.width,
				    MXC_ISI_MIN_WIDTH, MXC_ISI_MAX_WIDTH_CHAINED);
	fmt->format.height = clamp_t(unsigned int, fmt->format.height,
				     MXC_ISI_MIN_HEIGHT, MXC_ISI_MAX_HEIGHT);
	fmt->format.field = V4L2_FIELD_NONE;

	/*
	 * Set the format on the sink stream and propagate it to the source
	 * streams.
	 */
	sink_fmt = v4l2_subdev_state_get_format(state, fmt->pad, fmt->stream);
	if (!sink_fmt)
		return -EINVAL;

	*sink_fmt = fmt->format;

	/* TODO: A format propagation helper would be useful. */
	for_each_active_route(&state->routing, route) {
		struct v4l2_mbus_framefmt *source_fmt;

		if (route->sink_pad != fmt->pad ||
		    route->sink_stream != fmt->stream)
			continue;

		source_fmt = v4l2_subdev_state_get_format(state,
							  route->source_pad,
							  route->source_stream);
		if (!source_fmt)
			return -EINVAL;

		*source_fmt = fmt->format;
	}

	return 0;
}

static int mxc_isi_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
				  struct v4l2_mbus_frame_desc *fd)
{
	struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
	struct device *dev = xbar->isi->dev;
	struct v4l2_subdev_route *route;
	struct v4l2_subdev_state *state;
	int ret;

	if (pad < xbar->num_sinks)
		return -EINVAL;

	memset(fd, 0, sizeof(*fd));

	fd->type = V4L2_MBUS_FRAME_DESC_TYPE_CSI2;

	state = v4l2_subdev_lock_and_get_active_state(sd);

	for_each_active_route(&state->routing, route) {
		struct v4l2_mbus_frame_desc_entry *source_entry = NULL;
		struct v4l2_mbus_frame_desc source_fd;
		struct v4l2_subdev *remote_sd = NULL;
		struct media_pad *remote_pad;
		unsigned int i;

		if (route->source_pad != pad)
			continue;

		remote_pad = media_pad_remote_pad_first(&xbar->pads[route->sink_pad]);
		if (remote_pad)
			remote_sd = media_entity_to_v4l2_subdev(remote_pad->entity);
		if (!remote_sd) {
			dev_err(dev, "no entity connected to crossbar input %u\n",
				route->sink_pad);
			ret = -EPIPE;
			goto out_unlock;
		}

		/* Don't call get_frame_desc if the remote_sd is not a CSI bridge */
		if (remote_sd->entity.function == MEDIA_ENT_F_IO_DTV)
			continue;
		ret = v4l2_subdev_call(remote_sd, pad, get_frame_desc,
				       remote_pad->index, &source_fd);
		if (ret < 0) {
			dev_err(dev, "Failed to get source frame desc from pad %u\n",
				route->sink_pad);
			goto out_unlock;
		}

		for (i = 0; i < source_fd.num_entries; i++) {
			if (source_fd.entry[i].stream == route->sink_stream) {
				source_entry = &source_fd.entry[i];
				break;
			}
		}

		if (!source_entry) {
			dev_err(dev, "Failed to find stream from source frames desc, pad %u\n",
				route->sink_pad);
			ret = -EPIPE;
			goto out_unlock;
		}

		fd->entry[fd->num_entries].stream = route->source_stream;
		fd->entry[fd->num_entries].flags = source_entry->flags;
		fd->entry[fd->num_entries].length = source_entry->length;
		fd->entry[fd->num_entries].pixelcode = source_entry->pixelcode;
		fd->entry[fd->num_entries].bus.csi2.vc = source_entry->bus.csi2.vc;
		fd->entry[fd->num_entries].bus.csi2.dt = source_entry->bus.csi2.dt;

		fd->num_entries++;
	}

out_unlock:
	v4l2_subdev_unlock_state(state);
	return ret;
}

static int mxc_isi_crossbar_set_routing(struct v4l2_subdev *sd,
					struct v4l2_subdev_state *state,
					enum v4l2_subdev_format_whence which,
					struct v4l2_subdev_krouting *routing)
{
	if (which == V4L2_SUBDEV_FORMAT_ACTIVE &&
	    media_entity_is_streaming(&sd->entity))
    {   
        printk("%s(%d) Error EBUSY\n",__FUNCTION__,__LINE__);
		return -EBUSY;
       }

	return __mxc_isi_crossbar_set_routing(sd, state, routing);
}

static int mxc_isi_crossbar_enable_streams(struct v4l2_subdev *sd,
					   struct v4l2_subdev_state *state,
					   u32 pad, u64 streams_mask)
{
	struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
	struct v4l2_subdev *remote_sd;
	struct mxc_isi_input *input;
	u64 sink_streams;
	u32 sink_pad;
	u32 remote_pad;
	u8 stream_index;
	int ret;

	remote_sd = mxc_isi_crossbar_xlate_streams(xbar, state, pad, streams_mask,
						   &sink_pad, &sink_streams,
						   &remote_pad);
	if (IS_ERR(remote_sd))
		return PTR_ERR(remote_sd);

	input = &xbar->inputs[sink_pad];

	/*
	 * TODO: Track per-stream enable counts to support multiplexed
	 * streams.
	 */
	if (!input->enabled_streams) {
		ret = mxc_isi_crossbar_gasket_enable(xbar, state, remote_sd,
						     remote_pad, sink_pad);
		if (ret)
			return ret;
	}

	stream_index = clamp_t(u8, ffs(sink_streams), 1, xbar->num_sources);

	/*
	 * For enabled streams, increase the stream counter and return
	 * directly to support ISI stream duplicated feature
	 */
	if (input->enabled_streams & sink_streams) {
		input->enabled_count[(stream_index - 1)]++;
		return 0;
	}

	ret = v4l2_subdev_enable_streams(remote_sd, remote_pad, sink_streams);
	if (ret < 0) {
		if (!input->enabled_streams)
			mxc_isi_crossbar_gasket_disable(xbar, sink_pad);
		return ret;
	}

	input->enabled_streams |= sink_streams;
	input->enabled_count[(stream_index - 1)]++;

	return 0;
}