Merge remote-tracking branch 'origin/main' into fix-kp-9000-6xhml-x2-led-mux

# Conflicts:
#	machine.c

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
 

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

html/stat.js

@@ -180,7 +180,7 @@ function fillStats() {
       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>';
+      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;
     }
   }

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 (c == NUL) {
-			if (u != '\0' && u != ' ' && u != '\t' && u != ':' && u != '?' && u != '=' && u != '\n' && u != '\r')
+			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 (s == NUL) {
-			if (u != '\0' && u != ' ' && u != '\t' && u != ':' && u != '?' && u != '=' && u != '\n' && u != '\r')
+			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");
+			dbg_string("Configuration upload\n");
-			uptr = CONFIG_START;
 			verify_crc = 0;
-			max_upload = 2048;
+			config_upload = 1;
-			flash_region.addr = CONFIG_START;
+			write_len = 0;
-			flash_sector_erase();
 		}
 		// 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")) {

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++) {
+	for (uint8_t i = 16; i < 64; i++) {
-		if (i >= 36 && i < 40) // Skip Non-ASCII codes
+		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
+	sfp_read_block(slot, reg, len);
-		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);
-	}
 
-	reg_read_m(RTL837X_REG_I2C_CTRL);
+	for (uint8_t i = 0; i < len; i++)
-	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);
+		byte_to_html(sfp_buf[i]);
-	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)]);
-	}
 }
 
 
@@ -309,17 +289,26 @@ void send_vlan(uint16_t vlan)
 	slen += strtox(outbuf + slen, "\"}");
 }
 
-
+/* Send counters
-void send_counters(char port)
+ * 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_string("sending counters\n"); dbg_byte(phys_port_idx);
-	dbg_byte(port);
+
-	uint8_t i = machine.phys_to_log_port[port];
+	char_to_html('[');
-	slen += strtox(outbuf + slen, "[");
 	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;

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);

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

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

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

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);

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

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;
+}

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)
+			print_phys_port(port);
-				write_char(machine.log_to_phys_port[port] + '0');
-			else
-				print_string("CPU");
 		}
 
 		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');

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];
 

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++) {
+	for (uint8_t i = 16; i < 64; i++) {
-		if (i >= 36 && i < 40) // Skip Non-ASCII codes
+		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[length] = NUL;
-		dst[i] = sfp_read_reg(sfp, start + i);
+	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);
+	if (!sfp_read_field(sfp_module_vendor[sfp], sfp, 20, 16))
-	sfp_read_field(sfp_module_model[sfp], sfp, 40, 16);
+		return false;
-	sfp_read_field(sfp_module_serial[sfp], sfp, 68, 16);
+	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,17 +1316,17 @@ void setup_sfp_gpio(void)
 	}
 }
 
-void handle_sfp(void)
+static bool sfp_module_read(uint8_t sfp)
 {
-	for (uint8_t sfp = 0; sfp < machine.n_sfp; sfp++) {
+	uint8_t rate;
-		if (!gpio_pin_test(machine.sfp_port[sfp].pin_detect)) {
+
-			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_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)
@@ -1317,13 +1336,36 @@ void handle_sfp(void)
 	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("  Encoding: "); print_byte(sfp_buf[0]);
-				print_string("  Module: "); sfp_print_info(sfp);
+	print_string("  Module: ");
+	if (!sfp_print_info(sfp))
+		return false;
 	print_string("\n");
-				sfp_options[sfp] = sfp_read_reg(sfp, 92);
+
-				sfp_get_info(sfp);
+	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++) {
+		if (!gpio_pin_test(machine.sfp_port[sfp].pin_detect)) {
+			if (sfp_pins_last & (0x1 << (sfp << 2))) {
+				sfp_pins_last &= ~(0x01 << (sfp << 2));
+				print_string("\n<MODULE INSERTED>  Slot: "); write_char('1' + sfp);
+				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;
 }