Work on bug #48824: ethernet.c extensions: Implement ethernet_output();

Fixes bug #48862: ethip6 does not support setting vlan

src/core/ipv4/etharp.c

@@ -393,47 +393,6 @@ etharp_find_entry(const ip4_addr_t *ipaddr, u8_t flags, struct netif* netif)
   return (err_t)i;
 }
 
-/**
- * Send an IP packet on the network using netif->linkoutput
- * The ethernet header is filled in before sending.
- *
- * @params netif the lwIP network interface on which to send the packet
- * @params p the packet to send, p->payload pointing to the (uninitialized) ethernet header
- * @params src the source MAC address to be copied into the ethernet header
- * @params dst the destination MAC address to be copied into the ethernet header
- * @return ERR_OK if the packet was sent, any other err_t on failure
- */
-static err_t
-etharp_send_ip(struct netif *netif, struct pbuf *p, struct eth_addr *src, const struct eth_addr *dst)
-{
-  struct eth_hdr *ethhdr = (struct eth_hdr *)p->payload;
-#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 the same as ETH_HWADDR_LEN for etharp!",
-              (netif->hwaddr_len == ETH_HWADDR_LEN));
-#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 = PP_HTONS(ETHTYPE_IP);
-  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 = PP_HTONS(ETHTYPE_IP);
-#if ETHARP_SUPPORT_VLAN && defined(LWIP_HOOK_VLAN_SET)
-  }
-#endif /* ETHARP_SUPPORT_VLAN && defined(LWIP_HOOK_VLAN_SET) */
-  ETHADDR32_COPY(&ethhdr->dest, dst);
-  ETHADDR16_COPY(&ethhdr->src, src);
-  LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE, ("etharp_send_ip: sending packet %p\n", (void *)p));
-  /* send the packet */
-  return netif->linkoutput(netif, p);
-}
-
 /**
  * Update (or insert) a IP/MAC address pair in the ARP cache.
  *
@@ -517,7 +476,7 @@ etharp_update_arp_entry(struct netif *netif, const ip4_addr_t *ipaddr, struct et
     arp_table[i].q = NULL;
 #endif /* ARP_QUEUEING */
     /* send the queued IP packet */
-    etharp_send_ip(netif, p, (struct eth_addr*)(netif->hwaddr), ethaddr);
+    ethernet_output(netif, p, (struct eth_addr*)(netif->hwaddr), ethaddr, ETHTYPE_IP);
     /* free the queued IP packet */
     pbuf_free(p);
   }
@@ -898,8 +857,7 @@ etharp_output_to_arp_index(struct netif *netif, struct pbuf *q, u8_t arp_idx)
     }
   }
 
-  return etharp_send_ip(netif, q, (struct eth_addr*)(netif->hwaddr),
+  return ethernet_output(netif, q, (struct eth_addr*)(netif->hwaddr), &arp_table[arp_idx].ethaddr, ETHTYPE_IP);
-    &arp_table[arp_idx].ethaddr);
 }
 
 /**
@@ -931,19 +889,6 @@ etharp_output(struct netif *netif, struct pbuf *q, const ip4_addr_t *ipaddr)
   LWIP_ASSERT("q != NULL", q != NULL);
   LWIP_ASSERT("ipaddr != NULL", ipaddr != NULL);
 
-  /* make room for Ethernet header - should not fail */
-#if ETHARP_SUPPORT_VLAN && defined(LWIP_HOOK_VLAN_SET)
-  if (pbuf_header(q, sizeof(struct eth_hdr) + SIZEOF_VLAN_HDR) != 0) {
-#else /* ETHARP_SUPPORT_VLAN && defined(LWIP_HOOK_VLAN_SET) */
-  if (pbuf_header(q, sizeof(struct eth_hdr)) != 0) {
-#endif /* ETHARP_SUPPORT_VLAN && defined(LWIP_HOOK_VLAN_SET) */
-    /* bail out */
-    LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE | LWIP_DBG_LEVEL_SERIOUS,
-      ("etharp_output: could not allocate room for header.\n"));
-    LINK_STATS_INC(link.lenerr);
-    return ERR_BUF;
-  }
-
   /* Determine on destination hardware address. Broadcasts and multicasts
    * are special, other IP addresses are looked up in the ARP table. */
 
