From 2c618705f0d0fa4601a1007a658d617fc32d1e62 Mon Sep 17 00:00:00 2001 From: goldsimon Date: Tue, 25 Aug 2009 15:24:49 +0000 Subject: [PATCH] task #9033: Support IEEE 802.1q tagged frame (VLAN), New configuration options ETHARP_SUPPORT_VLAN and ETHARP_VLAN_CHECK. --- CHANGELOG | 4 ++ src/include/lwip/opt.h | 10 +++++ src/include/netif/etharp.h | 38 ++++++++++++------- src/netif/etharp.c | 75 +++++++++++++++++++++++++++----------- 4 files changed, 93 insertions(+), 34 deletions(-) diff --git a/CHANGELOG b/CHANGELOG index e9991d30..82bf2a4e 100644 --- a/CHANGELOG +++ b/CHANGELOG @@ -19,6 +19,10 @@ HISTORY ++ New features: + 2009-08-25 Simon Goldschmidt + * opt.h, etharp.h/.c: task #9033: Support IEEE 802.1q tagged frame (VLAN), + New configuration options ETHARP_SUPPORT_VLAN and ETHARP_VLAN_CHECK. + 2009-08-25 Simon Goldschmidt * ip_addr.h, netdb.c: patch #6900: added define ip_ntoa(struct ip_addr*) diff --git a/src/include/lwip/opt.h b/src/include/lwip/opt.h index e389d4e3..1723b35a 100644 --- a/src/include/lwip/opt.h +++ b/src/include/lwip/opt.h @@ -366,6 +366,16 @@ #define ETHARP_TRUST_IP_MAC 1 #endif +/** + * ETHARP_SUPPORT_VLAN==1: support receiving ethernet packets with VLAN header. + * Additionally, you can define ETHARP_VLAN_CHECK to an u16_t VLAN ID to check. + * If ETHARP_VLAN_CHECK is defined, only VLAN-traffic for this VLAN is accepted. + * If ETHARP_VLAN_CHECK is not defined, all traffic is accepted. + */ +#ifndef ETHARP_SUPPORT_VLAN +#define ETHARP_SUPPORT_VLAN 0 +#endif + /* -------------------------------- ---------- IP options ---------- diff --git a/src/include/netif/etharp.h b/src/include/netif/etharp.h index db691d91..72b8d79f 100644 --- a/src/include/netif/etharp.h +++ b/src/include/netif/etharp.h @@ -85,13 +85,34 @@ PACK_STRUCT_END # include "arch/epstruct.h" #endif +#define SIZEOF_ETH_HDR (14 + ETH_PAD_SIZE) + +#if ETHARP_SUPPORT_VLAN + +#ifdef PACK_STRUCT_USE_INCLUDES +# include "arch/bpstruct.h" +#endif +PACK_STRUCT_BEGIN +struct eth_vlan_hdr { + PACK_STRUCT_FIELD(u16_t tpid); + PACK_STRUCT_FIELD(u16_t prio_vid); +} PACK_STRUCT_STRUCT; +PACK_STRUCT_END +#ifdef PACK_STRUCT_USE_INCLUDES +# include "arch/epstruct.h" +#endif + +#define SIZEOF_VLAN_HDR 4 +#define VLAN_ID(vlan_hdr) (htons((vlan_hdr)->prio_vid) & 0xFFF) + +#endif /* ETHARP_SUPPORT_VLAN */ + #ifdef PACK_STRUCT_USE_INCLUDES # include "arch/bpstruct.h" #endif PACK_STRUCT_BEGIN /** the ARP message */ struct etharp_hdr { - PACK_STRUCT_FIELD(struct eth_hdr ethhdr); PACK_STRUCT_FIELD(u16_t hwtype); PACK_STRUCT_FIELD(u16_t proto); PACK_STRUCT_FIELD(u16_t _hwlen_protolen); @@ -106,24 +127,15 @@ PACK_STRUCT_END # include "arch/epstruct.h" #endif -#ifdef PACK_STRUCT_USE_INCLUDES -# include "arch/bpstruct.h" -#endif -PACK_STRUCT_BEGIN -struct ethip_hdr { - PACK_STRUCT_FIELD(struct eth_hdr eth); - PACK_STRUCT_FIELD(struct ip_hdr ip); -} PACK_STRUCT_STRUCT; -PACK_STRUCT_END -#ifdef PACK_STRUCT_USE_INCLUDES -# include "arch/epstruct.h" -#endif +#define SIZEOF_ETHARP_HDR 28 +#define SIZEOF_ETHARP_PACKET (SIZEOF_ETH_HDR + SIZEOF_ETHARP_HDR) /** 5 seconds period */ #define ARP_TMR_INTERVAL 5000 #define ETHTYPE_ARP 0x0806 #define ETHTYPE_IP 0x0800 +#define ETHTYPE_VLAN 0x8100 #define ETHTYPE_PPPOEDISC 0x8863 /* PPP Over Ethernet Discovery Stage */ #define ETHTYPE_PPPOE 0x8864 /* PPP Over Ethernet Session Stage */ diff --git a/src/netif/etharp.c b/src/netif/etharp.c index 73ea2117..3c7d78a0 100644 --- a/src/netif/etharp.c +++ b/src/netif/etharp.c @@ -579,13 +579,21 @@ etharp_find_addr(struct netif *netif, struct ip_addr *ipaddr, void etharp_ip_input(struct netif *netif, struct pbuf *p) { - struct ethip_hdr *hdr; + struct eth_hdr *ethhdr; + struct ip_hdr *iphdr; LWIP_ERROR("netif != NULL", (netif != NULL), return;); /* Only insert an entry if the source IP address of the incoming IP packet comes from a host on the local network. */ - hdr = p->payload; + ethhdr = p->payload; + iphdr = (struct ip_hdr *)((u8_t*)ethhdr + SIZEOF_ETH_HDR); +#if ETHARP_SUPPORT_VLAN + if (ethhdr->type == ETHTYPE_VLAN) { + iphdr = (struct ip_hdr *)((u8_t*)ethhdr + SIZEOF_ETH_HDR + SIZEOF_VLAN_HDR); + } +#endif /* ETHARP_SUPPORT_VLAN */ + /* source is not on the local network? */ - if (!ip_addr_netcmp(&(hdr->ip.src), &(netif->ip_addr), &(netif->netmask))) { + if (!ip_addr_netcmp(&(iphdr->src), &(netif->ip_addr), &(netif->netmask))) { /* do nothing */ return; } @@ -594,7 +602,7 @@ etharp_ip_input(struct netif *netif, struct pbuf *p) /* update ARP table */ /* @todo We could use ETHARP_TRY_HARD if we think we are going to talk * back soon (for example, if the destination IP address is ours. */ - update_arp_entry(netif, &(hdr->ip.src), &(hdr->eth.src), 0); + update_arp_entry(netif, &(iphdr->src), &(ethhdr->src), 0); } @@ -617,6 +625,7 @@ void etharp_arp_input(struct netif *netif, struct eth_addr *ethaddr, struct pbuf *p) { struct etharp_hdr *hdr; + struct eth_hdr *ethhdr; /* these are aligned properly, whereas the ARP header fields might not be */ struct ip_addr sipaddr, dipaddr; u8_t i; @@ -629,24 +638,30 @@ etharp_arp_input(struct netif *netif, struct eth_addr *ethaddr, struct pbuf *p) /* drop short ARP packets: we have to check for p->len instead of p->tot_len here since a struct etharp_hdr is pointed to p->payload, so it musn't be chained! */ - if (p->len < sizeof(struct etharp_hdr)) { - LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE | 1, ("etharp_arp_input: packet dropped, too short (%"S16_F"/%"S16_F")\n", p->tot_len, (s16_t)sizeof(struct etharp_hdr))); + if (p->len < SIZEOF_ETHARP_PACKET) { + LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE | 1, ("etharp_arp_input: packet dropped, too short (%"S16_F"/%"S16_F")\n", p->tot_len, (s16_t)SIZEOF_ETHARP_PACKET)); ETHARP_STATS_INC(etharp.lenerr); ETHARP_STATS_INC(etharp.drop); pbuf_free(p); return; } - hdr = p->payload; + ethhdr = p->payload; + hdr = (struct etharp_hdr *)((u8_t*)ethhdr + SIZEOF_ETH_HDR); +#if ETHARP_SUPPORT_VLAN + if (ethhdr->type == ETHTYPE_VLAN) { + hdr = (struct etharp_hdr *)(((u8_t*)ethhdr) + SIZEOF_ETH_HDR + SIZEOF_VLAN_HDR); + } +#endif /* ETHARP_SUPPORT_VLAN */ /* RFC 826 "Packet Reception": */ if ((hdr->hwtype != htons(HWTYPE_ETHERNET)) || (hdr->_hwlen_protolen != htons((ETHARP_HWADDR_LEN << 8) | sizeof(struct ip_addr))) || (hdr->proto != htons(ETHTYPE_IP)) || - (hdr->ethhdr.type != htons(ETHTYPE_ARP))) { + (ethhdr->type != htons(ETHTYPE_ARP))) { LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE | 1, ("etharp_arp_input: packet dropped, wrong hw type, hwlen, proto, protolen or ethernet type (%"U16_F"/%"U16_F"/%"U16_F"/%"U16_F"/%"U16_F")\n", - hdr->hwtype, ARPH_HWLEN(hdr), hdr->proto, ARPH_PROTOLEN(hdr), hdr->ethhdr.type)); + hdr->hwtype, ARPH_HWLEN(hdr), hdr->proto, ARPH_PROTOLEN(hdr), ethhdr->type)); ETHARP_STATS_INC(etharp.proterr); ETHARP_STATS_INC(etharp.drop); pbuf_free(p); @@ -719,12 +734,12 @@ etharp_arp_input(struct netif *netif, struct eth_addr *ethaddr, struct pbuf *p) i--; hdr->dhwaddr.addr[i] = hdr->shwaddr.addr[i]; #if LWIP_AUTOIP - hdr->ethhdr.dest.addr[i] = ethdst_hwaddr[i]; + ethhdr->dest.addr[i] = ethdst_hwaddr[i]; #else /* LWIP_AUTOIP */ - hdr->ethhdr.dest.addr[i] = hdr->shwaddr.addr[i]; + ethhdr->dest.addr[i] = hdr->shwaddr.addr[i]; #endif /* LWIP_AUTOIP */ hdr->shwaddr.addr[i] = ethaddr->addr[i]; - hdr->ethhdr.src.addr[i] = ethaddr->addr[i]; + ethhdr->src.addr[i] = ethaddr->addr[i]; } /* hwtype, hwaddr_len, proto, protolen and the type in the ethernet header @@ -1034,13 +1049,14 @@ etharp_raw(struct netif *netif, const struct eth_addr *ethsrc_addr, struct pbuf *p; err_t result = ERR_OK; u8_t k; /* ARP entry index */ + struct eth_hdr *ethhdr; struct etharp_hdr *hdr; #if LWIP_AUTOIP const u8_t * ethdst_hwaddr; #endif /* LWIP_AUTOIP */ /* allocate a pbuf for the outgoing ARP request packet */ - p = pbuf_alloc(PBUF_RAW, sizeof(struct etharp_hdr), PBUF_RAM); + p = pbuf_alloc(PBUF_RAW, SIZEOF_ETHARP_PACKET, PBUF_RAM); /* could allocate a pbuf for an ARP request? */ if (p == NULL) { LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE | 2, ("etharp_raw: could not allocate pbuf for ARP request.\n")); @@ -1048,9 +1064,10 @@ etharp_raw(struct netif *netif, const struct eth_addr *ethsrc_addr, return ERR_MEM; } LWIP_ASSERT("check that first pbuf can hold struct etharp_hdr", - (p->len >= sizeof(struct etharp_hdr))); + (p->len >= SIZEOF_ETHARP_PACKET)); - hdr = p->payload; + ethhdr = p->payload; + hdr = (struct etharp_hdr *)((u8_t*)ethhdr + SIZEOF_ETH_HDR); LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE, ("etharp_raw: sending raw ARP packet.\n")); hdr->opcode = htons(opcode); @@ -1070,11 +1087,11 @@ etharp_raw(struct netif *netif, const struct eth_addr *ethsrc_addr, hdr->dhwaddr.addr[k] = hwdst_addr->addr[k]; /* Write the Ethernet MAC-Addresses */ #if LWIP_AUTOIP - hdr->ethhdr.dest.addr[k] = ethdst_hwaddr[k]; + ethhdr->dest.addr[k] = ethdst_hwaddr[k]; #else /* LWIP_AUTOIP */ - hdr->ethhdr.dest.addr[k] = ethdst_addr->addr[k]; + ethhdr->dest.addr[k] = ethdst_addr->addr[k]; #endif /* LWIP_AUTOIP */ - hdr->ethhdr.src.addr[k] = ethsrc_addr->addr[k]; + ethhdr->src.addr[k] = ethsrc_addr->addr[k]; } hdr->sipaddr = *(struct ip_addr2 *)ipsrc_addr; hdr->dipaddr = *(struct ip_addr2 *)ipdst_addr; @@ -1084,7 +1101,7 @@ etharp_raw(struct netif *netif, const struct eth_addr *ethsrc_addr, /* set hwlen and protolen together */ hdr->_hwlen_protolen = htons((ETHARP_HWADDR_LEN << 8) | sizeof(struct ip_addr)); - hdr->ethhdr.type = htons(ETHTYPE_ARP); + ethhdr->type = htons(ETHTYPE_ARP); /* send ARP query */ result = netif->linkoutput(netif, p); ETHARP_STATS_INC(etharp.xmit); @@ -1126,6 +1143,7 @@ err_t ethernet_input(struct pbuf *p, struct netif *netif) { struct eth_hdr* ethhdr; + u16_t type; /* points to packet payload, which starts with an Ethernet header */ ethhdr = p->payload; @@ -1137,7 +1155,22 @@ ethernet_input(struct pbuf *p, struct netif *netif) (unsigned)ethhdr->src.addr[3], (unsigned)ethhdr->src.addr[4], (unsigned)ethhdr->src.addr[5], (unsigned)htons(ethhdr->type))); - switch (htons(ethhdr->type)) { + type = htons(ethhdr->type); +#if ETHARP_SUPPORT_VLAN + if (type == ETHTYPE_VLAN) { + struct eth_vlan_hdr *vlan = (struct eth_vlan_hdr*)(((char*)ethhdr) + SIZEOF_ETH_HDR); +#ifdef ETHARP_VLAN_CHECK /* if not, allow all VLANs */ + if (VLAN_ID(vlan) != ETHARP_VLAN_CHECK) { + /* silently ignore this packet: not for our VLAN */ + pbuf_free(p); + return ERR_OK; + } +#endif /* ETHARP_VLAN_CHECK */ + type = htons(vlan->tpid); + } +#endif /* ETHARP_SUPPORT_VLAN */ + + switch (type) { /* IP packet? */ case ETHTYPE_IP: #if ETHARP_TRUST_IP_MAC @@ -1145,7 +1178,7 @@ ethernet_input(struct pbuf *p, struct netif *netif) etharp_ip_input(netif, p); #endif /* ETHARP_TRUST_IP_MAC */ /* skip Ethernet header */ - if(pbuf_header(p, -(s16_t)sizeof(struct eth_hdr))) { + if(pbuf_header(p, -(s16_t)SIZEOF_ETH_HDR)) { LWIP_ASSERT("Can't move over header in packet", 0); pbuf_free(p); p = NULL;