diff --git a/lib/spdylay_map.c b/lib/spdylay_map.c
index 977fe1ad..de98ebaf 100644
--- a/lib/spdylay_map.c
+++ b/lib/spdylay_map.c
@@ -24,24 +24,18 @@
  */
 #include "spdylay_map.h"
 
+typedef enum {
+  SUB_LEFT = 1,
+  SUB_RIGHT = 1 << 1,
+  SUB_ALL = (1 << 2) - 1
+} spdylay_map_subtr;
+
 void spdylay_map_init(spdylay_map *map)
 {
   map->root = NULL;
   map->size = 0;
 }
 
-static void spdylay_map_entry_free_recur(spdylay_map_entry *entry,
-                                         int (*func)(spdylay_map_entry *entry,
-                                                     void *ptr),
-                                         void *ptr)
-{
-  if(entry != NULL) {
-    spdylay_map_entry_free_recur(entry->left, func, ptr);
-    spdylay_map_entry_free_recur(entry->right, func, ptr);
-    func(entry, ptr);
-  }
-}
-
 void spdylay_map_free(spdylay_map *map)
 {
   map->root = NULL;
@@ -51,7 +45,24 @@ void spdylay_map_each_free(spdylay_map *map,
                            int (*func)(spdylay_map_entry *entry, void *ptr),
                            void *ptr)
 {
-  spdylay_map_entry_free_recur(map->root, func, ptr);
+  spdylay_map_entry *entry = map->root;
+  while(entry) {
+    if(entry->flags == SUB_ALL) {
+      spdylay_map_entry *parent = entry->parent;
+      func(entry, ptr);
+      entry = parent;
+    } else if(entry->flags == SUB_LEFT) {
+      entry->flags |= SUB_RIGHT;
+      if(entry->right) {
+        entry = entry->right;
+      }
+    } else {
+      entry->flags |= SUB_LEFT;
+      if(entry->left) {
+        entry = entry->left;
+      }
+    }
+  }
   map->root = NULL;
 }
 
@@ -74,8 +85,9 @@ static uint32_t hash32shift(uint32_t key)
 void spdylay_map_entry_init(spdylay_map_entry *entry, key_type key)
 {
   entry->key = key;
-  entry->left = entry->right = NULL;
+  entry->parent = entry->left = entry->right = NULL;
   entry->priority = hash32shift(key);
+  entry->flags = 0;
 }
 
 static spdylay_map_entry* rotate_left(spdylay_map_entry *entry)
@@ -83,6 +95,19 @@ static spdylay_map_entry* rotate_left(spdylay_map_entry *entry)
   spdylay_map_entry *root = entry->right;
   entry->right = root->left;
   root->left = entry;
+
+  root->parent = entry->parent;
+  entry->parent = root;
+  if(root->parent) {
+    if(root->parent->left == entry) {
+      root->parent->left = root;
+    } else {
+      root->parent->right = root;
+    }
+  }
+  if(entry->right) {
+    entry->right->parent = entry;
+  }
   return root;
 }
 
@@ -91,38 +116,74 @@ static spdylay_map_entry* rotate_right(spdylay_map_entry* entry)
   spdylay_map_entry *root = entry->left;
   entry->left = root->right;
   root->right = entry;
-  return root;
-}
 
