video: tegra: host: Implement MPE context switch

Implement context switching for MPE. This allows doing multiple video
encodings at the same time.

Context switching relies on wait base being in sync with sync point. As
MPE user land does not use wait bases, the patch also enables automatic
wait base syncing.

This patch does not enable the context save/restore.

Bug 827192

Change-Id: I510c02fb6d02ffbc1b9537d33474d46022b6cf59
Signed-off-by: Terje Bergstrom <tbergstrom@nvidia.com>
Reviewed-on: http://git-master/r/66881
Reviewed-by: Rohan Somvanshi <rsomvanshi@nvidia.com>
Tested-by: Rohan Somvanshi <rsomvanshi@nvidia.com>

8 files changed, 674 insertions, 15 deletions

diff --git a/drivers/video/tegra/host/nvhost_channel.h b/drivers/video/tegra/host/nvhost_channel.h
index f48fff615401..b21e3bf035a2 100644
--- a/drivers/video/tegra/host/nvhost_channel.h
+++ b/drivers/video/tegra/host/nvhost_channel.h
@@ -74,16 +74,6 @@ struct nvhost_channel {
 	struct nvhost_cdma cdma;
 };
 
-struct nvhost_op_pair {
-	u32 op1;
-	u32 op2;
-};
-
-struct nvhost_cpuinterrupt {
-	u32 syncpt_val;
-	void *intr_data;
-};
-
 int nvhost_channel_init(
 	struct nvhost_channel *ch,
 	struct nvhost_master *dev, int index);
diff --git a/drivers/video/tegra/host/t20/Makefile b/drivers/video/tegra/host/t20/Makefile
index a17adf71f7d2..7903e1b96de3 100644
--- a/drivers/video/tegra/host/t20/Makefile
+++ b/drivers/video/tegra/host/t20/Makefile
@@ -8,6 +8,7 @@ nvhost-t20-objs  = \
 	intr_t20.o \
 	cdma_t20.o \
 	3dctx_t20.o \
+	mpectx_t20.o \
 	debug_t20.o
 
 obj-$(CONFIG_TEGRA_GRHOST) += nvhost-t20.o
diff --git a/drivers/video/tegra/host/t20/channel_t20.c b/drivers/video/tegra/host/t20/channel_t20.c
index 70fb0289b62e..f78210c7596a 100644
--- a/drivers/video/tegra/host/t20/channel_t20.c
+++ b/drivers/video/tegra/host/t20/channel_t20.c
@@ -32,6 +32,7 @@
 #include "../dev.h"
 #include "3dctx_t20.h"
 #include "../3dctx_common.h"
+#include "mpectx_t20.h"
 
 #define NVHOST_NUMCHANNELS (NV_HOST1X_CHANNELS - 1)
 #define NVHOST_CHANNEL_BASE 0
