diff --git a/rtl/Makefile b/rtl/Makefile
index 38831be8..e7ff0673 100644
--- a/rtl/Makefile
+++ b/rtl/Makefile
@@ -7,14 +7,15 @@ WAVE := NONE
 RTL_IP := ./ip/spram_model.v \
       ./ip/spimemio.v \
       ./ip/simpleuart.v \
-	  ./ip/i2c_master_bit_ctrl.v \
+      ./ip/i2c_master_bit_ctrl.v \
       ./ip/i2c_master_byte_ctrl.v \
       ./ip/simple_i2c_master.v \
+      ./ip/i2c_slave.v \
       ./ip/simple_spi_master.v \
       ./ip/counter_timer.v \
-	  ./ip/spi_slave.v \
+      ./ip/spi_slave.v \
       ./ip/ravenna_spi.v \
-	  ./ip/spiflash.v
+      ./ip/spiflash.v
 
 RTL_TECH := ../tech/tc_io.v \
             ../tech/tc_clk.v \
diff --git a/rtl/firmware.c b/rtl/firmware.c
index 3bbe06c9..971b456f 100644
--- a/rtl/firmware.c
+++ b/rtl/firmware.c
@@ -571,20 +571,32 @@ void cmd_echo()
         putchar(c);
 }
 
+void print_reg(char *info, uint32_t val, int digits)
+{
+    print(info);
+    print_hex(val, digits);
+    print("\n");
+}
+
 void ip_counter_timer_test()
 {
     print("[IP] counter timer test\n");
-    reg_timer_value = 0xffffffff;
+    print_reg("[TIM VALUE] ", reg_timer_value, 8);
+    print_reg("[TIM CONFIG] ", reg_timer_config, 8);
+
+    reg_timer_value = (uint32_t)0xffffffff;
     // reg_timer_config = 1;
     // reg_timer_config = 2;
     // reg_timer_config = 4; // One-shot mode, where the counter triggers an interrupt when it reaches the value of reg_timer_data
     reg_timer_config = 8; // 1000, timer enable, continuous mode, count down, irq disable
     // reg_timer_config = 15;
+
+    print_reg("[TIM VALUE] ", reg_timer_value, 8);
+    print_reg("[TIM CONFIG] ", reg_timer_config, 8);
+
     for (int i = 0; i < 10; ++i)
     {
-        print("[TIM] ");
-        print_hex(reg_timer_data, 7);
-        print("\n");
+        print_reg("[TIM DATA] ", reg_timer_data, 8);
     }
 }
 
@@ -596,16 +608,56 @@ void ip_gpio_test()
 
     reg_gpio_data = 0xffff;
     reg_gpio_data = 0x0000;
-    for(int i = 0; i < 10; ++i) {
+    for (int i = 0; i < 10; ++i)
+    {
         print("[GPIO] ");
         print_hex(reg_gpio_data, 7);
         print("\n");
     }
 }
 
+void i2c_init(unsigned int pre)
+{
+    reg_i2c_control = (uint16_t)(I2C_CTRL_EN | I2C_CTRL_IEN);
+    reg_i2c_prescale = (uint16_t)pre;
+}
+
+int i2c_send(unsigned char saddr, unsigned char sdata)
+{
+    int volatile y;
+    reg_i2c_data = saddr;
+    reg_i2c_command = I2C_CMD_STA | I2C_CMD_WR;
+
+    while ((reg_i2c_status & I2C_STAT_TIP) != 0)
+        ;
+
+    if ((reg_i2c_status & I2C_STAT_RXACK) == 1)
+    {
+        reg_i2c_command = I2C_CMD_STO;
+        return 0;
+    }
+
+    reg_i2c_data = sdata;
+    reg_i2c_command = I2C_CMD_WR;
+
+    while (reg_i2c_status & I2C_STAT_TIP)
+        ;
+    reg_i2c_command = I2C_CMD_STO;
+
+    if ((reg_i2c_status & I2C_STAT_RXACK) == 1)
+        return 0;
+    else
+        return 1;
+}
+
 void ip_i2c_test()
 {
     print("[IP] i2c test\n");
+    reg_i2c_config = 0;
+    reg_i2c_data = 0;
+    i2c_init(5);
+    // Send command 6, data byte 0xfa
+    i2c_send(0x6, 0xfa);
 }
 
 void main()
@@ -636,11 +688,16 @@ void main()
     // cmd_read_flash_id();
     // cmd_read_flash_regs();
     cmd_print_spi_state();
-    // cmd_echo();
-    ip_counter_timer_test();
-    ip_gpio_test();
 
-    while (1);
+    ip_counter_timer_test();
+    // ip_gpio_test();
+    ip_i2c_test();
+    cmd_benchmark(true, 0);
+    // cmd_benchmark_all();
+    cmd_memtest();
+    cmd_echo();
+    while (1)
+        ;
     // while (1) {
     //     print("\n");
     //     print("Select an action:\n");
diff --git a/rtl/firmware.h b/rtl/firmware.h
index 3fd05ee9..afa61654 100644
--- a/rtl/firmware.h
+++ b/rtl/firmware.h
@@ -1,5 +1,5 @@
-#ifndef _RETROSOC_H_
-#define _RETROSOC_H_
+#ifndef _RETROSOC_DEF_H_
+#define _RETROSOC_DEF_H_
 
 #include <stdint.h>
 #include <stdbool.h>
@@ -43,4 +43,40 @@
 
 #define reg_irq7_source    (*(volatile uint32_t*)0x030000b0)
 #define reg_irq8_source    (*(volatile uint32_t*)0x030000b4)
+
+// command register bits
+// bit 7 = start
+// bit 6 = stop
+// bit 5 = read
+// bit 4 = write
+
+// control register bits
+// bit 27 = acknowledge
+// bit 24 = interrupt acknowledge
+// bit 23 = enable
+// bit 22 = interrupt enable
+
+// bits 15-0:  clock prescaler
+#define     I2C_CMD_STA         0x80
+#define     I2C_CMD_STO         0x40
+#define     I2C_CMD_RD          0x20
+#define     I2C_CMD_WR          0x10
+#define     I2C_CMD_ACK         0x08
+#define     I2C_CMD_IACK        0x01
+
+#define     I2C_CTRL_EN         0x80
+#define     I2C_CTRL_IEN        0x40
+
+// status regiter bits:
+// bit 7 = receive acknowledge
+// bit 6 = busy (start signal detected)
+// bit 5 = arbitration lost
+// bit 1 = transfer in progress
+// bit 0 = interrupt flag
+#define     I2C_STAT_RXACK      0x80
+#define     I2C_STAT_BUSY       0x40
+#define     I2C_STAT_AL         0x20
+#define     I2C_STAT_TIP        0x02
+#define     I2C_STAT_IF         0x01
+
 #endif