21 files changed, 238 insertions, 137 deletions

diff --git a/arch/arm/include/asm/hardware/cache-l2x0.h b/arch/arm/include/asm/hardware/cache-l2x0.h
index cc42d5fdee17..5aeec1e1735c 100644
--- a/arch/arm/include/asm/hardware/cache-l2x0.h
+++ b/arch/arm/include/asm/hardware/cache-l2x0.h
@@ -59,7 +59,17 @@
 #define L2X0_CACHE_ID_PART_MASK         (0xf << 6)
 #define L2X0_CACHE_ID_PART_L210         (1 << 6)
 #define L2X0_CACHE_ID_PART_L310         (3 << 6)
-#define L2X0_AUX_CTRL_WAY_SIZE_MASK     (0x3 << 17)
+
+#define L2X0_AUX_CTRL_MASK                      0xc0000fff
+#define L2X0_AUX_CTRL_ASSOCIATIVITY_SHIFT       16
+#define L2X0_AUX_CTRL_WAY_SIZE_SHIFT            17
+#define L2X0_AUX_CTRL_WAY_SIZE_MASK             (0x3 << 17)
+#define L2X0_AUX_CTRL_SHARE_OVERRIDE_SHIFT      22
+#define L2X0_AUX_CTRL_NS_LOCKDOWN_SHIFT         26
+#define L2X0_AUX_CTRL_NS_INT_CTRL_SHIFT         27
+#define L2X0_AUX_CTRL_DATA_PREFETCH_SHIFT       28
+#define L2X0_AUX_CTRL_INSTR_PREFETCH_SHIFT      29
+#define L2X0_AUX_CTRL_EARLY_BRESP_SHIFT         30
 
 #ifndef __ASSEMBLY__
 extern void __init l2x0_init(void __iomem *base, __u32 aux_val, __u32 aux_mask);
diff --git a/arch/arm/include/asm/io.h b/arch/arm/include/asm/io.h
index 815efa2d4e07..20e0f7c9e03e 100644
--- a/arch/arm/include/asm/io.h
+++ b/arch/arm/include/asm/io.h
@@ -241,18 +241,15 @@ extern void _memset_io(volatile void __iomem *, int, size_t);
  *
  */
 #ifndef __arch_ioremap
-#define ioremap(cookie,size)            __arm_ioremap(cookie, size, MT_DEVICE)
-#define ioremap_nocache(cookie,size)    __arm_ioremap(cookie, size, MT_DEVICE)
-#define ioremap_cached(cookie,size)     __arm_ioremap(cookie, size, MT_DEVICE_CACHED)
-#define ioremap_wc(cookie,size)         __arm_ioremap(cookie, size, MT_DEVICE_WC)
-#define iounmap(cookie)                 __iounmap(cookie)
-#else
+#define __arch_ioremap                  __arm_ioremap
+#define __arch_iounmap                  __iounmap
+#endif
+
 #define ioremap(cookie,size)            __arch_ioremap((cookie), (size), MT_DEVICE)
 #define ioremap_nocache(cookie,size)    __arch_ioremap((cookie), (size), MT_DEVICE)
 #define ioremap_cached(cookie,size)     __arch_ioremap((cookie), (size), MT_DEVICE_CACHED)
 #define ioremap_wc(cookie,size)         __arch_ioremap((cookie), (size), MT_DEVICE_WC)
