Add StarFive Camera Subsystem driver

Add the bindings documentation for Starfive JH7110 Camera Subsystem which is used for handing image sensor data.

Reviewed-by Krzysztof Kozlowski <[email protected]>

Signed-off-by Jack Zhu <[email protected]>

---
 .../bindings/media/starfive,jh7110-camss.yaml | 180 ++++++++++++++++++
 MAINTAINERS                                   |   7 +
 2 files changed, 187 insertions(+)
 create mode 100644 Documentation/devicetree/bindings/media/starfive,jh7110-camss.yaml

Documentation/devicetree/bindings/media/starfive,jh7110-camss.yaml new file +180 -0

new file mode 100644
index 000000000000..c66586d90fa2
@@ -0,0 +1,180 @@
+# SPDX-License-Identifier: (GPL-2.0 OR BSD-2-Clause)
+%YAML 1.2
+---
+$id: http://devicetree.org/schemas/media/starfive,jh7110-camss.yaml#
+$schema: http://devicetree.org/meta-schemas/core.yaml#
+
+title: Starfive SoC CAMSS ISP
+
+maintainers:
+  - Jack Zhu <[email protected]>
+  - Changhuang Liang <[email protected]>
+
+description:
+  The Starfive CAMSS ISP is a Camera interface for Starfive JH7110 SoC. It
+  consists of a VIN controller (Video In Controller, a top-level control unit)
+  and an ISP.
+
+properties:
+  compatible:
+    const: starfive,jh7110-camss
+
+  reg:
+    maxItems: 2
+
+  reg-names:
+    items:
+      - const: syscon
+      - const: isp
+
+  clocks:
+    maxItems: 7
+
+  clock-names:
+    items:
+      - const: apb_func
+      - const: wrapper_clk_c
+      - const: dvp_inv
+      - const: axiwr
+      - const: mipi_rx0_pxl
+      - const: ispcore_2x
+      - const: isp_axi
+
+  resets:
+    maxItems: 6
+
+  reset-names:
+    items:
+      - const: wrapper_p
+      - const: wrapper_c
+      - const: axird
+      - const: axiwr
+      - const: isp_top_n
+      - const: isp_top_axi
+
+  power-domains:
+    items:
+      - description: JH7110 ISP Power Domain Switch Controller.
+
+  interrupts:
+    maxItems: 4
+
+  ports:
+    $ref: /schemas/graph.yaml#/properties/ports
+
+    properties:
+      port@0:
+        $ref: /schemas/graph.yaml#/$defs/port-base
+        unevaluatedProperties: false
+        description: Input port for receiving DVP data.
+
+        properties:
+          endpoint:
+            $ref: video-interfaces.yaml#
+            unevaluatedProperties: false
+
+            properties:
+              bus-type:
+                enum: [5, 6]
+
+              bus-width:
+                enum: [8, 10, 12]
+
+              data-shift:
+                enum: [0, 2]
+                default: 0
+
+              hsync-active:
+                enum: [0, 1]
+                default: 1
+
+              vsync-active:
+                enum: [0, 1]
+                default: 1
+
+            required:
+              - bus-type
+              - bus-width
+
+      port@1:
+        $ref: /schemas/graph.yaml#/properties/port
+        description: Input port for receiving CSI data.
+
+    required:
+      - port@0
+      - port@1
+
+required:
+  - compatible
+  - reg
+  - reg-names
+  - clocks
+  - clock-names
+  - resets
+  - reset-names
+  - power-domains
+  - interrupts
+  - ports
+
+additionalProperties: false
+
+examples:
+  - |
+    isp@19840000 {
+        compatible = "starfive,jh7110-camss";
+        reg = <0x19840000 0x10000>,
+              <0x19870000 0x30000>;
+        reg-names = "syscon", "isp";
+        clocks = <&ispcrg 0>,
+                 <&ispcrg 13>,
+                 <&ispcrg 2>,
+                 <&ispcrg 12>,
+                 <&ispcrg 1>,
+                 <&syscrg 51>,
+                 <&syscrg 52>;
+        clock-names = "apb_func",
+                      "wrapper_clk_c",
+                      "dvp_inv",
+                      "axiwr",
+                      "mipi_rx0_pxl",
+                      "ispcore_2x",
+                      "isp_axi";
+        resets = <&ispcrg 0>,
+                 <&ispcrg 1>,
+                 <&ispcrg 10>,
+                 <&ispcrg 11>,
+                 <&syscrg 41>,
+                 <&syscrg 42>;
+        reset-names = "wrapper_p",
+                      "wrapper_c",
+                      "axird",
+                      "axiwr",
+                      "isp_top_n",
+                      "isp_top_axi";
+        power-domains = <&pwrc 5>;
+        interrupts = <92>, <87>, <88>, <90>;
+
+        ports {
+            #address-cells = <1>;
+            #size-cells = <0>;
+            port@0 {
+                reg = <0>;
+                vin_from_sc2235: endpoint {
+                    remote-endpoint = <&sc2235_to_vin>;
+                    bus-type = <5>;
+                    bus-width = <8>;
+                    data-shift = <2>;
+                    hsync-active = <1>;
+                    vsync-active = <0>;
+                    pclk-sample = <1>;
+                };
+            };
+
+            port@1 {
+                reg = <1>;
+                vin_from_csi2rx: endpoint {
+                    remote-endpoint = <&csi2rx_to_vin>;
+                };
+            };
+        };
+    };

MAINTAINERS +7 -0

index 869e1c7fc869..9ef5d0e5dff0 100644
@@ -20070,6 +20070,13 @@ M:	Ion Badulescu <[email protected]>
 S:	Odd Fixes
 F:	drivers/net/ethernet/adaptec/starfire*
 
+STARFIVE CAMERA SUBSYSTEM DRIVER
+M:	Jack Zhu <[email protected]>
+M:	Changhuang Liang <[email protected]>
+L:	[email protected]
+S:	Maintained
+F:	Documentation/devicetree/bindings/media/starfive,jh7110-camss.yaml
+
 STARFIVE DEVICETREES
 M:	Emil Renner Berthing <[email protected]>
 S:	Maintained

Add basic platform driver for StarFive Camera Subsystem.

Reviewed-by Bryan O'Donoghue <[email protected]>

Signed-off-by Jack Zhu <[email protected]>

---
 MAINTAINERS                                  |   1 +
 drivers/media/platform/Kconfig               |   1 +
 drivers/media/platform/Makefile              |   1 +
 drivers/media/platform/starfive/Kconfig      |  19 +
 drivers/media/platform/starfive/Makefile     |   9 +
 drivers/media/platform/starfive/stf_camss.c  | 372 +++++++++++++++++++
 drivers/media/platform/starfive/stf_camss.h  | 153 ++++++++
 drivers/media/platform/starfive/stf_common.h |  18 +
 8 files changed, 574 insertions(+)
 create mode 100644 drivers/media/platform/starfive/Kconfig
 create mode 100644 drivers/media/platform/starfive/Makefile
 create mode 100644 drivers/media/platform/starfive/stf_camss.c
 create mode 100644 drivers/media/platform/starfive/stf_camss.h
 create mode 100644 drivers/media/platform/starfive/stf_common.h

MAINTAINERS +1 -0

index 71291dc58671..33a752569942 100644
@@ -20077,6 +20077,7 @@ L:	[email protected]
 S:	Maintained
 F:	Documentation/admin-guide/media/starfive_camss.rst
 F:	Documentation/devicetree/bindings/media/starfive,jh7110-camss.yaml
+F:	drivers/media/platform/starfive/
 
 STARFIVE DEVICETREES
 M:	Emil Renner Berthing <[email protected]>

drivers/media/platform/Kconfig +1 -0

index ee579916f874..627eaa0ab3ee 100644
@@ -80,6 +80,7 @@ source "drivers/media/platform/renesas/Kconfig"
 source "drivers/media/platform/rockchip/Kconfig"
 source "drivers/media/platform/samsung/Kconfig"
 source "drivers/media/platform/st/Kconfig"
+source "drivers/media/platform/starfive/Kconfig"
 source "drivers/media/platform/sunxi/Kconfig"
 source "drivers/media/platform/ti/Kconfig"
 source "drivers/media/platform/verisilicon/Kconfig"

drivers/media/platform/Makefile +1 -0

index 5453bb868e67..5a038498a370 100644
@@ -23,6 +23,7 @@ obj-y += renesas/
 obj-y += rockchip/
 obj-y += samsung/
 obj-y += st/
+obj-y += starfive/
 obj-y += sunxi/
 obj-y += ti/
 obj-y += verisilicon/

drivers/media/platform/starfive/Kconfig new file +0 -0

new file mode 100644
index 000000000000..1a2293475bd2

drivers/media/platform/starfive/Kconfig new file +10 -0

+	select MEDIA_CONTROLLER
+	select VIDEO_V4L2_SUBDEV_API
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_FWNODE
+	help
+	   Enable this to support for the Starfive Camera subsystem
+	   found on Starfive JH7110 SoC.
+
+	   To compile this driver as a module, choose M here: the
+	   module will be called stf-camss.

drivers/media/platform/starfive/Makefile new file +9 -0

new file mode 100644
index 000000000000..796775fa52f4
@@ -0,0 +1,9 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for StarFive camera subsystem driver.
+#
+
+starfive-camss-objs += \
+		stf_camss.o
+
+obj-$(CONFIG_VIDEO_STARFIVE_CAMSS) += starfive-camss.o \

drivers/media/platform/starfive/stf_camss.c new file +63 -0

new file mode 100644
index 000000000000..cb2b62d49416
@@ -0,0 +1,372 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * stf_camss.c
+ *
+ * Starfive Camera Subsystem driver
+ *
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ */
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_graph.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/videodev2.h>
+#include <media/media-device.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-mc.h>
+
+#include "stf_camss.h"
+
+static const char * const stfcamss_clocks[] = {
+	"clk_apb_func",
+	"clk_wrapper_clk_c",
+	"clk_dvp_inv",
+	"clk_axiwr",
+	"clk_mipi_rx0_pxl",
+	"clk_ispcore_2x",
+	"clk_isp_axi",
+};
+
+static const char * const stfcamss_resets[] = {
+	"rst_wrapper_p",
+	"rst_wrapper_c",
+	"rst_axird",
+	"rst_axiwr",
+	"rst_isp_top_n",
+	"rst_isp_top_axi",
+};
+
+static int stfcamss_get_mem_res(struct platform_device *pdev,
+				struct stfcamss *stfcamss)
+{
+	stfcamss->syscon_base =
+		devm_platform_ioremap_resource_byname(pdev, "syscon");
+	if (IS_ERR(stfcamss->syscon_base))
+		return PTR_ERR(stfcamss->syscon_base);
+
+	stfcamss->isp_base =
+		devm_platform_ioremap_resource_byname(pdev, "isp");
+	if (IS_ERR(stfcamss->isp_base))
+		return PTR_ERR(stfcamss->isp_base);
+
+	return 0;
+}
+
+/*
+ * stfcamss_of_parse_endpoint_node - Parse port endpoint node
+ * @dev: Device
+ * @node: Device node to be parsed
+ * @csd: Parsed data from port endpoint node
+ *
+ * Return 0 on success or a negative error code on failure

drivers/media/platform/starfive/stf_camss.c new file +26 -0

+
+	csd->port = vep.base.port;
+
+	return 0;
+}
+
+/*
+ * stfcamss_of_parse_ports - Parse ports node
+ * @stfcamss: STFCAMSS device
+ *
+ * Return number of "port" nodes found in "ports" node
+ */
+static int stfcamss_of_parse_ports(struct stfcamss *stfcamss)
+{
+	struct device *dev = stfcamss->dev;
+	struct device_node *node = NULL;
+	struct device_node *remote = NULL;
+	int ret, num_subdevs = 0;
+
+	for_each_endpoint_of_node(dev->of_node, node) {
+		struct stfcamss_async_subdev *csd;
+
+		if (!of_device_is_available(node))
+			continue;
+
+		remote = of_graph_get_remote_port_parent(node);

drivers/media/platform/starfive/stf_camss.c new file +18 -0

+		if (IS_ERR(csd)) {
+			ret = PTR_ERR(csd);
+			goto err_cleanup;
+		}
+
+		ret = stfcamss_of_parse_endpoint_node(dev, node, csd);
+		if (ret < 0)
+			goto err_cleanup;
+
+		num_subdevs++;
+	}
+
+	return num_subdevs;
+
+err_cleanup:
+	of_node_put(node);
+	return ret;
+}

