Diff: stack/pico_device.c

Revision:

152:a3d286bf94e5

Parent:

149:5f4cb161cec3

Child:

154:6c0e92a80c4a

--- a/stack/pico_device.c	Wed Apr 09 17:32:25 2014 +0200
+++ b/stack/pico_device.c	Mon Sep 28 13:16:18 2015 +0200
@@ -1,5 +1,5 @@
 /*********************************************************************
-   PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+   PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved.
    See LICENSE and COPYING for usage.
 
    .
@@ -7,7 +7,6 @@
    Authors: Daniele Lacamera
  *********************************************************************/
 
-
 #include "pico_config.h"
 #include "pico_device.h"
 #include "pico_stack.h"
@@ -16,6 +15,8 @@
 #include "pico_ipv6.h"
 #include "pico_ipv4.h"
 #include "pico_icmp6.h"
+#include "pico_eth.h"
+#define PICO_DEVICE_DEFAULT_MTU (1500)
 
 struct pico_devices_rr_info {
     struct pico_tree_node *node_in, *node_out;
@@ -39,14 +40,114 @@
 
 PICO_TREE_DECLARE(Device_tree, pico_dev_cmp);
 
-int pico_device_init(struct pico_device *dev, const char *name, uint8_t *mac)
+#ifdef PICO_SUPPORT_IPV6
+static void device_init_ipv6_final(struct pico_device *dev, struct pico_ip6 *linklocal)
+{
+    dev->hostvars.basetime = PICO_ND_REACHABLE_TIME;
+    /* RFC 4861 $6.3.2 value between 0.5 and 1.5 times basetime */
+    dev->hostvars.reachabletime = ((5 + (pico_rand() % 10)) * PICO_ND_REACHABLE_TIME) / 10;
+    dev->hostvars.retranstime = PICO_ND_RETRANS_TIMER;
+    pico_icmp6_router_solicitation(dev, linklocal);
+    dev->hostvars.hoplimit = PICO_IPV6_DEFAULT_HOP;
+}
+
+struct pico_ipv6_link *pico_ipv6_link_add_local(struct pico_device *dev, const struct pico_ip6 *prefix)
+{
+    struct pico_ip6 newaddr;
+    struct pico_ip6 netmask64 = {{0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};
+    struct pico_ipv6_link *link;
+    memcpy(newaddr.addr, prefix->addr, PICO_SIZE_IP6);
+    /* modified EUI-64 + invert universal/local bit */
+    newaddr.addr[8] = (dev->eth->mac.addr[0] ^ 0x02);
+    newaddr.addr[9] = dev->eth->mac.addr[1];
+    newaddr.addr[10] = dev->eth->mac.addr[2];
+    newaddr.addr[11] = 0xff;
+    newaddr.addr[12] = 0xfe;
+    newaddr.addr[13] = dev->eth->mac.addr[3];
+    newaddr.addr[14] = dev->eth->mac.addr[4];
+    newaddr.addr[15] = dev->eth->mac.addr[5];
+    link = pico_ipv6_link_add(dev, newaddr, netmask64);
+    if (link) {
+        device_init_ipv6_final(dev, &newaddr);
+    }
+
+    return link;
+}
+#endif
+
+static int device_init_mac(struct pico_device *dev, uint8_t *mac)
+{
+    #ifdef PICO_SUPPORT_IPV6
+    struct pico_ip6 linklocal = {{0xfe, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xaa, 0xaa, 0xaa, 0xff, 0xfe, 0xaa, 0xaa, 0xaa}};
+    #endif
+    dev->eth = PICO_ZALLOC(sizeof(struct pico_ethdev));
+    if (dev->eth) {
+        memcpy(dev->eth->mac.addr, mac, PICO_SIZE_ETH);
+        #ifdef PICO_SUPPORT_IPV6
+        if (pico_ipv6_link_add_local(dev, &linklocal) == NULL) {
+            PICO_FREE(dev->q_in);
+            PICO_FREE(dev->q_out);
+            PICO_FREE(dev->eth);
+            return -1;
+        }
+
+        #endif
+    } else {
+        pico_err = PICO_ERR_ENOMEM;
+        return -1;
+    }
+
+    return 0;
+}
+
+int pico_device_ipv6_random_ll(struct pico_device *dev)
 {
     #ifdef PICO_SUPPORT_IPV6
     struct pico_ip6 linklocal = {{0xfe, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xaa, 0xaa, 0xaa, 0xff, 0xfe, 0xaa, 0xaa, 0xaa}};
     struct pico_ip6 netmask6 = {{0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};
+    uint32_t len = (uint32_t)strlen(dev->name);
+    if (strcmp(dev->name, "loop")) {
+        do {
+            /* privacy extension + unset universal/local and individual/group bit */
+            len = pico_rand();
+            linklocal.addr[8]  = (uint8_t)((len & 0xffu) & (uint8_t)(~0x03));
+            linklocal.addr[9]  = (uint8_t)(len >> 8);
+            linklocal.addr[10] = (uint8_t)(len >> 16);
+            linklocal.addr[11] = (uint8_t)(len >> 24);
+            len = pico_rand();
+            linklocal.addr[12] = (uint8_t)len;
+            linklocal.addr[13] = (uint8_t)(len >> 8);
+            linklocal.addr[14] = (uint8_t)(len >> 16);
+            linklocal.addr[15] = (uint8_t)(len >> 24);
+            pico_rand_feed(dev->hash);
+        } while (pico_ipv6_link_get(&linklocal));
+
+        if (pico_ipv6_link_add(dev, linklocal, netmask6) == NULL) {
+            return -1;
+        }
+    }
+
     #endif
+    return 0;
+}
+
+static int device_init_nomac(struct pico_device *dev)
+{
+    if (pico_device_ipv6_random_ll(dev) < 0) {
+        PICO_FREE(dev->q_in);
+        PICO_FREE(dev->q_out);
+        return -1;
+    }
+
+    dev->eth = NULL;
+    return 0;
+}
+
+int pico_device_init(struct pico_device *dev, const char *name, uint8_t *mac)
+{
 
     uint32_t len = (uint32_t)strlen(name);
+    int ret = 0;
     if(len > MAX_DEVICE_NAME)
         len = MAX_DEVICE_NAME;
 
@@ -56,80 +157,26 @@
     Devices_rr_info.node_in  = NULL;
     Devices_rr_info.node_out = NULL;
     dev->q_in = PICO_ZALLOC(sizeof(struct pico_queue));
-    dev->q_out = PICO_ZALLOC(sizeof(struct pico_queue));
-    if (!dev->q_in || !dev->q_out)
+    if (!dev->q_in)
         return -1;
 
+    dev->q_out = PICO_ZALLOC(sizeof(struct pico_queue));
+    if (!dev->q_out) {
+        PICO_FREE(dev->q_in);
+        return -1;
+    }
+
     pico_tree_insert(&Device_tree, dev);
+    if (!dev->mtu)
+        dev->mtu = PICO_DEVICE_DEFAULT_MTU;
 
     if (mac) {
-        dev->eth = PICO_ZALLOC(sizeof(struct pico_ethdev));
-        if (dev->eth) {
-            memcpy(dev->eth->mac.addr, mac, PICO_SIZE_ETH);
-            #ifdef PICO_SUPPORT_IPV6
-            /* modified EUI-64 + invert universal/local bit */
-            linklocal.addr[8] = (mac[0] ^ 0x02);
-            linklocal.addr[9] = mac[1];
-            linklocal.addr[10] = mac[2];
-            linklocal.addr[13] = mac[3];
-            linklocal.addr[14] = mac[4];
-            linklocal.addr[15] = mac[5];
-            if (pico_ipv6_link_add(dev, linklocal, netmask6)) {
-                PICO_FREE(dev->q_in);
-                PICO_FREE(dev->q_out);
-                PICO_FREE(dev->eth);
-                return -1;
-            }
-
-            #endif
-        }
-
-
+        ret = device_init_mac(dev, mac);
     } else {
-        dev->eth = NULL;
-        #ifdef PICO_SUPPORT_IPV6
-        if (strcmp(dev->name, "loop")) {
-            do {
-                /* privacy extension + unset universal/local and individual/group bit */
-                len = pico_rand();
-                linklocal.addr[8]  = (uint8_t)((len & 0xffu) & (uint8_t)(~0x03));
-                linklocal.addr[9]  = (uint8_t)(len >> 8);
-                linklocal.addr[10] = (uint8_t)(len >> 16);
-                linklocal.addr[11] = (uint8_t)(len >> 24);
-                len = pico_rand();
-                linklocal.addr[12] = (uint8_t)len;
-                linklocal.addr[13] = (uint8_t)(len >> 8);
-                linklocal.addr[14] = (uint8_t)(len >> 16);
-                linklocal.addr[15] = (uint8_t)(len >> 24);
-                pico_rand_feed(dev->hash);
-            } while (pico_ipv6_link_get(&linklocal));
-
-            if (pico_ipv6_link_add(dev, linklocal, netmask6)) {
-                PICO_FREE(dev->q_in);
-                PICO_FREE(dev->q_out);
-                return -1;
-            }
-        }
-
-        #endif
+        ret = device_init_nomac(dev);
     }
 
-    #ifdef PICO_SUPPORT_IPV6
-    if (dev->eth)
-    {
-        dev->hostvars.mtu = PICO_ETH_MTU;
-        dev->hostvars.basetime = PICO_ND_REACHABLE_TIME;
-        /* RFC 4861 $6.3.2 value between 0.5 and 1.5 times basetime */
-        dev->hostvars.reachabletime = ((5 + (pico_rand() % 10)) * PICO_ND_REACHABLE_TIME) / 10;
-        dev->hostvars.retranstime = PICO_ND_RETRANS_TIMER;
-        pico_icmp6_router_solicitation(dev, &linklocal);
-    }
-
-    dev->hostvars.hoplimit = PICO_IPV6_DEFAULT_HOP;
-    #endif
-
-
-    return 0;
+    return ret;
 }
 
 static void pico_queue_destroy(struct pico_queue *q)
@@ -159,8 +206,6 @@
 #endif
     pico_tree_delete(&Device_tree, dev);
 
-
-    pico_tree_delete(&Device_tree, dev);
     Devices_rr_info.node_in  = NULL;
     Devices_rr_info.node_out = NULL;
     PICO_FREE(dev);
@@ -197,7 +242,7 @@
         if (f) {
             if (dev->eth) {
                 f->datalink_hdr = f->buffer;
-                pico_ethernet_receive(f);
+                (void)pico_ethernet_receive(f);
             } else {
                 f->net_hdr = f->buffer;
                 pico_network_receive(f);
@@ -212,29 +257,12 @@
 static int devloop_sendto_dev(struct pico_device *dev, struct pico_frame *f)
 {
 
-    int ret;
     if (dev->eth) {
-        ret = pico_ethernet_send(f);
-        if (0 <= ret) {
-            return -1;
-        } else {
-            if (!pico_source_is_local(f)) {
-                dbg("Destination unreachable -------> SEND ICMP\n");
-                pico_notify_dest_unreachable(f);
-            } else {
-                dbg("Destination unreachable -------> LOCAL\n");
-            }
-
-            pico_frame_discard(f);
-            return 1;
-        }
+        /* Ethernet: pass management of the frame to the pico_ethernet_send() rdv function */
+        return pico_ethernet_send(f);
     } else {
-        /* non-ethernet */
-        if (dev->send(dev, f->start, (int)f->len) <= 0)
-            return -1;
-
-        pico_frame_discard(f);
-        return 1;
+        /* non-ethernet: no post-processing needed */
+        return (dev->send(dev, f->start, (int)f->len) <= 0); /* Return 0 upon success, which is dev->send() > 0 */
     }
 }
 
@@ -246,14 +274,17 @@
             break;
 
         /* Device dequeue + send */
-        f = pico_dequeue(dev->q_out);
+        f = pico_queue_peek(dev->q_out);
         if (!f)
             break;
 
-        if (devloop_sendto_dev(dev, f) < 0)
-            break;
+        if (devloop_sendto_dev(dev, f) == 0) { /* success. */
+            f = pico_dequeue(dev->q_out);
+            pico_frame_discard(f); /* SINGLE POINT OF DISCARD for OUTGOING FRAMES */
+            loop_score--;
+        } else
+            break; /* Don't discard */
 
-        loop_score--;
     }
     return loop_score;
 }
@@ -354,7 +385,7 @@
             struct pico_frame *copy = pico_frame_copy(f);
 
             if(!copy)
-                return -1;
+                break;
 
             copy->dev = dev;
             copy->dev->send(copy->dev, copy->start, (int)copy->len);
@@ -365,6 +396,13 @@
             ret = f->dev->send(f->dev, f->start, (int)f->len);
         }
     }
-
     return ret;
 }
+
+int pico_device_link_state(struct pico_device *dev)
+{
+    if (!dev->link_state)
+        return 1; /* Not supported, assuming link is always up */
+
+    return dev->link_state(dev);
+}