From ce3d8f2baae2ded0820ec6262962182a09e1c85b Mon Sep 17 00:00:00 2001 From: Flyinghead Date: Sat, 15 Sep 2018 21:34:50 +0200 Subject: [PATCH] picotcp initial commit - missing files --- core/deps/picotcp/rules/crc.mk | 1 + core/deps/picotcp/rules/debug.mk | 207 ++ core/deps/picotcp/rules/dhcp_client.mk | 2 + core/deps/picotcp/rules/dhcp_server.mk | 2 + core/deps/picotcp/rules/dns_client.mk | 2 + core/deps/picotcp/rules/eth.mk | 3 + core/deps/picotcp/rules/icmp4.mk | 5 + core/deps/picotcp/rules/ipv4.mk | 2 + core/deps/picotcp/rules/ipv4frag.mk | 2 + core/deps/picotcp/rules/ppp.mk | 3 + core/deps/picotcp/rules/tap.mk | 1 + core/deps/picotcp/rules/tcp.mk | 3 + core/deps/picotcp/rules/tun.mk | 1 + core/deps/picotcp/rules/udp.mk | 3 + core/deps/picotcp/stack/pico_device.c | 482 ++++ core/deps/picotcp/stack/pico_frame.c | 329 +++ core/deps/picotcp/stack/pico_md5.c | 43 + core/deps/picotcp/stack/pico_protocol.c | 226 ++ core/deps/picotcp/stack/pico_socket.c | 2287 +++++++++++++++++ .../picotcp/stack/pico_socket_multicast.c | 1478 +++++++++++ core/deps/picotcp/stack/pico_stack.c | 962 +++++++ core/deps/picotcp/stack/pico_tree.c | 565 ++++ 22 files changed, 6609 insertions(+) create mode 100644 core/deps/picotcp/rules/crc.mk create mode 100644 core/deps/picotcp/rules/debug.mk create mode 100644 core/deps/picotcp/rules/dhcp_client.mk create mode 100644 core/deps/picotcp/rules/dhcp_server.mk create mode 100644 core/deps/picotcp/rules/dns_client.mk create mode 100644 core/deps/picotcp/rules/eth.mk create mode 100644 core/deps/picotcp/rules/icmp4.mk create mode 100644 core/deps/picotcp/rules/ipv4.mk create mode 100644 core/deps/picotcp/rules/ipv4frag.mk create mode 100644 core/deps/picotcp/rules/ppp.mk create mode 100644 core/deps/picotcp/rules/tap.mk create mode 100644 core/deps/picotcp/rules/tcp.mk create mode 100644 core/deps/picotcp/rules/tun.mk create mode 100644 core/deps/picotcp/rules/udp.mk create mode 100644 core/deps/picotcp/stack/pico_device.c create mode 100644 core/deps/picotcp/stack/pico_frame.c create mode 100644 core/deps/picotcp/stack/pico_md5.c create mode 100644 core/deps/picotcp/stack/pico_protocol.c create mode 100644 core/deps/picotcp/stack/pico_socket.c create mode 100644 core/deps/picotcp/stack/pico_socket_multicast.c create mode 100644 core/deps/picotcp/stack/pico_stack.c create mode 100644 core/deps/picotcp/stack/pico_tree.c diff --git a/core/deps/picotcp/rules/crc.mk b/core/deps/picotcp/rules/crc.mk new file mode 100644 index 000000000..8ec93440c --- /dev/null +++ b/core/deps/picotcp/rules/crc.mk @@ -0,0 +1 @@ +OPTIONS+=-DPICO_SUPPORT_CRC diff --git a/core/deps/picotcp/rules/debug.mk b/core/deps/picotcp/rules/debug.mk new file mode 100644 index 000000000..481c45dbe --- /dev/null +++ b/core/deps/picotcp/rules/debug.mk @@ -0,0 +1,207 @@ +DEBUG_ALL?=0 + +DEBUG_ARP?=0 +DEBUG_AODV?=0 +DEBUG_PPP?=0 +DEBUG_TAP_ALL?=0 +DEBUG_TAP_GENERAL?=0 +DEBUG_TAP_INFO?=0 +DEBUG_TAP_WIN?=0 +DEBUG_TAP_REG?=0 +DEBUG_DHCP_CLIENT?=0 +DEBUG_DHCP_SERVER?=0 +DEBUG_DNS?=0 +DEBUG_DNS_SD?=0 +DEBUG_FRAG?=0 +DEBUG_IGMP?=0 +DEBUG_IPF?=0 +DEBUG_MCAST?=0 +DEBUG_IPV6?=0 +DEBUG_IPV6_ROUTE?=0 +DEBUG_IPV6_ND?=0 +DEBUG_MDNS?=0 +DEBUG_MLD?=0 +DEBUG_MM?=0 +DEBUG_NAT?=0 +DEBUG_OLSR?=0 +DEBUG_SLAACV4?=0 +DEBUG_SNTP?=0 +DEBUG_TCP_ALL?=0 +DEBUG_TCP_NAGLE?=0 +DEBUG_TCP_OPTIONS?=0 +DEBUG_TCP_GENERAL?=0 +DEBUG_TFTP?=0 +DEBUG_UDP?=0 +DEBUG_6LOWPAN?=0 +DEBUG_RADIOTEST?=0 + +ifneq ($(DEBUG_ALL),0) + DEBUG_ARP=1 + DEBUG_AODV=1 + DEBUG_PPP=1 + DEBUG_TAP_ALL=1 + DEBUG_DHCP_CLIENT=1 + DEBUG_DHCP_SERVER=1 + DEBUG_DNS=1 + DEBUG_DNS_SD=1 + DEBUG_FRAG=1 + DEBUG_IGMP=1 + DEBUG_IPF=1 + DEBUG_MCAST=1 + DEBUG_IPV6=1 + DEBUG_IPV6_ROUTE=1 + DEBUG_IPV6_ND=1 + DEBUG_MDNS=1 + DEBUG_MLD=1 + DEBUG_MM=1 + DEBUG_NAT=1 + DEBUG_OLSR=1 + DEBUG_SLAACV4=1 + DEBUG_SNTP=1 + DEBUG_TCP_ALL=1 + DEBUG_TFTP=1 + DEBUG_UDP=1 + DEBUG_6LOWPAN=1 + DEBUG_RADIOTEST=1 +endif + +ifneq ($(DEBUG_TCP_ALL),0) + DEBUG_TCP_NAGLE=1 + DEBUG_TCP_OPTIONS=1 + DEBUG_TCP_GENERAL=1 +endif + +ifneq ($(DEBUG_TAP_ALL),0) + DEBUG_TAP_GENERAL=1 + DEBUG_TAP_INFO=1 + DEBUG_TAP_WIN=1 + DEBUG_TAP_REG=1 +endif + +ifneq ($(DEBUG_ARP),0) + CFLAGS+=-DDEBUG_ARP +endif + +ifneq ($(DEBUG_AODV),0) + CFLAGS+=-DDEBUG_AODV +endif + +ifneq ($(DEBUG_PPP),0) + CFLAGS+=-DDEBUG_PPP +endif + +ifneq ($(DEBUG_TAP_GENERAL),0) + CFLAGS+=-DDEBUG_TAP_GENERAL +endif + +ifneq ($(DEBUG_TAP_INFO),0) + CFLAGS+=-DDEBUG_TAP_INFO +endif + +ifneq ($(DEBUG_TAP_WIN),0) + CFLAGS+=-DDEBUG_TAP_WIN +endif + +ifneq ($(DEBUG_TAP_REG),0) + CFLAGS+=-DDEBUG_TAP_REG +endif + +ifneq ($(DEBUG_DHCP_CLIENT),0) + CFLAGS+=-DDEBUG_DHCP_CLIENT +endif + +ifneq ($(DEBUG_DHCP_SERVER),0) + CFLAGS+=-DDEBUG_DHCP_SERVER +endif + +ifneq ($(DEBUG_DNS),0) + CFLAGS+=-DDEBUG_DNS +endif + +ifneq ($(DEBUG_DNS_SD),0) + CFLAGS+=-DDEBUG_DNS_SD +endif + +ifneq ($(DEBUG_FRAG),0) + CFLAGS+=-DDEBUG_FRAG +endif + +ifneq ($(DEBUG_IGMP),0) + CFLAGS+=-DDEBUG_IGMP +endif + +ifneq ($(DEBUG_IPF),0) + CFLAGS+=-DDEBUG_IPF +endif + +ifneq ($(DEBUG_MCAST),0) + CFLAGS+=-DDEBUG_MCAST +endif + +ifneq ($(DEBUG_IPV6),0) + CFLAGS+=-DDEBUG_IPV6 +endif + +ifneq ($(DEBUG_IPV6_ROUTE),0) + CFLAGS+=-DDEBUG_IPV6_ROUTE +endif + +ifneq ($(DEBUG_IPV6_ND),0) + CFLAGS+=-DDEBUG_IPV6_ND +endif + +ifneq ($(DEBUG_MDNS),0) + CFLAGS+=-DDEBUG_MDNS +endif + +ifneq ($(DEBUG_MLD),0) + CFLAGS+=-DDEBUG_MLD +endif + +ifneq ($(DEBUG_MM),0) + CFLAGS+=-DDEBUG_MM +endif + +ifneq ($(DEBUG_NAT),0) + CFLAGS+=-DDEBUG_NAT +endif + +ifneq ($(DEBUG_OLSR),0) + CFLAGS+=-DDEBUG_OLSR +endif + +ifneq ($(DEBUG_SLAACV4),0) + CFLAGS+=-DDEBUG_SLAACV4 +endif + +ifneq ($(DEBUG_SNTP),0) + CFLAGS+=-DDEBUG_SNTP +endif + +ifneq ($(DEBUG_TCP_NAGLE),0) + CFLAGS+=-DDEBUG_TCP_NAGLE +endif + +ifneq ($(DEBUG_TCP_OPTIONS),0) + CFLAGS+=-DDEBUG_TCP_OPTIONS +endif + +ifneq ($(DEBUG_TCP_GENERAL),0) + CFLAGS+=-DDEBUG_TCP_GENERAL +endif + +ifneq ($(DEBUG_TFTP),0) + CFLAGS+=-DDEBUG_TFTP +endif + +ifneq ($(DEBUG_UDP),0) + CFLAGS+=-DDEBUG_UDP +endif + +ifneq ($(DEBUG_6LOWPAN),0) + CFLAGS+=-DDEBUG_6LOWPAN +endif + +ifneq ($(DEBUG_RADIOTEST), 0) + CFLAGS+=-DDEBUG_RADIOTEST +endif diff --git a/core/deps/picotcp/rules/dhcp_client.mk b/core/deps/picotcp/rules/dhcp_client.mk new file mode 100644 index 000000000..94a15077d --- /dev/null +++ b/core/deps/picotcp/rules/dhcp_client.mk @@ -0,0 +1,2 @@ +OPTIONS+=-DPICO_SUPPORT_DHCPC +MOD_OBJ+=$(LIBBASE)modules/pico_dhcp_client.o $(LIBBASE)modules/pico_dhcp_common.o diff --git a/core/deps/picotcp/rules/dhcp_server.mk b/core/deps/picotcp/rules/dhcp_server.mk new file mode 100644 index 000000000..c6d2b5918 --- /dev/null +++ b/core/deps/picotcp/rules/dhcp_server.mk @@ -0,0 +1,2 @@ +OPTIONS+=-DPICO_SUPPORT_DHCPD +MOD_OBJ+=$(LIBBASE)modules/pico_dhcp_server.o $(LIBBASE)modules/pico_dhcp_common.o diff --git a/core/deps/picotcp/rules/dns_client.mk b/core/deps/picotcp/rules/dns_client.mk new file mode 100644 index 000000000..f5e14c746 --- /dev/null +++ b/core/deps/picotcp/rules/dns_client.mk @@ -0,0 +1,2 @@ +OPTIONS+=-DPICO_SUPPORT_DNS_CLIENT +MOD_OBJ+=$(LIBBASE)modules/pico_dns_client.o $(LIBBASE)modules/pico_dns_common.o diff --git a/core/deps/picotcp/rules/eth.mk b/core/deps/picotcp/rules/eth.mk new file mode 100644 index 000000000..50b598a79 --- /dev/null +++ b/core/deps/picotcp/rules/eth.mk @@ -0,0 +1,3 @@ +OPTIONS+=-DPICO_SUPPORT_ETH +MOD_OBJ+=$(LIBBASE)modules/pico_arp.o +MOD_OBJ+=$(LIBBASE)modules/pico_ethernet.o diff --git a/core/deps/picotcp/rules/icmp4.mk b/core/deps/picotcp/rules/icmp4.mk new file mode 100644 index 000000000..826e0e09e --- /dev/null +++ b/core/deps/picotcp/rules/icmp4.mk @@ -0,0 +1,5 @@ +OPTIONS+=-DPICO_SUPPORT_ICMP4 +MOD_OBJ+=$(LIBBASE)modules/pico_icmp4.o +ifneq ($(PING),0) + OPTIONS+=-DPICO_SUPPORT_PING +endif diff --git a/core/deps/picotcp/rules/ipv4.mk b/core/deps/picotcp/rules/ipv4.mk new file mode 100644 index 000000000..34d210816 --- /dev/null +++ b/core/deps/picotcp/rules/ipv4.mk @@ -0,0 +1,2 @@ +OPTIONS+=-DPICO_SUPPORT_IPV4 +MOD_OBJ+=$(LIBBASE)modules/pico_ipv4.o diff --git a/core/deps/picotcp/rules/ipv4frag.mk b/core/deps/picotcp/rules/ipv4frag.mk new file mode 100644 index 000000000..a79ccb349 --- /dev/null +++ b/core/deps/picotcp/rules/ipv4frag.mk @@ -0,0 +1,2 @@ +OPTIONS+=-DPICO_SUPPORT_IPV4FRAG +MOD_OBJ+=$(LIBBASE)modules/pico_fragments.o diff --git a/core/deps/picotcp/rules/ppp.mk b/core/deps/picotcp/rules/ppp.mk new file mode 100644 index 000000000..f47c08368 --- /dev/null +++ b/core/deps/picotcp/rules/ppp.mk @@ -0,0 +1,3 @@ +OPTIONS+=-DPICO_SUPPORT_PPP +MOD_OBJ+=$(LIBBASE)modules/pico_dev_ppp.o + diff --git a/core/deps/picotcp/rules/tap.mk b/core/deps/picotcp/rules/tap.mk new file mode 100644 index 000000000..2f3e14978 --- /dev/null +++ b/core/deps/picotcp/rules/tap.mk @@ -0,0 +1 @@ +MOD_OBJ+=$(LIBBASE)modules/pico_dev_tap.o diff --git a/core/deps/picotcp/rules/tcp.mk b/core/deps/picotcp/rules/tcp.mk new file mode 100644 index 000000000..ae53034a5 --- /dev/null +++ b/core/deps/picotcp/rules/tcp.mk @@ -0,0 +1,3 @@ +OPTIONS+=-DPICO_SUPPORT_TCP +MOD_OBJ+=$(LIBBASE)modules/pico_tcp.o +MOD_OBJ+=$(LIBBASE)modules/pico_socket_tcp.o diff --git a/core/deps/picotcp/rules/tun.mk b/core/deps/picotcp/rules/tun.mk new file mode 100644 index 000000000..a5b93fec9 --- /dev/null +++ b/core/deps/picotcp/rules/tun.mk @@ -0,0 +1 @@ +MOD_OBJ+=$(LIBBASE)modules/pico_dev_tun.o diff --git a/core/deps/picotcp/rules/udp.mk b/core/deps/picotcp/rules/udp.mk new file mode 100644 index 000000000..ec9157c11 --- /dev/null +++ b/core/deps/picotcp/rules/udp.mk @@ -0,0 +1,3 @@ +OPTIONS+=-DPICO_SUPPORT_UDP +MOD_OBJ+=$(LIBBASE)modules/pico_udp.o +MOD_OBJ+=$(LIBBASE)modules/pico_socket_udp.o diff --git a/core/deps/picotcp/stack/pico_device.c b/core/deps/picotcp/stack/pico_device.c new file mode 100644 index 000000000..4d06f1a74 --- /dev/null +++ b/core/deps/picotcp/stack/pico_device.c @@ -0,0 +1,482 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2017 Altran Intelligent Systems. Some rights reserved. + See COPYING, LICENSE.GPLv2 and LICENSE.GPLv3 for usage. + + . + + Authors: Daniele Lacamera + *********************************************************************/ + +#include "pico_config.h" +#include "pico_device.h" +#include "pico_stack.h" +#include "pico_protocol.h" +#include "pico_tree.h" +#include "pico_ipv6.h" +#include "pico_ipv4.h" +#include "pico_icmp6.h" +#include "pico_eth.h" +#include "pico_802154.h" +#include "pico_6lowpan.h" +#include "pico_6lowpan_ll.h" +#include "pico_addressing.h" +#define PICO_DEVICE_DEFAULT_MTU (1500) + +struct pico_devices_rr_info { + struct pico_tree_node *node_in, *node_out; +}; + +static struct pico_devices_rr_info Devices_rr_info = { + NULL, NULL +}; + +static int pico_dev_cmp(void *ka, void *kb) +{ + struct pico_device *a = ka, *b = kb; + if (a->hash < b->hash) + return -1; + + + if (a->hash > b->hash) + return 1; + + return 0; +} + +PICO_TREE_DECLARE(Device_tree, pico_dev_cmp); + +#ifdef PICO_SUPPORT_6LOWPAN +static struct pico_ipv6_link * pico_6lowpan_link_add(struct pico_device *dev, const struct pico_ip6 *prefix) +{ + struct pico_ip6 netmask64 = {{0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}; + struct pico_6lowpan_info *info = (struct pico_6lowpan_info *)dev->eth; + struct pico_ipv6_link *link = NULL; /* Make sure to return NULL */ + struct pico_ip6 newaddr; + + memcpy(newaddr.addr, prefix->addr, PICO_SIZE_IP6); + memcpy(newaddr.addr + 8, info->addr_ext.addr, SIZE_6LOWPAN_EXT); + newaddr.addr[8] = newaddr.addr[8] ^ 0x02; /* Toggle U/L bit */ + + /* RFC6775: No Duplicate Address Detection (DAD) is performed if + * EUI-64-based IPv6 addresses are used (as these addresses are assumed + * to be globally unique). */ + if ((link = pico_ipv6_link_add_no_dad(dev, newaddr, netmask64))) { + if (pico_ipv6_is_linklocal(newaddr.addr)) + pico_6lp_nd_start_soliciting(link, NULL); + else + pico_6lp_nd_register(link); + } + + return link; +} + +static int pico_6lowpan_store_info(struct pico_device *dev, const uint8_t *mac) +{ + if ((dev->eth = PICO_ZALLOC(sizeof(struct pico_6lowpan_info)))) { + memcpy(dev->eth, mac, sizeof(struct pico_6lowpan_info)); + return 0; + } else { + pico_err = PICO_ERR_ENOMEM; + return -1; + } +} +#endif + +#ifdef PICO_SUPPORT_IPV6 +static void device_init_ipv6_final(struct pico_device *dev, struct pico_ip6 *linklocal) +{ + dev->hostvars.basetime = PICO_ND_REACHABLE_TIME; + /* RFC 4861 $6.3.2 value between 0.5 and 1.5 times basetime */ + dev->hostvars.reachabletime = ((5 + (pico_rand() % 10)) * PICO_ND_REACHABLE_TIME) / 10; + dev->hostvars.retranstime = PICO_ND_RETRANS_TIMER; + pico_icmp6_router_solicitation(dev, linklocal, NULL); + dev->hostvars.hoplimit = PICO_IPV6_DEFAULT_HOP; +} + +struct pico_ipv6_link *pico_ipv6_link_add_local(struct pico_device *dev, const struct pico_ip6 *prefix) +{ + struct pico_ip6 netmask64 = {{0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}; + struct pico_ipv6_link *link = NULL; /* Make sure to return NULL */ + struct pico_ip6 newaddr; + + if (0) {} +#ifdef PICO_SUPPORT_6LOWPAN + else if (PICO_DEV_IS_6LOWPAN(dev)) { + link = pico_6lowpan_link_add(dev, prefix); + } +#endif + else { + memcpy(newaddr.addr, prefix->addr, PICO_SIZE_IP6); + /* modified EUI-64 + invert universal/local bit */ + newaddr.addr[8] = (dev->eth->mac.addr[0] ^ 0x02); + newaddr.addr[9] = dev->eth->mac.addr[1]; + newaddr.addr[10] = dev->eth->mac.addr[2]; + newaddr.addr[11] = 0xff; + newaddr.addr[12] = 0xfe; + newaddr.addr[13] = dev->eth->mac.addr[3]; + newaddr.addr[14] = dev->eth->mac.addr[4]; + newaddr.addr[15] = dev->eth->mac.addr[5]; + if ((link = pico_ipv6_link_add(dev, newaddr, netmask64))) { + device_init_ipv6_final(dev, &newaddr); + } + } + + return link; +} +#endif +static int device_init_mac(struct pico_device *dev, const uint8_t *mac) +{ +#ifdef PICO_SUPPORT_IPV6 + struct pico_ip6 linklocal = {{0xfe, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xaa, 0xaa, 0xaa, 0xff, 0xfe, 0xaa, 0xaa, 0xaa}}; +#endif + + if (0) {} +#ifdef PICO_SUPPORT_6LOWPAN + else if (PICO_DEV_IS_6LOWPAN(dev)) { + if (pico_6lowpan_store_info(dev, mac)) + return -1; + } +#endif + else { + if ((dev->eth = PICO_ZALLOC(sizeof(struct pico_ethdev)))) { + memcpy(dev->eth->mac.addr, mac, PICO_SIZE_ETH); + } else { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + } + +#ifdef PICO_SUPPORT_IPV6 + if (pico_ipv6_link_add_local(dev, &linklocal) == NULL) { + PICO_FREE(dev->q_in); + PICO_FREE(dev->q_out); + PICO_FREE(dev->eth); + return -1; + } +#endif + + return 0; +} + +int pico_device_ipv6_random_ll(struct pico_device *dev) +{ +#ifdef PICO_SUPPORT_IPV6 + struct pico_ip6 linklocal = {{0xfe, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xaa, 0xaa, 0xaa, 0xff, 0xfe, 0xaa, 0xaa, 0xaa}}; + struct pico_ip6 netmask6 = {{0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}; + uint32_t len = (uint32_t)strlen(dev->name); + if (strcmp(dev->name, "loop")) { + do { + /* privacy extension + unset universal/local and individual/group bit */ + len = pico_rand(); + linklocal.addr[8] = (uint8_t)((len & 0xffu) & (uint8_t)(~0x03)); + linklocal.addr[9] = (uint8_t)(len >> 8); + linklocal.addr[10] = (uint8_t)(len >> 16); + linklocal.addr[11] = (uint8_t)(len >> 24); + len = pico_rand(); + linklocal.addr[12] = (uint8_t)len; + linklocal.addr[13] = (uint8_t)(len >> 8); + linklocal.addr[14] = (uint8_t)(len >> 16); + linklocal.addr[15] = (uint8_t)(len >> 24); + pico_rand_feed(dev->hash); + } while (pico_ipv6_link_get(&linklocal)); + + if (pico_ipv6_link_add(dev, linklocal, netmask6) == NULL) { + return -1; + } + } + +#endif + return 0; +} + +static int device_init_nomac(struct pico_device *dev) +{ + if (pico_device_ipv6_random_ll(dev) < 0) { + PICO_FREE(dev->q_in); + PICO_FREE(dev->q_out); + return -1; + } + + dev->eth = NULL; + return 0; +} + +#define DEBUG_IPV6(ip) { \ + char ipstr[40] = { 0 }; \ + pico_ipv6_to_string(ipstr, (ip).addr); \ + dbg("IPv6 (%s)\n", ipstr); \ + } + +int pico_device_init(struct pico_device *dev, const char *name, const uint8_t *mac) +{ + uint32_t len = (uint32_t)strlen(name); + int ret = 0; + + if(len > MAX_DEVICE_NAME) + len = MAX_DEVICE_NAME; + + memcpy(dev->name, name, len); + dev->hash = pico_hash(dev->name, len); + + Devices_rr_info.node_in = NULL; + Devices_rr_info.node_out = NULL; + dev->q_in = PICO_ZALLOC(sizeof(struct pico_queue)); + if (!dev->q_in) + return -1; + + dev->q_out = PICO_ZALLOC(sizeof(struct pico_queue)); + if (!dev->q_out) { + PICO_FREE(dev->q_in); + return -1; + } + + if (pico_tree_insert(&Device_tree, dev)) { + PICO_FREE(dev->q_in); + PICO_FREE(dev->q_out); + return -1; + } + if (!dev->mtu) + dev->mtu = PICO_DEVICE_DEFAULT_MTU; + +#ifdef PICO_SUPPORT_6LOWPAN + if (PICO_DEV_IS_6LOWPAN(dev) && LL_MODE_ETHERNET == dev->mode) + return -1; +#endif + + if (mac) { + ret = device_init_mac(dev, mac); + } else { + if (!dev->mode) { + ret = device_init_nomac(dev); + } +#ifdef PICO_SUPPORT_6LOWPAN + else { + /* RFC6775: Link Local to be formed based on EUI-64 as per RFC6775 */ + dbg("Link local address to be formed based on EUI-64\n"); + return -1; + } +#endif + } + return ret; +} + +static void pico_queue_destroy(struct pico_queue *q) +{ + if (q) { + pico_queue_empty(q); + PICO_FREE(q); + } +} + +void pico_device_destroy(struct pico_device *dev) +{ + + pico_queue_destroy(dev->q_in); + pico_queue_destroy(dev->q_out); + + if (!dev->mode && dev->eth) + PICO_FREE(dev->eth); + +#ifdef PICO_SUPPORT_IPV4 + pico_ipv4_cleanup_links(dev); +#endif +#ifdef PICO_SUPPORT_IPV6 + pico_ipv6_cleanup_links(dev); +#endif + pico_tree_delete(&Device_tree, dev); + + if (dev->destroy) + dev->destroy(dev); + + Devices_rr_info.node_in = NULL; + Devices_rr_info.node_out = NULL; + PICO_FREE(dev); +} + +static int check_dev_serve_interrupt(struct pico_device *dev, int loop_score) +{ + if ((dev->__serving_interrupt) && (dev->dsr)) { + /* call dsr routine */ + loop_score = dev->dsr(dev, loop_score); + } + + return loop_score; +} + +static int check_dev_serve_polling(struct pico_device *dev, int loop_score) +{ + if (dev->poll) { + loop_score = dev->poll(dev, loop_score); + } + + return loop_score; +} + +static int devloop_in(struct pico_device *dev, int loop_score) +{ + struct pico_frame *f; + while(loop_score > 0) { + if (dev->q_in->frames == 0) + break; + + /* Receive */ + f = pico_dequeue(dev->q_in); + if (f) { + pico_datalink_receive(f); + loop_score--; + } + } + return loop_score; +} + +static int devloop_sendto_dev(struct pico_device *dev, struct pico_frame *f) +{ +#ifdef PICO_SUPPORT_6LOWPAN + if (PICO_DEV_IS_6LOWPAN(dev)) { + return (pico_6lowpan_ll_sendto_dev(dev, f) <= 0); + } +#endif + return (dev->send(dev, f->start, (int)f->len) <= 0); +} + +static int devloop_out(struct pico_device *dev, int loop_score) +{ + struct pico_frame *f; + while(loop_score > 0) { + if (dev->q_out->frames == 0) + break; + + /* Device dequeue + send */ + f = pico_queue_peek(dev->q_out); + if (!f) + break; + + if (devloop_sendto_dev(dev, f) == 0) { /* success. */ + f = pico_dequeue(dev->q_out); + pico_frame_discard(f); /* SINGLE POINT OF DISCARD for OUTGOING FRAMES */ + loop_score--; + } else + break; /* Don't discard */ + + } + + return loop_score; +} + +static int devloop(struct pico_device *dev, int loop_score, int direction) +{ + /* If device supports interrupts, read the value of the condition and trigger the dsr */ + loop_score = check_dev_serve_interrupt(dev, loop_score); + + /* If device supports polling, give control. Loop score is managed internally, + * remaining loop points are returned. */ + loop_score = check_dev_serve_polling(dev, loop_score); + + if (direction == PICO_LOOP_DIR_OUT) + loop_score = devloop_out(dev, loop_score); + else + loop_score = devloop_in(dev, loop_score); + + return loop_score; +} + + +static struct pico_tree_node *pico_dev_roundrobin_start(int direction) +{ + if (Devices_rr_info.node_in == NULL) + Devices_rr_info.node_in = pico_tree_firstNode(Device_tree.root); + + if (Devices_rr_info.node_out == NULL) + Devices_rr_info.node_out = pico_tree_firstNode(Device_tree.root); + + if (direction == PICO_LOOP_DIR_IN) + return Devices_rr_info.node_in; + else + return Devices_rr_info.node_out; +} + +static void pico_dev_roundrobin_end(int direction, struct pico_tree_node *last) +{ + if (direction == PICO_LOOP_DIR_IN) + Devices_rr_info.node_in = last; + else + Devices_rr_info.node_out = last; +} + +#define DEV_LOOP_MIN 16 + +int pico_devices_loop(int loop_score, int direction) +{ + struct pico_device *start, *next; + struct pico_tree_node *next_node = pico_dev_roundrobin_start(direction); + + if (!next_node) + return loop_score; + + next = next_node->keyValue; + start = next; + + /* round-robin all devices, break if traversed all devices */ + while ((loop_score > DEV_LOOP_MIN) && (next != NULL)) { + loop_score = devloop(next, loop_score, direction); + next_node = pico_tree_next(next_node); + next = next_node->keyValue; + if (next == NULL) + { + next_node = pico_tree_firstNode(Device_tree.root); + next = next_node->keyValue; + } + + if (next == start) + break; + } + pico_dev_roundrobin_end(direction, next_node); + return loop_score; +} + +struct pico_device *pico_get_device(const char*name) +{ + struct pico_device *dev; + struct pico_tree_node *index; + pico_tree_foreach(index, &Device_tree){ + dev = index->keyValue; + if(strcmp(name, dev->name) == 0) + return dev; + } + return NULL; +} + +int32_t pico_device_broadcast(struct pico_frame *f) +{ + struct pico_tree_node *index; + int32_t ret = -1; + + pico_tree_foreach(index, &Device_tree) + { + struct pico_device *dev = index->keyValue; + if(dev != f->dev) + { + struct pico_frame *copy = pico_frame_copy(f); + + if(!copy) + break; + + copy->dev = dev; + copy->dev->send(copy->dev, copy->start, (int)copy->len); + pico_frame_discard(copy); + } + else + { + ret = f->dev->send(f->dev, f->start, (int)f->len); + } + } + return ret; +} + +int pico_device_link_state(struct pico_device *dev) +{ + if (!dev->link_state) + return 1; /* Not supported, assuming link is always up */ + + return dev->link_state(dev); +} diff --git a/core/deps/picotcp/stack/pico_frame.c b/core/deps/picotcp/stack/pico_frame.c new file mode 100644 index 000000000..7aad7f3b1 --- /dev/null +++ b/core/deps/picotcp/stack/pico_frame.c @@ -0,0 +1,329 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2017 Altran Intelligent Systems. Some rights reserved. + See COPYING, LICENSE.GPLv2 and LICENSE.GPLv3 for usage. + + . + + Authors: Daniele Lacamera + *********************************************************************/ + +#include "pico_config.h" +#include "pico_frame.h" +#include "pico_protocol.h" +#include "pico_stack.h" +#include "pico_socket.h" + +#ifdef PICO_SUPPORT_DEBUG_MEMORY +static int n_frames_allocated; +#endif + +/** frame alloc/dealloc/copy **/ +void pico_frame_discard(struct pico_frame *f) +{ + if (!f) + return; + + (*f->usage_count)--; + if (*f->usage_count == 0) { + if (f->flags & PICO_FRAME_FLAG_EXT_USAGE_COUNTER) + PICO_FREE(f->usage_count); + +#ifdef PICO_SUPPORT_DEBUG_MEMORY + dbg("Discarded buffer @%p, caller: %p\n", f->buffer, __builtin_return_address(3)); + dbg("DEBUG MEMORY: %d frames in use.\n", --n_frames_allocated); +#endif + if (!(f->flags & PICO_FRAME_FLAG_EXT_BUFFER)) + PICO_FREE(f->buffer); + else if (f->notify_free) + f->notify_free(f->buffer); + + if (f->info) + PICO_FREE(f->info); + } + +#ifdef PICO_SUPPORT_DEBUG_MEMORY + else { + dbg("Removed frame @%p(copy), usage count now: %d\n", f, *f->usage_count); + } +#endif + PICO_FREE(f); +} + +struct pico_frame *pico_frame_copy(struct pico_frame *f) +{ + struct pico_frame *new = PICO_ZALLOC(sizeof(struct pico_frame)); + if (!new) + return NULL; + + memcpy(new, f, sizeof(struct pico_frame)); + *(new->usage_count) += 1; +#ifdef PICO_SUPPORT_DEBUG_MEMORY + dbg("Copied frame @%p, into %p, usage count now: %d\n", f, new, *new->usage_count); +#endif + new->next = NULL; + return new; +} + +static struct pico_frame *pico_frame_do_alloc(uint32_t size, int zerocopy, int ext_buffer) +{ + struct pico_frame *p = PICO_ZALLOC(sizeof(struct pico_frame)); + uint32_t frame_buffer_size = size; + if (!p) + return NULL; + + if (ext_buffer && !zerocopy) { + /* external buffer implies zerocopy flag! */ + PICO_FREE(p); + return NULL; + } + + if (!zerocopy) { + unsigned int align = size % sizeof(uint32_t); + /* Ensure that usage_count starts on an aligned address */ + if (align) { + frame_buffer_size += (uint32_t)sizeof(uint32_t) - align; + } + + p->buffer = PICO_ZALLOC((size_t)frame_buffer_size + sizeof(uint32_t)); + if (!p->buffer) { + PICO_FREE(p); + return NULL; + } + + p->usage_count = (uint32_t *)(((uint8_t*)p->buffer) + frame_buffer_size); + } else { + p->buffer = NULL; + p->flags |= PICO_FRAME_FLAG_EXT_USAGE_COUNTER; + p->usage_count = PICO_ZALLOC(sizeof(uint32_t)); + if (!p->usage_count) { + PICO_FREE(p); + return NULL; + } + } + + p->buffer_len = size; + + /* By default, frame content is the full buffer. */ + p->start = p->buffer; + p->len = p->buffer_len; + *p->usage_count = 1; + p->net_hdr = p->buffer; + p->datalink_hdr = p->buffer; + p->transport_hdr = p->buffer; + p->app_hdr = p->buffer; + p->payload = p->buffer; + + if (ext_buffer) + p->flags |= PICO_FRAME_FLAG_EXT_BUFFER; + +#ifdef PICO_SUPPORT_DEBUG_MEMORY + dbg("Allocated buffer @%p, len= %d caller: %p\n", p->buffer, p->buffer_len, __builtin_return_address(2)); + dbg("DEBUG MEMORY: %d frames in use.\n", ++n_frames_allocated); +#endif + return p; +} + +struct pico_frame *pico_frame_alloc(uint32_t size) +{ + return pico_frame_do_alloc(size, 0, 0); +} + +static uint8_t * +pico_frame_new_buffer(struct pico_frame *f, uint32_t size, uint32_t *oldsize) +{ + uint8_t *oldbuf; + uint32_t usage_count, *p_old_usage; + uint32_t frame_buffer_size; + unsigned int align; + + if (!f || (size < f->buffer_len)) { + return NULL; + } + + align = size % sizeof(uint32_t); + frame_buffer_size = size; + if (align) { + frame_buffer_size += (uint32_t)sizeof(uint32_t) - align; + } + + oldbuf = f->buffer; + *oldsize = f->buffer_len; + usage_count = *(f->usage_count); + p_old_usage = f->usage_count; + f->buffer = PICO_ZALLOC((size_t)frame_buffer_size + sizeof(uint32_t)); + if (!f->buffer) { + f->buffer = oldbuf; + return NULL; + } + + f->usage_count = (uint32_t *)(((uint8_t*)f->buffer) + frame_buffer_size); + *f->usage_count = usage_count; + f->buffer_len = size; + + if (f->flags & PICO_FRAME_FLAG_EXT_USAGE_COUNTER) + PICO_FREE(p_old_usage); + /* Now, the frame is not zerocopy anymore, and the usage counter has been moved within it */ + return oldbuf; +} + +static int +pico_frame_update_pointers(struct pico_frame *f, ptrdiff_t addr_diff, uint8_t *oldbuf) +{ + f->net_hdr += addr_diff; + f->datalink_hdr += addr_diff; + f->transport_hdr += addr_diff; + f->app_hdr += addr_diff; + f->start += addr_diff; + f->payload += addr_diff; + + if (!(f->flags & PICO_FRAME_FLAG_EXT_BUFFER)) + PICO_FREE(oldbuf); + else if (f->notify_free) + f->notify_free(oldbuf); + + f->flags = 0; + return 0; +} + +int pico_frame_grow_head(struct pico_frame *f, uint32_t size) +{ + ptrdiff_t addr_diff = 0; + uint32_t oldsize = 0; + uint8_t *oldbuf = pico_frame_new_buffer(f, size, &oldsize); + if (!oldbuf) + return -1; + + /* Put old buffer at the end of new buffer */ + memcpy(f->buffer + f->buffer_len - oldsize, oldbuf, (size_t)oldsize); + addr_diff = (ptrdiff_t)(f->buffer + f->buffer_len - oldsize - oldbuf); + + return pico_frame_update_pointers(f, addr_diff, oldbuf); +} + +int pico_frame_grow(struct pico_frame *f, uint32_t size) +{ + ptrdiff_t addr_diff = 0; + uint32_t oldsize = 0; + uint8_t *oldbuf = pico_frame_new_buffer(f, size, &oldsize); + if (!oldbuf) + return -1; + + /* Just put old buffer at the beginning of new buffer */ + memcpy(f->buffer, oldbuf, (size_t)oldsize); + addr_diff = (ptrdiff_t)(f->buffer - oldbuf); + + return pico_frame_update_pointers(f, addr_diff, oldbuf); +} + +struct pico_frame *pico_frame_alloc_skeleton(uint32_t size, int ext_buffer) +{ + return pico_frame_do_alloc(size, 1, ext_buffer); +} + +int pico_frame_skeleton_set_buffer(struct pico_frame *f, void *buf) +{ + if (!buf) + return -1; + + f->buffer = (uint8_t *) buf; + f->start = f->buffer; + return 0; +} + +struct pico_frame *pico_frame_deepcopy(struct pico_frame *f) +{ + struct pico_frame *new = pico_frame_alloc(f->buffer_len); + ptrdiff_t addr_diff; + unsigned char *buf; + uint32_t *uc; + if (!new) + return NULL; + + /* Save the two key pointers... */ + buf = new->buffer; + uc = new->usage_count; + + /* Overwrite all fields with originals */ + memcpy(new, f, sizeof(struct pico_frame)); + + /* ...restore the two key pointers */ + new->buffer = buf; + new->usage_count = uc; + + /* Update in-buffer pointers with offset */ + addr_diff = (ptrdiff_t)(new->buffer - f->buffer); + new->datalink_hdr += addr_diff; + new->net_hdr += addr_diff; + new->transport_hdr += addr_diff; + new->app_hdr += addr_diff; + new->start += addr_diff; + new->payload += addr_diff; + + if (f->info) { + new->info = PICO_ZALLOC(sizeof(struct pico_remote_endpoint)); + if (!new->info) { + pico_frame_discard(new); + return NULL; + } + memcpy(new->info, f->info, sizeof(struct pico_remote_endpoint)); + } + +#ifdef PICO_SUPPORT_DEBUG_MEMORY + dbg("Deep-Copied frame @%p, into %p, usage count now: %d\n", f, new, *new->usage_count); +#endif + new->next = NULL; + return new; +} + + +static inline uint32_t pico_checksum_adder(uint32_t sum, void *data, uint32_t len) +{ + uint16_t *buf = (uint16_t *)data; + uint16_t *stop; + + if (len & 0x01) { + --len; +#ifdef PICO_BIGENDIAN + sum += (((uint8_t *)data)[len]) << 8; +#else + sum += ((uint8_t *)data)[len]; +#endif + } + + stop = (uint16_t *)(((uint8_t *)data) + len); + + while (buf < stop) { + sum += *buf++; + } + return sum; +} + +static inline uint16_t pico_checksum_finalize(uint32_t sum) +{ + while (sum >> 16) { /* a second carry is possible! */ + sum = (sum & 0x0000FFFF) + (sum >> 16); + } + return short_be((uint16_t) ~sum); +} + +/** + * Calculate checksum of a given string + */ +uint16_t pico_checksum(void *inbuf, uint32_t len) +{ + uint32_t sum; + + sum = pico_checksum_adder(0, inbuf, len); + return pico_checksum_finalize(sum); +} + +/* WARNING: len1 MUST be an EVEN number */ +uint16_t pico_dualbuffer_checksum(void *inbuf1, uint32_t len1, void *inbuf2, uint32_t len2) +{ + uint32_t sum; + + sum = pico_checksum_adder(0, inbuf1, len1); + sum = pico_checksum_adder(sum, inbuf2, len2); + return pico_checksum_finalize(sum); +} + diff --git a/core/deps/picotcp/stack/pico_md5.c b/core/deps/picotcp/stack/pico_md5.c new file mode 100644 index 000000000..2c235be94 --- /dev/null +++ b/core/deps/picotcp/stack/pico_md5.c @@ -0,0 +1,43 @@ +/********************************************************************* + * PicoTCP. Copyright (c) 2015-2017 Altran Intelligent Systems. Some rights reserved. + * See COPYING, LICENSE.GPLv2 and LICENSE.GPLv3 for usage. + * + * Authors: Daniele Lacamera + * *********************************************************************/ + + +#include + +#if defined (PICO_SUPPORT_CYASSL) +#include + +void pico_md5sum(uint8_t *dst, const uint8_t *src, size_t len) +{ + Md5 md5; + InitMd5(&md5); + Md5Update(&md5, src, len); + Md5Final(&md5, dst); +} + +#elif defined (PICO_SUPPORT_POLARSSL) +#include + +void pico_md5sum(uint8_t *dst, const uint8_t *src, size_t len) +{ + md5(src, len, dst); +} + +#else +static void (*do_pico_md5sum)(uint8_t *dst, const uint8_t *src, size_t len); +void pico_md5sum(uint8_t *dst, const uint8_t *src, size_t len) +{ + if (do_pico_md5sum) { + do_pico_md5sum(dst, src, len); + } +} + +void pico_register_md5sum(void (*md5)(uint8_t *, const uint8_t *, size_t)) +{ + do_pico_md5sum = md5; +} +#endif diff --git a/core/deps/picotcp/stack/pico_protocol.c b/core/deps/picotcp/stack/pico_protocol.c new file mode 100644 index 000000000..085a00da9 --- /dev/null +++ b/core/deps/picotcp/stack/pico_protocol.c @@ -0,0 +1,226 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2017 Altran Intelligent Systems. Some rights reserved. + See COPYING, LICENSE.GPLv2 and LICENSE.GPLv3 for usage. + + . + + Authors: Daniele Lacamera + *********************************************************************/ + + +#include "pico_protocol.h" +#include "pico_tree.h" + +struct pico_proto_rr +{ + struct pico_tree *t; + struct pico_tree_node *node_in, *node_out; +}; + + +static int pico_proto_cmp(void *ka, void *kb) +{ + struct pico_protocol *a = ka, *b = kb; + if (a->hash < b->hash) + return -1; + + if (a->hash > b->hash) + return 1; + + return 0; +} + +static PICO_TREE_DECLARE(Datalink_proto_tree, pico_proto_cmp); +static PICO_TREE_DECLARE(Network_proto_tree, pico_proto_cmp); +static PICO_TREE_DECLARE(Transport_proto_tree, pico_proto_cmp); +static PICO_TREE_DECLARE(Socket_proto_tree, pico_proto_cmp); + +/* Static variables to keep track of the round robin loop */ +static struct pico_proto_rr proto_rr_datalink = { + &Datalink_proto_tree, NULL, NULL +}; +static struct pico_proto_rr proto_rr_network = { + &Network_proto_tree, NULL, NULL +}; +static struct pico_proto_rr proto_rr_transport = { + &Transport_proto_tree, NULL, NULL +}; +static struct pico_proto_rr proto_rr_socket = { + &Socket_proto_tree, NULL, NULL +}; + +static int proto_loop_in(struct pico_protocol *proto, int loop_score) +{ + struct pico_frame *f; + while(loop_score > 0) { + if (proto->q_in->frames == 0) + break; + + f = pico_dequeue(proto->q_in); + if ((f) && (proto->process_in(proto, f) > 0)) { + loop_score--; + } + } + return loop_score; +} + +static int proto_loop_out(struct pico_protocol *proto, int loop_score) +{ + struct pico_frame *f; + while(loop_score > 0) { + if (proto->q_out->frames == 0) + break; + + f = pico_dequeue(proto->q_out); + if ((f) && (proto->process_out(proto, f) > 0)) { + loop_score--; + } + } + return loop_score; +} + +static int proto_loop(struct pico_protocol *proto, int loop_score, int direction) +{ + + if (direction == PICO_LOOP_DIR_IN) + loop_score = proto_loop_in(proto, loop_score); + else if (direction == PICO_LOOP_DIR_OUT) + loop_score = proto_loop_out(proto, loop_score); + + return loop_score; +} + +static struct pico_tree_node *roundrobin_init(struct pico_proto_rr *rr, int direction) +{ + struct pico_tree_node *next_node = NULL; + /* Initialization (takes place only once) */ + if (rr->node_in == NULL) + rr->node_in = pico_tree_firstNode(rr->t->root); + + if (rr->node_out == NULL) + rr->node_out = pico_tree_firstNode(rr->t->root); + + if (direction == PICO_LOOP_DIR_IN) + next_node = rr->node_in; + else + next_node = rr->node_out; + + return next_node; +} + +static void roundrobin_end(struct pico_proto_rr *rr, int direction, struct pico_tree_node *last) +{ + if (direction == PICO_LOOP_DIR_IN) + rr->node_in = last; + else + rr->node_out = last; +} + +static int pico_protocol_generic_loop(struct pico_proto_rr *rr, int loop_score, int direction) +{ + struct pico_protocol *start, *next; + struct pico_tree_node *next_node = roundrobin_init(rr, direction); + + if (!next_node) + return loop_score; + + next = next_node->keyValue; + + /* init start node */ + start = next; + + /* round-robin all layer protocols, break if traversed all protocols */ + while (loop_score > 1 && next != NULL) { + loop_score = proto_loop(next, loop_score, direction); + next_node = pico_tree_next(next_node); + next = next_node->keyValue; + if (next == NULL) + { + next_node = pico_tree_firstNode(rr->t->root); + next = next_node->keyValue; + } + + if (next == start) + break; + } + roundrobin_end(rr, direction, next_node); + return loop_score; +} + +int pico_protocol_datalink_loop(int loop_score, int direction) +{ + return pico_protocol_generic_loop(&proto_rr_datalink, loop_score, direction); +} + +int pico_protocol_network_loop(int loop_score, int direction) +{ + return pico_protocol_generic_loop(&proto_rr_network, loop_score, direction); +} + +int pico_protocol_transport_loop(int loop_score, int direction) +{ + return pico_protocol_generic_loop(&proto_rr_transport, loop_score, direction); +} + +int pico_protocol_socket_loop(int loop_score, int direction) +{ + return pico_protocol_generic_loop(&proto_rr_socket, loop_score, direction); +} + +int pico_protocols_loop(int loop_score) +{ +/* + loop_score = pico_protocol_datalink_loop(loop_score); + loop_score = pico_protocol_network_loop(loop_score); + loop_score = pico_protocol_transport_loop(loop_score); + loop_score = pico_protocol_socket_loop(loop_score); + */ + return loop_score; +} + +static void proto_layer_rr_reset(struct pico_proto_rr *rr) +{ + rr->node_in = NULL; + rr->node_out = NULL; +} + +void pico_protocol_init(struct pico_protocol *p) +{ + struct pico_tree *tree = NULL; + struct pico_proto_rr *proto = NULL; + + if (!p) + return; + + p->hash = pico_hash(p->name, (uint32_t)strlen(p->name)); + switch (p->layer) { + case PICO_LAYER_DATALINK: + tree = &Datalink_proto_tree; + proto = &proto_rr_datalink; + break; + case PICO_LAYER_NETWORK: + tree = &Network_proto_tree; + proto = &proto_rr_network; + break; + case PICO_LAYER_TRANSPORT: + tree = &Transport_proto_tree; + proto = &proto_rr_transport; + break; + case PICO_LAYER_SOCKET: + tree = &Socket_proto_tree; + proto = &proto_rr_socket; + break; + default: + dbg("Unknown protocol: %s (layer: %d)\n", p->name, p->layer); + return; + } + + if (pico_tree_insert(tree, p)) { + dbg("Failed to insert protocol %s\n", p->name); + return; + } + + proto_layer_rr_reset(proto); + dbg("Protocol %s registered (layer: %d).\n", p->name, p->layer); +} + diff --git a/core/deps/picotcp/stack/pico_socket.c b/core/deps/picotcp/stack/pico_socket.c new file mode 100644 index 000000000..8f11a3e82 --- /dev/null +++ b/core/deps/picotcp/stack/pico_socket.c @@ -0,0 +1,2287 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2017 Altran Intelligent Systems. Some rights reserved. + See COPYING, LICENSE.GPLv2 and LICENSE.GPLv3 for usage. + + + Authors: Daniele Lacamera + *********************************************************************/ + + +#include "pico_config.h" +#include "pico_queue.h" +#include "pico_socket.h" +#include "pico_ipv4.h" +#include "pico_ipv6.h" +#include "pico_udp.h" +#include "pico_tcp.h" +#include "pico_stack.h" +#include "pico_icmp4.h" +#include "pico_nat.h" +#include "pico_tree.h" +#include "pico_device.h" +#include "pico_socket_multicast.h" +#include "pico_socket_tcp.h" +#include "pico_socket_udp.h" + +#if defined (PICO_SUPPORT_IPV4) || defined (PICO_SUPPORT_IPV6) +#if defined (PICO_SUPPORT_TCP) || defined (PICO_SUPPORT_UDP) + + +#define PROTO(s) ((s)->proto->proto_number) +#define PICO_MIN_MSS (1280) +#define TCP_STATE(s) (s->state & PICO_SOCKET_STATE_TCP) + +#ifdef PICO_SUPPORT_MUTEX +static void *Mutex = NULL; +#endif + +/* Mockables */ +#if defined UNIT_TEST +# define MOCKABLE __attribute__((weak)) +#else +# define MOCKABLE +#endif + +#define PROTO(s) ((s)->proto->proto_number) + +#define PICO_SOCKET_MTU 1480 /* Ethernet MTU(1500) - IP header size(20) */ + +#ifdef PICO_SUPPORT_IPV4FRAG + +#ifdef DEBUG_FRAG +#define frag_dbg dbg +#else +#define frag_dbg(...) do {} while(0) +#endif + +#endif + +static struct pico_sockport *sp_udp = NULL, *sp_tcp = NULL; + +struct pico_frame *pico_socket_frame_alloc(struct pico_socket *s, struct pico_device *dev, uint16_t len); + +static int socket_cmp_family(struct pico_socket *a, struct pico_socket *b) +{ + uint32_t a_is_ip6 = is_sock_ipv6(a); + uint32_t b_is_ip6 = is_sock_ipv6(b); + (void)a; + (void)b; + if (a_is_ip6 < b_is_ip6) + return -1; + + if (a_is_ip6 > b_is_ip6) + return 1; + + return 0; +} + + +static int socket_cmp_ipv6(struct pico_socket *a, struct pico_socket *b) +{ + int ret = 0; + (void)a; + (void)b; +#ifdef PICO_SUPPORT_IPV6 + if (!is_sock_ipv6(a) || !is_sock_ipv6(b)) + return 0; + + 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)) + ret = 0; + else + ret = memcmp(a->local_addr.ip6.addr, b->local_addr.ip6.addr, PICO_SIZE_IP6); + +#endif + return ret; +} + +static int socket_cmp_ipv4(struct pico_socket *a, struct pico_socket *b) +{ + int ret = 0; + (void)a; + (void)b; + if (!is_sock_ipv4(a) || !is_sock_ipv4(b)) + return 0; + +#ifdef PICO_SUPPORT_IPV4 + if ((a->local_addr.ip4.addr == PICO_IP4_ANY) || (b->local_addr.ip4.addr == PICO_IP4_ANY)) + ret = 0; + else + ret = (int)(a->local_addr.ip4.addr - b->local_addr.ip4.addr); + +#endif + return ret; +} + +static int socket_cmp_remotehost(struct pico_socket *a, struct pico_socket *b) +{ + int ret = 0; + if (is_sock_ipv6(a)) + ret = memcmp(a->remote_addr.ip6.addr, b->remote_addr.ip6.addr, PICO_SIZE_IP6); + else + ret = (int)(a->remote_addr.ip4.addr - b->remote_addr.ip4.addr); + + return ret; +} + +static int socket_cmp_addresses(struct pico_socket *a, struct pico_socket *b) +{ + int ret = 0; + /* At this point, sort by local host */ + ret = socket_cmp_ipv6(a, b); + + if (ret == 0) + ret = socket_cmp_ipv4(a, b); + + /* Sort by remote host */ + if (ret == 0) + ret = socket_cmp_remotehost(a, b); + + return ret; +} + +static int socket_cmp(void *ka, void *kb) +{ + struct pico_socket *a = ka, *b = kb; + int ret = 0; + + /* First, order by network family */ + ret = socket_cmp_family(a, b); + + /* Then, compare by source/destination addresses */ + if (ret == 0) + ret = socket_cmp_addresses(a, b); + + /* And finally by remote port. The two sockets are coincident if the quad is the same. */ + if (ret == 0) + ret = b->remote_port - a->remote_port; + + return ret; +} + + +#define INIT_SOCKPORT { {&LEAF, socket_cmp}, 0, 0 } + +static int sockport_cmp(void *ka, void *kb) +{ + struct pico_sockport *a = ka, *b = kb; + if (a->number < b->number) + return -1; + + if (a->number > b->number) + return 1; + + return 0; +} + +static PICO_TREE_DECLARE(UDPTable, sockport_cmp); +static PICO_TREE_DECLARE(TCPTable, sockport_cmp); + +struct pico_sockport *pico_get_sockport(uint16_t proto, uint16_t port) +{ + struct pico_sockport test = INIT_SOCKPORT; + test.number = port; + + if (proto == PICO_PROTO_UDP) + return pico_tree_findKey(&UDPTable, &test); + + else if (proto == PICO_PROTO_TCP) + return pico_tree_findKey(&TCPTable, &test); + + else return NULL; +} + +#ifdef PICO_SUPPORT_IPV4 + +static int pico_port_in_use_by_nat(uint16_t proto, uint16_t port) +{ + int ret = 0; + (void) proto; + (void) port; +#ifdef PICO_SUPPORT_NAT + if (pico_ipv4_nat_find(port, NULL, 0, (uint8_t)proto)) { + dbg("In use by nat....\n"); + ret = 1; + } + +#endif + return ret; +} + +static int pico_port_in_use_with_this_ipv4_address(struct pico_sockport *sp, struct pico_ip4 ip) +{ + if (sp) { + struct pico_ip4 *s_local; + struct pico_tree_node *idx; + struct pico_socket *s; + pico_tree_foreach(idx, &sp->socks) { + s = idx->keyValue; + if (s->net == &pico_proto_ipv4) { + s_local = (struct pico_ip4*) &s->local_addr; + if ((s_local->addr == PICO_IPV4_INADDR_ANY) || (s_local->addr == ip.addr)) { + return 1; + } + } + } + } + + return 0; +} + + +static int pico_port_in_use_ipv4(struct pico_sockport *sp, void *addr) +{ + struct pico_ip4 ip; + /* IPv4 */ + if (addr) + ip.addr = ((struct pico_ip4 *)addr)->addr; + else + ip.addr = PICO_IPV4_INADDR_ANY; + + if (ip.addr == PICO_IPV4_INADDR_ANY) { + if (!sp) + return 0; + else { + dbg("In use, and asked for ANY\n"); + return 1; + } + } + + return pico_port_in_use_with_this_ipv4_address(sp, ip); +} +#endif + +#ifdef PICO_SUPPORT_IPV6 +static int pico_port_in_use_with_this_ipv6_address(struct pico_sockport *sp, struct pico_ip6 ip) +{ + if (sp) { + struct pico_ip6 *s_local; + struct pico_tree_node *idx; + struct pico_socket *s; + pico_tree_foreach(idx, &sp->socks) { + s = idx->keyValue; + if (s->net == &pico_proto_ipv6) { + s_local = (struct pico_ip6*) &s->local_addr; + if ((pico_ipv6_is_unspecified(s_local->addr)) || (!memcmp(s_local->addr, ip.addr, PICO_SIZE_IP6))) { + return 1; + } + } + } + } + + return 0; +} + +static int pico_port_in_use_ipv6(struct pico_sockport *sp, void *addr) +{ + struct pico_ip6 ip; + /* IPv6 */ + if (addr) + memcpy(ip.addr, ((struct pico_ip6 *)addr)->addr, sizeof(struct pico_ip6)); + else + memcpy(ip.addr, PICO_IP6_ANY, sizeof(struct pico_ip6)); + + if (memcmp(ip.addr, PICO_IP6_ANY, sizeof(struct pico_ip6)) == 0) { + if (!sp) + return 0; + else { + dbg("In use, and asked for ANY\n"); + return 1; + } + } + + return pico_port_in_use_with_this_ipv6_address(sp, ip); +} +#endif + + + +static int pico_generic_port_in_use(uint16_t proto, uint16_t port, struct pico_sockport *sp, void *addr, void *net) +{ +#ifdef PICO_SUPPORT_IPV4 + if (net == &pico_proto_ipv4) + { + if (pico_port_in_use_by_nat(proto, port)) { + return 1; + } + + if (pico_port_in_use_ipv4(sp, addr)) { + return 1; + } + } + +#endif + +#ifdef PICO_SUPPORT_IPV6 + if (net == &pico_proto_ipv6) + { + if (pico_port_in_use_ipv6(sp, addr)) { + return 1; + } + } + +#endif + + return 0; +} + +int pico_is_port_free(uint16_t proto, uint16_t port, void *addr, void *net) +{ + struct pico_sockport *sp; + sp = pico_get_sockport(proto, port); + + if (pico_generic_port_in_use(proto, port, sp, addr, net)) + return 0; + + return 1; +} + +static int pico_check_socket(struct pico_socket *s) +{ + struct pico_sockport *test; + struct pico_socket *found; + struct pico_tree_node *index; + + test = pico_get_sockport(PROTO(s), s->local_port); + + if (!test) { + return -1; + } + + pico_tree_foreach(index, &test->socks){ + found = index->keyValue; + if (s == found) { + return 0; + } + } + + return -1; +} + +struct pico_socket *pico_sockets_find(uint16_t local, uint16_t remote) +{ + struct pico_socket *sock = NULL; + struct pico_tree_node *index = NULL; + struct pico_sockport *sp = NULL; + + sp = pico_get_sockport(PICO_PROTO_TCP, local); + if(sp) + { + pico_tree_foreach(index, &sp->socks) + { + if(((struct pico_socket *)index->keyValue)->remote_port == remote) + { + sock = (struct pico_socket *)index->keyValue; + break; + } + } + } + + return sock; +} + + +int8_t pico_socket_add(struct pico_socket *s) +{ + struct pico_sockport *sp; + if (PROTO(s) != PICO_PROTO_UDP && PROTO(s) != PICO_PROTO_TCP) + { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + sp = pico_get_sockport(PROTO(s), s->local_port); + PICOTCP_MUTEX_LOCK(Mutex); + if (!sp) { + /* dbg("Creating sockport..%04x\n", s->local_port); / * In comment due to spam during test * / */ + sp = PICO_ZALLOC(sizeof(struct pico_sockport)); + + if (!sp) { + pico_err = PICO_ERR_ENOMEM; + PICOTCP_MUTEX_UNLOCK(Mutex); + return -1; + } + + sp->proto = PROTO(s); + sp->number = s->local_port; + sp->socks.root = &LEAF; + sp->socks.compare = socket_cmp; + + if (PROTO(s) == PICO_PROTO_UDP) + { + if (pico_tree_insert(&UDPTable, sp)) { + PICO_FREE(sp); + PICOTCP_MUTEX_UNLOCK(Mutex); + return -1; + } + + } + else if (PROTO(s) == PICO_PROTO_TCP) + { + if (pico_tree_insert(&TCPTable, sp)) { + PICO_FREE(sp); + PICOTCP_MUTEX_UNLOCK(Mutex); + return -1; + } + } + } + + if (pico_tree_insert(&sp->socks, s)) { + PICOTCP_MUTEX_UNLOCK(Mutex); + return -1; + } + s->state |= PICO_SOCKET_STATE_BOUND; + PICOTCP_MUTEX_UNLOCK(Mutex); +#ifdef DEBUG_SOCKET_TREE + { + struct pico_tree_node *index; + pico_tree_foreach(index, &sp->socks){ + s = index->keyValue; + dbg(">>>> List Socket lc=%hu rm=%hu\n", short_be(s->local_port), short_be(s->remote_port)); + } + + } +#endif + return 0; +} + + +static void socket_clean_queues(struct pico_socket *sock) +{ + struct pico_frame *f_in = pico_dequeue(&sock->q_in); + struct pico_frame *f_out = pico_dequeue(&sock->q_out); + while(f_in || f_out) + { + if(f_in) + { + pico_frame_discard(f_in); + f_in = pico_dequeue(&sock->q_in); + } + + if(f_out) + { + pico_frame_discard(f_out); + f_out = pico_dequeue(&sock->q_out); + } + } + pico_queue_deinit(&sock->q_in); + pico_queue_deinit(&sock->q_out); + pico_socket_tcp_cleanup(sock); +} + +static void socket_garbage_collect(pico_time now, void *arg) +{ + struct pico_socket *s = (struct pico_socket *) arg; + IGNORE_PARAMETER(now); + + socket_clean_queues(s); + PICO_FREE(s); +} + + +static void pico_socket_check_empty_sockport(struct pico_socket *s, struct pico_sockport *sp) +{ + if(pico_tree_empty(&sp->socks)) { + if (PROTO(s) == PICO_PROTO_UDP) + { + pico_tree_delete(&UDPTable, sp); + } + else if (PROTO(s) == PICO_PROTO_TCP) + { + pico_tree_delete(&TCPTable, sp); + } + + if(sp_tcp == sp) + sp_tcp = NULL; + + if(sp_udp == sp) + sp_udp = NULL; + + PICO_FREE(sp); + } +} + +int8_t pico_socket_del(struct pico_socket *s) +{ + struct pico_sockport *sp = pico_get_sockport(PROTO(s), s->local_port); + if (!sp) { + pico_err = PICO_ERR_ENXIO; + return -1; + } + + PICOTCP_MUTEX_LOCK(Mutex); + pico_tree_delete(&sp->socks, s); + pico_socket_check_empty_sockport(s, sp); +#ifdef PICO_SUPPORT_MCAST + pico_multicast_delete(s); +#endif + pico_socket_tcp_delete(s); + s->state = PICO_SOCKET_STATE_CLOSED; + if (!pico_timer_add((pico_time)10, socket_garbage_collect, s)) { + dbg("SOCKET: Failed to start garbage collect timer, doing garbage collection now\n"); + PICOTCP_MUTEX_UNLOCK(Mutex); + socket_garbage_collect((pico_time)0, s); + return -1; + } + PICOTCP_MUTEX_UNLOCK(Mutex); + return 0; +} + +static void pico_socket_update_tcp_state(struct pico_socket *s, uint16_t tcp_state) +{ + if (tcp_state) { + s->state &= 0x00FF; + s->state |= tcp_state; + } +} + +static int8_t pico_socket_alter_state(struct pico_socket *s, uint16_t more_states, uint16_t less_states, uint16_t tcp_state) +{ + struct pico_sockport *sp; + if (more_states & PICO_SOCKET_STATE_BOUND) + return pico_socket_add(s); + + if (less_states & PICO_SOCKET_STATE_BOUND) + return pico_socket_del(s); + + sp = pico_get_sockport(PROTO(s), s->local_port); + if (!sp) { + pico_err = PICO_ERR_ENXIO; + return -1; + } + + s->state |= more_states; + s->state = (uint16_t)(s->state & (~less_states)); + pico_socket_update_tcp_state(s, tcp_state); + return 0; +} + + +static int pico_socket_transport_deliver(struct pico_protocol *p, struct pico_sockport *sp, struct pico_frame *f) +{ +#ifdef PICO_SUPPORT_TCP + if (p->proto_number == PICO_PROTO_TCP) + return pico_socket_tcp_deliver(sp, f); + +#endif + +#ifdef PICO_SUPPORT_UDP + if (p->proto_number == PICO_PROTO_UDP) + return pico_socket_udp_deliver(sp, f); + +#endif + + return -1; +} + + +static int pico_socket_deliver(struct pico_protocol *p, struct pico_frame *f, uint16_t localport) +{ + struct pico_sockport *sp = NULL; + struct pico_trans *tr = (struct pico_trans *) f->transport_hdr; + + if (!tr) + return -1; + + sp = pico_get_sockport(p->proto_number, localport); + if (!sp) { + dbg("No such port %d\n", short_be(localport)); + return -1; + } + + return pico_socket_transport_deliver(p, sp, f); +} + +int pico_socket_set_family(struct pico_socket *s, uint16_t family) +{ + (void) family; + + #ifdef PICO_SUPPORT_IPV4 + if (family == PICO_PROTO_IPV4) + s->net = &pico_proto_ipv4; + + #endif + + #ifdef PICO_SUPPORT_IPV6 + if (family == PICO_PROTO_IPV6) + s->net = &pico_proto_ipv6; + + #endif + + if (s->net == NULL) + return -1; + + return 0; +} + +static struct pico_socket *pico_socket_transport_open(uint16_t proto, uint16_t family) +{ + struct pico_socket *s = NULL; + (void)family; +#ifdef PICO_SUPPORT_UDP + if (proto == PICO_PROTO_UDP) + s = pico_socket_udp_open(); + +#endif + +#ifdef PICO_SUPPORT_TCP + if (proto == PICO_PROTO_TCP) + s = pico_socket_tcp_open(family); + +#endif + + return s; + +} + +struct pico_socket *MOCKABLE pico_socket_open(uint16_t net, uint16_t proto, void (*wakeup)(uint16_t ev, struct pico_socket *)) +{ + + struct pico_socket *s = NULL; + + s = pico_socket_transport_open(proto, net); + + if (!s) { + pico_err = PICO_ERR_EPROTONOSUPPORT; + return NULL; + } + + if (pico_socket_set_family(s, net) != 0) { + PICO_FREE(s); + pico_err = PICO_ERR_ENETUNREACH; + return NULL; + } + + s->q_in.max_size = PICO_DEFAULT_SOCKETQ; + s->q_out.max_size = PICO_DEFAULT_SOCKETQ; + + s->wakeup = wakeup; + return s; +} + + +static void pico_socket_clone_assign_address(struct pico_socket *s, struct pico_socket *facsimile) +{ + +#ifdef PICO_SUPPORT_IPV4 + if (facsimile->net == &pico_proto_ipv4) { + s->net = &pico_proto_ipv4; + memcpy(&s->local_addr, &facsimile->local_addr, sizeof(struct pico_ip4)); + memcpy(&s->remote_addr, &facsimile->remote_addr, sizeof(struct pico_ip4)); + } + +#endif + +#ifdef PICO_SUPPORT_IPV6 + if (facsimile->net == &pico_proto_ipv6) { + s->net = &pico_proto_ipv6; + memcpy(&s->local_addr, &facsimile->local_addr, sizeof(struct pico_ip6)); + memcpy(&s->remote_addr, &facsimile->remote_addr, sizeof(struct pico_ip6)); + } + +#endif + +} + +struct pico_socket *pico_socket_clone(struct pico_socket *facsimile) +{ + struct pico_socket *s = NULL; + + s = pico_socket_transport_open(facsimile->proto->proto_number, facsimile->net->proto_number); + if (!s) { + pico_err = PICO_ERR_EPROTONOSUPPORT; + return NULL; + } + + s->local_port = facsimile->local_port; + s->remote_port = facsimile->remote_port; + s->state = facsimile->state; + pico_socket_clone_assign_address(s, facsimile); + if (!s->net) { + PICO_FREE(s); + pico_err = PICO_ERR_ENETUNREACH; + return NULL; + } + + s->q_in.max_size = PICO_DEFAULT_SOCKETQ; + s->q_out.max_size = PICO_DEFAULT_SOCKETQ; + s->wakeup = NULL; + return s; +} + +static int pico_socket_transport_read(struct pico_socket *s, void *buf, int len) +{ + if (PROTO(s) == PICO_PROTO_UDP) + { + /* make sure cast to uint16_t doesn't give unexpected results */ + if(len > 0xFFFF) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + return pico_socket_udp_recv(s, buf, (uint16_t)len, NULL, NULL); + } + else if (PROTO(s) == PICO_PROTO_TCP) + return pico_socket_tcp_read(s, buf, (uint32_t)len); + else return 0; +} + +int pico_socket_read(struct pico_socket *s, void *buf, int len) +{ + if (!s || buf == NULL) { + pico_err = PICO_ERR_EINVAL; + return -1; + } else { + /* check if exists in tree */ + /* See task #178 */ + if (pico_check_socket(s) != 0) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + } + + if ((s->state & PICO_SOCKET_STATE_BOUND) == 0) { + pico_err = PICO_ERR_EIO; + return -1; + } + + return pico_socket_transport_read(s, buf, len); +} + +static int pico_socket_write_check_state(struct pico_socket *s) +{ + if ((s->state & PICO_SOCKET_STATE_BOUND) == 0) { + pico_err = PICO_ERR_EIO; + return -1; + } + + if ((s->state & PICO_SOCKET_STATE_CONNECTED) == 0) { + pico_err = PICO_ERR_ENOTCONN; + return -1; + } + + if (s->state & PICO_SOCKET_STATE_SHUT_LOCAL) { /* check if in shutdown state */ + pico_err = PICO_ERR_ESHUTDOWN; + return -1; + } + + return 0; +} + +static int pico_socket_write_attempt(struct pico_socket *s, const void *buf, int len) +{ + if (pico_socket_write_check_state(s) < 0) { + return -1; + } else { + return pico_socket_sendto(s, buf, len, &s->remote_addr, s->remote_port); + } +} + +int pico_socket_write(struct pico_socket *s, const void *buf, int len) +{ + if (!s || buf == NULL) { + pico_err = PICO_ERR_EINVAL; + return -1; + } else { + /* check if exists in tree */ + /* See task #178 */ + if (pico_check_socket(s) != 0) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + } + + return pico_socket_write_attempt(s, buf, len); +} + +static uint16_t pico_socket_high_port(uint16_t proto) +{ + uint16_t port; + if (0 || +#ifdef PICO_SUPPORT_TCP + (proto == PICO_PROTO_TCP) || +#endif +#ifdef PICO_SUPPORT_UDP + (proto == PICO_PROTO_UDP) || +#endif + 0) { + do { + uint32_t rand = pico_rand(); + port = (uint16_t) (rand & 0xFFFFU); + port = (uint16_t)((port % (65535 - 1024)) + 1024U); + if (pico_is_port_free(proto, port, NULL, NULL)) { + return short_be(port); + } + } while(1); + } + else return 0U; +} + +static void *pico_socket_sendto_get_ip4_src(struct pico_socket *s, struct pico_ip4 *dst) +{ + struct pico_ip4 *src4 = NULL; + +#ifdef PICO_SUPPORT_IPV4 + /* Check if socket is connected: destination address MUST match the + * current connected endpoint + */ + if ((s->state & PICO_SOCKET_STATE_CONNECTED)) { + src4 = &s->local_addr.ip4; + if (s->remote_addr.ip4.addr != ((struct pico_ip4 *)dst)->addr ) { + pico_err = PICO_ERR_EADDRNOTAVAIL; + return NULL; + } + } else { + + src4 = pico_ipv4_source_find(dst); + if (!src4) { + pico_err = PICO_ERR_EHOSTUNREACH; + return NULL; + } + + } + + if (src4->addr != PICO_IPV4_INADDR_ANY) + s->local_addr.ip4.addr = src4->addr; + +#else + pico_err = PICO_ERR_EPROTONOSUPPORT; +#endif + return src4; +} + +static void *pico_socket_sendto_get_ip6_src(struct pico_socket *s, struct pico_ip6 *dst) +{ + struct pico_ip6 *src6 = NULL; + (void)s; + (void)dst; + +#ifdef PICO_SUPPORT_IPV6 + + /* Check if socket is connected: destination address MUST match the + * current connected endpoint + */ + if ((s->state & PICO_SOCKET_STATE_CONNECTED)) { + src6 = &s->local_addr.ip6; + if (memcmp(&s->remote_addr, dst, PICO_SIZE_IP6)) { + pico_err = PICO_ERR_EADDRNOTAVAIL; + return NULL; + } + } else { + src6 = pico_ipv6_source_find(dst); + if (!src6) { + pico_err = PICO_ERR_EHOSTUNREACH; + return NULL; + } + + if (!pico_ipv6_is_unspecified(src6->addr)) + s->local_addr.ip6 = *src6; + } + +#else + pico_err = PICO_ERR_EPROTONOSUPPORT; +#endif + return src6; +} + + +static int pico_socket_sendto_dest_check(struct pico_socket *s, void *dst, uint16_t port) +{ + + /* For the sendto call to be valid, + * dst and remote_port should be always populated. + */ + if (!dst || !port) { + pico_err = PICO_ERR_EADDRNOTAVAIL; + return -1; + } + + /* When coming from pico_socket_send (or _write), + * the destination is automatically assigned to the currently connected endpoint. + * This check will ensure that there is no mismatch when sendto() is called directly + * on a connected socket + */ + if ((s->state & PICO_SOCKET_STATE_CONNECTED) != 0) { + if (port != s->remote_port) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + } + + return 0; +} + +static int pico_socket_sendto_initial_checks(struct pico_socket *s, const void *buf, const int len, void *dst, uint16_t remote_port) +{ + if (len < 0) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + if (buf == NULL || s == NULL) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + return pico_socket_sendto_dest_check(s, dst, remote_port); +} + +static void *pico_socket_sendto_get_src(struct pico_socket *s, void *dst) +{ + void *src = NULL; + if (is_sock_ipv4(s)) + src = pico_socket_sendto_get_ip4_src(s, (struct pico_ip4 *)dst); + + if (is_sock_ipv6(s)) + src = pico_socket_sendto_get_ip6_src(s, (struct pico_ip6 *)dst); + + return src; +} + +static struct pico_remote_endpoint *pico_socket_sendto_destination_ipv4(struct pico_socket *s, struct pico_ip4 *dst, uint16_t port) +{ + struct pico_remote_endpoint *ep = NULL; + (void)s; + ep = PICO_ZALLOC(sizeof(struct pico_remote_endpoint)); + if (!ep) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + ep->remote_addr.ip4.addr = ((struct pico_ip4 *)dst)->addr; + ep->remote_port = port; + return ep; +} + +static void pico_endpoint_free(struct pico_remote_endpoint *ep) +{ + if (ep) + PICO_FREE(ep); +} + +static struct pico_remote_endpoint *pico_socket_sendto_destination_ipv6(struct pico_socket *s, struct pico_ip6 *dst, uint16_t port) +{ + struct pico_remote_endpoint *ep = NULL; + (void)s; + (void)dst; + (void)port; +#ifdef PICO_SUPPORT_IPV6 + ep = PICO_ZALLOC(sizeof(struct pico_remote_endpoint)); + if (!ep) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + memcpy(&ep->remote_addr.ip6, dst, sizeof(struct pico_ip6)); + ep->remote_port = port; +#endif + return ep; +} + + +static struct pico_remote_endpoint *pico_socket_sendto_destination(struct pico_socket *s, void *dst, uint16_t port) +{ + struct pico_remote_endpoint *ep = NULL; + (void)pico_socket_sendto_destination_ipv6; + /* socket remote info could change in a consecutive call, make persistent */ +# ifdef PICO_SUPPORT_UDP + if (PROTO(s) == PICO_PROTO_UDP) { +# ifdef PICO_SUPPORT_IPV6 + if (is_sock_ipv6(s)) + ep = pico_socket_sendto_destination_ipv6(s, (struct pico_ip6 *)dst, port); + +# endif +# ifdef PICO_SUPPORT_IPV4 + if (is_sock_ipv4(s)) + ep = pico_socket_sendto_destination_ipv4(s, (struct pico_ip4 *)dst, port); + +# endif + } + +# endif + return ep; +} + +static int32_t pico_socket_sendto_set_localport(struct pico_socket *s) +{ + + if ((s->state & PICO_SOCKET_STATE_BOUND) == 0) { + s->local_port = pico_socket_high_port(s->proto->proto_number); + if (s->local_port == 0) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + pico_socket_alter_state(s, PICO_SOCKET_STATE_BOUND, 0, 0); + } + + return s->local_port; +} + +static int pico_socket_sendto_transport_offset(struct pico_socket *s) +{ + int header_offset = -1; + #ifdef PICO_SUPPORT_TCP + if (PROTO(s) == PICO_PROTO_TCP) + header_offset = pico_tcp_overhead(s); + + #endif + + #ifdef PICO_SUPPORT_UDP + if (PROTO(s) == PICO_PROTO_UDP) + header_offset = sizeof(struct pico_udp_hdr); + + #endif + return header_offset; +} + + +static struct pico_remote_endpoint *pico_socket_set_info(struct pico_remote_endpoint *ep) +{ + struct pico_remote_endpoint *info; + info = PICO_ZALLOC(sizeof(struct pico_remote_endpoint)); + if (!info) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + memcpy(info, ep, sizeof(struct pico_remote_endpoint)); + return info; +} + +static void pico_xmit_frame_set_nofrag(struct pico_frame *f) +{ +#ifdef PICO_SUPPORT_IPV4FRAG + f->frag = PICO_IPV4_DONTFRAG; +#else + (void)f; +#endif +} + +static int pico_socket_final_xmit(struct pico_socket *s, struct pico_frame *f) +{ + if (s->proto->push(s->proto, f) > 0) { + return f->payload_len; + } else { + pico_frame_discard(f); + return 0; + } +} + +static int pico_socket_xmit_one(struct pico_socket *s, const void *buf, const int len, void *src, + struct pico_remote_endpoint *ep, struct pico_msginfo *msginfo) +{ + struct pico_frame *f; + struct pico_device *dev = NULL; + uint16_t hdr_offset = (uint16_t)pico_socket_sendto_transport_offset(s); + int ret = 0; + (void)src; + + if (msginfo) { + dev = msginfo->dev; + } +#ifdef PICO_SUPPORT_IPV6 + else if (IS_SOCK_IPV6(s) && ep && pico_ipv6_is_multicast(&ep->remote_addr.ip6.addr[0])) { + dev = pico_ipv6_link_find(src); + } +#endif + else if (IS_SOCK_IPV6(s) && ep) { + dev = pico_ipv6_source_dev_find(&ep->remote_addr.ip6); + } else if (IS_SOCK_IPV4(s) && ep) { + dev = pico_ipv4_source_dev_find(&ep->remote_addr.ip4); + } else { + dev = get_sock_dev(s); + } + + if (!dev) { + return -1; + } + + f = pico_socket_frame_alloc(s, dev, (uint16_t)(len + hdr_offset)); + if (!f) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + f->payload += hdr_offset; + f->payload_len = (uint16_t)(len); + f->sock = s; + transport_flags_update(f, s); + pico_xmit_frame_set_nofrag(f); + if (ep && !f->info) { + f->info = pico_socket_set_info(ep); + if (!f->info) { + pico_frame_discard(f); + return -1; + } + } + + if (msginfo) { + f->send_ttl = (uint8_t)msginfo->ttl; + f->send_tos = (uint8_t)msginfo->tos; + } + + memcpy(f->payload, (const uint8_t *)buf, f->payload_len); + /* dbg("Pushing segment, hdr len: %d, payload_len: %d\n", header_offset, f->payload_len); */ + ret = pico_socket_final_xmit(s, f); + return ret; +} + +static int pico_socket_xmit_avail_space(struct pico_socket *s); + +#ifdef PICO_SUPPORT_IPV4FRAG +static void pico_socket_xmit_first_fragment_setup(struct pico_frame *f, int space, int hdr_offset) +{ + frag_dbg("FRAG: first fragmented frame %p | len = %u offset = 0\n", f, f->payload_len); + /* transport header length field contains total length + header length */ + f->transport_len = (uint16_t)(space); + f->frag = PICO_IPV4_MOREFRAG; + f->payload += hdr_offset; +} + +static void pico_socket_xmit_next_fragment_setup(struct pico_frame *f, int hdr_offset, int total_payload_written, int len) +{ + /* no transport header in fragmented IP */ + f->payload = f->transport_hdr; + /* set offset in octets */ + f->frag = (uint16_t)((total_payload_written + (uint16_t)hdr_offset) >> 3u); /* first fragment had a header offset */ + if (total_payload_written + f->payload_len < len) { + frag_dbg("FRAG: intermediate fragmented frame %p | len = %u offset = %u\n", f, f->payload_len, short_be(f->frag)); + f->frag |= PICO_IPV4_MOREFRAG; + } else { + frag_dbg("FRAG: last fragmented frame %p | len = %u offset = %u\n", f, f->payload_len, short_be(f->frag)); + f->frag &= PICO_IPV4_FRAG_MASK; + } +} +#endif + +/* Implies ep discarding! */ +static int pico_socket_xmit_fragments(struct pico_socket *s, const void *buf, const int len, + void *src, struct pico_remote_endpoint *ep, struct pico_msginfo *msginfo) +{ + int space = pico_socket_xmit_avail_space(s); + int hdr_offset = pico_socket_sendto_transport_offset(s); + int total_payload_written = 0; + int retval = 0; + struct pico_frame *f = NULL; + + if (space < 0) { + pico_err = PICO_ERR_EPROTONOSUPPORT; + pico_endpoint_free(ep); + return -1; + } + + if (space > len) { + retval = pico_socket_xmit_one(s, buf, len, src, ep, msginfo); + pico_endpoint_free(ep); + return retval; + } + +#ifdef PICO_SUPPORT_IPV6 + /* Can't fragment IPv6 */ + if (is_sock_ipv6(s)) { + retval = pico_socket_xmit_one(s, buf, space, src, ep, msginfo); + pico_endpoint_free(ep); + return retval; + } + +#endif + +#ifdef PICO_SUPPORT_IPV4FRAG + while(total_payload_written < len) { + /* Always allocate the max space available: space + offset */ + if (len < space) + space = len; + + if (space > len - total_payload_written) /* update space for last fragment */ + space = len - total_payload_written; + + f = pico_socket_frame_alloc(s, get_sock_dev(s), (uint16_t)(space + hdr_offset)); + if (!f) { + pico_err = PICO_ERR_ENOMEM; + pico_endpoint_free(ep); + return -1; + } + + f->sock = s; + if (ep) { + f->info = pico_socket_set_info(ep); + if (!f->info) { + pico_frame_discard(f); + pico_endpoint_free(ep); + return -1; + } + } + + f->payload_len = (uint16_t) space; + if (total_payload_written == 0) { + /* First fragment: no payload written yet! */ + pico_socket_xmit_first_fragment_setup(f, space, hdr_offset); + space += hdr_offset; /* only first fragments contains transport header */ + hdr_offset = 0; + } else { + /* Next fragment */ + pico_socket_xmit_next_fragment_setup(f, pico_socket_sendto_transport_offset(s), total_payload_written, len); + } + + memcpy(f->payload, (const uint8_t *)buf + total_payload_written, f->payload_len); + transport_flags_update(f, s); + if (s->proto->push(s->proto, f) > 0) { + total_payload_written += f->payload_len; + } else { + pico_frame_discard(f); + break; + } + } /* while() */ + pico_endpoint_free(ep); + return total_payload_written; + +#else + /* Careful with that axe, Eugene! + * + * cropping down datagrams to the MTU value. + */ + (void) f; + (void) hdr_offset; + (void) total_payload_written; + retval = pico_socket_xmit_one(s, buf, space, src, ep, msginfo); + pico_endpoint_free(ep); + return retval; + +#endif +} + +struct pico_device *get_sock_dev(struct pico_socket *s) +{ + if (0) {} + +#ifdef PICO_SUPPORT_IPV6 + else if (is_sock_ipv6(s)) + s->dev = pico_ipv6_source_dev_find(&s->remote_addr.ip6); +#endif +#ifdef PICO_SUPPORT_IPV4 + else if (is_sock_ipv4(s)) + s->dev = pico_ipv4_source_dev_find(&s->remote_addr.ip4); +#endif + + return s->dev; +} + + +static uint32_t pico_socket_adapt_mss_to_proto(struct pico_socket *s, uint32_t mss) +{ +#ifdef PICO_SUPPORT_IPV6 + if (is_sock_ipv6(s)) + mss -= PICO_SIZE_IP6HDR; + else +#endif + mss -= PICO_SIZE_IP4HDR; + return mss; +} + +uint32_t pico_socket_get_mss(struct pico_socket *s) +{ + uint32_t mss = PICO_MIN_MSS; + if (!s) + return mss; + + if (!s->dev) + get_sock_dev(s); + + if (!s->dev) { + mss = PICO_MIN_MSS; + } else { + mss = s->dev->mtu; + } + + return pico_socket_adapt_mss_to_proto(s, mss); +} + + +static int pico_socket_xmit_avail_space(struct pico_socket *s) +{ + int transport_len; + int header_offset; + +#ifdef PICO_SUPPORT_TCP + if (PROTO(s) == PICO_PROTO_TCP) { + transport_len = (uint16_t)pico_tcp_get_socket_mss(s); + } else +#endif + transport_len = (uint16_t)pico_socket_get_mss(s); + header_offset = pico_socket_sendto_transport_offset(s); + if (header_offset < 0) { + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; + } + + transport_len -= pico_socket_sendto_transport_offset(s); + return transport_len; +} + + +static int pico_socket_xmit(struct pico_socket *s, const void *buf, const int len, void *src, + struct pico_remote_endpoint *ep, struct pico_msginfo *msginfo) +{ + int space = pico_socket_xmit_avail_space(s); + int total_payload_written = 0; + + if (space < 0) { + pico_err = PICO_ERR_EPROTONOSUPPORT; + pico_endpoint_free(ep); + return -1; + } + + if ((PROTO(s) == PICO_PROTO_UDP) && (len > space)) { + total_payload_written = pico_socket_xmit_fragments(s, buf, len, src, ep, msginfo); + /* Implies ep discarding */ + return total_payload_written; + } + + while (total_payload_written < len) { + int w, chunk_len = len - total_payload_written; + if (chunk_len > space) + chunk_len = space; + + w = pico_socket_xmit_one(s, (const void *)((const uint8_t *)buf + total_payload_written), chunk_len, src, ep, msginfo); + if (w <= 0) { + break; + } + + total_payload_written += w; + if (PROTO(s) == PICO_PROTO_UDP) { + /* Break after the first datagram sent with at most MTU bytes. */ + break; + } + } + pico_endpoint_free(ep); + return total_payload_written; +} + +static void pico_socket_sendto_set_dport(struct pico_socket *s, uint16_t port) +{ + if ((s->state & PICO_SOCKET_STATE_CONNECTED) == 0) { + s->remote_port = port; + } +} + + +int MOCKABLE pico_socket_sendto_extended(struct pico_socket *s, const void *buf, const int len, + void *dst, uint16_t remote_port, struct pico_msginfo *msginfo) +{ + struct pico_remote_endpoint *remote_endpoint = NULL; + void *src = NULL; + + if(len == 0) + return 0; + + if (pico_socket_sendto_initial_checks(s, buf, len, dst, remote_port) < 0) + return -1; + + + src = pico_socket_sendto_get_src(s, dst); + if (!src) { +#ifdef PICO_SUPPORT_IPV6 + if((s->net->proto_number == PICO_PROTO_IPV6) + && msginfo && msginfo->dev + && pico_ipv6_is_multicast(((struct pico_ip6 *)dst)->addr)) + { + src = &(pico_ipv6_linklocal_get(msginfo->dev)->address); + } + else +#endif + return -1; + } + + remote_endpoint = pico_socket_sendto_destination(s, dst, remote_port); + if (pico_socket_sendto_set_localport(s) < 0) { + pico_endpoint_free(remote_endpoint); + return -1; + } + + pico_socket_sendto_set_dport(s, remote_port); + return pico_socket_xmit(s, buf, len, src, remote_endpoint, msginfo); /* Implies discarding the endpoint */ +} + +int MOCKABLE pico_socket_sendto(struct pico_socket *s, const void *buf, const int len, void *dst, uint16_t remote_port) +{ + return pico_socket_sendto_extended(s, buf, len, dst, remote_port, NULL); +} + +int pico_socket_send(struct pico_socket *s, const void *buf, int len) +{ + if (!s || buf == NULL) { + pico_err = PICO_ERR_EINVAL; + return -1; + } else { + /* check if exists in tree */ + /* See task #178 */ + if (pico_check_socket(s) != 0) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + } + + if ((s->state & PICO_SOCKET_STATE_CONNECTED) == 0) { + pico_err = PICO_ERR_ENOTCONN; + return -1; + } + + return pico_socket_sendto(s, buf, len, &s->remote_addr, s->remote_port); +} + +int pico_socket_recvfrom_extended(struct pico_socket *s, void *buf, int len, void *orig, + uint16_t *remote_port, struct pico_msginfo *msginfo) +{ + if (!s || buf == NULL) { /* / || orig == NULL || remote_port == NULL) { */ + pico_err = PICO_ERR_EINVAL; + return -1; + } else { + /* check if exists in tree */ + if (pico_check_socket(s) != 0) { + pico_err = PICO_ERR_EINVAL; + /* See task #178 */ + return -1; + } + } + + if ((s->state & PICO_SOCKET_STATE_BOUND) == 0) { + pico_err = PICO_ERR_EADDRNOTAVAIL; + return -1; + } + +#ifdef PICO_SUPPORT_UDP + if (PROTO(s) == PICO_PROTO_UDP) { + /* make sure cast to uint16_t doesn't give unexpected results */ + if(len > 0xFFFF) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + return pico_udp_recv(s, buf, (uint16_t)len, orig, remote_port, msginfo); + } + +#endif +#ifdef PICO_SUPPORT_TCP + if (PROTO(s) == PICO_PROTO_TCP) { + /* check if in shutdown state and if tcpq_in empty */ + if ((s->state & PICO_SOCKET_STATE_SHUT_REMOTE) && pico_tcp_queue_in_is_empty(s)) { + pico_err = PICO_ERR_ESHUTDOWN; + return -1; + } else { + /* dbg("socket tcp recv\n"); */ + return (int)pico_tcp_read(s, buf, (uint32_t)len); + } + } + +#endif + /* dbg("socket return 0\n"); */ + return 0; +} + +int MOCKABLE pico_socket_recvfrom(struct pico_socket *s, void *buf, int len, void *orig, + uint16_t *remote_port) +{ + return pico_socket_recvfrom_extended(s, buf, len, orig, remote_port, NULL); + +} + +int pico_socket_recv(struct pico_socket *s, void *buf, int len) +{ + return pico_socket_recvfrom(s, buf, len, NULL, NULL); +} + + +int pico_socket_getname(struct pico_socket *s, void *local_addr, uint16_t *port, uint16_t *proto) +{ + + if (!s || !local_addr || !port || !proto) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + if (is_sock_ipv4(s)) { + #ifdef PICO_SUPPORT_IPV4 + struct pico_ip4 *ip = (struct pico_ip4 *)local_addr; + ip->addr = s->local_addr.ip4.addr; + *proto = PICO_PROTO_IPV4; + #endif + } else if (is_sock_ipv6(s)) { + #ifdef PICO_SUPPORT_IPV6 + struct pico_ip6 *ip = (struct pico_ip6 *)local_addr; + memcpy(ip->addr, s->local_addr.ip6.addr, PICO_SIZE_IP6); + *proto = PICO_PROTO_IPV6; + #endif + } else { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + *port = s->local_port; + return 0; +} + +int pico_socket_getpeername(struct pico_socket *s, void *remote_addr, uint16_t *port, uint16_t *proto) +{ + if (!s || !remote_addr || !port || !proto) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + if ((s->state & PICO_SOCKET_STATE_CONNECTED) == 0) { + pico_err = PICO_ERR_ENOTCONN; + return -1; + } + + if (is_sock_ipv4(s)) { + #ifdef PICO_SUPPORT_IPV4 + struct pico_ip4 *ip = (struct pico_ip4 *)remote_addr; + ip->addr = s->remote_addr.ip4.addr; + *proto = PICO_PROTO_IPV4; + #endif + } else if (is_sock_ipv6(s)) { + #ifdef PICO_SUPPORT_IPV6 + struct pico_ip6 *ip = (struct pico_ip6 *)remote_addr; + memcpy(ip->addr, s->remote_addr.ip6.addr, PICO_SIZE_IP6); + *proto = PICO_PROTO_IPV6; + #endif + } else { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + *port = s->remote_port; + return 0; + +} + +int MOCKABLE pico_socket_bind(struct pico_socket *s, void *local_addr, uint16_t *port) +{ + if (!s || !local_addr || !port) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + if (is_sock_ipv4(s)) { + #ifdef PICO_SUPPORT_IPV4 + struct pico_ip4 *ip = (struct pico_ip4 *)local_addr; + if (ip->addr != PICO_IPV4_INADDR_ANY) { + if (!pico_ipv4_link_find(local_addr)) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + } + + #endif + } else if (is_sock_ipv6(s)) { + #ifdef PICO_SUPPORT_IPV6 + struct pico_ip6 *ip = (struct pico_ip6 *)local_addr; + if (!pico_ipv6_is_unspecified(ip->addr)) { + if (!pico_ipv6_link_find(local_addr)) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + } + + #endif + } else { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* When given port = 0, get a random high port to bind to. */ + if (*port == 0) { + *port = pico_socket_high_port(PROTO(s)); + if (*port == 0) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + } + + if (pico_is_port_free(PROTO(s), *port, local_addr, s->net) == 0) { + pico_err = PICO_ERR_EADDRINUSE; + return -1; + } + + s->local_port = *port; + + if (is_sock_ipv4(s)) { + #ifdef PICO_SUPPORT_IPV4 + struct pico_ip4 *ip = (struct pico_ip4 *)local_addr; + s->local_addr.ip4 = *ip; + #endif + } else if (is_sock_ipv6(s)) { + #ifdef PICO_SUPPORT_IPV6 + struct pico_ip6 *ip = (struct pico_ip6 *)local_addr; + s->local_addr.ip6 = *ip; + #endif + } else { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + return pico_socket_alter_state(s, PICO_SOCKET_STATE_BOUND, 0, 0); +} + + +int pico_socket_connect(struct pico_socket *s, const void *remote_addr, uint16_t remote_port) +{ + int ret = -1; + pico_err = PICO_ERR_EPROTONOSUPPORT; + if (!s || remote_addr == NULL || remote_port == 0) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + s->remote_port = remote_port; + + if (s->local_port == 0) { + s->local_port = pico_socket_high_port(PROTO(s)); + if (!s->local_port) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + } + + if (is_sock_ipv4(s)) { + #ifdef PICO_SUPPORT_IPV4 + struct pico_ip4 *local = NULL; + const struct pico_ip4 *ip = (const struct pico_ip4 *)remote_addr; + s->remote_addr.ip4 = *ip; + local = pico_ipv4_source_find(ip); + if (local) { + get_sock_dev(s); + s->local_addr.ip4 = *local; + } else { + pico_err = PICO_ERR_EHOSTUNREACH; + return -1; + } + + #endif + } else if (is_sock_ipv6(s)) { + #ifdef PICO_SUPPORT_IPV6 + struct pico_ip6 *local = NULL; + const struct pico_ip6 *ip = (const struct pico_ip6 *)remote_addr; + s->remote_addr.ip6 = *ip; + local = pico_ipv6_source_find(ip); + if (local) { + get_sock_dev(s); + s->local_addr.ip6 = *local; + } else { + pico_err = PICO_ERR_EHOSTUNREACH; + return -1; + } + + #endif + } else { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + pico_socket_alter_state(s, PICO_SOCKET_STATE_BOUND, 0, 0); + +#ifdef PICO_SUPPORT_UDP + if (PROTO(s) == PICO_PROTO_UDP) { + pico_socket_alter_state(s, PICO_SOCKET_STATE_CONNECTED, 0, 0); + pico_err = PICO_ERR_NOERR; + ret = 0; + } + +#endif + +#ifdef PICO_SUPPORT_TCP + if (PROTO(s) == PICO_PROTO_TCP) { + if (pico_tcp_initconn(s) == 0) { + pico_socket_alter_state(s, PICO_SOCKET_STATE_CONNECTED | PICO_SOCKET_STATE_TCP_SYN_SENT, PICO_SOCKET_STATE_CLOSED, 0); + pico_err = PICO_ERR_NOERR; + ret = 0; + } else { + pico_err = PICO_ERR_EHOSTUNREACH; + } + } + +#endif + + return ret; +} + + +#ifdef PICO_SUPPORT_TCP + +int pico_socket_listen(struct pico_socket *s, int backlog) +{ + if (!s || backlog < 1) { + pico_err = PICO_ERR_EINVAL; + return -1; + } else { + /* check if exists in tree */ + /* See task #178 */ + if (pico_check_socket(s) != 0) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + } + + if (PROTO(s) == PICO_PROTO_UDP) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + if ((s->state & PICO_SOCKET_STATE_BOUND) == 0) { + pico_err = PICO_ERR_EISCONN; + return -1; + } + + if (PROTO(s) == PICO_PROTO_TCP) + pico_socket_alter_state(s, PICO_SOCKET_STATE_TCP_SYN_SENT, 0, PICO_SOCKET_STATE_TCP_LISTEN); + + s->max_backlog = (uint16_t)backlog; + + return 0; +} + +struct pico_socket *pico_socket_accept(struct pico_socket *s, void *orig, uint16_t *port) +{ + if (!s || !orig || !port) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + pico_err = PICO_ERR_EINVAL; + + if ((s->state & PICO_SOCKET_STATE_BOUND) == 0) { + return NULL; + } + + if (PROTO(s) == PICO_PROTO_UDP) { + return NULL; + } + + if (TCPSTATE(s) == PICO_SOCKET_STATE_TCP_LISTEN) { + struct pico_sockport *sp = pico_get_sockport(PICO_PROTO_TCP, s->local_port); + struct pico_socket *found; + uint32_t socklen = sizeof(struct pico_ip4); + /* If at this point no incoming connection socket is found, + * the accept call is valid, but no connection is established yet. + */ + pico_err = PICO_ERR_EAGAIN; + if (sp) { + struct pico_tree_node *index; + /* RB_FOREACH(found, socket_tree, &sp->socks) { */ + pico_tree_foreach(index, &sp->socks){ + found = index->keyValue; + if ((s == found->parent) && ((found->state & PICO_SOCKET_STATE_TCP) == PICO_SOCKET_STATE_TCP_ESTABLISHED)) { + found->parent = NULL; + pico_err = PICO_ERR_NOERR; + #ifdef PICO_SUPPORT_IPV6 + if (is_sock_ipv6(s)) + socklen = sizeof(struct pico_ip6); + + #endif + memcpy(orig, &found->remote_addr, socklen); + *port = found->remote_port; + s->number_of_pending_conn--; + return found; + } + } + } + } + + return NULL; +} + +#else + +int pico_socket_listen(struct pico_socket *s, int backlog) +{ + IGNORE_PARAMETER(s); + IGNORE_PARAMETER(backlog); + pico_err = PICO_ERR_EINVAL; + return -1; +} + +struct pico_socket *pico_socket_accept(struct pico_socket *s, void *orig, uint16_t *local_port) +{ + IGNORE_PARAMETER(s); + IGNORE_PARAMETER(orig); + IGNORE_PARAMETER(local_port); + pico_err = PICO_ERR_EINVAL; + return NULL; +} + +#endif + + +int MOCKABLE pico_socket_setoption(struct pico_socket *s, int option, void *value) +{ + + if (s == NULL) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + + if (PROTO(s) == PICO_PROTO_TCP) + return pico_setsockopt_tcp(s, option, value); + + if (PROTO(s) == PICO_PROTO_UDP) + return pico_setsockopt_udp(s, option, value); + + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; +} + + +int pico_socket_getoption(struct pico_socket *s, int option, void *value) +{ + if (s == NULL) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + + if (PROTO(s) == PICO_PROTO_TCP) + return pico_getsockopt_tcp(s, option, value); + + if (PROTO(s) == PICO_PROTO_UDP) + return pico_getsockopt_udp(s, option, value); + + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; +} + + +int pico_socket_shutdown(struct pico_socket *s, int mode) +{ + if (!s) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* Check if the socket has already been closed */ + if (s->state & PICO_SOCKET_STATE_CLOSED) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* unbound sockets can be deleted immediately */ + if (!(s->state & PICO_SOCKET_STATE_BOUND)) + { + socket_garbage_collect((pico_time)10, s); + return 0; + } + +#ifdef PICO_SUPPORT_UDP + if (PROTO(s) == PICO_PROTO_UDP) { + if ((mode & PICO_SHUT_RDWR) == PICO_SHUT_RDWR) + pico_socket_alter_state(s, PICO_SOCKET_STATE_CLOSED, PICO_SOCKET_STATE_CLOSING | PICO_SOCKET_STATE_BOUND | PICO_SOCKET_STATE_CONNECTED, 0); + else if (mode & PICO_SHUT_RD) + pico_socket_alter_state(s, 0, PICO_SOCKET_STATE_BOUND, 0); + } + +#endif +#ifdef PICO_SUPPORT_TCP + if (PROTO(s) == PICO_PROTO_TCP) { + if ((mode & PICO_SHUT_RDWR) == PICO_SHUT_RDWR) + { + pico_socket_alter_state(s, PICO_SOCKET_STATE_SHUT_LOCAL | PICO_SOCKET_STATE_SHUT_REMOTE, 0, 0); + pico_tcp_notify_closing(s); + } + else if (mode & PICO_SHUT_WR) { + pico_socket_alter_state(s, PICO_SOCKET_STATE_SHUT_LOCAL, 0, 0); + pico_tcp_notify_closing(s); + } else if (mode & PICO_SHUT_RD) + pico_socket_alter_state(s, PICO_SOCKET_STATE_SHUT_REMOTE, 0, 0); + + } + +#endif + return 0; +} + +int MOCKABLE pico_socket_close(struct pico_socket *s) +{ + if (!s) + return -1; + +#ifdef PICO_SUPPORT_TCP + if (PROTO(s) == PICO_PROTO_TCP) { + if (pico_tcp_check_listen_close(s) == 0) + return 0; + } + +#endif + return pico_socket_shutdown(s, PICO_SHUT_RDWR); +} + +#ifdef PICO_SUPPORT_CRC +static inline int pico_transport_crc_check(struct pico_frame *f) +{ + struct pico_ipv4_hdr *net_hdr = (struct pico_ipv4_hdr *) f->net_hdr; + struct pico_udp_hdr *udp_hdr = NULL; + uint16_t checksum_invalid = 1; + + switch (net_hdr->proto) + { +#ifdef PICO_SUPPORT_TCP + case PICO_PROTO_TCP: + checksum_invalid = short_be(pico_tcp_checksum(f)); + /* dbg("TCP CRC validation == %u\n", checksum_invalid); */ + if (checksum_invalid) { + dbg("TCP CRC: validation failed!\n"); + pico_frame_discard(f); + return 0; + } + + break; +#endif /* PICO_SUPPORT_TCP */ + +#ifdef PICO_SUPPORT_UDP + case PICO_PROTO_UDP: + udp_hdr = (struct pico_udp_hdr *) f->transport_hdr; + if (short_be(udp_hdr->crc)) { +#ifdef PICO_SUPPORT_IPV4 + if (IS_IPV4(f)) + checksum_invalid = short_be(pico_udp_checksum_ipv4(f)); + +#endif +#ifdef PICO_SUPPORT_IPV6 + if (IS_IPV6(f)) + checksum_invalid = short_be(pico_udp_checksum_ipv6(f)); + +#endif + /* dbg("UDP CRC validation == %u\n", checksum_invalid); */ + if (checksum_invalid) { + /* dbg("UDP CRC: validation failed!\n"); */ + pico_frame_discard(f); + return 0; + } + } + + break; +#endif /* PICO_SUPPORT_UDP */ + + default: + /* Do nothing */ + break; + } + return 1; +} +#else +static inline int pico_transport_crc_check(struct pico_frame *f) +{ + IGNORE_PARAMETER(f); + return 1; +} +#endif /* PICO_SUPPORT_CRC */ + +int pico_transport_process_in(struct pico_protocol *self, struct pico_frame *f) +{ + struct pico_trans *hdr = (struct pico_trans *) f->transport_hdr; + int ret = 0; + + if (!hdr) { + pico_err = PICO_ERR_EFAULT; + return -1; + } + + ret = pico_transport_crc_check(f); + if (ret < 1) + return ret; + else + ret = 0; + + if ((hdr) && (pico_socket_deliver(self, f, hdr->dport) == 0)) + return ret; + + if (!IS_BCAST(f)) { + dbg("Socket not found... \n"); + pico_notify_socket_unreachable(f); + ret = -1; + pico_err = PICO_ERR_ENOENT; + } + + pico_frame_discard(f); + return ret; +} + +#define SL_LOOP_MIN 1 + +#ifdef PICO_SUPPORT_TCP +static int check_socket_sanity(struct pico_socket *s) +{ + + /* checking for pending connections */ + if(TCP_STATE(s) == PICO_SOCKET_STATE_TCP_SYN_RECV) { + if((PICO_TIME_MS() - s->timestamp) >= PICO_SOCKET_BOUND_TIMEOUT) + return -1; + } + + return 0; +} +#endif + + +static int pico_sockets_loop_udp(int loop_score) +{ + +#ifdef PICO_SUPPORT_UDP + static struct pico_tree_node *index_udp; + struct pico_sockport *start; + struct pico_socket *s; + struct pico_frame *f; + + if (sp_udp == NULL) + { + index_udp = pico_tree_firstNode(UDPTable.root); + sp_udp = index_udp->keyValue; + } + + /* init start node */ + start = sp_udp; + + /* round-robin all transport protocols, break if traversed all protocols */ + while (loop_score > SL_LOOP_MIN && sp_udp != NULL) { + struct pico_tree_node *index; + + pico_tree_foreach(index, &sp_udp->socks){ + s = index->keyValue; + f = pico_dequeue(&s->q_out); + while (f && (loop_score > 0)) { + pico_proto_udp.push(&pico_proto_udp, f); + loop_score -= 1; + if (loop_score > 0) /* only dequeue if there is still loop_score, otherwise f might get lost */ + f = pico_dequeue(&s->q_out); + } + } + + index_udp = pico_tree_next(index_udp); + sp_udp = index_udp->keyValue; + + if (sp_udp == NULL) + { + index_udp = pico_tree_firstNode(UDPTable.root); + sp_udp = index_udp->keyValue; + } + + if (sp_udp == start) + break; + } +#endif + return loop_score; +} + +static int pico_sockets_loop_tcp(int loop_score) +{ +#ifdef PICO_SUPPORT_TCP + struct pico_sockport *start; + struct pico_socket *s; + static struct pico_tree_node *index_tcp; + if (sp_tcp == NULL) + { + index_tcp = pico_tree_firstNode(TCPTable.root); + sp_tcp = index_tcp->keyValue; + } + + /* init start node */ + start = sp_tcp; + + while (loop_score > SL_LOOP_MIN && sp_tcp != NULL) { + struct pico_tree_node *index = NULL, *safe_index = NULL; + pico_tree_foreach_safe(index, &sp_tcp->socks, safe_index){ + s = index->keyValue; + loop_score = pico_tcp_output(s, loop_score); + if ((s->ev_pending) && s->wakeup) { + s->wakeup(s->ev_pending, s); + if(!s->parent) + s->ev_pending = 0; + } + + if (loop_score <= 0) { + loop_score = 0; + break; + } + + if(check_socket_sanity(s) < 0) + { + pico_socket_del(s); + index_tcp = NULL; /* forcing the restart of loop */ + sp_tcp = NULL; + break; + } + } + + /* check if RB_FOREACH ended, if not, break to keep the cur sp_tcp */ + if (!index_tcp || (index && index->keyValue)) + break; + + index_tcp = pico_tree_next(index_tcp); + sp_tcp = index_tcp->keyValue; + + if (sp_tcp == NULL) + { + index_tcp = pico_tree_firstNode(TCPTable.root); + sp_tcp = index_tcp->keyValue; + } + + if (sp_tcp == start) + break; + } +#endif + return loop_score; + + +} + +int pico_sockets_loop(int loop_score) +{ + loop_score = pico_sockets_loop_udp(loop_score); + loop_score = pico_sockets_loop_tcp(loop_score); + return loop_score; +} + +int pico_count_sockets(uint8_t proto) +{ + struct pico_sockport *sp; + struct pico_tree_node *idx_sp, *idx_s; + int count = 0; + + if ((proto == 0) || (proto == PICO_PROTO_TCP)) { + pico_tree_foreach(idx_sp, &TCPTable) { + sp = idx_sp->keyValue; + if (sp) { + pico_tree_foreach(idx_s, &sp->socks) + count++; + } + } + } + + if ((proto == 0) || (proto == PICO_PROTO_UDP)) { + pico_tree_foreach(idx_sp, &UDPTable) { + sp = idx_sp->keyValue; + if (sp) { + pico_tree_foreach(idx_s, &sp->socks) + count++; + } + } + } + + return count; +} + + +struct pico_frame *pico_socket_frame_alloc(struct pico_socket *s, struct pico_device *dev, uint16_t len) +{ + struct pico_frame *f = NULL; + +#ifdef PICO_SUPPORT_IPV6 + if (is_sock_ipv6(s)) + f = pico_proto_ipv6.alloc(&pico_proto_ipv6, dev, len); + +#endif + +#ifdef PICO_SUPPORT_IPV4 + if (is_sock_ipv4(s)) + f = pico_proto_ipv4.alloc(&pico_proto_ipv4, dev, len); + +#endif + if (!f) { + pico_err = PICO_ERR_ENOMEM; + return f; + } + + f->payload = f->transport_hdr; + f->payload_len = len; + f->sock = s; + return f; +} + +static void pico_transport_error_set_picoerr(int code) +{ + /* dbg("SOCKET ERROR FROM ICMP NOTIFICATION. (icmp code= %d)\n\n", code); */ + switch(code) { + case PICO_ICMP_UNREACH_NET: + pico_err = PICO_ERR_ENETUNREACH; + break; + + case PICO_ICMP_UNREACH_HOST: + pico_err = PICO_ERR_EHOSTUNREACH; + break; + + case PICO_ICMP_UNREACH_PROTOCOL: + pico_err = PICO_ERR_ENOPROTOOPT; + break; + + case PICO_ICMP_UNREACH_PORT: + pico_err = PICO_ERR_ECONNREFUSED; + break; + + case PICO_ICMP_UNREACH_NET_UNKNOWN: + pico_err = PICO_ERR_ENETUNREACH; + break; + + case PICO_ICMP_UNREACH_HOST_UNKNOWN: + pico_err = PICO_ERR_EHOSTDOWN; + break; + + case PICO_ICMP_UNREACH_ISOLATED: + pico_err = PICO_ERR_ENONET; + break; + + case PICO_ICMP_UNREACH_NET_PROHIB: + case PICO_ICMP_UNREACH_HOST_PROHIB: + pico_err = PICO_ERR_EHOSTUNREACH; + break; + + default: + pico_err = PICO_ERR_EOPNOTSUPP; + } +} + +int pico_transport_error(struct pico_frame *f, uint8_t proto, int code) +{ + int ret = -1; + struct pico_trans *trans = (struct pico_trans*) f->transport_hdr; + struct pico_sockport *port = NULL; + struct pico_socket *s = NULL; + switch (proto) { + + +#ifdef PICO_SUPPORT_UDP + case PICO_PROTO_UDP: + port = pico_get_sockport(proto, trans->sport); + break; +#endif + +#ifdef PICO_SUPPORT_TCP + case PICO_PROTO_TCP: + port = pico_get_sockport(proto, trans->sport); + break; +#endif + + default: + /* Protocol not available */ + ret = -1; + } + if (port) { + struct pico_tree_node *index; + ret = 0; + + pico_tree_foreach(index, &port->socks) { + s = index->keyValue; + if (trans->dport == s->remote_port) { + if (s->wakeup) { + pico_transport_error_set_picoerr(code); + s->state |= PICO_SOCKET_STATE_SHUT_REMOTE; + s->wakeup(PICO_SOCK_EV_ERR, s); + } + + break; + } + } + } + + pico_frame_discard(f); + return ret; +} +#endif +#endif diff --git a/core/deps/picotcp/stack/pico_socket_multicast.c b/core/deps/picotcp/stack/pico_socket_multicast.c new file mode 100644 index 000000000..8e69e45ce --- /dev/null +++ b/core/deps/picotcp/stack/pico_socket_multicast.c @@ -0,0 +1,1478 @@ +#include "pico_config.h" +#include "pico_stack.h" +#include "pico_socket.h" +#include "pico_socket_multicast.h" +#include "pico_tree.h" +#include "pico_ipv4.h" +#include "pico_ipv6.h" +#include "pico_udp.h" + +#ifdef PICO_SUPPORT_MCAST + +#ifdef DEBUG_MCAST +#define so_mcast_dbg dbg +#else +#define so_mcast_dbg(...) do { } while(0) +#endif + +/* socket + * | + * MCASTListen + * | | | + * ------------ | ------------ + * | | | + * MCASTSources MCASTSources MCASTSources + * | | | | | | | | | | | | + * S S S S S S S S S S S S + * + * MCASTListen: RBTree(mcast_link, mcast_group) + * MCASTSources: RBTree(source) + */ +struct pico_mcast_listen +{ + int8_t filter_mode; + union pico_address mcast_link; + union pico_address mcast_group; + struct pico_tree MCASTSources; + struct pico_tree MCASTSources_ipv6; + uint16_t proto; +}; +/* Parameters */ +struct pico_mcast +{ + struct pico_socket *s; + struct pico_ip_mreq *mreq; + struct pico_ip_mreq_source *mreq_s; + union pico_address *address; + union pico_link *mcast_link; + struct pico_mcast_listen *listen; +}; +static int mcast_listen_link_cmp(struct pico_mcast_listen *a, struct pico_mcast_listen *b) +{ + + if (a->proto < b->proto) + return -1; + + if (a->proto > b->proto) + return 1; + + return pico_address_compare(&a->mcast_link, &b->mcast_link, a->proto); +} + +static int mcast_listen_grp_cmp(struct pico_mcast_listen *a, struct pico_mcast_listen *b) +{ + if (a->mcast_group.ip4.addr < b->mcast_group.ip4.addr) + return -1; + + if (a->mcast_group.ip4.addr > b->mcast_group.ip4.addr) + return 1; + + return mcast_listen_link_cmp(a, b); +} +#ifdef PICO_SUPPORT_IPV6 +static int mcast_listen_grp_cmp_ipv6(struct pico_mcast_listen *a, struct pico_mcast_listen *b) +{ + int tmp = memcmp(&a->mcast_group.ip6, &b->mcast_group.ip6, sizeof(struct pico_ip6)); + if(!tmp) + return mcast_listen_link_cmp(a, b); + + return tmp; +} +#endif + +static int mcast_listen_cmp(void *ka, void *kb) +{ + struct pico_mcast_listen *a = ka, *b = kb; + if (a->proto < b->proto) + return -1; + + if (a->proto > b->proto) + return 1; + + return mcast_listen_grp_cmp(a, b); +} +#ifdef PICO_SUPPORT_IPV6 +static int mcast_listen_cmp_ipv6(void *ka, void *kb) +{ + struct pico_mcast_listen *a = ka, *b = kb; + if (a->proto < b->proto) + return -1; + + if (a->proto > b->proto) + return 1; + + return mcast_listen_grp_cmp_ipv6(a, b); +} +#endif +static int mcast_sources_cmp(void *ka, void *kb) +{ + union pico_address *a = ka, *b = kb; + if (a->ip4.addr < b->ip4.addr) + return -1; + + if (a->ip4.addr > b->ip4.addr) + return 1; + + return 0; +} +#ifdef PICO_SUPPORT_IPV6 +static int mcast_sources_cmp_ipv6(void *ka, void *kb) +{ + union pico_address *a = ka, *b = kb; + return memcmp(&a->ip6, &b->ip6, sizeof(struct pico_ip6)); +} +#endif +static int mcast_socket_cmp(void *ka, void *kb) +{ + struct pico_socket *a = ka, *b = kb; + if (a < b) + return -1; + + if (a > b) + return 1; + + return 0; +} + +/* gather all multicast sockets to hasten filter aggregation */ +static PICO_TREE_DECLARE(MCASTSockets, mcast_socket_cmp); + +static int mcast_filter_cmp(void *ka, void *kb) +{ + union pico_address *a = ka, *b = kb; + if (a->ip4.addr < b->ip4.addr) + return -1; + + if (a->ip4.addr > b->ip4.addr) + return 1; + + return 0; +} +/* gather sources to be filtered */ +static PICO_TREE_DECLARE(MCASTFilter, mcast_filter_cmp); + +static int mcast_filter_cmp_ipv6(void *ka, void *kb) +{ + union pico_address *a = ka, *b = kb; + return memcmp(&a->ip6, &b->ip6, sizeof(struct pico_ip6)); +} +/* gather sources to be filtered */ +static PICO_TREE_DECLARE(MCASTFilter_ipv6, mcast_filter_cmp_ipv6); + +inline static struct pico_tree *mcast_get_src_tree(struct pico_socket *s, struct pico_mcast *mcast) +{ + if( IS_SOCK_IPV4(s)) { + mcast->listen->MCASTSources.compare = mcast_sources_cmp; + return &mcast->listen->MCASTSources; + } + +#ifdef PICO_SUPPORT_IPV6 + else if( IS_SOCK_IPV6(s)) { + mcast->listen->MCASTSources_ipv6.compare = mcast_sources_cmp_ipv6; + return &mcast->listen->MCASTSources_ipv6; + } +#endif + return NULL; +} +inline static struct pico_tree *mcast_get_listen_tree(struct pico_socket *s) +{ + if( IS_SOCK_IPV4(s)) + return s->MCASTListen; + +#ifdef PICO_SUPPORT_IPV6 + else if( IS_SOCK_IPV6(s)) + return s->MCASTListen_ipv6; +#endif + return NULL; +} +inline static void mcast_set_listen_tree_p_null(struct pico_socket *s) +{ + if( IS_SOCK_IPV4(s)) + s->MCASTListen = NULL; + +#ifdef PICO_SUPPORT_IPV6 + else if( IS_SOCK_IPV6(s)) + s->MCASTListen_ipv6 = NULL; +#endif +} +static struct pico_mcast_listen *listen_find(struct pico_socket *s, union pico_address *lnk, union pico_address *grp) +{ + struct pico_mcast_listen ltest = { + 0 + }; + ltest.mcast_link = *lnk; + ltest.mcast_group = *grp; + + if(IS_SOCK_IPV4(s)) + return pico_tree_findKey(s->MCASTListen, <est); + +#ifdef PICO_SUPPORT_IPV6 + else if(IS_SOCK_IPV6(s)) { + ltest.proto = PICO_PROTO_IPV6; + return pico_tree_findKey(s->MCASTListen_ipv6, <est); + } +#endif + return NULL; +} +static union pico_address *pico_mcast_get_link_address(struct pico_socket *s, union pico_link *mcast_link) +{ + if( IS_SOCK_IPV4(s)) + return (union pico_address *) &mcast_link->ipv4.address; + +#ifdef PICO_SUPPORT_IPV6 + if( IS_SOCK_IPV6(s)) + return (union pico_address *) &mcast_link->ipv6.address; + +#endif + return NULL; +} +static int8_t pico_mcast_filter_excl_excl(struct pico_mcast_listen *listen) +{ + /* filter = intersection of EXCLUDEs */ + /* any record with filter mode EXCLUDE, causes the interface mode to be EXCLUDE */ + /* remove from the interface EXCLUDE filter any source not in the socket EXCLUDE filter */ + struct pico_tree_node *index = NULL, *_tmp = NULL; + union pico_address *source = NULL; + if(!pico_tree_empty(&MCASTFilter)) { + pico_tree_foreach_safe(index, &MCASTFilter, _tmp) + { + source = pico_tree_findKey(&listen->MCASTSources, index->keyValue); + if (!source) + pico_tree_delete(&MCASTFilter, index->keyValue); + } + } + +#ifdef PICO_SUPPORT_IPV6 + if(!pico_tree_empty(&MCASTFilter_ipv6)) { + pico_tree_foreach_safe(index, &MCASTFilter_ipv6, _tmp) + { + source = pico_tree_findKey(&listen->MCASTSources_ipv6, index->keyValue); + if (!source) + pico_tree_delete(&MCASTFilter_ipv6, index->keyValue); + } + } + +#endif + return PICO_IP_MULTICAST_EXCLUDE; +} + +static int8_t pico_mcast_filter_excl_incl(struct pico_mcast_listen *listen) +{ + /* filter = EXCLUDE - INCLUDE */ + /* any record with filter mode EXCLUDE, causes the interface mode to be EXCLUDE */ + /* remove from the interface EXCLUDE filter any source in the socket INCLUDE filter */ + struct pico_tree_node *index = NULL, *_tmp = NULL; + union pico_address *source = NULL; + if(!pico_tree_empty(&listen->MCASTSources)) { + pico_tree_foreach_safe(index, &listen->MCASTSources, _tmp) + { + source = pico_tree_findKey(&MCASTFilter, index->keyValue); + if (source) + pico_tree_delete(&MCASTFilter, source); + } + } + +#ifdef PICO_SUPPORT_IPV6 + if(!pico_tree_empty(&listen->MCASTSources_ipv6)) { + pico_tree_foreach_safe(index, &listen->MCASTSources_ipv6, _tmp) + { + source = pico_tree_findKey(&MCASTFilter_ipv6, index->keyValue); + if (source) + pico_tree_delete(&MCASTFilter_ipv6, source); + } + } + +#endif + return PICO_IP_MULTICAST_EXCLUDE; +} + +static int8_t pico_mcast_filter_incl_excl(struct pico_mcast_listen *listen) +{ + /* filter = EXCLUDE - INCLUDE */ + /* delete from the interface INCLUDE filter any source NOT in the socket EXCLUDE filter */ + struct pico_tree_node *index = NULL, *_tmp = NULL; + union pico_address *source = NULL; + if(!pico_tree_empty(&listen->MCASTSources)) { + pico_tree_foreach_safe(index, &MCASTFilter, _tmp) + { + source = pico_tree_findKey(&listen->MCASTSources, index->keyValue); + if (!source) + pico_tree_delete(&MCASTFilter, index->keyValue); + } + } + +#ifdef PICO_SUPPORT_IPV6 + if(!pico_tree_empty(&listen->MCASTSources_ipv6)) { + pico_tree_foreach_safe(index, &MCASTFilter_ipv6, _tmp) + { + source = pico_tree_findKey(&listen->MCASTSources_ipv6, index->keyValue); + if (!source) + pico_tree_delete(&MCASTFilter_ipv6, index->keyValue); + } + } + +#endif + /* any record with filter mode EXCLUDE, causes the interface mode to be EXCLUDE */ + + /* add to the interface EXCLUDE filter any socket source NOT in the former interface INCLUDE filter */ + if(!pico_tree_empty(&listen->MCASTSources)) { + pico_tree_foreach_safe(index, &listen->MCASTSources, _tmp) + { + source = pico_tree_insert(&MCASTFilter, index->keyValue); + if (source) { + if ((void *)source == (void *)&LEAF) + return -1; + else + pico_tree_delete(&MCASTFilter, source); + } + } + } + +#ifdef PICO_SUPPORT_IPV6 + if(!pico_tree_empty(&listen->MCASTSources_ipv6)) { + pico_tree_foreach_safe(index, &listen->MCASTSources_ipv6, _tmp) + { + source = pico_tree_insert(&MCASTFilter_ipv6, index->keyValue); + if (source) { + if ((void *)source == (void *)&LEAF) + return -1; + else + pico_tree_delete(&MCASTFilter_ipv6, source); + } + } + } + +#endif + return PICO_IP_MULTICAST_EXCLUDE; +} + +static int8_t pico_mcast_filter_incl_incl(struct pico_mcast_listen *listen) +{ + /* filter = summation of INCLUDEs */ + /* mode stays INCLUDE, add all sources to filter */ + struct pico_tree_node *index = NULL, *_tmp = NULL; + union pico_address *source = NULL; + + if( !pico_tree_empty(&listen->MCASTSources)) { + pico_tree_foreach_safe(index, &listen->MCASTSources, _tmp) + { + source = index->keyValue; + if (pico_tree_insert(&MCASTFilter, source) == &LEAF) + return -1; + } + } + +#ifdef PICO_SUPPORT_IPV6 + if( !pico_tree_empty(&listen->MCASTSources_ipv6)) { + pico_tree_foreach_safe(index, &listen->MCASTSources_ipv6, _tmp) + { + source = index->keyValue; + if (pico_tree_insert(&MCASTFilter_ipv6, source) == &LEAF) + return -1; + } + } + +#endif + return PICO_IP_MULTICAST_INCLUDE; +} + +struct pico_mcast_filter_aggregation +{ + int8_t (*call)(struct pico_mcast_listen *); +}; + +static const struct pico_mcast_filter_aggregation mcast_filter_aggr_call[2][2] = +{ + { + /* EXCL + EXCL */ {.call = pico_mcast_filter_excl_excl}, + /* EXCL + INCL */ {.call = pico_mcast_filter_excl_incl} + }, + + { + /* INCL + EXCL */ {.call = pico_mcast_filter_incl_excl}, + /* INCL + INCL */ {.call = pico_mcast_filter_incl_incl} + } +}; + +static int mcast_aggr_validate(int8_t fm, struct pico_mcast_listen *l) +{ + if (!l) + return -1; + + if (fm > 1 || fm < 0) + return -1; + + if (l->filter_mode > 1) + return -1; + + return 0; +} + + +/* MCASTFilter will be empty if no socket is listening on mcast_group on mcast_link anymore */ +static int pico_socket_aggregate_mcastfilters(union pico_address *mcast_link, union pico_address *mcast_group) +{ + int8_t filter_mode = PICO_IP_MULTICAST_INCLUDE; + struct pico_mcast_listen *listen = NULL; + struct pico_socket *mcast_sock = NULL; + struct pico_tree_node *index = NULL, *_tmp = NULL; + + /* cleanup old filter */ + if(!pico_tree_empty(&MCASTFilter)) { + pico_tree_foreach_safe(index, &MCASTFilter, _tmp) + { + pico_tree_delete(&MCASTFilter, index->keyValue); + } + } + +#ifdef PICO_SUPPORT_IPV6 + if(!pico_tree_empty(&MCASTFilter_ipv6)) { + pico_tree_foreach_safe(index, &MCASTFilter_ipv6, _tmp) + { + pico_tree_delete(&MCASTFilter_ipv6, index->keyValue); + } + } + +#endif + /* construct new filter */ + pico_tree_foreach_safe(index, &MCASTSockets, _tmp) + { + mcast_sock = index->keyValue; + listen = listen_find(mcast_sock, mcast_link, mcast_group); + if (listen) { + if (mcast_aggr_validate(filter_mode, listen) < 0) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + if (mcast_filter_aggr_call[filter_mode][listen->filter_mode].call) { + filter_mode = mcast_filter_aggr_call[filter_mode][listen->filter_mode].call(listen); + if (filter_mode > 1 || filter_mode < 0) + return -1; + } + } + } + return filter_mode; +} + +static int pico_socket_mcast_filter_include(struct pico_mcast_listen *listen, union pico_address *src) +{ + struct pico_tree_node *index = NULL; +#ifdef PICO_DEBUG_MCAST + char tmp_string[PICO_IPV6_STRING]; +#endif + if(!pico_tree_empty(&listen->MCASTSources)) { + pico_tree_foreach(index, &listen->MCASTSources) + { + if (src->ip4.addr == ((union pico_address *)index->keyValue)->ip4.addr) { + so_mcast_dbg("MCAST: IP %08X in included socket source list\n", src->ip4.addr); + return 0; + } + } + } + +#ifdef PICO_SUPPORT_IPV6 + if(!pico_tree_empty(&listen->MCASTSources_ipv6)) { + pico_tree_foreach(index, &listen->MCASTSources_ipv6) + { + if (memcmp(&src->ip6, &((union pico_address *)index->keyValue)->ip6, sizeof(struct pico_ip6))) { +#ifdef PICO_DEBUG_MCAST + pico_ipv6_to_string(tmp_string, src->ip6.addr); + so_mcast_dbg("MCAST: IP %s in included socket source list\n", tmp_string); +#endif + return 0; + } + } + } + +#endif + /* XXX IPV6 ADDRESS */ + so_mcast_dbg("MCAST: IP %08X NOT in included socket source list\n", src->ip4.addr); + return -1; + +} + +static int pico_socket_mcast_filter_exclude(struct pico_mcast_listen *listen, union pico_address *src) +{ + struct pico_tree_node *index = NULL; +#ifdef PICO_DEBUG_MCAST + char tmp_string[PICO_IPV6_STRING]; +#endif + if(!pico_tree_empty(&listen->MCASTSources)) { + pico_tree_foreach(index, &listen->MCASTSources) + { + if (src->ip4.addr == ((union pico_address *)index->keyValue)->ip4.addr) { + so_mcast_dbg("MCAST: IP %08X in excluded socket source list\n", src->ip4.addr); + return -1; + } + } + } + +#ifdef PICO_SUPPORT_IPV6 + if(!pico_tree_empty(&listen->MCASTSources_ipv6)) { + pico_tree_foreach(index, &listen->MCASTSources_ipv6) + { + if (memcmp(&src->ip6, &((union pico_address *)index->keyValue)->ip6, sizeof(struct pico_ip6))) { +#ifdef PICO_DEBUG_MCAST + pico_ipv6_to_string(tmp_string, src->ip6.addr); + so_mcast_dbg("MCAST: IP %s in excluded socket source list\n", tmp_string); +#endif + return 0; + } + } + } + +#endif + /* XXX IPV6 ADDRESS */ + so_mcast_dbg("MCAST: IP %08X NOT in excluded socket source list\n", src->ip4.addr); + return 0; +} + +static int pico_socket_mcast_source_filtering(struct pico_mcast_listen *listen, union pico_address *src) +{ + /* perform source filtering */ + if (listen->filter_mode == PICO_IP_MULTICAST_INCLUDE) + return pico_socket_mcast_filter_include(listen, src); + + if (listen->filter_mode == PICO_IP_MULTICAST_EXCLUDE) + return pico_socket_mcast_filter_exclude(listen, src); + + return -1; +} + +static void *pico_socket_mcast_filter_link_get(struct pico_socket *s) +{ + /* check if no multicast enabled on socket */ + if (!s->MCASTListen) + return NULL; + + if( IS_SOCK_IPV4(s)) { + if (!s->local_addr.ip4.addr) + return pico_ipv4_get_default_mcastlink(); + + return pico_ipv4_link_get(&s->local_addr.ip4); + } + +#ifdef PICO_SUPPORT_IPV6 + else if( IS_SOCK_IPV6(s)) { + if (pico_ipv6_is_null_address(&s->local_addr.ip6)) + return pico_ipv6_get_default_mcastlink(); + + return pico_ipv6_link_get(&s->local_addr.ip6); + } +#endif + return NULL; +} + +int pico_socket_mcast_filter(struct pico_socket *s, union pico_address *mcast_group, union pico_address *src) +{ + void *mcast_link = NULL; + struct pico_mcast_listen *listen = NULL; + mcast_link = pico_socket_mcast_filter_link_get(s); + if (!mcast_link) + return -1; + + if(IS_SOCK_IPV4(s)) + listen = listen_find(s, (union pico_address *) &((struct pico_ipv4_link*)(mcast_link))->address, mcast_group); + +#ifdef PICO_SUPPORT_IPV6 + else if(IS_SOCK_IPV6(s)) + listen = listen_find(s, (union pico_address *)&((struct pico_ipv6_link*)(mcast_link))->address, mcast_group); +#endif + if (!listen) + return -1; + + return pico_socket_mcast_source_filtering(listen, src); +} + + +static struct pico_ipv4_link *get_mcast_link(union pico_address *a) +{ + if (!a->ip4.addr) + return pico_ipv4_get_default_mcastlink(); + + return pico_ipv4_link_get(&a->ip4); +} +#ifdef PICO_SUPPORT_IPV6 +static struct pico_ipv6_link *get_mcast_link_ipv6(union pico_address *a) +{ + + if (pico_ipv6_is_null_address(&a->ip6)) { + return pico_ipv6_get_default_mcastlink(); + } + + return pico_ipv6_link_get(&a->ip6); +} +#endif + +static int pico_socket_setoption_pre_validation(struct pico_ip_mreq *mreq) +{ + if (!mreq) + return -1; + + if (!mreq->mcast_group_addr.ip4.addr) + return -1; + + return 0; +} +#ifdef PICO_SUPPORT_IPV6 +static int pico_socket_setoption_pre_validation_ipv6(struct pico_ip_mreq *mreq) +{ + if (!mreq) + return -1; + + if (pico_ipv6_is_null_address((struct pico_ip6*)&mreq->mcast_group_addr)) + return -1; + + return 0; +} +#endif + +static struct pico_ipv4_link *pico_socket_setoption_validate_mreq(struct pico_ip_mreq *mreq) +{ + if (pico_socket_setoption_pre_validation(mreq) < 0) + return NULL; + + if (pico_ipv4_is_unicast(mreq->mcast_group_addr.ip4.addr)) + return NULL; + + return get_mcast_link((union pico_address *)&mreq->mcast_link_addr); +} + +#ifdef PICO_SUPPORT_IPV6 +static struct pico_ipv6_link *pico_socket_setoption_validate_mreq_ipv6(struct pico_ip_mreq *mreq) +{ + if (pico_socket_setoption_pre_validation_ipv6(mreq) < 0) + return NULL; + + if (pico_ipv6_is_unicast((struct pico_ip6 *)&mreq->mcast_group_addr)) + return NULL; + + return get_mcast_link_ipv6((union pico_address *)&mreq->mcast_link_addr); +} +#endif + +static int pico_socket_setoption_pre_validation_s(struct pico_ip_mreq_source *mreq) +{ + if (!mreq) + return -1; + + if (!mreq->mcast_group_addr.ip4.addr) + return -1; + + return 0; +} +#ifdef PICO_SUPPORT_IPV6 +static int pico_socket_setoption_pre_validation_s_ipv6(struct pico_ip_mreq_source *mreq) +{ + if (!mreq) + return -1; + + if (pico_ipv6_is_null_address((struct pico_ip6 *)&mreq->mcast_group_addr)) + return -1; + + return 0; +} +#endif + +static struct pico_ipv4_link *pico_socket_setoption_validate_s_mreq(struct pico_ip_mreq_source *mreq) +{ + if (pico_socket_setoption_pre_validation_s(mreq) < 0) + return NULL; + + if (pico_ipv4_is_unicast(mreq->mcast_group_addr.ip4.addr)) + return NULL; + + if (!pico_ipv4_is_unicast(mreq->mcast_source_addr.ip4.addr)) + return NULL; + + return get_mcast_link((union pico_address *)&mreq->mcast_link_addr); +} +#ifdef PICO_SUPPORT_IPV6 +static struct pico_ipv6_link *pico_socket_setoption_validate_s_mreq_ipv6(struct pico_ip_mreq_source *mreq) +{ + if (pico_socket_setoption_pre_validation_s_ipv6(mreq) < 0) { + return NULL; + } + + if (pico_ipv6_is_unicast((struct pico_ip6 *)&mreq->mcast_group_addr)) { + return NULL; + } + + if (!pico_ipv6_is_unicast((struct pico_ip6 *)&mreq->mcast_source_addr)) { + return NULL; + } + + return get_mcast_link_ipv6(&mreq->mcast_link_addr); +} +#endif + +static struct pico_ipv4_link *setop_multicast_link_search(void *value, int bysource) +{ + + struct pico_ip_mreq *mreq = NULL; + struct pico_ipv4_link *mcast_link = NULL; + struct pico_ip_mreq_source *mreq_src = NULL; + if (!bysource) { + mreq = (struct pico_ip_mreq *)value; + mcast_link = pico_socket_setoption_validate_mreq(mreq); + if (!mcast_link) + return NULL; + + if (!mreq->mcast_link_addr.ip4.addr) + mreq->mcast_link_addr.ip4.addr = mcast_link->address.addr; + } else { + mreq_src = (struct pico_ip_mreq_source *)value; + if (!mreq_src) { + return NULL; + } + + mcast_link = pico_socket_setoption_validate_s_mreq(mreq_src); + if (!mcast_link) { + return NULL; + } + + if (!mreq_src->mcast_link_addr.ip4.addr) + mreq_src->mcast_link_addr.ip4 = mcast_link->address; + } + + return mcast_link; +} +#ifdef PICO_SUPPORT_IPV6 +static struct pico_ipv6_link *setop_multicast_link_search_ipv6(void *value, int bysource) +{ + struct pico_ip_mreq *mreq = NULL; + struct pico_ipv6_link *mcast_link = NULL; + struct pico_ip_mreq_source *mreq_src = NULL; + if (!bysource) { + mreq = (struct pico_ip_mreq *)value; + mcast_link = pico_socket_setoption_validate_mreq_ipv6(mreq); + if (!mcast_link) { + return NULL; + } + + if (pico_ipv6_is_null_address(&mreq->mcast_link_addr.ip6)) + mreq->mcast_link_addr.ip6 = mcast_link->address; + } else { + mreq_src = (struct pico_ip_mreq_source *)value; + if (!mreq_src) { + return NULL; + } + + mcast_link = pico_socket_setoption_validate_s_mreq_ipv6(mreq_src); + if (!mcast_link) { + return NULL; + } + + if (pico_ipv6_is_null_address(&mreq_src->mcast_link_addr.ip6)) + mreq_src->mcast_link_addr.ip6 = mcast_link->address; + } + + return mcast_link; +} +#endif +static int setop_verify_listen_tree(struct pico_socket *s, int alloc) +{ + if(!alloc) + return -1; + + if( IS_SOCK_IPV4(s)) { + + s->MCASTListen = PICO_ZALLOC(sizeof(struct pico_tree)); + if (!s->MCASTListen) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + s->MCASTListen->root = &LEAF; + s->MCASTListen->compare = mcast_listen_cmp; + return 0; + } + +#ifdef PICO_SUPPORT_IPV6 + else if( IS_SOCK_IPV6(s)) { + s->MCASTListen_ipv6 = PICO_ZALLOC(sizeof(struct pico_tree)); + if (!s->MCASTListen_ipv6) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + s->MCASTListen_ipv6->root = &LEAF; + s->MCASTListen_ipv6->compare = mcast_listen_cmp_ipv6; + return 0; + + } +#endif + return -1; +} + + +static void *setopt_multicast_check(struct pico_socket *s, void *value, int alloc, int bysource) +{ + void *mcast_link = NULL; + struct pico_tree *listen_tree = mcast_get_listen_tree(s); + if (!value) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + if(IS_SOCK_IPV4(s)) + mcast_link = setop_multicast_link_search(value, bysource); + +#ifdef PICO_SUPPORT_IPV6 + else if(IS_SOCK_IPV6(s)) + mcast_link = setop_multicast_link_search_ipv6(value, bysource); +#endif + if (!mcast_link) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + if (!listen_tree) { /* No RBTree allocated yet */ + if (setop_verify_listen_tree(s, alloc) < 0) { + return NULL; + } + } + + return mcast_link; +} + +void pico_multicast_delete(struct pico_socket *s) +{ + int filter_mode; + struct pico_tree_node *index = NULL, *_tmp = NULL, *index2 = NULL, *_tmp2 = NULL; + struct pico_mcast_listen *listen = NULL; + union pico_address *source = NULL; + struct pico_tree *tree, *listen_tree; + struct pico_mcast mcast; + listen_tree = mcast_get_listen_tree(s); + if(listen_tree) { + pico_tree_delete(&MCASTSockets, s); + pico_tree_foreach_safe(index, listen_tree, _tmp) + { + listen = index->keyValue; + mcast.listen = listen; + tree = mcast_get_src_tree(s, &mcast); + if (tree) { + pico_tree_foreach_safe(index2, tree, _tmp2) + { + source = index2->keyValue; + pico_tree_delete(tree, source); + PICO_FREE(source); + } + } + + filter_mode = pico_socket_aggregate_mcastfilters((union pico_address *)&listen->mcast_link, (union pico_address *)&listen->mcast_group); + if (filter_mode >= 0) { + if(IS_SOCK_IPV4(s)) + pico_ipv4_mcast_leave(&listen->mcast_link.ip4, &listen->mcast_group.ip4, 1, (uint8_t)filter_mode, &MCASTFilter); + +#ifdef PICO_SUPPORT_IPV6 + else if(IS_SOCK_IPV6(s)) + pico_ipv6_mcast_leave(&listen->mcast_link.ip6, &listen->mcast_group.ip6, 1, (uint8_t)filter_mode, &MCASTFilter_ipv6); +#endif + } + + pico_tree_delete(listen_tree, listen); + PICO_FREE(listen); + } + PICO_FREE(listen_tree); + mcast_set_listen_tree_p_null(s); + } +} + + +int pico_getsockopt_mcast(struct pico_socket *s, int option, void *value) +{ + switch(option) { + case PICO_IP_MULTICAST_IF: + pico_err = PICO_ERR_EOPNOTSUPP; + return -1; + + case PICO_IP_MULTICAST_TTL: + if (s->proto->proto_number == PICO_PROTO_UDP) { + pico_udp_get_mc_ttl(s, (uint8_t *) value); + } else { + *(uint8_t *)value = 0; + pico_err = PICO_ERR_EINVAL; + return -1; + } + + break; + + case PICO_IP_MULTICAST_LOOP: + if (s->proto->proto_number == PICO_PROTO_UDP) { + *(uint8_t *)value = (uint8_t)PICO_SOCKET_GETOPT(s, PICO_SOCKET_OPT_MULTICAST_LOOP); + } else { + *(uint8_t *)value = 0; + pico_err = PICO_ERR_EINVAL; + return -1; + } + + break; + default: + pico_err = PICO_ERR_EINVAL; + return -1; + } + + return 0; +} + +static int mcast_so_loop(struct pico_socket *s, void *value) +{ + uint8_t val = (*(uint8_t *)value); + if (val == 0u) { + PICO_SOCKET_SETOPT_DIS(s, PICO_SOCKET_OPT_MULTICAST_LOOP); + return 0; + } else if (val == 1u) { + PICO_SOCKET_SETOPT_EN(s, PICO_SOCKET_OPT_MULTICAST_LOOP); + return 0; + } + + pico_err = PICO_ERR_EINVAL; + return -1; +} +static int mcast_get_param(struct pico_mcast *mcast, struct pico_socket *s, void *value, int alloc, int by_source) +{ + if(by_source) + mcast->mreq_s = (struct pico_ip_mreq_source *)value; + else + mcast->mreq = (struct pico_ip_mreq *)value; + + mcast->mcast_link = setopt_multicast_check(s, value, alloc, by_source); + if (!mcast->mcast_link) + return -1; + + mcast->address = pico_mcast_get_link_address(s, mcast->mcast_link); + if(by_source) + mcast->listen = listen_find(s, &(mcast->mreq_s)->mcast_link_addr, &mcast->mreq_s->mcast_group_addr); + else + mcast->listen = listen_find(s, &(mcast->mreq)->mcast_link_addr, &mcast->mreq->mcast_group_addr); + + return 0; +} +static int mcast_so_addm(struct pico_socket *s, void *value) +{ + int filter_mode = 0; + struct pico_mcast mcast; + struct pico_tree *tree, *listen_tree; + if(mcast_get_param(&mcast, s, value, 1, 0) < 0) + return -1; + + if (mcast.listen) { + if (mcast.listen->filter_mode != PICO_IP_MULTICAST_EXCLUDE) { + so_mcast_dbg("pico_socket_setoption: ERROR any-source multicast (exclude) on source-specific multicast (include)\n"); + } else { + so_mcast_dbg("pico_socket_setoption: ERROR duplicate PICO_IP_ADD_MEMBERSHIP\n"); + } + + pico_err = PICO_ERR_EINVAL; + return -1; + } + + mcast.listen = PICO_ZALLOC(sizeof(struct pico_mcast_listen)); + if (!mcast.listen) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + mcast.listen->filter_mode = PICO_IP_MULTICAST_EXCLUDE; + mcast.listen->mcast_link = mcast.mreq->mcast_link_addr; + mcast.listen->mcast_group = mcast.mreq->mcast_group_addr; + mcast.listen->proto = s->net->proto_number; + + tree = mcast_get_src_tree(s, &mcast); + listen_tree = mcast_get_listen_tree(s); +#ifdef PICO_SUPPORT_IPV6 + if( IS_SOCK_IPV6(s)) + mcast.listen->proto = PICO_PROTO_IPV6; + +#endif + tree->root = &LEAF; + if (pico_tree_insert(listen_tree, mcast.listen)) { + PICO_FREE(mcast.listen); + return -1; + } + + if (pico_tree_insert(&MCASTSockets, s) == &LEAF) { + pico_tree_delete(listen_tree, mcast.listen); + PICO_FREE(mcast.listen); + return -1; + } + + filter_mode = pico_socket_aggregate_mcastfilters(mcast.address, &mcast.mreq->mcast_group_addr); + if (filter_mode < 0) + return -1; + + so_mcast_dbg("PICO_IP_ADD_MEMBERSHIP - success, added %p\n", s); + if(IS_SOCK_IPV4(s)) + return pico_ipv4_mcast_join((struct pico_ip4*)&mcast.mreq->mcast_link_addr, (struct pico_ip4*) &mcast.mreq->mcast_group_addr, 1, (uint8_t)filter_mode, &MCASTFilter); + +#ifdef PICO_SUPPORT_IPV6 + else if(IS_SOCK_IPV6(s)) { + return pico_ipv6_mcast_join((struct pico_ip6*)&mcast.mreq->mcast_link_addr, (struct pico_ip6*)&mcast.mreq->mcast_group_addr, 1, (uint8_t)filter_mode, &MCASTFilter_ipv6); + } +#endif + return -1; +} + +static int mcast_so_dropm(struct pico_socket *s, void *value) +{ + int filter_mode = 0; + union pico_address *source = NULL; + struct pico_tree_node *_tmp, *index; + struct pico_mcast mcast; + struct pico_tree *listen_tree, *tree; + if(mcast_get_param(&mcast, s, value, 0, 0) < 0) + return -1; + + if (!mcast.listen) { + so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_DROP_MEMBERSHIP before PICO_IP_ADD_MEMBERSHIP/SOURCE_MEMBERSHIP\n"); + pico_err = PICO_ERR_EADDRNOTAVAIL; + return -1; + } + + tree = mcast_get_src_tree(s, &mcast); + listen_tree = mcast_get_listen_tree(s); + + pico_tree_foreach_safe(index, tree, _tmp) + { + source = index->keyValue; + pico_tree_delete(tree, source); + } + pico_tree_delete(listen_tree, mcast.listen); + PICO_FREE(mcast.listen); + if (pico_tree_empty(listen_tree)) { + PICO_FREE(listen_tree); + mcast_set_listen_tree_p_null(s); + pico_tree_delete(&MCASTSockets, s); + } + + filter_mode = pico_socket_aggregate_mcastfilters(mcast.address, &mcast.mreq->mcast_group_addr); + if (filter_mode < 0) + return -1; + + if(IS_SOCK_IPV4(s)) + return pico_ipv4_mcast_leave((struct pico_ip4*) &mcast.mreq->mcast_link_addr, (struct pico_ip4 *) &mcast.mreq->mcast_group_addr, 1, (uint8_t)filter_mode, &MCASTFilter); + +#ifdef PICO_SUPPORT_IPV6 + else if(IS_SOCK_IPV6(s)) { } + return pico_ipv6_mcast_leave((struct pico_ip6*)&mcast.mreq->mcast_link_addr, (struct pico_ip6*)&mcast.mreq->mcast_group_addr, 1, (uint8_t)filter_mode, &MCASTFilter_ipv6); +#endif + return -1; +} + +static int mcast_so_unblock_src(struct pico_socket *s, void *value) +{ + int filter_mode = 0; + union pico_address stest, *source = NULL; + struct pico_mcast mcast; + if(mcast_get_param(&mcast, s, value, 0, 1) < 0) + return -1; + + memset(&stest, 0, sizeof(union pico_address)); + if (!mcast.listen) { + so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_UNBLOCK_SOURCE before PICO_IP_ADD_MEMBERSHIP\n"); + pico_err = PICO_ERR_EINVAL; + return -1; + } + + if (mcast.listen->filter_mode != PICO_IP_MULTICAST_EXCLUDE) { + so_mcast_dbg("pico_socket_setoption: ERROR any-source multicast (exclude) on source-specific multicast (include)\n"); + pico_err = PICO_ERR_EINVAL; + return -1; + } + + stest = mcast.mreq_s->mcast_source_addr; + if( IS_SOCK_IPV4(s)) + source = pico_tree_findKey(&mcast.listen->MCASTSources, &stest); + +#ifdef PICO_SUPPORT_IPV6 + else if( IS_SOCK_IPV6(s)) + source = pico_tree_findKey(&mcast.listen->MCASTSources_ipv6, &stest); +#endif + if (!source) { + so_mcast_dbg("pico_socket_setoption: ERROR address to unblock not in source list\n"); + pico_err = PICO_ERR_EADDRNOTAVAIL; + return -1; + } + + if( IS_SOCK_IPV4(s)) + pico_tree_delete(&mcast.listen->MCASTSources, source); + +#ifdef PICO_SUPPORT_IPV6 + else if( IS_SOCK_IPV6(s)) + pico_tree_delete(&mcast.listen->MCASTSources_ipv6, source); +#endif + + filter_mode = pico_socket_aggregate_mcastfilters(mcast.address, &mcast.mreq_s->mcast_group_addr); + if (filter_mode < 0) + return -1; + + if(IS_SOCK_IPV4(s)) + return pico_ipv4_mcast_leave((struct pico_ip4 *)&mcast.mreq_s->mcast_link_addr, (struct pico_ip4*) &mcast.mreq_s->mcast_group_addr, 0, (uint8_t)filter_mode, &MCASTFilter); + +#ifdef PICO_SUPPORT_IPV6 + else if(IS_SOCK_IPV6(s)) { } + return pico_ipv6_mcast_leave((struct pico_ip6*)&mcast.mreq_s->mcast_link_addr, (struct pico_ip6*)&mcast.mreq_s->mcast_group_addr, 0, (uint8_t)filter_mode, &MCASTFilter_ipv6); +#endif + return -1; +} + +static int mcast_so_block_src(struct pico_socket *s, void *value) +{ + int filter_mode = 0; + union pico_address stest, *source = NULL; + struct pico_mcast mcast; + if(mcast_get_param(&mcast, s, value, 0, 1) < 0) + return -1; + + memset(&stest, 0, sizeof(union pico_address)); + if (!mcast.listen) { + dbg("pico_socket_setoption: ERROR PICO_IP_BLOCK_SOURCE before PICO_IP_ADD_MEMBERSHIP\n"); + pico_err = PICO_ERR_EINVAL; + return -1; + } + + if (mcast.listen->filter_mode != PICO_IP_MULTICAST_EXCLUDE) { + so_mcast_dbg("pico_socket_setoption: ERROR any-source multicast (exclude) on source-specific multicast (include)\n"); + pico_err = PICO_ERR_EINVAL; + return -1; + } + + stest = mcast.mreq_s->mcast_source_addr; + if( IS_SOCK_IPV4(s)) + source = pico_tree_findKey(&mcast.listen->MCASTSources, &stest); + +#ifdef PICO_SUPPORT_IPV6 + else if( IS_SOCK_IPV6(s)) + source = pico_tree_findKey(&mcast.listen->MCASTSources_ipv6, &stest); +#endif + if (source) { + so_mcast_dbg("pico_socket_setoption: ERROR address to block already in source list\n"); + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + source = PICO_ZALLOC(sizeof(union pico_address)); + if (!source) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + *source = mcast.mreq_s->mcast_source_addr; + if( IS_SOCK_IPV4(s)) { + if (pico_tree_insert(&mcast.listen->MCASTSources, source)) { + PICO_FREE(source); + return -1; + } + } + +#ifdef PICO_SUPPORT_IPV6 + else if( IS_SOCK_IPV6(s)) + if (pico_tree_insert(&mcast.listen->MCASTSources_ipv6, source)) { + PICO_FREE(source); + return -1; + } +#endif + + filter_mode = pico_socket_aggregate_mcastfilters(mcast.address, &mcast.mreq_s->mcast_group_addr); + if (filter_mode < 0) + return -1; + + if(IS_SOCK_IPV4(s)) + return pico_ipv4_mcast_join((struct pico_ip4 *) &mcast.mreq_s->mcast_link_addr, (struct pico_ip4*)&mcast.mreq_s->mcast_group_addr, 0, (uint8_t)filter_mode, &MCASTFilter); + +#ifdef PICO_SUPPORT_IPV6 + else if(IS_SOCK_IPV6(s)) { } + return pico_ipv6_mcast_join((struct pico_ip6 *)&mcast.mreq_s->mcast_link_addr, (struct pico_ip6*)&mcast.mreq_s->mcast_group_addr, 0, (uint8_t)filter_mode, &MCASTFilter_ipv6); +#endif + return -1; +} + +static int mcast_so_addsrcm(struct pico_socket *s, void *value) +{ + int filter_mode = 0, reference_count = 0; + union pico_address stest, *source = NULL; + struct pico_mcast mcast; + struct pico_tree *tree, *listen_tree; + if(mcast_get_param(&mcast, s, value, 1, 1) < 0) + return -1; + + memset(&stest, 0, sizeof(union pico_address)); + listen_tree = mcast_get_listen_tree(s); + if (mcast.listen) { + tree = mcast_get_src_tree(s, &mcast); + if (mcast.listen->filter_mode != PICO_IP_MULTICAST_INCLUDE) { + so_mcast_dbg("pico_socket_setoption: ERROR source-specific multicast (include) on any-source multicast (exclude)\n"); + pico_err = PICO_ERR_EINVAL; + return -1; + } + + stest = mcast.mreq_s->mcast_source_addr; + source = pico_tree_findKey(tree, &stest); + if (source) { + so_mcast_dbg("pico_socket_setoption: ERROR source address to allow already in source list\n"); + pico_err = PICO_ERR_EADDRNOTAVAIL; + return -1; + } + + source = PICO_ZALLOC(sizeof(union pico_address)); + if (!source) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + *source = mcast.mreq_s->mcast_source_addr; + if (pico_tree_insert(tree, source)) { + PICO_FREE(source); + return -1; + } + + } else { + mcast.listen = PICO_ZALLOC(sizeof(struct pico_mcast_listen)); + if (!mcast.listen) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + tree = mcast_get_src_tree(s, &mcast); + mcast.listen->filter_mode = PICO_IP_MULTICAST_INCLUDE; + mcast.listen->mcast_link = mcast.mreq_s->mcast_link_addr; + mcast.listen->mcast_group = mcast.mreq_s->mcast_group_addr; + tree->root = &LEAF; + source = PICO_ZALLOC(sizeof(union pico_address)); + if (!source) { + PICO_FREE(mcast.listen); + pico_err = PICO_ERR_ENOMEM; + return -1; + } + +#ifdef PICO_SUPPORT_IPV6 + if( IS_SOCK_IPV6(s)) + mcast.listen->proto = PICO_PROTO_IPV6; + +#endif + *source = mcast.mreq_s->mcast_source_addr; + if (pico_tree_insert(tree, source)) { + PICO_FREE(mcast.listen); + PICO_FREE(source); + return -1; + } + + if (pico_tree_insert(listen_tree, mcast.listen)) { + pico_tree_delete(tree, source); + PICO_FREE(source); + PICO_FREE(mcast.listen); + return -1; + } + reference_count = 1; + } + + if (pico_tree_insert(&MCASTSockets, s) == &LEAF) { + return -1; + } + + filter_mode = pico_socket_aggregate_mcastfilters(mcast.address, &mcast.mreq_s->mcast_group_addr); + if (filter_mode < 0) + return -1; + + if(IS_SOCK_IPV4(s)) + return pico_ipv4_mcast_join((struct pico_ip4 *)&mcast.mreq_s->mcast_link_addr, (struct pico_ip4*)&mcast.mreq_s->mcast_group_addr, (uint8_t)reference_count, (uint8_t)filter_mode, &MCASTFilter); + +#ifdef PICO_SUPPORT_IPV6 + else if(IS_SOCK_IPV6(s)) { } + return pico_ipv6_mcast_join((struct pico_ip6 *) &mcast.mreq_s->mcast_link_addr, (struct pico_ip6*)&mcast.mreq_s->mcast_group_addr, (uint8_t)reference_count, (uint8_t)filter_mode, &MCASTFilter_ipv6); +#endif + return -1; +} + +static int mcast_so_dropsrcm(struct pico_socket *s, void *value) +{ + int filter_mode = 0, reference_count = 0; + union pico_address stest, *source = NULL; + struct pico_mcast mcast; + struct pico_tree *tree, *listen_tree; + if(mcast_get_param(&mcast, s, value, 0, 1) < 0) + return -1; + + memset(&stest, 0, sizeof(union pico_address)); + listen_tree = mcast_get_listen_tree(s); + if (!mcast.listen) { + so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_DROP_SOURCE_MEMBERSHIP before PICO_IP_ADD_SOURCE_MEMBERSHIP\n"); + pico_err = PICO_ERR_EADDRNOTAVAIL; + return -1; + } + + if (mcast.listen->filter_mode != PICO_IP_MULTICAST_INCLUDE) { + so_mcast_dbg("pico_socket_setoption: ERROR source-specific multicast (include) on any-source multicast (exclude)\n"); + pico_err = PICO_ERR_EINVAL; + return -1; + } + + tree = mcast_get_src_tree(s, &mcast); + stest = mcast.mreq_s->mcast_source_addr; + source = pico_tree_findKey(tree, &stest); + if (!source) { + so_mcast_dbg("pico_socket_setoption: ERROR address to drop not in source list\n"); + pico_err = PICO_ERR_EADDRNOTAVAIL; + return -1; + } + + pico_tree_delete(tree, source); + if (pico_tree_empty(tree)) { /* 1 if empty, 0 otherwise */ + reference_count = 1; + pico_tree_delete(listen_tree, mcast.listen); + PICO_FREE(mcast.listen); + if (pico_tree_empty(listen_tree)) { + PICO_FREE(listen_tree); + mcast_set_listen_tree_p_null(s); + pico_tree_delete(&MCASTSockets, s); + } + } + + filter_mode = pico_socket_aggregate_mcastfilters(mcast.address, &mcast.mreq_s->mcast_group_addr); + if (filter_mode < 0) + return -1; + + if(IS_SOCK_IPV4(s)) + return pico_ipv4_mcast_leave((struct pico_ip4 *) &mcast.mreq_s->mcast_link_addr, (struct pico_ip4*)&mcast.mreq_s->mcast_group_addr, (uint8_t)reference_count, (uint8_t)filter_mode, &MCASTFilter); + +#ifdef PICO_SUPPORT_IPV6 + else if(IS_SOCK_IPV6(s)) { } + return pico_ipv6_mcast_leave((struct pico_ip6 *)&mcast.mreq_s->mcast_link_addr, (struct pico_ip6*)&mcast.mreq_s->mcast_group_addr, (uint8_t)reference_count, (uint8_t)filter_mode, &MCASTFilter_ipv6); +#endif + return -1; +} + + +struct pico_setsockopt_mcast_call +{ + int option; + int (*call)(struct pico_socket *, void *); +}; + +static const struct pico_setsockopt_mcast_call mcast_so_calls[1 + PICO_IP_DROP_SOURCE_MEMBERSHIP - PICO_IP_MULTICAST_IF] = +{ + { PICO_IP_MULTICAST_IF, NULL }, + { PICO_IP_MULTICAST_TTL, pico_udp_set_mc_ttl }, + { PICO_IP_MULTICAST_LOOP, mcast_so_loop }, + { PICO_IP_ADD_MEMBERSHIP, mcast_so_addm }, + { PICO_IP_DROP_MEMBERSHIP, mcast_so_dropm }, + { PICO_IP_UNBLOCK_SOURCE, mcast_so_unblock_src }, + { PICO_IP_BLOCK_SOURCE, mcast_so_block_src }, + { PICO_IP_ADD_SOURCE_MEMBERSHIP, mcast_so_addsrcm }, + { PICO_IP_DROP_SOURCE_MEMBERSHIP, mcast_so_dropsrcm } +}; + + +static int mcast_so_check_socket(struct pico_socket *s) +{ + pico_err = PICO_ERR_EINVAL; + if (!s) + return -1; + + if (!s->proto) + return -1; + + if (s->proto->proto_number != PICO_PROTO_UDP) + return -1; + + pico_err = PICO_ERR_NOERR; + return 0; +} + +int pico_setsockopt_mcast(struct pico_socket *s, int option, void *value) +{ + int arrayn = option - PICO_IP_MULTICAST_IF; + if (option < PICO_IP_MULTICAST_IF || option > PICO_IP_DROP_SOURCE_MEMBERSHIP) { + pico_err = PICO_ERR_EOPNOTSUPP; + return -1; + } + + if (mcast_so_check_socket(s) < 0) + return -1; + + if (!mcast_so_calls[arrayn].call) { + pico_err = PICO_ERR_EOPNOTSUPP; + return -1; + } + + return (mcast_so_calls[arrayn].call(s, value)); +} + +int pico_udp_set_mc_ttl(struct pico_socket *s, void *_ttl) +{ + struct pico_socket_udp *u; + uint8_t ttl = *(uint8_t *)_ttl; + if(!s) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + u = (struct pico_socket_udp *) s; + u->mc_ttl = ttl; + return 0; +} + +int pico_udp_get_mc_ttl(struct pico_socket *s, uint8_t *ttl) +{ + struct pico_socket_udp *u; + if(!s) + return -1; + + u = (struct pico_socket_udp *) s; + *ttl = u->mc_ttl; + return 0; +} +#else +int pico_udp_set_mc_ttl(struct pico_socket *s, void *_ttl) +{ + IGNORE_PARAMETER(s); + IGNORE_PARAMETER(_ttl); + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; +} + +int pico_udp_get_mc_ttl(struct pico_socket *s, uint8_t *ttl) +{ + IGNORE_PARAMETER(s); + IGNORE_PARAMETER(ttl); + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; +} + +int pico_socket_mcast_filter(struct pico_socket *s, union pico_address *mcast_group, union pico_address *src) +{ + IGNORE_PARAMETER(s); + IGNORE_PARAMETER(mcast_group); + IGNORE_PARAMETER(src); + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; +} + +void pico_multicast_delete(struct pico_socket *s) +{ + (void)s; +} + +int pico_getsockopt_mcast(struct pico_socket *s, int option, void *value) +{ + (void)s; + (void)option; + (void)value; + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; +} + +int pico_setsockopt_mcast(struct pico_socket *s, int option, void *value) +{ + (void)s; + (void)option; + (void)value; + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; + +} +#endif /* PICO_SUPPORT_MCAST */ + diff --git a/core/deps/picotcp/stack/pico_stack.c b/core/deps/picotcp/stack/pico_stack.c new file mode 100644 index 000000000..007cd40bf --- /dev/null +++ b/core/deps/picotcp/stack/pico_stack.c @@ -0,0 +1,962 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2017 Altran Intelligent Systems. Some rights reserved. + See COPYING, LICENSE.GPLv2 and LICENSE.GPLv3 for usage. + + . + + Authors: Daniele Lacamera + *********************************************************************/ + + +#include "pico_config.h" +#include "pico_frame.h" +#include "pico_device.h" +#include "pico_protocol.h" +#include "pico_stack.h" +#include "pico_addressing.h" +#include "pico_dns_client.h" + +#include "pico_6lowpan_ll.h" +#include "pico_ethernet.h" +#include "pico_6lowpan.h" +#include "pico_olsr.h" +#include "pico_aodv.h" +#include "pico_eth.h" +#include "pico_arp.h" +#include "pico_ipv4.h" +#include "pico_ipv6.h" +#include "pico_icmp4.h" +#include "pico_icmp6.h" +#include "pico_igmp.h" +#include "pico_udp.h" +#include "pico_tcp.h" +#include "pico_socket.h" +#include "heap.h" + +/* Mockables */ +#if defined UNIT_TEST +# define MOCKABLE __attribute__((weak)) +#else +# define MOCKABLE +#endif + + +volatile pico_time pico_tick; +volatile pico_err_t pico_err; + +static uint32_t _rand_seed; + +void WEAK pico_rand_feed(uint32_t feed) +{ + if (!feed) + return; + + _rand_seed *= 1664525; + _rand_seed += 1013904223; + _rand_seed ^= ~(feed); +} + +uint32_t WEAK pico_rand(void) +{ + pico_rand_feed((uint32_t)pico_tick); + return _rand_seed; +} + +void pico_to_lowercase(char *str) +{ + int i = 0; + if (!str) + return; + + while(str[i]) { + if ((str[i] <= 'Z') && (str[i] >= 'A')) + str[i] = (char) (str[i] - (char)('A' - 'a')); + + i++; + } +} + +/* NOTIFICATIONS: distributed notifications for stack internal errors. + */ + +int pico_notify_socket_unreachable(struct pico_frame *f) +{ + if (0) {} + +#ifdef PICO_SUPPORT_ICMP4 + else if (IS_IPV4(f)) { + pico_icmp4_port_unreachable(f); + } +#endif +#ifdef PICO_SUPPORT_ICMP6 + else if (IS_IPV6(f)) { + pico_icmp6_port_unreachable(f); + } +#endif + + return 0; +} + +int pico_notify_proto_unreachable(struct pico_frame *f) +{ + if (0) {} + +#ifdef PICO_SUPPORT_ICMP4 + else if (IS_IPV4(f)) { + pico_icmp4_proto_unreachable(f); + } +#endif +#ifdef PICO_SUPPORT_ICMP6 + else if (IS_IPV6(f)) { + pico_icmp6_proto_unreachable(f); + } +#endif + return 0; +} + +int pico_notify_dest_unreachable(struct pico_frame *f) +{ + if (0) {} + +#ifdef PICO_SUPPORT_ICMP4 + else if (IS_IPV4(f)) { + pico_icmp4_dest_unreachable(f); + } +#endif +#ifdef PICO_SUPPORT_ICMP6 + else if (IS_IPV6(f)) { + pico_icmp6_dest_unreachable(f); + } +#endif + return 0; +} + +int pico_notify_ttl_expired(struct pico_frame *f) +{ + if (0) {} + +#ifdef PICO_SUPPORT_ICMP4 + else if (IS_IPV4(f)) { + pico_icmp4_ttl_expired(f); + } +#endif +#ifdef PICO_SUPPORT_ICMP6 + else if (IS_IPV6(f)) { + pico_icmp6_ttl_expired(f); + } +#endif + return 0; +} + +int pico_notify_frag_expired(struct pico_frame *f) +{ + if (0) {} + +#ifdef PICO_SUPPORT_ICMP4 + else if (IS_IPV4(f)) { + pico_icmp4_frag_expired(f); + } +#endif +#ifdef PICO_SUPPORT_ICMP6 + else if (IS_IPV6(f)) { + pico_icmp6_frag_expired(f); + } +#endif + return 0; +} + +int pico_notify_pkt_too_big(struct pico_frame *f) +{ + if (0) {} + +#ifdef PICO_SUPPORT_ICMP4 + else if (IS_IPV4(f)) { + pico_icmp4_mtu_exceeded(f); + } +#endif +#ifdef PICO_SUPPORT_ICMP6 + else if (IS_IPV6(f)) { + pico_icmp6_pkt_too_big(f); + } +#endif + return 0; +} + +/******************************************************************************* + * TRANSPORT LAYER + ******************************************************************************/ + +MOCKABLE int32_t pico_transport_receive(struct pico_frame *f, uint8_t proto) +{ + int32_t ret = -1; + switch (proto) { + +#ifdef PICO_SUPPORT_ICMP4 + case PICO_PROTO_ICMP4: + ret = pico_enqueue(pico_proto_icmp4.q_in, f); + break; +#endif + +#ifdef PICO_SUPPORT_ICMP6 + case PICO_PROTO_ICMP6: + ret = pico_enqueue(pico_proto_icmp6.q_in, f); + break; +#endif + + +#if defined(PICO_SUPPORT_IGMP) && defined(PICO_SUPPORT_MCAST) + case PICO_PROTO_IGMP: + ret = pico_enqueue(pico_proto_igmp.q_in, f); + break; +#endif + +#ifdef PICO_SUPPORT_UDP + case PICO_PROTO_UDP: + ret = pico_enqueue(pico_proto_udp.q_in, f); + break; +#endif + +#ifdef PICO_SUPPORT_TCP + case PICO_PROTO_TCP: + ret = pico_enqueue(pico_proto_tcp.q_in, f); + break; +#endif + + default: + /* Protocol not available */ + dbg("pkt: no such protocol (%d)\n", proto); + pico_notify_proto_unreachable(f); + pico_frame_discard(f); + ret = -1; + } + return ret; +} + +/******************************************************************************* + * NETWORK LAYER + ******************************************************************************/ + +MOCKABLE int32_t pico_network_receive(struct pico_frame *f) +{ + if (0) {} + +#ifdef PICO_SUPPORT_IPV4 + else if (IS_IPV4(f)) { + pico_enqueue(pico_proto_ipv4.q_in, f); + } +#endif +#ifdef PICO_SUPPORT_IPV6 + else if (IS_IPV6(f)) { + pico_enqueue(pico_proto_ipv6.q_in, f); + } +#endif + else { + dbg("Network not found.\n"); + pico_frame_discard(f); + return -1; + } + return (int32_t)f->buffer_len; +} + +/// Interface towards socket for frame sending +int32_t pico_network_send(struct pico_frame *f) +{ + if (!f || !f->sock || !f->sock->net) { + pico_frame_discard(f); + return -1; + } + + return f->sock->net->push(f->sock->net, f); +} + +int pico_source_is_local(struct pico_frame *f) +{ + if (0) { } + +#ifdef PICO_SUPPORT_IPV4 + else if (IS_IPV4(f)) { + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr; + if (hdr->src.addr == PICO_IPV4_INADDR_ANY) + return 1; + + if (pico_ipv4_link_find(&hdr->src)) + return 1; + } +#endif +#ifdef PICO_SUPPORT_IPV6 + else if (IS_IPV6(f)) { + struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr; + if (pico_ipv6_is_unspecified(hdr->src.addr) || pico_ipv6_link_find(&hdr->src)) + return 1; + } +#endif + return 0; +} + +void pico_store_network_origin(void *src, struct pico_frame *f) +{ + #ifdef PICO_SUPPORT_IPV4 + struct pico_ip4 *ip4; + #endif + + #ifdef PICO_SUPPORT_IPV6 + struct pico_ip6 *ip6; + #endif + + #ifdef PICO_SUPPORT_IPV4 + if (IS_IPV4(f)) { + struct pico_ipv4_hdr *hdr; + hdr = (struct pico_ipv4_hdr *) f->net_hdr; + ip4 = (struct pico_ip4 *) src; + ip4->addr = hdr->src.addr; + } + + #endif + #ifdef PICO_SUPPORT_IPV6 + if (IS_IPV6(f)) { + struct pico_ipv6_hdr *hdr; + hdr = (struct pico_ipv6_hdr *) f->net_hdr; + ip6 = (struct pico_ip6 *) src; + memcpy(ip6->addr, hdr->src.addr, PICO_SIZE_IP6); + } + + #endif +} + +int pico_address_compare(union pico_address *a, union pico_address *b, uint16_t proto) +{ + #ifdef PICO_SUPPORT_IPV6 + if (proto == PICO_PROTO_IPV6) { + return pico_ipv6_compare(&a->ip6, &b->ip6); + } + + #endif + #ifdef PICO_SUPPORT_IPV4 + if (proto == PICO_PROTO_IPV4) { + return pico_ipv4_compare(&a->ip4, &b->ip4); + } + + #endif + return 0; + +} + +int pico_frame_dst_is_unicast(struct pico_frame *f) +{ + if (0) { + return 0; + } + +#ifdef PICO_SUPPORT_IPV4 + if (IS_IPV4(f)) { + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr; + if (pico_ipv4_is_multicast(hdr->dst.addr) || pico_ipv4_is_broadcast(hdr->dst.addr)) + return 0; + + return 1; + } + +#endif + +#ifdef PICO_SUPPORT_IPV6 + if (IS_IPV6(f)) { + struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr; + if (pico_ipv6_is_multicast(hdr->dst.addr) || pico_ipv6_is_unspecified(hdr->dst.addr)) + return 0; + + return 1; + } + +#endif + else return 0; +} + +/******************************************************************************* + * DATALINK LAYER + ******************************************************************************/ + +int pico_datalink_receive(struct pico_frame *f) +{ + if (f->dev->eth) { + /* If device has stack with datalink-layer pass frame through it */ + switch (f->dev->mode) { + #ifdef PICO_SUPPORT_802154 + case LL_MODE_IEEE802154: + f->datalink_hdr = f->buffer; + return pico_enqueue(pico_proto_6lowpan_ll.q_in, f); + #endif + default: + #ifdef PICO_SUPPORT_ETH + f->datalink_hdr = f->buffer; + return pico_enqueue(pico_proto_ethernet.q_in,f); + #else + return -1; + #endif + } + } else { + /* If device handles raw IP-frames send it straight to network-layer */ + f->net_hdr = f->buffer; + pico_network_receive(f); + } + + return 0; +} + +MOCKABLE int pico_datalink_send(struct pico_frame *f) +{ + if (f->dev->eth) { + switch (f->dev->mode) { + #ifdef PICO_SUPPORT_802154 + case LL_MODE_IEEE802154: + return pico_enqueue(pico_proto_6lowpan.q_out, f); + #endif + default: + #ifdef PICO_SUPPORT_ETH + return pico_enqueue(pico_proto_ethernet.q_out, f); + #else + return -1; + #endif + } + } else { + /* non-ethernet: no post-processing needed */ + return pico_sendto_dev(f); + } +} + +/******************************************************************************* + * PHYSICAL LAYER + ******************************************************************************/ + +struct pico_frame *pico_stack_recv_new_frame(struct pico_device *dev, uint8_t *buffer, uint32_t len) +{ + struct pico_frame *f; + if (len == 0) + return NULL; + + f = pico_frame_alloc(len); + if (!f) + { + dbg("Cannot alloc incoming frame!\n"); + return NULL; + } + + /* Association to the device that just received the frame. */ + f->dev = dev; + + /* Setup the start pointer, length. */ + f->start = f->buffer; + f->len = f->buffer_len; + if (f->len > 8) { + uint32_t rand, mid_frame = (f->buffer_len >> 2) << 1; + mid_frame -= (mid_frame % 4); + memcpy(&rand, f->buffer + mid_frame, sizeof(uint32_t)); + pico_rand_feed(rand); + } + + memcpy(f->buffer, buffer, len); + return f; +} + +/* LOWEST LEVEL: interface towards devices. */ +/* Device driver will call this function which returns immediately. + * Incoming packet will be processed later on in the dev loop. + */ +int32_t pico_stack_recv(struct pico_device *dev, uint8_t *buffer, uint32_t len) +{ + struct pico_frame *f = pico_stack_recv_new_frame (dev, buffer, len); + int32_t ret; + + if (!f) + return -1; + + ret = pico_enqueue(dev->q_in, f); + if (ret <= 0) { + pico_frame_discard(f); + } + return ret; +} + +static int32_t _pico_stack_recv_zerocopy(struct pico_device *dev, uint8_t *buffer, uint32_t len, int ext_buffer, void (*notify_free)(uint8_t *)) +{ + struct pico_frame *f; + int ret; + if (len == 0) + return -1; + + f = pico_frame_alloc_skeleton(len, ext_buffer); + if (!f) + { + dbg("Cannot alloc incoming frame!\n"); + return -1; + } + + if (pico_frame_skeleton_set_buffer(f, buffer) < 0) + { + dbg("Invalid zero-copy buffer!\n"); + PICO_FREE(f->usage_count); + PICO_FREE(f); + return -1; + } + + if (notify_free) { + f->notify_free = notify_free; + } + + f->dev = dev; + ret = pico_enqueue(dev->q_in, f); + if (ret <= 0) { + pico_frame_discard(f); + } + + return ret; +} + +int32_t pico_stack_recv_zerocopy(struct pico_device *dev, uint8_t *buffer, uint32_t len) +{ + return _pico_stack_recv_zerocopy(dev, buffer, len, 0, NULL); +} + +int32_t pico_stack_recv_zerocopy_ext_buffer(struct pico_device *dev, uint8_t *buffer, uint32_t len) +{ + return _pico_stack_recv_zerocopy(dev, buffer, len, 1, NULL); +} + +int32_t pico_stack_recv_zerocopy_ext_buffer_notify(struct pico_device *dev, uint8_t *buffer, uint32_t len, void (*notify_free)(uint8_t *buffer)) +{ + return _pico_stack_recv_zerocopy(dev, buffer, len, 1, notify_free); +} + +int32_t pico_sendto_dev(struct pico_frame *f) +{ + if (!f->dev) { + pico_frame_discard(f); + return -1; + } else { + if (f->len > 8) { + uint32_t rand, mid_frame = (f->buffer_len >> 2) << 1; + mid_frame -= (mid_frame % 4); + memcpy(&rand, f->buffer + mid_frame, sizeof(uint32_t)); + pico_rand_feed(rand); + } + + return pico_enqueue(f->dev->q_out, f); + } +} + +struct pico_timer +{ + void *arg; + void (*timer)(pico_time timestamp, void *arg); +}; + + +static uint32_t tmr_id = 0u; +struct pico_timer_ref +{ + pico_time expire; + uint32_t id; + uint32_t hash; + struct pico_timer *tmr; +}; + +typedef struct pico_timer_ref pico_timer_ref; + +DECLARE_HEAP(pico_timer_ref, expire); + +static heap_pico_timer_ref *Timers; + +int32_t pico_seq_compare(uint32_t a, uint32_t b) +{ + uint32_t thresh = ((uint32_t)(-1)) >> 1; + + if (a > b) /* return positive number, if not wrapped */ + { + if ((a - b) > thresh) /* b wrapped */ + return -(int32_t)(b - a); /* b = very small, a = very big */ + else + return (int32_t)(a - b); /* a = biggest, b = a bit smaller */ + + } + + if (a < b) /* return negative number, if not wrapped */ + { + if ((b - a) > thresh) /* a wrapped */ + return (int32_t)(a - b); /* a = very small, b = very big */ + else + return -(int32_t)(b - a); /* b = biggest, a = a bit smaller */ + + } + + return 0; +} + +static void pico_check_timers(void) +{ + struct pico_timer *t; + struct pico_timer_ref tref_unused, *tref = heap_first(Timers); + pico_tick = PICO_TIME_MS(); + while((tref) && (tref->expire < pico_tick)) { + t = tref->tmr; + if (t && t->timer) + t->timer(pico_tick, t->arg); + + if (t) + { + PICO_FREE(t); + } + + heap_peek(Timers, &tref_unused); + tref = heap_first(Timers); + } +} + +void MOCKABLE pico_timer_cancel(uint32_t id) +{ + uint32_t i; + struct pico_timer_ref *tref; + if (id == 0u) + return; + + for (i = 1; i <= Timers->n; i++) { + tref = heap_get_element(Timers, i); + if (tref->id == id) { + if (tref->tmr) + { + PICO_FREE(tref->tmr); + tref->tmr = NULL; + tref->id = 0; + } + break; + } + } +} + +void pico_timer_cancel_hashed(uint32_t hash) +{ + uint32_t i; + struct pico_timer_ref *tref; + if (hash == 0u) + return; + + for (i = 1; i <= Timers->n; i++) { + tref = heap_get_element(Timers, i); + if (tref->hash == hash) { + if (tref->tmr) + { + PICO_FREE(tref->tmr); + tref->tmr = NULL; + tref[i].id = 0; + } + } + } +} + +#define PROTO_DEF_NR 11 +#define PROTO_DEF_AVG_NR 4 +#define PROTO_DEF_SCORE 32 +#define PROTO_MIN_SCORE 32 +#define PROTO_MAX_SCORE 128 +#define PROTO_LAT_IND 3 /* latency indication 0-3 (lower is better latency performance), x1, x2, x4, x8 */ +#define PROTO_MAX_LOOP (PROTO_MAX_SCORE << PROTO_LAT_IND) /* max global loop score, so per tick */ + +static int calc_score(int *score, int *index, int avg[][PROTO_DEF_AVG_NR], int *ret) +{ + int temp, i, j, sum; + int max_total = PROTO_MAX_LOOP, total = 0; + + /* dbg("USED SCORES> "); */ + + for (i = 0; i < PROTO_DEF_NR; i++) { + + /* if used looped score */ + if (ret[i] < score[i]) { + temp = score[i] - ret[i]; /* remaining loop score */ + + /* dbg("%3d - ",temp); */ + + if (index[i] >= PROTO_DEF_AVG_NR) + index[i] = 0; /* reset index */ + + j = index[i]; + avg[i][j] = temp; + + index[i]++; + + if (ret[i] == 0 && ((score[i] * 2) <= PROTO_MAX_SCORE) && ((total + (score[i] * 2)) < max_total)) { /* used all loop score -> increase next score directly */ + score[i] *= 2; + total += score[i]; + continue; + } + + sum = 0; + for (j = 0; j < PROTO_DEF_AVG_NR; j++) + sum += avg[i][j]; /* calculate sum */ + + sum /= 4; /* divide by 4 to get average used score */ + + /* criterion to increase next loop score */ + if (sum > (score[i] - (score[i] / 4)) && ((score[i] * 2) <= PROTO_MAX_SCORE) && ((total + (score[i] / 2)) < max_total)) { /* > 3/4 */ + score[i] *= 2; /* double loop score */ + total += score[i]; + continue; + } + + /* criterion to decrease next loop score */ + if ((sum < (score[i] / 4)) && ((score[i] / 2) >= PROTO_MIN_SCORE)) { /* < 1/4 */ + score[i] /= 2; /* half loop score */ + total += score[i]; + continue; + } + + /* also add non-changed scores */ + total += score[i]; + } + else if (ret[i] == score[i]) { + /* no used loop score - gradually decrease */ + + /* dbg("%3d - ",0); */ + + if (index[i] >= PROTO_DEF_AVG_NR) + index[i] = 0; /* reset index */ + + j = index[i]; + avg[i][j] = 0; + + index[i]++; + + sum = 0; + for (j = 0; j < PROTO_DEF_AVG_NR; j++) + sum += avg[i][j]; /* calculate sum */ + + sum /= 2; /* divide by 4 to get average used score */ + + if ((sum == 0) && ((score[i] / 2) >= PROTO_MIN_SCORE)) { + score[i] /= 2; /* half loop score */ + total += score[i]; + for (j = 0; j < PROTO_DEF_AVG_NR; j++) + avg[i][j] = score[i]; + } + + } + } + /* dbg("\n"); */ + + return 0; +} + +void pico_stack_tick(void) +{ + static int score[PROTO_DEF_NR] = { + PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE + }; + static int index[PROTO_DEF_NR] = { + 0, 0, 0, 0, 0, 0 + }; + static int avg[PROTO_DEF_NR][PROTO_DEF_AVG_NR]; + static int ret[PROTO_DEF_NR] = { + 0 + }; + + pico_check_timers(); + + /* dbg("LOOP_SCORES> %3d - %3d - %3d - %3d - %3d - %3d - %3d - %3d - %3d - %3d - %3d\n",score[0],score[1],score[2],score[3],score[4],score[5],score[6],score[7],score[8],score[9],score[10]); */ + + /* score = pico_protocols_loop(100); */ + + ret[0] = pico_devices_loop(score[0], PICO_LOOP_DIR_IN); + pico_rand_feed((uint32_t)ret[0]); + + ret[1] = pico_protocol_datalink_loop(score[1], PICO_LOOP_DIR_IN); + pico_rand_feed((uint32_t)ret[1]); + + ret[2] = pico_protocol_network_loop(score[2], PICO_LOOP_DIR_IN); + pico_rand_feed((uint32_t)ret[2]); + + ret[3] = pico_protocol_transport_loop(score[3], PICO_LOOP_DIR_IN); + pico_rand_feed((uint32_t)ret[3]); + + + ret[5] = score[5]; +#if defined (PICO_SUPPORT_IPV4) || defined (PICO_SUPPORT_IPV6) +#if defined (PICO_SUPPORT_TCP) || defined (PICO_SUPPORT_UDP) + ret[5] = pico_sockets_loop(score[5]); /* swapped */ + pico_rand_feed((uint32_t)ret[5]); +#endif +#endif + + ret[4] = pico_protocol_socket_loop(score[4], PICO_LOOP_DIR_IN); + pico_rand_feed((uint32_t)ret[4]); + + + ret[6] = pico_protocol_socket_loop(score[6], PICO_LOOP_DIR_OUT); + pico_rand_feed((uint32_t)ret[6]); + + ret[7] = pico_protocol_transport_loop(score[7], PICO_LOOP_DIR_OUT); + pico_rand_feed((uint32_t)ret[7]); + + ret[8] = pico_protocol_network_loop(score[8], PICO_LOOP_DIR_OUT); + pico_rand_feed((uint32_t)ret[8]); + + ret[9] = pico_protocol_datalink_loop(score[9], PICO_LOOP_DIR_OUT); + pico_rand_feed((uint32_t)ret[9]); + + ret[10] = pico_devices_loop(score[10], PICO_LOOP_DIR_OUT); + pico_rand_feed((uint32_t)ret[10]); + + /* calculate new loop scores for next iteration */ + calc_score(score, index, (int (*)[])avg, ret); +} + +void pico_stack_loop(void) +{ + while(1) { + pico_stack_tick(); + PICO_IDLE(); + } +} + +static uint32_t +pico_timer_ref_add(pico_time expire, struct pico_timer *t, uint32_t id, uint32_t hash) +{ + struct pico_timer_ref tref; + + tref.expire = PICO_TIME_MS() + expire; + tref.tmr = t; + tref.id = id; + tref.hash = hash; + + if (heap_insert(Timers, &tref) < 0) { + dbg("Error: failed to insert timer(ID %u) into heap\n", id); + PICO_FREE(t); + pico_err = PICO_ERR_ENOMEM; + return 0; + } + if (Timers->n > PICO_MAX_TIMERS) { + dbg("Warning: I have %d timers\n", (int)Timers->n); + } + + return tref.id; +} + +static struct pico_timer * +pico_timer_create(void (*timer)(pico_time, void *), void *arg) +{ + struct pico_timer *t = PICO_ZALLOC(sizeof(struct pico_timer)); + + if (!t) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + t->arg = arg; + t->timer = timer; + + return t; +} + +MOCKABLE uint32_t pico_timer_add(pico_time expire, void (*timer)(pico_time, void *), void *arg) +{ + struct pico_timer *t = pico_timer_create(timer, arg); + + /* zero is guard for timers */ + if (tmr_id == 0u) { + tmr_id++; + } + + if (!t) + return 0; + + return pico_timer_ref_add(expire, t, tmr_id++, 0); +} + +uint32_t pico_timer_add_hashed(pico_time expire, void (*timer)(pico_time, void *), void *arg, uint32_t hash) +{ + struct pico_timer *t = pico_timer_create(timer, arg); + + /* zero is guard for timers */ + if (tmr_id == 0u) { + tmr_id++; + } + + if (!t) + return 0; + + return pico_timer_ref_add(expire, t, tmr_id++, hash); +} /* Static path count: 4 */ + +int MOCKABLE pico_stack_init(void) +{ +#ifdef PICO_SUPPORT_ETH + pico_protocol_init(&pico_proto_ethernet); +#endif + +#ifdef PICO_SUPPORT_6LOWPAN + pico_protocol_init(&pico_proto_6lowpan); + pico_protocol_init(&pico_proto_6lowpan_ll); +#endif + +#ifdef PICO_SUPPORT_IPV4 + pico_protocol_init(&pico_proto_ipv4); +#endif + +#ifdef PICO_SUPPORT_IPV6 + pico_protocol_init(&pico_proto_ipv6); +#endif + +#ifdef PICO_SUPPORT_ICMP4 + pico_protocol_init(&pico_proto_icmp4); +#endif + +#ifdef PICO_SUPPORT_ICMP6 + pico_protocol_init(&pico_proto_icmp6); +#endif + +#if defined(PICO_SUPPORT_IGMP) && defined(PICO_SUPPORT_MCAST) + pico_protocol_init(&pico_proto_igmp); +#endif + +#ifdef PICO_SUPPORT_UDP + pico_protocol_init(&pico_proto_udp); +#endif + +#ifdef PICO_SUPPORT_TCP + pico_protocol_init(&pico_proto_tcp); +#endif + +#ifdef PICO_SUPPORT_DNS_CLIENT + pico_dns_client_init(); +#endif + + pico_rand_feed(123456); + + /* Initialize timer heap */ + Timers = heap_init(); + if (!Timers) + return -1; + +#if ((defined PICO_SUPPORT_IPV4) && (defined PICO_SUPPORT_ETH)) + /* Initialize ARP module */ + pico_arp_init(); +#endif + +#ifdef PICO_SUPPORT_IPV6 + /* Initialize Neighbor discovery module */ + pico_ipv6_nd_init(); +#endif + +#ifdef PICO_SUPPORT_OLSR + pico_olsr_init(); +#endif +#ifdef PICO_SUPPORT_AODV + pico_aodv_init(); +#endif +#ifdef PICO_SUPPORT_6LOWPAN + if (pico_6lowpan_init()) + return -1; +#endif + pico_stack_tick(); + pico_stack_tick(); + pico_stack_tick(); + return 0; +} + diff --git a/core/deps/picotcp/stack/pico_tree.c b/core/deps/picotcp/stack/pico_tree.c new file mode 100644 index 000000000..8955696dc --- /dev/null +++ b/core/deps/picotcp/stack/pico_tree.c @@ -0,0 +1,565 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2017 Altran Intelligent Systems. Some rights reserved. + See COPYING, LICENSE.GPLv2 and LICENSE.GPLv3 for usage. + + Author: Andrei Carp + *********************************************************************/ + +#include "pico_tree.h" +#include "pico_config.h" +#include "pico_protocol.h" +#include "pico_mm.h" + +#define RED 0 +#define BLACK 1 + +/* By default the null leafs are black */ +struct pico_tree_node LEAF = { + NULL, /* key */ + &LEAF, &LEAF, &LEAF, /* parent, left,right */ + BLACK, /* color */ +}; + +#define IS_LEAF(x) (x == &LEAF) +#define IS_NOT_LEAF(x) (x != &LEAF) +#define INIT_LEAF (&LEAF) + +#define AM_I_LEFT_CHILD(x) (x == x->parent->leftChild) +#define AM_I_RIGHT_CHILD(x) (x == x->parent->rightChild) + +#define PARENT(x) (x->parent) +#define GRANPA(x) (x->parent->parent) + +/* + * Local Functions + */ +static struct pico_tree_node *create_node(struct pico_tree *tree, void *key, uint8_t allocator); +static void rotateToLeft(struct pico_tree*tree, struct pico_tree_node*node); +static void rotateToRight(struct pico_tree*root, struct pico_tree_node*node); +static void fix_insert_collisions(struct pico_tree*tree, struct pico_tree_node*node); +static void fix_delete_collisions(struct pico_tree*tree, struct pico_tree_node *node); +static void switchNodes(struct pico_tree*tree, struct pico_tree_node*nodeA, struct pico_tree_node*nodeB); +void *pico_tree_insert_implementation(struct pico_tree *tree, void *key, uint8_t allocator); +void *pico_tree_delete_implementation(struct pico_tree *tree, void *key, uint8_t allocator); + +#ifdef PICO_SUPPORT_MM +/* The memory manager also uses the pico_tree to keep track of all the different slab sizes it has. + * These nodes should be placed in the manager page which is in a different memory region then the nodes + * which are used for the pico stack in general. + * Therefore the following 2 functions are created so that pico_tree can use them to to put these nodes + * into the correct memory regions. + * If pico_tree_insert is called from the memory manager module, then create_node should use + * pico_mem_page0_zalloc to create a node. The same for pico_tree_delete. + */ +extern void*pico_mem_page0_zalloc(size_t len); +extern void pico_mem_page0_free(void*ptr); +#endif /* PICO_SUPPORT_MM */ + +/* + * Exported functions + */ + +struct pico_tree_node *pico_tree_firstNode(struct pico_tree_node *node) +{ + while(IS_NOT_LEAF(node->leftChild)) + node = node->leftChild; + return node; +} + +struct pico_tree_node *pico_tree_lastNode(struct pico_tree_node *node) +{ + while(IS_NOT_LEAF(node->rightChild)) + node = node->rightChild; + return node; +} + +struct pico_tree_node *pico_tree_next(struct pico_tree_node *node) +{ + if (!node) + return NULL; + + if(IS_NOT_LEAF(node->rightChild)) + { + node = node->rightChild; + while(IS_NOT_LEAF(node->leftChild)) + node = node->leftChild; + } + else + { + if (IS_NOT_LEAF(node->parent) && AM_I_LEFT_CHILD(node)) + node = node->parent; + else { + while (IS_NOT_LEAF(node->parent) && AM_I_RIGHT_CHILD(node)) + node = node->parent; + node = node->parent; + } + } + + return node; +} + +struct pico_tree_node *pico_tree_prev(struct pico_tree_node *node) +{ + if (IS_NOT_LEAF(node->leftChild)) { + node = node->leftChild; + while (IS_NOT_LEAF(node->rightChild)) + node = node->rightChild; + } else { + if (IS_NOT_LEAF(node->parent) && AM_I_RIGHT_CHILD(node)) + node = node->parent; + else { + while (IS_NOT_LEAF(node) && AM_I_LEFT_CHILD(node)) + node = node->parent; + node = node->parent; + } + } + + return node; +} + +/* The memory manager also uses the pico_tree to keep track of all the different slab sizes it has. + * These nodes should be placed in the manager page which is in a different memory region then the nodes + * which are used for the pico stack in general. + * Therefore the following wrapper for pico_tree_insert is created. + * The actual implementation can be found in pico_tree_insert_implementation. + */ +void *pico_tree_insert(struct pico_tree *tree, void *key) +{ + return pico_tree_insert_implementation(tree, key, USE_PICO_ZALLOC); +} + +static void pico_tree_insert_node(struct pico_tree *tree, struct pico_tree_node *insert) +{ + struct pico_tree_node *temp = tree->root; + struct pico_tree_node *last_node = INIT_LEAF; + int result = 0; + + /* search for the place to insert the new node */ + while(IS_NOT_LEAF(temp)) + { + last_node = temp; + result = tree->compare(insert->keyValue, temp->keyValue); + + temp = (result < 0) ? (temp->leftChild) : (temp->rightChild); + } + /* make the needed connections */ + insert->parent = last_node; + + if(IS_LEAF(last_node)) + tree->root = insert; + else{ + result = tree->compare(insert->keyValue, last_node->keyValue); + if(result < 0) + last_node->leftChild = insert; + else + last_node->rightChild = insert; + } +} + +void *pico_tree_insert_implementation(struct pico_tree *tree, void *key, uint8_t allocator) +{ + struct pico_tree_node *insert; + void *LocalKey; + + LocalKey = (IS_NOT_LEAF(tree->root) ? pico_tree_findKey(tree, key) : NULL); + + /* if node already in, bail out */ + if(LocalKey) { + pico_err = PICO_ERR_EEXIST; + return LocalKey; + } + + insert = create_node(tree, key, allocator); + + if(!insert) + { + pico_err = PICO_ERR_ENOMEM; + /* to let the user know that it couldn't insert */ + return (void *)&LEAF; + } + + pico_tree_insert_node(tree, insert); + + /* fix colour issues */ + fix_insert_collisions(tree, insert); + + return NULL; +} + +struct pico_tree_node *pico_tree_findNode(struct pico_tree *tree, void *key) +{ + struct pico_tree_node *found; + + found = tree->root; + + while(IS_NOT_LEAF(found)) + { + int result; + result = tree->compare(found->keyValue, key); + if(result == 0) + return found; + else if(result < 0) + found = found->rightChild; + else + found = found->leftChild; + } + return NULL; +} + +void *pico_tree_findKey(struct pico_tree *tree, void *key) +{ + struct pico_tree_node *found; + + found = pico_tree_findNode(tree, key); + if (found == NULL) + return NULL; + return found->keyValue; +} + +void *pico_tree_first(struct pico_tree *tree) +{ + return pico_tree_firstNode(tree->root)->keyValue; +} + +void *pico_tree_last(struct pico_tree *tree) +{ + return pico_tree_lastNode(tree->root)->keyValue; +} + +static uint8_t pico_tree_delete_node(struct pico_tree *tree, struct pico_tree_node *d, struct pico_tree_node **temp) +{ + struct pico_tree_node *min; + struct pico_tree_node *ltemp = d; + uint8_t nodeColor; + min = pico_tree_firstNode(d->rightChild); + nodeColor = min->color; + + *temp = min->rightChild; + if(min->parent == ltemp && IS_NOT_LEAF(*temp)) + (*temp)->parent = min; + else{ + switchNodes(tree, min, min->rightChild); + min->rightChild = ltemp->rightChild; + if(IS_NOT_LEAF(min->rightChild)) min->rightChild->parent = min; + } + + switchNodes(tree, ltemp, min); + min->leftChild = ltemp->leftChild; + + if(IS_NOT_LEAF(min->leftChild)) + min->leftChild->parent = min; + + min->color = ltemp->color; + return nodeColor; +} + +static uint8_t pico_tree_delete_check_switch(struct pico_tree *tree, struct pico_tree_node *delete, struct pico_tree_node **temp) +{ + struct pico_tree_node *ltemp = delete; + uint8_t nodeColor = delete->color; + if(IS_LEAF(delete->leftChild)) + { + *temp = ltemp->rightChild; + switchNodes(tree, ltemp, ltemp->rightChild); + } + else + if(IS_LEAF(delete->rightChild)) + { + struct pico_tree_node *_ltemp = delete; + *temp = _ltemp->leftChild; + switchNodes(tree, _ltemp, _ltemp->leftChild); + } + else{ + nodeColor = pico_tree_delete_node(tree, delete, temp); + } + + return nodeColor; + +} + +/* The memory manager also uses the pico_tree to keep track of all the different slab sizes it has. + * These nodes should be placed in the manager page which is in a different memory region then the nodes + * which are used for the pico stack in general. + * Therefore the following wrapper for pico_tree_delete is created. + * The actual implementation can be found in pico_tree_delete_implementation. + */ +void *pico_tree_delete(struct pico_tree *tree, void *key) +{ + return pico_tree_delete_implementation(tree, key, USE_PICO_ZALLOC); +} + +static inline void if_nodecolor_black_fix_collisions(struct pico_tree *tree, struct pico_tree_node *temp, uint8_t nodeColor) +{ + /* deleted node is black, this will mess up the black path property */ + if(nodeColor == BLACK) + fix_delete_collisions(tree, temp); +} + +void *pico_tree_delete_implementation(struct pico_tree *tree, void *key, uint8_t allocator) +{ + struct pico_tree_node *temp; + uint8_t nodeColor; /* keeps the color of the node to be deleted */ + void *lkey; /* keeps a copy of the key which will be removed */ + struct pico_tree_node *delete; /* keeps a copy of the node to be extracted */ + if (!key) + return NULL; + + delete = pico_tree_findNode(tree, key); + + /* this key isn't in the tree, bail out */ + if(!delete) + return NULL; + + lkey = delete->keyValue; + nodeColor = pico_tree_delete_check_switch(tree, delete, &temp); + + if_nodecolor_black_fix_collisions(tree, temp, nodeColor); + + if(allocator == USE_PICO_ZALLOC) + PICO_FREE(delete); + +#ifdef PICO_SUPPORT_MM + else + pico_mem_page0_free(delete); +#endif + return lkey; +} + +int pico_tree_empty(struct pico_tree *tree) +{ + return (!tree->root || IS_LEAF(tree->root)); +} + +/* + * Private functions + */ +static void rotateToLeft(struct pico_tree*tree, struct pico_tree_node*node) +{ + struct pico_tree_node*temp; + + temp = node->rightChild; + + if(temp == &LEAF) return; + + node->rightChild = temp->leftChild; + + if(IS_NOT_LEAF(temp->leftChild)) + temp->leftChild->parent = node; + + temp->parent = node->parent; + + if(IS_LEAF(node->parent)) + tree->root = temp; + else + if(node == node->parent->leftChild) + node->parent->leftChild = temp; + else + node->parent->rightChild = temp; + + temp->leftChild = node; + node->parent = temp; +} + + +static void rotateToRight(struct pico_tree *tree, struct pico_tree_node *node) +{ + struct pico_tree_node*temp; + + temp = node->leftChild; + node->leftChild = temp->rightChild; + + if(temp == &LEAF) return; + + if(IS_NOT_LEAF(temp->rightChild)) + temp->rightChild->parent = node; + + temp->parent = node->parent; + + if(IS_LEAF(node->parent)) + tree->root = temp; + else + if(node == node->parent->rightChild) + node->parent->rightChild = temp; + else + node->parent->leftChild = temp; + + temp->rightChild = node; + node->parent = temp; + return; +} + +static struct pico_tree_node *create_node(struct pico_tree *tree, void*key, uint8_t allocator) +{ + struct pico_tree_node *temp = NULL; + IGNORE_PARAMETER(tree); + if(allocator == USE_PICO_ZALLOC) + temp = (struct pico_tree_node *)PICO_ZALLOC(sizeof(struct pico_tree_node)); + +#ifdef PICO_SUPPORT_MM + else + temp = (struct pico_tree_node *)pico_mem_page0_zalloc(sizeof(struct pico_tree_node)); +#endif + + if(!temp) + return NULL; + + temp->keyValue = key; + temp->parent = &LEAF; + temp->leftChild = &LEAF; + temp->rightChild = &LEAF; + /* by default every new node is red */ + temp->color = RED; + return temp; +} + +/* + * This function fixes the possible collisions in the tree. + * Eg. if a node is red his children must be black ! + */ +static void fix_insert_collisions(struct pico_tree*tree, struct pico_tree_node*node) +{ + struct pico_tree_node*temp; + + while(node->parent->color == RED && IS_NOT_LEAF(GRANPA(node))) + { + if(AM_I_RIGHT_CHILD(node->parent)) + { + temp = GRANPA(node)->leftChild; + if(temp->color == RED) { + node->parent->color = BLACK; + temp->color = BLACK; + GRANPA(node)->color = RED; + node = GRANPA(node); + } + else if(temp->color == BLACK) { + if(AM_I_LEFT_CHILD(node)) { + node = node->parent; + rotateToRight(tree, node); + } + + node->parent->color = BLACK; + GRANPA(node)->color = RED; + rotateToLeft(tree, GRANPA(node)); + } + } + else if(AM_I_LEFT_CHILD(node->parent)) + { + temp = GRANPA(node)->rightChild; + if(temp->color == RED) { + node->parent->color = BLACK; + temp->color = BLACK; + GRANPA(node)->color = RED; + node = GRANPA(node); + } + else if(temp->color == BLACK) { + if(AM_I_RIGHT_CHILD(node)) { + node = node->parent; + rotateToLeft(tree, node); + } + + node->parent->color = BLACK; + GRANPA(node)->color = RED; + rotateToRight(tree, GRANPA(node)); + } + } + } + /* make sure that the root of the tree stays black */ + tree->root->color = BLACK; +} + +static void switchNodes(struct pico_tree*tree, struct pico_tree_node*nodeA, struct pico_tree_node*nodeB) +{ + + if(IS_LEAF(nodeA->parent)) + tree->root = nodeB; + else + if(IS_NOT_LEAF(nodeA)) + { + if(AM_I_LEFT_CHILD(nodeA)) + nodeA->parent->leftChild = nodeB; + else + nodeA->parent->rightChild = nodeB; + } + + if(IS_NOT_LEAF(nodeB)) nodeB->parent = nodeA->parent; + +} + +/* + * This function fixes the possible collisions in the tree. + * Eg. if a node is red his children must be black ! + * In this case the function fixes the constant black path property. + */ +static void fix_delete_collisions(struct pico_tree*tree, struct pico_tree_node *node) +{ + struct pico_tree_node*temp; + + while( node != tree->root && node->color == BLACK && IS_NOT_LEAF(node)) + { + if(AM_I_LEFT_CHILD(node)) { + + temp = node->parent->rightChild; + if(temp->color == RED) + { + temp->color = BLACK; + node->parent->color = RED; + rotateToLeft(tree, node->parent); + temp = node->parent->rightChild; + } + + if(temp->leftChild->color == BLACK && temp->rightChild->color == BLACK) + { + temp->color = RED; + node = node->parent; + } + else + { + if(temp->rightChild->color == BLACK) + { + temp->leftChild->color = BLACK; + temp->color = RED; + rotateToRight(tree, temp); + temp = temp->parent->rightChild; + } + + temp->color = node->parent->color; + node->parent->color = BLACK; + temp->rightChild->color = BLACK; + rotateToLeft(tree, node->parent); + node = tree->root; + } + } + else{ + temp = node->parent->leftChild; + if(temp->color == RED) + { + temp->color = BLACK; + node->parent->color = RED; + rotateToRight(tree, node->parent); + temp = node->parent->leftChild; + } + + if(temp->rightChild->color == BLACK && temp->leftChild->color == BLACK) + { + temp->color = RED; + node = node->parent; + } + else{ + if(temp->leftChild->color == BLACK) + { + temp->rightChild->color = BLACK; + temp->color = RED; + rotateToLeft(tree, temp); + temp = temp->parent->leftChild; + } + + temp->color = node->parent->color; + node->parent->color = BLACK; + temp->leftChild->color = BLACK; + rotateToRight(tree, node->parent); + node = tree->root; + } + } + } + node->color = BLACK; +}