Merge pull request #49 from logicog/igmp

Add basic IGMP support.

README.md

@@ -309,7 +309,8 @@ The following documents give further documentation on specific features of
 the RTL837x SoCs:
 - [CPU Port](doc/CpuPort.md)
 - [L2 learning](doc/l2.md) 
-- [Mirroring](doc/mirroring.md)
+- [CPU Port](doc/CpuPort.md)
+- [IGMP (IP-MC streaming)](doc/igmp.md)
 - [SFP+ ports](doc/sfp.md) 
 - [Trunking aka. port aggregation](doc/trunking.md)  
 - [VLAN](doc/vlan.md)

cmd_parser.c

@@ -12,6 +12,7 @@
 #include "rtl837x_regs.h"
 #include "rtl837x_sfr.h"
 #include "rtl837x_stp.h"
+#include "rtl837x_igmp.h"
 #include "uip/uip.h"
 #include "version.h"
 
@@ -741,6 +742,13 @@ void cmd_parser(void) __banked
 				port_l2_forget();
 			else
 				port_l2_learned();
+		} else if (cmd_compare(0, "igmp")) {
+			if (cmd_words_b[1] > 0 && cmd_compare(1, "on"))
+				igmp_enable();
+			else if (cmd_words_b[1] > 0 && cmd_compare(1, "show"))
+				igmp_show();
+			else
+				igmp_setup();  // Reverts to default with IP-MC being flooded
 		} else if (cmd_compare(0, "stp")) {
 			if (cmd_words_b[1] > 0 && cmd_compare(1, "on")) {
 				print_string("STP enabled\n");

doc/igmp.md

@@ -0,0 +1,135 @@
+# IGMP (Internet Group Management Protocol) and MLD (Multicast Listener Discovery)
+IGMP (for IPv4) and MLD (for IPv6) are protocols that control the distribution
+of Layer-3 Multicast packets on the LAN, which otherwise would be flooded across the
+entire network. For this to work, IGMP/MLD messages are sent, in particular
+from MC consumers (e.g. the video-player that plays an IP-Multicast stream), but
+also Multicast-aware routers to control switching of the IP-MC or underlying
+L2-MC packets. The main usage in home networks is IPTV. 
+
+The RTL8372/3 SoC supports managing IPv4-MC using either Destination-IP (the IPv4
+multicast group address)/Source-IP (typically 0.0.0.0) matching or via controlling
+the switching of the underlying L2-MC packets (i.e. packets in 01:00:5e:xx:yy:zz, where
+xx:yy:zz are the LSBs of the IPv4-MC address). The DIP/SIP-based switching is
+not VLAN-aware, meaning a stream will be available in all VLANs if subscribed to.
+This is not a problem in a typical home network, however. The L2-based method
+is VLAN aware, but currently not supported in the software.
+
+Although there is hardware support for IPv6/MLD-based Multicast management (i.e. intelligent
+management by the switch), the current software does not implement managing IPv6 Multicast.
+Instead, all IPv6 Multicast pakets will be flooded to all ports, just as an unmanaged
+switch would do.
+
+The current software support works by trapping IGMP packets (only v3 supported, which
+is used in the vast majority of today's networks) to the CPU of the switch which will
+update the L3 and L2 switching tables to include switch ports in a stream or remove
+them. This trapping to the CPU is also called IGMP snooping. While there is support
+in the HW to handle IGMP/MLD packets (v3 has only limited support) entirely in hardware
+and even send out reports, it is currently not understood
+how this works, and instead IGMP is handled entirely in software, which also allows
+to fully support IGMPv3 packet which are the standard in present-day networks.
+
+## IP-MC control
+The relevant registers for controlling IP-MC switching are:
+```
+#define RTL837X_IPV4_PORT_MC_LM_ACT	0x4f78
+#define RTL837X_IPV6_PORT_MC_LM_ACT	0x4f7c
+#define RTL837X_IGMP_PORT_CFG		0x52a0
+#define IGMP_MAX_GROUP			0x00ff0000
+#define IGMP_PROTOCOL_ENABLE		0x00007c00
+#define IGMP_TRAP			0x0000002a
+#define IGMP_FLOOD			0x00000015
+#define IGMP_ASIC			0x00000000
+#define RTL837X_IGMP_ROUTER_PORT	0x529c
+#define RTL837X_IPV4_UNKN_MC_FLD_PMSK	0x5368
+#define RTL837X_IPV6_UNKN_MC_FLD_PMSK	0x536c
+#define RTL837X_IGMP_TRAP_CFG		0x50bc
+#define IGMP_TRAP_PRIORITY		0x7
+#define IGMP_CPU_PORT			0x00010000
+```
+`RTL837X_IPV4_PORT_MC_LM_ACT/RTL837X_IPV6_PORT_MC_LM_ACT` control the action when an
+IP-MC packet is encountered at a switch port and there is no rule for forwarding in
+the forwarding tables. The default action is to flood such Lookup-Miss packets to all
+ports. This is the configuration without IGMP/MLD enabled.
+
+When IGMP/MLD is turned on, the Lookup-Miss action will be changed to drop such packets
+unless a rule is found in the forwarding tables, which will need to be configured by
+IGMP packets.
+
+Switching on IGMP also configures all ports via `RTL837X_IGMP_PORT_CFG` to trap all
+incoming IGMP packets to the CPU. `RTL837X_IGMP_TRAP_CFG` then is used to configure
+priority and CPU-Port of trapped IGMP/MLD packets. 
+
+Configuration of the IP-MC-forwarding to the listening ports is done by managing the
+forwarding tables of the switch, see [L2 learning](l2.md).
+
+
+## IGMP API
+The code currently provides the following functions:
+```
+void igmp_setup(void) __banked;
+void igmp_enable(void) __banked;
+void igmp_router_port_set(uint16_t pmask) __banked;
+void igmp_packet_handler(void) __banked;
+void igmp_show(void) __banked;
+```c
+`igmp_setup()` is called at boot-time and configures flooding of all IP-MC packets by
+default, as otherwise no IP-MC would be possible in the network.
+
+`igmp_enable()`starts IGMP which cause IGMP packets to be handled by the CPU and forwarding
+of IP-MC packets to be limited to only subscribed ports.
+
+`igmp_router_port_set()`configures forwarding ports for IGMP messages.
+
+`igmp_packet_handler()` implements handling of trapped IGMP packets by the CPU.
+
+`igmp_show()` prints out the IGMP configuration on the CLI.
+
+
+## IGMP configuration on the Serial Console
+For testing the following commands are provided on the serial console:
+```
+> igmp [on/off]
+  Enables or disables IGMP
+
+> igmp show
+  Shows information on IGMP
+```
+
+## LAG configuration via the Web Interface
+Not implemented, yet!
+
+## A Test with IP-MC streaming using vlc
+The following is a simple test verifying the IGMP and IP-MC switching capabilities.
+
+You will need 2 Linux/Windows devices with a GUI plus a switch.
+
+Connect the switch to an MC-aware router (e.g. to your home network). Connect the 2 Linux/Windows
+devices to the switch. The connection to the router makes sure that Linux/Windows will send
+out IGMP messages on the ports connected to the switch, which they will only do if they are aware
+that there is a MC-aware router in the network. Make sure the 2 GUI devices are in the home network
+(e.g. via DHCP).
+
+Start streaming on one of the Linux/Windows machines:
+```
+$ vlc your_video.mp4 --sout="#std{access=udp, mux=ts, dst=239.255.0.1:8090}"
+```
+At this point you should see all switch ports flickering heavily as the MC stream is switched to all
+switch ports, including flooding your home network. If you do not see any packets arriving at the switch,
+you can force the output interface of vlc by using `--miface=<ifname>`
+
+Enable IGMP on the switch-CLI:
+```
+> igmp on
+```
+The flickering should now stop on all ports except the port where the streaming device is connected:
+the switch drops all IP-MC packets as there are no listeners.
+
+Now, on the second Linux/Windows device start listening to the stream:
+```
+$ vlc udp://@239.255.0.1:8090
+```
+You should see the port-led of the port the displaying machine is connected to, to start flickering
+and after some synchronization, the video should start playing.
+
+Stopping vlc should also switching of the IP-MC frames to the listening device, i.e. the port-leds
+should stop flickering.

rtl837x_common.h

@@ -17,6 +17,11 @@
 #define PMASK_6		0x1f8
 #define PMASK_CPU	0x200
 
+// Defines a port mask for dropping all packets on Lookup-miss
+#define LOOKUP_MISS_DROP_6  0x00015540
+#define LOOKUP_MISS_DROP_9  0x00015555
+#define LOOKUP_MISS_FLOOD   0x00000000
+
 // The serial buffer. Defines the command line size
 // Must be 2^x and <= 128
 #define SBUF_SIZE 128
@@ -65,6 +70,14 @@ struct flash_region_t {
 
 extern __xdata uint8_t uip_buf[UIP_CONF_BUFFER_SIZE+2];
 
+// 8899 04 0000 20 0004
+struct rtl_tag {
+	uint16_t tag;
+	uint8_t version;
+	uint8_t reason;
+	uint16_t flags;
+	uint16_t pmask;  // A bit mask for a TX pkt, 4-bit port-number for RX
+};
 
 // Headers for calls in the common code area (HOME/BANK0)
 void print_string(__code char *p);

rtl837x_igmp.c

@@ -6,6 +6,8 @@
 // #define REGDBG
 // #define DEBUG
 
+#define IPMC_USES_L3MC
+
 #include <stdint.h>
 #include "rtl837x_common.h"
 #include "rtl837x_sfr.h"
@@ -15,37 +17,361 @@
 
 extern __code struct machine machine;
 
+#include "uip.h"
+
+#pragma codeseg BANK1
+#pragma constseg BANK1
+
+extern __xdata uint8_t cpuPort;
+extern __xdata uint8_t sfr_data[4];
+
+extern __xdata uint8_t uip_buf[UIP_CONF_BUFFER_SIZE + 2];
+
+__xdata uint16_t idx;
+
+#ifdef IPMC_USES_L3MC
+struct ipmc_table_entry {
+	uint8_t sip[4];
+	uint8_t dip[4];
+	uint16_t pmask;
+	uint8_t igmp_index;
+	uint8_t igmp_asic;
+};
+static __xdata struct ipmc_table_entry entry;
+#else
+struct l2mc_table_entry {
+	uint8_t mac[6];
+	uint16_t vlan;
+	uint16_t pmask;
+	uint8_t is_svl;
+	uint8_t igmp_index;
+	uint8_t igmp_asic;
+};
+static __xdata struct l2mc_table_entry entry;
+#endif
+
+struct igmp_pkt {
+	uint8_t ipv4mc_addr[6];
+	uint8_t src_addr[6];
+	struct rtl_tag rtl_tag;
+	uint16_t ipv4_tag;
+	uint8_t hlen;
+	uint8_t dscp;
+	uint16_t len;
+	uint16_t id;
+	uint16_t flags;
+	uint8_t ttl;
+	uint8_t protocol;
+	uint16_t checksum;
+	uint8_t src_ip[4];
+	uint8_t dst_ip[4];
+	uint8_t ip_opt;
+	uint8_t ip_len;
+	uint16_t ra;
+	uint8_t igmp_type;
+	uint8_t igmp_res1;
+	uint16_t igmp_checksum;
+	uint16_t igmp_res2;
+	uint16_t igmp_records;
+	uint8_t igmp_rtype;
+	uint8_t igmp_auxlen;
+	uint16_t igmp_nsrc;
+	uint8_t mc_ip[4];
+};
+#define IGMP_I ((__xdata struct igmp_pkt *)&uip_buf[0])
+
+
 void igmp_setup(void) __banked
 {
 	uint8_t i;
-
+	print_string("igmp_setup called\n");
-	// For now, forward all unkown MC pkts (2 bits per port. 00: flood via floodmask, 01: drop, 10: trap, 11: to rport)
+	// For now, forward all unkown IP-MC pkts (2 bits per port. 00: flood via floodmask, 01: drop, 10: trap, 11: to rport)
-	REG_SET(RTL837X_MC_LOOKUPMISS_ACTIONS, 0x00000000); //0x4f78
+	REG_SET(RTL837X_IPV4_PORT_MC_LM_ACT, LOOKUP_MISS_FLOOD);
+	REG_SET(RTL837X_IPV6_PORT_MC_LM_ACT, LOOKUP_MISS_FLOOD);
 
 	// Define ports where unknown MC addresses are flooded to:
-	REG_SET(RTL837X_MC_FLOODMASK, machine.isRTL8373 ? PMASK_9 : PMASK_6);
+	REG_SET(RTL837X_IPV4_UNKN_MC_FLD_PMSK, machine.isRTL8373? PMASK_9: PMASK_6);
+	REG_SET(RTL837X_IPV6_UNKN_MC_FLD_PMSK, machine.isRTL8373? PMASK_9: PMASK_6);
 
 	// Enable lookup of IPv4 MC addresses in table
-	reg_bit_set(RTL837X_L2_CTRL, 3); // 0x5350
+	reg_bit_set(RTL837X_L2_CTRL, L2_CTRL_LUT_IPMC_HASH);
 
 	// Configure per-port IGMP configuration, bits 0-10 enable MC protocol snooping,
 	// bits 16-24 configure max MC group used by that port. For now all protocols are flooded (01)
 	for (i = machine.min_port; i <= machine.max_port; i++)
 		REG_SET(RTL837X_IGMP_PORT_CFG + (i << 2), 0x00ff7c15); 
+
+	/* Configure per-port IGMP operations when protocol messages are received
+	 * bits 0-9 enable MC protocol snooping
+	 * bit 10: Enable dynamic router port learning
+	 * bit 11: Enable MRP (Multicast Routing Protocol)
+	 * bit 12: Allow fast leave
+	 * bit 13: Allow IGMP reporting
+	 * bit 14: Allow queries
+	 * bits 16-24 configure max MC group used by that port.
+	 * Operations for IGMP packets are:
+	 * 00: handle in HW by ASIC
+	 * 01: flood
+	 * 10: trap
+	 * 11: drop
+	 * Bits 0-1: IGMPv1, 2-3: IGMPv2, 4-5: IGMPv3, 6-7: MLDv1 (for IPv6), 8-9: MLDv2
+	 * For now the IGMP protocols are flooded (01), MLD which MAC-based is handled by ASIC
+	 * All messages are allowed and maximum MC group is 0xff
+	 */
+	for (i = machine.min_port; i <= machine.max_port; i++) {
+		print_byte(i); write_char(':');
+		REG_SET(RTL837X_IGMP_PORT_CFG + (i << 2), IGMP_MAX_GROUP | IGMP_PROTOCOL_ENABLE | IGMP_FLOOD);
+		write_char('\n');
+	}
+
+/*	// Allow all physical ports to be dynamic router ports
+	reg_read_m(RTL837X_IGMP_ROUTER_PORT);
+	if (isRTL8373) {
+		REG_WRITE(RTL837X_IGMP_ROUTER_PORT, PMASK_9 >> 8, PMASK_9 & 0xff, sfr_data[1], sfr_data[0]);
+	} else {
+		REG_WRITE(RTL837X_IGMP_ROUTER_PORT, PMASK_6 >> 8, PMASK_6 & 0xff, sfr_data[1], sfr_data[0]);
+	}
+*/
 }
 
 
 void igmp_enable(void) __banked
 {
-	uint8_t i;
+	print_string("igmp_enable called\n");
+	// Configure trapping of unhandled IGMP protocol packets to CPU
+	REG_SET(RTL837X_IGMP_TRAP_CFG, IGMP_CPU_PORT | IGMP_TRAP_PRIORITY);
 
-	REG_SET(0x50bc, 00010007); // Trap control?
+	// Drop unknown IP-MC packets
-
+	REG_SET(RTL837X_IPV4_PORT_MC_LM_ACT, machine.isRTL8373? LOOKUP_MISS_DROP_9: LOOKUP_MISS_DROP_6);
-	// Drop unknown MC messages
+//	REG_SET(RTL837X_IPV6_PORT_MC_LM_ACT, machine.isRTL8373? LOOKUP_MISS_DROP_9: LOOKUP_MISS_DROP_6);
-	REG_SET(RTL837X_MC_LOOKUPMISS_ACTIONS, 0x00015540); //0x4f78
 
 	// Configure per-port IGMP configuration, bits 0-10 enable MC protocol snooping,
 	// bits 16-24 configure max MC group used by that port. Trap to CPU (10)
-	for (i = machine.min_port; i <= machine.max_port; i++)
+	for (uint8_t i = machine.min_port; i <= machine.max_port; i++) {
-		REG_SET(RTL837X_IGMP_PORT_CFG + (i << 2), 0x00ff7c2a); // 0x00ff7000: Handling by ASIC (00)
+		REG_SET(RTL837X_IGMP_PORT_CFG + (i << 2), IGMP_MAX_GROUP | IGMP_PROTOCOL_ENABLE | IGMP_TRAP);
+	}
+}
+
+
+/*
+ * Configures the IGMP static router port(s) that will receive all IGMP
+ * Report and Leave messages
+ */
+void igmp_router_port_set(uint16_t pmask) __banked
+{
+	print_string("igmp_router_port_set: "); print_short(pmask); print_string(", currently set to:\n");
+	reg_read_m(RTL837X_IGMP_ROUTER_PORT);
+	print_sfr_data(); write_char('\n');
+	REG_WRITE(RTL837X_IGMP_ROUTER_PORT, sfr_data[0], sfr_data[1], pmask >> 8, pmask & 0xff);
+}
+
+
+/*
+ * IGMP show the current entries and state
+ */
+void igmp_show(void) __banked
+{
+	print_string("igmp_show called\n");
+	for (uint8_t i = machine.min_port; i <= machine.max_port; i++) {
+		write_char('0' + i); write_char(':');
+		reg_read_m(RTL837X_IGMP_PORT_CFG + (i << 2));
+		print_sfr_data();
+		write_char('\n');
+	}
+	// TODO: print all L3MC entries in the table
+}
+
+
+#ifdef IPMC_USES_L3MC
+void entry_to_l3mc(void)
+{
+	REG_WRITE(RTL837x_TBL_DATA_IN_A, entry.sip[0], entry.sip[1], entry.sip[2], entry.sip[3]);
+	REG_WRITE(RTL837x_TBL_DATA_IN_B, ((entry.pmask & 0x3) << 6) | (entry.dip[0] & 0xf) | 0x10, entry.dip[1], entry.dip[2], entry.dip[3]);
+	REG_WRITE(RTL837x_TBL_DATA_IN_C, 0x00, entry.igmp_asic & 1, entry.igmp_index, entry.pmask >> 2);
+}
+#else
+void entry_to_l2mc(void)
+{
+	// R5cb8-5e004201 R5cbc-20010100 R5cc0-00000020 R5cac-00000403
+	REG_WRITE(RTL837x_TBL_DATA_IN_A, entry.mac[2], entry.mac[3], entry.mac[4], entry.mac[5]);
+	REG_WRITE(RTL837x_TBL_DATA_IN_B, 0x20 | ((entry.pmask & 0x3) << 6) | (entry.vlan >> 8), entry.vlan & 0xff, entry.mac[0], entry.mac[1]);
+	REG_WRITE(RTL837x_TBL_DATA_IN_C, 0x00, entry.igmp_asic & 1, entry.igmp_index, entry.pmask >> 2);
+}
+#endif
+
+
+void igmp_packet_handler(void) __banked
+{
+	// By default we do not send anything out
+	uip_len = 0;
+
+#ifdef DEBUG
+	print_string("\nIPv4 MC packet:\n");
+	for (uint8_t i = 0; i < 80; i++) {
+		print_byte(uip_buf[i]);
+		write_char(' ');
+	}
+	write_char('\n');
+#endif
+	if (IGMP_I->protocol != 2)
+		return;
+#ifdef DEBUG
+	print_string("Found IGMP, type: "); print_byte(IGMP_I->igmp_type); write_char('\n');
+#endif
+	// We react to IGMPv1/v2 and v3 membership reports
+	if (!(IGMP_I->igmp_type == 0x12 || IGMP_I->igmp_type == 0x16 || IGMP_I->igmp_type == 0x22))
+		return;
+#ifdef DEBUG
+	print_string("IGMP membership report, type "); print_byte(IGMP_I->igmp_rtype); write_char('\n');
+#endif
+
+#ifdef IPMC_USES_L3MC
+	memset(&entry, 0, sizeof(struct ipmc_table_entry));
+	// For IPv4 MC, the Source-IP is 0.0.0.0
+	entry.sip[0] = 0x00; entry.sip[1] = 0x00; entry.sip[2] = 0x00; entry.sip[3] = 0x00;
+	// For IPv4 MC, the Destination-IP is the IPv4 MC address
+	entry.dip[0] = IGMP_I->mc_ip[0]; entry.dip[1] = IGMP_I->mc_ip[1]; entry.dip[2] = IGMP_I->mc_ip[2]; entry.dip[3] = IGMP_I->mc_ip[3];
+	entry_to_l3mc();
+#else
+	/* The L2 Multicast MAC for IP-Multicast is 01:00:5e:xx:yy:zz, where
+	 * xx = MC_IP[1] & 0x7f
+	 * yy = MC_IP[2]
+	 * zz = MC_IP[3]
+	 */
+	memset(&entry, 0, sizeof(struct l2mc_table_entry));
+	entry.mac[0] = 0x01; entry.mac[1] = 0x00; entry.mac[2] = 0x5e;
+	entry.mac[3] = IGMP_I->mc_ip[1] & 0x7f; entry.mac[4] = IGMP_I->mc_ip[2]; entry.mac[5] = IGMP_I->mc_ip[3];
+	entry.vlan = 1; //TODO: Get this out of the packet and compare with VLAN table!
+	entry_to_ipmc();
+#endif
+	// Wait for any pending Table operations to end
+	do {
+		reg_read_m(RTL837X_TBL_CTRL);
+	} while (sfr_data[3] & 1);
+
+	reg_read_m(RTL837x_TBL_DATA_0);
+#ifdef DEBUG
+	print_sfr_data();
+#endif
+	sfr_data[2] &= 0x3f;  // Sets the Read-method to 0 (why MAC-lookup?) and clear the CLEAR-Entry bit
+	sfr_data[1] &= 0xf8;
+	reg_write_m(RTL837x_TBL_DATA_0);
+#ifdef DEBUG
+	print_string(" l2 ctrl now: ");
+	print_sfr_data();
+#endif
+	// First try to find entry to see whether it needs to be updated
+	REG_WRITE(RTL837X_TBL_CTRL, 0x00, 0x00, TBL_L2_UNICAST, TBL_EXECUTE);
+	do {
+		reg_read_m(RTL837X_TBL_CTRL);
+	} while (sfr_data[3] & 0x1);
+#ifdef DEBUG
+	print_string("\nsearch done\n");
+	print_string("Table data searched:\n");
+	reg_read_m(RTL837x_TBL_DATA_IN_A);
+	print_sfr_data(); write_char(' ');
+	reg_read_m(RTL837x_TBL_DATA_IN_B);
+	print_sfr_data(); write_char(' ');
+	reg_read_m(RTL837x_TBL_DATA_IN_C);
+	print_sfr_data(); write_char('\n');
+	print_string("Table data gotten:\n");
+	reg_read_m(RTL837x_L2_DATA_OUT_A); write_char(' ');
+	print_sfr_data();
+	reg_read_m(RTL837x_L2_DATA_OUT_B);
+	print_sfr_data(); write_char(' ');
+	reg_read_m(RTL837x_L2_DATA_OUT_C);
+	print_sfr_data(); write_char('\n');
+	print_string("Result: ");
+#endif
+	reg_read_m(RTL837x_TBL_DATA_0);
+#ifdef DEBUG
+	print_sfr_data();
+#endif
+	idx = ((sfr_data[2] & 0xf) << 8) | sfr_data[3];
+	if (IGMP_I->igmp_rtype == 0x4) {// Join group
+		if (sfr_data[2] & 0x10) {
+			print_string("\nIGMP-Entry FOUND\n");
+			reg_read_m(RTL837x_L2_DATA_OUT_B);
+			entry.pmask = sfr_data[0] >> 6;
+			reg_read_m(RTL837x_L2_DATA_OUT_C);
+			entry.pmask |= ((uint16_t)sfr_data[3]) << 2;
+		}
+		// Update (found) entry with portmask from trapped Packet
+		entry.pmask |= (1L << (IGMP_I->rtl_tag.pmask >> 8));  // Swap bytes from network order, only 4 LSB count
+//		print_string("\nPort-Mask: "); print_short(entry.pmask); write_char('\n');
+	} else if (IGMP_I->igmp_rtype == 0x3){  // Leave group
+		if (sfr_data[2] & 0x10) {
+			print_string("\nIGMP_Entry FOUND\n");
+			reg_read_m(RTL837x_L2_DATA_OUT_B);
+			entry.pmask = sfr_data[0] >> 6;
+			reg_read_m(RTL837x_L2_DATA_OUT_C);
+			entry.pmask |= ((uint16_t)sfr_data[3]) << 2;
+#ifdef DEBUG
+			print_string("Portmask: ");
+			print_short(entry.pmask);
+			print_string("Index: ");
+			print_short(idx);
+			write_char('\n');
+#endif
+			// Remove portmask of IGMP packet from entry
+			entry.pmask &= ~(1L << (IGMP_I->rtl_tag.pmask >> 8));  // Swap bytes from network order, only 4 LSB count
+//			print_string("\nPort-Mask: "); print_short(entry.pmask); write_char('\n');
+		} else {
+			print_string("IGMP Entry already deleted\n");
+			return;
+		}
+		if (!entry.pmask && idx) { // No more ports in that group and an actual entry?
+			// Delete Entry
+			reg_read_m(RTL837x_TBL_DATA_0);
+			sfr_data[1] |= 0x04;	// Clear entry
+			reg_write_m(RTL837x_TBL_DATA_0);
+			REG_WRITE(RTL837X_TBL_CTRL, idx >> 8, idx & 0xff, TBL_L2_UNICAST, TBL_WRITE | TBL_EXECUTE);
+			do {
+				reg_read_m(RTL837X_TBL_CTRL);
+			} while (sfr_data[3] & 0x1);
+			print_string("IGMP Entry deleted\n");
+			return;
+		}
+	} else {  // Unknown message: ignore.
+		return;
+	}
+
+	if (!entry.pmask)
+		return;
+	print_string("Updating IGMP entry\n");
+	// Write the updated entry
+#ifdef IPMC_USES_L3MC
+	entry_to_l3mc();
+#else
+	entry_to_ipmc();
+#endif
+	reg_read_m(RTL837x_TBL_DATA_0);
+#ifdef DEBUG
+	print_sfr_data();
+#endif
+	sfr_data[2] &= 0x3f;  // Set the Read-method to 0 and clear the CLEAR-Entry bit
+	sfr_data[1] &= 0xf8;
+	reg_write_m(RTL837x_TBL_DATA_0);
+#ifdef DEBUG
+	print_string(" l2 ctrl now: ");
+	print_sfr_data();
+#endif
+	reg_read_m(RTL837X_TBL_CTRL);
+	REG_WRITE(RTL837X_TBL_CTRL, sfr_data[0], sfr_data[1], TBL_L2_UNICAST, TBL_WRITE | TBL_EXECUTE);
+	do {
+		reg_read_m(RTL837X_TBL_CTRL);
+	} while (sfr_data[3] & 0x1);
+#ifdef DEBUG
+	print_string("\nupdate done\n");
+	print_string("Table data written:\n");
+	reg_read_m(RTL837x_TBL_DATA_IN_A);
+	print_sfr_data(); write_char(' ');
+	reg_read_m(RTL837x_TBL_DATA_IN_B);
+	print_sfr_data(); write_char(' ');
+	reg_read_m(RTL837x_TBL_DATA_IN_C);
+	print_sfr_data(); write_char('\n');
+	print_string("Result: ");
+	reg_read_m(RTL837x_TBL_DATA_0);
+	print_sfr_data();
+#endif
 }

rtl837x_igmp.h

@@ -5,5 +5,8 @@
 
 void igmp_setup(void) __banked;
 void igmp_enable(void) __banked;
+void igmp_router_port_set(uint16_t pmask) __banked;
+void igmp_packet_handler(void) __banked;
+void igmp_show(void) __banked;
 
 #endif

rtl837x_phy.c

@@ -282,6 +282,41 @@ void phy_show(uint8_t port) __banked
 {
 	uint16_t v;
 
+	// The actual PHY speed is in a Realtek propriatary register
+	print_string("\nLink speed: ");
+	phy_read(port, PHY_MMD_CTRL, 0xA434);
+	v = SFR_DATA_U16;
+	v = ((v & 0x0600) >> 7) | ((v & 0x0030) >> 4);
+	switch(v) {
+	case 0:
+		print_string("10M");
+		break;
+	case 1:
+		print_string("100M");
+		break;
+	case 2:
+		print_string("1000M");
+		break;
+	case 3:
+		print_string("500M");
+		break;
+	case 4:
+		print_string("10G");
+		break;
+	case 5:
+		print_string("2500M");
+		break;
+	case 6:
+		print_string("5G");
+		break;
+	default:
+		print_string("10M");
+	}
+	if (v & 0x8)
+		print_string(" full duplex");
+	else
+		print_string(" half duplex");
+
 	phy_read(port,  PHY_MMD_AN, 0x00);
 	v = SFR_DATA_U16;
 	if (!(v & 0x1000)) { // AN disabled, we are in forced mode

rtl837x_regs.h

@@ -121,11 +121,14 @@
 #define TBL_VLAN 	0x03
 
 #define RTL837X_L2_CTRL		0x5350
+#define L2_CTRL_LUT_IPMC_HASH	3
 #define RTL837x_TBL_DATA_0	0x5cb0
 #define RTL837x_L2_DATA_OUT_A	0x5ccc
 #define RTL837x_L2_DATA_OUT_B	0x5cd0
 #define RTL837x_L2_DATA_OUT_C	0x5cd4
 #define RTL837x_TBL_DATA_IN_A	0x5cb8
+#define RTL837x_TBL_DATA_IN_B	0x5cbc
+#define RTL837x_TBL_DATA_IN_C	0x5cc0
 #define RTL837x_PVID_BASE_REG	0x4e1c
 
 #define RTL837x_L2_TBL_FLUSH_CTRL	0x53d4
@@ -180,9 +183,20 @@
 /*
  * Multicast handling
  */
-#define RTL837X_MC_LOOKUPMISS_ACTIONS	0x4f78
+#define RTL837X_IPV4_PORT_MC_LM_ACT	0x4f78
+#define RTL837X_IPV6_PORT_MC_LM_ACT	0x4f7c
 #define RTL837X_IGMP_PORT_CFG		0x52a0
-#define RTL837X_MC_FLOODMASK		0x5368
+#define IGMP_MAX_GROUP			0x00ff0000
+#define IGMP_PROTOCOL_ENABLE		0x00007c00
+#define IGMP_TRAP			0x0000002a
+#define IGMP_FLOOD			0x00000015
+#define IGMP_ASIC			0x00000000
+#define RTL837X_IGMP_ROUTER_PORT	0x529c
+#define RTL837X_IPV4_UNKN_MC_FLD_PMSK	0x5368
+#define RTL837X_IPV6_UNKN_MC_FLD_PMSK	0x536c
+#define RTL837X_IGMP_TRAP_CFG		0x50bc
+#define IGMP_TRAP_PRIORITY		0x7
+#define IGMP_CPU_PORT			0x00010000
 
 /*
  * Loop detection / STP

rtl837x_stp.c

@@ -35,16 +35,6 @@ __xdata uint16_t port_timers[10];
 __xdata uint16_t port_hello[10];
 
 
-// 8899 04 0000 20 0004
-struct rtl_tag {
-	uint16_t tag;
-	uint8_t version;
-	uint16_t dummy;
-	uint8_t flag;
-	uint16_t pmask;
-};
-
-
 struct stp_pkt {
 	uint8_t stp_addr[6];
 	uint8_t src_addr[6];
@@ -161,8 +151,8 @@ void stp_cnf_send(uint8_t port)
 
 	STP_O->rtl_tag.tag = HTONS(0x8899);
 	STP_O->rtl_tag.version = 0x04;
-	STP_O->rtl_tag.dummy = 0x0000;
+	STP_O->rtl_tag.reason = 0x00;
-	STP_O->rtl_tag.flag = 0x20; // WHY ???
+	STP_O->rtl_tag.flags = 0x0020; // Disable L2 learning
 	STP_O->rtl_tag.pmask = HTONS(((uint16_t)1) << port);
 
 	STP_O->msg_len = HTONS(0x27);

rtlplayground.c

@@ -11,6 +11,7 @@
 #include "rtl837x_phy.h"
 #include "rtl837x_port.h"
 #include "rtl837x_stp.h"
+#include "rtl837x_igmp.h"
 #include "cmd_parser.h"
 #include "uip/uipopt.h"
 #include "uip/uip.h"
@@ -848,6 +849,8 @@ void handle_rx(void)
 		rx_packet_vlan |= uip_buf[12 + RTL_TAG_SIZE + 3];
 #ifdef RXTXDBG
 		print_string(" RX-VLAN: "); print_short(rx_packet_vlan); write_char('\n');
+		print_string(" RX dst: "); print_byte(uip_buf[0]); print_byte(uip_buf[1]); print_byte(uip_buf[2]);
+		print_byte(uip_buf[3]); print_byte(uip_buf[4]); print_byte(uip_buf[5]); write_char('\n');
 #endif
 		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) {
@@ -856,13 +859,19 @@ void handle_rx(void)
 				print_string("STP TX\n");
 				tcpip_output();
 			}
+		} else if (uip_buf[0] == 0x01 && uip_buf[1] == 0x00 && uip_buf[2] == 0x5e // IPv4-MC packet?
+			&& uip_buf[3] == 0x00 && uip_buf[4] == 0x00 && uip_buf[5] == 0x16) {
+			igmp_packet_handler();
+			if (uip_len) {
+				tcpip_output();
+			}
 		} else if (uip_buf[ETHERTYPE_OFFSET] == 0x08 && uip_buf[ETHERTYPE_OFFSET + 1] == 0x06) { // ARP?
 			uip_arp_arpin();
 			if (uip_len) {
 			    tcpip_output();
 			}
-		} else if (uip_buf[ETHERTYPE_OFFSET] == 0x08 && uip_buf[ETHERTYPE_OFFSET + 1] == 0x00) {
+		} else if (uip_buf[ETHERTYPE_OFFSET] == 0x08 && uip_buf[ETHERTYPE_OFFSET + 1] == 0x00) { // TCP?
-			uip_arp_ipin();
+			uip_arp_ipin();	// Learn MAC addresses in TCP packets
 			uip_input();
 			if (uip_len) {
 				// Add ethernet frame
@@ -1812,6 +1821,7 @@ void bootloader(void)
 	nic_setup();
 	vlan_setup();
 	port_l2_setup();
+	igmp_setup();
 	uip_init();
 	uip_arp_init();
 	httpd_init();