drivers/media/platform/starfive/stf_camss.c new file +22 -0

+
+	for (i = 0; i < source->num_pads; i++) {
+		if (source->pads[i].flags & MEDIA_PAD_FL_SOURCE)
+			break;
+	}
+
+	if (i == source->num_pads) {
+		dev_err(stfcamss->dev, "No source pad in external entity\n");
+		return -EINVAL;
+	}
+
+	for (j = 0; host_data->host_entity[j] && (j < HOST_ENTITY_MAX); j++) {
+		struct media_entity *input;
+		int ret;
+
+		input = host_data->host_entity[j];
+
+		ret = media_create_pad_link(
+			source,
+			i,
+			input,
+			STF_PAD_SINK,

drivers/media/platform/starfive/stf_camss.c new file +21 -0

+
+	return 0;
+}
+
+static int stfcamss_subdev_notifier_complete(struct v4l2_async_notifier *ntf)
+{
+	struct stfcamss *stfcamss =
+		container_of(ntf, struct stfcamss, notifier);
+
+	return v4l2_device_register_subdev_nodes(&stfcamss->v4l2_dev);
+}
+
+static const struct v4l2_async_notifier_operations
+stfcamss_subdev_notifier_ops = {
+	.bound = stfcamss_subdev_notifier_bound,
+	.complete = stfcamss_subdev_notifier_complete,
+};
+
+static const struct media_device_ops stfcamss_media_ops = {
+	.link_notify = v4l2_pipeline_link_notify,
+};

drivers/media/platform/starfive/stf_camss.c new file +7 -0

+	stfcamss->media_dev.ops = &stfcamss_media_ops;
+	media_device_init(&stfcamss->media_dev);
+
+	stfcamss->v4l2_dev.mdev = &stfcamss->media_dev;
+}
+
+/*

drivers/media/platform/starfive/stf_camss.c new file +26 -0

+	}
+
+	stfcamss->nclks = ARRAY_SIZE(stfcamss->sys_clk);
+	for (i = 0; i < stfcamss->nclks; ++i)
+		stfcamss->sys_clk[i].id = stfcamss_clocks[i];
+	ret = devm_clk_bulk_get(dev, stfcamss->nclks, stfcamss->sys_clk);
+	if (ret) {
+		dev_err(dev, "Failed to get clk controls\n");
+		return ret;
+	}
+
+	stfcamss->nrsts = ARRAY_SIZE(stfcamss->sys_rst);
+	for (i = 0; i < stfcamss->nrsts; ++i)
+		stfcamss->sys_rst[i].id = stfcamss_resets[i];
+	ret = devm_reset_control_bulk_get_shared(dev, stfcamss->nrsts,
+						 stfcamss->sys_rst);
+	if (ret) {
+		dev_err(dev, "Failed to get reset controls\n");
+		return ret;
+	}
+
+	ret = stfcamss_get_mem_res(pdev, stfcamss);
+	if (ret) {
+		dev_err(dev, "Could not map registers\n");
+		return ret;
+	}

drivers/media/platform/starfive/stf_camss.c new file +15 -0

+	}
+
+	stfcamss_mc_init(pdev, stfcamss);
+
+	ret = v4l2_device_register(stfcamss->dev, &stfcamss->v4l2_dev);
+	if (ret < 0) {
+		dev_err(dev, "Failed to register V4L2 device: %d\n", ret);
+		goto err_cleanup_notifier;
+	}
+
+	ret = media_device_register(&stfcamss->media_dev);
+	if (ret) {
+		dev_err(dev, "Failed to register media device: %d\n", ret);
+		goto err_unregister_device;
+	}

drivers/media/platform/starfive/stf_camss.c new file +44 -0

+
+	return 0;
+
+err_unregister_media_dev:
+	media_device_unregister(&stfcamss->media_dev);
+err_unregister_device:
+	v4l2_device_unregister(&stfcamss->v4l2_dev);
+err_cleanup_notifier:
+	v4l2_async_nf_cleanup(&stfcamss->notifier);
+	return ret;
+}
+
+/*
+ * stfcamss_remove - Remove STFCAMSS platform device
+ * @pdev: Pointer to STFCAMSS platform device
+ *
+ * Always returns 0.
+ */
+static int stfcamss_remove(struct platform_device *pdev)
+{
+	struct stfcamss *stfcamss = platform_get_drvdata(pdev);
+
+	v4l2_device_unregister(&stfcamss->v4l2_dev);
+	media_device_cleanup(&stfcamss->media_dev);
+	pm_runtime_disable(&pdev->dev);
+
+	return 0;
+}
+
+static const struct of_device_id stfcamss_of_match[] = {
+	{ .compatible = "starfive,jh7110-camss" },
+	{ /* sentinel */ },
+};
+
+MODULE_DEVICE_TABLE(of, stfcamss_of_match);
+
+static int __maybe_unused stfcamss_runtime_suspend(struct device *dev)
+{
+	struct stfcamss *stfcamss = dev_get_drvdata(dev);
+
+	reset_control_assert(stfcamss->sys_rst[STF_RST_ISP_TOP_AXI].rstc);
+	reset_control_assert(stfcamss->sys_rst[STF_RST_ISP_TOP_N].rstc);
+	clk_disable_unprepare(stfcamss->sys_clk[STF_CLK_ISP_AXI].clk);
+	clk_disable_unprepare(stfcamss->sys_clk[STF_CLK_ISPCORE_2X].clk);

drivers/media/platform/starfive/stf_camss.c new file +27 -0

+	reset_control_deassert(stfcamss->sys_rst[STF_RST_ISP_TOP_N].rstc);
+	reset_control_deassert(stfcamss->sys_rst[STF_RST_ISP_TOP_AXI].rstc);
+
+	return 0;
+}
+
+static const struct dev_pm_ops stfcamss_pm_ops = {
+	SET_RUNTIME_PM_OPS(stfcamss_runtime_suspend,
+			   stfcamss_runtime_resume,
+			   NULL)
+};
+
+static struct platform_driver stfcamss_driver = {
+	.probe = stfcamss_probe,
+	.remove = stfcamss_remove,
+	.driver = {
+		.name = DRV_NAME,
+		.pm = &stfcamss_pm_ops,
+		.of_match_table = of_match_ptr(stfcamss_of_match),
+	},
+};
+
+module_platform_driver(stfcamss_driver);
+
+MODULE_AUTHOR("StarFive Corporation");
+MODULE_DESCRIPTION("StarFive Camera Subsystem driver");
+MODULE_LICENSE("GPL");

drivers/media/platform/starfive/stf_camss.h new file +6 -0

new file mode 100644
index 000000000000..8f39cd65950c
@@ -0,0 +1,153 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * stf_camss.h
+ *
+ * Starfive Camera Subsystem driver
+ *

drivers/media/platform/starfive/stf_camss.h new file +23 -0

+};
+
+enum stf_clk {
+	STF_CLK_APB_FUNC = 0,
+	STF_CLK_WRAPPER_CLK_C,
+	STF_CLK_DVP_INV,
+	STF_CLK_AXIWR,
+	STF_CLK_MIPI_RX0_PXL,
+	STF_CLK_ISPCORE_2X,
+	STF_CLK_ISP_AXI,
+	STF_CLK_NUM
+};
+
+enum stf_rst {
+	STF_RST_WRAPPER_P = 0,
+	STF_RST_WRAPPER_C,
+	STF_RST_AXIRD,
+	STF_RST_AXIWR,
+	STF_RST_ISP_TOP_N,
+	STF_RST_ISP_TOP_AXI,
+	STF_RST_NUM
+};
+

drivers/media/platform/starfive/stf_camss.h new file +88 -0

+	struct media_entity *host_entity[HOST_ENTITY_MAX];
+};
+
+struct stfcamss {
+	struct v4l2_device v4l2_dev;
+	struct media_device media_dev;
+	struct media_pipeline pipe;
+	struct device *dev;
+	struct v4l2_async_notifier notifier;
+	struct host_data host_data;
+	void __iomem *syscon_base;
+	void __iomem *isp_base;
+	int irq[STF_IRQ_NUM];
+	struct clk_bulk_data sys_clk[STF_CLK_NUM];
+	int nclks;
+	struct reset_control_bulk_data sys_rst[STF_RST_NUM];
+	int nrsts;
+};
+
+struct stfcamss_async_subdev {
+	struct v4l2_async_subdev asd;  /* must be first */
+	enum port_num port;
+};
+
+static inline u32 stf_isp_reg_read(struct stfcamss *stfcamss, u32 reg)
+{
+	return ioread32(stfcamss->isp_base + reg);
+}
+
+static inline void stf_isp_reg_write(struct stfcamss *stfcamss,
+				     u32 reg, u32 val)
+{
+	iowrite32(val, stfcamss->isp_base + reg);
+}
+
+static inline void stf_isp_reg_write_delay(struct stfcamss *stfcamss,
+					   u32 reg, u32 val, u32 delay)
+{
+	iowrite32(val, stfcamss->isp_base + reg);
+	usleep_range(1000 * delay, 1000 * delay + 100);
+}
+
+static inline void stf_isp_reg_set_bit(struct stfcamss *stfcamss,
+				       u32 reg, u32 mask, u32 val)
+{
+	u32 value;
+
+	value = ioread32(stfcamss->isp_base + reg) & ~mask;
+	val &= mask;
+	val |= value;
+	iowrite32(val, stfcamss->isp_base + reg);
+}
+
+static inline void stf_isp_reg_set(struct stfcamss *stfcamss, u32 reg, u32 mask)
+{
+	iowrite32(ioread32(stfcamss->isp_base + reg) | mask,
+		  stfcamss->isp_base + reg);
+}
+
+static inline u32 stf_syscon_reg_read(struct stfcamss *stfcamss, u32 reg)
+{
+	return ioread32(stfcamss->syscon_base + reg);
+}
+
+static inline void stf_syscon_reg_write(struct stfcamss *stfcamss,
+					u32 reg, u32 val)
+{
+	iowrite32(val, stfcamss->syscon_base + reg);
+}
+
+static inline void stf_syscon_reg_set_bit(struct stfcamss *stfcamss,
+					  u32 reg, u32 bit_mask)
+{
+	u32 value;
+
+	value = ioread32(stfcamss->syscon_base + reg);
+	iowrite32(value | bit_mask, stfcamss->syscon_base + reg);
+}
+
+static inline void stf_syscon_reg_clear_bit(struct stfcamss *stfcamss,
+					    u32 reg, u32 bit_mask)
+{
+	u32 value;
+
+	value = ioread32(stfcamss->syscon_base + reg);
+	iowrite32(value & ~bit_mask, stfcamss->syscon_base + reg);
+}
+#endif /* STF_CAMSS_H */

drivers/media/platform/starfive/stf_common.h new file +6 -0

new file mode 100644
index 000000000000..e04fca2d9cd4
@@ -0,0 +1,18 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * stf_common.h
+ *
+ * StarFive Camera Subsystem - Common definitions
+ *

drivers/media/platform/starfive/stf_common.h new file +2 -0

+
+#endif /* STF_COMMON_H */

Hi Jack,

Thank you for the patch.

Quoted 66 lines · Jack Zhu

Add basic platform driver for StarFive Camera Subsystem.

