linux-zen-desktop/drivers/media/platform/nxp/imx8-isi/imx8-isi-crossbar.c

// SPDX-License-Identifier: GPL-2.0-only
/*
 * i.MX8 ISI - Input crossbar switch
 *
 * Copyright (c) 2022 Laurent Pinchart <laurent.pinchart@ideasonboard.com>
 */

#include <linux/device.h>
#include <linux/errno.h>
#include <linux/kernel.h>
#include <linux/minmax.h>
#include <linux/regmap.h>
#include <linux/slab.h>
#include <linux/string.h>
#include <linux/types.h>

#include <media/media-entity.h>
#include <media/mipi-csi2.h>
#include <media/v4l2-subdev.h>

#include "imx8-isi-core.h"

static inline struct mxc_isi_crossbar *to_isi_crossbar(struct v4l2_subdev *sd)
{
	return container_of(sd, struct mxc_isi_crossbar, sd);
}

/* -----------------------------------------------------------------------------
 * Media block control (i.MX8MN and i.MX8MP only)
 */
#define GASKET_BASE(n)				(0x0060 + (n) * 0x30)

#define GASKET_CTRL				0x0000
#define GASKET_CTRL_DATA_TYPE(dt)		((dt) << 8)
#define GASKET_CTRL_DATA_TYPE_MASK		(0x3f << 8)
#define GASKET_CTRL_DUAL_COMP_ENABLE		BIT(1)
#define GASKET_CTRL_ENABLE			BIT(0)

#define GASKET_HSIZE				0x0004
#define GASKET_VSIZE				0x0008

static int mxc_isi_crossbar_gasket_enable(struct mxc_isi_crossbar *xbar,
					  struct v4l2_subdev_state *state,
					  struct v4l2_subdev *remote_sd,
					  u32 remote_pad, unsigned int port)
{
	struct mxc_isi_dev *isi = xbar->isi;
	const struct v4l2_mbus_framefmt *fmt;
	struct v4l2_mbus_frame_desc fd;
	u32 val;
	int ret;

	if (!isi->pdata->has_gasket)
		return 0;

	/*
	 * Configure and enable the gasket with the frame size and CSI-2 data
	 * type. For YUV422 8-bit, enable dual component mode unconditionally,
	 * to match the configuration of the CSIS.
	 */

	ret = v4l2_subdev_call(remote_sd, pad, get_frame_desc, remote_pad, &fd);
	if (ret) {
		dev_err(isi->dev,
			"failed to get frame descriptor from '%s':%u: %d\n",
			remote_sd->name, remote_pad, ret);
		return ret;
	}

	if (fd.num_entries != 1) {
		dev_err(isi->dev, "invalid frame descriptor for '%s':%u\n",
			remote_sd->name, remote_pad);
		return -EINVAL;
	}

	fmt = v4l2_subdev_state_get_stream_format(state, port, 0);
	if (!fmt)
		return -EINVAL;

	regmap_write(isi->gasket, GASKET_BASE(port) + GASKET_HSIZE, fmt->width);
	regmap_write(isi->gasket, GASKET_BASE(port) + GASKET_VSIZE, fmt->height);

	val = GASKET_CTRL_DATA_TYPE(fd.entry[0].bus.csi2.dt)
	    | GASKET_CTRL_ENABLE;

	if (fd.entry[0].bus.csi2.dt == MIPI_CSI2_DT_YUV422_8B)
		val |= GASKET_CTRL_DUAL_COMP_ENABLE;

	regmap_write(isi->gasket, GASKET_BASE(port) + GASKET_CTRL, val);

	return 0;
}

static void mxc_isi_crossbar_gasket_disable(struct mxc_isi_crossbar *xbar,
					    unsigned int port)
{
	struct mxc_isi_dev *isi = xbar->isi;

	if (!isi->pdata->has_gasket)
		return;

	regmap_write(isi->gasket, GASKET_BASE(port) + GASKET_CTRL, 0);
}

/* -----------------------------------------------------------------------------
 * V4L2 subdev operations
 */

static const struct v4l2_mbus_framefmt mxc_isi_crossbar_default_format = {
	.code = MXC_ISI_DEF_MBUS_CODE_SINK,
	.width = MXC_ISI_DEF_WIDTH,
	.height = MXC_ISI_DEF_HEIGHT,
	.field = V4L2_FIELD_NONE,
	.colorspace = MXC_ISI_DEF_COLOR_SPACE,
	.ycbcr_enc = MXC_ISI_DEF_YCBCR_ENC,
	.quantization = MXC_ISI_DEF_QUANTIZATION,
	.xfer_func = MXC_ISI_DEF_XFER_FUNC,
};

