Daniele Lacamera
CDC/ECM driver for mbed, based on USBDevice by mbed-official. Uses PicoTCP to access Ethernet USB device. License: GPLv2
Fork of
USB_Ethernet
by 
Daniele Lacamera
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Wed Jul 13 2022 02:20:45 by
1.7.2