Reviewed-by Bryan O'Donoghue <[email protected]>

Signed-off-by Jack Zhu <[email protected]>

---
 MAINTAINERS                                  |   1 +
 drivers/media/platform/Kconfig               |   1 +
 drivers/media/platform/Makefile              |   1 +
 drivers/media/platform/starfive/Kconfig      |  19 +
 drivers/media/platform/starfive/Makefile     |   9 +
 drivers/media/platform/starfive/stf_camss.c  | 372 +++++++++++++++++++
 drivers/media/platform/starfive/stf_camss.h  | 153 ++++++++
 drivers/media/platform/starfive/stf_common.h |  18 +
 8 files changed, 574 insertions(+)
 create mode 100644 drivers/media/platform/starfive/Kconfig
 create mode 100644 drivers/media/platform/starfive/Makefile
 create mode 100644 drivers/media/platform/starfive/stf_camss.c
 create mode 100644 drivers/media/platform/starfive/stf_camss.h
 create mode 100644 drivers/media/platform/starfive/stf_common.h

MAINTAINERS +1 -0

index 71291dc58671..33a752569942 100644
@@ -20077,6 +20077,7 @@ L:	[email protected]
 S:	Maintained
 F:	Documentation/admin-guide/media/starfive_camss.rst
 F:	Documentation/devicetree/bindings/media/starfive,jh7110-camss.yaml
+F:	drivers/media/platform/starfive/
 
 STARFIVE DEVICETREES
 M:	Emil Renner Berthing <[email protected]>

drivers/media/platform/Kconfig +1 -0

index ee579916f874..627eaa0ab3ee 100644
@@ -80,6 +80,7 @@ source "drivers/media/platform/renesas/Kconfig"
 source "drivers/media/platform/rockchip/Kconfig"
 source "drivers/media/platform/samsung/Kconfig"
 source "drivers/media/platform/st/Kconfig"
+source "drivers/media/platform/starfive/Kconfig"
 source "drivers/media/platform/sunxi/Kconfig"
 source "drivers/media/platform/ti/Kconfig"
 source "drivers/media/platform/verisilicon/Kconfig"

drivers/media/platform/Makefile +1 -0

index 5453bb868e67..5a038498a370 100644
@@ -23,6 +23,7 @@ obj-y += renesas/
 obj-y += rockchip/
 obj-y += samsung/
 obj-y += st/
+obj-y += starfive/
 obj-y += sunxi/
 obj-y += ti/
 obj-y += verisilicon/

drivers/media/platform/starfive/Kconfig new file +4 -0

new file mode 100644
index 000000000000..1a2293475bd2
@@ -0,0 +1,19 @@
+# SPDX-License-Identifier: GPL-2.0-only
+
+comment "Starfive media platform drivers"
+

drivers/media/platform/starfive/Kconfig line 9

Quoted 5 lines

drivers/media/platform/starfive/Kconfig new file +5 -0

+config VIDEO_STARFIVE_CAMSS
+	tristate "Starfive Camera Subsystem driver"
+	depends on V4L_PLATFORM_DRIVERS
+	depends on VIDEO_DEV && OF
+	depends on DMA_CMA

I think you can drop this, the driver doesn't depend on DMA. It uses the DMA mapping API though, so you should depend on HAS_DMA instead.

Quoted 98 lines

drivers/media/platform/starfive/Kconfig new file +10 -0

+	select MEDIA_CONTROLLER
+	select VIDEO_V4L2_SUBDEV_API
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_FWNODE
+	help
+	   Enable this to support for the Starfive Camera subsystem
+	   found on Starfive JH7110 SoC.
+
+	   To compile this driver as a module, choose M here: the
+	   module will be called stf-camss.

drivers/media/platform/starfive/Makefile new file +9 -0

new file mode 100644
index 000000000000..796775fa52f4
@@ -0,0 +1,9 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for StarFive camera subsystem driver.
+#
+
+starfive-camss-objs += \
+		stf_camss.o
+
+obj-$(CONFIG_VIDEO_STARFIVE_CAMSS) += starfive-camss.o \

drivers/media/platform/starfive/stf_camss.c new file +67 -0

new file mode 100644
index 000000000000..cb2b62d49416
@@ -0,0 +1,372 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * stf_camss.c
+ *
+ * Starfive Camera Subsystem driver
+ *
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ */
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_graph.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/videodev2.h>
+#include <media/media-device.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-mc.h>
+
+#include "stf_camss.h"
+
+static const char * const stfcamss_clocks[] = {
+	"clk_apb_func",
+	"clk_wrapper_clk_c",
+	"clk_dvp_inv",
+	"clk_axiwr",
+	"clk_mipi_rx0_pxl",
+	"clk_ispcore_2x",
+	"clk_isp_axi",
+};
+
+static const char * const stfcamss_resets[] = {
+	"rst_wrapper_p",
+	"rst_wrapper_c",
+	"rst_axird",
+	"rst_axiwr",
+	"rst_isp_top_n",
+	"rst_isp_top_axi",
+};
+
+static int stfcamss_get_mem_res(struct platform_device *pdev,
+				struct stfcamss *stfcamss)
+{
+	stfcamss->syscon_base =
+		devm_platform_ioremap_resource_byname(pdev, "syscon");
+	if (IS_ERR(stfcamss->syscon_base))
+		return PTR_ERR(stfcamss->syscon_base);
+
+	stfcamss->isp_base =
+		devm_platform_ioremap_resource_byname(pdev, "isp");
+	if (IS_ERR(stfcamss->isp_base))
+		return PTR_ERR(stfcamss->isp_base);
+
+	return 0;
+}
+
+/*
+ * stfcamss_of_parse_endpoint_node - Parse port endpoint node
+ * @dev: Device
+ * @node: Device node to be parsed
+ * @csd: Parsed data from port endpoint node
+ *
+ * Return 0 on success or a negative error code on failure
+ */
+static int stfcamss_of_parse_endpoint_node(struct device *dev,
+					   struct device_node *node,
+					   struct stfcamss_async_subdev *csd)

drivers/media/platform/starfive/stf_camss.c line 73

Quoted 6 lines

drivers/media/platform/starfive/stf_camss.c new file +6 -0

+{
+	struct v4l2_fwnode_endpoint vep = { { 0 } };
+
+	v4l2_fwnode_endpoint_parse(of_fwnode_handle(node), &vep);
+	dev_dbg(dev, "vep.base.port = 0x%x, id = 0x%x\n",
+		vep.base.port, vep.base.id);

You can print the port and id as decimal numbers, they're indices.

Quoted 31 lines

drivers/media/platform/starfive/stf_camss.c new file +31 -0

+
+	csd->port = vep.base.port;
+
+	return 0;
+}
+
+/*
+ * stfcamss_of_parse_ports - Parse ports node
+ * @stfcamss: STFCAMSS device
+ *
+ * Return number of "port" nodes found in "ports" node
+ */
+static int stfcamss_of_parse_ports(struct stfcamss *stfcamss)
+{
+	struct device *dev = stfcamss->dev;
+	struct device_node *node = NULL;
+	struct device_node *remote = NULL;
+	int ret, num_subdevs = 0;
+
+	for_each_endpoint_of_node(dev->of_node, node) {
+		struct stfcamss_async_subdev *csd;
+
+		if (!of_device_is_available(node))
+			continue;
+
+		remote = of_graph_get_remote_port_parent(node);
+		if (!remote) {
+			dev_err(dev, "Cannot get remote parent\n");
+			ret = -EINVAL;
+			goto err_cleanup;
+		}

drivers/media/platform/starfive/stf_camss.c line 109

Quoted 5 lines

drivers/media/platform/starfive/stf_camss.c new file +5 -0

+
+		csd = v4l2_async_nf_add_fwnode(&stfcamss->notifier,
+					       of_fwnode_handle(remote),
+					       struct stfcamss_async_subdev);
+		of_node_put(remote);

You can use v4l2_async_nf_add_fwnode_remote() to avoid the need to call of_graph_get_remote_port_parent() yourself.

Quoted 22 lines

drivers/media/platform/starfive/stf_camss.c new file +22 -0

+		if (IS_ERR(csd)) {
+			ret = PTR_ERR(csd);
+			goto err_cleanup;
+		}
+
+		ret = stfcamss_of_parse_endpoint_node(dev, node, csd);
+		if (ret < 0)
+			goto err_cleanup;
+
+		num_subdevs++;
+	}
+
+	return num_subdevs;
+
+err_cleanup:
+	of_node_put(node);
+	return ret;
+}
+
+static int stfcamss_subdev_notifier_bound(struct v4l2_async_notifier *async,
+					  struct v4l2_subdev *subdev,
+					  struct v4l2_async_subdev *asd)

drivers/media/platform/starfive/stf_camss.c line 137

Quoted 6 lines

drivers/media/platform/starfive/stf_camss.c new file +6 -0

