1 files changed, 96 insertions, 44 deletions

diff --git a/arch/sh/mm/pmb.c b/arch/sh/mm/pmb.c
index 55d21902d707..75b8861ec624 100644
--- a/arch/sh/mm/pmb.c
+++ b/arch/sh/mm/pmb.c
@@ -128,13 +128,67 @@ static inline unsigned long pgprot_to_pmb_flags(pgprot_t prot)
 	return pmb_flags;
 }
 
-static bool pmb_can_merge(struct pmb_entry *a, struct pmb_entry *b)
+static inline bool pmb_can_merge(struct pmb_entry *a, struct pmb_entry *b)
 {
 	return (b->vpn == (a->vpn + a->size)) &&
 	       (b->ppn == (a->ppn + a->size)) &&
 	       (b->flags == a->flags);
 }
 
+static bool pmb_mapping_exists(unsigned long vaddr, phys_addr_t phys,
+			       unsigned long size)
+{
+	int i;
+
+	read_lock(&pmb_rwlock);
+
+	for (i = 0; i < ARRAY_SIZE(pmb_entry_list); i++) {
+		struct pmb_entry *pmbe, *iter;
+		unsigned long span;
+
+		if (!test_bit(i, pmb_map))
+			continue;
+
+		pmbe = &pmb_entry_list[i];
+
+		/*
+		 * See if VPN and PPN are bounded by an existing mapping.
+		 */
+		if ((vaddr < pmbe->vpn) || (vaddr >= (pmbe->vpn + pmbe->size)))
+			continue;
+		if ((phys < pmbe->ppn) || (phys >= (pmbe->ppn + pmbe->size)))
+			continue;
+
+		/*
+		 * Now see if we're in range of a simple mapping.
+		 */
+		if (size <= pmbe->size) {
+			read_unlock(&pmb_rwlock);
+			return true;
+		}
+
+		span = pmbe->size;
+
+		/*
+		 * Finally for sizes that involve compound mappings, walk
+		 * the chain.
+		 */
+		for (iter = pmbe->link; iter; iter = iter->link)
+			span += iter->size;
+
+		/*
+		 * Nothing else to do if the range requirements are met.
+		 */
+		if (size <= span) {
+			read_unlock(&pmb_rwlock);
+			return true;
+		}
+	}
+
+	read_unlock(&pmb_rwlock);
+	return false;
+}
+
 static bool pmb_size_valid(unsigned long size)
 {
 	int i;
@@ -272,64 +326,62 @@ int pmb_bolt_mapping(unsigned long vaddr, phys_addr_t phys,
 		     unsigned long size, pgprot_t prot)
 {
 	struct pmb_entry *pmbp, *pmbe;
-	unsigned long pmb_flags;
+	unsigned long flags, pmb_flags;
 	int i, mapped;
 
 	if (!pmb_addr_valid(vaddr, size))
 		return -EFAULT;
+	if (pmb_mapping_exists(vaddr, phys, size))
+		return 0;
 
 	pmb_flags = pgprot_to_pmb_flags(prot);
 	pmbp = NULL;
 
-again:
-	for (i = mapped = 0; i < ARRAY_SIZE(pmb_sizes); i++) {
-		unsigned long flags;
-
-		if (size < pmb_sizes[i].size)
-			continue;
-
-		pmbe = pmb_alloc(vaddr, phys, pmb_flags | pmb_sizes[i].flag,
-				 PMB_NO_ENTRY);
-		if (IS_ERR(pmbe)) {
-			pmb_unmap_entry(pmbp, mapped);
-			return PTR_ERR(pmbe);
-		}
-
-		spin_lock_irqsave(&pmbe->lock, flags);
+	do {
+		for (i = mapped = 0; i < ARRAY_SIZE(pmb_sizes); i++) {
+			if (size < pmb_sizes[i].size)
+				continue;
+
+			pmbe = pmb_alloc(vaddr, phys, pmb_flags |
+					 pmb_sizes[i].flag, PMB_NO_ENTRY);
+			if (IS_ERR(pmbe)) {
+				pmb_unmap_entry(pmbp, mapped);
+				return PTR_ERR(pmbe);
+			}
 
-		pmbe->size = pmb_sizes[i].size;
+			spin_lock_irqsave(&pmbe->lock, flags);
 
-		__set_pmb_entry(pmbe);
+			pmbe->size = pmb_sizes[i].size;
 
-		phys	+= pmbe->size;
-		vaddr	+= pmbe->size;
-		size	-= pmbe->size;
+			__set_pmb_entry(pmbe);
 
-		/*
-		 * Link adjacent entries that span multiple PMB entries
-		 * for easier tear-down.
-		 */
-		if (likely(pmbp)) {
-			spin_lock(&pmbp->lock);
-			pmbp->link = pmbe;
-			spin_unlock(&pmbp->lock);
-		}
+			phys	+= pmbe->size;
+			vaddr	+= pmbe->size;
+			size	-= pmbe->size;
 
-		pmbp = pmbe;
+			/*
+			 * Link adjacent entries that span multiple PMB
+			 * entries for easier tear-down.
+			 */
+			if (likely(pmbp)) {
+				spin_lock(&pmbp->lock);
+				pmbp->link = pmbe;
+				spin_unlock(&pmbp->lock);
+			}
 
-		/*
-		 * Instead of trying smaller sizes on every iteration
-		 * (even if we succeed in allocating space), try using
-		 * pmb_sizes[i].size again.
-		 */
-		i--;
-		mapped++;
+			pmbp = pmbe;
 
-		spin_unlock_irqrestore(&pmbe->lock, flags);
-	}
+			/*
+			 * Instead of trying smaller sizes on every
+			 * iteration (even if we succeed in allocating
+			 * space), try using pmb_sizes[i].size again.
+			 */
+			i--;
+			mapped++;
 
-	if (size >= SZ_16M)
-		goto again;
+			spin_unlock_irqrestore(&pmbe->lock, flags);
+		}
+	} while (size >= SZ_16M);
 
 	return 0;
 }
@@ -374,7 +426,7 @@ void __iomem *pmb_remap_caller(phys_addr_t phys, unsigned long size,
 	orig_addr = vaddr = (unsigned long)area->addr;
 
 	ret = pmb_bolt_mapping(vaddr, phys, size, prot);
-	if (ret != 0)
+	if (unlikely(ret != 0))
 		return ERR_PTR(ret);
 
 	return (void __iomem *)(offset + (char *)orig_addr);