diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index eee110b..e34f157 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -162,4 +162,16 @@ rtc_code sim:
     - cd hw/efinix_fpga/simulation
     - make clean
     - TEST_PROGRAM_NAME=rtc_test make rtc_code_tb
-    - ./rtc_code_tb
\ No newline at end of file
+    - ./rtc_code_tb
+
+devices_setup_code sim:
+  tags:
+    - linux
+    - iverilog
+  stage: simulate
+  script:
+    - source init_env.sh
+    - cd hw/efinix_fpga/simulation
+    - make clean
+    - TEST_PROGRAM_NAME=devices_setup_test make devices_setup_code_tb  
+    - ./devices_setup_code_tb
\ No newline at end of file
diff --git a/hw/efinix_fpga/simulation/Makefile b/hw/efinix_fpga/simulation/Makefile
index 3f76c94..b2d283c 100644
--- a/hw/efinix_fpga/simulation/Makefile
+++ b/hw/efinix_fpga/simulation/Makefile
@@ -10,7 +10,8 @@ TEST_FOLDER?=$(REPO_TOP)/sw/test_code/$(TEST_PROGRAM_NAME)
 TEST_PROGRAM?=$(REPO_TOP)/sw/test_code/$(TEST_PROGRAM_NAME)/$(TEST_PROGRAM_NAME).hex
 
 STANDALONE_TB= interrupt_controller_tb mapper_tb rtc_tb
-CODE_TB= interrupt_controller_code_tb mapper_code_tb rtc_code_tb
+CODE_TB= interrupt_controller_code_tb mapper_code_tb rtc_code_tb \
+		devices_setup_code_tb
 
 #TODO implement something like sources.list
 