-static spdylay_map_entry* insert_recur(spdylay_map_entry *entry,
-                                       spdylay_map_entry *new_entry,
-                                       int *error)
-{
-  if(entry == NULL) {
-    entry = new_entry;
-  } else if(new_entry->key == entry->key) {
-    *error = SPDYLAY_ERR_INVALID_ARGUMENT;
-  } else if(new_entry->key < entry->key) {
-    entry->left = insert_recur(entry->left, new_entry, error);
-  } else {
-    entry->right = insert_recur(entry->right, new_entry, error);
+  root->parent = entry->parent;
+  entry->parent = root;
+  if(root->parent) {
+    if(root->parent->left == entry) {
+      root->parent->left = root;
+    } else {
+      root->parent->right = root;
+    }
   }
-  if(entry->left != NULL && entry->priority > entry->left->priority) {
-    entry = rotate_right(entry);
-  } else if(entry->right != NULL && entry->priority > entry->right->priority) {
-    entry = rotate_left(entry);
+  if(entry->left) {
+    entry->left->parent = entry;
   }
-  return entry;
+  return root;
 }
 
 int spdylay_map_insert(spdylay_map *map, spdylay_map_entry *new_entry)
 {
-  int error = 0;
-  map->root = insert_recur(map->root, new_entry, &error);
-  if(!error) {
-    ++map->size;
+  spdylay_map_entry *entry = map->root, *parent = NULL;
+  if(map->root == NULL) {
+    map->root = new_entry;
+    map->size = 1;
+    return 0;
   }
-  return error;
+  /* Find position to insert. */
+  while(1) {
+    if(new_entry->key == entry->key) {
+      return SPDYLAY_ERR_INVALID_ARGUMENT;
+    } else {
+      if(new_entry->key < entry->key) {
+        if(entry->left) {
+          entry = entry->left;
+        } else {
+          parent = entry;
+          parent->left = new_entry;
+          break;
+        }
+      } else {
+        if(entry->right) {
+          entry = entry->right;
+        } else {
+          parent = entry;
+          parent->right = new_entry;
+          break;
+        }
+      }
+    }
+  }
+  new_entry->parent = parent;
+
+  /* Rotate tree to satisfy heap property. */
+  for(entry = parent; ; entry = entry->parent) {
+    if(entry->left && entry->priority > entry->left->priority) {
+      entry = rotate_right(entry);
+    } else if(entry->right && entry->priority > entry->right->priority) {
+      entry = rotate_left(entry);
+    } else {
+      /* At this point, tree forms heap. */
+      break;
+    }
+    /* If no parent is assigned, then it is a root node. */
+    if(!entry->parent) {
+      map->root = entry;
+      break;
+    }
+  }
+  ++map->size;
+  return 0;
 }
 
 spdylay_map_entry* spdylay_map_find(spdylay_map *map, key_type key)
@@ -140,51 +201,69 @@ spdylay_map_entry* spdylay_map_find(spdylay_map *map, key_type key)
   return NULL;
 }
 
-static spdylay_map_entry* remove_rotate_recur(spdylay_map_entry *entry)
-{
-  if(entry->left == NULL) {
-    spdylay_map_entry *right = entry->right;
-    return right;
-  } else if(entry->right == NULL) {
-    spdylay_map_entry *left = entry->left;
-    return left;
-  } else if(entry->left->priority < entry->right->priority) {
-    entry = rotate_right(entry);
-    entry->right = remove_rotate_recur(entry->right);
-    return entry;
-  } else {
-    entry = rotate_left(entry);
-    entry->left = remove_rotate_recur(entry->left);
-    return entry;
-  }
-}
-
-static spdylay_map_entry* remove_recur(spdylay_map_entry *entry, key_type key,
-                                      int *error)
-{
-  if(entry == NULL) {
-    *error = SPDYLAY_ERR_INVALID_ARGUMENT;
-  } else if(key < entry->key) {
-    entry->left = remove_recur(entry->left, key, error);
-  } else if(key > entry->key) {
-    entry->right = remove_recur(entry->right, key, error);
-  } else {
-    entry = remove_rotate_recur(entry);
-  }
-  return entry;
-}
-
 int spdylay_map_remove(spdylay_map *map, key_type key)
 {
-  if(map->root != NULL) {
-    int error = 0;
-    map->root = remove_recur(map->root, key, &error);
-    if(!error) {
-      --map->size;
-    }
-    return error;
+  spdylay_map_entry *entry = map->root;
+
+  if(map->root == NULL) {
+    return SPDYLAY_ERR_INVALID_ARGUMENT;
   }
-  return SPDYLAY_ERR_INVALID_ARGUMENT;
+  /* Locate entry to delete. */
+  while(entry) {
+    if(key < entry->key) {
+      entry = entry->left;
+    } else if(key > entry->key) {
+      entry = entry->right;
+    } else {
+      break;
+    }
+  }
+  if(!entry) {
+    /* Not found */
+    return SPDYLAY_ERR_INVALID_ARGUMENT;
+  }
+  /* Rotate and bubble down to satisfy heap property. */
+  for(;;) {
+    if(!entry->left) {
+      if(!entry->parent) {
+        map->root = entry->right;
+      } else if(entry->parent->left == entry) {
+        entry->parent->left = entry->right;
+      } else {
+        entry->parent->right = entry->right;
+      }
+      if(entry->right) {
+        entry->right->parent = entry->parent;
+      }
+      break;
+    } else if(!entry->right) {
+      if(!entry->parent) {
+        map->root = entry->left;
+      } else if(entry->parent->left == entry) {
+        entry->parent->left = entry->left;
+      } else {
+        entry->parent->right = entry->left;
+      }
+      if(entry->left) {
+        entry->left->parent = entry->parent;
+      }
+      break;
+    } else if(entry->left->priority < entry->right->priority) {
+      entry = rotate_right(entry);
+      if(!entry->parent) {
+        map->root = entry;
+      }
+      entry = entry->right;
+    } else {
+      entry = rotate_left(entry);
+      if(!entry->parent) {
+        map->root = entry;
+      }
+      entry = entry->left;
+    }
+  }
+  --map->size;
+  return 0;
 }
 
 size_t spdylay_map_size(spdylay_map *map)
@@ -192,24 +271,35 @@ size_t spdylay_map_size(spdylay_map *map)
   return map->size;
 }
 
-static int for_each(spdylay_map_entry *entry,
-                    int (*func)(spdylay_map_entry *entry, void *ptr),
-                    void *ptr)
-{
-  if(entry) {
-    int rv;
-    if((rv = for_each(entry->left, func, ptr)) != 0 ||
-       (rv = func(entry, ptr)) != 0 ||
-       (rv = for_each(entry->right, func, ptr)) != 0) {
-      return rv;
-    }
-  }
-  return 0;
-}
-
 int spdylay_map_each(spdylay_map *map,
                      int (*func)(spdylay_map_entry *entry, void *ptr),
                      void *ptr)
 {
-  return for_each(map->root, func, ptr);
+  spdylay_map_entry *entry = map->root;
+  while(entry) {
+    if(entry->flags == SUB_ALL) {
+      entry->flags = 0;
+      entry = entry->parent;
+    } else if(entry->flags == SUB_LEFT) {
+      int rv;
+      rv = func(entry, ptr);
+      if(rv != 0) {
+        while(entry) {
+          entry->flags = 0;
+          entry = entry->parent;
+        }
+        return rv;
+      }
+      entry->flags |= SUB_RIGHT;
+      if(entry->right) {
+        entry = entry->right;
+      }
+    } else {
+      entry->flags |= SUB_LEFT;
+      if(entry->left) {
+        entry = entry->left;
+      }
+    }
+  }
+  return 0;
 }
diff --git a/lib/spdylay_map.h b/lib/spdylay_map.h
index c9a99de5..4fcd7e67 100644
--- a/lib/spdylay_map.h
+++ b/lib/spdylay_map.h
@@ -39,8 +39,9 @@ typedef uint32_t pri_type;
 
 typedef struct spdylay_map_entry {
   key_type key;
-  struct spdylay_map_entry *left, *right;
+  struct spdylay_map_entry *parent, *left, *right;
   pri_type priority;
+  uint8_t flags;
 } spdylay_map_entry;
 
 typedef struct {
diff --git a/tests/main.c b/tests/main.c
index 131b6454..f0c2e6c2 100644
--- a/tests/main.c
+++ b/tests/main.c
@@ -68,6 +68,7 @@ int main(int argc, char* argv[])
    /* add the tests to the suite */
    if(!CU_add_test(pSuite, "pq", test_spdylay_pq) ||
       !CU_add_test(pSuite, "map", test_spdylay_map) ||
+      !CU_add_test(pSuite, "map_functional", test_spdylay_map_functional) ||
       !CU_add_test(pSuite, "map_each_free", test_spdylay_map_each_free) ||
       !CU_add_test(pSuite, "queue", test_spdylay_queue) ||
       !CU_add_test(pSuite, "buffer", test_spdylay_buffer) ||
diff --git a/tests/spdylay_map_test.c b/tests/spdylay_map_test.c
index 66cf5b3b..29d14b09 100644
--- a/tests/spdylay_map_test.c
+++ b/tests/spdylay_map_test.c
@@ -92,6 +92,57 @@ void test_spdylay_map(void)
   spdylay_map_free(&map);
 }
 
+static void shuffle(int *a, int n)
+{
+  int i;
+  for(i = n - 1; i >= 1; --i) {
+    size_t j = (int)((double)(i + 1) * rand() / (RAND_MAX + 1.0));
+    int t = a[j];
+    a[j] = a[i];
+    a[i] = t;
+  }
+}
+
+static int eachfun(spdylay_map_entry *entry, void *ptr)
+{
+  return 0;
+}
+
+#define NUM_ENT 6000
+strentry arr[NUM_ENT];
+int order[NUM_ENT];
+
+void test_spdylay_map_functional(void)
+{
+  spdylay_map map;
+  int i;
+
+  spdylay_map_init(&map);
+  for(i = 0; i < NUM_ENT; ++i) {
+    strentry_init(&arr[i], i + 1, "foo");
+    order[i] = i + 1;
+  }
+  // insertion
+  shuffle(order, NUM_ENT);
+  for(i = 0; i < NUM_ENT; ++i) {
+    CU_ASSERT(0 == spdylay_map_insert(&map, &arr[i].map_entry));
+  }
+  // traverse
+  spdylay_map_each(&map, eachfun, NULL);
+  // find
+  shuffle(order, NUM_ENT);
+  for(i = 0; i < NUM_ENT; ++i) {
+    spdylay_map_find(&map, order[i]);
+  }
+  // remove
+  shuffle(order, NUM_ENT);
+  for(i = 0; i < NUM_ENT; ++i) {
+    CU_ASSERT(0 == spdylay_map_remove(&map, order[i]));
+  }
+
+  spdylay_map_free(&map);
+}
+
 static int entry_free(spdylay_map_entry *entry, void *ptr)
 {
   free(entry);
diff --git a/tests/spdylay_map_test.h b/tests/spdylay_map_test.h
index 7cefec18..62bbaaad 100644
--- a/tests/spdylay_map_test.h
+++ b/tests/spdylay_map_test.h
@@ -26,6 +26,7 @@
 #define SPDYLAY_MAP_TEST_H
 
 void test_spdylay_map(void);
+void test_spdylay_map_functional(void);
 void test_spdylay_map_each_free(void);
 
 #endif /* SPDYLAY_MAP_TEST_H */