From 8c61010a845b553acfb80332965861f6c033000e Mon Sep 17 00:00:00 2001
From: logicog <git@logicog.de>
Date: Thu, 28 May 2026 14:03:11 +0200
Subject: [PATCH] Add support for configuring SFP modules speeds

---
 cmd_parser.c     | 70 +++++++++++++++++++++++++++++++++++++++---------
 rtl837x_common.h |  9 +++++++
 rtlplayground.c  |  9 +++++++
 3 files changed, 76 insertions(+), 12 deletions(-)

diff --git a/cmd_parser.c b/cmd_parser.c
index 729ed1a..45308ba 100644
--- a/cmd_parser.c
+++ b/cmd_parser.c
@@ -48,6 +48,8 @@ __xdata uint16_t vlan_ptr;
 __xdata char port_names[9][PORT_NAME_SIZE];
 
 extern __xdata uint16_t management_vlan;
+extern __xdata uint8_t sfp_speed[2];
+extern __xdata uint8_t sfp_pins_last;
 __xdata uint8_t gpio_last_value[8] = { 0 };
 
 // Temporatly for str to hex convertion value.
@@ -748,6 +750,61 @@ void sfp_print_measurements(uint8_t sfp)
 }
 
 
+void parse_sfp(void)
+{
+	uint8_t slot;
+
+	if (cmd_words_len != 1 && cmd_words_len != 3)
+		goto err;
+
+	if (cmd_words_len == 1) {
+		for (slot = 0; slot < machine.n_sfp; slot++) {
+			print_string("\nSlot "); write_char('1' + slot);
+			if (gpio_pin_test(machine.sfp_port[slot].pin_detect)) {
+				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));
+			write_char('\n');
+			sfp_print_info(slot);
+			sfp_print_measurements(slot);
+		}
+		return;
+	}
+	if (cmd_buffer[cmd_words_b[1]] < '1' || cmd_buffer[cmd_words_b[1]] > '2' || cmd_buffer[cmd_words_b[1] + 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;
+	}
+
+	if (cmd_compare(2, "10g")) {
+		print_string(" 10G\n");
+		sfp_speed[slot] = SFP_SPEED_10G;
+	} else if (cmd_compare(2, "2g5")) {
+		print_string(" 2.5G\n");
+		sfp_speed[slot] = SFP_SPEED_2G5;
+	} else if (cmd_compare(2, "1g")) {
+		print_string(" 1G\n");
+		sfp_speed[slot] = SFP_SPEED_1G;
+	} else if (cmd_compare(2, "auto")) {
+		print_string(" AUTO\n");
+		sfp_speed[slot] = SFP_SPEED_AUTO;
+	} else {
+		goto err;
+	}
+	sfp_pins_last |= 0x1 << (slot << 2);
+	handle_sfp();
+	return;
+err:
+	print_string("\nUsage:\n\tsfp\n\tsfp [1|2] [1g|2g5|10g]\n");
+}
+
+
 void parse_regget(void)
 {
 	uint16_t reg = 0;
@@ -1355,18 +1412,7 @@ void cmd_parser(void) __banked
 			print_string("\nRESET\n\n");
 			reset_chip();
 		} else if (cmd_compare(0, "sfp")) {
-			print_string("\nSlot 1 - Rate: "); print_byte(sfp_read_reg(0, 12));
-			print_string("  Encoding: "); print_byte(sfp_read_reg(0, 11));
-			print_string("\n");
-			sfp_print_info(0);
-			sfp_print_measurements(0);
-			if (machine.n_sfp == 2) {
-				print_string("\nSlot 2 - 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);
-				sfp_print_measurements(1);
-			}
+			parse_sfp();
 		} else if (cmd_compare(0, "stat")) {
 			port_stats_print();
 		} else if (cmd_compare(0, "flash") && cmd_words_len == 2) {
diff --git a/rtl837x_common.h b/rtl837x_common.h
index dbb39a8..3be8c7f 100644
--- a/rtl837x_common.h
+++ b/rtl837x_common.h
@@ -92,6 +92,14 @@ struct vlan_tag {
 #define CMD_HISTORY_SIZE 0x400
 #define CMD_HISTORY_MASK (CMD_HISTORY_SIZE - 1)
 
+enum sfp_speeds {
+	SFP_SPEED_AUTO = 0,
+	SFP_SPEED_1G,
+	SFP_SPEED_2G5,
+	SFP_SPEED_5G,
+	SFP_SPEED_10G
+};
+
 /**
  * Representation of a 48-bit Ethernet address.
  */
@@ -160,4 +168,5 @@ void sds_read(uint8_t sds_id, uint8_t page, uint8_t reg);
 void sds_write_v(uint8_t sds_id, uint8_t page, uint8_t reg, uint16_t v);
 void sds_config_mac(uint8_t sds, uint8_t mode);
 void sds_config(uint8_t sds, uint8_t mode);
+void handle_sfp(void);
 #endif
diff --git a/rtlplayground.c b/rtlplayground.c
index 18e7e3c..7499f45 100644
--- a/rtlplayground.c
+++ b/rtlplayground.c
@@ -137,6 +137,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_speed[2];
 __xdata bool button_last;
 __xdata uint8_t button_sec_counter_last;
 volatile __bit tx_buf_empty;
@@ -1234,6 +1235,12 @@ void handle_sfp(void)
 				// 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_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);
@@ -1996,6 +2003,8 @@ void main(void)
 	// Print SW version
 	print_sw_version();
 
+	// Set AUTONEG for SFP ports
+	sfp_speed[0] = sfp_speed[1] = SFP_SPEED_AUTO;
 	// Reset NIC
 	reg_bit_set(RTL837X_REG_RESET, RESET_NIC_BIT);
 	do {