diff --git a/.gitignore b/.gitignore
index 548bd9e..85c55a8 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@ build/.Xil
 build/xsim.dir
 build/*.wcfg
 build/vivado_*
+build/vivado.*
diff --git a/build/tb_top.Makefile b/build/tb_top.Makefile
index b7290f0..77c6487 100644
--- a/build/tb_top.Makefile
+++ b/build/tb_top.Makefile
@@ -1,7 +1,8 @@
 TB       = tb_top
-SOURCES  = gb.sv cpu.sv alu.sv registers.sv control.sv decode.sv rom.sv ram.sv tb_top.sv clkgen.sv
-INCLUDES = cpu_pkg.svh
-PATH_SRC = ../rtl:../rtl/cpu:../rtl/shared:../sim/tbench:../sim/shared
+SOURCES  = gb.sv cpu.sv ppu.sv idec.sv ctrl.sv alu.sv alu16.sv regbank.sv rom.sv ram.sv tb_top.sv clkgen.sv
+INCLUDES = cpu_pkg.svh sva_common.svh
+PATH_SRC = ../rtl:../rtl/cpu:../sim
+
+DEFINES = SVA_ENABLE
 
 include ../synthflow/vivado/Makefile.rules
-
diff --git a/rtl/cpu/alu.sv b/rtl/cpu/alu.sv
index 1793bf7..e7bbd99 100644
--- a/rtl/cpu/alu.sv
+++ b/rtl/cpu/alu.sv
@@ -3,95 +3,167 @@
 import cpu_pkg::*;
 
 module alu (
-    input logic       clk_i,
-    input logic       nreset_i,
+  input  logic    clk,
+  input  logic    nreset,
 
-    input logic       alu_op_valid_i,
-    input alu_op_t    alu_op_i,
-    input logic [7:0] operand_i,
-    input logic [2:0] inx8_i, // Only used for bit/set/res
+  input  alu_ctrl_t alu_ctrl_i,
 
-    input logic       rot_op_valid_i,
-    input rot_op_t    rot_op_i,
+  input  logic [ 7:0] operand_in_i,
+  output logic [ 7:0] operand_out_o,
 
-    input alu16_op_t   alu16_op_i,
-    input logic [15:0] inx16_i,
-    input logic [15:0] iny16_i,
-
-    output logic [ 7:0] a_o,
-    output logic [ 7:0] f_o,
-
-    output logic [ 7:0] out8_o, // Only used for inc/dec reg8 and rot
-    output logic [15:0] out16_o
+  output logic [ 7:0] reg_a_o,
+  output logic [ 7:0] reg_f_o
 );
 
-    logic        a_we;
-    logic [ 7:0] a_r;
-    logic [ 7:0] a_next;
 
-    logic        f_we;
-    logic [ 7:0] f_r;
-    logic [ 7:0] f_next;
+logic        nop_valid;
+logic        a_we;
+logic [ 7:0] a_r;
+logic [ 7:0] a_next;
 
-    logic [ 7:0] a_inc;
-    logic [ 7:0] f_inc;
+logic        f_we;
+logic [ 7:0] f_r;
+logic [ 7:0] f_next;
 
-    logic [ 7:0] a_xor;
-    logic [ 7:0] f_xor;
+always_ff @(posedge clk or negedge nreset) begin
+  if (!nreset)
+    a_r <= '0;
+  else if (a_we)
+    a_r <= a_next;
+end
 
-    logic [ 7:0] a_bit; // Never written, only to test
-    logic [ 7:0] f_bit;
+always_ff @(posedge clk or negedge nreset) begin
+  if (!nreset)
+    f_r <= '0;
+  else if (f_we)
+    f_r <= f_next;
+end
+
+assign a_we = alu_ctrl_i.update_a;
+assign f_we = alu_ctrl_i.update_f;
+
+assign reg_a_o = a_r;
+assign reg_f_o = f_r;
+
+logic [ 7:0] alu_a;
+logic [ 7:0] alu_f;
+
+logic [ 7:0] shift_a;
+logic [ 7:0] shift_f;
+
+logic [ 7:0] bsr_a;
+logic [ 7:0] bsr_f;
+
+logic [ 7:0] incdec_a;
+logic [ 7:0] incdec_f;
+
+assign nop_valid = ~alu_ctrl_i.alu_op_valid    &
+                   ~alu_ctrl_i.shift_op_valid  &
+                   ~alu_ctrl_i.bsr_op_valid    &
+                   ~alu_ctrl_i.incdec_op_valid;
+
+assign a_next = ({8{alu_ctrl_i.alu_op_valid   }} &        alu_a) |
+                ({8{alu_ctrl_i.shift_op_valid }} &      shift_a) |
+                ({8{alu_ctrl_i.bsr_op_valid   }} &        bsr_a) |
+                ({8{alu_ctrl_i.incdec_op_valid}} &     incdec_a) |
+                ({8{nop_valid                 }} & operand_in_i);
+
+assign f_next = ({8{alu_ctrl_i.alu_op_valid   }} &        alu_f) |
+                ({8{alu_ctrl_i.shift_op_valid }} &      shift_f) |
+                ({8{alu_ctrl_i.bsr_op_valid   }} &        bsr_f) |
+                ({8{alu_ctrl_i.incdec_op_valid}} &     incdec_f) |
+                ({8{nop_valid                 }} & operand_in_i);
+
+logic [ 8:0] add_a;
+logic [ 7:0] add_f;
+logic [ 8:0] sub_a;
+logic [ 7:0] sub_f;
+logic [ 7:0] xor_a;
+logic [ 7:0] xor_f;
+logic [ 7:0] cp_a;
+logic [ 7:0] cp_f;
+
+assign alu_a = ({8{alu_ctrl_i.alu_op == ALU_OP_ADD}} & add_a) |
+               ({8{alu_ctrl_i.alu_op == ALU_OP_SUB}} & sub_a) |
+               ({8{alu_ctrl_i.alu_op == ALU_OP_XOR}} & xor_a) |
+               ({8{alu_ctrl_i.alu_op == ALU_OP_CP }} & cp_a);
+assign alu_f = ({8{alu_ctrl_i.alu_op == ALU_OP_ADD}} & add_f) |
+               ({8{alu_ctrl_i.alu_op == ALU_OP_SUB}} & sub_f) |
+               ({8{alu_ctrl_i.alu_op == ALU_OP_XOR}} & xor_f) |
+               ({8{alu_ctrl_i.alu_op == ALU_OP_CP }} & cp_f);
+
+assign add_a = {1'b0, a_r} + {1'b0, operand_in_i};
+assign add_f = {~(|sub_a[7:0]), 1'b0, 1'b0, sub_a[8], f_r[3:0]}; // TODO: implement H
+
+assign sub_a = {1'b0, a_r} - {1'b0, operand_in_i};
+assign sub_f = {~(|sub_a[7:0]), 1'b1, 1'b0, sub_a[8], f_r[3:0]}; // TODO: implement H
+
+assign xor_a = a_r ^ operand_in_i;
+assign xor_f = {~(|xor_a), 3'b0, f_r[3:0]};
+
+assign cp_a = a_r;
+assign cp_f = sub_f;
+
+logic [ 7:0] bit_a;
+logic [ 7:0] bit_f;
+
+assign bsr_a = ({8{alu_ctrl_i.bsr_op.b}} & bit_a);
+assign bsr_f = ({8{alu_ctrl_i.bsr_op.b}} & bit_f);
+
+assign bit_a = {7'b0, ~operand_in_i[alu_ctrl_i.bsr_op.operand]};
+assign bit_f = {bit_a[0], 2'b10, f_r[4:0]};
+
+logic [ 7:0] rl_a;
+logic [ 7:0] rl_f;
+
+assign shift_a = ({8{alu_ctrl_i.shift_op == SHIFT_OP_RL}} & rl_a);
+assign shift_f = ({8{alu_ctrl_i.shift_op == SHIFT_OP_RL}} & rl_f);
+
+assign rl_a  = {operand_in_i[6:0], f_r[4]};
+assign rl_f  = {~(|rl_a), 2'b0, operand_in_i[7], f_r[3:0]};
 
 
-    logic [ 7:0] out8_incr;
-    logic [ 7:0] f_incr;
+logic [ 7:0] incdec_input;
+logic [ 7:0] inc_a;
+logic [ 7:0] inc_f;
+logic [ 7:0] dec_a;
+logic [ 7:0] dec_f;
 
-    logic [ 7:0] out8_rl;
-    logic [ 7:0] f_rl;
+assign incdec_input = alu_ctrl_i.incdec_op.dst_a ? a_r         :
+                                                   operand_in_i;
 
-    logic [15:0] out16_inc;
-    logic [15:0] out16_dec;
+assign incdec_a = ({8{alu_ctrl_i.incdec_op.inc}} & inc_a) |
+                  ({8{alu_ctrl_i.incdec_op.dec}} & dec_a);
 
-    `DEF_FF(a_r, a_next, a_we, '0);
-    `DEF_FF(f_r, f_next, f_we, '0);
+assign incdec_f  = ({8{alu_ctrl_i.incdec_op.inc}} & inc_f) |
+                   ({8{alu_ctrl_i.incdec_op.dec}} & dec_f);
 
-    assign a_we   = alu_op_valid_i;
-    assign a_next = (alu_op_valid_i & (alu_op_i == ALU_OP_XOR)) ? a_xor     :
-                    (alu_op_valid_i & (alu_op_i == ALU_OP_NOP)) ? operand_i :
-                    (alu_op_valid_i & (alu_op_i == ALU_OP_INC)) ? a_inc     :
-                    (alu_op_valid_i & (alu_op_i == ALU_OP_ROT) & (rot_op_i == ROT_OP_RL)) ? out8_rl   :
-                                                                   a_r;
+assign inc_a = incdec_input + 8'h01;
+assign inc_f = {~(|inc_a), 1'b0, inc_a[4], f_r[4], f_r[3:0]};
 
-    assign f_we   = alu_op_valid_i | rot_op_valid_i;
-    assign f_next = (alu_op_valid_i & (alu_op_i == ALU_OP_XOR))  ? f_xor  :
-                    (alu_op_valid_i & (alu_op_i == ALU_OP_BIT))  ? f_bit  :
-                    (alu_op_valid_i & (alu_op_i == ALU_OP_INC))  ? f_inc  :
-                    (alu_op_valid_i & (alu_op_i == ALU_OP_INCR)) ? f_incr :
-                    (rot_op_valid_i & (rot_op_i == ROT_OP_RL))   ? f_rl   :
-                                                                   f_r;
+assign dec_a = incdec_input - 8'h01;
+assign dec_f = {~(|dec_a), 1'b1, dec_a[4], f_r[4], f_r[3:0]};
 
-    assign a_xor = (a_r ^ operand_i);
-    assign f_xor = {~(|a_xor), 3'b0, f_r[3:0]};
+assign operand_out_o = ({8{alu_ctrl_i.shift_op_valid }} &  shift_a) |
+                       ({8{alu_ctrl_i.incdec_op_valid}} & incdec_a);
 
-    assign a_inc = (a_r + 8'h01);
-    assign f_inc = {~(|a_inc), 1'b0, a_inc[4], f_r[3:0]};
+`include "sva_common.svh"
+`SVA_DEF_CLK(clk);
+`SVA_DEF_NRESET(nreset);
 
-    assign a_bit = (operand_i - {4'b0, inx8_i});
-    assign f_bit = {~(|a_bit), 2'b10, f_r[4:0]};
+`SVA_ASSERT_PROP_FATAL(undefined_alu_op,
+  tb_top.gb_inst.cpu_inst.instr_valid & alu_ctrl_i.alu_op_valid |-> alu_ctrl_i.alu_op inside {ALU_OP_ADD, ALU_OP_SUB, ALU_OP_XOR, ALU_OP_CP},
+  $sformatf("Undefined alu operation pushed: 0x%X", alu_ctrl_i.alu_op)
+);
 
-    assign out8_rl  = {operand_i[6:0], f_r[4]};
-    assign f_rl  = {~(|out8_rl), 2'b0, operand_i[7], f_r[3:0]};
+`SVA_ASSERT_PROP_FATAL(undefined_shift_op,
+  tb_top.gb_inst.cpu_inst.instr_valid & alu_ctrl_i.shift_op_valid |-> alu_ctrl_i.shift_op inside {SHIFT_OP_RL},
+  $sformatf("Undefined shift operation pushed: 0x%X", alu_ctrl_i.shift_op)
+);
 
+`SVA_ASSERT_PROP_FATAL(undefined_bsr_op,
+  tb_top.gb_inst.cpu_inst.instr_valid & alu_ctrl_i.bsr_op_valid |-> alu_ctrl_i.bsr_op.b,
+  $sformatf("Undefined bsr operation pushed: 0x%X", alu_ctrl_i.bsr_op)
+);
 
-    assign out8_incr = (operand_i + 8'h01);
-    assign f_incr = {~(|out8_incr), 1'b0, out8_incr[4], f_r[3:0]};
-
-    assign out16_dec = (inx16_i - 16'h01);
-
-    assign a_o = a_r;
-    assign f_o = f_r;
-
-    assign out8_o  = ({8{alu_op_valid_i}} & out8_incr) | {8{rot_op_valid_i}} & out8_rl;
-    assign out16_o = out16_dec;
-
-endmodule : alu
\ No newline at end of file
+endmodule : alu
diff --git a/rtl/cpu/alu16.sv b/rtl/cpu/alu16.sv
new file mode 100644
index 0000000..1adcbb5
--- /dev/null
+++ b/rtl/cpu/alu16.sv
@@ -0,0 +1,21 @@
+`include "cpu_pkg.svh"
+
+import cpu_pkg::*;
+
+module alu16 (
+    input  logic [15:0] operand_in_i,
+    output logic [15:0] operand_out_o,
+
+    input  logic        increment_i,
+    input  logic        decrement_i
+);
+
+logic [15:0] op_inc;
+logic [15:0] op_dec;
+
+assign op_inc = (operand_in_i + 16'h01);
+assign op_dec = (operand_in_i - 16'h01);
+
+assign operand_out_o = ({16{increment_i}} & op_inc) |
+                       ({16{decrement_i}} & op_dec);
+endmodule : alu16
diff --git a/rtl/cpu/control.sv b/rtl/cpu/control.sv
deleted file mode 100644
index 1414413..0000000
--- a/rtl/cpu/control.sv
+++ /dev/null
@@ -1,222 +0,0 @@
-`include "cpu_pkg.svh"
-
-import cpu_pkg::*;
-
-module control (
-    input  logic   clk_i,
-    input  logic   nreset_i,
-
-    input  logic [ 7:0] rdata_i,
-
-    input  logic [ 7:0] f_i,
-
-    output state_t state_o,
-
-    output logic [15:0] pc_o,
-    output logic [15:0] sp_o,
-
-    output logic        instr_valid_o,
-    output logic        instr_undef_o,
-    output logic [ 7:0] operand8_o,
-    output logic [15:0] operand16_o,
-
-    output logic       alu_op_valid_o,
-    output alu_op_t    alu_op_o,
-    output logic       reg_write_alu_o,
-    output logic       rot_op_valid_o,
-    output rot_op_t    rot_op_o,
-    output op_src_t    op_src_o,
-    output op_dest_t   op_dest_o,
-    output reg8_t      reg8_dest_o,
-    output reg8_t      reg8_src_o,
-    output alu16_op_t  alu16_op_o,
-    output reg16_t     reg16_src_o,
-    output reg16_t     reg16_dest_o,
-
-    output logic       memory_we_o,
-    output adr_src_t   adr_src_o
-);
-
-    logic   nreset_r;
-    logic   halted_r;
-
-    logic   state_we;
-    state_t state_r;
-    state_t state_next;
-
-    logic        pc_we;
-    logic [15:0] pc_r;
-    logic [15:0] pc_next;
-
-
-    logic        sp_we;
-    logic [15:0] sp_r;
-    logic [15:0] sp_next;
-
-    logic        instr_pc_we;
-    logic [15:0] instr_pc_r;
-    logic [15:0] instr_pc_next;
-
-    logic        instr_we;
-    logic [ 7:0] instr_r;
-    logic [ 7:0] instr_next;
-
-    logic        operand0_we;
-    logic [ 7:0] operand0_r;
-    logic [ 7:0] operand0_next;
-
-    logic        operand1_we;
-    logic [ 7:0] operand1_r;
-    logic [ 7:0] operand1_next;
-
-    logic [ 7:0] decoder_instr0_selected;
-    logic [ 7:0] decoder_instr1_selected;
-    logic [ 7:0] decoder_instr2_selected;
-    logic        decoder_need_instr1;
-    logic        decoder_need_instr2;
-    logic        decoder_is_multicycle;
-    logic        decoder_is_multicycle2;
-    logic        decoder_is_undef;
-    logic        decoder_alu_op_valid;
-    logic        decoder_rot_op_valid;
-    logic        decoder_memory_we;
-    adr_src_t    decoder_adr_src;
-    logic        decoder_sp_we;
-
-    logic        is_undef;
-    logic        pc_incr;
-    logic        instr_valid;
-    logic        branch_taken;
-    logic        branch_cc_true;
-    logic        branch_always;
-
-    sp_src_t     sp_src;
-    pc_src_t     pc_src;
-    cc_t         cc;
-
-    always_ff @(posedge clk_i) begin
-        nreset_r <= nreset_i;
-    end
-
-    always_ff @(posedge clk_i or negedge nreset_i) begin
-        if (~nreset_i)
-            halted_r <= 1'b0;
-        else if (is_undef)
-            halted_r <= halted_r | is_undef;
-    end
-
-    `DEF_FF(state_r,    state_next,    state_we,    ST0_ADDR);
-    `DEF_FF(pc_r,       pc_next,       pc_we,       '0);
-    `DEF_FF(sp_r,       sp_next,       sp_we,       '0);
-    `DEF_FF(instr_pc_r, instr_pc_next, instr_pc_we, '0);
-    `DEF_FF(instr_r,    instr_next,    instr_we,    '0);
-    `DEF_FF(operand0_r, operand0_next, operand0_we, '0);
-    `DEF_FF(operand1_r, operand1_next, operand1_we, '0);
-
-    assign pc_we   = (nreset_r & ~is_undef & ~halted_r) & (pc_incr | branch_taken);
-    assign pc_next = (branch_taken & pc_src == PC_SRC_OPERAND8)  ? (pc_r + {{8{operand0_r[7]}}, operand0_r}) :
-                     (branch_taken & pc_src == PC_SRC_OPERAND16) ? {operand1_r, operand0_r}                  :
-                                                                   (pc_r + 16'b1);
-
-    assign sp_we   = instr_valid & decoder_sp_we;
-    assign sp_next = (sp_src == SP_SRC_OPERAND16) ? {operand1_r, operand0_r} :
-                     (sp_src == SP_SRC_INC)       ? (sp_r + 16'h01) :
-                     (sp_src == SP_SRC_DEC)       ? (sp_r + 16'hFFFF) :
-                                                    '0;
-
-    assign instr_we   = (state_r == ST1_DEC);
-    assign instr_next = rdata_i;
-
-    assign instr_pc_we   = (state_r == ST0_ADDR);
-    assign instr_pc_next = pc_r;
-
-    assign operand0_we   = (state_r == ST2_DEC);
-    assign operand0_next = rdata_i;
-
-    assign operand1_we   = (state_r == ST3_DEC);
-    assign operand1_next = rdata_i;
-
-    assign state_we = nreset_r & ~is_undef & ~halted_r;
-    always_comb begin
-        case (state_r)
-            ST0_ADDR:     state_next = ST1_DEC;
-            ST1_DEC:      state_next = decoder_need_instr1   ? ST2_DEC  : ST2_EXEC;
-            ST2_EXEC:     state_next = decoder_is_multicycle ? ST3_EXEC : ST0_ADDR;
-
-            ST2_DEC:      state_next = decoder_need_instr2 ? ST3_DEC : ST3_EXEC;
-            ST3_DEC:      state_next = ST4_EXEC;
-            ST3_EXEC:     state_next = decoder_is_multicycle2 ? ST4_EXEC : ST0_ADDR;
-            ST4_EXEC:     state_next = decoder_need_instr2 & decoder_is_multicycle ? ST5_EXEC : ST0_ADDR;
-            ST5_EXEC:     state_next = ST0_ADDR;
-        endcase
-    end
-
-    decode decode_inst (
-        .instr0_i     (decoder_instr0_selected),
-        .instr1_i     (decoder_instr1_selected),
-        .instr2_i     (decoder_instr2_selected),
-        .state_i      (state_r),
-
-        .need_instr1_o  (decoder_need_instr1),
-        .need_instr2_o  (decoder_need_instr2),
-        .is_multicycle_o(decoder_is_multicycle),
-        .is_multicycle2_o(decoder_is_multicycle2),
-        .undef_o        (decoder_is_undef),
-
-        .sp_we_o      (decoder_sp_we),
-        .sp_src_o     (sp_src),
-
-        .alu_op_valid_o(decoder_alu_op_valid),
-        .alu_op_o      (alu_op_o),
-        .reg_write_alu_o(reg_write_alu_o),
-        .rot_op_valid_o(decoder_rot_op_valid),
-        .rot_op_o      (rot_op_o),
-        .op_src_o      (op_src_o),
-        .op_dest_o     (op_dest_o),
-
-        .reg8_dest_o   (reg8_dest_o),
-        .reg8_src_o    (reg8_src_o),
-
-        .alu16_op_o    (alu16_op_o),
-        .reg16_src_o   (reg16_src_o),
-        .reg16_dest_o  (reg16_dest_o),
-
-        .memory_we_o   (decoder_memory_we),
-        .adr_src_o     (decoder_adr_src),
-        .branch_always_o(branch_always),
-        .cc_o          (cc),
-        .pc_src_o      (pc_src)
-    );
-
-    assign decoder_instr0_selected = (state_r == ST1_DEC)  ? rdata_i : instr_r;
-    assign decoder_instr1_selected = (state_r == ST2_DEC)  ? rdata_i : operand0_r;
-    assign decoder_instr2_selected = (state_r == ST3_DEC)  ? rdata_i : operand1_r;
-
-    assign is_undef    = instr_valid & decoder_is_undef;
-    assign instr_valid = (state_r == ST2_EXEC | state_r == ST3_EXEC | state_r == ST4_EXEC | state_r == ST5_EXEC);
-    assign alu_op_valid_o = decoder_alu_op_valid & instr_valid;
-    assign rot_op_valid_o = decoder_rot_op_valid & instr_valid;
-
-    assign pc_incr = (state_r == ST0_ADDR) | (state_r == ST1_DEC & decoder_need_instr1) | (state_r == ST2_DEC & decoder_need_instr2);
-
-    assign branch_taken   = instr_valid & (pc_src != PC_SRC_SEQ) & (branch_cc_true | branch_always);
-    assign branch_cc_true = ((cc == CC_NZ) & ~f_i[7]) |
-                            ((cc == CC_Z)  &  f_i[7]) |
-                            ((cc == CC_NC) & ~f_i[4]) |
-                            ((cc == CC_C)  &  f_i[4]);
-
-    assign state_o = state_r;
-    assign pc_o    = pc_r;
-    assign sp_o    = sp_r;
-
-    assign instr_valid_o = instr_valid;
-    assign instr_undef_o = is_undef;
-
-    assign operand8_o  = operand0_r;
-    assign operand16_o = {operand1_r, operand0_r};
-
-    assign memory_we_o = instr_valid & decoder_memory_we;
-    assign adr_src_o   = instr_valid ? decoder_adr_src :
-                                       ADR_SRC_PC;
-
-endmodule : control
diff --git a/rtl/cpu/cpu.sv b/rtl/cpu/cpu.sv
index 602f27b..34c4032 100644
--- a/rtl/cpu/cpu.sv
+++ b/rtl/cpu/cpu.sv
@@ -3,150 +3,280 @@
 import cpu_pkg::*;
 
 module cpu (
-    input  logic clk_i,
-    input  logic nreset_i,
+  input  logic clk,
+  input  logic nreset,
 
-    output logic [15:0] address_o,
-    input  logic [ 7:0] rdata_i,
-    output logic        we_o,
-    output logic [ 7:0] wdata_o
+  output logic [15:0] addr_o,
+  input  logic [ 7:0] rdata_i,
+  output logic        we_o,
+  output logic [ 7:0] wdata_o
 );
 
-    state_t   state;
 
-    logic     alu_op_valid;
-    alu_op_t  alu_op;
-    op_src_t  op_src;
-    op_dest_t op_dest;
-    adr_src_t adr_src;
-    logic     reg_write_alu;
-    logic     rot_op_valid;
-    rot_op_t  rot_op;
+logic        instr_valid;
+logic [ 7:0] instr[2:0];
 
-    logic [ 7:0] alu_operand;
+logic        incr_pc;
+logic        pc_we;
+logic [15:0] pc_r;
+logic [15:0] pc_next;
+logic [15:0] pc_bus;
 
-    logic [ 7:0] rega;
-    logic [ 7:0] regf;
+cc_t     ctrl_cc;
+logic    ctrl_pc_we;
+logic    ctrl_pc_we_cc;
+pc_src_t ctrl_pc_src;
 
-    logic [15:0] pc;
-    logic [15:0] sp;
-    logic [15:0] hl;
+logic [ 1:0] sp_we;
+logic [15:0] sp_r;
+logic [15:0] sp_next;
 
-    logic [15:0] sp_p1;
+reg8_t       reg8_rsel;
+reg8_t       reg8_wsel;
+logic        reg8_we;
+logic  [7:0] reg8_wdata;
+logic  [7:0] reg8_rdata;
 
-    logic instr_valid;
-    logic instr_undef;
+reg16_t       reg16_sel;
+logic         reg16_we;
+logic  [15:0] reg16_wdata;
+logic  [15:0] reg16_rdata;
 
-    logic [ 7:0] operand8;
-    logic [15:0] operand16;
+addr_src_t addr_src;
 
-    reg8_t       reg8_wselect;
-    logic        reg8_we;
-    logic [ 7:0] reg8_wdata;
-    reg16_t      reg16_wselect;
-    logic        reg16_we;
-    logic [15:0] reg16_wdata;
-    reg8_t       reg8_rselect;
-    reg16_t      reg16_rselect;
-    reg16_t      reg16_rselect2;
-    logic [ 7:0] reg8_rdata;
-    logic [15:0] reg16_rdata;
-    logic [15:0] reg16_rdata2;
-    alu16_op_t   alu16_op;
-    logic [ 7:0] alu_out8;
-    logic [15:0] alu_out16;
+logic [ 7:0] reg_a;
+logic [ 7:0] reg_f;
 
-    control control_inst (
-        .clk_i         (clk_i),
-        .nreset_i      (nreset_i),
-        .rdata_i       (rdata_i),
-        .state_o       (state),
-        .f_i           (regf),
-        .pc_o          (pc),
-        .sp_o          (sp),
-        .alu_op_valid_o(alu_op_valid),
-        .alu_op_o      (alu_op),
-        .reg_write_alu_o(reg_write_alu),
-        .rot_op_valid_o(rot_op_valid),
-        .rot_op_o      (rot_op),
-        .op_src_o      (op_src),
-        .op_dest_o     (op_dest),
-        .instr_valid_o (instr_valid),
-        .instr_undef_o (instr_undef),
-        .operand8_o    (operand8),
-        .operand16_o   (operand16),
-        .reg8_dest_o   (reg8_wselect),
-        .reg8_src_o    (reg8_rselect),
-        .alu16_op_o    (alu16_op),
-        .reg16_src_o   (reg16_rselect),
-        .reg16_dest_o  (reg16_wselect),
-        .memory_we_o   (we_o),
-        .adr_src_o     (adr_src)
-    );
+logic        alu_update_a;
+logic        alu_update_f;
+alu_ctrl_t   ctrl_alu_ctrl;
+alu_ctrl_t   alu_ctrl;
+logic [ 7:0] alu_operand_in;
+logic [ 7:0] alu_operand_out;
 
-    alu alu_inst (
-        .clk_i         (clk_i),
-        .nreset_i      (nreset_i),
-        .alu_op_valid_i(alu_op_valid),
-        .alu_op_i      (alu_op),
-        .rot_op_valid_i(rot_op_valid),
-        .rot_op_i      (rot_op),
-        .operand_i     (alu_operand),
-        .inx8_i        (operand8[5:3]), // Used for bit/set/res
-        .a_o           (rega),
-        .f_o           (regf),
-        .alu16_op_i    (alu16_op),
-        .inx16_i       (reg16_rdata2),
-        .iny16_i       (reg16_rdata),
-        .out8_o        (alu_out8),
-        .out16_o       (alu_out16)
-    );
+logic        alu16_en;
+logic [15:0] alu16_in;
+logic [15:0] alu16_out;
+logic        alu16_inc;
+logic        alu16_dec;
 
-    registers registers_inst (
-        .clk_i   (clk_i),
-        .nreset_i(nreset_i),
+bus_ctrl_t   bus_x_ctrl;
+bus_ctrl_t   bus_y_ctrl;
+bus16_ctrl_t bus_p_ctrl;
+bus16_ctrl_t bus_q_ctrl;
+logic [ 7:0] bus_x;
+logic [ 7:0] bus_y;
+logic [15:0] bus_p;
+logic [15:0] bus_q;
 
-        .reg8_wselect_i (reg8_wselect),
-        .reg8_we_i      (reg8_we),
-        .reg8_wdata_i   (reg8_wdata),
-        .reg16_wselect_i(reg16_wselect),
-        .reg16_we_i     (reg16_we),
-        .reg16_wdata_i  (reg16_wdata),
-        .reg8_rselect_i (reg8_rselect),
-        .reg8_rdata_o   (reg8_rdata),
-        .reg16_rselect_i(reg16_rselect),
-        .reg16_rdata_o  (reg16_rdata),
-        .reg16_rselect2_i(reg16_rselect2),
-        .reg16_rdata2_o  (reg16_rdata2),
-        .hl_o            (hl)
-    );
+logic        cc_true;
 
-    assign sp_p1 = (sp + 16'h01);
+always_ff @(posedge clk or negedge nreset) begin
+  if (!nreset)
+    pc_r <= '0;
+  else if (pc_we)
+    pc_r <= pc_next;
+end
 
-    assign alu_operand = (op_src == OP_SRC_A)        ? rega       :
-                         (op_src == OP_SRC_F)        ? regf       :
-                         (op_src == OP_SRC_OPERAND8) ? operand8   :
-                         (op_src == OP_SRC_REG8)     ? reg8_rdata :
-                         (op_src == OP_SRC_MEMORY)   ? rdata_i    :
-                         (op_src == OP_SRC_PC_L)     ? pc[7:0]    :
-                         (op_src == OP_SRC_PC_H)     ? pc[15:8]   :
-                                                       'X;
+assign pc_we = ctrl_pc_we | (ctrl_pc_we_cc & cc_true);
 
-    assign reg8_we       = instr_valid & (op_dest == OP_DEST_REG8);
-    assign reg8_wdata    = reg_write_alu ? alu_out8 : alu_operand;
-    assign reg16_we      = instr_valid & (op_dest == OP_DEST_REG16);
-    assign reg16_wdata   = (op_src == OP_SRC_OPERAND16) ? operand16 : alu_out16;
-    assign reg16_rselect2 = reg16_wselect;
+assign pc_next = (ctrl_pc_src == PC_SRC_OPERAND8 ) ? (pc_r + {{8{instr[1][7]}}, instr[1]}) :
+                 (ctrl_pc_src == PC_SRC_OPERAND16) ? {instr[2], instr[1]}                  :
+                 (ctrl_pc_src == PC_SRC_BUS)       ? pc_bus                                :
+                                                     pc_r + 16'h01;
 
-    assign address_o = (adr_src == ADR_SRC_HL)        ? hl                  :
-                       (adr_src == ADR_SRC_SP)        ? sp                  :
-                       (adr_src == ADR_SRC_SP_P1)     ? sp_p1               :
-                       (adr_src == ADR_SRC_REG8)      ? {8'hFF, reg8_rdata} :
-                       (adr_src == ADR_SRC_REG16)     ? reg16_rdata         :
-                       (adr_src == ADR_SRC_OPERAND8)  ? {8'hFF, operand8}   :
-                       (adr_src == ADR_SRC_OPERAND16) ? operand16           :
-                                                        pc;
-    assign wdata_o   = (op_dest == OP_DEST_MEMORY) ? alu_operand :
-                                                     rega; // ldi/ldd hl, a use the normal control paths for HL
+assign cc_true = ((ctrl_cc == CC_NZ) & ~reg_f[7]) |
+                 ((ctrl_cc == CC_Z)  &  reg_f[7]) |
+                 ((ctrl_cc == CC_NC) & ~reg_f[4]) |
+                 ((ctrl_cc == CC_C)  &  reg_f[4]);
 
-endmodule : cpu
+assign pc_bus[15:8] = ({8{bus_x_ctrl.dst == BUS_DST_PC_H}} & bus_x) |
+                      ({8{bus_y_ctrl.dst == BUS_DST_PC_H}} & bus_y);
+
+assign pc_bus[ 7:0] = ({8{bus_x_ctrl.dst == BUS_DST_PC_L}} & bus_x) |
+                      ({8{bus_y_ctrl.dst == BUS_DST_PC_L}} & bus_y);
+
+always_ff @(posedge clk or negedge nreset) begin
+  if (!nreset)
+    sp_r[15:0] <= '0;
+  else if (sp_we[1])
+    sp_r[15:0] <= sp_next;
+end
+always_ff @(posedge clk or negedge nreset) begin
+  if (!nreset)
+    sp_r[ 7:0] <= '0;
+  else if (sp_we[0])
+    sp_r[ 7:0] <= sp_next;
+end
+
+assign sp_we[1] = instr_valid & (
+  (bus_x_ctrl.wen & (bus_x_ctrl.dst == BUS_DST_SP_H)) |
+  (bus_y_ctrl.wen & (bus_y_ctrl.dst == BUS_DST_SP_H)) |
+  (bus_p_ctrl.wen & (bus_p_ctrl.dst == BUS16_DST_SP)) |
+  (bus_q_ctrl.wen & (bus_q_ctrl.dst == BUS16_DST_SP)));
+
+assign sp_we[0] = instr_valid & (
+  (bus_x_ctrl.wen & (bus_x_ctrl.dst == BUS_DST_SP_L)) |
+  (bus_y_ctrl.wen & (bus_y_ctrl.dst == BUS_DST_SP_L)) |
+  (bus_p_ctrl.wen & (bus_p_ctrl.dst == BUS16_DST_SP)) |
+  (bus_q_ctrl.wen & (bus_q_ctrl.dst == BUS16_DST_SP)));
+
+assign sp_next[15:8] = ({8{bus_x_ctrl.dst == BUS_DST_SP_H}} & bus_x) |
+                       ({8{bus_y_ctrl.dst == BUS_DST_SP_H}} & bus_y) |
+                       ({8{bus_p_ctrl.dst == BUS16_DST_SP}} & bus_p[15:8]) |
+                       ({8{bus_q_ctrl.dst == BUS16_DST_SP}} & bus_q[15:8]);
+assign sp_next[ 7:0] = ({8{bus_x_ctrl.dst == BUS_DST_SP_L}} & bus_x) |
+                       ({8{bus_y_ctrl.dst == BUS_DST_SP_L}} & bus_y) |
+                       ({8{bus_p_ctrl.dst == BUS16_DST_SP}} & bus_p[ 7:0]) |
+                       ({8{bus_q_ctrl.dst == BUS16_DST_SP}} & bus_q[ 7:0]);
+
+ctrl ctrl_inst (
+  .clk          (clk),
+  .nreset       (nreset),
+
+  .rdata_i      (rdata_i),
+
+  .instr_valid_o(instr_valid),
+  .instr_o      (instr),
+
+  .pc_we_o      (ctrl_pc_we),
+  .pc_we_cc_o   (ctrl_pc_we_cc),
+  .pc_src_o     (ctrl_pc_src),
+  .cc_o         (ctrl_cc),
+
+  .reg8_src_o    (reg8_rsel),
+  .reg8_dest_o   (reg8_wsel),
+  .reg16_sel_o   (reg16_sel),
+
+  .alu_ctrl_o    (ctrl_alu_ctrl),
+
+  .addr_src_o    (addr_src),
+
+  .alu16_inc_o   (alu16_inc),
+  .alu16_dec_o   (alu16_dec),
+
+  .bus_x_ctrl_o (bus_x_ctrl),
+  .bus_y_ctrl_o (bus_y_ctrl),
+  .bus_p_ctrl_o (bus_p_ctrl),
+  .bus_q_ctrl_o (bus_q_ctrl)
+);
+
+
+regbank regbank_inst (
+  .clk             (clk),
+  .nreset          (nreset),
+
+
+  .reg16_wselect_i (reg16_sel),
+  .reg16_rselect_i (reg16_sel),
+  .reg16_we_i      (reg16_we),
+  .reg16_wdata_i   (reg16_wdata),
+  .reg16_rdata_o   (reg16_rdata),
+
+  .reg8_wselect_i  (reg8_wsel),
+  .reg8_we_i       (reg8_we),
+  .reg8_wdata_i    (reg8_wdata),
+  .reg8_rselect_i  (reg8_rsel),
+  .reg8_rdata_o    (reg8_rdata)
+);
+
+assign reg8_we = instr_valid & (
+  (bus_x_ctrl.wen & (bus_x_ctrl.dst == BUS_DST_REG8)) |
+  (bus_y_ctrl.wen & (bus_y_ctrl.dst == BUS_DST_REG8)));
+
+assign reg8_wdata = ({8{bus_x_ctrl.dst == BUS_DST_REG8}} & bus_x) |
+                    ({8{bus_y_ctrl.dst == BUS_DST_REG8}} & bus_y);
+
+assign reg16_we =
+  instr_valid & (
+    (bus_x_ctrl.wen & (bus_x_ctrl.dst == BUS_DST_REG16_H | bus_x_ctrl.dst == BUS_DST_REG16_L)) |
+    (bus_y_ctrl.wen & (bus_y_ctrl.dst == BUS_DST_REG16_H | bus_y_ctrl.dst == BUS_DST_REG16_L)) |
+    (bus_p_ctrl.wen & (bus_p_ctrl.dst == BUS16_DST_REG16)) |
+    (bus_q_ctrl.wen & (bus_q_ctrl.dst == BUS16_DST_REG16)));
+
+assign reg16_wdata[15:8] = ({8{bus_x_ctrl.dst == BUS_DST_REG16_H}} & bus_x) |
+                           ({8{bus_y_ctrl.dst == BUS_DST_REG16_H}} & bus_y) |
+                           ({8{bus_p_ctrl.dst == BUS16_DST_REG16}} & bus_p[15:8]) |
+                           ({8{bus_q_ctrl.dst == BUS16_DST_REG16}} & bus_q[15:8]);
+assign reg16_wdata[ 7:0] = ({8{bus_x_ctrl.dst == BUS_DST_REG16_L}} & bus_x) |
+                           ({8{bus_y_ctrl.dst == BUS_DST_REG16_L}} & bus_y) |
+                           ({8{bus_p_ctrl.dst == BUS16_DST_REG16}} & bus_p[ 7:0]) |
+                           ({8{bus_q_ctrl.dst == BUS16_DST_REG16}} & bus_q[ 7:0]);
+
+alu alu_inst (
+  .clk          (clk),
+  .nreset       (nreset),
+
+  .alu_ctrl_i   (alu_ctrl),
+
+  .operand_in_i (alu_operand_in),
+  .operand_out_o(alu_operand_out),
+
+  .reg_a_o       (reg_a),
+  .reg_f_o       (reg_f)
+);
+
+always_comb begin
+  alu_ctrl          = ctrl_alu_ctrl;
+  alu_ctrl.update_a = instr_valid & alu_ctrl.update_a;
+  alu_ctrl.update_f = instr_valid & alu_ctrl.update_f;
+end
+
+assign alu_operand_in = ({8{bus_x_ctrl.dst == BUS_DST_ALU}} & bus_x) |
+                        ({8{bus_y_ctrl.dst == BUS_DST_ALU}} & bus_y);
+
+alu16 alu16_inst (
+  .operand_in_i (alu16_in),
+  .operand_out_o(alu16_out),
+  .increment_i  (alu16_inc),
+  .decrement_i  (alu16_dec)
+);
+assign alu16_en = alu16_inc | alu16_dec;
+
+assign alu16_in = ({16{bus_p_ctrl.dst == BUS16_DST_ALU16}} & bus_p) |
+                  ({16{bus_q_ctrl.dst == BUS16_DST_ALU16}} & bus_q);
+
+assign addr_o = ({16{addr_src == ADDR_SRC_PC       }} &            pc_r       ) |
+                ({16{addr_src == ADDR_SRC_SP       }} &            sp_r       ) |
+                ({16{addr_src == ADDR_SRC_REG16    }} &            reg16_rdata) |
+                ({16{addr_src == ADDR_SRC_REG8     }} & {   8'hFF, reg8_rdata}) |
+                ({16{addr_src == ADDR_SRC_BUS_P    }} &            bus_p      ) |
+                ({16{addr_src == ADDR_SRC_BUS_Q    }} &            bus_q      ) |
+                ({16{addr_src == ADDR_SRC_OPERAND8 }} & {   8'hFF, instr[1]  }) |
+                ({16{addr_src == ADDR_SRC_OPERAND16}} & {instr[2], instr[1]  });
+
+assign we_o   = instr_valid & (
+  (bus_x_ctrl.wen & bus_x_ctrl.dst == BUS_DST_MEM) |
+  (bus_y_ctrl.wen & bus_y_ctrl.dst == BUS_DST_MEM));
+
+assign wdata_o = ({8{bus_x_ctrl.dst == BUS_DST_MEM}} & bus_x) |
+                 ({8{bus_y_ctrl.dst == BUS_DST_MEM}} & bus_y);
+
+`define INST_BUS(x) \
+  assign bus_``x = ({8{bus_``x``_ctrl.wen & (bus_``x``_ctrl.src == BUS_SRC_OPERAND8_H)}} & instr[2])          | \
+                   ({8{bus_``x``_ctrl.wen & (bus_``x``_ctrl.src == BUS_SRC_OPERAND8_L)}} & instr[1])          | \
+                   ({8{bus_``x``_ctrl.wen & (bus_``x``_ctrl.src == BUS_SRC_REG8      )}} & reg8_rdata)        | \
+                   ({8{bus_``x``_ctrl.wen & (bus_``x``_ctrl.src == BUS_SRC_A         )}} & reg_a)             | \
+                   ({8{bus_``x``_ctrl.wen & (bus_``x``_ctrl.src == BUS_SRC_PC_H      )}} & pc_r[15:8])        | \
+                   ({8{bus_``x``_ctrl.wen & (bus_``x``_ctrl.src == BUS_SRC_PC_L      )}} & pc_r[ 7:0])        | \
+                   ({8{bus_``x``_ctrl.wen & (bus_``x``_ctrl.src == BUS_SRC_REG16_H   )}} & reg16_rdata[15:8]) | \
+                   ({8{bus_``x``_ctrl.wen & (bus_``x``_ctrl.src == BUS_SRC_REG16_L   )}} & reg16_rdata[ 7:0]) | \
+                   ({8{bus_``x``_ctrl.wen & (bus_``x``_ctrl.src == BUS_SRC_MEM       )}} & rdata_i          ) | \
+                   ({8{bus_``x``_ctrl.wen & (bus_``x``_ctrl.src == BUS_SRC_ALU       )}} & alu_operand_out);
+
+`INST_BUS(x);
+`INST_BUS(y);
+
+`undef INST_BUS
+
+`define INST_BUS16(x) \
+  assign bus_``x = ({16{bus_``x``_ctrl.wen & (bus_``x``_ctrl.src == BUS16_SRC_REG16)}} & reg16_rdata) | \
+                   ({16{bus_``x``_ctrl.wen & (bus_``x``_ctrl.src == BUS16_SRC_ALU16)}} & alu16_out  ) | \
+                   ({16{bus_``x``_ctrl.wen & (bus_``x``_ctrl.src == BUS16_SRC_SP   )}} & sp_r);
+
+`INST_BUS16(p);
+`INST_BUS16(q);
+
+`undef INST_BUS16
+
+endmodule : cpu
\ No newline at end of file
diff --git a/rtl/cpu/cpu_pkg.svh b/rtl/cpu/cpu_pkg.svh
index 25c7517..a310fb0 100644
--- a/rtl/cpu/cpu_pkg.svh
+++ b/rtl/cpu/cpu_pkg.svh
@@ -1,128 +1,144 @@
 package cpu_pkg;
 
-    //
-    // 4  | ST0_ADDR -> ST1_DEC -> ST2_EXEC -> ST3_NOP
-    // 12 |                    '-> ST2_DEC  -> ST3_DEC -> ST4_EXEC -> .... -> ST11_NOP
-    //
-    typedef enum {
-        ST0_ADDR,
-        ST1_DEC,
-        ST2_EXEC,
-        ST2_DEC,
-        ST3_DEC,
-        ST3_EXEC,
-        ST4_EXEC,
-        ST5_EXEC
-    } state_t;
+typedef enum logic [2:0] {
+  REG8_B   = 3'h00,
+  REG8_C   = 3'h01,
+  REG8_D   = 3'h02,
+  REG8_E   = 3'h03,
+  REG8_H   = 3'h04,
+  REG8_L   = 3'h05,
+  REG8_PHL = 3'h06,
+  REG8_A   = 3'h07
+} reg8_t;
 
-    typedef enum logic [2:0] {
-        REG8_B   = 3'h00,
-        REG8_C   = 3'h01,
-        REG8_D   = 3'h02,
-        REG8_E   = 3'h03,
-        REG8_H   = 3'h04,
-        REG8_L   = 3'h05,
-        REG8_PHL = 3'h06,
-        REG8_A   = 3'h07
-    } reg8_t;
+typedef enum logic [1:0] {
+  REG16_BC    = 2'h00,
+  REG16_DE    = 2'h01,
+  REG16_HL    = 2'h02,
+  REG16_SP_AF = 2'h03
+} reg16_t;
 
-    typedef enum logic [1:0] {
-        REG16_BC    = 2'h00,
-        REG16_DE    = 2'h01,
-        REG16_HL    = 2'h02,
-        REG16_SP_AF = 2'h03
-    } reg16_t;
+typedef enum logic [1:0] {
+  CC_NZ = 2'h00,
+  CC_Z  = 2'h01,
+  CC_NC = 2'h02,
+  CC_C  = 2'h03
+} cc_t;
 
-    typedef enum logic [1:0] {
-        CC_NZ = 2'h00,
-        CC_Z  = 2'h01,
-        CC_NC = 2'h02,
-        CC_C  = 2'h03
-    } cc_t;
+typedef enum logic [2:0] {
+  ALU_OP_ADD  = 3'h00,
+  ALU_OP_ADC  = 3'h01,
+  ALU_OP_SUB  = 3'h02,
+  ALU_OP_SBC  = 3'h03,
+  ALU_OP_AND  = 3'h04,
+  ALU_OP_XOR  = 3'h05,
+  ALU_OP_OR   = 3'h06,
+  ALU_OP_CP   = 3'h07
+} alu_op_t;
 
-    typedef enum logic [4:0] {
-        ALU_OP_ADD  = 5'h00,
-        ALU_OP_ADC  = 5'h01,
-        ALU_OP_SUB  = 5'h02,
-        ALU_OP_SBC  = 5'h03,
-        ALU_OP_AND  = 5'h04,
-        ALU_OP_XOR  = 5'h05,
-        ALU_OP_OR   = 5'h06,
-        ALU_OP_CP   = 5'h07,
-        ALU_OP_BIT  = 5'h08,
-        ALU_OP_NOP  = 5'h09, // Write operand to a
-        ALU_OP_NOPF = 5'h0a, // Write operand to f
-        ALU_OP_ROT  = 5'h0b,
-        ALU_OP_INC  = 5'h0c,
-        ALU_OP_INCR = 5'h0d // Increment register instead of a
-    } alu_op_t;
+typedef enum logic [2:0] {
+  SHIFT_OP_RLC = 3'h00,
+  SHIFT_OP_RRC = 3'h01,
+  SHIFT_OP_RL  = 3'h02,
+  SHIFT_OP_RR  = 3'h03,
+  SHIFT_OP_SLA = 3'h04,
+  SHIFT_OP_SRA = 3'h05,
+  SHIFT_OP_SLL = 3'h06,
+  SHIFT_OP_SRL = 3'h07
+} shift_op_t;
 
-    typedef enum logic [2:0] {
-        ROT_OP_RLC = 3'h00,
-        ROT_OP_RRC = 3'h01,
-        ROT_OP_RL  = 3'h02,
-        ROT_OP_RR  = 3'h03,
-        ROT_OP_SLA = 3'h04,
-        ROT_OP_SRA = 3'h05,
-        ROT_OP_SLL = 3'h06,
-        ROT_OP_SRL = 3'h07
-    } rot_op_t;
+typedef struct packed {
+  logic       b;
+  logic       s;
+  logic       r;
+  logic [2:0] operand;
+} bsr_op_t;
 
-    typedef enum logic [1:0] {
-        ALU16_OP_ADD = 2'h00,
-        ALU16_INC    = 2'h01,
-        ALU16_DEC    = 2'h02
-    } alu16_op_t;
+typedef struct packed {
+  logic inc;
+  logic dec;
+  logic dst_a;
+} incdec_op_t;
 
-    typedef enum {
-        OP_SRC_A,
-        OP_SRC_F,
-        OP_SRC_REG8,
-        OP_SRC_OPERAND8,
-        OP_SRC_MEMORY,
-        OP_SRC_OPERAND16,
-        OP_SRC_REG16,
-        OP_SRC_PC_L,
-        OP_SRC_PC_H
-    } op_src_t;
+typedef struct packed {
+  logic       update_a;
+  logic       update_f;
+  logic       alu_op_valid;
+  alu_op_t    alu_op;
+  logic       shift_op_valid;
+  shift_op_t  shift_op;
+  logic       bsr_op_valid;
+  bsr_op_t    bsr_op;
+  logic       incdec_op_valid;
+  incdec_op_t incdec_op;
+} alu_ctrl_t;
 
-    typedef enum {
-        OP_DEST_A,
-        OP_DEST_REG8,
-        OP_DEST_REG16,
-        OP_DEST_MEMORY
-    } op_dest_t;
+typedef enum {
+  ADDR_SRC_PC,
+  ADDR_SRC_SP,
+  ADDR_SRC_REG16, // TODO reuse BUS_Q
+  ADDR_SRC_REG8,  // TODO reuse BUS_Q
+  ADDR_SRC_OPERAND8,
+  ADDR_SRC_OPERAND16,
+  ADDR_SRC_BUS_P,
+  ADDR_SRC_BUS_Q
+} addr_src_t;
 
-    typedef enum {
-        SP_SRC_OPERAND16,
-        SP_SRC_INC,
-        SP_SRC_DEC
-    } sp_src_t;
+typedef enum {
+  BUS_SRC_A,
+  BUS_SRC_F,
+  BUS_SRC_REG8,
+  BUS_SRC_OPERAND8_H,
+  BUS_SRC_OPERAND8_L,
+  BUS_SRC_ALU,
+  BUS_SRC_MEM,
+  BUS_SRC_PC_H,
+  BUS_SRC_PC_L,
+  BUS_SRC_REG16_H,
+  BUS_SRC_REG16_L
+} bus_src_t;
 
-    typedef enum {
-        ADR_SRC_PC,
-        ADR_SRC_HL,
-        ADR_SRC_SP,
-        ADR_SRC_SP_P1, // SP + 1 for pop
-        ADR_SRC_REG8, // extended with FF
-        ADR_SRC_REG16,
-        ADR_SRC_OPERAND8, // extended with FF
-        ADR_SRC_OPERAND16
-    } adr_src_t;
+typedef enum {
+  BUS_DST_SP_H,
+  BUS_DST_SP_L,
+  BUS_DST_ALU,
+  BUS_DST_REG8,
+  BUS_DST_REG16_H,
+  BUS_DST_REG16_L,
+  BUS_DST_PC_H,
+  BUS_DST_PC_L,
+  BUS_DST_MEM
+} bus_dst_t;
 
-    typedef enum {
-        PC_SRC_SEQ,
-        PC_SRC_OPERAND8,
-        PC_SRC_OPERAND16
-    } pc_src_t;
+typedef struct packed {
+  bus_src_t   src;
+  bus_dst_t   dst;
+  logic       wen;
+} bus_ctrl_t;
 
-endpackage
+typedef enum {
+  BUS16_SRC_REG16,
+  BUS16_SRC_ALU16,
+  BUS16_SRC_SP
+} bus16_src_t;
 
-`define DEF_FF(register, next, we, rst_value) \
-    always_ff @(posedge clk_i or negedge nreset_i) begin \
-        if (~nreset_i) begin \
-            register <= (rst_value); \
-        end else if ((we)) begin \
-            register <= (next); \
-        end \
-    end
+typedef enum {
+  BUS16_DST_REG16,
+  BUS16_DST_ALU16,
+  BUS16_DST_SP
+} bus16_dst_t;
+
+typedef struct packed {
+  bus16_src_t src;
+  bus16_dst_t dst;
+  logic       wen;
+} bus16_ctrl_t;
+
+typedef enum {
+  PC_SRC_INC,
+  PC_SRC_OPERAND8,
+  PC_SRC_OPERAND16,
+  PC_SRC_BUS
+} pc_src_t;
+
+endpackage : cpu_pkg
diff --git a/rtl/cpu/ctrl.sv b/rtl/cpu/ctrl.sv
new file mode 100644
index 0000000..50b0847
--- /dev/null
+++ b/rtl/cpu/ctrl.sv
@@ -0,0 +1,180 @@
+`include "cpu_pkg.svh"
+
+import cpu_pkg::*;
+
+module ctrl (
+  input  logic clk,
+  input  logic nreset,
+
+  input  logic [ 7:0] rdata_i,
+
+  output logic        instr_valid_o,
+  output logic [ 7:0] instr_o      [2:0],
+
+  output logic        pc_we_o,    // update PC always
+  output logic        pc_we_cc_o, // update PC conditionally
+  output pc_src_t     pc_src_o,
+  output cc_t         cc_o,
+
+  output reg8_t      reg8_src_o,
+  output reg8_t      reg8_dest_o,
+  output reg16_t     reg16_sel_o,
+
+  output alu_ctrl_t  alu_ctrl_o,
+
+  output logic       alu16_inc_o,
+  output logic       alu16_dec_o,
+
+  output addr_src_t  addr_src_o,
+
+  output bus_ctrl_t   bus_x_ctrl_o,
+  output bus_ctrl_t   bus_y_ctrl_o,
+  output bus16_ctrl_t bus_p_ctrl_o,
+  output bus16_ctrl_t bus_q_ctrl_o
+);
+
+typedef enum {
+  ST_RESET,
+  ST_ADDR,
+  ST_FETCH0,
+  ST_FETCH1,
+  ST_FETCH2,
+  ST_EXEC1,
+  ST_EXEC2,
+  ST_EXEC3
+} state_t;
+
+logic   halt;
+logic   instr_undef;
+
+logic   halted_r;
+
+logic   state_en;
+state_t state_r;
+state_t state_next;
+logic   state_ex;
+
+logic        instr_we        [2:0];
+logic [ 7:0] instr_r         [2:0];
+logic [ 7:0] instr           [2:0];
+
+logic            req_instr1;
+logic            req_instr2;
+logic            req_ex2;
+logic            req_ex3;
+
+addr_src_t       instr_addr_src;
+
+logic is_ex1;
+logic is_ex2;
+logic is_ex3;
+
+logic    incr_pc;
+logic    idec_pc_we;
+logic    idec_pc_we_cc;
+pc_src_t idec_pc_src;
+
+idec idec_inst (
+  .instr_i      (instr),
+  .is_ex1_i     (is_ex1),
+  .is_ex2_i     (is_ex2),
+  .is_ex3_i     (is_ex3),
+
+  .req_instr1_o (req_instr1),
+  .req_instr2_o (req_instr2),
+  .req_ex2_o    (req_ex2),
+  .req_ex3_o    (req_ex3),
+
+  .reg8_src_o    (reg8_src_o),
+  .reg8_dest_o   (reg8_dest_o),
+  .reg16_sel_o   (reg16_sel_o),
+
+  .bus_x_ctrl_o (bus_x_ctrl_o),
+  .bus_y_ctrl_o (bus_y_ctrl_o),
+  .bus_p_ctrl_o (bus_p_ctrl_o),
+  .bus_q_ctrl_o (bus_q_ctrl_o),
+
+  .alu_ctrl_o    (alu_ctrl_o),
+
+  .alu16_inc_o   (alu16_inc_o),
+  .alu16_dec_o   (alu16_dec_o),
+
+  .addr_src_o    (instr_addr_src),
+
+  .pc_we_o        (idec_pc_we),
+  .pc_we_cc_o     (idec_pc_we_cc),
+  .cc_o           (cc_o),
+  .pc_src_o       (idec_pc_src),
+
+  .instr_undef_o (instr_undef)
+);
+
+assign is_ex1 = state_r == ST_EXEC1;
+assign is_ex2 = state_r == ST_EXEC2;
+assign is_ex3 = state_r == ST_EXEC3;
+
+assign halt = instr_undef & is_ex1;
+
+assign state_en = ~halted_r;
+
+always_ff @(posedge clk or negedge nreset) begin
+  if (!nreset)
+    state_r <= ST_RESET;
+  else if (state_en)
+    state_r <= state_next;
+end
+
+always_ff @(posedge clk or negedge nreset) begin
+  if (!nreset)
+    halted_r <= 1'b0;
+  else if (halt)
+    halted_r <= 1'b1;
+end
+
+generate for (genvar i = 0; i < 3; i++) begin : gen_instr_r
+  always_ff @(posedge clk or negedge nreset) begin
+    if (!nreset)
+      instr_r[i] <= 8'h0;
+    else if (instr_we[i])
+      instr_r[i] <= rdata_i;
+  end
+end endgenerate
+
+assign instr_we[0] = (state_r == ST_FETCH0);
+assign instr_we[1] = (state_r == ST_FETCH1);
+assign instr_we[2] = (state_r == ST_FETCH2);
+
+always_comb begin
+  case (state_r)
+    ST_RESET:  state_next = ST_ADDR;
+    ST_ADDR:   state_next = ST_FETCH0;
+    ST_FETCH0: state_next = req_instr1 ? ST_FETCH1 : ST_EXEC1;
+    ST_FETCH1: state_next = req_instr2 ? ST_FETCH2 : ST_EXEC1;
+    ST_FETCH2: state_next = ST_EXEC1;
+    ST_EXEC1:  state_next = req_ex2 ? ST_EXEC2 : ST_ADDR;
+    ST_EXEC2:  state_next = req_ex3 ? ST_EXEC3 : ST_ADDR;
+    ST_EXEC3:  state_next = ST_ADDR;
+  endcase
+end
+
+assign state_ex = is_ex1 | is_ex2 | is_ex3;
+
+assign incr_pc = (state_r == ST_ADDR)                |
+                 (state_r == ST_FETCH0 & req_instr1) |
+                 (state_r == ST_FETCH1 & req_instr2);
+
+assign pc_we_o    = state_ex ? idec_pc_we  : incr_pc;
+assign pc_we_cc_o = state_ex & idec_pc_we_cc;
+assign pc_src_o   = state_ex ? idec_pc_src : PC_SRC_INC;
+
+assign instr_valid_o = state_ex & ~instr_undef;
+
+assign instr[0]    = (state_r == ST_FETCH0) ? rdata_i : instr_r[0];
+assign instr[1]    = (state_r == ST_FETCH1) ? rdata_i : instr_r[1];
+assign instr[2]    = (state_r == ST_FETCH2) ? rdata_i : instr_r[2];
+
+assign instr_o = instr;
+
+assign addr_src_o = state_ex ? instr_addr_src : ADDR_SRC_PC;
+
+endmodule : ctrl
diff --git a/rtl/cpu/decode.sv b/rtl/cpu/decode.sv
deleted file mode 100644
index 32d2b0f..0000000
--- a/rtl/cpu/decode.sv
+++ /dev/null
@@ -1,233 +0,0 @@
-`include "cpu_pkg.svh"
-
-import cpu_pkg::*;
-
-module decode (
-    input  logic [7:0] instr0_i,
-    input  logic [7:0] instr1_i,
-    input  logic [7:0] instr2_i,
-
-    input  state_t     state_i,
-
-    output logic       need_instr1_o,
-    output logic       need_instr2_o,
-    output logic       is_multicycle_o,
-    output logic       is_multicycle2_o,
-    output logic       undef_o,
-
-    output logic       sp_we_o,
-    output sp_src_t    sp_src_o,
-
-    output logic       alu_op_valid_o,
-    output alu_op_t    alu_op_o,
-    output logic       reg_write_alu_o,
-    output logic       rot_op_valid_o,
-    output rot_op_t    rot_op_o,
-    output op_src_t    op_src_o,
-    output op_dest_t   op_dest_o,
-
-    output reg8_t      reg8_dest_o,
-    output reg8_t      reg8_src_o,
-    output alu16_op_t  alu16_op_o,
-    output reg16_t     reg16_dest_o,
-    output reg16_t     reg16_src_o,
-
-    output logic       branch_always_o,
-    output cc_t        cc_o,
-
-    output pc_src_t    pc_src_o,
-    output adr_src_t   adr_src_o,
-    output logic       memory_we_o
-);
-
-    logic [1:0] dec_x;
-    logic [2:0] dec_y;
-    logic [2:0] dec_z;
-    logic [1:0] dec_p;
-    logic       dec_q;
-
-    logic is_cb;
-    logic is_ld_r_r;
-    logic is_ld_a_n;
-    logic is_ld_r_n;
-    logic is_ldh_c_a;
-    logic is_ldh_n_a;
-    logic is_ld_rr_a;
-    logic is_ld_nn_a;
-    logic is_ld_a_rr;
-    logic is_ld_a_nn;
-    logic is_ld_rr_nn;
-    logic is_ld_sp_nn;
-    logic is_ldd_hl_a;
-    logic is_alu_a_r;
-    logic is_bit_n_r;
-    logic is_rot_r;
-    logic is_rla;
-    logic is_inc_a;
-    logic is_inc_r;
-    logic is_jr_cc_n;
-    logic is_call_nn;
-    logic is_push_rr;
-    logic is_pop_rr;
-
-    reg8_t reg8_dest;
-    reg8_t reg8_src;
-    reg16_t reg16_src;
-
-    assign dec_x = instr0_i[7:6];
-    assign dec_y = instr0_i[5:3];
-    assign dec_z = instr0_i[2:0];
-    assign dec_p = instr0_i[5:4];
-    assign dec_q = instr0_i[3];
-
-    assign is_cb = (instr0_i == 8'hCB);
-
-    assign is_ld_r_r   = (dec_x == 2'h1) & ((dec_z != 3'h6) | (dec_y != 3'h6));
-    assign is_ld_a_n   = is_ld_r_n & (reg8_dest == REG8_A);
-    assign is_ld_r_n   = (dec_x == 2'h0) & (dec_z == 3'h6);
-    assign is_ld_rr_nn = (dec_x == 2'h0) & (dec_z == 3'h1) & ~dec_q & (dec_p != 2'h3);
-    assign is_ld_sp_nn = (dec_x == 2'h0) & (dec_z == 3'h1) & ~dec_q & (dec_p == 2'h3);
-    assign is_ldd_hl_a = (dec_x == 2'h0) & (dec_z == 3'h2) & ~dec_q & (dec_p == 2'h3);
-    assign is_ld_rr_a  = (dec_x == 2'h0) & (dec_z == 3'h2) & ~dec_q & (dec_p != 2'h3);
-    assign is_ld_nn_a  = (dec_x == 2'h0) & (dec_z == 3'h2) & ~dec_q & (dec_p == 2'h2);
-    assign is_ld_a_rr  = (dec_x == 2'h0) & (dec_z == 3'h2) &  dec_q & (dec_p != 2'h3);
-    assign is_ld_a_nn  = (dec_x == 2'h0) & (dec_z == 3'h2) &  dec_q & (dec_p == 2'h3);
-    assign is_ldh_c_a  = instr0_i == 8'hE2;
-    assign is_ldh_n_a  = instr0_i == 8'hE0;
-
-    assign is_alu_a_r = (dec_x == 3'h2);
-    assign is_bit_n_r = (is_cb & instr1_i[7:6] == 2'h1);
-    assign is_rot_r   = (is_cb & instr1_i[7:6] == 2'h0);
-
-    assign is_rla   = (dec_x == 3'h0) & (dec_z == 3'h7) & (dec_y == 3'h2);
-
-    assign is_inc_a = is_inc_r & (reg8_dest == REG8_A);
-    assign is_inc_r = (dec_x == 2'h0) & (dec_z == 3'h4);
-
-    assign is_jr_cc_n = (dec_x == 2'h0) & (dec_z == 2'h0) & dec_y[2];
-    assign is_call_nn = (instr0_i == 8'hCD);
-    assign is_push_rr = (dec_x == 2'h3) & (dec_z == 3'h5) & ~dec_q;
-    assign is_pop_rr  = (dec_x == 2'h3) & (dec_z == 3'h1) & ~dec_q;
-
-    assign reg8_src = is_cb                              ? reg8_t'(instr1_i[2:0]) :
-                      is_ldh_c_a                         ? REG8_C                 :
-                      is_inc_r                           ? reg8_t'(dec_y)         :
-                      (is_push_rr & state_i == ST2_EXEC) ? reg8_t'({dec_p, 1'b0})  :
-                      (is_push_rr & state_i == ST3_EXEC) ? reg8_t'({dec_p, 1'b1})  :
-                                                           reg8_t'(dec_z);
-    assign reg8_dest = is_rot_r ? reg8_t'(dec_z) :
-                      (is_pop_rr  & state_i == ST3_EXEC) ? reg8_t'({dec_p, 1'b1})  :
-                      (is_pop_rr  & state_i == ST4_EXEC) ? reg8_t'({dec_p, 1'b0})  :
-                                  reg8_t'(dec_y);
-
-    assign reg16_src = is_ldd_hl_a ? REG16_HL : reg16_t'(dec_p);
-
-    assign need_instr1_o   = is_ld_sp_nn | is_ld_rr_nn | is_ld_a_nn | is_call_nn | is_cb | is_jr_cc_n | is_ld_r_n | is_ldh_n_a;
-    assign need_instr2_o   = is_ld_sp_nn | is_ld_rr_nn | is_ld_a_nn | is_call_nn;
-    assign is_multicycle_o = is_ld_a_rr | is_call_nn | is_push_rr | is_pop_rr;
-    assign is_multicycle2_o = is_pop_rr;
-
-    assign undef_o = ~(is_ldh_n_a | is_ld_r_r | is_ld_sp_nn | is_ld_rr_nn | is_alu_a_r | is_ldd_hl_a | is_bit_n_r |
-                       is_jr_cc_n | is_ld_r_n | is_ldh_c_a  | is_inc_r    | is_ld_rr_a | is_ld_a_rr  | is_call_nn |
-                       is_push_rr | is_pop_rr | (is_rot_r & (instr1_i[5:3] == 3'h02)) | is_rla);
-
-    assign sp_we_o  = is_ld_sp_nn | is_call_nn | is_push_rr | (is_pop_rr & ((state_i == ST2_EXEC) | (state_i == ST3_EXEC)));
-    assign sp_src_o = is_ld_sp_nn ? SP_SRC_OPERAND16 :
-                      is_call_nn  ? SP_SRC_DEC       :
-                      is_push_rr  ? SP_SRC_DEC       :
-                      is_pop_rr   ? SP_SRC_INC       :
-                                    sp_src_t'('X);
-
-    assign alu_op_valid_o = is_alu_a_r | is_bit_n_r | is_ld_a_n | is_ld_a_rr | is_ld_a_nn | is_inc_r | is_rla | (is_pop_rr & reg16_src == REG16_SP_AF);
-    assign alu_op_o       = is_bit_n_r ? ALU_OP_BIT  :
-                            is_ld_a_n  ? ALU_OP_NOP  :
-                            is_ld_a_nn ? ALU_OP_NOP  :
-                            is_ld_a_rr ? ALU_OP_NOP  :
-                            is_inc_a   ? ALU_OP_INC  :
-                            is_inc_r   ? ALU_OP_INCR :
-                            is_rla     ? ALU_OP_ROT  :
-                            (is_pop_rr & reg16_src == REG16_SP_AF & state_i == ST2_EXEC) ? ALU_OP_NOP  :
-                            (is_pop_rr & reg16_src == REG16_SP_AF & state_i == ST3_EXEC) ? ALU_OP_NOPF :
-                                         alu_op_t'({1'b0, dec_y});
-    assign reg_write_alu_o = is_inc_r | is_rot_r;
-
-    assign rot_op_valid_o  = is_rot_r | is_rla;
-    assign rot_op_o        = is_rla ? ROT_OP_RL :
-                                      rot_op_t'(instr1_i[5:3]);
-
-    assign op_dest_o      = (is_ld_r_r & reg8_dest == REG8_PHL) ? OP_DEST_MEMORY :
-                            (is_ld_r_n & reg8_dest == REG8_PHL) ? OP_DEST_MEMORY :
-                            (is_ld_r_r & reg8_dest == REG8_A)   ? OP_DEST_A      :
-                            is_ld_r_r                           ? OP_DEST_REG8   :
-                            is_ld_a_n                           ? OP_DEST_A      :
-                            is_ld_a_rr                          ? OP_DEST_A      :
-                            is_ld_a_nn                          ? OP_DEST_A      :
-                            is_ld_r_n                           ? OP_DEST_REG8   :
-                            is_ldh_c_a                          ? OP_DEST_MEMORY :
-                            is_ldh_n_a                          ? OP_DEST_MEMORY :
-                            is_ld_rr_nn                         ? OP_DEST_REG16  :
-                            is_ldd_hl_a                         ? OP_DEST_REG16  :
-                            is_inc_a                            ? OP_DEST_A      :
-                            is_inc_r                            ? OP_DEST_REG8   :
-                            is_call_nn                          ? OP_DEST_MEMORY :
-                            is_push_rr                          ? OP_DEST_MEMORY :
-                            (is_pop_rr & reg16_src != REG16_SP_AF) ? OP_DEST_REG8 :
-                            (is_pop_rr & reg16_src == REG16_SP_AF) ? OP_DEST_A   :
-                            is_rot_r                            ? OP_DEST_REG8   :
-                            is_rla                              ? OP_DEST_A      :
-                                                                  op_dest_t'('X);
-
-    assign op_src_o       = (is_ld_r_r  & reg8_src == REG8_A)   ? OP_SRC_A         :
-                            (is_alu_a_r & reg8_src == REG8_A)   ? OP_SRC_A         :
-                            (is_alu_a_r & reg8_src != REG8_A)   ? OP_SRC_REG8      :
-                            (is_ld_r_r  & reg8_src == REG8_PHL) ? OP_SRC_MEMORY    :
-                            is_inc_r                            ? OP_SRC_REG8      :
-                            is_ld_r_n                           ? OP_SRC_OPERAND8  :
-                            is_ldh_c_a                          ? OP_SRC_A         :
-                            is_ldh_n_a                          ? OP_SRC_A         :
-                            is_ld_a_rr                          ? OP_SRC_MEMORY    :
-                            is_ld_a_nn                          ? OP_SRC_MEMORY    :
-                            is_ld_rr_nn                         ? OP_SRC_OPERAND16 :
-                            is_ldd_hl_a                         ? OP_SRC_REG16     :
-                            is_bit_n_r                          ? OP_SRC_REG8      :
-                            is_rot_r                            ? OP_SRC_REG8      :
-                            is_rla                              ? OP_SRC_A         :
-                            (is_call_nn & state_i == ST4_EXEC)  ? OP_SRC_PC_L      :
-                            (is_call_nn & state_i == ST5_EXEC)  ? OP_SRC_PC_H      :
-                            (is_push_rr & reg16_src != REG16_SP_AF) ? OP_SRC_REG8  :
-                            (is_push_rr & reg16_src == REG16_SP_AF & state_i == ST2_EXEC) ? OP_SRC_A  :
-                            (is_push_rr & reg16_src == REG16_SP_AF & state_i == ST3_EXEC) ? OP_SRC_F  :
-                            is_pop_rr                           ? OP_SRC_MEMORY    :
-                                                                  op_src_t'('X);
-
-    assign reg8_dest_o  = reg8_dest;
-    assign reg8_src_o   = reg8_src;
-    assign reg16_src_o  = reg16_src;
-    assign reg16_dest_o = reg16_src;
-
-    assign pc_src_o = is_jr_cc_n ? PC_SRC_OPERAND8  :
-                      is_call_nn ? PC_SRC_OPERAND16 :
-                                   PC_SRC_SEQ;
-
-    assign adr_src_o = (is_ld_r_r & reg8_dest == REG8_PHL) ? ADR_SRC_HL        :
-                       (is_ld_r_r & reg8_src  == REG8_PHL) ? ADR_SRC_HL        :
-                       is_ldd_hl_a                         ? ADR_SRC_HL        :
-                       (is_ld_r_n & reg8_dest == REG8_PHL) ? ADR_SRC_HL        :
-                       is_ldh_c_a                          ? ADR_SRC_REG8      :
-                       is_ldh_n_a                          ? ADR_SRC_OPERAND8  :
-                       is_ld_nn_a                          ? ADR_SRC_OPERAND16 :
-                       is_ld_a_nn                          ? ADR_SRC_OPERAND16 :
-                       is_ld_a_rr                          ? ADR_SRC_REG16     :
-                       is_ld_rr_a                          ? ADR_SRC_REG16     :
-                       is_call_nn                          ? ADR_SRC_SP        :
-                       is_push_rr                          ? ADR_SRC_SP        :
-                       is_pop_rr                           ? ADR_SRC_SP_P1     :
-                                                             ADR_SRC_PC;
-
-    assign branch_always_o = is_call_nn;
-    assign cc_o = cc_t'(dec_y[1:0]);
-
-    assign memory_we_o = is_ldd_hl_a | is_ldh_c_a | is_ldh_n_a | is_ld_rr_a | (is_ld_r_n & reg8_dest == REG8_PHL) | (is_ld_r_r & reg8_dest == REG8_PHL) |
-                         is_call_nn  | is_push_rr;
-
-endmodule : decode
diff --git a/rtl/cpu/idec.sv b/rtl/cpu/idec.sv
new file mode 100644
index 0000000..652db9e
--- /dev/null
+++ b/rtl/cpu/idec.sv
@@ -0,0 +1,347 @@
+`include "cpu_pkg.svh"
+
+import cpu_pkg::*;
+
+module idec (
+  input  logic [7:0] instr_i[2:0],
+
+  input  logic       is_ex1_i,
+  input  logic       is_ex2_i,
+  input  logic       is_ex3_i,
+
+  output logic       req_instr1_o,
+  output logic       req_instr2_o,
+  output logic       req_ex2_o,
+  output logic       req_ex3_o,
+
+  output bus_ctrl_t   bus_x_ctrl_o,
+  output bus_ctrl_t   bus_y_ctrl_o,
+  output bus16_ctrl_t bus_p_ctrl_o,
+  output bus16_ctrl_t bus_q_ctrl_o,
+
+  output reg8_t      reg8_src_o,
+  output reg8_t      reg8_dest_o,
+  output reg16_t     reg16_sel_o,
+
+  output alu_ctrl_t  alu_ctrl_o,
+
+  output logic       alu16_inc_o,
+  output logic       alu16_dec_o,
+
+  output addr_src_t  addr_src_o,
+
+  output logic       pc_we_o,
+  output logic       pc_we_cc_o,
+  output cc_t        cc_o,
+  output pc_src_t    pc_src_o,
+
+  output logic       instr_undef_o
+
+);
+
+logic [1:0] dec_x;
+logic [2:0] dec_y;
+logic [2:0] dec_z;
+logic [1:0] dec_p;
+logic       dec_q;
+
+logic is_alu_a_r;
+logic is_alu_a_n;
+logic is_ld_sp_nnnn;
+logic is_ld_rr_nnnn;
+logic is_ld_pnnnn_a;
+logic is_ldd_hl_a;
+logic is_ldi_hl_a;
+logic is_ldd_a_hl;
+logic is_ldi_a_hl;
+logic is_cb;
+logic is_ld_r_r;
+logic is_ld_r_n;
+logic is_ldh_c_a;
+logic is_ldh_n_a;
+logic is_ldh_a_n;
+logic is_ld_rr_a;
+logic is_ld_a_pbc;
+logic is_ld_a_pde;
+logic is_ld_a_nn;
+logic is_bit_n_r;
+logic is_shift_r;
+logic is_rla;
+logic is_inc_r;
+logic is_dec_r;
+logic is_jr_cc_nn;
+logic is_jr_nn;
+logic is_call_nn;
+logic is_ret;
+logic is_push_rr;
+logic is_pop_rr;
+logic is_inc_rr;
+logic is_dec_rr;
+
+reg8_t  reg8_src;
+reg8_t  reg8_dest;
+reg16_t reg16_sel;
+
+assign req_instr1_o = is_ld_sp_nnnn | is_ld_rr_nnnn | is_call_nn | is_ld_pnnnn_a | is_cb | is_jr_nn | is_jr_cc_nn | is_ld_r_n | is_ldh_n_a | is_ldh_a_n | is_alu_a_n;
+assign req_instr2_o = is_ld_sp_nnnn | is_ld_rr_nnnn | is_call_nn | is_ld_pnnnn_a;
+assign req_ex2_o    = is_call_nn | is_ret | is_push_rr | is_pop_rr | ((is_ld_r_r | is_alu_a_r) & (reg8_src == REG8_PHL));
+assign req_ex3_o    = is_ret | is_pop_rr;
+
+assign dec_x = instr_i[0][7:6];
+assign dec_y = instr_i[0][5:3];
+assign dec_z = instr_i[0][2:0];
+assign dec_p = instr_i[0][5:4];
+assign dec_q = instr_i[0][3];
+
+assign is_jr_nn      = instr_i[0] == 8'h18;
+assign is_cb         = instr_i[0] == 8'hCB;
+assign is_call_nn    = instr_i[0] == 8'hCD;
+assign is_ret        = instr_i[0] == 8'hC9;
+assign is_ldh_c_a    = instr_i[0] == 8'hE2;
+assign is_ldh_n_a    = instr_i[0] == 8'hE0;
+assign is_ldh_a_n    = instr_i[0] == 8'hF0;
+assign is_ld_pnnnn_a = instr_i[0] == 8'hEA;
+
+assign is_jr_cc_nn   = (dec_x == 2'h0) & (dec_z == 2'h0) & dec_y[2];
+assign is_ld_rr_nnnn = (dec_x == 2'h0) & (dec_z == 3'h1) & ~dec_q & (dec_p != 2'h3);
+assign is_ld_sp_nnnn = (dec_x == 2'h0) & (dec_z == 3'h1) & ~dec_q & (dec_p == 2'h3);
+assign is_ldi_hl_a   = (dec_x == 2'h0) & (dec_z == 3'h2) & ~dec_q & (dec_p == 2'h2);
+assign is_ldd_hl_a   = (dec_x == 2'h0) & (dec_z == 3'h2) & ~dec_q & (dec_p == 2'h3);
+assign is_ld_a_pbc   = (dec_x == 2'h0) & (dec_z == 3'h2) &  dec_q & (dec_p == 2'h0);
+assign is_ld_a_pde   = (dec_x == 2'h0) & (dec_z == 3'h2) &  dec_q & (dec_p == 2'h1);
+assign is_ldi_a_hl   = (dec_x == 2'h0) & (dec_z == 3'h2) &  dec_q & (dec_p == 2'h2);
+assign is_ldd_a_hl   = (dec_x == 2'h0) & (dec_z == 3'h2) &  dec_q & (dec_p == 2'h3);
+assign is_inc_rr     = (dec_x == 2'h0) & (dec_z == 3'h3) & ~dec_q;
+assign is_dec_rr     = (dec_x == 2'h0) & (dec_z == 3'h3) &  dec_q;
+assign is_inc_r      = (dec_x == 2'h0) & (dec_z == 3'h4);
+assign is_dec_r      = (dec_x == 2'h0) & (dec_z == 3'h5);
+assign is_ld_r_n     = (dec_x == 2'h0) & (dec_z == 3'h6);
+assign is_ld_rr_a    = '0;
+assign is_rla        = (dec_x == 3'h0) & (dec_z == 3'h7) & (dec_y == 3'h2);
+
+assign is_ld_r_r     = (dec_x == 2'h1) & ((dec_z != 3'h6) | (dec_y != 3'h6));
+
+assign is_alu_a_r    = (dec_x == 2'h2);
+
+assign is_pop_rr     = (dec_x == 2'h3) & (dec_z == 3'h1) & ~dec_q;
+assign is_push_rr    = (dec_x == 2'h3) & (dec_z == 3'h5) & ~dec_q;
+assign is_alu_a_n    = (dec_x == 2'h3) & (dec_z == 3'h6);
+
+assign is_bit_n_r = (is_cb & instr_i[1][7:6] == 2'h1);
+assign is_shift_r = (is_cb & instr_i[1][7:6] == 2'h0) | is_rla;
+
+
+assign bus_x_ctrl_o.src = (is_ld_sp_nnnn                         ) ? BUS_SRC_OPERAND8_H :
+                          (is_ld_rr_nnnn                         ) ? BUS_SRC_OPERAND8_H :
+                          (is_ld_pnnnn_a                         ) ? BUS_SRC_A          :
+                          (is_ldd_hl_a                           ) ? BUS_SRC_A          :
+                          (is_ldi_hl_a                           ) ? BUS_SRC_A          :
+                          (is_ldh_c_a                            ) ? BUS_SRC_A          :
+                          (is_ldh_n_a                            ) ? BUS_SRC_A          :
+                          (is_ldh_a_n                            ) ? BUS_SRC_MEM        :
+                          (is_alu_a_r    & (reg8_src == REG8_A  )) ? BUS_SRC_A          :
+                          (is_alu_a_r    & (reg8_src == REG8_PHL)) ? BUS_SRC_MEM        :
+                          (is_alu_a_r                            ) ? BUS_SRC_REG8       :
+                          (is_alu_a_n                            ) ? BUS_SRC_OPERAND8_L :
+                          (is_bit_n_r    & (reg8_src == REG8_A  )) ? BUS_SRC_A          :
+                          (is_bit_n_r    & (reg8_src != REG8_A  )) ? BUS_SRC_REG8       :
+                          (is_ld_r_n                             ) ? BUS_SRC_OPERAND8_L :
+                          (is_inc_r                              ) ? BUS_SRC_REG8       :
+                          (is_dec_r                              ) ? BUS_SRC_REG8       :
+                          (is_ld_r_r     & (reg8_src == REG8_A  )) ? BUS_SRC_A          :
+                          (is_ld_r_r     & (reg8_src == REG8_PHL)) ? BUS_SRC_MEM        :
+                          (is_ld_r_r                             ) ? BUS_SRC_REG8       :
+                          (is_ld_a_pbc                           ) ? BUS_SRC_MEM        :
+                          (is_ld_a_pde                           ) ? BUS_SRC_MEM        :
+                          (is_call_nn    & ~is_ex2_i             ) ? BUS_SRC_PC_L       :
+                          (is_call_nn    &  is_ex2_i             ) ? BUS_SRC_PC_H       :
+                          (is_push_rr    & ~is_ex2_i             ) ? BUS_SRC_REG16_L    :
+                          (is_push_rr    &  is_ex2_i             ) ? BUS_SRC_REG16_H    :
+                          (is_pop_rr                             ) ? BUS_SRC_MEM        :
+                          (is_ret                                ) ? BUS_SRC_MEM        :
+                          (is_shift_r    & (reg8_src == REG8_A  )) ? BUS_SRC_A          :
+                          (is_shift_r    & (reg8_src != REG8_A  )) ? BUS_SRC_REG8       :
+                                                                     BUS_SRC_MEM;
+
+assign bus_x_ctrl_o.dst = is_ld_sp_nnnn                            ? BUS_DST_SP_H    :
+                          is_ld_rr_nnnn                            ? BUS_DST_REG16_H :
+                          is_ld_pnnnn_a                            ? BUS_DST_MEM     :
+                          is_ldd_hl_a                              ? BUS_DST_MEM     :
+                          is_ldi_hl_a                              ? BUS_DST_MEM     :
+                          is_ldh_c_a                               ? BUS_DST_MEM     :
+                          is_ldh_n_a                               ? BUS_DST_MEM     :
+                          is_ldh_a_n                               ? BUS_DST_ALU     :
+                          is_alu_a_r                               ? BUS_DST_ALU     :
+                          is_alu_a_n                               ? BUS_DST_ALU     :
+                          is_bit_n_r                               ? BUS_DST_ALU     :
+                          is_inc_r                                 ? BUS_DST_ALU     :
+                          is_dec_r                                 ? BUS_DST_ALU     :
+                          is_ld_a_pbc                              ? BUS_DST_ALU     :
+                          is_ld_a_pde                              ? BUS_DST_ALU     :
+                          is_call_nn                               ? BUS_DST_MEM     :
+                          is_push_rr                               ? BUS_DST_MEM     :
+                          is_pop_rr     &  is_ex2_i                ? BUS_DST_REG16_H :
+                          is_pop_rr     &  is_ex3_i                ? BUS_DST_REG16_L :
+                          is_ret        &  is_ex2_i                ? BUS_DST_PC_H    :
+                          is_ret        &  is_ex3_i                ? BUS_DST_PC_L    :
+                          (is_ld_r_n    & (reg8_dest == REG8_A  )) ? BUS_DST_ALU     :
+                          (is_ld_r_n    & (reg8_dest != REG8_A  )) ? BUS_DST_REG8    :
+                          (is_ld_r_r    & (reg8_dest == REG8_A  )) ? BUS_DST_ALU     :
+                          (is_ld_r_r    & (reg8_dest == REG8_PHL)) ? BUS_DST_MEM     :
+                          (is_ld_r_r                             ) ? BUS_DST_REG8    :
+                          (is_shift_r                            ) ? BUS_DST_ALU     :
+                                                                     BUS_DST_SP_H;
+assign bus_x_ctrl_o.wen =
+  is_ld_sp_nnnn | is_ld_rr_nnnn | is_alu_a_n | is_ldd_hl_a |
+  is_ldi_hl_a | is_bit_n_r | is_ld_r_n | is_ldh_c_a | is_ldh_n_a | is_ldh_a_n |
+  is_call_nn | is_ret | is_push_rr | (is_pop_rr & (is_ex2_i | is_ex3_i)) |
+  (is_ret & (is_ex2_i | is_ex3_i)) | is_shift_r | is_inc_r | is_dec_r | is_ld_pnnnn_a |
+  (is_ld_r_r  & (reg8_src != REG8_PHL))            |
+  (is_ld_r_r  & (reg8_src == REG8_PHL) & is_ex2_i) |
+  (is_alu_a_r & (reg8_src != REG8_PHL))            |
+  (is_alu_a_r & (reg8_src == REG8_PHL) & is_ex2_i);
+
+assign bus_y_ctrl_o.src = is_ld_sp_nnnn             ? BUS_SRC_OPERAND8_L :
+                          is_ld_rr_nnnn             ? BUS_SRC_OPERAND8_L :
+                          is_inc_r                  ? BUS_SRC_ALU        :
+                          is_dec_r                  ? BUS_SRC_ALU        :
+                          is_shift_r                ? BUS_SRC_ALU        :
+                          is_pop_rr     & is_ex2_i  ? BUS_SRC_REG16_L    :
+                          is_pop_rr     & is_ex3_i  ? BUS_SRC_REG16_H    :
+                          is_ret        & is_ex2_i  ? BUS_SRC_PC_L       :
+                          is_ret        & is_ex3_i  ? BUS_SRC_PC_H       :
+                                                      BUS_SRC_MEM;
+
+assign bus_y_ctrl_o.dst = is_ld_sp_nnnn             ? BUS_DST_SP_L    :
+                          is_ld_rr_nnnn             ? BUS_DST_REG16_L :
+                          is_inc_r                  ? BUS_DST_REG8    :
+                          is_dec_r                  ? BUS_DST_REG8    :
+                          is_shift_r                ? BUS_DST_REG8    :
+                          is_pop_rr     & is_ex2_i  ? BUS_DST_REG16_L :
+                          is_pop_rr     & is_ex3_i  ? BUS_DST_REG16_H :
+                          is_ret        & is_ex2_i  ? BUS_DST_PC_L    :
+                          is_ret        & is_ex3_i  ? BUS_DST_PC_H    :
+                                                      BUS_DST_REG8;
+assign bus_y_ctrl_o.wen = is_ld_sp_nnnn | is_ld_rr_nnnn | (is_inc_r & reg8_dest != REG8_A) | (is_shift_r & reg8_dest != REG8_A) | (is_dec_r & reg8_dest != REG8_A) | (is_pop_rr & (is_ex2_i | is_ex3_i)) | (is_ret & (is_ex2_i | is_ex3_i));
+
+assign bus_p_ctrl_o.src = is_ldd_hl_a ? BUS16_SRC_REG16 :
+                          is_ldi_hl_a ? BUS16_SRC_REG16 :
+                          is_call_nn  ? BUS16_SRC_SP    :
+                          is_ret      ? BUS16_SRC_SP    :
+                          is_push_rr  ? BUS16_SRC_SP    :
+                          is_pop_rr   ? BUS16_SRC_SP    :
+                          is_inc_rr & (reg16_sel == REG16_SP_AF) ? BUS16_SRC_SP    :
+                          is_inc_rr & (reg16_sel != REG16_SP_AF) ? BUS16_SRC_REG16 :
+                          is_dec_rr & (reg16_sel == REG16_SP_AF) ? BUS16_SRC_SP    :
+                          is_dec_rr & (reg16_sel != REG16_SP_AF) ? BUS16_SRC_REG16 :
+                                        BUS16_SRC_REG16;
+assign bus_p_ctrl_o.dst = BUS16_DST_ALU16;
+assign bus_p_ctrl_o.wen = is_ldd_hl_a | is_ldi_hl_a | is_call_nn | is_ret | is_push_rr | is_pop_rr | is_inc_rr | is_dec_rr;
+
+assign bus_q_ctrl_o.src = BUS16_SRC_ALU16;
+assign bus_q_ctrl_o.dst = is_ldd_hl_a ? BUS16_DST_REG16 :
+                          is_ldi_hl_a ? BUS16_DST_REG16 :
+                          is_call_nn  ? BUS16_DST_SP    :
+                          is_ret      ? BUS16_DST_SP    :
+                          is_push_rr  ? BUS16_DST_SP    :
+                          is_pop_rr   ? BUS16_DST_SP    :
+                          is_inc_rr & (reg16_sel == REG16_SP_AF) ? BUS16_DST_SP    :
+                          is_inc_rr & (reg16_sel != REG16_SP_AF) ? BUS16_DST_REG16 :
+                          is_dec_rr & (reg16_sel == REG16_SP_AF) ? BUS16_DST_SP    :
+                          is_dec_rr & (reg16_sel != REG16_SP_AF) ? BUS16_DST_REG16 :
+                                        BUS16_DST_REG16;
+assign bus_q_ctrl_o.wen = is_ldd_hl_a | is_ldi_hl_a | is_call_nn | (is_ret & (is_ex1_i | is_ex2_i)) | is_push_rr | (is_pop_rr & (is_ex1_i | is_ex2_i)) | is_inc_rr | is_dec_rr;
+
+assign reg8_src  = is_ldh_c_a ? REG8_C                   :
+                   is_rla     ? REG8_A                   :
+                   is_inc_r   ? reg8_dest                :
+                   is_dec_r   ? reg8_dest                :
+                   is_cb      ? reg8_t'(instr_i[1][2:0]) :
+                                reg8_t'(dec_z);
+
+assign reg8_dest = is_rla     ? REG8_A                   :
+                   is_cb      ? reg8_t'(instr_i[1][2:0]) :
+                                reg8_t'(dec_y);
+
+assign reg16_sel = is_ldd_hl_a                                                     ? REG16_HL :
+                   is_ldi_hl_a                                                     ? REG16_HL :
+                   is_ld_r_r  & ((reg8_src == REG8_PHL) | (reg8_dest == REG8_PHL)) ? REG16_HL :
+                   is_alu_a_r &  (reg8_src == REG8_PHL)                            ? REG16_HL :
+                   is_ld_a_pbc                                                     ? REG16_BC :
+                   is_ld_a_pde                                                     ? REG16_DE :
+                                                                                     reg16_t'(dec_p);
+
+assign reg8_src_o  = reg8_src;
+assign reg8_dest_o = reg8_dest;
+assign reg16_sel_o = reg16_sel;
+
+assign alu_ctrl_o.alu_op_valid =
+   is_alu_a_n                                      |
+  (is_alu_a_r & (reg8_src != REG8_PHL))            |
+  (is_alu_a_r & (reg8_src == REG8_PHL) & is_ex2_i);
+
+assign alu_ctrl_o.alu_op       = alu_op_t'(dec_y);
+
+assign alu_ctrl_o.shift_op_valid = is_shift_r;
+assign alu_ctrl_o.shift_op       = is_rla ? SHIFT_OP_RL :
+                                            shift_op_t'(instr_i[1][5:3]);
+
+assign alu_ctrl_o.bsr_op_valid   = is_bit_n_r;
+assign alu_ctrl_o.bsr_op.b       = is_bit_n_r;
+assign alu_ctrl_o.bsr_op.s       = '0;
+assign alu_ctrl_o.bsr_op.r       = '0;
+assign alu_ctrl_o.bsr_op.operand = instr_i[1][5:3];
+
+assign alu_ctrl_o.incdec_op_valid = is_inc_r | is_dec_r;
+assign alu_ctrl_o.incdec_op.inc   = is_inc_r;
+assign alu_ctrl_o.incdec_op.dec   = is_dec_r;
+assign alu_ctrl_o.incdec_op.dst_a = reg8_dest == REG8_A;
+
+assign alu_ctrl_o.update_a =
+  is_alu_a_n |
+  (is_alu_a_r & (reg8_src != REG8_PHL))            |
+  (is_alu_a_r & (reg8_src == REG8_PHL) & is_ex2_i) |
+  (is_ld_r_n & (reg8_dest == REG8_A)) | (is_ld_r_r & (reg8_dest == REG8_A)) |
+  (is_inc_r  & (reg8_dest == REG8_A)) | (is_dec_r  & (reg8_dest == REG8_A)) |
+  is_ld_a_pbc | is_ld_a_pde | (is_shift_r & (reg8_dest == REG8_A)) |
+  is_ldh_a_n;
+assign alu_ctrl_o.update_f = is_alu_a_n | is_bit_n_r | is_inc_r | is_dec_r | is_shift_r |
+  (is_alu_a_r & (reg8_src != REG8_PHL))            |
+  (is_alu_a_r & (reg8_src == REG8_PHL) & is_ex2_i);
+
+assign alu16_inc_o = is_ldi_hl_a | is_ret     | is_pop_rr  | is_inc_rr;
+assign alu16_dec_o = is_ldd_hl_a | is_call_nn | is_push_rr | is_dec_rr;
+
+assign pc_we_o         = is_call_nn | (is_ret & (is_ex2_i | is_ex3_i)) | is_jr_nn;
+assign pc_we_cc_o      = is_jr_cc_nn;
+assign cc_o            = cc_t'(dec_y[1:0]);
+assign pc_src_o        = is_ret     ? PC_SRC_BUS       :
+                         is_call_nn ? PC_SRC_OPERAND16 :
+                                      PC_SRC_OPERAND8;
+
+assign addr_src_o = is_ldd_hl_a                                                    ? ADDR_SRC_REG16    :
+                    is_ldi_hl_a                                                    ? ADDR_SRC_REG16    :
+                    is_ld_a_pbc                                                    ? ADDR_SRC_REG16    :
+                    is_ld_a_pde                                                    ? ADDR_SRC_REG16    :
+                    is_ldh_c_a                                                     ? ADDR_SRC_REG8     :
+                    is_ldh_n_a                                                     ? ADDR_SRC_OPERAND8 :
+                    is_ldh_a_n                                                     ? ADDR_SRC_OPERAND8 :
+                    is_call_nn                                                     ? ADDR_SRC_SP       :
+                    is_ret    & is_ex3_i                                           ? ADDR_SRC_BUS_P    :
+                    is_ret                                                         ? ADDR_SRC_BUS_Q    :
+                    is_push_rr                                                     ? ADDR_SRC_SP       :
+                    is_pop_rr & is_ex3_i                                           ? ADDR_SRC_BUS_P    :
+                    is_pop_rr                                                      ? ADDR_SRC_BUS_Q    :
+                    is_ld_r_r & ((reg8_src == REG8_PHL) | (reg8_dest == REG8_PHL)) ? ADDR_SRC_REG16    :
+                    is_alu_a_r & (reg8_src == REG8_PHL)                            ? ADDR_SRC_REG16    :
+                    is_ld_pnnnn_a                                                  ? ADDR_SRC_OPERAND16:
+                                                                                     ADDR_SRC_PC;
+
+assign instr_undef_o = ~(
+  is_ld_sp_nnnn | is_alu_a_r | is_alu_a_n | is_ld_rr_nnnn | is_ldd_hl_a | is_ldi_hl_a |
+  is_bit_n_r | is_jr_nn | is_jr_cc_nn | is_ld_r_n | is_ldh_c_a | is_inc_r | is_dec_r | is_ld_r_r |
+  is_ldh_n_a | is_ldh_a_n | is_ld_a_pbc | is_ld_a_pde | is_call_nn | is_push_rr | is_pop_rr |
+  (is_shift_r & (instr_i[1][5:3] == 3'h02)) | is_rla | is_inc_rr | is_dec_rr | is_ret |
+  is_ld_pnnnn_a);
+
+endmodule : idec
diff --git a/rtl/cpu/registers.sv b/rtl/cpu/regbank.sv
similarity index 68%
rename from rtl/cpu/registers.sv
rename to rtl/cpu/regbank.sv
index fff95c4..3de6da7 100644
--- a/rtl/cpu/registers.sv
+++ b/rtl/cpu/regbank.sv
@@ -2,9 +2,9 @@
 
 import cpu_pkg::*;
 
-module registers (
-    input  logic clk_i,
-    input  logic nreset_i,
+module regbank (
+    input  logic clk,
+    input  logic nreset,
 
     input  reg8_t reg8_wselect_i,
     input  logic  reg8_we_i,
@@ -29,15 +29,15 @@ module registers (
     logic [ 1:0] reg_we  [0:2];
 
     generate for (genvar i = 0; i <= 2; i++) begin : gen_regs
-        always_ff @(posedge clk_i or negedge nreset_i) begin
-            if (~nreset_i) begin
+        always_ff @(posedge clk or negedge nreset) begin
+            if (!nreset) begin
                 reg_r[i][7:0] <= '0;
             end else if (reg_we[i][0]) begin
                 reg_r[i][7:0] <= reg_next[7:0];
             end
         end
-        always_ff @(posedge clk_i or negedge nreset_i) begin
-            if (~nreset_i) begin
+        always_ff @(posedge clk or negedge nreset) begin
+            if (!nreset) begin
                 reg_r[i][15:8] <= '0;
             end else if (reg_we[i][1]) begin
                 reg_r[i][15:8] <= reg_next[15:8];
@@ -45,19 +45,38 @@ module registers (
         end
 
         assign reg_we[i][0] = (reg16_we_i & (reg16_wselect_i == i[1:0])) |
-                              (reg8_we_i  & (reg8_wselect_i  == {i[1:0], 1'b0}));
-        assign reg_we[i][1] = (reg16_we_i & (reg16_wselect_i == i[1:0])) |
                               (reg8_we_i  & (reg8_wselect_i  == {i[1:0], 1'b1}));
+        assign reg_we[i][1] = (reg16_we_i & (reg16_wselect_i == i[1:0])) |
+                              (reg8_we_i  & (reg8_wselect_i  == {i[1:0], 1'b0}));
     end endgenerate
 
     assign reg_next = ({16{reg8_we_i}}  & {reg8_wdata_i, reg8_wdata_i}) |
                       ({16{reg16_we_i}} & reg16_wdata_i);
 
-    assign reg8_rdata_o  = reg8_rselect_i[0] ? reg_r[reg8_rselect_i[2:1]][15:8] :
-                                               reg_r[reg8_rselect_i[2:1]][ 7:0];
+    assign reg8_rdata_o  = reg8_rselect_i[0] ? reg_r[reg8_rselect_i[2:1]][ 7:0] :
+                                               reg_r[reg8_rselect_i[2:1]][15:8];
     assign reg16_rdata_o  = reg_r[reg16_rselect_i];
     assign reg16_rdata2_o = reg_r[reg16_rselect2_i];
 
     assign hl_o = reg_r[REG16_HL];
 
-endmodule : registers
+`include "sva_common.svh"
+`SVA_DEF_CLK(clk)
+`SVA_DEF_NRESET(nreset)
+
+`SVA_ASSERT_PROP_FATAL(regbank_write_reg_a,
+  reg8_we_i |-> (reg8_wselect_i != REG8_A),
+  "Register bank received write to register A"
+)
+
+`SVA_ASSERT_PROP_FATAL(regbank_write_reg_phl,
+  reg8_we_i |-> (reg8_wselect_i != REG8_PHL),
+  "Register bank received write to register (HL)"
+)
+
+`SVA_ASSERT_PROP_FATAL(regbank_write_reg_sp_af,
+  reg16_we_i |-> (reg16_wselect_i != REG16_SP_AF),
+  "Register bank received write to register SP/AF"
+)
+
+endmodule : regbank
diff --git a/rtl/gb.sv b/rtl/gb.sv
index ef34564..76f2bcb 100644
--- a/rtl/gb.sv
+++ b/rtl/gb.sv
@@ -1,54 +1,105 @@
+// Top level gb module
 module gb (
-    input  logic clk_i,
-    input  logic nreset_i
+  input  logic clk,
+  input  logic nreset,
+
+  // Cartridge bus
+  output  logic        cart_clk_o,
+  output  logic        cart_nreset_o,
+  output  logic        cart_nrd_o,
+  output  logic        cart_nwr_o,
+  output  logic        cart_ncs_o,
+  output  logic [15:0] cart_addr_o,
+  input   logic [ 7:0] cart_data_i
+
 );
 
-    logic        we;
-    logic [15:0] address;
-    logic [ 7:0] rdata;
-    logic [ 7:0] wdata;
+logic [15:0] cpu_addr;
+logic [ 7:0] cpu_rdata;
+logic        cpu_we;
+logic [ 7:0] cpu_wdata;
 
-    logic        rom_cs;
-    logic [ 7:0] rom_rdata;
+logic [ 7:0] cpu_ppu_rdata;
 
-    logic        hi_ram_cs;
-    logic [ 7:0] hi_ram_rdata;
+logic        rom_enable_r;
+logic        rom_sel;
+logic [ 7:0] rom_rdata;
 
-    cpu cpu_inst (
-        .clk_i    (clk_i),
-        .nreset_i (nreset_i),
-        .address_o(address),
-        .rdata_i  (rdata),
-        .we_o     (we),
-        .wdata_o  (wdata)
-    );
+logic        hiram_sel;
+logic        hiram_we;
+logic [ 7:0] hiram_rdata;
 
-    rom #(
-        .FILE_NAME("DMG_ROM.bin"),
-        .ADDR_W   (8),
-        .DATA_W   (8)
-    ) rom_inst (
-        .clk_i    (clk_i),
-        .cs_i     (rom_cs),
-        .address_i(address[7:0]),
-        .rdata_o  (rom_rdata)
-    );
+logic        vram_sel;
+logic        vram_we;
+logic [ 7:0] vram_rdata;
 
-    ram #(
-        .ADDR_W (7),
-        .DATA_W (8)
-    ) hi_ram_inst (
-        .clk_i     (clk_i),
-        .cs_i      (hi_ram_cs),
-        .address_i (address[6:0]),
-        .rdata_o   (hi_ram_rdata),
-        .we_i      (we),
-        .wdata_i   (wdata)
-    );
+cpu cpu_inst (
+  .clk    (clk),
+  .nreset (nreset),
 
-    assign rom_cs    = ~(|address[15:8]);
-    assign hi_ram_cs = (&address[15:7]) & ~(&address[6:0]);
+  .addr_o (cpu_addr),
+  .rdata_i(cpu_rdata),
+  .we_o   (cpu_we),
+  .wdata_o(cpu_wdata)
+);
 
-    assign rdata = rom_rdata | hi_ram_rdata;
+ppu ppu_inst (
+  .clk    (clk),
+  .nreset (nreset),
 
-endmodule : gb
+  .cpu_addr_i (cpu_addr),
+  .cpu_rdata_o(cpu_ppu_rdata),
+  .cpu_we_i   (cpu_we),
+  .cpu_wdata_i(cpu_wdata),
+  .cpu_addr_sel_o(ppu_sel)
+);
+
+assign rom_enable_r = '1;
+
+assign rom_sel   = rom_enable_r & ~(|cpu_addr[15:8]);
+assign vram_sel  = (cpu_addr[15:13] == 3'b100);
+assign hiram_sel = (&cpu_addr[15:7]) & ~(&cpu_addr[6:0]);
+
+assign vram_we  = vram_sel  & cpu_we;
+assign hiram_we = hiram_sel & cpu_we;
+
+assign cpu_rdata = ({8{  rom_sel}} & rom_rdata)  |
+                   ({8{ vram_sel}} & vram_rdata) |
+                   ({8{hiram_sel}} & hiram_rdata)|
+                   ({8{  ppu_sel}} & cpu_ppu_rdata);
+
+rom #(
+  .FILE_NAME("DMG_ROM.bin"),
+  .ADDR_W   (8),
+  .DATA_W   (8)
+) rom_inst (
+  .clk      (clk),
+  .address_i(cpu_addr[7:0]),
+  .rdata_o  (rom_rdata)
+);
+
+ram #(
+  .ADDR_W   (13),
+  .DATA_W   (8)
+) vram_inst (
+  .clk       (clk),
+  .address_i (cpu_addr[12:0]),
+  .rdata_o   (vram_rdata),
+  .we_i      (vram_we),
+  .wdata_i   (cpu_wdata)
+);
+
+ram #(
+  .ADDR_W (7),
+  .DATA_W (8)
+) hiram_inst (
+  .clk       (clk),
+  .address_i (cpu_addr[6:0]),
+  .rdata_o   (hiram_rdata),
+  .we_i      (hiram_we),
+  .wdata_i   (cpu_wdata)
+);
+
+
+
+endmodule : gb
\ No newline at end of file
diff --git a/rtl/ppu.sv b/rtl/ppu.sv
new file mode 100644
index 0000000..bfa6303
--- /dev/null
+++ b/rtl/ppu.sv
@@ -0,0 +1,30 @@
+module ppu (
+  input  logic clk,
+  input  logic nreset,
+
+  input  logic [15:0] cpu_addr_i,
+  output logic [ 7:0] cpu_rdata_o,
+  input  logic        cpu_we_i,
+  input  logic [ 7:0] cpu_wdata_i,
+
+  output logic        cpu_addr_sel_o
+);
+
+logic ly_sel;
+
+logic ly_r;
+logic ly_we;
+
+assign ly_sel = (cpu_addr_i == 16'hFF44);
+assign ly_we  = ly_sel & cpu_we_i;
+
+always_ff @(posedge clk or negedge nreset) begin
+  if (!nreset)
+    ly_r <= 8'h90; // vsync hack
+  else if (ly_we)
+    ly_r <= 8'h90;
+end
+
+assign cpu_addr_sel_o = ly_sel;
+
+endmodule : ppu
diff --git a/rtl/ram.sv b/rtl/ram.sv
new file mode 100644
index 0000000..1a97f44
--- /dev/null
+++ b/rtl/ram.sv
@@ -0,0 +1,28 @@
+module ram #(
+  parameter DATA_W = 8,
+  parameter ADDR_W = 8
+)(
+  input  logic              clk,
+
+  input  logic [ADDR_W-1:0] address_i,
+  input  logic              we_i,
+  input  logic [DATA_W-1:0] wdata_i,
+  output logic [DATA_W-1:0] rdata_o
+);
+
+localparam RAM_SIZE = 2**ADDR_W;
+
+logic [DATA_W-1:0] ram [RAM_SIZE-1:0];
+
+logic [DATA_W-1:0] rdata;
+
+always_ff @(posedge clk)
+  if (we_i)
+    ram[address_i] <= wdata_i;
+
+always_ff @(posedge clk)
+  rdata <= ram[address_i];
+
+assign rdata_o = rdata;
+
+endmodule
diff --git a/rtl/rom.sv b/rtl/rom.sv
new file mode 100644
index 0000000..4ee3cae
--- /dev/null
+++ b/rtl/rom.sv
@@ -0,0 +1,26 @@
+module rom #(
+  parameter string           FILE_NAME = "",
+  parameter integer unsigned ADDR_W = 8,
+  parameter integer unsigned DATA_W = 8
+) (
+  input  logic              clk,
+  input  logic [ADDR_W-1:0] address_i,
+  output logic [DATA_W-1:0] rdata_o
+);
+
+localparam ROM_SIZE = 2**ADDR_W;
+
+logic [DATA_W-1:0] rom [ROM_SIZE-1:0];
+logic [DATA_W-1:0] rdata;
+
+always_ff @(posedge clk)
+  rdata <= rom[address_i];
+
+assign rdata_o = rdata;
+
+initial begin
+    static integer fd = $fopen(FILE_NAME, "rb");
+    static integer rv = $fread(rom, fd);
+end
+
+endmodule : rom
diff --git a/rtl/shared/ram.sv b/rtl/shared/ram.sv
deleted file mode 100644
index a105c69..0000000
--- a/rtl/shared/ram.sv
+++ /dev/null
@@ -1,34 +0,0 @@
-module ram #(
-    parameter DATA_W = 8,
-    parameter ADDR_W = 8
-)(
-    input  logic              clk_i,
-
-    input  logic              cs_i,
-
-    input  logic [ADDR_W-1:0] address_i,
-    input  logic              we_i,
-    input  logic [DATA_W-1:0] wdata_i,
-    output logic [DATA_W-1:0] rdata_o
-);
-
-localparam RAM_SIZE = 2**ADDR_W;
-
-logic [DATA_W-1:0] ram [RAM_SIZE-1:0];
-
-logic              wenable;
-logic [DATA_W-1:0] rdata;
-
-assign wenable = cs_i & we_i;
-
-always_ff @(posedge clk_i)
-    if (wenable)
-        ram[address_i] <= wdata_i;
-
-always_ff @(posedge clk_i)
-    if (cs_i)
-        rdata <= ram[address_i];
-
-assign rdata_o = {DATA_W{cs_i}} & rdata;
-
-endmodule
diff --git a/rtl/shared/rom.sv b/rtl/shared/rom.sv
deleted file mode 100644
index 2a9b460..0000000
--- a/rtl/shared/rom.sv
+++ /dev/null
@@ -1,29 +0,0 @@
-module rom #(
-    parameter string           FILE_NAME = "",
-    parameter integer unsigned ADDR_W = 8,
-    parameter integer unsigned DATA_W = 8
-) (
-    input  logic              clk_i,
-    input  logic              cs_i,
-    input  logic [ADDR_W-1:0] address_i,
-
-    output logic [DATA_W-1:0] rdata_o
-);
-
-localparam ROM_SIZE = 2**ADDR_W;
-
-logic [DATA_W-1:0] rom [ROM_SIZE-1:0];
-logic [DATA_W-1:0] rdata;
-
-always_ff @(posedge clk_i)
-    if (cs_i)
-        rdata <= rom[address_i];
-
-assign rdata_o = {DATA_W{cs_i}} & rdata;
-
-initial begin
-    static integer fd = $fopen(FILE_NAME, "rb");
-    static integer rv = $fread(rom, fd);
-end
-
-endmodule : rom
diff --git a/rtl/sva_common.svh b/rtl/sva_common.svh
new file mode 100644
index 0000000..3bf180d
--- /dev/null
+++ b/rtl/sva_common.svh
@@ -0,0 +1,39 @@
+`ifndef SVA_COMMON_SVH
+`define SVA_COMMON_SVH
+
+`ifdef SVA_ENABLE
+`define SVA_DEF_CLK(clk) \
+  logic  sva_clk; \
+  assign sva_clk = clk;
+
+`define SVA_DEF_NRESET(nrst) \
+  logic  sva_nreset; \
+  assign sva_nreset = nrst;
+
+`define SVA_ASSERT_PROP(label, prop, err) \
+  property label``_prop; \
+    @(posedge sva_clk) disable iff (!nreset) \
+    (prop); \
+  endproperty \
+  label``_sva: assert property (label``_prop) else $display($sformatf("[%0t] %s", $time, err));
+
+`define SVA_ASSERT_PROP_FATAL(label, prop, err) \
+  property label``_prop; \
+    @(posedge sva_clk) disable iff (!nreset) \
+    (prop); \
+  endproperty \
+  label``_sva: assert property (label``_prop) else begin \
+    $display($sformatf("[%0t] %s", $time, err)); \
+    $fatal(); \
+  end
+
+`else /* SVA_ENABLE */
+
+`define SVA_DEF_CLK(clk)
+`define SVA_DEF_NRESET(nrst)
+`define SVA_ASSERT_PROP(label, prop, err)
+`define SVA_ASSERT_PROP_FATAL(label, prop, err)
+
+`endif /* SVA_ENABLE */
+
+`endif /* SVA_COMMON_SVH */
diff --git a/sim/clkgen.sv b/sim/clkgen.sv
new file mode 100644
index 0000000..ff3acfe
--- /dev/null
+++ b/sim/clkgen.sv
@@ -0,0 +1,18 @@
+module clkgen #(
+PERIOD_NS      = 10,
+RESET_DELAY_NS = 45
+) (
+  output logic clk,
+  output logic nreset
+);
+
+initial begin
+  clk    <= 1'b1;
+  nreset <= 1'b0;
+  #RESET_DELAY_NS nreset <= 1'b1;
+end
+
+always
+  #(PERIOD_NS/2) clk <= ~clk;
+
+endmodule
diff --git a/sim/shared/clkgen.sv b/sim/shared/clkgen.sv
deleted file mode 100644
index 851d10d..0000000
--- a/sim/shared/clkgen.sv
+++ /dev/null
@@ -1,18 +0,0 @@
-module clkgen #(
-    PERIOD_NS      = 10,
-    RESET_DELAY_NS = 100
-) (
-    output logic clk_o,
-    output logic nreset_o
-);
-
-    initial begin
-        clk_o    <= 1'b1;
-        nreset_o <= 1'b0;
-        #RESET_DELAY_NS nreset_o <= 1'b1;
-    end
-
-    always
-        #(PERIOD_NS/2) clk_o <= ~clk_o;
-
-endmodule
diff --git a/sim/tb_top.sv b/sim/tb_top.sv
new file mode 100644
index 0000000..88b0ef9
--- /dev/null
+++ b/sim/tb_top.sv
@@ -0,0 +1,76 @@
+module tb_top;
+
+logic clk;
+logic nreset;
+
+clkgen clkgen_inst (
+  .clk   (clk),
+  .nreset(nreset)
+);
+
+gb gb_inst (
+  .clk   (clk),
+  .nreset(nreset)
+);
+
+logic        instr_valid;
+logic        instr_undef;
+logic        halted;
+logic [15:0] current_pc;
+logic [ 7:0] current_opcode[2:0];
+
+logic        we;
+logic [15:0] last_write_address;
+logic [ 7:0] last_write_value;
+
+logic        vram_sel;
+logic        hiram_sel;
+
+assign halted      = gb_inst.cpu_inst.ctrl_inst.halted_r;
+assign instr_valid = gb_inst.cpu_inst.instr_valid;
+assign instr_undef = gb_inst.cpu_inst.ctrl_inst.instr_undef;
+assign current_opcode = gb_inst.cpu_inst.instr;
+
+assign we      = gb_inst.cpu_we;
+
+assign vram_sel  = gb_inst.vram_sel;
+assign hiram_sel = gb_inst.hiram_sel;
+
+always_ff @(posedge clk or negedge nreset) begin
+  if (!nreset)
+    current_pc <= '0;
+  else if (instr_valid)
+    current_pc <= gb_inst.cpu_inst.pc_r;
+end
+
+always_ff @(posedge clk or negedge nreset) begin
+  if (!nreset) begin
+    last_write_address <= '0;
+    last_write_value   <= '0;
+  end else if (we) begin
+    last_write_address <= gb_inst.cpu_addr;
+    last_write_value   <= gb_inst.cpu_wdata;
+  end
+end
+
+
+// SVA code here
+`include "sva_common.svh"
+`SVA_DEF_CLK(clk);
+`SVA_DEF_NRESET(nreset);
+
+`SVA_ASSERT_PROP_FATAL(undefined_opcode_pushed,
+  halted |-> !instr_undef,
+  $sformatf("PC: 0x%X | Undefined opcode pushed: 0x%X (0x%X, 0x%X)", current_pc, current_opcode[0], current_opcode[1], current_opcode[2])
+);
+
+logic selected_memory_implemented;
+assign selected_memory_implemented = hiram_sel | vram_sel;
+
+`SVA_ASSERT_PROP(write_to_unimplemented_memory,
+  we |-> selected_memory_implemented,
+  $sformatf("PC: 0x%X | Write to unimplemented memory: 0x%X <= 0x%X",
+    current_pc, last_write_address, last_write_value)
+);
+
+endmodule : tb_top
diff --git a/sim/tbench/tb_top.sv b/sim/tbench/tb_top.sv
deleted file mode 100644
index fc82f24..0000000
--- a/sim/tbench/tb_top.sv
+++ /dev/null
@@ -1,50 +0,0 @@
-module tb_top ();
-
-    logic clk;
-    logic nreset;
-
-    clkgen clkgen_inst (
-        .clk_o   (clk),
-        .nreset_o(nreset)
-    );
-
-    gb gb_inst (
-        .clk_i   (clk),
-        .nreset_i(nreset)
-    );
-
-    // Testbench code
-    logic        instr_undef;
-    logic        halted;
-    logic [15:0] last_pc;
-    logic [ 7:0] last_opcode;
-    logic [ 7:0] last_op1;
-    logic [ 7:0] last_op2;
-
-    logic        we;
-    logic [15:0] address;
-    logic [ 7:0] wdata;
-
-    assign halted      = gb_inst.cpu_inst.control_inst.halted_r;
-    assign instr_undef = gb_inst.cpu_inst.control_inst.is_undef;
-    assign last_pc     = gb_inst.cpu_inst.control_inst.instr_pc_r;
-    assign last_opcode = gb_inst.cpu_inst.control_inst.instr_r;
-    assign last_op1    = gb_inst.cpu_inst.control_inst.operand0_r;
-    assign last_op2    = gb_inst.cpu_inst.control_inst.operand1_r;
-
-    assign we      = gb_inst.we;
-    assign address = gb_inst.address;
-    assign wdata   = gb_inst.wdata;
-
-    always @(posedge clk) begin
-        if (nreset && instr_undef & ~halted) begin
-            $display($sformatf("[%0t] %X: Undefined opcode %X | [%X | %X]", $time(), last_pc, last_opcode, last_op1, last_op2));
-            $fatal(0);
-            $finish();
-        end
-        if (nreset && we) begin
-            $display($sformatf("[%0t] Write: [%X] <= %X", $time(), address, wdata));
-        end
-    end
-
-endmodule : tb_top
diff --git a/synthflow b/synthflow
index 3f8450a..1486728 160000
--- a/synthflow
+++ b/synthflow
@@ -1 +1 @@
-Subproject commit 3f8450ac6a619226060b09f22ac5418537b40d70
+Subproject commit 1486728ced51acd63f20caf41cc47b44f2efa436