RTLPlayground/rtl837x_stp.c

/*
 * This is a driver implementation for the Spanning Tree Protocol features for the RTL837x platform
 * This code is in the Public Domain
 *
 * Configurable per 802.1D-2004/802.1w: bridge priority, hello time, max age,
 * forward delay, force-version (RSTP/STP), tx hold count; per port: enable,
 * admin/auto edge, path cost, port priority, BPDU guard, root guard, BPDU
 * filter. CLI: "stp ..." (see stp_parse), status: /stp.json (send_stp).
 *
 * The engine itself stays deliberately simple (no proposal/agreement
 * handshake, no full port-role machine): we elect a root from received BPDUs,
 * promote ports to forwarding after the listen period (or immediately for
 * edge ports), age the root out via max age, and block ports on which we see
 * our own BPDU (loop!) or - with root guard - a better root.
 */

// #define REGDBG
// #define DEBUG

/* Place this module's code and constants in code bank 2 (cf. rtl837x_igmp.c):
 * the always-mapped common area is nearly full, and the full state machine
 * does not fit there. */
#pragma codeseg BANK2
#pragma constseg BANK2

#include <stdint.h>
#include "rtl837x_common.h"
#include "rtl837x_sfr.h"
#include "rtl837x_regs.h"
#include "rtl837x_stp.h"
#include "rtl837x_port.h"	/* port_pvid_get(), port_l2mc_set() */
#include "uip.h"
#include "machine.h"

extern __code struct machine machine;
extern __xdata uint8_t sfr_data[4];
extern __xdata struct machine_runtime machine_detected;	/* owned by rtl837x_port.c */

/* Scratch for stp_fdb_update(), in xdata: plain locals would overflow the
 * near-full internal-RAM overlay (OSEG), cf. rtl837x_lacp.c. */
__xdata uint16_t stp_fdb_vid;
__xdata uint8_t  stp_fdb_i;

extern __xdata struct uip_eth_addr uip_ethaddr;

extern __xdata uint8_t uip_buf[UIP_CONF_BUFFER_SIZE + 2];
extern __xdata uint16_t management_vlan;	/* owned by rtlplayground.c; suppressed per-frame for BPDUs */

/* CLI tokenizer state + helpers (owned by cmd_parser.c, HOME bank) */
extern __xdata uint8_t cmd_buffer[CMD_BUF_SIZE];
extern __xdata uint8_t cmd_words_len;
extern __xdata uint8_t cmd_words_b[15];
uint8_t cmd_compare(uint8_t start, __code uint8_t * cmd);
uint8_t atoi_byte(__xdata uint8_t *out, uint8_t idx);

/* ---- Configuration ---- */
__xdata uint8_t  stp_prio;	/* bridge priority high byte (0x80 = 32768) */
__xdata uint8_t  stp_hello_s;
__xdata uint8_t  stp_maxage_s;
__xdata uint8_t  stp_fwddelay_s;
__xdata uint8_t  stp_rstp;
__xdata uint8_t  stp_txhold;

/* Management failsafe: if any port is held out of Forwarding while no HTTP
 * request has been seen for stp_failsafe_s seconds, assume STP just cut off
 * in-band management (mgmt VLAN rides a blockable front port!) and disable
 * itself, restoring forwarding. Commit-confirm pattern; hardware lockout of
 * 2026-07-20 is the motivating incident. 0 disables the watchdog. */
__xdata uint8_t  stp_failsafe_s;
__xdata uint8_t  stp_failsafe_cnt;	/* seconds left before the trip */
__xdata uint8_t  stp_failsafe_tripped;
extern volatile __xdata uint8_t mgmt_alive;	/* set by httpd on any request */

__xdata uint8_t  stp_pflags[10];
__xdata uint32_t stp_pcost[10];
__xdata uint8_t  stp_pprio[10];
__xdata uint8_t  stp_pp2p[10];

__xdata struct bridge stp_dbridge[10];
__xdata uint16_t stp_dpid[10];
__xdata uint32_t stp_dcost[10];

/* ---- Status / runtime ---- */
__xdata struct bridge root_bridge;
__xdata uint32_t root_bridge_cost;	/* our cost to the root (rx cost + root port cost) */
__xdata uint8_t  stp_root_port;		/* 0xff = we are the root */
__xdata uint16_t stp_tc_count;

