pico_stack.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 
00011 #include "pico_config.h"
00012 #include "pico_frame.h"
00013 #include "pico_device.h"
00014 #include "pico_protocol.h"
00015 #include "pico_stack.h"
00016 #include "pico_addressing.h"
00017 #include "pico_dns_client.h"
00018 
00019 #include "pico_olsr.h"
00020 #include "pico_aodv.h"
00021 #include "pico_eth.h"
00022 #include "pico_arp.h"
00023 #include "pico_ipv4.h"
00024 #include "pico_ipv6.h"
00025 #include "pico_icmp4.h"
00026 #include "pico_icmp6.h"
00027 #include "pico_igmp.h"
00028 #include "pico_udp.h"
00029 #include "pico_tcp.h"
00030 #include "pico_socket.h"
00031 #include "heap.h"
00032 
00033 #define IS_LIMITED_BCAST(f) (((struct pico_ipv4_hdr *) f->net_hdr)->dst.addr == PICO_IP4_BCAST)
00034 
00035 const uint8_t PICO_ETHADDR_ALL[6] = {
00036     0xff, 0xff, 0xff, 0xff, 0xff, 0xff
00037 };
00038 
00039 # define PICO_SIZE_MCAST 3
00040 const uint8_t PICO_ETHADDR_MCAST[6] = {
00041     0x01, 0x00, 0x5e, 0x00, 0x00, 0x00
00042 };
00043 
00044 #ifdef PICO_SUPPORT_IPV6
00045 # define PICO_SIZE_MCAST6 2
00046 const uint8_t PICO_ETHADDR_MCAST6[6] = {
00047     0x33, 0x33, 0x00, 0x00, 0x00, 0x00
00048 };
00049 #endif
00050 
00051 
00052 volatile pico_time pico_tick;
00053 volatile pico_err_t pico_err;
00054 
00055 static uint32_t _rand_seed;
00056 
00057 void WEAK pico_rand_feed(uint32_t feed)
00058 {
00059     if (!feed)
00060         return;
00061 
00062     _rand_seed *= 1664525;
00063     _rand_seed += 1013904223;
00064     _rand_seed ^= ~(feed);
00065 }
00066 
00067 uint32_t WEAK pico_rand(void)
00068 {
00069     pico_rand_feed((uint32_t)pico_tick);
00070     return _rand_seed;
00071 }
00072 
00073 void pico_to_lowercase(char *str)
00074 {
00075     int i = 0;
00076     if (!str)
00077         return;
00078 
00079     while(str[i]) {
00080         if ((str[i] <= 'Z') && (str[i] >= 'A'))
00081             str[i] = (char) (str[i] - (char)('A' - 'a'));
00082 
00083         i++;
00084     }
00085 }
00086 
00087 /* NOTIFICATIONS: distributed notifications for stack internal errors.
00088  */
00089 
00090 int pico_notify_socket_unreachable(struct pico_frame *f)
00091 {
00092     if (0) {}
00093 
00094 #ifdef PICO_SUPPORT_ICMP4
00095     else if (IS_IPV4(f)) {
00096         pico_icmp4_port_unreachable(f);
00097     }
00098 #endif
00099 #ifdef PICO_SUPPORT_ICMP6
00100     else if (IS_IPV6(f)) {
00101         pico_icmp6_port_unreachable(f);
00102     }
00103 #endif
00104 
00105     return 0;
00106 }
00107 
00108 int pico_notify_proto_unreachable(struct pico_frame *f)
00109 {
00110     if (0) {}
00111 
00112 #ifdef PICO_SUPPORT_ICMP4
00113     else if (IS_IPV4(f)) {
00114         pico_icmp4_proto_unreachable(f);
00115     }
00116 #endif
00117 #ifdef PICO_SUPPORT_ICMP6
00118     else if (IS_IPV6(f)) {
00119         pico_icmp6_proto_unreachable(f);
00120     }
00121 #endif
00122     return 0;
00123 }
00124 
00125 int pico_notify_dest_unreachable(struct pico_frame *f)
00126 {
00127     if (0) {}
00128 
00129 #ifdef PICO_SUPPORT_ICMP4
00130     else if (IS_IPV4(f)) {
00131         pico_icmp4_dest_unreachable(f);
00132     }
00133 #endif
00134 #ifdef PICO_SUPPORT_ICMP6
00135     else if (IS_IPV6(f)) {
00136         pico_icmp6_dest_unreachable(f);
00137     }
00138 #endif
00139     return 0;
00140 }
00141 
00142 int pico_notify_ttl_expired(struct pico_frame *f)
00143 {
00144     if (0) {}
00145 
00146 #ifdef PICO_SUPPORT_ICMP4
00147     else if (IS_IPV4(f)) {
00148         pico_icmp4_ttl_expired(f);
00149     }
00150 #endif
00151 #ifdef PICO_SUPPORT_ICMP6
00152     else if (IS_IPV6(f)) {
00153         pico_icmp6_ttl_expired(f);
00154     }
00155 #endif
00156     return 0;
00157 }
00158 
00159 int pico_notify_frag_expired(struct pico_frame *f)
00160 {
00161     if (0) {}
00162 
00163 #ifdef PICO_SUPPORT_ICMP4
00164     else if (IS_IPV4(f)) {
00165         pico_icmp4_frag_expired(f);
00166     }
00167 #endif
00168 #ifdef PICO_SUPPORT_ICMP6
00169     else if (IS_IPV6(f)) {
00170         pico_icmp6_frag_expired(f);
00171     }
00172 #endif
00173     return 0;
00174 }
00175 
00176 int pico_notify_pkt_too_big(struct pico_frame *f)
00177 {
00178     if (0) {}
00179 
00180 #ifdef PICO_SUPPORT_ICMP4
00181     else if (IS_IPV4(f)) {
00182         pico_icmp4_mtu_exceeded(f);
00183     }
00184 #endif
00185 #ifdef PICO_SUPPORT_ICMP6
00186     else if (IS_IPV6(f)) {
00187         pico_icmp6_pkt_too_big(f);
00188     }
00189 #endif
00190     return 0;
00191 }
00192 
00193 
00194 /* Transport layer */
00195 MOCKABLE int32_t pico_transport_receive(struct pico_frame *f, uint8_t proto)
00196 {
00197     int32_t ret = -1;
00198     switch (proto) {
00199 
00200 #ifdef PICO_SUPPORT_ICMP4
00201     case PICO_PROTO_ICMP4:
00202         ret = pico_enqueue(pico_proto_icmp4.q_in, f);
00203         break;
00204 #endif
00205 
00206 #ifdef PICO_SUPPORT_ICMP6
00207     case PICO_PROTO_ICMP6:
00208         ret = pico_enqueue(pico_proto_icmp6.q_in, f);
00209         break;
00210 #endif
00211 
00212 
00213 #if defined(PICO_SUPPORT_IGMP) && defined(PICO_SUPPORT_MCAST)
00214     case PICO_PROTO_IGMP:
00215         ret = pico_enqueue(pico_proto_igmp.q_in, f);
00216         break;
00217 #endif
00218 
00219 #ifdef PICO_SUPPORT_UDP
00220     case PICO_PROTO_UDP:
00221         ret = pico_enqueue(pico_proto_udp.q_in, f);
00222         break;
00223 #endif
00224 
00225 #ifdef PICO_SUPPORT_TCP
00226     case PICO_PROTO_TCP:
00227         ret = pico_enqueue(pico_proto_tcp.q_in, f);
00228         break;
00229 #endif
00230 
00231     default:
00232         /* Protocol not available */
00233         dbg("pkt: no such protocol (%d)\n", proto);
00234         pico_notify_proto_unreachable(f);
00235         pico_frame_discard(f);
00236         ret = -1;
00237     }
00238     return ret;
00239 }
00240 
00241 int32_t pico_network_receive(struct pico_frame *f)
00242 {
00243     if (0) {}
00244 
00245 #ifdef PICO_SUPPORT_IPV4
00246     else if (IS_IPV4(f)) {
00247         pico_enqueue(pico_proto_ipv4.q_in, f);
00248     }
00249 #endif
00250 #ifdef PICO_SUPPORT_IPV6
00251     else if (IS_IPV6(f)) {
00252         pico_enqueue(pico_proto_ipv6.q_in, f);
00253     }
00254 #endif
00255     else {
00256         dbg("Network not found.\n");
00257         pico_frame_discard(f);
00258         return -1;
00259     }
00260     return (int32_t)f->buffer_len;
00261 }
00262 
00263 /* Network layer: interface towards socket for frame sending */
00264 int32_t pico_network_send(struct pico_frame *f)
00265 {
00266     if (!f || !f->sock || !f->sock->net) {
00267         pico_frame_discard(f);
00268         return -1;
00269     }
00270 
00271     return f->sock->net->push(f->sock->net, f);
00272 }
00273 
00274 int pico_source_is_local(struct pico_frame *f)
00275 {
00276     if (0) { }
00277 
00278 #ifdef PICO_SUPPORT_IPV4
00279     else if (IS_IPV4(f)) {
00280         struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr;
00281         if (hdr->src.addr == PICO_IPV4_INADDR_ANY)
00282             return 1;
00283 
00284         if (pico_ipv4_link_find(&hdr->src))
00285             return 1;
00286     }
00287 #endif
00288 #ifdef PICO_SUPPORT_IPV6
00289     else if (IS_IPV6(f)) {
00290         struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr;
00291         if (pico_ipv6_is_unspecified(hdr->src.addr) || pico_ipv6_link_find(&hdr->src))
00292             return 1;
00293     }
00294 #endif
00295     return 0;
00296 }
00297 
00298 #ifdef PICO_SUPPORT_ETH
00299 /* DATALINK LEVEL: interface from network to the device
00300  * and vice versa.
00301  */
00302 
00303 /* The pico_ethernet_receive() function is used by
00304  * those devices supporting ETH in order to push packets up
00305  * into the stack.
00306  */
00307 
00308 static int destination_is_bcast(struct pico_frame *f)
00309 {
00310     if (!f)
00311         return 0;
00312 
00313     if (IS_IPV6(f))
00314         return 0;
00315 
00316 #ifdef PICO_SUPPORT_IPV4
00317     else {
00318         struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
00319         return pico_ipv4_is_broadcast(hdr->dst.addr);
00320     }
00321 #else
00322     return 0;
00323 #endif
00324 }
00325 
00326 static int destination_is_mcast(struct pico_frame *f)
00327 {
00328     int ret = 0;
00329     if (!f)
00330         return 0;
00331 
00332 #ifdef PICO_SUPPORT_IPV6
00333     if (IS_IPV6(f)) {
00334         struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *) f->net_hdr;
00335         ret = pico_ipv6_is_multicast(hdr->dst.addr);
00336     }
00337 
00338 #endif
00339 #ifdef PICO_SUPPORT_IPV4
00340     else {
00341         struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
00342         ret = pico_ipv4_is_multicast(hdr->dst.addr);
00343     }
00344 #endif
00345 
00346     return ret;
00347 }
00348 
00349 #ifdef PICO_SUPPORT_IPV4
00350 static int32_t pico_ipv4_ethernet_receive(struct pico_frame *f)
00351 {
00352     if (IS_IPV4(f)) {
00353         pico_enqueue(pico_proto_ipv4.q_in, f);
00354     } else {
00355         (void)pico_icmp4_param_problem(f, 0);
00356         pico_frame_discard(f);
00357         return -1;
00358     }
00359 
00360     return (int32_t)f->buffer_len;
00361 }
00362 #endif
00363 
00364 #ifdef PICO_SUPPORT_IPV6
00365 static int32_t pico_ipv6_ethernet_receive(struct pico_frame *f)
00366 {
00367     if (IS_IPV6(f)) {
00368         pico_enqueue(pico_proto_ipv6.q_in, f);
00369     } else {
00370         /* Wrong version for link layer type */
00371         pico_frame_discard(f);
00372         return -1;
00373     }
00374 
00375     return (int32_t)f->buffer_len;
00376 }
00377 #endif
00378 
00379 static int32_t pico_ll_receive(struct pico_frame *f)
00380 {
00381     struct pico_eth_hdr *hdr = (struct pico_eth_hdr *) f->datalink_hdr;
00382     f->net_hdr = f->datalink_hdr + sizeof(struct pico_eth_hdr);
00383 
00384 #if (defined PICO_SUPPORT_IPV4) && (defined PICO_SUPPORT_ETH)
00385     if (hdr->proto == PICO_IDETH_ARP)
00386         return pico_arp_receive(f);
00387 
00388 #endif
00389 
00390 #if defined (PICO_SUPPORT_IPV4)
00391     if (hdr->proto == PICO_IDETH_IPV4)
00392         return pico_ipv4_ethernet_receive(f);
00393 
00394 #endif
00395 
00396 #if defined (PICO_SUPPORT_IPV6)
00397     if (hdr->proto == PICO_IDETH_IPV6)
00398         return pico_ipv6_ethernet_receive(f);
00399 
00400 #endif
00401 
00402     pico_frame_discard(f);
00403     return -1;
00404 }
00405 
00406 static void pico_ll_check_bcast(struct pico_frame *f)
00407 {
00408     struct pico_eth_hdr *hdr = (struct pico_eth_hdr *) f->datalink_hdr;
00409     /* Indicate a link layer broadcast packet */
00410     if (memcmp(hdr->daddr, PICO_ETHADDR_ALL, PICO_SIZE_ETH) == 0)
00411         f->flags |= PICO_FRAME_FLAG_BCAST;
00412 }
00413 
00414 int32_t pico_ethernet_receive(struct pico_frame *f)
00415 {
00416     struct pico_eth_hdr *hdr;
00417     if (!f || !f->dev || !f->datalink_hdr)
00418     {
00419         pico_frame_discard(f);
00420         return -1;
00421     }
00422 
00423     hdr = (struct pico_eth_hdr *) f->datalink_hdr;
00424     if ((memcmp(hdr->daddr, f->dev->eth->mac.addr, PICO_SIZE_ETH) != 0) &&
00425         (memcmp(hdr->daddr, PICO_ETHADDR_MCAST, PICO_SIZE_MCAST) != 0) &&
00426 #ifdef PICO_SUPPORT_IPV6
00427         (memcmp(hdr->daddr, PICO_ETHADDR_MCAST6, PICO_SIZE_MCAST6) != 0) &&
00428 #endif
00429         (memcmp(hdr->daddr, PICO_ETHADDR_ALL, PICO_SIZE_ETH) != 0))
00430     {
00431         pico_frame_discard(f);
00432         return -1;
00433     }
00434 
00435     pico_ll_check_bcast(f);
00436     return pico_ll_receive(f);
00437 }
00438 
00439 static struct pico_eth *pico_ethernet_mcast_translate(struct pico_frame *f, uint8_t *pico_mcast_mac)
00440 {
00441     struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
00442 
00443     /* place 23 lower bits of IP in lower 23 bits of MAC */
00444     pico_mcast_mac[5] = (long_be(hdr->dst.addr) & 0x000000FFu);
00445     pico_mcast_mac[4] = (uint8_t)((long_be(hdr->dst.addr) & 0x0000FF00u) >> 8u);
00446     pico_mcast_mac[3] = (uint8_t)((long_be(hdr->dst.addr) & 0x007F0000u) >> 16u);
00447 
00448     return (struct pico_eth *)pico_mcast_mac;
00449 }
00450 
00451 
00452 #ifdef PICO_SUPPORT_IPV6
00453 static struct pico_eth *pico_ethernet_mcast6_translate(struct pico_frame *f, uint8_t *pico_mcast6_mac)
00454 {
00455     struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr;
00456 
00457     /* first 2 octets are 0x33, last four are the last four of dst */
00458     pico_mcast6_mac[5] = hdr->dst.addr[PICO_SIZE_IP6 - 1];
00459     pico_mcast6_mac[4] = hdr->dst.addr[PICO_SIZE_IP6 - 2];
00460     pico_mcast6_mac[3] = hdr->dst.addr[PICO_SIZE_IP6 - 3];
00461     pico_mcast6_mac[2] = hdr->dst.addr[PICO_SIZE_IP6 - 4];
00462 
00463     return (struct pico_eth *)pico_mcast6_mac;
00464 }
00465 #endif
00466 
00467 static int pico_ethernet_ipv6_dst(struct pico_frame *f, struct pico_eth *const dstmac)
00468 {
00469     int retval = -1;
00470     if (!dstmac)
00471         return -1;
00472 
00473     #ifdef PICO_SUPPORT_IPV6
00474     if (destination_is_mcast(f)) {
00475         uint8_t pico_mcast6_mac[6] = {
00476             0x33, 0x33, 0x00, 0x00, 0x00, 0x00
00477         };
00478         pico_ethernet_mcast6_translate(f, pico_mcast6_mac);
00479         memcpy(dstmac, pico_mcast6_mac, PICO_SIZE_ETH);
00480         retval = 0;
00481     } else {
00482         struct pico_eth *neighbor = pico_ipv6_get_neighbor(f);
00483         if (neighbor)
00484         {
00485             memcpy(dstmac, neighbor, PICO_SIZE_ETH);
00486             retval = 0;
00487         }
00488     }
00489 
00490     #else
00491     (void)f;
00492     pico_err = PICO_ERR_EPROTONOSUPPORT;
00493     #endif
00494     return retval;
00495 }
00496 
00497 
00498 /* Ethernet send, first attempt: try our own address.
00499  * Returns 0 if the packet is not for us.
00500  * Returns 1 if the packet is cloned to our own receive queue, so the caller can discard the original frame.
00501  * */
00502 static int32_t pico_ethsend_local(struct pico_frame *f, struct pico_eth_hdr *hdr)
00503 {
00504     if (!hdr)
00505         return 0;
00506 
00507     /* Check own mac */
00508     if(!memcmp(hdr->daddr, hdr->saddr, PICO_SIZE_ETH)) {
00509         struct pico_frame *clone = pico_frame_copy(f);
00510         dbg("sending out packet destined for our own mac\n");
00511         (void)pico_ethernet_receive(clone);
00512         return 1;
00513     }
00514 
00515     return 0;
00516 }
00517 
00518 /* Ethernet send, second attempt: try bcast.
00519  * Returns 0 if the packet is not bcast, so it will be handled somewhere else.
00520  * Returns 1 if the packet is handled by the pico_device_broadcast() function, so it can be discarded.
00521  * */
00522 static int32_t pico_ethsend_bcast(struct pico_frame *f)
00523 {
00524     if (IS_LIMITED_BCAST(f)) {
00525         (void)pico_device_broadcast(f); /* We can discard broadcast even if it's not sent. */
00526         return 1;
00527     }
00528 
00529     return 0;
00530 }
00531 
00532 /* Ethernet send, third attempt: try unicast.
00533  * If the device driver is busy, we return 0, so the stack won't discard the frame.
00534  * In case of success, we can safely return 1.
00535  */
00536 static int32_t pico_ethsend_dispatch(struct pico_frame *f)
00537 {
00538     int ret = f->dev->send(f->dev, f->start, (int) f->len);
00539     if (ret <= 0)
00540         return 0; /* Failure to deliver! */
00541     else {
00542         return 1; /* Frame is in flight by now. */
00543     }
00544 }
00545 
00546 
00547 
00548 
00549 /* This function looks for the destination mac address
00550  * in order to send the frame being processed.
00551  */
00552 
00553 int32_t MOCKABLE pico_ethernet_send(struct pico_frame *f)
00554 {
00555     struct pico_eth dstmac;
00556     uint8_t dstmac_valid = 0;
00557     uint16_t proto = PICO_IDETH_IPV4;
00558 
00559 #ifdef PICO_SUPPORT_IPV6
00560     /* Step 1: If the frame has an IPv6 packet,
00561      * destination address is taken from the ND tables
00562      */
00563     if (IS_IPV6(f)) {
00564         if (pico_ethernet_ipv6_dst(f, &dstmac) < 0)
00565         {
00566             pico_ipv6_nd_postpone(f);
00567             return 0; /* I don't care if frame was actually postponed. If there is no room in the ND table, discard safely. */
00568         }
00569 
00570         dstmac_valid = 1;
00571         proto = PICO_IDETH_IPV6;
00572     }
00573     else
00574 #endif
00575 
00576     /* In case of broadcast (IPV4 only), dst mac is FF:FF:... */
00577     if (IS_BCAST(f) || destination_is_bcast(f))
00578     {
00579         memcpy(&dstmac, PICO_ETHADDR_ALL, PICO_SIZE_ETH);
00580         dstmac_valid = 1;
00581     }
00582 
00583     /* In case of multicast, dst mac is translated from the group address */
00584     else if (destination_is_mcast(f)) {
00585         uint8_t pico_mcast_mac[6] = {
00586             0x01, 0x00, 0x5e, 0x00, 0x00, 0x00
00587         };
00588         pico_ethernet_mcast_translate(f, pico_mcast_mac);
00589         memcpy(&dstmac, pico_mcast_mac, PICO_SIZE_ETH);
00590         dstmac_valid = 1;
00591     }
00592 
00593 #if (defined PICO_SUPPORT_IPV4)
00594     else {
00595         struct pico_eth *arp_get;
00596         arp_get = pico_arp_get(f);
00597         if (arp_get) {
00598             memcpy(&dstmac, arp_get, PICO_SIZE_ETH);
00599             dstmac_valid = 1;
00600         } else {
00601             /* At this point, ARP will discard the frame in any case.
00602              * It is safe to return without discarding.
00603              */
00604             pico_arp_postpone(f);
00605             return 0;
00606             /* Same case as for IPv6 ... */
00607         }
00608 
00609     }
00610 #endif
00611 
00612     /* This sets destination and source address, then pushes the packet to the device. */
00613     if (dstmac_valid) {
00614         struct pico_eth_hdr *hdr;
00615         hdr = (struct pico_eth_hdr *) f->datalink_hdr;
00616         if ((f->start > f->buffer) && ((f->start - f->buffer) >= PICO_SIZE_ETHHDR))
00617         {
00618             f->start -= PICO_SIZE_ETHHDR;
00619             f->len += PICO_SIZE_ETHHDR;
00620             f->datalink_hdr = f->start;
00621             hdr = (struct pico_eth_hdr *) f->datalink_hdr;
00622             memcpy(hdr->saddr, f->dev->eth->mac.addr, PICO_SIZE_ETH);
00623             memcpy(hdr->daddr, &dstmac, PICO_SIZE_ETH);
00624             hdr->proto = proto;
00625         }
00626 
00627         if (pico_ethsend_local(f, hdr) || pico_ethsend_bcast(f) || pico_ethsend_dispatch(f)) {
00628             /* one of the above functions has delivered the frame accordingly. (returned != 0)
00629              * It is safe to directly return success.
00630              * */
00631             return 0;
00632         }
00633     }
00634 
00635     /* Failure: do not dequeue the frame, keep it for later. */
00636     return -1;
00637 }
00638 
00639 #endif /* PICO_SUPPORT_ETH */
00640 
00641 
00642 void pico_store_network_origin(void *src, struct pico_frame *f)
00643 {
00644   #ifdef PICO_SUPPORT_IPV4
00645     struct pico_ip4 *ip4;
00646   #endif
00647 
00648   #ifdef PICO_SUPPORT_IPV6
00649     struct pico_ip6 *ip6;
00650   #endif
00651 
00652   #ifdef PICO_SUPPORT_IPV4
00653     if (IS_IPV4(f)) {
00654         struct pico_ipv4_hdr *hdr;
00655         hdr = (struct pico_ipv4_hdr *) f->net_hdr;
00656         ip4 = (struct pico_ip4 *) src;
00657         ip4->addr = hdr->src.addr;
00658     }
00659 
00660   #endif
00661   #ifdef PICO_SUPPORT_IPV6
00662     if (IS_IPV6(f)) {
00663         struct pico_ipv6_hdr *hdr;
00664         hdr = (struct pico_ipv6_hdr *) f->net_hdr;
00665         ip6 = (struct pico_ip6 *) src;
00666         memcpy(ip6->addr, hdr->src.addr, PICO_SIZE_IP6);
00667     }
00668 
00669   #endif
00670 }
00671 
00672 int pico_address_compare(union pico_address *a, union pico_address *b, uint16_t proto)
00673 {
00674     #ifdef PICO_SUPPORT_IPV6
00675     if (proto == PICO_PROTO_IPV6) {
00676         return pico_ipv6_compare(&a->ip6, &b->ip6);
00677     }
00678 
00679     #endif
00680     #ifdef PICO_SUPPORT_IPV4
00681     if (proto == PICO_PROTO_IPV4) {
00682         return pico_ipv4_compare(&a->ip4, &b->ip4);
00683     }
00684 
00685     #endif
00686     return 0;
00687 
00688 }
00689 
00690 int pico_frame_dst_is_unicast(struct pico_frame *f)
00691 {
00692     if (0) {
00693         return 0;
00694     }
00695 
00696 #ifdef PICO_SUPPORT_IPV4
00697     if (IS_IPV4(f)) {
00698         struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr;
00699         if (pico_ipv4_is_multicast(hdr->dst.addr) || pico_ipv4_is_broadcast(hdr->dst.addr))
00700             return 0;
00701 
00702         return 1;
00703     }
00704 
00705 #endif
00706 
00707 #ifdef PICO_SUPPORT_IPV6
00708     if (IS_IPV6(f)) {
00709         struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr;
00710         if (pico_ipv6_is_multicast(hdr->dst.addr) || pico_ipv6_is_unspecified(hdr->dst.addr))
00711             return 0;
00712 
00713         return 1;
00714     }
00715 
00716 #endif
00717     else return 0;
00718 }
00719 
00720 
00721 /* LOWEST LEVEL: interface towards devices. */
00722 /* Device driver will call this function which returns immediately.
00723  * Incoming packet will be processed later on in the dev loop.
00724  */
00725 int32_t pico_stack_recv(struct pico_device *dev, uint8_t *buffer, uint32_t len)
00726 {
00727     struct pico_frame *f;
00728     int32_t ret;
00729     if (len == 0)
00730         return -1;
00731 
00732     f = pico_frame_alloc(len);
00733     if (!f)
00734     {
00735         dbg("Cannot alloc incoming frame!\n");
00736         return -1;
00737     }
00738 
00739     /* Association to the device that just received the frame. */
00740     f->dev = dev;
00741 
00742     /* Setup the start pointer, length. */
00743     f->start = f->buffer;
00744     f->len = f->buffer_len;
00745     if (f->len > 8) {
00746         uint32_t rand, mid_frame = (f->buffer_len >> 2) << 1;
00747         mid_frame -= (mid_frame % 4);
00748         memcpy(&rand, f->buffer + mid_frame, sizeof(uint32_t));
00749         pico_rand_feed(rand);
00750     }
00751 
00752     memcpy(f->buffer, buffer, len);
00753     ret = pico_enqueue(dev->q_in, f);
00754     if (ret <= 0) {
00755         pico_frame_discard(f);
00756     }
00757 
00758     return ret;
00759 }
00760 
00761 static int32_t _pico_stack_recv_zerocopy(struct pico_device *dev, uint8_t *buffer, uint32_t len, int ext_buffer, void (*notify_free)(uint8_t *))
00762 {
00763     struct pico_frame *f;
00764     int ret;
00765     if (len == 0)
00766         return -1;
00767 
00768     f = pico_frame_alloc_skeleton(len, ext_buffer);
00769     if (!f)
00770     {
00771         dbg("Cannot alloc incoming frame!\n");
00772         return -1;
00773     }
00774 
00775     if (pico_frame_skeleton_set_buffer(f, buffer) < 0)
00776     {
00777         dbg("Invalid zero-copy buffer!\n");
00778         PICO_FREE(f->usage_count);
00779         PICO_FREE(f);
00780         return -1;
00781     }
00782 
00783     if (notify_free) {
00784         f->notify_free = notify_free;
00785     }
00786 
00787     f->dev = dev;
00788     ret = pico_enqueue(dev->q_in, f);
00789     if (ret <= 0) {
00790         pico_frame_discard(f);
00791     }
00792 
00793     return ret;
00794 }
00795 
00796 int32_t pico_stack_recv_zerocopy(struct pico_device *dev, uint8_t *buffer, uint32_t len)
00797 {
00798     return _pico_stack_recv_zerocopy(dev, buffer, len, 0, NULL);
00799 }
00800 
00801 int32_t pico_stack_recv_zerocopy_ext_buffer(struct pico_device *dev, uint8_t *buffer, uint32_t len)
00802 {
00803     return _pico_stack_recv_zerocopy(dev, buffer, len, 1, NULL);
00804 }
00805 
00806 int32_t pico_stack_recv_zerocopy_ext_buffer_notify(struct pico_device *dev, uint8_t *buffer, uint32_t len, void (*notify_free)(uint8_t *buffer))
00807 {
00808     return _pico_stack_recv_zerocopy(dev, buffer, len, 1, notify_free);
00809 }
00810 
00811 int32_t pico_sendto_dev(struct pico_frame *f)
00812 {
00813     if (!f->dev) {
00814         pico_frame_discard(f);
00815         return -1;
00816     } else {
00817         if (f->len > 8) {
00818             uint32_t rand, mid_frame = (f->buffer_len >> 2) << 1;
00819             mid_frame -= (mid_frame % 4);
00820             memcpy(&rand, f->buffer + mid_frame, sizeof(uint32_t));
00821             pico_rand_feed(rand);
00822         }
00823 
00824         return pico_enqueue(f->dev->q_out, f);
00825     }
00826 }
00827 
00828 struct pico_timer
00829 {
00830     void *arg;
00831     void (*timer)(pico_time timestamp, void *arg);
00832 };
00833 
00834 struct pico_timer_ref
00835 {
00836     pico_time expire;
00837     struct pico_timer *tmr;
00838 };
00839 
00840 typedef struct pico_timer_ref pico_timer_ref;
00841 
00842 DECLARE_HEAP(pico_timer_ref, expire);
00843 
00844 static heap_pico_timer_ref *Timers;
00845 
00846 int32_t pico_seq_compare(uint32_t a, uint32_t b)
00847 {
00848     uint32_t thresh = ((uint32_t)(-1)) >> 1;
00849 
00850     if (a > b) /* return positive number, if not wrapped */
00851     {
00852         if ((a - b) > thresh) /* b wrapped */
00853             return -(int32_t)(b - a); /* b = very small,     a = very big      */
00854         else
00855             return (int32_t)(a - b); /* a = biggest,        b = a bit smaller */
00856 
00857     }
00858 
00859     if (a < b) /* return negative number, if not wrapped */
00860     {
00861         if ((b - a) > thresh) /* a wrapped */
00862             return (int32_t)(a - b); /* a = very small,     b = very big      */
00863         else
00864             return -(int32_t)(b - a); /* b = biggest,        a = a bit smaller */
00865 
00866     }
00867 
00868     return 0;
00869 }
00870 
00871 static void pico_check_timers(void)
00872 {
00873     struct pico_timer *t;
00874     struct pico_timer_ref tref_unused, *tref = heap_first(Timers);
00875     pico_tick = PICO_TIME_MS();
00876     while((tref) && (tref->expire < pico_tick)) {
00877         t = tref->tmr;
00878         if (t && t->timer)
00879             t->timer(pico_tick, t->arg);
00880 
00881         if (t)
00882         {
00883             PICO_FREE(t);
00884         }
00885 
00886         t = NULL;
00887         heap_peek(Timers, &tref_unused);
00888         tref = heap_first(Timers);
00889     }
00890 }
00891 
00892 void MOCKABLE pico_timer_cancel(struct pico_timer *t)
00893 {
00894     uint32_t i;
00895     struct pico_timer_ref *tref = Timers->top;
00896     if (!t)
00897         return;
00898 
00899     for (i = 1; i <= Timers->n; i++) {
00900         if (tref[i].tmr == t) {
00901             Timers->top[i].tmr = NULL;
00902             PICO_FREE(t);
00903             break;
00904         }
00905     }
00906 }
00907 
00908 #define PROTO_DEF_NR      11
00909 #define PROTO_DEF_AVG_NR  4
00910 #define PROTO_DEF_SCORE   32
00911 #define PROTO_MIN_SCORE   32
00912 #define PROTO_MAX_SCORE   128
00913 #define PROTO_LAT_IND     3   /* latency indication 0-3 (lower is better latency performance), x1, x2, x4, x8 */
00914 #define PROTO_MAX_LOOP    (PROTO_MAX_SCORE << PROTO_LAT_IND) /* max global loop score, so per tick */
00915 
00916 static int calc_score(int *score, int *index, int avg[][PROTO_DEF_AVG_NR], int *ret)
00917 {
00918     int temp, i, j, sum;
00919     int max_total = PROTO_MAX_LOOP, total = 0;
00920 
00921     /* dbg("USED SCORES> "); */
00922 
00923     for (i = 0; i < PROTO_DEF_NR; i++) {
00924 
00925         /* if used looped score */
00926         if (ret[i] < score[i]) {
00927             temp = score[i] - ret[i]; /* remaining loop score */
00928 
00929             /* dbg("%3d - ",temp); */
00930 
00931             if (index[i] >= PROTO_DEF_AVG_NR)
00932                 index[i] = 0;   /* reset index */
00933 
00934             j = index[i];
00935             avg[i][j] = temp;
00936 
00937             index[i]++;
00938 
00939             if (ret[i] == 0 && ((score[i] * 2) <= PROTO_MAX_SCORE) && ((total + (score[i] * 2)) < max_total)) { /* used all loop score -> increase next score directly */
00940                 score[i] *= 2;
00941                 total += score[i];
00942                 continue;
00943             }
00944 
00945             sum = 0;
00946             for (j = 0; j < PROTO_DEF_AVG_NR; j++)
00947                 sum += avg[i][j]; /* calculate sum */
00948 
00949             sum /= 4;           /* divide by 4 to get average used score */
00950 
00951             /* criterion to increase next loop score */
00952             if (sum > (score[i] - (score[i] / 4))  && ((score[i] * 2) <= PROTO_MAX_SCORE) && ((total + (score[i] / 2)) < max_total)) { /* > 3/4 */
00953                 score[i] *= 2; /* double loop score */
00954                 total += score[i];
00955                 continue;
00956             }
00957 
00958             /* criterion to decrease next loop score */
00959             if ((sum < (score[i] / 4)) && ((score[i] / 2) >= PROTO_MIN_SCORE)) { /* < 1/4 */
00960                 score[i] /= 2; /* half loop score */
00961                 total += score[i];
00962                 continue;
00963             }
00964 
00965             /* also add non-changed scores */
00966             total += score[i];
00967         }
00968         else if (ret[i] == score[i]) {
00969             /* no used loop score - gradually decrease */
00970 
00971             /*  dbg("%3d - ",0); */
00972 
00973             if (index[i] >= PROTO_DEF_AVG_NR)
00974                 index[i] = 0;   /* reset index */
00975 
00976             j = index[i];
00977             avg[i][j] = 0;
00978 
00979             index[i]++;
00980 
00981             sum = 0;
00982             for (j = 0; j < PROTO_DEF_AVG_NR; j++)
00983                 sum += avg[i][j]; /* calculate sum */
00984 
00985             sum /= 2;          /* divide by 4 to get average used score */
00986 
00987             if ((sum == 0) && ((score[i] / 2) >= PROTO_MIN_SCORE)) {
00988                 score[i] /= 2; /* half loop score */
00989                 total += score[i];
00990                 for (j = 0; j < PROTO_DEF_AVG_NR; j++)
00991                     avg[i][j] = score[i];
00992             }
00993 
00994         }
00995     }
00996     /* dbg("\n"); */
00997 
00998     return 0;
00999 }
01000 
01001 void pico_stack_tick(void)
01002 {
01003     static int score[PROTO_DEF_NR] = {
01004         PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE
01005     };
01006     static int index[PROTO_DEF_NR] = {
01007         0, 0, 0, 0, 0, 0
01008     };
01009     static int avg[PROTO_DEF_NR][PROTO_DEF_AVG_NR];
01010     static int ret[PROTO_DEF_NR] = {
01011         0
01012     };
01013 
01014     pico_check_timers();
01015 
01016     /* dbg("LOOP_SCORES> %3d - %3d - %3d - %3d - %3d - %3d - %3d - %3d - %3d - %3d - %3d\n",score[0],score[1],score[2],score[3],score[4],score[5],score[6],score[7],score[8],score[9],score[10]); */
01017 
01018     /* score = pico_protocols_loop(100); */
01019 
01020     ret[0] = pico_devices_loop(score[0], PICO_LOOP_DIR_IN);
01021     pico_rand_feed((uint32_t)ret[0]);
01022 
01023     ret[1] = pico_protocol_datalink_loop(score[1], PICO_LOOP_DIR_IN);
01024     pico_rand_feed((uint32_t)ret[1]);
01025 
01026     ret[2] = pico_protocol_network_loop(score[2], PICO_LOOP_DIR_IN);
01027     pico_rand_feed((uint32_t)ret[2]);
01028 
01029     ret[3] = pico_protocol_transport_loop(score[3], PICO_LOOP_DIR_IN);
01030     pico_rand_feed((uint32_t)ret[3]);
01031 
01032 
01033     ret[5] = score[5];
01034 #if defined (PICO_SUPPORT_IPV4) || defined (PICO_SUPPORT_IPV6)
01035 #if defined (PICO_SUPPORT_TCP) || defined (PICO_SUPPORT_UDP)
01036     ret[5] = pico_sockets_loop(score[5]); /* swapped */
01037     pico_rand_feed((uint32_t)ret[5]);
01038 #endif
01039 #endif
01040 
01041     ret[4] = pico_protocol_socket_loop(score[4], PICO_LOOP_DIR_IN);
01042     pico_rand_feed((uint32_t)ret[4]);
01043 
01044 
01045     ret[6] = pico_protocol_socket_loop(score[6], PICO_LOOP_DIR_OUT);
01046     pico_rand_feed((uint32_t)ret[6]);
01047 
01048     ret[7] = pico_protocol_transport_loop(score[7], PICO_LOOP_DIR_OUT);
01049     pico_rand_feed((uint32_t)ret[7]);
01050 
01051     ret[8] = pico_protocol_network_loop(score[8], PICO_LOOP_DIR_OUT);
01052     pico_rand_feed((uint32_t)ret[8]);
01053 
01054     ret[9] = pico_protocol_datalink_loop(score[9], PICO_LOOP_DIR_OUT);
01055     pico_rand_feed((uint32_t)ret[9]);
01056 
01057     ret[10] = pico_devices_loop(score[10], PICO_LOOP_DIR_OUT);
01058     pico_rand_feed((uint32_t)ret[10]);
01059 
01060     /* calculate new loop scores for next iteration */
01061     calc_score(score, index, (int (*)[])avg, ret);
01062 }
01063 
01064 void pico_stack_loop(void)
01065 {
01066     while(1) {
01067         pico_stack_tick();
01068         PICO_IDLE();
01069     }
01070 }
01071 
01072 MOCKABLE struct pico_timer *pico_timer_add(pico_time expire, void (*timer)(pico_time, void *), void *arg)
01073 {
01074     struct pico_timer *t = PICO_ZALLOC(sizeof(struct pico_timer));
01075     struct pico_timer_ref tref;
01076     if (!t) {
01077         pico_err = PICO_ERR_ENOMEM;
01078         return NULL;
01079     }
01080 
01081     tref.expire = PICO_TIME_MS() + expire;
01082     t->arg = arg;
01083     t->timer = timer;
01084     tref.tmr = t;
01085     heap_insert(Timers, &tref);
01086     if (Timers->n > PICO_MAX_TIMERS) {
01087         dbg("Warning: I have %d timers\n", (int)Timers->n);
01088     }
01089 
01090     return t;
01091 }
01092 
01093 int pico_stack_init(void)
01094 {
01095 
01096 #ifdef PICO_SUPPORT_IPV4
01097     pico_protocol_init(&pico_proto_ipv4);
01098 #endif
01099 
01100 #ifdef PICO_SUPPORT_IPV6
01101     pico_protocol_init(&pico_proto_ipv6);
01102 #endif
01103 
01104 #ifdef PICO_SUPPORT_ICMP4
01105     pico_protocol_init(&pico_proto_icmp4);
01106 #endif
01107 
01108 #ifdef PICO_SUPPORT_ICMP6
01109     pico_protocol_init(&pico_proto_icmp6);
01110 #endif
01111 
01112 #if defined(PICO_SUPPORT_IGMP) && defined(PICO_SUPPORT_MCAST)
01113     pico_protocol_init(&pico_proto_igmp);
01114 #endif
01115 
01116 #ifdef PICO_SUPPORT_UDP
01117     pico_protocol_init(&pico_proto_udp);
01118 #endif
01119 
01120 #ifdef PICO_SUPPORT_TCP
01121     pico_protocol_init(&pico_proto_tcp);
01122 #endif
01123 
01124 #ifdef PICO_SUPPORT_DNS_CLIENT
01125     pico_dns_client_init();
01126 #endif
01127 
01128     pico_rand_feed(123456);
01129 
01130     /* Initialize timer heap */
01131     Timers = heap_init();
01132     if (!Timers)
01133         return -1;
01134 
01135 #if ((defined PICO_SUPPORT_IPV4) && (defined PICO_SUPPORT_ETH))
01136     /* Initialize ARP module */
01137     pico_arp_init();
01138 #endif
01139 
01140 #ifdef PICO_SUPPORT_IPV6
01141     /* Initialize Neighbor discovery module */
01142     pico_ipv6_nd_init();
01143 #endif
01144 
01145 #ifdef PICO_SUPPORT_OLSR
01146     pico_olsr_init();
01147 #endif
01148 #ifdef PICO_SUPPORT_AODV
01149     pico_aodv_init();
01150 #endif
01151 
01152     pico_stack_tick();
01153     pico_stack_tick();
01154     pico_stack_tick();
01155     return 0;
01156 }
01157