static void make_response(struct xen_blkif *blkif, u64 id,
                          unsigned short op, int st);
 
-#define foreach_grant(pos, rbtree, node) \
-       for ((pos) = container_of(rb_first((rbtree)), typeof(*(pos)), node); \
+#define foreach_grant_safe(pos, n, rbtree, node) \
+       for ((pos) = container_of(rb_first((rbtree)), typeof(*(pos)), node), \
+            (n) = rb_next(&(pos)->node); \
             &(pos)->node != NULL; \
-            (pos) = container_of(rb_next(&(pos)->node), typeof(*(pos)), node))
+            (pos) = container_of(n, typeof(*(pos)), node), \
+            (n) = (&(pos)->node != NULL) ? rb_next(&(pos)->node) : NULL)
 
 
 static void add_persistent_gnt(struct rb_root *root,
        struct gnttab_unmap_grant_ref unmap[BLKIF_MAX_SEGMENTS_PER_REQUEST];
        struct page *pages[BLKIF_MAX_SEGMENTS_PER_REQUEST];
        struct persistent_gnt *persistent_gnt;
+       struct rb_node *n;
        int ret = 0;
        int segs_to_unmap = 0;
 
-       foreach_grant(persistent_gnt, root, node) {
+       foreach_grant_safe(persistent_gnt, n, root, node) {
                BUG_ON(persistent_gnt->handle ==
                        BLKBACK_INVALID_HANDLE);
                gnttab_set_unmap_op(&unmap[segs_to_unmap],
                        persistent_gnt->handle);
 
                pages[segs_to_unmap] = persistent_gnt->page;
-               rb_erase(&persistent_gnt->node, root);
-               kfree(persistent_gnt);
-               num--;
 
                if (++segs_to_unmap == BLKIF_MAX_SEGMENTS_PER_REQUEST ||
                        !rb_next(&persistent_gnt->node)) {
                        BUG_ON(ret);
                        segs_to_unmap = 0;
                }
+
+               rb_erase(&persistent_gnt->node, root);
+               kfree(persistent_gnt);
+               num--;
        }
        BUG_ON(num != 0);
 }