+{
+	struct stfcamss *stfcamss =
+		container_of(async, struct stfcamss, notifier);
+	struct host_data *host_data = &stfcamss->host_data;
+	struct media_entity *source;
+	int i, j;

i and j are never negative, you can make them unsigned int.

drivers/media/platform/starfive/stf_camss.c line 139

Quoted 2 lines

drivers/media/platform/starfive/stf_camss.c new file +2 -0

+
+	source = &subdev->entity;

You can assign the variable when declaring it:

	struct media_entity *source = &subdev->entity;

Quoted 22 lines

drivers/media/platform/starfive/stf_camss.c new file +22 -0

+
+	for (i = 0; i < source->num_pads; i++) {
+		if (source->pads[i].flags & MEDIA_PAD_FL_SOURCE)
+			break;
+	}
+
+	if (i == source->num_pads) {
+		dev_err(stfcamss->dev, "No source pad in external entity\n");
+		return -EINVAL;
+	}
+
+	for (j = 0; host_data->host_entity[j] && (j < HOST_ENTITY_MAX); j++) {
+		struct media_entity *input;
+		int ret;
+
+		input = host_data->host_entity[j];
+
+		ret = media_create_pad_link(
+			source,
+			i,
+			input,
+			STF_PAD_SINK,

drivers/media/platform/starfive/stf_camss.c line 171

Quoted 10 lines

drivers/media/platform/starfive/stf_camss.c new file +10 -0

+			source->function == MEDIA_ENT_F_CAM_SENSOR ?
+			MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED :
+			0);
+		if (ret < 0) {
+			dev_err(stfcamss->dev,
+				"Failed to link %s->%s entities: %d\n",
+				source->name, input->name, ret);
+			return ret;
+		}
+	}

I think you can simplify this. First of all, the port to which the newly bound subdev is attached is available in the stfcamss_async_subdev structure. You can cast the asd to a stfcamss_async_subdev with container_of(), access the port number from there, and get the corresponding sink subdev and pad (for the DVP or CSI-2 port). Then, I think you can use v4l2_create_fwnode_links_to_pad(), which will search for the source pad in the source subdev, instead of doing it yourself.

I believe the stfcamss.host_data field can be dropped, as it's only used in this function.

Quoted 26 lines

drivers/media/platform/starfive/stf_camss.c new file +26 -0

+
+	return 0;
+}
+
+static int stfcamss_subdev_notifier_complete(struct v4l2_async_notifier *ntf)
+{
+	struct stfcamss *stfcamss =
+		container_of(ntf, struct stfcamss, notifier);
+
+	return v4l2_device_register_subdev_nodes(&stfcamss->v4l2_dev);
+}
+
+static const struct v4l2_async_notifier_operations
+stfcamss_subdev_notifier_ops = {
+	.bound = stfcamss_subdev_notifier_bound,
+	.complete = stfcamss_subdev_notifier_complete,
+};
+
+static const struct media_device_ops stfcamss_media_ops = {
+	.link_notify = v4l2_pipeline_link_notify,
+};
+
+static void stfcamss_mc_init(struct platform_device *pdev,
+			     struct stfcamss *stfcamss)
+{
+	stfcamss->media_dev.dev = stfcamss->dev;

drivers/media/platform/starfive/stf_camss.c line 202

Quoted 5 lines

drivers/media/platform/starfive/stf_camss.c new file +5 -0

+	strscpy(stfcamss->media_dev.model, "Starfive Camera Subsystem",
+		sizeof(stfcamss->media_dev.model));
+	snprintf(stfcamss->media_dev.bus_info,
+		 sizeof(stfcamss->media_dev.bus_info),
+		 "%s:%s", dev_bus_name(&pdev->dev), pdev->name);

No need to set bus_info manually, the framework does if for you in media_device_init().

drivers/media/platform/starfive/stf_camss.c line 203

Quoted 1 line

drivers/media/platform/starfive/stf_camss.c new file +1 -0

+	stfcamss->media_dev.hw_revision = 0x01;

Would the hardware have a version or revision register that you could use to fill this, by any chance ?

Quoted 7 lines

drivers/media/platform/starfive/stf_camss.c new file +7 -0

+	stfcamss->media_dev.ops = &stfcamss_media_ops;
+	media_device_init(&stfcamss->media_dev);
+
+	stfcamss->v4l2_dev.mdev = &stfcamss->media_dev;
+}
+
+/*

drivers/media/platform/starfive/stf_camss.c line 220

Quoted 10 lines

drivers/media/platform/starfive/stf_camss.c new file +10 -0

+ * stfcamss_probe - Probe STFCAMSS platform device
+ * @pdev: Pointer to STFCAMSS platform device
+ *
+ * Return 0 on success or a negative error code on failure
+ */
+static int stfcamss_probe(struct platform_device *pdev)
+{
+	struct stfcamss *stfcamss;
+	struct device *dev = &pdev->dev;
+	int ret = 0, i, num_subdevs;

No need to initialize ret to 0.

i is never negative, make it an unsigned int.

Quoted 4 lines

drivers/media/platform/starfive/stf_camss.c new file +4 -0

+
+	stfcamss = devm_kzalloc(dev, sizeof(*stfcamss), GFP_KERNEL);
+	if (!stfcamss)
+		return -ENOMEM;

drivers/media/platform/starfive/stf_camss.c line 230

Quoted 6 lines

drivers/media/platform/starfive/stf_camss.c new file +6 -0

+
+	for (i = 0; i < ARRAY_SIZE(stfcamss->irq); ++i) {
+		stfcamss->irq[i] = platform_get_irq(pdev, i);
+		if (stfcamss->irq[i] < 0)
+			return dev_err_probe(&pdev->dev, stfcamss->irq[i],
+					     "Failed to get clock%d", i);

I think you meant "irq %u", not clock.

Quoted 26 lines

drivers/media/platform/starfive/stf_camss.c new file +26 -0

+	}
+
+	stfcamss->nclks = ARRAY_SIZE(stfcamss->sys_clk);
+	for (i = 0; i < stfcamss->nclks; ++i)
+		stfcamss->sys_clk[i].id = stfcamss_clocks[i];
+	ret = devm_clk_bulk_get(dev, stfcamss->nclks, stfcamss->sys_clk);
+	if (ret) {
+		dev_err(dev, "Failed to get clk controls\n");
+		return ret;
+	}
+
+	stfcamss->nrsts = ARRAY_SIZE(stfcamss->sys_rst);
+	for (i = 0; i < stfcamss->nrsts; ++i)
+		stfcamss->sys_rst[i].id = stfcamss_resets[i];
+	ret = devm_reset_control_bulk_get_shared(dev, stfcamss->nrsts,
+						 stfcamss->sys_rst);
+	if (ret) {
+		dev_err(dev, "Failed to get reset controls\n");
+		return ret;
+	}
+
+	ret = stfcamss_get_mem_res(pdev, stfcamss);
+	if (ret) {
+		dev_err(dev, "Could not map registers\n");
+		return ret;
+	}

drivers/media/platform/starfive/stf_camss.c line 266

Quoted 10 lines

drivers/media/platform/starfive/stf_camss.c new file +10 -0

+
+	stfcamss->dev = dev;
+	platform_set_drvdata(pdev, stfcamss);
+
+	v4l2_async_nf_init(&stfcamss->notifier);
+
+	num_subdevs = stfcamss_of_parse_ports(stfcamss);
+	if (num_subdevs < 0) {
+		dev_err(dev, "Failed to find subdevices\n");
+		return -ENODEV;

I would

		ret = -ENODEV;
		goto err_cleanup_notifier;

here.

Quoted 21 lines

drivers/media/platform/starfive/stf_camss.c new file +21 -0

+	}
+
+	stfcamss_mc_init(pdev, stfcamss);
+
+	ret = v4l2_device_register(stfcamss->dev, &stfcamss->v4l2_dev);
+	if (ret < 0) {
+		dev_err(dev, "Failed to register V4L2 device: %d\n", ret);
+		goto err_cleanup_notifier;
+	}
+
+	ret = media_device_register(&stfcamss->media_dev);
+	if (ret) {
+		dev_err(dev, "Failed to register media device: %d\n", ret);
+		goto err_unregister_device;
+	}
+
+	stfcamss->notifier.ops = &stfcamss_subdev_notifier_ops;
+	ret = v4l2_async_nf_register(&stfcamss->v4l2_dev, &stfcamss->notifier);
+	if (ret) {
+		dev_err(dev, "Failed to register async subdev nodes: %d\n",
+			ret);

drivers/media/platform/starfive/stf_camss.c line 291

Quoted 4 lines

drivers/media/platform/starfive/stf_camss.c new file +4 -0

+		goto err_unregister_media_dev;
+	}
+
+	pm_runtime_enable(dev);

You should enable runtime PM before registering the notifier, otherwise there's a race condition where userspace could start using the device before runtime PM gets enabled.

Quoted 48 lines

drivers/media/platform/starfive/stf_camss.c new file +48 -0

+
+	return 0;
+
+err_unregister_media_dev:
+	media_device_unregister(&stfcamss->media_dev);
+err_unregister_device:
+	v4l2_device_unregister(&stfcamss->v4l2_dev);
+err_cleanup_notifier:
+	v4l2_async_nf_cleanup(&stfcamss->notifier);
+	return ret;
+}
+
+/*
+ * stfcamss_remove - Remove STFCAMSS platform device
+ * @pdev: Pointer to STFCAMSS platform device
+ *
+ * Always returns 0.
+ */
+static int stfcamss_remove(struct platform_device *pdev)
+{
+	struct stfcamss *stfcamss = platform_get_drvdata(pdev);
+
+	v4l2_device_unregister(&stfcamss->v4l2_dev);
+	media_device_cleanup(&stfcamss->media_dev);
+	pm_runtime_disable(&pdev->dev);
+
+	return 0;
+}
+
+static const struct of_device_id stfcamss_of_match[] = {
+	{ .compatible = "starfive,jh7110-camss" },
+	{ /* sentinel */ },
+};
+
+MODULE_DEVICE_TABLE(of, stfcamss_of_match);
+
+static int __maybe_unused stfcamss_runtime_suspend(struct device *dev)
+{
+	struct stfcamss *stfcamss = dev_get_drvdata(dev);
+
+	reset_control_assert(stfcamss->sys_rst[STF_RST_ISP_TOP_AXI].rstc);
+	reset_control_assert(stfcamss->sys_rst[STF_RST_ISP_TOP_N].rstc);
+	clk_disable_unprepare(stfcamss->sys_clk[STF_CLK_ISP_AXI].clk);
+	clk_disable_unprepare(stfcamss->sys_clk[STF_CLK_ISPCORE_2X].clk);
+
+	return 0;
+}
+

drivers/media/platform/starfive/stf_camss.c line 345

Quoted 6 lines

drivers/media/platform/starfive/stf_camss.c new file +6 -0

+static int __maybe_unused stfcamss_runtime_resume(struct device *dev)
+{
+	struct stfcamss *stfcamss = dev_get_drvdata(dev);
+
+	clk_prepare_enable(stfcamss->sys_clk[STF_CLK_ISPCORE_2X].clk);
+	clk_prepare_enable(stfcamss->sys_clk[STF_CLK_ISP_AXI].clk);

Error handling is missing for those two calls.

Quoted 44 lines

drivers/media/platform/starfive/stf_camss.c new file +27 -0

+	reset_control_deassert(stfcamss->sys_rst[STF_RST_ISP_TOP_N].rstc);
+	reset_control_deassert(stfcamss->sys_rst[STF_RST_ISP_TOP_AXI].rstc);
+
+	return 0;
+}
+
+static const struct dev_pm_ops stfcamss_pm_ops = {
+	SET_RUNTIME_PM_OPS(stfcamss_runtime_suspend,
+			   stfcamss_runtime_resume,
+			   NULL)
+};
+
+static struct platform_driver stfcamss_driver = {
+	.probe = stfcamss_probe,
+	.remove = stfcamss_remove,
+	.driver = {
+		.name = DRV_NAME,
+		.pm = &stfcamss_pm_ops,
+		.of_match_table = of_match_ptr(stfcamss_of_match),
+	},
+};
+
+module_platform_driver(stfcamss_driver);
+
+MODULE_AUTHOR("StarFive Corporation");
+MODULE_DESCRIPTION("StarFive Camera Subsystem driver");
+MODULE_LICENSE("GPL");

drivers/media/platform/starfive/stf_camss.h new file +11 -0

new file mode 100644
index 000000000000..8f39cd65950c
@@ -0,0 +1,153 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * stf_camss.h
+ *
+ * Starfive Camera Subsystem driver
+ *
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ */
+
+#ifndef STF_CAMSS_H
+#define STF_CAMSS_H

drivers/media/platform/starfive/stf_camss.h line 16

Quoted 5 lines

drivers/media/platform/starfive/stf_camss.h new file +5 -0

+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/reset.h>
+#include <media/v4l2-device.h>

You should include media/media-device.h, media/media-entity.h and media/v4l2-async.h.

drivers/media/platform/starfive/stf_camss.h

Quoted 8 lines

drivers/media/platform/starfive/stf_camss.h new file +6 -0

+	struct device *dev;
 
+};
 
+
+#include "stf_common.h"
+
+#define DRV_NAME     "starfive-camss"

DRV_NAME is used in stfcamss_driver.driver.name only. I would drop the macro and write

.name = "starfive-camss",

above, it will be more readable.

Quoted 8 lines

drivers/media/platform/starfive/stf_camss.h new file +8 -0

+#define STF_DVP_NAME "stf_dvp"
+#define STF_CSI_NAME "cdns_csi2rx"
+#define STF_ISP_NAME "stf_isp"
+#define STF_VIN_NAME "stf_vin"
+
+#define STF_PAD_SINK   0
+#define STF_PAD_SRC    1
+#define STF_PADS_NUM   2

drivers/media/platform/starfive/stf_camss.h line 32

Quoted 4 lines

drivers/media/platform/starfive/stf_camss.h new file +4 -0

