pico_device.c

00001 /*********************************************************************
00002    PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved.
00003    See LICENSE and COPYING for usage.
00004 
00005    .
00006 
00007    Authors: Daniele Lacamera
00008  *********************************************************************/
00009 
00010 #include "pico_config.h"
00011 #include "pico_device.h"
00012 #include "pico_stack.h"
00013 #include "pico_protocol.h"
00014 #include "pico_tree.h"
00015 #include "pico_ipv6.h"
00016 #include "pico_ipv4.h"
00017 #include "pico_icmp6.h"
00018 #include "pico_eth.h"
00019 #define PICO_DEVICE_DEFAULT_MTU (1500)
00020 
00021 struct pico_devices_rr_info {
00022     struct pico_tree_node *node_in, *node_out;
00023 };
00024 
00025 static struct pico_devices_rr_info Devices_rr_info = {
00026     NULL, NULL
00027 };
00028 
00029 static int pico_dev_cmp(void *ka, void *kb)
00030 {
00031     struct pico_device *a = ka, *b = kb;
00032     if (a->hash < b->hash)
00033         return -1;
00034 
00035     if (a->hash > b->hash)
00036         return 1;
00037 
00038     return 0;
00039 }
00040 
00041 PICO_TREE_DECLARE(Device_tree, pico_dev_cmp);
00042 
00043 #ifdef PICO_SUPPORT_IPV6
00044 static void device_init_ipv6_final(struct pico_device *dev, struct pico_ip6 *linklocal)
00045 {
00046     dev->hostvars.basetime = PICO_ND_REACHABLE_TIME;
00047     /* RFC 4861 $6.3.2 value between 0.5 and 1.5 times basetime */
00048     dev->hostvars.reachabletime = ((5 + (pico_rand() % 10)) * PICO_ND_REACHABLE_TIME) / 10;
00049     dev->hostvars.retranstime = PICO_ND_RETRANS_TIMER;
00050     pico_icmp6_router_solicitation(dev, linklocal);
00051     dev->hostvars.hoplimit = PICO_IPV6_DEFAULT_HOP;
00052 }
00053 
00054 struct pico_ipv6_link *pico_ipv6_link_add_local(struct pico_device *dev, const struct pico_ip6 *prefix)
00055 {
00056     struct pico_ip6 newaddr;
00057     struct pico_ip6 netmask64 = {{0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};
00058     struct pico_ipv6_link *link;
00059     memcpy(newaddr.addr, prefix->addr, PICO_SIZE_IP6);
00060     /* modified EUI-64 + invert universal/local bit */
00061     newaddr.addr[8] = (dev->eth->mac.addr[0] ^ 0x02);
00062     newaddr.addr[9] = dev->eth->mac.addr[1];
00063     newaddr.addr[10] = dev->eth->mac.addr[2];
00064     newaddr.addr[11] = 0xff;
00065     newaddr.addr[12] = 0xfe;
00066     newaddr.addr[13] = dev->eth->mac.addr[3];
00067     newaddr.addr[14] = dev->eth->mac.addr[4];
00068     newaddr.addr[15] = dev->eth->mac.addr[5];
00069     link = pico_ipv6_link_add(dev, newaddr, netmask64);
00070     if (link) {
00071         device_init_ipv6_final(dev, &newaddr);
00072     }
00073 
00074     return link;
00075 }
00076 #endif
00077 
00078 static int device_init_mac(struct pico_device *dev, uint8_t *mac)
00079 {
00080     #ifdef PICO_SUPPORT_IPV6
00081     struct pico_ip6 linklocal = {{0xfe, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xaa, 0xaa, 0xaa, 0xff, 0xfe, 0xaa, 0xaa, 0xaa}};
00082     #endif
00083     dev->eth = PICO_ZALLOC(sizeof(struct pico_ethdev));
00084     if (dev->eth) {
00085         memcpy(dev->eth->mac.addr, mac, PICO_SIZE_ETH);
00086         #ifdef PICO_SUPPORT_IPV6
00087         if (pico_ipv6_link_add_local(dev, &linklocal) == NULL) {
00088             PICO_FREE(dev->q_in);
00089             PICO_FREE(dev->q_out);
00090             PICO_FREE(dev->eth);
00091             return -1;
00092         }
00093 
00094         #endif
00095     } else {
00096         pico_err = PICO_ERR_ENOMEM;
00097         return -1;
00098     }
00099 
00100     return 0;
00101 }
00102 
00103 int pico_device_ipv6_random_ll(struct pico_device *dev)
00104 {
00105     #ifdef PICO_SUPPORT_IPV6
00106     struct pico_ip6 linklocal = {{0xfe, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xaa, 0xaa, 0xaa, 0xff, 0xfe, 0xaa, 0xaa, 0xaa}};
00107     struct pico_ip6 netmask6 = {{0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};
00108     uint32_t len = (uint32_t)strlen(dev->name);
00109     if (strcmp(dev->name, "loop")) {
00110         do {
00111             /* privacy extension + unset universal/local and individual/group bit */
00112             len = pico_rand();
00113             linklocal.addr[8]  = (uint8_t)((len & 0xffu) & (uint8_t)(~0x03));
00114             linklocal.addr[9]  = (uint8_t)(len >> 8);
00115             linklocal.addr[10] = (uint8_t)(len >> 16);
00116             linklocal.addr[11] = (uint8_t)(len >> 24);
00117             len = pico_rand();
00118             linklocal.addr[12] = (uint8_t)len;
00119             linklocal.addr[13] = (uint8_t)(len >> 8);
00120             linklocal.addr[14] = (uint8_t)(len >> 16);
00121             linklocal.addr[15] = (uint8_t)(len >> 24);
00122             pico_rand_feed(dev->hash);
00123         } while (pico_ipv6_link_get(&linklocal));
00124 
00125         if (pico_ipv6_link_add(dev, linklocal, netmask6) == NULL) {
00126             return -1;
00127         }
00128     }
00129 
00130     #endif
00131     return 0;
00132 }
00133 
00134 static int device_init_nomac(struct pico_device *dev)
00135 {
00136     if (pico_device_ipv6_random_ll(dev) < 0) {
00137         PICO_FREE(dev->q_in);
00138         PICO_FREE(dev->q_out);
00139         return -1;
00140     }
00141 
00142     dev->eth = NULL;
00143     return 0;
00144 }
00145 
00146 int pico_device_init(struct pico_device *dev, const char *name, uint8_t *mac)
00147 {
00148 
00149     uint32_t len = (uint32_t)strlen(name);
00150     int ret = 0;
00151     if(len > MAX_DEVICE_NAME)
00152         len = MAX_DEVICE_NAME;
00153 
00154     memcpy(dev->name, name, len);
00155     dev->hash = pico_hash(dev->name, len);
00156 
00157     Devices_rr_info.node_in  = NULL;
00158     Devices_rr_info.node_out = NULL;
00159     dev->q_in = PICO_ZALLOC(sizeof(struct pico_queue));
00160     if (!dev->q_in)
00161         return -1;
00162 
00163     dev->q_out = PICO_ZALLOC(sizeof(struct pico_queue));
00164     if (!dev->q_out) {
00165         PICO_FREE(dev->q_in);
00166         return -1;
00167     }
00168 
00169     pico_tree_insert(&Device_tree, dev);
00170     if (!dev->mtu)
00171         dev->mtu = PICO_DEVICE_DEFAULT_MTU;
00172 
00173     if (mac) {
00174         ret = device_init_mac(dev, mac);
00175     } else {
00176         ret = device_init_nomac(dev);
00177     }
00178 
00179     return ret;
00180 }
00181 
00182 static void pico_queue_destroy(struct pico_queue *q)
00183 {
00184     if (q) {
00185         pico_queue_empty(q);
00186         PICO_FREE(q);
00187     }
00188 }
00189 
00190 void pico_device_destroy(struct pico_device *dev)
00191 {
00192     if (dev->destroy)
00193         dev->destroy(dev);
00194 
00195     pico_queue_destroy(dev->q_in);
00196     pico_queue_destroy(dev->q_out);
00197 
00198     if (dev->eth)
00199         PICO_FREE(dev->eth);
00200 
00201 #ifdef PICO_SUPPORT_IPV4
00202     pico_ipv4_cleanup_links(dev);
00203 #endif
00204 #ifdef PICO_SUPPORT_IPV6
00205     pico_ipv6_cleanup_links(dev);
00206 #endif
00207     pico_tree_delete(&Device_tree, dev);
00208 
00209     Devices_rr_info.node_in  = NULL;
00210     Devices_rr_info.node_out = NULL;
00211     PICO_FREE(dev);
00212 }
00213 
00214 static int check_dev_serve_interrupt(struct pico_device *dev, int loop_score)
00215 {
00216     if ((dev->__serving_interrupt) && (dev->dsr)) {
00217         /* call dsr routine */
00218         loop_score = dev->dsr(dev, loop_score);
00219     }
00220 
00221     return loop_score;
00222 }
00223 
00224 static int check_dev_serve_polling(struct pico_device *dev, int loop_score)
00225 {
00226     if (dev->poll) {
00227         loop_score = dev->poll(dev, loop_score);
00228     }
00229 
00230     return loop_score;
00231 }
00232 
00233 static int devloop_in(struct pico_device *dev, int loop_score)
00234 {
00235     struct pico_frame *f;
00236     while(loop_score > 0) {
00237         if (dev->q_in->frames == 0)
00238             break;
00239 
00240         /* Receive */
00241         f = pico_dequeue(dev->q_in);
00242         if (f) {
00243             if (dev->eth) {
00244                 f->datalink_hdr = f->buffer;
00245                 (void)pico_ethernet_receive(f);
00246             } else {
00247                 f->net_hdr = f->buffer;
00248                 pico_network_receive(f);
00249             }
00250 
00251             loop_score--;
00252         }
00253     }
00254     return loop_score;
00255 }
00256 
00257 static int devloop_sendto_dev(struct pico_device *dev, struct pico_frame *f)
00258 {
00259 
00260     if (dev->eth) {
00261         /* Ethernet: pass management of the frame to the pico_ethernet_send() rdv function */
00262         return pico_ethernet_send(f);
00263     } else {
00264         /* non-ethernet: no post-processing needed */
00265         return (dev->send(dev, f->start, (int)f->len) <= 0); /* Return 0 upon success, which is dev->send() > 0 */
00266     }
00267 }
00268 
00269 static int devloop_out(struct pico_device *dev, int loop_score)
00270 {
00271     struct pico_frame *f;
00272     while(loop_score > 0) {
00273         if (dev->q_out->frames == 0)
00274             break;
00275 
00276         /* Device dequeue + send */
00277         f = pico_queue_peek(dev->q_out);
00278         if (!f)
00279             break;
00280 
00281         if (devloop_sendto_dev(dev, f) == 0) { /* success. */
00282             f = pico_dequeue(dev->q_out);
00283             pico_frame_discard(f); /* SINGLE POINT OF DISCARD for OUTGOING FRAMES */
00284             loop_score--;
00285         } else
00286             break; /* Don't discard */
00287 
00288     }
00289     return loop_score;
00290 }
00291 
00292 static int devloop(struct pico_device *dev, int loop_score, int direction)
00293 {
00294     /* If device supports interrupts, read the value of the condition and trigger the dsr */
00295     loop_score = check_dev_serve_interrupt(dev, loop_score);
00296 
00297     /* If device supports polling, give control. Loop score is managed internally,
00298      * remaining loop points are returned. */
00299     loop_score = check_dev_serve_polling(dev, loop_score);
00300 
00301     if (direction == PICO_LOOP_DIR_OUT)
00302         loop_score = devloop_out(dev, loop_score);
00303     else
00304         loop_score = devloop_in(dev, loop_score);
00305 
00306     return loop_score;
00307 }
00308 
00309 
00310 static struct pico_tree_node *pico_dev_roundrobin_start(int direction)
00311 {
00312     if (Devices_rr_info.node_in == NULL)
00313         Devices_rr_info.node_in = pico_tree_firstNode(Device_tree.root);
00314 
00315     if (Devices_rr_info.node_out == NULL)
00316         Devices_rr_info.node_out = pico_tree_firstNode(Device_tree.root);
00317 
00318     if (direction == PICO_LOOP_DIR_IN)
00319         return Devices_rr_info.node_in;
00320     else
00321         return Devices_rr_info.node_out;
00322 }
00323 
00324 static void pico_dev_roundrobin_end(int direction, struct pico_tree_node *last)
00325 {
00326     if (direction == PICO_LOOP_DIR_IN)
00327         Devices_rr_info.node_in = last;
00328     else
00329         Devices_rr_info.node_out = last;
00330 }
00331 
00332 #define DEV_LOOP_MIN  16
00333 
00334 int pico_devices_loop(int loop_score, int direction)
00335 {
00336     struct pico_device *start, *next;
00337     struct pico_tree_node *next_node  = pico_dev_roundrobin_start(direction);
00338 
00339     if (!next_node)
00340         return loop_score;
00341 
00342     next = next_node->keyValue;
00343     start = next;
00344 
00345     /* round-robin all devices, break if traversed all devices */
00346     while ((loop_score > DEV_LOOP_MIN) && (next != NULL)) {
00347         loop_score = devloop(next, loop_score, direction);
00348         next_node = pico_tree_next(next_node);
00349         next = next_node->keyValue;
00350         if (next == NULL)
00351         {
00352             next_node = pico_tree_firstNode(Device_tree.root);
00353             next = next_node->keyValue;
00354         }
00355 
00356         if (next == start)
00357             break;
00358     }
00359     pico_dev_roundrobin_end(direction, next_node);
00360     return loop_score;
00361 }
00362 
00363 struct pico_device *pico_get_device(const char*name)
00364 {
00365     struct pico_device *dev;
00366     struct pico_tree_node *index;
00367     pico_tree_foreach(index, &Device_tree){
00368         dev = index->keyValue;
00369         if(strcmp(name, dev->name) == 0)
00370             return dev;
00371     }
00372     return NULL;
00373 }
00374 
00375 int32_t pico_device_broadcast(struct pico_frame *f)
00376 {
00377     struct pico_tree_node *index;
00378     int32_t ret = -1;
00379 
00380     pico_tree_foreach(index, &Device_tree)
00381     {
00382         struct pico_device *dev = index->keyValue;
00383         if(dev != f->dev)
00384         {
00385             struct pico_frame *copy = pico_frame_copy(f);
00386 
00387             if(!copy)
00388                 break;
00389 
00390             copy->dev = dev;
00391             copy->dev->send(copy->dev, copy->start, (int)copy->len);
00392             pico_frame_discard(copy);
00393         }
00394         else
00395         {
00396             ret = f->dev->send(f->dev, f->start, (int)f->len);
00397         }
00398     }
00399     return ret;
00400 }
00401 
00402 int pico_device_link_state(struct pico_device *dev)
00403 {
00404     if (!dev->link_state)
00405         return 1; /* Not supported, assuming link is always up */
00406 
00407     return dev->link_state(dev);
00408 }