Free (GPLv2) TCP/IP stack developed by TASS Belgium
Fork of PicoTCP by
Diff: modules/pico_ipv4.c
- Revision:
- 51:18637a3d071f
- Parent:
- 10:dd7111d4279f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/modules/pico_ipv4.c Sat Aug 03 08:50:27 2013 +0000 @@ -0,0 +1,1418 @@ +/********************************************************************* +PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved. +See LICENSE and COPYING for usage. + +Authors: Daniele Lacamera, Markian Yskout +*********************************************************************/ + + +#include "pico_config.h" +#include "pico_ipfilter.h" +#include "pico_ipv4.h" +#include "pico_icmp4.h" +#include "pico_stack.h" +#include "pico_eth.h" +#include "pico_udp.h" +#include "pico_tcp.h" +#include "pico_socket.h" +#include "pico_device.h" +#include "pico_nat.h" +#include "pico_igmp.h" +#include "pico_tree.h" + +#ifdef PICO_SUPPORT_IPV4 + +#ifdef PICO_SUPPORT_MCAST +# define ip_mcast_dbg(...) do{}while(0) /* so_mcast_dbg in pico_socket.c */ +# define PICO_MCAST_ALL_HOSTS long_be(0xE0000001) /* 224.0.0.1 */ +/* Default network interface for multicast transmission */ +static struct pico_ipv4_link *mcast_default_link = NULL; +#endif +#ifdef PICO_SUPPORT_IPFRAG +# define reassembly_dbg(...) do{}while(0) +#endif + +/* Queues */ +static struct pico_queue in = {}; +static struct pico_queue out = {}; + +/* Functions */ +static int ipv4_route_compare(void *ka, void * kb); + +int pico_ipv4_to_string(char *ipbuf, const uint32_t ip) +{ + const unsigned char *addr = (unsigned char *) &ip; + int i; + + if (!ipbuf) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + for(i = 0; i < 4; i++) + { + if(addr[i] > 99){ + *ipbuf++ = '0' + (addr[i] / 100); + *ipbuf++ = '0' + ((addr[i] % 100) / 10); + *ipbuf++ = '0' + ((addr[i] % 100) % 10); + }else if(addr[i] > 9){ + *ipbuf++ = '0' + (addr[i] / 10); + *ipbuf++ = '0' + (addr[i] % 10); + }else{ + *ipbuf++ = '0' + addr[i]; + } + if(i < 3) + *ipbuf++ = '.'; + } + *ipbuf = '\0'; + + return 0; +} + +int pico_string_to_ipv4(const char *ipstr, uint32_t *ip) +{ + unsigned char buf[4] = {0}; + int cnt = 0; + int p; + + if(!ipstr || !ip) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + while((p = *ipstr++) != 0) + { + if(pico_is_digit(p)){ + buf[cnt] = (10 * buf[cnt]) + (p - '0'); + }else if(p == '.'){ + cnt++; + }else{ + return -1; + } + } + + /* Handle short notation */ + if(cnt == 1){ + buf[3] = buf[1]; + buf[1] = 0; + buf[2] = 0; + }else if (cnt == 2){ + buf[3] = buf[2]; + buf[2] = 0; + }else if(cnt != 3){ + /* String could not be parsed, return error */ + return -1; + } + + *ip = long_from(buf); + + return 0; + +} + +int pico_ipv4_valid_netmask(uint32_t mask) +{ + int cnt = 0; + int end = 0; + int i; + uint32_t mask_swap = long_be(mask); + + /* + * Swap bytes for convenient parsing + * e.g. 0x..f8ff will become 0xfff8.. + * Then, we count the consecutive bits + * + * */ + + for(i = 0; i < 32; i++){ + if((mask_swap << i) & (1 << 31)){ + if(end) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + cnt++; + }else{ + end = 1; + } + } + return cnt; +} + +int pico_ipv4_is_unicast(uint32_t address) +{ + const unsigned char *addr = (unsigned char *) &address; + if((addr[0] & 0xe0) == 0xe0) + return 0; /* multicast */ + + return 1; +} + +int pico_ipv4_is_multicast(uint32_t address) +{ + const unsigned char *addr = (unsigned char *) &address; + if((addr[0] != 0xff) && ((addr[0] & 0xe0) == 0xe0)) + return 1; /* multicast */ + + return 0; +} + +static int pico_ipv4_checksum(struct pico_frame *f) +{ + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; + if (!hdr) + return -1; + hdr->crc = 0; + hdr->crc = short_be(pico_checksum(hdr, f->net_len)); + return 0; +} + +#ifdef PICO_SUPPORT_IPFRAG +struct pico_ipv4_fragmented_packet { + uint16_t id; + uint8_t proto; + struct pico_ip4 src; + struct pico_ip4 dst; + uint16_t total_len; + struct pico_tree *t; +}; + +static int pico_ipv4_fragmented_packet_cmp(void *ka, void *kb) +{ + struct pico_ipv4_fragmented_packet *a = ka, *b = kb; + + if (a->id < b->id) + return -1; + else if (a->id > b->id) + return 1; + else { + if (a->proto < b->proto) + return -1; + else if (a->proto > b->proto) + return 1; + else { + if (a->src.addr < b->src.addr) + return -1; + else if (a->src.addr > b->src.addr) + return 1; + else { + if (a->dst.addr < b->dst.addr) + return -1; + else if (a->dst.addr > b->dst.addr) + return 1; + else + return 0; + } + } + } +} + +static int pico_ipv4_fragmented_element_cmp(void *ka, void *kb) +{ + struct pico_frame *frame_a = ka, *frame_b = kb; + struct pico_ipv4_hdr *a, *b; + a = (struct pico_ipv4_hdr *) frame_a->net_hdr; + b = (struct pico_ipv4_hdr *) frame_b->net_hdr; + + if (short_be((a->frag & PICO_IPV4_FRAG_MASK)) < short_be((b->frag & PICO_IPV4_FRAG_MASK))) + return -1; + else if (short_be((a->frag & PICO_IPV4_FRAG_MASK)) > short_be((b->frag & PICO_IPV4_FRAG_MASK))) + return 1; + else + return 0; +} + +PICO_TREE_DECLARE(pico_ipv4_fragmented_tree, pico_ipv4_fragmented_packet_cmp); + +static inline void pico_ipv4_fragmented_cleanup(struct pico_ipv4_fragmented_packet *pfrag) +{ + struct pico_tree_node *index = NULL, *_tmp = NULL; + struct pico_frame *f_frag = NULL; + + pico_tree_foreach_safe(index, pfrag->t, _tmp) { + f_frag = index->keyValue; + reassembly_dbg("REASSEMBLY: remove packet with offset %u\n", short_be(((struct pico_ipv4_hdr *)f_frag->net_hdr)->frag) & PICO_IPV4_FRAG_MASK); + pico_tree_delete(pfrag->t, f_frag); + pico_frame_discard(f_frag); + } + pico_tree_delete(&pico_ipv4_fragmented_tree, pfrag); + pico_free(pfrag->t); + pico_free(pfrag); +} +#endif /* PICO_SUPPORT_IPFRAG */ + +#ifdef PICO_SUPPORT_IPFRAG +static inline int pico_ipv4_fragmented_check(struct pico_protocol *self, struct pico_frame **f) +{ + uint8_t *running_pointer = NULL; + uint16_t running_offset = 0; + uint16_t offset = 0; + uint16_t data_len = 0; + struct pico_ipv4_hdr *f_frag_hdr = NULL, *hdr = (struct pico_ipv4_hdr *) (*f)->net_hdr; + struct pico_udp_hdr *udp_hdr = NULL; + struct pico_tcp_hdr *tcp_hdr = NULL; + struct pico_ipv4_fragmented_packet *pfrag = NULL, frag; + struct pico_frame *f_new = NULL, *f_frag = NULL; + struct pico_tree_node *index, *_tmp; + + data_len = short_be(hdr->len) - (*f)->net_len; + offset = short_be(hdr->frag) & PICO_IPV4_FRAG_MASK; + if (short_be(hdr->frag) & PICO_IPV4_MOREFRAG) { + if (!offset) { + reassembly_dbg("REASSEMBLY: first element of a fragmented packet with id %X and offset %u\n", short_be(hdr->id), offset); + if (!pico_tree_empty(&pico_ipv4_fragmented_tree)) { + reassembly_dbg("REASSEMBLY: cleanup tree\n"); + // only one entry allowed in this tree + pfrag = pico_tree_first(&pico_ipv4_fragmented_tree); + pico_ipv4_fragmented_cleanup(pfrag); + } + // add entry in tree for this ID and create secondary tree to contain fragmented elements + pfrag = pico_zalloc(sizeof(struct pico_ipv4_fragmented_packet)); + if (!pfrag) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + pfrag->id = short_be(hdr->id); + pfrag->proto = hdr->proto; + pfrag->src.addr = long_be(hdr->src.addr); + pfrag->dst.addr = long_be(hdr->dst.addr); + pfrag->total_len = short_be(hdr->len) - (*f)->net_len; + pfrag->t = pico_zalloc(sizeof(struct pico_tree)); + if (!pfrag->t) { + pico_free(pfrag); + pico_err = PICO_ERR_ENOMEM; + return -1; + } + pfrag->t->root = &LEAF; + pfrag->t->compare = pico_ipv4_fragmented_element_cmp; + + pico_tree_insert(pfrag->t, *f); + pico_tree_insert(&pico_ipv4_fragmented_tree, pfrag); + return 0; + } + else { + reassembly_dbg("REASSEMBLY: intermediate element of a fragmented packet with id %X and offset %u\n", short_be(hdr->id), offset); + frag.id = short_be(hdr->id); + frag.proto = hdr->proto; + frag.src.addr = long_be(hdr->src.addr); + frag.dst.addr = long_be(hdr->dst.addr); + pfrag = pico_tree_findKey(&pico_ipv4_fragmented_tree, &frag); + if (pfrag) { + pfrag->total_len += (short_be(hdr->len) - (*f)->net_len); + pico_tree_insert(pfrag->t, *f); + return 0; + } else { + reassembly_dbg("REASSEMBLY: silently discard intermediate frame, first packet was lost or disallowed (one fragmented packet at a time)\n"); + pico_frame_discard(*f); + return 0; + } + } + } else if (offset) { + reassembly_dbg("REASSEMBLY: last element of a fragmented packet with id %X and offset %u\n", short_be(hdr->id), offset); + frag.id = short_be(hdr->id); + frag.proto = hdr->proto; + frag.src.addr = long_be(hdr->src.addr); + frag.dst.addr = long_be(hdr->dst.addr); + pfrag = pico_tree_findKey(&pico_ipv4_fragmented_tree, &frag); + if (pfrag) { + pfrag->total_len += (short_be(hdr->len) - (*f)->net_len); + reassembly_dbg("REASSEMBLY: fragmented packet in tree, reassemble packet of %u data bytes\n", pfrag->total_len); + f_new = self->alloc(self, pfrag->total_len); + + f_frag = pico_tree_first(pfrag->t); + reassembly_dbg("REASSEMBLY: copy IP header information len = %lu\n", f_frag->net_len); + f_frag_hdr = (struct pico_ipv4_hdr *)f_frag->net_hdr; + data_len = short_be(f_frag_hdr->len) - f_frag->net_len; + memcpy(f_new->net_hdr, f_frag->net_hdr, f_frag->net_len); + memcpy(f_new->transport_hdr, f_frag->transport_hdr, data_len); + running_pointer = f_new->transport_hdr + data_len; + offset = short_be(f_frag_hdr->frag) & PICO_IPV4_FRAG_MASK; + running_offset = data_len / 8; + pico_tree_delete(pfrag->t, f_frag); + pico_frame_discard(f_frag); + reassembly_dbg("REASSEMBLY: reassembled first packet of %u data bytes, offset = %u next expected offset = %u\n", data_len, offset, running_offset); + + pico_tree_foreach_safe(index, pfrag->t, _tmp) + { + f_frag = index->keyValue; + f_frag_hdr = (struct pico_ipv4_hdr *)f_frag->net_hdr; + data_len = short_be(f_frag_hdr->len) - f_frag->net_len; + memcpy(running_pointer, f_frag->transport_hdr, data_len); + running_pointer += data_len; + offset = short_be(f_frag_hdr->frag) & PICO_IPV4_FRAG_MASK; + if (offset != running_offset) { + reassembly_dbg("REASSEMBLY: error reassembling intermediate packet: offset %u != expected offset %u (missing fragment)\n", offset, running_offset); + pico_ipv4_fragmented_cleanup(pfrag); + return -1; + } + running_offset += (data_len / 8); + pico_tree_delete(pfrag->t, f_frag); + pico_frame_discard(f_frag); + reassembly_dbg("REASSEMBLY: reassembled intermediate packet of %u data bytes, offset = %u next expected offset = %u\n", data_len, offset, running_offset); + } + pico_tree_delete(&pico_ipv4_fragmented_tree, pfrag); + pico_free(pfrag); + + data_len = short_be(hdr->len) - (*f)->net_len; + memcpy(running_pointer, (*f)->transport_hdr, data_len); + offset = short_be(hdr->frag) & PICO_IPV4_FRAG_MASK; + pico_frame_discard(*f); + reassembly_dbg("REASSEMBLY: reassembled last packet of %u data bytes, offset = %u\n", data_len, offset); + + hdr = (struct pico_ipv4_hdr *)f_new->net_hdr; + hdr->len = pfrag->total_len; + hdr->frag = 0; /* flags cleared and no offset */ + hdr->crc = 0; + hdr->crc = short_be(pico_checksum(hdr, f_new->net_len)); + /* Optional, the UDP/TCP CRC should already be correct */ + if (0) { + #ifdef PICO_SUPPORT_TCP + } else if (hdr->proto == PICO_PROTO_TCP) { + tcp_hdr = (struct pico_tcp_hdr *) f_new->transport_hdr; + tcp_hdr->crc = 0; + tcp_hdr->crc = short_be(pico_tcp_checksum_ipv4(f_new)); + #endif + #ifdef PICO_SUPPORT_UDP + } else if (hdr->proto == PICO_PROTO_UDP){ + udp_hdr = (struct pico_udp_hdr *) f_new->transport_hdr; + udp_hdr->crc = 0; + udp_hdr->crc = short_be(pico_udp_checksum_ipv4(f_new)); + #endif + } + reassembly_dbg("REASSEMBLY: packet with id %X reassembled correctly\n", short_be(hdr->id)); + *f = f_new; + return 1; + } else { + reassembly_dbg("REASSEMBLY: silently discard last frame, first packet was lost or disallowed (one fragmented packet at a time)\n"); + pico_frame_discard(*f); + return 0; + } + } else { + return 1; + } +} +#else +static inline int pico_ipv4_fragmented_check(struct pico_protocol *self, struct pico_frame **f) +{ + return 1; +} +#endif /* PICO_SUPPORT_IPFRAG */ + +#ifdef PICO_SUPPORT_CRC +static inline int pico_ipv4_crc_check(struct pico_frame *f) +{ + uint16_t checksum_invalid = 1; + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; + + checksum_invalid = short_be(pico_checksum(hdr, f->net_len)); + if (checksum_invalid) { + dbg("IP: checksum failed!\n"); + pico_frame_discard(f); + return 0; + } + return 1; +} +#else +static inline int pico_ipv4_crc_check(struct pico_frame *f) +{ + return 1; +} +#endif /* PICO_SUPPORT_CRC */ + +static int pico_ipv4_forward(struct pico_frame *f); +#ifdef PICO_SUPPORT_MCAST +static int pico_ipv4_mcast_filter(struct pico_frame *f); +#endif + +static int ipv4_link_compare(void *ka, void *kb) +{ + struct pico_ipv4_link *a = ka, *b =kb; + if (a->address.addr < b->address.addr) + return -1; + if (a->address.addr > b->address.addr) + return 1; + + //zero can be assigned multiple times (e.g. for DHCP) + if (a->dev != NULL && b->dev != NULL && a->address.addr == PICO_IP4_ANY && b->address.addr == PICO_IP4_ANY){ + if (a->dev < b->dev) + return -1; + if (a->dev > b->dev) + return 1; + } + return 0; +} + +PICO_TREE_DECLARE(Tree_dev_link, ipv4_link_compare); + +static int pico_ipv4_process_in(struct pico_protocol *self, struct pico_frame *f) +{ + uint8_t option_len = 0; + int ret = 0; + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; + struct pico_ipv4_link test = {.address = {.addr = PICO_IP4_ANY}, .dev = NULL}; + + /* NAT needs transport header information */ + if(((hdr->vhl) & 0x0F )> 5){ + option_len = 4*(((hdr->vhl) & 0x0F)-5); + } + f->transport_hdr = ((uint8_t *)f->net_hdr) + PICO_SIZE_IP4HDR + option_len; + f->transport_len = short_be(hdr->len) - PICO_SIZE_IP4HDR - option_len; + f->net_len = PICO_SIZE_IP4HDR + option_len; + +#ifdef PICO_SUPPORT_IPFILTER + if (ipfilter(f)) { + /*pico_frame is discarded as result of the filtering*/ + return 0; + } +#endif + + /* ret == 1 indicates to continue the function */ + ret = pico_ipv4_crc_check(f); + if (ret < 1) + return ret; + ret = pico_ipv4_fragmented_check(self, &f); + if (ret < 1) + return ret; + +#ifdef PICO_SUPPORT_MCAST + /* Multicast address in source, discard quietly */ + if (pico_ipv4_is_multicast(hdr->src.addr)) { + ip_mcast_dbg("MCAST: ERROR multicast address %08X in source address\n", hdr->src.addr); + pico_frame_discard(f); + return 0; + } +#endif + if (hdr->frag & 0x80) { + pico_frame_discard(f); //RFC 3514 + return 0; + } + if (0) { +#ifdef PICO_SUPPORT_UDP + } else if (pico_ipv4_is_broadcast(hdr->dst.addr) && (hdr->proto == PICO_PROTO_UDP)) { + /* Receiving UDP broadcast datagram */ + f->flags |= PICO_FRAME_FLAG_BCAST; + pico_enqueue(pico_proto_udp.q_in, f); +#endif + } else if (pico_ipv4_is_multicast(hdr->dst.addr)) { +#ifdef PICO_SUPPORT_MCAST + /* Receiving UDP multicast datagram TODO set f->flags? */ + if (hdr->proto == PICO_PROTO_IGMP) { + ip_mcast_dbg("MCAST: received IGMP message\n"); + pico_transport_receive(f, PICO_PROTO_IGMP); + } else if ((pico_ipv4_mcast_filter(f) == 0) && (hdr->proto == PICO_PROTO_UDP)) { + pico_enqueue(pico_proto_udp.q_in, f); + } else { + pico_frame_discard(f); + } +#endif + } else if (pico_ipv4_link_find(&hdr->dst)) { + if (pico_ipv4_nat_isenabled_in(f) == 0) { /* if NAT enabled (dst port registerd), do NAT */ + if(pico_ipv4_nat(f, hdr->dst) != 0) { + return -1; + } + pico_ipv4_forward(f); /* Local packet became forward packet after NAT */ + } else { /* no NAT so enqueue to next layer */ + pico_transport_receive(f, hdr->proto); + } + } else if (pico_tree_findKey(&Tree_dev_link, &test)){ +#ifdef PICO_SUPPORT_UDP + //address of this device is apparently 0.0.0.0; might be a DHCP packet + pico_enqueue(pico_proto_udp.q_in, f); +#endif + } else { + /* Packet is not local. Try to forward. */ + if (pico_ipv4_forward(f) != 0) { + pico_frame_discard(f); + } + } + return 0; +} + +PICO_TREE_DECLARE(Routes, ipv4_route_compare); + + +static int pico_ipv4_process_out(struct pico_protocol *self, struct pico_frame *f) +{ + f->start = (uint8_t*) f->net_hdr; + #ifdef PICO_SUPPORT_IPFILTER + if (ipfilter(f)) { + /*pico_frame is discarded as result of the filtering*/ + return 0; + } + #endif + return pico_sendto_dev(f); +} + + +static struct pico_frame *pico_ipv4_alloc(struct pico_protocol *self, int size) +{ + struct pico_frame *f = pico_frame_alloc(size + PICO_SIZE_IP4HDR + PICO_SIZE_ETHHDR); + if (!f) + return NULL; + f->datalink_hdr = f->buffer; + f->net_hdr = f->buffer + PICO_SIZE_ETHHDR; + f->net_len = PICO_SIZE_IP4HDR; + f->transport_hdr = f->net_hdr + PICO_SIZE_IP4HDR; + f->transport_len = size; + f->len = size + PICO_SIZE_IP4HDR; + return f; +} + +static int pico_ipv4_frame_sock_push(struct pico_protocol *self, struct pico_frame *f); + +/* Interface: protocol definition */ +struct pico_protocol pico_proto_ipv4 = { + .name = "ipv4", + .proto_number = PICO_PROTO_IPV4, + .layer = PICO_LAYER_NETWORK, + .alloc = pico_ipv4_alloc, + .process_in = pico_ipv4_process_in, + .process_out = pico_ipv4_process_out, + .push = pico_ipv4_frame_sock_push, + .q_in = &in, + .q_out = &out, +}; + +struct pico_ipv4_route +{ + struct pico_ip4 dest; + struct pico_ip4 netmask; + struct pico_ip4 gateway; + struct pico_ipv4_link *link; + uint32_t metric; +}; + + +static int ipv4_route_compare(void *ka, void * kb) +{ + struct pico_ipv4_route *a = ka, *b = kb; + + /* Routes are sorted by (host side) netmask len, then by addr, then by metric. */ + if (long_be(a->netmask.addr) < long_be(b->netmask.addr)) + return -1; + + if (long_be(a->netmask.addr) > long_be(b->netmask.addr)) + return 1; + + if (a->dest.addr < b->dest.addr) + return -1; + + if (a->dest.addr > b->dest.addr) + return 1; + + if (a->metric < b->metric) + return -1; + + if (a->metric > b->metric) + return 1; + + return 0; +} + +static struct pico_ipv4_route *route_find(struct pico_ip4 *addr) +{ + struct pico_ipv4_route *r; + struct pico_tree_node * index; + + if(addr->addr != PICO_IP4_BCAST) + { + pico_tree_foreach_reverse(index, &Routes) { + r = index->keyValue; + if ((addr->addr & (r->netmask.addr)) == (r->dest.addr)) { + return r; + } + } + } + else + { + r = pico_tree_first(&Routes); + if(!r->netmask.addr) + { + return r; + } + else + { + dbg("WARNING: no default route for a global broadcast found\n"); + } + } + + return NULL; +} + +struct pico_ip4 pico_ipv4_route_get_gateway(struct pico_ip4 *addr) +{ + struct pico_ip4 nullip; + struct pico_ipv4_route *route; + nullip.addr = 0U; + + if(!addr) { + pico_err = PICO_ERR_EINVAL; + return nullip; + } + + route = route_find(addr); + if (!route) { + pico_err = PICO_ERR_EHOSTUNREACH; + return nullip; + } + else + return route->gateway; +} + +struct pico_ip4 *pico_ipv4_source_find(struct pico_ip4 *dst) +{ + struct pico_ip4 *myself = NULL; + struct pico_ipv4_route *rt; + + if(!dst) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + rt = route_find(dst); + if (rt) { + myself = &rt->link->address; + } else + pico_err = PICO_ERR_EHOSTUNREACH; + return myself; +} + + +#ifdef PICO_SUPPORT_MCAST +/* link + * | + * MCASTGroups + * | | | + * ------------ | ------------ + * | | | + * MCASTSources MCASTSources MCASTSources + * | | | | | | | | | | | | + * S S S S S S S S S S S S + * + * MCASTGroups: RBTree(mcast_group) + * MCASTSources: RBTree(source) + */ +static int ipv4_mcast_groups_cmp(void * ka, void * kb) +{ + struct pico_mcast_group *a = ka, *b = kb; + if (a->mcast_addr.addr < b->mcast_addr.addr) { + return -1; + } else if (a->mcast_addr.addr > b->mcast_addr.addr) { + return 1; + } else { + return 0; + } +} + +static int ipv4_mcast_sources_cmp(void *ka, void *kb) +{ + struct pico_ip4 *a = ka, *b = kb; + if (a->addr < b->addr) + return -1; + if (a->addr > b->addr) + return 1; + return 0; +} + +static void pico_ipv4_mcast_print_groups(struct pico_ipv4_link *mcast_link) +{ + uint16_t i = 0; + struct pico_mcast_group __attribute__ ((unused)) *g = NULL; + struct pico_ip4 __attribute__ ((unused)) *source = NULL; + struct pico_tree_node *index = NULL, *index2 = NULL; + + ip_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"); + ip_mcast_dbg("+ MULTICAST list interface %-16s +\n", mcast_link->dev->name); + ip_mcast_dbg("+---------------------------------------------------------------------------------+\n"); + ip_mcast_dbg("+ nr | interface | host group | reference count | filter mode | source +\n"); + ip_mcast_dbg("+---------------------------------------------------------------------------------+\n"); + + pico_tree_foreach(index, mcast_link->MCASTGroups) + { + g = index->keyValue; + ip_mcast_dbg("+ %04d | %16s | %08X | %05u | %u | %8s +\n", i, mcast_link->dev->name, g->mcast_addr.addr, g->reference_count, g->filter_mode, ""); + pico_tree_foreach(index2, &g->MCASTSources) + { + source = index2->keyValue; + ip_mcast_dbg("+ %4s | %16s | %8s | %5s | %s | %08X +\n", "", "", "", "", "", source->addr); + } + i++; + } + ip_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"); +} + +int pico_ipv4_mcast_join(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter) +{ + struct pico_mcast_group *g = NULL, test = {0}; + struct pico_ipv4_link *link = NULL; + struct pico_tree_node *index = NULL, *_tmp = NULL; + struct pico_ip4 *source = NULL; + + if (mcast_link) + link = pico_ipv4_link_get(mcast_link); + else + link = mcast_default_link; + + test.mcast_addr = *mcast_group; + g = pico_tree_findKey(link->MCASTGroups, &test); + if (g) { + if (reference_count) + g->reference_count++; + pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_UPDATE); + } else { + g = pico_zalloc(sizeof(struct pico_mcast_group)); + if (!g) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + /* "non-existent" state of filter mode INCLUDE and empty source list */ + g->filter_mode = PICO_IP_MULTICAST_INCLUDE; + g->reference_count = 1; + g->mcast_addr = *mcast_group; + g->MCASTSources.root = &LEAF; + g->MCASTSources.compare = ipv4_mcast_sources_cmp; + pico_tree_insert(link->MCASTGroups, g); + pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_CREATE); + } + + /* cleanup filter */ + pico_tree_foreach_safe(index, &g->MCASTSources, _tmp) + { + source = index->keyValue; + pico_tree_delete(&g->MCASTSources, source); + pico_free(source); + } + /* insert new filter */ + if (MCASTFilter) { + pico_tree_foreach(index, MCASTFilter) + { + source = pico_zalloc(sizeof(struct pico_ip4)); + if (!source) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + source->addr = ((struct pico_ip4 *)index->keyValue)->addr; + pico_tree_insert(&g->MCASTSources, source); + } + } + g->filter_mode = filter_mode; + + pico_ipv4_mcast_print_groups(link); + return 0; +} + +int pico_ipv4_mcast_leave(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter) +{ + + struct pico_mcast_group *g = NULL, test = {0}; + struct pico_ipv4_link *link = NULL; + struct pico_tree_node *index = NULL, *_tmp = NULL; + struct pico_ip4 *source = NULL; + + if (mcast_link) + link = pico_ipv4_link_get(mcast_link); + else + link = mcast_default_link; + + test.mcast_addr = *mcast_group; + g = pico_tree_findKey(link->MCASTGroups, &test); + if (!g) { + pico_err = PICO_ERR_EINVAL; + return -1; + } else { + if (reference_count && (--(g->reference_count) < 1)) { + pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_DELETE); + /* cleanup filter */ + pico_tree_foreach_safe(index, &g->MCASTSources, _tmp) + { + source = index->keyValue; + pico_tree_delete(&g->MCASTSources, source); + pico_free(source); + } + pico_tree_delete(link->MCASTGroups, g); + pico_free(g); + } else { + pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_UPDATE); + /* cleanup filter */ + pico_tree_foreach_safe(index, &g->MCASTSources, _tmp) + { + source = index->keyValue; + pico_tree_delete(&g->MCASTSources, source); + pico_free(source); + } + /* insert new filter */ + if (MCASTFilter) { + pico_tree_foreach(index, MCASTFilter) + { + source = pico_zalloc(sizeof(struct pico_ip4)); + if (!source) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + source->addr = ((struct pico_ip4 *)index->keyValue)->addr; + pico_tree_insert(&g->MCASTSources, source); + } + } + g->filter_mode = filter_mode; + } + } + + pico_ipv4_mcast_print_groups(link); + return 0; +} + +struct pico_ipv4_link *pico_ipv4_get_default_mcastlink(void) +{ + return mcast_default_link; +} + +static int pico_ipv4_mcast_filter(struct pico_frame *f) +{ + struct pico_ipv4_link *link = NULL; + struct pico_tree_node *index = NULL, *index2 = NULL; + struct pico_mcast_group *g = NULL, test = {0}; + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; + + test.mcast_addr = hdr->dst; + + pico_tree_foreach(index, &Tree_dev_link) + { + link = index->keyValue; + g = pico_tree_findKey(link->MCASTGroups, &test); + if (g) { + if (f->dev == link->dev) { + ip_mcast_dbg("MCAST: IP %08X is group member of current link %s\n", hdr->dst.addr, f->dev->name); + /* perform source filtering */ + switch (g->filter_mode) + { + case PICO_IP_MULTICAST_INCLUDE: + pico_tree_foreach(index2, &g->MCASTSources) + { + if (hdr->src.addr == ((struct pico_ip4 *)index2->keyValue)->addr) { + ip_mcast_dbg("MCAST: IP %08X in included interface source list\n", hdr->src.addr); + return 0; + } + } + ip_mcast_dbg("MCAST: IP %08X NOT in included interface source list\n", hdr->src.addr); + return -1; + break; + + case PICO_IP_MULTICAST_EXCLUDE: + pico_tree_foreach(index2, &g->MCASTSources) + { + if (hdr->src.addr == ((struct pico_ip4 *)index2->keyValue)->addr) { + ip_mcast_dbg("MCAST: IP %08X in excluded interface source list\n", hdr->src.addr); + return -1; + } + } + ip_mcast_dbg("MCAST: IP %08X NOT in excluded interface source list\n", hdr->src.addr); + return 0; + break; + + default: + return -1; + break; + } + } else { + ip_mcast_dbg("MCAST: IP %08X is group member of different link %s\n", hdr->dst.addr, link->dev->name); + } + } else { + ip_mcast_dbg("MCAST: IP %08X is not a group member of link %s\n", hdr->dst.addr, f->dev->name); + } + } + return -1; +} + +#else + +int pico_ipv4_mcast_join(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter) +{ + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; +} +int pico_ipv4_mcast_leave(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter) +{ + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; +} +struct pico_ipv4_link *pico_ipv4_get_default_mcastlink(void) +{ + pico_err = PICO_ERR_EPROTONOSUPPORT; + return NULL; +} +#endif /* PICO_SUPPORT_MCAST */ + +int pico_ipv4_frame_push(struct pico_frame *f, struct pico_ip4 *dst, uint8_t proto) +{ + + struct pico_ipv4_route *route; + struct pico_ipv4_link *link; + struct pico_ipv4_hdr *hdr; + uint8_t ttl = PICO_IPV4_DEFAULT_TTL; + uint8_t vhl = 0x45; /* version 4, header length 20 */ + static uint16_t ipv4_progressive_id = 0x91c0; +#ifdef PICO_SUPPORT_MCAST + struct pico_tree_node *index; +#endif + + if(!f || !dst) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + hdr = (struct pico_ipv4_hdr *) f->net_hdr; + if (!hdr) { + dbg("IP header error\n"); + pico_err = PICO_ERR_EINVAL; + goto drop; + } + + if (dst->addr == 0) { + dbg("IP destination addr error\n"); + pico_err = PICO_ERR_EINVAL; + goto drop; + } + + route = route_find(dst); + if (!route) { + dbg("Route to %08x not found.\n", long_be(dst->addr)); + pico_err = PICO_ERR_EHOSTUNREACH; + goto drop; + } else { + link = route->link; +#ifdef PICO_SUPPORT_MCAST + if (pico_ipv4_is_multicast(dst->addr)) { /* if multicast */ + switch (proto) { + case PICO_PROTO_UDP: + if(pico_udp_get_mc_ttl(f->sock, &ttl) < 0) + ttl = PICO_IP_DEFAULT_MULTICAST_TTL; + break; + case PICO_PROTO_IGMP: + vhl = 0x46; /* header length 24 */ + ttl = 1; + /* router alert (RFC 2113) */ + hdr->options[0] = 0x94; + hdr->options[1] = 0x04; + hdr->options[2] = 0x00; + hdr->options[3] = 0x00; + if (f->dev && link->dev != f->dev) { /* default link is not requested link */ + pico_tree_foreach(index, &Tree_dev_link) { + link = index->keyValue; + if (link->dev == f->dev) + break; + } + } + break; + default: + ttl = PICO_IPV4_DEFAULT_TTL; + } + } +#endif + } + + hdr->vhl = vhl; + hdr->len = short_be(f->transport_len + f->net_len); + if (f->transport_hdr != f->payload) + ipv4_progressive_id++; + hdr->id = short_be(ipv4_progressive_id); + hdr->dst.addr = dst->addr; + hdr->src.addr = link->address.addr; + hdr->ttl = ttl; + hdr->proto = proto; + hdr->frag = short_be(PICO_IPV4_DONTFRAG); +#ifdef PICO_SUPPORT_IPFRAG +# ifdef PICO_SUPPORT_UDP + if (proto == PICO_PROTO_UDP) { + /* first fragment, can not use transport_len to calculate IP length */ + if (f->transport_hdr != f->payload) + hdr->len = short_be(f->payload_len + sizeof(struct pico_udp_hdr) + f->net_len); + /* set fragmentation flags and offset calculated in socket layer */ + hdr->frag = f->frag; + } +# endif /* PICO_SUPPORT_UDP */ +#endif /* PICO_SUPPORT_IPFRAG */ + pico_ipv4_checksum(f); + + if (f->sock && f->sock->dev){ + //if the socket has its device set, use that (currently used for DHCP) + f->dev = f->sock->dev; + } else { + f->dev = link->dev; + } + +#ifdef PICO_SUPPORT_MCAST + if (pico_ipv4_is_multicast(hdr->dst.addr)) { + struct pico_frame *cpy; + /* Sending UDP multicast datagram, am I member? If so, loopback copy */ + if ((proto != PICO_PROTO_IGMP) && (pico_ipv4_mcast_filter(f) == 0)) { + ip_mcast_dbg("MCAST: sender is member of group, loopback copy\n"); + cpy = pico_frame_copy(f); + pico_enqueue(&in, cpy); + } + } +#endif + + if(pico_ipv4_link_get(&hdr->dst)){ + //it's our own IP + return pico_enqueue(&in, f); + }else{ + /* TODO: Check if there are members subscribed here */ + return pico_enqueue(&out, f); + } + +drop: + pico_frame_discard(f); + return -1; +} + + +static int pico_ipv4_frame_sock_push(struct pico_protocol *self, struct pico_frame *f) +{ + struct pico_ip4 *dst; + struct pico_remote_duple *remote_duple = (struct pico_remote_duple *) f->info; + if (!f->sock) { + pico_frame_discard(f); + return -1; + } + + if (remote_duple) { + dst = &remote_duple->remote_addr.ip4; + } else { + dst = &f->sock->remote_addr.ip4; + } + + return pico_ipv4_frame_push(f, dst, f->sock->proto->proto_number); +} + + +#ifdef DEBUG_ROUTE +static void dbg_route(void) +{ + struct pico_ipv4_route *r; + struct pico_tree_node * index; + pico_tree_foreach(index,&Routes){ + r = index->keyValue; + dbg("Route to %08x/%08x, gw %08x, dev: %s, metric: %d\n", r->dest.addr, r->netmask.addr, r->gateway.addr, r->link->dev->name, r->metric); + } +} +#else +#define dbg_route() do{ }while(0) +#endif + +int pico_ipv4_route_add(struct pico_ip4 address, struct pico_ip4 netmask, struct pico_ip4 gateway, int metric, struct pico_ipv4_link *link) +{ + struct pico_ipv4_route test, *new; + test.dest.addr = address.addr; + test.netmask.addr = netmask.addr; + test.metric = metric; + + if(pico_tree_findKey(&Routes,&test)){ + pico_err = PICO_ERR_EINVAL; + return -1; + } + + new = pico_zalloc(sizeof(struct pico_ipv4_route)); + if (!new) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + new->dest.addr = address.addr; + new->netmask.addr = netmask.addr; + new->gateway.addr = gateway.addr; + new->metric = metric; + if (gateway.addr == 0) { + /* No gateway provided, use the link */ + new->link = link; + } else { + struct pico_ipv4_route *r = route_find(&gateway); + if (!r ) { /* Specified Gateway is unreachable */ + pico_err = PICO_ERR_EHOSTUNREACH; + pico_free(new); + return -1; + } + if (r->gateway.addr) { /* Specified Gateway is not a neighbor */ + pico_err = PICO_ERR_ENETUNREACH; + pico_free(new); + return -1; + } + new->link = r->link; + } + if (!new->link) { + pico_err = PICO_ERR_EINVAL; + pico_free(new); + return -1; + } + + pico_tree_insert(&Routes,new); + dbg_route(); + return 0; +} + +int pico_ipv4_route_del(struct pico_ip4 address, struct pico_ip4 netmask, struct pico_ip4 gateway, int metric, struct pico_ipv4_link *link) +{ + struct pico_ipv4_route test, *found; + if (!link) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + test.dest.addr = address.addr; + test.netmask.addr = netmask.addr; + test.metric = metric; + + found = pico_tree_findKey(&Routes,&test); + if (found) { + + pico_tree_delete(&Routes,found); + pico_free(found); + + dbg_route(); + return 0; + } + pico_err = PICO_ERR_EINVAL; + return -1; +} + + +int pico_ipv4_link_add(struct pico_device *dev, struct pico_ip4 address, struct pico_ip4 netmask) +{ + struct pico_ipv4_link test, *new; + struct pico_ip4 network, gateway; + char ipstr[30]; + + if(!dev) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + test.address.addr = address.addr; + test.netmask.addr = netmask.addr; + test.dev = dev; + /** XXX: Valid netmask / unicast address test **/ + + if(pico_tree_findKey(&Tree_dev_link, &test)) { + dbg("IPv4: Trying to assign an invalid address (in use)\n"); + pico_err = PICO_ERR_EADDRINUSE; + return -1; + } + + /** XXX: Check for network already in use (e.g. trying to assign 10.0.0.1/24 where 10.1.0.1/8 is in use) **/ + new = pico_zalloc(sizeof(struct pico_ipv4_link)); + if (!new) { + dbg("IPv4: Out of memory!\n"); + pico_err = PICO_ERR_ENOMEM; + return -1; + } + new->address.addr = address.addr; + new->netmask.addr = netmask.addr; + new->dev = dev; +#ifdef PICO_SUPPORT_MCAST + new->MCASTGroups = pico_zalloc(sizeof(struct pico_tree)); + if (!new->MCASTGroups) { + pico_free(new); + dbg("IPv4: Out of memory!\n"); + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + new->MCASTGroups->root = &LEAF; + new->MCASTGroups->compare = ipv4_mcast_groups_cmp; + new->mcast_compatibility = PICO_IGMPV3; /* default RFC 3376 $7.2.1 */ + new->mcast_last_query_interval = PICO_IGMP_QUERY_INTERVAL; +#endif + + pico_tree_insert(&Tree_dev_link, new); +#ifdef PICO_SUPPORT_MCAST + do { + struct pico_ip4 mcast_all_hosts, mcast_addr, mcast_nm, mcast_gw; + if (!mcast_default_link) { + mcast_addr.addr = long_be(0xE0000000); /* 224.0.0.0 */ + mcast_nm.addr = long_be(0xF0000000); /* 15.0.0.0 */ + mcast_gw.addr = long_be(0x00000000); + mcast_default_link = new; + pico_ipv4_route_add(mcast_addr, mcast_nm, mcast_gw, 1, new); + } + mcast_all_hosts.addr = PICO_MCAST_ALL_HOSTS; + pico_ipv4_mcast_join(&address, &mcast_all_hosts, 1, PICO_IP_MULTICAST_EXCLUDE, NULL); + } while(0); +#endif + + network.addr = address.addr & netmask.addr; + gateway.addr = 0U; + pico_ipv4_route_add(network, netmask, gateway, 1, new); + pico_ipv4_to_string(ipstr, new->address.addr); + dbg("Assigned ipv4 %s to device %s\n", ipstr, new->dev->name); + return 0; +} + + +int pico_ipv4_link_del(struct pico_device *dev, struct pico_ip4 address) +{ + struct pico_ipv4_link test, *found; + struct pico_ip4 network; + + if(!dev) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + test.address.addr = address.addr; + test.dev = dev; + found = pico_tree_findKey(&Tree_dev_link, &test); + if (!found) { + pico_err = PICO_ERR_ENXIO; + return -1; + } + + network.addr = found->address.addr & found->netmask.addr; + pico_ipv4_route_del(network, found->netmask,pico_ipv4_route_get_gateway(&found->address), 1, found); +#ifdef PICO_SUPPORT_MCAST + do { + struct pico_ip4 mcast_all_hosts, mcast_addr, mcast_nm, mcast_gw; + struct pico_mcast_group *g = NULL; + struct pico_tree_node * index, * _tmp; + if (found == mcast_default_link) { + mcast_addr.addr = long_be(0xE0000000); /* 224.0.0.0 */ + mcast_nm.addr = long_be(0xF0000000); /* 15.0.0.0 */ + mcast_gw.addr = long_be(0x00000000); + mcast_default_link = NULL; + pico_ipv4_route_del(mcast_addr, mcast_nm, mcast_gw, 1, found); + } + mcast_all_hosts.addr = PICO_MCAST_ALL_HOSTS; + pico_ipv4_mcast_leave(&address, &mcast_all_hosts, 1, PICO_IP_MULTICAST_EXCLUDE, NULL); + pico_tree_foreach_safe(index,found->MCASTGroups, _tmp) + { + g = index->keyValue; + pico_tree_delete(found->MCASTGroups, g); + pico_free(g); + } + } while(0); +#endif + + pico_tree_delete(&Tree_dev_link, found); + /* XXX: pico_free(found); */ + /* XXX: cleanup all routes containing the removed link */ + return 0; +} + + +struct pico_ipv4_link *pico_ipv4_link_get(struct pico_ip4 *address) +{ + struct pico_ipv4_link test = {0}, *found = NULL; + test.address.addr = address->addr; + + found = pico_tree_findKey(&Tree_dev_link, &test); + if (!found) + return NULL; + else + return found; +} + +struct pico_ipv4_link *pico_ipv4_link_by_dev(struct pico_device *dev) +{ + struct pico_tree_node *index = NULL; + struct pico_ipv4_link *link = NULL; + + pico_tree_foreach(index, &Tree_dev_link) + { + link = index->keyValue; + if (link->dev == dev) + return link; + } + return NULL; +} + + +struct pico_device *pico_ipv4_link_find(struct pico_ip4 *address) +{ + struct pico_ipv4_link test, *found; + if(!address) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + test.dev = NULL; + test.address.addr = address->addr; + found = pico_tree_findKey(&Tree_dev_link, &test); + if (!found) { + pico_err = PICO_ERR_ENXIO; + return NULL; + } + return found->dev; +} + +int pico_ipv4_rebound(struct pico_frame *f) +{ + struct pico_ip4 dst; + struct pico_ipv4_hdr *hdr; + if(!f) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + hdr = (struct pico_ipv4_hdr *) f->net_hdr; + if (!hdr) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + dst.addr = hdr->src.addr; + return pico_ipv4_frame_push(f, &dst, hdr->proto); +} + +static int pico_ipv4_forward(struct pico_frame *f) +{ + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr; + struct pico_ipv4_route *rt; + if (!hdr) { + return -1; + } + + //dbg("IP> FORWARDING.\n"); + rt = route_find(&hdr->dst); + if (!rt) { + pico_notify_dest_unreachable(f); + return -1; + } + //dbg("ROUTE: valid..\n"); + f->dev = rt->link->dev; + hdr->ttl-=1; + if (hdr->ttl < 1) { + pico_notify_ttl_expired(f); + return -1; + } + hdr->crc++; + + /* check if NAT enbled on link and do NAT if so */ + if (pico_ipv4_nat_isenabled_out(rt->link) == 0) + pico_ipv4_nat(f, rt->link->address); + + //dbg("Routing towards %s\n", f->dev->name); + f->start = f->net_hdr; + if(f->dev->eth != NULL) + f->len -= PICO_SIZE_ETHHDR; + pico_sendto_dev(f); + return 0; + +} + +int pico_ipv4_is_broadcast(uint32_t addr) +{ + struct pico_ipv4_link *link; + struct pico_tree_node * index; + if (addr == PICO_IP4_ANY) + return 1; + if (addr == PICO_IP4_BCAST) + return 1; + + pico_tree_foreach(index,&Tree_dev_link) { + link = index->keyValue; + if ((link->address.addr | (~link->netmask.addr)) == addr) + return 1; + } + return 0; +} + +void pico_ipv4_unreachable(struct pico_frame *f, int err) +{ + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; +#if defined PICO_SUPPORT_TCP || defined PICO_SUPPORT_UDP + f->transport_hdr = ((uint8_t *)f->net_hdr) + PICO_SIZE_IP4HDR; + pico_transport_error(f, hdr->proto, err); +#endif +} + +#endif