From bde4c8071b44f3b77ecb2cc06dbc16fb57d1725c Mon Sep 17 00:00:00 2001
From: "Liam R. Howlett" <Liam.Howlett@Oracle.com>
Date: Tue, 30 Jun 2020 13:30:06 -0400
Subject: [PATCH] wip, maple_tree: Pass 824089 - rebalance and spanning store
 using most of the same code now

Signed-off-by: Liam R. Howlett <Liam.Howlett@Oracle.com>
---
 lib/maple_tree.c      | 1016 +++++++++++++++++++++--------------------
 lib/test_maple_tree.c |    1 -
 2 files changed, 510 insertions(+), 507 deletions(-)

diff --git a/lib/maple_tree.c b/lib/maple_tree.c
index 1687189e53f6..f069560b9945 100644
--- a/lib/maple_tree.c
+++ b/lib/maple_tree.c
@@ -2,7 +2,7 @@
 /*
  * Maple Tree implementation
  * Copyright (c) 2018 Oracle Corporation
- * Authors: Liam R. Howlett <jedix@infradead.org>
+ * Authors: Liam R. Howlett <Liam.Howlett@oracle.com>
  *	    Matthew Wilcox <willy@infradead.org>
  */
 
@@ -817,7 +817,7 @@ bool mas_retry(struct ma_state *mas, const void *entry)
 
 static inline void mas_ascend(struct ma_state *mas)
 {
-	struct maple_enode *p_enode; // parent enode.
+	struct maple_enode *p_enode = mas->node; // parent enode.
 	struct maple_enode *a_enode = mas->node; // ancestor enode.
 	struct maple_node *a_node = mas_mn(mas); // ancestor node.
 	unsigned char a_slot = 0;
@@ -825,12 +825,12 @@ static inline void mas_ascend(struct ma_state *mas)
 	unsigned long max = 0, min = ULONG_MAX;
 	bool set_max = false, set_min = false;
 
-	p_enode = mt_mk_node(mte_parent(mas->node),
-			mas_parent_enum(mas, mas->node));
-
 	if (_ma_is_root(a_node))
 		goto no_parent;
 
+	p_enode = mt_mk_node(mte_parent(mas->node),
+			mas_parent_enum(mas, mas->node));
+
 	a_type = mas_parent_enum(mas, mas->node);
 	a_enode = p_enode;
 
@@ -865,7 +865,13 @@ no_parent:
 	}
 
 	if (!max || min == ULONG_MAX) {
+		if (mas->node == a_enode) {
+			//FIXME: Restart and retry?
+			printk("Dead node %p\n", mas_mn(mas));
+			MT_BUG_ON(mas->tree, mas->node == a_enode);
+		}
 		mas->node = a_enode;
+		printk("ascend again\n");
 		goto ascend;
 	}
 
@@ -1446,6 +1452,7 @@ static inline void _mas_replace(struct ma_state *mas, bool free, bool push,
 		mn->parent = ma_parent_ptr(
 			      ((unsigned long)mas->tree | MA_ROOT_PARENT));
 		rcu_assign_pointer(mas->tree->ma_root, mte_mk_root(mas->node));
+		mas->tree->ma_height = mas->depth;
 	} else {
 		mte_update_rcu_slot(parent, slot, mas->node);
 	}
