diff --git a/cmd_parser.c b/cmd_parser.c
index 7bf6eca..f976fe2 100644
--- a/cmd_parser.c
+++ b/cmd_parser.c
@@ -13,6 +13,7 @@
 #include "rtl837x_flash.h"
 #include "rtl837x_phy.h"
 #include "rtl837x_regs.h"
+#include "rtl837x_sfr.h"
 #include "uip/uip.h"
 
 #pragma codeseg BANK1
@@ -24,6 +25,7 @@ extern __xdata uint8_t isRTL8373;
 extern __xdata uint16_t mpos;
 
 extern volatile __xdata uint32_t ticks;
+extern volatile __xdata uint8_t sfr_data[4];
 
 extern __code uint8_t * __code greeting;
 extern __code uint8_t * __code hex;
@@ -31,6 +33,11 @@ extern __code uint8_t * __code hex;
 extern __xdata uint8_t flash_buf[256];
 __xdata uint8_t vlan_names[VLAN_NAMES_SIZE];
 __xdata uint16_t vlan_ptr;
+__xdata uint8_t gpio_last_value[8] = { 0 };
+
+// Temporatly for str to hex convertion value.
+// Support up to 32_bits.
+__xdata uint8_t hexvalue[4] = { 0 };
 
 
 // Buffer for writing to flash 0x1fd000, copy to 0x1fe000
@@ -53,7 +60,20 @@ __code uint8_t phys_to_log_port[6] = {
 
 inline uint8_t isletter(uint8_t l)
 {
-	return (l >= 'a' && l <= 'z') || (l >= 'A' && l <= 'Z');
+	// return (l >= 'a' && l <= 'z') || (l >= 'A' && l <= 'Z');
+
+	// Make it lowercase
+	l |= 0x20;
+	l -= 'a';
+	return (l <= ('z'-'a'));
+}
+
+
+inline uint8_t isnumber(uint8_t l)
+{
+	// return (l >= '0' && l <= '9');
+	l -= '0';
+	return (l <= ('9'-'0'));
 }
 
 
@@ -78,12 +98,56 @@ uint8_t cmd_compare(uint8_t start, uint8_t * __code cmd)
 }
 
 