+
+enum port_num {
+	PORT_NUMBER_DVP_SENSOR = 0,
+	PORT_NUMBER_CSI2RX

Please name this stf_port_num and STF_PORT_*, to avoid namespace clashes. I would drop "NUMBER_", and name the two ports STF_PORT_DVP and STF_PORT_CSI2RX.

Quoted 27 lines

drivers/media/platform/starfive/stf_camss.h new file +27 -0

+};
+
+enum stf_clk {
+	STF_CLK_APB_FUNC = 0,
+	STF_CLK_WRAPPER_CLK_C,
+	STF_CLK_DVP_INV,
+	STF_CLK_AXIWR,
+	STF_CLK_MIPI_RX0_PXL,
+	STF_CLK_ISPCORE_2X,
+	STF_CLK_ISP_AXI,
+	STF_CLK_NUM
+};
+
+enum stf_rst {
+	STF_RST_WRAPPER_P = 0,
+	STF_RST_WRAPPER_C,
+	STF_RST_AXIRD,
+	STF_RST_AXIWR,
+	STF_RST_ISP_TOP_N,
+	STF_RST_ISP_TOP_AXI,
+	STF_RST_NUM
+};
+
+enum stf_irq {
+	STF_IRQ_VINWR = 0,
+	STF_IRQ_ISP,
+	STF_IRQ_ISPCSIL,

drivers/media/platform/starfive/stf_camss.h line 65

Quoted 6 lines

drivers/media/platform/starfive/stf_camss.h new file +6 -0

+	STF_IRQ_NUM
+};
+
+#define HOST_ENTITY_MAX		2
+
+struct host_data {

Same here, please name this stf_host_data.

Quoted 106 lines

drivers/media/platform/starfive/stf_camss.h new file +88 -0

+	struct media_entity *host_entity[HOST_ENTITY_MAX];
+};
+
+struct stfcamss {
+	struct v4l2_device v4l2_dev;
+	struct media_device media_dev;
+	struct media_pipeline pipe;
+	struct device *dev;
+	struct v4l2_async_notifier notifier;
+	struct host_data host_data;
+	void __iomem *syscon_base;
+	void __iomem *isp_base;
+	int irq[STF_IRQ_NUM];
+	struct clk_bulk_data sys_clk[STF_CLK_NUM];
+	int nclks;
+	struct reset_control_bulk_data sys_rst[STF_RST_NUM];
+	int nrsts;
+};
+
+struct stfcamss_async_subdev {
+	struct v4l2_async_subdev asd;  /* must be first */
+	enum port_num port;
+};
+
+static inline u32 stf_isp_reg_read(struct stfcamss *stfcamss, u32 reg)
+{
+	return ioread32(stfcamss->isp_base + reg);
+}
+
+static inline void stf_isp_reg_write(struct stfcamss *stfcamss,
+				     u32 reg, u32 val)
+{
+	iowrite32(val, stfcamss->isp_base + reg);
+}
+
+static inline void stf_isp_reg_write_delay(struct stfcamss *stfcamss,
+					   u32 reg, u32 val, u32 delay)
+{
+	iowrite32(val, stfcamss->isp_base + reg);
+	usleep_range(1000 * delay, 1000 * delay + 100);
+}
+
+static inline void stf_isp_reg_set_bit(struct stfcamss *stfcamss,
+				       u32 reg, u32 mask, u32 val)
+{
+	u32 value;
+
+	value = ioread32(stfcamss->isp_base + reg) & ~mask;
+	val &= mask;
+	val |= value;
+	iowrite32(val, stfcamss->isp_base + reg);
+}
+
+static inline void stf_isp_reg_set(struct stfcamss *stfcamss, u32 reg, u32 mask)
+{
+	iowrite32(ioread32(stfcamss->isp_base + reg) | mask,
+		  stfcamss->isp_base + reg);
+}
+
+static inline u32 stf_syscon_reg_read(struct stfcamss *stfcamss, u32 reg)
+{
+	return ioread32(stfcamss->syscon_base + reg);
+}
+
+static inline void stf_syscon_reg_write(struct stfcamss *stfcamss,
+					u32 reg, u32 val)
+{
+	iowrite32(val, stfcamss->syscon_base + reg);
+}
+
+static inline void stf_syscon_reg_set_bit(struct stfcamss *stfcamss,
+					  u32 reg, u32 bit_mask)
+{
+	u32 value;
+
+	value = ioread32(stfcamss->syscon_base + reg);
+	iowrite32(value | bit_mask, stfcamss->syscon_base + reg);
+}
+
+static inline void stf_syscon_reg_clear_bit(struct stfcamss *stfcamss,
+					    u32 reg, u32 bit_mask)
+{
+	u32 value;
+
+	value = ioread32(stfcamss->syscon_base + reg);
+	iowrite32(value & ~bit_mask, stfcamss->syscon_base + reg);
+}
+#endif /* STF_CAMSS_H */

drivers/media/platform/starfive/stf_common.h new file +12 -0

new file mode 100644
index 000000000000..e04fca2d9cd4
@@ -0,0 +1,18 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * stf_common.h
+ *
+ * StarFive Camera Subsystem - Common definitions
+ *
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ */
+
+#ifndef STF_COMMON_H
+#define STF_COMMON_H
+

drivers/media/platform/starfive/stf_common.h line 16

Quoted 4 lines

drivers/media/platform/starfive/stf_common.h new file +4 -0

+enum stf_subdev_type {
+	STF_SUBDEV_TYPE_VIN,
+	STF_SUBDEV_TYPE_ISP,
+};

Looking at the whole series, these two values are set in the struct vin_line and stf_isp_dev sdev_type fields, but they are never read. You can drop those two fields, drop this enum, and drop this header.

Quoted 3 lines

drivers/media/platform/starfive/stf_common.h new file +2 -0

+
+#endif /* STF_COMMON_H */

Add basic platform driver for StarFive Camera Subsystem.

Reviewed-by Bryan O'Donoghue <[email protected]>

Signed-off-by Jack Zhu <[email protected]>

Quoted 4 lines · Jack Zhu

Add basic platform driver for StarFive Camera Subsystem.

Reviewed-by Bryan O'Donoghue <[email protected]>

Signed-off-by Jack Zhu <[email protected]>

One thing here is the patch title and hence commit message.

"media: starfive:" doesn't really tell you this is a camera versus say a encoder/decoder.

I see you've used the name "camss" for your driver, which I think is a perfectly good and logical choice - however if you started to make commits along the lines of "media: camss" that would conflict with the qcom camss.

How about for starfive and qcom by the way, we do what Laurent did in

commit 3e8537b4c15172bfe1b285c3155ed5c37d523cd3
Author: Laurent Pinchart <[email protected]> Date:   Wed Dec 21 10:33:39 2022 +0100

    media: ti: omap3isp: Use media_pipeline_for_each_entity()

i.e. future StarFive commits for camera would be "media: starfive: camss" and similarly for Qualcomm "media: qualcomm: camss"

The point being the commit title namespace should be instructive and specific re: 3e8537b4c15172bfe1b285c3155ed5c37d523cd3

---
bod

Add video driver for StarFive Camera Subsystem.

Signed-off-by Jack Zhu <[email protected]>

---
 drivers/media/platform/starfive/Makefile    |   3 +-
 drivers/media/platform/starfive/stf_video.c | 864 ++++++++++++++++++++
 drivers/media/platform/starfive/stf_video.h |  95 +++
 3 files changed, 961 insertions(+), 1 deletion(-)
 create mode 100644 drivers/media/platform/starfive/stf_video.c
 create mode 100644 drivers/media/platform/starfive/stf_video.h

drivers/media/platform/starfive/Makefile +2 -1

index 796775fa52f4..d7a42b3a938c 100644
@@ -4,6 +4,7 @@
 #
 
 starfive-camss-objs += \
-		stf_camss.o
+		stf_camss.o \
+		stf_video.o
 
 obj-$(CONFIG_VIDEO_STARFIVE_CAMSS) += starfive-camss.o \

drivers/media/platform/starfive/stf_video.c new file +22 -0