diff --git a/hw/efinix_fpga/simulation/tbs/devices_setup_code_tb.sv b/hw/efinix_fpga/simulation/tbs/devices_setup_code_tb.sv
new file mode 100644
index 0000000..bceafe1
--- /dev/null
+++ b/hw/efinix_fpga/simulation/tbs/devices_setup_code_tb.sv
@@ -0,0 +1,27 @@
+`timescale 1ns/1ps
+
+module devices_setup_code_tb();
+
+sim_top u_sim_top();
+
+always begin
+    if (
+        u_sim_top.w_cpu_addr == 16'h0 &&
+        u_sim_top.w_cpu_we == '1
+    ) begin
+        if (u_sim_top.w_cpu_data_from_cpu == 8'h6d) begin
+            $display("Good finish!");
+            $finish();
+        end else begin
+            $display("Bad finish!");
+            $finish_and_return(-1);
+        end
+    end
+    # 1;
+end
+
+always @(u_sim_top.u_dut.u_rtc.r_output) begin
+    $display("counter: %d", u_sim_top.u_dut.u_rtc.r_output);
+end
+
+endmodule
\ No newline at end of file
diff --git a/sw/kernel/devices/interrupt_controller.s b/sw/kernel/devices/interrupt_controller.s
index b7cf594..4c45167 100644
--- a/sw/kernel/devices/interrupt_controller.s
+++ b/sw/kernel/devices/interrupt_controller.s
@@ -64,7 +64,7 @@ loop:
     lda IRQ_DAT_ADDR
     sta tmp1
     pla
-    and $07     ; A is now 0-7
+    and #$07     ; A is now 0-7
     tax
     inx         ; X is now 1-8
     lda #$01
diff --git a/sw/kernel/devices/rtc.s b/sw/kernel/devices/rtc.s
index e0778b3..a9e3df7 100644
--- a/sw/kernel/devices/rtc.s
+++ b/sw/kernel/devices/rtc.s
@@ -67,6 +67,12 @@ RTC_CONTROL         = $30
     sta RTC_CMD
     lda #$00
     sta RTC_DAT
+
+    lda #$30
+    sta RTC_CMD
+    lda #$3
+    sta RTC_DAT
+    
     rts
 
 .endproc
diff --git a/sw/test_code/devices_setup_test/Makefile b/sw/test_code/devices_setup_test/Makefile
new file mode 100644
index 0000000..2af39c8
--- /dev/null
+++ b/sw/test_code/devices_setup_test/Makefile
@@ -0,0 +1,54 @@
+CC=../../cc65/bin/cl65
+LD=../../cc65/bin/cl65
+CFLAGS=-T -t none -I. --cpu "65C02"
+LDFLAGS=-C link.ld -m $(NAME).map
+
+NAME=devices_setup_test
+
+DEVICES=../../kernel/devices
+
+BIN=$(NAME).bin
+HEX=$(NAME).hex
+
+LISTS=lists
+
+KERNEL_SRCS=rtc.s interrupt_controller.s
+KERNEL_OBJS=$(patsubst %.s,%.o,$(filter %s,$(KERNEL_SRCS)))
+
+SRCS=$(wildcard *.s) $(wildcard *.c) 
+SRCS+=$(wildcard **/*.s) $(wildcard **/*.c)
+OBJS+=$(patsubst %.s,%.o,$(filter %s,$(SRCS)))
+OBJS+=$(patsubst %.c,%.o,$(filter %c,$(SRCS)))
+
+# Make sure the kernel linked to correct address, no relocation!
+all: $(KERNEL_SRCS) $(HEX)
+	rm rtc.s
+	rm interrupt_controller.s
+
+$(HEX): $(BIN)
+	objcopy --input-target=binary --output-target=verilog $(BIN) $(HEX)
+
+$(BIN): $(OBJS) $(KERNEL_OBJS)
+	$(CC) $(CFLAGS) $(LDFLAGS) $(OBJS) $(KERNEL_OBJS) -o $@
+
+%.o: %.c $(LISTS)
+	$(CC) $(CFLAGS) -l $(LISTS)/$<.list -c $< -o $@
+
+%.o: %.s $(LISTS)
+	$(CC) $(CFLAGS) -l $(LISTS)/$<.list -c $< -o $@
+
+$(LISTS):
+	mkdir -p $(addprefix $(LISTS)/,$(sort $(dir $(SRCS))))
+
+rtc.s: $(DEVICES)/rtc.s
+	cp $(DEVICES)/rtc.s .
+
+interrupt_controller.s: $(DEVICES)/interrupt_controller.s
+	cp $(DEVICES)/interrupt_controller.s .
+
+.PHONY: clean
+clean:
+	rm -rf $(OBJS) $(BIN) $(HEX) $(LISTS) $(NAME).map
+	rm -rf $(KERNEL_SRCS)
+	rm -rf $(KERNEL_OBJS)
+
diff --git a/sw/test_code/devices_setup_test/link.ld b/sw/test_code/devices_setup_test/link.ld
new file mode 100644
index 0000000..66a42fe
--- /dev/null
+++ b/sw/test_code/devices_setup_test/link.ld
@@ -0,0 +1,35 @@
+MEMORY
+{
+  ZP:  start = $0,    size = $100,  type = rw, define = yes;
+  SDRAM: start = $9200, size = $4d00, type = rw, define = yes;
+  ROM: start = $F000, size = $1000, file = %O;
+}
+
+SEGMENTS {
+    ZEROPAGE: load = ZP,  type = zp,  define   = yes;
+    DATA:     load = ROM, type = rw,  define   = yes, run = SDRAM;
+    BSS:      load = SDRAM, type = bss, define   = yes;
+    HEAP:     load = SDRAM, type = bss, optional = yes;
+    STARTUP:  load = ROM, type = ro;
+    ONCE:     load = ROM, type = ro,  optional = yes;
+    CODE:     load = ROM, type = ro;
+    RODATA:   load = ROM, type = ro;
+    VECTORS:  load = ROM, type = ro,  start    = $FFFA;
+}
+
+FEATURES {
+    CONDES:    segment = STARTUP,
+               type    = constructor,
+               label   = __CONSTRUCTOR_TABLE__,
+               count   = __CONSTRUCTOR_COUNT__;
+    CONDES:    segment = STARTUP,
+               type    = destructor,
+               label   = __DESTRUCTOR_TABLE__,
+               count   = __DESTRUCTOR_COUNT__;
+}
+
+SYMBOLS {
+    # Define the stack size for the application
+    __STACKSIZE__:  value = $0200, type = weak;
+    __STACKSTART__: type = weak, value = $0800; # 2k stack
+}
diff --git a/sw/test_code/devices_setup_test/main.s b/sw/test_code/devices_setup_test/main.s
new file mode 100644
index 0000000..a380d19
--- /dev/null
+++ b/sw/test_code/devices_setup_test/main.s
@@ -0,0 +1,39 @@
+.export _init, _nmi_int, _irq_int
+
+.autoimport
+
+.import _init_interrupt_controller
+.import _init_rtc
+
+.zeropage
+
+finish: .res 1
+
+.code
+
+_nmi_int:
+_irq_int:
+    lda #$6d
+    sta $00
+
+_init:
+    ldx #$ff
+    txs
+
+    LDA     #<(__STACKSTART__ + __STACKSIZE__)
+    STA     sp
+    LDA     #>(__STACKSTART__ + __STACKSIZE__)
+    STA     sp+1
+
+    jsr _init_interrupt_controller
+    jsr _init_rtc
+
+    ; enable interrupt 0
+    lda #$00
+    jsr pusha
+    lda #$0
+    jsr _enable_irq
+
+    cli
+
+@end:   bra @end
\ No newline at end of file
diff --git a/sw/test_code/devices_setup_test/vectors.s b/sw/test_code/devices_setup_test/vectors.s
new file mode 100644
index 0000000..81ae6e0
--- /dev/null
+++ b/sw/test_code/devices_setup_test/vectors.s
@@ -0,0 +1,14 @@
+; ---------------------------------------------------------------------------
+; vectors.s
+; ---------------------------------------------------------------------------
+;
+; Defines the interrupt vector table.
+
+.import    _init
+.import    _nmi_int, _irq_int
+
+.segment  "VECTORS"
+
+.addr      _nmi_int    ; NMI vector
+.addr      _init       ; Reset vector
+.addr      _irq_int    ; IRQ/BRK vector
\ No newline at end of file