From 16bf3e9bb26d323fd53ec968c0d20a4383c4b4d8 Mon Sep 17 00:00:00 2001
From: Naushir Patuck <naush@raspberrypi.com>
Date: Tue, 14 Feb 2023 20:58:59 +0000
Subject: [PATCH] media: rp1: Add downstream CFE (Camera Front End) driver

Signed-off-by: Naushir Patuck <naush@raspberrypi.com>

v4l2: Add pisp compression format support to v4l2

Signed-off-by: Naushir Patuck <naush@raspberrypi.com>

media: rp1: cfe: Fix use of freed memory on errors

cfe_probe_complete() calls cfe_put() on both success and fail code paths.
This works for the success path, but causes the cfe_device struct to be
freed, even if it will be used later in the teardown code.

Fix this by making the ref handling a bit saner: Let the video nodes
have the refs as they do now, but also keep a ref in the "main" driver,
released only at cfe_remove() time. This way the driver does not depend
on the video nodes keeping the refs.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

media: rp1: cfe: Fix width & height in cfe_start_channel()

The logic for handling width & height in cfe_start_channel() is somewhat
odd and, afaics, broken. The code reads:

bool start_fe = is_fe_enabled(cfe) &&
                test_all_nodes(cfe, NODE_ENABLED, NODE_STREAMING);

if (start_fe || is_image_output_node(node)) {
        width = node->fmt.fmt.pix.width;
        height = node->fmt.fmt.pix.height;
}

cfe_start_channel() is called for all video nodes that will be used. So
this means that if, say, fe_stats is enabled as the last node, start_fe
will be true, and width and height will be taken from fe_stats' node.
The width and height will thus contain garbage, which then gets
programmed to the csi2 registers.

It seems that this often still works fine, though, probably if the width
& height are large enough.

Drop the above code, and instead get the width & height from the csi2
subdev's sink pad for the csi2 channel that is used. For metadata the
width & height will be 0 as before.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

media: rp1: cfe: Fix verbose debug print

The debug print in cfe_schedule_next_csi2_job() is printed every frame,
and should thus use cfe_dbg_irq() to avoid spamming, rather than cfe_dbg().

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

media: rp1: cfe: Rename xxx_dbg_irq() to xxx_dbg_verbose()

Rename the xxx_dbg_irq() macros to xxx_dbg_verbose(), as they can be
used to verbose debugs outside irq context too.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

media: rp1: cfe: Add verbose debug module parameter

Expose the verbose debug flag as a module parameter.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

media: rp1: cfe: Drop unused field

Drop 'sensor_embedded_data' field, as it is unused.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

media: rp1: cfe: Fix default meta format's field

Set default meta format's field to V4L2_FIELD_NONE, instead of zeroing
it which indicates V4L2_FIELD_ANY. Metadata doesn't have fields, so NONE
makes sense, and furthermore the default v4l2 link validation will check
for matching fields, or that the sink field is NONE.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

media: rp1: cfe: Fail streaming if FE_CONFIG node is not enabled

When the FE is enabled, ensure that the FE_CONFIG node is enabled.
Otherwise fail cfe_start_streaming() entirely.

Signed-off-by: Naushir Patuck <naush@raspberrypi.com>

drivers: media: rp1_cfe: Remove PISP specific MBUS formats

Remove the MEDIA_BUS_FMT_PISP* format codcs entirely. For the image
pad formats, use the 16-bit Bayer format mbus codes instead. For the
config and stats pad formats, use MEDIA_BUS_FMT_FIXED.

Signed-off-by: Naushir Patuck <naush@raspberrypi.com>

drivers: media: rp1_cfe: Fix link validate test for pixel format

Now that we have removed unique PISP media bus codes, the cfe format
table has multiple entries with the same media bus code for 16-bit
formats. The test in cfe_video_link_validate() did not account for this.
Fix it by testing the media bus code and the V4L2 pixelformat 4cc
together.

As a drive-by, ensure we have a valid CSI2 datatype id when programming
the hardware block.

Signed-off-by: Naushir Patuck <naush@raspberrypi.com>

drivers: media: cfe: Set the CSI-2 link frequency correctly

Use the sensor provided link frequency to set the DPHY timing parameters
on stream_on. This replaces the hard-coded 999 MHz value currently being
used. As a fallback, revert to the original 999 Mhz link frequency.

As a drive-by, fix a 80-character line formatting error.

Signed-off-by: Naushir Patuck <naush@raspberrypi.com>

drivers: media: cfe: Don't confuse MHz and Mbps

The driver was interchaning these units when talking about link rate.
Fix this to avoid confusion. Apart from the logging message change,
there is no function change in this commit.

Signed-off-by: Naushir Patuck <naush@raspberrypi.com>

media: rp1: csi2: Fix missing reg writes

The driver has two places where it writes a register based on a
condition, and when that condition is false, the driver presumes that
the register has the reset value. This is not a good idea, so fix those
places to always write the register.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

media: rp1: fe: Use ~0, not -1, when working with unsigned values

Use ~0, not -1, when working with unsigned values (-1 is not unsigned).

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

media: rp1: Add back reg write debug prints

Add back debug prints in csi2 and pisp_fe reg_write() functions, but use
the 'irq' variants to avoid spamming in normal situation.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

media: rp1: csi2: Track CSI-2 errors

Track the errors from the CSI-2 receiver: overflows and discards. These
are recorded in a table which can be read by the userspace via debugfs.

As tracking the errors may cause much more interrupt load, the tracking
needs to be enabled with a module parameter.

Note that the recording is not perfect: we only record the last
discarded DT for each discard type, instead of recording all of them.
This means that e.g. if the device is discarding two unmatched DTs, the
debugfs file only shows the last one recorded. Recording all of them
would need a more sophisticated recording system to avoid the need of a
very large table, or dynamic allocation.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

media: rp1: csi2: Set values for enum csi2_mode

Set hardcoded values for enum csi2_mode, as the values will be
programmed to HW registers.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

media: rp1: fe: Fix default mbus code

When pisp_fe_pad_set_fmt() is given an mbus code that CFE does not
support, it currently defaults to MEDIA_BUS_FMT_SBGGR10_1X10. This is
not correct, as FE does not support SBGGR10.

Set the default to MEDIA_BUS_FMT_SRGGB16_1X16 instead.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

drivers: media: cfe: Find the source pads on the sensor entity

The driver was assuming that pad 0 on the sensor entity was the
appropriate source pad, but this isn't necessarily the case.
With video-mux, it has the sink pads first, and then the source
pad as the last one.

Iterate through the sensor pads to find the relevant source pads.

Signed-off-by: Dave Stevenson <dave.stevenson@raspberrypi.com>

media: rp1: cfe: Expose find_format_by_pix()

Make find_format_by_pix() accessible to other files in the driver.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

media: rp1: cfe: Add cfe_find_16bit_code() and cfe_find_compressed_code()

Add helper functions which, given an mbus code, return the 16-bit
remapped mbus code or the compressed mbus code.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

media: rp1: csi2: Use get_frame_desc to get CSI-2 VC and DT

Use get_frame_desc pad op for asking the CSI-2 VC and DT from the source
device driver, instead of hardcoding to VC 0, and getting the DT from a
formats table. To keep backward compatibility with sources that do not
implement get_frame_desc, implement a fallback mechanism that always
uses VC 0, and gets the DT from the formats table, based on the CSI2's
sink pad's format.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

media: rp1: cfe: Add is_image_node()

The hardware supports streaming from memory (in addition to streaming
from the CSI-2 RX), but the driver does not support this at the moment.

There are multiple places in the driver which uses
is_image_output_node(), even if the "output" part is not relevant. Thus,
in a minor preparation for the possible support for streaming from
memory, and to make it more obvious that the pieces of code are not
about the "output", add is_image_node() which will return true for both
input and output video nodes.

While at it, reformat also the metadata related macros to fit inside 80
columns.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

media: rp1: cfe: Dual purpose video nodes

The RP1 CSI-2 DMA can capture both video and metadata just fine, but at
the moment the video nodes are only set to support either video or
metadata.

Make the changes to support both video and metadata. This mostly means
tracking both video format and metadata format separately for each video
node, and using vb2_queue_change_type() to change the vb2 queue type
when needed.

Briefly, this means that the user can get/set both video and meta
formats to a single video node. The vb2 queue buffer type will be
changed when the user calls REQBUFS or CREATE_BUFS ioctls. This buffer
type will be then used as the "mode" for the video node when the user
starts the streaming, and based on that either the video or the meta
format will be used.

A bunch of macros are added (node_supports_xxx()), which tell if a node
can support a particular mode, whereas the existing macros
(is_xxx_node()) will tell if the node is currently in a particular mode.
Note that the latter will only work correctly between the start of the
streaming and the end of the streaming, and thus should be only used in
those code paths.

However, as the userspace (libcamera) does not support dual purpose
video nodes, for the time being let's keep the second video node as
V4L2_CAP_META_CAPTURE only to keep the userspace working.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

media: rp1: Drop LE handling

The driver registers for line-end interrupts, but never uses them. This
just causes extra interrupt load, with more complexity in the driver.

Drop the LE handling. It can easily be added back if later needed.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

media: rp1: cfe: Improve link validation for metadata

Improve the link validation for metadata by:
- Allowing capture buffers that are larger than the incoming frame
  (instead of requiring exact match).

- Instead of assuming that a metadata unit ("pixel") is 8 bits, use
  find_format_by_code() to get the format and use the bit depth from
  there. E.g. bit depth for RAW10 metadata will be 10 bits, when we
  move to the upstream metadata formats.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

drivers: media: cfe: Add more robust ISR handlers

Update the ISR logic to be more robust to sensors in problematic states
where interrupts may start arriving overlapped and/or missing.

1) Test for cur_frame in the FE handler, and if present, dequeue it in
an error state so that it does not get orphaned.

2) Move the sequence counter and timestamp variables to the node
structures.  This allows the ISR to track channels running ahead when
interrupts arrive unordered.

3) Add a test to ensure we don't have a spurios (but harmlesS) call to
the FE handler in some circumstances.

Signed-off-by: Naushir Patuck <naush@raspberrypi.com>

media: rp1: cfe: Fix error paths in cfe_start_streaming

Noted that if we get "node link is not enabled", then we also
get the videobuf2 splat for the driver not cleaning up correctly
on a failed start_streaming, and indeed we weren't returning the
buffers.

Checking the other error paths, noted that the "FE enabled, but
FE_CONFIG node is not" path was not calling pm_runtime_put.

Fix both paths.

Signed-off-by: Dave Stevenson <dave.stevenson@raspberrypi.com>

drivers: media: cfe: Increase default size of embedded buffer

Increase the size of the default embedded buffer to 16k. This is done to
match what is advertised by the IMX219 driver and workaround a problem
where the embedded stream is not actually used. Without full streams API
support, the media pipeline validation will fail in these circumstances.

Signed-off-by: Naushir Patuck <naush@raspberrypi.com>

media: rp1: cfe: Actually use the number of lanes configured

The driver was calling get_mbus_config to ask the sensor subdev
how many CSI2 data lanes it wished to use and with what other
properties, but then failed to pass that to the DPHY configuration.

Signed-off-by: Dave Stevenson <dave.stevenson@raspberrypi.com>

media: rp1: csi2: Fix csi2_pad_set_fmt()

The CSI-2 subdev's set_fmt currently allows setting the source and sink
pad formats quite freely. This is not right, as the CSI-2 block can only
do one of the following when processing the stream: 1) pass through as
is, 2) expand to 16-bits, 3) compress.

The csi2_pad_set_fmt() should take this into account, and only allow
changing the source side mbus code, compared to the sink side format.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

media: rp1: csi2: Use standard link_validate

The current csi2_link_validate() skips some important checks. Let's
rather use the standard v4l2_subdev_link_validate_default() as the
link_validate hook.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

media: rp1: csi2: Squash fixes

media: rp1: fe: Fix pisp_fe_pad_set_fmt()

pisp_fe_pad_set_fmt() allows setting the pad formats quite freely. This
is not correct, and the function should only allow formats as supported
by the hardware. Fix this by:

Allow no format changes for FE_CONFIG_PAD and FE_STATS_PAD. They should
always be the hardcoded initial ones.

Allow setting FE_STREAM_PAD freely (but the mbus code must be
supported), and propagate the format to the FE_OUTPUT0_PAD and
FE_OUTPUT1_PAD pads.

Allow changing the mbus code for FE_OUTPUT0_PAD and FE_OUTPUT1_PAD pads
only if the mbus code is the compressed version of the sink side code.

TODO: FE supports scaling and cropping. This should be represented here
too?

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

media: rp1: fe: Use standard link_validate

The current pisp_fe_link_validate() skips some important checks. Let's
rather use the standard v4l2_subdev_link_validate_default() as the
link_validate hook.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

drivers: media: cfe: Add 16-bit and compressed mono format support

Signed-off-by: Naushir Patuck <naush@raspberrypi.com>

media: rp1: cfe: Add missing remaps

8-bit bayer formats are missing remap definitions. Add them.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

media: rp1: cfe: Add missing compressed remaps

16-bit bayer formats are missing compressed remap definitions. Add them.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>

drivers: media: pisp_be: pisp_fe: Update UAPI header licenses

Update the license tags on the pisp UAPI header files with the
"Linux-syscall-note" clause.  Also replace the "GPL-2.0" tag with the
preferred "GPL-2.0-only" tag.

Signed-off-by: Naushir Patuck <naush@raspberrypi.com>

media: rp1: cfe: Use the MIPI_CSI2_DT_xxx defines for csi_dt

Seeing as we now have the CSI2 data types defined, make use of
them instead of hardcoding the values.

Signed-off-by: Dave Stevenson <dave.stevenson@raspberrypi.com>

media: rp1: cfe: Add a csi_dt value for 16bit formats

Raw 16bit formats didn't have a csi_dt value defined, which
presumably would trip the WARN_ON(!fmt->csi_dt); in
cfe_start_channel.

The value is defined in CSI2 v2.0 as 0x2e, so set it accordingly.

Signed-off-by: Dave Stevenson <dave.stevenson@raspberrypi.com>

drivers: media: pisp_be: Add mono and 48-bit RGB pixel format support

Signed-off-by: Naushir Patuck <naush@raspberrypi.com>

drivers: media: pisp_be: Update seqeuence numbers of the buffers

Add a framebuffer sequence counter and increment on every completed job.
This counter is then used to update the VB2 buffer sequence count before
calling vb2_buffer_done().

Signed-off-by: Naushir Patuck <naush@raspberrypi.com>

drivers: media: cfe: Add remap entries for mono formats

The 8-bit and 16-bit mono formats were missing the appropriate remap
entries in the format table.

Signed-off-by: Naushir Patuck <naush@raspberrypi.com>

media: rp1-cfe: Fix up link validation for CFE CFG input

