diff --git a/drivers/media/platform/raspberrypi/rp1_cfe/cfe.c b/drivers/media/platform/raspberrypi/rp1_cfe/cfe.c
index e096869a86209b..05c3009f5aa045 100644
--- a/drivers/media/platform/raspberrypi/rp1_cfe/cfe.c
+++ b/drivers/media/platform/raspberrypi/rp1_cfe/cfe.c
@@ -49,11 +49,13 @@
 #define CFE_MODULE_NAME	"rp1-cfe"
 #define CFE_VERSION	"1.0"
 
-bool cfe_debug_irq;
+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_irq(fmt, arg...)                              \
+#define cfe_dbg_verbose(fmt, arg...)                          \
 	do {                                                  \
-		if (cfe_debug_irq)                            \
+		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)
@@ -108,6 +110,7 @@ const struct v4l2_mbus_framefmt cfe_default_meta_format = {
 	.width = 8192,
 	.height = 1,
 	.code = MEDIA_BUS_FMT_SENSOR_DATA,
+	.field = V4L2_FIELD_NONE,
 };
 
 enum node_ids {
@@ -290,7 +293,6 @@ struct cfe_device {
 	struct csi2_device csi2;
 	struct pisp_fe_device fe;
 
-	bool sensor_embedded_data;
 	int fe_csi2_channel;
 
 	unsigned int sequence;
@@ -518,8 +520,8 @@ static void cfe_schedule_next_csi2_job(struct cfe_device *cfe)
 		node->next_frm = buf;
 		list_del(&buf->list);
 
-		cfe_dbg("%s: [%s] buffer:%p\n",
-			__func__, node_desc[node->id].name, &buf->vb.vb2_buf);
+		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->fmt.fmt.meta.buffersize;
@@ -550,8 +552,8 @@ static void cfe_schedule_next_pisp_job(struct cfe_device *cfe)
 		buf = list_first_entry(&node->dma_queue, struct cfe_buffer,
 				       list);
 
-		cfe_dbg_irq("%s: [%s] buffer:%p\n", __func__,
-			    node_desc[node->id].name, &buf->vb.vb2_buf);
+		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;
@@ -573,8 +575,8 @@ static bool cfe_check_job_ready(struct cfe_device *cfe)
 			continue;
 
 		if (list_empty(&node->dma_queue)) {
-			cfe_dbg_irq("%s: [%s] has no buffer, unable to schedule job\n",
-				    __func__, node_desc[i].name);
+			cfe_dbg_verbose("%s: [%s] has no buffer, unable to schedule job\n",
+				__func__, node_desc[i].name);
 			return false;
 		}
 	}
@@ -592,7 +594,7 @@ static void cfe_prepare_next_job(struct cfe_device *cfe)
 	/* Flag if another job is ready after this. */
 	cfe->job_ready = cfe_check_job_ready(cfe);
 
-	cfe_dbg_irq("%s: end with scheduled job\n", __func__);
+	cfe_dbg_verbose("%s: end with scheduled job\n", __func__);
 }
 
 static void cfe_process_buffer_complete(struct cfe_node *node,
@@ -600,8 +602,8 @@ static void cfe_process_buffer_complete(struct cfe_node *node,
 {
 	struct cfe_device *cfe = node->cfe;
 
-	cfe_dbg_irq("%s: [%s] buffer:%p\n", __func__, node_desc[node->id].name,
-		    &node->cur_frm->vb.vb2_buf);
+	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 = sequence;
 	vb2_buffer_done(&node->cur_frm->vb.vb2_buf, VB2_BUF_STATE_DONE);
@@ -621,8 +623,8 @@ static void cfe_sof_isr_handler(struct cfe_node *node)
 {
 	struct cfe_device *cfe = node->cfe;
 
-	cfe_dbg_irq("%s: [%s] seq %u\n", __func__, node_desc[node->id].name,
-		    cfe->sequence);
+	cfe_dbg_verbose("%s: [%s] seq %u\n", __func__, node_desc[node->id].name,
+			cfe->sequence);
 
 	node->cur_frm = node->next_frm;
 	node->next_frm = NULL;
@@ -651,8 +653,8 @@ static void cfe_eof_isr_handler(struct cfe_node *node)
 {
 	struct cfe_device *cfe = node->cfe;
 
-	cfe_dbg_irq("%s: [%s] seq %u\n", __func__, node_desc[node->id].name,
-		    cfe->sequence);
+	cfe_dbg_verbose("%s: [%s] seq %u\n", __func__, node_desc[node->id].name,
+			cfe->sequence);
 
 	if (node->cur_frm)
 		cfe_process_buffer_complete(node, cfe->sequence);
@@ -763,20 +765,16 @@ static void cfe_start_channel(struct cfe_node *node)
 	struct v4l2_mbus_framefmt *source_fmt;
 	const struct cfe_fmt *fmt;
 	unsigned long flags;
-	unsigned int width = 0, height = 0;
 	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);
 
-	if (start_fe || is_image_output_node(node)) {
-		width = node->fmt.fmt.pix.width;
-		height = node->fmt.fmt.pix.height;
-	}
-
 	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,
@@ -785,6 +783,9 @@ static void cfe_start_channel(struct cfe_node *node)
 		source_fmt = v4l2_subdev_get_pad_format(&cfe->csi2.sd, state, cfe->fe_csi2_channel);
 		fmt = find_format_by_code(source_fmt->code);
 
+		width = source_fmt->width;
+		height = source_fmt->height;
+
 		/*
 		 * Start the associated CSI2 Channel as well.
 		 *
@@ -800,6 +801,8 @@ static void cfe_start_channel(struct cfe_node *node)
 	}
 
 	if (is_csi2_node(node)) {
+		unsigned int width = 0, height = 0;
+
 		u32 mode = CSI2_MODE_NORMAL;
 
 		source_fmt = v4l2_subdev_get_pad_format(&cfe->csi2.sd, state,
@@ -807,6 +810,9 @@ static void cfe_start_channel(struct cfe_node *node)
 		fmt = find_format_by_code(source_fmt->code);
 
 		if (is_image_output_node(node)) {
+			width = source_fmt->width;
+			height = source_fmt->height;
+
 			if (node->fmt.fmt.pix.pixelformat ==
 					fmt->remap[CFE_REMAP_16BIT])
 				mode = CSI2_MODE_REMAP;
@@ -917,8 +923,8 @@ static int cfe_buffer_prepare(struct vb2_buffer *vb)
 	struct cfe_buffer *buf = to_cfe_buffer(vb);
 	unsigned long size;
 
-	cfe_dbg_irq("%s: [%s] buffer:%p\n", __func__, node_desc[node->id].name,
-		    vb);
+	cfe_dbg_verbose("%s: [%s] buffer:%p\n", __func__,
+			node_desc[node->id].name, vb);
 
 	size = is_image_output_node(node) ? node->fmt.fmt.pix.sizeimage :
 					    node->fmt.fmt.meta.buffersize;
@@ -950,8 +956,8 @@ static void cfe_buffer_queue(struct vb2_buffer *vb)
 	struct cfe_buffer *buf = to_cfe_buffer(vb);
 	unsigned long flags;
 
-	cfe_dbg_irq("%s: [%s] buffer:%p\n", __func__, node_desc[node->id].name,
-		    vb);
+	cfe_dbg_verbose("%s: [%s] buffer:%p\n", __func__,
+			node_desc[node->id].name, vb);
 
 	spin_lock_irqsave(&cfe->state_lock, flags);
 
@@ -991,6 +997,14 @@ static int cfe_start_streaming(struct vb2_queue *vq, unsigned int count)
 		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_streaming;
+	}
+
 	ret = media_pipeline_start(&node->pad, &cfe->pipe);
 	if (ret < 0) {
 		cfe_err("Failed to start media pipeline: %d\n", ret);
@@ -1815,8 +1829,6 @@ static int cfe_probe_complete(struct cfe_device *cfe)
 
 	cfe->v4l2_dev.notify = cfe_notify;
 
-	cfe->sensor_embedded_data = (cfe->sensor->entity.num_pads >= 2);
-
 	for (i = 0; i < NUM_NODES; i++) {
 		ret = cfe_register_node(cfe, i);
 		if (ret) {
@@ -1837,17 +1849,10 @@ static int cfe_probe_complete(struct cfe_device *cfe)
 		goto unregister;
 	}
 
-	/*
-	 * Release the initial reference, all references are now owned by the
-	 * video devices.
-	 */
-	cfe_put(cfe);
 	return 0;
 
 unregister:
 	cfe_unregister_nodes(cfe);
-	cfe_put(cfe);
-
 	return ret;
 }
 
@@ -2129,6 +2134,8 @@ static int cfe_remove(struct platform_device *pdev)
 
 	v4l2_device_unregister(&cfe->v4l2_dev);
 
+	cfe_put(cfe);
+
 	return 0;
 }
 
diff --git a/drivers/media/platform/raspberrypi/rp1_cfe/cfe.h b/drivers/media/platform/raspberrypi/rp1_cfe/cfe.h
index 28e01be71544bd..6d3baecf7df631 100644
--- a/drivers/media/platform/raspberrypi/rp1_cfe/cfe.h
+++ b/drivers/media/platform/raspberrypi/rp1_cfe/cfe.h
@@ -11,7 +11,7 @@
 #include <linux/media-bus-format.h>
 #include <linux/videodev2.h>
 
-extern bool cfe_debug_irq;
+extern bool cfe_debug_verbose;
 
 enum cfe_remap_types {
 	CFE_REMAP_16BIT,
diff --git a/drivers/media/platform/raspberrypi/rp1_cfe/csi2.c b/drivers/media/platform/raspberrypi/rp1_cfe/csi2.c
index c3847e962a4ca1..898a35e273d961 100644
--- a/drivers/media/platform/raspberrypi/rp1_cfe/csi2.c
+++ b/drivers/media/platform/raspberrypi/rp1_cfe/csi2.c
@@ -16,9 +16,13 @@
 #include "csi2.h"
 #include "cfe.h"
 
-#define csi2_dbg_irq(fmt, arg...)                                 \
+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_irq)                                \
+		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)
@@ -32,9 +36,28 @@
 #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)
@@ -92,6 +115,7 @@ static inline u32 csi2_reg_read(struct csi2_device *csi2, u32 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)
@@ -148,13 +172,99 @@ static int csi2_regs_show(struct seq_file *s, void *data)
 
 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, bool *lci)
 {
 	unsigned int i;
 	u32 status;
 
 	status = csi2_reg_read(csi2, CSI2_STATUS);
-	csi2_dbg_irq("ISR: STA: 0x%x\n", status);
+	csi2_dbg_verbose("ISR: STA: 0x%x\n", status);
 
 	/* Write value back to clear the interrupts */
 	csi2_reg_write(csi2, CSI2_STATUS, status);
@@ -167,21 +277,24 @@ void csi2_isr(struct csi2_device *csi2, bool *sof, bool *eof, bool *lci)
 
 		dbg = csi2_reg_read(csi2, CSI2_CH_DEBUG(i));
 
-		csi2_dbg_irq("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);
+		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));
 		lci[i] = !!(status & IRQ_LE_ACK(i));
 	}