new file mode 100644
index 000000000000..f027c7308dfb
@@ -0,0 +1,864 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * stf_video.c
+ *
+ * StarFive Camera Subsystem - V4L2 device node
+ *
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ */
+
+#include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-mc.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "stf_camss.h"
+#include "stf_video.h"
+
+static const struct stfcamss_format_info formats_pix_wr[] = {
+	{ MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_PIX_FMT_SRGGB10, 1,
+	  { { 1, 1 } }, { { 1, 1 } }, { 10 } },
+	{ MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_PIX_FMT_SGRBG10, 1,

drivers/media/platform/starfive/stf_video.c new file +260 -0

+{
+	int i;
+
+	for (i = 0; i < nformats; i++) {
+		if (formats[i].code == code &&
+		    formats[i].pixelformat == pixelformat)
+			return i;
+	}
+
+	for (i = 0; i < nformats; i++)
+		if (formats[i].code == code)
+			return i;
+
+	for (i = 0; i < nformats; i++)
+		if (formats[i].pixelformat == pixelformat)
+			return i;
+
+	return -EINVAL;
+}
+
+static int __video_try_fmt(struct stfcamss_video *video, struct v4l2_format *f)
+{
+	struct v4l2_pix_format *pix;
+	const struct stfcamss_format_info *fi;
+	u32 width, height;
+	u32 bpl;
+	int i;
+
+	pix = &f->fmt.pix;
+
+	for (i = 0; i < video->nformats; i++)
+		if (pix->pixelformat == video->formats[i].pixelformat)
+			break;
+
+	if (i == video->nformats)
+		i = 0; /* default format */
+
+	fi = &video->formats[i];
+	width = pix->width;
+	height = pix->height;
+
+	memset(pix, 0, sizeof(*pix));
+
+	pix->pixelformat = fi->pixelformat;
+	pix->width = clamp_t(u32, width, STFCAMSS_FRAME_MIN_WIDTH,
+			     STFCAMSS_FRAME_MAX_WIDTH);
+	pix->height = clamp_t(u32, height, STFCAMSS_FRAME_MIN_HEIGHT,
+			      STFCAMSS_FRAME_MAX_HEIGHT);
+	bpl = pix->width / fi->hsub[0].numerator *
+	      fi->hsub[0].denominator * fi->bpp[0] / 8;
+	bpl = ALIGN(bpl, video->bpl_alignment);
+	pix->bytesperline = bpl;
+	pix->sizeimage = pix->height / fi->vsub[0].numerator *
+			 fi->vsub[0].denominator * bpl;
+
+	pix->field = V4L2_FIELD_NONE;
+	pix->colorspace = V4L2_COLORSPACE_SRGB;
+	pix->flags = 0;
+	pix->ycbcr_enc =
+		V4L2_MAP_YCBCR_ENC_DEFAULT(pix->colorspace);
+	pix->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
+							  pix->colorspace,
+							  pix->ycbcr_enc);
+	pix->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(pix->colorspace);
+
+	return 0;
+}
+
+static int stf_video_init_format(struct stfcamss_video *video)
+{
+	int ret;
+	struct v4l2_format format = {
+		.type = video->type,
+		.fmt.pix = {
+			.width = 1920,
+			.height = 1080,
+			.pixelformat = V4L2_PIX_FMT_RGB565,
+		},
+	};
+
+	ret = __video_try_fmt(video, &format);
+
+	if (ret < 0)
+		return ret;
+
+	video->active_fmt = format;
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * Video queue operations
+ */
+
+static int video_queue_setup(struct vb2_queue *q,
+			     unsigned int *num_buffers,
+			     unsigned int *num_planes,
+			     unsigned int sizes[],
+			     struct device *alloc_devs[])
+{
+	struct stfcamss_video *video = vb2_get_drv_priv(q);
+	const struct v4l2_pix_format *format = &video->active_fmt.fmt.pix;
+
+	if (*num_planes) {
+		if (*num_planes != 1)
+			return -EINVAL;
+
+		if (sizes[0] < format->sizeimage)
+			return -EINVAL;
+	}
+
+	*num_planes = 1;
+	sizes[0] = format->sizeimage;
+	if (!sizes[0])
+		dev_err(video->stfcamss->dev,
+			"%s: error size is zero!!!\n", __func__);
+
+	dev_dbg(video->stfcamss->dev, "planes = %d, size = %d\n",
+		*num_planes, sizes[0]);
+
+	return 0;
+}
+
+static int video_buf_init(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct stfcamss_video *video = vb2_get_drv_priv(vb->vb2_queue);
+	struct stfcamss_buffer *buffer =
+		container_of(vbuf, struct stfcamss_buffer, vb);
+	const struct v4l2_pix_format *fmt = &video->active_fmt.fmt.pix;
+	dma_addr_t *paddr;
+
+	buffer->sizeimage = 0;
+
+	paddr = vb2_plane_cookie(vb, 0);
+	buffer->sizeimage = vb2_plane_size(vb, 0);
+	buffer->addr[0] = *paddr;
+	if (fmt->pixelformat == V4L2_PIX_FMT_NV12 ||
+	    fmt->pixelformat == V4L2_PIX_FMT_NV21 ||
+	    fmt->pixelformat == V4L2_PIX_FMT_NV16 ||
+	    fmt->pixelformat == V4L2_PIX_FMT_NV61)
+		buffer->addr[1] =
+			buffer->addr[0] + fmt->bytesperline * fmt->height;
+
+	return 0;
+}
+
+static int video_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct stfcamss_video *video = vb2_get_drv_priv(vb->vb2_queue);
+	const struct v4l2_pix_format *fmt = &video->active_fmt.fmt.pix;
+
+	if (fmt->sizeimage > vb2_plane_size(vb, 0)) {
+		dev_err(video->stfcamss->dev,
+			"sizeimage = %d, plane size = %d\n",
+			fmt->sizeimage, (unsigned int)vb2_plane_size(vb, 0));
+		return -EINVAL;
+	}
+	vb2_set_plane_payload(vb, 0, fmt->sizeimage);
+
+	vbuf->field = V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static void video_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct stfcamss_video *video = vb2_get_drv_priv(vb->vb2_queue);
+	struct stfcamss_buffer *buffer =
+		container_of(vbuf, struct stfcamss_buffer, vb);
+
+	video->ops->queue_buffer(video, buffer);
+}
+
+/*
+ * video_mbus_to_pix - Convert v4l2_mbus_framefmt to v4l2_pix_format
+ * @mbus: v4l2_mbus_framefmt format (input)
+ * @pix: v4l2_pix_format_mplane format (output)
+ * @f: a pointer to formats array element to be used for the conversion
+ * @alignment: bytesperline alignment value
+ *
+ * Fill the output pix structure with information from the input mbus format.
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int video_mbus_to_pix(const struct v4l2_mbus_framefmt *mbus,
+			     struct v4l2_pix_format *pix,
+			     const struct stfcamss_format_info *f,
+			     unsigned int alignment)
+{
+	u32 bytesperline;
+
+	memset(pix, 0, sizeof(*pix));
+	v4l2_fill_pix_format(pix, mbus);
+	pix->pixelformat = f->pixelformat;
+	bytesperline = pix->width / f->hsub[0].numerator *
+		       f->hsub[0].denominator * f->bpp[0] / 8;
+	bytesperline = ALIGN(bytesperline, alignment);
+	pix->bytesperline = bytesperline;
+	pix->sizeimage = pix->height / f->vsub[0].numerator *
+			 f->vsub[0].denominator * bytesperline;
+	return 0;
+}
+
+static struct v4l2_subdev *video_remote_subdev(struct stfcamss_video *video,
+					       u32 *pad)
+{
+	struct media_pad *remote;
+
+	remote = media_pad_remote_pad_first(&video->pad);
+
+	if (!remote || !is_media_entity_v4l2_subdev(remote->entity))
+		return NULL;
+
+	if (pad)
+		*pad = remote->index;
+
+	return media_entity_to_v4l2_subdev(remote->entity);
+}
+
+static int video_get_subdev_format(struct stfcamss_video *video,
+				   struct v4l2_format *format)
+{
+	struct v4l2_pix_format *pix = &video->active_fmt.fmt.pix;
+	struct v4l2_subdev_format fmt;
+	struct v4l2_subdev *subdev;
+	u32 pixelformat;
+	u32 pad;
+	int ret;
+
+	subdev = video_remote_subdev(video, &pad);
+	if (!subdev)
+		return -EPIPE;
+
+	fmt.pad = pad;
+	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+
+	ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
+	if (ret)
+		return ret;
+
+	pixelformat = pix->pixelformat;
+	ret = video_find_format(fmt.format.code, pixelformat,
+				video->formats, video->nformats);
+	if (ret < 0)
+		return ret;
+
+	format->type = video->type;
+
+	return video_mbus_to_pix(&fmt.format, &format->fmt.pix,
+				 &video->formats[ret], video->bpl_alignment);
+}
+
+static int video_check_format(struct stfcamss_video *video)
+{
+	struct v4l2_pix_format *pix = &video->active_fmt.fmt.pix;
+	struct v4l2_format format;
+	struct v4l2_pix_format *sd_pix = &format.fmt.pix;

drivers/media/platform/starfive/stf_video.c new file +40 -0

+	    pix->field != format.fmt.pix.field) {
+		dev_err(video->stfcamss->dev,
+			"%s, not match:\n"
+			"0x%x 0x%x\n0x%x 0x%x\n0x%x 0x%x\n",
+			__func__,
+			pix->pixelformat, sd_pix->pixelformat,
+			pix->height, sd_pix->height,
+			pix->field, format.fmt.pix.field);
+		return -EPIPE;
+	}
+
+	return 0;
+}
+
+static int video_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct stfcamss_video *video = vb2_get_drv_priv(q);
+	struct video_device *vdev = &video->vdev;
+	struct media_entity *entity;
+	struct media_pad *pad;
+	struct v4l2_subdev *subdev;
+	int ret;
+
+	ret = video_device_pipeline_start(vdev, &video->stfcamss->pipe);
+	if (ret < 0) {
+		dev_err(video->stfcamss->dev,
+			"Failed to media_pipeline_start: %d\n", ret);
+		return ret;
+	}
+
+	ret = video_check_format(video);
+	if (ret < 0)
+		goto error;
+	entity = &vdev->entity;
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+
+		pad = media_pad_remote_pad_first(pad);

drivers/media/platform/starfive/stf_video.c new file +50 -0

+	return 0;
+
+error:
+	video_device_pipeline_stop(vdev);
+	video->ops->flush_buffers(video, VB2_BUF_STATE_QUEUED);
+	return ret;
+}
+
+static void video_stop_streaming(struct vb2_queue *q)
+{
+	struct stfcamss_video *video = vb2_get_drv_priv(q);
+	struct video_device *vdev = &video->vdev;
+	struct media_entity *entity;
+	struct media_pad *pad;
+	struct v4l2_subdev *subdev;
+
+	entity = &vdev->entity;
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+
+		pad = media_pad_remote_pad_first(pad);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			break;
+
+		entity = pad->entity;
+		subdev = media_entity_to_v4l2_subdev(entity);
+
+		v4l2_subdev_call(subdev, video, s_stream, 0);
+	}
+
+	video_device_pipeline_stop(vdev);
+	video->ops->flush_buffers(video, VB2_BUF_STATE_ERROR);
+}
+
+static const struct vb2_ops stf_video_vb2_q_ops = {
+	.queue_setup     = video_queue_setup,
+	.wait_prepare    = vb2_ops_wait_prepare,
+	.wait_finish     = vb2_ops_wait_finish,
+	.buf_init        = video_buf_init,
+	.buf_prepare     = video_buf_prepare,
+	.buf_queue       = video_buf_queue,
+	.start_streaming = video_start_streaming,
+	.stop_streaming  = video_stop_streaming,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 ioctls
+ */

drivers/media/platform/starfive/stf_video.c new file +140 -0

+	return 0;
+}
+
+static int video_get_pfmt_by_index(struct stfcamss_video *video, int ndx)
+{
+	int i, j, k;
+
+	/* find index "i" of "k"th unique pixelformat in formats array */
+	k = -1;
+	for (i = 0; i < video->nformats; i++) {
+		for (j = 0; j < i; j++) {
+			if (video->formats[i].pixelformat ==
+			    video->formats[j].pixelformat)
+				break;
+		}
+
+		if (j == i)
+			k++;
+
+		if (k == ndx)
+			return i;
+	}
+
+	return -EINVAL;
+}
+
+static int video_get_pfmt_by_mcode(struct stfcamss_video *video, u32 mcode)
+{
+	int i;
+
+	for (i = 0; i < video->nformats; i++) {
+		if (video->formats[i].code == mcode)
+			return i;
+	}
+
+	return -EINVAL;
+}
+
+static int video_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f)
+{
+	struct stfcamss_video *video = video_drvdata(file);
+	int i;
+
+	if (f->type != video->type)
+		return -EINVAL;
+	if (f->index >= video->nformats)
+		return -EINVAL;
+
+	if (f->mbus_code) {
+		/* Each entry in formats[] table has unique mbus_code */
+		if (f->index > 0)
+			return -EINVAL;
+
+		i = video_get_pfmt_by_mcode(video, f->mbus_code);
+	} else {
+		i = video_get_pfmt_by_index(video, f->index);
+	}
+
+	if (i < 0)
+		return -EINVAL;
+
+	f->pixelformat = video->formats[i].pixelformat;
+
+	return 0;
+}
+
+static int video_enum_framesizes(struct file *file, void *fh,
+				 struct v4l2_frmsizeenum *fsize)
+{
+	struct stfcamss_video *video = video_drvdata(file);
+	int i;
+
+	if (fsize->index)
+		return -EINVAL;
+
+	for (i = 0; i < video->nformats; i++) {
+		if (video->formats[i].pixelformat == fsize->pixel_format)
+			break;
+	}
+
+	if (i == video->nformats)
+		return -EINVAL;
+
+	fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
+	fsize->stepwise.min_width = STFCAMSS_FRAME_MIN_WIDTH;
+	fsize->stepwise.max_width = STFCAMSS_FRAME_MAX_WIDTH;
+	fsize->stepwise.min_height = STFCAMSS_FRAME_MIN_HEIGHT;
+	fsize->stepwise.max_height = STFCAMSS_FRAME_MAX_HEIGHT;
+	fsize->stepwise.step_width = 1;
+	fsize->stepwise.step_height = 1;
+
+	return 0;
+}
+
+static int video_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct stfcamss_video *video = video_drvdata(file);
+
+	*f = video->active_fmt;
+
+	return 0;
+}
+
+static int video_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct stfcamss_video *video = video_drvdata(file);
+	int ret;
+
+	if (vb2_is_busy(&video->vb2_q))
+		return -EBUSY;
+
+	ret = __video_try_fmt(video, f);
+	if (ret < 0)
+		return ret;
+
+	video->active_fmt = *f;
+
+	return 0;
+}
+
+static int video_try_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct stfcamss_video *video = video_drvdata(file);
+
+	return __video_try_fmt(video, f);
+}
+
+static int video_enum_input(struct file *file, void *fh,
+			    struct v4l2_input *input)
+{
+	if (input->index > 0)
+		return -EINVAL;
+
+	strscpy(input->name, "camera", sizeof(input->name));
+	input->type = V4L2_INPUT_TYPE_CAMERA;
+
+	return 0;
+}
+
+static int video_g_input(struct file *file, void *fh, unsigned int *input)

drivers/media/platform/starfive/stf_video.c new file +15 -0

+
+static int video_g_parm(struct file *file, void *priv,
+			struct v4l2_streamparm *p)
+{
+	struct stfcamss_video *video = video_drvdata(file);
+	struct video_device *vdev = &video->vdev;
+	struct media_entity *entity;
+	struct v4l2_subdev *subdev;
+	struct media_pad *pad;
+	int ret, is_support = 0;
+
+	entity = &vdev->entity;
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))

drivers/media/platform/starfive/stf_video.c new file +89 -0