After commit 5fd3e2412ade ("media: v4l2-subdev: Support hybrid links
in v4l2_subdev_link_validate()") link_validate is called on V4L2
OUTPUT devices such as the CFE cfg buffers input.
The CFE link_validate function was assuming it was always the
sink of a link, which goes wrong on that port and does an invalid
dereference.

Signed-off-by: Dave Stevenson <dave.stevenson@raspberrypi.com>

media: rp1-cfe: Swap "raspberypi,rp1-cfe" compatible to downstream driver

Whilst we are wanting to maintain the downstream driver at the same time
as having the upstream merged, swap the "raspberypi,rp1-cfe" compatible
string across.

Signed-off-by: Dave Stevenson <dave.stevenson@raspberrypi.com>

media/platform/raspberypi/rp1_cfe: Candidate fix for #5821

To avoid lost frame start in a subsequent session, avoid setting
the number of lanes back to 1 or putting CSI-2 Host into reset.

It's not clear if this is a watertight fix -- what if the camera
itself produced a truncated or garbled packet, or continued to
send until the next start? -- but it does seem to fix the issue.

Signed-off-by: Nick Hollinghurst <nick.hollinghurst@raspberrypi.com>

drivers: media: rpi: cfe: Avoid unpack operation for 16-bit formats

The unpack operation is redundant for 16-bit sensor formats, don't set
the hardware to do it in these cases.

Signed-off-by: Naushir Patuck <naush@raspberrypi.com>

drivers: media: cfe: Workaround for 16-bit mismatch in the hardware

Set the data type for 16-bit modes to 0 (wildcard) to workarond the
16-bit mismatch in hardware.

We also need to suppress the warning about DT == 0 on start stream in
these cases.

Signed-off-by: Naushir Patuck <naush@raspberrypi.com>
---
 drivers/media/platform/raspberrypi/Kconfig    |    1 +
 drivers/media/platform/raspberrypi/Makefile   |    1 +
 .../raspberrypi/pisp_be/pisp_be_config.h      |  533 ++++
 .../media/platform/raspberrypi/rp1-cfe/cfe.c  |    2 +-
 .../platform/raspberrypi/rp1_cfe/Kconfig      |   15 +
 .../platform/raspberrypi/rp1_cfe/Makefile     |    6 +
 .../media/platform/raspberrypi/rp1_cfe/cfe.c  | 2471 +++++++++++++++++
 .../media/platform/raspberrypi/rp1_cfe/cfe.h  |   43 +
 .../platform/raspberrypi/rp1_cfe/cfe_fmts.h   |  318 +++
 .../media/platform/raspberrypi/rp1_cfe/csi2.c |  628 +++++
 .../media/platform/raspberrypi/rp1_cfe/csi2.h |   90 +
 .../media/platform/raspberrypi/rp1_cfe/dphy.c |  187 ++
 .../media/platform/raspberrypi/rp1_cfe/dphy.h |   27 +
 .../raspberrypi/rp1_cfe/pisp_common.h         |   69 +
 .../platform/raspberrypi/rp1_cfe/pisp_fe.c    |  569 ++++
 .../platform/raspberrypi/rp1_cfe/pisp_fe.h    |   53 +
 .../raspberrypi/rp1_cfe/pisp_fe_config.h      |  272 ++
 .../raspberrypi/rp1_cfe/pisp_statistics.h     |   62 +
 .../platform/raspberrypi/rp1_cfe/pisp_types.h |  144 +
 19 files changed, 5490 insertions(+), 1 deletion(-)
 create mode 100644 drivers/media/platform/raspberrypi/pisp_be/pisp_be_config.h
 create mode 100644 drivers/media/platform/raspberrypi/rp1_cfe/Kconfig
 create mode 100644 drivers/media/platform/raspberrypi/rp1_cfe/Makefile
 create mode 100644 drivers/media/platform/raspberrypi/rp1_cfe/cfe.c
 create mode 100644 drivers/media/platform/raspberrypi/rp1_cfe/cfe.h
 create mode 100644 drivers/media/platform/raspberrypi/rp1_cfe/cfe_fmts.h
 create mode 100644 drivers/media/platform/raspberrypi/rp1_cfe/csi2.c
 create mode 100644 drivers/media/platform/raspberrypi/rp1_cfe/csi2.h
 create mode 100644 drivers/media/platform/raspberrypi/rp1_cfe/dphy.c
 create mode 100644 drivers/media/platform/raspberrypi/rp1_cfe/dphy.h
 create mode 100644 drivers/media/platform/raspberrypi/rp1_cfe/pisp_common.h
 create mode 100644 drivers/media/platform/raspberrypi/rp1_cfe/pisp_fe.c
 create mode 100644 drivers/media/platform/raspberrypi/rp1_cfe/pisp_fe.h
 create mode 100644 drivers/media/platform/raspberrypi/rp1_cfe/pisp_fe_config.h
 create mode 100644 drivers/media/platform/raspberrypi/rp1_cfe/pisp_statistics.h
 create mode 100644 drivers/media/platform/raspberrypi/rp1_cfe/pisp_types.h

--- a/drivers/media/platform/raspberrypi/Kconfig
+++ b/drivers/media/platform/raspberrypi/Kconfig
@@ -4,3 +4,4 @@ comment "Raspberry Pi media platform dri
 
 source "drivers/media/platform/raspberrypi/pisp_be/Kconfig"
 source "drivers/media/platform/raspberrypi/rp1-cfe/Kconfig"
+source "drivers/media/platform/raspberrypi/rp1_cfe/Kconfig"
--- a/drivers/media/platform/raspberrypi/Makefile
+++ b/drivers/media/platform/raspberrypi/Makefile
@@ -2,3 +2,4 @@
 
 obj-y += pisp_be/
 obj-y += rp1-cfe/
+obj-y += rp1_cfe/
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/pisp_be/pisp_be_config.h
@@ -0,0 +1,533 @@
+/* SPDX-License-Identifier: GPL-2.0-only WITH Linux-syscall-note */
+/*
+ * PiSP Back End configuration definitions.
+ *
+ * Copyright (C) 2021 - Raspberry Pi Ltd
+ *
+ */
+#ifndef _PISP_BE_CONFIG_H_
+#define _PISP_BE_CONFIG_H_
+
+#include <linux/types.h>
+
+#include <media/raspberrypi/pisp_common.h>
+
+/* byte alignment for inputs */
+#define PISP_BACK_END_INPUT_ALIGN 4u
+/* alignment for compressed inputs */
+#define PISP_BACK_END_COMPRESSED_ALIGN 8u
+/* minimum required byte alignment for outputs */
+#define PISP_BACK_END_OUTPUT_MIN_ALIGN 16u
+/* preferred byte alignment for outputs */
+#define PISP_BACK_END_OUTPUT_MAX_ALIGN 64u
+
+/* minimum allowed tile width anywhere in the pipeline */
+#define PISP_BACK_END_MIN_TILE_WIDTH 16u
+/* minimum allowed tile width anywhere in the pipeline */
+#define PISP_BACK_END_MIN_TILE_HEIGHT 16u
+
+#define PISP_BACK_END_NUM_OUTPUTS 2
+#define PISP_BACK_END_HOG_OUTPUT 1
+
+#define PISP_BACK_END_NUM_TILES 64
+
+enum pisp_be_bayer_enable {
+	PISP_BE_BAYER_ENABLE_INPUT = 0x000001,
+	PISP_BE_BAYER_ENABLE_DECOMPRESS = 0x000002,
+	PISP_BE_BAYER_ENABLE_DPC = 0x000004,
+	PISP_BE_BAYER_ENABLE_GEQ = 0x000008,
+	PISP_BE_BAYER_ENABLE_TDN_INPUT = 0x000010,
+	PISP_BE_BAYER_ENABLE_TDN_DECOMPRESS = 0x000020,
+	PISP_BE_BAYER_ENABLE_TDN = 0x000040,
+	PISP_BE_BAYER_ENABLE_TDN_COMPRESS = 0x000080,
+	PISP_BE_BAYER_ENABLE_TDN_OUTPUT = 0x000100,
+	PISP_BE_BAYER_ENABLE_SDN = 0x000200,
+	PISP_BE_BAYER_ENABLE_BLC = 0x000400,
+	PISP_BE_BAYER_ENABLE_STITCH_INPUT = 0x000800,
+	PISP_BE_BAYER_ENABLE_STITCH_DECOMPRESS = 0x001000,
+	PISP_BE_BAYER_ENABLE_STITCH = 0x002000,
+	PISP_BE_BAYER_ENABLE_STITCH_COMPRESS = 0x004000,
+	PISP_BE_BAYER_ENABLE_STITCH_OUTPUT = 0x008000,
+	PISP_BE_BAYER_ENABLE_WBG = 0x010000,
+	PISP_BE_BAYER_ENABLE_CDN = 0x020000,
+	PISP_BE_BAYER_ENABLE_LSC = 0x040000,
+	PISP_BE_BAYER_ENABLE_TONEMAP = 0x080000,
+	PISP_BE_BAYER_ENABLE_CAC = 0x100000,
+	PISP_BE_BAYER_ENABLE_DEBIN = 0x200000,
+	PISP_BE_BAYER_ENABLE_DEMOSAIC = 0x400000,
+};
+
+enum pisp_be_rgb_enable {
+	PISP_BE_RGB_ENABLE_INPUT = 0x000001,
+	PISP_BE_RGB_ENABLE_CCM = 0x000002,
+	PISP_BE_RGB_ENABLE_SAT_CONTROL = 0x000004,
+	PISP_BE_RGB_ENABLE_YCBCR = 0x000008,
+	PISP_BE_RGB_ENABLE_FALSE_COLOUR = 0x000010,
+	PISP_BE_RGB_ENABLE_SHARPEN = 0x000020,
+	/* Preferred colours would occupy 0x000040 */
+	PISP_BE_RGB_ENABLE_YCBCR_INVERSE = 0x000080,
+	PISP_BE_RGB_ENABLE_GAMMA = 0x000100,
+	PISP_BE_RGB_ENABLE_CSC0 = 0x000200,
+	PISP_BE_RGB_ENABLE_CSC1 = 0x000400,
+	PISP_BE_RGB_ENABLE_DOWNSCALE0 = 0x001000,
+	PISP_BE_RGB_ENABLE_DOWNSCALE1 = 0x002000,
+	PISP_BE_RGB_ENABLE_RESAMPLE0 = 0x008000,
+	PISP_BE_RGB_ENABLE_RESAMPLE1 = 0x010000,
+	PISP_BE_RGB_ENABLE_OUTPUT0 = 0x040000,
+	PISP_BE_RGB_ENABLE_OUTPUT1 = 0x080000,
+	PISP_BE_RGB_ENABLE_HOG = 0x200000
+};
+
+#define PISP_BE_RGB_ENABLE_CSC(i) (PISP_BE_RGB_ENABLE_CSC0 << (i))
+#define PISP_BE_RGB_ENABLE_DOWNSCALE(i) (PISP_BE_RGB_ENABLE_DOWNSCALE0 << (i))
+#define PISP_BE_RGB_ENABLE_RESAMPLE(i) (PISP_BE_RGB_ENABLE_RESAMPLE0 << (i))
+#define PISP_BE_RGB_ENABLE_OUTPUT(i) (PISP_BE_RGB_ENABLE_OUTPUT0 << (i))
+
+/*
+ * We use the enable flags to show when blocks are "dirty", but we need some
+ * extra ones too.
+ */
+enum pisp_be_dirty {
+	PISP_BE_DIRTY_GLOBAL = 0x0001,
+	PISP_BE_DIRTY_SH_FC_COMBINE = 0x0002,
+	PISP_BE_DIRTY_CROP = 0x0004
+};
+
+struct pisp_be_global_config {
+	u32 bayer_enables;
+	u32 rgb_enables;
+	u8 bayer_order;
+	u8 pad[3];
+};
+
+struct pisp_be_input_buffer_config {
+	/* low 32 bits followed by high 32 bits (for each of up to 3 planes) */
+	u32 addr[3][2];
+};
+
+struct pisp_be_dpc_config {
+	u8 coeff_level;
+	u8 coeff_range;
+	u8 pad;
+#define PISP_BE_DPC_FLAG_FOLDBACK 1
+	u8 flags;
+};
+
+struct pisp_be_geq_config {
+	u16 offset;
+#define PISP_BE_GEQ_SHARPER BIT(15)
+#define PISP_BE_GEQ_SLOPE ((1 << 10) - 1)
+	/* top bit is the "sharper" flag, slope value is bottom 10 bits */
+	u16 slope_sharper;
+	u16 min;
+	u16 max;
+};
+
+struct pisp_be_tdn_input_buffer_config {
+	/* low 32 bits followed by high 32 bits */
+	u32 addr[2];
+};
+
+struct pisp_be_tdn_config {
+	u16 black_level;
+	u16 ratio;
+	u16 noise_constant;
+	u16 noise_slope;
+	u16 threshold;
+	u8 reset;
+	u8 pad;
+};
+
+struct pisp_be_tdn_output_buffer_config {
+	/* low 32 bits followed by high 32 bits */
+	u32 addr[2];
+};
+
+struct pisp_be_sdn_config {
+	u16 black_level;
+	u8 leakage;
+	u8 pad;
+	u16 noise_constant;
+	u16 noise_slope;
+	u16 noise_constant2;
+	u16 noise_slope2;
+};
+
+struct pisp_be_stitch_input_buffer_config {
+	/* low 32 bits followed by high 32 bits */
+	u32 addr[2];
+};
+
+#define PISP_BE_STITCH_STREAMING_LONG 0x8000
+#define PISP_BE_STITCH_EXPOSURE_RATIO_MASK 0x7fff
+
+struct pisp_be_stitch_config {
+	u16 threshold_lo;
+	u8 threshold_diff_power;
+	u8 pad;
+
+	/* top bit indicates whether streaming input is the long exposure */
+	u16 exposure_ratio;
+
+	u8 motion_threshold_256;
+	u8 motion_threshold_recip;
+};
+
+struct pisp_be_stitch_output_buffer_config {
+	/* low 32 bits followed by high 32 bits */
+	u32 addr[2];
+};
+
+struct pisp_be_cdn_config {
+	u16 thresh;
+	u8 iir_strength;
+	u8 g_adjust;
+};
+
+#define PISP_BE_LSC_LOG_GRID_SIZE 5
+#define PISP_BE_LSC_GRID_SIZE (1 << PISP_BE_LSC_LOG_GRID_SIZE)
+#define PISP_BE_LSC_STEP_PRECISION 18
+
+struct pisp_be_lsc_config {
+	/* (1<<18) / grid_cell_width */
+	u16 grid_step_x;
+	/* (1<<18) / grid_cell_height */
+	u16 grid_step_y;
+	/* RGB gains jointly encoded in 32 bits */
+	u32 lut_packed[PISP_BE_LSC_GRID_SIZE + 1]
+			   [PISP_BE_LSC_GRID_SIZE + 1];
+};
+
+struct pisp_be_lsc_extra {
+	u16 offset_x;
+	u16 offset_y;
+};
+
+#define PISP_BE_CAC_LOG_GRID_SIZE 3
+#define PISP_BE_CAC_GRID_SIZE (1 << PISP_BE_CAC_LOG_GRID_SIZE)
+#define PISP_BE_CAC_STEP_PRECISION 20
+
+struct pisp_be_cac_config {
+	/* (1<<20) / grid_cell_width */
+	u16 grid_step_x;
+	/* (1<<20) / grid_cell_height */
+	u16 grid_step_y;
+	/* [gridy][gridx][rb][xy] */
+	s8 lut[PISP_BE_CAC_GRID_SIZE + 1][PISP_BE_CAC_GRID_SIZE + 1][2][2];
+};
+
+struct pisp_be_cac_extra {
+	u16 offset_x;
+	u16 offset_y;
+};
+
+#define PISP_BE_DEBIN_NUM_COEFFS 4
+
+struct pisp_be_debin_config {
+	s8 coeffs[PISP_BE_DEBIN_NUM_COEFFS];
+	s8 h_enable;
+	s8 v_enable;
+	s8 pad[2];
+};
+
+#define PISP_BE_TONEMAP_LUT_SIZE 64
+
+struct pisp_be_tonemap_config {
+	u16 detail_constant;
+	u16 detail_slope;
+	u16 iir_strength;
+	u16 strength;
+	u32 lut[PISP_BE_TONEMAP_LUT_SIZE];
+};
+
+struct pisp_be_demosaic_config {
+	u8 sharper;
+	u8 fc_mode;
+	u8 pad[2];
+};
+
+struct pisp_be_ccm_config {
+	s16 coeffs[9];
+	u8 pad[2];
+	s32 offsets[3];
+};
+
+struct pisp_be_sat_control_config {
+	u8 shift_r;
+	u8 shift_g;
+	u8 shift_b;
+	u8 pad;
+};
+
+struct pisp_be_false_colour_config {
+	u8 distance;
+	u8 pad[3];
+};
+
+#define PISP_BE_SHARPEN_SIZE 5
+#define PISP_BE_SHARPEN_FUNC_NUM_POINTS 9
+
+struct pisp_be_sharpen_config {
+	s8 kernel0[PISP_BE_SHARPEN_SIZE * PISP_BE_SHARPEN_SIZE];
+	s8 pad0[3];
+	s8 kernel1[PISP_BE_SHARPEN_SIZE * PISP_BE_SHARPEN_SIZE];
+	s8 pad1[3];
+	s8 kernel2[PISP_BE_SHARPEN_SIZE * PISP_BE_SHARPEN_SIZE];
+	s8 pad2[3];
+	s8 kernel3[PISP_BE_SHARPEN_SIZE * PISP_BE_SHARPEN_SIZE];
+	s8 pad3[3];
+	s8 kernel4[PISP_BE_SHARPEN_SIZE * PISP_BE_SHARPEN_SIZE];
+	s8 pad4[3];
+	u16 threshold_offset0;
+	u16 threshold_slope0;
+	u16 scale0;
+	u16 pad5;
+	u16 threshold_offset1;
+	u16 threshold_slope1;
+	u16 scale1;
+	u16 pad6;
+	u16 threshold_offset2;
+	u16 threshold_slope2;
+	u16 scale2;
+	u16 pad7;
+	u16 threshold_offset3;
+	u16 threshold_slope3;
+	u16 scale3;
+	u16 pad8;
+	u16 threshold_offset4;
+	u16 threshold_slope4;
+	u16 scale4;
+	u16 pad9;
+	u16 positive_strength;
+	u16 positive_pre_limit;
+	u16 positive_func[PISP_BE_SHARPEN_FUNC_NUM_POINTS];
+	u16 positive_limit;
+	u16 negative_strength;
+	u16 negative_pre_limit;
+	u16 negative_func[PISP_BE_SHARPEN_FUNC_NUM_POINTS];
+	u16 negative_limit;
+	u8 enables;
+	u8 white;
+	u8 black;
+	u8 grey;
+};
+
+struct pisp_be_sh_fc_combine_config {
+	u8 y_factor;
+	u8 c1_factor;
+	u8 c2_factor;
+	u8 pad;
+};
+
+#define PISP_BE_GAMMA_LUT_SIZE 64
+
+struct pisp_be_gamma_config {
+	u32 lut[PISP_BE_GAMMA_LUT_SIZE];
+};
+
+struct pisp_be_crop_config {
+	u16 offset_x, offset_y;
+	u16 width, height;
+};
+
+#define PISP_BE_RESAMPLE_FILTER_SIZE 96
+
+struct pisp_be_resample_config {
+	u16 scale_factor_h, scale_factor_v;
+	s16 coef[PISP_BE_RESAMPLE_FILTER_SIZE];
+};
+
+struct pisp_be_resample_extra {
+	u16 scaled_width;
+	u16 scaled_height;
+	s16 initial_phase_h[3];
+	s16 initial_phase_v[3];
+};
+
+struct pisp_be_downscale_config {
+	u16 scale_factor_h;
+	u16 scale_factor_v;
+	u16 scale_recip_h;
+	u16 scale_recip_v;
+};
+
+struct pisp_be_downscale_extra {
+	u16 scaled_width;
+	u16 scaled_height;
+};
+
+struct pisp_be_hog_config {
+	u8 compute_signed;
+	u8 channel_mix[3];
+	u32 stride;
+};
+
+struct pisp_be_axi_config {
+	u8 r_qos; /* Read QoS */
+	u8 r_cache_prot; /* Read { prot[2:0], cache[3:0] } */
+	u8 w_qos; /* Write QoS */
+	u8 w_cache_prot; /* Write { prot[2:0], cache[3:0] } */
+};
+
+enum pisp_be_transform {
+	PISP_BE_TRANSFORM_NONE = 0x0,
+	PISP_BE_TRANSFORM_HFLIP = 0x1,
+	PISP_BE_TRANSFORM_VFLIP = 0x2,
+	PISP_BE_TRANSFORM_ROT180 =
+		(PISP_BE_TRANSFORM_HFLIP | PISP_BE_TRANSFORM_VFLIP)
+};
+
+struct pisp_be_output_format_config {
+	struct pisp_image_format_config image;
+	u8 transform;
+	u8 pad[3];
+	u16 lo;
+	u16 hi;
+	u16 lo2;
+	u16 hi2;
+};
+
+struct pisp_be_output_buffer_config {
+	/* low 32 bits followed by high 32 bits (for each of 3 planes) */
+	u32 addr[3][2];
+};
+
+struct pisp_be_hog_buffer_config {
+	/* low 32 bits followed by high 32 bits */
+	u32 addr[2];
+};
+
+struct pisp_be_config {
+	/* I/O configuration: */
+	struct pisp_be_input_buffer_config input_buffer;
+	struct pisp_be_tdn_input_buffer_config tdn_input_buffer;
+	struct pisp_be_stitch_input_buffer_config stitch_input_buffer;
+	struct pisp_be_tdn_output_buffer_config tdn_output_buffer;
+	struct pisp_be_stitch_output_buffer_config stitch_output_buffer;
+	struct pisp_be_output_buffer_config
+				output_buffer[PISP_BACK_END_NUM_OUTPUTS];
+	struct pisp_be_hog_buffer_config hog_buffer;
+	/* Processing configuration: */
+	struct pisp_be_global_config global;
+	struct pisp_image_format_config input_format;
+	struct pisp_decompress_config decompress;
+	struct pisp_be_dpc_config dpc;
+	struct pisp_be_geq_config geq;
+	struct pisp_image_format_config tdn_input_format;
+	struct pisp_decompress_config tdn_decompress;
+	struct pisp_be_tdn_config tdn;
+	struct pisp_compress_config tdn_compress;
+	struct pisp_image_format_config tdn_output_format;
+	struct pisp_be_sdn_config sdn;
+	struct pisp_bla_config blc;
+	struct pisp_compress_config stitch_compress;
+	struct pisp_image_format_config stitch_output_format;
+	struct pisp_image_format_config stitch_input_format;
+	struct pisp_decompress_config stitch_decompress;
+	struct pisp_be_stitch_config stitch;
+	struct pisp_be_lsc_config lsc;
+	struct pisp_wbg_config wbg;
+	struct pisp_be_cdn_config cdn;
+	struct pisp_be_cac_config cac;
+	struct pisp_be_debin_config debin;
+	struct pisp_be_tonemap_config tonemap;
+	struct pisp_be_demosaic_config demosaic;
+	struct pisp_be_ccm_config ccm;
+	struct pisp_be_sat_control_config sat_control;
+	struct pisp_be_ccm_config ycbcr;
+	struct pisp_be_sharpen_config sharpen;
+	struct pisp_be_false_colour_config false_colour;
+	struct pisp_be_sh_fc_combine_config sh_fc_combine;
+	struct pisp_be_ccm_config ycbcr_inverse;
+	struct pisp_be_gamma_config gamma;
+	struct pisp_be_ccm_config csc[PISP_BACK_END_NUM_OUTPUTS];
+	struct pisp_be_downscale_config downscale[PISP_BACK_END_NUM_OUTPUTS];
+	struct pisp_be_resample_config resample[PISP_BACK_END_NUM_OUTPUTS];
+	struct pisp_be_output_format_config
+				output_format[PISP_BACK_END_NUM_OUTPUTS];
+	struct pisp_be_hog_config hog;
+	struct pisp_be_axi_config axi;
+	/* Non-register fields: */
+	struct pisp_be_lsc_extra lsc_extra;
+	struct pisp_be_cac_extra cac_extra;
+	struct pisp_be_downscale_extra
+				downscale_extra[PISP_BACK_END_NUM_OUTPUTS];
+	struct pisp_be_resample_extra resample_extra[PISP_BACK_END_NUM_OUTPUTS];
+	struct pisp_be_crop_config crop;
+	struct pisp_image_format_config hog_format;
+	u32 dirty_flags_bayer; /* these use pisp_be_bayer_enable */
+	u32 dirty_flags_rgb; /* use pisp_be_rgb_enable */
+	u32 dirty_flags_extra; /* these use pisp_be_dirty_t */
+};
+
+/*
+ * We also need a tile structure to describe the size of the tiles going
+ * through the pipeline.
+ */
+
+enum pisp_tile_edge {
+	PISP_LEFT_EDGE = (1 << 0),
+	PISP_RIGHT_EDGE = (1 << 1),
+	PISP_TOP_EDGE = (1 << 2),
+	PISP_BOTTOM_EDGE = (1 << 3)
+};
+
+struct pisp_tile {
+	u8 edge; // enum pisp_tile_edge
+	u8 pad0[3];
+	// 4 bytes
+	u32 input_addr_offset;
+	u32 input_addr_offset2;
+	u16 input_offset_x;
+	u16 input_offset_y;
+	u16 input_width;
+	u16 input_height;
+	// 20 bytes
+	u32 tdn_input_addr_offset;
+	u32 tdn_output_addr_offset;
+	u32 stitch_input_addr_offset;
+	u32 stitch_output_addr_offset;
+	// 36 bytes
+	u32 lsc_grid_offset_x;
+	u32 lsc_grid_offset_y;
+	// 44 bytes
+	u32 cac_grid_offset_x;
+	u32 cac_grid_offset_y;
+	// 52 bytes
+	u16 crop_x_start[PISP_BACK_END_NUM_OUTPUTS];
+	u16 crop_x_end[PISP_BACK_END_NUM_OUTPUTS];
+	u16 crop_y_start[PISP_BACK_END_NUM_OUTPUTS];
+	u16 crop_y_end[PISP_BACK_END_NUM_OUTPUTS];
+	// 68 bytes
+	/* Ordering is planes then branches */
+	u16 downscale_phase_x[3 * PISP_BACK_END_NUM_OUTPUTS];
+	u16 downscale_phase_y[3 * PISP_BACK_END_NUM_OUTPUTS];
+	// 92 bytes
+	u16 resample_in_width[PISP_BACK_END_NUM_OUTPUTS];
+	u16 resample_in_height[PISP_BACK_END_NUM_OUTPUTS];
+	// 100 bytes
+	/* Ordering is planes then branches */
+	u16 resample_phase_x[3 * PISP_BACK_END_NUM_OUTPUTS];
+	u16 resample_phase_y[3 * PISP_BACK_END_NUM_OUTPUTS];
+	// 124 bytes
+	u16 output_offset_x[PISP_BACK_END_NUM_OUTPUTS];
+	u16 output_offset_y[PISP_BACK_END_NUM_OUTPUTS];
+	u16 output_width[PISP_BACK_END_NUM_OUTPUTS];
+	u16 output_height[PISP_BACK_END_NUM_OUTPUTS];
+	// 140 bytes
+	u32 output_addr_offset[PISP_BACK_END_NUM_OUTPUTS];
+	u32 output_addr_offset2[PISP_BACK_END_NUM_OUTPUTS];
+	// 156 bytes
+	u32 output_hog_addr_offset;
+	// 160 bytes
+};
+
+static_assert(sizeof(struct pisp_tile) == 160);
+
+struct pisp_be_tiles_config {
+	struct pisp_be_config config;
+	struct pisp_tile tiles[PISP_BACK_END_NUM_TILES];
+	int num_tiles;
+};
+
+#endif /* _PISP_BE_CONFIG_H_ */
--- a/drivers/media/platform/raspberrypi/rp1-cfe/cfe.c
+++ b/drivers/media/platform/raspberrypi/rp1-cfe/cfe.c
@@ -2482,7 +2482,7 @@ static const struct dev_pm_ops cfe_pm_op
 };
 
 static const struct of_device_id cfe_of_match[] = {
-	{ .compatible = "raspberrypi,rp1-cfe" },
+	{ .compatible = "raspberrypi,rp1-cfe-upstream" },
 	{ /* sentinel */ },
 };
 MODULE_DEVICE_TABLE(of, cfe_of_match);
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1_cfe/Kconfig
@@ -0,0 +1,15 @@
+# RP1 V4L2 camera support
+
+config VIDEO_RP1_CFE_DOWNSTREAM
+	tristate "RP1 DOWNSTREAM Camera Frond End (CFE) video capture driver"
+	depends on VIDEO_DEV
+	select VIDEO_V4L2_SUBDEV_API
+	select MEDIA_CONTROLLER
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_FWNODE
+	help
+	  Say Y here to enable support for the RP1 Camera Front End using
+	  the downstream driver.
+
+	  To compile this driver as a module, choose M here. The module will be
+	  called rp1-cfe-downstream.
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1_cfe/Makefile
@@ -0,0 +1,6 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for RP1 Camera Front End driver
+#
+rp1-cfe-downstream-objs := cfe.o csi2.o pisp_fe.o dphy.o
+obj-$(CONFIG_VIDEO_RP1_CFE_DOWNSTREAM) += rp1-cfe-downstream.o
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1_cfe/cfe.c
@@ -0,0 +1,2471 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * RP1 Camera Front End Driver
+ *
+ * Copyright (C) 2021-2022 - Raspberry Pi Ltd.
+ *
+ */
+
+#include <linux/atomic.h>
+#include <linux/clk.h>
+#include <linux/debugfs.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/of_graph.h>
+#include <linux/phy/phy.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/seq_file.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-async.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-dv-timings.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "cfe.h"
+#include "cfe_fmts.h"
+#include "csi2.h"
+#include "pisp_fe.h"
+#include "pisp_fe_config.h"
+#include "pisp_statistics.h"
+
+#define CFE_MODULE_NAME	"rp1-cfe"
+#define CFE_VERSION	"1.0"
+
+bool cfe_debug_verbose;
+module_param_named(verbose_debug, cfe_debug_verbose, bool, 0644);
+MODULE_PARM_DESC(verbose_debug, "verbose debugging messages");
+
+#define cfe_dbg_verbose(fmt, arg...)                          \
+	do {                                                  \
+		if (cfe_debug_verbose)                        \
+			dev_dbg(&cfe->pdev->dev, fmt, ##arg); \
+	} while (0)
+#define cfe_dbg(fmt, arg...) dev_dbg(&cfe->pdev->dev, fmt, ##arg)
+#define cfe_info(fmt, arg...) dev_info(&cfe->pdev->dev, fmt, ##arg)
+#define cfe_err(fmt, arg...) dev_err(&cfe->pdev->dev, fmt, ##arg)
+
+/* MIPICFG registers */
+#define MIPICFG_CFG		0x004
+#define MIPICFG_INTR		0x028
+#define MIPICFG_INTE		0x02c
+#define MIPICFG_INTF		0x030
+#define MIPICFG_INTS		0x034
+
+#define MIPICFG_CFG_SEL_CSI	BIT(0)
+
+#define MIPICFG_INT_CSI_DMA	BIT(0)
+#define MIPICFG_INT_CSI_HOST	BIT(2)
+#define MIPICFG_INT_PISP_FE	BIT(4)
+
+#define BPL_ALIGNMENT 16
+#define MAX_BYTESPERLINE 0xffffff00
+#define MAX_BUFFER_SIZE  0xffffff00
+/*
+ * Max width is therefore determined by the max stride divided by the number of
+ * bits per pixel.
+ *
+ * However, to avoid overflow issues let's use a 16k maximum. This lets us
+ * calculate 16k * 16k * 4 with 32bits. If we need higher maximums, a careful
+ * review and adjustment of the code is needed so that it will deal with
+ * overflows correctly.
+ */
+#define MAX_WIDTH 16384
+#define MAX_HEIGHT MAX_WIDTH
+/* Define a nominal minimum image size */
+#define MIN_WIDTH 16
+#define MIN_HEIGHT 16
+/* Default size of the embedded buffer */
+#define DEFAULT_EMBEDDED_SIZE 16384
+
+const struct v4l2_mbus_framefmt cfe_default_format = {
+	.width = 640,
+	.height = 480,
+	.code = MEDIA_BUS_FMT_SRGGB10_1X10,
+	.field = V4L2_FIELD_NONE,
+	.colorspace = V4L2_COLORSPACE_RAW,
+	.ycbcr_enc = V4L2_YCBCR_ENC_601,
+	.quantization = V4L2_QUANTIZATION_FULL_RANGE,
+	.xfer_func = V4L2_XFER_FUNC_NONE,
+};
+
+const struct v4l2_mbus_framefmt cfe_default_meta_format = {
+	.width = DEFAULT_EMBEDDED_SIZE,
+	.height = 1,
+	.code = MEDIA_BUS_FMT_SENSOR_DATA,
+	.field = V4L2_FIELD_NONE,
+};
+
+enum node_ids {
+	/* CSI2 HW output nodes first. */
+	CSI2_CH0,
+	CSI2_CH1,
+	CSI2_CH2,
+	CSI2_CH3,
+	/* FE only nodes from here on. */
+	FE_OUT0,
+	FE_OUT1,
+	FE_STATS,
+	FE_CONFIG,
+	NUM_NODES
+};
+
+struct node_description {
+	unsigned int id;
+	const char *name;
+	unsigned int caps;
+	unsigned int pad_flags;
+	unsigned int link_pad;
+};
+
+/* Must match the ordering of enum ids */
+static const struct node_description node_desc[NUM_NODES] = {
+	[CSI2_CH0] = {
+		.name = "csi2_ch0",
+		.caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_META_CAPTURE,
+		.pad_flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT,
+		.link_pad = CSI2_NUM_CHANNELS + 0
+	},
+	/*
+	 * TODO: This node should be named "csi2_ch1" and the caps should be set
+	 * to both video and meta capture. However, to keep compatibility with
+	 * the current libcamera, keep the name as "embedded" and support
+	 * only meta capture.
+	 */
+	[CSI2_CH1] = {
+		.name = "embedded",
+		.caps = V4L2_CAP_META_CAPTURE,
+		.pad_flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT,
+		.link_pad = CSI2_NUM_CHANNELS + 1
+	},
+	[CSI2_CH2] = {
+		.name = "csi2_ch2",
+		.caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_META_CAPTURE,
+		.pad_flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT,
+		.link_pad = CSI2_NUM_CHANNELS + 2
+	},
+	[CSI2_CH3] = {
+		.name = "csi2_ch3",
+		.caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_META_CAPTURE,
+		.pad_flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT,
+		.link_pad = CSI2_NUM_CHANNELS + 3
+	},
+	[FE_OUT0] = {
+		.name = "fe_image0",
+		.caps = V4L2_CAP_VIDEO_CAPTURE,
+		.pad_flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT,
+		.link_pad = FE_OUTPUT0_PAD
+	},
+	[FE_OUT1] = {
+		.name = "fe_image1",
+		.caps = V4L2_CAP_VIDEO_CAPTURE,
+		.pad_flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT,
+		.link_pad = FE_OUTPUT1_PAD
+	},
+	[FE_STATS] = {
+		.name = "fe_stats",
+		.caps = V4L2_CAP_META_CAPTURE,
+		.pad_flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT,
+		.link_pad = FE_STATS_PAD
+	},
+	[FE_CONFIG] = {
+		.name = "fe_config",
+		.caps = V4L2_CAP_META_OUTPUT,
+		.pad_flags = MEDIA_PAD_FL_SOURCE | MEDIA_PAD_FL_MUST_CONNECT,
+		.link_pad = FE_CONFIG_PAD
+	},
+};
+
+#define is_fe_node(node) (((node)->id) >= FE_OUT0)
+#define is_csi2_node(node) (!is_fe_node(node))
+
+#define node_supports_image_output(node) \
+	(!!(node_desc[(node)->id].caps & V4L2_CAP_VIDEO_CAPTURE))
+#define node_supports_meta_output(node) \
+	(!!(node_desc[(node)->id].caps & V4L2_CAP_META_CAPTURE))
+#define node_supports_image_input(node) \
+	(!!(node_desc[(node)->id].caps & V4L2_CAP_VIDEO_OUTPUT))
+#define node_supports_meta_input(node) \
+	(!!(node_desc[(node)->id].caps & V4L2_CAP_META_OUTPUT))
+#define node_supports_image(node) \
+	(node_supports_image_output(node) || node_supports_image_input(node))
+#define node_supports_meta(node) \
+	(node_supports_meta_output(node) || node_supports_meta_input(node))
+
+#define is_image_output_node(node) \
+	((node)->buffer_queue.type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+#define is_image_input_node(node) \
+	((node)->buffer_queue.type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+#define is_image_node(node) \
+	(is_image_output_node(node) || is_image_input_node(node))
+#define is_meta_output_node(node) \
+	((node)->buffer_queue.type == V4L2_BUF_TYPE_META_CAPTURE)
+#define is_meta_input_node(node) \
+	((node)->buffer_queue.type == V4L2_BUF_TYPE_META_OUTPUT)
+#define is_meta_node(node) \
+	(is_meta_output_node(node) || is_meta_input_node(node))
+
+/* To track state across all nodes. */
+#define NUM_STATES		5
+#define NODE_REGISTERED		BIT(0)
+#define NODE_ENABLED		BIT(1)
+#define NODE_STREAMING		BIT(2)
+#define FS_INT			BIT(3)
+#define FE_INT			BIT(4)
+
+struct cfe_buffer {
+	struct vb2_v4l2_buffer vb;
+	struct list_head list;
+};
+
+struct cfe_config_buffer {
+	struct cfe_buffer buf;
+	struct pisp_fe_config config;
+};
+
+static inline struct cfe_buffer *to_cfe_buffer(struct vb2_buffer *vb)
+{
+	return container_of(vb, struct cfe_buffer, vb.vb2_buf);
+}
+
+static inline
+struct cfe_config_buffer *to_cfe_config_buffer(struct cfe_buffer *buf)
+{
+	return container_of(buf, struct cfe_config_buffer, buf);
+}
+
+struct cfe_node {
+	unsigned int id;
+	/* Pointer pointing to current v4l2_buffer */
+	struct cfe_buffer *cur_frm;
+	/* Pointer pointing to next v4l2_buffer */
+	struct cfe_buffer *next_frm;
+	/* Used to store current pixel format */
+	struct v4l2_format vid_fmt;
+	/* Used to store current meta format */
+	struct v4l2_format meta_fmt;
+	/* Buffer queue used in video-buf */
+	struct vb2_queue buffer_queue;
+	/* Queue of filled frames */
+	struct list_head dma_queue;
+	/* lock used to access this structure */
+	struct mutex lock;
+	/* Identifies video device for this channel */
+	struct video_device video_dev;
+	/* Pointer to the parent handle */
+	struct cfe_device *cfe;
+	struct media_pad pad;
+	unsigned int fs_count;
+	u64 ts;
+};
+
+struct cfe_device {
+	struct dentry *debugfs;
+	struct kref kref;
+
+	/* V4l2 specific parameters */
+	struct v4l2_async_connection *asd;
+
+	/* peripheral base address */
+	void __iomem *mipi_cfg_base;
+
+	struct clk *clk;
+
+	/* V4l2 device */
+	struct v4l2_device v4l2_dev;
+	struct media_device mdev;
+	struct media_pipeline pipe;
+
+	/* IRQ lock for node state and DMA queues */
+	spinlock_t state_lock;
+	bool job_ready;
+	bool job_queued;
+
+	/* parent device */
+	struct platform_device *pdev;
+	/* subdevice async Notifier */
+	struct v4l2_async_notifier notifier;
+
+	/* ptr to sub device */
+	struct v4l2_subdev *sensor;
+
+	struct cfe_node node[NUM_NODES];
+	DECLARE_BITMAP(node_flags, NUM_STATES * NUM_NODES);
+
+	struct csi2_device csi2;
+	struct pisp_fe_device fe;
+
+	int fe_csi2_channel;
+};
+
+static inline bool is_fe_enabled(struct cfe_device *cfe)
+{
+	return cfe->fe_csi2_channel != -1;
+}
+
+static inline struct cfe_device *to_cfe_device(struct v4l2_device *v4l2_dev)
+{
+	return container_of(v4l2_dev, struct cfe_device, v4l2_dev);
+}
+
+static inline u32 cfg_reg_read(struct cfe_device *cfe, u32 offset)
+{
+	return readl(cfe->mipi_cfg_base + offset);
+}
+
+static inline void cfg_reg_write(struct cfe_device *cfe, u32 offset, u32 val)
+{
+	writel(val, cfe->mipi_cfg_base + offset);
+}
+
+static bool check_state(struct cfe_device *cfe, unsigned long state,
+			unsigned int node_id)
+{
+	unsigned long bit;
+
+	for_each_set_bit(bit, &state, sizeof(state)) {
+		if (!test_bit(bit + (node_id * NUM_STATES), cfe->node_flags))
+			return false;
+	}
+	return true;
+}
+
+static void set_state(struct cfe_device *cfe, unsigned long state,
+		      unsigned int node_id)
+{
+	unsigned long bit;
+
+	for_each_set_bit(bit, &state, sizeof(state))
+		set_bit(bit + (node_id * NUM_STATES), cfe->node_flags);
+}
+
+static void clear_state(struct cfe_device *cfe, unsigned long state,
+			unsigned int node_id)
+{
+	unsigned long bit;
+
+	for_each_set_bit(bit, &state, sizeof(state))
+		clear_bit(bit + (node_id * NUM_STATES), cfe->node_flags);
+}
+
+static bool test_any_node(struct cfe_device *cfe, unsigned long cond)
+{
+	unsigned int i;
+
+	for (i = 0; i < NUM_NODES; i++) {
+		if (check_state(cfe, cond, i))
+			return true;
+	}
+
+	return false;
+}
+
+static bool test_all_nodes(struct cfe_device *cfe, unsigned long precond,
+			   unsigned long cond)
+{
+	unsigned int i;
+
+	for (i = 0; i < NUM_NODES; i++) {
+		if (check_state(cfe, precond, i)) {
+			if (!check_state(cfe, cond, i))
+				return false;
+		}
+	}
+
+	return true;
+}
+
+static int mipi_cfg_regs_show(struct seq_file *s, void *data)
+{
+	struct cfe_device *cfe = s->private;
+	int ret;
+
+	ret = pm_runtime_resume_and_get(&cfe->pdev->dev);
+	if (ret)
+		return ret;
+
+#define DUMP(reg) seq_printf(s, #reg " \t0x%08x\n", cfg_reg_read(cfe, reg))
+	DUMP(MIPICFG_CFG);
+	DUMP(MIPICFG_INTR);
+	DUMP(MIPICFG_INTE);
+	DUMP(MIPICFG_INTF);
+	DUMP(MIPICFG_INTS);
+#undef DUMP
+
+	pm_runtime_put(&cfe->pdev->dev);
+
+	return 0;
+}
+
+static int format_show(struct seq_file *s, void *data)
+{
+	struct cfe_device *cfe = s->private;
+	unsigned int i;
+
+	for (i = 0; i < NUM_NODES; i++) {
+		struct cfe_node *node = &cfe->node[i];
+		unsigned long sb, state = 0;
+
+		for (sb = 0; sb < NUM_STATES; sb++) {
+			if (check_state(cfe, BIT(sb), i))
+				state |= BIT(sb);
+		}
+
+		seq_printf(s, "\nNode %u (%s) state: 0x%lx\n", i,
+			   node_desc[i].name, state);
+
+		if (node_supports_image(node))
+			seq_printf(s, "format: " V4L2_FOURCC_CONV " 0x%x\n"
+				      "resolution: %ux%u\nbpl: %u\nsize: %u\n",
+				   V4L2_FOURCC_CONV_ARGS(node->vid_fmt.fmt.pix.pixelformat),
+				   node->vid_fmt.fmt.pix.pixelformat,
+				   node->vid_fmt.fmt.pix.width,
+				   node->vid_fmt.fmt.pix.height,
+				   node->vid_fmt.fmt.pix.bytesperline,
+				   node->vid_fmt.fmt.pix.sizeimage);
+
+		if (node_supports_meta(node))
+			seq_printf(s, "format: " V4L2_FOURCC_CONV " 0x%x\nsize: %u\n",
+				   V4L2_FOURCC_CONV_ARGS(node->meta_fmt.fmt.meta.dataformat),
+				   node->meta_fmt.fmt.meta.dataformat,
+				   node->meta_fmt.fmt.meta.buffersize);
+	}
+
+	return 0;
+}
+
+DEFINE_SHOW_ATTRIBUTE(mipi_cfg_regs);
+DEFINE_SHOW_ATTRIBUTE(format);
+
+/* Format setup functions */
+const struct cfe_fmt *find_format_by_code(u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(formats); i++) {
+		if (formats[i].code == code)
+			return &formats[i];
+	}
+
+	return NULL;
+}
+
+const struct cfe_fmt *find_format_by_pix(u32 pixelformat)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(formats); i++) {
+		if (formats[i].fourcc == pixelformat)
+			return &formats[i];
+	}
+
+	return NULL;
+}
+
+/*
+ * Given the mbus code, find the 16 bit remapped code. Returns 0 if no remap
+ * possible.
+ */
+u32 cfe_find_16bit_code(u32 code)
+{
+	const struct cfe_fmt *cfe_fmt;
+
+	cfe_fmt = find_format_by_code(code);
+
+	if (!cfe_fmt || !cfe_fmt->remap[CFE_REMAP_16BIT])
+		return 0;
+
+	cfe_fmt = find_format_by_pix(cfe_fmt->remap[CFE_REMAP_16BIT]);
+	if (!cfe_fmt)
+		return 0;
+
+	return cfe_fmt->code;
+}
+
+/*
+ * Given the mbus code, find the 8 bit compressed code. Returns 0 if no remap
+ * possible.
+ */
+u32 cfe_find_compressed_code(u32 code)
+{
+	const struct cfe_fmt *cfe_fmt;
+
+	cfe_fmt = find_format_by_code(code);
+
+	if (!cfe_fmt || !cfe_fmt->remap[CFE_REMAP_COMPRESSED])
+		return 0;
+
+	cfe_fmt = find_format_by_pix(cfe_fmt->remap[CFE_REMAP_COMPRESSED]);
+	if (!cfe_fmt)
+		return 0;
+
+	return cfe_fmt->code;
+}
+
+static int cfe_calc_format_size_bpl(struct cfe_device *cfe,
+				    const struct cfe_fmt *fmt,
+				    struct v4l2_format *f)
+{
+	unsigned int min_bytesperline;
+
+	v4l_bound_align_image(&f->fmt.pix.width, MIN_WIDTH, MAX_WIDTH, 2,
+			      &f->fmt.pix.height, MIN_HEIGHT, MAX_HEIGHT, 0, 0);
+
+	min_bytesperline =
+		ALIGN((f->fmt.pix.width * fmt->depth) >> 3, BPL_ALIGNMENT);
+
+	if (f->fmt.pix.bytesperline > min_bytesperline &&
+	    f->fmt.pix.bytesperline <= MAX_BYTESPERLINE)
+		f->fmt.pix.bytesperline =
+			ALIGN(f->fmt.pix.bytesperline, BPL_ALIGNMENT);
+	else
+		f->fmt.pix.bytesperline = min_bytesperline;
+
+	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+
+	cfe_dbg("%s: " V4L2_FOURCC_CONV " size: %ux%u bpl:%u img_size:%u\n",
+		__func__, V4L2_FOURCC_CONV_ARGS(f->fmt.pix.pixelformat),
+		f->fmt.pix.width, f->fmt.pix.height,
+		f->fmt.pix.bytesperline, f->fmt.pix.sizeimage);
+
+	return 0;
+}
+
+static void cfe_schedule_next_csi2_job(struct cfe_device *cfe)
+{
+	struct cfe_buffer *buf;
+	unsigned int i;
+	dma_addr_t addr;
+
+	for (i = 0; i < CSI2_NUM_CHANNELS; i++) {
+		struct cfe_node *node = &cfe->node[i];
+		unsigned int stride, size;
+
+		if (!check_state(cfe, NODE_STREAMING, i))
+			continue;
+
+		buf = list_first_entry(&node->dma_queue, struct cfe_buffer,
+				       list);
+		node->next_frm = buf;
+		list_del(&buf->list);
+
+		cfe_dbg_verbose("%s: [%s] buffer:%p\n", __func__,
+				node_desc[node->id].name, &buf->vb.vb2_buf);
+
+		if (is_meta_node(node)) {
+			size = node->meta_fmt.fmt.meta.buffersize;
+			stride = 0;
+		} else {
+			size = node->vid_fmt.fmt.pix.sizeimage;
+			stride = node->vid_fmt.fmt.pix.bytesperline;
+		}
+
+		addr = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0);
+		csi2_set_buffer(&cfe->csi2, node->id, addr, stride, size);
+	}
+}
+
+static void cfe_schedule_next_pisp_job(struct cfe_device *cfe)
+{
+	struct vb2_buffer *vb2_bufs[FE_NUM_PADS] = { 0 };
+	struct cfe_config_buffer *config_buf;
+	struct cfe_buffer *buf;
+	unsigned int i;
+
+	for (i = CSI2_NUM_CHANNELS; i < NUM_NODES; i++) {
+		struct cfe_node *node = &cfe->node[i];
+
+		if (!check_state(cfe, NODE_STREAMING, i))
+			continue;
+
+		buf = list_first_entry(&node->dma_queue, struct cfe_buffer,
+				       list);
+
+		cfe_dbg_verbose("%s: [%s] buffer:%p\n", __func__,
+				node_desc[node->id].name, &buf->vb.vb2_buf);
+
+		node->next_frm = buf;
+		vb2_bufs[node_desc[i].link_pad] = &buf->vb.vb2_buf;
+		list_del(&buf->list);
+	}
+
+	config_buf = to_cfe_config_buffer(cfe->node[FE_CONFIG].next_frm);
+	pisp_fe_submit_job(&cfe->fe, vb2_bufs, &config_buf->config);
+}
+
+static bool cfe_check_job_ready(struct cfe_device *cfe)
+{
+	unsigned int i;
+
+	for (i = 0; i < NUM_NODES; i++) {
+		struct cfe_node *node = &cfe->node[i];
+
+		if (!check_state(cfe, NODE_ENABLED, i))
+			continue;
+
+		if (list_empty(&node->dma_queue)) {
+			cfe_dbg_verbose("%s: [%s] has no buffer, unable to schedule job\n",
+				__func__, node_desc[i].name);
+			return false;
+		}
+	}
+
+	return true;
+}
+
+static void cfe_prepare_next_job(struct cfe_device *cfe)
+{
+	cfe->job_queued = true;
+	cfe_schedule_next_csi2_job(cfe);
+	if (is_fe_enabled(cfe))
+		cfe_schedule_next_pisp_job(cfe);
+
+	/* Flag if another job is ready after this. */
+	cfe->job_ready = cfe_check_job_ready(cfe);
+
+	cfe_dbg_verbose("%s: end with scheduled job\n", __func__);
+}
+
+static void cfe_process_buffer_complete(struct cfe_node *node,
+					enum vb2_buffer_state state)
+{
+	struct cfe_device *cfe = node->cfe;
+
+	cfe_dbg_verbose("%s: [%s] buffer:%p\n", __func__,
+			node_desc[node->id].name, &node->cur_frm->vb.vb2_buf);
+
+	node->cur_frm->vb.sequence = node->fs_count - 1;
+	vb2_buffer_done(&node->cur_frm->vb.vb2_buf, state);
+}
+
+static void cfe_queue_event_sof(struct cfe_node *node)
+{
+	struct v4l2_event event = {
+		.type = V4L2_EVENT_FRAME_SYNC,
+		.u.frame_sync.frame_sequence = node->fs_count - 1,
+	};
+
+	v4l2_event_queue(&node->video_dev, &event);
+}
+
+static void cfe_sof_isr_handler(struct cfe_node *node)
+{
+	struct cfe_device *cfe = node->cfe;
+	bool matching_fs = true;
+	unsigned int i;
+
+	cfe_dbg_verbose("%s: [%s] seq %u\n", __func__, node_desc[node->id].name,
+			node->fs_count);
+
+	/*
+	 * If the sensor is producing unexpected frame event ordering over a
+	 * sustained period of time, guard against the possibility of coming
+	 * here and orphaning the cur_frm if it's not been dequeued already.
+	 * Unfortunately, there is not enough hardware state to tell if this
+	 * may have occurred.
+	 */
+	if (WARN(node->cur_frm, "%s: [%s] Orphanded frame at seq %u\n",
+		 __func__, node_desc[node->id].name, node->fs_count))
+		cfe_process_buffer_complete(node, VB2_BUF_STATE_ERROR);
+
+	node->cur_frm = node->next_frm;
+	node->next_frm = NULL;
+	node->fs_count++;
+
+	node->ts = ktime_get_ns();
+	for (i = 0; i < NUM_NODES; i++) {
+		if (!check_state(cfe, NODE_STREAMING, i) || i == node->id)
+			continue;
+		/*
+		 * This checks if any other node has seen a FS. If yes, use the
+		 * same timestamp, eventually across all node buffers.
+		 */
+		if (cfe->node[i].fs_count >= node->fs_count)
+			node->ts = cfe->node[i].ts;
+		/*
+		 * This checks if all other node have seen a matching FS. If
+		 * yes, we can flag another job to be queued.
+		 */
+		if (matching_fs && cfe->node[i].fs_count != node->fs_count)
+			matching_fs = false;
+	}
+
+	if (matching_fs)
+		cfe->job_queued = false;
+
+	if (node->cur_frm)
+		node->cur_frm->vb.vb2_buf.timestamp = node->ts;
+
+	set_state(cfe, FS_INT, node->id);
+	clear_state(cfe, FE_INT, node->id);
+
+	if (is_image_output_node(node))
+		cfe_queue_event_sof(node);
+}
+
+static void cfe_eof_isr_handler(struct cfe_node *node)
+{
+	struct cfe_device *cfe = node->cfe;
+
+	cfe_dbg_verbose("%s: [%s] seq %u\n", __func__, node_desc[node->id].name,
+			node->fs_count - 1);
+
+	if (node->cur_frm)
+		cfe_process_buffer_complete(node, VB2_BUF_STATE_DONE);
+
+	node->cur_frm = NULL;
+	set_state(cfe, FE_INT, node->id);
+	clear_state(cfe, FS_INT, node->id);
+}
+
+static irqreturn_t cfe_isr(int irq, void *dev)
+{
+	struct cfe_device *cfe = dev;
+	unsigned int i;
+	bool sof[NUM_NODES] = {0}, eof[NUM_NODES] = {0};
+	u32 sts;
+
+	sts = cfg_reg_read(cfe, MIPICFG_INTS);
+
+	if (sts & MIPICFG_INT_CSI_DMA)
+		csi2_isr(&cfe->csi2, sof, eof);
+
+	if (sts & MIPICFG_INT_PISP_FE)
+		pisp_fe_isr(&cfe->fe, sof + CSI2_NUM_CHANNELS,
+			    eof + CSI2_NUM_CHANNELS);
+
+	spin_lock(&cfe->state_lock);
+
+	for (i = 0; i < NUM_NODES; i++) {
+		struct cfe_node *node = &cfe->node[i];
+
+		/*
+		 * The check_state(NODE_STREAMING) is to ensure we do not loop
+		 * over the CSI2_CHx nodes when the FE is active since they
+		 * generate interrupts even though the node is not streaming.
+		 */
+		if (!check_state(cfe, NODE_STREAMING, i) ||
+		    !(sof[i] || eof[i]))
+			continue;
+
+		/*
+		 * There are 3 cases where we could get FS + FE_ACK at
+		 * the same time:
+		 * 1) FE of the current frame, and FS of the next frame.
+		 * 2) FS + FE of the same frame.
+		 * 3) FE of the current frame, and FS + FE of the next
+		 *    frame. To handle this, see the sof handler below.
+		 *
+		 * (1) is handled implicitly by the ordering of the FE and FS
+		 * handlers below.
+		 */
+		if (eof[i]) {
+			/*
+			 * The condition below tests for (2). Run the FS handler
+			 * first before the FE handler, both for the current
+			 * frame.
+			 */
+			if (sof[i] && !check_state(cfe, FS_INT, i)) {
+				cfe_sof_isr_handler(node);
+				sof[i] = false;
+			}
+
+			cfe_eof_isr_handler(node);
+		}
+
+		if (sof[i]) {
+			/*
+			 * The condition below tests for (3). In such cases, we
+			 * come in here with FS flag set in the node state from
+			 * the previous frame since it only gets cleared in
+			 * eof_isr_handler(). Handle the FE for the previous
+			 * frame first before the FS handler for the current
+			 * frame.
+			 */
+			if (check_state(cfe, FS_INT, node->id) &&
+			    !check_state(cfe, FE_INT, node->id)) {
+				cfe_dbg("%s: [%s] Handling missing previous FE interrupt\n",
+					__func__, node_desc[node->id].name);
+				cfe_eof_isr_handler(node);
+			}
+
+			cfe_sof_isr_handler(node);
+		}
+
+		if (!cfe->job_queued && cfe->job_ready)
+			cfe_prepare_next_job(cfe);
+	}
+
+	spin_unlock(&cfe->state_lock);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * Stream helpers
+ */
+
+static void cfe_start_channel(struct cfe_node *node)
+{
+	struct cfe_device *cfe = node->cfe;
+	struct v4l2_subdev_state *state;
+	struct v4l2_mbus_framefmt *source_fmt;
+	const struct cfe_fmt *fmt;
+	unsigned long flags;
+	bool start_fe = is_fe_enabled(cfe) &&
+			test_all_nodes(cfe, NODE_ENABLED, NODE_STREAMING);
+
+	cfe_dbg("%s: [%s]\n", __func__, node_desc[node->id].name);
+
+	state = v4l2_subdev_lock_and_get_active_state(&cfe->csi2.sd);
+
+	if (start_fe) {
+		unsigned int width, height;
+
+		WARN_ON(!is_fe_enabled(cfe));
+		cfe_dbg("%s: %s using csi2 channel %d\n",
+			__func__, node_desc[FE_OUT0].name,
+			cfe->fe_csi2_channel);
+
+		source_fmt = v4l2_subdev_state_get_format(state,
+							cfe->fe_csi2_channel);
+		fmt = find_format_by_code(source_fmt->code);
+
+		width = source_fmt->width;
+		height = source_fmt->height;
+
+		/*
+		 * Must have a valid CSI2 datatype, but it will be 0 for 16-bit
+		 * formats to work-around the HW mismatch.
+		 */
+		WARN_ON(fmt->depth != 16 && !fmt->csi_dt);
+
+		/*
+		 * Start the associated CSI2 Channel as well.
+		 *
+		 * Must write to the ADDR register to latch the ctrl values
+		 * even if we are connected to the front end. Once running,
+		 * this is handled by the CSI2 AUTO_ARM mode.
+		 */
+		csi2_start_channel(&cfe->csi2, cfe->fe_csi2_channel,
+				   CSI2_MODE_FE_STREAMING,
+				   true, false, width, height);
+		csi2_set_buffer(&cfe->csi2, cfe->fe_csi2_channel, 0, 0, -1);
+		pisp_fe_start(&cfe->fe);
+	}
+
+	if (is_csi2_node(node)) {
+		unsigned int width = 0, height = 0;
+
+		u32 mode = CSI2_MODE_NORMAL;
+
+		source_fmt = v4l2_subdev_state_get_format(state,
+			node_desc[node->id].link_pad - CSI2_NUM_CHANNELS);
+		fmt = find_format_by_code(source_fmt->code);
+
+		/* Must have a valid CSI2 datatype. */
+		WARN_ON(!fmt->csi_dt);
+
+		if (is_image_output_node(node)) {
+			width = source_fmt->width;
+			height = source_fmt->height;
+
+			/* We don't need to repack in the case of 16-bit output. */
+			if (fmt->depth != 16 &&
+			    node->vid_fmt.fmt.pix.pixelformat ==
+					fmt->remap[CFE_REMAP_16BIT])
+				mode = CSI2_MODE_REMAP;
+			else if (node->vid_fmt.fmt.pix.pixelformat ==
+					fmt->remap[CFE_REMAP_COMPRESSED]) {
+				mode = CSI2_MODE_COMPRESSED;
+				csi2_set_compression(&cfe->csi2, node->id,
+						     CSI2_COMPRESSION_DELTA, 0,
+						     0);
+			}
+		}
+		/* Unconditionally start this CSI2 channel. */
+		csi2_start_channel(&cfe->csi2, node->id,
+				   mode,
+				   /* Auto arm */
+				   false,
+				   /* Pack bytes */
+				   is_meta_node(node) ? true : false,
+				   width, height);
+	}
+
+	v4l2_subdev_unlock_state(state);
+
+	spin_lock_irqsave(&cfe->state_lock, flags);
+	if (cfe->job_ready && test_all_nodes(cfe, NODE_ENABLED, NODE_STREAMING))
+		cfe_prepare_next_job(cfe);
+	spin_unlock_irqrestore(&cfe->state_lock, flags);
+}
+
+static void cfe_stop_channel(struct cfe_node *node, bool fe_stop)
+{
+	struct cfe_device *cfe = node->cfe;
+
+	cfe_dbg("%s: [%s] fe_stop %u\n", __func__,
+		node_desc[node->id].name, fe_stop);
+
+	if (fe_stop) {
+		csi2_stop_channel(&cfe->csi2, cfe->fe_csi2_channel);
+		pisp_fe_stop(&cfe->fe);
+	}
+
+	if (is_csi2_node(node))
+		csi2_stop_channel(&cfe->csi2, node->id);
+}
+
+static void cfe_return_buffers(struct cfe_node *node,
+			       enum vb2_buffer_state state)
+{
+	struct cfe_device *cfe = node->cfe;
+	struct cfe_buffer *buf, *tmp;
+	unsigned long flags;
+
+	cfe_dbg("%s: [%s]\n", __func__, node_desc[node->id].name);
+
+	spin_lock_irqsave(&cfe->state_lock, flags);
+	list_for_each_entry_safe(buf, tmp, &node->dma_queue, list) {
+		list_del(&buf->list);
+		vb2_buffer_done(&buf->vb.vb2_buf, state);
+	}
+
+	if (node->cur_frm)
+		vb2_buffer_done(&node->cur_frm->vb.vb2_buf, state);
+	if (node->next_frm && node->cur_frm != node->next_frm)
+		vb2_buffer_done(&node->next_frm->vb.vb2_buf, state);
+
+	node->cur_frm = NULL;
+	node->next_frm = NULL;
+	spin_unlock_irqrestore(&cfe->state_lock, flags);
+}
+
+/*
+ * vb2 ops
+ */
+
+static int cfe_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
+			   unsigned int *nplanes, unsigned int sizes[],
+			   struct device *alloc_devs[])
+{
+	struct cfe_node *node = vb2_get_drv_priv(vq);
+	struct cfe_device *cfe = node->cfe;
+	unsigned int size = is_image_node(node) ? node->vid_fmt.fmt.pix.sizeimage :
+						  node->meta_fmt.fmt.meta.buffersize;
+
+	cfe_dbg("%s: [%s] type:%u\n", __func__, node_desc[node->id].name,
+		node->buffer_queue.type);
+
+	if (vb2_get_num_buffers(vq) + *nbuffers < 3)
+		*nbuffers = 3 - vb2_get_num_buffers(vq);
+
+	if (*nplanes) {
+		if (sizes[0] < size) {
+			cfe_err("sizes[0] %i < size %u\n", sizes[0], size);
+			return -EINVAL;
+		}
+		size = sizes[0];
+	}
+
+	*nplanes = 1;
+	sizes[0] = size;
+
+	return 0;
+}
+
+static int cfe_buffer_prepare(struct vb2_buffer *vb)
+{
+	struct cfe_node *node = vb2_get_drv_priv(vb->vb2_queue);
+	struct cfe_device *cfe = node->cfe;
+	struct cfe_buffer *buf = to_cfe_buffer(vb);
+	unsigned long size;
+
+	cfe_dbg_verbose("%s: [%s] buffer:%p\n", __func__,
+			node_desc[node->id].name, vb);
+
+	size = is_image_node(node) ? node->vid_fmt.fmt.pix.sizeimage :
+				     node->meta_fmt.fmt.meta.buffersize;
+	if (vb2_plane_size(vb, 0) < size) {
+		cfe_err("data will not fit into plane (%lu < %lu)\n",
+			vb2_plane_size(vb, 0), size);
+		return -EINVAL;
+	}
+
+	vb2_set_plane_payload(&buf->vb.vb2_buf, 0, size);
+
+	if (node->id == FE_CONFIG) {
+		struct cfe_config_buffer *b = to_cfe_config_buffer(buf);
+		void *addr = vb2_plane_vaddr(vb, 0);
+
+		memcpy(&b->config, addr, sizeof(struct pisp_fe_config));
+		return pisp_fe_validate_config(&cfe->fe, &b->config,
+					       &cfe->node[FE_OUT0].vid_fmt,
+					       &cfe->node[FE_OUT1].vid_fmt);
+	}
+
+	return 0;
+}
+
+static void cfe_buffer_queue(struct vb2_buffer *vb)
+{
+	struct cfe_node *node = vb2_get_drv_priv(vb->vb2_queue);
+	struct cfe_device *cfe = node->cfe;
+	struct cfe_buffer *buf = to_cfe_buffer(vb);
+	unsigned long flags;
+
+	cfe_dbg_verbose("%s: [%s] buffer:%p\n", __func__,
+			node_desc[node->id].name, vb);
+
+	spin_lock_irqsave(&cfe->state_lock, flags);
+
+	list_add_tail(&buf->list, &node->dma_queue);
+
+	if (!cfe->job_ready)
+		cfe->job_ready = cfe_check_job_ready(cfe);
+
+	if (!cfe->job_queued && cfe->job_ready &&
+	    test_all_nodes(cfe, NODE_ENABLED, NODE_STREAMING)) {
+		cfe_dbg("Preparing job immediately for channel %u\n",
+			node->id);
+		cfe_prepare_next_job(cfe);
+	}
+
+	spin_unlock_irqrestore(&cfe->state_lock, flags);
+}
+
+static u64 sensor_link_rate(struct cfe_device *cfe)
+{
+	struct v4l2_mbus_framefmt *source_fmt;
+	struct v4l2_subdev_state *state;
+	struct media_entity *entity;
+	struct v4l2_subdev *subdev;
+	const struct cfe_fmt *fmt;
+	struct media_pad *pad;
+	s64 link_freq;
+
+	state = v4l2_subdev_lock_and_get_active_state(&cfe->csi2.sd);
+	source_fmt = v4l2_subdev_state_get_format(state, 0);
+	fmt = find_format_by_code(source_fmt->code);
+	v4l2_subdev_unlock_state(state);
+
+	/*
+	 * Walk up the media graph to find either the sensor entity, or another
+	 * entity that advertises the V4L2_CID_LINK_FREQ or V4L2_CID_PIXEL_RATE
+	 * control through the subdev.
+	 */
+	entity = &cfe->csi2.sd.entity;
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			goto err;
+
+		pad = media_pad_remote_pad_first(pad);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			goto err;
+
+		entity = pad->entity;
+		subdev = media_entity_to_v4l2_subdev(entity);
+		if (entity->function == MEDIA_ENT_F_CAM_SENSOR ||
+		    v4l2_ctrl_find(subdev->ctrl_handler, V4L2_CID_LINK_FREQ) ||
+		    v4l2_ctrl_find(subdev->ctrl_handler, V4L2_CID_PIXEL_RATE))
+			break;
+	}
+
+	link_freq = v4l2_get_link_freq(pad, fmt->depth,
+				       cfe->csi2.dphy.active_lanes * 2);
+	if (link_freq < 0)
+		goto err;
+
+	/* x2 for DDR. */
+	link_freq *= 2;
+	cfe_info("Using a link rate of %lld Mbps\n", link_freq / (1000 * 1000));
+	return link_freq;
+
+err:
+	cfe_err("Unable to determine sensor link rate, using 999 Mbps\n");
+	return 999 * 1000000UL;
+}
+
+static int cfe_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct v4l2_mbus_config mbus_config = { 0 };
+	struct cfe_node *node = vb2_get_drv_priv(vq);
+	struct cfe_device *cfe = node->cfe;
+	int ret;
+
+	cfe_dbg("%s: [%s] begin.\n", __func__, node_desc[node->id].name);
+
+	if (!check_state(cfe, NODE_ENABLED, node->id)) {
+		cfe_err("%s node link is not enabled.\n",
+			node_desc[node->id].name);
+		ret = -EINVAL;
+		goto err_streaming;
+	}
+
+	ret = pm_runtime_resume_and_get(&cfe->pdev->dev);
+	if (ret < 0) {
+		cfe_err("pm_runtime_resume_and_get failed\n");
+		goto err_streaming;
+	}
+
+	/* When using the Frontend, we must enable the FE_CONFIG node. */
+	if (is_fe_enabled(cfe) &&
+	    !check_state(cfe, NODE_ENABLED, cfe->node[FE_CONFIG].id)) {
+		cfe_err("FE enabled, but FE_CONFIG node is not\n");
+		ret = -EINVAL;
+		goto err_pm_put;
+	}
+
+	ret = media_pipeline_start(&node->pad, &cfe->pipe);
+	if (ret < 0) {
+		cfe_err("Failed to start media pipeline: %d\n", ret);
+		goto err_pm_put;
+	}
+
+	clear_state(cfe, FS_INT | FE_INT, node->id);
+	set_state(cfe, NODE_STREAMING, node->id);
+	node->fs_count = 0;
+	cfe_start_channel(node);
+
+	if (!test_all_nodes(cfe, NODE_ENABLED, NODE_STREAMING)) {
+		cfe_dbg("Not all nodes are set to streaming yet!\n");
+		return 0;
+	}
+
+	cfg_reg_write(cfe, MIPICFG_CFG, MIPICFG_CFG_SEL_CSI);
+	cfg_reg_write(cfe, MIPICFG_INTE, MIPICFG_INT_CSI_DMA | MIPICFG_INT_PISP_FE);
+
+	ret = v4l2_subdev_call(cfe->sensor, pad, get_mbus_config, 0,
+			       &mbus_config);
+	if (ret < 0 && ret != -ENOIOCTLCMD) {
+		cfe_err("g_mbus_config failed\n");
+		goto err_pm_put;
+	}
+
+	cfe->csi2.dphy.active_lanes = mbus_config.bus.mipi_csi2.num_data_lanes;
+	if (!cfe->csi2.dphy.active_lanes)
+		cfe->csi2.dphy.active_lanes = cfe->csi2.dphy.max_lanes;
+	if (cfe->csi2.dphy.active_lanes > cfe->csi2.dphy.max_lanes) {
+		cfe_err("Device has requested %u data lanes, which is >%u configured in DT\n",
+			cfe->csi2.dphy.active_lanes, cfe->csi2.dphy.max_lanes);
+		ret = -EINVAL;
+		goto err_disable_cfe;
+	}
+
+	cfe_dbg("Configuring CSI-2 block - %u data lanes\n", cfe->csi2.dphy.active_lanes);
+	cfe->csi2.dphy.dphy_rate = sensor_link_rate(cfe) / 1000000UL;
+	csi2_open_rx(&cfe->csi2);
+
+	cfe_dbg("Starting sensor streaming\n");
+	ret = v4l2_subdev_call(cfe->sensor, video, s_stream, 1);
+	if (ret < 0) {
+		cfe_err("stream on failed in subdev\n");
+		goto err_disable_cfe;
+	}
+
+	cfe_dbg("%s: [%s] end.\n", __func__, node_desc[node->id].name);
+
+	return 0;
+
+err_disable_cfe:
+	csi2_close_rx(&cfe->csi2);
+	cfe_stop_channel(node, true);
+	media_pipeline_stop(&node->pad);
+err_pm_put:
+	pm_runtime_put(&cfe->pdev->dev);
+err_streaming:
+	cfe_return_buffers(node, VB2_BUF_STATE_QUEUED);
+	clear_state(cfe, NODE_STREAMING, node->id);
+
+	return ret;
+}
+
+static void cfe_stop_streaming(struct vb2_queue *vq)
+{
+	struct cfe_node *node = vb2_get_drv_priv(vq);
+	struct cfe_device *cfe = node->cfe;
+	unsigned long flags;
+	bool fe_stop;
+
+	cfe_dbg("%s: [%s] begin.\n", __func__, node_desc[node->id].name);
+
+	spin_lock_irqsave(&cfe->state_lock, flags);
+	fe_stop = is_fe_enabled(cfe) &&
+		  test_all_nodes(cfe, NODE_ENABLED, NODE_STREAMING);
+
+	cfe->job_ready = false;
+	clear_state(cfe, NODE_STREAMING, node->id);
+	spin_unlock_irqrestore(&cfe->state_lock, flags);
+
+	cfe_stop_channel(node, fe_stop);
+
+	if (!test_any_node(cfe, NODE_STREAMING)) {
+		/* Stop streaming the sensor and disable the peripheral. */
+		if (v4l2_subdev_call(cfe->sensor, video, s_stream, 0) < 0)
+			cfe_err("stream off failed in subdev\n");
+
+		csi2_close_rx(&cfe->csi2);
+
+		cfg_reg_write(cfe, MIPICFG_INTE, 0);
+	}
+
+	media_pipeline_stop(&node->pad);
+
+	/* Clear all queued buffers for the node */
+	cfe_return_buffers(node, VB2_BUF_STATE_ERROR);
+
+	pm_runtime_put(&cfe->pdev->dev);
+
+	cfe_dbg("%s: [%s] end.\n", __func__, node_desc[node->id].name);
+}
+
+static const struct vb2_ops cfe_video_qops = {
+	.wait_prepare = vb2_ops_wait_prepare,
+	.wait_finish = vb2_ops_wait_finish,
+	.queue_setup = cfe_queue_setup,
+	.buf_prepare = cfe_buffer_prepare,
+	.buf_queue = cfe_buffer_queue,
+	.start_streaming = cfe_start_streaming,
+	.stop_streaming = cfe_stop_streaming,
+};
+
+/*
+ * v4l2 ioctl ops
+ */
+
+static int cfe_querycap(struct file *file, void *priv,
+			struct v4l2_capability *cap)
+{
+	struct cfe_node *node = video_drvdata(file);
+	struct cfe_device *cfe = node->cfe;
+
+	strscpy(cap->driver, CFE_MODULE_NAME, sizeof(cap->driver));
+	strscpy(cap->card, CFE_MODULE_NAME, sizeof(cap->card));
+
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+		 dev_name(&cfe->pdev->dev));
+
+	cap->capabilities |= V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_META_CAPTURE |
+			     V4L2_CAP_META_OUTPUT;
+
+	return 0;
+}
+
+static int cfe_enum_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_fmtdesc *f)
+{
+	struct cfe_node *node = video_drvdata(file);
+	struct cfe_device *cfe = node->cfe;
+	unsigned int i, j;
+
+	if (!node_supports_image_output(node))
+		return -EINVAL;
+
+	cfe_dbg("%s: [%s]\n", __func__, node_desc[node->id].name);
+
+	for (i = 0, j = 0; i < ARRAY_SIZE(formats); i++) {
+		if (f->mbus_code && formats[i].code != f->mbus_code)
+			continue;
+
+		if (formats[i].flags & CFE_FORMAT_FLAG_META_OUT ||
+		    formats[i].flags & CFE_FORMAT_FLAG_META_CAP)
+			continue;
+
+		if (is_fe_node(node) &&
+		    !(formats[i].flags & CFE_FORMAT_FLAG_FE_OUT))
+			continue;
+
+		if (j == f->index) {
+			f->pixelformat = formats[i].fourcc;
+			f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+			return 0;
+		}
+		j++;
+	}
+
+	return -EINVAL;
+}
+
+static int cfe_g_fmt(struct file *file, void *priv,
+		     struct v4l2_format *f)
+{
+	struct cfe_node *node = video_drvdata(file);
+	struct cfe_device *cfe = node->cfe;
+
+	cfe_dbg("%s: [%s]\n", __func__, node_desc[node->id].name);
+
+	if (!node_supports_image(node))
+		return -EINVAL;
+
+	*f = node->vid_fmt;
+
+	return 0;
+}
+
+static int try_fmt_vid_cap(struct cfe_node *node, struct v4l2_format *f)
+{
+	struct cfe_device *cfe = node->cfe;
+	const struct cfe_fmt *fmt;
+
+	cfe_dbg("%s: [%s] %ux%u, V4L2 pix " V4L2_FOURCC_CONV "\n",
+		__func__, node_desc[node->id].name,
+		f->fmt.pix.width, f->fmt.pix.height,
+		V4L2_FOURCC_CONV_ARGS(f->fmt.pix.pixelformat));
+
+	if (!node_supports_image_output(node))
+		return -EINVAL;
+
+	/*
+	 * Default to a format that works for both CSI2 and FE.
+	 */
+	fmt = find_format_by_pix(f->fmt.pix.pixelformat);
+	if (!fmt)
+		fmt = find_format_by_code(MEDIA_BUS_FMT_SBGGR10_1X10);
+
+	f->fmt.pix.pixelformat = fmt->fourcc;
+
+	if (is_fe_node(node) && fmt->remap[CFE_REMAP_16BIT]) {
+		f->fmt.pix.pixelformat = fmt->remap[CFE_REMAP_16BIT];
+		fmt = find_format_by_pix(f->fmt.pix.pixelformat);
+	}
+
+	f->fmt.pix.field = V4L2_FIELD_NONE;
+
+	cfe_calc_format_size_bpl(cfe, fmt, f);
+
+	return 0;
+}
+
+static int cfe_s_fmt_vid_cap(struct file *file, void *priv,
+			     struct v4l2_format *f)
+{
+	struct cfe_node *node = video_drvdata(file);
+	struct cfe_device *cfe = node->cfe;
+	struct vb2_queue *q = &node->buffer_queue;
+	int ret;
+
+	cfe_dbg("%s: [%s]\n", __func__, node_desc[node->id].name);
+
+	if (vb2_is_busy(q))
+		return -EBUSY;
+
+	ret = try_fmt_vid_cap(node, f);
+	if (ret)
+		return ret;
+
+	node->vid_fmt = *f;
+
+	cfe_dbg("%s: Set %ux%u, V4L2 pix " V4L2_FOURCC_CONV "\n", __func__,
+		node->vid_fmt.fmt.pix.width, node->vid_fmt.fmt.pix.height,
+		V4L2_FOURCC_CONV_ARGS(node->vid_fmt.fmt.pix.pixelformat));
+
+	return 0;
+}
+
+static int cfe_try_fmt_vid_cap(struct file *file, void *priv,
+			       struct v4l2_format *f)
+{
+	struct cfe_node *node = video_drvdata(file);
+	struct cfe_device *cfe = node->cfe;
+
+	cfe_dbg("%s: [%s]\n", __func__, node_desc[node->id].name);
+
+	return try_fmt_vid_cap(node, f);
+}
+
+static int cfe_enum_fmt_meta(struct file *file, void *priv,
+			     struct v4l2_fmtdesc *f)
+{
+	struct cfe_node *node = video_drvdata(file);
+	struct cfe_device *cfe = node->cfe;
+
+	cfe_dbg("%s: [%s]\n", __func__, node_desc[node->id].name);
+
+	if (!node_supports_meta(node) || f->index != 0)
+		return -EINVAL;
+
+	switch (node->id) {
+	case CSI2_CH0...CSI2_CH3:
+		f->pixelformat = V4L2_META_FMT_SENSOR_DATA;
+		return 0;
+	case FE_STATS:
+		f->pixelformat = V4L2_META_FMT_RPI_FE_STATS;
+		return 0;
+	case FE_CONFIG:
+		f->pixelformat = V4L2_META_FMT_RPI_FE_CFG;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int try_fmt_meta(struct cfe_node *node, struct v4l2_format *f)
+{
+	if (!node_supports_meta(node))
+		return -EINVAL;
+
+	switch (node->id) {
+	case CSI2_CH0...CSI2_CH3:
+		f->fmt.meta.dataformat = V4L2_META_FMT_SENSOR_DATA;
+		if (!f->fmt.meta.buffersize)
+			f->fmt.meta.buffersize = DEFAULT_EMBEDDED_SIZE;
+		f->fmt.meta.buffersize =
+			min_t(u32, f->fmt.meta.buffersize, MAX_BUFFER_SIZE);
+		f->fmt.meta.buffersize =
+			ALIGN(f->fmt.meta.buffersize, BPL_ALIGNMENT);
+		return 0;
+	case FE_STATS:
+		f->fmt.meta.dataformat = V4L2_META_FMT_RPI_FE_STATS;
+		f->fmt.meta.buffersize = sizeof(struct pisp_statistics);
+		return 0;
+	case FE_CONFIG:
+		f->fmt.meta.dataformat = V4L2_META_FMT_RPI_FE_CFG;
+		f->fmt.meta.buffersize = sizeof(struct pisp_fe_config);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int cfe_g_fmt_meta(struct file *file, void *priv, struct v4l2_format *f)
+{
+	struct cfe_node *node = video_drvdata(file);
+	struct cfe_device *cfe = node->cfe;
+
+	cfe_dbg("%s: [%s]\n", __func__, node_desc[node->id].name);
+
+	if (!node_supports_meta(node))
+		return -EINVAL;
+
+	*f = node->meta_fmt;
+
+	return 0;
+}
+
+static int cfe_s_fmt_meta(struct file *file, void *priv, struct v4l2_format *f)
+{
+	struct cfe_node *node = video_drvdata(file);
+	struct cfe_device *cfe = node->cfe;
+	struct vb2_queue *q = &node->buffer_queue;
+	int ret;
+
+	cfe_dbg("%s: [%s]\n", __func__, node_desc[node->id].name);
+
+	if (vb2_is_busy(q))
+		return -EBUSY;
+
+	if (!node_supports_meta(node))
+		return -EINVAL;
+
+	ret = try_fmt_meta(node, f);
+	if (ret)
+		return ret;
+
+	node->meta_fmt = *f;
+
+	cfe_dbg("%s: Set " V4L2_FOURCC_CONV "\n", __func__,
+		V4L2_FOURCC_CONV_ARGS(node->meta_fmt.fmt.meta.dataformat));
+
+	return 0;
+}
+
+static int cfe_try_fmt_meta(struct file *file, void *priv,
+			    struct v4l2_format *f)
+{
+	struct cfe_node *node = video_drvdata(file);
+	struct cfe_device *cfe = node->cfe;
+
+	cfe_dbg("%s: [%s]\n", __func__, node_desc[node->id].name);
+	return try_fmt_meta(node, f);
+}
+
+static int cfe_enum_framesizes(struct file *file, void *priv,
+			       struct v4l2_frmsizeenum *fsize)
+{
+	struct cfe_node *node = video_drvdata(file);
+	struct cfe_device *cfe = node->cfe;
+	const struct cfe_fmt *fmt;
+
+	cfe_dbg("%s [%s]\n", __func__, node_desc[node->id].name);
+
+	if (fsize->index > 0)
+		return -EINVAL;
+
+	/* check for valid format */
+	fmt = find_format_by_pix(fsize->pixel_format);
+	if (!fmt) {
+		cfe_dbg("Invalid pixel code: %x\n", fsize->pixel_format);
+		return -EINVAL;
+	}
+
+	/* TODO: Do we have limits on the step_width? */
+
+	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
+	fsize->stepwise.min_width = MIN_WIDTH;
+	fsize->stepwise.max_width = MAX_WIDTH;
+	fsize->stepwise.step_width = 2;
+	fsize->stepwise.min_height = MIN_HEIGHT;
+	fsize->stepwise.max_height = MAX_HEIGHT;
+	fsize->stepwise.step_height = 1;
+
+	return 0;
+}
+
+static int cfe_vb2_ioctl_reqbufs(struct file *file, void *priv,
+				 struct v4l2_requestbuffers *p)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct cfe_node *node = video_get_drvdata(vdev);
+	struct cfe_device *cfe = node->cfe;
+	int ret;
+
+	cfe_dbg("%s: [%s] type:%u\n", __func__, node_desc[node->id].name,
+		p->type);
+
+	if (p->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+	    p->type != V4L2_BUF_TYPE_META_CAPTURE &&
+	    p->type != V4L2_BUF_TYPE_META_OUTPUT)
+		return -EINVAL;
+
+	ret = vb2_queue_change_type(vdev->queue, p->type);
+	if (ret)
+		return ret;
+
+	return vb2_ioctl_reqbufs(file, priv, p);
+}
+
+static int cfe_vb2_ioctl_create_bufs(struct file *file, void *priv,
+				     struct v4l2_create_buffers *p)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct cfe_node *node = video_get_drvdata(vdev);
+	struct cfe_device *cfe = node->cfe;
+	int ret;
+
+	cfe_dbg("%s: [%s] type:%u\n", __func__, node_desc[node->id].name,
+		p->format.type);
+
+	if (p->format.type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+	    p->format.type != V4L2_BUF_TYPE_META_CAPTURE &&
+	    p->format.type != V4L2_BUF_TYPE_META_OUTPUT)
+		return -EINVAL;
+
+	ret = vb2_queue_change_type(vdev->queue, p->format.type);
+	if (ret)
+		return ret;
+
+	return vb2_ioctl_create_bufs(file, priv, p);
+}
+
+static int cfe_subscribe_event(struct v4l2_fh *fh,
+			       const struct v4l2_event_subscription *sub)
+{
+	struct cfe_node *node = video_get_drvdata(fh->vdev);
+
+	switch (sub->type) {
+	case V4L2_EVENT_FRAME_SYNC:
+		if (!node_supports_image_output(node))
+			break;
+
+		return v4l2_event_subscribe(fh, sub, 2, NULL);
+	case V4L2_EVENT_SOURCE_CHANGE:
+		if (!node_supports_image_output(node) &&
+		    !node_supports_meta_output(node))
+			break;
+
+		return v4l2_event_subscribe(fh, sub, 4, NULL);
+	}
+
+	return v4l2_ctrl_subscribe_event(fh, sub);
+}
+
+static const struct v4l2_ioctl_ops cfe_ioctl_ops = {
+	.vidioc_querycap = cfe_querycap,
+	.vidioc_enum_fmt_vid_cap = cfe_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap = cfe_g_fmt,
+	.vidioc_s_fmt_vid_cap = cfe_s_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap = cfe_try_fmt_vid_cap,
+
+	.vidioc_enum_fmt_meta_cap = cfe_enum_fmt_meta,
+	.vidioc_g_fmt_meta_cap = cfe_g_fmt_meta,
+	.vidioc_s_fmt_meta_cap = cfe_s_fmt_meta,
+	.vidioc_try_fmt_meta_cap = cfe_try_fmt_meta,
+
+	.vidioc_enum_fmt_meta_out = cfe_enum_fmt_meta,
+	.vidioc_g_fmt_meta_out = cfe_g_fmt_meta,
+	.vidioc_s_fmt_meta_out = cfe_s_fmt_meta,
+	.vidioc_try_fmt_meta_out = cfe_try_fmt_meta,
+
+	.vidioc_enum_framesizes = cfe_enum_framesizes,
+
+	.vidioc_reqbufs = cfe_vb2_ioctl_reqbufs,
+	.vidioc_create_bufs = cfe_vb2_ioctl_create_bufs,
+	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
+	.vidioc_querybuf = vb2_ioctl_querybuf,
+	.vidioc_qbuf = vb2_ioctl_qbuf,
+	.vidioc_dqbuf = vb2_ioctl_dqbuf,
+	.vidioc_expbuf = vb2_ioctl_expbuf,
+	.vidioc_streamon = vb2_ioctl_streamon,
+	.vidioc_streamoff = vb2_ioctl_streamoff,
+
+	.vidioc_subscribe_event = cfe_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+};
+
+static void cfe_notify(struct v4l2_subdev *sd, unsigned int notification,
+		       void *arg)
+{
+	struct cfe_device *cfe = to_cfe_device(sd->v4l2_dev);
+	unsigned int i;
+
+	switch (notification) {
+	case V4L2_DEVICE_NOTIFY_EVENT:
+		for (i = 0; i < NUM_NODES; i++) {
+			struct cfe_node *node = &cfe->node[i];
+
+			if (check_state(cfe, NODE_REGISTERED, i))
+				continue;
+
+			v4l2_event_queue(&node->video_dev, arg);
+		}
+		break;
+	default:
+		break;
+	}
+}
+
+/* cfe capture driver file operations */
+static const struct v4l2_file_operations cfe_fops = {
+	.owner = THIS_MODULE,
+	.open = v4l2_fh_open,
+	.release = vb2_fop_release,
+	.poll = vb2_fop_poll,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap = vb2_fop_mmap,
+};
+
+static int cfe_video_link_validate(struct cfe_node *node,
+				   struct v4l2_subdev *remote_sd,
+				   struct v4l2_mbus_framefmt *remote_fmt)
+{
+	struct cfe_device *cfe = node->cfe;
+
+	if (!remote_fmt) {
+		return -EINVAL;
+	}
+
+	if (is_image_output_node(node)) {
+		struct v4l2_pix_format *pix_fmt = &node->vid_fmt.fmt.pix;
+		const struct cfe_fmt *fmt = NULL;
+		unsigned int i;
+
+		if (remote_fmt->width != pix_fmt->width ||
+		    remote_fmt->height != pix_fmt->height) {
+			cfe_err("Wrong width or height %ux%u (remote pad set to %ux%u)\n",
+				pix_fmt->width, pix_fmt->height,
+				remote_fmt->width,
+				remote_fmt->height);
+			return -EINVAL;
+		}
+
+		for (i = 0; i < ARRAY_SIZE(formats); i++) {
+			if (formats[i].code == remote_fmt->code &&
+			    formats[i].fourcc == pix_fmt->pixelformat) {
+				fmt = &formats[i];
+				break;
+			}
+		}
+		if (!fmt) {
+			cfe_err("Format mismatch!\n");
+			return -EINVAL;
+		}
+	} else if (is_csi2_node(node) && is_meta_output_node(node)) {
+		struct v4l2_meta_format *meta_fmt = &node->meta_fmt.fmt.meta;
+		const struct cfe_fmt *fmt;
+		u32 remote_size;
+
+		fmt = find_format_by_code(remote_fmt->code);
+		if (!fmt || fmt->fourcc != meta_fmt->dataformat) {
+			cfe_err("Metadata format mismatch!\n");
+			return -EINVAL;
+		}
+
+		remote_size = DIV_ROUND_UP(remote_fmt->width * remote_fmt->height * fmt->depth, 8);
+
+		if (remote_fmt->code != MEDIA_BUS_FMT_SENSOR_DATA) {
+			cfe_err("Bad metadata mbus format\n");
+			return -EINVAL;
+		}
+
+		if (remote_size > meta_fmt->buffersize) {
+			cfe_err("Metadata buffer too small: %u < %u\n",
+				meta_fmt->buffersize, remote_size);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static int cfe_video_link_validate_output(struct media_link *link)
+{
+	struct video_device *vd = container_of(link->source->entity,
+					       struct video_device, entity);
+	struct cfe_node *node = container_of(vd, struct cfe_node, video_dev);
+	struct cfe_device *cfe = node->cfe;
+	struct v4l2_mbus_framefmt *sink_fmt;
+	struct v4l2_subdev_state *state;
+	struct v4l2_subdev *sink_sd;
+	int ret;
+
+	cfe_dbg("%s: [%s] link \"%s\":%u -> \"%s\":%u\n", __func__,
+		node_desc[node->id].name,
+		link->source->entity->name, link->source->index,
+		link->sink->entity->name, link->sink->index);
+
+	if (!media_entity_remote_source_pad_unique(link->source->entity)) {
+		cfe_err("video node %s pad not connected\n", vd->name);
+		return -ENOTCONN;
+	}
+
+	sink_sd = media_entity_to_v4l2_subdev(link->sink->entity);
+
+	state = v4l2_subdev_lock_and_get_active_state(sink_sd);
+
+	sink_fmt = v4l2_subdev_state_get_format(state, link->sink->index);
+
+	ret = cfe_video_link_validate(node, sink_sd, sink_fmt);
+
+	v4l2_subdev_unlock_state(state);
+
+	return ret;
+}
+
+static int cfe_video_link_validate_capture(struct media_link *link)
+{
+	struct video_device *vd = container_of(link->sink->entity,
+					       struct video_device, entity);
+	struct cfe_node *node = container_of(vd, struct cfe_node, video_dev);
+	struct cfe_device *cfe = node->cfe;
+	struct v4l2_mbus_framefmt *source_fmt;
+	struct v4l2_subdev_state *state;
+	struct v4l2_subdev *source_sd;
+	int ret;
+
+	cfe_dbg("%s: [%s] link \"%s\":%u -> \"%s\":%u\n", __func__,
+		node_desc[node->id].name,
+		link->source->entity->name, link->source->index,
+		link->sink->entity->name, link->sink->index);
+
+	if (!media_entity_remote_source_pad_unique(link->sink->entity)) {
+		cfe_err("video node %s pad not connected\n", vd->name);
+		return -ENOTCONN;
+	}
+
+	source_sd = media_entity_to_v4l2_subdev(link->source->entity);
+
+	state = v4l2_subdev_lock_and_get_active_state(source_sd);
+
+	source_fmt = v4l2_subdev_state_get_format(state, link->source->index);
+
+	ret = cfe_video_link_validate(node, source_sd, source_fmt);
+
+	v4l2_subdev_unlock_state(state);
+
+	return ret;
+}
+
+static const struct media_entity_operations cfe_media_entity_ops_output = {
+	.link_validate = cfe_video_link_validate_output,
+};
+
+static const struct media_entity_operations cfe_media_entity_ops_capture = {
+	.link_validate = cfe_video_link_validate_capture,
+};
+
+static int cfe_video_link_notify(struct media_link *link, u32 flags,
+				 unsigned int notification)
+{
+	struct media_device *mdev = link->graph_obj.mdev;
+	struct cfe_device *cfe = container_of(mdev, struct cfe_device, mdev);
+	struct media_entity *fe = &cfe->fe.sd.entity;
+	struct media_entity *csi2 = &cfe->csi2.sd.entity;
+	unsigned long lock_flags;
+	unsigned int i;
+
+	if (notification != MEDIA_DEV_NOTIFY_POST_LINK_CH)
+		return 0;
+
+	cfe_dbg("%s: %s[%u] -> %s[%u] 0x%x", __func__,
+		link->source->entity->name, link->source->index,
+		link->sink->entity->name, link->sink->index, flags);
+
+	spin_lock_irqsave(&cfe->state_lock, lock_flags);
+
+	for (i = 0; i < NUM_NODES; i++) {
+		if (link->sink->entity != &cfe->node[i].video_dev.entity &&
+		    link->source->entity != &cfe->node[i].video_dev.entity)
+			continue;
+
+		if (link->flags & MEDIA_LNK_FL_ENABLED)
+			set_state(cfe, NODE_ENABLED, i);
+		else
+			clear_state(cfe, NODE_ENABLED, i);
+
+		break;
+	}
+
+	spin_unlock_irqrestore(&cfe->state_lock, lock_flags);
+
+	if (link->source->entity != csi2)
+		return 0;
+	if (link->sink->entity != fe)
+		return 0;
+	if (link->sink->index != 0)
+		return 0;
+
+	cfe->fe_csi2_channel = -1;
+	if (link->flags & MEDIA_LNK_FL_ENABLED) {
+		if (link->source->index == node_desc[CSI2_CH0].link_pad)
+			cfe->fe_csi2_channel = CSI2_CH0;
+		else if (link->source->index == node_desc[CSI2_CH1].link_pad)
+			cfe->fe_csi2_channel = CSI2_CH1;
+		else if (link->source->index == node_desc[CSI2_CH2].link_pad)
+			cfe->fe_csi2_channel = CSI2_CH2;
+		else if (link->source->index == node_desc[CSI2_CH3].link_pad)
+			cfe->fe_csi2_channel = CSI2_CH3;
+	}
+
+	if (is_fe_enabled(cfe))
+		cfe_dbg("%s: Found CSI2:%d -> FE:0 link\n", __func__,
+			cfe->fe_csi2_channel);
+	else
+		cfe_dbg("%s: Unable to find CSI2:x -> FE:0 link\n", __func__);
+
+	return 0;
+}
+
+static const struct media_device_ops cfe_media_device_ops = {
+	.link_notify = cfe_video_link_notify,
+};
+
+static void cfe_release(struct kref *kref)
+{
+	struct cfe_device *cfe = container_of(kref, struct cfe_device, kref);
+
+	media_device_cleanup(&cfe->mdev);
+
+	kfree(cfe);
+}
+
+static void cfe_put(struct cfe_device *cfe)
+{
+	kref_put(&cfe->kref, cfe_release);
+}
+
+static void cfe_get(struct cfe_device *cfe)
+{
+	kref_get(&cfe->kref);
+}
+
+static void cfe_node_release(struct video_device *vdev)
+{
+	struct cfe_node *node = video_get_drvdata(vdev);
+
+	cfe_put(node->cfe);
+}
+
+static int cfe_register_node(struct cfe_device *cfe, int id)
+{
+	struct video_device *vdev;
+	const struct cfe_fmt *fmt;
+	struct vb2_queue *q;
+	struct cfe_node *node = &cfe->node[id];
+	int ret;
+
+	node->cfe = cfe;
+	node->id = id;
+
+	if (node_supports_image(node)) {
+		if (node_supports_image_output(node))
+			node->vid_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		else
+			node->vid_fmt.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+
+		fmt = find_format_by_code(cfe_default_format.code);
+		if (!fmt) {
+			cfe_err("Failed to find format code\n");
+			return -EINVAL;
+		}
+
+		node->vid_fmt.fmt.pix.pixelformat = fmt->fourcc;
+		v4l2_fill_pix_format(&node->vid_fmt.fmt.pix, &cfe_default_format);
+
+		ret = try_fmt_vid_cap(node, &node->vid_fmt);
+		if (ret)
+			return ret;
+	}
+
+	if (node_supports_meta(node)) {
+		if (node_supports_meta_output(node))
+			node->meta_fmt.type = V4L2_BUF_TYPE_META_CAPTURE;
+		else
+			node->meta_fmt.type = V4L2_BUF_TYPE_META_OUTPUT;
+
+		ret = try_fmt_meta(node, &node->meta_fmt);
+		if (ret)
+			return ret;
+	}
+
+	mutex_init(&node->lock);
+
+	q = &node->buffer_queue;
+	q->type = node_supports_image(node) ? node->vid_fmt.type :
+					      node->meta_fmt.type;
+	q->io_modes = VB2_MMAP | VB2_DMABUF;
+	q->drv_priv = node;
+	q->ops = &cfe_video_qops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->buf_struct_size = id == FE_CONFIG ? sizeof(struct cfe_config_buffer)
+					     : sizeof(struct cfe_buffer);
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock = &node->lock;
+	q->min_queued_buffers = 1;
+	q->dev = &cfe->pdev->dev;
+
+	ret = vb2_queue_init(q);
+	if (ret) {
+		cfe_err("vb2_queue_init() failed\n");
+		return ret;
+	}
+
+	INIT_LIST_HEAD(&node->dma_queue);
+
+	vdev = &node->video_dev;
+	vdev->release = cfe_node_release;
+	vdev->fops = &cfe_fops;
+	vdev->ioctl_ops = &cfe_ioctl_ops;
+	vdev->v4l2_dev = &cfe->v4l2_dev;
+	if ((node_supports_image_output(node) ||
+	     node_supports_meta_output(node))) {
+		vdev->entity.ops = &cfe_media_entity_ops_capture;
+		vdev->vfl_dir = VFL_DIR_RX;
+	} else {
+		vdev->entity.ops = &cfe_media_entity_ops_output;
+		vdev->vfl_dir = VFL_DIR_TX;
+	}
+	vdev->queue = q;
+	vdev->lock = &node->lock;
+	vdev->device_caps = node_desc[id].caps;
+	vdev->device_caps |= V4L2_CAP_STREAMING | V4L2_CAP_IO_MC;
+
+	/* Define the device names */
+	snprintf(vdev->name, sizeof(vdev->name), "%s-%s", CFE_MODULE_NAME,
+		 node_desc[id].name);
+
+	video_set_drvdata(vdev, node);
+	if (node->id == FE_OUT0)
+		vdev->entity.flags |= MEDIA_ENT_FL_DEFAULT;
+	node->pad.flags = node_desc[id].pad_flags;
+	media_entity_pads_init(&vdev->entity, 1, &node->pad);
+
+	if (!node_supports_image(node)) {
+		v4l2_disable_ioctl(&node->video_dev,
+				   VIDIOC_ENUM_FRAMEINTERVALS);
+		v4l2_disable_ioctl(&node->video_dev,
+				   VIDIOC_ENUM_FRAMESIZES);
+	}
+
+	ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
+	if (ret) {
+		cfe_err("Unable to register video device %s\n", vdev->name);
+		return ret;
+	}
+
+	cfe_info("Registered [%s] node id %d successfully as /dev/video%u\n",
+		 vdev->name, id, vdev->num);
+
+	/*
+	 * Acquire a reference to cfe, which will be released when the video
+	 * device will be unregistered and userspace will have closed all open
+	 * file handles.
+	 */
+	cfe_get(cfe);
+	set_state(cfe, NODE_REGISTERED, id);
+
+	return 0;
+}
+
+static void cfe_unregister_nodes(struct cfe_device *cfe)
+{
+	unsigned int i;
+
+	for (i = 0; i < NUM_NODES; i++) {
+		struct cfe_node *node = &cfe->node[i];
+
+		if (check_state(cfe, NODE_REGISTERED, i)) {
+			clear_state(cfe, NODE_REGISTERED, i);
+			video_unregister_device(&node->video_dev);
+		}
+	}
+}
+
+static int cfe_link_node_pads(struct cfe_device *cfe)
+{
+	unsigned int i, source_pad = 0;
+	int ret;
+
+	for (i = 0; i < CSI2_NUM_CHANNELS; i++) {
+		struct cfe_node *node = &cfe->node[i];
+
+		if (!check_state(cfe, NODE_REGISTERED, i))
+			continue;
+
+		/* Find next source pad */
+		while (source_pad < cfe->sensor->entity.num_pads &&
+		       !(cfe->sensor->entity.pads[source_pad].flags &
+							MEDIA_PAD_FL_SOURCE))
+			source_pad++;
+
+		if (source_pad < cfe->sensor->entity.num_pads) {
+			/* Sensor -> CSI2 */
+			ret = media_create_pad_link(&cfe->sensor->entity, source_pad,
+						    &cfe->csi2.sd.entity, i,
+						    MEDIA_LNK_FL_IMMUTABLE |
+						    MEDIA_LNK_FL_ENABLED);
+			if (ret)
+				return ret;
+
+			/* Dealt with that source_pad, look at the next one next time */
+			source_pad++;
+		}
+
+		/* CSI2 channel # -> /dev/video# */
+		ret = media_create_pad_link(&cfe->csi2.sd.entity,
+					    node_desc[i].link_pad,
+					    &node->video_dev.entity, 0, 0);
+		if (ret)
+			return ret;
+
+		if (node_supports_image(node)) {
+			/* CSI2 channel # -> FE Input */
+			ret = media_create_pad_link(&cfe->csi2.sd.entity,
+						    node_desc[i].link_pad,
+						    &cfe->fe.sd.entity,
+						    FE_STREAM_PAD, 0);
+			if (ret)
+				return ret;
+		}
+	}
+
+	for (; i < NUM_NODES; i++) {
+		struct cfe_node *node = &cfe->node[i];
+		struct media_entity *src, *dst;
+		unsigned int src_pad, dst_pad;
+
+		if (node_desc[i].pad_flags & MEDIA_PAD_FL_SINK) {
+			/* FE -> /dev/video# */
+			src = &cfe->fe.sd.entity;
+			src_pad = node_desc[i].link_pad;
+			dst = &node->video_dev.entity;
+			dst_pad = 0;
+		} else {
+			/* /dev/video# -> FE */
+			dst = &cfe->fe.sd.entity;
+			dst_pad = node_desc[i].link_pad;
+			src = &node->video_dev.entity;
+			src_pad = 0;
+		}
+
+		ret = media_create_pad_link(src, src_pad, dst, dst_pad, 0);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int cfe_probe_complete(struct cfe_device *cfe)
+{
+	unsigned int i;
+	int ret;
+
+	cfe->v4l2_dev.notify = cfe_notify;
+
+	for (i = 0; i < NUM_NODES; i++) {
+		ret = cfe_register_node(cfe, i);
+		if (ret) {
+			cfe_err("Unable to register video node %u.\n", i);
+			goto unregister;
+		}
+	}
+
+	ret = cfe_link_node_pads(cfe);
+	if (ret) {
+		cfe_err("Unable to link node pads.\n");
+		goto unregister;
+	}
+
+	ret = v4l2_device_register_subdev_nodes(&cfe->v4l2_dev);
+	if (ret) {
+		cfe_err("Unable to register subdev nodes.\n");
+		goto unregister;
+	}
+
+	return 0;
+
+unregister:
+	cfe_unregister_nodes(cfe);
+	return ret;
+}
+
+static int cfe_async_bound(struct v4l2_async_notifier *notifier,
+			   struct v4l2_subdev *subdev,
+			   struct v4l2_async_connection *asd)
+{
+	struct cfe_device *cfe = to_cfe_device(notifier->v4l2_dev);
+
+	if (cfe->sensor) {
+		cfe_info("Rejecting subdev %s (Already set!!)", subdev->name);
+		return 0;
+	}
+
+	cfe->sensor = subdev;
+	cfe_info("Using sensor %s for capture\n", subdev->name);
+
+	return 0;
+}
+
+static int cfe_async_complete(struct v4l2_async_notifier *notifier)
+{
+	struct cfe_device *cfe = to_cfe_device(notifier->v4l2_dev);
+
+	return cfe_probe_complete(cfe);
+}
+
+static const struct v4l2_async_notifier_operations cfe_async_ops = {
+	.bound = cfe_async_bound,
+	.complete = cfe_async_complete,
+};
+
+static int of_cfe_connect_subdevs(struct cfe_device *cfe)
+{
+	struct platform_device *pdev = cfe->pdev;
+	struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
+	struct device_node *node = pdev->dev.of_node;
+	struct device_node *ep_node;
+	struct device_node *sensor_node;
+	unsigned int lane;
+	int ret = -EINVAL;
+
+	/* Get the local endpoint and remote device. */
+	ep_node = of_graph_get_next_endpoint(node, NULL);
+	if (!ep_node) {
+		cfe_err("can't get next endpoint\n");
+		return -EINVAL;
+	}
+
+	cfe_dbg("ep_node is %pOF\n", ep_node);
+
+	sensor_node = of_graph_get_remote_port_parent(ep_node);
+	if (!sensor_node) {
+		cfe_err("can't get remote parent\n");
+		goto cleanup_exit;
+	}
+
+	cfe_info("found subdevice %pOF\n", sensor_node);
+
+	/* Parse the local endpoint and validate its configuration. */
+	v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep_node), &ep);
+
+	cfe->csi2.multipacket_line =
+		fwnode_property_present(of_fwnode_handle(ep_node),
+					"multipacket-line");
+
+	if (ep.bus_type != V4L2_MBUS_CSI2_DPHY) {
+		cfe_err("endpoint node type != CSI2\n");
+		return -EINVAL;
+	}
+
+	for (lane = 0; lane < ep.bus.mipi_csi2.num_data_lanes; lane++) {
+		if (ep.bus.mipi_csi2.data_lanes[lane] != lane + 1) {
+			cfe_err("subdevice %pOF: data lanes reordering not supported\n",
+				sensor_node);
+			goto cleanup_exit;
+		}
+	}
+
+	cfe->csi2.dphy.max_lanes = ep.bus.mipi_csi2.num_data_lanes;
+	cfe->csi2.bus_flags = ep.bus.mipi_csi2.flags;
+
+	cfe_dbg("subdevice %pOF: %u data lanes, flags=0x%08x, multipacket_line=%u\n",
+		sensor_node, cfe->csi2.dphy.max_lanes, cfe->csi2.bus_flags,
+		cfe->csi2.multipacket_line);
+
+	/* Initialize and register the async notifier. */
+	v4l2_async_nf_init(&cfe->notifier, &cfe->v4l2_dev);
+	cfe->notifier.ops = &cfe_async_ops;
+
+	cfe->asd = v4l2_async_nf_add_fwnode(&cfe->notifier,
+					    of_fwnode_handle(sensor_node),
+					    struct v4l2_async_connection);
+	if (IS_ERR(cfe->asd)) {
+		cfe_err("Error adding subdevice: %d\n", ret);
+		goto cleanup_exit;
+	}
+
+	ret = v4l2_async_nf_register(&cfe->notifier);
+	if (ret) {
+		cfe_err("Error registering async notifier: %d\n", ret);
+		ret = -EINVAL;
+	}
+
+cleanup_exit:
+	of_node_put(sensor_node);
+	of_node_put(ep_node);
+
+	return ret;
+}
+
+static int cfe_probe(struct platform_device *pdev)
+{
+	struct cfe_device *cfe;
+	char debugfs_name[32];
+	int ret;
+
+	cfe = kzalloc(sizeof(*cfe), GFP_KERNEL);
+	if (!cfe)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, cfe);
+
+	kref_init(&cfe->kref);
+	cfe->pdev = pdev;
+	cfe->fe_csi2_channel = -1;
+	spin_lock_init(&cfe->state_lock);
+
+	cfe->csi2.base = devm_platform_ioremap_resource(pdev, 0);
+	if (IS_ERR(cfe->csi2.base)) {
+		dev_err(&pdev->dev, "Failed to get dma io block\n");
+		ret = PTR_ERR(cfe->csi2.base);
+		goto err_cfe_put;
+	}
+
+	cfe->csi2.dphy.base = devm_platform_ioremap_resource(pdev, 1);
+	if (IS_ERR(cfe->csi2.dphy.base)) {
+		dev_err(&pdev->dev, "Failed to get host io block\n");
+		ret = PTR_ERR(cfe->csi2.dphy.base);
+		goto err_cfe_put;
+	}
+
+	cfe->mipi_cfg_base = devm_platform_ioremap_resource(pdev, 2);
+	if (IS_ERR(cfe->mipi_cfg_base)) {
+		dev_err(&pdev->dev, "Failed to get mipi cfg io block\n");
+		ret = PTR_ERR(cfe->mipi_cfg_base);
+		goto err_cfe_put;
+	}
+
+	cfe->fe.base = devm_platform_ioremap_resource(pdev, 3);
+	if (IS_ERR(cfe->fe.base)) {
+		dev_err(&pdev->dev, "Failed to get pisp fe io block\n");
+		ret = PTR_ERR(cfe->fe.base);
+		goto err_cfe_put;
+	}
+
+	ret = platform_get_irq(pdev, 0);
+	if (ret <= 0) {
+		dev_err(&pdev->dev, "No IRQ resource\n");
+		ret = -EINVAL;
+		goto err_cfe_put;
+	}
+
+	ret = devm_request_irq(&pdev->dev, ret, cfe_isr, 0, "rp1-cfe", cfe);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to request interrupt\n");
+		ret = -EINVAL;
+		goto err_cfe_put;
+	}
+
+	ret = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(64));
+	if (ret) {
+		dev_err(&pdev->dev, "DMA enable failed\n");
+		goto err_cfe_put;
+	}
+
+	/* TODO: Enable clock only when running. */
+	cfe->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(cfe->clk))
+		return dev_err_probe(&pdev->dev, PTR_ERR(cfe->clk),
+				     "clock not found\n");
+
+	cfe->mdev.dev = &pdev->dev;
+	cfe->mdev.ops = &cfe_media_device_ops;
+	strscpy(cfe->mdev.model, CFE_MODULE_NAME, sizeof(cfe->mdev.model));
+	strscpy(cfe->mdev.serial, "", sizeof(cfe->mdev.serial));
+	snprintf(cfe->mdev.bus_info, sizeof(cfe->mdev.bus_info), "platform:%s",
+		 dev_name(&pdev->dev));
+
+	media_device_init(&cfe->mdev);
+
+	cfe->v4l2_dev.mdev = &cfe->mdev;
+
+	ret = v4l2_device_register(&pdev->dev, &cfe->v4l2_dev);
+	if (ret) {
+		cfe_err("Unable to register v4l2 device.\n");
+		goto err_cfe_put;
+	}
+
+	snprintf(debugfs_name, sizeof(debugfs_name), "rp1-cfe:%s",
+		 dev_name(&pdev->dev));
+	cfe->debugfs = debugfs_create_dir(debugfs_name, NULL);
+	debugfs_create_file("format", 0444, cfe->debugfs, cfe, &format_fops);
+	debugfs_create_file("regs", 0444, cfe->debugfs, cfe,
+			    &mipi_cfg_regs_fops);
+
+	/* Enable the block power domain */
+	pm_runtime_enable(&pdev->dev);
+
+	ret = pm_runtime_resume_and_get(&cfe->pdev->dev);
+	if (ret)
+		goto err_runtime_disable;
+
+	cfe->csi2.v4l2_dev = &cfe->v4l2_dev;
+	ret = csi2_init(&cfe->csi2, cfe->debugfs);
+	if (ret) {
+		cfe_err("Failed to init csi2 (%d)\n", ret);
+		goto err_runtime_put;
+	}
+
+	cfe->fe.v4l2_dev = &cfe->v4l2_dev;
+	ret = pisp_fe_init(&cfe->fe, cfe->debugfs);
+	if (ret) {
+		cfe_err("Failed to init pisp fe (%d)\n", ret);
+		goto err_csi2_uninit;
+	}
+
+	cfe->mdev.hw_revision = cfe->fe.hw_revision;
+	ret = media_device_register(&cfe->mdev);
+	if (ret < 0) {
+		cfe_err("Unable to register media-controller device.\n");
+		goto err_pisp_fe_uninit;
+	}
+
+	ret = of_cfe_connect_subdevs(cfe);
+	if (ret) {
+		cfe_err("Failed to connect subdevs\n");
+		goto err_media_unregister;
+	}
+
+	pm_runtime_put(&cfe->pdev->dev);
+
+	return 0;
+
+err_media_unregister:
+	media_device_unregister(&cfe->mdev);
+err_pisp_fe_uninit:
+	pisp_fe_uninit(&cfe->fe);
+err_csi2_uninit:
+	csi2_uninit(&cfe->csi2);
+err_runtime_put:
+	pm_runtime_put(&cfe->pdev->dev);
+err_runtime_disable:
+	pm_runtime_disable(&pdev->dev);
+	debugfs_remove(cfe->debugfs);
+	v4l2_device_unregister(&cfe->v4l2_dev);
+err_cfe_put:
+	cfe_put(cfe);
+
+	return ret;
+}
+
+static void cfe_remove(struct platform_device *pdev)
+{
+	struct cfe_device *cfe = platform_get_drvdata(pdev);
+
+	debugfs_remove(cfe->debugfs);
+
+	v4l2_async_nf_unregister(&cfe->notifier);
+	media_device_unregister(&cfe->mdev);
+	cfe_unregister_nodes(cfe);
+
+	pisp_fe_uninit(&cfe->fe);
+	csi2_uninit(&cfe->csi2);
+
+	pm_runtime_disable(&pdev->dev);
+
+	v4l2_device_unregister(&cfe->v4l2_dev);
+
+	cfe_put(cfe);
+}
+
+static int cfe_runtime_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct cfe_device *cfe = platform_get_drvdata(pdev);
+
+	clk_disable_unprepare(cfe->clk);
+
+	return 0;
+}
+
+static int cfe_runtime_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct cfe_device *cfe = platform_get_drvdata(pdev);
+	int ret;
+
+	ret = clk_prepare_enable(cfe->clk);
+	if (ret) {
+		dev_err(dev, "Unable to enable clock\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static const struct dev_pm_ops cfe_pm_ops = {
+	SET_RUNTIME_PM_OPS(cfe_runtime_suspend, cfe_runtime_resume, NULL)
+	SET_LATE_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend, pm_runtime_force_resume)
+};
+
+static const struct of_device_id cfe_of_match[] = {
+	{ .compatible = "raspberrypi,rp1-cfe" },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, cfe_of_match);
+
+static struct platform_driver cfe_driver = {
+	.probe		= cfe_probe,
+	.remove		= cfe_remove,
+	.driver = {
+		.name	= CFE_MODULE_NAME,
+		.of_match_table = cfe_of_match,
+		.pm = &cfe_pm_ops,
+	},
+};
+
+module_platform_driver(cfe_driver);
+
+MODULE_AUTHOR("Naushir Patuck <naush@raspberrypi.com>");
+MODULE_DESCRIPTION("RP1 Camera Front End driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(CFE_VERSION);
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1_cfe/cfe.h
@@ -0,0 +1,43 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * RP1 CFE driver.
+ * Copyright (c) 2021 Raspberry Pi Ltd.
+ *
+ */
+#ifndef _RP1_CFE_
+#define _RP1_CFE_
+
+#include <linux/types.h>
+#include <linux/media-bus-format.h>
+#include <linux/videodev2.h>
+
+extern bool cfe_debug_verbose;
+
+enum cfe_remap_types {
+	CFE_REMAP_16BIT,
+	CFE_REMAP_COMPRESSED,
+	CFE_NUM_REMAP,
+};
+
+#define CFE_FORMAT_FLAG_META_OUT	BIT(0)
+#define CFE_FORMAT_FLAG_META_CAP	BIT(1)
+#define CFE_FORMAT_FLAG_FE_OUT		BIT(2)
+
+struct cfe_fmt {
+	u32 fourcc;
+	u32 code;
+	u8 depth;
+	u8 csi_dt;
+	u32 remap[CFE_NUM_REMAP];
+	u32 flags;
+};
+
+extern const struct v4l2_mbus_framefmt cfe_default_format;
+extern const struct v4l2_mbus_framefmt cfe_default_meta_format;
+
+const struct cfe_fmt *find_format_by_code(u32 code);
+const struct cfe_fmt *find_format_by_pix(u32 pixelformat);
+u32 cfe_find_16bit_code(u32 code);
+u32 cfe_find_compressed_code(u32 code);
+
+#endif
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1_cfe/cfe_fmts.h
@@ -0,0 +1,318 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * RP1 Camera Front End formats definition
+ *
+ * Copyright (C) 2021 - Raspberry Pi Ltd.
+ *
+ */
+#ifndef _CFE_FMTS_H_
+#define _CFE_FMTS_H_
+
+#include "cfe.h"
+#include <media/mipi-csi2.h>
+
+static const struct cfe_fmt formats[] = {
+	/* YUV Formats */
+	{
+		.fourcc = V4L2_PIX_FMT_YUYV,
+		.code = MEDIA_BUS_FMT_YUYV8_1X16,
+		.depth = 16,
+		.csi_dt = MIPI_CSI2_DT_YUV422_8B,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_UYVY,
+		.code = MEDIA_BUS_FMT_UYVY8_1X16,
+		.depth = 16,
+		.csi_dt = MIPI_CSI2_DT_YUV422_8B,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_YVYU,
+		.code = MEDIA_BUS_FMT_YVYU8_1X16,
+		.depth = 16,
+		.csi_dt = MIPI_CSI2_DT_YUV422_8B,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_VYUY,
+		.code = MEDIA_BUS_FMT_VYUY8_1X16,
+		.depth = 16,
+		.csi_dt = MIPI_CSI2_DT_YUV422_8B,
+	},
+	{
+		/* RGB Formats */
+		.fourcc = V4L2_PIX_FMT_RGB565, /* gggbbbbb rrrrrggg */
+		.code = MEDIA_BUS_FMT_RGB565_2X8_LE,
+		.depth = 16,
+		.csi_dt = MIPI_CSI2_DT_RGB565,
+	},
+	{	.fourcc = V4L2_PIX_FMT_RGB565X, /* rrrrrggg gggbbbbb */
+		.code = MEDIA_BUS_FMT_RGB565_2X8_BE,
+		.depth = 16,
+		.csi_dt = MIPI_CSI2_DT_RGB565,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_RGB555, /* gggbbbbb arrrrrgg */
+		.code = MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE,
+		.depth = 16,
+		.csi_dt = MIPI_CSI2_DT_RGB555,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_RGB555X, /* arrrrrgg gggbbbbb */
+		.code = MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE,
+		.depth = 16,
+		.csi_dt = MIPI_CSI2_DT_RGB555,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_RGB24, /* rgb */
+		.code = MEDIA_BUS_FMT_RGB888_1X24,
+		.depth = 24,
+		.csi_dt = MIPI_CSI2_DT_RGB888,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_BGR24, /* bgr */
+		.code = MEDIA_BUS_FMT_BGR888_1X24,
+		.depth = 24,
+		.csi_dt = MIPI_CSI2_DT_RGB888,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_RGB32, /* argb */
+		.code = MEDIA_BUS_FMT_ARGB8888_1X32,
+		.depth = 32,
+		.csi_dt = 0x0,
+	},
+
+	/* Bayer Formats */
+	{
+		.fourcc = V4L2_PIX_FMT_SBGGR8,
+		.code = MEDIA_BUS_FMT_SBGGR8_1X8,
+		.depth = 8,
+		.csi_dt = MIPI_CSI2_DT_RAW8,
+		.remap = { V4L2_PIX_FMT_SBGGR16, V4L2_PIX_FMT_PISP_COMP1_BGGR },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SGBRG8,
+		.code = MEDIA_BUS_FMT_SGBRG8_1X8,
+		.depth = 8,
+		.csi_dt = MIPI_CSI2_DT_RAW8,
+		.remap = { V4L2_PIX_FMT_SGBRG16, V4L2_PIX_FMT_PISP_COMP1_GBRG },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SGRBG8,
+		.code = MEDIA_BUS_FMT_SGRBG8_1X8,
+		.depth = 8,
+		.csi_dt = MIPI_CSI2_DT_RAW8,
+		.remap = { V4L2_PIX_FMT_SGRBG16, V4L2_PIX_FMT_PISP_COMP1_GRBG },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SRGGB8,
+		.code = MEDIA_BUS_FMT_SRGGB8_1X8,
+		.depth = 8,
+		.csi_dt = MIPI_CSI2_DT_RAW8,
+		.remap = { V4L2_PIX_FMT_SRGGB16, V4L2_PIX_FMT_PISP_COMP1_RGGB },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SBGGR10P,
+		.code = MEDIA_BUS_FMT_SBGGR10_1X10,
+		.depth = 10,
+		.csi_dt = MIPI_CSI2_DT_RAW10,
+		.remap = { V4L2_PIX_FMT_SBGGR16, V4L2_PIX_FMT_PISP_COMP1_BGGR },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SGBRG10P,
+		.code = MEDIA_BUS_FMT_SGBRG10_1X10,
+		.depth = 10,
+		.csi_dt = MIPI_CSI2_DT_RAW10,
+		.remap = { V4L2_PIX_FMT_SGBRG16, V4L2_PIX_FMT_PISP_COMP1_GBRG },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SGRBG10P,
+		.code = MEDIA_BUS_FMT_SGRBG10_1X10,
+		.depth = 10,
+		.csi_dt = MIPI_CSI2_DT_RAW10,
+		.remap = { V4L2_PIX_FMT_SGRBG16, V4L2_PIX_FMT_PISP_COMP1_GRBG },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SRGGB10P,
+		.code = MEDIA_BUS_FMT_SRGGB10_1X10,
+		.depth = 10,
+		.csi_dt = MIPI_CSI2_DT_RAW10,
+		.remap = { V4L2_PIX_FMT_SRGGB16, V4L2_PIX_FMT_PISP_COMP1_RGGB },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SBGGR12P,
+		.code = MEDIA_BUS_FMT_SBGGR12_1X12,
+		.depth = 12,
+		.csi_dt = MIPI_CSI2_DT_RAW12,
+		.remap = { V4L2_PIX_FMT_SBGGR16, V4L2_PIX_FMT_PISP_COMP1_BGGR },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SGBRG12P,
+		.code = MEDIA_BUS_FMT_SGBRG12_1X12,
+		.depth = 12,
+		.csi_dt = MIPI_CSI2_DT_RAW12,
+		.remap = { V4L2_PIX_FMT_SGBRG16, V4L2_PIX_FMT_PISP_COMP1_GBRG },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SGRBG12P,
+		.code = MEDIA_BUS_FMT_SGRBG12_1X12,
+		.depth = 12,
+		.csi_dt = MIPI_CSI2_DT_RAW12,
+		.remap = { V4L2_PIX_FMT_SGRBG16, V4L2_PIX_FMT_PISP_COMP1_GRBG },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SRGGB12P,
+		.code = MEDIA_BUS_FMT_SRGGB12_1X12,
+		.depth = 12,
+		.csi_dt = MIPI_CSI2_DT_RAW12,
+		.remap = { V4L2_PIX_FMT_SRGGB16, V4L2_PIX_FMT_PISP_COMP1_RGGB },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SBGGR14P,
+		.code = MEDIA_BUS_FMT_SBGGR14_1X14,
+		.depth = 14,
+		.csi_dt = MIPI_CSI2_DT_RAW14,
+		.remap = { V4L2_PIX_FMT_SBGGR16, V4L2_PIX_FMT_PISP_COMP1_BGGR },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SGBRG14P,
+		.code = MEDIA_BUS_FMT_SGBRG14_1X14,
+		.depth = 14,
+		.csi_dt = MIPI_CSI2_DT_RAW14,
+		.remap = { V4L2_PIX_FMT_SGBRG16, V4L2_PIX_FMT_PISP_COMP1_GBRG },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SGRBG14P,
+		.code = MEDIA_BUS_FMT_SGRBG14_1X14,
+		.depth = 14,
+		.csi_dt = MIPI_CSI2_DT_RAW14,
+		.remap = { V4L2_PIX_FMT_SGRBG16, V4L2_PIX_FMT_PISP_COMP1_GRBG },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SRGGB14P,
+		.code = MEDIA_BUS_FMT_SRGGB14_1X14,
+		.depth = 14,
+		.csi_dt = MIPI_CSI2_DT_RAW14,
+		.remap = { V4L2_PIX_FMT_SRGGB16, V4L2_PIX_FMT_PISP_COMP1_RGGB },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SBGGR16,
+		.code = MEDIA_BUS_FMT_SBGGR16_1X16,
+		.depth = 16,
+		.csi_dt = 0, /* Avoid RP1 HW mismatch for 16-bit modes. */
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+		.remap = { V4L2_PIX_FMT_SBGGR16, V4L2_PIX_FMT_PISP_COMP1_BGGR },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SGBRG16,
+		.code = MEDIA_BUS_FMT_SGBRG16_1X16,
+		.depth = 16,
+		.csi_dt = 0, /* Avoid RP1 HW mismatch for 16-bit modes. */
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+		.remap = { V4L2_PIX_FMT_SGBRG16, V4L2_PIX_FMT_PISP_COMP1_GBRG },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SGRBG16,
+		.code = MEDIA_BUS_FMT_SGRBG16_1X16,
+		.depth = 16,
+		.csi_dt = 0, /* Avoid RP1 HW mismatch for 16-bit modes. */
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+		.remap = { V4L2_PIX_FMT_SGRBG16, V4L2_PIX_FMT_PISP_COMP1_GRBG },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SRGGB16,
+		.code = MEDIA_BUS_FMT_SRGGB16_1X16,
+		.depth = 16,
+		.csi_dt = 0, /* Avoid RP1 HW mismatch for 16-bit modes. */
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+		.remap = { V4L2_PIX_FMT_SRGGB16, V4L2_PIX_FMT_PISP_COMP1_RGGB },
+	},
+	/* PiSP Compressed Mode 1 */
+	{
+		.fourcc = V4L2_PIX_FMT_PISP_COMP1_RGGB,
+		.code = MEDIA_BUS_FMT_SRGGB16_1X16,
+		.depth = 8,
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_PISP_COMP1_BGGR,
+		.code = MEDIA_BUS_FMT_SBGGR16_1X16,
+		.depth = 8,
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_PISP_COMP1_GBRG,
+		.code = MEDIA_BUS_FMT_SGBRG16_1X16,
+		.depth = 8,
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_PISP_COMP1_GRBG,
+		.code = MEDIA_BUS_FMT_SGRBG16_1X16,
+		.depth = 8,
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+	},
+	/* Greyscale format */
+	{
+		.fourcc = V4L2_PIX_FMT_GREY,
+		.code = MEDIA_BUS_FMT_Y8_1X8,
+		.depth = 8,
+		.csi_dt = MIPI_CSI2_DT_RAW8,
+		.remap = { V4L2_PIX_FMT_Y16, V4L2_PIX_FMT_PISP_COMP1_MONO },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_Y10P,
+		.code = MEDIA_BUS_FMT_Y10_1X10,
+		.depth = 10,
+		.csi_dt = MIPI_CSI2_DT_RAW10,
+		.remap = { V4L2_PIX_FMT_Y16, V4L2_PIX_FMT_PISP_COMP1_MONO },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_Y12P,
+		.code = MEDIA_BUS_FMT_Y12_1X12,
+		.depth = 12,
+		.csi_dt = MIPI_CSI2_DT_RAW12,
+		.remap = { V4L2_PIX_FMT_Y16, V4L2_PIX_FMT_PISP_COMP1_MONO },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_Y14P,
+		.code = MEDIA_BUS_FMT_Y14_1X14,
+		.depth = 14,
+		.csi_dt = MIPI_CSI2_DT_RAW14,
+		.remap = { V4L2_PIX_FMT_Y16, V4L2_PIX_FMT_PISP_COMP1_MONO },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_Y16,
+		.code = MEDIA_BUS_FMT_Y16_1X16,
+		.depth = 16,
+		.csi_dt = MIPI_CSI2_DT_RAW16,
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+		.remap = { V4L2_PIX_FMT_Y16, V4L2_PIX_FMT_PISP_COMP1_MONO },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_PISP_COMP1_MONO,
+		.code = MEDIA_BUS_FMT_Y16_1X16,
+		.depth = 8,
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+	},
+	/* Embedded data format */
+	{
+		.fourcc = V4L2_META_FMT_SENSOR_DATA,
+		.code = MEDIA_BUS_FMT_SENSOR_DATA,
+		.depth = 8,
+		.csi_dt = MIPI_CSI2_DT_EMBEDDED_8B,
+		.flags = CFE_FORMAT_FLAG_META_CAP,
+	},
+
+	/* Frontend formats */
+	{
+		.fourcc = V4L2_META_FMT_RPI_FE_CFG,
+		.code = MEDIA_BUS_FMT_FIXED,
+		.flags = CFE_FORMAT_FLAG_META_OUT,
+	},
+	{
+		.fourcc = V4L2_META_FMT_RPI_FE_STATS,
+		.code = MEDIA_BUS_FMT_FIXED,
+		.flags = CFE_FORMAT_FLAG_META_CAP,
+	},
+};
+
+#endif /* _CFE_FMTS_H_ */
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1_cfe/csi2.c
@@ -0,0 +1,628 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * RP1 CSI-2 Driver
+ *
+ * Copyright (C) 2021 - Raspberry Pi Ltd.
+ *
+ */
+
+#include <linux/delay.h>
+#include <linux/moduleparam.h>
+#include <linux/pm_runtime.h>
+#include <linux/seq_file.h>
+
+#include <media/videobuf2-dma-contig.h>
+
+#include "csi2.h"
+#include "cfe.h"
+
+static bool csi2_track_errors;
+module_param_named(track_csi2_errors, csi2_track_errors, bool, 0);
+MODULE_PARM_DESC(track_csi2_errors, "track csi-2 errors");
+
+#define csi2_dbg_verbose(fmt, arg...)                             \
+	do {                                                      \
+		if (cfe_debug_verbose)                            \
+			dev_dbg(csi2->v4l2_dev->dev, fmt, ##arg); \
+	} while (0)
+#define csi2_dbg(fmt, arg...) dev_dbg(csi2->v4l2_dev->dev, fmt, ##arg)
+#define csi2_info(fmt, arg...) dev_info(csi2->v4l2_dev->dev, fmt, ##arg)
+#define csi2_err(fmt, arg...) dev_err(csi2->v4l2_dev->dev, fmt, ##arg)
+
+/* CSI2-DMA registers */
+#define CSI2_STATUS		0x000
+#define CSI2_QOS		0x004
+#define CSI2_DISCARDS_OVERFLOW	0x008
+#define CSI2_DISCARDS_INACTIVE	0x00c
+#define CSI2_DISCARDS_UNMATCHED	0x010
+#define CSI2_DISCARDS_LEN_LIMIT	0x014
+
+#define CSI2_DISCARDS_AMOUNT_SHIFT	0
+#define CSI2_DISCARDS_AMOUNT_MASK	GENMASK(23, 0)
+#define CSI2_DISCARDS_DT_SHIFT		24
+#define CSI2_DISCARDS_DT_MASK		GENMASK(29, 24)
+#define CSI2_DISCARDS_VC_SHIFT		30
+#define CSI2_DISCARDS_VC_MASK		GENMASK(31, 30)
+
+#define CSI2_LLEV_PANICS	0x018
+#define CSI2_ULEV_PANICS	0x01c
+#define CSI2_IRQ_MASK		0x020
+#define CSI2_IRQ_MASK_IRQ_OVERFLOW		BIT(0)
+#define CSI2_IRQ_MASK_IRQ_DISCARD_OVERFLOW	BIT(1)
+#define CSI2_IRQ_MASK_IRQ_DISCARD_LENGTH_LIMIT	BIT(2)
+#define CSI2_IRQ_MASK_IRQ_DISCARD_UNMATCHED	BIT(3)
+#define CSI2_IRQ_MASK_IRQ_DISCARD_INACTIVE	BIT(4)
+#define CSI2_IRQ_MASK_IRQ_ALL                                              \
+	(CSI2_IRQ_MASK_IRQ_OVERFLOW | CSI2_IRQ_MASK_IRQ_DISCARD_OVERFLOW | \
+	 CSI2_IRQ_MASK_IRQ_DISCARD_LENGTH_LIMIT |                          \
+	 CSI2_IRQ_MASK_IRQ_DISCARD_UNMATCHED |                             \
+	 CSI2_IRQ_MASK_IRQ_DISCARD_INACTIVE)
+
+#define CSI2_CTRL		0x024
+#define CSI2_CH_CTRL(x)		((x) * 0x40 + 0x28)
+#define CSI2_CH_ADDR0(x)	((x) * 0x40 + 0x2c)
+#define CSI2_CH_ADDR1(x)	((x) * 0x40 + 0x3c)
+#define CSI2_CH_STRIDE(x)	((x) * 0x40 + 0x30)
+#define CSI2_CH_LENGTH(x)	((x) * 0x40 + 0x34)
+#define CSI2_CH_DEBUG(x)	((x) * 0x40 + 0x38)
+#define CSI2_CH_FRAME_SIZE(x)	((x) * 0x40 + 0x40)
+#define CSI2_CH_COMP_CTRL(x)	((x) * 0x40 + 0x44)
+#define CSI2_CH_FE_FRAME_ID(x)	((x) * 0x40 + 0x48)
+
+/* CSI2_STATUS */
+#define IRQ_FS(x)		(BIT(0) << (x))
+#define IRQ_FE(x)		(BIT(4) << (x))
+#define IRQ_FE_ACK(x)		(BIT(8) << (x))
+#define IRQ_LE(x)		(BIT(12) << (x))
+#define IRQ_LE_ACK(x)		(BIT(16) << (x))
+#define IRQ_CH_MASK(x)		(IRQ_FS(x) | IRQ_FE(x) | IRQ_FE_ACK(x) | IRQ_LE(x) | IRQ_LE_ACK(x))
+#define IRQ_OVERFLOW		BIT(20)
+#define IRQ_DISCARD_OVERFLOW	BIT(21)
+#define IRQ_DISCARD_LEN_LIMIT	BIT(22)
+#define IRQ_DISCARD_UNMATCHED	BIT(23)
+#define IRQ_DISCARD_INACTIVE	BIT(24)
+
+/* CSI2_CTRL */
+#define EOP_IS_EOL		BIT(0)
+
+/* CSI2_CH_CTRL */
+#define DMA_EN			BIT(0)
+#define FORCE			BIT(3)
+#define AUTO_ARM		BIT(4)
+#define IRQ_EN_FS		BIT(13)
+#define IRQ_EN_FE		BIT(14)
+#define IRQ_EN_FE_ACK		BIT(15)
+#define IRQ_EN_LE		BIT(16)
+#define IRQ_EN_LE_ACK		BIT(17)
+#define FLUSH_FE		BIT(28)
+#define PACK_LINE		BIT(29)
+#define PACK_BYTES		BIT(30)
+#define CH_MODE_MASK		GENMASK(2, 1)
+#define VC_MASK			GENMASK(6, 5)
+#define DT_MASK			GENMASK(12, 7)
+#define LC_MASK			GENMASK(27, 18)
+
+/* CHx_COMPRESSION_CONTROL */
+#define COMP_OFFSET_MASK	GENMASK(15, 0)
+#define COMP_SHIFT_MASK		GENMASK(19, 16)
+#define COMP_MODE_MASK		GENMASK(25, 24)
+
+static inline u32 csi2_reg_read(struct csi2_device *csi2, u32 offset)
+{
+	return readl(csi2->base + offset);
+}
+
+static inline void csi2_reg_write(struct csi2_device *csi2, u32 offset, u32 val)
+{
+	writel(val, csi2->base + offset);
+	csi2_dbg_verbose("csi2: write 0x%04x -> 0x%03x\n", val, offset);
+}
+
+static inline void set_field(u32 *valp, u32 field, u32 mask)
+{
+	u32 val = *valp;
+
+	val &= ~mask;
+	val |= (field << __ffs(mask)) & mask;
+	*valp = val;
+}
+
+static int csi2_regs_show(struct seq_file *s, void *data)
+{
+	struct csi2_device *csi2 = s->private;
+	unsigned int i;
+	int ret;
+
+	ret = pm_runtime_resume_and_get(csi2->v4l2_dev->dev);
+	if (ret)
+		return ret;
+
+#define DUMP(reg) seq_printf(s, #reg " \t0x%08x\n", csi2_reg_read(csi2, reg))
+#define DUMP_CH(idx, reg) seq_printf(s, #reg "(%u) \t0x%08x\n", idx, csi2_reg_read(csi2, reg(idx)))
+
+	DUMP(CSI2_STATUS);
+	DUMP(CSI2_DISCARDS_OVERFLOW);
+	DUMP(CSI2_DISCARDS_INACTIVE);
+	DUMP(CSI2_DISCARDS_UNMATCHED);
+	DUMP(CSI2_DISCARDS_LEN_LIMIT);
+	DUMP(CSI2_LLEV_PANICS);
+	DUMP(CSI2_ULEV_PANICS);
+	DUMP(CSI2_IRQ_MASK);
+	DUMP(CSI2_CTRL);
+
+	for (i = 0; i < CSI2_NUM_CHANNELS; ++i) {
+		DUMP_CH(i, CSI2_CH_CTRL);
+		DUMP_CH(i, CSI2_CH_ADDR0);
+		DUMP_CH(i, CSI2_CH_ADDR1);
+		DUMP_CH(i, CSI2_CH_STRIDE);
+		DUMP_CH(i, CSI2_CH_LENGTH);
+		DUMP_CH(i, CSI2_CH_DEBUG);
+		DUMP_CH(i, CSI2_CH_FRAME_SIZE);
+		DUMP_CH(i, CSI2_CH_COMP_CTRL);
+		DUMP_CH(i, CSI2_CH_FE_FRAME_ID);
+	}
+
+#undef DUMP
+#undef DUMP_CH
+
+	pm_runtime_put(csi2->v4l2_dev->dev);
+
+	return 0;
+}
+
+DEFINE_SHOW_ATTRIBUTE(csi2_regs);
+
+static int csi2_errors_show(struct seq_file *s, void *data)
+{
+	struct csi2_device *csi2 = s->private;
+	unsigned long flags;
+	u32 discards_table[DISCARDS_TABLE_NUM_VCS][DISCARDS_TABLE_NUM_ENTRIES];
+	u32 discards_dt_table[DISCARDS_TABLE_NUM_ENTRIES];
+	u32 overflows;
+
+	spin_lock_irqsave(&csi2->errors_lock, flags);
+
+	memcpy(discards_table, csi2->discards_table, sizeof(discards_table));
+	memcpy(discards_dt_table, csi2->discards_dt_table,
+	       sizeof(discards_dt_table));
+	overflows = csi2->overflows;
+
+	csi2->overflows = 0;
+	memset(csi2->discards_table, 0, sizeof(discards_table));
+	memset(csi2->discards_dt_table, 0, sizeof(discards_dt_table));
+
+	spin_unlock_irqrestore(&csi2->errors_lock, flags);
+
+	seq_printf(s, "Overflows %u\n", overflows);
+	seq_puts(s, "Discards:\n");
+	seq_puts(s, "VC            OVLF        LEN  UNMATCHED   INACTIVE\n");
+
+	for (unsigned int vc = 0; vc < DISCARDS_TABLE_NUM_VCS; ++vc) {
+		seq_printf(s, "%u       %10u %10u %10u %10u\n", vc,
+			   discards_table[vc][DISCARDS_TABLE_OVERFLOW],
+			   discards_table[vc][DISCARDS_TABLE_LENGTH_LIMIT],
+			   discards_table[vc][DISCARDS_TABLE_UNMATCHED],
+			   discards_table[vc][DISCARDS_TABLE_INACTIVE]);
+	}
+
+	seq_printf(s, "Last DT %10u %10u %10u %10u\n",
+		   discards_dt_table[DISCARDS_TABLE_OVERFLOW],
+		   discards_dt_table[DISCARDS_TABLE_LENGTH_LIMIT],
+		   discards_dt_table[DISCARDS_TABLE_UNMATCHED],
+		   discards_dt_table[DISCARDS_TABLE_INACTIVE]);
+
+	return 0;
+}
+
+DEFINE_SHOW_ATTRIBUTE(csi2_errors);
+
+static void csi2_isr_handle_errors(struct csi2_device *csi2, u32 status)
+{
+	spin_lock(&csi2->errors_lock);
+
+	if (status & IRQ_OVERFLOW)
+		csi2->overflows++;
+
+	for (unsigned int i = 0; i < DISCARDS_TABLE_NUM_ENTRIES; ++i) {
+		static const u32 discard_bits[] = {
+			IRQ_DISCARD_OVERFLOW,
+			IRQ_DISCARD_LEN_LIMIT,
+			IRQ_DISCARD_UNMATCHED,
+			IRQ_DISCARD_INACTIVE,
+		};
+		static const u8 discard_regs[] = {
+			CSI2_DISCARDS_OVERFLOW,
+			CSI2_DISCARDS_LEN_LIMIT,
+			CSI2_DISCARDS_UNMATCHED,
+			CSI2_DISCARDS_INACTIVE,
+		};
+		u32 amount;
+		u8 dt, vc;
+		u32 v;
+
+		if (!(status & discard_bits[i]))
+			continue;
+
+		v = csi2_reg_read(csi2, discard_regs[i]);
+		csi2_reg_write(csi2, discard_regs[i], 0);
+
+		amount = (v & CSI2_DISCARDS_AMOUNT_MASK) >>
+			 CSI2_DISCARDS_AMOUNT_SHIFT;
+		dt = (v & CSI2_DISCARDS_DT_MASK) >> CSI2_DISCARDS_DT_SHIFT;
+		vc = (v & CSI2_DISCARDS_VC_MASK) >> CSI2_DISCARDS_VC_SHIFT;
+
+		csi2->discards_table[vc][i] += amount;
+		csi2->discards_dt_table[i] = dt;
+	}
+
+	spin_unlock(&csi2->errors_lock);
+}
+
+void csi2_isr(struct csi2_device *csi2, bool *sof, bool *eof)
+{
+	unsigned int i;
+	u32 status;
+
+	status = csi2_reg_read(csi2, CSI2_STATUS);
+	csi2_dbg_verbose("ISR: STA: 0x%x\n", status);
+
+	/* Write value back to clear the interrupts */
+	csi2_reg_write(csi2, CSI2_STATUS, status);
+
+	for (i = 0; i < CSI2_NUM_CHANNELS; i++) {
+		u32 dbg;
+
+		if ((status & IRQ_CH_MASK(i)) == 0)
+			continue;
+
+		dbg = csi2_reg_read(csi2, CSI2_CH_DEBUG(i));
+
+		csi2_dbg_verbose("ISR: [%u], %s%s%s%s%s frame: %u line: %u\n",
+				 i, (status & IRQ_FS(i)) ? "FS " : "",
+				 (status & IRQ_FE(i)) ? "FE " : "",
+				 (status & IRQ_FE_ACK(i)) ? "FE_ACK " : "",
+				 (status & IRQ_LE(i)) ? "LE " : "",
+				 (status & IRQ_LE_ACK(i)) ? "LE_ACK " : "",
+				 dbg >> 16,
+				 csi2->num_lines[i] ?
+					 ((dbg & 0xffff) % csi2->num_lines[i]) :
+					 0);
+
+		sof[i] = !!(status & IRQ_FS(i));
+		eof[i] = !!(status & IRQ_FE_ACK(i));
+	}
+
+	if (csi2_track_errors)
+		csi2_isr_handle_errors(csi2, status);
+}
+
+void csi2_set_buffer(struct csi2_device *csi2, unsigned int channel,
+		     dma_addr_t dmaaddr, unsigned int stride, unsigned int size)
+{
+	u64 addr = dmaaddr;
+	/*
+	 * ADDRESS0 must be written last as it triggers the double buffering
+	 * mechanism for all buffer registers within the hardware.
+	 */
+	addr >>= 4;
+	csi2_reg_write(csi2, CSI2_CH_LENGTH(channel), size >> 4);
+	csi2_reg_write(csi2, CSI2_CH_STRIDE(channel), stride >> 4);
+	csi2_reg_write(csi2, CSI2_CH_ADDR1(channel), addr >> 32);
+	csi2_reg_write(csi2, CSI2_CH_ADDR0(channel), addr & 0xffffffff);
+}
+
+void csi2_set_compression(struct csi2_device *csi2, unsigned int channel,
+			  enum csi2_compression_mode mode, unsigned int shift,
+			  unsigned int offset)
+{
+	u32 compression = 0;
+
+	set_field(&compression, COMP_OFFSET_MASK, offset);
+	set_field(&compression, COMP_SHIFT_MASK, shift);
+	set_field(&compression, COMP_MODE_MASK, mode);
+	csi2_reg_write(csi2, CSI2_CH_COMP_CTRL(channel), compression);
+}
+
+static int csi2_get_vc_dt_fallback(struct csi2_device *csi2,
+				   unsigned int channel, u8 *vc, u8 *dt)
+{
+	struct v4l2_subdev *sd = &csi2->sd;
+	struct v4l2_subdev_state *state;
+	struct v4l2_mbus_framefmt *fmt;
+	const struct cfe_fmt *cfe_fmt;
+
+	state = v4l2_subdev_get_locked_active_state(sd);
+
+	/* Without Streams API, the channel number matches the sink pad */
+	fmt = v4l2_subdev_state_get_format(state, channel);
+	if (!fmt)
+		return -EINVAL;
+
+	cfe_fmt = find_format_by_code(fmt->code);
+	if (!cfe_fmt)
+		return -EINVAL;
+
+	*vc = 0;
+	*dt = cfe_fmt->csi_dt;
+
+	return 0;
+}
+
+static int csi2_get_vc_dt(struct csi2_device *csi2, unsigned int channel,
+			  u8 *vc, u8 *dt)
+{
+	struct v4l2_mbus_frame_desc remote_desc;
+	const struct media_pad *remote_pad;
+	struct v4l2_subdev *source_sd;
+	int ret;
+
+	/* Without Streams API, the channel number matches the sink pad */
+	remote_pad = media_pad_remote_pad_first(&csi2->pad[channel]);
+	if (!remote_pad)
+		return -EPIPE;
+
+	source_sd = media_entity_to_v4l2_subdev(remote_pad->entity);
+
+	ret = v4l2_subdev_call(source_sd, pad, get_frame_desc,
+			       remote_pad->index, &remote_desc);
+	if (ret == -ENOIOCTLCMD) {
+		csi2_dbg("source does not support get_frame_desc, use fallback\n");
+		return csi2_get_vc_dt_fallback(csi2, channel, vc, dt);
+	} else if (ret) {
+		csi2_err("Failed to get frame descriptor\n");
+		return ret;
+	}
+
+	if (remote_desc.type != V4L2_MBUS_FRAME_DESC_TYPE_CSI2) {
+		csi2_err("Frame descriptor does not describe CSI-2 link");
+		return -EINVAL;
+	}
+
+	if (remote_desc.num_entries != 1) {
+		csi2_err("Frame descriptor does not have a single entry");
+		return -EINVAL;
+	}
+
+	*vc = remote_desc.entry[0].bus.csi2.vc;
+	*dt = remote_desc.entry[0].bus.csi2.dt;
+
+	return 0;
+}
+
+void csi2_start_channel(struct csi2_device *csi2, unsigned int channel,
+			enum csi2_mode mode, bool auto_arm,
+			bool pack_bytes, unsigned int width,
+			unsigned int height)
+{
+	u32 ctrl;
+	int ret;
+	u8 vc, dt;
+
+	ret = csi2_get_vc_dt(csi2, channel, &vc, &dt);
+	if (ret)
+		return;
+
+	csi2_dbg("%s [%u]\n", __func__, channel);
+
+	csi2_reg_write(csi2, CSI2_CH_CTRL(channel), 0);
+	csi2_reg_write(csi2, CSI2_CH_DEBUG(channel), 0);
+	csi2_reg_write(csi2, CSI2_STATUS, IRQ_CH_MASK(channel));
+
+	/* Enable channel and FS/FE interrupts. */
+	ctrl = DMA_EN | IRQ_EN_FS | IRQ_EN_FE_ACK | PACK_LINE;
+	/* PACK_BYTES ensures no striding for embedded data. */
+	if (pack_bytes)
+		ctrl |= PACK_BYTES;
+
+	if (auto_arm)
+		ctrl |= AUTO_ARM;
+
+	if (width && height) {
+		set_field(&ctrl, mode, CH_MODE_MASK);
+		csi2_reg_write(csi2, CSI2_CH_FRAME_SIZE(channel),
+			       (height << 16) | width);
+	} else {
+		set_field(&ctrl, 0x0, CH_MODE_MASK);
+		csi2_reg_write(csi2, CSI2_CH_FRAME_SIZE(channel), 0);
+	}
+
+	set_field(&ctrl, vc, VC_MASK);
+	set_field(&ctrl, dt, DT_MASK);
+	csi2_reg_write(csi2, CSI2_CH_CTRL(channel), ctrl);
+	csi2->num_lines[channel] = height;
+}
+
+void csi2_stop_channel(struct csi2_device *csi2, unsigned int channel)
+{
+	csi2_dbg("%s [%u]\n", __func__, channel);
+
+	/* Channel disable.  Use FORCE to allow stopping mid-frame. */
+	csi2_reg_write(csi2, CSI2_CH_CTRL(channel), FORCE);
+	/* Latch the above change by writing to the ADDR0 register. */
+	csi2_reg_write(csi2, CSI2_CH_ADDR0(channel), 0);
+	/* Write this again, the HW needs it! */
+	csi2_reg_write(csi2, CSI2_CH_ADDR0(channel), 0);
+}
+
+void csi2_open_rx(struct csi2_device *csi2)
+{
+	csi2_reg_write(csi2, CSI2_IRQ_MASK,
+		       csi2_track_errors ? CSI2_IRQ_MASK_IRQ_ALL : 0);
+
+	dphy_start(&csi2->dphy);
+
+	csi2_reg_write(csi2, CSI2_CTRL,
+		       csi2->multipacket_line ? 0 : EOP_IS_EOL);
+}
+
+void csi2_close_rx(struct csi2_device *csi2)
+{
+	dphy_stop(&csi2->dphy);
+
+	csi2_reg_write(csi2, CSI2_IRQ_MASK, 0);
+}
+
+static int csi2_init_cfg(struct v4l2_subdev *sd,
+			 struct v4l2_subdev_state *state)
+{
+	struct v4l2_mbus_framefmt *fmt;
+
+	for (unsigned int i = 0; i < CSI2_NUM_CHANNELS; ++i) {
+		const struct v4l2_mbus_framefmt *def_fmt;
+
+		/* CSI2_CH1_EMBEDDED */
+		if (i == 1)
+			def_fmt = &cfe_default_meta_format;
+		else
+			def_fmt = &cfe_default_format;
+
+		fmt = v4l2_subdev_state_get_format(state, i);
+		*fmt = *def_fmt;
+
+		fmt = v4l2_subdev_state_get_format(state, i + CSI2_NUM_CHANNELS);
+		*fmt = *def_fmt;
+	}
+
+	return 0;
+}
+
+static int csi2_pad_set_fmt(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *state,
+			    struct v4l2_subdev_format *format)
+{
+	if (format->pad < CSI2_NUM_CHANNELS) {
+		/*
+		 * Store the sink pad format and propagate it to the source pad.
+		 */
+
+		struct v4l2_mbus_framefmt *fmt;
+
+		fmt = v4l2_subdev_state_get_format(state, format->pad);
+		if (!fmt)
+			return -EINVAL;
+
+		*fmt = format->format;
+
+		fmt = v4l2_subdev_state_get_format(state,
+			format->pad + CSI2_NUM_CHANNELS);
+		if (!fmt)
+			return -EINVAL;
+
+		format->format.field = V4L2_FIELD_NONE;
+
+		*fmt = format->format;
+	} else {
+		/*
+		 * Only allow changing the source pad mbus code.
+		 */
+
+		struct v4l2_mbus_framefmt *sink_fmt, *source_fmt;
+		u32 sink_code;
+		u32 code;
+
+		sink_fmt = v4l2_subdev_state_get_format(state,
+			format->pad - CSI2_NUM_CHANNELS);
+		if (!sink_fmt)
+			return -EINVAL;
+
+		source_fmt = v4l2_subdev_state_get_format(state, format->pad);
+		if (!source_fmt)
+			return -EINVAL;
+
+		sink_code = sink_fmt->code;
+		code = format->format.code;
+
+		/*
+		 * If the source code from the user does not match the code in
+		 * the sink pad, check that the source code matches either the
+		 * 16-bit version or the compressed version of the sink code.
+		 */
+
+		if (code != sink_code &&
+		    (code == cfe_find_16bit_code(sink_code) ||
+		     code == cfe_find_compressed_code(sink_code)))
+			source_fmt->code = code;
+
+		format->format.code = source_fmt->code;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_internal_ops csi2_internal_ops = {
+	.init_state = csi2_init_cfg,
+};
+
+static const struct v4l2_subdev_pad_ops csi2_subdev_pad_ops = {
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_fmt = csi2_pad_set_fmt,
+	.link_validate = v4l2_subdev_link_validate_default,
+};
+
+static const struct media_entity_operations csi2_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_ops csi2_subdev_ops = {
+	.pad = &csi2_subdev_pad_ops,
+};
+
+int csi2_init(struct csi2_device *csi2, struct dentry *debugfs)
+{
+	unsigned int i, ret;
+
+	spin_lock_init(&csi2->errors_lock);
+
+	csi2->dphy.dev = csi2->v4l2_dev->dev;
+	dphy_probe(&csi2->dphy);
+
+	debugfs_create_file("csi2_regs", 0444, debugfs, csi2, &csi2_regs_fops);
+
+	if (csi2_track_errors)
+		debugfs_create_file("csi2_errors", 0444, debugfs, csi2,
+				    &csi2_errors_fops);
+
+	for (i = 0; i < CSI2_NUM_CHANNELS * 2; i++)
+		csi2->pad[i].flags = i < CSI2_NUM_CHANNELS ?
+				     MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&csi2->sd.entity, ARRAY_SIZE(csi2->pad),
+				     csi2->pad);
+	if (ret)
+		return ret;
+
+	/* Initialize subdev */
+	v4l2_subdev_init(&csi2->sd, &csi2_subdev_ops);
+	csi2->sd.internal_ops = &csi2_internal_ops;
+	csi2->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+	csi2->sd.entity.ops = &csi2_entity_ops;
+	csi2->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+	csi2->sd.owner = THIS_MODULE;
+	snprintf(csi2->sd.name, sizeof(csi2->sd.name), "csi2");
+
+	ret = v4l2_subdev_init_finalize(&csi2->sd);
+	if (ret)
+		goto err_entity_cleanup;
+
+	ret = v4l2_device_register_subdev(csi2->v4l2_dev, &csi2->sd);
+	if (ret) {
+		csi2_err("Failed register csi2 subdev (%d)\n", ret);
+		goto err_subdev_cleanup;
+	}
+
+	return 0;
+
+err_subdev_cleanup:
+	v4l2_subdev_cleanup(&csi2->sd);
+err_entity_cleanup:
+	media_entity_cleanup(&csi2->sd.entity);
+
+	return ret;
+}
+
+void csi2_uninit(struct csi2_device *csi2)
+{
+	v4l2_device_unregister_subdev(&csi2->sd);
+	v4l2_subdev_cleanup(&csi2->sd);
+	media_entity_cleanup(&csi2->sd.entity);
+}
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1_cfe/csi2.h
@@ -0,0 +1,90 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * RP1 CSI-2 driver.
+ * Copyright (c) 2021 Raspberry Pi Ltd.
+ *
+ */
+#ifndef _RP1_CSI2_
+#define _RP1_CSI2_
+
+#include <linux/debugfs.h>
+#include <linux/io.h>
+#include <linux/types.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+
+#include "dphy.h"
+
+#define CSI2_NUM_CHANNELS 4
+
+#define DISCARDS_TABLE_NUM_VCS 4
+
+enum csi2_mode {
+	CSI2_MODE_NORMAL = 0,
+	CSI2_MODE_REMAP = 1,
+	CSI2_MODE_COMPRESSED = 2,
+	CSI2_MODE_FE_STREAMING = 3,
+};
+
+enum csi2_compression_mode {
+	CSI2_COMPRESSION_DELTA = 1,
+	CSI2_COMPRESSION_SIMPLE = 2,
+	CSI2_COMPRESSION_COMBINED = 3,
+};
+
+struct csi2_cfg {
+	u16 width;
+	u16 height;
+	u32 stride;
+	u32 buffer_size;
+};
+
+enum discards_table_index {
+	DISCARDS_TABLE_OVERFLOW = 0,
+	DISCARDS_TABLE_LENGTH_LIMIT,
+	DISCARDS_TABLE_UNMATCHED,
+	DISCARDS_TABLE_INACTIVE,
+	DISCARDS_TABLE_NUM_ENTRIES,
+};
+
+struct csi2_device {
+	/* Parent V4l2 device */
+	struct v4l2_device *v4l2_dev;
+
+	void __iomem *base;
+
+	struct dphy_data dphy;
+
+	enum v4l2_mbus_type bus_type;
+	unsigned int bus_flags;
+	bool multipacket_line;
+	unsigned int num_lines[CSI2_NUM_CHANNELS];
+
+	struct media_pad pad[CSI2_NUM_CHANNELS * 2];
+	struct v4l2_subdev sd;
+
+	/* lock for csi2 errors counters */
+	spinlock_t errors_lock;
+	u32 overflows;
+	u32 discards_table[DISCARDS_TABLE_NUM_VCS][DISCARDS_TABLE_NUM_ENTRIES];
+	u32 discards_dt_table[DISCARDS_TABLE_NUM_ENTRIES];
+};
+
+void csi2_isr(struct csi2_device *csi2, bool *sof, bool *eof);
+void csi2_set_buffer(struct csi2_device *csi2, unsigned int channel,
+		     dma_addr_t dmaaddr, unsigned int stride,
+		     unsigned int size);
+void csi2_set_compression(struct csi2_device *csi2, unsigned int channel,
+			  enum csi2_compression_mode mode, unsigned int shift,
+			  unsigned int offset);
+void csi2_start_channel(struct csi2_device *csi2, unsigned int channel,
+			enum csi2_mode mode, bool auto_arm,
+			bool pack_bytes, unsigned int width,
+			unsigned int height);
+void csi2_stop_channel(struct csi2_device *csi2, unsigned int channel);
+void csi2_open_rx(struct csi2_device *csi2);
+void csi2_close_rx(struct csi2_device *csi2);
+int csi2_init(struct csi2_device *csi2, struct dentry *debugfs);
+void csi2_uninit(struct csi2_device *csi2);
+
+#endif
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1_cfe/dphy.c
@@ -0,0 +1,187 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * RP1 CSI-2 Driver
+ *
+ * Copyright (C) 2021 - Raspberry Pi Ltd.
+ *
+ */
+
+#include <linux/delay.h>
+#include <linux/dev_printk.h>
+#include <linux/pm_runtime.h>
+
+#include "dphy.h"
+
+#define dphy_dbg(fmt, arg...) dev_dbg(dphy->dev, fmt, ##arg)
+#define dphy_info(fmt, arg...) dev_info(dphy->dev, fmt, ##arg)
+#define dphy_err(fmt, arg...) dev_err(dphy->dev, fmt, ##arg)
+
+/* DW dphy Host registers */
+#define VERSION		0x000
+#define N_LANES		0x004
+#define RESETN		0x008
+#define PHY_SHUTDOWNZ	0x040
+#define PHY_RSTZ	0x044
+#define PHY_RX		0x048
+#define	PHY_STOPSTATE	0x04c
+#define PHY_TST_CTRL0	0x050
+#define PHY_TST_CTRL1	0x054
+#define PHY2_TST_CTRL0	0x058
+#define PHY2_TST_CTRL1	0x05c
+
+/* DW dphy Host Transactions */
+#define DPHY_HS_RX_CTRL_LANE0_OFFSET	0x44
+#define DPHY_PLL_INPUT_DIV_OFFSET	0x17
+#define DPHY_PLL_LOOP_DIV_OFFSET	0x18
+#define DPHY_PLL_DIV_CTRL_OFFSET	0x19
+
+static u32 dw_csi2_host_read(struct dphy_data *dphy, u32 offset)
+{
+	return readl(dphy->base + offset);
+}
+
+static void dw_csi2_host_write(struct dphy_data *dphy, u32 offset, u32 data)
+{
+	writel(data, dphy->base + offset);
+}
+
+static void set_tstclr(struct dphy_data *dphy, u32 val)
+{
+	u32 ctrl0 = dw_csi2_host_read(dphy, PHY_TST_CTRL0);
+
+	dw_csi2_host_write(dphy, PHY_TST_CTRL0, (ctrl0 & ~1) | val);
+}
+
+static void set_tstclk(struct dphy_data *dphy, u32 val)
+{
+	u32 ctrl0 = dw_csi2_host_read(dphy, PHY_TST_CTRL0);
+
+	dw_csi2_host_write(dphy, PHY_TST_CTRL0, (ctrl0 & ~2) | (val << 1));
+}
+
+static uint8_t get_tstdout(struct dphy_data *dphy)
+{
+	u32 ctrl1 = dw_csi2_host_read(dphy, PHY_TST_CTRL1);
+
+	return ((ctrl1 >> 8) & 0xff);
+}
+
+static void set_testen(struct dphy_data *dphy, u32 val)
+{
+	u32 ctrl1 = dw_csi2_host_read(dphy, PHY_TST_CTRL1);
+
+	dw_csi2_host_write(dphy, PHY_TST_CTRL1,
+			   (ctrl1 & ~(1 << 16)) | (val << 16));
+}
+
+static void set_testdin(struct dphy_data *dphy, u32 val)
+{
+	u32 ctrl1 = dw_csi2_host_read(dphy, PHY_TST_CTRL1);
+
+	dw_csi2_host_write(dphy, PHY_TST_CTRL1, (ctrl1 & ~0xff) | val);
+}
+
+static uint8_t dphy_transaction(struct dphy_data *dphy, u8 test_code,
+				uint8_t test_data)
+{
+	/* See page 101 of the MIPI DPHY databook. */
+	set_tstclk(dphy, 1);
+	set_testen(dphy, 0);
+	set_testdin(dphy, test_code);
+	set_testen(dphy, 1);
+	set_tstclk(dphy, 0);
+	set_testen(dphy, 0);
+	set_testdin(dphy, test_data);
+	set_tstclk(dphy, 1);
+	return get_tstdout(dphy);
+}
+
+static void dphy_set_hsfreqrange(struct dphy_data *dphy, uint32_t mbps)
+{
+	/* See Table 5-1 on page 65 of dphy databook */
+	static const u16 hsfreqrange_table[][2] = {
+		{ 89, 0b000000 },   { 99, 0b010000 },	{ 109, 0b100000 },
+		{ 129, 0b000001 },  { 139, 0b010001 },	{ 149, 0b100001 },
+		{ 169, 0b000010 },  { 179, 0b010010 },	{ 199, 0b100010 },
+		{ 219, 0b000011 },  { 239, 0b010011 },	{ 249, 0b100011 },
+		{ 269, 0b000100 },  { 299, 0b010100 },	{ 329, 0b000101 },
+		{ 359, 0b010101 },  { 399, 0b100101 },	{ 449, 0b000110 },
+		{ 499, 0b010110 },  { 549, 0b000111 },	{ 599, 0b010111 },
+		{ 649, 0b001000 },  { 699, 0b011000 },	{ 749, 0b001001 },
+		{ 799, 0b011001 },  { 849, 0b101001 },	{ 899, 0b111001 },
+		{ 949, 0b001010 },  { 999, 0b011010 },	{ 1049, 0b101010 },
+		{ 1099, 0b111010 }, { 1149, 0b001011 }, { 1199, 0b011011 },
+		{ 1249, 0b101011 }, { 1299, 0b111011 }, { 1349, 0b001100 },
+		{ 1399, 0b011100 }, { 1449, 0b101100 }, { 1500, 0b111100 },
+	};
+	unsigned int i;
+
+	if (mbps < 80 || mbps > 1500)
+		dphy_err("DPHY: Datarate %u Mbps out of range\n", mbps);
+
+	for (i = 0; i < ARRAY_SIZE(hsfreqrange_table) - 1; i++) {
+		if (mbps <= hsfreqrange_table[i][0])
+			break;
+	}
+
+	dphy_transaction(dphy, DPHY_HS_RX_CTRL_LANE0_OFFSET,
+			 hsfreqrange_table[i][1] << 1);
+}
+
+static void dphy_init(struct dphy_data *dphy)
+{
+	dw_csi2_host_write(dphy, PHY_RSTZ, 0);
+	dw_csi2_host_write(dphy, PHY_SHUTDOWNZ, 0);
+	set_tstclk(dphy, 1);
+	set_testen(dphy, 0);
+	set_tstclr(dphy, 1);
+	usleep_range(15, 20);
+	set_tstclr(dphy, 0);
+	usleep_range(15, 20);
+
+	dphy_set_hsfreqrange(dphy, dphy->dphy_rate);
+
+	usleep_range(5, 10);
+	dw_csi2_host_write(dphy, PHY_SHUTDOWNZ, 1);
+	usleep_range(5, 10);
+	dw_csi2_host_write(dphy, PHY_RSTZ, 1);
+}
+
+void dphy_start(struct dphy_data *dphy)
+{
+	dw_csi2_host_write(dphy, RESETN, 0);
+	dw_csi2_host_write(dphy, N_LANES, (dphy->active_lanes - 1));
+	dphy_init(dphy);
+	dw_csi2_host_write(dphy, RESETN, 0xffffffff);
+	usleep_range(10, 50);
+}
+
+void dphy_stop(struct dphy_data *dphy)
+{
+	/*
+	 * We no longer go into reset here, because the camera might still be
+	 * streaming. If we kill the CSI-2 Host in mid-packet, it can leave the
+	 * IDI interface in a bad state, causing the next packet to be lost.
+	 *
+	 * XXX Is it safe to assume it will be idle before the next dphy_start?
+	 * XXX What happens if the camera itself generates an incomplete packet?
+	 *
+	 * TODO: Instead, should we consider resetting the *entire* MIPI block
+	 * (including CSI2AXI and ISP-FE)? That can't safely be done until
+	 * all AXI traffic has completed. It would cause APB access to hang.
+	 */
+}
+
+void dphy_probe(struct dphy_data *dphy)
+{
+	u32 host_ver;
+	u8 host_ver_major, host_ver_minor;
+
+	host_ver = dw_csi2_host_read(dphy, VERSION);
+	host_ver_major = (u8)((host_ver >> 24) - '0');
+	host_ver_minor = (u8)((host_ver >> 16) - '0');
+	host_ver_minor = host_ver_minor * 10;
+	host_ver_minor += (u8)((host_ver >> 8) - '0');
+
+	dphy_info("DW dphy Host HW v%u.%u\n", host_ver_major, host_ver_minor);
+}
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1_cfe/dphy.h
@@ -0,0 +1,27 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2021 Raspberry Pi Ltd.
+ *
+ */
+
+#ifndef _RP1_DPHY_
+#define _RP1_DPHY_
+
+#include <linux/io.h>
+#include <linux/types.h>
+
+struct dphy_data {
+	struct device *dev;
+
+	void __iomem *base;
+
+	u32 dphy_rate;
+	u32 max_lanes;
+	u32 active_lanes;
+};
+
+void dphy_probe(struct dphy_data *dphy);
+void dphy_start(struct dphy_data *dphy);
+void dphy_stop(struct dphy_data *dphy);
+
+#endif
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1_cfe/pisp_common.h
@@ -0,0 +1,69 @@
+/* SPDX-License-Identifier: GPL-2.0-only WITH Linux-syscall-note */
+/*
+ * RP1 PiSP common definitions.
+ *
+ * Copyright (C) 2021 - Raspberry Pi Ltd.
+ *
+ */
+#ifndef _PISP_COMMON_H_
+#define _PISP_COMMON_H_
+
+#include "pisp_types.h"
+
+struct pisp_bla_config {
+	u16 black_level_r;
+	u16 black_level_gr;
+	u16 black_level_gb;
+	u16 black_level_b;
+	u16 output_black_level;
+	u8 pad[2];
+};
+
+struct pisp_wbg_config {
+	u16 gain_r;
+	u16 gain_g;
+	u16 gain_b;
+	u8 pad[2];
+};
+
+struct pisp_compress_config {
+	/* value subtracted from incoming data */
+	u16 offset;
+	u8 pad;
+	/* 1 => Companding; 2 => Delta (recommended); 3 => Combined (for HDR) */
+	u8 mode;
+};
+
+struct pisp_decompress_config {
+	/* value added to reconstructed data */
+	u16 offset;
+	u8 pad;
+	/* 1 => Companding; 2 => Delta (recommended); 3 => Combined (for HDR) */
+	u8 mode;
+};
+
+enum pisp_axi_flags {
+	/*
+	 * round down bursts to end at a 32-byte boundary, to align following
+	 * bursts
+	 */
+	PISP_AXI_FLAG_ALIGN = 128,
+	/* for FE writer: force WSTRB high, to pad output to 16-byte boundary */
+	PISP_AXI_FLAG_PAD = 64,
+	/* for FE writer: Use Output FIFO level to trigger "panic" */
+	PISP_AXI_FLAG_PANIC = 32,
+};
+
+struct pisp_axi_config {
+	/*
+	 * burst length minus one, which must be in the range 0:15; OR'd with
+	 * flags
+	 */
+	u8 maxlen_flags;
+	/* { prot[2:0], cache[3:0] } fields, echoed on AXI bus */
+	u8 cache_prot;
+	/* QoS field(s) (4x4 bits for FE writer; 4 bits for other masters) */
+	u16 qos;
+};
+
+#endif /* _PISP_COMMON_H_ */
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1_cfe/pisp_fe.c
@@ -0,0 +1,569 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * PiSP Front End driver.
+ * Copyright (c) 2021 Raspberry Pi Ltd.
+ *
+ */
+
+#include <linux/bitops.h>
+#include <linux/delay.h>
+#include <linux/moduleparam.h>
+#include <linux/pm_runtime.h>
+#include <linux/seq_file.h>
+
+#include <media/videobuf2-dma-contig.h>
+
+#include "pisp_fe.h"
+#include "cfe.h"
+
+#define FE_VERSION		0x000
+#define FE_CONTROL		0x004
+#define FE_STATUS		0x008
+#define FE_FRAME_STATUS		0x00c
+#define FE_ERROR_STATUS		0x010
+#define FE_OUTPUT_STATUS	0x014
+#define FE_INT_EN		0x018
+#define FE_INT_STATUS		0x01c
+
+/* CONTROL */
+#define FE_CONTROL_QUEUE	BIT(0)
+#define FE_CONTROL_ABORT	BIT(1)
+#define FE_CONTROL_RESET	BIT(2)
+#define FE_CONTROL_LATCH_REGS	BIT(3)
+
+/* INT_EN / INT_STATUS */
+#define FE_INT_EOF		BIT(0)
+#define FE_INT_SOF		BIT(1)
+#define FE_INT_LINES0		BIT(8)
+#define FE_INT_LINES1		BIT(9)
+#define FE_INT_STATS		BIT(16)
+#define FE_INT_QREADY		BIT(24)
+
+/* STATUS */
+#define FE_STATUS_QUEUED	BIT(0)
+#define FE_STATUS_WAITING	BIT(1)
+#define FE_STATUS_ACTIVE	BIT(2)
+
+#define PISP_FE_CONFIG_BASE_OFFSET	0x0040
+
+#define PISP_FE_ENABLE_STATS_CLUSTER \
+	(PISP_FE_ENABLE_STATS_CROP | PISP_FE_ENABLE_DECIMATE    | \
+	 PISP_FE_ENABLE_BLC        | PISP_FE_ENABLE_CDAF_STATS  | \
+	 PISP_FE_ENABLE_AWB_STATS  | PISP_FE_ENABLE_RGBY        | \
+	 PISP_FE_ENABLE_LSC        | PISP_FE_ENABLE_AGC_STATS)
+
+#define PISP_FE_ENABLE_OUTPUT_CLUSTER(i)				\
+	((PISP_FE_ENABLE_CROP0     | PISP_FE_ENABLE_DOWNSCALE0 |	\
+	  PISP_FE_ENABLE_COMPRESS0 | PISP_FE_ENABLE_OUTPUT0) << (4 * (i)))
+
+struct pisp_fe_config_param {
+	u32 dirty_flags;
+	u32 dirty_flags_extra;
+	size_t offset;
+	size_t size;
+};
+
+static const struct pisp_fe_config_param pisp_fe_config_map[] = {
+	/* *_dirty_flag_extra types */
+	{ 0, PISP_FE_DIRTY_GLOBAL,     offsetof(struct pisp_fe_config, global),
+					sizeof(struct pisp_fe_global_config)         },
+	{ 0, PISP_FE_DIRTY_FLOATING,   offsetof(struct pisp_fe_config, floating_stats),
+					sizeof(struct pisp_fe_floating_stats_config) },
+	{ 0, PISP_FE_DIRTY_OUTPUT_AXI, offsetof(struct pisp_fe_config, output_axi),
+					sizeof(struct pisp_fe_output_axi_config)     },
+	/* *_dirty_flag types */
+	{ PISP_FE_ENABLE_INPUT,      0, offsetof(struct pisp_fe_config, input),
+					sizeof(struct pisp_fe_input_config)          },
+	{ PISP_FE_ENABLE_DECOMPRESS, 0, offsetof(struct pisp_fe_config, decompress),
+					sizeof(struct pisp_decompress_config)        },
+	{ PISP_FE_ENABLE_DECOMPAND,  0, offsetof(struct pisp_fe_config, decompand),
+					sizeof(struct pisp_fe_decompand_config)      },
+	{ PISP_FE_ENABLE_BLA,        0, offsetof(struct pisp_fe_config, bla),
+					sizeof(struct pisp_bla_config)               },
+	{ PISP_FE_ENABLE_DPC,        0, offsetof(struct pisp_fe_config, dpc),
+					sizeof(struct pisp_fe_dpc_config)            },
+	{ PISP_FE_ENABLE_STATS_CROP, 0, offsetof(struct pisp_fe_config, stats_crop),
+					sizeof(struct pisp_fe_crop_config)           },
+	{ PISP_FE_ENABLE_BLC,	     0, offsetof(struct pisp_fe_config, blc),
+					sizeof(struct pisp_bla_config)               },
+	{ PISP_FE_ENABLE_CDAF_STATS, 0, offsetof(struct pisp_fe_config, cdaf_stats),
+					sizeof(struct pisp_fe_cdaf_stats_config)     },
+	{ PISP_FE_ENABLE_AWB_STATS,  0, offsetof(struct pisp_fe_config, awb_stats),
+					sizeof(struct pisp_fe_awb_stats_config)      },
+	{ PISP_FE_ENABLE_RGBY,       0, offsetof(struct pisp_fe_config, rgby),
+					sizeof(struct pisp_fe_rgby_config)           },
+	{ PISP_FE_ENABLE_LSC,        0, offsetof(struct pisp_fe_config, lsc),
+					sizeof(struct pisp_fe_lsc_config)            },
+	{ PISP_FE_ENABLE_AGC_STATS,  0, offsetof(struct pisp_fe_config, agc_stats),
+					sizeof(struct pisp_agc_statistics)           },
+	{ PISP_FE_ENABLE_CROP0,      0, offsetof(struct pisp_fe_config, ch[0].crop),
+					sizeof(struct pisp_fe_crop_config)           },
+	{ PISP_FE_ENABLE_DOWNSCALE0, 0, offsetof(struct pisp_fe_config, ch[0].downscale),
+					sizeof(struct pisp_fe_downscale_config)      },
+	{ PISP_FE_ENABLE_COMPRESS0,  0, offsetof(struct pisp_fe_config, ch[0].compress),
+					sizeof(struct pisp_compress_config)          },
+	{ PISP_FE_ENABLE_OUTPUT0,    0, offsetof(struct pisp_fe_config, ch[0].output),
+					sizeof(struct pisp_fe_output_config)         },
+	{ PISP_FE_ENABLE_CROP1,      0, offsetof(struct pisp_fe_config, ch[1].crop),
+					sizeof(struct pisp_fe_crop_config)           },
+	{ PISP_FE_ENABLE_DOWNSCALE1, 0, offsetof(struct pisp_fe_config, ch[1].downscale),
+					sizeof(struct pisp_fe_downscale_config)      },
+	{ PISP_FE_ENABLE_COMPRESS1,  0, offsetof(struct pisp_fe_config, ch[1].compress),
+					sizeof(struct pisp_compress_config)          },
+	{ PISP_FE_ENABLE_OUTPUT1,    0, offsetof(struct pisp_fe_config, ch[1].output),
+					sizeof(struct pisp_fe_output_config)         },
+};
+
+#define pisp_fe_dbg_verbose(fmt, arg...)                        \
+	do {                                                    \
+		if (cfe_debug_verbose)                          \
+			dev_dbg(fe->v4l2_dev->dev, fmt, ##arg); \
+	} while (0)
+#define pisp_fe_dbg(fmt, arg...) dev_dbg(fe->v4l2_dev->dev, fmt, ##arg)
+#define pisp_fe_info(fmt, arg...) dev_info(fe->v4l2_dev->dev, fmt, ##arg)
+#define pisp_fe_err(fmt, arg...) dev_err(fe->v4l2_dev->dev, fmt, ##arg)
+
+static inline u32 pisp_fe_reg_read(struct pisp_fe_device *fe, u32 offset)
+{
+	return readl(fe->base + offset);
+}
+
+static inline void pisp_fe_reg_write(struct pisp_fe_device *fe, u32 offset,
+				     u32 val)
+{
+	writel(val, fe->base + offset);
+	pisp_fe_dbg_verbose("fe: write 0x%04x -> 0x%03x\n", val, offset);
+}
+
+static inline void pisp_fe_reg_write_relaxed(struct pisp_fe_device *fe, u32 offset,
+					     u32 val)
+{
+	writel_relaxed(val, fe->base + offset);
+	pisp_fe_dbg_verbose("fe: write 0x%04x -> 0x%03x\n", val, offset);
+}
+
+static int pisp_regs_show(struct seq_file *s, void *data)
+{
+	struct pisp_fe_device *fe = s->private;
+	int ret;
+
+	ret = pm_runtime_resume_and_get(fe->v4l2_dev->dev);
+	if (ret)
+		return ret;
+
+	pisp_fe_reg_write(fe, FE_CONTROL, FE_CONTROL_LATCH_REGS);
+
+#define DUMP(reg) seq_printf(s, #reg " \t0x%08x\n", pisp_fe_reg_read(fe, reg))
+	DUMP(FE_VERSION);
+	DUMP(FE_CONTROL);
+	DUMP(FE_STATUS);
+	DUMP(FE_FRAME_STATUS);
+	DUMP(FE_ERROR_STATUS);
+	DUMP(FE_OUTPUT_STATUS);
+	DUMP(FE_INT_EN);
+	DUMP(FE_INT_STATUS);
+#undef DUMP
+
+	pm_runtime_put(fe->v4l2_dev->dev);
+
+	return 0;
+}
+
+DEFINE_SHOW_ATTRIBUTE(pisp_regs);
+
+static void pisp_config_write(struct pisp_fe_device *fe,
+			      struct pisp_fe_config *config,
+			      unsigned int start_offset,
+			      unsigned int size)
+{
+	const unsigned int max_offset =
+		offsetof(struct pisp_fe_config, ch[PISP_FE_NUM_OUTPUTS]);
+	unsigned int i, end_offset;
+	u32 *cfg = (u32 *)config;
+
+	start_offset = min(start_offset, max_offset);
+	end_offset = min(start_offset + size, max_offset);
+
+	cfg += start_offset >> 2;
+	for (i = start_offset; i < end_offset; i += 4, cfg++)
+		pisp_fe_reg_write_relaxed(fe, PISP_FE_CONFIG_BASE_OFFSET + i,
+					  *cfg);
+}
+
+void pisp_fe_isr(struct pisp_fe_device *fe, bool *sof, bool *eof)
+{
+	u32 status, int_status, out_status, frame_status, error_status;
+	unsigned int i;
+
+	pisp_fe_reg_write(fe, FE_CONTROL, FE_CONTROL_LATCH_REGS);
+	status = pisp_fe_reg_read(fe, FE_STATUS);
+	out_status = pisp_fe_reg_read(fe, FE_OUTPUT_STATUS);
+	frame_status = pisp_fe_reg_read(fe, FE_FRAME_STATUS);
+	error_status = pisp_fe_reg_read(fe, FE_ERROR_STATUS);
+
+	int_status = pisp_fe_reg_read(fe, FE_INT_STATUS);
+	pisp_fe_reg_write(fe, FE_INT_STATUS, int_status);
+
+	pisp_fe_dbg_verbose("%s: status 0x%x out 0x%x frame 0x%x error 0x%x int 0x%x\n",
+		__func__, status, out_status, frame_status, error_status,
+		int_status);
+
+	/* We do not report interrupts for the input/stream pad. */
+	for (i = 0; i < FE_NUM_PADS - 1; i++) {
+		sof[i] = !!(int_status & FE_INT_SOF);
+		eof[i] = !!(int_status & FE_INT_EOF);
+	}
+}
+
+static bool pisp_fe_validate_output(struct pisp_fe_config const *cfg,
+				    unsigned int c, struct v4l2_format const *f)
+{
+	unsigned int wbytes;
+
+	wbytes = cfg->ch[c].output.format.width;
+	if (cfg->ch[c].output.format.format & PISP_IMAGE_FORMAT_BPS_MASK)
+		wbytes *= 2;
+
+	/* Check output image dimensions are nonzero and not too big */
+	if (cfg->ch[c].output.format.width < 2 ||
+	    cfg->ch[c].output.format.height < 2 ||
+	    cfg->ch[c].output.format.height > f->fmt.pix.height ||
+	    cfg->ch[c].output.format.stride > f->fmt.pix.bytesperline ||
+	    wbytes > f->fmt.pix.bytesperline)
+		return false;
+
+	/* Check for zero-sized crops, which could cause lockup */
+	if ((cfg->global.enables & PISP_FE_ENABLE_CROP(c)) &&
+	    ((cfg->ch[c].crop.offset_x >= (cfg->input.format.width & ~1) ||
+	      cfg->ch[c].crop.offset_y >= cfg->input.format.height ||
+	      cfg->ch[c].crop.width < 2 ||
+	      cfg->ch[c].crop.height < 2)))
+		return false;
+
+	if ((cfg->global.enables & PISP_FE_ENABLE_DOWNSCALE(c)) &&
+	    (cfg->ch[c].downscale.output_width < 2 ||
+	     cfg->ch[c].downscale.output_height < 2))
+		return false;
+
+	return true;
+}
+
+static bool pisp_fe_validate_stats(struct pisp_fe_config const *cfg)
+{
+	/* Check for zero-sized crop, which could cause lockup */
+	return (!(cfg->global.enables & PISP_FE_ENABLE_STATS_CROP) ||
+		(cfg->stats_crop.offset_x < (cfg->input.format.width & ~1) &&
+		 cfg->stats_crop.offset_y < cfg->input.format.height &&
+		 cfg->stats_crop.width >= 2 &&
+		 cfg->stats_crop.height >= 2));
+}
+
+int pisp_fe_validate_config(struct pisp_fe_device *fe,
+			    struct pisp_fe_config *cfg,
+			    struct v4l2_format const *f0,
+			    struct v4l2_format const *f1)
+{
+	unsigned int i;
+
+	/*
+	 * Check the input is enabled, streaming and has nonzero size;
+	 * to avoid cases where the hardware might lock up or try to
+	 * read inputs from memory (which this driver doesn't support).
+	 */
+	if (!(cfg->global.enables & PISP_FE_ENABLE_INPUT) ||
+	    cfg->input.streaming != 1 || cfg->input.format.width < 2 ||
+	    cfg->input.format.height < 2) {
+		pisp_fe_err("%s: Input config not valid", __func__);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < PISP_FE_NUM_OUTPUTS; i++) {
+		if (!(cfg->global.enables & PISP_FE_ENABLE_OUTPUT(i))) {
+			if (cfg->global.enables &
+					PISP_FE_ENABLE_OUTPUT_CLUSTER(i)) {
+				pisp_fe_err("%s: Output %u not valid",
+					    __func__, i);
+				return -EINVAL;
+			}
+			continue;
+		}
+
+		if (!pisp_fe_validate_output(cfg, i, i ? f1 : f0))
+			return -EINVAL;
+	}
+
+	if ((cfg->global.enables & PISP_FE_ENABLE_STATS_CLUSTER) &&
+	    !pisp_fe_validate_stats(cfg)) {
+		pisp_fe_err("%s: Stats config not valid", __func__);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+void pisp_fe_submit_job(struct pisp_fe_device *fe, struct vb2_buffer **vb2_bufs,
+			struct pisp_fe_config *cfg)
+{
+	unsigned int i;
+	u64 addr;
+	u32 status;
+
+	/*
+	 * Check output buffers exist and outputs are correctly configured.
+	 * If valid, set the buffer's DMA address; otherwise disable.
+	 */
+	for (i = 0; i < PISP_FE_NUM_OUTPUTS; i++) {
+		struct vb2_buffer *buf = vb2_bufs[FE_OUTPUT0_PAD + i];
+
+		if (!(cfg->global.enables & PISP_FE_ENABLE_OUTPUT(i)))
+			continue;
+
+		addr = vb2_dma_contig_plane_dma_addr(buf, 0);
+		cfg->output_buffer[i].addr_lo = addr & 0xffffffff;
+		cfg->output_buffer[i].addr_hi = addr >> 32;
+	}
+
+	if (vb2_bufs[FE_STATS_PAD]) {
+		addr = vb2_dma_contig_plane_dma_addr(vb2_bufs[FE_STATS_PAD], 0);
+		cfg->stats_buffer.addr_lo = addr & 0xffffffff;
+		cfg->stats_buffer.addr_hi = addr >> 32;
+	}
+
+	/* Set up ILINES interrupts 3/4 of the way down each output */
+	cfg->ch[0].output.ilines =
+		max(0x80u, (3u * cfg->ch[0].output.format.height) >> 2);
+	cfg->ch[1].output.ilines =
+		max(0x80u, (3u * cfg->ch[1].output.format.height) >> 2);
+
+	/*
+	 * The hardware must have consumed the previous config by now.
+	 * This read of status also serves as a memory barrier before the
+	 * sequence of relaxed writes which follow.
+	 */
+	status = pisp_fe_reg_read(fe, FE_STATUS);
+	pisp_fe_dbg_verbose("%s: status = 0x%x\n", __func__, status);
+	if (WARN_ON(status & FE_STATUS_QUEUED))
+		return;
+
+	/*
+	 * Unconditionally write buffers, global and input parameters.
+	 * Write cropping and output parameters whenever they are enabled.
+	 * Selectively write other parameters that have been marked as
+	 * changed through the dirty flags.
+	 */
+	pisp_config_write(fe, cfg, 0,
+			  offsetof(struct pisp_fe_config, decompress));
+	cfg->dirty_flags_extra &= ~PISP_FE_DIRTY_GLOBAL;
+	cfg->dirty_flags &= ~PISP_FE_ENABLE_INPUT;
+	cfg->dirty_flags |= (cfg->global.enables &
+			     (PISP_FE_ENABLE_STATS_CROP        |
+			      PISP_FE_ENABLE_OUTPUT_CLUSTER(0) |
+			      PISP_FE_ENABLE_OUTPUT_CLUSTER(1)));
+	for (i = 0; i < ARRAY_SIZE(pisp_fe_config_map); i++) {
+		const struct pisp_fe_config_param *p = &pisp_fe_config_map[i];
+
+		if (cfg->dirty_flags & p->dirty_flags ||
+		    cfg->dirty_flags_extra & p->dirty_flags_extra)
+			pisp_config_write(fe, cfg, p->offset, p->size);
+	}
+
+	/* This final non-relaxed write serves as a memory barrier */
+	pisp_fe_reg_write(fe, FE_CONTROL, FE_CONTROL_QUEUE);
+}
+
+void pisp_fe_start(struct pisp_fe_device *fe)
+{
+	pisp_fe_reg_write(fe, FE_CONTROL, FE_CONTROL_RESET);
+	pisp_fe_reg_write(fe, FE_INT_STATUS, ~0);
+	pisp_fe_reg_write(fe, FE_INT_EN, FE_INT_EOF | FE_INT_SOF | FE_INT_LINES0 | FE_INT_LINES1);
+	fe->inframe_count = 0;
+}
+
+void pisp_fe_stop(struct pisp_fe_device *fe)
+{
+	pisp_fe_reg_write(fe, FE_INT_EN, 0);
+	pisp_fe_reg_write(fe, FE_CONTROL, FE_CONTROL_ABORT);
+	usleep_range(1000, 2000);
+	WARN_ON(pisp_fe_reg_read(fe, FE_STATUS));
+	pisp_fe_reg_write(fe, FE_INT_STATUS, ~0);
+}
+
+static int pisp_fe_init_cfg(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *state)
+{
+	struct v4l2_mbus_framefmt *fmt;
+
+	fmt = v4l2_subdev_state_get_format(state, FE_STREAM_PAD);
+	*fmt = cfe_default_format;
+	fmt->code = MEDIA_BUS_FMT_SRGGB16_1X16;
+
+	fmt = v4l2_subdev_state_get_format(state, FE_CONFIG_PAD);
+	*fmt = cfe_default_meta_format;
+	fmt->code = MEDIA_BUS_FMT_FIXED;
+
+	fmt = v4l2_subdev_state_get_format(state, FE_OUTPUT0_PAD);
+	*fmt = cfe_default_format;
+	fmt->code = MEDIA_BUS_FMT_SRGGB16_1X16;
+
+	fmt = v4l2_subdev_state_get_format(state, FE_OUTPUT1_PAD);
+	*fmt = cfe_default_format;
+	fmt->code = MEDIA_BUS_FMT_SRGGB16_1X16;
+
+	fmt = v4l2_subdev_state_get_format(state, FE_STATS_PAD);
+	*fmt = cfe_default_meta_format;
+	fmt->code = MEDIA_BUS_FMT_FIXED;
+
+	return 0;
+}
+
+static int pisp_fe_pad_set_fmt(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *state,
+			       struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt;
+	const struct cfe_fmt *cfe_fmt;
+
+	/* TODO: format propagation to source pads */
+	/* TODO: format validation */
+
+	switch (format->pad) {
+	case FE_STREAM_PAD:
+		cfe_fmt = find_format_by_code(format->format.code);
+		if (!cfe_fmt || !(cfe_fmt->flags & CFE_FORMAT_FLAG_FE_OUT))
+			cfe_fmt = find_format_by_code(MEDIA_BUS_FMT_SRGGB16_1X16);
+
+		format->format.code = cfe_fmt->code;
+		format->format.field = V4L2_FIELD_NONE;
+
+		fmt = v4l2_subdev_state_get_format(state, FE_STREAM_PAD);
+		*fmt = format->format;
+
+		fmt = v4l2_subdev_state_get_format(state, FE_OUTPUT0_PAD);
+		*fmt = format->format;
+
+		fmt = v4l2_subdev_state_get_format(state, FE_OUTPUT1_PAD);
+		*fmt = format->format;
+
+		return 0;
+
+	case FE_OUTPUT0_PAD:
+	case FE_OUTPUT1_PAD: {
+		/*
+		 * TODO: we should allow scaling and cropping by allowing the
+		 * user to set the size here.
+		 */
+		struct v4l2_mbus_framefmt *sink_fmt, *source_fmt;
+		u32 sink_code;
+		u32 code;
+
+		sink_fmt = v4l2_subdev_state_get_format(state, FE_STREAM_PAD);
+		if (!sink_fmt)
+			return -EINVAL;
+
+		source_fmt = v4l2_subdev_state_get_format(state, format->pad);
+		if (!source_fmt)
+			return -EINVAL;
+
+		sink_code = sink_fmt->code;
+		code = format->format.code;
+
+		/*
+		 * If the source code from the user does not match the code in
+		 * the sink pad, check that the source code matches the
+		 * compressed version of the sink code.
+		 */
+
+		if (code != sink_code &&
+		    code == cfe_find_compressed_code(sink_code))
+			source_fmt->code = code;
+
+		return 0;
+	}
+
+	case FE_CONFIG_PAD:
+	case FE_STATS_PAD:
+	default:
+		return v4l2_subdev_get_fmt(sd, state, format);
+	}
+}
+
+static const struct v4l2_subdev_internal_ops pisp_fe_internal_ops = {
+	.init_state = pisp_fe_init_cfg,
+};
+
+static const struct v4l2_subdev_pad_ops pisp_fe_subdev_pad_ops = {
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_fmt = pisp_fe_pad_set_fmt,
+	.link_validate = v4l2_subdev_link_validate_default,
+};
+
+static const struct media_entity_operations pisp_fe_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_ops pisp_fe_subdev_ops = {
+	.pad = &pisp_fe_subdev_pad_ops,
+};
+
+int pisp_fe_init(struct pisp_fe_device *fe, struct dentry *debugfs)
+{
+	int ret;
+
+	debugfs_create_file("pisp_regs", 0444, debugfs, fe, &pisp_regs_fops);
+
+	fe->hw_revision = pisp_fe_reg_read(fe, FE_VERSION);
+	pisp_fe_info("PiSP FE HW v%u.%u\n",
+		     (fe->hw_revision >> 24) & 0xff,
+		     (fe->hw_revision >> 20) & 0x0f);
+
+	fe->pad[FE_STREAM_PAD].flags =
+		MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT;
+	fe->pad[FE_CONFIG_PAD].flags = MEDIA_PAD_FL_SINK;
+	fe->pad[FE_OUTPUT0_PAD].flags = MEDIA_PAD_FL_SOURCE;
+	fe->pad[FE_OUTPUT1_PAD].flags = MEDIA_PAD_FL_SOURCE;
+	fe->pad[FE_STATS_PAD].flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&fe->sd.entity, ARRAY_SIZE(fe->pad),
+				     fe->pad);
+	if (ret)
+		return ret;
+
+	/* Initialize subdev */
+	v4l2_subdev_init(&fe->sd, &pisp_fe_subdev_ops);
+	fe->sd.internal_ops = &pisp_fe_internal_ops;
+	fe->sd.entity.function = MEDIA_ENT_F_PROC_VIDEO_SCALER;
+	fe->sd.entity.ops = &pisp_fe_entity_ops;
+	fe->sd.entity.name = "pisp-fe";
+	fe->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+	fe->sd.owner = THIS_MODULE;
+	snprintf(fe->sd.name, sizeof(fe->sd.name), "pisp-fe");
+
+	ret = v4l2_subdev_init_finalize(&fe->sd);
+	if (ret)
+		goto err_entity_cleanup;
+
+	ret = v4l2_device_register_subdev(fe->v4l2_dev, &fe->sd);
+	if (ret) {
+		pisp_fe_err("Failed register pisp fe subdev (%d)\n", ret);
+		goto err_subdev_cleanup;
+	}
+
+	/* Must be in IDLE state (STATUS == 0) here. */
+	WARN_ON(pisp_fe_reg_read(fe, FE_STATUS));
+
+	return 0;
+
+err_subdev_cleanup:
+	v4l2_subdev_cleanup(&fe->sd);
+err_entity_cleanup:
+	media_entity_cleanup(&fe->sd.entity);
+
+	return ret;
+}
+
+void pisp_fe_uninit(struct pisp_fe_device *fe)
+{
+	v4l2_device_unregister_subdev(&fe->sd);
+	v4l2_subdev_cleanup(&fe->sd);
+	media_entity_cleanup(&fe->sd.entity);
+}
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1_cfe/pisp_fe.h
@@ -0,0 +1,53 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * PiSP Front End driver.
+ * Copyright (c) 2021 Raspberry Pi Ltd.
+ *
+ */
+#ifndef _PISP_FE_H_
+#define _PISP_FE_H_
+
+#include <linux/debugfs.h>
+#include <linux/io.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+
+#include <media/media-device.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+
+#include "pisp_fe_config.h"
+
+enum pisp_fe_pads {
+	FE_STREAM_PAD,
+	FE_CONFIG_PAD,
+	FE_OUTPUT0_PAD,
+	FE_OUTPUT1_PAD,
+	FE_STATS_PAD,
+	FE_NUM_PADS
+};
+
+struct pisp_fe_device {
+	/* Parent V4l2 device */
+	struct v4l2_device *v4l2_dev;
+	void __iomem *base;
+	u32 hw_revision;
+
+	u16 inframe_count;
+	struct media_pad pad[FE_NUM_PADS];
+	struct v4l2_subdev sd;
+};
+
+void pisp_fe_isr(struct pisp_fe_device *fe, bool *sof, bool *eof);
+int pisp_fe_validate_config(struct pisp_fe_device *fe,
+			    struct pisp_fe_config *cfg,
+			    struct v4l2_format const *f0,
+			    struct v4l2_format const *f1);
+void pisp_fe_submit_job(struct pisp_fe_device *fe, struct vb2_buffer **vb2_bufs,
+			struct pisp_fe_config *cfg);
+void pisp_fe_start(struct pisp_fe_device *fe);
+void pisp_fe_stop(struct pisp_fe_device *fe);
+int pisp_fe_init(struct pisp_fe_device *fe, struct dentry *debugfs);
+void pisp_fe_uninit(struct pisp_fe_device *fe);
+
+#endif
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1_cfe/pisp_fe_config.h
@@ -0,0 +1,272 @@
+/* SPDX-License-Identifier: GPL-2.0-only WITH Linux-syscall-note */
+/*
+ * RP1 PiSP Front End Driver Configuration structures
+ *
+ * Copyright (C) 2021 - Raspberry Pi Ltd.
+ *
+ */
+#ifndef _PISP_FE_CONFIG_
+#define _PISP_FE_CONFIG_
+
+#include "pisp_common.h"
+
+#include "pisp_statistics.h"
+
+#define PISP_FE_NUM_OUTPUTS 2
+
+enum pisp_fe_enable {
+	PISP_FE_ENABLE_INPUT = 0x000001,
+	PISP_FE_ENABLE_DECOMPRESS = 0x000002,
+	PISP_FE_ENABLE_DECOMPAND = 0x000004,
+	PISP_FE_ENABLE_BLA = 0x000008,
+	PISP_FE_ENABLE_DPC = 0x000010,
+	PISP_FE_ENABLE_STATS_CROP = 0x000020,
+	PISP_FE_ENABLE_DECIMATE = 0x000040,
+	PISP_FE_ENABLE_BLC = 0x000080,
+	PISP_FE_ENABLE_CDAF_STATS = 0x000100,
+	PISP_FE_ENABLE_AWB_STATS = 0x000200,
+	PISP_FE_ENABLE_RGBY = 0x000400,
+	PISP_FE_ENABLE_LSC = 0x000800,
+	PISP_FE_ENABLE_AGC_STATS = 0x001000,
+	PISP_FE_ENABLE_CROP0 = 0x010000,
+	PISP_FE_ENABLE_DOWNSCALE0 = 0x020000,
+	PISP_FE_ENABLE_COMPRESS0 = 0x040000,
+	PISP_FE_ENABLE_OUTPUT0 = 0x080000,
+	PISP_FE_ENABLE_CROP1 = 0x100000,
+	PISP_FE_ENABLE_DOWNSCALE1 = 0x200000,
+	PISP_FE_ENABLE_COMPRESS1 = 0x400000,
+	PISP_FE_ENABLE_OUTPUT1 = 0x800000
+};
+
+#define PISP_FE_ENABLE_CROP(i) (PISP_FE_ENABLE_CROP0 << (4 * (i)))
+#define PISP_FE_ENABLE_DOWNSCALE(i) (PISP_FE_ENABLE_DOWNSCALE0 << (4 * (i)))
+#define PISP_FE_ENABLE_COMPRESS(i) (PISP_FE_ENABLE_COMPRESS0 << (4 * (i)))
+#define PISP_FE_ENABLE_OUTPUT(i) (PISP_FE_ENABLE_OUTPUT0 << (4 * (i)))
+
+/*
+ * We use the enable flags to show when blocks are "dirty", but we need some
+ * extra ones too.
+ */
+enum pisp_fe_dirty {
+	PISP_FE_DIRTY_GLOBAL = 0x0001,
+	PISP_FE_DIRTY_FLOATING = 0x0002,
+	PISP_FE_DIRTY_OUTPUT_AXI = 0x0004
+};
+
+struct pisp_fe_global_config {
+	u32 enables;
+	u8 bayer_order;
+	u8 pad[3];
+};
+
+struct pisp_fe_input_axi_config {
+	/* burst length minus one, in the range 0..15; OR'd with flags */
+	u8 maxlen_flags;
+	/* { prot[2:0], cache[3:0] } fields */
+	u8 cache_prot;
+	/* QoS (only 4 LS bits are used) */
+	u16 qos;
+};
+
+struct pisp_fe_output_axi_config {
+	/* burst length minus one, in the range 0..15; OR'd with flags */
+	u8 maxlen_flags;
+	/* { prot[2:0], cache[3:0] } fields */
+	u8 cache_prot;
+	/* QoS (4 bitfields of 4 bits each for different panic levels) */
+	u16 qos;
+	/*  For Panic mode: Output FIFO panic threshold */
+	u16 thresh;
+	/*  For Panic mode: Output FIFO statistics throttle threshold */
+	u16 throttle;
+};
+
+struct pisp_fe_input_config {
+	u8 streaming;
+	u8 pad[3];
+	struct pisp_image_format_config format;
+	struct pisp_fe_input_axi_config axi;
+	/* Extra cycles delay before issuing each burst request */
+	u8 holdoff;
+	u8 pad2[3];
+};
+
+struct pisp_fe_output_config {
+	struct pisp_image_format_config format;
+	u16 ilines;
+	u8 pad[2];
+};
+
+struct pisp_fe_input_buffer_config {
+	u32 addr_lo;
+	u32 addr_hi;
+	u16 frame_id;
+	u16 pad;
+};
+
+#define PISP_FE_DECOMPAND_LUT_SIZE 65
+
+struct pisp_fe_decompand_config {
+	u16 lut[PISP_FE_DECOMPAND_LUT_SIZE];
+	u16 pad;
+};
+
+struct pisp_fe_dpc_config {
+	u8 coeff_level;
+	u8 coeff_range;
+	u8 coeff_range2;
+#define PISP_FE_DPC_FLAG_FOLDBACK 1
+#define PISP_FE_DPC_FLAG_VFLAG 2
+	u8 flags;
+};
+
+#define PISP_FE_LSC_LUT_SIZE 16
+
+struct pisp_fe_lsc_config {
+	u8 shift;
+	u8 pad0;
+	u16 scale;
+	u16 centre_x;
+	u16 centre_y;
+	u16 lut[PISP_FE_LSC_LUT_SIZE];
+};
+
+struct pisp_fe_rgby_config {
+	u16 gain_r;
+	u16 gain_g;
+	u16 gain_b;
+	u8 maxflag;
+	u8 pad;
+};
+
+struct pisp_fe_agc_stats_config {
+	u16 offset_x;
+	u16 offset_y;
+	u16 size_x;
+	u16 size_y;
+	/* each weight only 4 bits */
+	u8 weights[PISP_AGC_STATS_NUM_ZONES / 2];
+	u16 row_offset_x;
+	u16 row_offset_y;
+	u16 row_size_x;
+	u16 row_size_y;
+	u8 row_shift;
+	u8 float_shift;
+	u8 pad1[2];
+};
+
+struct pisp_fe_awb_stats_config {
+	u16 offset_x;
+	u16 offset_y;
+	u16 size_x;
+	u16 size_y;
+	u8 shift;
+	u8 pad[3];
+	u16 r_lo;
+	u16 r_hi;
+	u16 g_lo;
+	u16 g_hi;
+	u16 b_lo;
+	u16 b_hi;
+};
+
+struct pisp_fe_floating_stats_region {
+	u16 offset_x;
+	u16 offset_y;
+	u16 size_x;
+	u16 size_y;
+};
+
+struct pisp_fe_floating_stats_config {
+	struct pisp_fe_floating_stats_region
+					regions[PISP_FLOATING_STATS_NUM_ZONES];
+};
+
+#define PISP_FE_CDAF_NUM_WEIGHTS 8
+
+struct pisp_fe_cdaf_stats_config {
+	u16 noise_constant;
+	u16 noise_slope;
+	u16 offset_x;
+	u16 offset_y;
+	u16 size_x;
+	u16 size_y;
+	u16 skip_x;
+	u16 skip_y;
+	u32 mode;
+};
+
+struct pisp_fe_stats_buffer_config {
+	u32 addr_lo;
+	u32 addr_hi;
+};
+
+struct pisp_fe_crop_config {
+	u16 offset_x;
+	u16 offset_y;
+	u16 width;
+	u16 height;
+};
+
+enum pisp_fe_downscale_flags {
+	DOWNSCALE_BAYER =
+		1, /* downscale the four Bayer components independently... */
+	DOWNSCALE_BIN =
+		2 /* ...without trying to preserve their spatial relationship */
+};
+
+struct pisp_fe_downscale_config {
+	u8 xin;
+	u8 xout;
+	u8 yin;
+	u8 yout;
+	u8 flags; /* enum pisp_fe_downscale_flags */
+	u8 pad[3];
+	u16 output_width;
+	u16 output_height;
+};
+
+struct pisp_fe_output_buffer_config {
+	u32 addr_lo;
+	u32 addr_hi;
+};
+
+/* Each of the two output channels/branches: */
+struct pisp_fe_output_branch_config {
+	struct pisp_fe_crop_config crop;
+	struct pisp_fe_downscale_config downscale;
+	struct pisp_compress_config compress;
+	struct pisp_fe_output_config output;
+	u32 pad;
+};
+
+/* And finally one to rule them all: */
+struct pisp_fe_config {
+	/* I/O configuration: */
+	struct pisp_fe_stats_buffer_config stats_buffer;
+	struct pisp_fe_output_buffer_config output_buffer[PISP_FE_NUM_OUTPUTS];
+	struct pisp_fe_input_buffer_config input_buffer;
+	/* processing configuration: */
+	struct pisp_fe_global_config global;
+	struct pisp_fe_input_config input;
+	struct pisp_decompress_config decompress;
+	struct pisp_fe_decompand_config decompand;
+	struct pisp_bla_config bla;
+	struct pisp_fe_dpc_config dpc;
+	struct pisp_fe_crop_config stats_crop;
+	u32 spare1; /* placeholder for future decimate configuration */
+	struct pisp_bla_config blc;
+	struct pisp_fe_rgby_config rgby;
+	struct pisp_fe_lsc_config lsc;
+	struct pisp_fe_agc_stats_config agc_stats;
+	struct pisp_fe_awb_stats_config awb_stats;
+	struct pisp_fe_cdaf_stats_config cdaf_stats;
+	struct pisp_fe_floating_stats_config floating_stats;
+	struct pisp_fe_output_axi_config output_axi;
+	struct pisp_fe_output_branch_config ch[PISP_FE_NUM_OUTPUTS];
+	/* non-register fields: */
+	u32 dirty_flags; /* these use pisp_fe_enable */
+	u32 dirty_flags_extra; /* these use pisp_fe_dirty */
+};
+
+#endif /* _PISP_FE_CONFIG_ */
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1_cfe/pisp_statistics.h
@@ -0,0 +1,62 @@
+/* SPDX-License-Identifier: GPL-2.0-only WITH Linux-syscall-note */
+/*
+ * RP1 PiSP Front End statistics definitions
+ *
+ * Copyright (C) 2021 - Raspberry Pi Ltd.
+ *
+ */
+#ifndef _PISP_FE_STATISTICS_H_
+#define _PISP_FE_STATISTICS_H_
+
+#define PISP_FLOATING_STATS_NUM_ZONES 4
+#define PISP_AGC_STATS_NUM_BINS 1024
+#define PISP_AGC_STATS_SIZE 16
+#define PISP_AGC_STATS_NUM_ZONES (PISP_AGC_STATS_SIZE * PISP_AGC_STATS_SIZE)
+#define PISP_AGC_STATS_NUM_ROW_SUMS 512
+
+struct pisp_agc_statistics_zone {
+	u64 Y_sum;
+	u32 counted;
+	u32 pad;
+};
+
+struct pisp_agc_statistics {
+	u32 row_sums[PISP_AGC_STATS_NUM_ROW_SUMS];
+	/*
+	 * 32-bits per bin means an image (just less than) 16384x16384 pixels
+	 * in size can weight every pixel from 0 to 15.
+	 */
+	u32 histogram[PISP_AGC_STATS_NUM_BINS];
+	struct pisp_agc_statistics_zone floating[PISP_FLOATING_STATS_NUM_ZONES];
+};
+
+#define PISP_AWB_STATS_SIZE 32
+#define PISP_AWB_STATS_NUM_ZONES (PISP_AWB_STATS_SIZE * PISP_AWB_STATS_SIZE)
+
+struct pisp_awb_statistics_zone {
+	u32 R_sum;
+	u32 G_sum;
+	u32 B_sum;
+	u32 counted;
+};
+
+struct pisp_awb_statistics {
+	struct pisp_awb_statistics_zone zones[PISP_AWB_STATS_NUM_ZONES];
+	struct pisp_awb_statistics_zone floating[PISP_FLOATING_STATS_NUM_ZONES];
+};
+
+#define PISP_CDAF_STATS_SIZE 8
+#define PISP_CDAF_STATS_NUM_FOMS (PISP_CDAF_STATS_SIZE * PISP_CDAF_STATS_SIZE)
+
+struct pisp_cdaf_statistics {
+	u64 foms[PISP_CDAF_STATS_NUM_FOMS];
+	u64 floating[PISP_FLOATING_STATS_NUM_ZONES];
+};
+
+struct pisp_statistics {
+	struct pisp_awb_statistics awb;
+	struct pisp_agc_statistics agc;
+	struct pisp_cdaf_statistics cdaf;
+};
+
+#endif /* _PISP_FE_STATISTICS_H_ */
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1_cfe/pisp_types.h
@@ -0,0 +1,144 @@
+/* SPDX-License-Identifier: GPL-2.0-only WITH Linux-syscall-note */
+/*
+ * RP1 PiSP Front End image definitions.
+ *
+ * Copyright (C) 2021 - Raspberry Pi Ltd.
+ *
+ */
+#ifndef _PISP_FE_TYPES_H_
+#define _PISP_FE_TYPES_H_
+
+/* This definition must match the format description in the hardware exactly! */
+struct pisp_image_format_config {
+	/* size in pixels */
+	u16 width, height;
+	/* must match struct pisp_image_format below */
+	u32 format;
+	s32 stride;
+	/* some planar image formats will need a second stride */
+	s32 stride2;
+};
+
+static_assert(sizeof(struct pisp_image_format_config) == 16);
+
+enum pisp_bayer_order {
+	/*
+	 * Note how bayer_order&1 tells you if G is on the even pixels of the
+	 * checkerboard or not, and bayer_order&2 tells you if R is on the even
+	 * rows or is swapped with B. Note that if the top (of the 8) bits is
+	 * set, this denotes a monochrome or greyscale image, and the lower bits
+	 * should all be ignored.
+	 */
+	PISP_BAYER_ORDER_RGGB = 0,
+	PISP_BAYER_ORDER_GBRG = 1,
+	PISP_BAYER_ORDER_BGGR = 2,
+	PISP_BAYER_ORDER_GRBG = 3,
+	PISP_BAYER_ORDER_GREYSCALE = 128
+};
+
+enum pisp_image_format {
+	/*
+	 * Precise values are mostly tbd. Generally these will be portmanteau
+	 * values comprising bit fields and flags. This format must be shared
+	 * throughout the PiSP.
+	 */
+	PISP_IMAGE_FORMAT_BPS_8 = 0x00000000,
+	PISP_IMAGE_FORMAT_BPS_10 = 0x00000001,
+	PISP_IMAGE_FORMAT_BPS_12 = 0x00000002,
+	PISP_IMAGE_FORMAT_BPS_16 = 0x00000003,
+	PISP_IMAGE_FORMAT_BPS_MASK = 0x00000003,
+
+	PISP_IMAGE_FORMAT_PLANARITY_INTERLEAVED = 0x00000000,
+	PISP_IMAGE_FORMAT_PLANARITY_SEMI_PLANAR = 0x00000010,
+	PISP_IMAGE_FORMAT_PLANARITY_PLANAR = 0x00000020,
+	PISP_IMAGE_FORMAT_PLANARITY_MASK = 0x00000030,
+
+	PISP_IMAGE_FORMAT_SAMPLING_444 = 0x00000000,
+	PISP_IMAGE_FORMAT_SAMPLING_422 = 0x00000100,
+	PISP_IMAGE_FORMAT_SAMPLING_420 = 0x00000200,
+	PISP_IMAGE_FORMAT_SAMPLING_MASK = 0x00000300,
+
+	PISP_IMAGE_FORMAT_ORDER_NORMAL = 0x00000000,
+	PISP_IMAGE_FORMAT_ORDER_SWAPPED = 0x00001000,
+
+	PISP_IMAGE_FORMAT_SHIFT_0 = 0x00000000,
+	PISP_IMAGE_FORMAT_SHIFT_1 = 0x00010000,
+	PISP_IMAGE_FORMAT_SHIFT_2 = 0x00020000,
+	PISP_IMAGE_FORMAT_SHIFT_3 = 0x00030000,
+	PISP_IMAGE_FORMAT_SHIFT_4 = 0x00040000,
+	PISP_IMAGE_FORMAT_SHIFT_5 = 0x00050000,
+	PISP_IMAGE_FORMAT_SHIFT_6 = 0x00060000,
+	PISP_IMAGE_FORMAT_SHIFT_7 = 0x00070000,
+	PISP_IMAGE_FORMAT_SHIFT_8 = 0x00080000,
+	PISP_IMAGE_FORMAT_SHIFT_MASK = 0x000f0000,
+
+	PISP_IMAGE_FORMAT_UNCOMPRESSED = 0x00000000,
+	PISP_IMAGE_FORMAT_COMPRESSION_MODE_1 = 0x01000000,
+	PISP_IMAGE_FORMAT_COMPRESSION_MODE_2 = 0x02000000,
+	PISP_IMAGE_FORMAT_COMPRESSION_MODE_3 = 0x03000000,
+	PISP_IMAGE_FORMAT_COMPRESSION_MASK = 0x03000000,
+
+	PISP_IMAGE_FORMAT_HOG_SIGNED = 0x04000000,
+	PISP_IMAGE_FORMAT_HOG_UNSIGNED = 0x08000000,
+	PISP_IMAGE_FORMAT_INTEGRAL_IMAGE = 0x10000000,
+	PISP_IMAGE_FORMAT_WALLPAPER_ROLL = 0x20000000,
+	PISP_IMAGE_FORMAT_THREE_CHANNEL = 0x40000000,
+
+	/* Lastly a few specific instantiations of the above. */
+	PISP_IMAGE_FORMAT_SINGLE_16 = PISP_IMAGE_FORMAT_BPS_16,
+	PISP_IMAGE_FORMAT_THREE_16 =
+		PISP_IMAGE_FORMAT_BPS_16 | PISP_IMAGE_FORMAT_THREE_CHANNEL
+};
+
+#define PISP_IMAGE_FORMAT_bps_8(fmt)                                           \
+	(((fmt) & PISP_IMAGE_FORMAT_BPS_MASK) == PISP_IMAGE_FORMAT_BPS_8)
+#define PISP_IMAGE_FORMAT_bps_10(fmt)                                          \
+	(((fmt) & PISP_IMAGE_FORMAT_BPS_MASK) == PISP_IMAGE_FORMAT_BPS_10)
+#define PISP_IMAGE_FORMAT_bps_12(fmt)                                          \
+	(((fmt) & PISP_IMAGE_FORMAT_BPS_MASK) == PISP_IMAGE_FORMAT_BPS_12)
+#define PISP_IMAGE_FORMAT_bps_16(fmt)                                          \
+	(((fmt) & PISP_IMAGE_FORMAT_BPS_MASK) == PISP_IMAGE_FORMAT_BPS_16)
+#define PISP_IMAGE_FORMAT_bps(fmt)                                             \
+	(((fmt) & PISP_IMAGE_FORMAT_BPS_MASK) ?                                \
+		       8 + (2 << (((fmt) & PISP_IMAGE_FORMAT_BPS_MASK) - 1)) : \
+		       8)
+#define PISP_IMAGE_FORMAT_shift(fmt)                                           \
+	(((fmt) & PISP_IMAGE_FORMAT_SHIFT_MASK) / PISP_IMAGE_FORMAT_SHIFT_1)
+#define PISP_IMAGE_FORMAT_three_channel(fmt)                                   \
+	((fmt) & PISP_IMAGE_FORMAT_THREE_CHANNEL)
+#define PISP_IMAGE_FORMAT_single_channel(fmt)                                  \
+	(!((fmt) & PISP_IMAGE_FORMAT_THREE_CHANNEL))
+#define PISP_IMAGE_FORMAT_compressed(fmt)                                      \
+	(((fmt) & PISP_IMAGE_FORMAT_COMPRESSION_MASK) !=                       \
+	 PISP_IMAGE_FORMAT_UNCOMPRESSED)
+#define PISP_IMAGE_FORMAT_sampling_444(fmt)                                    \
+	(((fmt) & PISP_IMAGE_FORMAT_SAMPLING_MASK) ==                          \
+	 PISP_IMAGE_FORMAT_SAMPLING_444)
+#define PISP_IMAGE_FORMAT_sampling_422(fmt)                                    \
+	(((fmt) & PISP_IMAGE_FORMAT_SAMPLING_MASK) ==                          \
+	 PISP_IMAGE_FORMAT_SAMPLING_422)
+#define PISP_IMAGE_FORMAT_sampling_420(fmt)                                    \
+	(((fmt) & PISP_IMAGE_FORMAT_SAMPLING_MASK) ==                          \
+	 PISP_IMAGE_FORMAT_SAMPLING_420)
+#define PISP_IMAGE_FORMAT_order_normal(fmt)                                    \
+	(!((fmt) & PISP_IMAGE_FORMAT_ORDER_SWAPPED))
+#define PISP_IMAGE_FORMAT_order_swapped(fmt)                                   \
+	((fmt) & PISP_IMAGE_FORMAT_ORDER_SWAPPED)
+#define PISP_IMAGE_FORMAT_interleaved(fmt)                                     \
+	(((fmt) & PISP_IMAGE_FORMAT_PLANARITY_MASK) ==                         \
+	 PISP_IMAGE_FORMAT_PLANARITY_INTERLEAVED)
+#define PISP_IMAGE_FORMAT_semiplanar(fmt)                                      \
+	(((fmt) & PISP_IMAGE_FORMAT_PLANARITY_MASK) ==                         \
+	 PISP_IMAGE_FORMAT_PLANARITY_SEMI_PLANAR)
+#define PISP_IMAGE_FORMAT_planar(fmt)                                          \
+	(((fmt) & PISP_IMAGE_FORMAT_PLANARITY_MASK) ==                         \
+	 PISP_IMAGE_FORMAT_PLANARITY_PLANAR)
+#define PISP_IMAGE_FORMAT_wallpaper(fmt)                                       \
+	((fmt) & PISP_IMAGE_FORMAT_WALLPAPER_ROLL)
+#define PISP_IMAGE_FORMAT_HOG(fmt)                                             \
+	((fmt) &                                                               \
+	 (PISP_IMAGE_FORMAT_HOG_SIGNED | PISP_IMAGE_FORMAT_HOG_UNSIGNED))
+
+#define PISP_WALLPAPER_WIDTH 128 // in bytes
+
+#endif /* _PISP_FE_TYPES_H_ */
