Index: cont.c
===================================================================
--- cont.c (revision 36623)
+++ cont.c (working copy)
@@ -880,6 +880,7 @@ make_passing_arg(int argc, VALUE *argv)
 {
     switch(argc) {
       case 0:
+      case -2:
 	return Qnil;
       case 1:
 	return argv[0];
@@ -1133,13 +1134,32 @@ rb_fiber_terminate(rb_fiber_t *fib)
     rb_fiber_transfer(return_fiber(), 1, &value);
 }
 
+static VALUE
+fiber_start_i(VALUE tag, rb_context_t *cont)
+{
+    rb_thread_t *th = GET_THREAD();
+    rb_proc_t *proc;
+    int argc;
+    VALUE *argv, args;
+    GetProcPtr(cont->saved_thread.first_proc, proc);
+    args = cont->value;
+    argv = (argc = cont->argc) > 1 ? RARRAY_PTR(args) : &args;
+    cont->value = Qnil;
+    th->errinfo = Qnil;
+    th->root_lep = rb_vm_ep_local_ep(proc->block.ep);
+    th->root_svar = Qnil;
+
+    cont->value = rb_vm_invoke_proc(th, proc, proc->block.self, argc, argv, 0);
+
+    return Qfalse;
+}
+
 void
 rb_fiber_start(void)
 {
     rb_thread_t *th = GET_THREAD();
     rb_fiber_t *fib;
     rb_context_t *cont;
-    rb_proc_t *proc;
     int state;
 
     GetFiberPtr(th->fiber, fib);
@@ -1147,18 +1167,8 @@ rb_fiber_start(void)
 
     TH_PUSH_TAG(th);
     if ((state = EXEC_TAG()) == 0) {
-	int argc;
-	VALUE *argv, args;
-	GetProcPtr(cont->saved_thread.first_proc, proc);
-	args = cont->value;
-	argv = (argc = cont->argc) > 1 ? RARRAY_PTR(args) : &args;
-	cont->value = Qnil;
-	th->errinfo = Qnil;
-	th->root_lep = rb_vm_ep_local_ep(proc->block.ep);
-	th->root_svar = Qnil;
-
 	fib->status = RUNNING;
-	cont->value = rb_vm_invoke_proc(th, proc, proc->block.self, argc, argv, 0);
+	rb_catch_obj(-2, fiber_start_i, (VALUE)cont);
     }
     TH_POP_TAG();
 
@@ -1253,6 +1263,7 @@ fiber_store(rb_fiber_t *next_fib)
 	/* restored */
 	GetFiberPtr(th->fiber, fib);
 	if (fib->cont.argc == -1) rb_exc_raise(fib->cont.value);
+	else if (fib->cont.argc == -2) rb_throw_obj(-2, Qnil);
 	return fib->cont.value;
     }
 #if !FIBER_USE_NATIVE
@@ -1496,6 +1507,56 @@ rb_fiber_s_current(VALUE klass)
     return rb_fiber_current();
 }
 
+static void
+fiber_terminate_on_th(rb_thread_t *th, int ignore_mark)
+{
+    VALUE fibval, current_fibval;
+    rb_fiber_t *fib, *root_fib, *prev_fiber;
+    rb_thread_t *_th = GET_THREAD();
+
+    fibval = th->root_fiber;
+    if (!RTEST(fibval)) return;
+    GetFiberPtr(fibval, root_fib);
+
+    rb_thread_set_current(th);
+    fib = root_fib->prev_fiber;
+    current_fibval = rb_fiber_current();
+    while (fib != root_fib) {
+	prev_fiber = fib->prev_fiber;
+	fibval = fib->cont.self;
+	if (fib->status == RUNNING && current_fibval != fibval) {
+	    if(ignore_mark || !rb_gc_marked_p(fibval)) {
+		fib->prev = current_fibval;
+		fiber_switch(fibval, -2, 0, 0);
+	    }
+	}
+	fib = prev_fiber;
+    }
+    rb_thread_set_current(_th);
+}
+
+static int
+fiber_terminate_unused_i(st_data_t key, st_data_t val, st_data_t data)
+{
+    rb_thread_t *th;
+
+    GetThreadPtr((VALUE)key, th);
+    fiber_terminate_on_th(th, FALSE);
+    return ST_CONTINUE;
+}
+
+void
+rb_fiber_terminate_unused(void)
+{
+    st_foreach(GET_VM()->living_threads, fiber_terminate_unused_i, 0);
+}
+
+void
+rb_thread_terminate_fibers(rb_thread_t *th)
+{
+    fiber_terminate_on_th(th, TRUE);
+}
+
 
 
 /*
Index: eval.c
===================================================================
--- eval.c (revision 36623)
+++ eval.c (working copy)
@@ -158,6 +158,12 @@ ruby_cleanup(volatile int ex)
     rb_thread_t *th = GET_THREAD();
     int nerr;
 
+    PUSH_TAG();
+    if ((state = EXEC_TAG()) == 0) {
+	rb_thread_terminate_fibers(th);
+    }
+    POP_TAG();
+
     rb_threadptr_interrupt(th);
     rb_threadptr_check_signal(th);
     PUSH_TAG();
Index: gc.c
===================================================================
--- gc.c (revision 36623)
+++ gc.c (working copy)
@@ -2163,9 +2163,26 @@ ready_to_gc(rb_objspace_t *objspace)
     return TRUE;
 }
 
+int
+rb_gc_marked_p(VALUE p)
+{
+    return MARKED_IN_BITMAP(GET_HEAP_BITMAP(p), p);
+}
+
 static void
 before_gc_sweep(rb_objspace_t *objspace)
 {
+    int _dont_gc;
+
+    _dont_gc = dont_gc;
+    during_gc = 0;
+    objspace->flags.dont_lazy_sweep = 1;
+    dont_gc = 1;
+    rb_fiber_terminate_unused();
+    dont_gc = _dont_gc;
+    during_gc = 0;
+    objspace->flags.dont_lazy_sweep = 0;
+
     objspace->heap.do_heap_free = (size_t)((heaps_used * HEAP_OBJ_LIMIT) * 0.65);
     objspace->heap.free_min = (size_t)((heaps_used * HEAP_OBJ_LIMIT)  * 0.2);
     if (objspace->heap.free_min < initial_free_min) {
Index: internal.h
===================================================================
--- internal.h (revision 36623)
+++ internal.h (working copy)
@@ -70,6 +70,7 @@ VALUE rb_insns_name_array(void);
 /* cont.c */
 VALUE rb_obj_is_fiber(VALUE);
 void rb_fiber_reset_root_local_storage(VALUE);
