diff --git a/drivers/phy/rockchip/Makefile b/drivers/phy/rockchip/Makefile index 062d39827180..c0a8d1e5876c 100644 --- a/drivers/phy/rockchip/Makefile +++ b/drivers/phy/rockchip/Makefile @@ -1,5 +1,6 @@ # SPDX-License-Identifier: GPL-2.0 -obj-$(CONFIG_PHY_ROCKCHIP_CSI2_DPHY) += phy-rockchip-csi2-dphy-hw.o +obj-$(CONFIG_PHY_ROCKCHIP_CSI2_DPHY) += phy-rockchip-csi2-dphy-hw.o \ + phy-rockchip-csi2-dphy.o obj-$(CONFIG_PHY_ROCKCHIP_DP) += phy-rockchip-dp.o obj-$(CONFIG_PHY_ROCKCHIP_EMMC) += phy-rockchip-emmc.o obj-$(CONFIG_PHY_ROCKCHIP_INNO_COMBPHY) += phy-rockchip-inno-combphy.o diff --git a/drivers/phy/rockchip/phy-rockchip-csi2-dphy-common.h b/drivers/phy/rockchip/phy-rockchip-csi2-dphy-common.h index 5d799baa0ba2..c7c886589338 100644 --- a/drivers/phy/rockchip/phy-rockchip-csi2-dphy-common.h +++ b/drivers/phy/rockchip/phy-rockchip-csi2-dphy-common.h @@ -97,4 +97,6 @@ struct csi2_dphy_hw { int (*stream_off)(struct csi2_dphy *dphy, struct v4l2_subdev *sd); }; +extern struct platform_driver rockchip_csi2_dphy_driver; + #endif diff --git a/drivers/phy/rockchip/phy-rockchip-csi2-dphy-hw.c b/drivers/phy/rockchip/phy-rockchip-csi2-dphy-hw.c index 6c5d4f24ee24..c4041abe8aba 100644 --- a/drivers/phy/rockchip/phy-rockchip-csi2-dphy-hw.c +++ b/drivers/phy/rockchip/phy-rockchip-csi2-dphy-hw.c @@ -619,6 +619,8 @@ static int rockchip_csi2_dphy_hw_probe(struct platform_device *pdev) pm_runtime_enable(&pdev->dev); + platform_driver_register(&rockchip_csi2_dphy_driver); + dev_info(dev, "csi2 dphy hw probe successfully!\n"); return 0; diff --git a/drivers/phy/rockchip/phy-rockchip-csi2-dphy.c b/drivers/phy/rockchip/phy-rockchip-csi2-dphy.c new file mode 100644 index 000000000000..33e777bbbaf0 --- /dev/null +++ b/drivers/phy/rockchip/phy-rockchip-csi2-dphy.c @@ -0,0 +1,681 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Rockchip MIPI CSI2 DPHY driver + * + * Copyright (C) 2021 Rockchip Electronics Co., Ltd. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "phy-rockchip-csi2-dphy-common.h" + +struct sensor_async_subdev { + struct v4l2_async_subdev asd; + struct v4l2_mbus_config mbus; + int lanes; +}; + +static DEFINE_MUTEX(csi2dphy_dev_mutex); +static LIST_HEAD(csi2dphy_device_list); + +static inline struct csi2_dphy *to_csi2_dphy(struct v4l2_subdev *subdev) +{ + return container_of(subdev, struct csi2_dphy, sd); +} + +static struct v4l2_subdev *get_remote_sensor(struct v4l2_subdev *sd) +{ + struct media_pad *local, *remote; + struct media_entity *sensor_me; + + local = &sd->entity.pads[CSI2_DPHY_RX_PAD_SINK]; + remote = media_entity_remote_pad(local); + if (!remote) { + v4l2_warn(sd, "No link between dphy and sensor\n"); + return NULL; + } + + sensor_me = media_entity_remote_pad(local)->entity; + return media_entity_to_v4l2_subdev(sensor_me); +} + +static struct csi2_sensor *sd_to_sensor(struct csi2_dphy *dphy, + struct v4l2_subdev *sd) +{ + int i; + + for (i = 0; i < dphy->num_sensors; ++i) + if (dphy->sensors[i].sd == sd) + return &dphy->sensors[i]; + + return NULL; +} + +static int csi2_dphy_get_sensor_data_rate(struct v4l2_subdev *sd) +{ + struct csi2_dphy *dphy = to_csi2_dphy(sd); + struct v4l2_subdev *sensor_sd = get_remote_sensor(sd); + struct v4l2_ctrl *link_freq; + struct v4l2_querymenu qm = { .id = V4L2_CID_LINK_FREQ, }; + int ret; + + link_freq = v4l2_ctrl_find(sensor_sd->ctrl_handler, V4L2_CID_LINK_FREQ); + if (!link_freq) { + v4l2_warn(sd, "No pixel rate control in subdev\n"); + return -EPIPE; + } + + qm.index = v4l2_ctrl_g_ctrl(link_freq); + ret = v4l2_querymenu(sensor_sd->ctrl_handler, &qm); + if (ret < 0) { + v4l2_err(sd, "Failed to get menu item\n"); + return ret; + } + + if (!qm.value) { + v4l2_err(sd, "Invalid link_freq\n"); + return -EINVAL; + } + dphy->data_rate_mbps = qm.value * 2; + do_div(dphy->data_rate_mbps, 1000 * 1000); + v4l2_info(sd, "dphy%d, data_rate_mbps %lld\n", + dphy->phy_index, dphy->data_rate_mbps); + return 0; +} + +static int csi2_dphy_update_sensor_mbus(struct v4l2_subdev *sd) +{ + struct csi2_dphy *dphy = to_csi2_dphy(sd); + struct v4l2_subdev *sensor_sd = get_remote_sensor(sd); + struct csi2_sensor *sensor = sd_to_sensor(dphy, sensor_sd); + struct v4l2_mbus_config mbus; + int ret; + + ret = v4l2_subdev_call(sensor_sd, video, g_mbus_config, &mbus); + if (ret) + return ret; + + sensor->mbus = mbus; + switch (mbus.flags & V4L2_MBUS_CSI2_LANES) { + case V4L2_MBUS_CSI2_1_LANE: + sensor->lanes = 1; + break; + case V4L2_MBUS_CSI2_2_LANE: + sensor->lanes = 2; + break; + case V4L2_MBUS_CSI2_3_LANE: + sensor->lanes = 3; + break; + case V4L2_MBUS_CSI2_4_LANE: + sensor->lanes = 4; + break; + default: + return -EINVAL; + } + + return 0; +} + +static int csi2_dphy_s_stream_start(struct v4l2_subdev *sd) +{ + struct csi2_dphy *dphy = to_csi2_dphy(sd); + struct csi2_dphy_hw *hw = dphy->dphy_hw; + int ret = 0; + + if (dphy->is_streaming) + return 0; + + ret = csi2_dphy_get_sensor_data_rate(sd); + if (ret < 0) + return ret; + + csi2_dphy_update_sensor_mbus(sd); + + if (hw->stream_on) + hw->stream_on(dphy, sd); + + dphy->is_streaming = true; + + return 0; +} + +static int csi2_dphy_s_stream_stop(struct v4l2_subdev *sd) +{ + struct csi2_dphy *dphy = to_csi2_dphy(sd); + struct csi2_dphy_hw *hw = dphy->dphy_hw; + + if (!dphy->is_streaming) + return 0; + + if (hw->stream_off) + hw->stream_off(dphy, sd); + + dphy->is_streaming = false; + + dev_info(dphy->dev, "%s stream stop, dphy%d\n", + __func__, dphy->phy_index); + + return 0; +} + +static int csi2_dphy_s_stream(struct v4l2_subdev *sd, int on) +{ + struct csi2_dphy *dphy = to_csi2_dphy(sd); + int ret = 0; + + mutex_lock(&dphy->mutex); + if (on) + ret = csi2_dphy_s_stream_start(sd); + else + ret = csi2_dphy_s_stream_stop(sd); + mutex_unlock(&dphy->mutex); + + dev_info(dphy->dev, "%s stream on:%d, dphy%d\n", + __func__, on, dphy->phy_index); + + return ret; +} + +static int csi2_dphy_g_frame_interval(struct v4l2_subdev *sd, + struct v4l2_subdev_frame_interval *fi) +{ + struct v4l2_subdev *sensor = get_remote_sensor(sd); + + if (sensor) + return v4l2_subdev_call(sensor, video, g_frame_interval, fi); + + return -EINVAL; +} + +static int csi2_dphy_g_mbus_config(struct v4l2_subdev *sd, + struct v4l2_mbus_config *config) +{ + struct csi2_dphy *dphy = to_csi2_dphy(sd); + struct v4l2_subdev *sensor_sd = get_remote_sensor(sd); + struct csi2_sensor *sensor; + + if (!sensor_sd) + return -ENODEV; + sensor = sd_to_sensor(dphy, sensor_sd); + csi2_dphy_update_sensor_mbus(sd); + *config = sensor->mbus; + + return 0; +} + +static int csi2_dphy_s_power(struct v4l2_subdev *sd, int on) +{ + struct csi2_dphy *dphy = to_csi2_dphy(sd); + + if (on) + return pm_runtime_get_sync(dphy->dev); + else + return pm_runtime_put(dphy->dev); +} + +static __maybe_unused int csi2_dphy_runtime_suspend(struct device *dev) +{ + struct media_entity *me = dev_get_drvdata(dev); + struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(me); + struct csi2_dphy *dphy = to_csi2_dphy(sd); + struct csi2_dphy_hw *hw = dphy->dphy_hw; + + if (hw) + clk_bulk_disable_unprepare(hw->num_clks, hw->clks); + + return 0; +} + +static __maybe_unused int csi2_dphy_runtime_resume(struct device *dev) +{ + struct media_entity *me = dev_get_drvdata(dev); + struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(me); + struct csi2_dphy *dphy = to_csi2_dphy(sd); + struct csi2_dphy_hw *hw = dphy->dphy_hw; + int ret; + + if (hw) { + ret = clk_bulk_prepare_enable(hw->num_clks, hw->clks); + if (ret) { + dev_err(hw->dev, "failed to enable clks\n"); + return ret; + } + } + + return 0; +} + +/* dphy accepts all fmt/size from sensor */ +static int csi2_dphy_get_set_fmt(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_format *fmt) +{ + struct csi2_dphy *dphy = to_csi2_dphy(sd); + struct v4l2_subdev *sensor_sd = get_remote_sensor(sd); + struct csi2_sensor *sensor = sd_to_sensor(dphy, sensor_sd); + int ret; + /* + * Do not allow format changes and just relay whatever + * set currently in the sensor. + */ + if (!sensor_sd) + return -ENODEV; + ret = v4l2_subdev_call(sensor_sd, pad, get_fmt, NULL, fmt); + if (!ret && fmt->pad == 0) + sensor->format = fmt->format; + return ret; +} + +static int csi2_dphy_get_selection(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_selection *sel) +{ + struct v4l2_subdev *sensor = get_remote_sensor(sd); + + return v4l2_subdev_call(sensor, pad, get_selection, NULL, sel); +} + +static const struct v4l2_subdev_core_ops csi2_dphy_core_ops = { + .s_power = csi2_dphy_s_power, +}; + +static const struct v4l2_subdev_video_ops csi2_dphy_video_ops = { + .g_frame_interval = csi2_dphy_g_frame_interval, + .g_mbus_config = csi2_dphy_g_mbus_config, + .s_stream = csi2_dphy_s_stream, +}; + +static const struct v4l2_subdev_pad_ops csi2_dphy_subdev_pad_ops = { + .set_fmt = csi2_dphy_get_set_fmt, + .get_fmt = csi2_dphy_get_set_fmt, + .get_selection = csi2_dphy_get_selection, +}; + +static const struct v4l2_subdev_ops csi2_dphy_subdev_ops = { + .core = &csi2_dphy_core_ops, + .video = &csi2_dphy_video_ops, + .pad = &csi2_dphy_subdev_pad_ops, +}; + +/* The .bound() notifier callback when a match is found */ +static int +rockchip_csi2_dphy_notifier_bound(struct v4l2_async_notifier *notifier, + struct v4l2_subdev *sd, + struct v4l2_async_subdev *asd) +{ + struct csi2_dphy *dphy = container_of(notifier, + struct csi2_dphy, + notifier); + struct sensor_async_subdev *s_asd = container_of(asd, + struct sensor_async_subdev, asd); + struct csi2_sensor *sensor; + unsigned int pad, ret; + + if (dphy->num_sensors == ARRAY_SIZE(dphy->sensors)) + return -EBUSY; + + sensor = &dphy->sensors[dphy->num_sensors++]; + sensor->lanes = s_asd->lanes; + sensor->mbus = s_asd->mbus; + sensor->sd = sd; + + dev_info(dphy->dev, "dphy%d matches %s:bus type %d\n", + dphy->phy_index, sd->name, s_asd->mbus.type); + + for (pad = 0; pad < sensor->sd->entity.num_pads; pad++) + if (sensor->sd->entity.pads[pad].flags & MEDIA_PAD_FL_SOURCE) + break; + + if (pad == sensor->sd->entity.num_pads) { + dev_err(dphy->dev, + "failed to find src pad for %s\n", + sensor->sd->name); + + return -ENXIO; + } + + ret = media_create_pad_link( + &sensor->sd->entity, pad, + &dphy->sd.entity, CSI2_DPHY_RX_PAD_SINK, + dphy->num_sensors != 1 ? 0 : MEDIA_LNK_FL_ENABLED); + if (ret) { + dev_err(dphy->dev, + "failed to create link for %s\n", + sensor->sd->name); + return ret; + } + + return 0; +} + +/* The .unbind callback */ +static void +rockchip_csi2_dphy_notifier_unbind(struct v4l2_async_notifier *notifier, + struct v4l2_subdev *sd, + struct v4l2_async_subdev *asd) +{ + struct csi2_dphy *dphy = container_of(notifier, + struct csi2_dphy, + notifier); + struct csi2_sensor *sensor = sd_to_sensor(dphy, sd); + + sensor->sd = NULL; +} + +static const struct +v4l2_async_notifier_operations rockchip_csi2_dphy_async_ops = { + .bound = rockchip_csi2_dphy_notifier_bound, + .unbind = rockchip_csi2_dphy_notifier_unbind, +}; + +static int rockchip_csi2_dphy_fwnode_parse(struct device *dev, + struct v4l2_fwnode_endpoint *vep, + struct v4l2_async_subdev *asd) +{ + struct sensor_async_subdev *s_asd = + container_of(asd, struct sensor_async_subdev, asd); + struct v4l2_mbus_config *config = &s_asd->mbus; + + if (vep->base.port != 0) { + dev_err(dev, "The PHY has only port 0\n"); + return -EINVAL; + } + + if (vep->bus_type == V4L2_MBUS_CSI2) { + config->type = V4L2_MBUS_CSI2; + config->flags = vep->bus.mipi_csi2.flags; + s_asd->lanes = vep->bus.mipi_csi2.num_data_lanes; + } else { + dev_err(dev, "Only CSI2 type is currently supported\n"); + return -EINVAL; + } + + switch (s_asd->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; + default: + return -EINVAL; + } + + return 0; +} + +static int rockchip_csi2dphy_media_init(struct csi2_dphy *dphy) +{ + int ret; + + dphy->pads[CSI2_DPHY_RX_PAD_SOURCE].flags = + MEDIA_PAD_FL_SOURCE | MEDIA_PAD_FL_MUST_CONNECT; + dphy->pads[CSI2_DPHY_RX_PAD_SINK].flags = + MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT; + dphy->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE; + ret = media_entity_pads_init(&dphy->sd.entity, + CSI2_DPHY_RX_PADS_NUM, dphy->pads); + if (ret < 0) + return ret; + + ret = v4l2_async_notifier_parse_fwnode_endpoints_by_port( + dphy->dev, &dphy->notifier, + sizeof(struct sensor_async_subdev), 0, + rockchip_csi2_dphy_fwnode_parse); + if (ret < 0) + return ret; + + if (!dphy->notifier.num_subdevs) + return -ENODEV; /* no endpoint */ + + dphy->sd.subdev_notifier = &dphy->notifier; + dphy->notifier.ops = &rockchip_csi2_dphy_async_ops; + ret = v4l2_async_subdev_notifier_register(&dphy->sd, &dphy->notifier); + if (ret) { + dev_err(dphy->dev, + "failed to register async notifier : %d\n", ret); + v4l2_async_notifier_cleanup(&dphy->notifier); + return ret; + } + + return v4l2_async_register_subdev(&dphy->sd); +} + +static int rockchip_csi2_dphy_attach_hw(struct csi2_dphy *dphy) +{ + struct platform_device *plat_dev; + struct device *dev = dphy->dev; + struct csi2_dphy_hw *dphy_hw; + struct device_node *np; + enum csi2_dphy_lane_mode target_mode; + int i; + + if (dphy->phy_index == 0) + target_mode = LANE_MODE_FULL; + else + target_mode = LANE_MODE_SPLIT; + + np = of_parse_phandle(dev->of_node, "rockchip,hw", 0); + if (!np || !of_device_is_available(np)) { + dev_err(dphy->dev, + "failed to get dphy%d hw node\n", dphy->phy_index); + return -ENODEV; + } + + plat_dev = of_find_device_by_node(np); + of_node_put(np); + if (!plat_dev) { + dev_err(dphy->dev, + "failed to get dphy%d hw from node\n", + dphy->phy_index); + return -ENODEV; + } + + dphy_hw = platform_get_drvdata(plat_dev); + if (!dphy_hw) { + dev_err(dphy->dev, + "failed attach dphy%d hw\n", + dphy->phy_index); + return -EINVAL; + } + + if (dphy_hw->lane_mode == LANE_MODE_UNDEF) { + dphy_hw->lane_mode = target_mode; + } else { + struct csi2_dphy *phy = dphy_hw->dphy_dev[0]; + + for (i = 0; i < dphy_hw->dphy_dev_num; i++) { + if (dphy_hw->dphy_dev[i]->lane_mode == dphy_hw->lane_mode) { + phy = dphy_hw->dphy_dev[i]; + break; + } + } + + if (target_mode != dphy_hw->lane_mode) { + dev_err(dphy->dev, + "Err:csi2 dphy hw has been set as %s mode by phy%d, target mode is:%s\n", + dphy_hw->lane_mode == LANE_MODE_FULL ? "full" : "split", + phy->phy_index, + target_mode == LANE_MODE_FULL ? "full" : "split"); + return -ENODEV; + } + } + + dphy_hw->dphy_dev[dphy_hw->dphy_dev_num] = dphy; + dphy_hw->dphy_dev_num++; + dphy->dphy_hw = dphy_hw; + + return 0; +} + +static int rockchip_csi2_dphy_detach_hw(struct csi2_dphy *dphy) +{ + struct csi2_dphy_hw *dphy_hw = dphy->dphy_hw; + struct csi2_dphy *csi2_dphy = NULL; + int i; + + for (i = 0; i < dphy_hw->dphy_dev_num; i++) { + csi2_dphy = dphy_hw->dphy_dev[i]; + if (csi2_dphy && + csi2_dphy->phy_index == dphy->phy_index) { + dphy_hw->dphy_dev[i] = NULL; + dphy_hw->dphy_dev_num--; + break; + } + } + + return 0; +} + +static const struct of_device_id rockchip_csi2_dphy_match_id[] = { + { + .compatible = "rockchip,rk3568-csi2-dphy", + }, + {} +}; +MODULE_DEVICE_TABLE(of, rockchip_csi2_dphy_match_id); + +static int rockchip_csi2_dphy_probe(struct platform_device *pdev) +{ + struct device *dev = &pdev->dev; + const struct of_device_id *of_id; + struct csi2_dphy *csi2dphy; + struct v4l2_subdev *sd; + int ret; + + csi2dphy = devm_kzalloc(dev, sizeof(*csi2dphy), GFP_KERNEL); + if (!csi2dphy) + return -ENOMEM; + csi2dphy->dev = dev; + + of_id = of_match_device(rockchip_csi2_dphy_match_id, dev); + if (!of_id) + return -EINVAL; + + csi2dphy->phy_index = of_alias_get_id(dev->of_node, "csi2dphy"); + if (csi2dphy->phy_index < 0 || csi2dphy->phy_index > 2) + csi2dphy->phy_index = 0; + + ret = rockchip_csi2_dphy_attach_hw(csi2dphy); + if (ret) { + dev_err(dev, + "csi2 dphy hw can't be attached, register dphy%d failed!\n", + csi2dphy->phy_index); + return -ENODEV; + } + + sd = &csi2dphy->sd; + mutex_init(&csi2dphy->mutex); + v4l2_subdev_init(sd, &csi2_dphy_subdev_ops); + sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE; + snprintf(sd->name, sizeof(sd->name), + "rockchip-csi2-dphy%d", csi2dphy->phy_index); + sd->dev = dev; + + platform_set_drvdata(pdev, &sd->entity); + + ret = rockchip_csi2dphy_media_init(csi2dphy); + if (ret < 0) + goto detach_hw; + + pm_runtime_enable(&pdev->dev); + + dev_info(dev, "csi2 dphy%d probe successfully!\n", csi2dphy->phy_index); + + return 0; + +detach_hw: + mutex_destroy(&csi2dphy->mutex); + rockchip_csi2_dphy_detach_hw(csi2dphy); + + return 0; +} + +static int rockchip_csi2_dphy_remove(struct platform_device *pdev) +{ + struct media_entity *me = platform_get_drvdata(pdev); + struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(me); + struct csi2_dphy *dphy = to_csi2_dphy(sd); + + media_entity_cleanup(&sd->entity); + + pm_runtime_disable(&pdev->dev); + mutex_destroy(&dphy->mutex); + return 0; +} +static int __maybe_unused __rockchip_csi2dphy_clr_unready_dev(void) +{ + struct csi2_dphy *csi2dphy; + + mutex_lock(&csi2dphy_dev_mutex); + list_for_each_entry(csi2dphy, &csi2dphy_device_list, list) + v4l2_async_notifier_clr_unready_dev(&csi2dphy->notifier); + mutex_unlock(&csi2dphy_dev_mutex); + + return 0; +} + +static int rockchip_csi2dphy_clr_unready_dev_param_set(const char *val, + const struct kernel_param *kp) +{ +#ifdef MODULE + __rockchip_csi2dphy_clr_unready_dev(); +#endif + + return 0; +} + +module_param_call(clr_unready_dev, + rockchip_csi2dphy_clr_unready_dev_param_set, + NULL, NULL, 0200); +MODULE_PARM_DESC(clr_unready_dev, "clear unready devices"); + +#ifndef MODULE +static int __init rockchip_csi2_dphy_clr_unready_dev(void) +{ + __rockchip_csi2dphy_clr_unready_dev(); + + return 0; +} +late_initcall_sync(rockchip_csi2_dphy_clr_unready_dev); +#endif + +static const struct dev_pm_ops rockchip_csi2_dphy_pm_ops = { + SET_RUNTIME_PM_OPS(csi2_dphy_runtime_suspend, + csi2_dphy_runtime_resume, NULL) +}; + +struct platform_driver rockchip_csi2_dphy_driver = { + .probe = rockchip_csi2_dphy_probe, + .remove = rockchip_csi2_dphy_remove, + .driver = { + .name = "rockchip-csi2-dphy", + .pm = &rockchip_csi2_dphy_pm_ops, + .of_match_table = rockchip_csi2_dphy_match_id, + }, +}; +EXPORT_SYMBOL(rockchip_csi2_dphy_driver); + +MODULE_AUTHOR("Rockchip Camera/ISP team"); +MODULE_DESCRIPTION("Rockchip MIPI CSI2 DPHY driver"); +MODULE_LICENSE("GPL v2");