diff --git a/servant/servant.v b/servant/servant.v
index 92ab6f7..943fd88 100644
--- a/servant/servant.v
+++ b/servant/servant.v
@@ -39,7 +39,9 @@ module servant
    wire [31:0] 	wb_mem_rdt;
    wire 	wb_mem_ack;
    wire 	wb_gpio_dat;
+   wire 	wb_gpio_we;
    wire 	wb_gpio_cyc;
+   wire 	wb_gpio_rdt;
    wire [31:0] 	wb_timer_dat;
    wire 	wb_timer_we;
    wire 	wb_timer_cyc;
@@ -94,7 +96,9 @@ servant_arbiter servant_arbiter
    .i_wb_mem_rdt (wb_dmux_mem_rdt),
 
    .o_wb_gpio_dat (wb_gpio_dat),
+   .o_wb_gpio_we  (wb_gpio_we),
    .o_wb_gpio_cyc (wb_gpio_cyc),
+   .i_wb_gpio_rdt (wb_gpio_rdt),
 
    .o_wb_timer_dat (wb_timer_dat),
    .o_wb_timer_we  (wb_timer_we),
@@ -143,7 +147,9 @@ servant_arbiter servant_arbiter
    servant_gpio gpio
      (.i_wb_clk (wb_clk),
       .i_wb_dat (wb_gpio_dat),
+      .i_wb_we  (wb_gpio_we),
       .i_wb_cyc (wb_gpio_cyc),
+      .o_wb_rdt (wb_gpio_rdt),
       .o_gpio   (q));
 
    serv_top
diff --git a/servant/servant_gpio.v b/servant/servant_gpio.v
index dd14ea5..da1debc 100644
--- a/servant/servant_gpio.v
+++ b/servant/servant_gpio.v
@@ -1,11 +1,14 @@
 module servant_gpio
-  (
-   input wire i_wb_clk,
+  (input wire i_wb_clk,
    input wire i_wb_dat,
+   input wire i_wb_we,
    input wire i_wb_cyc,
+   output reg o_wb_rdt,
    output reg o_gpio);
 
-   always @(posedge i_wb_clk)
-     if (i_wb_cyc)
-       o_gpio <= i_wb_dat;
+   always @(posedge i_wb_clk) begin
+      o_wb_rdt <= o_gpio;
+      if (i_wb_cyc & i_wb_we)
+	o_gpio <= i_wb_dat;
+   end
 endmodule
diff --git a/servant/servant_mux.v b/servant/servant_mux.v
index 072d764..8c11eff 100644
--- a/servant/servant_mux.v
+++ b/servant/servant_mux.v
@@ -24,7 +24,9 @@ module servant_mux
    input wire [31:0]  i_wb_mem_rdt,
 
    output wire 	      o_wb_gpio_dat,
+   output wire 	      o_wb_gpio_we,
    output wire 	      o_wb_gpio_cyc,
+   input wire 	      i_wb_gpio_rdt,
 
    output wire [31:0] o_wb_timer_dat,
    output wire 	      o_wb_timer_we,
@@ -35,7 +37,8 @@ module servant_mux
 
    wire [1:0] 	  s = i_wb_cpu_adr[31:30];
 
-   assign o_wb_cpu_rdt = s[1] ? i_wb_timer_rdt : i_wb_mem_rdt;
+   assign o_wb_cpu_rdt = s[1] ? i_wb_timer_rdt :
+			 s[0] ? {31'd0,i_wb_gpio_rdt} : i_wb_mem_rdt;
    always @(posedge i_clk) begin
       o_wb_cpu_ack <= 1'b0;
       if (i_wb_cpu_cyc & !o_wb_cpu_ack)
@@ -51,6 +54,7 @@ module servant_mux
    assign o_wb_mem_cyc = i_wb_cpu_cyc & (s == 2'b00);
 
    assign o_wb_gpio_dat = i_wb_cpu_dat[0];
+   assign o_wb_gpio_we  = i_wb_cpu_we;
    assign o_wb_gpio_cyc = i_wb_cpu_cyc & (s == 2'b01);
 
    assign o_wb_timer_dat = i_wb_cpu_dat;