__xdata uint16_t port_timers[10];	/* listen-period countdown (0 = not listening) */
__xdata uint16_t port_hello[10];	/* hello TX countdown */
__xdata uint16_t stp_bpdu_age[10];	/* ticks since last BPDU seen on port (saturating) */
__xdata uint8_t  stp_tx_budget[10];	/* tx hold: BPDUs left in the current second */
__xdata uint16_t stp_sec_tick;		/* 1 s window for the tx budget */

/* Scratch (8051: locals would overflow the internal-RAM overlay area) */
__xdata uint8_t  stp_scratch;
__xdata uint8_t  stp_tx_flags_extra;	/* one-shot flags OR-ed into the next BPDU (TCA) */
__xdata uint16_t stp_rxlen;		/* received frame length, saved before uip_len is consumed */
__xdata uint8_t  stp_msg_age;		/* message age of the root info we hold, seconds */
__xdata uint16_t stp_tc_while;		/* ticks left to set the TC flag in our BPDUs */
__xdata uint8_t  stp_i;		/* shared loop iterator (DSEG relief) */
__xdata uint32_t stp_cost_scratch;

/* stp_timers() runs at ~64 Hz (main loop ~256 Hz / (STP_TICK_DIVIDER+1)) */
#define STP_HZ		64
#define STP_EDGE_DELAY	(3 * STP_HZ)	/* auto-edge: forward after 3 s without BPDU */

#define AUTO_COST	20000UL		/* path cost used when stp_pcost == 0 (1G default) */
#define PCOST(i)	(stp_pcost[i] ? stp_pcost[i] : AUTO_COST)

struct stp_pkt {
	uint8_t stp_addr[6];
	uint8_t src_addr[6];
	struct rtl_tag rtl_tag;
	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;
	uint8_t version1_length;	/* RST BPDU only: length of the (empty) v1 part */
};

struct stp_pkt_in {
	uint8_t stp_addr[6];
	uint8_t src_addr[6];
	struct rtl_tag rtl_tag;
	struct vlan_tag vlan_tag;
	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;
	uint8_t version1_length;	/* RST BPDU only: length of the (empty) v1 part */
};

#define STP_O ((__xdata struct stp_pkt *)&uip_buf[RTL_FRAME_DESC_SIZE])
#define STP_I ((__xdata struct stp_pkt_in *)&uip_buf[0])

signed char cmpMAC(__xdata uint8_t *m1, __xdata uint8_t *m2) __reentrant
{
	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;
}


/* Write one port's 2-bit state into the ASIC's MSTP register.
 * 00 disable, 01 blocking, 10 learning, 11 forwarding. */
static void stp_state_set(uint8_t port, uint8_t state) __reentrant
{
	reg_read_m(RTL837X_MSTP_STATES);
	stp_scratch = 3 - (port >> 2);
	sfr_data[stp_scratch] &= ~(uint8_t)(0b11 << ((port << 1) & 0x7));
	sfr_data[stp_scratch] |= (uint8_t)(state << ((port << 1) & 0x7));
	reg_write_m(RTL837X_MSTP_STATES);
}


/* Signal a topology change: flush the stale forwarding entries of the port
 * that changed, count it, and set the TC flag in our BPDUs for one
 * max-age+forward-delay period (802.1D 8.6.14) so the neighbours age their
 * own tables out too. Edge ports are exempt: a host appearing or leaving is
 * not a topology change. */
static void stp_topology_change(uint8_t port) __reentrant
{
	if (stp_pflags[port] & STP_PF_OPEREDGE)
		return;
	stp_tc_count++;
	stp_tc_while = ((uint16_t)stp_maxage_s + stp_fwddelay_s) * STP_HZ;
	port_l2_forget_port(port);
}


/* Take the bridge back as root of its own tree (initial state / root aged out) */
static void stp_claim_root(void)
{
	root_bridge.prio = stp_prio;
	root_bridge.ext = 0x00;
	memcpy(root_bridge.mac, uip_ethaddr.addr, 6);
	root_bridge_cost = 0;
	stp_root_port = 0xff;
	stp_msg_age = 0;
}


