Sfp and Link status work

cmd_parser.c

@@ -514,15 +514,16 @@ void cmd_parser(void) __banked
 			reset_chip();
 		}
 		if (cmd_compare(0, "sfp")) {
-			uint8_t rate = sfp_read_reg(0, 12);
-			print_string("\nRate: "); print_byte(rate);
+			print_string("\nSlot 0 - Rate: "); print_byte(sfp_read_reg(0, 12));
 			print_string("  Encoding: "); print_byte(sfp_read_reg(0, 11));
 			print_string("\n");
-			for (uint8_t i = 20; i < 60; i++) {
-				uint8_t c = sfp_read_reg(0, i);
-				if (c)
-					write_char(c);
-			}
+			sfp_print_info(0);
+#if NSFP == 2
+			print_string("\nSlot 1 - Rate: "); print_byte(sfp_read_reg(1, 12));
+			print_string("  Encoding: "); print_byte(sfp_read_reg(1, 11));
+			print_string("\n");
+			sfp_print_info(1);
+#endif
 		}
 		if (cmd_compare(0, "stat")) {
 			port_stats_print();

rtl837x_common.h

@@ -90,6 +90,6 @@ uint16_t strlen_x(register __xdata const char *s);
 uint16_t strtox(register __xdata uint8_t *dst, register __code const char *s);
 void tcpip_output(void);
 void print_string_x(__xdata char *p);
-
+void sfp_print_info(uint8_t sfp);
 
 #endif

rtl837x_port.c

@@ -398,15 +398,41 @@ void port_stats_print(void) __banked
 {
 	print_string("\n Port\tState\tLink\tTxGood\t\tTxBad\t\tRxGood\t\tRxBad\n");
 	for (uint8_t i = minPort; i <= maxPort; i++) {
-		write_char('1' + i); write_char('\t');
-		phy_read(i, 0x1f, 0xa610); // p001f.a610:2058
-		if (i <= maxPort - nSFPPorts) {
+		if (!isRTL8373) {
+			write_char('0' + log_to_phys_port[i]); write_char('\t');
+		} else {
+			write_char('1' + i); write_char('\t');
+		}
+
+		if (!IS_SFP(i)) {
+			phy_read(i, 0x1f, 0xa610);
 			if (SFR_DATA_8 == 0x20)
 				print_string("On\t");
 			else
 				print_string("Off\t");
-			reg_read_m(RTL837X_REG_LINKS);
-			uint8_t b = sfr_data[3 - (i >> 1)];
+		} else {  // An SFP Module
+			if (i != 3) {
+				reg_read_m(RTL837X_REG_GPIO_00_31_INPUT);
+				if (!(sfr_data[0] & 0x40)) {
+					print_string("SFP OK\t");
+				} else {
+					print_string("NO SFP\t");
+				}
+			} else {
+				reg_read_m(RTL837X_REG_GPIO_32_63_INPUT);
+				if (!(sfr_data[1] & 0x04)) {
+					print_string("SFP OK\t");
+				} else {
+					print_string("NO SFP\t");
+				}
+			}
+		}
+
+		if (i < 8)
+				reg_read_m(RTL837X_REG_LINKS);
+			else
+				reg_read_m(RTL837X_REG_LINKS_89);
+			uint8_t b = sfr_data[3 - ((i & 7) >> 1)];
 			b = (i & 1) ? b >> 4 : b & 0xf;
 			switch (b) {
 			case 0:
@@ -418,34 +444,15 @@ void port_stats_print(void) __banked
 			case 2:
 				print_string("1000M\t");
 				break;
+			case 4:
+				print_string("10G\t");
+				break;
 			case 5:
 				print_string("2.5G\t");
 				break;
 			default:
 				print_string("Up\t");
 				break;
-			}
-		} else {  // An SFP Module TODO: This is for 1 module devices
-			reg_read_m(RTL837X_REG_GPIO_00_31_INPUT);
-			if (!(sfr_data[0] & 0x40)) {
-				print_string("SFP OK\t");
-			} else {
-				print_string("NO SFP\t");
-			}
-			reg_read_m(RTL837X_REG_GPIO_32_63_INPUT);
-			if (sfr_data[3] & 0x20) {
-				print_string("Down\t");
-			} else {
-				uint8_t rate = sfp_read_reg(0, 12);
-				if (rate == 0xd)
-					print_string("1000BX\t");
-				else if (rate == 0x1f)
-					print_string("2500G\t");
-				else if (rate > 0x65 && rate < 0x70)
-					print_string("10G\t");
-				else
-					print_string("Up\t");
-			}
 		}
 		STAT_GET(0x2f, i);
 		print_reg(RTL837X_STAT_V_LOW); write_char('\t');

rtl837x_regs.h

@@ -31,7 +31,8 @@
 #define SDS_10GR		0x1a
 #define SDS_OFF			0x1f
 
-#define RTL837X_REG_LINKS 0x63f0
+#define RTL837X_REG_LINKS	0x63f0
+#define RTL837X_REG_LINKS_89	0x63f4
 /* Each nibble encodes the link state of a port.
    Port 0 appears to be the CPU port
    The RTL8372 serves ports 4-7, port 3 is the RTL8221

rtlplayground.c

@@ -101,6 +101,7 @@ __code uint16_t bit_mask[16] = {
 
 __xdata uint8_t was_offline;
 __xdata uint8_t linkbits_last[4];
+__xdata uint8_t linkbits_last_p89;
 __xdata uint8_t sfp_pins_last;
 
 
@@ -880,7 +881,10 @@ static inline uint8_t sfp_rate_to_sds_config(register uint8_t rate)
 
 void 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
+			continue;
 		uint8_t c = sfp_read_reg(sfp, i);
 		if (c)
 			write_char(c);
@@ -920,7 +924,7 @@ void handle_sfp(void)
 		sfp_pins_last |= 0x02;
 		print_string("\n<SFP-RX LOS>\n");
 	}
-
+#if NSFP == 2
 	reg_read_m(RTL837X_REG_GPIO_32_63_INPUT);
 	if ((sfp_pins_last & 0x10) && (!(sfr_data[1] & 0x04))) {
 		sfp_pins_last &= ~0x10;
@@ -939,6 +943,17 @@ void handle_sfp(void)
 		sfp_pins_last |= 0x10;
 		print_string("\n<MODULE 2 REMOVED>\n");
 	}
+
+	reg_read_m(RTL837X_REG_GPIO_00_31_INPUT);
+	if ((sfp_pins_last & 0x20) && (!(sfr_data[2] & 0x08))) {
+		sfp_pins_last &= ~0x20;
+		print_string("\n<SFP2-RX OK>\n");
+	}
+	if ((!(sfp_pins_last & 0x20)) && (sfr_data[2] & 0x08)) {
+		sfp_pins_last |= 0x20;
+		print_string("\n<SFP2-RX LOS>\n");
+	}
+#endif
 }
 
 
@@ -980,17 +995,25 @@ void idle(void)
 #endif
 	}
 
+	// Check for Link changes
+	reg_read_m(RTL837X_REG_LINKS_89);
+	__xdata uint8_t linkbits_p89 = sfr_data[3];
+
 	reg_read_m(RTL837X_REG_LINKS);
-	if (!isRTL8373 && cmp_4(sfr_data, linkbits_last)) {
+	if (cmp_4(sfr_data, linkbits_last) || (linkbits_p89 != linkbits_last_p89)) {
 		print_string("\n<new link: ");
-		print_long_x(sfr_data);
+		print_byte(linkbits_p89); print_byte(sfr_data[0]); print_byte(sfr_data[1]);
+		print_byte(sfr_data[2]); print_byte(sfr_data[3]);
 		print_string(", was ");
-		print_long_x(linkbits_last);
+		print_byte(linkbits_last_p89); print_byte(linkbits_last[0]); print_byte(linkbits_last[1]);
+		print_byte(linkbits_last[2]); print_byte(linkbits_last[3]); 
 		print_string(">\n");
-		if (nSFPPorts != 2) {
+		linkbits_last_p89 = linkbits_p89;
+		if (!isRTL8373 && nSFPPorts != 2) {
 			uint8_t p5 = sfr_data[2] >> 4;
 			uint8_t p5_last = linkbits_last[2] >> 4;
 			cpy_4(linkbits_last, sfr_data);
+			// Handle link change of the RTL8221 PHY, adjust SDS mode
 			if (p5_last != p5) {
 				if (p5 == 0x5) // 2.5GBit Mode
 					sds_config(0, SDS_HISGMII);
@@ -1003,7 +1026,6 @@ void idle(void)
 	}
 
 	// Check for changes with SFP modules
-
 	handle_sfp();
 
 	/* Button pressed on KL-8xhm-x2:
@@ -1693,7 +1715,7 @@ void bootloader(void)
 	// Set default for SFP pins so we can start up a module already inserted
 	sfp_pins_last = 0x33; // signal LOS and no module inserted (for both slots, even if only 1 present)
 	// We have not detected any link
-	linkbits_last[0] = linkbits_last[1] = linkbits_last[2] = linkbits_last[3] = 0;
+	linkbits_last[0] = linkbits_last[1] = linkbits_last[2] = linkbits_last[3] = linkbits_last_p89 = 0;
 
 	print_string("Detecting CPU: ");
 	isRTL8373 = 0;