Add some partial IP forwarding logic.

net/devif/ipv6_forward.c

@@ -52,6 +52,35 @@
 
 #if defined(CONFIG_NET_IPFORWARD) && defined(CONFIG_NET_IPv6)
 
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+#if defined(CONFIG_NET_UDP) || defined(CONFIG_NET_ICMPv6)
+/* IPv6 + UDP or ICMPv6 header */
+
+struct ipv6l3_hdr_s
+{
+  struct ipv6_hdr_s       ipv6;
+  union
+  {
+#ifdef CONFIG_NET_UDP
+    struct udp_hdr_s      udp;
+#ifdef CONFIG_NET_ICMPv6
+    struct icmpv6_iphdr_s icmp;
+  } u;
+};
+#endif
+
+/* This is the send state structure */
+
+struct forward_s
+{
+  FAR net_driver_s      *dev;  /* Forwarding device */
+  struct ipv6l3_hdr_s    hdr;  /* Copy of origin L2+L3 headers */
+  FAR struct iob_queue_s iobq; /* IOBs contained the data payload */
+};
+
 /****************************************************************************
  * Private Functions
  ****************************************************************************/
@@ -118,6 +147,149 @@ static int ipv6_packet_conversion(FAR struct net_driver_s *dev,
 # define ipv6_packet_conversion(dev, ipv6)
 #endif /* CONFIG_NET_6LOWPAN */
 
+/****************************************************************************
+ * Name: ipv6_hdrsize
+ *
+ * Description:
+ *   Return the size of the IPv6 header and the following.
+ *
+ * Input Parameters:
+ *   ipv6  - A pointer to the IPv6 header in within the IPv6 packet.  This
+ *           is immeidately followed by the L3 header which may be TCP, UDP,
+ *           or ICMPv6.
+ *
+ * Returned Value:
+ *   The size of the combined L2 + L3 headers is returned on success.  An
+ *   error is returned only if the prototype is not supported.
+ *
+ ****************************************************************************/
+
+static int ipv6_hdrsize(FAR struct ipv6_hdr_s *ipv6)
+{
+  /* Copy the following protocol header, */
+
+  switch (ipv6->proto)
+    {
+#ifdef CONFIG_NET_TCP
+    case IP_PROTO_TCP:
+      {
+        FAR struct tcp_hdr_s *tcp =
+          (FAR struct tcp_hdr_s *)((FAR uintptr_t *)ipv6 + IPv6_HDRLEN);
+        unsigned int tcpsize;
+
+        /* The TCP header length is encoded in the top 4 bits of the
+         * tcpoffset field (in units of 32-bit words).
+         */
+
+        tcpsize = ((uint16_t)tcp->tcpoffset >> 4) << 2;
+        return IPv6_HDRLEN + tcpsize;
+      }
+      break;
+#endif
+
+#ifdef CONFIG_NET_UDP
+    case IP_PROTO_UDP:
+      return IPv6_HDRLEN + UDP_HDRLEN;
+      break;
+#endif
+
+#ifdef CONFIG_NET_ICMPv6
+    case IP_PROTO_ICMP6:
+      return IPv6_HDRLEN + ICMPv6_HDRLEN;
+      break;
+#endif
+
+    default:
+      nwarn("WARNING: Unrecognized proto: %u\n", ipv6->proto);
+      return -EPROTONOSUPPORT;
+    }
+}
+
+/****************************************************************************
+ * Name: ipv6_dev_forward
+ *
+ * Description:
+ *   Set up to forward the UDP or ICMPv6 packet on the specified device.
+ *   This function will set up a send "interrupt" handler that will perform
+ *   the actual send asynchronously.
+ *
+ * Input Parameters:
+ *   dev   - The device on which the packet was received and which contains
+ *           the IPv6 packet.
+ *   ipv6  - A convenience pointer to the IPv6 header in within the IPv6
+ *           packet.  This is immeidately followed by the L3 header which may
+ *           be UDP or ICMPv6.
+ *   iob   - A list of IOBs containing the data payload to be sent.
+ *
+ *
+ *   On input:
+ *   - dev->d_buf holds the received packet.
+ *   - dev->d_len holds the length of the received packet MINUS the
+ *     size of the L1 header.  That was subtracted out by ipv6_input.
+ *   - ipv6 points to the IPv6 header with dev->d_buf.
+ *
+ * Returned Value:
+ *   Zero is returned if the packet was successfully forward;  A negated
+ *   errno value is returned if the packet is not forwardable.  In that
+ *   latter case, the caller (ipv6_input()) should drop the packet.
+ *
+ ****************************************************************************/
+
+#if defined(CONFIG_NETDEV_MULTINIC)
+static int ipv6_dev_forward(FAR struct net_driver_s *dev,
+                            FAR struct ipv6_hdr_s *ipv6,
+                            FAR struct iob_s *iob)
+{
+  /* Notify the forwarding device that TX data is available */
+
+  /* Set up to send the packet when the selected device polls for TX data.
+   * If the packet is TCP, it must obey ACK and windowing rules.
+   */
+
+#warning Missing logic
+
+  /* REVISIT:  For Ethernet we may have to fix up the Ethernet header:
+   * - source MAC, the MAC of the current device.
+   * - dest MAC, the MAC associated with the destination IPv6 adress.
+   *   This will involve ICMPv6 and Neighbor Discovery.
+   * - Because of TCP window, the packet may have to be sent in smaller
+   *   pieces.
+   */
+
+#  warning Missing logic
+   nwarn("WARNING: UPD/ICMPv6 packet forwarding not yet supported\n");
+  return -ENOSYS;
+}
+#endif
+
+/****************************************************************************
+ * Name: ipv6_dropstats
+ *
+ * Description:
+ *   Update statistics for a droped packet.
+ *
+ * Input Parameters:
+ *   ipv6  - A convenience pointer to the IPv6 header in within the IPv6
+ *           packet to be dropped.
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_NET_STATISTICS
+static void ipv6_dropstats(FAR struct ipv6_hdr_s *ipv)
+{
+g_netstats.icmpv6.drop++
+g_netstats.udp.drop++;
+g_netstats.tcp.drop++;
+
+g_netstats.ipv6.drop++
+}
+#else
+#  define ipv6_dropstats(ipv6)
+#endif
+
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
@@ -187,35 +359,83 @@ int ipv6_forward(FAR struct net_driver_s *dev, FAR struct ipv6_hdr_s *ipv6)
       ret = ipv6_packet_conversion(dev, fwddev, ipv6);
       if (ret < 0)
         {
-          /* Extract the IPv6 + L3 header; Move the data payload to an IOB
+          FAR struct iob_s *iob = NULL;
-           * chain.
+          FAR uint8_t *payload;
+          unsigned int paysize;
+          int hdrsize;
+
+          /* Get the size of the IPv6 + L3 header.  Use this to determine
+           * start of the data payload.
+           *
+           * Remember that the size of the L1 header has already been
+           * subtracted from dev->d_len.
            */
 
-          /* Notify the forwarding device that TX data is available */
+          hdrsize = ipv6_hdrsize(ipv6);
+          if (hdrsize < 0)
+            {
+              goto drop;
+            }
 
-          /* Set up to send the packet when the selected device polls for TX
+          payload = (FAR uint8_t *)ipv6 + hdrsize;
-           * data.
+          paysize = dev->d_len - hdrsize;
+
+          if (paysize > 0)
+            {
+              /* Try to allocate the head of an IOB chain.  If this fails,
+               * the the packet will be dropped; we are not operating in a
+               * context where waiting for an IOB is a good idea
                */
 
-          /* REVISIT:  For Ethernet we may have to fix up the Ethernet header:
+              iob = iob_tryalloc(false);
-           * - source MAC, the MAC of the current device.
+              if (iob == NULL)
-           * - dest MAC, the MAC associated with the destination IPv6 adress.
+                {
-           *   This will involve ICMPv6 and Neighbor Discovery.
+                  ret = -ENOMEM;
+                  goto errout_with_iob;
+                }
+
+              /* Copy the packet data payload into an IOB chain.
+               * iob_trycopin() will not wait, but will fail there are no
+               * available IOBs.
                */
 
-          /* Return success with dev->d_len = 0 */
+              ret = iob_trycopyin(iob, payload, paysize, 0);
+              if (ret < 0)
+                {
+                  goto errout_with_iob;
+                }
+            }
 
-#  warning Missing logic
+          /* Then set up to forward the packet */
-          nwarn("WARNING: Packet forwarding not yet supported "
+
-                          "across different devices\n");
+          if (ipv6->proto == IP_PROTO_TCP)
+            {
+              ret = tcp_ipv6_forward(dev, ipv6, iob);
+            }
+          else
+            {
+              ret = ipv6_dev_forward(dev, ipv6, iob);
+            }
+
+          if (ret >= 0)
+            {
+              dev->d_len = 0;
+              return OK;
+            }
+
+errout_with_iob:
+          iob_free_chain(iob);
+
+drop:
+          ipv6_dropstats(ipv6);
+          dev->d_len = 0;
           return -ENOSYS;
         }
     }
   else
 #endif /* CONFIG_NETDEV_MULTINIC */
 
-#if defined(CONFIG_NET_6LOWPAN) /* REVISIT:  Currently only suport for
+#if defined(CONFIG_NET_6LOWPAN) /* REVISIT:  Currently only suport for 6LoWPAN */
-                                   * 6LoWPAN */
     {
       /* Single network device */