From patchwork Fri Sep 22 10:08:23 2017 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: Maxime Ripard X-Patchwork-Id: 114016 Delivered-To: patch@linaro.org Received: by 10.140.106.117 with SMTP id d108csp3155826qgf; Fri, 22 Sep 2017 04:10:58 -0700 (PDT) X-Google-Smtp-Source: AOwi7QD7z8t7oSLQdOCiD/kYw3llOK4Lkzwti88RHK3KuiaZAoEDdl7WLENRZDCStjUrWvgUDc3O X-Received: by 10.84.143.165 with SMTP id 34mr8379334plz.91.1506078658873; Fri, 22 Sep 2017 04:10:58 -0700 (PDT) ARC-Seal: i=1; a=rsa-sha256; t=1506078658; cv=none; d=google.com; s=arc-20160816; b=xZMbv8LREd/2eLBqa7U3WKvT31y/ftrLipCe03GC1UFT3nqM8Jy8avZqKA0uJ/nsYd 21pARALyie6wtjVFX2paa2GwDhh7MruGz9sh3KTZlHkX6fpjdchyuAWixwHf8eeWQ9nu rEodw3zNMAxJ6ZeXcIP8w6EKvkXALR1uDxBR02C1URkT+nNyFFV9NsaKKUauOWWtpkPe alqd1JrUMdflbJ7/+P33ZwoAMC1Uo7dMLort2r/mMfLQwkKIQ0CvAR5Pk/ij8tre0X3Z rd+KbRUuCBk7RsY7Fxl2bURyCPQzt8S7q3JynpIgtR71HRAxzZ3FSgMPLdgSI0x+88gr USfQ== ARC-Message-Signature: i=1; a=rsa-sha256; c=relaxed/relaxed; d=google.com; s=arc-20160816; h=list-id:precedence:sender:references:in-reply-to:message-id:date :subject:cc:to:from:arc-authentication-results; bh=J6lS/AgYtD0R9HPwhB8zHf7+5ams0QxvUEb19cLC4Is=; b=Qcu2HaPxc57gcPuqAOwjq2Jm3zvftso+SLGb0sU4FHQdpAivt7PHNVqq2f8TaWaL6B 3Ne75zS99Xxow8Ad0pmDdCsZWpHkwAgHfe7LygOLGf5ppAfXiJmKOzdBXD5pf9VDmO2g 2hx2H+tpq+Ey+FRBDaXDIe3Jc+u+OPNMTUdIEsVb9aq7KsSyvZummYEH0dbqXMQm4QOP Lzyv5z/YBQ05HSpSpPEZRFNNbgsm00egJ0SoSZZ+TwLfLlfFhx6ANExowS5zsrcgR9dl p/IIrJ2hClctKpbU0XJieqkCH7AR/QCHrGCj1LVVpfJ7QuD87J0HrZZExNbFxVz9avBQ 1Oqg== ARC-Authentication-Results: i=1; mx.google.com; spf=pass (google.com: best guess record for domain of devicetree-owner@vger.kernel.org designates 209.132.180.67 as permitted sender) smtp.mailfrom=devicetree-owner@vger.kernel.org Return-Path: Received: from vger.kernel.org (vger.kernel.org. [209.132.180.67]) by mx.google.com with ESMTP id p125si2647931pfg.232.2017.09.22.04.10.58; Fri, 22 Sep 2017 04:10:58 -0700 (PDT) Received-SPF: pass (google.com: best guess record for domain of devicetree-owner@vger.kernel.org designates 209.132.180.67 as permitted sender) client-ip=209.132.180.67; Authentication-Results: mx.google.com; spf=pass (google.com: best guess record for domain of devicetree-owner@vger.kernel.org designates 209.132.180.67 as permitted sender) smtp.mailfrom=devicetree-owner@vger.kernel.org Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S1752105AbdIVLK5 (ORCPT + 6 others); Fri, 22 Sep 2017 07:10:57 -0400 Received: from mail.free-electrons.com ([62.4.15.54]:58831 "EHLO mail.free-electrons.com" rhost-flags-OK-OK-OK-OK) by vger.kernel.org with ESMTP id S1751977AbdIVLK4 (ORCPT ); Fri, 22 Sep 2017 07:10:56 -0400 Received: by mail.free-electrons.com (Postfix, from userid 110) id 6C1F2209EF; Fri, 22 Sep 2017 13:10:54 +0200 (CEST) X-Spam-Checker-Version: SpamAssassin 3.4.0 (2014-02-07) on mail.free-electrons.com X-Spam-Level: X-Spam-Status: No, score=-1.0 required=5.0 tests=ALL_TRUSTED,SHORTCIRCUIT shortcircuit=ham autolearn=disabled version=3.4.0 Received: from localhost (LStLambert-657-1-97-87.w90-63.abo.wanadoo.fr [90.63.216.87]) by mail.free-electrons.com (Postfix) with ESMTPSA id 3F7F9208EF; Fri, 22 Sep 2017 13:10:54 +0200 (CEST) From: Maxime Ripard To: Mauro Carvalho Chehab , Mark Rutland , Rob Herring Cc: Laurent Pinchart , linux-media@vger.kernel.org, devicetree@vger.kernel.org, Cyprian Wronka , Richard Sproul , Alan Douglas , Steve Creaney , Thomas Petazzoni , Boris Brezillon , =?utf-8?q?Niklas_S=C3=B6derlund?= , Hans Verkuil , Sakari Ailus , Benoit Parrot , nm@ti.com, Maxime Ripard Subject: [PATCH v4 2/2] v4l: cadence: Add Cadence MIPI-CSI2 RX driver Date: Fri, 22 Sep 2017 12:08:23 +0200 Message-Id: <20170922100823.18184-3-maxime.ripard@free-electrons.com> X-Mailer: git-send-email 2.13.5 In-Reply-To: <20170922100823.18184-1-maxime.ripard@free-electrons.com> References: <20170922100823.18184-1-maxime.ripard@free-electrons.com> Sender: devicetree-owner@vger.kernel.org Precedence: bulk List-ID: X-Mailing-List: devicetree@vger.kernel.org The Cadence CSI-2 RX Controller is an hardware block meant to be used as a bridge between a CSI-2 bus and pixel grabbers. It supports operating with internal or external D-PHY, with up to 4 lanes, or without any D-PHY. The current code only supports the former case. It also support dynamic mapping of the CSI-2 virtual channels to the associated pixel grabbers, but that isn't allowed at the moment either. Signed-off-by: Maxime Ripard --- drivers/media/platform/Kconfig | 1 + drivers/media/platform/Makefile | 2 + drivers/media/platform/cadence/Kconfig | 12 + drivers/media/platform/cadence/Makefile | 1 + drivers/media/platform/cadence/cdns-csi2rx.c | 486 +++++++++++++++++++++++++++ 5 files changed, 502 insertions(+) create mode 100644 drivers/media/platform/cadence/Kconfig create mode 100644 drivers/media/platform/cadence/Makefile create mode 100644 drivers/media/platform/cadence/cdns-csi2rx.c -- 2.13.5 -- To unsubscribe from this list: send the line "unsubscribe devicetree" in the body of a message to majordomo@vger.kernel.org More majordomo info at http://vger.kernel.org/majordomo-info.html diff --git a/drivers/media/platform/Kconfig b/drivers/media/platform/Kconfig index 7e7cc49b8674..adfced50504c 100644 --- a/drivers/media/platform/Kconfig +++ b/drivers/media/platform/Kconfig @@ -26,6 +26,7 @@ config VIDEO_VIA_CAMERA # # Platform multimedia device configuration # +source "drivers/media/platform/cadence/Kconfig" source "drivers/media/platform/davinci/Kconfig" diff --git a/drivers/media/platform/Makefile b/drivers/media/platform/Makefile index c1ef946bf032..44d39b7c7bf7 100644 --- a/drivers/media/platform/Makefile +++ b/drivers/media/platform/Makefile @@ -2,6 +2,8 @@ # Makefile for the video capture/playback device drivers. # +obj-$(CONFIG_VIDEO_CADENCE) += cadence/ + obj-$(CONFIG_VIDEO_M32R_AR_M64278) += arv.o obj-$(CONFIG_VIDEO_VIA_CAMERA) += via-camera.o diff --git a/drivers/media/platform/cadence/Kconfig b/drivers/media/platform/cadence/Kconfig new file mode 100644 index 000000000000..d1b6bbb6a0eb --- /dev/null +++ b/drivers/media/platform/cadence/Kconfig @@ -0,0 +1,12 @@ +config VIDEO_CADENCE + bool "Cadence Video Devices" + +if VIDEO_CADENCE + +config VIDEO_CADENCE_CSI2RX + tristate "Cadence MIPI-CSI2 RX Controller v1.3" + depends on MEDIA_CONTROLLER + depends on VIDEO_V4L2_SUBDEV_API + select V4L2_FWNODE + +endif diff --git a/drivers/media/platform/cadence/Makefile b/drivers/media/platform/cadence/Makefile new file mode 100644 index 000000000000..99a4086b7448 --- /dev/null +++ b/drivers/media/platform/cadence/Makefile @@ -0,0 +1 @@ +obj-$(CONFIG_VIDEO_CADENCE_CSI2RX) += cdns-csi2rx.o diff --git a/drivers/media/platform/cadence/cdns-csi2rx.c b/drivers/media/platform/cadence/cdns-csi2rx.c new file mode 100644 index 000000000000..0a35ad0b697f --- /dev/null +++ b/drivers/media/platform/cadence/cdns-csi2rx.c @@ -0,0 +1,486 @@ +/* + * Driver for Cadence MIPI-CSI2 RX Controller v1.3 + * + * Copyright (C) 2017 Cadence Design Systems Inc. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation; either version 2 of the License, or (at your + * option) any later version. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#define CSI2RX_DEVICE_CFG_REG 0x000 + +#define CSI2RX_SOFT_RESET_REG 0x004 +#define CSI2RX_SOFT_RESET_PROTOCOL BIT(1) +#define CSI2RX_SOFT_RESET_FRONT BIT(0) + +#define CSI2RX_STATIC_CFG_REG 0x008 +#define CSI2RX_STATIC_CFG_DLANE_MAP(llane, plane) ((plane) << (16 + (llane) * 4)) +#define CSI2RX_STATIC_CFG_LANES_MASK GENMASK(11, 8) + +#define CSI2RX_STREAM_BASE(n) (((n) + 1) * 0x100) + +#define CSI2RX_STREAM_CTRL_REG(n) (CSI2RX_STREAM_BASE(n) + 0x000) +#define CSI2RX_STREAM_CTRL_START BIT(0) + +#define CSI2RX_STREAM_DATA_CFG_REG(n) (CSI2RX_STREAM_BASE(n) + 0x008) +#define CSI2RX_STREAM_DATA_CFG_EN_VC_SELECT BIT(31) +#define CSI2RX_STREAM_DATA_CFG_VC_SELECT(n) BIT((n) + 16) + +#define CSI2RX_STREAM_CFG_REG(n) (CSI2RX_STREAM_BASE(n) + 0x00c) +#define CSI2RX_STREAM_CFG_FIFO_MODE_LARGE_BUF (1 << 8) + +#define CSI2RX_LANES_MAX 4 +#define CSI2RX_STREAMS_MAX 4 + +enum csi2rx_pads { + CSI2RX_PAD_SINK, + CSI2RX_PAD_SOURCE_STREAM0, + CSI2RX_PAD_SOURCE_STREAM1, + CSI2RX_PAD_SOURCE_STREAM2, + CSI2RX_PAD_SOURCE_STREAM3, + CSI2RX_PAD_MAX, +}; + +struct csi2rx_priv { + struct device *dev; + atomic_t count; + + void __iomem *base; + struct clk *sys_clk; + struct clk *p_clk; + struct clk *pixel_clk[CSI2RX_STREAMS_MAX]; + struct phy *dphy; + + u8 lanes[CSI2RX_LANES_MAX]; + u8 num_lanes; + u8 max_lanes; + u8 max_streams; + bool has_internal_dphy; + + struct v4l2_subdev subdev; + struct v4l2_async_notifier notifier; + struct media_pad pads[CSI2RX_PAD_MAX]; + + /* Remote source */ + struct v4l2_async_subdev asd; + struct v4l2_subdev *source_subdev; + int source_pad; +}; + +static inline +struct csi2rx_priv *v4l2_subdev_to_csi2rx(struct v4l2_subdev *subdev) +{ + return container_of(subdev, struct csi2rx_priv, subdev); +} + +static void csi2rx_reset(struct csi2rx_priv *csi2rx) +{ + writel(CSI2RX_SOFT_RESET_PROTOCOL | CSI2RX_SOFT_RESET_FRONT, + csi2rx->base + CSI2RX_SOFT_RESET_REG); + + usleep_range(10, 20); + + writel(0, csi2rx->base + CSI2RX_SOFT_RESET_REG); +} + +static int csi2rx_start(struct csi2rx_priv *csi2rx) +{ + unsigned int i; + u32 reg; + int ret; + + /* + * We're not the first users, there's no need to enable the + * whole controller. + */ + if (atomic_inc_return(&csi2rx->count) > 1) + return 0; + + clk_prepare_enable(csi2rx->p_clk); + + csi2rx_reset(csi2rx); + + reg = csi2rx->num_lanes << 8; + for (i = 0; i < csi2rx->num_lanes; i++) + reg |= CSI2RX_STATIC_CFG_DLANE_MAP(i, csi2rx->lanes[i]); + + writel(reg, csi2rx->base + CSI2RX_STATIC_CFG_REG); + + ret = v4l2_subdev_call(csi2rx->source_subdev, video, s_stream, true); + if (ret) + return ret; + + /* + * Create a static mapping between the CSI virtual channels + * and the output stream. + * + * This should be enhanced, but v4l2 lacks the support for + * changing that mapping dynamically. + * + * We also cannot enable and disable independant streams here, + * hence the reference counting. + */ + for (i = 0; i < csi2rx->max_streams; i++) { + clk_prepare_enable(csi2rx->pixel_clk[i]); + + writel(CSI2RX_STREAM_CFG_FIFO_MODE_LARGE_BUF, + csi2rx->base + CSI2RX_STREAM_CFG_REG(i)); + + writel(CSI2RX_STREAM_DATA_CFG_EN_VC_SELECT | + CSI2RX_STREAM_DATA_CFG_VC_SELECT(i), + csi2rx->base + CSI2RX_STREAM_DATA_CFG_REG(i)); + + writel(CSI2RX_STREAM_CTRL_START, + csi2rx->base + CSI2RX_STREAM_CTRL_REG(i)); + } + + clk_prepare_enable(csi2rx->sys_clk); + + clk_disable_unprepare(csi2rx->p_clk); + + return 0; +} + +static int csi2rx_stop(struct csi2rx_priv *csi2rx) +{ + unsigned int i; + + /* + * Let the last user turn off the lights + */ + if (!atomic_dec_and_test(&csi2rx->count)) + return 0; + + clk_prepare_enable(csi2rx->p_clk); + + for (i = 0; i < csi2rx->max_streams; i++) { + writel(0, csi2rx->base + CSI2RX_STREAM_CTRL_REG(i)); + + clk_disable_unprepare(csi2rx->pixel_clk[i]); + } + + clk_disable_unprepare(csi2rx->p_clk); + + return v4l2_subdev_call(csi2rx->source_subdev, video, s_stream, false); +} + +static int csi2rx_s_stream(struct v4l2_subdev *sd, int enable) +{ + struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(sd); + int ret; + + if (enable) + ret = csi2rx_start(csi2rx); + else + ret = csi2rx_stop(csi2rx); + + return ret; +} + +static const struct v4l2_subdev_video_ops csi2rx_video_ops = { + .s_stream = csi2rx_s_stream, +}; + +static const struct v4l2_subdev_ops csi2rx_subdev_ops = { + .video = &csi2rx_video_ops, +}; + +static int csi2rx_async_bound(struct v4l2_async_notifier *notifier, + struct v4l2_subdev *s_subdev, + struct v4l2_async_subdev *asd) +{ + struct v4l2_subdev *subdev = notifier->sd; + struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev); + + csi2rx->source_pad = media_entity_get_fwnode_pad(&s_subdev->entity, + s_subdev->fwnode, + MEDIA_PAD_FL_SOURCE); + if (csi2rx->source_pad < 0) { + dev_err(csi2rx->dev, "Couldn't find output pad for subdev %s\n", + s_subdev->name); + return csi2rx->source_pad; + } + + csi2rx->source_subdev = s_subdev; + + dev_dbg(csi2rx->dev, "Bound %s pad: %d\n", s_subdev->name, + csi2rx->source_pad); + + return 0; +} + +static int csi2rx_async_complete(struct v4l2_async_notifier *notifier) +{ + struct v4l2_subdev *subdev = notifier->sd; + struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev); + + return media_create_pad_link(&csi2rx->source_subdev->entity, + csi2rx->source_pad, + &csi2rx->subdev.entity, 0, + MEDIA_LNK_FL_ENABLED | + MEDIA_LNK_FL_IMMUTABLE); +} + +static void csi2rx_async_unbind(struct v4l2_async_notifier *notifier, + struct v4l2_subdev *s_subdev, + struct v4l2_async_subdev *asd) +{ + struct v4l2_subdev *subdev = notifier->sd; + struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev); + + dev_dbg(csi2rx->dev, "Unbound %s pad: %d\n", s_subdev->name, + csi2rx->source_pad); + + csi2rx->source_subdev = NULL; + csi2rx->source_pad = -EINVAL; +} + +static const struct v4l2_async_notifier_operations csi2rx_notifier_ops = { + .bound = csi2rx_async_bound, + .complete = csi2rx_async_complete, + .unbind = csi2rx_async_unbind, +}; + +static int csi2rx_get_resources(struct csi2rx_priv *csi2rx, + struct platform_device *pdev) +{ + struct resource *res; + unsigned char i; + u32 reg; + + res = platform_get_resource(pdev, IORESOURCE_MEM, 0); + csi2rx->base = devm_ioremap_resource(&pdev->dev, res); + if (IS_ERR(csi2rx->base)) + return PTR_ERR(csi2rx->base); + + csi2rx->sys_clk = devm_clk_get(&pdev->dev, "sys_clk"); + if (IS_ERR(csi2rx->sys_clk)) { + dev_err(&pdev->dev, "Couldn't get sys clock\n"); + return PTR_ERR(csi2rx->sys_clk); + } + + csi2rx->p_clk = devm_clk_get(&pdev->dev, "p_clk"); + if (IS_ERR(csi2rx->p_clk)) { + dev_err(&pdev->dev, "Couldn't get P clock\n"); + return PTR_ERR(csi2rx->p_clk); + } + + csi2rx->dphy = devm_phy_optional_get(&pdev->dev, "dphy"); + if (IS_ERR(csi2rx->dphy)) { + dev_err(&pdev->dev, "Couldn't get external D-PHY\n"); + return PTR_ERR(csi2rx->dphy); + } + + /* + * FIXME: Once we'll have external D-PHY support, the check + * will need to be removed. + */ + if (csi2rx->dphy) { + dev_err(&pdev->dev, "External D-PHY not supported yet\n"); + return -EINVAL; + } + + clk_prepare_enable(csi2rx->p_clk); + reg = readl(csi2rx->base + CSI2RX_DEVICE_CFG_REG); + clk_disable_unprepare(csi2rx->p_clk); + + csi2rx->max_lanes = (reg & 7); + if (csi2rx->max_lanes > CSI2RX_LANES_MAX) { + dev_err(&pdev->dev, "Invalid number of lanes: %u\n", + csi2rx->max_lanes); + return -EINVAL; + } + + csi2rx->max_streams = ((reg >> 4) & 7); + if (csi2rx->max_streams > CSI2RX_STREAMS_MAX) { + dev_err(&pdev->dev, "Invalid number of streams: %u\n", + csi2rx->max_streams); + return -EINVAL; + } + + csi2rx->has_internal_dphy = (reg & BIT(3)) ? true : false; + + /* + * FIXME: Once we'll have internal D-PHY support, the check + * will need to be removed. + */ + if (csi2rx->has_internal_dphy) { + dev_err(&pdev->dev, "Internal D-PHY not supported yet\n"); + return -EINVAL; + } + + for (i = 0; i < csi2rx->max_streams; i++) { + char clk_name[16]; + + snprintf(clk_name, sizeof(clk_name), "pixel_if%u_clk", i); + csi2rx->pixel_clk[i] = devm_clk_get(&pdev->dev, clk_name); + if (IS_ERR(csi2rx->pixel_clk[i])) { + dev_err(&pdev->dev, "Couldn't get clock %s\n", clk_name); + return PTR_ERR(csi2rx->pixel_clk[i]); + } + } + + return 0; +} + +static int csi2rx_parse_dt(struct csi2rx_priv *csi2rx) +{ + struct v4l2_fwnode_endpoint v4l2_ep; + struct device_node *ep, *remote; + int ret; + + ep = of_graph_get_endpoint_by_regs(csi2rx->dev->of_node, 0, 0); + if (!ep) + return -EINVAL; + + ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep), &v4l2_ep); + if (ret) { + dev_err(csi2rx->dev, "Could not parse v4l2 endpoint\n"); + goto out; + } + + if (v4l2_ep.bus_type != V4L2_MBUS_CSI2) { + dev_err(csi2rx->dev, "Unsupported media bus type: 0x%x\n", + v4l2_ep.bus_type); + ret = -EINVAL; + goto out; + } + + memcpy(csi2rx->lanes, v4l2_ep.bus.mipi_csi2.data_lanes, + sizeof(csi2rx->lanes)); + csi2rx->num_lanes = v4l2_ep.bus.mipi_csi2.num_data_lanes; + if (csi2rx->num_lanes > csi2rx->max_lanes) { + dev_err(csi2rx->dev, "Unsupported number of data-lanes: %d\n", + csi2rx->num_lanes); + ret = -EINVAL; + goto out; + } + +out: + of_node_put(ep); + return ret; +} + +static int csi2rx_probe(struct platform_device *pdev) +{ + struct csi2rx_priv *csi2rx; + unsigned int i; + int ret; + + /* + * Since the v4l2_subdev structure is embedded in our + * csi2rx_priv structure, and that the structure is exposed to + * the user-space, we cannot just use the devm_variant + * here. Indeed, that would lead to a use-after-free in a + * open() - unbind - close() pattern. + */ + csi2rx = kzalloc(sizeof(*csi2rx), GFP_KERNEL); + if (!csi2rx) + return -ENOMEM; + platform_set_drvdata(pdev, csi2rx); + csi2rx->dev = &pdev->dev; + + ret = csi2rx_get_resources(csi2rx, pdev); + if (ret) + goto err_free_priv; + + ret = csi2rx_parse_dt(csi2rx); + if (ret) + goto err_free_priv; + + csi2rx->notifier.ops = &csi2rx_notifier_ops; + csi2rx->notifier.max_subdevs = 1; + csi2rx->subdev.owner = THIS_MODULE; + csi2rx->subdev.dev = &pdev->dev; + v4l2_subdev_init(&csi2rx->subdev, &csi2rx_subdev_ops); + v4l2_set_subdevdata(&csi2rx->subdev, &pdev->dev); + snprintf(csi2rx->subdev.name, V4L2_SUBDEV_NAME_SIZE, "%s.%s", + KBUILD_MODNAME, dev_name(&pdev->dev)); + + /* Create our media pads */ + csi2rx->subdev.entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER; + csi2rx->pads[CSI2RX_PAD_SINK].flags = MEDIA_PAD_FL_SINK; + for (i = CSI2RX_PAD_SOURCE_STREAM0; i < CSI2RX_PAD_MAX; i++) + csi2rx->pads[i].flags = MEDIA_PAD_FL_SOURCE; + + ret = v4l2_async_notifier_parse_fwnode_endpoints_by_port(csi2rx->dev, + &csi2rx->notifier, + sizeof(struct v4l2_async_subdev), + 0, NULL); + if (ret < 0) { + dev_err(csi2rx->dev, "Failed to parse our endpoints\n"); + goto err_free_priv; + } + + ret = v4l2_async_subdev_notifier_register(&csi2rx->subdev, + &csi2rx->notifier); + if (ret < 0) { + dev_err(csi2rx->dev, "Failed to register our notifier\n"); + goto err_free_priv; + } + + ret = media_entity_pads_init(&csi2rx->subdev.entity, CSI2RX_PAD_MAX, + csi2rx->pads); + if (ret) + goto err_free_priv; + + ret = v4l2_async_register_subdev(&csi2rx->subdev); + if (ret < 0) + goto err_free_priv; + + dev_info(&pdev->dev, + "Probed CSI2RX with %u/%u lanes, %u streams, %s D-PHY\n", + csi2rx->num_lanes, csi2rx->max_lanes, csi2rx->max_streams, + csi2rx->has_internal_dphy ? "internal" : "no"); + + return 0; + +err_free_priv: + kfree(csi2rx); + return ret; +} + +static int csi2rx_remove(struct platform_device *pdev) +{ + struct csi2rx_priv *csi2rx = platform_get_drvdata(pdev); + + v4l2_async_unregister_subdev(&csi2rx->subdev); + kfree(csi2rx); + + return 0; +} + +static const struct of_device_id csi2rx_of_table[] = { + { .compatible = "cdns,csi2rx" }, + { }, +}; +MODULE_DEVICE_TABLE(of, csi2rx_of_table); + +static struct platform_driver csi2rx_driver = { + .probe = csi2rx_probe, + .remove = csi2rx_remove, + + .driver = { + .name = "cdns-csi2rx", + .of_match_table = csi2rx_of_table, + }, +}; +module_platform_driver(csi2rx_driver);