diff --git a/verilog/dv/caravel/mgmt_soc/gpio/gpio.c b/verilog/dv/caravel/mgmt_soc/gpio/gpio.c
index 73dd397..ae6e621 100644
--- a/verilog/dv/caravel/mgmt_soc/gpio/gpio.c
+++ b/verilog/dv/caravel/mgmt_soc/gpio/gpio.c
@@ -1,115 +1,51 @@
-/*
- * SPDX-FileCopyrightText: 2020 Efabless Corporation
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- * SPDX-License-Identifier: Apache-2.0
- */
-
 #include "../../defs.h"
+#include "../../stub.c"
+#include "printio.h"
 
-// --------------------------------------------------------
+void main() {
+    unsigned int s = 0;
+    for(int i = 0; i < 10000; i++) s += i;
+    printstr("test\n");
+    printstr((char*)0x30000050);
+    printstr("\n");
 
-/*
- *	GPIO Test
- *		Tests PU and PD on the lower 8 pins while being driven from outside
- *		Tests Writing to the upper 8 pins
- *		Tests reading from the lower 8 pins
- */
+    // Load key as 114849718896073566083416993308083054187
+    *((volatile uint32_t*) 0x30000040) = 1449607251;
+    *((volatile uint32_t*) 0x30000040) = 1734159223;
+    *((volatile uint32_t*) 0x30000040) = 1713582630;
+    *((volatile uint32_t*) 0x30000040) = 908945003;
 
-void main()
-{
-	int i;
+    // Update key
+    *((volatile uint32_t*) 0x3000000C) = 1;
+    printstr("key\n");
+    while((*((volatile uint32_t*) 0x30000000) & 0x3) != 0x3);
 
-	/* Set data out to zero */
-	reg_mprj_datal = 0;
+    // Set IV to 94605682877008349257125771496548550738
+    *((volatile uint32_t*) 0x30000000) = 0;
+    *((volatile uint32_t*) 0x30000010) = 1194091594;
+    *((volatile uint32_t*) 0x30000010) = 577847158;
+    *((volatile uint32_t*) 0x30000010) = 1245724226;
+    *((volatile uint32_t*) 0x30000010) = 1512654930;
+    *((volatile uint32_t*) 0x30000008) = 1;
+    printstr("iv\n");
+    while((*((volatile uint32_t*) 0x30000000) & 0x4) != 0x4);
+    *((volatile uint32_t*) 0x30000000) = 8;
 
-	/* Lower 8 pins are input and upper 8 pins are output */
-	reg_mprj_io_31 = GPIO_MODE_MGMT_STD_OUTPUT;
-	reg_mprj_io_30 = GPIO_MODE_MGMT_STD_OUTPUT;
-	reg_mprj_io_29 = GPIO_MODE_MGMT_STD_OUTPUT;
-	reg_mprj_io_28 = GPIO_MODE_MGMT_STD_OUTPUT;
-	reg_mprj_io_27 = GPIO_MODE_MGMT_STD_OUTPUT;
-	reg_mprj_io_26 = GPIO_MODE_MGMT_STD_OUTPUT;
-	reg_mprj_io_25 = GPIO_MODE_MGMT_STD_OUTPUT;
-	reg_mprj_io_24 = GPIO_MODE_MGMT_STD_OUTPUT;
+    // Encrypt 54758945827980741386470298169332301409
+    *((volatile uint32_t*) 0x30000010) = 691155065;
+    *((volatile uint32_t*) 0x30000010) = 845884219;
+    *((volatile uint32_t*) 0x30000010) = 1382968177;
+    *((volatile uint32_t*) 0x30000010) = 1333362273;
+    *((volatile uint32_t*) 0x30000004) = 1;
+    printstr("enc\n");
+    while((*((volatile uint32_t*) 0x30000000) & 0x4) != 0x4);
 
-	reg_mprj_io_23 = GPIO_MODE_MGMT_STD_INPUT_NOPULL;
-	reg_mprj_io_22 = GPIO_MODE_MGMT_STD_INPUT_NOPULL;
-	reg_mprj_io_21 = GPIO_MODE_MGMT_STD_INPUT_NOPULL;
-	reg_mprj_io_20 = GPIO_MODE_MGMT_STD_INPUT_NOPULL;
-	reg_mprj_io_19 = GPIO_MODE_MGMT_STD_INPUT_NOPULL;
-	reg_mprj_io_18 = GPIO_MODE_MGMT_STD_INPUT_NOPULL;
-	reg_mprj_io_17 = GPIO_MODE_MGMT_STD_INPUT_NOPULL;
-	reg_mprj_io_16 = GPIO_MODE_MGMT_STD_INPUT_NOPULL;
+    // Read back result
+    printhex(*((volatile uint32_t*) 0x30000030), true); // should be 2f93dcae
+    printhex(*((volatile uint32_t*) 0x30000034), true); // should be dbcedead
+    printhex(*((volatile uint32_t*) 0x30000038), true); // should be 545145d7
+    printhex(*((volatile uint32_t*) 0x3000003C), true); // should be aeb28360
 
-	/* Apply configuration */
-	reg_mprj_xfer = 1;
-	while (reg_mprj_xfer == 1);
-
-	// change the pull up and pull down (checked by the TB)
-	reg_mprj_datal = 0xa0000000;
-
-	reg_mprj_io_23 = GPIO_MODE_MGMT_STD_INPUT_PULLDOWN;
-	reg_mprj_io_22 = GPIO_MODE_MGMT_STD_INPUT_PULLDOWN;
-	reg_mprj_io_21 = GPIO_MODE_MGMT_STD_INPUT_PULLDOWN;
-	reg_mprj_io_20 = GPIO_MODE_MGMT_STD_INPUT_PULLDOWN;
-
-	reg_mprj_io_19 = GPIO_MODE_MGMT_STD_INPUT_PULLUP;
-	reg_mprj_io_18 = GPIO_MODE_MGMT_STD_INPUT_PULLUP;
-	reg_mprj_io_17 = GPIO_MODE_MGMT_STD_INPUT_PULLUP;
-	reg_mprj_io_16 = GPIO_MODE_MGMT_STD_INPUT_PULLUP;
-
-	/* Apply configuration */
-	reg_mprj_xfer = 1;
-	while (reg_mprj_xfer == 1);
-
-	reg_mprj_datal = 0x0b000000;
-
-	reg_mprj_io_23 = GPIO_MODE_MGMT_STD_INPUT_PULLUP;
-	reg_mprj_io_22 = GPIO_MODE_MGMT_STD_INPUT_PULLUP;
-	reg_mprj_io_21 = GPIO_MODE_MGMT_STD_INPUT_PULLUP;
-	reg_mprj_io_20 = GPIO_MODE_MGMT_STD_INPUT_PULLUP;
-
-	reg_mprj_io_19 = GPIO_MODE_MGMT_STD_INPUT_PULLDOWN;
-	reg_mprj_io_18 = GPIO_MODE_MGMT_STD_INPUT_PULLDOWN;
-	reg_mprj_io_17 = GPIO_MODE_MGMT_STD_INPUT_PULLDOWN;
-	reg_mprj_io_16 = GPIO_MODE_MGMT_STD_INPUT_PULLDOWN;
-
-	/* Apply configuration */
-	reg_mprj_xfer = 1;
-	while (reg_mprj_xfer == 1);
-
-	reg_mprj_io_23 = GPIO_MODE_MGMT_STD_INPUT_PULLDOWN;
-	reg_mprj_io_22 = GPIO_MODE_MGMT_STD_INPUT_PULLDOWN;
-	reg_mprj_io_21 = GPIO_MODE_MGMT_STD_INPUT_PULLDOWN;
-	reg_mprj_io_20 = GPIO_MODE_MGMT_STD_INPUT_PULLDOWN;
-
-	reg_mprj_io_19 = GPIO_MODE_MGMT_STD_INPUT_PULLUP;
-	reg_mprj_io_18 = GPIO_MODE_MGMT_STD_INPUT_PULLUP;
-	reg_mprj_io_17 = GPIO_MODE_MGMT_STD_INPUT_PULLUP;
-	reg_mprj_io_16 = GPIO_MODE_MGMT_STD_INPUT_PULLUP;
-
-	/* Apply configuration */
-	reg_mprj_xfer = 1;
-	while (reg_mprj_xfer == 1);
-
-	// read the lower 8 pins, add 1 then output the result
-	// checked by the TB
-	reg_mprj_datal = 0xab000000;
-
-	while (1){
-		int x = (reg_mprj_datal & 0xff0000) >> 16;
-		reg_mprj_datal = (x+1) << 24;
-	}
+    endtest();
 }
 
diff --git a/verilog/dv/caravel/mgmt_soc/gpio/printio.h b/verilog/dv/caravel/mgmt_soc/gpio/printio.h
new file mode 100644
index 0000000..94da39d
--- /dev/null
+++ b/verilog/dv/caravel/mgmt_soc/gpio/printio.h
@@ -0,0 +1,115 @@
+void shift_char(char c) {
+    *((volatile char*)&reg_mprj_datal) = c;
+
+    reg_gpio_ena = 0;
+    reg_gpio_data = 1;
+    reg_gpio_data = 1;
+    // should be enough delay to detect output
+    reg_gpio_data = 0;
+}
+
+void setup_out() {
+    reg_mprj_io_7  = GPIO_MODE_MGMT_STD_OUTPUT;
+    reg_mprj_io_6  = GPIO_MODE_MGMT_STD_OUTPUT;
+    reg_mprj_io_5  = GPIO_MODE_MGMT_STD_OUTPUT;
+    reg_mprj_io_4  = GPIO_MODE_MGMT_STD_OUTPUT;
+    reg_mprj_io_3  = GPIO_MODE_MGMT_STD_OUTPUT;
+    reg_mprj_io_2  = GPIO_MODE_MGMT_STD_OUTPUT;
+    reg_mprj_io_1  = GPIO_MODE_MGMT_STD_OUTPUT;
+    reg_mprj_io_0  = GPIO_MODE_MGMT_STD_OUTPUT;
+
+    reg_mprj_xfer = 1;
+    while (reg_mprj_xfer == 1);
+}
+
+void write_char(char c) {
+    uint32_t tmp7 = reg_mprj_io_7;
+    uint32_t tmp6 = reg_mprj_io_6;
+    uint32_t tmp5 = reg_mprj_io_5;
+    uint32_t tmp4 = reg_mprj_io_4;
+    uint32_t tmp3 = reg_mprj_io_3;
+    uint32_t tmp2 = reg_mprj_io_2;
+    uint32_t tmp1 = reg_mprj_io_1;
+    uint32_t tmp0 = reg_mprj_io_0;
+
+    setup_out();
+
+    shift_char(c);
+
+    reg_mprj_io_7 = tmp7;
+    reg_mprj_io_6 = tmp6;
+    reg_mprj_io_5 = tmp5;
+    reg_mprj_io_4 = tmp4;
+    reg_mprj_io_3 = tmp3;
+    reg_mprj_io_2 = tmp2;
+    reg_mprj_io_1 = tmp1;
+    reg_mprj_io_0 = tmp0;
+
+    reg_mprj_xfer = 1;
+    while (reg_mprj_xfer == 1);
+}
+
+void printhex(uint32_t x, bool newline) {
+    uint32_t tmp7 = reg_mprj_io_7;
+    uint32_t tmp6 = reg_mprj_io_6;
+    uint32_t tmp5 = reg_mprj_io_5;
+    uint32_t tmp4 = reg_mprj_io_4;
+    uint32_t tmp3 = reg_mprj_io_3;
+    uint32_t tmp2 = reg_mprj_io_2;
+    uint32_t tmp1 = reg_mprj_io_1;
+    uint32_t tmp0 = reg_mprj_io_0;
+
+    setup_out();
+
+    char *s = "0123456789abcdef";
+
+    for (int i = 0; i < 8; i++) {
+        shift_char(s[(x & 0xF0000000) >> 28]);
+        x = x << 4;
+    }
+    if(newline) shift_char('\n');
+
+    reg_mprj_io_7 = tmp7;
+    reg_mprj_io_6 = tmp6;
+    reg_mprj_io_5 = tmp5;
+    reg_mprj_io_4 = tmp4;
+    reg_mprj_io_3 = tmp3;
+    reg_mprj_io_2 = tmp2;
+    reg_mprj_io_1 = tmp1;
+    reg_mprj_io_0 = tmp0;
+
+    reg_mprj_xfer = 1;
+    while (reg_mprj_xfer == 1);
+}
+
+void printstr(char *str) {
+    uint32_t tmp7 = reg_mprj_io_7;
+    uint32_t tmp6 = reg_mprj_io_6;
+    uint32_t tmp5 = reg_mprj_io_5;
+    uint32_t tmp4 = reg_mprj_io_4;
+    uint32_t tmp3 = reg_mprj_io_3;
+    uint32_t tmp2 = reg_mprj_io_2;
+    uint32_t tmp1 = reg_mprj_io_1;
+    uint32_t tmp0 = reg_mprj_io_0;
+
+    setup_out();
+
+    char c;
+    while (*str) shift_char(*str++);
+
+    reg_mprj_io_7 = tmp7;
+    reg_mprj_io_6 = tmp6;
+    reg_mprj_io_5 = tmp5;
+    reg_mprj_io_4 = tmp4;
+    reg_mprj_io_3 = tmp3;
+    reg_mprj_io_2 = tmp2;
+    reg_mprj_io_1 = tmp1;
+    reg_mprj_io_0 = tmp0;
+
+    reg_mprj_xfer = 1;
+    while (reg_mprj_xfer == 1);
+}
+
+void endtest() {
+    printstr("\x04");
+}