Merge pull request #17 from logicog/stp

STP: initial work

cmd_parser.c

@@ -14,6 +14,7 @@
 #include "rtl837x_phy.h"
 #include "rtl837x_regs.h"
 #include "rtl837x_sfr.h"
+#include "rtl837x_stp.h"
 #include "uip/uip.h"
 
 #pragma codeseg BANK1
@@ -23,6 +24,7 @@ extern __xdata uint8_t maxPort;
 extern __xdata uint8_t nSFPPorts;
 extern __xdata uint8_t isRTL8373;
 extern __xdata uint16_t mpos;
+extern __xdata uint8_t stpEnabled;
 
 extern volatile __xdata uint32_t ticks;
 extern volatile __xdata uint8_t sfr_data[4];
@@ -574,6 +576,17 @@ void cmd_parser(void) __banked
 			else
 				port_l2_learned();
 		}
+		if (cmd_compare(0, "stp")) {
+			if (cmd_words_b[1] > 0 && cmd_compare(1, "on")) {
+				print_string("STP enabled\n");
+				stpEnabled = 1;
+				stp_setup();
+			} else {
+				print_string("STP disabled\n");
+				stp_off();
+				stpEnabled = 0;
+			}
+		}
 		if (cmd_compare(0, "pvid") && cmd_words_b[1] > 0 && cmd_words_b[2] > 0) {
 			__xdata uint16_t pvid;
 			uint8_t port;

rtl837x_regs.h

@@ -154,7 +154,7 @@
 #define RTL8373_RLDP_TIMER		0x1074
 #define RTL837X_RMA0_CONF		0x4ecc
 #define RTL837X_RMA_CONF		0x4f1c
-
+#define RTL837X_MSTP_STATES		0x5310
 
 #ifdef REGDBG
 

rtl837x_stp.c

@@ -10,7 +10,7 @@
 #include "rtl837x_common.h"
 #include "rtl837x_sfr.h"
 #include "rtl837x_regs.h"
-#include "rtl837x_igmp.h"
+#include "rtl837x_stp.h"
 #include "uip.h"
 
 extern __xdata uint8_t minPort;
@@ -18,10 +18,25 @@ extern __xdata uint8_t maxPort;
 extern __xdata uint8_t nSFPPorts;
 extern __xdata uint8_t cpuPort;
 extern __xdata uint8_t isRTL8373;
+extern __xdata uint8_t sfr_data[4];
 
 extern __code struct uip_eth_addr uip_ethaddr;
 
-extern __xdata uint8_t uip_buf[UIP_CONF_BUFFER_SIZE+2];
+extern __xdata uint8_t uip_buf[UIP_CONF_BUFFER_SIZE + 2];
+
+struct bridge {
+	uint8_t prio;
+	uint8_t ext;
+	uint8_t mac[6];
+};
+
+__xdata struct bridge root_bridge;
+__xdata uint32_t root_bridge_cost;
+
+__xdata uint8_t port_types[10];
+__xdata uint16_t port_timers[10];
+__xdata uint16_t port_hello[10];
+
 
 // 8899 04 0000 20 0004
 struct rtl_tag {
@@ -45,13 +60,9 @@ struct stp_pkt {
 	uint8_t version;
 	uint8_t bpdu_type;
 	uint8_t flags;
-	uint8_t root_prio;
+	struct bridge root;
-	uint8_t root_ext;
-	uint8_t root_mac[6];
 	uint32_t root_path_cost;
-	uint8_t bridge_prio;
+	struct bridge bridge;
-	uint8_t bridge_ext;
-	uint8_t bridge_mac[6];
 	uint8_t port_prio;
 	uint8_t port_id;
 	uint16_t age;
@@ -60,10 +71,93 @@ struct stp_pkt {
 	uint16_t fwd_delay;
 };
 
+struct stp_pkt_in {
+	uint8_t stp_addr[6];
+	uint8_t src_addr[6];
+	struct rtl_tag rtl_tag;
+	uint8_t vtag[4];
+	uint16_t msg_len;
+	uint8_t dsap;
+	uint8_t ssap;
+	uint8_t ctrl;
+	uint16_t proto;
+	uint8_t version;
+	uint8_t bpdu_type;
+	uint8_t flags;
+	struct bridge root;
+	uint32_t root_path_cost;
+	struct bridge bridge;
+	uint8_t port_prio;
+	uint8_t port_id;
+	uint16_t age;
+	uint16_t age_max;
+	uint16_t hello;
+	uint16_t fwd_delay;
+};
 
 #define STP_O ((__xdata struct stp_pkt *)&uip_buf[RTL_TAG_SIZE + VLAN_TAG_SIZE])
+#define STP_I ((__xdata struct stp_pkt_in *)&uip_buf[0])
 
-void stp_cnf_send(uint8_t port) __banked
+#define FLAG_PROPOSAL 0x02
+#define P_DESIGNATED ((STP_I->flags & 0x0c) == 0x0c)
+#define P_PROPOSAL (STP_I->flags & FLAG_PROPOSAL)
+
+signed char cmpMAC(__xdata uint8_t *m1, __xdata uint8_t *m2)
+{
+	for (uint8_t i = 0; i < 6; i++) {
+		if (m1[i] == m2[i])
+			continue;
+		if (m1[i] < m2[i])
+			return -1;
+		return 1;
+	}
+	return 0;
+}
+
+
+void stp_in(void) __banked
+{
+	// By default we do not send anything out
+	uip_len = 0;
+	// MSTPSTP_I_STATES 0x5310
+	// reg_read_m(RTL837X_MSTP_STATES);
+
+	print_string("Check BPDU... \n");
+	for (uint8_t i = 0; i < 80; i++) {
+		print_byte(uip_buf[i]);
+		write_char(' ');
+	}
+	write_char('\n');
+	print_byte(STP_I->dsap);
+	print_byte(STP_I->ssap);
+	print_byte(STP_I->ctrl);
+	
+	write_char('\n');
+	// Make sure this is the type of RSTP packet we are interested in:
+	if (!(STP_I->dsap == 0x42 && STP_I->ssap == 0x42 && STP_I->ctrl == 0x03))
+		return;
+	print_string("Checking RSTP\n");
+	if (STP_I->proto)
+		return;
+//	write_char('A'); print_byte(STP_I->version); write_char('\n');
+	if (STP_I->version != 2)
+		return;
+//	write_char('B'); print_byte(STP_I->bpdu_type); write_char('\n');
+	if (STP_I->bpdu_type != 2)
+		return;
+//	write_char('\n');
+//	print_string("Flags: "); print_byte(STP_I->flags); write_char('\n');
+	print_string("Check new Root\n");
+	if (STP_I->root.prio < root_bridge.prio
+		|| ((STP_I->root.prio == root_bridge.prio) && cmpMAC(STP_I->root.mac, STP_I->root.mac) < 0)) {
+		print_string("Updating Root bridge\n");
+			root_bridge.prio = STP_I->root.prio;
+			memcpy(root_bridge.mac, STP_I->root.mac, 6);
+	}
+}
+
+
+void stp_cnf_send(uint8_t port)
 {
 	STP_O->stp_addr[0] = 0x01; STP_O->stp_addr[1] = 0x80; STP_O->stp_addr[2] = 0xc2;
 	STP_O->stp_addr[3] = STP_O->stp_addr[4] = STP_O->stp_addr[5] = 0x00;
@@ -84,15 +178,15 @@ void stp_cnf_send(uint8_t port) __banked
 	STP_O->flags = 0x81;
 
 	memcpyc(STP_O->src_addr, uip_ethaddr.addr, 6);
-	memcpyc(STP_O->root_mac, uip_ethaddr.addr, 6); // For now we are the root-bridge
+	memcpy(STP_O->root.mac, root_bridge.mac, 6);
-	memcpyc(STP_O->bridge_mac, uip_ethaddr.addr, 6);
+	memcpyc(STP_O->bridge.mac, uip_ethaddr.addr, 6);
 
-	STP_O->root_prio = 0x80;
+	STP_O->root.prio = root_bridge.prio;
-	STP_O->root_ext = 0x00;
+	STP_O->root.ext = 0x00;
 	STP_O->root_path_cost = 0x00000000;
 
-	STP_O->bridge_prio = 0x80;
+	STP_O->bridge.prio = 0x80;
-	STP_O->bridge_ext = 0x00;
+	STP_O->bridge.ext = 0x00;
 
 	STP_O->port_prio = 0x80;
 	STP_O->port_id = port;
@@ -103,4 +197,56 @@ void stp_cnf_send(uint8_t port) __banked
 
 //	uip_len = 0x27 + sizeof(struct rtl_tag);
 	uip_len = sizeof(struct stp_pkt);
+	tcpip_output();
+}
+
+
+void stp_timers(void) __banked
+{
+	for (uint8_t i = minPort; i <= maxPort; i++) {
+		port_hello[i]--;
+		if (!port_hello[i]) {
+			port_hello[i] = TIME_HELLO;
+			print_string("STP_HELLO port ");
+			print_byte(i); write_char('\n');
+			stp_cnf_send(i);
+		}
+	}
+}
+
+
+void stp_setup(void) __banked
+{
+	print_string("Enabling STP: ");
+	sfr_data[0] = sfr_data[1] = sfr_data[2] = sfr_data[3] = 0;
+	for (uint8_t i = minPort; i <= maxPort; i++) {
+		// Set STP port state to blocking
+		// States are: 00 disable, 01 blocking, 10 learning, 11 forwarding
+		uint8_t bit_mask = 0b01 << ( (i << 1) & 0x7);
+		sfr_data[3 - (i >> 2)] |= bit_mask;
+		port_hello[i] = TIME_HELLO;
+		port_timers[i] = 0xa00;	// 10 sec in blocking state
+	}
+	sfr_data[1] |= 0x0f; // Do not block CPU-Port
+	reg_write_m(RTL837X_MSTP_STATES); // R5310-000d555f 
+
+	print_reg(RTL837X_MSTP_STATES); write_char('\n');
+
+	root_bridge.prio = 0x80; // This corresponds to 32768
+	root_bridge.ext	= 0x00;
+	memcpyc(root_bridge.mac, uip_ethaddr.addr, 6);
+}
+
+
+void stp_off(void) __banked
+{
+	sfr_data[0] = sfr_data[1] = sfr_data[2] = sfr_data[3] = 0;
+	for (uint8_t i = minPort; i <= maxPort; i++) {
+		// Set STP port state to forwarding
+		// States are: 00 disable, 01 blocking, 10 learning, 11 forwarding
+		uint8_t bit_mask = 0b11 << ( (i << 1) & 0x7);
+		sfr_data[3 - (i >> 2)] |= bit_mask;
+	}
+	sfr_data[1] |= 0x0f; // Do not block CPU-Port
+	reg_write_m(RTL837X_MSTP_STATES);
 }

rtl837x_stp.h

@@ -2,6 +2,11 @@
 #define _RTL837X_STP_H_
 
 #include <stdint.h>
-void stp_cnf_send(uint8_t port) __banked;
+void stp_in(void) __banked;
+void stp_setup(void) __banked;
+void stp_timers(void) __banked;
+void stp_off(void) __banked;
+
+#define TIME_HELLO 0x200 // 2 sec
 
 #endif

rtlplayground.c

@@ -54,6 +54,10 @@ __xdata uint8_t isRTL8373;
 volatile __xdata uint32_t ticks;
 volatile __xdata uint8_t sec_counter;
 volatile __xdata uint16_t sleep_ticks;
+__xdata uint8_t stp_clock;
+
+#define STP_TICK_DIVIDER 3
+
 
 // Buffer for serial input, SBUF_SIZE must be power of 2 < 256
 __xdata volatile uint8_t sbuf_ptr;
@@ -79,6 +83,7 @@ __xdata uint8_t minPort;
 __xdata uint8_t maxPort;
 __xdata uint8_t nSFPPorts;
 __xdata uint8_t cpuPort;
+__xdata uint8_t stpEnabled;
 
 __code uint16_t bit_mask[16] = {
 	0x0001, 0x0002, 0x0004, 0x0008, 0x0010, 0x0020, 0x0040, 0x0080,
@@ -371,7 +376,7 @@ void reg_bit_clear(uint16_t reg_addr, char bit)
 }
 
 /*
- * This masks the sfr data fields, first &-ing with ~mask, the setting the bits in set
+ * This masks the sfr data fields, first &-ing with ~mask, then setting the bits in set
  */
 void sfr_mask_data(uint8_t n, uint8_t mask, uint8_t set)
 {
@@ -816,16 +821,11 @@ void handle_rx(void)
 #ifdef RXTXDBG
 		print_string(" RX-VLAN: "); print_short(rx_packet_vlan); write_char('\n');
 #endif
-		if (uip_buf[0] == 0x01 && uip_buf[1] == 0x80 && uip_buf[2] == 0xc2 // STP packet?
+		if (stpEnabled && uip_buf[0] == 0x01 && uip_buf[1] == 0x80 && uip_buf[2] == 0xc2 // STP packet?
 			&& uip_buf[3] == 0x00 && uip_buf[4] == 0x00 && uip_buf[5] == 0x00) {
-			print_string("STP: \n");
+			stp_in();
-			for (uint8_t i = 0; i < 80; i++) {
+			if (uip_len) {
-				print_byte(uip_buf[i]);
+				print_string("STP TX\n");
-				write_char(' ');
-			}
-			write_char('\n');
-			for (uint8_t i = minPort; i <=maxPort; i++ ) {
-				stp_cnf_send(i);
 				tcpip_output();
 			}
 		} else if (uip_buf[ETHERTYPE_OFFSET] == 0x08 && uip_buf[ETHERTYPE_OFFSET + 1] == 0x06) { // ARP?
@@ -1008,6 +1008,15 @@ void idle(void)
 	handle_rx();
 	// Check UIP for packets to transmit
 	handle_tx();
+	// If STP protocol enabled, decrease STP timers to trigger actions
+	if (stpEnabled) {
+		if (!stp_clock) {
+			stp_clock = STP_TICK_DIVIDER;
+			stp_timers();
+		} else {
+			stp_clock--;
+		}
+	}
 }
 
 
@@ -1641,6 +1650,7 @@ void setup_i2c(void)
 void bootloader(void)
 {
 	ticks = 0;
+	stp_clock = STP_TICK_DIVIDER;
 	sbuf_ptr = 0;
 
 	CKCON = 0;	// Initial Clock configuration
@@ -1720,7 +1730,7 @@ void bootloader(void)
 	REG_SET(RTL837X_REG_SEC_COUNTER, 0x3); write_char(' ');
 	print_reg(RTL837X_REG_SEC_COUNTER);
 #endif
-
+	stpEnabled = 0;
 	nic_setup();
 	vlan_setup();
 	port_l2_setup();