[PATCH v3 0/7] Add FSD CSIS support

[PATCH v3 0/7] Add FSD CSIS support

[Inbaraj E]

The Tesla FSD SoC includes a MIPI CSI-2 receiver IP core named CSIS
(Camera Serial Interface Slave) IP, which is compatible with the CSIS
IP present in NXP i.MX7 and i.MX8 SoC.

The Tesla FSD SoC CSIS IP version is 4.3. This version of the CSIS IP
bundles MIPI CSI-2 link controller and video capture interface. The MIPI
CSI-2 link operation are integrated into the imx-mipi-csis driver, while 
the video capture interface is implemented in the fsd-csis driver.

The Tesla FSD SoC contains 12 instances of CSIS IP and 3 D-PHYs, with 4
CSIS instances sharing a single D-PHY.

The D-PHY interface exposes only the reset through Samsung SYSREG
controller.

This patch series does the following:
1) Add the necessary DT-binding documents.
2) Refactor the imx-mipi-csis driver to support platform specific
clock names and interrupt handlers through device specific data
(struct mipi_csis_info).
3) Add support to dynamically select the VC from the subdevice.
4) Add Telsa FSD CSIS link controller support in imx-mipi-csis driver.
5) Introduce a new media driver(fsd-csis) for the Telsa FSD video
capture interface to capture frames.

These patches were tested on the FSD platform using the
capture_raw_frames application.

Changes since v1:
1. Addressed review comments from Laurent Pinchart to integrate the
with imx-mipi-csis.c to handle the CSIS and expose it as a subdev.

Here is the link to v1 patch for reference:
https://lore.kernel.org/linux-media/7e7832c16925386b771ddb7e00e08661115aa0ea.1668963790.git.sathya@samsung.com/ 

Changes since v2:
1. Improved commit description across all the patches.
2. Changed syscon prefix from samsung to tesla.
3. Changed config from VIDEO_FSD_CSIS to VIDEO_TESLA_FSD_CSIS.
4. Fixed pll clock leak in probe.
5. Rebased the patches on top of laurent pinchart patchset.

Here is the link to v2 patch for reference.
https://lore.kernel.org/linux-media/20250814140943.22531-1-inbaraj.e@samsung.com/

This patchset is dependent on following laurent pinchart patchset.
https://lore.kernel.org/linux-media/20250822002734.23516-1-laurent.pinchart@ideasonboard.com/

Inbaraj E (7):
  dt-bindings: media: nxp: Add support for FSD SoC
  dt-bindings: media: fsd: Add CSIS video capture interface
  media: imx-mipi-csis: Move clk to mipi_csis_info structure
  media: imx-mipi-csis: Move irq flag and handler to mipi_csis_info
    structure
  media: imx-mipi-csis: Add support for dynamic VC selection
  media: imx-mipi-csis: Add support for Telsa FSD CSIS.
  media: fsd-csis: Add FSD CSIS video capture interface support

 .../bindings/media/nxp,imx-mipi-csi2.yaml     |   91 +-
 .../bindings/media/tesla,fsd-csis-media.yaml  |   76 +
 MAINTAINERS                                   |    8 +
 drivers/media/platform/nxp/imx-mipi-csis.c    |  330 +++-
 drivers/media/platform/samsung/Kconfig        |    1 +
 drivers/media/platform/samsung/Makefile       |    1 +
 .../media/platform/samsung/fsd-csis/Kconfig   |   18 +
 .../media/platform/samsung/fsd-csis/Makefile  |    3 +
 .../platform/samsung/fsd-csis/fsd-csis.c      | 1693 +++++++++++++++++
 9 files changed, 2161 insertions(+), 60 deletions(-)
 create mode 100644 Documentation/devicetree/bindings/media/tesla,fsd-csis-media.yaml
 create mode 100644 drivers/media/platform/samsung/fsd-csis/Kconfig
 create mode 100644 drivers/media/platform/samsung/fsd-csis/Makefile
 create mode 100644 drivers/media/platform/samsung/fsd-csis/fsd-csis.c
-- 
2.49.0

[PATCH v3 1/7] dt-bindings: media: nxp: Add support for FSD SoC

[Inbaraj E]

The Tesla FSD CSIS link controller is used to configure MIPI CSI-2
Rx link operations.

The Tesla FSD SoC include a MIPI CSI-2 Rx IP core named CSIS, which is
compatible with the CSIS IP found in NXP i.MX7 and i.MX8 SoCs. Add the
compatible string "tesla,fsd-mipi-csi2" to support the MIPI CSI-2 Rx
link operation on the Tesla FSD SoC.

Signed-off-by: Inbaraj E <inbaraj.e@samsung.com>
---
 .../bindings/media/nxp,imx-mipi-csi2.yaml     | 91 +++++++++++++++----
 1 file changed, 71 insertions(+), 20 deletions(-)

diff --git a/Documentation/devicetree/bindings/media/nxp,imx-mipi-csi2.yaml b/Documentation/devicetree/bindings/media/nxp,imx-mipi-csi2.yaml
index 41ad5b84eaeb..39b9447fd40c 100644
--- a/Documentation/devicetree/bindings/media/nxp,imx-mipi-csi2.yaml
+++ b/Documentation/devicetree/bindings/media/nxp,imx-mipi-csi2.yaml
@@ -14,7 +14,7 @@ description: |-
   The NXP i.MX7 and i.MX8 families contain SoCs that include a MIPI CSI-2
   receiver IP core named CSIS. The IP core originates from Samsung, and may be
   compatible with some of the Exynos4 and S5P SoCs. i.MX7 SoCs use CSIS version
-  3.3, and i.MX8 SoCs use CSIS version 3.6.3.
+  3.3, i.MX8 SoCs use CSIS version 3.6.3 and FSD SoC uses CSIS version 4.3.
 
   While the CSI-2 receiver is separate from the MIPI D-PHY IP core, the PHY is
   completely wrapped by the CSIS and doesn't expose a control interface of its
@@ -26,6 +26,7 @@ properties:
       - enum:
           - fsl,imx7-mipi-csi2
           - fsl,imx8mm-mipi-csi2
+          - tesla,fsd-mipi-csi2
       - items:
           - enum:
               - fsl,imx8mp-mipi-csi2
@@ -38,24 +39,21 @@ properties:
     maxItems: 1
 
   clocks:
-    minItems: 3
-    items:
-      - description: The peripheral clock (a.k.a. APB clock)
-      - description: The external clock (optionally used as the pixel clock)
-      - description: The MIPI D-PHY clock
-      - description: The AXI clock
+    minItems: 2
+    maxItems: 4
 
   clock-names:
-    minItems: 3
-    items:
-      - const: pclk
-      - const: wrap
-      - const: phy
-      - const: axi
+    minItems: 2
+    maxItems: 4
 
   power-domains:
     maxItems: 1
 
+  tesla,syscon-csis:
+    $ref: /schemas/types.yaml#/definitions/phandle-array
+    description:
+      Syscon used to hold and release the reset of MIPI D-PHY
+
   phy-supply:
     description: The MIPI D-PHY digital power supply
 
@@ -93,7 +91,8 @@ properties:
             properties:
               data-lanes:
                 description:
-                  Note that 'fsl,imx7-mipi-csi2' only supports up to 2 data lines.
+                  Note that 'fsl,imx7-mipi-csi2' only supports up to 2 data
+                  lines.
                 minItems: 1
                 items:
                   - const: 1
@@ -115,7 +114,6 @@ required:
   - interrupts
   - clocks
   - clock-names
-  - power-domains
   - ports
 
 additionalProperties: false
@@ -124,20 +122,73 @@ allOf:
   - if:
       properties:
         compatible:
-          contains:
-            const: fsl,imx7-mipi-csi2
+          const: fsl,imx7-mipi-csi2
     then:
+      properties:
+        clocks:
+          items:
+            - description: The peripheral clock (a.k.a. APB clock)
+            - description: The external clock (optionally used as the pixel
+                clock)
+            - description: The MIPI D-PHY clock
+        clock-names:
+          items:
+            - const: pclk
+            - const: wrap
+            - const: phy
+        tesla,syscon-csis: false
+        fsl,num-channels: false
       required:
+        - power-domains
         - phy-supply
         - resets
-    else:
+
+  - if:
+      properties:
+        compatible:
+          const: fsl,imx8mm-mipi-csi2
+    then:
       properties:
         clocks:
-          minItems: 4
+          items:
+            - description: The peripheral clock (a.k.a. APB clock)
+            - description: The external clock (optionally used as the pixel
+                clock)
+            - description: The MIPI D-PHY clock
+            - description: The AXI clock
         clock-names:
-          minItems: 4
+          items:
+            - const: pclk
+            - const: wrap
+            - const: phy
+            - const: axi
+        tesla,syscon-csis: false
+        fsl,num-channels: false
         phy-supply: false
         resets: false
+      required:
+        - power-domains
+
+  - if:
+      properties:
+        compatible:
+          const: tesla,fsd-mipi-csi2
+    then:
+      properties:
+        clocks:
+          items:
+            - description: The peripheral clock (a.k.a. APB clock)
+            - description: The DMA clock
+        clocks-names:
+          items:
+            - const: pclk
+            - const: aclk
+        phy-supply: false
+        resets: false
+        power-domains: false
+      required:
+        - tesla,syscon-csis
+        - fsl,num-channels
 
 examples:
   - |
-- 
2.49.0

Re: [PATCH v3 1/7] dt-bindings: media: nxp: Add support for FSD SoC

[Rob Herring]

On Thu, Aug 28, 2025 at 02:29:05PM +0530, Inbaraj E wrote:
> The Tesla FSD CSIS link controller is used to configure MIPI CSI-2
> Rx link operations.
> 
> The Tesla FSD SoC include a MIPI CSI-2 Rx IP core named CSIS, which is
> compatible with the CSIS IP found in NXP i.MX7 and i.MX8 SoCs. Add the
> compatible string "tesla,fsd-mipi-csi2" to support the MIPI CSI-2 Rx
> link operation on the Tesla FSD SoC.
> 
> Signed-off-by: Inbaraj E <inbaraj.e@samsung.com>
> ---
>  .../bindings/media/nxp,imx-mipi-csi2.yaml     | 91 +++++++++++++++----
>  1 file changed, 71 insertions(+), 20 deletions(-)
> 
> diff --git a/Documentation/devicetree/bindings/media/nxp,imx-mipi-csi2.yaml b/Documentation/devicetree/bindings/media/nxp,imx-mipi-csi2.yaml
> index 41ad5b84eaeb..39b9447fd40c 100644
> --- a/Documentation/devicetree/bindings/media/nxp,imx-mipi-csi2.yaml
> +++ b/Documentation/devicetree/bindings/media/nxp,imx-mipi-csi2.yaml
> @@ -14,7 +14,7 @@ description: |-
>    The NXP i.MX7 and i.MX8 families contain SoCs that include a MIPI CSI-2
>    receiver IP core named CSIS. The IP core originates from Samsung, and may be
>    compatible with some of the Exynos4 and S5P SoCs. i.MX7 SoCs use CSIS version
> -  3.3, and i.MX8 SoCs use CSIS version 3.6.3.
> +  3.3, i.MX8 SoCs use CSIS version 3.6.3 and FSD SoC uses CSIS version 4.3.
>  
>    While the CSI-2 receiver is separate from the MIPI D-PHY IP core, the PHY is
>    completely wrapped by the CSIS and doesn't expose a control interface of its
> @@ -26,6 +26,7 @@ properties:
>        - enum:
>            - fsl,imx7-mipi-csi2
>            - fsl,imx8mm-mipi-csi2
> +          - tesla,fsd-mipi-csi2
>        - items:
>            - enum:
>                - fsl,imx8mp-mipi-csi2
> @@ -38,24 +39,21 @@ properties:
>      maxItems: 1
>  
>    clocks:
> -    minItems: 3
> -    items:
> -      - description: The peripheral clock (a.k.a. APB clock)
> -      - description: The external clock (optionally used as the pixel clock)
> -      - description: The MIPI D-PHY clock
> -      - description: The AXI clock
> +    minItems: 2
> +    maxItems: 4
>  
>    clock-names:
> -    minItems: 3
> -    items:
> -      - const: pclk
> -      - const: wrap
> -      - const: phy
> -      - const: axi
> +    minItems: 2
> +    maxItems: 4
>  
>    power-domains:
>      maxItems: 1
>  
> +  tesla,syscon-csis:
> +    $ref: /schemas/types.yaml#/definitions/phandle-array
> +    description:
> +      Syscon used to hold and release the reset of MIPI D-PHY

Reset? Sounds like you should be using the reset binding.

> +
>    phy-supply:
>      description: The MIPI D-PHY digital power supply
>  
> @@ -93,7 +91,8 @@ properties:
>              properties:
>                data-lanes:
>                  description:
> -                  Note that 'fsl,imx7-mipi-csi2' only supports up to 2 data lines.
> +                  Note that 'fsl,imx7-mipi-csi2' only supports up to 2 data
> +                  lines.

Reformatting should be a separate patch.

>                  minItems: 1
>                  items:
>                    - const: 1
> @@ -115,7 +114,6 @@ required:
>    - interrupts
>    - clocks
>    - clock-names
> -  - power-domains
>    - ports
>  
>  additionalProperties: false
> @@ -124,20 +122,73 @@ allOf:
>    - if:
>        properties:
>          compatible:
> -          contains:
> -            const: fsl,imx7-mipi-csi2
> +          const: fsl,imx7-mipi-csi2

'contains' was correct. It is more future proof when there is another 
SoC that is backwards compatible with imx7.

>      then:
> +      properties:
> +        clocks:
> +          items:
> +            - description: The peripheral clock (a.k.a. APB clock)
> +            - description: The external clock (optionally used as the pixel
> +                clock)
> +            - description: The MIPI D-PHY clock
> +        clock-names:
> +          items:
> +            - const: pclk
> +            - const: wrap
> +            - const: phy
> +        tesla,syscon-csis: false
> +        fsl,num-channels: false

blank line

>        required:
> +        - power-domains
>          - phy-supply
>          - resets
> -    else:
> +
> +  - if:
> +      properties:
> +        compatible:
> +          const: fsl,imx8mm-mipi-csi2
> +    then:
>        properties:
>          clocks:
> -          minItems: 4
> +          items:
> +            - description: The peripheral clock (a.k.a. APB clock)
> +            - description: The external clock (optionally used as the pixel
> +                clock)
> +            - description: The MIPI D-PHY clock
> +            - description: The AXI clock
>          clock-names:
> -          minItems: 4
> +          items:
> +            - const: pclk
> +            - const: wrap
> +            - const: phy
> +            - const: axi
> +        tesla,syscon-csis: false
> +        fsl,num-channels: false
>          phy-supply: false
>          resets: false

blank line

> +      required:
> +        - power-domains
> +
> +  - if:
> +      properties:
> +        compatible:
> +          const: tesla,fsd-mipi-csi2
> +    then:
> +      properties:
> +        clocks:
> +          items:
> +            - description: The peripheral clock (a.k.a. APB clock)
> +            - description: The DMA clock

Wouldn't this be the same as the "AXI clock"?

> +        clocks-names:
> +          items:
> +            - const: pclk
> +            - const: aclk
> +        phy-supply: false
> +        resets: false
> +        power-domains: false

blank line

> +      required:
> +        - tesla,syscon-csis
> +        - fsl,num-channels
>  
>  examples:
>    - |
> -- 
> 2.49.0
> 

[PATCH v3 2/7] dt-bindings: media: fsd: Add CSIS video capture interface

[Inbaraj E]

The Tesla FSD CSIS video capture interface is used to capture frames.

Signed-off-by: Inbaraj E <inbaraj.e@samsung.com>
---
 .../bindings/media/tesla,fsd-csis-media.yaml  | 76 +++++++++++++++++++
 1 file changed, 76 insertions(+)
 create mode 100644 Documentation/devicetree/bindings/media/tesla,fsd-csis-media.yaml

