From 571bfef810b6c76ec1c609d1b1d49bc33b4247f9 Mon Sep 17 00:00:00 2001 From: logicog Date: Mon, 8 Sep 2025 08:18:18 +0200 Subject: [PATCH] Add reading of RSTP-CNF packets and send own CNF packets This reads RSTP configuration packages and updates the state information about the believed Root-bridge. New RSTP packages are sent with the updated information. --- rtl837x_stp.c | 176 +++++++++++++++++++++++++++++++++++++++++++++----- rtl837x_stp.h | 7 +- 2 files changed, 167 insertions(+), 16 deletions(-) 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