diff --git a/verilog/rtl/wb_pio_top.v b/verilog/rtl/wb_pio_top.v
index fc10948..aa464cc 100644
--- a/verilog/rtl/wb_pio_top.v
+++ b/verilog/rtl/wb_pio_top.v
@@ -79,32 +79,25 @@
     wire clk;
     wire rst;
 
-    wire [`MPRJ_IO_PADS-1:0] io_in;
-    wire [`MPRJ_IO_PADS-1:0] io_out;
-    wire [`MPRJ_IO_PADS-1:0] io_oeb;
-
-    wire [31:0] wdata;
-    wire [BITS-1:0] count;
-
     wire valid;
     wire [3:0] wstrb;
     wire [31:0] la_write;
 
-    // WB MI A
-    assign valid = wbs_cyc_i && wbs_stb_i && wbs_adr_i[31:16] == 16'h761c;
-    assign wstrb = wbs_sel_i & {4{wbs_we_i}};
-    assign wdata = wbs_dat_i;
+    reg manual_reset = 0;
 
-    // IO
-    assign io_out = count;
+    // WB MI A
+    assign valid = wbs_cyc_i && wbs_stb_i && wbs_adr_i[31:16] == 16'h3000;
+    assign wstrb = wbs_sel_i & {4{wbs_we_i}};
+
+    assign wbs_dat_o = pio_dout;
 
     // PIO registers and wires
-    reg [31:0]  din;        // Data sent to PIO
+    wire [31:0]  pio_din;    // Data sent to PIO
     reg [4:0]   index;      // Instruction index
     reg [1:0]   mindex;     // Machine index
 
-    reg [31:0]  dout;       // Output from PIO
-    wire [3:0]  action;     // Action to be done by PIO
+    wire [31:0] pio_dout;   // Output from PIO
+    reg [3:0]   action = 4'b0000;     // Action to be done by PIO
     wire        irq0, irq1; // IRQ flags from PIO
     wire [3:0]  tx_full;    // Set when TX fifo is full  
     wire [3:0]  rx_empty;   // Set when RX fifo is empty
@@ -113,50 +106,68 @@
 
     reg ack;
     assign wbs_ack_o = ack;
-    assign wbs_dat_o = dout;
 
     always @(posedge wb_clk_i) begin
-        if (wb_rst_i) begin
-            ack <= 1'b0;
+        if (wb_rst_i | manual_reset) begin
+            index <= 0;
+            manual_reset <= 0;
         end else begin
             ack <= 1'b0;
-            if (valid && !ack) begin
-                ack <= 1'b1;
-                if (wbs_we_i) begin
-                    action[3:0] <= wbs_adr_i[5:2];
-                    if (wbs_we_i) begin
-                        if (wstrb[0]) din[7:0]   <= wbs_dat_i[7:0];
-                        if (wstrb[1]) din[15:8]  <= wbs_dat_i[15:8];
-                        if (wstrb[2]) din[23:16] <= wbs_dat_i[23:16];
-                        if (wstrb[3]) din[31:24] <= wbs_dat_i[31:24];
-                    end else begin
-                        if (wstrb[0]) wbs_dat_o[7:0] <= dout[7:0];
-                        if (wstrb[1]) wbs_dat_o[15:8] <= dout[15:8];
-                        if (wstrb[2]) wbs_dat_o[23:16] <= dout[23:16];
-                        if (wstrb[3]) wbs_dat_o[31:24] <= dout[31:24];
-                    end
+            if (valid) begin
+                if (!ack) begin
+                     ack <= 1'b1;
+                     if ((action == 4'b0000) && (pio_din[31] == 1'b1)) begin
+                        manual_reset <= 1;
+                     end
                 end else begin
-                    // When there is no action, set the action to `NONE`
-                    action[3:0] <= 4'b0000;
+                    if (action == 4'b0001) begin
+                        // Increment the program counter when injecting values into
+                        // the PIO memory space.
+                        index <= index + 1 + pio_din[19:16];
+                    end
                 end
             end
         end
     end
 
+    // Tie unused pins down, so as to avoid uninitialized values.
+    assign io_out[4:0] = 0;
+    assign io_out[37] = 0;
+    assign io_oeb[4:0] = 0;
+    assign io_oeb[37] = 0;
+
+    // Wire up the PIO data input lines. Ideally we should consult
+    // `wstrb` here to figure out which values to copy.
+    assign pio_din[7:0]   = wbs_dat_i[7:0];
+    assign pio_din[15:8]  = wbs_dat_i[15:8];
+    assign pio_din[23:16] = wbs_dat_i[23:16];
+    assign pio_din[31:24] = wbs_dat_i[31:24];
+
+    always @(*) begin
+        // Default action to `NONE`
+        action <= 0;
+        mindex <= 0;
+        // When there's a valid command, set the action.
+        if (valid) begin
+            action <= wbs_adr_i[5:2];
+            mindex <= wbs_adr_i[7:6];
+        end
+    end
+
     // PIO instance 1
     pio pio_1 (
         .clk(wb_clk_i),
-        .reset(wb_rst_i),
+        .reset(wb_rst_i | manual_reset),
 
         .mindex(mindex),
-        .din(din),
+        .din(pio_din),
         .index(index),
         .action(action),
-        .dout(dout),
+        .dout(pio_dout),
 
-        .gpio_in(io_in),
-        .gpio_out(io_out),
-        .gpio_dir(io_oeb),
+        .gpio_in(io_in[36:5]),
+        .gpio_out(io_out[36:5]),
+        .gpio_dir(io_oeb[36:5]),
         .irq0(irq[0]),
         .irq1(irq[1]),