-#define iounmap(cookie)                 __arch_iounmap(cookie)
-#endif
+#define iounmap                         __arch_iounmap
 
 /*
  * io{read,write}{8,16,32} macros
diff --git a/arch/arm/mach-davinci/include/mach/io.h b/arch/arm/mach-davinci/include/mach/io.h
index 62b0a90309ad..d1b954955c12 100644
--- a/arch/arm/mach-davinci/include/mach/io.h
+++ b/arch/arm/mach-davinci/include/mach/io.h
@@ -22,8 +22,8 @@
 #define __mem_isa(a)            (a)
 
 #ifndef __ASSEMBLER__
-#define __arch_ioremap(p, s, t) davinci_ioremap(p, s, t)
-#define __arch_iounmap(v)       davinci_iounmap(v)
+#define __arch_ioremap          davinci_ioremap
+#define __arch_iounmap          davinci_iounmap
 
 void __iomem *davinci_ioremap(unsigned long phys, size_t size,
                               unsigned int type);
diff --git a/arch/arm/mach-iop13xx/include/mach/io.h b/arch/arm/mach-iop13xx/include/mach/io.h
index a6e0f9e6ddcf..dffb234bb967 100644
--- a/arch/arm/mach-iop13xx/include/mach/io.h
+++ b/arch/arm/mach-iop13xx/include/mach/io.h
@@ -35,7 +35,7 @@ extern u32 iop13xx_atux_mem_base;
 extern size_t iop13xx_atue_mem_size;
 extern size_t iop13xx_atux_mem_size;
 
-#define __arch_ioremap(a, s, f) __iop13xx_ioremap(a, s, f)
-#define __arch_iounmap(a)        __iop13xx_iounmap(a)
+#define __arch_ioremap  __iop13xx_ioremap
+#define __arch_iounmap  __iop13xx_iounmap
 
 #endif
diff --git a/arch/arm/mach-iop32x/include/mach/io.h b/arch/arm/mach-iop32x/include/mach/io.h
index 339e5854728b..059c783ce0b2 100644
--- a/arch/arm/mach-iop32x/include/mach/io.h
+++ b/arch/arm/mach-iop32x/include/mach/io.h
@@ -21,7 +21,7 @@ extern void __iop3xx_iounmap(void __iomem *addr);
 #define __io(p)         ((void __iomem *)IOP3XX_PCI_IO_PHYS_TO_VIRT(p))
 #define __mem_pci(a)            (a)
 
-#define __arch_ioremap(a, s, f) __iop3xx_ioremap(a, s, f)
-#define __arch_iounmap(a)        __iop3xx_iounmap(a)
+#define __arch_ioremap  __iop3xx_ioremap
+#define __arch_iounmap  __iop3xx_iounmap
 
 #endif
diff --git a/arch/arm/mach-iop33x/include/mach/io.h b/arch/arm/mach-iop33x/include/mach/io.h
index e99a7ed6d050..39e893e97c21 100644
--- a/arch/arm/mach-iop33x/include/mach/io.h
+++ b/arch/arm/mach-iop33x/include/mach/io.h
@@ -21,7 +21,7 @@ extern void __iop3xx_iounmap(void __iomem *addr);
 #define __io(p)         ((void __iomem *)IOP3XX_PCI_IO_PHYS_TO_VIRT(p))
 #define __mem_pci(a)            (a)
 
-#define __arch_ioremap(a, s, f) __iop3xx_ioremap(a, s, f)
-#define __arch_iounmap(a)        __iop3xx_iounmap(a)
+#define __arch_ioremap  __iop3xx_ioremap
+#define __arch_iounmap  __iop3xx_iounmap
 
 #endif
diff --git a/arch/arm/mach-ixp23xx/include/mach/io.h b/arch/arm/mach-ixp23xx/include/mach/io.h
index fd9ef8e519f7..a1749d0fd896 100644
--- a/arch/arm/mach-ixp23xx/include/mach/io.h
+++ b/arch/arm/mach-ixp23xx/include/mach/io.h
@@ -45,8 +45,8 @@ ixp23xx_iounmap(void __iomem *addr)
         __iounmap(addr);
 }
 
-#define __arch_ioremap(a,s,f)   ixp23xx_ioremap(a,s,f)
-#define __arch_iounmap(a)       ixp23xx_iounmap(a)
+#define __arch_ioremap  ixp23xx_ioremap
+#define __arch_iounmap  ixp23xx_iounmap
 
 
 #endif
diff --git a/arch/arm/mach-ixp4xx/include/mach/io.h b/arch/arm/mach-ixp4xx/include/mach/io.h
index de274a1f19d7..57b5410c31f4 100644
--- a/arch/arm/mach-ixp4xx/include/mach/io.h
+++ b/arch/arm/mach-ixp4xx/include/mach/io.h
@@ -74,8 +74,8 @@ static inline void __indirect_iounmap(void __iomem *addr)
                 __iounmap(addr);
 }
 
-#define __arch_ioremap(a, s, f)         __indirect_ioremap(a, s, f)
-#define __arch_iounmap(a)               __indirect_iounmap(a)
+#define __arch_ioremap                  __indirect_ioremap
+#define __arch_iounmap                  __indirect_iounmap
 
 #define writeb(v, p)                    __indirect_writeb(v, p)
 #define writew(v, p)                    __indirect_writew(v, p)
diff --git a/arch/arm/mach-kirkwood/include/mach/io.h b/arch/arm/mach-kirkwood/include/mach/io.h
index 44e8be04f259..1aaddc364f2e 100644
--- a/arch/arm/mach-kirkwood/include/mach/io.h
+++ b/arch/arm/mach-kirkwood/include/mach/io.h
@@ -42,8 +42,8 @@ __arch_iounmap(void __iomem *addr)
                 __iounmap(addr);
 }
 
-#define __arch_ioremap(p, s, m) __arch_ioremap(p, s, m)
-#define __arch_iounmap(a)       __arch_iounmap(a)
+#define __arch_ioremap          __arch_ioremap
+#define __arch_iounmap          __arch_iounmap
 #define __io(a)                 __io(a)
 #define __mem_pci(a)            (a)
 
diff --git a/arch/arm/mach-omap2/mailbox.c b/arch/arm/mach-omap2/mailbox.c
index 40ddecab93a9..394413dc7deb 100644
--- a/arch/arm/mach-omap2/mailbox.c
+++ b/arch/arm/mach-omap2/mailbox.c
@@ -281,7 +281,7 @@ static struct omap_mbox_ops omap2_mbox_ops = {
 
 /* FIXME: the following structs should be filled automatically by the user id */
 
