gitproxy/RTLPlayground
1
0 Fork 0
mirror of https://github.com/logicog/RTLPlayground.git synced 2026-10-29 17:54:14 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
b3ac10cae7b4a95ca93fab375ee22d6c9802a8d0
RTLPlayground/rtl837x_igmp.c
T
DrDoof 87db2dd4b8 igmp: handle IGMPv1/v2 reports and v2 leaves 
Only frames to 01:00:5e:00:00:16 reached the IGMP handler, so v1/v2
reports, which go to the group address, were never seen. The handler
also took the group from the first v3 group record for every type.

Pass all IPv4 multicast to the handler, which takes the IGMP header
from the IP header length, the group from the IGMP header for v1/v2
reports and v2 leaves and from the first group record for v3, and
returns 0 for anything that is not IGMP so it goes on to the IP stack.
The message and record types have names now.
2026-10-01 20:42:22 +02:00

402 lines
12 KiB
C
Raw Blame History

/*
* This is a driver implementation for the IGMP features for the RTL827x platform
* This code is in the Public Domain
*/
// #define REGDBG
// #define DEBUG
#define IPMC_USES_L3MC
#include <stdint.h>
#include <stddef.h>
#include "rtl837x_common.h"
#include "rtl837x_sfr.h"
#include "rtl837x_regs.h"
#include "rtl837x_igmp.h"
#include "machine.h"
extern __code const struct machine machine;
extern __xdata uint8_t igmpEnabled;
#include "uip.h"
#include "uip_arp.h"
#pragma codeseg BANK1
#pragma constseg BANK1
extern __xdata uint8_t cpuPort;
extern __xdata uint8_t sfr_data[4];
extern __xdata struct machine_runtime machine_detected;
extern __xdata uint8_t uip_buf[UIP_CONF_BUFFER_SIZE + 2];
__xdata uint16_t idx;
#define IGMP_IP_HDR offsetof(struct igmp_pkt, hlen)
#define IGMP_IP_IHL 0x0f
#define IGMP_GROUP_OFS 4
#define IGMP_V3_RECORD_OFS 8
static __xdata uint8_t igmp_grp;
static __xdata uint8_t igmp_join;
#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");
igmpEnabled = 0;
// 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_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:
uint16_t mask = PMASK_6;
if (machine_detected.isRTL8373)
mask = PMASK_9;
REG_SET(RTL837X_IPV4_UNKN_MC_FLD_PMSK, mask);
REG_SET(RTL837X_IPV6_UNKN_MC_FLD_PMSK, mask);
// Enable lookup of IPv4 MC addresses in table
reg_bit_set(RTL837X_L2_CTRL, L2_CTRL_LUT_IPMC_HASH);
/* 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++)
REG_SET(RTL837X_IGMP_PORT_CFG + (i << 2), IGMP_MAX_GROUP | IGMP_PROTOCOL_ENABLE | IGMP_FLOOD);
/* // Allow all physical ports to be dynamic router ports
reg_read_m(RTL837X_IGMP_ROUTER_PORT);
if (machine_detected.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
{
print_string("igmp_enable called\n");
igmpEnabled = 1;
// Configure trapping of unhandled IGMP protocol packets to CPU
REG_SET(RTL837X_IGMP_TRAP_CFG, IGMP_CPU_PORT | IGMP_TRAP_PRIORITY);
// Drop unknown IP-MC packets
REG_SET(RTL837X_IPV4_PORT_MC_LM_ACT, machine_detected.isRTL8373? LOOKUP_MISS_DROP_9: LOOKUP_MISS_DROP_6);
// REG_SET(RTL837X_IPV6_PORT_MC_LM_ACT, machine_detected.isRTL8373? LOOKUP_MISS_DROP_9: LOOKUP_MISS_DROP_6);
// 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 (uint8_t i = machine.min_port; i <= machine.max_port; i++) {
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
uint8_t igmp_packet_handler(void) __banked
{
if (IGMP_I->ipv4_tag != HTONS(UIP_ETHTYPE_IP) || IGMP_I->protocol != IGMP_PROTO)
return 0;
// 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
igmp_grp = IGMP_IP_HDR + ((IGMP_I->hlen & IGMP_IP_IHL) << 2);
#ifdef DEBUG
print_string("Found IGMP, type: "); print_byte(uip_buf[igmp_grp]); write_char('\n');
#endif
switch (uip_buf[igmp_grp]) {
case IGMP_V1_REPORT:
case IGMP_V2_REPORT:
igmp_join = 1;
break;
case IGMP_V2_LEAVE:
igmp_join = 0;
break;
case IGMP_V3_REPORT:
igmp_grp += IGMP_V3_RECORD_OFS;
if (uip_buf[igmp_grp] == IGMP_V3_TO_EXCLUDE)
igmp_join = 1;
else if (uip_buf[igmp_grp] == IGMP_V3_TO_INCLUDE)
igmp_join = 0;
else
return 1;
break;
default:
return 1;
}
igmp_grp += IGMP_GROUP_OFS;
#ifdef DEBUG
print_string("IGMP join "); print_byte(igmp_join); write_char('\n');
#endif
#ifdef IPMC_USES_L3MC
memset((__xdata uint8_t *)&entry, 0, sizeof(struct ipmc_table_entry));
// For IPv4 MC, the Source-IP is 0.0.0.0
// For IPv4 MC, the Destination-IP is the IPv4 MC address
memcpy(entry.dip, &uip_buf[igmp_grp], sizeof(entry.dip));
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((__xdata uint8_t *)&entry, 0, sizeof(struct l2mc_table_entry));
entry.mac[0] = 0x01; entry.mac[1] = 0x00; entry.mac[2] = 0x5e;
entry.mac[3] = uip_buf[igmp_grp + 1] & 0x7f; entry.mac[4] = uip_buf[igmp_grp + 2]; entry.mac[5] = uip_buf[igmp_grp + 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_join) {// 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 |= ((uint16_t)1) << ((IGMP_I->rtl_tag.pmask >> 8) & 0x0f); // Swap bytes from network order, only 4 LSB count
// print_string("\nPort-Mask: "); print_short(entry.pmask); write_char('\n');
} else { // 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 &= ~(((uint16_t)1) << ((IGMP_I->rtl_tag.pmask >> 8) & 0x0f)); // 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 1;
}
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 1;
}
}
if (!entry.pmask)
return 1;
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
return 1;
}
Reference in new issue View Git Blame Copy Permalink