diff --git a/Documentation/devicetree/bindings/media/tesla,fsd-csis-media.yaml b/Documentation/devicetree/bindings/media/tesla,fsd-csis-media.yaml
new file mode 100644
index 000000000000..f045094ae539
--- /dev/null
+++ b/Documentation/devicetree/bindings/media/tesla,fsd-csis-media.yaml
@@ -0,0 +1,76 @@
+# SPDX-License-Identifier: (GPL-2.0-only OR BSD-2-Clause)
+%YAML 1.2
+---
+$id: http://devicetree.org/schemas/media/tesla,fsd-csis-media.yaml#
+$schema: http://devicetree.org/meta-schemas/core.yaml#
+
+title: Tesla FSD SoC MIPI CSI-2 video capture interface(Bridge device).
+
+maintainers:
+  - Inbaraj E <inbaraj.e@samsung.com>
+
+description:
+  The Tesla FSD CSIS has an internal video capture interface to capture
+  frames originating from the sensor. The power supply for the IP is
+  managed by custom firmware and is expected to remain enabled
+  permanently, so power supply control is not added in linux.
+
+properties:
+  compatible:
+    const: tesla,fsd-csis-media
+
+  reg:
+    maxItems: 1
+
+  interrupts:
+    maxItems: 1
+
+  clocks:
+    maxItems: 3
+
+  clock-names:
+    items:
+      - const: aclk
+      - const: pclk
+      - const: pll
+
+  iommus:
+    maxItems: 1
+
+  port:
+    $ref: /schemas/graph.yaml#/properties/port
+
+required:
+  - compatible
+  - reg
+  - interrupts
+  - clocks
+  - clock-names
+  - iommus
+  - port
+
+additionalProperties: false
+
+examples:
+  - |
+    #include <dt-bindings/clock/fsd-clk.h>
+    #include <dt-bindings/interrupt-controller/arm-gic.h>
+
+    csis0: csis@12641000 {
+        compatible = "tesla,fsd-csis-media";
+        reg = <0x12661000 0x44c>;
+        interrupts = <GIC_SPI 8 IRQ_TYPE_LEVEL_HIGH>;
+        clocks = <&clock_csi CAM_CSI0_0_IPCLKPORT_I_ACLK>,
+                 <&clock_csi CAM_CSI0_0_IPCLKPORT_I_PCLK>,
+                 <&clock_csi CAM_CSI_PLL>;
+        clock-names = "aclk", "pclk", "pll";
+        iommus = <&smmu_isp 0x0 0x0>;
+
+        port {
+            csi_in_0: endpoint {
+                remote-endpoint = <&mipi_csis_0_out>;
+            };
+        };
+    };
+
+...
-- 
2.49.0

Re: [PATCH v3 2/7] dt-bindings: media: fsd: Add CSIS video capture interface

[Krzysztof Kozlowski]

On Thu, Aug 28, 2025 at 02:29:06PM +0530, Inbaraj E wrote:
> The Tesla FSD CSIS video capture interface is used to capture frames.
> 
> Signed-off-by: Inbaraj E <inbaraj.e@samsung.com>
> ---
>  .../bindings/media/tesla,fsd-csis-media.yaml  | 76 +++++++++++++++++++
>  1 file changed, 76 insertions(+)
>  create mode 100644 Documentation/devicetree/bindings/media/tesla,fsd-csis-media.yaml
> 
> diff --git a/Documentation/devicetree/bindings/media/tesla,fsd-csis-media.yaml b/Documentation/devicetree/bindings/media/tesla,fsd-csis-media.yaml
> new file mode 100644
> index 000000000000..f045094ae539
> --- /dev/null
> +++ b/Documentation/devicetree/bindings/media/tesla,fsd-csis-media.yaml
> @@ -0,0 +1,76 @@
> +# SPDX-License-Identifier: (GPL-2.0-only OR BSD-2-Clause)
> +%YAML 1.2
> +---
> +$id: http://devicetree.org/schemas/media/tesla,fsd-csis-media.yaml#
> +$schema: http://devicetree.org/meta-schemas/core.yaml#
> +
> +title: Tesla FSD SoC MIPI CSI-2 video capture interface(Bridge device).

