pico_socket.c

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