+
+	if (csi2_track_errors)
+		csi2_isr_handle_errors(csi2, status);
 }
 
 void csi2_set_buffer(struct csi2_device *csi2, unsigned int channel,
@@ -253,6 +366,7 @@ void csi2_start_channel(struct csi2_device *csi2, unsigned int channel,
 		 */
 		set_field(&ctrl, 0x3ff, LC_MASK);
 		set_field(&ctrl, 0x00, CH_MODE_MASK);
+		csi2_reg_write(csi2, CSI2_CH_FRAME_SIZE(channel), 0);
 	}
 
 	set_field(&ctrl, dt, DT_MASK);
@@ -275,15 +389,20 @@ void csi2_stop_channel(struct csi2_device *csi2, unsigned int channel)
 
 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);
 
-	if (!csi2->multipacket_line)
-		csi2_reg_write(csi2, CSI2_CTRL, EOP_IS_EOL);
+	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 struct csi2_device *to_csi2_device(struct v4l2_subdev *subdev)
@@ -396,11 +515,17 @@ 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;
diff --git a/drivers/media/platform/raspberrypi/rp1_cfe/csi2.h b/drivers/media/platform/raspberrypi/rp1_cfe/csi2.h
index 94d43f0d8828e0..e55e0817f4e18c 100644
--- a/drivers/media/platform/raspberrypi/rp1_cfe/csi2.h
+++ b/drivers/media/platform/raspberrypi/rp1_cfe/csi2.h
@@ -17,11 +17,13 @@
 
 #define CSI2_NUM_CHANNELS 4
 
