1 files changed, 68 insertions, 15 deletions

diff --git a/lib_generic/lmb.c b/lib_generic/lmb.c
index afe33197de..6b46fa8868 100644
--- a/lib_generic/lmb.c
+++ b/lib_generic/lmb.c
@@ -181,6 +181,55 @@ long lmb_add(struct lmb *lmb, phys_addr_t base, phys_size_t size)
 	return lmb_add_region(_rgn, base, size);
 }
 
+long lmb_free(struct lmb *lmb, phys_addr_t base, phys_size_t size)
+{
+	struct lmb_region *rgn = &(lmb->reserved);
+	phys_addr_t rgnbegin, rgnend;
+	phys_addr_t end = base + size;
+	int i;
+
+	rgnbegin = rgnend = 0; /* supress gcc warnings */
+
+	/* Find the region where (base, size) belongs to */
+	for (i=0; i < rgn->cnt; i++) {
+		rgnbegin = rgn->region[i].base;
+		rgnend = rgnbegin + rgn->region[i].size;
+
+		if ((rgnbegin <= base) && (end <= rgnend))
+			break;
+	}
+
+	/* Didn't find the region */
+	if (i == rgn->cnt)
+		return -1;
+
+	/* Check to see if we are removing entire region */
+	if ((rgnbegin == base) && (rgnend == end)) {
+		lmb_remove_region(rgn, i);
+		return 0;
+	}
+
+	/* Check to see if region is matching at the front */
+	if (rgnbegin == base) {
+		rgn->region[i].base = end;
+		rgn->region[i].size -= size;
+		return 0;
+	}
+
+	/* Check to see if the region is matching at the end */
+	if (rgnend == end) {
+		rgn->region[i].size -= size;
+		return 0;
+	}
+
+	/*
+	 * We need to split the entry -  adjust the current one to the
+	 * beginging of the hole and add the region after hole.
+	 */
+	rgn->region[i].size = base - rgn->region[i].base;
+	return lmb_add_region(rgn, end, rgnend - end);
+}
+
 long lmb_reserve(struct lmb *lmb, phys_addr_t base, phys_size_t size)
 {
 	struct lmb_region *_rgn = &(lmb->reserved);
@@ -236,11 +285,14 @@ phys_addr_t __lmb_alloc_base(struct lmb *lmb, phys_size_t size, ulong align, phy
 {
 	long i, j;
 	phys_addr_t base = 0;
+	phys_addr_t res_base;
 
 	for (i = lmb->memory.cnt-1; i >= 0; i--) {
 		phys_addr_t lmbbase = lmb->memory.region[i].base;
 		phys_size_t lmbsize = lmb->memory.region[i].size;
 
+		if (lmbsize < size)
+			continue;
 		if (max_addr == LMB_ALLOC_ANYWHERE)
 			base = lmb_align_down(lmbbase + lmbsize - size, align);
 		else if (lmbbase < max_addr) {
@@ -249,22 +301,23 @@ phys_addr_t __lmb_alloc_base(struct lmb *lmb, phys_size_t size, ulong align, phy
 		} else
 			continue;
 
-		while ((lmbbase <= base) &&
-		       ((j = lmb_overlaps_region(&(lmb->reserved), base, size)) >= 0) )
-			base = lmb_align_down(lmb->reserved.region[j].base - size,
-					      align);
-
-		if ((base != 0) && (lmbbase <= base))
-			break;
+		while (base && lmbbase <= base) {
+			j = lmb_overlaps_region(&lmb->reserved, base, size);
+			if (j < 0) {
+				/* This area isn't reserved, take it */
+				if (lmb_add_region(&lmb->reserved, base,
+							lmb_align_up(size,
+								align)) < 0)
+					return 0;
+				return base;
+			}
+			res_base = lmb->reserved.region[j].base;
+			if (res_base < size)
+				break;
+			base = lmb_align_down(res_base - size, align);
+		}
 	}
-
-	if (i < 0)
-		return 0;
-
-	if (lmb_add_region(&(lmb->reserved), base, lmb_align_up(size, align)) < 0)
-		return 0;
-
-	return base;
+	return 0;
 }
 
 int lmb_is_reserved(struct lmb *lmb, phys_addr_t addr)