diff --git a/Makefile b/Makefile
index 11d89b4..2a3f01f 100644
--- a/Makefile
+++ b/Makefile
@@ -53,6 +53,7 @@ create_build_dir:
 # Keep machine.c in first position to fail immediately on invalid $MACHINE value
 SRCS = \
 	machine.c \
+	machine_init.c \
 	cmd_editor.c \
 	cmd_parser.c \
 	dhcp.c \
@@ -142,6 +143,7 @@ machine_check:
 	do \
 	echo "Checking $${MACHINE}"; \
 	$(CC) $(CC_FLAGS) -DMACHINE_$${MACHINE} -MMD -o $(BUILDDIR)/tmp/machine_check -c machine.c; \
+	$(CC) $(CC_FLAGS) -DMACHINE_$${MACHINE} -MMD -o $(BUILDDIR)/tmp/machine_check -c machine_init.c; \
 	done
 	@rm -rf $(BUILDDIR)/tmp
 
diff --git a/cmd_editor.c b/cmd_editor.c
index 866cf40..aebfe7f 100644
--- a/cmd_editor.c
+++ b/cmd_editor.c
@@ -193,7 +193,7 @@ void cmd_edit(void) __banked
 		// Check whether return was pressed:
 		if (sbuf[l] == '\n' || sbuf[l] == '\r') {
 			write_char('\n');
-			cmd_buffer[cmd_line_len] = '\0';
+			cmd_buffer[cmd_line_len] = NUL;
 //			write_char('>'); print_string_x(cmd_buffer); write_char('<');
 			// If there is a command we print the prompt after execution
 			// otherwise immediately because there is nothing to execute
diff --git a/cmd_parser.c b/cmd_parser.c
index 93c51e6..66b05bd 100644
--- a/cmd_parser.c
+++ b/cmd_parser.c
@@ -27,7 +27,6 @@
 
 extern __code struct machine machine;
 extern __xdata uint8_t stpEnabled;
-extern __code uint8_t log_to_phys_port[9];
 
 extern volatile __xdata uint32_t ticks;
 extern volatile __xdata uint8_t sfr_data[4];
@@ -80,6 +79,9 @@ __xdata uint16_t cmd_history_ptr;
 // Error set by commands
 __xdata uint8_t err_status;
 
+__xdata uint16_t atoi_results_short;
+__xdata uint8_t atoi_results_u8;
+
 inline uint8_t isletter(uint8_t l)
 {
 	// return (l >= 'a' && l <= 'z') || (l >= 'A' && l <= 'Z');
@@ -111,9 +113,9 @@ uint8_t cmd_compare(uint8_t start, __code uint8_t * cmd)
 		uint8_t c = cmd[j];
 		uint8_t b = cmd_buffer[i];
 
-		// cmd is garanteerd to be NULL-terminated.
-        if (c == '\0') {
-            if ((b == ' ') || (b == '\0')) {
+		// cmd is guaranteed to be NUL-terminated.
+        if (c == NUL) {
+            if ((b == ' ') || (b == NUL)) {
 				// Match
                 return 1;
             }
@@ -145,7 +147,7 @@ uint8_t atoi_hex(uint8_t idx)
 	while(1) {
 		c = cmd_buffer[idx];
 
-		if (c == '\0' || c == ' ') {
+		if (c == NUL || c == ' ') {
 			break;
 		}
 
@@ -185,61 +187,177 @@ uint8_t atoi_hex(uint8_t idx)
 }
 
 
-uint8_t atoi_byte(__xdata uint8_t *out, uint8_t idx)
+// Returns 0 or the number of digits taken into account for conversion.
+// Stops at any non-digit '0'-'9' char or more than 3 bytes.
+uint8_t atoi_byte(uint8_t idx)
 {
-	uint8_t err = 1;
+	uint8_t cnt = 0;
 	uint8_t num = 0;
 
-	while (isnumber(cmd_buffer[idx])) {
-		uint8_t val = cmd_buffer[idx] - '0';
-		err = 0;
-		if (num > 25 || (num == 25 && val > 5))
-			return 1;
+	uint8_t * ptr = &cmd_buffer[idx];
+
+	while (1) {
+		uint8_t val = *ptr++ - '0';
+		if (val > 9)
+			break;
+		if (num > 25 || (num == 25 && val > 5) || cnt >= 3)
+			return 0;
 		num = (num * 10) + val;
-		idx++;
+		cnt++;
 	}
 
-	*out = num;
-	return err;
+	atoi_results_u8 = num;
+	return cnt;
 }
 
 
-uint8_t atoi_short(__xdata uint16_t *vlan, uint8_t idx)
+// Returns 0 or the number of digits taken into account for conversion.
+// Stops at any non-digit '0'-'9' char or bytes is more then 5.
+uint8_t atoi_short(uint8_t idx)
 {
-	uint8_t err = 1;
-	*vlan = 0;
+	uint8_t cnt = 0;
+	atoi_results_short = 0;
 
-	while (isnumber(cmd_buffer[idx])) {
-		err = 0;
-		uint8_t val = cmd_buffer[idx] - '0';
-		if (*vlan > 6553 || (*vlan == 6553 && val > 5))
-			return 1;
-		*vlan = (*vlan * 10) + val;
-		idx++;
+	uint8_t *ptr = &cmd_buffer[idx];
+
+	while (1) {
+		uint8_t val = *ptr++ - '0';
+		if (val > 9)
+			break;
+		if (atoi_results_short > 6553 || (atoi_results_short == 6553 && val > 5) || cnt >= 5)
+			return 0;
+		atoi_results_short = (atoi_results_short * 10) + val;
+		cnt++;
 	}
 
-	return err;
+	return cnt;
+}
+
+/* Parse, validate and translate phys_to_log_port physical port argument.
+ * The CPU-port, i.e. port 0, is not a valid argument.
+ * returns 0 when on parser error or invalid value.
+ * returns non-zero number of characters consumed.
+ * Store the value in atoi_results_u8.
+ */
+uint8_t cmd_parse_port(uint8_t idx) {
+	uint8_t port = cmd_buffer[idx] - '0' - 1;
+	if (port > 8)
+		return 0;
+
+	port = machine.phys_to_log_port[port];
+	if (port < machine.min_port || port > machine.max_port)
+		return 0;
+
+	atoi_results_u8 = port;
+	return 1;
 }
 
 
+// Same as cmd_parse_port() but additionally check for trailing SPACE or NUL.
+// returns 0 when on parser error or invalid value or no SPACE or no NUL.
+// returns non-zero number of characters consumed including the SPACE.
+uint8_t cmd_parse_port_separator(uint8_t idx) {
+	uint8_t ret = cmd_parse_port(idx);
+	if (ret != 0) {
+		idx += ret;
+		uint8_t c = cmd_buffer[idx];
+		if (c == ' ') {
+			ret++;
+		} else if (c != NUL)
+			ret = 0;
+	}
+	return ret;
+}
+
+// Same as cmd_parse_port_separator() but additionally allow CPU-port.
+// returns 0 when on parser error or invalid value or no SPACE or no NUL.
+// returns number of characters consumed including the SPACE.
+uint8_t cmd_parse_port_cpu_separator(uint8_t idx) {
+	uint8_t ret = cmd_parse_port(idx);
+
+	if (ret == 0 && cmd_buffer[idx] == '0') {
+		ret = 1;
+		atoi_results_u8 = CPU_PORT;
+	}
+
+	if (ret != 0) {
+		idx += ret;
+		uint8_t c = cmd_buffer[idx];
+		if (c == ' ') {
+			ret++;
+		} else if (c != NUL)
+			ret = 0;
+	}
+	return ret;
+}
+
+
+// check if the cmd_buffer[idx] is a SPACE.
+__bit cmd_is_space(uint8_t idx) {
+	return cmd_buffer[idx] == ' ';
+}
+
+// check if the cmd_buffer[idx] is a SPACE or NUL.
+__bit cmd_is_space_or_nul(uint8_t idx) {
+	uint8_t c = cmd_buffer[idx];
+	return c == ' ' || c == NUL;
+}
+
+// Parse an IPv4 address
+// returns 0 when on parse error or invalid value or it don't ends with SPACE or NUL.
+// returns non-zero number of characters consumed including the SPACE.
 uint8_t parse_ip(uint8_t idx)
 {
-	__xdata uint8_t b;
+	uint8_t b = 0;
+	uint8_t ret;
+	uint8_t idx_start = idx;
 
-	for (b = 0; b < 4; b++) {
-		ip[b] = 0;
-		while (isnumber(cmd_buffer[idx])) {
-			ip[b] = (ip[b] * 10) + cmd_buffer[idx] - '0';
-			idx++;
-		}
-		if (b < 3 && cmd_buffer[idx++] != '.') {
-			print_string("Error in IP format, expecting '.'\n");
-			return -1;
+	while(1) {
+		ret = atoi_byte(idx);
+		if (ret == 0)
+			goto err;
+
+		idx += ret;
+		ip[b++] = atoi_results_u8;
+
+		ret = cmd_buffer[idx];
+		if (b == 4) {
+			if (ret == ' ') {
+				idx++;
+				break;
+			}
+			if (ret == NUL)
+				break;
+			goto err;
 		}
+		idx++;
+
+		if (ret != '.')
+			goto err;
 	}
+	return idx - idx_start;
+
+err:
+	print_string("Error in IP format\n");
 	return 0;
 }
 
+// Prints an IPv4 address.
+void print_ip(__xdata uint8_t * ptr)
+{
+	uint8_t idx = 0;
+	uint8_t num;
+
+	while(1) {
+		num = *ptr++;
+		itoa(num);
+		if (++idx == 4)
+			break;
+
+		write_char('.');
+	}
+}
+
 
 void parse_lag(void)
 {
@@ -258,7 +376,7 @@ void parse_lag(void)
 			print_string(" member ports: ");
 			for (uint8_t j = 0; j < 10; j++) {
 				if (members & 1) {
-					write_char('0' + machine.log_to_phys_port[j]);
+					print_phys_port(j);
 					write_char(' ');
 				}
 				members >>= 1;
@@ -271,30 +389,26 @@ void parse_lag(void)
 		return;
 	}
 
-	if (cmd_words_len < 2 || !isnumber(cmd_buffer[cmd_words_b[1]]))
+	if (cmd_words_len < 2)
 		goto err;
-	group = cmd_buffer[cmd_words_b[1]] - '1';
-	if (group > 3)		/* '0' wraps well past three, so one test does both ends */
+
+	// Parse group, expect only one number 0-9;
+	if (atoi_byte(cmd_words_b[1]) != 1)
+		goto err;
+
+	group = atoi_results_u8 - 1;
+	if (group > 3)
 		goto err;
 
 	uint8_t w = 2;
 	while (w < cmd_words_len) {
 //		write_char('|'); print_byte(w); write_char(':'); write_char(cmd_buffer[cmd_words_b[w]]); write_char('-');
-		uint8_t port;
-		if (isnumber(cmd_buffer[cmd_words_b[w]])) {
-			port = cmd_buffer[cmd_words_b[w]] - '1';
-			if (isnumber(cmd_buffer[cmd_words_b[w] + 1]))
-				port = (port + 1) * 10 + cmd_buffer[cmd_words_b[w] + 1] - '1';
-			if (port > 8)	/* phys_to_log_port holds nine entries */
-				goto err;
-			port = machine.phys_to_log_port[port];
-		} else {
+
+		// Parse port.
+		if (cmd_parse_port_separator(cmd_words_b[w++]) == 0)
 			goto err;
-		}
-		if (port > machine.max_port)
-			goto err;
-		members |= ((uint16_t)1) << port;
-		w++;
+
+		members |= ((uint16_t)1) << atoi_results_u8;
 	}
 	port_lag_members_set(group, members);
 	return;
@@ -305,15 +419,15 @@ err:
 
 void parse_lag_hash(void)
 {
-	__xdata uint8_t group;
-	__xdata uint8_t hash = 0;
-
-	if (cmd_words_len < 2 || !isnumber(cmd_buffer[cmd_words_b[1]]))
-		goto err;
-	group = cmd_buffer[cmd_words_b[1]] - '1';
-	if (group > 3)		/* '0' wraps well past three, so one test does both ends */
+	// Parse group, expect only one number 0-9.
+	if (cmd_words_len < 3 || atoi_byte(cmd_words_b[1]) != 1)
 		goto err;
 
+	uint8_t group = atoi_results_u8 - 1;
+	if (group > 3)
+		goto err;
+
+	uint8_t hash = 0;
 	uint8_t w = 2;
 	while (w < cmd_words_len) {
 		if (cmd_compare(w, "spa"))
@@ -334,13 +448,14 @@ void parse_lag_hash(void)
 			print_string("Error: invalid hash type:");
 			print_string_x(&cmd_buffer[cmd_words_b[w]]);
 			write_char('\n');
+			goto err;
 		}
 		w++;
 	}
 	port_lag_hash_set(group, hash);
 	return;
 err:
-	print_string("Error: lag hash <1-4> [type]...\n");
+	print_string("Error: laghash <1-4> [smac|dmac|sip|dip|sport|dport]\n");
 }
 
 
@@ -349,16 +464,17 @@ void parse_vlan(void)
 	vlan_settings.vlan = 0;
 	vlan_settings.members = 0;
 	vlan_settings.tagged = 0;
+
 	if (cmd_words_len < 2)
 		goto err;
-	if (!atoi_short(&vlan_settings.vlan, cmd_words_b[1])) {
-		if (cmd_words_len == 3 && cmd_buffer[cmd_words_b[2]] == 'd') {
-			vlan_delete(vlan_settings.vlan);
-			return;
-		}
+
+	// Parse the VLAN number
+	if (atoi_short(cmd_words_b[1]) != 0) {
+		if (atoi_results_short > 4094)
+			goto err;
+		vlan_settings.vlan = atoi_results_short;
+
 		if (cmd_compare(2, "mgmt")) {
-			if (vlan_settings.vlan > 4094)
-				goto err;
 			management_vlan = vlan_settings.vlan;
 			if (!vlan_settings.vlan)
 				print_string("Management VLAN disabled\n");
@@ -366,41 +482,51 @@ void parse_vlan(void)
 				print_string("Management VLAN set to "); print_short(management_vlan); write_char('\n');
 			return;
 		}
-		if (!vlan_settings.vlan || vlan_settings.vlan > 4094)
+
+		// Other commands vlan 0 is invalid
+		if (!vlan_settings.vlan)
 			goto err;
+
+		if (cmd_words_len == 3 && cmd_buffer[cmd_words_b[2]] == 'd') {
+			vlan_delete(vlan_settings.vlan);
+			return;
+		}
+
 		uint8_t w = 2;
 		if (cmd_words_len > w && isletter(cmd_buffer[cmd_words_b[w]])) {
-			register uint8_t i = 0;
+			uint8_t i = 0;
 			vlan_name_remove(vlan_settings.vlan);
 			vlan_names[vlan_ptr++] = hex[(vlan_settings.vlan >> 8) & 0xf];
 			vlan_names[vlan_ptr++] = hex[(vlan_settings.vlan >> 4) & 0xf] ;
 			vlan_names[vlan_ptr++] = hex[vlan_settings.vlan & 0xf];
-			while(cmd_buffer[cmd_words_b[w] + i] != ' ' && cmd_buffer[cmd_words_b[w] + i] != '\0') {
+			while(!cmd_is_space_or_nul(cmd_words_b[w] + i)) {
 				write_char(cmd_buffer[cmd_words_b[w] + i]);
 				vlan_names[vlan_ptr++] = cmd_buffer[cmd_words_b[w] + i++];
 			}
-			vlan_names[vlan_ptr++] = ' '; vlan_names[vlan_ptr] = '\0';
+			vlan_names[vlan_ptr++] = ' '; vlan_names[vlan_ptr] = NUL;
 			w++;
 			print_string("<\n");
 		}
+
+		uint8_t ret;
+		uint8_t idx;
 		while (cmd_words_len > w) {
-			__xdata uint8_t port;
-			if (isnumber(cmd_buffer[cmd_words_b[w]])) {
-				port = cmd_buffer[cmd_words_b[w]] - '1';
-				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')
-						vlan_settings.tagged |= ((uint16_t)1) << port;
-				} else {
-						port = machine.phys_to_log_port[port];
-					if (cmd_buffer[cmd_words_b[w] + 1] == 't')
-						vlan_settings.tagged |= ((uint16_t)1) << port;
-				}
-				if (port > machine.max_port)
-					goto err;
-				vlan_settings.members |= ((uint16_t)1) << port;
+			idx = cmd_words_b[w++];
+			ret = cmd_parse_port(idx);
+			if (ret == 0)
+				goto err;
+
+			idx += ret;
+			uint16_t pmask = ((uint16_t)1) << atoi_results_u8;
+			vlan_settings.members |= pmask;
+
+			if (cmd_buffer[idx] == 't') {
+				vlan_settings.tagged |= pmask;
+				idx++;
 			}
-			w++;
+
+			if (!cmd_is_space_or_nul(idx))
+				goto err;
 		}
 		vlan_create();
 	} else if (cmd_compare(1, "show")) {
@@ -421,19 +547,16 @@ err:
 
 void parse_isolate(void)
 {
-	__xdata uint16_t members = 0;
+	uint16_t members = 0;
 
 	if (cmd_words_len < 3)
 		goto err;
 
 	print_string("\nISOLATE ");
 
-	if (!isnumber(cmd_buffer[cmd_words_b[1]]) || cmd_buffer[cmd_words_b[1]] == '0'
-	    || isnumber(cmd_buffer[cmd_words_b[1] + 1]))
-		goto err;
-	__xdata uint8_t port_configured = machine.phys_to_log_port[cmd_buffer[cmd_words_b[1]] - '1'];
-	if (port_configured < machine.min_port || port_configured > machine.max_port)
+	if (cmd_parse_port_separator(cmd_words_b[1]) == 0)
 		goto err;
+	uint8_t port_configured = atoi_results_u8;
 
 	print_byte(port_configured); write_char('\n');
 
@@ -441,16 +564,13 @@ void parse_isolate(void)
 		members = port_isolation_get(port_configured);
 		for (uint8_t i = 0; i < 10; i++) {
 			if (members & 1) {
-				if (i < 9)
-					write_char(machine.log_to_phys_port[i] + '0');
-				else
-					print_string("CPU");
+				print_phys_port(i);
 				write_char(' ');
 			}
 			members >>= 1;
 		}
 		return;
-        }
+    }
 
 	if (cmd_compare(2, "off")) {
 		for (uint8_t i = machine.min_port; i < machine.max_port; i++)
@@ -462,22 +582,12 @@ void parse_isolate(void)
 
 	uint8_t w = 2;
 	while (w < cmd_words_len) {
-		__xdata uint8_t port;
-		if (isnumber(cmd_buffer[cmd_words_b[w]])) {
-			port = cmd_buffer[cmd_words_b[w]] - '1';
-			if (isnumber(cmd_buffer[cmd_words_b[w] + 1])) {
-				port = (port + 1) * 10 + cmd_buffer[cmd_words_b[w] + 1] - '1'; // logical port
-				if (port != 9) // CPU port is logical port 9
-					goto err;
-			} else {
-				port = machine.phys_to_log_port[port];
-				if (port < machine.min_port || port > machine.max_port)
-					goto err;
-			}
-			members |= ((uint16_t)1) << port;
-		}
-		w++;
+		if (cmd_parse_port_cpu_separator(cmd_words_b[w++]) == 0)
+			goto err;
+		uint8_t port = atoi_results_u8;
+		members |= ((uint16_t)1) << port;
 	}
+
 	port_isolate(port_configured, members);
 	return;
 
@@ -509,52 +619,54 @@ void parse_ingress(void)
 	if (cmd_words_len < 2) {
 		goto err;
 	}
-	__xdata uint8_t log_port = 0;
+	uint8_t log_port = 0;
 	__xdata vlan_ingress_mode_t mode = VLAN_INVALID;
+	uint8_t idx = cmd_words_b[1];
 
-	if (vlan_ingress_mode_parse(cmd_buffer[cmd_words_b[1]], &mode)) {
+	if (vlan_ingress_mode_parse(cmd_buffer[idx++], &mode)) {
+		if (!cmd_is_space_or_nul(idx))
+			goto err;
 		// Setting mode for all ports at once
 		for (log_port = machine.min_port; log_port <= machine.max_port; log_port++) {
 			if (!port_ingress_filter(log_port, mode)) {
-				print_string("Error setting ingress filter for port "); print_byte(machine.log_to_phys_port[log_port]); write_char('\n');
+				print_string("Error setting ingress filter for port "); print_phys_port(log_port); write_char('\n');
 				return;
 			}
 			print_string("All ports ingress filter set to: ");
 			print_port_ingress_filter_mode(mode); write_char('\n');
 		}
-		return;
 	} else {
 		for(uint8_t w = 1; w < cmd_words_len; w++) {
-			uint8_t p = cmd_buffer[cmd_words_b[w]];
-			if (!isnumber(p)) {
+			idx = cmd_words_b[w];
+			uint8_t ret = cmd_parse_port(idx);
+			if (ret != 1) {
+				print_string("Invalid physical port number\n");
 				continue;
 			}
-			if (p < '1') {
-				print_string("Invalid physical port number: "); write_char(p); write_char('\n');
-				continue;
-			}
-			log_port = machine.phys_to_log_port[p - '1'];
-			if (!vlan_ingress_mode_parse(cmd_buffer[cmd_words_b[w] + 1], &mode)) {
-				print_string("Invalid ingress mode for port "); write_char(p); print_string(" in ingress command\n");
+			log_port = atoi_results_u8;
+			idx += ret;
+
+			if (!vlan_ingress_mode_parse(cmd_buffer[idx++], &mode) || !cmd_is_space_or_nul(idx)) {
+				print_string("Invalid ingress mode for port "); print_phys_port(log_port); print_string(" in ingress command\n");
 				goto err;
 			}
 			if (!port_ingress_filter(log_port, mode)) {
-				print_string("Error setting ingress filter for port "); write_char(p); write_char('\n');
+				print_string("Error setting ingress filter for port "); print_phys_port(log_port); write_char('\n');
 				return;
 			}
-			print_string("Port "); write_char(p);
+			print_string("Port "); print_phys_port(log_port);
 			print_string(" ingress filter set to: ");
 			print_port_ingress_filter_mode(mode); write_char('\n');
 		}
-		return;	
 	}
+	return;
 err:
-	print_string("Error: ingress [p]<u/t/a>... \n");
+	print_string("Error: ingress [p]<u/t/a>...\n");
 }
 
 void parse_mirror(void)
 {
-	__xdata uint8_t mirroring_port;
+	__xdata uint8_t mirroring_port = 0;
 	__xdata uint16_t rx_pmask = 0;
 	__xdata uint16_t tx_pmask = 0;
 
@@ -567,7 +679,7 @@ void parse_mirror(void)
 			print_string("NOT Enabled: ");
 		}
 		print_string("Mirroring port: ");
-		write_char('0' + machine.log_to_phys_port[mPort >> 1]);
+		print_phys_port(mirroring_port);
 		reg_read_m(RTL837x_MIRROR_CONF);
 		uint16_t m = sfr_data[0];
 		m = (m << 8) | sfr_data[1];
@@ -584,48 +696,38 @@ void parse_mirror(void)
 		return;
 	}
 
-	if (cmd_words_len < 2 || !isnumber(cmd_buffer[cmd_words_b[1]])) {
-		print_string("Port/command missing: mirror [status/off/<mirroring port> [port][t/r]]...\n");
-		return;
-	}
-
-	mirroring_port = cmd_buffer[cmd_words_b[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';
-	mirroring_port = machine.phys_to_log_port[mirroring_port];
-	
+	if (cmd_words_len < 2)
+		goto err;
 
 	uint8_t w = 2;
+	uint8_t port;
+	uint8_t ret;
 	while (w < cmd_words_len) {
-		uint8_t port;
-		if (isnumber(cmd_buffer[cmd_words_b[w]])) {
-			port = cmd_buffer[cmd_words_b[w]] - '1';
-			if (isnumber(cmd_buffer[cmd_words_b[w] + 1])) {
-				port = (port + 1) * 10 + cmd_buffer[cmd_words_b[w] + 1] - '1';
-				port = machine.phys_to_log_port[port];
-				if (cmd_buffer[cmd_words_b[w] + 2] == 'r')
-					rx_pmask |= ((uint16_t)1) << port;
-				else if (cmd_buffer[cmd_words_b[w] + 2] == 't')
-					tx_pmask |= ((uint16_t)1) << port;
-				else {
-					rx_pmask |= ((uint16_t)1) << port;
-					tx_pmask |= ((uint16_t)1) << port;
-				}
-			} else {
-				port = machine.phys_to_log_port[port];
-				if (cmd_buffer[cmd_words_b[w] + 1] == 'r')
-					rx_pmask |= ((uint16_t)1) << port;
-				else if (cmd_buffer[cmd_words_b[w] + 1] == 't')
-					tx_pmask |= ((uint16_t)1) << port;
-				else {
-					rx_pmask |= ((uint16_t)1) << port;
-					tx_pmask |= ((uint16_t)1) << port;
-				}
-			}
-		}
-		w++;
+		uint8_t idx = cmd_words_b[w++];
+		ret = cmd_parse_port(idx);
+		if (ret == 0)
+			goto err;
+
+		idx += ret;
+		port = atoi_results_u8;
+
+		// Use the first port argument as mirroring_port
+		if (w == 2)
+			mirroring_port = port;
+
+		ret = cmd_buffer[idx];
+		uint16_t pmask = ((uint16_t)1) << port;
+		if (ret != 't')
+			rx_pmask |= pmask;
+		if (ret != 'r')
+			tx_pmask |= pmask;
 	}
 	port_mirror_set(mirroring_port, rx_pmask, tx_pmask);
+	return;
+
+err:
+	print_string("Port/command missing: mirror [status/off/<mirroring port> [port][t/r]]...\n");
+	return;
 }
 
 
@@ -639,16 +741,11 @@ void parse_port(void)
 		return;
 	}
 
-	if (cmd_buffer[cmd_words_b[1]] < '1' || cmd_buffer[cmd_words_b[1]] > '9' || cmd_buffer[cmd_words_b[1] + 1] != ' ' ) {
-		print_string("Illegal port number\n");
-		return;
-	}
-	phy_settings.port = cmd_buffer[cmd_words_b[1]] - '1';
-	phy_settings.port = machine.phys_to_log_port[phy_settings.port];
-	if (phy_settings.port > machine.max_port || phy_settings.port < machine.min_port) {
-		print_string("This machine has no port with the specified number\n");
+	if (cmd_parse_port_separator(cmd_words_b[1]) == 0) {
+		print_string("Invalid port number\n");
 		return;
 	}
+	phy_settings.port = atoi_results_u8;
 
 	print_string("Logical Port: "); print_byte(phy_settings.port); write_char('\n');
 	phy_settings.duplex = PHY_DUPLEX_BOTH;
@@ -661,11 +758,11 @@ void parse_port(void)
 		}
 	} else if (cmd_compare(2, "name")) {
 		uint8_t i = 0;
-		while ( (i < PORT_NAME_SIZE-1) && (cmd_buffer[cmd_words_b[3] + i] != '\0') ) {
+		while ( (i < PORT_NAME_SIZE-1) && (cmd_buffer[cmd_words_b[3] + i] != NUL) ) {
 			port_names[phy_settings.port][i] = cmd_buffer[cmd_words_b[3] + i];
 			i++;
 		}
-		port_names[phy_settings.port][i] = '\0';
+		port_names[phy_settings.port][i] = NUL;
 		print_string("\nName set to: \"");
 		print_string_x(port_names[phy_settings.port]);
 		print_string("\"\n");
@@ -730,47 +827,59 @@ void parse_port(void)
 
 void parse_mtu(void)
 {
-	__xdata uint16_t mtu;
 	uint8_t p;
 
 	if (cmd_compare(1, "show")) {
 		for (p = machine.min_port; p <= machine.max_port; p++) {
 			reg_read_m(RTL8373_REG_MAC_L2_PORT_MAX_LEN + ((uint16_t) p << 8));
-			mtu = SFR_DATA_U16 & 0x3fff;
-			print_string("Port "); print_byte(machine.log_to_phys_port[p]);
+			uint16_t mtu = SFR_DATA_U16 & 0x3fff;
+			print_string("Port "); print_phys_port(p);
 			write_char(' '); print_short(mtu); write_char('\n');
 		}
 		return;
 	}
-	if (cmd_words_len != 3 || cmd_buffer[cmd_words_b[1]] < '1'
-	    || cmd_buffer[cmd_words_b[1]] > '9'
-	    || cmd_buffer[cmd_words_b[1] + 1] > ' ') {
-		print_string("mtu [port] [size]\n");
-		return;
-	}
-	p = machine.phys_to_log_port[cmd_buffer[cmd_words_b[1]] - '1'];
+	if (cmd_words_len != 3)
+		goto err;
+
+	if (cmd_parse_port_separator(cmd_words_b[1]) == 0)
+		goto err;
+
+	p = atoi_results_u8;
 	print_byte(p);
-	if (atoi_short(&mtu, cmd_words_b[2]) || mtu < 64 || mtu > 0x3fff) {
+
+	if (atoi_short(cmd_words_b[2]) == 0 || atoi_results_short < 64 || atoi_results_short > 0x3fff) {
 		print_string("MTU must be 64..16383\n");
 		return;
 	}
-	REG_WRITE(RTL8373_REG_MAC_L2_PORT_MAX_LEN + ((uint16_t) p << 8), (mtu >> 10) & 0xf, (mtu >> 2) & 0xff,
-		  ((mtu & 0x3) << 6) | ((mtu >> 8) & 0x3f), mtu & 0xff);
+	REG_WRITE(RTL8373_REG_MAC_L2_PORT_MAX_LEN + ((uint16_t) p << 8), (atoi_results_short >> 10) & 0xf, (atoi_results_short >> 2) & 0xff,
+		  ((atoi_results_short & 0x3) << 6) | ((atoi_results_short >> 8) & 0x3f), atoi_results_short & 0xff);
 	write_char('\n');
+	return;
+
+err:
+	print_string("mtu [port] [size]\n");
+	return;
 }
 
-void sfp_print_measurements(uint8_t sfp)
+bool sfp_print_measurements(uint8_t sfp)
 {
-	print_string("Options: "); print_byte(sfp_read_reg(sfp, 92)); write_char('\n');
+	if (!sfp_read_block(sfp, 92, 1))
+		return false;
+
+	print_string("Options: "); print_byte(sfp_buf[0]); write_char('\n');
 	if (!(sfp_options[sfp] & 0x40))
-		return;
-	print_string("Temp: "); print_byte(sfp_read_reg(sfp, 224)); print_byte(sfp_read_reg(sfp, 225)); write_char('\n');
-	print_string("Vcc: "); print_byte(sfp_read_reg(sfp, 226)); print_byte(sfp_read_reg(sfp, 227)); write_char('\n');
-	print_string("TX Bias: "); print_byte(sfp_read_reg(sfp, 228)); print_byte(sfp_read_reg(sfp, 229)); write_char('\n');
-	print_string("TX Power: "); print_byte(sfp_read_reg(sfp, 230)); print_byte(sfp_read_reg(sfp, 231)); write_char('\n');
-	print_string("RX Power: "); print_byte(sfp_read_reg(sfp, 232)); print_byte(sfp_read_reg(sfp, 233)); write_char('\n');
-	print_string("Laser: "); print_byte(sfp_read_reg(sfp, 234)); print_byte(sfp_read_reg(sfp, 235)); write_char('\n');
-	print_string("State: "); print_byte(sfp_read_reg(sfp, 238)); write_char('\n');
+		return true;
+	if (!sfp_read_block(sfp, 224, 16))
+		return false;
+	print_string("Temp: "); print_byte(sfp_buf[0]); print_byte(sfp_buf[1]); write_char('\n');
+	print_string("Vcc: "); print_byte(sfp_buf[2]); print_byte(sfp_buf[3]); write_char('\n');
+	print_string("TX Bias: "); print_byte(sfp_buf[4]); print_byte(sfp_buf[5]); write_char('\n');
+	print_string("TX Power: "); print_byte(sfp_buf[6]); print_byte(sfp_buf[7]); write_char('\n');
+	print_string("RX Power: "); print_byte(sfp_buf[8]); print_byte(sfp_buf[9]); write_char('\n');
+	print_string("Laser: "); print_byte(sfp_buf[10]); print_byte(sfp_buf[11]); write_char('\n');
+	print_string("State: "); print_byte(sfp_buf[14]); write_char('\n');
+
+	return true;
 }
 
 
@@ -788,19 +897,26 @@ void parse_sfp(void)
 				print_string(" - empty\n");
 				continue;
 			}
-			print_string(" - Rate: "); print_byte(sfp_read_reg(slot, 12));
-			print_string("  Encoding: "); print_byte(sfp_read_reg(slot, 11));
+			if (!sfp_read_block(slot, 11, 2)) {
+				print_string(" - I2C read failed on this slot\n");
+				continue;
+			}
+			print_string(" - Rate: "); print_byte(sfp_buf[1]);
+			print_string("  Encoding: "); print_byte(sfp_buf[0]);
 			write_char('\n');
-			sfp_print_info(slot);
-			sfp_print_measurements(slot);
+			if (!sfp_print_info(slot) || !sfp_print_measurements(slot))
+				print_string("I2C read failed on this slot\n");
 		}
 		return;
 	}
-	if (cmd_buffer[cmd_words_b[1]] < '1' || cmd_buffer[cmd_words_b[1]] > '2' || cmd_buffer[cmd_words_b[1] + 1] != ' ' ) {
+	uint8_t idx = cmd_words_b[1];
+	uint8_t ret = atoi_byte(idx);
+	idx += ret;
+	slot = atoi_results_u8 - 1;
+	if (ret == 0 || !cmd_is_space(idx) || slot > 1) {
 		print_string("Illegal SFP slot number\n");
 		return;
 	}
-	slot = cmd_buffer[cmd_words_b[1]] - '1';
 	if (slot >= machine.n_sfp) {
 		print_string("SFP slot not present\n");
 		return;
@@ -834,8 +950,6 @@ err:
 
 void parse_regget(void)
 {
-	uint16_t reg = 0;
-
 	if (cmd_words_len != 2) {
 		goto err;
 	}
@@ -846,7 +960,7 @@ void parse_regget(void)
 		goto err;
 	}
 
-	reg = hexvalue[0];
+	uint16_t reg = hexvalue[0];
 	if (hex_size == 2) {
 		reg <<= 8;
 		reg |= hexvalue[1];
@@ -869,8 +983,6 @@ err:
 
 void parse_regset(void)
 {
-	uint16_t reg = 0;
-
 	if (cmd_words_len != 3) {
 		goto err;
 	}
@@ -880,7 +992,7 @@ void parse_regset(void)
 		goto err;
 	}
 
-	reg = hexvalue[0];
+	uint16_t reg = hexvalue[0];
 	if (hex_size == 2) {
 		reg <<= 8;
 		reg |= hexvalue[1];
@@ -923,9 +1035,10 @@ void parse_sdsget(void)
 		goto err;
 	}
 
-	if (atoi_byte(&sds_id, cmd_words_b[1])) {
+	if (!atoi_byte(cmd_words_b[1])) {
 		goto err;
 	}
+	sds_id = atoi_results_u8;
 
 	hex_size = atoi_hex(cmd_words_b[2]);
 	if (hex_size != 1) {
@@ -967,9 +1080,10 @@ void parse_sdsset(void)
 		goto err;
 	}
 
-	if (atoi_byte(&sds_id, cmd_words_b[1])) {
+	if (!atoi_byte(cmd_words_b[1])) {
 		goto err;
 	}
+	sds_id = atoi_results_u8;
 
 	hex_size = atoi_hex(cmd_words_b[2]);
 	if (hex_size != 1) {
@@ -1022,13 +1136,16 @@ void parse_phyget(void)
 		goto err;
 	}
 
-	if (atoi_byte(&phy_id, cmd_words_b[1])) {
+	if (!atoi_byte(cmd_words_b[1])) {
 		goto err;
 	}
+	phy_id = atoi_results_u8;
 
-	if (atoi_byte(&dev_id, cmd_words_b[2])) {
+
+	if (!atoi_byte(cmd_words_b[2])) {
 		goto err;
 	}
+	dev_id = atoi_results_u8;
 
 	hex_size = atoi_hex(cmd_words_b[3]);
 	if (hex_size == 0 || hex_size > 2) {
@@ -1068,13 +1185,16 @@ void parse_physet(void)
 		goto err;
 	}
 
-	if (atoi_byte(&phy_id, cmd_words_b[1])) {
+	if (!atoi_byte(cmd_words_b[1])) {
 		goto err;
 	}
+	phy_id = atoi_results_u8;
 
-	if (atoi_byte(&dev_id, cmd_words_b[2])) {
+
+	if (!atoi_byte(cmd_words_b[2])) {
 		goto err;
 	}
+	dev_id = atoi_results_u8;
 
 	hex_size = atoi_hex(cmd_words_b[3]);
 	if (hex_size == 0 || hex_size > 2) {
@@ -1134,7 +1254,7 @@ void parse_rnd(void)
 
 void parse_passwd(void)
 {
-	// cmd_words_len can be more then 2 if a space in the password.
+	// cmd_words_len can be more then 2 if a SPACE in the password.
 	if (cmd_words_len >= 2) {
 		uint8_t i = cmd_words_b[1];		
 		uint8_t c = 0;
@@ -1142,8 +1262,8 @@ void parse_passwd(void)
 		do {
 			c = cmd_buffer[i++];
 			passwd[j++] = c;
-		} while (c != '\0' && j < 20);
-		passwd[j] = '\0';
+		} while (c != NUL && j < 20);
+		passwd[j] = NUL;
 		return;
 	}
 	print_string("Missing password\n");
@@ -1167,10 +1287,13 @@ void parse_eee(void)
 		if (cmd_buffer[idx] == 'g' || cmd_buffer[idx] == 'm') {
 			// Word 2 is a speed (e.g., "2g5", "100m", "1g")
 			speed_word = 2;
-		} else if (cmd_buffer[idx] == ' ' || cmd_buffer[idx] == '\0') {
+		} else if (cmd_is_space_or_nul(idx)) {
 			// Word 2 is a port number
-			port = cmd_buffer[cmd_words_b[2]] - '1';
-			port = machine.phys_to_log_port[port];
+			if (cmd_parse_port_separator(idx) == 0) {
+				print_string("Speed word invalid, use: [100m|1g|2g5]\n");
+				return;
+			}
+			port = atoi_results_u8;
 			// Check if word 3 is a speed
 			if (cmd_words_len >= 4)
 				speed_word = 3;
@@ -1213,17 +1336,12 @@ void parse_eee(void)
 
 void parse_bw(void)
 {
-	__xdata uint8_t port;
-	__xdata uint32_t bw = 0;
-
-	if (cmd_words_len < 2) // Check for at least 2 arguments
+	if (cmd_words_len < 3) // Check for at least 3 arguments
 		goto err;
 
-	port = cmd_buffer[cmd_words_b[2]] - '1';
-	if (port > 9)
+	if (cmd_parse_port_separator(cmd_words_b[2]) == 0)
 		goto err;
-
-	port = machine.phys_to_log_port[port];
+	uint8_t port = atoi_results_u8;
 
 	if (cmd_compare(1, "status")) {
 		bandwidth_status(port);
@@ -1233,8 +1351,13 @@ void parse_bw(void)
 	if (cmd_words_len < 4) // Check for at least 4 arguments
 		goto err;
 
+	// Ensure first argument is only `in` or `out`.
+	__bit is_in = cmd_compare(1, "in") != 0;
+	if (!(is_in || cmd_compare(1, "out")))
+		goto err;
+
 	if (cmd_compare(3, "drop")) {
-		if (cmd_compare(1, "in")) {
+		if (is_in) {
 			bandwidth_ingress_drop(port);
 			return;
 		}
@@ -1242,7 +1365,7 @@ void parse_bw(void)
 	}
 
 	if (cmd_compare(3, "fc")) {
-		if (cmd_compare(1, "in")) {
+		if (is_in) {
 			bandwidth_ingress_fc(port);
 			return;
 		}
@@ -1250,34 +1373,30 @@ void parse_bw(void)
 	}
 
 	if (cmd_compare(3, "off")) {
-		if (cmd_compare(1, "in")) {
+		if (is_in) {
 			bandwidth_ingress_disable(port);
-			return;
-		} else if (cmd_compare(1, "out")) {
+		} else  {
 			bandwidth_egress_disable(port);
-			return;
 		}
-		goto err;
+		return;
 	}
 
+	__xdata uint32_t bw = 0;
 	uint8_t hex_size = atoi_hex(cmd_words_b[3]);
-	if (hex_size == 0 || hex_size > 4) {
+	if (hex_size == 0 || hex_size > 4)
 		goto err;
-	}
+
 	uint8_t i = 0;
-	while (hex_size) {
+	do {
 		hex_size--;
 		*(((uint8_t *) &bw) + hex_size) = hexvalue[i++];
-	}
+	} while (hex_size);
 
-	if (cmd_compare(1, "in")) {
+	if (is_in) {
 		bandwidth_ingress_set(port, bw);
-	} else if (cmd_compare(1, "out")) {
-		bandwidth_egress_set(port, bw);
 	} else {
-		goto err;
+		bandwidth_egress_set(port, bw);
 	}
-
 	return;
 
 err:
@@ -1291,8 +1410,7 @@ void parse_syslog(void)
 		print_string("Current syslog status: ");
 		if (syslog_state.enabled) {
 			print_string("enabled, sending to ");
-			itoa(syslog_state.server_ip[0]); write_char('.'); itoa(syslog_state.server_ip[1]); write_char('.');
-			itoa(syslog_state.server_ip[2]); write_char('.'); itoa(syslog_state.server_ip[3]);
+			print_ip(syslog_state.server_ip);
 			write_char('\n');
 		} else {
 			print_string("disabled\n");
@@ -1307,10 +1425,9 @@ void parse_syslog(void)
 	} else if (cmd_compare(1, "ip")) {
 		if (cmd_words_len < 3) { // no additional arguemnt -> print current ip
 			print_string("Current syslog IP: ");
-			itoa(syslog_state.server_ip[0]); write_char('.'); itoa(syslog_state.server_ip[1]); write_char('.');
-			itoa(syslog_state.server_ip[2]); write_char('.'); itoa(syslog_state.server_ip[3]);
+			print_ip(syslog_state.server_ip);
 			return;
-		} else if (!parse_ip(cmd_words_b[2])) {
+		} else if (parse_ip(cmd_words_b[2]) != 0) {
 			uint8_t was_enabled = syslog_state.enabled;
 			if (was_enabled)
 				syslog_stop();
@@ -1350,7 +1467,7 @@ void cmd_tokenize(void) __banked
 	while(1) {
 		c = cmd_buffer[line_ptr];
 		
-		if (c == '\0') {
+		if (c == NUL) {
 			// Store the word count
 			cmd_words_len = word;
 			break;
@@ -1477,8 +1594,7 @@ void cmd_parser(void) __banked
 				dhcp_start();
 			} else if (cmd_words_len == 1) {
 				print_string("Current IP: ");
-				itoa(uip_hostaddr[0]); write_char('.'); itoa(uip_hostaddr[0] >> 8); write_char('.');
-				itoa(uip_hostaddr[1]); write_char('.'); itoa(uip_hostaddr[1] >> 8);
+				print_ip(uip_hostaddr);
 				if (dhcp_state.state == DHCP_LEASING) {
 					print_string(" (dhcp, renewal in sec: ");
 					print_short(dhcp_state.dhcp_timer);
@@ -1490,46 +1606,43 @@ void cmd_parser(void) __banked
 			} else {
 				if (dhcp_state.state)
 					dhcp_stop();
-				if (!parse_ip(cmd_words_b[1])) {
+				if (parse_ip(cmd_words_b[1]) != 0) {
 					uip_ipaddr(&uip_hostaddr, ip[0], ip[1], ip[2], ip[3]);
 					print_string("Setting ip: ");
-					itoa(ip[0]); write_char('.'); itoa(ip[1]); write_char('.');
-					itoa(ip[2]); write_char('.'); itoa(ip[3]); write_char('\n');
+					print_ip(ip); write_char('\n');
 				} else {
-					print_string("Invalid IP address\n");
-					print_string("Error: ip [<ip-address>|dhcp]\n");
-					print_string("  The dhcp option enables the dhcp client, calling ip without options prints the current IP\n");
-					print_string("  Calling with a valid IP address will stop any ongoing dhcp client and set the IP address\n");
+					print_string("Invalid IP address\n" \
+								 "Error: ip [<ip-address>|dhcp]\n" \
+								 "  The dhcp option enables the dhcp client, calling ip without options prints the current IP\n" \
+								 "  Calling with a valid IP address will stop any ongoing dhcp client and set the IP address\n");
 				}
 			}
 		} else if (cmd_compare(0, "gw")) {
 			if (cmd_words_len == 1) {
 				print_string("Current gw: ");
-				itoa(uip_draddr[0]); write_char('.'); itoa(uip_draddr[0] >> 8); write_char('.');
-				itoa(uip_draddr[1]); write_char('.'); itoa(uip_draddr[1] >> 8);
+				print_ip(uip_draddr); write_char('\n');
 			} else {
-				if (!parse_ip(cmd_words_b[1]))
+				if (parse_ip(cmd_words_b[1]) != 0) {
 					uip_ipaddr(&uip_draddr, ip[0], ip[1], ip[2], ip[3]);
-				else
-					print_string("Invalid IP address\n");
-				print_string("Setting gw: ");
-				itoa(ip[0]); write_char('.'); itoa(ip[1]); write_char('.');
-				itoa(ip[2]); write_char('.'); itoa(ip[3]);
+					print_string("Setting gw: ");
+					print_ip(ip); write_char('\n');
+				} else {
+					print_string("Invalid IP address\n" \
+								 "Error: gw <ip-address>\n");
+				}
 			}
-			write_char('\n');
 		} else if (cmd_compare(0, "netmask")) {
 			if (cmd_words_len == 1) {
 				print_string("Current netmask: ");
-				itoa(uip_netmask[0]); write_char('.'); itoa(uip_netmask[0] >> 8); write_char('.');
-				itoa(uip_netmask[1]); write_char('.'); itoa(uip_netmask[1] >> 8);
+				print_ip(uip_netmask); write_char('\n');
 			} else {
-				if (!parse_ip(cmd_words_b[1]))
+				if (parse_ip(cmd_words_b[1]) != 0) {
 					uip_ipaddr(&uip_netmask, ip[0], ip[1], ip[2], ip[3]);
-				else
+					print_string("Setting netmask: ");
+					print_ip(ip); write_char('\n');
+				} else {
 					print_string("Invalid IP address\n");
-				print_string("Setting netmask: ");
-				itoa(ip[0]); write_char('.'); itoa(ip[1]); write_char('.');
-				itoa(ip[2]); write_char('.'); itoa(ip[3]);
+				}
 			}
 			write_char('\n');
 		} else if (cmd_compare(0, "l2")) {
@@ -1559,13 +1672,13 @@ void cmd_parser(void) __banked
 				__xdata char *dst = hostname;
 				for (uint8_t hn = 0; hn < sizeof(hostname) - 1; hn++) {
 					uint8_t c = *hp++;
-					if (c == '\0' || c == '\r' || c == '\n')
+					if (c == NUL || c == '\r' || c == '\n')
 						break;
 					if (c < 0x20 || c > 0x7e || c == '"' || c == '\\')
 						c = '.';
 					*dst++ = c;
 				}
-				*dst = '\0';
+				*dst = NUL;
 			} else {
 				print_string("Error: hostname [name] - the name must not contain spaces\n");
 			}
@@ -1579,13 +1692,10 @@ void cmd_parser(void) __banked
 				stp_off();
 				stpEnabled = 0;
 			}
-		} else if (cmd_compare(0, "pvid") && cmd_words_len == 3) {
-			__xdata uint16_t pvid;
-			if (cmd_buffer[cmd_words_b[1]] >= '1'
-			    && cmd_buffer[cmd_words_b[1]] <= '9'
-			    && cmd_buffer[cmd_words_b[1] + 1] <= ' '
-			    && !atoi_short(&pvid, cmd_words_b[2]) && pvid && pvid <= 4094)
-				port_pvid_set(machine.phys_to_log_port[cmd_buffer[cmd_words_b[1]] - '1'], pvid);
+		} else if (cmd_compare(0, "pvid")) {
+			if (cmd_words_len == 3 && cmd_parse_port_separator(cmd_words_b[1]) != 0
+			    && atoi_short(cmd_words_b[2]) && atoi_results_short && atoi_results_short <= 4094)
+				port_pvid_set(atoi_results_u8, atoi_results_short);
 			else
 				print_string("Error: pvid <port> <1-4094>\n");
 		} else if (cmd_compare(0, "vlan")) {
@@ -1652,11 +1762,11 @@ void cmd_parser(void) __banked
 
 
 		if (save_cmd && cmd_words_len) {
-			// Find end of the cmd-buffer, looking for the NULL-byte.
+			// Find end of the cmd-buffer, looking for the NUL-byte.
 			uint8_t i = cmd_words_b[cmd_words_len - 1];
 			do {
 				i++;
-			} while(cmd_buffer[i] != '\0');
+			} while(cmd_buffer[i] != NUL);
 
 			// Copy last cmd-buffer to history.
 			cmd_history_ptr = (cmd_history_ptr + i) & CMD_HISTORY_MASK;
@@ -1704,7 +1814,7 @@ void execute_config(void) __banked
 		uint8_t c = 0;
 		do {
 			if (cmd_idx >= (CMD_BUF_SIZE - 1)) {
-				cmd_buffer[cmd_idx] = '\0';
+				cmd_buffer[cmd_idx] = NUL;
 				print_string("ERROR: Command too long: ");
 				print_string_x(cmd_buffer);
 				write_char('\n');
@@ -1713,7 +1823,7 @@ void execute_config(void) __banked
 			}
 			c = flash_buf[cfg_idx++];
 			if (c == 0 || c == '\n') {
-				cmd_buffer[cmd_idx] = '\0';
+				cmd_buffer[cmd_idx] = NUL;
 				if (cmd_idx) {
 					cmd_tokenize();
 					if (err_status != ERR_OK)
@@ -1749,7 +1859,7 @@ void execute_commands(__xdata uint8_t *p) __banked {
 	while (1) {
 		if (*p == 0 || *p == '\n' || *p == '\r') {
 			if (cmd_idx) {
-				cmd_buffer[cmd_idx] = '\0';
+				cmd_buffer[cmd_idx] = NUL;
 				cmd_tokenize();
 				if (err_status != ERR_OK)
 					return;
@@ -1762,7 +1872,7 @@ void execute_commands(__xdata uint8_t *p) __banked {
 			if (cmd_idx < (CMD_BUF_SIZE - 1)) {
 				cmd_buffer[cmd_idx++] = *p;
 			} else {
-				cmd_buffer[CMD_BUF_SIZE - 1] = '\0';
+				cmd_buffer[CMD_BUF_SIZE - 1] = NUL;
 				print_string("ERROR: Command too long: ");
 				print_string_x(cmd_buffer);
 				write_char('\n');
diff --git a/html/stat.js b/html/stat.js
index 2709eeb..0944d96 100644
--- a/html/stat.js
+++ b/html/stat.js
@@ -109,7 +109,7 @@ const mib_counters = [
 function getCounters(port) {
   var xhttp = new XMLHttpRequest();
   const popup = document.getElementById('popup');
-  xhttp.onreadystatechange = function() {
+  xhttp.onreadystatechange = function () {
     if (this.readyState == 4 && this.status == 200) {
       const s = JSON.parse(xhttp.responseText);
       console.log("Counters: ", JSON.stringify(s));
@@ -118,16 +118,16 @@ function getCounters(port) {
       console.log("Counter 0: ", BigInt(s[0]).toString(), " length: ", s.length);
       var c = 0;
       for (i = 0; i < mib_counters.length; i += 4) {
-        console.log(i, " ", mib_counters[i], ": ", mib_counters[i+1]);
+        console.log(i, " ", mib_counters[i], ": ", mib_counters[i + 1]);
         if (mib_counters[i] == "" && mib_counters[i + 1] == 8) {
           console.log("c " + i + ": continue");
           continue;
         }
-        var count = BigInt(s[i/4]);
-        if (mib_counters[i+1] == 8) {
+        var count = BigInt(s[i / 4]);
+        if (mib_counters[i + 1] == 8) {
           tableHtml += "<td>" + mib_counters[i] + "</td><td>" + count.toString() + "</td>";
           c += 1;
-        } else if (mib_counters[i+1] == 4) {
+        } else if (mib_counters[i + 1] == 4) {
           if (mib_counters[i] != "") {
             tableHtml += "<td>" + mib_counters[i] + "</td><td>" + (count >> 32n).toString() + "</td>";
             c += 1;
@@ -136,8 +136,8 @@ function getCounters(port) {
             tableHtml += "</tr> <tr>";
             c = 0;
           }
-          if (mib_counters[i+2] != "") {
-            tableHtml += "<td>" + mib_counters[i+2] + "</td><td>" + (count & 4294967295n).toString() + "</td>";
+          if (mib_counters[i + 2] != "") {
+            tableHtml += "<td>" + mib_counters[i + 2] + "</td><td>" + (count & 4294967295n).toString() + "</td>";
             c += 1;
           }
         }
@@ -161,26 +161,26 @@ function fillStats() {
     return;
   if (tbl.rows.length > 1) {
     for (let i = 0; i < numPorts; i++) {
-      console.log("Table Update row: " + i + " state " + pState[i] + " is " + linkS[pState[i] +1]);
-      tbl.rows[i+1].cells[2].innerHTML = linkText(pState[i]+1);
-      tbl.rows[i+1].cells[3].innerHTML = `${txG[i]}` + t('common_pkts');
-      tbl.rows[i+1].cells[4].innerHTML = `${txB[i]}` + t('common_pkts');
-      tbl.rows[i+1].cells[5].innerHTML = `${rxG[i]}` + t('common_pkts');
-      tbl.rows[i+1].cells[6].innerHTML = `${rxB[i]}` + t('common_pkts');
+      console.log("Table Update row: " + i + " state " + pState[i] + " is " + linkS[pState[i] + 1]);
+      tbl.rows[i + 1].cells[2].innerHTML = linkText(pState[i] + 1);
+      tbl.rows[i + 1].cells[3].innerHTML = `${txG[i]}` + t('common_pkts');
+      tbl.rows[i + 1].cells[4].innerHTML = `${txB[i]}` + t('common_pkts');
+      tbl.rows[i + 1].cells[5].innerHTML = `${rxG[i]}` + t('common_pkts');
+      tbl.rows[i + 1].cells[6].innerHTML = `${rxB[i]}` + t('common_pkts');
     }
   } else {
     for (let i = 0; i < numPorts; i++) {
       console.log("Table row: " + i);
       const tr = tbl.insertRow();
-      let td = tr.insertCell(); td.appendChild(document.createTextNode(t('common_port') + (i+1)));
+      let td = tr.insertCell(); td.appendChild(document.createTextNode(t('common_port') + (i + 1)));
       let portName = portNames[physToLogPort[i]] || '';
       td = tr.insertCell(); td.appendChild(document.createTextNode(portName));
-      td = tr.insertCell(); td.appendChild(document.createTextNode(linkText(pState[i]+1)));
+      td = tr.insertCell(); td.appendChild(document.createTextNode(linkText(pState[i] + 1)));
       td = tr.insertCell(); td.appendChild(document.createTextNode(`${txG[i]}` + t('common_pkts')));
-      td = tr.insertCell();td.appendChild(document.createTextNode(`${txB[i]}` + t('common_pkts')));
-      td = tr.insertCell();td.appendChild(document.createTextNode(`${rxG[i]}` + t('common_pkts')));
-      td = tr.insertCell();td.appendChild(document.createTextNode(`${rxB[i]}` + t('common_pkts')));
-      var button = '<button type="button" style="margin: 0 0 0 24px" onclick="getCounters(' + i + ');">' + t('stat_show') + '</button>';
+      td = tr.insertCell(); td.appendChild(document.createTextNode(`${txB[i]}` + t('common_pkts')));
+      td = tr.insertCell(); td.appendChild(document.createTextNode(`${rxG[i]}` + t('common_pkts')));
+      td = tr.insertCell(); td.appendChild(document.createTextNode(`${rxB[i]}` + t('common_pkts')));
+      var button = '<button type="button" style="margin: 0 0 0 24px" onclick="getCounters(' + (i + 1) + ');">' + t('stat_show') + '</button>';
       td = tr.insertCell(); td.innerHTML = button;
     }
   }
@@ -197,8 +197,8 @@ window.addEventListener('click', (event) => {
   }
 });
 
-window.addEventListener("load", function() {
-  update( () => {
+window.addEventListener("load", function () {
+  update(() => {
     update();
     fillStats();
     const stat = setInterval(fillStats, 1000);
diff --git a/httpd/httpd.c b/httpd/httpd.c
index d856ccd..411ba0c 100644
--- a/httpd/httpd.c
+++ b/httpd/httpd.c
@@ -41,6 +41,15 @@ __xdata uint32_t cont_addr;
 
 // HTTP header properties
 __xdata uint8_t boundary[72];
+
+// a client may split the request anywhere, including inside a boundary or a
+// part header, so a configuration upload is parsed only once it is complete;
+// sized for a full config sector plus the multipart framing around it
+#define CONFIG_UPLOAD_BUF (CONFIG_LEN + 384)
+__xdata uint8_t config_upload;
+__xdata uint8_t config_buf[CONFIG_UPLOAD_BUF];
+__xdata uint16_t cfg_pos, cfg_hdr, cfg_body, cfg_end, cfg_last;
+__xdata uint8_t cfg_bl;
 __xdata uint8_t *content_type = 0;
 __xdata uint8_t *session = 0;
 
@@ -77,6 +86,7 @@ inline uint8_t is_separator(uint8_t c)
 
 void httpd_init(void) __banked
 {
+	config_upload = 0; // xdata is not zeroed by the startup code
 	__xdata struct httpd_state * __xdata s = &(uip_conn->appstate);
 	// Start listening to port 80
 	uip_listen(HTONS(80));
@@ -109,8 +119,8 @@ bool is_word(__xdata uint8_t *xdata_str_p, __code uint8_t * __xdata code_str_p)
 		u = *xdata_str_p++;
 		c = *code_str_p++;
 
-		if (c == '\0') {
-			if (u != '\0' && u != ' ' && u != '\t' && u != ':' && u != '?' && u != '=' && u != '\n' && u != '\r')
+		if (c == NUL) {
+			if (u != NUL && u != ' ' && u != '\t' && u != ':' && u != '?' && u != '=' && u != '\n' && u != '\r')
 				return false;
 			return true;
 		}
@@ -130,8 +140,8 @@ bool is_url_word_x(__xdata uint8_t *uri_str_p, __xdata uint8_t *src_str_p)
 		u = *uri_str_p++;
 		s = *src_str_p++;
 
-		if (s == '\0') {
-			if (u != '\0' && u != ' ' && u != '\t' && u != ':' && u != '?' && u != '=' && u != '\n' && u != '\r')
+		if (s == NUL) {
+			if (u != NUL && u != ' ' && u != '\t' && u != ':' && u != '?' && u != '=' && u != '\n' && u != '\r')
 				return false;
 			return true;
 		}
@@ -173,9 +183,9 @@ bool is_word_x(__xdata uint8_t *lhs_str_p, __xdata uint8_t *rhs_str_p)
 		u = *lhs_str_p++;
 		c = *rhs_str_p++;
 
-		if (c == '\0') {
+		if (c == NUL) {
 			/* ';' separates cookies in a Cookie header, so it ends a value too. */
-			if (u != '\0' && u != ' ' && u != '\t' && u != ':' && u != '?' && u != '=' && u != '\n' && u != '\r' && u != ';')
+			if (u != NUL && u != ' ' && u != '\t' && u != ':' && u != '?' && u != '=' && u != '\n' && u != '\r' && u != ';')
 				return false;
 			return true;
 		}
@@ -315,6 +325,65 @@ void gen_random_bytes(__xdata uint8_t *b, uint8_t bytes)
 }
 
 
+/* 0: body incomplete, 1: configuration stored, 2: malformed */
+static uint8_t config_take(void)
+{
+	cfg_bl = strlen_x(boundary);
+
+	// the body is complete once the closing boundary has arrived
+	cfg_last = 0;
+	while (1) {
+		if (cfg_last + cfg_bl + 1 >= write_len)
+			return 0;
+		if (strstart_x(&config_buf[cfg_last], boundary)
+		    && strstart(&config_buf[cfg_last + cfg_bl], "--"))
+			break;
+		cfg_last++;
+	}
+
+	// every part lies ahead of the closing boundary, so it bounds the walk
+	cfg_pos = 0;
+	while (cfg_pos < cfg_last) {
+		if (!strstart_x(&config_buf[cfg_pos], boundary)) {
+			cfg_pos++;
+			continue;
+		}
+		cfg_hdr = cfg_pos + cfg_bl;
+		cfg_body = cfg_hdr;
+		while (1) {
+			if (cfg_body + 3 >= cfg_last)
+				return 2;
+			if (strstart(&config_buf[cfg_body], "\r\n\r\n"))
+				break;
+			cfg_body++;
+		}
+		cfg_end = cfg_body;
+		cfg_body += 4;
+		// reaching cfg_last is a match: the last part ends at the closing boundary
+		while (cfg_end < cfg_last && !strstart_x(&config_buf[cfg_end], boundary))
+			cfg_end++;
+		while (cfg_hdr + 8 < cfg_body) {
+			// the part carrying a filename holds the configuration
+			if (strstart(&config_buf[cfg_hdr], "filename")) {
+				// the payload plus its terminator must fit the sector
+				if (cfg_end - cfg_body + 1 > CONFIG_LEN)
+					return 2;
+				config_buf[cfg_end] = 0;
+				flash_region.addr = CONFIG_START;
+				flash_sector_erase();
+				flash_region.addr = CONFIG_START;
+				flash_region.len = cfg_end - cfg_body + 1;
+				flash_write_bytes(config_buf + cfg_body);
+				return 1;
+			}
+			cfg_hdr++;
+		}
+		cfg_pos = cfg_end;
+	}
+	return 2;
+}
+
+
 /*
  * Reads post data from the http stream and writes it into flash memory
  * Input: the current position in the TCP buffer (uip_appdata)
@@ -418,10 +487,10 @@ void handle_post(void)
 		// Find end of request path
 		while (*p && !is_separator(*p))
 			p++;
-		*p++ = '\0';
+		*p++ = NUL;
 
 		// Find end of request header
-		boundary[0] ='\0';
+		boundary[0] =NUL;
 		p = scan_header(p);
 		dbg_string("Boundary: >"); dbg_string_x(boundary); dbg_string("<\n");
 		if (!*p || !content_type) {
@@ -437,6 +506,7 @@ void handle_post(void)
 				return;
 			}
 			print_string("Firmware upload started.");
+			config_upload = 0;
 			uptr = FIRMWARE_UPLOAD_START;
 			verify_crc = 1;
 			max_upload = 1024576;
@@ -445,12 +515,10 @@ void handle_post(void)
 				send_unauthorized();
 				return;
 			}
-			dbg_string("Configuration upload, erasing config mem!\n");
-			uptr = CONFIG_START;
+			dbg_string("Configuration upload\n");
 			verify_crc = 0;
-			max_upload = 2048;
-			flash_region.addr = CONFIG_START;
-			flash_sector_erase();
+			config_upload = 1;
+			write_len = 0;
 		}
 		// Check for other POST requests, which are not multipart, below
 	} else {
@@ -482,7 +550,7 @@ void handle_post(void)
 			dbg_string("Password accepted!\n");
 			read_reg_timer(&last_session_use);
 			gen_random_bytes(session_id, SESSION_ID_LENGTH);
-			session_id[SESSION_ID_LENGTH] = '\0';
+			session_id[SESSION_ID_LENGTH] = NUL;
 			slen = strtox(outbuf, "HTTP/1.1 302 Found\r\nConnection: close\r\nLocation: index.html\r\n" \
 					      "Set-Cookie: session=");
 			for (register uint8_t i = 0; i < SESSION_ID_LENGTH; i++)
@@ -504,6 +572,32 @@ void handle_post(void)
 			send_bad_request();
 			return;
 		}
+		if (config_upload) {
+			cfg_pos = uip_len - (p - uip_appdata);
+			if (write_len + cfg_pos >= CONFIG_UPLOAD_BUF) {
+				print_string("Configuration too large, aborting.\n");
+				config_upload = 0;
+				s->tstate = TSTATE_NONE;
+				send_bad_request();
+				return;
+			}
+			memcpy(config_buf + write_len, p, cfg_pos);
+			write_len += cfg_pos;
+			uint8_t taken = config_take();
+
+			if (!taken) {
+				s->tstate = TSTATE_MULTIPART;
+				return;
+			}
+			config_upload = 0;
+			s->tstate = TSTATE_NONE;
+			if (taken == 2) {
+				send_bad_request();
+				return;
+			}
+			slen = strtox(outbuf, "HTTP/1.1 200 OK\r\nConnection: close\r\n\r\n");
+			return;
+		}
 		// We skip the intial parts as part of the header
 		do {
 			p = skip_boundary(p);
@@ -642,7 +736,7 @@ void httpd_appcall(void)
 		__xdata uint8_t *q = p;
 		while (*p && !is_separator(*p))
 			p++;
-		*p = '\0';
+		*p = NUL;
 		dbg_string_x(q);
 		dbg_char('\n');
 
@@ -664,16 +758,9 @@ void httpd_appcall(void)
 				parse_short(q + 15);
 				send_vlan(short_parsed);
 			} else if (is_word(q, "/counters.json")) {
-				/* The port is one raw character of the request line and
-				 * indexes a nine entry table, so bound it here instead
-				 * of trusting the client to have sent a digit. Anything
-				 * below '0' wraps well past eight, so the one test
-				 * covers both ends. */
 				uint8_t cport = q[20] - '0';
-				if (cport > 8)
+				if (send_counters(cport))
 					send_bad_request();
-				else
-					send_counters(cport);
 			} else if (is_word(q, "/eee.json")) {
 				send_eee();
 			} else if (is_word(q, "/bandwidth.json")) {
diff --git a/httpd/page_impl.c b/httpd/page_impl.c
index 4d3b90f..d55d4fe 100644
--- a/httpd/page_impl.c
+++ b/httpd/page_impl.c
@@ -177,10 +177,12 @@ void reg_to_html_long(register uint16_t reg)
 void send_sfp_info(uint8_t sfp)
 {
 	// This loops over the Vendor-name, Vendor OUI, Vendor PN and Vendor rev ASCII fields
-	for (uint8_t i = 20; i < 60; i++) {
-		if (i >= 36 && i < 40) // Skip Non-ASCII codes
+	for (uint8_t i = 16; i < 64; i++) {
+		if (!(i & 0xf))
+			sfp_read_block(sfp, i, 16);
+		if (i < 20 || i >= 60 || (i >= 36 && i < 40)) // Skip Non-ASCII codes
 			continue;
-		uint8_t c = sfp_read_reg(sfp, i);
+		uint8_t c = sfp_buf[i & 0xf];
 		if (c && c != 0xa0) // a0 is the byte read from a non-existant I2C EEPROM
 			char_to_html(c);
 	}
@@ -193,32 +195,10 @@ void sfp_send_data(uint8_t slot, uint8_t reg, uint8_t len)
 	if (len > 16)
 		return;
 
-	if (reg & 0x80) {	// Configure SFP readings address (0x51) as I2C device address
-		reg &= 0x7f;
-		REG_WRITE(RTL837X_REG_I2C_CTRL, 0x00, 0x1 << (I2C_MEM_ADDR_WIDTH-16) | (len - 1) & 0xf,  0x51 >> 5, (0x51 << 3) & 0xff);
-	} else {
-		REG_WRITE(RTL837X_REG_I2C_CTRL, 0x00, 0x1 << (I2C_MEM_ADDR_WIDTH-16) | (len - 1) & 0xf,  0x50 >> 5, (0x50 << 3) & 0xff);
-	}
+	sfp_read_block(slot, reg, len);
 
-	reg_read_m(RTL837X_REG_I2C_CTRL);
-	sfr_mask_data(1, 0xfc, i2c_bus_from_scl_pin(machine.sfp_port[slot].i2c.scl) << 5 | i2c_bus_from_sda_pin(machine.sfp_port[slot].i2c.sda) << 2);
-	reg_write_m(RTL837X_REG_I2C_CTRL);
-
-	REG_WRITE(RTL837X_REG_I2C_IN, 0, 0, 0, reg);
-
-	// Execute I2C Read
-	reg_bit_set(RTL837X_REG_I2C_CTRL, 0);
-
-	// Wait for execution to finish
-	do {
-		reg_read_m(RTL837X_REG_I2C_CTRL);
-	} while (sfr_data[3] & 0x1);
-
-	for (uint8_t i = 0; i < len; i++) {
-		if (!(i & 0x3))
-			reg_read_m(RTL837X_REG_I2C_OUT + i);
-		byte_to_html(sfr_data[3 - (i & 0x3)]);
-	}
+	for (uint8_t i = 0; i < len; i++)
+		byte_to_html(sfp_buf[i]);
 }
 
 
@@ -309,17 +289,26 @@ void send_vlan(uint16_t vlan)
 	slen += strtox(outbuf + slen, "\"}");
 }
 
-
-void send_counters(char port)
+/* Send counters
+ * Only accepts physical port 1..9.
+ * Returns an error if the port physical don't exists.
+ */
+bool send_counters(uint8_t phys_port)
 {
-	dbg_string("send_counters called: "); dbg_byte(port); dbg_char('\n');
+	uint8_t phys_port_idx = phys_port - 1;
+	if (phys_port_idx > 8)
+		goto err;
+	uint8_t log_port = machine.phys_to_log_port[phys_port_idx];
+	if (log_port == 0)
+		goto err;
+
+	dbg_string("send_counters called: "); dbg_byte(phys_port_idx); dbg_char('\n');
 	slen = strtox(outbuf, HTTP_RESPONCE_JSON);
-	dbg_string("sending counters\n");
-	dbg_byte(port);
-	uint8_t i = machine.phys_to_log_port[port];
-	slen += strtox(outbuf + slen, "[");
+	dbg_string("sending counters\n"); dbg_byte(phys_port_idx);
+
+	char_to_html('[');
 	for (uint8_t counter = 0; counter < 0x37; counter++) {
-		STAT_GET(counter, i);
+		STAT_GET(counter, log_port);
 		slen += strtox(outbuf + slen, "\"0x");
 		reg_to_html(RTL837X_STAT_V_HIGH);
 		reg_to_html_long(RTL837X_STAT_V_LOW);
@@ -328,6 +317,12 @@ void send_counters(char port)
 			char_to_html(',');
 	}
 	char_to_html(']');
+
+	return false;
+
+err:
+	dbg_string("Error: counters: phy_port_idx don't exists\n");
+	return true;
 }
 
 
@@ -822,7 +817,7 @@ found_end:
 	if (valid_len > (TCP_OUTBUF_SIZE - slen)) {
 		cont_len = valid_len - (TCP_OUTBUF_SIZE - slen);
 		valid_len = TCP_OUTBUF_SIZE - slen;
-		cont_addr = valid_len;
+		cont_addr = CONFIG_START + valid_len;
 	}
 	
 	flash_region.addr = CONFIG_START;
diff --git a/httpd/page_impl.h b/httpd/page_impl.h
index 7907285..02ed4eb 100644
--- a/httpd/page_impl.h
+++ b/httpd/page_impl.h
@@ -1,7 +1,9 @@
 #ifndef __PAGE_IMPL_H__
 #define __PAGE_IMPL_H__
 
-void send_counters(char port);
+#include <stdbool.h>
+
+bool send_counters(uint8_t phys_port);
 void send_status(void);
 void send_vlan(uint16_t vlan);
 void send_basic_info(void);
diff --git a/machine.c b/machine.c
index ecc46d2..7f4a512 100644
--- a/machine.c
+++ b/machine.c
@@ -56,8 +56,6 @@ __code const struct machine machine = {
 		    },
 };
 
-void machine_custom_init(void) { }
-
 #elif defined MACHINE_KP_9000_6XH_X
 __code const struct machine machine = {
 	.machine_name = "keepLink KP-9000-6XH-X",
@@ -86,8 +84,6 @@ __code const struct machine machine = {
 		    },
 };
 
-void machine_custom_init(void) { }
-
 #elif defined(MACHINE_KP_9000_6XH_X2) || defined(MACHINE_KP_9000_6XH_X2_V2_1) || defined(MACHINE_KP_9000_6XHML_X2_V2_1)
 __code const struct machine machine = {
 #if defined(MACHINE_KP_9000_6XHML_X2_V2_1)
@@ -142,10 +138,6 @@ __code const struct machine machine = {
 		},
 	};
 
-void machine_custom_init(void) {
-	reg_bit_set(RTL837X_REG_LED_GLB_IO_EN, 6);
-}
-
 #elif defined MACHINE_KP_9000_9XH_X_EU
 __code const struct machine machine = {
 	.machine_name = "keepLink KP-9000-9XH-X-EU",
@@ -171,8 +163,6 @@ __code const struct machine machine = {
 		    },
 };
 
-void machine_custom_init(void) { }
-
 #elif defined MACHINE_KP_9000_9XHML_X_V2_2
 __code const struct machine machine = {
 	.machine_name = "keepLink KP-9000-9XHML-X V2.2",
@@ -224,8 +214,6 @@ __code const struct machine machine = {
 	},
 };
 
-void machine_custom_init(void) { }
-
 #elif defined MACHINE_KP_9000_9XHML_X_V3_1
 __code const struct machine machine = {
 	.machine_name = "keepLink KP-9000-9XHML-X V3.1",
@@ -261,8 +249,6 @@ __code const struct machine machine = {
 				0x1a, 0x19, 0x1d, 0x1e, 0x1c, 0x1d, 0x20, 0x21},
 };
 
-void machine_custom_init(void) { }
-
 #elif defined MACHINE_SWGT024_V2_0_MANAGED
 __code const struct machine machine = {
 	.machine_name = "SWGT024 V2.0 Managed",
@@ -304,8 +290,6 @@ __code const struct machine machine = {
 	},
 };
 
-void machine_custom_init(void) { }
-
 #elif defined MACHINE_SWGT024_V2_0_UNMANAGED
 __code const struct machine machine = {
 	.machine_name = "SWGT024 V2.0 Unmanaged",
@@ -347,8 +331,6 @@ __code const struct machine machine = {
 	},
 };
 
-void machine_custom_init(void) { }
-
 #elif defined MACHINE_SWTG018AS_A_V_2_0
 __code const struct machine machine = {
 	.machine_name = "SWTG018AS-A V2.0",
@@ -388,8 +370,6 @@ __code const struct machine machine = {
 		     0x1d, 0x20, 0x21 },
 };
 
-void machine_custom_init(void) { }
-
 #elif defined MACHINE_HG0402XG_V1_1
 __code const struct machine machine = {
 	.machine_name = "HG0402XG V1.1",
@@ -428,8 +408,6 @@ __code const struct machine machine = {
 		    },
 };
 
-void machine_custom_init(void) { }
-
 #elif defined MACHINE_SWTGW218AS
 
 __code const struct machine machine = {
@@ -463,7 +441,48 @@ __code const struct machine machine = {
 				},
 };
 
-void machine_custom_init(void) { }
+#elif defined MACHINE_PCB_SWTG018AS_V2_1_0 // Sold as Sodola SL902 / Horaco "SWTGW218AS"; the SWTGW218AS label also covers other PCBs with different SFP and LED wiring (see MACHINE_SWTGW218AS)
+
+__code const struct machine machine = {
+	.machine_name = "SWTGW218AS (SWTG018AS-V2.1.0)",
+	.isRTL8373 = 1,
+	.mac_flash_offset = 0x1FC000,
+	.min_port = 0,
+	.max_port = 8,
+	.n_sfp = 1,
+	.log_to_phys_port = {1, 2, 3, 4, 5, 6, 7, 8, 9},
+	.phys_to_log_port = {0, 1, 2, 3, 4, 5, 6, 7, 8},
+	.is_sfp = {0, 0, 0, 0, 0, 0, 0, 0, 1},
+	.sfp_port[0].pin_detect = GPIO38,   // pulled low on module insert
+	.sfp_port[0].pin_los = GPIO_NA,     // no LOS pin wired
+	.sfp_port[0].pin_tx_disable = GPIO_NA,
+	.sfp_port[0].sds = 1,
+	.sfp_port[0].i2c = { .sda = GPIO39_I2C_SDA4, .scl = GPIO40_I2C_SCL3_MDC1 },
+	.reset_pin = GPIO54_ACL_BIT2_EN,
+	.high_leds = { .mux = LED_27 | LED_28_SYS | LED_29, .enable = LED_28_SYS | LED_29 },
+	.port_led_set = { 0, 0, 0, 0, 0, 0, 0, 0, 1},
+	// LED wiring matches the SWTG018AS-A V2.0 (same PCB family)
+	.led_sets = {
+		{   /* RJ45: First LED, yellow, second LED: green */
+			LEDS_2G5 | LEDS_LINK,
+			LEDS_2G5 | LEDS_1G | LEDS_100M | LEDS_10M | LEDS_LINK | LEDS_ACT,
+			0,
+			0,
+		}, { /* SFP set (superseded by the raw register override in machine_custom_init) */
+			LEDS_2G5 | LEDS_1G | LEDS_100M | LEDS_10M | LEDS_LINK | LEDS_ACT | LEDS_10G,
+			0,
+			0,
+			0,
+		}},
+	.led_mux_custom = 1,
+	.led_mux = { 0x00, 0x01, 0x04, 0x05, 0x08, // 65e0
+		     0x09, 0x0c, 0x09, 0x0d, 0x10, // 65e4
+		     0x11, 0x0e, 0x14, 0x11, 0x12, // 65e8
+		     0x15, 0x15, 0x16, 0x18, 0x19, // 65ec
+		     0x1a, 0x19, 0x1d, 0x1e, 0x1c, // 65f0
+		     0x1d, 0x20, 0x21 },
+};
+
 #elif defined MACHINE_LIANGUO_ZX_SWTGW215AS // Has PCB branded PCB-SWTG115AS-V2.0 but is labeled and reports as a ZX-SWTGW215AS, seems to be identical to the "real" ZX-SWTGW215AS except for the LEDs
 __code const struct machine machine = {
 	.machine_name = "Lianguo ZX-SWTGW215AS",
@@ -496,8 +515,6 @@ __code const struct machine machine = {
 	.led_mux_custom = 0,
 };
 
-void machine_custom_init(void) { }
-
 #elif defined MACHINE_DEFAULT_8C_1SFP
 __code const struct machine machine = {
 	.machine_name = "8+1 SFP Port Switch",
@@ -523,8 +540,6 @@ __code const struct machine machine = {
 		    },
 };
 
-void machine_custom_init(void) { }
-
 #elif defined MACHINE_TRENDNET_TEG_S562
 __code const struct machine machine = {
 	.machine_name = "Trendnet TEG-S562",
@@ -564,8 +579,6 @@ __code const struct machine machine = {
 
 };
 
-void machine_custom_init(void) { }
-
 #elif defined(MACHINE_PCB_K0402WS_V3) || defined(MACHINE_HI_K0402WS)  // Sold as a variety of devices, see doc/
 __code const struct machine machine = {
 	.machine_name = "PCB-K0402WS-V3.0",
@@ -612,10 +625,6 @@ __code const struct machine machine = {
 		},
 	};
 
-void machine_custom_init(void) { 
-	reg_bit_set(RTL837X_REG_LED_GLB_IO_EN, 6);
-}
-
 #elif defined MACHINE_K0501W_V2_0
 __code const struct machine machine = {
 	.machine_name = "K0501W V2.0",
@@ -650,8 +659,6 @@ __code const struct machine machine = {
 	 },
 };
 
-void machine_custom_init(void) { }
-
 #elif defined MACHINE_ZX310S_4T2XH
 __code const struct machine machine = {
 	.machine_name = "ZX310S-4T2XH",
@@ -695,8 +702,6 @@ __code const struct machine machine = {
 		    },
 };
 
-void machine_custom_init(void) { }
-
 #elif defined MACHINE_STEAMEMO_IG204_V1
 __code const struct machine machine = {
 	.machine_name = "Steamemo IG204 V1",
@@ -747,8 +752,6 @@ __code const struct machine machine = {
 	},
 };
 
-void machine_custom_init(void) { }
-
 #elif defined MACHINE_HI_K0801WS
 __code const struct machine machine = {
     .machine_name = "Hi-Source HI-k0801WS",
@@ -798,8 +801,6 @@ __code const struct machine machine = {
     },
 };
 
-void machine_custom_init(void) { }
-
 #elif defined MACHINE_FNS1200P
 __code const struct machine machine = {
     .machine_name = "FNS-1200P",
@@ -855,11 +856,6 @@ __code const struct machine machine = {
     },
 };
 
-void machine_custom_init(void)
-{
-    reg_bit_set(RTL837X_REG_LED_GLB_IO_EN, 6);
-}
-
 
 #elif defined MACHINE_PCB_SWTG024AS_A_2_0_1
 __code const struct machine machine = {
@@ -909,14 +905,6 @@ __code const struct machine machine = {
             },
     };
 
-void machine_custom_init(void)
-{
-    reg_bit_set(RTL837X_REG_LED_GLB_IO_EN, 6);
-    reg_bit_set(RTL837X_REG_LED_MODE, 17);
-    reg_bit_clear(RTL837X_REG_LED_MODE, 9);
-    reg_bit_clear(RTL837X_REG_LED_MODE, 7);
-}
-
 #elif defined MACHINE_SWTG024AS_A_2_0_1_5C_1SFP
 __code const struct machine machine = {
     .machine_name = "SWTG024AS-A-V2.0.1-5C-1SFP",
@@ -960,25 +948,6 @@ __code const struct machine machine = {
             },
     };
 
-void machine_custom_init(void)
-{
-    uint16_t pval;
-
-    reg_bit_set(RTL837X_REG_LED_GLB_IO_EN, 6);
-    reg_bit_set(RTL837X_REG_LED_MODE, 17);
-    reg_bit_clear(RTL837X_REG_LED_MODE, 9);
-    reg_bit_clear(RTL837X_REG_LED_MODE, 7);
-
-    // OEM firmware sets these companion SDS0 polarity bits for the RTL8221B.
-    sds_read(0, 0, 0);
-    pval = SFR_DATA_U16;
-    sds_write_v(0, 0, 0, pval | 0x100);
-
-    sds_read(0, 6, 2);
-    pval = SFR_DATA_U16;
-    sds_write_v(0, 6, 2, pval | 0x4000);
-}
-
 #elif defined MACHINE_SWTG024AS_V2_0
 __code const struct machine machine = {
     .machine_name = "SWTG024AS-V2.0",
@@ -1022,25 +991,6 @@ __code const struct machine machine = {
             },
     };
 
-void machine_custom_init(void)
-{
-    uint16_t pval;
-
-    reg_bit_set(RTL837X_REG_LED_GLB_IO_EN, 6);
-    reg_bit_set(RTL837X_REG_LED_MODE, 17);
-    reg_bit_clear(RTL837X_REG_LED_MODE, 9);
-    reg_bit_clear(RTL837X_REG_LED_MODE, 7);
-
-    // OEM firmware sets these companion SDS0 polarity bits for the RTL8221B.
-    sds_read(0, 0, 0);
-    pval = SFR_DATA_U16;
-    sds_write_v(0, 0, 0, pval | 0x100);
-
-    sds_read(0, 6, 2);
-    pval = SFR_DATA_U16;
-    sds_write_v(0, 6, 2, pval | 0x4000);
-}
-
 #elif defined MACHINE_ZX310S_4T2XT
 __code const struct machine machine = {
 	.machine_name = "ZX310S_4T2XT",
@@ -1078,12 +1028,6 @@ __code const struct machine machine = {
 		    },
 };
 
-void machine_custom_init(void) {
-	// For this device, the reset value of RTL837X_PIN_MUX_0 is 0x30000000,
-	// which would disables all LEDS, enable them manually:
-	REG_SET(RTL837X_PIN_MUX_0, 0x30db68bf);
-}
-
 #elif defined MACHINE_FG_4GT_2SX_V2_0
 __code const struct machine machine = {
 	.machine_name = "FG-4GT-2SX_V2.0",
@@ -1150,10 +1094,6 @@ __code const struct machine machine = {
 		},
 	};
 
-void machine_custom_init(void) {
-	REG_SET(RTL837X_REG_LED_GLB_IO_EN, 0x7624155b);
-}
-
 #else
 	#error "Please select a machine type in machine.h"
 #endif
diff --git a/machine.h b/machine.h
index 3e90d6e..9efa675 100644
--- a/machine.h
+++ b/machine.h
@@ -28,6 +28,7 @@
 // #define MACHINE_HG0402XG_V1_1
 // #define MACHINE_SWTG018AS_A_V_2_0
 // #define MACHINE_SWTGW218AS
+// #define MACHINE_PCB_SWTG018AS_V2_1_0
 // #define MACHINE_PCB_K0402WS_V3
 // #define MACHINE_K0501W_V2_0
 // #define MACHINE_LIANGUO_ZX_SWTGW215AS
@@ -104,6 +105,6 @@ typedef struct machine_runtime
 	uint8_t isN : 1;
 };
 
-void machine_custom_init(void);
+void machine_custom_init(void) __banked;
 
 #endif
diff --git a/machine_init.c b/machine_init.c
new file mode 100644
index 0000000..e11e7f2
--- /dev/null
+++ b/machine_init.c
@@ -0,0 +1,122 @@
+/*
+ * Per-machine one-shot boot hooks, hosted in BANK2 so board-specific
+ * tables and code do not consume the common bank.
+ */
+#include <stdint.h>
+#include "machine.h"
+#include "rtl837x_pins.h"
+#include "rtl837x_leds.h"
+#include "rtl837x_sfr.h"
+#include "rtl837x_regs.h"
+#include "rtl837x_common.h"
+
+#pragma codeseg BANK2
+#pragma constseg BANK2
+
+#if defined(MACHINE_KP_9000_6XH_X2) || \
+	defined(MACHINE_KP_9000_6XH_X2_V2_1) || \
+	defined(MACHINE_KP_9000_6XHML_X2_V2_1)
+void machine_custom_init(void) __banked
+{
+	reg_bit_set(RTL837X_REG_LED_GLB_IO_EN, 6);
+}
+
+#elif defined MACHINE_PCB_SWTG018AS_V2_1_0
+// Stock-firmware values for what the LED-set encoding cannot express: the
+// bi-color SFP LED (blue pin at 10G) and the PIN_MUX_0 routing of that pin
+// to the LED controller. Runs after leds_setup(), which covers the rest.
+static __code const struct { uint16_t reg; uint32_t val; } custom_init_regs[] = {
+	{ RTL837X_REG_LED3_0_SET1,   0x00100000UL },
+	{ RTL837X_REG_LED1_0_SET1,   0x01400155UL },
+	{ RTL837X_REG_LED1_0_SET0,   0x01740141UL },
+	{ RTL837X_REG_LED_GLB_IO_EN, 0x7f24977fUL },
+	{ RTL837X_PIN_MUX_0,         0x20db6880UL },
+};
+
+void machine_custom_init(void) __banked
+{
+	uint8_t i;
+	// REG_SET is a multi-statement macro without a do-while wrapper: braces required
+	for (i = 0; i < sizeof(custom_init_regs) / sizeof(custom_init_regs[0]); i++) {
+		REG_SET(custom_init_regs[i].reg, custom_init_regs[i].val);
+	}
+}
+
+#elif defined(MACHINE_PCB_K0402WS_V3) || defined(MACHINE_HI_K0402WS)
+void machine_custom_init(void) __banked
+{
+	reg_bit_set(RTL837X_REG_LED_GLB_IO_EN, 6);
+}
+
+#elif defined MACHINE_FNS1200P
+void machine_custom_init(void) __banked
+{
+    reg_bit_set(RTL837X_REG_LED_GLB_IO_EN, 6);
+}
+
+#elif defined MACHINE_PCB_SWTG024AS_A_2_0_1
+void machine_custom_init(void) __banked
+{
+    reg_bit_set(RTL837X_REG_LED_GLB_IO_EN, 6);
+    reg_bit_set(RTL837X_REG_LED_MODE, 17);
+    reg_bit_clear(RTL837X_REG_LED_MODE, 9);
+    reg_bit_clear(RTL837X_REG_LED_MODE, 7);
+}
+
+#elif defined MACHINE_SWTG024AS_A_2_0_1_5C_1SFP
+void machine_custom_init(void) __banked
+{
+    uint16_t pval;
+
+    reg_bit_set(RTL837X_REG_LED_GLB_IO_EN, 6);
+    reg_bit_set(RTL837X_REG_LED_MODE, 17);
+    reg_bit_clear(RTL837X_REG_LED_MODE, 9);
+    reg_bit_clear(RTL837X_REG_LED_MODE, 7);
+
+    // OEM firmware sets these companion SDS0 polarity bits for the RTL8221B.
+    sds_read(0, 0, 0);
+    pval = SFR_DATA_U16;
+    sds_write_v(0, 0, 0, pval | 0x100);
+
+    sds_read(0, 6, 2);
+    pval = SFR_DATA_U16;
+    sds_write_v(0, 6, 2, pval | 0x4000);
+}
+
+#elif defined MACHINE_SWTG024AS_V2_0
+void machine_custom_init(void) __banked
+{
+    uint16_t pval;
+
+    reg_bit_set(RTL837X_REG_LED_GLB_IO_EN, 6);
+    reg_bit_set(RTL837X_REG_LED_MODE, 17);
+    reg_bit_clear(RTL837X_REG_LED_MODE, 9);
+    reg_bit_clear(RTL837X_REG_LED_MODE, 7);
+
+    // OEM firmware sets these companion SDS0 polarity bits for the RTL8221B.
+    sds_read(0, 0, 0);
+    pval = SFR_DATA_U16;
+    sds_write_v(0, 0, 0, pval | 0x100);
+
+    sds_read(0, 6, 2);
+    pval = SFR_DATA_U16;
+    sds_write_v(0, 6, 2, pval | 0x4000);
+}
+
+#elif defined MACHINE_ZX310S_4T2XT
+void machine_custom_init(void) __banked
+{
+	// For this device, the reset value of RTL837X_PIN_MUX_0 is 0x30000000,
+	// which would disables all LEDS, enable them manually:
+	REG_SET(RTL837X_PIN_MUX_0, 0x30db68bf);
+}
+
+#elif defined MACHINE_FG_4GT_2SX_V2_0
+void machine_custom_init(void) __banked
+{
+	REG_SET(RTL837X_REG_LED_GLB_IO_EN, 0x7624155b);
+}
+
+#else
+void machine_custom_init(void) __banked { }
+#endif
diff --git a/rtl837x_common.h b/rtl837x_common.h
index 23108a3..432260e 100644
--- a/rtl837x_common.h
+++ b/rtl837x_common.h
@@ -8,6 +8,7 @@
 #define SYS_TICK_HZ 200
 
 #define CPU_PORT        9
+#define NUL			'\0'
 
 // Define Port-masks for 9-port devices and 6-port devices
 #define PMASK_9		0x1ff
@@ -135,6 +136,7 @@ void itoa(uint8_t v);
 void print_sfr_data(void);
 void print_phy_data(void);
 void print_cmd_prompt(void);
+void print_phys_port(uint8_t port);
 void phy_write_mask(uint16_t phy_mask, uint8_t dev_id, uint16_t reg, uint16_t v);
 void phy_write(uint8_t phy_id, uint8_t dev_id, uint16_t reg, uint16_t v);
 void phy_read(uint8_t phy_id, uint8_t dev_id, uint16_t reg);
@@ -150,7 +152,8 @@ void sleep(uint16_t t);
 void write_char_no_syslog(char c);
 void write_char(char c);
 void print_reg(uint16_t reg);
-uint8_t sfp_read_reg(uint8_t slot, uint8_t reg);
+bool sfp_read_block(uint8_t slot, uint8_t reg, uint8_t len) __banked __reentrant;
+extern __xdata uint8_t sfp_buf[16];
 void reg_bit_set(uint16_t reg_addr, char bit);
 void reg_bit_clear(uint16_t reg_addr, char bit);
 uint8_t reg_bit_test(uint16_t reg_addr, char bit);
@@ -165,11 +168,13 @@ uint16_t strlen_x(register __xdata const char *s);
 uint16_t strtox(register __xdata uint8_t *dst, register __code const char *s);
 uint16_t strcpy(register __xdata uint8_t *dst, register const char *s);
 char strcmp(register __xdata const uint8_t *a, register __code const uint8_t *b);
+bool strstart(__xdata const uint8_t *a, __code const uint8_t *b);
+bool strstart_x(__xdata const uint8_t *a, __xdata const uint8_t *b);
 void tcpip_output(void);
 uint8_t read_flash(uint8_t bank, __code uint8_t *addr);
 void get_random_32(void);
 void read_reg_timer(__xdata uint32_t * tmr);
-void sfp_print_info(uint8_t sfp);
+bool sfp_print_info(uint8_t sfp);
 bool gpio_pin_test(uint8_t pin);
 void set_sys_led_state(uint8_t state);
 void sds_read(uint8_t sds_id, uint8_t page, uint8_t reg);
diff --git a/rtl837x_phy.c b/rtl837x_phy.c
index 16fd214..240d1d0 100644
--- a/rtl837x_phy.c
+++ b/rtl837x_phy.c
@@ -262,7 +262,7 @@ void phy_set_speed(void) __banked
 {
 	uint16_t v;
 
-	print_string("Setting port "); write_char(machine.log_to_phys_port[phy_settings.port] + '0');
+	print_string("Setting port "); print_phys_port(phy_settings.port);
 	if (machine.n_10g && phy_settings.port == 3)
 		phy_settings.is10g_port = 1;
 	if (machine.n_10g == 2 && phy_settings.port == 8)
@@ -381,7 +381,7 @@ void phy_set_duplex(void) __banked
 {
 	uint16_t v;
 
-	print_string("Setting port "); write_char(machine.log_to_phys_port[phy_settings.port] + '0');
+	print_string("Setting port "); print_phys_port(phy_settings.port);
 	if (phy_settings.duplex)
 		print_string(" to full duplex");
 	else
diff --git a/rtl837x_pins.c b/rtl837x_pins.c
index 90015b7..bee9818 100644
--- a/rtl837x_pins.c
+++ b/rtl837x_pins.c
@@ -1,6 +1,11 @@
 #include "rtl837x_pins.h"
 #include "rtl837x_common.h"
+#include "rtl837x_sfr.h"
 #include "rtl837x_regs.h"
+#include "machine.h"
+
+extern __code const struct machine machine;
+extern __xdata uint8_t sfr_data[4];
 
 #pragma codeseg BANK2
 #pragma constseg BANK2
@@ -121,3 +126,56 @@ void gpio_output_setup(uint8_t pin, __xdata uint8_t initial_val) __banked{
 
 	reg_bit_set(gpio_direction_reg(pin), (pin % 32));
 }
+
+
+/*
+ * Read up to 16 consecutive registers of the EEPROM via I2C into sfp_buf
+ */
+bool sfp_read_block(uint8_t slot, uint8_t reg, uint8_t len) __banked __reentrant
+{
+	uint8_t dev;
+	uint8_t val;
+
+	len--;
+	if (len > 15)
+		return false;
+
+	dev = (reg & 0x80) ? 0x51 : 0x50;	// 0x51 holds the diagnostics, 0x50 the module data
+	reg &= 0x7f;
+
+	REG_WRITE(RTL837X_REG_I2C_IN, 0, 0, 0, reg);
+
+	REG_WRITE(RTL837X_REG_I2C_CTRL, 0x00,
+		  0x1 << (I2C_MEM_ADDR_WIDTH - 16) | len,
+		  (dev >> 5) | i2c_bus_from_scl_pin(machine.sfp_port[slot].i2c.scl) << 5
+		  | i2c_bus_from_sda_pin(machine.sfp_port[slot].i2c.sda) << 2,
+		  ((dev << 3) & 0xff) | 0x1);
+
+	do {
+		reg_read(RTL837X_REG_I2C_CTRL);
+	} while (SFR_DATA_0 & 0x1);
+
+	if (SFR_DATA_0 & 0x2)
+		return false;
+
+	for (uint8_t i = 0; i <= len; i++) {
+		switch (i & 0x3) {
+		case 0:
+			reg_read(RTL837X_REG_I2C_OUT + i);
+			val = SFR_DATA_0;
+			break;
+		case 1:
+			val = SFR_DATA_8;
+			break;
+		case 2:
+			val = SFR_DATA_16;
+			break;
+		default:
+			val = SFR_DATA_24;
+			break;
+		}
+		sfp_buf[i] = val;
+	}
+
+	return true;
+}
diff --git a/rtl837x_port.c b/rtl837x_port.c
index a85e90d..103ac4e 100644
--- a/rtl837x_port.c
+++ b/rtl837x_port.c
@@ -390,10 +390,7 @@ void port_l2_learned(void) __banked
 				print_string("\tlearned\t");
 
 			port |= (sfr_data[3] & 0x3) << 2;
-			if (port < 9)
-				write_char(machine.log_to_phys_port[port] + '0');
-			else
-				print_string("CPU");
+			print_phys_port(port);
 		}
 
 		entry++;
@@ -431,7 +428,7 @@ void port_stats_print(void) __banked
 {
 	print_string("\nPort\tState\tLink\tTxGood\t\tTxBad\t\tRxGood\t\tRxBad\n");
 	for (uint8_t i = machine.min_port; i <= machine.max_port; i++) {
-		write_char('0' + machine.log_to_phys_port[i]); write_char('\t');
+		print_phys_port(i); write_char('\t');
 
 		if (!machine.is_sfp[i]) {
 			phy_read(i, PHY_MMD31, 0xa610);
@@ -606,7 +603,7 @@ void port_eee_disable(uint8_t port) __banked
 
 void port_eee_status(uint8_t port) __banked
 {
-	print_string("Port: "); write_char('0' + machine.log_to_phys_port[port]);
+	print_string("Port: "); print_phys_port(port);
 	print_string(": ");
 	if (machine.is_sfp[port]) {
 		print_string("SFP\n");
@@ -801,16 +798,6 @@ void print_port_ingress_filter_mode(vlan_ingress_mode_t mode) __banked
 	}
 }
 
-static void print_phys_port(uint8_t port) __banked
-{
-	if (port >= machine.min_port && port <= machine.max_port)
-		write_char(machine.log_to_phys_port[port] + '0');
-	else if (port == 9)
-		write_char('9');
-	else
-		write_char('?');
-}
-
 void print_vlan_ingress_port(uint8_t log_port) __banked
 {
 	print_phys_port(log_port);write_char('\t');
diff --git a/rtl837x_stp.c b/rtl837x_stp.c
index 2eb4f92..116c64c 100644
--- a/rtl837x_stp.c
+++ b/rtl837x_stp.c
@@ -14,6 +14,11 @@
 #include "uip.h"
 #include "machine.h"
 
+// All entry points are __banked and nothing here runs from an interrupt,
+// so the module does not need to stay in the resident bank
+#pragma codeseg BANK2
+#pragma constseg BANK2
+
 extern __code struct machine machine;
 extern __xdata uint8_t sfr_data[4];
 
diff --git a/rtlplayground.c b/rtlplayground.c
index f074397..f0a3292 100644
--- a/rtlplayground.c
+++ b/rtlplayground.c
@@ -140,6 +140,7 @@ __xdata char sfp_module_vendor[2][17];
 __xdata char sfp_module_model[2][17];
 __xdata char sfp_module_serial[2][17];
 __xdata uint8_t sfp_options[2];
+__xdata uint8_t sfp_buf[16];	/* scratch for one I2C transaction, the controller reads at most 16 bytes */
 __xdata uint8_t sfp_speed[2];
 __xdata uint8_t sfp_quirks[2];
 __xdata bool button_last;
@@ -366,6 +367,32 @@ char strcmp(register __xdata const uint8_t *a, register __code const uint8_t *b)
 }
 
 
+/*
+ * True when b is a prefix of a. Unlike strcmp() the byte after the match is not
+ * compared, and unlike is_word_x() it need not be a separator.
+ */
+bool strstart(__xdata const uint8_t *a, __code const uint8_t *b)
+{
+	uint8_t i = 0;
+
+	while (b[i] && (b[i] == a[i]))
+		i++;
+
+	return !b[i];
+}
+
+
+bool strstart_x(__xdata const uint8_t *a, __xdata const uint8_t *b)
+{
+	uint8_t i = 0;
+
+	while (b[i] && (b[i] == a[i]))
+		i++;
+
+	return !b[i];
+}
+
+
 void print_short(uint16_t a)
 {
 	// allocating the registers first improves the sdcc code here
@@ -786,6 +813,19 @@ void print_reg(uint16_t reg)
 	print_sfr_data();
 }
 
+// Print the physical port of a logical port number.
+void print_phys_port(uint8_t port)
+{
+	if (port < CPU_PORT)
+		write_char(machine.log_to_phys_port[port] + '0');
+	else if (port == CPU_PORT)
+		print_string("CPU");
+	else {
+		print_string("UNKNOWN ");
+		write_char(port + '0');
+	}
+}
+
 
 /*
 // TODO: This uses 2 DSEG bytes and is not used!
@@ -1042,37 +1082,6 @@ void sds_config(uint8_t sds, uint8_t mode)
 }
 
 
-/*
- * Read a register of the EEPROM via I2C
- */
-uint8_t sfp_read_reg(uint8_t slot, uint8_t reg)
-{
-	if (reg & 0x80) {	// Configure SFP readings address (0x51) as I2C device address
-		reg &= 0x7f;
-		REG_WRITE(RTL837X_REG_I2C_CTRL, 0x00, 0x1 << (I2C_MEM_ADDR_WIDTH-16) | 0,  0x51 >> 5, (0x51 << 3) & 0xff);
-	} else {
-		REG_WRITE(RTL837X_REG_I2C_CTRL, 0x00, 0x1 << (I2C_MEM_ADDR_WIDTH-16) | 0,  0x50 >> 5, (0x50 << 3) & 0xff);
-	}
-
-	reg_read_m(RTL837X_REG_I2C_CTRL);
-	sfr_mask_data(1, 0xfc, i2c_bus_from_scl_pin(machine.sfp_port[slot].i2c.scl) << 5 | i2c_bus_from_sda_pin(machine.sfp_port[slot].i2c.sda) << 2);
-	reg_write_m(RTL837X_REG_I2C_CTRL);
-
-	REG_WRITE(RTL837X_REG_I2C_IN, 0, 0, 0, reg);
-
-	// Execute I2C Read
-	reg_bit_set(RTL837X_REG_I2C_CTRL, 0);
-
-	// Wait for execution to finish
-	do {
-		reg_read_m(RTL837X_REG_I2C_CTRL);
-	} while (sfr_data[3] & 0x1);
-
-	reg_read_m(RTL837X_REG_I2C_OUT);
-	return sfr_data[3];
-}
-
-
 /*
  * Adds TX Header to uip_buf and calls nic_tx_packet to send the packet
  * over the wire
@@ -1225,36 +1234,46 @@ static inline uint8_t sfp_rate_to_sds_config(register uint8_t rate)
 }
 
 
-void sfp_print_info(uint8_t sfp)
+bool sfp_print_info(uint8_t sfp)
 {
 	// This loops over the Vendor-name, Vendor OUI, Vendor PN and Vendor rev ASCII fields
-	for (uint8_t i = 20; i < 60; i++) {
-		if (i >= 36 && i < 40) // Skip Non-ASCII codes
+	for (uint8_t i = 16; i < 64; i++) {
+		if (!(i & 0xf) && !sfp_read_block(sfp, i, 16))
+			return false;
+		if (i < 20 || i >= 60 || (i >= 36 && i < 40)) // Skip Non-ASCII codes
 			continue;
-		uint8_t c = sfp_read_reg(sfp, i);
+		uint8_t c = sfp_buf[i & 0xf];
 		if (c)
 			write_char(c);
 	}
 	print_string("\n");
+
+	return true;
 }
 
 // Normalize strings from EEPROM by removing any trailing spaces; this allows simpler comparisons
-void sfp_read_field(__xdata char *dst, uint8_t sfp, uint8_t start, uint8_t length) __reentrant
+bool sfp_read_field(__xdata char *dst, uint8_t sfp, uint8_t start, uint8_t length) __reentrant
 {
-	dst[length] = '\0';
+	if (!sfp_read_block(sfp, start, length))
+		return false;
 
-	for (uint8_t i = 0; i < length; i++)
-		dst[i] = sfp_read_reg(sfp, start + i);
+	dst[length] = NUL;
+	memcpy(dst, sfp_buf, length);
 
 	while (length > 0 && dst[--length] == ' ')
-		dst[length] = '\0';
+		dst[length] = NUL;
+
+	return true;
 }
 
-void sfp_get_info(uint8_t sfp)
+bool sfp_get_info(uint8_t sfp)
 {
-	sfp_read_field(sfp_module_vendor[sfp], sfp, 20, 16);
-	sfp_read_field(sfp_module_model[sfp], sfp, 40, 16);
-	sfp_read_field(sfp_module_serial[sfp], sfp, 68, 16);
+	if (!sfp_read_field(sfp_module_vendor[sfp], sfp, 20, 16))
+		return false;
+	if (!sfp_read_field(sfp_module_model[sfp], sfp, 40, 16))
+		return false;
+
+	return sfp_read_field(sfp_module_serial[sfp], sfp, 68, 16);
 }
 
 void sfp_apply_quirks(uint8_t sfp) __reentrant
@@ -1273,7 +1292,7 @@ void sfp_apply_quirks(uint8_t sfp) __reentrant
 		if (!(sfp_options[sfp] & 0x40)) {
 			// The module reports that DDM is not implemented, but try a dummy read to confirm
 			// 0xff would mean a failed I2C read or an impossible (per spec) voltage greater than 6.5V
-			if (sfp_read_reg(sfp, 226) != 0xff) {
+			if (sfp_read_block(sfp, 226, 1) && sfp_buf[0] != 0xff) {
 				sfp_options[sfp] |= 0x40;
 			}
 		}
@@ -1297,6 +1316,45 @@ void setup_sfp_gpio(void)
 	}
 }
 
+static bool sfp_module_read(uint8_t sfp)
+{
+	uint8_t rate;
+
+	// Read Reg 11: Encoding, see SFF-8472 and SFF-8024
+	// Read Reg 12: Signalling rate (including overhead) in 100Mbit: 0xd: 1Gbit, 0x67:10Gbit
+	delay(100); // Delay, because some modules need time to wake up
+	if (!sfp_read_block(sfp, 11, 2))
+		return false;
+
+	rate = sfp_buf[1];
+	if (sfp_speed[sfp] == SFP_SPEED_100M)
+		rate = 0x1;
+	else if (sfp_speed[sfp] == SFP_SPEED_1G)
+		rate = 0xc;
+	else if (sfp_speed[sfp] == SFP_SPEED_2G5)
+		rate = 0x19;
+	else if (sfp_speed[sfp] == SFP_SPEED_10G)
+		rate = 0x69;
+	print_string("  Rate: "); print_byte(rate);  // Normally 1, but 0 for DAC, can be ignored?
+	print_string("  Encoding: "); print_byte(sfp_buf[0]);
+	print_string("  Module: ");
+	if (!sfp_print_info(sfp))
+		return false;
+	print_string("\n");
+
+	if (!sfp_read_block(sfp, 92, 1))
+		return false;
+	sfp_options[sfp] = sfp_buf[0];
+	if (!sfp_get_info(sfp))
+		return false;
+
+	sfp_apply_quirks(sfp);
+	sds_config(machine.sfp_port[sfp].sds, sfp_rate_to_sds_config(rate));
+
+	return true;
+}
+
+
 void handle_sfp(void)
 {
 	for (uint8_t sfp = 0; sfp < machine.n_sfp; sfp++) {
@@ -1304,26 +1362,10 @@ void handle_sfp(void)
 			if (sfp_pins_last & (0x1 << (sfp << 2))) {
 				sfp_pins_last &= ~(0x01 << (sfp << 2));
 				print_string("\n<MODULE INSERTED>  Slot: "); write_char('1' + sfp);
-				// Read Reg 11: Encoding, see SFF-8472 and SFF-8024
-				// Read Reg 12: Signalling rate (including overhead) in 100Mbit: 0xd: 1Gbit, 0x67:10Gbit
-				delay(100); // Delay, because some modules need time to wake up
-				uint8_t rate = sfp_read_reg(sfp, 12);
-				if (sfp_speed[sfp] == SFP_SPEED_100M)
-					rate = 0x1;
-				else if (sfp_speed[sfp] == SFP_SPEED_1G)
-					rate = 0xc;
-				else if (sfp_speed[sfp] == SFP_SPEED_2G5)
-					rate = 0x19;
-				else if (sfp_speed[sfp] == SFP_SPEED_10G)
-					rate = 0x69;
-				print_string("  Rate: "); print_byte(rate);  // Normally 1, but 0 for DAC, can be ignored?
-				print_string("  Encoding: "); print_byte(sfp_read_reg(sfp, 11));
-				print_string("  Module: "); sfp_print_info(sfp);
-				print_string("\n");
-				sfp_options[sfp] = sfp_read_reg(sfp, 92);
-				sfp_get_info(sfp);
-				sfp_apply_quirks(sfp);
-				sds_config(machine.sfp_port[sfp].sds, sfp_rate_to_sds_config(rate));
+				if (!sfp_module_read(sfp)) {
+					print_string("SFP: an I2C read failed, retrying on the next poll\n");
+					sfp_pins_last |= 0x01 << (sfp << 2);
+				}
 			}
 		} else {
 			if (!(sfp_pins_last & (0x1 << (sfp << 2)))) {
@@ -2027,7 +2069,7 @@ void check_and_flash_update_image(void)
  * because itohex() is inline and brings its own frame. */
 void set_hostname_default(void)
 {
-	if (hostname[0] != '\0')
+	if (hostname[0] != NUL)
 		return;
 
 	strcpy((__xdata uint8_t *)hostname, "RTLPlayground-");
@@ -2037,7 +2079,7 @@ void set_hostname_default(void)
 	hostname[17] = hex[uip_ethaddr.addr[4] & 0xf];
 	hostname[18] = hex[uip_ethaddr.addr[5] >> 4];
 	hostname[19] = hex[uip_ethaddr.addr[5] & 0xf];
-	hostname[20] = '\0';
+	hostname[20] = NUL;
 }