-#if defined(CONFIG_ARCH_OMAP3430) || defined(CONFIG_ARCH_OMAP2420)
+#if defined(CONFIG_ARCH_OMAP3) || defined(CONFIG_ARCH_OMAP2)
 /* DSP */
 static struct omap_mbox2_priv omap2_mbox_dsp_priv = {
         .tx_fifo = {
@@ -306,7 +306,7 @@ struct omap_mbox mbox_dsp_info = {
 };
 #endif
 
-#if defined(CONFIG_ARCH_OMAP3430)
+#if defined(CONFIG_ARCH_OMAP3)
 struct omap_mbox *omap3_mboxes[] = { &mbox_dsp_info, NULL };
 #endif
 
@@ -394,15 +394,19 @@ static int __devinit omap2_mbox_probe(struct platform_device *pdev)
 
         if (false)
                 ;
-#if defined(CONFIG_ARCH_OMAP3430)
-        else if (cpu_is_omap3430()) {
+#if defined(CONFIG_ARCH_OMAP3)
+        else if (cpu_is_omap34xx()) {
                 list = omap3_mboxes;
 
                 list[0]->irq = platform_get_irq_byname(pdev, "dsp");
         }
 #endif
-#if defined(CONFIG_ARCH_OMAP2420)
-        else if (cpu_is_omap2420()) {
+#if defined(CONFIG_ARCH_OMAP2)
+        else if (cpu_is_omap2430()) {
+                list = omap2_mboxes;
+
+                list[0]->irq = platform_get_irq_byname(pdev, "dsp");
+        } else if (cpu_is_omap2420()) {
                 list = omap2_mboxes;
 
                 list[0]->irq = platform_get_irq_byname(pdev, "dsp");
@@ -432,9 +436,8 @@ static int __devinit omap2_mbox_probe(struct platform_device *pdev)
                 iounmap(mbox_base);
                 return ret;
         }
-        return 0;
 
-        return ret;
+        return 0;
 }
 
 static int __devexit omap2_mbox_remove(struct platform_device *pdev)
diff --git a/arch/arm/mach-omap2/omap-iommu.c b/arch/arm/mach-omap2/omap-iommu.c
index f5a1aad1a5c0..3fc5dc7233da 100644
--- a/arch/arm/mach-omap2/omap-iommu.c
+++ b/arch/arm/mach-omap2/omap-iommu.c
@@ -33,9 +33,11 @@ static struct iommu_device omap3_devices[] = {
                         .name = "isp",
                         .nr_tlb_entries = 8,
                         .clk_name = "cam_ick",
+                        .da_start = 0x0,
+                        .da_end = 0xFFFFF000,
                 },
         },
-#if defined(CONFIG_MPU_BRIDGE_IOMMU)
+#if defined(CONFIG_OMAP_IOMMU_IVA2)
         {
                 .base = 0x5d000000,
                 .irq = 28,
@@ -43,6 +45,8 @@ static struct iommu_device omap3_devices[] = {
                         .name = "iva2",
                         .nr_tlb_entries = 32,
                         .clk_name = "iva2_ck",
+                        .da_start = 0x11000000,
+                        .da_end = 0xFFFFF000,
                 },
         },
 #endif
@@ -64,6 +68,8 @@ static struct iommu_device omap4_devices[] = {
                         .name = "ducati",
                         .nr_tlb_entries = 32,
                         .clk_name = "ducati_ick",
+                        .da_start = 0x0,
+                        .da_end = 0xFFFFF000,
                 },
         },
 #if defined(CONFIG_MPU_TESLA_IOMMU)
@@ -74,6 +80,8 @@ static struct iommu_device omap4_devices[] = {
                         .name = "tesla",
                         .nr_tlb_entries = 32,
                         .clk_name = "tesla_ick",
+                        .da_start = 0x0,
+                        .da_end = 0xFFFFF000,
                 },
         },
 #endif
