drivers/media/i2c/adv748x/adv748x-csi2.c - linux - Git at Google

 // SPDX-License-Identifier: GPL-2.0+
 /*
  * Driver for Analog Devices ADV748X CSI-2 Transmitter
  *
  * Copyright (C) 2017 Renesas Electronics Corp.
  */

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

 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>

 #include "adv748x.h"

 int adv748x_csi2_set_virtual_channel(struct adv748x_csi2 *tx, unsigned int vc)
 {
 	return tx_write(tx, ADV748X_CSI_VC_REF, vc << ADV748X_CSI_VC_REF_SHIFT);
 }

 /**
  * adv748x_csi2_register_link : Register and link internal entities
  *
  * @tx: CSI2 private entity
  * @v4l2_dev: Video registration device
  * @src: Source subdevice to establish link
  * @src_pad: Pad number of source to link to this @tx
  * @enable: Link enabled flag
  *
  * Ensure that the subdevice is registered against the v4l2_device, and link the
  * source pad to the sink pad of the CSI2 bus entity.
  */
 static int adv748x_csi2_register_link(struct adv748x_csi2 *tx,
 				      struct v4l2_device *v4l2_dev,
 				      struct v4l2_subdev *src,
 				      unsigned int src_pad,
 				      bool enable)
 {
 	int ret;

 	if (!src->v4l2_dev) {
 		ret = v4l2_device_register_subdev(v4l2_dev, src);
 		if (ret)
 			return ret;
 	}

 	ret = media_create_pad_link(&src->entity, src_pad,
 				    &tx->sd.entity, ADV748X_CSI2_SINK,
 				    enable ? MEDIA_LNK_FL_ENABLED : 0);
 	if (ret)
 		return ret;

 	if (enable)
 		tx->src = src;

 	return 0;
 }

 /* -----------------------------------------------------------------------------
  * v4l2_subdev_internal_ops
  *
  * We use the internal registered operation to be able to ensure that our
  * incremental subdevices (not connected in the forward path) can be registered
  * against the resulting video path and media device.
  */

 static int adv748x_csi2_registered(struct v4l2_subdev *sd)
 {
 	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
 	struct adv748x_state *state = tx->state;
 	int ret;

 	adv_dbg(state, "Registered %s (%s)", is_txa(tx) ? "TXA":"TXB",
 			sd->name);

 	/*
 	 * Link TXA to AFE and HDMI, and TXB to AFE only as TXB cannot output
 	 * HDMI.
 	 *
 	 * The HDMI->TXA link is enabled by default, as is the AFE->TXB one.
 	 */
 	if (is_afe_enabled(state)) {
 		ret = adv748x_csi2_register_link(tx, sd->v4l2_dev,
 						 &state->afe.sd,
 						 ADV748X_AFE_SOURCE,
 						 is_txb(tx));
 		if (ret)
 			return ret;

 		/* TXB can output AFE signals only. */
 		if (is_txb(tx))
 			state->afe.tx = tx;
 	}

 	/* Register link to HDMI for TXA only. */
 	if (is_txb(tx) || !is_hdmi_enabled(state))
 		return 0;

 	ret = adv748x_csi2_register_link(tx, sd->v4l2_dev, &state->hdmi.sd,
 					 ADV748X_HDMI_SOURCE, true);
 	if (ret)
 		return ret;

 	/* The default HDMI output is TXA. */
 	state->hdmi.tx = tx;

 	return 0;
 }

 static const struct v4l2_subdev_internal_ops adv748x_csi2_internal_ops = {
 	.registered = adv748x_csi2_registered,
 };

 /* -----------------------------------------------------------------------------
  * v4l2_subdev_video_ops
  */

 static int adv748x_csi2_s_stream(struct v4l2_subdev *sd, int enable)
 {
 	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
 	struct v4l2_subdev *src;

 	src = adv748x_get_remote_sd(&tx->pads[ADV748X_CSI2_SINK]);
 	if (!src)
 		return -EPIPE;

 	return v4l2_subdev_call(src, video, s_stream, enable);
 }

 static const struct v4l2_subdev_video_ops adv748x_csi2_video_ops = {
 	.s_stream = adv748x_csi2_s_stream,
 };

 /* -----------------------------------------------------------------------------
  * v4l2_subdev_pad_ops
  *
  * The CSI2 bus pads are ignorant to the data sizes or formats.
  * But we must support setting the pad formats for format propagation.
  */

 static struct v4l2_mbus_framefmt *
 adv748x_csi2_get_pad_format(struct v4l2_subdev *sd,
 			    struct v4l2_subdev_state *sd_state,
 			    unsigned int pad, u32 which)
 {
 	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);

 	if (which == V4L2_SUBDEV_FORMAT_TRY)
 		return v4l2_subdev_get_try_format(sd, sd_state, pad);

 	return &tx->format;
 }

 static int adv748x_csi2_get_format(struct v4l2_subdev *sd,
 				   struct v4l2_subdev_state *sd_state,
 				   struct v4l2_subdev_format *sdformat)
 {
 	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
 	struct adv748x_state *state = tx->state;
 	struct v4l2_mbus_framefmt *mbusformat;

 	mbusformat = adv748x_csi2_get_pad_format(sd, sd_state, sdformat->pad,
 						 sdformat->which);
 	if (!mbusformat)
 		return -EINVAL;

 	mutex_lock(&state->mutex);

 	sdformat->format = *mbusformat;

 	mutex_unlock(&state->mutex);

 	return 0;
 }

 static int adv748x_csi2_set_format(struct v4l2_subdev *sd,
 				   struct v4l2_subdev_state *sd_state,
 				   struct v4l2_subdev_format *sdformat)
 {
 	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
 	struct adv748x_state *state = tx->state;
 	struct v4l2_mbus_framefmt *mbusformat;
 	int ret = 0;

 	mbusformat = adv748x_csi2_get_pad_format(sd, sd_state, sdformat->pad,
 						 sdformat->which);
 	if (!mbusformat)
 		return -EINVAL;

 	mutex_lock(&state->mutex);

 	if (sdformat->pad == ADV748X_CSI2_SOURCE) {
 		const struct v4l2_mbus_framefmt *sink_fmt;

 		sink_fmt = adv748x_csi2_get_pad_format(sd, sd_state,
 						       ADV748X_CSI2_SINK,
 						       sdformat->which);

 		if (!sink_fmt) {
 			ret = -EINVAL;
 			goto unlock;
 		}

 		sdformat->format = *sink_fmt;
 	}

 	*mbusformat = sdformat->format;

 unlock:
 	mutex_unlock(&state->mutex);

 	return ret;
 }

 static int adv748x_csi2_get_mbus_config(struct v4l2_subdev *sd, unsigned int pad,
 					struct v4l2_mbus_config *config)
 {
 	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);

 	if (pad != ADV748X_CSI2_SOURCE)
 		return -EINVAL;

 	config->type = V4L2_MBUS_CSI2_DPHY;
 	switch (tx->active_lanes) {
 	case 1:
 		config->flags = V4L2_MBUS_CSI2_1_LANE;
 		break;

 	case 2:
 		config->flags = V4L2_MBUS_CSI2_2_LANE;
 		break;

 	case 3:
 		config->flags = V4L2_MBUS_CSI2_3_LANE;
 		break;

 	case 4:
 		config->flags = V4L2_MBUS_CSI2_4_LANE;
 		break;
 	}

 	return 0;
 }

 static const struct v4l2_subdev_pad_ops adv748x_csi2_pad_ops = {
 	.get_fmt = adv748x_csi2_get_format,
 	.set_fmt = adv748x_csi2_set_format,
 	.get_mbus_config = adv748x_csi2_get_mbus_config,
 };

 /* -----------------------------------------------------------------------------
  * v4l2_subdev_ops
  */

 static const struct v4l2_subdev_ops adv748x_csi2_ops = {
 	.video = &adv748x_csi2_video_ops,
 	.pad = &adv748x_csi2_pad_ops,
 };

 /* -----------------------------------------------------------------------------
  * Subdev module and controls
  */

 int adv748x_csi2_set_pixelrate(struct v4l2_subdev *sd, s64 rate)
 {
 	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);

 	if (!tx->pixel_rate)
 		return -EINVAL;

 	return v4l2_ctrl_s_ctrl_int64(tx->pixel_rate, rate);
 }

 static int adv748x_csi2_s_ctrl(struct v4l2_ctrl *ctrl)
 {
 	switch (ctrl->id) {
 	case V4L2_CID_PIXEL_RATE:
 		return 0;
 	default:
 		return -EINVAL;
 	}
 }

 static const struct v4l2_ctrl_ops adv748x_csi2_ctrl_ops = {
 	.s_ctrl = adv748x_csi2_s_ctrl,
 };

 static int adv748x_csi2_init_controls(struct adv748x_csi2 *tx)
 {

 	v4l2_ctrl_handler_init(&tx->ctrl_hdl, 1);

 	tx->pixel_rate = v4l2_ctrl_new_std(&tx->ctrl_hdl,
 					   &adv748x_csi2_ctrl_ops,
 					   V4L2_CID_PIXEL_RATE, 1, INT_MAX,
 					   1, 1);

 	tx->sd.ctrl_handler = &tx->ctrl_hdl;
 	if (tx->ctrl_hdl.error) {
 		v4l2_ctrl_handler_free(&tx->ctrl_hdl);
 		return tx->ctrl_hdl.error;
 	}

 	return v4l2_ctrl_handler_setup(&tx->ctrl_hdl);
 }

 int adv748x_csi2_init(struct adv748x_state *state, struct adv748x_csi2 *tx)
 {
 	int ret;

 	if (!is_tx_enabled(tx))
 		return 0;

 	adv748x_subdev_init(&tx->sd, state, &adv748x_csi2_ops,
 			    MEDIA_ENT_F_VID_IF_BRIDGE,
 			    is_txa(tx) ? "txa" : "txb");

 	/* Ensure that matching is based upon the endpoint fwnodes */
 	tx->sd.fwnode = of_fwnode_handle(state->endpoints[tx->port]);

 	/* Register internal ops for incremental subdev registration */
 	tx->sd.internal_ops = &adv748x_csi2_internal_ops;

 	tx->pads[ADV748X_CSI2_SINK].flags = MEDIA_PAD_FL_SINK;
 	tx->pads[ADV748X_CSI2_SOURCE].flags = MEDIA_PAD_FL_SOURCE;

 	ret = media_entity_pads_init(&tx->sd.entity, ADV748X_CSI2_NR_PADS,
 				     tx->pads);
 	if (ret)
 		return ret;

 	ret = adv748x_csi2_init_controls(tx);
 	if (ret)
 		goto err_free_media;

 	ret = v4l2_async_register_subdev(&tx->sd);
 	if (ret)
 		goto err_free_ctrl;

 	return 0;

 err_free_ctrl:
 	v4l2_ctrl_handler_free(&tx->ctrl_hdl);
 err_free_media:
 	media_entity_cleanup(&tx->sd.entity);

 	return ret;
 }

 void adv748x_csi2_cleanup(struct adv748x_csi2 *tx)
 {
 	if (!is_tx_enabled(tx))
 		return;

 	v4l2_async_unregister_subdev(&tx->sd);
 	media_entity_cleanup(&tx->sd.entity);
 	v4l2_ctrl_handler_free(&tx->ctrl_hdl);
 }
	// SPDX-License-Identifier: GPL-2.0+
	/*
	* Driver for Analog Devices ADV748X CSI-2 Transmitter
	*
	* Copyright (C) 2017 Renesas Electronics Corp.
	*/

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

	#include <media/v4l2-ctrls.h>
	#include <media/v4l2-device.h>
	#include <media/v4l2-ioctl.h>

	#include "adv748x.h"

	int adv748x_csi2_set_virtual_channel(struct adv748x_csi2 *tx, unsigned int vc)
	{
	return tx_write(tx, ADV748X_CSI_VC_REF, vc << ADV748X_CSI_VC_REF_SHIFT);
	}

	/**
	* adv748x_csi2_register_link : Register and link internal entities
	*
	* @tx: CSI2 private entity
	* @v4l2_dev: Video registration device
	* @src: Source subdevice to establish link
	* @src_pad: Pad number of source to link to this @tx
	* @enable: Link enabled flag
	*
	* Ensure that the subdevice is registered against the v4l2_device, and link the
	* source pad to the sink pad of the CSI2 bus entity.
	*/
	static int adv748x_csi2_register_link(struct adv748x_csi2 *tx,
	struct v4l2_device *v4l2_dev,
	struct v4l2_subdev *src,
	unsigned int src_pad,
	bool enable)
	{
	int ret;

	if (!src->v4l2_dev) {
	ret = v4l2_device_register_subdev(v4l2_dev, src);
	if (ret)
	return ret;
	}

	ret = media_create_pad_link(&src->entity, src_pad,
	&tx->sd.entity, ADV748X_CSI2_SINK,
	enable ? MEDIA_LNK_FL_ENABLED : 0);
	if (ret)
	return ret;

	if (enable)
	tx->src = src;

	return 0;
	}

	/* -----------------------------------------------------------------------------
	* v4l2_subdev_internal_ops
	*
	* We use the internal registered operation to be able to ensure that our
	* incremental subdevices (not connected in the forward path) can be registered
	* against the resulting video path and media device.
	*/

	static int adv748x_csi2_registered(struct v4l2_subdev *sd)
	{
	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
	struct adv748x_state *state = tx->state;
	int ret;

	adv_dbg(state, "Registered %s (%s)", is_txa(tx) ? "TXA":"TXB",
	sd->name);

	/*
	* Link TXA to AFE and HDMI, and TXB to AFE only as TXB cannot output
	* HDMI.
	*
	* The HDMI->TXA link is enabled by default, as is the AFE->TXB one.
	*/
	if (is_afe_enabled(state)) {
	ret = adv748x_csi2_register_link(tx, sd->v4l2_dev,
	&state->afe.sd,
	ADV748X_AFE_SOURCE,
	is_txb(tx));
	if (ret)
	return ret;

	/* TXB can output AFE signals only. */
	if (is_txb(tx))
	state->afe.tx = tx;
	}

	/* Register link to HDMI for TXA only. */
	if (is_txb(tx) \|\| !is_hdmi_enabled(state))
	return 0;

	ret = adv748x_csi2_register_link(tx, sd->v4l2_dev, &state->hdmi.sd,
	ADV748X_HDMI_SOURCE, true);
	if (ret)
	return ret;

	/* The default HDMI output is TXA. */
	state->hdmi.tx = tx;

	return 0;
	}

	static const struct v4l2_subdev_internal_ops adv748x_csi2_internal_ops = {
	.registered = adv748x_csi2_registered,
	};

	/* -----------------------------------------------------------------------------
	* v4l2_subdev_video_ops
	*/

	static int adv748x_csi2_s_stream(struct v4l2_subdev *sd, int enable)
	{
	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
	struct v4l2_subdev *src;

	src = adv748x_get_remote_sd(&tx->pads[ADV748X_CSI2_SINK]);
	if (!src)
	return -EPIPE;

	return v4l2_subdev_call(src, video, s_stream, enable);
	}

	static const struct v4l2_subdev_video_ops adv748x_csi2_video_ops = {
	.s_stream = adv748x_csi2_s_stream,
	};

	/* -----------------------------------------------------------------------------
	* v4l2_subdev_pad_ops
	*
	* The CSI2 bus pads are ignorant to the data sizes or formats.
	* But we must support setting the pad formats for format propagation.
	*/

	static struct v4l2_mbus_framefmt *
	adv748x_csi2_get_pad_format(struct v4l2_subdev *sd,
	struct v4l2_subdev_state *sd_state,
	unsigned int pad, u32 which)
	{
	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);

	if (which == V4L2_SUBDEV_FORMAT_TRY)
	return v4l2_subdev_get_try_format(sd, sd_state, pad);

	return &tx->format;
	}

	static int adv748x_csi2_get_format(struct v4l2_subdev *sd,
	struct v4l2_subdev_state *sd_state,
	struct v4l2_subdev_format *sdformat)
	{
	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
	struct adv748x_state *state = tx->state;
	struct v4l2_mbus_framefmt *mbusformat;

	mbusformat = adv748x_csi2_get_pad_format(sd, sd_state, sdformat->pad,
	sdformat->which);
	if (!mbusformat)
	return -EINVAL;

	mutex_lock(&state->mutex);

	sdformat->format = *mbusformat;

	mutex_unlock(&state->mutex);

	return 0;
	}

	static int adv748x_csi2_set_format(struct v4l2_subdev *sd,
	struct v4l2_subdev_state *sd_state,
	struct v4l2_subdev_format *sdformat)
	{
	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
	struct adv748x_state *state = tx->state;
	struct v4l2_mbus_framefmt *mbusformat;
	int ret = 0;

	mbusformat = adv748x_csi2_get_pad_format(sd, sd_state, sdformat->pad,
	sdformat->which);
	if (!mbusformat)
	return -EINVAL;

	mutex_lock(&state->mutex);

	if (sdformat->pad == ADV748X_CSI2_SOURCE) {
	const struct v4l2_mbus_framefmt *sink_fmt;

	sink_fmt = adv748x_csi2_get_pad_format(sd, sd_state,
	ADV748X_CSI2_SINK,
	sdformat->which);

	if (!sink_fmt) {
	ret = -EINVAL;
	goto unlock;
	}

	sdformat->format = *sink_fmt;
	}

	*mbusformat = sdformat->format;

	unlock:
	mutex_unlock(&state->mutex);

	return ret;
	}

	static int adv748x_csi2_get_mbus_config(struct v4l2_subdev *sd, unsigned int pad,
	struct v4l2_mbus_config *config)
	{
	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);

	if (pad != ADV748X_CSI2_SOURCE)
	return -EINVAL;

	config->type = V4L2_MBUS_CSI2_DPHY;
	switch (tx->active_lanes) {
	case 1:
	config->flags = V4L2_MBUS_CSI2_1_LANE;
	break;

	case 2:
	config->flags = V4L2_MBUS_CSI2_2_LANE;
	break;

	case 3:
	config->flags = V4L2_MBUS_CSI2_3_LANE;
	break;

	case 4:
	config->flags = V4L2_MBUS_CSI2_4_LANE;
	break;
	}

	return 0;
	}

	static const struct v4l2_subdev_pad_ops adv748x_csi2_pad_ops = {
	.get_fmt = adv748x_csi2_get_format,
	.set_fmt = adv748x_csi2_set_format,
	.get_mbus_config = adv748x_csi2_get_mbus_config,
	};

	/* -----------------------------------------------------------------------------
	* v4l2_subdev_ops
	*/

	static const struct v4l2_subdev_ops adv748x_csi2_ops = {
	.video = &adv748x_csi2_video_ops,
	.pad = &adv748x_csi2_pad_ops,
	};

	/* -----------------------------------------------------------------------------
	* Subdev module and controls
	*/

	int adv748x_csi2_set_pixelrate(struct v4l2_subdev *sd, s64 rate)
	{
	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);

	if (!tx->pixel_rate)
	return -EINVAL;

	return v4l2_ctrl_s_ctrl_int64(tx->pixel_rate, rate);
	}

	static int adv748x_csi2_s_ctrl(struct v4l2_ctrl *ctrl)
	{
	switch (ctrl->id) {
	case V4L2_CID_PIXEL_RATE:
	return 0;
	default:
	return -EINVAL;
	}
	}

	static const struct v4l2_ctrl_ops adv748x_csi2_ctrl_ops = {
	.s_ctrl = adv748x_csi2_s_ctrl,
	};

	static int adv748x_csi2_init_controls(struct adv748x_csi2 *tx)
	{

	v4l2_ctrl_handler_init(&tx->ctrl_hdl, 1);

	tx->pixel_rate = v4l2_ctrl_new_std(&tx->ctrl_hdl,
	&adv748x_csi2_ctrl_ops,
	V4L2_CID_PIXEL_RATE, 1, INT_MAX,
	1, 1);

	tx->sd.ctrl_handler = &tx->ctrl_hdl;
	if (tx->ctrl_hdl.error) {
	v4l2_ctrl_handler_free(&tx->ctrl_hdl);
	return tx->ctrl_hdl.error;
	}

	return v4l2_ctrl_handler_setup(&tx->ctrl_hdl);
	}

	int adv748x_csi2_init(struct adv748x_state state, struct adv748x_csi2 tx)
	{
	int ret;

	if (!is_tx_enabled(tx))
	return 0;

	adv748x_subdev_init(&tx->sd, state, &adv748x_csi2_ops,
	MEDIA_ENT_F_VID_IF_BRIDGE,
	is_txa(tx) ? "txa" : "txb");

	/* Ensure that matching is based upon the endpoint fwnodes */
	tx->sd.fwnode = of_fwnode_handle(state->endpoints[tx->port]);

	/* Register internal ops for incremental subdev registration */
	tx->sd.internal_ops = &adv748x_csi2_internal_ops;

	tx->pads[ADV748X_CSI2_SINK].flags = MEDIA_PAD_FL_SINK;
	tx->pads[ADV748X_CSI2_SOURCE].flags = MEDIA_PAD_FL_SOURCE;

	ret = media_entity_pads_init(&tx->sd.entity, ADV748X_CSI2_NR_PADS,
	tx->pads);
	if (ret)
	return ret;

	ret = adv748x_csi2_init_controls(tx);
	if (ret)
	goto err_free_media;

	ret = v4l2_async_register_subdev(&tx->sd);
	if (ret)
	goto err_free_ctrl;

	return 0;

	err_free_ctrl:
	v4l2_ctrl_handler_free(&tx->ctrl_hdl);
	err_free_media:
	media_entity_cleanup(&tx->sd.entity);

	return ret;
	}

	void adv748x_csi2_cleanup(struct adv748x_csi2 *tx)
	{
	if (!is_tx_enabled(tx))
	return;

	v4l2_async_unregister_subdev(&tx->sd);
	media_entity_cleanup(&tx->sd.entity);
	v4l2_ctrl_handler_free(&tx->ctrl_hdl);
	}