Drop final full stop. Also missing space before (


> +
> +maintainers:
> +  - Inbaraj E <inbaraj.e@samsung.com>
> +
> +description:
> +  The Tesla FSD CSIS has an internal video capture interface to capture
> +  frames originating from the sensor. The power supply for the IP is
> +  managed by custom firmware and is expected to remain enabled
> +  permanently, so power supply control is not added in linux.
> +
> +properties:
> +  compatible:
> +    const: tesla,fsd-csis-media
> +
> +  reg:
> +    maxItems: 1
> +
> +  interrupts:
> +    maxItems: 1
> +
> +  clocks:
> +    maxItems: 3
> +
> +  clock-names:
> +    items:
> +      - const: aclk
> +      - const: pclk
> +      - const: pll
> +
> +  iommus:
> +    maxItems: 1
> +
> +  port:
> +    $ref: /schemas/graph.yaml#/properties/port
> +
> +required:
> +  - compatible
> +  - reg
> +  - interrupts
> +  - clocks
> +  - clock-names
> +  - iommus
> +  - port
> +
> +additionalProperties: false
> +
> +examples:
> +  - |
> +    #include <dt-bindings/clock/fsd-clk.h>
> +    #include <dt-bindings/interrupt-controller/arm-gic.h>
> +
> +    csis0: csis@12641000 {

Incorrect unit address.

> +        compatible = "tesla,fsd-csis-media";
> +        reg = <0x12661000 0x44c>;


Best regards,
Krzysztof

[PATCH v3 3/7] media: imx-mipi-csis: Move clk to mipi_csis_info structure

[Inbaraj E]

The Clock names and number of Clocks in v3.3 and v3.6.3 differ v4.3. To
extend the driver for Tesla FSD SoC support, move the Clock names and
number of Clocks into the mipi_csis_info structure.

Signed-off-by: Inbaraj E <inbaraj.e@samsung.com>
---
 drivers/media/platform/nxp/imx-mipi-csis.c | 14 ++++++--------
 1 file changed, 6 insertions(+), 8 deletions(-)

diff --git a/drivers/media/platform/nxp/imx-mipi-csis.c b/drivers/media/platform/nxp/imx-mipi-csis.c
index b1136336a57f..cec035059445 100644
--- a/drivers/media/platform/nxp/imx-mipi-csis.c
+++ b/drivers/media/platform/nxp/imx-mipi-csis.c
@@ -304,6 +304,8 @@ static const struct mipi_csis_event mipi_csis_events[] = {
 #define MIPI_CSIS_NUM_EVENTS		ARRAY_SIZE(mipi_csis_events)
 #define MIPI_CSIS_NUM_ERROR_EVENTS	(MIPI_CSIS_NUM_EVENTS - 20)
 
+#define MIPI_CSIS_MAX_CLOCKS	4
+
 enum mipi_csis_clk {
 	MIPI_CSIS_CLK_PCLK,
 	MIPI_CSIS_CLK_WRAP,
@@ -311,13 +313,6 @@ enum mipi_csis_clk {
 	MIPI_CSIS_CLK_AXI,
 };
 
-static const char * const mipi_csis_clk_id[] = {
-	"pclk",
-	"wrap",
-	"phy",
-	"axi",
-};
-
 enum mipi_csis_version {
 	MIPI_CSIS_V3_3,
 	MIPI_CSIS_V3_6_3,
@@ -326,6 +321,7 @@ enum mipi_csis_version {
 struct mipi_csis_info {
 	enum mipi_csis_version version;
 	unsigned int num_clocks;
+	const char *clk_names[MIPI_CSIS_MAX_CLOCKS];
 };
 
 struct mipi_csis_device {
@@ -735,7 +731,7 @@ static int mipi_csis_clk_get(struct mipi_csis_device *csis)
 		return -ENOMEM;
 
 	for (i = 0; i < csis->info->num_clocks; i++)
-		csis->clks[i].id = mipi_csis_clk_id[i];
+		csis->clks[i].id = csis->info->clk_names[i];
 
 	ret = devm_clk_bulk_get(csis->dev, csis->info->num_clocks,
 				csis->clks);
@@ -1609,12 +1605,14 @@ static const struct of_device_id mipi_csis_of_match[] = {
 		.data = &(const struct mipi_csis_info){
 			.version = MIPI_CSIS_V3_3,
 			.num_clocks = 3,
+			.clk_names = {"pclk", "wrap", "phy"},
 		},
 	}, {
 		.compatible = "fsl,imx8mm-mipi-csi2",
 		.data = &(const struct mipi_csis_info){
 			.version = MIPI_CSIS_V3_6_3,
 			.num_clocks = 4,
+			.clk_names = {"pclk", "wrap", "phy", "axi"},
 		},
 	},
 	{ /* sentinel */ },
-- 
2.49.0

[PATCH v3 4/7] media: imx-mipi-csis: Move irq flag and handler to mipi_csis_info structure

[Inbaraj E]

The Tesla FSD CSIS IP has single IRQ line, which is shared between
imx-mipi-csis and fsd-csis drivers. To extend this driver for Tesla FSD
SoC support, move the IRQ flag and IRQ handler into the device
data(structure mipi_csis_info).

Signed-off-by: Inbaraj E <inbaraj.e@samsung.com>
---
 drivers/media/platform/nxp/imx-mipi-csis.c | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/drivers/media/platform/nxp/imx-mipi-csis.c b/drivers/media/platform/nxp/imx-mipi-csis.c
index cec035059445..2443906377bd 100644
--- a/drivers/media/platform/nxp/imx-mipi-csis.c
+++ b/drivers/media/platform/nxp/imx-mipi-csis.c
@@ -322,6 +322,8 @@ struct mipi_csis_info {
 	enum mipi_csis_version version;
 	unsigned int num_clocks;
 	const char *clk_names[MIPI_CSIS_MAX_CLOCKS];
+	unsigned int irq_flag;
+	irq_handler_t irq_handler;
 };
 
 struct mipi_csis_device {
@@ -1532,7 +1534,7 @@ static int mipi_csis_probe(struct platform_device *pdev)
 	mipi_csis_phy_reset(csis);
 
 	/* Now that the hardware is initialized, request the interrupt. */
-	ret = devm_request_irq(dev, irq, mipi_csis_irq_handler, 0,
+	ret = devm_request_irq(dev, irq, csis->info->irq_handler, csis->info->irq_flag,
 			       dev_name(dev), csis);
 	if (ret) {
 		dev_err(dev, "Interrupt request failed\n");
@@ -1606,6 +1608,8 @@ static const struct of_device_id mipi_csis_of_match[] = {
 			.version = MIPI_CSIS_V3_3,
 			.num_clocks = 3,
 			.clk_names = {"pclk", "wrap", "phy"},
+			.irq_flag = 0,
+			.irq_handler = mipi_csis_irq_handler,
 		},
 	}, {
 		.compatible = "fsl,imx8mm-mipi-csi2",
@@ -1613,6 +1617,8 @@ static const struct of_device_id mipi_csis_of_match[] = {
 			.version = MIPI_CSIS_V3_6_3,
 			.num_clocks = 4,
 			.clk_names = {"pclk", "wrap", "phy", "axi"},
+			.irq_flag = 0,
+			.irq_handler = mipi_csis_irq_handler,
 		},
 	},
 	{ /* sentinel */ },
-- 
2.49.0

[PATCH v3 5/7] media: imx-mipi-csis: Add support for dynamic VC selection

[Inbaraj E]

The existing implementation configures VC0 by default for streaming.
This patch adds support to obtain the VC dynamically from the
subdevice(Sensor) through the get_frame_desc() operation and configure
the corresponding VC when starting the stream.

If get_frame_desc() is not implemented by the subdevice, VC0 will be
selected by default for configuration and streaming.

Signed-off-by: Inbaraj E <inbaraj.e@samsung.com>
---
 drivers/media/platform/nxp/imx-mipi-csis.c | 61 ++++++++++++++++++----
 1 file changed, 51 insertions(+), 10 deletions(-)

diff --git a/drivers/media/platform/nxp/imx-mipi-csis.c b/drivers/media/platform/nxp/imx-mipi-csis.c
index 2443906377bd..b7ab441cc78a 100644
--- a/drivers/media/platform/nxp/imx-mipi-csis.c
+++ b/drivers/media/platform/nxp/imx-mipi-csis.c
@@ -334,6 +334,7 @@ struct mipi_csis_device {
 	struct regulator *mipi_phy_regulator;
 
 	const struct mipi_csis_info *info;
+	unsigned int vc;
 	unsigned int num_channels;
 
 	struct v4l2_subdev sd;
@@ -585,7 +586,7 @@ static void __mipi_csis_set_format(struct mipi_csis_device *csis,
 	u32 val;
 
 	/* Color format */
-	val = mipi_csis_read(csis, MIPI_CSIS_ISP_CONFIG_CH(0));
+	val = mipi_csis_read(csis, MIPI_CSIS_ISP_CONFIG_CH(csis->vc));
 	val &= ~(MIPI_CSIS_ISPCFG_PARALLEL | MIPI_CSIS_ISPCFG_PIXEL_MODE_MASK |
 		 MIPI_CSIS_ISPCFG_DATAFORMAT_MASK);
 
@@ -606,10 +607,10 @@ static void __mipi_csis_set_format(struct mipi_csis_device *csis,
 		val |= MIPI_CSIS_ISPCFG_PIXEL_MODE_DUAL;
 
 	val |= MIPI_CSIS_ISPCFG_DATAFORMAT(csis_fmt->data_type);
-	mipi_csis_write(csis, MIPI_CSIS_ISP_CONFIG_CH(0), val);
+	mipi_csis_write(csis, MIPI_CSIS_ISP_CONFIG_CH(csis->vc), val);
 
 	/* Pixel resolution */
-	mipi_csis_write(csis, MIPI_CSIS_ISP_RESOL_CH(0),
+	mipi_csis_write(csis, MIPI_CSIS_ISP_RESOL_CH(csis->vc),
 			MIPI_CSIS_ISP_RESOL_VRESOL(format->height) |
 			MIPI_CSIS_ISP_RESOL_HRESOL(format->width));
 }
@@ -683,14 +684,14 @@ static void mipi_csis_set_params(struct mipi_csis_device *csis,
 			MIPI_CSIS_DPHY_CMN_CTRL_HSSETTLE(csis->hs_settle) |
 			MIPI_CSIS_DPHY_CMN_CTRL_CLKSETTLE(csis->clk_settle));
 
-	mipi_csis_write(csis, MIPI_CSIS_ISP_SYNC_CH(0),
+	mipi_csis_write(csis, MIPI_CSIS_ISP_SYNC_CH(csis->vc),
 			MIPI_CSIS_ISP_SYNC_HSYNC_LINTV(0) |
 			MIPI_CSIS_ISP_SYNC_VSYNC_SINTV(0) |
 			MIPI_CSIS_ISP_SYNC_VSYNC_EINTV(0));
 
 	val = mipi_csis_read(csis, MIPI_CSIS_CLK_CTRL);
-	val |= MIPI_CSIS_CLK_CTRL_WCLK_SRC(0);
-	val |= MIPI_CSIS_CLK_CTRL_CLKGATE_TRAIL(0, 15);
+	val |= MIPI_CSIS_CLK_CTRL_WCLK_SRC(csis->vc);
+	val |= MIPI_CSIS_CLK_CTRL_CLKGATE_TRAIL(csis->vc, 15);
 	val &= ~MIPI_CSIS_CLK_CTRL_CLKGATE_EN_MSK;
 	mipi_csis_write(csis, MIPI_CSIS_CLK_CTRL, val);
 
@@ -707,7 +708,7 @@ static void mipi_csis_set_params(struct mipi_csis_device *csis,
 	/* Update the shadow register. */
 	val = mipi_csis_read(csis, MIPI_CSIS_CMN_CTRL);
 	mipi_csis_write(csis, MIPI_CSIS_CMN_CTRL,
-			val | MIPI_CSIS_CMN_CTRL_UPDATE_SHADOW(0) |
+			val | MIPI_CSIS_CMN_CTRL_UPDATE_SHADOW(csis->vc) |
 			MIPI_CSIS_CMN_CTRL_UPDATE_SHADOW_CTRL);
 }
 
@@ -778,7 +779,7 @@ static void mipi_csis_queue_event_sof(struct mipi_csis_device *csis)
 	};
 	u32 frame;
 
-	frame = mipi_csis_read(csis, MIPI_CSIS_FRAME_COUNTER_CH(0));
+	frame = mipi_csis_read(csis, MIPI_CSIS_FRAME_COUNTER_CH(csis->vc));
 	event.u.frame_sync.frame_sequence = frame;
 	v4l2_event_queue(csis->sd.devnode, &event);
 }
@@ -810,7 +811,7 @@ static irqreturn_t mipi_csis_irq_handler(int irq, void *dev_id)
 		}
 	}
 
-	if (status & MIPI_CSIS_INT_SRC_FRAME_START(0))
+	if (status & MIPI_CSIS_INT_SRC_FRAME_START(csis->vc))
 		mipi_csis_queue_event_sof(csis);
 
 	spin_unlock_irqrestore(&csis->slock, flags);
@@ -1191,7 +1192,7 @@ static int mipi_csis_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
 
 	entry->flags = 0;
 	entry->pixelcode = csis_fmt->code;
-	entry->bus.csi2.vc = 0;
+	entry->bus.csi2.vc = csis->vc;
 	entry->bus.csi2.dt = csis_fmt->data_type;
 
 	return 0;
@@ -1269,6 +1270,39 @@ static const struct v4l2_subdev_internal_ops mipi_csis_internal_ops = {
 	.init_state		= mipi_csis_init_state,
 };
 
+static int mipi_csis_get_vc(struct mipi_csis_device *csis)
+{
+	struct v4l2_mbus_frame_desc fd = { };
+	int ret;
+
+	ret = v4l2_subdev_call(csis->source.sd, pad, get_frame_desc, csis->source.pad->index, &fd);
+	if (ret < 0 && ret != -ENOIOCTLCMD) {
+		dev_err(csis->dev, "get_frame_desc failed on source subdev\n");
+		return ret;
+	}
+
+	/* If remote subdev does not implement .get_frame_desc default to VC0 */
+	if (ret == -ENOIOCTLCMD)
+		return 0;
+
+	if (fd.type != V4L2_MBUS_FRAME_DESC_TYPE_CSI2) {
+		dev_err(csis->dev, "get_frame_desc returned invalid bus type %d\n", fd.type);
+		return -EINVAL;
+	}
+
+	if (!fd.num_entries) {
+		dev_err(csis->dev, "get_frame_desc returned zero enteries\n");
+		return -EINVAL;
+	}
+
+	if (fd.entry[0].bus.csi2.vc >= csis->num_channels) {
+		dev_err(csis->dev, "get_frame_desc returned invalid virtual channel\n");
+		return -EINVAL;
+	}
+
+	return fd.entry[0].bus.csi2.vc;
+}
+
 /* -----------------------------------------------------------------------------
  * Media entity operations
  */
@@ -1296,6 +1330,13 @@ static int mipi_csis_link_setup(struct media_entity *entity,
 
 		csis->source.sd = remote_sd;
 		csis->source.pad = remote_pad;
+
+		ret = mipi_csis_get_vc(csis);
+
+		if (ret < 0)
+			return -EBUSY;
+
+		csis->vc = ret;
 	} else {
 		csis->source.sd = NULL;
 		csis->source.pad = NULL;
-- 
2.49.0

[PATCH v3 6/7] media: imx-mipi-csis: Add support for Telsa FSD CSIS.

[Inbaraj E]

The Telsa FSD SoC features a newer version(v4.3) of the CSI-2 receiver
IP, similar to the one found in the i.MX7 and i.MX8MM, with the
following differences.

- Ability to select any one VC for streaming from the four available
VCs.
- Built-in DMA support, which is implemented by the fsd-csis driver.

Signed-off-by: Inbaraj E <inbaraj.e@samsung.com>
---
 drivers/media/platform/nxp/imx-mipi-csis.c | 256 +++++++++++++++++++--
 1 file changed, 231 insertions(+), 25 deletions(-)

diff --git a/drivers/media/platform/nxp/imx-mipi-csis.c b/drivers/media/platform/nxp/imx-mipi-csis.c
index b7ab441cc78a..07ce312a2ed5 100644
--- a/drivers/media/platform/nxp/imx-mipi-csis.c
+++ b/drivers/media/platform/nxp/imx-mipi-csis.c
@@ -35,6 +35,8 @@
 #include <media/v4l2-fwnode.h>
 #include <media/v4l2-mc.h>
 #include <media/v4l2-subdev.h>
+#include <linux/mfd/syscon.h>
+#include <linux/regmap.h>
 
 #define CSIS_DRIVER_NAME			"imx-mipi-csis"
 
@@ -45,6 +47,9 @@
 #define MIPI_CSIS_DEF_PIX_WIDTH			640
 #define MIPI_CSIS_DEF_PIX_HEIGHT		480
 
+/* CSIS V4_3 SYSREG macros */
+#define FSD_NO_CSI_PER_PHY_V4_3			4
+#define FSD_CSIS_RESETEN_DPHY_MASK_V4_3(phy)	BIT_MASK(phy)
 /* Register map definition */
 
 /* CSIS version */
@@ -53,19 +58,22 @@
 #define MIPI_CSIS_VERSION_IMX8MP		0x03060301
 
 /* CSIS common control */
-#define MIPI_CSIS_CMN_CTRL			0x04
-#define MIPI_CSIS_CMN_CTRL_UPDATE_SHADOW(n)	BIT((n) + 16)
-#define MIPI_CSIS_CMN_CTRL_INTERLEAVE_MODE_NONE	(0 << 10)
-#define MIPI_CSIS_CMN_CTRL_INTERLEAVE_MODE_DT	(1 << 10)
-#define MIPI_CSIS_CMN_CTRL_LANE_NUMBER(n)	((n) << 8)
-#define MIPI_CSIS_CMN_CTRL_LANE_NUMBER_MASK	(3 << 8)
-#define MIPI_CSIS_CMN_CTRL_UPDATE_SHADOW_CTRL	BIT(2)
-#define MIPI_CSIS_CMN_CTRL_SW_RESET		BIT(1)
-#define MIPI_CSIS_CMN_CTRL_CSI_EN		BIT(0)
+#define MIPI_CSIS_CMN_CTRL				0x04
+#define MIPI_CSIS_CMN_CTRL_UPDATE_SHADOW(n)		BIT((n) + 16)
+#define MIPI_CSIS_CMN_CTRL_DESKEW_ENABLE		BIT(12)
+#define MIPI_CSIS_CMN_CTRL_INTERLEAVE_MODE_NONE		(0 << 10)
+#define MIPI_CSIS_CMN_CTRL_INTERLEAVE_MODE_DT		(1 << 10)
+#define MIPI_CSIS_CMN_CTRL_INTERLEAVE_MODE_VC_AND_DT	(3 << 10)
+#define MIPI_CSIS_CMN_CTRL_LANE_NUMBER(n)		((n) << 8)
+#define MIPI_CSIS_CMN_CTRL_LANE_NUMBER_MASK		(3 << 8)
+#define MIPI_CSIS_CMN_CTRL_UPDATE_SHADOW_CTRL		BIT(2)
+#define MIPI_CSIS_CMN_CTRL_SW_RESET			BIT(1)
+#define MIPI_CSIS_CMN_CTRL_CSI_EN			BIT(0)
 
 /* CSIS clock control */
 #define MIPI_CSIS_CLK_CTRL			0x08
 #define MIPI_CSIS_CLK_CTRL_CLKGATE_TRAIL(n, x)	((x) << ((n) * 4 + 16))
+#define MIPI_CSIS_CLK_CTRL_CLKGATE_EN(n)	(1 << ((n) + 4))
 #define MIPI_CSIS_CLK_CTRL_CLKGATE_EN_MSK	(0xf << 4)
 #define MIPI_CSIS_CLK_CTRL_WCLK_SRC(n)		BIT(n)
 
@@ -107,6 +115,12 @@
 #define MIPI_CSIS_INT_SRC_ERR_ID		BIT(0)
 #define MIPI_CSIS_INT_SRC_ERRORS		0xfffff
 
+/* CSIS Interrupt mask1 */
+#define MIPI_CSIS_INT_MSK1			0x18
+
+/* CSIS Interrupt source1 */
+#define MIPI_CSIS_INT_SRC1			0x1C
+
 /* D-PHY status control */
 #define MIPI_CSIS_DPHY_STATUS			0x20
 #define MIPI_CSIS_DPHY_STATUS_ULPS_DAT		BIT(8)
@@ -120,8 +134,9 @@
 #define MIPI_CSIS_DPHY_CMN_CTRL_HSSETTLE_MASK	GENMASK(31, 24)
 #define MIPI_CSIS_DPHY_CMN_CTRL_CLKSETTLE(n)	((n) << 22)
 #define MIPI_CSIS_DPHY_CMN_CTRL_CLKSETTLE_MASK	GENMASK(23, 22)
-#define MIPI_CSIS_DPHY_CMN_CTRL_S_DPDN_SWAP_CLK	BIT(6)
-#define MIPI_CSIS_DPHY_CMN_CTRL_S_DPDN_SWAP_DAT	BIT(5)
+#define MIPI_CSIS_DPHY_CMN_CTRL_S_BYTE_CLK_EN	BIT(21)
+#define MIPI_CSIS_DPHY_CMN_CTRL_DPDN_SWAP_CLK	BIT(6)
+#define MIPI_CSIS_DPHY_CMN_CTRL_DPDN_SWAP_DAT	BIT(5)
 #define MIPI_CSIS_DPHY_CMN_CTRL_ENABLE_DAT	BIT(1)
 #define MIPI_CSIS_DPHY_CMN_CTRL_ENABLE_CLK	BIT(0)
 #define MIPI_CSIS_DPHY_CMN_CTRL_ENABLE		(0x1f << 0)
@@ -167,7 +182,10 @@
 /* D-PHY Slave Control register Low */
 #define MIPI_CSIS_DPHY_SCTRL_L			0x38
 /* D-PHY Slave Control register High */
-#define MIPI_CSIS_DPHY_SCTRL_H			0x3c
+#define MIPI_CSIS_DPHY_SCTRL_H					0x3c
+#define MIPI_CSIS_DPHY_SCTRL_H_SKEW_CAL_MAX_SKEW_CODE_CTRL	(0x24 << 2)
+#define MIPI_CSIS_DPHY_SCTRL_H_SKEW_CAL_MAX_SKEW_CODE_CTRL_MASK GENMASK(7, 2)
+#define MIPI_CSIS_DPHY_SCTRL_H_SKEW_CAL_EN			BIT(1)
 
 /* ISP Configuration register */
 #define MIPI_CSIS_ISP_CONFIG_CH(n)		(0x40 + (n) * 0x10)
@@ -222,6 +240,12 @@
 
 #define MIPI_CSIS_FRAME_COUNTER_CH(n)		(0x0100 + (n) * 4)
 
+/* VC Passing register */
+#define MIPI_CSIS_VC_PASSING_REG	0x120
+#define MIPI_CSIS_VC_PASSING(n)		((n) << 8)
+#define MIPI_CSIS_VC_PASSING_MASK	GENMASK(9, 8)
+#define MIPI_CSIS_VC_PASSING_EN		BIT(7)
+
 /* Non-image packet data buffers */
 #define MIPI_CSIS_PKTDATA_ODD			0x2000
 #define MIPI_CSIS_PKTDATA_EVEN			0x3000
@@ -303,6 +327,7 @@ static const struct mipi_csis_event mipi_csis_events[] = {
 
 #define MIPI_CSIS_NUM_EVENTS		ARRAY_SIZE(mipi_csis_events)
 #define MIPI_CSIS_NUM_ERROR_EVENTS	(MIPI_CSIS_NUM_EVENTS - 20)
+#define MIPI_CSIS_INT_SRC_NUM_EVENTS_V4_3	17
 
 #define MIPI_CSIS_MAX_CLOCKS	4
 
@@ -316,6 +341,7 @@ enum mipi_csis_clk {
 enum mipi_csis_version {
 	MIPI_CSIS_V3_3,
 	MIPI_CSIS_V3_6_3,
+	MIPI_CSIS_V4_3,
 };
 
 struct mipi_csis_info {
@@ -329,6 +355,8 @@ struct mipi_csis_info {
 struct mipi_csis_device {
 	struct device *dev;
 	void __iomem *regs;
+	struct regmap *sysreg_map;
+	unsigned int phy_rst_off;
 	struct clk_bulk_data *clks;
 	struct reset_control *mrst;
 	struct regulator *mipi_phy_regulator;
@@ -391,6 +419,11 @@ static const struct csis_pix_format mipi_csis_formats[] = {
 		.output = MEDIA_BUS_FMT_RGB888_1X24,
 		.data_type = MIPI_CSI2_DT_RGB888,
 		.width = 24,
+	}, {
+		.code = MEDIA_BUS_FMT_RGB888_1X24,
+		.output = MEDIA_BUS_FMT_RGB888_1X24,
+		.data_type = MIPI_CSI2_DT_RGB888,
+		.width = 24,
 	},
 	/* RAW (Bayer and greyscale) formats. */
 	{
@@ -547,7 +580,11 @@ static inline void mipi_csis_write(struct mipi_csis_device *csis, u32 reg,
 static void mipi_csis_enable_interrupts(struct mipi_csis_device *csis, bool on)
 {
 	mipi_csis_write(csis, MIPI_CSIS_INT_MSK, on ? 0xffffffff : 0);
-	mipi_csis_write(csis, MIPI_CSIS_DBG_INTR_MSK, on ? 0xffffffff : 0);
+
+	if (csis->info->version == MIPI_CSIS_V4_3)
+		mipi_csis_write(csis, MIPI_CSIS_INT_MSK1, on ? 0xffffffff : 0);
+	else
+		mipi_csis_write(csis, MIPI_CSIS_DBG_INTR_MSK, on ? 0xffffffff : 0);
 }
 
 static void mipi_csis_sw_reset(struct mipi_csis_device *csis)
@@ -576,6 +613,8 @@ static void mipi_csis_system_enable(struct mipi_csis_device *csis, int on)
 		mask = (1 << (csis->bus.num_data_lanes + 1)) - 1;
 		val |= (mask & MIPI_CSIS_DPHY_CMN_CTRL_ENABLE);
 	}
+	if (csis->info->version == MIPI_CSIS_V4_3)
+		val |= MIPI_CSIS_DPHY_CMN_CTRL_S_BYTE_CLK_EN;
 	mipi_csis_write(csis, MIPI_CSIS_DPHY_CMN_CTRL, val);
 }
 
@@ -587,8 +626,11 @@ static void __mipi_csis_set_format(struct mipi_csis_device *csis,
 
 	/* Color format */
 	val = mipi_csis_read(csis, MIPI_CSIS_ISP_CONFIG_CH(csis->vc));
-	val &= ~(MIPI_CSIS_ISPCFG_PARALLEL | MIPI_CSIS_ISPCFG_PIXEL_MODE_MASK |
-		 MIPI_CSIS_ISPCFG_DATAFORMAT_MASK);
+	if (csis->info->version == MIPI_CSIS_V4_3)
+		val &= ~(MIPI_CSIS_ISPCFG_DATAFORMAT_MASK | MIPI_CSIS_ISPCFG_PIXEL_MODE_MASK);
+	else
+		val &= ~(MIPI_CSIS_ISPCFG_PARALLEL | MIPI_CSIS_ISPCFG_DATAFORMAT_MASK
+		| MIPI_CSIS_ISPCFG_PIXEL_MODE_MASK);
 
 	/*
 	 * YUV 4:2:2 can be transferred with 8 or 16 bits per clock sample
@@ -632,7 +674,7 @@ static int mipi_csis_calculate_params(struct mipi_csis_device *csis,
 
 	lane_rate = link_freq * 2;
 
-	if (lane_rate < 80000000 || lane_rate > 1500000000) {
+	if (lane_rate < 80000000 || lane_rate > 1600000000) {
 		dev_dbg(csis->dev, "Out-of-bound lane rate %u\n", lane_rate);
 		return -EINVAL;
 	}
@@ -674,8 +716,12 @@ static void mipi_csis_set_params(struct mipi_csis_device *csis,
 	val = mipi_csis_read(csis, MIPI_CSIS_CMN_CTRL);
 	val &= ~MIPI_CSIS_CMN_CTRL_LANE_NUMBER_MASK;
 	val |= MIPI_CSIS_CMN_CTRL_LANE_NUMBER(lanes - 1);
-	if (csis->info->version == MIPI_CSIS_V3_3)
+	if (csis->info->version == MIPI_CSIS_V3_3) {
 		val |= MIPI_CSIS_CMN_CTRL_INTERLEAVE_MODE_DT;
+	} else if (csis->info->version == MIPI_CSIS_V4_3) {
+		val |= MIPI_CSIS_CMN_CTRL_INTERLEAVE_MODE_VC_AND_DT;
+		val |= MIPI_CSIS_CMN_CTRL_DESKEW_ENABLE;
+	}
 	mipi_csis_write(csis, MIPI_CSIS_CMN_CTRL, val);
 
 	__mipi_csis_set_format(csis, format, csis_fmt);
@@ -684,15 +730,23 @@ static void mipi_csis_set_params(struct mipi_csis_device *csis,
 			MIPI_CSIS_DPHY_CMN_CTRL_HSSETTLE(csis->hs_settle) |
 			MIPI_CSIS_DPHY_CMN_CTRL_CLKSETTLE(csis->clk_settle));
 
-	mipi_csis_write(csis, MIPI_CSIS_ISP_SYNC_CH(csis->vc),
-			MIPI_CSIS_ISP_SYNC_HSYNC_LINTV(0) |
-			MIPI_CSIS_ISP_SYNC_VSYNC_SINTV(0) |
-			MIPI_CSIS_ISP_SYNC_VSYNC_EINTV(0));
+	if (csis->info->version == MIPI_CSIS_V4_3)
+		val = MIPI_CSIS_ISP_SYNC_HSYNC_LINTV(0x20);
+	else
+		val = MIPI_CSIS_ISP_SYNC_HSYNC_LINTV(0)
+			| MIPI_CSIS_ISP_SYNC_VSYNC_SINTV(0)
+			| MIPI_CSIS_ISP_SYNC_VSYNC_EINTV(0);
+	mipi_csis_write(csis, MIPI_CSIS_ISP_SYNC_CH(csis->vc), val);
 
 	val = mipi_csis_read(csis, MIPI_CSIS_CLK_CTRL);
-	val |= MIPI_CSIS_CLK_CTRL_WCLK_SRC(csis->vc);
-	val |= MIPI_CSIS_CLK_CTRL_CLKGATE_TRAIL(csis->vc, 15);
-	val &= ~MIPI_CSIS_CLK_CTRL_CLKGATE_EN_MSK;
+	if (csis->info->version == MIPI_CSIS_V4_3) {
+		val |= MIPI_CSIS_CLK_CTRL_CLKGATE_EN(csis->vc);
+		val |= MIPI_CSIS_CLK_CTRL_CLKGATE_TRAIL(csis->vc, 0x07);
+	} else {
+		val |= MIPI_CSIS_CLK_CTRL_WCLK_SRC(csis->vc);
+		val |= MIPI_CSIS_CLK_CTRL_CLKGATE_TRAIL(csis->vc, 15);
+		val &= ~MIPI_CSIS_CLK_CTRL_CLKGATE_EN_MSK;
+	}
 	mipi_csis_write(csis, MIPI_CSIS_CLK_CTRL, val);
 
 	mipi_csis_write(csis, MIPI_CSIS_DPHY_BCTRL_L,
@@ -712,6 +766,25 @@ static void mipi_csis_set_params(struct mipi_csis_device *csis,
 			MIPI_CSIS_CMN_CTRL_UPDATE_SHADOW_CTRL);
 }
 
+static int mipi_csis_get_sysreg(struct mipi_csis_device *csis)
+{
+	unsigned int args;
+
+	if (csis->info->version != MIPI_CSIS_V4_3)
+		return 0;
+
+	csis->sysreg_map = syscon_regmap_lookup_by_phandle_args
+				(csis->dev->of_node, "tesla,syscon-csis", 1,
+				 &args);
+
+	if (IS_ERR(csis->sysreg_map))
+		return PTR_ERR(csis->sysreg_map);
+
+	csis->phy_rst_off = args;
+
+	return 0;
+}
+
 static int mipi_csis_clk_enable(struct mipi_csis_device *csis)
 {
 	return clk_bulk_prepare_enable(csis->info->num_clocks, csis->clks);
@@ -756,11 +829,71 @@ static int mipi_csis_clk_get(struct mipi_csis_device *csis)
 	return ret;
 }
 
+static void mipi_csis_dphy_reset_release_v4_3(struct mipi_csis_device *csis)
+{
+	unsigned int idx = 0, val = 0x0;
+
+	/* There are 4 CSIs per each D-PHY i/f */
+	idx = csis->vc;
+
+	regmap_read(csis->sysreg_map, csis->phy_rst_off, &val);
+
+	val &= ~FSD_CSIS_RESETEN_DPHY_MASK_V4_3(idx);
+	regmap_write(csis->sysreg_map, csis->phy_rst_off, val);
+
+	usleep_range(500, 1000);
+
+	val |= FSD_CSIS_RESETEN_DPHY_MASK_V4_3(idx);
+	regmap_write(csis->sysreg_map, csis->phy_rst_off, val);
+}
+
+static void mipi_csis_dphy_init_v4_3(struct mipi_csis_device *csis)
+{
+	u32 val = 0;
+
+	mipi_csis_dphy_reset_release_v4_3(csis);
+
+	val = readl(csis->regs + MIPI_CSIS_DPHY_SCTRL_H);
+
+	val |= MIPI_CSIS_DPHY_SCTRL_H_SKEW_CAL_EN;
+	val |= MIPI_CSIS_DPHY_SCTRL_H_SKEW_CAL_MAX_SKEW_CODE_CTRL;
+	writel(val, csis->regs + MIPI_CSIS_DPHY_SCTRL_H);
+}
+
+static void mipi_csis_set_vc_passing(struct mipi_csis_device *csis)
+{
+	u32 val;
+	unsigned int vc = csis->vc;
+
+	val = readl(csis->regs + MIPI_CSIS_VC_PASSING_REG);
+
+	val &= ~MIPI_CSIS_VC_PASSING_MASK;
+	val |= MIPI_CSIS_VC_PASSING(vc);
+	val |= MIPI_CSIS_VC_PASSING_EN;
+	writel(val, csis->regs + MIPI_CSIS_VC_PASSING_REG);
+}
+
+static void mipi_csis_get_irq_status(struct mipi_csis_device *csis,
+				     unsigned int *sts)
+{
+	*sts = readl(csis->regs + MIPI_CSIS_INT_SRC1);
+}
+
+static void mipi_csis_clear_irq_status(struct mipi_csis_device *csis,
+				       unsigned int *sts)
+{
+	writel(*sts, csis->regs + MIPI_CSIS_INT_SRC1);
+}
+
 static void mipi_csis_start_stream(struct mipi_csis_device *csis,
 				   const struct v4l2_mbus_framefmt *format,
 				   const struct csis_pix_format *csis_fmt)
 {
 	mipi_csis_sw_reset(csis);
+	if (csis->info->version == MIPI_CSIS_V4_3) {
+		mipi_csis_dphy_init_v4_3(csis);
+		mipi_csis_set_vc_passing(csis);
+	}
 	mipi_csis_set_params(csis, format, csis_fmt);
 	mipi_csis_system_enable(csis, true);
 	mipi_csis_enable_interrupts(csis, true);
@@ -784,6 +917,31 @@ static void mipi_csis_queue_event_sof(struct mipi_csis_device *csis)
 	v4l2_event_queue(csis->sd.devnode, &event);
 }
 
+static irqreturn_t mipi_csis_irq_handler_v4_3(int irq, void *dev_id)
+{
+	struct mipi_csis_device *csis = dev_id;
+	unsigned long flags;
+	u32 status;
+	unsigned int i;
+
+	status = mipi_csis_read(csis, MIPI_CSIS_INT_SRC);
+
+	spin_lock_irqsave(&csis->slock, flags);
+	if ((status & MIPI_CSIS_INT_SRC_ERRORS)) {
+		for (i = 0; i < MIPI_CSIS_INT_SRC_NUM_EVENTS_V4_3; i++) {
+			struct mipi_csis_event *event = &csis->events[i];
+
+			if (status & event->mask)
+				event->counter++;
+		}
+	}
+	spin_unlock_irqrestore(&csis->slock, flags);
+
+	mipi_csis_write(csis, MIPI_CSIS_INT_SRC, status);
+
+	return IRQ_NONE;
+}
+
 static irqreturn_t mipi_csis_irq_handler(int irq, void *dev_id)
 {
 	struct mipi_csis_device *csis = dev_id;
@@ -1000,7 +1158,6 @@ static void mipi_csis_debugfs_exit(struct mipi_csis_device *csis)
 /* -----------------------------------------------------------------------------
  * V4L2 subdev operations
  */
-
 static struct mipi_csis_device *sd_to_mipi_csis_device(struct v4l2_subdev *sdev)
 {
 	return container_of(sdev, struct mipi_csis_device, sd);
@@ -1064,6 +1221,39 @@ static int mipi_csis_s_stream(struct v4l2_subdev *sd, int enable)
 	return ret;
 }
 
+static void mipi_csis_read_vc_frame_counter(struct mipi_csis_device *csis,
+					    u32 *current_frame_counter)
+{
+	unsigned int vc = csis->vc;
+	*current_frame_counter = readl(csis->regs + MIPI_CSIS_FRAME_COUNTER_CH(vc));
+}
+
+static long mipi_csis_command(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
+{
+	struct mipi_csis_device *csis = sd_to_mipi_csis_device(sd);
+	long ret = 0;
+
+	switch (cmd) {
+	case 1:
+		mipi_csis_system_enable(csis, true);
+		break;
+	case 2:
+		mipi_csis_get_irq_status(csis, arg);
+		break;
+	case 3:
+		mipi_csis_clear_irq_status(csis, arg);
+		break;
+	case 5:
+		mipi_csis_read_vc_frame_counter(csis, arg);
+		break;
+	default:
+		dev_err(csis->dev, "Invalid command\n");
+		ret = -1;
+	}
+
+	return ret;
+}
+
 static int mipi_csis_enum_mbus_code(struct v4l2_subdev *sd,
 				    struct v4l2_subdev_state *state,
 				    struct v4l2_subdev_mbus_code_enum *code)
@@ -1171,6 +1361,7 @@ static int mipi_csis_set_fmt(struct v4l2_subdev *sd,
 static int mipi_csis_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
 				    struct v4l2_mbus_frame_desc *fd)
 {
+	struct mipi_csis_device *csis = sd_to_mipi_csis_device(sd);
 	struct v4l2_mbus_frame_desc_entry *entry = &fd->entry[0];
 	const struct csis_pix_format *csis_fmt;
 	const struct v4l2_mbus_framefmt *fmt;
@@ -1247,6 +1438,7 @@ static const struct v4l2_subdev_core_ops mipi_csis_core_ops = {
 	.log_status	= mipi_csis_log_status,
 	.subscribe_event =  mipi_csis_subscribe_event,
 	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+	.command = mipi_csis_command,
 };
 
 static const struct v4l2_subdev_video_ops mipi_csis_video_ops = {
@@ -1314,6 +1506,7 @@ static int mipi_csis_link_setup(struct media_entity *entity,
 	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
 	struct mipi_csis_device *csis = sd_to_mipi_csis_device(sd);
 	struct v4l2_subdev *remote_sd;
+	int ret;
 
 	dev_dbg(csis->dev, "link setup %s -> %s", remote_pad->entity->name,
 		local_pad->entity->name);
@@ -1563,6 +1756,10 @@ static int mipi_csis_probe(struct platform_device *pdev)
 	if (irq < 0)
 		return irq;
 
+	ret = mipi_csis_get_sysreg(csis);
+	if (ret < 0)
+		return ret;
+
 	ret = mipi_csis_phy_init(csis);
 	if (ret < 0)
 		return ret;
@@ -1661,6 +1858,15 @@ static const struct of_device_id mipi_csis_of_match[] = {
 			.irq_flag = 0,
 			.irq_handler = mipi_csis_irq_handler,
 		},
+	}, {
+		.compatible = "tesla,fsd-mipi-csi2",
+		.data = &(const struct mipi_csis_info){
+			.version = MIPI_CSIS_V4_3,
+			.num_clocks = 2,
+			.clk_names = { "aclk", "pclk"},
+			.irq_flag = IRQF_SHARED,
+			.irq_handler = mipi_csis_irq_handler_v4_3,
+		},
 	},
 	{ /* sentinel */ },
 };
-- 
2.49.0

[PATCH v3 7/7] media: fsd-csis: Add FSD CSIS video capture interface support

[Inbaraj E]

The Tesla FSD CSIS IP bundles video capture interface to capture frames
from MIPI-CSI2 bus.

This driver exposes video device file to userspace for frame capture and
a media device file to configure camera pipeline.

Signed-off-by: Inbaraj E <inbaraj.e@samsung.com>
---
 MAINTAINERS                                   |    8 +
 drivers/media/platform/samsung/Kconfig        |    1 +
 drivers/media/platform/samsung/Makefile       |    1 +
 .../media/platform/samsung/fsd-csis/Kconfig   |   18 +
 .../media/platform/samsung/fsd-csis/Makefile  |    3 +
 .../platform/samsung/fsd-csis/fsd-csis.c      | 1690 +++++++++++++++++
 6 files changed, 1721 insertions(+)
 create mode 100644 drivers/media/platform/samsung/fsd-csis/Kconfig
 create mode 100644 drivers/media/platform/samsung/fsd-csis/Makefile
 create mode 100644 drivers/media/platform/samsung/fsd-csis/fsd-csis.c

diff --git a/MAINTAINERS b/MAINTAINERS
index c5171a5dcba6..4d86fe921ca8 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -3334,6 +3334,14 @@ S:	Maintained
 F:	Documentation/devicetree/bindings/media/samsung,s5p-mfc.yaml
 F:	drivers/media/platform/samsung/s5p-mfc/
 
+TESLA FSD BRIDGE DRIVER
+M:	Inbaraj E <inbaraj.e@samsung.com>
+L:	linux-samsung-soc@vger.kernel.org (moderated for non-subscribers)
+L:	linux-media@vger.kernel.org
+S:	Maintained
+F:	Documentation/devicetree/bindings/media/tesla,fsd-csis-media.yaml
+F:	drivers/media/platform/samsung/fsd-csis/fsd-csis.c
+
 ARM/SOCFPGA ARCHITECTURE
 M:	Dinh Nguyen <dinguyen@kernel.org>
 S:	Maintained
diff --git a/drivers/media/platform/samsung/Kconfig b/drivers/media/platform/samsung/Kconfig
index 0e34c5fc1dfc..4cebe2ae24a3 100644
--- a/drivers/media/platform/samsung/Kconfig
+++ b/drivers/media/platform/samsung/Kconfig
@@ -4,6 +4,7 @@ comment "Samsung media platform drivers"
 
 source "drivers/media/platform/samsung/exynos-gsc/Kconfig"
 source "drivers/media/platform/samsung/exynos4-is/Kconfig"
+source "drivers/media/platform/samsung/fsd-csis/Kconfig"
 source "drivers/media/platform/samsung/s3c-camif/Kconfig"
 source "drivers/media/platform/samsung/s5p-g2d/Kconfig"
 source "drivers/media/platform/samsung/s5p-jpeg/Kconfig"
diff --git a/drivers/media/platform/samsung/Makefile b/drivers/media/platform/samsung/Makefile
index 21fea3330e4b..fde1b9626713 100644
--- a/drivers/media/platform/samsung/Makefile
+++ b/drivers/media/platform/samsung/Makefile
@@ -1,6 +1,7 @@
 # SPDX-License-Identifier: GPL-2.0-only
 obj-y += exynos-gsc/
 obj-y += exynos4-is/
+obj-y += fsd-csis/
 obj-y += s3c-camif/
 obj-y += s5p-g2d/
 obj-y += s5p-jpeg/
diff --git a/drivers/media/platform/samsung/fsd-csis/Kconfig b/drivers/media/platform/samsung/fsd-csis/Kconfig
new file mode 100644
index 000000000000..64ae57f81d22
--- /dev/null
+++ b/drivers/media/platform/samsung/fsd-csis/Kconfig
@@ -0,0 +1,18 @@
+# SPDX-License-Identifier: GPL-2.0-only
+#
+# FSD MIPI CSI-2 Rx controller configurations
+
+config VIDEO_TESLA_FSD_CSIS
+	tristate "FSD SoC MIPI-CSI2 media controller driver"
+	depends on VIDEO_DEV && VIDEO_V4L2_SUBDEV_API
+	depends on HAS_DMA
+	depends on ARCH_TESLA_FSD
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_FWNODE
+	help
+	  This is a video4linux2 driver for TESLA FSD SoC MIPI-CSI2 Rx.
+	  The driver provides interface for capturing frames.
+
+	  To compile this driver as a module, choose M here. The module
+	  will be called fsd-csis.
+
diff --git a/drivers/media/platform/samsung/fsd-csis/Makefile b/drivers/media/platform/samsung/fsd-csis/Makefile
new file mode 100644
index 000000000000..754d628770b0
--- /dev/null
+++ b/drivers/media/platform/samsung/fsd-csis/Makefile
@@ -0,0 +1,3 @@
+# SPDX-License-Identifier: GPL-2.0-only
+
+obj-$(CONFIG_VIDEO_TESLA_FSD_CSIS) += fsd-csis.o
diff --git a/drivers/media/platform/samsung/fsd-csis/fsd-csis.c b/drivers/media/platform/samsung/fsd-csis/fsd-csis.c
new file mode 100644
index 000000000000..c3029433eb27
--- /dev/null
+++ b/drivers/media/platform/samsung/fsd-csis/fsd-csis.c
@@ -0,0 +1,1690 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2025 Samsung Electronics Co., Ltd.
+ *             https://www.samsung.com
+ *
+ * TESLA FSD CSIS V4L2 Capture driver for TESLA FSD SoC.
+ */
+
+#include <linux/clk.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/v4l2-mc.h>
+
+#define FSD_CSIS_DMA_COHERENT_MASK_SIZE		32
+#define FSD_CSIS_NB_MIN_CH			2
+#define FSD_CSIS_NB_VC				4
+#define FSD_CSIS_MEDIA_NUM_PADS			2
+#define FSD_CSIS_NB_DMA_OUT_CH			8
+#define FSD_CSIS_MAX_VC				4
+#define FSD_CSIS_NB_CLOCK			2
+#define FSD_CSIS_NB_OF_BUFS_ON_DMA_CHANNELS	2
+#define FSD_CSIS_DMA_LINE_ALIGN_SIZE		128
+#define FSD_CSIS_DMA_CH_OFFSET			0x100
+
+/**
+ * (Interrupt Source & mask register 1)
+ */
+#define FSD_CSIS_DMA_OTF_OVERLAP_MASK	GENMASK(17, 14)
+#define FSD_CSIS_DMA_ABORT_DONE_MASK	BIT(13)
+#define FSD_CSIS_DMA_ERROR_MASK		BIT(12)
+#define FSD_CSIS_INT_SRC1_ERR_ALL_MASK	(FSD_CSIS_DMA_ERROR_MASK | \
+					FSD_CSIS_DMA_ABORT_DONE_MASK | \
+					FSD_CSIS_DMA_ERROR_MASK)
+#define FDS_CSIS_DMA_FRM_END_MASK	GENMASK(11, 8)
+#define FSD_CSIS_DMA_FRM_START_MASK	GENMASK(7, 4)
+#define FSD_CSIS_LINE_END_MASK		GENMASK(3, 0)
+#define FSD_CSIS_DMA_CH0_MASK		0x4111U
+
+/* DMA Reg offsets */
+#define FSD_CSIS_DMA0_CTRL	0x0
+#define FSD_CSIS_DMA_CTRL(vc)	(FSD_CSIS_DMA0_CTRL + (vc) * FSD_CSIS_DMA_CH_OFFSET)
+#define FSD_CSIS_DMA_DISABLE	BIT(0)
+
+#define FSD_CSIS_DMA0_FMT	0x4
+#define FSD_CSIS_DMA_FMT(vc)	(FSD_CSIS_DMA0_FMT + (vc) * FSD_CSIS_DMA_CH_OFFSET)
+#define FSD_CSIS_DMA_DIM	BIT(15)
+#define FSD_CSIS_DMA_DUMP	BIT(13)
+
+#define FSD_CSIS_DMA0_ADDR1     0x10
+#define FSD_CSIS_DMA_ADDR1(vc)	(FSD_CSIS_DMA0_ADDR1 + (vc) * FSD_CSIS_DMA_CH_OFFSET)
+
+#define FSD_CSIS_DMA0_ACT_CTRL		0x30
+#define FSD_CSIS_DMA_ACT_CTRL(vc)	(FSD_CSIS_DMA0_ACT_CTRL + (vc) * FSD_CSIS_DMA_CH_OFFSET)
+#define FSD_CSIS_ACTIVE_DMA_PACK_MASK		GENMASK(17, 16)
+#define FSD_CSIS_ACTIVE_DMA_PACK(n)		((n) << 16)
+#define FSD_CSIS_ACTIVE_DMA_FRAMEPTR_MASK	GENMASK(4, 2)
+
+#define FSD_CSIS_DMA_ERR_CODE		0x404
+#define FSD_CSIS_DMAFIFO_FULL_MASK	BIT_MASK(5)
+#define FSD_CSIS_TRXFIFO_FULL_MASK	BIT_MASK(4)
+
+#define FSD_CSIS_DMA_CLK_CTRL			0x408
+#define FSD_CSIS_DMA_CLK_GATE_TRAIL_MASK	GENMASK(4, 1)
+#define FSD_CSIS_DMA_CLK_GATE_TRAIL(n)		((n) << 1)
+#define FSD_CSIS_DMA_CLK_GATE_EN		BIT(0)
+
+enum CSIS_DMA_PACK {
+	DMA_PACK_NORMAL,
+	DMA_PACK_10,
+	DMA_PACK_12,
+	DMA_PACK_14,
+	DMA_PACK_18,
+	DMA_PACK_20,
+};
+
+static const char * const fsd_csis_clk_id[] = {
+	"aclk",
+	"pclk",
+};
+
+struct fsd_csis_pixfmt {
+	u32 fourcc;
+	const u32 *codes;
+	int bpp;
+	bool is_yuv;
+};
+
+struct fsd_csis_vb2_buffer {
+	struct vb2_v4l2_buffer vb;
+	struct list_head list;
+	const struct fsd_csis_pixfmt *fmt;
+	unsigned long sequence_num;
+};
+
+struct fsd_csis {
+	struct device *dev;
+	const struct fsd_csis_info *info;
+	struct clk_bulk_data *clks;
+	struct clk *pll;
+	struct media_device mdev;
+	struct v4l2_device v4l2_dev;
+	struct v4l2_async_notifier notifier;
+	struct media_pipeline pipe;
+
+	/* source node */
+	struct {
+		struct v4l2_subdev *subdev;
+		struct media_pad *pad;
+	} source;
+
+	/* Internal subdev */
+	struct {
+		struct v4l2_subdev sd;
+		struct media_pad pad[FSD_CSIS_MEDIA_NUM_PADS];
+	} subdev;
+
+	struct video_device *vdev;
+	struct media_pad vdev_pad;
+	struct vb2_queue q;
+	/* Protect vdev operation */
+	struct mutex vdev_mutex;
+	void __iomem *dma_base;
+	int irq;
+	u64 frame_addr[FSD_CSIS_NB_DMA_OUT_CH];
+	struct fsd_csis_vb2_buffer *frame[FSD_CSIS_NB_DMA_OUT_CH];
+	struct v4l2_pix_format vdev_fmt;
+	const struct fsd_csis_pixfmt *vdev_cc;
+	struct v4l2_rect vdev_compose;
+	u32 num_reqbufs;
+	u8 prev_dma_ptr;
+	u8 current_dma_ptr;
+	u8 number_of_ready_bufs;
+	u32 prev_frame_counter;
+	u32 current_frame_counter;
+	unsigned int num_active_fmt;
+	struct list_head ready_q;
+	/* Protect ready_q */
+	spinlock_t q_lock;
+	/* Protect DMA channel register's */
+	spinlock_t dma_reg_lock;
+	unsigned int current_vc;
+	unsigned long sequence;
+	u32 dma_error;
+	int is_streaming;
+};
+
+static inline u32 get_bits(u32 value, u32 mask)
+{
+	return (((value) & (mask)) >> (ffs(mask) - 1));
+}
+
+static inline u32 bytes_per_line(u32 width, int bpp)
+{
+	return (ALIGN((width * bpp), FSD_CSIS_DMA_LINE_ALIGN_SIZE) >> 3);
+}
+
+static inline uint8_t fsd_csis_current_dma_ptr(struct fsd_csis *csis)
+
+{
+	return (readl(csis->dma_base + FSD_CSIS_DMA_ACT_CTRL(csis->current_vc))
+			& 0x01C) >> 2;
+}
+
+#define FSD_CSIS_MODULE_NAME	"fsd-csis"
+#define FSD_CSIS_MODULE_VERSION	"0.0.1"
+
+#define FSD_CSIS_DEF_MBUS_CODE		MEDIA_BUS_FMT_RGB888_1X24
+#define FSD_CSIS_DEF_PIX_FORMAT		V4L2_PIX_FMT_RGB24
+#define FSD_CSIS_DEF_PIX_WIDTH		1280
+#define FSD_CSIS_DEF_PIX_HEIGHT		964
+
+#define FSD_CSIS_PAD_SINK	0
+#define FSD_CSIS_PAD_SRC	1
+#define FSD_CSIS_PADS_NUM	2
+
+#define FSD_CSIS_BUS_FMTS(fmt...) ((const u32[]) {fmt, 0 })
+
+static const struct v4l2_mbus_framefmt fsd_csis_default_format = {
+	.width = 640,
+	.height = 480,
+	.code = MEDIA_BUS_FMT_UYVY8_1X16,
+	.field = V4L2_FIELD_NONE,
+};
+
+static const struct fsd_csis_pixfmt pixel_formats[] = {
+	/* YUV formats start here */
+	{
+		.fourcc = V4L2_PIX_FMT_UYVY,
+		.codes  = FSD_CSIS_BUS_FMTS(MEDIA_BUS_FMT_UYVY8_2X8,
+					    MEDIA_BUS_FMT_UYVY8_1X16),
+		.is_yuv    = true,
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_YUYV,
+		.codes  = FSD_CSIS_BUS_FMTS(MEDIA_BUS_FMT_YUYV8_2X8,
+					   MEDIA_BUS_FMT_YUYV8_1X16),
+		.is_yuv    = true,
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SBGGR8,
+		.codes  = FSD_CSIS_BUS_FMTS(MEDIA_BUS_FMT_SBGGR8_1X8),
+		.bpp    = 8,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SGBRG8,
+		.codes  = FSD_CSIS_BUS_FMTS(MEDIA_BUS_FMT_SGBRG8_1X8),
+		.bpp    = 8,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SGRBG8,
+		.codes  = FSD_CSIS_BUS_FMTS(MEDIA_BUS_FMT_SGRBG8_1X8),
+		.bpp    = 8,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SRGGB8,
+		.codes  = FSD_CSIS_BUS_FMTS(MEDIA_BUS_FMT_SRGGB8_1X8),
+		.bpp    = 8,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SBGGR10,
+		.codes  = FSD_CSIS_BUS_FMTS(MEDIA_BUS_FMT_SBGGR10_1X10),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SGBRG10,
+		.codes  = FSD_CSIS_BUS_FMTS(MEDIA_BUS_FMT_SGBRG10_1X10),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SGRBG10,
+		.codes  = FSD_CSIS_BUS_FMTS(MEDIA_BUS_FMT_SGRBG10_1X10),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SRGGB10,
+		.codes  = FSD_CSIS_BUS_FMTS(MEDIA_BUS_FMT_SRGGB10_1X10),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SBGGR12,
+		.codes  = FSD_CSIS_BUS_FMTS(MEDIA_BUS_FMT_SBGGR12_1X12),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SGBRG12,
+		.codes  = FSD_CSIS_BUS_FMTS(MEDIA_BUS_FMT_SGBRG12_1X12),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SGRBG12,
+		.codes  = FSD_CSIS_BUS_FMTS(MEDIA_BUS_FMT_SGRBG12_1X12),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SRGGB12,
+		.codes  = FSD_CSIS_BUS_FMTS(MEDIA_BUS_FMT_SRGGB12_1X12),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SBGGR14,
+		.codes  = FSD_CSIS_BUS_FMTS(MEDIA_BUS_FMT_SBGGR14_1X14),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SGBRG14,
+		.codes  = FSD_CSIS_BUS_FMTS(MEDIA_BUS_FMT_SGBRG14_1X14),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SGRBG14,
+		.codes  = FSD_CSIS_BUS_FMTS(MEDIA_BUS_FMT_SGRBG14_1X14),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SRGGB14,
+		.codes  = FSD_CSIS_BUS_FMTS(MEDIA_BUS_FMT_SRGGB14_1X14),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_GREY,
+		.codes  = FSD_CSIS_BUS_FMTS(MEDIA_BUS_FMT_Y8_1X8),
+		.bpp    = 8,
+	}, {
+		.fourcc = V4L2_PIX_FMT_Y10,
+		.codes  = FSD_CSIS_BUS_FMTS(MEDIA_BUS_FMT_Y10_1X10),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_Y12,
+		.codes  = FSD_CSIS_BUS_FMTS(MEDIA_BUS_FMT_Y12_1X12),
+		.bpp    = 16,
+	}, {
+		.fourcc = V4L2_PIX_FMT_Y14,
+		.codes  = FSD_CSIS_BUS_FMTS(MEDIA_BUS_FMT_Y14_1X14),
+		.bpp    = 16,
+	}, {
+		.fourcc	= V4L2_PIX_FMT_RGB24,
+		.codes	= FSD_CSIS_BUS_FMTS(MEDIA_BUS_FMT_RGB888_1X24),
+		.bpp	= 24,
+	}
+};
+
+static void fsd_csis_dma_enable(struct fsd_csis *csis, bool en_dma)
+{
+	unsigned int dma_ctrl, vc = csis->current_vc;
+
+	dma_ctrl = readl(csis->dma_base + FSD_CSIS_DMA_CTRL(vc));
+	dma_ctrl |= FSD_CSIS_DMA_DISABLE;
+
+	if (en_dma)
+		dma_ctrl &= ~FSD_CSIS_DMA_DISABLE;
+
+	writel(dma_ctrl, csis->dma_base + FSD_CSIS_DMA_CTRL(vc));
+}
+
+static void fsd_csis_set_dma_clk(struct fsd_csis *csis)
+{
+	unsigned int dma_clk_ctrl;
+
+	dma_clk_ctrl = readl(csis->dma_base + FSD_CSIS_DMA_CLK_CTRL);
+
+	dma_clk_ctrl &= ~FSD_CSIS_DMA_CLK_GATE_EN;
+	dma_clk_ctrl &= ~FSD_CSIS_DMA_CLK_GATE_TRAIL_MASK;
+	dma_clk_ctrl |= FSD_CSIS_DMA_CLK_GATE_TRAIL(0x7);
+
+	writel(dma_clk_ctrl, csis->dma_base + FSD_CSIS_DMA_CLK_CTRL);
+}
+
+static void fsd_csis_set_pack(struct fsd_csis *csis, u32 vc,
+			      enum CSIS_DMA_PACK dma_pack)
+{
+	u32 dma_fmt;
+
+	dma_fmt = readl(csis->dma_base + FSD_CSIS_DMA_CTRL(vc));
+	dma_fmt &= ~FSD_CSIS_ACTIVE_DMA_PACK_MASK;
+	dma_fmt |= FSD_CSIS_ACTIVE_DMA_PACK(dma_pack);
+	writel(dma_fmt, csis->dma_base + FSD_CSIS_DMA_CTRL(vc));
+}
+
+static void fsd_csis_set_dma_dump(struct fsd_csis *csis, unsigned int vc,
+				  bool set_dump)
+{
+	u32 dma_fmt;
+
+	dma_fmt = readl(csis->dma_base + FSD_CSIS_DMA_CTRL(vc));
+	dma_fmt &= ~FSD_CSIS_DMA_DUMP;
+
+	if (set_dump)
+		dma_fmt |= FSD_CSIS_DMA_DUMP;
+
+	writel(dma_fmt, csis->dma_base + FSD_CSIS_DMA_CTRL(vc));
+}
+
+static void fsd_csis_set_dma_dimension(struct fsd_csis *csis, u32 vc, bool set_dim)
+{
+	u32 dma_fmt;
+
+	dma_fmt = readl(csis->dma_base + FSD_CSIS_DMA_FMT(vc));
+	dma_fmt &= ~FSD_CSIS_DMA_DIM;
+
+	if (set_dim)
+		dma_fmt |= FSD_CSIS_DMA_DIM;
+
+	writel(dma_fmt, csis->dma_base + FSD_CSIS_DMA_FMT(vc));
+}
+
+static void fsd_csis_set_dma_format(struct fsd_csis *csis,
+				    const struct fsd_csis_pixfmt *cc)
+{
+	unsigned int fourcc = cc->fourcc;
+
+	switch (fourcc) {
+	case V4L2_PIX_FMT_SBGGR10:
+	case V4L2_PIX_FMT_SGBRG10:
+	case V4L2_PIX_FMT_SGRBG10:
+	case V4L2_PIX_FMT_SRGGB10:
+		fsd_csis_set_pack(csis, csis->current_vc, DMA_PACK_10);
+		break;
+	case V4L2_PIX_FMT_SBGGR12:
+	case V4L2_PIX_FMT_SGBRG12:
+	case V4L2_PIX_FMT_SGRBG12:
+	case V4L2_PIX_FMT_SRGGB12:
+		fsd_csis_set_pack(csis, csis->current_vc, DMA_PACK_12);
+		break;
+	case V4L2_PIX_FMT_SBGGR14P:
+		fsd_csis_set_pack(csis, csis->current_vc, DMA_PACK_14);
+		break;
+	case V4L2_PIX_FMT_BGR666:
+		fsd_csis_set_pack(csis, csis->current_vc, DMA_PACK_18);
+		break;
+	case V4L2_PIX_FMT_UYVY:
+		fsd_csis_set_pack(csis, csis->current_vc, DMA_PACK_NORMAL);
+		break;
+	default:
+		dev_err(csis->dev, "Set DMA format %x not supported\n", fourcc);
+		break;
+	}
+
+	fsd_csis_set_dma_dump(csis, csis->current_vc, false);
+	fsd_csis_set_dma_dimension(csis, csis->current_vc, false);
+}
+
+static inline struct fsd_csis *notifier_to_csis(struct v4l2_async_notifier *n)
+{
+	return container_of(n, struct fsd_csis, notifier);
+}
+
+static int fsd_csis_queue_setup(struct vb2_queue *vq,
+				unsigned int *nbuffers, unsigned int *nplanes,
+				unsigned int sizes[],
+				struct device *alloc_devs[])
+{
+	struct fsd_csis *csis = vb2_get_drv_priv(vq);
+	struct v4l2_pix_format *pix = &csis->vdev_fmt;
+	unsigned int size = pix->sizeimage;
+
+	if (*nplanes) {
+		if (sizes[0] < size)
+			return -EINVAL;
+		size = sizes[0];
+	}
+
+	*nplanes = 1;
+	sizes[0] = size;
+
+	dev_info(csis->dev, "nbuffers %d size %d\n", *nbuffers, sizes[0]);
+
+	return 0;
+}
+
+static int fsd_csis_buffer_prepare(struct vb2_buffer *vb)
+{
+	struct fsd_csis *csis = vb2_get_drv_priv(vb->vb2_queue);
+	struct fsd_csis_vb2_buffer *buf = container_of(vb, struct fsd_csis_vb2_buffer,
+			vb.vb2_buf);
+	struct v4l2_pix_format *pix = &csis->vdev_fmt;
+
+	if (WARN_ON(!csis->vdev_cc))
+		return -EINVAL;
+
+	if (vb2_plane_size(vb, 0) < pix->sizeimage) {
+		dev_info(csis->dev, "Data will not fit into plane (%lu < %u)\n",
+			 vb2_plane_size(vb, 0), pix->sizeimage);
+		return -EINVAL;
+	}
+
+	vb2_set_plane_payload(&buf->vb.vb2_buf, 0, pix->sizeimage);
+
+	return 0;
+}
+
+static void fsd_csis_buffer_queue(struct vb2_buffer *vb)
+{
+	unsigned long flags;
+	struct fsd_csis *csis = vb2_get_drv_priv(vb->vb2_queue);
+	struct fsd_csis_vb2_buffer *buf =
+		container_of(vb, struct fsd_csis_vb2_buffer, vb.vb2_buf);
+
+	spin_lock_irqsave(&csis->q_lock, flags);
+	list_add_tail(&buf->list, &csis->ready_q);
+	buf->sequence_num = csis->sequence++;
+	spin_unlock_irqrestore(&csis->q_lock, flags);
+}
+
+static void fsd_csis_dma_set_vid_base_addr(struct fsd_csis *csis, int frm_no,
+					   unsigned long addr)
+{
+	unsigned int dma_addr;
+	unsigned long flags;
+
+	dma_addr = FSD_CSIS_DMA_ADDR1(csis->current_vc);
+	dma_addr = dma_addr + (frm_no * 4);
+	spin_lock_irqsave(&csis->dma_reg_lock, flags);
+	writel(addr, csis->dma_base + dma_addr);
+	spin_unlock_irqrestore(&csis->dma_reg_lock, flags);
+}
+
+static void fsd_csis_add_to_ring_buffer(struct fsd_csis *csis,
+					struct fsd_csis_vb2_buffer *buf,
+					uint8_t index)
+{
+	u8 modulo_addr;
+	unsigned int i;
+
+	for (i = 0; i < FSD_CSIS_NB_DMA_OUT_CH;
+			i += FSD_CSIS_NB_OF_BUFS_ON_DMA_CHANNELS) {
+		modulo_addr = (index + i) % FSD_CSIS_NB_DMA_OUT_CH;
+		csis->frame[modulo_addr] = buf;
+		csis->frame_addr[modulo_addr] =
+			vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0);
+		fsd_csis_dma_set_vid_base_addr(csis, modulo_addr,
+					       csis->frame_addr[modulo_addr]);
+	}
+}
+
+static int fsd_csis_get_vc(struct fsd_csis *csis)
+{
+	struct v4l2_mbus_frame_desc fd = { };
+	struct media_pad *remote_pad;
+	int ret;
+
+	remote_pad = media_pad_remote_pad_unique(&csis->subdev.pad[FSD_CSIS_PAD_SINK]);
+	ret = v4l2_subdev_call(csis->source.subdev, pad, get_frame_desc, remote_pad->index, &fd);
+	if (ret < 0 && ret != -ENOIOCTLCMD) {
+		dev_err(csis->dev, "get_frame_desc failed on source subdev\n");
+		return ret;
+	}
+
+	/* If remote subdev does not implement ..get_frame_desc default to VC0 */
+	if (ret == -ENOIOCTLCMD)
+		return 0;
+
+	if (!fd.num_entries) {
+		dev_err(csis->dev, "get_frame_desc returned zero entries\n");
+		return -EINVAL;
+	}
+
+	return fd.entry[0].bus.csi2.vc;
+}
+
+static int fsd_csis_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct fsd_csis *csis = vb2_get_drv_priv(q);
+	struct fsd_csis_vb2_buffer *buf, *tmp;
+	unsigned long flags;
+	u8 i;
+	int ret;
+
+	mutex_lock(&csis->mdev.graph_mutex);
+
+	ret = __video_device_pipeline_start(csis->vdev, &csis->pipe);
+	if (ret)
+		goto err_unlock;
+
+	ret = fsd_csis_get_vc(csis);
+
+	if (ret < 0)
+		goto err_unlock;
+
+	csis->current_vc = ret;
+
+	ret = v4l2_subdev_enable_streams(&csis->subdev.sd, FSD_CSIS_PAD_SRC,
+					 BIT(0));
+	if (ret) {
+		dev_err(csis->dev, "stream on failed in subdev\n");
+		goto err_stop;
+	}
+
+	mutex_unlock(&csis->mdev.graph_mutex);
+	fsd_csis_set_dma_clk(csis);
+	fsd_csis_set_dma_format(csis, csis->vdev_cc);
+
+	for (i = 0; i < FSD_CSIS_NB_OF_BUFS_ON_DMA_CHANNELS; i++) {
+		spin_lock_irqsave(&csis->q_lock, flags);
+		if (list_empty(&csis->ready_q)) {
+			spin_unlock_irqrestore(&csis->q_lock, flags);
+			dev_err(csis->dev, "Failed to fill buffer address!\n");
+			return -EIO;
+		}
+
+		buf = list_entry(csis->ready_q.next, struct fsd_csis_vb2_buffer, list);
+		list_del(&buf->list);
+		fsd_csis_add_to_ring_buffer(csis, buf, i);
+		spin_unlock_irqrestore(&csis->q_lock, flags);
+	}
+
+	fsd_csis_dma_enable(csis, true);
+
+	return 0;
+err_stop:
+	v4l2_subdev_disable_streams(&csis->subdev.sd, FSD_CSIS_PAD_SRC,
+				    BIT(0));
+	__video_device_pipeline_stop(csis->vdev);
+err_unlock:
+	mutex_unlock(&csis->mdev.graph_mutex);
+
+	spin_lock_irqsave(&csis->q_lock, flags);
+	list_for_each_entry_safe(buf, tmp, &csis->ready_q, list) {
+		list_del(&buf->list);
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
+	}
+	spin_unlock_irqrestore(&csis->q_lock, flags);
+
+	dev_err(csis->dev, "pipeline start failed with %d\n", ret);
+	return ret;
+}
+
+/**
+ * fsd_stop_streaming() - stop streaming for CSI context
+ * @q: pointer to vb2_queue in use
+ * Return: none
+ */
+static void fsd_csis_stop_streaming(struct vb2_queue *q)
+{
+	unsigned long flags;
+	struct fsd_csis *csis = vb2_get_drv_priv(q);
+	struct fsd_csis_vb2_buffer *buf, *tmp;
+	unsigned int timeout_cnt = 0;
+	int i;
+	void __iomem *dma_act_ctrl = 0;
+
+	fsd_csis_dma_enable(csis, false);
+
+	dma_act_ctrl = csis->dma_base + FSD_CSIS_DMA_ACT_CTRL(csis->current_vc);
+
+	while ((readl(dma_act_ctrl) & 0x1) == 0x0) {
+		if (timeout_cnt > 50) {
+			dev_dbg(csis->dev, "DMA did not finish in 500ms.\n");
+			break;
+		}
+		usleep_range(10000, 20000); /* Wait min 10ms, max 20ms */
+		timeout_cnt++;
+	}
+
+	mutex_lock(&csis->mdev.graph_mutex);
+	v4l2_subdev_disable_streams(&csis->subdev.sd, FSD_CSIS_PAD_SRC,
+				    BIT(0));
+	__video_device_pipeline_stop(csis->vdev);
+	mutex_unlock(&csis->mdev.graph_mutex);
+	/*
+	 * If still DMA operation exists after disabled irq, it will
+	 * update dma_done part in interrupt source register. For next
+	 * streaming session, this could be interpreted as current session's
+	 * first frame done. To prevent this incorrect dma_done receiving,
+	 * clearing interrupt source register here.
+	 */
+
+	/* Release all active buffers */
+	spin_lock_irqsave(&csis->q_lock, flags);
+	list_for_each_entry_safe(buf, tmp, &csis->ready_q, list) {
+		list_del(&buf->list);
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+	}
+	spin_unlock_irqrestore(&csis->q_lock, flags);
+
+	for (i = 0; i < FSD_CSIS_NB_OF_BUFS_ON_DMA_CHANNELS; i++) {
+		buf = csis->frame[i];
+		if (buf)
+			vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+	}
+}
+
+static int fsd_csis_video_open(struct file *file)
+{
+	struct fsd_csis *csis = video_drvdata(file);
+	int ret;
+	struct vb2_queue *q = &csis->q;
+
+	if (vb2_is_busy(q)) {
+		dev_err(csis->dev, "device busy\n");
+		return -EBUSY;
+	}
+
+	ret = pm_runtime_resume_and_get(csis->dev);
+	if (ret < 0)
+		return ret;
+
+	ret = v4l2_fh_open(file);
+
+	if (ret) {
+		dev_err(csis->dev, "v4l2_fh_open failed\n");
+		goto err;
+	}
+
+	return ret;
+
+err:
+	pm_runtime_put(csis->dev);
+	return ret;
+}
+
+static void fsd_csis_irq_worker(struct fsd_csis *csis)
+{
+	struct fsd_csis_vb2_buffer *buf_from;
+	struct fsd_csis_vb2_buffer *buf_to;
+	struct v4l2_subdev *subdev = csis->source.subdev;
+	u8 i;
+	void __iomem *dma_act_ctrl;
+	unsigned long flags;
+
+	dma_act_ctrl = csis->dma_base + FSD_CSIS_DMA_ACT_CTRL(csis->current_vc);
+	csis->current_dma_ptr = readl(dma_act_ctrl);
+	csis->current_dma_ptr = get_bits(csis->current_dma_ptr,
+					 FSD_CSIS_ACTIVE_DMA_FRAMEPTR_MASK);
+
+	v4l2_subdev_call(subdev, core, command, 5,
+			 &csis->current_frame_counter);
+
+	if (csis->dma_error) {
+		dev_err(csis->dev, "prev_dma: %d, cur_dma: %d, prev_frm: %d, cur_frm: %d\n",
+			csis->prev_dma_ptr, csis->current_dma_ptr, csis->prev_frame_counter,
+			csis->current_frame_counter);
+		csis->prev_dma_ptr = csis->current_dma_ptr;
+		goto update_prev_counters;
+	}
+
+	if (csis->current_dma_ptr >= csis->prev_dma_ptr)
+		csis->number_of_ready_bufs =
+			csis->current_dma_ptr - csis->prev_dma_ptr;
+	else
+		csis->number_of_ready_bufs =
+			FSD_CSIS_NB_DMA_OUT_CH - csis->prev_dma_ptr
+			+ csis->current_dma_ptr;
+
+	if (csis->number_of_ready_bufs >= FSD_CSIS_NB_OF_BUFS_ON_DMA_CHANNELS ||
+	    ((csis->current_frame_counter - csis->prev_frame_counter)
+	    >= FSD_CSIS_NB_DMA_OUT_CH)) {
+		/* In case of CSIS_NB_OF_BUFS_ON_DMA_CHANNELS or CSIS_NUM_DMA_OUT_CH number
+		 * of frames delays or more, set how many recent frames are ready to be read
+		 * in the next interrupt. This cannot be more than
+		 * CSIS_NB_OF_BUFS_ON_DMA_CHANNELS-1 frames.
+		 */
+		csis->number_of_ready_bufs = FSD_CSIS_NB_OF_BUFS_ON_DMA_CHANNELS - 1;
+		csis->prev_dma_ptr = (csis->current_dma_ptr -
+					FSD_CSIS_NB_OF_BUFS_ON_DMA_CHANNELS)
+					& (FSD_CSIS_NB_DMA_OUT_CH - 1);
+		dev_err(csis->dev, "interrupt delayed %d frames\n",
+			csis->number_of_ready_bufs);
+	}
+
+	if (csis->number_of_ready_bufs == 0) {
+		dev_err(csis->dev, "Interrupt burst number_of_ready_bufs: %d\n",
+			csis->number_of_ready_bufs);
+		goto update_prev_counters;
+	} else {
+		if (csis->number_of_ready_bufs > 1) {
+			/*
+			 * Interrupt has been missed. Do not populate DMA_ACT_CTRL pointer.
+			 * Notify buffers ready until (DMA_ACT_CTRL - 1) pointer.
+			 * Because,the delayed interrupt might be arrived in DMA active
+			 * time.
+			 */
+			csis->number_of_ready_bufs--;
+			dev_err(csis->dev, "interrupt got delayed %d frames\n",
+				csis->number_of_ready_bufs);
+		}
+	}
+
+	for (i = 0; i < csis->number_of_ready_bufs; i++) {
+		bool is_same_modulo;
+
+		csis->prev_dma_ptr = (csis->prev_dma_ptr + 1) % FSD_CSIS_NB_DMA_OUT_CH;
+		is_same_modulo = !((csis->prev_dma_ptr - (csis->current_dma_ptr + 1)) %
+					FSD_CSIS_NB_OF_BUFS_ON_DMA_CHANNELS);
+
+		spin_lock_irqsave(&csis->q_lock, flags);
+
+		/*
+		 * Before dequeuing buffer from DMA at least
+		 * one buffer should be ready in vb2_queue
+		 */
+		if (list_empty(&csis->ready_q)) {
+			spin_unlock_irqrestore(&csis->q_lock, flags);
+			csis->prev_dma_ptr = csis->current_dma_ptr;
+			goto update_prev_counters;
+
+		} else {
+			buf_from = list_entry(csis->ready_q.next,
+					      struct fsd_csis_vb2_buffer, list);
+			list_del(&buf_from->list);
+		}
+
+		spin_unlock_irqrestore(&csis->q_lock, flags);
+
+		buf_to = csis->frame[csis->prev_dma_ptr];
+
+		if (is_same_modulo) {
+			if (csis->current_dma_ptr != fsd_csis_current_dma_ptr(csis)) {
+				spin_lock_irqsave(&csis->q_lock, flags);
+				list_add_tail(&buf_from->list, &csis->ready_q);
+				spin_unlock_irqrestore(&csis->q_lock, flags);
+				continue;
+			}
+		}
+
+		fsd_csis_add_to_ring_buffer(csis, buf_from, csis->prev_dma_ptr);
+
+		if (buf_to) {
+			buf_to->vb.vb2_buf.timestamp = ktime_get_ns();
+			vb2_buffer_done(&buf_to->vb.vb2_buf,
+					VB2_BUF_STATE_DONE);
+		}
+	}
+
+update_prev_counters:
+	csis->prev_frame_counter = csis->current_frame_counter;
+}
+
+static irqreturn_t csis_irq_handler(int irq_csis, void *data)
+{
+	struct fsd_csis *csis = data;
+	struct v4l2_subdev *subdev = csis->source.subdev;
+	unsigned int int_src1 = 0x0;
+	unsigned int int1_err = 0x0;
+	unsigned int dma_error = 0x0, dma_err_code = 0x0, dma_error_vc = 0x0;
+	unsigned int err = 0x0;
+	unsigned int dma_frame_end = 0x0, dma_frame_end_vc = 0x0;
+	int i;
+
+	v4l2_subdev_call(subdev, core, command, 2, &int_src1);
+	int1_err = get_bits(int_src1, FSD_CSIS_INT_SRC1_ERR_ALL_MASK);
+
+	dma_frame_end = get_bits(int_src1, FDS_CSIS_DMA_FRM_END_MASK);
+
+	if (int1_err) {
+		err = get_bits(int_src1, FSD_CSIS_DMA_OTF_OVERLAP_MASK);
+		if (err)
+			dev_err(csis->dev, "DMA OTF OVERLAP %x\n", err);
+
+		dma_error = get_bits(int_src1, FSD_CSIS_DMA_ERROR_MASK);
+
+		if (dma_error) {
+			dev_err(csis->dev, "DMA ERROR %x\n", dma_error);
+			dma_err_code = readl(csis->dma_base + FSD_CSIS_DMA_ERR_CODE);
+			dev_err(csis->dev, "Error code %x", dma_err_code);
+		}
+	}
+
+	if (dma_frame_end || dma_error) {
+		for (i = 0; i < FSD_CSIS_MAX_VC; i++) {
+			dma_frame_end_vc = (dma_frame_end >> i) & 0x01;
+			if (dma_error) {
+				dma_error_vc = int_src1 & (FSD_CSIS_DMA_CH0_MASK << i);
+				dma_error_vc |= ((dma_err_code & (FSD_CSIS_DMAFIFO_FULL_MASK |
+								FSD_CSIS_TRXFIFO_FULL_MASK |
+								0x01 << i)) << 18);
+			}
+
+			if (dma_frame_end_vc || dma_error_vc) {
+				csis->dma_error = dma_error_vc;
+				fsd_csis_irq_worker(csis);
+			}
+		}
+	}
+
+	v4l2_subdev_call(subdev, core, command, 3, &int_src1);
+
+	return IRQ_HANDLED;
+}
+
+static int fsd_csis_video_release(struct file *file)
+{
+	struct fsd_csis *csis = video_drvdata(file);
+	int ret;
+
+	ret = vb2_fop_release(file);
+
+	if (ret)
+		return ret;
+
+	pm_runtime_put(csis->dev);
+
+	return ret;
+}
+
+static int fsd_csis_video_querycap(struct file *file, void *priv,
+				   struct v4l2_capability *cap)
+{
+	struct fsd_csis *csis = video_drvdata(file);
+
+	strscpy(cap->driver, FSD_CSIS_MODULE_NAME, sizeof(cap->driver));
+	strscpy(cap->card, FSD_CSIS_MODULE_NAME, sizeof(cap->card));
+
+	snprintf(cap->bus_info, sizeof(cap->bus_info),
+		 "platform:%s", dev_name(csis->dev));
+	return 0;
+}
+
+static int fsd_csis_video_enum_fmt_vid_cap(struct file *file, void *priv,
+					   struct v4l2_fmtdesc *f)
+{
+	unsigned int index = f->index;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(pixel_formats); i++) {
+		const struct fsd_csis_pixfmt *fmt = &pixel_formats[i];
+
+		if (f->mbus_code) {
+			unsigned int j;
+
+			if (!fmt->codes)
+				continue;
+
+			for (j = 0; fmt->codes[j]; j++) {
+				if (f->mbus_code == fmt->codes[j])
+					break;
+			}
+
+			if (!fmt->codes[j])
+				continue;
+		}
+
+		if (index == 0) {
+			f->pixelformat = fmt->fourcc;
+			return 0;
+		}
+
+		index--;
+	}
+
+	return -EINVAL;
+}
+
+/*
+ * Search in the pixel_formats[] array for an entry with the given fourcc
+ * return it.
+ */
+static const struct fsd_csis_pixfmt *fsd_csis_find_pixel_format(u32 fourcc)
+{
+	const struct fsd_csis_pixfmt *fmt;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(pixel_formats); i++) {
+		fmt = &pixel_formats[i];
+
+		if (fmt->fourcc == fourcc)
+			return fmt;
+	}
+
+	return NULL;
+}
+
+/*
+ * Search in the pixel_formats[] array for an entry with the given media
+ * bus code and return it.
+ */
+static const struct fsd_csis_pixfmt *fsd_csis_find_mbus_format(u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(pixel_formats); i++) {
+		const struct fsd_csis_pixfmt *fmt = &pixel_formats[i];
+		unsigned int j;
+
+		if (!fmt->codes)
+			continue;
+
+		for (j = 0; fmt->codes[j]; j++) {
+			if (code == fmt->codes[j])
+				return fmt;
+		}
+	}
+
+	return NULL;
+}
+
+static int fsd_csis_mbus_fmt_to_pix_fmt(struct v4l2_pix_format *pix,
+					const struct v4l2_mbus_framefmt *mbus,
+					const struct fsd_csis_pixfmt *cc)
+{
+	u32 width;
+	u32 stride;
+
+	if (!cc) {
+		cc = fsd_csis_find_mbus_format(mbus->code);
+		if (!cc)
+			return -EINVAL;
+	}
+
+	/* Round up width for minimum burst size */
+	width = round_up(mbus->width, 8);
+
+	/* Round up stride for IDMAC line start address alignment */
+	stride = round_up((width * cc->bpp) >> 3, 8);
+
+	pix->width = width;
+	pix->height = mbus->height;
+	pix->pixelformat = cc->fourcc;
+	pix->colorspace = mbus->colorspace;
+	pix->xfer_func = mbus->xfer_func;
+	pix->ycbcr_enc = mbus->ycbcr_enc;
+	pix->quantization = mbus->quantization;
+	pix->field = mbus->field;
+	pix->bytesperline = stride;
+	pix->sizeimage = stride * pix->height;
+
+	return 0;
+}
+
+static const struct fsd_csis_pixfmt
+	*__fsd_csis_video_try_fmt_vid_cap(struct fsd_csis *csis,
+					  struct v4l2_pix_format *pixfmt)
+{
+	struct v4l2_mbus_framefmt fmt_src;
+	const struct fsd_csis_pixfmt *cc;
+	struct v4l2_rect *compose = &csis->vdev_compose;
+
+	/*
+	 * Find the pixel format, default to the first supported format if not
+	 * found.
+	 */
+	cc = fsd_csis_find_pixel_format(pixfmt->pixelformat);
+
+	if (!cc) {
+		pixfmt->pixelformat = FSD_CSIS_DEF_PIX_FORMAT;
+		pixfmt->height = FSD_CSIS_DEF_PIX_HEIGHT;
+		pixfmt->width = FSD_CSIS_DEF_PIX_WIDTH;
+		pixfmt->colorspace = V4L2_COLORSPACE_SRGB;
+		pixfmt->field = V4L2_FIELD_NONE;
+		cc = fsd_csis_find_pixel_format(pixfmt->pixelformat);
+	}
+
+	v4l2_fill_mbus_format(&fmt_src, pixfmt, cc->codes[0]);
+	fsd_csis_mbus_fmt_to_pix_fmt(pixfmt, &fmt_src, cc);
+
+	compose->width = fmt_src.width;
+	compose->height = fmt_src.height;
+
+	csis->vdev_fmt = *pixfmt;
+	return cc;
+}
+
+static int fsd_csis_video_try_fmt_vid_cap(struct file *file, void *fh,
+					  struct v4l2_format *f)
+{
+	struct fsd_csis *csis = video_drvdata(file);
+
+	__fsd_csis_video_try_fmt_vid_cap(csis, &f->fmt.pix);
+	return 0;
+}
+
+static int fsd_csis_video_s_fmt_vid_cap(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct fsd_csis *csis = video_drvdata(file);
+	struct v4l2_subdev *sd = &csis->subdev.sd;
+	const struct fsd_csis_pixfmt *cc;
+	struct vb2_queue *q = &csis->q;
+	int ret;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.pad = 0,
+	};
+
+	if (vb2_is_busy(q)) {
+		dev_err(csis->dev, "%s queue busy\n", __func__);
+		return -EBUSY;
+	}
+
+	cc = __fsd_csis_video_try_fmt_vid_cap(csis, &f->fmt.pix);
+	v4l2_fill_mbus_format(&fmt.format, &f->fmt.pix, cc->codes[0]);
+	ret = v4l2_subdev_call(sd, pad, set_fmt, sd->active_state, &fmt);
+
+	if (ret < 0) {
+		dev_err(csis->dev, "subdev format set failed %d\n", ret);
+		return ret;
+	}
+
+	csis->vdev_cc = cc;
+	csis->vdev_fmt = f->fmt.pix;
+	return 0;
+}
+
+static int fsd_csis_video_g_fmt_vid_cap(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct fsd_csis *csis = video_drvdata(file);
+
+	f->fmt.pix = csis->vdev_fmt;
+
+	return 0;
+}
+
+static const struct vb2_ops fsd_csis_video_qops = {
+	.queue_setup            = fsd_csis_queue_setup,
+	.buf_prepare            = fsd_csis_buffer_prepare,
+	.buf_queue              = fsd_csis_buffer_queue,
+	.start_streaming        = fsd_csis_start_streaming,
+	.stop_streaming         = fsd_csis_stop_streaming,
+	.wait_prepare           = vb2_ops_wait_prepare,
+	.wait_finish            = vb2_ops_wait_finish,
+};
+
+static const struct v4l2_ioctl_ops fsd_csis_video_ioctl_ops = {
+	.vidioc_querycap      = fsd_csis_video_querycap,
+
+	.vidioc_enum_fmt_vid_cap  = fsd_csis_video_enum_fmt_vid_cap,
+
+	.vidioc_try_fmt_vid_cap   = fsd_csis_video_try_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap     = fsd_csis_video_s_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap     = fsd_csis_video_g_fmt_vid_cap,
+
+	.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_prepare_buf   = vb2_ioctl_prepare_buf,
+	.vidioc_create_bufs   = vb2_ioctl_create_bufs,
+	.vidioc_streamon      = vb2_ioctl_streamon,
+	.vidioc_streamoff     = vb2_ioctl_streamoff,
+};
+
+/**
+ * V4L2 File operations
+ */
+static const struct v4l2_file_operations fsd_csis_video_fops = {
+	.owner          = THIS_MODULE,
+	.open           = fsd_csis_video_open,
+	.release        = fsd_csis_video_release,
+	.read           = vb2_fop_read,
+	.poll           = vb2_fop_poll,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap           = vb2_fop_mmap,
+};
+
+static int fsd_csi_notify_bound(struct v4l2_async_notifier *notifier,
+				struct v4l2_subdev *subdev,
+				struct v4l2_async_connection *asd)
+{
+	struct fsd_csis *csis = notifier_to_csis(notifier);
+	struct media_pad *sink = &csis->subdev.pad[FSD_CSIS_PAD_SINK];
+	struct media_pad *source;
+	int ret;
+
+	dev_dbg(csis->dev, "Hooked csis subdevice: %s to parent\n",
+		subdev->name);
+
+	ret = v4l2_create_fwnode_links_to_pad(subdev, sink, MEDIA_LNK_FL_ENABLED);
+
+	if (ret)
+		return ret;
+
+	source = media_pad_remote_pad_unique(sink);
+	if (IS_ERR(source)) {
+		dev_err(csis->dev, "No connected source pad\n");
+		return PTR_ERR(source);
+	}
+
+	csis->source.subdev = subdev;
+	csis->source.pad = source;
+
+	return 0;
+}
+
+static const struct v4l2_async_notifier_operations fsd_csi_notify_ops = {
+	.bound = fsd_csi_notify_bound,
+};
+
+static const struct media_device_ops fsd_csis_media_ops = {
+	.link_notify = v4l2_pipeline_link_notify,
+};
+
+static const struct media_entity_operations fsd_csis_entity_ops = {
+	.link_validate  = v4l2_subdev_link_validate,
+	.get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1,
+};
+
+static int fsd_csis_media_dev_init(struct fsd_csis *csis)
+{
+	int ret;
+
+	strscpy(csis->mdev.model, "fsd-csis-media", sizeof(csis->mdev.model));
+	csis->mdev.ops = &fsd_csis_media_ops;
+	csis->mdev.dev = csis->dev;
+
+	csis->v4l2_dev.mdev = &csis->mdev;
+	strscpy(csis->v4l2_dev.name, "fsd-csis-media",
+		sizeof(csis->v4l2_dev.name));
+	snprintf(csis->mdev.bus_info, sizeof(csis->mdev.bus_info),
+		 "platform:%s", dev_name(csis->mdev.dev));
+
+	media_device_init(&csis->mdev);
+
+	ret = v4l2_device_register(csis->dev, &csis->v4l2_dev);
+
+	if (ret < 0) {
+		v4l2_err(&csis->v4l2_dev,
+			 "Failed to register v4l2_device: %d\n", ret);
+		goto cleanup;
+	}
+
+	return 0;
+
+cleanup:
+	media_device_cleanup(&csis->mdev);
+
+	return ret;
+}
+
+static void fsd_csis_media_cleanup(struct fsd_csis *csis)
+{
+	v4l2_device_unregister(&csis->v4l2_dev);
+	media_device_unregister(&csis->mdev);
+	v4l2_subdev_cleanup(&csis->subdev.sd);
+	media_device_cleanup(&csis->mdev);
+}
+
+static int fsd_csis_video_init(struct fsd_csis *csis)
+{
+	struct video_device *vdev;
+	struct vb2_queue *vq;
+	int ret;
+
+	mutex_init(&csis->vdev_mutex);
+	INIT_LIST_HEAD(&csis->ready_q);
+	spin_lock_init(&csis->q_lock);
+	spin_lock_init(&csis->dma_reg_lock);
+
+	/* Allocate and initialize the video device.*/
+	vdev = video_device_alloc();
+	if (!vdev)
+		return -ENOMEM;
+
+	vdev->fops = &fsd_csis_video_fops;
+	vdev->ioctl_ops = &fsd_csis_video_ioctl_ops;
+	vdev->minor = -1;
+	vdev->release = video_device_release;
+	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	vdev->lock = &csis->vdev_mutex;
+	vdev->queue = &csis->q;
+
+	snprintf(vdev->name, sizeof(vdev->name), "%s capture", csis->subdev.sd.name);
+
+	video_set_drvdata(vdev, csis);
+	csis->vdev = vdev;
+
+	/* Initialize the video device pad. */
+	csis->vdev_pad.flags = MEDIA_PAD_FL_SINK;
+
+	ret = media_entity_pads_init(&vdev->entity, 1, &csis->vdev_pad);
+	if (ret) {
+		video_device_release(vdev);
+		return ret;
+	}
+
+	/* Initialize the vb2 queue. */
+	vq = &csis->q;
+	vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+	vq->drv_priv = csis;
+	vq->buf_struct_size = sizeof(struct fsd_csis_vb2_buffer);
+	vq->ops = &fsd_csis_video_qops;
+	vq->mem_ops = &vb2_dma_contig_memops;
+	vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	vq->lock = &csis->vdev_mutex;
+	vq->min_reqbufs_allocation = FSD_CSIS_NB_OF_BUFS_ON_DMA_CHANNELS + 1;
+	vq->min_queued_buffers = FSD_CSIS_NB_MIN_CH;
+	vq->dev = csis->dev;
+
+	ret = vb2_queue_init(vq);
+	if (ret) {
+		dev_err(csis->dev, "vb2_queue_init failed\n");
+		video_device_release(vdev);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void fsd_csis_video_init_format(struct fsd_csis *csis)
+{
+	csis->vdev_fmt.width = FSD_CSIS_DEF_PIX_WIDTH;
+	csis->vdev_fmt.height = FSD_CSIS_DEF_PIX_HEIGHT;
+	csis->vdev_fmt.pixelformat = FSD_CSIS_DEF_PIX_FORMAT;
+	csis->vdev_fmt.colorspace = V4L2_COLORSPACE_SRGB;
+	csis->vdev_fmt.field = V4L2_FIELD_NONE;
+
+	csis->vdev_cc = fsd_csis_find_pixel_format(csis->vdev_fmt.pixelformat);
+
+	csis->vdev_fmt.bytesperline = bytes_per_line(FSD_CSIS_DEF_PIX_WIDTH,
+						     csis->vdev_cc->bpp);
+	csis->vdev_fmt.sizeimage = csis->vdev_fmt.bytesperline *
+					csis->vdev_fmt.height;
+}
+
+static int fsd_csis_video_register(struct fsd_csis *csis)
+{
+	struct v4l2_subdev *sd = &csis->subdev.sd;
+	struct v4l2_device *v4l2_dev = sd->v4l2_dev;
+	struct video_device *vdev = csis->vdev;
+	int ret;
+
+	vdev->v4l2_dev = v4l2_dev;
+
+	/* Initialize the default format and compose rectangle. */
+	fsd_csis_video_init_format(csis);
+
+	/* Register the video device. */
+	ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
+	if (ret) {
+		dev_err(csis->dev, "Failed to register video device\n");
+		return ret;
+	}
+
+	dev_info(csis->dev, "Registered %s as /dev/%s\n", vdev->name,
+		 video_device_node_name(vdev));
+
+	/* Create the link from the CSI subdev to the video device. */
+	ret = media_create_pad_link(&sd->entity, FSD_CSIS_PAD_SRC,
+				    &vdev->entity, 0, MEDIA_LNK_FL_IMMUTABLE |
+				    MEDIA_LNK_FL_ENABLED);
+	if (ret) {
+		dev_err(csis->dev, "failed to create link to device node\n");
+		video_unregister_device(vdev);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void fsd_csis_video_unregister(struct fsd_csis *csis)
+{
+	media_entity_cleanup(&csis->vdev->entity);
+	video_unregister_device(csis->vdev);
+}
+
+static int fsd_csis_registered(struct v4l2_subdev *sd)
+{
+	struct fsd_csis *csis = v4l2_get_subdevdata(sd);
+	int ret;
+
+	ret = fsd_csis_video_init(csis);
+	if (ret)
+		return ret;
+
+	ret = fsd_csis_video_register(csis);
+	if (ret)
+		return ret;
+
+	ret = v4l2_device_register_subdev_nodes(&csis->v4l2_dev);
+	if (ret)
+		goto err_unregister;
+
+	ret = media_device_register(&csis->mdev);
+	if (ret)
+		goto err_unregister;
+
+	return 0;
+
+err_unregister:
+	fsd_csis_video_unregister(csis);
+
+	return ret;
+}
+
+static void fsd_csis_unregistered(struct v4l2_subdev *sd)
+{
+	struct fsd_csis *csis = v4l2_get_subdevdata(sd);
+
+	fsd_csis_video_unregister(csis);
+}
+
+static int fsd_csis_sd_set_fmt(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *sd_state,
+			       struct v4l2_subdev_format *sdformat)
+{
+	struct fsd_csis *csis = v4l2_get_subdevdata(sd);
+	struct v4l2_subdev *subdev = csis->source.subdev;
+	struct v4l2_mbus_framefmt *fmt;
+	const struct fsd_csis_pixfmt *cc;
+
+	if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE && csis->is_streaming)
+		return -EBUSY;
+
+	if (sdformat->pad == FSD_CSIS_PAD_SRC)
+		return v4l2_subdev_get_fmt(sd, sd_state, sdformat);
+
+	if (sdformat->pad != FSD_CSIS_PAD_SINK)
+		return -EINVAL;
+
+	cc = fsd_csis_find_mbus_format(sdformat->format.code);
+	if (!cc)
+		cc = fsd_csis_find_mbus_format(FSD_CSIS_DEF_MBUS_CODE);
+
+	fmt = v4l2_subdev_state_get_format(sd_state, sdformat->pad);
+
+	fmt->code = cc->codes[0];
+	fmt->width = sdformat->format.width;
+	fmt->height = sdformat->format.height;
+	fmt->field = V4L2_FIELD_NONE;
+
+	sdformat->format = *fmt;
+
+	/* Propagate the format from sink to source. */
+	fmt = v4l2_subdev_state_get_format(sd_state, FSD_CSIS_PAD_SRC);
+	*fmt = sdformat->format;
+
+	return v4l2_subdev_call(subdev, pad, set_fmt, subdev->active_state, sdformat);
+}
+
+static int __fsd_csis_sd_set_routing(struct v4l2_subdev *sd,
+				     struct v4l2_subdev_state *state,
+				     struct v4l2_subdev_krouting *routing)
+{
+	struct v4l2_subdev_route *route;
+	int ret;
+
+	ret = v4l2_subdev_routing_validate(sd, routing,
+					   V4L2_SUBDEV_ROUTING_ONLY_1_TO_1);
+	if (ret)
+		return ret;
+
+	ret = v4l2_subdev_set_routing(sd, state, routing);
+	if (ret)
+		return ret;
+
+	for_each_active_route(&state->routing, route) {
+		const struct v4l2_mbus_framefmt *def_fmt;
+		struct v4l2_mbus_framefmt *fmt;
+
+		def_fmt = &fsd_csis_default_format;
+
+		fmt = v4l2_subdev_state_get_format(state, route->sink_pad,
+						   route->sink_stream);
+		*fmt = *def_fmt;
+		fmt = v4l2_subdev_state_get_format(state, route->source_pad,
+						   route->source_stream);
+		*fmt = *def_fmt;
+	}
+
+	return 0;
+}
+
+static int fsd_csis_sd_set_routing(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *state,
+				   enum v4l2_subdev_format_whence which,
+				   struct v4l2_subdev_krouting *routing)
+{
+	struct fsd_csis *csis = v4l2_get_subdevdata(sd);
+
+	if (which == V4L2_SUBDEV_FORMAT_ACTIVE && csis->is_streaming)
+		return -EBUSY;
+
+	return __fsd_csis_sd_set_routing(sd, state, routing);
+}
+
+static int fsd_csis_sd_enable_streams(struct v4l2_subdev *sd,
+				      struct v4l2_subdev_state *state,
+				      u32 pad, u64 streams_mask)
+{
+	struct fsd_csis *csis = v4l2_get_subdevdata(sd);
+
+	return v4l2_subdev_enable_streams(csis->source.subdev,
+					FSD_CSIS_PAD_SRC, BIT(0));
+}
+
+static int fsd_csis_sd_disable_streams(struct v4l2_subdev *sd,
+				       struct v4l2_subdev_state *state,
+				       u32 pad, u64 streams_mask)
+{
+	struct fsd_csis *csis = v4l2_get_subdevdata(sd);
+
+	return v4l2_subdev_disable_streams(csis->source.subdev,
+					FSD_CSIS_PAD_SRC, BIT(0));
+}
+
+static int fsd_csis_init_state(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *state)
+{
+	struct v4l2_subdev_route routes[] = {
+		{
+			.sink_pad = FSD_CSIS_PAD_SINK,
+			.sink_stream = 0,
+			.source_pad = FSD_CSIS_PAD_SRC,
+			.source_stream = 0,
+			.flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE,
+		},
+	};
+
+	struct v4l2_subdev_krouting routing = {
+		.len_routes = ARRAY_SIZE(routes),
+		.num_routes = ARRAY_SIZE(routes),
+		.routes = routes,
+	};
+
+	return __fsd_csis_sd_set_routing(sd, state, &routing);
+}
+
+static const struct v4l2_subdev_internal_ops fsd_csis_internal_ops = {
+	.init_state	= fsd_csis_init_state,
+	.registered	= fsd_csis_registered,
+	.unregistered	= fsd_csis_unregistered,
+};
+
+static const struct v4l2_subdev_pad_ops fsd_csis_pad_ops = {
+	.get_fmt		= v4l2_subdev_get_fmt,
+	.set_fmt		= fsd_csis_sd_set_fmt,
+	.set_routing		= fsd_csis_sd_set_routing,
+	.enable_streams		= fsd_csis_sd_enable_streams,
+	.disable_streams	= fsd_csis_sd_disable_streams,
+};
+
+static const struct v4l2_subdev_ops fsd_csis_subdev_ops = {
+	.pad = &fsd_csis_pad_ops,
+};
+
+static int fsd_csis_media_init(struct fsd_csis *csis)
+{
+	struct v4l2_subdev *sd = &csis->subdev.sd;
+	int ret, i;
+
+	/* add media device */
+	ret = fsd_csis_media_dev_init(csis);
+	if (ret)
+		return ret;
+
+	v4l2_subdev_init(sd, &fsd_csis_subdev_ops);
+	v4l2_set_subdevdata(sd, csis);
+	sd->internal_ops = &fsd_csis_internal_ops;
+	sd->entity.ops = &fsd_csis_entity_ops;
+	sd->entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+	sd->dev = csis->dev;
+	sd->owner = THIS_MODULE;
+	sd->flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+	snprintf(sd->name, sizeof(sd->name), "csis");
+
+	for (i = 0; i < FSD_CSIS_PADS_NUM; i++)
+		csis->subdev.pad[i].flags = (i == FSD_CSIS_PAD_SINK) ?
+			MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&sd->entity, FSD_CSIS_PADS_NUM,
+				     csis->subdev.pad);
+
+	if (ret)
+		goto error;
+
+	ret = v4l2_subdev_init_finalize(sd);
+	if (ret)
+		goto error;
+
+	ret = v4l2_device_register_subdev(&csis->v4l2_dev, sd);
+	if (ret)
+		goto error;
+
+	return 0;
+error:
+	fsd_csis_media_cleanup(csis);
+	return ret;
+}
+
+static int fsd_csis_async_register(struct fsd_csis *csis)
+{
+	struct v4l2_async_connection *asd;
+	struct fwnode_handle *ep;
+	int ret;
+
+	v4l2_async_nf_init(&csis->notifier, &csis->v4l2_dev);
+
+	ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(csis->dev), 0, 0,
+					     FWNODE_GRAPH_ENDPOINT_NEXT);
+
+	if (ep) {
+		asd = v4l2_async_nf_add_fwnode_remote(&csis->notifier, ep,
+						      struct v4l2_async_connection);
+		fwnode_handle_put(ep);
+
+		if (IS_ERR(asd)) {
+			ret = PTR_ERR(asd);
+			/* OK if asd already exists */
+			if (ret != -EEXIST)
+				goto error;
+		}
+	}
+
+	csis->notifier.ops = &fsd_csi_notify_ops;
+
+	ret = v4l2_async_nf_register(&csis->notifier);
+	if (ret)
+		goto error;
+
+	return 0;
+
+error:
+	v4l2_async_nf_cleanup(&csis->notifier);
+	return ret;
+}
+
+static int fsd_csis_clk_get(struct fsd_csis *csis)
+{
+	int i;
+
+	csis->clks = devm_kcalloc(csis->dev, FSD_CSIS_NB_CLOCK, sizeof(*csis->clks), GFP_KERNEL);
+
+	if (!csis->clks)
+		return -ENOMEM;
+
+	for (i = 0; i < FSD_CSIS_NB_CLOCK; i++)
+		csis->clks[i].id = fsd_csis_clk_id[i];
+
+	return devm_clk_bulk_get(csis->dev, FSD_CSIS_NB_CLOCK, csis->clks);
+}
+
+static int fsd_csis_runtime_suspend(struct device *dev)
+{
+	struct fsd_csis *csis = dev_get_drvdata(dev);
+
+	clk_bulk_disable_unprepare(FSD_CSIS_NB_CLOCK, csis->clks);
+
+	return 0;
+}
+
+static int fsd_csis_runtime_resume(struct device *dev)
+{
+	struct fsd_csis *csis = dev_get_drvdata(dev);
+
+	return clk_bulk_prepare_enable(FSD_CSIS_NB_CLOCK, csis->clks);
+}
+
+static const struct dev_pm_ops fsd_csis_pm_ops = {
+	SET_RUNTIME_PM_OPS(fsd_csis_runtime_suspend, fsd_csis_runtime_resume,
+			   NULL)
+};
+
+static int fsd_csis_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct fsd_csis *csis;
+	int ret = 0;
+	int irq;
+
+	csis = devm_kzalloc(dev, sizeof(*csis), GFP_KERNEL);
+	if (!csis)
+		return -ENOMEM;
+
+	csis->dev = dev;
+	csis->info = of_device_get_match_data(dev);
+
+	csis->dma_base = devm_platform_ioremap_resource(pdev, 0);
+	if (IS_ERR(csis->dma_base))
+		return PTR_ERR(csis->dma_base);
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return irq;
+
+	ret = devm_request_irq(dev, irq,
+			       csis_irq_handler, IRQF_SHARED, pdev->name, csis);
+
+	ret = fsd_csis_clk_get(csis);
+	if (ret < 0)
+		return ret;
+
+	pm_runtime_enable(dev);
+	if (!pm_runtime_enabled(dev)) {
+		ret = fsd_csis_runtime_resume(dev);
+		if (ret < 0)
+			return ret;
+	}
+
+	platform_set_drvdata(pdev, csis);
+
+	csis->pll = devm_clk_get_enabled(dev, "pll");
+	if (IS_ERR(csis->pll)) {
+		dev_err(dev, "Failed to enable pll\n");
+		return PTR_ERR(csis->pll);
+	}
+
+	ret = fsd_csis_media_init(csis);
+	if (ret)
+		return ret;
+
+	ret = fsd_csis_async_register(csis);
+	if (ret)
+		goto err_media_cleanup;
+
+	return 0;
+
+err_media_cleanup:
+	fsd_csis_media_cleanup(csis);
+
+	return ret;
+}
+
+static void fsd_csis_remove(struct platform_device *pdev)
+{
+	struct fsd_csis *csis = platform_get_drvdata(pdev);
+
+	fsd_csis_media_cleanup(csis);
+
+	v4l2_async_nf_unregister(&csis->notifier);
+	v4l2_async_nf_cleanup(&csis->notifier);
+	v4l2_async_unregister_subdev(&csis->subdev.sd);
+
+	if (!pm_runtime_enabled(csis->dev))
+		fsd_csis_runtime_suspend(csis->dev);
+
+	pm_runtime_disable(csis->dev);
+	pm_runtime_set_suspended(csis->dev);
+}
+
+static const struct of_device_id fsd_csis_of_match[] = {
+	{ .compatible = "tesla,fsd-csis-media", },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(of, fsd_csis_of_match);
+
+static struct platform_driver fsd_csis_driver = {
+	.probe		= fsd_csis_probe,
+	.remove		= fsd_csis_remove,
+	.driver		= {
+		.name		= FSD_CSIS_MODULE_NAME,
+		.of_match_table = fsd_csis_of_match,
+		.pm		= &fsd_csis_pm_ops,
+	},
+};
+
+module_platform_driver(fsd_csis_driver);
+
+MODULE_DESCRIPTION("FSD CSIS Video Capture Driver");
+MODULE_AUTHOR("Inbaraj E <inbaraj.e@samsung.com>");
+MODULE_LICENSE("GPL");
+
-- 
2.49.0