+		if (ret < 0 && ret != -ENOIOCTLCMD)
+			break;
+		if (!ret)
+			is_support = 1;
+	}
+
+	return is_support ? 0 : ret;
+}
+
+static int video_s_parm(struct file *file, void *priv,
+			struct v4l2_streamparm *p)
+{
+	struct stfcamss_video *video = video_drvdata(file);
+	struct video_device *vdev = &video->vdev;
+	struct media_entity *entity;
+	struct v4l2_subdev *subdev;
+	struct media_pad *pad;
+	struct v4l2_streamparm tmp_p;
+	int ret, is_support = 0;
+
+	entity = &vdev->entity;
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+
+		pad = media_pad_remote_pad_first(pad);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			break;
+
+		entity = pad->entity;
+		subdev = media_entity_to_v4l2_subdev(entity);
+
+		tmp_p = *p;
+		ret = v4l2_s_parm_cap(vdev, subdev, &tmp_p);
+		if (ret < 0 && ret != -ENOIOCTLCMD)
+			break;
+		if (!ret) {
+			is_support = 1;
+			*p = tmp_p;
+		}
+	}
+
+	return is_support ? 0 : ret;
+}
+
+static const struct v4l2_ioctl_ops stf_vid_vin_ioctl_ops = {
+	.vidioc_querycap                = video_querycap,
+	.vidioc_enum_fmt_vid_cap        = video_enum_fmt,
+	.vidioc_enum_fmt_vid_out        = video_enum_fmt,
+	.vidioc_enum_framesizes         = video_enum_framesizes,
+	.vidioc_g_fmt_vid_cap           = video_g_fmt,
+	.vidioc_s_fmt_vid_cap           = video_s_fmt,
+	.vidioc_try_fmt_vid_cap         = video_try_fmt,
+	.vidioc_g_fmt_vid_out           = video_g_fmt,
+	.vidioc_s_fmt_vid_out           = video_s_fmt,
+	.vidioc_try_fmt_vid_out         = video_try_fmt,
+	.vidioc_reqbufs                 = vb2_ioctl_reqbufs,
+	.vidioc_querybuf                = vb2_ioctl_querybuf,
+	.vidioc_qbuf                    = vb2_ioctl_qbuf,
+	.vidioc_expbuf                  = vb2_ioctl_expbuf,
+	.vidioc_dqbuf                   = vb2_ioctl_dqbuf,
+	.vidioc_create_bufs             = vb2_ioctl_create_bufs,
+	.vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
+	.vidioc_streamon                = vb2_ioctl_streamon,
+	.vidioc_streamoff               = vb2_ioctl_streamoff,
+	.vidioc_enum_input              = video_enum_input,
+	.vidioc_g_input                 = video_g_input,
+	.vidioc_s_input                 = video_s_input,
+	.vidioc_g_parm                  = video_g_parm,
+	.vidioc_s_parm                  = video_s_parm,
+};
+
+static const struct v4l2_ioctl_ops stf_vid_isp_ioctl_ops = {
+	.vidioc_querycap                = video_querycap,
+	.vidioc_enum_fmt_vid_cap        = video_enum_fmt,
+	.vidioc_enum_fmt_vid_out        = video_enum_fmt,
+	.vidioc_enum_framesizes         = video_enum_framesizes,
+	.vidioc_g_fmt_vid_cap           = video_g_fmt,
+	.vidioc_s_fmt_vid_cap           = video_s_fmt,
+	.vidioc_try_fmt_vid_cap         = video_try_fmt,
+	.vidioc_g_fmt_vid_out           = video_g_fmt,
+	.vidioc_s_fmt_vid_out           = video_s_fmt,
+	.vidioc_try_fmt_vid_out         = video_try_fmt,
+	.vidioc_reqbufs                 = vb2_ioctl_reqbufs,
+	.vidioc_querybuf                = vb2_ioctl_querybuf,
+	.vidioc_qbuf                    = vb2_ioctl_qbuf,
+	.vidioc_expbuf                  = vb2_ioctl_expbuf,
+	.vidioc_dqbuf                   = vb2_ioctl_dqbuf,

drivers/media/platform/starfive/stf_video.c new file +16 -0

+
+/* -----------------------------------------------------------------------------
+ * V4L2 file operations
+ */
+
+static int video_open(struct file *file)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct stfcamss_video *video = video_drvdata(file);
+	struct v4l2_fh *vfh;
+	int ret;
+
+	mutex_lock(&video->lock);
+
+	vfh = kzalloc(sizeof(*vfh), GFP_KERNEL);
+	if (!vfh) {

drivers/media/platform/starfive/stf_video.c new file +143 -0

+	if (ret < 0) {
+		dev_err(video->stfcamss->dev,
+			"Failed to power up pipeline: %d\n", ret);
+		goto error_pm_use;
+	}
+	mutex_unlock(&video->lock);
+
+	return 0;
+
+error_pm_use:
+	v4l2_fh_release(file);
+error_alloc:
+	mutex_unlock(&video->lock);
+	return ret;
+}
+
+static int video_release(struct file *file)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	vb2_fop_release(file);
+	v4l2_pipeline_pm_put(&vdev->entity);
+	file->private_data = NULL;
+
+	return 0;
+}
+
+static const struct v4l2_file_operations stf_vid_fops = {
+	.owner          = THIS_MODULE,
+	.unlocked_ioctl = video_ioctl2,
+	.open           = video_open,
+	.release        = video_release,
+	.poll           = vb2_fop_poll,
+	.mmap           = vb2_fop_mmap,
+	.read           = vb2_fop_read,
+};
+
+/* -----------------------------------------------------------------------------
+ * STFCAMSS video core
+ */
+
+static void stf_video_release(struct video_device *vdev)
+{
+	struct stfcamss_video *video = video_get_drvdata(vdev);
+
+	media_entity_cleanup(&vdev->entity);
+
+	mutex_destroy(&video->q_lock);
+	mutex_destroy(&video->lock);
+}
+
+int stf_video_register(struct stfcamss_video *video,
+		       struct v4l2_device *v4l2_dev, const char *name)
+{
+	struct video_device *vdev;
+	struct vb2_queue *q;
+	struct media_pad *pad = &video->pad;
+	int ret;
+
+	vdev = &video->vdev;
+
+	mutex_init(&video->q_lock);
+
+	q = &video->vb2_q;
+	q->drv_priv = video;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->ops = &stf_video_vb2_q_ops;
+	q->type = video->type;
+	q->io_modes = VB2_DMABUF | VB2_MMAP | VB2_READ;
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->buf_struct_size = sizeof(struct stfcamss_buffer);
+	q->dev = video->stfcamss->dev;
+	q->lock = &video->q_lock;
+	q->min_buffers_needed = STFCAMSS_MIN_BUFFERS;
+	ret = vb2_queue_init(q);
+	if (ret < 0) {
+		dev_err(video->stfcamss->dev,
+			"Failed to init vb2 queue: %d\n", ret);
+		goto err_vb2_init;
+	}
+
+	pad->flags = MEDIA_PAD_FL_SINK;
+	ret = media_entity_pads_init(&vdev->entity, 1, pad);
+	if (ret < 0) {
+		dev_err(video->stfcamss->dev,
+			"Failed to init video entity: %d\n", ret);
+		goto err_vb2_init;
+	}
+
+	mutex_init(&video->lock);
+
+	if (video->id == STF_V_LINE_WR) {
+		video->formats = formats_pix_wr;
+		video->nformats = ARRAY_SIZE(formats_pix_wr);
+		video->bpl_alignment = STFCAMSS_FRAME_WIDTH_ALIGN_8;
+		vdev->ioctl_ops = &stf_vid_vin_ioctl_ops;
+	} else {
+		video->formats = formats_pix_isp;
+		video->nformats = ARRAY_SIZE(formats_pix_isp);
+		video->bpl_alignment = STFCAMSS_FRAME_WIDTH_ALIGN_8;
+		vdev->ioctl_ops = &stf_vid_isp_ioctl_ops;
+	}
+
+	ret = stf_video_init_format(video);
+	if (ret < 0) {
+		dev_err(video->stfcamss->dev,
+			"Failed to init format: %d\n", ret);
+		goto err_vid_init_format;
+	}
+
+	vdev->fops = &stf_vid_fops;
+	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE;
+	vdev->vfl_dir = VFL_DIR_RX;
+	vdev->device_caps |= V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+	vdev->release = stf_video_release;
+	vdev->v4l2_dev = v4l2_dev;
+	vdev->queue = &video->vb2_q;
+	vdev->lock = &video->lock;
+	strscpy(vdev->name, name, sizeof(vdev->name));
+
+	ret = video_register_device(vdev, VFL_TYPE_VIDEO, video->id);
+	if (ret < 0) {
+		dev_err(video->stfcamss->dev,
+			"Failed to register video device: %d\n", ret);
+		goto err_vid_reg;
+	}
+
+	video_set_drvdata(vdev, video);
+	return 0;
+
+err_vid_reg:
+err_vid_init_format:
+	media_entity_cleanup(&vdev->entity);
+	mutex_destroy(&video->lock);
+err_vb2_init:
+	mutex_destroy(&video->q_lock);
+	return ret;
+}
+
+void stf_video_unregister(struct stfcamss_video *video)
+{
+	vb2_video_unregister_device(&video->vdev);
+}

drivers/media/platform/starfive/stf_video.h new file +2 -0

new file mode 100644
index 000000000000..63a9a7706a03
@@ -0,0 +1,95 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*

drivers/media/platform/starfive/stf_video.h new file +18 -0

+#include <linux/mutex.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-v4l2.h>
+
+#define STFCAMSS_FRAME_MIN_WIDTH		64
+#define STFCAMSS_FRAME_MAX_WIDTH		1920
+#define STFCAMSS_FRAME_MIN_HEIGHT		64
+#define STFCAMSS_FRAME_MAX_HEIGHT		1080
+#define STFCAMSS_FRAME_WIDTH_ALIGN_8		8
+#define STFCAMSS_FRAME_WIDTH_ALIGN_128		128
+#define STFCAMSS_MIN_BUFFERS			2
+
+#define STFCAMSS_MAX_ENTITY_NAME_LEN		27
+
+enum stf_v_line_id {

drivers/media/platform/starfive/stf_video.h new file +12 -0

+};
+
+struct fract {
+	u8 numerator;
+	u8 denominator;
+};
+
+/*
+ * struct stfcamss_format_info - ISP media bus format information
+ * @code: V4L2 media bus format code
+ * @pixelformat: V4L2 pixel format FCC identifier
+ * @planes: Number of planes

drivers/media/platform/starfive/stf_video.h new file +23 -0

+	struct v4l2_format active_fmt;
+	enum v4l2_buf_type type;
+	const struct stfcamss_video_ops *ops;
+	struct mutex lock;	 /* serialize device access */
+	struct mutex q_lock;	 /* protects the queue */
+	unsigned int bpl_alignment;
+	const struct stfcamss_format_info *formats;
+	unsigned int nformats;
+};
+
+struct stfcamss_video_ops {
+	int (*queue_buffer)(struct stfcamss_video *vid,
+			    struct stfcamss_buffer *buf);
+	int (*flush_buffers)(struct stfcamss_video *vid,
+			     enum vb2_buffer_state state);
+};
+
+int stf_video_register(struct stfcamss_video *video,
+		       struct v4l2_device *v4l2_dev, const char *name);
+
+void stf_video_unregister(struct stfcamss_video *video);
+
+#endif /* STF_VIDEO_H */

Hi Jack,

Thank you for the patch.

Quoted 52 lines · Jack Zhu

Add video driver for StarFive Camera Subsystem.

Signed-off-by Jack Zhu <[email protected]>

---
 drivers/media/platform/starfive/Makefile    |   3 +-
 drivers/media/platform/starfive/stf_video.c | 864 ++++++++++++++++++++
 drivers/media/platform/starfive/stf_video.h |  95 +++
 3 files changed, 961 insertions(+), 1 deletion(-)
 create mode 100644 drivers/media/platform/starfive/stf_video.c
 create mode 100644 drivers/media/platform/starfive/stf_video.h

drivers/media/platform/starfive/Makefile +2 -1

index 796775fa52f4..d7a42b3a938c 100644
@@ -4,6 +4,7 @@
 #
 
 starfive-camss-objs += \
-		stf_camss.o
+		stf_camss.o \
+		stf_video.o
 
 obj-$(CONFIG_VIDEO_STARFIVE_CAMSS) += starfive-camss.o \

