pico_ipv4.c

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