diff --git a/soc.vhdl b/soc.vhdl
index 045679f..4ccbc12 100644
--- a/soc.vhdl
+++ b/soc.vhdl
@@ -35,6 +35,8 @@ architecture behaviour of soc is
     signal wishbone_dcore_out : wishbone_master_out;
     signal wishbone_icore_in : wishbone_slave_out;
     signal wishbone_icore_out : wishbone_master_out;
+    signal wishbone_debug_in : wishbone_slave_out;
+    signal wishbone_debug_out : wishbone_master_out;
 
     -- Wishbone master (output of arbiter):
     signal wb_master_in : wishbone_slave_out;
@@ -75,16 +77,17 @@ begin
     -- Wishbone bus master arbiter & mux
     wishbone_arbiter_0: entity work.wishbone_arbiter
 	port map(
-	    clk => system_clk,
-	    rst => rst,
-	    wb1_in => wishbone_dcore_out,
-	    wb1_out => wishbone_dcore_in,
-	    wb2_in => wishbone_icore_out,
-	    wb2_out => wishbone_icore_in,
-	    wb_out => wb_master_out,
-	    wb_in => wb_master_in
+	    clk => system_clk, rst => rst,
+	    wb1_in => wishbone_dcore_out, wb1_out => wishbone_dcore_in,
+	    wb2_in => wishbone_icore_out, wb2_out => wishbone_icore_in,
+	    wb3_in => wishbone_debug_out, wb3_out => wishbone_debug_in,
+	    wb_out => wb_master_out, wb_in => wb_master_in
 	    );
 
+    -- Dummy wishbone debug module
+    wishbone_debug_out.cyc <= '0';
+    wishbone_debug_out.stb <= '0';
+
     -- Wishbone slaves address decoder & mux
     slave_intercon: process(wb_master_out, wb_bram_out, wb_uart0_out)
 	-- Selected slave
diff --git a/wishbone_arbiter.vhdl b/wishbone_arbiter.vhdl
index 7d5cbcd..d839b31 100644
--- a/wishbone_arbiter.vhdl
+++ b/wishbone_arbiter.vhdl
@@ -4,54 +4,76 @@ use ieee.std_logic_1164.all;
 library work;
 use work.wishbone_types.all;
 
+-- TODO: Use an array of master/slaves with parametric size
 entity wishbone_arbiter is
-    port (
-        clk     : in std_ulogic;
-        rst     : in std_ulogic;
+    port (clk     : in std_ulogic;
+	  rst     : in std_ulogic;
 
-        wb1_in  : in wishbone_master_out;
-        wb1_out : out wishbone_slave_out;
+	  wb1_in  : in wishbone_master_out;
+	  wb1_out : out wishbone_slave_out;
 
-        wb2_in  : in wishbone_master_out;
-        wb2_out : out wishbone_slave_out;
+	  wb2_in  : in wishbone_master_out;
+	  wb2_out : out wishbone_slave_out;
 
-        wb_out  : out wishbone_master_out;
-        wb_in   : in wishbone_slave_out
-        );
+	  wb3_in  : in wishbone_master_out;
+	  wb3_out : out wishbone_slave_out;
+
+	  wb_out  : out wishbone_master_out;
+	  wb_in   : in wishbone_slave_out
+	  );
 end wishbone_arbiter;
 
 architecture behave of wishbone_arbiter is
-    type wishbone_arbiter_state_t is (IDLE, WB1_BUSY, WB2_BUSY);
+    type wishbone_arbiter_state_t is (IDLE, WB1_BUSY, WB2_BUSY, WB3_BUSY);
     signal state : wishbone_arbiter_state_t := IDLE;
 begin
-    wb1_out <= wb_in when state = WB1_BUSY else wishbone_slave_out_init;
-    wb2_out <= wb_in when state = WB2_BUSY else wishbone_slave_out_init;
 
-    wb_out <= wb1_in when state = WB1_BUSY else wb2_in when state = WB2_BUSY else wishbone_master_out_init;
+    wishbone_muxes: process(state, wb_in, wb1_in, wb2_in, wb3_in)
+    begin
+	-- Requests from masters are fully muxed
+	wb_out <= wb1_in when state = WB1_BUSY else
+		  wb2_in when state = WB2_BUSY else
+		  wb3_in when state = WB3_BUSY else
+		  wishbone_master_out_init;
+
+	-- Responses from slave don't need to mux the data bus
+	wb1_out.dat <= wb_in.dat;
+	wb2_out.dat <= wb_in.dat;
+	wb3_out.dat <= wb_in.dat;
+	wb1_out.ack <= wb_in.ack when state = WB1_BUSY else '0';
+	wb2_out.ack <= wb_in.ack when state = WB2_BUSY else '0';
+	wb3_out.ack <= wb_in.ack when state = WB3_BUSY else '0';
+    end process;
 
     wishbone_arbiter_process: process(clk)
     begin
-        if rising_edge(clk) then
-            if rst = '1' then
-                state <= IDLE;
-            else
-                case state is
-                    when IDLE =>
-                        if wb1_in.cyc = '1' then
-                            state <= WB1_BUSY;
-                        elsif wb2_in.cyc = '1' then
-                            state <= WB2_BUSY;
-                        end if;
-                    when WB1_BUSY =>
-                        if wb1_in.cyc = '0' then
-                            state <= IDLE;
-                        end if;
-                    when WB2_BUSY =>
-                        if wb2_in.cyc = '0' then
-                            state <= IDLE;
-                        end if;
-                end case;
-            end if;
-        end if;
+	if rising_edge(clk) then
+	    if rst = '1' then
+		state <= IDLE;
+	    else
+		case state is
+		when IDLE =>
+		    if wb1_in.cyc = '1' then
+			state <= WB1_BUSY;
+		    elsif wb2_in.cyc = '1' then
+			state <= WB2_BUSY;
+		    elsif wb3_in.cyc = '1' then
+			state <= WB3_BUSY;
+		    end if;
+		when WB1_BUSY =>
+		    if wb1_in.cyc = '0' then
+			state <= IDLE;
+		    end if;
+		when WB2_BUSY =>
+		    if wb2_in.cyc = '0' then
+			state <= IDLE;
+		    end if;
+		when WB3_BUSY =>
+		    if wb3_in.cyc = '0' then
+			state <= IDLE;
+		    end if;
+		end case;
+	    end if;
+	end if;
     end process;
 end behave;