static int __mxc_isi_crossbar_set_routing(struct v4l2_subdev *sd,
					  struct v4l2_subdev_state *state,
					  struct v4l2_subdev_krouting *routing)
{
	struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
	struct v4l2_subdev_route *route;
	int ret;

	ret = v4l2_subdev_routing_validate(sd, routing,
					   V4L2_SUBDEV_ROUTING_NO_N_TO_1);
	if (ret)
		return ret;

	/* The memory input can be routed to the first pipeline only. */
	for_each_active_route(&state->routing, route) {
		if (route->sink_pad == xbar->num_sinks - 1 &&
		    route->source_pad != xbar->num_sinks) {
			dev_dbg(xbar->isi->dev,
				"invalid route from memory input (%u) to pipe %u\n",
				route->sink_pad,
				route->source_pad - xbar->num_sinks);
			return -EINVAL;
		}
	}

	return v4l2_subdev_set_routing_with_fmt(sd, state, routing,
						&mxc_isi_crossbar_default_format);
}

static struct v4l2_subdev *
mxc_isi_crossbar_xlate_streams(struct mxc_isi_crossbar *xbar,
			       struct v4l2_subdev_state *state,
			       u32 source_pad, u64 source_streams,
			       u32 *__sink_pad, u64 *__sink_streams,
			       u32 *remote_pad)
{
	struct v4l2_subdev_route *route;
	struct v4l2_subdev *sd;
	struct media_pad *pad;
	u64 sink_streams = 0;
	int sink_pad = -1;

	/*
	 * Translate the source pad and streams to the sink side. The routing
	 * validation forbids stream merging, so all matching entries in the
	 * routing table are guaranteed to have the same sink pad.
	 *
	 * TODO: This is likely worth a helper function, it could perhaps be
	 * supported by v4l2_subdev_state_xlate_streams() with pad1 set to -1.
	 */
	for_each_active_route(&state->routing, route) {
		if (route->source_pad != source_pad ||
		    !(source_streams & BIT(route->source_stream)))
			continue;

		sink_streams |= BIT(route->sink_stream);
		sink_pad = route->sink_pad;
	}

	if (sink_pad < 0) {
		dev_dbg(xbar->isi->dev,
			"no stream connected to pipeline %u\n",
			source_pad - xbar->num_sinks);
		return ERR_PTR(-EPIPE);
	}

	pad = media_pad_remote_pad_first(&xbar->pads[sink_pad]);
	sd = media_entity_to_v4l2_subdev(pad->entity);

	if (!sd) {
		dev_dbg(xbar->isi->dev,
			"no entity connected to crossbar input %u\n",
			sink_pad);
		return ERR_PTR(-EPIPE);
	}

	*__sink_pad = sink_pad;
	*__sink_streams = sink_streams;
	*remote_pad = pad->index;

	return sd;
}

static int mxc_isi_crossbar_init_cfg(struct v4l2_subdev *sd,
				     struct v4l2_subdev_state *state)
{
	struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
	struct v4l2_subdev_krouting routing = { };
	struct v4l2_subdev_route *routes;
	unsigned int i;
	int ret;

	/*
	 * Create a 1:1 mapping between pixel link inputs and outputs to
	 * pipelines by default.
	 */
	routes = kcalloc(xbar->num_sources, sizeof(*routes), GFP_KERNEL);
	if (!routes)
		return -ENOMEM;

	for (i = 0; i < xbar->num_sources; ++i) {
		struct v4l2_subdev_route *route = &routes[i];

		route->sink_pad = i;
		route->source_pad = i + xbar->num_sinks;
		route->flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE;
	}

	routing.num_routes = xbar->num_sources;
	routing.routes = routes;

	ret = __mxc_isi_crossbar_set_routing(sd, state, &routing);

	kfree(routes);

	return ret;
}

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]))
		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_stream_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_stream_format(state, route->source_pad,
								 route->source_stream);
		if (!source_fmt)
			return -EINVAL;

		*source_fmt = fmt->format;
	}

	return 0;
}

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))
		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;
	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->enable_count) {
		ret = mxc_isi_crossbar_gasket_enable(xbar, state, remote_sd,
						     remote_pad, sink_pad);
		if (ret)
			return ret;

		ret = v4l2_subdev_enable_streams(remote_sd, remote_pad,
						 sink_streams);
		if (ret) {
			dev_err(xbar->isi->dev,
				"failed to %s streams 0x%llx on '%s':%u: %d\n",
				"enable", sink_streams, remote_sd->name,
				remote_pad, ret);
			mxc_isi_crossbar_gasket_disable(xbar, sink_pad);
			return ret;
		}
	}

	input->enable_count++;

	return 0;
}