@@ -120,9 +121,11 @@ const struct nvhost_channeldesc nvhost_t20_channelmap[] = {
 			 BIT(NVSYNCPT_MPE_WR_SAFE),
 	.waitbases     = BIT(NVWAITBASE_MPE),
 	.class	       = NV_VIDEO_ENCODE_MPEG_CLASS_ID,
+	.waitbasesync  = true,
 	.exclusive     = true,
 	.keepalive     = true,
 	.module        = {
+			.prepare_poweroff = nvhost_mpectx_prepare_power_off,
 			.clocks = {{"mpe", UINT_MAX}, {"emc", UINT_MAX}, {} },
 			.powergate_ids = {TEGRA_POWERGATE_MPE, -1},
 			NVHOST_DEFAULT_CLOCKGATE_DELAY,
@@ -153,7 +156,8 @@ static inline int t20_nvhost_hwctx_handler_init(
 {
 	if (strcmp(module, "gr3d") == 0)
 		return t20_nvhost_3dctx_handler_init(h);
-
+	else if (strcmp(module, "mpe") == 0)
+		return t20_nvhost_mpectx_handler_init(h);
 	return 0;
 }
 
@@ -564,6 +568,7 @@ done:
 	return err;
 }
 
+
 int nvhost_init_t20_channel_support(struct nvhost_master *host)
 {
 
diff --git a/drivers/video/tegra/host/t20/mpectx_t20.c b/drivers/video/tegra/host/t20/mpectx_t20.c
new file mode 100644
index 000000000000..21dc579211c0
--- /dev/null
+++ b/drivers/video/tegra/host/t20/mpectx_t20.c
@@ -0,0 +1,572 @@
+/*
+ * drivers/video/tegra/host/nvhost_mpectx.c
+ *
+ * Tegra Graphics Host MPE HW Context
+ *
+ * Copyright (c) 2010-2011, NVIDIA Corporation.
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include "../nvhost_hwctx.h"
+#include "../dev.h"
+#include "hardware_t20.h"
+#include "syncpt_t20.h"
+#include "t20.h"
+#include <linux/slab.h>
+
+enum {
+	HWCTX_REGINFO_NORMAL = 0,
+	HWCTX_REGINFO_STASH,
+	HWCTX_REGINFO_CALCULATE,
+	HWCTX_REGINFO_WRITEBACK
+};
+
+const struct hwctx_reginfo ctxsave_regs_mpe[] = {
+	HWCTX_REGINFO(0x124,  1, STASH),
+	HWCTX_REGINFO(0x123,  1, STASH),
+	HWCTX_REGINFO(0x103,  1, STASH),
+	HWCTX_REGINFO(0x074,  1, STASH),
+	HWCTX_REGINFO(0x021,  1, NORMAL),
+	HWCTX_REGINFO(0x020,  1, STASH),
+	HWCTX_REGINFO(0x024,  2, NORMAL),
+	HWCTX_REGINFO(0x0e6,  1, NORMAL),
+	HWCTX_REGINFO(0x3fc,  1, NORMAL),
+	HWCTX_REGINFO(0x3d0,  1, NORMAL),
+	HWCTX_REGINFO(0x3d4,  1, NORMAL),
+	HWCTX_REGINFO(0x013,  1, NORMAL),
+	HWCTX_REGINFO(0x022,  1, NORMAL),
+	HWCTX_REGINFO(0x030,  4, NORMAL),
+	HWCTX_REGINFO(0x023,  1, NORMAL),
+	HWCTX_REGINFO(0x070,  1, NORMAL),
+	HWCTX_REGINFO(0x0a0,  9, NORMAL),
+	HWCTX_REGINFO(0x071,  1, NORMAL),
+	HWCTX_REGINFO(0x100,  4, NORMAL),
+	HWCTX_REGINFO(0x104,  2, NORMAL),
+	HWCTX_REGINFO(0x108,  9, NORMAL),
+	HWCTX_REGINFO(0x112,  2, NORMAL),
+	HWCTX_REGINFO(0x114,  1, STASH),
+	HWCTX_REGINFO(0x014,  1, NORMAL),
+	HWCTX_REGINFO(0x072,  1, NORMAL),
+	HWCTX_REGINFO(0x200,  1, NORMAL),
+	HWCTX_REGINFO(0x0d1,  1, NORMAL),
+	HWCTX_REGINFO(0x0d0,  1, NORMAL),
+	HWCTX_REGINFO(0x0c0,  1, NORMAL),
+	HWCTX_REGINFO(0x0c3,  2, NORMAL),
+	HWCTX_REGINFO(0x0d2,  1, NORMAL),
+	HWCTX_REGINFO(0x0d8,  1, NORMAL),
+	HWCTX_REGINFO(0x0e0,  2, NORMAL),
+	HWCTX_REGINFO(0x07f,  2, NORMAL),
+	HWCTX_REGINFO(0x084,  8, NORMAL),
+	HWCTX_REGINFO(0x0d3,  1, NORMAL),
+	HWCTX_REGINFO(0x040, 13, NORMAL),
+	HWCTX_REGINFO(0x050,  6, NORMAL),
+	HWCTX_REGINFO(0x058,  1, NORMAL),
+	HWCTX_REGINFO(0x057,  1, NORMAL),
+	HWCTX_REGINFO(0x111,  1, NORMAL),
+	HWCTX_REGINFO(0x130,  3, NORMAL),
+	HWCTX_REGINFO(0x201,  1, NORMAL),
+	HWCTX_REGINFO(0x068,  2, NORMAL),
+	HWCTX_REGINFO(0x08c,  1, NORMAL),
+	HWCTX_REGINFO(0x0cf,  1, NORMAL),
+	HWCTX_REGINFO(0x082,  2, NORMAL),
+	HWCTX_REGINFO(0x075,  1, NORMAL),
+	HWCTX_REGINFO(0x0e8,  1, NORMAL),
+	HWCTX_REGINFO(0x056,  1, NORMAL),
+	HWCTX_REGINFO(0x057,  1, NORMAL),
+	HWCTX_REGINFO(0x073,  1, CALCULATE),
+	HWCTX_REGINFO(0x074,  1, NORMAL),
+	HWCTX_REGINFO(0x075,  1, NORMAL),
+	HWCTX_REGINFO(0x076,  1, STASH),
+	HWCTX_REGINFO(0x11a,  9, NORMAL),
+	HWCTX_REGINFO(0x123,  1, NORMAL),
+	HWCTX_REGINFO(0x124,  1, NORMAL),
+	HWCTX_REGINFO(0x12a,  5, NORMAL),
+	HWCTX_REGINFO(0x12f,  1, STASH),
+	HWCTX_REGINFO(0x125,  2, NORMAL),
+	HWCTX_REGINFO(0x034,  1, NORMAL),
+	HWCTX_REGINFO(0x133,  2, NORMAL),
+	HWCTX_REGINFO(0x127,  1, NORMAL),
+	HWCTX_REGINFO(0x106,  1, WRITEBACK),
+	HWCTX_REGINFO(0x107,  1, WRITEBACK)
+};
+
+#define NR_STASHES 8
+#define NR_WRITEBACKS 2
+
+#define RC_RAM_LOAD_CMD 0x115
+#define RC_RAM_LOAD_DATA 0x116
+#define RC_RAM_READ_CMD 0x128
+#define RC_RAM_READ_DATA 0x129
+#define RC_RAM_SIZE 692
+
+#define IRFR_RAM_LOAD_CMD 0xc5
+#define IRFR_RAM_LOAD_DATA 0xc6
+#define IRFR_RAM_READ_CMD 0xcd
+#define IRFR_RAM_READ_DATA 0xce
+#define IRFR_RAM_SIZE 408
+
+struct mpe_save_info {
+	u32 in[NR_STASHES];
+	u32 out[NR_WRITEBACKS];
+	unsigned in_pos;
+	unsigned out_pos;
+	u32 h264_mode;
+};
+
+
+/*** restore ***/
+
+static unsigned int restore_size;
+
+static void restore_begin(u32 *ptr)
+{
+	/* set class to host */
+	ptr[0] = nvhost_opcode_setclass(NV_HOST1X_CLASS_ID,
+					NV_CLASS_HOST_INCR_SYNCPT_BASE, 1);
+	/* increment sync point base */
+	ptr[1] = nvhost_class_host_incr_syncpt_base(NVWAITBASE_MPE, 1);
+	/* set class to MPE */
+	ptr[2] = nvhost_opcode_setclass(NV_VIDEO_ENCODE_MPEG_CLASS_ID, 0, 0);
+}
+#define RESTORE_BEGIN_SIZE 3
+
+static void restore_ram(u32 *ptr, unsigned words,
+			unsigned cmd_reg, unsigned data_reg)
+{
+	ptr[0] = nvhost_opcode_imm(cmd_reg, words);
+	ptr[1] = nvhost_opcode_nonincr(data_reg, words);
+}
+#define RESTORE_RAM_SIZE 2
+
+static void restore_end(u32 *ptr)
+{
+	/* syncpt increment to track restore gather. */
+	ptr[0] = nvhost_opcode_imm_incr_syncpt(NV_SYNCPT_OP_DONE,
+			NVSYNCPT_MPE);
+}
+#define RESTORE_END_SIZE 1
+
+static u32 *setup_restore_regs(u32 *ptr,
+			const struct hwctx_reginfo *regs,
+			unsigned int nr_regs)
+{
+	const struct hwctx_reginfo *rend = regs + nr_regs;
+
+	for ( ; regs != rend; ++regs) {
+		u32 offset = regs->offset;
+		u32 count = regs->count;
+		*ptr++ = nvhost_opcode_incr(offset, count);
+		ptr += count;
+	}
+	return ptr;
+}
+
+static u32 *setup_restore_ram(u32 *ptr, unsigned words,
+			unsigned cmd_reg, unsigned data_reg)
+{
+	restore_ram(ptr, words, cmd_reg, data_reg);
+	return ptr + (RESTORE_RAM_SIZE + words);
+}
+
+static void setup_restore(u32 *ptr)
+{
+	restore_begin(ptr);
+	ptr += RESTORE_BEGIN_SIZE;
+
+	ptr = setup_restore_regs(ptr, ctxsave_regs_mpe,
+				ARRAY_SIZE(ctxsave_regs_mpe));
+
+	ptr = setup_restore_ram(ptr, RC_RAM_SIZE,
+			RC_RAM_LOAD_CMD, RC_RAM_LOAD_DATA);
+
+	ptr = setup_restore_ram(ptr, IRFR_RAM_SIZE,
+			IRFR_RAM_LOAD_CMD, IRFR_RAM_LOAD_DATA);
+
+	restore_end(ptr);
+
+	wmb();
+}
+
+
+/*** save ***/
+
+/* the same context save command sequence is used for all contexts. */
+static struct nvmap_handle_ref *save_buf;
+static phys_addr_t save_phys;
+static unsigned int save_size;
+
+struct save_info {
+	u32 *ptr;
+	unsigned int save_count;
+	unsigned int restore_count;
+};
+
+static void __init save_begin(u32 *ptr)
+{
+	/* MPE: when done, increment syncpt to base+1 */
+	ptr[0] = nvhost_opcode_setclass(NV_VIDEO_ENCODE_MPEG_CLASS_ID, 0, 0);
+	ptr[1] = nvhost_opcode_imm_incr_syncpt(NV_SYNCPT_OP_DONE,
+			NVSYNCPT_MPE);
+	/* host: wait for syncpt base+1 */
+	ptr[2] = nvhost_opcode_setclass(NV_HOST1X_CLASS_ID,
+					NV_CLASS_HOST_WAIT_SYNCPT_BASE, 1);
+	ptr[3] = nvhost_class_host_wait_syncpt_base(NVSYNCPT_MPE,
+						NVWAITBASE_MPE, 1);
+	/* host: signal context read thread to start reading */
+	ptr[4] = nvhost_opcode_imm_incr_syncpt(NV_SYNCPT_IMMEDIATE,
+			NVSYNCPT_MPE);
+}
+#define SAVE_BEGIN_SIZE 5
+
+static void __init save_direct(u32 *ptr, u32 start_reg, u32 count)
+{
+	ptr[0] = nvhost_opcode_setclass(NV_HOST1X_CLASS_ID,
+					NV_CLASS_HOST_INDOFF, 1);
+	ptr[1] = nvhost_class_host_indoff_reg_read(NV_HOST_MODULE_MPE,
+						start_reg, true);
+	ptr[2] = nvhost_opcode_nonincr(NV_CLASS_HOST_INDDATA, count);
+}
+#define SAVE_DIRECT_SIZE 3
+
+static void __init save_set_ram_cmd(u32 *ptr, u32 cmd_reg, u32 count)
+{
+	ptr[0] = nvhost_opcode_setclass(NV_VIDEO_ENCODE_MPEG_CLASS_ID,
+					cmd_reg, 1);
+	ptr[1] = count;
+}
+#define SAVE_SET_RAM_CMD_SIZE 2
+
+static void __init save_read_ram_data_nasty(u32 *ptr, u32 data_reg)
+{
+	ptr[0] = nvhost_opcode_setclass(NV_HOST1X_CLASS_ID,
+					NV_CLASS_HOST_INDOFF, 1);
+	ptr[1] = nvhost_class_host_indoff_reg_read(NV_HOST_MODULE_MPE,
+						data_reg, false);
+	ptr[2] = nvhost_opcode_imm(NV_CLASS_HOST_INDDATA, 0);
+	/* write junk data to avoid 'cached problem with register memory' */
+	ptr[3] = nvhost_opcode_setclass(NV_VIDEO_ENCODE_MPEG_CLASS_ID,
+					data_reg, 1);
+	ptr[4] = 0x99;
+}
+#define SAVE_READ_RAM_DATA_NASTY_SIZE 5
+
+static void __init save_end(u32 *ptr)
+{
+	/* Wait for context read service to finish (cpu incr 3) */
+	ptr[0] = nvhost_opcode_setclass(NV_HOST1X_CLASS_ID,
+					NV_CLASS_HOST_WAIT_SYNCPT_BASE, 1);
+	ptr[1] = nvhost_class_host_wait_syncpt_base(NVSYNCPT_MPE,
+						NVWAITBASE_MPE, 3);
+	/* Advance syncpoint base */
+	ptr[2] = nvhost_opcode_nonincr(NV_CLASS_HOST_INCR_SYNCPT_BASE, 1);
+	ptr[3] = nvhost_class_host_incr_syncpt_base(NVWAITBASE_MPE, 3);
+	/* set class back to the unit */
+	ptr[4] = nvhost_opcode_setclass(NV_VIDEO_ENCODE_MPEG_CLASS_ID, 0, 0);
+}
+#define SAVE_END_SIZE 5
+
+static void __init setup_save_regs(struct save_info *info,
+			const struct hwctx_reginfo *regs,
+			unsigned int nr_regs)
+{
+	const struct hwctx_reginfo *rend = regs + nr_regs;
+	u32 *ptr = info->ptr;
+	unsigned int save_count = info->save_count;
+	unsigned int restore_count = info->restore_count;
+
+	for ( ; regs != rend; ++regs) {
+		u32 offset = regs->offset;
+		u32 count = regs->count;
+		if (regs->type != HWCTX_REGINFO_WRITEBACK) {
+			if (ptr) {
+				save_direct(ptr, offset, count);
+				ptr += SAVE_DIRECT_SIZE;
+				memset(ptr, 0, count * 4);
+				ptr += count;
+			}
+			save_count += (SAVE_DIRECT_SIZE + count);
+		}
+		restore_count += (1 + count);
+	}
+
+	info->ptr = ptr;
+	info->save_count = save_count;
+	info->restore_count = restore_count;
+}
+
+static void __init setup_save_ram_nasty(struct save_info *info,	unsigned words,
+					unsigned cmd_reg, unsigned data_reg)
+{
+	u32 *ptr = info->ptr;
+	unsigned int save_count = info->save_count;
+	unsigned int restore_count = info->restore_count;
+	unsigned i;
+
+	if (ptr) {
+		save_set_ram_cmd(ptr, cmd_reg, words);
+		ptr += SAVE_SET_RAM_CMD_SIZE;
+		for (i = words; i; --i) {
+			save_read_ram_data_nasty(ptr, data_reg);
+			ptr += SAVE_READ_RAM_DATA_NASTY_SIZE;
+		}
+	}
+
+	save_count += SAVE_SET_RAM_CMD_SIZE;
+	save_count += words * SAVE_READ_RAM_DATA_NASTY_SIZE;
+	restore_count += (RESTORE_RAM_SIZE + words);
+
+	info->ptr = ptr;
+	info->save_count = save_count;
+	info->restore_count = restore_count;
+}
+
+static void __init setup_save(u32 *ptr)
+{
+	struct save_info info = {
+		ptr,
+		SAVE_BEGIN_SIZE,
+		RESTORE_BEGIN_SIZE
+	};
+
+	if (info.ptr) {
+		save_begin(info.ptr);
+		info.ptr += SAVE_BEGIN_SIZE;
+	}
+
+	setup_save_regs(&info, ctxsave_regs_mpe,
+			ARRAY_SIZE(ctxsave_regs_mpe));
+
+	setup_save_ram_nasty(&info, RC_RAM_SIZE,
+			RC_RAM_READ_CMD, RC_RAM_READ_DATA);
+
+	setup_save_ram_nasty(&info, IRFR_RAM_SIZE,
+			IRFR_RAM_READ_CMD, IRFR_RAM_READ_DATA);
+
+	if (info.ptr) {
+		save_end(info.ptr);
+		info.ptr += SAVE_END_SIZE;
+	}
+
+	wmb();
+
+	save_size = info.save_count + SAVE_END_SIZE;
+	restore_size = info.restore_count + RESTORE_END_SIZE;
+}
+
+
+static u32 calculate_mpe(u32 word, struct mpe_save_info *msi)
+{
+	u32 buffer_full_read = msi->in[0] & 0x01ffffff;
+	u32 byte_len = msi->in[1];
+	u32 drain = (msi->in[2] >> 2) & 0x007fffff;
+	u32 rep_frame = msi->in[3] & 0x0000ffff;
+	u32 h264_mode = (msi->in[4] >> 11) & 1;
+	int new_buffer_full;
+
+	if (h264_mode)
+		byte_len >>= 3;
+	new_buffer_full = buffer_full_read + byte_len - (drain * 4);
+	msi->out[0] = max(0, new_buffer_full);
+	msi->out[1] = rep_frame;
+	if (rep_frame == 0)
+		word &= 0xffff0000;
+	return word;
+}
+
+static u32 *save_regs(u32 *ptr, unsigned int *pending,
+		struct nvhost_channel *channel,
+		const struct hwctx_reginfo *regs,
+		unsigned int nr_regs,
+		struct mpe_save_info *msi)
+{
+	const struct hwctx_reginfo *rend = regs + nr_regs;
+
+	for ( ; regs != rend; ++regs) {
+		u32 count = regs->count;
+		++ptr; /* restore incr */
+		if (regs->type == HWCTX_REGINFO_NORMAL) {
+			nvhost_drain_read_fifo(channel->aperture,
+						ptr, count, pending);
+			ptr += count;
+		} else {
+			u32 word;
+			if (regs->type == HWCTX_REGINFO_WRITEBACK) {
+				BUG_ON(msi->out_pos >= NR_WRITEBACKS);
+				word = msi->out[msi->out_pos++];
+			} else {
+				nvhost_drain_read_fifo(channel->aperture,
+							&word, 1, pending);
+				if (regs->type == HWCTX_REGINFO_STASH) {
+					BUG_ON(msi->in_pos >= NR_STASHES);
+					msi->in[msi->in_pos++] = word;
+				} else {
+					word = calculate_mpe(word, msi);
+				}
+			}
+			*ptr++ = word;
+		}
+	}
+	return ptr;
+}
+
+static u32 *save_ram(u32 *ptr, unsigned int *pending,
+		struct nvhost_channel *channel,
+		unsigned words,	unsigned cmd_reg, unsigned data_reg)
+{
+	int err = 0;
+	ptr += RESTORE_RAM_SIZE;
+	err = nvhost_drain_read_fifo(channel->aperture, ptr, words, pending);
+	WARN_ON(err);
+	return ptr + words;
+}
+
+
+/*** ctxmpe ***/
+
+static struct nvhost_hwctx *ctxmpe_alloc(struct nvhost_channel *ch)
+{
+	struct nvmap_client *nvmap = ch->dev->nvmap;
+	struct nvhost_hwctx *ctx;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return NULL;
+	ctx->restore = nvmap_alloc(nvmap, restore_size * 4, 32,
+				NVMAP_HANDLE_WRITE_COMBINE);
+	if (IS_ERR_OR_NULL(ctx->restore)) {
+		kfree(ctx);
+		return NULL;
+	}
+
+	ctx->restore_virt = nvmap_mmap(ctx->restore);
+	if (!ctx->restore_virt) {
+		nvmap_free(nvmap, ctx->restore);
+		kfree(ctx);
+		return NULL;
+	}
+
+	kref_init(&ctx->ref);
+	ctx->channel = ch;
+	ctx->valid = false;
+	ctx->save = save_buf;
+	ctx->save_incrs = 3;
+	ctx->save_thresh = 2;
+	ctx->restore_phys = nvmap_pin(nvmap, ctx->restore);
+	ctx->restore_size = restore_size;
+	ctx->restore_incrs = 1;
+
+	setup_restore(ctx->restore_virt);
+
+	return ctx;
+}
+
+static void ctxmpe_get(struct nvhost_hwctx *ctx)
+{
+	kref_get(&ctx->ref);
+}
+
+static void ctxmpe_free(struct kref *ref)
+{
+	struct nvhost_hwctx *ctx = container_of(ref, struct nvhost_hwctx, ref);
+	struct nvmap_client *nvmap = ctx->channel->dev->nvmap;
+
+	if (ctx->restore_virt)
+		nvmap_munmap(ctx->restore, ctx->restore_virt);
+	nvmap_unpin(nvmap, ctx->restore);
+	nvmap_free(nvmap, ctx->restore);
+	kfree(ctx);
+}
+
+static void ctxmpe_put(struct nvhost_hwctx *ctx)
+{
+	kref_put(&ctx->ref, ctxmpe_free);
+}
+
+static void ctxmpe_save_push(struct nvhost_cdma *cdma, struct nvhost_hwctx *ctx)
+{
+	nvhost_cdma_push(cdma,
+			nvhost_opcode_gather(save_size),
+			save_phys);
+}
+
+static void ctxmpe_save_service(struct nvhost_hwctx *ctx)
+{
+	u32 *ptr = (u32 *)ctx->restore_virt + RESTORE_BEGIN_SIZE;
+	unsigned int pending = 0;
+	struct mpe_save_info msi;
+
+	msi.in_pos = 0;
+	msi.out_pos = 0;
+
+	ptr = save_regs(ptr, &pending, ctx->channel,
+			ctxsave_regs_mpe, ARRAY_SIZE(ctxsave_regs_mpe), &msi);
+
+	ptr = save_ram(ptr, &pending, ctx->channel,
+		RC_RAM_SIZE, RC_RAM_READ_CMD, RC_RAM_READ_DATA);
+
+	ptr = save_ram(ptr, &pending, ctx->channel,
+		IRFR_RAM_SIZE, IRFR_RAM_READ_CMD, IRFR_RAM_READ_DATA);
+
+	wmb();
+	nvhost_syncpt_cpu_incr(&ctx->channel->dev->syncpt, NVSYNCPT_MPE);
+}
+
+
+/*** nvhost_mpectx ***/
+
+int __init t20_nvhost_mpectx_handler_init(struct nvhost_hwctx_handler *h)
+{
+	struct nvhost_channel *ch;
+	struct nvmap_client *nvmap;
+	u32 *save_ptr;
+
+	ch = container_of(h, struct nvhost_channel, ctxhandler);
+	nvmap = ch->dev->nvmap;
+
+	setup_save(NULL);
+
+	save_buf = nvmap_alloc(nvmap, save_size * 4, 32,
+				NVMAP_HANDLE_WRITE_COMBINE);
+	if (IS_ERR(save_buf)) {
+		int err = PTR_ERR(save_buf);
+		save_buf = NULL;
+		return err;
+	}
+
+	save_ptr = nvmap_mmap(save_buf);
+	if (!save_ptr) {
+		nvmap_free(nvmap, save_buf);
+		save_buf = NULL;
+		return -ENOMEM;
+	}
+
+	save_phys = nvmap_pin(nvmap, save_buf);
+
+	setup_save(save_ptr);
+
+	h->alloc = ctxmpe_alloc;
+	h->save_push = ctxmpe_save_push;
+	h->save_service = ctxmpe_save_service;
+	h->get = ctxmpe_get;
+	h->put = ctxmpe_put;
+
+	return 0;
+}
+
+int nvhost_mpectx_prepare_power_off(struct nvhost_module *mod)
+{
+	return nvhost_t20_save_context(mod, NVSYNCPT_MPE);
+}
diff --git a/drivers/video/tegra/host/nvhost_mpectx.c b/drivers/video/tegra/host/t20/mpectx_t20.h
index a5812e7469a3..e5e520e51884 100644
--- a/drivers/video/tegra/host/nvhost_mpectx.c
+++ b/drivers/video/tegra/host/t20/mpectx_t20.h
@@ -1,9 +1,9 @@
 /*
- * drivers/video/tegra/host/nvhost_mpectx.c
+ * drivers/video/tegra/host/t20/mpectx_t20.h
  *
- * Tegra Graphics Host MPE HW Context
+ * Tegra Graphics Host MPE context saving for T20
  *
- * Copyright (c) 2010, NVIDIA Corporation.
+ * Copyright (c) 2011, NVIDIA Corporation.
  *
  * 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
@@ -20,4 +20,12 @@
  * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  */
 
-/* Placeholder */
+#ifndef __NVHOST_MPECTX_T20_H
+#define __NVHOST_MPECTX_T20_H
+
+struct nvhost_hwctx_handler;
+
+int t20_nvhost_mpectx_handler_init(struct nvhost_hwctx_handler *h);
+int nvhost_mpectx_prepare_power_off(struct nvhost_module *mod);
+
+#endif
diff --git a/drivers/video/tegra/host/t20/t20.c b/drivers/video/tegra/host/t20/t20.c
index c846ce7cf289..a663f4598f02 100644
--- a/drivers/video/tegra/host/t20/t20.c
+++ b/drivers/video/tegra/host/t20/t20.c
@@ -20,6 +20,7 @@
  * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  */
 
+#include <linux/slab.h>
 #include "../dev.h"
 
 #include "t20.h"
@@ -49,3 +50,79 @@ int nvhost_init_t20_support(struct nvhost_master *host)
 		return err;
 	return 0;
 }
+
+int nvhost_t20_save_context(struct nvhost_module *mod, u32 syncpt_id)
+{
+	struct nvhost_channel *ch =
+			container_of(mod, struct nvhost_channel, mod);
+	struct nvhost_hwctx *hwctx_to_save;
+	DECLARE_WAIT_QUEUE_HEAD_ONSTACK(wq);
+	u32 syncpt_incrs, syncpt_val;
+	int err = 0;
+	void *ref;
+	void *ctx_waiter = NULL, *wakeup_waiter = NULL;
+
+	ctx_waiter = nvhost_intr_alloc_waiter();
+	wakeup_waiter = nvhost_intr_alloc_waiter();
+	if (!ctx_waiter || !wakeup_waiter) {
+		err = -ENOMEM;
+		goto done;
+	}
+
+	if (mod->desc->busy)
+		mod->desc->busy(mod);
+
+	mutex_lock(&ch->submitlock);
+	hwctx_to_save = ch->cur_ctx;
+	if (!hwctx_to_save) {
+		mutex_unlock(&ch->submitlock);
+		goto done;
+	}
+
+	err = nvhost_cdma_begin(&ch->cdma, hwctx_to_save->timeout);
+	if (err) {
+		mutex_unlock(&ch->submitlock);
+		goto done;
+	}
+
+	hwctx_to_save->valid = true;
+	ch->ctxhandler.get(hwctx_to_save);
+	ch->cur_ctx = NULL;
+
+	syncpt_incrs = hwctx_to_save->save_incrs;
+	syncpt_val = nvhost_syncpt_incr_max(&ch->dev->syncpt,
+					syncpt_id, syncpt_incrs);
+
+	ch->ctxhandler.save_push(&ch->cdma, hwctx_to_save);
+	nvhost_cdma_end(&ch->cdma, ch->dev->nvmap, syncpt_id, syncpt_val,
+			NULL, 0, hwctx_to_save->timeout);
+
+	err = nvhost_intr_add_action(&ch->dev->intr, syncpt_id,
+			syncpt_val - syncpt_incrs + hwctx_to_save->save_thresh,
+			NVHOST_INTR_ACTION_CTXSAVE, hwctx_to_save,
+			ctx_waiter,
+			NULL);
+	ctx_waiter = NULL;
+	WARN(err, "Failed to set context save interrupt");
+
+	err = nvhost_intr_add_action(&ch->dev->intr, syncpt_id, syncpt_val,
+			NVHOST_INTR_ACTION_WAKEUP, &wq,
+			wakeup_waiter,
+			&ref);
+	wakeup_waiter = NULL;
+	WARN(err, "Failed to set wakeup interrupt");
+	wait_event(wq,
+		nvhost_syncpt_min_cmp(&ch->dev->syncpt,
+				syncpt_id, syncpt_val));
+
+	nvhost_intr_put_ref(&ch->dev->intr, ref);
+
+	nvhost_cdma_update(&ch->cdma);
+
+	mutex_unlock(&ch->submitlock);
+
+done:
+	kfree(ctx_waiter);
+	kfree(wakeup_waiter);
+	return err;
+}
diff --git a/drivers/video/tegra/host/t20/t20.h b/drivers/video/tegra/host/t20/t20.h
index 406841092f01..528718cc9c1d 100644
--- a/drivers/video/tegra/host/t20/t20.h
+++ b/drivers/video/tegra/host/t20/t20.h
@@ -30,5 +30,6 @@ int nvhost_init_t20_debug_support(struct nvhost_master *);
 int nvhost_init_t20_syncpt_support(struct nvhost_master *);
 int nvhost_init_t20_intr_support(struct nvhost_master *);
 int nvhost_init_t20_cpuaccess_support(struct nvhost_master *);
+int nvhost_t20_save_context(struct nvhost_module *mod, u32 syncpt_id);
 
 #endif /* _NVHOST_T20_H_ */
diff --git a/drivers/video/tegra/host/t30/channel_t30.c b/drivers/video/tegra/host/t30/channel_t30.c
index 1c529d396457..ef346181c0c7 100644
--- a/drivers/video/tegra/host/t30/channel_t30.c
+++ b/drivers/video/tegra/host/t30/channel_t30.c
@@ -29,6 +29,7 @@
 #include "../t20/syncpt_t20.h"
 #include "../3dctx_common.h"
 #include "3dctx_t30.h"
+#include "../t20/mpectx_t20.h"
 #include "scale3d.h"
 
 #define NVMODMUTEX_2D_FULL   (1)
@@ -130,9 +131,11 @@ const struct nvhost_channeldesc nvhost_t30_channelmap[] = {
 			 BIT(NVSYNCPT_MPE_WR_SAFE),
 	.waitbases     = BIT(NVWAITBASE_MPE),
 	.class	       = NV_VIDEO_ENCODE_MPEG_CLASS_ID,
+	.waitbasesync  = true,
 	.exclusive     = true,
 	.keepalive     = true,
 	.module        = {
+			.prepare_poweroff = nvhost_mpectx_prepare_power_off,
 			.clocks = {{"mpe", UINT_MAX}, {"emc", UINT_MAX}, {} },
 			.powergate_ids  = {TEGRA_POWERGATE_MPE, -1},
 			NVHOST_DEFAULT_CLOCKGATE_DELAY,
@@ -159,6 +162,8 @@ static inline int t30_nvhost_hwctx_handler_init(
 {
 	if (strcmp(module, "gr3d") == 0)
 		return t30_nvhost_3dctx_handler_init(h);
+	else if (strcmp(module, "mpe") == 0)
+		return t20_nvhost_mpectx_handler_init(h);
 
 	return 0;
 }