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_socket.c
00001 /********************************************************************* 00002 PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved. 00003 See LICENSE and COPYING for usage. 00004 00005 00006 Authors: Daniele Lacamera 00007 *********************************************************************/ 00008 00009 00010 #include "pico_config.h" 00011 #include "pico_queue.h" 00012 #include "pico_socket.h" 00013 #include "pico_ipv4.h" 00014 #include "pico_ipv6.h" 00015 #include "pico_udp.h" 00016 #include "pico_tcp.h" 00017 #include "pico_stack.h" 00018 #include "pico_icmp4.h" 00019 #include "pico_nat.h" 00020 #include "pico_tree.h" 00021 #include "pico_device.h" 00022 00023 #if defined (PICO_SUPPORT_IPV4) || defined (PICO_SUPPORT_IPV6) 00024 #if defined (PICO_SUPPORT_TCP) || defined (PICO_SUPPORT_UDP) 00025 00026 00027 #ifdef PICO_SUPPORT_MUTEX 00028 static void * Mutex = NULL; 00029 #define LOCK(x) {\ 00030 if (x == NULL) \ 00031 x = pico_mutex_init(); \ 00032 pico_mutex_lock(x); \ 00033 } 00034 #define UNLOCK(x) pico_mutex_unlock(x); 00035 00036 #else 00037 #define LOCK(x) do{}while(0) 00038 #define UNLOCK(x) do{}while(0) 00039 #endif 00040 00041 00042 #define PROTO(s) ((s)->proto->proto_number) 00043 00044 #ifdef PICO_SUPPORT_TCP 00045 # define IS_NAGLE_ENABLED(s) (!(!(!(s->opt_flags & (1 << PICO_SOCKET_OPT_TCPNODELAY))))) 00046 #endif 00047 00048 #define PICO_SOCKET_MTU 1480 /* Ethernet MTU(1500) - IP header size(20) */ 00049 00050 #ifdef PICO_SUPPORT_IPV4 00051 # define IS_SOCK_IPV4(s) ((s->net == &pico_proto_ipv4)) 00052 #else 00053 # define IS_SOCK_IPV4(s) (0) 00054 #endif 00055 00056 #ifdef PICO_SUPPORT_IPV6 00057 # define IS_SOCK_IPV6(s) ((s->net == &pico_proto_ipv6)) 00058 #else 00059 # define IS_SOCK_IPV6(s) (0) 00060 #endif 00061 00062 #ifdef PICO_SUPPORT_IPFRAG 00063 # define frag_dbg(...) do{}while(0) 00064 #endif 00065 00066 #ifdef PICO_SUPPORT_MCAST 00067 # define so_mcast_dbg(...) do{}while(0) /* ip_mcast_dbg in pico_ipv4.c */ 00068 #endif 00069 00070 static struct pico_sockport *sp_udp = NULL ,*sp_tcp = NULL; 00071 00072 struct pico_frame *pico_socket_frame_alloc(struct pico_socket *s, int len); 00073 00074 static int socket_cmp(void * ka, void * kb) 00075 { 00076 struct pico_socket *a = ka, *b = kb; 00077 int a_is_ip6 = is_sock_ipv6(a); 00078 int b_is_ip6 = is_sock_ipv6(b); 00079 00080 int diff; 00081 00082 /* First, order by network ver */ 00083 if (a_is_ip6 < b_is_ip6) 00084 return -1; 00085 if (a_is_ip6 > b_is_ip6) 00086 return 1; 00087 00088 /* If either socket is PICO_IPV4_INADDR_ANY mode, skip local address comparison */ 00089 00090 /* At this point, sort by local host */ 00091 00092 if (0) { 00093 #ifdef PICO_SUPPORT_IPV6 00094 } else if (a_is_ip6) { 00095 if ((memcmp(a->local_addr.ip6.addr, PICO_IP6_ANY, PICO_SIZE_IP6)==0) || memcmp((b->local_addr.ip6.addr, PICO_IP6_ANY, PICO_SIZE_IP6) == 0)) 00096 diff = 0; 00097 else 00098 diff = memcmp(a->local_addr.ip6.addr, b->local_addr.ip6.addr, PICO_SIZE_IP6); 00099 #endif 00100 } else { 00101 if ((a->local_addr.ip4.addr == PICO_IP4_ANY) || (b->local_addr.ip4.addr == PICO_IP4_ANY)) 00102 diff = 0; 00103 else 00104 diff = a->local_addr.ip4.addr - b->local_addr.ip4.addr; 00105 } 00106 00107 if (diff) 00108 return diff; 00109 00110 00111 /* Sort by remote host */ 00112 if (a_is_ip6) 00113 diff = memcmp(a->remote_addr.ip6.addr, b->remote_addr.ip6.addr, PICO_SIZE_IP6); 00114 else 00115 diff = a->remote_addr.ip4.addr - b->remote_addr.ip4.addr; 00116 00117 if (diff) 00118 return diff; 00119 00120 /* And finally by remote port. The two sockets are coincident if the quad is the same. */ 00121 return b->remote_port - a->remote_port; 00122 } 00123 00124 struct pico_sockport 00125 { 00126 struct pico_tree socks; // how you make the connection ? 00127 uint16_t number; 00128 uint16_t proto; 00129 }; 00130 00131 #define INIT_SOCKPORT { {&LEAF , socket_cmp}, 0, 0 } 00132 00133 int sockport_cmp(void * ka, void * kb) 00134 { 00135 struct pico_sockport *a = ka, *b = kb; 00136 if (a->number < b->number) 00137 return -1; 00138 if (a->number > b->number) 00139 return 1; 00140 return 0; 00141 } 00142 00143 PICO_TREE_DECLARE(UDPTable,sockport_cmp); 00144 PICO_TREE_DECLARE(TCPTable,sockport_cmp); 00145 00146 #ifdef PICO_SUPPORT_MCAST 00147 /* socket 00148 * | 00149 * MCASTListen 00150 * | | | 00151 * ------------ | ------------ 00152 * | | | 00153 * MCASTSources MCASTSources MCASTSources 00154 * | | | | | | | | | | | | 00155 * S S S S S S S S S S S S 00156 * 00157 * MCASTListen: RBTree(mcast_link, mcast_group) 00158 * MCASTSources: RBTree(source) 00159 */ 00160 struct pico_mcast_listen 00161 { 00162 uint8_t filter_mode; 00163 struct pico_ip4 mcast_link; 00164 struct pico_ip4 mcast_group; 00165 struct pico_tree MCASTSources; 00166 }; 00167 00168 static int mcast_listen_cmp(void *ka, void *kb) 00169 { 00170 struct pico_mcast_listen *a = ka, *b = kb; 00171 if (a->mcast_group.addr < b->mcast_group.addr) 00172 return -1; 00173 if (a->mcast_group.addr > b->mcast_group.addr) 00174 return 1; 00175 00176 if (a->mcast_link.addr < b->mcast_link.addr) 00177 return -1; 00178 if (a->mcast_link.addr > b->mcast_link.addr) 00179 return 1; 00180 00181 return 0; 00182 } 00183 00184 static int mcast_sources_cmp(void *ka, void *kb) 00185 { 00186 struct pico_ip4 *a = ka, *b = kb; 00187 if (a->addr < b->addr) 00188 return -1; 00189 if (a->addr > b->addr) 00190 return 1; 00191 return 0; 00192 } 00193 00194 static int mcast_socket_cmp(void *ka, void *kb) 00195 { 00196 struct pico_socket *a = ka, *b = kb; 00197 if (a < b) 00198 return -1; 00199 if (a > b) 00200 return 1; 00201 return 0; 00202 } 00203 /* gather all multicast sockets to hasten filter aggregation */ 00204 PICO_TREE_DECLARE(MCASTSockets, mcast_socket_cmp); 00205 00206 static int mcast_filter_cmp(void *ka, void *kb) 00207 { 00208 struct pico_ip4 *a = ka, *b = kb; 00209 if (a->addr < b->addr) 00210 return -1; 00211 if (a->addr > b->addr) 00212 return 1; 00213 return 0; 00214 } 00215 /* gather sources to be filtered */ 00216 PICO_TREE_DECLARE(MCASTFilter, mcast_filter_cmp); 00217 00218 /* MCASTFilter will be empty if no socket is listening on mcast_group on mcast_link anymore */ 00219 static int pico_socket_aggregate_mcastfilters(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group) 00220 { 00221 uint8_t filter_mode = PICO_IP_MULTICAST_INCLUDE; 00222 struct pico_mcast_listen *listen = NULL, ltest = {0}; 00223 struct pico_ip4 *source = NULL; 00224 struct pico_socket *mcast_sock = NULL; 00225 struct pico_tree_node *index = NULL, *_tmp = NULL, *index2 = NULL, *_tmp2 = NULL; 00226 00227 ltest.mcast_link = *mcast_link; 00228 ltest.mcast_group = *mcast_group; 00229 00230 /* cleanup old filter */ 00231 pico_tree_foreach_safe(index, &MCASTFilter, _tmp) 00232 { 00233 pico_tree_delete(&MCASTFilter, index->keyValue); 00234 } 00235 00236 /* construct new filter */ 00237 pico_tree_foreach(index, &MCASTSockets) 00238 { 00239 mcast_sock = index->keyValue; 00240 listen = pico_tree_findKey(mcast_sock->MCASTListen, <est); 00241 if (listen) { 00242 /* aggregate filter */ 00243 switch(filter_mode) 00244 { 00245 case PICO_IP_MULTICAST_INCLUDE: 00246 switch (listen->filter_mode) 00247 { 00248 case PICO_IP_MULTICAST_INCLUDE: 00249 /* filter = summation of INCLUDEs */ 00250 /* mode stays INCLUDE, add all sources to filter */ 00251 pico_tree_foreach(index2, &listen->MCASTSources) 00252 { 00253 source = index2->keyValue; 00254 pico_tree_insert(&MCASTFilter, source); 00255 } 00256 break; 00257 00258 case PICO_IP_MULTICAST_EXCLUDE: 00259 /* filter = EXCLUDE - INCLUDE */ 00260 /* delete from the interface INCLUDE filter any source NOT in the socket EXCLUDE filter */ 00261 pico_tree_foreach_safe(index2, &MCASTFilter, _tmp2) 00262 { 00263 source = pico_tree_findKey(&listen->MCASTSources, index2->keyValue); 00264 if (!source) 00265 pico_tree_delete(&MCASTFilter, index2->keyValue); 00266 } 00267 /* any record with filter mode EXCLUDE, causes the interface mode to be EXCLUDE */ 00268 filter_mode = PICO_IP_MULTICAST_EXCLUDE; 00269 /* add to the interface EXCLUDE filter any socket source NOT in the former interface INCLUDE filter */ 00270 pico_tree_foreach(index2, &listen->MCASTSources) 00271 { 00272 source = pico_tree_insert(&MCASTFilter, index2->keyValue); 00273 if (source) 00274 pico_tree_delete(&MCASTFilter, source); 00275 } 00276 break; 00277 00278 default: 00279 return -1; 00280 } 00281 break; 00282 00283 case PICO_IP_MULTICAST_EXCLUDE: 00284 switch (listen->filter_mode) 00285 { 00286 case PICO_IP_MULTICAST_INCLUDE: 00287 /* filter = EXCLUDE - INCLUDE */ 00288 /* any record with filter mode EXCLUDE, causes the interface mode to be EXCLUDE */ 00289 /* remove from the interface EXCLUDE filter any source in the socket INCLUDE filter */ 00290 pico_tree_foreach(index2, &listen->MCASTSources) 00291 { 00292 source = pico_tree_findKey(&MCASTFilter, index2->keyValue); 00293 if (source) 00294 pico_tree_delete(&MCASTFilter, source); 00295 } 00296 break; 00297 00298 case PICO_IP_MULTICAST_EXCLUDE: 00299 /* filter = intersection of EXCLUDEs */ 00300 /* any record with filter mode EXCLUDE, causes the interface mode to be EXCLUDE */ 00301 /* remove from the interface EXCLUDE filter any source not in the socket EXCLUDE filter */ 00302 pico_tree_foreach_safe(index2, &MCASTFilter, _tmp2) 00303 { 00304 source = pico_tree_findKey(&listen->MCASTSources, index2->keyValue); 00305 if (!source) 00306 pico_tree_delete(&MCASTFilter, index2->keyValue); 00307 } 00308 break; 00309 00310 default: 00311 return -1; 00312 } 00313 break; 00314 00315 default: 00316 return -1; 00317 } 00318 } 00319 } 00320 return filter_mode; 00321 } 00322 00323 static int pico_socket_mcast_filter(struct pico_socket *s, struct pico_ip4 *mcast_group, struct pico_ip4 *src) 00324 { 00325 struct pico_ipv4_link *mcast_link = NULL; 00326 struct pico_mcast_listen *listen = NULL, ltest = {0}; 00327 struct pico_tree_node *index = NULL; 00328 00329 /* no multicast enabled on socket */ 00330 if (!s->MCASTListen) 00331 return 0; 00332 00333 mcast_link = pico_ipv4_link_get(&s->local_addr.ip4); 00334 if (!mcast_link) 00335 return -1; 00336 00337 ltest.mcast_link.addr = mcast_link->address.addr; 00338 ltest.mcast_group = *mcast_group; 00339 listen = pico_tree_findKey(s->MCASTListen, <est); 00340 if (!listen) 00341 return -1; 00342 00343 /* perform source filtering */ 00344 switch (listen->filter_mode) 00345 { 00346 case PICO_IP_MULTICAST_INCLUDE: 00347 pico_tree_foreach(index, &listen->MCASTSources) 00348 { 00349 if (src->addr == ((struct pico_ip4 *)index->keyValue)->addr) { 00350 so_mcast_dbg("MCAST: IP %08X in included socket source list\n", src->addr); 00351 return 0; 00352 } 00353 } 00354 so_mcast_dbg("MCAST: IP %08X NOT in included socket source list\n", src->addr); 00355 return -1; 00356 break; 00357 00358 case PICO_IP_MULTICAST_EXCLUDE: 00359 pico_tree_foreach(index, &listen->MCASTSources) 00360 { 00361 if (src->addr == ((struct pico_ip4 *)index->keyValue)->addr) { 00362 so_mcast_dbg("MCAST: IP %08X in excluded socket source list\n", src->addr); 00363 return -1; 00364 } 00365 } 00366 so_mcast_dbg("MCAST: IP %08X NOT in excluded socket source list\n", src->addr); 00367 return 0; 00368 break; 00369 00370 default: 00371 return -1; 00372 break; 00373 } 00374 return -1; 00375 } 00376 00377 static inline struct pico_ipv4_link *pico_socket_setoption_mcastargs_validation(struct pico_ip_mreq *mreq, struct pico_ip_mreq_source *mreq_source) 00378 { 00379 struct pico_ipv4_link *mcast_link = NULL; 00380 00381 if (!mreq && !mreq_source) 00382 return NULL; 00383 00384 if (mreq) { 00385 if (!mreq->mcast_group_addr.addr) 00386 return NULL; 00387 if (pico_ipv4_is_unicast(mreq->mcast_group_addr.addr)) 00388 return NULL; 00389 00390 if (!mreq->mcast_link_addr.addr) { 00391 mcast_link = pico_ipv4_get_default_mcastlink(); 00392 if (!mcast_link) 00393 return NULL; 00394 } else { 00395 mcast_link = pico_ipv4_link_get(&mreq->mcast_link_addr); 00396 if (!mcast_link) 00397 return NULL; 00398 } 00399 } 00400 if (mreq_source) { 00401 if (!mreq_source->mcast_group_addr.addr) 00402 return NULL; 00403 if (pico_ipv4_is_unicast(mreq_source->mcast_group_addr.addr)) 00404 return NULL; 00405 00406 if (!mreq_source->mcast_source_addr.addr) 00407 return NULL; 00408 if (!pico_ipv4_is_unicast(mreq_source->mcast_source_addr.addr)) 00409 return NULL; 00410 00411 if (!mreq_source->mcast_link_addr.addr) { 00412 mcast_link = pico_ipv4_get_default_mcastlink(); 00413 if (!mcast_link) 00414 return NULL; 00415 } else { 00416 mcast_link = pico_ipv4_link_get(&mreq_source->mcast_link_addr); 00417 if (!mcast_link) 00418 return NULL; 00419 } 00420 } 00421 return mcast_link; 00422 } 00423 #else 00424 static int pico_socket_mcast_filter(struct pico_socket *s, struct pico_ip4 *mcast_group, struct pico_ip4 *src) { 00425 return 0; 00426 } 00427 #endif /* PICO_SUPPORT_MCAST */ 00428 00429 static struct pico_sockport *pico_get_sockport(uint16_t proto, uint16_t port) 00430 { 00431 struct pico_sockport test = INIT_SOCKPORT; 00432 test.number = port; 00433 00434 if (proto == PICO_PROTO_UDP) 00435 return pico_tree_findKey(&UDPTable,&test); 00436 00437 else if (proto == PICO_PROTO_TCP) 00438 return pico_tree_findKey(&TCPTable,&test); 00439 00440 else return NULL; 00441 } 00442 00443 int pico_is_port_free(uint16_t proto, uint16_t port, void *addr, void *net) 00444 { 00445 struct pico_sockport *sp; 00446 struct pico_ip4 ip; 00447 sp = pico_get_sockport(proto, port); 00448 00449 if (!net) 00450 net = &pico_proto_ipv4; 00451 00452 /** IPv6 (wip) ***/ 00453 if (net != &pico_proto_ipv4) { 00454 dbg("IPV6!!!!!\n"); 00455 return (!sp); 00456 } 00457 00458 /* IPv4 */ 00459 #ifdef PICO_SUPPORT_NAT 00460 if (pico_ipv4_nat_find(port,NULL, 0,proto) == 0) { 00461 dbg("In use by nat....\n"); 00462 return 0; 00463 } 00464 #endif 00465 if (addr) 00466 ip.addr = ((struct pico_ip4 *)addr)->addr; 00467 else 00468 ip.addr = PICO_IPV4_INADDR_ANY; 00469 00470 if (ip.addr == PICO_IPV4_INADDR_ANY) { 00471 if (!sp) return 1; 00472 else { 00473 dbg("In use, and asked for ANY\n"); 00474 return 0; 00475 } 00476 } 00477 if (sp) { 00478 struct pico_ip4 *s_local; 00479 struct pico_tree_node *idx; 00480 struct pico_socket *s; 00481 pico_tree_foreach(idx, &sp->socks) { 00482 s = idx->keyValue; 00483 if (s->net == &pico_proto_ipv4) { 00484 s_local = (struct pico_ip4*) &s->local_addr; 00485 if ((s_local->addr == PICO_IPV4_INADDR_ANY) || (s_local->addr == ip.addr)) 00486 return 0; 00487 } 00488 } 00489 } 00490 return 1; 00491 } 00492 00493 static int pico_check_socket(struct pico_socket *s) 00494 { 00495 struct pico_sockport *test; 00496 struct pico_socket *found; 00497 struct pico_tree_node * index; 00498 00499 test = pico_get_sockport(PROTO(s), s->local_port); 00500 00501 if (!test) { 00502 return -1; 00503 } 00504 00505 pico_tree_foreach(index,&test->socks){ 00506 found = index->keyValue; 00507 if (s == found) { 00508 return 0; 00509 } 00510 } 00511 00512 return -1; 00513 } 00514 00515 00516 int pico_socket_add(struct pico_socket *s) 00517 { 00518 struct pico_sockport *sp = pico_get_sockport(PROTO(s), s->local_port); 00519 LOCK(Mutex); 00520 if (!sp) { 00521 //dbg("Creating sockport..%04x\n", s->local_port); /* In comment due to spam during test */ 00522 sp = pico_zalloc(sizeof(struct pico_sockport)); 00523 00524 if (!sp) { 00525 pico_err = PICO_ERR_ENOMEM; 00526 UNLOCK(Mutex); 00527 return -1; 00528 } 00529 sp->proto = PROTO(s); 00530 sp->number = s->local_port; 00531 sp->socks.root = &LEAF; 00532 sp->socks.compare = socket_cmp; 00533 00534 if (PROTO(s) == PICO_PROTO_UDP) 00535 { 00536 pico_tree_insert(&UDPTable,sp); 00537 } 00538 else if (PROTO(s) == PICO_PROTO_TCP) 00539 { 00540 pico_tree_insert(&TCPTable,sp); 00541 } 00542 } 00543 00544 pico_tree_insert(&sp->socks,s); 00545 s->state |= PICO_SOCKET_STATE_BOUND; 00546 UNLOCK(Mutex); 00547 #if DEBUG_SOCKET_TREE 00548 { 00549 struct pico_tree_node * index; 00550 //RB_FOREACH(s, socket_tree, &sp->socks) { 00551 pico_tree_foreach(index,&sp->socks){ 00552 s = index->keyValue; 00553 dbg(">>>> List Socket lc=%hu rm=%hu\n", short_be(s->local_port), short_be(s->remote_port)); 00554 } 00555 00556 } 00557 #endif 00558 return 0; 00559 } 00560 00561 static void socket_garbage_collect(unsigned long now, void *arg) 00562 { 00563 struct pico_socket *s = (struct pico_socket *) arg; 00564 pico_free(s); 00565 } 00566 00567 int pico_socket_del(struct pico_socket *s) 00568 { 00569 struct pico_sockport *sp = pico_get_sockport(PROTO(s), s->local_port); 00570 00571 if (!sp) { 00572 pico_err = PICO_ERR_ENXIO; 00573 return -1; 00574 } 00575 00576 LOCK(Mutex); 00577 pico_tree_delete(&sp->socks,s); 00578 s->net = NULL; 00579 if(pico_tree_empty(&sp->socks)){ 00580 if (PROTO(s) == PICO_PROTO_UDP) 00581 { 00582 pico_tree_delete(&UDPTable,sp); 00583 } 00584 else if (PROTO(s) == PICO_PROTO_TCP) 00585 { 00586 pico_tree_delete(&TCPTable,sp); 00587 } 00588 00589 if(sp_tcp == sp) sp_tcp = NULL; 00590 00591 if(sp_udp == sp) sp_udp = NULL; 00592 00593 pico_free(sp); 00594 00595 } 00596 00597 #ifdef PICO_SUPPORT_MCAST 00598 do { 00599 uint8_t filter_mode = 0; 00600 struct pico_tree_node *index = NULL, *_tmp = NULL, *index2 = NULL, *_tmp2 = NULL; 00601 struct pico_mcast_listen *listen = NULL; 00602 struct pico_ip4 *source = NULL; 00603 if (s->MCASTListen) { 00604 pico_tree_delete(&MCASTSockets, s); 00605 pico_tree_foreach_safe(index, s->MCASTListen, _tmp) 00606 { 00607 listen = index->keyValue; 00608 pico_tree_foreach_safe(index2, &listen->MCASTSources, _tmp2) 00609 { 00610 source = index->keyValue; 00611 pico_tree_delete(&listen->MCASTSources, source); 00612 pico_free(source); 00613 } 00614 filter_mode = pico_socket_aggregate_mcastfilters(&listen->mcast_link, &listen->mcast_group); 00615 pico_ipv4_mcast_leave(&listen->mcast_link, &listen->mcast_group, 1, filter_mode, &MCASTFilter); 00616 pico_tree_delete(s->MCASTListen, listen); 00617 pico_free(listen); 00618 } 00619 pico_free(s->MCASTListen); 00620 } 00621 } while (0); 00622 #endif 00623 00624 s->state = PICO_SOCKET_STATE_CLOSED; 00625 pico_timer_add(3000, socket_garbage_collect, s); 00626 UNLOCK(Mutex); 00627 return 0; 00628 } 00629 00630 static int pico_socket_alter_state(struct pico_socket *s, uint16_t more_states, uint16_t less_states, uint16_t tcp_state) 00631 { 00632 struct pico_sockport *sp; 00633 if (more_states & PICO_SOCKET_STATE_BOUND) 00634 return pico_socket_add(s); 00635 00636 if (less_states & PICO_SOCKET_STATE_BOUND) 00637 return pico_socket_del(s); 00638 00639 sp = pico_get_sockport(PROTO(s), s->local_port); 00640 if (!sp) { 00641 pico_err = PICO_ERR_ENXIO; 00642 return -1; 00643 } 00644 00645 s->state |= more_states; 00646 s->state &= (~less_states); 00647 if (tcp_state) { 00648 s->state &= 0x00FF; 00649 s->state |= tcp_state; 00650 } 00651 00652 return 0; 00653 } 00654 00655 static int pico_socket_deliver(struct pico_protocol *p, struct pico_frame *f, uint16_t localport) 00656 { 00657 struct pico_frame *cpy = NULL; 00658 struct pico_sockport *sp = NULL; 00659 struct pico_socket *s = NULL, *found = NULL; 00660 struct pico_tree_node *index = NULL; 00661 struct pico_tree_node *_tmp; 00662 struct pico_trans *tr = (struct pico_trans *) f->transport_hdr; 00663 #ifdef PICO_SUPPORT_IPV4 00664 struct pico_ipv4_hdr *ip4hdr; 00665 #endif 00666 #ifdef PICO_SUPPORT_IPV6 00667 struct pico_ipv6_hdr *ip6hdr; 00668 #endif 00669 00670 if (!tr) 00671 return -1; 00672 00673 sp = pico_get_sockport(p->proto_number, localport); 00674 00675 if (!sp) { 00676 dbg("No such port %d\n",short_be(localport)); 00677 return -1; 00678 } 00679 00680 #ifdef PICO_SUPPORT_TCP 00681 if (p->proto_number == PICO_PROTO_TCP) { 00682 pico_tree_foreach_safe(index,&sp->socks, _tmp){ 00683 s = index->keyValue; 00684 /* 4-tuple identification of socket (port-IP) */ 00685 #ifdef PICO_SUPPORT_IPV4 00686 if (IS_IPV4(f)) { 00687 struct pico_ip4 s_local, s_remote, p_src, p_dst; 00688 ip4hdr = (struct pico_ipv4_hdr*)(f->net_hdr); 00689 s_local.addr = s->local_addr.ip4.addr; 00690 s_remote.addr = s->remote_addr.ip4.addr; 00691 p_src.addr = ip4hdr->src.addr; 00692 p_dst.addr = ip4hdr->dst.addr; 00693 if ( (s->remote_port == tr->sport) && /* remote port check */ 00694 (s_remote.addr == p_src.addr) && /* remote addr check */ 00695 ((s_local.addr == PICO_IPV4_INADDR_ANY) || (s_local.addr == p_dst.addr))) { /* Either local socket is ANY, or matches dst */ 00696 found = s; 00697 break; 00698 } else if ( (s->remote_port == 0) && /* not connected... listening */ 00699 ((s_local.addr == PICO_IPV4_INADDR_ANY) || (s_local.addr == p_dst.addr))) { /* Either local socket is ANY, or matches dst */ 00700 /* listen socket */ 00701 found = s; 00702 } 00703 } 00704 #endif 00705 #ifdef PICO_SUPPORT_IPV6 /* XXX TODO make compare for ipv6 addresses */ 00706 if (IS_IPV6(f)) { 00707 ip6hdr = (struct pico_ipv6_hdr*)(f->net_hdr); 00708 if ( (s->remote_port == localport) ) { // && (((struct pico_ip6) s->remote_addr.ip6).addr == ((struct pico_ip6)(ip6hdr->src)).addr) ) { 00709 found = s; 00710 break; 00711 } else if (s->remote_port == 0) { 00712 /* listen socket */ 00713 found = s; 00714 } 00715 } 00716 #endif 00717 } /* FOREACH */ 00718 if (found != NULL) { 00719 pico_tcp_input(found,f); 00720 if ((found->ev_pending) && found->wakeup) { 00721 found->wakeup(found->ev_pending, found); 00722 } 00723 return 0; 00724 } else { 00725 dbg("SOCKET> mmm something wrong (prob sockport)\n"); 00726 return -1; 00727 } 00728 } /* TCP CASE */ 00729 #endif 00730 00731 #ifdef PICO_SUPPORT_UDP 00732 if (p->proto_number == PICO_PROTO_UDP) { 00733 pico_tree_foreach_safe(index,&sp->socks, _tmp){ 00734 s = index->keyValue; 00735 if (IS_IPV4(f)) { /* IPV4 */ 00736 struct pico_ip4 s_local, p_dst; 00737 ip4hdr = (struct pico_ipv4_hdr*)(f->net_hdr); 00738 s_local.addr = s->local_addr.ip4.addr; 00739 p_dst.addr = ip4hdr->dst.addr; 00740 if ((pico_ipv4_is_broadcast(p_dst.addr)) || pico_ipv4_is_multicast(p_dst.addr)) { 00741 struct pico_device *dev = pico_ipv4_link_find(&s->local_addr.ip4); 00742 if (pico_ipv4_is_multicast(p_dst.addr) && (pico_socket_mcast_filter(s, &ip4hdr->dst, &ip4hdr->src) < 0)) 00743 return -1; 00744 if ((s_local.addr == PICO_IPV4_INADDR_ANY) || /* If our local ip is ANY, or.. */ 00745 (dev == f->dev) ) { /* the source of the bcast packet is a neighbor... */ 00746 cpy = pico_frame_copy(f); 00747 if (!cpy) 00748 return -1; 00749 if (pico_enqueue(&s->q_in, cpy) > 0) { 00750 if (s->wakeup) 00751 s->wakeup(PICO_SOCK_EV_RD, s); 00752 } 00753 } 00754 } else if ((s_local.addr == PICO_IPV4_INADDR_ANY) || (s_local.addr == p_dst.addr)) 00755 { /* Either local socket is ANY, or matches dst */ 00756 cpy = pico_frame_copy(f); 00757 if (!cpy) 00758 return -1; 00759 if (pico_enqueue(&s->q_in, cpy) > 0) { 00760 if (s->wakeup) 00761 s->wakeup(PICO_SOCK_EV_RD, s); 00762 } 00763 } 00764 } else { 00765 /*... IPv6 */ 00766 } 00767 } /* FOREACH */ 00768 pico_frame_discard(f); 00769 if (s) 00770 return 0; 00771 else 00772 return -1; 00773 } 00774 #endif 00775 return -1; 00776 } 00777 00778 struct pico_socket *pico_socket_open(uint16_t net, uint16_t proto, void (*wakeup)(uint16_t ev, struct pico_socket *)) 00779 { 00780 00781 struct pico_socket *s = NULL; 00782 00783 #ifdef PICO_SUPPORT_UDP 00784 if (proto == PICO_PROTO_UDP) { 00785 s = pico_udp_open(); 00786 s->proto = &pico_proto_udp; 00787 } 00788 #endif 00789 00790 #ifdef PICO_SUPPORT_TCP 00791 if (proto == PICO_PROTO_TCP) { 00792 s = pico_tcp_open(); 00793 s->proto = &pico_proto_tcp; 00794 /*check if Nagle enabled */ 00795 if (!IS_NAGLE_ENABLED(s)) 00796 dbg("ERROR Nagle should be enabled here\n\n"); 00797 } 00798 #endif 00799 00800 if (!s) { 00801 pico_err = PICO_ERR_EPROTONOSUPPORT; 00802 return NULL; 00803 } 00804 00805 #ifdef PICO_SUPPORT_IPV4 00806 if (net == PICO_PROTO_IPV4) 00807 s->net = &pico_proto_ipv4; 00808 #endif 00809 00810 #ifdef PICO_SUPPORT_IPV6 00811 if (net == PICO_PROTO_IPV6) 00812 s->net = &pico_proto_ipv6; 00813 #endif 00814 00815 s->q_in.max_size = PICO_DEFAULT_SOCKETQ; 00816 s->q_out.max_size = PICO_DEFAULT_SOCKETQ; 00817 s->wakeup = wakeup; 00818 00819 if (!s->net) { 00820 pico_free(s); 00821 pico_err = PICO_ERR_ENETUNREACH; 00822 return NULL; 00823 } 00824 return s; 00825 } 00826 00827 00828 struct pico_socket *pico_socket_clone(struct pico_socket *facsimile) 00829 { 00830 struct pico_socket *s = NULL; 00831 00832 #ifdef PICO_SUPPORT_UDP 00833 if (facsimile->proto->proto_number == PICO_PROTO_UDP) { 00834 s = pico_udp_open(); 00835 s->proto = &pico_proto_udp; 00836 } 00837 #endif 00838 00839 #ifdef PICO_SUPPORT_TCP 00840 if (facsimile->proto->proto_number == PICO_PROTO_TCP) { 00841 s = pico_tcp_open(); 00842 s->proto = &pico_proto_tcp; 00843 } 00844 #endif 00845 00846 if (!s) { 00847 pico_err = PICO_ERR_EPROTONOSUPPORT; 00848 return NULL; 00849 } 00850 s->local_port = facsimile->local_port; 00851 s->remote_port = facsimile->remote_port; 00852 s->state = facsimile->state; 00853 00854 #ifdef PICO_SUPPORT_IPV4 00855 if (facsimile->net == &pico_proto_ipv4) { 00856 s->net = &pico_proto_ipv4; 00857 memcpy(&s->local_addr, &facsimile->local_addr, sizeof(struct pico_ip4)); 00858 memcpy(&s->remote_addr, &facsimile->remote_addr, sizeof(struct pico_ip4)); 00859 } 00860 #endif 00861 00862 #ifdef PICO_SUPPORT_IPV6 00863 if (net == &pico_proto_ipv6) { 00864 s->net = &pico_proto_ipv6; 00865 memcpy(&s->local_addr, &facsimile->local_addr, sizeof(struct pico_ip6)); 00866 memcpy(&s->remote_addr, &facsimile->remote_addr, sizeof(struct pico_ip6)); 00867 } 00868 #endif 00869 s->q_in.max_size = PICO_DEFAULT_SOCKETQ; 00870 s->q_out.max_size = PICO_DEFAULT_SOCKETQ; 00871 s->wakeup = NULL; 00872 if (!s->net) { 00873 pico_free(s); 00874 pico_err = PICO_ERR_ENETUNREACH; 00875 return NULL; 00876 } 00877 return s; 00878 } 00879 00880 int pico_socket_read(struct pico_socket *s, void *buf, int len) 00881 { 00882 if (!s || buf == NULL) { 00883 pico_err = PICO_ERR_EINVAL; 00884 return -1; 00885 } else { 00886 /* check if exists in tree */ 00887 /* See task #178 */ 00888 if (pico_check_socket(s) != 0) { 00889 pico_err = PICO_ERR_EINVAL; 00890 return -1; 00891 } 00892 } 00893 00894 if ((s->state & PICO_SOCKET_STATE_BOUND) == 0) { 00895 pico_err = PICO_ERR_EIO; 00896 return -1; 00897 } 00898 #ifdef PICO_SUPPORT_UDP 00899 if (PROTO(s) == PICO_PROTO_UDP) 00900 return pico_udp_recv(s, buf, len, NULL, NULL); 00901 #endif 00902 00903 #ifdef PICO_SUPPORT_TCP 00904 if (PROTO(s) == PICO_PROTO_TCP){ 00905 /* check if in shutdown state and if no more data in tcpq_in */ 00906 if ((s->state & PICO_SOCKET_STATE_SHUT_REMOTE) && pico_tcp_queue_in_is_empty(s) ) { 00907 pico_err = PICO_ERR_ESHUTDOWN; 00908 return -1; 00909 } else { 00910 return pico_tcp_read(s, buf, len); 00911 } 00912 } 00913 #endif 00914 return 0; 00915 } 00916 00917 int pico_socket_write(struct pico_socket *s, void *buf, int len) 00918 { 00919 if (!s || buf == NULL) { 00920 pico_err = PICO_ERR_EINVAL; 00921 return -1; 00922 } else { 00923 /* check if exists in tree */ 00924 /* See task #178 */ 00925 if (pico_check_socket(s) != 0) { 00926 pico_err = PICO_ERR_EINVAL; 00927 return -1; 00928 } 00929 } 00930 00931 if ((s->state & PICO_SOCKET_STATE_BOUND) == 0) { 00932 pico_err = PICO_ERR_EIO; 00933 return -1; 00934 } 00935 if ((s->state & PICO_SOCKET_STATE_CONNECTED) == 0) { 00936 pico_err = PICO_ERR_ENOTCONN; 00937 return -1; 00938 } else if (s->state & PICO_SOCKET_STATE_SHUT_LOCAL) { /* check if in shutdown state */ 00939 pico_err = PICO_ERR_ESHUTDOWN; 00940 return -1; 00941 } else { 00942 return pico_socket_sendto(s, buf, len, &s->remote_addr, s->remote_port); 00943 } 00944 } 00945 00946 uint16_t pico_socket_high_port(uint16_t proto) 00947 { 00948 uint16_t port; 00949 if (0 || 00950 #ifdef PICO_SUPPORT_TCP 00951 (proto == PICO_PROTO_TCP) || 00952 #endif 00953 #ifdef PICO_SUPPORT_TCP 00954 (proto == PICO_PROTO_UDP) || 00955 #endif 00956 0) { 00957 do { 00958 uint32_t rand = pico_rand(); 00959 port = (uint16_t) (rand & 0xFFFFU); 00960 port = (uint16_t)(port % (65535 - 1024)) + 1024U; 00961 if (pico_is_port_free(proto, port, NULL, NULL)) { 00962 return short_be(port); 00963 } 00964 } while(1); 00965 } 00966 else return 0U; 00967 } 00968 00969 00970 int pico_socket_sendto(struct pico_socket *s, void *buf, int len, void *dst, uint16_t remote_port) 00971 { 00972 struct pico_frame *f; 00973 struct pico_remote_duple *remote_duple = NULL; 00974 int header_offset = 0; 00975 int total_payload_written = 0; 00976 #ifdef PICO_SUPPORT_IPV4 00977 struct pico_ip4 *src4; 00978 #endif 00979 00980 #ifdef PICO_SUPPORT_IPV6 00981 struct pico_ip6 *src6; 00982 #endif 00983 if (len == 0) { 00984 return 0; 00985 } else if (len < 0) { 00986 pico_err = PICO_ERR_EINVAL; 00987 return -1; 00988 } 00989 00990 if (buf == NULL || s == NULL) { 00991 pico_err = PICO_ERR_EINVAL; 00992 return -1; 00993 } 00994 00995 if (!dst || !remote_port) { 00996 pico_err = PICO_ERR_EADDRNOTAVAIL; 00997 return -1; 00998 } 00999 01000 if ((s->state & PICO_SOCKET_STATE_CONNECTED) != 0) { 01001 if (remote_port != s->remote_port) { 01002 pico_err = PICO_ERR_EINVAL; 01003 return -1; 01004 } 01005 } 01006 01007 #ifdef PICO_SUPPORT_IPV4 01008 if (IS_SOCK_IPV4(s)) { 01009 if ((s->state & PICO_SOCKET_STATE_CONNECTED)) { 01010 if (s->remote_addr.ip4.addr != ((struct pico_ip4 *)dst)->addr ) { 01011 pico_err = PICO_ERR_EADDRNOTAVAIL; 01012 return -1; 01013 } 01014 } else { 01015 src4 = pico_ipv4_source_find(dst); 01016 if (!src4) { 01017 pico_err = PICO_ERR_EHOSTUNREACH; 01018 return -1; 01019 } 01020 if (src4->addr != PICO_IPV4_INADDR_ANY) 01021 s->local_addr.ip4.addr = src4->addr; 01022 # ifdef PICO_SUPPORT_UDP 01023 /* socket remote info could change in a consecutive call, make persistent */ 01024 if (PROTO(s) == PICO_PROTO_UDP) { 01025 remote_duple = pico_zalloc(sizeof(struct pico_remote_duple)); 01026 remote_duple->remote_addr.ip4.addr = ((struct pico_ip4 *)dst)->addr; 01027 remote_duple->remote_port = remote_port; 01028 } 01029 # endif 01030 } 01031 } 01032 #endif 01033 01034 #ifdef PICO_SUPPORT_IPV6 01035 if (IS_SOCK_IPV6(s)) { 01036 if (s->state & PICO_SOCKET_STATE_CONNECTED) { 01037 if (memcmp(&s->remote_addr, dst, PICO_SIZE_IP6)) 01038 return -1; 01039 } else { 01040 src6 = pico_ipv6_source_find(dst); 01041 if (!src6) { 01042 pico_err = PICO_ERR_EHOSTUNREACH; 01043 return -1; 01044 } 01045 memcpy(&s->local_addr, src6, PICO_SIZE_IP6); 01046 memcpy(&s->remote_addr, dst, PICO_SIZE_IP6); 01047 # ifdef PICO_SUPPORT_UDP 01048 if (PROTO(s) == PICO_PROTO_UDP) { 01049 remote_duple = pico_zalloc(sizeof(struct pico_remote_duple)); 01050 remote_duple->remote_addr.ip6.addr = ((struct pico_ip6 *)dst)->addr; 01051 remote_duple->remote_port = remote_port; 01052 } 01053 # endif 01054 } 01055 } 01056 #endif 01057 01058 if ((s->state & PICO_SOCKET_STATE_BOUND) == 0) { 01059 s->local_port = pico_socket_high_port(s->proto->proto_number); 01060 if (s->local_port == 0) { 01061 pico_err = PICO_ERR_EINVAL; 01062 return -1; 01063 } 01064 } 01065 if ((s->state & PICO_SOCKET_STATE_CONNECTED) == 0) { 01066 s->remote_port = remote_port; 01067 } 01068 01069 #ifdef PICO_SUPPORT_TCP 01070 if (PROTO(s) == PICO_PROTO_TCP) 01071 header_offset = pico_tcp_overhead(s); 01072 #endif 01073 01074 #ifdef PICO_SUPPORT_UDP 01075 if (PROTO(s) == PICO_PROTO_UDP) 01076 header_offset = sizeof(struct pico_udp_hdr); 01077 #endif 01078 01079 while (total_payload_written < len) { 01080 int transport_len = (len - total_payload_written) + header_offset; 01081 if (transport_len > PICO_SOCKET_MTU) 01082 transport_len = PICO_SOCKET_MTU; 01083 #ifdef PICO_SUPPORT_IPFRAG 01084 else { 01085 if (total_payload_written) 01086 transport_len -= header_offset; /* last fragment, do not allocate memory for transport header */ 01087 } 01088 #endif /* PICO_SUPPORT_IPFRAG */ 01089 01090 f = pico_socket_frame_alloc(s, transport_len); 01091 if (!f) { 01092 pico_err = PICO_ERR_ENOMEM; 01093 return -1; 01094 } 01095 f->payload += header_offset; 01096 f->payload_len -= header_offset; 01097 f->sock = s; 01098 if (remote_duple) { 01099 f->info = pico_zalloc(sizeof(struct pico_remote_duple)); 01100 memcpy(f->info, remote_duple, sizeof(struct pico_remote_duple)); 01101 } 01102 01103 #ifdef PICO_SUPPORT_IPFRAG 01104 # ifdef PICO_SUPPORT_UDP 01105 if (PROTO(s) == PICO_PROTO_UDP && ((len + header_offset) > PICO_SOCKET_MTU)) { 01106 /* hacking way to identify fragmentation frames: payload != transport_hdr -> first frame */ 01107 if (!total_payload_written) { 01108 frag_dbg("FRAG: first fragmented frame %p | len = %u offset = 0\n", f, f->payload_len); 01109 /* transport header length field contains total length + header length */ 01110 f->transport_len = len + header_offset; 01111 f->frag = short_be(PICO_IPV4_MOREFRAG); 01112 } else { 01113 /* no transport header in fragmented IP */ 01114 f->payload = f->transport_hdr; 01115 f->payload_len += header_offset; 01116 /* set offset in octets */ 01117 f->frag = short_be((total_payload_written + header_offset) / 8); 01118 if (total_payload_written + f->payload_len < len) { 01119 frag_dbg("FRAG: intermediate fragmented frame %p | len = %u offset = %u\n", f, f->payload_len, short_be(f->frag)); 01120 f->frag |= short_be(PICO_IPV4_MOREFRAG); 01121 } else { 01122 frag_dbg("FRAG: last fragmented frame %p | len = %u offset = %u\n", f, f->payload_len, short_be(f->frag)); 01123 f->frag &= short_be(PICO_IPV4_FRAG_MASK); 01124 } 01125 } 01126 } else { 01127 f->frag = short_be(PICO_IPV4_DONTFRAG); 01128 } 01129 # endif /* PICO_SUPPORT_UDP */ 01130 #endif /* PICO_SUPPORT_IPFRAG */ 01131 01132 if (f->payload_len <= 0) { 01133 pico_frame_discard(f); 01134 if (remote_duple) 01135 pico_free(remote_duple); 01136 return total_payload_written; 01137 } 01138 01139 memcpy(f->payload, buf + total_payload_written, f->payload_len); 01140 //dbg("Pushing segment, hdr len: %d, payload_len: %d\n", header_offset, f->payload_len); 01141 01142 if (s->proto->push(s->proto, f) > 0) { 01143 total_payload_written += f->payload_len; 01144 } else { 01145 pico_frame_discard(f); 01146 pico_err = PICO_ERR_EAGAIN; 01147 break; 01148 } 01149 } 01150 if (remote_duple) 01151 pico_free(remote_duple); 01152 return total_payload_written; 01153 } 01154 01155 int pico_socket_send(struct pico_socket *s, void *buf, int len) 01156 { 01157 if (!s || buf == NULL) { 01158 pico_err = PICO_ERR_EINVAL; 01159 return -1; 01160 } else { 01161 /* check if exists in tree */ 01162 /* See task #178 */ 01163 if (pico_check_socket(s) != 0) { 01164 pico_err = PICO_ERR_EINVAL; 01165 return -1; 01166 } 01167 } 01168 01169 if ((s->state & PICO_SOCKET_STATE_CONNECTED) == 0) { 01170 pico_err = PICO_ERR_ENOTCONN; 01171 return -1; 01172 } 01173 return pico_socket_sendto(s, buf, len, &s->remote_addr, s->remote_port); 01174 } 01175 01176 int pico_socket_recvfrom(struct pico_socket *s, void *buf, int len, void *orig, uint16_t *remote_port) 01177 { 01178 if (!s || buf == NULL) { /// || orig == NULL || remote_port == NULL) { 01179 pico_err = PICO_ERR_EINVAL; 01180 return -1; 01181 } else { 01182 /* check if exists in tree */ 01183 if (pico_check_socket(s) != 0) { 01184 pico_err = PICO_ERR_EINVAL; 01185 /* See task #178 */ 01186 return -1; 01187 } 01188 } 01189 01190 if ((s->state & PICO_SOCKET_STATE_BOUND) == 0) { 01191 pico_err = PICO_ERR_EADDRNOTAVAIL; 01192 return -1; 01193 } 01194 #ifdef PICO_SUPPORT_UDP 01195 if (PROTO(s) == PICO_PROTO_UDP) { 01196 return pico_udp_recv(s, buf, len, orig, remote_port); 01197 } 01198 #endif 01199 #ifdef PICO_SUPPORT_TCP 01200 if (PROTO(s) == PICO_PROTO_TCP) { 01201 /* check if in shutdown state and if tcpq_in empty */ 01202 if ((s->state & PICO_SOCKET_STATE_SHUT_REMOTE) && pico_tcp_queue_in_is_empty(s)) { 01203 pico_err = PICO_ERR_ESHUTDOWN; 01204 return -1; 01205 } else { 01206 //dbg("socket tcp recv\n"); 01207 return pico_tcp_read(s, buf, len); 01208 } 01209 } 01210 #endif 01211 //dbg("socket return 0\n"); 01212 return 0; 01213 } 01214 01215 int pico_socket_recv(struct pico_socket *s, void *buf, int len) 01216 { 01217 return pico_socket_recvfrom(s, buf, len, NULL, NULL); 01218 } 01219 01220 01221 int pico_socket_bind(struct pico_socket *s, void *local_addr, uint16_t *port) 01222 { 01223 if (!s || !local_addr || !port) { 01224 pico_err = PICO_ERR_EINVAL; 01225 return -1; 01226 } 01227 01228 if (!is_sock_ipv6(s)) { 01229 struct pico_ip4 *ip = (struct pico_ip4 *)local_addr; 01230 if (ip->addr != PICO_IPV4_INADDR_ANY) { 01231 if (!pico_ipv4_link_find(local_addr)) { 01232 pico_err = PICO_ERR_EINVAL; 01233 return -1; 01234 } 01235 } 01236 } else { 01237 /*... IPv6 */ 01238 } 01239 01240 01241 /* When given port = 0, get a random high port to bind to. */ 01242 if (*port == 0) { 01243 *port = pico_socket_high_port(PROTO(s)); 01244 if (*port == 0) { 01245 pico_err = PICO_ERR_EINVAL; 01246 return -1; 01247 } 01248 } 01249 01250 if (pico_is_port_free(PROTO(s), *port, local_addr, s->net) == 0) { 01251 pico_err = PICO_ERR_EADDRINUSE; 01252 return -1; 01253 } 01254 s->local_port = *port; 01255 01256 if (is_sock_ipv6(s)) { 01257 struct pico_ip6 *ip = (struct pico_ip6 *) local_addr; 01258 memcpy(s->local_addr.ip6.addr, ip, PICO_SIZE_IP6); 01259 /* XXX: port ipv4 functionality to ipv6 */ 01260 /* Check for port already in use */ 01261 if (pico_is_port_free(PROTO(s), *port, &local_addr, s->net)) { 01262 pico_err = PICO_ERR_EADDRINUSE; 01263 return -1; 01264 } 01265 } else if (is_sock_ipv4(s)) { 01266 struct pico_ip4 *ip = (struct pico_ip4 *) local_addr; 01267 s->local_addr.ip4.addr = ip->addr; 01268 } 01269 return pico_socket_alter_state(s, PICO_SOCKET_STATE_BOUND, 0, 0); 01270 } 01271 01272 int pico_socket_connect(struct pico_socket *s, void *remote_addr, uint16_t remote_port) 01273 { 01274 int ret = -1; 01275 pico_err = PICO_ERR_EPROTONOSUPPORT; 01276 if (!s || remote_addr == NULL || remote_port == 0) { 01277 pico_err = PICO_ERR_EINVAL; 01278 return -1; 01279 } 01280 01281 s->remote_port = remote_port; 01282 01283 if (s->local_port == 0) { 01284 s->local_port = pico_socket_high_port(PROTO(s)); 01285 if (!s->local_port) { 01286 pico_err = PICO_ERR_EINVAL; 01287 return -1; 01288 } 01289 } 01290 01291 if (is_sock_ipv6(s)) { 01292 struct pico_ip6 *ip = (struct pico_ip6 *) remote_addr; 01293 memcpy(s->remote_addr.ip6.addr, ip, PICO_SIZE_IP6); 01294 } else if (is_sock_ipv4(s)) { 01295 struct pico_ip4 *local, *ip = (struct pico_ip4 *) remote_addr; 01296 s->remote_addr.ip4.addr = ip->addr; 01297 local = pico_ipv4_source_find(ip); 01298 if (local) { 01299 s->local_addr.ip4.addr = local->addr; 01300 } else { 01301 pico_err = PICO_ERR_EHOSTUNREACH; 01302 return -1; 01303 } 01304 } 01305 01306 pico_socket_alter_state(s, PICO_SOCKET_STATE_BOUND, 0, 0); 01307 01308 #ifdef PICO_SUPPORT_UDP 01309 if (PROTO(s) == PICO_PROTO_UDP) { 01310 pico_socket_alter_state(s, PICO_SOCKET_STATE_CONNECTED, 0, 0); 01311 pico_err = PICO_ERR_NOERR; 01312 ret = 0; 01313 } 01314 #endif 01315 01316 #ifdef PICO_SUPPORT_TCP 01317 if (PROTO(s) == PICO_PROTO_TCP) { 01318 if (pico_tcp_initconn(s) == 0) { 01319 pico_socket_alter_state(s, PICO_SOCKET_STATE_CONNECTED | PICO_SOCKET_STATE_TCP_SYN_SENT, 0, 0); 01320 pico_err = PICO_ERR_NOERR; 01321 ret = 0; 01322 } else { 01323 pico_err = PICO_ERR_EHOSTUNREACH; 01324 } 01325 } 01326 #endif 01327 01328 return ret; 01329 } 01330 01331 #ifdef PICO_SUPPORT_TCP 01332 01333 int pico_socket_listen(struct pico_socket *s, int backlog) 01334 { 01335 if (!s || backlog < 1) { 01336 pico_err = PICO_ERR_EINVAL; 01337 return -1; 01338 } else { 01339 /* check if exists in tree */ 01340 /* See task #178 */ 01341 if (pico_check_socket(s) != 0) { 01342 pico_err = PICO_ERR_EINVAL; 01343 return -1; 01344 } 01345 } 01346 01347 if (PROTO(s) == PICO_PROTO_UDP) { 01348 pico_err = PICO_ERR_EINVAL; 01349 return -1; 01350 } 01351 01352 if ((s->state & PICO_SOCKET_STATE_BOUND) == 0) { 01353 pico_err = PICO_ERR_EISCONN; 01354 return -1; 01355 } 01356 01357 if (backlog < 1) { 01358 pico_err = PICO_ERR_EINVAL; 01359 return -1; 01360 } 01361 01362 if (PROTO(s) == PICO_PROTO_TCP) 01363 pico_socket_alter_state(s, PICO_SOCKET_STATE_TCP_SYN_SENT, 0, PICO_SOCKET_STATE_TCP_LISTEN); 01364 s->max_backlog = backlog; 01365 01366 return 0; 01367 } 01368 01369 struct pico_socket *pico_socket_accept(struct pico_socket *s, void *orig, uint16_t *port) 01370 { 01371 if (!s || !orig || !port) { 01372 pico_err = PICO_ERR_EINVAL; 01373 return NULL; 01374 } 01375 01376 pico_err = PICO_ERR_EINVAL; 01377 01378 if ((s->state & PICO_SOCKET_STATE_BOUND) == 0) { 01379 return NULL; 01380 } 01381 01382 if (PROTO(s) == PICO_PROTO_UDP) { 01383 return NULL; 01384 } 01385 01386 if (TCPSTATE(s) == PICO_SOCKET_STATE_TCP_LISTEN) { 01387 struct pico_sockport *sp = pico_get_sockport(PICO_PROTO_TCP, s->local_port); 01388 struct pico_socket *found; 01389 /* If at this point no incoming connection socket is found, 01390 * the accept call is valid, but no connection is established yet. 01391 */ 01392 pico_err = PICO_ERR_EAGAIN; 01393 if (sp) { 01394 struct pico_tree_node * index; 01395 //RB_FOREACH(found, socket_tree, &sp->socks) { 01396 pico_tree_foreach(index,&sp->socks){ 01397 found = index->keyValue; 01398 if (s == found->parent) { 01399 found->parent = NULL; 01400 pico_err = PICO_ERR_NOERR; 01401 memcpy(orig, &found->remote_addr, sizeof(struct pico_ip4)); 01402 *port = found->remote_port; 01403 return found; 01404 } 01405 } 01406 } 01407 } 01408 return NULL; 01409 } 01410 01411 #else 01412 01413 int pico_socket_listen(struct pico_socket *s, int backlog) 01414 { 01415 pico_err = PICO_ERR_EINVAL; 01416 return -1; 01417 } 01418 01419 struct pico_socket *pico_socket_accept(struct pico_socket *s, void *orig, uint16_t *local_port) 01420 { 01421 pico_err = PICO_ERR_EINVAL; 01422 return NULL; 01423 } 01424 01425 #endif 01426 01427 #define PICO_SOCKET_SETOPT_EN(socket,index) (socket->opt_flags |= (1 << index)) 01428 #define PICO_SOCKET_SETOPT_DIS(socket,index) (socket->opt_flags &= ~(1 << index)) 01429 01430 int pico_socket_setoption(struct pico_socket *s, int option, void *value) // XXX no check against proto (vs setsockopt) or implicit by socket? 01431 { 01432 if (s == NULL) { 01433 pico_err = PICO_ERR_EINVAL; 01434 return -1; 01435 } 01436 01437 pico_err = PICO_ERR_NOERR; 01438 01439 switch (option) 01440 { 01441 #ifdef PICO_SUPPORT_TCP 01442 case PICO_TCP_NODELAY: 01443 if (!value) { 01444 pico_err = PICO_ERR_EINVAL; 01445 return -1; 01446 } 01447 if (s->proto->proto_number == PICO_PROTO_TCP) { 01448 int *val = (int*)value; 01449 if (*val > 0) { 01450 dbg("setsockopt: Nagle algorithm disabled.\n"); 01451 PICO_SOCKET_SETOPT_EN(s,PICO_SOCKET_OPT_TCPNODELAY); 01452 } else { 01453 dbg("setsockopt: Nagle algorithm enabled.\n"); 01454 PICO_SOCKET_SETOPT_DIS(s,PICO_SOCKET_OPT_TCPNODELAY); 01455 } 01456 } else { 01457 pico_err = PICO_ERR_EINVAL; 01458 } 01459 break; 01460 #endif 01461 01462 01463 #ifdef PICO_SUPPORT_MCAST 01464 case PICO_IP_MULTICAST_IF: 01465 pico_err = PICO_ERR_EOPNOTSUPP; 01466 return -1; 01467 break; 01468 01469 case PICO_IP_MULTICAST_TTL: 01470 if (s->proto->proto_number == PICO_PROTO_UDP) { 01471 return pico_udp_set_mc_ttl(s, *((uint8_t *) value)); 01472 } 01473 break; 01474 01475 case PICO_IP_MULTICAST_LOOP: 01476 if (s->proto->proto_number == PICO_PROTO_UDP) { 01477 switch (*(uint8_t *) value) 01478 { 01479 case 0: 01480 /* do not loop back multicast datagram */ 01481 PICO_SOCKET_SETOPT_DIS(s,PICO_SOCKET_OPT_MULTICAST_LOOP); 01482 break; 01483 01484 case 1: 01485 /* do loop back multicast datagram */ 01486 PICO_SOCKET_SETOPT_EN(s,PICO_SOCKET_OPT_MULTICAST_LOOP); 01487 break; 01488 01489 default: 01490 pico_err = PICO_ERR_EINVAL; 01491 return -1; 01492 } 01493 } 01494 break; 01495 01496 case PICO_IP_ADD_MEMBERSHIP: 01497 /* EXCLUDE mode */ 01498 if (s->proto->proto_number == PICO_PROTO_UDP) { 01499 uint8_t filter_mode = 0; 01500 struct pico_ip_mreq *mreq = NULL; 01501 struct pico_mcast_listen *listen = NULL, ltest = {0}; 01502 struct pico_ipv4_link *mcast_link = NULL; 01503 01504 if (!value) { 01505 pico_err = PICO_ERR_EINVAL; 01506 return -1; 01507 } 01508 mreq = (struct pico_ip_mreq *) value; 01509 mcast_link = pico_socket_setoption_mcastargs_validation(mreq, NULL); 01510 if (!mcast_link) { 01511 pico_err = PICO_ERR_EINVAL; 01512 return -1; 01513 } 01514 if (!mreq->mcast_link_addr.addr) 01515 mreq->mcast_link_addr.addr = mcast_link->address.addr; 01516 01517 if (!s->MCASTListen) { /* No RBTree allocated yet */ 01518 s->MCASTListen = pico_zalloc(sizeof(struct pico_tree)); 01519 if (!s->MCASTListen) { 01520 pico_err = PICO_ERR_ENOMEM; 01521 return -1; 01522 } 01523 s->MCASTListen->root = &LEAF; 01524 s->MCASTListen->compare = mcast_listen_cmp; 01525 } 01526 ltest.mcast_link = mreq->mcast_link_addr; 01527 ltest.mcast_group = mreq->mcast_group_addr; 01528 listen = pico_tree_findKey(s->MCASTListen, <est); 01529 if (listen) { 01530 if (listen->filter_mode != PICO_IP_MULTICAST_EXCLUDE) { 01531 so_mcast_dbg("pico_socket_setoption: ERROR any-source multicast (exclude) on source-specific multicast (include)\n"); 01532 pico_err = PICO_ERR_EINVAL; 01533 return -1; 01534 } else { 01535 so_mcast_dbg("pico_socket_setoption: ERROR duplicate PICO_IP_ADD_MEMBERSHIP\n"); 01536 pico_err = PICO_ERR_EINVAL; 01537 return -1; 01538 } 01539 } else { 01540 listen = pico_zalloc(sizeof(struct pico_mcast_listen)); 01541 if (!listen) { 01542 pico_err = PICO_ERR_ENOMEM; 01543 return -1; 01544 } 01545 listen->filter_mode = PICO_IP_MULTICAST_EXCLUDE; 01546 listen->mcast_link = mreq->mcast_link_addr; 01547 listen->mcast_group = mreq->mcast_group_addr; 01548 listen->MCASTSources.root = &LEAF; 01549 listen->MCASTSources.compare = mcast_sources_cmp; 01550 pico_tree_insert(s->MCASTListen, listen); 01551 } 01552 01553 pico_tree_insert(&MCASTSockets, s); 01554 filter_mode = pico_socket_aggregate_mcastfilters(&mcast_link->address, &mreq->mcast_group_addr); 01555 if (filter_mode < 0) 01556 return -1; 01557 01558 return pico_ipv4_mcast_join(&mreq->mcast_link_addr, &mreq->mcast_group_addr, 1, filter_mode, &MCASTFilter); 01559 } 01560 break; 01561 01562 case PICO_IP_DROP_MEMBERSHIP: 01563 /* EXCLUDE mode */ 01564 if (s->proto->proto_number == PICO_PROTO_UDP) { 01565 uint8_t filter_mode = 0; 01566 struct pico_ip_mreq *mreq = NULL; 01567 struct pico_mcast_listen *listen = NULL, ltest = {0}; 01568 struct pico_ip4 *source = NULL; 01569 struct pico_ipv4_link *mcast_link = NULL; 01570 struct pico_tree_node *index, *_tmp; 01571 01572 if (!value) { 01573 pico_err = PICO_ERR_EINVAL; 01574 return -1; 01575 } 01576 mreq = (struct pico_ip_mreq *) value; 01577 mcast_link = pico_socket_setoption_mcastargs_validation(mreq, NULL); 01578 if (!mcast_link) { 01579 pico_err = PICO_ERR_EINVAL; 01580 return -1; 01581 } 01582 if (!mreq->mcast_link_addr.addr) 01583 mreq->mcast_link_addr.addr = mcast_link->address.addr; 01584 01585 if (!s->MCASTListen) { /* No RBTree allocated yet */ 01586 so_mcast_dbg("socket_setoption: ERROR PICO_IP_DROP_MEMBERSHIP before any PICO_IP_ADD_MEMBERSHIP/SOURCE_MEMBERSHIP\n"); 01587 pico_err = PICO_ERR_EINVAL; 01588 return -1; 01589 } 01590 ltest.mcast_link = mreq->mcast_link_addr; 01591 ltest.mcast_group = mreq->mcast_group_addr; 01592 listen = pico_tree_findKey(s->MCASTListen, <est); 01593 if (!listen) { 01594 so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_DROP_MEMBERSHIP before PICO_IP_ADD_MEMBERSHIP/SOURCE_MEMBERSHIP\n"); 01595 pico_err = PICO_ERR_EADDRNOTAVAIL; 01596 return -1; 01597 } else { 01598 pico_tree_foreach_safe(index, &listen->MCASTSources, _tmp) 01599 { 01600 source = index->keyValue; 01601 pico_tree_delete(&listen->MCASTSources, source); 01602 pico_free(source); 01603 } 01604 pico_tree_delete(s->MCASTListen, listen); 01605 pico_free(listen); 01606 if (pico_tree_empty(s->MCASTListen)) { 01607 pico_free(s->MCASTListen); 01608 s->MCASTListen = NULL; 01609 pico_tree_delete(&MCASTSockets, s); 01610 } 01611 } 01612 01613 filter_mode = pico_socket_aggregate_mcastfilters(&mcast_link->address, &mreq->mcast_group_addr); 01614 if (filter_mode < 0) 01615 return -1; 01616 01617 return pico_ipv4_mcast_leave(&mreq->mcast_link_addr, &mreq->mcast_group_addr, 1, filter_mode, &MCASTFilter); 01618 } 01619 break; 01620 01621 case PICO_IP_UNBLOCK_SOURCE: 01622 /* EXCLUDE mode */ 01623 if (s->proto->proto_number == PICO_PROTO_UDP) { 01624 uint8_t filter_mode = 0; 01625 struct pico_ip_mreq_source *mreq = NULL; 01626 struct pico_mcast_listen *listen = NULL, ltest = {0}; 01627 struct pico_ip4 *source = NULL, stest = {0}; 01628 struct pico_ipv4_link *mcast_link = NULL; 01629 01630 if (!value) { 01631 pico_err = PICO_ERR_EINVAL; 01632 return -1; 01633 } 01634 mreq = (struct pico_ip_mreq_source *) value; 01635 mcast_link = pico_socket_setoption_mcastargs_validation(NULL, mreq); 01636 if (!mcast_link) { 01637 pico_err = PICO_ERR_EINVAL; 01638 return -1; 01639 } 01640 if (!mreq->mcast_link_addr.addr) 01641 mreq->mcast_link_addr.addr = mcast_link->address.addr; 01642 01643 if (!s->MCASTListen) { /* No RBTree allocated yet */ 01644 so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_UNBLOCK_SOURCE before any PICO_IP_ADD_MEMBERSHIP\n"); 01645 pico_err = PICO_ERR_EINVAL; 01646 return -1; 01647 } 01648 ltest.mcast_link = mreq->mcast_link_addr; 01649 ltest.mcast_group = mreq->mcast_group_addr; 01650 listen = pico_tree_findKey(s->MCASTListen, <est); 01651 if (!listen) { 01652 so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_UNBLOCK_SOURCE before PICO_IP_ADD_MEMBERSHIP\n"); 01653 pico_err = PICO_ERR_EINVAL; 01654 return -1; 01655 } else { 01656 if (listen->filter_mode != PICO_IP_MULTICAST_EXCLUDE) { 01657 so_mcast_dbg("pico_socket_setoption: ERROR any-source multicast (exclude) on source-specific multicast (include)\n"); 01658 pico_err = PICO_ERR_EINVAL; 01659 return -1; 01660 } 01661 stest.addr = mreq->mcast_source_addr.addr; 01662 source = pico_tree_findKey(&listen->MCASTSources, &stest); 01663 if (!source) { 01664 so_mcast_dbg("pico_socket_setoption: ERROR address to unblock not in source list\n"); 01665 pico_err = PICO_ERR_EADDRNOTAVAIL; 01666 return -1; 01667 } else { 01668 pico_tree_delete(&listen->MCASTSources, source); 01669 pico_free(source); 01670 } 01671 } 01672 01673 filter_mode = pico_socket_aggregate_mcastfilters(&mcast_link->address, &mreq->mcast_group_addr); 01674 if (filter_mode < 0) 01675 return -1; 01676 01677 return pico_ipv4_mcast_leave(&mreq->mcast_link_addr, &mreq->mcast_group_addr, 0, filter_mode, &MCASTFilter); 01678 } 01679 break; 01680 01681 case PICO_IP_BLOCK_SOURCE: 01682 /* EXCLUDE mode */ 01683 if (s->proto->proto_number == PICO_PROTO_UDP) { 01684 uint8_t filter_mode = 0; 01685 struct pico_ip_mreq_source *mreq = NULL; 01686 struct pico_mcast_listen *listen = NULL, ltest = {0}; 01687 struct pico_ip4 *source = NULL, stest = {0}; 01688 struct pico_ipv4_link *mcast_link = NULL; 01689 01690 if (!value) { 01691 pico_err = PICO_ERR_EINVAL; 01692 return -1; 01693 } 01694 mreq = (struct pico_ip_mreq_source *) value; 01695 mcast_link = pico_socket_setoption_mcastargs_validation(NULL, mreq); 01696 if (!mcast_link) { 01697 pico_err = PICO_ERR_EINVAL; 01698 return -1; 01699 } 01700 if (!mreq->mcast_link_addr.addr) 01701 mreq->mcast_link_addr.addr = mcast_link->address.addr; 01702 01703 if (!s->MCASTListen) { /* No RBTree allocated yet */ 01704 so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_BLOCK_SOURCE before any PICO_IP_ADD_MEMBERSHIP\n"); 01705 pico_err = PICO_ERR_EINVAL; 01706 return -1; 01707 } 01708 ltest.mcast_link = mreq->mcast_link_addr; 01709 ltest.mcast_group = mreq->mcast_group_addr; 01710 listen = pico_tree_findKey(s->MCASTListen, <est); 01711 if (!listen) { 01712 so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_BLOCK_SOURCE before PICO_IP_ADD_MEMBERSHIP\n"); 01713 pico_err = PICO_ERR_EINVAL; 01714 return -1; 01715 } else { 01716 if (listen->filter_mode != PICO_IP_MULTICAST_EXCLUDE) { 01717 so_mcast_dbg("pico_socket_setoption: ERROR any-source multicast (exclude) on source-specific multicast (include)\n"); 01718 pico_err = PICO_ERR_EINVAL; 01719 return -1; 01720 } 01721 stest.addr = mreq->mcast_source_addr.addr; 01722 source = pico_tree_findKey(&listen->MCASTSources, &stest); 01723 if (source) { 01724 so_mcast_dbg("pico_socket_setoption: ERROR address to block already in source list\n"); 01725 pico_err = PICO_ERR_EADDRNOTAVAIL; 01726 return -1; 01727 } else { 01728 source = pico_zalloc(sizeof(struct pico_ip4)); 01729 if (!source) { 01730 pico_err = PICO_ERR_ENOMEM; 01731 return -1; 01732 } 01733 source->addr = mreq->mcast_source_addr.addr; 01734 pico_tree_insert(&listen->MCASTSources, source); 01735 } 01736 } 01737 01738 filter_mode = pico_socket_aggregate_mcastfilters(&mcast_link->address, &mreq->mcast_group_addr); 01739 if (filter_mode < 0) 01740 return -1; 01741 01742 return pico_ipv4_mcast_join(&mreq->mcast_link_addr, &mreq->mcast_group_addr, 0, filter_mode, &MCASTFilter); 01743 } 01744 break; 01745 01746 case PICO_IP_ADD_SOURCE_MEMBERSHIP: 01747 /* INCLUDE mode */ 01748 if (s->proto->proto_number == PICO_PROTO_UDP) { 01749 uint8_t filter_mode = 0, reference_count = 0; 01750 struct pico_ip_mreq_source *mreq = NULL; 01751 struct pico_mcast_listen *listen = NULL, ltest = {0}; 01752 struct pico_ip4 *source = NULL, stest = {0}; 01753 struct pico_ipv4_link *mcast_link = NULL; 01754 01755 if (!value) { 01756 pico_err = PICO_ERR_EINVAL; 01757 return -1; 01758 } 01759 mreq = (struct pico_ip_mreq_source *) value; 01760 mcast_link = pico_socket_setoption_mcastargs_validation(NULL, mreq); 01761 if (!mcast_link) { 01762 pico_err = PICO_ERR_EINVAL; 01763 return -1; 01764 } 01765 if (!mreq->mcast_link_addr.addr) 01766 mreq->mcast_link_addr.addr = mcast_link->address.addr; 01767 01768 if (!s->MCASTListen) { /* No RBTree allocated yet */ 01769 s->MCASTListen = pico_zalloc(sizeof(struct pico_tree)); 01770 if (!s->MCASTListen) { 01771 pico_err = PICO_ERR_ENOMEM; 01772 return -1; 01773 } 01774 s->MCASTListen->root = &LEAF; 01775 s->MCASTListen->compare = mcast_listen_cmp; 01776 } 01777 ltest.mcast_link = mreq->mcast_link_addr; 01778 ltest.mcast_group = mreq->mcast_group_addr; 01779 listen = pico_tree_findKey(s->MCASTListen, <est); 01780 if (listen) { 01781 if (listen->filter_mode != PICO_IP_MULTICAST_INCLUDE) { 01782 so_mcast_dbg("pico_socket_setoption: ERROR source-specific multicast (include) on any-source multicast (exclude)\n"); 01783 pico_err = PICO_ERR_EINVAL; 01784 return -1; 01785 } 01786 stest.addr = mreq->mcast_source_addr.addr; 01787 source = pico_tree_findKey(&listen->MCASTSources, &stest); 01788 if (source) { 01789 so_mcast_dbg("pico_socket_setoption: ERROR source address to allow already in source list\n"); 01790 pico_err = PICO_ERR_EADDRNOTAVAIL; 01791 return -1; 01792 } else { 01793 source = pico_zalloc(sizeof(struct pico_ip4)); 01794 if (!source) { 01795 pico_err = PICO_ERR_ENOMEM; 01796 return -1; 01797 } 01798 source->addr = mreq->mcast_source_addr.addr; 01799 pico_tree_insert(&listen->MCASTSources, source); 01800 } 01801 } else { 01802 listen = pico_zalloc(sizeof(struct pico_mcast_listen)); 01803 if (!listen) { 01804 pico_err = PICO_ERR_ENOMEM; 01805 return -1; 01806 } 01807 listen->filter_mode = PICO_IP_MULTICAST_INCLUDE; 01808 listen->mcast_link = mreq->mcast_link_addr; 01809 listen->mcast_group = mreq->mcast_group_addr; 01810 listen->MCASTSources.root = &LEAF; 01811 listen->MCASTSources.compare = mcast_sources_cmp; 01812 source = pico_zalloc(sizeof(struct pico_ip4)); 01813 if (!source) { 01814 pico_free(listen); 01815 pico_err = PICO_ERR_ENOMEM; 01816 return -1; 01817 } 01818 source->addr = mreq->mcast_source_addr.addr; 01819 pico_tree_insert(&listen->MCASTSources, source); 01820 pico_tree_insert(s->MCASTListen, listen); 01821 reference_count = 1; 01822 } 01823 01824 pico_tree_insert(&MCASTSockets, s); 01825 filter_mode = pico_socket_aggregate_mcastfilters(&mcast_link->address, &mreq->mcast_group_addr); 01826 if (filter_mode < 0) 01827 return -1; 01828 01829 return pico_ipv4_mcast_join(&mreq->mcast_link_addr, &mreq->mcast_group_addr, reference_count, filter_mode, &MCASTFilter); 01830 } 01831 break; 01832 01833 case PICO_IP_DROP_SOURCE_MEMBERSHIP: 01834 /* INCLUDE mode */ 01835 if (s->proto->proto_number == PICO_PROTO_UDP) { 01836 uint8_t filter_mode = 0, reference_count = 0; 01837 struct pico_ip_mreq_source *mreq = NULL; 01838 struct pico_mcast_listen *listen = NULL, ltest = {0}; 01839 struct pico_ip4 *source = NULL, stest = {0}; 01840 struct pico_ipv4_link *mcast_link = NULL; 01841 01842 if (!value) { 01843 pico_err = PICO_ERR_EINVAL; 01844 return -1; 01845 } 01846 mreq = (struct pico_ip_mreq_source *) value; 01847 mcast_link = pico_socket_setoption_mcastargs_validation(NULL, mreq); 01848 if (!mcast_link) { 01849 pico_err = PICO_ERR_EINVAL; 01850 return -1; 01851 } 01852 if (!mreq->mcast_link_addr.addr) 01853 mreq->mcast_link_addr.addr = mcast_link->address.addr; 01854 01855 if (!s->MCASTListen) { /* No RBTree allocated yet */ 01856 so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_DROP_SOURCE_MEMBERSHIP before any PICO_IP_ADD_SOURCE_MEMBERSHIP\n"); 01857 pico_err = PICO_ERR_EINVAL; 01858 return -1; 01859 } 01860 ltest.mcast_link = mreq->mcast_link_addr; 01861 ltest.mcast_group = mreq->mcast_group_addr; 01862 listen = pico_tree_findKey(s->MCASTListen, <est); 01863 if (!listen) { 01864 so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_DROP_SOURCE_MEMBERSHIP before PICO_IP_ADD_SOURCE_MEMBERSHIP\n"); 01865 pico_err = PICO_ERR_EADDRNOTAVAIL; 01866 return -1; 01867 } else { 01868 if (listen->filter_mode != PICO_IP_MULTICAST_INCLUDE) { 01869 so_mcast_dbg("pico_socket_setoption: ERROR source-specific multicast (include) on any-source multicast (exclude)\n"); 01870 pico_err = PICO_ERR_EINVAL; 01871 return -1; 01872 } 01873 stest.addr = mreq->mcast_source_addr.addr; 01874 source = pico_tree_findKey(&listen->MCASTSources, &stest); 01875 if (!source) { 01876 so_mcast_dbg("pico_socket_setoption: ERROR address to drop not in source list\n"); 01877 pico_err = PICO_ERR_EADDRNOTAVAIL; 01878 return -1; 01879 } else { 01880 pico_tree_delete(&listen->MCASTSources, source); 01881 pico_free(source); 01882 if (pico_tree_empty(&listen->MCASTSources)) { /* 1 if empty, 0 otherwise */ 01883 reference_count = 1; 01884 pico_tree_delete(s->MCASTListen, listen); 01885 pico_free(listen); 01886 if (pico_tree_empty(s->MCASTListen)) { 01887 pico_free(s->MCASTListen); 01888 s->MCASTListen = NULL; 01889 pico_tree_delete(&MCASTSockets, s); 01890 } 01891 } 01892 } 01893 } 01894 01895 filter_mode = pico_socket_aggregate_mcastfilters(&mcast_link->address, &mreq->mcast_group_addr); 01896 if (filter_mode < 0) 01897 return -1; 01898 01899 return pico_ipv4_mcast_leave(&mreq->mcast_link_addr, &mreq->mcast_group_addr, reference_count, filter_mode, &MCASTFilter); 01900 } 01901 break; 01902 #endif /* PICO_SUPPORT_MCAST */ 01903 01904 default: 01905 pico_err = PICO_ERR_EINVAL; 01906 return -1; 01907 } 01908 01909 if (pico_err != PICO_ERR_NOERR) 01910 return -1; 01911 else 01912 return 0; 01913 } 01914 01915 #define PICO_SOCKET_GETOPT(socket,index) ((socket->opt_flags & (1 << index)) != 0) 01916 01917 int pico_socket_getoption(struct pico_socket *s, int option, void *value) 01918 { 01919 if (!s || !value) { 01920 pico_err = PICO_ERR_EINVAL; 01921 return -1; 01922 } 01923 01924 switch (option) 01925 { 01926 #ifdef PICO_SUPPORT_TCP 01927 case PICO_TCP_NODELAY: 01928 if (s->proto->proto_number == PICO_PROTO_TCP) 01929 /* state of the NODELAY option */ 01930 *(int *)value = PICO_SOCKET_GETOPT(s,PICO_SOCKET_OPT_TCPNODELAY); 01931 else 01932 *(int *)value = 0; 01933 break; 01934 #endif 01935 01936 #ifdef PICO_SUPPORT_MCAST 01937 case PICO_IP_MULTICAST_IF: 01938 pico_err = PICO_ERR_EOPNOTSUPP; 01939 return -1; 01940 break; 01941 01942 case PICO_IP_MULTICAST_TTL: 01943 if (s->proto->proto_number == PICO_PROTO_UDP) { 01944 pico_udp_get_mc_ttl(s, (uint8_t *) value); 01945 } else { 01946 *(uint8_t *)value = 0; 01947 pico_err = PICO_ERR_EINVAL; 01948 return -1; 01949 } 01950 break; 01951 01952 case PICO_IP_MULTICAST_LOOP: 01953 if (s->proto->proto_number == PICO_PROTO_UDP) { 01954 *(uint8_t *)value = PICO_SOCKET_GETOPT(s,PICO_SOCKET_OPT_MULTICAST_LOOP); 01955 } else { 01956 *(uint8_t *)value = 0; 01957 pico_err = PICO_ERR_EINVAL; 01958 return -1; 01959 } 01960 break; 01961 #endif /* PICO_SUPPORT_MCAST */ 01962 01963 default: 01964 pico_err = PICO_ERR_EINVAL; 01965 return -1; 01966 } 01967 01968 return 0; 01969 } 01970 01971 01972 int pico_socket_shutdown(struct pico_socket *s, int mode) 01973 { 01974 if (!s) { 01975 pico_err = PICO_ERR_EINVAL; 01976 return -1; 01977 } else { 01978 /* check if exists in tree */ 01979 /* See task #178 */ 01980 if (pico_check_socket(s) != 0) { 01981 pico_free(s); /* close socket after bind or connect failed */ 01982 return 0; 01983 } 01984 } 01985 01986 #ifdef PICO_SUPPORT_UDP 01987 if (PROTO(s) == PICO_PROTO_UDP) { 01988 if (mode & PICO_SHUT_RDWR) 01989 pico_socket_alter_state(s, PICO_SOCKET_STATE_CLOSED, PICO_SOCKET_STATE_CLOSING |PICO_SOCKET_STATE_BOUND | PICO_SOCKET_STATE_CONNECTED, 0); 01990 else if (mode & PICO_SHUT_RD) 01991 pico_socket_alter_state(s, PICO_SOCKET_STATE_BOUND, 0, 0); 01992 } 01993 #endif 01994 #ifdef PICO_SUPPORT_TCP 01995 if (PROTO(s) == PICO_PROTO_TCP) { 01996 if(mode & PICO_SHUT_RDWR) 01997 pico_socket_alter_state(s, PICO_SOCKET_STATE_SHUT_LOCAL | PICO_SOCKET_STATE_SHUT_REMOTE, 0, 0); 01998 else if (mode & PICO_SHUT_WR) 01999 pico_socket_alter_state(s, PICO_SOCKET_STATE_SHUT_LOCAL, 0, 0); 02000 else if (mode & PICO_SHUT_RD) 02001 pico_socket_alter_state(s, PICO_SOCKET_STATE_SHUT_REMOTE, 0, 0); 02002 02003 } 02004 #endif 02005 return 0; 02006 } 02007 02008 int pico_socket_close(struct pico_socket *s) 02009 { 02010 return pico_socket_shutdown(s, PICO_SHUT_RDWR); 02011 } 02012 02013 #ifdef PICO_SUPPORT_CRC 02014 static inline int pico_transport_crc_check(struct pico_frame *f) 02015 { 02016 struct pico_ipv4_hdr *net_hdr = (struct pico_ipv4_hdr *) f->net_hdr; 02017 struct pico_udp_hdr *udp_hdr = NULL; 02018 uint16_t checksum_invalid = 1; 02019 02020 switch (net_hdr->proto) 02021 { 02022 case PICO_PROTO_TCP: 02023 checksum_invalid = short_be(pico_tcp_checksum_ipv4(f)); 02024 //dbg("TCP CRC validation == %u\n", checksum_invalid); 02025 if (checksum_invalid) { 02026 //dbg("TCP CRC: validation failed!\n"); 02027 pico_frame_discard(f); 02028 return 0; 02029 } 02030 break; 02031 02032 case PICO_PROTO_UDP: 02033 udp_hdr = (struct pico_udp_hdr *) f->transport_hdr; 02034 if (short_be(udp_hdr->crc)) { 02035 checksum_invalid = short_be(pico_udp_checksum_ipv4(f)); 02036 //dbg("UDP CRC validation == %u\n", checksum_invalid); 02037 if (checksum_invalid) { 02038 //dbg("UDP CRC: validation failed!\n"); 02039 pico_frame_discard(f); 02040 return 0; 02041 } 02042 } 02043 break; 02044 02045 default: 02046 // Do nothing 02047 break; 02048 } 02049 return 1; 02050 } 02051 #else 02052 static inline int pico_transport_crc_check(struct pico_frame *f) 02053 { 02054 return 1; 02055 } 02056 #endif /* PICO_SUPPORT_CRC */ 02057 02058 int pico_transport_process_in(struct pico_protocol *self, struct pico_frame *f) 02059 { 02060 struct pico_trans *hdr = (struct pico_trans *) f->transport_hdr; 02061 int ret = 0; 02062 02063 if (!hdr) { 02064 pico_err = PICO_ERR_EFAULT; 02065 return -1; 02066 } 02067 02068 ret = pico_transport_crc_check(f); 02069 if (ret < 1) 02070 return ret; 02071 else 02072 ret = 0; 02073 02074 if ((hdr) && (pico_socket_deliver(self, f, hdr->dport) == 0)) 02075 return ret; 02076 02077 if (!IS_BCAST(f)) { 02078 dbg("Socket not found... \n"); 02079 pico_notify_socket_unreachable(f); 02080 #ifdef PICO_SUPPORT_TCP 02081 /* if tcp protocol send RST segment */ 02082 //if (self->proto_number == PICO_PROTO_TCP) 02083 // pico_tcp_reply_rst(f); 02084 #endif 02085 ret = -1; 02086 pico_err = PICO_ERR_ENOENT; 02087 } 02088 pico_frame_discard(f); 02089 return ret; 02090 } 02091 02092 #define SL_LOOP_MIN 1 02093 02094 02095 int pico_sockets_loop(int loop_score) 02096 { 02097 static struct pico_tree_node *index_udp, * index_tcp; 02098 02099 struct pico_sockport *start; 02100 struct pico_socket *s; 02101 02102 #ifdef PICO_SUPPORT_UDP 02103 struct pico_frame *f; 02104 02105 if (sp_udp == NULL) 02106 { 02107 index_udp = pico_tree_firstNode(UDPTable.root); 02108 sp_udp = index_udp->keyValue; 02109 } 02110 02111 /* init start node */ 02112 start = sp_udp; 02113 02114 /* round-robin all transport protocols, break if traversed all protocols */ 02115 while (loop_score > SL_LOOP_MIN && sp_udp != NULL) { 02116 struct pico_tree_node * index; 02117 02118 pico_tree_foreach(index,&sp_udp->socks){ 02119 s = index->keyValue; 02120 f = pico_dequeue(&s->q_out); 02121 while (f && (loop_score > 0)) { 02122 pico_proto_udp.push(&pico_proto_udp, f); 02123 loop_score -= 1; 02124 f = pico_dequeue(&s->q_out); 02125 } 02126 } 02127 02128 index_udp = pico_tree_next(index_udp); 02129 sp_udp = index_udp->keyValue; 02130 02131 if (sp_udp == NULL) 02132 { 02133 index_udp = pico_tree_firstNode(UDPTable.root); 02134 sp_udp = index_udp->keyValue; 02135 } 02136 if (sp_udp == start) 02137 break; 02138 } 02139 #endif 02140 02141 #ifdef PICO_SUPPORT_TCP 02142 if (sp_tcp == NULL) 02143 { 02144 index_tcp = pico_tree_firstNode(TCPTable.root); 02145 sp_tcp = index_tcp->keyValue; 02146 } 02147 02148 /* init start node */ 02149 start = sp_tcp; 02150 02151 while (loop_score > SL_LOOP_MIN && sp_tcp != NULL) { 02152 struct pico_tree_node * index; 02153 pico_tree_foreach(index, &sp_tcp->socks){ 02154 s = index->keyValue; 02155 loop_score = pico_tcp_output(s, loop_score); 02156 if ((s->ev_pending) && s->wakeup) { 02157 s->wakeup(s->ev_pending, s); 02158 } 02159 if (loop_score <= 0) { 02160 loop_score = 0; 02161 break; 02162 } 02163 } 02164 02165 /* check if RB_FOREACH ended, if not, break to keep the cur sp_tcp */ 02166 if (s != NULL) 02167 break; 02168 02169 index_tcp = pico_tree_next(index_tcp); 02170 sp_tcp = index_tcp->keyValue; 02171 02172 if (sp_tcp == NULL) 02173 { 02174 index_tcp = pico_tree_firstNode(TCPTable.root); 02175 sp_tcp = index_tcp->keyValue; 02176 } 02177 if (sp_tcp == start) 02178 break; 02179 } 02180 #endif 02181 02182 return loop_score; 02183 } 02184 02185 02186 struct pico_frame *pico_socket_frame_alloc(struct pico_socket *s, int len) 02187 { 02188 struct pico_frame *f = NULL; 02189 02190 #ifdef PICO_SUPPORT_IPV6 02191 if (IS_SOCK_IPV6(s)) 02192 f = pico_proto_ipv6.alloc(&pico_proto_ipv6, len); 02193 #endif 02194 02195 #ifdef PICO_SUPPORT_IPV4 02196 if (IS_SOCK_IPV4(s)) 02197 f = pico_proto_ipv4.alloc(&pico_proto_ipv4, len); 02198 #endif 02199 if (!f) { 02200 pico_err = PICO_ERR_ENOMEM; 02201 return f; 02202 } 02203 f->payload = f->transport_hdr; 02204 f->payload_len = len; 02205 f->sock = s; 02206 return f; 02207 } 02208 02209 int pico_transport_error(struct pico_frame *f, uint8_t proto, int code) 02210 { 02211 int ret = -1; 02212 struct pico_trans *trans = (struct pico_trans*) f->transport_hdr; 02213 struct pico_sockport *port = NULL; 02214 struct pico_socket *s = NULL; 02215 switch (proto) { 02216 02217 02218 #ifdef PICO_SUPPORT_UDP 02219 case PICO_PROTO_UDP: 02220 port = pico_get_sockport(proto, trans->sport); 02221 break; 02222 #endif 02223 02224 #ifdef PICO_SUPPORT_TCP 02225 case PICO_PROTO_TCP: 02226 port = pico_get_sockport(proto, trans->sport); 02227 break; 02228 #endif 02229 02230 default: 02231 /* Protocol not available */ 02232 ret = -1; 02233 } 02234 if (port) { 02235 struct pico_tree_node * index; 02236 ret = 0; 02237 02238 pico_tree_foreach(index,&port->socks) { 02239 s = index->keyValue; 02240 if (trans->dport == s->remote_port) { 02241 if (s->wakeup) { 02242 //dbg("SOCKET ERROR FROM ICMP NOTIFICATION. (icmp code= %d)\n\n", code); 02243 switch(code) { 02244 case PICO_ICMP_UNREACH_PROTOCOL: 02245 pico_err = PICO_ERR_EPROTO; 02246 break; 02247 02248 case PICO_ICMP_UNREACH_PORT: 02249 pico_err = PICO_ERR_ECONNREFUSED; 02250 break; 02251 02252 case PICO_ICMP_UNREACH_NET: 02253 case PICO_ICMP_UNREACH_NET_PROHIB: 02254 case PICO_ICMP_UNREACH_NET_UNKNOWN: 02255 pico_err = PICO_ERR_ENETUNREACH; 02256 break; 02257 02258 default: 02259 pico_err = PICO_ERR_EHOSTUNREACH; 02260 } 02261 s->wakeup(PICO_SOCK_EV_ERR, s); 02262 } 02263 break; 02264 } 02265 } 02266 } 02267 pico_frame_discard(f); 02268 return ret; 02269 } 02270 #endif 02271 #endif
Generated on Wed Jul 13 2022 02:20:45 by
1.7.2