These changes are the raw update to linux-4.4.6-rt14. Kernel sources

are taken from kernel.org, and rt patch from the rt wiki download page.

During the rebasing, the following patch collided:

Force tick interrupt and get rid of softirq magic(I70131fb85).

Collisions have been removed because its logic was found on the
source already.

Change-Id: I7f57a4081d9deaa0d9ccfc41a6c8daccdee3b769
Signed-off-by: José Pekkarinen <jose.pekkarinen@nokia.com>

17 files changed, 6203 insertions, 0 deletions

diff --git a/kernel/drivers/media/platform/sti/bdisp/Makefile b/kernel/drivers/media/platform/sti/bdisp/Makefile
new file mode 100644
index 000000000..bc53496fa
--- /dev/null
+++ b/kernel/drivers/media/platform/sti/bdisp/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_VIDEO_STI_BDISP) := bdisp.o
+
+bdisp-objs := bdisp-v4l2.o bdisp-hw.o bdisp-debug.o
diff --git a/kernel/drivers/media/platform/sti/bdisp/bdisp-debug.c b/kernel/drivers/media/platform/sti/bdisp/bdisp-debug.c
new file mode 100644
index 000000000..79c56356a
--- /dev/null
+++ b/kernel/drivers/media/platform/sti/bdisp/bdisp-debug.c
@@ -0,0 +1,687 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2014
+ * Authors: Fabien Dessenne <fabien.dessenne@st.com> for STMicroelectronics.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#include <linux/debugfs.h>
+#include <linux/pm_runtime.h>
+
+#include "bdisp.h"
+#include "bdisp-filter.h"
+#include "bdisp-reg.h"
+
+void bdisp_dbg_perf_begin(struct bdisp_dev *bdisp)
+{
+	bdisp->dbg.hw_start = ktime_get();
+}
+
+void bdisp_dbg_perf_end(struct bdisp_dev *bdisp)
+{
+	s64 time_us;
+
+	time_us = ktime_us_delta(ktime_get(), bdisp->dbg.hw_start);
+
+	if (!bdisp->dbg.min_duration)
+		bdisp->dbg.min_duration = time_us;
+	else
+		bdisp->dbg.min_duration = min(time_us, bdisp->dbg.min_duration);
+
+	bdisp->dbg.last_duration = time_us;
+	bdisp->dbg.max_duration = max(time_us, bdisp->dbg.max_duration);
+	bdisp->dbg.tot_duration += time_us;
+}
+
+static void bdisp_dbg_dump_ins(struct seq_file *s, u32 val)
+{
+	seq_printf(s, "INS\t0x%08X\t", val);
+
+	switch (val & BLT_INS_S1_MASK) {
+	case BLT_INS_S1_OFF:
+		break;
+	case BLT_INS_S1_MEM:
+		seq_puts(s, "SRC1=mem - ");
+		break;
+	case BLT_INS_S1_CF:
+		seq_puts(s, "SRC1=ColorFill - ");
+		break;
+	case BLT_INS_S1_COPY:
+		seq_puts(s, "SRC1=copy - ");
+		break;
+	case BLT_INS_S1_FILL:
+		seq_puts(s, "SRC1=fil - ");
+		break;
+	default:
+		seq_puts(s, "SRC1=??? - ");
+		break;
+	}
+
+	switch (val & BLT_INS_S2_MASK) {
+	case BLT_INS_S2_OFF:
+		break;
+	case BLT_INS_S2_MEM:
+		seq_puts(s, "SRC2=mem - ");
+		break;
+	case BLT_INS_S2_CF:
+		seq_puts(s, "SRC2=ColorFill - ");
+		break;
+	default:
+		seq_puts(s, "SRC2=??? - ");
+		break;
+	}
+
+	if ((val & BLT_INS_S3_MASK) == BLT_INS_S3_MEM)
+		seq_puts(s, "SRC3=mem - ");
+
+	if (val & BLT_INS_IVMX)
+		seq_puts(s, "IVMX - ");
+	if (val & BLT_INS_CLUT)
+		seq_puts(s, "CLUT - ");
+	if (val & BLT_INS_SCALE)
+		seq_puts(s, "Scale - ");
+	if (val & BLT_INS_FLICK)
+		seq_puts(s, "Flicker - ");
+	if (val & BLT_INS_CLIP)
+		seq_puts(s, "Clip - ");
+	if (val & BLT_INS_CKEY)
+		seq_puts(s, "ColorKey - ");
+	if (val & BLT_INS_OVMX)
+		seq_puts(s, "OVMX - ");
+	if (val & BLT_INS_DEI)
+		seq_puts(s, "Deint - ");
+	if (val & BLT_INS_PMASK)
+		seq_puts(s, "PlaneMask - ");
+	if (val & BLT_INS_VC1R)
+		seq_puts(s, "VC1R - ");
+	if (val & BLT_INS_ROTATE)
+		seq_puts(s, "Rotate - ");
+	if (val & BLT_INS_GRAD)
+		seq_puts(s, "GradFill - ");
+	if (val & BLT_INS_AQLOCK)
+		seq_puts(s, "AQLock - ");
+	if (val & BLT_INS_PACE)
+		seq_puts(s, "Pace - ");
+	if (val & BLT_INS_IRQ)
+		seq_puts(s, "IRQ - ");
+
+	seq_puts(s, "\n");
+}
+
+static void bdisp_dbg_dump_tty(struct seq_file *s, u32 val)
+{
+	seq_printf(s, "TTY\t0x%08X\t", val);
+	seq_printf(s, "Pitch=%d - ", val & 0xFFFF);
+
+	switch ((val & BLT_TTY_COL_MASK) >> BLT_TTY_COL_SHIFT) {
+	case BDISP_RGB565:
+		seq_puts(s, "RGB565 - ");
+		break;
+	case BDISP_RGB888:
+		seq_puts(s, "RGB888 - ");
+		break;
+	case BDISP_XRGB8888:
+		seq_puts(s, "xRGB888 - ");
+		break;
+	case BDISP_ARGB8888:
+		seq_puts(s, "ARGB8888 - ");
+		break;
+	case BDISP_NV12:
+		seq_puts(s, "NV12 - ");
+		break;
+	case BDISP_YUV_3B:
+		seq_puts(s, "YUV420P - ");
+		break;
+	default:
+		seq_puts(s, "ColorFormat ??? - ");
+		break;
+	}
+
+	if (val & BLT_TTY_ALPHA_R)
+		seq_puts(s, "AlphaRange - ");
+	if (val & BLT_TTY_CR_NOT_CB)
+		seq_puts(s, "CrNotCb - ");
+	if (val & BLT_TTY_MB)
+		seq_puts(s, "MB - ");
+	if (val & BLT_TTY_HSO)
+		seq_puts(s, "HSO inverse - ");
+	if (val & BLT_TTY_VSO)
+		seq_puts(s, "VSO inverse - ");
+	if (val & BLT_TTY_DITHER)
+		seq_puts(s, "Dither - ");
+	if (val & BLT_TTY_CHROMA)
+		seq_puts(s, "Write CHROMA - ");
+	if (val & BLT_TTY_BIG_END)
+		seq_puts(s, "BigEndian - ");
+
+	seq_puts(s, "\n");
+}
+
+static void bdisp_dbg_dump_xy(struct seq_file *s, u32 val, char *name)
+{
+	seq_printf(s, "%s\t0x%08X\t", name, val);
+	seq_printf(s, "(%d,%d)\n", val & 0xFFFF, (val >> 16));
+}
+
+static void bdisp_dbg_dump_sz(struct seq_file *s, u32 val, char *name)
+{
+	seq_printf(s, "%s\t0x%08X\t", name, val);
+	seq_printf(s, "%dx%d\n", val & 0x1FFF, (val >> 16) & 0x1FFF);
+}
+
+static void bdisp_dbg_dump_sty(struct seq_file *s,
+			       u32 val, u32 addr, char *name)
+{
+	bool s1, s2, s3;
+
+	seq_printf(s, "%s\t0x%08X\t", name, val);
+
+	if (!addr || !name || (strlen(name) < 2))
+		goto done;
+
+	s1 = name[strlen(name) - 1] == '1';
+	s2 = name[strlen(name) - 1] == '2';
+	s3 = name[strlen(name) - 1] == '3';
+
+	seq_printf(s, "Pitch=%d - ", val & 0xFFFF);
+
+	switch ((val & BLT_TTY_COL_MASK) >> BLT_TTY_COL_SHIFT) {
+	case BDISP_RGB565:
+		seq_puts(s, "RGB565 - ");
+		break;
+	case BDISP_RGB888:
+		seq_puts(s, "RGB888 - ");
+		break;
+	case BDISP_XRGB8888:
+		seq_puts(s, "xRGB888 - ");
+		break;
+	case BDISP_ARGB8888:
+		seq_puts(s, "ARGB888 - ");
+		break;
+	case BDISP_NV12:
+		seq_puts(s, "NV12 - ");
+		break;
+	case BDISP_YUV_3B:
+		seq_puts(s, "YUV420P - ");
+		break;
+	default:
+		seq_puts(s, "ColorFormat ??? - ");
+		break;
+	}
+
+	if ((val & BLT_TTY_ALPHA_R) && !s3)
+		seq_puts(s, "AlphaRange - ");
+	if ((val & BLT_S1TY_A1_SUBSET) && !s3)
+		seq_puts(s, "A1SubSet - ");
+	if ((val & BLT_TTY_MB) && !s1)
+		seq_puts(s, "MB - ");
+	if (val & BLT_TTY_HSO)
+		seq_puts(s, "HSO inverse - ");
+	if (val & BLT_TTY_VSO)
+		seq_puts(s, "VSO inverse - ");
+	if ((val & BLT_S1TY_CHROMA_EXT) && (s1 || s2))
+		seq_puts(s, "ChromaExt - ");
+	if ((val & BLT_S3TY_BLANK_ACC) && s3)
+		seq_puts(s, "Blank Acc - ");
+	if ((val & BTL_S1TY_SUBBYTE) && !s3)
+		seq_puts(s, "SubByte - ");
+	if ((val & BLT_S1TY_RGB_EXP) && !s3)
+		seq_puts(s, "RGBExpand - ");
+	if ((val & BLT_TTY_BIG_END) && !s3)
+		seq_puts(s, "BigEndian - ");
+
+done:
+	seq_puts(s, "\n");
+}
+
+static void bdisp_dbg_dump_fctl(struct seq_file *s, u32 val)
+{
+	seq_printf(s, "FCTL\t0x%08X\t", val);
+
+	if ((val & BLT_FCTL_Y_HV_SCALE) == BLT_FCTL_Y_HV_SCALE)
+		seq_puts(s, "Resize Luma - ");
+	else if ((val & BLT_FCTL_Y_HV_SCALE) == BLT_FCTL_Y_HV_SAMPLE)
+		seq_puts(s, "Sample Luma - ");
+
+	if ((val & BLT_FCTL_HV_SCALE) == BLT_FCTL_HV_SCALE)
+		seq_puts(s, "Resize Chroma");
+	else if ((val & BLT_FCTL_HV_SCALE) == BLT_FCTL_HV_SAMPLE)
+		seq_puts(s, "Sample Chroma");
+
+	seq_puts(s, "\n");
+}
+
+static void bdisp_dbg_dump_rsf(struct seq_file *s, u32 val, char *name)
+{
+	u32 inc;
+
+	seq_printf(s, "%s\t0x%08X\t", name, val);
+
+	if (!val)
+		goto done;
+
+	inc = val & 0xFFFF;
+	seq_printf(s, "H: %d(6.10) / scale~%dx0.1 - ", inc, 1024 * 10 / inc);
+
+	inc = val >> 16;
+	seq_printf(s, "V: %d(6.10) / scale~%dx0.1", inc, 1024 * 10 / inc);
+
+done:
+	seq_puts(s, "\n");
+}
+
+static void bdisp_dbg_dump_rzi(struct seq_file *s, u32 val, char *name)
+{
+	seq_printf(s, "%s\t0x%08X\t", name, val);
+
+	if (!val)
+		goto done;
+
+	seq_printf(s, "H: init=%d repeat=%d - ", val & 0x3FF, (val >> 12) & 7);
+	val >>= 16;
+	seq_printf(s, "V: init=%d repeat=%d", val & 0x3FF, (val >> 12) & 7);
+
+done:
+	seq_puts(s, "\n");
+}
+
+static void bdisp_dbg_dump_ivmx(struct seq_file *s,
+				u32 c0, u32 c1, u32 c2, u32 c3)
+{
+	seq_printf(s, "IVMX0\t0x%08X\n", c0);
+	seq_printf(s, "IVMX1\t0x%08X\n", c1);
+	seq_printf(s, "IVMX2\t0x%08X\n", c2);
+	seq_printf(s, "IVMX3\t0x%08X\t", c3);
+
+	if (!c0 && !c1 && !c2 && !c3) {
+		seq_puts(s, "\n");
+		return;
+	}
+
+	if ((c0 == bdisp_rgb_to_yuv[0]) &&
+	    (c1 == bdisp_rgb_to_yuv[1]) &&
+	    (c2 == bdisp_rgb_to_yuv[2]) &&
+	    (c3 == bdisp_rgb_to_yuv[3])) {
+		seq_puts(s, "RGB to YUV\n");
+		return;
+	}
+
+	if ((c0 == bdisp_yuv_to_rgb[0]) &&
+	    (c1 == bdisp_yuv_to_rgb[1]) &&
+	    (c2 == bdisp_yuv_to_rgb[2]) &&
+	    (c3 == bdisp_yuv_to_rgb[3])) {
+		seq_puts(s, "YUV to RGB\n");
+		return;
+	}
+	seq_puts(s, "Unknown conversion\n");
+}
+
+static int bdisp_dbg_last_nodes(struct seq_file *s, void *data)
+{
+	/* Not dumping all fields, focusing on significant ones */
+	struct bdisp_dev *bdisp = s->private;
+	struct bdisp_node *node;
+	int i = 0;
+
+	if (!bdisp->dbg.copy_node[0]) {
+		seq_puts(s, "No node built yet\n");
+		return 0;
+	}
+
+	do {
+		node = bdisp->dbg.copy_node[i];
+		if (!node)
+			break;
+		seq_printf(s, "--------\nNode %d:\n", i);
+		seq_puts(s, "-- General --\n");
+		seq_printf(s, "NIP\t0x%08X\n", node->nip);
+		seq_printf(s, "CIC\t0x%08X\n", node->cic);
+		bdisp_dbg_dump_ins(s, node->ins);
+		seq_printf(s, "ACK\t0x%08X\n", node->ack);
+		seq_puts(s, "-- Target --\n");
+		seq_printf(s, "TBA\t0x%08X\n", node->tba);
+		bdisp_dbg_dump_tty(s, node->tty);
+		bdisp_dbg_dump_xy(s, node->txy, "TXY");
+		bdisp_dbg_dump_sz(s, node->tsz, "TSZ");
+		/* Color Fill not dumped */
+		seq_puts(s, "-- Source 1 --\n");
+		seq_printf(s, "S1BA\t0x%08X\n", node->s1ba);
+		bdisp_dbg_dump_sty(s, node->s1ty, node->s1ba, "S1TY");
+		bdisp_dbg_dump_xy(s, node->s1xy, "S1XY");
+		seq_puts(s, "-- Source 2 --\n");
+		seq_printf(s, "S2BA\t0x%08X\n", node->s2ba);
+		bdisp_dbg_dump_sty(s, node->s2ty, node->s2ba, "S2TY");
+		bdisp_dbg_dump_xy(s, node->s2xy, "S2XY");
+		bdisp_dbg_dump_sz(s, node->s2sz, "S2SZ");
+		seq_puts(s, "-- Source 3 --\n");
+		seq_printf(s, "S3BA\t0x%08X\n", node->s3ba);
+		bdisp_dbg_dump_sty(s, node->s3ty, node->s3ba, "S3TY");
+		bdisp_dbg_dump_xy(s, node->s3xy, "S3XY");
+		bdisp_dbg_dump_sz(s, node->s3sz, "S3SZ");
+		/* Clipping not dumped */
+		/* CLUT not dumped */
+		seq_puts(s, "-- Filter & Mask --\n");
+		bdisp_dbg_dump_fctl(s, node->fctl);
+		/* PMK not dumped */
+		seq_puts(s, "-- Chroma Filter --\n");
+		bdisp_dbg_dump_rsf(s, node->rsf, "RSF");
+		bdisp_dbg_dump_rzi(s, node->rzi, "RZI");
+		seq_printf(s, "HFP\t0x%08X\n", node->hfp);
+		seq_printf(s, "VFP\t0x%08X\n", node->vfp);
+		seq_puts(s, "-- Luma Filter --\n");
+		bdisp_dbg_dump_rsf(s, node->y_rsf, "Y_RSF");
+		bdisp_dbg_dump_rzi(s, node->y_rzi, "Y_RZI");
+		seq_printf(s, "Y_HFP\t0x%08X\n", node->y_hfp);
+		seq_printf(s, "Y_VFP\t0x%08X\n", node->y_vfp);
+		/* Flicker not dumped */
+		/* Color key not dumped */
+		/* Reserved not dumped */
+		/* Static Address & User not dumped */
+		seq_puts(s, "-- Input Versatile Matrix --\n");
+		bdisp_dbg_dump_ivmx(s, node->ivmx0, node->ivmx1,
+				    node->ivmx2, node->ivmx3);
+		/* Output Versatile Matrix not dumped */
+		/* Pace not dumped */
+		/* VC1R & DEI not dumped */
+		/* Gradient Fill not dumped */
+	} while ((++i < MAX_NB_NODE) && node->nip);
+
+	return 0;
+}
+
+static int bdisp_dbg_last_nodes_raw(struct seq_file *s, void *data)
+{
+	struct bdisp_dev *bdisp = s->private;
+	struct bdisp_node *node;
+	u32 *val;
+	int j, i = 0;
+
+	if (!bdisp->dbg.copy_node[0]) {
+		seq_puts(s, "No node built yet\n");
+		return 0;
+	}
+
+	do {
+		node = bdisp->dbg.copy_node[i];
+		if (!node)
+			break;
+
+		seq_printf(s, "--------\nNode %d:\n", i);
+		val = (u32 *)node;
+		for (j = 0; j < sizeof(struct bdisp_node) / sizeof(u32); j++)
+			seq_printf(s, "0x%08X\n", *val++);
+	} while ((++i < MAX_NB_NODE) && node->nip);
+
+	return 0;
+}
+
+static const char *bdisp_fmt_to_str(struct bdisp_frame frame)
+{
+	switch (frame.fmt->pixelformat) {
+	case V4L2_PIX_FMT_YUV420:
+		return "YUV420P";
+	case V4L2_PIX_FMT_NV12:
+		if (frame.field == V4L2_FIELD_INTERLACED)
+			return "NV12 interlaced";
+		else
+			return "NV12";
+	case V4L2_PIX_FMT_RGB565:
+		return "RGB16";
+	case V4L2_PIX_FMT_RGB24:
+		return "RGB24";
+	case V4L2_PIX_FMT_XBGR32:
+		return "XRGB";
+	case V4L2_PIX_FMT_ABGR32:
+		return "ARGB";
+	default:
+		return "????";
+	}
+}
+
+static int bdisp_dbg_last_request(struct seq_file *s, void *data)
+{
+	struct bdisp_dev *bdisp = s->private;
+	struct bdisp_request *request = &bdisp->dbg.copy_request;
+	struct bdisp_frame src, dst;
+
+	if (!request->nb_req) {
+		seq_puts(s, "No request\n");
+		return 0;
+	}
+
+	src = request->src;
+	dst = request->dst;
+
+	seq_printf(s, "\nRequest #%d\n", request->nb_req);
+
+	seq_printf(s, "Format:    %s\t\t\t%s\n",
+		   bdisp_fmt_to_str(src), bdisp_fmt_to_str(dst));
+	seq_printf(s, "Crop area: %dx%d @ %d,%d  ==>\t%dx%d @ %d,%d\n",
+		   src.crop.width, src.crop.height,
+		   src.crop.left, src.crop.top,
+		   dst.crop.width, dst.crop.height,
+		   dst.crop.left, dst.crop.top);
+	seq_printf(s, "Buff size: %dx%d\t\t%dx%d\n\n",
+		   src.width, src.height, dst.width, dst.height);
+
+	if (request->hflip)
+		seq_puts(s, "Horizontal flip\n\n");
+
+	if (request->vflip)
+		seq_puts(s, "Vertical flip\n\n");
+
+	return 0;
+}
+
+#define DUMP(reg) seq_printf(s, #reg " \t0x%08X\n", readl(bdisp->regs + reg))
+
+static int bdisp_dbg_regs(struct seq_file *s, void *data)
+{
+	struct bdisp_dev *bdisp = s->private;
+	int ret;
+	unsigned int i;
+
+	ret = pm_runtime_get_sync(bdisp->dev);
+	if (ret < 0) {
+		seq_puts(s, "Cannot wake up IP\n");
+		return 0;
+	}
+
+	seq_printf(s, "Reg @ = 0x%p\n", bdisp->regs);
+
+	seq_puts(s, "\nStatic:\n");
+	DUMP(BLT_CTL);
+	DUMP(BLT_ITS);
+	DUMP(BLT_STA1);
+	DUMP(BLT_AQ1_CTL);
+	DUMP(BLT_AQ1_IP);
+	DUMP(BLT_AQ1_LNA);
+	DUMP(BLT_AQ1_STA);
+	DUMP(BLT_ITM0);
+
+	seq_puts(s, "\nPlugs:\n");
+	DUMP(BLT_PLUGS1_OP2);
+	DUMP(BLT_PLUGS1_CHZ);
+	DUMP(BLT_PLUGS1_MSZ);
+	DUMP(BLT_PLUGS1_PGZ);
+	DUMP(BLT_PLUGS2_OP2);
+	DUMP(BLT_PLUGS2_CHZ);
+	DUMP(BLT_PLUGS2_MSZ);
+	DUMP(BLT_PLUGS2_PGZ);
+	DUMP(BLT_PLUGS3_OP2);
+	DUMP(BLT_PLUGS3_CHZ);
+	DUMP(BLT_PLUGS3_MSZ);
+	DUMP(BLT_PLUGS3_PGZ);
+	DUMP(BLT_PLUGT_OP2);
+	DUMP(BLT_PLUGT_CHZ);
+	DUMP(BLT_PLUGT_MSZ);
+	DUMP(BLT_PLUGT_PGZ);
+
+	seq_puts(s, "\nNode:\n");
+	DUMP(BLT_NIP);
+	DUMP(BLT_CIC);
+	DUMP(BLT_INS);
+	DUMP(BLT_ACK);
+	DUMP(BLT_TBA);
+	DUMP(BLT_TTY);
+	DUMP(BLT_TXY);
+	DUMP(BLT_TSZ);
+	DUMP(BLT_S1BA);
+	DUMP(BLT_S1TY);
+	DUMP(BLT_S1XY);
+	DUMP(BLT_S2BA);
+	DUMP(BLT_S2TY);
+	DUMP(BLT_S2XY);
+	DUMP(BLT_S2SZ);
+	DUMP(BLT_S3BA);
+	DUMP(BLT_S3TY);
+	DUMP(BLT_S3XY);
+	DUMP(BLT_S3SZ);
+	DUMP(BLT_FCTL);
+	DUMP(BLT_RSF);
+	DUMP(BLT_RZI);
+	DUMP(BLT_HFP);
+	DUMP(BLT_VFP);
+	DUMP(BLT_Y_RSF);
+	DUMP(BLT_Y_RZI);
+	DUMP(BLT_Y_HFP);
+	DUMP(BLT_Y_VFP);
+	DUMP(BLT_IVMX0);
+	DUMP(BLT_IVMX1);
+	DUMP(BLT_IVMX2);
+	DUMP(BLT_IVMX3);
+	DUMP(BLT_OVMX0);
+	DUMP(BLT_OVMX1);
+	DUMP(BLT_OVMX2);
+	DUMP(BLT_OVMX3);
+	DUMP(BLT_DEI);
+
+	seq_puts(s, "\nFilter:\n");
+	for (i = 0; i < BLT_NB_H_COEF; i++) {
+		seq_printf(s, "BLT_HFC%d \t0x%08X\n", i,
+			   readl(bdisp->regs + BLT_HFC_N + i * 4));
+	}
+	for (i = 0; i < BLT_NB_V_COEF; i++) {
+		seq_printf(s, "BLT_VFC%d \t0x%08X\n", i,
+			   readl(bdisp->regs + BLT_VFC_N + i * 4));
+	}
+
+	seq_puts(s, "\nLuma filter:\n");
+	for (i = 0; i < BLT_NB_H_COEF; i++) {
+		seq_printf(s, "BLT_Y_HFC%d \t0x%08X\n", i,
+			   readl(bdisp->regs + BLT_Y_HFC_N + i * 4));
+	}
+	for (i = 0; i < BLT_NB_V_COEF; i++) {
+		seq_printf(s, "BLT_Y_VFC%d \t0x%08X\n", i,
+			   readl(bdisp->regs + BLT_Y_VFC_N + i * 4));
+	}
+
+	pm_runtime_put(bdisp->dev);
+
+	return 0;
+}
+
+#define SECOND 1000000
+
+static int bdisp_dbg_perf(struct seq_file *s, void *data)
+{
+	struct bdisp_dev *bdisp = s->private;
+	struct bdisp_request *request = &bdisp->dbg.copy_request;
+	s64 avg_time_us;
+	int avg_fps, min_fps, max_fps, last_fps;
+
+	if (!request->nb_req) {
+		seq_puts(s, "No request\n");
+		return 0;
+	}
+
+	avg_time_us = div64_s64(bdisp->dbg.tot_duration, request->nb_req);
+	if (avg_time_us > SECOND)
+		avg_fps = 0;
+	else
+		avg_fps = SECOND / (s32)avg_time_us;
+
+	if (bdisp->dbg.min_duration > SECOND)
+		min_fps = 0;
+	else
+		min_fps = SECOND / (s32)bdisp->dbg.min_duration;
+
+	if (bdisp->dbg.max_duration > SECOND)
+		max_fps = 0;
+	else
+		max_fps = SECOND / (s32)bdisp->dbg.max_duration;
+
+	if (bdisp->dbg.last_duration > SECOND)
+		last_fps = 0;
+	else
+		last_fps = SECOND / (s32)bdisp->dbg.last_duration;
+
+	seq_printf(s, "HW processing (%d requests):\n", request->nb_req);
+	seq_printf(s, " Average: %5lld us  (%3d fps)\n",
+		   avg_time_us, avg_fps);
+	seq_printf(s, " Min-Max: %5lld us  (%3d fps) - %5lld us  (%3d fps)\n",
+		   bdisp->dbg.min_duration, min_fps,
+		   bdisp->dbg.max_duration, max_fps);
+	seq_printf(s, " Last:    %5lld us  (%3d fps)\n",
+		   bdisp->dbg.last_duration, last_fps);
+
+	return 0;
+}
+
+#define bdisp_dbg_declare(name) \
+	static int bdisp_dbg_##name##_open(struct inode *i, struct file *f) \
+	{ \
+		return single_open(f, bdisp_dbg_##name, i->i_private); \
+	} \
+	static const struct file_operations bdisp_dbg_##name##_fops = { \
+		.open           = bdisp_dbg_##name##_open, \
+		.read           = seq_read, \
+		.llseek         = seq_lseek, \
+		.release        = single_release, \
+	}
+
+#define bdisp_dbg_create_entry(name) \
+	debugfs_create_file(#name, S_IRUGO, bdisp->dbg.debugfs_entry, bdisp, \
+			    &bdisp_dbg_##name##_fops)
+
+bdisp_dbg_declare(regs);
+bdisp_dbg_declare(last_nodes);
+bdisp_dbg_declare(last_nodes_raw);
+bdisp_dbg_declare(last_request);
+bdisp_dbg_declare(perf);
+
+int bdisp_debugfs_create(struct bdisp_dev *bdisp)
+{
+	char dirname[16];
+
+	snprintf(dirname, sizeof(dirname), "%s%d", BDISP_NAME, bdisp->id);
+	bdisp->dbg.debugfs_entry = debugfs_create_dir(dirname, NULL);
+	if (!bdisp->dbg.debugfs_entry)
+		goto err;
+
+	if (!bdisp_dbg_create_entry(regs))
+		goto err;
+
+	if (!bdisp_dbg_create_entry(last_nodes))
+		goto err;
+
+	if (!bdisp_dbg_create_entry(last_nodes_raw))
+		goto err;
+
+	if (!bdisp_dbg_create_entry(last_request))
+		goto err;
+
+	if (!bdisp_dbg_create_entry(perf))
+		goto err;
+
+	return 0;
+
+err:
+	bdisp_debugfs_remove(bdisp);
+	return 0;
+}
+
+void bdisp_debugfs_remove(struct bdisp_dev *bdisp)
+{
+	debugfs_remove_recursive(bdisp->dbg.debugfs_entry);
+	bdisp->dbg.debugfs_entry = NULL;
+}
diff --git a/kernel/drivers/media/platform/sti/bdisp/bdisp-filter.h b/kernel/drivers/media/platform/sti/bdisp/bdisp-filter.h
new file mode 100644
index 000000000..fc8c54f72
--- /dev/null
+++ b/kernel/drivers/media/platform/sti/bdisp/bdisp-filter.h
@@ -0,0 +1,346 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2014
+ * Authors: Fabien Dessenne <fabien.dessenne@st.com> for STMicroelectronics.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#define BDISP_HF_NB             64
+#define BDISP_VF_NB             40
+
+/**
+ * struct bdisp_filter_h_spec - Horizontal filter specification
+ *
+ * @min:        min scale factor for this filter (6.10 fixed point)
+ * @max:        max scale factor for this filter (6.10 fixed point)
+ * coef:        filter coefficients
+ */
+struct bdisp_filter_h_spec {
+	const u16 min;
+	const u16 max;
+	const u8 coef[BDISP_HF_NB];
+};
+
+static const struct bdisp_filter_h_spec bdisp_h_spec[] = {
+	{
+		.min = 0,
+		.max = 921,
+		.coef = {
+			0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
+			0x00, 0x00, 0xff, 0x07, 0x3d, 0xfc, 0x01, 0x00,
+			0x00, 0x01, 0xfd, 0x11, 0x36, 0xf9, 0x02, 0x00,
+			0x00, 0x01, 0xfb, 0x1b, 0x2e, 0xf9, 0x02, 0x00,
+			0x00, 0x01, 0xf9, 0x26, 0x26, 0xf9, 0x01, 0x00,
+			0x00, 0x02, 0xf9, 0x30, 0x19, 0xfb, 0x01, 0x00,
+			0x00, 0x02, 0xf9, 0x39, 0x0e, 0xfd, 0x01, 0x00,
+			0x00, 0x01, 0xfc, 0x3e, 0x06, 0xff, 0x00, 0x00
+		}
+	},
+	{
+		.min = 921,
+		.max = 1024,
+		.coef = {
+			0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
+			0xff, 0x03, 0xfd, 0x08, 0x3e, 0xf9, 0x04, 0xfe,
+			0xfd, 0x06, 0xf8, 0x13, 0x3b, 0xf4, 0x07, 0xfc,
+			0xfb, 0x08, 0xf5, 0x1f, 0x34, 0xf1, 0x09, 0xfb,
+			0xfb, 0x09, 0xf2, 0x2b, 0x2a, 0xf1, 0x09, 0xfb,
+			0xfb, 0x09, 0xf2, 0x35, 0x1e, 0xf4, 0x08, 0xfb,
+			0xfc, 0x07, 0xf5, 0x3c, 0x12, 0xf7, 0x06, 0xfd,
+			0xfe, 0x04, 0xfa, 0x3f, 0x07, 0xfc, 0x03, 0xff
+		}
+	},
+	{
+		.min = 1024,
+		.max = 1126,
+		.coef = {
+			0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
+			0xff, 0x03, 0xfd, 0x08, 0x3e, 0xf9, 0x04, 0xfe,
+			0xfd, 0x06, 0xf8, 0x13, 0x3b, 0xf4, 0x07, 0xfc,
+			0xfb, 0x08, 0xf5, 0x1f, 0x34, 0xf1, 0x09, 0xfb,
+			0xfb, 0x09, 0xf2, 0x2b, 0x2a, 0xf1, 0x09, 0xfb,
+			0xfb, 0x09, 0xf2, 0x35, 0x1e, 0xf4, 0x08, 0xfb,
+			0xfc, 0x07, 0xf5, 0x3c, 0x12, 0xf7, 0x06, 0xfd,
+			0xfe, 0x04, 0xfa, 0x3f, 0x07, 0xfc, 0x03, 0xff
+		}
+	},
+	{
+		.min = 1126,
+		.max = 1228,
+		.coef = {
+			0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
+			0xff, 0x03, 0xfd, 0x08, 0x3e, 0xf9, 0x04, 0xfe,
+			0xfd, 0x06, 0xf8, 0x13, 0x3b, 0xf4, 0x07, 0xfc,
+			0xfb, 0x08, 0xf5, 0x1f, 0x34, 0xf1, 0x09, 0xfb,
+			0xfb, 0x09, 0xf2, 0x2b, 0x2a, 0xf1, 0x09, 0xfb,
+			0xfb, 0x09, 0xf2, 0x35, 0x1e, 0xf4, 0x08, 0xfb,
+			0xfc, 0x07, 0xf5, 0x3c, 0x12, 0xf7, 0x06, 0xfd,
+			0xfe, 0x04, 0xfa, 0x3f, 0x07, 0xfc, 0x03, 0xff
+		}
+	},
+	{
+		.min = 1228,
+		.max = 1331,
+		.coef = {
+			0xfd, 0x04, 0xfc, 0x05, 0x39, 0x05, 0xfc, 0x04,
+			0xfc, 0x06, 0xf9, 0x0c, 0x39, 0xfe, 0x00, 0x02,
+			0xfb, 0x08, 0xf6, 0x17, 0x35, 0xf9, 0x02, 0x00,
+			0xfc, 0x08, 0xf4, 0x20, 0x30, 0xf4, 0x05, 0xff,
+			0xfd, 0x07, 0xf4, 0x29, 0x28, 0xf3, 0x07, 0xfd,
+			0xff, 0x05, 0xf5, 0x31, 0x1f, 0xf3, 0x08, 0xfc,
+			0x00, 0x02, 0xf9, 0x38, 0x14, 0xf6, 0x08, 0xfb,
+			0x02, 0x00, 0xff, 0x3a, 0x0b, 0xf8, 0x06, 0xfc
+		}
+	},
+	{
+		.min = 1331,
+		.max = 1433,
+		.coef = {
+			0xfc, 0x06, 0xf9, 0x09, 0x34, 0x09, 0xf9, 0x06,
+			0xfd, 0x07, 0xf7, 0x10, 0x32, 0x02, 0xfc, 0x05,
+			0xfe, 0x07, 0xf6, 0x17, 0x2f, 0xfc, 0xff, 0x04,
+			0xff, 0x06, 0xf5, 0x20, 0x2a, 0xf9, 0x01, 0x02,
+			0x00, 0x04, 0xf6, 0x27, 0x25, 0xf6, 0x04, 0x00,
+			0x02, 0x01, 0xf9, 0x2d, 0x1d, 0xf5, 0x06, 0xff,
+			0x04, 0xff, 0xfd, 0x31, 0x15, 0xf5, 0x07, 0xfe,
+			0x05, 0xfc, 0x02, 0x35, 0x0d, 0xf7, 0x07, 0xfd
+		}
+	},
+	{
+		.min = 1433,
+		.max = 1536,
+		.coef = {
+			0xfe, 0x06, 0xf8, 0x0b, 0x30, 0x0b, 0xf8, 0x06,
+			0xff, 0x06, 0xf7, 0x12, 0x2d, 0x05, 0xfa, 0x06,
+			0x00, 0x04, 0xf6, 0x18, 0x2c, 0x00, 0xfc, 0x06,
+			0x01, 0x02, 0xf7, 0x1f, 0x27, 0xfd, 0xff, 0x04,
+			0x03, 0x00, 0xf9, 0x24, 0x24, 0xf9, 0x00, 0x03,
+			0x04, 0xff, 0xfd, 0x29, 0x1d, 0xf7, 0x02, 0x01,
+			0x06, 0xfc, 0x00, 0x2d, 0x17, 0xf6, 0x04, 0x00,
+			0x06, 0xfa, 0x05, 0x30, 0x0f, 0xf7, 0x06, 0xff
+		}
+	},
+	{
+		.min = 1536,
+		.max = 2048,
+		.coef = {
+			0x05, 0xfd, 0xfb, 0x13, 0x25, 0x13, 0xfb, 0xfd,
+			0x05, 0xfc, 0xfd, 0x17, 0x24, 0x0f, 0xf9, 0xff,
+			0x04, 0xfa, 0xff, 0x1b, 0x24, 0x0b, 0xf9, 0x00,
+			0x03, 0xf9, 0x01, 0x1f, 0x23, 0x08, 0xf8, 0x01,
+			0x02, 0xf9, 0x04, 0x22, 0x20, 0x04, 0xf9, 0x02,
+			0x01, 0xf8, 0x08, 0x25, 0x1d, 0x01, 0xf9, 0x03,
+			0x00, 0xf9, 0x0c, 0x25, 0x1a, 0xfe, 0xfa, 0x04,
+			0xff, 0xf9, 0x10, 0x26, 0x15, 0xfc, 0xfc, 0x05
+		}
+	},
+	{
+		.min = 2048,
+		.max = 3072,
+		.coef = {
+			0xfc, 0xfd, 0x06, 0x13, 0x18, 0x13, 0x06, 0xfd,
+			0xfc, 0xfe, 0x08, 0x15, 0x17, 0x12, 0x04, 0xfc,
+			0xfb, 0xfe, 0x0a, 0x16, 0x18, 0x10, 0x03, 0xfc,
+			0xfb, 0x00, 0x0b, 0x18, 0x17, 0x0f, 0x01, 0xfb,
+			0xfb, 0x00, 0x0d, 0x19, 0x17, 0x0d, 0x00, 0xfb,
+			0xfb, 0x01, 0x0f, 0x19, 0x16, 0x0b, 0x00, 0xfb,
+			0xfc, 0x03, 0x11, 0x19, 0x15, 0x09, 0xfe, 0xfb,
+			0xfc, 0x04, 0x12, 0x1a, 0x12, 0x08, 0xfe, 0xfc
+		}
+	},
+	{
+		.min = 3072,
+		.max = 4096,
+		.coef = {
+			0xfe, 0x02, 0x09, 0x0f, 0x0e, 0x0f, 0x09, 0x02,
+			0xff, 0x02, 0x09, 0x0f, 0x10, 0x0e, 0x08, 0x01,
+			0xff, 0x03, 0x0a, 0x10, 0x10, 0x0d, 0x07, 0x00,
+			0x00, 0x04, 0x0b, 0x10, 0x0f, 0x0c, 0x06, 0x00,
+			0x00, 0x05, 0x0c, 0x10, 0x0e, 0x0c, 0x05, 0x00,
+			0x00, 0x06, 0x0c, 0x11, 0x0e, 0x0b, 0x04, 0x00,
+			0x00, 0x07, 0x0d, 0x11, 0x0f, 0x0a, 0x03, 0xff,
+			0x01, 0x08, 0x0e, 0x11, 0x0e, 0x09, 0x02, 0xff
+		}
+	},
+	{
+		.min = 4096,
+		.max = 5120,
+		.coef = {
+			0x00, 0x04, 0x09, 0x0c, 0x0e, 0x0c, 0x09, 0x04,
+			0x01, 0x05, 0x09, 0x0c, 0x0d, 0x0c, 0x08, 0x04,
+			0x01, 0x05, 0x0a, 0x0c, 0x0e, 0x0b, 0x08, 0x03,
+			0x02, 0x06, 0x0a, 0x0d, 0x0c, 0x0b, 0x07, 0x03,
+			0x02, 0x07, 0x0a, 0x0d, 0x0d, 0x0a, 0x07, 0x02,
+			0x03, 0x07, 0x0b, 0x0d, 0x0c, 0x0a, 0x06, 0x02,
+			0x03, 0x08, 0x0b, 0x0d, 0x0d, 0x0a, 0x05, 0x01,
+			0x04, 0x08, 0x0c, 0x0d, 0x0c, 0x09, 0x05, 0x01
+		}
+	},
+	{
+		.min = 5120,
+		.max = 65535,
+		.coef = {
+			0x03, 0x06, 0x09, 0x0b, 0x09, 0x0b, 0x09, 0x06,
+			0x03, 0x06, 0x09, 0x0b, 0x0c, 0x0a, 0x08, 0x05,
+			0x03, 0x06, 0x09, 0x0b, 0x0c, 0x0a, 0x08, 0x05,
+			0x04, 0x07, 0x09, 0x0b, 0x0b, 0x0a, 0x08, 0x04,
+			0x04, 0x07, 0x0a, 0x0b, 0x0b, 0x0a, 0x07, 0x04,
+			0x04, 0x08, 0x0a, 0x0b, 0x0b, 0x09, 0x07, 0x04,
+			0x05, 0x08, 0x0a, 0x0b, 0x0c, 0x09, 0x06, 0x03,
+			0x05, 0x08, 0x0a, 0x0b, 0x0c, 0x09, 0x06, 0x03
+		}
+	}
+};
+
+/**
+ * struct bdisp_filter_v_spec - Vertical filter specification
+ *
+ * @min:	min scale factor for this filter (6.10 fixed point)
+ * @max:	max scale factor for this filter (6.10 fixed point)
+ * coef:	filter coefficients
+ */
+struct bdisp_filter_v_spec {
+	const u16 min;
+	const u16 max;
+	const u8 coef[BDISP_VF_NB];
+};
+
+static const struct bdisp_filter_v_spec bdisp_v_spec[] = {
+	{
+		.min = 0,
+		.max = 1024,
+		.coef = {
+			0x00, 0x00, 0x40, 0x00, 0x00,
+			0x00, 0x06, 0x3d, 0xfd, 0x00,
+			0xfe, 0x0f, 0x38, 0xfb, 0x00,
+			0xfd, 0x19, 0x2f, 0xfb, 0x00,
+			0xfc, 0x24, 0x24, 0xfc, 0x00,
+			0xfb, 0x2f, 0x19, 0xfd, 0x00,
+			0xfb, 0x38, 0x0f, 0xfe, 0x00,
+			0xfd, 0x3d, 0x06, 0x00, 0x00
+		}
+	},
+	{
+		.min = 1024,
+		.max = 1331,
+		.coef = {
+			0xfc, 0x05, 0x3e, 0x05, 0xfc,
+			0xf8, 0x0e, 0x3b, 0xff, 0x00,
+			0xf5, 0x18, 0x38, 0xf9, 0x02,
+			0xf4, 0x21, 0x31, 0xf5, 0x05,
+			0xf4, 0x2a, 0x27, 0xf4, 0x07,
+			0xf6, 0x30, 0x1e, 0xf4, 0x08,
+			0xf9, 0x35, 0x15, 0xf6, 0x07,
+			0xff, 0x37, 0x0b, 0xf9, 0x06
+		}
+	},
+	{
+		.min = 1331,
+		.max = 1433,
+		.coef = {
+			0xf8, 0x0a, 0x3c, 0x0a, 0xf8,
+			0xf6, 0x12, 0x3b, 0x02, 0xfb,
+			0xf4, 0x1b, 0x35, 0xfd, 0xff,
+			0xf4, 0x23, 0x30, 0xf8, 0x01,
+			0xf6, 0x29, 0x27, 0xf6, 0x04,
+			0xf9, 0x2e, 0x1e, 0xf5, 0x06,
+			0xfd, 0x31, 0x16, 0xf6, 0x06,
+			0x02, 0x32, 0x0d, 0xf8, 0x07
+		}
+	},
+	{
+		.min = 1433,
+		.max = 1536,
+		.coef = {
+			0xf6, 0x0e, 0x38, 0x0e, 0xf6,
+			0xf5, 0x15, 0x38, 0x06, 0xf8,
+			0xf5, 0x1d, 0x33, 0x00, 0xfb,
+			0xf6, 0x23, 0x2d, 0xfc, 0xfe,
+			0xf9, 0x28, 0x26, 0xf9, 0x00,
+			0xfc, 0x2c, 0x1e, 0xf7, 0x03,
+			0x00, 0x2e, 0x18, 0xf6, 0x04,
+			0x05, 0x2e, 0x11, 0xf7, 0x05
+		}
+	},
+	{
+		.min = 1536,
+		.max = 2048,
+		.coef = {
+			0xfb, 0x13, 0x24, 0x13, 0xfb,
+			0xfd, 0x17, 0x23, 0x0f, 0xfa,
+			0xff, 0x1a, 0x23, 0x0b, 0xf9,
+			0x01, 0x1d, 0x22, 0x07, 0xf9,
+			0x04, 0x20, 0x1f, 0x04, 0xf9,
+			0x07, 0x22, 0x1c, 0x01, 0xfa,
+			0x0b, 0x24, 0x17, 0xff, 0xfb,
+			0x0f, 0x24, 0x14, 0xfd, 0xfc
+		}
+	},
+	{
+		.min = 2048,
+		.max = 3072,
+		.coef = {
+			0x05, 0x10, 0x16, 0x10, 0x05,
+			0x06, 0x11, 0x16, 0x0f, 0x04,
+			0x08, 0x13, 0x15, 0x0e, 0x02,
+			0x09, 0x14, 0x16, 0x0c, 0x01,
+			0x0b, 0x15, 0x15, 0x0b, 0x00,
+			0x0d, 0x16, 0x13, 0x0a, 0x00,
+			0x0f, 0x17, 0x13, 0x08, 0xff,
+			0x11, 0x18, 0x12, 0x07, 0xfe
+		}
+	},
+	{
+		.min = 3072,
+		.max = 4096,
+		.coef = {
+			0x09, 0x0f, 0x10, 0x0f, 0x09,
+			0x09, 0x0f, 0x12, 0x0e, 0x08,
+			0x0a, 0x10, 0x11, 0x0e, 0x07,
+			0x0b, 0x11, 0x11, 0x0d, 0x06,
+			0x0c, 0x11, 0x12, 0x0c, 0x05,
+			0x0d, 0x12, 0x11, 0x0c, 0x04,
+			0x0e, 0x12, 0x11, 0x0b, 0x04,
+			0x0f, 0x13, 0x11, 0x0a, 0x03
+		}
+	},
+	{
+		.min = 4096,
+		.max = 5120,
+		.coef = {
+			0x0a, 0x0e, 0x10, 0x0e, 0x0a,
+			0x0b, 0x0e, 0x0f, 0x0e, 0x0a,
+			0x0b, 0x0f, 0x10, 0x0d, 0x09,
+			0x0c, 0x0f, 0x10, 0x0d, 0x08,
+			0x0d, 0x0f, 0x0f, 0x0d, 0x08,
+			0x0d, 0x10, 0x10, 0x0c, 0x07,
+			0x0e, 0x10, 0x0f, 0x0c, 0x07,
+			0x0f, 0x10, 0x10, 0x0b, 0x06
+		}
+	},
+	{
+		.min = 5120,
+		.max = 65535,
+		.coef = {
+			0x0b, 0x0e, 0x0e, 0x0e, 0x0b,
+			0x0b, 0x0e, 0x0f, 0x0d, 0x0b,
+			0x0c, 0x0e, 0x0f, 0x0d, 0x0a,
+			0x0c, 0x0e, 0x0f, 0x0d, 0x0a,
+			0x0d, 0x0f, 0x0e, 0x0d, 0x09,
+			0x0d, 0x0f, 0x0f, 0x0c, 0x09,
+			0x0e, 0x0f, 0x0e, 0x0c, 0x09,
+			0x0e, 0x0f, 0x0f, 0x0c, 0x08
+		}
+	}
+};
+
+#define NB_H_FILTER ARRAY_SIZE(bdisp_h_spec)
+#define NB_V_FILTER ARRAY_SIZE(bdisp_v_spec)
+
+/* RGB YUV 601 standard conversion */
+static const u32 bdisp_rgb_to_yuv[] = {
+		0x0e1e8bee, 0x08420419, 0xfb5ed471, 0x08004080,
+};
+
+static const u32 bdisp_yuv_to_rgb[] = {
+		0x3324a800, 0xe604ab9c, 0x0004a957, 0x32121eeb,
+};
diff --git a/kernel/drivers/media/platform/sti/bdisp/bdisp-hw.c b/kernel/drivers/media/platform/sti/bdisp/bdisp-hw.c
new file mode 100644
index 000000000..052c932ac
--- /dev/null
+++ b/kernel/drivers/media/platform/sti/bdisp/bdisp-hw.c
@@ -0,0 +1,823 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2014
+ * Authors: Fabien Dessenne <fabien.dessenne@st.com> for STMicroelectronics.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#include <linux/delay.h>
+
+#include "bdisp.h"
+#include "bdisp-filter.h"
+#include "bdisp-reg.h"
+
+/* Max width of the source frame in a single node */
+#define MAX_SRC_WIDTH           2048
+
+/* Reset & boot poll config */
+#define POLL_RST_MAX            50
+#define POLL_RST_DELAY_MS       20
+
+enum bdisp_target_plan {
+	BDISP_RGB,
+	BDISP_Y,
+	BDISP_CBCR
+};
+
+struct bdisp_op_cfg {
+	bool cconv;          /* RGB - YUV conversion */
+	bool hflip;          /* Horizontal flip */
+	bool vflip;          /* Vertical flip */
+	bool wide;           /* Wide (>MAX_SRC_WIDTH) */
+	bool scale;          /* Scale */
+	u16  h_inc;          /* Horizontal increment in 6.10 format */
+	u16  v_inc;          /* Vertical increment in 6.10 format */
+	bool src_interlaced; /* is the src an interlaced buffer */
+	u8   src_nbp;        /* nb of planes of the src */
+	bool src_yuv;        /* is the src a YUV color format */
+	bool src_420;        /* is the src 4:2:0 chroma subsampled */
+	u8   dst_nbp;        /* nb of planes of the dst */
+	bool dst_yuv;        /* is the dst a YUV color format */
+	bool dst_420;        /* is the dst 4:2:0 chroma subsampled */
+};
+
+struct bdisp_filter_addr {
+	u16 min;             /* Filter min scale factor (6.10 fixed point) */
+	u16 max;             /* Filter max scale factor (6.10 fixed point) */
+	void *virt;          /* Virtual address for filter table */
+	dma_addr_t paddr;    /* Physical address for filter table */
+};
+
+static struct bdisp_filter_addr bdisp_h_filter[NB_H_FILTER];
+static struct bdisp_filter_addr bdisp_v_filter[NB_V_FILTER];
+
+/**
+ * bdisp_hw_reset
+ * @bdisp:      bdisp entity
+ *
+ * Resets HW
+ *
+ * RETURNS:
+ * 0 on success.
+ */
+int bdisp_hw_reset(struct bdisp_dev *bdisp)
+{
+	unsigned int i;
+
+	dev_dbg(bdisp->dev, "%s\n", __func__);
+
+	/* Mask Interrupt */
+	writel(0, bdisp->regs + BLT_ITM0);
+
+	/* Reset */
+	writel(readl(bdisp->regs + BLT_CTL) | BLT_CTL_RESET,
+	       bdisp->regs + BLT_CTL);
+	writel(0, bdisp->regs + BLT_CTL);
+
+	/* Wait for reset done */
+	for (i = 0; i < POLL_RST_MAX; i++) {
+		if (readl(bdisp->regs + BLT_STA1) & BLT_STA1_IDLE)
+			break;
+		msleep(POLL_RST_DELAY_MS);
+	}
+	if (i == POLL_RST_MAX)
+		dev_err(bdisp->dev, "Reset timeout\n");
+
+	return (i == POLL_RST_MAX) ? -EAGAIN : 0;
+}
+
+/**
+ * bdisp_hw_get_and_clear_irq
+ * @bdisp:      bdisp entity
+ *
+ * Read then reset interrupt status
+ *
+ * RETURNS:
+ * 0 if expected interrupt was raised.
+ */
+int bdisp_hw_get_and_clear_irq(struct bdisp_dev *bdisp)
+{
+	u32 its;
+
+	its = readl(bdisp->regs + BLT_ITS);
+
+	/* Check for the only expected IT: LastNode of AQ1 */
+	if (!(its & BLT_ITS_AQ1_LNA)) {
+		dev_dbg(bdisp->dev, "Unexpected IT status: 0x%08X\n", its);
+		writel(its, bdisp->regs + BLT_ITS);
+		return -1;
+	}
+
+	/* Clear and mask */
+	writel(its, bdisp->regs + BLT_ITS);
+	writel(0, bdisp->regs + BLT_ITM0);
+
+	return 0;
+}
+
+/**
+ * bdisp_hw_free_nodes
+ * @ctx:        bdisp context
+ *
+ * Free node memory
+ *
+ * RETURNS:
+ * None
+ */
+void bdisp_hw_free_nodes(struct bdisp_ctx *ctx)
+{
+	if (ctx && ctx->node[0]) {
+		DEFINE_DMA_ATTRS(attrs);
+
+		dma_set_attr(DMA_ATTR_WRITE_COMBINE, &attrs);
+		dma_free_attrs(ctx->bdisp_dev->dev,
+			       sizeof(struct bdisp_node) * MAX_NB_NODE,
+			       ctx->node[0], ctx->node_paddr[0], &attrs);
+	}
+}
+
+/**
+ * bdisp_hw_alloc_nodes
+ * @ctx:        bdisp context
+ *
+ * Allocate dma memory for nodes
+ *
+ * RETURNS:
+ * 0 on success
+ */
+int bdisp_hw_alloc_nodes(struct bdisp_ctx *ctx)
+{
+	struct device *dev = ctx->bdisp_dev->dev;
+	unsigned int i, node_size = sizeof(struct bdisp_node);
+	void *base;
+	dma_addr_t paddr;
+	DEFINE_DMA_ATTRS(attrs);
+
+	/* Allocate all the nodes within a single memory page */
+	dma_set_attr(DMA_ATTR_WRITE_COMBINE, &attrs);
+	base = dma_alloc_attrs(dev, node_size * MAX_NB_NODE, &paddr,
+			       GFP_KERNEL | GFP_DMA, &attrs);
+	if (!base) {
+		dev_err(dev, "%s no mem\n", __func__);
+		return -ENOMEM;
+	}
+
+	memset(base, 0, node_size * MAX_NB_NODE);
+
+	for (i = 0; i < MAX_NB_NODE; i++) {
+		ctx->node[i] = base;
+		ctx->node_paddr[i] = paddr;
+		dev_dbg(dev, "node[%d]=0x%p (paddr=%pad)\n", i, ctx->node[i],
+			&paddr);
+		base += node_size;
+		paddr += node_size;
+	}
+
+	return 0;
+}
+
+/**
+ * bdisp_hw_free_filters
+ * @dev:        device
+ *
+ * Free filters memory
+ *
+ * RETURNS:
+ * None
+ */
+void bdisp_hw_free_filters(struct device *dev)
+{
+	int size = (BDISP_HF_NB * NB_H_FILTER) + (BDISP_VF_NB * NB_V_FILTER);
+
+	if (bdisp_h_filter[0].virt) {
+		DEFINE_DMA_ATTRS(attrs);
+
+		dma_set_attr(DMA_ATTR_WRITE_COMBINE, &attrs);
+		dma_free_attrs(dev, size, bdisp_h_filter[0].virt,
+			       bdisp_h_filter[0].paddr, &attrs);
+	}
+}
+
+/**
+ * bdisp_hw_alloc_filters
+ * @dev:        device
+ *
+ * Allocate dma memory for filters
+ *
+ * RETURNS:
+ * 0 on success
+ */
+int bdisp_hw_alloc_filters(struct device *dev)
+{
+	unsigned int i, size;
+	void *base;
+	dma_addr_t paddr;
+	DEFINE_DMA_ATTRS(attrs);
+
+	/* Allocate all the filters within a single memory page */
+	size = (BDISP_HF_NB * NB_H_FILTER) + (BDISP_VF_NB * NB_V_FILTER);
+	dma_set_attr(DMA_ATTR_WRITE_COMBINE, &attrs);
+	base = dma_alloc_attrs(dev, size, &paddr, GFP_KERNEL | GFP_DMA, &attrs);
+	if (!base)
+		return -ENOMEM;
+
+	/* Setup filter addresses */
+	for (i = 0; i < NB_H_FILTER; i++) {
+		bdisp_h_filter[i].min = bdisp_h_spec[i].min;
+		bdisp_h_filter[i].max = bdisp_h_spec[i].max;
+		memcpy(base, bdisp_h_spec[i].coef, BDISP_HF_NB);
+		bdisp_h_filter[i].virt = base;
+		bdisp_h_filter[i].paddr = paddr;
+		base += BDISP_HF_NB;
+		paddr += BDISP_HF_NB;
+	}
+
+	for (i = 0; i < NB_V_FILTER; i++) {
+		bdisp_v_filter[i].min = bdisp_v_spec[i].min;
+		bdisp_v_filter[i].max = bdisp_v_spec[i].max;
+		memcpy(base, bdisp_v_spec[i].coef, BDISP_VF_NB);
+		bdisp_v_filter[i].virt = base;
+		bdisp_v_filter[i].paddr = paddr;
+		base += BDISP_VF_NB;
+		paddr += BDISP_VF_NB;
+	}
+
+	return 0;
+}
+
+/**
+ * bdisp_hw_get_hf_addr
+ * @inc:        resize increment
+ *
+ * Find the horizontal filter table that fits the resize increment
+ *
+ * RETURNS:
+ * table physical address
+ */
+static dma_addr_t bdisp_hw_get_hf_addr(u16 inc)
+{
+	unsigned int i;
+
+	for (i = NB_H_FILTER - 1; i > 0; i--)
+		if ((bdisp_h_filter[i].min < inc) &&
+		    (inc <= bdisp_h_filter[i].max))
+			break;
+
+	return bdisp_h_filter[i].paddr;
+}
+
+/**
+ * bdisp_hw_get_vf_addr
+ * @inc:        resize increment
+ *
+ * Find the vertical filter table that fits the resize increment
+ *
+ * RETURNS:
+ * table physical address
+ */
+static dma_addr_t bdisp_hw_get_vf_addr(u16 inc)
+{
+	unsigned int i;
+
+	for (i = NB_V_FILTER - 1; i > 0; i--)
+		if ((bdisp_v_filter[i].min < inc) &&
+		    (inc <= bdisp_v_filter[i].max))
+			break;
+
+	return bdisp_v_filter[i].paddr;
+}
+
+/**
+ * bdisp_hw_get_inc
+ * @from:       input size
+ * @to:         output size
+ * @inc:        resize increment in 6.10 format
+ *
+ * Computes the increment (inverse of scale) in 6.10 format
+ *
+ * RETURNS:
+ * 0 on success
+ */
+static int bdisp_hw_get_inc(u32 from, u32 to, u16 *inc)
+{
+	u32 tmp;
+
+	if (!to)
+		return -EINVAL;
+
+	if (to == from) {
+		*inc = 1 << 10;
+		return 0;
+	}
+
+	tmp = (from << 10) / to;
+	if ((tmp > 0xFFFF) || (!tmp))
+		/* overflow (downscale x 63) or too small (upscale x 1024) */
+		return -EINVAL;
+
+	*inc = (u16)tmp;
+
+	return 0;
+}
+
+/**
+ * bdisp_hw_get_hv_inc
+ * @ctx:        device context
+ * @h_inc:      horizontal increment
+ * @v_inc:      vertical increment
+ *
+ * Computes the horizontal & vertical increments (inverse of scale)
+ *
+ * RETURNS:
+ * 0 on success
+ */
+static int bdisp_hw_get_hv_inc(struct bdisp_ctx *ctx, u16 *h_inc, u16 *v_inc)
+{
+	u32 src_w, src_h, dst_w, dst_h;
+
+	src_w = ctx->src.crop.width;
+	src_h = ctx->src.crop.height;
+	dst_w = ctx->dst.crop.width;
+	dst_h = ctx->dst.crop.height;
+
+	if (bdisp_hw_get_inc(src_w, dst_w, h_inc) ||
+	    bdisp_hw_get_inc(src_h, dst_h, v_inc)) {
+		dev_err(ctx->bdisp_dev->dev,
+			"scale factors failed (%dx%d)->(%dx%d)\n",
+			src_w, src_h, dst_w, dst_h);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/**
+ * bdisp_hw_get_op_cfg
+ * @ctx:        device context
+ * @c:          operation configuration
+ *
+ * Check which blitter operations are expected and sets the scaling increments
+ *
+ * RETURNS:
+ * 0 on success
+ */
+static int bdisp_hw_get_op_cfg(struct bdisp_ctx *ctx, struct bdisp_op_cfg *c)
+{
+	struct device *dev = ctx->bdisp_dev->dev;
+	struct bdisp_frame *src = &ctx->src;
+	struct bdisp_frame *dst = &ctx->dst;
+
+	if (src->width > MAX_SRC_WIDTH * MAX_VERTICAL_STRIDES) {
+		dev_err(dev, "Image width out of HW caps\n");
+		return -EINVAL;
+	}
+
+	c->wide = src->width > MAX_SRC_WIDTH;
+
+	c->hflip = ctx->hflip;
+	c->vflip = ctx->vflip;
+
+	c->src_interlaced = (src->field == V4L2_FIELD_INTERLACED);
+
+	c->src_nbp = src->fmt->nb_planes;
+	c->src_yuv = (src->fmt->pixelformat == V4L2_PIX_FMT_NV12) ||
+			(src->fmt->pixelformat == V4L2_PIX_FMT_YUV420);
+	c->src_420 = c->src_yuv;
+
+	c->dst_nbp = dst->fmt->nb_planes;
+	c->dst_yuv = (dst->fmt->pixelformat == V4L2_PIX_FMT_NV12) ||
+			(dst->fmt->pixelformat == V4L2_PIX_FMT_YUV420);
+	c->dst_420 = c->dst_yuv;
+
+	c->cconv = (c->src_yuv != c->dst_yuv);
+
+	if (bdisp_hw_get_hv_inc(ctx, &c->h_inc, &c->v_inc)) {
+		dev_err(dev, "Scale factor out of HW caps\n");
+		return -EINVAL;
+	}
+
+	/* Deinterlacing adjustment : stretch a field to a frame */
+	if (c->src_interlaced)
+		c->v_inc /= 2;
+
+	if ((c->h_inc != (1 << 10)) || (c->v_inc != (1 << 10)))
+		c->scale = true;
+	else
+		c->scale = false;
+
+	return 0;
+}
+
+/**
+ * bdisp_hw_color_format
+ * @pixelformat: v4l2 pixel format
+ *
+ * v4l2 to bdisp pixel format convert
+ *
+ * RETURNS:
+ * bdisp pixel format
+ */
+static u32 bdisp_hw_color_format(u32 pixelformat)
+{
+	u32 ret;
+
+	switch (pixelformat) {
+	case V4L2_PIX_FMT_YUV420:
+		ret = (BDISP_YUV_3B << BLT_TTY_COL_SHIFT);
+		break;
+	case V4L2_PIX_FMT_NV12:
+		ret = (BDISP_NV12 << BLT_TTY_COL_SHIFT) | BLT_TTY_BIG_END;
+		break;
+	case V4L2_PIX_FMT_RGB565:
+		ret = (BDISP_RGB565 << BLT_TTY_COL_SHIFT);
+		break;
+	case V4L2_PIX_FMT_XBGR32: /* This V4L format actually refers to xRGB */
+		ret = (BDISP_XRGB8888 << BLT_TTY_COL_SHIFT);
+		break;
+	case V4L2_PIX_FMT_RGB24:  /* RGB888 format */
+		ret = (BDISP_RGB888 << BLT_TTY_COL_SHIFT) | BLT_TTY_BIG_END;
+		break;
+	case V4L2_PIX_FMT_ABGR32: /* This V4L format actually refers to ARGB */
+
+	default:
+		ret = (BDISP_ARGB8888 << BLT_TTY_COL_SHIFT) | BLT_TTY_ALPHA_R;
+		break;
+	}
+
+	return ret;
+}
+
+/**
+ * bdisp_hw_build_node
+ * @ctx:        device context
+ * @cfg:        operation configuration
+ * @node:       node to be set
+ * @t_plan:     whether the node refers to a RGB/Y or a CbCr plane
+ * @src_x_offset: x offset in the source image
+ *
+ * Build a node
+ *
+ * RETURNS:
+ * None
+ */
+static void bdisp_hw_build_node(struct bdisp_ctx *ctx,
+				struct bdisp_op_cfg *cfg,
+				struct bdisp_node *node,
+				enum bdisp_target_plan t_plan, int src_x_offset)
+{
+	struct bdisp_frame *src = &ctx->src;
+	struct bdisp_frame *dst = &ctx->dst;
+	u16 h_inc, v_inc, yh_inc, yv_inc;
+	struct v4l2_rect src_rect = src->crop;
+	struct v4l2_rect dst_rect = dst->crop;
+	int dst_x_offset;
+	s32 dst_width = dst->crop.width;
+	u32 src_fmt, dst_fmt;
+	const u32 *ivmx;
+
+	dev_dbg(ctx->bdisp_dev->dev, "%s\n", __func__);
+
+	memset(node, 0, sizeof(*node));
+
+	/* Adjust src and dst areas wrt src_x_offset */
+	src_rect.left += src_x_offset;
+	src_rect.width -= src_x_offset;
+	src_rect.width = min_t(__s32, MAX_SRC_WIDTH, src_rect.width);
+
+	dst_x_offset = (src_x_offset * dst_width) / ctx->src.crop.width;
+	dst_rect.left += dst_x_offset;
+	dst_rect.width = (src_rect.width * dst_width) / ctx->src.crop.width;
+
+	/* General */
+	src_fmt = src->fmt->pixelformat;
+	dst_fmt = dst->fmt->pixelformat;
+
+	node->nip = 0;
+	node->cic = BLT_CIC_ALL_GRP;
+	node->ack = BLT_ACK_BYPASS_S2S3;
+
+	switch (cfg->src_nbp) {
+	case 1:
+		/* Src2 = RGB / Src1 = Src3 = off */
+		node->ins = BLT_INS_S1_OFF | BLT_INS_S2_MEM | BLT_INS_S3_OFF;
+		break;
+	case 2:
+		/* Src3 = Y
+		 * Src2 = CbCr or ColorFill if writing the Y plane
+		 * Src1 = off */
+		node->ins = BLT_INS_S1_OFF | BLT_INS_S3_MEM;
+		if (t_plan == BDISP_Y)
+			node->ins |= BLT_INS_S2_CF;
+		else
+			node->ins |= BLT_INS_S2_MEM;
+		break;
+	case 3:
+	default:
+		/* Src3 = Y
+		 * Src2 = Cb or ColorFill if writing the Y plane
+		 * Src1 = Cr or ColorFill if writing the Y plane */
+		node->ins = BLT_INS_S3_MEM;
+		if (t_plan == BDISP_Y)
+			node->ins |= BLT_INS_S2_CF | BLT_INS_S1_CF;
+		else
+			node->ins |= BLT_INS_S2_MEM | BLT_INS_S1_MEM;
+		break;
+	}
+
+	/* Color convert */
+	node->ins |= cfg->cconv ? BLT_INS_IVMX : 0;
+	/* Scale needed if scaling OR 4:2:0 up/downsampling */
+	node->ins |= (cfg->scale || cfg->src_420 || cfg->dst_420) ?
+			BLT_INS_SCALE : 0;
+
+	/* Target */
+	node->tba = (t_plan == BDISP_CBCR) ? dst->paddr[1] : dst->paddr[0];
+
+	node->tty = dst->bytesperline;
+	node->tty |= bdisp_hw_color_format(dst_fmt);
+	node->tty |= BLT_TTY_DITHER;
+	node->tty |= (t_plan == BDISP_CBCR) ? BLT_TTY_CHROMA : 0;
+	node->tty |= cfg->hflip ? BLT_TTY_HSO : 0;
+	node->tty |= cfg->vflip ? BLT_TTY_VSO : 0;
+
+	if (cfg->dst_420 && (t_plan == BDISP_CBCR)) {
+		/* 420 chroma downsampling */
+		dst_rect.height /= 2;
+		dst_rect.width /= 2;
+		dst_rect.left /= 2;
+		dst_rect.top /= 2;
+		dst_x_offset /= 2;
+		dst_width /= 2;
+	}
+
+	node->txy = cfg->vflip ? (dst_rect.height - 1) : dst_rect.top;
+	node->txy <<= 16;
+	node->txy |= cfg->hflip ? (dst_width - dst_x_offset - 1) :
+			dst_rect.left;
+
+	node->tsz = dst_rect.height << 16 | dst_rect.width;
+
+	if (cfg->src_interlaced) {
+		/* handle only the top field which is half height of a frame */
+		src_rect.top /= 2;
+		src_rect.height /= 2;
+	}
+
+	if (cfg->src_nbp == 1) {
+		/* Src 2 : RGB */
+		node->s2ba = src->paddr[0];
+
+		node->s2ty = src->bytesperline;
+		if (cfg->src_interlaced)
+			node->s2ty *= 2;
+
+		node->s2ty |= bdisp_hw_color_format(src_fmt);
+
+		node->s2xy = src_rect.top << 16 | src_rect.left;
+		node->s2sz = src_rect.height << 16 | src_rect.width;
+	} else {
+		/* Src 2 : Cb or CbCr */
+		if (cfg->src_420) {
+			/* 420 chroma upsampling */
+			src_rect.top /= 2;
+			src_rect.left /= 2;
+			src_rect.width /= 2;
+			src_rect.height /= 2;
+		}
+
+		node->s2ba = src->paddr[1];
+
+		node->s2ty = src->bytesperline;
+		if (cfg->src_nbp == 3)
+			node->s2ty /= 2;
+		if (cfg->src_interlaced)
+			node->s2ty *= 2;
+
+		node->s2ty |= bdisp_hw_color_format(src_fmt);
+
+		node->s2xy = src_rect.top << 16 | src_rect.left;
+		node->s2sz = src_rect.height << 16 | src_rect.width;
+
+		if (cfg->src_nbp == 3) {
+			/* Src 1 : Cr */
+			node->s1ba = src->paddr[2];
+
+			node->s1ty = node->s2ty;
+			node->s1xy = node->s2xy;
+		}
+
+		/* Src 3 : Y */
+		node->s3ba = src->paddr[0];
+
+		node->s3ty = src->bytesperline;
+		if (cfg->src_interlaced)
+			node->s3ty *= 2;
+		node->s3ty |= bdisp_hw_color_format(src_fmt);
+
+		if ((t_plan != BDISP_CBCR) && cfg->src_420) {
+			/* No chroma upsampling for output RGB / Y plane */
+			node->s3xy = node->s2xy * 2;
+			node->s3sz = node->s2sz * 2;
+		} else {
+			/* No need to read Y (Src3) when writing Chroma */
+			node->s3ty |= BLT_S3TY_BLANK_ACC;
+			node->s3xy = node->s2xy;
+			node->s3sz = node->s2sz;
+		}
+	}
+
+	/* Resize (scale OR 4:2:0: chroma up/downsampling) */
+	if (node->ins & BLT_INS_SCALE) {
+		/* no need to compute Y when writing CbCr from RGB input */
+		bool skip_y = (t_plan == BDISP_CBCR) && !cfg->src_yuv;
+
+		/* FCTL */
+		if (cfg->scale) {
+			node->fctl = BLT_FCTL_HV_SCALE;
+			if (!skip_y)
+				node->fctl |= BLT_FCTL_Y_HV_SCALE;
+		} else {
+			node->fctl = BLT_FCTL_HV_SAMPLE;
+			if (!skip_y)
+				node->fctl |= BLT_FCTL_Y_HV_SAMPLE;
+		}
+
+		/* RSF - Chroma may need to be up/downsampled */
+		h_inc = cfg->h_inc;
+		v_inc = cfg->v_inc;
+		if (!cfg->src_420 && cfg->dst_420 && (t_plan == BDISP_CBCR)) {
+			/* RGB to 4:2:0 for Chroma: downsample */
+			h_inc *= 2;
+			v_inc *= 2;
+		} else if (cfg->src_420 && !cfg->dst_420) {
+			/* 4:2:0: to RGB: upsample*/
+			h_inc /= 2;
+			v_inc /= 2;
+		}
+		node->rsf = v_inc << 16 | h_inc;
+
+		/* RZI */
+		node->rzi = BLT_RZI_DEFAULT;
+
+		/* Filter table physical addr */
+		node->hfp = bdisp_hw_get_hf_addr(h_inc);
+		node->vfp = bdisp_hw_get_vf_addr(v_inc);
+
+		/* Y version */
+		if (!skip_y) {
+			yh_inc = cfg->h_inc;
+			yv_inc = cfg->v_inc;
+
+			node->y_rsf = yv_inc << 16 | yh_inc;
+			node->y_rzi = BLT_RZI_DEFAULT;
+			node->y_hfp = bdisp_hw_get_hf_addr(yh_inc);
+			node->y_vfp = bdisp_hw_get_vf_addr(yv_inc);
+		}
+	}
+
+	/* Versatile matrix for RGB / YUV conversion */
+	if (cfg->cconv) {
+		ivmx = cfg->src_yuv ? bdisp_yuv_to_rgb : bdisp_rgb_to_yuv;
+
+		node->ivmx0 = ivmx[0];
+		node->ivmx1 = ivmx[1];
+		node->ivmx2 = ivmx[2];
+		node->ivmx3 = ivmx[3];
+	}
+}
+
+/**
+ * bdisp_hw_build_all_nodes
+ * @ctx:        device context
+ *
+ * Build all the nodes for the blitter operation
+ *
+ * RETURNS:
+ * 0 on success
+ */
+static int bdisp_hw_build_all_nodes(struct bdisp_ctx *ctx)
+{
+	struct bdisp_op_cfg cfg;
+	unsigned int i, nid = 0;
+	int src_x_offset = 0;
+
+	for (i = 0; i < MAX_NB_NODE; i++)
+		if (!ctx->node[i]) {
+			dev_err(ctx->bdisp_dev->dev, "node %d is null\n", i);
+			return -EINVAL;
+		}
+
+	/* Get configuration (scale, flip, ...) */
+	if (bdisp_hw_get_op_cfg(ctx, &cfg))
+		return -EINVAL;
+
+	/* Split source in vertical strides (HW constraint) */
+	for (i = 0; i < MAX_VERTICAL_STRIDES; i++) {
+		/* Build RGB/Y node and link it to the previous node */
+		bdisp_hw_build_node(ctx, &cfg, ctx->node[nid],
+				    cfg.dst_nbp == 1 ? BDISP_RGB : BDISP_Y,
+				    src_x_offset);
+		if (nid)
+			ctx->node[nid - 1]->nip = ctx->node_paddr[nid];
+		nid++;
+
+		/* Build additional Cb(Cr) node, link it to the previous one */
+		if (cfg.dst_nbp > 1) {
+			bdisp_hw_build_node(ctx, &cfg, ctx->node[nid],
+					    BDISP_CBCR, src_x_offset);
+			ctx->node[nid - 1]->nip = ctx->node_paddr[nid];
+			nid++;
+		}
+
+		/* Next stride until full width covered */
+		src_x_offset += MAX_SRC_WIDTH;
+		if (src_x_offset >= ctx->src.crop.width)
+			break;
+	}
+
+	/* Mark last node as the last */
+	ctx->node[nid - 1]->nip = 0;
+
+	return 0;
+}
+
+/**
+ * bdisp_hw_save_request
+ * @ctx:        device context
+ *
+ * Save a copy of the request and of the built nodes
+ *
+ * RETURNS:
+ * None
+ */
+static void bdisp_hw_save_request(struct bdisp_ctx *ctx)
+{
+	struct bdisp_node **copy_node = ctx->bdisp_dev->dbg.copy_node;
+	struct bdisp_request *request = &ctx->bdisp_dev->dbg.copy_request;
+	struct bdisp_node **node = ctx->node;
+	int i;
+
+	/* Request copy */
+	request->src = ctx->src;
+	request->dst = ctx->dst;
+	request->hflip = ctx->hflip;
+	request->vflip = ctx->vflip;
+	request->nb_req++;
+
+	/* Nodes copy */
+	for (i = 0; i < MAX_NB_NODE; i++) {
+		/* Allocate memory if not done yet */
+		if (!copy_node[i]) {
+			copy_node[i] = devm_kzalloc(ctx->bdisp_dev->dev,
+						    sizeof(*copy_node[i]),
+						    GFP_KERNEL);
+			if (!copy_node[i])
+				return;
+		}
+		*copy_node[i] = *node[i];
+	}
+}
+
+/**
+ * bdisp_hw_update
+ * @ctx:        device context
+ *
+ * Send the request to the HW
+ *
+ * RETURNS:
+ * 0 on success
+ */
+int bdisp_hw_update(struct bdisp_ctx *ctx)
+{
+	int ret;
+	struct bdisp_dev *bdisp = ctx->bdisp_dev;
+	struct device *dev = bdisp->dev;
+	unsigned int node_id;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	/* build nodes */
+	ret = bdisp_hw_build_all_nodes(ctx);
+	if (ret) {
+		dev_err(dev, "cannot build nodes (%d)\n", ret);
+		return ret;
+	}
+
+	/* Save a copy of the request */
+	bdisp_hw_save_request(ctx);
+
+	/* Configure interrupt to 'Last Node Reached for AQ1' */
+	writel(BLT_AQ1_CTL_CFG, bdisp->regs + BLT_AQ1_CTL);
+	writel(BLT_ITS_AQ1_LNA, bdisp->regs + BLT_ITM0);
+
+	/* Write first node addr */
+	writel(ctx->node_paddr[0], bdisp->regs + BLT_AQ1_IP);
+
+	/* Find and write last node addr : this starts the HW processing */
+	for (node_id = 0; node_id < MAX_NB_NODE - 1; node_id++) {
+		if (!ctx->node[node_id]->nip)
+			break;
+	}
+	writel(ctx->node_paddr[node_id], bdisp->regs + BLT_AQ1_LNA);
+
+	return 0;
+}
diff --git a/kernel/drivers/media/platform/sti/bdisp/bdisp-reg.h b/kernel/drivers/media/platform/sti/bdisp/bdisp-reg.h
new file mode 100644
index 000000000..e7e1a425f
--- /dev/null
+++ b/kernel/drivers/media/platform/sti/bdisp/bdisp-reg.h
@@ -0,0 +1,235 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2014
+ * Authors: Fabien Dessenne <fabien.dessenne@st.com> for STMicroelectronics.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+struct bdisp_node {
+	/* 0 - General */
+	u32 nip;
+	u32 cic;
+	u32 ins;
+	u32 ack;
+	/* 1 - Target */
+	u32 tba;
+	u32 tty;
+	u32 txy;
+	u32 tsz;
+	/* 2 - Color Fill */
+	u32 s1cf;
+	u32 s2cf;
+	/* 3 - Source 1 */
+	u32 s1ba;
+	u32 s1ty;
+	u32 s1xy;
+	u32 s1sz_tsz;
+	/* 4 - Source 2 */
+	u32 s2ba;
+	u32 s2ty;
+	u32 s2xy;
+	u32 s2sz;
+	/* 5 - Source 3 */
+	u32 s3ba;
+	u32 s3ty;
+	u32 s3xy;
+	u32 s3sz;
+	/* 6 - Clipping */
+	u32 cwo;
+	u32 cws;
+	/* 7 - CLUT */
+	u32 cco;
+	u32 cml;
+	/* 8 - Filter & Mask */
+	u32 fctl;
+	u32 pmk;
+	/* 9 - Chroma Filter */
+	u32 rsf;
+	u32 rzi;
+	u32 hfp;
+	u32 vfp;
+	/* 10 - Luma Filter */
+	u32 y_rsf;
+	u32 y_rzi;
+	u32 y_hfp;
+	u32 y_vfp;
+	/* 11 - Flicker */
+	u32 ff0;
+	u32 ff1;
+	u32 ff2;
+	u32 ff3;
+	/* 12 - Color Key */
+	u32 key1;
+	u32 key2;
+	/* 14 - Static Address & User */
+	u32 sar;
+	u32 usr;
+	/* 15 - Input Versatile Matrix */
+	u32 ivmx0;
+	u32 ivmx1;
+	u32 ivmx2;
+	u32 ivmx3;
+	/* 16 - Output Versatile Matrix */
+	u32 ovmx0;
+	u32 ovmx1;
+	u32 ovmx2;
+	u32 ovmx3;
+	/* 17 - Pace */
+	u32 pace;
+	/* 18 - VC1R & DEI */
+	u32 vc1r;
+	u32 dei;
+	/* 19 - Gradient Fill */
+	u32 hgf;
+	u32 vgf;
+};
+
+/* HW registers : static */
+#define BLT_CTL                 0x0A00
+#define BLT_ITS                 0x0A04
+#define BLT_STA1                0x0A08
+#define BLT_AQ1_CTL             0x0A60
+#define BLT_AQ1_IP              0x0A64
+#define BLT_AQ1_LNA             0x0A68
+#define BLT_AQ1_STA             0x0A6C
+#define BLT_ITM0                0x0AD0
+/* HW registers : plugs */
+#define BLT_PLUGS1_OP2          0x0B04
+#define BLT_PLUGS1_CHZ          0x0B08
+#define BLT_PLUGS1_MSZ          0x0B0C
+#define BLT_PLUGS1_PGZ          0x0B10
+#define BLT_PLUGS2_OP2          0x0B24
+#define BLT_PLUGS2_CHZ          0x0B28
+#define BLT_PLUGS2_MSZ          0x0B2C
+#define BLT_PLUGS2_PGZ          0x0B30
+#define BLT_PLUGS3_OP2          0x0B44
+#define BLT_PLUGS3_CHZ          0x0B48
+#define BLT_PLUGS3_MSZ          0x0B4C
+#define BLT_PLUGS3_PGZ          0x0B50
+#define BLT_PLUGT_OP2           0x0B84
+#define BLT_PLUGT_CHZ           0x0B88
+#define BLT_PLUGT_MSZ           0x0B8C
+#define BLT_PLUGT_PGZ           0x0B90
+/* HW registers : node */
+#define BLT_NIP                 0x0C00
+#define BLT_CIC                 0x0C04
+#define BLT_INS                 0x0C08
+#define BLT_ACK                 0x0C0C
+#define BLT_TBA                 0x0C10
+#define BLT_TTY                 0x0C14
+#define BLT_TXY                 0x0C18
+#define BLT_TSZ                 0x0C1C
+#define BLT_S1BA                0x0C28
+#define BLT_S1TY                0x0C2C
+#define BLT_S1XY                0x0C30
+#define BLT_S2BA                0x0C38
+#define BLT_S2TY                0x0C3C
+#define BLT_S2XY                0x0C40
+#define BLT_S2SZ                0x0C44
+#define BLT_S3BA                0x0C48
+#define BLT_S3TY                0x0C4C
+#define BLT_S3XY                0x0C50
+#define BLT_S3SZ                0x0C54
+#define BLT_FCTL                0x0C68
+#define BLT_RSF                 0x0C70
+#define BLT_RZI                 0x0C74
+#define BLT_HFP                 0x0C78
+#define BLT_VFP                 0x0C7C
+#define BLT_Y_RSF               0x0C80
+#define BLT_Y_RZI               0x0C84
+#define BLT_Y_HFP               0x0C88
+#define BLT_Y_VFP               0x0C8C
+#define BLT_IVMX0               0x0CC0
+#define BLT_IVMX1               0x0CC4
+#define BLT_IVMX2               0x0CC8
+#define BLT_IVMX3               0x0CCC
+#define BLT_OVMX0               0x0CD0
+#define BLT_OVMX1               0x0CD4
+#define BLT_OVMX2               0x0CD8
+#define BLT_OVMX3               0x0CDC
+#define BLT_DEI                 0x0CEC
+/* HW registers : filters */
+#define BLT_HFC_N               0x0D00
+#define BLT_VFC_N               0x0D90
+#define BLT_Y_HFC_N             0x0E00
+#define BLT_Y_VFC_N             0x0E90
+#define BLT_NB_H_COEF           16
+#define BLT_NB_V_COEF           10
+
+/* Registers values */
+#define BLT_CTL_RESET           BIT(31)         /* Global soft reset */
+
+#define BLT_ITS_AQ1_LNA         BIT(12)         /* AQ1 LNA reached */
+
+#define BLT_STA1_IDLE           BIT(0)          /* BDISP idle */
+
+#define BLT_AQ1_CTL_CFG         0x80400003      /* Enable, P3, LNA reached */
+
+#define BLT_INS_S1_MASK         (BIT(0) | BIT(1) | BIT(2))
+#define BLT_INS_S1_OFF          0x00000000      /* src1 disabled */
+#define BLT_INS_S1_MEM          0x00000001      /* src1 fetched from memory */
+#define BLT_INS_S1_CF           0x00000003      /* src1 color fill */
+#define BLT_INS_S1_COPY         0x00000004      /* src1 direct copy */
+#define BLT_INS_S1_FILL         0x00000007      /* src1 firect fill */
+#define BLT_INS_S2_MASK         (BIT(3) | BIT(4))
+#define BLT_INS_S2_OFF          0x00000000      /* src2 disabled */
+#define BLT_INS_S2_MEM          0x00000008      /* src2 fetched from memory */
+#define BLT_INS_S2_CF           0x00000018      /* src2 color fill */
+#define BLT_INS_S3_MASK         BIT(5)
+#define BLT_INS_S3_OFF          0x00000000      /* src3 disabled */
+#define BLT_INS_S3_MEM          0x00000020      /* src3 fetched from memory */
+#define BLT_INS_IVMX            BIT(6)          /* Input versatile matrix */
+#define BLT_INS_CLUT            BIT(7)          /* Color Look Up Table */
+#define BLT_INS_SCALE           BIT(8)          /* Scaling */
+#define BLT_INS_FLICK           BIT(9)          /* Flicker filter */
+#define BLT_INS_CLIP            BIT(10)         /* Clipping */
+#define BLT_INS_CKEY            BIT(11)         /* Color key */
+#define BLT_INS_OVMX            BIT(12)         /* Output versatile matrix */
+#define BLT_INS_DEI             BIT(13)         /* Deinterlace */
+#define BLT_INS_PMASK           BIT(14)         /* Plane mask */
+#define BLT_INS_VC1R            BIT(17)         /* VC1 Range mapping */
+#define BLT_INS_ROTATE          BIT(18)         /* Rotation */
+#define BLT_INS_GRAD            BIT(19)         /* Gradient fill */
+#define BLT_INS_AQLOCK          BIT(29)         /* AQ lock */
+#define BLT_INS_PACE            BIT(30)         /* Pace down */
+#define BLT_INS_IRQ             BIT(31)         /* Raise IRQ when node done */
+#define BLT_CIC_ALL_GRP         0x000FDFFC      /* all valid groups present */
+#define BLT_ACK_BYPASS_S2S3     0x00000007      /* Bypass src2 and src3 */
+
+#define BLT_TTY_COL_SHIFT       16              /* Color format */
+#define BLT_TTY_COL_MASK        0x001F0000      /* Color format mask */
+#define BLT_TTY_ALPHA_R         BIT(21)         /* Alpha range */
+#define BLT_TTY_CR_NOT_CB       BIT(22)         /* CR not Cb */
+#define BLT_TTY_MB              BIT(23)         /* MB frame / field*/
+#define BLT_TTY_HSO             BIT(24)         /* H scan order */
+#define BLT_TTY_VSO             BIT(25)         /* V scan order */
+#define BLT_TTY_DITHER          BIT(26)         /* Dithering */
+#define BLT_TTY_CHROMA          BIT(27)         /* Write chroma / luma */
+#define BLT_TTY_BIG_END         BIT(30)         /* Big endianness */
+
+#define BLT_S1TY_A1_SUBSET      BIT(22)         /* A1 subset */
+#define BLT_S1TY_CHROMA_EXT     BIT(26)         /* Chroma Extended */
+#define BTL_S1TY_SUBBYTE        BIT(28)         /* Sub-byte fmt, pixel order */
+#define BLT_S1TY_RGB_EXP        BIT(29)         /* RGB expansion mode */
+
+#define BLT_S2TY_A1_SUBSET      BIT(22)         /* A1 subset */
+#define BLT_S2TY_CHROMA_EXT     BIT(26)         /* Chroma Extended */
+#define BTL_S2TY_SUBBYTE        BIT(28)         /* Sub-byte fmt, pixel order */
+#define BLT_S2TY_RGB_EXP        BIT(29)         /* RGB expansion mode */
+
+#define BLT_S3TY_BLANK_ACC      BIT(26)         /* Blank access */
+
+#define BLT_FCTL_HV_SCALE       0x00000055      /* H/V resize + color filter */
+#define BLT_FCTL_Y_HV_SCALE     0x33000000      /* Luma version */
+
+#define BLT_FCTL_HV_SAMPLE      0x00000044      /* H/V resize */
+#define BLT_FCTL_Y_HV_SAMPLE    0x22000000      /* Luma version */
+
+#define BLT_RZI_DEFAULT         0x20003000      /* H/VNB_repeat = 3/2 */
+
+/* Color format */
+#define BDISP_RGB565            0x00            /* RGB565 */
+#define BDISP_RGB888            0x01            /* RGB888 */
+#define BDISP_XRGB8888          0x02            /* RGB888_32 */
+#define BDISP_ARGB8888          0x05            /* ARGB888 */
+#define BDISP_NV12              0x16            /* YCbCr42x R2B */
+#define BDISP_YUV_3B            0x1E            /* YUV (3 buffer) */
diff --git a/kernel/drivers/media/platform/sti/bdisp/bdisp-v4l2.c b/kernel/drivers/media/platform/sti/bdisp/bdisp-v4l2.c
new file mode 100644
index 000000000..a0d267e01
--- /dev/null
+++ b/kernel/drivers/media/platform/sti/bdisp/bdisp-v4l2.c
@@ -0,0 +1,1444 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2014
+ * Authors: Fabien Dessenne <fabien.dessenne@st.com> for STMicroelectronics.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+
+#include <media/v4l2-event.h>
+#include <media/v4l2-ioctl.h>
+
+#include "bdisp.h"
+
+#define BDISP_MAX_CTRL_NUM      10
+
+#define BDISP_WORK_TIMEOUT      ((100 * HZ) / 1000)
+
+/* User configuration change */
+#define BDISP_PARAMS            BIT(0) /* Config updated */
+#define BDISP_SRC_FMT           BIT(1) /* Source set */
+#define BDISP_DST_FMT           BIT(2) /* Destination set */
+#define BDISP_CTX_STOP_REQ      BIT(3) /* Stop request */
+#define BDISP_CTX_ABORT         BIT(4) /* Abort while device run */
+
+#define BDISP_MIN_W             1
+#define BDISP_MAX_W             8191
+#define BDISP_MIN_H             1
+#define BDISP_MAX_H             8191
+
+#define fh_to_ctx(__fh) container_of(__fh, struct bdisp_ctx, fh)
+
+enum bdisp_dev_flags {
+	ST_M2M_OPEN,            /* Driver opened */
+	ST_M2M_RUNNING,         /* HW device running */
+	ST_M2M_SUSPENDED,       /* Driver suspended */
+	ST_M2M_SUSPENDING,      /* Driver being suspended */
+};
+
+static const struct bdisp_fmt bdisp_formats[] = {
+	/* ARGB888. [31:0] A:R:G:B 8:8:8:8 little endian */
+	{
+		.pixelformat    = V4L2_PIX_FMT_ABGR32, /* is actually ARGB */
+		.nb_planes      = 1,
+		.bpp            = 32,
+		.bpp_plane0     = 32,
+		.w_align        = 1,
+		.h_align        = 1
+	},
+	/* XRGB888. [31:0] x:R:G:B 8:8:8:8 little endian */
+	{
+		.pixelformat    = V4L2_PIX_FMT_XBGR32, /* is actually xRGB */
+		.nb_planes      = 1,
+		.bpp            = 32,
+		.bpp_plane0     = 32,
+		.w_align        = 1,
+		.h_align        = 1
+	},
+	/* RGB565. [15:0] R:G:B 5:6:5 little endian */
+	{
+		.pixelformat    = V4L2_PIX_FMT_RGB565,
+		.nb_planes      = 1,
+		.bpp            = 16,
+		.bpp_plane0     = 16,
+		.w_align        = 1,
+		.h_align        = 1
+	},
+	/* NV12. YUV420SP - 1 plane for Y + 1 plane for (CbCr) */
+	{
+		.pixelformat    = V4L2_PIX_FMT_NV12,
+		.nb_planes      = 2,
+		.bpp            = 12,
+		.bpp_plane0     = 8,
+		.w_align        = 2,
+		.h_align        = 2
+	},
+	/* RGB888. [23:0] B:G:R 8:8:8 little endian */
+	{
+		.pixelformat    = V4L2_PIX_FMT_RGB24,
+		.nb_planes      = 1,
+		.bpp            = 24,
+		.bpp_plane0     = 24,
+		.w_align        = 1,
+		.h_align        = 1
+	},
+	/* YU12. YUV420P - 1 plane for Y + 1 plane for Cb + 1 plane for Cr
+	 * To keep as the LAST element of this table (no support on capture)
+	 */
+	{
+		.pixelformat    = V4L2_PIX_FMT_YUV420,
+		.nb_planes      = 3,
+		.bpp            = 12,
+		.bpp_plane0     = 8,
+		.w_align        = 2,
+		.h_align        = 2
+	}
+};
+
+/* Default format : HD ARGB32*/
+#define BDISP_DEF_WIDTH         1920
+#define BDISP_DEF_HEIGHT        1080
+
+static const struct bdisp_frame bdisp_dflt_fmt = {
+		.width          = BDISP_DEF_WIDTH,
+		.height         = BDISP_DEF_HEIGHT,
+		.fmt            = &bdisp_formats[0],
+		.field          = V4L2_FIELD_NONE,
+		.bytesperline   = BDISP_DEF_WIDTH * 4,
+		.sizeimage      = BDISP_DEF_WIDTH * BDISP_DEF_HEIGHT * 4,
+		.colorspace     = V4L2_COLORSPACE_REC709,
+		.crop           = {0, 0, BDISP_DEF_WIDTH, BDISP_DEF_HEIGHT},
+		.paddr          = {0, 0, 0, 0}
+};
+
+static inline void bdisp_ctx_state_lock_set(u32 state, struct bdisp_ctx *ctx)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ctx->bdisp_dev->slock, flags);
+	ctx->state |= state;
+	spin_unlock_irqrestore(&ctx->bdisp_dev->slock, flags);
+}
+
+static inline void bdisp_ctx_state_lock_clear(u32 state, struct bdisp_ctx *ctx)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ctx->bdisp_dev->slock, flags);
+	ctx->state &= ~state;
+	spin_unlock_irqrestore(&ctx->bdisp_dev->slock, flags);
+}
+
+static inline bool bdisp_ctx_state_is_set(u32 mask, struct bdisp_ctx *ctx)
+{
+	unsigned long flags;
+	bool ret;
+
+	spin_lock_irqsave(&ctx->bdisp_dev->slock, flags);
+	ret = (ctx->state & mask) == mask;
+	spin_unlock_irqrestore(&ctx->bdisp_dev->slock, flags);
+
+	return ret;
+}
+
+static const struct bdisp_fmt *bdisp_find_fmt(u32 pixelformat)
+{
+	const struct bdisp_fmt *fmt;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(bdisp_formats); i++) {
+		fmt = &bdisp_formats[i];
+		if (fmt->pixelformat == pixelformat)
+			return fmt;
+	}
+
+	return NULL;
+}
+
+static struct bdisp_frame *ctx_get_frame(struct bdisp_ctx *ctx,
+					 enum v4l2_buf_type type)
+{
+	switch (type) {
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+		return &ctx->src;
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		return &ctx->dst;
+	default:
+		dev_err(ctx->bdisp_dev->dev,
+			"Wrong buffer/video queue type (%d)\n", type);
+		break;
+	}
+
+	return ERR_PTR(-EINVAL);
+}
+
+static void bdisp_job_finish(struct bdisp_ctx *ctx, int vb_state)
+{
+	struct vb2_v4l2_buffer *src_vb, *dst_vb;
+
+	if (WARN(!ctx || !ctx->fh.m2m_ctx, "Null hardware context\n"))
+		return;
+
+	dev_dbg(ctx->bdisp_dev->dev, "%s\n", __func__);
+
+	src_vb = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+	dst_vb = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+
+	if (src_vb && dst_vb) {
+		dst_vb->timestamp = src_vb->timestamp;
+		dst_vb->timecode = src_vb->timecode;
+		dst_vb->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+		dst_vb->flags |= src_vb->flags &
+					  V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+
+		v4l2_m2m_buf_done(src_vb, vb_state);
+		v4l2_m2m_buf_done(dst_vb, vb_state);
+
+		v4l2_m2m_job_finish(ctx->bdisp_dev->m2m.m2m_dev,
+				    ctx->fh.m2m_ctx);
+	}
+}
+
+static int bdisp_ctx_stop_req(struct bdisp_ctx *ctx)
+{
+	struct bdisp_ctx *curr_ctx;
+	struct bdisp_dev *bdisp = ctx->bdisp_dev;
+	int ret;
+
+	dev_dbg(ctx->bdisp_dev->dev, "%s\n", __func__);
+
+	cancel_delayed_work(&bdisp->timeout_work);
+
+	curr_ctx = v4l2_m2m_get_curr_priv(bdisp->m2m.m2m_dev);
+	if (!test_bit(ST_M2M_RUNNING, &bdisp->state) || (curr_ctx != ctx))
+		return 0;
+
+	bdisp_ctx_state_lock_set(BDISP_CTX_STOP_REQ, ctx);
+
+	ret = wait_event_timeout(bdisp->irq_queue,
+			!bdisp_ctx_state_is_set(BDISP_CTX_STOP_REQ, ctx),
+			BDISP_WORK_TIMEOUT);
+
+	if (!ret) {
+		dev_err(ctx->bdisp_dev->dev, "%s IRQ timeout\n", __func__);
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static void __bdisp_job_abort(struct bdisp_ctx *ctx)
+{
+	int ret;
+
+	ret = bdisp_ctx_stop_req(ctx);
+	if ((ret == -ETIMEDOUT) || (ctx->state & BDISP_CTX_ABORT)) {
+		bdisp_ctx_state_lock_clear(BDISP_CTX_STOP_REQ | BDISP_CTX_ABORT,
+					   ctx);
+		bdisp_job_finish(ctx, VB2_BUF_STATE_ERROR);
+	}
+}
+
+static void bdisp_job_abort(void *priv)
+{
+	__bdisp_job_abort((struct bdisp_ctx *)priv);
+}
+
+static int bdisp_get_addr(struct bdisp_ctx *ctx, struct vb2_buffer *vb,
+			  struct bdisp_frame *frame, dma_addr_t *paddr)
+{
+	if (!vb || !frame)
+		return -EINVAL;
+
+	paddr[0] = vb2_dma_contig_plane_dma_addr(vb, 0);
+
+	if (frame->fmt->nb_planes > 1)
+		/* UV (NV12) or U (420P) */
+		paddr[1] = (dma_addr_t)(paddr[0] +
+				frame->bytesperline * frame->height);
+
+	if (frame->fmt->nb_planes > 2)
+		/* V (420P) */
+		paddr[2] = (dma_addr_t)(paddr[1] +
+				(frame->bytesperline * frame->height) / 4);
+
+	if (frame->fmt->nb_planes > 3)
+		dev_dbg(ctx->bdisp_dev->dev, "ignoring some planes\n");
+
+	dev_dbg(ctx->bdisp_dev->dev,
+		"%s plane[0]=%pad plane[1]=%pad plane[2]=%pad\n",
+		__func__, &paddr[0], &paddr[1], &paddr[2]);
+
+	return 0;
+}
+
+static int bdisp_get_bufs(struct bdisp_ctx *ctx)
+{
+	struct bdisp_frame *src, *dst;
+	struct vb2_v4l2_buffer *src_vb, *dst_vb;
+	int ret;
+
+	src = &ctx->src;
+	dst = &ctx->dst;
+
+	src_vb = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	ret = bdisp_get_addr(ctx, &src_vb->vb2_buf, src, src->paddr);
+	if (ret)
+		return ret;
+
+	dst_vb = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+	ret = bdisp_get_addr(ctx, &dst_vb->vb2_buf, dst, dst->paddr);
+	if (ret)
+		return ret;
+
+	dst_vb->timestamp = src_vb->timestamp;
+
+	return 0;
+}
+
+static void bdisp_device_run(void *priv)
+{
+	struct bdisp_ctx *ctx = priv;
+	struct bdisp_dev *bdisp;
+	unsigned long flags;
+	int err = 0;
+
+	if (WARN(!ctx, "Null hardware context\n"))
+		return;
+
+	bdisp = ctx->bdisp_dev;
+	dev_dbg(bdisp->dev, "%s\n", __func__);
+	spin_lock_irqsave(&bdisp->slock, flags);
+
+	if (bdisp->m2m.ctx != ctx) {
+		dev_dbg(bdisp->dev, "ctx updated: %p -> %p\n",
+			bdisp->m2m.ctx, ctx);
+		ctx->state |= BDISP_PARAMS;
+		bdisp->m2m.ctx = ctx;
+	}
+
+	if (ctx->state & BDISP_CTX_STOP_REQ) {
+		ctx->state &= ~BDISP_CTX_STOP_REQ;
+		ctx->state |= BDISP_CTX_ABORT;
+		wake_up(&bdisp->irq_queue);
+		goto out;
+	}
+
+	err = bdisp_get_bufs(ctx);
+	if (err) {
+		dev_err(bdisp->dev, "cannot get address\n");
+		goto out;
+	}
+
+	bdisp_dbg_perf_begin(bdisp);
+
+	err = bdisp_hw_reset(bdisp);
+	if (err) {
+		dev_err(bdisp->dev, "could not get HW ready\n");
+		goto out;
+	}
+
+	err = bdisp_hw_update(ctx);
+	if (err) {
+		dev_err(bdisp->dev, "could not send HW request\n");
+		goto out;
+	}
+
+	queue_delayed_work(bdisp->work_queue, &bdisp->timeout_work,
+			   BDISP_WORK_TIMEOUT);
+	set_bit(ST_M2M_RUNNING, &bdisp->state);
+out:
+	ctx->state &= ~BDISP_PARAMS;
+	spin_unlock_irqrestore(&bdisp->slock, flags);
+	if (err)
+		bdisp_job_finish(ctx, VB2_BUF_STATE_ERROR);
+}
+
+static struct v4l2_m2m_ops bdisp_m2m_ops = {
+	.device_run     = bdisp_device_run,
+	.job_abort      = bdisp_job_abort,
+};
+
+static int __bdisp_s_ctrl(struct bdisp_ctx *ctx, struct v4l2_ctrl *ctrl)
+{
+	if (ctrl->flags & V4L2_CTRL_FLAG_INACTIVE)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_HFLIP:
+		ctx->hflip = ctrl->val;
+		break;
+	case V4L2_CID_VFLIP:
+		ctx->vflip = ctrl->val;
+		break;
+	default:
+		dev_err(ctx->bdisp_dev->dev, "unknown control %d\n", ctrl->id);
+		return -EINVAL;
+	}
+
+	ctx->state |= BDISP_PARAMS;
+
+	return 0;
+}
+
+static int bdisp_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct bdisp_ctx *ctx = container_of(ctrl->handler, struct bdisp_ctx,
+						ctrl_handler);
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&ctx->bdisp_dev->slock, flags);
+	ret = __bdisp_s_ctrl(ctx, ctrl);
+	spin_unlock_irqrestore(&ctx->bdisp_dev->slock, flags);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops bdisp_c_ops = {
+	.s_ctrl = bdisp_s_ctrl,
+};
+
+static int bdisp_ctrls_create(struct bdisp_ctx *ctx)
+{
+	if (ctx->ctrls_rdy)
+		return 0;
+
+	v4l2_ctrl_handler_init(&ctx->ctrl_handler, BDISP_MAX_CTRL_NUM);
+
+	ctx->bdisp_ctrls.hflip = v4l2_ctrl_new_std(&ctx->ctrl_handler,
+				&bdisp_c_ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
+	ctx->bdisp_ctrls.vflip = v4l2_ctrl_new_std(&ctx->ctrl_handler,
+				&bdisp_c_ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	if (ctx->ctrl_handler.error) {
+		int err = ctx->ctrl_handler.error;
+
+		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+		return err;
+	}
+
+	ctx->ctrls_rdy = true;
+
+	return 0;
+}
+
+static void bdisp_ctrls_delete(struct bdisp_ctx *ctx)
+{
+	if (ctx->ctrls_rdy) {
+		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+		ctx->ctrls_rdy = false;
+	}
+}
+
+static int bdisp_queue_setup(struct vb2_queue *vq,
+			     const void *parg,
+			     unsigned int *nb_buf, unsigned int *nb_planes,
+			     unsigned int sizes[], void *allocators[])
+{
+	const struct v4l2_format *fmt = parg;
+	struct bdisp_ctx *ctx = vb2_get_drv_priv(vq);
+	struct bdisp_frame *frame = ctx_get_frame(ctx, vq->type);
+
+	if (IS_ERR(frame)) {
+		dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
+		return PTR_ERR(frame);
+	}
+
+	if (!frame->fmt) {
+		dev_err(ctx->bdisp_dev->dev, "Invalid format\n");
+		return -EINVAL;
+	}
+
+	if (fmt && fmt->fmt.pix.sizeimage < frame->sizeimage)
+		return -EINVAL;
+
+	*nb_planes = 1;
+	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : frame->sizeimage;
+	allocators[0] = ctx->bdisp_dev->alloc_ctx;
+
+	return 0;
+}
+
+static int bdisp_buf_prepare(struct vb2_buffer *vb)
+{
+	struct bdisp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct bdisp_frame *frame = ctx_get_frame(ctx, vb->vb2_queue->type);
+
+	if (IS_ERR(frame)) {
+		dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
+		return PTR_ERR(frame);
+	}
+
+	if (vb->vb2_queue->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		vb2_set_plane_payload(vb, 0, frame->sizeimage);
+
+	return 0;
+}
+
+static void bdisp_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct bdisp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+
+	/* return to V4L2 any 0-size buffer so it can be dequeued by user */
+	if (!vb2_get_plane_payload(vb, 0)) {
+		dev_dbg(ctx->bdisp_dev->dev, "0 data buffer, skip it\n");
+		vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
+		return;
+	}
+
+	if (ctx->fh.m2m_ctx)
+		v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
+}
+
+static int bdisp_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct bdisp_ctx *ctx = q->drv_priv;
+	struct vb2_v4l2_buffer *buf;
+	int ret = pm_runtime_get_sync(ctx->bdisp_dev->dev);
+
+	if (ret < 0) {
+		dev_err(ctx->bdisp_dev->dev, "failed to set runtime PM\n");
+
+		if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+			while ((buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx)))
+				v4l2_m2m_buf_done(buf, VB2_BUF_STATE_QUEUED);
+		} else {
+			while ((buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx)))
+				v4l2_m2m_buf_done(buf, VB2_BUF_STATE_QUEUED);
+		}
+
+		return ret;
+	}
+
+	return 0;
+}
+
+static void bdisp_stop_streaming(struct vb2_queue *q)
+{
+	struct bdisp_ctx *ctx = q->drv_priv;
+
+	__bdisp_job_abort(ctx);
+
+	pm_runtime_put(ctx->bdisp_dev->dev);
+}
+
+static struct vb2_ops bdisp_qops = {
+	.queue_setup     = bdisp_queue_setup,
+	.buf_prepare     = bdisp_buf_prepare,
+	.buf_queue       = bdisp_buf_queue,
+	.wait_prepare    = vb2_ops_wait_prepare,
+	.wait_finish     = vb2_ops_wait_finish,
+	.stop_streaming  = bdisp_stop_streaming,
+	.start_streaming = bdisp_start_streaming,
+};
+
+static int queue_init(void *priv,
+		      struct vb2_queue *src_vq, struct vb2_queue *dst_vq)
+{
+	struct bdisp_ctx *ctx = priv;
+	int ret;
+
+	memset(src_vq, 0, sizeof(*src_vq));
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	src_vq->drv_priv = ctx;
+	src_vq->ops = &bdisp_qops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->lock = &ctx->bdisp_dev->lock;
+
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	memset(dst_vq, 0, sizeof(*dst_vq));
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	dst_vq->drv_priv = ctx;
+	dst_vq->ops = &bdisp_qops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->lock = &ctx->bdisp_dev->lock;
+
+	return vb2_queue_init(dst_vq);
+}
+
+static int bdisp_open(struct file *file)
+{
+	struct bdisp_dev *bdisp = video_drvdata(file);
+	struct bdisp_ctx *ctx = NULL;
+	int ret;
+
+	if (mutex_lock_interruptible(&bdisp->lock))
+		return -ERESTARTSYS;
+
+	/* Allocate memory for both context and node */
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx) {
+		ret = -ENOMEM;
+		goto unlock;
+	}
+	ctx->bdisp_dev = bdisp;
+
+	if (bdisp_hw_alloc_nodes(ctx)) {
+		dev_err(bdisp->dev, "no memory for nodes\n");
+		ret = -ENOMEM;
+		goto mem_ctx;
+	}
+
+	v4l2_fh_init(&ctx->fh, bdisp->m2m.vdev);
+
+	ret = bdisp_ctrls_create(ctx);
+	if (ret) {
+		dev_err(bdisp->dev, "Failed to create control\n");
+		goto error_fh;
+	}
+
+	/* Use separate control handler per file handle */
+	ctx->fh.ctrl_handler = &ctx->ctrl_handler;
+	file->private_data = &ctx->fh;
+	v4l2_fh_add(&ctx->fh);
+
+	/* Default format */
+	ctx->src = bdisp_dflt_fmt;
+	ctx->dst = bdisp_dflt_fmt;
+
+	/* Setup the device context for mem2mem mode. */
+	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(bdisp->m2m.m2m_dev, ctx,
+					    queue_init);
+	if (IS_ERR(ctx->fh.m2m_ctx)) {
+		dev_err(bdisp->dev, "Failed to initialize m2m context\n");
+		ret = PTR_ERR(ctx->fh.m2m_ctx);
+		goto error_ctrls;
+	}
+
+	bdisp->m2m.refcnt++;
+	set_bit(ST_M2M_OPEN, &bdisp->state);
+
+	dev_dbg(bdisp->dev, "driver opened, ctx = 0x%p\n", ctx);
+
+	mutex_unlock(&bdisp->lock);
+
+	return 0;
+
+error_ctrls:
+	bdisp_ctrls_delete(ctx);
+error_fh:
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	bdisp_hw_free_nodes(ctx);
+mem_ctx:
+	kfree(ctx);
+unlock:
+	mutex_unlock(&bdisp->lock);
+
+	return ret;
+}
+
+static int bdisp_release(struct file *file)
+{
+	struct bdisp_ctx *ctx = fh_to_ctx(file->private_data);
+	struct bdisp_dev *bdisp = ctx->bdisp_dev;
+
+	dev_dbg(bdisp->dev, "%s\n", __func__);
+
+	if (mutex_lock_interruptible(&bdisp->lock))
+		return -ERESTARTSYS;
+
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+
+	bdisp_ctrls_delete(ctx);
+
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+
+	if (--bdisp->m2m.refcnt <= 0)
+		clear_bit(ST_M2M_OPEN, &bdisp->state);
+
+	bdisp_hw_free_nodes(ctx);
+
+	kfree(ctx);
+
+	mutex_unlock(&bdisp->lock);
+
+	return 0;
+}
+
+static const struct v4l2_file_operations bdisp_fops = {
+	.owner          = THIS_MODULE,
+	.open           = bdisp_open,
+	.release        = bdisp_release,
+	.poll           = v4l2_m2m_fop_poll,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap           = v4l2_m2m_fop_mmap,
+};
+
+static int bdisp_querycap(struct file *file, void *fh,
+			  struct v4l2_capability *cap)
+{
+	struct bdisp_ctx *ctx = fh_to_ctx(fh);
+	struct bdisp_dev *bdisp = ctx->bdisp_dev;
+
+	strlcpy(cap->driver, bdisp->pdev->name, sizeof(cap->driver));
+	strlcpy(cap->card, bdisp->pdev->name, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s%d",
+		 BDISP_NAME, bdisp->id);
+
+	cap->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M;
+
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+	return 0;
+}
+
+static int bdisp_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f)
+{
+	struct bdisp_ctx *ctx = fh_to_ctx(fh);
+	const struct bdisp_fmt *fmt;
+
+	if (f->index >= ARRAY_SIZE(bdisp_formats))
+		return -EINVAL;
+
+	fmt = &bdisp_formats[f->index];
+
+	if ((fmt->pixelformat == V4L2_PIX_FMT_YUV420) &&
+	    (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)) {
+		dev_dbg(ctx->bdisp_dev->dev, "No YU12 on capture\n");
+		return -EINVAL;
+	}
+	f->pixelformat = fmt->pixelformat;
+
+	return 0;
+}
+
+static int bdisp_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct bdisp_ctx *ctx = fh_to_ctx(fh);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct bdisp_frame *frame  = ctx_get_frame(ctx, f->type);
+
+	if (IS_ERR(frame)) {
+		dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
+		return PTR_ERR(frame);
+	}
+
+	pix = &f->fmt.pix;
+	pix->width = frame->width;
+	pix->height = frame->height;
+	pix->pixelformat = frame->fmt->pixelformat;
+	pix->field = frame->field;
+	pix->bytesperline = frame->bytesperline;
+	pix->sizeimage = frame->sizeimage;
+	pix->colorspace = (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) ?
+				frame->colorspace : bdisp_dflt_fmt.colorspace;
+
+	return 0;
+}
+
+static int bdisp_try_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct bdisp_ctx *ctx = fh_to_ctx(fh);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	const struct bdisp_fmt *format;
+	u32 in_w, in_h;
+
+	format = bdisp_find_fmt(pix->pixelformat);
+	if (!format) {
+		dev_dbg(ctx->bdisp_dev->dev, "Unknown format 0x%x\n",
+			pix->pixelformat);
+		return -EINVAL;
+	}
+
+	/* YUV420P only supported for VIDEO_OUTPUT */
+	if ((format->pixelformat == V4L2_PIX_FMT_YUV420) &&
+	    (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)) {
+		dev_dbg(ctx->bdisp_dev->dev, "No YU12 on capture\n");
+		return -EINVAL;
+	}
+
+	/* Field (interlaced only supported on OUTPUT) */
+	if ((f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) ||
+	    (pix->field != V4L2_FIELD_INTERLACED))
+		pix->field = V4L2_FIELD_NONE;
+
+	/* Adjust width & height */
+	in_w = pix->width;
+	in_h = pix->height;
+	v4l_bound_align_image(&pix->width,
+			      BDISP_MIN_W, BDISP_MAX_W,
+			      ffs(format->w_align) - 1,
+			      &pix->height,
+			      BDISP_MIN_H, BDISP_MAX_H,
+			      ffs(format->h_align) - 1,
+			      0);
+	if ((pix->width != in_w) || (pix->height != in_h))
+		dev_dbg(ctx->bdisp_dev->dev,
+			"%s size updated: %dx%d -> %dx%d\n", __func__,
+			in_w, in_h, pix->width, pix->height);
+
+	pix->bytesperline = (pix->width * format->bpp_plane0) / 8;
+	pix->sizeimage = (pix->width * pix->height * format->bpp) / 8;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		pix->colorspace = bdisp_dflt_fmt.colorspace;
+
+	return 0;
+}
+
+static int bdisp_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct bdisp_ctx *ctx = fh_to_ctx(fh);
+	struct vb2_queue *vq;
+	struct bdisp_frame *frame;
+	struct v4l2_pix_format *pix;
+	int ret;
+	u32 state;
+
+	ret = bdisp_try_fmt(file, fh, f);
+	if (ret) {
+		dev_err(ctx->bdisp_dev->dev, "Cannot set format\n");
+		return ret;
+	}
+
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+	if (vb2_is_streaming(vq)) {
+		dev_err(ctx->bdisp_dev->dev, "queue (%d) busy\n", f->type);
+		return -EBUSY;
+	}
+
+	frame = (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) ?
+			&ctx->src : &ctx->dst;
+	pix = &f->fmt.pix;
+	frame->fmt = bdisp_find_fmt(pix->pixelformat);
+	if (!frame->fmt) {
+		dev_err(ctx->bdisp_dev->dev, "Unknown format 0x%x\n",
+			pix->pixelformat);
+		return -EINVAL;
+	}
+
+	frame->width = pix->width;
+	frame->height = pix->height;
+	frame->bytesperline = pix->bytesperline;
+	frame->sizeimage = pix->sizeimage;
+	frame->field = pix->field;
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		frame->colorspace = pix->colorspace;
+
+	frame->crop.width = frame->width;
+	frame->crop.height = frame->height;
+	frame->crop.left = 0;
+	frame->crop.top = 0;
+
+	state = BDISP_PARAMS;
+	state |= (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) ?
+			BDISP_DST_FMT : BDISP_SRC_FMT;
+	bdisp_ctx_state_lock_set(state, ctx);
+
+	return 0;
+}
+
+static int bdisp_g_selection(struct file *file, void *fh,
+			     struct v4l2_selection *s)
+{
+	struct bdisp_frame *frame;
+	struct bdisp_ctx *ctx = fh_to_ctx(fh);
+
+	frame = ctx_get_frame(ctx, s->type);
+	if (IS_ERR(frame)) {
+		dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
+		return PTR_ERR(frame);
+	}
+
+	switch (s->type) {
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+		switch (s->target) {
+		case V4L2_SEL_TGT_CROP:
+			/* cropped frame */
+			s->r = frame->crop;
+			break;
+		case V4L2_SEL_TGT_CROP_DEFAULT:
+		case V4L2_SEL_TGT_CROP_BOUNDS:
+			/* complete frame */
+			s->r.left = 0;
+			s->r.top = 0;
+			s->r.width = frame->width;
+			s->r.height = frame->height;
+			break;
+		default:
+			dev_err(ctx->bdisp_dev->dev, "Invalid target\n");
+			return -EINVAL;
+		}
+		break;
+
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		switch (s->target) {
+		case V4L2_SEL_TGT_COMPOSE:
+		case V4L2_SEL_TGT_COMPOSE_PADDED:
+			/* composed (cropped) frame */
+			s->r = frame->crop;
+			break;
+		case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+		case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+			/* complete frame */
+			s->r.left = 0;
+			s->r.top = 0;
+			s->r.width = frame->width;
+			s->r.height = frame->height;
+			break;
+		default:
+			dev_err(ctx->bdisp_dev->dev, "Invalid target\n");
+			return -EINVAL;
+		}
+		break;
+
+	default:
+		dev_err(ctx->bdisp_dev->dev, "Invalid type\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int is_rect_enclosed(struct v4l2_rect *a, struct v4l2_rect *b)
+{
+	/* Return 1 if a is enclosed in b, or 0 otherwise. */
+
+	if (a->left < b->left || a->top < b->top)
+		return 0;
+
+	if (a->left + a->width > b->left + b->width)
+		return 0;
+
+	if (a->top + a->height > b->top + b->height)
+		return 0;
+
+	return 1;
+}
+
+static int bdisp_s_selection(struct file *file, void *fh,
+			     struct v4l2_selection *s)
+{
+	struct bdisp_frame *frame;
+	struct bdisp_ctx *ctx = fh_to_ctx(fh);
+	struct v4l2_rect *in, out;
+	bool valid = false;
+
+	if ((s->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) &&
+	    (s->target == V4L2_SEL_TGT_CROP))
+		valid = true;
+
+	if ((s->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) &&
+	    (s->target == V4L2_SEL_TGT_COMPOSE))
+		valid = true;
+
+	if (!valid) {
+		dev_err(ctx->bdisp_dev->dev, "Invalid type / target\n");
+		return -EINVAL;
+	}
+
+	frame = ctx_get_frame(ctx, s->type);
+	if (IS_ERR(frame)) {
+		dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
+		return PTR_ERR(frame);
+	}
+
+	in = &s->r;
+	out = *in;
+
+	/* Align and check origin */
+	out.left = ALIGN(in->left, frame->fmt->w_align);
+	out.top = ALIGN(in->top, frame->fmt->h_align);
+
+	if ((out.left < 0) || (out.left >= frame->width) ||
+	    (out.top < 0) || (out.top >= frame->height)) {
+		dev_err(ctx->bdisp_dev->dev,
+			"Invalid crop: %dx%d@(%d,%d) vs frame: %dx%d\n",
+			out.width, out.height, out.left, out.top,
+			frame->width, frame->height);
+		return -EINVAL;
+	}
+
+	/* Align and check size */
+	out.width = ALIGN(in->width, frame->fmt->w_align);
+	out.height = ALIGN(in->height, frame->fmt->w_align);
+
+	if (((out.left + out.width) > frame->width) ||
+	    ((out.top + out.height) > frame->height)) {
+		dev_err(ctx->bdisp_dev->dev,
+			"Invalid crop: %dx%d@(%d,%d) vs frame: %dx%d\n",
+			out.width, out.height, out.left, out.top,
+			frame->width, frame->height);
+		return -EINVAL;
+	}
+
+	/* Checks adjust constraints flags */
+	if (s->flags & V4L2_SEL_FLAG_LE && !is_rect_enclosed(&out, in))
+		return -ERANGE;
+
+	if (s->flags & V4L2_SEL_FLAG_GE && !is_rect_enclosed(in, &out))
+		return -ERANGE;
+
+	if ((out.left != in->left) || (out.top != in->top) ||
+	    (out.width != in->width) || (out.height != in->height)) {
+		dev_dbg(ctx->bdisp_dev->dev,
+			"%s crop updated: %dx%d@(%d,%d) -> %dx%d@(%d,%d)\n",
+			__func__, in->width, in->height, in->left, in->top,
+			out.width, out.height, out.left, out.top);
+		*in = out;
+	}
+
+	frame->crop = out;
+
+	bdisp_ctx_state_lock_set(BDISP_PARAMS, ctx);
+
+	return 0;
+}
+
+static int bdisp_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
+{
+	struct bdisp_ctx *ctx = fh_to_ctx(fh);
+
+	if ((type == V4L2_BUF_TYPE_VIDEO_OUTPUT) &&
+	    !bdisp_ctx_state_is_set(BDISP_SRC_FMT, ctx)) {
+		dev_err(ctx->bdisp_dev->dev, "src not defined\n");
+		return -EINVAL;
+	}
+
+	if ((type == V4L2_BUF_TYPE_VIDEO_CAPTURE) &&
+	    !bdisp_ctx_state_is_set(BDISP_DST_FMT, ctx)) {
+		dev_err(ctx->bdisp_dev->dev, "dst not defined\n");
+		return -EINVAL;
+	}
+
+	return v4l2_m2m_streamon(file, ctx->fh.m2m_ctx, type);
+}
+
+static const struct v4l2_ioctl_ops bdisp_ioctl_ops = {
+	.vidioc_querycap                = bdisp_querycap,
+	.vidioc_enum_fmt_vid_cap        = bdisp_enum_fmt,
+	.vidioc_enum_fmt_vid_out        = bdisp_enum_fmt,
+	.vidioc_g_fmt_vid_cap           = bdisp_g_fmt,
+	.vidioc_g_fmt_vid_out           = bdisp_g_fmt,
+	.vidioc_try_fmt_vid_cap         = bdisp_try_fmt,
+	.vidioc_try_fmt_vid_out         = bdisp_try_fmt,
+	.vidioc_s_fmt_vid_cap           = bdisp_s_fmt,
+	.vidioc_s_fmt_vid_out           = bdisp_s_fmt,
+	.vidioc_g_selection		= bdisp_g_selection,
+	.vidioc_s_selection		= bdisp_s_selection,
+	.vidioc_reqbufs                 = v4l2_m2m_ioctl_reqbufs,
+	.vidioc_create_bufs             = v4l2_m2m_ioctl_create_bufs,
+	.vidioc_expbuf                  = v4l2_m2m_ioctl_expbuf,
+	.vidioc_querybuf                = v4l2_m2m_ioctl_querybuf,
+	.vidioc_qbuf                    = v4l2_m2m_ioctl_qbuf,
+	.vidioc_dqbuf                   = v4l2_m2m_ioctl_dqbuf,
+	.vidioc_streamon                = bdisp_streamon,
+	.vidioc_streamoff               = v4l2_m2m_ioctl_streamoff,
+	.vidioc_subscribe_event         = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
+};
+
+static int bdisp_register_device(struct bdisp_dev *bdisp)
+{
+	int ret;
+
+	if (!bdisp)
+		return -ENODEV;
+
+	bdisp->vdev.fops        = &bdisp_fops;
+	bdisp->vdev.ioctl_ops   = &bdisp_ioctl_ops;
+	bdisp->vdev.release     = video_device_release_empty;
+	bdisp->vdev.lock        = &bdisp->lock;
+	bdisp->vdev.vfl_dir     = VFL_DIR_M2M;
+	bdisp->vdev.v4l2_dev    = &bdisp->v4l2_dev;
+	snprintf(bdisp->vdev.name, sizeof(bdisp->vdev.name), "%s.%d",
+		 BDISP_NAME, bdisp->id);
+
+	video_set_drvdata(&bdisp->vdev, bdisp);
+
+	bdisp->m2m.vdev = &bdisp->vdev;
+	bdisp->m2m.m2m_dev = v4l2_m2m_init(&bdisp_m2m_ops);
+	if (IS_ERR(bdisp->m2m.m2m_dev)) {
+		dev_err(bdisp->dev, "failed to initialize v4l2-m2m device\n");
+		return PTR_ERR(bdisp->m2m.m2m_dev);
+	}
+
+	ret = video_register_device(&bdisp->vdev, VFL_TYPE_GRABBER, -1);
+	if (ret) {
+		dev_err(bdisp->dev,
+			"%s(): failed to register video device\n", __func__);
+		v4l2_m2m_release(bdisp->m2m.m2m_dev);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void bdisp_unregister_device(struct bdisp_dev *bdisp)
+{
+	if (!bdisp)
+		return;
+
+	if (bdisp->m2m.m2m_dev)
+		v4l2_m2m_release(bdisp->m2m.m2m_dev);
+
+	video_unregister_device(bdisp->m2m.vdev);
+}
+
+static irqreturn_t bdisp_irq_thread(int irq, void *priv)
+{
+	struct bdisp_dev *bdisp = priv;
+	struct bdisp_ctx *ctx;
+
+	spin_lock(&bdisp->slock);
+
+	bdisp_dbg_perf_end(bdisp);
+
+	cancel_delayed_work(&bdisp->timeout_work);
+
+	if (!test_and_clear_bit(ST_M2M_RUNNING, &bdisp->state))
+		goto isr_unlock;
+
+	if (test_and_clear_bit(ST_M2M_SUSPENDING, &bdisp->state)) {
+		set_bit(ST_M2M_SUSPENDED, &bdisp->state);
+		wake_up(&bdisp->irq_queue);
+		goto isr_unlock;
+	}
+
+	ctx = v4l2_m2m_get_curr_priv(bdisp->m2m.m2m_dev);
+	if (!ctx || !ctx->fh.m2m_ctx)
+		goto isr_unlock;
+
+	spin_unlock(&bdisp->slock);
+
+	bdisp_job_finish(ctx, VB2_BUF_STATE_DONE);
+
+	if (bdisp_ctx_state_is_set(BDISP_CTX_STOP_REQ, ctx)) {
+		bdisp_ctx_state_lock_clear(BDISP_CTX_STOP_REQ, ctx);
+		wake_up(&bdisp->irq_queue);
+	}
+
+	return IRQ_HANDLED;
+
+isr_unlock:
+	spin_unlock(&bdisp->slock);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t bdisp_irq_handler(int irq, void *priv)
+{
+	if (bdisp_hw_get_and_clear_irq((struct bdisp_dev *)priv))
+		return IRQ_NONE;
+	else
+		return IRQ_WAKE_THREAD;
+}
+
+static void bdisp_irq_timeout(struct work_struct *ptr)
+{
+	struct delayed_work *twork = to_delayed_work(ptr);
+	struct bdisp_dev *bdisp = container_of(twork, struct bdisp_dev,
+			timeout_work);
+	struct bdisp_ctx *ctx;
+
+	ctx = v4l2_m2m_get_curr_priv(bdisp->m2m.m2m_dev);
+
+	dev_err(ctx->bdisp_dev->dev, "Device work timeout\n");
+
+	spin_lock(&bdisp->slock);
+	clear_bit(ST_M2M_RUNNING, &bdisp->state);
+	spin_unlock(&bdisp->slock);
+
+	bdisp_hw_reset(bdisp);
+
+	bdisp_job_finish(ctx, VB2_BUF_STATE_ERROR);
+}
+
+static int bdisp_m2m_suspend(struct bdisp_dev *bdisp)
+{
+	unsigned long flags;
+	int timeout;
+
+	spin_lock_irqsave(&bdisp->slock, flags);
+	if (!test_bit(ST_M2M_RUNNING, &bdisp->state)) {
+		spin_unlock_irqrestore(&bdisp->slock, flags);
+		return 0;
+	}
+	clear_bit(ST_M2M_SUSPENDED, &bdisp->state);
+	set_bit(ST_M2M_SUSPENDING, &bdisp->state);
+	spin_unlock_irqrestore(&bdisp->slock, flags);
+
+	timeout = wait_event_timeout(bdisp->irq_queue,
+				     test_bit(ST_M2M_SUSPENDED, &bdisp->state),
+				     BDISP_WORK_TIMEOUT);
+
+	clear_bit(ST_M2M_SUSPENDING, &bdisp->state);
+
+	if (!timeout) {
+		dev_err(bdisp->dev, "%s IRQ timeout\n", __func__);
+		return -EAGAIN;
+	}
+
+	return 0;
+}
+
+static int bdisp_m2m_resume(struct bdisp_dev *bdisp)
+{
+	struct bdisp_ctx *ctx;
+	unsigned long flags;
+
+	spin_lock_irqsave(&bdisp->slock, flags);
+	ctx = bdisp->m2m.ctx;
+	bdisp->m2m.ctx = NULL;
+	spin_unlock_irqrestore(&bdisp->slock, flags);
+
+	if (test_and_clear_bit(ST_M2M_SUSPENDED, &bdisp->state))
+		bdisp_job_finish(ctx, VB2_BUF_STATE_ERROR);
+
+	return 0;
+}
+
+static int bdisp_runtime_resume(struct device *dev)
+{
+	struct bdisp_dev *bdisp = dev_get_drvdata(dev);
+	int ret = clk_enable(bdisp->clock);
+
+	if (ret)
+		return ret;
+
+	return bdisp_m2m_resume(bdisp);
+}
+
+static int bdisp_runtime_suspend(struct device *dev)
+{
+	struct bdisp_dev *bdisp = dev_get_drvdata(dev);
+	int ret = bdisp_m2m_suspend(bdisp);
+
+	if (!ret)
+		clk_disable(bdisp->clock);
+
+	return ret;
+}
+
+static int bdisp_resume(struct device *dev)
+{
+	struct bdisp_dev *bdisp = dev_get_drvdata(dev);
+	unsigned long flags;
+	int opened;
+
+	spin_lock_irqsave(&bdisp->slock, flags);
+	opened = test_bit(ST_M2M_OPEN, &bdisp->state);
+	spin_unlock_irqrestore(&bdisp->slock, flags);
+
+	if (!opened)
+		return 0;
+
+	if (!pm_runtime_suspended(dev))
+		return bdisp_runtime_resume(dev);
+
+	return 0;
+}
+
+static int bdisp_suspend(struct device *dev)
+{
+	if (!pm_runtime_suspended(dev))
+		return bdisp_runtime_suspend(dev);
+
+	return 0;
+}
+
+static const struct dev_pm_ops bdisp_pm_ops = {
+	.suspend                = bdisp_suspend,
+	.resume                 = bdisp_resume,
+	.runtime_suspend        = bdisp_runtime_suspend,
+	.runtime_resume         = bdisp_runtime_resume,
+};
+
+static int bdisp_remove(struct platform_device *pdev)
+{
+	struct bdisp_dev *bdisp = platform_get_drvdata(pdev);
+
+	bdisp_unregister_device(bdisp);
+
+	bdisp_hw_free_filters(bdisp->dev);
+
+	vb2_dma_contig_cleanup_ctx(bdisp->alloc_ctx);
+
+	pm_runtime_disable(&pdev->dev);
+
+	bdisp_debugfs_remove(bdisp);
+
+	v4l2_device_unregister(&bdisp->v4l2_dev);
+
+	if (!IS_ERR(bdisp->clock))
+		clk_unprepare(bdisp->clock);
+
+	dev_dbg(&pdev->dev, "%s driver unloaded\n", pdev->name);
+
+	return 0;
+}
+
+static int bdisp_probe(struct platform_device *pdev)
+{
+	struct bdisp_dev *bdisp;
+	struct resource *res;
+	struct device *dev = &pdev->dev;
+	int ret;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	bdisp = devm_kzalloc(dev, sizeof(struct bdisp_dev), GFP_KERNEL);
+	if (!bdisp)
+		return -ENOMEM;
+
+	bdisp->pdev = pdev;
+	bdisp->dev = dev;
+	platform_set_drvdata(pdev, bdisp);
+
+	if (dev->of_node)
+		bdisp->id = of_alias_get_id(pdev->dev.of_node, BDISP_NAME);
+	else
+		bdisp->id = pdev->id;
+
+	init_waitqueue_head(&bdisp->irq_queue);
+	INIT_DELAYED_WORK(&bdisp->timeout_work, bdisp_irq_timeout);
+	bdisp->work_queue = create_workqueue(BDISP_NAME);
+
+	spin_lock_init(&bdisp->slock);
+	mutex_init(&bdisp->lock);
+
+	/* get resources */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	bdisp->regs = devm_ioremap_resource(dev, res);
+	if (IS_ERR(bdisp->regs)) {
+		dev_err(dev, "failed to get regs\n");
+		return PTR_ERR(bdisp->regs);
+	}
+
+	bdisp->clock = devm_clk_get(dev, BDISP_NAME);
+	if (IS_ERR(bdisp->clock)) {
+		dev_err(dev, "failed to get clock\n");
+		return PTR_ERR(bdisp->clock);
+	}
+
+	ret = clk_prepare(bdisp->clock);
+	if (ret < 0) {
+		dev_err(dev, "clock prepare failed\n");
+		bdisp->clock = ERR_PTR(-EINVAL);
+		return ret;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res) {
+		dev_err(dev, "failed to get IRQ resource\n");
+		goto err_clk;
+	}
+
+	ret = devm_request_threaded_irq(dev, res->start, bdisp_irq_handler,
+					bdisp_irq_thread, IRQF_ONESHOT,
+					pdev->name, bdisp);
+	if (ret) {
+		dev_err(dev, "failed to install irq\n");
+		goto err_clk;
+	}
+
+	/* v4l2 register */
+	ret = v4l2_device_register(dev, &bdisp->v4l2_dev);
+	if (ret) {
+		dev_err(dev, "failed to register\n");
+		goto err_clk;
+	}
+
+	/* Debug */
+	ret = bdisp_debugfs_create(bdisp);
+	if (ret) {
+		dev_err(dev, "failed to create debugfs\n");
+		goto err_v4l2;
+	}
+
+	/* Power management */
+	pm_runtime_enable(dev);
+	ret = pm_runtime_get_sync(dev);
+	if (ret < 0) {
+		dev_err(dev, "failed to set PM\n");
+		goto err_dbg;
+	}
+
+	/* Continuous memory allocator */
+	bdisp->alloc_ctx = vb2_dma_contig_init_ctx(dev);
+	if (IS_ERR(bdisp->alloc_ctx)) {
+		ret = PTR_ERR(bdisp->alloc_ctx);
+		goto err_pm;
+	}
+
+	/* Filters */
+	if (bdisp_hw_alloc_filters(bdisp->dev)) {
+		dev_err(bdisp->dev, "no memory for filters\n");
+		ret = -ENOMEM;
+		goto err_vb2_dma;
+	}
+
+	/* Register */
+	ret = bdisp_register_device(bdisp);
+	if (ret) {
+		dev_err(dev, "failed to register\n");
+		goto err_filter;
+	}
+
+	dev_info(dev, "%s%d registered as /dev/video%d\n", BDISP_NAME,
+		 bdisp->id, bdisp->vdev.num);
+
+	pm_runtime_put(dev);
+
+	return 0;
+
+err_filter:
+	bdisp_hw_free_filters(bdisp->dev);
+err_vb2_dma:
+	vb2_dma_contig_cleanup_ctx(bdisp->alloc_ctx);
+err_pm:
+	pm_runtime_put(dev);
+err_dbg:
+	bdisp_debugfs_remove(bdisp);
+err_v4l2:
+	v4l2_device_unregister(&bdisp->v4l2_dev);
+err_clk:
+	if (!IS_ERR(bdisp->clock))
+		clk_unprepare(bdisp->clock);
+
+	return ret;
+}
+
+static const struct of_device_id bdisp_match_types[] = {
+	{
+		.compatible = "st,stih407-bdisp",
+	},
+	{ /* end node */ }
+};
+
+MODULE_DEVICE_TABLE(of, bdisp_match_types);
+
+static struct platform_driver bdisp_driver = {
+	.probe          = bdisp_probe,
+	.remove         = bdisp_remove,
+	.driver         = {
+		.name           = BDISP_NAME,
+		.of_match_table = bdisp_match_types,
+		.pm             = &bdisp_pm_ops,
+	},
+};
+
+module_platform_driver(bdisp_driver);
+
+MODULE_DESCRIPTION("2D blitter for STMicroelectronics SoC");
+MODULE_AUTHOR("Fabien Dessenne <fabien.dessenne@st.com>");
+MODULE_LICENSE("GPL");
diff --git a/kernel/drivers/media/platform/sti/bdisp/bdisp.h b/kernel/drivers/media/platform/sti/bdisp/bdisp.h
new file mode 100644
index 000000000..0cf985772
--- /dev/null
+++ b/kernel/drivers/media/platform/sti/bdisp/bdisp.h
@@ -0,0 +1,216 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2014
+ * Authors: Fabien Dessenne <fabien.dessenne@st.com> for STMicroelectronics.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#include <linux/clk.h>
+#include <linux/ktime.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-mem2mem.h>
+
+#include <media/videobuf2-dma-contig.h>
+
+#define BDISP_NAME              "bdisp"
+
+/*
+ *  Max nb of nodes in node-list:
+ *   - 2 nodes to handle wide 4K pictures
+ *   - 2 nodes to handle two planes (Y & CbCr) */
+#define MAX_OUTPUT_PLANES       2
+#define MAX_VERTICAL_STRIDES    2
+#define MAX_NB_NODE             (MAX_OUTPUT_PLANES * MAX_VERTICAL_STRIDES)
+
+/* struct bdisp_ctrls - bdisp control set
+ * @hflip:      horizontal flip
+ * @vflip:      vertical flip
+ */
+struct bdisp_ctrls {
+	struct v4l2_ctrl        *hflip;
+	struct v4l2_ctrl        *vflip;
+};
+
+/**
+ * struct bdisp_fmt - driver's internal color format data
+ * @pixelformat:fourcc code for this format
+ * @nb_planes:  number of planes  (ex: [0]=RGB/Y - [1]=Cb/Cr, ...)
+ * @bpp:        bits per pixel (general)
+ * @bpp_plane0: byte per pixel for the 1st plane
+ * @w_align:    width alignment in pixel (multiple of)
+ * @h_align:    height alignment in pixel (multiple of)
+ */
+struct bdisp_fmt {
+	u32                     pixelformat;
+	u8                      nb_planes;
+	u8                      bpp;
+	u8                      bpp_plane0;
+	u8                      w_align;
+	u8                      h_align;
+};
+
+/**
+ * struct bdisp_frame - frame properties
+ *
+ * @width:      frame width (including padding)
+ * @height:     frame height (including padding)
+ * @fmt:        pointer to frame format descriptor
+ * @field:      frame / field type
+ * @bytesperline: stride of the 1st plane
+ * @sizeimage:  image size in bytes
+ * @colorspace: colorspace
+ * @crop:       crop area
+ * @paddr:      image physical addresses per plane ([0]=RGB/Y - [1]=Cb/Cr, ...)
+ */
+struct bdisp_frame {
+	u32                     width;
+	u32                     height;
+	const struct bdisp_fmt  *fmt;
+	enum v4l2_field         field;
+	u32                     bytesperline;
+	u32                     sizeimage;
+	enum v4l2_colorspace    colorspace;
+	struct v4l2_rect        crop;
+	dma_addr_t              paddr[4];
+};
+
+/**
+ * struct bdisp_request - bdisp request
+ *
+ * @src:        source frame properties
+ * @dst:        destination frame properties
+ * @hflip:      horizontal flip
+ * @vflip:      vertical flip
+ * @nb_req:     number of run request
+ */
+struct bdisp_request {
+	struct bdisp_frame      src;
+	struct bdisp_frame      dst;
+	unsigned int            hflip:1;
+	unsigned int            vflip:1;
+	int                     nb_req;
+};
+
+/**
+ * struct bdisp_ctx - device context data
+ *
+ * @src:        source frame properties
+ * @dst:        destination frame properties
+ * @state:      flags to keep track of user configuration
+ * @hflip:      horizontal flip
+ * @vflip:      vertical flip
+ * @bdisp_dev:  the device this context applies to
+ * @node:       node array
+ * @node_paddr: node physical address array
+ * @fh:         v4l2 file handle
+ * @ctrl_handler: v4l2 controls handler
+ * @bdisp_ctrls: bdisp control set
+ * @ctrls_rdy:  true if the control handler is initialized
+ */
+struct bdisp_ctx {
+	struct bdisp_frame      src;
+	struct bdisp_frame      dst;
+	u32                     state;
+	unsigned int            hflip:1;
+	unsigned int            vflip:1;
+	struct bdisp_dev        *bdisp_dev;
+	struct bdisp_node       *node[MAX_NB_NODE];
+	dma_addr_t              node_paddr[MAX_NB_NODE];
+	struct v4l2_fh          fh;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct bdisp_ctrls      bdisp_ctrls;
+	bool                    ctrls_rdy;
+};
+
+/**
+ * struct bdisp_m2m_device - v4l2 memory-to-memory device data
+ *
+ * @vdev:       video device node for v4l2 m2m mode
+ * @m2m_dev:    v4l2 m2m device data
+ * @ctx:        hardware context data
+ * @refcnt:     reference counter
+ */
+struct bdisp_m2m_device {
+	struct video_device     *vdev;
+	struct v4l2_m2m_dev     *m2m_dev;
+	struct bdisp_ctx        *ctx;
+	int                     refcnt;
+};
+
+/**
+ * struct bdisp_dbg - debug info
+ *
+ * @debugfs_entry: debugfs
+ * @copy_node:     array of last used nodes
+ * @copy_request:  last bdisp request
+ * @hw_start:      start time of last HW request
+ * @last_duration: last HW processing duration in microsecs
+ * @min_duration:  min HW processing duration in microsecs
+ * @max_duration:  max HW processing duration in microsecs
+ * @tot_duration:  total HW processing duration in microsecs
+ */
+struct bdisp_dbg {
+	struct dentry           *debugfs_entry;
+	struct bdisp_node       *copy_node[MAX_NB_NODE];
+	struct bdisp_request    copy_request;
+	ktime_t                 hw_start;
+	s64                     last_duration;
+	s64                     min_duration;
+	s64                     max_duration;
+	s64                     tot_duration;
+};
+
+/**
+ * struct bdisp_dev - abstraction for bdisp entity
+ *
+ * @v4l2_dev:   v4l2 device
+ * @vdev:       video device
+ * @pdev:       platform device
+ * @dev:        device
+ * @lock:       mutex protecting this data structure
+ * @slock:      spinlock protecting this data structure
+ * @id:         device index
+ * @m2m:        memory-to-memory V4L2 device information
+ * @state:      flags used to synchronize m2m and capture mode operation
+ * @alloc_ctx:  videobuf2 memory allocator context
+ * @clock:      IP clock
+ * @regs:       registers
+ * @irq_queue:  interrupt handler waitqueue
+ * @work_queue: workqueue to handle timeouts
+ * @timeout_work: IRQ timeout structure
+ * @dbg:        debug info
+ */
+struct bdisp_dev {
+	struct v4l2_device      v4l2_dev;
+	struct video_device     vdev;
+	struct platform_device  *pdev;
+	struct device           *dev;
+	spinlock_t              slock;
+	struct mutex            lock;
+	u16                     id;
+	struct bdisp_m2m_device m2m;
+	unsigned long           state;
+	struct vb2_alloc_ctx    *alloc_ctx;
+	struct clk              *clock;
+	void __iomem            *regs;
+	wait_queue_head_t       irq_queue;
+	struct workqueue_struct *work_queue;
+	struct delayed_work     timeout_work;
+	struct bdisp_dbg        dbg;
+};
+
+void bdisp_hw_free_nodes(struct bdisp_ctx *ctx);
+int bdisp_hw_alloc_nodes(struct bdisp_ctx *ctx);
+void bdisp_hw_free_filters(struct device *dev);
+int bdisp_hw_alloc_filters(struct device *dev);
+int bdisp_hw_reset(struct bdisp_dev *bdisp);
+int bdisp_hw_get_and_clear_irq(struct bdisp_dev *bdisp);
+int bdisp_hw_update(struct bdisp_ctx *ctx);
+
+void bdisp_debugfs_remove(struct bdisp_dev *bdisp);
+int bdisp_debugfs_create(struct bdisp_dev *bdisp);
+void bdisp_dbg_perf_begin(struct bdisp_dev *bdisp);
+void bdisp_dbg_perf_end(struct bdisp_dev *bdisp);
diff --git a/kernel/drivers/media/platform/sti/c8sectpfe/Kconfig b/kernel/drivers/media/platform/sti/c8sectpfe/Kconfig
new file mode 100644
index 000000000..7420a5057
--- /dev/null
+++ b/kernel/drivers/media/platform/sti/c8sectpfe/Kconfig
@@ -0,0 +1,27 @@
+config DVB_C8SECTPFE
+	tristate "STMicroelectronics C8SECTPFE DVB support"
+	depends on PINCTRL && DVB_CORE && I2C
+	depends on ARCH_STI || ARCH_MULTIPLATFORM || COMPILE_TEST
+	select FW_LOADER
+	select DEBUG_FS
+	select DVB_LNBP21 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_STV090x if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_STB6100 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_STV6110 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_STV0900 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_STV0367 if MEDIA_SUBDRV_AUTOSELECT
+	select MEDIA_TUNER_TDA18212 if MEDIA_SUBDRV_AUTOSELECT
+
+	---help---
+	  This adds support for DVB front-end cards connected
+	  to TS inputs of STiH407/410 SoC.
+
+	  The driver currently supports C8SECTPFE's TS input block,
+	  memdma engine, and HW PID filtering.
+
+	  Supported DVB front-end cards are:
+	  - STMicroelectronics DVB-T B2100A (STV0367 + TDA18212)
+	  - STMicroelectronics DVB-S/S2 STV0903 + STV6110 + LNBP24 board
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called c8sectpfe.
diff --git a/kernel/drivers/media/platform/sti/c8sectpfe/Makefile b/kernel/drivers/media/platform/sti/c8sectpfe/Makefile
new file mode 100644
index 000000000..b578c7cb4
--- /dev/null
+++ b/kernel/drivers/media/platform/sti/c8sectpfe/Makefile
@@ -0,0 +1,9 @@
+c8sectpfe-y += c8sectpfe-core.o c8sectpfe-common.o c8sectpfe-dvb.o \
+		c8sectpfe-debugfs.o
+
+obj-$(CONFIG_DVB_C8SECTPFE) += c8sectpfe.o
+
+ccflags-y += -Idrivers/media/i2c
+ccflags-y += -Idrivers/media/common
+ccflags-y += -Idrivers/media/dvb-core/ -Idrivers/media/dvb-frontends/ \
+		-Idrivers/media/tuners/
diff --git a/kernel/drivers/media/platform/sti/c8sectpfe/c8sectpfe-common.c b/kernel/drivers/media/platform/sti/c8sectpfe/c8sectpfe-common.c
new file mode 100644
index 000000000..95223ab71
--- /dev/null
+++ b/kernel/drivers/media/platform/sti/c8sectpfe/c8sectpfe-common.c
@@ -0,0 +1,265 @@
+/*
+ * c8sectpfe-common.c - C8SECTPFE STi DVB driver
+ *
+ * Copyright (c) STMicroelectronics 2015
+ *
+ *   Author: Peter Griffin <peter.griffin@linaro.org>
+ *
+ *      This program is free software; you can redistribute it and/or
+ *      modify it under the terms of the GNU General Public License as
+ *      published by the Free Software Foundation; either version 2 of
+ *      the License, or (at your option) any later version.
+ */
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dvb/dmx.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/time.h>
+#include <linux/wait.h>
+
+#include "dmxdev.h"
+#include "dvbdev.h"
+#include "dvb_demux.h"
+#include "dvb_frontend.h"
+#include "dvb_net.h"
+
+#include "c8sectpfe-common.h"
+#include "c8sectpfe-core.h"
+#include "c8sectpfe-dvb.h"
+
+static int register_dvb(struct stdemux *demux, struct dvb_adapter *adap,
+				void *start_feed, void *stop_feed,
+				struct c8sectpfei *fei)
+{
+	int result;
+
+	demux->dvb_demux.dmx.capabilities = DMX_TS_FILTERING |
+					DMX_SECTION_FILTERING |
+					DMX_MEMORY_BASED_FILTERING;
+
+	demux->dvb_demux.priv = demux;
+	demux->dvb_demux.filternum = C8SECTPFE_MAXCHANNEL;
+	demux->dvb_demux.feednum = C8SECTPFE_MAXCHANNEL;
+
+	demux->dvb_demux.start_feed = start_feed;
+	demux->dvb_demux.stop_feed = stop_feed;
+	demux->dvb_demux.write_to_decoder = NULL;
+
+	result = dvb_dmx_init(&demux->dvb_demux);
+	if (result < 0) {
+		dev_err(fei->dev, "dvb_dmx_init failed (errno = %d)\n",
+			result);
+		goto err_dmx;
+	}
+
+	demux->dmxdev.filternum = demux->dvb_demux.filternum;
+	demux->dmxdev.demux = &demux->dvb_demux.dmx;
+	demux->dmxdev.capabilities = 0;
+
+	result = dvb_dmxdev_init(&demux->dmxdev, adap);
+	if (result < 0) {
+		dev_err(fei->dev, "dvb_dmxdev_init failed (errno = %d)\n",
+			result);
+
+		goto err_dmxdev;
+	}
+
+	demux->hw_frontend.source = DMX_FRONTEND_0 + demux->tsin_index;
+
+	result = demux->dvb_demux.dmx.add_frontend(&demux->dvb_demux.dmx,
+						&demux->hw_frontend);
+	if (result < 0) {
+		dev_err(fei->dev, "add_frontend failed (errno = %d)\n", result);
+		goto err_fe_hw;
+	}
+
+	demux->mem_frontend.source = DMX_MEMORY_FE;
+	result = demux->dvb_demux.dmx.add_frontend(&demux->dvb_demux.dmx,
+						&demux->mem_frontend);
+	if (result < 0) {
+		dev_err(fei->dev, "add_frontend failed (%d)\n", result);
+		goto err_fe_mem;
+	}
+
+	result = demux->dvb_demux.dmx.connect_frontend(&demux->dvb_demux.dmx,
+							&demux->hw_frontend);
+	if (result < 0) {
+		dev_err(fei->dev, "connect_frontend (%d)\n", result);
+		goto err_fe_con;
+	}
+
+	return 0;
+
+err_fe_con:
+	demux->dvb_demux.dmx.remove_frontend(&demux->dvb_demux.dmx,
+						     &demux->mem_frontend);
+err_fe_mem:
+	demux->dvb_demux.dmx.remove_frontend(&demux->dvb_demux.dmx,
+						     &demux->hw_frontend);
+err_fe_hw:
+	dvb_dmxdev_release(&demux->dmxdev);
+err_dmxdev:
+	dvb_dmx_release(&demux->dvb_demux);
+err_dmx:
+	return result;
+
+}
+
+static void unregister_dvb(struct stdemux *demux)
+{
+
+	demux->dvb_demux.dmx.remove_frontend(&demux->dvb_demux.dmx,
+						     &demux->mem_frontend);
+
+	demux->dvb_demux.dmx.remove_frontend(&demux->dvb_demux.dmx,
+						     &demux->hw_frontend);
+
+	dvb_dmxdev_release(&demux->dmxdev);
+
+	dvb_dmx_release(&demux->dvb_demux);
+}
+
+static struct c8sectpfe *c8sectpfe_create(struct c8sectpfei *fei,
+				void *start_feed,
+				void *stop_feed)
+{
+	struct c8sectpfe *c8sectpfe;
+	int result;
+	int i, j;
+
+	short int ids[] = { -1 };
+
+	c8sectpfe = kzalloc(sizeof(struct c8sectpfe), GFP_KERNEL);
+	if (!c8sectpfe)
+		goto err1;
+
+	mutex_init(&c8sectpfe->lock);
+
+	c8sectpfe->device = fei->dev;
+
+	result = dvb_register_adapter(&c8sectpfe->adapter, "STi c8sectpfe",
+					THIS_MODULE, fei->dev, ids);
+	if (result < 0) {
+		dev_err(fei->dev, "dvb_register_adapter failed (errno = %d)\n",
+			result);
+		goto err2;
+	}
+
+	c8sectpfe->adapter.priv = fei;
+
+	for (i = 0; i < fei->tsin_count; i++) {
+
+		c8sectpfe->demux[i].tsin_index = i;
+		c8sectpfe->demux[i].c8sectpfei = fei;
+
+		result = register_dvb(&c8sectpfe->demux[i], &c8sectpfe->adapter,
+				start_feed, stop_feed, fei);
+		if (result < 0) {
+			dev_err(fei->dev,
+				"register_dvb feed=%d failed (errno = %d)\n",
+				result, i);
+
+			/* we take a all or nothing approach */
+			for (j = 0; j < i; j++)
+				unregister_dvb(&c8sectpfe->demux[j]);
+			goto err3;
+		}
+	}
+
+	c8sectpfe->num_feeds = fei->tsin_count;
+
+	return c8sectpfe;
+err3:
+	dvb_unregister_adapter(&c8sectpfe->adapter);
+err2:
+	kfree(c8sectpfe);
+err1:
+	return NULL;
+};
+
+static void c8sectpfe_delete(struct c8sectpfe *c8sectpfe)
+{
+	int i;
+
+	if (!c8sectpfe)
+		return;
+
+	for (i = 0; i < c8sectpfe->num_feeds; i++)
+		unregister_dvb(&c8sectpfe->demux[i]);
+
+	dvb_unregister_adapter(&c8sectpfe->adapter);
+
+	kfree(c8sectpfe);
+};
+
+void c8sectpfe_tuner_unregister_frontend(struct c8sectpfe *c8sectpfe,
+					struct c8sectpfei *fei)
+{
+	int n;
+	struct channel_info *tsin;
+
+	for (n = 0; n < fei->tsin_count; n++) {
+
+		tsin = fei->channel_data[n];
+
+		if (tsin && tsin->frontend) {
+			dvb_unregister_frontend(tsin->frontend);
+			dvb_frontend_detach(tsin->frontend);
+		}
+
+		if (tsin && tsin->i2c_adapter)
+			i2c_put_adapter(tsin->i2c_adapter);
+
+		if (tsin && tsin->i2c_client) {
+			if (tsin->i2c_client->dev.driver->owner)
+				module_put(tsin->i2c_client->dev.driver->owner);
+			i2c_unregister_device(tsin->i2c_client);
+		}
+	}
+
+	c8sectpfe_delete(c8sectpfe);
+};
+
+int c8sectpfe_tuner_register_frontend(struct c8sectpfe **c8sectpfe,
+				struct c8sectpfei *fei,
+				void *start_feed,
+				void *stop_feed)
+{
+	struct channel_info *tsin;
+	struct dvb_frontend *frontend;
+	int n, res;
+
+	*c8sectpfe = c8sectpfe_create(fei, start_feed, stop_feed);
+	if (!*c8sectpfe)
+		return -ENOMEM;
+
+	for (n = 0; n < fei->tsin_count; n++) {
+		tsin = fei->channel_data[n];
+
+		res = c8sectpfe_frontend_attach(&frontend, *c8sectpfe, tsin, n);
+		if (res)
+			goto err;
+
+		res = dvb_register_frontend(&c8sectpfe[0]->adapter, frontend);
+		if (res < 0) {
+			dev_err(fei->dev, "dvb_register_frontend failed (%d)\n",
+				res);
+			goto err;
+		}
+
+		tsin->frontend = frontend;
+	}
+
+	return 0;
+
+err:
+	c8sectpfe_tuner_unregister_frontend(*c8sectpfe, fei);
+	return res;
+}
diff --git a/kernel/drivers/media/platform/sti/c8sectpfe/c8sectpfe-common.h b/kernel/drivers/media/platform/sti/c8sectpfe/c8sectpfe-common.h
new file mode 100644
index 000000000..da21c0ac0
--- /dev/null
+++ b/kernel/drivers/media/platform/sti/c8sectpfe/c8sectpfe-common.h
@@ -0,0 +1,64 @@
+/*
+ * c8sectpfe-common.h - C8SECTPFE STi DVB driver
+ *
+ * Copyright (c) STMicroelectronics 2015
+ *
+ *   Author: Peter Griffin <peter.griffin@linaro.org>
+ *
+ *      This program is free software; you can redistribute it and/or
+ *      modify it under the terms of the GNU General Public License as
+ *      published by the Free Software Foundation; either version 2 of
+ *      the License, or (at your option) any later version.
+ */
+#ifndef _C8SECTPFE_COMMON_H_
+#define _C8SECTPFE_COMMON_H_
+
+#include <linux/dvb/dmx.h>
+#include <linux/dvb/frontend.h>
+#include <linux/gpio.h>
+#include <linux/version.h>
+
+#include "dmxdev.h"
+#include "dvb_demux.h"
+#include "dvb_frontend.h"
+#include "dvb_net.h"
+
+/* Maximum number of channels */
+#define C8SECTPFE_MAXADAPTER (4)
+#define C8SECTPFE_MAXCHANNEL 64
+#define STPTI_MAXCHANNEL 64
+
+#define MAX_INPUTBLOCKS 7
+
+struct c8sectpfe;
+struct stdemux;
+
+struct stdemux {
+	struct dvb_demux	dvb_demux;
+	struct dmxdev		dmxdev;
+	struct dmx_frontend	hw_frontend;
+	struct dmx_frontend	mem_frontend;
+	int			tsin_index;
+	int			running_feed_count;
+	struct			c8sectpfei *c8sectpfei;
+};
+
+struct c8sectpfe {
+	struct stdemux demux[MAX_INPUTBLOCKS];
+	struct mutex lock;
+	struct dvb_adapter adapter;
+	struct device *device;
+	int mapping;
+	int num_feeds;
+};
+
+/* Channel registration */
+int c8sectpfe_tuner_register_frontend(struct c8sectpfe **c8sectpfe,
+					struct c8sectpfei *fei,
+					void *start_feed,
+					void *stop_feed);
+
+void c8sectpfe_tuner_unregister_frontend(struct c8sectpfe *c8sectpfe,
+						struct c8sectpfei *fei);
+
+#endif
diff --git a/kernel/drivers/media/platform/sti/c8sectpfe/c8sectpfe-core.c b/kernel/drivers/media/platform/sti/c8sectpfe/c8sectpfe-core.c
new file mode 100644
index 000000000..8490a65ae
--- /dev/null
+++ b/kernel/drivers/media/platform/sti/c8sectpfe/c8sectpfe-core.c
@@ -0,0 +1,1235 @@
+/*
+ * c8sectpfe-core.c - C8SECTPFE STi DVB driver
+ *
+ * Copyright (c) STMicroelectronics 2015
+ *
+ *   Author:Peter Bennett <peter.bennett@st.com>
+ *	    Peter Griffin <peter.griffin@linaro.org>
+ *
+ *	This program is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU General Public License as
+ *	published by the Free Software Foundation; either version 2 of
+ *	the License, or (at your option) any later version.
+ */
+#include <linux/atomic.h>
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/dvb/dmx.h>
+#include <linux/dvb/frontend.h>
+#include <linux/errno.h>
+#include <linux/firmware.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of_gpio.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/usb.h>
+#include <linux/slab.h>
+#include <linux/time.h>
+#include <linux/version.h>
+#include <linux/wait.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "c8sectpfe-core.h"
+#include "c8sectpfe-common.h"
+#include "c8sectpfe-debugfs.h"
+#include "dmxdev.h"
+#include "dvb_demux.h"
+#include "dvb_frontend.h"
+#include "dvb_net.h"
+
+#define FIRMWARE_MEMDMA "pti_memdma_h407.elf"
+MODULE_FIRMWARE(FIRMWARE_MEMDMA);
+
+#define PID_TABLE_SIZE 1024
+#define POLL_MSECS 50
+
+static int load_c8sectpfe_fw_step1(struct c8sectpfei *fei);
+
+#define TS_PKT_SIZE 188
+#define HEADER_SIZE (4)
+#define PACKET_SIZE (TS_PKT_SIZE+HEADER_SIZE)
+
+#define FEI_ALIGNMENT (32)
+/* hw requires minimum of 8*PACKET_SIZE and padded to 8byte boundary */
+#define FEI_BUFFER_SIZE (8*PACKET_SIZE*340)
+
+#define FIFO_LEN 1024
+
+static void c8sectpfe_timer_interrupt(unsigned long ac8sectpfei)
+{
+	struct c8sectpfei *fei = (struct c8sectpfei *)ac8sectpfei;
+	struct channel_info *channel;
+	int chan_num;
+
+	/* iterate through input block channels */
+	for (chan_num = 0; chan_num < fei->tsin_count; chan_num++) {
+		channel = fei->channel_data[chan_num];
+
+		/* is this descriptor initialised and TP enabled */
+		if (channel->irec && readl(channel->irec + DMA_PRDS_TPENABLE))
+			tasklet_schedule(&channel->tsklet);
+	}
+
+	fei->timer.expires = jiffies +	msecs_to_jiffies(POLL_MSECS);
+	add_timer(&fei->timer);
+}
+
+static void channel_swdemux_tsklet(unsigned long data)
+{
+	struct channel_info *channel = (struct channel_info *)data;
+	struct c8sectpfei *fei = channel->fei;
+	unsigned long wp, rp;
+	int pos, num_packets, n, size;
+	u8 *buf;
+
+	if (unlikely(!channel || !channel->irec))
+		return;
+
+	wp = readl(channel->irec + DMA_PRDS_BUSWP_TP(0));
+	rp = readl(channel->irec + DMA_PRDS_BUSRP_TP(0));
+
+	pos = rp - channel->back_buffer_busaddr;
+
+	/* has it wrapped */
+	if (wp < rp)
+		wp = channel->back_buffer_busaddr + FEI_BUFFER_SIZE;
+
+	size = wp - rp;
+	num_packets = size / PACKET_SIZE;
+
+	/* manage cache so data is visible to CPU */
+	dma_sync_single_for_cpu(fei->dev,
+				rp,
+				size,
+				DMA_FROM_DEVICE);
+
+	buf = (u8 *) channel->back_buffer_aligned;
+
+	dev_dbg(fei->dev,
+		"chan=%d channel=%p num_packets = %d, buf = %p, pos = 0x%x\n\t"
+		"rp=0x%lx, wp=0x%lx\n",
+		channel->tsin_id, channel, num_packets, buf, pos, rp, wp);
+
+	for (n = 0; n < num_packets; n++) {
+		dvb_dmx_swfilter_packets(
+			&fei->c8sectpfe[0]->
+				demux[channel->demux_mapping].dvb_demux,
+			&buf[pos], 1);
+
+		pos += PACKET_SIZE;
+	}
+
+	/* advance the read pointer */
+	if (wp == (channel->back_buffer_busaddr + FEI_BUFFER_SIZE))
+		writel(channel->back_buffer_busaddr, channel->irec +
+			DMA_PRDS_BUSRP_TP(0));
+	else
+		writel(wp, channel->irec + DMA_PRDS_BUSWP_TP(0));
+}
+
+static int c8sectpfe_start_feed(struct dvb_demux_feed *dvbdmxfeed)
+{
+	struct dvb_demux *demux = dvbdmxfeed->demux;
+	struct stdemux *stdemux = (struct stdemux *)demux->priv;
+	struct c8sectpfei *fei = stdemux->c8sectpfei;
+	struct channel_info *channel;
+	u32 tmp;
+	unsigned long *bitmap;
+
+	switch (dvbdmxfeed->type) {
+	case DMX_TYPE_TS:
+		break;
+	case DMX_TYPE_SEC:
+		break;
+	default:
+		dev_err(fei->dev, "%s:%d Error bailing\n"
+			, __func__, __LINE__);
+		return -EINVAL;
+	}
+
+	if (dvbdmxfeed->type == DMX_TYPE_TS) {
+		switch (dvbdmxfeed->pes_type) {
+		case DMX_PES_VIDEO:
+		case DMX_PES_AUDIO:
+		case DMX_PES_TELETEXT:
+		case DMX_PES_PCR:
+		case DMX_PES_OTHER:
+			break;
+		default:
+			dev_err(fei->dev, "%s:%d Error bailing\n"
+				, __func__, __LINE__);
+			return -EINVAL;
+		}
+	}
+
+	if (!atomic_read(&fei->fw_loaded)) {
+		dev_err(fei->dev, "%s: c8sectpfe fw not loaded\n", __func__);
+		return -EINVAL;
+	}
+
+	mutex_lock(&fei->lock);
+
+	channel = fei->channel_data[stdemux->tsin_index];
+
+	bitmap = (unsigned long *) channel->pid_buffer_aligned;
+
+	/* 8192 is a special PID */
+	if (dvbdmxfeed->pid == 8192) {
+		tmp = readl(fei->io + C8SECTPFE_IB_PID_SET(channel->tsin_id));
+		tmp &= ~C8SECTPFE_PID_ENABLE;
+		writel(tmp, fei->io + C8SECTPFE_IB_PID_SET(channel->tsin_id));
+
+	} else {
+		bitmap_set(bitmap, dvbdmxfeed->pid, 1);
+	}
+
+	/* manage cache so PID bitmap is visible to HW */
+	dma_sync_single_for_device(fei->dev,
+					channel->pid_buffer_busaddr,
+					PID_TABLE_SIZE,
+					DMA_TO_DEVICE);
+
+	channel->active = 1;
+
+	if (fei->global_feed_count == 0) {
+		fei->timer.expires = jiffies +
+			msecs_to_jiffies(msecs_to_jiffies(POLL_MSECS));
+
+		add_timer(&fei->timer);
+	}
+
+	if (stdemux->running_feed_count == 0) {
+
+		dev_dbg(fei->dev, "Starting channel=%p\n", channel);
+
+		tasklet_init(&channel->tsklet, channel_swdemux_tsklet,
+			     (unsigned long) channel);
+
+		/* Reset the internal inputblock sram pointers */
+		writel(channel->fifo,
+			fei->io + C8SECTPFE_IB_BUFF_STRT(channel->tsin_id));
+		writel(channel->fifo + FIFO_LEN - 1,
+			fei->io + C8SECTPFE_IB_BUFF_END(channel->tsin_id));
+
+		writel(channel->fifo,
+			fei->io + C8SECTPFE_IB_READ_PNT(channel->tsin_id));
+		writel(channel->fifo,
+			fei->io + C8SECTPFE_IB_WRT_PNT(channel->tsin_id));
+
+
+		/* reset read / write memdma ptrs for this channel */
+		writel(channel->back_buffer_busaddr, channel->irec +
+			DMA_PRDS_BUSBASE_TP(0));
+
+		tmp = channel->back_buffer_busaddr + FEI_BUFFER_SIZE - 1;
+		writel(tmp, channel->irec + DMA_PRDS_BUSTOP_TP(0));
+
+		writel(channel->back_buffer_busaddr, channel->irec +
+			DMA_PRDS_BUSWP_TP(0));
+
+		/* Issue a reset and enable InputBlock */
+		writel(C8SECTPFE_SYS_ENABLE | C8SECTPFE_SYS_RESET
+			, fei->io + C8SECTPFE_IB_SYS(channel->tsin_id));
+
+		/* and enable the tp */
+		writel(0x1, channel->irec + DMA_PRDS_TPENABLE);
+
+		dev_dbg(fei->dev, "%s:%d Starting DMA feed on stdemux=%p\n"
+			, __func__, __LINE__, stdemux);
+	}
+
+	stdemux->running_feed_count++;
+	fei->global_feed_count++;
+
+	mutex_unlock(&fei->lock);
+
+	return 0;
+}
+
+static int c8sectpfe_stop_feed(struct dvb_demux_feed *dvbdmxfeed)
+{
+
+	struct dvb_demux *demux = dvbdmxfeed->demux;
+	struct stdemux *stdemux = (struct stdemux *)demux->priv;
+	struct c8sectpfei *fei = stdemux->c8sectpfei;
+	struct channel_info *channel;
+	int idlereq;
+	u32 tmp;
+	int ret;
+	unsigned long *bitmap;
+
+	if (!atomic_read(&fei->fw_loaded)) {
+		dev_err(fei->dev, "%s: c8sectpfe fw not loaded\n", __func__);
+		return -EINVAL;
+	}
+
+	mutex_lock(&fei->lock);
+
+	channel = fei->channel_data[stdemux->tsin_index];
+
+	bitmap = (unsigned long *) channel->pid_buffer_aligned;
+
+	if (dvbdmxfeed->pid == 8192) {
+		tmp = readl(fei->io + C8SECTPFE_IB_PID_SET(channel->tsin_id));
+		tmp |= C8SECTPFE_PID_ENABLE;
+		writel(tmp, fei->io + C8SECTPFE_IB_PID_SET(channel->tsin_id));
+	} else {
+		bitmap_clear(bitmap, dvbdmxfeed->pid, 1);
+	}
+
+	/* manage cache so data is visible to HW */
+	dma_sync_single_for_device(fei->dev,
+					channel->pid_buffer_busaddr,
+					PID_TABLE_SIZE,
+					DMA_TO_DEVICE);
+
+	if (--stdemux->running_feed_count == 0) {
+
+		channel = fei->channel_data[stdemux->tsin_index];
+
+		/* TP re-configuration on page 168 of functional spec */
+
+		/* disable IB (prevents more TS data going to memdma) */
+		writel(0, fei->io + C8SECTPFE_IB_SYS(channel->tsin_id));
+
+		/* disable this channels descriptor */
+		writel(0,  channel->irec + DMA_PRDS_TPENABLE);
+
+		tasklet_disable(&channel->tsklet);
+
+		/* now request memdma channel goes idle */
+		idlereq = (1 << channel->tsin_id) | IDLEREQ;
+		writel(idlereq, fei->io + DMA_IDLE_REQ);
+
+		/* wait for idle irq handler to signal completion */
+		ret = wait_for_completion_timeout(&channel->idle_completion,
+						msecs_to_jiffies(100));
+
+		if (ret == 0)
+			dev_warn(fei->dev,
+				"Timeout waiting for idle irq on tsin%d\n",
+				channel->tsin_id);
+
+		reinit_completion(&channel->idle_completion);
+
+		/* reset read / write ptrs for this channel */
+
+		writel(channel->back_buffer_busaddr,
+			channel->irec + DMA_PRDS_BUSBASE_TP(0));
+
+		tmp = channel->back_buffer_busaddr + FEI_BUFFER_SIZE - 1;
+		writel(tmp, channel->irec + DMA_PRDS_BUSTOP_TP(0));
+
+		writel(channel->back_buffer_busaddr,
+			channel->irec + DMA_PRDS_BUSWP_TP(0));
+
+		dev_dbg(fei->dev,
+			"%s:%d stopping DMA feed on stdemux=%p channel=%d\n",
+			__func__, __LINE__, stdemux, channel->tsin_id);
+
+		/* turn off all PIDS in the bitmap */
+		memset((void *)channel->pid_buffer_aligned
+			, 0x00, PID_TABLE_SIZE);
+
+		/* manage cache so data is visible to HW */
+		dma_sync_single_for_device(fei->dev,
+					channel->pid_buffer_busaddr,
+					PID_TABLE_SIZE,
+					DMA_TO_DEVICE);
+
+		channel->active = 0;
+	}
+
+	if (--fei->global_feed_count == 0) {
+		dev_dbg(fei->dev, "%s:%d global_feed_count=%d\n"
+			, __func__, __LINE__, fei->global_feed_count);
+
+		del_timer(&fei->timer);
+	}
+
+	mutex_unlock(&fei->lock);
+
+	return 0;
+}
+
+static struct channel_info *find_channel(struct c8sectpfei *fei, int tsin_num)
+{
+	int i;
+
+	for (i = 0; i < C8SECTPFE_MAX_TSIN_CHAN; i++) {
+		if (!fei->channel_data[i])
+			continue;
+
+		if (fei->channel_data[i]->tsin_id == tsin_num)
+			return fei->channel_data[i];
+	}
+
+	return NULL;
+}
+
+static void c8sectpfe_getconfig(struct c8sectpfei *fei)
+{
+	struct c8sectpfe_hw *hw = &fei->hw_stats;
+
+	hw->num_ib = readl(fei->io + SYS_CFG_NUM_IB);
+	hw->num_mib = readl(fei->io + SYS_CFG_NUM_MIB);
+	hw->num_swts = readl(fei->io + SYS_CFG_NUM_SWTS);
+	hw->num_tsout = readl(fei->io + SYS_CFG_NUM_TSOUT);
+	hw->num_ccsc = readl(fei->io + SYS_CFG_NUM_CCSC);
+	hw->num_ram = readl(fei->io + SYS_CFG_NUM_RAM);
+	hw->num_tp = readl(fei->io + SYS_CFG_NUM_TP);
+
+	dev_info(fei->dev, "C8SECTPFE hw supports the following:\n");
+	dev_info(fei->dev, "Input Blocks: %d\n", hw->num_ib);
+	dev_info(fei->dev, "Merged Input Blocks: %d\n", hw->num_mib);
+	dev_info(fei->dev, "Software Transport Stream Inputs: %d\n"
+				, hw->num_swts);
+	dev_info(fei->dev, "Transport Stream Output: %d\n", hw->num_tsout);
+	dev_info(fei->dev, "Cable Card Converter: %d\n", hw->num_ccsc);
+	dev_info(fei->dev, "RAMs supported by C8SECTPFE: %d\n", hw->num_ram);
+	dev_info(fei->dev, "Tango TPs supported by C8SECTPFE: %d\n"
+			, hw->num_tp);
+}
+
+static irqreturn_t c8sectpfe_idle_irq_handler(int irq, void *priv)
+{
+	struct c8sectpfei *fei = priv;
+	struct channel_info *chan;
+	int bit;
+	unsigned long tmp = readl(fei->io + DMA_IDLE_REQ);
+
+	/* page 168 of functional spec: Clear the idle request
+	   by writing 0 to the C8SECTPFE_DMA_IDLE_REQ register. */
+
+	/* signal idle completion */
+	for_each_set_bit(bit, &tmp, fei->hw_stats.num_ib) {
+
+		chan = find_channel(fei, bit);
+
+		if (chan)
+			complete(&chan->idle_completion);
+	}
+
+	writel(0, fei->io + DMA_IDLE_REQ);
+
+	return IRQ_HANDLED;
+}
+
+
+static void free_input_block(struct c8sectpfei *fei, struct channel_info *tsin)
+{
+	if (!fei || !tsin)
+		return;
+
+	if (tsin->back_buffer_busaddr)
+		if (!dma_mapping_error(fei->dev, tsin->back_buffer_busaddr))
+			dma_unmap_single(fei->dev, tsin->back_buffer_busaddr,
+				FEI_BUFFER_SIZE, DMA_BIDIRECTIONAL);
+
+	kfree(tsin->back_buffer_start);
+
+	if (tsin->pid_buffer_busaddr)
+		if (!dma_mapping_error(fei->dev, tsin->pid_buffer_busaddr))
+			dma_unmap_single(fei->dev, tsin->pid_buffer_busaddr,
+				PID_TABLE_SIZE, DMA_BIDIRECTIONAL);
+
+	kfree(tsin->pid_buffer_start);
+}
+
+#define MAX_NAME 20
+
+static int configure_memdma_and_inputblock(struct c8sectpfei *fei,
+				struct channel_info *tsin)
+{
+	int ret;
+	u32 tmp;
+	char tsin_pin_name[MAX_NAME];
+
+	if (!fei || !tsin)
+		return -EINVAL;
+
+	dev_dbg(fei->dev, "%s:%d Configuring channel=%p tsin=%d\n"
+		, __func__, __LINE__, tsin, tsin->tsin_id);
+
+	init_completion(&tsin->idle_completion);
+
+	tsin->back_buffer_start = kzalloc(FEI_BUFFER_SIZE +
+					FEI_ALIGNMENT, GFP_KERNEL);
+
+	if (!tsin->back_buffer_start) {
+		ret = -ENOMEM;
+		goto err_unmap;
+	}
+
+	/* Ensure backbuffer is 32byte aligned */
+	tsin->back_buffer_aligned = tsin->back_buffer_start
+		+ FEI_ALIGNMENT;
+
+	tsin->back_buffer_aligned = (void *)
+		(((uintptr_t) tsin->back_buffer_aligned) & ~0x1F);
+
+	tsin->back_buffer_busaddr = dma_map_single(fei->dev,
+					(void *)tsin->back_buffer_aligned,
+					FEI_BUFFER_SIZE,
+					DMA_BIDIRECTIONAL);
+
+	if (dma_mapping_error(fei->dev, tsin->back_buffer_busaddr)) {
+		dev_err(fei->dev, "failed to map back_buffer\n");
+		ret = -EFAULT;
+		goto err_unmap;
+	}
+
+	/*
+	 * The pid buffer can be configured (in hw) for byte or bit
+	 * per pid. By powers of deduction we conclude stih407 family
+	 * is configured (at SoC design stage) for bit per pid.
+	 */
+	tsin->pid_buffer_start = kzalloc(2048, GFP_KERNEL);
+
+	if (!tsin->pid_buffer_start) {
+		ret = -ENOMEM;
+		goto err_unmap;
+	}
+
+	/*
+	 * PID buffer needs to be aligned to size of the pid table
+	 * which at bit per pid is 1024 bytes (8192 pids / 8).
+	 * PIDF_BASE register enforces this alignment when writing
+	 * the register.
+	 */
+
+	tsin->pid_buffer_aligned = tsin->pid_buffer_start +
+		PID_TABLE_SIZE;
+
+	tsin->pid_buffer_aligned = (void *)
+		(((uintptr_t) tsin->pid_buffer_aligned) & ~0x3ff);
+
+	tsin->pid_buffer_busaddr = dma_map_single(fei->dev,
+						tsin->pid_buffer_aligned,
+						PID_TABLE_SIZE,
+						DMA_BIDIRECTIONAL);
+
+	if (dma_mapping_error(fei->dev, tsin->pid_buffer_busaddr)) {
+		dev_err(fei->dev, "failed to map pid_bitmap\n");
+		ret = -EFAULT;
+		goto err_unmap;
+	}
+
+	/* manage cache so pid bitmap is visible to HW */
+	dma_sync_single_for_device(fei->dev,
+				tsin->pid_buffer_busaddr,
+				PID_TABLE_SIZE,
+				DMA_TO_DEVICE);
+
+	snprintf(tsin_pin_name, MAX_NAME, "tsin%d-%s", tsin->tsin_id,
+		(tsin->serial_not_parallel ? "serial" : "parallel"));
+
+	tsin->pstate = pinctrl_lookup_state(fei->pinctrl, tsin_pin_name);
+	if (IS_ERR(tsin->pstate)) {
+		dev_err(fei->dev, "%s: pinctrl_lookup_state couldn't find %s state\n"
+			, __func__, tsin_pin_name);
+		ret = PTR_ERR(tsin->pstate);
+		goto err_unmap;
+	}
+
+	ret = pinctrl_select_state(fei->pinctrl, tsin->pstate);
+
+	if (ret) {
+		dev_err(fei->dev, "%s: pinctrl_select_state failed\n"
+			, __func__);
+		goto err_unmap;
+	}
+
+	/* Enable this input block */
+	tmp = readl(fei->io + SYS_INPUT_CLKEN);
+	tmp |= BIT(tsin->tsin_id);
+	writel(tmp, fei->io + SYS_INPUT_CLKEN);
+
+	if (tsin->serial_not_parallel)
+		tmp |= C8SECTPFE_SERIAL_NOT_PARALLEL;
+
+	if (tsin->invert_ts_clk)
+		tmp |= C8SECTPFE_INVERT_TSCLK;
+
+	if (tsin->async_not_sync)
+		tmp |= C8SECTPFE_ASYNC_NOT_SYNC;
+
+	tmp |= C8SECTPFE_ALIGN_BYTE_SOP | C8SECTPFE_BYTE_ENDIANNESS_MSB;
+
+	writel(tmp, fei->io + C8SECTPFE_IB_IP_FMT_CFG(tsin->tsin_id));
+
+	writel(C8SECTPFE_SYNC(0x9) |
+		C8SECTPFE_DROP(0x9) |
+		C8SECTPFE_TOKEN(0x47),
+		fei->io + C8SECTPFE_IB_SYNCLCKDRP_CFG(tsin->tsin_id));
+
+	writel(TS_PKT_SIZE, fei->io + C8SECTPFE_IB_PKT_LEN(tsin->tsin_id));
+
+	/* Place the FIFO's at the end of the irec descriptors */
+
+	tsin->fifo = (tsin->tsin_id * FIFO_LEN);
+
+	writel(tsin->fifo, fei->io + C8SECTPFE_IB_BUFF_STRT(tsin->tsin_id));
+	writel(tsin->fifo + FIFO_LEN - 1,
+		fei->io + C8SECTPFE_IB_BUFF_END(tsin->tsin_id));
+
+	writel(tsin->fifo, fei->io + C8SECTPFE_IB_READ_PNT(tsin->tsin_id));
+	writel(tsin->fifo, fei->io + C8SECTPFE_IB_WRT_PNT(tsin->tsin_id));
+
+	writel(tsin->pid_buffer_busaddr,
+		fei->io + PIDF_BASE(tsin->tsin_id));
+
+	dev_info(fei->dev, "chan=%d PIDF_BASE=0x%x pid_bus_addr=%pad\n",
+		tsin->tsin_id, readl(fei->io + PIDF_BASE(tsin->tsin_id)),
+		&tsin->pid_buffer_busaddr);
+
+	/* Configure and enable HW PID filtering */
+
+	/*
+	 * The PID value is created by assembling the first 8 bytes of
+	 * the TS packet into a 64-bit word in big-endian format. A
+	 * slice of that 64-bit word is taken from
+	 * (PID_OFFSET+PID_NUM_BITS-1) to PID_OFFSET.
+	 */
+	tmp = (C8SECTPFE_PID_ENABLE | C8SECTPFE_PID_NUMBITS(13)
+		| C8SECTPFE_PID_OFFSET(40));
+
+	writel(tmp, fei->io + C8SECTPFE_IB_PID_SET(tsin->tsin_id));
+
+	dev_dbg(fei->dev, "chan=%d setting wp: %d, rp: %d, buf: %d-%d\n",
+		tsin->tsin_id,
+		readl(fei->io + C8SECTPFE_IB_WRT_PNT(tsin->tsin_id)),
+		readl(fei->io + C8SECTPFE_IB_READ_PNT(tsin->tsin_id)),
+		readl(fei->io + C8SECTPFE_IB_BUFF_STRT(tsin->tsin_id)),
+		readl(fei->io + C8SECTPFE_IB_BUFF_END(tsin->tsin_id)));
+
+	/* Get base addpress of pointer record block from DMEM */
+	tsin->irec = fei->io + DMA_MEMDMA_OFFSET + DMA_DMEM_OFFSET +
+			readl(fei->io + DMA_PTRREC_BASE);
+
+	/* fill out pointer record data structure */
+
+	/* advance pointer record block to our channel */
+	tsin->irec += (tsin->tsin_id * DMA_PRDS_SIZE);
+
+	writel(tsin->fifo, tsin->irec + DMA_PRDS_MEMBASE);
+
+	writel(tsin->fifo + FIFO_LEN - 1, tsin->irec + DMA_PRDS_MEMTOP);
+
+	writel((188 + 7)&~7, tsin->irec + DMA_PRDS_PKTSIZE);
+
+	writel(0x1, tsin->irec + DMA_PRDS_TPENABLE);
+
+	/* read/write pointers with physical bus address */
+
+	writel(tsin->back_buffer_busaddr, tsin->irec + DMA_PRDS_BUSBASE_TP(0));
+
+	tmp = tsin->back_buffer_busaddr + FEI_BUFFER_SIZE - 1;
+	writel(tmp, tsin->irec + DMA_PRDS_BUSTOP_TP(0));
+
+	writel(tsin->back_buffer_busaddr, tsin->irec + DMA_PRDS_BUSWP_TP(0));
+	writel(tsin->back_buffer_busaddr, tsin->irec + DMA_PRDS_BUSRP_TP(0));
+
+	/* initialize tasklet */
+	tasklet_init(&tsin->tsklet, channel_swdemux_tsklet,
+		(unsigned long) tsin);
+
+	return 0;
+
+err_unmap:
+	free_input_block(fei, tsin);
+	return ret;
+}
+
+static irqreturn_t c8sectpfe_error_irq_handler(int irq, void *priv)
+{
+	struct c8sectpfei *fei = priv;
+
+	dev_err(fei->dev, "%s: error handling not yet implemented\n"
+		, __func__);
+
+	/*
+	 * TODO FIXME we should detect some error conditions here
+	 * and ideally so something about them!
+	 */
+
+	return IRQ_HANDLED;
+}
+
+static int c8sectpfe_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *child, *np = dev->of_node;
+	struct c8sectpfei *fei;
+	struct resource *res;
+	int ret, index = 0;
+	struct channel_info *tsin;
+
+	/* Allocate the c8sectpfei structure */
+	fei = devm_kzalloc(dev, sizeof(struct c8sectpfei), GFP_KERNEL);
+	if (!fei)
+		return -ENOMEM;
+
+	fei->dev = dev;
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "c8sectpfe");
+	fei->io = devm_ioremap_resource(dev, res);
+	if (IS_ERR(fei->io))
+		return PTR_ERR(fei->io);
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+					"c8sectpfe-ram");
+	fei->sram = devm_ioremap_resource(dev, res);
+	if (IS_ERR(fei->sram))
+		return PTR_ERR(fei->sram);
+
+	fei->sram_size = res->end - res->start;
+
+	fei->idle_irq = platform_get_irq_byname(pdev, "c8sectpfe-idle-irq");
+	if (fei->idle_irq < 0) {
+		dev_err(dev, "Can't get c8sectpfe-idle-irq\n");
+		return fei->idle_irq;
+	}
+
+	fei->error_irq = platform_get_irq_byname(pdev, "c8sectpfe-error-irq");
+	if (fei->error_irq < 0) {
+		dev_err(dev, "Can't get c8sectpfe-error-irq\n");
+		return fei->error_irq;
+	}
+
+	platform_set_drvdata(pdev, fei);
+
+	fei->c8sectpfeclk = devm_clk_get(dev, "c8sectpfe");
+	if (IS_ERR(fei->c8sectpfeclk)) {
+		dev_err(dev, "c8sectpfe clk not found\n");
+		return PTR_ERR(fei->c8sectpfeclk);
+	}
+
+	ret = clk_prepare_enable(fei->c8sectpfeclk);
+	if (ret) {
+		dev_err(dev, "Failed to enable c8sectpfe clock\n");
+		return ret;
+	}
+
+	/* to save power disable all IP's (on by default) */
+	writel(0, fei->io + SYS_INPUT_CLKEN);
+
+	/* Enable memdma clock */
+	writel(MEMDMAENABLE, fei->io + SYS_OTHER_CLKEN);
+
+	/* clear internal sram */
+	memset_io(fei->sram, 0x0, fei->sram_size);
+
+	c8sectpfe_getconfig(fei);
+
+	ret = devm_request_irq(dev, fei->idle_irq, c8sectpfe_idle_irq_handler,
+			0, "c8sectpfe-idle-irq", fei);
+	if (ret) {
+		dev_err(dev, "Can't register c8sectpfe-idle-irq IRQ.\n");
+		goto err_clk_disable;
+	}
+
+	ret = devm_request_irq(dev, fei->error_irq,
+				c8sectpfe_error_irq_handler, 0,
+				"c8sectpfe-error-irq", fei);
+	if (ret) {
+		dev_err(dev, "Can't register c8sectpfe-error-irq IRQ.\n");
+		goto err_clk_disable;
+	}
+
+	fei->tsin_count = of_get_child_count(np);
+
+	if (fei->tsin_count > C8SECTPFE_MAX_TSIN_CHAN ||
+		fei->tsin_count > fei->hw_stats.num_ib) {
+
+		dev_err(dev, "More tsin declared than exist on SoC!\n");
+		ret = -EINVAL;
+		goto err_clk_disable;
+	}
+
+	fei->pinctrl = devm_pinctrl_get(dev);
+
+	if (IS_ERR(fei->pinctrl)) {
+		dev_err(dev, "Error getting tsin pins\n");
+		ret = PTR_ERR(fei->pinctrl);
+		goto err_clk_disable;
+	}
+
+	for_each_child_of_node(np, child) {
+		struct device_node *i2c_bus;
+
+		fei->channel_data[index] = devm_kzalloc(dev,
+						sizeof(struct channel_info),
+						GFP_KERNEL);
+
+		if (!fei->channel_data[index]) {
+			ret = -ENOMEM;
+			goto err_clk_disable;
+		}
+
+		tsin = fei->channel_data[index];
+
+		tsin->fei = fei;
+
+		ret = of_property_read_u32(child, "tsin-num", &tsin->tsin_id);
+		if (ret) {
+			dev_err(&pdev->dev, "No tsin_num found\n");
+			goto err_clk_disable;
+		}
+
+		/* sanity check value */
+		if (tsin->tsin_id > fei->hw_stats.num_ib) {
+			dev_err(&pdev->dev,
+				"tsin-num %d specified greater than number\n\t"
+				"of input block hw in SoC! (%d)",
+				tsin->tsin_id, fei->hw_stats.num_ib);
+			ret = -EINVAL;
+			goto err_clk_disable;
+		}
+
+		tsin->invert_ts_clk = of_property_read_bool(child,
+							"invert-ts-clk");
+
+		tsin->serial_not_parallel = of_property_read_bool(child,
+							"serial-not-parallel");
+
+		tsin->async_not_sync = of_property_read_bool(child,
+							"async-not-sync");
+
+		ret = of_property_read_u32(child, "dvb-card",
+					&tsin->dvb_card);
+		if (ret) {
+			dev_err(&pdev->dev, "No dvb-card found\n");
+			goto err_clk_disable;
+		}
+
+		i2c_bus = of_parse_phandle(child, "i2c-bus", 0);
+		if (!i2c_bus) {
+			dev_err(&pdev->dev, "No i2c-bus found\n");
+			goto err_clk_disable;
+		}
+		tsin->i2c_adapter =
+			of_find_i2c_adapter_by_node(i2c_bus);
+		if (!tsin->i2c_adapter) {
+			dev_err(&pdev->dev, "No i2c adapter found\n");
+			of_node_put(i2c_bus);
+			goto err_clk_disable;
+		}
+		of_node_put(i2c_bus);
+
+		tsin->rst_gpio = of_get_named_gpio(child, "rst-gpio", 0);
+
+		ret = gpio_is_valid(tsin->rst_gpio);
+		if (!ret) {
+			dev_err(dev,
+				"reset gpio for tsin%d not valid (gpio=%d)\n",
+				tsin->tsin_id, tsin->rst_gpio);
+			goto err_clk_disable;
+		}
+
+		ret = devm_gpio_request_one(dev, tsin->rst_gpio,
+					GPIOF_OUT_INIT_LOW, "NIM reset");
+		if (ret && ret != -EBUSY) {
+			dev_err(dev, "Can't request tsin%d reset gpio\n"
+				, fei->channel_data[index]->tsin_id);
+			goto err_clk_disable;
+		}
+
+		if (!ret) {
+			/* toggle reset lines */
+			gpio_direction_output(tsin->rst_gpio, 0);
+			usleep_range(3500, 5000);
+			gpio_direction_output(tsin->rst_gpio, 1);
+			usleep_range(3000, 5000);
+		}
+
+		tsin->demux_mapping = index;
+
+		dev_dbg(fei->dev,
+			"channel=%p n=%d tsin_num=%d, invert-ts-clk=%d\n\t"
+			"serial-not-parallel=%d pkt-clk-valid=%d dvb-card=%d\n",
+			fei->channel_data[index], index,
+			tsin->tsin_id, tsin->invert_ts_clk,
+			tsin->serial_not_parallel, tsin->async_not_sync,
+			tsin->dvb_card);
+
+		index++;
+	}
+
+	/* Setup timer interrupt */
+	init_timer(&fei->timer);
+	fei->timer.function = c8sectpfe_timer_interrupt;
+	fei->timer.data = (unsigned long)fei;
+
+	mutex_init(&fei->lock);
+
+	/* Get the configuration information about the tuners */
+	ret = c8sectpfe_tuner_register_frontend(&fei->c8sectpfe[0],
+					(void *)fei,
+					c8sectpfe_start_feed,
+					c8sectpfe_stop_feed);
+	if (ret) {
+		dev_err(dev, "c8sectpfe_tuner_register_frontend failed (%d)\n",
+			ret);
+		goto err_clk_disable;
+	}
+
+	/* ensure all other init has been done before requesting firmware */
+	ret = load_c8sectpfe_fw_step1(fei);
+	if (ret) {
+		dev_err(dev, "Couldn't load slim core firmware\n");
+		goto err_clk_disable;
+	}
+
+	c8sectpfe_debugfs_init(fei);
+
+	return 0;
+
+err_clk_disable:
+	/* TODO uncomment when upstream has taken a reference on this clk */
+	/*clk_disable_unprepare(fei->c8sectpfeclk);*/
+	return ret;
+}
+
+static int c8sectpfe_remove(struct platform_device *pdev)
+{
+	struct c8sectpfei *fei = platform_get_drvdata(pdev);
+	struct channel_info *channel;
+	int i;
+
+	wait_for_completion(&fei->fw_ack);
+
+	c8sectpfe_tuner_unregister_frontend(fei->c8sectpfe[0], fei);
+
+	/*
+	 * Now loop through and un-configure each of the InputBlock resources
+	 */
+	for (i = 0; i < fei->tsin_count; i++) {
+		channel = fei->channel_data[i];
+		free_input_block(fei, channel);
+	}
+
+	c8sectpfe_debugfs_exit(fei);
+
+	dev_info(fei->dev, "Stopping memdma SLIM core\n");
+	if (readl(fei->io + DMA_CPU_RUN))
+		writel(0x0,  fei->io + DMA_CPU_RUN);
+
+	/* unclock all internal IP's */
+	if (readl(fei->io + SYS_INPUT_CLKEN))
+		writel(0, fei->io + SYS_INPUT_CLKEN);
+
+	if (readl(fei->io + SYS_OTHER_CLKEN))
+		writel(0, fei->io + SYS_OTHER_CLKEN);
+
+	/* TODO uncomment when upstream has taken a reference on this clk */
+	/*
+	if (fei->c8sectpfeclk)
+		clk_disable_unprepare(fei->c8sectpfeclk);
+	*/
+
+	return 0;
+}
+
+
+static int configure_channels(struct c8sectpfei *fei)
+{
+	int index = 0, ret;
+	struct channel_info *tsin;
+	struct device_node *child, *np = fei->dev->of_node;
+
+	/* iterate round each tsin and configure memdma descriptor and IB hw */
+	for_each_child_of_node(np, child) {
+
+		tsin = fei->channel_data[index];
+
+		ret = configure_memdma_and_inputblock(fei,
+						fei->channel_data[index]);
+
+		if (ret) {
+			dev_err(fei->dev,
+				"configure_memdma_and_inputblock failed\n");
+			goto err_unmap;
+		}
+		index++;
+	}
+
+	return 0;
+
+err_unmap:
+	for (index = 0; index < fei->tsin_count; index++) {
+		tsin = fei->channel_data[index];
+		free_input_block(fei, tsin);
+	}
+	return ret;
+}
+
+static int
+c8sectpfe_elf_sanity_check(struct c8sectpfei *fei, const struct firmware *fw)
+{
+	struct elf32_hdr *ehdr;
+	char class;
+
+	if (!fw) {
+		dev_err(fei->dev, "failed to load %s\n", FIRMWARE_MEMDMA);
+		return -EINVAL;
+	}
+
+	if (fw->size < sizeof(struct elf32_hdr)) {
+		dev_err(fei->dev, "Image is too small\n");
+		return -EINVAL;
+	}
+
+	ehdr = (struct elf32_hdr *)fw->data;
+
+	/* We only support ELF32 at this point */
+	class = ehdr->e_ident[EI_CLASS];
+	if (class != ELFCLASS32) {
+		dev_err(fei->dev, "Unsupported class: %d\n", class);
+		return -EINVAL;
+	}
+
+	if (ehdr->e_ident[EI_DATA] != ELFDATA2LSB) {
+		dev_err(fei->dev, "Unsupported firmware endianness\n");
+		return -EINVAL;
+	}
+
+	if (fw->size < ehdr->e_shoff + sizeof(struct elf32_shdr)) {
+		dev_err(fei->dev, "Image is too small\n");
+		return -EINVAL;
+	}
+
+	if (memcmp(ehdr->e_ident, ELFMAG, SELFMAG)) {
+		dev_err(fei->dev, "Image is corrupted (bad magic)\n");
+		return -EINVAL;
+	}
+
+	/* Check ELF magic */
+	ehdr = (Elf32_Ehdr *)fw->data;
+	if (ehdr->e_ident[EI_MAG0] != ELFMAG0 ||
+	    ehdr->e_ident[EI_MAG1] != ELFMAG1 ||
+	    ehdr->e_ident[EI_MAG2] != ELFMAG2 ||
+	    ehdr->e_ident[EI_MAG3] != ELFMAG3) {
+		dev_err(fei->dev, "Invalid ELF magic\n");
+		return -EINVAL;
+	}
+
+	if (ehdr->e_type != ET_EXEC) {
+		dev_err(fei->dev, "Unsupported ELF header type\n");
+		return -EINVAL;
+	}
+
+	if (ehdr->e_phoff > fw->size) {
+		dev_err(fei->dev, "Firmware size is too small\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+
+static void load_imem_segment(struct c8sectpfei *fei, Elf32_Phdr *phdr,
+			const struct firmware *fw, u8 __iomem *dest,
+			int seg_num)
+{
+	const u8 *imem_src = fw->data + phdr->p_offset;
+	int i;
+
+	/*
+	 * For IMEM segments, the segment contains 24-bit
+	 * instructions which must be padded to 32-bit
+	 * instructions before being written. The written
+	 * segment is padded with NOP instructions.
+	 */
+
+	dev_dbg(fei->dev,
+		"Loading IMEM segment %d 0x%08x\n\t"
+		" (0x%x bytes) -> 0x%p (0x%x bytes)\n", seg_num,
+		phdr->p_paddr, phdr->p_filesz,
+		dest, phdr->p_memsz + phdr->p_memsz / 3);
+
+	for (i = 0; i < phdr->p_filesz; i++) {
+
+		writeb(readb((void __iomem *)imem_src), (void __iomem *)dest);
+
+		/* Every 3 bytes, add an additional
+		 * padding zero in destination */
+		if (i % 3 == 2) {
+			dest++;
+			writeb(0x00, (void __iomem *)dest);
+		}
+
+		dest++;
+		imem_src++;
+	}
+}
+
+static void load_dmem_segment(struct c8sectpfei *fei, Elf32_Phdr *phdr,
+			const struct firmware *fw, u8 __iomem *dst, int seg_num)
+{
+	/*
+	 * For DMEM segments copy the segment data from the ELF
+	 * file and pad segment with zeroes
+	 */
+
+	dev_dbg(fei->dev,
+		"Loading DMEM segment %d 0x%08x\n\t"
+		"(0x%x bytes) -> 0x%p (0x%x bytes)\n",
+		seg_num, phdr->p_paddr, phdr->p_filesz,
+		dst, phdr->p_memsz);
+
+	memcpy((void __force *)dst, (void *)fw->data + phdr->p_offset,
+		phdr->p_filesz);
+
+	memset((void __force *)dst + phdr->p_filesz, 0,
+		phdr->p_memsz - phdr->p_filesz);
+}
+
+static int load_slim_core_fw(const struct firmware *fw, void *context)
+{
+	struct c8sectpfei *fei = context;
+	Elf32_Ehdr *ehdr;
+	Elf32_Phdr *phdr;
+	u8 __iomem *dst;
+	int err = 0, i;
+
+	if (!fw || !context)
+		return -EINVAL;
+
+	ehdr = (Elf32_Ehdr *)fw->data;
+	phdr = (Elf32_Phdr *)(fw->data + ehdr->e_phoff);
+
+	/* go through the available ELF segments */
+	for (i = 0; i < ehdr->e_phnum; i++, phdr++) {
+
+		/* Only consider LOAD segments */
+		if (phdr->p_type != PT_LOAD)
+			continue;
+
+		/*
+		 * Check segment is contained within the fw->data buffer
+		 */
+		if (phdr->p_offset + phdr->p_filesz > fw->size) {
+			dev_err(fei->dev,
+				"Segment %d is outside of firmware file\n", i);
+			err = -EINVAL;
+			break;
+		}
+
+		/*
+		 * MEMDMA IMEM has executable flag set, otherwise load
+		 * this segment into DMEM.
+		 *
+		 */
+
+		if (phdr->p_flags & PF_X) {
+			dst = (u8 __iomem *) fei->io + DMA_MEMDMA_IMEM;
+			/*
+			 * The Slim ELF file uses 32-bit word addressing for
+			 * load offsets.
+			 */
+			dst += (phdr->p_paddr & 0xFFFFF) * sizeof(unsigned int);
+			load_imem_segment(fei, phdr, fw, dst, i);
+		} else {
+			dst = (u8 __iomem *) fei->io + DMA_MEMDMA_DMEM;
+			/*
+			 * The Slim ELF file uses 32-bit word addressing for
+			 * load offsets.
+			 */
+			dst += (phdr->p_paddr & 0xFFFFF) * sizeof(unsigned int);
+			load_dmem_segment(fei, phdr, fw, dst, i);
+		}
+	}
+
+	release_firmware(fw);
+	return err;
+}
+
+static void load_c8sectpfe_fw_cb(const struct firmware *fw, void *context)
+{
+	struct c8sectpfei *fei = context;
+	int err;
+
+	err = c8sectpfe_elf_sanity_check(fei, fw);
+	if (err) {
+		dev_err(fei->dev, "c8sectpfe_elf_sanity_check failed err=(%d)\n"
+			, err);
+		goto err;
+	}
+
+	err = load_slim_core_fw(fw, context);
+	if (err) {
+		dev_err(fei->dev, "load_slim_core_fw failed err=(%d)\n", err);
+		goto err;
+	}
+
+	/* now the firmware is loaded configure the input blocks */
+	err = configure_channels(fei);
+	if (err) {
+		dev_err(fei->dev, "configure_channels failed err=(%d)\n", err);
+		goto err;
+	}
+
+	/*
+	 * STBus target port can access IMEM and DMEM ports
+	 * without waiting for CPU
+	 */
+	writel(0x1, fei->io + DMA_PER_STBUS_SYNC);
+
+	dev_info(fei->dev, "Boot the memdma SLIM core\n");
+	writel(0x1,  fei->io + DMA_CPU_RUN);
+
+	atomic_set(&fei->fw_loaded, 1);
+err:
+	complete_all(&fei->fw_ack);
+}
+
+static int load_c8sectpfe_fw_step1(struct c8sectpfei *fei)
+{
+	int err;
+
+	dev_info(fei->dev, "Loading firmware: %s\n", FIRMWARE_MEMDMA);
+
+	init_completion(&fei->fw_ack);
+	atomic_set(&fei->fw_loaded, 0);
+
+	err = request_firmware_nowait(THIS_MODULE, FW_ACTION_HOTPLUG,
+				FIRMWARE_MEMDMA, fei->dev, GFP_KERNEL, fei,
+				load_c8sectpfe_fw_cb);
+
+	if (err) {
+		dev_err(fei->dev, "request_firmware_nowait err: %d.\n", err);
+		complete_all(&fei->fw_ack);
+		return err;
+	}
+
+	return 0;
+}
+
+static const struct of_device_id c8sectpfe_match[] = {
+	{ .compatible = "st,stih407-c8sectpfe" },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, c8sectpfe_match);
+
+static struct platform_driver c8sectpfe_driver = {
+	.driver = {
+		.name = "c8sectpfe",
+		.of_match_table = of_match_ptr(c8sectpfe_match),
+	},
+	.probe	= c8sectpfe_probe,
+	.remove	= c8sectpfe_remove,
+};
+
+module_platform_driver(c8sectpfe_driver);
+
+MODULE_AUTHOR("Peter Bennett <peter.bennett@st.com>");
+MODULE_AUTHOR("Peter Griffin <peter.griffin@linaro.org>");
+MODULE_DESCRIPTION("C8SECTPFE STi DVB Driver");
+MODULE_LICENSE("GPL");
diff --git a/kernel/drivers/media/platform/sti/c8sectpfe/c8sectpfe-core.h b/kernel/drivers/media/platform/sti/c8sectpfe/c8sectpfe-core.h
new file mode 100644
index 000000000..39e7a221a
--- /dev/null
+++ b/kernel/drivers/media/platform/sti/c8sectpfe/c8sectpfe-core.h
@@ -0,0 +1,288 @@
+/*
+ * c8sectpfe-core.h - C8SECTPFE STi DVB driver
+ *
+ * Copyright (c) STMicroelectronics 2015
+ *
+ *   Author:Peter Bennett <peter.bennett@st.com>
+ *	    Peter Griffin <peter.griffin@linaro.org>
+ *
+ *	This program is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU General Public License as
+ *	published by the Free Software Foundation; either version 2 of
+ *	the License, or (at your option) any later version.
+ */
+#ifndef _C8SECTPFE_CORE_H_
+#define _C8SECTPFE_CORE_H_
+
+#define C8SECTPFEI_MAXCHANNEL 16
+#define C8SECTPFEI_MAXADAPTER 3
+
+#define C8SECTPFE_MAX_TSIN_CHAN 8
+
+struct channel_info {
+
+	int tsin_id;
+	bool invert_ts_clk;
+	bool serial_not_parallel;
+	bool async_not_sync;
+	int i2c;
+	int dvb_card;
+
+	int rst_gpio;
+
+	struct i2c_adapter  *i2c_adapter;
+	struct i2c_adapter  *tuner_i2c;
+	struct i2c_adapter  *lnb_i2c;
+	struct i2c_client   *i2c_client;
+	struct dvb_frontend *frontend;
+
+	struct pinctrl_state *pstate;
+
+	int demux_mapping;
+	int active;
+
+	void *back_buffer_start;
+	void *back_buffer_aligned;
+	dma_addr_t back_buffer_busaddr;
+
+	void *pid_buffer_start;
+	void *pid_buffer_aligned;
+	dma_addr_t pid_buffer_busaddr;
+
+	unsigned long  fifo;
+
+	struct completion idle_completion;
+	struct tasklet_struct tsklet;
+
+	struct c8sectpfei *fei;
+	void __iomem *irec;
+
+};
+
+struct c8sectpfe_hw {
+	int num_ib;
+	int num_mib;
+	int num_swts;
+	int num_tsout;
+	int num_ccsc;
+	int num_ram;
+	int num_tp;
+};
+
+struct c8sectpfei {
+
+	struct device *dev;
+	struct pinctrl *pinctrl;
+
+	struct dentry *root;
+	struct debugfs_regset32	*regset;
+	struct completion fw_ack;
+	atomic_t fw_loaded;
+
+	int tsin_count;
+
+	struct c8sectpfe_hw hw_stats;
+
+	struct c8sectpfe *c8sectpfe[C8SECTPFEI_MAXADAPTER];
+
+	int mapping[C8SECTPFEI_MAXCHANNEL];
+
+	struct mutex lock;
+
+	struct timer_list timer;	/* timer interrupts for outputs */
+
+	void __iomem *io;
+	void __iomem *sram;
+
+	unsigned long sram_size;
+
+	struct channel_info *channel_data[C8SECTPFE_MAX_TSIN_CHAN];
+
+	struct clk *c8sectpfeclk;
+	int nima_rst_gpio;
+	int nimb_rst_gpio;
+
+	int idle_irq;
+	int error_irq;
+
+	int global_feed_count;
+};
+
+/* C8SECTPFE SYS Regs list */
+
+#define SYS_INPUT_ERR_STATUS	0x0
+#define SYS_OTHER_ERR_STATUS	0x8
+#define SYS_INPUT_ERR_MASK	0x10
+#define SYS_OTHER_ERR_MASK	0x18
+#define SYS_DMA_ROUTE		0x20
+#define SYS_INPUT_CLKEN		0x30
+#define IBENABLE_MASK			0x7F
+
+#define SYS_OTHER_CLKEN		0x38
+#define TSDMAENABLE			BIT(1)
+#define MEMDMAENABLE			BIT(0)
+
+#define SYS_CFG_NUM_IB		0x200
+#define SYS_CFG_NUM_MIB		0x204
+#define SYS_CFG_NUM_SWTS	0x208
+#define SYS_CFG_NUM_TSOUT	0x20C
+#define SYS_CFG_NUM_CCSC	0x210
+#define SYS_CFG_NUM_RAM		0x214
+#define SYS_CFG_NUM_TP		0x218
+
+/* Input Block Regs */
+
+#define C8SECTPFE_INPUTBLK_OFFSET	0x1000
+#define C8SECTPFE_CHANNEL_OFFSET(x)	((x*0x40) + C8SECTPFE_INPUTBLK_OFFSET)
+
+#define C8SECTPFE_IB_IP_FMT_CFG(x)      (C8SECTPFE_CHANNEL_OFFSET(x) + 0x00)
+#define C8SECTPFE_IGNORE_ERR_AT_SOP     BIT(7)
+#define C8SECTPFE_IGNORE_ERR_IN_PKT     BIT(6)
+#define C8SECTPFE_IGNORE_ERR_IN_BYTE    BIT(5)
+#define C8SECTPFE_INVERT_TSCLK          BIT(4)
+#define C8SECTPFE_ALIGN_BYTE_SOP        BIT(3)
+#define C8SECTPFE_ASYNC_NOT_SYNC        BIT(2)
+#define C8SECTPFE_BYTE_ENDIANNESS_MSB    BIT(1)
+#define C8SECTPFE_SERIAL_NOT_PARALLEL   BIT(0)
+
+#define C8SECTPFE_IB_SYNCLCKDRP_CFG(x)   (C8SECTPFE_CHANNEL_OFFSET(x) + 0x04)
+#define C8SECTPFE_SYNC(x)                (x & 0xf)
+#define C8SECTPFE_DROP(x)                ((x<<4) & 0xf)
+#define C8SECTPFE_TOKEN(x)               ((x<<8) & 0xff00)
+#define C8SECTPFE_SLDENDIANNESS          BIT(16)
+
+#define C8SECTPFE_IB_TAGBYTES_CFG(x)     (C8SECTPFE_CHANNEL_OFFSET(x) + 0x08)
+#define C8SECTPFE_TAG_HEADER(x)          (x << 16)
+#define C8SECTPFE_TAG_COUNTER(x)         ((x<<1) & 0x7fff)
+#define C8SECTPFE_TAG_ENABLE             BIT(0)
+
+#define C8SECTPFE_IB_PID_SET(x)          (C8SECTPFE_CHANNEL_OFFSET(x) + 0x0C)
+#define C8SECTPFE_PID_OFFSET(x)          (x & 0x3f)
+#define C8SECTPFE_PID_NUMBITS(x)         ((x << 6) & 0xfff)
+#define C8SECTPFE_PID_ENABLE             BIT(31)
+
+#define C8SECTPFE_IB_PKT_LEN(x)          (C8SECTPFE_CHANNEL_OFFSET(x) + 0x10)
+
+#define C8SECTPFE_IB_BUFF_STRT(x)        (C8SECTPFE_CHANNEL_OFFSET(x) + 0x14)
+#define C8SECTPFE_IB_BUFF_END(x)         (C8SECTPFE_CHANNEL_OFFSET(x) + 0x18)
+#define C8SECTPFE_IB_READ_PNT(x)         (C8SECTPFE_CHANNEL_OFFSET(x) + 0x1C)
+#define C8SECTPFE_IB_WRT_PNT(x)          (C8SECTPFE_CHANNEL_OFFSET(x) + 0x20)
+
+#define C8SECTPFE_IB_PRI_THRLD(x)        (C8SECTPFE_CHANNEL_OFFSET(x) + 0x24)
+#define C8SECTPFE_PRI_VALUE(x)           (x & 0x7fffff)
+#define C8SECTPFE_PRI_LOWPRI(x)          ((x & 0xf) << 24)
+#define C8SECTPFE_PRI_HIGHPRI(x)         ((x & 0xf) << 28)
+
+#define C8SECTPFE_IB_STAT(x)             (C8SECTPFE_CHANNEL_OFFSET(x) + 0x28)
+#define C8SECTPFE_STAT_FIFO_OVERFLOW(x)  (x & 0x1)
+#define C8SECTPFE_STAT_BUFFER_OVERFLOW(x) (x & 0x2)
+#define C8SECTPFE_STAT_OUTOFORDERRP(x)   (x & 0x4)
+#define C8SECTPFE_STAT_PID_OVERFLOW(x)   (x & 0x8)
+#define C8SECTPFE_STAT_PKT_OVERFLOW(x)   (x & 0x10)
+#define C8SECTPFE_STAT_ERROR_PACKETS(x)  ((x >> 8) & 0xf)
+#define C8SECTPFE_STAT_SHORT_PACKETS(x)  ((x >> 12) & 0xf)
+
+#define C8SECTPFE_IB_MASK(x)             (C8SECTPFE_CHANNEL_OFFSET(x) + 0x2C)
+#define C8SECTPFE_MASK_FIFO_OVERFLOW     BIT(0)
+#define C8SECTPFE_MASK_BUFFER_OVERFLOW   BIT(1)
+#define C8SECTPFE_MASK_OUTOFORDERRP(x)   BIT(2)
+#define C8SECTPFE_MASK_PID_OVERFLOW(x)   BIT(3)
+#define C8SECTPFE_MASK_PKT_OVERFLOW(x)   BIT(4)
+#define C8SECTPFE_MASK_ERROR_PACKETS(x)  ((x & 0xf) << 8)
+#define C8SECTPFE_MASK_SHORT_PACKETS(x)  ((x & 0xf) >> 12)
+
+#define C8SECTPFE_IB_SYS(x)              (C8SECTPFE_CHANNEL_OFFSET(x) + 0x30)
+#define C8SECTPFE_SYS_RESET              BIT(1)
+#define C8SECTPFE_SYS_ENABLE             BIT(0)
+
+/*
+ * Ponter record data structure required for each input block
+ * see Table 82 on page 167 of functional specification.
+ */
+
+#define DMA_PRDS_MEMBASE	0x0 /* Internal sram base address */
+#define DMA_PRDS_MEMTOP		0x4 /* Internal sram top address */
+
+/*
+ * TS packet size, including tag bytes added by input block,
+ * rounded up to the next multiple of 8 bytes. The packet size,
+ * including any tagging bytes and rounded up to the nearest
+ * multiple of 8 bytes must be less than 255 bytes.
+ */
+#define DMA_PRDS_PKTSIZE	0x8
+#define DMA_PRDS_TPENABLE	0xc
+
+#define TP0_OFFSET		0x10
+#define DMA_PRDS_BUSBASE_TP(x)	((0x10*x) + TP0_OFFSET)
+#define DMA_PRDS_BUSTOP_TP(x)	((0x10*x) + TP0_OFFSET + 0x4)
+#define DMA_PRDS_BUSWP_TP(x)	((0x10*x) + TP0_OFFSET + 0x8)
+#define DMA_PRDS_BUSRP_TP(x)	((0x10*x) + TP0_OFFSET + 0xc)
+
+#define DMA_PRDS_SIZE		(0x20)
+
+#define DMA_MEMDMA_OFFSET	0x4000
+#define DMA_IMEM_OFFSET		0x0
+#define DMA_DMEM_OFFSET		0x4000
+#define DMA_CPU			0x8000
+#define DMA_PER_OFFSET		0xb000
+
+#define DMA_MEMDMA_DMEM (DMA_MEMDMA_OFFSET + DMA_DMEM_OFFSET)
+#define DMA_MEMDMA_IMEM (DMA_MEMDMA_OFFSET + DMA_IMEM_OFFSET)
+
+/* XP70 Slim core regs */
+#define DMA_CPU_ID	(DMA_MEMDMA_OFFSET + DMA_CPU + 0x0)
+#define DMA_CPU_VCR	(DMA_MEMDMA_OFFSET + DMA_CPU + 0x4)
+#define DMA_CPU_RUN	(DMA_MEMDMA_OFFSET + DMA_CPU + 0x8)
+#define DMA_CPU_CLOCKGATE	(DMA_MEMDMA_OFFSET + DMA_CPU + 0xc)
+#define DMA_CPU_PC	(DMA_MEMDMA_OFFSET + DMA_CPU + 0x20)
+
+/* Enable Interrupt for a IB */
+#define DMA_PER_TPn_DREQ_MASK	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xd00)
+/* Ack interrupt by setting corresponding bit */
+#define DMA_PER_TPn_DACK_SET	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xd80)
+#define DMA_PER_TPn_DREQ	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xe00)
+#define DMA_PER_TPn_DACK	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xe80)
+#define DMA_PER_DREQ_MODE	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xf80)
+#define DMA_PER_STBUS_SYNC	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xf88)
+#define DMA_PER_STBUS_ACCESS	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xf8c)
+#define DMA_PER_STBUS_ADDRESS	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xf90)
+#define DMA_PER_IDLE_INT	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfa8)
+#define DMA_PER_PRIORITY	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfac)
+#define DMA_PER_MAX_OPCODE	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfb0)
+#define DMA_PER_MAX_CHUNK	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfb4)
+#define DMA_PER_PAGE_SIZE	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfbc)
+#define DMA_PER_MBOX_STATUS	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfc0)
+#define DMA_PER_MBOX_SET	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfc8)
+#define DMA_PER_MBOX_CLEAR	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfd0)
+#define DMA_PER_MBOX_MASK	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfd8)
+#define DMA_PER_INJECT_PKT_SRC	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfe0)
+#define DMA_PER_INJECT_PKT_DEST	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfe4)
+#define DMA_PER_INJECT_PKT_ADDR	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfe8)
+#define DMA_PER_INJECT_PKT	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfec)
+#define DMA_PER_PAT_PTR_INIT	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xff0)
+#define DMA_PER_PAT_PTR		(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xff4)
+#define DMA_PER_SLEEP_MASK	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xff8)
+#define DMA_PER_SLEEP_COUNTER	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xffc)
+/* #define DMA_RF_CPUREGn	DMA_RFBASEADDR n=0 to 15) slim regsa */
+
+/* The following are from DMA_DMEM_BaseAddress */
+#define DMA_FIRMWARE_VERSION	(DMA_MEMDMA_OFFSET + DMA_DMEM_OFFSET + 0x0)
+#define DMA_PTRREC_BASE		(DMA_MEMDMA_OFFSET + DMA_DMEM_OFFSET + 0x4)
+#define DMA_PTRREC_INPUT_OFFSET	(DMA_MEMDMA_OFFSET + DMA_DMEM_OFFSET + 0x8)
+#define DMA_ERRREC_BASE		(DMA_MEMDMA_OFFSET + DMA_DMEM_OFFSET + 0xc)
+#define DMA_ERROR_RECORD(n)	((n*4) + DMA_ERRREC_BASE + 0x4)
+#define DMA_IDLE_REQ		(DMA_MEMDMA_OFFSET + DMA_DMEM_OFFSET + 0x10)
+#define IDLEREQ			BIT(31)
+
+#define DMA_FIRMWARE_CONFIG	(DMA_MEMDMA_OFFSET + DMA_DMEM_OFFSET + 0x14)
+
+/* Regs for PID Filter */
+
+#define PIDF_OFFSET		0x2800
+#define PIDF_BASE(n)		((n*4) + PIDF_OFFSET)
+#define PIDF_LEAK_ENABLE	(PIDF_OFFSET + 0x100)
+#define PIDF_LEAK_STATUS	(PIDF_OFFSET + 0x108)
+#define PIDF_LEAK_COUNT_RESET	(PIDF_OFFSET + 0x110)
+#define PIDF_LEAK_COUNTER	(PIDF_OFFSET + 0x114)
+
+#endif /* _C8SECTPFE_CORE_H_ */
diff --git a/kernel/drivers/media/platform/sti/c8sectpfe/c8sectpfe-debugfs.c b/kernel/drivers/media/platform/sti/c8sectpfe/c8sectpfe-debugfs.c
new file mode 100644
index 000000000..e9ba13db4
--- /dev/null
+++ b/kernel/drivers/media/platform/sti/c8sectpfe/c8sectpfe-debugfs.c
@@ -0,0 +1,271 @@
+/*
+ * c8sectpfe-debugfs.c - C8SECTPFE STi DVB driver
+ *
+ * Copyright (c) STMicroelectronics 2015
+ *
+ * Author: Peter Griffin <peter.griffin@linaro.org>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2  of
+ * the License as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#include <linux/debugfs.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/seq_file.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+
+#include "c8sectpfe-debugfs.h"
+
+#define dump_register(nm ...)			\
+{						\
+	.name	= #nm,				\
+	.offset	= nm,				\
+}
+
+static const struct debugfs_reg32 fei_sys_regs[] = {
+	dump_register(SYS_INPUT_ERR_STATUS),
+	dump_register(SYS_OTHER_ERR_STATUS),
+	dump_register(SYS_INPUT_ERR_MASK),
+	dump_register(SYS_DMA_ROUTE),
+	dump_register(SYS_INPUT_CLKEN),
+	dump_register(IBENABLE_MASK),
+	dump_register(SYS_OTHER_CLKEN),
+	dump_register(SYS_CFG_NUM_IB),
+	dump_register(SYS_CFG_NUM_MIB),
+	dump_register(SYS_CFG_NUM_SWTS),
+	dump_register(SYS_CFG_NUM_TSOUT),
+	dump_register(SYS_CFG_NUM_CCSC),
+	dump_register(SYS_CFG_NUM_RAM),
+	dump_register(SYS_CFG_NUM_TP),
+
+	dump_register(C8SECTPFE_IB_IP_FMT_CFG(0)),
+	dump_register(C8SECTPFE_IB_TAGBYTES_CFG(0)),
+	dump_register(C8SECTPFE_IB_PID_SET(0)),
+	dump_register(C8SECTPFE_IB_PKT_LEN(0)),
+	dump_register(C8SECTPFE_IB_BUFF_STRT(0)),
+	dump_register(C8SECTPFE_IB_BUFF_END(0)),
+	dump_register(C8SECTPFE_IB_READ_PNT(0)),
+	dump_register(C8SECTPFE_IB_WRT_PNT(0)),
+	dump_register(C8SECTPFE_IB_PRI_THRLD(0)),
+	dump_register(C8SECTPFE_IB_STAT(0)),
+	dump_register(C8SECTPFE_IB_MASK(0)),
+	dump_register(C8SECTPFE_IB_SYS(0)),
+
+	dump_register(C8SECTPFE_IB_IP_FMT_CFG(1)),
+	dump_register(C8SECTPFE_IB_TAGBYTES_CFG(1)),
+	dump_register(C8SECTPFE_IB_PID_SET(1)),
+	dump_register(C8SECTPFE_IB_PKT_LEN(1)),
+	dump_register(C8SECTPFE_IB_BUFF_STRT(1)),
+	dump_register(C8SECTPFE_IB_BUFF_END(1)),
+	dump_register(C8SECTPFE_IB_READ_PNT(1)),
+	dump_register(C8SECTPFE_IB_WRT_PNT(1)),
+	dump_register(C8SECTPFE_IB_PRI_THRLD(1)),
+	dump_register(C8SECTPFE_IB_STAT(1)),
+	dump_register(C8SECTPFE_IB_MASK(1)),
+	dump_register(C8SECTPFE_IB_SYS(1)),
+
+	dump_register(C8SECTPFE_IB_IP_FMT_CFG(2)),
+	dump_register(C8SECTPFE_IB_TAGBYTES_CFG(2)),
+	dump_register(C8SECTPFE_IB_PID_SET(2)),
+	dump_register(C8SECTPFE_IB_PKT_LEN(2)),
+	dump_register(C8SECTPFE_IB_BUFF_STRT(2)),
+	dump_register(C8SECTPFE_IB_BUFF_END(2)),
+	dump_register(C8SECTPFE_IB_READ_PNT(2)),
+	dump_register(C8SECTPFE_IB_WRT_PNT(2)),
+	dump_register(C8SECTPFE_IB_PRI_THRLD(2)),
+	dump_register(C8SECTPFE_IB_STAT(2)),
+	dump_register(C8SECTPFE_IB_MASK(2)),
+	dump_register(C8SECTPFE_IB_SYS(2)),
+
+	dump_register(C8SECTPFE_IB_IP_FMT_CFG(3)),
+	dump_register(C8SECTPFE_IB_TAGBYTES_CFG(3)),
+	dump_register(C8SECTPFE_IB_PID_SET(3)),
+	dump_register(C8SECTPFE_IB_PKT_LEN(3)),
+	dump_register(C8SECTPFE_IB_BUFF_STRT(3)),
+	dump_register(C8SECTPFE_IB_BUFF_END(3)),
+	dump_register(C8SECTPFE_IB_READ_PNT(3)),
+	dump_register(C8SECTPFE_IB_WRT_PNT(3)),
+	dump_register(C8SECTPFE_IB_PRI_THRLD(3)),
+	dump_register(C8SECTPFE_IB_STAT(3)),
+	dump_register(C8SECTPFE_IB_MASK(3)),
+	dump_register(C8SECTPFE_IB_SYS(3)),
+
+	dump_register(C8SECTPFE_IB_IP_FMT_CFG(4)),
+	dump_register(C8SECTPFE_IB_TAGBYTES_CFG(4)),
+	dump_register(C8SECTPFE_IB_PID_SET(4)),
+	dump_register(C8SECTPFE_IB_PKT_LEN(4)),
+	dump_register(C8SECTPFE_IB_BUFF_STRT(4)),
+	dump_register(C8SECTPFE_IB_BUFF_END(4)),
+	dump_register(C8SECTPFE_IB_READ_PNT(4)),
+	dump_register(C8SECTPFE_IB_WRT_PNT(4)),
+	dump_register(C8SECTPFE_IB_PRI_THRLD(4)),
+	dump_register(C8SECTPFE_IB_STAT(4)),
+	dump_register(C8SECTPFE_IB_MASK(4)),
+	dump_register(C8SECTPFE_IB_SYS(4)),
+
+	dump_register(C8SECTPFE_IB_IP_FMT_CFG(5)),
+	dump_register(C8SECTPFE_IB_TAGBYTES_CFG(5)),
+	dump_register(C8SECTPFE_IB_PID_SET(5)),
+	dump_register(C8SECTPFE_IB_PKT_LEN(5)),
+	dump_register(C8SECTPFE_IB_BUFF_STRT(5)),
+	dump_register(C8SECTPFE_IB_BUFF_END(5)),
+	dump_register(C8SECTPFE_IB_READ_PNT(5)),
+	dump_register(C8SECTPFE_IB_WRT_PNT(5)),
+	dump_register(C8SECTPFE_IB_PRI_THRLD(5)),
+	dump_register(C8SECTPFE_IB_STAT(5)),
+	dump_register(C8SECTPFE_IB_MASK(5)),
+	dump_register(C8SECTPFE_IB_SYS(5)),
+
+	dump_register(C8SECTPFE_IB_IP_FMT_CFG(6)),
+	dump_register(C8SECTPFE_IB_TAGBYTES_CFG(6)),
+	dump_register(C8SECTPFE_IB_PID_SET(6)),
+	dump_register(C8SECTPFE_IB_PKT_LEN(6)),
+	dump_register(C8SECTPFE_IB_BUFF_STRT(6)),
+	dump_register(C8SECTPFE_IB_BUFF_END(6)),
+	dump_register(C8SECTPFE_IB_READ_PNT(6)),
+	dump_register(C8SECTPFE_IB_WRT_PNT(6)),
+	dump_register(C8SECTPFE_IB_PRI_THRLD(6)),
+	dump_register(C8SECTPFE_IB_STAT(6)),
+	dump_register(C8SECTPFE_IB_MASK(6)),
+	dump_register(C8SECTPFE_IB_SYS(6)),
+
+	dump_register(DMA_CPU_ID),
+	dump_register(DMA_CPU_VCR),
+	dump_register(DMA_CPU_RUN),
+	dump_register(DMA_CPU_PC),
+
+	dump_register(DMA_PER_TPn_DREQ_MASK),
+	dump_register(DMA_PER_TPn_DACK_SET),
+	dump_register(DMA_PER_TPn_DREQ),
+	dump_register(DMA_PER_TPn_DACK),
+	dump_register(DMA_PER_DREQ_MODE),
+	dump_register(DMA_PER_STBUS_SYNC),
+	dump_register(DMA_PER_STBUS_ACCESS),
+	dump_register(DMA_PER_STBUS_ADDRESS),
+	dump_register(DMA_PER_IDLE_INT),
+	dump_register(DMA_PER_PRIORITY),
+	dump_register(DMA_PER_MAX_OPCODE),
+	dump_register(DMA_PER_MAX_CHUNK),
+	dump_register(DMA_PER_PAGE_SIZE),
+	dump_register(DMA_PER_MBOX_STATUS),
+	dump_register(DMA_PER_MBOX_SET),
+	dump_register(DMA_PER_MBOX_CLEAR),
+	dump_register(DMA_PER_MBOX_MASK),
+	dump_register(DMA_PER_INJECT_PKT_SRC),
+	dump_register(DMA_PER_INJECT_PKT_DEST),
+	dump_register(DMA_PER_INJECT_PKT_ADDR),
+	dump_register(DMA_PER_INJECT_PKT),
+	dump_register(DMA_PER_PAT_PTR_INIT),
+	dump_register(DMA_PER_PAT_PTR),
+	dump_register(DMA_PER_SLEEP_MASK),
+	dump_register(DMA_PER_SLEEP_COUNTER),
+
+	dump_register(DMA_FIRMWARE_VERSION),
+	dump_register(DMA_PTRREC_BASE),
+	dump_register(DMA_PTRREC_INPUT_OFFSET),
+	dump_register(DMA_ERRREC_BASE),
+
+	dump_register(DMA_ERROR_RECORD(0)),
+	dump_register(DMA_ERROR_RECORD(1)),
+	dump_register(DMA_ERROR_RECORD(2)),
+	dump_register(DMA_ERROR_RECORD(3)),
+	dump_register(DMA_ERROR_RECORD(4)),
+	dump_register(DMA_ERROR_RECORD(5)),
+	dump_register(DMA_ERROR_RECORD(6)),
+	dump_register(DMA_ERROR_RECORD(7)),
+	dump_register(DMA_ERROR_RECORD(8)),
+	dump_register(DMA_ERROR_RECORD(9)),
+	dump_register(DMA_ERROR_RECORD(10)),
+	dump_register(DMA_ERROR_RECORD(11)),
+	dump_register(DMA_ERROR_RECORD(12)),
+	dump_register(DMA_ERROR_RECORD(13)),
+	dump_register(DMA_ERROR_RECORD(14)),
+	dump_register(DMA_ERROR_RECORD(15)),
+	dump_register(DMA_ERROR_RECORD(16)),
+	dump_register(DMA_ERROR_RECORD(17)),
+	dump_register(DMA_ERROR_RECORD(18)),
+	dump_register(DMA_ERROR_RECORD(19)),
+	dump_register(DMA_ERROR_RECORD(20)),
+	dump_register(DMA_ERROR_RECORD(21)),
+	dump_register(DMA_ERROR_RECORD(22)),
+
+	dump_register(DMA_IDLE_REQ),
+	dump_register(DMA_FIRMWARE_CONFIG),
+
+	dump_register(PIDF_BASE(0)),
+	dump_register(PIDF_BASE(1)),
+	dump_register(PIDF_BASE(2)),
+	dump_register(PIDF_BASE(3)),
+	dump_register(PIDF_BASE(4)),
+	dump_register(PIDF_BASE(5)),
+	dump_register(PIDF_BASE(6)),
+	dump_register(PIDF_BASE(7)),
+	dump_register(PIDF_BASE(8)),
+	dump_register(PIDF_BASE(9)),
+	dump_register(PIDF_BASE(10)),
+	dump_register(PIDF_BASE(11)),
+	dump_register(PIDF_BASE(12)),
+	dump_register(PIDF_BASE(13)),
+	dump_register(PIDF_BASE(14)),
+	dump_register(PIDF_BASE(15)),
+	dump_register(PIDF_BASE(16)),
+	dump_register(PIDF_BASE(17)),
+	dump_register(PIDF_BASE(18)),
+	dump_register(PIDF_BASE(19)),
+	dump_register(PIDF_BASE(20)),
+	dump_register(PIDF_BASE(21)),
+	dump_register(PIDF_BASE(22)),
+	dump_register(PIDF_LEAK_ENABLE),
+	dump_register(PIDF_LEAK_STATUS),
+	dump_register(PIDF_LEAK_COUNT_RESET),
+	dump_register(PIDF_LEAK_COUNTER),
+};
+
+void c8sectpfe_debugfs_init(struct c8sectpfei *fei)
+{
+	struct dentry		*root;
+	struct dentry		*file;
+
+	root = debugfs_create_dir("c8sectpfe", NULL);
+	if (!root)
+		goto err;
+
+	fei->root = root;
+
+	fei->regset =  devm_kzalloc(fei->dev, sizeof(*fei->regset), GFP_KERNEL);
+	if (!fei->regset)
+		goto err;
+
+	fei->regset->regs = fei_sys_regs;
+	fei->regset->nregs = ARRAY_SIZE(fei_sys_regs);
+	fei->regset->base = fei->io;
+
+	file = debugfs_create_regset32("registers", S_IRUGO, root,
+				fei->regset);
+	if (!file) {
+		dev_err(fei->dev,
+			"%s not able to create 'registers' debugfs\n"
+			, __func__);
+		goto err;
+	}
+
+	return;
+
+err:
+	debugfs_remove_recursive(root);
+}
+
+void c8sectpfe_debugfs_exit(struct c8sectpfei *fei)
+{
+	debugfs_remove_recursive(fei->root);
+	fei->root = NULL;
+}
diff --git a/kernel/drivers/media/platform/sti/c8sectpfe/c8sectpfe-debugfs.h b/kernel/drivers/media/platform/sti/c8sectpfe/c8sectpfe-debugfs.h
new file mode 100644
index 000000000..8af1ac137
--- /dev/null
+++ b/kernel/drivers/media/platform/sti/c8sectpfe/c8sectpfe-debugfs.h
@@ -0,0 +1,26 @@
+/**
+ * c8sectpfe-debugfs.h - C8SECTPFE STi DVB driver debugfs header
+ *
+ * Copyright (c) STMicroelectronics 2015
+ *
+ * Authors: Peter Griffin <peter.griffin@linaro.org>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2  of
+ * the License as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __C8SECTPFE_DEBUG_H
+#define __C8SECTPFE_DEBUG_H
+
+#include "c8sectpfe-core.h"
+
+void c8sectpfe_debugfs_init(struct c8sectpfei *);
+void c8sectpfe_debugfs_exit(struct c8sectpfei *);
+
+#endif /* __C8SECTPFE_DEBUG_H */
diff --git a/kernel/drivers/media/platform/sti/c8sectpfe/c8sectpfe-dvb.c b/kernel/drivers/media/platform/sti/c8sectpfe/c8sectpfe-dvb.c
new file mode 100644
index 000000000..69d7fe447
--- /dev/null
+++ b/kernel/drivers/media/platform/sti/c8sectpfe/c8sectpfe-dvb.c
@@ -0,0 +1,244 @@
+/*
+ *  c8sectpfe-dvb.c - C8SECTPFE STi DVB driver
+ *
+ * Copyright (c) STMicroelectronics 2015
+ *
+ *  Author Peter Griffin <peter.griffin@linaro.org>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ *  GNU General Public License for more details.
+ */
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/version.h>
+
+#include <dt-bindings/media/c8sectpfe.h>
+
+#include "c8sectpfe-common.h"
+#include "c8sectpfe-core.h"
+#include "c8sectpfe-dvb.h"
+
+#include "dvb-pll.h"
+#include "lnbh24.h"
+#include "stv0367.h"
+#include "stv0367_priv.h"
+#include "stv6110x.h"
+#include "stv090x.h"
+#include "tda18212.h"
+
+static inline const char *dvb_card_str(unsigned int c)
+{
+	switch (c) {
+	case STV0367_TDA18212_NIMA_1:	return "STV0367_TDA18212_NIMA_1";
+	case STV0367_TDA18212_NIMA_2:	return "STV0367_TDA18212_NIMA_2";
+	case STV0367_TDA18212_NIMB_1:	return "STV0367_TDA18212_NIMB_1";
+	case STV0367_TDA18212_NIMB_2:	return "STV0367_TDA18212_NIMB_2";
+	case STV0903_6110_LNB24_NIMA:	return "STV0903_6110_LNB24_NIMA";
+	case STV0903_6110_LNB24_NIMB:	return "STV0903_6110_LNB24_NIMB";
+	default:			return "unknown dvb frontend card";
+	}
+}
+
+static struct stv090x_config stv090x_config = {
+	.device                 = STV0903,
+	.demod_mode             = STV090x_SINGLE,
+	.clk_mode               = STV090x_CLK_EXT,
+	.xtal                   = 16000000,
+	.address                = 0x69,
+
+	.ts1_mode               = STV090x_TSMODE_SERIAL_CONTINUOUS,
+	.ts2_mode               = STV090x_TSMODE_SERIAL_CONTINUOUS,
+
+	.repeater_level         = STV090x_RPTLEVEL_64,
+
+	.tuner_init             = NULL,
+	.tuner_set_mode         = NULL,
+	.tuner_set_frequency    = NULL,
+	.tuner_get_frequency    = NULL,
+	.tuner_set_bandwidth    = NULL,
+	.tuner_get_bandwidth    = NULL,
+	.tuner_set_bbgain       = NULL,
+	.tuner_get_bbgain       = NULL,
+	.tuner_set_refclk       = NULL,
+	.tuner_get_status       = NULL,
+};
+
+static struct stv6110x_config stv6110x_config = {
+	.addr                   = 0x60,
+	.refclk                 = 16000000,
+};
+
+#define NIMA 0
+#define NIMB 1
+
+static struct stv0367_config stv0367_tda18212_config[] = {
+	{
+		.demod_address = 0x1c,
+		.xtal = 16000000,
+		.if_khz = 4500,
+		.if_iq_mode = FE_TER_NORMAL_IF_TUNER,
+		.ts_mode = STV0367_SERIAL_PUNCT_CLOCK,
+		.clk_pol = STV0367_CLOCKPOLARITY_DEFAULT,
+	}, {
+		.demod_address = 0x1d,
+		.xtal = 16000000,
+		.if_khz = 4500,
+		.if_iq_mode = FE_TER_NORMAL_IF_TUNER,
+		.ts_mode = STV0367_SERIAL_PUNCT_CLOCK,
+		.clk_pol = STV0367_CLOCKPOLARITY_DEFAULT,
+	}, {
+		.demod_address = 0x1e,
+		.xtal = 16000000,
+		.if_khz = 4500,
+		.if_iq_mode = FE_TER_NORMAL_IF_TUNER,
+		.ts_mode = STV0367_SERIAL_PUNCT_CLOCK,
+		.clk_pol = STV0367_CLOCKPOLARITY_DEFAULT,
+	},
+};
+
+static struct tda18212_config tda18212_conf = {
+	.if_dvbt_6 = 4150,
+	.if_dvbt_7 = 4150,
+	.if_dvbt_8 = 4500,
+	.if_dvbc = 5000,
+};
+
+int c8sectpfe_frontend_attach(struct dvb_frontend **fe,
+		struct c8sectpfe *c8sectpfe,
+		struct channel_info *tsin, int chan_num)
+{
+	struct tda18212_config *tda18212;
+	struct stv6110x_devctl *fe2;
+	struct i2c_client *client;
+	struct i2c_board_info tda18212_info = {
+		.type = "tda18212",
+		.addr = 0x60,
+	};
+
+	if (!tsin)
+		return -EINVAL;
+
+	switch (tsin->dvb_card) {
+
+	case STV0367_TDA18212_NIMA_1:
+	case STV0367_TDA18212_NIMA_2:
+	case STV0367_TDA18212_NIMB_1:
+	case STV0367_TDA18212_NIMB_2:
+		if (tsin->dvb_card == STV0367_TDA18212_NIMA_1)
+			*fe = dvb_attach(stv0367ter_attach,
+				 &stv0367_tda18212_config[0],
+					tsin->i2c_adapter);
+		else if (tsin->dvb_card == STV0367_TDA18212_NIMB_1)
+			*fe = dvb_attach(stv0367ter_attach,
+				 &stv0367_tda18212_config[1],
+					tsin->i2c_adapter);
+		else
+			*fe = dvb_attach(stv0367ter_attach,
+				 &stv0367_tda18212_config[2],
+					tsin->i2c_adapter);
+
+		if (!*fe) {
+			dev_err(c8sectpfe->device,
+				"%s: stv0367ter_attach failed for NIM card %s\n"
+				, __func__, dvb_card_str(tsin->dvb_card));
+			return -ENODEV;
+		};
+
+		/*
+		 * init the demod so that i2c gate_ctrl
+		 * to the tuner works correctly
+		 */
+		(*fe)->ops.init(*fe);
+
+		/* Allocate the tda18212 structure */
+		tda18212 = devm_kzalloc(c8sectpfe->device,
+					sizeof(struct tda18212_config),
+					GFP_KERNEL);
+		if (!tda18212) {
+			dev_err(c8sectpfe->device,
+				"%s: devm_kzalloc failed\n", __func__);
+			return -ENOMEM;
+		}
+
+		memcpy(tda18212, &tda18212_conf,
+			sizeof(struct tda18212_config));
+
+		tda18212->fe = (*fe);
+
+		tda18212_info.platform_data = tda18212;
+
+		/* attach tuner */
+		request_module("tda18212");
+		client = i2c_new_device(tsin->i2c_adapter, &tda18212_info);
+		if (!client || !client->dev.driver) {
+			dvb_frontend_detach(*fe);
+			return -ENODEV;
+		}
+
+		if (!try_module_get(client->dev.driver->owner)) {
+			i2c_unregister_device(client);
+			dvb_frontend_detach(*fe);
+			return -ENODEV;
+		}
+
+		tsin->i2c_client = client;
+
+		break;
+
+	case STV0903_6110_LNB24_NIMA:
+		*fe = dvb_attach(stv090x_attach,	&stv090x_config,
+				tsin->i2c_adapter, STV090x_DEMODULATOR_0);
+		if (!*fe) {
+			dev_err(c8sectpfe->device, "%s: stv090x_attach failed\n"
+				"\tfor NIM card %s\n",
+				__func__, dvb_card_str(tsin->dvb_card));
+			return -ENODEV;
+		}
+
+		fe2 = dvb_attach(stv6110x_attach, *fe,
+					&stv6110x_config, tsin->i2c_adapter);
+		if (!fe2) {
+			dev_err(c8sectpfe->device,
+				"%s: stv6110x_attach failed for NIM card %s\n"
+				, __func__, dvb_card_str(tsin->dvb_card));
+			return -ENODEV;
+		};
+
+		stv090x_config.tuner_init = fe2->tuner_init;
+		stv090x_config.tuner_set_mode = fe2->tuner_set_mode;
+		stv090x_config.tuner_set_frequency = fe2->tuner_set_frequency;
+		stv090x_config.tuner_get_frequency = fe2->tuner_get_frequency;
+		stv090x_config.tuner_set_bandwidth = fe2->tuner_set_bandwidth;
+		stv090x_config.tuner_get_bandwidth = fe2->tuner_get_bandwidth;
+		stv090x_config.tuner_set_bbgain = fe2->tuner_set_bbgain;
+		stv090x_config.tuner_get_bbgain = fe2->tuner_get_bbgain;
+		stv090x_config.tuner_set_refclk = fe2->tuner_set_refclk;
+		stv090x_config.tuner_get_status = fe2->tuner_get_status;
+
+		dvb_attach(lnbh24_attach, *fe, tsin->i2c_adapter, 0, 0, 0x9);
+		break;
+
+	default:
+		dev_err(c8sectpfe->device,
+			"%s: DVB frontend card %s not yet supported\n",
+			__func__, dvb_card_str(tsin->dvb_card));
+		return -ENODEV;
+	}
+
+	(*fe)->id = chan_num;
+
+	dev_info(c8sectpfe->device,
+			"DVB frontend card %s successfully attached",
+			dvb_card_str(tsin->dvb_card));
+	return 0;
+}
diff --git a/kernel/drivers/media/platform/sti/c8sectpfe/c8sectpfe-dvb.h b/kernel/drivers/media/platform/sti/c8sectpfe/c8sectpfe-dvb.h
new file mode 100644
index 000000000..bd366dbc8
--- /dev/null
+++ b/kernel/drivers/media/platform/sti/c8sectpfe/c8sectpfe-dvb.h
@@ -0,0 +1,20 @@
+/*
+ * c8sectpfe-common.h - C8SECTPFE STi DVB driver
+ *
+ * Copyright (c) STMicroelectronics 2015
+ *
+ *   Author: Peter Griffin <peter.griffin@linaro.org>
+ *
+ *      This program is free software; you can redistribute it and/or
+ *      modify it under the terms of the GNU General Public License as
+ *      published by the Free Software Foundation; either version 2 of
+ *      the License, or (at your option) any later version.
+ */
+#ifndef _C8SECTPFE_DVB_H_
+#define _C8SECTPFE_DVB_H_
+
+int c8sectpfe_frontend_attach(struct dvb_frontend **fe,
+			struct c8sectpfe *c8sectpfe, struct channel_info *tsin,
+			int chan_num);
+
+#endif