+#define DISCARDS_TABLE_NUM_VCS 4
+
 enum csi2_mode {
-	CSI2_MODE_NORMAL,
-	CSI2_MODE_REMAP,
-	CSI2_MODE_COMPRESSED,
-	CSI2_MODE_FE_STREAMING
+	CSI2_MODE_NORMAL = 0,
+	CSI2_MODE_REMAP = 1,
+	CSI2_MODE_COMPRESSED = 2,
+	CSI2_MODE_FE_STREAMING = 3,
 };
 
 enum csi2_compression_mode {
@@ -37,6 +39,14 @@ struct csi2_cfg {
 	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;
@@ -53,6 +63,12 @@ struct csi2_device {
 
 	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, bool *lci);
diff --git a/drivers/media/platform/raspberrypi/rp1_cfe/pisp_fe.c b/drivers/media/platform/raspberrypi/rp1_cfe/pisp_fe.c
index 91a3b5f6e53c4b..31fb86067adf78 100644
--- a/drivers/media/platform/raspberrypi/rp1_cfe/pisp_fe.c
+++ b/drivers/media/platform/raspberrypi/rp1_cfe/pisp_fe.c
@@ -114,9 +114,9 @@ static const struct pisp_fe_config_param pisp_fe_config_map[] = {
 					sizeof(struct pisp_fe_output_config)         },
 };
 
-#define pisp_fe_dbg_irq(fmt, arg...)                            \
+#define pisp_fe_dbg_verbose(fmt, arg...)                        \
 	do {                                                    \
-		if (cfe_debug_irq)                              \
+		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)
@@ -132,12 +132,14 @@ 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)
@@ -202,9 +204,9 @@ void pisp_fe_isr(struct pisp_fe_device *fe, bool *sof, bool *eof)
 	int_status = pisp_fe_reg_read(fe, FE_INT_STATUS);
 	pisp_fe_reg_write(fe, FE_INT_STATUS, int_status);
 