+void rb_fiber_terminate_unused(void);
 
 /* debug.c */
 PRINTF_ARGS(void ruby_debug_printf(const char*, ...), 1, 2);
@@ -107,6 +108,7 @@ void Init_File(void);
 /* gc.c */
 void Init_heap(void);
 void *ruby_mimmalloc(size_t size);
+int rb_gc_marked_p(VALUE);
 
 /* inits.c */
 void rb_call_inits(void);
Index: test/ruby/test_fiber.rb
===================================================================
--- test/ruby/test_fiber.rb (revision 36623)
+++ test/ruby/test_fiber.rb (working copy)
@@ -261,5 +261,69 @@ class TestFiber < Test::Unit::TestCase
     assert_equal(0, status.exitstatus, bug5700)
     assert_equal(false, status.signaled?, bug5700)
   end
+
+  def test_ensure
+    bug595 = '[ruby-dev:36511]'
+
+    assert_in_out_err([], <<-EOS, ["ok"], [], bug595)
+      Fiber.new{
+        begin
+          Fiber.yield
+        ensure
+          puts "ok"
+        end
+      }.resume
+    EOS
+
+    assert_in_out_err([], <<-EOS, ["ok"] * 1000, [], "[ruby-dev:41035]")
+      1000.times do
+        Fiber.new do
+          begin
+            Fiber.yield
+          ensure
+            puts "ok"
+          end
+        end.resume
+        GC.start
+      end
+    EOS
+    
+    assert_in_out_err([], <<-EOS, ["ok"], [], "[ruby-dev:41035]")
+      Thread.new do
+        Fiber.new do
+          begin
+            Fiber.yield
+          ensure
+            puts "ok"
+          end
+        end.resume
+      end
+      sleep 1
+    EOS
+
+    assert_normal_exit %q{
+      Fiber.new do
+        begin
+          Fiber.yield
+        ensure
+          raise
+        end
+      end.resume
+    }
+
+    assert_normal_exit %q{
+      Thread.new do
+        Fiber.new do
+          begin
+            Fiber.yield
+          ensure
+            raise
+          end
+        end.resume
+        sleep
+      end
+      sleep 0.1
+    }
+  end
 end
 
Index: thread.c
===================================================================
--- thread.c (revision 36623)
+++ thread.c (working copy)
@@ -507,6 +507,11 @@ thread_start_func_2(rb_thread_t *th, VALUE *stack_start, VALUE *register_stack_s
 
 	/* delete self other than main thread from living_threads */
 	if (th != main_th) {
+	    TH_PUSH_TAG(th);
+	    if (EXEC_TAG() == 0) {
+		rb_thread_terminate_fibers(th);
+	    }
+	    TH_POP_TAG();
 	    st_delete_wrap(th->vm->living_threads, th->self);
 	}
 
Index: vm_core.h
===================================================================
--- vm_core.h (revision 36623)
+++ vm_core.h (working copy)
@@ -736,6 +736,7 @@ int rb_vm_get_sourceline(const rb_control_frame_t *);
 VALUE rb_name_err_mesg_new(VALUE obj, VALUE mesg, VALUE recv, VALUE method);
 void rb_vm_stack_to_heap(rb_thread_t *th);
 void ruby_thread_init_stack(rb_thread_t *th);
+void rb_thread_terminate_fibers(rb_thread_t *);
 
 NOINLINE(void rb_gc_save_machine_context(rb_thread_t *));
 void rb_gc_mark_machine_stack(rb_thread_t *th);
