On Fri, Oct 23, 2020 at 07:45:40PM +0200, Paul Kocialkowski wrote:
> +static int sun6i_mipi_csi2_s_stream(struct v4l2_subdev *subdev, int on)
> +{
> + struct sun6i_mipi_csi2_video *video =
> + sun6i_mipi_csi2_subdev_video(subdev);
> + struct sun6i_mipi_csi2_dev *cdev = sun6i_mipi_csi2_video_dev(video);
> + struct v4l2_subdev *remote_subdev = video->remote_subdev;
> + struct v4l2_fwnode_bus_mipi_csi2 *bus_mipi_csi2 =
> + &video->endpoint.bus.mipi_csi2;
> + union phy_configure_opts dphy_opts = { 0 };
> + struct phy_configure_opts_mipi_dphy *dphy_cfg = &dphy_opts.mipi_dphy;
> + struct regmap *regmap = cdev->regmap;
> + struct v4l2_ctrl *ctrl;
> + unsigned int lanes_count;
> + unsigned int bpp;
> + unsigned long pixel_rate;
> + u8 data_type = 0;
> + u32 version = 0;
> + /* Initialize to 0 to use both in disable label (ret != 0) and off. */
> + int ret = 0;
> +
> + if (!remote_subdev)
> + return -ENODEV;
> +
> + if (!on) {
> + v4l2_subdev_call(remote_subdev, video, s_stream, 0);
> +
> +disable:
> + regmap_update_bits(regmap, SUN6I_MIPI_CSI2_CTL_REG,
> + SUN6I_MIPI_CSI2_CTL_EN, 0);
> +
> + phy_power_off(cdev->dphy);
> +
> + return ret;
This would be better as a separate function. (I hate backwards gotos
like this.)
> + }
> +
> + switch (video->mbus_code) {
> + case MEDIA_BUS_FMT_SBGGR8_1X8:
> + case MEDIA_BUS_FMT_SGBRG8_1X8:
> + case MEDIA_BUS_FMT_SGRBG8_1X8:
> + case MEDIA_BUS_FMT_SRGGB8_1X8:
> + data_type = MIPI_CSI2_DATA_TYPE_RAW8;
> + bpp = 8;
> + break;
> + case MEDIA_BUS_FMT_SBGGR10_1X10:
> + case MEDIA_BUS_FMT_SGBRG10_1X10:
> + case MEDIA_BUS_FMT_SGRBG10_1X10:
> + case MEDIA_BUS_FMT_SRGGB10_1X10:
> + data_type = MIPI_CSI2_DATA_TYPE_RAW10;
> + bpp = 10;
> + break;
> + default:
> + return -EINVAL;
> + }
> +
> + /* Sensor pixel rate */
> +
> + ctrl = v4l2_ctrl_find(remote_subdev->ctrl_handler, V4L2_CID_PIXEL_RATE);
> + if (!ctrl) {
> + dev_err(cdev->dev,
> + "%s: no MIPI CSI-2 pixel rate from the sensor\n",
> + __func__);
> + return -ENODEV;
> + }
> +
> + pixel_rate = (unsigned long)v4l2_ctrl_g_ctrl_int64(ctrl);
> + if (!pixel_rate) {
> + dev_err(cdev->dev,
> + "%s: zero MIPI CSI-2 pixel rate from the sensor\n",
> + __func__);
> + return -ENODEV;
> + }
> +
> + /* D-PHY configuration */
> +
> + lanes_count = bus_mipi_csi2->num_data_lanes;
> + phy_mipi_dphy_get_default_config(pixel_rate, bpp, lanes_count,
> + dphy_cfg);
> +
> +
> + /*
> + * Note that our hardware is using DDR, which is not taken in account by
> + * phy_mipi_dphy_get_default_config when calculating hs_clk_rate from
> + * the pixel rate, lanes count and bpp.
> + *
> + * The resulting clock rate is basically the symbol rate over the whole
> + * link. The actual clock rate is calculated with division by two since
> + * DDR samples both on rising and falling edges.
> + */
> +
> + dev_dbg(cdev->dev, "A31 MIPI CSI-2 config:\n");
> + dev_dbg(cdev->dev, "%ld pixels/s, %u bits/pixel, %lu Hz clock\n",
> + pixel_rate, bpp, dphy_cfg->hs_clk_rate / 2);
> +
> + ret = 0;
> + ret |= phy_reset(cdev->dphy);
> + ret |= phy_set_mode_ext(cdev->dphy, PHY_MODE_MIPI_DPHY,
> + PHY_MIPI_DPHY_SUBMODE_RX);
> + ret |= phy_configure(cdev->dphy, &dphy_opts);
> +
> + if (ret) {
> + dev_err(cdev->dev, "failed to setup MIPI D-PHY\n");
> + return ret;
> + }
> +
> + ret = phy_power_on(cdev->dphy);
> + if (ret) {
> + dev_err(cdev->dev, "failed to power on MIPI D-PHY\n");
> + return ret;
> + }
> +
> + /* MIPI CSI-2 controller setup */
> +
> + /*
> + * The enable flow in the Allwinner BSP is a bit different: the enable
> + * and reset bits are set together before starting the CSI controller.
> + *
> + * In mainline we enable the CSI controller first (due to subdev logic).
> + * One reliable way to make this work is to deassert reset, configure
> + * registers and enable the controller when everything's ready.
> + *
> + * However, reading the version appears necessary for it to work
> + * reliably. Replacing it with a delay doesn't do the trick.
> + */
> + regmap_write(regmap, SUN6I_MIPI_CSI2_CTL_REG,
> + SUN6I_MIPI_CSI2_CTL_RESET_N |
> + SUN6I_MIPI_CSI2_CTL_VERSION_EN |
> + SUN6I_MIPI_CSI2_CTL_UNPK_EN);
> +
> + regmap_read(regmap, SUN6I_MIPI_CSI2_VERSION_REG, &version);
> +
> + regmap_update_bits(regmap, SUN6I_MIPI_CSI2_CTL_REG,
> + SUN6I_MIPI_CSI2_CTL_VERSION_EN, 0);
> +
> + dev_dbg(cdev->dev, "A31 MIPI CSI-2 version: %04x\n", version);
> +
> + regmap_write(regmap, SUN6I_MIPI_CSI2_CFG_REG,
> + SUN6I_MIPI_CSI2_CFG_CHANNEL_MODE(1) |
> + SUN6I_MIPI_CSI2_CFG_LANE_COUNT(lanes_count));
> +
> + regmap_write(regmap, SUN6I_MIPI_CSI2_VCDT_RX_REG,
> + SUN6I_MIPI_CSI2_VCDT_RX_CH_VC(3, 3) |
> + SUN6I_MIPI_CSI2_VCDT_RX_CH_VC(2, 2) |
> + SUN6I_MIPI_CSI2_VCDT_RX_CH_VC(1, 1) |
> + SUN6I_MIPI_CSI2_VCDT_RX_CH_VC(0, 0) |
> + SUN6I_MIPI_CSI2_VCDT_RX_CH_DT(0, data_type));
> +
> + regmap_update_bits(regmap, SUN6I_MIPI_CSI2_CTL_REG,
> + SUN6I_MIPI_CSI2_CTL_EN, SUN6I_MIPI_CSI2_CTL_EN);
> +
> + ret = v4l2_subdev_call(remote_subdev, video, s_stream, 1);
> + if (ret)
> + goto disable;
> +
> + return 0;
> +}
> +
[ snip ]
> +static int sun6i_mipi_csi2_v4l2_setup(struct sun6i_mipi_csi2_dev *cdev)
> +{
> + struct sun6i_mipi_csi2_video *video = &cdev->video;
> + struct v4l2_subdev *subdev = &video->subdev;
> + struct v4l2_async_notifier *notifier = &video->notifier;
> + struct fwnode_handle *handle;
> + struct v4l2_fwnode_endpoint *endpoint;
> + int ret;
> +
> + /* Subdev */
> +
> + v4l2_subdev_init(subdev, &sun6i_mipi_csi2_subdev_ops);
> + subdev->dev = cdev->dev;
> + strscpy(subdev->name, MODULE_NAME, sizeof(subdev->name));
> + v4l2_set_subdevdata(subdev, cdev);
> +
> + /* Entity */
> +
> + subdev->entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
> + subdev->entity.ops = &sun6i_mipi_csi2_entity_ops;
> +
> + /* Pads */
> +
> + video->pads[0].flags = MEDIA_PAD_FL_SINK;
> + video->pads[1].flags = MEDIA_PAD_FL_SOURCE;
> +
> + ret = media_entity_pads_init(&subdev->entity, 2, video->pads);
> + if (ret)
> + return ret;
> +
> + /* Endpoint */
> +
> + handle = fwnode_graph_get_endpoint_by_id(dev_fwnode(cdev->dev), 0, 0,
> + FWNODE_GRAPH_ENDPOINT_NEXT);
> + if (!handle)
> + goto error_media_entity;
Missing error code.
> +
> + endpoint = &video->endpoint;
> + endpoint->bus_type = V4L2_MBUS_CSI2_DPHY;
> +
> + ret = v4l2_fwnode_endpoint_parse(handle, endpoint);
> + fwnode_handle_put(handle);
> + if (ret)
> + goto error_media_entity;
> +
> + /* Notifier */
> +
> + v4l2_async_notifier_init(notifier);
> +
> + ret = v4l2_async_notifier_add_fwnode_remote_subdev(notifier, handle,
> + &video->subdev_async);
> + if (ret)
> + goto error_media_entity;
> +
> + video->notifier.ops = &sun6i_mipi_csi2_notifier_ops;
> +
> + ret = v4l2_async_subdev_notifier_register(subdev, notifier);
> + if (ret < 0)
> + goto error_notifier;
> +
> + /* Subdev */
> +
> + ret = v4l2_async_register_subdev(subdev);
> + if (ret < 0)
> + goto error_notifier_registered;
> +
> + return 0;
> +
> +error_notifier_registered:
> + v4l2_async_notifier_unregister(notifier);
> +error_notifier:
> + v4l2_async_notifier_cleanup(notifier);
> +error_media_entity:
> + media_entity_cleanup(&subdev->entity);
> +
> + return ret;
> +}
regards,
dan carpenter
