diff --git a/cmd_parser.c b/cmd_parser.c index f976fe2..86fa251 100644 --- a/cmd_parser.c +++ b/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; diff --git a/rtl837x_regs.h b/rtl837x_regs.h index 326acb8..027d926 100644 --- a/rtl837x_regs.h +++ b/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 diff --git a/rtl837x_stp.c b/rtl837x_stp.c index 3053c61..2e6c08a 100644 --- a/rtl837x_stp.c +++ b/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; - uint8_t root_ext; - uint8_t root_mac[6]; + struct bridge root; uint32_t root_path_cost; - uint8_t bridge_prio; - uint8_t bridge_ext; - uint8_t bridge_mac[6]; + struct bridge bridge; 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 - memcpyc(STP_O->bridge_mac, uip_ethaddr.addr, 6); + memcpy(STP_O->root.mac, root_bridge.mac, 6); + memcpyc(STP_O->bridge.mac, uip_ethaddr.addr, 6); - STP_O->root_prio = 0x80; - STP_O->root_ext = 0x00; + STP_O->root.prio = root_bridge.prio; + STP_O->root.ext = 0x00; STP_O->root_path_cost = 0x00000000; - STP_O->bridge_prio = 0x80; - STP_O->bridge_ext = 0x00; + STP_O->bridge.prio = 0x80; + 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); } diff --git a/rtl837x_stp.h b/rtl837x_stp.h index 9fd2f7b..b1c2a71 100644 --- a/rtl837x_stp.h +++ b/rtl837x_stp.h @@ -2,6 +2,11 @@ #define _RTL837X_STP_H_ #include -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 diff --git a/rtlplayground.c b/rtlplayground.c index b5ea978..a657297 100644 --- a/rtlplayground.c +++ b/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"); - for (uint8_t i = 0; i < 80; i++) { - print_byte(uip_buf[i]); - write_char(' '); - } - write_char('\n'); - for (uint8_t i = minPort; i <=maxPort; i++ ) { - stp_cnf_send(i); + stp_in(); + if (uip_len) { + print_string("STP TX\n"); 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();