From 475d49440c6d07332565a6ea886628ccb9a179d2 Mon Sep 17 00:00:00 2001 From: Dirk Ziegelmeier Date: Thu, 25 Aug 2016 14:07:35 +0200 Subject: [PATCH] Fix handling of LWIP_HOOK_VLAN_SET(). Previous implementation supplied uninitialized arguments to the macro (struct eth_hdr). Change macro signature to be universal: netif, pbuf, src, dst, eth_type - whatever the user needs to decide about VLAN header. Return value <0 means "no VLAN header", 0 <= return_value <= 0xFFFF -> value is prio_vid of header. Clean up ethernet_output function to be more readable. --- UPGRADING | 2 ++ src/include/lwip/opt.h | 19 ++++++------ src/netif/ethernet.c | 65 ++++++++++++++++++++++-------------------- 3 files changed, 47 insertions(+), 39 deletions(-) diff --git a/UPGRADING b/UPGRADING index b9ad876a..2190ec98 100644 --- a/UPGRADING +++ b/UPGRADING @@ -57,6 +57,8 @@ with newer versions. * added hook LWIP_HOOK_MEMP_AVAILABLE() to get informed when a memp pool was empty and an item is now available + * Signature of LWIP_HOOK_VLAN_SET macro was changed + * LWIP_DECLARE_MEMORY_ALIGNED() may be used to declare aligned memory buffers (mem/memp) or to move buffers to dedicated memory using compiler attributes diff --git a/src/include/lwip/opt.h b/src/include/lwip/opt.h index 85d45821..d4061d8b 100644 --- a/src/include/lwip/opt.h +++ b/src/include/lwip/opt.h @@ -2495,17 +2495,20 @@ /** * LWIP_HOOK_VLAN_SET(netif, eth_hdr, vlan_hdr): - * - called from etharp_raw() and ethernet_output() if VLAN support is enabled - * - netif: struct netif that the packet will be sent through - * - eth_hdr: struct eth_hdr of the packet - * - vlan_hdr: struct eth_vlan_hdr of the packet - * Return values: - * - 0: Packet shall not contain VLAN header. - * - != 0: Packet shall contain VLAN header. * Hook can be used to set prio_vid field of vlan_hdr. + * Called from ethernet_output() if VLAN support is enabled. + * Arguments: + * - netif: struct netif that the packet will be sent through + * - p: struct pbuf packet to be sent + * - src: source eth address + * - dst: destination eth address + * - eth_type: ethernet type to packet to be sent + * Return values: + * - <0: Packet shall not contain VLAN header. + * - 0 <= return value <= 0xFFFF: Packet shall contain VLAN header. Return value is prio_vid in host byte order. */ #ifdef __DOXYGEN__ -#define LWIP_HOOK_VLAN_SET(netif, eth_hdr, vlan_hdr) +#define LWIP_HOOK_VLAN_SET(netif, p, src, dst, eth_type) #endif /** diff --git a/src/netif/ethernet.c b/src/netif/ethernet.c index 5e55596a..fffa48f9 100644 --- a/src/netif/ethernet.c +++ b/src/netif/ethernet.c @@ -253,49 +253,52 @@ free_and_return: * @return ERR_OK if the packet was sent, any other err_t on failure */ err_t -ethernet_output(struct netif* netif, struct pbuf* p, const struct eth_addr* src, const struct eth_addr* dst, u16_t eth_type) +ethernet_output(struct netif* netif, struct pbuf* p, + const struct eth_addr* src, const struct eth_addr* dst, + u16_t eth_type) { - struct eth_hdr *ethhdr; -#if ETHARP_SUPPORT_VLAN && defined(LWIP_HOOK_VLAN_SET) - struct eth_vlan_hdr *vlanhdr; -#endif /* ETHARP_SUPPORT_VLAN && defined(LWIP_HOOK_VLAN_SET) */ - - LWIP_ASSERT("netif->hwaddr_len must be 6 for ethernet_output!", (netif->hwaddr_len == ETH_HWADDR_LEN)); + struct eth_hdr* ethhdr; + u16_t eth_type_be = ntohs(eth_type); #if ETHARP_SUPPORT_VLAN && defined(LWIP_HOOK_VLAN_SET) - if (pbuf_header(p, sizeof(struct eth_hdr) + SIZEOF_VLAN_HDR) != 0) { -#else /* ETHARP_SUPPORT_VLAN && defined(LWIP_HOOK_VLAN_SET) */ - if (pbuf_header(p, sizeof(struct eth_hdr)) != 0) { + s32_t vlan_prio_vid = LWIP_HOOK_VLAN_SET(netif, p, src, dst, eth_type); + if (vlan_prio_vid >= 0) { + struct eth_vlan_hdr* vlanhdr; + + if (pbuf_header(p, SIZEOF_ETH_HDR + SIZEOF_VLAN_HDR) != 0) { + goto pbuf_header_failed; + } + vlanhdr = (struct eth_vlan_hdr*)(((u8_t*)p->payload) + SIZEOF_ETH_HDR); + vlanhdr->tpid = eth_type_be; + vlanhdr->prio_vid = ntohs((u16_t)vlan_prio_vid); + + eth_type_be = PP_HTONS(ETHTYPE_VLAN); + } else #endif /* ETHARP_SUPPORT_VLAN && defined(LWIP_HOOK_VLAN_SET) */ - LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE | LWIP_DBG_LEVEL_SERIOUS, - ("ethernet_output: could not allocate room for header.\n")); - LINK_STATS_INC(link.lenerr); - return ERR_BUF; + { + if (pbuf_header(p, SIZEOF_ETH_HDR) != 0) { + goto pbuf_header_failed; + } } - ethhdr = (struct eth_hdr *)p->payload; - -#if ETHARP_SUPPORT_VLAN && defined(LWIP_HOOK_VLAN_SET) - ethhdr->type = PP_HTONS(ETHTYPE_VLAN); - vlanhdr = (struct eth_vlan_hdr*)(((u8_t*)ethhdr) + SIZEOF_ETH_HDR); - vlanhdr->prio_vid = 0; - vlanhdr->tpid = ntohs(eth_type); - if (!LWIP_HOOK_VLAN_SET(netif, ethhdr, vlanhdr)) { - /* packet shall not contain VLAN header, so hide it and set correct ethertype */ - pbuf_header(p, -SIZEOF_VLAN_HDR); - ethhdr = (struct eth_hdr *)p->payload; -#endif /* ETHARP_SUPPORT_VLAN && defined(LWIP_HOOK_VLAN_SET) */ - ethhdr->type = ntohs(eth_type); -#if ETHARP_SUPPORT_VLAN && defined(LWIP_HOOK_VLAN_SET) - } -#endif /* ETHARP_SUPPORT_VLAN && defined(LWIP_HOOK_VLAN_SET) */ + ethhdr = (struct eth_hdr*)p->payload; + ethhdr->type = eth_type_be; ETHADDR32_COPY(ðhdr->dest, dst); ETHADDR16_COPY(ðhdr->src, src); - LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE, ("ethernet_output: sending packet %p\n", (void *)p)); + LWIP_ASSERT("netif->hwaddr_len must be 6 for ethernet_output!", + (netif->hwaddr_len == ETH_HWADDR_LEN)); + LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE, + ("ethernet_output: sending packet %p\n", (void *)p)); /* send the packet */ return netif->linkoutput(netif, p); + +pbuf_header_failed: + LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE | LWIP_DBG_LEVEL_SERIOUS, + ("ethernet_output: could not allocate room for header.\n")); + LINK_STATS_INC(link.lenerr); + return ERR_BUF; } #endif /* LWIP_ARP || LWIP_ETHERNET */