2 files changed, 103 insertions, 5 deletions

diff --git a/arch/xtensa/include/asm/sysmem.h b/arch/xtensa/include/asm/sysmem.h
index fe7ad750a158..c015c5c8e3f7 100644
--- a/arch/xtensa/include/asm/sysmem.h
+++ b/arch/xtensa/include/asm/sysmem.h
@@ -18,6 +18,11 @@ struct meminfo {
         unsigned long end;
 };
 
+/*
+ * Bank array is sorted by .start.
+ * Banks don't overlap and there's at least one page gap
+ * between adjacent bank entries.
+ */
 struct sysmem_info {
         int nr_banks;
         struct meminfo bank[SYSMEM_BANKS_MAX];
diff --git a/arch/xtensa/mm/init.c b/arch/xtensa/mm/init.c
index 4f78264e8bd8..79c0c3d52ae3 100644
--- a/arch/xtensa/mm/init.c
+++ b/arch/xtensa/mm/init.c
@@ -8,6 +8,7 @@
  * for more details.
  *
  * Copyright (C) 2001 - 2005 Tensilica Inc.
+ * Copyright (C) 2014 Cadence Design Systems Inc.
  *
  * Chris Zankel <chris@zankel.net>
  * Joe Taylor   <joe@tensilica.com, joetylr@yahoo.com>
@@ -31,17 +32,109 @@
 
 struct sysmem_info sysmem __initdata;
 
+/*
+ * Find bank with maximal .start such that bank.start <= start
+ */
+static inline struct meminfo * __init find_bank(unsigned long start)
+{
+        unsigned i;
+        struct meminfo *it = NULL;
+
+        for (i = 0; i < sysmem.nr_banks; ++i)
+                if (sysmem.bank[i].start <= start)
+                        it = sysmem.bank + i;
+                else
+                        break;
+        return it;
+}
+
+/*
+ * Move all memory banks starting at 'from' to a new place at 'to',
+ * adjust nr_banks accordingly.
+ * Both 'from' and 'to' must be inside the sysmem.bank.
+ *
+ * Returns: 0 (success), -ENOMEM (not enough space in the sysmem.bank).
+ */
+static int __init move_banks(struct meminfo *to, struct meminfo *from)
+{
+        unsigned n = sysmem.nr_banks - (from - sysmem.bank);
+
+        if (to > from && to - from + sysmem.nr_banks > SYSMEM_BANKS_MAX)
+                return -ENOMEM;
+        if (to != from)
+                memmove(to, from, n * sizeof(struct meminfo));
+        sysmem.nr_banks += to - from;
+        return 0;
+}
+
+/*
+ * Add new bank to sysmem. Resulting sysmem is the union of bytes of the
+ * original sysmem and the new bank.
+ *
+ * Returns: 0 (success), < 0 (error)
+ */
 int __init add_sysmem_bank(unsigned long start, unsigned long end)
 {
-        if (sysmem.nr_banks >= SYSMEM_BANKS_MAX) {
-                pr_warn("Ignoring memory bank 0x%08lx size %ldKB\n",
+        unsigned i;
+        struct meminfo *it = NULL;
+        unsigned long sz;
+        unsigned long bank_sz = 0;
+
+        if (start == end ||
+            (start < end) != (PAGE_ALIGN(start) < (end & PAGE_MASK))) {
+                pr_warn("Ignoring small memory bank 0x%08lx size: %ld bytes\n",
                         start, end - start);
                 return -EINVAL;
         }
-        sysmem.bank[sysmem.nr_banks].start = PAGE_ALIGN(start);
-        sysmem.bank[sysmem.nr_banks].end   = end & PAGE_MASK;
-        sysmem.nr_banks++;
 
+        start = PAGE_ALIGN(start);
+        end &= PAGE_MASK;
+        sz = end - start;
+
+        it = find_bank(start);
+
+        if (it)
+                bank_sz = it->end - it->start;
+
+        if (it && bank_sz >= start - it->start) {
+                if (end - it->start > bank_sz)
+                        it->end = end;
+                else
+                        return 0;
+        } else {
+                if (!it)
+                        it = sysmem.bank;
+                else
+                        ++it;
+
+                if (it - sysmem.bank < sysmem.nr_banks &&
+                    it->start - start <= sz) {
+                        it->start = start;
+                        if (it->end - it->start < sz)
+                                it->end = end;
+                        else
+                                return 0;
+                } else {
+                        if (move_banks(it + 1, it) < 0) {
+                                pr_warn("Ignoring memory bank 0x%08lx size %ld bytes\n",
+                                        start, end - start);
+                                return -EINVAL;
+                        }
+                        it->start = start;
+                        it->end = end;
+                        return 0;
+                }
+        }
+        sz = it->end - it->start;
+        for (i = it + 1 - sysmem.bank; i < sysmem.nr_banks; ++i)
+                if (sysmem.bank[i].start - it->start <= sz) {
+                        if (sz < sysmem.bank[i].end - it->start)
+                                it->end = sysmem.bank[i].end;
+                } else {
+                        break;
+                }
+
+        move_banks(it + 1, sysmem.bank + i);
         return 0;
 }