void stp_cnf_send(uint8_t port) __reentrant
{
	if (!(stp_pflags[port] & STP_PF_ENABLED) || (stp_pflags[port] & (STP_PF_FILTER | STP_PF_TRIPPED)))
		return;
	if (!stp_tx_budget[port])	/* tx hold count exhausted for this second */
		return;
	stp_tx_budget[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;

	STP_O->rtl_tag.tag = HTONS(RTL_FRAME_TAG_ID);
	STP_O->rtl_tag.version = RTL_FRAME_TAG_VERSION;
	STP_O->rtl_tag.reason = 0x00;
	/* Through HTONS like every tag field: raw 0x0020 lands on the wire as
	 * 0x2000 (EFID), the ASIC fails to parse the tag and floods the frame
	 * with the 0x8899 header still on it (same bug class as LACP had).
	 * NOTE: no RTL_TAG_KEEP here - hardware-verified that KEEP on an
	 * LLC/802.3 (length-field) frame makes the ASIC drop it entirely,
	 * while the same flag works fine on ethertype frames (LACP). */
	STP_O->rtl_tag.flags = HTONS(RTL_TAG_LEARN_DIS);
	STP_O->rtl_tag.pmask = HTONS(((uint16_t)1) << port);

	STP_O->dsap = 0x42;
	STP_O->ssap = 0x42;
	STP_O->ctrl = 0x03;
	STP_O->proto = 0x0000;
	if (stp_rstp) {
		/* 802.3 length = LLC (3) + RST BPDU body (36, incl. version1_length) */
		STP_O->msg_len = HTONS(0x27);
		STP_O->version = 0x02;		/* RSTP */
		STP_O->bpdu_type = 0x02;	/* Rapid Spanning Tree BPDU */
		/* flags: role designated (0b11 << 2) + learning + forwarding */
		STP_O->flags = 0x3c;
	} else {
		/* 802.3 length = LLC (3) + Config BPDU body (35) */
		STP_O->msg_len = HTONS(0x26);
		STP_O->version = 0x00;		/* legacy STP */
		STP_O->bpdu_type = 0x00;	/* Config BPDU */
		STP_O->flags = 0x00;
	}
	if (stp_tc_while)
		STP_O->flags |= 0x01;		/* Topology Change */
	STP_O->flags |= stp_tx_flags_extra;	/* e.g. TCA in reply to a TCN */
	stp_tx_flags_extra = 0;

	memcpy(STP_O->src_addr, uip_ethaddr.addr, 6);
	memcpy(STP_O->root.mac, root_bridge.mac, 6);
	memcpy(STP_O->bridge.mac, uip_ethaddr.addr, 6);

	STP_O->root.prio = root_bridge.prio;
	STP_O->root.ext = root_bridge.ext;
	/* Our root path cost, big-endian (0 while we are the root ourselves) */
	STP_O->root_path_cost = ((root_bridge_cost & 0xff) << 24)
	                      | ((root_bridge_cost & 0xff00) << 8)
	                      | ((root_bridge_cost >> 8) & 0xff00)
	                      | (root_bridge_cost >> 24);

	STP_O->bridge.prio = stp_prio;
	STP_O->bridge.ext = 0x00;

	STP_O->port_prio = stp_pprio[port];
	STP_O->port_id = port + 1;
	/* Message age, incremented by one second per bridge we relay through.
	 * The timer fields are in 1/256 s on the wire, and sdcc stores uint16
	 * little-endian, so assigning the plain second count lands the value in
	 * the high (seconds) octet - see age_max/hello/fwd_delay below. */
	STP_O->age = (stp_root_port == 0xff) ? 0 : (uint16_t)(stp_msg_age + 1);
	STP_O->age_max = stp_maxage_s;
	STP_O->hello = stp_hello_s;
	STP_O->fwd_delay = stp_fwddelay_s;
	STP_O->version1_length = 0;	/* RST BPDU: no version-1 information */

	/* BPDUs are link-local and must egress untagged: with a management VLAN
	 * set, tcpip_output() splices an 802.1Q tag after the SA, shifting the
	 * in-frame rtl_tag out of the position the ASIC parses - the CPU tag then
	 * leaks onto the wire as 0x8899 and the BPDU is flooded, not sent.
	 * Hardware-verified fix, same as lacp_send(). */
	{
	uint16_t saved_mgmt_vlan = management_vlan;
	management_vlan = 0;
	/* A legacy Config BPDU body is 35 bytes - without the trailing
	 * version-1 length byte that only the RST BPDU (36 bytes) carries. */
	uip_len = stp_rstp ? sizeof(struct stp_pkt) : sizeof(struct stp_pkt) - 1;
	tcpip_output();
	management_vlan = saved_mgmt_vlan;
	}
}


void stp_in(void) __banked
{
	/* Robustness: never read fields past the received frame. 33 covers the
	 * header through bpdu_type; the full Config/RST body is re-checked below.
	 * (uip_len is consumed and zeroed at the end - keep a local view.) */
	if (uip_len < 33) {
		uip_len = 0;
		return;
	}
	stp_rxlen = uip_len;

	// By default we do not send anything out (handle_rx would TX otherwise)
	uip_len = 0;

	/* Ingress port: low nibble of the CPU tag's pmask on RX */
	stp_scratch = ((uint8_t)HTONS(STP_I->rtl_tag.pmask)) & 0x0f;
	if (stp_scratch < machine.min_port || stp_scratch > machine.max_port)
		return;
	{
	__xdata static uint8_t port_l;	/* NOT stp_scratch: stp_state_set() clobbers it */
	uint8_t port = (port_l = stp_scratch);
	(void)port_l;

	// Make sure this is the type of (R)STP packet we are interested in:
	if (!(STP_I->dsap == 0x42 && STP_I->ssap == 0x42 && STP_I->ctrl == 0x03))
		return;
	if (STP_I->proto)
		return;
	/* Accept RSTP BPDUs (v2 type 2), legacy Config BPDUs (v0 type 0) and
	 * legacy TCN BPDUs (v0 type 0x80, 4-byte body) */
	if (!((STP_I->version == 2 && STP_I->bpdu_type == 2)
	      || (STP_I->version == 0
	          && (STP_I->bpdu_type == 0 || STP_I->bpdu_type == 0x80))))
		return;

	if (!(stp_pflags[port] & STP_PF_ENABLED) || (stp_pflags[port] & STP_PF_FILTER))
		return;

	/* BPDU guard: an edge-facing port must never see a BPDU - shut it down. */
	if (stp_pflags[port] & STP_PF_BPDUGUARD) {
		print_string("STP: BPDU guard tripped, disabling port ");
		print_byte(port); write_char('\n');
		stp_pflags[port] |= STP_PF_TRIPPED;
		stp_state_set(port, 0b00);
		stp_tc_count++;
		return;
	}

	stp_bpdu_age[port] = 0;

	if (STP_I->bpdu_type == 0x80) {
		/* TCN: a downstream bridge reports a topology change. Acknowledge it
		 * on this port so the sender stops repeating; the change itself is
		 * counted (and, once implemented, propagated rootward). */
		stp_tx_flags_extra = 0x80;	/* Topology Change Acknowledgment */
		stp_cnf_send(port);		/* transmits internally */
		uip_len = 0;			/* ...so handle_rx must not TX again */
		stp_tc_count++;
		return;
	}

	/* Everything below reads the full Config/RST body. */
	if (stp_rxlen < 64)
		return;

	/* Our own BPDU coming back at us = a loop in the network. Block the port
	 * for a listen period; if the loop persists the BPDUs keep arriving and
	 * the port stays blocked. */
	if (cmpMAC(STP_I->bridge.mac, uip_ethaddr.addr) == 0) {
		if (port_timers[port] == 0 && !(stp_pflags[port] & STP_PF_TRIPPED)) {
			print_string("STP: loop detected on port ");
			print_byte(port); write_char('\n');
			stp_state_set(port, 0b01);
			port_timers[port] = (uint16_t)stp_fwddelay_s * STP_HZ;
			stp_pflags[port] &= ~STP_PF_OPEREDGE;
			stp_topology_change(port);
		}
		return;
	}

	/* Better root than the one we know? */
	if (STP_I->root.prio < root_bridge.prio
		|| ((STP_I->root.prio == root_bridge.prio) && cmpMAC(STP_I->root.mac, root_bridge.mac) < 0)) {
		/* Root guard: this port must never become our path to the root. */
		if (stp_pflags[port] & STP_PF_ROOTGUARD) {
			print_string("STP: root guard blocking port ");
			print_byte(port); write_char('\n');
			stp_state_set(port, 0b01);
			port_timers[port] = (uint16_t)stp_fwddelay_s * STP_HZ;
			stp_pflags[port] &= ~STP_PF_OPEREDGE;
			return;
		}
		print_string("Updating Root bridge\n");
		root_bridge.prio = STP_I->root.prio;
		root_bridge.ext = STP_I->root.ext;
		memcpy(root_bridge.mac, STP_I->root.mac, 6);
		stp_root_port = port;
		stp_tc_count++;
	}

	/* Refresh our cost to the root when the update comes in on the root port */
	if (port == stp_root_port) {
		/* Age of the information we now hold (see the TX note on the wire
		 * format); saturate rather than wrap on absurd input. */
		stp_msg_age = (STP_I->age > 254) ? 254 : (uint8_t)STP_I->age;
		stp_cost_scratch = STP_I->root_path_cost;
		/* big-endian on the wire */
		root_bridge_cost = ((stp_cost_scratch & 0xff) << 24)
		                 | ((stp_cost_scratch & 0xff00) << 8)
		                 | ((stp_cost_scratch >> 8) & 0xff00)
		                 | (stp_cost_scratch >> 24);
		root_bridge_cost += PCOST(port);
	}
	}
}


void stp_timers(void) __banked
{
	/* Refill the per-port tx budgets once per second (tx hold count) */
	if (++stp_sec_tick >= STP_HZ) {
		stp_sec_tick = 0;
		for (stp_i = machine.min_port; stp_i <= machine.max_port; stp_i++)
			stp_tx_budget[stp_i] = stp_txhold;

		/* Management failsafe: plain commit-confirm. While STP is on, ANY
		 * HTTP request re-arms the countdown (the web UI polls /stp.json
		 * every 2 s, so an open browser keeps it alive); stp_failsafe_s
		 * seconds of management silence disable STP and restore the
		 * pre-STP state. Deliberately NOT conditioned on our own MSTP
		 * states: hardware incident 2026-07-21 showed a NEIGHBOR (TP-Link
		 * Easy Smart loop prevention) cutting our uplink in reaction to
		 * our BPDUs while our ASIC was all-forwarding - only going fully
		 * quiet (no BPDU TX) lets such a neighbor recover. */
		if (mgmt_alive) {
			mgmt_alive = 0;
			stp_failsafe_cnt = stp_failsafe_s;
		} else if (stp_failsafe_s && stp_failsafe_cnt && --stp_failsafe_cnt == 0) {
			print_string("STP failsafe: no management activity - disabling STP\n");
			stp_off();
			stpEnabled = 0;
			stp_failsafe_tripped = 1;
			return;
		}
	}

	for (stp_i = machine.min_port; stp_i <= machine.max_port; stp_i++) {
		if (!(stp_pflags[stp_i] & STP_PF_ENABLED))
			continue;

		if (stp_bpdu_age[stp_i] < 0xffff)
			stp_bpdu_age[stp_i]++;

		/* Periodic hello */
		if (port_hello[stp_i])
			port_hello[stp_i]--;
		if (!port_hello[stp_i]) {
			port_hello[stp_i] = (uint16_t)stp_hello_s * STP_HZ;
			/* Only designated ports announce periodically: the root port is
			 * where our own root information comes FROM, and echoing it back
			 * there just feeds the upstream bridge its own data (and looks
			 * like a competing designated bridge on that segment). */
			if (stp_i != stp_root_port)
				stp_cnf_send(stp_i);
		}

		/* Promote a port out of blocking once its listen period expires
		 * with no reason to stay blocked (no better root heard: we are
		 * the designated bridge on that port). */
		if (port_timers[stp_i]) {
			if (!--port_timers[stp_i]) {
				stp_state_set(stp_i, 0b11);
				print_string("STP: port forwarding ");
				print_byte(stp_i); write_char('\n');
				stp_topology_change(stp_i);
			} else if ((stp_pflags[stp_i] & STP_PF_AUTOEDGE)
			           && stp_bpdu_age[stp_i] > STP_EDGE_DELAY) {
				/* Auto edge: nothing talks (R)STP on this port - it is
				 * host-facing, go to forwarding without the full wait. */
				port_timers[stp_i] = 0;
				stp_pflags[stp_i] |= STP_PF_OPEREDGE;
				stp_state_set(stp_i, 0b11);
				print_string("STP: edge port forwarding ");
				print_byte(stp_i); write_char('\n');
			}
		}
	}

	if (stp_tc_while)
		stp_tc_while--;

	/* Age out a root that went silent: reclaim the tree. */
	if (stp_root_port != 0xff
	    && stp_bpdu_age[stp_root_port] > (uint16_t)stp_maxage_s * STP_HZ) {
		print_string("STP: root aged out, claiming root\n");
		stp_claim_root();
		stp_tc_count++;
	}
}


/* Reset all configuration to the 802.1D/802.1w defaults. Called once at boot
 * (before the startup config replays "stp ..." commands over it). */
void stp_defaults(void) __banked
{
	stp_prio = 0x80;	/* 32768 */
	stp_hello_s = 2;
	stp_maxage_s = 20;
	stp_fwddelay_s = 15;
	stp_rstp = 1;
	stp_txhold = 6;
	stp_failsafe_s = 180;
	stp_failsafe_tripped = 0;
	for (stp_i = 0; stp_i < 10; stp_i++) {
		/* enabled, auto-edge on: host-facing ports go forwarding after
		 * 3 s of BPDU silence instead of the full forward delay */
		stp_pflags[stp_i] = STP_PF_ENABLED | STP_PF_AUTOEDGE;
		stp_pcost[stp_i] = 0;	/* auto */
		stp_pprio[stp_i] = 0x80;
		stp_bpdu_age[stp_i] = 0;
		port_timers[stp_i] = 0;
		port_hello[stp_i] = 0;
		stp_tx_budget[stp_i] = 6;
	}
	stp_tc_count = 0;
	stp_tc_while = 0;
	stp_claim_root();
}


/*
 * Steer BPDUs (01:80:C2:00:00:00) while STP runs: one static CPU-only L2
 * multicast entry per PVID in use, so BPDUs reach the CPU without being
 * flooded to other ports (a bridge running STP must consume BPDUs, not
 * relay them - relaying poisons the neighbours' view of the topology).
 *
 * With STP off the same entries are retargeted to all ports + CPU, which
 * restores the previous flood behaviour ("BPDU transparency"): the
 * surrounding spanning tree can keep spanning *through* this switch, which
 * unmanaged setups rely on. Same per-PVID/IVL rules as the LACP steering -
 * see port_l2mc_set() and rtl837x_lacp.c. NOTE: changing a port's PVID
 * while STP runs needs `stp off`/`on` to refresh the entries.
 */
static void stp_fdb_update(__xdata uint16_t pmask)
{
	/* Unlike LACPDUs (always untagged, so per-PVID entries suffice), BPDUs
	 * can arrive VLAN-tagged and then classify into the tag's VID - cover
	 * every VLAN that exists in the VLAN table, plus every port's PVID for
	 * the untagged case. A duplicate VID just overwrites the same slot. */
	for (stp_fdb_vid = 1; stp_fdb_vid < 4095; stp_fdb_vid++) {
		if (vlan_get(stp_fdb_vid) < 0)
			continue;
		if (!(sfr_data[0] & 0x02))	/* bit 1: VLAN table entry valid */
			continue;
		port_l2mc_set(0x00, stp_fdb_vid, pmask);
	}
	for (stp_fdb_i = machine.min_port; stp_fdb_i <= machine.max_port; stp_fdb_i++) {
		stp_fdb_vid = port_pvid_get(stp_fdb_i);
		port_l2mc_set(0x00, stp_fdb_vid, pmask);
	}
}


void stp_setup(void) __banked
{
	print_string("Enabling STP: ");
	sfr_data[0] = sfr_data[1] = sfr_data[2] = sfr_data[3] = 0;
	for (stp_i = machine.min_port; stp_i <= machine.max_port; stp_i++) {
		stp_pflags[stp_i] &= ~(STP_PF_OPEREDGE | STP_PF_TRIPPED);
		stp_bpdu_age[stp_i] = 0;
		stp_tx_budget[stp_i] = stp_txhold;
		if (!(stp_pflags[stp_i] & STP_PF_ENABLED) || (stp_pflags[stp_i] & STP_PF_ADMEDGE)) {
			/* not participating, or admin edge: forwarding immediately */
			if (stp_pflags[stp_i] & STP_PF_ADMEDGE)
				stp_pflags[stp_i] |= STP_PF_OPEREDGE;
			sfr_data[3 - (stp_i >> 2)] |= (uint8_t)(0b11 << ((stp_i << 1) & 0x7));
			port_timers[stp_i] = 0;
		} else {
			/* listen first: blocking until the forward-delay expires */
			sfr_data[3 - (stp_i >> 2)] |= (uint8_t)(0b01 << ((stp_i << 1) & 0x7));
			port_timers[stp_i] = (uint16_t)stp_fwddelay_s * STP_HZ;
		}
		port_hello[stp_i] = (uint16_t)stp_hello_s * STP_HZ;
	}
	sfr_data[1] |= 0x0c; // Do not block the CPU port (bits 3:2 of byte 1 = port 9)
	reg_write_m(RTL837X_MSTP_STATES);

	print_reg(RTL837X_MSTP_STATES); write_char('\n');

	stp_claim_root();

	/* Take BPDUs to the CPU only - we are a participating bridge now. */
	stp_fdb_update(PMASK_CPU);
}


void stp_off(void) __banked
{
	sfr_data[0] = sfr_data[1] = sfr_data[2] = sfr_data[3] = 0;
	for (stp_i = machine.min_port; stp_i <= machine.max_port; stp_i++) {
		// Set STP port state to forwarding
		// States are: 00 disable, 01 blocking, 10 learning, 11 forwarding
		sfr_data[3 - (stp_i >> 2)] |= (uint8_t)(0b11 << ((stp_i << 1) & 0x7));
		stp_pflags[stp_i] &= ~(STP_PF_OPEREDGE | STP_PF_TRIPPED);
		port_timers[stp_i] = 0;
	}
	sfr_data[1] |= 0x0c; // Do not block the CPU port (bits 3:2 of byte 1 = port 9)
	reg_write_m(RTL837X_MSTP_STATES);

	/* Restore BPDU transparency: flood them again like an unmanaged switch. */
	stp_fdb_update(PMASK_CPU | (machine_detected.isRTL8373 ? PMASK_9 : PMASK_6));
}


/* ---- "stp ..." CLI ----
 *   stp on|off
 *   stp prio <0-15>            (bridge priority = n * 4096)
 *   stp hello <1-10> | stp maxage <6-40> | stp fwd <4-30> | stp txhold <1-10>
 *   stp version rstp|stp
 *   stp port <1-9> on|off
 *   stp port <1-9> edge on|off|auto
 *   stp port <1-9> cost <0-255>   (x1000; 0 = auto/20000)
 *   stp port <1-9> prio <0-240>
 *   stp port <1-9> guard none|bpdu|root
 *   stp port <1-9> filter on|off
 */
void stp_parse(void) __banked __reentrant
{
	if (cmd_compare(1, "on")) {
		print_string("STP enabled\n");
		stp_failsafe_tripped = 0;
		stp_failsafe_cnt = stp_failsafe_s;
		stpEnabled = 1;
		stp_setup();
		return;
	}
	if (cmd_compare(1, "off")) {
		print_string("STP disabled\n");
		stp_off();
		stpEnabled = 0;
		return;
	}
	if (cmd_words_len < 3)
		goto err;

	if (cmd_compare(1, "port")) {
		if (cmd_words_len < 4)
			goto err;
		if (atoi_byte(&stp_scratch, cmd_words_b[2]) || stp_scratch < 1 || stp_scratch > 9)
			goto err;
		{
		uint8_t port = machine.phys_to_log_port[stp_scratch - 1];
		/* every sub-command except on/off carries one more argument; without
		 * this check cmd_compare(4,..) would read a stale word from the
		 * PREVIOUS command line (cmd_words_b is not cleared between commands) */
		if (cmd_words_len < 5 && !cmd_compare(3, "on") && !cmd_compare(3, "off"))
			goto err;
		if (cmd_compare(3, "on")) {
			stp_pflags[port] |= STP_PF_ENABLED;
			stp_pflags[port] &= ~STP_PF_TRIPPED;
			if (stpEnabled) {	/* (re)join: listen first */
				stp_state_set(port, 0b01);
				port_timers[port] = (uint16_t)stp_fwddelay_s * STP_HZ;
			}
		} else if (cmd_compare(3, "off")) {
			stp_pflags[port] &= ~STP_PF_ENABLED;
			if (stpEnabled)
				stp_state_set(port, 0b11);	/* plain forwarding */
		} else if (cmd_compare(3, "edge")) {
			stp_pflags[port] &= ~(STP_PF_ADMEDGE | STP_PF_AUTOEDGE);
			if (cmd_compare(4, "on"))
				stp_pflags[port] |= STP_PF_ADMEDGE;
			else if (cmd_compare(4, "auto"))
				stp_pflags[port] |= STP_PF_AUTOEDGE;
			else if (!cmd_compare(4, "off"))
				goto err;
		} else if (cmd_compare(3, "cost")) {
			/* raw 802.1D value, 0..200000000; 0 = auto (speed-based) */
			stp_cost_scratch = 0;
			{
			__xdata uint8_t *cp = &cmd_buffer[cmd_words_b[4]];
			if (*cp < '0' || *cp > '9')
				goto err;
			while (*cp >= '0' && *cp <= '9') {
				stp_cost_scratch = stp_cost_scratch * 10 + (*cp - '0');
				cp++;
			}
			}
			if (stp_cost_scratch > 200000000UL)
				goto err;
			stp_pcost[port] = stp_cost_scratch;
		} else if (cmd_compare(3, "p2p")) {
			if (cmd_compare(4, "auto"))
				stp_pp2p[port] = 0;
			else if (cmd_compare(4, "on"))
				stp_pp2p[port] = 1;
			else if (cmd_compare(4, "off"))
				stp_pp2p[port] = 2;
			else
				goto err;
		} else if (cmd_compare(3, "prio")) {
			if (atoi_byte(&stp_scratch, cmd_words_b[4]))
				goto err;
			stp_pprio[port] = stp_scratch & 0xf0;
		} else if (cmd_compare(3, "guard")) {
			stp_pflags[port] &= ~(STP_PF_BPDUGUARD | STP_PF_ROOTGUARD);
			if (cmd_compare(4, "bpdu"))
				stp_pflags[port] |= STP_PF_BPDUGUARD;
			else if (cmd_compare(4, "root"))
				stp_pflags[port] |= STP_PF_ROOTGUARD;
			else if (!cmd_compare(4, "none"))
				goto err;
		} else if (cmd_compare(3, "filter")) {
			if (cmd_compare(4, "on"))
				stp_pflags[port] |= STP_PF_FILTER;
			else if (cmd_compare(4, "off"))
				stp_pflags[port] &= ~STP_PF_FILTER;
			else
				goto err;
		} else {
			goto err;
		}
		}
		return;
	}

	if (atoi_byte(&stp_scratch, cmd_words_b[2])) {
		if (cmd_compare(1, "version")) {
			if (cmd_compare(2, "rstp"))
				stp_rstp = 1;
			else if (cmd_compare(2, "stp"))
				stp_rstp = 0;
			else
				goto err;
			return;
		}
		goto err;
	}
	if (cmd_compare(1, "prio")) {
		if (stp_scratch > 15)
			goto err;
		stp_prio = stp_scratch << 4;	/* n * 4096, as the BPDU's high byte */
		if (stp_root_port == 0xff)
			stp_claim_root();	/* re-announce with the new priority */
	} else if (cmd_compare(1, "hello")) {
		if (stp_scratch < 1 || stp_scratch > 10)
			goto err;
		stp_hello_s = stp_scratch;
	} else if (cmd_compare(1, "maxage")) {
		if (stp_scratch < 6 || stp_scratch > 40)
			goto err;
		stp_maxage_s = stp_scratch;
	} else if (cmd_compare(1, "fwd")) {
		if (stp_scratch < 4 || stp_scratch > 30)
			goto err;
		stp_fwddelay_s = stp_scratch;
	} else if (cmd_compare(1, "txhold")) {
		if (stp_scratch < 1 || stp_scratch > 10)
			goto err;
		stp_txhold = stp_scratch;
	} else if (cmd_compare(1, "failsafe")) {
		/* 0 disables the management watchdog; otherwise seconds to trip */
		stp_failsafe_s = stp_scratch;
		stp_failsafe_cnt = stp_scratch;
	} else {
		goto err;
	}
	return;
err:
	print_string("Error: stp on|off | prio <0-15> | hello <1-10> | maxage <6-40> | fwd <4-30> | txhold <1-10> | version rstp|stp | port <1-9> on|off|edge|cost|prio|guard|filter ...\n");
}