diff --git a/arch/arm/mach-omap2/omap4-common.c b/arch/arm/mach-omap2/omap4-common.c
index 2f895553e6a8..e7a9b7f13b52 100644
--- a/arch/arm/mach-omap2/omap4-common.c
+++ b/arch/arm/mach-omap2/omap4-common.c
@@ -53,6 +53,8 @@ static void omap4_l2x0_disable(void)
 
 static int __init omap_l2_cache_init(void)
 {
+        u32 aux_ctrl = 0;
+
         /*
          * To avoid code running on other OMAPs in
          * multi-omap builds
@@ -64,18 +66,32 @@ static int __init omap_l2_cache_init(void)
         l2cache_base = ioremap(OMAP44XX_L2CACHE_BASE, SZ_4K);
         BUG_ON(!l2cache_base);
 
-        /* Enable PL310 L2 Cache controller */
-        omap_smc1(0x102, 0x1);
-
         /*
          * 16-way associativity, parity disabled
          * Way size - 32KB (es1.0)
          * Way size - 64KB (es2.0 +)
          */
-        if (omap_rev() == OMAP4430_REV_ES1_0)
-                l2x0_init(l2cache_base, 0x0e050000, 0xc0000fff);
-        else
-                l2x0_init(l2cache_base, 0x0e070000, 0xc0000fff);
+        aux_ctrl = ((1 << L2X0_AUX_CTRL_ASSOCIATIVITY_SHIFT) |
+                        (0x1 << 25) |
+                        (0x1 << L2X0_AUX_CTRL_NS_LOCKDOWN_SHIFT) |
+                        (0x1 << L2X0_AUX_CTRL_NS_INT_CTRL_SHIFT));
+
+        if (omap_rev() == OMAP4430_REV_ES1_0) {
+                aux_ctrl |= 0x2 << L2X0_AUX_CTRL_WAY_SIZE_SHIFT;
+        } else {
+                aux_ctrl |= ((0x3 << L2X0_AUX_CTRL_WAY_SIZE_SHIFT) |
+                        (1 << L2X0_AUX_CTRL_SHARE_OVERRIDE_SHIFT) |
+                        (1 << L2X0_AUX_CTRL_DATA_PREFETCH_SHIFT) |
+                        (1 << L2X0_AUX_CTRL_INSTR_PREFETCH_SHIFT) |
+                        (1 << L2X0_AUX_CTRL_EARLY_BRESP_SHIFT));
+        }
+        if (omap_rev() != OMAP4430_REV_ES1_0)
+                omap_smc1(0x109, aux_ctrl);
+
+        /* Enable PL310 L2 Cache controller */
+        omap_smc1(0x102, 0x1);
+
+        l2x0_init(l2cache_base, aux_ctrl, L2X0_AUX_CTRL_MASK);
 
         /*
          * Override default outer_cache.disable with a OMAP4
diff --git a/arch/arm/mach-orion5x/include/mach/io.h b/arch/arm/mach-orion5x/include/mach/io.h
index c47b033bd999..c5196101a237 100644
--- a/arch/arm/mach-orion5x/include/mach/io.h
+++ b/arch/arm/mach-orion5x/include/mach/io.h
@@ -38,8 +38,8 @@ __arch_iounmap(void __iomem *addr)
                 __iounmap(addr);
 }
 
-#define __arch_ioremap(p, s, m) __arch_ioremap(p, s, m)
-#define __arch_iounmap(a)       __arch_iounmap(a)
+#define __arch_ioremap          __arch_ioremap
+#define __arch_iounmap          __arch_iounmap
 #define __io(a)                 __typesafe_io(a)
 #define __mem_pci(a)            (a)
 
diff --git a/arch/arm/mach-tegra/include/mach/io.h b/arch/arm/mach-tegra/include/mach/io.h
index f0981b1ac59e..4cea2230c8dc 100644
--- a/arch/arm/mach-tegra/include/mach/io.h
+++ b/arch/arm/mach-tegra/include/mach/io.h
@@ -65,8 +65,8 @@
 
 #ifndef __ASSEMBLER__
 
-#define __arch_ioremap(p, s, t) tegra_ioremap(p, s, t)
-#define __arch_iounmap(v)       tegra_iounmap(v)
+#define __arch_ioremap          tegra_ioremap
+#define __arch_iounmap          tegra_iounmap
 
 void __iomem *tegra_ioremap(unsigned long phys, size_t size, unsigned int type);
 void tegra_iounmap(volatile void __iomem *addr);
diff --git a/arch/arm/plat-omap/Kconfig b/arch/arm/plat-omap/Kconfig
index 92c5bb7909f5..5e63e5069e0d 100644
--- a/arch/arm/plat-omap/Kconfig
+++ b/arch/arm/plat-omap/Kconfig
@@ -109,6 +109,9 @@ config OMAP_IOMMU_DEBUG
 
          Say N unless you know you need this.
 
+config OMAP_IOMMU_IVA2
+        bool
+
 choice
         prompt "System timer"
         default OMAP_32K_TIMER if !ARCH_OMAP15XX
diff --git a/arch/arm/plat-omap/include/plat/io.h b/arch/arm/plat-omap/include/plat/io.h
index 128b549c2796..204865f91d93 100644
--- a/arch/arm/plat-omap/include/plat/io.h
+++ b/arch/arm/plat-omap/include/plat/io.h
@@ -294,8 +294,8 @@ static inline void omap44xx_map_common_io(void)
 extern void omap2_init_common_hw(struct omap_sdrc_params *sdrc_cs0,
                                  struct omap_sdrc_params *sdrc_cs1);
 
-#define __arch_ioremap(p,s,t)   omap_ioremap(p,s,t)
-#define __arch_iounmap(v)       omap_iounmap(v)
+#define __arch_ioremap  omap_ioremap
+#define __arch_iounmap  omap_iounmap
 
 void __iomem *omap_ioremap(unsigned long phys, size_t size, unsigned int type);
 void omap_iounmap(volatile void __iomem *addr);
diff --git a/arch/arm/plat-omap/include/plat/iommu.h b/arch/arm/plat-omap/include/plat/iommu.h
index 33c7d41cb6a5..69230d685538 100644
--- a/arch/arm/plat-omap/include/plat/iommu.h
+++ b/arch/arm/plat-omap/include/plat/iommu.h
@@ -50,6 +50,8 @@ struct iommu {
         int (*isr)(struct iommu *obj);
 
         void *ctx; /* iommu context: registres saved area */
+        u32 da_start;
+        u32 da_end;
 };
 
 struct cr_regs {
@@ -103,6 +105,8 @@ struct iommu_platform_data {
         const char *name;
         const char *clk_name;
         const int nr_tlb_entries;
+        u32 da_start;
+        u32 da_end;
 };
 
 #if defined(CONFIG_ARCH_OMAP1)
@@ -152,6 +156,7 @@ extern void flush_iotlb_all(struct iommu *obj);
 extern int iopgtable_store_entry(struct iommu *obj, struct iotlb_entry *e);
 extern size_t iopgtable_clear_entry(struct iommu *obj, u32 iova);
 
+extern int iommu_set_da_range(struct iommu *obj, u32 start, u32 end);
 extern struct iommu *iommu_get(const char *name);
 extern void iommu_put(struct iommu *obj);
 
diff --git a/arch/arm/plat-omap/include/plat/mailbox.h b/arch/arm/plat-omap/include/plat/mailbox.h
index 997656552109..cc3921e9059c 100644
--- a/arch/arm/plat-omap/include/plat/mailbox.h
+++ b/arch/arm/plat-omap/include/plat/mailbox.h
@@ -46,8 +46,8 @@ struct omap_mbox_queue {
         struct kfifo            fifo;
         struct work_struct      work;
         struct tasklet_struct   tasklet;
-        int     (*callback)(void *);
         struct omap_mbox        *mbox;
+        bool full;
 };
 
 struct omap_mbox {
@@ -57,13 +57,15 @@ struct omap_mbox {
         struct omap_mbox_ops    *ops;
         struct device           *dev;
         void                    *priv;
+        int                     use_count;
+        struct blocking_notifier_head   notifier;
 };
 
 int omap_mbox_msg_send(struct omap_mbox *, mbox_msg_t msg);
 void omap_mbox_init_seq(struct omap_mbox *);
 
-struct omap_mbox *omap_mbox_get(const char *);
-void omap_mbox_put(struct omap_mbox *);
+struct omap_mbox *omap_mbox_get(const char *, struct notifier_block *nb);
+void omap_mbox_put(struct omap_mbox *mbox, struct notifier_block *nb);
 
 int omap_mbox_register(struct device *parent, struct omap_mbox **);
 int omap_mbox_unregister(void);
diff --git a/arch/arm/plat-omap/iommu.c b/arch/arm/plat-omap/iommu.c
index 6cd151b31bc5..b1107c08da56 100644
--- a/arch/arm/plat-omap/iommu.c
+++ b/arch/arm/plat-omap/iommu.c
@@ -830,6 +830,28 @@ static int device_match_by_alias(struct device *dev, void *data)
 }
 
 /**
+ * iommu_set_da_range - Set a valid device address range
+ * @obj:                target iommu
+ * @start               Start of valid range
+ * @end                 End of valid range
+ **/
+int iommu_set_da_range(struct iommu *obj, u32 start, u32 end)
+{
+
+        if (!obj)
+                return -EFAULT;
+
+        if (end < start || !PAGE_ALIGN(start | end))
+                return -EINVAL;
+
+        obj->da_start = start;
+        obj->da_end = end;
+
+        return 0;
+}
+EXPORT_SYMBOL_GPL(iommu_set_da_range);
+
+/**
  * iommu_get - Get iommu handler
  * @name:       target iommu name
  **/
@@ -922,6 +944,8 @@ static int __devinit omap_iommu_probe(struct platform_device *pdev)
         obj->name = pdata->name;
         obj->dev = &pdev->dev;
         obj->ctx = (void *)obj + sizeof(*obj);
+        obj->da_start = pdata->da_start;
+        obj->da_end = pdata->da_end;
 
         mutex_init(&obj->iommu_lock);
         mutex_init(&obj->mmap_lock);
diff --git a/arch/arm/plat-omap/iovmm.c b/arch/arm/plat-omap/iovmm.c
index 8ce0de247c71..6dc1296c8c77 100644
--- a/arch/arm/plat-omap/iovmm.c
+++ b/arch/arm/plat-omap/iovmm.c
@@ -87,35 +87,43 @@ static size_t sgtable_len(const struct sg_table *sgt)
 }
 #define sgtable_ok(x)   (!!sgtable_len(x))
 