@@ -970,11 +915,7 @@ etharp_output(struct netif *netif, struct pbuf *q, const ip4_addr_t *ipaddr)
     if (!ip4_addr_netcmp(ipaddr, netif_ip4_addr(netif), netif_ip4_netmask(netif)) &&
         !ip4_addr_islinklocal(ipaddr)) {
 #if LWIP_AUTOIP
-      struct ip_hdr *iphdr = (struct ip_hdr*)((u8_t*)q->payload +
+      struct ip_hdr *iphdr = (struct ip_hdr*)(size_t)q->payload;
-#if ETHARP_SUPPORT_VLAN && defined(LWIP_HOOK_VLAN_SET)
-        SIZEOF_VLAN_HDR +
-#endif /* ETHARP_SUPPORT_VLAN && defined(LWIP_HOOK_VLAN_SET) */
-        sizeof(struct eth_hdr));
       /* According to RFC 3297, chapter 2.6.2 (Forwarding Rules), a packet with
          a link-local source address must always be "directly to its destination
          on the same physical link. The host MUST NOT send the packet to any
@@ -1036,7 +977,7 @@ etharp_output(struct netif *netif, struct pbuf *q, const ip4_addr_t *ipaddr)
   /* continuation for multicast/broadcast destinations */
   /* obtain source Ethernet address of the given interface */
   /* send packet directly on the link */
-  return etharp_send_ip(netif, q, (struct eth_addr*)(netif->hwaddr), dest);
+  return ethernet_output(netif, q, (struct eth_addr*)(netif->hwaddr), dest, ETHTYPE_IP);
 }
 
 /**
@@ -1136,7 +1077,7 @@ etharp_query(struct netif *netif, const ip4_addr_t *ipaddr, struct pbuf *q)
     /* we have a valid IP->Ethernet address mapping */
     ETHARP_SET_HINT(netif, i);
     /* send the packet */
-    result = etharp_send_ip(netif, q, srcaddr, &(arp_table[i].ethaddr));
+    result = ethernet_output(netif, q, srcaddr, &(arp_table[i].ethaddr), ETHTYPE_IP);
   /* pending entry? (either just created or already pending */
   } else if (arp_table[i].state == ETHARP_STATE_PENDING) {
     /* entry is still pending, queue the given packet 'q' */

src/core/ipv6/ethip6.c

@@ -52,34 +52,10 @@
 #include "lwip/netif.h"
 #include "lwip/icmp6.h"
 #include "lwip/prot/ethernet.h"
+#include "netif/ethernet.h"
 
 #include <string.h>
 
-/**
- * Send an IPv6 packet on the network using netif->linkoutput
- * The ethernet header is filled in before sending.
- *
- * @params netif the lwIP network interface on which to send the packet
- * @params p the packet to send, p->payload pointing to the (uninitialized) ethernet header
- * @params src the source MAC address to be copied into the ethernet header
- * @params dst the destination MAC address to be copied into the ethernet header
- * @return ERR_OK if the packet was sent, any other err_t on failure
- */
-static err_t
-ethip6_send(struct netif *netif, struct pbuf *p, struct eth_addr *src, struct eth_addr *dst)
-{
-  struct eth_hdr *ethhdr = (struct eth_hdr *)p->payload;
-
-  LWIP_ASSERT("netif->hwaddr_len must be 6 for ethip6!",
-              (netif->hwaddr_len == 6));
-  SMEMCPY(&ethhdr->dest, dst, 6);
-  SMEMCPY(&ethhdr->src, src, 6);
-  ethhdr->type = PP_HTONS(ETHTYPE_IPV6);
-  LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE, ("ethip6_send: sending packet %p\n", (void *)p));
-  /* send the packet */
-  return netif->linkoutput(netif, p);
-}
-
 /**
  * Resolve and fill-in Ethernet address header for outgoing IPv6 packet.
  *
@@ -104,14 +80,6 @@ ethip6_output(struct netif *netif, struct pbuf *q, const ip6_addr_t *ip6addr)
   struct eth_addr dest;
   s8_t i;
 
-  /* make room for Ethernet header - should not fail */
-  if (pbuf_header(q, sizeof(struct eth_hdr)) != 0) {
-    /* bail out */
-    LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE | LWIP_DBG_LEVEL_SERIOUS,
-      ("etharp_output: could not allocate room for header.\n"));
-    return ERR_BUF;
-  }
-
   /* multicast destination IP address? */
   if (ip6_addr_ismulticast(ip6addr)) {
     /* Hash IP multicast address to MAC address.*/
@@ -123,7 +91,7 @@ ethip6_output(struct netif *netif, struct pbuf *q, const ip6_addr_t *ip6addr)
     dest.addr[5] = ((const u8_t *)(&(ip6addr->addr[3])))[3];
 
     /* Send out. */
-    return ethip6_send(netif, q, (struct eth_addr*)(netif->hwaddr), &dest);
+    return ethernet_output(netif, q, (struct eth_addr*)(netif->hwaddr), &dest, ETHTYPE_IPV6);
   }
 
   /* We have a unicast destination IP address */