drivers/media/platform/starfive/stf_video.c new file +22 -0

new file mode 100644
index 000000000000..f027c7308dfb
@@ -0,0 +1,864 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * stf_video.c
+ *
+ * StarFive Camera Subsystem - V4L2 device node
+ *
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ */
+
+#include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-mc.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "stf_camss.h"
+#include "stf_video.h"
+
+static const struct stfcamss_format_info formats_pix_wr[] = {
+	{ MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_PIX_FMT_SRGGB10, 1,
+	  { { 1, 1 } }, { { 1, 1 } }, { 10 } },
+	{ MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_PIX_FMT_SGRBG10, 1,

drivers/media/platform/starfive/stf_video.c line 32

Quoted 10 lines

drivers/media/platform/starfive/stf_video.c new file +10 -0

+	  { { 1, 1 } }, { { 1, 1 } }, { 10 } },
+	{ MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_PIX_FMT_SGBRG10, 1,
+	  { { 1, 1 } }, { { 1, 1 } }, { 10 } },
+	{ MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_PIX_FMT_SBGGR10, 1,
+	  { { 1, 1 } }, { { 1, 1 } }, { 10 } },
+};
+
+static const struct stfcamss_format_info formats_pix_isp[] = {
+	{ MEDIA_BUS_FMT_Y12_1X12, V4L2_PIX_FMT_NV12, 1,
+	  { { 1, 1 } }, { { 2, 3 } }, { 8 } },

The [hv]sub values look weird. NV12 subsamples by 2 in both directions for the chroma plane. The result of your bytesperline calculation is right before the chroma plane is subsampled by 2 horizontally, but contains both U and V components, so the number of bytes equals the width in pixels for both the luma and chroma planes. In the vertical direction, the calculation is fine too for NV12 as you're calculating sizeimage for both planes.

I would recommend something along the following lines:

struct stfcamss_format_info {
	u32 code;
	u32 pixelformat;
	u8 planes;
	u8 vsub[3];
	u8 bpp;
};

static const struct stfcamss_format_info formats_pix_isp[] = {
	{
		.code = MEDIA_BUS_FMT_Y12_1X12,
		.pixelformat = V4L2_PIX_FMT_NV12,
		.planes = 2,
		.vsub = { 1, 2 },
		.bpp = 8,
	},
};

{
	unsigned int bytesperline;
	unsigned int sizeimage = 0;

	bytesperline = width * info->bpp / 8;

	for (i = 0; i < info->planes; ++i)
		sizeimage += bytesperline * height / info->vsub[i];
}

Quoted 4 lines

drivers/media/platform/starfive/stf_video.c new file +4 -0

+};
+
+/* -----------------------------------------------------------------------------
+ * Helper functions

drivers/media/platform/starfive/stf_video.c line 41

Quoted 5 lines

drivers/media/platform/starfive/stf_video.c new file +5 -0

+ */
+
+static int video_find_format(u32 code, u32 pixelformat,
+			     const struct stfcamss_format_info *formats,
+			     unsigned int nformats)

I would pass the stfcamss_video pointer to this function instead of formats and nformats.

Quoted 264 lines

drivers/media/platform/starfive/stf_video.c new file +264 -0

+{
+	int i;
+
+	for (i = 0; i < nformats; i++) {
+		if (formats[i].code == code &&
+		    formats[i].pixelformat == pixelformat)
+			return i;
+	}
+
+	for (i = 0; i < nformats; i++)
+		if (formats[i].code == code)
+			return i;
+
+	for (i = 0; i < nformats; i++)
+		if (formats[i].pixelformat == pixelformat)
+			return i;
+
+	return -EINVAL;
+}
+
+static int __video_try_fmt(struct stfcamss_video *video, struct v4l2_format *f)
+{
+	struct v4l2_pix_format *pix;
+	const struct stfcamss_format_info *fi;
+	u32 width, height;
+	u32 bpl;
+	int i;
+
+	pix = &f->fmt.pix;
+
+	for (i = 0; i < video->nformats; i++)
+		if (pix->pixelformat == video->formats[i].pixelformat)
+			break;
+
+	if (i == video->nformats)
+		i = 0; /* default format */
+
+	fi = &video->formats[i];
+	width = pix->width;
+	height = pix->height;
+
+	memset(pix, 0, sizeof(*pix));
+
+	pix->pixelformat = fi->pixelformat;
+	pix->width = clamp_t(u32, width, STFCAMSS_FRAME_MIN_WIDTH,
+			     STFCAMSS_FRAME_MAX_WIDTH);
+	pix->height = clamp_t(u32, height, STFCAMSS_FRAME_MIN_HEIGHT,
+			      STFCAMSS_FRAME_MAX_HEIGHT);
+	bpl = pix->width / fi->hsub[0].numerator *
+	      fi->hsub[0].denominator * fi->bpp[0] / 8;
+	bpl = ALIGN(bpl, video->bpl_alignment);
+	pix->bytesperline = bpl;
+	pix->sizeimage = pix->height / fi->vsub[0].numerator *
+			 fi->vsub[0].denominator * bpl;
+
+	pix->field = V4L2_FIELD_NONE;
+	pix->colorspace = V4L2_COLORSPACE_SRGB;
+	pix->flags = 0;
+	pix->ycbcr_enc =
+		V4L2_MAP_YCBCR_ENC_DEFAULT(pix->colorspace);
+	pix->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
+							  pix->colorspace,
+							  pix->ycbcr_enc);
+	pix->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(pix->colorspace);
+
+	return 0;
+}
+
+static int stf_video_init_format(struct stfcamss_video *video)
+{
+	int ret;
+	struct v4l2_format format = {
+		.type = video->type,
+		.fmt.pix = {
+			.width = 1920,
+			.height = 1080,
+			.pixelformat = V4L2_PIX_FMT_RGB565,
+		},
+	};
+
+	ret = __video_try_fmt(video, &format);
+
+	if (ret < 0)
+		return ret;
+
+	video->active_fmt = format;
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * Video queue operations
+ */
+
+static int video_queue_setup(struct vb2_queue *q,
+			     unsigned int *num_buffers,
+			     unsigned int *num_planes,
+			     unsigned int sizes[],
+			     struct device *alloc_devs[])
+{
+	struct stfcamss_video *video = vb2_get_drv_priv(q);
+	const struct v4l2_pix_format *format = &video->active_fmt.fmt.pix;
+
+	if (*num_planes) {
+		if (*num_planes != 1)
+			return -EINVAL;
+
+		if (sizes[0] < format->sizeimage)
+			return -EINVAL;
+	}
+
+	*num_planes = 1;
+	sizes[0] = format->sizeimage;
+	if (!sizes[0])
+		dev_err(video->stfcamss->dev,
+			"%s: error size is zero!!!\n", __func__);
+
+	dev_dbg(video->stfcamss->dev, "planes = %d, size = %d\n",
+		*num_planes, sizes[0]);
+
+	return 0;
+}
+
+static int video_buf_init(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct stfcamss_video *video = vb2_get_drv_priv(vb->vb2_queue);
+	struct stfcamss_buffer *buffer =
+		container_of(vbuf, struct stfcamss_buffer, vb);
+	const struct v4l2_pix_format *fmt = &video->active_fmt.fmt.pix;
+	dma_addr_t *paddr;
+
+	buffer->sizeimage = 0;
+
+	paddr = vb2_plane_cookie(vb, 0);
+	buffer->sizeimage = vb2_plane_size(vb, 0);
+	buffer->addr[0] = *paddr;
+	if (fmt->pixelformat == V4L2_PIX_FMT_NV12 ||
+	    fmt->pixelformat == V4L2_PIX_FMT_NV21 ||
+	    fmt->pixelformat == V4L2_PIX_FMT_NV16 ||
+	    fmt->pixelformat == V4L2_PIX_FMT_NV61)
+		buffer->addr[1] =
+			buffer->addr[0] + fmt->bytesperline * fmt->height;
+
+	return 0;
+}
+
+static int video_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct stfcamss_video *video = vb2_get_drv_priv(vb->vb2_queue);
+	const struct v4l2_pix_format *fmt = &video->active_fmt.fmt.pix;
+
+	if (fmt->sizeimage > vb2_plane_size(vb, 0)) {
+		dev_err(video->stfcamss->dev,
+			"sizeimage = %d, plane size = %d\n",
+			fmt->sizeimage, (unsigned int)vb2_plane_size(vb, 0));
+		return -EINVAL;
+	}
+	vb2_set_plane_payload(vb, 0, fmt->sizeimage);
+
+	vbuf->field = V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static void video_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct stfcamss_video *video = vb2_get_drv_priv(vb->vb2_queue);
+	struct stfcamss_buffer *buffer =
+		container_of(vbuf, struct stfcamss_buffer, vb);
+
+	video->ops->queue_buffer(video, buffer);
+}
+
+/*
+ * video_mbus_to_pix - Convert v4l2_mbus_framefmt to v4l2_pix_format
+ * @mbus: v4l2_mbus_framefmt format (input)
+ * @pix: v4l2_pix_format_mplane format (output)
+ * @f: a pointer to formats array element to be used for the conversion
+ * @alignment: bytesperline alignment value
+ *
+ * Fill the output pix structure with information from the input mbus format.
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int video_mbus_to_pix(const struct v4l2_mbus_framefmt *mbus,
+			     struct v4l2_pix_format *pix,
+			     const struct stfcamss_format_info *f,
+			     unsigned int alignment)
+{
+	u32 bytesperline;
+
+	memset(pix, 0, sizeof(*pix));
+	v4l2_fill_pix_format(pix, mbus);
+	pix->pixelformat = f->pixelformat;
+	bytesperline = pix->width / f->hsub[0].numerator *
+		       f->hsub[0].denominator * f->bpp[0] / 8;
+	bytesperline = ALIGN(bytesperline, alignment);
+	pix->bytesperline = bytesperline;
+	pix->sizeimage = pix->height / f->vsub[0].numerator *
+			 f->vsub[0].denominator * bytesperline;
+	return 0;
+}
+
+static struct v4l2_subdev *video_remote_subdev(struct stfcamss_video *video,
+					       u32 *pad)
+{
+	struct media_pad *remote;
+
+	remote = media_pad_remote_pad_first(&video->pad);
+
+	if (!remote || !is_media_entity_v4l2_subdev(remote->entity))
+		return NULL;
+
+	if (pad)
+		*pad = remote->index;
+
+	return media_entity_to_v4l2_subdev(remote->entity);
+}
+
+static int video_get_subdev_format(struct stfcamss_video *video,
+				   struct v4l2_format *format)
+{
+	struct v4l2_pix_format *pix = &video->active_fmt.fmt.pix;
+	struct v4l2_subdev_format fmt;
+	struct v4l2_subdev *subdev;
+	u32 pixelformat;
+	u32 pad;
+	int ret;
+
+	subdev = video_remote_subdev(video, &pad);
+	if (!subdev)
+		return -EPIPE;
+
+	fmt.pad = pad;
+	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+
+	ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
+	if (ret)
+		return ret;
+
+	pixelformat = pix->pixelformat;
+	ret = video_find_format(fmt.format.code, pixelformat,
+				video->formats, video->nformats);
+	if (ret < 0)
+		return ret;
+
+	format->type = video->type;
+
+	return video_mbus_to_pix(&fmt.format, &format->fmt.pix,
+				 &video->formats[ret], video->bpl_alignment);
+}
+
+static int video_check_format(struct stfcamss_video *video)
+{
+	struct v4l2_pix_format *pix = &video->active_fmt.fmt.pix;
+	struct v4l2_format format;
+	struct v4l2_pix_format *sd_pix = &format.fmt.pix;
+	int ret;
+
+	sd_pix->pixelformat = pix->pixelformat;
+	ret = video_get_subdev_format(video, &format);