pico_ipv4.c

00001 /*********************************************************************
00002    PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved.
00003    See LICENSE and COPYING for usage.
00004 
00005    Authors: Daniele Lacamera, Markian Yskout
00006  *********************************************************************/
00007 
00008 
00009 #include "pico_config.h"
00010 #include "pico_ipfilter.h"
00011 #include "pico_ipv4.h"
00012 #include "pico_icmp4.h"
00013 #include "pico_stack.h"
00014 #include "pico_eth.h"
00015 #include "pico_udp.h"
00016 #include "pico_tcp.h"
00017 #include "pico_socket.h"
00018 #include "pico_device.h"
00019 #include "pico_nat.h"
00020 #include "pico_igmp.h"
00021 #include "pico_tree.h"
00022 #include "pico_aodv.h"
00023 #include "pico_socket_multicast.h"
00024 #include "pico_fragments.h"
00025 
00026 #ifdef PICO_SUPPORT_IPV4
00027 
00028 #ifdef PICO_SUPPORT_MCAST
00029 # define ip_mcast_dbg(...) do {} while(0) /* so_mcast_dbg in pico_socket.c */
00030 /* #define ip_mcast_dbg dbg */
00031 # define PICO_MCAST_ALL_HOSTS long_be(0xE0000001) /* 224.0.0.1 */
00032 /* Default network interface for multicast transmission */
00033 static struct pico_ipv4_link *mcast_default_link = NULL;
00034 #endif
00035 #ifdef PICO_SUPPORT_IPV4FRAG
00036 /* # define reassembly_dbg dbg */
00037 # define reassembly_dbg(...) do {} while(0)
00038 #endif
00039 
00040 /* Queues */
00041 static struct pico_queue in = {
00042     0
00043 };
00044 static struct pico_queue out = {
00045     0
00046 };
00047 
00048 /* Functions */
00049 static int ipv4_route_compare(void *ka, void *kb);
00050 static struct pico_frame *pico_ipv4_alloc(struct pico_protocol *self, uint16_t size);
00051 
00052 
00053 int pico_ipv4_compare(struct pico_ip4 *a, struct pico_ip4 *b)
00054 {
00055     if (a->addr < b->addr)
00056         return -1;
00057 
00058     if (a->addr > b->addr)
00059         return 1;
00060 
00061     return 0;
00062 }
00063 
00064 int pico_ipv4_to_string(char *ipbuf, const uint32_t ip)
00065 {
00066     const unsigned char *addr = (const unsigned char *) &ip;
00067     int i;
00068 
00069     if (!ipbuf) {
00070         pico_err = PICO_ERR_EINVAL;
00071         return -1;
00072     }
00073 
00074     for(i = 0; i < 4; i++)
00075     {
00076         if (addr[i] > 99) {
00077             *ipbuf++ = (char)('0' + (addr[i] / 100));
00078             *ipbuf++ = (char)('0' + ((addr[i] % 100) / 10));
00079             *ipbuf++ = (char)('0' + ((addr[i] % 100) % 10));
00080         } else if (addr[i] > 9) {
00081             *ipbuf++ = (char)('0' + (addr[i] / 10));
00082             *ipbuf++ = (char)('0' + (addr[i] % 10));
00083         } else {
00084             *ipbuf++ = (char)('0' + addr[i]);
00085         }
00086 
00087         if (i < 3)
00088             *ipbuf++ = '.';
00089     }
00090     *ipbuf = '\0';
00091 
00092     return 0;
00093 }
00094 
00095 static int pico_string_check_null_args(const char *ipstr, uint32_t *ip)
00096 {
00097 
00098     if (!ipstr || !ip) {
00099         pico_err = PICO_ERR_EINVAL;
00100         return -1;
00101     }
00102 
00103     return 0;
00104 
00105 }
00106 
00107 int pico_string_to_ipv4(const char *ipstr, uint32_t *ip)
00108 {
00109     unsigned char buf[PICO_SIZE_IP4] = {
00110         0
00111     };
00112     int cnt = 0;
00113     char p;
00114 
00115     if (pico_string_check_null_args(ipstr, ip) < 0)
00116         return -1;
00117 
00118     while((p = *ipstr++) != 0 && cnt < PICO_SIZE_IP4)
00119     {
00120         if (pico_is_digit(p)) {
00121             buf[cnt] = (uint8_t)((10 * buf[cnt]) + (p - '0'));
00122         } else if (p == '.') {
00123             cnt++;
00124         } else {
00125             return -1;
00126         }
00127     }
00128     /* Handle short notation */
00129     if (cnt == 1) {
00130         buf[3] = buf[1];
00131         buf[1] = 0;
00132         buf[2] = 0;
00133     } else if (cnt == 2) {
00134         buf[3] = buf[2];
00135         buf[2] = 0;
00136     } else if (cnt != 3) {
00137         /* String could not be parsed, return error */
00138         return -1;
00139     }
00140 
00141     *ip = long_from(buf);
00142 
00143     return 0;
00144 }
00145 
00146 int pico_ipv4_valid_netmask(uint32_t mask)
00147 {
00148     int cnt = 0;
00149     int end = 0;
00150     int i;
00151     uint32_t mask_swap = long_be(mask);
00152 
00153     /*
00154      * Swap bytes for convenient parsing
00155      * e.g. 0x..f8ff will become 0xfff8..
00156      * Then, we count the consecutive bits
00157      *
00158      * */
00159 
00160     for(i = 0; i < 32; i++) {
00161         if ((mask_swap << i) & 0x80000000) {
00162             if (end) {
00163                 pico_err = PICO_ERR_EINVAL;
00164                 return -1;
00165             }
00166 
00167             cnt++;
00168         } else {
00169             end = 1;
00170         }
00171     }
00172     return cnt;
00173 }
00174 
00175 int pico_ipv4_is_unicast(uint32_t address)
00176 {
00177     const unsigned char *addr = (unsigned char *) &address;
00178     if ((addr[0] & 0xe0) == 0xe0)
00179         return 0; /* multicast */
00180 
00181     return 1;
00182 }
00183 
00184 int pico_ipv4_is_multicast(uint32_t address)
00185 {
00186     const unsigned char *addr = (unsigned char *) &address;
00187     if ((addr[0] != 0xff) && ((addr[0] & 0xe0) == 0xe0))
00188         return 1; /* multicast */
00189 
00190     return 0;
00191 }
00192 
00193 int pico_ipv4_is_loopback(uint32_t address)
00194 {
00195     const unsigned char *addr = (unsigned char *) &address;
00196     if (addr[0] == 0x7f)
00197         return 1;
00198 
00199     return 0;
00200 }
00201 
00202 static int pico_ipv4_is_invalid_loopback(uint32_t address, struct pico_device *dev)
00203 {
00204     return pico_ipv4_is_loopback(address) && ((!dev) || strcmp(dev->name, "loop"));
00205 }
00206 
00207 int pico_ipv4_is_valid_src(uint32_t address, struct pico_device *dev)
00208 {
00209     if (pico_ipv4_is_broadcast(address)) {
00210         dbg("Source is a broadcast address, discard packet\n");
00211         return 0;
00212     } else if ( pico_ipv4_is_multicast(address)) {
00213         dbg("Source is a multicast address, discard packet\n");
00214         return 0;
00215     } else if (pico_ipv4_is_invalid_loopback(address, dev)) {
00216         dbg("Source is a loopback address, discard packet\n");
00217         return 0;
00218     } else {
00219 #ifdef PICO_SUPPORT_AODV
00220         union pico_address src;
00221         src.ip4.addr = address;
00222         pico_aodv_refresh(&src);
00223 #endif
00224         return 1;
00225     }
00226 }
00227 
00228 static int pico_ipv4_checksum(struct pico_frame *f)
00229 {
00230     struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
00231     if (!hdr)
00232         return -1;
00233 
00234     hdr->crc = 0;
00235     hdr->crc = short_be(pico_checksum(hdr, f->net_len));
00236     return 0;
00237 }
00238 
00239 
00240 #ifdef PICO_SUPPORT_CRC
00241 static inline int pico_ipv4_crc_check(struct pico_frame *f)
00242 {
00243     uint16_t checksum_invalid = 1;
00244     struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
00245 
00246     checksum_invalid = short_be(pico_checksum(hdr, f->net_len));
00247     if (checksum_invalid) {
00248         dbg("IP: checksum failed!\n");
00249         pico_frame_discard(f);
00250         return 0;
00251     }
00252 
00253     return 1;
00254 }
00255 #else
00256 static inline int pico_ipv4_crc_check(struct pico_frame *f)
00257 {
00258     IGNORE_PARAMETER(f);
00259     return 1;
00260 }
00261 #endif /* PICO_SUPPORT_CRC */
00262 
00263 static int pico_ipv4_forward(struct pico_frame *f);
00264 #ifdef PICO_SUPPORT_MCAST
00265 static int pico_ipv4_mcast_filter(struct pico_frame *f);
00266 #endif
00267 
00268 static int ipv4_link_compare(void *ka, void *kb)
00269 {
00270     struct pico_ipv4_link *a = ka, *b = kb;
00271     int cmp = pico_ipv4_compare(&a->address, &b->address);
00272     if (cmp)
00273         return cmp;
00274 
00275     /* zero can be assigned multiple times (e.g. for DHCP) */
00276     if (a->dev != NULL && b->dev != NULL && a->address.addr == PICO_IP4_ANY && b->address.addr == PICO_IP4_ANY) {
00277         if (a->dev < b->dev)
00278             return -1;
00279 
00280         if (a->dev > b->dev)
00281             return 1;
00282     }
00283 
00284     return 0;
00285 }
00286 
00287 PICO_TREE_DECLARE(Tree_dev_link, ipv4_link_compare);
00288 
00289 static int pico_ipv4_process_bcast_in(struct pico_frame *f)
00290 {
00291     struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
00292 #ifdef PICO_SUPPORT_UDP
00293     if (pico_ipv4_is_broadcast(hdr->dst.addr) && (hdr->proto == PICO_PROTO_UDP)) {
00294         /* Receiving UDP broadcast datagram */
00295         f->flags |= PICO_FRAME_FLAG_BCAST;
00296         pico_enqueue(pico_proto_udp.q_in, f);
00297         return 1;
00298     }
00299 
00300 #endif
00301 
00302 #ifdef PICO_SUPPORT_ICMP4
00303     if (pico_ipv4_is_broadcast(hdr->dst.addr) && (hdr->proto == PICO_PROTO_ICMP4)) {
00304         /* Receiving ICMP4 bcast packet */
00305         f->flags |= PICO_FRAME_FLAG_BCAST;
00306         pico_enqueue(pico_proto_icmp4.q_in, f);
00307         return 1;
00308     }
00309 
00310 #endif
00311     return 0;
00312 }
00313 
00314 static int pico_ipv4_process_mcast_in(struct pico_frame *f)
00315 {
00316     struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
00317     if (pico_ipv4_is_multicast(hdr->dst.addr)) {
00318 #ifdef PICO_SUPPORT_IGMP
00319         /* Receiving UDP multicast datagram TODO set f->flags? */
00320         if (hdr->proto == PICO_PROTO_IGMP) {
00321             ip_mcast_dbg("MCAST: received IGMP message\n");
00322             pico_transport_receive(f, PICO_PROTO_IGMP);
00323             return 1;
00324         } else if ((pico_ipv4_mcast_filter(f) == 0) && (hdr->proto == PICO_PROTO_UDP)) {
00325             pico_enqueue(pico_proto_udp.q_in, f);
00326             return 1;
00327         }
00328 
00329 #endif
00330         pico_frame_discard(f);
00331         return 1;
00332     }
00333 
00334     return 0;
00335 }
00336 
00337 static int pico_ipv4_process_local_unicast_in(struct pico_frame *f)
00338 {
00339     struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
00340     struct pico_ipv4_link test = {
00341         .address = {.addr = PICO_IP4_ANY}, .dev = NULL
00342     };
00343     if (pico_ipv4_link_find(&hdr->dst)) {
00344         if (pico_ipv4_nat_inbound(f, &hdr->dst) == 0)
00345             pico_enqueue(pico_proto_ipv4.q_in, f); /* dst changed, reprocess */
00346         else
00347             pico_transport_receive(f, hdr->proto);
00348 
00349         return 1;
00350     } else if (pico_tree_findKey(&Tree_dev_link, &test)) {
00351 #ifdef PICO_SUPPORT_UDP
00352         /* address of this device is apparently 0.0.0.0; might be a DHCP packet */
00353         /* XXX KRO: is obsolete. Broadcast flag is set on outgoing DHCP messages.
00354          * incomming DHCP messages are to be broadcasted. Our current DHCP server
00355          * implementation does not take this flag into account yet though ... */
00356         pico_enqueue(pico_proto_udp.q_in, f);
00357         return 1;
00358 #endif
00359     }
00360 
00361     return 0;
00362 }
00363 
00364 static void pico_ipv4_process_finally_try_forward(struct pico_frame *f)
00365 {
00366     struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
00367     if ((pico_ipv4_is_broadcast(hdr->dst.addr)) || ((f->flags & PICO_FRAME_FLAG_BCAST)!= 0)) {
00368         /* don't forward broadcast frame, discard! */
00369         pico_frame_discard(f);
00370     } else if (pico_ipv4_forward(f) != 0) {
00371         pico_frame_discard(f);
00372         /* dbg("Forward failed.\n"); */
00373     }
00374 }
00375 
00376 
00377 
00378 static int pico_ipv4_process_in(struct pico_protocol *self, struct pico_frame *f)
00379 {
00380     uint8_t option_len = 0;
00381     int ret = 0;
00382     struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
00383     uint16_t max_allowed = (uint16_t) ((int)f->buffer_len - (f->net_hdr - f->buffer) - (int)PICO_SIZE_IP4HDR);
00384     uint16_t flag = short_be(hdr->frag);
00385 
00386     (void)self;
00387     /* NAT needs transport header information */
00388     if (((hdr->vhl) & 0x0F) > 5) {
00389         option_len =  (uint8_t)(4 * (((hdr->vhl) & 0x0F) - 5));
00390     }
00391 
00392     f->transport_hdr = ((uint8_t *)f->net_hdr) + PICO_SIZE_IP4HDR + option_len;
00393     f->transport_len = (uint16_t)(short_be(hdr->len) - PICO_SIZE_IP4HDR - option_len);
00394     f->net_len = (uint16_t)(PICO_SIZE_IP4HDR + option_len);
00395 
00396     if (f->transport_len > max_allowed) {
00397         pico_frame_discard(f);
00398         return 0; /* Packet is discarded due to unfeasible length */
00399     }
00400 
00401 #ifdef PICO_SUPPORT_IPFILTER
00402     if (ipfilter(f)) {
00403         /*pico_frame is discarded as result of the filtering*/
00404         return 0;
00405     }
00406 
00407 #endif
00408 
00409 
00410     /* ret == 1 indicates to continue the function */
00411     ret = pico_ipv4_crc_check(f);
00412     if (ret < 1)
00413         return ret;
00414 
00415     /* Validate source IP address. Discard quietly if invalid */
00416     if (!pico_ipv4_is_valid_src(hdr->src.addr, f->dev)) {
00417         pico_frame_discard(f);
00418         return 0;
00419     }
00420 
00421     if (hdr->frag & short_be(PICO_IPV4_EVIL)) {
00422         (void)pico_icmp4_param_problem(f, 0);
00423         pico_frame_discard(f); /* RFC 3514 */
00424         return 0;
00425     }
00426 
00427     if ((hdr->vhl & 0x0f) < 5) {
00428         /* RFC 791: IHL minimum value is 5 */
00429         (void)pico_icmp4_param_problem(f, 0);
00430         pico_frame_discard(f);
00431         return 0;
00432     }
00433 
00434     if (flag & (PICO_IPV4_MOREFRAG | PICO_IPV4_FRAG_MASK))
00435     {
00436 #ifdef PICO_SUPPORT_IPV4FRAG
00437         pico_ipv4_process_frag(hdr, f, hdr ? hdr->proto : 0 );
00438         /* Frame can be discarded, frag will handle its own copy */
00439 #endif
00440         /* We do not support fragmentation, discard quietly */
00441         pico_frame_discard(f);
00442         return 0;
00443     }
00444 
00445     if (pico_ipv4_process_bcast_in(f) > 0)
00446         return 0;
00447 
00448     if (pico_ipv4_process_mcast_in(f) > 0)
00449         return 0;
00450 
00451     if (pico_ipv4_process_local_unicast_in(f) > 0)
00452         return 0;
00453 
00454     pico_ipv4_process_finally_try_forward(f);
00455 
00456     return 0;
00457 }
00458 
00459 PICO_TREE_DECLARE(Routes, ipv4_route_compare);
00460 
00461 
00462 static int pico_ipv4_process_out(struct pico_protocol *self, struct pico_frame *f)
00463 {
00464     IGNORE_PARAMETER(self);
00465     f->start = (uint8_t*) f->net_hdr;
00466 #ifdef PICO_SUPPORT_IPFILTER
00467     if (ipfilter(f)) {
00468         /*pico_frame is discarded as result of the filtering*/
00469         return 0;
00470     }
00471 
00472 #endif
00473     return pico_sendto_dev(f);
00474 }
00475 
00476 
00477 static struct pico_frame *pico_ipv4_alloc(struct pico_protocol *self, uint16_t size)
00478 {
00479     struct pico_frame *f =  pico_frame_alloc(size + PICO_SIZE_IP4HDR + PICO_SIZE_ETHHDR);
00480     IGNORE_PARAMETER(self);
00481 
00482     if (!f)
00483         return NULL;
00484 
00485     f->datalink_hdr = f->buffer;
00486     f->net_hdr = f->buffer + PICO_SIZE_ETHHDR;
00487     f->net_len = PICO_SIZE_IP4HDR;
00488     f->transport_hdr = f->net_hdr + PICO_SIZE_IP4HDR;
00489     f->transport_len = size;
00490     f->len =  size + PICO_SIZE_IP4HDR;
00491     return f;
00492 }
00493 
00494 static int pico_ipv4_frame_sock_push(struct pico_protocol *self, struct pico_frame *f);
00495 
00496 /* Interface: protocol definition */
00497 struct pico_protocol pico_proto_ipv4 = {
00498     .name = "ipv4",
00499     .proto_number = PICO_PROTO_IPV4,
00500     .layer = PICO_LAYER_NETWORK,
00501     .alloc = pico_ipv4_alloc,
00502     .process_in = pico_ipv4_process_in,
00503     .process_out = pico_ipv4_process_out,
00504     .push = pico_ipv4_frame_sock_push,
00505     .q_in = &in,
00506     .q_out = &out,
00507 };
00508 
00509 
00510 static int ipv4_route_compare(void *ka, void *kb)
00511 {
00512     struct pico_ipv4_route *a = ka, *b = kb;
00513     uint32_t a_nm, b_nm;
00514     int cmp;
00515 
00516     a_nm = long_be(a->netmask.addr);
00517     b_nm = long_be(b->netmask.addr);
00518 
00519     /* Routes are sorted by (host side) netmask len, then by addr, then by metric. */
00520     if (a_nm < b_nm)
00521         return -1;
00522 
00523     if (b_nm < a_nm)
00524         return 1;
00525 
00526     cmp = pico_ipv4_compare(&a->dest, &b->dest);
00527     if (cmp)
00528         return cmp;
00529 
00530     if (a->metric < b->metric)
00531         return -1;
00532 
00533     if (a->metric > b->metric)
00534         return 1;
00535 
00536     return 0;
00537 }
00538 
00539 
00540 static struct pico_ipv4_route default_bcast_route = {
00541     .dest = {PICO_IP4_BCAST},
00542     .netmask = {PICO_IP4_BCAST},
00543     .gateway  = { 0 },
00544     .link = NULL,
00545     .metric = 1000
00546 };
00547 
00548 static struct pico_ipv4_route *route_find_default_bcast(void)
00549 {
00550     return &default_bcast_route;
00551 }
00552 
00553 
00554 static struct pico_ipv4_route *route_find(const struct pico_ip4 *addr)
00555 {
00556     struct pico_ipv4_route *r;
00557     struct pico_tree_node *index;
00558 
00559     if (addr->addr != PICO_IP4_BCAST) {
00560         pico_tree_foreach_reverse(index, &Routes) {
00561             r = index->keyValue;
00562             if ((addr->addr & (r->netmask.addr)) == (r->dest.addr)) {
00563                 return r;
00564             }
00565         }
00566         return NULL;
00567     }
00568 
00569     return route_find_default_bcast();
00570 }
00571 
00572 struct pico_ip4 pico_ipv4_route_get_gateway(struct pico_ip4 *addr)
00573 {
00574     struct pico_ip4 nullip;
00575     struct pico_ipv4_route *route;
00576     nullip.addr = 0U;
00577 
00578     if (!addr) {
00579         pico_err = PICO_ERR_EINVAL;
00580         return nullip;
00581     }
00582 
00583     route = route_find(addr);
00584     if (!route) {
00585         pico_err = PICO_ERR_EHOSTUNREACH;
00586         return nullip;
00587     }
00588     else
00589         return route->gateway;
00590 }
00591 
00592 struct pico_ip4 *pico_ipv4_source_find(const struct pico_ip4 *dst)
00593 {
00594     struct pico_ip4 *myself = NULL;
00595     struct pico_ipv4_route *rt;
00596 #ifdef PICO_SUPPORT_AODV
00597     union pico_address node_address;
00598     node_address.ip4.addr = dst->addr;
00599     if (dst->addr && pico_ipv4_is_unicast(dst->addr))
00600         pico_aodv_lookup(&node_address);
00601 
00602 #endif
00603 
00604     if (!dst) {
00605         pico_err = PICO_ERR_EINVAL;
00606         return NULL;
00607     }
00608 
00609     rt = route_find(dst);
00610     if (rt && rt->link) {
00611         myself = &rt->link->address;
00612     } else {
00613         pico_err = PICO_ERR_EHOSTUNREACH;
00614     }
00615 
00616     return myself;
00617 }
00618 
00619 struct pico_device *pico_ipv4_source_dev_find(const struct pico_ip4 *dst)
00620 {
00621     struct pico_device *dev = NULL;
00622     struct pico_ipv4_route *rt;
00623 
00624     if (!dst) {
00625         pico_err = PICO_ERR_EINVAL;
00626         return NULL;
00627     }
00628 
00629     rt = route_find(dst);
00630     if (rt && rt->link) {
00631         dev = rt->link->dev;
00632     } else {
00633         pico_err = PICO_ERR_EHOSTUNREACH;
00634     }
00635 
00636     return dev;
00637 }
00638 
00639 
00640 #ifdef PICO_SUPPORT_MCAST
00641 /*                        link
00642  *                         |
00643  *                    MCASTGroups
00644  *                    |    |     |
00645  *         ------------    |     ------------
00646  *         |               |                |
00647  *   MCASTSources    MCASTSources     MCASTSources
00648  *   |  |  |  |      |  |  |  |       |  |  |  |
00649  *   S  S  S  S      S  S  S  S       S  S  S  S
00650  *
00651  *   MCASTGroups: RBTree(mcast_group)
00652  *   MCASTSources: RBTree(source)
00653  */
00654 static int ipv4_mcast_groups_cmp(void *ka, void *kb)
00655 {
00656     struct pico_mcast_group *a = ka, *b = kb;
00657     return pico_ipv4_compare(&a->mcast_addr, &b->mcast_addr);
00658 }
00659 
00660 static int ipv4_mcast_sources_cmp(void *ka, void *kb)
00661 {
00662     struct pico_ip4 *a = ka, *b = kb;
00663     return pico_ipv4_compare(a, b);
00664 }
00665 
00666 static void pico_ipv4_mcast_print_groups(struct pico_ipv4_link *mcast_link)
00667 {
00668     uint16_t i = 0;
00669     struct pico_mcast_group *g = NULL;
00670     struct pico_ip4 *source = NULL;
00671     struct pico_tree_node *index = NULL, *index2 = NULL;
00672     (void) source;
00673 
00674     ip_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
00675     ip_mcast_dbg("+                           MULTICAST list interface %-16s             +\n", mcast_link->dev->name);
00676     ip_mcast_dbg("+---------------------------------------------------------------------------------+\n");
00677     ip_mcast_dbg("+  nr  |    interface     | host group | reference count | filter mode |  source  +\n");
00678     ip_mcast_dbg("+---------------------------------------------------------------------------------+\n");
00679 
00680     pico_tree_foreach(index, mcast_link->MCASTGroups) {
00681         g = index->keyValue;
00682         ip_mcast_dbg("+ %04d | %16s |  %08X  |      %05u      |      %u      | %8s +\n", i, mcast_link->dev->name, g->mcast_addr.addr, g->reference_count, g->filter_mode, "");
00683         pico_tree_foreach(index2, &g->MCASTSources) {
00684             source = index2->keyValue;
00685             ip_mcast_dbg("+ %4s | %16s |  %8s  |      %5s      |      %s      | %08X +\n", "", "", "", "", "", source->addr);
00686         }
00687         i++;
00688     }
00689     ip_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
00690 }
00691 
00692 static int mcast_group_update(struct pico_mcast_group *g, struct pico_tree *MCASTFilter, uint8_t filter_mode)
00693 {
00694     struct pico_tree_node *index = NULL, *_tmp = NULL;
00695     struct pico_ip4 *source = NULL;
00696     /* cleanup filter */
00697     pico_tree_foreach_safe(index, &g->MCASTSources, _tmp) {
00698         source = index->keyValue;
00699         pico_tree_delete(&g->MCASTSources, source);
00700         PICO_FREE(source);
00701     }
00702     /* insert new filter */
00703     if (MCASTFilter) {
00704         pico_tree_foreach(index, MCASTFilter) {
00705             if (index->keyValue) {
00706                 source = PICO_ZALLOC(sizeof(struct pico_ip4));
00707                 if (!source) {
00708                     pico_err = PICO_ERR_ENOMEM;
00709                     return -1;
00710                 }
00711 
00712                 source->addr = ((struct pico_ip4 *)index->keyValue)->addr;
00713                 pico_tree_insert(&g->MCASTSources, source);
00714             }
00715         }
00716     }
00717 
00718     g->filter_mode = filter_mode;
00719     return 0;
00720 }
00721 
00722 int pico_ipv4_mcast_join(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter)
00723 {
00724     struct pico_mcast_group *g = NULL, test = {
00725         0
00726     };
00727     struct pico_ipv4_link *link = NULL;
00728 
00729     if (mcast_link)
00730         link = pico_ipv4_link_get(mcast_link);
00731 
00732     if (!link)
00733         link = mcast_default_link;
00734 
00735     test.mcast_addr = *mcast_group;
00736     g = pico_tree_findKey(link->MCASTGroups, &test);
00737     if (g) {
00738         if (reference_count)
00739             g->reference_count++;
00740 
00741         pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_UPDATE);
00742     } else {
00743         g = PICO_ZALLOC(sizeof(struct pico_mcast_group));
00744         if (!g) {
00745             pico_err = PICO_ERR_ENOMEM;
00746             return -1;
00747         }
00748 
00749         /* "non-existent" state of filter mode INCLUDE and empty source list */
00750         g->filter_mode = PICO_IP_MULTICAST_INCLUDE;
00751         g->reference_count = 1;
00752         g->mcast_addr = *mcast_group;
00753         g->MCASTSources.root = &LEAF;
00754         g->MCASTSources.compare = ipv4_mcast_sources_cmp;
00755         pico_tree_insert(link->MCASTGroups, g);
00756         pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_CREATE);
00757     }
00758 
00759     if (mcast_group_update(g, MCASTFilter, filter_mode) < 0) {
00760         dbg("Error in mcast_group update\n");
00761         return -1;
00762     }
00763 
00764     pico_ipv4_mcast_print_groups(link);
00765     return 0;
00766 }
00767 
00768 int pico_ipv4_mcast_leave(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter)
00769 {
00770 
00771     struct pico_mcast_group *g = NULL, test = {
00772         0
00773     };
00774     struct pico_ipv4_link *link = NULL;
00775     struct pico_tree_node *index = NULL, *_tmp = NULL;
00776     struct pico_ip4 *source = NULL;
00777 
00778     if (mcast_link)
00779         link = pico_ipv4_link_get(mcast_link);
00780 
00781     if (!link)
00782         link = mcast_default_link;
00783 
00784     test.mcast_addr = *mcast_group;
00785     g = pico_tree_findKey(link->MCASTGroups, &test);
00786     if (!g) {
00787         pico_err = PICO_ERR_EINVAL;
00788         return -1;
00789     } else {
00790         if (reference_count && (--(g->reference_count) < 1)) {
00791             pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_DELETE);
00792             /* cleanup filter */
00793             pico_tree_foreach_safe(index, &g->MCASTSources, _tmp) {
00794                 source = index->keyValue;
00795                 pico_tree_delete(&g->MCASTSources, source);
00796                 PICO_FREE(source);
00797             }
00798             pico_tree_delete(link->MCASTGroups, g);
00799             PICO_FREE(g);
00800         } else {
00801             pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_UPDATE);
00802             if (mcast_group_update(g, MCASTFilter, filter_mode) < 0)
00803                 return -1;
00804         }
00805     }
00806 
00807     pico_ipv4_mcast_print_groups(link);
00808     return 0;
00809 }
00810 
00811 struct pico_ipv4_link *pico_ipv4_get_default_mcastlink(void)
00812 {
00813     return mcast_default_link;
00814 }
00815 
00816 static int pico_ipv4_mcast_filter(struct pico_frame *f)
00817 {
00818     struct pico_ipv4_link *link = NULL;
00819     struct pico_tree_node *index = NULL, *index2 = NULL;
00820     struct pico_mcast_group *g = NULL, test = {
00821         0
00822     };
00823     struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
00824 
00825     test.mcast_addr = hdr->dst;
00826 
00827     pico_tree_foreach(index, &Tree_dev_link) {
00828         link = index->keyValue;
00829         g = pico_tree_findKey(link->MCASTGroups, &test);
00830         if (g) {
00831             if (f->dev == link->dev) {
00832                 ip_mcast_dbg("MCAST: IP %08X is group member of current link %s\n", hdr->dst.addr, f->dev->name);
00833                 /* perform source filtering */
00834                 switch (g->filter_mode) {
00835                 case PICO_IP_MULTICAST_INCLUDE:
00836                     pico_tree_foreach(index2, &g->MCASTSources) {
00837                         if (hdr->src.addr == ((struct pico_ip4 *)index2->keyValue)->addr) {
00838                             ip_mcast_dbg("MCAST: IP %08X in included interface source list\n", hdr->src.addr);
00839                             return 0;
00840                         }
00841                     }
00842                     ip_mcast_dbg("MCAST: IP %08X NOT in included interface source list\n", hdr->src.addr);
00843                     return -1;
00844 
00845                 case PICO_IP_MULTICAST_EXCLUDE:
00846                     pico_tree_foreach(index2, &g->MCASTSources) {
00847                         if (hdr->src.addr == ((struct pico_ip4 *)index2->keyValue)->addr) {
00848                             ip_mcast_dbg("MCAST: IP %08X in excluded interface source list\n", hdr->src.addr);
00849                             return -1;
00850                         }
00851                     }
00852                     ip_mcast_dbg("MCAST: IP %08X NOT in excluded interface source list\n", hdr->src.addr);
00853                     return 0;
00854 
00855                 default:
00856                     return -1;
00857                 }
00858             } else {
00859                 ip_mcast_dbg("MCAST: IP %08X is group member of different link %s\n", hdr->dst.addr, link->dev->name);
00860             }
00861         } else {
00862             ip_mcast_dbg("MCAST: IP %08X is not a group member of link %s\n", hdr->dst.addr, f->dev->name);
00863         }
00864     }
00865     return -1;
00866 }
00867 
00868 #else
00869 
00870 int pico_ipv4_mcast_join(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter)
00871 {
00872     pico_err = PICO_ERR_EPROTONOSUPPORT;
00873     return -1;
00874 }
00875 
00876 int pico_ipv4_mcast_leave(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter)
00877 {
00878     pico_err = PICO_ERR_EPROTONOSUPPORT;
00879     return -1;
00880 }
00881 
00882 struct pico_ipv4_link *pico_ipv4_get_default_mcastlink(void)
00883 {
00884     pico_err = PICO_ERR_EPROTONOSUPPORT;
00885     return NULL;
00886 }
00887 #endif /* PICO_SUPPORT_MCAST */
00888 
00889 /* #define DEBUG_ROUTE */
00890 #ifdef DEBUG_ROUTE
00891 void dbg_route(void)
00892 {
00893     struct pico_ipv4_route *r;
00894     struct pico_tree_node *index;
00895     int count_hosts = 0;
00896     dbg("==== ROUTING TABLE =====\n");
00897     pico_tree_foreach(index, &Routes) {
00898         r = index->keyValue;
00899         dbg("Route to %08x/%08x, gw %08x, dev: %s, metric: %d\n", r->dest.addr, r->netmask.addr, r->gateway.addr, r->link->dev->name, r->metric);
00900         if (r->netmask.addr == 0xFFFFFFFF)
00901             count_hosts++;
00902     }
00903     dbg("================ total HOST nodes: %d ======\n\n\n", count_hosts);
00904 }
00905 #else
00906 #define dbg_route() do { } while(0)
00907 #endif
00908 
00909 int pico_ipv4_frame_push(struct pico_frame *f, struct pico_ip4 *dst, uint8_t proto)
00910 {
00911 
00912     struct pico_ipv4_route *route;
00913     struct pico_ipv4_link *link;
00914     struct pico_ipv4_hdr *hdr;
00915     uint8_t ttl = PICO_IPV4_DEFAULT_TTL;
00916     uint8_t vhl = 0x45; /* version 4, header length 20 */
00917     static uint16_t ipv4_progressive_id = 0x91c0;
00918 #ifdef PICO_SUPPORT_MCAST
00919     struct pico_tree_node *index;
00920 #endif
00921 
00922     if (!f || !dst) {
00923         pico_err = PICO_ERR_EINVAL;
00924         return -1;
00925     }
00926 
00927 
00928     hdr = (struct pico_ipv4_hdr *) f->net_hdr;
00929     if (!hdr) {
00930         dbg("IP header error\n");
00931         pico_err = PICO_ERR_EINVAL;
00932         goto drop;
00933     }
00934 
00935     if (dst->addr == 0) {
00936         dbg("IP destination addr error\n");
00937         pico_err = PICO_ERR_EINVAL;
00938         goto drop;
00939     }
00940 
00941     route = route_find(dst);
00942     if (!route) {
00943         /* dbg("Route to %08x not found.\n", long_be(dst->addr)); */
00944 
00945 
00946         pico_err = PICO_ERR_EHOSTUNREACH;
00947         goto drop;
00948     } else {
00949         link = route->link;
00950 #ifdef PICO_SUPPORT_MCAST
00951         if (pico_ipv4_is_multicast(dst->addr)) { /* if multicast */
00952             switch (proto) {
00953             case PICO_PROTO_UDP:
00954                 if (pico_udp_get_mc_ttl(f->sock, &ttl) < 0)
00955                     ttl = PICO_IP_DEFAULT_MULTICAST_TTL;
00956 
00957                 break;
00958 #ifdef PICO_SUPPORT_IGMP
00959             case PICO_PROTO_IGMP:
00960                 vhl = 0x46; /* header length 24 */
00961                 ttl = 1;
00962                 /* router alert (RFC 2113) */
00963                 hdr->options[0] = 0x94;
00964                 hdr->options[1] = 0x04;
00965                 hdr->options[2] = 0x00;
00966                 hdr->options[3] = 0x00;
00967                 if (f->dev && link->dev != f->dev) { /* default link is not requested link */
00968                     pico_tree_foreach(index, &Tree_dev_link) {
00969                         link = index->keyValue;
00970                         if (link->dev == f->dev)
00971                             break;
00972                     }
00973                 }
00974 
00975                 break;
00976 #endif
00977             default:
00978                 ttl = PICO_IPV4_DEFAULT_TTL;
00979             }
00980         }
00981 
00982 #endif
00983     }
00984 
00985     hdr->vhl = vhl;
00986     hdr->len = short_be((uint16_t)(f->transport_len + f->net_len));
00987     if ((f->transport_hdr != f->payload)  &&
00988 #ifdef PICO_SUPPORT_IPV4FRAG
00989         ( (0 == (f->frag & PICO_IPV4_MOREFRAG)) ||
00990           (0 == (f->frag & PICO_IPV4_FRAG_MASK)) )
00991         &&
00992 #endif
00993         1 )
00994         ipv4_progressive_id++;
00995 
00996     if (f->send_ttl > 0) {
00997         ttl = f->send_ttl;
00998     }
00999 
01000     hdr->id = short_be(ipv4_progressive_id);
01001     hdr->dst.addr = dst->addr;
01002     hdr->src.addr = link->address.addr;
01003     hdr->ttl = ttl;
01004     hdr->tos = f->send_tos;
01005     hdr->proto = proto;
01006     hdr->frag = short_be(PICO_IPV4_DONTFRAG);
01007 
01008 #ifdef PICO_SUPPORT_IPV4FRAG
01009 #  ifdef PICO_SUPPORT_UDP
01010     if (proto == PICO_PROTO_UDP) {
01011         /* first fragment, can not use transport_len to calculate IP length */
01012         if (f->transport_hdr != f->payload)
01013             hdr->len = short_be((uint16_t)(f->payload_len + sizeof(struct pico_udp_hdr) + f->net_len));
01014 
01015         /* set fragmentation flags and offset calculated in socket layer */
01016         hdr->frag = short_be(f->frag);
01017     }
01018 
01019     if (proto == PICO_PROTO_ICMP4)
01020     {
01021         hdr->frag = short_be(f->frag);
01022     }
01023 
01024 #   endif
01025 #endif /* PICO_SUPPORT_IPV4FRAG */
01026     pico_ipv4_checksum(f);
01027 
01028     if (f->sock && f->sock->dev) {
01029         /* if the socket has its device set, use that (currently used for DHCP) */
01030         f->dev = f->sock->dev;
01031     } else {
01032         f->dev = link->dev;
01033         if (f->sock)
01034             f->sock->dev = f->dev;
01035     }
01036 
01037 #ifdef PICO_SUPPORT_MCAST
01038     if (pico_ipv4_is_multicast(hdr->dst.addr)) {
01039         struct pico_frame *cpy;
01040         /* Sending UDP multicast datagram, am I member? If so, loopback copy */
01041         if ((proto != PICO_PROTO_IGMP) && (pico_ipv4_mcast_filter(f) == 0)) {
01042             ip_mcast_dbg("MCAST: sender is member of group, loopback copy\n");
01043             cpy = pico_frame_copy(f);
01044             pico_enqueue(&in, cpy);
01045         }
01046     }
01047 
01048 #endif
01049 
01050 /* #ifdef PICO_SUPPORT_AODV */
01051 #if 0
01052     {
01053         union pico_address node_address;
01054         node_address.ip4.addr = hdr->dst.addr;
01055         if(hdr->dst.addr && pico_ipv4_is_unicast(hdr->dst.addr))
01056             pico_aodv_lookup(&node_address);
01057     }
01058 #endif
01059 
01060     if (pico_ipv4_link_get(&hdr->dst)) {
01061         /* it's our own IP */
01062         return pico_enqueue(&in, f);
01063     } else{
01064         /* TODO: Check if there are members subscribed here */
01065         return pico_enqueue(&out, f);
01066     }
01067 
01068 drop:
01069     pico_frame_discard(f);
01070     return -1;
01071 }
01072 
01073 
01074 static int pico_ipv4_frame_sock_push(struct pico_protocol *self, struct pico_frame *f)
01075 {
01076     struct pico_ip4 *dst;
01077     struct pico_remote_endpoint *remote_endpoint = (struct pico_remote_endpoint *) f->info;
01078     IGNORE_PARAMETER(self);
01079 
01080     if (!f->sock) {
01081         pico_frame_discard(f);
01082         return -1;
01083     }
01084 
01085     if (remote_endpoint) {
01086         dst = &remote_endpoint->remote_addr.ip4;
01087     } else {
01088         dst = &f->sock->remote_addr.ip4;
01089     }
01090 
01091     return pico_ipv4_frame_push(f, dst, (uint8_t)f->sock->proto->proto_number);
01092 }
01093 
01094 
01095 int MOCKABLE pico_ipv4_route_add(struct pico_ip4 address, struct pico_ip4 netmask, struct pico_ip4 gateway, int metric, struct pico_ipv4_link *link)
01096 {
01097     struct pico_ipv4_route test, *new;
01098     test.dest.addr = address.addr;
01099     test.netmask.addr = netmask.addr;
01100     test.metric = (uint32_t)metric;
01101 
01102     if (pico_tree_findKey(&Routes, &test)) {
01103         pico_err = PICO_ERR_EINVAL;
01104         return -1;
01105     }
01106 
01107     new = PICO_ZALLOC(sizeof(struct pico_ipv4_route));
01108     if (!new) {
01109         pico_err = PICO_ERR_ENOMEM;
01110         return -1;
01111     }
01112 
01113     new->dest.addr = address.addr;
01114     new->netmask.addr = netmask.addr;
01115     new->gateway.addr = gateway.addr;
01116     new->metric = (uint32_t)metric;
01117     if (gateway.addr == 0) {
01118         /* No gateway provided, use the link */
01119         new->link = link;
01120     } else {
01121         struct pico_ipv4_route *r = route_find(&gateway);
01122         if (!r ) { /* Specified Gateway is unreachable */
01123             pico_err = PICO_ERR_EHOSTUNREACH;
01124             PICO_FREE(new);
01125             return -1;
01126         }
01127 
01128         if (r->gateway.addr) { /* Specified Gateway is not a neighbor */
01129             pico_err = PICO_ERR_ENETUNREACH;
01130             PICO_FREE(new);
01131             return -1;
01132         }
01133 
01134         new->link = r->link;
01135     }
01136 
01137     if (!new->link) {
01138         pico_err = PICO_ERR_EINVAL;
01139         PICO_FREE(new);
01140         return -1;
01141     }
01142 
01143     pico_tree_insert(&Routes, new);
01144     dbg_route();
01145     return 0;
01146 }
01147 
01148 int pico_ipv4_route_del(struct pico_ip4 address, struct pico_ip4 netmask, int metric)
01149 {
01150     struct pico_ipv4_route test, *found;
01151 
01152     test.dest.addr = address.addr;
01153     test.netmask.addr = netmask.addr;
01154     test.metric = (uint32_t)metric;
01155 
01156     found = pico_tree_findKey(&Routes, &test);
01157     if (found) {
01158 
01159         pico_tree_delete(&Routes, found);
01160         PICO_FREE(found);
01161 
01162         dbg_route();
01163         return 0;
01164     }
01165 
01166     pico_err = PICO_ERR_EINVAL;
01167     return -1;
01168 }
01169 
01170 
01171 int pico_ipv4_link_add(struct pico_device *dev, struct pico_ip4 address, struct pico_ip4 netmask)
01172 {
01173     struct pico_ipv4_link test, *new;
01174     struct pico_ip4 network, gateway;
01175     char ipstr[30];
01176 
01177     if (!dev) {
01178         pico_err = PICO_ERR_EINVAL;
01179         return -1;
01180     }
01181 
01182     test.address.addr = address.addr;
01183     test.netmask.addr = netmask.addr;
01184     test.dev = dev;
01185     /** XXX: Valid netmask / unicast address test **/
01186 
01187     if (pico_tree_findKey(&Tree_dev_link, &test)) {
01188         pico_err = PICO_ERR_EADDRINUSE;
01189         return -1;
01190     }
01191 
01192     /** XXX: Check for network already in use (e.g. trying to assign 10.0.0.1/24 where 10.1.0.1/8 is in use) **/
01193     new = PICO_ZALLOC(sizeof(struct pico_ipv4_link));
01194     if (!new) {
01195         dbg("IPv4: Out of memory!\n");
01196         pico_err = PICO_ERR_ENOMEM;
01197         return -1;
01198     }
01199 
01200     new->address.addr = address.addr;
01201     new->netmask.addr = netmask.addr;
01202     new->dev = dev;
01203 #ifdef PICO_SUPPORT_MCAST
01204     new->MCASTGroups = PICO_ZALLOC(sizeof(struct pico_tree));
01205     if (!new->MCASTGroups) {
01206         PICO_FREE(new);
01207         dbg("IPv4: Out of memory!\n");
01208         pico_err = PICO_ERR_ENOMEM;
01209         return -1;
01210     }
01211 
01212     new->MCASTGroups->root = &LEAF;
01213     new->MCASTGroups->compare = ipv4_mcast_groups_cmp;
01214 #ifdef PICO_SUPPORT_IGMP
01215     new->mcast_compatibility = PICO_IGMPV3; /* default RFC 3376 $7.2.1 */
01216     new->mcast_last_query_interval = PICO_IGMP_QUERY_INTERVAL;
01217 #endif
01218 #endif
01219 
01220     pico_tree_insert(&Tree_dev_link, new);
01221 #ifdef PICO_SUPPORT_MCAST
01222     do {
01223         struct pico_ip4 mcast_all_hosts, mcast_addr, mcast_nm, mcast_gw;
01224         if (!mcast_default_link) {
01225             mcast_addr.addr = long_be(0xE0000000); /* 224.0.0.0 */
01226             mcast_nm.addr = long_be(0xF0000000); /* 15.0.0.0 */
01227             mcast_gw.addr = long_be(0x00000000);
01228             mcast_default_link = new;
01229             pico_ipv4_route_add(mcast_addr, mcast_nm, mcast_gw, 1, new);
01230         }
01231 
01232         mcast_all_hosts.addr = PICO_MCAST_ALL_HOSTS;
01233         pico_ipv4_mcast_join(&address, &mcast_all_hosts, 1, PICO_IP_MULTICAST_EXCLUDE, NULL);
01234     } while(0);
01235 #endif
01236 
01237     network.addr = address.addr & netmask.addr;
01238     gateway.addr = 0U;
01239     pico_ipv4_route_add(network, netmask, gateway, 1, new);
01240     pico_ipv4_to_string(ipstr, new->address.addr);
01241     dbg("Assigned ipv4 %s to device %s\n", ipstr, new->dev->name);
01242     if (default_bcast_route.link == NULL)
01243         default_bcast_route.link = new;
01244 
01245     return 0;
01246 }
01247 
01248 static int pico_ipv4_cleanup_routes(struct pico_ipv4_link *link)
01249 {
01250     struct pico_tree_node *index = NULL, *tmp = NULL;
01251     struct pico_ipv4_route *route = NULL;
01252 
01253     pico_tree_foreach_safe(index, &Routes, tmp) {
01254         route = index->keyValue;
01255         if (link == route->link)
01256             pico_ipv4_route_del(route->dest, route->netmask, (int)route->metric);
01257     }
01258     return 0;
01259 }
01260 
01261 void MOCKABLE pico_ipv4_route_set_bcast_link(struct pico_ipv4_link *link)
01262 {
01263     if (link)
01264         default_bcast_route.link = link;
01265 }
01266 
01267 int pico_ipv4_link_del(struct pico_device *dev, struct pico_ip4 address)
01268 {
01269     struct pico_ipv4_link test, *found;
01270 
01271     if (!dev) {
01272         pico_err = PICO_ERR_EINVAL;
01273         return -1;
01274     }
01275 
01276     test.address.addr = address.addr;
01277     test.dev = dev;
01278     found = pico_tree_findKey(&Tree_dev_link, &test);
01279     if (!found) {
01280         pico_err = PICO_ERR_ENXIO;
01281         return -1;
01282     }
01283 
01284 #ifdef PICO_SUPPORT_MCAST
01285     do {
01286         struct pico_ip4 mcast_all_hosts, mcast_addr, mcast_nm;
01287         struct pico_mcast_group *g = NULL;
01288         struct pico_tree_node *index, *_tmp;
01289         if (found == mcast_default_link) {
01290             mcast_addr.addr = long_be(0xE0000000); /* 224.0.0.0 */
01291             mcast_nm.addr = long_be(0xF0000000); /* 15.0.0.0 */
01292             mcast_default_link = NULL;
01293             pico_ipv4_route_del(mcast_addr, mcast_nm, 1);
01294         }
01295 
01296         mcast_all_hosts.addr = PICO_MCAST_ALL_HOSTS;
01297         pico_ipv4_mcast_leave(&address, &mcast_all_hosts, 1, PICO_IP_MULTICAST_EXCLUDE, NULL);
01298         pico_tree_foreach_safe(index, found->MCASTGroups, _tmp) {
01299             g = index->keyValue;
01300             pico_tree_delete(found->MCASTGroups, g);
01301             PICO_FREE(g);
01302         }
01303     } while(0);
01304 #endif
01305 
01306     pico_ipv4_cleanup_routes(found);
01307     pico_tree_delete(&Tree_dev_link, found);
01308     if (default_bcast_route.link == found)
01309         default_bcast_route.link = NULL;
01310 
01311     PICO_FREE(found);
01312 
01313     return 0;
01314 }
01315 
01316 
01317 struct pico_ipv4_link *pico_ipv4_link_get(struct pico_ip4 *address)
01318 {
01319     struct pico_ipv4_link test = {
01320         0
01321     }, *found = NULL;
01322     test.address.addr = address->addr;
01323 
01324     found = pico_tree_findKey(&Tree_dev_link, &test);
01325     if (!found)
01326         return NULL;
01327     else
01328         return found;
01329 }
01330 
01331 struct pico_ipv4_link *MOCKABLE pico_ipv4_link_by_dev(struct pico_device *dev)
01332 {
01333     struct pico_tree_node *index = NULL;
01334     struct pico_ipv4_link *link = NULL;
01335 
01336     pico_tree_foreach(index, &Tree_dev_link) {
01337         link = index->keyValue;
01338         if (link->dev == dev)
01339             return link;
01340     }
01341     return NULL;
01342 }
01343 
01344 struct pico_ipv4_link *pico_ipv4_link_by_dev_next(struct pico_device *dev, struct pico_ipv4_link *last)
01345 {
01346     struct pico_tree_node *index = NULL;
01347     struct pico_ipv4_link *link = NULL;
01348     int valid = 0;
01349 
01350     if (last == NULL)
01351         valid = 1;
01352 
01353     pico_tree_foreach(index, &Tree_dev_link) {
01354         link = index->keyValue;
01355         if (link->dev == dev) {
01356             if (last == link)
01357                 valid = 1;
01358             else if (valid > 0)
01359                 return link;
01360         }
01361     }
01362     return NULL;
01363 }
01364 
01365 struct pico_device *MOCKABLE pico_ipv4_link_find(struct pico_ip4 *address)
01366 {
01367     struct pico_ipv4_link test, *found;
01368     if (!address) {
01369         pico_err = PICO_ERR_EINVAL;
01370         return NULL;
01371     }
01372 
01373     test.dev = NULL;
01374     test.address.addr = address->addr;
01375     found = pico_tree_findKey(&Tree_dev_link, &test);
01376     if (!found) {
01377         pico_err = PICO_ERR_ENXIO;
01378         return NULL;
01379     }
01380 
01381     return found->dev;
01382 }
01383 
01384 
01385 static int pico_ipv4_rebound_large(struct pico_frame *f)
01386 {
01387 #ifdef PICO_SUPPORT_IPV4FRAG
01388     uint16_t total_payload_written = 0;
01389     uint32_t len = f->transport_len;
01390     struct pico_frame *fr;
01391     struct pico_ip4 dst;
01392     struct pico_ipv4_hdr *hdr;
01393     hdr = (struct pico_ipv4_hdr *) f->net_hdr;
01394     dst.addr = hdr->src.addr;
01395 
01396     while(total_payload_written < len) {
01397         uint32_t space = (uint32_t)len - total_payload_written;
01398         if (space > PICO_IPV4_MAXPAYLOAD)
01399             space = PICO_IPV4_MAXPAYLOAD;
01400 
01401         fr = pico_ipv4_alloc(&pico_proto_ipv4, (uint16_t)space);
01402         if (!fr) {
01403             pico_err = PICO_ERR_ENOMEM;
01404             return -1;
01405         }
01406 
01407         if (space + total_payload_written < len)
01408         {
01409             fr->frag |= PICO_IPV4_MOREFRAG;
01410         }
01411         else
01412         {
01413             fr->frag &= PICO_IPV4_FRAG_MASK;
01414         }
01415 
01416         fr->frag = (((total_payload_written) >> 3u) & 0xffffu) | fr->frag;
01417 
01418         memcpy(fr->transport_hdr, f->transport_hdr + total_payload_written, fr->transport_len);
01419         if (pico_ipv4_frame_push(fr, &dst, hdr->proto) > 0) {
01420             total_payload_written = (uint16_t)((uint16_t)fr->transport_len + total_payload_written);
01421         } else {
01422             pico_frame_discard(fr);
01423             break;
01424         }
01425     } /* while() */
01426     return (int)total_payload_written;
01427 #else
01428     (void)f;
01429     return -1;
01430 #endif
01431 }
01432 
01433 int pico_ipv4_rebound(struct pico_frame *f)
01434 {
01435     struct pico_ip4 dst;
01436     struct pico_ipv4_hdr *hdr;
01437     if (!f) {
01438         pico_err = PICO_ERR_EINVAL;
01439         return -1;
01440     }
01441 
01442     hdr = (struct pico_ipv4_hdr *) f->net_hdr;
01443     if (!hdr) {
01444         pico_err = PICO_ERR_EINVAL;
01445         return -1;
01446     }
01447 
01448     dst.addr = hdr->src.addr;
01449     if (f->transport_len > PICO_IPV4_MAXPAYLOAD) {
01450         return pico_ipv4_rebound_large(f);
01451     }
01452 
01453     return pico_ipv4_frame_push(f, &dst, hdr->proto);
01454 }
01455 
01456 static int pico_ipv4_pre_forward_checks(struct pico_frame *f)
01457 {
01458     static uint16_t last_id = 0;
01459     static uint16_t last_proto = 0;
01460     static struct pico_ip4 last_src = {
01461         0
01462     };
01463     static struct pico_ip4 last_dst = {
01464         0
01465     };
01466     struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr;
01467 
01468     /* Decrease TTL, check if expired */
01469     hdr->ttl = (uint8_t)(hdr->ttl - 1);
01470     if (hdr->ttl < 1) {
01471         pico_notify_ttl_expired(f);
01472         dbg(" ------------------- TTL EXPIRED\n");
01473         return -1;
01474     }
01475 
01476     /* HACK: increase crc to compensate decreased TTL */
01477     hdr->crc++;
01478 
01479     /* If source is local, discard anyway (packets bouncing back and forth) */
01480     if (pico_ipv4_link_get(&hdr->src))
01481         return -1;
01482 
01483     /* If this was the last forwarded packet, silently discard to prevent duplications */
01484     if ((last_src.addr == hdr->src.addr) && (last_id == hdr->id)
01485         && (last_dst.addr == hdr->dst.addr) && (last_proto == hdr->proto)) {
01486         return -1;
01487     } else {
01488         last_src.addr = hdr->src.addr;
01489         last_dst.addr = hdr->dst.addr;
01490         last_id = hdr->id;
01491         last_proto = hdr->proto;
01492     }
01493 
01494     return 0;
01495 }
01496 
01497 static int pico_ipv4_forward_check_dev(struct pico_frame *f)
01498 {
01499     if (f->dev->eth != NULL)
01500         f->len -= PICO_SIZE_ETHHDR;
01501 
01502     if (f->len > f->dev->mtu) {
01503         pico_notify_pkt_too_big(f);
01504         return -1;
01505     }
01506 
01507     return 0;
01508 }
01509 
01510 static int pico_ipv4_forward(struct pico_frame *f)
01511 {
01512     struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr;
01513     struct pico_ipv4_route *rt;
01514     if (!hdr) {
01515         return -1;
01516     }
01517 
01518     rt = route_find(&hdr->dst);
01519     if (!rt) {
01520         pico_notify_dest_unreachable(f);
01521         return -1;
01522     }
01523 
01524     f->dev = rt->link->dev;
01525 
01526     if (pico_ipv4_pre_forward_checks(f) < 0)
01527         return -1;
01528 
01529     pico_ipv4_nat_outbound(f, &rt->link->address);
01530 
01531     f->start = f->net_hdr;
01532 
01533     if (pico_ipv4_forward_check_dev(f) < 0)
01534         return -1;
01535 
01536     pico_sendto_dev(f);
01537     return 0;
01538 
01539 }
01540 
01541 int pico_ipv4_is_broadcast(uint32_t addr)
01542 {
01543     struct pico_ipv4_link *link;
01544     struct pico_tree_node *index;
01545     if (addr == PICO_IP4_BCAST)
01546         return 1;
01547 
01548     pico_tree_foreach(index, &Tree_dev_link) {
01549         link = index->keyValue;
01550         if ((link->address.addr | (~link->netmask.addr)) == addr)
01551             return 1;
01552     }
01553     return 0;
01554 }
01555 
01556 void pico_ipv4_unreachable(struct pico_frame *f, int err)
01557 {
01558     struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
01559 #if defined PICO_SUPPORT_TCP || defined PICO_SUPPORT_UDP
01560     f->transport_hdr = ((uint8_t *)f->net_hdr) + PICO_SIZE_IP4HDR;
01561     pico_transport_error(f, hdr->proto, err);
01562 #endif
01563 }
01564 
01565 int pico_ipv4_cleanup_links(struct pico_device *dev)
01566 {
01567     struct pico_tree_node *index = NULL, *_tmp = NULL;
01568     struct pico_ipv4_link *link = NULL;
01569 
01570     pico_tree_foreach_safe(index, &Tree_dev_link, _tmp) {
01571         link = index->keyValue;
01572         if (dev == link->dev)
01573             pico_ipv4_link_del(dev, link->address);
01574     }
01575     return 0;
01576 }
01577 
01578 
01579 #endif