-	pisp_fe_dbg_irq("%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);
+	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++) {
@@ -339,7 +341,7 @@ void pisp_fe_submit_job(struct pisp_fe_device *fe, struct vb2_buffer **vb2_bufs,
 	 * sequence of relaxed writes which follow.
 	 */
 	status = pisp_fe_reg_read(fe, FE_STATUS);
-	pisp_fe_dbg_irq("%s: status = 0x%x\n", __func__, status);
+	pisp_fe_dbg_verbose("%s: status = 0x%x\n", __func__, status);
 	if (WARN_ON(status & FE_STATUS_QUEUED))
 		return;
 
@@ -372,7 +374,7 @@ void pisp_fe_submit_job(struct pisp_fe_device *fe, struct vb2_buffer **vb2_bufs,
 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, -1);
+	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;
 }
@@ -383,7 +385,7 @@ void pisp_fe_stop(struct pisp_fe_device *fe)
 	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, -1);
+	pisp_fe_reg_write(fe, FE_INT_STATUS, ~0);
 }
 
 static struct pisp_fe_device *to_pisp_fe_device(struct v4l2_subdev *subdev)
@@ -435,7 +437,7 @@ static int pisp_fe_pad_set_fmt(struct v4l2_subdev *sd,
 	case FE_OUTPUT1_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_SBGGR10_1X10);
+			cfe_fmt = find_format_by_code(MEDIA_BUS_FMT_SRGGB16_1X16);
 
 		format->format.code = cfe_fmt->code;