@@ -1497,7 +1504,11 @@ static inline struct maple_enode *mas_check_split_parent(struct ma_state *mas,
 	if (!entry)
 		return NULL;
 
-	printk("parent of entry %u is %p\n", slot, mte_parent(entry));
+	printk("parent of %p[%u] is %p\n", mas_mn(mas), slot, mte_parent(entry));
+	if (mte_parent(entry) == entry) {
+		printk("%s: Dead node %p", __func__, entry);
+		MT_BUG_ON(mas->tree, 1);
+	}
 	if (mte_parent(entry) != mas_mn(mas))
 		return NULL;
 
@@ -1545,7 +1556,7 @@ static inline void mab_shift_right(struct maple_big_node *b_node,
 static inline int mab_calc_split(struct maple_big_node *b_node, int size,
 				     unsigned char slot_cnt, unsigned long min)
 {
-	int split = size / 2; // Assume equal split.
+	int split = (size - 1) / 2; // Assume equal split.
 
 	if (size > 2* slot_cnt) {
 		split = (size + 1) / 3;
@@ -1572,7 +1583,7 @@ static inline int mab_calc_split(struct maple_big_node *b_node, int size,
 }
 
 /*
- * Note, mas_end is exclusive.
+ * Note, mas_end is inclusive
  */
 static inline int mas_mab_cp(struct ma_state *mas, unsigned char mas_start,
 			  unsigned char mas_end, struct maple_big_node *b_node,
@@ -1580,7 +1591,7 @@ static inline int mas_mab_cp(struct ma_state *mas, unsigned char mas_start,
 {
 	int i, j;
 	printk("%s: cp %u - %u\n", __func__, mas_start, mas_end);
-	for (i = mas_start, j = mab_start; i < mas_end; i++, j++) {
+	for (i = mas_start, j = mab_start; i <= mas_end; i++, j++) {
 		//printk("cp mas %u\n", i);
 		b_node->slot[j] = mas_get_rcu_slot(mas, i);
 		if (!mte_is_leaf(mas->node) && mt_is_alloc(mas->tree)) {
@@ -1596,13 +1607,17 @@ static inline int mas_mab_cp(struct ma_state *mas, unsigned char mas_start,
 			b_node->pivot[j] = mas->max;
 			printk("Set b_node %u slot %p => piv %lu\n", j,
 			       b_node->slot[j], b_node->pivot[j]);
+			j++;
 			break;
 		}
 
 		if ((j && !b_node->pivot[j]) ||
-		    (mas->max == b_node->pivot[j])) // end of node.
+		    (mas->max == b_node->pivot[j])) { // end of node.
+			j++;
 			break;
+		}
 	}
+	printk("%s: return %u\n", __func__, j);
 	return j;
 }
 
@@ -1611,8 +1626,8 @@ static inline int mab_mas_cp(struct maple_big_node *b_node,
 			     struct ma_state *mas, unsigned char mas_start)
 {
 	int i, j;
-	printk("%s: cp %u - %u to mas %p\n", __func__, mab_start, mab_end - 1, mas_mn(mas));
-	for (i = mab_start, j = mas_start; i < mab_end; i++, j++) {
+	printk("%s: cp %u - %u to mas %p\n", __func__, mab_start, mab_end, mas_mn(mas));
+	for (i = mab_start, j = mas_start; i <= mab_end; i++, j++) {
 		//printk("i %d end %u\n", i, mab_end);
 		if(j && !b_node->pivot[i]) {
 			printk("no pivot at %u\n", i);
@@ -1689,204 +1704,476 @@ static inline void mas_descend_adopt(struct ma_state *mas)
 	}
 }
 
-static inline int mas_rebalance(struct ma_state *mas,
-				   struct maple_big_node *b_node,
-				   unsigned char new_end)
+
+static inline unsigned char mas_store_b_node(struct ma_state *mas,
+				    struct maple_big_node *b_node,
+				    void *entry)
 {
-	unsigned char p_end, end = 0, slot_cnt, split, b;
-	unsigned char l_child = 0, r_child = 0;
-	char empty = mas->full_cnt * -1;
-	int height = 0;
-	struct maple_enode *left, *right;
-	unsigned long l_piv, l_gap = 0, r_gap = 0;
-	printk("Need to rebalance %d levels\n", empty);
+	unsigned char slot = mas_get_slot(mas);
+	unsigned char end = mas_data_end(mas);
+	void *contents = mas_get_rcu_slot(mas, slot);
+	unsigned char b_end = 0;
+	unsigned long piv = mas->min;
 
-	MA_STATE(orig_mas, mas->tree, mas->index, mas->last);
+	if (slot) {
+		b_end = mas_mab_cp(mas, 0, slot - 1, b_node, 0);
+		piv = b_node->pivot[b_end - 1];
+	}
+
+	if (mas->index && piv < mas->index - 1) {
+		b_node->slot[b_end] = contents;
+		b_node->pivot[b_end++] = mas->index - 1;
+	}
+
+	printk("Store value at %u piv %lu\n", b_end, mas->last);
+	b_node->slot[b_end] = entry;
+	b_node->pivot[b_end] = mas->last;
+
+	piv = mas_get_safe_pivot(mas, slot);
+
+	if (piv > mas->last) {
+		b_node->slot[++b_end] = contents;
+		b_node->pivot[b_end] = piv;
+	} else
+		piv = mas->last;
+
+	if (piv >= mas->max)
+		return b_end;
+
+	printk("slot %u piv %lu\n", slot, piv);
+	do {
+		piv = mas_get_safe_pivot(mas, ++slot);
+	} while ((piv <= mas->last) && (slot <= end));
+
+	printk("slot %u piv %lu\n", slot, piv);
+
+	if (piv > mas->last) {
+		printk("Cp %u to %u\n", slot, end);
+		if (slot > end) {
+			b_node->slot[++b_end] = NULL;
+			b_node->pivot[b_end] = piv;
+		} else {
+			b_end = mas_mab_cp(mas, slot, end + 1, b_node, ++b_end);
+			b_end--;
+		}
+	}
+
+	return b_end;
+}
+
+static inline bool mas_node_walk(struct ma_state *mas, enum maple_type type,
+		unsigned long *range_min, unsigned long *range_max);
+
+static inline bool mas_prev_sibling(struct ma_state *mas)
+{
+	unsigned p_slot = mte_parent_slot(mas->node);
+
+	if (mte_is_root(mas->node))
+		return false;
+
+	if (!p_slot)
+		return false;
+
+	mas_ascend(mas);
+	mas_set_slot(mas, p_slot - 1);
+	mas_descend(mas);
+	return true;
+}
+/** Private
+ */
+static inline bool mas_next_sibling(struct ma_state *mas)
+{
+	unsigned char p_end, p_slot = mte_parent_slot(mas->node);
 	MA_STATE(parent, mas->tree, mas->index, mas->last);
-	MA_STATE(cp, mas->tree, mas->index, mas->last);
 
-	mas_dup_state(&orig_mas, mas);
+	if (mte_is_root(mas->node))
+		return false;
+
 	mas_dup_state(&parent, mas);
 	mas_ascend(&parent);
+	p_end = mas_data_end(&parent);
 
-	mas_node_cnt(mas, 1 + empty * 2);
-	if (mas_is_err(mas))
-		return 0;
-
-	while (height++ < empty) {
-		unsigned char p_slot = mte_parent_slot(mas->node);
+	if (p_end == p_slot)
+		return false;
 
-		p_end = mas_data_end(&parent);
-		slot_cnt = mt_slot_count(mas->node);
+	mas_ascend(mas);
+	mas_set_slot(mas, p_slot + 1);
+	mas_descend(mas);
+	return true;
+}
+/* Private
+ *
+ * mas_combine_separate() - Follow the tree upwards from @l_mas and @r_mas for
+ * @count, or until the root is hit.  First @b_node is split into two entries
+ * which are inserted into the next iteration of the loop.  @b_node is returned
+ * populated with the final iteration. @mas is used to obtain allocations.
+ *
+ * Returns the @count left.
+ */
+static inline int mas_combine_separate(struct ma_state *mas,
+				       struct ma_state *orig_l_mas,
+				       struct ma_state *orig_r_mas,
+				       struct maple_big_node *b_node,
+				       unsigned char b_end,
+				       unsigned char count)
+{
+	unsigned char slot_cnt, split;
+	unsigned char child = 0, l_slot, r_slot = 0;
+	struct maple_enode *l, *m, *r; // left, middle, right
+	unsigned long range_min, range_max;
 
-		printk("\nLooking to rebalance node %p\n", mas_mn(mas));
-		printk("new_end %u p_slot %u p_end %u\n", new_end, p_slot, p_end);
-		if (height >= empty) {
-			printk("Final entry.\n");
-		}
+	MA_STATE(l_mas, mas->tree, mas->index, mas->index);
+	MA_STATE(r_mas, mas->tree, mas->index, mas->index);
+	MA_STATE(m_mas, mas->tree, mas->index, mas->index);
+
+	l_mas.node = r_mas.node = m_mas.node = NULL;
+	l = r = m = NULL;
+
+	printk("Starting with orig of %p and %p\n", mas_mn(orig_l_mas),
+	       mas_mn(orig_r_mas));
+	printk("\t\t%d %u\n", __LINE__, b_end);
+	while(count--) {
+		printk("Iteration %u\n", count);
+		b_end--;
+		slot_cnt = mt_slot_count(orig_l_mas->node);
+		r = NULL;
+		m = NULL;
+		l = mt_mk_node(ma_mnode_ptr(mas_next_alloc(mas)),
+			       mte_node_type(orig_l_mas->node));
 
-		mas_dup_state(&cp, mas);
-		mas_set_slot(&cp, p_slot);
-		printk("p_slot %u p_end %u\n", p_slot, p_end);
-		if (p_slot < p_end) {
-			printk("Rebalance from next\n");
-			mas_next_node(&cp, ULONG_MAX);
-			end = mas_data_end(&cp);
-			mas_mab_cp(&cp, 0, end + 1, b_node, new_end + 1);
-			end += new_end + 1;
-			printk("End is now %u\n", end);
+		if (b_end < slot_cnt) {
+			split = b_end;
 		} else {
-			bool alloc  = false;
-
-			printk("Rebalance from prev\n");
-			if (mt_is_alloc(mas->tree) && !mte_is_leaf(mas->node))
-				alloc = true;
-			mas_prev_node(&cp, 0);
-			printk("prev node is %p\n", cp.node);
-			if (!mas_is_none(&cp)) {
-				end = mas_data_end(&cp);
-				printk("prev end %u and new_end %u\n", end, new_end);
-				mab_shift_right(b_node, new_end + 1, end + 1, alloc);
-				mas_mab_cp(&cp, 0, end + 1, b_node, 0);
-				end += new_end + 1;
-				p_slot--;
-			}
+			split = mab_calc_split(b_node, b_end, slot_cnt,
+					       orig_l_mas->min);
+			r = mt_mk_node(ma_mnode_ptr(mas_next_alloc(mas)),
+				       mte_node_type(orig_l_mas->node));
 		}
-
-		mas_dup_state(&cp, mas);
-		mas_set_slot(&cp, p_slot);
-		/* @end will now have the size of b_node.  This can be used to
-		 * determine if a right node is needed.
-		 * @p_slot is also pointing to the left node entry in the parent.
-		 */
-		left = mt_mk_node(ma_mnode_ptr(mas_next_alloc(mas)),
-				  mte_node_type(mas->node));
-
-		if (!mte_is_leaf(left)) {
-			mte_set_parent(b_node->slot[l_child], left, l_child);
-			mte_set_parent(b_node->slot[r_child], left, r_child);
+		if (b_end > slot_cnt*2)
+			m = mt_mk_node(ma_mnode_ptr(mas_next_alloc(mas)),
+				       mte_node_type(orig_l_mas->node));
+		printk("Splitting %u at %u\n", b_end, split);
+
+		/* Set parents from previous run */
+		if (l_mas.node) {
+			printk("Set parent of %p to either %p or %p\n", mas_mn(&l_mas),
+			       l, r);
+			printk("child %u split %u\n", child, split);
+			if (child <= split)
+				mte_set_parent(l_mas.node, l, child);
+			else
+				mte_set_parent(l_mas.node, r, child - split);
+		}
+		if (m_mas.node) {
+			child *= 2;
+			if (child <= split)
+				mte_set_parent(m_mas.node, l, child);
+			else
+				mte_set_parent(m_mas.node, r, child - split);
+			child++;
+		}
+		if (r_mas.node) {
+			child++;
+			if (child <= split)
+				mte_set_parent(r_mas.node, l, child);
+			else
+				mte_set_parent(r_mas.node, r, child - split);
 		}
 
-		/* Note, failure to allocate would set cp.node to error code
-		 * here and not mas->node.
-		 */
+		/* Copy data from b_node to new nodes */
+		l_mas.node = l;
+		m_mas.node = m;
+		r_mas.node = r;
+
+		l_mas.min = orig_l_mas->min;
+		mab_mas_cp(b_node, 0, split, &l_mas, 0);
+		l_mas.max = b_node->pivot[split];
+		printk("set from split l_mas.max %lu and %lu\n", l_mas.max, l_mas.min);
+		r_mas.max = l_mas.max;
 
-		printk("\t%s: end %u slot_Cnt %u\n", __func__, end, slot_cnt);
-		if (end < slot_cnt) {
-			split = end;
-			printk("Using end %u\n", split);
+
+		if (m) {
+			mab_mas_cp(b_node, 1 + split, split * 2, &m_mas, 0);
+			m_mas.min = b_node->pivot[split] + 1;
+			split *= 2;
+			m_mas.max = b_node->pivot[split];
+		}
+		if (r) {
+			printk("there is a r\n");
+			mab_mas_cp(b_node, 1 + split, b_end, &r_mas, 0);
+			r_mas.min = b_node->pivot[split] + 1;
+			r_mas.max = b_node->pivot[b_end];
+			printk("r_max.max from %u\n", b_end);
 		}
-		else if (mte_is_leaf(mas->node)) // leaf split is special
-			split = mab_calc_split(b_node, end, slot_cnt,
-					       mas->min);
-		else // internal nodes share 1/2, with the larger half going left.
-			split = (end + 1) / 2;
-
-		printk("Cp 0-%u\n", split);
-		cp.node = left;
-		printk("%s: left will have %u - %u\n", __func__, 0, split + 1);
-		mab_mas_cp(b_node, 0, split + 1, &cp, 0);
-		l_piv = cp.max;
-		if (mt_is_alloc(mas->tree))
-			l_gap = mas_find_gap(&cp);
 
-		if (end >= slot_cnt) {
-			right = mt_mk_node(ma_mnode_ptr(mas_next_alloc(mas)),
-					   mte_node_type(mas->node));
-			cp.node = right;
-			printk("%s: right will have %u - %u\n", __func__, split + 1, end + 1);
-			mab_mas_cp(b_node, split + 1, end + 1, &cp, 0);
-			if (mt_is_alloc(mas->tree))
-				r_gap = mas_find_gap(&cp);
 
-		} else {
-			// Totally merged with the left.
-			right = NULL;
+		/* Copy data from next level in the tree to b_node from next iteration */
+		memset(b_node, 0, sizeof(struct maple_big_node));
+		orig_l_mas->depth++;
+
+		if (!l_mas.min && l_mas.max == ULONG_MAX) {
+			printk("New root detected by range\n");
+
+			mas_mn(&l_mas)->parent = ma_parent_ptr(
+			      ((unsigned long)mas->tree | MA_ROOT_PARENT));
+			mas->depth = orig_l_mas->depth;
+			orig_l_mas->node = l_mas.node;
+			return 0;
 		}
 
+		mas_ascend(orig_l_mas);
+		mas_ascend(orig_r_mas);
+
+		printk("l_mas min is %lu\n", orig_l_mas->min);
+		mas_set_slot(orig_r_mas, 0);
+		orig_r_mas->index = r_mas.max + 1;
+		printk("Update r_mas.max to %lu\n", r_mas.max);
+		if (orig_r_mas->index) {
+			printk("R walk %p for %lu\n", mas_mn(orig_r_mas), orig_r_mas->index);
+			orig_r_mas->index++;
+			orig_r_mas->last = orig_r_mas->index;
+			mas_node_walk(orig_r_mas,
+				      mte_node_type(orig_r_mas->node),
+				      &range_min, &range_max);
+			r_slot = mas_get_slot(orig_r_mas);
+			orig_r_mas->index = orig_r_mas->last = r_mas.max;
+		}
 
+		mas_set_slot(orig_l_mas, 0);
+		orig_l_mas->index = l_mas.min;
+		printk("L walk %p for %lu\n", mas_mn(orig_l_mas), orig_l_mas->index); 
+		mas_node_walk(orig_l_mas, mte_node_type(orig_l_mas->node),
+			      &range_min, &range_max);
+		l_slot = mas_get_slot(orig_l_mas);
 
-		mas_ascend(mas);
-		printk("Next empty! %p\n\n", mas_mn(mas));
+		b_end = 0;
+		if (l_slot) {
+			printk("Copy in orig_l_mas %p\n", mas_mn(orig_l_mas));
+			b_end = mas_mab_cp(orig_l_mas, 0, l_slot - 1, b_node, 0);
+		}
+		// Not a leaf means new l_mas is placed at b_end.
+		printk("Put L %p at %u %lu\n", l_mas.node, b_end, l_mas.max);
+		child = b_end;
+		b_node->slot[b_end] = l_mas.node;
+		if(mt_is_alloc(mas->tree)) {
+			b_node->gap[b_end] = mas_find_gap(&l_mas);
+			printk("gap %u is %lu\n", b_end, b_node->gap[b_end]);
+		}
+		b_node->pivot[b_end++] = l_mas.max;
+		printk("l_mas max is %lu\n", b_node->pivot[b_end - 1]);
 
-		if (!mte_is_root(parent.node))
-			mas_ascend(&parent);
+		if (m) {
+			// FIXME: What about descend_adopt?
+			printk("Put M! %p at %u %lu\n", m_mas.node, b_end, m_mas.max);
+			b_node->slot[b_end] = m;
+			if(mt_is_alloc(mas->tree)) {
+				b_node->gap[b_end] =
+					mas_find_gap(&m_mas);
+				printk("gap %u is %lu\n", b_end, b_node->gap[b_end]);
+			}
+			b_node->pivot[b_end++] = m_mas.max;
+		}
 
-		end = mas_data_end(mas);
-		if (mte_is_root(mas->node)) {
-			if (!end || (end == 1 && !right)) {
-				mas_mn(&cp)->parent = mas_mn(&parent)->parent;
-				goto new_root_node;
+		if (r) {
+			printk("Put R %p at %u %lu\n", r_mas.node, b_end, r_mas.max);
+			b_node->slot[b_end] = r;
+			if(mt_is_alloc(mas->tree)) {
+				b_node->gap[b_end] =
+					mas_find_gap(&r_mas);
+				printk("gap %u is %lu\n", b_end, b_node->gap[b_end]);
 			}
+			b_node->pivot[b_end++] = r_mas.max;
+			printk("r_mas max is %lu\n", b_node->pivot[b_end]);
 		}
 
-		memset(b_node, 0, sizeof(struct maple_big_node));
-		b = mas_mab_cp(mas, 0, p_slot, b_node, 0);
-		r_child = l_child = b;
-		printk("Set %u to %p\n", b, mte_to_node(left));
-		b_node->slot[b] = left;
-		if (mt_is_alloc(mas->tree))
-			b_node->gap[b] = l_gap;
-		b_node->pivot[b++] = l_piv;
-		p_slot++;
-		if (right) {
-			printk("Include right at b_node %u\n", b);
-			r_child = b;
-			b_node->slot[b] = right;
-			printk("Set %u to %p\n", b, mte_to_node(right));
-			if (mt_is_alloc(mas->tree))
-				b_node->gap[b] = r_gap;
-			b_node->pivot[b++] = mas_get_safe_pivot(mas, p_slot);
+		// Add orig_r_mas->node to clean_list.
+		if ((orig_r_mas->last) &&
+		    (b_node->pivot[b_end - 1] < ULONG_MAX)) {
+			printk("Copy in orig_r_mas %p\n", mas_mn(orig_r_mas));
+			b_end = mas_mab_cp(orig_r_mas, r_slot,
+					   mas_data_end(orig_r_mas),
+					   b_node, b_end);
+		}
+
+		// Attempt to balance from this parent
+		if (b_end - 1 < mt_min_slot_cnt(orig_l_mas->node)) {
+			unsigned char end;
+			printk("\t merge with other node\n");
+			if (mas_next_sibling(orig_r_mas)) {
+				printk("r sibling merger\n");
+				end = mas_data_end(orig_r_mas);
+				b_end = mas_mab_cp(orig_r_mas, 0, end, b_node,
+						   b_end);
+				if (!count)
+					count++;
+			} else if (mas_prev_sibling(orig_l_mas)) {
+				printk("l sibling merger\n");
+				end = mas_data_end(orig_l_mas);
+				// shift b_node by prev size
+				mab_shift_right(b_node, b_end - 1, end + 1,
+						(mt_is_alloc(mas->tree) ? true : false));
+				// copy in prev.
+				mas_mab_cp(orig_l_mas, 0, end, b_node, 0);
+				b_end += end + 1;
+				if (!count)
+					count++;
+			}
 		}
-		p_slot++;
 
-		if (end >= p_slot)
-			b = mas_mab_cp(mas, p_slot, end + 1, b_node, b) + 1;
 
-		// If the parent didn't decrease in size, then we are done.
-		if (b >= mt_min_slot_cnt(mas->node)) {
-			end = b;
-			break;
+		// Ensure there is enough data for the next iteration.
+		if (b_end - 1 < mt_min_slot_cnt(orig_l_mas->node)) {
+			unsigned char end;
+			printk("\tSteal some data b_end = %u\n", b_end);
+			mas_set_slot(orig_r_mas,
+				     mte_parent_slot(orig_r_mas->node));
+			mas_next_node(orig_r_mas, ULONG_MAX);
+			if (!mas_is_none(orig_r_mas)) {
+				// Add r_mas.node to clean_list.
+				// r_mas is now at the next node..
+				printk("Taking from right\n");
+				end = mas_data_end(orig_r_mas);
+				b_end = mas_mab_cp(orig_r_mas, 0, end,
+						   b_node, b_end);
+			} else {
+				printk("Trying left\n");
+				// Put left into right.
+				mas_dup_state(orig_r_mas, orig_l_mas);
+				mas_dup_state(&r_mas, &l_mas);
+				r_slot = l_slot;
+				// Use prev for left.
+				mas_prev_node(orig_l_mas, 0);
+				if (mas_is_none(orig_l_mas)) {
+					// This is going to be a new root of
+					// only what is in b_node
+					printk("%s: %d New root? %u\n",
+					       __func__, __LINE__, b_end);
+					mas_dup_state(orig_l_mas, orig_r_mas);
+					mas->depth = orig_l_mas->depth;
+					b_end--;
+					break;
+				}
+
+				mas_set_slot(orig_l_mas, 0);
+				orig_l_mas->index = orig_l_mas->min;
+				l_slot = 0;
+				end = mas_data_end(orig_l_mas);
+				// shift b_node by prev size
+				mab_shift_right(b_node, b_end - 1, end + 1,
+						(mt_is_alloc(mas->tree) ? true : false));
+				// copy in prev.
+				mas_mab_cp(orig_l_mas, 0, end, b_node, 0);
+				b_end += end + 1;
+				printk("b_end is %u\n", b_end);
+			}
+			if (!count)
+				count++;
 		}
+		printk("\t\t%d %u\n", __LINE__, b_end);
+		printk("Next\n");
+	}
+
+	l_mas.node = mt_mk_node(ma_mnode_ptr(mas_next_alloc(mas)),
+				    mte_node_type(orig_l_mas->node));
+
+	printk("All done making replacement at %p\n", mas_mn(&l_mas));
+	mab_mas_cp(b_node, 0, b_end, &l_mas, 0);
+	printk("\t\t==>Setting parent of %p to %p[%u]\n", l, mas_mn(&l_mas),
+	       child);
+
+	mte_set_parent(l, l_mas.node, child);
+	if (m)
+		mte_set_parent(m, l_mas.node, ++child);
 
-		new_end = b - 1;
-		printk("Going with new_end = %u\n", new_end);
+	if (r)
+		mte_set_parent(r, l_mas.node, ++child);
+
+	if (!b_end) {
+		mas_mn(&l_mas)->parent = ma_parent_ptr(
+				((unsigned long)mas->tree | MA_ROOT_PARENT));
+	} else {
+		mas_mn(&l_mas)->parent = mas_mn(orig_l_mas)->parent;
 	}
 
-	/* In the end, b_node will have the contents of the new parent - unless
-	 * it is root
-	 */
-	printk("\nThis is the end\n");
-	cp.node = mt_mk_node(ma_mnode_ptr(mas_next_alloc(mas)),
-			     mte_node_type(mas->node));
-	mas_mn(&cp)->parent = mas_mn(mas)->parent;
-	printk("the end copies 0-%u\n", end+1);
-	mab_mas_cp(b_node, 0, end + 1, &cp, 0);
+	orig_l_mas->node = l_mas.node;
 
+	return b_end;
+}
 
+static inline int mas_rebalance(struct ma_state *mas,
+				   struct maple_big_node *b_node,
+				   unsigned char new_end)
+{
+	char empty = mas->full_cnt * -1;
+	unsigned char b_end = 0;
 
-new_root_node:
+	printk("%s: rebalance %u levels\n", __func__, empty);
+
+	mas_node_cnt(mas, 1 + empty * 2);
+	if (mas_is_err(mas))
+		return 0;
 
-	mas->node = cp.node;
-	/* Kill leaf node(s) */
-	if (!mte_is_root(cp.node)) {
-		mas_set_node_dead(&orig_mas);
-		smp_wmb();
+	MA_STATE(l_mas, mas->tree, mas->index, mas->last);
+	MA_STATE(r_mas, mas->tree, mas->index, mas->last);
+
+	mas_dup_state(&l_mas, mas);
+	mas_dup_state(&r_mas, mas);
+
+
+
+	if (mas_next_sibling(&r_mas)) {
+		b_end = mas_mab_cp(&r_mas, 0, mas_data_end(&r_mas), b_node,
+				   new_end + 1);
+		r_mas.last = r_mas.index = r_mas.max;
+
+	} else {
+		unsigned char shift;
+		mas_prev_sibling(&l_mas);
+		if (mas_is_none(&l_mas)) {
+			printk("%s: Drop a level..\n", __func__);
+			b_end = 0;
+			goto new_root_node;
+		}
+		shift = mas_data_end(&l_mas) + 1;
+		mab_shift_right(b_node, new_end, shift,
+				(mt_is_alloc(mas->tree) ? true : false));
+		b_end = mas_mab_cp(&l_mas, 0, mas_data_end(&l_mas), b_node, 0);
+		b_end += new_end + 1;
+		printk("b_end is %u\n", b_end);
+		l_mas.index = l_mas.last = l_mas.min;
 	}
 
-	/* Insert new data */
+	b_end = mas_combine_separate(mas, &l_mas, &r_mas, b_node, b_end, empty);
+
+new_root_node:
+	if (!b_end) {
+		printk("new root\n");
+	}
+
+	// Set up mas for insertion.
+	mas_set_node_dead(mas);
+	mas->node = l_mas.node;
+	smp_wmb();
+
+	// FIXME: Mark modified nodes as dead.
+
+	// Insert new sub-tree
+	// FIXME: If mas isn't being replaced, kill the right node and add it to
+	// to the list.
 	_mas_replace(mas, false, false, false);
 
+	if (mte_is_leaf(mas->node))
+		return 1;
+
 	mas_descend_adopt(mas);
 
 	if (mt_is_alloc(mas->tree))
 		mas_update_gap(mas, false);
-	/* Adoption routine... */
-
-
 
+	// Free list of collected nodes.
+	// FIXME
 
-	return 0;
+	return 1;
 }
 
 static inline int mas_split(struct ma_state *mas,
@@ -1988,12 +2275,12 @@ static inline int mas_split(struct ma_state *mas,
 						mas->min);
 			printk("Splitting at %u\n", split);
 			if (split < slot_cnt)
-				i = mab_mas_cp(b_node, 0, split + 1, &l_mas, 0);
+				i = mab_mas_cp(b_node, 0, split, &l_mas, 0);
 			else
 				i = mab_mas_cp(b_node, 0, slot_cnt, &l_mas, 0);
 
 			mte_set_pivot(r_mas.node, 0, r_mas.max);
-			mab_mas_cp(b_node, i, new_end+ 1, &r_mas, 0);
+			mab_mas_cp(b_node, i, new_end, &r_mas, 0);
 
 			printk("l_max is %lu\n", l_mas.max);
 			mas_set_slot(&l_mas, mte_parent_slot(mas->node));
@@ -2019,11 +2306,11 @@ static inline int mas_split(struct ma_state *mas,
 			split = slot_cnt / 2;
 			printk("split is %u %u %u\n", split, mas_get_slot(&l_mas),
 			       mas_get_slot(&r_mas));
-			i = mab_mas_cp(b_node, 0, split + 1, &l_mas, 0);
+			i = mab_mas_cp(b_node, 0, split, &l_mas, 0);
 			l_mas.max = b_node->pivot[i - 1];
 			printk("l_mas.max %lu\n", l_mas.max);
 			r_mas.min = l_mas.max + 1;
-			mab_mas_cp(b_node, i, slot_cnt + 1, &r_mas, 0);
+			mab_mas_cp(b_node, i, slot_cnt, &r_mas, 0);
 			p_slot = mas_get_slot(&orig_l_mas);
 			printk("setting parents of %p and %p to %p and %p\n",
 			       mas_mn(&orig_l_mas), mas_mn(&orig_r_mas),
@@ -2060,8 +2347,9 @@ static inline int mas_split(struct ma_state *mas,
 			mas_set_slot(mas, mte_parent_slot(mas->node));
 		}
 
-		if (cp)
-			i = mas_mab_cp(mas, 0, mas_get_slot(&l_mas), b_node, 0);
+		if (cp && mas_get_slot(&l_mas))
+			i = mas_mab_cp(mas, 0, mas_get_slot(&l_mas) - 1,
+				       b_node, 0);
 
 		split = i;
 		printk("Split is %u\n", split);
@@ -2131,11 +2419,11 @@ static inline int mas_commit_b_node(struct ma_state *mas,
 	new_node = mt_mk_node(mas_next_alloc(mas), mte_node_type(mas->node));
 	printk("Set %p parent to %p\n", mte_to_node(new_node), mas_mn(mas)->parent);
 	mte_to_node(new_node)->parent = mas_mn(mas)->parent;
-	printk("Replace %p\n", mas_mn(mas));
+	printk("going to replace %p\n", mas_mn(mas));
 	mas->node = new_node;
 
 	printk("Copy out 0-%u\n", end);
-	mab_mas_cp(b_node, 0, end + 1, mas, 0);
+	mab_mas_cp(b_node, 0, end, mas, 0);
 	_mas_replace(mas, true, false, true);
 	if (mt_is_alloc(mas->tree))
 		mas_update_gap(mas, false);
@@ -2302,6 +2590,12 @@ static inline bool mas_node_walk(struct ma_state *mas, enum maple_type type,
 	mas_set_slot(mas, i);
 	return ret;
 }
+static inline int mas_cnt_positive(struct ma_state *mas)
+{
+	if (mas->full_cnt < 0)
+		return mas->full_cnt * -1;
+	return mas->full_cnt;
+}
 static inline void mas_cnt_full(struct ma_state *mas)
 {
 	if (mas->full_cnt < 0)
@@ -2436,21 +2730,15 @@ static inline int mas_spanning_store(struct ma_state *mas, void *entry)
 	unsigned long range_min, range_max;
 	unsigned char b_end = 0; // big_node end.
 	unsigned char l_slot, r_slot;
+	unsigned char count = 0;
 	struct maple_big_node b_node;
 	int node_cnt = 0;
-	void *contents = NULL;
-	struct maple_enode *l, *r, *next = NULL;
-
-	MA_STATE(orig_l_mas, mas->tree, mas->index, mas->index);
-	MA_STATE(orig_r_mas, mas->tree, mas->index, mas->index);
 
 	// Holds new left and right sub-tree
 	MA_STATE(l_mas, mas->tree, mas->index, mas->index);
 	MA_STATE(r_mas, mas->tree, mas->index, mas->index);
 
 	// Leaf nodes
-	MA_STATE(prev_l_mas, mas->tree, mas->index, mas->index);
-	MA_STATE(prev_r_mas, mas->tree, mas->index, mas->index);
 	printk("%s: %d\n", __func__, __LINE__);
 
 	if (mas->full_cnt > 0)
@@ -2471,12 +2759,12 @@ static inline int mas_spanning_store(struct ma_state *mas, void *entry)
 		return 0;
 
 	mas_dup_state(&l_mas, mas);
-	l_mas.last = l_mas.index; // Avoid spanning store
 
 	// Set up right side.
 	mas_dup_state(&r_mas, mas);
 	r_mas.last++;
 	r_mas.index = r_mas.last;
+	mas_set_slot(&r_mas, 0);
 	__mas_walk(&r_mas, &range_min, &range_max);
 	r_mas.last = r_mas.index = mas->last;
 	r_slot = mas_get_slot(&r_mas);
@@ -2488,20 +2776,18 @@ static inline int mas_spanning_store(struct ma_state *mas, void *entry)
 			mas->last = mas_get_safe_pivot(&r_mas, r_slot);
 			if (!mas->last)
 				mas->last = mas->max;
-			r_mas.last = r_mas.index = mas->last;
 			r_slot++;
 		}
 		if (range_max == r_mas.max)
 			printk("NULL could be in the next node?\n");
 	}
+	r_mas.last = r_mas.index = mas->last;
+	mas_set_slot(&r_mas, r_slot);
 
-	// Set up lift side.
+	// Set up left side.
+	mas_set_slot(&l_mas, 0);
 	__mas_walk(&l_mas, &range_min, &range_max);
 	l_slot = mas_get_slot(&l_mas);
-	printk("Start at %u\n", l_slot);
-	contents = mas_get_rcu_slot(&l_mas, l_slot);
-
-
 	if (!entry) {
 		/* Check if there is a null in the previous slot on the left */
 		if (!l_slot) {
@@ -2514,311 +2800,55 @@ static inline int mas_spanning_store(struct ma_state *mas, void *entry)
 			l_slot--;
 		}
 	}
-	printk("end at %u\n", l_slot);
-
-	printk("%s: final range is %lu-%lu at slot %u or %u\n", __func__,
-	       mas->index, mas->last, mas_get_slot(&l_mas), mas_get_slot(&r_mas));
-
-
-	mas_dup_state(&orig_l_mas, &l_mas);
-	mas_dup_state(&orig_r_mas, &r_mas);
-	while (orig_l_mas.node != mas->span_enode) {
-		unsigned char split, slot_cnt = mt_slot_count(orig_l_mas.node);
-		memset(&b_node, 0, sizeof(struct maple_big_node));
-		// Add l_mas.node to clean_list.
-		// Fill out big node with left contents.
-		printk("\n%s: %p and %p\n", __func__, mas_mn(&orig_l_mas),
-		       mas_mn(&orig_r_mas));
-		printk("l_slot %u r_slot %u\n", l_slot, r_slot);
-		b_end = mas_mab_cp(&orig_l_mas, 0, l_slot, &b_node, 0);
-
-		// if leaf set data
-		if (mte_is_leaf(orig_l_mas.node)) {
-			printk("Leaf and b_end %u\n", b_end);
-			// Terminate old entry at l_slot.
-			if (range_min < mas->index) {
-				b_node.slot[b_end] = contents;
-				b_node.pivot[b_end++] = mas->index - 1;
-			}
-
-			// Insert new entry.
-			b_node.slot[b_end] = entry;
-			b_node.pivot[b_end] = mas->last;
-			printk("Set %lu to %p\n", mas->last, entry);
-		} else {
-			// Not a leaf means new l_mas is placed at b_end.
-			b_node.slot[b_end] = l_mas.node;
-			if(mt_is_alloc(mas->tree)) {
-				b_node.gap[b_end] = mas_find_gap(&l_mas);
-				printk("gap %u is %lu\n", b_end, b_node.gap[b_end]);
-			}
-			b_node.pivot[b_end] = l_mas.max;
-			printk("l_mas max is %lu\n", b_node.pivot[b_end - 1]);
-
-			if (next) {
-				// FIXME: What about gap?
-				// FIXME: What about descend_adopt?
-				b_node.slot[++b_end] = next;
-				b_node.pivot[b_end] = r_mas.min - 1;
-			}
-			if (r_mas.node != l_mas.node) {
-				b_node.slot[++b_end] = r_mas.node;
-				if(mt_is_alloc(mas->tree)) {
-					b_node.gap[b_end] =
-						mas_find_gap(&r_mas);
-					printk("gap %u is %lu\n", b_end, b_node.gap[b_end]);
-				}
-				b_node.pivot[b_end] = r_mas.max;
-				printk("r_mas max is %lu\n", b_node.pivot[b_end - 1]);
-			}
-		}
-
-		// Add orig_r_mas.node to clean_list.
-		if (b_node.pivot[b_end] < ULONG_MAX) {
-			b_end = mas_mab_cp(&orig_r_mas, r_slot,
-					   mas_data_end(&orig_r_mas) + 1,
-					   &b_node, b_end + 1);
-		}
-
-		if (b_end < mt_min_slot_cnt(orig_l_mas.node)) {
-			unsigned char end;
-			printk("\tSteal some data from right\n");
-			mas_set_slot(&orig_r_mas,
-				     mte_parent_slot(orig_r_mas.node));
-			mas_next_node(&orig_r_mas, ULONG_MAX);
-			if (!mas_is_none(&orig_r_mas)) {
-				// Add r_mas.node to clean_list.
-				// r_mas is now at the next node..
-				end = mas_data_end(&orig_r_mas);
-				b_end = mas_mab_cp(&orig_r_mas, 0, end + 1,
-						   &b_node, b_end + 1);
-			} else {
-				// Put left into right.
-				mas_dup_state(&orig_r_mas, &orig_l_mas);
-				mas_dup_state(&r_mas, &l_mas);
-				r_slot = l_slot;
-				// Use prev for left.
-				mas_prev_node(&orig_l_mas, 0);
-				if (mas_is_none(&orig_l_mas)) {
-					// This is going to be a new root of
-					// only what is in b_node
-					printk("%s: %d New root?\n", __func__, __LINE__);
-					return 0;
-				}
-
-				mas_set_slot(&orig_l_mas, 0);
-				l_slot = 0;
-//				printk("Walk %p for %lu\n", mas_mn(&orig_l_mas), orig_l_mas.index); 
-//				mas_node_walk(&orig_l_mas, mte_node_type(orig_l_mas.node),
-//								      &range_min, &range_max);
-//				l_slot = mas_get_slot(&orig_l_mas);
-
-				end = mas_data_end(&orig_l_mas);
-				// shift b_node by prev size
-				mab_shift_right(&b_node, b_end, end,
-				    (mt_is_alloc(mas->tree) ? true : false));
-				// copy in prev.
-				mas_mab_cp(&orig_l_mas, 0, end, &b_node, 0);
-				b_end += end + 1;
-				printk("b_end is %u\n", b_end);
-			}
-		}
-		/* Now that l_mas data and r_mas data is in big_node, divide the data
-		 * into two nodes of equal size and continue to do so up the tree for
-		 * each level.
-		 */
-
-		/* Calc split between 1 or 3 nodes */
-		printk("b_end %u slot_cnt %u\n", b_end, slot_cnt);
-		if (b_end < slot_cnt) {
-			split = b_end;
-		} else {
-			split = mab_calc_split(&b_node, b_end - 1, slot_cnt,
-					       orig_l_mas.min);
-		}
-
-		printk("%s: %u entries in b_node split is %u\n", __func__,
-		       b_end, split);
-
-
-		l = mt_mk_node(ma_mnode_ptr(mas_next_alloc(mas)),
-			       mte_node_type(orig_l_mas.node));
-
-		if (split == b_end)
-			r = NULL;
-		else
-			r = mt_mk_node(ma_mnode_ptr(mas_next_alloc(mas)),
-				       mte_node_type(orig_r_mas.node));
-
-		// Set the parents of the previous loop.
-		if (!mte_is_leaf(orig_l_mas.node)) {
-			if (l_slot <= split)
-				mte_set_parent(l_mas.node, l, l_slot);
-			else
-				mte_set_parent(l_mas.node, r, l_slot - split);
-			l_slot++;
-			if (next) {
-				if (l_slot <= split)
-					mte_set_parent(next, l, l_slot);
-				else
-					mte_set_parent(next, r, l_slot - split);
-				l_slot++;
-			}
-			if (r_mas.node != l_mas.node) {
-				if (l_slot <= split)
-					mte_set_parent(r_mas.node, l, l_slot);
-				else
-					mte_set_parent(r_mas.node, r,
-						       l_slot - split);
-			}
-
-		}
-
-		if (b_end > (slot_cnt + mt_slot_count(orig_r_mas.node)))
-			next = mt_mk_node(ma_mnode_ptr(mas_next_alloc(mas)),
-					  mte_node_type(orig_r_mas.node));
-
-		l_mas.node = l;
-		r_mas.node = l;
-		if (r)
-			r_mas.node = r;
-
-		l_mas.min = orig_l_mas.min;
-		mab_mas_cp(&b_node, 0, split + 1, &l_mas, 0);
-		l_mas.max = b_node.pivot[split];
-		printk("l_mas is %lu split %u\n", l_mas.max, split);
-
-		if (r)
-			r_mas.min = l_mas.max + 1;
-		if (next) {
-			printk("Copy to 3rd node\n");
-			r_mas.node = next;
-			mab_mas_cp(&b_node, split + 1, 1 + split * 2, &r_mas, 0);
-			r_mas.node = r;
-			split *= 2;
-			r_mas.min = b_node.pivot[split];
-		}
-		if (r) {
-			mab_mas_cp(&b_node, split + 1, b_end + 1, &r_mas, 0);
-			r_mas.max = b_node.pivot[b_end];
-			printk("r_mas.max is %lu\n", r_mas.max);
-		} else {
-			r_mas.max = l_mas.max;
-		}
-
-		if (mte_is_leaf(l)) {
-			mas_dup_state(&prev_l_mas, &l_mas);
-			if (l_mas.node == r_mas.node)
-				mas_dup_state(&prev_r_mas, &l_mas);
-			else
-				mas_dup_state(&prev_r_mas, &r_mas);
-		}
-
-
-		printk("Setup for next loop %p %p\n", mas_mn(&orig_l_mas),
-		       mas_mn(&orig_r_mas));
-		mas_ascend(&orig_l_mas);
-		mas_ascend(&orig_r_mas);
-
-		mas_set_slot(&orig_r_mas, 0);
-		orig_r_mas.index = r_mas.max;
-		if (r_mas.max != ULONG_MAX)
-			orig_r_mas.index++;
-
-		orig_r_mas.last = orig_r_mas.index;
-		printk("node r walk %p for %lu\n", mas_mn(&orig_r_mas), orig_r_mas.index);
-		mas_node_walk(&orig_r_mas, mte_node_type(orig_r_mas.node),
-			      &range_min, &range_max);
-		r_slot = mas_get_slot(&orig_r_mas);
-		orig_r_mas.index = orig_r_mas.last = r_mas.max;
-
-		mas_set_slot(&orig_l_mas, 0);
-		printk("Walk l %p for %lu\n", mas_mn(&orig_l_mas), orig_l_mas.index); 
-		mas_node_walk(&orig_l_mas, mte_node_type(orig_l_mas.node),
-			      &range_min, &range_max);
-		l_slot = mas_get_slot(&orig_l_mas);
-	}
-	printk("Done looping over spanning mess at %p\n", mas_mn(&orig_l_mas));
-
-	if (!l_slot && mte_is_root(orig_l_mas.node) &&
-	    l_mas.max == orig_l_mas.max) {
-		printk("New root %p\n", mas_mn(&l_mas));
-		l = l_mas.node;
-		goto new_root;
-	}
-	memset(&b_node, 0, sizeof(struct maple_big_node));
-	b_end = mas_mab_cp(&orig_l_mas, 0, l_slot, &b_node, 0);
-	b_node.slot[b_end] = l_mas.node;
-	b_node.gap[b_end] = mas_find_gap(&l_mas);
-	printk("Set b_end %u to left %p gap %lu\n", b_end, l_mas.node, b_node.gap[b_end]);
-	printk("l_mas.max = %lu\n", l_mas.max);
-	b_node.pivot[b_end] = l_mas.max;
-	if (next) {
-		b_node.slot[++b_end] = next;
-		// FIXME: Need to find gap.
-		printk("Set b_end %u to next %p\n", b_end, next);
-		b_node.pivot[++b_end] = r_mas.min - 1;
-	}
-
-	if (l_mas.node != r_mas.node) {
-		b_node.slot[++b_end] = r_mas.node;
-		b_node.gap[b_end] = mas_find_gap(&r_mas);
-		printk("Set b_end %u to right %p %lu gap %lu\n", b_end, r_mas.node, r_mas.max, b_node.gap[b_end]);
-		b_node.pivot[b_end] = r_mas.max;
-	}
-
-	while (mas_get_safe_pivot(&orig_l_mas, r_slot) <= mas->last &&
-	       r_slot <= mas_data_end(&orig_l_mas)) {
-		printk("skip %u\n", r_slot);
-		r_slot++;
-	}
-
-	if (orig_l_mas.max != r_mas.max &&
-	    orig_l_mas.max != l_mas.max)
-		b_end = mas_mab_cp(&orig_l_mas, r_slot,
-				   mas_data_end(&orig_l_mas) + 1, &b_node,
-				   b_end + 1);
-
-
-	// Check for overfull node and split upwards or minimum entries.
-	if (b_end > mt_slot_count(orig_l_mas.node)) {
-		mas_cnt_full(mas);
-		printk("Splitting up required (%u)\n", b_end);
-	} else if (!mte_is_root(orig_l_mas.node) &&
-		   b_end < mt_min_slot_cnt(orig_l_mas.node)) {
-		printk("Rebalance needed (%u)\n", b_end);
-		mas_cnt_empty(mas);
-		_mas_rebalance(mas, &b_node, b_end);
+	l_mas.last = mas->last;
+	l_mas.index = mas->index;
+	mas_set_slot(&l_mas, l_slot);
+
+	printk("%s: final range is %lu-%lu at slot %u\n", __func__,
+	       mas->index, mas->last, mas_get_slot(&l_mas));
+
+
+	// Copy l_mas and store the value in b_node.
+	b_end = mas_store_b_node(&l_mas, &b_node, entry);
+	// Copy r_mas into b_node.
+	b_end = mas_mab_cp(&r_mas, r_slot, mas_data_end(&r_mas),
+			   &b_node, b_end + 1);
+	// Stop spanning searches by searching for just index.
+	l_mas.index = l_mas.last = mas->index;
+	// Calc the number of iterations of combining and splitting that will
+	// need to occur.
+	count = mas_cnt_positive(mas) + mas->tree->ma_height - mas->depth + 1;
+
+	// Combine l_mas and r_mas and split them up evenly again.
+	printk("b_end start is %u and count is %u\n", b_end, count);
+	l_mas.depth = 0;
+	b_end = mas_combine_separate(mas, &l_mas, &r_mas, &b_node, b_end,
+			     count);
+	printk("Done looping over spanning mess at %p\n", mas_mn(&l_mas));
+	printk("\n\n");
+
+	if (!b_end) {
+		printk("new root\n");
 	}
+	printk("Going to insert %p\n", mas_mn(&l_mas));
 
-	l = mt_mk_node(ma_mnode_ptr(mas_next_alloc(mas)),
-		       mte_node_type(orig_l_mas.node));
-
-	printk("l_slot is %u for %p\n", l_slot, mas_mn(&l_mas));
-	mte_set_parent(l_mas.node, l, l_slot);
-	if (next)
-		mte_set_parent(next, l, ++l_slot);
-	if (r_mas.node != l_mas.node) {
-		mte_set_parent(r_mas.node, l, ++l_slot);
-		printk("l_slot is %u for %p\n", l_slot, mas_mn(&r_mas));
-	}
-
-
-	mas_dup_state(&l_mas, &orig_l_mas);
-	l_mas.node = l;
-	mab_mas_cp(&b_node, 0, b_end + 1, &l_mas, 0);
-new_root:
-	mas_mn(&l_mas)->parent = mas_mn(mas)->parent;
+	// Set up mas for insertion.
+	mas_set_node_dead(mas);
 	mas->node = l_mas.node;
+	smp_wmb();
 
 	// FIXME: Mark modified nodes as dead.
 
 	// Insert new sub-tree
+	// FIXME: If mas isn't being replaced, kill the right node and add it to
+	// to the list.
 	_mas_replace(mas, false, false, false);
 
 	if (mte_is_leaf(mas->node))
 		return 1;
 
+	mt_dump(mas->tree);
+
 	mas_descend_adopt(mas);
 
 	if (mt_is_alloc(mas->tree))
@@ -2910,27 +2940,11 @@ static inline void *_mas_store(struct ma_state *mas, void *entry, bool overwrite
 	}
 
 	memset(&b_node, 0, sizeof(struct maple_big_node));
-	printk("Copy 0-%u\n", slot);
-	new_end = mas_mab_cp(mas, 0, slot, &b_node, 0);
+	printk("Copy %p 0-%u\n", mas_mn(mas), slot);
+	mas_set_slot(mas, slot);
+	new_end = mas_store_b_node(mas, &b_node, entry);
 	printk("new_end is %u\n", new_end);
 
-	if(r_min < mas->index) {
-		b_node.slot[new_end] = content;
-		printk("Setting %u %p\n", new_end, content);
-		b_node.pivot[new_end++] = mas->index - 1;
-	}
-
-	b_node.slot[new_end] = entry;
-	printk("Setting %u %p\n", new_end, entry);
-	b_node.pivot[new_end] = mas->last;
-
-	if (mas->last < r_max) {
-		b_node.slot[++new_end] = content;
-		printk("Setting %u %p\n", new_end, content);
-		b_node.pivot[new_end] = r_max;
-	}
-
-
 	// Check if this is an append operation.
 	end = mas_data_end(mas);
 	if ((new_end < slot_cnt) && ((slot > end) || !end)) {
@@ -2944,15 +2958,6 @@ static inline void *_mas_store(struct ma_state *mas, void *entry, bool overwrite
 		goto done;
 	}
 
-	// Skip overwritten data
-	do {
-		printk("Skip %u\n", slot);
-		r_max = mas_get_safe_pivot(mas, ++slot);
-	} while ((r_max <= mas->last) && (slot <= end));
-
-	if (mas->last < mas_get_safe_pivot(mas, slot))
-		new_end = mas_mab_cp(mas, slot, end + 1, &b_node, new_end + 1);
-
 	// count the node as full if it has not already been counted.
 	if (new_end >= slot_cnt && end < slot_cnt)
 		mas_cnt_full(mas);
@@ -3291,7 +3296,6 @@ static inline bool mas_prev_nentry(struct ma_state *mas, unsigned long limit,
 	mas_set_slot(mas, slot);
 	return true;
 }
-
 /** Private
  * mas_next_nentry() - Next node entry.  Set the @mas slot to the next valid
  * entry and range_start to the start value for that entry.  If there is no
diff --git a/lib/test_maple_tree.c b/lib/test_maple_tree.c
index 04dcf52dc2c5..da4b81fe9d54 100644
--- a/lib/test_maple_tree.c
+++ b/lib/test_maple_tree.c
@@ -30859,7 +30859,6 @@ static int maple_tree_seed(void)
 	mtree_init(&tree, 0);
 	check_erase2_sets(&tree);
 	mtree_destroy(&tree);
-	return (maple_tree_tests_run == maple_tree_tests_passed) ? 0 : -EINVAL;
 
 	mtree_init(&tree, MAPLE_ALLOC_RANGE);
 	check_gap_combining(&tree);
-- 
2.50.1