applied patches from i9305 jb sources, updated mali to r3p0

Change-Id: Iec4bc4e2fb59e2cf5b4d25568a644d4e3719565e

14 files changed, 886 insertions, 6 deletions

diff --git a/arch/arm/plat-s5p/Kconfig b/arch/arm/plat-s5p/Kconfig
index 3398d3b..f1eb177 100644
--- a/arch/arm/plat-s5p/Kconfig
+++ b/arch/arm/plat-s5p/Kconfig
@@ -147,7 +147,7 @@ config S5P_DEV_DP
 	  Compile in platform device definitions for DP controller
 
 config S5P_DEV_TVOUT
-	bool
+	bool "enable S5P_DEV_TVOUT"
 	depends on VIDEO_TVOUT
 	default y
 	help
diff --git a/arch/arm/plat-s5p/dev-csis-s5p.c b/arch/arm/plat-s5p/dev-csis-s5p.c
index f3a12a7..295542d 100644
--- a/arch/arm/plat-s5p/dev-csis-s5p.c
+++ b/arch/arm/plat-s5p/dev-csis-s5p.c
@@ -43,12 +43,22 @@ static struct s3c_platform_csis default_csis0_data __initdata = {
 	.clk_rate	= 166000000,
 };
 
+int fimc_clk_rate(void)
+{
+	if (samsung_rev() >= EXYNOS4412_REV_2_0)
+		return 180000000;
+	else
+		return 166750000;
+}
+
 void __init s3c_csis0_set_platdata(struct s3c_platform_csis *pd)
 {
 	struct s3c_platform_csis *npd;
 
-	if (!pd)
+	if (!pd) {
+		default_csis0_data.clk_rate = fimc_clk_rate();
 		pd = &default_csis0_data;
+	}
 
 	npd = kmemdup(pd, sizeof(struct s3c_platform_csis), GFP_KERNEL);
 	if (!npd) {
@@ -94,8 +104,10 @@ void __init s3c_csis1_set_platdata(struct s3c_platform_csis *pd)
 {
 	struct s3c_platform_csis *npd;
 
-	if (!pd)
+	if (!pd) {
+		default_csis1_data.clk_rate = fimc_clk_rate();
 		pd = &default_csis1_data;
+	}
 
 	npd = kmemdup(pd, sizeof(struct s3c_platform_csis), GFP_KERNEL);
 	if (!npd) {
diff --git a/arch/arm/plat-s5p/dev-csis0.c b/arch/arm/plat-s5p/dev-csis0.c
index a071c7b..69a7468 100644
--- a/arch/arm/plat-s5p/dev-csis0.c
+++ b/arch/arm/plat-s5p/dev-csis0.c
@@ -35,7 +35,7 @@ struct platform_device s5p_device_mipi_csis0 = {
 };
 
 struct s5p_platform_mipi_csis s5p_mipi_csis0_default_data __initdata = {
-	.clk_rate 	= 166000000,
+	.clk_rate	= 166000000,
 	.lanes		= 2,
 	.alignment	= 32,
 	.hs_settle	= 12,
diff --git a/arch/arm/plat-s5p/dev-csis1.c b/arch/arm/plat-s5p/dev-csis1.c
index 5cf9efa..41a40c8 100644
--- a/arch/arm/plat-s5p/dev-csis1.c
+++ b/arch/arm/plat-s5p/dev-csis1.c
@@ -35,7 +35,7 @@ struct platform_device s5p_device_mipi_csis1 = {
 };
 
 struct s5p_platform_mipi_csis s5p_mipi_csis1_default_data __initdata = {
-	.clk_rate 	= 166000000,
+	.clk_rate	= 166000000,
 	.lanes		= 2,
 	.alignment	= 32,
 	.hs_settle	= 12,
diff --git a/arch/arm/plat-s5p/dev-fimc-s5p.c b/arch/arm/plat-s5p/dev-fimc-s5p.c
index 53a256d..617861d 100644
--- a/arch/arm/plat-s5p/dev-fimc-s5p.c
+++ b/arch/arm/plat-s5p/dev-fimc-s5p.c
@@ -18,6 +18,11 @@
 #include <plat/devs.h>
 #include <plat/cpu.h>
 #include <plat/fimc.h>
+#ifdef CONFIG_USE_FIMC_CMA
+#include <linux/dma-mapping.h>
+
+static u64 s3c_fimc_dmamask = DMA_BIT_MASK(32);
+#endif
 
 static struct resource s3c_fimc0_resource[] = {
 	[0] = {
@@ -86,6 +91,12 @@ static struct resource s3c_fimc1_resource[] = {
 struct platform_device s3c_device_fimc1 = {
 	.name		= "s3c-fimc",
 	.id		= 1,
+#ifdef CONFIG_USE_FIMC_CMA
+	.dev		= {
+		.dma_mask		= &s3c_fimc_dmamask,
+		.coherent_dma_mask	= DMA_BIT_MASK(32),
+	},
+#endif
 	.num_resources	= ARRAY_SIZE(s3c_fimc1_resource),
 	.resource	= s3c_fimc1_resource,
 };
diff --git a/arch/arm/plat-s5p/dev-mfc.c b/arch/arm/plat-s5p/dev-mfc.c
index e6b9483..216ca3c 100644
--- a/arch/arm/plat-s5p/dev-mfc.c
+++ b/arch/arm/plat-s5p/dev-mfc.c
@@ -29,10 +29,20 @@ static struct resource s5p_mfc_resource[] = {
 	},
 };
 
+#if defined(CONFIG_DMA_CMA) && defined(CONFIG_USE_MFC_CMA)
+static u64 s5p_mfc_dma_mask = DMA_BIT_MASK(32);
+#endif
+
 struct platform_device s5p_device_mfc = {
 	.name		= "s3c-mfc",
 	.id		= -1,
 	.num_resources	= ARRAY_SIZE(s5p_mfc_resource),
 	.resource	= s5p_mfc_resource,
+#if defined(CONFIG_DMA_CMA) && defined(CONFIG_USE_MFC_CMA)
+	.dev		= {
+		.dma_mask	= &s5p_mfc_dma_mask,
+		.coherent_dma_mask = DMA_BIT_MASK(32),
+	},
+#endif
 };
 
diff --git a/arch/arm/plat-s5p/dev-tvout.c b/arch/arm/plat-s5p/dev-tvout.c
index dd057dd..a2f6d54 100644
--- a/arch/arm/plat-s5p/dev-tvout.c
+++ b/arch/arm/plat-s5p/dev-tvout.c
@@ -93,6 +93,20 @@ struct platform_device s5p_device_hpd = {
 };
 EXPORT_SYMBOL(s5p_device_hpd);
 
+#ifdef CONFIG_HDMI_TX_STRENGTH
+void __init s5p_hdmi_tvout_set_platdata(struct s5p_platform_tvout *pd)
+{
+	struct s5p_platform_tvout *npd;
+
+	npd = kmemdup(pd, sizeof(struct s5p_platform_tvout), GFP_KERNEL);
+	if (!npd)
+		printk(KERN_ERR "%s: no memory for platform data\n", __func__);
+	else {
+		s5p_device_tvout.dev.platform_data = npd;
+	}
+}
+#endif
+
 void __init s5p_hdmi_hpd_set_platdata(struct s5p_platform_hpd *pd)
 {
 	struct s5p_platform_hpd *npd;
diff --git a/arch/arm/plat-s5p/dev-usbgadget.c b/arch/arm/plat-s5p/dev-usbgadget.c
index 3890ce3..5dd3eb2 100644
--- a/arch/arm/plat-s5p/dev-usbgadget.c
+++ b/arch/arm/plat-s5p/dev-usbgadget.c
@@ -168,6 +168,14 @@ static struct android_usb_product usb_products[] = {
 		.functions	= usb_functions_mtp,
 	},
 #endif
+	{
+/* debug mode : using MS Composite*/
+#ifdef CONFIG_USB_ANDROID_SAMSUNG_DEBUG_MTP
+		.product_id     = SAMSUNG_KIES_PRODUCT_ID,
+		.num_functions  = ARRAY_SIZE(usb_functions_mtp_acm_adb),
+		.functions      = usb_functions_mtp_acm_adb,
+#endif
+	},
 #if	defined(CONFIG_USB_ANDROID_MASS_STORAGE) && defined(CONFIG_USB_ANDROID_ADB)
 	{
 		.product_id	= SAMSUNG_UMS_PRODUCT_ID,
diff --git a/arch/arm/plat-s5p/reserve_mem.c b/arch/arm/plat-s5p/reserve_mem.c
index 5b91823..a103ec9 100644
--- a/arch/arm/plat-s5p/reserve_mem.c
+++ b/arch/arm/plat-s5p/reserve_mem.c
@@ -155,14 +155,49 @@ void __init s5p_cma_region_reserve(struct cma_region *regions_normal,
 		}
 
 		if (paddr_last) {
+#ifndef CONFIG_DMA_CMA
 			while (memblock_reserve(paddr_last, size_secure))
 				paddr_last -= align_secure;
+#else
+			if (!reg->start) {
+				while (memblock_reserve(paddr_last,
+							size_secure))
+					paddr_last -= align_secure;
+			}
+#endif
 
 			do {
+#ifndef CONFIG_DMA_CMA
 				reg->start = paddr_last;
 				reg->reserved = 1;
 				paddr_last += reg->size;
-
+#else
+				if (reg->start) {
+					reg->reserved = 1;
+#if defined(CONFIG_USE_MFC_CMA) && defined(CONFIG_MACH_M0)
+					if (reg->start == 0x5C100000) {
+						if (memblock_reserve(0x5C100000,
+								0x700000))
+							panic("memblock\n");
+						if (memblock_reserve(0x5F000000,
+								0x200000))
+							panic("memblock\n");
+					} else {
+						if (memblock_reserve(reg->start,
+								reg->size))
+							panic("memblock\n");
+					}
+#else
+					if (memblock_reserve(reg->start,
+								reg->size))
+						panic("memblock\n");
+#endif
+				} else {
+					reg->start = paddr_last;
+					reg->reserved = 1;
+					paddr_last += reg->size;
+				}
+#endif
 				pr_info("S5P/CMA: "
 					"Reserved 0x%08x/0x%08x for '%s'\n",
 					reg->start, reg->size, reg->name);
diff --git a/arch/arm/plat-s5p/reset.c b/arch/arm/plat-s5p/reset.c
index 96dfdab..691002d 100644
--- a/arch/arm/plat-s5p/reset.c
+++ b/arch/arm/plat-s5p/reset.c
@@ -15,6 +15,18 @@
 #include <plat/system-reset.h>
 #include <plat/watchdog-reset.h>
 
+#ifdef CONFIG_MACH_P4NOTE
+#include <mach/regs-pmu.h>
+static void exynos_sw_reset(void)
+{
+	int count = 3;
+
+	while (count--) {
+		__raw_writel(0x1, S5P_SWRESET);
+		mdelay(500);
+	}
+}
+#endif
 
 void (*s5p_reset_hook)(void);
 
@@ -24,6 +36,10 @@ void arch_reset(char mode, const char *cmd)
 
 	if (s5p_reset_hook)
 		s5p_reset_hook();
+#ifdef CONFIG_MACH_P4NOTE
+	else
+		exynos_sw_reset();
+#endif
 
 	/* Perform reset using Watchdog reset
 	 * if there is no s5p_reset_hook()
diff --git a/arch/arm/plat-s5p/s5p-sysmmu.c b/arch/arm/plat-s5p/s5p-sysmmu.c
index dae74e1..70edd49 100644
--- a/arch/arm/plat-s5p/s5p-sysmmu.c
+++ b/arch/arm/plat-s5p/s5p-sysmmu.c
@@ -85,6 +85,11 @@ static struct sysmmu_drvdata *get_sysmmu_data(struct device *owner,
 	return NULL;
 }
 
+struct list_head *get_sysmmu_list(void)
+{
+	return &sysmmu_list;
+}
+
 static struct sysmmu_drvdata *get_sysmmu_data_rollback(struct device *owner,
 						struct sysmmu_drvdata *start)
 {
@@ -148,6 +153,12 @@ static void __sysmmu_tlb_invalidate(void __iomem *sfrbase)
 	__raw_writel(0x1, sfrbase + S5P_MMU_FLUSH);
 }
 
+static void __sysmmu_tlb_invalidate_entry(void __iomem *sfrbase,
+						unsigned long iova)
+{
+	__raw_writel((iova & PAGE_MASK) | 1, sfrbase + S5P_MMU_FLUSH_ENTRY);
+}
+
 static void __sysmmu_set_ptbase(void __iomem *sfrbase,
 				       unsigned long pgd)
 {
@@ -322,6 +333,8 @@ void s5p_sysmmu_set_tablebase_pgd(struct device *owner, unsigned long pgd)
 {
 	struct sysmmu_drvdata *mmudata = NULL;
 
+	s5p_sysmmu_tlb_invalidate(owner);
+
 	while ((mmudata = get_sysmmu_data(owner, mmudata))) {
 		unsigned long flags;
 
@@ -467,6 +480,35 @@ void s5p_sysmmu_tlb_invalidate(struct device *owner)
 	}
 }
 
+void s5p_sysmmu_tlb_invalidate_entry(struct device *owner, unsigned long iova,
+					unsigned int count,
+					unsigned long page_size)
+{
+	struct sysmmu_drvdata *mmudata = NULL;
+
+	while ((mmudata = get_sysmmu_data(owner, mmudata))) {
+		unsigned long flags;
+
+		read_lock_irqsave(&mmudata->lock, flags);
+
+		if (is_sysmmu_active(mmudata)) {
+			while (count > 0) {
+				sysmmu_block(mmudata->sfrbase);
+				__sysmmu_tlb_invalidate_entry(mmudata->sfrbase,
+								iova);
+				sysmmu_unblock(mmudata->sfrbase);
+				count--;
+				iova += page_size;
+			}
+		} else {
+			dev_dbg(mmudata->dev,
+				"Disabled: Skipping invalidating TLB.\n");
+		}
+
+		read_unlock_irqrestore(&mmudata->lock, flags);
+	}
+}
+
 static int s5p_sysmmu_probe(struct platform_device *pdev)
 {
 	struct resource *res, *ioarea;
diff --git a/arch/arm/plat-s5p/s5p_iommu.c b/arch/arm/plat-s5p/s5p_iommu.c
index a900c84..32a0de5 100644
--- a/arch/arm/plat-s5p/s5p_iommu.c
+++ b/arch/arm/plat-s5p/s5p_iommu.c
@@ -124,6 +124,56 @@ static void s5p_iommu_domain_destroy(struct iommu_domain *domain)
 	domain->priv = NULL;
 }
 
+#ifdef CONFIG_DRM_EXYNOS_IOMMU
+static int s5p_iommu_attach_device(struct iommu_domain *domain,
+				   struct device *dev)
+{
+	int ret;
+	struct s5p_iommu_domain *s5p_domain = domain->priv;
+	struct sysmmu_drvdata *data = NULL;
+
+	mutex_lock(&s5p_domain->lock);
+
+	/*
+	 * get sysmmu_drvdata to dev.
+	 * owner device was set to sysmmu->platform_data at machine code.
+	 */
+	data = get_sysmmu_data(dev, data);
+	if (!data)
+		return -EFAULT;
+
+	mutex_unlock(&s5p_domain->lock);
+
+	ret = s5p_sysmmu_enable(dev, virt_to_phys(s5p_domain->pgtable));
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static void s5p_iommu_detach_device(struct iommu_domain *domain,
+				    struct device *dev)
+{
+	struct sysmmu_drvdata *data = NULL;
+	struct s5p_iommu_domain *s5p_domain = domain->priv;
+
+	mutex_lock(&s5p_domain->lock);
+
+	/*
+	 * get sysmmu_drvdata to dev.
+	 * owner device was set to sysmmu->platform_data at machine code.
+	 */
+	data = get_sysmmu_data(dev, data);
+	if (!data) {
+		dev_err(dev, "failed to detach device.\n");
+		return;
+	}
+
+	s5p_sysmmu_disable(dev);
+
+	mutex_unlock(&s5p_domain->lock);
+}
+#else
 static int s5p_iommu_attach_device(struct iommu_domain *domain,
 				   struct device *dev)
 {
@@ -168,6 +218,7 @@ static void s5p_iommu_detach_device(struct iommu_domain *domain,
 	}
 
 }
+#endif
 
 static bool section_available(struct iommu_domain *domain,
 			      unsigned long *lv1entry)
@@ -349,6 +400,92 @@ mapping_done:
 	return ret;
 }
 
+#ifdef CONFIG_DRM_EXYNOS_IOMMU
+static int s5p_iommu_unmap(struct iommu_domain *domain, unsigned long iova,
+			   int gfp_order)
+{
+	struct s5p_iommu_domain *s5p_domain = domain->priv;
+	struct sysmmu_drvdata *data;
+	struct list_head *sysmmu_list, *pos;
+	unsigned long *entry;
+	int num_entry;
+
+	BUG_ON(s5p_domain->pgtable == NULL);
+
+	mutex_lock(&s5p_domain->lock);
+
+	entry = s5p_domain->pgtable + (iova >> S5P_SECTION_SHIFT);
+
+	if (gfp_order >= S5P_SECTION_ORDER) {
+		num_entry = 1 << (gfp_order - S5P_SECTION_ORDER);
+		while (num_entry--) {
+			if (S5P_SECTION_LV1_ENTRY(*entry)) {
+				MAKE_FAULT_ENTRY(*entry);
+			} else if (S5P_PAGE_LV1_ENTRY(*entry)) {
+				unsigned long *lv2beg, *lv2end;
+				lv2beg = phys_to_virt(
+						*entry & S5P_LV2TABLE_MASK);
+				lv2end = lv2beg + S5P_LV2TABLE_ENTRIES;
+				while (lv2beg != lv2end) {
+					MAKE_FAULT_ENTRY(*lv2beg);
+					lv2beg++;
+				}
+			}
+			entry++;
+		}
+	} else {
+		entry = GET_LV2ENTRY(*entry, iova);
+
+		BUG_ON(S5P_LPAGE_LV2_ENTRY(*entry) &&
+						(gfp_order < S5P_LPAGE_ORDER));
+
+		num_entry = 1 << gfp_order;
+
+		while (num_entry--) {
+			MAKE_FAULT_ENTRY(*entry);
+			entry++;
+		}
+	}
+
+	sysmmu_list = get_sysmmu_list();
+
+	/*
+	 * invalidate tlb entries to iova(device address) to each iommu
+	 * registered in sysmmu_list.
+	 *
+	 * P.S. a device using iommu was set to data->owner at machine code
+	 * and enabled iommu was added in sysmmu_list at sysmmu probe
+	 */
+	list_for_each(pos, sysmmu_list) {
+		unsigned int page_size, count;
+
+		/*
+		 * get entry count and page size to device address space
+		 * mapped with iommu page table and invalidate each entry.
+		 */
+		if (gfp_order >= S5P_SECTION_ORDER) {
+			count = 1 << (gfp_order - S5P_SECTION_ORDER);
+			page_size = S5P_SECTION_SIZE;
+		} else if (gfp_order >= S5P_LPAGE_ORDER) {
+			count = 1 << (gfp_order - S5P_LPAGE_ORDER);
+			page_size = S5P_LPAGE_SIZE;
+		} else {
+			count = 1 << (gfp_order - S5P_SPAGE_ORDER);
+			page_size = S5P_SPAGE_SIZE;
+		}
+
+		data = list_entry(pos, struct sysmmu_drvdata, node);
+		if (data)
+			s5p_sysmmu_tlb_invalidate_entry(data->owner, iova,
+							count, page_size);
+	}
+
+	mutex_unlock(&s5p_domain->lock);
+
+	return 0;
+}
+#else
+
 static int s5p_iommu_unmap(struct iommu_domain *domain, unsigned long iova,
 			   int gfp_order)
 {
@@ -400,6 +537,7 @@ static int s5p_iommu_unmap(struct iommu_domain *domain, unsigned long iova,
 
 	return 0;
 }
+#endif
 
 static phys_addr_t s5p_iommu_iova_to_phys(struct iommu_domain *domain,
 					  unsigned long iova)
diff --git a/arch/arm/plat-s5p/s5p_iovmm.c b/arch/arm/plat-s5p/s5p_iovmm.c
index a56ccef..c5f366f 100644
--- a/arch/arm/plat-s5p/s5p_iovmm.c
+++ b/arch/arm/plat-s5p/s5p_iovmm.c
@@ -67,6 +67,287 @@ static struct s5p_vm_region *find_region(struct s5p_iovmm *vmm, dma_addr_t iova)
 	return NULL;
 }
 
+#ifdef CONFIG_DRM_EXYNOS_IOMMU
+void *iovmm_setup(unsigned long s_iova, unsigned long size)
+{
+	struct s5p_iovmm *vmm;
+	int ret;
+
+	vmm = kzalloc(sizeof(*vmm), GFP_KERNEL);
+	if (!vmm) {
+		ret = -ENOMEM;
+		goto err_setup_alloc;
+	}
+
+	vmm->vmm_pool = gen_pool_create(PAGE_SHIFT, -1);
+	if (!vmm->vmm_pool) {
+		ret = -ENOMEM;
+		goto err_setup_genalloc;
+	}
+
+	/* device address space starts from s_iova to s_iova + size */
+	ret = gen_pool_add(vmm->vmm_pool, s_iova, size, -1);
+	if (ret)
+		goto err_setup_domain;
+
+	vmm->domain = iommu_domain_alloc();
+	if (!vmm->domain) {
+		ret = -ENOMEM;
+		goto err_setup_domain;
+	}
+
+	mutex_init(&vmm->lock);
+
+	INIT_LIST_HEAD(&vmm->node);
+	INIT_LIST_HEAD(&vmm->regions_list);
+
+	write_lock(&iovmm_list_lock);
+	list_add(&vmm->node, &s5p_iovmm_list);
+	write_unlock(&iovmm_list_lock);
+
+	return vmm;
+err_setup_domain:
+	gen_pool_destroy(vmm->vmm_pool);
+err_setup_genalloc:
+	kfree(vmm);
+err_setup_alloc:
+	return ERR_PTR(ret);
+}
+
+void iovmm_cleanup(void *in_vmm)
+{
+	struct s5p_iovmm *vmm = in_vmm;
+
+	WARN_ON(!vmm);
+
+	if (vmm) {
+		struct list_head *pos, *tmp;
+
+		iommu_domain_free(vmm->domain);
+
+		list_for_each_safe(pos, tmp, &vmm->regions_list) {
+			struct s5p_vm_region *region;
+
+			region = list_entry(pos, struct s5p_vm_region, node);
+
+			/* No need to unmap the region because
+			 * iommu_domain_free() frees the page table */
+			gen_pool_free(vmm->vmm_pool, region->start,
+								region->size);
+
+			kfree(list_entry(pos, struct s5p_vm_region, node));
+		}
+
+		gen_pool_destroy(vmm->vmm_pool);
+
+		write_lock(&iovmm_list_lock);
+		list_del(&vmm->node);
+		write_unlock(&iovmm_list_lock);
+
+		kfree(vmm);
+	}
+}
+
+int iovmm_activate(void *in_vmm, struct device *dev)
+{
+	struct s5p_iovmm *vmm = in_vmm;
+	int ret = 0;
+
+	if (WARN_ON(!vmm))
+		return -EINVAL;
+
+	mutex_lock(&vmm->lock);
+
+	ret = iommu_attach_device(vmm->domain, dev);
+	if (!ret)
+		vmm->active = true;
+
+	mutex_unlock(&vmm->lock);
+
+	return ret;
+}
+
+void iovmm_deactivate(void *in_vmm, struct device *dev)
+{
+	struct s5p_iovmm *vmm = in_vmm;
+
+	if (WARN_ON(!vmm))
+		return;
+
+	iommu_detach_device(vmm->domain, dev);
+
+	vmm->active = false;
+}
+
+dma_addr_t iovmm_map(void *in_vmm, struct scatterlist *sg, off_t offset,
+								size_t size)
+{
+	off_t start_off;
+	dma_addr_t addr, start = 0;
+	size_t mapped_size = 0;
+	struct s5p_vm_region *region;
+	struct s5p_iovmm *vmm = in_vmm;
+	int order;
+#ifdef CONFIG_S5P_SYSTEM_MMU_WA5250ERR
+	size_t iova_size = 0;
+#endif
+
+	BUG_ON(!sg);
+
+	if (WARN_ON(!vmm))
+		goto err_map_nomem;
+
+	for (; sg_dma_len(sg) < offset; sg = sg_next(sg))
+		offset -= sg_dma_len(sg);
+
+	mutex_lock(&vmm->lock);
+
+	start_off = offset_in_page(sg_phys(sg) + offset);
+	size = PAGE_ALIGN(size + start_off);
+
+	order = __fls(min(size, (size_t)SZ_1M));
+#ifdef CONFIG_S5P_SYSTEM_MMU_WA5250ERR
+	iova_size = ALIGN(size, SZ_64K);
+	start = (dma_addr_t)gen_pool_alloc_aligned(vmm->vmm_pool, iova_size,
+									order);
+#else
+	start = (dma_addr_t)gen_pool_alloc_aligned(vmm->vmm_pool, size, order);
+#endif
+	if (!start)
+		goto err_map_nomem_lock;
+
+	addr = start;
+	do {
+		phys_addr_t phys;
+		size_t len;
+
+		phys = sg_phys(sg);
+		len = sg_dma_len(sg);
+
+		if (offset > 0) {
+			len -= offset;
+			phys += offset;
+			offset = 0;
+		}
+
+		if (offset_in_page(phys)) {
+			len += offset_in_page(phys);
+			phys = round_down(phys, PAGE_SIZE);
+		}
+
+		len = PAGE_ALIGN(len);
+
+		if (len > (size - mapped_size))
+			len = size - mapped_size;
+
+		while (len > 0) {
+			order = min3(__ffs(phys), __ffs(addr), __fls(len));
+
+			if (iommu_map(vmm->domain, addr, phys,
+							order - PAGE_SHIFT, 0))
+				goto err_map_map;
+
+			addr += (1 << order);
+			phys += (1 << order);
+			len -= (1 << order);
+			mapped_size += (1 << order);
+		}
+	} while ((sg = sg_next(sg)) && (mapped_size < size));
+
+	BUG_ON(mapped_size > size);
+
+	if (mapped_size < size)
+		goto err_map_map;
+
+#ifdef CONFIG_S5P_SYSTEM_MMU_WA5250ERR
+	if (iova_size != size) {
+		/* System MMU v3 support in SMDK5250 EVT0 */
+		addr = start + size;
+		size = iova_size;
+
+		for (; addr < start + size; addr += PAGE_SIZE) {
+			if (iommu_map(vmm->domain, addr,
+					page_to_phys(ZERO_PAGE(0)), 0, 0)) {
+				goto err_map_map;
+			}
+			mapped_size += PAGE_SIZE;
+		}
+	}
+#endif
+	region = kmalloc(sizeof(*region), GFP_KERNEL);
+	if (!region)
+		goto err_map_map;
+
+	region->start = start + start_off;
+	region->size = size;
+	INIT_LIST_HEAD(&region->node);
+
+	list_add(&region->node, &vmm->regions_list);
+
+	mutex_unlock(&vmm->lock);
+
+	return region->start;
+err_map_map:
+	while (addr >= start) {
+		int order;
+		mapped_size = addr - start;
+
+		if (mapped_size == 0) /* Mapping failed at the first page */
+			mapped_size = size;
+
+		BUG_ON(mapped_size < PAGE_SIZE);
+
+		order = min(__fls(mapped_size), __ffs(start));
+
+		iommu_unmap(vmm->domain, start, order - PAGE_SHIFT);
+
+		start += 1 << order;
+		mapped_size -= 1 << order;
+	}
+	gen_pool_free(vmm->vmm_pool, start, size);
+
+err_map_nomem_lock:
+	mutex_unlock(&vmm->lock);
+err_map_nomem:
+	return (dma_addr_t)0;
+}
+
+void iovmm_unmap(void *in_vmm, dma_addr_t iova)
+{
+	struct s5p_vm_region *region;
+	struct s5p_iovmm *vmm = in_vmm;
+
+	if (WARN_ON(!vmm))
+		return;
+
+	mutex_lock(&vmm->lock);
+
+	region = find_region(vmm, iova);
+	if (WARN_ON(!region))
+		goto err_region_not_found;
+
+	region->start = round_down(region->start, PAGE_SIZE);
+
+	gen_pool_free(vmm->vmm_pool, region->start, region->size);
+	list_del(&region->node);
+
+	while (region->size != 0) {
+		int order;
+
+		order = min(__fls(region->size), __ffs(region->start));
+
+		iommu_unmap(vmm->domain, region->start, order - PAGE_SHIFT);
+
+		region->start += 1 << order;
+		region->size -= 1 << order;
+	}
+
+	kfree(region);
+
+err_region_not_found:
+	mutex_unlock(&vmm->lock);
+}
+#else
 int iovmm_setup(struct device *dev)
 {
 	struct s5p_iovmm *vmm;
@@ -357,6 +638,7 @@ void iovmm_unmap(struct device *dev, dma_addr_t iova)
 err_region_not_found:
 	mutex_unlock(&vmm->lock);
 }
+#endif
 
 static int __init s5p_iovmm_init(void)
 {
diff --git a/arch/arm/plat-s5p/sysmmu.c b/arch/arm/plat-s5p/sysmmu.c
new file mode 100644
index 0000000..54f5edd
--- /dev/null
+++ b/arch/arm/plat-s5p/sysmmu.c
@@ -0,0 +1,312 @@
+/* linux/arch/arm/plat-s5p/sysmmu.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+
+#include <asm/pgtable.h>
+
+#include <mach/map.h>
+#include <mach/regs-sysmmu.h>
+#include <plat/sysmmu.h>
+
+#define CTRL_ENABLE	0x5
+#define CTRL_BLOCK	0x7
+#define CTRL_DISABLE	0x0
+
+static struct device *dev;
+
+static unsigned short fault_reg_offset[SYSMMU_FAULTS_NUM] = {
+	S5P_PAGE_FAULT_ADDR,
+	S5P_AR_FAULT_ADDR,
+	S5P_AW_FAULT_ADDR,
+	S5P_DEFAULT_SLAVE_ADDR,
+	S5P_AR_FAULT_ADDR,
+	S5P_AR_FAULT_ADDR,
+	S5P_AW_FAULT_ADDR,
+	S5P_AW_FAULT_ADDR
+};
+
+static char *sysmmu_fault_name[SYSMMU_FAULTS_NUM] = {
+	"PAGE FAULT",
+	"AR MULTI-HIT FAULT",
+	"AW MULTI-HIT FAULT",
+	"BUS ERROR",
+	"AR SECURITY PROTECTION FAULT",
+	"AR ACCESS PROTECTION FAULT",
+	"AW SECURITY PROTECTION FAULT",
+	"AW ACCESS PROTECTION FAULT"
+};
+
+static int (*fault_handlers[S5P_SYSMMU_TOTAL_IPNUM])(
+		enum S5P_SYSMMU_INTERRUPT_TYPE itype,
+		unsigned long pgtable_base,
+		unsigned long fault_addr);
+
+/*
+ * If adjacent 2 bits are true, the system MMU is enabled.
+ * The system MMU is disabled, otherwise.
+ */
+static unsigned long sysmmu_states;
+
+static inline void set_sysmmu_active(sysmmu_ips ips)
+{
+	sysmmu_states |= 3 << (ips * 2);
+}
+
+static inline void set_sysmmu_inactive(sysmmu_ips ips)
+{
+	sysmmu_states &= ~(3 << (ips * 2));
+}
+
+static inline int is_sysmmu_active(sysmmu_ips ips)
+{
+	return sysmmu_states & (3 << (ips * 2));
+}
+
+static void __iomem *sysmmusfrs[S5P_SYSMMU_TOTAL_IPNUM];
+
+static inline void sysmmu_block(sysmmu_ips ips)
+{
+	__raw_writel(CTRL_BLOCK, sysmmusfrs[ips] + S5P_MMU_CTRL);
+	dev_dbg(dev, "%s is blocked.\n", sysmmu_ips_name[ips]);
+}
+
+static inline void sysmmu_unblock(sysmmu_ips ips)
+{
+	__raw_writel(CTRL_ENABLE, sysmmusfrs[ips] + S5P_MMU_CTRL);
+	dev_dbg(dev, "%s is unblocked.\n", sysmmu_ips_name[ips]);
+}
+
+static inline void __sysmmu_tlb_invalidate(sysmmu_ips ips)
+{
+	__raw_writel(0x1, sysmmusfrs[ips] + S5P_MMU_FLUSH);
+	dev_dbg(dev, "TLB of %s is invalidated.\n", sysmmu_ips_name[ips]);
+}
+
+static inline void __sysmmu_set_ptbase(sysmmu_ips ips, unsigned long pgd)
+{
+	if (unlikely(pgd == 0)) {
+		pgd = (unsigned long)ZERO_PAGE(0);
+		__raw_writel(0x20, sysmmusfrs[ips] + S5P_MMU_CFG); /* 4KB LV1 */
+	} else {
+		__raw_writel(0x0, sysmmusfrs[ips] + S5P_MMU_CFG); /* 16KB LV1 */
+	}
+
+	__raw_writel(pgd, sysmmusfrs[ips] + S5P_PT_BASE_ADDR);
+
+	dev_dbg(dev, "Page table base of %s is initialized with 0x%08lX.\n",
+						sysmmu_ips_name[ips], pgd);
+	__sysmmu_tlb_invalidate(ips);
+}
+
+void sysmmu_set_fault_handler(sysmmu_ips ips,
+			int (*handler)(enum S5P_SYSMMU_INTERRUPT_TYPE itype,
+					unsigned long pgtable_base,
+					unsigned long fault_addr))
+{
+	BUG_ON(!((ips >= SYSMMU_MDMA) && (ips < S5P_SYSMMU_TOTAL_IPNUM)));
+	fault_handlers[ips] = handler;
+}
+
+static irqreturn_t s5p_sysmmu_irq(int irq, void *dev_id)
+{
+	/* SYSMMU is in blocked when interrupt occurred. */
+	unsigned long base = 0;
+	sysmmu_ips ips = (sysmmu_ips)dev_id;
+	enum S5P_SYSMMU_INTERRUPT_TYPE itype;
+
+	itype = (enum S5P_SYSMMU_INTERRUPT_TYPE)
+		__ffs(__raw_readl(sysmmusfrs[ips] + S5P_INT_STATUS));
+
+	BUG_ON(!((itype >= 0) && (itype < 8)));
+
+	dev_alert(dev, "%s occurred by %s.\n", sysmmu_fault_name[itype],
+							sysmmu_ips_name[ips]);
+
+	if (fault_handlers[ips]) {
+		unsigned long addr;
+
+		base = __raw_readl(sysmmusfrs[ips] + S5P_PT_BASE_ADDR);
+		addr = __raw_readl(sysmmusfrs[ips] + fault_reg_offset[itype]);
+
+		if (fault_handlers[ips](itype, base, addr)) {
+			__raw_writel(1 << itype,
+					sysmmusfrs[ips] + S5P_INT_CLEAR);
+			dev_notice(dev, "%s from %s is resolved."
+					" Retrying translation.\n",
+				sysmmu_fault_name[itype], sysmmu_ips_name[ips]);
+		} else {
+			base = 0;
+		}
+	}
+
+	sysmmu_unblock(ips);
+
+	if (!base)
+		dev_notice(dev, "%s from %s is not handled.\n",
+			sysmmu_fault_name[itype], sysmmu_ips_name[ips]);
+
+	return IRQ_HANDLED;
+}
+
+void s5p_sysmmu_set_tablebase_pgd(sysmmu_ips ips, unsigned long pgd)
+{
+	if (is_sysmmu_active(ips)) {
+		sysmmu_block(ips);
+		__sysmmu_set_ptbase(ips, pgd);
+		sysmmu_unblock(ips);
+	} else {
+		dev_dbg(dev, "%s is disabled. "
+			"Skipping initializing page table base.\n",
+						sysmmu_ips_name[ips]);
+	}
+}
+
+void s5p_sysmmu_enable(sysmmu_ips ips, unsigned long pgd)
+{
+	if (!is_sysmmu_active(ips)) {
+		sysmmu_clk_enable(ips);
+
+		__sysmmu_set_ptbase(ips, pgd);
+
+		__raw_writel(CTRL_ENABLE, sysmmusfrs[ips] + S5P_MMU_CTRL);
+
+		set_sysmmu_active(ips);
+		dev_dbg(dev, "%s is enabled.\n", sysmmu_ips_name[ips]);
+	} else {
+		dev_dbg(dev, "%s is already enabled.\n", sysmmu_ips_name[ips]);
+	}
+}
+
+void s5p_sysmmu_disable(sysmmu_ips ips)
+{
+	if (is_sysmmu_active(ips)) {
+		__raw_writel(CTRL_DISABLE, sysmmusfrs[ips] + S5P_MMU_CTRL);
+		set_sysmmu_inactive(ips);
+		sysmmu_clk_disable(ips);
+		dev_dbg(dev, "%s is disabled.\n", sysmmu_ips_name[ips]);
+	} else {
+		dev_dbg(dev, "%s is already disabled.\n", sysmmu_ips_name[ips]);
+	}
+}
+
+void s5p_sysmmu_tlb_invalidate(sysmmu_ips ips)
+{
+	if (is_sysmmu_active(ips)) {
+		sysmmu_block(ips);
+		__sysmmu_tlb_invalidate(ips);
+		sysmmu_unblock(ips);
+	} else {
+		dev_dbg(dev, "%s is disabled. "
+			"Skipping invalidating TLB.\n", sysmmu_ips_name[ips]);
+	}
+}
+
+static int s5p_sysmmu_probe(struct platform_device *pdev)
+{
+	int i, ret;
+	struct resource *res, *mem;
+
+	dev = &pdev->dev;
+
+	for (i = 0; i < S5P_SYSMMU_TOTAL_IPNUM; i++) {
+		int irq;
+
+		sysmmu_clk_init(dev, i);
+		sysmmu_clk_disable(i);
+
+		res = platform_get_resource(pdev, IORESOURCE_MEM, i);
+		if (!res) {
+			dev_err(dev, "Failed to get the resource of %s.\n",
+							sysmmu_ips_name[i]);
+			ret = -ENODEV;
+			goto err_res;
+		}
+
+		mem = request_mem_region(res->start,
+				((res->end) - (res->start)) + 1, pdev->name);
+		if (!mem) {
+			dev_err(dev, "Failed to request the memory region of %s.\n",
+							sysmmu_ips_name[i]);
+			ret = -EBUSY;
+			goto err_res;
+		}
+
+		sysmmusfrs[i] = ioremap(res->start, res->end - res->start + 1);
+		if (!sysmmusfrs[i]) {
+			dev_err(dev, "Failed to ioremap() for %s.\n",
+							sysmmu_ips_name[i]);
+			ret = -ENXIO;
+			goto err_reg;
+		}
+
+		irq = platform_get_irq(pdev, i);
+		if (irq <= 0) {
+			dev_err(dev, "Failed to get the IRQ resource of %s.\n",
+							sysmmu_ips_name[i]);
+			ret = -ENOENT;
+			goto err_map;
+		}
+
+		if (request_irq(irq, s5p_sysmmu_irq, IRQF_DISABLED,
+						pdev->name, (void *)i)) {
+			dev_err(dev, "Failed to request IRQ for %s.\n",
+							sysmmu_ips_name[i]);
+			ret = -ENOENT;
+			goto err_map;
+		}
+	}
+
+	return 0;
+
+err_map:
+	iounmap(sysmmusfrs[i]);
+err_reg:
+	release_mem_region(mem->start, resource_size(mem));
+err_res:
+	return ret;
+}
+
+static int s5p_sysmmu_remove(struct platform_device *pdev)
+{
+	return 0;
+}
+int s5p_sysmmu_runtime_suspend(struct device *dev)
+{
+	return 0;
+}
+
+int s5p_sysmmu_runtime_resume(struct device *dev)
+{
+	return 0;
+}
+
+const struct dev_pm_ops s5p_sysmmu_pm_ops = {
+	.runtime_suspend	= s5p_sysmmu_runtime_suspend,
+	.runtime_resume		= s5p_sysmmu_runtime_resume,
+};
+
+static struct platform_driver s5p_sysmmu_driver = {
+	.probe		= s5p_sysmmu_probe,
+	.remove		= s5p_sysmmu_remove,
+	.driver		= {
+		.owner		= THIS_MODULE,
+		.name		= "s5p-sysmmu",
+		.pm		= &s5p_sysmmu_pm_ops,
+	}
+};
+
+static int __init s5p_sysmmu_init(void)
+{
+	return platform_driver_register(&s5p_sysmmu_driver);
+}
+arch_initcall(s5p_sysmmu_init);