+/* Converts ascii-hex array into value.
+	returns number of hexvalue[] entries has been written.
+	return value = 0 means error.
+*/
+uint8_t atoi_hex(uint8_t idx)
+{
+	uint8_t h_idx = 0;
+	uint8_t val = 0;
+	uint8_t c;
+
+	while(1) {
+		c = cmd_buffer[idx];
+
+		if (c == '\0' || c == ' ') {
+			break;
+		}
+
+		// swap hex nibbles
+		val = (val >> 4) | (val << 4);
+
+		if (c - '0' < 10) {
+			val |= c - '0';
+		} else {
+			c |= 0x20;
+			c -= 'a';
+			if (c > 5) {
+				h_idx = 0;
+				break;
+			}
+			val |= c + 10;
+		}
+
+		idx++;
+		hexvalue[h_idx >> 1] = val;
+
+		if (h_idx & 1 == 1) {
+			val = 0;
+		}
+		h_idx++;
+	}
+
+	return ((h_idx + 1) >> 1);
+}
+
 uint8_t atoi_short(register uint16_t *vlan, register uint8_t idx)
 {
 	uint8_t err = 1;
 	*vlan = 0;
 
-	while (cmd_buffer[idx] >= '0' && cmd_buffer[idx] <= '9') {
+	while (isnumber(cmd_buffer[idx])) {
 		err = 0;
 		*vlan = (*vlan * 10) + cmd_buffer[idx] - '0';
 		idx++;
@@ -98,7 +162,7 @@ uint8_t parse_ip(register uint8_t idx)
 
 	for (b = 0; b < 4; b++) {
 		ip[b] = 0;
-		while (cmd_buffer[idx] >= '0' && cmd_buffer[idx] <= '9') {
+		while (isnumber(cmd_buffer[idx])) {
 			ip[b] = (ip[b] * 10) + cmd_buffer[idx] - '0';
 			idx++;
 		}
@@ -121,7 +185,7 @@ void parse_trunk(void)
 	uint8_t w = 2;
 	while (cmd_words_b[w] > 0) {
 		uint8_t port;
-		if (cmd_buffer[cmd_words_b[w]] >= '0' && cmd_buffer[cmd_words_b[w]] <= '9') {
+		if (isnumber(cmd_buffer[cmd_words_b[w]])) {
 			port = cmd_buffer[cmd_words_b[w]] - '1';
 			if (port > maxPort)
 				goto err;
@@ -166,9 +230,9 @@ void parse_vlan(void)
 		}
 		while (cmd_words_b[w] > 0) {
 			uint8_t port;
-			if (cmd_buffer[cmd_words_b[w]] >= '0' && cmd_buffer[cmd_words_b[w]] <= '9') {
+			if (isnumber(cmd_buffer[cmd_words_b[w]])) {
 				port = cmd_buffer[cmd_words_b[w]] - '1';
-				if (cmd_buffer[cmd_words_b[w] + 1] >= '0' && cmd_buffer[cmd_words_b[w] + 1] <= '9') {
+				if (isnumber(cmd_buffer[cmd_words_b[w] + 1])) {
 					port = (port + 1) * 10 + cmd_buffer[cmd_words_b[w] + 1] - '1';
 					if (cmd_buffer[cmd_words_b[w] + 2] == 't')
 						tagged |= ((uint16_t)1) << port;
@@ -202,23 +266,23 @@ void parse_mirror(void)
 	__xdata uint16_t rx_pmask = 0;
 	__xdata uint16_t tx_pmask = 0;
 
-	if (cmd_buffer[cmd_words_b[1]] < '0' || cmd_buffer[cmd_words_b[1]] > '9') {
+	if (!isnumber(cmd_buffer[cmd_words_b[1]])) {
 		print_string("Port missing: port <mirroring port> [port][t/r]...");
 		return;
 	}
 
 	mirroring_port = cmd_buffer[cmd_words_b[1]] - '1';
-	if (cmd_buffer[cmd_words_b[1] + 1] >= '0' && cmd_buffer[cmd_words_b[1] + 1] <= '9')
-	mirroring_port = (mirroring_port + 1) * 10 + cmd_buffer[cmd_words_b[1] + 1] - '1';
+	if (isnumber(cmd_buffer[cmd_words_b[1] + 1]))
+		mirroring_port = (mirroring_port + 1) * 10 + cmd_buffer[cmd_words_b[1] + 1] - '1';
 	if (!isRTL8373)
 		mirroring_port = phys_to_log_port[mirroring_port];
 
 	uint8_t w = 2;
 	while (cmd_words_b[w] > 0) {
 		uint8_t port;
-		if (cmd_buffer[cmd_words_b[w]] >= '0' && cmd_buffer[cmd_words_b[w]] <= '9') {
+		if (isnumber(cmd_buffer[cmd_words_b[w]])) {
 			port = cmd_buffer[cmd_words_b[w]] - '1';
-			if (cmd_buffer[cmd_words_b[w] + 1] >= '0' && cmd_buffer[cmd_words_b[w] + 1] <= '9') {
+			if (isnumber(cmd_buffer[cmd_words_b[w] + 1])) {
 				port = (port + 1) * 10 + cmd_buffer[cmd_words_b[w] + 1] - '1';
 				if (!isRTL8373)
 					port = phys_to_log_port[port];
@@ -249,6 +313,87 @@ void parse_mirror(void)
 }
 
 
+void parse_regget(void)
+{
+	uint16_t reg = 0;
+
+	if (cmd_words_b[1] < 0) {
+		goto err;
+	}
+
+	uint8_t hex_size = atoi_hex(cmd_words_b[1]);
+
+	if (hex_size == 0 || hex_size > 2) {
+		goto err;
+	}
+
+	reg = hexvalue[0];
+	if (hex_size == 2) {
+		reg <<= 8;
+		reg |= hexvalue[1];
+	}
+
+	print_string("REGGET: ");
+	print_short(reg);
+	print_string(": VAL: ");
+
+	reg_read_m(reg);
+	print_sfr_data();
+	return;
+
+err:
+	print_string("usage: regget <hexvalue>\n\tlike: regget 0BB0 or regget 0c");
+	return;
+}
+
+
+void parse_regset(void)
+{
+	uint16_t reg = 0;
+
+	if (cmd_words_b[2] < 0) {
+		goto err;
+	}
+
+	uint8_t hex_size = atoi_hex(cmd_words_b[1]);
+	if (hex_size == 0 || hex_size > 2) {
+		goto err;
+	}
+
+	reg = hexvalue[0];
+	if (hex_size == 2) {
+		reg <<= 8;
+		reg |= hexvalue[1];
+	}
+
+	hex_size = atoi_hex(cmd_words_b[2]);
+	if (hex_size == 0 || hex_size > 4) {
+		goto err;
+	}
+
+	// zero sfr memory data
+	sfr_set_zero();
+
+	// copy data over sfr memory
+	uint8_t offset = 4 - hex_size;
+	while(hex_size) {
+		hex_size -= 1;
+		sfr_data[offset + hex_size] = hexvalue[hex_size];
+	}
+	print_string("REGSET: ");
+	print_short(reg);
+
+	reg_write_m(reg);
+
+	print_string(": VAL: ");
+	print_sfr_data();
+	return;
+
+err:
+	print_string("usage: regset <hexvalue> <hexvalue>\n\tlike regset 0b abcd1234.");
+}
+
+
 // Parse command into words
 uint8_t cmd_tokenize(void) __banked
 {
@@ -282,6 +427,33 @@ uint8_t cmd_tokenize(void) __banked
 	return 0;
 }
 
+// Print GPIO status
+void print_gpio_status(void) {
+	for (uint8_t idx = 0; idx < 2; idx++) {
+		reg_read(RTL837X_REG_GPIO_00_31_INPUT + (idx * 4));
+		print_string("GPIO ");
+		write_char(idx + '0');
+		write_char(':');
+		write_char(' ');
+
+		print_byte(SFR_DATA_24);
+		print_byte(SFR_DATA_16);
+		print_byte(SFR_DATA_8);
+		print_byte(SFR_DATA_0);
+
+		write_char(' ');
+		print_byte( gpio_last_value[(idx *4)] ^ SFR_DATA_24);
+		gpio_last_value[(idx *4)] = SFR_DATA_24;
+		print_byte( gpio_last_value[(idx *4) + 1] ^ SFR_DATA_16);
+		gpio_last_value[(idx *4) + 1] = SFR_DATA_16;
+		print_byte( gpio_last_value[(idx *4) + 2] ^ SFR_DATA_8);
+		gpio_last_value[(idx *4) + 2] = SFR_DATA_8;
+		print_byte( gpio_last_value[(idx *4) + 3] ^ SFR_DATA_0);
+		gpio_last_value[(idx *4) + 3] = SFR_DATA_0;
+		write_char('\n');
+	}
+}
+
 
 // Identify command
 void cmd_parser(void) __banked
@@ -423,10 +595,18 @@ void cmd_parser(void) __banked
 		if (cmd_compare(0, "sds")) {
 			print_reg(RTL837X_REG_SDS_MODES);
 		}
+		if (cmd_compare(0, "gpio")) {
+			print_gpio_status();
+		}
+		if (cmd_compare(0, "regget")) {
+			parse_regget();
+		}
+		if (cmd_compare(0, "regset")) {
+			parse_regset();
+		}
 	}
 }
 
-
 void execute_config(void) __banked
 {
 	__xdata uint32_t pos = CONFIG_START;
@@ -445,3 +625,4 @@ void execute_config(void) __banked
 		}
 	} while (mpos != 0xffff);
 }
+
diff --git a/rtl837x_common.h b/rtl837x_common.h
index 46f480d..35d1e49 100644
--- a/rtl837x_common.h
+++ b/rtl837x_common.h
@@ -75,6 +75,7 @@ void print_reg(uint16_t reg);
 uint8_t sfp_read_reg(uint8_t slot, uint8_t reg);
 void reg_bit_set(uint16_t reg_addr, char bit);
 void reg_bit_clear(uint16_t reg_addr, char bit);
+void sfr_set_zero(void);
 void reset_chip(void);
 void memcpy(__xdata void * __xdata dst, __xdata const void * __xdata src, uint16_t len);
 void memcpyc(register __xdata uint8_t *dst, register __code uint8_t *src, register uint16_t len);
diff --git a/rtl837x_port.c b/rtl837x_port.c
index f999196..4383bf0 100644
--- a/rtl837x_port.c
+++ b/rtl837x_port.c
@@ -423,13 +423,13 @@ void port_stats_print(void) __banked
 				break;
 			}
 		} else {  // An SFP Module TODO: This is for 1 module devices
-			reg_read_m(RTL837X_REG_GPIO_B);
+			reg_read_m(RTL837X_REG_GPIO_00_31_INPUT);
 			if (!(sfr_data[0] & 0x40)) {
 				print_string("SFP OK\t");
 			} else {
 				print_string("NO SFP\t");
 			}
-			reg_read_m(RTL837X_REG_GPIO_C);
+			reg_read_m(RTL837X_REG_GPIO_32_63_INPUT);
 			if (sfr_data[3] & 0x20) {
 				print_string("Down\t");
 			} else {
diff --git a/rtl837x_regs.h b/rtl837x_regs.h
index a07f2be..326acb8 100644
--- a/rtl837x_regs.h
+++ b/rtl837x_regs.h
@@ -44,22 +44,24 @@
  * Pin configuration (pinmux)
  */
 
-#define RTL837X_PIN_MUX_A	0x7f8c
-#define RTL837X_PIN_MUX_B	0x7f90
+#define RTL837X_PIN_MUX_0	0x7f8c
+#define RTL837X_PIN_MUX_1	0x7f90
 
-#define RTL837X_REG_GPIO_A 0x40
+// Output Registers
+#define RTL837X_REG_GPIO_00_31_OUTPUT 0x3c
+#define RTL837X_REG_GPIO_32_63_OUTPUT 0x40
 // BIT 4 resets RTL8224 on 9000-9XH
 
-#define RTL837X_REG_GPIO_B 0x44
+// Input Registers
+#define RTL837X_REG_GPIO_00_31_INPUT 0x44
+#define RTL837X_REG_GPIO_32_63_INPUT 0x48
 // Bit 1e cleared: SFP Module inserted on 9000-6XH (MOD_DEF0 pin)
 
-#define RTL837X_REG_GPIO_C 0x48
 // BIT 5 set: SIGNAL LOS of SFP module on 9000-6XH (RX_LOS pin)
 
-#define RTL837X_REG_GPIO_CONF_A 0x50
-// Configures IO direction for bank a
-
-#define RTL837X_REG_GPIO_EXT 0x63e8
+// Direction Registers, 0 = input, 1 = output
+#define RTL837X_REG_GPIO_00_31_DIRECTION 0x4c
+#define RTL837X_REG_GPIO_32_63_DIRECTION 0x50
 
 /*
  * I2C controller
diff --git a/rtlplayground.c b/rtlplayground.c
index c140362..b5ea978 100644
--- a/rtlplayground.c
+++ b/rtlplayground.c
@@ -61,6 +61,7 @@ __xdata uint8_t sbuf[SBUF_SIZE];
 __xdata uint8_t sfr_data[4];
 
 extern __xdata uint8_t cmd_buffer[SBUF_SIZE];
+extern __xdata uint8_t gpio_last_value[8];
 
 __code uint8_t * __code greeting = "\nA minimal prompt to explore the RTL8372:\n";
 __code uint8_t * __code hex = "0123456789abcdef";
@@ -380,6 +381,16 @@ void sfr_mask_data(uint8_t n, uint8_t mask, uint8_t set)
 	sfr_data[3-n] = b;
 }
 
+/*
+ * This zeros all the sfr data fields
+ */
+void sfr_set_zero(void) {
+	uint8_t idx = 4;
+	while (idx) {
+		idx -= 1;
+		sfr_data[idx] = 0;
+	}
+}
 
 /*
  * Transfer Network Interface RX data from the ASIC to the 8051 XMEM
@@ -873,7 +884,7 @@ void sfp_print_info(uint8_t sfp)
 
 void handle_sfp(void)
 {
-	reg_read_m(RTL837X_REG_GPIO_B);
+	reg_read_m(RTL837X_REG_GPIO_00_31_INPUT);
 	if ((sfp_pins_last & 0x1) && (!(sfr_data[0] & 0x40))) {
 		sfp_pins_last &= ~0x01;
 		print_string("\n<MODULE INSERTED>  ");
@@ -893,7 +904,7 @@ void handle_sfp(void)
 		print_string("\n<MODULE REMOVED>\n");
 	}
 
-	reg_read_m(RTL837X_REG_GPIO_C);
+	reg_read_m(RTL837X_REG_GPIO_32_63_INPUT);
 	if ((sfp_pins_last & 0x2) && (!(sfr_data[3] & 0x20))) {
 		sfp_pins_last &= ~0x02;
 		print_string("\n<SFP-RX OK>\n");
@@ -903,7 +914,7 @@ void handle_sfp(void)
 		print_string("\n<SFP-RX LOS>\n");
 	}
 
-	reg_read_m(RTL837X_REG_GPIO_C);
+	reg_read_m(RTL837X_REG_GPIO_32_63_INPUT);
 	if ((sfp_pins_last & 0x10) && (!(sfr_data[1] & 0x04))) {
 		sfp_pins_last &= ~0x10;
 		print_string("\n<MODULE 2 INSERTED>  ");
@@ -989,7 +1000,7 @@ void idle(void)
 	handle_sfp();
 
 	/* Button pressed on KL-8xhm-x2:
-	reg_read(RTL837X_REG_GPIO_C);
+	reg_read(RTL837X_REG_GPIO_32_63_INPUT);
 	if (!(sfr_data[2] & 0x40))
 		print_string("Button pressed\n");
 	*/
@@ -1038,12 +1049,12 @@ void setup_external_irqs(void)
 void rtl8224_enable(void)
 {
 	// Set Pin 4 low
-	reg_bit_clear(RTL837X_REG_GPIO_A, 4);
+	reg_bit_clear(RTL837X_REG_GPIO_32_63_OUTPUT, 4);
 	// Configure Pin as output
-	reg_bit_set(RTL837X_REG_GPIO_CONF_A, 4);
+	reg_bit_set(RTL837X_REG_GPIO_32_63_DIRECTION, 4);
 	delay(100);
 	// Set pin 4 high
-	reg_bit_set(RTL837X_REG_GPIO_A, 4);
+	reg_bit_set(RTL837X_REG_GPIO_32_63_OUTPUT, 4);
 	delay(500);
 }
 
@@ -1064,9 +1075,9 @@ void setup_clock(void)
 	reg_write_m(RTL837X_REG_HW_CONF);
 
 	// Enable serial interface, set bit 0
-	reg_read_m(RTL837X_PIN_MUX_B);
+	reg_read_m(RTL837X_PIN_MUX_1);
 	sfr_mask_data(0, 0x1, 0x1);
-	reg_write_m(RTL837X_PIN_MUX_B);
+	reg_write_m(RTL837X_PIN_MUX_1);
 }
 
 
@@ -1286,7 +1297,7 @@ void led_config_9xh(void)
 	reg_bit_clear(0x65dc, 0x1b);
 
 	// r7f8c:30000000 R7f8c-30000000 r7f8c:30000000 R7f8c-38000000
-	reg_bit_set(RTL837X_PIN_MUX_A, 0x1b);
+	reg_bit_set(RTL837X_PIN_MUX_0, 0x1b);
 
 	// R6548-0041017f
 	REG_SET(0x6548, 0x0041017f);
@@ -1338,13 +1349,13 @@ void led_config(void)
 
 	// Set bits 1b/1d of 0x7f8c: r7f8c:30000000 R7f8c-30000000 r7f8c:30000000 R7f8c-38000000
 	if (nSFPPorts == 2) {
-		reg_bit_set(RTL837X_PIN_MUX_A, 0x1b); // R7f8c-28000000
-		reg_bit_clear(RTL837X_PIN_MUX_A, 0x1c); // R7f8c-28000000
-		reg_bit_set(RTL837X_PIN_MUX_A, 0x1d); // R7f8c-28000000
+		reg_bit_set(RTL837X_PIN_MUX_0, 0x1b); // R7f8c-28000000
+		reg_bit_clear(RTL837X_PIN_MUX_0, 0x1c); // R7f8c-28000000
+		reg_bit_set(RTL837X_PIN_MUX_0, 0x1d); // R7f8c-28000000
 	} else {
-		reg_bit_set(RTL837X_PIN_MUX_A, 0x1d);
-		reg_bit_set(RTL837X_PIN_MUX_A, 0x1c);
-		reg_bit_set(RTL837X_PIN_MUX_A, 0x1b);
+		reg_bit_set(RTL837X_PIN_MUX_0, 0x1d);
+		reg_bit_set(RTL837X_PIN_MUX_0, 0x1c);
+		reg_bit_set(RTL837X_PIN_MUX_0, 0x1b);
 	}
 	// LED setup
 	// r6520:0021fdb0 R6520-0021e7b0 r6520:0021e7b0 R6520-0021e6b0 r65f8:00000018 R65f8-00000018 R65fc-fffff000 r6600:00000000 R6600-0000000f r65dc:5fffff00 R65dc-7fffff00 r65dc:7fffff00 R65dc-77ffff00
@@ -1620,10 +1631,10 @@ void setup_i2c(void)
 	REG_SET(0x041c, 0);
 
 	// HW Control register, enable I2C?
-	reg_read_m(RTL837X_PIN_MUX_B);
+	reg_read_m(RTL837X_PIN_MUX_1);
 	sfr_mask_data(3, 0x20, 0x00); // Clear bit 29
 	sfr_mask_data(0, 0x60, 0x40); // Set bits 5-6 to 0b10
-	reg_write_m(RTL837X_PIN_MUX_B);
+	reg_write_m(RTL837X_PIN_MUX_1);
 }
 
 
diff --git a/rtlplayground.mem b/rtlplayground.mem
index 5b1efa1..0259cd7 100644
--- a/rtlplayground.mem
+++ b/rtlplayground.mem
@@ -25,5 +25,5 @@ Other memory:
    Name             Start    End      Size     Max     
    ---------------- -------- -------- -------- --------
    PAGED EXT. RAM                         0      256   
-   EXTERNAL RAM     0x0001   0x1ad8    6872    16777216
-   ROM/EPROM/FLASH  0x0000   0x1c195  45278    16777216
+   EXTERNAL RAM     0x0001   0x1ae8    6888    16777216
+   ROM/EPROM/FLASH  0x0000   0x1c69f  46879    16777216