+static unsigned max_alignment(u32 addr)
+{
+        int i;
+        unsigned pagesize[] = { SZ_16M, SZ_1M, SZ_64K, SZ_4K, };
+        for (i = 0; i < ARRAY_SIZE(pagesize) && addr & (pagesize[i] - 1); i++)
+                ;
+        return (i < ARRAY_SIZE(pagesize)) ? pagesize[i] : 0;
+}
+
 /*
  * calculate the optimal number sg elements from total bytes based on
  * iommu superpages
  */
-static unsigned int sgtable_nents(size_t bytes)
+static unsigned sgtable_nents(size_t bytes, u32 da, u32 pa)
 {
-        int i;
-        unsigned int nr_entries;
-        const unsigned long pagesize[] = { SZ_16M, SZ_1M, SZ_64K, SZ_4K, };
+        unsigned nr_entries = 0, ent_sz;
 
         if (!IS_ALIGNED(bytes, PAGE_SIZE)) {
                 pr_err("%s: wrong size %08x\n", __func__, bytes);
                 return 0;
         }
 
-        nr_entries = 0;
-        for (i = 0; i < ARRAY_SIZE(pagesize); i++) {
-                if (bytes >= pagesize[i]) {
-                        nr_entries += (bytes / pagesize[i]);
-                        bytes %= pagesize[i];
-                }
+        while (bytes) {
+                ent_sz = max_alignment(da | pa);
+                ent_sz = min_t(unsigned, ent_sz, iopgsz_max(bytes));
+                nr_entries++;
+                da += ent_sz;
+                pa += ent_sz;
+                bytes -= ent_sz;
         }
-        BUG_ON(bytes);
 
         return nr_entries;
 }
 
 /* allocate and initialize sg_table header(a kind of 'superblock') */
-static struct sg_table *sgtable_alloc(const size_t bytes, u32 flags)
+static struct sg_table *sgtable_alloc(const size_t bytes, u32 flags,
+                                                        u32 da, u32 pa)
 {
         unsigned int nr_entries;
         int err;
@@ -127,9 +135,8 @@ static struct sg_table *sgtable_alloc(const size_t bytes, u32 flags)
         if (!IS_ALIGNED(bytes, PAGE_SIZE))
                 return ERR_PTR(-EINVAL);
 
-        /* FIXME: IOVMF_DA_FIXED should support 'superpages' */
-        if ((flags & IOVMF_LINEAR) && (flags & IOVMF_DA_ANON)) {
-                nr_entries = sgtable_nents(bytes);
+        if (flags & IOVMF_LINEAR) {
+                nr_entries = sgtable_nents(bytes, da, pa);
                 if (!nr_entries)
                         return ERR_PTR(-EINVAL);
         } else
@@ -273,13 +280,14 @@ static struct iovm_struct *alloc_iovm_area(struct iommu *obj, u32 da,
         alignement = PAGE_SIZE;
 
         if (flags & IOVMF_DA_ANON) {
-                /*
-                 * Reserve the first page for NULL
-                 */
-                start = PAGE_SIZE;
+                start = obj->da_start;
+
                 if (flags & IOVMF_LINEAR)
                         alignement = iopgsz_max(bytes);
                 start = roundup(start, alignement);
+        } else if (start < obj->da_start || start > obj->da_end ||
+                                        obj->da_end - start < bytes) {
+                return ERR_PTR(-EINVAL);
         }
 
         tmp = NULL;
@@ -289,19 +297,19 @@ static struct iovm_struct *alloc_iovm_area(struct iommu *obj, u32 da,
         prev_end = 0;
         list_for_each_entry(tmp, &obj->mmap, list) {
 
-                if (prev_end >= start)
+                if (prev_end > start)
                         break;
 
-                if (start + bytes < tmp->da_start)
+                if (tmp->da_start > start && (tmp->da_start - start) >= bytes)
                         goto found;
 
-                if (flags & IOVMF_DA_ANON)
+                if (tmp->da_end >= start && flags & IOVMF_DA_ANON)
                         start = roundup(tmp->da_end + 1, alignement);
 
                 prev_end = tmp->da_end;
         }
 
-        if ((start > prev_end) && (ULONG_MAX - start >= bytes))
+        if ((start >= prev_end) && (obj->da_end - start >= bytes))
                 goto found;
 
         dev_dbg(obj->dev, "%s: no space to fit %08x(%x) flags: %08x\n",
@@ -409,7 +417,8 @@ static inline void sgtable_drain_vmalloc(struct sg_table *sgt)
         BUG_ON(!sgt);
 }
 
-static void sgtable_fill_kmalloc(struct sg_table *sgt, u32 pa, size_t len)
+static void sgtable_fill_kmalloc(struct sg_table *sgt, u32 pa, u32 da,
+                                                                size_t len)
 {
         unsigned int i;
         struct scatterlist *sg;
@@ -418,9 +427,10 @@ static void sgtable_fill_kmalloc(struct sg_table *sgt, u32 pa, size_t len)
         va = phys_to_virt(pa);
 
         for_each_sg(sgt->sgl, sg, sgt->nents, i) {
-                size_t bytes;
+                unsigned bytes;
 
-                bytes = iopgsz_max(len);
+                bytes = max_alignment(da | pa);
+                bytes = min_t(unsigned, bytes, iopgsz_max(len));
 
                 BUG_ON(!iopgsz_ok(bytes));
 
@@ -429,6 +439,7 @@ static void sgtable_fill_kmalloc(struct sg_table *sgt, u32 pa, size_t len)
                  * 'pa' is cotinuous(linear).
                  */
                 pa += bytes;
+                da += bytes;
                 len -= bytes;
         }
         BUG_ON(len);
@@ -695,18 +706,18 @@ u32 iommu_vmalloc(struct iommu *obj, u32 da, size_t bytes, u32 flags)
         if (!va)
                 return -ENOMEM;
 
-        sgt = sgtable_alloc(bytes, flags);
+        flags &= IOVMF_HW_MASK;
+        flags |= IOVMF_DISCONT;
+        flags |= IOVMF_ALLOC;
+        flags |= (da ? IOVMF_DA_FIXED : IOVMF_DA_ANON);
+
+        sgt = sgtable_alloc(bytes, flags, da, 0);
         if (IS_ERR(sgt)) {
                 da = PTR_ERR(sgt);
                 goto err_sgt_alloc;
         }
         sgtable_fill_vmalloc(sgt, va);
 
-        flags &= IOVMF_HW_MASK;
-        flags |= IOVMF_DISCONT;
-        flags |= IOVMF_ALLOC;
-        flags |= (da ? IOVMF_DA_FIXED : IOVMF_DA_ANON);
-
         da = __iommu_vmap(obj, da, sgt, va, bytes, flags);
         if (IS_ERR_VALUE(da))
                 goto err_iommu_vmap;
@@ -746,11 +757,11 @@ static u32 __iommu_kmap(struct iommu *obj, u32 da, u32 pa, void *va,
 {
         struct sg_table *sgt;
 
-        sgt = sgtable_alloc(bytes, flags);
+        sgt = sgtable_alloc(bytes, flags, da, pa);
         if (IS_ERR(sgt))
                 return PTR_ERR(sgt);
 
-        sgtable_fill_kmalloc(sgt, pa, bytes);
+        sgtable_fill_kmalloc(sgt, pa, da, bytes);
 
         da = map_iommu_region(obj, da, sgt, va, bytes, flags);
         if (IS_ERR_VALUE(da)) {
@@ -811,7 +822,7 @@ void iommu_kunmap(struct iommu *obj, u32 da)
         struct sg_table *sgt;
         typedef void (*func_t)(const void *);
 
-        sgt = unmap_vm_area(obj, da, (func_t)__iounmap,
+        sgt = unmap_vm_area(obj, da, (func_t)iounmap,
                             IOVMF_LINEAR | IOVMF_MMIO);
         if (!sgt)
                 dev_dbg(obj->dev, "%s: No sgt\n", __func__);
diff --git a/arch/arm/plat-omap/mailbox.c b/arch/arm/plat-omap/mailbox.c
index d2fafb892f7f..459b319a9fad 100644
--- a/arch/arm/plat-omap/mailbox.c
+++ b/arch/arm/plat-omap/mailbox.c
@@ -28,12 +28,12 @@
 #include <linux/slab.h>
 #include <linux/kfifo.h>
 #include <linux/err.h>
+#include <linux/notifier.h>
 
 #include <plat/mailbox.h>
 
 static struct workqueue_struct *mboxd;
 static struct omap_mbox **mboxes;
-static bool rq_full;
 
 static int mbox_configured;
 static DEFINE_MUTEX(mbox_configured_lock);
@@ -93,20 +93,25 @@ int omap_mbox_msg_send(struct omap_mbox *mbox, mbox_msg_t msg)
         struct omap_mbox_queue *mq = mbox->txq;
         int ret = 0, len;
 
-        spin_lock(&mq->lock);
+        spin_lock_bh(&mq->lock);
 
         if (kfifo_avail(&mq->fifo) < sizeof(msg)) {
                 ret = -ENOMEM;
                 goto out;
         }
 
+        if (kfifo_is_empty(&mq->fifo) && !__mbox_poll_for_space(mbox)) {
+                mbox_fifo_write(mbox, msg);
+                goto out;
+        }
+
         len = kfifo_in(&mq->fifo, (unsigned char *)&msg, sizeof(msg));
         WARN_ON(len != sizeof(msg));
 
         tasklet_schedule(&mbox->txq->tasklet);
 
 out:
-        spin_unlock(&mq->lock);
+        spin_unlock_bh(&mq->lock);
         return ret;
 }
 EXPORT_SYMBOL(omap_mbox_msg_send);
@@ -146,8 +151,14 @@ static void mbox_rx_work(struct work_struct *work)
                 len = kfifo_out(&mq->fifo, (unsigned char *)&msg, sizeof(msg));
                 WARN_ON(len != sizeof(msg));
 
-                if (mq->callback)
-                        mq->callback((void *)msg);
+                blocking_notifier_call_chain(&mq->mbox->notifier, len,
+                                                                (void *)msg);
+                spin_lock_irq(&mq->lock);
+                if (mq->full) {
+                        mq->full = false;
+                        omap_mbox_enable_irq(mq->mbox, IRQ_RX);
+                }
+                spin_unlock_irq(&mq->lock);
         }
 }
 
@@ -170,7 +181,7 @@ static void __mbox_rx_interrupt(struct omap_mbox *mbox)
         while (!mbox_fifo_empty(mbox)) {
                 if (unlikely(kfifo_avail(&mq->fifo) < sizeof(msg))) {
                         omap_mbox_disable_irq(mbox, IRQ_RX);
-                        rq_full = true;
+                        mq->full = true;
                         goto nomem;
                 }
 
@@ -239,73 +250,77 @@ static int omap_mbox_startup(struct omap_mbox *mbox)
         int ret = 0;
         struct omap_mbox_queue *mq;
 
-        if (mbox->ops->startup) {
-                mutex_lock(&mbox_configured_lock);
-                if (!mbox_configured)
+        mutex_lock(&mbox_configured_lock);
+        if (!mbox_configured++) {
+                if (likely(mbox->ops->startup)) {
                         ret = mbox->ops->startup(mbox);
-
-                if (ret) {
-                        mutex_unlock(&mbox_configured_lock);
-                        return ret;
-                }
-                mbox_configured++;
-                mutex_unlock(&mbox_configured_lock);
-        }
-
-        ret = request_irq(mbox->irq, mbox_interrupt, IRQF_SHARED,
-                                mbox->name, mbox);
-        if (ret) {
-                printk(KERN_ERR
-                        "failed to register mailbox interrupt:%d\n", ret);
-                goto fail_request_irq;
+                        if (unlikely(ret))
+                                goto fail_startup;
+                } else
+                        goto fail_startup;
         }
 
-        mq = mbox_queue_alloc(mbox, NULL, mbox_tx_tasklet);
-        if (!mq) {
-                ret = -ENOMEM;
-                goto fail_alloc_txq;
-        }
-        mbox->txq = mq;
+        if (!mbox->use_count++) {
+                ret = request_irq(mbox->irq, mbox_interrupt, IRQF_SHARED,
+                                                        mbox->name, mbox);
+                if (unlikely(ret)) {
+                        pr_err("failed to register mailbox interrupt:%d\n",
+                                                                        ret);
+                        goto fail_request_irq;
+                }
+                mq = mbox_queue_alloc(mbox, NULL, mbox_tx_tasklet);
+                if (!mq) {
+                        ret = -ENOMEM;
+                        goto fail_alloc_txq;
+                }
+                mbox->txq = mq;
 
-        mq = mbox_queue_alloc(mbox, mbox_rx_work, NULL);
-        if (!mq) {
-                ret = -ENOMEM;
-                goto fail_alloc_rxq;
+                mq = mbox_queue_alloc(mbox, mbox_rx_work, NULL);
+                if (!mq) {
+                        ret = -ENOMEM;
+                        goto fail_alloc_rxq;
+                }
+                mbox->rxq = mq;
+                mq->mbox = mbox;
         }
-        mbox->rxq = mq;
-
+        mutex_unlock(&mbox_configured_lock);
         return 0;
 
- fail_alloc_rxq:
+fail_alloc_rxq:
         mbox_queue_free(mbox->txq);
- fail_alloc_txq:
+fail_alloc_txq:
         free_irq(mbox->irq, mbox);
- fail_request_irq:
+fail_request_irq:
         if (mbox->ops->shutdown)
                 mbox->ops->shutdown(mbox);
-
+        mbox->use_count--;
+fail_startup:
+        mbox_configured--;
+        mutex_unlock(&mbox_configured_lock);
         return ret;
 }
 
 static void omap_mbox_fini(struct omap_mbox *mbox)
 {
-        free_irq(mbox->irq, mbox);
-        tasklet_kill(&mbox->txq->tasklet);
-        flush_work(&mbox->rxq->work);
-        mbox_queue_free(mbox->txq);
-        mbox_queue_free(mbox->rxq);
+        mutex_lock(&mbox_configured_lock);
+
+        if (!--mbox->use_count) {
+                free_irq(mbox->irq, mbox);
+                tasklet_kill(&mbox->txq->tasklet);
+                flush_work(&mbox->rxq->work);
+                mbox_queue_free(mbox->txq);
+                mbox_queue_free(mbox->rxq);
+        }
 
-        if (mbox->ops->shutdown) {
-                mutex_lock(&mbox_configured_lock);
-                if (mbox_configured > 0)
-                        mbox_configured--;
-                if (!mbox_configured)
+        if (likely(mbox->ops->shutdown)) {
+                if (!--mbox_configured)
                         mbox->ops->shutdown(mbox);
-                mutex_unlock(&mbox_configured_lock);
         }
+
+        mutex_unlock(&mbox_configured_lock);
 }
 
-struct omap_mbox *omap_mbox_get(const char *name)
+struct omap_mbox *omap_mbox_get(const char *name, struct notifier_block *nb)
 {
         struct omap_mbox *mbox;
         int ret;
@@ -324,12 +339,16 @@ struct omap_mbox *omap_mbox_get(const char *name)
         if (ret)
                 return ERR_PTR(-ENODEV);
 
+        if (nb)
+                blocking_notifier_chain_register(&mbox->notifier, nb);
+
         return mbox;
 }
 EXPORT_SYMBOL(omap_mbox_get);
 
-void omap_mbox_put(struct omap_mbox *mbox)
+void omap_mbox_put(struct omap_mbox *mbox, struct notifier_block *nb)
 {
+        blocking_notifier_chain_unregister(&mbox->notifier, nb);
         omap_mbox_fini(mbox);
 }
 EXPORT_SYMBOL(omap_mbox_put);
@@ -353,6 +372,8 @@ int omap_mbox_register(struct device *parent, struct omap_mbox **list)
                         ret = PTR_ERR(mbox->dev);
                         goto err_out;
                 }
+
+                BLOCKING_INIT_NOTIFIER_HEAD(&mbox->notifier);
         }
         return 0;
 
@@ -391,7 +412,8 @@ static int __init omap_mbox_init(void)
 
         /* kfifo size sanity check: alignment and minimal size */
         mbox_kfifo_size = ALIGN(mbox_kfifo_size, sizeof(mbox_msg_t));
-        mbox_kfifo_size = max_t(unsigned int, mbox_kfifo_size, sizeof(mbox_msg_t));
+        mbox_kfifo_size = max_t(unsigned int, mbox_kfifo_size,
+                                                        sizeof(mbox_msg_t));
 
         return 0;
 }