iommu: msm: Implement support for LPAE

Implement LPAE support in IOMMU driver. This enables bus masters
behind an IOMMU to access physical addresses greater than 32 bits.

Change-Id: I5716e9c8d1b3177e1d4e432ec43fa839dbe5cffd
Signed-off-by: Olav Haugan <ohaugan@codeaurora.org>

11 files changed, 1094 insertions, 86 deletions

diff --git a/arch/arm/mach-msm/include/mach/iommu.h b/arch/arm/mach-msm/include/mach/iommu.h
index 7cfbd93da03c..15a2108fca65 100644
--- a/arch/arm/mach-msm/include/mach/iommu.h
+++ b/arch/arm/mach-msm/include/mach/iommu.h
@@ -366,4 +366,10 @@ static inline int msm_soc_version_supports_iommu_v0(void)
 	soc_supports_v0 = 1;
 	return 1;
 }
+
+u32 msm_iommu_get_mair0(void);
+u32 msm_iommu_get_mair1(void);
+u32 msm_iommu_get_prrr(void);
+u32 msm_iommu_get_nmrr(void);
+
 #endif
diff --git a/arch/arm/mach-msm/include/mach/iommu_hw-v1.h b/arch/arm/mach-msm/include/mach/iommu_hw-v1.h
index 04cd441d95a2..999bf0fae294 100644
--- a/arch/arm/mach-msm/include/mach/iommu_hw-v1.h
+++ b/arch/arm/mach-msm/include/mach/iommu_hw-v1.h
@@ -27,6 +27,9 @@
 #define SET_CTX_REG(reg, base, ctx, val) \
 	writel_relaxed((val), \
 		((base) + CTX_OFFSET + (reg) + ((ctx) << CTX_SHIFT)))
+#define SET_CTX_REG_L(reg, base, ctx, val) \
+	writell_relaxed((val), \
+		((base) + CTX_OFFSET + (reg) + ((ctx) << CTX_SHIFT)))
 
 /* Wrappers for numbered registers */
 #define SET_GLOBAL_REG_N(b, n, r, v) SET_GLOBAL_REG((b), ((r) + (n << 2)), (v))
