Index: generic/src/mm/as.c
===================================================================
--- generic/src/mm/as.c	(revision 040542aa18c2031f9e751738ff84939e979b0d49)
+++ generic/src/mm/as.c	(revision 567891259f1a757ed23463709625e224f86d885d)
@@ -93,6 +93,6 @@
 static as_area_t *find_area_and_lock(as_t *as, __address va);
 static bool check_area_conflicts(as_t *as, __address va, size_t size, as_area_t *avoid_area);
-int used_space_insert(as_area_t *a, __address page, count_t count);
-int used_space_remove(as_area_t *a, __address page, count_t count);
+static int used_space_insert(as_area_t *a, __address page, count_t count);
+static int used_space_remove(as_area_t *a, __address page, count_t count);
 
 /** Initialize address space subsystem. */
@@ -245,5 +245,6 @@
 	
 	if (pages < area->pages) {
-		int i;
+		bool cond;
+		__address start_free = area->base + pages*PAGE_SIZE;
 
 		/*
@@ -251,24 +252,59 @@
 		 * No need to check for overlaps.
 		 */
-		for (i = pages; i < area->pages; i++) {
-			pte_t *pte;
+
+		/*
+		 * Remove frames belonging to used space starting from
+		 * the highest addresses downwards until an overlap with
+		 * the resized address space area is found. Note that this
+		 * is also the right way to remove part of the used_space
+		 * B+tree leaf list.
+		 */		
+		for (cond = true; cond;) {
+			btree_node_t *node;
+		
+			ASSERT(!list_empty(&area->used_space.leaf_head));
+			node = list_get_instance(area->used_space.leaf_head.prev, btree_node_t, leaf_link);
+			if ((cond = (bool) node->keys)) {
+				__address b = node->key[node->keys - 1];
+				count_t c = (count_t) node->value[node->keys - 1];
+				int i = 0;
 			
-			/*
-			 * Releasing physical memory.
-			 * This depends on the fact that the memory was allocated using frame_alloc().
-			 */
-			page_table_lock(as, false);
-			pte = page_mapping_find(as, area->base + i*PAGE_SIZE);
-			if (pte && PTE_VALID(pte)) {
-				__address frame;
-
-				ASSERT(PTE_PRESENT(pte));
-				frame = PTE_GET_FRAME(pte);
-				page_mapping_remove(as, area->base + i*PAGE_SIZE);
-				page_table_unlock(as, false);
-
-				frame_free(ADDR2PFN(frame));
-			} else {
-				page_table_unlock(as, false);
+				if (overlaps(b, c*PAGE_SIZE, area->base, pages*PAGE_SIZE)) {
+					
+					if (b + c*PAGE_SIZE <= start_free) {
+						/*
+						 * The whole interval fits completely
+						 * in the resized address space area.
+						 */
+						break;
+					}
+		
+					/*
+					 * Part of the interval corresponding to b and c
+					 * overlaps with the resized address space area.
+					 */
+		
+					cond = false;	/* we are almost done */
+					i = (start_free - b) >> PAGE_WIDTH;
+					if (!used_space_remove(area, start_free, c - i))
+						panic("Could not remove used space.");
+				} else {
+					/*
+					 * The interval of used space can be completely removed.
+					 */
+					if (!used_space_remove(area, b, c))
+						panic("Could not remove used space.\n");
+				}
+			
+				for (; i < c; i++) {
+					pte_t *pte;
+			
+					page_table_lock(as, false);
+					pte = page_mapping_find(as, b + i*PAGE_SIZE);
+					ASSERT(pte && PTE_VALID(pte) && PTE_PRESENT(pte));
+					frame_free(ADDR2PFN(PTE_GET_FRAME(pte)));
+					page_mapping_remove(as, b + i*PAGE_SIZE);
+					page_table_unlock(as, false);
+				}
 			}
 		}
@@ -313,5 +349,4 @@
 	__address base;
 	ipl_t ipl;
-	int i;
 
 	ipl = interrupts_disable();
@@ -325,8 +360,8 @@
 	}
 
-	base = area->base;	
-	for (i = 0; i < area->pages; i++) {
-		pte_t *pte;
-
+	base = area->base;
+	if (!(area->flags & AS_AREA_DEVICE)) {
+		bool cond;	
+	
 		/*
 		 * Releasing physical memory.
@@ -334,19 +369,38 @@
 		 * areas backing frame_alloc()'ed memory.
 		 */
-		page_table_lock(as, false);
-		pte = page_mapping_find(as, area->base + i*PAGE_SIZE);
-		if (pte && PTE_VALID(pte)) {
-			ASSERT(PTE_PRESENT(pte));
-			page_mapping_remove(as, area->base + i*PAGE_SIZE);
-			if (area->flags & AS_AREA_DEVICE) {
-				__address frame;
-				frame = PTE_GET_FRAME(pte);
-				frame_free(ADDR2PFN(frame));
+
+		/*
+		 * Visit only the pages mapped by used_space B+tree.
+		 * Note that we must be very careful when walking the tree
+		 * leaf list and removing used space as the leaf list changes
+		 * unpredictibly after each remove. The solution is to actually
+		 * not walk the tree at all, but to remove items from the head
+		 * of the leaf list until there are some keys left.
+		 */
+		for (cond = true; cond;) {
+			btree_node_t *node;
+		
+			ASSERT(!list_empty(&area->used_space.leaf_head));
+			node = list_get_instance(area->used_space.leaf_head.next, btree_node_t, leaf_link);
+			if ((cond = (bool) node->keys)) {
+				__address b = node->key[0];
+				count_t i;
+				pte_t *pte;
+			
+				for (i = 0; i < (count_t) node->value[0]; i++) {
+					page_table_lock(as, false);
+					pte = page_mapping_find(as, b + i*PAGE_SIZE);
+					ASSERT(pte && PTE_VALID(pte) && PTE_PRESENT(pte));
+					frame_free(ADDR2PFN(PTE_GET_FRAME(pte)));
+					page_mapping_remove(as, b + i*PAGE_SIZE);
+					page_table_unlock(as, false);
+				}
+				if (!used_space_remove(area, b, i))
+					panic("Could not remove used space.\n");
 			}
-			page_table_unlock(as, false);
-		} else {
-			page_table_unlock(as, false);
-		}
-	}
+		}
+	}
+	btree_destroy(&area->used_space);
+
 	/*
 	 * Invalidate TLB's.
@@ -419,5 +473,4 @@
 	mutex_unlock(&src_area->lock);
 	mutex_unlock(&src_as->lock);
-
 
 	if (src_size != acc_size) {
@@ -513,8 +566,10 @@
 	area = find_area_and_lock(as, page);
 	if (!area) {
-		panic("page not part of any as_area\n");
+		panic("Page not part of any as_area.\n");
 	}
 
 	page_mapping_insert(as, page, frame, get_area_flags(area));
+	if (!used_space_insert(area, page, 1))
+		panic("Could not insert used space.\n");
 	
 	mutex_unlock(&area->lock);
@@ -606,4 +661,6 @@
 	 */
 	page_mapping_insert(AS, page, frame, get_area_flags(area));
+	if (!used_space_insert(area, ALIGN_DOWN(page, PAGE_SIZE), 1))
+		panic("Could not insert used space.\n");
 	page_table_unlock(AS, false);
 	
@@ -997,10 +1054,10 @@
 		} else if ((page == left_pg + left_cnt*PAGE_SIZE) && (page + count*PAGE_SIZE == right_pg)) {
 			/* The interval can be added by merging the two already present intervals. */
-			node->value[node->keys - 1] += (count_t) count + right_cnt;
+			node->value[node->keys - 1] += count + right_cnt;
 			btree_remove(&a->used_space, right_pg, leaf);
 			return 1; 
 		} else if (page == left_pg + left_cnt*PAGE_SIZE) {
 			/* The interval can be added by simply growing the left interval. */
-			node->value[node->keys - 1] += (count_t) count;
+			node->value[node->keys - 1] += count;
 			return 1;
 		} else if (page + count*PAGE_SIZE == right_pg) {
@@ -1009,5 +1066,5 @@
 			 * interval down and increasing its size accordingly.
 			 */
-			leaf->value[0] += (count_t) count;
+			leaf->value[0] += count;
 			leaf->key[0] = page;
 			return 1;
@@ -1038,5 +1095,5 @@
 			 */
 			leaf->key[0] = page;
-			leaf->value[0] += (count_t) count;
+			leaf->value[0] += count;
 			return 1;
 		} else {
@@ -1071,10 +1128,10 @@
 		} else if ((page == left_pg + left_cnt*PAGE_SIZE) && (page + count*PAGE_SIZE == right_pg)) {
 			/* The interval can be added by merging the two already present intervals. */
-			leaf->value[leaf->keys - 1] += (count_t) count + right_cnt;
+			leaf->value[leaf->keys - 1] += count + right_cnt;
 			btree_remove(&a->used_space, right_pg, node);
 			return 1; 
 		} else if (page == left_pg + left_cnt*PAGE_SIZE) {
 			/* The interval can be added by simply growing the left interval. */
-			leaf->value[leaf->keys - 1] += (count_t) count;
+			leaf->value[leaf->keys - 1] +=  count;
 			return 1;
 		} else if (page + count*PAGE_SIZE == right_pg) {
@@ -1083,5 +1140,5 @@
 			 * interval down and increasing its size accordingly.
 			 */
-			node->value[0] += (count_t) count;
+			node->value[0] += count;
 			node->key[0] = page;
 			return 1;
@@ -1104,9 +1161,9 @@
 		 
 		if (overlaps(page, count*PAGE_SIZE, left_pg, left_cnt*PAGE_SIZE)) {
-			/* The interval intersects with the right interval. */
+			/* The interval intersects with the left interval. */
 			return 0;
 		} else if (left_pg + left_cnt*PAGE_SIZE == page) {
 			/* The interval can be added by growing the left interval. */
-			leaf->value[leaf->keys - 1] += (count_t) count;
+			leaf->value[leaf->keys - 1] += count;
 			return 1;
 		} else {
@@ -1142,10 +1199,10 @@
 			} else if ((page == left_pg + left_cnt*PAGE_SIZE) && (page + count*PAGE_SIZE == right_pg)) {
 				/* The interval can be added by merging the two already present intervals. */
-				leaf->value[i - 1] += (count_t) count + right_cnt;
+				leaf->value[i - 1] += count + right_cnt;
 				btree_remove(&a->used_space, right_pg, leaf);
 				return 1; 
 			} else if (page == left_pg + left_cnt*PAGE_SIZE) {
 				/* The interval can be added by simply growing the left interval. */
-				leaf->value[i - 1] += (count_t) count;
+				leaf->value[i - 1] += count;
 				return 1;
 			} else if (page + count*PAGE_SIZE == right_pg) {
@@ -1154,5 +1211,5 @@
 			 	 * interval down and increasing its size accordingly.
 			 	 */
-				leaf->value[i] += (count_t) count;
+				leaf->value[i] += count;
 				leaf->key[i] = page;
 				return 1;
@@ -1199,4 +1256,5 @@
 		} else if (count == pages) {
 			btree_remove(&a->used_space, page, leaf);
+			return 1;
 		} else {
 			/*
@@ -1207,5 +1265,5 @@
 				if (leaf->key[i] == page) {
 					leaf->key[i] += count*PAGE_SIZE;
-					leaf->value[i] -= (count_t) count;
+					leaf->value[i] -= count;
 					return 1;
 				}
@@ -1227,8 +1285,8 @@
 				 * updating the size of the bigger interval.
 				 */
-				node->value[node->keys - 1] -= (count_t) count;
+				node->value[node->keys - 1] -= count;
 				return 1;
 			} else if (page + count*PAGE_SIZE < left_pg + left_cnt*PAGE_SIZE) {
-				count_t new_cnt = (left_pg + left_cnt*PAGE_SIZE) - (page + count*PAGE_SIZE);
+				count_t new_cnt;
 				
 				/*
@@ -1238,5 +1296,6 @@
 				 * also inserting a new interval.
 				 */
-				node->value[node->keys - 1] -= (count_t) count + new_cnt;
+				new_cnt = ((left_pg + left_cnt*PAGE_SIZE) - (page + count*PAGE_SIZE)) >> PAGE_WIDTH;
+				node->value[node->keys - 1] -= count + new_cnt;
 				btree_insert(&a->used_space, page + count*PAGE_SIZE, (void *) new_cnt, leaf);
 				return 1;
@@ -1259,8 +1318,8 @@
 				 * of the bigger interval.
 				 */
-				leaf->value[leaf->keys - 1] -= (count_t) count;
+				leaf->value[leaf->keys - 1] -= count;
 				return 1;
 			} else if (page + count*PAGE_SIZE < left_pg + left_cnt*PAGE_SIZE) {
-				count_t new_cnt = (left_pg + left_cnt*PAGE_SIZE) - (page + count*PAGE_SIZE);
+				count_t new_cnt;
 				
 				/*
@@ -1270,5 +1329,6 @@
 				 * also inserting a new interval.
 				 */
-				leaf->value[leaf->keys - 1] -= (count_t) count + new_cnt;
+				new_cnt = ((left_pg + left_cnt*PAGE_SIZE) - (page + count*PAGE_SIZE)) >> PAGE_WIDTH;
+				leaf->value[leaf->keys - 1] -= count + new_cnt;
 				btree_insert(&a->used_space, page + count*PAGE_SIZE, (void *) new_cnt, leaf);
 				return 1;
@@ -1297,8 +1357,8 @@
 					 * of the bigger interval.
 					 */
-					leaf->value[i - 1] -= (count_t) count;
+					leaf->value[i - 1] -= count;
 					return 1;
 				} else if (page + count*PAGE_SIZE < left_pg + left_cnt*PAGE_SIZE) {
-					count_t new_cnt = (left_pg + left_cnt*PAGE_SIZE) - (page + count*PAGE_SIZE);
+					count_t new_cnt;
 				
 					/*
@@ -1308,5 +1368,6 @@
 					 * also inserting a new interval.
 					 */
-					leaf->value[i - 1] -= (count_t) count + new_cnt;
+					new_cnt = ((left_pg + left_cnt*PAGE_SIZE) - (page + count*PAGE_SIZE)) >> PAGE_WIDTH;
+					leaf->value[i - 1] -= count + new_cnt;
 					btree_insert(&a->used_space, page + count*PAGE_SIZE, (void *) new_cnt, leaf);
 					return 1;