static int mxc_isi_crossbar_disable_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;
	int ret = 0;

	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];

	input->enable_count--;

	if (!input->enable_count) {
		ret = v4l2_subdev_disable_streams(remote_sd, remote_pad,
						  sink_streams);
		if (ret)
			dev_err(xbar->isi->dev,
				"failed to %s streams 0x%llx on '%s':%u: %d\n",
				"disable", sink_streams, remote_sd->name,
				remote_pad, ret);

		mxc_isi_crossbar_gasket_disable(xbar, sink_pad);
	}

	return ret;
}

static const struct v4l2_subdev_pad_ops mxc_isi_crossbar_subdev_pad_ops = {
	.init_cfg = mxc_isi_crossbar_init_cfg,
	.enum_mbus_code = mxc_isi_crossbar_enum_mbus_code,
	.get_fmt = v4l2_subdev_get_fmt,
	.set_fmt = mxc_isi_crossbar_set_fmt,
	.set_routing = mxc_isi_crossbar_set_routing,
	.enable_streams = mxc_isi_crossbar_enable_streams,
	.disable_streams = mxc_isi_crossbar_disable_streams,
};

static const struct v4l2_subdev_ops mxc_isi_crossbar_subdev_ops = {
	.pad = &mxc_isi_crossbar_subdev_pad_ops,
};

static const struct media_entity_operations mxc_isi_cross_entity_ops = {
	.get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1,
	.link_validate	= v4l2_subdev_link_validate,
	.has_pad_interdep = v4l2_subdev_has_pad_interdep,
};

/* -----------------------------------------------------------------------------
 * Init & cleanup
 */

int mxc_isi_crossbar_init(struct mxc_isi_dev *isi)
{
	struct mxc_isi_crossbar *xbar = &isi->crossbar;
	struct v4l2_subdev *sd = &xbar->sd;
	unsigned int num_pads;
	unsigned int i;
	int ret;

	xbar->isi = isi;

	v4l2_subdev_init(sd, &mxc_isi_crossbar_subdev_ops);
	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_STREAMS;
	strscpy(sd->name, "crossbar", sizeof(sd->name));
	sd->dev = isi->dev;

	sd->entity.function = MEDIA_ENT_F_VID_MUX;
	sd->entity.ops = &mxc_isi_cross_entity_ops;

	/*
	 * The subdev has one sink and one source per port, plus one sink for
	 * the memory input.
	 */
	xbar->num_sinks = isi->pdata->num_ports + 1;
	xbar->num_sources = isi->pdata->num_ports;
	num_pads = xbar->num_sinks + xbar->num_sources;

	xbar->pads = kcalloc(num_pads, sizeof(*xbar->pads), GFP_KERNEL);
	if (!xbar->pads)
		return -ENOMEM;

	xbar->inputs = kcalloc(xbar->num_sinks, sizeof(*xbar->inputs),
			       GFP_KERNEL);
	if (!xbar->inputs) {
		ret = -ENOMEM;
		goto err_free;
	}

	for (i = 0; i < xbar->num_sinks; ++i)
		xbar->pads[i].flags = MEDIA_PAD_FL_SINK;
	for (i = 0; i < xbar->num_sources; ++i)
		xbar->pads[i + xbar->num_sinks].flags = MEDIA_PAD_FL_SOURCE;

	ret = media_entity_pads_init(&sd->entity, num_pads, xbar->pads);
	if (ret)
		goto err_free;

	ret = v4l2_subdev_init_finalize(sd);
	if (ret < 0)
		goto err_entity;

	return 0;

err_entity:
	media_entity_cleanup(&sd->entity);
err_free:
	kfree(xbar->pads);
	kfree(xbar->inputs);

	return ret;
}

void mxc_isi_crossbar_cleanup(struct mxc_isi_crossbar *xbar)
{
	media_entity_cleanup(&xbar->sd.entity);
	kfree(xbar->pads);
	kfree(xbar->inputs);
}

int mxc_isi_crossbar_register(struct mxc_isi_crossbar *xbar)
{
	return v4l2_device_register_subdev(&xbar->isi->v4l2_dev, &xbar->sd);
}

void mxc_isi_crossbar_unregister(struct mxc_isi_crossbar *xbar)
{
}