@@ -38,12 +41,18 @@
 #define GET_CONTEXT_FIELD(b, c, r, F) \
 	GET_FIELD(((b) + CTX_OFFSET + (r) + ((c) << CTX_SHIFT)), \
 			r##_##F##_MASK, r##_##F##_SHIFT)
+#define GET_CONTEXT_FIELD_L(b, c, r, F) \
+	GET_FIELD_L(((b) + CTX_OFFSET + (r) + ((c) << CTX_SHIFT)), \
+			r##_##F##_MASK, r##_##F##_SHIFT)
 
 #define SET_GLOBAL_FIELD(b, r, F, v) \
 	SET_FIELD(((b) + (r)), r##_##F##_MASK, r##_##F##_SHIFT, (v))
 #define SET_CONTEXT_FIELD(b, c, r, F, v) \
 	SET_FIELD(((b) + CTX_OFFSET + (r) + ((c) << CTX_SHIFT)), \
 			r##_##F##_MASK, r##_##F##_SHIFT, (v))
+#define SET_CONTEXT_FIELD_L(b, c, r, F, v) \
+	SET_FIELD_L(((b) + CTX_OFFSET + (r) + ((c) << CTX_SHIFT)), \
+			r##_##F##_MASK, r##_##F##_SHIFT, (v))
 
 /* Wrappers for numbered field registers */
 #define SET_GLOBAL_FIELD_N(b, n, r, F, v) \
@@ -52,6 +61,8 @@
 	GET_FIELD(((b) + ((n) << 2) + (r)), r##_##F##_MASK, r##_##F##_SHIFT)
 
 #define GET_FIELD(addr, mask, shift) ((readl_relaxed(addr) >> (shift)) & (mask))
+#define GET_FIELD_L(addr, mask, shift) \
+	((readll_relaxed(addr) >> (shift)) & (mask))
 
 #define SET_FIELD(addr, mask, shift, v) \
 do { \
@@ -60,6 +71,13 @@ do { \
 			(mask)) << (shift)), addr); \
 } while (0)
 
+#define SET_FIELD_L(addr, mask, shift, v) \
+do { \
+	u64 t = readll_relaxed(addr); \
+	writell_relaxed((t & ~(((u64) mask) << (shift))) + (((v) & \
+			((u64) mask)) << (shift)), addr); \
+} while (0)
+
 
 /* Global register space 0 setters / getters */
 #define SET_CR0(b, v)            SET_GLOBAL_REG(CR0, (b), (v))
@@ -163,8 +181,6 @@ do { \
 #define SET_SCTLR(b, c, v)       SET_CTX_REG(CB_SCTLR, (b), (c), (v))
 #define SET_ACTLR(b, c, v)       SET_CTX_REG(CB_ACTLR, (b), (c), (v))
 #define SET_RESUME(b, c, v)      SET_CTX_REG(CB_RESUME, (b), (c), (v))
-#define SET_TTBR0(b, c, v)       SET_CTX_REG(CB_TTBR0, (b), (c), (v))
-#define SET_TTBR1(b, c, v)       SET_CTX_REG(CB_TTBR1, (b), (c), (v))
 #define SET_TTBCR(b, c, v)       SET_CTX_REG(CB_TTBCR, (b), (c), (v))
 #define SET_CONTEXTIDR(b, c, v)  SET_CTX_REG(CB_CONTEXTIDR, (b), (c), (v))
 #define SET_PRRR(b, c, v)        SET_CTX_REG(CB_PRRR, (b), (c), (v))
@@ -201,7 +217,7 @@ do { \
 #define GET_PAR(b, c)            GET_CTX_REG_L(CB_PAR, (b), (c))
 #define GET_FSR(b, c)            GET_CTX_REG(CB_FSR, (b), (c))
 #define GET_FSRRESTORE(b, c)     GET_CTX_REG(CB_FSRRESTORE, (b), (c))
-#define GET_FAR(b, c)            GET_CTX_REG(CB_FAR, (b), (c))
+#define GET_FAR(b, c)            GET_CTX_REG_L(CB_FAR, (b), (c))
 #define GET_FSYNR0(b, c)         GET_CTX_REG(CB_FSYNR0, (b), (c))
 #define GET_FSYNR1(b, c)         GET_CTX_REG(CB_FSYNR1, (b), (c))
 #define GET_TLBIVA(b, c)         GET_CTX_REG(CB_TLBIVA, (b), (c))
@@ -879,25 +895,74 @@ do { \
 			GET_CONTEXT_FIELD(b, c, CB_TLBSTATUS, SACTIVE)
 
 /* Translation Table Base Control Register: CB_TTBCR */
-#define SET_CB_TTBCR_T0SZ(b, c, v)   SET_CONTEXT_FIELD(b, c, CB_TTBCR, T0SZ, v)
-#define SET_CB_TTBCR_PD0(b, c, v)    SET_CONTEXT_FIELD(b, c, CB_TTBCR, PD0, v)
-#define SET_CB_TTBCR_PD1(b, c, v)    SET_CONTEXT_FIELD(b, c, CB_TTBCR, PD1, v)
+/* These are shared between VMSA and LPAE */
+#define GET_CB_TTBCR_EAE(b, c)       GET_CONTEXT_FIELD(b, c, CB_TTBCR, EAE)
+#define SET_CB_TTBCR_EAE(b, c, v)    SET_CONTEXT_FIELD(b, c, CB_TTBCR, EAE, v)
+
 #define SET_CB_TTBCR_NSCFG0(b, c, v) \
 			SET_CONTEXT_FIELD(b, c, CB_TTBCR, NSCFG0, v)
 #define SET_CB_TTBCR_NSCFG1(b, c, v) \
 			SET_CONTEXT_FIELD(b, c, CB_TTBCR, NSCFG1, v)
-#define SET_CB_TTBCR_EAE(b, c, v)    SET_CONTEXT_FIELD(b, c, CB_TTBCR, EAE, v)
 
-#define GET_CB_TTBCR_T0SZ(b, c)      GET_CONTEXT_FIELD(b, c, CB_TTBCR, T0SZ)
-#define GET_CB_TTBCR_PD0(b, c)       GET_CONTEXT_FIELD(b, c, CB_TTBCR, PD0)
-#define GET_CB_TTBCR_PD1(b, c)       GET_CONTEXT_FIELD(b, c, CB_TTBCR, PD1)
 #define GET_CB_TTBCR_NSCFG0(b, c)    \
 			GET_CONTEXT_FIELD(b, c, CB_TTBCR, NSCFG0)
 #define GET_CB_TTBCR_NSCFG1(b, c)    \
 			GET_CONTEXT_FIELD(b, c, CB_TTBCR, NSCFG1)
-#define GET_CB_TTBCR_EAE(b, c)       GET_CONTEXT_FIELD(b, c, CB_TTBCR, EAE)
+
+#ifdef CONFIG_IOMMU_LPAE
+
+/* LPAE format */
 
 /* Translation Table Base Register 0: CB_TTBR */
+#define SET_TTBR0(b, c, v)       SET_CTX_REG_L(CB_TTBR0, (b), (c), (v))
+#define SET_TTBR1(b, c, v)       SET_CTX_REG_L(CB_TTBR1, (b), (c), (v))
+
+#define SET_CB_TTBR0_ASID(b, c, v)  SET_CONTEXT_FIELD_L(b, c, CB_TTBR0, ASID, v)
+#define SET_CB_TTBR0_ADDR(b, c, v)  SET_CONTEXT_FIELD_L(b, c, CB_TTBR0, ADDR, v)
+
+#define GET_CB_TTBR0_ASID(b, c)     GET_CONTEXT_FIELD_L(b, c, CB_TTBR0, ASID)
+#define GET_CB_TTBR0_ADDR(b, c)     GET_CONTEXT_FIELD_L(b, c, CB_TTBR0, ADDR)
+#define GET_CB_TTBR0(b, c)          GET_CTX_REG_L(CB_TTBR0, (b), (c))
+
+/* Translation Table Base Control Register: CB_TTBCR */
+#define SET_CB_TTBCR_T0SZ(b, c, v)   SET_CONTEXT_FIELD(b, c, CB_TTBCR, T0SZ, v)
+#define SET_CB_TTBCR_T1SZ(b, c, v)   SET_CONTEXT_FIELD(b, c, CB_TTBCR, T1SZ, v)
+#define SET_CB_TTBCR_EPD0(b, c, v)   SET_CONTEXT_FIELD(b, c, CB_TTBCR, EPD0, v)
+#define SET_CB_TTBCR_EPD1(b, c, v)   SET_CONTEXT_FIELD(b, c, CB_TTBCR, EPD1, v)
+#define SET_CB_TTBCR_IRGN0(b, c, v)  SET_CONTEXT_FIELD(b, c, CB_TTBCR, IRGN0, v)
+#define SET_CB_TTBCR_IRGN1(b, c, v)  SET_CONTEXT_FIELD(b, c, CB_TTBCR, IRGN1, v)
+#define SET_CB_TTBCR_ORGN0(b, c, v)  SET_CONTEXT_FIELD(b, c, CB_TTBCR, ORGN0, v)
+#define SET_CB_TTBCR_ORGN1(b, c, v)  SET_CONTEXT_FIELD(b, c, CB_TTBCR, ORGN1, v)
+#define SET_CB_TTBCR_NSCFG0(b, c, v) \
+				SET_CONTEXT_FIELD(b, c, CB_TTBCR, NSCFG0, v)
+#define SET_CB_TTBCR_NSCFG1(b, c, v) \
+				SET_CONTEXT_FIELD(b, c, CB_TTBCR, NSCFG1, v)
+
+#define SET_CB_TTBCR_SH0(b, c, v)    SET_CONTEXT_FIELD(b, c, CB_TTBCR, SH0, v)
+#define SET_CB_TTBCR_SH1(b, c, v)    SET_CONTEXT_FIELD(b, c, CB_TTBCR, SH1, v)
+#define SET_CB_TTBCR_A1(b, c, v)     SET_CONTEXT_FIELD(b, c, CB_TTBCR, A1, v)
+
+#define GET_CB_TTBCR_T0SZ(b, c)      GET_CONTEXT_FIELD(b, c, CB_TTBCR, T0SZ)
+#define GET_CB_TTBCR_T1SZ(b, c)      GET_CONTEXT_FIELD(b, c, CB_TTBCR, T1SZ)
+#define GET_CB_TTBCR_EPD0(b, c)      GET_CONTEXT_FIELD(b, c, CB_TTBCR, EPD0)
+#define GET_CB_TTBCR_EPD1(b, c)      GET_CONTEXT_FIELD(b, c, CB_TTBCR, EPD1)
+#define GET_CB_TTBCR_IRGN0(b, c, v)  GET_CONTEXT_FIELD(b, c, CB_TTBCR, IRGN0)
+#define GET_CB_TTBCR_IRGN1(b, c, v)  GET_CONTEXT_FIELD(b, c, CB_TTBCR, IRGN1)
+#define GET_CB_TTBCR_ORGN0(b, c, v)  GET_CONTEXT_FIELD(b, c, CB_TTBCR, ORGN0)
+#define GET_CB_TTBCR_ORGN1(b, c, v) GET_CONTEXT_FIELD(b, c, CB_TTBCR, ORGN1)
+
+#define SET_CB_MAIR0(b, c, v)        SET_CTX_REG(CB_MAIR0, (b), (c), (v))
+#define SET_CB_MAIR1(b, c, v)        SET_CTX_REG(CB_MAIR1, (b), (c), (v))
+
+#define GET_CB_MAIR0(b, c)           GET_CTX_REG(CB_MAIR0, (b), (c))
+#define GET_CB_MAIR1(b, c)           GET_CTX_REG(CB_MAIR1, (b), (c))
+#else
+#define SET_TTBR0(b, c, v)           SET_CTX_REG(CB_TTBR0, (b), (c), (v))
+#define SET_TTBR1(b, c, v)           SET_CTX_REG(CB_TTBR1, (b), (c), (v))
+
+#define SET_CB_TTBCR_PD0(b, c, v)    SET_CONTEXT_FIELD(b, c, CB_TTBCR, PD0, v)
+#define SET_CB_TTBCR_PD1(b, c, v)    SET_CONTEXT_FIELD(b, c, CB_TTBCR, PD1, v)
+
 #define SET_CB_TTBR0_IRGN1(b, c, v) SET_CONTEXT_FIELD(b, c, CB_TTBR0, IRGN1, v)
 #define SET_CB_TTBR0_S(b, c, v)     SET_CONTEXT_FIELD(b, c, CB_TTBR0, S, v)
 #define SET_CB_TTBR0_RGN(b, c, v)   SET_CONTEXT_FIELD(b, c, CB_TTBR0, RGN, v)
@@ -911,6 +976,7 @@ do { \
 #define GET_CB_TTBR0_NOS(b, c)      GET_CONTEXT_FIELD(b, c, CB_TTBR0, NOS)
 #define GET_CB_TTBR0_IRGN0(b, c)    GET_CONTEXT_FIELD(b, c, CB_TTBR0, IRGN0)
 #define GET_CB_TTBR0_ADDR(b, c)     GET_CONTEXT_FIELD(b, c, CB_TTBR0, ADDR)
+#endif
 
 /* Translation Table Base Register 1: CB_TTBR1 */
 #define SET_CB_TTBR1_IRGN1(b, c, v) SET_CONTEXT_FIELD(b, c, CB_TTBR1, IRGN1, v)
@@ -1006,7 +1072,9 @@ do { \
 #define CB_TTBCR	(0x030)
 #define CB_CONTEXTIDR	(0x034)
 #define CB_PRRR		(0x038)
+#define CB_MAIR0	(0x038)
 #define CB_NMRR		(0x03C)
+#define CB_MAIR1	(0x03C)
 #define CB_PAR		(0x050)
 #define CB_FSR		(0x058)
 #define CB_FSRRESTORE	(0x05C)
@@ -1385,12 +1453,31 @@ do { \
 						CB_TLBSTATUS_SACTIVE_SHIFT)
 
 /* Translation Table Base Control Register: CB_TTBCR */
+#define CB_TTBCR_EAE         (CB_TTBCR_EAE_MASK     << CB_TTBCR_EAE_SHIFT)
+
+#define CB_TTBR0_ADDR        (CB_TTBR0_ADDR_MASK    << CB_TTBR0_ADDR_SHIFT)
+
+#ifdef CONFIG_IOMMU_LPAE
+/* Translation Table Base Register: CB_TTBR */
+#define CB_TTBR0_ASID        (CB_TTBR0_ASID_MASK    << CB_TTBR0_ASID_SHIFT)
+#define CB_TTBR1_ASID        (CB_TTBR1_ASID_MASK    << CB_TTBR1_ASID_SHIFT)
+
+/* Translation Table Base Control Register: CB_TTBCR */
 #define CB_TTBCR_T0SZ        (CB_TTBCR_T0SZ_MASK    << CB_TTBCR_T0SZ_SHIFT)
-#define CB_TTBCR_PD0         (CB_TTBCR_PD0_MASK     << CB_TTBCR_PD0_SHIFT)
-#define CB_TTBCR_PD1         (CB_TTBCR_PD1_MASK     << CB_TTBCR_PD1_SHIFT)
+#define CB_TTBCR_T1SZ        (CB_TTBCR_T1SZ_MASK    << CB_TTBCR_T1SZ_SHIFT)
+#define CB_TTBCR_EPD0        (CB_TTBCR_EPD0_MASK    << CB_TTBCR_EPD0_SHIFT)
+#define CB_TTBCR_EPD1        (CB_TTBCR_EPD1_MASK    << CB_TTBCR_EPD1_SHIFT)
+#define CB_TTBCR_IRGN0       (CB_TTBCR_IRGN0_MASK   << CB_TTBCR_IRGN0_SHIFT)
+#define CB_TTBCR_IRGN1       (CB_TTBCR_IRGN1_MASK   << CB_TTBCR_IRGN1_SHIFT)
+#define CB_TTBCR_ORGN0       (CB_TTBCR_ORGN0_MASK   << CB_TTBCR_ORGN0_SHIFT)
+#define CB_TTBCR_ORGN1       (CB_TTBCR_ORGN1_MASK   << CB_TTBCR_ORGN1_SHIFT)
 #define CB_TTBCR_NSCFG0      (CB_TTBCR_NSCFG0_MASK  << CB_TTBCR_NSCFG0_SHIFT)
 #define CB_TTBCR_NSCFG1      (CB_TTBCR_NSCFG1_MASK  << CB_TTBCR_NSCFG1_SHIFT)
-#define CB_TTBCR_EAE         (CB_TTBCR_EAE_MASK     << CB_TTBCR_EAE_SHIFT)
+#define CB_TTBCR_SH0         (CB_TTBCR_SH0_MASK     << CB_TTBCR_SH0_SHIFT)
+#define CB_TTBCR_SH1         (CB_TTBCR_SH1_MASK     << CB_TTBCR_SH1_SHIFT)
+#define CB_TTBCR_A1          (CB_TTBCR_A1_MASK      << CB_TTBCR_A1_SHIFT)
+
+#else
 
 /* Translation Table Base Register 0: CB_TTBR0 */
 #define CB_TTBR0_IRGN1       (CB_TTBR0_IRGN1_MASK   << CB_TTBR0_IRGN1_SHIFT)
@@ -1398,7 +1485,6 @@ do { \
 #define CB_TTBR0_RGN         (CB_TTBR0_RGN_MASK     << CB_TTBR0_RGN_SHIFT)
 #define CB_TTBR0_NOS         (CB_TTBR0_NOS_MASK     << CB_TTBR0_NOS_SHIFT)
 #define CB_TTBR0_IRGN0       (CB_TTBR0_IRGN0_MASK   << CB_TTBR0_IRGN0_SHIFT)
-#define CB_TTBR0_ADDR        (CB_TTBR0_ADDR_MASK    << CB_TTBR0_ADDR_SHIFT)
 
 /* Translation Table Base Register 1: CB_TTBR1 */
 #define CB_TTBR1_IRGN1       (CB_TTBR1_IRGN1_MASK   << CB_TTBR1_IRGN1_SHIFT)
@@ -1406,7 +1492,7 @@ do { \
 #define CB_TTBR1_RGN         (CB_TTBR1_RGN_MASK     << CB_TTBR1_RGN_SHIFT)
 #define CB_TTBR1_NOS         (CB_TTBR1_NOS_MASK     << CB_TTBR1_NOS_SHIFT)
 #define CB_TTBR1_IRGN0       (CB_TTBR1_IRGN0_MASK   << CB_TTBR1_IRGN0_SHIFT)
-#define CB_TTBR1_ADDR        (CB_TTBR1_ADDR_MASK    << CB_TTBR1_ADDR_SHIFT)
+#endif
 
 /* Global Register Masks */
 /* Configuration Register 0 */
@@ -1760,13 +1846,26 @@ do { \
 
 /* Translation Table Base Control Register: CB_TTBCR */
 #define CB_TTBCR_T0SZ_MASK         0x07
-#define CB_TTBCR_PD0_MASK          0x01
-#define CB_TTBCR_PD1_MASK          0x01
+#define CB_TTBCR_T1SZ_MASK         0x07
+#define CB_TTBCR_EPD0_MASK         0x01
+#define CB_TTBCR_EPD1_MASK         0x01
+#define CB_TTBCR_IRGN0_MASK        0x03
+#define CB_TTBCR_IRGN1_MASK        0x03
+#define CB_TTBCR_ORGN0_MASK        0x03
+#define CB_TTBCR_ORGN1_MASK        0x03
 #define CB_TTBCR_NSCFG0_MASK       0x01
 #define CB_TTBCR_NSCFG1_MASK       0x01
+#define CB_TTBCR_SH0_MASK          0x03
+#define CB_TTBCR_SH1_MASK          0x03
+#define CB_TTBCR_A1_MASK           0x01
 #define CB_TTBCR_EAE_MASK          0x01
 
 /* Translation Table Base Register 0/1: CB_TTBR */
+#ifdef CONFIG_IOMMU_LPAE
+#define CB_TTBR0_ADDR_MASK         0x7FFFFFFFFULL
+#define CB_TTBR0_ASID_MASK         0xFF
+#define CB_TTBR1_ASID_MASK         0xFF
+#else
 #define CB_TTBR0_IRGN1_MASK        0x01
 #define CB_TTBR0_S_MASK            0x01
 #define CB_TTBR0_RGN_MASK          0x01
@@ -1779,7 +1878,7 @@ do { \
 #define CB_TTBR1_RGN_MASK          0x1
 #define CB_TTBR1_NOS_MASK          0X1
 #define CB_TTBR1_IRGN0_MASK        0X1
-#define CB_TTBR1_ADDR_MASK         0xFFFFFF
+#endif
 
 /* Global Register Shifts */
 /* Configuration Register: CR0 */
@@ -2130,14 +2229,27 @@ do { \
 #define CB_TLBSTATUS_SACTIVE_SHIFT  0
 
 /* Translation Table Base Control Register: CB_TTBCR */
-#define CB_TTBCR_T0SZ_SHIFT         0
-#define CB_TTBCR_PD0_SHIFT          4
-#define CB_TTBCR_PD1_SHIFT          5
+#define CB_TTBCR_T0SZ_SHIFT          0
+#define CB_TTBCR_T1SZ_SHIFT         16
+#define CB_TTBCR_EPD0_SHIFT          4
+#define CB_TTBCR_EPD1_SHIFT          5
 #define CB_TTBCR_NSCFG0_SHIFT       14
 #define CB_TTBCR_NSCFG1_SHIFT       30
 #define CB_TTBCR_EAE_SHIFT          31
+#define CB_TTBCR_IRGN0_SHIFT         8
+#define CB_TTBCR_IRGN1_SHIFT        24
+#define CB_TTBCR_ORGN0_SHIFT        10
+#define CB_TTBCR_ORGN1_SHIFT        26
+#define CB_TTBCR_A1_SHIFT           22
+#define CB_TTBCR_SH0_SHIFT          12
+#define CB_TTBCR_SH1_SHIFT          28
 
 /* Translation Table Base Register 0/1: CB_TTBR */
+#ifdef CONFIG_IOMMU_LPAE
+#define CB_TTBR0_ADDR_SHIFT         5
+#define CB_TTBR0_ASID_SHIFT         48
+#define CB_TTBR1_ASID_SHIFT         48
+#else
 #define CB_TTBR0_IRGN1_SHIFT        0
 #define CB_TTBR0_S_SHIFT            1
 #define CB_TTBR0_RGN_SHIFT          3
@@ -2151,5 +2263,6 @@ do { \
 #define CB_TTBR1_NOS_SHIFT          5
 #define CB_TTBR1_IRGN0_SHIFT        6
 #define CB_TTBR1_ADDR_SHIFT         14
+#endif
 
 #endif
diff --git a/arch/arm/mach-msm/include/mach/msm_iommu_priv.h b/arch/arm/mach-msm/include/mach/msm_iommu_priv.h
index a2f483699ebf..e34eb3d99562 100644
--- a/arch/arm/mach-msm/include/mach/msm_iommu_priv.h
+++ b/arch/arm/mach-msm/include/mach/msm_iommu_priv.h
@@ -18,12 +18,22 @@
  * attributes.
  * fl_table: Pointer to the first level page table.
  * redirect: Set to 1 if L2 redirect for page tables are enabled, 0 otherwise.
+ * unaligned_fl_table: Original address of memory for the page table.
+ * fl_table is manually aligned (as per spec) but we need the original address
+ * to free the table.
  */
+#ifdef CONFIG_IOMMU_LPAE
+struct msm_iommu_pt {
+	u64 *fl_table;
+	int redirect;
+	u64 *unaligned_fl_table;
+};
+#else
 struct msm_iommu_pt {
 	unsigned long *fl_table;
 	int redirect;
 };
-
+#endif
 /**
  * struct msm_iommu_priv - Container for page table attributes and other
  * private iommu domain information.
diff --git a/drivers/iommu/Kconfig b/drivers/iommu/Kconfig
index 40aa81ce2f4c..fef4d9511e0a 100644
--- a/drivers/iommu/Kconfig
+++ b/drivers/iommu/Kconfig
@@ -79,6 +79,16 @@ config IOMMU_PGTABLES_L2
          section mappings and TLB misses should be quite infrequent.
          Most people can probably say Y here.
 
+config IOMMU_LPAE
+	bool "Enable support for LPAE in IOMMU"
+	depends on MSM_IOMMU
+	help
+	  Enables Large Physical Address Extension (LPAE) for IOMMU. This allows
+	  clients of IOMMU to access physical addresses that are greater than
+	  32 bits.
+
+	  If unsure, say N here.
+
 config IOMMU_NON_SECURE
 	bool "Turns on programming of secure SMMU by kernel"
 	depends on MSM_IOMMU
diff --git a/drivers/iommu/Makefile b/drivers/iommu/Makefile
index 325a692335d8..eea0eb8f850b 100644
--- a/drivers/iommu/Makefile
+++ b/drivers/iommu/Makefile
@@ -2,7 +2,7 @@ obj-$(CONFIG_IOMMU_API) += iommu.o
 obj-$(CONFIG_OF_IOMMU)	+= of_iommu.o
 obj-$(CONFIG_MSM_IOMMU) += msm_iommu.o
 obj-$(CONFIG_MSM_IOMMU_V0) += msm_iommu-v0.o msm_iommu_dev-v0.o
-obj-$(CONFIG_MSM_IOMMU_V1) += msm_iommu-v1.o msm_iommu_dev-v1.o msm_iommu_pagetable.o msm_iommu_sec.o
+obj-$(CONFIG_MSM_IOMMU_V1) += msm_iommu-v1.o msm_iommu_dev-v1.o msm_iommu_sec.o
 obj-$(CONFIG_MSM_IOMMU_PMON) += msm_iommu_perfmon.o
 ifdef CONFIG_MSM_IOMMU_V0
 obj-$(CONFIG_MSM_IOMMU_PMON) += msm_iommu_perfmon-v0.o
@@ -10,6 +10,11 @@ endif
 ifdef CONFIG_MSM_IOMMU_V1
 obj-$(CONFIG_MSM_IOMMU_PMON) += msm_iommu_perfmon-v1.o
 endif
+ifdef CONFIG_IOMMU_LPAE
+obj-$(CONFIG_MSM_IOMMU_V1) += msm_iommu_pagetable_lpae.o
+else
+obj-$(CONFIG_MSM_IOMMU_V1) += msm_iommu_pagetable.o
+endif
 obj-$(CONFIG_AMD_IOMMU) += amd_iommu.o amd_iommu_init.o
 obj-$(CONFIG_AMD_IOMMU_V2) += amd_iommu_v2.o
 obj-$(CONFIG_DMAR_TABLE) += dmar.o
diff --git a/drivers/iommu/msm_iommu-v0.c b/drivers/iommu/msm_iommu-v0.c
index f10d8833fa16..1676decf8f84 100644
--- a/drivers/iommu/msm_iommu-v0.c
+++ b/drivers/iommu/msm_iommu-v0.c
@@ -34,14 +34,6 @@
 #include <mach/msm_smem.h>
 #include <mach/msm_bus.h>
 
-#define MRC(reg, processor, op1, crn, crm, op2)				\
-__asm__ __volatile__ (							\
-"   mrc   "   #processor "," #op1 ", %0,"  #crn "," #crm "," #op2 "\n"  \
-: "=r" (reg))
-
-#define RCP15_PRRR(reg)		MRC(reg, p15, 0, c10, c2, 0)
-#define RCP15_NMRR(reg)		MRC(reg, p15, 0, c10, c2, 1)
-
 /* Sharability attributes of MSM IOMMU mappings */
 #define MSM_IOMMU_ATTR_NON_SH		0x0
 #define MSM_IOMMU_ATTR_SH		0x4
@@ -355,8 +347,8 @@ static void __program_context(void __iomem *base, void __iomem *glb_base,
 	SET_TRE(base, ctx, 1);
 
 	/* Set TEX remap attributes */
-	RCP15_PRRR(prrr);
-	RCP15_NMRR(nmrr);
+	prrr = msm_iommu_get_prrr();
+	nmrr = msm_iommu_get_nmrr();
 	SET_PRRR(base, ctx, prrr);
 	SET_NMRR(base, ctx, nmrr);
 
@@ -1398,8 +1390,8 @@ static int __init get_tex_class(int icp, int ocp, int mt, int nos)
 	unsigned int nmrr = 0;
 	int c_icp, c_ocp, c_mt, c_nos;
 
-	RCP15_PRRR(prrr);
-	RCP15_NMRR(nmrr);
+	prrr = msm_iommu_get_prrr();
+	nmrr = msm_iommu_get_nmrr();
 
 	for (i = 0; i < NUM_TEX_CLASS; i++) {
 		c_nos = PRRR_NOS(prrr, i);
diff --git a/drivers/iommu/msm_iommu-v1.c b/drivers/iommu/msm_iommu-v1.c
index c5169d01b54e..dec008c21c71 100644
--- a/drivers/iommu/msm_iommu-v1.c
+++ b/drivers/iommu/msm_iommu-v1.c
@@ -35,8 +35,13 @@
 #include <mach/msm_bus.h>
 #include "msm_iommu_pagetable.h"
 
+#ifdef CONFIG_IOMMU_LPAE
+/* bitmap of the page sizes currently supported */
+#define MSM_IOMMU_PGSIZES	(SZ_4K | SZ_64K | SZ_2M | SZ_32M | SZ_1G)
+#else
 /* bitmap of the page sizes currently supported */
 #define MSM_IOMMU_PGSIZES	(SZ_4K | SZ_64K | SZ_1M | SZ_16M)
+#endif
 
 static DEFINE_MUTEX(msm_iommu_lock);
 struct dump_regs_tbl dump_regs_tbl[MAX_DUMP_REGS];
@@ -355,6 +360,20 @@ static void __release_smg(void __iomem *base, int ctx)
 			SET_SMR_VALID(base, i, 0);
 }
 
+#ifdef CONFIG_IOMMU_LPAE
+static void msm_iommu_set_ASID(void __iomem *base, unsigned int ctx_num,
+			       unsigned int asid)
+{
+	SET_CB_TTBR0_ASID(base, ctx_num, asid);
+}
+#else
+static void msm_iommu_set_ASID(void __iomem *base, unsigned int ctx_num,
+			       unsigned int asid)
+{
+	SET_CB_CONTEXTIDR_ASID(base, ctx_num, asid);
+}
+#endif
+
 static void msm_iommu_assign_ASID(const struct msm_iommu_drvdata *iommu_drvdata,
 				  struct msm_iommu_ctx_drvdata *curr_ctx,
 				  struct msm_iommu_priv *priv)
@@ -372,8 +391,6 @@ static void msm_iommu_assign_ASID(const struct msm_iommu_drvdata *iommu_drvdata,
 
 		++iommu_drvdata->asid[tmp_drvdata->asid - 1];
 		curr_ctx->asid = tmp_drvdata->asid;
-
-		SET_CB_CONTEXTIDR_ASID(base, curr_ctx->num, curr_ctx->asid);
 		found = 1;
 	}
 
@@ -383,23 +400,79 @@ static void msm_iommu_assign_ASID(const struct msm_iommu_drvdata *iommu_drvdata,
 			if (iommu_drvdata->asid[i] == 0) {
 				++iommu_drvdata->asid[i];
 				curr_ctx->asid = i + 1;
-
-				SET_CB_CONTEXTIDR_ASID(base, curr_ctx->num,
-						       curr_ctx->asid);
 				found = 1;
 				break;
 			}
 		}
 		BUG_ON(!found);
 	}
+
+	msm_iommu_set_ASID(base, curr_ctx->num, curr_ctx->asid);
+}
+
+#ifdef CONFIG_IOMMU_LPAE
+static void msm_iommu_setup_ctx(void __iomem *base, unsigned int ctx)
+{
+	SET_CB_TTBCR_EAE(base, ctx, 1); /* Extended Address Enable (EAE) */
+}
+
+static void msm_iommu_setup_memory_remap(void __iomem *base, unsigned int ctx)
+{
+	SET_CB_MAIR0(base, ctx, msm_iommu_get_mair0());
+	SET_CB_MAIR1(base, ctx, msm_iommu_get_mair1());
+}
+
+static void msm_iommu_setup_pg_l2_redirect(void __iomem *base, unsigned int ctx)
+{
+	/*
+	 * Configure page tables as inner-cacheable and shareable to reduce
+	 * the TLB miss penalty.
+	 */
+	SET_CB_TTBCR_SH0(base, ctx, 3); /* Inner shareable */
+	SET_CB_TTBCR_ORGN0(base, ctx, 1); /* outer cachable*/
+	SET_CB_TTBCR_IRGN0(base, ctx, 1); /* inner cachable*/
+	SET_CB_TTBCR_T0SZ(base, ctx, 0); /* 0GB-4GB */
+
+
+	SET_CB_TTBCR_SH1(base, ctx, 3); /* Inner shareable */
+	SET_CB_TTBCR_ORGN1(base, ctx, 1); /* outer cachable*/
+	SET_CB_TTBCR_IRGN1(base, ctx, 1); /* inner cachable*/
+	SET_CB_TTBCR_T1SZ(base, ctx, 0); /* TTBR1 not used */
+}
+
+#else
+
+static void msm_iommu_setup_ctx(void __iomem *base, unsigned int ctx)
+{
+	/* Turn on TEX Remap */
+	SET_CB_SCTLR_TRE(base, ctx, 1);
+}
+
+static void msm_iommu_setup_memory_remap(void __iomem *base, unsigned int ctx)
+{
+	SET_PRRR(base, ctx, msm_iommu_get_prrr());
+	SET_NMRR(base, ctx, msm_iommu_get_nmrr());
+}
+
+static void msm_iommu_setup_pg_l2_redirect(void __iomem *base, unsigned int ctx)
+{
+	/* Configure page tables as inner-cacheable and shareable to reduce
+	 * the TLB miss penalty.
+	 */
+	SET_CB_TTBR0_S(base, ctx, 1);
+	SET_CB_TTBR0_NOS(base, ctx, 1);
+	SET_CB_TTBR0_IRGN1(base, ctx, 0); /* WB, WA */
+	SET_CB_TTBR0_IRGN0(base, ctx, 1);
+	SET_CB_TTBR0_RGN(base, ctx, 1);   /* WB, WA */
 }
 
+#endif
+
 static void __program_context(struct msm_iommu_drvdata *iommu_drvdata,
 			      struct msm_iommu_ctx_drvdata *ctx_drvdata,
 			      struct msm_iommu_priv *priv, bool is_secure)
 {
-	unsigned int prrr, nmrr;
-	unsigned int pn;
+	phys_addr_t pn;
 	int num = 0, i, smt_size;
 	void __iomem *base = iommu_drvdata->base;
 	unsigned int ctx = ctx_drvdata->num;
@@ -408,9 +481,14 @@ static void __program_context(struct msm_iommu_drvdata *iommu_drvdata,
 	phys_addr_t pgtable = __pa(priv->pt.fl_table);
 
 	__reset_context(base, ctx);
+	msm_iommu_setup_ctx(base, ctx);
+
+	if (priv->pt.redirect)
+		msm_iommu_setup_pg_l2_redirect(base, ctx);
+
+	msm_iommu_setup_memory_remap(base, ctx);
 
 	pn = pgtable >> CB_TTBR0_ADDR_SHIFT;
-	SET_TTBCR(base, ctx, 0);
 	SET_CB_TTBR0_ADDR(base, ctx, pn);
 
 	/* Enable context fault interrupt */
@@ -421,29 +499,9 @@ static void __program_context(struct msm_iommu_drvdata *iommu_drvdata,
 	SET_CB_ACTLR_BPRCOSH(base, ctx, 1);
 	SET_CB_ACTLR_BPRCNSH(base, ctx, 1);
 
-	/* Turn on TEX Remap */
-	SET_CB_SCTLR_TRE(base, ctx, 1);
-
 	/* Enable private ASID namespace */
 	SET_CB_SCTLR_ASIDPNE(base, ctx, 1);
 
-	/* Set TEX remap attributes */
-	RCP15_PRRR(prrr);
-	RCP15_NMRR(nmrr);
-	SET_PRRR(base, ctx, prrr);
-	SET_NMRR(base, ctx, nmrr);
-
-	/* Configure page tables as inner-cacheable and shareable to reduce
-	 * the TLB miss penalty.
-	 */
-	if (priv->pt.redirect) {
-		SET_CB_TTBR0_S(base, ctx, 1);
-		SET_CB_TTBR0_NOS(base, ctx, 1);
-		SET_CB_TTBR0_IRGN1(base, ctx, 0); /* WB, WA */
-		SET_CB_TTBR0_IRGN0(base, ctx, 1);
-		SET_CB_TTBR0_RGN(base, ctx, 1);   /* WB, WA */
-	}
-
 	if (!is_secure) {
 		smt_size = GET_IDR0_NUMSMRG(base);
 		/* Program the M2V tables for this context */
@@ -776,6 +834,29 @@ static int msm_iommu_unmap_range(struct iommu_domain *domain, unsigned int va,
 	return 0;
 }
 
+#ifdef CONFIG_IOMMU_LPAE
+static phys_addr_t msm_iommu_get_phy_from_PAR(unsigned long va, u64 par)
+{
+	phys_addr_t phy;
+	/* Upper 28 bits from PAR, lower 12 from VA */
+	phy = (par & 0xFFFFFFF000ULL) | (va & 0x00000FFF);
+	return phy;
+}
+#else
+static phys_addr_t msm_iommu_get_phy_from_PAR(unsigned long va, u64 par)
+{
+	phys_addr_t phy;
+
+	/* We are dealing with a supersection */
+	if (par & CB_PAR_SS)
+		phy = (par & 0xFF000000) | (va & 0x00FFFFFF);
+	else /* Upper 20 bits from PAR, lower 12 from VA */
+		phy = (par & 0xFFFFF000) | (va & 0x00000FFF);
+
+	return phy;
+}
+#endif
+
 static phys_addr_t msm_iommu_iova_to_phys(struct iommu_domain *domain,
 					  phys_addr_t va)
 {
@@ -834,11 +915,7 @@ static phys_addr_t msm_iommu_iova_to_phys(struct iommu_domain *domain,
 			(par & CB_PAR_STAGE) ? "S2 " : "S1 ");
 		ret = 0;
 	} else {
-		/* We are dealing with a supersection */
-		if (ret & CB_PAR_SS)
-			ret = (par & 0xFF000000) | (va & 0x00FFFFFF);
-		else /* Upper 20 bits from PAR, lower 12 from VA */
-			ret = (par & 0xFFFFF000) | (va & 0x00000FFF);
+		ret = msm_iommu_get_phy_from_PAR(va, par);
 	}
 
 fail:
@@ -852,6 +929,20 @@ static int msm_iommu_domain_has_cap(struct iommu_domain *domain,
 	return 0;
 }
 
+#ifdef CONFIG_IOMMU_LPAE
+static inline void print_ctx_mem_attr_regs(struct msm_iommu_context_reg regs[])
+{
+	pr_err("MAIR0   = %08x    MAIR1   = %08x\n",
+		 regs[DUMP_REG_MAIR0].val, regs[DUMP_REG_MAIR1].val);
+}
+#else
+static inline void print_ctx_mem_attr_regs(struct msm_iommu_context_reg regs[])
+{
+	pr_err("PRRR   = %08x    NMRR   = %08x\n",
+		 regs[DUMP_REG_PRRR].val, regs[DUMP_REG_NMRR].val);
+}
+#endif
+
 void print_ctx_regs(struct msm_iommu_context_reg regs[])
 {
 	uint32_t fsr = regs[DUMP_REG_FSR].val;
@@ -896,8 +987,7 @@ void print_ctx_regs(struct msm_iommu_context_reg regs[])
 
 	pr_err("SCTLR  = %08x    ACTLR  = %08x\n",
 		 regs[DUMP_REG_SCTLR].val, regs[DUMP_REG_ACTLR].val);
-	pr_err("PRRR   = %08x    NMRR   = %08x\n",
-		 regs[DUMP_REG_PRRR].val, regs[DUMP_REG_NMRR].val);
+	print_ctx_mem_attr_regs(regs);
 }
 
 static void __print_ctx_regs(void __iomem *base, int ctx, unsigned int fsr)
diff --git a/drivers/iommu/msm_iommu.c b/drivers/iommu/msm_iommu.c
index ebecd114e3cb..06f01b4e5251 100644
--- a/drivers/iommu/msm_iommu.c
+++ b/drivers/iommu/msm_iommu.c
@@ -23,6 +23,17 @@
 static DEFINE_MUTEX(iommu_list_lock);
 static LIST_HEAD(iommu_list);
 
+#define MRC(reg, processor, op1, crn, crm, op2)				\
+__asm__ __volatile__ (							\
+"   mrc   "   #processor "," #op1 ", %0,"  #crn "," #crm "," #op2 "\n"  \
+: "=r" (reg))
+
+#define RCP15_PRRR(reg)   MRC(reg, p15, 0, c10, c2, 0)
+#define RCP15_NMRR(reg)   MRC(reg, p15, 0, c10, c2, 1)
+
+#define RCP15_MAIR0(reg)   MRC(reg, p15, 0, c10, c2, 0)
+#define RCP15_MAIR1(reg)   MRC(reg, p15, 0, c10, c2, 1)
+
 static struct iommu_access_ops *iommu_access_ops;
 
 struct bus_type msm_iommu_sec_bus_type = {
@@ -95,3 +106,88 @@ struct device *msm_iommu_get_ctx(const char *ctx_name)
 }
 EXPORT_SYMBOL(msm_iommu_get_ctx);
 
+/* These values come from proc-v7-2level.S */
+#define PRRR_VALUE 0xff0a81a8
+#define NMRR_VALUE 0x40e040e0
+
+/* These values come from proc-v7-3level.S */
+#define MAIR0_VALUE 0xeeaa4400
+#define MAIR1_VALUE 0xff000004
+
+#ifdef CONFIG_IOMMU_LPAE
+#ifdef CONFIG_ARM_LPAE
+/*
+ * If CONFIG_ARM_LPAE AND CONFIG_IOMMU_LPAE are enabled we can use the MAIR
+ * register directly
+ */
+u32 msm_iommu_get_mair0(void)
+{
+	unsigned int mair0;
+
+	RCP15_MAIR0(mair0);
+	return mair0;
+}
+
+u32 msm_iommu_get_mair1(void)
+{
+	unsigned int mair1;
+
+	RCP15_MAIR1(mair1);
+	return mair1;
+}
+#else
+/*
+ * However, If CONFIG_ARM_LPAE is not enabled but CONFIG_IOMMU_LPAE is enabled
+ * we'll just use the hard coded values directly..
+ */
+u32 msm_iommu_get_mair0(void)
+{
+	return MAIR0_VALUE;
+}
+
+u32 msm_iommu_get_mair1(void)
+{
+	return MAIR1_VALUE;
+}
+#endif
+
+#else
+#ifdef CONFIG_ARM_LPAE
+/*
+ * If CONFIG_ARM_LPAE is enabled AND CONFIG_IOMMU_LPAE is disabled
+ * we must use the hardcoded values.
+ */
+u32 msm_iommu_get_prrr(void)
+{
+	return PRRR_VALUE;
+}
+
+u32 msm_iommu_get_nmrr(void)
+{
+	return NMRR_VALUE;
+}
+#else
+/*
+ * If both CONFIG_ARM_LPAE AND CONFIG_IOMMU_LPAE are disabled
+ * we can use the registers directly.
+ */
+#define RCP15_PRRR(reg)		MRC(reg, p15, 0, c10, c2, 0)
+#define RCP15_NMRR(reg)		MRC(reg, p15, 0, c10, c2, 1)
+
+u32 msm_iommu_get_prrr(void)
+{
+	u32 prrr;
+
+	RCP15_PRRR(prrr);
+	return prrr;
+}
+
+u32 msm_iommu_get_nmrr(void)
+{
+	u32 nmrr;
+
+	RCP15_NMRR(nmrr);
+	return nmrr;
+}
+#endif
+#endif
diff --git a/drivers/iommu/msm_iommu_pagetable.c b/drivers/iommu/msm_iommu_pagetable.c
index 1e4bff8d4967..2815e0d6c646 100644
--- a/drivers/iommu/msm_iommu_pagetable.c
+++ b/drivers/iommu/msm_iommu_pagetable.c
@@ -618,12 +618,12 @@ void msm_iommu_pagetable_unmap_range(struct msm_iommu_pt *pt, unsigned int va,
 static int __init get_tex_class(int icp, int ocp, int mt, int nos)
 {
 	int i = 0;
-	unsigned int prrr = 0;
-	unsigned int nmrr = 0;
+	unsigned int prrr;
+	unsigned int nmrr;
 	int c_icp, c_ocp, c_mt, c_nos;
 
-	RCP15_PRRR(prrr);
-	RCP15_NMRR(nmrr);
+	prrr = msm_iommu_get_prrr();
+	nmrr = msm_iommu_get_nmrr();
 
 	for (i = 0; i < NUM_TEX_CLASS; i++) {
 		c_nos = PRRR_NOS(prrr, i);
diff --git a/drivers/iommu/msm_iommu_pagetable.h b/drivers/iommu/msm_iommu_pagetable.h
index a5ea31830b25..0e096317a176 100644
--- a/drivers/iommu/msm_iommu_pagetable.h
+++ b/drivers/iommu/msm_iommu_pagetable.h
@@ -13,17 +13,6 @@
 #ifndef __ARCH_ARM_MACH_MSM_IOMMU_PAGETABLE_H
 #define __ARCH_ARM_MACH_MSM_IOMMU_PAGETABLE_H
 
-#define MRC(reg, processor, op1, crn, crm, op2)                         \
-__asm__ __volatile__ (                                                  \
-"   mrc   "   #processor "," #op1 ", %0,"  #crn "," #crm "," #op2 "\n"  \
-: "=r" (reg))
-
-#define RCP15_PRRR(reg)   MRC(reg, p15, 0, c10, c2, 0)
-#define RCP15_NMRR(reg)   MRC(reg, p15, 0, c10, c2, 1)
-
-#define RCP15_MAIR0(reg)   MRC(reg, p15, 0, c10, c2, 0)
-#define RCP15_MAIR1(reg)   MRC(reg, p15, 0, c10, c2, 1)
-
 struct msm_iommu_pt;
 
 void msm_iommu_pagetable_init(void);
diff --git a/drivers/iommu/msm_iommu_pagetable_lpae.c b/drivers/iommu/msm_iommu_pagetable_lpae.c
new file mode 100644
index 000000000000..a08db5b66345
--- /dev/null
+++ b/drivers/iommu/msm_iommu_pagetable_lpae.c
@@ -0,0 +1,697 @@
+/* Copyright (c) 2013, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 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/errno.h>
+#include <linux/iommu.h>
+#include <linux/scatterlist.h>
+#include <linux/slab.h>
+
+#include <asm/cacheflush.h>
+
+#include <mach/msm_iommu_priv.h>
+#include <trace/events/kmem.h>
+#include "msm_iommu_pagetable.h"
+
+#define NUM_FL_PTE      4   /* First level */
+#define NUM_SL_PTE      512 /* Second level */
+#define NUM_TL_PTE      512 /* Third level */
+
+#define PTE_SIZE	8
+
+#define FL_ALIGN	0x20
+
+/* First-level/second-level page table bits */
+#define FL_OFFSET(va)           (((va) & 0xC0000000) >> 30)
+
+#define FLSL_BASE_MASK            (0xFFFFFFF000ULL)
+#define FLSL_1G_BLOCK_MASK        (0xFFC0000000ULL)
+#define FLSL_BLOCK_MASK           (0xFFFFE00000ULL)
+#define FLSL_TYPE_BLOCK           (1 << 0)
+#define FLSL_TYPE_TABLE           (3 << 0)
+#define FLSL_PTE_TYPE_MASK        (3 << 0)
+#define FLSL_APTABLE_RO           (2 << 61)
+#define FLSL_APTABLE_RW           (0 << 61)
+
+#define FL_TYPE_SECT              (2 << 0)
+#define FL_SUPERSECTION           (1 << 18)
+#define FL_AP0                    (1 << 10)
+#define FL_AP1                    (1 << 11)
+#define FL_AP2                    (1 << 15)
+#define FL_SHARED                 (1 << 16)
+#define FL_BUFFERABLE             (1 << 2)
+#define FL_CACHEABLE              (1 << 3)
+#define FL_TEX0                   (1 << 12)
+#define FL_NG                     (1 << 17)
+
+/* Second-level page table bits */
+#define SL_OFFSET(va)             (((va) & 0x3FE00000) >> 21)
+
+/* Third-level page table bits */
+#define TL_OFFSET(va)             (((va) & 0x1FF000) >> 12)
+
+#define TL_TYPE_PAGE              (3 << 0)
+#define TL_PAGE_MASK              (0xFFFFFFF000ULL)
+#define TL_ATTR_INDEX_MASK        (0x7)
+#define TL_ATTR_INDEX_SHIFT       (0x2)
+#define TL_NS                     (0x1 << 5)
+#define TL_AP_RO                  (0x3 << 6) /* Access Permission: R */
+#define TL_AP_RW                  (0x1 << 6) /* Access Permission: RW */
+#define TL_SH_ISH                 (0x3 << 8) /* Inner shareable */
+#define TL_SH_OSH                 (0x2 << 8) /* Outer shareable */
+#define TL_SH_NSH                 (0x0 << 8) /* Non-shareable */
+#define TL_AF                     (0x1 << 10)  /* Access Flag */
+#define TL_NG                     (0x1 << 11) /* Non-Global */
+#define TL_CH                     (0x1ULL << 52) /* Contiguous hint */
+#define TL_PXN                    (0x1ULL << 53) /* Privilege Execute Never */
+#define TL_XN                     (0x1ULL << 54) /* Execute Never */
+
+/* normal non-cacheable */
+#define PTE_MT_BUFFERABLE         (1 << 2)
+/* normal inner write-alloc */
+#define PTE_MT_WRITEALLOC         (7 << 2)
+
+#define PTE_MT_MASK               (7 << 2)
+
+#define FOLLOW_TO_NEXT_TABLE(pte) ((u64 *) __va(((*pte) & FLSL_BASE_MASK)))
+
+static void __msm_iommu_pagetable_unmap_range(struct msm_iommu_pt *pt, u32 va,
+					      u32 len, u32 silent);
+
+static inline void clean_pte(u64 *start, u64 *end,
+				s32 redirect)
+{
+	if (!redirect)
+		dmac_flush_range(start, end);
+}
+
+s32 msm_iommu_pagetable_alloc(struct msm_iommu_pt *pt)
+{
+	u32 size = PTE_SIZE * NUM_FL_PTE + FL_ALIGN;
+	phys_addr_t fl_table_phys;
+
+	pt->unaligned_fl_table = kzalloc(size, GFP_KERNEL);
+	if (!pt->unaligned_fl_table)
+		return -ENOMEM;
+
+
+	fl_table_phys = virt_to_phys(pt->unaligned_fl_table);
+	fl_table_phys = ALIGN(fl_table_phys, FL_ALIGN);
+	pt->fl_table = phys_to_virt(fl_table_phys);
+
+	clean_pte(pt->fl_table, pt->fl_table + NUM_FL_PTE, pt->redirect);
+	return 0;
+}
+
+void msm_iommu_pagetable_free(struct msm_iommu_pt *pt)
+{
+	s32 i;
+	u64 *fl_table = pt->fl_table;
+
+	for (i = 0; i < NUM_FL_PTE; ++i) {
+		if ((fl_table[i] & FLSL_TYPE_TABLE) == FLSL_TYPE_TABLE) {
+			u64 p = fl_table[i] & FLSL_BASE_MASK;
+			free_page((u32)phys_to_virt(p));
+		}
+	}
+	kfree(pt->unaligned_fl_table);
+	pt->unaligned_fl_table = 0;
+	pt->fl_table = 0;
+}
+
+#ifdef CONFIG_ARM_LPAE
+/*
+ * If LPAE is enabled in the ARM processor then just use the same
+ * cache policy as the kernel for the SMMU cached mappings.
+ */
+static inline u32 __get_cache_attr(void)
+{
+	return pgprot_kernel & PTE_MT_MASK;
+}
+#else
+/*
+ * If LPAE is NOT enabled in the ARM processor then hard code the policy.
+ * This is mostly for debugging so that we can enable SMMU LPAE without
+ * ARM CPU LPAE.
+ */
+static inline u32 __get_cache_attr(void)
+{
+	return PTE_MT_WRITEALLOC;
+}
+
+#endif
+
+/*
+ * Get the IOMMU attributes for the ARM LPAE long descriptor format page
+ * table entry bits. The only upper attribute bits we currently use is the
+ * contiguous bit which is set when we actually have a contiguous mapping.
+ * Lower attribute bits specify memory attributes and the protection
+ * (Read/Write/Execute).
+ */
+static inline void __get_attr(s32 prot, u64 *upper_attr, u64 *lower_attr)
+{
+	u32 attr_idx = PTE_MT_BUFFERABLE;
+
+	*upper_attr = 0;
+	*lower_attr = 0;
+
+	if (!(prot & (IOMMU_READ | IOMMU_WRITE))) {
+		prot |= IOMMU_READ | IOMMU_WRITE;
+		WARN_ONCE(1, "No attributes in iommu mapping; assuming RW\n");
+	}
+
+	if ((prot & IOMMU_WRITE) && !(prot & IOMMU_READ)) {
+		prot |= IOMMU_READ;
+		WARN_ONCE(1, "Write-only unsupported; falling back to RW\n");
+	}
+
+	if (prot & IOMMU_CACHE)
+		attr_idx = __get_cache_attr();
+
+	*lower_attr |= attr_idx;
+	*lower_attr |= TL_NG | TL_AF;
+	*lower_attr |= (prot & IOMMU_CACHE) ? TL_SH_ISH : TL_SH_NSH;
+	*lower_attr |= (prot & IOMMU_WRITE) ? TL_AP_RW : TL_AP_RO;
+}
+
+static inline u64 *make_second_level_tbl(s32 redirect, u64 *fl_pte)
+{
+	u64 *sl = (u64 *) __get_free_page(GFP_KERNEL);
+
+	if (!sl) {
+		pr_err("Could not allocate second level table\n");
+		goto fail;
+	}
+	memset(sl, 0, SZ_4K);
+	clean_pte(sl, sl + NUM_SL_PTE, redirect);
+
+	/* Leave APTable bits 0 to let next level decide access permissinons */
+	*fl_pte = (((phys_addr_t)__pa(sl)) & FLSL_BASE_MASK) | FLSL_TYPE_TABLE;
+	clean_pte(fl_pte, fl_pte + 1, redirect);
+fail:
+	return sl;
+}
+
+static inline u64 *make_third_level_tbl(s32 redirect, u64 *sl_pte)
+{
+	u64 *tl = (u64 *) __get_free_page(GFP_KERNEL);
+
+	if (!tl) {
+		pr_err("Could not allocate third level table\n");
+		goto fail;
+	}
+	memset(tl, 0, SZ_4K);
+	clean_pte(tl, tl + NUM_TL_PTE, redirect);
+
+	/* Leave APTable bits 0 to let next level decide access permissions */
+	*sl_pte = (((phys_addr_t)__pa(tl)) & FLSL_BASE_MASK) | FLSL_TYPE_TABLE;
+
+	clean_pte(sl_pte, sl_pte + 1, redirect);
+fail:
+	return tl;
+}
+
+static inline s32 tl_4k_map(u64 *tl_pte, phys_addr_t pa,
+			    u64 upper_attr, u64 lower_attr, s32 redirect)
+{
+	s32 ret = 0;
+
+	if (*tl_pte) {
+		ret = -EBUSY;
+		goto fail;
+	}
+
+	*tl_pte = upper_attr | (pa & TL_PAGE_MASK) | lower_attr | TL_TYPE_PAGE;
+	clean_pte(tl_pte, tl_pte + 1, redirect);
+fail:
+	return ret;
+}
+
+static inline s32 tl_64k_map(u64 *tl_pte, phys_addr_t pa,
+			     u64 upper_attr, u64 lower_attr, s32 redirect)
+{
+	s32 ret = 0;
+	s32 i;
+
+	for (i = 0; i < 16; ++i)
+		if (*(tl_pte+i)) {
+			ret = -EBUSY;
+			goto fail;
+		}
+
+	/* Add Contiguous hint TL_CH */
+	upper_attr |= TL_CH;
+
+	for (i = 0; i < 16; ++i)
+		*(tl_pte+i) = upper_attr | (pa & TL_PAGE_MASK) |
+			      lower_attr | TL_TYPE_PAGE;
+	clean_pte(tl_pte, tl_pte + 16, redirect);
+fail:
+	return ret;
+}
+
+static inline s32 sl_2m_map(u64 *sl_pte, phys_addr_t pa,
+			    u64 upper_attr, u64 lower_attr, s32 redirect)
+{
+	s32 ret = 0;
+
+	if (*sl_pte) {
+		ret = -EBUSY;
+		goto fail;
+	}
+
+	*sl_pte = upper_attr | (pa & FLSL_BLOCK_MASK) |
+		  lower_attr | FLSL_TYPE_BLOCK;
+	clean_pte(sl_pte, sl_pte + 1, redirect);
+fail:
+	return ret;
+}
+
+static inline s32 sl_32m_map(u64 *sl_pte, phys_addr_t pa,
+			     u64 upper_attr, u64 lower_attr, s32 redirect)
+{
+	s32 i;
+	s32 ret = 0;
+
+	for (i = 0; i < 16; ++i) {
+		if (*(sl_pte+i)) {
+			ret = -EBUSY;
+			goto fail;
+		}
+	}
+
+	/* Add Contiguous hint TL_CH */
+	upper_attr |= TL_CH;
+
+	for (i = 0; i < 16; ++i)
+		*(sl_pte+i) = upper_attr | (pa & FLSL_BLOCK_MASK) |
+			      lower_attr | FLSL_TYPE_BLOCK;
+	clean_pte(sl_pte, sl_pte + 16, redirect);
+fail:
+	return ret;
+}
+
+static inline s32 fl_1G_map(u64 *fl_pte, phys_addr_t pa,
+			    u64 upper_attr, u64 lower_attr, s32 redirect)
+{
+	s32 ret = 0;
+
+	if (*fl_pte) {
+		ret = -EBUSY;
+		goto fail;
+	}
+
+	*fl_pte = upper_attr | (pa & FLSL_1G_BLOCK_MASK) |
+		  lower_attr | FLSL_TYPE_BLOCK;
+
+	clean_pte(fl_pte, fl_pte + 1, redirect);
+fail:
+	return ret;
+}
+
+static inline s32 common_error_check(size_t len, u64 const *fl_table)
+{
+	s32 ret = 0;
+
+	if (len != SZ_1G && len != SZ_32M && len != SZ_2M &&
+	    len != SZ_64K && len != SZ_4K) {
+		pr_err("Bad length: %d\n", len);
+		ret = -EINVAL;
+	} else if (!fl_table) {
+		pr_err("Null page table\n");
+		ret = -EINVAL;
+	}
+	return ret;
+}
+
+static inline s32 handle_1st_lvl(u64 *fl_pte, phys_addr_t pa, u64 upper_attr,
+				     u64 lower_attr, size_t len, s32 redirect)
+{
+	s32 ret = 0;
+
+	if (len == SZ_1G) {
+		ret = fl_1G_map(fl_pte, pa, upper_attr, lower_attr, redirect);
+	} else {
+		/* Need second level page table */
+		if (*fl_pte == 0) {
+			if (make_second_level_tbl(redirect, fl_pte) == NULL)
+				ret = -ENOMEM;
+		}
+		if (!ret) {
+			if ((*fl_pte & FLSL_TYPE_TABLE) != FLSL_TYPE_TABLE)
+				ret = -EBUSY;
+		}
+	}
+	return ret;
+}
+
+static inline s32 handle_3rd_lvl(u64 *sl_pte, u32 va, phys_addr_t pa,
+				 u64 upper_attr, u64 lower_attr, size_t len,
+				 s32 redirect)
+{
+	u64 *tl_table;
+	u64 *tl_pte;
+	u32 tl_offset;
+	s32 ret = 0;
+
+	/* Need a 3rd level table */
+	if (*sl_pte == 0) {
+		if (make_third_level_tbl(redirect, sl_pte) == NULL) {
+			ret = -ENOMEM;
+			goto fail;
+		}
+	}
+
+	if ((*sl_pte & FLSL_TYPE_TABLE) != FLSL_TYPE_TABLE) {
+		ret = -EBUSY;
+		goto fail;
+	}
+
+	tl_table = FOLLOW_TO_NEXT_TABLE(sl_pte);
+	tl_offset = TL_OFFSET(va);
+	tl_pte = tl_table + tl_offset;
+
+	if (len == SZ_64K)
+		ret = tl_64k_map(tl_pte, pa, upper_attr, lower_attr, redirect);
+	else
+		ret = tl_4k_map(tl_pte, pa, upper_attr, lower_attr, redirect);
+
+fail:
+	return ret;
+}
+
+int msm_iommu_pagetable_map(struct msm_iommu_pt *pt, unsigned long va,
+			    phys_addr_t pa, size_t len, int prot)
+{
+	u64 *fl_pte;
+	u32 fl_offset;
+	u32 sl_offset;
+	u64 *sl_table;
+	u64 *sl_pte;
+	u64 upper_attr;
+	u64 lower_attr;
+	s32 ret;
+	u32 redirect = pt->redirect;
+
+	ret = common_error_check(len, pt->fl_table);
+	if (ret)
+		goto fail;
+
+	if (!pt->fl_table) {
+		pr_err("Null page table\n");
+		ret = -EINVAL;
+		goto fail;
+	}
+
+	__get_attr(prot, &upper_attr, &lower_attr);
+
+	fl_offset = FL_OFFSET(va);
+	fl_pte = pt->fl_table + fl_offset;
+
+	ret = handle_1st_lvl(fl_pte, pa, upper_attr, lower_attr, len, redirect);
+	if (ret)
+		goto fail;
+
+	sl_table = FOLLOW_TO_NEXT_TABLE(fl_pte);
+	sl_offset = SL_OFFSET(va);
+	sl_pte = sl_table + sl_offset;
+
+	if (len == SZ_32M)
+		ret = sl_32m_map(sl_pte, pa, upper_attr, lower_attr, redirect);
+	else if (len == SZ_2M)
+		ret = sl_2m_map(sl_pte, pa, upper_attr, lower_attr, redirect);
+	else if (len == SZ_64K || len == SZ_4K)
+		ret = handle_3rd_lvl(sl_pte, va, pa, upper_attr, lower_attr,
+				     len, redirect);
+
+fail:
+	return ret;
+}
+
+static u32 free_table(u64 *prev_level_pte, u64 *table, u32 table_len,
+		       s32 redirect, u32 check)
+{
+	u32 i;
+	u32 used = 0;
+
+	if (check) {
+		for (i = 0; i < table_len; ++i)
+			if (table[i]) {
+				used = 1;
+				break;
+			}
+	}
+	if (!used) {
+		free_page((u32)table);
+		*prev_level_pte = 0;
+		clean_pte(prev_level_pte, prev_level_pte + 1, redirect);
+	}
+	return !used;
+}
+
+static void fl_1G_unmap(u64 *fl_pte, s32 redirect)
+{
+	*fl_pte = 0;
+	clean_pte(fl_pte, fl_pte + 1, redirect);
+}
+
+size_t msm_iommu_pagetable_unmap(struct msm_iommu_pt *pt, unsigned long va,
+				size_t len)
+{
+	msm_iommu_pagetable_unmap_range(pt, va, len);
+	return len;
+}
+
+static phys_addr_t get_phys_addr(struct scatterlist *sg)
+{
+	/*
+	 * Try sg_dma_address first so that we can
+	 * map carveout regions that do not have a
+	 * struct page associated with them.
+	 */
+	phys_addr_t pa = sg_dma_address(sg);
+	if (pa == 0)
+		pa = sg_phys(sg);
+	return pa;
+}
+
+static inline s32 is_fully_aligned(u32 va, phys_addr_t pa, size_t len,
+				   s32 align)
+{
+	return  IS_ALIGNED(va | pa, align) && (len >= align);
+}
+
+s32 msm_iommu_pagetable_map_range(struct msm_iommu_pt *pt, u32 va,
+		       struct scatterlist *sg, u32 len, s32 prot)
+{
+	phys_addr_t pa;
+	u32 offset = 0;
+	u64 *fl_pte;
+	u64 *sl_pte;
+	u32 fl_offset;
+	u32 sl_offset;
+	u64 *sl_table = NULL;
+	u32 chunk_size, chunk_offset = 0;
+	s32 ret = 0;
+	u64 up_at;
+	u64 lo_at;
+	u32 redirect = pt->redirect;
+	unsigned int start_va = va;
+
+	BUG_ON(len & (SZ_4K - 1));
+
+	if (!pt->fl_table) {
+		pr_err("Null page table\n");
+		ret = -EINVAL;
+		goto fail;
+	}
+
+	__get_attr(prot, &up_at, &lo_at);
+
+	pa = get_phys_addr(sg);
+
+	while (offset < len) {
+		u32 chunk_left = sg->length - chunk_offset;
+
+		fl_offset = FL_OFFSET(va);
+		fl_pte = pt->fl_table + fl_offset;
+
+		chunk_size = SZ_4K;
+		if (is_fully_aligned(va, pa, chunk_left, SZ_1G))
+			chunk_size = SZ_1G;
+		else if (is_fully_aligned(va, pa, chunk_left, SZ_32M))
+			chunk_size = SZ_32M;
+		else if (is_fully_aligned(va, pa, chunk_left, SZ_2M))
+			chunk_size = SZ_2M;
+		else if (is_fully_aligned(va, pa, chunk_left, SZ_64K))
+			chunk_size = SZ_64K;
+
+		trace_iommu_map_range(va, pa, sg->length, chunk_size);
+
+		ret = handle_1st_lvl(fl_pte, pa, up_at, lo_at,
+				     chunk_size, redirect);
+		if (ret)
+			goto fail;
+
+		sl_table = FOLLOW_TO_NEXT_TABLE(fl_pte);
+		sl_offset = SL_OFFSET(va);
+		sl_pte = sl_table + sl_offset;
+
+		if (chunk_size == SZ_32M)
+			ret = sl_32m_map(sl_pte, pa, up_at, lo_at, redirect);
+		else if (chunk_size == SZ_2M)
+			ret = sl_2m_map(sl_pte, pa, up_at, lo_at, redirect);
+		else if (chunk_size == SZ_64K || chunk_size == SZ_4K)
+			ret = handle_3rd_lvl(sl_pte, va, pa, up_at, lo_at,
+					     chunk_size, redirect);
+		if (ret)
+			goto fail;
+
+		offset += chunk_size;
+		chunk_offset += chunk_size;
+		va += chunk_size;
+		pa += chunk_size;
+
+		if (chunk_offset >= sg->length && offset < len) {
+			chunk_offset = 0;
+			sg = sg_next(sg);
+			pa = get_phys_addr(sg);
+		}
+	}
+fail:
+	if (ret && offset > 0)
+		__msm_iommu_pagetable_unmap_range(pt, start_va, offset, 1);
+	return ret;
+}
+
+void msm_iommu_pagetable_unmap_range(struct msm_iommu_pt *pt, u32 va, u32 len)
+{
+	__msm_iommu_pagetable_unmap_range(pt, va, len, 0);
+}
+
+static void __msm_iommu_pagetable_unmap_range(struct msm_iommu_pt *pt, u32 va,
+					      u32 len, u32 silent)
+{
+	u32 offset = 0;
+	u64 *fl_pte;
+	u64 *sl_pte;
+	u64 *tl_pte;
+	u32 fl_offset;
+	u32 sl_offset;
+	u64 *sl_table;
+	u64 *tl_table;
+	u32 sl_start, sl_end;
+	u32 tl_start, tl_end;
+	u32 redirect = pt->redirect;
+
+	BUG_ON(len & (SZ_4K - 1));
+
+	while (offset < len) {
+		u32 entries;
+		u32 check;
+		u32 left_to_unmap = len - offset;
+		u32 type;
+
+		fl_offset = FL_OFFSET(va);
+		fl_pte = pt->fl_table + fl_offset;
+
+		if (*fl_pte == 0) {
+			if (!silent)
+				pr_err("First level PTE is 0 at index 0x%x (offset: 0x%x)\n",
+					fl_offset, offset);
+			return;
+		}
+		type = *fl_pte & FLSL_PTE_TYPE_MASK;
+
+		if (type == FLSL_TYPE_BLOCK) {
+			fl_1G_unmap(fl_pte, redirect);
+			va += SZ_1G;
+			offset += SZ_1G;
+		} else if (type == FLSL_TYPE_TABLE) {
+			sl_table = FOLLOW_TO_NEXT_TABLE(fl_pte);
+			sl_offset = SL_OFFSET(va);
+			sl_pte = sl_table + sl_offset;
+			type = *sl_pte & FLSL_PTE_TYPE_MASK;
+
+			if (type == FLSL_TYPE_BLOCK) {
+				sl_start = sl_offset;
+				sl_end = (left_to_unmap / SZ_2M) + sl_start;
+
+				if (sl_end > NUM_TL_PTE)
+					sl_end = NUM_TL_PTE;
+
+				entries = sl_end - sl_start;
+
+				memset(sl_table + sl_start, 0,
+				       entries * sizeof(*sl_pte));
+
+				clean_pte(sl_table + sl_start,
+					  sl_table + sl_end, redirect);
+
+				/* If we just unmapped the whole table, don't
+				 * bother seeing if there are still used
+				 * entries left.
+				 */
+				check = ((sl_end - sl_start) != NUM_SL_PTE);
+
+				free_table(fl_pte, sl_table, NUM_SL_PTE,
+					   redirect, check);
+
+				offset += entries * SZ_2M;
+				va += entries * SZ_2M;
+			} else if (type == FLSL_TYPE_TABLE) {
+				u32 tbl_freed;
+
+				tl_start = TL_OFFSET(va);
+				tl_table =  FOLLOW_TO_NEXT_TABLE(sl_pte);
+				tl_end = (left_to_unmap / SZ_4K) + tl_start;
+
+				if (tl_end > NUM_TL_PTE)
+					tl_end = NUM_TL_PTE;
+
+				entries = tl_end - tl_start;
+
+				memset(tl_table + tl_start, 0,
+				       entries * sizeof(*tl_pte));
+
+				clean_pte(tl_table + tl_start,
+					  tl_table + tl_end, redirect);
+
+				/* If we just unmapped the whole table, don't
+				 * bother seeing if there are still used
+				 * entries left.
+				 */
+				check = entries != NUM_TL_PTE;
+
+				tbl_freed = free_table(sl_pte, tl_table,
+						NUM_TL_PTE, redirect, check);
+				if (tbl_freed)
+					free_table(fl_pte, sl_table, NUM_SL_PTE,
+						   redirect, 1);
+
+				offset += entries * SZ_4K;
+				va += entries * SZ_4K;
+			} else {
+				if (!silent)
+					pr_err("Second level PTE (0x%llx) is invalid at index 0x%x (offset: 0x%x)\n",
+						*sl_pte, sl_offset, offset);
+			}
+		} else {
+			if (!silent)
+				pr_err("First level PTE (0x%llx) is invalid at index 0x%x (offset: 0x%x)\n",
+					*fl_pte, fl_offset, offset);
+		}
+	}
+}
+
+void __init msm_iommu_pagetable_init(void)
+{
+}