@@ -148,11 +116,10 @@ ethip6_output(struct netif *netif, struct pbuf *q, const ip6_addr_t *ip6addr)
 
     /* Send out. */
     SMEMCPY(dest.addr, neighbor_cache[i].lladdr, 6);
-    return ethip6_send(netif, q, (struct eth_addr*)(netif->hwaddr), &dest);
+    return ethernet_output(netif, q, (struct eth_addr*)(netif->hwaddr), &dest, ETHTYPE_IPV6);
   }
 
   /* We should queue packet on this interface. */
-  pbuf_header(q, -(s16_t)SIZEOF_ETH_HDR);
   return nd6_queue_packet(i, q);
 }
 

src/include/netif/ethernet.h

@@ -64,6 +64,7 @@ extern "C" {
 #endif
 
 err_t ethernet_input(struct pbuf *p, struct netif *netif);
+err_t ethernet_output(struct netif* netif, struct pbuf* p, const struct eth_addr* src, const struct eth_addr* dst, u16_t eth_type);
 
 extern const struct eth_addr ethbroadcast, ethzero;
 

src/netif/ethernet.c

@@ -222,4 +222,62 @@ free_and_return:
   pbuf_free(p);
   return ERR_OK;
 }
+
+/**
+ * Send an ethernet packet on the network using netif->linkoutput
+ * The ethernet header is filled in before sending.
+ *
+ * @params netif the lwIP network interface on which to send the packet
+ * @params p the packet to send, p->payload pointing to the (uninitialized) ethernet header
+ * @params src the source MAC address to be copied into the ethernet header
+ * @params dst the destination MAC address to be copied into the ethernet header
+ * @params eth_type ethernet type
+ * @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)
+{
+  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_send!", (netif->hwaddr_len == ETH_HWADDR_LEN));
+
+#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) {
+#endif /* ETHARP_SUPPORT_VLAN && defined(LWIP_HOOK_VLAN_SET) */
+    LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE | LWIP_DBG_LEVEL_SERIOUS,
+      ("ethernet_send: could not allocate room for header.\n"));
+    LINK_STATS_INC(link.lenerr);
+    return ERR_BUF;
+  }
+  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) */
+
+  ETHADDR32_COPY(&ethhdr->dest, dst);
+  ETHADDR16_COPY(&ethhdr->src,  src);
+
+  LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE, ("ethernet_send: sending packet %p\n", (void *)p));
+
+  /* send the packet */
+  return netif->linkoutput(netif, p);
+}
+
 #endif /* LWIP_ARP || LWIP_ETHERNET */