diff --git a/hdl/rtl/hydra_core.vhd b/hdl/rtl/hydra_core.vhd
index 5064ef88b9680b38e51f06b28e73ee71b989c9f1..fa1b83efefcfe9366258374e8bbc5c298fda49a3 100644
--- a/hdl/rtl/hydra_core.vhd
+++ b/hdl/rtl/hydra_core.vhd
@@ -143,7 +143,7 @@ architecture arch of hydra_core is
 begin
   dwb_o <= dwb_out;
 
-  cpu_rst2 <= "111" when rst_n_i = '0' or cpu_rst_n_i = '0' else "000";
+  cpu_rst2 <= "111" when rst_n_i = '0' or cpu_rst_n_i = '0' else cpu_rst;
   cpu_err_o <= '1' when cpu_rst /= "111" else '0';
       
   inst_cpus : entity work.hydra_triple_cpu
@@ -500,22 +500,29 @@ begin
       else
         case state is
           when S_VOTER =>
-            cpu_rst <= "000";
+            if dm_cycle_in_progress = '0' then
+              --  Release reset only when the bus is idle.
+              cpu_rst <= "000";
+            end if;
             --  Software can clear recovery flag.
             if cpu_wr = '1' and cpu_recovery_in = '0' then
               cpu_recovery <= '0';
             end if;
+            --  Data bus errors counter
             if err_cpu_dm = '1' then
               nbr_cpu_data_err <= std_logic_vector(unsigned(nbr_cpu_data_err) + 1);
             end if;
             if cpu_sync = "110" or cpu_sync = "101" or cpu_sync = "011" then
               --  Disable the cpu out of sync.
               cpu_rst <= not cpu_sync;
+              --  Error counter
               nbr_cpu_iaddr_err <= std_logic_vector(unsigned(nbr_cpu_iaddr_err) + 1);
+              --  Go to lock-step
               state <= S_LOCK;
             end if;
           when S_LOCK =>
             if cpu_wr = '1' and cpu_recovery_in = '1' then
+              --  Start recovery (reset)
               cpu_recovery <= '1';
             end if;
         end case;
diff --git a/sw/sf2-test/main.c b/sw/sf2-test/main.c
index 54bdf2ce29c1c5778d287fc61b6a900483764d45..4f588eb37408145d34259480337d3279a5c66179 100644
--- a/sw/sf2-test/main.c
+++ b/sw/sf2-test/main.c
@@ -47,7 +47,7 @@ uart_raw_putc (unsigned char c)
   /* Wait until TEMT (transmit empty) is set.  */
   /* NB: bit 5 for RX not empty */
   while (!(*(volatile unsigned*)UART_LSR & 0x20))
-    ;
+    SUPERVISOR->wd_key = WD_KEY;
   *(volatile unsigned *)UART_TX = c;
 }
 
@@ -137,11 +137,14 @@ unreachable(void)
 int
 main (void)
 {
+  SUPERVISOR->wd_key = WD_KEY;
   uart_init ();
 
   uart_puts ("Rst: ");
   unsigned v = SUPERVISOR->reset_cause;
   uart_put_hex_digit (v & 0x0f);
+  uart_puts (", tst: ");
+  uart_put_hex_digit (next_test);
   uart_putc('\n');
 
   if (v == 0)