1 files changed, 31 insertions, 6 deletions

diff --git a/arch/sh/mm/pmb.c b/arch/sh/mm/pmb.c
index b1a714a92b14..aade31102112 100644
--- a/arch/sh/mm/pmb.c
+++ b/arch/sh/mm/pmb.c
@@ -33,6 +33,8 @@
 
 #define NR_PMB_ENTRIES  16
 
+static void __pmb_unmap(struct pmb_entry *);
+
 static struct kmem_cache *pmb_cache;
 static unsigned long pmb_map;
 
@@ -218,9 +220,10 @@ static struct {
 long pmb_remap(unsigned long vaddr, unsigned long phys,
                unsigned long size, unsigned long flags)
 {
-        struct pmb_entry *pmbp;
+        struct pmb_entry *pmbp, *pmbe;
         unsigned long wanted;
         int pmb_flags, i;
+        long err;
 
         /* Convert typical pgprot value to the PMB equivalent */
         if (flags & _PAGE_CACHABLE) {
@@ -236,20 +239,22 @@ long pmb_remap(unsigned long vaddr, unsigned long phys,
 
 again:
         for (i = 0; i < ARRAY_SIZE(pmb_sizes); i++) {
-                struct pmb_entry *pmbe;
                 int ret;
 
                 if (size < pmb_sizes[i].size)
                         continue;
 
                 pmbe = pmb_alloc(vaddr, phys, pmb_flags | pmb_sizes[i].flag);
-                if (IS_ERR(pmbe))
-                        return PTR_ERR(pmbe);
+                if (IS_ERR(pmbe)) {
+                        err = PTR_ERR(pmbe);
+                        goto out;
+                }
 
                 ret = set_pmb_entry(pmbe);
                 if (ret != 0) {
                         pmb_free(pmbe);
-                        return -EBUSY;
+                        err = -EBUSY;
+                        goto out;
                 }
 
                 phys    += pmb_sizes[i].size;
@@ -264,12 +269,25 @@ again:
                         pmbp->link = pmbe;
 
                 pmbp = pmbe;
+
+                /*
+                 * Instead of trying smaller sizes on every iteration
+                 * (even if we succeed in allocating space), try using
+                 * pmb_sizes[i].size again.
+                 */
+                i--;
         }
 
         if (size >= 0x1000000)
                 goto again;
 
         return wanted - size;
+
+out:
+        if (pmbp)
+                __pmb_unmap(pmbp);
+
+        return err;
 }
 
 void pmb_unmap(unsigned long addr)
@@ -283,12 +301,19 @@ void pmb_unmap(unsigned long addr)
         if (unlikely(!pmbe))
                 return;
 
+        __pmb_unmap(pmbe);
+}
+
+static void __pmb_unmap(struct pmb_entry *pmbe)
+{
         WARN_ON(!test_bit(pmbe->entry, &pmb_map));
 
         do {
                 struct pmb_entry *pmblink = pmbe;
 
-                clear_pmb_entry(pmbe);
+                if (pmbe->entry != PMB_NO_ENTRY)
+                        clear_pmb_entry(pmbe);
+
                 pmbe = pmblink->link;
 
                 pmb_free(pmblink);