--- a/hotspot/src/share/vm/gc/g1/g1CollectedHeap.cpp	Mon Oct 19 16:33:12 2015 +0200
+++ b/hotspot/src/share/vm/gc/g1/g1CollectedHeap.cpp	Thu Nov 26 09:50:22 2015 +0100
@@ -36,7 +36,6 @@
 #include "gc/g1/g1CollectorPolicy.hpp"
 #include "gc/g1/g1CollectorState.hpp"
 #include "gc/g1/g1ErgoVerbose.hpp"
-#include "gc/g1/g1EvacFailure.hpp"
 #include "gc/g1/g1EvacStats.inline.hpp"
 #include "gc/g1/g1GCPhaseTimes.hpp"
 #include "gc/g1/g1Log.hpp"
@@ -4090,21 +4089,21 @@
   return true;
 }
 
+void G1CollectedHeap::restore_preserved_marks() {
+  G1RestorePreservedMarksTask rpm_task(_preserved_objs);
+  workers()->run_task(&rpm_task);
+}
+
 void G1CollectedHeap::remove_self_forwarding_pointers() {
-  double remove_self_forwards_start = os::elapsedTime();
-
   G1ParRemoveSelfForwardPtrsTask rsfp_task;
   workers()->run_task(&rsfp_task);
-
-  // Now restore saved marks, if any.
-  for (uint i = 0; i < ParallelGCThreads; i++) {
-    OopAndMarkOopStack& cur = _preserved_objs[i];
-    while (!cur.is_empty()) {
-      OopAndMarkOop elem = cur.pop();
-      elem.set_mark();
-    }
-    cur.clear(true);
-  }
+}
+
+void G1CollectedHeap::restore_after_evac_failure() {
+  double remove_self_forwards_start = os::elapsedTime();
+
+  remove_self_forwarding_pointers();
+  restore_preserved_marks();
 
   g1_policy()->phase_times()->record_evac_fail_remove_self_forwards((os::elapsedTime() - remove_self_forwards_start) * 1000.0);
 }
@@ -5193,7 +5192,7 @@
   g1_rem_set()->cleanup_after_oops_into_collection_set_do();
 
   if (evacuation_failed()) {
-    remove_self_forwarding_pointers();
+    restore_after_evac_failure();
 
     // Reset the G1EvacuationFailureALot counters and flags
     // Note: the values are reset only when an actual

--- a/hotspot/src/share/vm/gc/g1/g1CollectedHeap.hpp	Mon Oct 19 16:33:12 2015 +0200
+++ b/hotspot/src/share/vm/gc/g1/g1CollectedHeap.hpp	Thu Nov 26 09:50:22 2015 +0100
@@ -33,6 +33,7 @@
 #include "gc/g1/g1HRPrinter.hpp"
 #include "gc/g1/g1InCSetState.hpp"
 #include "gc/g1/g1MonitoringSupport.hpp"
+#include "gc/g1/g1EvacFailure.hpp"
 #include "gc/g1/g1EvacStats.hpp"
 #include "gc/g1/g1SATBCardTableModRefBS.hpp"
 #include "gc/g1/g1YCTypes.hpp"
@@ -780,20 +781,13 @@
   // forwarding pointers to themselves.  Reset them.
   void remove_self_forwarding_pointers();
 
-  struct OopAndMarkOop {
-   private:
-    oop _o;
-    markOop _m;
-   public:
-    OopAndMarkOop(oop obj, markOop m) : _o(obj), _m(m) {
-    }
+  // Restore the preserved mark words for objects with self-forwarding pointers.
+  void restore_preserved_marks();
 
-    void set_mark() {
-      _o->set_mark(_m);
-    }
-  };
+  // Restore the objects in the regions in the collection set after an
+  // evacuation failure.
+  void restore_after_evac_failure();
 
-  typedef Stack<OopAndMarkOop,mtGC> OopAndMarkOopStack;
   // Stores marks with the corresponding oop that we need to preserve during evacuation
   // failure.
   OopAndMarkOopStack*  _preserved_objs;

--- a/hotspot/src/share/vm/gc/g1/g1EvacFailure.cpp	Mon Oct 19 16:33:12 2015 +0200
+++ b/hotspot/src/share/vm/gc/g1/g1EvacFailure.cpp	Thu Nov 26 09:50:22 2015 +0100
@@ -259,3 +259,16 @@
   HeapRegion* hr = _g1h->start_cset_region_for_worker(worker_id);
   _g1h->collection_set_iterate_from(hr, &rsfp_cl);
 }
+
+G1RestorePreservedMarksTask::G1RestorePreservedMarksTask(OopAndMarkOopStack* preserved_objs) :
+  AbstractGangTask("G1 Restore Preserved Marks"),
+  _preserved_objs(preserved_objs) {}
+
+void G1RestorePreservedMarksTask::work(uint worker_id) {
+  OopAndMarkOopStack& cur = _preserved_objs[worker_id];
+  while (!cur.is_empty()) {
+    OopAndMarkOop elem = cur.pop();
+    elem.set_mark();
+  }
+  cur.clear(true);
+}

--- a/hotspot/src/share/vm/gc/g1/g1EvacFailure.hpp	Mon Oct 19 16:33:12 2015 +0200
+++ b/hotspot/src/share/vm/gc/g1/g1EvacFailure.hpp	Thu Nov 26 09:50:22 2015 +0100
@@ -32,6 +32,20 @@
 
 class G1CollectedHeap;
 
+class OopAndMarkOop {
+  oop _o;
+  markOop _m;
+ public:
+  OopAndMarkOop(oop obj, markOop m) : _o(obj), _m(m) {
+  }
+
+  void set_mark() {
+    _o->set_mark(_m);
+  }
+};
+
+typedef Stack<OopAndMarkOop,mtGC> OopAndMarkOopStack;
+
 // Task to fixup self-forwarding pointers
 // installed as a result of an evacuation failure.
 class G1ParRemoveSelfForwardPtrsTask: public AbstractGangTask {
@@ -45,4 +59,12 @@
   void work(uint worker_id);
 };
 
+class G1RestorePreservedMarksTask : public AbstractGangTask {
+  OopAndMarkOopStack* _preserved_objs;
+ public:
+  G1RestorePreservedMarksTask(OopAndMarkOopStack* preserved_objs);
+
+  void work(uint worker_id);
+};
+
 #endif // SHARE_VM_GC_G1_G1EVACFAILURE_HPP