From 8514c6bcab54126410e746a7d1ae131df6f3eff2 Mon Sep 17 00:00:00 2001 From: Joseph Henry Date: Fri, 14 Oct 2016 14:03:06 -0700 Subject: [PATCH] partially-working picoTCP integration --- ext/picotcp/.gitignore | 24 + ext/picotcp/.travis.yml | 13 + ext/picotcp/COPYING | 8 + ext/picotcp/LICENSE | 339 ++ ext/picotcp/MODTREE | 23 + ext/picotcp/Makefile | 423 ++ ext/picotcp/Makefile.watcom | 403 ++ ext/picotcp/README.md | 103 + ext/picotcp/include/arch/pico_arm9.h | 35 + ext/picotcp/include/arch/pico_avr.h | 39 + ext/picotcp/include/arch/pico_cortex_m.h | 12 + ext/picotcp/include/arch/pico_esp8266.h | 55 + ext/picotcp/include/arch/pico_generic_gcc.h | 102 + ext/picotcp/include/arch/pico_linux.h | 33 + ext/picotcp/include/arch/pico_mbed.h | 185 + ext/picotcp/include/arch/pico_msp430.h | 38 + ext/picotcp/include/arch/pico_none.h | 22 + ext/picotcp/include/arch/pico_pic24.h | 100 + ext/picotcp/include/arch/pico_posix.h | 130 + ext/picotcp/include/heap.h | 83 + ext/picotcp/include/pico_addressing.h | 52 + ext/picotcp/include/pico_config.h | 234 ++ ext/picotcp/include/pico_constants.h | 54 + ext/picotcp/include/pico_device.h | 54 + ext/picotcp/include/pico_eth.h | 21 + ext/picotcp/include/pico_frame.h | 122 + ext/picotcp/include/pico_md5.h | 17 + ext/picotcp/include/pico_module_eth.h | 33 + ext/picotcp/include/pico_protocol.h | 95 + ext/picotcp/include/pico_queue.h | 166 + ext/picotcp/include/pico_socket.h | 261 ++ ext/picotcp/include/pico_socket_multicast.h | 10 + ext/picotcp/include/pico_stack.h | 85 + ext/picotcp/include/pico_tree.h | 93 + ext/picotcp/mkdeps.sh | 16 + ext/picotcp/modcheck.py | 50 + ext/picotcp/modules/pico_aodv.c | 674 ++++ ext/picotcp/modules/pico_aodv.h | 130 + ext/picotcp/modules/pico_arp.c | 537 +++ ext/picotcp/modules/pico_arp.h | 35 + ext/picotcp/modules/pico_dev_loop.c | 66 + ext/picotcp/modules/pico_dev_loop.h | 15 + ext/picotcp/modules/pico_dev_mock.c | 307 ++ ext/picotcp/modules/pico_dev_mock.h | 47 + ext/picotcp/modules/pico_dev_null.c | 60 + ext/picotcp/modules/pico_dev_null.h | 15 + ext/picotcp/modules/pico_dev_pcap.c | 96 + ext/picotcp/modules/pico_dev_pcap.h | 19 + ext/picotcp/modules/pico_dev_ppp.c | 2294 +++++++++++ ext/picotcp/modules/pico_dev_ppp.h | 26 + ext/picotcp/modules/pico_dev_tap.c | 220 ++ ext/picotcp/modules/pico_dev_tap.h | 15 + ext/picotcp/modules/pico_dev_tap_windows.c | 1083 ++++++ ext/picotcp/modules/pico_dev_tap_windows.h | 17 + .../modules/pico_dev_tap_windows_private.h | 89 + ext/picotcp/modules/pico_dev_tun.c | 110 + ext/picotcp/modules/pico_dev_tun.h | 15 + ext/picotcp/modules/pico_dev_vde.c | 115 + ext/picotcp/modules/pico_dev_vde.h | 18 + ext/picotcp/modules/pico_dhcp_client.c | 990 +++++ ext/picotcp/modules/pico_dhcp_client.h | 32 + ext/picotcp/modules/pico_dhcp_common.c | 190 + ext/picotcp/modules/pico_dhcp_common.h | 191 + ext/picotcp/modules/pico_dhcp_server.c | 411 ++ ext/picotcp/modules/pico_dhcp_server.h | 34 + ext/picotcp/modules/pico_dns_client.c | 808 ++++ ext/picotcp/modules/pico_dns_client.h | 46 + ext/picotcp/modules/pico_dns_common.c | 1773 +++++++++ ext/picotcp/modules/pico_dns_common.h | 523 +++ ext/picotcp/modules/pico_dns_sd.c | 549 +++ ext/picotcp/modules/pico_dns_sd.h | 91 + ext/picotcp/modules/pico_fragments.c | 391 ++ ext/picotcp/modules/pico_fragments.h | 11 + ext/picotcp/modules/pico_hotplug_detection.c | 134 + ext/picotcp/modules/pico_hotplug_detection.h | 20 + ext/picotcp/modules/pico_icmp4.c | 395 ++ ext/picotcp/modules/pico_icmp4.h | 162 + ext/picotcp/modules/pico_icmp6.c | 698 ++++ ext/picotcp/modules/pico_icmp6.h | 256 ++ ext/picotcp/modules/pico_igmp.c | 1276 ++++++ ext/picotcp/modules/pico_igmp.h | 26 + ext/picotcp/modules/pico_ipfilter.c | 458 +++ ext/picotcp/modules/pico_ipfilter.h | 29 + ext/picotcp/modules/pico_ipv4.c | 1585 ++++++++ ext/picotcp/modules/pico_ipv4.h | 124 + ext/picotcp/modules/pico_ipv6.c | 1921 +++++++++ ext/picotcp/modules/pico_ipv6.h | 175 + ext/picotcp/modules/pico_ipv6_nd.c | 1009 +++++ ext/picotcp/modules/pico_ipv6_nd.h | 26 + ext/picotcp/modules/pico_mdns.c | 3417 +++++++++++++++++ ext/picotcp/modules/pico_mdns.h | 185 + ext/picotcp/modules/pico_mld.c | 1179 ++++++ ext/picotcp/modules/pico_mld.h | 119 + ext/picotcp/modules/pico_mm.c | 1605 ++++++++ ext/picotcp/modules/pico_mm.h | 98 + ext/picotcp/modules/pico_nat.c | 576 +++ ext/picotcp/modules/pico_nat.h | 90 + ext/picotcp/modules/pico_olsr.c | 1143 ++++++ ext/picotcp/modules/pico_olsr.h | 32 + ext/picotcp/modules/pico_posix.c | 99 + ext/picotcp/modules/pico_slaacv4.c | 266 ++ ext/picotcp/modules/pico_slaacv4.h | 18 + ext/picotcp/modules/pico_sntp_client.c | 395 ++ ext/picotcp/modules/pico_sntp_client.h | 22 + ext/picotcp/modules/pico_socket_tcp.c | 267 ++ ext/picotcp/modules/pico_socket_tcp.h | 33 + ext/picotcp/modules/pico_socket_udp.c | 256 ++ ext/picotcp/modules/pico_socket_udp.h | 19 + ext/picotcp/modules/pico_strings.c | 101 + ext/picotcp/modules/pico_strings.h | 21 + ext/picotcp/modules/pico_tcp.c | 3236 ++++++++++++++++ ext/picotcp/modules/pico_tcp.h | 106 + ext/picotcp/modules/pico_tftp.c | 1300 +++++++ ext/picotcp/modules/pico_tftp.h | 83 + ext/picotcp/modules/pico_udp.c | 217 ++ ext/picotcp/modules/pico_udp.h | 45 + ext/picotcp/rules/crc.mk | 1 + ext/picotcp/rules/cyassl.mk | 3 + ext/picotcp/rules/devloop.mk | 2 + ext/picotcp/rules/dhcp_client.mk | 2 + ext/picotcp/rules/dhcp_server.mk | 2 + ext/picotcp/rules/dns_client.mk | 2 + ext/picotcp/rules/dns_sd.mk | 2 + ext/picotcp/rules/eth.mk | 2 + ext/picotcp/rules/icmp4.mk | 5 + ext/picotcp/rules/igmp.mk | 3 + ext/picotcp/rules/ipfilter.mk | 2 + ext/picotcp/rules/ipv4.mk | 2 + ext/picotcp/rules/ipv4frag.mk | 2 + ext/picotcp/rules/ipv6.mk | 3 + ext/picotcp/rules/ipv6frag.mk | 2 + ext/picotcp/rules/mcast.mk | 1 + ext/picotcp/rules/mdns.mk | 2 + ext/picotcp/rules/memory_manager.mk | 2 + ext/picotcp/rules/mld.mk | 3 + ext/picotcp/rules/nat.mk | 2 + ext/picotcp/rules/olsr.mk | 2 + ext/picotcp/rules/pcap.mk | 1 + ext/picotcp/rules/polarssl.mk | 2 + ext/picotcp/rules/ppp.mk | 3 + ext/picotcp/rules/slaacv4.mk | 2 + ext/picotcp/rules/sntp_client.mk | 2 + ext/picotcp/rules/tap.mk | 1 + ext/picotcp/rules/tcp.mk | 3 + ext/picotcp/rules/tun.mk | 1 + ext/picotcp/rules/udp.mk | 3 + ext/picotcp/rules/wolfssl.mk | 3 + ext/picotcp/stack/pico_device.c | 408 ++ ext/picotcp/stack/pico_frame.c | 286 ++ ext/picotcp/stack/pico_md5.c | 43 + ext/picotcp/stack/pico_protocol.c | 214 ++ ext/picotcp/stack/pico_socket.c | 2228 +++++++++++ ext/picotcp/stack/pico_socket_multicast.c | 1340 +++++++ ext/picotcp/stack/pico_stack.c | 1165 ++++++ ext/picotcp/stack/pico_tree.c | 575 +++ ext/picotcp/uncrustify.cfg | 1579 ++++++++ make-linux.mk | 40 +- src/SDK_Debug.h | 14 +- src/SDK_EthernetTap.cpp | 290 +- src/SDK_EthernetTap.hpp | 51 +- src/SDK_defs.h | 26 + src/SDK_jip.hpp | 126 + src/SDK_lwip.hpp | 384 ++ src/SDK_pico.hpp | 188 + 164 files changed, 47767 insertions(+), 56 deletions(-) create mode 100644 ext/picotcp/.gitignore create mode 100644 ext/picotcp/.travis.yml create mode 100644 ext/picotcp/COPYING create mode 100644 ext/picotcp/LICENSE create mode 100644 ext/picotcp/MODTREE create mode 100644 ext/picotcp/Makefile create mode 100644 ext/picotcp/Makefile.watcom create mode 100644 ext/picotcp/README.md create mode 100644 ext/picotcp/include/arch/pico_arm9.h create mode 100644 ext/picotcp/include/arch/pico_avr.h create mode 100644 ext/picotcp/include/arch/pico_cortex_m.h create mode 100644 ext/picotcp/include/arch/pico_esp8266.h create mode 100644 ext/picotcp/include/arch/pico_generic_gcc.h create mode 100644 ext/picotcp/include/arch/pico_linux.h create mode 100644 ext/picotcp/include/arch/pico_mbed.h create mode 100644 ext/picotcp/include/arch/pico_msp430.h create mode 100644 ext/picotcp/include/arch/pico_none.h create mode 100644 ext/picotcp/include/arch/pico_pic24.h create mode 100644 ext/picotcp/include/arch/pico_posix.h create mode 100644 ext/picotcp/include/heap.h create mode 100644 ext/picotcp/include/pico_addressing.h create mode 100644 ext/picotcp/include/pico_config.h create mode 100644 ext/picotcp/include/pico_constants.h create mode 100644 ext/picotcp/include/pico_device.h create mode 100644 ext/picotcp/include/pico_eth.h create mode 100644 ext/picotcp/include/pico_frame.h create mode 100644 ext/picotcp/include/pico_md5.h create mode 100644 ext/picotcp/include/pico_module_eth.h create mode 100644 ext/picotcp/include/pico_protocol.h create mode 100644 ext/picotcp/include/pico_queue.h create mode 100644 ext/picotcp/include/pico_socket.h create mode 100644 ext/picotcp/include/pico_socket_multicast.h create mode 100644 ext/picotcp/include/pico_stack.h create mode 100644 ext/picotcp/include/pico_tree.h create mode 100755 ext/picotcp/mkdeps.sh create mode 100755 ext/picotcp/modcheck.py create mode 100644 ext/picotcp/modules/pico_aodv.c create mode 100644 ext/picotcp/modules/pico_aodv.h create mode 100644 ext/picotcp/modules/pico_arp.c create mode 100644 ext/picotcp/modules/pico_arp.h create mode 100644 ext/picotcp/modules/pico_dev_loop.c create mode 100644 ext/picotcp/modules/pico_dev_loop.h create mode 100644 ext/picotcp/modules/pico_dev_mock.c create mode 100644 ext/picotcp/modules/pico_dev_mock.h create mode 100644 ext/picotcp/modules/pico_dev_null.c create mode 100644 ext/picotcp/modules/pico_dev_null.h create mode 100644 ext/picotcp/modules/pico_dev_pcap.c create mode 100644 ext/picotcp/modules/pico_dev_pcap.h create mode 100644 ext/picotcp/modules/pico_dev_ppp.c create mode 100644 ext/picotcp/modules/pico_dev_ppp.h create mode 100644 ext/picotcp/modules/pico_dev_tap.c create mode 100644 ext/picotcp/modules/pico_dev_tap.h create mode 100644 ext/picotcp/modules/pico_dev_tap_windows.c create mode 100755 ext/picotcp/modules/pico_dev_tap_windows.h create mode 100644 ext/picotcp/modules/pico_dev_tap_windows_private.h create mode 100644 ext/picotcp/modules/pico_dev_tun.c create mode 100644 ext/picotcp/modules/pico_dev_tun.h create mode 100644 ext/picotcp/modules/pico_dev_vde.c create mode 100644 ext/picotcp/modules/pico_dev_vde.h create mode 100644 ext/picotcp/modules/pico_dhcp_client.c create mode 100644 ext/picotcp/modules/pico_dhcp_client.h create mode 100644 ext/picotcp/modules/pico_dhcp_common.c create mode 100644 ext/picotcp/modules/pico_dhcp_common.h create mode 100644 ext/picotcp/modules/pico_dhcp_server.c create mode 100644 ext/picotcp/modules/pico_dhcp_server.h create mode 100644 ext/picotcp/modules/pico_dns_client.c create mode 100644 ext/picotcp/modules/pico_dns_client.h create mode 100644 ext/picotcp/modules/pico_dns_common.c create mode 100644 ext/picotcp/modules/pico_dns_common.h create mode 100644 ext/picotcp/modules/pico_dns_sd.c create mode 100644 ext/picotcp/modules/pico_dns_sd.h create mode 100644 ext/picotcp/modules/pico_fragments.c create mode 100644 ext/picotcp/modules/pico_fragments.h create mode 100644 ext/picotcp/modules/pico_hotplug_detection.c create mode 100644 ext/picotcp/modules/pico_hotplug_detection.h create mode 100644 ext/picotcp/modules/pico_icmp4.c create mode 100644 ext/picotcp/modules/pico_icmp4.h create mode 100644 ext/picotcp/modules/pico_icmp6.c create mode 100644 ext/picotcp/modules/pico_icmp6.h create mode 100644 ext/picotcp/modules/pico_igmp.c create mode 100644 ext/picotcp/modules/pico_igmp.h create mode 100644 ext/picotcp/modules/pico_ipfilter.c create mode 100644 ext/picotcp/modules/pico_ipfilter.h create mode 100644 ext/picotcp/modules/pico_ipv4.c create mode 100644 ext/picotcp/modules/pico_ipv4.h create mode 100644 ext/picotcp/modules/pico_ipv6.c create mode 100644 ext/picotcp/modules/pico_ipv6.h create mode 100644 ext/picotcp/modules/pico_ipv6_nd.c create mode 100644 ext/picotcp/modules/pico_ipv6_nd.h create mode 100644 ext/picotcp/modules/pico_mdns.c create mode 100644 ext/picotcp/modules/pico_mdns.h create mode 100644 ext/picotcp/modules/pico_mld.c create mode 100644 ext/picotcp/modules/pico_mld.h create mode 100644 ext/picotcp/modules/pico_mm.c create mode 100644 ext/picotcp/modules/pico_mm.h create mode 100644 ext/picotcp/modules/pico_nat.c create mode 100644 ext/picotcp/modules/pico_nat.h create mode 100644 ext/picotcp/modules/pico_olsr.c create mode 100644 ext/picotcp/modules/pico_olsr.h create mode 100644 ext/picotcp/modules/pico_posix.c create mode 100644 ext/picotcp/modules/pico_slaacv4.c create mode 100644 ext/picotcp/modules/pico_slaacv4.h create mode 100644 ext/picotcp/modules/pico_sntp_client.c create mode 100644 ext/picotcp/modules/pico_sntp_client.h create mode 100644 ext/picotcp/modules/pico_socket_tcp.c create mode 100644 ext/picotcp/modules/pico_socket_tcp.h create mode 100644 ext/picotcp/modules/pico_socket_udp.c create mode 100644 ext/picotcp/modules/pico_socket_udp.h create mode 100644 ext/picotcp/modules/pico_strings.c create mode 100644 ext/picotcp/modules/pico_strings.h create mode 100644 ext/picotcp/modules/pico_tcp.c create mode 100644 ext/picotcp/modules/pico_tcp.h create mode 100644 ext/picotcp/modules/pico_tftp.c create mode 100644 ext/picotcp/modules/pico_tftp.h create mode 100644 ext/picotcp/modules/pico_udp.c create mode 100644 ext/picotcp/modules/pico_udp.h create mode 100644 ext/picotcp/rules/crc.mk create mode 100644 ext/picotcp/rules/cyassl.mk create mode 100644 ext/picotcp/rules/devloop.mk create mode 100644 ext/picotcp/rules/dhcp_client.mk create mode 100644 ext/picotcp/rules/dhcp_server.mk create mode 100644 ext/picotcp/rules/dns_client.mk create mode 100644 ext/picotcp/rules/dns_sd.mk create mode 100644 ext/picotcp/rules/eth.mk create mode 100644 ext/picotcp/rules/icmp4.mk create mode 100644 ext/picotcp/rules/igmp.mk create mode 100644 ext/picotcp/rules/ipfilter.mk create mode 100644 ext/picotcp/rules/ipv4.mk create mode 100644 ext/picotcp/rules/ipv4frag.mk create mode 100644 ext/picotcp/rules/ipv6.mk create mode 100644 ext/picotcp/rules/ipv6frag.mk create mode 100644 ext/picotcp/rules/mcast.mk create mode 100644 ext/picotcp/rules/mdns.mk create mode 100644 ext/picotcp/rules/memory_manager.mk create mode 100644 ext/picotcp/rules/mld.mk create mode 100644 ext/picotcp/rules/nat.mk create mode 100644 ext/picotcp/rules/olsr.mk create mode 100644 ext/picotcp/rules/pcap.mk create mode 100644 ext/picotcp/rules/polarssl.mk create mode 100644 ext/picotcp/rules/ppp.mk create mode 100644 ext/picotcp/rules/slaacv4.mk create mode 100644 ext/picotcp/rules/sntp_client.mk create mode 100644 ext/picotcp/rules/tap.mk create mode 100644 ext/picotcp/rules/tcp.mk create mode 100644 ext/picotcp/rules/tun.mk create mode 100644 ext/picotcp/rules/udp.mk create mode 100644 ext/picotcp/rules/wolfssl.mk create mode 100644 ext/picotcp/stack/pico_device.c create mode 100644 ext/picotcp/stack/pico_frame.c create mode 100644 ext/picotcp/stack/pico_md5.c create mode 100644 ext/picotcp/stack/pico_protocol.c create mode 100644 ext/picotcp/stack/pico_socket.c create mode 100644 ext/picotcp/stack/pico_socket_multicast.c create mode 100644 ext/picotcp/stack/pico_stack.c create mode 100644 ext/picotcp/stack/pico_tree.c create mode 100644 ext/picotcp/uncrustify.cfg create mode 100644 src/SDK_defs.h create mode 100644 src/SDK_jip.hpp create mode 100644 src/SDK_lwip.hpp create mode 100644 src/SDK_pico.hpp diff --git a/ext/picotcp/.gitignore b/ext/picotcp/.gitignore new file mode 100644 index 0000000..88593da --- /dev/null +++ b/ext/picotcp/.gitignore @@ -0,0 +1,24 @@ +*.d +*.o +*.a +*.out +*.swp +tags +build +UNIT_* +core +core.* +.DS_Store +cscope.files +cscope.out +*.so +*.aux +*.pdf +*.toc +*.gz +*.log +*.pyc +*.elf +*.gcov +*.gcda +*.gcno diff --git a/ext/picotcp/.travis.yml b/ext/picotcp/.travis.yml new file mode 100644 index 0000000..0a87dd4 --- /dev/null +++ b/ext/picotcp/.travis.yml @@ -0,0 +1,13 @@ +before_install: + - sudo apt-get update -qq + - sudo apt-get install -y vde2 check libvdeplug2-dev libpcap-dev psmisc + - sudo pip install cpp-coveralls + - make clean + - rm -f *.gc* + +install: make GCOV=1 && make units ARCH=faulty GCOV=1 && make test GCOV=1 +language: c +script: + - ./test/coverage.sh +after_success: + - coveralls --exclude test/ --exclude modules/ptsocket --exclude build --exclude modules/pico_dev_mock.c --exclude modules/pico_dev_null.c --exclude modules/pico_dev_pcap.c --exclude modules/pico_dev_tap_windows.c --exclude modules/pico_dev_tun.c --gcov-options='\-lp' diff --git a/ext/picotcp/COPYING b/ext/picotcp/COPYING new file mode 100644 index 0000000..8a5fe4b --- /dev/null +++ b/ext/picotcp/COPYING @@ -0,0 +1,8 @@ +PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. + +Released under the GNU General Public License, version 2. +See LICENSE for details. + +Different licensing models may exist, at the sole discretion of +the Copyright holders. + diff --git a/ext/picotcp/LICENSE b/ext/picotcp/LICENSE new file mode 100644 index 0000000..d159169 --- /dev/null +++ b/ext/picotcp/LICENSE @@ -0,0 +1,339 @@ + GNU GENERAL PUBLIC LICENSE + Version 2, June 1991 + + Copyright (C) 1989, 1991 Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +License is intended to guarantee your freedom to share and change free +software--to make sure the software is free for all its users. This +General Public License applies to most of the Free Software +Foundation's software and to any other program whose authors commit to +using it. (Some other Free Software Foundation software is covered by +the GNU Lesser General Public License instead.) You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +this service if you wish), that you receive source code or can get it +if you want it, that you can change the software or use pieces of it +in new free programs; and that you know you can do these things. + + To protect your rights, we need to make restrictions that forbid +anyone to deny you these rights or to ask you to surrender the rights. +These restrictions translate to certain responsibilities for you if you +distribute copies of the software, or if you modify it. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must give the recipients all the rights that +you have. You must make sure that they, too, receive or can get the +source code. And you must show them these terms so they know their +rights. + + We protect your rights with two steps: (1) copyright the software, and +(2) offer you this license which gives you legal permission to copy, +distribute and/or modify the software. + + Also, for each author's protection and ours, we want to make certain +that everyone understands that there is no warranty for this free +software. If the software is modified by someone else and passed on, we +want its recipients to know that what they have is not the original, so +that any problems introduced by others will not reflect on the original +authors' reputations. + + Finally, any free program is threatened constantly by software +patents. We wish to avoid the danger that redistributors of a free +program will individually obtain patent licenses, in effect making the +program proprietary. To prevent this, we have made it clear that any +patent must be licensed for everyone's free use or not licensed at all. + + The precise terms and conditions for copying, distribution and +modification follow. + + GNU GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License applies to any program or other work which contains +a notice placed by the copyright holder saying it may be distributed +under the terms of this General Public License. The "Program", below, +refers to any such program or work, and a "work based on the Program" +means either the Program or any derivative work under copyright law: +that is to say, a work containing the Program or a portion of it, +either verbatim or with modifications and/or translated into another +language. (Hereinafter, translation is included without limitation in +the term "modification".) Each licensee is addressed as "you". + +Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running the Program is not restricted, and the output from the Program +is covered only if its contents constitute a work based on the +Program (independent of having been made by running the Program). +Whether that is true depends on what the Program does. + + 1. You may copy and distribute verbatim copies of the Program's +source code as you receive it, in any medium, provided that you +conspicuously and appropriately publish on each copy an appropriate +copyright notice and disclaimer of warranty; keep intact all the +notices that refer to this License and to the absence of any warranty; +and give any other recipients of the Program a copy of this License +along with the Program. + +You may charge a fee for the physical act of transferring a copy, and +you may at your option offer warranty protection in exchange for a fee. + + 2. You may modify your copy or copies of the Program or any portion +of it, thus forming a work based on the Program, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) You must cause the modified files to carry prominent notices + stating that you changed the files and the date of any change. + + b) You must cause any work that you distribute or publish, that in + whole or in part contains or is derived from the Program or any + part thereof, to be licensed as a whole at no charge to all third + parties under the terms of this License. + + c) If the modified program normally reads commands interactively + when run, you must cause it, when started running for such + interactive use in the most ordinary way, to print or display an + announcement including an appropriate copyright notice and a + notice that there is no warranty (or else, saying that you provide + a warranty) and that users may redistribute the program under + these conditions, and telling the user how to view a copy of this + License. (Exception: if the Program itself is interactive but + does not normally print such an announcement, your work based on + the Program is not required to print an announcement.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Program, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Program, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Program. + +In addition, mere aggregation of another work not based on the Program +with the Program (or with a work based on the Program) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may copy and distribute the Program (or a work based on it, +under Section 2) in object code or executable form under the terms of +Sections 1 and 2 above provided that you also do one of the following: + + a) Accompany it with the complete corresponding machine-readable + source code, which must be distributed under the terms of Sections + 1 and 2 above on a medium customarily used for software interchange; or, + + b) Accompany it with a written offer, valid for at least three + years, to give any third party, for a charge no more than your + cost of physically performing source distribution, a complete + machine-readable copy of the corresponding source code, to be + distributed under the terms of Sections 1 and 2 above on a medium + customarily used for software interchange; or, + + c) Accompany it with the information you received as to the offer + to distribute corresponding source code. (This alternative is + allowed only for noncommercial distribution and only if you + received the program in object code or executable form with such + an offer, in accord with Subsection b above.) + +The source code for a work means the preferred form of the work for +making modifications to it. For an executable work, complete source +code means all the source code for all modules it contains, plus any +associated interface definition files, plus the scripts used to +control compilation and installation of the executable. However, as a +special exception, the source code distributed need not include +anything that is normally distributed (in either source or binary +form) with the major components (compiler, kernel, and so on) of the +operating system on which the executable runs, unless that component +itself accompanies the executable. + +If distribution of executable or object code is made by offering +access to copy from a designated place, then offering equivalent +access to copy the source code from the same place counts as +distribution of the source code, even though third parties are not +compelled to copy the source along with the object code. + + 4. You may not copy, modify, sublicense, or distribute the Program +except as expressly provided under this License. Any attempt +otherwise to copy, modify, sublicense or distribute the Program is +void, and will automatically terminate your rights under this License. +However, parties who have received copies, or rights, from you under +this License will not have their licenses terminated so long as such +parties remain in full compliance. + + 5. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Program or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Program (or any work based on the +Program), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Program or works based on it. + + 6. Each time you redistribute the Program (or any work based on the +Program), the recipient automatically receives a license from the +original licensor to copy, distribute or modify the Program subject to +these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties to +this License. + + 7. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Program at all. For example, if a patent +license would not permit royalty-free redistribution of the Program by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Program. + +If any portion of this section is held invalid or unenforceable under +any particular circumstance, the balance of the section is intended to +apply and the section as a whole is intended to apply in other +circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system, which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 8. If the distribution and/or use of the Program is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Program under this License +may add an explicit geographical distribution limitation excluding +those countries, so that distribution is permitted only in or among +countries not thus excluded. In such case, this License incorporates +the limitation as if written in the body of this License. + + 9. The Free Software Foundation may publish revised and/or new versions +of the General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + +Each version is given a distinguishing version number. If the Program +specifies a version number of this License which applies to it and "any +later version", you have the option of following the terms and conditions +either of that version or of any later version published by the Free +Software Foundation. If the Program does not specify a version number of +this License, you may choose any version ever published by the Free Software +Foundation. + + 10. If you wish to incorporate parts of the Program into other free +programs whose distribution conditions are different, write to the author +to ask for permission. For software which is copyrighted by the Free +Software Foundation, write to the Free Software Foundation; we sometimes +make exceptions for this. Our decision will be guided by the two goals +of preserving the free status of all derivatives of our free software and +of promoting the sharing and reuse of software generally. + + NO WARRANTY + + 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY +FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN +OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES +PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED +OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS +TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE +PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, +REPAIR OR CORRECTION. + + 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR +REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, +INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING +OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED +TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY +YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER +PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE +POSSIBILITY OF SUCH DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License along + with this program; if not, write to the Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + +Also add information on how to contact you by electronic and paper mail. + +If the program is interactive, make it output a short notice like this +when it starts in an interactive mode: + + Gnomovision version 69, Copyright (C) year name of author + Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, the commands you use may +be called something other than `show w' and `show c'; they could even be +mouse-clicks or menu items--whatever suits your program. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the program, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the program + `Gnomovision' (which makes passes at compilers) written by James Hacker. + + , 1 April 1989 + Ty Coon, President of Vice + +This General Public License does not permit incorporating your program into +proprietary programs. If your program is a subroutine library, you may +consider it more useful to permit linking proprietary applications with the +library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. diff --git a/ext/picotcp/MODTREE b/ext/picotcp/MODTREE new file mode 100644 index 0000000..6e343a9 --- /dev/null +++ b/ext/picotcp/MODTREE @@ -0,0 +1,23 @@ +RTOS: +IPV4: +IPV6: +DEVLOOP: +CRC: +ETH: IPV4 +TCP: IPV4 +UDP: IPV4 +IPV4FRAG: IPV4 +NAT: IPV4 UDP +ICMP4: IPV4 +MCAST: UDP +PING: ICMP4 +DHCP_CLIENT: UDP +DHCP_SERVER: UDP +DNS_CLIENT: UDP +IPFILTER: IPV4 +OLSR: MCAST +SLAACV4: IPV4 +SNTP_CLIENT: DNS_CLIENT +TFTP: UDP +MDNS: MCAST +DNS_SD: MDNS diff --git a/ext/picotcp/Makefile b/ext/picotcp/Makefile new file mode 100644 index 0000000..8ce14e6 --- /dev/null +++ b/ext/picotcp/Makefile @@ -0,0 +1,423 @@ +-include ../../config.mk +-include ../../tools/kconfig/.config + +CC:=$(CROSS_COMPILE)gcc +LD:=$(CROSS_COMPILE)ld +AR:=$(CROSS_COMPILE)ar +RANLIB:=$(CROSS_COMPILE)ranlib +SIZE:=$(CROSS_COMPILE)size +STRIP_BIN:=$(CROSS_COMPILE)strip +TEST_LDFLAGS=-pthread $(PREFIX)/modules/*.o $(PREFIX)/lib/*.o -lvdeplug +LIBNAME:="libpicotcp.a" + +PREFIX?=$(PWD)/build +DEBUG?=1 +PROFILE?=0 +PERF?=0 +ENDIAN?=little +STRIP?=0 +RTOS?=0 +GENERIC?=0 +PTHREAD?=0 +ADDRESS_SANITIZER?=1 + +# Default compiled-in protocols +# +TCP?=1 +UDP?=1 +ETH?=1 +IPV4?=1 +IPV4FRAG?=1 +IPV6FRAG?=0 +NAT?=1 +ICMP4?=1 +MCAST?=1 +DEVLOOP?=1 +PING?=1 +DHCP_CLIENT?=1 +DHCP_SERVER?=1 +DNS_CLIENT?=1 +MDNS?=1 +DNS_SD?=1 +SNTP_CLIENT?=1 +IPFILTER?=1 +CRC?=1 +OLSR?=0 +SLAACV4?=1 +TFTP?=1 +AODV?=1 +MEMORY_MANAGER?=0 +MEMORY_MANAGER_PROFILING?=0 +TUN?=0 +TAP?=0 +PCAP?=0 +PPP?=1 +CYASSL?=0 +WOLFSSL?=0 +POLARSSL?=0 + +#IPv6 related +IPV6?=1 + +EXTRA_CFLAGS+=-DPICO_COMPILE_TIME=`date +%s` +EXTRA_CFLAGS+=$(PLATFORM_CFLAGS) + +CFLAGS=-I$(PREFIX)/include -Iinclude -Imodules -Wall -Wdeclaration-after-statement -W -Wextra -Wshadow -Wcast-qual -Wwrite-strings -Wunused-variable -Wundef -Wunused-function $(EXTRA_CFLAGS) +# extra flags recommanded by TIOBE TICS framework to score an A on compiler warnings +CFLAGS+= -Wconversion +# request from Toon +CFLAGS+= -Wcast-align +CFLAGS+= -Wmissing-prototypes +CFLAGS+= -Wno-missing-field-initializers + + +ifeq ($(DEBUG),1) + CFLAGS+=-ggdb +else + ifeq ($(PERF), 1) + CFLAGS+=-O3 + else + CFLAGS+=-Os + endif +endif + +ifeq ($(PROFILE),1) + CFLAGS+=-pg +endif + +ifeq ($(TFTP),1) + MOD_OBJ+=$(LIBBASE)modules/pico_strings.o $(LIBBASE)modules/pico_tftp.o + OPTIONS+=-DPICO_SUPPORT_TFTP +endif + +ifeq ($(AODV),1) + MOD_OBJ+=$(LIBBASE)modules/pico_aodv.o + OPTIONS+=-DPICO_SUPPORT_AODV +endif + +ifeq ($(GENERIC),1) + CFLAGS+=-DGENERIC +endif + +ifeq ($(PTHREAD),1) + CFLAGS+=-DPICO_SUPPORT_PTHREAD +endif + + +ifneq ($(ENDIAN),little) + CFLAGS+=-DPICO_BIGENDIAN +endif + +ifneq ($(RTOS),0) + OPTIONS+=-DPICO_SUPPORT_RTOS +endif + +ifeq ($(ARCH),cortexm4-hardfloat) + CFLAGS+=-DCORTEX_M4_HARDFLOAT -mcpu=cortex-m4 -mthumb -mlittle-endian -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb-interwork -fsingle-precision-constant +endif + +ifeq ($(ARCH),cortexm4-softfloat) + CFLAGS+=-DCORTEX_M4_SOFTFLOAT -mcpu=cortex-m4 -mthumb -mlittle-endian -mfloat-abi=soft -mthumb-interwork +endif + +ifeq ($(ARCH),cortexm3) + CFLAGS+=-DCORTEX_M3 -mcpu=cortex-m3 -mthumb -mlittle-endian -mthumb-interwork +endif + +ifeq ($(ARCH),arm9) + CFLAGS+=-DARM9 -mcpu=arm9e -march=armv5te -gdwarf-2 -Wall -marm -mthumb-interwork -fpack-struct +endif + +ifeq ($(ADDRESS_SANITIZER),1) + TEST_LDFLAGS+=-fsanitize=address -fno-omit-frame-pointer +endif + +ifeq ($(ARCH),faulty) + CFLAGS+=-DFAULTY -DUNIT_TEST + ifeq ($(ADDRESS_SANITIZER),1) + CFLAGS+=-fsanitize=address + endif + CFLAGS+=-fno-omit-frame-pointer + UNITS_OBJ+=test/pico_faulty.o + TEST_OBJ+=test/pico_faulty.o + DUMMY_EXTRA+=test/pico_faulty.o +endif + +ifeq ($(ARCH),msp430) + CFLAGS+=-DMSP430 +endif + +ifeq ($(ARCH),esp8266) + CFLAGS+=-DESP8266 -Wl,-EL -fno-inline-functions -nostdlib -mlongcalls -mtext-section-literals +endif + +ifeq ($(ARCH),mt7681) + CFLAGS+=-DMT7681 -fno-builtin -ffunction-sections -fno-strict-aliasing -m16bit -mabi=2 -mbaseline=V2 -mcpu=n9 -mno-div -mel -mmw-count=8 -mno-ext-mac -mno-dx-regs +endif + +ifeq ($(ARCH),pic24) + CFLAGS+=-DPIC24 -c -mcpu=24FJ256GA106 -MMD -MF -g -omf=elf \ + -mlarge-code -mlarge-data -msmart-io=1 -msfr-warn=off +endif + +ifeq ($(ARCH),atmega128) + CFLAGS+=-Wall -mmcu=atmega128 -DAVR +endif + +ifeq ($(ARCH),none) + CFLAGS+=-DARCHNONE +endif + +ifeq ($(ARCH),shared) + CFLAGS+=-fPIC +endif + +%.o:%.c deps + $(CC) -c $(CFLAGS) -o $@ $< + +CORE_OBJ= stack/pico_stack.o \ + stack/pico_frame.o \ + stack/pico_device.o \ + stack/pico_protocol.o \ + stack/pico_socket.o \ + stack/pico_socket_multicast.o \ + stack/pico_tree.o \ + stack/pico_md5.o + +POSIX_OBJ+= modules/pico_dev_vde.o \ + modules/pico_dev_tun.o \ + modules/pico_dev_tap.o \ + modules/pico_dev_mock.o + +ifneq ($(ETH),0) + include rules/eth.mk +endif +ifneq ($(IPV4),0) + include rules/ipv4.mk +endif +ifneq ($(IPV4FRAG),0) + include rules/ipv4frag.mk +endif +ifneq ($(ICMP4),0) + include rules/icmp4.mk +endif +ifneq ($(TCP),0) + include rules/tcp.mk +endif +ifneq ($(UDP),0) + include rules/udp.mk +endif +ifneq ($(MCAST),0) + include rules/mcast.mk + include rules/igmp.mk + include rules/mld.mk +endif +ifneq ($(NAT),0) + include rules/nat.mk +endif +ifneq ($(DEVLOOP),0) + include rules/devloop.mk +endif +ifneq ($(DHCP_CLIENT),0) + include rules/dhcp_client.mk +endif +ifneq ($(DHCP_SERVER),0) + include rules/dhcp_server.mk +endif +ifneq ($(DNS_CLIENT),0) + include rules/dns_client.mk +endif +ifneq ($(MDNS),0) + include rules/mdns.mk +endif +ifneq ($(DNS_SD),0) + include rules/dns_sd.mk +endif +ifneq ($(IPFILTER),0) + include rules/ipfilter.mk +endif +ifneq ($(CRC),0) + include rules/crc.mk +endif +ifneq ($(OLSR),0) + include rules/olsr.mk +endif +ifneq ($(SLAACV4),0) + include rules/slaacv4.mk +endif +ifneq ($(IPV6),0) + include rules/ipv6.mk +endif +ifneq ($(MEMORY_MANAGER),0) + include rules/memory_manager.mk +endif +ifneq ($(MEMORY_MANAGER_PROFILING),0) + OPTIONS+=-DPICO_SUPPORT_MM_PROFILING +endif +ifneq ($(SNTP_CLIENT),0) + include rules/sntp_client.mk +endif +ifneq ($(TUN),0) + include rules/tun.mk +endif +ifneq ($(TAP),0) + include rules/tap.mk +endif +ifneq ($(PCAP),0) + include rules/pcap.mk +endif +ifneq ($(PPP),0) + include rules/ppp.mk +endif +ifneq ($(CYASSL),0) + include rules/cyassl.mk +endif +ifneq ($(WOLFSSL),0) + include rules/wolfssl.mk +endif +ifneq ($(POLARSSL),0) + include rules/polarssl.mk +endif + +all: mod core lib + +core: $(CORE_OBJ) + @mkdir -p $(PREFIX)/lib + @mv stack/*.o $(PREFIX)/lib + +mod: $(MOD_OBJ) + @mkdir -p $(PREFIX)/modules + @mv modules/*.o $(PREFIX)/modules || echo + +posix: all $(POSIX_OBJ) + @mv modules/*.o $(PREFIX)/modules || echo + + +TEST_ELF= test/picoapp.elf +TEST6_ELF= test/picoapp6.elf + + +test: posix + @mkdir -p $(PREFIX)/test/ + @make -C test/examples PREFIX=$(PREFIX) + @echo -e "\t[CC] picoapp.o" + @$(CC) -c -o $(PREFIX)/examples/picoapp.o test/picoapp.c $(CFLAGS) -Itest/examples + @echo -e "\t[LD] $@" + @$(CC) -g -o $(TEST_ELF) -I include -I modules -I $(PREFIX)/include -Wl,--start-group $(TEST_LDFLAGS) $(TEST_OBJ) $(PREFIX)/examples/*.o -Wl,--end-group + @mv test/*.elf $(PREFIX)/test + @install $(PREFIX)/$(TEST_ELF) $(PREFIX)/$(TEST6_ELF) + +tst: test + +$(PREFIX)/include/pico_defines.h: + @mkdir -p $(PREFIX)/lib + @mkdir -p $(PREFIX)/include + @bash ./mkdeps.sh $(PREFIX) $(OPTIONS) + + +deps: $(PREFIX)/include/pico_defines.h + + + +lib: mod core + @cp -f include/*.h $(PREFIX)/include + @cp -fa include/arch $(PREFIX)/include + @cp -f modules/*.h $(PREFIX)/include + @echo -e "\t[AR] $(PREFIX)/lib/$(LIBNAME)" + @$(AR) cru $(PREFIX)/lib/$(LIBNAME) $(PREFIX)/modules/*.o $(PREFIX)/lib/*.o \ + || $(AR) cru $(PREFIX)/lib/$(LIBNAME) $(PREFIX)/lib/*.o + @echo -e "\t[RANLIB] $(PREFIX)/lib/$(LIBNAME)" + @$(RANLIB) $(PREFIX)/lib/$(LIBNAME) + @test $(STRIP) -eq 1 && (echo -e "\t[STRIP] $(PREFIX)/lib/$(LIBNAME)" \ + && $(STRIP_BIN) $(PREFIX)/lib/$(LIBNAME)) \ + || echo -e "\t[KEEP SYMBOLS] $(PREFIX)/lib/$(LIBNAME)" + @echo -e "\t[LIBSIZE] `du -b $(PREFIX)/lib/$(LIBNAME)`" + @echo -e "`size -t $(PREFIX)/lib/$(LIBNAME)`" + +loop: mod core + mkdir -p $(PREFIX)/test + @$(CC) -c -o $(PREFIX)/modules/pico_dev_loop.o modules/pico_dev_loop.c $(CFLAGS) + @$(CC) -c -o $(PREFIX)/loop_ping.o test/loop_ping.c $(CFLAGS) -ggdb + +units: mod core lib $(UNITS_OBJ) $(MOD_OBJ) + @echo -e "\n\t[UNIT TESTS SUITE]" + @mkdir -p $(PREFIX)/test + @echo -e "\t[CC] units.o" + @$(CC) -g -c -o $(PREFIX)/test/units.o test/units.c $(CFLAGS) -I stack -I modules -I includes -I test/unit -DUNIT_TEST + @echo -e "\t[LD] $(PREFIX)/test/units" + @$(CC) -o $(PREFIX)/test/units $(CFLAGS) $(PREFIX)/test/units.o -lcheck -lm -pthread -lrt \ + $(UNITS_OBJ) $(PREFIX)/modules/pico_aodv.o \ + $(PREFIX)/modules/pico_fragments.o + @$(CC) -o $(PREFIX)/test/modunit_pico_protocol.elf $(CFLAGS) -I. test/unit/modunit_pico_protocol.c stack/pico_tree.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) + @$(CC) -o $(PREFIX)/test/modunit_pico_frame.elf $(CFLAGS) -I. test/unit/modunit_pico_frame.c stack/pico_tree.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) + @$(CC) -o $(PREFIX)/test/modunit_seq.elf $(CFLAGS) -I. test/unit/modunit_seq.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_tcp.elf $(CFLAGS) -I. test/unit/modunit_pico_tcp.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_dns_client.elf $(CFLAGS) -I. test/unit/modunit_pico_dns_client.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_dns_common.elf $(CFLAGS) -I. test/unit/modunit_pico_dns_common.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_mdns.elf $(CFLAGS) -I. test/unit/modunit_pico_mdns.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_dns_sd.elf $(CFLAGS) -I. test/unit/modunit_pico_dns_sd.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_dev_loop.elf $(CFLAGS) -I. test/unit/modunit_pico_dev_loop.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) + @$(CC) -o $(PREFIX)/test/modunit_ipv6_nd.elf $(CFLAGS) -I. test/unit/modunit_pico_ipv6_nd.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_pico_stack.elf $(CFLAGS) -I. test/unit/modunit_pico_stack.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_tftp.elf $(CFLAGS) -I. test/unit/modunit_pico_tftp.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_sntp_client.elf $(CFLAGS) -I. test/unit/modunit_pico_sntp_client.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) + @$(CC) -o $(PREFIX)/test/modunit_ipfilter.elf $(CFLAGS) -I. test/unit/modunit_pico_ipfilter.c stack/pico_tree.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) + @$(CC) -o $(PREFIX)/test/modunit_aodv.elf $(CFLAGS) -I. test/unit/modunit_pico_aodv.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_fragments.elf $(CFLAGS) -I. test/unit/modunit_pico_fragments.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_queue.elf $(CFLAGS) -I. test/unit/modunit_queue.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) + @$(CC) -o $(PREFIX)/test/modunit_dev_ppp.elf $(CFLAGS) -I. test/unit/modunit_pico_dev_ppp.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_mld.elf $(CFLAGS) -I. test/unit/modunit_pico_mld.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_igmp.elf $(CFLAGS) -I. test/unit/modunit_pico_igmp.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_hotplug_detection.elf $(CFLAGS) -I. test/unit/modunit_pico_hotplug_detection.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + +devunits: mod core lib + @echo -e "\n\t[UNIT TESTS SUITE: device drivers]" + @mkdir -p $(PREFIX)/test/unit/device/ + @echo -e "\t[CC] picotcp_mock.o" + @$(CC) -c -o $(PREFIX)/test/unit/device/picotcp_mock.o $(CFLAGS) -I stack -I modules -I includes -I test/unit test/unit/device/picotcp_mock.c + @$(CC) -c -o $(PREFIX)/test/unit/device/unit_dev_vde.o $(CFLAGS) -I stack -I modules -I includes -I test/unit test/unit/device/unit_dev_vde.c + @echo -e "\t[LD] $(PREFIX)/test/devunits" + @$(CC) -o $(PREFIX)/test/devunits $(CFLAGS) -I stack $(PREFIX)/test/unit/device/*.o -lcheck -lm -pthread -lrt + +units_mm: mod core lib + @echo -e "\n\t[UNIT TESTS SUITE]" + @mkdir -p $(PREFIX)/test + @echo -e "\t[CC] units_mm.o" + @$(CC) -c -o $(PREFIX)/test/units_mm.o test/unit/unit_mem_manager.c $(CFLAGS) -I stack -I modules -I includes -I test/unit + @echo -e "\t[LD] $(PREFIX)/test/units" + @$(CC) -o $(PREFIX)/test/units_mm $(CFLAGS) $(PREFIX)/test/units_mm.o -lcheck -lm -pthread -lrt + + +clean: + @echo -e "\t[CLEAN] $(PREFIX)/" + @rm -rf $(PREFIX) tags + +mbed: + @echo -e "\t[Creating PicoTCP.zip]" + @rm -f PicoTCP.zip + @cp include/pico_socket.h include/socket.tmp + @echo "#define MBED\n" > include/mbed.tmp + @cat include/mbed.tmp include/socket.tmp > include/pico_socket.h + @zip -0 PicoTCP.zip -r include modules stack -x include/arch/ include/arch/* include/pico_config.h include/*.tmp modules/pico_dev_* + @rm include/pico_socket.h include/mbed.tmp + @mv include/socket.tmp include/pico_socket.h + + +style: + @find . -iname "*.[c|h]" | xargs -x uncrustify --replace -l C -c uncrustify.cfg || true + @find . -iname "*unc-backup*" |xargs -x rm || true + +dummy: mod core lib $(DUMMY_EXTRA) + @echo testing configuration... + @$(CC) -c -o test/dummy.o test/dummy.c $(CFLAGS) + @$(CC) -o dummy test/dummy.o $(DUMMY_EXTRA) $(PREFIX)/lib/libpicotcp.a $(LDFLAGS) $(CFLAGS) + @echo done. + @rm -f test/dummy.o dummy + +ppptest: test/ppp.c lib + gcc -ggdb -c -o ppp.o test/ppp.c -I build/include/ -I build/modules/ $(CFLAGS) + gcc -o ppp ppp.o build/lib/libpicotcp.a $(LDFLAGS) $(CFLAGS) + rm -f ppp.o + + +FORCE: diff --git a/ext/picotcp/Makefile.watcom b/ext/picotcp/Makefile.watcom new file mode 100644 index 0000000..cbe04d7 --- /dev/null +++ b/ext/picotcp/Makefile.watcom @@ -0,0 +1,403 @@ +-include ../../config.mk +-include ../../tools/kconfig/.config + +WATCOM_PATH:=/opt/watcom +CC:=$(WATCOM_PATH)/binl/$(CROSS_COMPILE)wcc386 +LD:=$(WATCOM_PATH)/binl/$(CROSS_COMPILE)wcl386 +AR:=$(WATCOM_PATH)/binl/$(CROSS_COMPILE)wlib +RANLIB:=$(WATCOM_PATH)/binl/$(CROSS_COMPILE)ranlib +SIZE:=$(CROSS_COMPILE)size +STRIP_BIN:=$(CROSS_COMPILE)strip +TEST_LDFLAGS=-pthread $(PREFIX)/modules/*.o $(PREFIX)/lib/*.o -lvdeplug +LIBNAME:=libpicotcp.a + +PREFIX?=$(PWD)/build +DEBUG?=1 +PROFILE?=0 +PERF?=0 +ENDIAN?=little +STRIP?=0 +RTOS?=0 +GENERIC?=0 +PTHREAD?=0 +ADDRESS_SANITIZER?=1 + +# Default compiled-in protocols +# +TCP?=1 +UDP?=1 +ETH?=1 +IPV4?=1 +IPV4FRAG?=1 +IPV6FRAG?=0 +NAT?=1 +ICMP4?=1 +MCAST?=1 +DEVLOOP?=1 +PING?=1 +DHCP_CLIENT?=1 +DHCP_SERVER?=1 +DNS_CLIENT?=1 +MDNS?=1 +DNS_SD?=1 +SNTP_CLIENT?=1 +IPFILTER?=1 +CRC?=1 +OLSR?=0 +SLAACV4?=1 +TFTP?=1 +AODV?=1 +MEMORY_MANAGER?=0 +MEMORY_MANAGER_PROFILING?=0 +TUN?=0 +TAP?=0 +PCAP?=0 +PPP?=0 +CYASSL?=0 +WOLFSSL?=0 +POLARSSL?=0 + +#IPv6 related +IPV6?=1 + +EXTRA_CFLAGS+=-dPICO_COMPILE_TIME=`date +%s` +EXTRA_CFLAGS+=$(PLATFORM_CFLAGS) + +CFLAGS=-i=$(WATCOM_PATH)/h -i=$(PREFIX)/include -i=include -i=modules $(EXTRA_CFLAGS) -q + + +ifeq ($(DEBUG),1) + CFLAGS+=-od -of -d9 +else + ifeq ($(PERF), 1) + CFLAGS+= + else + CFLAGS+= + endif +endif + +ifeq ($(TFTP),1) + MOD_OBJ+=$(LIBBASE)modules/pico_strings.o $(LIBBASE)modules/pico_tftp.o + OPTIONS+=-dPICO_SUPPORT_TFTP +endif + +ifeq ($(AODV),1) + MOD_OBJ+=$(LIBBASE)modules/pico_aodv.o + OPTIONS+=-dPICO_SUPPORT_AODV +endif + +ifeq ($(GENERIC),1) + CFLAGS+=-dGENERIC +endif + +ifeq ($(PTHREAD),1) + CFLAGS+=-dPICO_SUPPORT_PTHREAD +endif + + +ifneq ($(ENDIAN),little) + CFLAGS+=-dPICO_BIGENDIAN +endif + +ifneq ($(RTOS),0) + OPTIONS+=-dPICO_SUPPORT_RTOS +endif + +ifeq ($(ARCH),cortexm4-hardfloat) + CFLAGS+=-dCORTEX_M4_HARDFLOAT -mcpu=cortex-m4 -mthumb -mlittle-endian -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb-interwork -fsingle-precision-constant +endif + +ifeq ($(ARCH),cortexm4-softfloat) + CFLAGS+=-dCORTEX_M4_SOFTFLOAT -mcpu=cortex-m4 -mthumb -mlittle-endian -mfloat-abi=soft -mthumb-interwork +endif + +ifeq ($(ARCH),cortexm3) + CFLAGS+=-dCORTEX_M3 -mcpu=cortex-m3 -mthumb -mlittle-endian -mthumb-interwork +endif + +ifeq ($(ARCH),arm9) + CFLAGS+=-dARM9 -mcpu=arm9e -march=armv5te -gdwarf-2 -Wall -marm -mthumb-interwork -fpack-struct +endif + +ifeq ($(ADDRESS_SANITIZER),1) + TEST_LDFLAGS+=-fsanitize=address -fno-omit-frame-pointer +endif + +ifeq ($(ARCH),faulty) + CFLAGS+=-dFAULTY -dUNIT_TEST + CFLAGS+=-fsanitize=address -fno-omit-frame-pointer + UNITS_OBJ+=test/pico_faulty.o + TEST_OBJ+=test/pico_faulty.o + DUMMY_EXTRA+=test/pico_faulty.o +endif + +ifeq ($(ARCH),msp430) + CFLAGS+=-dMSP430 +endif + +ifeq ($(ARCH),esp8266) + CFLAGS+=-dESP8266 -Wl,-EL -fno-inline-functions -nostdlib -mlongcalls -mtext-section-literals +endif + +ifeq ($(ARCH),mt7681) + CFLAGS+=-dMT7681 -fno-builtin -ffunction-sections -fno-strict-aliasing -m16bit -mabi=2 -mbaseline=V2 -mcpu=n9 -mno-div -mel -mmw-count=8 -mno-ext-mac -mno-dx-regs +endif + +ifeq ($(ARCH),pic24) + CFLAGS+=-dPIC24 -mcpu=24FJ256GA106 -MMD -MF -g -omf=elf \ + -mlarge-code -mlarge-data -msmart-io=1 -msfr-warn=off +endif + +ifeq ($(ARCH),atmega128) + CFLAGS+=-Wall -mmcu=atmega128 -dAVR +endif + +ifeq ($(ARCH),none) + CFLAGS+=-dARCHNONE +endif + +ifeq ($(ARCH),shared) + CFLAGS+=-fPIC +endif + +%.o:%.c deps + $(CC) $(CFLAGS) -fo=$@ $< + +CORE_OBJ= stack/pico_stack.o \ + stack/pico_frame.o \ + stack/pico_device.o \ + stack/pico_protocol.o \ + stack/pico_socket.o \ + stack/pico_socket_multicast.o \ + stack/pico_tree.o \ + stack/pico_md5.o + +POSIX_OBJ+= modules/pico_dev_vde.o \ + modules/pico_dev_tun.o \ + modules/pico_dev_tap.o \ + modules/pico_dev_mock.o + +ifneq ($(ETH),0) + include rules/eth.mk +endif +ifneq ($(IPV4),0) + include rules/ipv4.mk +endif +ifneq ($(IPV4FRAG),0) + include rules/ipv4frag.mk +endif +ifneq ($(ICMP4),0) + include rules/icmp4.mk +endif +ifneq ($(TCP),0) + include rules/tcp.mk +endif +ifneq ($(UDP),0) + include rules/udp.mk +endif +ifneq ($(MCAST),0) + include rules/mcast.mk + include rules/igmp.mk +endif +ifneq ($(NAT),0) + include rules/nat.mk +endif +ifneq ($(DEVLOOP),0) + include rules/devloop.mk +endif +ifneq ($(DHCP_CLIENT),0) + include rules/dhcp_client.mk +endif +ifneq ($(DHCP_SERVER),0) + include rules/dhcp_server.mk +endif +ifneq ($(DNS_CLIENT),0) + include rules/dns_client.mk +endif +ifneq ($(MDNS),0) + include rules/mdns.mk +endif +ifneq ($(DNS_SD),0) + include rules/dns_sd.mk +endif +ifneq ($(IPFILTER),0) + include rules/ipfilter.mk +endif +ifneq ($(CRC),0) + include rules/crc.mk +endif +ifneq ($(OLSR),0) + include rules/olsr.mk +endif +ifneq ($(SLAACV4),0) + include rules/slaacv4.mk +endif +ifneq ($(IPV6),0) + include rules/ipv6.mk +endif +ifneq ($(MEMORY_MANAGER),0) + include rules/memory_manager.mk +endif +ifneq ($(MEMORY_MANAGER_PROFILING),0) + OPTIONS+=-dPICO_SUPPORT_MM_PROFILING +endif +ifneq ($(SNTP_CLIENT),0) + include rules/sntp_client.mk +endif +ifneq ($(TUN),0) + include rules/tun.mk +endif +ifneq ($(TAP),0) + include rules/tap.mk +endif +ifneq ($(PCAP),0) + include rules/pcap.mk +endif +ifneq ($(PPP),0) + include rules/ppp.mk +endif +ifneq ($(CYASSL),0) + include rules/cyassl.mk +endif +ifneq ($(WOLFSSL),0) + include rules/wolfssl.mk +endif +ifneq ($(POLARSSL),0) + include rules/polarssl.mk +endif + +all: mod core lib + +core: $(CORE_OBJ) + @mkdir -p $(PREFIX)/lib + @mv stack/*.o $(PREFIX)/lib + +mod: $(MOD_OBJ) + @mkdir -p $(PREFIX)/modules + @mv modules/*.o $(PREFIX)/modules || echo + +posix: all $(POSIX_OBJ) + @mv modules/*.o $(PREFIX)/modules || echo + + +TEST_ELF= test/picoapp.elf +TEST6_ELF= test/picoapp6.elf + + +test: posix + @mkdir -p $(PREFIX)/test/ + @make -C test/examples PREFIX=$(PREFIX) + @echo -e "\t[CC] picoapp.o" + @$(CC) -c -o $(PREFIX)/examples/picoapp.o test/picoapp.c $(CFLAGS) -Itest/examples + @echo -e "\t[LD] $@" + @$(CC) -g -o $(TEST_ELF) -I include -I modules -I $(PREFIX)/include -Wl,--start-group $(TEST_LDFLAGS) $(TEST_OBJ) $(PREFIX)/examples/*.o -Wl,--end-group + @mv test/*.elf $(PREFIX)/test + @install $(PREFIX)/$(TEST_ELF) $(PREFIX)/$(TEST6_ELF) + +tst: test + +$(PREFIX)/include/pico_defines.h: + @mkdir -p $(PREFIX)/lib + @mkdir -p $(PREFIX)/include + @bash ./mkdeps.sh $(PREFIX) $(OPTIONS) + + +deps: $(PREFIX)/include/pico_defines.h + + + +lib: mod core + @cp -f include/*.h $(PREFIX)/include + @cp -fa include/arch $(PREFIX)/include + @cp -f modules/*.h $(PREFIX)/include + @echo -e "\t[AR] $(PREFIX)/lib/$(LIBNAME)" + $(AR) -q -b -n -fag -o=$(PREFIX)/lib/$(LIBNAME) $(PREFIX)/modules/*.o $(PREFIX)/lib/*.o + @echo || $(AR) cru $(PREFIX)/lib/$(LIBNAME) $(PREFIX)/lib/*.o + @echo -e "\t[RANLIB] $(PREFIX)/lib/$(LIBNAME)" + @$(RANLIB) $(PREFIX)/lib/$(LIBNAME) + @echo -e "\t[LIBSIZE] `du -b $(PREFIX)/lib/$(LIBNAME)`" + +loop: mod core + mkdir -p $(PREFIX)/test + @$(CC) -c -o $(PREFIX)/modules/pico_dev_loop.o modules/pico_dev_loop.c $(CFLAGS) + @$(CC) -c -o $(PREFIX)/loop_ping.o test/loop_ping.c $(CFLAGS) -ggdb + +units: mod core lib $(UNITS_OBJ) $(MOD_OBJ) + @echo -e "\n\t[UNIT TESTS SUITE]" + @mkdir -p $(PREFIX)/test + @echo -e "\t[CC] units.o" + @$(CC) -c -o $(PREFIX)/test/units.o test/units.c $(CFLAGS) -I stack -I modules -I includes -I test/unit -dUNIT_TEST + @echo -e "\t[LD] $(PREFIX)/test/units" + @$(CC) -o $(PREFIX)/test/units $(CFLAGS) $(PREFIX)/test/units.o -lcheck -lm -pthread -lrt \ + $(UNITS_OBJ) $(PREFIX)/modules/pico_aodv.o \ + $(PREFIX)/modules/pico_fragments.o + @$(CC) -o $(PREFIX)/test/modunit_pico_protocol.elf $(CFLAGS) -I. test/unit/modunit_pico_protocol.c stack/pico_tree.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) + @$(CC) -o $(PREFIX)/test/modunit_pico_frame.elf $(CFLAGS) -I. test/unit/modunit_pico_frame.c stack/pico_tree.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) + @$(CC) -o $(PREFIX)/test/modunit_seq.elf $(CFLAGS) -I. test/unit/modunit_seq.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_tcp.elf $(CFLAGS) -I. test/unit/modunit_pico_tcp.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_dns_client.elf $(CFLAGS) -I. test/unit/modunit_pico_dns_client.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_dns_common.elf $(CFLAGS) -I. test/unit/modunit_pico_dns_common.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_mdns.elf $(CFLAGS) -I. test/unit/modunit_pico_mdns.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_dns_sd.elf $(CFLAGS) -I. test/unit/modunit_pico_dns_sd.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_dev_loop.elf $(CFLAGS) -I. test/unit/modunit_pico_dev_loop.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) + @$(CC) -o $(PREFIX)/test/modunit_ipv6_nd.elf $(CFLAGS) -I. test/unit/modunit_pico_ipv6_nd.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_pico_stack.elf $(CFLAGS) -I. test/unit/modunit_pico_stack.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_tftp.elf $(CFLAGS) -I. test/unit/modunit_pico_tftp.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_sntp_client.elf $(CFLAGS) -I. test/unit/modunit_pico_sntp_client.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) + @$(CC) -o $(PREFIX)/test/modunit_ipfilter.elf $(CFLAGS) -I. test/unit/modunit_pico_ipfilter.c stack/pico_tree.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) + @$(CC) -o $(PREFIX)/test/modunit_aodv.elf $(CFLAGS) -I. test/unit/modunit_pico_aodv.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_fragments.elf $(CFLAGS) -I. test/unit/modunit_pico_fragments.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + @$(CC) -o $(PREFIX)/test/modunit_queue.elf $(CFLAGS) -I. test/unit/modunit_queue.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) + @$(CC) -o $(PREFIX)/test/modunit_dev_ppp.elf $(CFLAGS) -I. test/unit/modunit_pico_dev_ppp.c -lcheck -lm -pthread -lrt $(UNITS_OBJ) $(PREFIX)/lib/libpicotcp.a + +devunits: mod core lib + @echo -e "\n\t[UNIT TESTS SUITE: device drivers]" + @mkdir -p $(PREFIX)/test/unit/device/ + @echo -e "\t[CC] picotcp_mock.o" + @$(CC) -c -o $(PREFIX)/test/unit/device/picotcp_mock.o $(CFLAGS) -I stack -I modules -I includes -I test/unit test/unit/device/picotcp_mock.c + @$(CC) -c -o $(PREFIX)/test/unit/device/unit_dev_vde.o $(CFLAGS) -I stack -I modules -I includes -I test/unit test/unit/device/unit_dev_vde.c + @echo -e "\t[LD] $(PREFIX)/test/devunits" + @$(CC) -o $(PREFIX)/test/devunits $(CFLAGS) -I stack $(PREFIX)/test/unit/device/*.o -lcheck -lm -pthread -lrt + +units_mm: mod core lib + @echo -e "\n\t[UNIT TESTS SUITE]" + @mkdir -p $(PREFIX)/test + @echo -e "\t[CC] units_mm.o" + @$(CC) -c -o $(PREFIX)/test/units_mm.o test/unit/unit_mem_manager.c $(CFLAGS) -I stack -I modules -I includes -I test/unit + @echo -e "\t[LD] $(PREFIX)/test/units" + @$(CC) -o $(PREFIX)/test/units_mm $(CFLAGS) $(PREFIX)/test/units_mm.o -lcheck -lm -pthread -lrt + + +clean: + @echo -e "\t[CLEAN] $(PREFIX)/" + @rm -rf $(PREFIX) tags + +mbed: + @echo -e "\t[Creating PicoTCP.zip]" + @rm -f PicoTCP.zip + @cp include/pico_socket.h include/socket.tmp + @echo "#define MBED\n" > include/mbed.tmp + @cat include/mbed.tmp include/socket.tmp > include/pico_socket.h + @zip -0 PicoTCP.zip -r include modules stack -x include/arch/ include/arch/* include/pico_config.h include/*.tmp modules/pico_dev_* + @rm include/pico_socket.h include/mbed.tmp + @mv include/socket.tmp include/pico_socket.h + + +style: + @find . -iname "*.[c|h]" | xargs -x uncrustify --replace -l C -c uncrustify.cfg || true + @find . -iname "*unc-backup*" |xargs -x rm || true + +dummy: mod core lib $(DUMMY_EXTRA) + @echo testing configuration... + @$(CC) -c -o test/dummy.o test/dummy.c $(CFLAGS) + @$(CC) -o dummy test/dummy.o $(DUMMY_EXTRA) $(PREFIX)/lib/libpicotcp.a $(LDFLAGS) $(CFLAGS) + @echo done. + @rm -f test/dummy.o dummy + +ppptest: test/ppp.c lib + gcc -ggdb -c -o ppp.o test/ppp.c -I build/include/ -I build/modules/ $(CFLAGS) + gcc -o ppp ppp.o build/lib/libpicotcp.a $(LDFLAGS) $(CFLAGS) + rm -f ppp.o + + +FORCE: diff --git a/ext/picotcp/README.md b/ext/picotcp/README.md new file mode 100644 index 0000000..37c5d43 --- /dev/null +++ b/ext/picotcp/README.md @@ -0,0 +1,103 @@ +picoTCP + +--------------- + +Welcome to the one and only picoTCP repository. + +picoTCP is a small-footprint, modular TCP/IP stack designed for embedded systems and the Internet of Things. It's actively being developed by *[Altran Intelligent Systems](http://intelligent-systems.altran.com/)*. Textual information about picoTCP, you can find on the [about page of our website](http://picotcp.com/about). + +This code is released under the terms of GNU GPL v2 only. Some rights reserved. +Other licenses may apply at the sole discretion of the copyright holders. + +Learn how to use picoTCP in your project by going through the **Getting Started guide** on our [GitHub wiki](https://github.com/tass-belgium/picotcp/wiki). + +For more information visit the [picoTCP website](http://www.picotcp.com), send us an email or contact us on [Twitter](https://twitter.com/picotcp), [Facebook](https://www.facebook.com/picoTCP) or [Reddit](http://www.reddit.com/r/picotcp/). + +Wondering about picoTCP's code quality? Check [our TiCS score](http://tics.picotcp.com:42506/tiobeweb/TICS/TqiDashboard.html#axes=Project%28%29&metric=tqi&sel=Project%28PicoTCP_rel%29) + + +--------------- + +Continuous integration + +Jenkins Functional tests: +[![Jenkins autotest](http://jenkins.picotcp.com:8080/buildStatus/icon?job=picoTCP_Rel/PicoTCP_rel_autotest)](http://jenkins.picotcp.com:8080/job/picoTCP_Rel/job/PicoTCP_rel_autotest) + +Jenkins Unit tests : +[![Jenkins unit tests](http://jenkins.picotcp.com:8080/buildStatus/icon?job=picoTCP_Rel/PicoTCP_rel_unit_tests)](http://jenkins.picotcp.com:8080/job/picoTCP_Rel/job/PicoTCP_rel_unit_tests) + +Jenkins RFC compliance : +[![Jenkins RFC Compliance](http://jenkins.picotcp.com:8080/buildStatus/icon?job=picoTCP_Rel/PicoTCP_rel_RF_mbed)](http://jenkins.picotcp.com:8080/job/picoTCP_Rel/job/PicoTCP_rel_RF_mbed) + +Jenkins TICS quality : +[![Jenkins TICS](http://jenkins.picotcp.com:8080/buildStatus/icon?job=picoTCP_Rel/PicoTCP_rel_TICS)](http://jenkins.picotcp.com:8080/job/picoTCP_Rel/job/PicoTCP_rel_TICS/) + +Coverity Scan Build status: +[![Coverity Scan Build Status](https://scan.coverity.com/projects/7944/badge.svg)](https://scan.coverity.com/projects/7944) + +--------------- + +Works with ... +## Platforms +* ARM Cortex-M series + * ST Micro STM + * NXP LPC + * TI Stellaris + * Freescale K64F +* ARM ARM9-series + * ST Micro STR9 +* Texas Instruments + * MSP430 +* Microchip + * PIC24 +* Atmel + * AVR 8bit +* Linux + * User space (TUN/TAP) + * Kernel space +* Windows + * User space (TAP) + +## Network drivers +* BCM43362 (IEEE 802.11) +* MRF24WG (IEEE 802.11) +* LPC Ethernet ENET/EMAC (IEEE 802.3) +* Stellaris Ethernet (IEEE 802.3) +* STM32 Ethernet (IEEE 802.3) +* Wiznet W5100 (IEEE 802.3) +* USB CDC-ECM (CDC1.2) +* PPP +* Virtual drivers + * TUN/TAP + * VDE + * Libpcap + +## (RT)OSes +* No OS / Bare metal +* FreeRTOS +* mbed-RTOS +* Frosted +* linux / POSIX +* MS DOS +* MS Windows + +## Libraries +* wolfSSL +* mbedTLS +* Mongoose RESTful library +* MicroPython +* HTTP library +* ZeroMQ (WIP) +* MQTT + +## Compilers +* GCC +* Clang +* TCC +* ARM-RCVT +* IAR +* XC-16 +* MSP-GCC +* AVR-GCC + +Your favorite not in the list? Check out the wiki for information and examples on how to port picoTCP to a new platform! diff --git a/ext/picotcp/include/arch/pico_arm9.h b/ext/picotcp/include/arch/pico_arm9.h new file mode 100644 index 0000000..0d2d2e3 --- /dev/null +++ b/ext/picotcp/include/arch/pico_arm9.h @@ -0,0 +1,35 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + *********************************************************************/ +#define dbg(...) do {} while(0) + +/******************/ + +/*** MACHINE CONFIGURATION ***/ +/* Temporary (POSIX) stuff. */ +#include +#include + +extern volatile uint32_t __str9_tick; + +#define pico_native_malloc(x) calloc(x, 1) +#define pico_native_free(x) free(x) + +static inline unsigned long PICO_TIME(void) +{ + register uint32_t tick = __str9_tick; + return tick / 1000; +} + +static inline unsigned long PICO_TIME_MS(void) +{ + return __str9_tick; +} + +static inline void PICO_IDLE(void) +{ + unsigned long tick_now = __str9_tick; + while(tick_now == __str9_tick) ; +} + diff --git a/ext/picotcp/include/arch/pico_avr.h b/ext/picotcp/include/arch/pico_avr.h new file mode 100644 index 0000000..cd7e4ab --- /dev/null +++ b/ext/picotcp/include/arch/pico_avr.h @@ -0,0 +1,39 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#define dbg(...) do {} while(0) +/* #define dbg printf */ + +/*************************/ + +/*** MACHINE CONFIGURATION ***/ +/* Temporary (POSIX) stuff. */ +#include +#include +#include +#include "pico_mm.h" + +extern volatile uint32_t __avr_tick; + +#define pico_zalloc(x) calloc(x, 1) +#define pico_free(x) free(x) + +static inline unsigned long PICO_TIME(void) +{ + register uint32_t tick = __avr_tick; + return tick / 1000; +} + +static inline unsigned long PICO_TIME_MS(void) +{ + return __avr_tick; +} + +static inline void PICO_IDLE(void) +{ + unsigned long tick_now = __avr_tick; + while(tick_now == __avr_tick) ; +} + diff --git a/ext/picotcp/include/arch/pico_cortex_m.h b/ext/picotcp/include/arch/pico_cortex_m.h new file mode 100644 index 0000000..cfd12f3 --- /dev/null +++ b/ext/picotcp/include/arch/pico_cortex_m.h @@ -0,0 +1,12 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#ifndef _INCLUDE_PICO_CORTEX_M +#define _INCLUDE_PICO_CORTEX_M + +#include "pico_generic_gcc.h" + +#endif /* PICO_CORTEX_M */ + diff --git a/ext/picotcp/include/arch/pico_esp8266.h b/ext/picotcp/include/arch/pico_esp8266.h new file mode 100644 index 0000000..5419e1f --- /dev/null +++ b/ext/picotcp/include/arch/pico_esp8266.h @@ -0,0 +1,55 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2014-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#ifndef _INCLUDE_PICO_ESP8266 +#define _INCLUDE_PICO_ESP8266 + +#include + +#include +#include +#include +#include "pico_constants.h" + +/* -------------- DEBUG ------------- */ + +/* #define dbg(...) */ +#define dbg printf + +/* -------------- MEMORY ------------- */ +extern void *pvPortMalloc( size_t xWantedSize ); +extern void vPortFree( void *pv ); + +#define pico_free vPortFree + +static inline void *pico_zalloc(size_t size) +{ + void *ptr = (void *)pvPortMalloc(size); + + if(ptr) + memset(ptr, 0u, size); + + return ptr; +} + +/* -------------- TIME ------------- */ + +extern volatile uint32_t esp_tick; + +static inline pico_time PICO_TIME_MS(void) { + return (pico_time) esp_tick; +} + +static inline pico_time PICO_TIME(void) { + return PICO_TIME_MS() / 1000; +} + +static inline void PICO_IDLE(void) { + uint32_t now = esp_tick; + while (now == esp_tick) + ; +} + +#endif diff --git a/ext/picotcp/include/arch/pico_generic_gcc.h b/ext/picotcp/include/arch/pico_generic_gcc.h new file mode 100644 index 0000000..5958e3f --- /dev/null +++ b/ext/picotcp/include/arch/pico_generic_gcc.h @@ -0,0 +1,102 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#ifndef _INCLUDE_PICO_GCC +#define _INCLUDE_PICO_GCC + +#include +#include +#include +#include "pico_constants.h" + +/* monotonically increasing tick, + * typically incremented every millisecond in a systick interrupt */ +extern volatile unsigned int pico_ms_tick; + +#define dbg(...) + +#ifdef PICO_SUPPORT_PTHREAD + #define PICO_SUPPORT_MUTEX +#endif + +#ifdef PICO_SUPPORT_RTOS + #define PICO_SUPPORT_MUTEX + +extern void *pico_mutex_init(void); +extern void pico_mutex_lock(void*); +extern void pico_mutex_unlock(void*); +extern void *pvPortMalloc( size_t xSize ); +extern void vPortFree( void *pv ); + + #define pico_free(x) vPortFree(x) + #define free(x) vPortFree(x) + +static inline void *pico_zalloc(size_t size) +{ + void *ptr = pvPortMalloc(size); + + if(ptr) + memset(ptr, 0u, size); + + return ptr; +} + +static inline pico_time PICO_TIME_MS() +{ + return pico_ms_tick; +} + +static inline pico_time PICO_TIME() +{ + return pico_ms_tick / 1000; +} + +static inline void PICO_IDLE(void) +{ + pico_time now = PICO_TIME_MS(); + while(now == PICO_TIME_MS()) ; +} + +#else /* NO RTOS SUPPORT */ + + #ifdef MEM_MEAS +/* These functions should be implemented elsewhere */ +extern void *memmeas_zalloc(size_t size); +extern void memmeas_free(void *); + #define pico_free(x) memmeas_free(x) + #define pico_zalloc(x) memmeas_zalloc(x) + #else +/* Use plain C-lib malloc and free */ + #define pico_free(x) free(x) +static inline void *pico_zalloc(size_t size) +{ + void *ptr = malloc(size); + if(ptr) + memset(ptr, 0u, size); + + return ptr; +} + #endif + +static inline pico_time PICO_TIME_MS(void) +{ + return (pico_time)pico_ms_tick; +} + +static inline pico_time PICO_TIME(void) +{ + return (pico_time)(PICO_TIME_MS() / 1000); +} + +static inline void PICO_IDLE(void) +{ + unsigned int now = pico_ms_tick; + while(now == pico_ms_tick) ; +} + +#endif /* IFNDEF RTOS */ + +#endif /* PICO_GCC */ + diff --git a/ext/picotcp/include/arch/pico_linux.h b/ext/picotcp/include/arch/pico_linux.h new file mode 100644 index 0000000..3910c25 --- /dev/null +++ b/ext/picotcp/include/arch/pico_linux.h @@ -0,0 +1,33 @@ +#ifndef PICO_SUPPORT_LINUX +#define PICO_SUPPORT_LINUX + +#include "linux/types.h" +#include "linux/mm.h" +#include "linux/slab.h" +#include "linux/jiffies.h" + +#define dbg printk + +#define pico_zalloc(x) kcalloc(x, 1, GFP_ATOMIC) /* All allocations are GFP_ATOMIC for now */ +#define pico_free(x) kfree(x) + + +static inline unsigned long PICO_TIME(void) +{ + return (unsigned long)(jiffies_to_msecs(jiffies) / 1000); +} + +static inline unsigned long PICO_TIME_MS(void) +{ + return (unsigned long)jiffies_to_msecs(jiffies); +} + +static inline void PICO_IDLE(void) +{ + unsigned long now = jiffies; + while (now == jiffies) { + ; + } +} + +#endif diff --git a/ext/picotcp/include/arch/pico_mbed.h b/ext/picotcp/include/arch/pico_mbed.h new file mode 100644 index 0000000..3c3a272 --- /dev/null +++ b/ext/picotcp/include/arch/pico_mbed.h @@ -0,0 +1,185 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + File: pico_mbed.h + Author: Toon Peters + *********************************************************************/ + +#ifndef PICO_SUPPORT_MBED +#define PICO_SUPPORT_MBED +#include +#include +/* #include "mbed.h" */ +/* #include "serial_api.h" */ + +/* #define TIME_PRESCALE */ +/* #define PICO_MEASURE_STACK */ +/* #define MEMORY_MEASURE */ +/* + Debug needs initialization: + * void serial_init (serial_t *obj, PinName tx, PinName rx); + * void serial_baud (serial_t *obj, int baudrate); + * void serial_format (serial_t *obj, int data_bits, SerialParity parity, int stop_bits); + */ + +#define dbg(...) + +/* + #define MEMORY_MEASURE + #define JENKINS_DEBUG + */ + +/* Intended for Mr. Jenkins endurance test loggings */ +#ifdef JENKINS_DEBUG +#include "PicoTerm.h" +#define jenkins_dbg ptm_dbg +#endif + +#ifdef PICO_MEASURE_STACK + +extern int freeStack; +#define STACK_TOTAL_WORDS 1000u +#define STACK_PATTERN (0xC0CAC01Au) + +void stack_fill_pattern(void *ptr); +void stack_count_free_words(void *ptr); +int stack_get_free_words(void); +#else +#define stack_fill_pattern(...) do {} while(0) +#define stack_count_free_words(...) do {} while(0) +#define stack_get_free_words() (0) +#endif + +#ifdef MEMORY_MEASURE /* in case, comment out the two defines above me. */ +extern uint32_t max_mem; +extern uint32_t cur_mem; + +struct mem_chunk_stats { +#ifdef MEMORY_MEASURE_ADV + uint32_t signature; + void *mem; +#endif + uint32_t size; +}; + +static inline void *pico_zalloc(int x) +{ + struct mem_chunk_stats *stats; + if ((cur_mem + x) > (10 * 1024)) + return NULL; + + stats = (struct mem_chunk_stats *)calloc(x + sizeof(struct mem_chunk_stats), 1); +#ifdef MEMORY_MEASURE_ADV + stats->signature = 0xdeadbeef; + stats->mem = ((uint8_t *)stats) + sizeof(struct mem_chunk_stats); +#endif + stats->size = x; + + /* Intended for Mr. Jenkins endurance test loggings */ + #ifdef JENKINS_DEBUG + if (!stats) { + jenkins_dbg(">> OUT OF MEM\n"); + while(1) ; + ; + } + + #endif + cur_mem += x; + if (cur_mem > max_mem) { + max_mem = cur_mem; + /* printf("max mem: %lu\n", max_mem); */ + } + +#ifdef MEMORY_MEASURE_ADV + return (void*)(stats->mem); +#else + return (void*) (((uint8_t *)stats) + sizeof(struct mem_chunk_stats)); +#endif +} + +static inline void pico_free(void *x) +{ + struct mem_chunk_stats *stats = (struct mem_chunk_stats *) ((uint8_t *)x - sizeof(struct mem_chunk_stats)); + + #ifdef JENKINS_DEBUG + #ifdef MEMORY_MEASURE_ADV + if ((stats->signature != 0xdeadbeef) || (x != stats->mem)) { + jenkins_dbg(">> FREE ERROR: caller is %p\n", __builtin_return_address(0)); + while(1) ; + ; + } + + #endif + + #endif + + cur_mem -= stats->size; + memset(stats, 0, sizeof(struct mem_chunk_stats)); + free(stats); +} +#else + +#define pico_zalloc(x) calloc(x, 1) +#define pico_free(x) free(x) + +#endif + +#define PICO_SUPPORT_MUTEX +extern void *pico_mutex_init(void); +extern void pico_mutex_lock(void*); +extern void pico_mutex_unlock(void*); +extern void pico_mutex_deinit(void*); + +extern uint32_t os_time; +extern pico_time local_time; +extern uint32_t last_os_time; + +#ifdef TIME_PRESCALE +extern int32_t prescale_time; +#endif + +#define UPDATE_LOCAL_TIME() do {local_time = local_time + ((pico_time)os_time - (pico_time)last_os_time);last_os_time = os_time;} while(0) + +static inline pico_time PICO_TIME(void) +{ + UPDATE_LOCAL_TIME(); + #ifdef TIME_PRESCALE + return (prescale_time < 0) ? (pico_time)(local_time / 1000 << (-prescale_time)) : \ + (pico_time)(local_time / 1000 >> prescale_time); + #else + return (pico_time)(local_time / 1000); + #endif +} + +static inline pico_time PICO_TIME_MS(void) +{ + UPDATE_LOCAL_TIME(); + #ifdef TIME_PRESCALE + return (prescale_time < 0) ? (pico_time)(local_time << (-prescale_time)) : \ + (pico_time)(local_time >> prescale_time); + #else + return (pico_time)local_time; + #endif +} + +static inline void PICO_IDLE(void) +{ + /* TODO needs implementation */ +} +/* + static inline void PICO_DEBUG(const char * formatter, ... ) + { + char buffer[256]; + char *ptr; + va_list args; + va_start(args, formatter); + vsnprintf(buffer, 256, formatter, args); + ptr = buffer; + while(*ptr != '\0') + serial_putc(serial_t *obj, (int) (*(ptr++))); + va_end(args); + //TODO implement serial_t + }*/ + +#endif diff --git a/ext/picotcp/include/arch/pico_msp430.h b/ext/picotcp/include/arch/pico_msp430.h new file mode 100644 index 0000000..27e3e5a --- /dev/null +++ b/ext/picotcp/include/arch/pico_msp430.h @@ -0,0 +1,38 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#ifndef _INCLUDE_PICO_LPC +#define _INCLUDE_PICO_LPC + +#include +#include +#include +#include "pico_constants.h" + +extern pico_time msp430_time_s(void); +extern pico_time msp430_time_ms(void); +extern void *malloc(size_t); +extern void free(void *); + + +#define PICO_TIME() msp430_time_s() +#define PICO_TIME_MS() msp430_time_ms() +#define PICO_IDLE() do {} while(0) + +#define pico_free(x) free(x) + +static inline void *pico_zalloc(size_t size) +{ + void *ptr = malloc(size); + + if(ptr) + memset(ptr, 0u, size); + + return ptr; +} + +#define dbg(...) + +#endif diff --git a/ext/picotcp/include/arch/pico_none.h b/ext/picotcp/include/arch/pico_none.h new file mode 100644 index 0000000..fee74f9 --- /dev/null +++ b/ext/picotcp/include/arch/pico_none.h @@ -0,0 +1,22 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + *********************************************************************/ + +#ifndef PICO_SUPPORT_ARCHNONE +#define PICO_SUPPORT_ARCHNONE + +#include +#include +#include +#include + +#define dbg(...) do {} while(0) +#define pico_zalloc(x) NULL +#define pico_free(x) do {} while(0) +#define PICO_TIME() 666 +#define PICO_TIME_MS() 666000 +#define PICO_IDLE() do {} while(0) + +#endif /* PICO_SUPPORT_ARCHNONE */ + diff --git a/ext/picotcp/include/arch/pico_pic24.h b/ext/picotcp/include/arch/pico_pic24.h new file mode 100644 index 0000000..27ff39e --- /dev/null +++ b/ext/picotcp/include/arch/pico_pic24.h @@ -0,0 +1,100 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + *********************************************************************/ +#ifndef PICO_SUPPORT_PIC24 +#define PICO_SUPPORT_PIC24 +#define dbg printf +/* #define dbg(...) */ + +/*************************/ + +/*** MACHINE CONFIGURATION ***/ +#include +#include + +/* #include "phalox_development_board.h" */ + +#ifndef __PIC24F__ +#define __PIC24F__ +#endif + +/* + #ifndef __PIC24FJ256GA106__ + #define __PIC24FJ256GA106__ + #endif + */ + +#ifndef PICO_MAX_SOCKET_FRAMES +#define PICO_MAX_SOCKET_FRAMES 16 +#endif + +/* Device header file */ + +#if defined(__PIC24E__) +# include +#elif defined(__PIC24F__) +# include +#elif defined(__PIC24H__) +# include +#endif + + +#define TIMBASE_INT_E IEC0bits.T2IE + +#ifdef PICO_SUPPORT_DEBUG_MEMORY +static inline void *pico_zalloc(int len) +{ + /* dbg("%s: Alloc object of len %d, caller: %p\n", __FUNCTION__, len, __builtin_return_address(0)); */ + return calloc(len, 1); +} + +static inline void pico_free(void *tgt) +{ + /* dbg("%s: Discarded object @%p, caller: %p\n", __FUNCTION__, tgt, __builtin_return_address(0)); */ + free(tgt); +} +#else +# define pico_zalloc(x) calloc(x, 1) +# define pico_free(x) free(x) +#endif + +extern void *pvPortMalloc( size_t xWantedSize ); +extern volatile pico_time __pic24_tick; + +static inline unsigned long PICO_TIME(void) +{ + unsigned long tick; + /* Disable timer interrupts */ + TIMBASE_INT_E = 0; + tick = __pic24_tick; + /* Enable timer interrupts */ + TIMBASE_INT_E = 1; + return tick / 1000; +} + +static inline unsigned long PICO_TIME_MS(void) +{ + unsigned long tick; + /* Disable timer interrupts */ + TIMBASE_INT_E = 0; + tick = __pic24_tick; + /* Enable timer interrupts */ + TIMBASE_INT_E = 1; + return tick; +} + +static inline void PICO_IDLE(void) +{ + unsigned long tick_now; + /* Disable timer interrupts */ + TIMBASE_INT_E = 0; + tick_now = (unsigned long)pico_tick; + /* Enable timer interrupts */ + TIMBASE_INT_E = 1; + /* Doesn't matter that this call isn't interrupt safe, */ + /* we just check for the value to change */ + while(tick_now == __pic24_tick) ; +} + +#endif diff --git a/ext/picotcp/include/arch/pico_posix.h b/ext/picotcp/include/arch/pico_posix.h new file mode 100644 index 0000000..30ce884 --- /dev/null +++ b/ext/picotcp/include/arch/pico_posix.h @@ -0,0 +1,130 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + *********************************************************************/ + +#ifndef PICO_SUPPORT_POSIX +#define PICO_SUPPORT_POSIX + +#include +#include +#include +#include + +/* + #define MEMORY_MEASURE + #define TIME_PRESCALE + #define PICO_SUPPORT_THREADING + */ +#define dbg printf + +#define stack_fill_pattern(...) do {} while(0) +#define stack_count_free_words(...) do {} while(0) +#define stack_get_free_words() (0) + +/* measure allocated memory */ +#ifdef MEMORY_MEASURE +extern uint32_t max_mem; +extern uint32_t cur_mem; + +static inline void *pico_zalloc(int x) +{ + uint32_t *ptr; + if ((cur_mem + x) > (10 * 1024)) + return NULL; + + ptr = (uint32_t *)calloc(x + 4, 1); + *ptr = (uint32_t)x; + cur_mem += x; + if (cur_mem > max_mem) { + max_mem = cur_mem; + } + + return (void*)(ptr + 1); +} + +static inline void pico_free(void *x) +{ + uint32_t *ptr = (uint32_t*)(((uint8_t *)x) - 4); + cur_mem -= *ptr; + free(ptr); +} +#else +#define pico_zalloc(x) calloc(x, 1) +#define pico_free(x) free(x) +#endif + +/* time prescaler */ +#ifdef TIME_PRESCALE +extern int32_t prescale_time; +#endif + +#if defined(PICO_SUPPORT_RTOS) || defined (PICO_SUPPORT_PTHREAD) +/* pico_ms_tick must be defined */ +extern volatile unsigned long int pico_ms_tick; + + +static inline uint32_t PICO_TIME(void) +{ + return pico_ms_tick / 1000; +} + +static inline uint32_t PICO_TIME_MS(void) +{ + return pico_ms_tick; +} + +#else + +static inline uint32_t PICO_TIME(void) +{ + struct timeval t; + gettimeofday(&t, NULL); + #ifdef TIME_PRESCALE + return (prescale_time < 0) ? (uint32_t)(t.tv_sec / 1000 << (-prescale_time)) : \ + (uint32_t)(t.tv_sec / 1000 >> prescale_time); + #else + return (uint32_t)t.tv_sec; + #endif +} + +static inline uint32_t PICO_TIME_MS(void) +{ + struct timeval t; + gettimeofday(&t, NULL); + #ifdef TIME_PRESCALER + uint32_t tmp = ((t.tv_sec * 1000) + (t.tv_usec / 1000)); + return (prescale_time < 0) ? (uint32_t)(tmp / 1000 << (-prescale_time)) : \ + (uint32_t)(tmp / 1000 >> prescale_time); + #else + return (uint32_t)((t.tv_sec * 1000) + (t.tv_usec / 1000)); + #endif +} +#endif + +#ifdef PICO_SUPPORT_THREADING +#define PICO_SUPPORT_MUTEX +/* mutex implementations */ +extern void *pico_mutex_init(void); +extern void pico_mutex_lock(void *mux); +extern void pico_mutex_unlock(void *mux); + +/* semaphore implementations (only used in wrapper code) */ +extern void *pico_sem_init(void); +extern void pico_sem_destroy(void *sem); +extern void pico_sem_post(void *sem); +/* returns -1 on timeout (in ms), else returns 0 */ +/* if timeout < 0, the semaphore waits forever */ +extern int pico_sem_wait(void *sem, int timeout); + +/* thread implementations */ +extern void *pico_thread_create(void *(*routine)(void *), void *arg); +#endif /* PICO_SUPPORT_THREADING */ + +static inline void PICO_IDLE(void) +{ + usleep(5000); +} + +#endif /* PICO_SUPPORT_POSIX */ + diff --git a/ext/picotcp/include/heap.h b/ext/picotcp/include/heap.h new file mode 100644 index 0000000..88f495d --- /dev/null +++ b/ext/picotcp/include/heap.h @@ -0,0 +1,83 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ + +#define DECLARE_HEAP(type, orderby) \ + struct heap_ ## type { \ + uint32_t size; \ + uint32_t n; \ + type *top; \ + }; \ + typedef struct heap_ ## type heap_ ## type; \ + static inline int heap_insert(struct heap_ ## type *heap, type * el) \ + { \ + uint32_t i; \ + type *newTop; \ + if (++heap->n >= heap->size) { \ + newTop = PICO_ZALLOC((heap->n + 1) * sizeof(type)); \ + if(!newTop) { \ + heap->n--; \ + return -1; \ + } \ + if (heap->top) { \ + memcpy(newTop, heap->top, heap->n * sizeof(type)); \ + PICO_FREE(heap->top); \ + } \ + heap->top = newTop; \ + heap->size++; \ + } \ + if (heap->n == 1) { \ + memcpy(&heap->top[1], el, sizeof(type)); \ + return 0; \ + } \ + for (i = heap->n; ((i > 1) && (heap->top[i / 2].orderby > el->orderby)); i /= 2) { \ + memcpy(&heap->top[i], &heap->top[i / 2], sizeof(type)); \ + } \ + memcpy(&heap->top[i], el, sizeof(type)); \ + return 0; \ + } \ + static inline int heap_peek(struct heap_ ## type *heap, type * first) \ + { \ + type *last; \ + uint32_t i, child; \ + if(heap->n == 0) { \ + return -1; \ + } \ + memcpy(first, &heap->top[1], sizeof(type)); \ + last = &heap->top[heap->n--]; \ + for(i = 1; (i * 2u) <= heap->n; i = child) { \ + child = 2u * i; \ + if ((child != heap->n) && \ + (heap->top[child + 1]).orderby \ + < (heap->top[child]).orderby) \ + child++; \ + if (last->orderby > \ + heap->top[child].orderby) \ + memcpy(&heap->top[i], &heap->top[child], \ + sizeof(type)); \ + else \ + break; \ + } \ + memcpy(&heap->top[i], last, sizeof(type)); \ + return 0; \ + } \ + static inline type *heap_first(heap_ ## type * heap) \ + { \ + if (heap->n == 0) \ + return NULL; \ + return &heap->top[1]; \ + } \ + static inline heap_ ## type *heap_init(void) \ + { \ + heap_ ## type * p = (heap_ ## type *)PICO_ZALLOC(sizeof(heap_ ## type)); \ + return p; \ + } \ + /*static inline void heap_destroy(heap_ ## type * h) \ + { \ + PICO_FREE(h->top); \ + PICO_FREE(h); \ + } \*/ + + diff --git a/ext/picotcp/include/pico_addressing.h b/ext/picotcp/include/pico_addressing.h new file mode 100644 index 0000000..de62281 --- /dev/null +++ b/ext/picotcp/include/pico_addressing.h @@ -0,0 +1,52 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#ifndef INCLUDE_PICO_ADDRESSING +#define INCLUDE_PICO_ADDRESSING + +#include "pico_config.h" + +PACKED_STRUCT_DEF pico_ip4 +{ + uint32_t addr; +}; + +PACKED_STRUCT_DEF pico_ip6 +{ + uint8_t addr[16]; +}; + +union pico_address +{ + struct pico_ip4 ip4; + struct pico_ip6 ip6; +}; + +PACKED_STRUCT_DEF pico_eth +{ + uint8_t addr[6]; + uint8_t padding[2]; +}; + +extern const uint8_t PICO_ETHADDR_ALL[]; + + +PACKED_STRUCT_DEF pico_trans +{ + uint16_t sport; + uint16_t dport; + +}; + +/* Here are some protocols. */ +#define PICO_PROTO_IPV4 0 +#define PICO_PROTO_ICMP4 1 +#define PICO_PROTO_IGMP 2 +#define PICO_PROTO_TCP 6 +#define PICO_PROTO_UDP 17 +#define PICO_PROTO_IPV6 41 +#define PICO_PROTO_ICMP6 58 + +#endif diff --git a/ext/picotcp/include/pico_config.h b/ext/picotcp/include/pico_config.h new file mode 100644 index 0000000..3e5047f --- /dev/null +++ b/ext/picotcp/include/pico_config.h @@ -0,0 +1,234 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#include "pico_defines.h" +#ifndef INCLUDE_PICO_CONFIG +#define INCLUDE_PICO_CONFIG +#ifndef __KERNEL__ +#include +#include +#include +#else +#include +#endif + +#if defined __IAR_SYSTEMS_ICC__ || defined ATOP +# define PACKED_STRUCT_DEF __packed struct +# define PEDANTIC_STRUCT_DEF __packed struct +# define PACKED_UNION_DEF __packed union +# define WEAK +#elif defined __WATCOMC__ +# define PACKED_STRUCT_DEF _Packed struct +# define PEDANTIC_STRUCT_DEF struct +# define PACKED_UNION_DEF _Packed union +# define WEAK +#else +# define PACKED_STRUCT_DEF struct __attribute__((packed)) +# define PEDANTIC_STRUCT_DEF struct +# define PACKED_UNION_DEF union /* Sane compilers do not require packed unions */ +# define WEAK __attribute__((weak)) +# ifdef __GNUC__ +# define GCC_VERSION (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__) +# if ((GCC_VERSION >= 40800)) +# define BYTESWAP_GCC +# endif +# endif +#endif + +#ifdef PICO_BIGENDIAN + +# define PICO_IDETH_IPV4 0x0800 +# define PICO_IDETH_ARP 0x0806 +# define PICO_IDETH_IPV6 0x86DD + +# define PICO_ARP_REQUEST 0x0001 +# define PICO_ARP_REPLY 0x0002 +# define PICO_ARP_HTYPE_ETH 0x0001 + +#define short_be(x) (x) +#define long_be(x) (x) +#define long_long_be(x) (x) + +static inline uint16_t short_from(void *_p) +{ + unsigned char *p = (unsigned char *)_p; + uint16_t r, p0, p1; + p0 = p[0]; + p1 = p[1]; + r = (p0 << 8) + p1; + return r; +} + +static inline uint32_t long_from(void *_p) +{ + unsigned char *p = (unsigned char *)_p; + uint32_t r, p0, p1, p2, p3; + p0 = p[0]; + p1 = p[1]; + p2 = p[2]; + p3 = p[3]; + r = (p0 << 24) + (p1 << 16) + (p2 << 8) + p3; + return r; +} + +#else + +static inline uint16_t short_from(void *_p) +{ + unsigned char *p = (unsigned char *)_p; + uint16_t r, _p0, _p1; + _p0 = p[0]; + _p1 = p[1]; + r = (uint16_t)((_p1 << 8u) + _p0); + return r; +} + +static inline uint32_t long_from(void *_p) +{ + unsigned char *p = (unsigned char *)_p; + uint32_t r, _p0, _p1, _p2, _p3; + _p0 = p[0]; + _p1 = p[1]; + _p2 = p[2]; + _p3 = p[3]; + r = (_p3 << 24) + (_p2 << 16) + (_p1 << 8) + _p0; + return r; +} + + +# define PICO_IDETH_IPV4 0x0008 +# define PICO_IDETH_ARP 0x0608 +# define PICO_IDETH_IPV6 0xDD86 + +# define PICO_ARP_REQUEST 0x0100 +# define PICO_ARP_REPLY 0x0200 +# define PICO_ARP_HTYPE_ETH 0x0100 + +# ifndef BYTESWAP_GCC +static inline uint16_t short_be(uint16_t le) +{ + return (uint16_t)(((le & 0xFFu) << 8) | ((le >> 8u) & 0xFFu)); +} + +static inline uint32_t long_be(uint32_t le) +{ + uint8_t *b = (uint8_t *)≤ + uint32_t be = 0; + uint32_t b0, b1, b2; + b0 = b[0]; + b1 = b[1]; + b2 = b[2]; + be = b[3] + (b2 << 8) + (b1 << 16) + (b0 << 24); + return be; +} +static inline uint64_t long_long_be(uint64_t le) +{ + uint8_t *b = (uint8_t *)≤ + uint64_t be = 0; + uint64_t b0, b1, b2, b3, b4, b5, b6; + b0 = b[0]; + b1 = b[1]; + b2 = b[2]; + b3 = b[3]; + b4 = b[4]; + b5 = b[5]; + b6 = b[6]; + be = b[7] + (b6 << 8) + (b5 << 16) + (b4 << 24) + (b3 << 32) + (b2 << 40) + (b1 << 48) + (b0 << 56); + return be; +} +# else +/* + extern uint32_t __builtin_bswap32(uint32_t); + extern uint16_t __builtin_bswap16(uint16_t); + extern uint64_t __builtin_bswap64(uint64_t); + */ + +static inline uint32_t long_be(uint32_t le) +{ + return (uint32_t)__builtin_bswap32(le); +} + +static inline uint16_t short_be(uint16_t le) +{ + return (uint16_t)__builtin_bswap16(le); +} + +static inline uint64_t long_long_be(uint64_t le) +{ + return (uint64_t)__builtin_bswap64(le); +} + +# endif /* BYTESWAP_GCC */ +#endif + + +/* Mockables */ +#if defined UNIT_TEST +# define MOCKABLE __attribute__((weak)) +#else +# define MOCKABLE +#endif + +#include "pico_constants.h" +#include "pico_mm.h" + +#define IGNORE_PARAMETER(x) ((void)x) + +#define PICO_MEM_DEFAULT_SLAB_SIZE 1600 +#define PICO_MEM_PAGE_SIZE 4096 +#define PICO_MEM_PAGE_LIFETIME 100 +#define PICO_MIN_HEAP_SIZE 600 +#define PICO_MIN_SLAB_SIZE 1200 +#define PICO_MAX_SLAB_SIZE 1600 +#define PICO_MEM_MINIMUM_OBJECT_SIZE 4 + + +/*** *** *** *** *** *** *** + *** PLATFORM SPECIFIC *** + *** *** *** *** *** *** ***/ +#if defined PICO_PORT_CUSTOM +# include "pico_port.h" +#elif defined CORTEX_M4_HARDFLOAT +# include "arch/pico_cortex_m.h" +#elif defined CORTEX_M4_SOFTFLOAT +# include "arch/pico_cortex_m.h" +#elif defined CORTEX_M3 +# include "arch/pico_cortex_m.h" +#elif defined PIC24 +# include "arch/pico_pic24.h" +#elif defined MSP430 +# include "arch/pico_msp430.h" +#elif defined MBED_TEST +# include "arch/pico_mbed.h" +#elif defined AVR +# include "arch/pico_avr.h" +#elif defined ARM9 +# include "arch/pico_arm9.h" +#elif defined ESP8266 +# include "arch/pico_esp8266.h" +#elif defined MT7681 +# include "arch/pico_generic_gcc.h" +#elif defined FAULTY +# include "../test/pico_faulty.h" +#elif defined ARCHNONE +# include "arch/pico_none.h" +#elif defined GENERIC +# include "arch/pico_generic_gcc.h" +#elif defined __KERNEL__ +# include "arch/pico_linux.h" +/* #elif defined ... */ +#else +# include "arch/pico_posix.h" +#endif + +#ifdef PICO_SUPPORT_MM +#define PICO_ZALLOC(x) pico_mem_zalloc(x) +#define PICO_FREE(x) pico_mem_free(x) +#else +#define PICO_ZALLOC(x) pico_zalloc(x) +#define PICO_FREE(x) pico_free(x) +#endif /* PICO_SUPPORT_MM */ + +#endif diff --git a/ext/picotcp/include/pico_constants.h b/ext/picotcp/include/pico_constants.h new file mode 100644 index 0000000..f043408 --- /dev/null +++ b/ext/picotcp/include/pico_constants.h @@ -0,0 +1,54 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#ifndef INCLUDE_PICO_CONST +#define INCLUDE_PICO_CONST +/* Included from pico_config.h */ + +/** Non-endian dependant constants */ +#define PICO_SIZE_IP4 4 +#define PICO_SIZE_IP6 16 +#define PICO_SIZE_ETH 6 +#define PICO_SIZE_TRANS 8 + +/** Endian-dependant constants **/ +typedef uint64_t pico_time; +extern volatile uint64_t pico_tick; + + +/*** *** *** *** *** *** *** + *** ARP CONFIG *** + *** *** *** *** *** *** ***/ + +#include "pico_addressing.h" + +/* Maximum amount of accepted ARP requests per burst interval */ +#define PICO_ARP_MAX_RATE 1 +/* Duration of the burst interval in milliseconds */ +#define PICO_ARP_INTERVAL 1000 + +/* Add well-known host numbers here. (bigendian constants only beyond this point) */ +#define PICO_IP4_ANY (0x00000000U) +#define PICO_IP4_BCAST (0xffffffffU) + +/* defined in modules/pico_ipv6.c */ +#ifdef PICO_SUPPORT_IPV6 +extern const uint8_t PICO_IPV6_ANY[PICO_SIZE_IP6]; +#endif + +static inline uint32_t pico_hash(const void *buf, uint32_t size) +{ + uint32_t hash = 5381; + uint32_t i; + const uint8_t *ptr = (const uint8_t *)buf; + for(i = 0; i < size; i++) + hash = ((hash << 5) + hash) + ptr[i]; /* hash * 33 + char */ + return hash; +} + +/* Debug */ +/* #define PICO_SUPPORT_DEBUG_MEMORY */ +/* #define PICO_SUPPORT_DEBUG_TOOLS */ +#endif diff --git a/ext/picotcp/include/pico_device.h b/ext/picotcp/include/pico_device.h new file mode 100644 index 0000000..a2d03ec --- /dev/null +++ b/ext/picotcp/include/pico_device.h @@ -0,0 +1,54 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#ifndef INCLUDE_PICO_DEVICE +#define INCLUDE_PICO_DEVICE +#include "pico_queue.h" +#include "pico_frame.h" +#include "pico_addressing.h" +#include "pico_tree.h" +extern struct pico_tree Device_tree; +#include "pico_ipv6_nd.h" +#define MAX_DEVICE_NAME 16 + + +struct pico_ethdev { + struct pico_eth mac; +}; + +struct pico_device { + char name[MAX_DEVICE_NAME]; + uint32_t hash; + uint32_t overhead; + uint32_t mtu; + struct pico_ethdev *eth; /* Null if non-ethernet */ + struct pico_queue *q_in; + struct pico_queue *q_out; + int (*link_state)(struct pico_device *self); + int (*send)(struct pico_device *self, void *buf, int len); /* Send function. Return 0 if busy */ + int (*poll)(struct pico_device *self, int loop_score); + void (*destroy)(struct pico_device *self); + int (*dsr)(struct pico_device *self, int loop_score); + int __serving_interrupt; + /* used to signal the upper layer the number of events arrived since the last processing */ + volatile int eventCnt; + #ifdef PICO_SUPPORT_IPV6 + struct pico_nd_hostvars hostvars; + #endif +}; + + +int pico_device_init(struct pico_device *dev, const char *name, uint8_t *mac); +void pico_device_destroy(struct pico_device *dev); +int pico_devices_loop(int loop_score, int direction); +struct pico_device*pico_get_device(const char*name); +int32_t pico_device_broadcast(struct pico_frame *f); +int pico_device_link_state(struct pico_device *dev); +int pico_device_ipv6_random_ll(struct pico_device *dev); +#ifdef PICO_SUPPORT_IPV6 +struct pico_ipv6_link *pico_ipv6_link_add_local(struct pico_device *dev, const struct pico_ip6 *prefix); +#endif + +#endif diff --git a/ext/picotcp/include/pico_eth.h b/ext/picotcp/include/pico_eth.h new file mode 100644 index 0000000..89846c7 --- /dev/null +++ b/ext/picotcp/include/pico_eth.h @@ -0,0 +1,21 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#ifndef INCLUDE_PICO_ETH +#define INCLUDE_PICO_ETH +#include "pico_addressing.h" +#include "pico_ipv4.h" +#include "pico_ipv6.h" + + +PACKED_STRUCT_DEF pico_eth_hdr { + uint8_t daddr[6]; + uint8_t saddr[6]; + uint16_t proto; +}; + +#define PICO_SIZE_ETHHDR 14 + +#endif diff --git a/ext/picotcp/include/pico_frame.h b/ext/picotcp/include/pico_frame.h new file mode 100644 index 0000000..18b2d4d --- /dev/null +++ b/ext/picotcp/include/pico_frame.h @@ -0,0 +1,122 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#ifndef INCLUDE_PICO_FRAME +#define INCLUDE_PICO_FRAME +#include "pico_config.h" + + +#define PICO_FRAME_FLAG_BCAST (0x01) +#define PICO_FRAME_FLAG_EXT_BUFFER (0x02) +#define PICO_FRAME_FLAG_EXT_USAGE_COUNTER (0x04) +#define PICO_FRAME_FLAG_SACKED (0x80) +#define IS_BCAST(f) ((f->flags & PICO_FRAME_FLAG_BCAST) == PICO_FRAME_FLAG_BCAST) + + +struct pico_socket; + + +struct pico_frame { + + /* Connector for queues */ + struct pico_frame *next; + + /* Start of the whole buffer, total frame length. */ + unsigned char *buffer; + uint32_t buffer_len; + + /* For outgoing packets: this is the meaningful buffer. */ + unsigned char *start; + uint32_t len; + + /* Pointer to usage counter */ + uint32_t *usage_count; + + /* Pointer to protocol headers */ + uint8_t *datalink_hdr; + + uint8_t *net_hdr; + uint16_t net_len; + uint8_t *transport_hdr; + uint16_t transport_len; + uint8_t *app_hdr; + uint16_t app_len; + + /* Pointer to the phisical device this packet belongs to. + * Should be valid in both routing directions + */ + struct pico_device *dev; + + pico_time timestamp; + + /* Failures due to bad datalink addressing. */ + uint16_t failure_count; + + /* Protocol over IP */ + uint8_t proto; + + /* PICO_FRAME_FLAG_* */ + uint8_t flags; + + /* Pointer to payload */ + unsigned char *payload; + uint16_t payload_len; + +#if defined(PICO_SUPPORT_IPV4FRAG) || defined(PICO_SUPPORT_IPV6FRAG) + /* Payload fragmentation info */ + uint16_t frag; +#endif + + /* Pointer to socket */ + struct pico_socket *sock; + + /* Pointer to transport info, used to store remote UDP endpoint (IP + port) */ + void *info; + + /*Priority. "best-effort" priority, the default value is 0. Priority can be in between -10 and +10*/ + int8_t priority; + uint8_t transport_flags_saved; + + /* Callback to notify listener when the buffer has been discarded */ + void (*notify_free)(uint8_t *); + + uint8_t send_ttl; /* Special TTL/HOPS value, 0 = auto assign */ + uint8_t send_tos; /* Type of service */ +}; + +/** frame alloc/dealloc/copy **/ +void pico_frame_discard(struct pico_frame *f); +struct pico_frame *pico_frame_copy(struct pico_frame *f); +struct pico_frame *pico_frame_deepcopy(struct pico_frame *f); +struct pico_frame *pico_frame_alloc(uint32_t size); +int pico_frame_grow(struct pico_frame *f, uint32_t size); +struct pico_frame *pico_frame_alloc_skeleton(uint32_t size, int ext_buffer); +int pico_frame_skeleton_set_buffer(struct pico_frame *f, void *buf); +uint16_t pico_checksum(void *inbuf, uint32_t len); +uint16_t pico_dualbuffer_checksum(void *b1, uint32_t len1, void *b2, uint32_t len2); + +static inline int pico_is_digit(char c) +{ + if (c < '0' || c > '9') + return 0; + + return 1; +} + +static inline int pico_is_hex(char c) +{ + if (c >= '0' && c <= '9') + return 1; + + if (c >= 'a' && c <= 'f') + return 1; + + if (c >= 'A' && c <= 'F') + return 1; + + return 0; +} + +#endif diff --git a/ext/picotcp/include/pico_md5.h b/ext/picotcp/include/pico_md5.h new file mode 100644 index 0000000..0ba89c4 --- /dev/null +++ b/ext/picotcp/include/pico_md5.h @@ -0,0 +1,17 @@ +/********************************************************************* + * PicoTCP. Copyright (c) 2015 Altran Intelligent Systems. Some rights reserved. + * See LICENSE and COPYING for usage. + * + * Authors: Daniele Lacamera + * *********************************************************************/ + +#ifndef PICO_MD5_INCLUDE +#define PICO_MD5_INCLUDE + +#include +#include + +void pico_md5sum(uint8_t *dst, const uint8_t *src, size_t len); +void pico_register_md5sum(void (*md5)(uint8_t *, const uint8_t *, size_t)); + +#endif /* PICO_MD5_INCLUDE */ diff --git a/ext/picotcp/include/pico_module_eth.h b/ext/picotcp/include/pico_module_eth.h new file mode 100644 index 0000000..c86680d --- /dev/null +++ b/ext/picotcp/include/pico_module_eth.h @@ -0,0 +1,33 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#ifndef PICO_MODULE_IPV4_H +#define PICO_MODULE_IPV4_H + +struct pico_arp_entry { + struct eth dest; +#ifdef PICO_CONFIG_IPV4 + struct ipv4 addr_ipv4; +#endif + RB_ENTRY(pico_arp_entry) node; +}; + +/* Configured device */ +struct pico_eth_link { + struct pico_device *dev; + struct eth address; + struct eth netmask; + RB_ENTRY(pico_eth_link) node; +}; + +#ifndef IS_MODULE_ETH +# define _mod extern +#else +# define _mod +#endif +_mod struct pico_module pico_module_eth; +#undef _mod + +#endif diff --git a/ext/picotcp/include/pico_protocol.h b/ext/picotcp/include/pico_protocol.h new file mode 100644 index 0000000..fd7c122 --- /dev/null +++ b/ext/picotcp/include/pico_protocol.h @@ -0,0 +1,95 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#ifndef INCLUDE_PICO_PROTOCOL +#define INCLUDE_PICO_PROTOCOL +#include "pico_config.h" +#include "pico_queue.h" + +#define PICO_LOOP_DIR_IN 1 +#define PICO_LOOP_DIR_OUT 2 + +enum pico_layer { + PICO_LAYER_DATALINK = 2, /* Ethernet only. */ + PICO_LAYER_NETWORK = 3, /* IPv4, IPv6, ARP. Arp is there because it communicates with L2 */ + PICO_LAYER_TRANSPORT = 4, /* UDP, TCP, ICMP */ + PICO_LAYER_SOCKET = 5 /* Socket management */ +}; + +enum pico_err_e { + PICO_ERR_NOERR = 0, + PICO_ERR_EPERM = 1, + PICO_ERR_ENOENT = 2, + /* ... */ + PICO_ERR_EINTR = 4, + PICO_ERR_EIO = 5, + PICO_ERR_ENXIO = 6, + /* ... */ + PICO_ERR_EAGAIN = 11, + PICO_ERR_ENOMEM = 12, + PICO_ERR_EACCESS = 13, + PICO_ERR_EFAULT = 14, + /* ... */ + PICO_ERR_EBUSY = 16, + PICO_ERR_EEXIST = 17, + /* ... */ + PICO_ERR_EINVAL = 22, + /* ... */ + PICO_ERR_ENONET = 64, + /* ... */ + PICO_ERR_EPROTO = 71, + /* ... */ + PICO_ERR_ENOPROTOOPT = 92, + PICO_ERR_EPROTONOSUPPORT = 93, + /* ... */ + PICO_ERR_EOPNOTSUPP = 95, + PICO_ERR_EADDRINUSE = 98, + PICO_ERR_EADDRNOTAVAIL = 99, + PICO_ERR_ENETDOWN = 100, + PICO_ERR_ENETUNREACH = 101, + /* ... */ + PICO_ERR_ECONNRESET = 104, + /* ... */ + PICO_ERR_EISCONN = 106, + PICO_ERR_ENOTCONN = 107, + PICO_ERR_ESHUTDOWN = 108, + /* ... */ + PICO_ERR_ETIMEDOUT = 110, + PICO_ERR_ECONNREFUSED = 111, + PICO_ERR_EHOSTDOWN = 112, + PICO_ERR_EHOSTUNREACH = 113, +}; + +typedef enum pico_err_e pico_err_t; +extern volatile pico_err_t pico_err; + +#define IS_IPV6(f) (f && f->net_hdr && ((((uint8_t *)(f->net_hdr))[0] & 0xf0) == 0x60)) +#define IS_IPV4(f) (f && f->net_hdr && ((((uint8_t *)(f->net_hdr))[0] & 0xf0) == 0x40)) + +#define MAX_PROTOCOL_NAME 16 + +struct pico_protocol { + char name[MAX_PROTOCOL_NAME]; + uint32_t hash; + enum pico_layer layer; + uint16_t proto_number; + struct pico_queue *q_in; + struct pico_queue *q_out; + struct pico_frame *(*alloc)(struct pico_protocol *self, uint16_t size); /* Frame allocation. */ + int (*push)(struct pico_protocol *self, struct pico_frame *p); /* Push function, for active outgoing pkts from above */ + int (*process_out)(struct pico_protocol *self, struct pico_frame *p); /* Send loop. */ + int (*process_in)(struct pico_protocol *self, struct pico_frame *p); /* Recv loop. */ + uint16_t (*get_mtu)(struct pico_protocol *self); +}; + +int pico_protocols_loop(int loop_score); +void pico_protocol_init(struct pico_protocol *p); + +int pico_protocol_datalink_loop(int loop_score, int direction); +int pico_protocol_network_loop(int loop_score, int direction); +int pico_protocol_transport_loop(int loop_score, int direction); +int pico_protocol_socket_loop(int loop_score, int direction); + +#endif diff --git a/ext/picotcp/include/pico_queue.h b/ext/picotcp/include/pico_queue.h new file mode 100644 index 0000000..6cbb979 --- /dev/null +++ b/ext/picotcp/include/pico_queue.h @@ -0,0 +1,166 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#ifndef INCLUDE_PICO_QUEUE +#define INCLUDE_PICO_QUEUE +#include "pico_config.h" +#include "pico_frame.h" + +#define Q_LIMIT 0 + +#ifndef NULL +#define NULL ((void *)0) +#endif + +void *pico_mutex_init(void); +void pico_mutex_deinit(void *mutex); +void pico_mutex_lock(void *mutex); +int pico_mutex_lock_timeout(void *mutex, int timeout); +void pico_mutex_unlock(void *mutex); +void pico_mutex_unlock_ISR(void *mutex); + +struct pico_queue { + uint32_t frames; + uint32_t size; + uint32_t max_frames; + uint32_t max_size; + struct pico_frame *head; + struct pico_frame *tail; +#ifdef PICO_SUPPORT_MUTEX + void *mutex; +#endif + uint8_t shared; + uint16_t overhead; +}; + +#ifdef PICO_SUPPORT_MUTEX +#define PICOTCP_MUTEX_LOCK(x) { \ + if (x == NULL) \ + x = pico_mutex_init(); \ + pico_mutex_lock(x); \ +} +#define PICOTCP_MUTEX_UNLOCK(x) pico_mutex_unlock(x) +#define PICOTCP_MUTEX_DEL(x) pico_mutex_deinit(x) + +#else +#define PICOTCP_MUTEX_LOCK(x) do {} while(0) +#define PICOTCP_MUTEX_UNLOCK(x) do {} while(0) +#define PICOTCP_MUTEX_DEL(x) do {} while(0) +#endif + +#ifdef PICO_SUPPORT_DEBUG_TOOLS +static void debug_q(struct pico_queue *q) +{ + struct pico_frame *p = q->head; + dbg("%d: ", q->frames); + while(p) { + dbg("(%p)-->", p); + p = p->next; + } + dbg("X\n"); +} + +#else + +#define debug_q(x) do {} while(0) +#endif + +static inline int32_t pico_enqueue(struct pico_queue *q, struct pico_frame *p) +{ + if ((q->max_frames) && (q->max_frames <= q->frames)) + return -1; + +#if (Q_LIMIT != 0) + if ((Q_LIMIT < p->buffer_len + q->size)) + return -1; + +#endif + + if ((q->max_size) && (q->max_size < (p->buffer_len + q->size))) + return -1; + + if (q->shared) + PICOTCP_MUTEX_LOCK(q->mutex); + + p->next = NULL; + if (!q->head) { + q->head = p; + q->tail = p; + q->size = 0; + q->frames = 0; + } else { + q->tail->next = p; + q->tail = p; + } + + q->size += p->buffer_len + q->overhead; + q->frames++; + debug_q(q); + + if (q->shared) + PICOTCP_MUTEX_UNLOCK(q->mutex); + + return (int32_t)q->size; +} + +static inline struct pico_frame *pico_dequeue(struct pico_queue *q) +{ + struct pico_frame *p = q->head; + if (!p) + return NULL; + + if (q->frames < 1) + return NULL; + + if (q->shared) + PICOTCP_MUTEX_LOCK(q->mutex); + + q->head = p->next; + q->frames--; + q->size -= p->buffer_len - q->overhead; + if (q->head == NULL) + q->tail = NULL; + + debug_q(q); + + p->next = NULL; + if (q->shared) + PICOTCP_MUTEX_UNLOCK(q->mutex); + + return p; +} + +static inline struct pico_frame *pico_queue_peek(struct pico_queue *q) +{ + struct pico_frame *p = q->head; + if (q->frames < 1) + return NULL; + + debug_q(q); + return p; +} + +static inline void pico_queue_deinit(struct pico_queue *q) +{ + if (q->shared) { + PICOTCP_MUTEX_DEL(q->mutex); + } +} + +static inline void pico_queue_empty(struct pico_queue *q) +{ + struct pico_frame *p = pico_dequeue(q); + while(p) { + pico_frame_discard(p); + p = pico_dequeue(q); + } +} + +static inline void pico_queue_protect(struct pico_queue *q) +{ + q->shared = 1; +} + +#endif diff --git a/ext/picotcp/include/pico_socket.h b/ext/picotcp/include/pico_socket.h new file mode 100644 index 0000000..784a722 --- /dev/null +++ b/ext/picotcp/include/pico_socket.h @@ -0,0 +1,261 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#ifndef INCLUDE_PICO_SOCKET +#define INCLUDE_PICO_SOCKET +#include "pico_queue.h" +#include "pico_addressing.h" +#include "pico_config.h" +#include "pico_protocol.h" +#include "pico_tree.h" + +#ifdef __linux__ + #define PICO_DEFAULT_SOCKETQ (16 * 1024) /* Linux host, so we want full throttle */ +#else + #define PICO_DEFAULT_SOCKETQ (6 * 1024) /* seems like an acceptable default for small embedded systems */ +#endif + +#define PICO_SHUT_RD 1 +#define PICO_SHUT_WR 2 +#define PICO_SHUT_RDWR 3 + +#ifdef PICO_SUPPORT_IPV4 +# define IS_SOCK_IPV4(s) ((s->net == &pico_proto_ipv4)) +#else +# define IS_SOCK_IPV4(s) (0) +#endif + +#ifdef PICO_SUPPORT_IPV6 +# define IS_SOCK_IPV6(s) ((s->net == &pico_proto_ipv6)) +#else +# define IS_SOCK_IPV6(s) (0) +#endif + + +struct pico_sockport +{ + struct pico_tree socks; /* how you make the connection ? */ + uint16_t number; + uint16_t proto; +}; + + +struct pico_socket { + struct pico_protocol *proto; + struct pico_protocol *net; + + union pico_address local_addr; + union pico_address remote_addr; + + uint16_t local_port; + uint16_t remote_port; + + struct pico_queue q_in; + struct pico_queue q_out; + + void (*wakeup)(uint16_t ev, struct pico_socket *s); + + +#ifdef PICO_SUPPORT_TCP + /* For the TCP backlog queue */ + struct pico_socket *backlog; + struct pico_socket *next; + struct pico_socket *parent; + uint16_t max_backlog; + uint16_t number_of_pending_conn; +#endif +#ifdef PICO_SUPPORT_MCAST + struct pico_tree *MCASTListen; +#ifdef PICO_SUPPORT_IPV6 + struct pico_tree *MCASTListen_ipv6; +#endif +#endif + uint16_t ev_pending; + + struct pico_device *dev; + + /* Private field. */ + int id; + uint16_t state; + uint16_t opt_flags; + pico_time timestamp; + void *priv; +}; + +struct pico_remote_endpoint { + union pico_address remote_addr; + uint16_t remote_port; +}; + + +struct pico_ip_mreq { + union pico_address mcast_group_addr; + union pico_address mcast_link_addr; +}; +struct pico_ip_mreq_source { + union pico_address mcast_group_addr; + union pico_address mcast_source_addr; + union pico_address mcast_link_addr; +}; + + +#define PICO_SOCKET_STATE_UNDEFINED 0x0000u +#define PICO_SOCKET_STATE_SHUT_LOCAL 0x0001u +#define PICO_SOCKET_STATE_SHUT_REMOTE 0x0002u +#define PICO_SOCKET_STATE_BOUND 0x0004u +#define PICO_SOCKET_STATE_CONNECTED 0x0008u +#define PICO_SOCKET_STATE_CLOSING 0x0010u +#define PICO_SOCKET_STATE_CLOSED 0x0020u + +# define PICO_SOCKET_STATE_TCP 0xFF00u +# define PICO_SOCKET_STATE_TCP_UNDEF 0x00FFu +# define PICO_SOCKET_STATE_TCP_CLOSED 0x0100u +# define PICO_SOCKET_STATE_TCP_LISTEN 0x0200u +# define PICO_SOCKET_STATE_TCP_SYN_SENT 0x0300u +# define PICO_SOCKET_STATE_TCP_SYN_RECV 0x0400u +# define PICO_SOCKET_STATE_TCP_ESTABLISHED 0x0500u +# define PICO_SOCKET_STATE_TCP_CLOSE_WAIT 0x0600u +# define PICO_SOCKET_STATE_TCP_LAST_ACK 0x0700u +# define PICO_SOCKET_STATE_TCP_FIN_WAIT1 0x0800u +# define PICO_SOCKET_STATE_TCP_FIN_WAIT2 0x0900u +# define PICO_SOCKET_STATE_TCP_CLOSING 0x0a00u +# define PICO_SOCKET_STATE_TCP_TIME_WAIT 0x0b00u +# define PICO_SOCKET_STATE_TCP_ARRAYSIZ 0x0cu + + +/* Socket options */ +# define PICO_TCP_NODELAY 1 +# define PICO_SOCKET_OPT_TCPNODELAY 0x0000u + +# define PICO_IP_MULTICAST_EXCLUDE 0 +# define PICO_IP_MULTICAST_INCLUDE 1 +# define PICO_IP_MULTICAST_IF 32 +# define PICO_IP_MULTICAST_TTL 33 +# define PICO_IP_MULTICAST_LOOP 34 +# define PICO_IP_ADD_MEMBERSHIP 35 +# define PICO_IP_DROP_MEMBERSHIP 36 +# define PICO_IP_UNBLOCK_SOURCE 37 +# define PICO_IP_BLOCK_SOURCE 38 +# define PICO_IP_ADD_SOURCE_MEMBERSHIP 39 +# define PICO_IP_DROP_SOURCE_MEMBERSHIP 40 + +# define PICO_SOCKET_OPT_MULTICAST_LOOP 1 +# define PICO_SOCKET_OPT_KEEPIDLE 4 +# define PICO_SOCKET_OPT_KEEPINTVL 5 +# define PICO_SOCKET_OPT_KEEPCNT 6 + +#define PICO_SOCKET_OPT_LINGER 13 + +# define PICO_SOCKET_OPT_RCVBUF 52 +# define PICO_SOCKET_OPT_SNDBUF 53 + + +/* Constants */ +# define PICO_IP_DEFAULT_MULTICAST_TTL 1 +# define PICO_IP_DEFAULT_MULTICAST_LOOP 1 + +#define PICO_SOCKET_TIMEOUT 5000u /* 5 seconds */ +#define PICO_SOCKET_LINGER_TIMEOUT 3000u /* 3 seconds */ +#define PICO_SOCKET_BOUND_TIMEOUT 30000u /* 30 seconds */ + +#define PICO_SOCKET_SHUTDOWN_WRITE 0x01u +#define PICO_SOCKET_SHUTDOWN_READ 0x02u +#define TCPSTATE(s) ((s)->state & PICO_SOCKET_STATE_TCP) + +#define PICO_SOCK_EV_RD 1u +#define PICO_SOCK_EV_WR 2u +#define PICO_SOCK_EV_CONN 4u +#define PICO_SOCK_EV_CLOSE 8u +#define PICO_SOCK_EV_FIN 0x10u +#define PICO_SOCK_EV_ERR 0x80u + +struct pico_msginfo { + struct pico_device *dev; + uint8_t ttl; + uint8_t tos; +}; + +struct pico_socket *pico_socket_open(uint16_t net, uint16_t proto, void (*wakeup)(uint16_t ev, struct pico_socket *s)); + +int pico_socket_read(struct pico_socket *s, void *buf, int len); +int pico_socket_write(struct pico_socket *s, const void *buf, int len); + +int pico_socket_sendto(struct pico_socket *s, const void *buf, int len, void *dst, uint16_t remote_port); +int pico_socket_sendto_extended(struct pico_socket *s, const void *buf, const int len, + void *dst, uint16_t remote_port, struct pico_msginfo *msginfo); + +int pico_socket_recvfrom(struct pico_socket *s, void *buf, int len, void *orig, uint16_t *local_port); +int pico_socket_recvfrom_extended(struct pico_socket *s, void *buf, int len, void *orig, + uint16_t *remote_port, struct pico_msginfo *msginfo); + +int pico_socket_send(struct pico_socket *s, const void *buf, int len); +int pico_socket_recv(struct pico_socket *s, void *buf, int len); + +int pico_socket_bind(struct pico_socket *s, void *local_addr, uint16_t *port); +int pico_socket_getname(struct pico_socket *s, void *local_addr, uint16_t *port, uint16_t *proto); +int pico_socket_getpeername(struct pico_socket *s, void *remote_addr, uint16_t *port, uint16_t *proto); + +int pico_socket_connect(struct pico_socket *s, const void *srv_addr, uint16_t remote_port); +int pico_socket_listen(struct pico_socket *s, const int backlog); +struct pico_socket *pico_socket_accept(struct pico_socket *s, void *orig, uint16_t *port); +int8_t pico_socket_del(struct pico_socket *s); + +int pico_socket_setoption(struct pico_socket *s, int option, void *value); +int pico_socket_getoption(struct pico_socket *s, int option, void *value); + +int pico_socket_shutdown(struct pico_socket *s, int mode); +int pico_socket_close(struct pico_socket *s); + +struct pico_frame *pico_socket_frame_alloc(struct pico_socket *s, uint16_t len); + +#ifdef PICO_SUPPORT_IPV4 +# define is_sock_ipv4(x) (x->net == &pico_proto_ipv4) +#else +# define is_sock_ipv4(x) (0) +#endif + +#ifdef PICO_SUPPORT_IPV6 +# define is_sock_ipv6(x) (x->net == &pico_proto_ipv6) +#else +# define is_sock_ipv6(x) (0) +#endif + +#ifdef PICO_SUPPORT_UDP +# define is_sock_udp(x) (x->proto == &pico_proto_udp) +#else +# define is_sock_udp(x) (0) +#endif + +#ifdef PICO_SUPPORT_TCP +# define is_sock_tcp(x) (x->proto == &pico_proto_tcp) +#else +# define is_sock_tcp(x) (0) +#endif + +/* Interface towards transport protocol */ +int pico_transport_process_in(struct pico_protocol *self, struct pico_frame *f); +struct pico_socket *pico_socket_clone(struct pico_socket *facsimile); +int8_t pico_socket_add(struct pico_socket *s); +int pico_transport_error(struct pico_frame *f, uint8_t proto, int code); + +/* Socket loop */ +int pico_sockets_loop(int loop_score); +struct pico_socket*pico_sockets_find(uint16_t local, uint16_t remote); +/* Port check */ +int pico_is_port_free(uint16_t proto, uint16_t port, void *addr, void *net); + +struct pico_sockport *pico_get_sockport(uint16_t proto, uint16_t port); + +uint32_t pico_socket_get_mss(struct pico_socket *s); +int pico_socket_set_family(struct pico_socket *s, uint16_t family); + +int pico_count_sockets(uint8_t proto); + +#define PICO_SOCKET_SETOPT_EN(socket, index) (socket->opt_flags |= (1 << index)) +#define PICO_SOCKET_SETOPT_DIS(socket, index) (socket->opt_flags &= (uint16_t) ~(1 << index)) +#define PICO_SOCKET_GETOPT(socket, index) ((socket->opt_flags & (1u << index)) != 0) + + +#endif diff --git a/ext/picotcp/include/pico_socket_multicast.h b/ext/picotcp/include/pico_socket_multicast.h new file mode 100644 index 0000000..44c30c5 --- /dev/null +++ b/ext/picotcp/include/pico_socket_multicast.h @@ -0,0 +1,10 @@ +#ifndef PICO_SOCKET_MULTICAST_H +#define PICO_SOCKET_MULTICAST_H +int pico_socket_mcast_filter(struct pico_socket *s, union pico_address *mcast_group, union pico_address *src); +void pico_multicast_delete(struct pico_socket *s); +int pico_setsockopt_mcast(struct pico_socket *s, int option, void *value); +int pico_getsockopt_mcast(struct pico_socket *s, int option, void *value); +int pico_udp_get_mc_ttl(struct pico_socket *s, uint8_t *ttl); +int pico_udp_set_mc_ttl(struct pico_socket *s, void *_ttl); + +#endif diff --git a/ext/picotcp/include/pico_stack.h b/ext/picotcp/include/pico_stack.h new file mode 100644 index 0000000..ab34df6 --- /dev/null +++ b/ext/picotcp/include/pico_stack.h @@ -0,0 +1,85 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#ifndef INCLUDE_PICO_STACK +#define INCLUDE_PICO_STACK +#include "pico_config.h" +#include "pico_frame.h" + +#define PICO_MAX_TIMERS 20 + +#define PICO_ETH_MRU (1514u) +#define PICO_IP_MRU (1500u) + +/* ===== RECEIVING FUNCTIONS (from dev up to socket) ===== */ + +/* TRANSPORT LEVEL */ +/* interface towards network */ +int32_t pico_transport_receive(struct pico_frame *f, uint8_t proto); + +/* NETWORK LEVEL */ +/* interface towards ethernet */ +int32_t pico_network_receive(struct pico_frame *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. + * The zerocopy version will associate the current buffer to the newly created frame. + * Warning: the buffer used in the zerocopy version MUST have been allocated using PICO_ZALLOC() + */ +int32_t pico_stack_recv(struct pico_device *dev, uint8_t *buffer, uint32_t len); +int32_t pico_stack_recv_zerocopy(struct pico_device *dev, uint8_t *buffer, uint32_t len); +int32_t pico_stack_recv_zerocopy_ext_buffer(struct pico_device *dev, uint8_t *buffer, uint32_t len); +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)); + +/* ===== SENDING FUNCTIONS (from socket down to dev) ===== */ + +int32_t pico_network_send(struct pico_frame *f); +int32_t pico_sendto_dev(struct pico_frame *f); + +#ifdef PICO_SUPPORT_ETH +int32_t pico_ethernet_send(struct pico_frame *f); + +/* The pico_ethernet_receive() function is used by + * those devices supporting ETH in order to push packets up + * into the stack. + */ +/* DATALINK LEVEL */ +int32_t pico_ethernet_receive(struct pico_frame *f); +#else +/* When ETH is not supported by the stack... */ +# define pico_ethernet_send(f) (-1) +# define pico_ethernet_receive(f) (-1) +#endif + +/* ----- Initialization ----- */ +int pico_stack_init(void); + +/* ----- Loop Function. ----- */ +void pico_stack_tick(void); +void pico_stack_loop(void); + +/* ---- Notifications for stack errors */ +int pico_notify_socket_unreachable(struct pico_frame *f); +int pico_notify_proto_unreachable(struct pico_frame *f); +int pico_notify_dest_unreachable(struct pico_frame *f); +int pico_notify_ttl_expired(struct pico_frame *f); +int pico_notify_frag_expired(struct pico_frame *f); +int pico_notify_pkt_too_big(struct pico_frame *f); + +/* Various. */ +int pico_source_is_local(struct pico_frame *f); +int pico_frame_dst_is_unicast(struct pico_frame *f); +void pico_store_network_origin(void *src, struct pico_frame *f); +uint32_t pico_timer_add(pico_time expire, void (*timer)(pico_time, void *), void *arg); +void pico_timer_cancel(uint32_t id); +uint32_t pico_rand(void); +void pico_rand_feed(uint32_t feed); +void pico_to_lowercase(char *str); +int pico_address_compare(union pico_address *a, union pico_address *b, uint16_t proto); +int32_t pico_seq_compare(uint32_t a, uint32_t b); + +#endif diff --git a/ext/picotcp/include/pico_tree.h b/ext/picotcp/include/pico_tree.h new file mode 100644 index 0000000..8667a1f --- /dev/null +++ b/ext/picotcp/include/pico_tree.h @@ -0,0 +1,93 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Author: Andrei Carp + *********************************************************************/ + +#ifndef PICO_RBTREE_H +#define PICO_RBTREE_H + +#include "pico_config.h" + +/* This is used to declare a new tree, leaf root by default */ +#define PICO_TREE_DECLARE(name, compareFunction) \ + struct pico_tree name = \ + { \ + &LEAF, \ + compareFunction \ + } + +#define USE_PICO_PAGE0_ZALLOC (1) +#define USE_PICO_ZALLOC (2) + +struct pico_tree_node +{ + void*keyValue; /* generic key */ + struct pico_tree_node*parent; + struct pico_tree_node*leftChild; + struct pico_tree_node*rightChild; + uint8_t color; +}; + +struct pico_tree +{ + struct pico_tree_node *root; /* root of the tree */ + + /* this function directly provides the keys as parameters not the nodes. */ + int (*compare)(void*keyA, void*keyB); +}; + +extern struct pico_tree_node LEAF; /* generic leaf node */ + +#ifdef PICO_SUPPORT_MM +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); +#endif + + +/* + * Manipulation functions + */ +void *pico_tree_insert(struct pico_tree *tree, void *key); +void *pico_tree_delete(struct pico_tree *tree, void *key); +void *pico_tree_findKey(struct pico_tree *tree, void *key); +void pico_tree_drop(struct pico_tree *tree); +int pico_tree_empty(struct pico_tree *tree); +struct pico_tree_node *pico_tree_findNode(struct pico_tree *tree, void *key); + +void *pico_tree_first(struct pico_tree *tree); +void *pico_tree_last(struct pico_tree *tree); +/* + * Traverse functions + */ +struct pico_tree_node *pico_tree_lastNode(struct pico_tree_node *node); +struct pico_tree_node *pico_tree_firstNode(struct pico_tree_node *node); +struct pico_tree_node *pico_tree_next(struct pico_tree_node *node); +struct pico_tree_node *pico_tree_prev(struct pico_tree_node *node); + +/* + * For each macros + */ + +#define pico_tree_foreach(idx, tree) \ + for ((idx) = pico_tree_firstNode((tree)->root); \ + (idx) != &LEAF; \ + (idx) = pico_tree_next(idx)) + +#define pico_tree_foreach_reverse(idx, tree) \ + for ((idx) = pico_tree_lastNode((tree)->root); \ + (idx) != &LEAF; \ + (idx) = pico_tree_prev(idx)) + +#define pico_tree_foreach_safe(idx, tree, idx2) \ + for ((idx) = pico_tree_firstNode((tree)->root); \ + ((idx) != &LEAF) && ((idx2) = pico_tree_next(idx), 1); \ + (idx) = (idx2)) + +#define pico_tree_foreach_reverse_safe(idx, tree, idx2) \ + for ((idx) = pico_tree_lastNode((tree)->root); \ + ((idx) != &LEAF) && ((idx2) = pico_tree_prev(idx), 1); \ + (idx) = (idx2)) + +#endif diff --git a/ext/picotcp/mkdeps.sh b/ext/picotcp/mkdeps.sh new file mode 100755 index 0000000..e313ef2 --- /dev/null +++ b/ext/picotcp/mkdeps.sh @@ -0,0 +1,16 @@ +#!/bin/bash +PREFIX=$1 +shift +echo "/* PicoTCP - Definition file - DO NOT EDIT */" >$PREFIX/include/pico_defines.h +echo "/* This file is automatically generated at compile time */" >>$PREFIX/include/pico_defines.h +echo "#ifndef PICO_DEFINES_H" >>$PREFIX/include/pico_defines.h +echo "#define PICO_DEFINES_H" >>$PREFIX/include/pico_defines.h +echo >>$PREFIX/include/pico_defines.h + +for i in $@; do + if (echo $i | grep "^-D" |grep PICO_SUPPORT >/dev/null); then + my_def=`echo $i |sed -e "s/-D//g"` + echo "#define $my_def" >> $PREFIX/include/pico_defines.h + fi +done +echo "#endif" >>$PREFIX/include/pico_defines.h diff --git a/ext/picotcp/modcheck.py b/ext/picotcp/modcheck.py new file mode 100755 index 0000000..05d2075 --- /dev/null +++ b/ext/picotcp/modcheck.py @@ -0,0 +1,50 @@ +#!/usr/bin/python +import os,sys +import subprocess + +f = open('MODTREE') +mods = {} +commands = [] + +def get_deps(mod): + if not mod in mods.keys(): + return [] + deps = mods[mod] + retlist = [mod] + for i in deps.split(' '): + retlist.append(i) + for j in get_deps(i): + retlist.append(j) + return retlist + + +while(True): + r = f.readline() + if r == '': + break + if r != '\n': + strings = r.split(':') + mod = strings[0] + deps = strings[1].rstrip('\n') + mods[mod] = deps.strip(' ') + +for k,v in mods.iteritems(): + command = 'make dummy ' + deps = get_deps(k) + for i in mods.keys(): + if i in deps: + command += i + "=1 " + else: + command += i + "=0 " + commands.append(command) + +nul = open('/dev/null', 'w') +for i in commands: + print 'Checking config:\n\t%s' % i + os.system('make clean >/dev/null') + args = i.split(' ') + subprocess.call(['make','clean'], shell=True, stdout=nul, stderr=nul) + subprocess.call(args[-1], shell=True,stdout=nul, stderr=nul)==0 or sys.exit(1) + print "CONFIG OK!" + print +sys.exit(0) diff --git a/ext/picotcp/modules/pico_aodv.c b/ext/picotcp/modules/pico_aodv.c new file mode 100644 index 0000000..b0751da --- /dev/null +++ b/ext/picotcp/modules/pico_aodv.c @@ -0,0 +1,674 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + Author: Daniele Lacamera + *********************************************************************/ +#include +#include +#include +#include +#include + +#include +#ifdef PICO_SUPPORT_IPV4 + +#define pico_aodv_dbg(...) do {} while(0) +/* #define pico_aodv_dbg dbg */ + +#define AODV_MAX_PKT (64) +static const struct pico_ip4 HOST_NETMASK = { + 0xffffffff +}; +static struct pico_ip4 all_bcast = { + .addr = 0xFFFFFFFFu +}; + +static const struct pico_ip4 ANY_HOST = { + 0x0 +}; + +static uint32_t pico_aodv_local_id = 0; +static int aodv_node_compare(void *ka, void *kb) +{ + struct pico_aodv_node *a = ka, *b = kb; + if (a->dest.ip4.addr < b->dest.ip4.addr) + return -1; + + if (b->dest.ip4.addr < a->dest.ip4.addr) + return 1; + + return 0; +} + +static int aodv_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(aodv_nodes, aodv_node_compare); +PICO_TREE_DECLARE(aodv_devices, aodv_dev_cmp); + +static struct pico_socket *aodv_socket = NULL; + +static struct pico_aodv_node *get_node_by_addr(const union pico_address *addr) +{ + struct pico_aodv_node search; + memcpy(&search.dest, addr, sizeof(union pico_address)); + return pico_tree_findKey(&aodv_nodes, &search); + +} + +static void pico_aodv_set_dev(struct pico_device *dev) +{ + pico_ipv4_route_set_bcast_link(pico_ipv4_link_by_dev(dev)); +} + + +static int aodv_peer_refresh(struct pico_aodv_node *node, uint32_t seq) +{ + if ((0 == (node->flags & PICO_AODV_NODE_SYNC)) || (pico_seq_compare(seq, node->dseq) > 0)) { + node->dseq = seq; + node->flags |= PICO_AODV_NODE_SYNC; + node->last_seen = PICO_TIME_MS(); + return 0; + } + + return -1; +} + +static void aodv_elect_route(struct pico_aodv_node *node, union pico_address *gw, uint8_t metric, struct pico_device *dev) +{ + metric++; + if (!(PICO_AODV_ACTIVE(node)) || metric < node->metric) { + pico_ipv4_route_del(node->dest.ip4, HOST_NETMASK, node->metric); + if (!gw) { + pico_ipv4_route_add(node->dest.ip4, HOST_NETMASK, ANY_HOST, 1, pico_ipv4_link_by_dev(dev)); + node->metric = 1; + } else { + node->metric = metric; + pico_ipv4_route_add(node->dest.ip4, HOST_NETMASK, gw->ip4, metric, NULL); + } + } +} + +static struct pico_aodv_node *aodv_peer_new(const union pico_address *addr) +{ + struct pico_aodv_node *node = PICO_ZALLOC(sizeof(struct pico_aodv_node)); + if (!node) + return NULL; + + memcpy(&node->dest, addr, sizeof(union pico_address)); + pico_tree_insert(&aodv_nodes, node); + return node; +} + + +static struct pico_aodv_node *aodv_peer_eval(union pico_address *addr, uint32_t seq, int valid_seq) +{ + struct pico_aodv_node *node = NULL; + node = get_node_by_addr(addr); + if (!node) { + node = aodv_peer_new(addr); + } + + if (!valid_seq) + return node; + + if (node && (aodv_peer_refresh(node, long_be(seq)) == 0)) + return node; + + return NULL; +} + +static void aodv_forward(void *pkt, struct pico_msginfo *info, int reply) +{ + struct pico_aodv_node *orig; + union pico_address orig_addr; + struct pico_tree_node *index; + struct pico_device *dev; + pico_time now; + int size; + + pico_aodv_dbg("Forwarding %s packet\n", reply ? "REPLY" : "REQUEST"); + + if (reply) { + struct pico_aodv_rrep *rep = (struct pico_aodv_rrep *)pkt; + orig_addr.ip4.addr = rep->dest; + rep->hop_count++; + pico_aodv_dbg("RREP hop count: %d\n", rep->hop_count); + size = sizeof(struct pico_aodv_rrep); + } else { + struct pico_aodv_rreq *req = (struct pico_aodv_rreq *)pkt; + orig_addr.ip4.addr = req->orig; + req->hop_count++; + size = sizeof(struct pico_aodv_rreq); + } + + orig = get_node_by_addr(&orig_addr); + if (!orig) + orig = aodv_peer_new(&orig_addr); + + if (!orig) + return; + + now = PICO_TIME_MS(); + + pico_aodv_dbg("Forwarding %s: last fwd_time: %llu now: %llu ttl: %d ==== \n", reply ? "REPLY" : "REQUEST", + orig->fwd_time, now, info->ttl); + if (((orig->fwd_time == 0) || ((now - orig->fwd_time) > AODV_NODE_TRAVERSAL_TIME)) && (--info->ttl > 0)) { + orig->fwd_time = now; + info->dev = NULL; + pico_tree_foreach(index, &aodv_devices){ + dev = index->keyValue; + pico_aodv_set_dev(dev); + pico_socket_sendto_extended(aodv_socket, pkt, size, &all_bcast, short_be(PICO_AODV_PORT), info); + pico_aodv_dbg("Forwarding %s: complete! ==== \n", reply ? "REPLY" : "REQUEST"); + } + } +} + +static uint32_t aodv_lifetime(struct pico_aodv_node *node) +{ + uint32_t lifetime; + pico_time now = PICO_TIME_MS(); + if (!node->last_seen) + node->last_seen = now; + + if ((now - node->last_seen) > AODV_ACTIVE_ROUTE_TIMEOUT) + return 0; + + lifetime = AODV_ACTIVE_ROUTE_TIMEOUT - (uint32_t)(now - node->last_seen); + return lifetime; +} + +static void aodv_send_reply(struct pico_aodv_node *node, struct pico_aodv_rreq *req, int node_is_local, struct pico_msginfo *info) +{ + struct pico_aodv_rrep reply; + union pico_address dest; + union pico_address oaddr; + struct pico_aodv_node *orig; + oaddr.ip4.addr = req->orig; + orig = get_node_by_addr(&oaddr); + reply.type = AODV_TYPE_RREP; + reply.dest = req->dest; + reply.dseq = req->dseq; + reply.orig = req->orig; + if (!orig) + return; + + reply.hop_count = (uint8_t)(orig->metric - 1u); + + + dest.ip4.addr = 0xFFFFFFFF; /* wide broadcast */ + + if (short_be(req->req_flags) & AODV_RREQ_FLAG_G) { + dest.ip4.addr = req->orig; + } else { + pico_aodv_set_dev(info->dev); + } + + if (node_is_local) { + reply.lifetime = long_be(AODV_MY_ROUTE_TIMEOUT); + reply.dseq = long_be(++pico_aodv_local_id); + pico_socket_sendto(aodv_socket, &reply, sizeof(reply), &dest, short_be(PICO_AODV_PORT)); + } else if (((short_be(req->req_flags) & AODV_RREQ_FLAG_D) == 0) && (node->flags & PICO_AODV_NODE_SYNC)) { + reply.lifetime = long_be(aodv_lifetime(node)); + reply.dseq = long_be(node->dseq); + pico_aodv_dbg("Generating RREP for node %x, id=%x\n", reply.dest, reply.dseq); + pico_socket_sendto(aodv_socket, &reply, sizeof(reply), &dest, short_be(PICO_AODV_PORT)); + } + + pico_aodv_dbg("no rrep generated.\n"); +} + +/* Parser functions */ + +static int aodv_send_req(struct pico_aodv_node *node); + +static void aodv_reverse_path_discover(pico_time now, void *arg) +{ + struct pico_aodv_node *origin = (struct pico_aodv_node *)arg; + (void)now; + pico_aodv_dbg("Sending G RREQ to ORIGIN (metric = %d).\n", origin->metric); + origin->ring_ttl = origin->metric; + aodv_send_req(origin); +} + +static void aodv_recv_valid_rreq(struct pico_aodv_node *node, struct pico_aodv_rreq *req, struct pico_msginfo *info) +{ + struct pico_device *dev; + dev = pico_ipv4_link_find(&node->dest.ip4); + pico_aodv_dbg("Valid req.\n"); + if (dev || PICO_AODV_ACTIVE(node)) { + /* if destination is ourselves, or we have a possible route: Send reply. */ + aodv_send_reply(node, req, dev != NULL, info); + if (dev) { + /* if really for us, we need to build the return route. Initiate a gratuitous request. */ + union pico_address origin_addr; + struct pico_aodv_node *origin; + origin_addr.ip4.addr = req->orig; + origin = get_node_by_addr(&origin_addr); + if (origin) { + origin->flags |= PICO_AODV_NODE_ROUTE_DOWN; + pico_timer_add(AODV_PATH_DISCOVERY_TIME, aodv_reverse_path_discover, origin); + } + } + + pico_aodv_dbg("Replied.\n"); + } else { + /* destination unknown. Evaluate forwarding. */ + pico_aodv_dbg(" == Forwarding == .\n"); + aodv_forward(req, info, 0); + } +} + + +static void aodv_parse_rreq(union pico_address *from, uint8_t *buf, int len, struct pico_msginfo *msginfo) +{ + struct pico_aodv_rreq *req = (struct pico_aodv_rreq *) buf; + struct pico_aodv_node *node = NULL; + struct pico_device *dev; + union pico_address orig, dest; + (void)from; + if (len != (int)sizeof(struct pico_aodv_rreq)) + return; + + orig.ip4.addr = req->orig; + dev = pico_ipv4_link_find(&orig.ip4); + if (dev) { + pico_aodv_dbg("RREQ <-- myself\n"); + return; + } + + node = aodv_peer_eval(&orig, req->oseq, 1); + if (!node) { + pico_aodv_dbg("RREQ: Neighbor is not valid. oseq=%d\n", long_be(req->oseq)); + return; + } + + if (req->hop_count > 0) + aodv_elect_route(node, from, req->hop_count, msginfo->dev); + else + aodv_elect_route(node, NULL, 0, msginfo->dev); + + dest.ip4.addr = req->dest; + node = aodv_peer_eval(&dest, req->dseq, !(req->req_flags & short_be(AODV_RREQ_FLAG_U))); + if (!node) { + node = aodv_peer_new(&dest); + pico_aodv_dbg("RREQ: New peer! %08x\n", dest.ip4.addr); + } + + if (!node) + return; + + aodv_recv_valid_rreq(node, req, msginfo); +} + +static void aodv_parse_rrep(union pico_address *from, uint8_t *buf, int len, struct pico_msginfo *msginfo) +{ + struct pico_aodv_rrep *rep = (struct pico_aodv_rrep *) buf; + struct pico_aodv_node *node = NULL; + union pico_address dest; + union pico_address orig; + struct pico_device *dev = NULL; + if (len != (int)sizeof(struct pico_aodv_rrep)) + return; + + dest.ip4.addr = rep->dest; + orig.ip4.addr = rep->orig; + dev = pico_ipv4_link_find(&dest.ip4); + + if (dev) /* Our reply packet got rebounced, no useful information here, no need to fwd. */ + return; + + pico_aodv_dbg("::::::::::::: Parsing RREP for node %08x\n", rep->dest); + node = aodv_peer_eval(&dest, rep->dseq, 1); + if (node) { + pico_aodv_dbg("::::::::::::: Node found. Electing route and forwarding.\n"); + dest.ip4.addr = node->dest.ip4.addr; + if (rep->hop_count > 0) + aodv_elect_route(node, from, rep->hop_count, msginfo->dev); + else + aodv_elect_route(node, NULL, 0, msginfo->dev); + + /* If we are the final destination for the reply (orig), no need to forward. */ + if (pico_ipv4_link_find(&orig.ip4)) { + node->flags |= PICO_AODV_NODE_ROUTE_UP; + } else { + aodv_forward(rep, msginfo, 1); + } + } +} + +static void aodv_parse_rerr(union pico_address *from, uint8_t *buf, int len, struct pico_msginfo *msginfo) +{ + if ((uint32_t)len < sizeof(struct pico_aodv_rerr) || + (((uint32_t)len - sizeof(struct pico_aodv_rerr)) % sizeof(struct pico_aodv_unreachable)) > 0) + return; + + (void)from; + (void)buf; + (void)len; + (void)msginfo; + /* TODO: invalidate routes. This only makes sense if we are using HELLO messages. */ +} + +static void aodv_parse_rack(union pico_address *from, uint8_t *buf, int len, struct pico_msginfo *msginfo) +{ + if (len != (int)sizeof(struct pico_aodv_rack)) + return; + + (void)from; + (void)buf; + (void)len; + (void)msginfo; +} + +struct aodv_parser_s { + void (*call)(union pico_address *from, uint8_t *buf, int len, struct pico_msginfo *msginfo); +}; + +struct aodv_parser_s aodv_parser[5] = { + {.call = NULL}, + {.call = aodv_parse_rreq }, + {.call = aodv_parse_rrep }, + {.call = aodv_parse_rerr }, + {.call = aodv_parse_rack } +}; + + +static void pico_aodv_parse(union pico_address *from, uint8_t *buf, int len, struct pico_msginfo *msginfo) +{ + struct pico_aodv_node *node; + uint8_t hopcount = 0; + if ((buf[0] < 1) || (buf[0] > 4)) { + /* Type is invalid. Discard silently. */ + return; + } + + if (buf[0] == AODV_TYPE_RREQ) { + hopcount = ((struct pico_aodv_rreq *)buf)->hop_count; + } + + if (buf[0] == AODV_TYPE_RREP) { + hopcount = ((struct pico_aodv_rrep *)buf)->hop_count; + } + + node = aodv_peer_eval(from, 0, 0); + if (!node) + node = aodv_peer_new(from); + + if (node && (hopcount == 0)) { + aodv_elect_route(node, NULL, hopcount, msginfo->dev); + } + + pico_aodv_dbg("Received AODV packet, ttl = %d\n", msginfo->ttl); + aodv_parser[buf[0]].call(from, buf, len, msginfo); +} + +static void pico_aodv_socket_callback(uint16_t ev, struct pico_socket *s) +{ + static uint8_t aodv_pkt[AODV_MAX_PKT]; + static union pico_address from; + static struct pico_msginfo msginfo; + uint16_t sport; + int r; + if (s != aodv_socket) + return; + + if (ev & PICO_SOCK_EV_RD) { + r = pico_socket_recvfrom_extended(s, aodv_pkt, AODV_MAX_PKT, &from, &sport, &msginfo); + if (r <= 0) + return; + + pico_aodv_dbg("Received AODV packet: %d bytes \n", r); + + pico_aodv_parse(&from, aodv_pkt, r, &msginfo); + } +} + +static void aodv_make_rreq(struct pico_aodv_node *node, struct pico_aodv_rreq *req) +{ + memset(req, 0, sizeof(struct pico_aodv_rreq)); + req->type = AODV_TYPE_RREQ; + + if (0 == (node->flags & PICO_AODV_NODE_SYNC)) { + req->req_flags |= short_be(AODV_RREQ_FLAG_U); /* no known dseq, mark as U */ + req->dseq = 0; /* Unknown */ + } else { + req->dseq = long_be(node->dseq); + req->req_flags |= short_be(AODV_RREQ_FLAG_G); /* RFC3561 $6.3: we SHOULD set G flag as originators */ + } + + /* Hop count = 0; */ + req->rreq_id = long_be(++pico_aodv_local_id); + req->dest = node->dest.ip4.addr; + req->oseq = long_be(pico_aodv_local_id); +} + +static void aodv_retrans_rreq(pico_time now, void *arg) +{ + struct pico_aodv_node *node = (struct pico_aodv_node *)arg; + struct pico_device *dev; + struct pico_tree_node *index; + static struct pico_aodv_rreq rreq; + struct pico_ipv4_link *ip4l = NULL; + struct pico_msginfo info = { + .dev = NULL, .tos = 0, .ttl = AODV_TTL_START + }; + (void)now; + + memset(&rreq, 0, sizeof(rreq)); + + if (node->flags & PICO_AODV_NODE_ROUTE_UP) { + pico_aodv_dbg("------------------------------------------------------ Node %08x already active.\n", node->dest.ip4.addr); + return; + } + + if (node->ring_ttl > AODV_TTL_THRESHOLD) { + node->ring_ttl = AODV_NET_DIAMETER; + pico_aodv_dbg("----------- DIAMETER reached.\n"); + } + + + if (node->rreq_retry > AODV_RREQ_RETRIES) { + node->rreq_retry = 0; + node->ring_ttl = 0; + pico_aodv_dbg("Node is unreachable.\n"); + node->flags &= (uint16_t)(~PICO_AODV_NODE_ROUTE_DOWN); + return; + } + + if (node->ring_ttl == AODV_NET_DIAMETER) { + node->rreq_retry++; + pico_aodv_dbg("Retry #%d\n", node->rreq_retry); + } + + aodv_make_rreq(node, &rreq); + info.ttl = (uint8_t)node->ring_ttl; + pico_tree_foreach(index, &aodv_devices){ + dev = index->keyValue; + pico_aodv_set_dev(dev); + ip4l = pico_ipv4_link_by_dev(dev); + if (ip4l) { + rreq.orig = ip4l->address.addr; + pico_socket_sendto_extended(aodv_socket, &rreq, sizeof(rreq), &all_bcast, short_be(PICO_AODV_PORT), &info); + } + } + if (node->ring_ttl < AODV_NET_DIAMETER) + node->ring_ttl = (uint8_t)(node->ring_ttl + AODV_TTL_INCREMENT); + + pico_timer_add((pico_time)AODV_RING_TRAVERSAL_TIME(node->ring_ttl), aodv_retrans_rreq, node); +} + +static int aodv_send_req(struct pico_aodv_node *node) +{ + struct pico_device *dev; + struct pico_tree_node *index; + static struct pico_aodv_rreq rreq; + int n = 0; + struct pico_ipv4_link *ip4l = NULL; + struct pico_msginfo info = { + .dev = NULL, .tos = 0, .ttl = AODV_TTL_START + }; + memset(&rreq, 0, sizeof(rreq)); + + if (PICO_AODV_ACTIVE(node)) + return 0; + + node->flags |= PICO_AODV_NODE_REQUESTING; + + if (pico_tree_empty(&aodv_devices)) + return n; + + if (!aodv_socket) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + if (node->flags & PICO_AODV_NODE_ROUTE_DOWN) { + info.ttl = node->metric; + } + + aodv_make_rreq(node, &rreq); + pico_tree_foreach(index, &aodv_devices) { + dev = index->keyValue; + pico_aodv_set_dev(dev); + ip4l = pico_ipv4_link_by_dev(dev); + if (ip4l) { + rreq.orig = ip4l->address.addr; + pico_socket_sendto_extended(aodv_socket, &rreq, sizeof(rreq), &all_bcast, short_be(PICO_AODV_PORT), &info); + n++; + } + } + pico_timer_add((pico_time)AODV_RING_TRAVERSAL_TIME(1), aodv_retrans_rreq, node); + return n; +} + +static void pico_aodv_expired(struct pico_aodv_node *node) +{ + node->flags |= PICO_AODV_NODE_UNREACH; + node->flags &= (uint8_t)(~PICO_AODV_NODE_ROUTE_UP); + node->flags &= (uint8_t)(~PICO_AODV_NODE_ROUTE_DOWN); + pico_ipv4_route_del(node->dest.ip4, HOST_NETMASK, node->metric); + node->ring_ttl = 0; + /* TODO: send err */ + +} + +static void pico_aodv_collector(pico_time now, void *arg) +{ + struct pico_tree_node *index; + struct pico_aodv_node *node; + (void)arg; + (void)now; + pico_tree_foreach(index, &aodv_nodes){ + node = index->keyValue; + if (PICO_AODV_ACTIVE(node)) { + uint32_t lifetime = aodv_lifetime(node); + if (lifetime == 0) + pico_aodv_expired(node); + } + } + pico_timer_add(AODV_HELLO_INTERVAL, pico_aodv_collector, NULL); +} + +MOCKABLE int pico_aodv_init(void) +{ + struct pico_ip4 any = { + 0 + }; + uint16_t port = short_be(PICO_AODV_PORT); + if (aodv_socket) { + pico_err = PICO_ERR_EADDRINUSE; + return -1; + } + + aodv_socket = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_UDP, pico_aodv_socket_callback); + if (!aodv_socket) + return -1; + + if (pico_socket_bind(aodv_socket, &any, &port) != 0) { + uint16_t err = pico_err; + pico_socket_close(aodv_socket); + pico_err = err; + aodv_socket = NULL; + return -1; + } + + pico_aodv_local_id = pico_rand(); + pico_timer_add(AODV_HELLO_INTERVAL, pico_aodv_collector, NULL); + return 0; +} + + +int pico_aodv_add(struct pico_device *dev) +{ + return (pico_tree_insert(&aodv_devices, dev)) ? (0) : (-1); +} + +void pico_aodv_refresh(const union pico_address *addr) +{ + struct pico_aodv_node *node = get_node_by_addr(addr); + if (node) { + node->last_seen = PICO_TIME_MS(); + } +} + +int pico_aodv_lookup(const union pico_address *addr) +{ + struct pico_aodv_node *node = get_node_by_addr(addr); + if (!node) + node = aodv_peer_new(addr); + + if (!node) + return -1; + + if ((node->flags & PICO_AODV_NODE_ROUTE_UP) || (node->flags & PICO_AODV_NODE_ROUTE_DOWN)) + return 0; + + if (node->ring_ttl < AODV_TTL_START) { + node->ring_ttl = AODV_TTL_START; + aodv_send_req(node); + return 0; + } + + pico_err = PICO_ERR_EINVAL; + return -1; +} + +#else + +int pico_aodv_init(void) +{ + return -1; +} + +int pico_aodv_add(struct pico_device *dev) +{ + (void)dev; + return -1; +} + +int pico_aodv_lookup(const union pico_address *addr) +{ + (void)addr; + return -1; +} + +void pico_aodv_refresh(const union pico_address *addr) +{ + (void)addr; +} + +#endif diff --git a/ext/picotcp/modules/pico_aodv.h b/ext/picotcp/modules/pico_aodv.h new file mode 100644 index 0000000..0937ed1 --- /dev/null +++ b/ext/picotcp/modules/pico_aodv.h @@ -0,0 +1,130 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + Author: Daniele Lacamera + *********************************************************************/ +#ifndef PICO_AODV_H_ +#define PICO_AODV_H_ + +/* RFC3561 */ +#define PICO_AODV_PORT (654) + +/* RFC3561 $10 */ +#define AODV_ACTIVE_ROUTE_TIMEOUT (8000u) /* Conservative value for link breakage detection */ +#define AODV_DELETE_PERIOD (5 * AODV_ACTIVE_ROUTE_TIMEOUT) /* Recommended value K = 5 */ +#define AODV_ALLOWED_HELLO_LOSS (4) /* conservative */ +#define AODV_NET_DIAMETER ((uint8_t)(35)) +#define AODV_RREQ_RETRIES (2) +#define AODV_NODE_TRAVERSAL_TIME (40) +#define AODV_HELLO_INTERVAL (1000) +#define AODV_LOCAL_ADD_TTL 2 +#define AODV_RREQ_RATELIMIT (10) +#define AODV_TIMEOUT_BUFFER (2) +#define AODV_TTL_START ((uint8_t)(1)) +#define AODV_TTL_INCREMENT 2 +#define AODV_TTL_THRESHOLD ((uint8_t)(7)) +#define AODV_RERR_RATELIMIT (10) +#define AODV_MAX_REPAIR_TTL ((uint8_t)(AODV_NET_DIAMETER / 3)) +#define AODV_MY_ROUTE_TIMEOUT (2 * AODV_ACTIVE_ROUTE_TIMEOUT) +#define AODV_NET_TRAVERSAL_TIME (2 * AODV_NODE_TRAVERSAL_TIME * AODV_NET_DIAMETER) +#define AODV_BLACKLIST_TIMEOUT (AODV_RREQ_RETRIES * AODV_NET_TRAVERSAL_TIME) +#define AODV_NEXT_HOP_WAIT (AODV_NODE_TRAVERSAL_TIME + 10) +#define AODV_PATH_DISCOVERY_TIME (2 * AODV_NET_TRAVERSAL_TIME) +#define AODV_RING_TRAVERSAL_TIME(ttl) (2 * AODV_NODE_TRAVERSAL_TIME * (ttl + AODV_TIMEOUT_BUFFER)) +/* End section RFC3561 $10 */ + + +#define AODV_TYPE_RREQ 1 +#define AODV_TYPE_RREP 2 +#define AODV_TYPE_RERR 3 +#define AODV_TYPE_RACK 4 + +PACKED_STRUCT_DEF pico_aodv_rreq +{ + uint8_t type; + uint16_t req_flags; + uint8_t hop_count; + uint32_t rreq_id; + uint32_t dest; + uint32_t dseq; + uint32_t orig; + uint32_t oseq; +}; + +#define AODV_RREQ_FLAG_J 0x8000 +#define AODV_RREQ_FLAG_R 0x4000 +#define AODV_RREQ_FLAG_G 0x2000 +#define AODV_RREQ_FLAG_D 0x1000 +#define AODV_RREQ_FLAG_U 0x0800 +#define AODV_RREQ_FLAG_RESERVED 0x07FF + +PACKED_STRUCT_DEF pico_aodv_rrep +{ + uint8_t type; + uint8_t rep_flags; + uint8_t prefix_sz; + uint8_t hop_count; + uint32_t dest; + uint32_t dseq; + uint32_t orig; + uint32_t lifetime; +}; + +#define AODV_RREP_MAX_PREFIX 0x1F +#define AODV_RREP_FLAG_R 0x80 +#define AODV_RREP_FLAG_A 0x40 +#define AODV_RREP_FLAG_RESERVED 0x3F + +#define PICO_AODV_NODE_NEW 0x0000 +#define PICO_AODV_NODE_SYNC 0x0001 +#define PICO_AODV_NODE_REQUESTING 0x0002 +#define PICO_AODV_NODE_ROUTE_UP 0x0004 +#define PICO_AODV_NODE_ROUTE_DOWN 0x0008 +#define PICO_AODV_NODE_IDLING 0x0010 +#define PICO_AODV_NODE_UNREACH 0x0020 + +#define PICO_AODV_ACTIVE(node) ((node->flags & PICO_AODV_NODE_ROUTE_UP) && (node->flags & PICO_AODV_NODE_ROUTE_DOWN)) + + +struct pico_aodv_node +{ + union pico_address dest; + pico_time last_seen; + pico_time fwd_time; + uint32_t dseq; + uint16_t flags; + uint8_t metric; + uint8_t ring_ttl; + uint8_t rreq_retry; +}; + +PACKED_STRUCT_DEF pico_aodv_unreachable +{ + uint32_t addr; + uint32_t dseq; +}; + +PACKED_STRUCT_DEF pico_aodv_rerr +{ + uint8_t type; + uint16_t rerr_flags; + uint8_t dst_count; + uint32_t unreach_addr; + uint32_t unreach_dseq; + struct pico_aodv_unreachable unreach[1]; /* unrechable nodes: must be at least 1. See dst_count field above */ +}; + +PACKED_STRUCT_DEF pico_aodv_rack +{ + uint8_t type; + uint8_t reserved; +}; + +int pico_aodv_init(void); +int pico_aodv_add(struct pico_device *dev); +int pico_aodv_lookup(const union pico_address *addr); +void pico_aodv_refresh(const union pico_address *addr); +#endif diff --git a/ext/picotcp/modules/pico_arp.c b/ext/picotcp/modules/pico_arp.c new file mode 100644 index 0000000..850ca35 --- /dev/null +++ b/ext/picotcp/modules/pico_arp.c @@ -0,0 +1,537 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + Authors: Daniele Lacamera + *********************************************************************/ + + +#include "pico_config.h" +#include "pico_arp.h" +#include "pico_tree.h" +#include "pico_ipv4.h" +#include "pico_device.h" +#include "pico_stack.h" + +extern const uint8_t PICO_ETHADDR_ALL[6]; +#define PICO_ARP_TIMEOUT 600000llu +#define PICO_ARP_RETRY 300lu +#define PICO_ARP_MAX_PENDING 5 + +#ifdef DEBUG_ARP + #define arp_dbg dbg +#else + #define arp_dbg(...) do {} while(0) +#endif + +static int max_arp_reqs = PICO_ARP_MAX_RATE; +static struct pico_frame *frames_queued[PICO_ARP_MAX_PENDING] = { 0 }; + +static void pico_arp_queued_trigger(void) +{ + int i; + struct pico_frame *f; + for (i = 0; i < PICO_ARP_MAX_PENDING; i++) + { + f = frames_queued[i]; + if (f) { + if (!pico_ethernet_send(f)) + { + pico_frame_discard(f); + frames_queued[i] = NULL; + } + } + } +} + +static void update_max_arp_reqs(pico_time now, void *unused) +{ + IGNORE_PARAMETER(now); + IGNORE_PARAMETER(unused); + if (max_arp_reqs < PICO_ARP_MAX_RATE) + max_arp_reqs++; + + pico_timer_add(PICO_ARP_INTERVAL / PICO_ARP_MAX_RATE, &update_max_arp_reqs, NULL); +} + +void pico_arp_init(void) +{ + pico_timer_add(PICO_ARP_INTERVAL / PICO_ARP_MAX_RATE, &update_max_arp_reqs, NULL); +} + +PACKED_STRUCT_DEF pico_arp_hdr +{ + uint16_t htype; + uint16_t ptype; + uint8_t hsize; + uint8_t psize; + uint16_t opcode; + uint8_t s_mac[PICO_SIZE_ETH]; + struct pico_ip4 src; + uint8_t d_mac[PICO_SIZE_ETH]; + struct pico_ip4 dst; +}; + + + +/* Callback handler for ip conflict service (e.g. IPv4 SLAAC) + * Whenever the IP address registered here is seen in the network, + * the callback is awaken to take countermeasures against IP collisions. + * + */ + +struct arp_service_ipconflict { + struct pico_eth mac; + struct pico_ip4 ip; + void (*conflict)(int); +}; + +static struct arp_service_ipconflict conflict_ipv4; + + + +#define PICO_SIZE_ARPHDR ((sizeof(struct pico_arp_hdr))) + +/* Arp Entries for the tables. */ +struct pico_arp { +/* CAREFUL MAN! ARP entry MUST begin with a pico_eth structure, + * due to in-place casting!!! */ + struct pico_eth eth; + struct pico_ip4 ipv4; + int arp_status; + pico_time timestamp; + struct pico_device *dev; + uint32_t timer; +}; + + + +/*****************/ +/** ARP TREE **/ +/*****************/ + +/* Routing destination */ + +static int arp_compare(void *ka, void *kb) +{ + struct pico_arp *a = ka, *b = kb; + return pico_ipv4_compare(&a->ipv4, &b->ipv4); +} + +PICO_TREE_DECLARE(arp_tree, arp_compare); + +/*********************/ +/** END ARP TREE **/ +/*********************/ + +struct pico_eth *pico_arp_lookup(struct pico_ip4 *dst) +{ + struct pico_arp search, *found; + search.ipv4.addr = dst->addr; + found = pico_tree_findKey(&arp_tree, &search); + if (found && (found->arp_status != PICO_ARP_STATUS_STALE)) + return &found->eth; + + return NULL; +} + +struct pico_ip4 *pico_arp_reverse_lookup(struct pico_eth *dst) +{ + struct pico_arp*search; + struct pico_tree_node *index; + pico_tree_foreach(index, &arp_tree){ + search = index->keyValue; + if(memcmp(&(search->eth.addr), &dst->addr, 6) == 0) + return &search->ipv4; + } + return NULL; +} + +static void pico_arp_unreachable(struct pico_ip4 *a) +{ + int i; + struct pico_frame *f; + struct pico_ipv4_hdr *hdr; + struct pico_ip4 dst; + for (i = 0; i < PICO_ARP_MAX_PENDING; i++) + { + f = frames_queued[i]; + if (f) { + hdr = (struct pico_ipv4_hdr *) f->net_hdr; + dst = pico_ipv4_route_get_gateway(&hdr->dst); + if (!dst.addr) + dst.addr = hdr->dst.addr; + + if (dst.addr == a->addr) { + if (!pico_source_is_local(f)) { + pico_notify_dest_unreachable(f); + } + + pico_frame_discard(f); + frames_queued[i] = NULL; + } + } + } +} + +static void pico_arp_retry(struct pico_frame *f, struct pico_ip4 *where) +{ + if (++f->failure_count < 4) { + arp_dbg ("================= ARP REQUIRED: %d =============\n\n", f->failure_count); + /* check if dst is local (gateway = 0), or if to use gateway */ + pico_arp_request(f->dev, where, PICO_ARP_QUERY); + } else { + pico_arp_unreachable(where); + } +} + +struct pico_eth *pico_arp_get(struct pico_frame *f) +{ + struct pico_eth *a4; + struct pico_ip4 gateway; + struct pico_ip4 *where; + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; + struct pico_ipv4_link *l; + if (!hdr) + return NULL; + + l = pico_ipv4_link_get(&hdr->dst); + if(l) { + /* address belongs to ourself */ + return &l->dev->eth->mac; + } + + gateway = pico_ipv4_route_get_gateway(&hdr->dst); + /* check if dst is local (gateway = 0), or if to use gateway */ + if (gateway.addr != 0) + where = &gateway; + else + where = &hdr->dst; + + a4 = pico_arp_lookup(where); /* check if dst ip mac in cache */ + + if (!a4) + pico_arp_retry(f, where); + + return a4; +} + + +void pico_arp_postpone(struct pico_frame *f) +{ + int i; + for (i = 0; i < PICO_ARP_MAX_PENDING; i++) + { + if (!frames_queued[i]) { + frames_queued[i] = pico_frame_copy(f); + return; + } + } + /* Not possible to enqueue: caller will discard packet */ +} + + +#ifdef DEBUG_ARP +void dbg_arp(void) +{ + struct pico_arp *a; + struct pico_tree_node *index; + + pico_tree_foreach(index, &arp_tree) { + a = index->keyValue; + arp_dbg("ARP to %08x, mac: %02x:%02x:%02x:%02x:%02x:%02x\n", a->ipv4.addr, a->eth.addr[0], a->eth.addr[1], a->eth.addr[2], a->eth.addr[3], a->eth.addr[4], a->eth.addr[5] ); + } +} +#endif + +static void arp_expire(pico_time now, void *_stale) +{ + struct pico_arp *stale = (struct pico_arp *) _stale; + if (now >= (stale->timestamp + PICO_ARP_TIMEOUT)) { + stale->arp_status = PICO_ARP_STATUS_STALE; + arp_dbg("ARP: Setting arp_status to STALE\n"); + pico_arp_request(stale->dev, &stale->ipv4, PICO_ARP_QUERY); + } else { + /* Timer must be rescheduled, ARP entry has been renewed lately. + * No action required to refresh the entry, will check on the next timeout */ + pico_timer_add(PICO_ARP_TIMEOUT + stale->timestamp - now, arp_expire, stale); + } +} + +static void pico_arp_add_entry(struct pico_arp *entry) +{ + entry->arp_status = PICO_ARP_STATUS_REACHABLE; + entry->timestamp = PICO_TIME(); + + pico_tree_insert(&arp_tree, entry); + arp_dbg("ARP ## reachable.\n"); + pico_arp_queued_trigger(); + pico_timer_add(PICO_ARP_TIMEOUT, arp_expire, entry); +} + +int pico_arp_create_entry(uint8_t *hwaddr, struct pico_ip4 ipv4, struct pico_device *dev) +{ + struct pico_arp*arp = PICO_ZALLOC(sizeof(struct pico_arp)); + if(!arp) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + memcpy(arp->eth.addr, hwaddr, 6); + arp->ipv4.addr = ipv4.addr; + arp->dev = dev; + + pico_arp_add_entry(arp); + + return 0; +} + +static void pico_arp_check_conflict(struct pico_arp_hdr *hdr) +{ + if (conflict_ipv4.conflict) + { + if((conflict_ipv4.ip.addr == hdr->src.addr) && + (memcmp(hdr->s_mac, conflict_ipv4.mac.addr, PICO_SIZE_ETH) != 0)) + conflict_ipv4.conflict(PICO_ARP_CONFLICT_REASON_CONFLICT ); + if((hdr->src.addr == 0) && (hdr->dst.addr == conflict_ipv4.ip.addr) ) + conflict_ipv4.conflict(PICO_ARP_CONFLICT_REASON_PROBE ); + } +} + +static struct pico_arp *pico_arp_lookup_entry(struct pico_frame *f) +{ + struct pico_arp search; + struct pico_arp *found = NULL; + struct pico_arp_hdr *hdr = (struct pico_arp_hdr *) f->net_hdr; + /* Populate a new arp entry */ + search.ipv4.addr = hdr->src.addr; + + /* Search for already existing entry */ + found = pico_tree_findKey(&arp_tree, &search); + if (found) { + if (found->arp_status == PICO_ARP_STATUS_STALE) { + /* Replace if stale */ + pico_tree_delete(&arp_tree, found); + pico_arp_add_entry(found); + } else { + /* Update mac address */ + memcpy(found->eth.addr, hdr->s_mac, PICO_SIZE_ETH); + arp_dbg("ARP entry updated!\n"); + + /* Refresh timestamp, this will force a reschedule on the next timeout*/ + found->timestamp = PICO_TIME(); + } + } + + return found; +} + + +static int pico_arp_check_incoming_hdr_type(struct pico_arp_hdr *h) +{ + /* Check the hardware type and protocol */ + if ((h->htype != PICO_ARP_HTYPE_ETH) || (h->ptype != PICO_IDETH_IPV4)) + return -1; + + return 0; +} + +static int pico_arp_check_incoming_hdr(struct pico_frame *f, struct pico_ip4 *dst_addr) +{ + struct pico_arp_hdr *hdr = (struct pico_arp_hdr *) f->net_hdr; + if (!hdr) + return -1; + + dst_addr->addr = hdr->dst.addr; + if (pico_arp_check_incoming_hdr_type(hdr) < 0) + return -1; + + /* The source mac address must not be a multicast or broadcast address */ + if (hdr->s_mac[0] & 0x01) + return -1; + + return 0; +} + +static void pico_arp_reply_on_request(struct pico_frame *f, struct pico_ip4 me) +{ + struct pico_arp_hdr *hdr; + struct pico_eth_hdr *eh; + + hdr = (struct pico_arp_hdr *) f->net_hdr; + eh = (struct pico_eth_hdr *)f->datalink_hdr; + if (hdr->opcode != PICO_ARP_REQUEST) + return; + + hdr->opcode = PICO_ARP_REPLY; + memcpy(hdr->d_mac, hdr->s_mac, PICO_SIZE_ETH); + memcpy(hdr->s_mac, f->dev->eth->mac.addr, PICO_SIZE_ETH); + hdr->dst.addr = hdr->src.addr; + hdr->src.addr = me.addr; + + /* Prepare eth header for arp reply */ + memcpy(eh->daddr, eh->saddr, PICO_SIZE_ETH); + memcpy(eh->saddr, f->dev->eth->mac.addr, PICO_SIZE_ETH); + f->start = f->datalink_hdr; + f->len = PICO_SIZE_ETHHDR + PICO_SIZE_ARPHDR; + f->dev->send(f->dev, f->start, (int)f->len); +} + +static int pico_arp_check_flooding(struct pico_frame *f, struct pico_ip4 me) +{ + struct pico_device *link_dev; + struct pico_arp_hdr *hdr; + hdr = (struct pico_arp_hdr *) f->net_hdr; + + /* Prevent ARP flooding */ + link_dev = pico_ipv4_link_find(&me); + if ((link_dev == f->dev) && (hdr->opcode == PICO_ARP_REQUEST)) { + if (max_arp_reqs == 0) + return -1; + else + max_arp_reqs--; + } + + /* Check if we are the target IP address */ + if (link_dev != f->dev) + return -1; + + return 0; +} + +static int pico_arp_process_in(struct pico_frame *f, struct pico_arp_hdr *hdr, struct pico_arp *found) +{ + struct pico_ip4 me; + if (pico_arp_check_incoming_hdr(f, &me) < 0) { + pico_frame_discard(f); + return -1; + } + + if (pico_arp_check_flooding(f, me) < 0) { + pico_frame_discard(f); + return -1; + } + + /* If no existing entry was found, create a new entry, or fail trying. */ + if ((!found) && (pico_arp_create_entry(hdr->s_mac, hdr->src, f->dev) < 0)) { + pico_frame_discard(f); + return -1; + } + + /* If the packet is a request, send a reply */ + pico_arp_reply_on_request(f, me); + +#ifdef DEBUG_ARP + dbg_arp(); +#endif + pico_frame_discard(f); + return 0; +} + +int pico_arp_receive(struct pico_frame *f) +{ + struct pico_arp_hdr *hdr; + struct pico_arp *found = NULL; + + hdr = (struct pico_arp_hdr *) f->net_hdr; + if (!hdr) + return -1; + + pico_arp_check_conflict(hdr); + found = pico_arp_lookup_entry(f); + return pico_arp_process_in(f, hdr, found); + +} + +static int32_t pico_arp_request_xmit(struct pico_device *dev, struct pico_frame *f, struct pico_ip4 *src, struct pico_ip4 *dst, uint8_t type) +{ + struct pico_arp_hdr *ah = (struct pico_arp_hdr *) (f->start + PICO_SIZE_ETHHDR); + int ret; + + /* Fill arp header */ + ah->htype = PICO_ARP_HTYPE_ETH; + ah->ptype = PICO_IDETH_IPV4; + ah->hsize = PICO_SIZE_ETH; + ah->psize = PICO_SIZE_IP4; + ah->opcode = PICO_ARP_REQUEST; + memcpy(ah->s_mac, dev->eth->mac.addr, PICO_SIZE_ETH); + + switch (type) { + case PICO_ARP_ANNOUNCE: + ah->src.addr = dst->addr; + ah->dst.addr = dst->addr; + break; + case PICO_ARP_PROBE: + ah->src.addr = 0; + ah->dst.addr = dst->addr; + break; + case PICO_ARP_QUERY: + ah->src.addr = src->addr; + ah->dst.addr = dst->addr; + break; + default: + pico_frame_discard(f); + return -1; + } + arp_dbg("Sending arp request.\n"); + ret = dev->send(dev, f->start, (int) f->len); + pico_frame_discard(f); + return ret; +} + +int32_t pico_arp_request(struct pico_device *dev, struct pico_ip4 *dst, uint8_t type) +{ + struct pico_frame *q = pico_frame_alloc(PICO_SIZE_ETHHDR + PICO_SIZE_ARPHDR); + struct pico_eth_hdr *eh; + struct pico_ip4 *src = NULL; + + if (!q) + return -1; + + if (type == PICO_ARP_QUERY) + { + src = pico_ipv4_source_find(dst); + if (!src) { + pico_frame_discard(q); + return -1; + } + } + + arp_dbg("QUERY: %08x\n", dst->addr); + + eh = (struct pico_eth_hdr *)q->start; + + /* Fill eth header */ + memcpy(eh->saddr, dev->eth->mac.addr, PICO_SIZE_ETH); + memcpy(eh->daddr, PICO_ETHADDR_ALL, PICO_SIZE_ETH); + eh->proto = PICO_IDETH_ARP; + + return pico_arp_request_xmit(dev, q, src, dst, type); +} + +int pico_arp_get_neighbors(struct pico_device *dev, struct pico_ip4 *neighbors, int maxlen) +{ + struct pico_arp*search; + struct pico_tree_node *index; + int i = 0; + pico_tree_foreach(index, &arp_tree){ + search = index->keyValue; + if (search->dev == dev) { + neighbors[i++].addr = search->ipv4.addr; + if (i >= maxlen) + return i; + } + } + return i; +} + +void pico_arp_register_ipconflict(struct pico_ip4 *ip, struct pico_eth *mac, void (*cb)(int reason)) +{ + conflict_ipv4.conflict = cb; + conflict_ipv4.ip.addr = ip->addr; + if (mac != NULL) + memcpy(conflict_ipv4.mac.addr, mac, 6); +} + diff --git a/ext/picotcp/modules/pico_arp.h b/ext/picotcp/modules/pico_arp.h new file mode 100644 index 0000000..36f7210 --- /dev/null +++ b/ext/picotcp/modules/pico_arp.h @@ -0,0 +1,35 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#ifndef INCLUDE_PICO_ARP +#define INCLUDE_PICO_ARP +#include "pico_eth.h" +#include "pico_device.h" + +int pico_arp_receive(struct pico_frame *); + + +struct pico_eth *pico_arp_get(struct pico_frame *f); +int32_t pico_arp_request(struct pico_device *dev, struct pico_ip4 *dst, uint8_t type); + +#define PICO_ARP_STATUS_REACHABLE 0x00 +#define PICO_ARP_STATUS_PERMANENT 0x01 +#define PICO_ARP_STATUS_STALE 0x02 + +#define PICO_ARP_QUERY 0x00 +#define PICO_ARP_PROBE 0x01 +#define PICO_ARP_ANNOUNCE 0x02 + +#define PICO_ARP_CONFLICT_REASON_CONFLICT 0 +#define PICO_ARP_CONFLICT_REASON_PROBE 1 + +struct pico_eth *pico_arp_lookup(struct pico_ip4 *dst); +struct pico_ip4 *pico_arp_reverse_lookup(struct pico_eth *dst); +int pico_arp_create_entry(uint8_t*hwaddr, struct pico_ip4 ipv4, struct pico_device*dev); +int pico_arp_get_neighbors(struct pico_device *dev, struct pico_ip4 *neighbors, int maxlen); +void pico_arp_register_ipconflict(struct pico_ip4 *ip, struct pico_eth *mac, void (*cb)(int reason)); +void pico_arp_postpone(struct pico_frame *f); +void pico_arp_init(void); +#endif diff --git a/ext/picotcp/modules/pico_dev_loop.c b/ext/picotcp/modules/pico_dev_loop.c new file mode 100644 index 0000000..4416d0b --- /dev/null +++ b/ext/picotcp/modules/pico_dev_loop.c @@ -0,0 +1,66 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Daniele Lacamera + *********************************************************************/ + + +#include "pico_device.h" +#include "pico_dev_loop.h" +#include "pico_stack.h" + + +#define LOOP_MTU 1500 +static uint8_t l_buf[LOOP_MTU]; +static int l_bufsize = 0; + + +static int pico_loop_send(struct pico_device *dev, void *buf, int len) +{ + IGNORE_PARAMETER(dev); + if (len > LOOP_MTU) + return 0; + + if (l_bufsize == 0) { + memcpy(l_buf, buf, (size_t)len); + l_bufsize += len; + return len; + } + + return 0; +} + +static int pico_loop_poll(struct pico_device *dev, int loop_score) +{ + if (loop_score <= 0) + return 0; + + if (l_bufsize > 0) { + pico_stack_recv(dev, l_buf, (uint32_t)l_bufsize); + l_bufsize = 0; + loop_score--; + } + + return loop_score; +} + + +struct pico_device *pico_loop_create(void) +{ + struct pico_device *loop = PICO_ZALLOC(sizeof(struct pico_device)); + if (!loop) + return NULL; + + if( 0 != pico_device_init(loop, "loop", NULL)) { + dbg ("Loop init failed.\n"); + pico_device_destroy(loop); + return NULL; + } + + loop->send = pico_loop_send; + loop->poll = pico_loop_poll; + dbg("Device %s created.\n", loop->name); + return loop; +} + diff --git a/ext/picotcp/modules/pico_dev_loop.h b/ext/picotcp/modules/pico_dev_loop.h new file mode 100644 index 0000000..6cb8de1 --- /dev/null +++ b/ext/picotcp/modules/pico_dev_loop.h @@ -0,0 +1,15 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#ifndef INCLUDE_PICO_LOOP +#define INCLUDE_PICO_LOOP +#include "pico_config.h" +#include "pico_device.h" + +void pico_loop_destroy(struct pico_device *loop); +struct pico_device *pico_loop_create(void); + +#endif + diff --git a/ext/picotcp/modules/pico_dev_mock.c b/ext/picotcp/modules/pico_dev_mock.c new file mode 100644 index 0000000..59fae01 --- /dev/null +++ b/ext/picotcp/modules/pico_dev_mock.c @@ -0,0 +1,307 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Frederik Van Slycken + *********************************************************************/ + + +#include "pico_device.h" +#include "pico_dev_mock.h" +#include "pico_stack.h" +#include "pico_tree.h" + +#define MOCK_MTU 1500 + + + +/* Tree for finding mock_device based on pico_device* */ + + +static int mock_dev_cmp(void *ka, void *kb) +{ + struct mock_device *a = ka, *b = kb; + if (a->dev < b->dev) + return -1; + + if (a->dev > b->dev) + return 1; + + return 0; +} + +PICO_TREE_DECLARE(mock_device_tree, mock_dev_cmp); + +static int pico_mock_send(struct pico_device *dev, void *buf, int len) +{ + struct mock_device search = { + .dev = dev + }; + struct mock_device*mock = pico_tree_findKey(&mock_device_tree, &search); + struct mock_frame*frame; + + if(!mock) + return 0; + + if (len > MOCK_MTU) + return 0; + + frame = PICO_ZALLOC(sizeof(struct mock_frame)); + if(!frame) { + return 0; + } + + if(mock->out_head == NULL) + mock->out_head = frame; + else + mock->out_tail->next = frame; + + mock->out_tail = frame; + + mock->out_tail->buffer = PICO_ZALLOC((uint32_t)len); + if(!mock->out_tail->buffer) + return 0; + + memcpy(mock->out_tail->buffer, buf, (uint32_t)len); + mock->out_tail->len = len; + + return len; + +} + +static int pico_mock_poll(struct pico_device *dev, int loop_score) +{ + struct mock_device search = { + .dev = dev + }; + struct mock_device*mock = pico_tree_findKey(&mock_device_tree, &search); + struct mock_frame*nxt; + + if(!mock) + return 0; + + if (loop_score <= 0) + return 0; + + while(mock->in_head != NULL && loop_score > 0) + { + pico_stack_recv(dev, mock->in_head->buffer, (uint32_t)mock->in_head->len); + loop_score--; + + PICO_FREE(mock->in_head->buffer); + + if(mock->in_tail == mock->in_head) { + PICO_FREE(mock->in_head); + mock->in_tail = mock->in_head = NULL; + return loop_score; + } + + nxt = mock->in_head->next; + PICO_FREE(mock->in_head); + mock->in_head = nxt; + } + return loop_score; +} + +int pico_mock_network_read(struct mock_device*mock, void *buf, int len) +{ + struct mock_frame*nxt; + if(mock->out_head == NULL) + return 0; + + if(len > mock->out_head->len - mock->out_head->read) + len = mock->out_head->len - mock->out_head->read; + + memcpy(buf, mock->out_head->buffer, (uint32_t)len); + + if(len + mock->out_head->read != mock->out_head->len) { + mock->out_head->read += len; + return len; + } + + PICO_FREE(mock->out_head->buffer); + + if(mock->out_tail == mock->out_head) { + PICO_FREE(mock->out_head); + mock->out_tail = mock->out_head = NULL; + return len; + } + + nxt = mock->out_head->next; + PICO_FREE(mock->out_head); + mock->out_head = nxt; + + return len; +} + +int pico_mock_network_write(struct mock_device*mock, const void *buf, int len) +{ + struct mock_frame*frame; + if (len > MOCK_MTU) + return 0; + + frame = PICO_ZALLOC(sizeof(struct mock_frame)); + if(!frame) { + return 0; + } + + if(mock->in_head == NULL) + mock->in_head = frame; + else + mock->in_tail->next = frame; + + mock->in_tail = frame; + + mock->in_tail->buffer = PICO_ZALLOC((uint32_t)len); + if(!mock->in_tail->buffer) + return 0; + + memcpy(mock->in_tail->buffer, buf, (uint32_t)len); + mock->in_tail->len = len; + + return len; + +} + +/* Public interface: create/destroy. */ + +void pico_mock_destroy(struct pico_device *dev) +{ + struct mock_device search = { + .dev = dev + }; + struct mock_device*mock = pico_tree_findKey(&mock_device_tree, &search); + struct mock_frame*nxt; + + if(!mock) + return; + + nxt = mock->in_head; + while(nxt != NULL) { + mock->in_head = mock->in_head->next; + PICO_FREE(nxt); + nxt = mock->in_head; + } + nxt = mock->out_head; + while(nxt != NULL) { + mock->out_head = mock->out_head->next; + PICO_FREE(nxt); + nxt = mock->out_head; + } + pico_tree_delete(&mock_device_tree, mock); +} + +struct mock_device *pico_mock_create(uint8_t*mac) +{ + + struct mock_device*mock = PICO_ZALLOC(sizeof(struct mock_device)); + if(!mock) + return NULL; + + mock->dev = PICO_ZALLOC(sizeof(struct pico_device)); + if (!mock->dev) { + PICO_FREE(mock); + return NULL; + } + + if(mac != NULL) { + mock->mac = PICO_ZALLOC(6 * sizeof(uint8_t)); + if(!mock->mac) { + PICO_FREE(mock->mac); + PICO_FREE(mock); + return NULL; + } + + memcpy(mock->mac, mac, 6); + } + + if( 0 != pico_device_init((struct pico_device *)mock->dev, "mock", mac)) { + dbg ("Loop init failed.\n"); + pico_mock_destroy((struct pico_device *)mock->dev); + if(mock->mac != NULL) + PICO_FREE(mock->mac); + + PICO_FREE(mock); + return NULL; + } + + mock->dev->send = pico_mock_send; + mock->dev->poll = pico_mock_poll; + mock->dev->destroy = pico_mock_destroy; + dbg("Device %s created.\n", mock->dev->name); + + pico_tree_insert(&mock_device_tree, mock); + return mock; +} + +/* + * a few utility functions that check certain fields + */ + +uint32_t mock_get_sender_ip4(struct mock_device*mock, void*buf, int len) +{ + uint32_t ret; + int start = mock->mac ? 14 : 0; + if(start + 16 > len) { + dbg("out of range!\n"); + return 0; + } + + memcpy(&ret, buf + start + 12, 4); + return ret; +} + +/* + * TODO + * find a way to create ARP replies + * + * create the other utility functions, e.g. + * -is_arp_request + * -create_arp_reply + * -get_destination_ip4 + * -get_ip4_total_length + * -is_ip4_checksum_valid + * -is_tcp_syn + * -create_tcp_synack + * -is_tcp_checksum_valid + * etc. + * + */ + +int mock_ip_protocol(struct mock_device*mock, void*buf, int len) +{ + uint8_t type; + int start = mock->mac ? 14 : 0; + if(start + 10 > len) { + return 0; + } + + memcpy(&type, buf + start + 9, 1); + return type; +} + +/* note : this function doesn't check if the IP header has any options */ +int mock_icmp_type(struct mock_device*mock, void*buf, int len) +{ + uint8_t type; + int start = mock->mac ? 14 : 0; + if(start + 21 > len) { + return 0; + } + + memcpy(&type, buf + start + 20, 1); + return type; +} + +/* note : this function doesn't check if the IP header has any options */ +int mock_icmp_code(struct mock_device*mock, void*buf, int len) +{ + uint8_t type; + int start = mock->mac ? 14 : 0; + if(start + 22 > len) { + return 0; + } + + memcpy(&type, buf + start + 21, 1); + return type; +} diff --git a/ext/picotcp/modules/pico_dev_mock.h b/ext/picotcp/modules/pico_dev_mock.h new file mode 100644 index 0000000..dfc2cfc --- /dev/null +++ b/ext/picotcp/modules/pico_dev_mock.h @@ -0,0 +1,47 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#ifndef INCLUDE_PICO_MOCK +#define INCLUDE_PICO_MOCK +#include "pico_config.h" +#include "pico_device.h" + + +struct mock_frame { + uint8_t*buffer; + int len; + int read; + + struct mock_frame*next; +}; + +struct mock_device { + struct pico_device*dev; + struct mock_frame*in_head; + struct mock_frame*in_tail; + struct mock_frame*out_head; + struct mock_frame*out_tail; + + uint8_t*mac; + +}; + +struct mock_device; +/* A mockup-device for the purpose of testing. It provides a couple of extra "network"-functions, which represent the network-side of the device. A network_send will result in mock_poll reading something, a network_read will see if the stack has sent anything through our mock-device. */ +void pico_mock_destroy(struct pico_device *dev); +struct mock_device *pico_mock_create(uint8_t*mac); + +int pico_mock_network_read(struct mock_device*mock, void *buf, int len); +int pico_mock_network_write(struct mock_device*mock, const void *buf, int len); + +/* TODO */ +/* we could use a few checking functions, e.g. one to see if it's a valid IP packet, if it's TCP, if the IP-address matches,... */ +/* That would be useful to avoid having to manually create buffers of what you expect, probably with masks for things that are random,... */ +uint32_t mock_get_sender_ip4(struct mock_device*mock, void*buf, int len); + +int mock_ip_protocol(struct mock_device*mock, void*buf, int len); +int mock_icmp_type(struct mock_device*mock, void*buf, int len); +int mock_icmp_code(struct mock_device*mock, void*buf, int len); +#endif diff --git a/ext/picotcp/modules/pico_dev_null.c b/ext/picotcp/modules/pico_dev_null.c new file mode 100644 index 0000000..5fed494 --- /dev/null +++ b/ext/picotcp/modules/pico_dev_null.c @@ -0,0 +1,60 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Daniele Lacamera + *********************************************************************/ + + +#include "pico_device.h" +#include "pico_dev_null.h" +#include "pico_stack.h" + +struct pico_device_null { + struct pico_device dev; + int statistics_frames_out; +}; + +#define NULL_MTU 0 + +static int pico_null_send(struct pico_device *dev, void *buf, int len) +{ + struct pico_device_null *null = (struct pico_device_null *) dev; + IGNORE_PARAMETER(buf); + + /* Increase the statistic count */ + null->statistics_frames_out++; + + /* Discard the frame content silently. */ + return len; +} + +static int pico_null_poll(struct pico_device *dev, int loop_score) +{ + /* We never have packet to receive, no score is used. */ + IGNORE_PARAMETER(dev); + return loop_score; +} + +/* Public interface: create/destroy. */ + + +struct pico_device *pico_null_create(char *name) +{ + struct pico_device_null *null = PICO_ZALLOC(sizeof(struct pico_device_null)); + + if (!null) + return NULL; + + if( 0 != pico_device_init((struct pico_device *)null, name, NULL)) { + return NULL; + } + + null->dev.overhead = 0; + null->statistics_frames_out = 0; + null->dev.send = pico_null_send; + null->dev.poll = pico_null_poll; + dbg("Device %s created.\n", null->dev.name); + return (struct pico_device *)null; +} + diff --git a/ext/picotcp/modules/pico_dev_null.h b/ext/picotcp/modules/pico_dev_null.h new file mode 100644 index 0000000..a0eb98e --- /dev/null +++ b/ext/picotcp/modules/pico_dev_null.h @@ -0,0 +1,15 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#ifndef INCLUDE_PICO_NULL +#define INCLUDE_PICO_NULL +#include "pico_config.h" +#include "pico_device.h" + +void pico_null_destroy(struct pico_device *null); +struct pico_device *pico_null_create(char *name); + +#endif + diff --git a/ext/picotcp/modules/pico_dev_pcap.c b/ext/picotcp/modules/pico_dev_pcap.c new file mode 100644 index 0000000..f62fb14 --- /dev/null +++ b/ext/picotcp/modules/pico_dev_pcap.c @@ -0,0 +1,96 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Daniele Lacamera + *********************************************************************/ + + +#include +#include "pico_device.h" +#include "pico_dev_pcap.h" +#include "pico_stack.h" + +#include + +struct pico_device_pcap { + struct pico_device dev; + pcap_t *conn; +}; + +#define VDE_MTU 2048 + +static int pico_pcap_send(struct pico_device *dev, void *buf, int len) +{ + struct pico_device_pcap *pcap = (struct pico_device_pcap *) dev; + /* dbg("[%s] send %d bytes.\n", dev->name, len); */ + return pcap_inject(pcap->conn, buf, (uint32_t)len); +} + +static void pico_dev_pcap_cb(u_char *u, const struct pcap_pkthdr *h, const u_char *data) +{ + struct pico_device *dev = (struct pico_device *)u; + const uint8_t *buf = (const uint8_t *)data; + pico_stack_recv(dev, buf, (uint32_t)h->len); +} + + +static int pico_pcap_poll(struct pico_device *dev, int loop_score) +{ + struct pico_device_pcap *pcap = (struct pico_device_pcap *) dev; + loop_score -= pcap_dispatch(pcap->conn, loop_score, pico_dev_pcap_cb, (u_char *) pcap); + return loop_score; +} + +/* Public interface: create/destroy. */ + +void pico_pcap_destroy(struct pico_device *dev) +{ + struct pico_device_pcap *pcap = (struct pico_device_pcap *) dev; + pcap_close(pcap->conn); +} + +#define PICO_PCAP_MODE_LIVE 0 +#define PICO_PCAP_MODE_STORED 1 + +static struct pico_device *pico_pcap_create(char *if_file_name, char *name, uint8_t *mac, int mode) +{ + struct pico_device_pcap *pcap = PICO_ZALLOC(sizeof(struct pico_device_pcap)); + char errbuf[2000]; + if (!pcap) + return NULL; + + if( 0 != pico_device_init((struct pico_device *)pcap, name, mac)) { + dbg ("Pcap init failed.\n"); + pico_pcap_destroy((struct pico_device *)pcap); + return NULL; + } + + pcap->dev.overhead = 0; + + if (mode == PICO_PCAP_MODE_LIVE) + pcap->conn = pcap_open_live(if_file_name, 2000, 100, 10, errbuf); + else + pcap->conn = pcap_open_offline(if_file_name, errbuf); + + if (!pcap->conn) { + pico_pcap_destroy((struct pico_device *)pcap); + return NULL; + } + + pcap->dev.send = pico_pcap_send; + pcap->dev.poll = pico_pcap_poll; + pcap->dev.destroy = pico_pcap_destroy; + dbg("Device %s created.\n", pcap->dev.name); + return (struct pico_device *)pcap; +} + +struct pico_device *pico_pcap_create_fromfile(char *filename, char *name, uint8_t *mac) +{ + return pico_pcap_create(filename, name, mac, PICO_PCAP_MODE_STORED); +} + +struct pico_device *pico_pcap_create_live(char *ifname, char *name, uint8_t *mac) +{ + return pico_pcap_create(ifname, name, mac, PICO_PCAP_MODE_LIVE); +} diff --git a/ext/picotcp/modules/pico_dev_pcap.h b/ext/picotcp/modules/pico_dev_pcap.h new file mode 100644 index 0000000..887be27 --- /dev/null +++ b/ext/picotcp/modules/pico_dev_pcap.h @@ -0,0 +1,19 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + + Author: Daniele Lacamera + *********************************************************************/ +#ifndef INCLUDE_PICO_PCAP +#define INCLUDE_PICO_PCAP +#include "pico_config.h" +#include "pico_device.h" +#include + +void pico_pcap_destroy(struct pico_device *pcap); +struct pico_device *pico_pcap_create_live(char *ifname, char *name, uint8_t *mac); +struct pico_device *pico_pcap_create_fromfile(char *filename, char *name, uint8_t *mac); + +#endif + diff --git a/ext/picotcp/modules/pico_dev_ppp.c b/ext/picotcp/modules/pico_dev_ppp.c new file mode 100644 index 0000000..8c67f19 --- /dev/null +++ b/ext/picotcp/modules/pico_dev_ppp.c @@ -0,0 +1,2294 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Serge Gadeyne, Daniele Lacamera, Maxime Vincent + + *********************************************************************/ + + +#include +#include +#include + +#include "pico_device.h" +#include "pico_dev_ppp.h" +#include "pico_stack.h" +#include "pico_ipv4.h" +#include "pico_md5.h" +#include "pico_dns_client.h" + +#define ppp_dbg(...) do {} while(0) +/* #define ppp_dbg dbg */ + +/* We should define this in a global header. */ +#define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0])) + +#define PICO_PPP_MRU 1514 /* RFC default MRU */ +#define PICO_PPP_MTU 1500 +#define PPP_MAXPKT 2048 +#define PPP_MAX_APN 134 +#define PPP_MAX_USERNAME 134 +#define PPP_MAX_PASSWORD 134 +#define PPP_HDR_SIZE 3u +#define PPP_PROTO_SLOT_SIZE 2u +#define PPP_FCS_SIZE 2u +#define PPP_PROTO_LCP short_be(0xc021) +#define PPP_PROTO_IP short_be(0x0021) +#define PPP_PROTO_PAP short_be(0xc023) +#define PPP_PROTO_CHAP short_be(0xc223) +#define PPP_PROTO_IPCP short_be(0x8021) + +#define PICO_CONF_REQ 1 +#define PICO_CONF_ACK 2 +#define PICO_CONF_NAK 3 +#define PICO_CONF_REJ 4 +#define PICO_CONF_TERM 5 +#define PICO_CONF_TERM_ACK 6 +#define PICO_CONF_CODE_REJ 7 +#define PICO_CONF_PROTO_REJ 8 +#define PICO_CONF_ECHO_REQ 9 +#define PICO_CONF_ECHO_REP 10 +#define PICO_CONF_DISCARD_REQ 11 + +#define LCPOPT_MRU 1u /* param size: 4, fixed: MRU */ +#define LCPOPT_AUTH 3u /* param size: 4-5: AUTH proto */ +#define LCPOPT_QUALITY 4u /* unused for now */ +#define LCPOPT_MAGIC 5u /* param size: 6, fixed: Magic */ +#define LCPOPT_PROTO_COMP 7u /* param size: 0, flag */ +#define LCPOPT_ADDRCTL_COMP 8u /* param size: 0, flag */ + +#define CHAP_MD5_SIZE 16u +#define CHAP_CHALLENGE 1 +#define CHAP_RESPONSE 2 +#define CHAP_SUCCESS 3 +#define CHAP_FAILURE 4 +#define CHALLENGE_SIZE(ppp, ch) ((size_t)((1 + strlen(ppp->password) + short_be((ch)->len)))) + +#define PAP_AUTH_REQ 1 +#define PAP_AUTH_ACK 2 +#define PAP_AUTH_NAK 3 + + +#define PICO_PPP_DEFAULT_TIMER (3) /* seconds */ +#define PICO_PPP_DEFAULT_MAX_TERMINATE (2) +#define PICO_PPP_DEFAULT_MAX_CONFIGURE (10) +#define PICO_PPP_DEFAULT_MAX_FAILURE (5) +#define PICO_PPP_DEFAULT_MAX_DIALTIME (20) + +#define IPCP_ADDR_LEN 6u +#define IPCP_VJ_LEN 6u +#define IPCP_OPT_IP 0x03 +#define IPCP_OPT_VJ 0x02 +#define IPCP_OPT_DNS1 0x81 +#define IPCP_OPT_NBNS1 0x82 +#define IPCP_OPT_DNS2 0x83 +#define IPCP_OPT_NBNS2 0x84 + +static uint8_t LCPOPT_LEN[9] = { + 0, 4, 0, 4, 4, 6, 2, 2, 2 +}; + + +/* Protocol defines */ +static const unsigned char AT_S3 = 0x0du; +static const unsigned char AT_S4 = 0x0au; +static const unsigned char PPPF_FLAG_SEQ = 0x7eu; +static const unsigned char PPPF_CTRL_ESC = 0x7du; +static const unsigned char PPPF_ADDR = 0xffu; +static const unsigned char PPPF_CTRL = 0x03u; + +static int ppp_devnum = 0; +static uint8_t ppp_recv_buf[PPP_MAXPKT]; + +PACKED_STRUCT_DEF pico_lcp_hdr { + uint8_t code; + uint8_t id; + uint16_t len; +}; + +PACKED_STRUCT_DEF pico_chap_hdr { + uint8_t code; + uint8_t id; + uint16_t len; +}; + +PACKED_STRUCT_DEF pico_pap_hdr { + uint8_t code; + uint8_t id; + uint16_t len; +}; + +PACKED_STRUCT_DEF pico_ipcp_hdr { + uint8_t code; + uint8_t id; + uint16_t len; +}; + +#ifdef DEBUG_PPP +static int fifo_fd = -1; +#endif + +enum ppp_modem_state { + PPP_MODEM_STATE_INITIAL = 0, + PPP_MODEM_STATE_RESET, + PPP_MODEM_STATE_ECHO, + PPP_MODEM_STATE_CREG, + PPP_MODEM_STATE_CGREG, + PPP_MODEM_STATE_CGDCONT, + PPP_MODEM_STATE_CGATT, + PPP_MODEM_STATE_DIAL, + PPP_MODEM_STATE_CONNECTED, + PPP_MODEM_STATE_MAX +}; + +enum ppp_modem_event { + PPP_MODEM_EVENT_START = 0, + PPP_MODEM_EVENT_STOP, + PPP_MODEM_EVENT_OK, + PPP_MODEM_EVENT_CONNECT, + PPP_MODEM_EVENT_TIMEOUT, + PPP_MODEM_EVENT_MAX +}; + +enum ppp_lcp_state { + PPP_LCP_STATE_INITIAL = 0, + PPP_LCP_STATE_STARTING, + PPP_LCP_STATE_CLOSED, + PPP_LCP_STATE_STOPPED, + PPP_LCP_STATE_CLOSING, + PPP_LCP_STATE_STOPPING, + PPP_LCP_STATE_REQ_SENT, + PPP_LCP_STATE_ACK_RCVD, + PPP_LCP_STATE_ACK_SENT, + PPP_LCP_STATE_OPENED, + PPP_LCP_STATE_MAX +}; + +enum ppp_lcp_event { + PPP_LCP_EVENT_UP = 0, + PPP_LCP_EVENT_DOWN, + PPP_LCP_EVENT_OPEN, + PPP_LCP_EVENT_CLOSE, + PPP_LCP_EVENT_TO_POS, + PPP_LCP_EVENT_TO_NEG, + PPP_LCP_EVENT_RCR_POS, + PPP_LCP_EVENT_RCR_NEG, + PPP_LCP_EVENT_RCA, + PPP_LCP_EVENT_RCN, + PPP_LCP_EVENT_RTR, + PPP_LCP_EVENT_RTA, + PPP_LCP_EVENT_RUC, + PPP_LCP_EVENT_RXJ_POS, + PPP_LCP_EVENT_RXJ_NEG, + PPP_LCP_EVENT_RXR, + PPP_LCP_EVENT_MAX +}; + +enum ppp_auth_state { + PPP_AUTH_STATE_INITIAL = 0, + PPP_AUTH_STATE_STARTING, + PPP_AUTH_STATE_RSP_SENT, + PPP_AUTH_STATE_REQ_SENT, + PPP_AUTH_STATE_AUTHENTICATED, + PPP_AUTH_STATE_MAX +}; + +enum ppp_auth_event { + PPP_AUTH_EVENT_UP_NONE = 0, + PPP_AUTH_EVENT_UP_PAP, + PPP_AUTH_EVENT_UP_CHAP, + PPP_AUTH_EVENT_DOWN, + PPP_AUTH_EVENT_RAC, + PPP_AUTH_EVENT_RAA, + PPP_AUTH_EVENT_RAN, + PPP_AUTH_EVENT_TO, + PPP_AUTH_EVENT_MAX +}; + +enum ppp_ipcp_state { + PPP_IPCP_STATE_INITIAL = 0, + PPP_IPCP_STATE_REQ_SENT, + PPP_IPCP_STATE_ACK_RCVD, + PPP_IPCP_STATE_ACK_SENT, + PPP_IPCP_STATE_OPENED, + PPP_IPCP_STATE_MAX +}; + +enum ppp_ipcp_event { + PPP_IPCP_EVENT_UP = 0, + PPP_IPCP_EVENT_DOWN, + PPP_IPCP_EVENT_RCR_POS, + PPP_IPCP_EVENT_RCR_NEG, + PPP_IPCP_EVENT_RCA, + PPP_IPCP_EVENT_RCN, + PPP_IPCP_EVENT_TO, + PPP_IPCP_EVENT_MAX +}; + +enum pico_ppp_state { + PPP_MODEM_RST = 0, + PPP_MODEM_CREG, + PPP_MODEM_CGREG, + PPP_MODEM_CGDCONT, + PPP_MODEM_CGATT, + PPP_MODEM_CONNECT, + /* From here on, PPP states */ + PPP_ESTABLISH, + PPP_AUTH, + PPP_NETCONFIG, + PPP_NETWORK, + PPP_TERMINATE, + /* MAXSTATE is the last one */ + PPP_MODEM_MAXSTATE +}; + + +#define IPCP_ALLOW_IP 0x01u +#define IPCP_ALLOW_DNS1 0x02u +#define IPCP_ALLOW_DNS2 0x04u +#define IPCP_ALLOW_NBNS1 0x08u +#define IPCP_ALLOW_NBNS2 0x10u + +struct pico_device_ppp { + struct pico_device dev; + int autoreconnect; + enum ppp_modem_state modem_state; + enum ppp_lcp_state lcp_state; + enum ppp_auth_state auth_state; + enum ppp_ipcp_state ipcp_state; + enum pico_ppp_state state; + char apn[PPP_MAX_APN]; + char password[PPP_MAX_PASSWORD]; + char username[PPP_MAX_USERNAME]; + uint16_t lcpopt_local; + uint16_t lcpopt_peer; + uint8_t *pkt; + uint32_t len; + uint16_t rej; + uint16_t auth; + int (*serial_recv)(struct pico_device *dev, void *buf, int len); + int (*serial_send)(struct pico_device *dev, const void *buf, int len); + int (*serial_set_speed)(struct pico_device *dev, uint32_t speed); + uint32_t ipcp_allowed_fields; + uint32_t ipcp_ip; + uint32_t ipcp_dns1; + uint32_t ipcp_nbns1; + uint32_t ipcp_dns2; + uint32_t ipcp_nbns2; + uint32_t timer; + uint8_t timer_val; + uint8_t timer_count; + uint8_t frame_id; + uint8_t timer_on; + uint16_t mru; +}; + + +/* Unit test interceptor */ +static void (*mock_modem_state)(struct pico_device_ppp *ppp, enum ppp_modem_event event) = NULL; +static void (*mock_lcp_state)(struct pico_device_ppp *ppp, enum ppp_lcp_event event) = NULL; +static void (*mock_auth_state)(struct pico_device_ppp *ppp, enum ppp_auth_event event) = NULL; +static void (*mock_ipcp_state)(struct pico_device_ppp *ppp, enum ppp_ipcp_event event) = NULL; + +/* Debug prints */ +#ifdef PPP_DEBUG +static void lcp_optflags_print(struct pico_device_ppp *ppp, uint8_t *opts, uint32_t opts_len); +#endif + +#define PPP_TIMER_ON_MODEM 0x01u +#define PPP_TIMER_ON_LCPREQ 0x04u +#define PPP_TIMER_ON_LCPTERM 0x08u +#define PPP_TIMER_ON_AUTH 0x10u +#define PPP_TIMER_ON_IPCP 0x20u + +/* Escape and send */ +static int ppp_serial_send_escape(struct pico_device_ppp *ppp, void *buf, int len) +{ + uint8_t *in_buf = (uint8_t *)buf; + uint8_t *out_buf = NULL; + int esc_char_count = 0; + int newlen = 0, ret = -1; + int i, j; + +#ifdef PPP_DEBUG + { + uint32_t idx; + if (len > 0) { + ppp_dbg("PPP >>>> "); + for(idx = 0; idx < (uint32_t)len; idx++) { + ppp_dbg(" %02x", ((uint8_t *)buf)[idx]); + } + ppp_dbg("\n"); + } + } +#endif + + for (i = 1; i < (len - 1); i++) /* from 1 to len -1, as start/stop are not escaped */ + { + if (((in_buf[i] + 1u) >> 1) == 0x3Fu) + esc_char_count++; + } + if (!esc_char_count) { + return ppp->serial_send(&ppp->dev, buf, len); + } + + newlen = len + esc_char_count; + out_buf = PICO_ZALLOC((uint32_t)newlen); + if(!out_buf) + return -1; + + /* Start byte. */ + out_buf[0] = in_buf[0]; + for(i = 1, j = 1; i < (len - 1); i++) { + if (((in_buf[i] + 1u) >> 1) == 0x3Fu) { + out_buf[j++] = PPPF_CTRL_ESC; + out_buf[j++] = in_buf[i] ^ 0x20; + } else { + out_buf[j++] = in_buf[i]; + } + } + /* Stop byte. */ + out_buf[newlen - 1] = in_buf[len - 1]; + + ret = ppp->serial_send(&ppp->dev, out_buf, newlen); + + PICO_FREE(out_buf); + + if (ret == newlen) + return len; + + return ret; + +} + +static void lcp_timer_start(struct pico_device_ppp *ppp, uint8_t timer_type) +{ + uint8_t count = 0; + ppp->timer_on |= timer_type; + + if (ppp->timer_val == 0) { + ppp->timer_val = PICO_PPP_DEFAULT_TIMER; + } + + if (timer_type == PPP_TIMER_ON_LCPTERM) { + count = PICO_PPP_DEFAULT_MAX_TERMINATE; + } + + if (timer_type == PPP_TIMER_ON_LCPREQ) { + count = PICO_PPP_DEFAULT_MAX_CONFIGURE; + } + + if (timer_type == 0) { + ppp->timer_on |= PPP_TIMER_ON_LCPREQ; + ppp->timer_count = 0; + } + + if (ppp->timer_count == 0) + ppp->timer_count = count; +} + +static void lcp_zero_restart_count(struct pico_device_ppp *ppp) +{ + lcp_timer_start(ppp, 0); +} + +static void lcp_timer_stop(struct pico_device_ppp *ppp, uint8_t timer_type) +{ + ppp->timer_on = (uint8_t)ppp->timer_on & (uint8_t)(~timer_type); +} + + +#define PPP_FSM_MAX_ACTIONS 3 + +struct pico_ppp_fsm { + int next_state; + void (*event_handler[PPP_FSM_MAX_ACTIONS]) (struct pico_device_ppp *); +}; + +#define LCPOPT_SET_LOCAL(ppp, opt) ppp->lcpopt_local |= (uint16_t)(1u << opt) +#define LCPOPT_SET_PEER(ppp, opt) ppp->lcpopt_peer |= (uint16_t)(1u << opt) +#define LCPOPT_UNSET_LOCAL(ppp, opt) ppp->lcpopt_local &= (uint16_t)~(1u << opt) +#define LCPOPT_UNSET_LOCAL_MASK(ppp, opt) ppp->lcpopt_local &= (uint16_t)~(opt) +#define LCPOPT_UNSET_PEER(ppp, opt) ppp->lcpopt_peer &= (uint16_t)~(1u << opt) +#define LCPOPT_ISSET_LOCAL(ppp, opt) ((ppp->lcpopt_local & (uint16_t)(1u << opt)) != 0) +#define LCPOPT_ISSET_PEER(ppp, opt) ((ppp->lcpopt_peer & (uint16_t)(1u << opt)) != 0) + +static void evaluate_modem_state(struct pico_device_ppp *ppp, enum ppp_modem_event event); +static void evaluate_lcp_state(struct pico_device_ppp *ppp, enum ppp_lcp_event event); +static void evaluate_auth_state(struct pico_device_ppp *ppp, enum ppp_auth_event event); +static void evaluate_ipcp_state(struct pico_device_ppp *ppp, enum ppp_ipcp_event event); + + +static uint32_t ppp_ctl_packet_size(struct pico_device_ppp *ppp, uint16_t proto, uint32_t *size) +{ + uint32_t prefix = 0; + + IGNORE_PARAMETER(ppp); + IGNORE_PARAMETER(proto); + + prefix += PPP_HDR_SIZE; /* 7e ff 03 ... */ + prefix += PPP_PROTO_SLOT_SIZE; + *size += prefix; + *size += PPP_FCS_SIZE; + (*size)++; /* STOP byte 0x7e */ + return prefix; +} + +/* CRC16 / FCS Calculation */ +static uint16_t ppp_fcs_char(uint16_t old_crc, uint8_t data) +{ + uint16_t word = (old_crc ^ data) & (uint16_t)0x00FFu; + word = (uint16_t)(word ^ (uint16_t)((word << 4u) & (uint16_t)0x00FFu)); + word = (uint16_t)((word << 8u) ^ (word << 3u) ^ (word >> 4u)); + return ((old_crc >> 8u) ^ word); +} + +static uint16_t ppp_fcs_continue(uint16_t fcs, uint8_t *buf, uint32_t len) +{ + uint8_t *pos = buf; + for (pos = buf; pos < buf + len; pos++) + { + fcs = ppp_fcs_char(fcs, *pos); + } + return fcs; +} + +static uint16_t ppp_fcs_finish(uint16_t fcs) +{ + return fcs ^ 0xFFFF; +} + +static uint16_t ppp_fcs_start(uint8_t *buf, uint32_t len) +{ + uint16_t fcs = 0xFFFF; + return ppp_fcs_continue(fcs, buf, len); +} + +static int ppp_fcs_verify(uint8_t *buf, uint32_t len) +{ + uint16_t fcs = ppp_fcs_start(buf, len - 2); + fcs = ppp_fcs_finish(fcs); + if ((((fcs & 0xFF00u) >> 8) != buf[len - 1]) || ((fcs & 0xFFu) != buf[len - 2])) { + return -1; + } + + return 0; +} + +/* Serial send (DTE->DCE) functions */ +static int pico_ppp_ctl_send(struct pico_device *dev, uint16_t code, uint8_t *pkt, uint32_t len) +{ + struct pico_device_ppp *ppp = (struct pico_device_ppp *) dev; + uint16_t fcs; + uint8_t *ptr = pkt; + int i = 0; + + if (!ppp->serial_send) + return (int)len; + + /* PPP Header */ + ptr[i++] = PPPF_FLAG_SEQ; + ptr[i++] = PPPF_ADDR; + ptr[i++] = PPPF_CTRL; + /* protocol */ + ptr[i++] = (uint8_t)(code & 0xFFu); + ptr[i++] = (uint8_t)((code & 0xFF00u) >> 8); + + /* payload is already in place. Calculate FCS. */ + fcs = ppp_fcs_start(pkt + 1, len - 4); /* FCS excludes: start (1), FCS(2), stop(1), total 4 bytes */ + fcs = ppp_fcs_finish(fcs); + pkt[len - 3] = (uint8_t)(fcs & 0xFFu); + pkt[len - 2] = (uint8_t)((fcs & 0xFF00u) >> 8); + pkt[len - 1] = PPPF_FLAG_SEQ; + ppp_serial_send_escape(ppp, pkt, (int)len); + return (int)len; +} + +static uint8_t pico_ppp_data_buffer[PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE + PICO_PPP_MTU + PPP_FCS_SIZE + 1]; +static int pico_ppp_send(struct pico_device *dev, void *buf, int len) +{ + struct pico_device_ppp *ppp = (struct pico_device_ppp *) dev; + uint16_t fcs = 0; + int fcs_start; + int i = 0; + + ppp_dbg(" >>>>>>>>> PPP OUT\n"); + + if (ppp->ipcp_state != PPP_IPCP_STATE_OPENED) + return len; + + if (!ppp->serial_send) + return len; + + pico_ppp_data_buffer[i++] = PPPF_FLAG_SEQ; + if (!LCPOPT_ISSET_PEER(ppp, LCPOPT_ADDRCTL_COMP)) + { + pico_ppp_data_buffer[i++] = PPPF_ADDR; + pico_ppp_data_buffer[i++] = PPPF_CTRL; + } + + fcs_start = i; + + if (!LCPOPT_ISSET_PEER(ppp, LCPOPT_PROTO_COMP)) + { + pico_ppp_data_buffer[i++] = 0x00; + } + + pico_ppp_data_buffer[i++] = 0x21; + memcpy(pico_ppp_data_buffer + i, buf, (uint32_t)len); + i += len; + fcs = ppp_fcs_start(pico_ppp_data_buffer + fcs_start, (uint32_t)(i - fcs_start)); + fcs = ppp_fcs_finish(fcs); + pico_ppp_data_buffer[i++] = (uint8_t)(fcs & 0xFFu); + pico_ppp_data_buffer[i++] = (uint8_t)((fcs & 0xFF00u) >> 8); + pico_ppp_data_buffer[i++] = PPPF_FLAG_SEQ; + + ppp_serial_send_escape(ppp, pico_ppp_data_buffer, i); + return len; +} + + +/* FSM functions */ + +static void ppp_modem_start_timer(struct pico_device_ppp *ppp) +{ + ppp->timer_on = ppp->timer_on | PPP_TIMER_ON_MODEM; + ppp->timer_val = PICO_PPP_DEFAULT_TIMER; +} + +#define PPP_AT_CREG0 "ATZ\r\n" +static void ppp_modem_send_reset(struct pico_device_ppp *ppp) +{ + if (ppp->serial_send) + ppp->serial_send(&ppp->dev, PPP_AT_CREG0, strlen(PPP_AT_CREG0)); + + ppp_modem_start_timer(ppp); +} + +#define PPP_AT_CREG1 "ATE0\r\n" +static void ppp_modem_send_echo(struct pico_device_ppp *ppp) +{ + if (ppp->serial_send) + ppp->serial_send(&ppp->dev, PPP_AT_CREG1, strlen(PPP_AT_CREG1)); + + ppp_modem_start_timer(ppp); +} + +#define PPP_AT_CREG2 "AT+CREG=1\r\n" +static void ppp_modem_send_creg(struct pico_device_ppp *ppp) +{ + if (ppp->serial_send) + ppp->serial_send(&ppp->dev, PPP_AT_CREG2, strlen(PPP_AT_CREG2)); + + ppp_modem_start_timer(ppp); +} + +#define PPP_AT_CGREG "AT+CGREG=1\r\n" +static void ppp_modem_send_cgreg(struct pico_device_ppp *ppp) +{ + if (ppp->serial_send) + ppp->serial_send(&ppp->dev, PPP_AT_CGREG, strlen(PPP_AT_CGREG)); + + ppp_modem_start_timer(ppp); +} + + +#define PPP_AT_CGDCONT "AT+CGDCONT=1,\"IP\",\"%s\",,,\r\n" +static void ppp_modem_send_cgdcont(struct pico_device_ppp *ppp) +{ + char at_cgdcont[200]; + + if (ppp->serial_send) { + snprintf(at_cgdcont, 200, PPP_AT_CGDCONT, ppp->apn); + ppp->serial_send(&ppp->dev, at_cgdcont, (int)strlen(at_cgdcont)); + } + + ppp_modem_start_timer(ppp); +} + + +#define PPP_AT_CGATT "AT+CGATT=1\r\n" +static void ppp_modem_send_cgatt(struct pico_device_ppp *ppp) +{ + if (ppp->serial_send) + ppp->serial_send(&ppp->dev, PPP_AT_CGATT, strlen(PPP_AT_CGATT)); + + ppp_modem_start_timer(ppp); +} + +#ifdef PICOTCP_PPP_SUPPORT_QUERIES +#define PPP_AT_CGATT_Q "AT+CGATT?\r\n" +static void ppp_modem_send_cgatt_q(struct pico_device_ppp *ppp) +{ + if (ppp->serial_send) + ppp->serial_send(&ppp->dev, PPP_AT_CGATT_Q, strlen(PPP_AT_CGATT_Q)); + + ppp_modem_start_timer(ppp); +} +#define PPP_AT_CGDCONT_Q "AT+CGDCONT?\r\n" +static void ppp_modem_send_cgdcont_q(struct pico_device_ppp *ppp) +{ + if (ppp->serial_send) + ppp->serial_send(&ppp->dev, PPP_AT_CGDCONT_Q, strlen(PPP_AT_CGDCONT_Q)); + + ppp_modem_start_timer(ppp); +} + +#define PPP_AT_CGREG_Q "AT+CGREG?\r\n" +static void ppp_modem_send_cgreg_q(struct pico_device_ppp *ppp) +{ + if (ppp->serial_send) + ppp->serial_send(&ppp->dev, PPP_AT_CGREG_Q, strlen(PPP_AT_CGREG_Q)); + + ppp_modem_start_timer(ppp); +} + +#define PPP_AT_CREG3 "AT+CREG?\r\n" +static void ppp_modem_send_creg_q(struct pico_device_ppp *ppp) +{ + if (ppp->serial_send) + ppp->serial_send(&ppp->dev, PPP_AT_CREG3, strlen(PPP_AT_CREG3)); + + ppp_modem_start_timer(ppp); +} +#endif /* PICOTCP_PPP_SUPPORT_QUERIES */ + +#define PPP_AT_DIALIN "ATD*99***1#\r\n" +static void ppp_modem_send_dial(struct pico_device_ppp *ppp) +{ + if (ppp->serial_send) + ppp->serial_send(&ppp->dev, PPP_AT_DIALIN, strlen(PPP_AT_DIALIN)); + + ppp_modem_start_timer(ppp); + ppp->timer_val = PICO_PPP_DEFAULT_MAX_DIALTIME; +} + +static void ppp_modem_connected(struct pico_device_ppp *ppp) +{ + ppp_dbg("PPP: Modem connected to peer.\n"); + evaluate_lcp_state(ppp, PPP_LCP_EVENT_UP); +} + +#define PPP_ATH "+++ATH\r\n" +static void ppp_modem_disconnected(struct pico_device_ppp *ppp) +{ + ppp_dbg("PPP: Modem disconnected.\n"); + if (ppp->serial_send) + ppp->serial_send(&ppp->dev, PPP_ATH, strlen(PPP_ATH)); + + evaluate_lcp_state(ppp, PPP_LCP_EVENT_DOWN); +} + +static const struct pico_ppp_fsm ppp_modem_fsm[PPP_MODEM_STATE_MAX][PPP_MODEM_EVENT_MAX] = { + [PPP_MODEM_STATE_INITIAL] = { + [PPP_MODEM_EVENT_START] = { PPP_MODEM_STATE_RESET, {ppp_modem_send_reset} }, + [PPP_MODEM_EVENT_STOP] = { PPP_MODEM_STATE_INITIAL, {} }, + [PPP_MODEM_EVENT_OK] = { PPP_MODEM_STATE_INITIAL, {} }, + [PPP_MODEM_EVENT_CONNECT] = { PPP_MODEM_STATE_INITIAL, {} }, + [PPP_MODEM_EVENT_TIMEOUT] = { PPP_MODEM_STATE_INITIAL, {ppp_modem_send_reset} } + }, + [PPP_MODEM_STATE_RESET] = { + [PPP_MODEM_EVENT_START] = { PPP_MODEM_STATE_RESET, {} }, + [PPP_MODEM_EVENT_STOP] = { PPP_MODEM_STATE_INITIAL, {} }, + [PPP_MODEM_EVENT_OK] = { PPP_MODEM_STATE_ECHO, { ppp_modem_send_echo } }, + [PPP_MODEM_EVENT_CONNECT] = { PPP_MODEM_STATE_RESET, {} }, + [PPP_MODEM_EVENT_TIMEOUT] = { PPP_MODEM_STATE_RESET, {ppp_modem_send_reset} } + }, + [PPP_MODEM_STATE_ECHO] = { + [PPP_MODEM_EVENT_START] = { PPP_MODEM_STATE_ECHO, {} }, + [PPP_MODEM_EVENT_STOP] = { PPP_MODEM_STATE_INITIAL, {} }, + [PPP_MODEM_EVENT_OK] = { PPP_MODEM_STATE_CREG, { ppp_modem_send_creg } }, + [PPP_MODEM_EVENT_CONNECT] = { PPP_MODEM_STATE_ECHO, {} }, + [PPP_MODEM_EVENT_TIMEOUT] = { PPP_MODEM_STATE_RESET, {ppp_modem_send_reset} } + }, + [PPP_MODEM_STATE_CREG] = { + [PPP_MODEM_EVENT_START] = { PPP_MODEM_STATE_CREG, {} }, + [PPP_MODEM_EVENT_STOP] = { PPP_MODEM_STATE_INITIAL, {} }, + [PPP_MODEM_EVENT_OK] = { PPP_MODEM_STATE_CGREG, { ppp_modem_send_cgreg } }, + [PPP_MODEM_EVENT_CONNECT] = { PPP_MODEM_STATE_CREG, {} }, + [PPP_MODEM_EVENT_TIMEOUT] = { PPP_MODEM_STATE_RESET, {ppp_modem_send_reset} } + }, + [PPP_MODEM_STATE_CGREG] = { + [PPP_MODEM_EVENT_START] = { PPP_MODEM_STATE_CGREG, {} }, + [PPP_MODEM_EVENT_STOP] = { PPP_MODEM_STATE_INITIAL, {} }, + [PPP_MODEM_EVENT_OK] = { PPP_MODEM_STATE_CGDCONT, { ppp_modem_send_cgdcont } }, + [PPP_MODEM_EVENT_CONNECT] = { PPP_MODEM_STATE_CGREG, {} }, + [PPP_MODEM_EVENT_TIMEOUT] = { PPP_MODEM_STATE_RESET, {ppp_modem_send_reset} } + }, + [PPP_MODEM_STATE_CGDCONT] = { + [PPP_MODEM_EVENT_START] = { PPP_MODEM_STATE_CGDCONT, {} }, + [PPP_MODEM_EVENT_STOP] = { PPP_MODEM_STATE_INITIAL, {} }, + [PPP_MODEM_EVENT_OK] = { PPP_MODEM_STATE_CGATT, { ppp_modem_send_cgatt } }, + [PPP_MODEM_EVENT_CONNECT] = { PPP_MODEM_STATE_CGDCONT, {} }, + [PPP_MODEM_EVENT_TIMEOUT] = { PPP_MODEM_STATE_RESET, {ppp_modem_send_reset} } + }, + [PPP_MODEM_STATE_CGATT] = { + [PPP_MODEM_EVENT_START] = { PPP_MODEM_STATE_CGATT, {} }, + [PPP_MODEM_EVENT_STOP] = { PPP_MODEM_STATE_INITIAL, {} }, + [PPP_MODEM_EVENT_OK] = { PPP_MODEM_STATE_DIAL, { ppp_modem_send_dial } }, + [PPP_MODEM_EVENT_CONNECT] = { PPP_MODEM_STATE_CGATT, {} }, + [PPP_MODEM_EVENT_TIMEOUT] = { PPP_MODEM_STATE_RESET, {ppp_modem_send_reset} } + }, + [PPP_MODEM_STATE_DIAL] = { + [PPP_MODEM_EVENT_START] = { PPP_MODEM_STATE_DIAL, {} }, + [PPP_MODEM_EVENT_STOP] = { PPP_MODEM_STATE_INITIAL, {} }, + [PPP_MODEM_EVENT_OK] = { PPP_MODEM_STATE_DIAL, {} }, + [PPP_MODEM_EVENT_CONNECT] = { PPP_MODEM_STATE_CONNECTED, { ppp_modem_connected } }, + [PPP_MODEM_EVENT_TIMEOUT] = { PPP_MODEM_STATE_RESET, {ppp_modem_send_reset} } + }, + [PPP_MODEM_STATE_CONNECTED] = { + [PPP_MODEM_EVENT_START] = { PPP_MODEM_STATE_CONNECTED, {} }, + [PPP_MODEM_EVENT_STOP] = { PPP_MODEM_STATE_INITIAL, { ppp_modem_disconnected } }, + [PPP_MODEM_EVENT_OK] = { PPP_MODEM_STATE_CONNECTED, {} }, + [PPP_MODEM_EVENT_CONNECT] = { PPP_MODEM_STATE_CONNECTED, {} }, + [PPP_MODEM_EVENT_TIMEOUT] = { PPP_MODEM_STATE_CONNECTED, {} } + } +}; +static void evaluate_modem_state(struct pico_device_ppp *ppp, enum ppp_modem_event event) +{ + const struct pico_ppp_fsm *fsm; + int i; + if (mock_modem_state) { + mock_modem_state(ppp, event); + return; + } + + fsm = &ppp_modem_fsm[ppp->modem_state][event]; + + ppp->modem_state = (enum ppp_modem_state)fsm->next_state; + + for (i = 0; i < PPP_FSM_MAX_ACTIONS; i++) { + if (fsm->event_handler[i]) + fsm->event_handler[i](ppp); + } +} + +static void ppp_modem_recv(struct pico_device_ppp *ppp, void *data, uint32_t len) +{ + IGNORE_PARAMETER(len); + + ppp_dbg("PPP: Recv: '%s'\n", (char *)data); + + if (strcmp(data, "OK") == 0) { + evaluate_modem_state(ppp, PPP_MODEM_EVENT_OK); + } + + if (strcmp(data, "ERROR") == 0) { + evaluate_modem_state(ppp, PPP_MODEM_EVENT_STOP); + } + + if (strncmp(data, "CONNECT", 7) == 0) { + evaluate_modem_state(ppp, PPP_MODEM_EVENT_CONNECT); + } +} + +static void lcp_send_configure_request(struct pico_device_ppp *ppp) +{ +# define MY_LCP_REQ_SIZE 12 /* Max value. */ + struct pico_lcp_hdr *req; + uint8_t *lcpbuf, *opts; + uint32_t size = MY_LCP_REQ_SIZE; + uint32_t prefix; + uint32_t optsize = 0; + + prefix = ppp_ctl_packet_size(ppp, PPP_PROTO_LCP, &size); + lcpbuf = PICO_ZALLOC(size); + if (!lcpbuf) + return; + + req = (struct pico_lcp_hdr *)(lcpbuf + prefix); + + opts = lcpbuf + prefix + (sizeof(struct pico_lcp_hdr)); + /* uint8_t my_pkt[] = { 0x7e, 0xff, 0x03, 0xc0, 0x21, 0x01, 0x00, 0x00, 0x06, 0x07, 0x02, 0x64, 0x7b, 0x7e }; */ + + ppp_dbg("Sending LCP CONF REQ\n"); + req->code = PICO_CONF_REQ; + req->id = ppp->frame_id++; + + if (LCPOPT_ISSET_LOCAL(ppp, LCPOPT_PROTO_COMP)) { + opts[optsize++] = LCPOPT_PROTO_COMP; + opts[optsize++] = LCPOPT_LEN[LCPOPT_PROTO_COMP]; + } + + if (LCPOPT_ISSET_LOCAL(ppp, LCPOPT_MRU)) { + opts[optsize++] = LCPOPT_MRU; + opts[optsize++] = LCPOPT_LEN[LCPOPT_MRU]; + opts[optsize++] = (uint8_t)((ppp->mru >> 8) & 0xFF); + opts[optsize++] = (uint8_t)(ppp->mru & 0xFF); + } else { + ppp->mru = PICO_PPP_MRU; + } + + if (LCPOPT_ISSET_LOCAL(ppp, LCPOPT_ADDRCTL_COMP)) { + opts[optsize++] = LCPOPT_ADDRCTL_COMP; + opts[optsize++] = LCPOPT_LEN[LCPOPT_ADDRCTL_COMP]; + } + + req->len = short_be((uint16_t)((unsigned long)optsize + sizeof(struct pico_lcp_hdr))); + +#ifdef PPP_DEBUG + lcp_optflags_print(ppp, opts, optsize); +#endif + + pico_ppp_ctl_send(&ppp->dev, PPP_PROTO_LCP, + lcpbuf, /* Start of PPP packet */ + (uint32_t)(prefix + /* PPP Header, etc. */ + sizeof(struct pico_lcp_hdr) + /* LCP HDR */ + optsize + /* Actual options size */ + PPP_FCS_SIZE + /* FCS at the end of the frame */ + 1u) /* STOP Byte */ + ); + PICO_FREE(lcpbuf); + ppp->timer_val = PICO_PPP_DEFAULT_TIMER; + lcp_timer_start(ppp, PPP_TIMER_ON_LCPREQ); +} + +#ifdef PPP_DEBUG +static void lcp_optflags_print(struct pico_device_ppp *ppp, uint8_t *opts, uint32_t opts_len) +{ + uint8_t *p = opts; + int off; + IGNORE_PARAMETER(ppp); + ppp_dbg("Parsing options:\n"); + while(p < (opts + opts_len)) { + int i; + + ppp_dbg("-- LCP opt: %d - len: %d - data:", p[0], p[1]); + for (i=0; imru = (uint16_t)((p[2] << 8) + p[3]); + break; + case LCPOPT_AUTH: + ppp_dbg("Setting AUTH to %02x%02x\n", p[2], p[3]); + ppp->auth = (uint16_t)((p[2] << 8) + p[3]); + break; + default: + break; + } + } + + off = p[1]; /* opt length field */ + if (!off) + break; + + p += off; + } +#ifdef PPP_DEBUG + lcp_optflags_print(ppp, pkt + sizeof(struct pico_lcp_hdr), (uint32_t)(len - sizeof(struct pico_lcp_hdr)) ); +#endif + return flags; +} + + +static void lcp_send_configure_ack(struct pico_device_ppp *ppp) +{ + uint8_t ack[ppp->len + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE + sizeof(struct pico_lcp_hdr) + PPP_FCS_SIZE + 1]; + struct pico_lcp_hdr *ack_hdr = (struct pico_lcp_hdr *) (ack + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE); + struct pico_lcp_hdr *lcpreq = (struct pico_lcp_hdr *)ppp->pkt; + memcpy(ack + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE, ppp->pkt, ppp->len); + ack_hdr->code = PICO_CONF_ACK; + ack_hdr->id = lcpreq->id; + ack_hdr->len = lcpreq->len; + ppp_dbg("Sending LCP CONF ACK\n"); + pico_ppp_ctl_send(&ppp->dev, PPP_PROTO_LCP, ack, + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE + /* PPP Header, etc. */ + short_be(lcpreq->len) + /* Actual options size + hdr (whole lcp packet) */ + PPP_FCS_SIZE + /* FCS at the end of the frame */ + 1 /* STOP Byte */ + ); +} + +static void lcp_send_terminate_request(struct pico_device_ppp *ppp) +{ + uint8_t term[PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE + sizeof(struct pico_lcp_hdr) + PPP_FCS_SIZE + 1]; + struct pico_lcp_hdr *term_hdr = (struct pico_lcp_hdr *) (term + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE); + term_hdr->code = PICO_CONF_TERM; + term_hdr->id = ppp->frame_id++; + term_hdr->len = short_be((uint16_t)sizeof(struct pico_lcp_hdr)); + ppp_dbg("Sending LCP TERMINATE REQUEST\n"); + pico_ppp_ctl_send(&ppp->dev, PPP_PROTO_LCP, term, + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE + /* PPP Header, etc. */ + sizeof(struct pico_lcp_hdr) + /* Actual options size + hdr (whole lcp packet) */ + PPP_FCS_SIZE + /* FCS at the end of the frame */ + 1 /* STOP Byte */ + ); + lcp_timer_start(ppp, PPP_TIMER_ON_LCPTERM); +} + +static void lcp_send_terminate_ack(struct pico_device_ppp *ppp) +{ + uint8_t ack[ppp->len + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE + sizeof(struct pico_lcp_hdr) + PPP_FCS_SIZE + 1]; + struct pico_lcp_hdr *ack_hdr = (struct pico_lcp_hdr *) (ack + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE); + struct pico_lcp_hdr *lcpreq = (struct pico_lcp_hdr *)ppp->pkt; + memcpy(ack + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE, ppp->pkt, ppp->len); + ack_hdr->code = PICO_CONF_TERM_ACK; + ack_hdr->id = lcpreq->id; + ack_hdr->len = lcpreq->len; + ppp_dbg("Sending LCP TERM ACK\n"); + pico_ppp_ctl_send(&ppp->dev, PPP_PROTO_LCP, ack, + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE + /* PPP Header, etc. */ + short_be(lcpreq->len) + /* Actual options size + hdr (whole lcp packet) */ + PPP_FCS_SIZE + /* FCS at the end of the frame */ + 1 /* STOP Byte */ + ); +} + +static void lcp_send_configure_nack(struct pico_device_ppp *ppp) +{ + uint8_t reject[64]; + uint8_t *p = ppp->pkt + sizeof(struct pico_lcp_hdr); + struct pico_lcp_hdr *lcpreq = (struct pico_lcp_hdr *)ppp->pkt; + struct pico_lcp_hdr *lcprej = (struct pico_lcp_hdr *)(reject + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE); + uint8_t *dst_opts = reject + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE + sizeof(struct pico_lcp_hdr); + uint32_t dstopts_len = 0; + ppp_dbg("CONF_NACK: rej = %04X\n", ppp->rej); + while (p < (ppp->pkt + ppp->len)) { + uint8_t i = 0; + if ((1u << p[0]) & ppp->rej || (p[0] > 8u)) { /* Reject anything we dont support or with option id >8 */ + ppp_dbg("rejecting option %d -- ", p[0]); + dst_opts[dstopts_len++] = p[0]; + + ppp_dbg("len: %d -- ", p[1]); + dst_opts[dstopts_len++] = p[1]; + + ppp_dbg("data: "); + for(i = 0; i < p[1]-2u; i++) { /* length includes type, length and data fields */ + dst_opts[dstopts_len++] = p[2 + i]; + ppp_dbg("%02X ", p[2+i]); + } + ppp_dbg("\n"); + } + + p += p[1]; + } + lcprej->code = PICO_CONF_REJ; + lcprej->id = lcpreq->id; + lcprej->len = short_be((uint16_t)(dstopts_len + sizeof(struct pico_lcp_hdr))); + + ppp_dbg("Sending LCP CONF REJ\n"); +#ifdef PPP_DEBUG + lcp_optflags_print(ppp, dst_opts, dstopts_len); +#endif + + pico_ppp_ctl_send(&ppp->dev, PPP_PROTO_LCP, reject, + (uint32_t)(PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE + /* PPP Header, etc. */ + sizeof(struct pico_lcp_hdr) + /* LCP HDR */ + dstopts_len + /* Actual options size */ + PPP_FCS_SIZE + /* FCS at the end of the frame */ + 1u) /* STOP Byte */ + ); +} + +static void lcp_process_in(struct pico_device_ppp *ppp, uint8_t *pkt, uint32_t len) +{ + uint16_t optflags; + if (!ppp) + return; + if (pkt[0] == PICO_CONF_REQ) { + uint16_t rejected = 0; + ppp_dbg("Received LCP CONF REQ\n"); + optflags = lcp_optflags(ppp, pkt, len, 1u); + rejected = (uint16_t)(optflags & (~ppp->lcpopt_local)); + ppp->pkt = pkt; + ppp->len = len; + ppp->rej = rejected; + if (rejected) { + evaluate_lcp_state(ppp, PPP_LCP_EVENT_RCR_NEG); + } else { + ppp->lcpopt_peer = optflags; + evaluate_lcp_state(ppp, PPP_LCP_EVENT_RCR_POS); + } + + return; + } + + if (pkt[0] == PICO_CONF_ACK) { + ppp_dbg("Received LCP CONF ACK\nOptflags: %04x\n", lcp_optflags(NULL, pkt, len, 0u)); + evaluate_lcp_state(ppp, PPP_LCP_EVENT_RCA); + return; + } + + if (pkt[0] == PICO_CONF_NAK) { + /* Every instance of the received Configuration Options is recognizable, but some values are not acceptable */ + optflags = lcp_optflags(ppp, pkt, len, 1u); /* We want our options adjusted */ + ppp_dbg("Received LCP CONF NAK - changed optflags: %04X\n", optflags); + evaluate_lcp_state(ppp, PPP_LCP_EVENT_RCN); + return; + } + + if (pkt[0] == PICO_CONF_REJ) { + /* Some Configuration Options received in a Configure-Request are not recognizable or are not acceptable for negotiation */ + optflags = lcp_optflags(ppp, pkt, len, 0u); + ppp_dbg("Received LCP CONF REJ - will disable optflags: %04X\n", optflags); + /* Disable the options that are not supported by the peer */ + LCPOPT_UNSET_LOCAL_MASK(ppp, optflags); + evaluate_lcp_state(ppp, PPP_LCP_EVENT_RCN); + return; + } + + if (pkt[0] == PICO_CONF_ECHO_REQ) { + ppp_dbg("Received LCP ECHO REQ\n"); + evaluate_lcp_state(ppp, PPP_LCP_EVENT_RXR); + return; + } +} + +static void pap_process_in(struct pico_device_ppp *ppp, uint8_t *pkt, uint32_t len) +{ + struct pico_pap_hdr *p = (struct pico_pap_hdr *)pkt; + (void)len; + if (!p) + return; + + if (ppp->auth != 0xc023) + return; + + switch(p->code) { + case PAP_AUTH_ACK: + ppp_dbg("PAP: Received Authentication OK!\n"); + evaluate_auth_state(ppp, PPP_AUTH_EVENT_RAA); + break; + case PAP_AUTH_NAK: + ppp_dbg("PAP: Received Authentication Reject!\n"); + evaluate_auth_state(ppp, PPP_AUTH_EVENT_RAN); + break; + + default: + ppp_dbg("PAP: Received invalid packet with code %d\n", p->code); + } +} + + +static void chap_process_in(struct pico_device_ppp *ppp, uint8_t *pkt, uint32_t len) +{ + struct pico_chap_hdr *ch = (struct pico_chap_hdr *)pkt; + + if (!pkt) + return; + + if (ppp->auth != 0xc223) + return; + + switch(ch->code) { + case CHAP_CHALLENGE: + ppp_dbg("Received CHAP CHALLENGE\n"); + ppp->pkt = pkt; + ppp->len = len; + evaluate_auth_state(ppp, PPP_AUTH_EVENT_RAC); + break; + case CHAP_SUCCESS: + ppp_dbg("Received CHAP SUCCESS\n"); + evaluate_auth_state(ppp, PPP_AUTH_EVENT_RAA); + break; + case CHAP_FAILURE: + ppp_dbg("Received CHAP FAILURE\n"); + evaluate_auth_state(ppp, PPP_AUTH_EVENT_RAN); + break; + } +} + + +static void ipcp_send_ack(struct pico_device_ppp *ppp) +{ + uint8_t ack[ppp->len + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE + sizeof(struct pico_lcp_hdr) + PPP_FCS_SIZE + 1]; + struct pico_ipcp_hdr *ack_hdr = (struct pico_ipcp_hdr *) (ack + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE); + struct pico_ipcp_hdr *ipcpreq = (struct pico_ipcp_hdr *)ppp->pkt; + memcpy(ack + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE, ppp->pkt, ppp->len); + ack_hdr->code = PICO_CONF_ACK; + ack_hdr->id = ipcpreq->id; + ack_hdr->len = ipcpreq->len; + ppp_dbg("Sending IPCP CONF ACK\n"); + pico_ppp_ctl_send(&ppp->dev, PPP_PROTO_IPCP, ack, + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE + /* PPP Header, etc. */ + short_be(ipcpreq->len) + /* Actual options size + hdr (whole ipcp packet) */ + PPP_FCS_SIZE + /* FCS at the end of the frame */ + 1 /* STOP Byte */ + ); +} + +static inline uint32_t ipcp_request_options_size(struct pico_device_ppp *ppp) +{ + uint32_t size = 0; + +/* if (ppp->ipcp_ip) */ + size += IPCP_ADDR_LEN; +/* if (ppp->ipcp_dns1) */ + size += IPCP_ADDR_LEN; +/* if (ppp->ipcp_dns2) */ + size += IPCP_ADDR_LEN; + if (ppp->ipcp_nbns1) + size += IPCP_ADDR_LEN; + + if (ppp->ipcp_nbns2) + size += IPCP_ADDR_LEN; + + return size; +} + +static int ipcp_request_add_address(uint8_t *dst, uint8_t tag, uint32_t arg) +{ + uint32_t addr = long_be(arg); + dst[0] = tag; + dst[1] = IPCP_ADDR_LEN; + dst[2] = (uint8_t)((addr & 0xFF000000u) >> 24); + dst[3] = (uint8_t)((addr & 0x00FF0000u) >> 16); + dst[4] = (uint8_t)((addr & 0x0000FF00u) >> 8); + dst[5] = (addr & 0x000000FFu); + return IPCP_ADDR_LEN; +} + +static void ipcp_request_fill(struct pico_device_ppp *ppp, uint8_t *opts) +{ + if (ppp->ipcp_allowed_fields & IPCP_ALLOW_IP) + opts += ipcp_request_add_address(opts, IPCP_OPT_IP, ppp->ipcp_ip); + if (ppp->ipcp_allowed_fields & IPCP_ALLOW_DNS1) + opts += ipcp_request_add_address(opts, IPCP_OPT_DNS1, ppp->ipcp_dns1); + if (ppp->ipcp_allowed_fields & IPCP_ALLOW_DNS2) + opts += ipcp_request_add_address(opts, IPCP_OPT_DNS2, ppp->ipcp_dns2); + if ((ppp->ipcp_allowed_fields & IPCP_ALLOW_NBNS1) && (ppp->ipcp_nbns1)) + opts += ipcp_request_add_address(opts, IPCP_OPT_NBNS1, ppp->ipcp_nbns1); + if ((ppp->ipcp_allowed_fields & IPCP_ALLOW_NBNS2) && (ppp->ipcp_nbns2)) + opts += ipcp_request_add_address(opts, IPCP_OPT_NBNS2, ppp->ipcp_nbns2); +} + +static void ipcp_send_req(struct pico_device_ppp *ppp) +{ + uint8_t ipcp_req[PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE + sizeof(struct pico_ipcp_hdr) + ipcp_request_options_size(ppp) + PPP_FCS_SIZE + 1]; + uint32_t prefix = PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE; + struct pico_ipcp_hdr *ih = (struct pico_ipcp_hdr *) (ipcp_req + prefix); + uint8_t *p = ipcp_req + prefix + sizeof(struct pico_ipcp_hdr); + uint16_t len = (uint16_t)(ipcp_request_options_size(ppp) + sizeof(struct pico_ipcp_hdr)); + + ih->id = ppp->frame_id++; + ih->code = PICO_CONF_REQ; + ih->len = short_be(len); + ipcp_request_fill(ppp, p); + + ppp_dbg("Sending IPCP CONF REQ, ipcp size = %d\n", len); + pico_ppp_ctl_send(&ppp->dev, PPP_PROTO_IPCP, + ipcp_req, /* Start of PPP packet */ + (uint32_t)(prefix + /* PPP Header, etc. */ + (uint32_t)len + /* IPCP Header + options */ + PPP_FCS_SIZE + /* FCS at the end of the frame */ + 1u) /* STOP Byte */ + ); +} + +static void ipcp_reject_vj(struct pico_device_ppp *ppp, uint8_t *comp_req) +{ + uint8_t ipcp_req[PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE + sizeof(struct pico_ipcp_hdr) + IPCP_VJ_LEN + PPP_FCS_SIZE + 1]; + uint32_t prefix = PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE; + struct pico_ipcp_hdr *ih = (struct pico_ipcp_hdr *) (ipcp_req + prefix); + uint8_t *p = ipcp_req + prefix + sizeof(struct pico_ipcp_hdr); + uint32_t i; + + ih->id = ppp->frame_id++; + ih->code = PICO_CONF_REQ; + ih->len = short_be(IPCP_VJ_LEN + sizeof(struct pico_ipcp_hdr)); + for(i = 0; i < IPCP_OPT_VJ; i++) + p[i] = comp_req[i + sizeof(struct pico_ipcp_hdr)]; + ppp_dbg("Sending IPCP CONF REJ VJ\n"); + pico_ppp_ctl_send(&ppp->dev, PPP_PROTO_IPCP, + ipcp_req, /* Start of PPP packet */ + (uint32_t)(prefix + /* PPP Header, etc. */ + sizeof(struct pico_ipcp_hdr) + /* LCP HDR */ + IPCP_VJ_LEN + /* Actual options size */ + PPP_FCS_SIZE + /* FCS at the end of the frame */ + 1u) /* STOP Byte */ + ); +} + +static void ppp_ipv4_conf(struct pico_device_ppp *ppp) +{ + struct pico_ip4 ip; + struct pico_ip4 nm; + struct pico_ip4 dns1; + struct pico_ip4 dns2; + struct pico_ip4 any = { + 0 + }; + ip.addr = ppp->ipcp_ip; + nm.addr = 0xFFFFFF00; + pico_ipv4_link_add(&ppp->dev, ip, nm); + pico_ipv4_route_add(any, any, any, 1, pico_ipv4_link_by_dev(&ppp->dev)); + + dns1.addr = ppp->ipcp_dns1; + dns2.addr = ppp->ipcp_dns2; + pico_dns_client_nameserver(&dns1, PICO_DNS_NS_ADD); + pico_dns_client_nameserver(&dns2, PICO_DNS_NS_ADD); +} + + +static void ipcp_process_in(struct pico_device_ppp *ppp, uint8_t *pkt, uint32_t len) +{ + struct pico_ipcp_hdr *ih = (struct pico_ipcp_hdr *)pkt; + uint8_t *p = pkt + sizeof(struct pico_ipcp_hdr); + int reject = 0; + while (p < pkt + len) { + if (p[0] == IPCP_OPT_VJ) { + reject++; + } + + if (p[0] == IPCP_OPT_IP) { + if (ih->code != PICO_CONF_REJ) + ppp->ipcp_ip = long_be((uint32_t)((p[2] << 24) + (p[3] << 16) + (p[4] << 8) + p[5])); + else { + ppp->ipcp_allowed_fields &= (~IPCP_ALLOW_IP); + ppp->ipcp_ip = 0; + } + } + + if (p[0] == IPCP_OPT_DNS1) { + if (ih->code != PICO_CONF_REJ) + ppp->ipcp_dns1 = long_be((uint32_t)((p[2] << 24) + (p[3] << 16) + (p[4] << 8) + p[5])); + else { + ppp->ipcp_allowed_fields &= (~IPCP_ALLOW_DNS1); + ppp->ipcp_dns1 = 0; + } + } + + if (p[0] == IPCP_OPT_NBNS1) { + if (ih->code != PICO_CONF_REJ) + ppp->ipcp_nbns1 = long_be((uint32_t)((p[2] << 24) + (p[3] << 16) + (p[4] << 8) + p[5])); + else { + ppp->ipcp_allowed_fields &= (~IPCP_ALLOW_NBNS1); + ppp->ipcp_nbns1 = 0; + } + } + + if (p[0] == IPCP_OPT_DNS2) { + if (ih->code != PICO_CONF_REJ) + ppp->ipcp_dns2 = long_be((uint32_t)((p[2] << 24) + (p[3] << 16) + (p[4] << 8) + p[5])); + else { + ppp->ipcp_allowed_fields &= (~IPCP_ALLOW_DNS2); + ppp->ipcp_dns2 = 0; + } + } + + if (p[0] == IPCP_OPT_NBNS2) { + if (ih->code != PICO_CONF_REJ) + ppp->ipcp_nbns2 = long_be((uint32_t)((p[2] << 24) + (p[3] << 16) + (p[4] << 8) + p[5])); + else { + ppp->ipcp_allowed_fields &= (~IPCP_ALLOW_NBNS2); + ppp->ipcp_nbns2 = 0; + } + } + + p += p[1]; + } + if (reject) { + ipcp_reject_vj(ppp, p); + return; + } + + ppp->pkt = pkt; + ppp->len = len; + + switch(ih->code) { + case PICO_CONF_ACK: + ppp_dbg("Received IPCP CONF ACK\n"); + evaluate_ipcp_state(ppp, PPP_IPCP_EVENT_RCA); + break; + case PICO_CONF_REQ: + ppp_dbg("Received IPCP CONF REQ\n"); + evaluate_ipcp_state(ppp, PPP_IPCP_EVENT_RCR_POS); + break; + case PICO_CONF_NAK: + ppp_dbg("Received IPCP CONF NAK\n"); + evaluate_ipcp_state(ppp, PPP_IPCP_EVENT_RCN); + break; + case PICO_CONF_REJ: + ppp_dbg("Received IPCP CONF REJ\n"); + + evaluate_ipcp_state(ppp, PPP_IPCP_EVENT_RCN); + break; + } +} + +static void ipcp6_process_in(struct pico_device_ppp *ppp, uint8_t *pkt, uint32_t len) +{ + IGNORE_PARAMETER(ppp); + IGNORE_PARAMETER(pkt); + IGNORE_PARAMETER(len); +} + +static void ppp_process_packet_payload(struct pico_device_ppp *ppp, uint8_t *pkt, uint32_t len) +{ + if (pkt[0] == 0xc0) { + /* Link control packet */ + if (pkt[1] == 0x21) { + /* LCP */ + lcp_process_in(ppp, pkt + 2, len - 2); + } + + if (pkt[1] == 0x23) { + /* PAP */ + pap_process_in(ppp, pkt + 2, len - 2); + } + + return; + } + + if ((pkt[0] == 0xc2) && (pkt[1] == 0x23)) { + /* CHAP */ + chap_process_in(ppp, pkt + 2, len - 2); + return; + } + + if (pkt[0] == 0x80) { + /* IP assignment (IPCP/IPCP6) */ + if (pkt[1] == 0x21) { + /* IPCP */ + ipcp_process_in(ppp, pkt + 2, len - 2); + } + + if (pkt[1] == 0x57) { + /* IPCP6 */ + ipcp6_process_in(ppp, pkt + 2, len - 2); + } + + return; + } + + if (pkt[0] == 0x00) { + /* Uncompressed protocol: leading zero. */ + pkt++; + len--; + } + + if ((pkt[0] == 0x21) || (pkt[0] == 0x57)) { + /* IPv4 /v6 Data */ + pico_stack_recv(&ppp->dev, pkt + 1, len - 1); + return; + } + + ppp_dbg("PPP: Unrecognized protocol %02x%02x\n", pkt[0], pkt[1]); +} + +static void ppp_process_packet(struct pico_device_ppp *ppp, uint8_t *pkt, uint32_t len) +{ + /* Verify incoming FCS */ + if (ppp_fcs_verify(pkt, len) != 0) + return; + + /* Remove trailing FCS */ + len -= 2; + + /* Remove ADDR/CTRL, then process */ + if ((pkt[0] == PPPF_ADDR) && (pkt[1] == PPPF_CTRL)) { + pkt += 2; + len -= 2; + } + + ppp_process_packet_payload(ppp, pkt, len); + +} + +static void ppp_recv_data(struct pico_device_ppp *ppp, void *data, uint32_t len) +{ + uint8_t *pkt = (uint8_t *)data; + +#ifdef PPP_DEBUG + uint32_t idx; + if (len > 0) { + ppp_dbg("PPP <<<<< "); + for(idx = 0; idx < len; idx++) { + ppp_dbg(" %02x", ((uint8_t *)data)[idx]); + } + ppp_dbg("\n"); + } +#endif + + ppp_process_packet(ppp, pkt, len); +} + +static void lcp_this_layer_up(struct pico_device_ppp *ppp) +{ + ppp_dbg("PPP: LCP up.\n"); + + switch (ppp->auth) { + case 0x0000: + evaluate_auth_state(ppp, PPP_AUTH_EVENT_UP_NONE); + break; + case 0xc023: + evaluate_auth_state(ppp, PPP_AUTH_EVENT_UP_PAP); + break; + case 0xc223: + evaluate_auth_state(ppp, PPP_AUTH_EVENT_UP_CHAP); + break; + default: + ppp_dbg("PPP: Unknown authentication protocol.\n"); + break; + } +} + +static void lcp_this_layer_down(struct pico_device_ppp *ppp) +{ + ppp_dbg("PPP: LCP down.\n"); + evaluate_auth_state(ppp, PPP_AUTH_EVENT_DOWN); +} + +static void lcp_this_layer_started(struct pico_device_ppp *ppp) +{ + ppp_dbg("PPP: LCP started.\n"); + evaluate_modem_state(ppp, PPP_MODEM_EVENT_START); +} + +static void lcp_this_layer_finished(struct pico_device_ppp *ppp) +{ + ppp_dbg("PPP: LCP finished.\n"); + evaluate_modem_state(ppp, PPP_MODEM_EVENT_STOP); +} + +static void lcp_initialize_restart_count(struct pico_device_ppp *ppp) +{ + lcp_timer_start(ppp, PPP_TIMER_ON_LCPREQ); +} + +static void lcp_send_code_reject(struct pico_device_ppp *ppp) +{ + IGNORE_PARAMETER(ppp); +} + +static void lcp_send_echo_reply(struct pico_device_ppp *ppp) +{ + uint8_t reply[ppp->len + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE + sizeof(struct pico_lcp_hdr) + PPP_FCS_SIZE + 1]; + struct pico_lcp_hdr *reply_hdr = (struct pico_lcp_hdr *) (reply + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE); + struct pico_lcp_hdr *lcpreq = (struct pico_lcp_hdr *)ppp->pkt; + memcpy(reply + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE, ppp->pkt, ppp->len); + reply_hdr->code = PICO_CONF_ECHO_REP; + reply_hdr->id = lcpreq->id; + reply_hdr->len = lcpreq->len; + ppp_dbg("Sending LCP ECHO REPLY\n"); + pico_ppp_ctl_send(&ppp->dev, PPP_PROTO_LCP, reply, + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE + /* PPP Header, etc. */ + short_be(lcpreq->len) + /* Actual options size + hdr (whole lcp packet) */ + PPP_FCS_SIZE + /* FCS at the end of the frame */ + 1 /* STOP Byte */ + ); +} + +static const struct pico_ppp_fsm ppp_lcp_fsm[PPP_LCP_STATE_MAX][PPP_LCP_EVENT_MAX] = { + [PPP_LCP_STATE_INITIAL] = { + [PPP_LCP_EVENT_UP] = { PPP_LCP_STATE_CLOSED, {} }, + [PPP_LCP_EVENT_DOWN] = { PPP_LCP_STATE_INITIAL, {} }, + [PPP_LCP_EVENT_OPEN] = { PPP_LCP_STATE_STARTING, { lcp_this_layer_started } }, + [PPP_LCP_EVENT_CLOSE] = { PPP_LCP_STATE_INITIAL, {} }, + [PPP_LCP_EVENT_TO_POS] = { PPP_LCP_STATE_INITIAL, {} }, + [PPP_LCP_EVENT_TO_NEG] = { PPP_LCP_STATE_INITIAL, {} }, + [PPP_LCP_EVENT_RCR_POS] = { PPP_LCP_STATE_INITIAL, {} }, + [PPP_LCP_EVENT_RCR_NEG] = { PPP_LCP_STATE_INITIAL, {} }, + [PPP_LCP_EVENT_RCA] = { PPP_LCP_STATE_INITIAL, {} }, + [PPP_LCP_EVENT_RCN] = { PPP_LCP_STATE_INITIAL, {} }, + [PPP_LCP_EVENT_RTR] = { PPP_LCP_STATE_INITIAL, {} }, + [PPP_LCP_EVENT_RTA] = { PPP_LCP_STATE_INITIAL, {} }, + [PPP_LCP_EVENT_RUC] = { PPP_LCP_STATE_INITIAL, {} }, + [PPP_LCP_EVENT_RXJ_POS] = { PPP_LCP_STATE_INITIAL, {} }, + [PPP_LCP_EVENT_RXJ_NEG] = { PPP_LCP_STATE_INITIAL, {} }, + [PPP_LCP_EVENT_RXR] = { PPP_LCP_STATE_INITIAL, {} } + }, + [PPP_LCP_STATE_STARTING] = { + [PPP_LCP_EVENT_UP] = { PPP_LCP_STATE_REQ_SENT, { lcp_send_configure_request } }, + [PPP_LCP_EVENT_DOWN] = { PPP_LCP_STATE_STARTING, {} }, + [PPP_LCP_EVENT_OPEN] = { PPP_LCP_STATE_STARTING, {} }, + [PPP_LCP_EVENT_CLOSE] = { PPP_LCP_STATE_INITIAL, { lcp_this_layer_finished } }, + [PPP_LCP_EVENT_TO_POS] = { PPP_LCP_STATE_STARTING, {} }, + [PPP_LCP_EVENT_TO_NEG] = { PPP_LCP_STATE_STARTING, {} }, + [PPP_LCP_EVENT_RCR_POS] = { PPP_LCP_STATE_STARTING, {} }, + [PPP_LCP_EVENT_RCR_NEG] = { PPP_LCP_STATE_STARTING, {} }, + [PPP_LCP_EVENT_RCA] = { PPP_LCP_STATE_STARTING, {} }, + [PPP_LCP_EVENT_RCN] = { PPP_LCP_STATE_STARTING, {} }, + [PPP_LCP_EVENT_RTR] = { PPP_LCP_STATE_STARTING, {} }, + [PPP_LCP_EVENT_RTA] = { PPP_LCP_STATE_STARTING, {} }, + [PPP_LCP_EVENT_RUC] = { PPP_LCP_STATE_STARTING, {} }, + [PPP_LCP_EVENT_RXJ_POS] = { PPP_LCP_STATE_STARTING, {} }, + [PPP_LCP_EVENT_RXJ_NEG] = { PPP_LCP_STATE_STARTING, {} }, + [PPP_LCP_EVENT_RXR] = { PPP_LCP_STATE_STARTING, {} } + }, + [PPP_LCP_STATE_CLOSED] = { + [PPP_LCP_EVENT_UP] = { PPP_LCP_STATE_CLOSED, {} }, + [PPP_LCP_EVENT_DOWN] = { PPP_LCP_STATE_INITIAL, {} }, + [PPP_LCP_EVENT_OPEN] = { PPP_LCP_STATE_REQ_SENT, { lcp_send_configure_request} }, + [PPP_LCP_EVENT_CLOSE] = { PPP_LCP_STATE_CLOSED, {} }, + [PPP_LCP_EVENT_TO_POS] = { PPP_LCP_STATE_CLOSED, {} }, + [PPP_LCP_EVENT_TO_NEG] = { PPP_LCP_STATE_CLOSED, {} }, + [PPP_LCP_EVENT_RCR_POS] = { PPP_LCP_STATE_CLOSED, { lcp_send_terminate_ack } }, + [PPP_LCP_EVENT_RCR_NEG] = { PPP_LCP_STATE_CLOSED, { lcp_send_terminate_ack } }, + [PPP_LCP_EVENT_RCA] = { PPP_LCP_STATE_CLOSED, { lcp_send_terminate_ack } }, + [PPP_LCP_EVENT_RCN] = { PPP_LCP_STATE_CLOSED, { lcp_send_terminate_ack } }, + [PPP_LCP_EVENT_RTR] = { PPP_LCP_STATE_CLOSED, { lcp_send_terminate_ack } }, + [PPP_LCP_EVENT_RTA] = { PPP_LCP_STATE_CLOSED, {} }, + [PPP_LCP_EVENT_RUC] = { PPP_LCP_STATE_CLOSED, { lcp_send_code_reject } }, + [PPP_LCP_EVENT_RXJ_POS] = { PPP_LCP_STATE_CLOSED, {} }, + [PPP_LCP_EVENT_RXJ_NEG] = { PPP_LCP_STATE_CLOSED, { lcp_this_layer_finished } }, + [PPP_LCP_EVENT_RXR] = { PPP_LCP_STATE_CLOSED, {} } + }, + [PPP_LCP_STATE_STOPPED] = { + [PPP_LCP_EVENT_UP] = { PPP_LCP_STATE_STOPPED, {} }, + [PPP_LCP_EVENT_DOWN] = { PPP_LCP_STATE_STARTING, { lcp_this_layer_started } }, + [PPP_LCP_EVENT_OPEN] = { PPP_LCP_STATE_STOPPED, {}}, + [PPP_LCP_EVENT_CLOSE] = { PPP_LCP_STATE_CLOSED, {}}, + [PPP_LCP_EVENT_TO_POS] = { PPP_LCP_STATE_STOPPED, {} }, + [PPP_LCP_EVENT_TO_NEG] = { PPP_LCP_STATE_STOPPED, {} }, + [PPP_LCP_EVENT_RCR_POS] = { PPP_LCP_STATE_ACK_SENT, + { lcp_send_configure_request, lcp_send_configure_ack}}, + [PPP_LCP_EVENT_RCR_NEG] = { PPP_LCP_STATE_REQ_SENT, + { lcp_send_configure_request, lcp_send_configure_nack}}, + [PPP_LCP_EVENT_RCA] = { PPP_LCP_STATE_STOPPED, { lcp_send_terminate_ack } }, + [PPP_LCP_EVENT_RCN] = { PPP_LCP_STATE_STOPPED, { lcp_send_terminate_ack } }, + [PPP_LCP_EVENT_RTR] = { PPP_LCP_STATE_STOPPED, { lcp_send_terminate_ack } }, + [PPP_LCP_EVENT_RTA] = { PPP_LCP_STATE_STOPPED, {} }, + [PPP_LCP_EVENT_RUC] = { PPP_LCP_STATE_STOPPED, { lcp_send_code_reject } }, + [PPP_LCP_EVENT_RXJ_POS] = { PPP_LCP_STATE_STOPPED, {} }, + [PPP_LCP_EVENT_RXJ_NEG] = { PPP_LCP_STATE_STOPPED, { lcp_this_layer_finished } }, + [PPP_LCP_EVENT_RXR] = { PPP_LCP_STATE_STOPPED, {} } + }, + [PPP_LCP_STATE_CLOSING] = { + [PPP_LCP_EVENT_UP] = { PPP_LCP_STATE_CLOSING, {} }, + [PPP_LCP_EVENT_DOWN] = { PPP_LCP_STATE_INITIAL, {} }, + [PPP_LCP_EVENT_OPEN] = { PPP_LCP_STATE_STOPPING, {} }, + [PPP_LCP_EVENT_CLOSE] = { PPP_LCP_STATE_CLOSING, {} }, + [PPP_LCP_EVENT_TO_POS] = { PPP_LCP_STATE_CLOSING, { lcp_send_terminate_request } }, + [PPP_LCP_EVENT_TO_NEG] = { PPP_LCP_STATE_CLOSED, { lcp_this_layer_finished } }, + [PPP_LCP_EVENT_RCR_POS] = { PPP_LCP_STATE_CLOSING, {} }, + [PPP_LCP_EVENT_RCR_NEG] = { PPP_LCP_STATE_CLOSING, {} }, + [PPP_LCP_EVENT_RCA] = { PPP_LCP_STATE_CLOSING, {} }, + [PPP_LCP_EVENT_RCN] = { PPP_LCP_STATE_CLOSING, {} }, + [PPP_LCP_EVENT_RTR] = { PPP_LCP_STATE_CLOSING, { lcp_send_terminate_ack } }, + [PPP_LCP_EVENT_RTA] = { PPP_LCP_STATE_CLOSED, { lcp_this_layer_finished } }, + [PPP_LCP_EVENT_RUC] = { PPP_LCP_STATE_CLOSING, { lcp_send_code_reject } }, + [PPP_LCP_EVENT_RXJ_POS] = { PPP_LCP_STATE_CLOSING, {} }, + [PPP_LCP_EVENT_RXJ_NEG] = { PPP_LCP_STATE_CLOSED, { lcp_this_layer_finished } }, + [PPP_LCP_EVENT_RXR] = { PPP_LCP_STATE_CLOSING, {} } + }, + [PPP_LCP_STATE_STOPPING] = { + [PPP_LCP_EVENT_UP] = { PPP_LCP_STATE_STOPPING, {} }, + [PPP_LCP_EVENT_DOWN] = { PPP_LCP_STATE_STARTING, {} }, + [PPP_LCP_EVENT_OPEN] = { PPP_LCP_STATE_STOPPING, {} }, + [PPP_LCP_EVENT_CLOSE] = { PPP_LCP_STATE_CLOSING, {} }, + [PPP_LCP_EVENT_TO_POS] = { PPP_LCP_STATE_STOPPING, { lcp_send_terminate_request } }, + [PPP_LCP_EVENT_TO_NEG] = { PPP_LCP_STATE_STOPPED, { lcp_this_layer_finished } }, + [PPP_LCP_EVENT_RCR_POS] = { PPP_LCP_STATE_STOPPING, {} }, + [PPP_LCP_EVENT_RCR_NEG] = { PPP_LCP_STATE_STOPPING, {} }, + [PPP_LCP_EVENT_RCA] = { PPP_LCP_STATE_STOPPING, {} }, + [PPP_LCP_EVENT_RCN] = { PPP_LCP_STATE_STOPPING, {} }, + [PPP_LCP_EVENT_RTR] = { PPP_LCP_STATE_STOPPING, { lcp_send_terminate_ack } }, + [PPP_LCP_EVENT_RTA] = { PPP_LCP_STATE_STOPPED, { lcp_this_layer_finished } }, + [PPP_LCP_EVENT_RUC] = { PPP_LCP_STATE_STOPPING, { lcp_send_code_reject } }, + [PPP_LCP_EVENT_RXJ_POS] = { PPP_LCP_STATE_STOPPING, {} }, + [PPP_LCP_EVENT_RXJ_NEG] = { PPP_LCP_STATE_STOPPED, { lcp_this_layer_finished } }, + [PPP_LCP_EVENT_RXR] = { PPP_LCP_STATE_STOPPING, {} } + }, + [PPP_LCP_STATE_REQ_SENT] = { + [PPP_LCP_EVENT_UP] = { PPP_LCP_STATE_REQ_SENT, {} }, + [PPP_LCP_EVENT_DOWN] = { PPP_LCP_STATE_STARTING, {} }, + [PPP_LCP_EVENT_OPEN] = { PPP_LCP_STATE_REQ_SENT, {} }, + [PPP_LCP_EVENT_CLOSE] = { PPP_LCP_STATE_CLOSING, { lcp_send_terminate_request } }, + [PPP_LCP_EVENT_TO_POS] = { PPP_LCP_STATE_REQ_SENT, { lcp_send_configure_request } }, + [PPP_LCP_EVENT_TO_NEG] = { PPP_LCP_STATE_STOPPED, { lcp_this_layer_finished } }, + [PPP_LCP_EVENT_RCR_POS] = { PPP_LCP_STATE_ACK_SENT, { lcp_send_configure_ack } }, + [PPP_LCP_EVENT_RCR_NEG] = { PPP_LCP_STATE_REQ_SENT, { lcp_send_configure_nack } }, + [PPP_LCP_EVENT_RCA] = { PPP_LCP_STATE_ACK_RCVD, { lcp_initialize_restart_count } }, + [PPP_LCP_EVENT_RCN] = { PPP_LCP_STATE_REQ_SENT, { lcp_send_configure_request} }, + [PPP_LCP_EVENT_RTR] = { PPP_LCP_STATE_REQ_SENT, { lcp_send_terminate_ack } }, + [PPP_LCP_EVENT_RTA] = { PPP_LCP_STATE_REQ_SENT, {} }, + [PPP_LCP_EVENT_RUC] = { PPP_LCP_STATE_REQ_SENT, { lcp_send_code_reject } }, + [PPP_LCP_EVENT_RXJ_POS] = { PPP_LCP_STATE_REQ_SENT, {} }, + [PPP_LCP_EVENT_RXJ_NEG] = { PPP_LCP_STATE_STOPPED, { lcp_this_layer_finished } }, + [PPP_LCP_EVENT_RXR] = { PPP_LCP_STATE_REQ_SENT, {} } + }, + [PPP_LCP_STATE_ACK_RCVD] = { + [PPP_LCP_EVENT_UP] = { PPP_LCP_STATE_ACK_RCVD, {} }, + [PPP_LCP_EVENT_DOWN] = { PPP_LCP_STATE_STARTING, {} }, + [PPP_LCP_EVENT_OPEN] = { PPP_LCP_STATE_ACK_RCVD, {} }, + [PPP_LCP_EVENT_CLOSE] = { PPP_LCP_STATE_CLOSING, { lcp_send_terminate_request} }, + [PPP_LCP_EVENT_TO_POS] = { PPP_LCP_STATE_REQ_SENT, { lcp_send_configure_request } }, + [PPP_LCP_EVENT_TO_NEG] = { PPP_LCP_STATE_STOPPED, { lcp_this_layer_finished } }, + [PPP_LCP_EVENT_RCR_POS] = { PPP_LCP_STATE_OPENED, { lcp_send_configure_ack, lcp_this_layer_up} }, + [PPP_LCP_EVENT_RCR_NEG] = { PPP_LCP_STATE_ACK_RCVD, { lcp_send_configure_nack } }, + [PPP_LCP_EVENT_RCA] = { PPP_LCP_STATE_REQ_SENT, { lcp_send_configure_request } }, + [PPP_LCP_EVENT_RCN] = { PPP_LCP_STATE_REQ_SENT, { lcp_send_configure_request } }, + [PPP_LCP_EVENT_RTR] = { PPP_LCP_STATE_REQ_SENT, { lcp_send_terminate_ack } }, + [PPP_LCP_EVENT_RTA] = { PPP_LCP_STATE_REQ_SENT, {} }, + [PPP_LCP_EVENT_RUC] = { PPP_LCP_STATE_ACK_RCVD, { lcp_send_code_reject } }, + [PPP_LCP_EVENT_RXJ_POS] = { PPP_LCP_STATE_REQ_SENT, {} }, + [PPP_LCP_EVENT_RXJ_NEG] = { PPP_LCP_STATE_STOPPED, { lcp_this_layer_finished } }, + [PPP_LCP_EVENT_RXR] = { PPP_LCP_STATE_ACK_RCVD, {} } + }, + [PPP_LCP_STATE_ACK_SENT] = { + [PPP_LCP_EVENT_UP] = { PPP_LCP_STATE_ACK_SENT, {} }, + [PPP_LCP_EVENT_DOWN] = { PPP_LCP_STATE_STARTING, {} }, + [PPP_LCP_EVENT_OPEN] = { PPP_LCP_STATE_ACK_SENT, {} }, + [PPP_LCP_EVENT_CLOSE] = { PPP_LCP_STATE_CLOSING, { lcp_send_terminate_request} }, + [PPP_LCP_EVENT_TO_POS] = { PPP_LCP_STATE_ACK_SENT, { lcp_send_configure_request } }, + [PPP_LCP_EVENT_TO_NEG] = { PPP_LCP_STATE_STOPPED, { lcp_this_layer_finished } }, + [PPP_LCP_EVENT_RCR_POS] = { PPP_LCP_STATE_ACK_SENT, { lcp_send_configure_ack } }, + [PPP_LCP_EVENT_RCR_NEG] = { PPP_LCP_STATE_REQ_SENT, { lcp_send_configure_nack } }, + [PPP_LCP_EVENT_RCA] = { PPP_LCP_STATE_OPENED, { lcp_this_layer_up} }, + [PPP_LCP_EVENT_RCN] = { PPP_LCP_STATE_ACK_SENT, { lcp_send_configure_request} }, + [PPP_LCP_EVENT_RTR] = { PPP_LCP_STATE_REQ_SENT, { lcp_send_terminate_ack } }, + [PPP_LCP_EVENT_RTA] = { PPP_LCP_STATE_ACK_SENT, {} }, + [PPP_LCP_EVENT_RUC] = { PPP_LCP_STATE_ACK_SENT, { lcp_send_code_reject } }, + [PPP_LCP_EVENT_RXJ_POS] = { PPP_LCP_STATE_ACK_SENT, {} }, + [PPP_LCP_EVENT_RXJ_NEG] = { PPP_LCP_STATE_STOPPED, { lcp_this_layer_finished } }, + [PPP_LCP_EVENT_RXR] = { PPP_LCP_STATE_ACK_SENT, {} } + }, + [PPP_LCP_STATE_OPENED] = { + [PPP_LCP_EVENT_UP] = { PPP_LCP_STATE_OPENED, {} }, + [PPP_LCP_EVENT_DOWN] = { PPP_LCP_STATE_STARTING, {lcp_this_layer_down } }, + [PPP_LCP_EVENT_OPEN] = { PPP_LCP_STATE_OPENED, {} }, + [PPP_LCP_EVENT_CLOSE] = { PPP_LCP_STATE_CLOSING, + { lcp_this_layer_down, lcp_send_terminate_request }}, + [PPP_LCP_EVENT_TO_POS] = { PPP_LCP_STATE_OPENED, {} }, + [PPP_LCP_EVENT_TO_NEG] = { PPP_LCP_STATE_OPENED, {} }, + [PPP_LCP_EVENT_RCR_POS] = { PPP_LCP_STATE_ACK_SENT, + { lcp_this_layer_down, lcp_send_terminate_request, lcp_send_configure_ack }}, + [PPP_LCP_EVENT_RCR_NEG] = { PPP_LCP_STATE_REQ_SENT, + { lcp_this_layer_down, lcp_send_configure_request, lcp_send_configure_nack }}, + [PPP_LCP_EVENT_RCA] = { PPP_LCP_STATE_REQ_SENT, { lcp_this_layer_down, lcp_send_terminate_request } }, + [PPP_LCP_EVENT_RCN] = { PPP_LCP_STATE_REQ_SENT, { lcp_this_layer_down, lcp_send_terminate_request } }, + [PPP_LCP_EVENT_RTR] = { PPP_LCP_STATE_STOPPING, { lcp_this_layer_down, lcp_zero_restart_count, lcp_send_terminate_ack} }, + [PPP_LCP_EVENT_RTA] = { PPP_LCP_STATE_REQ_SENT, { lcp_this_layer_down, lcp_send_terminate_request} }, + [PPP_LCP_EVENT_RUC] = { PPP_LCP_STATE_OPENED, { lcp_send_code_reject } }, + [PPP_LCP_EVENT_RXJ_POS] = { PPP_LCP_STATE_OPENED, { } }, + [PPP_LCP_EVENT_RXJ_NEG] = { PPP_LCP_STATE_STOPPING, + {lcp_this_layer_down, lcp_send_terminate_request}}, + [PPP_LCP_EVENT_RXR] = { PPP_LCP_STATE_OPENED, { lcp_send_echo_reply} } + } +}; + +static void evaluate_lcp_state(struct pico_device_ppp *ppp, enum ppp_lcp_event event) +{ + const struct pico_ppp_fsm *fsm, *next_fsm_to; + int i; + if (!ppp) + return; + + if (mock_lcp_state) { + mock_lcp_state(ppp, event); + return; + } + + fsm = &ppp_lcp_fsm[ppp->lcp_state][event]; + ppp->lcp_state = (enum ppp_lcp_state)fsm->next_state; + /* RFC1661: The states in which the Restart timer is running are identifiable by + * the presence of TO events. + */ + next_fsm_to = &ppp_lcp_fsm[ppp->lcp_state][PPP_LCP_EVENT_TO_POS]; + if (!next_fsm_to->event_handler[0]) { + /* The Restart timer is stopped when transitioning + * from any state where the timer is running to a state where the timer + * is not running. + */ + lcp_timer_stop(ppp, PPP_TIMER_ON_LCPREQ); + lcp_timer_stop(ppp, PPP_TIMER_ON_LCPTERM); + } + + for (i = 0; i < PPP_FSM_MAX_ACTIONS; i++) { + if (fsm->event_handler[i]) + fsm->event_handler[i](ppp); + } +} + +static void auth(struct pico_device_ppp *ppp) +{ + ppp_dbg("PPP: Authenticated.\n"); + ppp->ipcp_allowed_fields = 0xFFFF; + evaluate_ipcp_state(ppp, PPP_IPCP_EVENT_UP); +} + +static void deauth(struct pico_device_ppp *ppp) +{ + ppp_dbg("PPP: De-authenticated.\n"); + evaluate_ipcp_state(ppp, PPP_IPCP_EVENT_DOWN); +} + +static void auth_abort(struct pico_device_ppp *ppp) +{ + ppp_dbg("PPP: Authentication failed!\n"); + ppp->timer_on = (uint8_t) (ppp->timer_on & (~PPP_TIMER_ON_AUTH)); + evaluate_lcp_state(ppp, PPP_LCP_EVENT_CLOSE); + +} + +static void auth_req(struct pico_device_ppp *ppp) +{ + uint16_t ppp_usr_len = 0; + uint16_t ppp_pwd_len = 0; + uint8_t *req = NULL, *p; + struct pico_pap_hdr *hdr; + uint16_t pap_len = 0; + uint8_t field_len = 0; + ppp_usr_len = (uint16_t)strlen(ppp->username); + ppp_pwd_len = (uint16_t)strlen(ppp->password); + + pap_len = (uint16_t)(sizeof(struct pico_pap_hdr) + 1u + 1u + ppp_usr_len + ppp_pwd_len); + + req = PICO_ZALLOC(PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE + pap_len + PPP_FCS_SIZE + 1); + if (!req) + return; + + hdr = (struct pico_pap_hdr *) (req + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE); + + hdr->code = PAP_AUTH_REQ; + hdr->id = ppp->frame_id++; + hdr->len = short_be(pap_len); + + p = req + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE + sizeof(struct pico_pap_hdr); + + /* Populate authentication domain */ + field_len = (uint8_t)(ppp_usr_len & 0xFF); + *p = field_len; + ++p; + if (ppp_usr_len > 0) { + memcpy(p, ppp->username, ppp_usr_len); + p += ppp_usr_len; + } + + /* Populate authentication password */ + field_len = (uint8_t)(ppp_pwd_len & 0xFF); + *p = field_len; + ++p; + if (ppp_pwd_len > 0) { + memcpy(p, ppp->password, ppp_pwd_len); + p += ppp_pwd_len; + } + ppp_dbg("PAP: Sending authentication request.\n"); + pico_ppp_ctl_send(&ppp->dev, PPP_PROTO_PAP, + req, /* Start of PPP packet */ + (uint32_t)( + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE + /* PPP Header, etc. */ + pap_len + /* Authentication packet len */ + PPP_FCS_SIZE + /* FCS */ + 1) /* STOP Byte */ + ); + PICO_FREE(req); +} + +static void auth_rsp(struct pico_device_ppp *ppp) +{ + struct pico_chap_hdr *ch = (struct pico_chap_hdr *)ppp->pkt; + uint8_t resp[PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE + sizeof(struct pico_chap_hdr) + CHAP_MD5_SIZE + PPP_FCS_SIZE + 2]; + struct pico_chap_hdr *rh = (struct pico_chap_hdr *) (resp + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE); + uint8_t *md5resp = resp + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE + sizeof(struct pico_chap_hdr) + 1; + uint8_t *md5resp_len = resp + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE + sizeof(struct pico_chap_hdr); + uint8_t *challenge; + uint32_t i = 0, pwdlen; + uint8_t *recvd_challenge_len = ppp->pkt + sizeof(struct pico_chap_hdr); + uint8_t *recvd_challenge = recvd_challenge_len + 1; + size_t challenge_size = CHALLENGE_SIZE(ppp, ch); + + challenge = PICO_ZALLOC(challenge_size); + + if (!challenge) + return; + + + pwdlen = (uint32_t)strlen(ppp->password); + challenge[i++] = ch->id; + memcpy(challenge + i, ppp->password, pwdlen); + i += pwdlen; + memcpy(challenge + i, recvd_challenge, *recvd_challenge_len); + i += *recvd_challenge_len; + pico_md5sum(md5resp, challenge, i); + PICO_FREE(challenge); + rh->id = ch->id; + rh->code = CHAP_RESPONSE; + rh->len = short_be(CHAP_MD5_SIZE + sizeof(struct pico_chap_hdr) + 1); + *md5resp_len = CHAP_MD5_SIZE; + ppp_dbg("Sending CHAP RESPONSE, \n"); + pico_ppp_ctl_send(&ppp->dev, PPP_PROTO_CHAP, + resp, /* Start of PPP packet */ + (uint32_t)( + PPP_HDR_SIZE + PPP_PROTO_SLOT_SIZE + /* PPP Header, etc. */ + sizeof(struct pico_chap_hdr) + /* CHAP HDR */ + 1 + /* Value length */ + CHAP_MD5_SIZE + /* Actual payload size */ + PPP_FCS_SIZE + /* FCS at the end of the frame */ + 1) /* STOP Byte */ + ); +} + +static void auth_start_timer(struct pico_device_ppp *ppp) +{ + ppp->timer_on = ppp->timer_on | PPP_TIMER_ON_AUTH; + ppp->timer_val = PICO_PPP_DEFAULT_TIMER; +} + +static const struct pico_ppp_fsm ppp_auth_fsm[PPP_AUTH_STATE_MAX][PPP_AUTH_EVENT_MAX] = { + [PPP_AUTH_STATE_INITIAL] = { + [PPP_AUTH_EVENT_UP_NONE] = { PPP_AUTH_STATE_AUTHENTICATED, {auth} }, + [PPP_AUTH_EVENT_UP_PAP] = { PPP_AUTH_STATE_REQ_SENT, {auth_req, auth_start_timer} }, + [PPP_AUTH_EVENT_UP_CHAP] = { PPP_AUTH_STATE_STARTING, {} }, + [PPP_AUTH_EVENT_DOWN] = { PPP_AUTH_STATE_INITIAL, {} }, + [PPP_AUTH_EVENT_RAC] = { PPP_AUTH_STATE_INITIAL, {} }, + [PPP_AUTH_EVENT_RAA] = { PPP_AUTH_STATE_INITIAL, {} }, + [PPP_AUTH_EVENT_RAN] = { PPP_AUTH_STATE_INITIAL, {auth_abort} }, + [PPP_AUTH_EVENT_TO] = { PPP_AUTH_STATE_INITIAL, {} } + }, + [PPP_AUTH_STATE_STARTING] = { + [PPP_AUTH_EVENT_UP_NONE] = { PPP_AUTH_STATE_STARTING, {} }, + [PPP_AUTH_EVENT_UP_PAP] = { PPP_AUTH_STATE_STARTING, {} }, + [PPP_AUTH_EVENT_UP_CHAP] = { PPP_AUTH_STATE_STARTING, {} }, + [PPP_AUTH_EVENT_DOWN] = { PPP_AUTH_STATE_INITIAL, {deauth} }, + [PPP_AUTH_EVENT_RAC] = { PPP_AUTH_STATE_RSP_SENT, {auth_rsp, auth_start_timer} }, + [PPP_AUTH_EVENT_RAA] = { PPP_AUTH_STATE_STARTING, {auth_start_timer} }, + [PPP_AUTH_EVENT_RAN] = { PPP_AUTH_STATE_STARTING, {auth_abort} }, + [PPP_AUTH_EVENT_TO] = { PPP_AUTH_STATE_INITIAL, {auth_req, auth_start_timer} } + }, + [PPP_AUTH_STATE_RSP_SENT] = { + [PPP_AUTH_EVENT_UP_NONE] = { PPP_AUTH_STATE_RSP_SENT, {} }, + [PPP_AUTH_EVENT_UP_PAP] = { PPP_AUTH_STATE_RSP_SENT, {} }, + [PPP_AUTH_EVENT_UP_CHAP] = { PPP_AUTH_STATE_RSP_SENT, {} }, + [PPP_AUTH_EVENT_DOWN] = { PPP_AUTH_STATE_INITIAL, {deauth} }, + [PPP_AUTH_EVENT_RAC] = { PPP_AUTH_STATE_RSP_SENT, {auth_rsp, auth_start_timer} }, + [PPP_AUTH_EVENT_RAA] = { PPP_AUTH_STATE_AUTHENTICATED, {auth} }, + [PPP_AUTH_EVENT_RAN] = { PPP_AUTH_STATE_STARTING, {auth_abort} }, + [PPP_AUTH_EVENT_TO] = { PPP_AUTH_STATE_STARTING, {auth_start_timer} } + }, + [PPP_AUTH_STATE_REQ_SENT] = { + [PPP_AUTH_EVENT_UP_NONE] = { PPP_AUTH_STATE_REQ_SENT, {} }, + [PPP_AUTH_EVENT_UP_PAP] = { PPP_AUTH_STATE_REQ_SENT, {} }, + [PPP_AUTH_EVENT_UP_CHAP] = { PPP_AUTH_STATE_REQ_SENT, {} }, + [PPP_AUTH_EVENT_DOWN] = { PPP_AUTH_STATE_INITIAL, {deauth} }, + [PPP_AUTH_EVENT_RAC] = { PPP_AUTH_STATE_REQ_SENT, {} }, + [PPP_AUTH_EVENT_RAA] = { PPP_AUTH_STATE_AUTHENTICATED, {auth} }, + [PPP_AUTH_EVENT_RAN] = { PPP_AUTH_STATE_REQ_SENT, {auth_abort} }, + [PPP_AUTH_EVENT_TO] = { PPP_AUTH_STATE_REQ_SENT, {auth_req, auth_start_timer} } + }, + [PPP_AUTH_STATE_AUTHENTICATED] = { + [PPP_AUTH_EVENT_UP_NONE] = { PPP_AUTH_STATE_AUTHENTICATED, {} }, + [PPP_AUTH_EVENT_UP_PAP] = { PPP_AUTH_STATE_AUTHENTICATED, {} }, + [PPP_AUTH_EVENT_UP_CHAP] = { PPP_AUTH_STATE_AUTHENTICATED, {} }, + [PPP_AUTH_EVENT_DOWN] = { PPP_AUTH_STATE_INITIAL, {deauth} }, + [PPP_AUTH_EVENT_RAC] = { PPP_AUTH_STATE_RSP_SENT, {auth_rsp} }, + [PPP_AUTH_EVENT_RAA] = { PPP_AUTH_STATE_AUTHENTICATED, {} }, + [PPP_AUTH_EVENT_RAN] = { PPP_AUTH_STATE_AUTHENTICATED, {} }, + [PPP_AUTH_EVENT_TO] = { PPP_AUTH_STATE_AUTHENTICATED, {} }, + } +}; + +static void evaluate_auth_state(struct pico_device_ppp *ppp, enum ppp_auth_event event) +{ + const struct pico_ppp_fsm *fsm; + int i; + if (mock_auth_state) { + mock_auth_state(ppp, event); + return; + } + + fsm = &ppp_auth_fsm[ppp->auth_state][event]; + + ppp->auth_state = (enum ppp_auth_state)fsm->next_state; + for (i = 0; i < PPP_FSM_MAX_ACTIONS; i++) { + if (fsm->event_handler[i]) + fsm->event_handler[i](ppp); + } +} + +static void ipcp_send_nack(struct pico_device_ppp *ppp) +{ + IGNORE_PARAMETER(ppp); +} + +static void ipcp_bring_up(struct pico_device_ppp *ppp) +{ + ppp_dbg("PPP: IPCP up.\n"); + + if (ppp->ipcp_ip) { + char my_ip[16], my_dns[16]; + pico_ipv4_to_string(my_ip, ppp->ipcp_ip); + ppp_dbg("Received IP config %s\n", my_ip); + pico_ipv4_to_string(my_dns, ppp->ipcp_dns1); + ppp_dbg("Received DNS: %s\n", my_dns); + ppp_ipv4_conf(ppp); + } +} + +static void ipcp_bring_down(struct pico_device_ppp *ppp) +{ + IGNORE_PARAMETER(ppp); + + ppp_dbg("PPP: IPCP down.\n"); +} + +static void ipcp_start_timer(struct pico_device_ppp *ppp) +{ + ppp->timer_on = ppp->timer_on | PPP_TIMER_ON_IPCP; + ppp->timer_val = PICO_PPP_DEFAULT_TIMER * PICO_PPP_DEFAULT_MAX_FAILURE; +} + +static const struct pico_ppp_fsm ppp_ipcp_fsm[PPP_IPCP_STATE_MAX][PPP_IPCP_EVENT_MAX] = { + [PPP_IPCP_STATE_INITIAL] = { + [PPP_IPCP_EVENT_UP] = { PPP_IPCP_STATE_REQ_SENT, {ipcp_send_req, ipcp_start_timer} }, + [PPP_IPCP_EVENT_DOWN] = { PPP_IPCP_STATE_INITIAL, {} }, + [PPP_IPCP_EVENT_RCR_POS] = { PPP_IPCP_STATE_INITIAL, {} }, + [PPP_IPCP_EVENT_RCR_NEG] = { PPP_IPCP_STATE_INITIAL, {} }, + [PPP_IPCP_EVENT_RCA] = { PPP_IPCP_STATE_INITIAL, {} }, + [PPP_IPCP_EVENT_RCN] = { PPP_IPCP_STATE_INITIAL, {} }, + [PPP_IPCP_EVENT_TO] = { PPP_IPCP_STATE_INITIAL, {} } + }, + [PPP_IPCP_STATE_REQ_SENT] = { + [PPP_IPCP_EVENT_UP] = { PPP_IPCP_STATE_REQ_SENT, {} }, + [PPP_IPCP_EVENT_DOWN] = { PPP_IPCP_STATE_INITIAL, {} }, + [PPP_IPCP_EVENT_RCR_POS] = { PPP_IPCP_STATE_ACK_SENT, {ipcp_send_ack} }, + [PPP_IPCP_EVENT_RCR_NEG] = { PPP_IPCP_STATE_REQ_SENT, {ipcp_send_nack} }, + [PPP_IPCP_EVENT_RCA] = { PPP_IPCP_STATE_ACK_RCVD, {} }, + [PPP_IPCP_EVENT_RCN] = { PPP_IPCP_STATE_REQ_SENT, {ipcp_send_req, ipcp_start_timer} }, + [PPP_IPCP_EVENT_TO] = { PPP_IPCP_STATE_REQ_SENT, {ipcp_send_req, ipcp_start_timer} } + }, + [PPP_IPCP_STATE_ACK_RCVD] = { + [PPP_IPCP_EVENT_UP] = { PPP_IPCP_STATE_ACK_RCVD, {} }, + [PPP_IPCP_EVENT_DOWN] = { PPP_IPCP_STATE_INITIAL, {} }, + [PPP_IPCP_EVENT_RCR_POS] = { PPP_IPCP_STATE_OPENED, {ipcp_send_ack, ipcp_bring_up} }, + [PPP_IPCP_EVENT_RCR_NEG] = { PPP_IPCP_STATE_ACK_RCVD, {ipcp_send_nack} }, + [PPP_IPCP_EVENT_RCA] = { PPP_IPCP_STATE_REQ_SENT, {ipcp_send_req, ipcp_start_timer} }, + [PPP_IPCP_EVENT_RCN] = { PPP_IPCP_STATE_REQ_SENT, {ipcp_send_req, ipcp_start_timer} }, + [PPP_IPCP_EVENT_TO] = { PPP_IPCP_STATE_ACK_RCVD, {ipcp_send_req, ipcp_start_timer} } + }, + [PPP_IPCP_STATE_ACK_SENT] = { + [PPP_IPCP_EVENT_UP] = { PPP_IPCP_STATE_ACK_SENT, {} }, + [PPP_IPCP_EVENT_DOWN] = { PPP_IPCP_STATE_INITIAL, {} }, + [PPP_IPCP_EVENT_RCR_POS] = { PPP_IPCP_STATE_ACK_SENT, {ipcp_send_ack} }, + [PPP_IPCP_EVENT_RCR_NEG] = { PPP_IPCP_STATE_REQ_SENT, {ipcp_send_nack} }, + [PPP_IPCP_EVENT_RCA] = { PPP_IPCP_STATE_OPENED, {ipcp_bring_up} }, + [PPP_IPCP_EVENT_RCN] = { PPP_IPCP_STATE_ACK_SENT, {ipcp_send_req, ipcp_start_timer} }, + [PPP_IPCP_EVENT_TO] = { PPP_IPCP_STATE_ACK_SENT, {ipcp_send_req, ipcp_start_timer} } + }, + [PPP_IPCP_STATE_OPENED] = { + [PPP_IPCP_EVENT_UP] = { PPP_IPCP_STATE_OPENED, {} }, + [PPP_IPCP_EVENT_DOWN] = { PPP_IPCP_STATE_INITIAL, {ipcp_bring_down} }, + [PPP_IPCP_EVENT_RCR_POS] = { PPP_IPCP_STATE_ACK_SENT, {ipcp_bring_down, ipcp_send_req, ipcp_send_ack} }, + [PPP_IPCP_EVENT_RCR_NEG] = { PPP_IPCP_STATE_REQ_SENT, {ipcp_bring_down, ipcp_send_req, ipcp_send_nack} }, + [PPP_IPCP_EVENT_RCA] = { PPP_IPCP_STATE_REQ_SENT, {ipcp_send_req} }, + [PPP_IPCP_EVENT_RCN] = { PPP_IPCP_STATE_REQ_SENT, {ipcp_send_req} }, + [PPP_IPCP_EVENT_TO] = { PPP_IPCP_STATE_OPENED, {} } + } +}; + +static void evaluate_ipcp_state(struct pico_device_ppp *ppp, enum ppp_ipcp_event event) +{ + const struct pico_ppp_fsm *fsm; + int i; + if (mock_ipcp_state) { + mock_ipcp_state(ppp, event); + return; + } + + fsm = &ppp_ipcp_fsm[ppp->ipcp_state][event]; + + ppp->ipcp_state = (enum ppp_ipcp_state)fsm->next_state; + for (i = 0; i < PPP_FSM_MAX_ACTIONS; i++) { + if (fsm->event_handler[i]) + fsm->event_handler[i](ppp); + } +} + +static int pico_ppp_poll(struct pico_device *dev, int loop_score) +{ + struct pico_device_ppp *ppp = (struct pico_device_ppp *) dev; + static uint32_t len = 0; + int r; + if (ppp->serial_recv) { + do { + r = ppp->serial_recv(&ppp->dev, &ppp_recv_buf[len], 1); + if (r <= 0) + break; + + if (ppp->modem_state == PPP_MODEM_STATE_CONNECTED) { + static int control_escape = 0; + + if (ppp_recv_buf[len] == PPPF_FLAG_SEQ) { + if (control_escape) { + /* Illegal sequence, discard frame */ + ppp_dbg("Illegal sequence, ppp_recv_buf[%d] = %d\n", len, ppp_recv_buf[len]); + control_escape = 0; + len = 0; + } + + if (len > 1) { + ppp_recv_data(ppp, ppp_recv_buf, len); + loop_score--; + len = 0; + } + } else if (control_escape) { + ppp_recv_buf[len] ^= 0x20; + control_escape = 0; + len++; + } else if (ppp_recv_buf[len] == PPPF_CTRL_ESC) { + control_escape = 1; + } else { + len++; + } + } else { + static int s3 = 0; + + if (ppp_recv_buf[len] == AT_S3) { + s3 = 1; + if (len > 0) { + ppp_recv_buf[len] = '\0'; + ppp_modem_recv(ppp, ppp_recv_buf, len); + len = 0; + } + } else if (ppp_recv_buf[len] == AT_S4) { + if (!s3) { + len++; + } + + s3 = 0; + } else { + s3 = 0; + len++; + } + } + } while ((r > 0) && (len < ARRAY_SIZE(ppp_recv_buf)) && (loop_score > 0)); + } + + return loop_score; +} + +/* Public interface: create/destroy. */ + +static int pico_ppp_link_state(struct pico_device *dev) +{ + struct pico_device_ppp *ppp = (struct pico_device_ppp *)dev; + if (ppp->ipcp_state == PPP_IPCP_STATE_OPENED) + return 1; + + return 0; +} + +void pico_ppp_destroy(struct pico_device *ppp) +{ + if (!ppp) + return; + + /* Perform custom cleanup here before calling 'pico_device_destroy' + * or register a custom cleanup function during initialization + * by setting 'ppp->dev.destroy'. */ + + pico_device_destroy(ppp); +} + +static void check_to_modem(struct pico_device_ppp *ppp) +{ + if (ppp->timer_on & PPP_TIMER_ON_MODEM) { + if (ppp->timer_val == 0) { + ppp->timer_on = (uint8_t) (ppp->timer_on & (~PPP_TIMER_ON_MODEM)); + evaluate_modem_state(ppp, PPP_MODEM_EVENT_TIMEOUT); + } + } +} + +static void check_to_lcp(struct pico_device_ppp *ppp) +{ + if (ppp->timer_on & (PPP_TIMER_ON_LCPREQ | PPP_TIMER_ON_LCPTERM)) { + if (ppp->timer_val == 0) { + if (ppp->timer_count == 0) + evaluate_lcp_state(ppp, PPP_LCP_EVENT_TO_NEG); + else{ + evaluate_lcp_state(ppp, PPP_LCP_EVENT_TO_POS); + ppp->timer_count--; + } + } + } +} + +static void check_to_auth(struct pico_device_ppp *ppp) +{ + if (ppp->timer_on & PPP_TIMER_ON_AUTH) { + if (ppp->timer_val == 0) { + ppp->timer_on = (uint8_t) (ppp->timer_on & (~PPP_TIMER_ON_AUTH)); + evaluate_auth_state(ppp, PPP_AUTH_EVENT_TO); + } + } +} + +static void check_to_ipcp(struct pico_device_ppp *ppp) +{ + if (ppp->timer_on & PPP_TIMER_ON_IPCP) { + if (ppp->timer_val == 0) { + ppp->timer_on = (uint8_t) (ppp->timer_on & (~PPP_TIMER_ON_IPCP)); + evaluate_ipcp_state(ppp, PPP_IPCP_EVENT_TO); + } + } +} + +static void pico_ppp_tick(pico_time t, void *arg) +{ + struct pico_device_ppp *ppp = (struct pico_device_ppp *) arg; + (void)t; + if (ppp->timer_val > 0) + ppp->timer_val--; + + check_to_modem(ppp); + check_to_lcp(ppp); + check_to_auth(ppp); + check_to_ipcp(ppp); + + if (ppp->autoreconnect && ppp->lcp_state == PPP_LCP_STATE_INITIAL) { + ppp_dbg("(Re)connecting...\n"); + evaluate_lcp_state(ppp, PPP_LCP_EVENT_OPEN); + } + + pico_timer_add(1000, pico_ppp_tick, arg); +} + +struct pico_device *pico_ppp_create(void) +{ + struct pico_device_ppp *ppp = PICO_ZALLOC(sizeof(struct pico_device_ppp)); + char devname[MAX_DEVICE_NAME]; + + if (!ppp) + return NULL; + + snprintf(devname, MAX_DEVICE_NAME, "ppp%d", ppp_devnum++); + + if( 0 != pico_device_init((struct pico_device *)ppp, devname, NULL)) { + return NULL; + } + + ppp->dev.overhead = PPP_HDR_SIZE; + ppp->dev.mtu = PICO_PPP_MTU; + ppp->dev.send = pico_ppp_send; + ppp->dev.poll = pico_ppp_poll; + ppp->dev.link_state = pico_ppp_link_state; + ppp->frame_id = (uint8_t)(pico_rand() % 0xFF); + + ppp->modem_state = PPP_MODEM_STATE_INITIAL; + ppp->lcp_state = PPP_LCP_STATE_INITIAL; + ppp->auth_state = PPP_AUTH_STATE_INITIAL; + ppp->ipcp_state = PPP_IPCP_STATE_INITIAL; + + ppp->timer = pico_timer_add(1000, pico_ppp_tick, ppp); + ppp->mru = PICO_PPP_MRU; + + LCPOPT_SET_LOCAL(ppp, LCPOPT_MRU); + LCPOPT_SET_LOCAL(ppp, LCPOPT_AUTH); /* We support authentication, even if it's not part of the req */ + LCPOPT_SET_LOCAL(ppp, LCPOPT_PROTO_COMP); + LCPOPT_SET_LOCAL(ppp, LCPOPT_ADDRCTL_COMP); + + + ppp_dbg("Device %s created.\n", ppp->dev.name); + return (struct pico_device *)ppp; +} + +int pico_ppp_connect(struct pico_device *dev) +{ + struct pico_device_ppp *ppp = (struct pico_device_ppp *)dev; + ppp->autoreconnect = 1; + return 0; +} + +int pico_ppp_disconnect(struct pico_device *dev) +{ + struct pico_device_ppp *ppp = (struct pico_device_ppp *)dev; + ppp->autoreconnect = 0; + evaluate_lcp_state(ppp, PPP_LCP_EVENT_CLOSE); + return 0; +} + +int pico_ppp_set_serial_read(struct pico_device *dev, int (*sread)(struct pico_device *, void *, int)) +{ + struct pico_device_ppp *ppp = (struct pico_device_ppp *)dev; + + if (!dev) + return -1; + + ppp->serial_recv = sread; + return 0; +} + +int pico_ppp_set_serial_write(struct pico_device *dev, int (*swrite)(struct pico_device *, const void *, int)) +{ + struct pico_device_ppp *ppp = (struct pico_device_ppp *)dev; + + if (!dev) + return -1; + + ppp->serial_send = swrite; + return 0; +} + +int pico_ppp_set_serial_set_speed(struct pico_device *dev, int (*sspeed)(struct pico_device *, uint32_t)) +{ + struct pico_device_ppp *ppp = (struct pico_device_ppp *)dev; + + if (!dev) + return -1; + + ppp->serial_set_speed = sspeed; + return 0; +} + +int pico_ppp_set_apn(struct pico_device *dev, const char *apn) +{ + struct pico_device_ppp *ppp = (struct pico_device_ppp *)dev; + + if (!dev) + return -1; + + if (!apn) + return -1; + + strncpy(ppp->apn, apn, sizeof(ppp->apn) - 1); + return 0; +} + +int pico_ppp_set_username(struct pico_device *dev, const char *username) +{ + struct pico_device_ppp *ppp = (struct pico_device_ppp *)dev; + + if (!dev) + return -1; + + if (!username) + return -1; + + strncpy(ppp->username, username, sizeof(ppp->username) - 1); + return 0; +} + +int pico_ppp_set_password(struct pico_device *dev, const char *password) +{ + struct pico_device_ppp *ppp = (struct pico_device_ppp *)dev; + + if (!dev) + return -1; + + if (!password) + return -1; + + strncpy(ppp->password, password, sizeof(ppp->password) - 1); + return 0; +} diff --git a/ext/picotcp/modules/pico_dev_ppp.h b/ext/picotcp/modules/pico_dev_ppp.h new file mode 100644 index 0000000..6110e50 --- /dev/null +++ b/ext/picotcp/modules/pico_dev_ppp.h @@ -0,0 +1,26 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#ifndef INCLUDE_PICO_PPP +#define INCLUDE_PICO_PPP + +#include "pico_config.h" +#include "pico_device.h" + +void pico_ppp_destroy(struct pico_device *ppp); +struct pico_device *pico_ppp_create(void); + +int pico_ppp_connect(struct pico_device *dev); +int pico_ppp_disconnect(struct pico_device *dev); + +int pico_ppp_set_serial_read(struct pico_device *dev, int (*sread)(struct pico_device *, void *, int)); +int pico_ppp_set_serial_write(struct pico_device *dev, int (*swrite)(struct pico_device *, const void *, int)); +int pico_ppp_set_serial_set_speed(struct pico_device *dev, int (*sspeed)(struct pico_device *, uint32_t)); + +int pico_ppp_set_apn(struct pico_device *dev, const char *apn); +int pico_ppp_set_username(struct pico_device *dev, const char *username); +int pico_ppp_set_password(struct pico_device *dev, const char *password); + +#endif /* INCLUDE_PICO_PPP */ diff --git a/ext/picotcp/modules/pico_dev_tap.c b/ext/picotcp/modules/pico_dev_tap.c new file mode 100644 index 0000000..b80ea6d --- /dev/null +++ b/ext/picotcp/modules/pico_dev_tap.c @@ -0,0 +1,220 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Daniele Lacamera + *********************************************************************/ + + +#include +#include +#include +#include +#include "pico_device.h" +#include "pico_dev_tap.h" +#include "pico_stack.h" + +#ifndef __FreeBSD__ +#include +#endif + +#include + +struct pico_device_tap { + struct pico_device dev; + int fd; +}; + +#define TUN_MTU 2048 + +// We only support one global link state - we only have two USR signals, we +// can't spread these out over an arbitrary amount of devices. When you unplug +// one tap, you unplug all of them. + +static int link_state = 0; + +static void sig_handler(int signo) +{ + if (signo == SIGUSR1) + link_state = 0; + if (signo == SIGUSR2) + link_state = 1; +} + +static int tap_link_state(__attribute__((unused)) struct pico_device *self) +{ + return link_state; +} + + +static int pico_tap_send(struct pico_device *dev, void *buf, int len) +{ + struct pico_device_tap *tap = (struct pico_device_tap *) dev; + return (int)write(tap->fd, buf, (uint32_t)len); +} + +static int pico_tap_poll(struct pico_device *dev, int loop_score) +{ + struct pico_device_tap *tap = (struct pico_device_tap *) dev; + struct pollfd pfd; + unsigned char buf[TUN_MTU]; + int len; + pfd.fd = tap->fd; + pfd.events = POLLIN; + do { + if (poll(&pfd, 1, 0) <= 0) + return loop_score; + + len = (int)read(tap->fd, buf, TUN_MTU); + if (len > 0) { + loop_score--; + pico_stack_recv(dev, buf, (uint32_t)len); + } + } while(loop_score > 0); + return 0; +} + +/* Public interface: create/destroy. */ + +void pico_tap_destroy(struct pico_device *dev) +{ + struct pico_device_tap *tap = (struct pico_device_tap *) dev; + if(tap->fd > 0) + close(tap->fd); +} + +#ifndef __FreeBSD__ +static int tap_open(char *name) +{ + struct ifreq ifr; + int tap_fd; + if((tap_fd = open("/dev/net/tun", O_RDWR)) < 0) { + return(-1); + } + + memset(&ifr, 0, sizeof(ifr)); + ifr.ifr_flags = IFF_TAP | IFF_NO_PI; + strncpy(ifr.ifr_name, name, IFNAMSIZ); + if(ioctl(tap_fd, TUNSETIFF, &ifr) < 0) { + return(-1); + } + + return tap_fd; +} +#else +static int tap_open(char *name) +{ + int tap_fd; + (void)name; + tap_fd = open("/dev/tap0", O_RDWR); + return tap_fd; +} +#endif + + +#ifndef __FreeBSD__ +static int tap_get_mac(char *name, uint8_t *mac) +{ + int sck; + struct ifreq eth; + int retval = -1; + + + + + sck = socket(AF_INET, SOCK_DGRAM, 0); + if(sck < 0) { + return retval; + } + + memset(ð, 0, sizeof(struct ifreq)); + strcpy(eth.ifr_name, name); + /* call the IOCTL */ + if (ioctl(sck, SIOCGIFHWADDR, ð) < 0) { + perror("ioctl(SIOCGIFHWADDR)"); + return -1; + ; + } + + memcpy (mac, ð.ifr_hwaddr.sa_data, 6); + + + close(sck); + return 0; + +} +#else +#include +#include +#include +static int tap_get_mac(char *name, uint8_t *mac) +{ + struct sockaddr_dl *sdl; + struct ifaddrs *ifap, *root; + if (getifaddrs(&ifap) != 0) + return -1; + + root = ifap; + while(ifap) { + if (strcmp(name, ifap->ifa_name) == 0) + sdl = (struct sockaddr_dl *) ifap->ifa_addr; + + if (sdl->sdl_type == IFT_ETHER) { + memcpy(mac, LLADDR(sdl), 6); + freeifaddrs(root); + return 0; + } + + ifap = ifap->ifa_next; + } + freeifaddrs(root); + return 0; +} +#endif + +struct pico_device *pico_tap_create(char *name) +{ + struct pico_device_tap *tap = PICO_ZALLOC(sizeof(struct pico_device_tap)); + uint8_t mac[6] = {}; + struct sigaction sa; + + if (!tap) + return NULL; + + sa.sa_flags = 0; + sigemptyset(&sa.sa_mask); + sa.sa_handler = sig_handler; + + if ((sigaction(SIGUSR1, &sa, NULL) == 0) && + (sigaction(SIGUSR2, &sa, NULL) == 0)) + tap->dev.link_state = &tap_link_state; + + tap->dev.overhead = 0; + tap->fd = tap_open(name); + if (tap->fd < 0) { + dbg("Tap creation failed.\n"); + pico_tap_destroy((struct pico_device *)tap); + return NULL; + } + + if (tap_get_mac(name, mac) < 0) { + dbg("Tap mac query failed.\n"); + pico_tap_destroy((struct pico_device *)tap); + return NULL; + } + + mac[5]++; + + if( 0 != pico_device_init((struct pico_device *)tap, name, mac)) { + dbg("Tap init failed.\n"); + pico_tap_destroy((struct pico_device *)tap); + return NULL; + } + + tap->dev.send = pico_tap_send; + tap->dev.poll = pico_tap_poll; + tap->dev.destroy = pico_tap_destroy; + dbg("Device %s created.\n", tap->dev.name); + return (struct pico_device *)tap; +} + diff --git a/ext/picotcp/modules/pico_dev_tap.h b/ext/picotcp/modules/pico_dev_tap.h new file mode 100644 index 0000000..ebc21fe --- /dev/null +++ b/ext/picotcp/modules/pico_dev_tap.h @@ -0,0 +1,15 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#ifndef INCLUDE_PICO_TAP +#define INCLUDE_PICO_TAP +#include "pico_config.h" +#include "pico_device.h" + +void pico_tap_destroy(struct pico_device *tap); +struct pico_device *pico_tap_create(char *name); + +#endif + diff --git a/ext/picotcp/modules/pico_dev_tap_windows.c b/ext/picotcp/modules/pico_dev_tap_windows.c new file mode 100644 index 0000000..2e003f8 --- /dev/null +++ b/ext/picotcp/modules/pico_dev_tap_windows.c @@ -0,0 +1,1083 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2014-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Maxime Vincent + Based on the OpenVPN tun.c driver, under GPL + + NOTES: This is the Windows-only driver, a Linux-equivalent is available, too + You need to have an OpenVPN TUN/TAP network adapter installed, first + This driver is barely working: + * Only TAP-mode is supported (TUN is not) + * it will simply open the first TAP device it can find + * there is memory being allocated that's never freed + * there is no destroy function, yet + * it has only been tested on a Windows 7 machine + *********************************************************************/ + +#include "pico_device.h" +#include "pico_dev_null.h" +#include "pico_stack.h" +#include "pico_dev_tap_windows.h" + +#include +#include +#include +#include "pico_dev_tap_windows_private.h" + +/* + * Debugging info + */ +#define dbg_tap_info(...) /* tap info messages */ +#define dbg_tap(...) /* first level debug */ +#define dbg_win32(...) /* second level detailed win32 debug */ +#define dbg_reg(...) /* third level: registry debug */ + +/* + * Tunnel types + */ +#define DEV_TYPE_UNDEF 0 +#define DEV_TYPE_NULL 1 +#define DEV_TYPE_TUN 2 /* point-to-point IP tunnel */ +#define DEV_TYPE_TAP 3 /* ethernet (802.3) tunnel */ + + +/* + * We try to do all Win32 I/O using overlapped + * (i.e. asynchronous) I/O for a performance win. + */ +struct overlapped_io { +# define IOSTATE_INITIAL 0 +# define IOSTATE_QUEUED 1 /* overlapped I/O has been queued */ +# define IOSTATE_IMMEDIATE_RETURN 2 /* I/O function returned immediately without queueing */ + int iostate; + OVERLAPPED overlapped; + DWORD size; + DWORD flags; + int status; + int addr_defined; + uint8_t *buf_init; + uint32_t buf_init_len; + uint8_t *buf; + uint32_t buf_len; +}; + +struct rw_handle { + HANDLE read; + HANDLE write; +}; + +struct tuntap +{ + int type; /* DEV_TYPE_x as defined in proto.h */ + int ipv6; + int persistent_if; /* if existed before, keep on program end */ + char *actual_name; /* actual name of TUN/TAP dev, usually including unit number */ + int post_open_mtu; + uint8_t mac[6]; + + /* Windows stuff */ + DWORD adapter_index; /*adapter index for TAP-Windows adapter, ~0 if undefined */ + HANDLE hand; + struct overlapped_io reads; /* for overlapped IO */ + struct overlapped_io writes; + struct rw_handle rw_handle; + +}; + + +struct pico_device_tap { + struct pico_device dev; + int statistics_frames_out; + struct tuntap *tt; +}; + + +/* + * Private function prototypes + */ +const struct tap_reg *get_tap_reg (void); +const struct panel_reg *get_panel_reg (void); + + +/* + * Private functions + */ + +/* Get TAP info from Windows registry */ +const struct tap_reg *get_tap_reg (void) +{ + HKEY adapter_key; + LONG status; + DWORD len; + struct tap_reg *first = NULL; + struct tap_reg *last = NULL; + int i = 0; + + status = RegOpenKeyEx( + HKEY_LOCAL_MACHINE, + ADAPTER_KEY, + 0, + KEY_READ, + &adapter_key); + + if (status != ERROR_SUCCESS) + { + dbg_reg("Error opening registry key: %s\n", ADAPTER_KEY); + return NULL; + } + + while (1) + { + char enum_name[256]; + char unit_string[256]; + HKEY unit_key; + char component_id_string[] = "ComponentId"; + char component_id[256]; + char net_cfg_instance_id_string[] = "NetCfgInstanceId"; + char net_cfg_instance_id[256]; + DWORD data_type; + + len = sizeof (enum_name); + status = RegEnumKeyEx( + adapter_key, + i, + enum_name, + &len, + NULL, + NULL, + NULL, + NULL); + if (status == ERROR_NO_MORE_ITEMS) + break; + else if (status != ERROR_SUCCESS) + dbg_reg("Error enumerating registry subkeys of key: %s.\n", ADAPTER_KEY); + + snprintf (unit_string, sizeof(unit_string), "%s\\%s", + ADAPTER_KEY, enum_name); + + status = RegOpenKeyEx( + HKEY_LOCAL_MACHINE, + unit_string, + 0, + KEY_READ, + &unit_key); + + if (status != ERROR_SUCCESS) + { + dbg_reg("Error opening registry key: %s\n", unit_string); + } + else + { + len = sizeof (component_id); + status = RegQueryValueEx( + unit_key, + component_id_string, + NULL, + &data_type, + (LPBYTE)component_id, + &len); + + if (status != ERROR_SUCCESS || data_type != REG_SZ) + { + dbg_reg("Error opening registry key: %s\\%s\n", unit_string, component_id_string); + } + else + { + len = sizeof (net_cfg_instance_id); + status = RegQueryValueEx( + unit_key, + net_cfg_instance_id_string, + NULL, + &data_type, + (LPBYTE)net_cfg_instance_id, + &len); + + if (status == ERROR_SUCCESS && data_type == REG_SZ) + { + if (!strcmp (component_id, TAP_WIN_COMPONENT_ID)) + { + struct tap_reg *reg; + reg = PICO_ZALLOC(sizeof(struct tap_reg), 1); + /* ALLOC_OBJ_CLEAR_GC (reg, struct tap_reg, gc); */ + if (!reg) + return NULL; + + /* reg->guid = string_alloc (net_cfg_instance_id, gc); */ + reg->guid = PICO_ZALLOC (strlen(net_cfg_instance_id) + 1, 1); + if (!(reg->guid)) + { + PICO_FREE(reg); + return NULL; + } + + strcpy((char *)reg->guid, net_cfg_instance_id); + /* link into return list */ + if (!first) + first = reg; + + if (last) + last->next = reg; + + last = reg; + } + } + } + + RegCloseKey (unit_key); + } + + ++i; + } + RegCloseKey (adapter_key); + return first; +} + +/* Get Panel info from Windows registry */ +const struct panel_reg *get_panel_reg (void) +{ + LONG status; + HKEY network_connections_key; + DWORD len; + struct panel_reg *first = NULL; + struct panel_reg *last = NULL; + int i = 0; + + status = RegOpenKeyEx( + HKEY_LOCAL_MACHINE, + NETWORK_CONNECTIONS_KEY, + 0, + KEY_READ, + &network_connections_key); + + if (status != ERROR_SUCCESS) + { + dbg_reg("Error opening registry key: %s\n", NETWORK_CONNECTIONS_KEY); + return NULL; + } + + while (1) + { + char enum_name[256]; + char connection_string[256]; + HKEY connection_key; + WCHAR name_data[256]; + DWORD name_type; + const WCHAR name_string[] = L"Name"; + + len = sizeof (enum_name); + status = RegEnumKeyEx( + network_connections_key, + i, + enum_name, + &len, + NULL, + NULL, + NULL, + NULL); + if (status == ERROR_NO_MORE_ITEMS) + break; + else if (status != ERROR_SUCCESS) + dbg_reg("Error enumerating registry subkeys of key: %s.\n", NETWORK_CONNECTIONS_KEY); + + snprintf (connection_string, sizeof(connection_string), "%s\\%s\\Connection", NETWORK_CONNECTIONS_KEY, enum_name); + + status = RegOpenKeyEx( + HKEY_LOCAL_MACHINE, + connection_string, + 0, + KEY_READ, + &connection_key); + if (status != ERROR_SUCCESS) + dbg_reg("Error opening registry key: %s\n", connection_string); + else + { + len = sizeof (name_data); + status = RegQueryValueExW( + connection_key, + name_string, + NULL, + &name_type, + (LPBYTE) name_data, + &len); + + if (status != ERROR_SUCCESS || name_type != REG_SZ) + dbg_reg("Error opening registry key: %s\\%s\\%S\n", NETWORK_CONNECTIONS_KEY, connection_string, name_string); + else + { + int n; + LPSTR name; + struct panel_reg *reg; + + /* ALLOC_OBJ_CLEAR_GC (reg, struct panel_reg, gc); */ + reg = PICO_ZALLOC(sizeof(struct panel_reg), 1); + if (!reg) + return NULL; + + n = WideCharToMultiByte (CP_UTF8, 0, name_data, -1, NULL, 0, NULL, NULL); + /* name = gc_malloc (n, false, gc); */ + name = PICO_ZALLOC(n, 1); + if (!name) + { + PICO_FREE(reg); + return NULL; + } + + WideCharToMultiByte (CP_UTF8, 0, name_data, -1, name, n, NULL, NULL); + reg->name = name; + /* reg->guid = string_alloc (enum_name, gc); */ + reg->guid = PICO_ZALLOC(strlen(enum_name) + 1, 1); + if (!reg->guid) + { + PICO_FREE((void *)reg->name); + PICO_FREE((void *)reg); + return NULL; + } + + strcpy((char *)reg->guid, enum_name); + + /* link into return list */ + if (!first) + first = reg; + + if (last) + last->next = reg; + + last = reg; + } + + RegCloseKey (connection_key); + } + + ++i; + } + RegCloseKey (network_connections_key); + + return first; +} + + +void show_tap_win_adapters (void) +{ + int warn_panel_null = 0; + int warn_panel_dup = 0; + int warn_tap_dup = 0; + + int links; + + const struct tap_reg *tr; + const struct tap_reg *tr1; + const struct panel_reg *pr; + + const struct tap_reg *tap_reg = get_tap_reg (); + const struct panel_reg *panel_reg = get_panel_reg (); + + if (!(tap_reg && panel_reg)) + return; + + dbg_tap_info("Available TAP-WIN32 adapters [name, GUID]:\n"); + + /* loop through each TAP-Windows adapter registry entry */ + for (tr = tap_reg; tr != NULL; tr = tr->next) + { + links = 0; + + /* loop through each network connections entry in the control panel */ + for (pr = panel_reg; pr != NULL; pr = pr->next) + { + if (!strcmp (tr->guid, pr->guid)) + { + dbg_tap_info("\t>> '%s' %s\n", pr->name, tr->guid); + ++links; + } + } + if (links > 1) + { + warn_panel_dup = 1; + } + else if (links == 0) + { + /* a TAP adapter exists without a link from the network + connections control panel */ + warn_panel_null = 1; + dbg_tap_info("\t>> [NULL] %s\n", tr->guid); + } + } + /* check for TAP-Windows adapter duplicated GUIDs */ + for (tr = tap_reg; tr != NULL; tr = tr->next) + { + for (tr1 = tap_reg; tr1 != NULL; tr1 = tr1->next) + { + if (tr != tr1 && !strcmp (tr->guid, tr1->guid)) + warn_tap_dup = 1; + } + } + /* warn on registry inconsistencies */ + if (warn_tap_dup) + dbg_tap_info("WARNING: Some TAP-Windows adapters have duplicate GUIDs\n"); + + if (warn_panel_dup) + dbg_tap_info("WARNING: Some TAP-Windows adapters have duplicate links from the Network Connections control panel\n"); + + if (warn_panel_null) + dbg_tap_info("WARNING: Some TAP-Windows adapters have no link from the Network Connections control panel\n"); +} + + +/* Get the GUID of the first TAP device found */ +const char *get_first_device_guid(const struct tap_reg *tap_reg, const struct panel_reg *panel_reg, char *name) +{ + const struct tap_reg *tr; + const struct panel_reg *pr; + /* loop through each TAP-Windows adapter registry entry */ + for (tr = tap_reg; tr != NULL; tr = tr->next) + { + /* loop through each network connections entry in the control panel */ + for (pr = panel_reg; pr != NULL; pr = pr->next) + { + if (!strcmp (tr->guid, pr->guid)) + { + dbg_tap_info("Using first TAP device: '%s' %s\n", pr->name, tr->guid); + if (name) + strcpy(name, pr->name); + + return tr->guid; + } + } + } + return NULL; +} + + + +int open_tun (const char *dev, const char *dev_type, const char *dev_node, struct tuntap *tt) +{ + char device_path[256]; + const char *device_guid = NULL; + DWORD len; + + dbg_tap_info("open_tun, tt->ipv6=%d\n", tt->ipv6 ); + + if (!(tt->type == DEV_TYPE_TAP || tt->type == DEV_TYPE_TUN)) + { + dbg_tap_info("Unknown virtual device type: '%s'\n", dev); + return -1; + } + + /* + * Lookup the device name in the registry, using the --dev-node high level name. + */ + { + const struct tap_reg *tap_reg = get_tap_reg(); + const struct panel_reg *panel_reg = get_panel_reg(); + char name[256]; + + if (!(tap_reg && panel_reg)) + { + dbg_tap_info("No TUN/TAP devices found\n"); + return -1; + } + + /* Get the device GUID for the device specified with --dev-node. */ + device_guid = get_first_device_guid (tap_reg, panel_reg, name); + + if (!device_guid) + dbg_tap_info("TAP-Windows adapter '%s' not found\n", dev_node); + + /* Open Windows TAP-Windows adapter */ + snprintf (device_path, sizeof(device_path), "%s%s%s", + USERMODEDEVICEDIR, + device_guid, + TAP_WIN_SUFFIX); + + tt->hand = CreateFile ( + device_path, + GENERIC_READ | GENERIC_WRITE, + 0, /* was: FILE_SHARE_READ */ + 0, + OPEN_EXISTING, + FILE_ATTRIBUTE_SYSTEM | FILE_FLAG_OVERLAPPED, + 0 + ); + + if (tt->hand == INVALID_HANDLE_VALUE) + dbg_tap_info("CreateFile failed on TAP device: %s\n", device_path); + + /* translate high-level device name into a device instance + GUID using the registry */ + tt->actual_name = PICO_ZALLOC(strlen(name) + 1); + if (tt->actual_name) + strcpy(tt->actual_name, name); + } + + dbg_tap_info("TAP-WIN32 device [%s] opened: %s\n", tt->actual_name, device_path); + /* TODO TODO TODO */ + /* tt->adapter_index = get_adapter_index (device_guid); */ + + /* get driver version info */ + { + ULONG info[3]; + /* TODO TODO TODO */ + /* CLEAR (info); */ + if (DeviceIoControl (tt->hand, TAP_WIN_IOCTL_GET_VERSION, &info, sizeof (info), &info, sizeof (info), &len, NULL)) + { + dbg_tap_info ("TAP-Windows Driver Version %d.%d %s\n", + (int) info[0], + (int) info[1], + (info[2] ? "(DEBUG)" : "")); + + } + + if (!(info[0] == TAP_WIN_MIN_MAJOR && info[1] >= TAP_WIN_MIN_MINOR)) + dbg_tap_info ("ERROR: This version of " PACKAGE_NAME " requires a TAP-Windows driver that is at least version %d.%d \ + -- If you recently upgraded your " PACKAGE_NAME " distribution, \ + a reboot is probably required at this point to get Windows to see the new driver.\n", + TAP_WIN_MIN_MAJOR, + TAP_WIN_MIN_MINOR); + + /* usage of numeric constants is ugly, but this is really tied to + * *this* version of the driver + */ + if ( tt->ipv6 && tt->type == DEV_TYPE_TUN && info[0] == 9 && info[1] < 8) + { + dbg_tap_info("WARNING: Tap-Win32 driver version %d.%d does not support IPv6 in TUN mode. IPv6 will be disabled. \ + Upgrade to Tap-Win32 9.8 (2.2-beta3 release or later) or use TAP mode to get IPv6\n", (int) info[0], (int) info[1] ); + tt->ipv6 = 0; + } + + /* tap driver 9.8 (2.2.0 and 2.2.1 release) is buggy + */ + if ( tt->type == DEV_TYPE_TUN && info[0] == 9 && info[1] == 8) + { + dbg_tap_info("ERROR: Tap-Win32 driver version %d.%d is buggy regarding small IPv4 packets in TUN mode. Upgrade to Tap-Win32 9.9 (2.2.2 release or later) or use TAP mode\n", (int) info[0], (int) info[1] ); + } + } + + /* get driver MTU */ + { + ULONG mtu; + if (DeviceIoControl (tt->hand, TAP_WIN_IOCTL_GET_MTU, + &mtu, sizeof (mtu), + &mtu, sizeof (mtu), &len, NULL)) + { + tt->post_open_mtu = (int) mtu; + dbg_tap_info("TAP-Windows MTU=%d\n", (int) mtu); + } + } + + + /* get driver MAC */ + { + uint8_t mac[6] = { + 0, 0, 0, 0, 0, 0 + }; + if (DeviceIoControl (tt->hand, TAP_WIN_IOCTL_GET_MAC, + mac, sizeof (mac), + mac, sizeof (mac), &len, NULL)) + { + dbg_tap_info("TAP-Windows MAC=[%x,%x,%x,%x,%x,%x]\n", mac[0], mac[1], mac[2], + mac[2], mac[4], mac[5]); + memcpy(tt->mac, mac, sizeof(mac)); + } + } + + /* set point-to-point mode if TUN device */ + + if (tt->type == DEV_TYPE_TUN) + { + dbg_tap_info("TUN type not supported for now...\n"); + return -1; + } + else if (tt->type == DEV_TYPE_TAP) + { /* TAP DEVICE */ + dbg_tap_info("TODO: Set Point-to-point through DeviceIoControl\n"); + } + + /* set driver media status to 'connected' */ + { + ULONG status = TRUE; + if (!DeviceIoControl (tt->hand, TAP_WIN_IOCTL_SET_MEDIA_STATUS, + &status, sizeof (status), + &status, sizeof (status), &len, NULL)) + dbg_tap_info("WARNING: The TAP-Windows driver rejected a TAP_WIN_IOCTL_SET_MEDIA_STATUS DeviceIoControl call."); + } + + return 0; +} + + + + +/* TODO: Closing a TUN device is currently not implemented */ +/* + void close_tun (struct tuntap *tt) + { + (void)tt; + } + */ + + +int tap_win_getinfo (const struct tuntap *tt, char *buf, int bufsize) +{ + if (tt && tt->hand != NULL && buf != NULL) + { + DWORD len; + if (DeviceIoControl (tt->hand, TAP_WIN_IOCTL_GET_INFO, + buf, bufsize, + buf, bufsize, + &len, NULL)) + { + return 0; + } + } + + return -1; +} + +void tun_show_debug (struct tuntap *tt, char *buf, int bufsize) +{ + if (tt && tt->hand != NULL && buf != NULL) + { + DWORD len; + while (DeviceIoControl (tt->hand, TAP_WIN_IOCTL_GET_LOG_LINE, + buf, bufsize, + buf, bufsize, + &len, NULL)) + { + dbg_tap_info("TAP-Windows: %s\n", buf); + } + } +} + + +/* returns the state */ +int tun_read_queue (struct tuntap *tt, uint8_t *buffer, int maxsize) +{ + if (tt->reads.iostate == IOSTATE_INITIAL) + { + DWORD len = 1500; + BOOL status; + int err; + + /* reset buf to its initial state */ + tt->reads.buf = tt->reads.buf_init; + tt->reads.buf_len = tt->reads.buf_init_len; + + len = maxsize ? maxsize : (tt->reads.buf_len); + if (len > (tt->reads.buf_len)) /* clip to buffer len */ + len = tt->reads.buf_len; + + /* the overlapped read will signal this event on I/O completion */ + if (!ResetEvent (tt->reads.overlapped.hEvent)) + dbg_tap("ResetEvent failed\n"); + + status = ReadFile( + tt->hand, + buffer, + len, + &tt->reads.size, + &tt->reads.overlapped + ); + + if (status) /* operation completed immediately? */ + { + /* since we got an immediate return, we must signal the event object ourselves */ + /* ASSERT (SetEvent (tt->reads.overlapped.hEvent)); */ + if (!SetEvent (tt->reads.overlapped.hEvent)) + dbg_tap("SetEvent failed\n"); + + tt->reads.iostate = IOSTATE_IMMEDIATE_RETURN; + tt->reads.status = 0; + + dbg_win32 ("WIN32 I/O: TAP Read immediate return [%d,%d]\n", + (int) len, + (int) tt->reads.size); + } + else + { + err = GetLastError (); + if (err == ERROR_IO_PENDING) /* operation queued? */ + { + tt->reads.iostate = IOSTATE_QUEUED; + tt->reads.status = err; + dbg_win32 ("WIN32 I/O: TAP Read queued [%d]\n", (int) len); + } + else /* error occurred */ + { + if (!SetEvent (tt->reads.overlapped.hEvent)) + dbg_tap("SetEvent failed\n"); + + tt->reads.iostate = IOSTATE_IMMEDIATE_RETURN; + tt->reads.status = err; + dbg_tap ("WIN32 I/O: TAP Read error [%d] : %d\n", (int) len, (int) err); + } + } + } + + return tt->reads.iostate; +} + +/* Finalize any pending overlapped IO's */ +int tun_finalize(HANDLE h, struct overlapped_io *io, uint8_t **buf, uint32_t *buf_len) +{ + int ret = -1; + BOOL status; + + switch (io->iostate) + { + case IOSTATE_QUEUED: + status = GetOverlappedResult( + h, + &io->overlapped, + &io->size, + 0u + ); + if (status) + { + /* successful return for a queued operation */ + if (buf) + { + *buf = io->buf; + *buf_len = io->buf_len; + } + + ret = io->size; + io->iostate = IOSTATE_INITIAL; + + if (!ResetEvent (io->overlapped.hEvent)) + dbg_tap("ResetEvent in finalize failed!\n"); + + dbg_win32 ("WIN32 I/O: TAP Completion success: QUEUED! [%d]\n", ret); + } + else + { + /* error during a queued operation */ + /* error, or just not completed? */ + ret = 0; + if (GetLastError() != ERROR_IO_INCOMPLETE) + { + /* if no error (i.e. just not finished yet), + then DON'T execute this code */ + io->iostate = IOSTATE_INITIAL; + if (!ResetEvent (io->overlapped.hEvent)) + dbg_tap("ResetEvent in finalize failed!\n"); + + dbg_tap("WIN32 I/O: TAP Completion error\n"); + ret = -1; /* There actually was an error */ + } + } + + break; + + case IOSTATE_IMMEDIATE_RETURN: + io->iostate = IOSTATE_INITIAL; + if (!ResetEvent (io->overlapped.hEvent)) + dbg_tap("ResetEvent in finalize failed!\n"); + + if (io->status) + { + /* error return for a non-queued operation */ + SetLastError (io->status); + ret = -1; + dbg_tap("WIN32 I/O: TAP Completion non-queued error\n"); + } + else + { + /* successful return for a non-queued operation */ + if (buf) + *buf = io->buf; + + ret = io->size; + dbg_win32 ("WIN32 I/O: TAP Completion non-queued success [%d]\n", ret); + } + + break; + + case IOSTATE_INITIAL: /* were we called without proper queueing? */ + SetLastError (ERROR_INVALID_FUNCTION); + ret = -1; + dbg_tap ("WIN32 I/O: TAP Completion BAD STATE\n"); + break; + + default: + dbg_tap ("Some weird case happened..\n"); + } + + if (buf) + *buf_len = ret; + + return ret; +} + + + +/* returns the amount of bytes written */ +int tun_write_queue (struct tuntap *tt, uint8_t *buf, uint32_t buf_len) +{ + if (tt->writes.iostate == IOSTATE_INITIAL) + { + BOOL status; + int err; + + /* make a private copy of buf */ + tt->writes.buf = tt->writes.buf_init; + tt->writes.buf_len = buf_len; + memcpy(tt->writes.buf, buf, buf_len); + + /* the overlapped write will signal this event on I/O completion */ + if (!ResetEvent (tt->writes.overlapped.hEvent)) + dbg_tap("ResetEvent in write_queue failed!\n"); + + status = WriteFile( + tt->hand, + tt->writes.buf, + tt->writes.buf_len, + &tt->writes.size, + &tt->writes.overlapped + ); + + if (status) /* operation completed immediately? */ + { + tt->writes.iostate = IOSTATE_IMMEDIATE_RETURN; + + /* since we got an immediate return, we must signal the event object ourselves */ + if (!SetEvent (tt->writes.overlapped.hEvent)) + dbg_tap("SetEvent in write_queue failed!\n"); + + tt->writes.status = 0; + + dbg_win32 ("WIN32 I/O: TAP Write immediate return [%d,%d]\n", + (int)(tt->writes.buf_len), + (int)tt->writes.size); + } + else + { + err = GetLastError (); + if (err == ERROR_IO_PENDING) /* operation queued? */ + { + tt->writes.iostate = IOSTATE_QUEUED; + tt->writes.status = err; + dbg_win32("WIN32 I/O: TAP Write queued [%d]\n", + (tt->writes.buf_len)); + } + else /* error occurred */ + { + if (!SetEvent (tt->writes.overlapped.hEvent)) + dbg_tap("SetEvent in write_queue failed!\n"); + + tt->writes.iostate = IOSTATE_IMMEDIATE_RETURN; + tt->writes.status = err; + dbg_tap ("WIN32 I/O: TAP Write error [%d] : %d\n", (int) &tt->writes.buf_len, (int) err); + } + } + } + + return tt->writes.iostate; +} + +static inline int overlapped_io_active (struct overlapped_io *o) +{ + return o->iostate == IOSTATE_QUEUED || o->iostate == IOSTATE_IMMEDIATE_RETURN; +} + +/* if >= 0: returns the amount of bytes read, otherwise error! */ +static int tun_write_win32 (struct tuntap *tt, uint8_t *buf, uint32_t buf_len) +{ + int err = 0; + int status = 0; + if (overlapped_io_active (&tt->writes)) + { + status = tun_finalize (tt->hand, &tt->writes, NULL, 0); + if (status == 0) + { + /* busy, just wait, do not schedule a new write */ + return 0; + } + + if (status < 0) + err = GetLastError (); + } + + /* the overlapped IO is done, now we can schedule a new write */ + tun_write_queue (tt, buf, buf_len); + if (status < 0) + { + SetLastError (err); + return status; + } + else + return buf_len; +} + + +/* if >= 0: returns the amount of bytes read, otherwise error! */ +static int tun_read_win32 (struct tuntap *tt, uint8_t *buf, uint32_t buf_len) +{ + int err = 0; + int status = 0; + + + /* First, finish possible pending IOs */ + if (overlapped_io_active (&tt->reads)) + { + status = tun_finalize (tt->hand, &tt->reads, &buf, &buf_len); + if (status == 0) + { + /* busy, just wait, do not schedule a new read */ + return 0; + } + + if (status < 0) + { + dbg_tap ("tun_finalize status < 0: %d\n", status); + err = GetLastError (); + } + + if (status > 0) + { + return buf_len; + } + } + + /* If no pending IOs, schedule a new read */ + /* queue, or immediate return */ + if (IOSTATE_IMMEDIATE_RETURN == tun_read_queue(tt, buf, buf_len)) + { + return tt->reads.size; + } + + /* If the pending IOs gave an error, report it */ + if (status < 0) + { + SetLastError (err); + return status; + } + else + { + /* no errors, but the newly scheduled read is now pending */ + return 0; + } +} + + +static int read_tun_buffered(struct tuntap *tt, uint8_t *buf, uint32_t buf_len) +{ + return tun_read_win32 (tt, buf, buf_len); +} + +static int write_tun_buffered(struct tuntap *tt, uint8_t *buf, uint32_t buf_len) +{ + return tun_write_win32 (tt, buf, buf_len); +} + + +static int pico_tap_send(struct pico_device *dev, void *buf, int len) +{ + uint32_t bytes_sent = 0; + struct pico_device_tap *tap = (struct pico_device_tap *) dev; + + /* Increase the statistic count */ + tap->statistics_frames_out++; + + bytes_sent = write_tun_buffered (tap->tt, buf, len); + dbg_tap("TX> sent %d bytes\n", bytes_sent); + + /* Discard the frame content silently. */ + return bytes_sent; +} + +uint8_t recv_buffer[1500]; + +static int pico_tap_poll(struct pico_device *dev, int loop_score) +{ + struct pico_device_tap *tap = (struct pico_device_tap *) dev; + while (loop_score) + { + int bytes_read = read_tun_buffered(tap->tt, recv_buffer, 1500); + loop_score--; + if (bytes_read > 0) + { + dbg_tap("RX< recvd: %d bytes\n", bytes_read); + pico_stack_recv(dev, recv_buffer, bytes_read); + /* break; */ + } + else + break; + } + return loop_score; +} + + + +#define CLEAR(x) memset(&(x), 0, sizeof(x)) + +void overlapped_io_init (struct overlapped_io *o, int event_state) +{ + CLEAR (*o); + + /* manual reset event, initially set according to event_state */ + o->overlapped.hEvent = CreateEvent (NULL, TRUE, event_state, NULL); + if (o->overlapped.hEvent == NULL) + dbg_tap ("Error: overlapped_io_init: CreateEvent failed\n"); + + /* allocate buffer for overlapped I/O */ + o->buf_init = PICO_ZALLOC(1500); /* XXX: MTU */ + o->buf_init_len = 1500; /* XXX: MTU */ + if (!(o->buf_init)) + dbg_tap("buffer alloc failed!\n"); /* XXX: return -1 or so? */ + else + dbg_tap("overlapped_io_init buffer allocated!\n"); +} + +void init_tun_post (struct tuntap *tt) +{ + dbg_tap("TUN post init (for overlapped io)\n"); + overlapped_io_init (&tt->reads, FALSE); + overlapped_io_init (&tt->writes, TRUE); + tt->rw_handle.read = tt->reads.overlapped.hEvent; + tt->rw_handle.write = tt->writes.overlapped.hEvent; +} + + +/* + * Public interface: pico_tap_create + * TODO: pico_tap_destroy + */ + +struct pico_device *pico_tap_create(char *name, uint8_t *mac) +{ + struct pico_device_tap *tap = PICO_ZALLOC(sizeof(struct pico_device_tap)); + struct tuntap *tt = PICO_ZALLOC(sizeof(struct tuntap), 1); + + if (!(tap) || !(tt)) + return NULL; + + tap->dev.overhead = 0; + tap->statistics_frames_out = 0; + tap->dev.send = pico_tap_send; + tap->dev.poll = pico_tap_poll; + + show_tap_win_adapters(); + + tt->type = DEV_TYPE_TAP; + if (open_tun(NULL, NULL, "tap0", tt)) + { + dbg_tap("Failed to create TAP device!\n"); + PICO_FREE(tt); + PICO_FREE(tap); + return NULL; + } + + tap->tt = tt; + + if( 0 != pico_device_init((struct pico_device *)tap, name, mac)) { + return NULL; + } + + init_tun_post(tt); /* init overlapped io */ + + dbg_tap("Device %s created.\n", tap->dev.name); + + return (struct pico_device *)tap; +} + diff --git a/ext/picotcp/modules/pico_dev_tap_windows.h b/ext/picotcp/modules/pico_dev_tap_windows.h new file mode 100755 index 0000000..25bd4fc --- /dev/null +++ b/ext/picotcp/modules/pico_dev_tap_windows.h @@ -0,0 +1,17 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2014-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#ifndef INCLUDE_PICO_TAP +#define INCLUDE_PICO_TAP +#include "pico_config.h" +#include "pico_device.h" + +/* will look for the first TAP device available, and use it */ +struct pico_device *pico_tap_create(char *name, uint8_t *mac); +/* TODO: not implemented yet */ +/* void pico_tap_destroy(struct pico_device *null); */ + +#endif + diff --git a/ext/picotcp/modules/pico_dev_tap_windows_private.h b/ext/picotcp/modules/pico_dev_tap_windows_private.h new file mode 100644 index 0000000..8f3b192 --- /dev/null +++ b/ext/picotcp/modules/pico_dev_tap_windows_private.h @@ -0,0 +1,89 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2014-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Maxime Vincent + Based on the OpenVPN tun.c driver, under GPL + + NOTES: This is the Windows-only driver, a Linux-equivalent is available, too + You need to have an OpenVPN TUN/TAP network adapter installed, first + This driver is barely working: + * Only TAP-mode is supported (TUN is not) + * it will simply open the first TAP device it can find + * there is memory being allocated that's never freed + * there is no destroy function, yet + * it has only been tested on a Windows 7 machine + *********************************************************************/ + +#ifndef __PICO_DEV_TAP_WINDOWS_PRIVATE_H +#define __PICO_DEV_TAP_WINDOWS_PRIVATE_H + +/* Extra defines (vnz) */ +#define TAP_WIN_COMPONENT_ID "tap0901" +#define TAP_WIN_MIN_MAJOR 9 +#define TAP_WIN_MIN_MINOR 9 +#define PACKAGE_NAME "PicoTCP WinTAP" + +/* Extra structs */ +struct tap_reg +{ + const char *guid; + struct tap_reg *next; +}; + +struct panel_reg +{ + const char *name; + const char *guid; + struct panel_reg *next; +}; + + +/* + * ============= + * TAP IOCTLs + * ============= + */ + +#define TAP_WIN_CONTROL_CODE(request, method) \ + CTL_CODE (FILE_DEVICE_UNKNOWN, request, method, FILE_ANY_ACCESS) + +/* Present in 8.1 */ + +#define TAP_WIN_IOCTL_GET_MAC TAP_WIN_CONTROL_CODE (1, METHOD_BUFFERED) +#define TAP_WIN_IOCTL_GET_VERSION TAP_WIN_CONTROL_CODE (2, METHOD_BUFFERED) +#define TAP_WIN_IOCTL_GET_MTU TAP_WIN_CONTROL_CODE (3, METHOD_BUFFERED) +#define TAP_WIN_IOCTL_GET_INFO TAP_WIN_CONTROL_CODE (4, METHOD_BUFFERED) +#define TAP_WIN_IOCTL_CONFIG_POINT_TO_POINT TAP_WIN_CONTROL_CODE (5, METHOD_BUFFERED) +#define TAP_WIN_IOCTL_SET_MEDIA_STATUS TAP_WIN_CONTROL_CODE (6, METHOD_BUFFERED) +#define TAP_WIN_IOCTL_CONFIG_DHCP_MASQ TAP_WIN_CONTROL_CODE (7, METHOD_BUFFERED) +#define TAP_WIN_IOCTL_GET_LOG_LINE TAP_WIN_CONTROL_CODE (8, METHOD_BUFFERED) +#define TAP_WIN_IOCTL_CONFIG_DHCP_SET_OPT TAP_WIN_CONTROL_CODE (9, METHOD_BUFFERED) + +/* Added in 8.2 */ + +/* obsoletes TAP_WIN_IOCTL_CONFIG_POINT_TO_POINT */ +#define TAP_WIN_IOCTL_CONFIG_TUN TAP_WIN_CONTROL_CODE (10, METHOD_BUFFERED) + +/* + * ================= + * Registry keys + * ================= + */ + +#define ADAPTER_KEY "SYSTEM\\CurrentControlSet\\Control\\Class\\{4D36E972-E325-11CE-BFC1-08002BE10318}" + +#define NETWORK_CONNECTIONS_KEY "SYSTEM\\CurrentControlSet\\Control\\Network\\{4D36E972-E325-11CE-BFC1-08002BE10318}" + +/* + * ====================== + * Filesystem prefixes + * ====================== + */ + +#define USERMODEDEVICEDIR "\\\\.\\Global\\" +#define SYSDEVICEDIR "\\Device\\" +#define USERDEVICEDIR "\\DosDevices\\Global\\" +#define TAP_WIN_SUFFIX ".tap" + +#endif diff --git a/ext/picotcp/modules/pico_dev_tun.c b/ext/picotcp/modules/pico_dev_tun.c new file mode 100644 index 0000000..c5791d6 --- /dev/null +++ b/ext/picotcp/modules/pico_dev_tun.c @@ -0,0 +1,110 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Daniele Lacamera + *********************************************************************/ + + +#include +#include +#include +#include +#include "pico_device.h" +#include "pico_dev_tun.h" +#include "pico_stack.h" + +#include + +struct pico_device_tun { + struct pico_device dev; + int fd; +}; + +#define TUN_MTU 2048 + +static int pico_tun_send(struct pico_device *dev, void *buf, int len) +{ + struct pico_device_tun *tun = (struct pico_device_tun *) dev; + return (int)write(tun->fd, buf, (uint32_t)len); +} + +static int pico_tun_poll(struct pico_device *dev, int loop_score) +{ + struct pico_device_tun *tun = (struct pico_device_tun *) dev; + struct pollfd pfd; + unsigned char buf[TUN_MTU]; + int len; + pfd.fd = tun->fd; + pfd.events = POLLIN; + do { + if (poll(&pfd, 1, 0) <= 0) + return loop_score; + + len = (int)read(tun->fd, buf, TUN_MTU); + if (len > 0) { + loop_score--; + pico_stack_recv(dev, buf, (uint32_t)len); + } + } while(loop_score > 0); + return 0; +} + +/* Public interface: create/destroy. */ + +void pico_tun_destroy(struct pico_device *dev) +{ + struct pico_device_tun *tun = (struct pico_device_tun *) dev; + if(tun->fd > 0) + close(tun->fd); +} + + +static int tun_open(char *name) +{ + struct ifreq ifr; + int tun_fd; + if((tun_fd = open("/dev/net/tun", O_RDWR)) < 0) { + return(-1); + } + + memset(&ifr, 0, sizeof(ifr)); + ifr.ifr_flags = IFF_TUN | IFF_NO_PI; + strncpy(ifr.ifr_name, name, IFNAMSIZ); + if(ioctl(tun_fd, TUNSETIFF, &ifr) < 0) { + return(-1); + } + + return tun_fd; +} + + + +struct pico_device *pico_tun_create(char *name) +{ + struct pico_device_tun *tun = PICO_ZALLOC(sizeof(struct pico_device_tun)); + + if (!tun) + return NULL; + + if( 0 != pico_device_init((struct pico_device *)tun, name, NULL)) { + dbg("Tun init failed.\n"); + pico_tun_destroy((struct pico_device *)tun); + return NULL; + } + + tun->dev.overhead = 0; + tun->fd = tun_open(name); + if (tun->fd < 0) { + dbg("Tun creation failed.\n"); + pico_tun_destroy((struct pico_device *)tun); + return NULL; + } + + tun->dev.send = pico_tun_send; + tun->dev.poll = pico_tun_poll; + tun->dev.destroy = pico_tun_destroy; + dbg("Device %s created.\n", tun->dev.name); + return (struct pico_device *)tun; +} + diff --git a/ext/picotcp/modules/pico_dev_tun.h b/ext/picotcp/modules/pico_dev_tun.h new file mode 100644 index 0000000..4dd477e --- /dev/null +++ b/ext/picotcp/modules/pico_dev_tun.h @@ -0,0 +1,15 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#ifndef INCLUDE_PICO_TUN +#define INCLUDE_PICO_TUN +#include "pico_config.h" +#include "pico_device.h" + +void pico_tun_destroy(struct pico_device *tun); +struct pico_device *pico_tun_create(char *name); + +#endif + diff --git a/ext/picotcp/modules/pico_dev_vde.c b/ext/picotcp/modules/pico_dev_vde.c new file mode 100644 index 0000000..df07a3b --- /dev/null +++ b/ext/picotcp/modules/pico_dev_vde.c @@ -0,0 +1,115 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Daniele Lacamera + *********************************************************************/ + +#ifndef UNIT_TEST +#include +#endif +#include "pico_device.h" +#include "pico_dev_vde.h" +#include "pico_stack.h" + +#include + +struct pico_device_vde { + struct pico_device dev; + char *sock; + VDECONN *conn; + uint32_t counter_in; + uint32_t counter_out; + uint32_t lost_in; + uint32_t lost_out; +}; + +#define VDE_MTU 65536 + +static int pico_vde_send(struct pico_device *dev, void *buf, int len) +{ + struct pico_device_vde *vde = (struct pico_device_vde *) dev; + /* dbg("[%s] send %d bytes.\n", dev->name, len); */ + if ((vde->lost_out == 0) || ((pico_rand() % 100) > vde->lost_out)) + return (int)vde_send(vde->conn, buf, (uint32_t)len, 0); + else + return len; /* Silently discarded "on the wire" */ + +} + +static int pico_vde_poll(struct pico_device *dev, int loop_score) +{ + struct pico_device_vde *vde = (struct pico_device_vde *) dev; + struct pollfd pfd; + unsigned char buf[VDE_MTU]; + int len; + pfd.fd = vde_datafd(vde->conn); + pfd.events = POLLIN; + do { + if (poll(&pfd, 1, 0) <= 0) + return loop_score; + + len = (int)vde_recv(vde->conn, buf, VDE_MTU, 0); + if (len > 0) { + /* dbg("Received pkt.\n"); */ + if ((vde->lost_in == 0) || ((pico_rand() % 100) > vde->lost_in)) { + loop_score--; + pico_stack_recv(dev, buf, (uint32_t)len); + } + } + } while(loop_score > 0); + return 0; +} + +/* Public interface: create/destroy. */ + +void pico_vde_destroy(struct pico_device *dev) +{ + struct pico_device_vde *vde = (struct pico_device_vde *) dev; + vde_close(vde->conn); + usleep(100000); + sync(); +} + +void pico_vde_set_packetloss(struct pico_device *dev, uint32_t in_pct, uint32_t out_pct) +{ + struct pico_device_vde *vde = (struct pico_device_vde *) dev; + vde->lost_in = in_pct; + vde->lost_out = out_pct; +} + + + +struct pico_device *pico_vde_create(char *sock, char *name, uint8_t *mac) +{ + struct pico_device_vde *vde = PICO_ZALLOC(sizeof(struct pico_device_vde)); + struct vde_open_args open_args = { + .mode = 0700 + }; + char vdename[] = "picotcp"; + + if (!vde) + return NULL; + + if( 0 != pico_device_init((struct pico_device *)vde, name, mac)) { + dbg ("Vde init failed.\n"); + pico_vde_destroy((struct pico_device *)vde); + return NULL; + } + + vde->dev.overhead = 0; + vde->sock = PICO_ZALLOC(strlen(sock) + 1); + memcpy(vde->sock, sock, strlen(sock)); + vde->conn = vde_open(sock, vdename, &open_args); + if (!vde->conn) { + pico_vde_destroy((struct pico_device *)vde); + return NULL; + } + + vde->dev.send = pico_vde_send; + vde->dev.poll = pico_vde_poll; + vde->dev.destroy = pico_vde_destroy; + dbg("Device %s created.\n", vde->dev.name); + return (struct pico_device *)vde; +} + diff --git a/ext/picotcp/modules/pico_dev_vde.h b/ext/picotcp/modules/pico_dev_vde.h new file mode 100644 index 0000000..c6c999e --- /dev/null +++ b/ext/picotcp/modules/pico_dev_vde.h @@ -0,0 +1,18 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + + *********************************************************************/ +#ifndef INCLUDE_PICO_VDE +#define INCLUDE_PICO_VDE +#include "pico_config.h" +#include "pico_device.h" +#include + +void pico_vde_destroy(struct pico_device *vde); +struct pico_device *pico_vde_create(char *sock, char *name, uint8_t *mac); +void pico_vde_set_packetloss(struct pico_device *dev, uint32_t in_pct, uint32_t out_pct); + +#endif + diff --git a/ext/picotcp/modules/pico_dhcp_client.c b/ext/picotcp/modules/pico_dhcp_client.c new file mode 100644 index 0000000..f834dd6 --- /dev/null +++ b/ext/picotcp/modules/pico_dhcp_client.c @@ -0,0 +1,990 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Kristof Roelants, Frederik Van Slycken + *********************************************************************/ + + +#include "pico_dhcp_client.h" +#include "pico_stack.h" +#include "pico_config.h" +#include "pico_device.h" +#include "pico_ipv4.h" +#include "pico_socket.h" +#include "pico_eth.h" + +#if (defined PICO_SUPPORT_DHCPC && defined PICO_SUPPORT_UDP) +#define dhcpc_dbg(...) do {} while(0) +/* #define dhcpc_dbg dbg */ + +/* timer values */ +#define DHCP_CLIENT_REINIT 6000 /* msec */ +#define DHCP_CLIENT_RETRANS 4 /* sec */ +#define DHCP_CLIENT_RETRIES 3 + +#define DHCP_CLIENT_TIMER_STOPPED 0 +#define DHCP_CLIENT_TIMER_STARTED 1 + +/* maximum size of a DHCP message */ +#define DHCP_CLIENT_MAXMSGZISE (PICO_IP_MRU - PICO_SIZE_IP4HDR) +#define PICO_DHCP_HOSTNAME_MAXLEN 64U + +static char dhcpc_host_name[PICO_DHCP_HOSTNAME_MAXLEN] = ""; +static char dhcpc_domain_name[PICO_DHCP_HOSTNAME_MAXLEN] = ""; + + +enum dhcp_client_state { + DHCP_CLIENT_STATE_INIT_REBOOT = 0, + DHCP_CLIENT_STATE_REBOOTING, + DHCP_CLIENT_STATE_INIT, + DHCP_CLIENT_STATE_SELECTING, + DHCP_CLIENT_STATE_REQUESTING, + DHCP_CLIENT_STATE_BOUND, + DHCP_CLIENT_STATE_RENEWING, + DHCP_CLIENT_STATE_REBINDING +}; + + +#define PICO_DHCPC_TIMER_INIT 0 +#define PICO_DHCPC_TIMER_REQUEST 1 +#define PICO_DHCPC_TIMER_RENEW 2 +#define PICO_DHCPC_TIMER_REBIND 3 +#define PICO_DHCPC_TIMER_T1 4 +#define PICO_DHCPC_TIMER_T2 5 +#define PICO_DHCPC_TIMER_LEASE 6 +#define PICO_DHCPC_TIMER_ARRAY_SIZE 7 + +struct dhcp_client_timer +{ + uint8_t state; + unsigned int type; + uint32_t xid; +}; + +struct pico_dhcp_client_cookie +{ + uint8_t event; + uint8_t retry; + uint32_t xid; + uint32_t *uid; + enum dhcp_client_state state; + void (*cb)(void*dhcpc, int code); + pico_time init_timestamp; + struct pico_socket *s; + struct pico_ip4 address; + struct pico_ip4 netmask; + struct pico_ip4 gateway; + struct pico_ip4 nameserver[2]; + struct pico_ip4 server_id; + struct pico_device *dev; + struct dhcp_client_timer *timer[PICO_DHCPC_TIMER_ARRAY_SIZE]; + uint32_t t1_time; + uint32_t t2_time; + uint32_t lease_time; + uint32_t renew_time; + uint32_t rebind_time; +}; + +static int pico_dhcp_client_init(struct pico_dhcp_client_cookie *dhcpc); +static int reset(struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf); +static int8_t pico_dhcp_client_msg(struct pico_dhcp_client_cookie *dhcpc, uint8_t msg_type); +static void pico_dhcp_client_wakeup(uint16_t ev, struct pico_socket *s); +static void pico_dhcp_state_machine(uint8_t event, struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf); + +static const struct pico_ip4 bcast_netmask = { + .addr = 0xFFFFFFFF +}; + +static struct pico_ip4 inaddr_any = { + 0 +}; + + +static int dhcp_cookies_cmp(void *ka, void *kb) +{ + struct pico_dhcp_client_cookie *a = ka, *b = kb; + if (a->xid == b->xid) + return 0; + + return (a->xid < b->xid) ? (-1) : (1); +} +PICO_TREE_DECLARE(DHCPCookies, dhcp_cookies_cmp); + +static struct pico_dhcp_client_cookie *pico_dhcp_client_add_cookie(uint32_t xid, struct pico_device *dev, void (*cb)(void *dhcpc, int code), uint32_t *uid) +{ + struct pico_dhcp_client_cookie *dhcpc = NULL, *found = NULL, test = { + 0 + }; + + test.xid = xid; + found = pico_tree_findKey(&DHCPCookies, &test); + if (found) { + pico_err = PICO_ERR_EAGAIN; + return NULL; + } + + dhcpc = PICO_ZALLOC(sizeof(struct pico_dhcp_client_cookie)); + if (!dhcpc) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + dhcpc->state = DHCP_CLIENT_STATE_INIT; + dhcpc->xid = xid; + dhcpc->uid = uid; + *(dhcpc->uid) = 0; + dhcpc->cb = cb; + dhcpc->dev = dev; + + pico_tree_insert(&DHCPCookies, dhcpc); + return dhcpc; +} + +static void pico_dhcp_client_stop_timers(struct pico_dhcp_client_cookie *dhcpc); +static int pico_dhcp_client_del_cookie(uint32_t xid) +{ + struct pico_dhcp_client_cookie test = { + 0 + }, *found = NULL; + + test.xid = xid; + found = pico_tree_findKey(&DHCPCookies, &test); + if (!found) + return -1; + + pico_dhcp_client_stop_timers(found); + pico_socket_close(found->s); + found->s = NULL; + pico_ipv4_link_del(found->dev, found->address); + pico_tree_delete(&DHCPCookies, found); + PICO_FREE(found); + return 0; +} + +static struct pico_dhcp_client_cookie *pico_dhcp_client_find_cookie(uint32_t xid) +{ + struct pico_dhcp_client_cookie test = { + 0 + }, *found = NULL; + + test.xid = xid; + found = pico_tree_findKey(&DHCPCookies, &test); + if (found) + return found; + else + return NULL; +} + +static void pico_dhcp_client_timer_handler(pico_time now, void *arg); +static void pico_dhcp_client_reinit(pico_time now, void *arg); +static struct dhcp_client_timer *pico_dhcp_timer_add(uint8_t type, uint32_t time, struct pico_dhcp_client_cookie *ck) +{ + struct dhcp_client_timer *t; + + t = PICO_ZALLOC(sizeof(struct dhcp_client_timer)); + if (!t) + return NULL; + + t->state = DHCP_CLIENT_TIMER_STARTED; + t->xid = ck->xid; + t->type = type; + pico_timer_add(time, pico_dhcp_client_timer_handler, t); + if (ck->timer[type]) { + ck->timer[type]->state = DHCP_CLIENT_TIMER_STOPPED; + } + + ck->timer[type] = t; + return t; +} + +static int dhcp_get_timer_event(struct pico_dhcp_client_cookie *dhcpc, unsigned int type) +{ + const int events[PICO_DHCPC_TIMER_ARRAY_SIZE] = + { + PICO_DHCP_EVENT_RETRANSMIT, + PICO_DHCP_EVENT_RETRANSMIT, + PICO_DHCP_EVENT_RETRANSMIT, + PICO_DHCP_EVENT_RETRANSMIT, + PICO_DHCP_EVENT_T1, + PICO_DHCP_EVENT_T2, + PICO_DHCP_EVENT_LEASE + }; + + if (type == PICO_DHCPC_TIMER_REQUEST) { + if (++dhcpc->retry > DHCP_CLIENT_RETRIES) { + reset(dhcpc, NULL); + return PICO_DHCP_EVENT_NONE; + } + } else if (type < PICO_DHCPC_TIMER_T1) { + dhcpc->retry++; + } + + return events[type]; +} + +static void pico_dhcp_client_timer_handler(pico_time now, void *arg) +{ + struct dhcp_client_timer *t = (struct dhcp_client_timer *)arg; + struct pico_dhcp_client_cookie *dhcpc; + + if (!t) + return; + + (void) now; + if (t->state != DHCP_CLIENT_TIMER_STOPPED) { + dhcpc = pico_dhcp_client_find_cookie(t->xid); + if (dhcpc && dhcpc->timer) { + t->state = DHCP_CLIENT_TIMER_STOPPED; + if ((t->type == PICO_DHCPC_TIMER_INIT) && (dhcpc->state < DHCP_CLIENT_STATE_SELECTING)) { + pico_dhcp_client_reinit(now, dhcpc); + } else if (t->type != PICO_DHCPC_TIMER_INIT) { + dhcpc->event = (uint8_t)dhcp_get_timer_event(dhcpc, t->type); + if (dhcpc->event != PICO_DHCP_EVENT_NONE) + pico_dhcp_state_machine(dhcpc->event, dhcpc, NULL); + } + } + } +} + +static void pico_dhcp_client_timer_stop(struct pico_dhcp_client_cookie *dhcpc, int type) +{ + if (dhcpc->timer[type]) { + dhcpc->timer[type]->state = DHCP_CLIENT_TIMER_STOPPED; + } + +} + + +static void pico_dhcp_client_reinit(pico_time now, void *arg) +{ + struct pico_dhcp_client_cookie *dhcpc = (struct pico_dhcp_client_cookie *)arg; + (void) now; + + if (dhcpc->s) { + pico_socket_close(dhcpc->s); + dhcpc->s = NULL; + } + + if (++dhcpc->retry > DHCP_CLIENT_RETRIES) { + pico_err = PICO_ERR_EAGAIN; + if (dhcpc->cb) + dhcpc->cb(dhcpc, PICO_DHCP_ERROR); + + pico_dhcp_client_del_cookie(dhcpc->xid); + return; + } + + pico_dhcp_client_init(dhcpc); + return; +} + + +static void pico_dhcp_client_stop_timers(struct pico_dhcp_client_cookie *dhcpc) +{ + int i; + dhcpc->retry = 0; + for (i = 0; i < PICO_DHCPC_TIMER_ARRAY_SIZE; i++) + pico_dhcp_client_timer_stop(dhcpc, i); +} + +static void pico_dhcp_client_start_init_timer(struct pico_dhcp_client_cookie *dhcpc) +{ + uint32_t time = 0; + /* timer value is doubled with every retry (exponential backoff) */ + time = (uint32_t) (DHCP_CLIENT_RETRANS << dhcpc->retry); + pico_dhcp_timer_add(PICO_DHCPC_TIMER_INIT, time * 1000, dhcpc); +} + +static void pico_dhcp_client_start_requesting_timer(struct pico_dhcp_client_cookie *dhcpc) +{ + uint32_t time = 0; + + /* timer value is doubled with every retry (exponential backoff) */ + time = (uint32_t)(DHCP_CLIENT_RETRANS << dhcpc->retry); + pico_dhcp_timer_add(PICO_DHCPC_TIMER_REQUEST, time * 1000, dhcpc); +} + +static void pico_dhcp_client_start_renewing_timer(struct pico_dhcp_client_cookie *dhcpc) +{ + uint32_t halftime = 0; + + /* wait one-half of the remaining time until T2, down to a minimum of 60 seconds */ + /* (dhcpc->retry + 1): initial -> divide by 2, 1st retry -> divide by 4, 2nd retry -> divide by 8, etc */ + pico_dhcp_client_stop_timers(dhcpc); + halftime = dhcpc->renew_time >> (dhcpc->retry + 1); + if (halftime < 60) + halftime = 60; + + pico_dhcp_timer_add(PICO_DHCPC_TIMER_RENEW, halftime * 1000, dhcpc); + + return; +} + +static void pico_dhcp_client_start_rebinding_timer(struct pico_dhcp_client_cookie *dhcpc) +{ + uint32_t halftime = 0; + + pico_dhcp_client_stop_timers(dhcpc); + halftime = dhcpc->rebind_time >> (dhcpc->retry + 1); + if (halftime < 60) + halftime = 60; + + pico_dhcp_timer_add(PICO_DHCPC_TIMER_REBIND, halftime * 1000, dhcpc); + + return; +} + +static void pico_dhcp_client_start_reacquisition_timers(struct pico_dhcp_client_cookie *dhcpc) +{ + + pico_dhcp_client_stop_timers(dhcpc); + pico_dhcp_timer_add(PICO_DHCPC_TIMER_T1, dhcpc->t1_time * 1000, dhcpc); + pico_dhcp_timer_add(PICO_DHCPC_TIMER_T2, dhcpc->t2_time * 1000, dhcpc); + pico_dhcp_timer_add(PICO_DHCPC_TIMER_LEASE, dhcpc->lease_time * 1000, dhcpc); +} + +static int pico_dhcp_client_init(struct pico_dhcp_client_cookie *dhcpc) +{ + uint16_t port = PICO_DHCP_CLIENT_PORT; + if (!dhcpc) + return -1; + + /* adding a link with address 0.0.0.0 and netmask 0.0.0.0, + * automatically adds a route for a global broadcast */ + pico_ipv4_link_add(dhcpc->dev, inaddr_any, bcast_netmask); + if (!dhcpc->s) + dhcpc->s = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_UDP, &pico_dhcp_client_wakeup); + + if (!dhcpc->s) { + pico_dhcp_timer_add(PICO_DHCPC_TIMER_INIT, DHCP_CLIENT_REINIT, dhcpc); + return 0; + } + + dhcpc->s->dev = dhcpc->dev; + if (pico_socket_bind(dhcpc->s, &inaddr_any, &port) < 0) { + pico_socket_close(dhcpc->s); + dhcpc->s = NULL; + pico_dhcp_timer_add(PICO_DHCPC_TIMER_INIT, DHCP_CLIENT_REINIT, dhcpc); + return 0; + } + + if (pico_dhcp_client_msg(dhcpc, PICO_DHCP_MSG_DISCOVER) < 0) { + pico_socket_close(dhcpc->s); + dhcpc->s = NULL; + pico_dhcp_timer_add(PICO_DHCPC_TIMER_INIT, DHCP_CLIENT_REINIT, dhcpc); + return 0; + } + + dhcpc->retry = 0; + dhcpc->init_timestamp = PICO_TIME_MS(); + pico_dhcp_client_start_init_timer(dhcpc); + return 0; +} + +int pico_dhcp_initiate_negotiation(struct pico_device *dev, void (*cb)(void *dhcpc, int code), uint32_t *uid) +{ + uint8_t retry = 32; + uint32_t xid = 0; + struct pico_dhcp_client_cookie *dhcpc = NULL; + + if (!dev || !cb || !uid) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + if (!dev->eth) { + pico_err = PICO_ERR_EOPNOTSUPP; + return -1; + } + + /* attempt to generate a correct xid, else fail */ + do { + xid = pico_rand(); + } while (!xid && --retry); + + if (!xid) { + pico_err = PICO_ERR_EAGAIN; + return -1; + } + + dhcpc = pico_dhcp_client_add_cookie(xid, dev, cb, uid); + if (!dhcpc) + return -1; + + dhcpc_dbg("DHCP client: cookie with xid %u\n", dhcpc->xid); + *uid = xid; + return pico_dhcp_client_init(dhcpc); +} + +static void pico_dhcp_client_recv_params(struct pico_dhcp_client_cookie *dhcpc, struct pico_dhcp_opt *opt) +{ + do { + switch (opt->code) + { + case PICO_DHCP_OPT_PAD: + break; + + case PICO_DHCP_OPT_END: + break; + + case PICO_DHCP_OPT_MSGTYPE: + dhcpc->event = opt->ext.msg_type.type; + dhcpc_dbg("DHCP client: message type %u\n", dhcpc->event); + break; + + case PICO_DHCP_OPT_LEASETIME: + dhcpc->lease_time = long_be(opt->ext.lease_time.time); + dhcpc_dbg("DHCP client: lease time %u\n", dhcpc->lease_time); + break; + + case PICO_DHCP_OPT_RENEWALTIME: + dhcpc->t1_time = long_be(opt->ext.renewal_time.time); + dhcpc_dbg("DHCP client: renewal time %u\n", dhcpc->t1_time); + break; + + case PICO_DHCP_OPT_REBINDINGTIME: + dhcpc->t2_time = long_be(opt->ext.rebinding_time.time); + dhcpc_dbg("DHCP client: rebinding time %u\n", dhcpc->t2_time); + break; + + case PICO_DHCP_OPT_ROUTER: + dhcpc->gateway = opt->ext.router.ip; + dhcpc_dbg("DHCP client: router %08X\n", dhcpc->gateway.addr); + break; + + case PICO_DHCP_OPT_DNS: + dhcpc->nameserver[0] = opt->ext.dns1.ip; + dhcpc_dbg("DHCP client: dns1 %08X\n", dhcpc->nameserver[0].addr); + if (opt->len >= 8) { + dhcpc->nameserver[1] = opt->ext.dns2.ip; + dhcpc_dbg("DHCP client: dns1 %08X\n", dhcpc->nameserver[1].addr); + } + + break; + + case PICO_DHCP_OPT_NETMASK: + dhcpc->netmask = opt->ext.netmask.ip; + dhcpc_dbg("DHCP client: netmask %08X\n", dhcpc->netmask.addr); + break; + + case PICO_DHCP_OPT_SERVERID: + dhcpc->server_id = opt->ext.server_id.ip; + dhcpc_dbg("DHCP client: server ID %08X\n", dhcpc->server_id.addr); + break; + + case PICO_DHCP_OPT_OPTOVERLOAD: + dhcpc_dbg("DHCP client: WARNING option overload present (not processed)"); + break; + + case PICO_DHCP_OPT_HOSTNAME: + { + uint32_t maxlen = PICO_DHCP_HOSTNAME_MAXLEN; + if (opt->len < maxlen) + maxlen = opt->len; + strncpy(dhcpc_host_name, opt->ext.string.txt, maxlen); + } + break; + + case PICO_DHCP_OPT_DOMAINNAME: + { + uint32_t maxlen = PICO_DHCP_HOSTNAME_MAXLEN; + if (opt->len < maxlen) + maxlen = opt->len; + strncpy(dhcpc_domain_name, opt->ext.string.txt, maxlen); + } + break; + + default: + dhcpc_dbg("DHCP client: WARNING unsupported option %u\n", opt->code); + break; + } + } while (pico_dhcp_next_option(&opt)); + + /* default values for T1 and T2 when not provided */ + if (!dhcpc->t1_time) + dhcpc->t1_time = dhcpc->lease_time >> 1; + + if (!dhcpc->t2_time) + dhcpc->t2_time = (dhcpc->lease_time * 875) / 1000; + + return; +} + +static int recv_offer(struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf) +{ + struct pico_dhcp_hdr *hdr = (struct pico_dhcp_hdr *)buf; + struct pico_dhcp_opt *opt = DHCP_OPT(hdr,0); + + pico_dhcp_client_recv_params(dhcpc, opt); + if ((dhcpc->event != PICO_DHCP_MSG_OFFER) || !dhcpc->server_id.addr || !dhcpc->netmask.addr || !dhcpc->lease_time) + return -1; + + dhcpc->address.addr = hdr->yiaddr; + + /* we skip state SELECTING, process first offer received */ + dhcpc->state = DHCP_CLIENT_STATE_REQUESTING; + dhcpc->retry = 0; + pico_dhcp_client_msg(dhcpc, PICO_DHCP_MSG_REQUEST); + pico_dhcp_client_start_requesting_timer(dhcpc); + return 0; +} + +static int recv_ack(struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf) +{ + struct pico_dhcp_hdr *hdr = (struct pico_dhcp_hdr *)buf; + struct pico_dhcp_opt *opt = DHCP_OPT(hdr,0); + struct pico_ip4 address = { + 0 + }; + struct pico_ip4 any_address = { + 0 + }; + + struct pico_ipv4_link *l; + + pico_dhcp_client_recv_params(dhcpc, opt); + if ((dhcpc->event != PICO_DHCP_MSG_ACK) || !dhcpc->server_id.addr || !dhcpc->netmask.addr || !dhcpc->lease_time) + return -1; + + /* Issue #20 the server can transmit on ACK a different IP than the one in OFFER */ + /* RFC2131 ch 4.3.2 ... The client SHOULD use the parameters in the DHCPACK message for configuration */ + if (dhcpc->state == DHCP_CLIENT_STATE_REQUESTING) + dhcpc->address.addr = hdr->yiaddr; + + + /* close the socket used for address (re)acquisition */ + pico_socket_close(dhcpc->s); + dhcpc->s = NULL; + + /* Delete all the links before adding the address */ + pico_ipv4_link_del(dhcpc->dev, address); + l = pico_ipv4_link_by_dev(dhcpc->dev); + while(l) { + pico_ipv4_link_del(dhcpc->dev, l->address); + l = pico_ipv4_link_by_dev_next(dhcpc->dev, l); + } + pico_ipv4_link_add(dhcpc->dev, dhcpc->address, dhcpc->netmask); + + dbg("DHCP client: renewal time (T1) %u\n", (unsigned int)dhcpc->t1_time); + dbg("DHCP client: rebinding time (T2) %u\n", (unsigned int)dhcpc->t2_time); + dbg("DHCP client: lease time %u\n", (unsigned int)dhcpc->lease_time); + + /* If router option is received, use it as default gateway */ + if (dhcpc->gateway.addr != 0U) { + pico_ipv4_route_add(any_address, any_address, dhcpc->gateway, 1, NULL); + } + + dhcpc->retry = 0; + dhcpc->renew_time = dhcpc->t2_time - dhcpc->t1_time; + dhcpc->rebind_time = dhcpc->lease_time - dhcpc->t2_time; + pico_dhcp_client_start_reacquisition_timers(dhcpc); + + *(dhcpc->uid) = dhcpc->xid; + if (dhcpc->cb) + dhcpc->cb(dhcpc, PICO_DHCP_SUCCESS); + + dhcpc->state = DHCP_CLIENT_STATE_BOUND; + return 0; +} + +static int renew(struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf) +{ + uint16_t port = PICO_DHCP_CLIENT_PORT; + (void) buf; + dhcpc->state = DHCP_CLIENT_STATE_RENEWING; + dhcpc->s = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_UDP, &pico_dhcp_client_wakeup); + if (!dhcpc->s) { + dhcpc_dbg("DHCP client ERROR: failure opening socket on renew, aborting DHCP! (%s)\n", strerror(pico_err)); + if (dhcpc->cb) + dhcpc->cb(dhcpc, PICO_DHCP_ERROR); + + return -1; + } + + if (pico_socket_bind(dhcpc->s, &dhcpc->address, &port) != 0) { + dhcpc_dbg("DHCP client ERROR: failure binding socket on renew, aborting DHCP! (%s)\n", strerror(pico_err)); + pico_socket_close(dhcpc->s); + dhcpc->s = NULL; + if (dhcpc->cb) + dhcpc->cb(dhcpc, PICO_DHCP_ERROR); + + return -1; + } + + dhcpc->retry = 0; + pico_dhcp_client_msg(dhcpc, PICO_DHCP_MSG_REQUEST); + pico_dhcp_client_start_renewing_timer(dhcpc); + + return 0; +} + +static int rebind(struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf) +{ + (void) buf; + + dhcpc->state = DHCP_CLIENT_STATE_REBINDING; + dhcpc->retry = 0; + pico_dhcp_client_msg(dhcpc, PICO_DHCP_MSG_REQUEST); + pico_dhcp_client_start_rebinding_timer(dhcpc); + + return 0; +} + +static int reset(struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf) +{ + struct pico_ip4 address = { + 0 + }; + (void) buf; + + if (dhcpc->state == DHCP_CLIENT_STATE_REQUESTING) + address.addr = PICO_IP4_ANY; + else + address.addr = dhcpc->address.addr; + + /* close the socket used for address (re)acquisition */ + pico_socket_close(dhcpc->s); + dhcpc->s = NULL; + /* delete the link with the currently in use address */ + pico_ipv4_link_del(dhcpc->dev, address); + + if (dhcpc->cb) + dhcpc->cb(dhcpc, PICO_DHCP_RESET); + + if (dhcpc->state < DHCP_CLIENT_STATE_BOUND) + { + /* pico_dhcp_client_timer_stop(dhcpc, PICO_DHCPC_TIMER_INIT); */ + } + + + dhcpc->state = DHCP_CLIENT_STATE_INIT; + pico_dhcp_client_stop_timers(dhcpc); + pico_dhcp_client_init(dhcpc); + return 0; +} + +static int retransmit(struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf) +{ + (void) buf; + switch (dhcpc->state) + { + case DHCP_CLIENT_STATE_INIT: + pico_dhcp_client_msg(dhcpc, PICO_DHCP_MSG_DISCOVER); + pico_dhcp_client_start_init_timer(dhcpc); + break; + + case DHCP_CLIENT_STATE_REQUESTING: + pico_dhcp_client_msg(dhcpc, PICO_DHCP_MSG_REQUEST); + pico_dhcp_client_start_requesting_timer(dhcpc); + break; + + case DHCP_CLIENT_STATE_RENEWING: + pico_dhcp_client_msg(dhcpc, PICO_DHCP_MSG_REQUEST); + pico_dhcp_client_start_renewing_timer(dhcpc); + break; + + case DHCP_CLIENT_STATE_REBINDING: + pico_dhcp_client_msg(dhcpc, PICO_DHCP_MSG_DISCOVER); + pico_dhcp_client_start_rebinding_timer(dhcpc); + break; + + default: + dhcpc_dbg("DHCP client WARNING: retransmit in incorrect state (%u)!\n", dhcpc->state); + return -1; + } + return 0; +} + +struct dhcp_action_entry { + int (*offer)(struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf); + int (*ack)(struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf); + int (*nak)(struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf); + int (*timer1)(struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf); + int (*timer2)(struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf); + int (*timer_lease)(struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf); + int (*timer_retransmit)(struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf); +}; + +static struct dhcp_action_entry dhcp_fsm[] = +{ /* event |offer |ack |nak |T1 |T2 |lease |retransmit */ +/* state init-reboot */ + { NULL, NULL, NULL, NULL, NULL, NULL, NULL }, +/* state rebooting */ { NULL, NULL, NULL, NULL, NULL, NULL, NULL }, +/* state init */ { recv_offer, NULL, NULL, NULL, NULL, NULL, retransmit }, +/* state selecting */ { NULL, NULL, NULL, NULL, NULL, NULL, NULL }, +/* state requesting */ { NULL, recv_ack, reset, NULL, NULL, NULL, retransmit }, +/* state bound */ { NULL, NULL, NULL, renew, NULL, NULL, NULL }, +/* state renewing */ { NULL, recv_ack, reset, NULL, rebind, NULL, retransmit }, +/* state rebinding */ { NULL, recv_ack, reset, NULL, NULL, reset, retransmit }, +}; + +/* TIMERS REMARK: + * In state bound we have T1, T2 and the lease timer running. If T1 goes off, we attempt to renew. + * If the renew succeeds a new T1, T2 and lease timer is started. The former T2 and lease timer is + * still running though. This poses no concerns as the T2 and lease event in state bound have a NULL + * pointer in the fsm. If the former T2 or lease timer goes off, nothing happens. Same situation + * applies for T2 and a succesfull rebind. */ + +static void pico_dhcp_state_machine(uint8_t event, struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf) +{ + switch (event) + { + case PICO_DHCP_MSG_OFFER: + dhcpc_dbg("DHCP client: received OFFER\n"); + if (dhcp_fsm[dhcpc->state].offer) + dhcp_fsm[dhcpc->state].offer(dhcpc, buf); + + break; + + case PICO_DHCP_MSG_ACK: + dhcpc_dbg("DHCP client: received ACK\n"); + if (dhcp_fsm[dhcpc->state].ack) + dhcp_fsm[dhcpc->state].ack(dhcpc, buf); + + break; + + case PICO_DHCP_MSG_NAK: + dhcpc_dbg("DHCP client: received NAK\n"); + if (dhcp_fsm[dhcpc->state].nak) + dhcp_fsm[dhcpc->state].nak(dhcpc, buf); + + break; + + case PICO_DHCP_EVENT_T1: + dhcpc_dbg("DHCP client: received T1 timeout\n"); + if (dhcp_fsm[dhcpc->state].timer1) + dhcp_fsm[dhcpc->state].timer1(dhcpc, NULL); + + break; + + case PICO_DHCP_EVENT_T2: + dhcpc_dbg("DHCP client: received T2 timeout\n"); + if (dhcp_fsm[dhcpc->state].timer2) + dhcp_fsm[dhcpc->state].timer2(dhcpc, NULL); + + break; + + case PICO_DHCP_EVENT_LEASE: + dhcpc_dbg("DHCP client: received LEASE timeout\n"); + if (dhcp_fsm[dhcpc->state].timer_lease) + dhcp_fsm[dhcpc->state].timer_lease(dhcpc, NULL); + + break; + + case PICO_DHCP_EVENT_RETRANSMIT: + dhcpc_dbg("DHCP client: received RETRANSMIT timeout\n"); + if (dhcp_fsm[dhcpc->state].timer_retransmit) + dhcp_fsm[dhcpc->state].timer_retransmit(dhcpc, NULL); + + break; + + default: + dhcpc_dbg("DHCP client WARNING: unrecognized event (%u)!\n", dhcpc->event); + return; + } + return; +} + +static int16_t pico_dhcp_client_opt_parse(void *ptr, uint16_t len) +{ + uint32_t optlen = len - (uint32_t)sizeof(struct pico_dhcp_hdr); + struct pico_dhcp_hdr *hdr = (struct pico_dhcp_hdr *)ptr; + struct pico_dhcp_opt *opt = DHCP_OPT(hdr,0); + + if (hdr->dhcp_magic != PICO_DHCPD_MAGIC_COOKIE) + return -1; + + if (!pico_dhcp_are_options_valid(opt, (int32_t)optlen)) + return -1; + + do { + if (opt->code == PICO_DHCP_OPT_MSGTYPE) + return opt->ext.msg_type.type; + } while (pico_dhcp_next_option(&opt)); + + return -1; +} + +static int8_t pico_dhcp_client_msg(struct pico_dhcp_client_cookie *dhcpc, uint8_t msg_type) +{ + int32_t r = 0; + uint16_t optlen = 0, offset = 0; + struct pico_ip4 destination = { + .addr = 0xFFFFFFFF + }; + struct pico_dhcp_hdr *hdr = NULL; + + + /* RFC 2131 3.1.3: Request is always BROADCAST */ + + /* Set again default route for the bcast request */ + pico_ipv4_route_set_bcast_link(pico_ipv4_link_by_dev(dhcpc->dev)); + + switch (msg_type) + { + case PICO_DHCP_MSG_DISCOVER: + dhcpc_dbg("DHCP client: sent DHCPDISCOVER\n"); + optlen = PICO_DHCP_OPTLEN_MSGTYPE + PICO_DHCP_OPTLEN_MAXMSGSIZE + PICO_DHCP_OPTLEN_PARAMLIST + PICO_DHCP_OPTLEN_END; + hdr = PICO_ZALLOC((size_t)(sizeof(struct pico_dhcp_hdr) + optlen)); + if (!hdr) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + /* specific options */ + offset = (uint16_t)(offset + pico_dhcp_opt_maxmsgsize(DHCP_OPT(hdr,offset), DHCP_CLIENT_MAXMSGZISE)); + break; + + case PICO_DHCP_MSG_REQUEST: + optlen = PICO_DHCP_OPTLEN_MSGTYPE + PICO_DHCP_OPTLEN_MAXMSGSIZE + PICO_DHCP_OPTLEN_PARAMLIST + PICO_DHCP_OPTLEN_REQIP + PICO_DHCP_OPTLEN_SERVERID + + PICO_DHCP_OPTLEN_END; + hdr = PICO_ZALLOC(sizeof(struct pico_dhcp_hdr) + optlen); + if (!hdr) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + /* specific options */ + offset = (uint16_t)(offset + pico_dhcp_opt_maxmsgsize(DHCP_OPT(hdr,offset), DHCP_CLIENT_MAXMSGZISE)); + if (dhcpc->state == DHCP_CLIENT_STATE_REQUESTING) { + offset = (uint16_t)(offset + pico_dhcp_opt_reqip(DHCP_OPT(hdr,offset), &dhcpc->address)); + offset = (uint16_t)(offset + pico_dhcp_opt_serverid(DHCP_OPT(hdr,offset), &dhcpc->server_id)); + } + + break; + + default: + return -1; + } + + /* common options */ + offset = (uint16_t)(offset + pico_dhcp_opt_msgtype(DHCP_OPT(hdr,offset), msg_type)); + offset = (uint16_t)(offset + pico_dhcp_opt_paramlist(DHCP_OPT(hdr,offset))); + offset = (uint16_t)(offset + pico_dhcp_opt_end(DHCP_OPT(hdr,offset))); + + switch (dhcpc->state) + { + case DHCP_CLIENT_STATE_BOUND: + destination.addr = dhcpc->server_id.addr; + hdr->ciaddr = dhcpc->address.addr; + break; + + case DHCP_CLIENT_STATE_RENEWING: + destination.addr = dhcpc->server_id.addr; + hdr->ciaddr = dhcpc->address.addr; + break; + + case DHCP_CLIENT_STATE_REBINDING: + hdr->ciaddr = dhcpc->address.addr; + break; + + default: + /* do nothing */ + break; + } + + /* header information */ + hdr->op = PICO_DHCP_OP_REQUEST; + hdr->htype = PICO_DHCP_HTYPE_ETH; + hdr->hlen = PICO_SIZE_ETH; + hdr->xid = dhcpc->xid; + /* hdr->flags = short_be(PICO_DHCP_FLAG_BROADCAST); / * Nope: see bug #96! * / */ + hdr->dhcp_magic = PICO_DHCPD_MAGIC_COOKIE; + /* copy client hardware address */ + memcpy(hdr->hwaddr, &dhcpc->dev->eth->mac, PICO_SIZE_ETH); + + if (destination.addr == PICO_IP4_BCAST) + pico_ipv4_route_set_bcast_link(pico_ipv4_link_get(&dhcpc->address)); + + r = pico_socket_sendto(dhcpc->s, hdr, (int)(sizeof(struct pico_dhcp_hdr) + optlen), &destination, PICO_DHCPD_PORT); + PICO_FREE(hdr); + if (r < 0) + return -1; + + return 0; +} + +static void pico_dhcp_client_wakeup(uint16_t ev, struct pico_socket *s) +{ + + uint8_t *buf; + int r = 0; + struct pico_dhcp_hdr *hdr = NULL; + struct pico_dhcp_client_cookie *dhcpc = NULL; + + if ((ev & PICO_SOCK_EV_RD) == 0) + return; + + buf = PICO_ZALLOC(DHCP_CLIENT_MAXMSGZISE); + if (!buf) { + return; + } + + r = pico_socket_recvfrom(s, buf, DHCP_CLIENT_MAXMSGZISE, NULL, NULL); + if (r < 0) + goto out_discard_buf; + + /* If the 'xid' of an arriving message does not match the 'xid' + * of the most recent transmitted message, the message must be + * silently discarded. */ + hdr = (struct pico_dhcp_hdr *)buf; + dhcpc = pico_dhcp_client_find_cookie(hdr->xid); + if (!dhcpc) + goto out_discard_buf; + + dhcpc->event = (uint8_t)pico_dhcp_client_opt_parse(buf, (uint16_t)r); + pico_dhcp_state_machine(dhcpc->event, dhcpc, buf); + +out_discard_buf: + PICO_FREE(buf); +} + +void *pico_dhcp_get_identifier(uint32_t xid) +{ + return (void *)pico_dhcp_client_find_cookie(xid); +} + +struct pico_ip4 pico_dhcp_get_address(void*dhcpc) +{ + return ((struct pico_dhcp_client_cookie*)dhcpc)->address; +} + +struct pico_ip4 pico_dhcp_get_gateway(void*dhcpc) +{ + return ((struct pico_dhcp_client_cookie*)dhcpc)->gateway; +} + +struct pico_ip4 pico_dhcp_get_netmask(void *dhcpc) +{ + return ((struct pico_dhcp_client_cookie*)dhcpc)->netmask; +} + +struct pico_ip4 pico_dhcp_get_nameserver(void*dhcpc, int index) +{ + struct pico_ip4 fault = { + .addr = 0xFFFFFFFFU + }; + if ((index != 0) && (index != 1)) + return fault; + + return ((struct pico_dhcp_client_cookie*)dhcpc)->nameserver[index]; +} + +int pico_dhcp_client_abort(uint32_t xid) +{ + return pico_dhcp_client_del_cookie(xid); +} + + +char *pico_dhcp_get_hostname(void) +{ + return dhcpc_host_name; +} + +char *pico_dhcp_get_domain(void) +{ + return dhcpc_domain_name; +} + +#endif diff --git a/ext/picotcp/modules/pico_dhcp_client.h b/ext/picotcp/modules/pico_dhcp_client.h new file mode 100644 index 0000000..4dafb2a --- /dev/null +++ b/ext/picotcp/modules/pico_dhcp_client.h @@ -0,0 +1,32 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + *********************************************************************/ +#ifndef INCLUDE_PICO_DHCP_CLIENT +#define INCLUDE_PICO_DHCP_CLIENT +#include "pico_defines.h" +#ifdef PICO_SUPPORT_UDP +#include "pico_dhcp_common.h" +#include "pico_addressing.h" +#include "pico_protocol.h" + +int pico_dhcp_initiate_negotiation(struct pico_device *device, void (*callback)(void*cli, int code), uint32_t *xid); +void *pico_dhcp_get_identifier(uint32_t xid); +struct pico_ip4 pico_dhcp_get_address(void *cli); +struct pico_ip4 pico_dhcp_get_gateway(void *cli); +struct pico_ip4 pico_dhcp_get_netmask(void *cli); +struct pico_ip4 pico_dhcp_get_nameserver(void*cli, int index); +int pico_dhcp_client_abort(uint32_t xid); +char *pico_dhcp_get_hostname(void); +char *pico_dhcp_get_domain(void); + +/* possible codes for the callback */ +#define PICO_DHCP_SUCCESS 0 +#define PICO_DHCP_ERROR 1 +#define PICO_DHCP_RESET 2 + +#endif +#endif diff --git a/ext/picotcp/modules/pico_dhcp_common.c b/ext/picotcp/modules/pico_dhcp_common.c new file mode 100644 index 0000000..7edb1e2 --- /dev/null +++ b/ext/picotcp/modules/pico_dhcp_common.c @@ -0,0 +1,190 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + Authors: Frederik Van Slycken + *********************************************************************/ + +#include "pico_config.h" +#include "pico_stack.h" +#include "pico_dhcp_common.h" + +#if defined (PICO_SUPPORT_DHCPC) || defined (PICO_SUPPORT_DHCPD) +/* pico_dhcp_are_options_valid needs to be called first to prevent illegal memory access */ +/* The argument pointer is moved forward to the next option */ +struct pico_dhcp_opt *pico_dhcp_next_option(struct pico_dhcp_opt **ptr) +{ + uint8_t **p = (uint8_t **)ptr; + struct pico_dhcp_opt *opt = *ptr; + + if (opt->code == PICO_DHCP_OPT_END) + return NULL; + + if (opt->code == PICO_DHCP_OPT_PAD) { + *p += 1; + return *ptr; + } + + *p += (opt->len + 2); /* (len + 2) to account for code and len octet */ + return *ptr; +} + +uint8_t pico_dhcp_are_options_valid(void *ptr, int32_t len) +{ + uint8_t optlen = 0, *p = ptr; + + while (len > 0) { + switch (*p) + { + case PICO_DHCP_OPT_END: + return 1; + + case PICO_DHCP_OPT_PAD: + p++; + len--; + break; + + default: + p++; /* move pointer from code octet to len octet */ + len--; + if ((len <= 0) || (len - (*p + 1) < 0)) /* (*p + 1) to account for len octet */ + return 0; + + optlen = *p; + p += optlen + 1; + len -= optlen; + break; + } + } + return 0; +} + +uint8_t pico_dhcp_opt_netmask(void *ptr, struct pico_ip4 *ip) +{ + struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)ptr; + + /* option: netmask */ + opt->code = PICO_DHCP_OPT_NETMASK; + opt->len = PICO_DHCP_OPTLEN_NETMASK - PICO_DHCP_OPTLEN_HDR; + opt->ext.netmask.ip = *ip; + return PICO_DHCP_OPTLEN_NETMASK; +} + +uint8_t pico_dhcp_opt_router(void *ptr, struct pico_ip4 *ip) +{ + struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)ptr; + + /* option: router */ + opt->code = PICO_DHCP_OPT_ROUTER; + opt->len = PICO_DHCP_OPTLEN_ROUTER - PICO_DHCP_OPTLEN_HDR; + opt->ext.router.ip = *ip; + return PICO_DHCP_OPTLEN_ROUTER; +} + +uint8_t pico_dhcp_opt_dns(void *ptr, struct pico_ip4 *ip) +{ + struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)ptr; + + /* option: dns */ + opt->code = PICO_DHCP_OPT_DNS; + opt->len = PICO_DHCP_OPTLEN_DNS - PICO_DHCP_OPTLEN_HDR; + opt->ext.dns1.ip = *ip; + return PICO_DHCP_OPTLEN_DNS; +} + +uint8_t pico_dhcp_opt_broadcast(void *ptr, struct pico_ip4 *ip) +{ + struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)ptr; + + /* option: broadcast */ + opt->code = PICO_DHCP_OPT_BROADCAST; + opt->len = PICO_DHCP_OPTLEN_BROADCAST - PICO_DHCP_OPTLEN_HDR; + opt->ext.broadcast.ip = *ip; + return PICO_DHCP_OPTLEN_BROADCAST; +} + +uint8_t pico_dhcp_opt_reqip(void *ptr, struct pico_ip4 *ip) +{ + struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)ptr; + + /* option: request IP address */ + opt->code = PICO_DHCP_OPT_REQIP; + opt->len = PICO_DHCP_OPTLEN_REQIP - PICO_DHCP_OPTLEN_HDR; + opt->ext.req_ip.ip = *ip; + return PICO_DHCP_OPTLEN_REQIP; +} + +uint8_t pico_dhcp_opt_leasetime(void *ptr, uint32_t time) +{ + struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)ptr; + + /* option: lease time */ + opt->code = PICO_DHCP_OPT_LEASETIME; + opt->len = PICO_DHCP_OPTLEN_LEASETIME - PICO_DHCP_OPTLEN_HDR; + opt->ext.lease_time.time = time; + return PICO_DHCP_OPTLEN_LEASETIME; +} + +uint8_t pico_dhcp_opt_msgtype(void *ptr, uint8_t type) +{ + struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)ptr; + + /* option: message type */ + opt->code = PICO_DHCP_OPT_MSGTYPE; + opt->len = PICO_DHCP_OPTLEN_MSGTYPE - PICO_DHCP_OPTLEN_HDR; + opt->ext.msg_type.type = type; + return PICO_DHCP_OPTLEN_MSGTYPE; +} + +uint8_t pico_dhcp_opt_serverid(void *ptr, struct pico_ip4 *ip) +{ + struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)ptr; + + /* option: server identifier */ + opt->code = PICO_DHCP_OPT_SERVERID; + opt->len = PICO_DHCP_OPTLEN_SERVERID - PICO_DHCP_OPTLEN_HDR; + opt->ext.server_id.ip = *ip; + return PICO_DHCP_OPTLEN_SERVERID; +} + +uint8_t pico_dhcp_opt_paramlist(void *ptr) +{ + struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)ptr; + uint8_t *param_code = &(opt->ext.param_list.code[0]); + + /* option: parameter list */ + opt->code = PICO_DHCP_OPT_PARAMLIST; + opt->len = PICO_DHCP_OPTLEN_PARAMLIST - PICO_DHCP_OPTLEN_HDR; + param_code[0] = PICO_DHCP_OPT_NETMASK; + param_code[1] = PICO_DHCP_OPT_TIME; + param_code[2] = PICO_DHCP_OPT_ROUTER; + param_code[3] = PICO_DHCP_OPT_HOSTNAME; + param_code[4] = PICO_DHCP_OPT_RENEWALTIME; + param_code[5] = PICO_DHCP_OPT_REBINDINGTIME; + param_code[6] = PICO_DHCP_OPT_DNS; + return PICO_DHCP_OPTLEN_PARAMLIST; +} + +uint8_t pico_dhcp_opt_maxmsgsize(void *ptr, uint16_t size) +{ + struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)ptr; + + /* option: maximum message size */ + opt->code = PICO_DHCP_OPT_MAXMSGSIZE; + opt->len = PICO_DHCP_OPTLEN_MAXMSGSIZE - PICO_DHCP_OPTLEN_HDR; + opt->ext.max_msg_size.size = short_be(size); + return PICO_DHCP_OPTLEN_MAXMSGSIZE; +} + +uint8_t pico_dhcp_opt_end(void *ptr) +{ + uint8_t *opt = (uint8_t *)ptr; + + /* option: end of options */ + *opt = PICO_DHCP_OPT_END; + return PICO_DHCP_OPTLEN_END; +} + +#endif diff --git a/ext/picotcp/modules/pico_dhcp_common.h b/ext/picotcp/modules/pico_dhcp_common.h new file mode 100644 index 0000000..62c94da --- /dev/null +++ b/ext/picotcp/modules/pico_dhcp_common.h @@ -0,0 +1,191 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + *********************************************************************/ +#ifndef INCLUDE_PICO_DHCP_COMMON +#define INCLUDE_PICO_DHCP_COMMON +#include "pico_config.h" +#include "pico_addressing.h" + +#define PICO_DHCPD_PORT (short_be(67)) +#define PICO_DHCP_CLIENT_PORT (short_be(68)) +#define PICO_DHCPD_MAGIC_COOKIE (long_be(0x63825363)) +#define PICO_DHCP_HTYPE_ETH 1 + +/* Macro to get DHCP option field */ +#define DHCP_OPT(hdr,off) ((struct pico_dhcp_opt *)(((uint8_t *)hdr)+sizeof(struct pico_dhcp_hdr) + off)) + +/* flags */ +#define PICO_DHCP_FLAG_BROADCAST 0x8000 + +/* options */ +#define PICO_DHCP_OPT_PAD 0x00 +#define PICO_DHCP_OPT_NETMASK 0x01 +#define PICO_DHCP_OPT_TIME 0x02 +#define PICO_DHCP_OPT_ROUTER 0x03 +#define PICO_DHCP_OPT_DNS 0x06 +#define PICO_DHCP_OPT_HOSTNAME 0x0c +#define PICO_DHCP_OPT_DOMAINNAME 0x0f +#define PICO_DHCP_OPT_MTU 0x1a +#define PICO_DHCP_OPT_BROADCAST 0x1c +#define PICO_DHCP_OPT_NETBIOSNS 0x2c +#define PICO_DHCP_OPT_NETBIOSSCOPE 0x2f +#define PICO_DHCP_OPT_REQIP 0x32 +#define PICO_DHCP_OPT_LEASETIME 0x33 +#define PICO_DHCP_OPT_OPTOVERLOAD 0x34 +#define PICO_DHCP_OPT_MSGTYPE 0x35 +#define PICO_DHCP_OPT_SERVERID 0x36 +#define PICO_DHCP_OPT_PARAMLIST 0x37 +#define PICO_DHCP_OPT_MESSAGE 0x38 +#define PICO_DHCP_OPT_MAXMSGSIZE 0x39 +#define PICO_DHCP_OPT_RENEWALTIME 0x3a +#define PICO_DHCP_OPT_REBINDINGTIME 0x3b +#define PICO_DHCP_OPT_VENDORID 0x3c +#define PICO_DHCP_OPT_CLIENTID 0x3d +#define PICO_DHCP_OPT_DOMAINSEARCH 0x77 +#define PICO_DHCP_OPT_STATICROUTE 0x79 +#define PICO_DHCP_OPT_END 0xFF + +/* options len */ +#define PICO_DHCP_OPTLEN_HDR 2 /* account for code and len field */ +#define PICO_DHCP_OPTLEN_NETMASK 6 +#define PICO_DHCP_OPTLEN_ROUTER 6 +#define PICO_DHCP_OPTLEN_DNS 6 +#define PICO_DHCP_OPTLEN_BROADCAST 6 +#define PICO_DHCP_OPTLEN_REQIP 6 +#define PICO_DHCP_OPTLEN_LEASETIME 6 +#define PICO_DHCP_OPTLEN_OPTOVERLOAD 3 +#define PICO_DHCP_OPTLEN_MSGTYPE 3 +#define PICO_DHCP_OPTLEN_SERVERID 6 +#define PICO_DHCP_OPTLEN_PARAMLIST 9 /* PicoTCP specific */ +#define PICO_DHCP_OPTLEN_MAXMSGSIZE 4 +#define PICO_DHCP_OPTLEN_RENEWALTIME 6 +#define PICO_DHCP_OPTLEN_REBINDINGTIME 6 +#define PICO_DHCP_OPTLEN_END 1 + +/* op codes */ +#define PICO_DHCP_OP_REQUEST 1 +#define PICO_DHCP_OP_REPLY 2 + +/* rfc message types */ +#define PICO_DHCP_MSG_DISCOVER 1 +#define PICO_DHCP_MSG_OFFER 2 +#define PICO_DHCP_MSG_REQUEST 3 +#define PICO_DHCP_MSG_DECLINE 4 +#define PICO_DHCP_MSG_ACK 5 +#define PICO_DHCP_MSG_NAK 6 +#define PICO_DHCP_MSG_RELEASE 7 +#define PICO_DHCP_MSG_INFORM 8 + +/* custom message types */ +#define PICO_DHCP_EVENT_T1 9 +#define PICO_DHCP_EVENT_T2 10 +#define PICO_DHCP_EVENT_LEASE 11 +#define PICO_DHCP_EVENT_RETRANSMIT 12 +#define PICO_DHCP_EVENT_NONE 0xff + +PACKED_STRUCT_DEF pico_dhcp_hdr +{ + uint8_t op; + uint8_t htype; + uint8_t hlen; + uint8_t hops; /* zero */ + uint32_t xid; /* store this in the request */ + uint16_t secs; /* ignore */ + uint16_t flags; + uint32_t ciaddr; /* client address - if asking for renewal */ + uint32_t yiaddr; /* your address (client) */ + uint32_t siaddr; /* dhcp offered address */ + uint32_t giaddr; /* relay agent, bootp. */ + uint8_t hwaddr[6]; + uint8_t hwaddr_padding[10]; + char hostname[64]; + char bootp_filename[128]; + uint32_t dhcp_magic; +}; + +PACKED_STRUCT_DEF pico_dhcp_opt +{ + uint8_t code; + uint8_t len; + PACKED_UNION_DEF dhcp_opt_ext_u { + PEDANTIC_STRUCT_DEF netmask_s { + struct pico_ip4 ip; + } netmask; + PEDANTIC_STRUCT_DEF router_s { + struct pico_ip4 ip; + } router; + PEDANTIC_STRUCT_DEF dns_s { + struct pico_ip4 ip; + } dns1; + struct dns_s dns2; + PEDANTIC_STRUCT_DEF broadcast_s { + struct pico_ip4 ip; + } broadcast; + PEDANTIC_STRUCT_DEF req_ip_s { + struct pico_ip4 ip; + } req_ip; + PEDANTIC_STRUCT_DEF lease_time_s { + uint32_t time; + } lease_time; + PEDANTIC_STRUCT_DEF opt_overload_s { + uint8_t value; + } opt_overload; + PEDANTIC_STRUCT_DEF tftp_server_s { + char name[1]; + } tftp_server; + PEDANTIC_STRUCT_DEF bootfile_s { + char name[1]; + } bootfile; + PEDANTIC_STRUCT_DEF msg_type_s { + uint8_t type; + } msg_type; + PEDANTIC_STRUCT_DEF server_id_s { + struct pico_ip4 ip; + } server_id; + PEDANTIC_STRUCT_DEF param_list_s { + uint8_t code[1]; + } param_list; + PEDANTIC_STRUCT_DEF message_s { + char error[1]; + } message; + PEDANTIC_STRUCT_DEF max_msg_size_s { + uint16_t size; + } max_msg_size; + PEDANTIC_STRUCT_DEF renewal_time_s { + uint32_t time; + } renewal_time; + PEDANTIC_STRUCT_DEF rebinding_time_s { + uint32_t time; + } rebinding_time; + PEDANTIC_STRUCT_DEF vendor_id_s { + uint8_t id[1]; + } vendor_id; + PEDANTIC_STRUCT_DEF client_id_s { + uint8_t id[1]; + } client_id; + PEDANTIC_STRUCT_DEF text_s { + char txt[1]; + } string; + } ext; +}; + +uint8_t dhcp_get_next_option(uint8_t *begin, uint8_t *data, int *len, uint8_t **nextopt); +struct pico_dhcp_opt *pico_dhcp_next_option(struct pico_dhcp_opt **ptr); +uint8_t pico_dhcp_are_options_valid(void *ptr, int32_t len); + +uint8_t pico_dhcp_opt_netmask(void *ptr, struct pico_ip4 *ip); +uint8_t pico_dhcp_opt_router(void *ptr, struct pico_ip4 *ip); +uint8_t pico_dhcp_opt_dns(void *ptr, struct pico_ip4 *ip); +uint8_t pico_dhcp_opt_broadcast(void *ptr, struct pico_ip4 *ip); +uint8_t pico_dhcp_opt_reqip(void *ptr, struct pico_ip4 *ip); +uint8_t pico_dhcp_opt_leasetime(void *ptr, uint32_t time); +uint8_t pico_dhcp_opt_msgtype(void *ptr, uint8_t type); +uint8_t pico_dhcp_opt_serverid(void *ptr, struct pico_ip4 *ip); +uint8_t pico_dhcp_opt_paramlist(void *ptr); +uint8_t pico_dhcp_opt_maxmsgsize(void *ptr, uint16_t size); +uint8_t pico_dhcp_opt_end(void *ptr); +#endif diff --git a/ext/picotcp/modules/pico_dhcp_server.c b/ext/picotcp/modules/pico_dhcp_server.c new file mode 100644 index 0000000..dbef3bb --- /dev/null +++ b/ext/picotcp/modules/pico_dhcp_server.c @@ -0,0 +1,411 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + + Authors: Frederik Van Slycken, Kristof Roelants + *********************************************************************/ + +#include "pico_dhcp_server.h" +#include "pico_config.h" +#include "pico_addressing.h" +#include "pico_socket.h" +#include "pico_udp.h" +#include "pico_stack.h" +#include "pico_arp.h" + +#if (defined PICO_SUPPORT_DHCPD && defined PICO_SUPPORT_UDP) + +#define dhcps_dbg(...) do {} while(0) +/* #define dhcps_dbg dbg */ + +/* default configurations */ +#define DHCP_SERVER_OPENDNS long_be(0xd043dede) /* OpenDNS DNS server 208.67.222.222 */ +#define DHCP_SERVER_POOL_START long_be(0x00000064) +#define DHCP_SERVER_POOL_END long_be(0x000000fe) +#define DHCP_SERVER_LEASE_TIME long_be(0x00000078) + +/* maximum size of a DHCP message */ +#define DHCP_SERVER_MAXMSGSIZE (PICO_IP_MRU - sizeof(struct pico_ipv4_hdr) - sizeof(struct pico_udp_hdr)) + +enum dhcp_server_state { + PICO_DHCP_STATE_DISCOVER = 0, + PICO_DHCP_STATE_OFFER, + PICO_DHCP_STATE_REQUEST, + PICO_DHCP_STATE_BOUND, + PICO_DHCP_STATE_RENEWING +}; + +struct pico_dhcp_server_negotiation { + uint32_t xid; + enum dhcp_server_state state; + struct pico_dhcp_server_setting *dhcps; + struct pico_ip4 ciaddr; + struct pico_eth hwaddr; + uint8_t bcast; +}; + +static inline int ip_address_is_in_dhcp_range(struct pico_dhcp_server_negotiation *n, uint32_t x) +{ + uint32_t ip_hostendian = long_be(x); + if (ip_hostendian < long_be(n->dhcps->pool_start)) + return 0; + + if (ip_hostendian > long_be(n->dhcps->pool_end)) + return 0; + + return 1; +} + + +static void pico_dhcpd_wakeup(uint16_t ev, struct pico_socket *s); + +static int dhcp_settings_cmp(void *ka, void *kb) +{ + struct pico_dhcp_server_setting *a = ka, *b = kb; + if (a->dev == b->dev) + return 0; + + return (a->dev < b->dev) ? (-1) : (1); +} +PICO_TREE_DECLARE(DHCPSettings, dhcp_settings_cmp); + +static int dhcp_negotiations_cmp(void *ka, void *kb) +{ + struct pico_dhcp_server_negotiation *a = ka, *b = kb; + if (a->xid == b->xid) + return 0; + + return (a->xid < b->xid) ? (-1) : (1); +} +PICO_TREE_DECLARE(DHCPNegotiations, dhcp_negotiations_cmp); + + +static inline void dhcps_set_default_pool_start_if_not_provided(struct pico_dhcp_server_setting *dhcps) +{ + if (!dhcps->pool_start) + dhcps->pool_start = (dhcps->server_ip.addr & dhcps->netmask.addr) | DHCP_SERVER_POOL_START; +} + +static inline void dhcps_set_default_pool_end_if_not_provided(struct pico_dhcp_server_setting *dhcps) +{ + if (!dhcps->pool_end) + dhcps->pool_end = (dhcps->server_ip.addr & dhcps->netmask.addr) | DHCP_SERVER_POOL_END; +} +static inline void dhcps_set_default_lease_time_if_not_provided(struct pico_dhcp_server_setting *dhcps) +{ + if (!dhcps->lease_time) + dhcps->lease_time = DHCP_SERVER_LEASE_TIME; +} + +static inline struct pico_dhcp_server_setting *dhcps_try_open_socket(struct pico_dhcp_server_setting *dhcps) +{ + uint16_t port = PICO_DHCPD_PORT; + dhcps->s = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_UDP, &pico_dhcpd_wakeup); + if (!dhcps->s) { + dhcps_dbg("DHCP server ERROR: failure opening socket (%s)\n", strerror(pico_err)); + PICO_FREE(dhcps); + return NULL; + } + + if (pico_socket_bind(dhcps->s, &dhcps->server_ip, &port) < 0) { + dhcps_dbg("DHCP server ERROR: failure binding socket (%s)\n", strerror(pico_err)); + PICO_FREE(dhcps); + return NULL; + } + + pico_tree_insert(&DHCPSettings, dhcps); + return dhcps; +} + +static struct pico_dhcp_server_setting *pico_dhcp_server_add_setting(struct pico_dhcp_server_setting *setting) +{ + struct pico_dhcp_server_setting *dhcps = NULL, *found = NULL, test = { + 0 + }; + struct pico_ipv4_link *link = NULL; + + link = pico_ipv4_link_get(&setting->server_ip); + if (!link) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + test.dev = setting->dev; + found = pico_tree_findKey(&DHCPSettings, &test); + if (found) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + dhcps = PICO_ZALLOC(sizeof(struct pico_dhcp_server_setting)); + if (!dhcps) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + dhcps->lease_time = setting->lease_time; + dhcps->pool_start = setting->pool_start; + dhcps->pool_next = setting->pool_next; + dhcps->pool_end = setting->pool_end; + dhcps->dev = link->dev; + dhcps->server_ip = link->address; + dhcps->netmask = link->netmask; + + /* default values if not provided */ + dhcps_set_default_lease_time_if_not_provided(dhcps); + dhcps_set_default_pool_end_if_not_provided(dhcps); + dhcps_set_default_pool_start_if_not_provided(dhcps); + + dhcps->pool_next = dhcps->pool_start; + + return dhcps_try_open_socket(dhcps); + +} + +static struct pico_dhcp_server_negotiation *pico_dhcp_server_find_negotiation(uint32_t xid) +{ + struct pico_dhcp_server_negotiation test = { + 0 + }, *found = NULL; + + test.xid = xid; + found = pico_tree_findKey(&DHCPNegotiations, &test); + if (found) + return found; + else + return NULL; +} + +static inline void dhcp_negotiation_set_ciaddr(struct pico_dhcp_server_negotiation *dhcpn) +{ + struct pico_ip4 *ciaddr = NULL; + ciaddr = pico_arp_reverse_lookup(&dhcpn->hwaddr); + if (!ciaddr) { + dhcpn->ciaddr.addr = dhcpn->dhcps->pool_next; + dhcpn->dhcps->pool_next = long_be(long_be(dhcpn->dhcps->pool_next) + 1); + pico_arp_create_entry(dhcpn->hwaddr.addr, dhcpn->ciaddr, dhcpn->dhcps->dev); + } else { + dhcpn->ciaddr = *ciaddr; + } +} + +static struct pico_dhcp_server_negotiation *pico_dhcp_server_add_negotiation(struct pico_device *dev, struct pico_dhcp_hdr *hdr) +{ + struct pico_dhcp_server_negotiation *dhcpn = NULL; + struct pico_dhcp_server_setting test = { + 0 + }; + + if (pico_dhcp_server_find_negotiation(hdr->xid)) + return NULL; + + dhcpn = PICO_ZALLOC(sizeof(struct pico_dhcp_server_negotiation)); + if (!dhcpn) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + dhcpn->xid = hdr->xid; + dhcpn->state = PICO_DHCP_STATE_DISCOVER; + dhcpn->bcast = ((short_be(hdr->flags) & PICO_DHCP_FLAG_BROADCAST) != 0) ? (1) : (0); + memcpy(dhcpn->hwaddr.addr, hdr->hwaddr, PICO_SIZE_ETH); + + test.dev = dev; + dhcpn->dhcps = pico_tree_findKey(&DHCPSettings, &test); + if (!dhcpn->dhcps) { + dhcps_dbg("DHCP server WARNING: received DHCP message on unconfigured link %s\n", dev->name); + PICO_FREE(dhcpn); + return NULL; + } + + dhcp_negotiation_set_ciaddr(dhcpn); + pico_tree_insert(&DHCPNegotiations, dhcpn); + return dhcpn; +} + +static void dhcpd_make_reply(struct pico_dhcp_server_negotiation *dhcpn, uint8_t msg_type) +{ + int r = 0, optlen = 0, offset = 0; + struct pico_ip4 broadcast = { + 0 + }, dns = { + 0 + }, destination = { + .addr = 0xFFFFFFFF + }; + struct pico_dhcp_hdr *hdr = NULL; + + dns.addr = DHCP_SERVER_OPENDNS; + broadcast.addr = dhcpn->dhcps->server_ip.addr | ~(dhcpn->dhcps->netmask.addr); + + optlen = PICO_DHCP_OPTLEN_MSGTYPE + PICO_DHCP_OPTLEN_SERVERID + PICO_DHCP_OPTLEN_LEASETIME + PICO_DHCP_OPTLEN_NETMASK + PICO_DHCP_OPTLEN_ROUTER + + PICO_DHCP_OPTLEN_BROADCAST + PICO_DHCP_OPTLEN_DNS + PICO_DHCP_OPTLEN_END; + hdr = PICO_ZALLOC(sizeof(struct pico_dhcp_hdr) + (uint32_t)optlen); + if (!hdr) { + return; + } + + hdr->op = PICO_DHCP_OP_REPLY; + hdr->htype = PICO_DHCP_HTYPE_ETH; + hdr->hlen = PICO_SIZE_ETH; + hdr->xid = dhcpn->xid; + hdr->yiaddr = dhcpn->ciaddr.addr; + hdr->siaddr = dhcpn->dhcps->server_ip.addr; + hdr->dhcp_magic = PICO_DHCPD_MAGIC_COOKIE; + memcpy(hdr->hwaddr, dhcpn->hwaddr.addr, PICO_SIZE_ETH); + + /* options */ + offset += pico_dhcp_opt_msgtype(DHCP_OPT(hdr,offset), msg_type); + offset += pico_dhcp_opt_serverid(DHCP_OPT(hdr,offset), &dhcpn->dhcps->server_ip); + offset += pico_dhcp_opt_leasetime(DHCP_OPT(hdr,offset), dhcpn->dhcps->lease_time); + offset += pico_dhcp_opt_netmask(DHCP_OPT(hdr,offset), &dhcpn->dhcps->netmask); + offset += pico_dhcp_opt_router(DHCP_OPT(hdr,offset), &dhcpn->dhcps->server_ip); + offset += pico_dhcp_opt_broadcast(DHCP_OPT(hdr,offset), &broadcast); + offset += pico_dhcp_opt_dns(DHCP_OPT(hdr,offset), &dns); + offset += pico_dhcp_opt_end(DHCP_OPT(hdr,offset)); + + if (dhcpn->bcast == 0) + destination.addr = hdr->yiaddr; + else { + hdr->flags |= short_be(PICO_DHCP_FLAG_BROADCAST); + destination.addr = broadcast.addr; + } + + r = pico_socket_sendto(dhcpn->dhcps->s, hdr, (int)(sizeof(struct pico_dhcp_hdr) + (uint32_t)optlen), &destination, PICO_DHCP_CLIENT_PORT); + if (r < 0) + dhcps_dbg("DHCP server WARNING: failure sending: %s!\n", strerror(pico_err)); + + PICO_FREE(hdr); + + return; +} + +static inline void parse_opt_msgtype(struct pico_dhcp_opt *opt, uint8_t *msgtype) +{ + if (opt->code == PICO_DHCP_OPT_MSGTYPE) { + *msgtype = opt->ext.msg_type.type; + dhcps_dbg("DHCP server: message type %u\n", msgtype); + } +} + +static inline void parse_opt_reqip(struct pico_dhcp_opt *opt, struct pico_ip4 *reqip) +{ + if (opt->code == PICO_DHCP_OPT_REQIP) + reqip->addr = opt->ext.req_ip.ip.addr; +} + +static inline void parse_opt_serverid(struct pico_dhcp_opt *opt, struct pico_ip4 *serverid) +{ + if (opt->code == PICO_DHCP_OPT_SERVERID) + *serverid = opt->ext.server_id.ip; +} + +static inline void dhcps_make_reply_to_request_msg(struct pico_dhcp_server_negotiation *dhcpn, int bound_valid_flag) +{ + if ((dhcpn->state == PICO_DHCP_STATE_BOUND) && bound_valid_flag) + dhcpd_make_reply(dhcpn, PICO_DHCP_MSG_ACK); + + if (dhcpn->state == PICO_DHCP_STATE_OFFER) { + dhcpn->state = PICO_DHCP_STATE_BOUND; + dhcpd_make_reply(dhcpn, PICO_DHCP_MSG_ACK); + } +} + +static inline void dhcps_make_reply_to_discover_or_request(struct pico_dhcp_server_negotiation *dhcpn, uint8_t msgtype, int bound_valid_flag) +{ + if (PICO_DHCP_MSG_DISCOVER == msgtype) { + dhcpd_make_reply(dhcpn, PICO_DHCP_MSG_OFFER); + dhcpn->state = PICO_DHCP_STATE_OFFER; + } else if (PICO_DHCP_MSG_REQUEST == msgtype) { + dhcps_make_reply_to_request_msg(dhcpn, bound_valid_flag); + } +} + +static inline void dhcps_parse_options_loop(struct pico_dhcp_server_negotiation *dhcpn, struct pico_dhcp_hdr *hdr) +{ + struct pico_dhcp_opt *opt = DHCP_OPT(hdr,0); + uint8_t msgtype = 0; + struct pico_ip4 reqip = { + 0 + }, server_id = { + 0 + }; + + do { + parse_opt_msgtype(opt, &msgtype); + parse_opt_reqip(opt, &reqip); + parse_opt_serverid(opt, &server_id); + } while (pico_dhcp_next_option(&opt)); + dhcps_make_reply_to_discover_or_request(dhcpn, msgtype, (!reqip.addr) && (!server_id.addr) && (hdr->ciaddr == dhcpn->ciaddr.addr)); +} + +static void pico_dhcp_server_recv(struct pico_socket *s, uint8_t *buf, uint32_t len) +{ + int32_t optlen = (int32_t)(len - sizeof(struct pico_dhcp_hdr)); + struct pico_dhcp_hdr *hdr = (struct pico_dhcp_hdr *)buf; + struct pico_dhcp_server_negotiation *dhcpn = NULL; + struct pico_device *dev = NULL; + + if (!pico_dhcp_are_options_valid(DHCP_OPT(hdr,0), optlen)) + return; + + dev = pico_ipv4_link_find(&s->local_addr.ip4); + dhcpn = pico_dhcp_server_find_negotiation(hdr->xid); + if (!dhcpn) + dhcpn = pico_dhcp_server_add_negotiation(dev, hdr); + + if (!ip_address_is_in_dhcp_range(dhcpn, dhcpn->ciaddr.addr)) + return; + + dhcps_parse_options_loop(dhcpn, hdr); + +} + +static void pico_dhcpd_wakeup(uint16_t ev, struct pico_socket *s) +{ + uint8_t buf[DHCP_SERVER_MAXMSGSIZE] = { + 0 + }; + int r = 0; + + if (ev != PICO_SOCK_EV_RD) + return; + + r = pico_socket_recvfrom(s, buf, DHCP_SERVER_MAXMSGSIZE, NULL, NULL); + if (r < 0) + return; + + pico_dhcp_server_recv(s, buf, (uint32_t)r); + return; +} + +int pico_dhcp_server_initiate(struct pico_dhcp_server_setting *setting) +{ + if (!setting || !setting->server_ip.addr) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + if (pico_dhcp_server_add_setting(setting) == NULL) + return -1; + + return 0; +} + +int pico_dhcp_server_destroy(struct pico_device *dev) +{ + struct pico_dhcp_server_setting *found, test = { 0 }; + test.dev = dev; + found = pico_tree_findKey(&DHCPSettings, &test); + if (!found) { + pico_err = PICO_ERR_ENOENT; + return -1; + } + + pico_tree_delete(&DHCPSettings, found); + PICO_FREE(found); + return 0; +} + +#endif /* PICO_SUPPORT_DHCP */ diff --git a/ext/picotcp/modules/pico_dhcp_server.h b/ext/picotcp/modules/pico_dhcp_server.h new file mode 100644 index 0000000..d5df820 --- /dev/null +++ b/ext/picotcp/modules/pico_dhcp_server.h @@ -0,0 +1,34 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#ifndef INCLUDE_PICO_DHCP_SERVER +#define INCLUDE_PICO_DHCP_SERVER +#include "pico_defines.h" +#ifdef PICO_SUPPORT_UDP + +#include "pico_dhcp_common.h" +#include "pico_addressing.h" + +struct pico_dhcp_server_setting +{ + uint32_t pool_start; + uint32_t pool_next; + uint32_t pool_end; + uint32_t lease_time; + struct pico_device *dev; + struct pico_socket *s; + struct pico_ip4 server_ip; + struct pico_ip4 netmask; + uint8_t flags; /* unused atm */ +}; + +/* required field: IP address of the interface to serve, only IPs of this network will be served. */ +int pico_dhcp_server_initiate(struct pico_dhcp_server_setting *dhcps); + +/* To destroy an existing DHCP server configuration, running on a given interface */ +int pico_dhcp_server_destroy(struct pico_device *dev); + +#endif /* _INCLUDE_PICO_DHCP_SERVER */ +#endif diff --git a/ext/picotcp/modules/pico_dns_client.c b/ext/picotcp/modules/pico_dns_client.c new file mode 100644 index 0000000..806cff5 --- /dev/null +++ b/ext/picotcp/modules/pico_dns_client.c @@ -0,0 +1,808 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved. + See LICENSE and COPYING for usage. + . + Authors: Kristof Roelants + *********************************************************************/ +#include "pico_config.h" +#include "pico_stack.h" +#include "pico_addressing.h" +#include "pico_socket.h" +#include "pico_ipv4.h" +#include "pico_ipv6.h" +#include "pico_dns_client.h" +#include "pico_dns_common.h" +#include "pico_tree.h" + +#ifdef PICO_SUPPORT_DNS_CLIENT + +#ifdef PICO_SUPPORT_IPV4 + +#define dns_dbg(...) do {} while(0) +/* #define dns_dbg dbg */ + +/* DNS response length */ +#define PICO_DNS_MAX_QUERY_LEN 255 +#define PICO_DNS_MAX_QUERY_LABEL_LEN 63 + +/* DNS client retransmission time (msec) + frequency */ +#define PICO_DNS_CLIENT_RETRANS 4000 +#define PICO_DNS_CLIENT_MAX_RETRANS 3 + +static void pico_dns_client_callback(uint16_t ev, struct pico_socket *s); +static void pico_dns_client_retransmission(pico_time now, void *arg); +static int pico_dns_client_getaddr_init(const char *url, uint16_t proto, void (*callback)(char *, void *), void *arg); + +struct pico_dns_ns +{ + struct pico_ip4 ns; /* nameserver */ +}; + +static int dns_ns_cmp(void *ka, void *kb) +{ + struct pico_dns_ns *a = ka, *b = kb; + return pico_ipv4_compare(&a->ns, &b->ns); +} +PICO_TREE_DECLARE(NSTable, dns_ns_cmp); + +struct pico_dns_query +{ + char *query; + uint16_t len; + uint16_t id; + uint16_t qtype; + uint16_t qclass; + uint8_t retrans; + struct pico_dns_ns q_ns; + struct pico_socket *s; + void (*callback)(char *, void *); + void *arg; +}; + +static int dns_query_cmp(void *ka, void *kb) +{ + struct pico_dns_query *a = ka, *b = kb; + if (a->id == b->id) + return 0; + + return (a->id < b->id) ? (-1) : (1); +} +PICO_TREE_DECLARE(DNSTable, dns_query_cmp); + +static int pico_dns_client_del_ns(struct pico_ip4 *ns_addr) +{ + struct pico_dns_ns test = {{0}}, *found = NULL; + + test.ns = *ns_addr; + found = pico_tree_findKey(&NSTable, &test); + if (!found) + return -1; + + pico_tree_delete(&NSTable, found); + PICO_FREE(found); + + /* no NS left, add default NS */ + if (pico_tree_empty(&NSTable)) + pico_dns_client_init(); + + return 0; +} + +static struct pico_dns_ns *pico_dns_client_add_ns(struct pico_ip4 *ns_addr) +{ + struct pico_dns_ns *dns = NULL, *found = NULL, test = {{0}}; + struct pico_ip4 zero = {0}; /* 0.0.0.0 */ + + /* Do not add 0.0.0.0 addresses, which some DHCP servers might reply */ + if (!pico_ipv4_compare(ns_addr, &zero)) + { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + dns = PICO_ZALLOC(sizeof(struct pico_dns_ns)); + if (!dns) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + dns->ns = *ns_addr; + + found = pico_tree_insert(&NSTable, dns); + if (found) { /* nameserver already present */ + PICO_FREE(dns); + return found; + } + + /* default NS found, remove it */ + pico_string_to_ipv4(PICO_DNS_NS_DEFAULT, (uint32_t *)&test.ns.addr); + found = pico_tree_findKey(&NSTable, &test); + if (found && (found->ns.addr != ns_addr->addr)) + pico_dns_client_del_ns(&found->ns); + + return dns; +} + +static struct pico_dns_ns pico_dns_client_next_ns(struct pico_ip4 *ns_addr) +{ + struct pico_dns_ns dns = {{0}}, *nxtdns = NULL; + struct pico_tree_node *node = NULL, *nxtnode = NULL; + + dns.ns = *ns_addr; + node = pico_tree_findNode(&NSTable, &dns); + if (!node) + return dns; /* keep using current NS */ + + nxtnode = pico_tree_next(node); + nxtdns = nxtnode->keyValue; + if (!nxtdns) + nxtdns = (struct pico_dns_ns *)pico_tree_first(&NSTable); + + return *nxtdns; +} + +static struct pico_dns_query *pico_dns_client_add_query(struct pico_dns_header *hdr, uint16_t len, struct pico_dns_question_suffix *suffix, + void (*callback)(char *, void *), void *arg) +{ + struct pico_dns_query *q = NULL, *found = NULL; + + q = PICO_ZALLOC(sizeof(struct pico_dns_query)); + if (!q) + return NULL; + + q->query = (char *)hdr; + q->len = len; + q->id = short_be(hdr->id); + q->qtype = short_be(suffix->qtype); + q->qclass = short_be(suffix->qclass); + q->retrans = 1; + q->q_ns = *((struct pico_dns_ns *)pico_tree_first(&NSTable)); + q->callback = callback; + q->arg = arg; + q->s = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_UDP, &pico_dns_client_callback); + if (!q->s) { + PICO_FREE(q); + return NULL; + } + + found = pico_tree_insert(&DNSTable, q); + if (found) { + pico_err = PICO_ERR_EAGAIN; + pico_socket_close(q->s); + PICO_FREE(q); + return NULL; + } + + return q; +} + +static int pico_dns_client_del_query(uint16_t id) +{ + struct pico_dns_query test = { + 0 + }, *found = NULL; + + test.id = id; + found = pico_tree_findKey(&DNSTable, &test); + if (!found) + return -1; + + PICO_FREE(found->query); + pico_socket_close(found->s); + pico_tree_delete(&DNSTable, found); + PICO_FREE(found); + return 0; +} + +static struct pico_dns_query *pico_dns_client_find_query(uint16_t id) +{ + struct pico_dns_query test = { + 0 + }, *found = NULL; + + test.id = id; + found = pico_tree_findKey(&DNSTable, &test); + if (found) + return found; + else + return NULL; +} + +/* seek end of string */ +static char *pico_dns_client_seek(char *ptr) +{ + if (!ptr) + return NULL; + + while (*ptr != 0) + ptr++; + return ptr + 1; +} + +static struct pico_dns_query *pico_dns_client_idcheck(uint16_t id) +{ + struct pico_dns_query test = { + 0 + }; + + test.id = id; + return pico_tree_findKey(&DNSTable, &test); +} + +static int pico_dns_client_query_header(struct pico_dns_header *hdr) +{ + uint16_t id = 0; + uint8_t retry = 32; + + do { + id = (uint16_t)(pico_rand() & 0xFFFFU); + dns_dbg("DNS: generated id %u\n", id); + } while (retry-- && pico_dns_client_idcheck(id)); + if (!retry) + return -1; + + hdr->id = short_be(id); + pico_dns_fill_packet_header(hdr, 1, 0, 0, 0); /* 1 question, 0 answers */ + + return 0; +} + +static int pico_dns_client_check_header(struct pico_dns_header *pre) +{ + if (pre->qr != PICO_DNS_QR_RESPONSE || pre->opcode != PICO_DNS_OPCODE_QUERY || pre->rcode != PICO_DNS_RCODE_NO_ERROR) { + dns_dbg("DNS ERROR: OPCODE %d | TC %d | RCODE %d\n", pre->opcode, pre->tc, pre->rcode); + return -1; + } + + if (short_be(pre->ancount) < 1) { + dns_dbg("DNS ERROR: ancount < 1\n"); + return -1; + } + + return 0; +} + +static int pico_dns_client_check_qsuffix(struct pico_dns_question_suffix *suf, struct pico_dns_query *q) +{ + if (!suf) + return -1; + + if (short_be(suf->qtype) != q->qtype || short_be(suf->qclass) != q->qclass) { + dns_dbg("DNS ERROR: received qtype (%u) or qclass (%u) incorrect\n", short_be(suf->qtype), short_be(suf->qclass)); + return -1; + } + + return 0; +} + +static int pico_dns_client_check_url(struct pico_dns_header *resp, struct pico_dns_query *q) +{ + char *recv_name = (char*)(resp) + sizeof(struct pico_dns_header) + PICO_DNS_LABEL_INITIAL; + char *exp_name = (char *)(q->query) + sizeof(struct pico_dns_header) + PICO_DNS_LABEL_INITIAL; + if (strcasecmp(recv_name, exp_name) != 0) + return -1; + + return 0; +} + +static int pico_dns_client_check_asuffix(struct pico_dns_record_suffix *suf, struct pico_dns_query *q) +{ + if (!suf) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + if (short_be(suf->rtype) != q->qtype || short_be(suf->rclass) != q->qclass) { + dns_dbg("DNS WARNING: received qtype (%u) or qclass (%u) incorrect\n", short_be(suf->rtype), short_be(suf->rclass)); + return -1; + } + + if (long_be(suf->rttl) > PICO_DNS_MAX_TTL) { + dns_dbg("DNS WARNING: received TTL (%u) > MAX (%u)\n", short_be(suf->rttl), PICO_DNS_MAX_TTL); + return -1; + } + + return 0; +} + +static char *pico_dns_client_seek_suffix(char *suf, struct pico_dns_header *pre, struct pico_dns_query *q) +{ + struct pico_dns_record_suffix *asuffix = NULL; + uint16_t comp = 0, compression = 0; + uint16_t i = 0; + + if (!suf) + return NULL; + + while (i++ < short_be(pre->ancount)) { + comp = short_from(suf); + compression = short_be(comp); + switch (compression >> 14) + { + case PICO_DNS_POINTER: + while (compression >> 14 == PICO_DNS_POINTER) { + dns_dbg("DNS: pointer\n"); + suf += sizeof(uint16_t); + comp = short_from(suf); + compression = short_be(comp); + } + break; + + case PICO_DNS_LABEL: + dns_dbg("DNS: label\n"); + suf = pico_dns_client_seek(suf); + break; + + default: + dns_dbg("DNS ERROR: incorrect compression (%u) value\n", compression); + return NULL; + } + + asuffix = (struct pico_dns_record_suffix *)suf; + if (!asuffix) + break; + + if (pico_dns_client_check_asuffix(asuffix, q) < 0) { + suf += (sizeof(struct pico_dns_record_suffix) + short_be(asuffix->rdlength)); + continue; + } + + return suf; + } + return NULL; +} + +static int pico_dns_client_send(struct pico_dns_query *q) +{ + uint16_t *paramID = PICO_ZALLOC(sizeof(uint16_t)); + if (!paramID) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + dns_dbg("DNS: sending query to %08X\n", q->q_ns.ns.addr); + if (!q->s) + goto failure; + + if (pico_socket_connect(q->s, &q->q_ns.ns, short_be(PICO_DNS_NS_PORT)) < 0) + goto failure; + + pico_socket_send(q->s, q->query, q->len); + *paramID = q->id; + pico_timer_add(PICO_DNS_CLIENT_RETRANS, pico_dns_client_retransmission, paramID); + + return 0; + +failure: + PICO_FREE(paramID); + return -1; +} + +static void pico_dns_client_retransmission(pico_time now, void *arg) +{ + struct pico_dns_query *q = NULL; + struct pico_dns_query dummy; + IGNORE_PARAMETER(now); + + if(!arg) + return; + + /* search for the dns query and free used space */ + dummy.id = *(uint16_t *)arg; + q = (struct pico_dns_query *)pico_tree_findKey(&DNSTable, &dummy); + PICO_FREE(arg); + + /* dns query successful? */ + if (!q) { + return; + } + + q->retrans++; + if (q->retrans <= PICO_DNS_CLIENT_MAX_RETRANS) { + q->q_ns = pico_dns_client_next_ns(&q->q_ns.ns); + pico_dns_client_send(q); + } else { + pico_err = PICO_ERR_EIO; + q->callback(NULL, q->arg); + pico_dns_client_del_query(q->id); + } +} + +static int pico_dns_client_user_callback(struct pico_dns_record_suffix *asuffix, struct pico_dns_query *q) +{ + uint32_t ip = 0; + char *str = NULL; + char *rdata = (char *) asuffix + sizeof(struct pico_dns_record_suffix); + + switch (q->qtype) + { + case PICO_DNS_TYPE_A: + ip = long_from(rdata); + str = PICO_ZALLOC(PICO_DNS_IPV4_ADDR_LEN); + pico_ipv4_to_string(str, ip); + break; +#ifdef PICO_SUPPORT_IPV6 + case PICO_DNS_TYPE_AAAA: + { + struct pico_ip6 ip6; + memcpy(&ip6.addr, rdata, sizeof(struct pico_ip6)); + str = PICO_ZALLOC(PICO_DNS_IPV6_ADDR_LEN); + pico_ipv6_to_string(str, ip6.addr); + break; + } +#endif + case PICO_DNS_TYPE_PTR: + /* TODO: check for decompression / rdlength vs. decompressed length */ + pico_dns_notation_to_name(rdata, short_be(asuffix->rdlength)); + str = PICO_ZALLOC((size_t)(short_be(asuffix->rdlength) - + PICO_DNS_LABEL_INITIAL)); + if (!str) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + memcpy(str, rdata + PICO_DNS_LABEL_INITIAL, short_be(asuffix->rdlength) - PICO_DNS_LABEL_INITIAL); + break; + + default: + dns_dbg("DNS ERROR: incorrect qtype (%u)\n", q->qtype); + break; + } + + if (q->retrans) { + q->callback(str, q->arg); + q->retrans = 0; + pico_dns_client_del_query(q->id); + } + + if (str) + PICO_FREE(str); + + return 0; +} + +static char dns_response[PICO_IP_MRU] = { + 0 +}; + +static void pico_dns_try_fallback_cname(struct pico_dns_query *q, struct pico_dns_header *h, struct pico_dns_question_suffix *qsuffix) +{ + uint16_t type = q->qtype; + uint16_t proto = PICO_PROTO_IPV4; + struct pico_dns_record_suffix *asuffix = NULL; + char *p_asuffix = NULL; + char *cname_orig = NULL; + char *cname = NULL; + uint16_t cname_len; + + /* Try to use CNAME only if A or AAAA query is ongoing */ + if (type != PICO_DNS_TYPE_A && type != PICO_DNS_TYPE_AAAA) + return; + + if (type == PICO_DNS_TYPE_AAAA) + proto = PICO_PROTO_IPV6; + + q->qtype = PICO_DNS_TYPE_CNAME; + p_asuffix = (char *)qsuffix + sizeof(struct pico_dns_question_suffix); + p_asuffix = pico_dns_client_seek_suffix(p_asuffix, h, q); + if (!p_asuffix) { + return; + } + + /* Found CNAME response. Re-initiating query. */ + asuffix = (struct pico_dns_record_suffix *)p_asuffix; + cname = pico_dns_decompress_name((char *)asuffix + sizeof(struct pico_dns_record_suffix), (pico_dns_packet *)h); /* allocates memory! */ + cname_orig = cname; /* to free later */ + + if (cname == NULL) + return; + + cname_len = (uint16_t)(pico_dns_strlen(cname) + 1); + + pico_dns_notation_to_name(cname, cname_len); + if (cname[0] == '.') + cname++; + + dns_dbg("Restarting query for name '%s'\n", cname); + pico_dns_client_getaddr_init(cname, proto, q->callback, q->arg); + PICO_FREE(cname_orig); + pico_dns_client_del_query(q->id); +} + +static void pico_dns_client_callback(uint16_t ev, struct pico_socket *s) +{ + struct pico_dns_header *header = NULL; + char *domain; + struct pico_dns_question_suffix *qsuffix = NULL; + struct pico_dns_record_suffix *asuffix = NULL; + struct pico_dns_query *q = NULL; + char *p_asuffix = NULL; + + if (ev == PICO_SOCK_EV_ERR) { + dns_dbg("DNS: socket error received\n"); + return; + } + + if (ev & PICO_SOCK_EV_RD) { + if (pico_socket_read(s, dns_response, PICO_IP_MRU) < 0) + return; + } + + header = (struct pico_dns_header *)dns_response; + domain = (char *)header + sizeof(struct pico_dns_header); + qsuffix = (struct pico_dns_question_suffix *)pico_dns_client_seek(domain); + /* valid asuffix is determined dynamically later on */ + + if (pico_dns_client_check_header(header) < 0) + return; + + q = pico_dns_client_find_query(short_be(header->id)); + if (!q) + return; + + if (pico_dns_client_check_qsuffix(qsuffix, q) < 0) + return; + + if (pico_dns_client_check_url(header, q) < 0) + return; + + p_asuffix = (char *)qsuffix + sizeof(struct pico_dns_question_suffix); + p_asuffix = pico_dns_client_seek_suffix(p_asuffix, header, q); + if (!p_asuffix) { + pico_dns_try_fallback_cname(q, header, qsuffix); + return; + } + + asuffix = (struct pico_dns_record_suffix *)p_asuffix; + pico_dns_client_user_callback(asuffix, q); + + return; +} + +static int pico_dns_create_message(struct pico_dns_header **header, struct pico_dns_question_suffix **qsuffix, enum pico_dns_arpa arpa, const char *url, uint16_t *urlen, uint16_t *hdrlen) +{ + char *domain; + char inaddr_arpa[14]; + uint16_t strlen = 0, arpalen = 0; + + if (!url) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + if(arpa == PICO_DNS_ARPA4) { + strcpy(inaddr_arpa, ".in-addr.arpa"); + strlen = pico_dns_strlen(url); + } + +#ifdef PICO_SUPPORT_IPV6 + else if (arpa == PICO_DNS_ARPA6) { + strcpy(inaddr_arpa, ".IP6.ARPA"); + strlen = STRLEN_PTR_IP6; + } +#endif + else { + strcpy(inaddr_arpa, ""); + strlen = pico_dns_strlen(url); + } + + arpalen = pico_dns_strlen(inaddr_arpa); + *urlen = (uint16_t)(PICO_DNS_LABEL_INITIAL + strlen + arpalen + PICO_DNS_LABEL_ROOT); + *hdrlen = (uint16_t)(sizeof(struct pico_dns_header) + *urlen + sizeof(struct pico_dns_question_suffix)); + *header = PICO_ZALLOC(*hdrlen); + if (!*header) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + *header = (struct pico_dns_header *)*header; + domain = (char *) *header + sizeof(struct pico_dns_header); + *qsuffix = (struct pico_dns_question_suffix *)(domain + *urlen); + + if(arpa == PICO_DNS_ARPA4) { + memcpy(domain + PICO_DNS_LABEL_INITIAL, url, strlen); + pico_dns_mirror_addr(domain + PICO_DNS_LABEL_INITIAL); + memcpy(domain + PICO_DNS_LABEL_INITIAL + strlen, inaddr_arpa, arpalen); + } + +#ifdef PICO_SUPPORT_IPV6 + else if (arpa == PICO_DNS_ARPA6) { + pico_dns_ipv6_set_ptr(url, domain + PICO_DNS_LABEL_INITIAL); + memcpy(domain + PICO_DNS_LABEL_INITIAL + STRLEN_PTR_IP6, inaddr_arpa, arpalen); + } +#endif + else { + memcpy(domain + PICO_DNS_LABEL_INITIAL, url, strlen); + } + + /* assemble dns message */ + pico_dns_client_query_header(*header); + pico_dns_name_to_dns_notation(domain, strlen); + + return 0; +} + +static int pico_dns_client_addr_label_check_len(const char *url) +{ + const char *p, *label; + int count; + label = url; + p = label; + + while(*p != (char) 0) { + count = 0; + while((*p != (char)0)) { + if (*p == '.') { + label = ++p; + break; + } + + count++; + p++; + if (count > PICO_DNS_MAX_QUERY_LABEL_LEN) + return -1; + } + } + return 0; +} + +static int pico_dns_client_getaddr_check(const char *url, void (*callback)(char *, void *)) +{ + if (!url || !callback) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + if (strlen(url) > PICO_DNS_MAX_QUERY_LEN) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + if (pico_dns_client_addr_label_check_len(url) < 0) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + return 0; +} + +static int pico_dns_client_getaddr_init(const char *url, uint16_t proto, void (*callback)(char *, void *), void *arg) +{ + struct pico_dns_header *header = NULL; + struct pico_dns_question_suffix *qsuffix = NULL; + struct pico_dns_query *q = NULL; + uint16_t len = 0, lblen = 0; + (void)proto; + + if (pico_dns_client_getaddr_check(url, callback) < 0) + return -1; + + if(pico_dns_create_message(&header, &qsuffix, PICO_DNS_NO_ARPA, url, &lblen, &len) != 0) + return -1; + +#ifdef PICO_SUPPORT_IPV6 + if (proto == PICO_PROTO_IPV6) { + pico_dns_question_fill_suffix(qsuffix, PICO_DNS_TYPE_AAAA, PICO_DNS_CLASS_IN); + } else +#endif + pico_dns_question_fill_suffix(qsuffix, PICO_DNS_TYPE_A, PICO_DNS_CLASS_IN); + + q = pico_dns_client_add_query(header, len, qsuffix, callback, arg); + if (!q) { + PICO_FREE(header); + return -1; + } + + if (pico_dns_client_send(q) < 0) { + pico_dns_client_del_query(q->id); /* frees msg */ + return -1; + } + + return 0; +} + +int pico_dns_client_getaddr(const char *url, void (*callback)(char *, void *), void *arg) +{ + return pico_dns_client_getaddr_init(url, PICO_PROTO_IPV4, callback, arg); +} + +int pico_dns_client_getaddr6(const char *url, void (*callback)(char *, void *), void *arg) +{ + return pico_dns_client_getaddr_init(url, PICO_PROTO_IPV6, callback, arg); +} + +static int pico_dns_getname_univ(const char *ip, void (*callback)(char *, void *), void *arg, enum pico_dns_arpa arpa) +{ + struct pico_dns_header *header = NULL; + struct pico_dns_question_suffix *qsuffix = NULL; + struct pico_dns_query *q = NULL; + uint16_t len = 0, lblen = 0; + + if (!ip || !callback) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + if(pico_dns_create_message(&header, &qsuffix, arpa, ip, &lblen, &len) != 0) + return -1; + + pico_dns_question_fill_suffix(qsuffix, PICO_DNS_TYPE_PTR, PICO_DNS_CLASS_IN); + q = pico_dns_client_add_query(header, len, qsuffix, callback, arg); + if (!q) { + PICO_FREE(header); + return -1; + } + + if (pico_dns_client_send(q) < 0) { + pico_dns_client_del_query(q->id); /* frees header */ + return -1; + } + + return 0; +} + +int pico_dns_client_getname(const char *ip, void (*callback)(char *, void *), void *arg) +{ + return pico_dns_getname_univ(ip, callback, arg, PICO_DNS_ARPA4); +} + + +int pico_dns_client_getname6(const char *ip, void (*callback)(char *, void *), void *arg) +{ + return pico_dns_getname_univ(ip, callback, arg, PICO_DNS_ARPA6); +} + +int pico_dns_client_nameserver(struct pico_ip4 *ns, uint8_t flag) +{ + if (!ns) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + switch (flag) + { + case PICO_DNS_NS_ADD: + if (!pico_dns_client_add_ns(ns)) + return -1; + + break; + + case PICO_DNS_NS_DEL: + if (pico_dns_client_del_ns(ns) < 0) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + break; + + default: + pico_err = PICO_ERR_EINVAL; + return -1; + } + return 0; +} + +int pico_dns_client_init(void) +{ + struct pico_ip4 default_ns = { + 0 + }; + + if (pico_string_to_ipv4(PICO_DNS_NS_DEFAULT, (uint32_t *)&default_ns.addr) < 0) + return -1; + + return pico_dns_client_nameserver(&default_ns, PICO_DNS_NS_ADD); +} + +#else + +int pico_dns_client_init(void) +{ + dbg("ERROR Trying to initialize DNS module: IPv4 not supported in this build.\n"); + return -1; +} +#endif /* PICO_SUPPORT_IPV4 */ + + +#endif /* PICO_SUPPORT_DNS_CLIENT */ + diff --git a/ext/picotcp/modules/pico_dns_client.h b/ext/picotcp/modules/pico_dns_client.h new file mode 100644 index 0000000..910cc92 --- /dev/null +++ b/ext/picotcp/modules/pico_dns_client.h @@ -0,0 +1,46 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + Authors: Kristof Roelants + *********************************************************************/ + +#ifndef INCLUDE_PICO_DNS_CLIENT +#define INCLUDE_PICO_DNS_CLIENT + +#define PICO_DNS_NS_DEL 0 +#define PICO_DNS_NS_ADD 1 +#include "pico_config.h" + +/* Compression values */ +#define PICO_DNS_LABEL 0 +#define PICO_DNS_POINTER 3 + +/* Label len */ +#define PICO_DNS_LABEL_INITIAL 1u +#define PICO_DNS_LABEL_ROOT 1 + +/* TTL values */ +#define PICO_DNS_MAX_TTL 604800 /* one week */ + +/* Len of an IPv4 address string */ +#define PICO_DNS_IPV4_ADDR_LEN 16 +#define PICO_DNS_IPV6_ADDR_LEN 54 + +/* Default nameservers + port */ +#define PICO_DNS_NS_DEFAULT "208.67.222.222" +#define PICO_DNS_NS_PORT 53 + +int pico_dns_client_init(void); +/* flag is PICO_DNS_NS_DEL or PICO_DNS_NS_ADD */ +int pico_dns_client_nameserver(struct pico_ip4 *ns, uint8_t flag); +int pico_dns_client_getaddr(const char *url, void (*callback)(char *ip, void *arg), void *arg); +int pico_dns_client_getname(const char *ip, void (*callback)(char *url, void *arg), void *arg); +#ifdef PICO_SUPPORT_IPV6 +int pico_dns_client_getaddr6(const char *url, void (*callback)(char *, void *), void *arg); +int pico_dns_client_getname6(const char *url, void (*callback)(char *, void *), void *arg); +#endif + +#endif /* _INCLUDE_PICO_DNS_CLIENT */ diff --git a/ext/picotcp/modules/pico_dns_common.c b/ext/picotcp/modules/pico_dns_common.c new file mode 100644 index 0000000..0c0250e --- /dev/null +++ b/ext/picotcp/modules/pico_dns_common.c @@ -0,0 +1,1773 @@ +/* **************************************************************************** + * PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved. + * See LICENSE and COPYING for usage. + * . + * Authors: Toon Stegen, Jelle De Vleeschouwer + * ****************************************************************************/ +#include "pico_config.h" +#include "pico_protocol.h" +#include "pico_stack.h" +#include "pico_addressing.h" +#include "pico_ipv4.h" +#include "pico_ipv6.h" +#include "pico_dns_common.h" +#include "pico_tree.h" + +#define dns_dbg(...) do {} while(0) +//#define dns_dbg dbg + +/* MARK: v NAME & IP FUNCTIONS */ +#define dns_name_foreach_label_safe(label, name, next, maxlen) \ + for ((label) = (name), (next) = (char *)((name) + *(name) + 1); \ + (*(label) != '\0') && ((uint16_t)((label) - (name)) < (maxlen)); \ + (label) = (next), (next) = (char *)((next) + *(next) + 1)) + +/* **************************************************************************** + * Checks if the DNS name doesn't exceed 256 bytes including zero-byte. + * + * @param namelen Length of the DNS name-string including zero-byte + * @return 0 when the length is correct + * ****************************************************************************/ +int +pico_dns_check_namelen( uint16_t namelen ) +{ + return ((namelen > 2u) && (namelen < 256u)) ? (0) : (-1); +} + +/* **************************************************************************** + * Returns the length of a name in a DNS-packet as if DNS name compression + * would be applied to the packet. If there's no compression present + * + * @param name Compressed name you want the calculate the strlen from + * @return Returns strlen of a compressed name, takes the first byte of compr- + * ession pointer into account but not the second byte, which acts + * like a trailing zero-byte + * ****************************************************************************/ +uint16_t +pico_dns_namelen_comp( char *name ) +{ + uint16_t len = 0; + char *label = NULL, *next = NULL; + + /* Check params */ + if (!name) { + pico_err = PICO_ERR_EINVAL; + return 0; + } + + /* Just count until the zero-byte or a pointer */ + dns_name_foreach_label_safe(label, name, next, 255) { + if ((0xC0 & *label)) + break; + } + + /* Calculate the length */ + len = (uint16_t)(label - name); + if(*label != '\0') + len++; + + return len; +} + +/* **************************************************************************** + * Returns the uncompressed name in DNS name format when DNS name compression + * is applied to the packet-buffer. + * + * @param name Compressed name, should be in the bounds of the actual packet + * @param packet Packet that contains the compressed name + * @return Returns the decompressed name, NULL on failure. + * ****************************************************************************/ +char * +pico_dns_decompress_name( char *name, pico_dns_packet *packet ) +{ + char decompressed_name[PICO_DNS_NAMEBUF_SIZE] = { + 0 + }; + char *return_name = NULL; + uint8_t *dest_iterator = NULL; + uint8_t *iterator = NULL; + uint16_t ptr = 0, nslen = 0; + + /* Initialise iterators */ + iterator = (uint8_t *) name; + dest_iterator = (uint8_t *) decompressed_name; + while (*iterator != '\0') { + if ((*iterator) & 0xC0) { + /* We have a pointer */ + ptr = (uint16_t)((((uint16_t) *iterator) & 0x003F) << 8); + ptr = (uint16_t)(ptr | (uint16_t) *(iterator + 1)); + iterator = (uint8_t *)((uint8_t *)packet + ptr); + } else { + /* We want to keep the label lengths */ + *dest_iterator = (uint8_t) *iterator; + /* Copy the label */ + memcpy(dest_iterator + 1, iterator + 1, *iterator); + /* Move to next length label */ + dest_iterator += (*iterator) + 1; + iterator += (*iterator) + 1; + } + } + /* Append final zero-byte */ + *dest_iterator = (uint8_t) '\0'; + + /* Provide storage for the name to return */ + nslen = (uint16_t)(pico_dns_strlen(decompressed_name) + 1); + if(!(return_name = PICO_ZALLOC((size_t)nslen))) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + memcpy((void *)return_name, (void *)decompressed_name, (size_t)nslen); + + return return_name; +} + +/* **************************************************************************** + * Determines the length of a given url as if it where a DNS name in reverse + * resolution format. + * + * @param url URL wanted to create a reverse resolution name from. + * @param arpalen Will get filled with the length of the ARPA-suffix depending + * on the proto-parameter. + * @param proto The protocol to create a ARPA-suffix for. Can be either + * 'PICO_PROTO_IPV4' or 'PICO_PROTO_IPV6' + * @return Returns the length of the reverse name + * ****************************************************************************/ +static uint16_t +pico_dns_url_get_reverse_len( const char *url, + uint16_t *arpalen, + uint16_t proto ) +{ + uint16_t slen = (uint16_t)(pico_dns_strlen(url) + 2u); + + /* Check if pointers given are not NULL */ + if (pico_dns_check_namelen(slen) && !arpalen) { + pico_err = PICO_ERR_EINVAL; + return 0; + } + + /* Get the length of arpa-suffix if needed */ + if (proto == PICO_PROTO_IPV4) + *arpalen = (uint16_t) pico_dns_strlen(PICO_ARPA_IPV4_SUFFIX); + +#ifdef PICO_SUPPORT_IPV6 + else if (proto == PICO_PROTO_IPV6) + { + *arpalen = (uint16_t) pico_dns_strlen(PICO_ARPA_IPV6_SUFFIX); + slen = STRLEN_PTR_IP6 + 2u; + } +#endif + return slen; +} + +/* **************************************************************************** + * Converts a DNS name in URL format to a reverse name in DNS name format. + * Provides space for the DNS name as well. PICO_FREE() should be called on the + * returned string buffer that contains the reverse DNS name. + * + * @param url DNS name in URL format to convert to reverse name + * @param proto Depending on the protocol given the ARPA-suffix will be added. + * @return Returns a pointer to a string-buffer with the reverse DNS name. + * ****************************************************************************/ +static char * +pico_dns_url_to_reverse_qname( const char *url, uint8_t proto ) +{ + char *reverse_qname = NULL; + uint16_t arpalen = 0; + uint16_t slen = pico_dns_url_get_reverse_len(url, &arpalen, proto); + + /* Check namelen */ + if (pico_dns_check_namelen(slen)) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + /* Provide space for the reverse name */ + if (!(reverse_qname = PICO_ZALLOC((size_t)(slen + arpalen)))) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + /* If reverse IPv4 address resolving, convert to IPv4 arpa-format */ + if (PICO_PROTO_IPV4 == proto) { + memcpy(reverse_qname + 1u, url, slen - 1u); + pico_dns_mirror_addr(reverse_qname + 1u); + memcpy(reverse_qname + slen - 1, PICO_ARPA_IPV4_SUFFIX, arpalen); + } + + /* If reverse IPv6 address resolving, convert to IPv6 arpa-format */ +#ifdef PICO_SUPPORT_IPV6 + else if (proto == PICO_PROTO_IPV6) { + pico_dns_ipv6_set_ptr(url, reverse_qname + 1u); + memcpy(reverse_qname + 1u + STRLEN_PTR_IP6, + PICO_ARPA_IPV6_SUFFIX, arpalen); + } +#endif + else { /* This shouldn't happen */ + PICO_FREE(reverse_qname); + return NULL; + } + + pico_dns_name_to_dns_notation(reverse_qname, (unsigned int)(slen + arpalen)); + return reverse_qname; +} + +/* **************************************************************************** + * Converts a DNS name in DNS name format to a name in URL format. Provides + * space for the name in URL format as well. PICO_FREE() should be called on + * the returned string buffer that contains the name in URL format. + * + * @param qname DNS name in DNS name format to convert + * @return Returns a pointer to a string-buffer with the URL name on success. + * ****************************************************************************/ +char * +pico_dns_qname_to_url( const char *qname ) +{ + char *url = NULL; + char temp[256] = { + 0 + }; + uint16_t namelen = pico_dns_strlen(qname); + + /* Check if qname is not a NULL-pointer and if the length is OK */ + if (pico_dns_check_namelen(namelen)) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + /* Provide space for the URL */ + if (!(url = PICO_ZALLOC(namelen))) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + /* Convert qname to an URL */ + memcpy(temp, qname, namelen); + pico_dns_notation_to_name(temp, namelen); + memcpy((void *)url, (void *)(temp + 1), (size_t)(namelen - 1)); + + return url; +} + +/* **************************************************************************** + * Converts a DNS name in URL format to a name in DNS name format. Provides + * space for the DNS name as well. PICO_FREE() should be called on the returned + * string buffer that contains the DNS name. + * + * @param url DNS name in URL format to convert + * @return Returns a pointer to a string-buffer with the DNS name on success. + * ****************************************************************************/ +char * +pico_dns_url_to_qname( const char *url ) +{ + char *qname = NULL; + uint16_t namelen = (uint16_t)(pico_dns_strlen(url) + 2u); + + /* Check if url or qname_addr is not a NULL-pointer */ + if (pico_dns_check_namelen(namelen)) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + /* Provide space for the qname */ + if (!(qname = PICO_ZALLOC(namelen))) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + /* Copy in the URL (+1 to leave space for leading '.') */ + memcpy(qname + 1, url, (size_t)(namelen - 1)); + pico_dns_name_to_dns_notation(qname, namelen); + return qname; +} + +/* **************************************************************************** + * @param url String-buffer + * @return Length of string-buffer in an uint16_t + * ****************************************************************************/ +uint16_t +pico_dns_strlen( const char *url ) +{ + if (!url) + return 0; + + return (uint16_t) strlen(url); +} + +/* **************************************************************************** + * Replaces .'s in a DNS name in URL format by the label lengths. So it + * actually converts a name in URL format to a name in DNS name format. + * f.e. "*www.google.be" => "3www6google2be0" + * + * @param url Location to buffer with name in URL format. The URL needs to + * be +1 byte offset in the actual buffer. Size is should be + * pico_dns_strlen(url) + 2. + * @param maxlen Maximum length of buffer so it doesn't cause a buffer overflow + * @return 0 on success, something else on failure. + * ****************************************************************************/ +int pico_dns_name_to_dns_notation( char *url, unsigned int maxlen ) +{ + char c = '\0'; + char *lbl = url, *i = url; + + /* Check params */ + if (!url || pico_dns_check_namelen((uint16_t)maxlen)) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* Iterate over url */ + while ((c = *++i) != '\0') { + if ('.' == c) { + *lbl = (char)(i - lbl - 1); + lbl = i; + } + + if ((uint16_t)(i - url) > (uint16_t)maxlen) break; + } + *lbl = (char)(i - lbl - 1); + + return 0; +} + +/* **************************************************************************** + * Replaces the label lengths in a DNS-name by .'s. So it actually converts a + * name in DNS format to a name in URL format. + * f.e. 3www6google2be0 => .www.google.be + * + * @param ptr Location to buffer with name in DNS name format + * @param maxlen Maximum length of buffer so it doesn't cause a buffer overflow + * @return 0 on success, something else on failure. + * ****************************************************************************/ +int pico_dns_notation_to_name( char *ptr, unsigned int maxlen ) +{ + char *label = NULL, *next = NULL; + + /* Iterate safely over the labels and update each label */ + dns_name_foreach_label_safe(label, ptr, next, (uint16_t)maxlen) { + *label = '.'; + } + + return 0; +} + +/* **************************************************************************** + * Determines the length of the first label of a DNS name in URL-format + * + * @param url DNS name in URL-format + * @return Length of the first label of DNS name in URL-format + * ****************************************************************************/ +uint16_t +pico_dns_first_label_length( const char *url ) +{ + const char *i = NULL; + uint16_t len = 0; + + /* Check params */ + if (!url) return 0; + + /* Count */ + i = url; + while (*i != '.' && *i != '\0') { + ++i; + ++len; + } + return len; +} + +/* **************************************************************************** + * Mirrors a dotted IPv4-address string. + * f.e. 192.168.0.1 => 1.0.168.192 + * + * @param ptr + * @return 0 on success, something else on failure. + * ****************************************************************************/ +int +pico_dns_mirror_addr( char *ip ) +{ + uint32_t addr = 0; + + /* Convert IPv4-string to network-order 32-bit number */ + if (pico_string_to_ipv4(ip, &addr) < 0) + return -1; + + /* Mirror the 32-bit number */ + addr = (uint32_t)((uint32_t)((addr & (uint32_t)0xFF000000u) >> 24) | + (uint32_t)((addr & (uint32_t)0xFF0000u) >> 8) | + (uint32_t)((addr & (uint32_t)0xFF00u) << 8) | + (uint32_t)((addr & (uint32_t)0xFFu) << 24)); + + return pico_ipv4_to_string(ip, addr); +} + +#ifdef PICO_SUPPORT_IPV6 +/* **************************************************************************** + * Get the ASCII value of the Most Significant Nibble of a byte + * + * @param byte Byte you want to extract the MSN from. + * @return The ASCII value of the Most Significant Nibble of the byte + * ****************************************************************************/ +static inline char +dns_ptr_ip6_nibble_lo( uint8_t byte ) +{ + uint8_t nibble = byte & 0x0f; + if (nibble < 10) + return (char)(nibble + '0'); + else + return (char)(nibble - 0xa + 'a'); +} + +/* **************************************************************************** + * Get the ASCII value of the Least Significant Nibble of a byte + * + * @param byte Byte you want to extract the LSN from. + * @return The ASCII value of the Least Significant Nibble of the byte + * ****************************************************************************/ +static inline char +dns_ptr_ip6_nibble_hi( uint8_t byte ) +{ + uint8_t nibble = (byte & 0xf0u) >> 4u; + if (nibble < 10u) + return (char)(nibble + '0'); + else + return (char)(nibble - 0xa + 'a'); +} + +/* **************************************************************************** + * Convert an IPv6-address in string-format to a IPv6-address in nibble-format. + * Doesn't add a IPv6 ARPA-suffix though. + * + * @param ip IPv6-address stored as a string + * @param dst Destination to store IPv6-address in nibble-format + * ****************************************************************************/ +void +pico_dns_ipv6_set_ptr( const char *ip, char *dst ) +{ + int i = 0, j = 0; + struct pico_ip6 ip6; + memset(&ip6, 0, sizeof(struct pico_ip6)); + pico_string_to_ipv6(ip, ip6.addr); + for (i = 15; i >= 0; i--) { + if ((j + 3) > 64) return; /* Don't want j to go out of bounds */ + + dst[j++] = dns_ptr_ip6_nibble_lo(ip6.addr[i]); + dst[j++] = '.'; + dst[j++] = dns_ptr_ip6_nibble_hi(ip6.addr[i]); + dst[j++] = '.'; + } +} +#endif + +/* MARK: ^ NAME & IP FUNCTIONS */ +/* MARK: v QUESTION FUNCTIONS */ + +/* **************************************************************************** + * Calculates the size of a single DNS Question. Void-pointer allows this + * function to be used with pico_tree_size. + * + * @param question Void-point to DNS Question + * @return Size in bytes of single DNS Question if it was copied flat. + * ****************************************************************************/ +static uint16_t pico_dns_question_size( void *question ) +{ + uint16_t size = 0; + struct pico_dns_question *q = (struct pico_dns_question *)question; + if (!q) + return 0; + + size = q->qname_length; + size = (uint16_t)(size + sizeof(struct pico_dns_question_suffix)); + return size; +} + +/* **************************************************************************** + * Deletes a single DNS Question. + * + * @param question Void-pointer to DNS Question. Can be used with pico_tree_- + * destroy. + * @return Returns 0 on success, something else on failure. + * ****************************************************************************/ +int +pico_dns_question_delete( void **question ) +{ + struct pico_dns_question **q = (struct pico_dns_question **)question; + + /* Check params */ + if ((!q) || !(*q)) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + if ((*q)->qname) + PICO_FREE(((*q)->qname)); + + if ((*q)->qsuffix) + PICO_FREE((*q)->qsuffix); + + PICO_FREE((*q)); + *question = NULL; + + return 0; +} + +/* **************************************************************************** + * Fills in the DNS question suffix-fields with the correct values. + * + * todo: Update pico_dns_client to make the same mechanism possible like with + * filling DNS Resource Record-suffixes. + * + * @param suf Pointer to the suffix member of the DNS question. + * @param qtype DNS type of the DNS question to be. + * @param qclass DNS class of the DNS question to be. + * @return Returns 0 on success, something else on failure. + * ****************************************************************************/ +int +pico_dns_question_fill_suffix( struct pico_dns_question_suffix *suf, + uint16_t qtype, + uint16_t qclass ) +{ + if (!suf) + return -1; + + suf->qtype = short_be(qtype); + suf->qclass = short_be(qclass); + return 0; +} + +/* **************************************************************************** + * Fills in the name of the DNS question. + * + * @param qname Pointer-pointer to the name-member of the DNS-question + * @param url Name in URL format you want to convert to a name in DNS name + * format. When reverse resolving, only the IP, either IPV4 or + * IPV6, should be given in string format. + * f.e. => for IPv4: "192.168.2.1" + * => for IPv6: "2001:0db8:85a3:0042:1000:8a2e:0370:7334" + * @param qtype DNS type type of the DNS question to be. + * @param proto When reverse is true the reverse resolution name will be + * generated depending on the protocol. Can be either + * PICO_PROTO_IPV4 or PICO_PROTO_IPV6. + * @param reverse When this is true a reverse resolution name will be generated + * from the URL. + * @return The eventual length of the generated name, 0 on failure. + * ****************************************************************************/ +static uint16_t +pico_dns_question_fill_name( char **qname, + const char *url, + uint16_t qtype, + uint8_t proto, + uint8_t reverse ) +{ + uint16_t slen = 0; + + /* Try to convert the URL to an FQDN */ + if (reverse && qtype == PICO_DNS_TYPE_PTR) + *qname = pico_dns_url_to_reverse_qname(url, proto); + else { + (*qname) = pico_dns_url_to_qname(url); + } + + if (!(*qname)) { + return 0; + } + + slen = (uint16_t)(pico_dns_strlen(*qname) + 1u); + return (pico_dns_check_namelen(slen)) ? ((uint16_t)0) : (slen); +} + +/* **************************************************************************** + * Creates a standalone DNS Question with a given name and type. + * + * @param url DNS question name in URL format. Will be converted to DNS + * name notation format. + * @param len Will be filled with the total length of the DNS question. + * @param proto Protocol for which you want to create a question. Can be + * either PICO_PROTO_IPV4 or PICO_PROTO_IPV6. + * @param qtype DNS type of the question to be. + * @param qclass DNS class of the question to be. + * @param reverse When this is true, a reverse resolution name will be gene- + * from the URL + * @return Returns pointer to the created DNS Question on success, NULL on + * failure. + * ****************************************************************************/ +struct pico_dns_question * +pico_dns_question_create( const char *url, + uint16_t *len, + uint8_t proto, + uint16_t qtype, + uint16_t qclass, + uint8_t reverse ) +{ + struct pico_dns_question *question = NULL; + uint16_t slen = 0; + int ret = 0; + + /* Check if valid arguments are provided */ + if (!url || !len) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + /* Allocate space for the question and the subfields */ + if (!(question = PICO_ZALLOC(sizeof(struct pico_dns_question)))) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + /* Fill name field */ + slen = pico_dns_question_fill_name(&(question->qname), url, + qtype, proto, reverse); + question->qname_length = (uint8_t)(slen); + question->proto = proto; + + /* Provide space for the question suffix & try to fill in */ + question->qsuffix = PICO_ZALLOC(sizeof(struct pico_dns_question_suffix)); + ret = pico_dns_question_fill_suffix(question->qsuffix, qtype, qclass); + if (ret || pico_dns_check_namelen(slen)) { + pico_dns_question_delete((void **)&question); + return NULL; + } + + /* Determine the entire length of the question */ + *len = (uint16_t)(slen + (uint16_t)sizeof(struct pico_dns_question_suffix)); + + return question; +} + +/* **************************************************************************** + * Decompresses the name of a single DNS question. + * + * @param question Question you want to decompress the name of + * @param packet Packet in which the DNS question is contained. + * @return Pointer to original name of the DNS question before decompressing. + * ****************************************************************************/ +char * +pico_dns_question_decompress( struct pico_dns_question *question, + pico_dns_packet *packet ) +{ + char *qname_original = question->qname; + + /* Try to decompress the question name */ + if (!(question->qname = pico_dns_decompress_name(question->qname, packet))) { + question->qname = qname_original; + } + + return qname_original; +} + + +/* MARK: ^ QUESTION FUNCTIONS */ +/* MARK: v RESOURCE RECORD FUNCTIONS */ + +/* **************************************************************************** + * Copies the contents of DNS Resource Record to a single flat memory-buffer. + * + * @param record Pointer to DNS record you want to copy flat. + * @param destination Pointer-pointer to flat memory buffer to copy DNS record + * to. When function returns, this will point to location + * right after the flat copied DNS Resource Record. + * @return Returns 0 on success, something else on failure. + * ****************************************************************************/ +static int +pico_dns_record_copy_flat( struct pico_dns_record *record, + uint8_t **destination ) +{ + char *dest_rname = NULL; /* rname destination location */ + struct pico_dns_record_suffix *dest_rsuffix = NULL; /* rsuffix destin. */ + uint8_t *dest_rdata = NULL; /* rdata destination location */ + + /* Check if there are no NULL-pointers given */ + if (!record || !destination || !(*destination)) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* Initialise the destination pointers to the right locations */ + dest_rname = (char *) *destination; + dest_rsuffix = (struct pico_dns_record_suffix *) + (dest_rname + record->rname_length); + dest_rdata = ((uint8_t *)dest_rsuffix + + sizeof(struct pico_dns_record_suffix)); + + /* Copy the rname of the resource record into the flat location */ + strcpy(dest_rname, record->rname); + + /* Copy the question suffix fields */ + dest_rsuffix->rtype = record->rsuffix->rtype; + dest_rsuffix->rclass = record->rsuffix->rclass; + dest_rsuffix->rttl = record->rsuffix->rttl; + dest_rsuffix->rdlength = record->rsuffix->rdlength; + + /* Copy the rdata of the resource */ + memcpy(dest_rdata, record->rdata, short_be(dest_rsuffix->rdlength)); + + /* Point to location right after flat resource record */ + *destination = (uint8_t *)(dest_rdata + + short_be(record->rsuffix->rdlength)); + return 0; +} + +/* **************************************************************************** + * Calculates the size of a single DNS Resource Record. Void-pointer allows + * this function to be used with pico_tree_size. + * + * @param record void-pointer to DNS record you want to know the size of. + * @return Size of single DNS record if it was copied flat. + * ****************************************************************************/ +static uint16_t +pico_dns_record_size( void *record ) +{ + uint16_t size = 0; + struct pico_dns_record *rr = (struct pico_dns_record *)record; + + if (!rr || !(rr->rsuffix)) + return 0; + + size = rr->rname_length; + size = (uint16_t)(size + sizeof(struct pico_dns_record_suffix)); + size = (uint16_t)(size + short_be(rr->rsuffix->rdlength)); + return size; +} + +/* **************************************************************************** + * Deletes a single DNS resource record. Void-pointer-pointer allows this + * function to be used with pico_tree_destroy. + * + * @param record void-pointer-pointer to DNS record you want to delete. + * @return Returns 0 on success, something else on failure. + * ****************************************************************************/ +int +pico_dns_record_delete( void **record ) +{ + struct pico_dns_record **rr = (struct pico_dns_record **)record; + + if ((!rr) || !(*rr)) + return 0; + + if ((*rr)->rname) + PICO_FREE((*rr)->rname); + + if ((*rr)->rsuffix) + PICO_FREE((*rr)->rsuffix); + + if ((*rr)->rdata) + PICO_FREE((*rr)->rdata); + + PICO_FREE((*rr)); + *record = NULL; + + return 0; +} + +/* **************************************************************************** + * Just copies a resource record hard. + * + * @param record DNS record you want to copy + * @return Pointer to copy of DNS record. + * ****************************************************************************/ +struct pico_dns_record * +pico_dns_record_copy( struct pico_dns_record *record ) +{ + struct pico_dns_record *copy = NULL; + + /* Check params */ + if (!record || !(record->rname) || !(record->rdata) || !(record->rsuffix)) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + /* Provide space for the copy */ + if (!(copy = PICO_ZALLOC(sizeof(struct pico_dns_record)))) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + /* Provide space for the subfields */ + copy->rname = PICO_ZALLOC((size_t)record->rname_length); + copy->rsuffix = PICO_ZALLOC(sizeof(struct pico_dns_record_suffix)); + copy->rdata = PICO_ZALLOC((size_t)short_be(record->rsuffix->rdlength)); + if (!(copy->rname) || !(copy->rsuffix) || !(copy->rdata)) { + pico_dns_record_delete((void **)©); + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + /* Fill in the rname field */ + memcpy((void *)(copy->rname), (void *)(record->rname), + (size_t)(record->rname_length)); + copy->rname_length = record->rname_length; + + /* Fill in the rsuffix fields */ + copy->rsuffix->rtype = record->rsuffix->rtype; + copy->rsuffix->rclass = record->rsuffix->rclass; + copy->rsuffix->rttl = record->rsuffix->rttl; + copy->rsuffix->rdlength = record->rsuffix->rdlength; + + /* Fill in the rdata field */ + memcpy(copy->rdata, record->rdata, short_be(record->rsuffix->rdlength)); + + return copy; +} + +/* **************************************************************************** + * Fills in the DNS resource record suffix-fields with the correct values. + * + * @param suf Pointer-pointer to rsuffix-member of struct pico_dns_record. + * @param rtype DNS type of the resource record to be. + * @param rclass DNS class of the resource record to be. + * @param rttl DNS ttl of the resource record to be. + * @param rdlength DNS rdlength of the resource record to be. + * @return Returns 0 on success, something else on failure. + * ****************************************************************************/ +static int +pico_dns_record_fill_suffix( struct pico_dns_record_suffix **suf, + uint16_t rtype, + uint16_t rclass, + uint32_t rttl, + uint16_t rdlength ) +{ + /* Try to provide space for the rsuffix */ + if (!(*suf = PICO_ZALLOC(sizeof(struct pico_dns_record_suffix)))) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + /* Fill in the fields */ + (*suf)->rtype = short_be(rtype); + (*suf)->rclass = short_be(rclass); + (*suf)->rttl = long_be(rttl); + (*suf)->rdlength = short_be(rdlength); + + return 0; +} + +/* **************************************************************************** + * Fills the data-buffer of a DNS resource record. + * + * @param rdata Pointer-pointer to rdata-member of struct pico_dns_record. + * @param _rdata Memory buffer with data to insert in the resource record. If + * data should contain a DNS name, the name in the databuffer + * needs to be in URL-format. + * @param datalen The exact length in bytes of the _rdata-buffer. If data of + * record should contain a DNS name, datalen needs to be + * pico_dns_strlen(_rdata). + * @param rtype DNS type of the resource record to be + * @return Returns 0 on failure, length of filled in rdata-member on success. + * Can differ from datalen-param because of URL to DNS Name conversion. + * ****************************************************************************/ +static uint16_t +pico_dns_record_fill_rdata( uint8_t **rdata, + void *_rdata, + uint16_t datalen, + uint16_t rtype ) +{ + uint16_t _datalen = 0; + + /* If type is PTR, rdata will be a DNS name in URL format */ + if (rtype == PICO_DNS_TYPE_PTR) { + _datalen = (uint16_t)(datalen + 2u); + if (!(*rdata = (uint8_t *)pico_dns_url_to_qname(_rdata))) { + pico_err = PICO_ERR_ENOMEM; + return 0; + } + } else { + /* Otherwise just copy in the databuffer */ + if (datalen == 0) { + return datalen; + } + _datalen = datalen; + if (!(*rdata = (uint8_t *)PICO_ZALLOC((size_t)datalen))) { + pico_err = PICO_ERR_ENOMEM; + return 0; + } + + memcpy((void *)*rdata, (void *)_rdata, datalen); + } + + return _datalen; +} + +/* **************************************************************************** + * Create a standalone DNS Resource Record with a given name. + * + * @param url DNS rrecord name in URL format. Will be converted to DNS + * name notation format. + * @param _rdata Memory buffer with data to insert in the resource record. If + * data should contain a DNS name, the name in the databuffer + * needs to be in URL-format. + * @param datalen The exact length in bytes of the _rdata-buffer. If data of + * record should contain a DNS name, datalen needs to be + * pico_dns_strlen(_rdata). + * @param len Will be filled with the total length of the DNS rrecord. + * @param rtype DNS type of the resource record to be. + * @param rclass DNS class of the resource record to be. + * @param rttl DNS ttl of the resource record to be. + * @return Returns pointer to the created DNS Resource Record + * ****************************************************************************/ +struct pico_dns_record * +pico_dns_record_create( const char *url, + void *_rdata, + uint16_t datalen, + uint16_t *len, + uint16_t rtype, + uint16_t rclass, + uint32_t rttl ) +{ + struct pico_dns_record *record = NULL; + uint16_t slen = (uint16_t)(pico_dns_strlen(url) + 2u); + int ret = 0; + + /* Check params */ + if (pico_dns_check_namelen(slen) || !_rdata || !len) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + /* Allocate space for the record and subfields */ + if (!(record = PICO_ZALLOC(sizeof(struct pico_dns_record)))) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + /* Provide space and convert the URL to a DNS name */ + record->rname = pico_dns_url_to_qname(url); + record->rname_length = slen; + + /* Provide space & fill in the rdata field */ + datalen = pico_dns_record_fill_rdata(&(record->rdata), _rdata, + datalen, rtype); + + /* Provide space & fill in the rsuffix */ + ret = pico_dns_record_fill_suffix(&(record->rsuffix), rtype, rclass, rttl, + datalen); + + /* Check if everything succeeded */ + if (!(record->rname) || ret) { + pico_dns_record_delete((void **)&record); + return NULL; + } + + /* Determine the complete length of resource record */ + *len = (uint16_t)(slen + sizeof(struct pico_dns_record_suffix) + datalen); + return record; +} + +/* **************************************************************************** + * Decompresses the name of single DNS record. + * + * @param record DNS record to decompress the name of. + * @param packet Packet in which is DNS record is present + * @return Pointer to original name of the DNS record before decompressing. + * ****************************************************************************/ +char * +pico_dns_record_decompress( struct pico_dns_record *record, + pico_dns_packet *packet ) +{ + char *rname_original = record->rname; + + /* Try to decompress the record name */ + if (!(record->rname = pico_dns_decompress_name(record->rname, packet))) { + record->rname = rname_original; + } + + return rname_original; +} + +static int pico_tolower(int c) +{ + if ((c >= 'A') && (c <= 'Z')) + c += 'a' - 'A'; + return c; +} + +/* MARK: ^ RESOURCE RECORD FUNCTIONS */ +/* MARK: v COMPARING */ + +/* **************************************************************************** + * Compares two databuffers against each other. + * + * @param a 1st Memory buffer to compare + * @param b 2nd Memory buffer to compare + * @param rdlength_a Length of 1st memory buffer + * @param rdlength_b Length of 2nd memory buffer + * @param caseinsensitive Whether or not the bytes are compared + * case-insensitive + * @return 0 when the buffers are equal, returns difference when they're not. + * ****************************************************************************/ +int +pico_dns_rdata_cmp( uint8_t *a, uint8_t *b, + uint16_t rdlength_a, uint16_t rdlength_b, uint8_t caseinsensitive ) +{ + uint16_t i = 0; + uint16_t slen = 0; + int dif = 0; + + /* Check params */ + if (!a || !b) { + if (!a && !b) + return 0; + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* Determine the smallest length */ + slen = rdlength_a; + if (rdlength_b < slen) + slen = rdlength_b; + + /* loop over slen */ + if(caseinsensitive){ + for (i = 0; i < slen; i++) { + if ((dif = pico_tolower((int)a[i]) - pico_tolower((int)b[i]))) { + return dif; + } + } + }else{ + for (i = 0; i < slen; i++) { + if ((dif = (int)a[i] - (int)b[i])){ + return dif; + } + } + } + + /* Return difference of buffer lengths */ + return (int)((int)rdlength_a - (int)rdlength_b); +} + +/* **************************************************************************** + * Compares 2 DNS questions + * + * @param qa DNS question A as a void-pointer (for pico_tree) + * @param qb DNS question A as a void-pointer (for pico_tree) + * @return 0 when questions are equal, returns difference when they're not. + * ****************************************************************************/ +int +pico_dns_question_cmp( void *qa, + void *qb ) +{ + int dif = 0; + uint16_t at = 0, bt = 0; + struct pico_dns_question *a = (struct pico_dns_question *)qa; + struct pico_dns_question *b = (struct pico_dns_question *)qb; + + /* Check params */ + if (!a || !b) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* First, compare the qtypes */ + at = short_be(a->qsuffix->qtype); + bt = short_be(b->qsuffix->qtype); + if ((dif = (int)((int)at - (int)bt))) + return dif; + + /* Then compare qnames */ + return pico_dns_rdata_cmp((uint8_t *)a->qname, (uint8_t *)b->qname, + pico_dns_strlen(a->qname), + pico_dns_strlen(b->qname), 1); +} + +/* **************************************************************************** + * Compares 2 DNS records by type and name only + * + * @param ra DNS record A as a void-pointer (for pico_tree) + * @param rb DNS record B as a void-pointer (for pico_tree) + * @return 0 when name and type of records are equal, returns difference when + * they're not. + * ****************************************************************************/ +int +pico_dns_record_cmp_name_type( void *ra, + void *rb ) +{ + int dif; + uint16_t at = 0, bt = 0; + struct pico_dns_record *a = (struct pico_dns_record *)ra; + struct pico_dns_record *b = (struct pico_dns_record *)rb; + + /* Check params */ + if (!a || !b) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* First, compare the rrtypes */ + at = short_be(a->rsuffix->rtype); + bt = short_be(b->rsuffix->rtype); + if ((dif = (int)((int)at - (int)bt))) + return dif; + + /* Then compare names */ + return pico_dns_rdata_cmp((uint8_t *)(a->rname), (uint8_t *)(b->rname), + (uint16_t)strlen(a->rname), + (uint16_t)strlen(b->rname), 1); +} + +/* **************************************************************************** + * Compares 2 DNS records by type, name AND rdata for a truly unique result + * + * @param ra DNS record A as a void-pointer (for pico_tree) + * @param rb DNS record B as a void-pointer (for pico_tree) + * @return 0 when records are equal, returns difference when they're not + * ****************************************************************************/ +int +pico_dns_record_cmp( void *ra, + void *rb ) +{ + int dif = 0; + struct pico_dns_record *a = (struct pico_dns_record *)ra; + struct pico_dns_record *b = (struct pico_dns_record *)rb; + + /* Check params */ + if (!a || !b) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* Compare type and name */ + if ((dif = pico_dns_record_cmp_name_type(a, b))) + return dif; + + /* Then compare rdata */ + return pico_dns_rdata_cmp(a->rdata, b->rdata, + short_be(a->rsuffix->rdlength), + short_be(b->rsuffix->rdlength), 0); +} + +/* MARK: ^ COMPARING */ +/* MARK: v PICO_TREE */ + +/* **************************************************************************** + * Erases a pico_tree entirely. + * + * @param tree Pointer to a pico_tree-instance + * @param node_delete Helper-function for type-specific deleting. + * @return Returns 0 on success, something else on failure. + * ****************************************************************************/ +int +pico_tree_destroy( struct pico_tree *tree, int (*node_delete)(void **)) +{ + struct pico_tree_node *node = NULL, *next = NULL; + void *item = NULL; + + /* Check params */ + if (!tree) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + pico_tree_foreach_safe(node, tree, next) { + item = node->keyValue; + pico_tree_delete(tree, node->keyValue); + if (item && node_delete) { + node_delete((void **)&item); + } + } + + return 0; +} + +/* **************************************************************************** + * Calculates the size in bytes of all the nodes contained in the tree summed + * up. And gets the amount of items in the tree as well. + * + * @param tree Pointer to pico_tree-instance + * @param size Will get filled with the size of all the nodes summed up. + * Make sure you clear out (set to 0) this param before you + * call this function because it doesn't happen inside and + * each size will be added to the initial value. + * @param node_size Helper-function for type-specific size-determination + * @return Amount of items in the tree. + * ****************************************************************************/ +static uint16_t +pico_tree_size( struct pico_tree *tree, + uint16_t *size, + uint16_t (*node_size)(void *)) +{ + struct pico_tree_node *node = NULL; + void *node_item = NULL; + uint16_t count = 0; + + /* Check params */ + if (!tree || !size) { + pico_err = PICO_ERR_EINVAL; + return 0; + } + + /* Add up the node sizes */ + pico_tree_foreach(node, tree) { + if ((node_item = node->keyValue)) { + *size = (uint16_t)((*size) + node_size(node_item)); + count++; + } + } + + return count; +} + +/* **************************************************************************** + * Determines the amount of nodes in a pico_tere + * + * @param tree Pointer to pico_tree-instance + * @return Amount of items in the tree. + * ****************************************************************************/ +uint16_t +pico_tree_count( struct pico_tree *tree ) +{ + struct pico_tree_node *node = NULL; + uint16_t count = 0; + + pico_tree_foreach(node, tree) { + if (node->keyValue) + count++; + } + + return count; +} + +/* **************************************************************************** + * Deletes all the questions with given DNS name from a pico_tree + * + * @param qtree Pointer to pico_tree-instance which contains DNS questions + * @param name Name of the questions you want to delete + * @return Returns 0 on success, something else on failure. + * ****************************************************************************/ +int +pico_dns_qtree_del_name( struct pico_tree *qtree, + const char *name ) +{ + struct pico_tree_node *node = NULL, *next = NULL; + struct pico_dns_question *question = NULL; + + /* Check params */ + if (!qtree || !name) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* Iterate over tree and delete every node with given name */ + pico_tree_foreach_safe(node, qtree, next) { + question = (struct pico_dns_question *)node->keyValue; + if ((question) && (strcasecmp(question->qname, name) == 0)) { + question = pico_tree_delete(qtree, (void *)question); + pico_dns_question_delete((void **)&question); + } + } + + return 0; +} + +/* **************************************************************************** + * Checks whether a question with given name is in the tree or not. + * + * @param qtree Pointer to pico_tree-instance which contains DNS questions + * @param name Name you want to check for + * @return 1 when the name is present in the qtree, 0 when it's not. + * ****************************************************************************/ +int +pico_dns_qtree_find_name( struct pico_tree *qtree, + const char *name ) +{ + struct pico_tree_node *node = NULL; + struct pico_dns_question *question = NULL; + + /* Check params */ + if (!qtree || !name) { + pico_err = PICO_ERR_EINVAL; + return 0; + } + + /* Iterate over tree and compare names */ + pico_tree_foreach(node, qtree) { + question = (struct pico_dns_question *)node->keyValue; + if ((question) && (strcasecmp(question->qname, name) == 0)) + return 1; + } + + return 0; +} + +/* MARK: ^ PICO_TREE */ +/* MARK: v DNS PACKET FUNCTIONS */ + +/* **************************************************************************** + * Fills the header section of a DNS packet with the correct flags and section + * -counts. + * + * @param hdr Header to fill in. + * @param qdcount Amount of questions added to the packet + * @param ancount Amount of answer records added to the packet + * @param nscount Amount of authority records added to the packet + * @param arcount Amount of additional records added to the packet + * ****************************************************************************/ +void +pico_dns_fill_packet_header( struct pico_dns_header *hdr, + uint16_t qdcount, + uint16_t ancount, + uint16_t nscount, + uint16_t arcount ) +{ + /* ID should be filled by caller */ + + if(qdcount > 0) { /* Questions present? Make it a query */ + hdr->qr = PICO_DNS_QR_QUERY; + hdr->aa = PICO_DNS_AA_NO_AUTHORITY; + } else { /* No questions present? Make it an answer*/ + hdr->qr = PICO_DNS_QR_RESPONSE; + hdr->aa = PICO_DNS_AA_IS_AUTHORITY; + } + + /* Fill in the flags and the fields */ + hdr->opcode = PICO_DNS_OPCODE_QUERY; + hdr->tc = PICO_DNS_TC_NO_TRUNCATION; + hdr->rd = PICO_DNS_RD_IS_DESIRED; + hdr->ra = PICO_DNS_RA_NO_SUPPORT; + hdr->z = 0; /* Z, AD, CD are 0 */ + hdr->rcode = PICO_DNS_RCODE_NO_ERROR; + hdr->qdcount = short_be(qdcount); + hdr->ancount = short_be(ancount); + hdr->nscount = short_be(nscount); + hdr->arcount = short_be(arcount); +} + +/* **************************************************************************** + * Fills a single DNS resource record section of a DNS packet. + * + * @param rtree Tree that contains the DNS resource records. + * @param dest Pointer-pointer to location where you want to insert records. + * Will point to location after current section on return. + * @return 0 on success, something else on failure. + * ****************************************************************************/ +static int +pico_dns_fill_packet_rr_section( struct pico_tree *rtree, + uint8_t **dest ) +{ + struct pico_tree_node *node = NULL; + struct pico_dns_record *record = NULL; + + pico_tree_foreach(node, rtree) { + record = node->keyValue; + if ((record) && pico_dns_record_copy_flat(record, dest)) { + dns_dbg("Could not copy record into Answer Section!\n"); + return -1; + } + } + return 0; +} + +/* **************************************************************************** + * Fills the resource record sections of a DNS packet with provided record- + * trees. + * + * @param packet Packet you want to fill + * @param qtree Question tree to determine where the rrsections begin. + * @param antree DNS records to put in Answer section + * @param nstree DNS records to put in Authority section + * @param artree DNS records to put in Additional section + * @return 0 on success, something else on failure. + * ****************************************************************************/ +static int +pico_dns_fill_packet_rr_sections( pico_dns_packet *packet, + struct pico_tree *qtree, + struct pico_tree *antree, + struct pico_tree *nstree, + struct pico_tree *artree ) +{ + int anret = 0, nsret = 0, arret = 0; + uint16_t temp = 0; + uint8_t *destination = NULL; + + /* Check params */ + if (!packet || !qtree || !antree || !nstree || !artree) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* Initialise the destination pointers before iterating */ + destination = (uint8_t *)packet + sizeof(struct pico_dns_header); + pico_tree_size(qtree, &temp, &pico_dns_question_size); + destination = destination + temp; + + /* Iterate over ANSWERS */ + anret = pico_dns_fill_packet_rr_section(antree, &destination); + + /* Iterate over AUTHORITIES */ + nsret = pico_dns_fill_packet_rr_section(nstree, &destination); + + /* Iterate over ADDITIONALS */ + arret = pico_dns_fill_packet_rr_section(artree, &destination); + + if (anret || nsret || arret) + return -1; + + return 0; +} + +/* **************************************************************************** + * Fills the question section of a DNS packet with provided questions in the + * tree. + * + * @param packet Packet you want to fill + * @param qtree Question tree with question you want to insert + * @return 0 on success, something else on failure. + * ****************************************************************************/ +static int +pico_dns_fill_packet_question_section( pico_dns_packet *packet, + struct pico_tree *qtree ) +{ + struct pico_tree_node *node = NULL; + struct pico_dns_question *question = NULL; + struct pico_dns_question_suffix *dest_qsuffix = NULL; + char *dest_qname = NULL; + + /* Check params */ + if (!packet || !qtree) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* Initialise pointer */ + dest_qname = (char *)((char *)packet + sizeof(struct pico_dns_header)); + + pico_tree_foreach(node, qtree) { + question = node->keyValue; + if (question) { + /* Copy the name */ + memcpy(dest_qname, question->qname, question->qname_length); + + /* Copy the suffix */ + dest_qsuffix = (struct pico_dns_question_suffix *) + (dest_qname + question->qname_length); + dest_qsuffix->qtype = question->qsuffix->qtype; + dest_qsuffix->qclass = question->qsuffix->qclass; + + /* Move to next question */ + dest_qname = (char *)((char *)dest_qsuffix + + sizeof(struct pico_dns_question_suffix)); + } + } + return 0; +} + +/* **************************************************************************** + * Looks for a name somewhere else in packet, more specifically between the + * beginning of the data buffer and the name itself. + * ****************************************************************************/ +static uint8_t * +pico_dns_packet_compress_find_ptr( uint8_t *name, + uint8_t *data, + uint16_t len ) +{ + uint8_t *iterator = NULL; + + /* Check params */ + if (!name || !data || !len) + return NULL; + + if ((name < data) || (name > (data + len))) + return NULL; + + iterator = data; + + /* Iterate from the beginning of data up until the name-ptr */ + while (iterator < name) { + /* Compare in each iteration of current name is equal to a section of + the DNS packet and if so return the pointer to that section */ + if (memcmp((void *)iterator++, (void *)name, + pico_dns_strlen((char *)name) + 1u) == 0) + return (iterator - 1); + } + return NULL; +} + +/* **************************************************************************** + * Compresses a single name by looking for the same name somewhere else in the + * packet-buffer. + * ****************************************************************************/ +static int +pico_dns_packet_compress_name( uint8_t *name, + uint8_t *packet, + uint16_t *len) +{ + uint8_t *lbl_iterator = NULL; /* To iterate over labels */ + uint8_t *compression_ptr = NULL; /* PTR to somewhere else in the packet */ + uint8_t *offset = NULL; /* PTR after compression pointer */ + uint8_t *ptr_after_str = NULL; + uint8_t *last_byte = NULL; + uint8_t *i = NULL; + uint16_t ptr = 0; + uint16_t difference = 0; + + /* Check params */ + if (!name || !packet || !len) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + if ((name < packet) || (name > (packet + *len))) { + dns_dbg("Name ptr OOB. name: %p max: %p\n", name, packet + *len); + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* Try to compress name */ + lbl_iterator = name; + while (lbl_iterator != '\0') { + /* Try to find a compression pointer with current name */ + compression_ptr = pico_dns_packet_compress_find_ptr(lbl_iterator, + packet + 12, *len); + /* If name can be compressed */ + if (compression_ptr) { + /* Point to place after current string */ + ptr_after_str = lbl_iterator + strlen((char *)lbl_iterator) + 1u; + + /* Calculate the compression pointer value */ + ptr = (uint16_t)(compression_ptr - packet); + + /* Set the compression pointer in the packet */ + *lbl_iterator = (uint8_t)(0xC0 | (uint8_t)(ptr >> 8)); + *(lbl_iterator + 1) = (uint8_t)(ptr & 0xFF); + + /* Move up the rest of the packet data to right after the pointer */ + offset = lbl_iterator + 2; + + /* Move up left over data */ + difference = (uint16_t)(ptr_after_str - offset); + last_byte = packet + *len; + for (i = ptr_after_str; i < last_byte; i++) + *((uint8_t *)(i - difference)) = *i; + /* Update length */ + *len = (uint16_t)(*len - difference); + break; + } + + /* Move to next length label */ + lbl_iterator = lbl_iterator + *(lbl_iterator) + 1; + } + return 0; +} + +/* **************************************************************************** + * Utility function compress a record section + * ****************************************************************************/ +static int +pico_dns_compress_record_sections( uint16_t qdcount, uint16_t count, + uint8_t *buf, uint8_t **iterator, + uint16_t *len ) +{ + struct pico_dns_record_suffix *rsuffix = NULL; + uint8_t *_iterator = NULL; + uint16_t i = 0; + + /* Check params */ + if (!iterator || !(*iterator) || !buf || !len) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + _iterator = *iterator; + + for (i = 0; i < count; i++) { + if (qdcount || i) + pico_dns_packet_compress_name(_iterator, buf, len); + + /* To get rdlength */ + rsuffix = (struct pico_dns_record_suffix *) + (_iterator + pico_dns_namelen_comp((char *)_iterator) + 1u); + + /* Move to next res record */ + _iterator = ((uint8_t *)rsuffix + + sizeof(struct pico_dns_record_suffix) + + short_be(rsuffix->rdlength)); + } + *iterator = _iterator; + return 0; +} + +/* **************************************************************************** + * Applies DNS name compression to an entire DNS packet + * ****************************************************************************/ +static int +pico_dns_packet_compress( pico_dns_packet *packet, uint16_t *len ) +{ + uint8_t *packet_buf = NULL; + uint8_t *iterator = NULL; + uint16_t qdcount = 0, rcount = 0, i = 0; + + /* Check params */ + if (!packet || !len) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + packet_buf = (uint8_t *)packet; + + /* Temporarily store the question & record counts */ + qdcount = short_be(packet->qdcount); + rcount = (uint16_t)(rcount + short_be(packet->ancount)); + rcount = (uint16_t)(rcount + short_be(packet->nscount)); + rcount = (uint16_t)(rcount + short_be(packet->arcount)); + + /* Move past the DNS packet header */ + iterator = (uint8_t *)((uint8_t *) packet + 12u); + + /* Start with the questions */ + for (i = 0; i < qdcount; i++) { + if (i) /* First question can't be compressed */ + pico_dns_packet_compress_name(iterator, packet_buf, len); + + /* Move to next question */ + iterator = (uint8_t *)(iterator + + pico_dns_namelen_comp((char *)iterator) + + sizeof(struct pico_dns_question_suffix) + 1u); + } + /* Then onto the answers */ + pico_dns_compress_record_sections(qdcount, rcount, packet_buf, &iterator, + len); + return 0; +} + +/* **************************************************************************** + * Calculates how big a packet needs be in order to store all the question & + * records in the tree. Also determines the amount of questions and records. + * + * @param qtree Tree with Questions. + * @param antree Tree with Answer Records. + * @param nstree Tree with Authority Records. + * @param artree Tree with Additional Records. + * @param qdcount Pointer to var to store amount of questions + * @param ancount Pointer to var to store amount of answers. + * @param nscount Pointer to var to store amount of authorities. + * @param arcount Pointer to var to store amount of additionals. + * @return Returns the total length that the DNS packet needs to be. + * ****************************************************************************/ +static uint16_t +pico_dns_packet_len( struct pico_tree *qtree, + struct pico_tree *antree, + struct pico_tree *nstree, + struct pico_tree *artree, + uint8_t *qdcount, uint8_t *ancount, + uint8_t *nscount, uint8_t *arcount ) +{ + uint16_t len = (uint16_t) sizeof(pico_dns_packet); + + /* Check params */ + if (!qtree || !antree || !nstree || !artree) { + pico_err = PICO_ERR_EINVAL; + return 0; + } + + *qdcount = (uint8_t)pico_tree_size(qtree, &len, &pico_dns_question_size); + *ancount = (uint8_t)pico_tree_size(antree, &len, &pico_dns_record_size); + *nscount = (uint8_t)pico_tree_size(nstree, &len, &pico_dns_record_size); + *arcount = (uint8_t)pico_tree_size(artree, &len, &pico_dns_record_size); + return len; +} + +/* **************************************************************************** + * Generic packet creation utility that just creates a DNS packet with given + * questions and resource records to put in the Resource Record Sections. If a + * NULL-pointer is provided for a certain tree, no records will be added to + * that particular section of the packet. + * + * @param qtree DNS Questions to put in the Question Section. + * @param antree DNS Records to put in the Answer Section. + * @param nstree DNS Records to put in the Authority Section. + * @param artree DNS Records to put in the Additional Section. + * @param len Will get fill with the entire size of the packet + * @return Pointer to created DNS packet + * ****************************************************************************/ +static pico_dns_packet * +pico_dns_packet_create( struct pico_tree *qtree, + struct pico_tree *antree, + struct pico_tree *nstree, + struct pico_tree *artree, + uint16_t *len ) +{ + PICO_DNS_QTREE_DECLARE(_qtree); + PICO_DNS_RTREE_DECLARE(_antree); + PICO_DNS_RTREE_DECLARE(_nstree); + PICO_DNS_RTREE_DECLARE(_artree); + pico_dns_packet *packet = NULL; + uint8_t qdcount = 0, ancount = 0, nscount = 0, arcount = 0; + + /* Set default vector, if arguments are NULL-pointers */ + _qtree = (qtree) ? (*qtree) : (_qtree); + _antree = (antree) ? (*antree) : (_antree); + _nstree = (nstree) ? (*nstree) : (_nstree); + _artree = (artree) ? (*artree) : (_artree); + + /* Get the size of the entire packet and determine the header counters */ + *len = pico_dns_packet_len(&_qtree, &_antree, &_nstree, &_artree, + &qdcount, &ancount, &nscount, &arcount); + + /* Provide space for the entire packet */ + if (!(packet = PICO_ZALLOC(*len))) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + /* Fill the Question Section with questions */ + if (qtree && pico_tree_count(&_qtree) != 0) { + if (pico_dns_fill_packet_question_section(packet, &_qtree)) { + dns_dbg("Could not fill Question Section correctly!\n"); + return NULL; + } + } + + /* Fill the Resource Record Sections with resource records */ + if (pico_dns_fill_packet_rr_sections(packet, &_qtree, &_antree, + &_nstree, &_artree)) { + dns_dbg("Could not fill Resource Record Sections correctly!\n"); + return NULL; + } + + /* Fill the DNS packet header and try to compress */ + pico_dns_fill_packet_header(packet, qdcount, ancount, nscount, arcount); + pico_dns_packet_compress(packet, len); + + return packet; +} + +/* **************************************************************************** + * Creates a DNS Query packet with given question and resource records to put + * the Resource Record Sections. If a NULL-pointer is provided for a certain + * tree, no records will be added to that particular section of the packet. + * + * @param qtree DNS Questions to put in the Question Section + * @param antree DNS Records to put in the Answer Section + * @param nstree DNS Records to put in the Authority Section + * @param artree DNS Records to put in the Additional Section + * @param len Will get filled with the entire size of the packet + * @return Pointer to created DNS packet + * ****************************************************************************/ +pico_dns_packet * +pico_dns_query_create( struct pico_tree *qtree, + struct pico_tree *antree, + struct pico_tree *nstree, + struct pico_tree *artree, + uint16_t *len ) +{ + return pico_dns_packet_create(qtree, antree, nstree, artree, len); +} + +/* **************************************************************************** + * Creates a DNS Answer packet with given resource records to put in the + * Resource Record Sections. If a NULL-pointer is provided for a certain tree, + * no records will be added to that particular section of the packet. + * + * @param antree DNS Records to put in the Answer Section + * @param nstree DNS Records to put in the Authority Section + * @param artree DNS Records to put in the Additional Section + * @param len Will get filled with the entire size of the packet + * @return Pointer to created DNS packet. + * ****************************************************************************/ +pico_dns_packet * +pico_dns_answer_create( struct pico_tree *antree, + struct pico_tree *nstree, + struct pico_tree *artree, + uint16_t *len ) +{ + return pico_dns_packet_create(NULL, antree, nstree, artree, len); +} +/* MARK: ^ DNS PACKET FUNCTIONS */ diff --git a/ext/picotcp/modules/pico_dns_common.h b/ext/picotcp/modules/pico_dns_common.h new file mode 100644 index 0000000..32a1a1d --- /dev/null +++ b/ext/picotcp/modules/pico_dns_common.h @@ -0,0 +1,523 @@ + +/********************************************************************* + PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved. + See LICENSE and COPYING for usage. + . + Authors: Toon Stegen, Jelle De Vleeschouwer + *********************************************************************/ + +#ifndef INCLUDE_PICO_DNS_COMMON +#define INCLUDE_PICO_DNS_COMMON + +#include "pico_config.h" +#include "pico_tree.h" + +/* TYPE values */ +#define PICO_DNS_TYPE_A 1 +#define PICO_DNS_TYPE_CNAME 5 +#define PICO_DNS_TYPE_PTR 12 +#define PICO_DNS_TYPE_TXT 16 +#define PICO_DNS_TYPE_AAAA 28 +#define PICO_DNS_TYPE_SRV 33 +#define PICO_DNS_TYPE_NSEC 47 +#define PICO_DNS_TYPE_ANY 255 + +/* CLASS values */ +#define PICO_DNS_CLASS_IN 1 + +/* FLAG values */ +#define PICO_DNS_QR_QUERY 0 +#define PICO_DNS_QR_RESPONSE 1 +#define PICO_DNS_OPCODE_QUERY 0 +#define PICO_DNS_OPCODE_IQUERY 1 +#define PICO_DNS_OPCODE_STATUS 2 +#define PICO_DNS_AA_NO_AUTHORITY 0 +#define PICO_DNS_AA_IS_AUTHORITY 1 +#define PICO_DNS_TC_NO_TRUNCATION 0 +#define PICO_DNS_TC_IS_TRUNCATED 1 +#define PICO_DNS_RD_NO_DESIRE 0 +#define PICO_DNS_RD_IS_DESIRED 1 +#define PICO_DNS_RA_NO_SUPPORT 0 +#define PICO_DNS_RA_IS_SUPPORTED 1 +#define PICO_DNS_RCODE_NO_ERROR 0 +#define PICO_DNS_RCODE_EFORMAT 1 +#define PICO_DNS_RCODE_ESERVER 2 +#define PICO_DNS_RCODE_ENAME 3 +#define PICO_DNS_RCODE_ENOIMP 4 +#define PICO_DNS_RCODE_EREFUSED 5 + +#define PICO_ARPA_IPV4_SUFFIX ".in-addr.arpa" + +#ifdef PICO_SUPPORT_IPV6 +#define STRLEN_PTR_IP6 63 +#define PICO_ARPA_IPV6_SUFFIX ".IP6.ARPA" +#endif + +#define PICO_DNS_NAMEBUF_SIZE (256) + +enum pico_dns_arpa +{ + PICO_DNS_ARPA4, + PICO_DNS_ARPA6, + PICO_DNS_NO_ARPA, +}; + +/* flags split in 2x uint8 due to endianness */ +PACKED_STRUCT_DEF pico_dns_header +{ + uint16_t id; /* Packet id */ + uint8_t rd : 1; /* Recursion Desired */ + uint8_t tc : 1; /* TrunCation */ + uint8_t aa : 1; /* Authoritative Answer */ + uint8_t opcode : 4; /* Opcode */ + uint8_t qr : 1; /* Query/Response */ + uint8_t rcode : 4; /* Response code */ + uint8_t z : 3; /* Zero */ + uint8_t ra : 1; /* Recursion Available */ + uint16_t qdcount; /* Question count */ + uint16_t ancount; /* Answer count */ + uint16_t nscount; /* Authority count */ + uint16_t arcount; /* Additional count */ +}; +typedef struct pico_dns_header pico_dns_packet; + +/* Question fixed-sized fields */ +PACKED_STRUCT_DEF pico_dns_question_suffix +{ + uint16_t qtype; + uint16_t qclass; +}; + +/* Resource record fixed-sized fields */ +PACKED_STRUCT_DEF pico_dns_record_suffix +{ + uint16_t rtype; + uint16_t rclass; + uint32_t rttl; + uint16_t rdlength; +}; + +/* DNS QUESTION */ +struct pico_dns_question +{ + char *qname; + struct pico_dns_question_suffix *qsuffix; + uint16_t qname_length; + uint8_t proto; +}; + +/* DNS RECORD */ +struct pico_dns_record +{ + char *rname; + struct pico_dns_record_suffix *rsuffix; + uint8_t *rdata; + uint16_t rname_length; +}; + +/* MARK: v NAME & IP FUNCTIONS */ + +/* **************************************************************************** + * Checks if the DNS name doesn't exceed 256 bytes including zero-byte. + * + * @param namelen Length of the DNS name-string including zero-byte + * @return 0 when the length is correct + * ****************************************************************************/ +int +pico_dns_check_namelen( uint16_t namelen ); + +/* **************************************************************************** + * Returns the length of a name in a DNS-packet as if DNS name compression + * would be applied to the packet. If there's no compression present this + * returns the strlen. If there's compression present this returns the length + * until the compression-pointer + 1. + * + * @param name Compressed name you want the calculate the strlen from + * @return Returns strlen of a compressed name, takes the first byte of compr- + * ession pointer into account but not the second byte, which acts + * like a trailing zero-byte. + * ****************************************************************************/ +uint16_t +pico_dns_namelen_comp( char *name ); + +/* **************************************************************************** + * Returns the uncompressed name in DNS name format when DNS name compression + * is applied to the packet-buffer. + * + * @param name Compressed name, should be in the bounds of the actual packet + * @param packet Packet that contains the compressed name + * @return Returns the decompressed name, NULL on failure. + * ****************************************************************************/ +char * +pico_dns_decompress_name( char *name, pico_dns_packet *packet ); + +/* **************************************************************************** + * Converts a DNS name in DNS name format to a name in URL format. Provides + * space for the name in URL format as well. PICO_FREE() should be called on + * the returned string buffer that contains the name in URL format. + * + * @param qname DNS name in DNS name format to convert + * @return Returns a pointer to a string-buffer with the URL name on success. + * ****************************************************************************/ +char * +pico_dns_qname_to_url( const char *qname ); + +/* **************************************************************************** + * Converts a DNS name in URL format to name in DNS name format. Provides + * space for the DNS name as well. PICO_FREE() should be called on the returned + * string buffer that contains the DNS name. + * + * @param url DNS name in URL format to convert + * @return Returns a pointer to a string-buffer with the DNS name on success. + * ****************************************************************************/ +char * +pico_dns_url_to_qname( const char *url ); + +/* **************************************************************************** + * @param url String-buffer + * @return Length of string-buffer in an uint16_t + * ****************************************************************************/ +uint16_t +pico_dns_strlen( const char *url ); + +/* **************************************************************************** + * Replaces .'s in a DNS name in URL format by the label lengths. So it + * actually converts a name in URL format to a name in DNS name format. + * f.e. "*www.google.be" => "3www6google2be0" + * + * @param url Location to buffer with name in URL format. The URL needs to + * be +1 byte offset in the actual buffer. Size is should be + * strlen(url) + 2. + * @param maxlen Maximum length of buffer so it doesn't cause a buffer overflow + * @return 0 on success, something else on failure. + * ****************************************************************************/ +int pico_dns_name_to_dns_notation( char *url, unsigned int maxlen ); + +/* **************************************************************************** + * Replaces the label lengths in a DNS-name by .'s. So it actually converts a + * name in DNS format to a name in URL format. + * f.e. 3www6google2be0 => .www.google.be + * + * @param ptr Location to buffer with name in DNS name format + * @param maxlen Maximum length of buffer so it doesn't cause a buffer overflow + * @return 0 on success, something else on failure. + * ****************************************************************************/ +int pico_dns_notation_to_name( char *ptr, unsigned int maxlen ); + +/* **************************************************************************** + * Determines the length of the first label of a DNS name in URL-format + * + * @param url DNS name in URL-format + * @return Length of the first label of DNS name in URL-format + * ****************************************************************************/ +uint16_t +pico_dns_first_label_length( const char *url ); + +/* **************************************************************************** + * Mirrors a dotted IPv4-address string. + * f.e. 192.168.0.1 => 1.0.168.192 + * + * @param ptr + * @return 0 on success, something else on failure. + * ****************************************************************************/ +int +pico_dns_mirror_addr( char *ptr ); + +/* **************************************************************************** + * Convert an IPv6-address in string-format to a IPv6-address in nibble-format. + * Doesn't add a IPv6 ARPA-suffix though. + * + * @param ip IPv6-address stored as a string + * @param dst Destination to store IPv6-address in nibble-format + * ****************************************************************************/ +void +pico_dns_ipv6_set_ptr( const char *ip, char *dst ); + +/* MARK: QUESTION FUNCTIONS */ + +/* **************************************************************************** + * Deletes a single DNS Question. + * + * @param question Void-pointer to DNS Question. Can be used with pico_tree_- + * destroy. + * @return Returns 0 on success, something else on failure. + * ****************************************************************************/ +int +pico_dns_question_delete( void **question); + +/* **************************************************************************** + * Fills in the DNS question suffix-fields with the correct values. + * + * todo: Update pico_dns_client to make the same mechanism possible as with + * filling DNS Resource Record-suffixes. This function shouldn't be an + * API-function. + * + * @param suf Pointer to the suffix member of the DNS question. + * @param qtype DNS type of the DNS question to be. + * @param qclass DNS class of the DNS question to be. + * @return Returns 0 on success, something else on failure. + * ****************************************************************************/ +int +pico_dns_question_fill_suffix( struct pico_dns_question_suffix *suf, + uint16_t qtype, + uint16_t qclass ); + +/* **************************************************************************** + * Creates a standalone DNS Question with a given name and type. + * + * @param url DNS question name in URL format. Will be converted to DNS + * name notation format. + * @param len Will be filled with the total length of the DNS question. + * @param proto Protocol for which you want to create a question. Can be + * either PICO_PROTO_IPV4 or PICO_PROTO_IPV6. + * @param qtype DNS type of the question to be. + * @param qclass DNS class of the question to be. + * @param reverse When this is true, a reverse resolution name will be gene- + * from the URL + * @return Returns pointer to the created DNS Question on success, NULL on + * failure. + * ****************************************************************************/ +struct pico_dns_question * +pico_dns_question_create( const char *url, + uint16_t *len, + uint8_t proto, + uint16_t qtype, + uint16_t qclass, + uint8_t reverse ); + +/* **************************************************************************** + * Decompresses the name of a single DNS question. + * + * @param question Question you want to decompress the name of + * @param packet Packet in which the DNS question is contained. + * @return Pointer to original name of the DNS question before decompressing. + * ****************************************************************************/ +char * +pico_dns_question_decompress( struct pico_dns_question *question, + pico_dns_packet *packet ); + +/* MARK: RESOURCE RECORD FUNCTIONS */ + +/* **************************************************************************** + * Deletes a single DNS resource record. + * + * @param record Void-pointer to DNS record. Can be used with pico_tree_destroy + * @return Returns 0 on success, something else on failure. + * ****************************************************************************/ +int +pico_dns_record_delete( void **record ); + +/* **************************************************************************** + * Just makes a hardcopy from a single DNS Resource Record + * + * @param record DNS record you want to copy + * @return Pointer to copy of DNS record. + * ****************************************************************************/ +struct pico_dns_record * +pico_dns_record_copy( struct pico_dns_record *record ); + +/* **************************************************************************** + * Create a standalone DNS Resource Record with given name, type and data. + * + * @param url DNS rrecord name in URL format. Will be converted to DNS + * name notation format. + * @param _rdata Memory buffer with data to insert in the resource record. If + * data of record should contain a DNS name, the name in the + * databuffer needs to be in URL-format. + * @param datalen The exact length in bytes of the _rdata-buffer. If data of + * record should contain a DNS name, datalen needs to be + * pico_dns_strlen(_rdata). + * @param len Will be filled with the total length of the DNS rrecord. + * @param rtype DNS type of the resource record to be. + * @param rclass DNS class of the resource record to be. + * @param rttl DNS ttl of the resource record to be. + * @return Returns pointer to the created DNS Resource Record + * ****************************************************************************/ +struct pico_dns_record * +pico_dns_record_create( const char *url, + void *_rdata, + uint16_t datalen, + uint16_t *len, + uint16_t rtype, + uint16_t rclass, + uint32_t rttl ); + +/* **************************************************************************** + * Decompresses the name of single DNS record. + * + * @param record DNS record to decompress the name of. + * @param packet Packet in which is DNS record is present + * @return Pointer to original name of the DNS record before decompressing. + * ****************************************************************************/ +char * +pico_dns_record_decompress( struct pico_dns_record *record, + pico_dns_packet *packet ); + +/* MARK: COMPARING */ + +/* **************************************************************************** + * Compares two databuffers against each other. + * + * @param a 1st Memory buffer to compare + * @param b 2nd Memory buffer to compare + * @param rdlength_a Length of 1st memory buffer + * @param rdlength_b Length of 2nd memory buffer + * @param caseinsensitive Whether or not the bytes are compared + * case-insensitive + * @return 0 when the buffers are equal, returns difference when they're not. + * ****************************************************************************/ +int +pico_dns_rdata_cmp( uint8_t *a, uint8_t *b, + uint16_t rdlength_a, uint16_t rdlength_b, uint8_t caseinsensitive ); + +/* **************************************************************************** + * Compares 2 DNS questions + * + * @param qa DNS question A as a void-pointer (for pico_tree) + * @param qb DNS question A as a void-pointer (for pico_tree) + * @return 0 when questions are equal, returns difference when they're not. + * ****************************************************************************/ +int +pico_dns_question_cmp( void *qa, + void *qb ); + +/* **************************************************************************** + * Compares 2 DNS records by type and name only + * + * @param ra DNS record A as a void-pointer (for pico_tree) + * @param rb DNS record B as a void-pointer (for pico_tree) + * @return 0 when name and type of records are equal, returns difference when + * they're not. + * ****************************************************************************/ +int +pico_dns_record_cmp_name_type( void *ra, + void *rb ); + +/* **************************************************************************** + * Compares 2 DNS records by type, name AND rdata for a truly unique result + * + * @param ra DNS record A as a void-pointer (for pico_tree) + * @param rb DNS record B as a void-pointer (for pico_tree) + * @return 0 when records are equal, returns difference when they're not + * ****************************************************************************/ +int +pico_dns_record_cmp( void *ra, + void *rb ); + +/* MARK: PICO_TREE */ + +/* **************************************************************************** + * Erases a pico_tree entirely. + * + * @param tree Pointer to a pico_tree-instance + * @param node_delete Helper-function for type-specific deleting. + * @return Returns 0 on success, something else on failure. + * ****************************************************************************/ +int +pico_tree_destroy( struct pico_tree *tree, int (*node_delete)(void **)); + +/* **************************************************************************** + * Determines the amount of nodes in a pico_tree + * + * @param tree Pointer to pico_tree-instance + * @return Amount of items in the tree. + * ****************************************************************************/ +uint16_t +pico_tree_count( struct pico_tree *tree ); + +/* **************************************************************************** + * Definition of DNS question tree + * ****************************************************************************/ +typedef struct pico_tree pico_dns_qtree; +#define PICO_DNS_QTREE_DECLARE(name) \ + pico_dns_qtree (name) = {&LEAF, pico_dns_question_cmp} +#define PICO_DNS_QTREE_DESTROY(qtree) \ + pico_tree_destroy(qtree, pico_dns_question_delete) + +/* **************************************************************************** + * Deletes all the questions with given DNS name from a pico_tree + * + * @param qtree Pointer to pico_tree-instance which contains DNS questions + * @param name Name of the questions you want to delete + * @return Returns 0 on success, something else on failure. + * ****************************************************************************/ +int +pico_dns_qtree_del_name( struct pico_tree *qtree, + const char *name ); + +/* **************************************************************************** + * Checks whether a question with given name is in the tree or not. + * + * @param qtree Pointer to pico_tree-instance which contains DNS questions + * @param name Name you want to check for + * @return 1 when the name is present in the qtree, 0 when it's not. + * ****************************************************************************/ +int +pico_dns_qtree_find_name( struct pico_tree *qtree, + const char *name ); + +/* **************************************************************************** + * Definition of DNS record tree + * ****************************************************************************/ +typedef struct pico_tree pico_dns_rtree; +#define PICO_DNS_RTREE_DECLARE(name) \ + pico_dns_rtree (name) = {&LEAF, pico_dns_record_cmp} +#define PICO_DNS_RTREE_DESTROY(rtree) \ + pico_tree_destroy((rtree), pico_dns_record_delete) + +/* MARK: DNS PACKET FUNCTIONS */ + +/* **************************************************************************** + * Fills the header section of a DNS packet with the correct flags and section + * -counts. + * + * @param hdr Header to fill in. + * @param qdcount Amount of questions added to the packet + * @param ancount Amount of answer records added to the packet + * @param nscount Amount of authority records added to the packet + * @param arcount Amount of additional records added to the packet + * ****************************************************************************/ +void +pico_dns_fill_packet_header( struct pico_dns_header *hdr, + uint16_t qdcount, + uint16_t ancount, + uint16_t authcount, + uint16_t addcount ); + +/* **************************************************************************** + * Creates a DNS Query packet with given question and resource records to put + * the Resource Record Sections. If a NULL-pointer is provided for a certain + * tree, no records will be added to that particular section of the packet. + * + * @param qtree DNS Questions to put in the Question Section + * @param antree DNS Records to put in the Answer Section + * @param nstree DNS Records to put in the Authority Section + * @param artree DNS Records to put in the Additional Section + * @param len Will get filled with the entire size of the packet + * @return Pointer to created DNS packet + * ****************************************************************************/ +pico_dns_packet * +pico_dns_query_create( struct pico_tree *qtree, + struct pico_tree *antree, + struct pico_tree *nstree, + struct pico_tree *artree, + uint16_t *len ); + +/* **************************************************************************** + * Creates a DNS Answer packet with given resource records to put in the + * Resource Record Sections. If a NULL-pointer is provided for a certain tree, + * no records will be added to that particular section of the packet. + * + * @param antree DNS Records to put in the Answer Section + * @param nstree DNS Records to put in the Authority Section + * @param artree DNS Records to put in the Additional Section + * @param len Will get filled with the entire size of the packet + * @return Pointer to created DNS packet. + * ****************************************************************************/ +pico_dns_packet * +pico_dns_answer_create( struct pico_tree *antree, + struct pico_tree *nstree, + struct pico_tree *artree, + uint16_t *len ); + +#endif /* _INCLUDE_PICO_DNS_COMMON */ diff --git a/ext/picotcp/modules/pico_dns_sd.c b/ext/picotcp/modules/pico_dns_sd.c new file mode 100644 index 0000000..1a60c2a --- /dev/null +++ b/ext/picotcp/modules/pico_dns_sd.c @@ -0,0 +1,549 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2014-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + . + Author: Jelle De Vleeschouwer + *********************************************************************/ + +#include "pico_dns_sd.h" + +#ifdef PICO_SUPPORT_DNS_SD + +/* --- Debugging --- */ +#define dns_sd_dbg(...) do {} while(0) +//#define dns_sd_dbg dbg + +/* --- PROTOTYPES --- */ +key_value_pair_t * +pico_dns_sd_kv_vector_get( kv_vector *vector, uint16_t index ); +int +pico_dns_sd_kv_vector_erase( kv_vector *vector ); +/* ------------------- */ + +typedef PACKED_STRUCT_DEF pico_dns_srv_record_prefix +{ + uint16_t priority; + uint16_t weight; + uint16_t port; +} pico_dns_srv_record; + +/* **************************************************************************** + * Determines the length of the resulting string when a string would be + * created from a key-value pair vector. + * + * @param vector Key-Value pair vector to determine the length of. + * @return The length of the key-value pair vector in bytes as if it would be + * converted to a string. + * ****************************************************************************/ +static uint16_t +pico_dns_sd_kv_vector_strlen( kv_vector *vector ) +{ + key_value_pair_t *iterator = NULL; + uint16_t i = 0, len = 0; + + /* Check params */ + if (!vector) { + pico_err = PICO_ERR_EINVAL; + return 0; + } + + /* Iterate over the key-value pairs */ + for (i = 0; i < vector->count; i++) { + iterator = pico_dns_sd_kv_vector_get(vector, i); + len = (uint16_t) (len + 1u + /* Length byte */ + strlen(iterator->key) /* Length of the key */); + if (iterator->value) + len = (uint16_t) (len + 1u /* '=' char */ + + strlen(iterator->value) /* Length of value */); + } + return len; +} + +/* **************************************************************************** + * Creates an mDNS record with the SRV record format. + * + * @param url Name of the SRV record in URL format. + * @param priority Priority, should be 0. + * @param weight Weight, should be 0. + * @param port Port to register the service on. + * @param target_url Hostname of the service-target, in URL-format + * @param ttl TTL of the SRV Record + * @param flags mDNS record flags to set specifications of the record. + * @return Pointer to newly created record on success, NULL on failure. + * ****************************************************************************/ +static struct pico_mdns_record * +pico_dns_sd_srv_record_create( const char *url, + uint16_t priority, + uint16_t weight, + uint16_t port, + const char *target_url, + uint32_t ttl, + uint8_t flags ) +{ + struct pico_mdns_record *record = NULL; + pico_dns_srv_record *srv_data = NULL; + char *target_rname = NULL; + uint16_t srv_length = 0; + + /* Check params */ + if (!url || !target_url) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + /* Determine the length the rdata buf needs to be */ + srv_length = (uint16_t) (6u + strlen(target_url) + 2u); + + /* Provide space for the data-buf */ + if (!(srv_data = (pico_dns_srv_record *) PICO_ZALLOC(srv_length))) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + /* Set the fields */ + srv_data->priority = short_be(priority); + srv_data->weight = short_be(weight); + srv_data->port = short_be(port); + + /* Copy in the URL and convert to DNS notation */ + if (!(target_rname = pico_dns_url_to_qname(target_url))) { + dns_sd_dbg("Could not convert URL to qname!\n"); + PICO_FREE(srv_data); + return NULL; + } + + strcpy((char *)srv_data + 6u, target_rname); + PICO_FREE(target_rname); + + /* Create and return new mDNS record */ + record = pico_mdns_record_create(url, srv_data, srv_length, + PICO_DNS_TYPE_SRV, + ttl, flags); + PICO_FREE(srv_data); + return record; +} + +/* **************************************************************************** + * Creates an mDNS record with the TXT record format. + * + * @param url Name of the TXT record in URL format. + * @param key_value_pairs Key-Value pair vector to generate the data from. + * @param ttl TTL of the TXT record. + * @param flags mDNS record flags to set specifications of the record + * @return Pointer to newly created record on success, NULL on failure. + * ****************************************************************************/ +static struct pico_mdns_record * +pico_dns_sd_txt_record_create( const char *url, + kv_vector key_value_pairs, + uint32_t ttl, + uint8_t flags ) +{ + struct pico_mdns_record *record = NULL; + key_value_pair_t *iterator = NULL; + char *txt = NULL; + uint16_t i = 0, txt_i = 0, pair_len = 0, key_len = 0, value_len = 0; + + /* Determine the length of the string to fit in all pairs */ + uint16_t len = (uint16_t)(pico_dns_sd_kv_vector_strlen(&key_value_pairs) + 1u); + + /* If kv-vector is empty don't bother to create a TXT record */ + if (len <= 1) + return NULL; + + /* Provide space for the txt buf */ + if (!(txt = (char *)PICO_ZALLOC(len))) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + /* Iterate over all the key-value pairs */ + for (i = 0; i < key_value_pairs.count; i++) { + iterator = pico_dns_sd_kv_vector_get(&key_value_pairs, i); + + /* Determine the length of the key */ + key_len = (uint16_t) strlen(iterator->key); + pair_len = key_len; + + /* If value is not a NULL-ptr */ + if (iterator->value) { + value_len = (uint16_t) strlen(iterator->value); + pair_len = (uint16_t) (pair_len + 1u + value_len); + } + + /* Set the pair length label */ + txt[txt_i] = (char)pair_len; + + /* Copy the key */ + strcpy(txt + txt_i + 1u, iterator->key); + + /* Copy the value if it is not a NULL-ptr */ + if (iterator->value) { + strcpy(txt + txt_i + 1u + key_len, "="); + strcpy(txt + txt_i + 2u + key_len, iterator->value); + txt_i = (uint16_t) (txt_i + 2u + key_len + value_len); + } else { + txt_i = (uint16_t) (txt_i + 1u + key_len); + } + } + record = pico_mdns_record_create(url, txt, (uint16_t)(len - 1u), PICO_DNS_TYPE_TXT, ttl, flags); + PICO_FREE(txt); + + return record; +} + +/* **************************************************************************** + * Deletes a single key-value pair instance + * + * @param kv_pair Pointer-pointer to to delete instance + * @return Returns 0 on success, something else on failure. + * ****************************************************************************/ +static int +pico_dns_sd_kv_delete( key_value_pair_t **kv_pair ) +{ + /* Check params */ + if (!kv_pair || !(*kv_pair)) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* Delete the fields */ + if ((*kv_pair)->key) + PICO_FREE((*kv_pair)->key); + + if ((*kv_pair)->value) + PICO_FREE((*kv_pair)->value); + + PICO_FREE(*kv_pair); + *kv_pair = NULL; + kv_pair = NULL; + + return 0; +} + +/* **************************************************************************** + * Creates a single key-value pair-instance + * + * @param key Key of the pair, cannot be NULL. + * @param value Value of the pair, can be NULL, empty ("") or filled ("qkejq") + * @return Pointer to newly created KV-instance on success, NULL on failure. + * ****************************************************************************/ +static key_value_pair_t * +pico_dns_sd_kv_create( const char *key, const char *value ) +{ + key_value_pair_t *kv_pair = NULL; + + /* Check params */ + if (!key || !(kv_pair = PICO_ZALLOC(sizeof(key_value_pair_t)))) { + pico_dns_sd_kv_delete(&kv_pair); + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + /* Provide space to copy the values */ + if (!(kv_pair->key = PICO_ZALLOC((size_t)(strlen(key) + 1)))) { + pico_err = PICO_ERR_ENOMEM; + pico_dns_sd_kv_delete(&kv_pair); + return NULL; + } + + strcpy(kv_pair->key, key); + + if (value) { + if (!(kv_pair->value = PICO_ZALLOC((size_t)(strlen(value) + 1)))) { + pico_err = PICO_ERR_ENOMEM; + pico_dns_sd_kv_delete(&kv_pair); + return NULL; + } + + strcpy(kv_pair->value, value); + } else + kv_pair->value = NULL; + + return kv_pair; +} + +/* **************************************************************************** + * Checks whether the type is correctly formatted ant it's label length are + * between the allowed boundaries. + * + * @param type Servicetype to check the format of. + * @return Returns 0 when the type is correctly formatted, something else when + * it's not. + * ****************************************************************************/ +static int +pico_dns_sd_check_type_format( const char *type ) +{ + uint16_t first_lbl = 0; + int8_t subtype_present = 0; + + /* Check params */ + if (!(first_lbl = pico_dns_first_label_length(type))) + return -1; + + subtype_present = !memcmp(type + first_lbl + 1, "_sub", 4); + + /* Check if there is a subtype present */ + if (subtype_present && (first_lbl > 63)) + return -1; + else if (subtype_present) + /* Get the length of the service name */ + first_lbl = pico_dns_first_label_length(type + first_lbl + 6); + else { + /* Check if type is not greater then 21 bytes (22 - 1, since the length + byte of the service name isn't included yet) */ + if (strlen(type) > (size_t) 21) + return -1; + } + + /* Check if the service name is not greater then 16 bytes (17 - 1) */ + return (first_lbl > ((uint16_t) 16u)); +} + +/* **************************************************************************** + * Checks whether the service instance name is correctly formatted and it's + * label length falls between the allowed boundaries. + * + * @param name Instance name to check the format of. + * @return Returns 0 when the name is correctly formatted, something else when + * it's not. + * ****************************************************************************/ +static int +pico_dns_sd_check_instance_name_format( const char *name ) +{ + /* First of all check if the total length is larger than 63 bytes */ + if (pico_dns_strlen(name) > 63 || !pico_dns_strlen(name)) + return -1; + + return 0; +} + +/* **************************************************************************** + * Append the instance name adn service type to create a '.local' service SIN. + * + * @param name Instance Name of the service, f.e. "Printer 2nd Floor". + * @param type ServiceType of the service, f.e. "_http._tcp". + * @return Pointer to newly created SIN on success, NULL on failure. + * ****************************************************************************/ +static char * +pico_dns_sd_create_service_url( const char *name, + const char *type ) +{ + char *url = NULL; + uint16_t len = 0, namelen = 0, typelen = 0; + + if (pico_dns_sd_check_type_format(type)) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + if (pico_dns_sd_check_instance_name_format(name)) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + namelen = (uint16_t)strlen(name); + typelen = (uint16_t)strlen(type); + + /* Determine the length that the URL needs to be */ + len = (uint16_t)(namelen + 1u /* for '.'*/ + + typelen + 7u /* for '.local\0' */); + url = (char *)PICO_ZALLOC(len); + if (!url) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + /* Append the parts together */ + strcpy(url, name); + strcpy(url + namelen, "."); + strcpy(url + namelen + 1, type); + strcpy(url + namelen + 1 + typelen, ".local"); + + return url; +} + +/* **************************************************************************** + * This function actually does exactly the same as pico_mdns_init(); + * ****************************************************************************/ +int +pico_dns_sd_init( const char *_hostname, + struct pico_ip4 address, + void (*callback)(pico_mdns_rtree *, + char *, + void *), + void *arg ) +{ + return pico_mdns_init(_hostname, address, callback, arg); +} + +/* **************************************************************************** + * Just calls pico_mdns_init in it's turn to initialise the mDNS-module. + * See pico_mdns.h for description. + * ****************************************************************************/ +int +pico_dns_sd_register_service( const char *name, + const char *type, + uint16_t port, + kv_vector *txt_data, + uint16_t ttl, + void (*callback)(pico_mdns_rtree *, + char *, + void *), + void *arg) +{ + PICO_MDNS_RTREE_DECLARE(rtree); + struct pico_mdns_record *srv_record = NULL; + struct pico_mdns_record *txt_record = NULL; + const char *hostname = pico_mdns_get_hostname(); + char *url = NULL; + + /* Try to create a service URL to create records with */ + if (!(url = pico_dns_sd_create_service_url(name, type)) || !txt_data || !hostname) { + if (url) + PICO_FREE(url); + pico_err = PICO_ERR_EINVAL; + return -1; + } + + dns_sd_dbg("\n>>>>>>>>>> Target: %s <<<<<<<<<<\n\n", hostname); + + /* Create the SRV record */ + srv_record = pico_dns_sd_srv_record_create(url, 0, 0, port, hostname, ttl, PICO_MDNS_RECORD_UNIQUE); + if (!srv_record) { + PICO_FREE(url); + return -1; + } + + /* Create the TXT record */ + txt_record = pico_dns_sd_txt_record_create(url, *txt_data, ttl, PICO_MDNS_RECORD_UNIQUE); + PICO_FREE(url); + + /* Erase the key-value pair vector, it's no longer needed */ + pico_dns_sd_kv_vector_erase(txt_data); + + if (txt_record) + pico_tree_insert(&rtree, txt_record); + + pico_tree_insert(&rtree, srv_record); + + if (pico_mdns_claim(rtree, callback, arg)) { + PICO_MDNS_RTREE_DESTROY(&rtree); + return -1; + } + pico_tree_destroy(&rtree, NULL); + return 0; +} + +/* **************************************************************************** + * Does nothing for now. + * + * @param type Type to browse for. + * @param callback Callback to call when something particular happens. + * @return When the module successfully started browsing the servicetype. + * ****************************************************************************/ +int +pico_dns_sd_browse_service( const char *type, + void (*callback)(pico_mdns_rtree *, + char *, + void *), + void *arg ) +{ + IGNORE_PARAMETER(type); + IGNORE_PARAMETER(callback); + IGNORE_PARAMETER(arg); + return 0; +} + +/* **************************************************************************** + * Add a key-value pair the a key-value pair vector. + * + * @param vector Vector to add the pair to. + * @param key Key of the pair, cannot be NULL. + * @param value Value of the pair, can be NULL, empty ("") or filled ("qkejq") + * @return Returns 0 when the pair is added successfully, something else on + * failure. + * ****************************************************************************/ +int +pico_dns_sd_kv_vector_add( kv_vector *vector, char *key, char *value ) +{ + key_value_pair_t *kv_pair = NULL; + key_value_pair_t **new_pairs = NULL; + uint16_t i = 0; + + /* Check params */ + if (!vector || !key || !(kv_pair = pico_dns_sd_kv_create(key, value))) { + pico_err = PICO_ERR_EINVAL; + pico_dns_sd_kv_delete(&kv_pair); + return -1; + } + + /* Provide enough space for the new pair pointers */ + if (!(new_pairs = PICO_ZALLOC(sizeof(key_value_pair_t *) * + (vector->count + 1u)))) { + pico_err = PICO_ERR_ENOMEM; + pico_dns_sd_kv_delete(&kv_pair); + return -1; + } + + /* Copy previous pairs and add new one */ + for (i = 0; i < vector->count; i++) + new_pairs[i] = vector->pairs[i]; + new_pairs[i] = kv_pair; + + /* Free the previous array */ + if (vector->pairs) + PICO_FREE(vector->pairs); + + vector->pairs = new_pairs; + vector->count++; + + return 0; +} + +/* **************************************************************************** + * Gets a single key-value pair form a Key-Value pair vector @ certain index. + * + * @param vector Vector to get KV-pair from. + * @param index Index of the KV-pair. + * @return key_value_pair_t* on success, NULL on failure. + * ****************************************************************************/ +key_value_pair_t * +pico_dns_sd_kv_vector_get( kv_vector *vector, uint16_t index ) +{ + /* Check params */ + if (!vector) + return NULL; + + /* Return record with conditioned index */ + if (index < vector->count) + return vector->pairs[index]; + + return NULL; +} + +/* **************************************************************************** + * Erase all the contents of a key-value pair vector. + * + * @param vector Key-Value pair vector. + * @return 0 on success, something else on failure. + * ****************************************************************************/ +int +pico_dns_sd_kv_vector_erase( kv_vector *vector ) +{ + uint16_t i = 0; + + /* Iterate over each key-value pair */ + for (i = 0; i < vector->count; i++) { + if (pico_dns_sd_kv_delete(&(vector->pairs[i])) < 0) { + dns_sd_dbg("Could not delete key-value pairs from vector"); + return -1; + } + } + PICO_FREE(vector->pairs); + vector->pairs = NULL; + vector->count = 0; + + return 0; +} + +#endif diff --git a/ext/picotcp/modules/pico_dns_sd.h b/ext/picotcp/modules/pico_dns_sd.h new file mode 100644 index 0000000..b93d599 --- /dev/null +++ b/ext/picotcp/modules/pico_dns_sd.h @@ -0,0 +1,91 @@ +/* **************************************************************************** + * PicoTCP. Copyright (c) 2014 TASS Belgium NV. Some rights reserved. + * See LICENSE and COPYING for usage. + * . + * Author: Jelle De Vleeschouwer + * ****************************************************************************/ +#ifndef INCLUDE_PICO_DNS_SD +#define INCLUDE_PICO_DNS_SD + +#include "pico_mdns.h" + +typedef struct +{ + char *key; + char *value; +} key_value_pair_t; + +typedef struct +{ + key_value_pair_t **pairs; + uint16_t count; +} kv_vector; + +#define PICO_DNS_SD_KV_VECTOR_DECLARE(name) \ + kv_vector (name) = {0} + +/* **************************************************************************** + * Just calls pico_mdns_init in it's turn to initialise the mDNS-module. + * See pico_mdns.h for description. + * ****************************************************************************/ +int +pico_dns_sd_init( const char *_hostname, + struct pico_ip4 address, + void (*callback)(pico_mdns_rtree *, + char *, + void *), + void *arg ); + +/* **************************************************************************** + * Register a DNS-SD service via Multicast DNS on the local network. + * + * @param name Instance Name of the service, f.e. "Printer 2nd Floor". + * @param type ServiceType of the service, f.e. "_http._tcp". + * @param port Port number on which the service runs. + * @param txt_data TXT data to create TXT record with, need kv_vector-type, + * Declare such a type with PICO_DNS_SD_KV_VECTOR_DECLARE(*) & + * add key-value pairs with pico_dns_sd_kv_vector_add(). + * @param ttl TTL + * @param callback Callback-function to call when the service is registered. + * @return + * ****************************************************************************/ +int +pico_dns_sd_register_service( const char *name, + const char *type, + uint16_t port, + kv_vector *txt_data, + uint16_t ttl, + void (*callback)(pico_mdns_rtree *, + char *, + void *), + void *arg); + +/* **************************************************************************** + * Does nothing for now. + * + * @param type Type to browse for. + * @param callback Callback to call when something particular happens. + * @return When the module successfully started browsing the servicetype. + * ****************************************************************************/ +int +pico_dns_sd_browse_service( const char *type, + void (*callback)(pico_mdns_rtree *, + char *, + void *), + void *arg ); + +/* **************************************************************************** + * Add a key-value pair the a key-value pair vector. + * + * @param vector Vector to add the pair to. + * @param key Key of the pair, cannot be NULL. + * @param value Value of the pair, can be NULL, empty ("") or filled ("qkejq") + * @return Returns 0 when the pair is added successfully, something else on + * failure. + * ****************************************************************************/ +int +pico_dns_sd_kv_vector_add( kv_vector *vector, char *key, char *value ); + + +#endif /* _INCLUDE_PICO_DNS_SD */ + diff --git a/ext/picotcp/modules/pico_fragments.c b/ext/picotcp/modules/pico_fragments.c new file mode 100644 index 0000000..33e69ca --- /dev/null +++ b/ext/picotcp/modules/pico_fragments.c @@ -0,0 +1,391 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Laurens Miers, Daniele Lacamera + *********************************************************************/ + + +#include "pico_config.h" +#ifdef PICO_SUPPORT_IPV6 +#include "pico_ipv6.h" +#include "pico_icmp6.h" +#endif +#ifdef PICO_SUPPORT_IPV4 +#include "pico_ipv4.h" +#include "pico_icmp4.h" +#endif +#include "pico_stack.h" +#include "pico_eth.h" +#include "pico_udp.h" +#include "pico_tcp.h" +#include "pico_socket.h" +#include "pico_device.h" +#include "pico_tree.h" +#include "pico_constants.h" +#include "pico_fragments.h" + +#define frag_dbg(...) do {} while(0) + +#if defined(PICO_SUPPORT_IPV6) && defined(PICO_SUPPORT_IPV6FRAG) +#define IP6_FRAG_OFF(x) ((x & 0xFFF8u)) +#define IP6_FRAG_MORE(x) ((x & 0x0001)) +#define IP6_FRAG_ID(x) ((uint32_t)((x->ext.frag.id[0] << 24) + (x->ext.frag.id[1] << 16) + \ + (x->ext.frag.id[2] << 8) + x->ext.frag.id[3])) +#else +#define IP6_FRAG_OFF(x) (0) +#define IP6_FRAG_MORE(x) (0) +#define IP6_FRAG_ID(x) (0) +#endif + +#if defined(PICO_SUPPORT_IPV4) && defined(PICO_SUPPORT_IPV4FRAG) +#define IP4_FRAG_OFF(frag) (((uint32_t)frag & PICO_IPV4_FRAG_MASK) << 3ul) +#define IP4_FRAG_MORE(frag) ((frag & PICO_IPV4_MOREFRAG) ? 1 : 0) +#define IP4_FRAG_ID(hdr) (hdr->id) +#else +#define IP4_FRAG_OFF(frag) (0) +#define IP4_FRAG_MORE(frag) (0) +#define IP4_FRAG_ID(hdr) (0) +#endif + +#define FRAG_OFF(net, frag) ((net == PICO_PROTO_IPV4) ? (IP4_FRAG_OFF(frag)) : (IP6_FRAG_OFF(frag))) +#define FRAG_MORE(net, frag) ((net == PICO_PROTO_IPV4) ? (IP4_FRAG_MORE(frag)) : (IP6_FRAG_MORE(frag))) + +#define PICO_IPV6_FRAG_TIMEOUT 60000 +#define PICO_IPV4_FRAG_TIMEOUT 15000 + +static void pico_frag_expire(pico_time now, void *arg); +static void pico_fragments_complete(unsigned int bookmark, uint8_t proto, uint8_t net); +static int pico_fragments_check_complete(uint8_t proto, uint8_t net); + +#if defined(PICO_SUPPORT_IPV6) && defined(PICO_SUPPORT_IPV6FRAG) +static uint32_t ipv6_cur_frag_id = 0u; +uint32_t ipv6_fragments_timer = 0u; + +static int pico_ipv6_frag_compare(void *ka, void *kb) +{ + struct pico_frame *a = ka, *b = kb; + if (IP6_FRAG_OFF(a->frag) > IP6_FRAG_OFF(b->frag)) + return 1; + + if (IP6_FRAG_OFF(a->frag) < IP6_FRAG_OFF(b->frag)) + return -1; + + return 0; +} +PICO_TREE_DECLARE(ipv6_fragments, pico_ipv6_frag_compare); + +static void pico_ipv6_fragments_complete(unsigned int len, uint8_t proto) +{ + struct pico_tree_node *index, *tmp; + struct pico_frame *f; + unsigned int bookmark = 0; + struct pico_frame *full = NULL; + struct pico_frame *first = pico_tree_first(&ipv6_fragments); + + full = pico_frame_alloc((uint16_t)(PICO_SIZE_IP6HDR + len)); + if (full) { + full->net_hdr = full->buffer; + full->net_len = PICO_SIZE_IP6HDR; + memcpy(full->net_hdr, first->net_hdr, full->net_len); + full->transport_hdr = full->net_hdr + full->net_len; + full->transport_len = (uint16_t)len; + full->dev = first->dev; + pico_tree_foreach_safe(index, &ipv6_fragments, tmp) { + f = index->keyValue; + memcpy(full->transport_hdr + bookmark, f->transport_hdr, f->transport_len); + bookmark += f->transport_len; + pico_tree_delete(&ipv6_fragments, f); + pico_frame_discard(f); + } + if (pico_transport_receive(full, proto) == -1) + { + pico_frame_discard(full); + } + pico_timer_cancel(ipv6_fragments_timer); + ipv6_fragments_timer = 0; + } +} + +static void pico_ipv6_frag_timer_on(void) +{ + ipv6_fragments_timer = pico_timer_add(PICO_IPV6_FRAG_TIMEOUT, pico_frag_expire, &ipv6_fragments); +} + +static int pico_ipv6_frag_match(struct pico_frame *a, struct pico_frame *b) +{ + struct pico_ipv6_hdr *ha, *hb; + if (!a || !b) + return 0; + + ha = (struct pico_ipv6_hdr *)a->net_hdr; + hb = (struct pico_ipv6_hdr *)b->net_hdr; + if (!ha || !hb) + return 0; + + if (memcmp(ha->src.addr, hb->src.addr, PICO_SIZE_IP6) != 0) + return 0; + + if (memcmp(ha->dst.addr, hb->dst.addr, PICO_SIZE_IP6) != 0) + return 0; + + return 1; +} +#endif + +#if defined(PICO_SUPPORT_IPV4) && defined(PICO_SUPPORT_IPV4FRAG) +static uint32_t ipv4_cur_frag_id = 0u; +uint32_t ipv4_fragments_timer = 0u; + +static int pico_ipv4_frag_compare(void *ka, void *kb) +{ + struct pico_frame *a = ka, *b = kb; + if (IP4_FRAG_OFF(a->frag) > IP4_FRAG_OFF(b->frag)) + return 1; + + if (IP4_FRAG_OFF(a->frag) < IP4_FRAG_OFF(b->frag)) + return -1; + + return 0; +} +PICO_TREE_DECLARE(ipv4_fragments, pico_ipv4_frag_compare); + +static void pico_ipv4_fragments_complete(unsigned int len, uint8_t proto) +{ + struct pico_tree_node *index, *tmp; + struct pico_frame *f; + unsigned int bookmark = 0; + struct pico_frame *full = NULL; + struct pico_frame *first = pico_tree_first(&ipv4_fragments); + + full = pico_frame_alloc((uint16_t)(PICO_SIZE_IP4HDR + len)); + if (full) { + full->net_hdr = full->buffer; + full->net_len = PICO_SIZE_IP4HDR; + memcpy(full->net_hdr, first->net_hdr, full->net_len); + full->transport_hdr = full->net_hdr + full->net_len; + full->transport_len = (uint16_t)len; + full->dev = first->dev; + pico_tree_foreach_safe(index, &ipv4_fragments, tmp) { + f = index->keyValue; + memcpy(full->transport_hdr + bookmark, f->transport_hdr, f->transport_len); + bookmark += f->transport_len; + pico_tree_delete(&ipv4_fragments, f); + pico_frame_discard(f); + } + ipv4_cur_frag_id = 0; + if (pico_transport_receive(full, proto) == -1) + { + pico_frame_discard(full); + } + pico_timer_cancel(ipv4_fragments_timer); + ipv4_fragments_timer = 0; + } +} + +static void pico_ipv4_frag_timer_on(void) +{ + ipv4_fragments_timer = pico_timer_add( PICO_IPV4_FRAG_TIMEOUT, pico_frag_expire, &ipv4_fragments); +} + +static int pico_ipv4_frag_match(struct pico_frame *a, struct pico_frame *b) +{ + struct pico_ipv4_hdr *ha, *hb; + if (!a || !b) + return 0; + + ha = (struct pico_ipv4_hdr *)a->net_hdr; + hb = (struct pico_ipv4_hdr *)b->net_hdr; + if (!ha || !hb) + return 0; + + if (memcmp(&(ha->src.addr), &(hb->src.addr), PICO_SIZE_IP4) != 0) + return 0; + + if (memcmp(&(ha->dst.addr), &(hb->dst.addr), PICO_SIZE_IP4) != 0) + return 0; + + return 1; +} +#endif + +static void pico_fragments_complete(unsigned int bookmark, uint8_t proto, uint8_t net) +{ + if (0) {} + +#if defined(PICO_SUPPORT_IPV4) && defined(PICO_SUPPORT_IPV4FRAG) + else if (net == PICO_PROTO_IPV4) + { + pico_ipv4_fragments_complete(bookmark, proto); + } +#endif +#if defined(PICO_SUPPORT_IPV6) && defined(PICO_SUPPORT_IPV6FRAG) + else if (net == PICO_PROTO_IPV6) + { + pico_ipv6_fragments_complete(bookmark, proto); + } +#endif +} + +static int pico_fragments_check_complete(uint8_t proto, uint8_t net) +{ + struct pico_tree_node *index, *temp; + struct pico_frame *cur; + unsigned int bookmark = 0; + struct pico_tree *tree = NULL; + +#if defined(PICO_SUPPORT_IPV4) && defined(PICO_SUPPORT_IPV4FRAG) + if (net == PICO_PROTO_IPV4) + { + tree = &ipv4_fragments; + } +#endif +#if defined(PICO_SUPPORT_IPV6) && defined(PICO_SUPPORT_IPV6FRAG) + if (net == PICO_PROTO_IPV6) + { + tree = &ipv6_fragments; + } +#endif + + pico_tree_foreach_safe(index, tree, temp) { + cur = index->keyValue; + if (FRAG_OFF(net, cur->frag) != bookmark) + return 0; + + bookmark += cur->transport_len; + if (!FRAG_MORE(net, cur->frag)) { + pico_fragments_complete(bookmark, proto, net); + return 1; + } + } + return 0; +} + +static void pico_frag_expire(pico_time now, void *arg) +{ + struct pico_tree_node *index, *tmp; + struct pico_frame *f = NULL; + struct pico_tree *tree = (struct pico_tree *) arg; + struct pico_frame *first = NULL; + uint8_t net = 0; + (void)now; + + if (!tree) + { + frag_dbg("Expired packet but no tree supplied!\n"); + return; + } + + first = pico_tree_first(tree); + + if (!first) { + frag_dbg("not first - not sending notify\n"); + return; + } + +#if defined(PICO_SUPPORT_IPV4) && defined(PICO_SUPPORT_IPV4FRAG) + if (IS_IPV4(first)) + { + net = PICO_PROTO_IPV4; + frag_dbg("Packet expired! ID:%hu\n", ipv4_cur_frag_id); + } +#endif +#if defined(PICO_SUPPORT_IPV6) && defined(PICO_SUPPORT_IPV6FRAG) + if (IS_IPV6(first)) + { + net = PICO_PROTO_IPV6; + frag_dbg("Packet expired! ID:%hu\n", ipv6_cur_frag_id); + } +#endif + + /* Empty the tree */ + pico_tree_foreach_safe(index, tree, tmp) { + f = index->keyValue; + pico_tree_delete(tree, f); + if (f != first) + pico_frame_discard(f); /* Later, after ICMP notification...*/ + + } + + if (((FRAG_OFF(net, first->frag) == 0) && (pico_frame_dst_is_unicast(first)))) + { + frag_dbg("sending notify\n"); + pico_notify_frag_expired(first); + } + + if (f) + pico_tree_delete(tree, f); + + pico_frame_discard(first); +} + +void pico_ipv6_process_frag(struct pico_ipv6_exthdr *frag, struct pico_frame *f, uint8_t proto) +{ +#if defined(PICO_SUPPORT_IPV6) && defined(PICO_SUPPORT_IPV6FRAG) + struct pico_frame *first = pico_tree_first(&ipv6_fragments); + + if (!first) { + if (ipv6_cur_frag_id && (IP6_FRAG_ID(frag) == ipv6_cur_frag_id)) { + /* Discard late arrivals, without firing the timer. */ + frag_dbg("discarded late arrival, exp:%hu found:%hu\n", ipv6_cur_frag_id, IP6_FRAG_ID(frag)); + return; + } + + pico_ipv6_frag_timer_on(); + ipv6_cur_frag_id = IP6_FRAG_ID(frag); + frag_dbg("Started new reassembly, ID:%hu\n", ipv6_cur_frag_id); + } + + if (!first || (pico_ipv6_frag_match(f, first) && (IP6_FRAG_ID(frag) == ipv6_cur_frag_id))) { + pico_tree_insert(&ipv6_fragments, pico_frame_copy(f)); + } + + pico_fragments_check_complete(proto, PICO_PROTO_IPV6); +#else + IGNORE_PARAMETER(frag); + IGNORE_PARAMETER(f); + IGNORE_PARAMETER(proto); +#endif +} + +void pico_ipv4_process_frag(struct pico_ipv4_hdr *hdr, struct pico_frame *f, uint8_t proto) +{ +#if defined(PICO_SUPPORT_IPV4) && defined(PICO_SUPPORT_IPV4FRAG) + struct pico_frame *first = pico_tree_first(&ipv4_fragments); + + /* fragments from old packets still in tree, and new first fragment ? */ + if (first && (IP4_FRAG_ID(hdr) != ipv4_cur_frag_id) && (IP4_FRAG_OFF(f->frag) == 0)) + { + /* Empty the tree */ + struct pico_tree_node *index, *tmp; + pico_tree_foreach_safe(index, &ipv4_fragments, tmp) { + struct pico_frame * old = index->keyValue; + pico_tree_delete(&ipv4_fragments, old); + pico_frame_discard(old); + } + first = NULL; + ipv4_cur_frag_id = 0; + } + + f->frag = short_be(hdr->frag); + if (!first) { + if (ipv4_cur_frag_id && (IP4_FRAG_ID(hdr) == ipv4_cur_frag_id)) { + /* Discard late arrivals, without firing the timer */ + return; + } + + pico_ipv4_frag_timer_on(); + ipv4_cur_frag_id = IP4_FRAG_ID(hdr); + } + + if (!first || (pico_ipv4_frag_match(f, first) && (IP4_FRAG_ID(hdr) == ipv4_cur_frag_id))) { + pico_tree_insert(&ipv4_fragments, pico_frame_copy(f)); + } + + pico_fragments_check_complete(proto, PICO_PROTO_IPV4); +#else + IGNORE_PARAMETER(hdr); + IGNORE_PARAMETER(f); + IGNORE_PARAMETER(proto); +#endif +} diff --git a/ext/picotcp/modules/pico_fragments.h b/ext/picotcp/modules/pico_fragments.h new file mode 100644 index 0000000..e51ec44 --- /dev/null +++ b/ext/picotcp/modules/pico_fragments.h @@ -0,0 +1,11 @@ +#ifndef PICO_FRAGMENTS_H +#define PICO_FRAGMENTS_H +#include "pico_ipv4.h" +#include "pico_ipv6.h" +#include "pico_addressing.h" +#include "pico_frame.h" + +void pico_ipv6_process_frag(struct pico_ipv6_exthdr *frag, struct pico_frame *f, uint8_t proto); +void pico_ipv4_process_frag(struct pico_ipv4_hdr *hdr, struct pico_frame *f, uint8_t proto); + +#endif diff --git a/ext/picotcp/modules/pico_hotplug_detection.c b/ext/picotcp/modules/pico_hotplug_detection.c new file mode 100644 index 0000000..732b4d7 --- /dev/null +++ b/ext/picotcp/modules/pico_hotplug_detection.c @@ -0,0 +1,134 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Frederik Van Slycken + *********************************************************************/ +#include "pico_protocol.h" +#include "pico_hotplug_detection.h" +#include "pico_tree.h" +#include "pico_device.h" + +struct pico_hotplug_device{ + struct pico_device *dev; + int prev_state; + struct pico_tree callbacks; +}; + +uint32_t timer_id = 0; + +static int pico_hotplug_dev_cmp(void *ka, void *kb) +{ + struct pico_hotplug_device *a = ka, *b = kb; + if (a->dev->hash < b->dev->hash) + return -1; + + if (a->dev->hash > b->dev->hash) + return 1; + + return 0; +} + +static int callback_compare(void *ka, void *kb) +{ + if (ka < kb) + return -1; + if (ka > kb) + return 1; + return 0; +} + +PICO_TREE_DECLARE(Hotplug_device_tree, pico_hotplug_dev_cmp); + +static void timer_cb(__attribute__((unused)) pico_time t, __attribute__((unused)) void* v) +{ + struct pico_tree_node *node = NULL, *safe = NULL, *cb_node = NULL, *cb_safe = NULL; + int new_state, event; + struct pico_hotplug_device *hpdev = NULL; + void (*cb)(struct pico_device *dev, int event); + + //we don't know if one of the callbacks might deregister, so be safe + pico_tree_foreach_safe(node, &Hotplug_device_tree, safe) + { + hpdev = node->keyValue; + new_state = hpdev->dev->link_state(hpdev->dev); + if (new_state != hpdev->prev_state) + { + if (new_state == 1){ + event = PICO_HOTPLUG_EVENT_UP; + } else { + event = PICO_HOTPLUG_EVENT_DOWN; + } + //we don't know if one of the callbacks might deregister, so be safe + pico_tree_foreach_safe(cb_node, &(hpdev->callbacks), cb_safe) + { + cb = cb_node->keyValue; + cb(hpdev->dev, event); + } + hpdev->prev_state = new_state; + } + } + + timer_id = pico_timer_add(PICO_HOTPLUG_INTERVAL, &timer_cb, NULL); +} + + +int pico_hotplug_register(struct pico_device *dev, void (*cb)(struct pico_device *dev, int event)) +{ + struct pico_hotplug_device *hotplug_dev; + struct pico_hotplug_device search = {.dev = dev}; + + if (dev->link_state == NULL){ + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; + } + + hotplug_dev = (struct pico_hotplug_device*)pico_tree_findKey(&Hotplug_device_tree, &search); + if (! hotplug_dev ) + { + hotplug_dev = PICO_ZALLOC(sizeof(struct pico_hotplug_device)); + if (!hotplug_dev) + { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + hotplug_dev->dev = dev; + hotplug_dev->prev_state = dev->link_state(hotplug_dev->dev); + hotplug_dev->callbacks.root = &LEAF; + hotplug_dev->callbacks.compare = &callback_compare; + pico_tree_insert(&Hotplug_device_tree, hotplug_dev); + } + pico_tree_insert(&(hotplug_dev->callbacks), cb); + + if (timer_id == 0) + { + timer_id = pico_timer_add(PICO_HOTPLUG_INTERVAL, &timer_cb, NULL); + } + + return 0; +} + +int pico_hotplug_deregister(struct pico_device *dev, void (*cb)(struct pico_device *dev, int event)) +{ + struct pico_hotplug_device* hotplug_dev; + struct pico_hotplug_device search = {.dev = dev}; + + hotplug_dev = (struct pico_hotplug_device*)pico_tree_findKey(&Hotplug_device_tree, &search); + if (!hotplug_dev) + //wasn't registered + return 0; + pico_tree_delete(&hotplug_dev->callbacks, cb); + if (pico_tree_empty(&hotplug_dev->callbacks)) + { + pico_tree_delete(&Hotplug_device_tree, hotplug_dev); + PICO_FREE(hotplug_dev); + } + + if (pico_tree_empty(&Hotplug_device_tree) && timer_id != 0) + { + pico_timer_cancel(timer_id); + timer_id = 0; + } + return 0; +} + diff --git a/ext/picotcp/modules/pico_hotplug_detection.h b/ext/picotcp/modules/pico_hotplug_detection.h new file mode 100644 index 0000000..19319b4 --- /dev/null +++ b/ext/picotcp/modules/pico_hotplug_detection.h @@ -0,0 +1,20 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Frederik Van Slycken + *********************************************************************/ +#ifndef INCLUDE_PICO_SUPPORT_HOTPLUG +#define INCLUDE_PICO_SUPPORT_HOTPLUG +#include "pico_stack.h" + +#define PICO_HOTPLUG_EVENT_UP 1 /* link went up */ +#define PICO_HOTPLUG_EVENT_DOWN 2 /* link went down */ + +#define PICO_HOTPLUG_INTERVAL 100 + +int pico_hotplug_register(struct pico_device *dev, void (*cb)(struct pico_device *dev, int event)); +int pico_hotplug_deregister(struct pico_device *dev, void (*cb)(struct pico_device *dev, int event)); + +#endif /* _INCLUDE_PICO_SUPPORT_HOTPLUG */ + diff --git a/ext/picotcp/modules/pico_icmp4.c b/ext/picotcp/modules/pico_icmp4.c new file mode 100644 index 0000000..da3d89c --- /dev/null +++ b/ext/picotcp/modules/pico_icmp4.c @@ -0,0 +1,395 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + Authors: Daniele Lacamera + *********************************************************************/ + + +#include "pico_icmp4.h" +#include "pico_config.h" +#include "pico_ipv4.h" +#include "pico_eth.h" +#include "pico_device.h" +#include "pico_stack.h" +#include "pico_tree.h" + +/* Queues */ +static struct pico_queue icmp_in = { + 0 +}; +static struct pico_queue icmp_out = { + 0 +}; + + +/* Functions */ + +static int pico_icmp4_checksum(struct pico_frame *f) +{ + struct pico_icmp4_hdr *hdr = (struct pico_icmp4_hdr *) f->transport_hdr; + if (!hdr) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + hdr->crc = 0; + hdr->crc = short_be(pico_checksum(hdr, f->transport_len)); + return 0; +} + +#ifdef PICO_SUPPORT_PING +static void ping_recv_reply(struct pico_frame *f); +#endif + +static int pico_icmp4_process_in(struct pico_protocol *self, struct pico_frame *f) +{ + struct pico_icmp4_hdr *hdr = (struct pico_icmp4_hdr *) f->transport_hdr; + static int firstpkt = 1; + static uint16_t last_id = 0; + static uint16_t last_seq = 0; + IGNORE_PARAMETER(self); + + if (hdr->type == PICO_ICMP_ECHO) { + hdr->type = PICO_ICMP_ECHOREPLY; + /* outgoing frames require a f->len without the ethernet header len */ + if (f->dev && f->dev->eth) + f->len -= PICO_SIZE_ETHHDR; + + if (!firstpkt && (hdr->hun.ih_idseq.idseq_id == last_id) && (last_seq == hdr->hun.ih_idseq.idseq_seq)) { + /* The network duplicated the echo. Do not reply. */ + pico_frame_discard(f); + return 0; + } + + firstpkt = 0; + last_id = hdr->hun.ih_idseq.idseq_id; + last_seq = hdr->hun.ih_idseq.idseq_seq; + pico_icmp4_checksum(f); + pico_ipv4_rebound(f); + } else if (hdr->type == PICO_ICMP_UNREACH) { + f->net_hdr = f->transport_hdr + PICO_ICMPHDR_UN_SIZE; + pico_ipv4_unreachable(f, hdr->code); + } else if (hdr->type == PICO_ICMP_ECHOREPLY) { +#ifdef PICO_SUPPORT_PING + ping_recv_reply(f); +#endif + pico_frame_discard(f); + } else { + pico_frame_discard(f); + } + + return 0; +} + +static int pico_icmp4_process_out(struct pico_protocol *self, struct pico_frame *f) +{ + IGNORE_PARAMETER(self); + IGNORE_PARAMETER(f); + dbg("Called %s\n", __FUNCTION__); + return 0; +} + +/* Interface: protocol definition */ +struct pico_protocol pico_proto_icmp4 = { + .name = "icmp4", + .proto_number = PICO_PROTO_ICMP4, + .layer = PICO_LAYER_TRANSPORT, + .process_in = pico_icmp4_process_in, + .process_out = pico_icmp4_process_out, + .q_in = &icmp_in, + .q_out = &icmp_out, +}; + +static int pico_icmp4_notify(struct pico_frame *f, uint8_t type, uint8_t code) +{ + struct pico_frame *reply; + struct pico_icmp4_hdr *hdr; + struct pico_ipv4_hdr *info; + uint16_t f_tot_len; + + f_tot_len = short_be(((struct pico_ipv4_hdr *)f->net_hdr)->len); + + if (f_tot_len < (sizeof(struct pico_ipv4_hdr))) + return -1; + + /* Truncate tot len to be at most 8 bytes + iphdr */ + if (f_tot_len > (sizeof(struct pico_ipv4_hdr) + 8u)) { + f_tot_len = (sizeof(struct pico_ipv4_hdr) + 8u); + } + + if (f == NULL) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + reply = pico_proto_ipv4.alloc(&pico_proto_ipv4, (uint16_t) (f_tot_len + PICO_ICMPHDR_UN_SIZE)); + info = (struct pico_ipv4_hdr*)(f->net_hdr); + hdr = (struct pico_icmp4_hdr *) reply->transport_hdr; + hdr->type = type; + hdr->code = code; + hdr->hun.ih_pmtu.ipm_nmtu = short_be(1500); + hdr->hun.ih_pmtu.ipm_void = 0; + reply->transport_len = (uint16_t)(f_tot_len + PICO_ICMPHDR_UN_SIZE); + reply->payload = reply->transport_hdr + PICO_ICMPHDR_UN_SIZE; + memcpy(reply->payload, f->net_hdr, f_tot_len); + pico_icmp4_checksum(reply); + pico_ipv4_frame_push(reply, &info->src, PICO_PROTO_ICMP4); + return 0; +} + +int pico_icmp4_port_unreachable(struct pico_frame *f) +{ + /*Parameter check executed in pico_icmp4_notify*/ + return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_PORT); +} + +int pico_icmp4_proto_unreachable(struct pico_frame *f) +{ + /*Parameter check executed in pico_icmp4_notify*/ + return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_PROTOCOL); +} + +int pico_icmp4_dest_unreachable(struct pico_frame *f) +{ + /*Parameter check executed in pico_icmp4_notify*/ + return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_HOST); +} + +int pico_icmp4_ttl_expired(struct pico_frame *f) +{ + /*Parameter check executed in pico_icmp4_notify*/ + return pico_icmp4_notify(f, PICO_ICMP_TIME_EXCEEDED, PICO_ICMP_TIMXCEED_INTRANS); +} + +MOCKABLE int pico_icmp4_frag_expired(struct pico_frame *f) +{ + /*Parameter check executed in pico_icmp4_notify*/ + return pico_icmp4_notify(f, PICO_ICMP_TIME_EXCEEDED, PICO_ICMP_TIMXCEED_REASS); +} + +int pico_icmp4_mtu_exceeded(struct pico_frame *f) +{ + /*Parameter check executed in pico_icmp4_notify*/ + return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_NEEDFRAG); +} + +int pico_icmp4_packet_filtered(struct pico_frame *f) +{ + /*Parameter check executed in pico_icmp4_notify*/ + /*Packet Filtered: type 3, code 13 (Communication Administratively Prohibited)*/ + return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_FILTER_PROHIB); +} + +int pico_icmp4_param_problem(struct pico_frame *f, uint8_t code) +{ + return pico_icmp4_notify(f, PICO_ICMP_PARAMPROB, code); +} + +/***********************/ +/* Ping implementation */ +/***********************/ +/***********************/ +/***********************/ +/***********************/ + + +#ifdef PICO_SUPPORT_PING + + +struct pico_icmp4_ping_cookie +{ + struct pico_ip4 dst; + uint16_t err; + uint16_t id; + uint16_t seq; + uint16_t size; + int count; + pico_time timestamp; + int interval; + int timeout; + void (*cb)(struct pico_icmp4_stats*); +}; + +static int cookie_compare(void *ka, void *kb) +{ + struct pico_icmp4_ping_cookie *a = ka, *b = kb; + if (a->id < b->id) + return -1; + + if (a->id > b->id) + return 1; + + return (a->seq - b->seq); +} + +PICO_TREE_DECLARE(Pings, cookie_compare); + +static int8_t pico_icmp4_send_echo(struct pico_icmp4_ping_cookie *cookie) +{ + struct pico_frame *echo = pico_proto_ipv4.alloc(&pico_proto_ipv4, (uint16_t)(PICO_ICMPHDR_UN_SIZE + cookie->size)); + struct pico_icmp4_hdr *hdr; + if (!echo) { + return -1; + } + + hdr = (struct pico_icmp4_hdr *) echo->transport_hdr; + + hdr->type = PICO_ICMP_ECHO; + hdr->code = 0; + hdr->hun.ih_idseq.idseq_id = short_be(cookie->id); + hdr->hun.ih_idseq.idseq_seq = short_be(cookie->seq); + echo->transport_len = (uint16_t)(PICO_ICMPHDR_UN_SIZE + cookie->size); + echo->payload = echo->transport_hdr + PICO_ICMPHDR_UN_SIZE; + echo->payload_len = cookie->size; + /* XXX: Fill payload */ + pico_icmp4_checksum(echo); + pico_ipv4_frame_push(echo, &cookie->dst, PICO_PROTO_ICMP4); + return 0; +} + + +static void ping_timeout(pico_time now, void *arg) +{ + struct pico_icmp4_ping_cookie *cookie = (struct pico_icmp4_ping_cookie *)arg; + IGNORE_PARAMETER(now); + + if(pico_tree_findKey(&Pings, cookie)) { + if (cookie->err == PICO_PING_ERR_PENDING) { + struct pico_icmp4_stats stats; + stats.dst = cookie->dst; + stats.seq = cookie->seq; + stats.time = 0; + stats.size = cookie->size; + stats.err = PICO_PING_ERR_TIMEOUT; + dbg(" ---- Ping timeout!!!\n"); + cookie->cb(&stats); + } + + pico_tree_delete(&Pings, cookie); + PICO_FREE(cookie); + } +} + +static void next_ping(pico_time now, void *arg); +static inline void send_ping(struct pico_icmp4_ping_cookie *cookie) +{ + pico_icmp4_send_echo(cookie); + cookie->timestamp = pico_tick; + pico_timer_add((uint32_t)cookie->timeout, ping_timeout, cookie); + if (cookie->seq < (uint16_t)cookie->count) + pico_timer_add((uint32_t)cookie->interval, next_ping, cookie); +} + +static void next_ping(pico_time now, void *arg) +{ + struct pico_icmp4_ping_cookie *newcookie, *cookie = (struct pico_icmp4_ping_cookie *)arg; + IGNORE_PARAMETER(now); + + if(pico_tree_findKey(&Pings, cookie)) { + if (cookie->err == PICO_PING_ERR_ABORTED) + return; + + if (cookie->seq < (uint16_t)cookie->count) { + newcookie = PICO_ZALLOC(sizeof(struct pico_icmp4_ping_cookie)); + if (!newcookie) + return; + + memcpy(newcookie, cookie, sizeof(struct pico_icmp4_ping_cookie)); + newcookie->seq++; + + pico_tree_insert(&Pings, newcookie); + send_ping(newcookie); + } + } +} + + +static void ping_recv_reply(struct pico_frame *f) +{ + struct pico_icmp4_ping_cookie test, *cookie; + struct pico_icmp4_hdr *hdr = (struct pico_icmp4_hdr *) f->transport_hdr; + test.id = short_be(hdr->hun.ih_idseq.idseq_id ); + test.seq = short_be(hdr->hun.ih_idseq.idseq_seq); + + cookie = pico_tree_findKey(&Pings, &test); + if (cookie) { + struct pico_icmp4_stats stats; + if (cookie->err == PICO_PING_ERR_ABORTED) + return; + + cookie->err = PICO_PING_ERR_REPLIED; + stats.dst = ((struct pico_ipv4_hdr *)f->net_hdr)->src; + stats.seq = cookie->seq; + stats.size = cookie->size; + stats.time = pico_tick - cookie->timestamp; + stats.err = cookie->err; + stats.ttl = ((struct pico_ipv4_hdr *)f->net_hdr)->ttl; + if(cookie->cb != NULL) + cookie->cb(&stats); + } else { + dbg("Reply for seq=%d, not found.\n", test.seq); + } +} + +int pico_icmp4_ping(char *dst, int count, int interval, int timeout, int size, void (*cb)(struct pico_icmp4_stats *)) +{ + static uint16_t next_id = 0x91c0; + struct pico_icmp4_ping_cookie *cookie; + + if((dst == NULL) || (interval == 0) || (timeout == 0) || (count == 0)) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + cookie = PICO_ZALLOC(sizeof(struct pico_icmp4_ping_cookie)); + if (!cookie) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + if (pico_string_to_ipv4(dst, (uint32_t *)&cookie->dst.addr) < 0) { + pico_err = PICO_ERR_EINVAL; + PICO_FREE(cookie); + return -1; + } + + cookie->seq = 1; + cookie->id = next_id++; + cookie->err = PICO_PING_ERR_PENDING; + cookie->size = (uint16_t)size; + cookie->interval = interval; + cookie->timeout = timeout; + cookie->cb = cb; + cookie->count = count; + + pico_tree_insert(&Pings, cookie); + send_ping(cookie); + + return cookie->id; + +} + +int pico_icmp4_ping_abort(int id) +{ + struct pico_tree_node *node; + int found = 0; + pico_tree_foreach(node, &Pings) + { + struct pico_icmp4_ping_cookie *ck = + (struct pico_icmp4_ping_cookie *) node->keyValue; + if (ck->id == (uint16_t)id) { + ck->err = PICO_PING_ERR_ABORTED; + found++; + } + } + if (found > 0) + return 0; /* OK if at least one pending ping has been canceled */ + + pico_err = PICO_ERR_ENOENT; + return -1; +} + +#endif diff --git a/ext/picotcp/modules/pico_icmp4.h b/ext/picotcp/modules/pico_icmp4.h new file mode 100644 index 0000000..45f0a62 --- /dev/null +++ b/ext/picotcp/modules/pico_icmp4.h @@ -0,0 +1,162 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + *********************************************************************/ +#ifndef INCLUDE_PICO_ICMP4 +#define INCLUDE_PICO_ICMP4 +#include "pico_defines.h" +#include "pico_addressing.h" +#include "pico_protocol.h" + + +extern struct pico_protocol pico_proto_icmp4; + +PACKED_STRUCT_DEF pico_icmp4_hdr { + uint8_t type; + uint8_t code; + uint16_t crc; + + /* hun */ + PACKED_UNION_DEF hun_u { + uint8_t ih_pptr; + struct pico_ip4 ih_gwaddr; + PEDANTIC_STRUCT_DEF ih_idseq_s { + uint16_t idseq_id; + uint16_t idseq_seq; + } ih_idseq; + uint32_t ih_void; + PEDANTIC_STRUCT_DEF ih_pmtu_s { + uint16_t ipm_void; + uint16_t ipm_nmtu; + } ih_pmtu; + PEDANTIC_STRUCT_DEF ih_rta_s { + uint8_t rta_numgw; + uint8_t rta_wpa; + uint16_t rta_lifetime; + } ih_rta; + } hun; + + /* dun */ + PACKED_UNION_DEF dun_u { + PEDANTIC_STRUCT_DEF id_ts_s { + uint32_t ts_otime; + uint32_t ts_rtime; + uint32_t ts_ttime; + } id_ts; + PEDANTIC_STRUCT_DEF id_ip_s { + uint32_t ip_options; + uint32_t ip_data_hi; + uint32_t ip_data_lo; + } id_ip; + PEDANTIC_STRUCT_DEF id_ra_s { + uint32_t ira_addr; + uint32_t ira_pref; + } id_ra; + uint32_t id_mask; + uint8_t id_data[1]; + } dun; +}; + +#define PICO_ICMPHDR_DRY_SIZE 4 +#define PICO_ICMPHDR_UN_SIZE 8u + +#define PICO_ICMP_ECHOREPLY 0 +#define PICO_ICMP_DEST_UNREACH 3 +#define PICO_ICMP_SOURCE_QUENCH 4 +#define PICO_ICMP_REDIRECT 5 +#define PICO_ICMP_ECHO 8 +#define PICO_ICMP_TIME_EXCEEDED 11 +#define PICO_ICMP_PARAMETERPROB 12 +#define PICO_ICMP_TIMESTAMP 13 +#define PICO_ICMP_TIMESTAMPREPLY 14 +#define PICO_ICMP_INFO_REQUEST 15 +#define PICO_ICMP_INFO_REPLY 16 +#define PICO_ICMP_ADDRESS 17 +#define PICO_ICMP_ADDRESSREPLY 18 + + +#define PICO_ICMP_UNREACH 3 +#define PICO_ICMP_SOURCEQUENCH 4 +#define PICO_ICMP_ROUTERADVERT 9 +#define PICO_ICMP_ROUTERSOLICIT 10 +#define PICO_ICMP_TIMXCEED 11 +#define PICO_ICMP_PARAMPROB 12 +#define PICO_ICMP_TSTAMP 13 +#define PICO_ICMP_TSTAMPREPLY 14 +#define PICO_ICMP_IREQ 15 +#define PICO_ICMP_IREQREPLY 16 +#define PICO_ICMP_MASKREQ 17 +#define PICO_ICMP_MASKREPLY 18 + +#define PICO_ICMP_MAXTYPE 18 + + +#define PICO_ICMP_UNREACH_NET 0 +#define PICO_ICMP_UNREACH_HOST 1 +#define PICO_ICMP_UNREACH_PROTOCOL 2 +#define PICO_ICMP_UNREACH_PORT 3 +#define PICO_ICMP_UNREACH_NEEDFRAG 4 +#define PICO_ICMP_UNREACH_SRCFAIL 5 +#define PICO_ICMP_UNREACH_NET_UNKNOWN 6 +#define PICO_ICMP_UNREACH_HOST_UNKNOWN 7 +#define PICO_ICMP_UNREACH_ISOLATED 8 +#define PICO_ICMP_UNREACH_NET_PROHIB 9 +#define PICO_ICMP_UNREACH_HOST_PROHIB 10 +#define PICO_ICMP_UNREACH_TOSNET 11 +#define PICO_ICMP_UNREACH_TOSHOST 12 +#define PICO_ICMP_UNREACH_FILTER_PROHIB 13 +#define PICO_ICMP_UNREACH_HOST_PRECEDENCE 14 +#define PICO_ICMP_UNREACH_PRECEDENCE_CUTOFF 15 + + +#define PICO_ICMP_REDIRECT_NET 0 +#define PICO_ICMP_REDIRECT_HOST 1 +#define PICO_ICMP_REDIRECT_TOSNET 2 +#define PICO_ICMP_REDIRECT_TOSHOST 3 + + +#define PICO_ICMP_TIMXCEED_INTRANS 0 +#define PICO_ICMP_TIMXCEED_REASS 1 + + +#define PICO_ICMP_PARAMPROB_OPTABSENT 1 + +#define PICO_SIZE_ICMP4HDR ((sizeof(struct pico_icmp4_hdr))) + +struct pico_icmp4_stats +{ + struct pico_ip4 dst; + unsigned long size; + unsigned long seq; + pico_time time; + unsigned long ttl; + int err; +}; + +int pico_icmp4_port_unreachable(struct pico_frame *f); +int pico_icmp4_proto_unreachable(struct pico_frame *f); +int pico_icmp4_dest_unreachable(struct pico_frame *f); +int pico_icmp4_mtu_exceeded(struct pico_frame *f); +int pico_icmp4_ttl_expired(struct pico_frame *f); +int pico_icmp4_frag_expired(struct pico_frame *f); +int pico_icmp4_ping(char *dst, int count, int interval, int timeout, int size, void (*cb)(struct pico_icmp4_stats *)); +int pico_icmp4_ping_abort(int id); + +#ifdef PICO_SUPPORT_ICMP4 +int pico_icmp4_packet_filtered(struct pico_frame *f); +int pico_icmp4_param_problem(struct pico_frame *f, uint8_t code); +#else +# define pico_icmp4_packet_filtered(f) (-1) +# define pico_icmp4_param_problem(f, c) (-1) +#endif /* PICO_SUPPORT_ICMP4 */ + +#define PICO_PING_ERR_REPLIED 0 +#define PICO_PING_ERR_TIMEOUT 1 +#define PICO_PING_ERR_UNREACH 2 +#define PICO_PING_ERR_ABORTED 3 +#define PICO_PING_ERR_PENDING 0xFFFF + +#endif diff --git a/ext/picotcp/modules/pico_icmp6.c b/ext/picotcp/modules/pico_icmp6.c new file mode 100644 index 0000000..3c46e7e --- /dev/null +++ b/ext/picotcp/modules/pico_icmp6.c @@ -0,0 +1,698 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + Authors: Kristof Roelants, Daniele Lacamera + *********************************************************************/ + +#include "pico_config.h" +#include "pico_icmp6.h" +#include "pico_ipv6_nd.h" +#include "pico_eth.h" +#include "pico_device.h" +#include "pico_stack.h" +#include "pico_tree.h" +#include "pico_socket.h" +#include "pico_mld.h" +#define icmp6_dbg(...) do { }while(0); + +static struct pico_queue icmp6_in; +static struct pico_queue icmp6_out; + +uint16_t pico_icmp6_checksum(struct pico_frame *f) +{ + struct pico_ipv6_hdr *ipv6_hdr = (struct pico_ipv6_hdr *)f->net_hdr; + + struct pico_icmp6_hdr *icmp6_hdr = (struct pico_icmp6_hdr *)f->transport_hdr; + struct pico_ipv6_pseudo_hdr pseudo; + + pseudo.src = ipv6_hdr->src; + pseudo.dst = ipv6_hdr->dst; + pseudo.len = long_be(f->transport_len); + pseudo.nxthdr = PICO_PROTO_ICMP6; + + pseudo.zero[0] = 0; + pseudo.zero[1] = 0; + pseudo.zero[2] = 0; + + return pico_dualbuffer_checksum(&pseudo, sizeof(struct pico_ipv6_pseudo_hdr), icmp6_hdr, f->transport_len); +} + +#ifdef PICO_SUPPORT_PING +static void pico_icmp6_ping_recv_reply(struct pico_frame *f); +#endif + +static int pico_icmp6_send_echoreply(struct pico_frame *echo) +{ + struct pico_frame *reply = NULL; + struct pico_icmp6_hdr *ehdr = NULL, *rhdr = NULL; + struct pico_ip6 src; + struct pico_ip6 dst; + + reply = pico_proto_ipv6.alloc(&pico_proto_ipv6, (uint16_t)(echo->transport_len)); + if (!reply) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + echo->payload = echo->transport_hdr + PICO_ICMP6HDR_ECHO_REQUEST_SIZE; + reply->payload = reply->transport_hdr + PICO_ICMP6HDR_ECHO_REQUEST_SIZE; + reply->payload_len = echo->transport_len; + reply->dev = echo->dev; + + ehdr = (struct pico_icmp6_hdr *)echo->transport_hdr; + rhdr = (struct pico_icmp6_hdr *)reply->transport_hdr; + rhdr->type = PICO_ICMP6_ECHO_REPLY; + rhdr->code = 0; + rhdr->msg.info.echo_reply.id = ehdr->msg.info.echo_reply.id; + rhdr->msg.info.echo_reply.seq = ehdr->msg.info.echo_request.seq; + memcpy(reply->payload, echo->payload, (uint32_t)(echo->transport_len - PICO_ICMP6HDR_ECHO_REQUEST_SIZE)); + rhdr->crc = 0; + rhdr->crc = short_be(pico_icmp6_checksum(reply)); + /* Get destination and source swapped */ + memcpy(dst.addr, ((struct pico_ipv6_hdr *)echo->net_hdr)->src.addr, PICO_SIZE_IP6); + memcpy(src.addr, ((struct pico_ipv6_hdr *)echo->net_hdr)->dst.addr, PICO_SIZE_IP6); + pico_ipv6_frame_push(reply, &src, &dst, PICO_PROTO_ICMP6, 0); + return 0; +} + +static int pico_icmp6_process_in(struct pico_protocol *self, struct pico_frame *f) +{ + struct pico_icmp6_hdr *hdr = (struct pico_icmp6_hdr *)f->transport_hdr; + + IGNORE_PARAMETER(self); + + icmp6_dbg("Process IN, type = %d\n", hdr->type); + + switch (hdr->type) + { + case PICO_ICMP6_DEST_UNREACH: + pico_ipv6_unreachable(f, hdr->code); + break; + + case PICO_ICMP6_ECHO_REQUEST: + icmp6_dbg("ICMP6: Received ECHO REQ\n"); + f->transport_len = (uint16_t)(f->len - f->net_len - (uint16_t)(f->net_hdr - f->buffer)); + pico_icmp6_send_echoreply(f); + pico_frame_discard(f); + break; + + case PICO_ICMP6_ECHO_REPLY: +#ifdef PICO_SUPPORT_PING + pico_icmp6_ping_recv_reply(f); +#endif + pico_frame_discard(f); + break; +#ifdef PICO_SUPPORT_MCAST + case PICO_MLD_QUERY: + case PICO_MLD_REPORT: + case PICO_MLD_DONE: + case PICO_MLD_REPORTV2: + pico_mld_process_in(f); + break; +#endif + default: + return pico_ipv6_nd_recv(f); /* CAUTION -- Implies: pico_frame_discard in any case, keep in the default! */ + } + return -1; +} + +static int pico_icmp6_process_out(struct pico_protocol *self, struct pico_frame *f) +{ + IGNORE_PARAMETER(self); + IGNORE_PARAMETER(f); + return 0; +} + +/* Interface: protocol definition */ +struct pico_protocol pico_proto_icmp6 = { + .name = "icmp6", + .proto_number = PICO_PROTO_ICMP6, + .layer = PICO_LAYER_TRANSPORT, + .process_in = pico_icmp6_process_in, + .process_out = pico_icmp6_process_out, + .q_in = &icmp6_in, + .q_out = &icmp6_out, +}; + +static int pico_icmp6_notify(struct pico_frame *f, uint8_t type, uint8_t code, uint32_t ptr) +{ + struct pico_frame *notice = NULL; + struct pico_ipv6_hdr *ipv6_hdr = NULL; + struct pico_icmp6_hdr *icmp6_hdr = NULL; + uint16_t len = 0; + + if (!f) + return -1; + + ipv6_hdr = (struct pico_ipv6_hdr *)(f->net_hdr); + len = (uint16_t)(short_be(ipv6_hdr->len) + PICO_SIZE_IP6HDR); + switch (type) + { + case PICO_ICMP6_DEST_UNREACH: + /* as much of invoking packet as possible without exceeding the minimum IPv6 MTU */ + if (PICO_SIZE_IP6HDR + PICO_ICMP6HDR_DEST_UNREACH_SIZE + len > PICO_IPV6_MIN_MTU) + len = PICO_IPV6_MIN_MTU - (PICO_SIZE_IP6HDR + PICO_ICMP6HDR_DEST_UNREACH_SIZE); + + notice = pico_proto_ipv6.alloc(&pico_proto_ipv6, (uint16_t)(PICO_ICMP6HDR_DEST_UNREACH_SIZE + len)); + if (!notice) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + notice->payload = notice->transport_hdr + PICO_ICMP6HDR_DEST_UNREACH_SIZE; + notice->payload_len = len; + icmp6_hdr = (struct pico_icmp6_hdr *)notice->transport_hdr; + icmp6_hdr->msg.err.dest_unreach.unused = 0; + break; + + case PICO_ICMP6_TIME_EXCEEDED: + /* as much of invoking packet as possible without exceeding the minimum IPv6 MTU */ + if (PICO_SIZE_IP6HDR + PICO_ICMP6HDR_TIME_XCEEDED_SIZE + len > PICO_IPV6_MIN_MTU) + len = PICO_IPV6_MIN_MTU - (PICO_SIZE_IP6HDR + PICO_ICMP6HDR_TIME_XCEEDED_SIZE); + + notice = pico_proto_ipv6.alloc(&pico_proto_ipv6, (uint16_t)(PICO_ICMP6HDR_TIME_XCEEDED_SIZE + len)); + if (!notice) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + notice->payload = notice->transport_hdr + PICO_ICMP6HDR_TIME_XCEEDED_SIZE; + notice->payload_len = len; + icmp6_hdr = (struct pico_icmp6_hdr *)notice->transport_hdr; + icmp6_hdr->msg.err.time_exceeded.unused = 0; + break; + + case PICO_ICMP6_PARAM_PROBLEM: + if (PICO_SIZE_IP6HDR + PICO_ICMP6HDR_PARAM_PROBLEM_SIZE + len > PICO_IPV6_MIN_MTU) + len = PICO_IPV6_MIN_MTU - (PICO_SIZE_IP6HDR + PICO_ICMP6HDR_PARAM_PROBLEM_SIZE); + + notice = pico_proto_ipv6.alloc(&pico_proto_ipv6, (uint16_t)(PICO_ICMP6HDR_PARAM_PROBLEM_SIZE + len)); + if (!notice) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + notice->payload = notice->transport_hdr + PICO_ICMP6HDR_PARAM_PROBLEM_SIZE; + notice->payload_len = len; + icmp6_hdr = (struct pico_icmp6_hdr *)notice->transport_hdr; + icmp6_hdr->msg.err.param_problem.ptr = long_be(ptr); + break; + + default: + return -1; + } + + icmp6_hdr->type = type; + icmp6_hdr->code = code; + memcpy(notice->payload, f->net_hdr, notice->payload_len); + notice->dev = f->dev; + /* f->src is set in frame_push, checksum calculated there */ + pico_ipv6_frame_push(notice, NULL, &ipv6_hdr->src, PICO_PROTO_ICMP6, 0); + return 0; +} + +int pico_icmp6_port_unreachable(struct pico_frame *f) +{ + struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr; + if (pico_ipv6_is_multicast(hdr->dst.addr)) + return 0; + + return pico_icmp6_notify(f, PICO_ICMP6_DEST_UNREACH, PICO_ICMP6_UNREACH_PORT, 0); +} + +int pico_icmp6_proto_unreachable(struct pico_frame *f) +{ + struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr; + if (pico_ipv6_is_multicast(hdr->dst.addr)) + return 0; + + return pico_icmp6_notify(f, PICO_ICMP6_DEST_UNREACH, PICO_ICMP6_UNREACH_ADDR, 0); +} + +int pico_icmp6_dest_unreachable(struct pico_frame *f) +{ + struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr; + if (pico_ipv6_is_multicast(hdr->dst.addr)) + return 0; + + return pico_icmp6_notify(f, PICO_ICMP6_DEST_UNREACH, PICO_ICMP6_UNREACH_ADDR, 0); +} + +int pico_icmp6_ttl_expired(struct pico_frame *f) +{ + struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr; + if (pico_ipv6_is_multicast(hdr->dst.addr)) + return 0; + + return pico_icmp6_notify(f, PICO_ICMP6_TIME_EXCEEDED, PICO_ICMP6_TIMXCEED_INTRANS, 0); +} + +int pico_icmp6_pkt_too_big(struct pico_frame *f) +{ + struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr; + if (pico_ipv6_is_multicast(hdr->dst.addr)) + return 0; + + return pico_icmp6_notify(f, PICO_ICMP6_PKT_TOO_BIG, 0, 0); +} + +#ifdef PICO_SUPPORT_IPFILTER +int pico_icmp6_packet_filtered(struct pico_frame *f) +{ + return pico_icmp6_notify(f, PICO_ICMP6_DEST_UNREACH, PICO_ICMP6_UNREACH_ADMIN, 0); +} +#endif + +int pico_icmp6_parameter_problem(struct pico_frame *f, uint8_t problem, uint32_t ptr) +{ + return pico_icmp6_notify(f, PICO_ICMP6_PARAM_PROBLEM, problem, ptr); +} + +MOCKABLE int pico_icmp6_frag_expired(struct pico_frame *f) +{ + struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr; + if (pico_ipv6_is_multicast(hdr->dst.addr)) + return 0; + + return pico_icmp6_notify(f, PICO_ICMP6_TIME_EXCEEDED, PICO_ICMP6_TIMXCEED_REASS, 0); +} + +/* RFC 4861 $7.2.2: sending neighbor solicitations */ +int pico_icmp6_neighbor_solicitation(struct pico_device *dev, struct pico_ip6 *dst, uint8_t type) +{ + struct pico_frame *sol = NULL; + struct pico_icmp6_hdr *icmp6_hdr = NULL; + struct pico_icmp6_opt_lladdr *opt = NULL; + struct pico_ip6 daddr = {{ 0xff, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x00, 0x00, 0x00 }}; + uint8_t i = 0; + uint16_t len = 0; + + if (pico_ipv6_is_multicast(dst->addr)) + return -1; + + len = PICO_ICMP6HDR_NEIGH_SOL_SIZE; + if (type != PICO_ICMP6_ND_DAD) + len = (uint16_t)(len + 8); + + sol = pico_proto_ipv6.alloc(&pico_proto_ipv6, len); + if (!sol) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + sol->payload = sol->transport_hdr + len; + sol->payload_len = 0; + + icmp6_hdr = (struct pico_icmp6_hdr *)sol->transport_hdr; + icmp6_hdr->type = PICO_ICMP6_NEIGH_SOL; + icmp6_hdr->code = 0; + icmp6_hdr->msg.info.neigh_sol.unused = 0; + icmp6_hdr->msg.info.neigh_sol.target = *dst; + + if (type != PICO_ICMP6_ND_DAD) { + opt = (struct pico_icmp6_opt_lladdr *)(((uint8_t *)&icmp6_hdr->msg.info.neigh_sol) + sizeof(struct neigh_sol_s)); + opt->type = PICO_ND_OPT_LLADDR_SRC; + opt->len = 1; + memcpy(opt->addr.mac.addr, dev->eth->mac.addr, PICO_SIZE_ETH); + } + + if (type == PICO_ICMP6_ND_SOLICITED || type == PICO_ICMP6_ND_DAD) { + for (i = 1; i <= 3; ++i) { + daddr.addr[PICO_SIZE_IP6 - i] = dst->addr[PICO_SIZE_IP6 - i]; + } + } else { + daddr = *dst; + } + + sol->dev = dev; + + /* f->src is set in frame_push, checksum calculated there */ + pico_ipv6_frame_push(sol, NULL, &daddr, PICO_PROTO_ICMP6, (type == PICO_ICMP6_ND_DAD)); + return 0; +} + +/* RFC 4861 $7.2.4: sending solicited neighbor advertisements */ +int pico_icmp6_neighbor_advertisement(struct pico_frame *f, struct pico_ip6 *target) +{ + struct pico_frame *adv = NULL; + struct pico_ipv6_hdr *ipv6_hdr = NULL; + struct pico_icmp6_hdr *icmp6_hdr = NULL; + struct pico_icmp6_opt_lladdr *opt = NULL; + struct pico_ip6 dst = {{0xff, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01}}; + + ipv6_hdr = (struct pico_ipv6_hdr *)f->net_hdr; + adv = pico_proto_ipv6.alloc(&pico_proto_ipv6, PICO_ICMP6HDR_NEIGH_ADV_SIZE + 8); + if (!adv) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + adv->payload = adv->transport_hdr + PICO_ICMP6HDR_NEIGH_ADV_SIZE + 8; + adv->payload_len = 0; + + icmp6_hdr = (struct pico_icmp6_hdr *)adv->transport_hdr; + icmp6_hdr->type = PICO_ICMP6_NEIGH_ADV; + icmp6_hdr->code = 0; + icmp6_hdr->msg.info.neigh_adv.target = *target; + icmp6_hdr->msg.info.neigh_adv.rsor = long_be(0x60000000); /* !router && solicited && override */ + if (pico_ipv6_is_unspecified(ipv6_hdr->src.addr)) { + /* solicited = clear && dst = all-nodes address (scope link-local) */ + icmp6_hdr->msg.info.neigh_adv.rsor ^= long_be(0x40000000); + } else { + /* solicited = set && dst = source of solicitation */ + dst = ipv6_hdr->src; + } + + /* XXX if the target address is either an anycast address or a unicast + * address for which the node is providing proxy service, or the target + * link-layer Address option is not included, the Override flag SHOULD + * be set to zero. + */ + + /* XXX if the target address is an anycast address, the sender SHOULD delay + * sending a response for a random time between 0 and MAX_ANYCAST_DELAY_TIME seconds. + */ + + opt = (struct pico_icmp6_opt_lladdr *)(((uint8_t *)&icmp6_hdr->msg.info.neigh_adv) + sizeof(struct neigh_adv_s)); + opt->type = PICO_ND_OPT_LLADDR_TGT; + opt->len = 1; + memcpy(opt->addr.mac.addr, f->dev->eth->mac.addr, PICO_SIZE_ETH); + adv->dev = f->dev; + + /* f->src is set in frame_push, checksum calculated there */ + pico_ipv6_frame_push(adv, NULL, &dst, PICO_PROTO_ICMP6, 0); + return 0; +} + +/* RFC 4861 $6.3.7: sending router solicitations */ +int pico_icmp6_router_solicitation(struct pico_device *dev, struct pico_ip6 *src) +{ + struct pico_frame *sol = NULL; + struct pico_icmp6_hdr *icmp6_hdr = NULL; + struct pico_icmp6_opt_lladdr *lladdr = NULL; + uint16_t len = 0; + struct pico_ip6 daddr = {{ 0xff, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02 }}; + + len = PICO_ICMP6HDR_ROUTER_SOL_SIZE; + if (!pico_ipv6_is_unspecified(src->addr)) + len = (uint16_t)(len + 8); + + sol = pico_proto_ipv6.alloc(&pico_proto_ipv6, len); + if (!sol) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + sol->payload = sol->transport_hdr + len; + sol->payload_len = 0; + + icmp6_hdr = (struct pico_icmp6_hdr *)sol->transport_hdr; + icmp6_hdr->type = PICO_ICMP6_ROUTER_SOL; + icmp6_hdr->code = 0; + + if (!pico_ipv6_is_unspecified(src->addr)) { + lladdr = (struct pico_icmp6_opt_lladdr *)(((uint8_t *)&icmp6_hdr->msg.info.router_sol) + sizeof(struct router_sol_s)); + lladdr->type = PICO_ND_OPT_LLADDR_SRC; + lladdr->len = 1; + memcpy(lladdr->addr.mac.addr, dev->eth->mac.addr, PICO_SIZE_ETH); + } + + sol->dev = dev; + + /* f->src is set in frame_push, checksum calculated there */ + pico_ipv6_frame_push(sol, NULL, &daddr, PICO_PROTO_ICMP6, 0); + return 0; +} + +#define PICO_RADV_VAL_LIFETIME (long_be(86400)) +#define PICO_RADV_PREF_LIFETIME (long_be(14400)) + +/* RFC 4861: sending router advertisements */ +int pico_icmp6_router_advertisement(struct pico_device *dev, struct pico_ip6 *dst) +{ + struct pico_frame *adv = NULL; + struct pico_icmp6_hdr *icmp6_hdr = NULL; + struct pico_icmp6_opt_lladdr *lladdr; + struct pico_icmp6_opt_prefix *prefix; + uint16_t len = 0; + struct pico_ip6 dst_mcast = {{ 0xff, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01 }}; + uint8_t *nxt_opt; + + len = PICO_ICMP6HDR_ROUTER_ADV_SIZE + PICO_ICMP6_OPT_LLADDR_SIZE + sizeof(struct pico_icmp6_opt_prefix); + + adv = pico_proto_ipv6.alloc(&pico_proto_ipv6, len); + if (!adv) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + adv->payload = adv->transport_hdr + len; + adv->payload_len = 0; + adv->dev = dev; + + icmp6_hdr = (struct pico_icmp6_hdr *)adv->transport_hdr; + icmp6_hdr->type = PICO_ICMP6_ROUTER_ADV; + icmp6_hdr->code = 0; + icmp6_hdr->msg.info.router_adv.life_time = short_be(45); + icmp6_hdr->msg.info.router_adv.hop = 64; + nxt_opt = (uint8_t *)&icmp6_hdr->msg.info.router_adv + sizeof(struct router_adv_s); + + prefix = (struct pico_icmp6_opt_prefix *)nxt_opt; + prefix->type = PICO_ND_OPT_PREFIX; + prefix->len = sizeof(struct pico_icmp6_opt_prefix) >> 3; + prefix->prefix_len = 64; /* Only /64 are forwarded */ + prefix->aac = 1; + prefix->onlink = 1; + prefix->val_lifetime = PICO_RADV_VAL_LIFETIME; + prefix->pref_lifetime = PICO_RADV_PREF_LIFETIME; + memcpy(&prefix->prefix, dst, sizeof(struct pico_ip6)); + + nxt_opt += (sizeof (struct pico_icmp6_opt_prefix)); + lladdr = (struct pico_icmp6_opt_lladdr *)nxt_opt; + lladdr->type = PICO_ND_OPT_LLADDR_SRC; + lladdr->len = 1; + memcpy(lladdr->addr.mac.addr, dev->eth->mac.addr, PICO_SIZE_ETH); + icmp6_hdr->crc = 0; + icmp6_hdr->crc = short_be(pico_icmp6_checksum(adv)); + /* f->src is set in frame_push, checksum calculated there */ + pico_ipv6_frame_push(adv, NULL, &dst_mcast, PICO_PROTO_ICMP6, 0); + return 0; +} + +/***********************/ +/* Ping implementation */ +/***********************/ + +#ifdef PICO_SUPPORT_PING +struct pico_icmp6_ping_cookie +{ + uint16_t id; + uint16_t seq; + uint16_t size; + uint16_t err; + int count; + int interval; + int timeout; + pico_time timestamp; + struct pico_ip6 dst; + struct pico_device *dev; + void (*cb)(struct pico_icmp6_stats*); +}; + +static int icmp6_cookie_compare(void *ka, void *kb) +{ + struct pico_icmp6_ping_cookie *a = ka, *b = kb; + if (a->id < b->id) + return -1; + + if (a->id > b->id) + return 1; + + return (a->seq - b->seq); +} +PICO_TREE_DECLARE(IPV6Pings, icmp6_cookie_compare); + +static int pico_icmp6_send_echo(struct pico_icmp6_ping_cookie *cookie) +{ + struct pico_frame *echo = NULL; + struct pico_icmp6_hdr *hdr = NULL; + + echo = pico_proto_ipv6.alloc(&pico_proto_ipv6, (uint16_t)(PICO_ICMP6HDR_ECHO_REQUEST_SIZE + cookie->size)); + if (!echo) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + echo->payload = echo->transport_hdr + PICO_ICMP6HDR_ECHO_REQUEST_SIZE; + echo->payload_len = cookie->size; + + hdr = (struct pico_icmp6_hdr *)echo->transport_hdr; + hdr->type = PICO_ICMP6_ECHO_REQUEST; + hdr->code = 0; + hdr->msg.info.echo_request.id = short_be(cookie->id); + hdr->msg.info.echo_request.seq = short_be(cookie->seq); + /* XXX: Fill payload */ + hdr->crc = 0; + hdr->crc = short_be(pico_icmp6_checksum(echo)); + echo->dev = cookie->dev; + pico_ipv6_frame_push(echo, NULL, &cookie->dst, PICO_PROTO_ICMP6, 0); + return 0; +} + + +static void pico_icmp6_ping_timeout(pico_time now, void *arg) +{ + struct pico_icmp6_ping_cookie *cookie = NULL; + + IGNORE_PARAMETER(now); + + cookie = (struct pico_icmp6_ping_cookie *)arg; + if (pico_tree_findKey(&IPV6Pings, cookie)) { + if (cookie->err == PICO_PING6_ERR_PENDING) { + struct pico_icmp6_stats stats = { + 0 + }; + stats.dst = cookie->dst; + stats.seq = cookie->seq; + stats.time = 0; + stats.size = cookie->size; + stats.err = PICO_PING6_ERR_TIMEOUT; + dbg(" ---- Ping6 timeout!!!\n"); + if (cookie->cb) + cookie->cb(&stats); + } + + pico_tree_delete(&IPV6Pings, cookie); + PICO_FREE(cookie); + } +} + +static void pico_icmp6_next_ping(pico_time now, void *arg); +static inline void pico_icmp6_send_ping(struct pico_icmp6_ping_cookie *cookie) +{ + pico_icmp6_send_echo(cookie); + cookie->timestamp = pico_tick; + pico_timer_add((pico_time)(cookie->interval), pico_icmp6_next_ping, cookie); + pico_timer_add((pico_time)(cookie->timeout), pico_icmp6_ping_timeout, cookie); +} + +static void pico_icmp6_next_ping(pico_time now, void *arg) +{ + struct pico_icmp6_ping_cookie *cookie = NULL, *new = NULL; + + IGNORE_PARAMETER(now); + + cookie = (struct pico_icmp6_ping_cookie *)arg; + if (pico_tree_findKey(&IPV6Pings, cookie)) { + if (cookie->err == PICO_PING6_ERR_ABORTED) + return; + + if (cookie->seq < (uint16_t)cookie->count) { + new = PICO_ZALLOC(sizeof(struct pico_icmp6_ping_cookie)); + if (!new) { + pico_err = PICO_ERR_ENOMEM; + return; + } + + memcpy(new, cookie, sizeof(struct pico_icmp6_ping_cookie)); + new->seq++; + + pico_tree_insert(&IPV6Pings, new); + pico_icmp6_send_ping(new); + } + } +} + +static void pico_icmp6_ping_recv_reply(struct pico_frame *f) +{ + struct pico_icmp6_ping_cookie *cookie = NULL, test = { + 0 + }; + struct pico_icmp6_hdr *hdr = NULL; + + hdr = (struct pico_icmp6_hdr *)f->transport_hdr; + test.id = short_be(hdr->msg.info.echo_reply.id); + test.seq = short_be(hdr->msg.info.echo_reply.seq); + cookie = pico_tree_findKey(&IPV6Pings, &test); + if (cookie) { + struct pico_icmp6_stats stats = { + 0 + }; + if (cookie->err == PICO_PING6_ERR_ABORTED) + return; + + cookie->err = PICO_PING6_ERR_REPLIED; + stats.dst = cookie->dst; + stats.seq = cookie->seq; + stats.size = cookie->size; + stats.time = pico_tick - cookie->timestamp; + stats.err = cookie->err; + stats.ttl = ((struct pico_ipv6_hdr *)f->net_hdr)->hop; + if(cookie->cb) + cookie->cb(&stats); + } else { + dbg("Reply for seq=%d, not found.\n", test.seq); + } +} + +int pico_icmp6_ping(char *dst, int count, int interval, int timeout, int size, void (*cb)(struct pico_icmp6_stats *), struct pico_device *dev) +{ + static uint16_t next_id = 0x91c0; + struct pico_icmp6_ping_cookie *cookie = NULL; + + if(!dst || !count || !interval || !timeout) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + cookie = PICO_ZALLOC(sizeof(struct pico_icmp6_ping_cookie)); + if (!cookie) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + if (pico_string_to_ipv6(dst, cookie->dst.addr) < 0) { + pico_err = PICO_ERR_EINVAL; + PICO_FREE(cookie); + return -1; + } + + cookie->seq = 1; + cookie->id = next_id++; + cookie->err = PICO_PING6_ERR_PENDING; + cookie->size = (uint16_t)size; + cookie->interval = interval; + cookie->timeout = timeout; + cookie->cb = cb; + cookie->count = count; + cookie->dev = dev; + + pico_tree_insert(&IPV6Pings, cookie); + pico_icmp6_send_ping(cookie); + return (int)cookie->id; +} + +int pico_icmp6_ping_abort(int id) +{ + struct pico_tree_node *node; + int found = 0; + pico_tree_foreach(node, &IPV6Pings) + { + struct pico_icmp6_ping_cookie *ck = + (struct pico_icmp6_ping_cookie *) node->keyValue; + if (ck->id == (uint16_t)id) { + ck->err = PICO_PING6_ERR_ABORTED; + found++; + } + } + if (found > 0) + return 0; /* OK if at least one pending ping has been canceled */ + + pico_err = PICO_ERR_ENOENT; + return -1; +} + +#endif diff --git a/ext/picotcp/modules/pico_icmp6.h b/ext/picotcp/modules/pico_icmp6.h new file mode 100644 index 0000000..7734060 --- /dev/null +++ b/ext/picotcp/modules/pico_icmp6.h @@ -0,0 +1,256 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + *********************************************************************/ +#ifndef _INCLUDE_PICO_ICMP6 +#define _INCLUDE_PICO_ICMP6 +#include "pico_addressing.h" +#include "pico_protocol.h" +#include "pico_mld.h" +/* ICMP header sizes */ +#define PICO_ICMP6HDR_DRY_SIZE 4 +#define PICO_ICMP6HDR_ECHO_REQUEST_SIZE 8 +#define PICO_ICMP6HDR_DEST_UNREACH_SIZE 8 +#define PICO_ICMP6HDR_TIME_XCEEDED_SIZE 8 +#define PICO_ICMP6HDR_PARAM_PROBLEM_SIZE 8 +#define PICO_ICMP6HDR_NEIGH_SOL_SIZE 24 +#define PICO_ICMP6HDR_NEIGH_ADV_SIZE 24 +#define PICO_ICMP6HDR_ROUTER_SOL_SIZE 8 +#define PICO_ICMP6HDR_ROUTER_ADV_SIZE 16 +#define PICO_ICMP6HDR_REDIRECT_SIZE 40 + +/* ICMP types */ +#define PICO_ICMP6_DEST_UNREACH 1 +#define PICO_ICMP6_PKT_TOO_BIG 2 +#define PICO_ICMP6_TIME_EXCEEDED 3 +#define PICO_ICMP6_PARAM_PROBLEM 4 +#define PICO_ICMP6_ECHO_REQUEST 128 +#define PICO_ICMP6_ECHO_REPLY 129 +#define PICO_ICMP6_ROUTER_SOL 133 +#define PICO_ICMP6_ROUTER_ADV 134 +#define PICO_ICMP6_NEIGH_SOL 135 +#define PICO_ICMP6_NEIGH_ADV 136 +#define PICO_ICMP6_REDIRECT 137 + +/* destination unreachable codes */ +#define PICO_ICMP6_UNREACH_NOROUTE 0 +#define PICO_ICMP6_UNREACH_ADMIN 1 +#define PICO_ICMP6_UNREACH_SRCSCOPE 2 +#define PICO_ICMP6_UNREACH_ADDR 3 +#define PICO_ICMP6_UNREACH_PORT 4 +#define PICO_ICMP6_UNREACH_SRCFILTER 5 +#define PICO_ICMP6_UNREACH_REJROUTE 6 + +/* time exceeded codes */ +#define PICO_ICMP6_TIMXCEED_INTRANS 0 +#define PICO_ICMP6_TIMXCEED_REASS 1 + +/* parameter problem codes */ +#define PICO_ICMP6_PARAMPROB_HDRFIELD 0 +#define PICO_ICMP6_PARAMPROB_NXTHDR 1 +#define PICO_ICMP6_PARAMPROB_IPV6OPT 2 + +/* ping error codes */ +#define PICO_PING6_ERR_REPLIED 0 +#define PICO_PING6_ERR_TIMEOUT 1 +#define PICO_PING6_ERR_UNREACH 2 +#define PICO_PING6_ERR_ABORTED 3 +#define PICO_PING6_ERR_PENDING 0xFFFF + +/* ND configuration */ +#define PICO_ND_MAX_FRAMES_QUEUED 4 /* max frames queued while awaiting address resolution */ + +/* ND RFC constants */ +#define PICO_ND_MAX_SOLICIT 3 +#define PICO_ND_MAX_NEIGHBOR_ADVERT 3 +#define PICO_ND_DELAY_INCOMPLETE 1000 /* msec */ +#define PICO_ND_DELAY_FIRST_PROBE_TIME 5000 /* msec */ + +/* neighbor discovery options */ +#define PICO_ND_OPT_LLADDR_SRC 1 +#define PICO_ND_OPT_LLADDR_TGT 2 +#define PICO_ND_OPT_PREFIX 3 +#define PICO_ND_OPT_REDIRECT 4 +#define PICO_ND_OPT_MTU 5 +#define PICO_ND_OPT_RDNSS 25 /* RFC 5006 */ + +/* ND advertisement flags */ +#define PICO_ND_ROUTER 0x80000000 +#define PICO_ND_SOLICITED 0x40000000 +#define PICO_ND_OVERRIDE 0x20000000 +#define IS_ROUTER(x) (long_be(x->msg.info.neigh_adv.rsor) & (PICO_ND_ROUTER)) /* router flag set? */ +#define IS_SOLICITED(x) (long_be(x->msg.info.neigh_adv.rsor) & (PICO_ND_SOLICITED)) /* solicited flag set? */ +#define IS_OVERRIDE(x) (long_be(x->msg.info.neigh_adv.rsor) & (PICO_ND_OVERRIDE)) /* override flag set? */ + +#define PICO_ND_PREFIX_LIFETIME_INF 0xFFFFFFFFu +/* #define PICO_ND_DESTINATION_LRU_TIME 600000u / * msecs (10min) * / */ + +/* custom defines */ +#define PICO_ICMP6_ND_UNICAST 0 +#define PICO_ICMP6_ND_ANYCAST 1 +#define PICO_ICMP6_ND_SOLICITED 2 +#define PICO_ICMP6_ND_DAD 3 + +#define PICO_ICMP6_MAX_RTR_SOL_DELAY 1000 + +#define PICO_SIZE_ICMP6HDR ((sizeof(struct pico_icmp6_hdr))) +#define PICO_ICMP6_OPT_LLADDR_SIZE (8) + +extern struct pico_protocol pico_proto_icmp6; + +PACKED_STRUCT_DEF pico_icmp6_hdr { + uint8_t type; + uint8_t code; + uint16_t crc; + + PACKED_UNION_DEF icmp6_msg_u { + /* error messages */ + PACKED_UNION_DEF icmp6_err_u { + PEDANTIC_STRUCT_DEF dest_unreach_s { + uint32_t unused; + } dest_unreach; + PEDANTIC_STRUCT_DEF pkt_too_big_s { + uint32_t mtu; + } pkt_too_big; + PEDANTIC_STRUCT_DEF time_exceeded_s { + uint32_t unused; + } time_exceeded; + PEDANTIC_STRUCT_DEF param_problem_s { + uint32_t ptr; + } param_problem; + } err; + + /* informational messages */ + PACKED_UNION_DEF icmp6_info_u { + PEDANTIC_STRUCT_DEF echo_request_s { + uint16_t id; + uint16_t seq; + } echo_request; + PEDANTIC_STRUCT_DEF echo_reply_s { + uint16_t id; + uint16_t seq; + } echo_reply; + PEDANTIC_STRUCT_DEF router_sol_s { + uint32_t unused; + } router_sol; + PEDANTIC_STRUCT_DEF router_adv_s { + uint8_t hop; + uint8_t mor; + uint16_t life_time; + uint32_t reachable_time; + uint32_t retrans_time; + } router_adv; + PEDANTIC_STRUCT_DEF neigh_sol_s { + uint32_t unused; + struct pico_ip6 target; + } neigh_sol; + PEDANTIC_STRUCT_DEF neigh_adv_s { + uint32_t rsor; + struct pico_ip6 target; + } neigh_adv; + PEDANTIC_STRUCT_DEF redirect_s { + uint32_t reserved; + struct pico_ip6 target; + struct pico_ip6 dest; + } redirect; + PEDANTIC_STRUCT_DEF mld_s { + uint16_t max_resp_time; + uint16_t reserved; + struct pico_ip6 mmcast_group; + /*MLDv2*/ + uint8_t reserverd; // With S and QRV + uint8_t QQIC; + uint16_t nbr_src; + struct pico_ip6 src[0]; + } mld; + } info; + } msg; +}; + +PACKED_STRUCT_DEF pico_icmp6_opt_lladdr +{ + uint8_t type; + uint8_t len; + PACKED_UNION_DEF icmp6_opt_hw_addr_u { + struct pico_eth mac; + } addr; +}; + +PACKED_STRUCT_DEF pico_icmp6_opt_prefix +{ + uint8_t type; + uint8_t len; + uint8_t prefix_len; + uint8_t res : 6; + uint8_t aac : 1; + uint8_t onlink : 1; + uint32_t val_lifetime; + uint32_t pref_lifetime; + uint32_t reserved; + struct pico_ip6 prefix; +}; + +PACKED_STRUCT_DEF pico_icmp6_opt_mtu +{ + uint8_t type; + uint8_t len; + uint16_t res; + uint32_t mtu; +}; + +PACKED_STRUCT_DEF pico_icmp6_opt_redirect +{ + uint8_t type; + uint8_t len; + uint16_t res0; + uint32_t res1; +}; + +PACKED_STRUCT_DEF pico_icmp6_opt_rdnss +{ + uint8_t type; + uint8_t len; + uint16_t res0; + uint32_t lifetime; + struct pico_ip6 *addr; +}; + +PACKED_STRUCT_DEF pico_icmp6_opt_na +{ + uint8_t type; + uint8_t len; +}; + +struct pico_icmp6_stats +{ + unsigned long size; + unsigned long seq; + pico_time time; + unsigned long ttl; + int err; + struct pico_ip6 dst; +}; + +int pico_icmp6_ping(char *dst, int count, int interval, int timeout, int size, void (*cb)(struct pico_icmp6_stats *), struct pico_device *dev); +int pico_icmp6_ping_abort(int id); + +int pico_icmp6_neighbor_solicitation(struct pico_device *dev, struct pico_ip6 *dst, uint8_t type); +int pico_icmp6_neighbor_advertisement(struct pico_frame *f, struct pico_ip6 *target); +int pico_icmp6_router_solicitation(struct pico_device *dev, struct pico_ip6 *src); + +int pico_icmp6_port_unreachable(struct pico_frame *f); +int pico_icmp6_proto_unreachable(struct pico_frame *f); +int pico_icmp6_dest_unreachable(struct pico_frame *f); +int pico_icmp6_ttl_expired(struct pico_frame *f); +int pico_icmp6_packet_filtered(struct pico_frame *f); +int pico_icmp6_parameter_problem(struct pico_frame *f, uint8_t problem, uint32_t ptr); +int pico_icmp6_pkt_too_big(struct pico_frame *f); +int pico_icmp6_frag_expired(struct pico_frame *f); + +uint16_t pico_icmp6_checksum(struct pico_frame *f); +int pico_icmp6_router_advertisement(struct pico_device *dev, struct pico_ip6 *dst); + +#endif diff --git a/ext/picotcp/modules/pico_igmp.c b/ext/picotcp/modules/pico_igmp.c new file mode 100644 index 0000000..e3ca5ea --- /dev/null +++ b/ext/picotcp/modules/pico_igmp.c @@ -0,0 +1,1276 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + RFC 1112, 2236, 3376, 3569, 3678, 4607 + + Authors: Kristof Roelants (IGMPv3), Simon Maes, Brecht Van Cauwenberghe + *********************************************************************/ + +#include "pico_stack.h" +#include "pico_ipv4.h" +#include "pico_igmp.h" +#include "pico_config.h" +#include "pico_eth.h" +#include "pico_addressing.h" +#include "pico_frame.h" +#include "pico_tree.h" +#include "pico_device.h" +#include "pico_socket.h" + +#if defined(PICO_SUPPORT_IGMP) && defined(PICO_SUPPORT_MCAST) + +#define igmp_dbg(...) do {} while(0) +/* #define igmp_dbg dbg */ + +/* membership states */ +#define IGMP_STATE_NON_MEMBER (0x0) +#define IGMP_STATE_DELAYING_MEMBER (0x1) +#define IGMP_STATE_IDLE_MEMBER (0x2) + +/* events */ +#define IGMP_EVENT_DELETE_GROUP (0x0) +#define IGMP_EVENT_CREATE_GROUP (0x1) +#define IGMP_EVENT_UPDATE_GROUP (0x2) +#define IGMP_EVENT_QUERY_RECV (0x3) +#define IGMP_EVENT_REPORT_RECV (0x4) +#define IGMP_EVENT_TIMER_EXPIRED (0x5) + +/* message types */ +#define IGMP_TYPE_MEM_QUERY (0x11) +#define IGMP_TYPE_MEM_REPORT_V1 (0x12) +#define IGMP_TYPE_MEM_REPORT_V2 (0x16) +#define IGMP_TYPE_LEAVE_GROUP (0x17) +#define IGMP_TYPE_MEM_REPORT_V3 (0x22) + +/* group record types */ +#define IGMP_MODE_IS_INCLUDE (1) +#define IGMP_MODE_IS_EXCLUDE (2) +#define IGMP_CHANGE_TO_INCLUDE_MODE (3) +#define IGMP_CHANGE_TO_EXCLUDE_MODE (4) +#define IGMP_ALLOW_NEW_SOURCES (5) +#define IGMP_BLOCK_OLD_SOURCES (6) + +/* host flag */ +#define IGMP_HOST_LAST (0x1) +#define IGMP_HOST_NOT_LAST (0x0) + +/* list of timers, counters and their default values */ +#define IGMP_ROBUSTNESS (2u) +#define IGMP_QUERY_INTERVAL (125) /* secs */ +#define IGMP_QUERY_RESPONSE_INTERVAL (10u) /* secs */ +#define IGMP_STARTUP_QUERY_INTERVAL (IGMPV3_QUERY_INTERVAL / 4) +#define IGMP_STARTUP_QUERY_COUNT (IGMPV3_ROBUSTNESS) +#define IGMP_LAST_MEMBER_QUERY_INTERVAL (1) /* secs */ +#define IGMP_LAST_MEMBER_QUERY_COUNT (IGMPV3_ROBUSTNESS) +#define IGMP_UNSOLICITED_REPORT_INTERVAL (1) /* secs */ +#define IGMP_DEFAULT_MAX_RESPONSE_TIME (100) + +/* custom timers types */ +#define IGMP_TIMER_GROUP_REPORT (1) +#define IGMP_TIMER_V1_QUERIER (2) +#define IGMP_TIMER_V2_QUERIER (3) + +/* IGMP groups */ +#define IGMP_ALL_HOST_GROUP long_be(0xE0000001) /* 224.0.0.1 */ +#define IGMP_ALL_ROUTER_GROUP long_be(0xE0000002) /* 224.0.0.2 */ +#define IGMPV3_ALL_ROUTER_GROUP long_be(0xE0000016) /* 224.0.0.22 */ + +/* misc */ +#define IGMP_TIMER_STOPPED (1) +#define IP_OPTION_ROUTER_ALERT_LEN (4u) +#define IGMP_MAX_GROUPS (32) /* max 255 */ + +PACKED_STRUCT_DEF igmp_message { + uint8_t type; + uint8_t max_resp_time; + uint16_t crc; + uint32_t mcast_group; +}; + +PACKED_STRUCT_DEF igmpv3_query { + uint8_t type; + uint8_t max_resp_time; + uint16_t crc; + uint32_t mcast_group; + uint8_t rsq; + uint8_t qqic; + uint16_t sources; +}; + +PACKED_STRUCT_DEF igmpv3_group_record { + uint8_t type; + uint8_t aux; + uint16_t sources; + uint32_t mcast_group; +}; + +PACKED_STRUCT_DEF igmpv3_report { + uint8_t type; + uint8_t res0; + uint16_t crc; + uint16_t res1; + uint16_t groups; +}; + +struct igmp_parameters { + uint8_t event; + uint8_t state; + uint8_t last_host; + uint8_t filter_mode; + uint8_t max_resp_time; + struct pico_ip4 mcast_link; + struct pico_ip4 mcast_group; + struct pico_tree *MCASTFilter; + struct pico_frame *f; +}; + +struct igmp_timer { + uint8_t type; + uint8_t stopped; + pico_time start; + pico_time delay; + struct pico_ip4 mcast_link; + struct pico_ip4 mcast_group; + struct pico_frame *f; + void (*callback)(struct igmp_timer *t); +}; + +/* queues */ +static struct pico_queue igmp_in = { + 0 +}; +static struct pico_queue igmp_out = { + 0 +}; + +/* finite state machine caller */ +static int pico_igmp_process_event(struct igmp_parameters *p); + +/* state callback prototype */ +typedef int (*callback)(struct igmp_parameters *); + +static inline int igmpt_type_compare(struct igmp_timer *a, struct igmp_timer *b) +{ + if (a->type < b->type) + return -1; + + if (a->type > b->type) + return 1; + + return 0; +} + + +static inline int igmpt_group_compare(struct igmp_timer *a, struct igmp_timer *b) +{ + return pico_ipv4_compare(&a->mcast_group, &b->mcast_group); +} + +static inline int igmpt_link_compare(struct igmp_timer *a, struct igmp_timer *b) +{ + return pico_ipv4_compare(&a->mcast_link, &b->mcast_link); +} + +/* redblack trees */ +static int igmp_timer_cmp(void *ka, void *kb) +{ + struct igmp_timer *a = ka, *b = kb; + int cmp = igmpt_type_compare(a, b); + if (cmp) + return cmp; + + cmp = igmpt_group_compare(a, b); + if (cmp) + return cmp; + + return igmpt_link_compare(a, b); + +} +PICO_TREE_DECLARE(IGMPTimers, igmp_timer_cmp); + +static inline int igmpparm_group_compare(struct igmp_parameters *a, struct igmp_parameters *b) +{ + return pico_ipv4_compare(&a->mcast_group, &b->mcast_group); +} + +static inline int igmpparm_link_compare(struct igmp_parameters *a, struct igmp_parameters *b) +{ + return pico_ipv4_compare(&a->mcast_link, &b->mcast_link); +} + +static int igmp_parameters_cmp(void *ka, void *kb) +{ + struct igmp_parameters *a = ka, *b = kb; + int cmp = igmpparm_group_compare(a, b); + if (cmp) + return cmp; + + return igmpparm_link_compare(a, b); +} +PICO_TREE_DECLARE(IGMPParameters, igmp_parameters_cmp); + +static int igmp_sources_cmp(void *ka, void *kb) +{ + struct pico_ip4 *a = ka, *b = kb; + return pico_ipv4_compare(a, b); +} +PICO_TREE_DECLARE(IGMPAllow, igmp_sources_cmp); +PICO_TREE_DECLARE(IGMPBlock, igmp_sources_cmp); + +static struct igmp_parameters *pico_igmp_find_parameter(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group) +{ + struct igmp_parameters test = { + 0 + }; + if (!mcast_link || !mcast_group) + return NULL; + + test.mcast_link.addr = mcast_link->addr; + test.mcast_group.addr = mcast_group->addr; + return pico_tree_findKey(&IGMPParameters, &test); +} + +static int pico_igmp_delete_parameter(struct igmp_parameters *p) +{ + if (pico_tree_delete(&IGMPParameters, p)) + PICO_FREE(p); + else + return -1; + + return 0; +} + +static void pico_igmp_timer_expired(pico_time now, void *arg) +{ + struct igmp_timer *t = NULL, *timer = NULL, test = { + 0 + }; + + IGNORE_PARAMETER(now); + t = (struct igmp_timer *)arg; + test.type = t->type; + test.mcast_link = t->mcast_link; + test.mcast_group = t->mcast_group; + igmp_dbg("IGMP: timer expired for %08X link %08X type %u, delay %lu\n", t->mcast_group.addr, t->mcast_link.addr, t->type, t->delay); + timer = pico_tree_findKey(&IGMPTimers, &test); + if (!timer) { + return; + } + + if (timer->stopped == IGMP_TIMER_STOPPED) { + pico_tree_delete(&IGMPTimers, timer); + PICO_FREE(t); + return; + } + + if (timer->start + timer->delay < PICO_TIME_MS()) { + pico_tree_delete(&IGMPTimers, timer); + if (timer->callback) + timer->callback(timer); + + PICO_FREE(timer); + } else { + igmp_dbg("IGMP: restart timer for %08X, delay %lu, new delay %lu\n", t->mcast_group.addr, t->delay, (timer->start + timer->delay) - PICO_TIME_MS()); + pico_timer_add((timer->start + timer->delay) - PICO_TIME_MS(), &pico_igmp_timer_expired, timer); + } + + return; +} + +static int pico_igmp_timer_reset(struct igmp_timer *t) +{ + struct igmp_timer *timer = NULL, test = { + 0 + }; + + igmp_dbg("IGMP: reset timer for %08X, delay %lu\n", t->mcast_group.addr, t->delay); + test.type = t->type; + test.mcast_link = t->mcast_link; + test.mcast_group = t->mcast_group; + timer = pico_tree_findKey(&IGMPTimers, &test); + if (!timer) + return -1; + + *timer = *t; + timer->start = PICO_TIME_MS(); + return 0; +} + +static int pico_igmp_timer_start(struct igmp_timer *t) +{ + struct igmp_timer *timer = NULL, test = { + 0 + }; + + igmp_dbg("IGMP: start timer for %08X link %08X type %u, delay %lu\n", t->mcast_group.addr, t->mcast_link.addr, t->type, t->delay); + test.type = t->type; + test.mcast_link = t->mcast_link; + test.mcast_group = t->mcast_group; + timer = pico_tree_findKey(&IGMPTimers, &test); + if (timer) + return pico_igmp_timer_reset(t); + + timer = PICO_ZALLOC(sizeof(struct igmp_timer)); + if (!timer) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + *timer = *t; + timer->start = PICO_TIME_MS(); + + pico_tree_insert(&IGMPTimers, timer); + pico_timer_add(timer->delay, &pico_igmp_timer_expired, timer); + return 0; +} + +static int pico_igmp_timer_stop(struct igmp_timer *t) +{ + struct igmp_timer *timer = NULL, test = { + 0 + }; + + test.type = t->type; + test.mcast_link = t->mcast_link; + test.mcast_group = t->mcast_group; + timer = pico_tree_findKey(&IGMPTimers, &test); + if (!timer) + return -1; + + igmp_dbg("IGMP: stop timer for %08X, delay %lu\n", timer->mcast_group.addr, timer->delay); + timer->stopped = IGMP_TIMER_STOPPED; + return 0; +} + +static int pico_igmp_timer_is_running(struct igmp_timer *t) +{ + struct igmp_timer *timer = NULL, test = { + 0 + }; + + test.type = t->type; + test.mcast_link = t->mcast_link; + test.mcast_group = t->mcast_group; + timer = pico_tree_findKey(&IGMPTimers, &test); + if (timer) + return 1; + + return 0; +} + +static struct igmp_timer *pico_igmp_find_timer(uint8_t type, struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group) +{ + struct igmp_timer test = { + 0 + }; + + test.type = type; + test.mcast_link = *mcast_link; + test.mcast_group = *mcast_group; + return pico_tree_findKey(&IGMPTimers, &test); +} + +static void pico_igmp_report_expired(struct igmp_timer *t) +{ + struct igmp_parameters *p = NULL; + + p = pico_igmp_find_parameter(&t->mcast_link, &t->mcast_group); + if (!p) + return; + + p->event = IGMP_EVENT_TIMER_EXPIRED; + pico_igmp_process_event(p); +} + +static void pico_igmp_v2querier_expired(struct igmp_timer *t) +{ + struct pico_ipv4_link *link = NULL; + struct pico_tree_node *index = NULL, *_tmp = NULL; + + link = pico_ipv4_link_by_dev(t->f->dev); + if (!link) + return; + + /* When changing compatibility mode, cancel all pending response + * and retransmission timers. + */ + pico_tree_foreach_safe(index, &IGMPTimers, _tmp) + { + ((struct igmp_timer *)index->keyValue)->stopped = IGMP_TIMER_STOPPED; + pico_tree_delete(&IGMPTimers, index->keyValue); + } + igmp_dbg("IGMP: switch to compatibility mode IGMPv3\n"); + link->mcast_compatibility = PICO_IGMPV3; + return; +} + +static int pico_igmp_is_checksum_valid(struct pico_frame *f) +{ + struct pico_ipv4_hdr *hdr = NULL; + uint8_t ihl = 24, datalen = 0; + + hdr = (struct pico_ipv4_hdr *)f->net_hdr; + ihl = (uint8_t)((hdr->vhl & 0x0F) * 4); /* IHL is in 32bit words */ + datalen = (uint8_t)(short_be(hdr->len) - ihl); + + if (short_be(pico_checksum(f->transport_hdr, datalen)) == 0) + return 1; + + igmp_dbg("IGMP: invalid checksum\n"); + return 0; +} + +/* RFC 3376 $7.1 */ +static int pico_igmp_compatibility_mode(struct pico_frame *f) +{ + struct pico_ipv4_hdr *hdr = NULL; + struct pico_ipv4_link *link = NULL; + struct pico_tree_node *index = NULL, *_tmp = NULL; + struct igmp_timer t = { + 0 + }; + uint8_t ihl = 24, datalen = 0; + + link = pico_ipv4_link_by_dev(f->dev); + if (!link) + return -1; + + hdr = (struct pico_ipv4_hdr *) f->net_hdr; + ihl = (uint8_t)((hdr->vhl & 0x0F) * 4); /* IHL is in 32bit words */ + datalen = (uint8_t)(short_be(hdr->len) - ihl); + igmp_dbg("IGMP: IHL = %u, LEN = %u, OCTETS = %u\n", ihl, short_be(hdr->len), datalen); + + if (datalen >= 12) { + /* IGMPv3 query */ + t.type = IGMP_TIMER_V2_QUERIER; + if (pico_igmp_timer_is_running(&t)) { /* IGMPv2 querier present timer still running */ + igmp_dbg("Timer is already running\n"); + return -1; + } else { + link->mcast_compatibility = PICO_IGMPV3; + igmp_dbg("IGMP Compatibility: v3\n"); + return 0; + } + } else if (datalen == 8) { + struct igmp_message *query = (struct igmp_message *)f->transport_hdr; + if (query->max_resp_time != 0) { + /* IGMPv2 query */ + /* When changing compatibility mode, cancel all pending response + * and retransmission timers. + */ + pico_tree_foreach_safe(index, &IGMPTimers, _tmp) + { + ((struct igmp_timer *)index->keyValue)->stopped = IGMP_TIMER_STOPPED; + pico_tree_delete(&IGMPTimers, index->keyValue); + } + igmp_dbg("IGMP: switch to compatibility mode IGMPv2\n"); + link->mcast_compatibility = PICO_IGMPV2; + t.type = IGMP_TIMER_V2_QUERIER; + t.delay = ((IGMP_ROBUSTNESS * link->mcast_last_query_interval) + IGMP_QUERY_RESPONSE_INTERVAL) * 1000; + t.f = f; + t.callback = pico_igmp_v2querier_expired; + /* only one of this type of timer may exist! */ + pico_igmp_timer_start(&t); + } else { + /* IGMPv1 query, not supported */ + return -1; + } + } else { + /* invalid query, silently ignored */ + return -1; + } + + return 0; +} + +static struct igmp_parameters *pico_igmp_analyse_packet(struct pico_frame *f) +{ + struct igmp_message *message = NULL; + struct igmp_parameters *p = NULL; + struct pico_ipv4_link *link = NULL; + struct pico_ip4 mcast_group = { + 0 + }; + + link = pico_ipv4_link_by_dev(f->dev); + if (!link) + return NULL; + + /* IGMPv2 and IGMPv3 have a similar structure for the first 8 bytes */ + message = (struct igmp_message *)f->transport_hdr; + mcast_group.addr = message->mcast_group; + p = pico_igmp_find_parameter(&link->address, &mcast_group); + if (!p && mcast_group.addr == 0) { /* general query */ + p = PICO_ZALLOC(sizeof(struct igmp_parameters)); + if (!p) + return NULL; + + p->state = IGMP_STATE_NON_MEMBER; + p->mcast_link.addr = link->address.addr; + p->mcast_group.addr = mcast_group.addr; + pico_tree_insert(&IGMPParameters, p); + } else if (!p) { + return NULL; + } + + switch (message->type) { + case IGMP_TYPE_MEM_QUERY: + p->event = IGMP_EVENT_QUERY_RECV; + break; + case IGMP_TYPE_MEM_REPORT_V1: + p->event = IGMP_EVENT_REPORT_RECV; + break; + case IGMP_TYPE_MEM_REPORT_V2: + p->event = IGMP_EVENT_REPORT_RECV; + break; + case IGMP_TYPE_MEM_REPORT_V3: + p->event = IGMP_EVENT_REPORT_RECV; + break; + default: + return NULL; + } + p->max_resp_time = message->max_resp_time; /* if IGMPv3 report this will be 0 (res0 field) */ + p->f = f; + + return p; +} + +static int pico_igmp_process_in(struct pico_protocol *self, struct pico_frame *f) +{ + struct igmp_parameters *p = NULL; + IGNORE_PARAMETER(self); + + if (!pico_igmp_is_checksum_valid(f)) + goto out; + + if (pico_igmp_compatibility_mode(f) < 0) + goto out; + + p = pico_igmp_analyse_packet(f); + if (!p) + goto out; + + return pico_igmp_process_event(p); + +out: + pico_frame_discard(f); + return 0; +} + +static int pico_igmp_process_out(struct pico_protocol *self, struct pico_frame *f) +{ + /* packets are directly transferred to the IP layer by calling pico_ipv4_frame_push */ + IGNORE_PARAMETER(self); + IGNORE_PARAMETER(f); + return 0; +} + +/* Interface: protocol definition */ +struct pico_protocol pico_proto_igmp = { + .name = "igmp", + .proto_number = PICO_PROTO_IGMP, + .layer = PICO_LAYER_TRANSPORT, + .process_in = pico_igmp_process_in, + .process_out = pico_igmp_process_out, + .q_in = &igmp_in, + .q_out = &igmp_out, +}; + +int pico_igmp_state_change(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t filter_mode, struct pico_tree *_MCASTFilter, uint8_t state) +{ + struct igmp_parameters *p = NULL; + + if (mcast_group->addr == IGMP_ALL_HOST_GROUP) + return 0; + + p = pico_igmp_find_parameter(mcast_link, mcast_group); + if (!p && state == PICO_IGMP_STATE_CREATE) { + p = PICO_ZALLOC(sizeof(struct igmp_parameters)); + if (!p) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + if (!mcast_link || !mcast_group) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + p->state = IGMP_STATE_NON_MEMBER; + p->mcast_link = *mcast_link; + p->mcast_group = *mcast_group; + pico_tree_insert(&IGMPParameters, p); + } else if (!p) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + switch (state) { + case PICO_IGMP_STATE_CREATE: + p->event = IGMP_EVENT_CREATE_GROUP; + break; + + case PICO_IGMP_STATE_UPDATE: + p->event = IGMP_EVENT_UPDATE_GROUP; + break; + + case PICO_IGMP_STATE_DELETE: + p->event = IGMP_EVENT_DELETE_GROUP; + break; + + default: + return -1; + } + p->filter_mode = filter_mode; + p->MCASTFilter = _MCASTFilter; + + return pico_igmp_process_event(p); +} + +static int pico_igmp_send_report(struct igmp_parameters *p, struct pico_frame *f) +{ + struct pico_ip4 dst = { + 0 + }; + struct pico_ip4 mcast_group = { + 0 + }; + struct pico_ipv4_link *link = NULL; + + link = pico_ipv4_link_get(&p->mcast_link); + if (!link) + return -1; + + mcast_group.addr = p->mcast_group.addr; + switch (link->mcast_compatibility) { + case PICO_IGMPV2: + if (p->event == IGMP_EVENT_DELETE_GROUP) + dst.addr = IGMP_ALL_ROUTER_GROUP; + else + dst.addr = mcast_group.addr; + + break; + + case PICO_IGMPV3: + dst.addr = IGMPV3_ALL_ROUTER_GROUP; + break; + + default: + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; + } + + igmp_dbg("IGMP: send membership report on group %08X to %08X\n", mcast_group.addr, dst.addr); + pico_ipv4_frame_push(f, &dst, PICO_PROTO_IGMP); + return 0; +} + +static int8_t pico_igmp_generate_report(struct igmp_parameters *p) +{ + struct pico_ipv4_link *link = NULL; + int i = 0; + + link = pico_ipv4_link_get(&p->mcast_link); + if (!link) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + switch (link->mcast_compatibility) { + case PICO_IGMPV1: + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; + + case PICO_IGMPV2: + { + struct igmp_message *report = NULL; + uint8_t report_type = IGMP_TYPE_MEM_REPORT_V2; + if (p->event == IGMP_EVENT_DELETE_GROUP) + report_type = IGMP_TYPE_LEAVE_GROUP; + + p->f = pico_proto_ipv4.alloc(&pico_proto_ipv4, IP_OPTION_ROUTER_ALERT_LEN + sizeof(struct igmp_message)); + p->f->net_len = (uint16_t)(p->f->net_len + IP_OPTION_ROUTER_ALERT_LEN); + p->f->transport_hdr += IP_OPTION_ROUTER_ALERT_LEN; + p->f->transport_len = (uint16_t)(p->f->transport_len - IP_OPTION_ROUTER_ALERT_LEN); + p->f->dev = pico_ipv4_link_find(&p->mcast_link); + /* p->f->len is correctly set by alloc */ + + report = (struct igmp_message *)p->f->transport_hdr; + report->type = report_type; + report->max_resp_time = IGMP_DEFAULT_MAX_RESPONSE_TIME; + report->mcast_group = p->mcast_group.addr; + + report->crc = 0; + report->crc = short_be(pico_checksum(report, sizeof(struct igmp_message))); + break; + } + case PICO_IGMPV3: + { + struct igmpv3_report *report = NULL; + struct igmpv3_group_record *record = NULL; + struct pico_mcast_group *g = NULL, test = { + 0 + }; + struct pico_tree_node *index = NULL, *_tmp = NULL; + struct pico_tree *IGMPFilter = NULL; + struct pico_ip4 *source = NULL; + uint8_t record_type = 0; + uint8_t sources = 0; + uint16_t len = 0; + + test.mcast_addr = p->mcast_group; + g = pico_tree_findKey(link->MCASTGroups, &test); + if (!g) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + if (p->event == IGMP_EVENT_DELETE_GROUP) { /* "non-existent" state of filter mode INCLUDE and empty source list */ + p->filter_mode = PICO_IP_MULTICAST_INCLUDE; + p->MCASTFilter = NULL; + } + + if (p->event == IGMP_EVENT_QUERY_RECV) { + goto igmp3_report; + } + + + /* cleanup filters */ + pico_tree_foreach_safe(index, &IGMPAllow, _tmp) + { + pico_tree_delete(&IGMPAllow, index->keyValue); + } + pico_tree_foreach_safe(index, &IGMPBlock, _tmp) + { + pico_tree_delete(&IGMPBlock, index->keyValue); + } + + switch (g->filter_mode) { + + case PICO_IP_MULTICAST_INCLUDE: + switch (p->filter_mode) { + case PICO_IP_MULTICAST_INCLUDE: + if (p->event == IGMP_EVENT_DELETE_GROUP) { /* all ADD_SOURCE_MEMBERSHIP had an equivalent DROP_SOURCE_MEMBERSHIP */ + /* TO_IN (B) */ + record_type = IGMP_CHANGE_TO_INCLUDE_MODE; + IGMPFilter = &IGMPAllow; + if (p->MCASTFilter) { + pico_tree_foreach(index, p->MCASTFilter) /* B */ + { + pico_tree_insert(&IGMPAllow, index->keyValue); + sources++; + } + } /* else { IGMPAllow stays empty } */ + + break; + } + + /* ALLOW (B-A) */ + /* if event is CREATE A will be empty, thus only ALLOW (B-A) has sense */ + if (p->event == IGMP_EVENT_CREATE_GROUP) /* first ADD_SOURCE_MEMBERSHIP */ + record_type = IGMP_CHANGE_TO_INCLUDE_MODE; + else + record_type = IGMP_ALLOW_NEW_SOURCES; + + IGMPFilter = &IGMPAllow; + pico_tree_foreach(index, p->MCASTFilter) /* B */ + { + pico_tree_insert(&IGMPAllow, index->keyValue); + sources++; + } + pico_tree_foreach(index, &g->MCASTSources) /* A */ + { + source = pico_tree_findKey(&IGMPAllow, index->keyValue); + if (source) { + pico_tree_delete(&IGMPAllow, source); + sources--; + } + } + if (!pico_tree_empty(&IGMPAllow)) /* record type is ALLOW */ + break; + + /* BLOCK (A-B) */ + record_type = IGMP_BLOCK_OLD_SOURCES; + IGMPFilter = &IGMPBlock; + pico_tree_foreach(index, &g->MCASTSources) /* A */ + { + pico_tree_insert(&IGMPBlock, index->keyValue); + sources++; + } + pico_tree_foreach(index, p->MCASTFilter) /* B */ + { + source = pico_tree_findKey(&IGMPBlock, index->keyValue); + if (source) { + pico_tree_delete(&IGMPBlock, source); + sources--; + } + } + if (!pico_tree_empty(&IGMPBlock)) /* record type is BLOCK */ + break; + + /* ALLOW (B-A) and BLOCK (A-B) are empty: do not send report (RFC 3376 $5.1) */ + p->f = NULL; + return 0; + + case PICO_IP_MULTICAST_EXCLUDE: + /* TO_EX (B) */ + record_type = IGMP_CHANGE_TO_EXCLUDE_MODE; + IGMPFilter = &IGMPBlock; + pico_tree_foreach(index, p->MCASTFilter) /* B */ + { + pico_tree_insert(&IGMPBlock, index->keyValue); + sources++; + } + break; + + default: + pico_err = PICO_ERR_EINVAL; + return -1; + } + break; + + case PICO_IP_MULTICAST_EXCLUDE: + switch (p->filter_mode) { + case PICO_IP_MULTICAST_INCLUDE: + /* TO_IN (B) */ + record_type = IGMP_CHANGE_TO_INCLUDE_MODE; + IGMPFilter = &IGMPAllow; + if (p->MCASTFilter) { + pico_tree_foreach(index, p->MCASTFilter) /* B */ + { + pico_tree_insert(&IGMPAllow, index->keyValue); + sources++; + } + } /* else { IGMPAllow stays empty } */ + + break; + + case PICO_IP_MULTICAST_EXCLUDE: + /* BLOCK (B-A) */ + record_type = IGMP_BLOCK_OLD_SOURCES; + IGMPFilter = &IGMPBlock; + pico_tree_foreach(index, p->MCASTFilter) + { + pico_tree_insert(&IGMPBlock, index->keyValue); + sources++; + } + pico_tree_foreach(index, &g->MCASTSources) /* A */ + { + source = pico_tree_findKey(&IGMPBlock, index->keyValue); /* B */ + if (source) { + pico_tree_delete(&IGMPBlock, source); + sources--; + } + } + if (!pico_tree_empty(&IGMPBlock)) /* record type is BLOCK */ + break; + + /* ALLOW (A-B) */ + record_type = IGMP_ALLOW_NEW_SOURCES; + IGMPFilter = &IGMPAllow; + pico_tree_foreach(index, &g->MCASTSources) + { + pico_tree_insert(&IGMPAllow, index->keyValue); + sources++; + } + pico_tree_foreach(index, p->MCASTFilter) /* B */ + { + source = pico_tree_findKey(&IGMPAllow, index->keyValue); /* A */ + if (source) { + pico_tree_delete(&IGMPAllow, source); + sources--; + } + } + if (!pico_tree_empty(&IGMPAllow)) /* record type is ALLOW */ + break; + + /* BLOCK (B-A) and ALLOW (A-B) are empty: do not send report (RFC 3376 $5.1) */ + p->f = NULL; + return 0; + + default: + pico_err = PICO_ERR_EINVAL; + return -1; + } + break; + + default: + pico_err = PICO_ERR_EINVAL; + return -1; + } + +igmp3_report: + len = (uint16_t)(sizeof(struct igmpv3_report) + sizeof(struct igmpv3_group_record) + (sources * sizeof(struct pico_ip4))); + p->f = pico_proto_ipv4.alloc(&pico_proto_ipv4, (uint16_t)(IP_OPTION_ROUTER_ALERT_LEN + len)); + p->f->net_len = (uint16_t)(p->f->net_len + IP_OPTION_ROUTER_ALERT_LEN); + p->f->transport_hdr += IP_OPTION_ROUTER_ALERT_LEN; + p->f->transport_len = (uint16_t)(p->f->transport_len - IP_OPTION_ROUTER_ALERT_LEN); + p->f->dev = pico_ipv4_link_find(&p->mcast_link); + /* p->f->len is correctly set by alloc */ + + report = (struct igmpv3_report *)p->f->transport_hdr; + report->type = IGMP_TYPE_MEM_REPORT_V3; + report->res0 = 0; + report->crc = 0; + report->res1 = 0; + report->groups = short_be(1); + + record = (struct igmpv3_group_record *)(((uint8_t *)report) + sizeof(struct igmpv3_report)); + record->type = record_type; + record->aux = 0; + record->sources = short_be(sources); + record->mcast_group = p->mcast_group.addr; + if (IGMPFilter && !pico_tree_empty(IGMPFilter)) { + uint32_t *source_addr = (uint32_t *)((uint8_t *)record + sizeof(struct igmpv3_group_record)); + i = 0; + pico_tree_foreach(index, IGMPFilter) + { + source_addr[i] = ((struct pico_ip4 *)index->keyValue)->addr; + i++; + } + } + + report->crc = short_be(pico_checksum(report, len)); + break; + } + + default: + pico_err = PICO_ERR_EINVAL; + return -1; + } + return 0; +} + +/* stop timer, send leave if flag set */ +static int stslifs(struct igmp_parameters *p) +{ + struct igmp_timer t = { + 0 + }; + + igmp_dbg("IGMP: event = leave group | action = stop timer, send leave if flag set\n"); + + t.type = IGMP_TIMER_GROUP_REPORT; + t.mcast_link = p->mcast_link; + t.mcast_group = p->mcast_group; + if (pico_igmp_timer_stop(&t) < 0) + return -1; + + /* always send leave, even if not last host */ + if (pico_igmp_send_report(p, p->f) < 0) + return -1; + + pico_igmp_delete_parameter(p); + igmp_dbg("IGMP: new state = non-member\n"); + return 0; +} + +/* send report, set flag, start timer */ +static int srsfst(struct igmp_parameters *p) +{ + struct igmp_timer t = { + 0 + }; + struct pico_frame *copy_frame = NULL; + + igmp_dbg("IGMP: event = join group | action = send report, set flag, start timer\n"); + + p->last_host = IGMP_HOST_LAST; + + if (pico_igmp_generate_report(p) < 0) + return -1; + + if (!p->f) + return 0; + + copy_frame = pico_frame_copy(p->f); + if (!copy_frame) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + if (pico_igmp_send_report(p, copy_frame) < 0) + return -1; + + t.type = IGMP_TIMER_GROUP_REPORT; + t.mcast_link = p->mcast_link; + t.mcast_group = p->mcast_group; + t.delay = (pico_rand() % (IGMP_UNSOLICITED_REPORT_INTERVAL * 10000)); + t.f = p->f; + t.callback = pico_igmp_report_expired; + pico_igmp_timer_start(&t); + + p->state = IGMP_STATE_DELAYING_MEMBER; + igmp_dbg("IGMP: new state = delaying member\n"); + return 0; +} + +/* merge report, send report, reset timer (IGMPv3 only) */ +static int mrsrrt(struct igmp_parameters *p) +{ + struct igmp_timer *t = NULL; + struct pico_frame *copy_frame = NULL; + struct pico_ipv4_link *link = NULL; + + igmp_dbg("IGMP: event = update group | action = merge report, send report, reset timer (IGMPv3 only)\n"); + + link = pico_ipv4_link_get(&p->mcast_link); + if (!link) + return -1; + + if (link->mcast_compatibility != PICO_IGMPV3) { + igmp_dbg("IGMP: no IGMPv3 compatible router on network\n"); + return -1; + } + + /* XXX: merge with pending report rfc 3376 $5.1 */ + + copy_frame = pico_frame_copy(p->f); + if (!copy_frame) + return -1; + + if (pico_igmp_send_report(p, copy_frame) < 0) + return -1; + + t = pico_igmp_find_timer(IGMP_TIMER_GROUP_REPORT, &p->mcast_link, &p->mcast_group); + if (!t) + return -1; + + t->delay = (pico_rand() % (IGMP_UNSOLICITED_REPORT_INTERVAL * 10000)); + pico_igmp_timer_reset(t); + + p->state = IGMP_STATE_DELAYING_MEMBER; + igmp_dbg("IGMP: new state = delaying member\n"); + return 0; +} + +/* send report, start timer (IGMPv3 only) */ +static int srst(struct igmp_parameters *p) +{ + struct igmp_timer t = { + 0 + }; + struct pico_frame *copy_frame = NULL; + struct pico_ipv4_link *link = NULL; + + igmp_dbg("IGMP: event = update group | action = send report, start timer (IGMPv3 only)\n"); + + link = pico_ipv4_link_get(&p->mcast_link); + if (!link) + return -1; + + if (link->mcast_compatibility != PICO_IGMPV3) { + igmp_dbg("IGMP: no IGMPv3 compatible router on network\n"); + return -1; + } + + if (pico_igmp_generate_report(p) < 0) + return -1; + + if (!p->f) + return 0; + + copy_frame = pico_frame_copy(p->f); + if (!copy_frame) + return -1; + + if (pico_igmp_send_report(p, copy_frame) < 0) + return -1; + + t.type = IGMP_TIMER_GROUP_REPORT; + t.mcast_link = p->mcast_link; + t.mcast_group = p->mcast_group; + t.delay = (pico_rand() % (IGMP_UNSOLICITED_REPORT_INTERVAL * 10000)); + t.f = p->f; + t.callback = pico_igmp_report_expired; + pico_igmp_timer_start(&t); + + p->state = IGMP_STATE_DELAYING_MEMBER; + igmp_dbg("IGMP: new state = delaying member\n"); + return 0; +} + +/* send leave if flag set */ +static int slifs(struct igmp_parameters *p) +{ + igmp_dbg("IGMP: event = leave group | action = send leave if flag set\n"); + + /* always send leave, even if not last host */ + if (pico_igmp_send_report(p, p->f) < 0) + return -1; + + pico_igmp_delete_parameter(p); + igmp_dbg("IGMP: new state = non-member\n"); + return 0; +} + +/* start timer */ +static int st(struct igmp_parameters *p) +{ + struct igmp_timer t = { + 0 + }; + + igmp_dbg("IGMP: event = query received | action = start timer\n"); + + if (pico_igmp_generate_report(p) < 0) { + igmp_dbg("Failed to generate report\n"); + return -1; + } + + if (!p->f) { + igmp_dbg("No pending frame\n"); + return -1; + } + + t.type = IGMP_TIMER_GROUP_REPORT; + t.mcast_link = p->mcast_link; + t.mcast_group = p->mcast_group; + t.delay = (pico_rand() % ((1u + p->max_resp_time) * 100u)); + t.f = p->f; + t.callback = pico_igmp_report_expired; + pico_igmp_timer_start(&t); + + p->state = IGMP_STATE_DELAYING_MEMBER; + igmp_dbg("IGMP: new state = delaying member\n"); + return 0; +} + +/* stop timer, clear flag */ +static int stcl(struct igmp_parameters *p) +{ + struct igmp_timer t = { + 0 + }; + + igmp_dbg("IGMP: event = report received | action = stop timer, clear flag\n"); + + t.type = IGMP_TIMER_GROUP_REPORT; + t.mcast_link = p->mcast_link; + t.mcast_group = p->mcast_group; + if (pico_igmp_timer_stop(&t) < 0) + return -1; + + p->last_host = IGMP_HOST_NOT_LAST; + p->state = IGMP_STATE_IDLE_MEMBER; + igmp_dbg("IGMP: new state = idle member\n"); + return 0; +} + +/* send report, set flag */ +static int srsf(struct igmp_parameters *p) +{ + igmp_dbg("IGMP: event = timer expired | action = send report, set flag\n"); + + if (pico_igmp_send_report(p, p->f) < 0) + return -1; + + p->state = IGMP_STATE_IDLE_MEMBER; + igmp_dbg("IGMP: new state = idle member\n"); + return 0; +} + +/* reset timer if max response time < current timer */ +static int rtimrtct(struct igmp_parameters *p) +{ + struct igmp_timer *t = NULL; + uint32_t time_to_run = 0; + + igmp_dbg("IGMP: event = query received | action = reset timer if max response time < current timer\n"); + + t = pico_igmp_find_timer(IGMP_TIMER_GROUP_REPORT, &p->mcast_link, &p->mcast_group); + if (!t) + return -1; + + time_to_run = (uint32_t)(t->start + t->delay - PICO_TIME_MS()); + if ((p->max_resp_time * 100u) < time_to_run) { /* max_resp_time in units of 1/10 seconds */ + t->delay = pico_rand() % ((1u + p->max_resp_time) * 100u); + pico_igmp_timer_reset(t); + } + + p->state = IGMP_STATE_DELAYING_MEMBER; + igmp_dbg("IGMP: new state = delaying member\n"); + return 0; +} + +static int discard(struct igmp_parameters *p) +{ + igmp_dbg("IGMP: ignore and discard frame\n"); + pico_frame_discard(p->f); + return 0; +} + +/* finite state machine table */ +const callback host_membership_diagram_table[3][6] = +{ /* event |Delete Group |Create Group |Update Group |Query Received |Report Received |Timer Expired */ +/* state Non-Member */ + { discard, srsfst, srsfst, discard, discard, discard }, +/* state Delaying Member */ { stslifs, mrsrrt, mrsrrt, rtimrtct, stcl, srsf }, +/* state Idle Member */ { slifs, srst, srst, st, discard, discard } +}; + +static int pico_igmp_process_event(struct igmp_parameters *p) +{ + struct pico_tree_node *index = NULL; + struct igmp_parameters *_p = NULL; + + igmp_dbg("IGMP: process event on group address %08X\n", p->mcast_group.addr); + if (p->event == IGMP_EVENT_QUERY_RECV && p->mcast_group.addr == 0) { /* general query */ + pico_tree_foreach(index, &IGMPParameters) { + _p = index->keyValue; + _p->max_resp_time = p->max_resp_time; + _p->event = IGMP_EVENT_QUERY_RECV; + igmp_dbg("IGMP: for each mcast_group = %08X | state = %u\n", _p->mcast_group.addr, _p->state); + host_membership_diagram_table[_p->state][_p->event](_p); + } + } else { + igmp_dbg("IGMP: state = %u (0: non-member - 1: delaying member - 2: idle member)\n", p->state); + host_membership_diagram_table[p->state][p->event](p); + } + + return 0; +} + +#else +static struct pico_queue igmp_in = { + 0 +}; +static struct pico_queue igmp_out = { + 0 +}; + +static int pico_igmp_process_in(struct pico_protocol *self, struct pico_frame *f) { + IGNORE_PARAMETER(self); + IGNORE_PARAMETER(f); + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; +} + +static int pico_igmp_process_out(struct pico_protocol *self, struct pico_frame *f) { + IGNORE_PARAMETER(self); + IGNORE_PARAMETER(f); + return -1; +} + +/* Interface: protocol definition */ +struct pico_protocol pico_proto_igmp = { + .name = "igmp", + .proto_number = PICO_PROTO_IGMP, + .layer = PICO_LAYER_TRANSPORT, + .process_in = pico_igmp_process_in, + .process_out = pico_igmp_process_out, + .q_in = &igmp_in, + .q_out = &igmp_out, +}; + +int pico_igmp_state_change(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t filter_mode, struct pico_tree *_MCASTFilter, uint8_t state) { + IGNORE_PARAMETER(mcast_link); + IGNORE_PARAMETER(mcast_group); + IGNORE_PARAMETER(filter_mode); + IGNORE_PARAMETER(_MCASTFilter); + IGNORE_PARAMETER(state); + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; +} +#endif diff --git a/ext/picotcp/modules/pico_igmp.h b/ext/picotcp/modules/pico_igmp.h new file mode 100644 index 0000000..6f8d74e --- /dev/null +++ b/ext/picotcp/modules/pico_igmp.h @@ -0,0 +1,26 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + Authors: Kristof Roelants, Simon Maes, Brecht Van Cauwenberghe + *********************************************************************/ + +#ifndef INCLUDE_PICO_IGMP +#define INCLUDE_PICO_IGMP + +#define PICO_IGMPV1 1 +#define PICO_IGMPV2 2 +#define PICO_IGMPV3 3 + +#define PICO_IGMP_STATE_CREATE 1 +#define PICO_IGMP_STATE_UPDATE 2 +#define PICO_IGMP_STATE_DELETE 3 + +#define PICO_IGMP_QUERY_INTERVAL 125 + +extern struct pico_protocol pico_proto_igmp; + +int pico_igmp_state_change(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t filter_mode, struct pico_tree *_MCASTFilter, uint8_t state); +#endif /* _INCLUDE_PICO_IGMP */ diff --git a/ext/picotcp/modules/pico_ipfilter.c b/ext/picotcp/modules/pico_ipfilter.c new file mode 100644 index 0000000..64ee349 --- /dev/null +++ b/ext/picotcp/modules/pico_ipfilter.c @@ -0,0 +1,458 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Andrei Carp + Simon Maes + *********************************************************************/ + +#include "pico_ipv4.h" +#include "pico_config.h" +#include "pico_icmp4.h" +#include "pico_stack.h" +#include "pico_eth.h" +#include "pico_socket.h" +#include "pico_device.h" +#include "pico_ipfilter.h" +#include "pico_tcp.h" +#include "pico_udp.h" +#include "pico_tree.h" + +/**************** LOCAL MACROS ****************/ +#define MAX_PRIORITY (10) +#define MIN_PRIORITY (-10) + +#define ipf_dbg(...) do {} while(0) + +/**************** LOCAL DECLARATIONS ****************/ +struct filter_node; +static int filter_compare(void *filterA, void *filterB); + +/**************** FILTER TREE ****************/ + +struct filter_node { + struct pico_device *fdev; + /* output address */ + uint32_t out_addr; + uint32_t out_addr_netmask; + /* input address */ + uint32_t in_addr; + uint32_t in_addr_netmask; + /* transport */ + uint16_t out_port; + uint16_t in_port; + /* filter details */ + uint8_t proto; + int8_t priority; + uint8_t tos; + uint32_t filter_id; + int (*function_ptr)(struct filter_node *filter, struct pico_frame *f); +}; + +PICO_TREE_DECLARE(filter_tree, &filter_compare); + +static inline int ipfilter_uint32_cmp(uint32_t a, uint32_t b) +{ + if (a < b) + return -1; + + if (b < a) + return 1; + + return 0; +} + +static inline int ipfilter_uint16_cmp(uint16_t a, uint16_t b) +{ + if (a < b) + return -1; + + if (b < a) + return 1; + + return 0; +} + +static inline int ipfilter_uint8_cmp(uint8_t a, uint8_t b) +{ + if (a < b) + return -1; + + if (b < a) + return 1; + + return 0; +} + +static inline int ipfilter_ptr_cmp(void *a, void *b) +{ + if (a < b) + return -1; + + if (b < a) + return 1; + + return 0; +} + + + +static inline int filter_compare_ports(struct filter_node *a, struct filter_node *b) +{ + int cmp; + cmp = ipfilter_uint16_cmp(a->in_port, b->in_port); + if (cmp) + return cmp; + + cmp = ipfilter_uint16_cmp(a->out_port, b->out_port); + return cmp; +} + +static inline int filter_compare_addresses(struct filter_node *a, struct filter_node *b) +{ + int cmp; + /* Compare source address */ + cmp = ipfilter_uint32_cmp((a->in_addr & a->in_addr_netmask), (b->in_addr & b->in_addr_netmask)); + if (cmp) + return cmp; + + /* Compare destination address */ + cmp = ipfilter_uint32_cmp((a->out_addr & a->out_addr_netmask), (b->out_addr & b->out_addr_netmask)); + return cmp; +} + +static inline int filter_compare_proto(struct filter_node *a, struct filter_node *b) +{ + return ipfilter_uint8_cmp(a->proto, b->proto); +} + +static inline int filter_compare_address_port(struct filter_node *a, struct filter_node *b) +{ + int cmp; + cmp = filter_compare_addresses(a, b); + if (cmp) + return cmp; + + return filter_compare_ports(a, b); +} + +static inline int filter_match_packet_dev(struct filter_node *a, struct filter_node *b, struct filter_node *rule) +{ + int cmp; + /* 1. Compare devices */ + if (rule->fdev) { + cmp = ipfilter_ptr_cmp(a->fdev, b->fdev); + if (cmp) + return cmp; + } + + return 0; + +} + +static inline int filter_match_packet_proto(struct filter_node *a, struct filter_node *b, struct filter_node *rule) +{ + int cmp; + /* 2. Compare protocol */ + if (rule->proto) { + cmp = filter_compare_proto(a, b); + if (cmp) + return cmp; + } + + return 0; + +} +static inline int filter_match_packet_addr_in(struct filter_node *a, struct filter_node *b, struct filter_node *rule) +{ + int cmp; + /* 3. Compare addresses order: in, out */ + if (rule->in_addr_netmask) { + cmp = ipfilter_uint32_cmp(a->in_addr & rule->in_addr_netmask, b->in_addr & rule->in_addr_netmask); + if (cmp) + return cmp; + } + + return 0; +} +static inline int filter_match_packet_addr_out(struct filter_node *a, struct filter_node *b, struct filter_node *rule) +{ + int cmp; + if (rule->out_addr_netmask) { + cmp = ipfilter_uint32_cmp(a->out_addr & rule->out_addr_netmask, b->out_addr & rule->out_addr_netmask); + if (cmp) { + return cmp; + } + } + + return 0; +} +static inline int filter_match_packet_port_in(struct filter_node *a, struct filter_node *b, struct filter_node *rule) +{ + int cmp; + /* 4. Compare ports order: in, out */ + if (rule->in_port) { + cmp = ipfilter_uint16_cmp(a->in_port, b->in_port); + if (cmp) + return cmp; + } + + return 0; +} +static inline int filter_match_packet_port_out(struct filter_node *a, struct filter_node *b, struct filter_node *rule) +{ + int cmp; + if (rule->out_port) { + cmp = ipfilter_uint16_cmp(a->out_port, b->out_port); + if (cmp) + return cmp; + } + + return 0; +} + +static inline int filter_match_packet_dev_and_proto(struct filter_node *a, struct filter_node *b, struct filter_node *rule) +{ + int cmp = filter_match_packet_dev(a, b, rule); + if (cmp) + return cmp; + + return filter_match_packet_proto(a, b, rule); +} + +static inline int filter_match_packet_addr(struct filter_node *a, struct filter_node *b, struct filter_node *rule) +{ + int cmp = filter_match_packet_addr_in(a, b, rule); + if (cmp) + return cmp; + + return filter_match_packet_addr_out(a, b, rule); + +} + +static inline int filter_match_packet_port(struct filter_node *a, struct filter_node *b, struct filter_node *rule) +{ + int cmp = filter_match_packet_port_in(a, b, rule); + if (cmp) + return cmp; + + return filter_match_packet_port_out(a, b, rule); +} + +static inline struct filter_node *filter_match_packet_find_rule(struct filter_node *a, struct filter_node *b) +{ + if (!a->filter_id) + return b; + + return a; +} + +static inline int filter_match_packet(struct filter_node *a, struct filter_node *b) +{ + struct filter_node *rule; + int cmp = 0; + rule = filter_match_packet_find_rule(a, b); + + cmp = filter_match_packet_dev_and_proto(a, b, rule); + if (cmp) + return cmp; + + cmp = filter_match_packet_addr(a, b, rule); + if (cmp) + return cmp; + + cmp = filter_match_packet_port(a, b, rule); + if (cmp) + return cmp; + + return 0; +} + + +int filter_compare(void *filterA, void *filterB) +{ + + struct filter_node *a = (struct filter_node *)filterA; + struct filter_node *b = (struct filter_node *)filterB; + int cmp = 0; + if (a->filter_id == 0 || b->filter_id == 0) { + return filter_match_packet(a, b); + } + + /* improve the search */ + if(a->filter_id == b->filter_id) + return 0; + + /* 1. Compare devices */ + cmp = ipfilter_ptr_cmp(a->fdev, a->fdev); + if (cmp) + return cmp; + + /* 2. Compare protocol */ + cmp = filter_compare_proto(a, b); + if(cmp) + return cmp; + + /* 3. Compare addresses order: in, out */ + /* 4. Compare ports order: in, out */ + cmp = filter_compare_address_port(a, b); + + return cmp; +} + +/**************** FILTER CALLBACKS ****************/ + +static int fp_priority(struct filter_node *filter, struct pico_frame *f) +{ + /* TODO do priority-stuff */ + IGNORE_PARAMETER(filter); + IGNORE_PARAMETER(f); + return 0; +} + +static int fp_reject(struct filter_node *filter, struct pico_frame *f) +{ +/* TODO check first if sender is pico itself or not */ + IGNORE_PARAMETER(filter); + ipf_dbg("ipfilter> reject\n"); + (void)pico_icmp4_packet_filtered(f); + pico_frame_discard(f); + return 1; +} + +static int fp_drop(struct filter_node *filter, struct pico_frame *f) +{ + IGNORE_PARAMETER(filter); + ipf_dbg("ipfilter> drop\n"); + pico_frame_discard(f); + return 1; +} + +struct fp_function { + int (*fn)(struct filter_node *filter, struct pico_frame *f); +}; + + +static const struct fp_function fp_function[FILTER_COUNT] = +{ + {&fp_priority}, + {&fp_reject}, + {&fp_drop} +}; + +static int pico_ipv4_filter_add_validate(int8_t priority, enum filter_action action) +{ + if ( priority > MAX_PRIORITY || priority < MIN_PRIORITY) { + return -1; + } + + if (action >= FILTER_COUNT) { + return -1; + } + + return 0; +} + + +/**************** FILTER API's ****************/ +uint32_t pico_ipv4_filter_add(struct pico_device *dev, uint8_t proto, + struct pico_ip4 *out_addr, struct pico_ip4 *out_addr_netmask, + struct pico_ip4 *in_addr, struct pico_ip4 *in_addr_netmask, + uint16_t out_port, uint16_t in_port, int8_t priority, + uint8_t tos, enum filter_action action) +{ + static uint32_t filter_id = 1u; /* 0 is a special value used in the binary-tree search for packets being processed */ + struct filter_node *new_filter; + + if (pico_ipv4_filter_add_validate(priority, action) < 0) { + pico_err = PICO_ERR_EINVAL; + return 0; + } + + new_filter = PICO_ZALLOC(sizeof(struct filter_node)); + if (!new_filter) { + pico_err = PICO_ERR_ENOMEM; + return 0; + } + + new_filter->fdev = dev; + new_filter->proto = proto; + new_filter->out_addr = (!out_addr) ? (0U) : (out_addr->addr); + new_filter->out_addr_netmask = (!out_addr_netmask) ? (0U) : (out_addr_netmask->addr); + new_filter->in_addr = (!in_addr) ? (0U) : (in_addr->addr); + new_filter->in_addr_netmask = (!in_addr_netmask) ? (0U) : (in_addr_netmask->addr); + new_filter->out_port = out_port; + new_filter->in_port = in_port; + new_filter->priority = priority; + new_filter->tos = tos; + new_filter->filter_id = filter_id++; + new_filter->function_ptr = fp_function[action].fn; + + if(pico_tree_insert(&filter_tree, new_filter)) + { + PICO_FREE(new_filter); + filter_id--; + return 0; + } + + return new_filter->filter_id; +} + +int pico_ipv4_filter_del(uint32_t filter_id) +{ + struct filter_node *node = NULL; + struct filter_node dummy = { 0 }; + + dummy.filter_id = filter_id; + if((node = pico_tree_delete(&filter_tree, &dummy)) == NULL) + { + ipf_dbg("ipfilter> failed to delete filter :%d\n", filter_id); + return -1; + } + + PICO_FREE(node); + return 0; +} + +static int ipfilter_apply_filter(struct pico_frame *f, struct filter_node *pkt) +{ + struct filter_node *filter_frame = NULL; + filter_frame = pico_tree_findKey(&filter_tree, pkt); + if(filter_frame) + { + filter_frame->function_ptr(filter_frame, f); + return 1; + } + + return 0; +} + +int ipfilter(struct pico_frame *f) +{ + struct filter_node temp; + struct pico_ipv4_hdr *ipv4_hdr = (struct pico_ipv4_hdr *) f->net_hdr; + struct pico_trans *trans; + struct pico_icmp4_hdr *icmp_hdr; + + memset(&temp, 0u, sizeof(struct filter_node)); + + temp.fdev = f->dev; + temp.out_addr = ipv4_hdr->dst.addr; + temp.in_addr = ipv4_hdr->src.addr; + if ((ipv4_hdr->proto == PICO_PROTO_TCP) || (ipv4_hdr->proto == PICO_PROTO_UDP)) { + trans = (struct pico_trans *) f->transport_hdr; + temp.out_port = short_be(trans->dport); + temp.in_port = short_be(trans->sport); + } + else if(ipv4_hdr->proto == PICO_PROTO_ICMP4) { + icmp_hdr = (struct pico_icmp4_hdr *) f->transport_hdr; + if(icmp_hdr->type == PICO_ICMP_UNREACH && icmp_hdr->type == PICO_ICMP_UNREACH_FILTER_PROHIB) + return 0; + } + + temp.proto = ipv4_hdr->proto; + temp.priority = f->priority; + temp.tos = ipv4_hdr->tos; + return ipfilter_apply_filter(f, &temp); +} + diff --git a/ext/picotcp/modules/pico_ipfilter.h b/ext/picotcp/modules/pico_ipfilter.h new file mode 100644 index 0000000..fb92e67 --- /dev/null +++ b/ext/picotcp/modules/pico_ipfilter.h @@ -0,0 +1,29 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Simon Maes + *********************************************************************/ +#ifndef INCLUDE_PICO_IPFILTER +#define INCLUDE_PICO_IPFILTER + +#include "pico_device.h" + +enum filter_action { + FILTER_PRIORITY = 0, + FILTER_REJECT, + FILTER_DROP, + FILTER_COUNT +}; + +uint32_t pico_ipv4_filter_add(struct pico_device *dev, uint8_t proto, + struct pico_ip4 *out_addr, struct pico_ip4 *out_addr_netmask, struct pico_ip4 *in_addr, + struct pico_ip4 *in_addr_netmask, uint16_t out_port, uint16_t in_port, + int8_t priority, uint8_t tos, enum filter_action action); + +int pico_ipv4_filter_del(uint32_t filter_id); + +int ipfilter(struct pico_frame *f); + +#endif /* _INCLUDE_PICO_IPFILTER */ + diff --git a/ext/picotcp/modules/pico_ipv4.c b/ext/picotcp/modules/pico_ipv4.c new file mode 100644 index 0000000..7159549 --- /dev/null +++ b/ext/picotcp/modules/pico_ipv4.c @@ -0,0 +1,1585 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Daniele Lacamera, Markian Yskout + *********************************************************************/ + + +#include "pico_config.h" +#include "pico_ipfilter.h" +#include "pico_ipv4.h" +#include "pico_icmp4.h" +#include "pico_stack.h" +#include "pico_eth.h" +#include "pico_udp.h" +#include "pico_tcp.h" +#include "pico_socket.h" +#include "pico_device.h" +#include "pico_nat.h" +#include "pico_igmp.h" +#include "pico_tree.h" +#include "pico_aodv.h" +#include "pico_socket_multicast.h" +#include "pico_fragments.h" + +#ifdef PICO_SUPPORT_IPV4 + +#ifdef PICO_SUPPORT_MCAST +# define ip_mcast_dbg(...) do {} while(0) /* so_mcast_dbg in pico_socket.c */ +/* #define ip_mcast_dbg dbg */ +# define PICO_MCAST_ALL_HOSTS long_be(0xE0000001) /* 224.0.0.1 */ +/* Default network interface for multicast transmission */ +static struct pico_ipv4_link *mcast_default_link = NULL; +#endif +#ifdef PICO_SUPPORT_IPV4FRAG +/* # define reassembly_dbg dbg */ +# define reassembly_dbg(...) do {} while(0) +#endif + +/* Queues */ +static struct pico_queue in = { + 0 +}; +static struct pico_queue out = { + 0 +}; + +/* Functions */ +static int ipv4_route_compare(void *ka, void *kb); +static struct pico_frame *pico_ipv4_alloc(struct pico_protocol *self, uint16_t size); + + +int pico_ipv4_compare(struct pico_ip4 *a, struct pico_ip4 *b) +{ + if (a->addr < b->addr) + return -1; + + if (a->addr > b->addr) + return 1; + + return 0; +} + +int pico_ipv4_to_string(char *ipbuf, const uint32_t ip) +{ + const unsigned char *addr = (const unsigned char *) &ip; + int i; + + if (!ipbuf) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + for(i = 0; i < 4; i++) + { + if (addr[i] > 99) { + *ipbuf++ = (char)('0' + (addr[i] / 100)); + *ipbuf++ = (char)('0' + ((addr[i] % 100) / 10)); + *ipbuf++ = (char)('0' + ((addr[i] % 100) % 10)); + } else if (addr[i] > 9) { + *ipbuf++ = (char)('0' + (addr[i] / 10)); + *ipbuf++ = (char)('0' + (addr[i] % 10)); + } else { + *ipbuf++ = (char)('0' + addr[i]); + } + + if (i < 3) + *ipbuf++ = '.'; + } + *ipbuf = '\0'; + + return 0; +} + +static int pico_string_check_null_args(const char *ipstr, uint32_t *ip) +{ + + if (!ipstr || !ip) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + return 0; + +} + +int pico_string_to_ipv4(const char *ipstr, uint32_t *ip) +{ + unsigned char buf[PICO_SIZE_IP4] = { + 0 + }; + int cnt = 0; + char p; + + if (pico_string_check_null_args(ipstr, ip) < 0) + return -1; + + while((p = *ipstr++) != 0 && cnt < PICO_SIZE_IP4) + { + if (pico_is_digit(p)) { + buf[cnt] = (uint8_t)((10 * buf[cnt]) + (p - '0')); + } else if (p == '.') { + cnt++; + } else { + return -1; + } + } + /* Handle short notation */ + if (cnt == 1) { + buf[3] = buf[1]; + buf[1] = 0; + buf[2] = 0; + } else if (cnt == 2) { + buf[3] = buf[2]; + buf[2] = 0; + } else if (cnt != 3) { + /* String could not be parsed, return error */ + return -1; + } + + *ip = long_from(buf); + + return 0; +} + +int pico_ipv4_valid_netmask(uint32_t mask) +{ + int cnt = 0; + int end = 0; + int i; + uint32_t mask_swap = long_be(mask); + + /* + * Swap bytes for convenient parsing + * e.g. 0x..f8ff will become 0xfff8.. + * Then, we count the consecutive bits + * + * */ + + for(i = 0; i < 32; i++) { + if ((mask_swap << i) & 0x80000000) { + if (end) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + cnt++; + } else { + end = 1; + } + } + return cnt; +} + +int pico_ipv4_is_unicast(uint32_t address) +{ + const unsigned char *addr = (unsigned char *) &address; + if ((addr[0] & 0xe0) == 0xe0) + return 0; /* multicast */ + + return 1; +} + +int pico_ipv4_is_multicast(uint32_t address) +{ + const unsigned char *addr = (unsigned char *) &address; + if ((addr[0] != 0xff) && ((addr[0] & 0xe0) == 0xe0)) + return 1; /* multicast */ + + return 0; +} + +int pico_ipv4_is_loopback(uint32_t address) +{ + const unsigned char *addr = (unsigned char *) &address; + if (addr[0] == 0x7f) + return 1; + + return 0; +} + +static int pico_ipv4_is_invalid_loopback(uint32_t address, struct pico_device *dev) +{ + return pico_ipv4_is_loopback(address) && ((!dev) || strcmp(dev->name, "loop")); +} + +int pico_ipv4_is_valid_src(uint32_t address, struct pico_device *dev) +{ + if (pico_ipv4_is_broadcast(address)) { + dbg("Source is a broadcast address, discard packet\n"); + return 0; + } else if ( pico_ipv4_is_multicast(address)) { + dbg("Source is a multicast address, discard packet\n"); + return 0; + } else if (pico_ipv4_is_invalid_loopback(address, dev)) { + dbg("Source is a loopback address, discard packet\n"); + return 0; + } else { +#ifdef PICO_SUPPORT_AODV + union pico_address src; + src.ip4.addr = address; + pico_aodv_refresh(&src); +#endif + return 1; + } +} + +static int pico_ipv4_checksum(struct pico_frame *f) +{ + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; + if (!hdr) + return -1; + + hdr->crc = 0; + hdr->crc = short_be(pico_checksum(hdr, f->net_len)); + return 0; +} + + +#ifdef PICO_SUPPORT_CRC +static inline int pico_ipv4_crc_check(struct pico_frame *f) +{ + uint16_t checksum_invalid = 1; + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; + + checksum_invalid = short_be(pico_checksum(hdr, f->net_len)); + if (checksum_invalid) { + dbg("IP: checksum failed!\n"); + pico_frame_discard(f); + return 0; + } + + return 1; +} +#else +static inline int pico_ipv4_crc_check(struct pico_frame *f) +{ + IGNORE_PARAMETER(f); + return 1; +} +#endif /* PICO_SUPPORT_CRC */ + +static int pico_ipv4_forward(struct pico_frame *f); +#ifdef PICO_SUPPORT_MCAST +static int pico_ipv4_mcast_filter(struct pico_frame *f); +#endif + +static int ipv4_link_compare(void *ka, void *kb) +{ + struct pico_ipv4_link *a = ka, *b = kb; + int cmp = pico_ipv4_compare(&a->address, &b->address); + if (cmp) + return cmp; + + /* zero can be assigned multiple times (e.g. for DHCP) */ + if (a->dev != NULL && b->dev != NULL && a->address.addr == PICO_IP4_ANY && b->address.addr == PICO_IP4_ANY) { + if (a->dev < b->dev) + return -1; + + if (a->dev > b->dev) + return 1; + } + + return 0; +} + +PICO_TREE_DECLARE(Tree_dev_link, ipv4_link_compare); + +static int pico_ipv4_process_bcast_in(struct pico_frame *f) +{ + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; +#ifdef PICO_SUPPORT_UDP + if (pico_ipv4_is_broadcast(hdr->dst.addr) && (hdr->proto == PICO_PROTO_UDP)) { + /* Receiving UDP broadcast datagram */ + f->flags |= PICO_FRAME_FLAG_BCAST; + pico_enqueue(pico_proto_udp.q_in, f); + return 1; + } + +#endif + +#ifdef PICO_SUPPORT_ICMP4 + if (pico_ipv4_is_broadcast(hdr->dst.addr) && (hdr->proto == PICO_PROTO_ICMP4)) { + /* Receiving ICMP4 bcast packet */ + f->flags |= PICO_FRAME_FLAG_BCAST; + pico_enqueue(pico_proto_icmp4.q_in, f); + return 1; + } + +#endif + return 0; +} + +static int pico_ipv4_process_mcast_in(struct pico_frame *f) +{ + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; + if (pico_ipv4_is_multicast(hdr->dst.addr)) { +#ifdef PICO_SUPPORT_IGMP + /* Receiving UDP multicast datagram TODO set f->flags? */ + if (hdr->proto == PICO_PROTO_IGMP) { + ip_mcast_dbg("MCAST: received IGMP message\n"); + pico_transport_receive(f, PICO_PROTO_IGMP); + return 1; + } else if ((pico_ipv4_mcast_filter(f) == 0) && (hdr->proto == PICO_PROTO_UDP)) { + pico_enqueue(pico_proto_udp.q_in, f); + return 1; + } + +#endif + pico_frame_discard(f); + return 1; + } + + return 0; +} + +static int pico_ipv4_process_local_unicast_in(struct pico_frame *f) +{ + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; + struct pico_ipv4_link test = { + .address = {.addr = PICO_IP4_ANY}, .dev = NULL + }; + if (pico_ipv4_link_find(&hdr->dst)) { + if (pico_ipv4_nat_inbound(f, &hdr->dst) == 0) + pico_enqueue(pico_proto_ipv4.q_in, f); /* dst changed, reprocess */ + else + pico_transport_receive(f, hdr->proto); + + return 1; + } else if (pico_tree_findKey(&Tree_dev_link, &test)) { +#ifdef PICO_SUPPORT_UDP + /* address of this device is apparently 0.0.0.0; might be a DHCP packet */ + /* XXX KRO: is obsolete. Broadcast flag is set on outgoing DHCP messages. + * incomming DHCP messages are to be broadcasted. Our current DHCP server + * implementation does not take this flag into account yet though ... */ + pico_enqueue(pico_proto_udp.q_in, f); + return 1; +#endif + } + + return 0; +} + +static void pico_ipv4_process_finally_try_forward(struct pico_frame *f) +{ + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; + if ((pico_ipv4_is_broadcast(hdr->dst.addr)) || ((f->flags & PICO_FRAME_FLAG_BCAST)!= 0)) { + /* don't forward broadcast frame, discard! */ + pico_frame_discard(f); + } else if (pico_ipv4_forward(f) != 0) { + pico_frame_discard(f); + /* dbg("Forward failed.\n"); */ + } +} + + + +static int pico_ipv4_process_in(struct pico_protocol *self, struct pico_frame *f) +{ + uint8_t option_len = 0; + int ret = 0; + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; + uint16_t max_allowed = (uint16_t) ((int)f->buffer_len - (f->net_hdr - f->buffer) - (int)PICO_SIZE_IP4HDR); + uint16_t flag = short_be(hdr->frag); + + (void)self; + + if (!hdr) + return -1; + /* NAT needs transport header information */ + if (((hdr->vhl) & 0x0F) > 5) { + option_len = (uint8_t)(4 * (((hdr->vhl) & 0x0F) - 5)); + } + + f->transport_hdr = ((uint8_t *)f->net_hdr) + PICO_SIZE_IP4HDR + option_len; + f->transport_len = (uint16_t)(short_be(hdr->len) - PICO_SIZE_IP4HDR - option_len); + f->net_len = (uint16_t)(PICO_SIZE_IP4HDR + option_len); + + if (f->transport_len > max_allowed) { + pico_frame_discard(f); + return 0; /* Packet is discarded due to unfeasible length */ + } + +#ifdef PICO_SUPPORT_IPFILTER + if (ipfilter(f)) { + /*pico_frame is discarded as result of the filtering*/ + return 0; + } + +#endif + + + /* ret == 1 indicates to continue the function */ + ret = pico_ipv4_crc_check(f); + if (ret < 1) + return ret; + + /* Validate source IP address. Discard quietly if invalid */ + if (!pico_ipv4_is_valid_src(hdr->src.addr, f->dev)) { + pico_frame_discard(f); + return 0; + } + + if (hdr->frag & short_be(PICO_IPV4_EVIL)) { + (void)pico_icmp4_param_problem(f, 0); + pico_frame_discard(f); /* RFC 3514 */ + return 0; + } + + if ((hdr->vhl & 0x0f) < 5) { + /* RFC 791: IHL minimum value is 5 */ + (void)pico_icmp4_param_problem(f, 0); + pico_frame_discard(f); + return 0; + } + + if (flag & (PICO_IPV4_MOREFRAG | PICO_IPV4_FRAG_MASK)) + { +#ifdef PICO_SUPPORT_IPV4FRAG + pico_ipv4_process_frag(hdr, f, hdr ? hdr->proto : 0 ); + /* Frame can be discarded, frag will handle its own copy */ +#endif + /* We do not support fragmentation, discard quietly */ + pico_frame_discard(f); + return 0; + } + + if (pico_ipv4_process_bcast_in(f) > 0) + return 0; + + if (pico_ipv4_process_mcast_in(f) > 0) + return 0; + + if (pico_ipv4_process_local_unicast_in(f) > 0) + return 0; + + pico_ipv4_process_finally_try_forward(f); + + return 0; +} + +PICO_TREE_DECLARE(Routes, ipv4_route_compare); + + +static int pico_ipv4_process_out(struct pico_protocol *self, struct pico_frame *f) +{ + IGNORE_PARAMETER(self); + f->start = (uint8_t*) f->net_hdr; +#ifdef PICO_SUPPORT_IPFILTER + if (ipfilter(f)) { + /*pico_frame is discarded as result of the filtering*/ + return 0; + } + +#endif + return pico_sendto_dev(f); +} + + +static struct pico_frame *pico_ipv4_alloc(struct pico_protocol *self, uint16_t size) +{ + struct pico_frame *f = pico_frame_alloc(size + PICO_SIZE_IP4HDR + PICO_SIZE_ETHHDR); + IGNORE_PARAMETER(self); + + if (!f) + return NULL; + + f->datalink_hdr = f->buffer; + f->net_hdr = f->buffer + PICO_SIZE_ETHHDR; + f->net_len = PICO_SIZE_IP4HDR; + f->transport_hdr = f->net_hdr + PICO_SIZE_IP4HDR; + f->transport_len = size; + f->len = size + PICO_SIZE_IP4HDR; + return f; +} + +static int pico_ipv4_frame_sock_push(struct pico_protocol *self, struct pico_frame *f); + +/* Interface: protocol definition */ +struct pico_protocol pico_proto_ipv4 = { + .name = "ipv4", + .proto_number = PICO_PROTO_IPV4, + .layer = PICO_LAYER_NETWORK, + .alloc = pico_ipv4_alloc, + .process_in = pico_ipv4_process_in, + .process_out = pico_ipv4_process_out, + .push = pico_ipv4_frame_sock_push, + .q_in = &in, + .q_out = &out, +}; + + +static int ipv4_route_compare(void *ka, void *kb) +{ + struct pico_ipv4_route *a = ka, *b = kb; + uint32_t a_nm, b_nm; + int cmp; + + a_nm = long_be(a->netmask.addr); + b_nm = long_be(b->netmask.addr); + + /* Routes are sorted by (host side) netmask len, then by addr, then by metric. */ + if (a_nm < b_nm) + return -1; + + if (b_nm < a_nm) + return 1; + + cmp = pico_ipv4_compare(&a->dest, &b->dest); + if (cmp) + return cmp; + + if (a->metric < b->metric) + return -1; + + if (a->metric > b->metric) + return 1; + + return 0; +} + + +static struct pico_ipv4_route default_bcast_route = { + .dest = {PICO_IP4_BCAST}, + .netmask = {PICO_IP4_BCAST}, + .gateway = { 0 }, + .link = NULL, + .metric = 1000 +}; + +static struct pico_ipv4_route *route_find_default_bcast(void) +{ + return &default_bcast_route; +} + + +static struct pico_ipv4_route *route_find(const struct pico_ip4 *addr) +{ + struct pico_ipv4_route *r; + struct pico_tree_node *index; + + if (addr->addr != PICO_IP4_BCAST) { + pico_tree_foreach_reverse(index, &Routes) { + r = index->keyValue; + if ((addr->addr & (r->netmask.addr)) == (r->dest.addr)) { + return r; + } + } + return NULL; + } + + return route_find_default_bcast(); +} + +struct pico_ip4 pico_ipv4_route_get_gateway(struct pico_ip4 *addr) +{ + struct pico_ip4 nullip; + struct pico_ipv4_route *route; + nullip.addr = 0U; + + if (!addr) { + pico_err = PICO_ERR_EINVAL; + return nullip; + } + + route = route_find(addr); + if (!route) { + pico_err = PICO_ERR_EHOSTUNREACH; + return nullip; + } + else + return route->gateway; +} + +struct pico_ip4 *pico_ipv4_source_find(const struct pico_ip4 *dst) +{ + struct pico_ip4 *myself = NULL; + struct pico_ipv4_route *rt; +#ifdef PICO_SUPPORT_AODV + union pico_address node_address; + node_address.ip4.addr = dst->addr; + if (dst->addr && pico_ipv4_is_unicast(dst->addr)) + pico_aodv_lookup(&node_address); + +#endif + + if (!dst) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + rt = route_find(dst); + if (rt && rt->link) { + myself = &rt->link->address; + } else { + pico_err = PICO_ERR_EHOSTUNREACH; + } + + return myself; +} + +struct pico_device *pico_ipv4_source_dev_find(const struct pico_ip4 *dst) +{ + struct pico_device *dev = NULL; + struct pico_ipv4_route *rt; + + if (!dst) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + rt = route_find(dst); + if (rt && rt->link) { + dev = rt->link->dev; + } else { + pico_err = PICO_ERR_EHOSTUNREACH; + } + + return dev; +} + + +#ifdef PICO_SUPPORT_MCAST +/* link + * | + * MCASTGroups + * | | | + * ------------ | ------------ + * | | | + * MCASTSources MCASTSources MCASTSources + * | | | | | | | | | | | | + * S S S S S S S S S S S S + * + * MCASTGroups: RBTree(mcast_group) + * MCASTSources: RBTree(source) + */ +static int ipv4_mcast_groups_cmp(void *ka, void *kb) +{ + struct pico_mcast_group *a = ka, *b = kb; + return pico_ipv4_compare(&a->mcast_addr, &b->mcast_addr); +} + +static int ipv4_mcast_sources_cmp(void *ka, void *kb) +{ + struct pico_ip4 *a = ka, *b = kb; + return pico_ipv4_compare(a, b); +} + +static void pico_ipv4_mcast_print_groups(struct pico_ipv4_link *mcast_link) +{ + uint16_t i = 0; + struct pico_mcast_group *g = NULL; + struct pico_ip4 *source = NULL; + struct pico_tree_node *index = NULL, *index2 = NULL; + (void) source; + + ip_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"); + ip_mcast_dbg("+ MULTICAST list interface %-16s +\n", mcast_link->dev->name); + ip_mcast_dbg("+---------------------------------------------------------------------------------+\n"); + ip_mcast_dbg("+ nr | interface | host group | reference count | filter mode | source +\n"); + ip_mcast_dbg("+---------------------------------------------------------------------------------+\n"); + + pico_tree_foreach(index, mcast_link->MCASTGroups) { + g = index->keyValue; + ip_mcast_dbg("+ %04d | %16s | %08X | %05u | %u | %8s +\n", i, mcast_link->dev->name, g->mcast_addr.addr, g->reference_count, g->filter_mode, ""); + pico_tree_foreach(index2, &g->MCASTSources) { + source = index2->keyValue; + ip_mcast_dbg("+ %4s | %16s | %8s | %5s | %s | %08X +\n", "", "", "", "", "", source->addr); + } + i++; + } + ip_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"); +} + +static int mcast_group_update(struct pico_mcast_group *g, struct pico_tree *MCASTFilter, uint8_t filter_mode) +{ + struct pico_tree_node *index = NULL, *_tmp = NULL; + struct pico_ip4 *source = NULL; + /* cleanup filter */ + pico_tree_foreach_safe(index, &g->MCASTSources, _tmp) { + source = index->keyValue; + pico_tree_delete(&g->MCASTSources, source); + PICO_FREE(source); + } + /* insert new filter */ + if (MCASTFilter) { + pico_tree_foreach(index, MCASTFilter) { + if (index->keyValue) { + source = PICO_ZALLOC(sizeof(struct pico_ip4)); + if (!source) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + source->addr = ((struct pico_ip4 *)index->keyValue)->addr; + pico_tree_insert(&g->MCASTSources, source); + } + } + } + + g->filter_mode = filter_mode; + return 0; +} + +int pico_ipv4_mcast_join(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter) +{ + struct pico_mcast_group *g = NULL, test = { + 0 + }; + struct pico_ipv4_link *link = NULL; + + if (mcast_link) + link = pico_ipv4_link_get(mcast_link); + + if (!link) + link = mcast_default_link; + + test.mcast_addr = *mcast_group; + g = pico_tree_findKey(link->MCASTGroups, &test); + if (g) { + if (reference_count) + g->reference_count++; + + pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_UPDATE); + } else { + g = PICO_ZALLOC(sizeof(struct pico_mcast_group)); + if (!g) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + /* "non-existent" state of filter mode INCLUDE and empty source list */ + g->filter_mode = PICO_IP_MULTICAST_INCLUDE; + g->reference_count = 1; + g->mcast_addr = *mcast_group; + g->MCASTSources.root = &LEAF; + g->MCASTSources.compare = ipv4_mcast_sources_cmp; + pico_tree_insert(link->MCASTGroups, g); + pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_CREATE); + } + + if (mcast_group_update(g, MCASTFilter, filter_mode) < 0) { + dbg("Error in mcast_group update\n"); + return -1; + } + + pico_ipv4_mcast_print_groups(link); + return 0; +} + +int pico_ipv4_mcast_leave(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter) +{ + + struct pico_mcast_group *g = NULL, test = { + 0 + }; + struct pico_ipv4_link *link = NULL; + struct pico_tree_node *index = NULL, *_tmp = NULL; + struct pico_ip4 *source = NULL; + + if (mcast_link) + link = pico_ipv4_link_get(mcast_link); + + if (!link) + link = mcast_default_link; + + if (!link) + return -1; + + test.mcast_addr = *mcast_group; + g = pico_tree_findKey(link->MCASTGroups, &test); + if (!g) { + pico_err = PICO_ERR_EINVAL; + return -1; + } else { + if (reference_count && (--(g->reference_count) < 1)) { + pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_DELETE); + /* cleanup filter */ + pico_tree_foreach_safe(index, &g->MCASTSources, _tmp) { + source = index->keyValue; + pico_tree_delete(&g->MCASTSources, source); + PICO_FREE(source); + } + pico_tree_delete(link->MCASTGroups, g); + PICO_FREE(g); + } else { + pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_UPDATE); + if (mcast_group_update(g, MCASTFilter, filter_mode) < 0) + return -1; + } + } + + pico_ipv4_mcast_print_groups(link); + return 0; +} + +struct pico_ipv4_link *pico_ipv4_get_default_mcastlink(void) +{ + return mcast_default_link; +} + +static int pico_ipv4_mcast_filter(struct pico_frame *f) +{ + struct pico_ipv4_link *link = NULL; + struct pico_tree_node *index = NULL, *index2 = NULL; + struct pico_mcast_group *g = NULL, test = { + 0 + }; + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; + + test.mcast_addr = hdr->dst; + + pico_tree_foreach(index, &Tree_dev_link) { + link = index->keyValue; + g = pico_tree_findKey(link->MCASTGroups, &test); + if (g) { + if (f->dev == link->dev) { + ip_mcast_dbg("MCAST: IP %08X is group member of current link %s\n", hdr->dst.addr, f->dev->name); + /* perform source filtering */ + switch (g->filter_mode) { + case PICO_IP_MULTICAST_INCLUDE: + pico_tree_foreach(index2, &g->MCASTSources) { + if (hdr->src.addr == ((struct pico_ip4 *)index2->keyValue)->addr) { + ip_mcast_dbg("MCAST: IP %08X in included interface source list\n", hdr->src.addr); + return 0; + } + } + ip_mcast_dbg("MCAST: IP %08X NOT in included interface source list\n", hdr->src.addr); + return -1; + + case PICO_IP_MULTICAST_EXCLUDE: + pico_tree_foreach(index2, &g->MCASTSources) { + if (hdr->src.addr == ((struct pico_ip4 *)index2->keyValue)->addr) { + ip_mcast_dbg("MCAST: IP %08X in excluded interface source list\n", hdr->src.addr); + return -1; + } + } + ip_mcast_dbg("MCAST: IP %08X NOT in excluded interface source list\n", hdr->src.addr); + return 0; + + default: + return -1; + } + } else { + ip_mcast_dbg("MCAST: IP %08X is group member of different link %s\n", hdr->dst.addr, link->dev->name); + } + } else { + ip_mcast_dbg("MCAST: IP %08X is not a group member of link %s\n", hdr->dst.addr, f->dev->name); + } + } + return -1; +} + +#else + +int pico_ipv4_mcast_join(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter) +{ + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; +} + +int pico_ipv4_mcast_leave(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter) +{ + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; +} + +struct pico_ipv4_link *pico_ipv4_get_default_mcastlink(void) +{ + pico_err = PICO_ERR_EPROTONOSUPPORT; + return NULL; +} +#endif /* PICO_SUPPORT_MCAST */ + +/* #define DEBUG_ROUTE */ +#ifdef DEBUG_ROUTE +void dbg_route(void) +{ + struct pico_ipv4_route *r; + struct pico_tree_node *index; + int count_hosts = 0; + dbg("==== ROUTING TABLE =====\n"); + pico_tree_foreach(index, &Routes) { + r = index->keyValue; + dbg("Route to %08x/%08x, gw %08x, dev: %s, metric: %d\n", r->dest.addr, r->netmask.addr, r->gateway.addr, r->link->dev->name, r->metric); + if (r->netmask.addr == 0xFFFFFFFF) + count_hosts++; + } + dbg("================ total HOST nodes: %d ======\n\n\n", count_hosts); +} +#else +#define dbg_route() do { } while(0) +#endif + +int pico_ipv4_frame_push(struct pico_frame *f, struct pico_ip4 *dst, uint8_t proto) +{ + + struct pico_ipv4_route *route; + struct pico_ipv4_link *link; + struct pico_ipv4_hdr *hdr; + uint8_t ttl = PICO_IPV4_DEFAULT_TTL; + uint8_t vhl = 0x45; /* version 4, header length 20 */ + static uint16_t ipv4_progressive_id = 0x91c0; +#ifdef PICO_SUPPORT_MCAST + struct pico_tree_node *index; +#endif + + if (!f || !dst) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + + hdr = (struct pico_ipv4_hdr *) f->net_hdr; + if (!hdr) { + dbg("IP header error\n"); + pico_err = PICO_ERR_EINVAL; + goto drop; + } + + if (dst->addr == 0) { + dbg("IP destination addr error\n"); + pico_err = PICO_ERR_EINVAL; + goto drop; + } + + route = route_find(dst); + if (!route) { + /* dbg("Route to %08x not found.\n", long_be(dst->addr)); */ + + + pico_err = PICO_ERR_EHOSTUNREACH; + goto drop; + } else { + link = route->link; +#ifdef PICO_SUPPORT_MCAST + if (pico_ipv4_is_multicast(dst->addr)) { /* if multicast */ + switch (proto) { + case PICO_PROTO_UDP: + if (pico_udp_get_mc_ttl(f->sock, &ttl) < 0) + ttl = PICO_IP_DEFAULT_MULTICAST_TTL; + + break; +#ifdef PICO_SUPPORT_IGMP + case PICO_PROTO_IGMP: + vhl = 0x46; /* header length 24 */ + ttl = 1; + /* router alert (RFC 2113) */ + hdr->options[0] = 0x94; + hdr->options[1] = 0x04; + hdr->options[2] = 0x00; + hdr->options[3] = 0x00; + if (f->dev && link->dev != f->dev) { /* default link is not requested link */ + pico_tree_foreach(index, &Tree_dev_link) { + link = index->keyValue; + if (link->dev == f->dev) + break; + } + } + + break; +#endif + default: + ttl = PICO_IPV4_DEFAULT_TTL; + } + } + +#endif + } + + hdr->vhl = vhl; + hdr->len = short_be((uint16_t)(f->transport_len + f->net_len)); + hdr->id = short_be(ipv4_progressive_id); + + if ( +#ifdef PICO_SUPPORT_IPV4FRAG + (0 == (f->frag & PICO_IPV4_MOREFRAG)) && +#endif + 1 ) + ipv4_progressive_id++; + + if (f->send_ttl > 0) { + ttl = f->send_ttl; + } + + hdr->dst.addr = dst->addr; + hdr->src.addr = link->address.addr; + hdr->ttl = ttl; + hdr->tos = f->send_tos; + hdr->proto = proto; + hdr->frag = short_be(PICO_IPV4_DONTFRAG); + +#ifdef PICO_SUPPORT_IPV4FRAG +# ifdef PICO_SUPPORT_UDP + if (proto == PICO_PROTO_UDP) { + /* first fragment, can not use transport_len to calculate IP length */ + if (f->transport_hdr != f->payload) + hdr->len = short_be((uint16_t)(f->payload_len + sizeof(struct pico_udp_hdr) + f->net_len)); + + /* set fragmentation flags and offset calculated in socket layer */ + hdr->frag = short_be(f->frag); + } + + if (proto == PICO_PROTO_ICMP4) + { + hdr->frag = short_be(f->frag); + } + +# endif +#endif /* PICO_SUPPORT_IPV4FRAG */ + pico_ipv4_checksum(f); + + if (f->sock && f->sock->dev) { + /* if the socket has its device set, use that (currently used for DHCP) */ + f->dev = f->sock->dev; + } else { + f->dev = link->dev; + if (f->sock) + f->sock->dev = f->dev; + } + +#ifdef PICO_SUPPORT_MCAST + if (pico_ipv4_is_multicast(hdr->dst.addr)) { + struct pico_frame *cpy; + /* Sending UDP multicast datagram, am I member? If so, loopback copy */ + if ((proto != PICO_PROTO_IGMP) && (pico_ipv4_mcast_filter(f) == 0)) { + ip_mcast_dbg("MCAST: sender is member of group, loopback copy\n"); + cpy = pico_frame_copy(f); + pico_enqueue(&in, cpy); + } + } + +#endif + +/* #ifdef PICO_SUPPORT_AODV */ +#if 0 + { + union pico_address node_address; + node_address.ip4.addr = hdr->dst.addr; + if(hdr->dst.addr && pico_ipv4_is_unicast(hdr->dst.addr)) + pico_aodv_lookup(&node_address); + } +#endif + + if (pico_ipv4_link_get(&hdr->dst)) { + /* it's our own IP */ + return pico_enqueue(&in, f); + } else{ + /* TODO: Check if there are members subscribed here */ + return pico_enqueue(&out, f); + } + +drop: + pico_frame_discard(f); + return -1; +} + + +static int pico_ipv4_frame_sock_push(struct pico_protocol *self, struct pico_frame *f) +{ + struct pico_ip4 *dst; + struct pico_remote_endpoint *remote_endpoint = (struct pico_remote_endpoint *) f->info; + IGNORE_PARAMETER(self); + + if (!f->sock) { + pico_frame_discard(f); + return -1; + } + + if (remote_endpoint) { + dst = &remote_endpoint->remote_addr.ip4; + } else { + dst = &f->sock->remote_addr.ip4; + } + + return pico_ipv4_frame_push(f, dst, (uint8_t)f->sock->proto->proto_number); +} + + +int MOCKABLE pico_ipv4_route_add(struct pico_ip4 address, struct pico_ip4 netmask, struct pico_ip4 gateway, int metric, struct pico_ipv4_link *link) +{ + struct pico_ipv4_route test, *new; + test.dest.addr = address.addr; + test.netmask.addr = netmask.addr; + test.metric = (uint32_t)metric; + + if (pico_tree_findKey(&Routes, &test)) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + new = PICO_ZALLOC(sizeof(struct pico_ipv4_route)); + if (!new) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + new->dest.addr = address.addr; + new->netmask.addr = netmask.addr; + new->gateway.addr = gateway.addr; + new->metric = (uint32_t)metric; + if (gateway.addr == 0) { + /* No gateway provided, use the link */ + new->link = link; + } else { + struct pico_ipv4_route *r = route_find(&gateway); + if (!r ) { /* Specified Gateway is unreachable */ + pico_err = PICO_ERR_EHOSTUNREACH; + PICO_FREE(new); + return -1; + } + + if (r->gateway.addr) { /* Specified Gateway is not a neighbor */ + pico_err = PICO_ERR_ENETUNREACH; + PICO_FREE(new); + return -1; + } + + new->link = r->link; + } + + if (!new->link) { + pico_err = PICO_ERR_EINVAL; + PICO_FREE(new); + return -1; + } + + pico_tree_insert(&Routes, new); + dbg_route(); + return 0; +} + +int pico_ipv4_route_del(struct pico_ip4 address, struct pico_ip4 netmask, int metric) +{ + struct pico_ipv4_route test, *found; + + test.dest.addr = address.addr; + test.netmask.addr = netmask.addr; + test.metric = (uint32_t)metric; + + found = pico_tree_findKey(&Routes, &test); + if (found) { + + pico_tree_delete(&Routes, found); + PICO_FREE(found); + + dbg_route(); + return 0; + } + + pico_err = PICO_ERR_EINVAL; + return -1; +} + + +int pico_ipv4_link_add(struct pico_device *dev, struct pico_ip4 address, struct pico_ip4 netmask) +{ + struct pico_ipv4_link test, *new; + struct pico_ip4 network, gateway; + char ipstr[30]; + + if (!dev) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + test.address.addr = address.addr; + test.netmask.addr = netmask.addr; + test.dev = dev; + /** XXX: Valid netmask / unicast address test **/ + + if (pico_tree_findKey(&Tree_dev_link, &test)) { + pico_err = PICO_ERR_EADDRINUSE; + return -1; + } + + /** XXX: Check for network already in use (e.g. trying to assign 10.0.0.1/24 where 10.1.0.1/8 is in use) **/ + new = PICO_ZALLOC(sizeof(struct pico_ipv4_link)); + if (!new) { + dbg("IPv4: Out of memory!\n"); + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + new->address.addr = address.addr; + new->netmask.addr = netmask.addr; + new->dev = dev; +#ifdef PICO_SUPPORT_MCAST + new->MCASTGroups = PICO_ZALLOC(sizeof(struct pico_tree)); + if (!new->MCASTGroups) { + PICO_FREE(new); + dbg("IPv4: Out of memory!\n"); + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + new->MCASTGroups->root = &LEAF; + new->MCASTGroups->compare = ipv4_mcast_groups_cmp; +#ifdef PICO_SUPPORT_IGMP + new->mcast_compatibility = PICO_IGMPV3; /* default RFC 3376 $7.2.1 */ + new->mcast_last_query_interval = PICO_IGMP_QUERY_INTERVAL; +#endif +#endif + + pico_tree_insert(&Tree_dev_link, new); +#ifdef PICO_SUPPORT_MCAST + do { + struct pico_ip4 mcast_all_hosts, mcast_addr, mcast_nm, mcast_gw; + if (!mcast_default_link) { + mcast_addr.addr = long_be(0xE0000000); /* 224.0.0.0 */ + mcast_nm.addr = long_be(0xF0000000); /* 15.0.0.0 */ + mcast_gw.addr = long_be(0x00000000); + mcast_default_link = new; + pico_ipv4_route_add(mcast_addr, mcast_nm, mcast_gw, 1, new); + } + + mcast_all_hosts.addr = PICO_MCAST_ALL_HOSTS; + pico_ipv4_mcast_join(&address, &mcast_all_hosts, 1, PICO_IP_MULTICAST_EXCLUDE, NULL); + } while(0); +#endif + + network.addr = address.addr & netmask.addr; + gateway.addr = 0U; + pico_ipv4_route_add(network, netmask, gateway, 1, new); + pico_ipv4_to_string(ipstr, new->address.addr); + dbg("Assigned ipv4 %s to device %s\n", ipstr, new->dev->name); + if (default_bcast_route.link == NULL) + default_bcast_route.link = new; + + return 0; +} + +static int pico_ipv4_cleanup_routes(struct pico_ipv4_link *link) +{ + struct pico_tree_node *index = NULL, *tmp = NULL; + struct pico_ipv4_route *route = NULL; + + pico_tree_foreach_safe(index, &Routes, tmp) { + route = index->keyValue; + if (link == route->link) + pico_ipv4_route_del(route->dest, route->netmask, (int)route->metric); + } + return 0; +} + +void MOCKABLE pico_ipv4_route_set_bcast_link(struct pico_ipv4_link *link) +{ + if (link) + default_bcast_route.link = link; +} + +int pico_ipv4_link_del(struct pico_device *dev, struct pico_ip4 address) +{ + struct pico_ipv4_link test, *found; + + if (!dev) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + test.address.addr = address.addr; + test.dev = dev; + found = pico_tree_findKey(&Tree_dev_link, &test); + if (!found) { + pico_err = PICO_ERR_ENXIO; + return -1; + } + +#ifdef PICO_SUPPORT_MCAST + do { + struct pico_ip4 mcast_all_hosts, mcast_addr, mcast_nm; + struct pico_mcast_group *g = NULL; + struct pico_tree_node *index, *_tmp; + if (found == mcast_default_link) { + mcast_addr.addr = long_be(0xE0000000); /* 224.0.0.0 */ + mcast_nm.addr = long_be(0xF0000000); /* 15.0.0.0 */ + mcast_default_link = NULL; + pico_ipv4_route_del(mcast_addr, mcast_nm, 1); + } + + mcast_all_hosts.addr = PICO_MCAST_ALL_HOSTS; + pico_ipv4_mcast_leave(&address, &mcast_all_hosts, 1, PICO_IP_MULTICAST_EXCLUDE, NULL); + pico_tree_foreach_safe(index, found->MCASTGroups, _tmp) { + g = index->keyValue; + pico_tree_delete(found->MCASTGroups, g); + PICO_FREE(g); + } + } while(0); + PICO_FREE(found->MCASTGroups); +#endif + + pico_ipv4_cleanup_routes(found); + pico_tree_delete(&Tree_dev_link, found); + if (default_bcast_route.link == found) + default_bcast_route.link = NULL; + + PICO_FREE(found); + + return 0; +} + + +struct pico_ipv4_link *pico_ipv4_link_get(struct pico_ip4 *address) +{ + struct pico_ipv4_link test = { + 0 + }, *found = NULL; + test.address.addr = address->addr; + + found = pico_tree_findKey(&Tree_dev_link, &test); + if (!found) + return NULL; + else + return found; +} + +struct pico_ipv4_link *MOCKABLE pico_ipv4_link_by_dev(struct pico_device *dev) +{ + struct pico_tree_node *index = NULL; + struct pico_ipv4_link *link = NULL; + + pico_tree_foreach(index, &Tree_dev_link) { + link = index->keyValue; + if (link->dev == dev) + return link; + } + return NULL; +} + +struct pico_ipv4_link *pico_ipv4_link_by_dev_next(struct pico_device *dev, struct pico_ipv4_link *last) +{ + struct pico_tree_node *index = NULL; + struct pico_ipv4_link *link = NULL; + int valid = 0; + + if (last == NULL) + valid = 1; + + pico_tree_foreach(index, &Tree_dev_link) { + link = index->keyValue; + if (link->dev == dev) { + if (last == link) + valid = 1; + else if (valid > 0) + return link; + } + } + return NULL; +} + +struct pico_device *MOCKABLE pico_ipv4_link_find(struct pico_ip4 *address) +{ + struct pico_ipv4_link test, *found; + if (!address) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + test.dev = NULL; + test.address.addr = address->addr; + found = pico_tree_findKey(&Tree_dev_link, &test); + if (!found) { + pico_err = PICO_ERR_ENXIO; + return NULL; + } + + return found->dev; +} + + +static int pico_ipv4_rebound_large(struct pico_frame *f) +{ +#ifdef PICO_SUPPORT_IPV4FRAG + uint16_t total_payload_written = 0; + uint32_t len = f->transport_len; + struct pico_frame *fr; + struct pico_ip4 dst; + struct pico_ipv4_hdr *hdr; + hdr = (struct pico_ipv4_hdr *) f->net_hdr; + dst.addr = hdr->src.addr; + + while(total_payload_written < len) { + uint32_t space = (uint32_t)len - total_payload_written; + if (space > PICO_IPV4_MAXPAYLOAD) + space = PICO_IPV4_MAXPAYLOAD; + + fr = pico_ipv4_alloc(&pico_proto_ipv4, (uint16_t)space); + if (!fr) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + if (space + total_payload_written < len) + { + fr->frag |= PICO_IPV4_MOREFRAG; + } + else + { + fr->frag &= PICO_IPV4_FRAG_MASK; + } + + fr->frag = (((total_payload_written) >> 3u) & 0xffffu) | fr->frag; + + memcpy(fr->transport_hdr, f->transport_hdr + total_payload_written, fr->transport_len); + if (pico_ipv4_frame_push(fr, &dst, hdr->proto) > 0) { + total_payload_written = (uint16_t)((uint16_t)fr->transport_len + total_payload_written); + } else { + pico_frame_discard(fr); + break; + } + } /* while() */ + return (int)total_payload_written; +#else + (void)f; + return -1; +#endif +} + +int pico_ipv4_rebound(struct pico_frame *f) +{ + struct pico_ip4 dst; + struct pico_ipv4_hdr *hdr; + if (!f) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + hdr = (struct pico_ipv4_hdr *) f->net_hdr; + if (!hdr) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + dst.addr = hdr->src.addr; + if (f->transport_len > PICO_IPV4_MAXPAYLOAD) { + return pico_ipv4_rebound_large(f); + } + + return pico_ipv4_frame_push(f, &dst, hdr->proto); +} + +static int pico_ipv4_pre_forward_checks(struct pico_frame *f) +{ + static uint16_t last_id = 0; + static uint16_t last_proto = 0; + static struct pico_ip4 last_src = { + 0 + }; + static struct pico_ip4 last_dst = { + 0 + }; + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr; + + /* Decrease TTL, check if expired */ + hdr->ttl = (uint8_t)(hdr->ttl - 1); + if (hdr->ttl < 1) { + pico_notify_ttl_expired(f); + dbg(" ------------------- TTL EXPIRED\n"); + return -1; + } + + /* HACK: increase crc to compensate decreased TTL */ + hdr->crc++; + + /* If source is local, discard anyway (packets bouncing back and forth) */ + if (pico_ipv4_link_get(&hdr->src)) + return -1; + + /* If this was the last forwarded packet, silently discard to prevent duplications */ + if ((last_src.addr == hdr->src.addr) && (last_id == hdr->id) + && (last_dst.addr == hdr->dst.addr) && (last_proto == hdr->proto)) { + return -1; + } else { + last_src.addr = hdr->src.addr; + last_dst.addr = hdr->dst.addr; + last_id = hdr->id; + last_proto = hdr->proto; + } + + return 0; +} + +static int pico_ipv4_forward_check_dev(struct pico_frame *f) +{ + if (f->dev->eth != NULL) + f->len -= PICO_SIZE_ETHHDR; + + if (f->len > f->dev->mtu) { + pico_notify_pkt_too_big(f); + return -1; + } + + return 0; +} + +static int pico_ipv4_forward(struct pico_frame *f) +{ + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr; + struct pico_ipv4_route *rt; + if (!hdr) { + return -1; + } + + rt = route_find(&hdr->dst); + if (!rt) { + pico_notify_dest_unreachable(f); + return -1; + } + + f->dev = rt->link->dev; + + if (pico_ipv4_pre_forward_checks(f) < 0) + return -1; + + pico_ipv4_nat_outbound(f, &rt->link->address); + + f->start = f->net_hdr; + + if (pico_ipv4_forward_check_dev(f) < 0) + return -1; + + pico_sendto_dev(f); + return 0; + +} + +int pico_ipv4_is_broadcast(uint32_t addr) +{ + struct pico_ipv4_link *link; + struct pico_tree_node *index; + if (addr == PICO_IP4_BCAST) + return 1; + + pico_tree_foreach(index, &Tree_dev_link) { + link = index->keyValue; + if ((link->address.addr | (~link->netmask.addr)) == addr) + return 1; + } + return 0; +} + +void pico_ipv4_unreachable(struct pico_frame *f, int err) +{ + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; +#if defined PICO_SUPPORT_TCP || defined PICO_SUPPORT_UDP + f->transport_hdr = ((uint8_t *)f->net_hdr) + PICO_SIZE_IP4HDR; + pico_transport_error(f, hdr->proto, err); +#endif +} + +int pico_ipv4_cleanup_links(struct pico_device *dev) +{ + struct pico_tree_node *index = NULL, *_tmp = NULL; + struct pico_ipv4_link *link = NULL; + + pico_tree_foreach_safe(index, &Tree_dev_link, _tmp) { + link = index->keyValue; + if (dev == link->dev) + pico_ipv4_link_del(dev, link->address); + } + return 0; +} + + +#endif diff --git a/ext/picotcp/modules/pico_ipv4.h b/ext/picotcp/modules/pico_ipv4.h new file mode 100644 index 0000000..2429815 --- /dev/null +++ b/ext/picotcp/modules/pico_ipv4.h @@ -0,0 +1,124 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + *********************************************************************/ +#ifndef INCLUDE_PICO_IPV4 +#define INCLUDE_PICO_IPV4 +#include "pico_addressing.h" +#include "pico_protocol.h" +#include "pico_tree.h" +#include "pico_device.h" + +#define PICO_IPV4_INADDR_ANY 0x00000000U + +#define PICO_IPV4_MTU (1500u) +#define PICO_SIZE_IP4HDR (uint32_t)((sizeof(struct pico_ipv4_hdr))) +#define PICO_IPV4_MAXPAYLOAD (PICO_IPV4_MTU - PICO_SIZE_IP4HDR) +#define PICO_IPV4_DONTFRAG 0x4000U +#define PICO_IPV4_MOREFRAG 0x2000U +#define PICO_IPV4_EVIL 0x8000U +#define PICO_IPV4_FRAG_MASK 0x1FFFU +#define PICO_IPV4_DEFAULT_TTL 64 +#ifndef MBED + #define PICO_IPV4_FRAG_MAX_SIZE (uint32_t)(63 * 1024) +#else + #define PICO_IPV4_FRAG_MAX_SIZE PICO_DEFAULT_SOCKETQ +#endif + +extern struct pico_protocol pico_proto_ipv4; + +PACKED_STRUCT_DEF pico_ipv4_hdr { + uint8_t vhl; + uint8_t tos; + uint16_t len; + uint16_t id; + uint16_t frag; + uint8_t ttl; + uint8_t proto; + uint16_t crc; + struct pico_ip4 src; + struct pico_ip4 dst; + uint8_t options[]; +}; + +PACKED_STRUCT_DEF pico_ipv4_pseudo_hdr +{ + struct pico_ip4 src; + struct pico_ip4 dst; + uint8_t zeros; + uint8_t proto; + uint16_t len; +}; + +/* Interface: link to device */ +struct pico_mcast_list; + +struct pico_ipv4_link +{ + struct pico_device *dev; + struct pico_ip4 address; + struct pico_ip4 netmask; +#ifdef PICO_SUPPORT_MCAST + struct pico_tree *MCASTGroups; + uint8_t mcast_compatibility; + uint8_t mcast_last_query_interval; +#endif +}; + +#ifdef PICO_SUPPORT_MCAST +struct pico_mcast_group { + uint8_t filter_mode; + uint16_t reference_count; + struct pico_ip4 mcast_addr; + struct pico_tree MCASTSources; +}; +#endif + +struct pico_ipv4_route +{ + struct pico_ip4 dest; + struct pico_ip4 netmask; + struct pico_ip4 gateway; + struct pico_ipv4_link *link; + uint32_t metric; +}; + +extern struct pico_tree Routes; + + +int pico_ipv4_compare(struct pico_ip4 *a, struct pico_ip4 *b); +int pico_ipv4_to_string(char *ipbuf, const uint32_t ip); +int pico_string_to_ipv4(const char *ipstr, uint32_t *ip); +int pico_ipv4_valid_netmask(uint32_t mask); +int pico_ipv4_is_unicast(uint32_t address); +int pico_ipv4_is_multicast(uint32_t address); +int pico_ipv4_is_broadcast(uint32_t addr); +int pico_ipv4_is_loopback(uint32_t addr); +int pico_ipv4_is_valid_src(uint32_t addr, struct pico_device *dev); + +int pico_ipv4_link_add(struct pico_device *dev, struct pico_ip4 address, struct pico_ip4 netmask); +int pico_ipv4_link_del(struct pico_device *dev, struct pico_ip4 address); +int pico_ipv4_rebound(struct pico_frame *f); + +int pico_ipv4_frame_push(struct pico_frame *f, struct pico_ip4 *dst, uint8_t proto); +struct pico_ipv4_link *pico_ipv4_link_get(struct pico_ip4 *address); +struct pico_ipv4_link *pico_ipv4_link_by_dev(struct pico_device *dev); +struct pico_ipv4_link *pico_ipv4_link_by_dev_next(struct pico_device *dev, struct pico_ipv4_link *last); +struct pico_device *pico_ipv4_link_find(struct pico_ip4 *address); +struct pico_ip4 *pico_ipv4_source_find(const struct pico_ip4 *dst); +struct pico_device *pico_ipv4_source_dev_find(const struct pico_ip4 *dst); +int pico_ipv4_route_add(struct pico_ip4 address, struct pico_ip4 netmask, struct pico_ip4 gateway, int metric, struct pico_ipv4_link *link); +int pico_ipv4_route_del(struct pico_ip4 address, struct pico_ip4 netmask, int metric); +struct pico_ip4 pico_ipv4_route_get_gateway(struct pico_ip4 *addr); +void pico_ipv4_route_set_bcast_link(struct pico_ipv4_link *link); +void pico_ipv4_unreachable(struct pico_frame *f, int err); + +int pico_ipv4_mcast_join(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter); +int pico_ipv4_mcast_leave(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter); +struct pico_ipv4_link *pico_ipv4_get_default_mcastlink(void); +int pico_ipv4_cleanup_links(struct pico_device *dev); + +#endif /* _INCLUDE_PICO_IPV4 */ diff --git a/ext/picotcp/modules/pico_ipv6.c b/ext/picotcp/modules/pico_ipv6.c new file mode 100644 index 0000000..3d532cf --- /dev/null +++ b/ext/picotcp/modules/pico_ipv6.c @@ -0,0 +1,1921 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Daniele Lacamera, Kristof Roelants + *********************************************************************/ + + +#include "pico_ipv6.h" +#include "pico_icmp6.h" +#include "pico_config.h" +#include "pico_stack.h" +#include "pico_eth.h" +#include "pico_udp.h" +#include "pico_tcp.h" +#include "pico_socket.h" +#include "pico_device.h" +#include "pico_tree.h" +#include "pico_fragments.h" +#include "pico_mld.h" + +#ifdef PICO_SUPPORT_IPV6 + + +#define PICO_IPV6_EXTHDR_OPT_PAD1 0 +#define PICO_IPV6_EXTHDR_OPT_PADN 1 +#define PICO_IPV6_EXTHDR_OPT_SRCADDR 201 + +#define PICO_IPV6_EXTHDR_OPT_ACTION_MASK 0xC0 /* highest-order two bits */ +#define PICO_IPV6_EXTHDR_OPT_ACTION_SKIP 0x00 /* skip and continue processing */ +#define PICO_IPV6_EXTHDR_OPT_ACTION_DISCARD 0x40 /* discard packet */ +#define PICO_IPV6_EXTHDR_OPT_ACTION_DISCARD_SI 0x80 /* discard and send ICMP parameter problem */ +#define PICO_IPV6_EXTHDR_OPT_ACTION_DISCARD_SINM 0xC0 /* discard and send ICMP parameter problem if not multicast */ + +#define PICO_IPV6_MAX_RTR_SOLICITATION_DELAY 1000 +#define PICO_IPV6_DEFAULT_DAD_RETRANS 1 + +#define ipv6_dbg(...) do { }while(0); +#define ipv6_mcast_dbg do{ }while(0); +static struct pico_ipv6_link *mcast_default_link_ipv6 = NULL; + +/* queues */ +static struct pico_queue ipv6_in; +static struct pico_queue ipv6_out; + +const uint8_t PICO_IP6_ANY[PICO_SIZE_IP6] = { + 0 +}; +#ifdef PICO_SUPPORT_MCAST +static int pico_ipv6_mcast_filter(struct pico_frame *f); +#endif + + +int pico_ipv6_compare(struct pico_ip6 *a, struct pico_ip6 *b) +{ + uint32_t i; + for (i = 0; i < sizeof(struct pico_ip6); i++) { + if (a->addr[i] < b->addr[i]) + return -1; + + if (a->addr[i] > b->addr[i]) + return 1; + } + return 0; +} + +static int ipv6_link_compare(void *ka, void *kb) +{ + struct pico_ipv6_link *a = ka, *b = kb; + struct pico_ip6 *a_addr, *b_addr; + int ret; + a_addr = &a->address; + b_addr = &b->address; + + ret = pico_ipv6_compare(a_addr, b_addr); + if (ret) + return ret; + + /* zero can be assigned multiple times (e.g. for DHCP) */ + if (a->dev != NULL && b->dev != NULL && !memcmp(a->address.addr, PICO_IP6_ANY, PICO_SIZE_IP6) && !memcmp(b->address.addr, PICO_IP6_ANY, PICO_SIZE_IP6)) { + /* XXX change PICO_IP6_ANY */ + if (a->dev < b->dev) + return -1; + + if (a->dev > b->dev) + return 1; + } + + return 0; +} + +static inline int ipv6_compare_metric(struct pico_ipv6_route *a, struct pico_ipv6_route *b) +{ + if (a->metric < b->metric) + return -1; + + if (a->metric > b->metric) + return 1; + + return 0; +} + +static int ipv6_route_compare(void *ka, void *kb) +{ + struct pico_ipv6_route *a = ka, *b = kb; + int ret; + + /* Routes are sorted by (host side) netmask len, then by addr, then by metric. */ + ret = pico_ipv6_compare(&a->netmask, &b->netmask); + if (ret) + return ret; + + ret = pico_ipv6_compare(&a->dest, &b->dest); + if (ret) + return ret; + + return ipv6_compare_metric(a, b); + +} + +PICO_TREE_DECLARE(Tree_dev_ip6_link, ipv6_link_compare); +PICO_TREE_DECLARE(IPV6Routes, ipv6_route_compare); +PICO_TREE_DECLARE(IPV6Links, ipv6_link_compare); + +static char pico_ipv6_dec_to_char(uint8_t u) +{ + if (u < 10) + return (char)('0' + u); + else if (u < 16) + return (char)('a' + (u - 10)); + else + return '0'; +} + +static int pico_ipv6_hex_to_dec(char c) +{ + if (c >= '0' && c <= '9') + return c - '0'; + + if (c >= 'a' && c <= 'f') + return 10 + (c - 'a'); + + if (c >= 'A' && c <= 'F') + return 10 + (c - 'A'); + + return 0; +} + +int pico_ipv6_to_string(char *ipbuf, const uint8_t ip[PICO_SIZE_IP6]) +{ + uint8_t dec = 0, i = 0; + + if (!ipbuf || !ip) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* every nibble is one char */ + for (i = 0; i < ((uint8_t)PICO_SIZE_IP6) * 2u; ++i) { + if (i % 4 == 0 && i != 0) + *ipbuf++ = ':'; + + if (i % 2 == 0) { /* upper nibble */ + dec = ip[i / 2] >> 4; + } else { /* lower nibble */ + dec = ip[i / 2] & 0x0F; + } + + *ipbuf++ = pico_ipv6_dec_to_char(dec); + } + *ipbuf = '\0'; + + return 0; +} + +int pico_string_to_ipv6(const char *ipstr, uint8_t *ip) +{ + uint8_t buf[PICO_SIZE_IP6] = { + 0 + }; + uint8_t doublecolon = 0, byte = 0; + char p = 0; + int i = 0, diff = 0, nibble = 0, hex = 0, colons = 0; + int zeros = 0, shift = 0; + + pico_err = PICO_ERR_EINVAL; + if (!ipstr || !ip) + return -1; + + memset(ip, 0, PICO_SIZE_IP6); + + while((p = *ipstr++) != 0) + { + if (pico_is_hex(p) || (p == ':') || *ipstr == '\0') { /* valid signs */ + if (pico_is_hex(p)) { + buf[byte] = (uint8_t)((buf[byte] << 4) + pico_ipv6_hex_to_dec(p)); + if (++nibble % 2 == 0) + ++byte; + } + + if (p == ':' || *ipstr == '\0') { /* account for leftout leading zeros */ + ++hex; + if (p == ':') + ++colons; + + diff = (hex * 4) - nibble; + nibble += diff; + switch (diff) { + case 0: + /* 16-bit hex block ok f.e. 1db8 */ + break; + case 1: + /* one zero f.e. db8: byte = 1, buf[byte-1] = 0xdb, buf[byte] = 0x08 */ + buf[byte] |= (uint8_t)(buf[byte - 1] << 4); + buf[byte - 1] >>= 4; + byte++; + break; + case 2: + /* two zeros f.e. b8: byte = 1, buf[byte] = 0x00, buf[byte-1] = 0xb8 */ + buf[byte] = buf[byte - 1]; + buf[byte - 1] = 0x00; + byte++; + break; + case 3: + /* three zeros f.e. 8: byte = 0, buf[byte] = 0x08, buf[byte+1] = 0x00 */ + buf[byte + 1] = buf[byte]; + buf[byte] = 0x00; + byte = (uint8_t)(byte + 2); + break; + case 4: + /* case of :: */ + if (doublecolon && colons != 2) /* catch case x::x::x but not ::x */ + return -1; + else + doublecolon = byte; + + break; + default: + /* case of missing colons f.e. 20011db8 instead of 2001:1db8 */ + return -1; + } + } + } else { + return -1; + } + } + if (colons < 2) /* valid IPv6 has atleast two colons */ + return -1; + + /* account for leftout :: zeros */ + zeros = PICO_SIZE_IP6 - byte; + if (zeros) { + shift = PICO_SIZE_IP6 - zeros - doublecolon; + for (i = shift; i >= 0; --i) { + /* (i-1) as arrays are indexed from 0 onwards */ + if ((doublecolon + (i - 1)) >= 0) + buf[doublecolon + zeros + (i - 1)] = buf[doublecolon + (i - 1)]; + } + memset(&buf[doublecolon], 0, (size_t)zeros); + } + + memcpy(ip, buf, 16); + pico_err = PICO_ERR_NOERR; + return 0; +} + +int pico_ipv6_is_linklocal(const uint8_t addr[PICO_SIZE_IP6]) +{ + /* prefix: fe80::/10 */ + if ((addr[0] == 0xfe) && ((addr[1] >> 6) == 0x02)) + return 1; + + return 0; +} + +int pico_ipv6_is_sitelocal(const uint8_t addr[PICO_SIZE_IP6]) +{ + /* prefix: fec0::/10 */ + if ((addr[0] == 0xfe) && ((addr[1] >> 6) == 0x03)) + return 1; + + return 0; +} + +int pico_ipv6_is_uniquelocal(const uint8_t addr[PICO_SIZE_IP6]) +{ + /* prefix: fc00::/7 */ + if (((addr[0] >> 1) == 0x7e)) + return 1; + + return 0; +} + +int pico_ipv6_is_global(const uint8_t addr[PICO_SIZE_IP6]) +{ + /* prefix: 2000::/3 */ + if (((addr[0] >> 5) == 0x01)) + return 1; + + return 0; +} + +int pico_ipv6_is_localhost(const uint8_t addr[PICO_SIZE_IP6]) +{ + const uint8_t localhost[PICO_SIZE_IP6] = { + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 + }; + if (memcmp(addr, localhost, PICO_SIZE_IP6) == 0) + return 1; + + return 0; + +} + +int pico_ipv6_is_unicast(struct pico_ip6 *a) +{ + if (pico_ipv6_is_global(a->addr)) + return 1; + else if (pico_ipv6_is_uniquelocal(a->addr)) + return 1; + else if (pico_ipv6_is_sitelocal(a->addr)) + return 1; + else if (pico_ipv6_is_linklocal(a->addr)) + return 1; + else if (pico_ipv6_is_localhost(a->addr)) + return 1; + else if(pico_ipv6_link_get(a)) + return 1; + else + return 0; + +} + +int pico_ipv6_is_multicast(const uint8_t addr[PICO_SIZE_IP6]) +{ + /* prefix: ff00::/8 */ + if ((addr[0] == 0xff)) + return 1; + + return 0; +} + +int pico_ipv6_is_allhosts_multicast(const uint8_t addr[PICO_SIZE_IP6]) +{ + struct pico_ip6 allhosts = {{ 0xff, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01 }}; + return !memcmp(allhosts.addr, addr, PICO_SIZE_IP6); +} + +int pico_ipv6_is_solicited(const uint8_t addr[PICO_SIZE_IP6]) +{ + struct pico_ip6 solicited_node = {{ 0xff, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x00, 0x00, 0x00 }}; + return !memcmp(solicited_node.addr, addr, 13); +} + +int pico_ipv6_is_solnode_multicast(const uint8_t addr[PICO_SIZE_IP6], struct pico_device *dev) +{ + struct pico_ipv6_link *link; + if (pico_ipv6_is_multicast(addr) == 0) + return 0; + + link = pico_ipv6_link_by_dev(dev); + while(link) { + if (pico_ipv6_is_linklocal(link->address.addr)) { + int i, match = 0; + for(i = 13; i < 16; i++) { + if (addr[i] == link->address.addr[i]) + ++match; + } + /* Solicitation: last 3 bytes match a local address. */ + if (match == 3) + return 1; + } + + link = pico_ipv6_link_by_dev_next(dev, link); + } + return 0; +} + +int pico_ipv6_is_unspecified(const uint8_t addr[PICO_SIZE_IP6]) +{ + return !memcmp(PICO_IP6_ANY, addr, PICO_SIZE_IP6); +} + +static struct pico_ipv6_route *pico_ipv6_route_find(const struct pico_ip6 *addr) +{ + struct pico_ipv6_route *r = NULL; + struct pico_tree_node *index = NULL; + int i = 0; + if (!pico_ipv6_is_localhost(addr->addr) && (pico_ipv6_is_linklocal(addr->addr) || pico_ipv6_is_sitelocal(addr->addr))) { + return NULL; + } + pico_tree_foreach_reverse(index, &IPV6Routes) + { + r = index->keyValue; + for (i = 0; i < PICO_SIZE_IP6; ++i) { + if ((addr->addr[i] & (r->netmask.addr[i])) != ((r->dest.addr[i]) & (r->netmask.addr[i]))) { + break; + } + + if (i + 1 == PICO_SIZE_IP6) { + return r; + } + } + } + return NULL; +} + +struct pico_ip6 *pico_ipv6_source_find(const struct pico_ip6 *dst) +{ + struct pico_ip6 *myself = NULL; + struct pico_ipv6_route *rt; + + if(!dst) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + rt = pico_ipv6_route_find(dst); + if (rt) { + myself = &rt->link->address; + } else + pico_err = PICO_ERR_EHOSTUNREACH; + + return myself; +} + +struct pico_device *pico_ipv6_source_dev_find(const struct pico_ip6 *dst) +{ + struct pico_device *dev = NULL; + struct pico_ipv6_route *rt; + + if(!dst) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + rt = pico_ipv6_route_find(dst); + if (rt && rt->link) { + dev = rt->link->dev; + } else + pico_err = PICO_ERR_EHOSTUNREACH; + + return dev; +} + +static int pico_ipv6_forward_check_dev(struct pico_frame *f) +{ + if(f->dev->eth != NULL) + f->len -= PICO_SIZE_ETHHDR; + + if(f->len > f->dev->mtu) { + pico_notify_pkt_too_big(f); + return -1; + } + + return 0; +} + +static int pico_ipv6_pre_forward_checks(struct pico_frame *f) +{ + struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr; + + /* Decrease HOP count, check if expired */ + hdr->hop = (uint8_t)(hdr->hop - 1); + if (hdr->hop < 1) { + pico_notify_ttl_expired(f); + dbg(" ------------------- HOP COUNT EXPIRED\n"); + return -1; + } + /* If source is local, discard anyway (packets bouncing back and forth) */ + if (pico_ipv6_link_get(&hdr->src)) + return -1; + + if (pico_ipv6_forward_check_dev(f) < 0) + return -1; + + return 0; +} + +static int pico_ipv6_forward(struct pico_frame *f) +{ + struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr; + struct pico_ipv6_route *rt; + if (!hdr) { + pico_frame_discard(f); + return -1; + } + + rt = pico_ipv6_route_find(&hdr->dst); + if (!rt) { + pico_notify_dest_unreachable(f); + pico_frame_discard(f); + return -1; + } + + f->dev = rt->link->dev; + + if (pico_ipv6_pre_forward_checks(f) < 0) + { + pico_frame_discard(f); + return -1; + } + + f->start = f->net_hdr; + + return pico_sendto_dev(f); +} + + +static int pico_ipv6_process_hopbyhop(struct pico_ipv6_exthdr *hbh, struct pico_frame *f) +{ + uint8_t *option = NULL; + uint8_t len = 0, optlen = 0; + uint32_t ptr = sizeof(struct pico_ipv6_hdr); + uint8_t *extensions_start = (uint8_t *)hbh; + uint8_t must_align = 1; + IGNORE_PARAMETER(f); + + option = ((uint8_t *)&hbh->ext.hopbyhop) + sizeof(struct hopbyhop_s); + len = (uint8_t)HBH_LEN(hbh); + ipv6_dbg("IPv6: hop by hop extension header length %u\n", len + 2); + while (len) { + switch (*option) + { + case PICO_IPV6_EXTHDR_OPT_PAD1: + ++option; + --len; + break; + + case PICO_IPV6_EXTHDR_OPT_PADN: + optlen = (uint8_t)((*(option + 1)) + 2); /* plus type and len byte */ + option += optlen; + len = (uint8_t)(len - optlen); + break; + case PICO_IPV6_EXTHDR_OPT_ROUTER_ALERT: + optlen = (uint8_t)((*(option + 1)) + 2); /* plus type and len byte */ + // MLD package + if(*(option+1) == 2) + must_align = 0; + option += optlen; + len = (uint8_t)(len - optlen); + break; + default: + /* unknown option */ + optlen = (uint8_t)(*(option + 1) + 2); /* plus type and len byte */ + switch ((*option) & PICO_IPV6_EXTHDR_OPT_ACTION_MASK) { + case PICO_IPV6_EXTHDR_OPT_ACTION_SKIP: + break; + case PICO_IPV6_EXTHDR_OPT_ACTION_DISCARD: + return -1; + case PICO_IPV6_EXTHDR_OPT_ACTION_DISCARD_SI: + pico_icmp6_parameter_problem(f, PICO_ICMP6_PARAMPROB_IPV6OPT, ptr + (uint32_t)(option - extensions_start)); + return -1; + case PICO_IPV6_EXTHDR_OPT_ACTION_DISCARD_SINM: + if (!pico_ipv6_is_multicast(((struct pico_ipv6_hdr *)(f->net_hdr))->dst.addr)) + pico_icmp6_parameter_problem(f, PICO_ICMP6_PARAMPROB_IPV6OPT, ptr + (uint32_t)(option - extensions_start)); + + return -1; + } + ipv6_dbg("IPv6: option with type %u and length %u\n", *option, optlen); + option += optlen; + len = (uint8_t)(len - optlen); + } + } + return must_align; +} + + +static int pico_ipv6_process_routing(struct pico_ipv6_exthdr *routing, struct pico_frame *f, uint32_t ptr) +{ + IGNORE_PARAMETER(f); + + if (routing->ext.routing.segleft == 0) + return 0; + + ipv6_dbg("IPv6: routing extension header with len %u\n", routing->ext.routing.len + 2); + switch (routing->ext.routing.routtype) { + case 0x00: + /* deprecated */ + pico_icmp6_parameter_problem(f, PICO_ICMP6_PARAMPROB_HDRFIELD, ptr + 2); + return -1; + case 0x02: + /* routing type for MIPv6: not supported yet */ + break; + default: + pico_icmp6_parameter_problem(f, PICO_ICMP6_PARAMPROB_HDRFIELD, ptr + 2); + return -1; + } + return 0; +} + +#define IP6FRAG_MORE(x) ((x & 0x0001)) + +static int pico_ipv6_process_destopt(struct pico_ipv6_exthdr *destopt, struct pico_frame *f, uint32_t opt_ptr) +{ + uint8_t *option = NULL; + uint8_t len = 0, optlen = 0; + opt_ptr += (uint32_t)(2u); /* Skip Dest_opts header */ + IGNORE_PARAMETER(f); + option = ((uint8_t *)&destopt->ext.destopt) + sizeof(struct destopt_s); + len = (uint8_t)(((destopt->ext.destopt.len + 1) << 3) - 2); /* len in bytes, minus nxthdr and len byte */ + ipv6_dbg("IPv6: destination option extension header length %u\n", len + 2); + while (len) { + optlen = (uint8_t)(*(option + 1) + 2); + switch (*option) + { + case PICO_IPV6_EXTHDR_OPT_PAD1: + break; + + case PICO_IPV6_EXTHDR_OPT_PADN: + break; + + case PICO_IPV6_EXTHDR_OPT_SRCADDR: + ipv6_dbg("IPv6: home address option with length %u\n", optlen); + break; + + default: + ipv6_dbg("IPv6: option with type %u and length %u\n", *option, optlen); + switch (*option & PICO_IPV6_EXTHDR_OPT_ACTION_MASK) { + case PICO_IPV6_EXTHDR_OPT_ACTION_SKIP: + break; + case PICO_IPV6_EXTHDR_OPT_ACTION_DISCARD: + return -1; + case PICO_IPV6_EXTHDR_OPT_ACTION_DISCARD_SI: + pico_icmp6_parameter_problem(f, PICO_ICMP6_PARAMPROB_IPV6OPT, opt_ptr); + return -1; + case PICO_IPV6_EXTHDR_OPT_ACTION_DISCARD_SINM: + if (!pico_ipv6_is_multicast(((struct pico_ipv6_hdr *)(f->net_hdr))->dst.addr)){ + pico_icmp6_parameter_problem(f, PICO_ICMP6_PARAMPROB_IPV6OPT, opt_ptr); + } + return -1; + } + break; + } + opt_ptr += optlen; + option += optlen; + len = (uint8_t)(len - optlen); + } + return 0; +} + +#define IPV6_OPTLEN(x) ((uint16_t)(((x + 1) << 3))) + +static int pico_ipv6_check_headers_sequence(struct pico_frame *f) +{ + struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr; + int ptr = sizeof(struct pico_ipv6_hdr); + int cur_nexthdr = 6; /* Starts with nexthdr field in ipv6 pkt */ + uint8_t nxthdr = hdr->nxthdr; + for (;; ) { + uint8_t optlen = *(f->net_hdr + ptr + 1); + switch (nxthdr) { + case PICO_IPV6_EXTHDR_DESTOPT: + case PICO_IPV6_EXTHDR_ROUTING: + case PICO_IPV6_EXTHDR_HOPBYHOP: + case PICO_IPV6_EXTHDR_ESP: + case PICO_IPV6_EXTHDR_AUTH: + optlen = (uint8_t)IPV6_OPTLEN(optlen); + break; + case PICO_IPV6_EXTHDR_FRAG: + optlen = 8; + break; + case PICO_IPV6_EXTHDR_NONE: + return 0; + + case PICO_PROTO_TCP: + case PICO_PROTO_UDP: + case PICO_PROTO_ICMP6: + return 0; + default: + /* Invalid next header */ + pico_icmp6_parameter_problem(f, PICO_ICMP6_PARAMPROB_NXTHDR, (uint32_t)cur_nexthdr); + return -1; + } + cur_nexthdr = ptr; + nxthdr = *(f->net_hdr + ptr); + ptr += optlen; + } +} + +static int pico_ipv6_check_aligned(struct pico_frame *f) +{ + struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr; + if ((short_be(hdr->len) % 8) != 0) { + pico_icmp6_parameter_problem(f, PICO_ICMP6_PARAMPROB_HDRFIELD, 4); + return -1; + } + + return 0; +} + +static int pico_ipv6_extension_headers(struct pico_frame *f) +{ + struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr; + uint8_t nxthdr = hdr->nxthdr; + struct pico_ipv6_exthdr *exthdr = NULL, *frag_hdr = NULL; + uint32_t ptr = sizeof(struct pico_ipv6_hdr); + uint16_t cur_optlen; + uint32_t cur_nexthdr = 6; + int must_align = 0; + + f->net_len = sizeof(struct pico_ipv6_hdr); + + if (pico_ipv6_check_headers_sequence(f) < 0) + return -1; + for (;; ) { + exthdr = (struct pico_ipv6_exthdr *)(f->net_hdr + f->net_len); + cur_optlen = 0; + + switch (nxthdr) { + case PICO_IPV6_EXTHDR_HOPBYHOP: + if (cur_nexthdr != 6) { + /* The Hop-by-Hop Options header, + * when present, must immediately follow the IPv6 header. + */ + pico_icmp6_parameter_problem(f, PICO_ICMP6_PARAMPROB_NXTHDR, cur_nexthdr); + return -1; + } + + cur_optlen = IPV6_OPTLEN(exthdr->ext.hopbyhop.len); + f->net_len = (uint16_t) (f->net_len + cur_optlen); + must_align = pico_ipv6_process_hopbyhop(exthdr, f); + if(must_align < 0) + return -1; + + break; + case PICO_IPV6_EXTHDR_ROUTING: + cur_optlen = IPV6_OPTLEN(exthdr->ext.routing.len); + f->net_len = (uint16_t) (f->net_len + cur_optlen); + if (pico_ipv6_process_routing(exthdr, f, ptr) < 0) + return -1; + + break; + case PICO_IPV6_EXTHDR_FRAG: + cur_optlen = 8u; + f->net_len = (uint16_t) (f->net_len + cur_optlen); + frag_hdr = exthdr; + f->frag = (uint16_t)((frag_hdr->ext.frag.om[0] << 8) + frag_hdr->ext.frag.om[1]); + /* If M-Flag is set, and packet is not 8B aligned, discard and alert */ + if (IP6FRAG_MORE(f->frag) && ((short_be(hdr->len) % 8) != 0)) { + pico_icmp6_parameter_problem(f, PICO_ICMP6_PARAMPROB_HDRFIELD, 4); + return -1; + } + + break; + case PICO_IPV6_EXTHDR_DESTOPT: + cur_optlen = IPV6_OPTLEN(exthdr->ext.destopt.len); + f->net_len = (uint16_t) (f->net_len + cur_optlen); + must_align = 1; + if (pico_ipv6_process_destopt(exthdr, f, ptr) < 0) + return -1; + + break; + case PICO_IPV6_EXTHDR_ESP: + /* not supported, ignored. */ + return 0; + case PICO_IPV6_EXTHDR_AUTH: + /* not supported, ignored */ + return 0; + case PICO_IPV6_EXTHDR_NONE: + /* no next header */ + if (must_align && (pico_ipv6_check_aligned(f) < 0)) + return -1; + + return 0; + + case PICO_PROTO_TCP: + case PICO_PROTO_UDP: + case PICO_PROTO_ICMP6: + if (must_align && (pico_ipv6_check_aligned(f) < 0)) + return -1; + + f->transport_hdr = f->net_hdr + f->net_len; + f->transport_len = (uint16_t)(short_be(hdr->len) - (f->net_len - sizeof(struct pico_ipv6_hdr))); + if (frag_hdr) { +#ifdef PICO_SUPPORT_IPV6FRAG + pico_ipv6_process_frag(frag_hdr, f, nxthdr); +#endif + return -1; + } else { + return nxthdr; + } + + break; + default: + /* Invalid next header */ + pico_icmp6_parameter_problem(f, PICO_ICMP6_PARAMPROB_NXTHDR, cur_nexthdr); + return -1; + } + nxthdr = exthdr->nxthdr; + cur_nexthdr = ptr; + ptr += cur_optlen; + } +} +static int pico_ipv6_process_mcast_in(struct pico_frame *f) +{ + struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *) f->net_hdr; + struct pico_ipv6_exthdr *hbh = NULL; + if (pico_ipv6_is_multicast(hdr->dst.addr)) { +#ifdef PICO_SUPPORT_MCAST + /* Receiving UDP multicast datagram TODO set f->flags? */ + if(hdr->nxthdr == 0) { + hbh = (struct pico_ipv6_exthdr *) (f->transport_hdr); + } + if (hdr->nxthdr == PICO_PROTO_ICMP6 || (hbh != NULL && hbh->nxthdr == PICO_PROTO_ICMP6)) { + pico_transport_receive(f, PICO_PROTO_ICMP6); + return 1; + } else if ((pico_ipv6_mcast_filter(f) == 0) && (hdr->nxthdr == PICO_PROTO_UDP)) { + pico_enqueue(pico_proto_udp.q_in, f); + return 1; + } + +#endif + pico_frame_discard(f); + return 1; + } + + return 0; +} +static int pico_ipv6_process_in(struct pico_protocol *self, struct pico_frame *f) +{ + int proto = 0; + struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr; + struct pico_ipv6_exthdr *hbh; + IGNORE_PARAMETER(self); + /* forward if not local, except if router alert is set */ + if (pico_ipv6_is_unicast(&hdr->dst) && !pico_ipv6_link_get(&hdr->dst)) { + if(hdr->nxthdr == 0) { + hbh = (struct pico_ipv6_exthdr *) f->transport_hdr; + if(hbh->ext.routing.routtype == 0) + return pico_ipv6_forward(f); + } else + /* not local, try to forward. */ + return pico_ipv6_forward(f); + } + + proto = pico_ipv6_extension_headers(f); + if (proto <= 0) { + pico_frame_discard(f); + return 0; + } + + f->proto = (uint8_t)proto; + ipv6_dbg("IPv6: payload %u net_len %u nxthdr %u\n", short_be(hdr->len), f->net_len, proto); + + if (pico_ipv6_is_unicast(&hdr->dst)) { + pico_transport_receive(f, f->proto); + } else if (pico_ipv6_is_multicast(hdr->dst.addr)) { + /* XXX perform multicast filtering: solicited-node multicast address MUST BE allowed! */ + if (pico_ipv6_process_mcast_in(f) > 0) + return 0; + pico_transport_receive(f, f->proto); + } + + return 0; +} + +static int pico_ipv6_process_out(struct pico_protocol *self, struct pico_frame *f) +{ + IGNORE_PARAMETER(self); + + f->start = (uint8_t*)f->net_hdr; + return pico_sendto_dev(f); +} + +/* allocates an IPv6 packet without extension headers. If extension headers are needed, + * include the len of the extension headers in the size parameter. Once a frame acquired + * increment net_len and transport_hdr with the len of the extension headers, decrement + * transport_len with this value. + */ +static struct pico_frame *pico_ipv6_alloc(struct pico_protocol *self, uint16_t size) +{ + struct pico_frame *f = pico_frame_alloc((uint32_t)(size + PICO_SIZE_IP6HDR + PICO_SIZE_ETHHDR)); + + IGNORE_PARAMETER(self); + + if (!f) + return NULL; + + f->datalink_hdr = f->buffer; + f->net_hdr = f->buffer + PICO_SIZE_ETHHDR; + f->net_len = PICO_SIZE_IP6HDR; + f->transport_hdr = f->net_hdr + PICO_SIZE_IP6HDR; + f->transport_len = (uint16_t)size; + /* PICO_SIZE_ETHHDR is accounted for in pico_ethernet_send */ + f->len = (uint32_t)(size + PICO_SIZE_IP6HDR); + return f; +} + +static inline int ipv6_pushed_frame_valid(struct pico_frame *f, struct pico_ip6 *dst) +{ + struct pico_ipv6_hdr *hdr = NULL; + if(!f || !dst) + return -1; + + hdr = (struct pico_ipv6_hdr *)f->net_hdr; + if (!hdr) { + dbg("IPv6: IP header error\n"); + return -1; + } + + return 0; +} +int pico_ipv6_is_null_address(struct pico_ip6 * ip6) { + struct pico_ip6 null_addr = {{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }}; + return !memcmp(ip6, &null_addr, sizeof(struct pico_ip6)); +} +#ifdef PICO_SUPPORT_MCAST +/* link + * | + * MCASTGroups + * | | | + * ------------ | ------------ + * | | | + * MCASTSources MCASTSources MCASTSources + * | | | | | | | | | | | | + * S S S S S S S S S S S S + * + * MCASTGroups: RBTree(mcast_group) + * MCASTSources: RBTree(source) + */ +static int ipv6_mcast_groups_cmp(void *ka, void *kb) +{ + struct pico_ipv6_mcast_group *a = ka, *b = kb; + return pico_ipv6_compare(&a->mcast_addr, &b->mcast_addr); +} +static int ipv6_mcast_sources_cmp(void *ka, void *kb) +{ + struct pico_ip6 *a = ka, *b = kb; + return pico_ipv6_compare(a, b); +} + +static void pico_ipv6_mcast_print_groups(struct pico_ipv6_link *mcast_link) +{ +#ifdef PICO_DEBUG_MULTICAST + uint16_t i = 0; + struct pico_ipv6_mcast_group *g = NULL; + struct pico_ip6 *source = NULL; + struct pico_tree_node *index = NULL, *index2 = NULL; + char *ipv6_addr; + (void) source; + ipv6_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"); + ipv6_mcast_dbg("+ MULTICAST list interface %-16s +\n", mcast_link->dev->name); + ipv6_mcast_dbg("+------------------------------------------------------------------------------------------+\n"); + ipv6_mcast_dbg("+ nr | interface | host group | reference count | filter mode | source +\n"); + ipv6_mcast_dbg("+------------------------------------------------------------------------------------------+\n"); + ipv6_addr = PICO_ZALLOC(PICO_IPV6_STRING); + pico_tree_foreach(index, mcast_link->MCASTGroups) { + g = index->keyValue; + pico_ipv6_to_string(ipv6_addr, &g->mcast_addr.addr[0]); + ipv6_mcast_dbg("+ %04d | %16s | %s | %05u | %u | %8s +\n", i, mcast_link->dev->name, ipv6_addr, g->reference_count, g->filter_mode, ""); + pico_tree_foreach(index2, &g->MCASTSources) { + source = index2->keyValue; + pico_ipv6_to_string(ipv6_addr, source->addr); + ipv6_mcast_dbg("+ %4s | %16s | %8s | %5s | %s | %s +\n", "", "", "", "", "", ipv6_addr); + } + i++; + } + PICO_FREE(ipv6_addr); + ipv6_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"); +#else + IGNORE_PARAMETER(mcast_link); +#endif + +} + +static int mcast_group_update_ipv6(struct pico_ipv6_mcast_group *g, struct pico_tree *_MCASTFilter, uint8_t filter_mode) +{ + struct pico_tree_node *index = NULL, *_tmp = NULL; + struct pico_ip6 *source = NULL; + /* cleanup filter */ + pico_tree_foreach_safe(index, &g->MCASTSources, _tmp) { + source = index->keyValue; + pico_tree_delete(&g->MCASTSources, source); + PICO_FREE(source); + } + /* insert new filter */ + if (_MCASTFilter) { + pico_tree_foreach(index, _MCASTFilter) { + if (index->keyValue) { + source = PICO_ZALLOC(sizeof(struct pico_ip6)); + if (!source) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + *source = *((struct pico_ip6 *)index->keyValue); + pico_tree_insert(&g->MCASTSources, source); + } + } + } + + g->filter_mode = filter_mode; + return 0; +} + +int pico_ipv6_mcast_join(struct pico_ip6 *mcast_link, struct pico_ip6 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *_MCASTFilter) +{ + struct pico_ipv6_mcast_group *g = NULL, test = { + 0 + }; + struct pico_ipv6_link *link = NULL; + int res = -1; + if (mcast_link) { + link = pico_ipv6_link_get(mcast_link); + } + if (!link) { + link = mcast_default_link_ipv6; + } + test.mcast_addr = *mcast_group; + g = pico_tree_findKey(link->MCASTGroups, &test); + if (g) { + if (reference_count) + g->reference_count++; + res = pico_mld_state_change(mcast_link, mcast_group, filter_mode, _MCASTFilter, PICO_MLD_STATE_UPDATE); + } else { + g = PICO_ZALLOC(sizeof(struct pico_ipv6_mcast_group)); + if (!g) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + /* "non-existent" state of filter mode INCLUDE and empty source list */ + g->filter_mode = PICO_IP_MULTICAST_INCLUDE; + g->reference_count = 1; + g->mcast_addr = *mcast_group; + g->MCASTSources.root = &LEAF; + g->MCASTSources.compare = ipv6_mcast_sources_cmp; + pico_tree_insert(link->MCASTGroups, g); + res = pico_mld_state_change(mcast_link, mcast_group, filter_mode, _MCASTFilter, PICO_MLD_STATE_CREATE); + } + + if (mcast_group_update_ipv6(g, _MCASTFilter, filter_mode) < 0) { + dbg("Error in mcast_group update\n"); + return -1; + } + pico_ipv6_mcast_print_groups(link); + return res; +} + +int pico_ipv6_mcast_leave(struct pico_ip6 *mcast_link, struct pico_ip6 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *_MCASTFilter) +{ + struct pico_ipv6_mcast_group *g = NULL, test = { + 0 + }; + struct pico_ipv6_link *link = NULL; + struct pico_tree_node *index = NULL, *_tmp = NULL; + struct pico_ip6 *source = NULL; + int res = -1; + if (mcast_link) + link = pico_ipv6_link_get(mcast_link); + + if (!link) + link = mcast_default_link_ipv6; + + test.mcast_addr = *mcast_group; + g = pico_tree_findKey(link->MCASTGroups, &test); + if (!g) { + pico_err = PICO_ERR_EINVAL; + return -1; + } else { + if (reference_count && (--(g->reference_count) < 1)) { + res = pico_mld_state_change(mcast_link, mcast_group, filter_mode, _MCASTFilter, PICO_MLD_STATE_DELETE); + /* cleanup filter */ + pico_tree_foreach_safe(index, &g->MCASTSources, _tmp) { + source = index->keyValue; + pico_tree_delete(&g->MCASTSources, source); + PICO_FREE(source); + } + pico_tree_delete(link->MCASTGroups, g); + PICO_FREE(g); + } else { + res = pico_mld_state_change(mcast_link, mcast_group, filter_mode, _MCASTFilter, PICO_MLD_STATE_UPDATE); + if (mcast_group_update_ipv6(g, _MCASTFilter, filter_mode) < 0) + return -1; + } + } + + pico_ipv6_mcast_print_groups(link); + return res; +} + +struct pico_ipv6_link *pico_ipv6_get_default_mcastlink(void) +{ + return mcast_default_link_ipv6; +} + +static int pico_ipv6_mcast_filter(struct pico_frame *f) +{ + struct pico_ipv6_link *link = NULL; + struct pico_tree_node *index = NULL, *index2 = NULL; + struct pico_ipv6_mcast_group *g = NULL, test = { + 0 + }; + struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *) f->net_hdr; +#ifdef PICO_DEBUG_MULTICAST + char ipv6_addr[PICO_IPV6_STRING]; +#endif + test.mcast_addr = hdr->dst; + + pico_tree_foreach(index, &Tree_dev_ip6_link) { + link = index->keyValue; + g = pico_tree_findKey(link->MCASTGroups, &test); + if (g) { + if (f->dev == link->dev) { +#ifdef PICO_DEBUG_MULTICAST + pico_ipv6_to_string( ipv6_addr, &hdr->dst.addr[0]); + ipv6_mcast_dbg("MCAST: IP %s is group member of current link %s\n", ipv6_addr, f->dev->name); +#endif + /* perform source filtering */ + switch (g->filter_mode) { + case PICO_IP_MULTICAST_INCLUDE: + pico_tree_foreach(index2, &g->MCASTSources) { + if (hdr->src.addr == ((struct pico_ip6 *)index2->keyValue)->addr) { +#ifdef PICO_DEBUG_MULTICAST + pico_ipv6_to_string(ipv6_addr,&hdr->src.addr[0]); + ipv6_mcast_dbg("MCAST: IP %s in included interface source list\n", ipv6_addr); +#endif + return 0; + } + } +#ifdef PICO_DEBUG_MULTICAST + pico_ipv6_to_string(ipv6_addr,&hdr->src.addr[0]); + ipv6_mcast_dbg("MCAST: IP %s NOT in included interface source list\n", ipv6_addr); +#endif + return -1; + + case PICO_IP_MULTICAST_EXCLUDE: + pico_tree_foreach(index2, &g->MCASTSources) { + if (memcmp(hdr->src.addr , (((struct pico_ip6 *)index2->keyValue)->addr) , sizeof(struct pico_ip6))== 0){ +#ifdef PICO_DEBUG_MULTICAST + pico_ipv6_to_string(ipv6_addr,&hdr->src.addr[0]); + ipv6_mcast_dbg("MCAST: IP %s in excluded interface source list\n", ipv6_addr); +#endif + return -1; + } + } +#ifdef PICO_DEBUG_MULTICAST + pico_ipv6_to_string(ipv6_addr,&hdr->src.addr[0]); + ipv6_mcast_dbg("MCAST: IP %s NOT in excluded interface source list\n", ipv6_addr); +#endif + return 0; + + default: + return -1; + } + } else { +#ifdef PICO_DEBUG_MULTICAST + pico_ipv6_to_string(ipv6_addr,&hdr->dst.addr[0]); + ipv6_mcast_dbg("MCAST: IP %s is group member of different link %s\n", ipv6_addr, link->dev->name); +#endif + } + } else { +#ifdef PICO_DEBUG_MULTICAST + pico_ipv6_to_string(ipv6_addr,&hdr->dst.addr[0]); + ipv6_mcast_dbg("MCAST: IP %s is not a group member of link %s\n", ipv6_addr, f->dev->name); +#endif + } + } + return -1; +} + +#else + +int pico_ipv6_mcast_join(struct pico_ip6 *mcast_link, struct pico_ip6 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *_MCASTFilter) +{ + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; +} + +int pico_ipv6_mcast_leave(struct pico_ip6 *mcast_link, struct pico_ip6 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *_MCASTFilter) +{ + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; +} + +struct pico_ipv6_link *pico_ipv6_get_default_mcastlink(void) +{ + pico_err = PICO_ERR_EPROTONOSUPPORT; + return NULL; +} +#endif /* PICO_SUPPORT_MCAST */ +static inline struct pico_ipv6_route *ipv6_pushed_frame_checks(struct pico_frame *f, struct pico_ip6 *dst) +{ + struct pico_ipv6_route *route = NULL; + + if (ipv6_pushed_frame_valid(f, dst) < 0) + return NULL; + + if (memcmp(dst->addr, PICO_IP6_ANY, PICO_SIZE_IP6) == 0) { + dbg("IPv6: IP destination address error\n"); + return NULL; + } + + route = pico_ipv6_route_find(dst); + if (!route && !f->dev) { + dbg("IPv6: route not found.\n"); + pico_err = PICO_ERR_EHOSTUNREACH; + return NULL; + } + + return route; +} + +static inline void ipv6_push_hdr_adjust(struct pico_frame *f, struct pico_ipv6_link *link, struct pico_ip6 *src, struct pico_ip6 *dst, uint8_t proto, int is_dad) +{ + struct pico_icmp6_hdr *icmp6_hdr = NULL; + struct pico_ipv6_hdr *hdr = NULL; + struct pico_ipv6_exthdr *hbh = NULL; + const uint8_t vtf = (uint8_t)long_be(0x60000000); /* version 6, traffic class 0, flow label 0 */ + + hdr = (struct pico_ipv6_hdr *)f->net_hdr; + hdr->vtf = vtf; + hdr->len = short_be((uint16_t)(f->transport_len + f->net_len - (uint16_t)sizeof(struct pico_ipv6_hdr))); + hdr->nxthdr = proto; + hdr->hop = f->dev->hostvars.hoplimit; + hdr->dst = *dst; + + if (!src || !pico_ipv6_is_unicast(src)) + /* Address defaults to the link information: src address selection is done via link */ + hdr->src = link->address; + else { + /* Sender protocol is forcing an IPv6 address */ + hdr->src = *src; + } + + if (f->send_ttl) { + hdr->hop = f->send_ttl; + } + + if (f->send_tos) { + hdr->vtf |= ((uint32_t)f->send_tos << 20u); + } + + /* make adjustments to defaults according to proto */ + switch (proto) + { + case 0: + { + hbh = (struct pico_ipv6_exthdr *) f->transport_hdr; + switch(hbh->nxthdr) { + case PICO_PROTO_ICMP6: + { + icmp6_hdr = (struct pico_icmp6_hdr *)(f->transport_hdr+sizeof(struct pico_ipv6_exthdr)); + if((icmp6_hdr->type >= PICO_MLD_QUERY && icmp6_hdr->type <= PICO_MLD_DONE) || icmp6_hdr->type == PICO_MLD_REPORTV2) { + hdr->hop = 1; + } + icmp6_hdr->crc = 0; + icmp6_hdr->crc = short_be(pico_mld_checksum(f)); + break; + } + } + break; + } + case PICO_PROTO_ICMP6: + { + icmp6_hdr = (struct pico_icmp6_hdr *)f->transport_hdr; + if (icmp6_hdr->type == PICO_ICMP6_NEIGH_SOL || icmp6_hdr->type == PICO_ICMP6_NEIGH_ADV || icmp6_hdr->type == PICO_ICMP6_ROUTER_SOL || icmp6_hdr->type == PICO_ICMP6_ROUTER_ADV) + hdr->hop = 255; + + if ((is_dad || link->istentative) && icmp6_hdr->type == PICO_ICMP6_NEIGH_SOL){ + memcpy(hdr->src.addr, PICO_IP6_ANY, PICO_SIZE_IP6); + + } + + icmp6_hdr->crc = 0; + icmp6_hdr->crc = short_be(pico_icmp6_checksum(f)); + break; + } +#ifdef PICO_SUPPORT_UDP + case PICO_PROTO_UDP: + { + struct pico_udp_hdr *udp_hdr = (struct pico_udp_hdr *) f->transport_hdr; + udp_hdr->crc = short_be(pico_udp_checksum_ipv6(f)); + break; + } +#endif + + default: + break; + } + +} + +static int ipv6_frame_push_final(struct pico_frame *f) +{ + struct pico_ipv6_hdr *hdr = NULL; + hdr = (struct pico_ipv6_hdr *)f->net_hdr; + if(pico_ipv6_link_get(&hdr->dst)) { + return pico_enqueue(&ipv6_in, f); + } + else { + return pico_enqueue(&ipv6_out, f); + } +} + +struct pico_ipv6_link *pico_ipv6_linklocal_get(struct pico_device *dev); + +int pico_ipv6_frame_push(struct pico_frame *f, struct pico_ip6 *src, struct pico_ip6 *dst, uint8_t proto, int is_dad) +{ + struct pico_ipv6_route *route = NULL; + struct pico_ipv6_link *link = NULL; + + if (pico_ipv6_is_linklocal(dst->addr) || pico_ipv6_is_multicast(dst->addr) || pico_ipv6_is_sitelocal(dst->addr)) { + if (!f->dev) { + pico_frame_discard(f); + return -1; + } + if (pico_ipv6_is_sitelocal(dst->addr)) + link = pico_ipv6_sitelocal_get(f->dev); + else + link = pico_ipv6_linklocal_get(f->dev); + + if (link) + goto push_final; + } + + if (pico_ipv6_is_localhost(dst->addr)) { + f->dev = pico_get_device("loop"); + } + + route = ipv6_pushed_frame_checks(f, dst); + if (!route) { + pico_frame_discard(f); + return -1; + } + + link = route->link; + + if (f->sock && f->sock->dev) + f->dev = f->sock->dev; + else { + f->dev = link->dev; + if (f->sock) + f->sock->dev = f->dev; + } + + + #if 0 + if (pico_ipv6_is_multicast(hdr->dst.addr)) { + /* XXX: reimplement loopback */ + } + + #endif + +push_final: + ipv6_push_hdr_adjust(f, link, src, dst, proto, is_dad); + return ipv6_frame_push_final(f); +} + +static int pico_ipv6_frame_sock_push(struct pico_protocol *self, struct pico_frame *f) +{ + struct pico_ip6 *dst = NULL; + struct pico_remote_endpoint *remote_endpoint = NULL; + + IGNORE_PARAMETER(self); + + if (!f->sock) { + pico_frame_discard(f); + return -1; + } + + remote_endpoint = (struct pico_remote_endpoint *)f->info; + if (remote_endpoint) { + dst = &remote_endpoint->remote_addr.ip6; + } else { + dst = &f->sock->remote_addr.ip6; + } + + return pico_ipv6_frame_push(f, NULL, dst, (uint8_t)f->sock->proto->proto_number, 0); +} + +/* interface: protocol definition */ +struct pico_protocol pico_proto_ipv6 = { + .name = "ipv6", + .proto_number = PICO_PROTO_IPV6, + .layer = PICO_LAYER_NETWORK, + .alloc = pico_ipv6_alloc, + .process_in = pico_ipv6_process_in, + .process_out = pico_ipv6_process_out, + .push = pico_ipv6_frame_sock_push, + .q_in = &ipv6_in, + .q_out = &ipv6_out, +}; + +#ifdef DEBUG_ROUTE +static void pico_ipv6_dbg_route(void) +{ + struct pico_ipv6_route *r; + struct pico_tree_node *index; + pico_tree_foreach(index, &Routes){ + r = index->keyValue; + dbg("Route to %08x/%08x, gw %08x, dev: %s, metric: %d\n", r->dest.addr, r->netmask.addr, r->gateway.addr, r->link->dev->name, r->metric); + } +} +#else +#define pico_ipv6_dbg_route() do { } while(0) +#endif + +static inline struct pico_ipv6_route *ipv6_route_add_link(struct pico_ip6 gateway) +{ + struct pico_ip6 zerogateway = {{0}}; + struct pico_ipv6_route *r = pico_ipv6_route_find(&gateway); + if (!r ) { /* Specified Gateway is unreachable */ + pico_err = PICO_ERR_EHOSTUNREACH; + return NULL; + } + + if (memcmp(r->gateway.addr, zerogateway.addr, PICO_SIZE_IP6) != 0) { /* Specified Gateway is not a neighbor */ + pico_err = PICO_ERR_ENETUNREACH; + return NULL; + } + + + return r; +} + +int pico_ipv6_route_add(struct pico_ip6 address, struct pico_ip6 netmask, struct pico_ip6 gateway, int metric, struct pico_ipv6_link *link) +{ + struct pico_ip6 zerogateway = {{0}}; + struct pico_ipv6_route test, *new = NULL; + test.dest = address; + test.netmask = netmask; + test.metric = (uint32_t)metric; + if (pico_tree_findKey(&IPV6Routes, &test)) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + new = PICO_ZALLOC(sizeof(struct pico_ipv6_route)); + if (!new) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + ipv6_dbg("Adding IPV6 static route\n"); + new->dest = address; + new->netmask = netmask; + new->gateway = gateway; + new->metric = (uint32_t)metric; + if (memcmp(gateway.addr, zerogateway.addr, PICO_SIZE_IP6) == 0) { + /* No gateway provided, use the link */ + new->link = link; + } else { + struct pico_ipv6_route *r = ipv6_route_add_link(gateway); + if (!r) { + if (link) + new->link = link; + else { + PICO_FREE(new); + return -1; + } + } else { + new->link = r->link; + } + } + + if (new->link && (pico_ipv6_is_global(address.addr)) && (!pico_ipv6_is_global(new->link->address.addr))) { + new->link = pico_ipv6_global_get(new->link->dev); + } + + if (!new->link) { + pico_err = PICO_ERR_EINVAL; + PICO_FREE(new); + return -1; + } + + + pico_tree_insert(&IPV6Routes, new); + pico_ipv6_dbg_route(); + return 0; +} + +int pico_ipv6_route_del(struct pico_ip6 address, struct pico_ip6 netmask, struct pico_ip6 gateway, int metric, struct pico_ipv6_link *link) +{ + struct pico_ipv6_route test, *found = NULL; + + IGNORE_PARAMETER(gateway); + + if (!link) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + test.dest = address; + test.netmask = netmask; + test.metric = (uint32_t)metric; + + found = pico_tree_findKey(&IPV6Routes, &test); + if (found) { + pico_tree_delete(&IPV6Routes, found); + PICO_FREE(found); + pico_ipv6_dbg_route(); + return 0; + } + + pico_err = PICO_ERR_EINVAL; + return -1; +} + +void pico_ipv6_router_down(struct pico_ip6 *address) +{ + struct pico_tree_node *index = NULL, *_tmp = NULL; + struct pico_ipv6_route *route = NULL; + if (!address) + return; + + pico_tree_foreach_safe(index, &IPV6Routes, _tmp) + { + route = index->keyValue; + if (pico_ipv6_compare(address, &route->gateway) == 0) + pico_ipv6_route_del(route->dest, route->netmask, route->gateway, (int)route->metric, route->link); + } +} + +#ifndef UNIT_TEST +static void pico_ipv6_nd_dad(pico_time now, void *arg) +{ + struct pico_ip6 *address = (struct pico_ip6 *)arg; + struct pico_ipv6_link *l = NULL; + struct pico_ip6 old_address; + if (!arg) + return; + + IGNORE_PARAMETER(now); + + l = pico_ipv6_link_istentative(address); + if (!l) + return; + + if (pico_device_link_state(l->dev) == 0) { + l->dad_timer = pico_timer_add(100, pico_ipv6_nd_dad, &l->address); + return; + } + + if (l->isduplicate) { + dbg("IPv6: duplicate address.\n"); + old_address = *address; + if (pico_ipv6_is_linklocal(address->addr)) { + address->addr[8] = (uint8_t)((uint8_t)(pico_rand() & 0xff) & (uint8_t)(~0x03)); + address->addr[9] = pico_rand() & 0xff; + address->addr[10] = pico_rand() & 0xff; + address->addr[11] = pico_rand() & 0xff; + address->addr[12] = pico_rand() & 0xff; + address->addr[13] = pico_rand() & 0xff; + address->addr[14] = pico_rand() & 0xff; + address->addr[15] = pico_rand() & 0xff; + pico_ipv6_link_add(l->dev, *address, l->netmask); + } + + pico_ipv6_link_del(l->dev, old_address); + } + else { + if (l->dup_detect_retrans-- == 0) { + dbg("IPv6: DAD verified valid address.\n"); + l->istentative = 0; + } else { + /* Duplicate Address Detection */ + pico_icmp6_neighbor_solicitation(l->dev, &l->address, PICO_ICMP6_ND_DAD); + l->dad_timer = pico_timer_add(PICO_ICMP6_MAX_RTR_SOL_DELAY, pico_ipv6_nd_dad, &l->address); + } + } +} +#endif + + +struct pico_ipv6_link *pico_ipv6_link_add(struct pico_device *dev, struct pico_ip6 address, struct pico_ip6 netmask) +{ + struct pico_ipv6_link test = { + 0 + }, *new = NULL; + struct pico_ip6 network = {{0}}, gateway = {{0}}; + struct pico_ip6 mcast_addr = {{ 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }}; + struct pico_ip6 mcast_nm = {{ 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }}; + struct pico_ip6 mcast_gw = {{0}}; + struct pico_ip6 all_hosts = {{ 0xff, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01 }}; +#ifdef PICO_DEBUG_IPV6 + char ipstr[40] = { + 0 + }; +#endif + int i = 0; + if (!dev) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + test.address = address; + test.dev = dev; + /** XXX: Valid netmask / unicast address test **/ + + if (pico_tree_findKey(&IPV6Links, &test)) { + dbg("IPv6: trying to assign an invalid address (in use)\n"); + pico_err = PICO_ERR_EADDRINUSE; + return NULL; + } + + /** XXX: Check for network already in use (e.g. trying to assign 10.0.0.1/24 where 10.1.0.1/8 is in use) **/ + new = PICO_ZALLOC(sizeof(struct pico_ipv6_link)); + if (!new) { + dbg("IPv6: out of memory!\n"); + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + new->address = address; + new->netmask = netmask; + new->dev = dev; + new->istentative = 1; + new->isduplicate = 0; +#ifdef PICO_SUPPORT_MCAST + new->MCASTGroups = PICO_ZALLOC(sizeof(struct pico_tree)); + if (!new->MCASTGroups) { + PICO_FREE(new); + dbg("IPv6: Out of memory!\n"); + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + new->MCASTGroups->root = &LEAF; + new->MCASTGroups->compare = ipv6_mcast_groups_cmp; + new->mcast_compatibility = PICO_MLDV2; + new->mcast_last_query_interval = MLD_QUERY_INTERVAL; +#endif + pico_tree_insert(&IPV6Links, new); + for (i = 0; i < PICO_SIZE_IP6; ++i) { + network.addr[i] = address.addr[i] & netmask.addr[i]; + } +#ifdef PICO_SUPPORT_MCAST + do { + if (!mcast_default_link_ipv6) { + mcast_default_link_ipv6 = new; + pico_ipv6_route_add(mcast_addr, mcast_nm, mcast_gw, 1, new); + } + pico_ipv6_mcast_join(&address, &all_hosts, 1, PICO_IP_MULTICAST_EXCLUDE, NULL); + } while(0); +#endif + pico_ipv6_route_add(network, netmask, gateway, 1, new); + pico_ipv6_route_add(mcast_addr, mcast_nm, mcast_gw, 1, new); + new->dup_detect_retrans = PICO_IPV6_DEFAULT_DAD_RETRANS; +#ifndef UNIT_TEST + /* Duplicate Address Detection */ + new->dad_timer = pico_timer_add(100, pico_ipv6_nd_dad, &new->address); +#else + new->istentative = 0; +#endif + +#ifdef PICO_DEBUG_IPV6 + pico_ipv6_to_string(ipstr, new->address.addr); + dbg("Assigned ipv6 %s to device %s\n", ipstr, new->dev->name); +#endif + return new; +} + +static int pico_ipv6_cleanup_routes(struct pico_ipv6_link *link) +{ + struct pico_tree_node *index = NULL, *_tmp = NULL; + struct pico_ipv6_route *route = NULL; + + pico_tree_foreach_safe(index, &IPV6Routes, _tmp) + { + route = index->keyValue; + if (link == route->link) + pico_ipv6_route_del(route->dest, route->netmask, route->gateway, (int)route->metric, route->link); + } + return 0; +} + +int pico_ipv6_cleanup_links(struct pico_device *dev) +{ + struct pico_tree_node *index = NULL, *_tmp = NULL; + struct pico_ipv6_link *link = NULL; + + pico_tree_foreach_safe(index, &IPV6Links, _tmp) + { + link = index->keyValue; + if (dev == link->dev) + pico_ipv6_link_del(dev, link->address); + } + return 0; +} + +int pico_ipv6_link_del(struct pico_device *dev, struct pico_ip6 address) +{ + struct pico_ipv6_link test = { + 0 + }, *found = NULL; + + if (!dev) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + test.address = address; + test.dev = dev; + found = pico_tree_findKey(&IPV6Links, &test); + if (!found) { + pico_err = PICO_ERR_ENXIO; + return -1; + } + + pico_ipv6_cleanup_routes(found); + if (found->dad_timer) + pico_timer_cancel(found->dad_timer); + + pico_tree_delete(&IPV6Links, found); + /* XXX MUST leave the solicited-node multicast address corresponding to the address (RFC 4861 $7.2.1) */ + PICO_FREE(found); + return 0; +} + +struct pico_ipv6_link *pico_ipv6_link_istentative(struct pico_ip6 *address) +{ + struct pico_ipv6_link test = { + 0 + }, *found = NULL; + test.address = *address; + + found = pico_tree_findKey(&IPV6Links, &test); + if (!found) + return NULL; + + if (found->istentative) + return found; + + return NULL; +} + +struct pico_ipv6_link *pico_ipv6_link_get(struct pico_ip6 *address) +{ + struct pico_ipv6_link test = { + 0 + }, *found = NULL; + test.address = *address; + found = pico_tree_findKey(&IPV6Links, &test); + if (!found) { + return NULL; + } + if (found->istentative) { + return NULL; + } + return found; +} + +struct pico_device *pico_ipv6_link_find(struct pico_ip6 *address) +{ + struct pico_ipv6_link test = { + 0 + }, *found = NULL; + if(!address) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + test.dev = NULL; + test.address = *address; + found = pico_tree_findKey(&IPV6Links, &test); + if (!found) { + pico_err = PICO_ERR_ENXIO; + return NULL; + } + + if (found->istentative) { + return NULL; + } + + return found->dev; +} + +struct pico_ip6 pico_ipv6_route_get_gateway(struct pico_ip6 *addr) +{ + struct pico_ip6 nullip = {{0}}; + struct pico_ipv6_route *route = NULL; + + if (!addr) { + pico_err = PICO_ERR_EINVAL; + return nullip; + } + + route = pico_ipv6_route_find(addr); + if (!route) { + pico_err = PICO_ERR_EHOSTUNREACH; + return nullip; + } + else + return route->gateway; +} + + +struct pico_ipv6_link *pico_ipv6_link_by_dev(struct pico_device *dev) +{ + struct pico_tree_node *index = NULL; + struct pico_ipv6_link *link = NULL; + + pico_tree_foreach(index, &IPV6Links) + { + link = index->keyValue; + if (dev == link->dev) + return link; + } + return NULL; +} + +struct pico_ipv6_link *pico_ipv6_link_by_dev_next(struct pico_device *dev, struct pico_ipv6_link *last) +{ + struct pico_tree_node *index = NULL; + struct pico_ipv6_link *link = NULL; + int valid = 0; + + if (last == NULL) + valid = 1; + + pico_tree_foreach(index, &IPV6Links) + { + link = index->keyValue; + if (link->dev == dev) { + if (last == link) + valid = 1; + else if (valid > 0) + return link; + } + } + return NULL; +} + +struct pico_ipv6_link *pico_ipv6_prefix_configured(struct pico_ip6 *prefix) +{ + unsigned int nm64_len = 8; + struct pico_tree_node *index = NULL; + struct pico_ipv6_link *link = NULL; + pico_tree_foreach(index, &IPV6Links) { + link = index->keyValue; + if (memcmp(link->address.addr, prefix->addr, nm64_len) == 0) + return link; + } + return NULL; +} + +struct pico_ipv6_link *pico_ipv6_linklocal_get(struct pico_device *dev) +{ + struct pico_ipv6_link *link = pico_ipv6_link_by_dev(dev); + while (link && !pico_ipv6_is_linklocal(link->address.addr)) { + link = pico_ipv6_link_by_dev_next(dev, link); + } + return link; +} + +struct pico_ipv6_link *pico_ipv6_sitelocal_get(struct pico_device *dev) +{ + struct pico_ipv6_link *link = pico_ipv6_link_by_dev(dev); + while (link && !pico_ipv6_is_sitelocal(link->address.addr)) { + link = pico_ipv6_link_by_dev_next(dev, link); + } + return link; +} + +struct pico_ipv6_link *pico_ipv6_global_get(struct pico_device *dev) +{ + struct pico_ipv6_link *link = pico_ipv6_link_by_dev(dev); + while (link && !pico_ipv6_is_global(link->address.addr)) { + link = pico_ipv6_link_by_dev_next(dev, link); + } + return link; +} + +#define TWO_HOURS ((pico_time)(1000 * 60 * 60 * 2)) + +void pico_ipv6_check_lifetime_expired(pico_time now, void *arg) +{ + struct pico_tree_node *index = NULL, *temp; + struct pico_ipv6_link *link = NULL; + (void)arg; + pico_tree_foreach_safe(index, &IPV6Links, temp) { + link = index->keyValue; + if ((link->expire_time > 0) && (link->expire_time < now)) { + dbg("Warning: IPv6 address has expired.\n"); + pico_ipv6_link_del(link->dev, link->address); + } + } + pico_timer_add(1000, pico_ipv6_check_lifetime_expired, NULL); +} + +int pico_ipv6_lifetime_set(struct pico_ipv6_link *l, pico_time expire) +{ + pico_time now = PICO_TIME_MS(); + if (expire <= now) { + return -1; + } + + if (expire > 0xFFFFFFFE) { + l->expire_time = 0u; + }else if ((expire > (now + TWO_HOURS)) || (expire > l->expire_time)) { + l->expire_time = expire; + } else { + l->expire_time = now + TWO_HOURS; + } + + return 0; +} + +int pico_ipv6_dev_routing_enable(struct pico_device *dev) +{ + dev->hostvars.routing = 1; + return 0; +} + +int pico_ipv6_dev_routing_disable(struct pico_device *dev) +{ + dev->hostvars.routing = 0; + return 0; +} + +void pico_ipv6_unreachable(struct pico_frame *f, uint8_t code) +{ + struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr; +#if defined PICO_SUPPORT_TCP || defined PICO_SUPPORT_UDP + pico_transport_error(f, hdr->nxthdr, code); +#endif +} + + + +#endif diff --git a/ext/picotcp/modules/pico_ipv6.h b/ext/picotcp/modules/pico_ipv6.h new file mode 100644 index 0000000..80af525 --- /dev/null +++ b/ext/picotcp/modules/pico_ipv6.h @@ -0,0 +1,175 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + *********************************************************************/ +#ifndef _INCLUDE_PICO_IPV6 +#define _INCLUDE_PICO_IPV6 +#include "pico_addressing.h" +#include "pico_protocol.h" +#include "pico_ipv4.h" +#define PICO_SIZE_IP6HDR ((uint32_t)(sizeof(struct pico_ipv6_hdr))) +#define PICO_IPV6_DEFAULT_HOP 64 +#define PICO_IPV6_MIN_MTU 1280 +#define PICO_IPV6_STRING 46 + +#define PICO_IPV6_EXTHDR_HOPBYHOP 0 +#define PICO_IPV6_EXTHDR_ROUTING 43 +#define PICO_IPV6_EXTHDR_FRAG 44 +#define PICO_IPV6_EXTHDR_ESP 50 +#define PICO_IPV6_EXTHDR_AUTH 51 +#define PICO_IPV6_EXTHDR_NONE 59 +#define PICO_IPV6_EXTHDR_DESTOPT 60 + + +#define PICO_IPV6_EXTHDR_OPT_ROUTER_ALERT 5 +#define PICO_IPV6_EXTHDR_OPT_ROUTER_ALERT_DATALEN 2 + +#define HBH_LEN(hbh) ((((hbh->ext.hopbyhop.len + 1) << 3) - 2)) /* len in bytes, minus nxthdr and len byte */ + +extern const uint8_t PICO_IP6_ANY[PICO_SIZE_IP6]; +extern struct pico_protocol pico_proto_ipv6; +extern struct pico_tree IPV6Routes; + +PACKED_STRUCT_DEF pico_ipv6_hdr { + uint32_t vtf; + uint16_t len; + uint8_t nxthdr; + uint8_t hop; + struct pico_ip6 src; + struct pico_ip6 dst; +}; + +PACKED_STRUCT_DEF pico_ipv6_pseudo_hdr +{ + struct pico_ip6 src; + struct pico_ip6 dst; + uint32_t len; + uint8_t zero[3]; + uint8_t nxthdr; +}; + +struct pico_ipv6_link +{ + struct pico_device *dev; + struct pico_ip6 address; + struct pico_ip6 netmask; + uint8_t istentative : 1; + uint8_t isduplicate : 1; + uint32_t dad_timer; + uint16_t dup_detect_retrans; + pico_time expire_time; +#ifdef PICO_SUPPORT_MCAST + struct pico_tree *MCASTGroups; + uint8_t mcast_compatibility; + uint8_t mcast_last_query_interval; +#endif + +}; +union pico_link { + struct pico_ipv4_link ipv4; + struct pico_ipv6_link ipv6; +}; + +struct pico_ipv6_hbhoption { + uint8_t type; + uint8_t len; +}; +#ifdef PICO_SUPPORT_MCAST +struct pico_ipv6_mcast_group { + uint8_t filter_mode; + uint16_t reference_count; + struct pico_ip6 mcast_addr; + struct pico_tree MCASTSources; +}; +#endif +struct pico_ipv6_destoption { + uint8_t type; + uint8_t len; +}; + +struct pico_ipv6_route +{ + struct pico_ip6 dest; + struct pico_ip6 netmask; + struct pico_ip6 gateway; + struct pico_ipv6_link *link; + uint32_t metric; +}; + +PACKED_STRUCT_DEF pico_ipv6_exthdr { + uint8_t nxthdr; + + PACKED_UNION_DEF ipv6_ext_u { + PEDANTIC_STRUCT_DEF hopbyhop_s { + uint8_t len; + } hopbyhop; + + PEDANTIC_STRUCT_DEF destopt_s { + uint8_t len; + } destopt; + + PEDANTIC_STRUCT_DEF routing_s { + uint8_t len; + uint8_t routtype; + uint8_t segleft; + } routing; + + PEDANTIC_STRUCT_DEF fragmentation_s { + uint8_t res; + uint8_t om[2]; + uint8_t id[4]; + } frag; + } ext; +}; + +int pico_ipv6_compare(struct pico_ip6 *a, struct pico_ip6 *b); +int pico_string_to_ipv6(const char *ipstr, uint8_t *ip); +int pico_ipv6_to_string(char *ipbuf, const uint8_t ip[PICO_SIZE_IP6]); +int pico_ipv6_is_unicast(struct pico_ip6 *a); +int pico_ipv6_is_multicast(const uint8_t addr[PICO_SIZE_IP6]); +int pico_ipv6_is_allhosts_multicast(const uint8_t addr[PICO_SIZE_IP6]); +int pico_ipv6_is_solnode_multicast(const uint8_t addr[PICO_SIZE_IP6], struct pico_device *dev); +int pico_ipv6_is_global(const uint8_t addr[PICO_SIZE_IP6]); +int pico_ipv6_is_uniquelocal(const uint8_t addr[PICO_SIZE_IP6]); +int pico_ipv6_is_sitelocal(const uint8_t addr[PICO_SIZE_IP6]); +int pico_ipv6_is_linklocal(const uint8_t addr[PICO_SIZE_IP6]); +int pico_ipv6_is_solicited(const uint8_t addr[PICO_SIZE_IP6]); +int pico_ipv6_is_unspecified(const uint8_t addr[PICO_SIZE_IP6]); +int pico_ipv6_is_localhost(const uint8_t addr[PICO_SIZE_IP6]); + +int pico_ipv6_frame_push(struct pico_frame *f, struct pico_ip6 *src, struct pico_ip6 *dst, uint8_t proto, int is_dad); +int pico_ipv6_route_add(struct pico_ip6 address, struct pico_ip6 netmask, struct pico_ip6 gateway, int metric, struct pico_ipv6_link *link); +int pico_ipv6_route_del(struct pico_ip6 address, struct pico_ip6 netmask, struct pico_ip6 gateway, int metric, struct pico_ipv6_link *link); +void pico_ipv6_unreachable(struct pico_frame *f, uint8_t code); + +struct pico_ipv6_link *pico_ipv6_link_add(struct pico_device *dev, struct pico_ip6 address, struct pico_ip6 netmask); +int pico_ipv6_link_del(struct pico_device *dev, struct pico_ip6 address); +int pico_ipv6_cleanup_links(struct pico_device *dev); +struct pico_ipv6_link *pico_ipv6_link_istentative(struct pico_ip6 *address); +struct pico_ipv6_link *pico_ipv6_link_get(struct pico_ip6 *address); +struct pico_device *pico_ipv6_link_find(struct pico_ip6 *address); +struct pico_ip6 pico_ipv6_route_get_gateway(struct pico_ip6 *addr); +struct pico_ip6 *pico_ipv6_source_find(const struct pico_ip6 *dst); +struct pico_device *pico_ipv6_source_dev_find(const struct pico_ip6 *dst); +struct pico_ipv6_link *pico_ipv6_link_by_dev(struct pico_device *dev); +struct pico_ipv6_link *pico_ipv6_link_by_dev_next(struct pico_device *dev, struct pico_ipv6_link *last); +struct pico_ipv6_link *pico_ipv6_global_get(struct pico_device *dev); +struct pico_ipv6_link *pico_ipv6_linklocal_get(struct pico_device *dev); +struct pico_ipv6_link *pico_ipv6_sitelocal_get(struct pico_device *dev); +struct pico_ipv6_link *pico_ipv6_prefix_configured(struct pico_ip6 *prefix); +int pico_ipv6_lifetime_set(struct pico_ipv6_link *l, pico_time expire); +void pico_ipv6_check_lifetime_expired(pico_time now, void *arg); +int pico_ipv6_dev_routing_enable(struct pico_device *dev); +int pico_ipv6_dev_routing_disable(struct pico_device *dev); +void pico_ipv6_router_down(struct pico_ip6 *address); + +int pico_ipv6_mcast_join(struct pico_ip6 *mcast_link, struct pico_ip6 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *_MCASTFilter); +int pico_ipv6_mcast_leave(struct pico_ip6 *mcast_link, struct pico_ip6 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *_MCASTFilter); + +struct pico_ipv6_link *pico_ipv6_get_default_mcastlink(void); + +int pico_ipv6_is_null_address(struct pico_ip6 * ip6); +#endif diff --git a/ext/picotcp/modules/pico_ipv6_nd.c b/ext/picotcp/modules/pico_ipv6_nd.c new file mode 100644 index 0000000..c1048b3 --- /dev/null +++ b/ext/picotcp/modules/pico_ipv6_nd.c @@ -0,0 +1,1009 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + Authors: Daniele Lacamera + *********************************************************************/ + +#include "pico_config.h" +#include "pico_tree.h" +#include "pico_icmp6.h" +#include "pico_ipv6.h" +#include "pico_stack.h" +#include "pico_device.h" +#include "pico_eth.h" +#include "pico_addressing.h" +#include "pico_ipv6_nd.h" + +#ifdef PICO_SUPPORT_IPV6 + + +#define nd_dbg(...) do {} while(0) + +static struct pico_frame *frames_queued_v6[PICO_ND_MAX_FRAMES_QUEUED] = { 0 }; + + +enum pico_ipv6_neighbor_state { + PICO_ND_STATE_INCOMPLETE = 0, + PICO_ND_STATE_REACHABLE, + PICO_ND_STATE_STALE, + PICO_ND_STATE_DELAY, + PICO_ND_STATE_PROBE +}; + +struct pico_ipv6_neighbor { + enum pico_ipv6_neighbor_state state; + struct pico_ip6 address; + struct pico_eth mac; + struct pico_device *dev; + uint16_t is_router; + uint16_t failure_count; + pico_time expire; +}; + +static int pico_ipv6_neighbor_compare(void *ka, void *kb) +{ + struct pico_ipv6_neighbor *a = ka, *b = kb; + return pico_ipv6_compare(&a->address, &b->address); +} + + +PICO_TREE_DECLARE(NCache, pico_ipv6_neighbor_compare); + +static struct pico_ipv6_neighbor *pico_nd_find_neighbor(struct pico_ip6 *dst) +{ + struct pico_ipv6_neighbor test = { + 0 + }; + + test.address = *dst; + return pico_tree_findKey(&NCache, &test); +} + +static void pico_ipv6_nd_queued_trigger(void) +{ + int i; + struct pico_frame *f; + for (i = 0; i < PICO_ND_MAX_FRAMES_QUEUED; i++) + { + f = frames_queued_v6[i]; + if (f) { + (void)pico_ethernet_send(f); + if(frames_queued_v6[i]) + pico_frame_discard(frames_queued_v6[i]); + frames_queued_v6[i] = NULL; + } + } +} + +static void ipv6_duplicate_detected(struct pico_ipv6_link *l) +{ + struct pico_device *dev; + int is_ll = pico_ipv6_is_linklocal(l->address.addr); + dev = l->dev; + dbg("IPV6: Duplicate address detected. Removing link.\n"); + pico_ipv6_link_del(l->dev, l->address); + if (is_ll) + pico_device_ipv6_random_ll(dev); +} + +static struct pico_ipv6_neighbor *pico_nd_add(struct pico_ip6 *addr, struct pico_device *dev) +{ + struct pico_ipv6_neighbor *n = PICO_ZALLOC(sizeof(struct pico_ipv6_neighbor)); + char address[120]; + if (!n) + return NULL; + + pico_ipv6_to_string(address, addr->addr); + nd_dbg("Adding address %s to cache...\n", address); + memcpy(&n->address, addr, sizeof(struct pico_ip6)); + n->dev = dev; + pico_tree_insert(&NCache, n); + return n; +} + +static void pico_ipv6_nd_unreachable(struct pico_ip6 *a) +{ + int i; + struct pico_frame *f; + struct pico_ipv6_hdr *hdr; + struct pico_ip6 dst; + for (i = 0; i < PICO_ND_MAX_FRAMES_QUEUED; i++) + { + f = frames_queued_v6[i]; + if (f) { + hdr = (struct pico_ipv6_hdr *) f->net_hdr; + dst = pico_ipv6_route_get_gateway(&hdr->dst); + if (pico_ipv6_is_unspecified(dst.addr)) + dst = hdr->dst; + + if (memcmp(dst.addr, a->addr, PICO_SIZE_IP6) == 0) { + if (!pico_source_is_local(f)) { + pico_notify_dest_unreachable(f); + } + + pico_frame_discard(f); + frames_queued_v6[i] = NULL; + } + } + } +} + +static void pico_nd_new_expire_time(struct pico_ipv6_neighbor *n) +{ + if (n->state == PICO_ND_STATE_REACHABLE) + n->expire = PICO_TIME_MS() + PICO_ND_REACHABLE_TIME; + else if ((n->state == PICO_ND_STATE_DELAY) || (n->state == PICO_ND_STATE_STALE)) + n->expire = PICO_TIME_MS() + PICO_ND_DELAY_FIRST_PROBE_TIME; + else { + n->expire = n->dev->hostvars.retranstime + PICO_TIME_MS(); + } +} + +static void pico_nd_discover(struct pico_ipv6_neighbor *n) +{ + char IPADDR[64]; + if (n->expire != (pico_time)0) + return; + + pico_ipv6_to_string(IPADDR, n->address.addr); + /* dbg("Sending NS for %s\n", IPADDR); */ + if (++n->failure_count > PICO_ND_MAX_SOLICIT) + return; + + if (n->state == PICO_ND_STATE_INCOMPLETE) { + pico_icmp6_neighbor_solicitation(n->dev, &n->address, PICO_ICMP6_ND_SOLICITED); + } else { + pico_icmp6_neighbor_solicitation(n->dev, &n->address, PICO_ICMP6_ND_UNICAST); + } + + pico_nd_new_expire_time(n); +} + +static struct pico_eth *pico_nd_get_neighbor(struct pico_ip6 *addr, struct pico_ipv6_neighbor *n, struct pico_device *dev) +{ + /* dbg("Finding neighbor %02x:...:%02x, state = %d\n", addr->addr[0], addr->addr[15], n?n->state:-1); */ + + if (!n) { + n = pico_nd_add(addr, dev); + pico_nd_discover(n); + return NULL; + } + + if (n->state == PICO_ND_STATE_INCOMPLETE) { + return NULL; + } + + if (n->state == PICO_ND_STATE_STALE) { + n->state = PICO_ND_STATE_DELAY; + pico_nd_new_expire_time(n); + } + + if (n->state != PICO_ND_STATE_REACHABLE) + pico_nd_discover(n); + + return &n->mac; + +} + +static struct pico_eth *pico_nd_get(struct pico_ip6 *address, struct pico_device *dev) +{ + struct pico_ip6 gateway = {{0}}, addr = {{0}}; + + /* should we use gateway, or is dst local (gateway == 0)? */ + gateway = pico_ipv6_route_get_gateway(address); + if (memcmp(gateway.addr, PICO_IP6_ANY, PICO_SIZE_IP6) == 0) + addr = *address; + else + addr = gateway; + + return pico_nd_get_neighbor(&addr, pico_nd_find_neighbor(&addr), dev); +} + +static int neigh_options(struct pico_frame *f, struct pico_icmp6_opt_lladdr *opt, uint8_t expected_opt) +{ + /* RFC 4861 $7.1.2 + $7.2.5. + * * The contents of any defined options that are not specified to be used + * * with Neighbor Advertisement messages MUST be ignored and the packet + * * processed as normal. The only defined option that may appear is the + * * Target Link-Layer Address option. + * */ + int optlen = 0; + uint8_t *option = NULL; + struct pico_icmp6_hdr *icmp6_hdr = NULL; + int len; + uint8_t type; + int found = 0; + + icmp6_hdr = (struct pico_icmp6_hdr *)f->transport_hdr; + optlen = f->transport_len - PICO_ICMP6HDR_NEIGH_ADV_SIZE; + if (optlen) + option = ((uint8_t *)&icmp6_hdr->msg.info.neigh_adv) + sizeof(struct neigh_adv_s); + + while (optlen > 0) { + type = ((struct pico_icmp6_opt_lladdr *)option)->type; + len = ((struct pico_icmp6_opt_lladdr *)option)->len; + optlen -= len << 3; /* len in units of 8 octets */ + if (len <= 0) + return -1; /* malformed option. */ + + if (type == expected_opt) { + if (found > 0) + return -1; /* malformed option: option is there twice. */ + + memcpy(opt, (struct pico_icmp6_opt_lladdr *)option, (size_t)(len << 3)); + found++; + } + + if (optlen > 0) { + option += len << 3; + } else { /* parsing options: terminated. */ + return found; + } + } + return found; +} + +static void pico_ipv6_neighbor_update(struct pico_ipv6_neighbor *n, struct pico_icmp6_opt_lladdr *opt) +{ + memcpy(n->mac.addr, opt->addr.mac.addr, PICO_SIZE_ETH); +} + +static int pico_ipv6_neighbor_compare_stored(struct pico_ipv6_neighbor *n, struct pico_icmp6_opt_lladdr *opt) +{ + return memcmp(n->mac.addr, opt->addr.mac.addr, PICO_SIZE_ETH); +} + +static void neigh_adv_reconfirm_router_option(struct pico_ipv6_neighbor *n, unsigned int isRouter) +{ + if (!isRouter && n->is_router) { + pico_ipv6_router_down(&n->address); + } + + if (isRouter) + n->is_router = 1; + else + n->is_router = 0; +} + + +static int neigh_adv_reconfirm_no_tlla(struct pico_ipv6_neighbor *n, struct pico_icmp6_hdr *hdr) +{ + if (IS_SOLICITED(hdr)) { + n->state = PICO_ND_STATE_REACHABLE; + n->failure_count = 0; + pico_ipv6_nd_queued_trigger(); + pico_nd_new_expire_time(n); + return 0; + } + + return -1; +} + + +static int neigh_adv_reconfirm(struct pico_ipv6_neighbor *n, struct pico_icmp6_opt_lladdr *opt, struct pico_icmp6_hdr *hdr) +{ + + if (IS_SOLICITED(hdr) && !IS_OVERRIDE(hdr) && (pico_ipv6_neighbor_compare_stored(n, opt) == 0)) { + n->state = PICO_ND_STATE_REACHABLE; + n->failure_count = 0; + pico_ipv6_nd_queued_trigger(); + pico_nd_new_expire_time(n); + return 0; + } + + if ((n->state == PICO_ND_STATE_REACHABLE) && IS_SOLICITED(hdr) && !IS_OVERRIDE(hdr)) { + n->state = PICO_ND_STATE_STALE; + return 0; + } + + if (IS_SOLICITED(hdr) && IS_OVERRIDE(hdr)) { + pico_ipv6_neighbor_update(n, opt); + n->state = PICO_ND_STATE_REACHABLE; + n->failure_count = 0; + pico_ipv6_nd_queued_trigger(); + pico_nd_new_expire_time(n); + return 0; + } + + if (!IS_SOLICITED(hdr) && IS_OVERRIDE(hdr) && (pico_ipv6_neighbor_compare_stored(n, opt) != 0)) { + pico_ipv6_neighbor_update(n, opt); + n->state = PICO_ND_STATE_STALE; + pico_ipv6_nd_queued_trigger(); + pico_nd_new_expire_time(n); + return 0; + } + + if ((n->state == PICO_ND_STATE_REACHABLE) && (!IS_SOLICITED(hdr)) && (!IS_OVERRIDE(hdr)) && + (pico_ipv6_neighbor_compare_stored(n, opt) != 0)) { + + /* I. If the Override flag is clear and the supplied link-layer address + * differs from that in the cache, then one of two actions takes + * place: + * a. If the state of the entry is REACHABLE, set it to STALE, but + * do not update the entry in any other way. + * b. Otherwise, the received advertisement should be ignored and + * MUST NOT update the cache. + */ + n->state = PICO_ND_STATE_STALE; + pico_nd_new_expire_time(n); + return 0; + } + + return -1; +} + +static void neigh_adv_process_incomplete(struct pico_ipv6_neighbor *n, struct pico_frame *f, struct pico_icmp6_opt_lladdr *opt) +{ + struct pico_icmp6_hdr *icmp6_hdr = NULL; + if (!n || !f) + return; + + icmp6_hdr = (struct pico_icmp6_hdr *)f->transport_hdr; + + if (!icmp6_hdr) + return; + + if (IS_SOLICITED(icmp6_hdr)) { + n->state = PICO_ND_STATE_REACHABLE; + n->failure_count = 0; + pico_nd_new_expire_time(n); + } else { + n->state = PICO_ND_STATE_STALE; + } + + if (opt) + pico_ipv6_neighbor_update(n, opt); + + pico_ipv6_nd_queued_trigger(); +} + + +static int neigh_adv_process(struct pico_frame *f) +{ + struct pico_icmp6_hdr *icmp6_hdr = NULL; + struct pico_ipv6_neighbor *n = NULL; + struct pico_icmp6_opt_lladdr opt = { + 0 + }; + int optres = neigh_options(f, &opt, PICO_ND_OPT_LLADDR_TGT); + icmp6_hdr = (struct pico_icmp6_hdr *)f->transport_hdr; + + if (optres < 0) { /* Malformed packet: option field cannot be processed. */ + return -1; + } + + n = pico_nd_find_neighbor(&icmp6_hdr->msg.info.neigh_adv.target); + if (!n) { + return 0; + } + + if ((optres == 0) || IS_OVERRIDE(icmp6_hdr) || (pico_ipv6_neighbor_compare_stored(n, &opt) == 0)) { + neigh_adv_reconfirm_router_option(n, IS_ROUTER(icmp6_hdr)); + } + + if ((optres > 0) && (n->state == PICO_ND_STATE_INCOMPLETE)) { + neigh_adv_process_incomplete(n, f, &opt); + return 0; + } + + if (optres > 0) + return neigh_adv_reconfirm(n, &opt, icmp6_hdr); + else + return neigh_adv_reconfirm_no_tlla(n, icmp6_hdr); + +} + + + +static struct pico_ipv6_neighbor *pico_ipv6_neighbor_from_sol_new(struct pico_ip6 *ip, struct pico_icmp6_opt_lladdr *opt, struct pico_device *dev) +{ + struct pico_ipv6_neighbor *n = NULL; + n = pico_nd_add(ip, dev); + if (!n) + return NULL; + + memcpy(n->mac.addr, opt->addr.mac.addr, PICO_SIZE_ETH); + n->state = PICO_ND_STATE_STALE; + pico_ipv6_nd_queued_trigger(); + return n; +} + +static void pico_ipv6_neighbor_from_unsolicited(struct pico_frame *f) +{ + struct pico_ipv6_neighbor *n = NULL; + struct pico_icmp6_opt_lladdr opt = { + 0 + }; + struct pico_ipv6_hdr *ip = (struct pico_ipv6_hdr *)f->net_hdr; + int valid_lladdr = neigh_options(f, &opt, PICO_ND_OPT_LLADDR_SRC); + + if (!pico_ipv6_is_unspecified(ip->src.addr) && (valid_lladdr > 0)) { + n = pico_nd_find_neighbor(&ip->src); + if (!n) { + n = pico_ipv6_neighbor_from_sol_new(&ip->src, &opt, f->dev); + } else if (memcmp(opt.addr.mac.addr, n->mac.addr, PICO_SIZE_ETH)) { + pico_ipv6_neighbor_update(n, &opt); + n->state = PICO_ND_STATE_STALE; + pico_ipv6_nd_queued_trigger(); + pico_nd_new_expire_time(n); + } + + if (!n) + return; + } +} + +static int neigh_sol_detect_dad(struct pico_frame *f) +{ + struct pico_ipv6_hdr *ipv6_hdr = NULL; + struct pico_icmp6_hdr *icmp6_hdr = NULL; + struct pico_ipv6_link *link = NULL; + ipv6_hdr = (struct pico_ipv6_hdr *)f->net_hdr; + icmp6_hdr = (struct pico_icmp6_hdr *)f->transport_hdr; + link = pico_ipv6_link_istentative(&icmp6_hdr->msg.info.neigh_adv.target); + if (link) { + if (pico_ipv6_is_unicast(&ipv6_hdr->src)) + { + /* RFC4862 5.4.3 : sender is performing address resolution, + * our address is not yet valid, discard silently. + */ + dbg("DAD:Sender performing AR\n"); + } + + else if (pico_ipv6_is_unspecified(ipv6_hdr->src.addr) && + !pico_ipv6_is_allhosts_multicast(ipv6_hdr->dst.addr)) + { + /* RFC4862 5.4.3 : sender is performing DaD */ + dbg("DAD:Sender performing DaD\n"); + ipv6_duplicate_detected(link); + } + + return 0; + } + + return -1; /* Current link is not tentative */ +} + +static int neigh_sol_process(struct pico_frame *f) +{ + struct pico_icmp6_hdr *icmp6_hdr = NULL; + struct pico_ipv6_link *link = NULL; + int valid_lladdr; + struct pico_icmp6_opt_lladdr opt = { + 0 + }; + icmp6_hdr = (struct pico_icmp6_hdr *)f->transport_hdr; + + valid_lladdr = neigh_options(f, &opt, PICO_ND_OPT_LLADDR_SRC); + pico_ipv6_neighbor_from_unsolicited(f); + + if ((valid_lladdr == 0) && (neigh_sol_detect_dad(f) == 0)) + return 0; + + if (valid_lladdr < 0) + return -1; /* Malformed packet. */ + + link = pico_ipv6_link_get(&icmp6_hdr->msg.info.neigh_adv.target); + if (!link) { /* Not for us. */ + return -1; + } + + pico_icmp6_neighbor_advertisement(f, &icmp6_hdr->msg.info.neigh_adv.target); + return 0; +} + +static int icmp6_initial_checks(struct pico_frame *f) +{ + /* Common "step 0" validation */ + struct pico_ipv6_hdr *ipv6_hdr = NULL; + struct pico_icmp6_hdr *icmp6_hdr = NULL; + + ipv6_hdr = (struct pico_ipv6_hdr *)f->net_hdr; + icmp6_hdr = (struct pico_icmp6_hdr *)f->transport_hdr; + + /* RFC4861 - 7.1.2 : + * - The IP Hop Limit field has a value of 255, i.e., the packet + * could not possibly have been forwarded by a router. + * - ICMP Checksum is valid. + * - ICMP Code is 0. + */ + if (ipv6_hdr->hop != 255 || pico_icmp6_checksum(f) != 0 || icmp6_hdr->code != 0) + return -1; + + return 0; +} + +static int neigh_adv_option_len_validity_check(struct pico_frame *f) +{ + /* Step 4 validation */ + struct pico_icmp6_hdr *icmp6_hdr = NULL; + uint8_t *opt; + int optlen = f->transport_len - PICO_ICMP6HDR_NEIGH_ADV_SIZE; + /* RFC4861 - 7.1.2 : + * - All included options have a length that is greater than zero. + */ + icmp6_hdr = (struct pico_icmp6_hdr *)f->transport_hdr; + opt = ((uint8_t *)&icmp6_hdr->msg.info.neigh_adv) + sizeof(struct neigh_adv_s); + + while(optlen > 0) { + int opt_size = (opt[1] << 3); + if (opt_size == 0) + return -1; + + opt = opt + opt_size; + optlen -= opt_size; + } + return 0; +} + +static int neigh_adv_mcast_validity_check(struct pico_frame *f) +{ + /* Step 3 validation */ + struct pico_ipv6_hdr *ipv6_hdr = NULL; + struct pico_icmp6_hdr *icmp6_hdr = NULL; + /* RFC4861 - 7.1.2 : + * - If the IP Destination Address is a multicast address the + * Solicited flag is zero. + */ + ipv6_hdr = (struct pico_ipv6_hdr *)f->net_hdr; + icmp6_hdr = (struct pico_icmp6_hdr *)f->transport_hdr; + if (pico_ipv6_is_multicast(ipv6_hdr->dst.addr) && IS_SOLICITED(icmp6_hdr)) + return -1; + + return neigh_adv_option_len_validity_check(f); +} + +static int neigh_adv_validity_checks(struct pico_frame *f) +{ + /* Step 2 validation */ + /* RFC4861 - 7.1.2: + * - ICMP length (derived from the IP length) is 24 or more octets. + */ + if (f->transport_len < PICO_ICMP6HDR_NEIGH_ADV_SIZE) + return -1; + + return neigh_adv_mcast_validity_check(f); +} + + +static int neigh_sol_mcast_validity_check(struct pico_frame *f) +{ + struct pico_icmp6_hdr *icmp6_hdr = NULL; + icmp6_hdr = (struct pico_icmp6_hdr *)f->transport_hdr; + if (pico_ipv6_is_solnode_multicast(icmp6_hdr->msg.info.neigh_sol.target.addr, f->dev) == 0) + return -1; + + return 0; +} + +static int neigh_sol_unicast_validity_check(struct pico_frame *f) +{ + struct pico_ipv6_link *link; + struct pico_icmp6_hdr *icmp6_hdr = NULL; + + link = pico_ipv6_link_by_dev(f->dev); + icmp6_hdr = (struct pico_icmp6_hdr *)f->transport_hdr; + while(link) { + if (pico_ipv6_compare(&link->address, &icmp6_hdr->msg.info.neigh_sol.target) == 0) + return 0; + + link = pico_ipv6_link_by_dev_next(f->dev, link); + } + return -1; + +} + +static int neigh_sol_validate_unspec(struct pico_frame *f) +{ + /* RFC4861, 7.1.1: + * + * - If the IP source address is the unspecified address, the IP + * destination address is a solicited-node multicast address. + * + * - If the IP source address is the unspecified address, there is no + * source link-layer address option in the message. + * + */ + + struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)(f->net_hdr); + struct pico_icmp6_opt_lladdr opt = { + 0 + }; + int valid_lladdr = neigh_options(f, &opt, PICO_ND_OPT_LLADDR_SRC); + if (pico_ipv6_is_solnode_multicast(hdr->dst.addr, f->dev) == 0) { + return -1; + } + + if (valid_lladdr) { + return -1; + } + + return 0; +} + +static int neigh_sol_validity_checks(struct pico_frame *f) +{ + /* Step 2 validation */ + struct pico_icmp6_hdr *icmp6_hdr = NULL; + struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)(f->net_hdr); + if (f->transport_len < PICO_ICMP6HDR_NEIGH_ADV_SIZE) + return -1; + + if ((pico_ipv6_is_unspecified(hdr->src.addr)) && (neigh_sol_validate_unspec(f) < 0)) + { + return -1; + } + + icmp6_hdr = (struct pico_icmp6_hdr *)f->transport_hdr; + if (pico_ipv6_is_multicast(icmp6_hdr->msg.info.neigh_adv.target.addr)) { + return neigh_sol_mcast_validity_check(f); + } + + return neigh_sol_unicast_validity_check(f); +} + +static int router_adv_validity_checks(struct pico_frame *f) +{ + /* Step 2 validation */ + if (f->transport_len < PICO_ICMP6HDR_ROUTER_ADV_SIZE) + return -1; + + return 0; +} + +static int neigh_adv_checks(struct pico_frame *f) +{ + /* Step 1 validation */ + if (icmp6_initial_checks(f) < 0) + return -1; + + return neigh_adv_validity_checks(f); +} + + +static int pico_nd_router_sol_recv(struct pico_frame *f) +{ + pico_ipv6_neighbor_from_unsolicited(f); + /* Host only: router solicitation is discarded. */ + return 0; +} + +static int radv_process(struct pico_frame *f) +{ + struct pico_icmp6_hdr *icmp6_hdr = NULL; + uint8_t *nxtopt, *opt_start; + struct pico_ipv6_link *link; + struct pico_ipv6_hdr *hdr; + struct pico_ip6 zero = { + .addr = {0} + }; + int optlen; + + hdr = (struct pico_ipv6_hdr *)f->net_hdr; + icmp6_hdr = (struct pico_icmp6_hdr *)f->transport_hdr; + optlen = f->transport_len - PICO_ICMP6HDR_ROUTER_ADV_SIZE; + opt_start = ((uint8_t *)&icmp6_hdr->msg.info.router_adv) + sizeof(struct router_adv_s); + nxtopt = opt_start; + + while (optlen > 0) { + uint8_t *type = (uint8_t *)nxtopt; + switch (*type) { + case PICO_ND_OPT_PREFIX: + { + pico_time now = PICO_TIME_MS(); + struct pico_icmp6_opt_prefix *prefix = + (struct pico_icmp6_opt_prefix *) nxtopt; + /* RFC4862 5.5.3 */ + /* a) If the Autonomous flag is not set, silently ignore the Prefix + * Information option. + */ + if (prefix->aac == 0) + goto ignore_opt_prefix; + + /* b) If the prefix is the link-local prefix, silently ignore the + * Prefix Information option + */ + if (pico_ipv6_is_linklocal(prefix->prefix.addr)) + goto ignore_opt_prefix; + + /* c) If the preferred lifetime is greater than the valid lifetime, + * silently ignore the Prefix Information option + */ + if (long_be(prefix->pref_lifetime) > long_be(prefix->val_lifetime)) + goto ignore_opt_prefix; + + if (prefix->val_lifetime == 0) + goto ignore_opt_prefix; + + + if (prefix->prefix_len != 64) { + return -1; + } + + link = pico_ipv6_prefix_configured(&prefix->prefix); + if (link) { + pico_ipv6_lifetime_set(link, now + (pico_time)(1000 * (long_be(prefix->val_lifetime)))); + goto ignore_opt_prefix; + } + + link = pico_ipv6_link_add_local(f->dev, &prefix->prefix); + if (link) { + pico_ipv6_lifetime_set(link, now + (pico_time)(1000 * (long_be(prefix->val_lifetime)))); + pico_ipv6_route_add(zero, zero, hdr->src, 10, link); + } + +ignore_opt_prefix: + optlen -= (prefix->len << 3); + nxtopt += (prefix->len << 3); + } + break; + case PICO_ND_OPT_LLADDR_SRC: + { + struct pico_icmp6_opt_lladdr *lladdr_src = + (struct pico_icmp6_opt_lladdr *) nxtopt; + optlen -= (lladdr_src->len << 3); + nxtopt += (lladdr_src->len << 3); + } + break; + case PICO_ND_OPT_MTU: + { + struct pico_icmp6_opt_mtu *mtu = + (struct pico_icmp6_opt_mtu *) nxtopt; + /* Skip this */ + optlen -= (mtu->len << 3); + nxtopt += (mtu->len << 3); + } + break; + case PICO_ND_OPT_REDIRECT: + { + struct pico_icmp6_opt_redirect *redirect = + (struct pico_icmp6_opt_redirect *) nxtopt; + /* Skip this */ + optlen -= (redirect->len << 3); + nxtopt += (redirect->len << 3); + + } + break; + case PICO_ND_OPT_RDNSS: + { + struct pico_icmp6_opt_rdnss *rdnss = + (struct pico_icmp6_opt_rdnss *) nxtopt; + /* Skip this */ + optlen -= (rdnss->len << 3); + nxtopt += (rdnss->len << 3); + } + break; + default: + pico_icmp6_parameter_problem(f, PICO_ICMP6_PARAMPROB_IPV6OPT, + (uint32_t)sizeof(struct pico_ipv6_hdr) + (uint32_t)PICO_ICMP6HDR_ROUTER_ADV_SIZE + (uint32_t)(nxtopt - opt_start)); + return -1; + } + } + if (icmp6_hdr->msg.info.router_adv.retrans_time != 0u) { + f->dev->hostvars.retranstime = long_be(icmp6_hdr->msg.info.router_adv.retrans_time); + } + + return 0; +} + + +static int pico_nd_router_adv_recv(struct pico_frame *f) +{ + if (icmp6_initial_checks(f) < 0) + return -1; + + if (router_adv_validity_checks(f) < 0) + return -1; + + pico_ipv6_neighbor_from_unsolicited(f); + return radv_process(f); +} + +static int pico_nd_neigh_sol_recv(struct pico_frame *f) +{ + if (icmp6_initial_checks(f) < 0) + return -1; + + if (neigh_sol_validity_checks(f) < 0) + return -1; + + return neigh_sol_process(f); +} + +static int pico_nd_neigh_adv_recv(struct pico_frame *f) +{ + struct pico_icmp6_hdr *icmp6_hdr = NULL; + struct pico_ipv6_link *link = NULL; + + icmp6_hdr = (struct pico_icmp6_hdr *)f->transport_hdr; + if (neigh_adv_checks(f) < 0) { + return -1; + } + + link = pico_ipv6_link_istentative(&icmp6_hdr->msg.info.neigh_adv.target); + if (link) + ipv6_duplicate_detected(link); + + return neigh_adv_process(f); +} + +static int pico_nd_redirect_recv(struct pico_frame *f) +{ + pico_ipv6_neighbor_from_unsolicited(f); + /* TODO */ + return 0; +} + +static void pico_ipv6_nd_timer_elapsed(pico_time now, struct pico_ipv6_neighbor *n) +{ + (void)now; + switch(n->state) { + case PICO_ND_STATE_INCOMPLETE: + /* intentional fall through */ + case PICO_ND_STATE_PROBE: + if (n->failure_count > PICO_ND_MAX_SOLICIT) { + pico_ipv6_nd_unreachable(&n->address); + pico_tree_delete(&NCache, n); + PICO_FREE(n); + return; + } + + n->expire = 0ull; + pico_nd_discover(n); + break; + + case PICO_ND_STATE_REACHABLE: + n->state = PICO_ND_STATE_STALE; + /* dbg("IPv6_ND: neighbor expired!\n"); */ + return; + + case PICO_ND_STATE_STALE: + break; + + case PICO_ND_STATE_DELAY: + n->expire = 0ull; + n->state = PICO_ND_STATE_PROBE; + break; + default: + dbg("IPv6_ND: neighbor in wrong state!\n"); + } + pico_nd_new_expire_time(n); +} + +static void pico_ipv6_nd_timer_callback(pico_time now, void *arg) +{ + struct pico_tree_node *index = NULL, *_tmp = NULL; + struct pico_ipv6_neighbor *n; + + (void)arg; + pico_tree_foreach_safe(index, &NCache, _tmp) + { + n = index->keyValue; + if ( now > n->expire) { + pico_ipv6_nd_timer_elapsed(now, n); + } + } + pico_timer_add(200, pico_ipv6_nd_timer_callback, NULL); +} + +#define PICO_IPV6_ND_MIN_RADV_INTERVAL (5000) +#define PICO_IPV6_ND_MAX_RADV_INTERVAL (15000) + +static void pico_ipv6_nd_ra_timer_callback(pico_time now, void *arg) +{ + struct pico_tree_node *devindex = NULL; + struct pico_tree_node *rindex = NULL; + struct pico_device *dev; + struct pico_ipv6_route *rt; + struct pico_ip6 nm64 = { {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0, 0, 0, 0, 0, 0, 0, 0 } }; + pico_time next_timer_expire = 0u; + + (void)arg; + (void)now; + pico_tree_foreach(rindex, &IPV6Routes) + { + rt = rindex->keyValue; + if (pico_ipv6_compare(&nm64, &rt->netmask) == 0) { + pico_tree_foreach(devindex, &Device_tree) { + dev = devindex->keyValue; + if ((!pico_ipv6_is_linklocal(rt->dest.addr)) && dev->hostvars.routing && (rt->link) && (dev != rt->link->dev)) { + pico_icmp6_router_advertisement(dev, &rt->dest); + } + } + } + } + next_timer_expire = PICO_IPV6_ND_MIN_RADV_INTERVAL + (pico_rand() % (PICO_IPV6_ND_MAX_RADV_INTERVAL - PICO_IPV6_ND_MIN_RADV_INTERVAL)); + pico_timer_add(next_timer_expire, pico_ipv6_nd_ra_timer_callback, NULL); +} + +/* Public API */ + +struct pico_eth *pico_ipv6_get_neighbor(struct pico_frame *f) +{ + struct pico_ipv6_hdr *hdr = NULL; + struct pico_ipv6_link *l = NULL; + if (!f) + return NULL; + + hdr = (struct pico_ipv6_hdr *)f->net_hdr; + /* If we are still probing for Duplicate Address, abort now. */ + if (pico_ipv6_link_istentative(&hdr->src)) + return NULL; + + /* address belongs to ourselves? */ + l = pico_ipv6_link_get(&hdr->dst); + if (l) + return &l->dev->eth->mac; + + return pico_nd_get(&hdr->dst, f->dev); +} + +void pico_ipv6_nd_postpone(struct pico_frame *f) +{ + int i; + static int last_enq = -1; + struct pico_frame *cp = pico_frame_copy(f); + for (i = 0; i < PICO_ND_MAX_FRAMES_QUEUED; i++) + { + if (!frames_queued_v6[i]) { + frames_queued_v6[i] = cp; + last_enq = i; + return; + } + } + /* Overwrite the oldest frame in the buffer */ + if (++last_enq >= PICO_ND_MAX_FRAMES_QUEUED) { + last_enq = 0; + } + + if (frames_queued_v6[last_enq]) + pico_frame_discard(frames_queued_v6[last_enq]); + + frames_queued_v6[last_enq] = cp; +} + + +int pico_ipv6_nd_recv(struct pico_frame *f) +{ + + struct pico_icmp6_hdr *hdr = (struct pico_icmp6_hdr *)f->transport_hdr; + int ret = -1; + switch(hdr->type) { + case PICO_ICMP6_ROUTER_SOL: + nd_dbg("ICMP6: received ROUTER SOL\n"); + ret = pico_nd_router_sol_recv(f); + break; + + case PICO_ICMP6_ROUTER_ADV: + ret = pico_nd_router_adv_recv(f); + break; + + case PICO_ICMP6_NEIGH_SOL: + nd_dbg("ICMP6: received NEIGH SOL\n"); + ret = pico_nd_neigh_sol_recv(f); + break; + + case PICO_ICMP6_NEIGH_ADV: + nd_dbg("ICMP6: received NEIGH ADV\n"); + ret = pico_nd_neigh_adv_recv(f); + break; + + case PICO_ICMP6_REDIRECT: + ret = pico_nd_redirect_recv(f); + break; + } + pico_frame_discard(f); + return ret; +} + +void pico_ipv6_nd_init(void) +{ + pico_timer_add(200, pico_ipv6_nd_timer_callback, NULL); + pico_timer_add(200, pico_ipv6_nd_ra_timer_callback, NULL); + pico_timer_add(1000, pico_ipv6_check_lifetime_expired, NULL); +} + +#endif diff --git a/ext/picotcp/modules/pico_ipv6_nd.h b/ext/picotcp/modules/pico_ipv6_nd.h new file mode 100644 index 0000000..f709b1e --- /dev/null +++ b/ext/picotcp/modules/pico_ipv6_nd.h @@ -0,0 +1,26 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + *********************************************************************/ +#ifndef _INCLUDE_PICO_ND +#define _INCLUDE_PICO_ND +#include "pico_frame.h" + +/* RFC constants */ +#define PICO_ND_REACHABLE_TIME 30000 /* msec */ +#define PICO_ND_RETRANS_TIMER 1000 /* msec */ + +struct pico_nd_hostvars { + uint8_t routing; + uint8_t hoplimit; + pico_time basetime; + pico_time reachabletime; + pico_time retranstime; +}; + +void pico_ipv6_nd_init(void); +struct pico_eth *pico_ipv6_get_neighbor(struct pico_frame *f); +void pico_ipv6_nd_postpone(struct pico_frame *f); +int pico_ipv6_nd_recv(struct pico_frame *f); +#endif diff --git a/ext/picotcp/modules/pico_mdns.c b/ext/picotcp/modules/pico_mdns.c new file mode 100644 index 0000000..6b5c5e9 --- /dev/null +++ b/ext/picotcp/modules/pico_mdns.c @@ -0,0 +1,3417 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2014-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + . + Author: Toon Stegen, Jelle De Vleeschouwer + *********************************************************************/ +#include "pico_config.h" +#include "pico_stack.h" +#include "pico_addressing.h" +#include "pico_socket.h" +#include "pico_ipv4.h" +#include "pico_ipv6.h" +#include "pico_tree.h" +#include "pico_mdns.h" + +#ifdef PICO_SUPPORT_MDNS + +/* --- Debugging --- */ +#define mdns_dbg(...) do {} while(0) +//#define mdns_dbg dbg + +#define PICO_MDNS_QUERY_TIMEOUT (10000) /* Ten seconds */ +#define PICO_MDNS_RR_TTL_TICK (1000) /* One second */ + +/* mDNS MTU size */ +#define PICO_MDNS_MAXBUF (1400u) + +/* --- Cookie flags --- */ +#define PICO_MDNS_PACKET_TYPE_ANNOUNCEMENT (0x01u) +#define PICO_MDNS_PACKET_TYPE_ANSWER (0x02u) +#define PICO_MDNS_PACKET_TYPE_QUERY (0x04u) +#define PICO_MDNS_PACKET_TYPE_PROBE (0x08u) +#define PICO_MDNS_PACKET_TYPE_QUERY_ANY (0x00u) +/* --- Cookie status --- */ +#define PICO_MDNS_COOKIE_STATUS_ACTIVE (0xffu) +#define PICO_MDNS_COOKIE_STATUS_INACTIVE (0x00u) +#define PICO_MDNS_COOKIE_STATUS_CANCELLED (0x77u) +#define PICO_MDNS_COOKIE_TIMEOUT (10u) + +#define PICO_MDNS_SECTION_ANSWERS (0) +#define PICO_MDNS_SECTION_AUTHORITIES (1) +#define PICO_MDNS_SETCTIO_ADDITIONALS (2) + +/* --- Question flags --- */ +#define PICO_MDNS_QUESTION_FLAG_PROBE (0x01u) +#define PICO_MDNS_QUESTION_FLAG_NO_PROBE (0x00u) +#define PICO_MDNS_QUESTION_FLAG_UNICAST_RES (0x02u) +#define PICO_MDNS_QUESTION_FLAG_MULTICAST_RES (0x00u) + +#define IS_QUESTION_PROBE_FLAG_SET(x) \ + (((x) & PICO_MDNS_QUESTION_FLAG_PROBE) ? (1) : (0)) +#define IS_QUESTION_UNICAST_FLAG_SET(x) \ + (((x) & PICO_MDNS_QUESTION_FLAG_UNICAST_RES) ? (1) : (0)) +#define IS_QUESTION_MULTICAST_FLAG_SET(x) \ + (((x) & PICO_MDNS_QUESTION_FLAG_UNICAST_RES) ? (0) : (1)) + +/* Resource Record flags */ +#define PICO_MDNS_RECORD_ADDITIONAL (0x08u) +#define PICO_MDNS_RECORD_SEND_UNICAST (0x10u) +#define PICO_MDNS_RECORD_CURRENTLY_PROBING (0x20u) +#define PICO_MDNS_RECORD_PROBED (0x40u) +#define PICO_MDNS_RECORD_CLAIMED (0x80u) + +#define IS_SHARED_RECORD(x) \ + ((x)->flags & PICO_MDNS_RECORD_SHARED) +#define IS_UNIQUE_RECORD(x) \ + (!((x)->flags & PICO_MDNS_RECORD_SHARED)) +#define IS_RECORD_PROBING(x) \ + ((x)->flags & PICO_MDNS_RECORD_CURRENTLY_PROBING) +#define IS_UNICAST_REQUESTED(x) \ + ((x)->flags & PICO_MDNS_RECORD_SEND_UNICAST) +#define IS_RECORD_VERIFIED(x) \ + ((x)->flags & PICO_MDNS_RECORD_PROBED) +#define IS_RECORD_CLAIMED(x) \ + ((x)->flags & PICO_MDNS_RECORD_CLAIMED) + +/* Set and clear flags */ +#define PICO_MDNS_SET_FLAG(x, b) (x = ((x) | (uint8_t)(b))) +#define PICO_MDNS_CLR_FLAG(x, b) (x = (uint8_t)(((x) & (~((uint8_t)(b)))))) + +/* Set and clear MSB of BE short */ +#define PICO_MDNS_SET_MSB(x) (x = x | (uint16_t)(0x8000u)) +#define PICO_MDNS_CLR_MSB(x) (x = x & (uint16_t)(0x7fffu)) +#define PICO_MDNS_SET_MSB_BE(x) (x = x | (uint16_t)(short_be(0x8000u))) +#define PICO_MDNS_CLR_MSB_BE(x) (x = x & (uint16_t)(short_be(0x7fffu))) +#define PICO_MDNS_IS_MSB_SET(x) ((x & 0x8000u) ? 1 : 0) + +/* **************************************************************************** + * mDNS cookie + * ****************************************************************************/ +struct pico_mdns_cookie +{ + pico_dns_qtree qtree; /* Question tree */ + pico_mdns_rtree antree; /* Answer tree */ + pico_mdns_rtree artree; /* Additional record tree */ + uint8_t count; /* Times to send the query */ + uint8_t type; /* QUERY/ANNOUNCE/PROBE/ANSWER */ + uint8_t status; /* Active status */ + uint8_t timeout; /* Timeout counter */ + uint32_t send_timer; /* For sending events */ + void (*callback)(pico_mdns_rtree *, + char *, + void *); /* Callback */ + void *arg; /* Argument to pass to callback */ +}; + +/* MARK: TREES & GLOBAL VARIABLES */ + +/* MDNS Communication variables */ +static struct pico_socket *mdns_sock_ipv4 = NULL; +static uint16_t mdns_port = 5353u; +static struct pico_ip4 inaddr_any = { + 0 +}; + +/* **************************************************************************** + * Hostname for this machine, only 1 hostname can be set. + * ****************************************************************************/ +static char *_hostname = NULL; +static void (*init_callback)(pico_mdns_rtree *, char *, void *) = 0; + +/* **************************************************************************** + * Compares 2 mDNS records by name and type only + * + * @param a mDNS record A + * @param b mDNS record B + * @return 0 when name and type of records are equal, returns difference when + * they're not. + * ****************************************************************************/ +static int +pico_mdns_record_cmp_name_type( void *a, void *b ) +{ + struct pico_mdns_record *_a = NULL, *_b = NULL; + + /* Check params */ + if (!(_a = (struct pico_mdns_record *)a) || + !(_b = (struct pico_mdns_record *)b)) { + pico_err = PICO_ERR_EINVAL; + return -1; /* Don't want a wrong result when NULL-pointers are passed */ + } + + return pico_dns_record_cmp_name_type(_a->record, _b->record); +} + +/* **************************************************************************** + * Compares 2 mDNS records by type, name AND rdata for a truly unique result + * + * @param ra mDNS record A + * @param rb mDNS record B + * @return 0 when records are equal, returns difference when they're not. + * ****************************************************************************/ +int +pico_mdns_record_cmp( void *a, void *b ) +{ + /* Check params */ + if (!a || !b) { + if (!a && !b) + return 0; + pico_err = PICO_ERR_EINVAL; + return -1; /* Don't want a wrong result when NULL-pointers are passed */ + } + + return pico_dns_record_cmp((void*)(((struct pico_mdns_record *)a)->record), + (void*)(((struct pico_mdns_record *)b)->record)); +} + +/* **************************************************************************** + * Compares 2 mDNS cookies again each other. Only compares questions since a + * only a cookie query will be added to the tree. And there shouldn't be 2 + * different cookies with the same questions in the tree. + * + * @param ka mDNS cookie A + * @param kb mDNS cookie B + * @return 0 when cookies are equal, returns difference when they're not. + * ****************************************************************************/ +static int +pico_mdns_cookie_cmp( void *ka, void *kb ) +{ + struct pico_mdns_cookie *a = (struct pico_mdns_cookie *)ka; + struct pico_mdns_cookie *b = (struct pico_mdns_cookie *)kb; + struct pico_dns_question *qa = NULL, *qb = 0; + struct pico_tree_node *na = NULL, *nb = 0; + uint16_t ca = 0, cb = 0; + int ret = 0; + + /* Check params */ + if (!a || !b) { + pico_err = PICO_ERR_EINVAL; + return -1; /* Don't want a wrong result when NULL-pointers are passed */ + } + + /* Start comparing the questions */ + for (na = pico_tree_firstNode(a->qtree.root), + nb = pico_tree_firstNode(b->qtree.root); + (na != &LEAF) && (nb != &LEAF); + na = pico_tree_next(na), + nb = pico_tree_next(nb)) { + qa = na->keyValue; + qb = nb->keyValue; + if ((qa) && (qb) && (ret = pico_dns_question_cmp(qa, qb))) + return ret; + } + /* Check for lengths difference */ + ca = pico_tree_count(&(a->qtree)); + cb = pico_tree_count(&(b->qtree)); + if (ca != cb) + return (int)((int)ca - (int)cb); + + /* Cookies contain same questions, shouldn't happen */ + return 0; +} + +#if PICO_MDNS_ALLOW_CACHING == 1 +/* Cache records from mDNS peers on the network */ +PICO_TREE_DECLARE(Cache, &pico_mdns_record_cmp); +#endif + +/* My records for which I want to have the authority */ +PICO_TREE_DECLARE(MyRecords, &pico_mdns_record_cmp_name_type); + +/* Cookie-tree */ +PICO_TREE_DECLARE(Cookies, &pico_mdns_cookie_cmp); + +/* **************************************************************************** + * MARK: PROTOTYPES */ +static int +pico_mdns_getrecord_generic( const char *url, uint16_t type, + void (*callback)(pico_mdns_rtree *, + char *, + void *), + void *arg); + +static void +pico_mdns_send_probe_packet( pico_time now, void *arg ); + +static int +pico_mdns_reclaim( pico_mdns_rtree record_tree, + void (*callback)(pico_mdns_rtree *, + char *, + void *), + void *arg ); +/* EOF PROTOTYPES + * ****************************************************************************/ + +/* MARK: v MDNS NAMES */ + +#define IS_NUM(c) (((c) >= '0') && ((c) <= '9')) +/* **************************************************************************** + * Tries to convert the characters after '-' to a numeric value. + * + * @param opening Pointer to dash index. + * @param closing Pointer to end of label. + * @return Numeric value of suffix on success + * ****************************************************************************/ +static inline uint16_t +pico_mdns_suffix_to_uint16( char *opening, char *closing) +{ + uint16_t n = 0; + char *i = 0; + + /* Check params */ + if (!opening || !closing || + ((closing - opening) > 5) || + ((closing - opening) < 0)) + return 0; + + for (i = (char *)(opening + 1); i < closing; i++) { + if (!IS_NUM(*i)) + return 0; + + n = (uint16_t)((n * 10) + (*i - '0')); + } + return n; +} + +#define iterate_first_label_name_reverse(iterator, name) \ + for ((iterator) = \ + (*name < (char)63) ? ((char *)(name + *name)) : (name); \ + (iterator) > (name); \ + (iterator)--) + +/* **************************************************************************** + * Checks whether there is already a conflict-suffix already present in the + * first label of a name or not. + * + * @param name Name in DNS name notation you want to check for a suffix. + * @param o_i Pointer-pointer, will get filled with location to '-'-char. + * @param c_i Pointer-pointer, will get filled with end of label. + * @return Returns value of the suffix, when it's present, 0 when no correct + * suffix is present. + * ****************************************************************************/ +static uint16_t +pico_mdns_is_suffix_present( char name[], + char **o_i, + char **c_i ) +{ + char *i = NULL; + uint16_t n = 0; + + *o_i = NULL; /* Clear out indexes */ + *c_i = NULL; + + /* Find the end of label. */ + *c_i = (name + *name + 1); + + iterate_first_label_name_reverse(i, name) { + /* Find the last dash */ + if ((*c_i) && (i < *c_i) && *i == '-') { + *o_i = i; + break; + } + } + + /* Convert the string suffix to a number */ + if (!(n = pico_mdns_suffix_to_uint16(*o_i, *c_i))) { + *o_i = NULL; + *c_i = NULL; + } + + return n; +} + +/* **************************************************************************** + * Manual string to uint16_t conversion. + * + * @param n Numeric value you want to convert. + * @param s String to convert to + * @return void + * ****************************************************************************/ +static void pico_itoa( uint16_t n, char s[] ) +{ + int i = 0, j = 0; + char c = 0; + + /* Get char values */ + do { + s[i++] = (char)(n % 10 + '0'); + } while ((n /= 10) > 0); + + /* Reverse the string */ + for (i = 0, j = (int)(pico_dns_strlen(s) - 1); i < j; i++, j--) { + c = s[i]; + s[i] = s[j]; + s[j] = c; + } +} + +/* **************************************************************************** + * Generates a new name by appending a conflict resolution suffix to the first + * label of an FQDN. + * + * @param rname Name you want to append the suffix to + * @return Newly created FQDN with suffix appended to first label. + * ****************************************************************************/ +static char * +pico_mdns_resolve_name_conflict( char rname[] ) +{ + char *new_rname = NULL; + char suffix[5] = { + 0 + }, nsuffix[5] = { + 0 + }, copy_offset = 0; + char *o_i = NULL, *c_i = NULL; + uint16_t new_len = (uint16_t)(pico_dns_strlen(rname) + 1); + uint8_t nslen = 0, slen = 0, ns = 0; + + /* Check params */ + if (pico_dns_check_namelen(new_len)) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + /* Check whether a conflict-suffix is already present in the name */ + if ((ns = (uint8_t)pico_mdns_is_suffix_present(rname, &o_i, &c_i))) { + pico_itoa(ns, suffix); + pico_itoa(++ns, nsuffix); + slen = (uint8_t)pico_dns_strlen(suffix); + nslen = (uint8_t)pico_dns_strlen(nsuffix); + new_len = (uint16_t)(new_len + nslen - slen); + } else { + /* If no suffix is present */ + c_i = (o_i = rname + *rname) + 1; + new_len = (uint16_t)(new_len + 2u); + memcpy((void *)nsuffix, "-2\0", (size_t)3); + } + + /* Provide space for the new name */ + if (!(new_rname = PICO_ZALLOC(new_len))) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + /* Assemble the new name again */ + copy_offset = (char)((o_i - rname + 1)); + memcpy(new_rname, rname, (size_t)(copy_offset)); + strcpy(new_rname + copy_offset, nsuffix); + strcpy(new_rname + copy_offset + pico_dns_strlen(nsuffix), c_i); + /* Set the first length-byte */ + new_rname[0] = (char)(new_rname[0] + new_len - pico_dns_strlen(rname) - 1); + return new_rname; +} + +/* MARK: ^ MDNS NAMES */ +/* MARK: v MDNS QUESTIONS */ + +/* **************************************************************************** + * Creates a standalone mDNS Question with a given name and type. + * + * @param url DNS question name in URL format. Will be converted to DNS + * name notation format. + * @param len Will be filled with the total length of the DNS question. + * @param proto Protocol for which you want to create a question. Can be + * either PICO_PROTO_IPV4 or PICO_PROTO_IPV6. + * @param qtype DNS type of the question to be. + * @param flags With the flags you can specify if the question should be + * a QU-question rather than a QM-question + * @param reverse When this is true, a reverse resolution name will be gene- + * from the URL + * @return Returns pointer to the created mDNS Question on success, NULL on + * failure. + * ****************************************************************************/ +static struct pico_dns_question * +pico_mdns_question_create( const char *url, + uint16_t *len, + uint8_t proto, + uint16_t qtype, + uint8_t flags, + uint8_t reverse ) +{ + uint16_t qclass = PICO_DNS_CLASS_IN; + + /* Set the MSB of the qclass field according to the mDNS format */ + if (IS_QUESTION_UNICAST_FLAG_SET(flags)) + PICO_MDNS_SET_MSB(qclass); + + /* Fill in the question suffix */ + if (IS_QUESTION_PROBE_FLAG_SET(flags)) + qtype = PICO_DNS_TYPE_ANY; + + /* Create a question as you would with plain DNS */ + return pico_dns_question_create(url, len, proto, qtype, qclass, reverse); +} + +/* MARK: ^ MDNS QUESTIONS */ +/* MARK: v MDNS RECORDS */ + +/* **************************************************************************** + * Just makes a hardcopy from a single mDNS resource record. + * + * @param record mDNS record you want to create a copy from + * @return Pointer to copied mDNS resource record + * ****************************************************************************/ +static struct pico_mdns_record * +pico_mdns_record_copy( struct pico_mdns_record *record ) +{ + struct pico_mdns_record *copy = NULL; + + /* Check params */ + if (!record) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + /* Provide space for the copy */ + if (!(copy = PICO_ZALLOC(sizeof(struct pico_mdns_record)))) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + /* Copy the DNS record */ + if (!(copy->record = pico_dns_record_copy(record->record))) { + PICO_FREE(copy); + return NULL; + } + + /* Copy the fields */ + copy->current_ttl = record->current_ttl; + copy->flags = record->flags; + copy->claim_id = record->claim_id; + + return copy; +} + +/* **************************************************************************** + * Looks for multiple mDNS records in a tree with the same name. + * + * @param tree Tree in which you want to search. + * @param name Name you want to search for. + * @return Tree with found hits, can possibly be empty + * ****************************************************************************/ +static pico_mdns_rtree +pico_mdns_rtree_find_name( pico_mdns_rtree *tree, + const char *name, + uint8_t copy ) +{ + PICO_MDNS_RTREE_DECLARE(hits); + struct pico_tree_node *node = NULL; + struct pico_mdns_record *record = NULL; + + /* Check params */ + if (!name || !tree) { + pico_err = PICO_ERR_EINVAL; + return hits; + } + + /* Iterate over tree */ + pico_tree_foreach(node, tree) { + record = node->keyValue; + if (record && strcasecmp(record->record->rname, name) == 0) { + if (copy) + record = pico_mdns_record_copy(record); + if (record) + pico_tree_insert(&hits, record); + } + } + + return hits; +} + +/* **************************************************************************** + * Looks for (possibly) multiple mDNS records in a tree with the same name and + * type. + * + * @param tree Tree in which you want to search. + * @param name Name you want to search for. + * @param rtype DNS type you want to search for. + * @return Tree with found hits, can possibly be empty. + * ****************************************************************************/ +static pico_mdns_rtree +pico_mdns_rtree_find_name_type( pico_mdns_rtree *tree, + char *name, + uint16_t rtype, + uint8_t copy ) +{ + PICO_MDNS_RTREE_DECLARE(hits); + + struct pico_dns_record_suffix test_dns_suffix = { 0, 1, 0, 0 }; + struct pico_dns_record test_dns_record = { 0 }; + struct pico_mdns_record test = { 0 }; + struct pico_tree_node *node = NULL; + struct pico_mdns_record *record = NULL; + test_dns_record.rsuffix = &test_dns_suffix; + test.record = &test_dns_record; + + /* Check params */ + if (!name || !tree) { + pico_err = PICO_ERR_EINVAL; + return hits; + } + + test.record->rname = name; + test.record->rsuffix->rtype = short_be(rtype); + + /* Iterate over the tree */ + pico_tree_foreach(node, tree) { + record = node->keyValue; + if ((record) && (0 == pico_mdns_record_cmp_name_type(record, &test))) { + if (copy) + record = pico_mdns_record_copy(record); + if (record) + pico_tree_insert(&hits, record); + } + } + + return hits; +} + +/* **************************************************************************** + * Deletes multiple mDNS records in a tree with the same name. + * + * @param tree Tree from which you want to delete records by name. + * @param name Name of records you want to delete from the tree. + * @return 0 on success, something else on failure. + * ****************************************************************************/ +static int +pico_mdns_rtree_del_name( pico_mdns_rtree *tree, + const char *name ) +{ + struct pico_tree_node *node = NULL, *safe = NULL; + struct pico_mdns_record *record = NULL; + + /* Check params */ + if (!name || !tree) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* Iterate over tree */ + pico_tree_foreach_safe(node, tree, safe) { + record = node->keyValue; + if (record && strcasecmp(record->record->rname, name) == 0) { + record = pico_tree_delete(tree, record); + pico_mdns_record_delete((void **)&record); + } + } + + return 0; +} + +/* **************************************************************************** + * Deletes (possibly) multiple mDNS records from a tree with same name and + * type. + * + * @param tree Tree from which you want to delete records by name and type. + * @param name Name of records you want to delete. + * @param type DNS type of records you want to delete. + * @return 0 on success, something else on failure. + * ****************************************************************************/ +#if PICO_MDNS_ALLOW_CACHING == 1 +static int +pico_mdns_rtree_del_name_type( pico_mdns_rtree *tree, + char *name, + uint16_t type ) +{ + struct pico_tree_node *node = NULL, *next = NULL; + struct pico_mdns_record *record = NULL; + struct pico_dns_record_suffix test_dns_suffix = { 0, 1, 0, 0 }; + struct pico_dns_record test_dns_record = { 0 }; + struct pico_mdns_record test = { 0 }; + + test_dns_record.rsuffix = &test_dns_suffix; + test.record = &test_dns_record; + + /* Check params */ + if (!name || !tree) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + test.record->rname = name; + test.record->rsuffix->rtype = short_be(type); + + /* Iterate over the tree */ + pico_tree_foreach_safe(node, tree, next) { + record = node->keyValue; + if ((record) && (0 == pico_mdns_record_cmp_name_type(record, &test))) { + record = pico_tree_delete(tree, record); + pico_mdns_record_delete((void **)&record); + } + } + + return 0; +} +#endif + +/* **************************************************************************** + * Makes a hardcopy from a single mDNS resource record, but sets a new name + * for the copy. + * + * @param record mDNS record you want to copy. + * @param new_rname New name you want to set the name of the record to. + * @return Pointer to the copy on success, NULL-pointer on failure. + * ****************************************************************************/ +static struct pico_mdns_record * +pico_mdns_record_copy_with_new_name( struct pico_mdns_record *record, + const char *new_rname ) +{ + struct pico_mdns_record *copy = NULL; + uint16_t slen = (uint16_t)(pico_dns_strlen(new_rname) + 1u); + + /* Check params */ + if (pico_dns_check_namelen(slen)) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + /* Copy the record */ + if (!(copy = pico_mdns_record_copy(record))) + return NULL; + + /* Provide a new string */ + PICO_FREE(copy->record->rname); + if (!(copy->record->rname = PICO_ZALLOC(slen))) { + pico_err = PICO_ERR_ENOMEM; + pico_mdns_record_delete((void **)©); + return NULL; + } + + memcpy((void *)(copy->record->rname), new_rname, slen); + copy->record->rname_length = slen; + + return copy; +} + +/* **************************************************************************** + * Generates (copies) new records from conflicting ones with another name. + * deletes + * + * @param conflict_records mDNS record tree that contains conflicting records + * @param conflict_name Name for which the conflict occurred. This is to be + * able to delete the conflicting records from the tree + * @param new_name To generate new records from the conflicting ones, + * with this new name. + * @return A mDNS record tree that contains all the newly generated records. + * ****************************************************************************/ +static pico_mdns_rtree +pico_mdns_generate_new_records( pico_mdns_rtree *conflict_records, + char *conflict_name, + char *new_name ) +{ + PICO_MDNS_RTREE_DECLARE(new_records); + struct pico_tree_node *node = NULL, *next = NULL; + struct pico_mdns_record *record = NULL, *new_record = NULL; + + /* Delete all the conflicting records from MyRecords */ + if (pico_mdns_rtree_del_name(&MyRecords, conflict_name)) + return new_records; + + pico_tree_foreach_safe(node, conflict_records, next) { + record = node->keyValue; + if (record && strcasecmp(record->record->rname, conflict_name) == 0) { + /* Create a new record */ + new_record = pico_mdns_record_copy_with_new_name(record, new_name); + if (!new_record) { + mdns_dbg("Could not create new non-conflicting record!\n"); + return new_records; + } + + new_record->flags &= (uint8_t)(~(PICO_MDNS_RECORD_PROBED | + PICO_MDNS_RECORD_SHARED | + PICO_MDNS_RECORD_CURRENTLY_PROBING)); + + /* Add the record to the new tree */ + pico_tree_insert(&new_records, new_record); + + /* Delete the old conflicting record */ + record = pico_tree_delete(conflict_records, record); + if (pico_mdns_record_delete((void **)&record)) { + mdns_dbg("Could not delete old conflict record from tree!\n"); + return new_records; + } + } + } + + return new_records; +} + +/* **************************************************************************** + * When hosts observe an unsolicited record, no cookie is currently active + * for that, so it has to check in MyRecords if no conflict occurred for a + * record it has already registered. When this occurs the conflict should be + * resolved as with a normal cookie, just without the cookie. + * + * @param record mDNS record for which the conflict occurred. + * @param rname DNS name for which the conflict occurred in DNS name notation. + * @return 0 when the resolving is applied successfully, 1 otherwise. + * ****************************************************************************/ +static int +pico_mdns_record_resolve_conflict( struct pico_mdns_record *record, + char *rname ) +{ + int retval; + PICO_MDNS_RTREE_DECLARE(new_records); + struct pico_mdns_record *copy = NULL; + char *new_name = NULL; + + /* Check params */ + if (!record || !rname || IS_SHARED_RECORD(record)) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* Step 2: Create a new name depending on current name */ + if (!(new_name = pico_mdns_resolve_name_conflict(rname))) + return -1; + + copy = pico_mdns_record_copy_with_new_name(record, new_name); + PICO_FREE(new_name); + if (copy) + pico_tree_insert(&new_records, copy); + + /* Step 3: delete conflicting record from my records */ + pico_tree_delete(&MyRecords, record); + pico_mdns_record_delete((void **)&record); + + /* Step 4: Try to reclaim the newly created records */ + retval = pico_mdns_reclaim(new_records, init_callback, NULL); + pico_tree_destroy(&new_records, NULL); + return retval; +} + +/* **************************************************************************** + * Determines if my_record is lexicographically later than peer_record, returns + * positive value when this is the case. Check happens by comparing rtype first + * and then rdata as prescribed by RFC6762. + * + * @param my_record Record this hosts want to claim. + * @param peer_record Record the peer host wants to claim (the enemy!) + * @return positive value when my record is lexicographically later + * ****************************************************************************/ +static int +pico_mdns_record_am_i_lexi_later( struct pico_mdns_record *my_record, + struct pico_mdns_record *peer_record) +{ + struct pico_dns_record *my = NULL, *peer = NULL; + uint16_t mclass = 0, pclass = 0, mtype = 0, ptype = 0; + int dif = 0; + + /* Check params */ + if (!my_record || !peer_record || + !(my = my_record->record) || !(peer = peer_record->record)) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* + * First compare the record class (excluding cache-flush bit described in + * section 10.2) + * The numerically greater class wins + */ + mclass = PICO_MDNS_CLR_MSB_BE(my->rsuffix->rclass); + pclass = PICO_MDNS_CLR_MSB_BE(peer->rsuffix->rclass); + if ((dif = (int)((int)mclass - (int)pclass))){ + return dif; + } + + /* Second, compare the rrtypes */ + mtype = (my->rsuffix->rtype); + ptype = (peer->rsuffix->rtype); + if ((dif = (int)((int)mtype - (int)ptype))){ + return dif; + } + + /* Third compare binary content of rdata (no regard for meaning or structure) */ + + /* When using name compression, names MUST be uncompressed before comparison. See secion 8.2 in RFC 6762 + This is already the case, but we won't check for it here. + The current execution stack to get here is: + > pico_mdns_handle_data_as_answers_generic + > > pico_dns_record_decompress + > > pico_mdns_handle_single_authority + > > > pico_mdns_cookie_apply_spt + > > > > pico_mdns_record_am_i_lexi_later + + Make sure pico_dns_record_decompress is executed before pico_mdns_record_am_i_lexi_later gets called, if problems ever arise with this function. + */ + + /* Then compare rdata */ + return pico_dns_rdata_cmp(my->rdata, peer->rdata, + short_be(my->rsuffix->rdlength), + short_be(peer->rsuffix->rdlength), 0); +} + +/* **************************************************************************** + * Deletes a single mDNS resource record. + * + * @param record Void-pointer to mDNS Resource Record. Can be used with pico_- + * tree-destroy. + * @return Returns 0 on success, something else on failure. + * ****************************************************************************/ +int +pico_mdns_record_delete( void **record ) +{ + struct pico_mdns_record **rr = (struct pico_mdns_record **)record; + + /* Check params */ + if (!rr || !(*rr)) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* Delete DNS record contained */ + if (((*rr)->record)) + pico_dns_record_delete((void **)&((*rr)->record)); + + /* Delete the record itself */ + PICO_FREE(*rr); + *record = NULL; + + return 0; +} + +/* **************************************************************************** + * Set the MSB of the QCLASS field of a DNS record depending on flags. + * + * @param record DNS record you want the set or clear the class MSB for. + * @param flags Depending on this the MSB will be set or not. Can be either + * PICO_MDNS_RECORD_UNIQUE or PICO_MDNS_RECORD_SHARED + * @return 0 on success, something else on failure. + * ****************************************************************************/ +static inline int +pico_mdns_record_set_class( struct pico_dns_record *record, + uint8_t flags ) +{ + uint16_t c = PICO_DNS_CLASS_IN; + PICO_MDNS_SET_MSB(c); + c = short_be(c); + + /* Set the MSB of the rclass field according to the mDNS format */ + if (!((flags) & PICO_MDNS_RECORD_SHARED)) + record->rsuffix->rclass = c; + + return 0; +} + +/* **************************************************************************** + * Creates a single standalone mDNS resource record with given name, type and + * data. + * + * @param url DNS rrecord name in URL format. Will be converted to DNS + * name notation format. + * @param _rdata Memory buffer with data to insert in the resource record. If + * data of record should contain a DNS name, the name in the + * data buffer needs to be in URL-format. + * @param datalen The exact length in bytes of the _rdata-buffer. If data of + * record should contain a DNS name, datalen needs to be + * pico_dns_strlen(_rdata). + * @param len Will be filled with the total length of the DNS rrecord. + * @param rtype DNS type of the resource record to be. + * @param rclass DNS class of the resource record to be. + * @param rttl DNS ttl of the resource record to be. + * @param flags You can specify if the mDNS record should be a shared record + * rather than a unique record. + * @return Pointer to newly created mDNS resource record. + * ****************************************************************************/ +struct pico_mdns_record * +pico_mdns_record_create( const char *url, + void *_rdata, + uint16_t datalen, + uint16_t rtype, + uint32_t rttl, + uint8_t flags ) +{ + struct pico_mdns_record *record = NULL; + uint16_t len = 0; + + /* Check params */ + if (!url || !_rdata) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + /* Provide space for the new mDNS resource record */ + if (!(record = PICO_ZALLOC(sizeof(struct pico_mdns_record)))) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + /* Try to create the actual DNS record */ + if (!(record->record = pico_dns_record_create(url, _rdata, datalen, + &len, rtype, + PICO_DNS_CLASS_IN, rttl))) { + mdns_dbg("Could not create DNS record for mDNS!\n"); + PICO_FREE(record); + return NULL; + } + + /* Initialise fields */ + record->current_ttl = rttl; + pico_mdns_record_set_class(record->record, flags); + record->flags = flags; + record->claim_id = 0; + + return record; +} + +/* MARK: ^ MDNS RECORDS */ +/* MARK: v MDNS COOKIES */ + +/* **************************************************************************** + * Deletes a single mDNS packet cookie and frees memory. + * + * @param cookie Void-pointer to mDNS cookie, allow to be used with pico_tree- + * destroy. + * @return Returns 0 on success, something else on failure. + * ****************************************************************************/ +static int +pico_mdns_cookie_delete( struct pico_mdns_cookie **c ) +{ + /* Check params */ + if (!c || !(*c)) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* Destroy the vectors contained */ + PICO_DNS_QTREE_DESTROY(&((*c)->qtree)); + PICO_MDNS_RTREE_DESTROY(&((*c)->antree)); + PICO_MDNS_RTREE_DESTROY(&((*c)->artree)); + + /* Delete the cookie itself */ + PICO_FREE(*c); + *c = NULL; + + return 0; +} + +/* **************************************************************************** + * Creates a single standalone mDNS cookie + * + * @param qtree DNS questions you want to insert in the cookie. + * @param antree mDNS answers/authority records you want to add to cookie. + * @param artree mDNS additional records you want to add to cookie. + * @param count Times you want to send the cookie as a packet on the wire. + * @param type Type of packet you want to create from the cookie. + * @param callback Callback when the host receives responses for the cookie. + * @return Pointer to newly create cookie, NULL on failure. + * ****************************************************************************/ +static struct pico_mdns_cookie * +pico_mdns_cookie_create( pico_dns_qtree qtree, + pico_mdns_rtree antree, + pico_mdns_rtree artree, + uint8_t count, + uint8_t type, + void (*callback)(pico_mdns_rtree *, + char *, + void *), + void *arg ) +{ + struct pico_mdns_cookie *cookie = NULL; /* Packet cookie to send */ + + /* Provide space for the mDNS packet cookie */ + cookie = PICO_ZALLOC(sizeof(struct pico_mdns_cookie)); + if (!cookie) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + /* Fill in the fields */ + cookie->qtree = qtree; + cookie->antree = antree; + cookie->artree = artree; + cookie->count = count; + cookie->type = type; + cookie->status = PICO_MDNS_COOKIE_STATUS_INACTIVE; + cookie->timeout = PICO_MDNS_COOKIE_TIMEOUT; + cookie->callback = callback; + cookie->arg = arg; + return cookie; +} + +/* **************************************************************************** + * Apply Simultaneous Probe Tiebreakin (S.P.T.) on a probe-cookie. + * + * @param cookie Cookie which contains the record which is simult. probed. + * @param answer Authority record received from peer which is simult. probed. + * @return 0 when SPT is applied correctly, 0 otherwise. + * ****************************************************************************/ +static int +pico_mdns_cookie_apply_spt( struct pico_mdns_cookie *cookie, + struct pico_dns_record *answer) +{ + struct pico_mdns_record *my_record = NULL; + struct pico_mdns_record peer_record; + + /* Check params */ + if ((!cookie) || !answer || (cookie->type != PICO_MDNS_PACKET_TYPE_PROBE)) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + cookie->status = PICO_MDNS_COOKIE_STATUS_INACTIVE; + + /* Implement Simultaneous Probe Tiebreaking */ + peer_record.record = answer; + my_record = pico_tree_findKey(&MyRecords, &peer_record); + if (!my_record || !IS_RECORD_PROBING(my_record)) { + mdns_dbg("This is weird! My record magically removed...\n"); + return -1; + } + + if (pico_mdns_record_am_i_lexi_later(my_record, &peer_record) > 0) { + mdns_dbg("My record is lexicographically later! Yay!\n"); + cookie->status = PICO_MDNS_COOKIE_STATUS_ACTIVE; + } else { + pico_timer_cancel(cookie->send_timer); + cookie->timeout = PICO_MDNS_COOKIE_TIMEOUT; + cookie->count = PICO_MDNS_PROBE_COUNT; + cookie->send_timer = pico_timer_add(1000, pico_mdns_send_probe_packet, + cookie); + mdns_dbg("Probing postponed with 1s because of S.P.T.\n"); + } + + return 0; +} + +/* **************************************************************************** + * Applies conflict resolution mechanism to a cookie, when a conflict occurs + * for a name which is present in the cookie. + * + * @param cookie Cookie on which you want to apply the conflict resolution- + * mechanism. + * @param rname Name for which the conflict occurred. A new non-conflicting + * name will be generated from this string. + * @return Returns 0 on success, something else on failure. + * ****************************************************************************/ +static int +pico_mdns_cookie_resolve_conflict( struct pico_mdns_cookie *cookie, + char *rname ) +{ + struct pico_tree_node *node = NULL; + struct pico_dns_question *question = NULL; + PICO_MDNS_RTREE_DECLARE(new_records); + PICO_MDNS_RTREE_DECLARE(antree); + char *new_name = NULL; + void (*callback)(pico_mdns_rtree *, char *, void *); + void *arg = NULL; + uint16_t qc = 0; + int retval; + + /* Check params */ + if ((!cookie) || !rname || (cookie->type != PICO_MDNS_PACKET_TYPE_PROBE)) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* Convert rname to url */ + mdns_dbg("CONFLICT for probe query with name '%s' occurred!\n", rname); + + /* DNS conflict is case-insensitive. However, we want to keep the original + * capitalisation for the new probe. */ + pico_tree_foreach(node, &(cookie->qtree)) { + question = (struct pico_dns_question *)node->keyValue; + if ((question) && (strcasecmp(question->qname, rname) == 0)) + /* Create a new name depending on current name */ + new_name = pico_mdns_resolve_name_conflict(question->qname); + } + + /* Step 1: Remove question with that name from cookie and store some + * useful information */ + pico_dns_qtree_del_name(&(cookie->qtree), rname); + antree = cookie->antree; + callback = cookie->callback; + arg = cookie->arg; + cookie->antree.root = &LEAF; + + /* Check if there are no questions left, cancel events if so and delete */ + if (!(qc = pico_tree_count(&(cookie->qtree)))) { + pico_timer_cancel(cookie->send_timer); + cookie = pico_tree_delete(&Cookies, cookie); + pico_mdns_cookie_delete(&cookie); + } + + /* Step 2: Check if the new name succeeded, if not: error. */ + if (!(new_name)) + return -1; + + /* Step 3: Create records with new name for the records with that name */ + new_records = pico_mdns_generate_new_records(&antree, rname, new_name); + PICO_FREE(new_name); + + /* Step 4: Try to reclaim the newly created records */ + retval = pico_mdns_reclaim(new_records, callback, arg); + pico_tree_destroy(&new_records, NULL); + return retval; +} + +/* **************************************************************************** + * Find a query cookie that contains a question for a specific name. + * + * @param name Name of question you want to look for. + * @return Pointer to cookie in tree when one is found, NULL on failure. + * ****************************************************************************/ +static struct pico_mdns_cookie * +pico_mdns_ctree_find_cookie( const char *name, uint8_t type ) +{ + struct pico_mdns_cookie *cookie = NULL; + struct pico_tree_node *node = NULL; + + /* Check params */ + if (!name) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + /* Find the cookie in the tree wherein the question is present */ + pico_tree_foreach(node, &Cookies) { + if ((cookie = node->keyValue) && + pico_dns_qtree_find_name(&(cookie->qtree), name)) { + if (type == PICO_MDNS_PACKET_TYPE_QUERY_ANY) + return cookie; + else if (cookie->type == type) + return cookie; + } + } + + return NULL; +} + +/* MARK: ^ MDNS COOKIES */ +/* MARK: v MY RECORDS */ + +/* **************************************************************************** + * Adds records contained in records-tree to MyRecords. Suppresses adding of + * duplicates. + * + * @param records Tree with records to add to 'MyRecords'. + * @param reclaim If the records contained in records are claimed again. + * @return 0 on success, something else on failure. + * ****************************************************************************/ +static int +pico_mdns_my_records_add( pico_mdns_rtree *records, uint8_t reclaim ) +{ + struct pico_tree_node *node = NULL; + struct pico_mdns_record *record = NULL; + static uint8_t claim_id_count = 0; + + if (!reclaim) + ++claim_id_count; + + /* Iterate over record vector */ + pico_tree_foreach(node, records) { + record = node->keyValue; + if (record) { + /* Set probed flag if record is a shared record */ + if (IS_SHARED_RECORD(record)) + PICO_MDNS_SET_FLAG(record->flags, PICO_MDNS_RECORD_PROBED); + + /* If record is not claimed again, set new claim-ID */ + if (!reclaim) + record->claim_id = claim_id_count; + pico_tree_insert(&MyRecords, record); + } + } + return 0; +} + +/* **************************************************************************** + * Generates a tree of all My Records for which the probe flag already has + * been set. Copies the records from MyRecords into a new tree. + * + * @return Tree with all records in MyRecords with the PROBED-flag set. + * ****************************************************************************/ +static pico_mdns_rtree +pico_mdns_my_records_find_probed( void ) +{ + PICO_MDNS_RTREE_DECLARE(probed); + struct pico_tree_node *node = NULL; + struct pico_mdns_record *record = NULL, *copy = NULL; + + /* Iterate over MyRecords */ + pico_tree_foreach(node, &MyRecords) { + record = node->keyValue; + if (record && IS_RECORD_VERIFIED(record) && !IS_RECORD_CLAIMED(record)) { + copy = pico_mdns_record_copy(record); + if (copy && pico_tree_insert(&probed, copy)) + pico_mdns_record_delete((void **)©); + } + } + + return probed; +} + +/* **************************************************************************** + * Generates a tree of all My Records for which the probe flag has not yet + * been set. Copies the record from MyRecords into a new tree. + * + * @return Tree with all records in MyRecords with the PROBED-flag not set. + * ****************************************************************************/ +static pico_mdns_rtree +pico_mdns_my_records_find_to_probe( void ) +{ + PICO_MDNS_RTREE_DECLARE(toprobe); + struct pico_tree_node *node = NULL; + struct pico_mdns_record *record = NULL, *copy = NULL; + + pico_tree_foreach(node, &MyRecords) { + record = node->keyValue; + /* Check if probed flag is not set of a record */ + if (record && + IS_UNIQUE_RECORD(record) && + !IS_RECORD_VERIFIED(record) && + !IS_RECORD_PROBING(record)) { + /* Set record to currently being probed status */ + record->flags |= PICO_MDNS_RECORD_CURRENTLY_PROBING; + copy = pico_mdns_record_copy(record); + if (copy && pico_tree_insert(&toprobe, copy)) + pico_mdns_record_delete((void **)©); + } + } + return toprobe; +} + +/* **************************************************************************** + * Checks whether all MyRecords with a certain claim ID are claimed or not. + * + * @param claim_id Claim ID of the records to check for already been probed. + * @param reg_records Tree in which all MyRecords with claim ID are inserted. + * @return 1 when all MyRecords with claim ID are probed, 0 when they're not. + * ****************************************************************************/ +static uint8_t +pico_mdns_my_records_claimed_id( uint8_t claim_id, + pico_mdns_rtree *reg_records ) +{ + struct pico_tree_node *node = NULL; + struct pico_mdns_record *record = NULL; + + /* Initialise the iterator for iterating over my records */ + pico_tree_foreach(node, &MyRecords) { + record = node->keyValue; + if (record && record->claim_id == claim_id) { + if (IS_RECORD_VERIFIED(record)) { + pico_tree_insert(reg_records, record); + } else { + return 0; + } + } + } + + return 1; +} + +/* **************************************************************************** + * Marks mDNS resource records in the tree as registered. Checks MyRecords for + * for other records with the same claim ID. If all records with the same claim + * ID as the records in the tree are claimed, the callback will get called. + * + * @param rtree Tree with mDNS records that are registered. + * @param callback Callback will get called when all records are registered. + * @return Returns 0 when everything went smooth, something else otherwise. + * ****************************************************************************/ +static int +pico_mdns_my_records_claimed( pico_mdns_rtree rtree, + void (*callback)(pico_mdns_rtree *, + char *, + void *), + void *arg ) +{ + PICO_MDNS_RTREE_DECLARE(claimed_records); + struct pico_mdns_record *record = NULL, *myrecord = NULL; + struct pico_tree_node *node = NULL; + uint8_t claim_id = 0; + + /* Iterate over records and set the PROBED flag */ + pico_tree_foreach(node, &rtree) { + if ((record = node->keyValue)) { + if (!claim_id) + claim_id = record->claim_id; + } + + if ((myrecord = pico_tree_findKey(&MyRecords, record))) + PICO_MDNS_SET_FLAG(myrecord->flags, PICO_MDNS_RECORD_CLAIMED); + } + + /* If all_claimed is still true */ + if (pico_mdns_my_records_claimed_id(claim_id, &claimed_records)) + callback(&claimed_records, _hostname, arg); + pico_tree_destroy(&claimed_records, NULL); + + mdns_dbg(">>>>>> DONE - CLAIM SESSION: %d\n", claim_id); + + return 0; +} + +/* **************************************************************************** + * Makes sure the cache flush bit is set of the records which are probed, and + * set the corresponding MyRecords to 'being probed'-state. + * + * @param records mDNS records which are probed. + * ****************************************************************************/ +static void +pico_mdns_my_records_probed( pico_mdns_rtree *records ) +{ + struct pico_tree_node *node = NULL; + struct pico_mdns_record *record = NULL, *found = NULL; + + pico_tree_foreach(node, records) { + if ((record = node->keyValue)) { + /* Set the cache flush bit again */ + PICO_MDNS_SET_MSB_BE(record->record->rsuffix->rclass); + if ((found = pico_tree_findKey(&MyRecords, record))) { + if (IS_HOSTNAME_RECORD(found)) { + if (_hostname) + PICO_FREE(_hostname); + + _hostname = pico_dns_qname_to_url(found->record->rname); + } + + PICO_MDNS_SET_FLAG(found->flags, PICO_MDNS_RECORD_PROBED); + PICO_MDNS_CLR_FLAG(found->flags, PICO_MDNS_RECORD_CURRENTLY_PROBING); + } else + mdns_dbg("Could not find my corresponding record...\n"); + } + } +} + +/* MARK: ^ MY RECORDS */ +/* MARK: v CACHE COHERENCY */ +#if PICO_MDNS_ALLOW_CACHING == 1 +/* **************************************************************************** + * Updates TTL of a cache entry. + * + * @param record Record of which you want to update the TTL of + * @param ttl TTL you want to update the TTL of the record to. + * @return void + * ****************************************************************************/ +static inline void +pico_mdns_cache_update_ttl( struct pico_mdns_record *record, + uint32_t ttl ) +{ + if(ttl > 0) { + /* Update the TTL's */ + record->record->rsuffix->rttl = long_be(ttl); + record->current_ttl = ttl; + } else { + /* TTL 0 means delete from cache but we need to wait one second */ + record->record->rsuffix->rttl = long_be(1u); + record->current_ttl = 1u; + } +} + +/* **************************************************************************** + * Adds a mDNS record to the cache. + * + * @param record mDNS record to add to the Cache. + * @return 0 when entry successfully added, something else when it all went ho- + * rribly wrong... + * ****************************************************************************/ +static int +pico_mdns_cache_add( struct pico_mdns_record *record ) +{ + struct pico_dns_record_suffix *suffix = NULL; + char *name = NULL; + uint16_t type = 0; + uint32_t rttl = 0; + + /* Check params */ + if (!record) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + suffix = record->record->rsuffix; + name = record->record->rname; + type = short_be(suffix->rtype); + rttl = long_be(suffix->rttl); + + /* Check if cache flush bit is set */ + if (PICO_MDNS_IS_MSB_SET(short_be(suffix->rclass))) { + mdns_dbg("FLUSH - Cache flush bit was set, triggered flush.\n"); + if (pico_mdns_rtree_del_name_type(&Cache, name, type)) { + mdns_dbg("Could not flush records from cache!\n"); + return -1; + } + } + + /* Check if the TTL is not 0*/ + if (!rttl) + return -1; + /* Set current TTL to the original TTL before inserting */ + record->current_ttl = rttl; + + if (pico_tree_insert(&Cache, record) != NULL) + return -1; + + mdns_dbg("RR cached. TICK TACK TICK TACK...\n"); + + return 0; +} + +/* **************************************************************************** + * Add a copy of an mDNS resource record to the cache tree. Checks whether the + * entry is already present in the Cache or not. + * + * @param record Record to add to the Cache-tree + * @return 0 on grrrreat success, something else on awkward failure. + * ****************************************************************************/ +static int +pico_mdns_cache_add_record( struct pico_mdns_record *record ) +{ + struct pico_mdns_record *found = NULL, *copy = NULL; + uint32_t rttl = 0; + + /* Check params */ + if (!record) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* See if the record is already contained in the cache */ + if ((found = pico_tree_findKey(&Cache, record))) { + rttl = long_be(record->record->rsuffix->rttl); + pico_mdns_cache_update_ttl(found, rttl); + } else if ((copy = pico_mdns_record_copy(record))) { + if (pico_mdns_cache_add(copy)) { + pico_mdns_record_delete((void **)©); + return -1; + } + } else + return -1; + + return 0; +} + +#if PICO_MDNS_CONTINUOUS_REFRESH == 1 +/* **************************************************************************** + * Determine if the current TTL is at a refreshing point. + * + * @param original Original TTL to calculate refreshing points + * @param current Current TTL to check. + * @return 1 when Current TTL is at refresh point. 0 when it's not. + * ****************************************************************************/ +static int +pico_mdns_ttl_at_refresh_time( uint32_t original, + uint32_t current ) +{ + uint32_t rnd = 0; + rnd = pico_rand() % 3; + + if (((original - current == + ((original * (80 + rnd)) / 100)) ? 1 : 0) || + ((original - current == + ((original * (85 + rnd)) / 100)) ? 1 : 0) || + ((original - current == + ((original * (90 + rnd)) / 100)) ? 1 : 0) || + ((original - current == + ((original * (95 + rnd)) / 100)) ? 1 : 0)) + return 1; + else + return 0; +} +#endif + +/* **************************************************************************** + * Utility function to update the TTL of cache entries and check for expired + * ones. When continuous refreshing is enabled the records will be reconfirmed + * @ 80%, 85%, 90% and 95% of their original TTL. + * ****************************************************************************/ +static void +pico_mdns_cache_check_expiries( void ) +{ + struct pico_tree_node *node = NULL, *next = NULL; + struct pico_mdns_record *record = NULL; +#if PICO_MDNS_CONTINUOUS_REFRESH == 1 + uint32_t current = 0, original = 0; + uint16_t type 0; + char *url = NULL; +#endif + + /* Check for expired cache records */ + pico_tree_foreach_safe(node, &Cache, next) { + if ((record = node->keyValue)) { + /* Update current ttl and delete when TTL is 0*/ + if ((--(record->current_ttl)) == 0) { + record = pico_tree_delete(&Cache, record); + pico_mdns_record_delete((void **)&record); + } + +#if PICO_MDNS_CONTINUOUS_REFRESH == 1 + /* Determine original and current ttl */ + original = long_be(record->record->rsuffix->rttl); + current = record->current_ttl; + + /* Cache refresh at 80 or 85/90/95% of TTL + 2% rnd */ + if (pico_mdns_ttl_at_refresh_time(original, current)) { + url = pico_dns_qname_to_url(record->record->rname); + type = short_be(record->record->rsuffix->rtype) + pico_mdns_getrecord_generic(url, type, NULL, NULL); + PICO_FREE(url); + } + +#endif + } + } +} +#endif /* PICO_MDNS_ALLOW_CACHING */ + +/* **************************************************************************** + * Utility function to update the TTL of cookies and check for expired + * ones. Deletes the expired ones as well. + * ****************************************************************************/ +static void +pico_mdns_cookies_check_timeouts( void ) +{ + struct pico_tree_node *node = NULL, *next = NULL; + struct pico_mdns_cookie *cookie = NULL; + + pico_tree_foreach_safe(node, &Cookies, next) { + if ((cookie = node->keyValue) && --(cookie->timeout) == 0) { + /* Call callback to allow error checking */ + if (cookie->callback) { + cookie->callback(NULL, NULL, cookie->arg); + } + + /* Delete cookie */ + cookie = pico_tree_delete(&Cookies, cookie); + pico_mdns_cookie_delete(&cookie); + + /* If the request was for a reconfirmation of a record, + flush the corresponding record after the timeout */ + } + } +} + +/* **************************************************************************** + * Global mDNS module tick-function, central point where all the timing is + * handled. + * + * @param now Ignore + * @param _arg Ignore + * ****************************************************************************/ +static void +pico_mdns_tick( pico_time now, void *_arg ) +{ + IGNORE_PARAMETER(now); + IGNORE_PARAMETER(_arg); + +#if PICO_MDNS_ALLOW_CACHING == 1 + /* Update the cache */ + pico_mdns_cache_check_expiries(); +#endif + + /* Update the cookies */ + pico_mdns_cookies_check_timeouts(); + + /* Schedule new tick */ + pico_timer_add(PICO_MDNS_RR_TTL_TICK, pico_mdns_tick, NULL); +} + +/* MARK: v MDNS PACKET UTILITIES */ + +/* **************************************************************************** + * Sends a Multicast packet on the wire to the mDNS destination port. + * + * @param packet Packet buffer in memory + * @param len Size of the packet in bytes + * @return 0 When the packet is passed successfully on to the lower layers of + * picoTCP. Doesn't mean the packet is successfully send on the wire. + * ****************************************************************************/ +static int +pico_mdns_send_packet( pico_dns_packet *packet, uint16_t len ) +{ + struct pico_ip4 dst4; + + /* Set the destination address to the mDNS multicast-address */ + pico_string_to_ipv4(PICO_MDNS_DEST_ADDR4, &dst4.addr); + + /* Send packet to IPv4 socket */ + return pico_socket_sendto(mdns_sock_ipv4, packet, (int)len, &dst4, + short_be(mdns_port)); +} + +/* **************************************************************************** + * Sends a Unicast packet on the wire to the mDNS destination port of specific + * peer in the network + * + * @param packet Packet buffer in memory + * @param len Size of the packet in bytes + * @param peer Peer in the network you want to send the packet to. + * @return 0 When the packet is passed successfully on to the lower layers of + * picoTCP. Doesn't mean the packet is successfully send on the wire. + * ****************************************************************************/ +static int +pico_mdns_send_packet_unicast( pico_dns_packet *packet, + uint16_t len, + struct pico_ip4 peer ) +{ + /* Send packet to IPv4 socket */ + return pico_socket_sendto(mdns_sock_ipv4, packet, (int)len, &peer, + short_be(mdns_port)); +} + + +/* **************************************************************************** + * Send DNS records as answers to a peer via unicast + * + * @param unicast_tree Tree with DNS records to send as answers. + * @param peer Peer IPv4-address + * @return 0 when the packet is properly send, something else otherwise. + * ****************************************************************************/ +static int +pico_mdns_unicast_reply( pico_dns_rtree *unicast_tree, + pico_dns_rtree *artree, + struct pico_ip4 peer ) +{ + union pico_address *local_addr = NULL; + pico_dns_packet *packet = NULL; + uint16_t len = 0; + + if (pico_tree_count(unicast_tree) > 0) { + /* Create response DNS packet */ + packet = pico_dns_answer_create(unicast_tree, NULL, artree, &len); + if (!packet || !len) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + packet->id = 0; + + /* Check if source address is on the local link */ + local_addr = (union pico_address *) pico_ipv4_source_find(&peer); + if (!local_addr) { + mdns_dbg("Peer not on same link!\n"); + /* Forced response via multicast */ + if (pico_mdns_send_packet(packet, len) != (int)len) { + mdns_dbg("Could not send multicast response!\n"); + return -1; + } + } else { + /* Send the packet via unicast */ + if (pico_mdns_send_packet_unicast(packet, len, peer) != (int)len) { + mdns_dbg("Could not send unicast response!\n"); + return -1; + } + + mdns_dbg("Unicast response sent successfully!\n"); + } + PICO_FREE(packet); + } + + return 0; +} + +/* **************************************************************************** + * Send DNS records as answers to mDNS peers via multicast + * + * @param multicast_tree Tree with DNS records to send as answers. + * @return 0 when the packet is properly send, something else otherwise. + * ****************************************************************************/ +static int +pico_mdns_multicast_reply( pico_dns_rtree *multicast_tree, + pico_dns_rtree *artree ) +{ + pico_dns_packet *packet = NULL; + uint16_t len = 0; + + /* If there are any multicast records */ + if (pico_tree_count(multicast_tree) > 0) { + /* Create response DNS packet */ + packet = pico_dns_answer_create(multicast_tree, NULL, artree, &len); + if (!packet || len == 0) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + packet->id = 0; + + /* Send the packet via multicast */ + if (pico_mdns_send_packet(packet, len) != (int)len) { + mdns_dbg("Could not send multicast response!\n"); + return -1; + } + + mdns_dbg("Multicast response sent successfully!\n"); + + PICO_FREE(packet); + } + + return 0; +} + +/* MARK: ^ MDNS PACKET UTILITIES */ +/* MARK: ASYNCHRONOUS MDNS RECEPTION */ + +/* **************************************************************************** + * Merges 2 pico_trees with each other. + * + * @param dest Destination tree to merge the other tree in. + * @param src Source tree to get the node from to insert into the dest-tree. + * @return Returns 0 when properly merged, or not.. + * ****************************************************************************/ +static int +pico_tree_merge( struct pico_tree *dest, struct pico_tree *src ) +{ + struct pico_tree_node *node = NULL; + + /* Check params */ + if (!dest || !src) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* Insert source nodes */ + pico_tree_foreach(node, src) { + if (node->keyValue) { + pico_tree_insert(dest, node->keyValue); + } + } + + return 0; +} + +/* **************************************************************************** + * Populates an mDNS record tree with answers from MyRecords depending on name + * , qtype and qclass. + * + * @param name Name of records to look for in MyRecords + * @param qtype Type of records to look for in MyRecords + * @param qclass Whether the answer should be sent via unicast or not. + * @return mDNS record tree with possible answers from MyRecords + * ****************************************************************************/ +static pico_mdns_rtree +pico_mdns_populate_antree( char *name, uint16_t qtype, uint16_t qclass ) +{ + PICO_MDNS_RTREE_DECLARE(antree); + struct pico_tree_node *node = NULL, *next; + struct pico_mdns_record *record = NULL; + + /* Create an answer record vector */ + if (PICO_DNS_TYPE_ANY == qtype) + antree = pico_mdns_rtree_find_name(&MyRecords, name, 1); + else + antree = pico_mdns_rtree_find_name_type(&MyRecords, name, qtype, 1); + + /* Remove answers which aren't successfully registered yet */ + pico_tree_foreach_safe(node, &antree, next) { + if ((record = node->keyValue) && !IS_RECORD_VERIFIED(record)) { + pico_tree_delete(&antree, record); + } + } + + /* Check if question is a QU-question */ + if (PICO_MDNS_IS_MSB_SET(qclass)) { + /* Set all the flags of the answer accordingly */ + pico_tree_foreach(node, &antree) { + if ((record = node->keyValue)) + PICO_MDNS_SET_FLAG(record->flags, + PICO_MDNS_RECORD_SEND_UNICAST); + } + } + + return antree; +} + +/* **************************************************************************** + * Handles a single received question. + * + * @param question DNS question to parse and handle. + * @param packet Received packet in which the DNS question was present. + * @return mDNS record tree with possible answer to the question. Can possibly + * be empty. + * ****************************************************************************/ +static pico_mdns_rtree +pico_mdns_handle_single_question( struct pico_dns_question *question, + pico_dns_packet *packet ) +{ + struct pico_mdns_cookie *cookie = NULL; + PICO_MDNS_RTREE_DECLARE(antree); + char *qname_original = NULL; + uint16_t qtype = 0, qclass = 0; + + /* Check params */ + if (!question || !packet) { + pico_err = PICO_ERR_EINVAL; + return antree; + } + + /* Decompress single DNS question */ + qname_original = pico_dns_question_decompress(question, packet); + mdns_dbg("Question RCVD for '%s'\n", question->qname); + + /* Find currently active query cookie */ + if ((cookie = pico_mdns_ctree_find_cookie(question->qname, + PICO_MDNS_PACKET_TYPE_QUERY))) { + mdns_dbg("Query cookie found for question, suppress duplicate.\n"); + cookie->status = PICO_MDNS_COOKIE_STATUS_CANCELLED; + } else { + qtype = short_be(question->qsuffix->qtype); + qclass = short_be(question->qsuffix->qclass); + antree = pico_mdns_populate_antree(question->qname, qtype, qclass); + } + + PICO_FREE(question->qname); + question->qname = qname_original; + return antree; +} + +/* **************************************************************************** + * When a query-cookie is found for a RCVD answer, the cookie should be + * handled accordingly. This function does that. + * + * @param cookie Cookie that contains the question for the RCVD answer. + * @param answer RCVD answer to handle cookie with + * @return Returns 0 when handling went OK, something else when it didn't. + * ****************************************************************************/ +static int +pico_mdns_handle_cookie_with_answer( struct pico_mdns_cookie *cookie, + struct pico_mdns_record *answer ) +{ + PICO_MDNS_RTREE_DECLARE(antree); + uint8_t type = 0, status = 0; + + /* Check params */ + if (!cookie || !answer) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + type = cookie->type; + status = cookie->status; + if (PICO_MDNS_COOKIE_STATUS_ACTIVE == status) { + if (PICO_MDNS_PACKET_TYPE_PROBE == type) { + /* Conflict occurred, resolve it! */ + pico_mdns_cookie_resolve_conflict(cookie, answer->record->rname); + } else if (PICO_MDNS_PACKET_TYPE_QUERY == type) { + if (cookie->callback) { + /* RCVD Answer on query, callback with answer. Callback is + * responsible for aggregating all the received answers. */ + pico_tree_insert(&antree, answer); + cookie->callback(&antree, NULL, cookie->arg); + } + } else { /* Don't handle answer cookies with answer */ + } + } + + return 0; +} + +/* **************************************************************************** + * Handles a single received answer record. + * + * @param answer Answer mDNS record. + * @return 0 when answer is properly handled, something else when it's not. + * ****************************************************************************/ +static int +pico_mdns_handle_single_answer( struct pico_mdns_record *answer ) +{ + struct pico_mdns_cookie *found = NULL; + struct pico_mdns_record *record = NULL; + + mdns_dbg("Answer RCVD for '%s'\n", answer->record->rname); + + /* Find currently active query cookie */ + found = pico_mdns_ctree_find_cookie(answer->record->rname, + PICO_MDNS_PACKET_TYPE_QUERY_ANY); + if (found && pico_mdns_handle_cookie_with_answer(found, answer)) { + mdns_dbg("Could not handle found cookie correctly!\n"); + return -1; + } else { + mdns_dbg("RCVD an unsolicited record!\n"); + if ((record = pico_tree_findKey(&MyRecords, answer)) && + !IS_RECORD_PROBING(record)) + return pico_mdns_record_resolve_conflict(record, + answer->record->rname); + } + + return 0; +} + +/* **************************************************************************** + * Handles a single received authority record. + * + * @param answer Authority mDNS record. + * @return 0 when authority is properly handled. -1 when it's not. + * ****************************************************************************/ +static int +pico_mdns_handle_single_authority( struct pico_mdns_record *answer ) +{ + struct pico_mdns_cookie *found = NULL; + char *name = NULL; + + name = answer->record->rname; + mdns_dbg("Authority RCVD for '%s'\n", name); + + /* Find currently active probe cookie */ + if ((found = pico_mdns_ctree_find_cookie(name, PICO_MDNS_PACKET_TYPE_PROBE)) + && PICO_MDNS_COOKIE_STATUS_ACTIVE == found->status) { + mdns_dbg("Simultaneous Probing occurred, went tiebreaking...\n"); + if (pico_mdns_cookie_apply_spt(found, answer->record) < 0) { + mdns_dbg("Could not apply S.P.T. to cookie!\n"); + return -1; + } + } + + return 0; +} + +/* **************************************************************************** + * Handles a single received additional [Temporarily unused] + * + * @param answer Additional mDNS record. + * @return 0 + * ****************************************************************************/ +static int +pico_mdns_handle_single_additional( struct pico_mdns_record *answer ) +{ + /* Don't need this for now ... */ + IGNORE_PARAMETER(answer); + return 0; +} + +/* **************************************************************************** + * Handles a flat chunk of memory as if it were all questions in it. + * Generates a tree with responses if there are any questions for records for + * which host has the authority to answer. + * + * @param ptr Pointer-Pointer to location of question section of packet. + * Will point to right after the question section on return. + * @param qdcount Amount of questions contained in the packet + * @param packet DNS packet where the questions are present. + * @return Tree with possible responses on the questions. + * ****************************************************************************/ +static pico_mdns_rtree +pico_mdns_handle_data_as_questions ( uint8_t **ptr, + uint16_t qdcount, + pico_dns_packet *packet ) +{ + PICO_MDNS_RTREE_DECLARE(antree); + PICO_MDNS_RTREE_DECLARE(rtree); + struct pico_dns_question question; + uint16_t i = 0; + + /* Check params */ + if ((!ptr) || !packet || !(*ptr)) { + pico_err = PICO_ERR_EINVAL; + return antree; + } + + for (i = 0; i < qdcount; i++) { + /* Set qname of the question to the correct location */ + question.qname = (char *)(*ptr); + + /* Set qsuffix of the question to the correct location */ + question.qsuffix = (struct pico_dns_question_suffix *) + (question.qname + pico_dns_namelen_comp(question.qname) + 1); + + /* Handle a single question and merge the returned tree */ + rtree = pico_mdns_handle_single_question(&question, packet); + pico_tree_merge(&antree, &rtree); + pico_tree_destroy(&rtree, NULL); + + /* Move to next question */ + *ptr = (uint8_t *)question.qsuffix + + sizeof(struct pico_dns_question_suffix); + } + if (pico_tree_count(&antree) == 0) { + mdns_dbg("No 'MyRecords' found that corresponds with this query.\n"); + } + + return antree; +} + +static int +pico_mdns_handle_data_as_answers_generic( uint8_t **ptr, + uint16_t count, + pico_dns_packet *packet, + uint8_t type ) +{ + struct pico_mdns_record mdns_answer = { + .record = NULL, .current_ttl = 0, + .flags = 0, .claim_id = 0 + }; + struct pico_dns_record answer; + char *orname = NULL; + uint16_t i = 0; + + /* Check params */ + if ((!ptr) || !packet || !(*ptr)) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + for (i = 0; i < count; i++) { + /* Set rname of the record to the correct location */ + answer.rname = (char *)(*ptr); + + /* Set rsuffix of the record to the correct location */ + answer.rsuffix = (struct pico_dns_record_suffix *) + (answer.rname + + pico_dns_namelen_comp(answer.rname) + 1u); + + /* Set rdata of the record to the correct location */ + answer.rdata = (uint8_t *) answer.rsuffix + + sizeof(struct pico_dns_record_suffix); + + /* Make an mDNS record from the DNS answer */ + orname = pico_dns_record_decompress(&answer, packet); + mdns_answer.record = &answer; + mdns_answer.record->rname_length = (uint16_t)(pico_dns_strlen(answer.rname) + 1u); + + /* Handle a single aswer */ + switch (type) { + case 1: + pico_mdns_handle_single_authority(&mdns_answer); + break; + case 2: + pico_mdns_handle_single_additional(&mdns_answer); + break; + default: + pico_mdns_handle_single_answer(&mdns_answer); +#if PICO_MDNS_ALLOW_CACHING == 1 + pico_mdns_cache_add_record(&mdns_answer); +#endif + break; + } + + /* Free decompressed name and mDNS record */ + PICO_FREE(mdns_answer.record->rname); + answer.rname = orname; + + /* Move to next record */ + *ptr = (uint8_t *) answer.rdata + short_be(answer.rsuffix->rdlength); + } + return 0; +} + +/* **************************************************************************** + * Splits an mDNS record tree into two DNS record tree, one to send via + * unicast, one to send via multicast. + * + * @param answers mDNS record tree to split up + * @param unicast_tree DNS record tree with unicast answers. + * @param multicast_tree DNS record tee with multicast answers. + * @return 0 when the tree is properly split up. + * ****************************************************************************/ +static int +pico_mdns_sort_unicast_multicast( pico_mdns_rtree *answers, + pico_dns_rtree *unicast_tree, + pico_dns_rtree *multicast_tree ) +{ + struct pico_mdns_record *record = NULL; + struct pico_tree_node *node = NULL; + + /* Check params */ + if (!answers || !unicast_tree || !multicast_tree) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + pico_tree_foreach(node, answers) { + record = node->keyValue; + if ((record = node->keyValue)) { + if (IS_UNICAST_REQUESTED(record)) { + if (record->record) + pico_tree_insert(unicast_tree, record->record); + } else { + if (record->record) + pico_tree_insert(multicast_tree, record->record); + } + } + } + + return 0; +} + +static uint16_t +pico_mdns_nsec_highest_type( pico_mdns_rtree *rtree ) +{ + struct pico_tree_node *node = NULL, *next = NULL; + struct pico_mdns_record *record = NULL; + uint16_t highest_type = 0, type = 0; + + pico_tree_foreach_safe(node, rtree, next) { + if ((record = node->keyValue)) { + if (IS_SHARED_RECORD(record)) + pico_tree_delete(rtree, record); + + type = short_be(record->record->rsuffix->rtype); + highest_type = (type > highest_type) ? (type) : (highest_type); + } + } + + return highest_type; +} + +static void +pico_mdns_nsec_gen_bitmap( uint8_t *ptr, pico_mdns_rtree *rtree ) +{ + struct pico_tree_node *node = NULL; + struct pico_mdns_record *record = NULL; + uint16_t type = 0; + + pico_tree_foreach(node, rtree) { + if ((record = node->keyValue)) { + type = short_be(record->record->rsuffix->rtype); + *(ptr + 1 + (type / 8)) = (uint8_t)(0x80 >> (type % 8)); + } + } +} + +/* **************************************************************************** + * Generates an NSEC record for a specific name. Looks in MyRecords for unique + * records with given name and generates the NSEC bitmap from them. + * + * @param name Name of the records you want to generate a bitmap for. + * @return Pointer to newly created NSEC record on success, NULL on failure. + * ****************************************************************************/ +static struct pico_mdns_record * +pico_mdns_gen_nsec_record( char *name ) +{ + PICO_MDNS_RTREE_DECLARE(rtree); + struct pico_mdns_record *record = NULL; + uint16_t highest_type = 0, rdlen = 0; + uint8_t bitmap_len = 0, *rdata = NULL, *ptr = NULL; + char *url = NULL; + + if (!name) { /* Check params */ + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + /* Determine the highest type of my unique records with this name */ + rtree = pico_mdns_rtree_find_name(&MyRecords, name, 0); + highest_type = pico_mdns_nsec_highest_type(&rtree); + + /* Determine the bimap_len */ + bitmap_len = (uint8_t)(highest_type / 8); + bitmap_len = (uint8_t)(bitmap_len + ((highest_type % 8) ? (1) : (0))); + + /* Provide rdata */ + rdlen = (uint16_t)(pico_dns_strlen(name) + 3u + bitmap_len); + if (!(rdata = PICO_ZALLOC((size_t)rdlen))) { + pico_err = PICO_ERR_ENOMEM; + pico_tree_destroy(&rtree, NULL); + return NULL; + } + + /* Set the next domain name */ + strcpy((char *)rdata, name); + /* Set the bitmap length */ + *(ptr = (uint8_t *)(rdata + pico_dns_strlen(name) + 2)) = bitmap_len; + /* Generate the bitmap */ + pico_mdns_nsec_gen_bitmap(ptr, &rtree); + pico_tree_destroy(&rtree, NULL); + + /* Generate the actual mDNS NSEC record */ + if (!(url = pico_dns_qname_to_url(name))) { + PICO_FREE(rdata); + return NULL; + } + + record = pico_mdns_record_create(url, (void *)rdata, rdlen, + PICO_DNS_TYPE_NSEC, + PICO_MDNS_SERVICE_TTL, + PICO_MDNS_RECORD_UNIQUE); + PICO_FREE(rdata); + PICO_FREE(url); + return record; +} + +/* **************************************************************************** + * Checks in additionals if there is an NSEC record already present with given + * name. If there's not, a new NSEC records will be generated and added to the + * additional tree. + * + * @param artree mDNS record-tree containing additional records. + * @param name Name to check for. + * @return 0 when NSEC is present in additional, whether it was already present + * or a new one is generated doesn't matter. + * ****************************************************************************/ +static int +pico_mdns_additionals_add_nsec( pico_mdns_rtree *artree, + char *name ) +{ + struct pico_mdns_record *record = NULL, *nsec = NULL; + struct pico_tree_node *node = NULL; + uint16_t type = 0; + + /* Check params */ + if (!artree || !name) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* Check if there is a NSEC already for this name */ + pico_tree_foreach(node, artree) { + if (node != &LEAF && (record = node->keyValue)) { + type = short_be(record->record->rsuffix->rtype); + if (PICO_DNS_TYPE_NSEC == type) { + if (strcasecmp(record->record->rname, name) == 0) + return 0; + } + } + } + + /* If there is none present generate one for given name */ + if ((nsec = pico_mdns_gen_nsec_record(name))) + pico_tree_insert(artree, nsec); + return 0; +} + +/* **************************************************************************** + * Adds hostname records to the additional records + * + * @param artree mDNS record-tree containing additional records. + * @return 0 when hostname records are added successfully to additionals. Rets + * something else on failure. + * ****************************************************************************/ +static int +pico_mdns_additionals_add_host( pico_mdns_rtree *artree ) +{ + struct pico_tree_node *node = NULL; + struct pico_mdns_record *record = NULL, *copy = NULL; + + pico_tree_foreach(node, &MyRecords) { + if ((record = node->keyValue) && + IS_HOSTNAME_RECORD(record) && + IS_RECORD_VERIFIED(record)) { + copy = pico_mdns_record_copy(record); + if (copy && pico_tree_insert(artree, copy)) + pico_mdns_record_delete((void **)©); + } + } + + return 0; +} + +static void +pico_rtree_add_copy( pico_mdns_rtree *tree, struct pico_mdns_record *record ) +{ + struct pico_mdns_record *copy = NULL; + + if (!tree || !record) { + pico_err = PICO_ERR_EINVAL; + return; + } + + if ((copy = pico_mdns_record_copy(record))) { + if (pico_tree_insert(tree, copy)) + pico_mdns_record_delete((void **)©); + } +} + +/* **************************************************************************** + * When a service is found additional records should be generated and added to + * the either the answer section or the additional sections. This happens here + * + * @param antree mDNS record tree with answers to send + * @param artree mDNS record tree with additionals to send + * @param srv_record Found SRV record in the answers + * @return 0 When additional records are properly generated + * ****************************************************************************/ +static int +pico_mdns_gather_service_meta( pico_mdns_rtree *antree, + pico_mdns_rtree *artree, + struct pico_mdns_record *srv_record ) +{ + struct pico_mdns_record *ptr_record = NULL, *meta_record = NULL; + char *sin = NULL, *service = NULL; + uint32_t ttl = 0; + + if (!antree || !artree || !srv_record) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* Add hostname records */ + pico_mdns_additionals_add_host(artree); + + /* Generate proper service instance name and service */ + if (!(sin = pico_dns_qname_to_url(srv_record->record->rname))) + return -1; + + service = sin + pico_dns_first_label_length(sin) + 1u; + ttl = long_be(srv_record->record->rsuffix->rttl); + + /* Generate PTR records */ + ptr_record = pico_mdns_record_create(service, (void *)sin, + (uint16_t)strlen(sin), + PICO_DNS_TYPE_PTR, + ttl, PICO_MDNS_RECORD_SHARED); + /* Meta DNS-SD record */ + meta_record = pico_mdns_record_create("_services._dns-sd._udp.local", + (void *)service, + (uint16_t)strlen(service), + PICO_DNS_TYPE_PTR, + ttl, PICO_MDNS_RECORD_SHARED); + PICO_FREE(sin); + if (!meta_record || !ptr_record) { + mdns_dbg("Could not generate META or PTR records!\n"); + pico_mdns_record_delete((void **)&ptr_record); + pico_mdns_record_delete((void **)&meta_record); + return -1; + } + + ptr_record->flags |= (PICO_MDNS_RECORD_PROBED | PICO_MDNS_RECORD_CLAIMED); + meta_record->flags |= (PICO_MDNS_RECORD_PROBED | PICO_MDNS_RECORD_CLAIMED); + + /* Add copies to the answer tree */ + pico_rtree_add_copy(antree, meta_record); + pico_rtree_add_copy(antree, ptr_record); + + /* Insert the created service record in MyRecords, if it's already in, destroy them */ + if (meta_record && pico_tree_insert(&MyRecords, meta_record)) { + pico_mdns_record_delete((void **)&meta_record); + } + if (ptr_record && pico_tree_insert(&MyRecords, ptr_record)) { + pico_mdns_record_delete((void **)&ptr_record); + } + + return 0; +} + +/* **************************************************************************** + * Gathers additional records for a to send response. Checks for services and + * whether or not there should be NSEC records added to the additional section + * + * @param antree mDNS record tree with answers to send + * @param artree mDNS record tree with additionals to send + * @return Returns 0 when additionals are properly generated and added + * ****************************************************************************/ +static int +pico_mdns_gather_additionals( pico_mdns_rtree *antree, + pico_mdns_rtree *artree ) +{ + struct pico_tree_node *node = NULL; + struct pico_mdns_record *record = NULL; + int ret = 0; + + /* Check params */ + if (!antree || !artree) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* Look for SRV records in the answer tree */ + pico_tree_foreach(node, antree) { + if ((record = node->keyValue) && + short_be(record->record->rsuffix->rtype) == PICO_DNS_TYPE_SRV && + (ret = pico_mdns_gather_service_meta(antree, artree, record))) + return ret; + } + + /* Look for unique records in the answer tree to generate NSEC records */ + pico_tree_foreach(node, antree) { + if ((record = node->keyValue) && IS_UNIQUE_RECORD(record) && + (ret = pico_mdns_additionals_add_nsec(artree, + record->record->rname))) + return ret; + } + + /* Look for unique records in the additional tree to generate NSEC records*/ + pico_tree_foreach(node, artree) { + if ((record = node->keyValue) && IS_UNIQUE_RECORD(record) && + (ret = pico_mdns_additionals_add_nsec(artree, + record->record->rname))) + return ret; + } + + return 0; +} + +/* **************************************************************************** + * Sends mDNS records to either multicast peer via unicast to a single peer. + * + * @param antree Tree with mDNS records to send as answers + * @param peer IPv4-address of peer who this host has RCVD a packet. + * @return 0 when answers are properly handled, something else otherwise. + * ****************************************************************************/ +static int +pico_mdns_reply( pico_mdns_rtree *antree, struct pico_ip4 peer ) +{ + PICO_DNS_RTREE_DECLARE(antree_m); + PICO_DNS_RTREE_DECLARE(antree_u); + PICO_MDNS_RTREE_DECLARE(artree); + PICO_DNS_RTREE_DECLARE(artree_dummy); + PICO_DNS_RTREE_DECLARE(artree_dns); + + /* Try to gather additionals for the to send response */ + if (pico_mdns_gather_additionals(antree, &artree)) { + mdns_dbg("Could not gather additionals properly!\n"); + return -1; + } + + /* Sort the answers into multicast and unicast answers */ + pico_mdns_sort_unicast_multicast(antree, &antree_u, &antree_m); + + /* Convert the mDNS additional tree to a DNS additional tree to send with + * the the unicast AND the multicast response */ + pico_mdns_sort_unicast_multicast(&artree, &artree_dummy, &artree_dns); + + /* Send response via unicast */ + if (pico_mdns_unicast_reply(&antree_u, &artree_dns, peer)) { + mdns_dbg("Could not sent reply via unicast!\n"); + return -1; + } + + /* Send response via multicast */ + if (pico_mdns_multicast_reply(&antree_m, &artree_dns)) { + mdns_dbg("Could not sent reply via multicast!\n"); + return -1; + } + + pico_tree_destroy(&antree_m, NULL); + pico_tree_destroy(&antree_u, NULL); + pico_tree_destroy(&artree_dummy, NULL); + pico_tree_destroy(&artree_dns, NULL); + PICO_MDNS_RTREE_DESTROY(&artree); + + return 0; +} + +/* **************************************************************************** + * Parses DNS records from a plain chunk of data and looks for them in the + * answer tree. If they're found, they will be removed from the tree. + * + * @param rtree Tree to look in for known answers + * @param packet DNS packet in which to look for known answers + * @param ancount Amount of answers in the DNS packet + * @param data Answer section of the DNS packet as a flat chunk of memory. + * @return 0 K.A.S. could be properly applied, something else when not. + * ****************************************************************************/ +static int +pico_mdns_apply_k_a_s( pico_mdns_rtree *rtree, + pico_dns_packet *packet, + uint16_t ancount, + uint8_t **data ) +{ + struct pico_tree_node *node = NULL, *next = NULL; + struct pico_mdns_record *record = NULL, ka = { + 0 + }; + struct pico_dns_record answer = { + 0 + }; + uint16_t i = 0; + + /* Check params */ + if ((!data) || !rtree || !packet || !(*data)) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + for (i = 0; i < ancount; i++) { + /* Set rname of the record to the correct location */ + answer.rname = (char *)(*data); + + /* Set rsuffix of the record to the correct location */ + answer.rsuffix = (struct pico_dns_record_suffix *) + (answer.rname + pico_dns_namelen_comp(answer.rname) + 1u); + + /* Set rdata of the record to the correct location */ + answer.rdata = (uint8_t *) answer.rsuffix + + sizeof(struct pico_dns_record_suffix); + + pico_dns_record_decompress(&answer, packet); + ka.record = &answer; + + /* If the answer is in the record vector */ + pico_tree_foreach_safe(node, rtree, next) { + if ((record = node->keyValue)) { + if (pico_mdns_record_cmp(record, &ka) == 0) + record = pico_tree_delete(rtree, record); + } + } + PICO_FREE(ka.record->rname); + ka.record = NULL; + + /* Move to next record */ + *data = (uint8_t *) answer.rdata + short_be(answer.rsuffix->rdlength); + } + return 0; +} + +/* **************************************************************************** + * Handles a single incoming query packet. Applies Known Answer Suppression + * after handling as well. + * + * @param packet Received packet + * @param peer IPv4 address of the peer who sent the received packet. + * @return Returns 0 when the query packet is properly handled. + * ****************************************************************************/ +static int +pico_mdns_handle_query_packet( pico_dns_packet *packet, struct pico_ip4 peer ) +{ + PICO_MDNS_RTREE_DECLARE(antree); + uint16_t qdcount = 0, ancount = 0; + uint8_t *data = NULL; + + /* Move to the data section of the packet */ + data = (uint8_t *)packet + sizeof(struct pico_dns_header); + + /* Generate a list of answers */ + qdcount = short_be(packet->qdcount); + antree = pico_mdns_handle_data_as_questions(&data, qdcount, packet); + if (pico_tree_count(&antree) == 0) { + mdns_dbg("No records found that correspond with this query!\n"); + return 0; + } + + /* Apply Known Answer Suppression */ + ancount = short_be(packet->ancount); + if (pico_mdns_apply_k_a_s(&antree, packet, ancount, &data)) { + mdns_dbg("Could not apply known answer suppression!\n"); + return -1; + } + + /* Try to reply with the left-over answers */ + pico_mdns_reply(&antree, peer); + PICO_MDNS_RTREE_DESTROY(&antree); + + return 0; +} + +/* **************************************************************************** + * Handles a single incoming probe packet. Checks for Simultaneous Probe + * Tiebreaking as well. + * + * @param packet Received probe packet. + * @param peer IPv4 address of the peer who sent the probe packet. + * @return Returns 0 when the probe packet is properly handled. + * ****************************************************************************/ +static int +pico_mdns_handle_probe_packet( pico_dns_packet *packet, struct pico_ip4 peer ) +{ + PICO_MDNS_RTREE_DECLARE(antree); + uint16_t qdcount = 0, nscount = 0; + uint8_t *data = NULL; + + /* Move to the data section of the packet */ + data = (uint8_t *)packet + sizeof(struct pico_dns_header); + + /* Generate a list of answers */ + qdcount = short_be(packet->qdcount); + antree = pico_mdns_handle_data_as_questions(&data, qdcount, packet); + + /* Check for Simultaneous Probe Tiebreaking */ + nscount = short_be(packet->nscount); + pico_mdns_handle_data_as_answers_generic(&data, nscount, packet, 1); + + /* Try to reply with the answers */ + if (pico_tree_count(&antree) != 0){ + int retval = pico_mdns_reply(&antree, peer); + PICO_MDNS_RTREE_DESTROY(&antree); + return retval; + } + + return 0; +} + +/* **************************************************************************** + * Handles a single incoming answer packet. + * + * @param packet Received answer packet. + * @return Returns 0 when the response packet is properly handled. + * ****************************************************************************/ +static int +pico_mdns_handle_response_packet( pico_dns_packet *packet ) +{ + uint8_t *data = NULL; + uint16_t ancount = 0; + + /* Move to the data section of the packet */ + data = (uint8_t *)packet + sizeof(struct pico_dns_header); + + /* Generate a list of answers */ + ancount = short_be(packet->ancount); + if (pico_mdns_handle_data_as_answers_generic(&data, ancount, packet, 0)) { + mdns_dbg("Could not handle data as answers\n"); + return -1; + } + + return 0; +} + +/* **************************************************************************** + * Parses an incoming packet and handles it according to the type of the + * packet. Packet type determination happens in this function. + * + * @param buf Memory buffer containing the received packet + * @param buflen Length in bytes of the memory buffer + * @param peer IPv4 address of the peer who sent the received packet. + * @return 0 when the packet is properly handled. Something else when it's not + * ****************************************************************************/ +static int +pico_mdns_recv( void *buf, int buflen, struct pico_ip4 peer ) +{ + pico_dns_packet *packet = (pico_dns_packet *) buf; + uint16_t qdcount = short_be(packet->qdcount); + uint16_t ancount = short_be(packet->ancount); + uint16_t authcount = short_be(packet->nscount); + uint16_t addcount = short_be(packet->arcount); + + mdns_dbg(">>>>>>> QDcount: %u, ANcount: %u, NScount: %u, ARcount: %u\n", + qdcount, ancount, authcount, addcount); + + IGNORE_PARAMETER(buflen); + IGNORE_PARAMETER(addcount); + + /* DNS PACKET TYPE DETERMINATION */ + if ((qdcount > 0)) { + if (authcount > 0) { + mdns_dbg(">>>>>>> RCVD a mDNS probe query:\n"); + /* Packet is probe query */ + if (pico_mdns_handle_probe_packet(packet, peer) < 0) { + mdns_dbg("Could not handle mDNS probe query!\n"); + return -1; + } + } else { + mdns_dbg(">>>>>>> RCVD a plain mDNS query:\n"); + /* Packet is a plain query */ + if (pico_mdns_handle_query_packet(packet, peer) < 0) { + mdns_dbg("Could not handle plain DNS query!\n"); + return -1; + } + } + } else { + if (ancount > 0) { + mdns_dbg(">>>>>>> RCVD a mDNS response:\n"); + /* Packet is a response */ + if (pico_mdns_handle_response_packet(packet) < 0) { + mdns_dbg("Could not handle DNS response!\n"); + return -1; + } + } else { + /* Something went wrong here... */ + mdns_dbg("RCVD Packet contains no questions or answers...\n"); + return -1; + } + } + + return 0; +} + +/* **************************************************************************** + * picoTCP callback for UDP IPv4 Socket events + * + * @param ev Determination of the occurred event + * @param s Socket on which the event occurred + * ****************************************************************************/ +static void +pico_mdns_event4( uint16_t ev, struct pico_socket *s ) +{ + char *recvbuf = NULL; + struct pico_ip4 peer = { + 0 + }; + int pico_read = 0; + uint16_t port = 0; + + /* process read event, data available */ + if (ev == PICO_SOCK_EV_RD) { + mdns_dbg("\n>>>>>>> READ EVENT! <<<<<<<\n"); + recvbuf = PICO_ZALLOC(PICO_MDNS_MAXBUF); + if (!recvbuf) { + pico_err = PICO_ERR_ENOMEM; + return; + } + + /* Receive while data is available in socket buffer */ + while((pico_read = pico_socket_recvfrom(s, recvbuf, PICO_MDNS_MAXBUF, + &peer, &port)) > 0) { + /* Handle the MDNS data received */ + pico_mdns_recv(recvbuf, pico_read, peer); + } + PICO_FREE(recvbuf); + mdns_dbg(">>>>>>>>>>>>>><<<<<<<<<<<<<\n\n"); + } else + mdns_dbg("Socket Error received. Bailing out.\n"); +} + +/* MARK: ADDRESS RESOLUTION */ + +/* **************************************************************************** + * Send a mDNS query packet on the wire. This is scheduled with a pico_timer- + * event. + * + * @param now Ignore + * @param arg Void-pointer to query-cookie + * ****************************************************************************/ +static void +pico_mdns_send_query_packet( pico_time now, void *arg ) +{ + struct pico_mdns_cookie *cookie = (struct pico_mdns_cookie *)arg; + pico_dns_qtree *questions = NULL; + pico_dns_packet *packet = NULL; + uint16_t len = 0; + + IGNORE_PARAMETER(now); + + /* Parse in the cookie */ + cookie = (struct pico_mdns_cookie *)arg; + if (!cookie || cookie->type != PICO_MDNS_PACKET_TYPE_QUERY) + return; + + /* Create DNS query packet */ + questions = &(cookie->qtree); + if (!(packet = pico_dns_query_create(questions, NULL, NULL, NULL, &len))) { + mdns_dbg("Could not create query packet!\n"); + return; + } + + packet->id = 0; + + if (cookie->status != PICO_MDNS_COOKIE_STATUS_CANCELLED) { + cookie->status = PICO_MDNS_COOKIE_STATUS_ACTIVE; + if(pico_mdns_send_packet(packet, len) != (int)len) { + mdns_dbg("Send error occurred!\n"); + return; + } + + mdns_dbg("DONE - Sent query.\n"); + } else { + mdns_dbg("DONE - Duplicate query suppressed.\n"); + pico_timer_cancel(cookie->send_timer); + /* Remove cookie from Cookies */ + cookie = pico_tree_delete(&Cookies, cookie); + pico_mdns_cookie_delete(&cookie); + } + PICO_FREE(packet); +} + +/* **************************************************************************** + * Generates a mDNS query packet and schedules a sending on the wire. + * + * @param url URL for the name of the question contained in the query + * @param type DNS type of the question contained in the query + * @param callback Callback to call when a response on this query is RCVD. + * @return 0 When the query is successfully generated and scheduled for sending + * ****************************************************************************/ +static int +pico_mdns_getrecord_generic( const char *url, uint16_t type, + void (*callback)(pico_mdns_rtree *, + char *, + void *), + void *arg) +{ + struct pico_mdns_cookie *cookie = NULL; + PICO_DNS_QTREE_DECLARE(qtree); + PICO_MDNS_RTREE_DECLARE(antree); + PICO_MDNS_RTREE_DECLARE(artree); + struct pico_dns_question *q = NULL; + uint16_t l = 0; + + /* Create a single question and add it to the tree */ + q = pico_mdns_question_create(url, &l, PICO_PROTO_IPV4, type, 0, 0); + if (!q) { + mdns_dbg("question_create returned NULL!\n"); + return -1; + } + pico_tree_insert(&qtree, q); + + /* Create a mDNS cookie to send */ + if (!(cookie = pico_mdns_cookie_create(qtree, antree, artree, 1, + PICO_MDNS_PACKET_TYPE_QUERY, + callback, arg))) { + PICO_DNS_QTREE_DESTROY(&qtree); + mdns_dbg("cookie_create returned NULL!\n"); + return -1; + } + + /* Add cookie to Cookies to be able to find it afterwards */ + pico_tree_insert(&Cookies, cookie); + /* Create new pico_timer-event to send packet */ + pico_timer_add((pico_rand() % 120) + 20, pico_mdns_send_query_packet, + (void *)cookie); + return 0; +} + +/* **************************************************************************** + * API-call to query a record with a certain URL and type. First checks the + * Cache for this record. If no cache-entry is found, a query will be sent on + * the wire for this record. + * + * @param url URL to query for. + * @param type DNS type top query for. + * @param callback Callback to call when records are found for the query. + * @return 0 when query is correctly parsed, something else on failure. + * ****************************************************************************/ +int +pico_mdns_getrecord( const char *url, uint16_t type, + void (*callback)(pico_mdns_rtree *, + char *, + void *), + void *arg ) +{ +#if PICO_MDNS_ALLOW_CACHING == 1 + PICO_MDNS_RTREE_DECLARE(cache_hits); + char *name = NULL; +#endif + + /* Check params */ + if (!url) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* First, try to find records in the cache */ +#if PICO_MDNS_ALLOW_CACHING == 1 + name = pico_dns_url_to_qname(url); + cache_hits = pico_mdns_rtree_find_name_type(&Cache, name, type, 0); + PICO_FREE(name); + if (pico_tree_count(&cache_hits) > 0) { + mdns_dbg("CACHE HIT! Passed cache records to callback.\n"); + callback(&cache_hits, NULL, arg); + } else { +#endif + mdns_dbg("CACHE MISS! Trying to resolve URL '%s'...\n", url); + return pico_mdns_getrecord_generic(url, type, callback, arg); +#if PICO_MDNS_ALLOW_CACHING == 1 +} +#endif + + return 0; +} + +/* MARK: PROBING & ANNOUNCING */ + +/* **************************************************************************** + * Function to create an announcement from an mDNS cookie and send it on the + * wire. + * + * @param now Ignore + * @param arg Void-pointer to mDNS announcement cookie + * ****************************************************************************/ +static void +pico_mdns_send_announcement_packet( pico_time now, void *arg ) +{ + struct pico_mdns_cookie *cookie = (struct pico_mdns_cookie *)arg; + + /* Check params */ + IGNORE_PARAMETER(now); + if (!cookie) + return; + + cookie->status = PICO_MDNS_COOKIE_STATUS_ACTIVE; + if (cookie->count > 0) { + /* Send the announcement on the wire */ + pico_mdns_reply(&(cookie->antree), inaddr_any); + mdns_dbg("DONE - Sent announcement!\n"); + + /* The host needs to send 2 announcements 1 second apart */ + if (--(cookie->count) == 0) { + cookie->status = PICO_MDNS_COOKIE_STATUS_INACTIVE; + + /* Update the states of the records */ + pico_mdns_my_records_claimed(cookie->antree, + cookie->callback, + cookie->arg); + + /* Try to delete the cookie */ + pico_tree_delete(&Cookies, cookie); + pico_mdns_cookie_delete(&cookie); + } + else /* Announcement should be sent with a delay of 1s in between. */ + pico_timer_add(1000, pico_mdns_send_announcement_packet, cookie); + } +} + +/* **************************************************************************** + * Announces all 'my records' which passed the probing-step or just shared + * records. + * + * @param callback Gets called when all records in the cookie are announced. + * @return 0 When the host successfully started announcing. + * ****************************************************************************/ +static int +pico_mdns_announce( void (*callback)(pico_mdns_rtree *, + char *, + void *), + void *arg ) +{ + struct pico_mdns_cookie *announcement_cookie = NULL; + PICO_DNS_QTREE_DECLARE(qtree); + PICO_MDNS_RTREE_DECLARE(antree); + PICO_MDNS_RTREE_DECLARE(artree); + + /* Check params */ + if (!callback) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + IGNORE_PARAMETER(arg); + + /* Find out which resource records can be announced */ + antree = pico_mdns_my_records_find_probed(); + if (pico_tree_count(&antree) == 0) + return 0; + + /* Create a mDNS packet cookie */ + if (!(announcement_cookie = pico_mdns_cookie_create(qtree, antree, artree, + PICO_MDNS_ANNOUNCEMENT_COUNT, + PICO_MDNS_PACKET_TYPE_ANNOUNCEMENT, + callback, arg))) { + mdns_dbg("cookie_create returned NULL!\n"); + PICO_MDNS_RTREE_DESTROY(&antree); + return -1; + } + + /* Send a first unsolicited announcement */ + pico_mdns_send_announcement_packet(0, announcement_cookie); + mdns_dbg("DONE - Started announcing.\n"); + + return 0; +} + +/* **************************************************************************** + * Makes sure the cache flush bit of the to probe records is cleared, and + * generates a DNS record tree to insert in the Authority Section of the DNS + * packet + * + * @param records mDNS records to probe. + * @return DNS record tree to with actual DNS records to insert in Authority + * Section of probe packet. + * ****************************************************************************/ +static pico_dns_rtree +pico_mdns_gen_probe_auths( pico_mdns_rtree *records ) +{ + PICO_DNS_RTREE_DECLARE(nstree); + struct pico_tree_node *node = NULL; + struct pico_mdns_record *record = NULL; + + pico_tree_foreach(node, records) { + if ((record = node->keyValue) && record->record) { + /* Clear the cache flush bit for authority records in probes */ + PICO_MDNS_CLR_MSB_BE(record->record->rsuffix->rclass); + /* Only the actual DNS records is required */ + pico_tree_insert(&nstree, record->record); + } + } + + return nstree; +} + +/* **************************************************************************** + * Function to create a probe form an mDNS cookie and send it on the wire. + * + * @param now Ignore + * @param arg Void-pointer to mDNS probe cookie + * ****************************************************************************/ +static void +pico_mdns_send_probe_packet( pico_time now, void *arg ) +{ + struct pico_mdns_cookie *cookie = (struct pico_mdns_cookie *)arg; + pico_dns_packet *packet = NULL; + PICO_DNS_RTREE_DECLARE(nstree); + uint16_t len = 0; + + /* Check params */ + IGNORE_PARAMETER(now); + if (!cookie || (cookie->type == PICO_MDNS_COOKIE_STATUS_INACTIVE)) { + pico_err = PICO_ERR_EINVAL; + return; + } + + /* Set the cookie to the active state */ + cookie->status = PICO_MDNS_COOKIE_STATUS_ACTIVE; + if (cookie->count-- > 0) { + /* Generate authority records */ + nstree = pico_mdns_gen_probe_auths(&(cookie->antree)); + + /* Create an mDNS answer */ + if (!(packet = pico_dns_query_create(&(cookie->qtree), NULL, + &nstree, NULL, &len))) { + PICO_DNS_RTREE_DESTROY(&nstree); + mdns_dbg("Could not create probe packet!\n"); + return; + } + pico_tree_destroy(&nstree, NULL); + + /* Send the mDNS answer unsolicited via multicast */ + if(pico_mdns_send_packet(packet, len) != (int)len) { + mdns_dbg("Send error occurred!\n"); + return; + } + PICO_FREE(packet); + + mdns_dbg("DONE - Sent probe!\n"); + + /* Probes should be sent with a delay in between of 250 ms */ + if (PICO_MDNS_COOKIE_STATUS_ACTIVE == cookie->status ) + cookie->send_timer = pico_timer_add(250, + pico_mdns_send_probe_packet, + (void *)cookie); + } else { + mdns_dbg("DONE - Probing.\n"); + + pico_mdns_my_records_probed(&(cookie->antree)); + + /* Start announcing */ + cookie->count = PICO_MDNS_ANNOUNCEMENT_COUNT; + cookie->type = PICO_MDNS_PACKET_TYPE_ANNOUNCEMENT; + pico_mdns_send_announcement_packet(0, (void*) cookie); + } + + + return; +} + +/* **************************************************************************** + * Adds a new probe question to the probe cookie questions, if a probe question + * for a new is already present in the question-tree, it will not be generated + * and inserted again + * + * @param qtree Probe question tree + * @param name Name for which the function has to create a probe question + * @return 0 when the probe question is already present or added successfully. + * ****************************************************************************/ +static int +pico_mdns_add_probe_question( pico_dns_qtree *qtree, + char *name ) +{ + struct pico_dns_question *new = NULL; + char *url = NULL; + uint16_t qlen = 0; + uint8_t flags = PICO_MDNS_QUESTION_FLAG_PROBE; + +#if PICO_MDNS_PROBE_UNICAST == 1 + flags |= PICO_MDNS_QUESTION_FLAG_UNICAST_RES; +#endif + + /* Convert name to URL and try to create a new probe question */ + if (!(url = pico_dns_qname_to_url(name))) + return -1; + + mdns_dbg("Probe question for URL: %s\n", url); + if (!(new = pico_mdns_question_create(url, &qlen, PICO_PROTO_IPV4, + PICO_DNS_TYPE_ANY, flags, 0))) { + PICO_FREE(url); + return -1; + } + + PICO_FREE(url); + + /* Try to find an existing question in the vector */ + if (pico_tree_insert(qtree, new)) + pico_dns_question_delete((void **)&new); + + return 0; +} + +/* **************************************************************************** + * Find any of my record that need to be probed and try to probe them. + * + * @param callback Callback to call when all records are properly registered + * @return When host successfully started probing. + * ****************************************************************************/ +static int pico_mdns_probe( void (*callback)(pico_mdns_rtree *, + char *, + void *), + void *arg ) +{ + struct pico_mdns_cookie *cookie = NULL; + struct pico_mdns_record *record = NULL; + struct pico_tree_node *node = NULL; + PICO_DNS_QTREE_DECLARE(qtree); + PICO_MDNS_RTREE_DECLARE(antree); + PICO_MDNS_RTREE_DECLARE(artree); + + /* Check params */ + if (!callback) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* Find my records that need to pass the probing step first */ + antree = pico_mdns_my_records_find_to_probe(); + + /* Crate probe questions for the to probe records */ + pico_tree_foreach(node, &antree) { + if ((record = node->keyValue)) { + pico_mdns_add_probe_question(&qtree, record->record->rname); + } + } + + /* Create a mDNS packet to send */ + cookie = pico_mdns_cookie_create(qtree, antree, artree, + PICO_MDNS_PROBE_COUNT, + PICO_MDNS_PACKET_TYPE_PROBE, + callback, arg); + if (!cookie) { + mdns_dbg("Cookie_create returned NULL @ probe()!\n"); + PICO_DNS_QTREE_DESTROY(&qtree); + PICO_MDNS_RTREE_DESTROY(&antree); + return -1; + } + + /* Add the probe cookie to the cookie tree */ + if (pico_tree_insert(&Cookies, cookie)) { + pico_mdns_cookie_delete(&cookie); + return -1; + } + + /* When the host is ready to send his probe query he SHOULD delay it's + transmission with a randomly chosen time between 0 and 250 ms. */ + cookie->send_timer = pico_timer_add(pico_rand() % 250, + pico_mdns_send_probe_packet, + (void *)cookie); + mdns_dbg("DONE - Started probing.\n"); + + return 0; +} + +/* MARK: API functions */ + +/* **************************************************************************** + * Claim or reclaim all the mDNS records contain in a tree in one single call + * + * @param rtree mDNS record tree with records to claim + * @param reclaim Whether or not the records in tree should be reclaimed. + * @param callback Callback to call when all records are properly registered + * @return 0 When claiming didn't horribly fail. + * ****************************************************************************/ +static int +pico_mdns_claim_generic( pico_mdns_rtree rtree, + uint8_t reclaim, + void (*callback)(pico_mdns_rtree *, + char *, + void *), + void *arg ) +{ + /* Check if arguments are passed correctly */ + if (!callback) { + mdns_dbg("NULL pointers passed to 'pico_mdns_claim()'!\n"); + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* Check if module is initialised */ + if (!mdns_sock_ipv4) { + mdns_dbg("Socket not initialised, did you call 'pico_mdns_init()'?\n"); + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* 1.) Appending records to 'my records' */ + pico_mdns_my_records_add(&rtree, reclaim); + + /* 2a.) Try to probe any records */ + pico_mdns_probe(callback, arg); + + /* 2b.) Try to announce any records */ + pico_mdns_announce(callback, arg); + + return 0; +} + +/* **************************************************************************** + * Claim all different mDNS records in a tree in a single API-call. All records + * in tree are called in a single new claim-session. + * + * @param rtree mDNS record tree with records to claim + * @param callback Callback to call when all record are properly claimed. + * @return 0 When claiming didn't horribly fail. + * ****************************************************************************/ +int +pico_mdns_claim( pico_mdns_rtree rtree, + void (*callback)(pico_mdns_rtree *, + char *, + void *), + void *arg ) +{ + return pico_mdns_claim_generic(rtree, 0, callback, arg); +} + +/* **************************************************************************** + * Reclaim records when a conflict occurred, claim-session will stay the same + * as the session in which the conflict occurred. + * + * @param rtree mDNS record tree with records to claim + * @param callback Callback to call when all record are properly claimed. + * @return 0 When claiming didn't horribly fail. + * ****************************************************************************/ +static int +pico_mdns_reclaim( pico_mdns_rtree rtree, + void (*callback)(pico_mdns_rtree *, + char *, + void *), + void *arg ) +{ + return pico_mdns_claim_generic(rtree, 1, callback, arg); +} + +/* **************************************************************************** + * Sets the hostname for this machine. Claims automatically a unique A record + * with the IPv4-address of this host. The hostname won't be set directly when + * this functions returns, but only if the claiming of the unique record succ- + * eeded. Init-callback will be called when the hostname-record is successfully + * registered. + * + * @param url URL to set the hostname to. + * @param arg Argument to pass to the init-callback. + * @return 0 when the host started registering the hostname-record successfully, + * Returns something else when it didn't succeeded. + * ****************************************************************************/ +int +pico_mdns_set_hostname( const char *url, void *arg ) +{ + PICO_MDNS_RTREE_DECLARE(rtree); + struct pico_mdns_record *record = NULL; + + /* Check params */ + if (!url) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* Check if module is initialised */ + if (!mdns_sock_ipv4) { + mdns_dbg("mDNS socket not initialised, did you call 'pico_mdns_init()'?\n"); + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* Create an A record for hostname */ + record = pico_mdns_record_create(url, + &(mdns_sock_ipv4->local_addr.ip4.addr), + PICO_SIZE_IP4, PICO_DNS_TYPE_A, + PICO_MDNS_DEFAULT_TTL, + (PICO_MDNS_RECORD_UNIQUE | + PICO_MDNS_RECORD_HOSTNAME)); + if (!record) { + mdns_dbg("Could not create A record for hostname %s!\n", + strerror(pico_err)); + return -1; + } + + /* TODO: Create IPv6 record */ + /* TODO: Create a reverse resolution record */ + + /* Try to claim the record */ + if (pico_tree_insert(&rtree, record)) { + pico_mdns_record_delete((void **)&record); + return -1; + } + + if (pico_mdns_claim(rtree, init_callback, arg)) { + mdns_dbg("Could not claim record for hostname %s!\n", url); + PICO_MDNS_RTREE_DESTROY(&rtree); + return -1; + } + pico_tree_destroy(&rtree, NULL); + + return 0; +} + +/* **************************************************************************** + * Get the hostname for this machine. + * + * @return Returns the hostname for this machine when the module is initialised + * Returns NULL when the module is not initialised. + * ****************************************************************************/ +const char * +pico_mdns_get_hostname( void ) +{ + /* Check if module is initialised */ + if (!mdns_sock_ipv4) { + mdns_dbg("mDNS socket not initialised, did you call 'pico_mdns_init()'?\n"); + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + return (const char *)_hostname; +} + +/* **************************************************************************** + * Initialises the entire mDNS-module and sets the hostname for this machine. + * Sets up the global mDNS socket properly and calls callback when succeeded. + * Only when the module is properly initialised records can be registered on + * the module. + * + * @param hostname_url URL to set the hostname to. + * @param address IPv4-address of this host to bind to. + * @param callback Callback to call when the hostname is registered and + * also the global mDNS module callback. Gets called when + * Passive conflicts occur, so changes in records can be + * tracked in this callback. + * @param arg Argument to pass to the init-callback. + * @return 0 when the module is properly initialised and the host started regis- + * tering the hostname. Returns something else went the host failed + * initialising the module or registering the hostname. + * ****************************************************************************/ +int +pico_mdns_init( const char *hostname, + struct pico_ip4 address, + void (*callback)(pico_mdns_rtree *, + char *, + void *), + void *arg ) +{ + struct pico_ip_mreq mreq4; + uint16_t proto4 = PICO_PROTO_IPV4, port = 0, loop = 0, ttl = 255; + + /* Initialise port */ + port = short_be(mdns_port); + + /* Check callback parameter */ + if(!callback || !hostname) { + mdns_dbg("No callback function supplied!\n"); + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* Open global IPv4 mDNS socket */ + mdns_sock_ipv4 = pico_socket_open(proto4, PICO_PROTO_UDP, &pico_mdns_event4); + if(!mdns_sock_ipv4) { + mdns_dbg("pico_socket_open returned NULL-ptr...\n"); + return -1; + } + + /* Convert the mDNS IPv4 destination address to struct */ + if(pico_string_to_ipv4(PICO_MDNS_DEST_ADDR4, &mreq4.mcast_group_addr.ip4.addr)) { + mdns_dbg("String to IPv4 error\n"); + return -1; + } + + /* Receive data on any network interface */ + mreq4.mcast_link_addr.ip4 = inaddr_any; + + /* Don't want the multicast data to be looped back to the host */ + if(pico_socket_setoption(mdns_sock_ipv4, PICO_IP_MULTICAST_LOOP, &loop)) { + mdns_dbg("socket_setoption PICO_IP_MULTICAST_LOOP failed\n"); + return -1; + } + + /* Tell the stack we're interested in this particular multicast group */ + if(pico_socket_setoption(mdns_sock_ipv4, PICO_IP_ADD_MEMBERSHIP, &mreq4)) { + mdns_dbg("socket_setoption PICO_IP_ADD_MEMBERSHIP failed\n"); + return -1; + } + + /* RFC: + * All multicast responses (including answers sent via unicast) SHOULD + * be send with IP TTL set to 255 for backward-compatibility reasons + */ + if(pico_socket_setoption(mdns_sock_ipv4, PICO_IP_MULTICAST_TTL, &ttl)) { + mdns_dbg("socket_setoption PICO_IP_MULTICAST_TTL failed\n"); + return -1; + } + + /* Bind to mDNS port */ + if (pico_socket_bind(mdns_sock_ipv4, (void *)&address, &port)) { + mdns_dbg("Bind error!\n"); + return -1; + } + + /* Set the global init callback variable */ + init_callback = callback; + pico_timer_add(PICO_MDNS_RR_TTL_TICK, pico_mdns_tick, NULL); + + /* Set the hostname eventually */ + return pico_mdns_set_hostname(hostname, arg); +} + +#endif /* PICO_SUPPORT_MDNS */ diff --git a/ext/picotcp/modules/pico_mdns.h b/ext/picotcp/modules/pico_mdns.h new file mode 100644 index 0000000..d4e6a4e --- /dev/null +++ b/ext/picotcp/modules/pico_mdns.h @@ -0,0 +1,185 @@ +/* **************************************************************************** + * PicoTCP. Copyright (c) 2014 TASS Belgium NV. Some rights reserved. + * See LICENSE and COPYING for usage. + * . + * Author: Toon Stegen, Jelle De Vleeschouwer + * ****************************************************************************/ +#ifndef INCLUDE_PICO_MDNS +#define INCLUDE_PICO_MDNS + +#include "pico_dns_common.h" +#include "pico_tree.h" +#include "pico_ipv4.h" + +/* ********************************* CONFIG ***********************************/ +#define PICO_MDNS_PROBE_UNICAST 1 /* Probe queries as QU-questions */ +#define PICO_MDNS_CONTINUOUS_REFRESH 0 /* Continuously update cache */ +#define PICO_MDNS_ALLOW_CACHING 1 /* Enable caching on this host */ +#define PICO_MDNS_DEFAULT_TTL 120 /* Default TTL of mDNS records */ +#define PICO_MDNS_SERVICE_TTL 120 /* Default TTL of SRV/TXT/PTR/NSEC */ +#define PICO_MDNS_PROBE_COUNT 4 /* Amount of probes to send */ +#define PICO_MDNS_ANNOUNCEMENT_COUNT 3 /* Amount of announcements to send */ +/* ****************************************************************************/ + +#define PICO_MDNS_DEST_ADDR4 "224.0.0.251" + +/* To make mDNS records unique or shared records */ +#define PICO_MDNS_RECORD_UNIQUE 0x00u +#define PICO_MDNS_RECORD_SHARED 0x01u + +/* Flag to check for when records are returned, to determine the hostname */ +#define PICO_MDNS_RECORD_HOSTNAME 0x02u +#define IS_HOSTNAME_RECORD(x) \ + (((x)->flags) & PICO_MDNS_RECORD_HOSTNAME) ? (1) : (0) + +/* --- MDNS resource record --- */ +struct pico_mdns_record +{ + struct pico_dns_record *record; /* DNS Resource Record */ + uint32_t current_ttl; /* Current TTL */ + uint8_t flags; /* Resource Record flags */ + uint8_t claim_id; /* Claim ID number */ +}; + +/* **************************************************************************** + * Compares 2 mDNS records by type, name AND rdata for a truly unique result + * + * @param ra mDNS record A + * @param rb mDNS record B + * @return 0 when records are equal, returns difference when they're not. + * ****************************************************************************/ +int +pico_mdns_record_cmp( void *a, void *b ); + +/* **************************************************************************** + * Deletes a single mDNS resource record. + * + * @param record Void-pointer to mDNS Resource Record. Can be used with pico_- + * tree-destroy. + * @return Returns 0 on success, something else on failure. + * ****************************************************************************/ +int +pico_mdns_record_delete( void **record ); + +/* **************************************************************************** + * Creates a single standalone mDNS resource record with given name, type and + * data to register on the network. + * + * @param url DNS rrecord name in URL format. Will be converted to DNS + * name notation format. + * @param _rdata Memory buffer with data to insert in the resource record. If + * data of record should contain a DNS name, the name in the + * databuffer needs to be in URL-format. + * @param datalen The exact length in bytes of the _rdata-buffer. If data of + * record should contain a DNS name, datalen needs to be + * pico_dns_strlen(_rdata). + * @param rtype DNS type of the resource record to be. + * @param rclass DNS class of the resource record to be. + * @param rttl DNS ttl of the resource record to be. + * @param flags You can specify if the mDNS record should be a shared record + * rather than a unique record. + * @return Pointer to newly created mDNS resource record. + * ****************************************************************************/ +struct pico_mdns_record * +pico_mdns_record_create( const char *url, + void *_rdata, + uint16_t datalen, + uint16_t rtype, + uint32_t rttl, + uint8_t flags ); + + + +/* **************************************************************************** + * Definition of DNS record tree + * ****************************************************************************/ +typedef struct pico_tree pico_mdns_rtree; +#define PICO_MDNS_RTREE_DECLARE(name) \ + pico_mdns_rtree (name) = {&LEAF, pico_mdns_record_cmp} +#define PICO_MDNS_RTREE_DESTROY(rtree) \ + pico_tree_destroy((rtree), pico_mdns_record_delete) +#define PICO_MDNS_RTREE_ADD(tree, record) \ + pico_tree_insert((tree), (record)) + +/* **************************************************************************** + * API-call to query a record with a certain URL and type. First checks the + * Cache for this record. If no cache-entry is found, a query will be sent on + * the wire for this record. + * + * @param url URL to query for. + * @param type DNS type top query for. + * @param callback Callback to call when records are found for the query. + * @return 0 when query is correctly parsed, something else on failure. + * ****************************************************************************/ +int +pico_mdns_getrecord( const char *url, uint16_t type, + void (*callback)(pico_mdns_rtree *, + char *, + void *), + void *arg ); + +/* **************************************************************************** + * Claim all different mDNS records in a tree in a single API-call. All records + * in tree are called in a single new claim-session. + * + * @param rtree mDNS record tree with records to claim + * @param callback Callback to call when all record are properly claimed. + * @return 0 When claiming didn't horribly fail. + * ****************************************************************************/ +int +pico_mdns_claim( pico_mdns_rtree record_tree, + void (*callback)(pico_mdns_rtree *, + char *, + void *), + void *arg ); + +/* **************************************************************************** + * Sets the hostname for this machine. Claims automatically a unique A record + * with the IPv4-address of this host. The hostname won't be set directly when + * this functions returns, but only if the claiming of the unique record succ- + * eeded. Init-callback will be called when the hostname-record is successfully + * registered. + * + * @param url URL to set the hostname to. + * @param arg Argument to pass to the init-callback. + * @return 0 when the host started registering the hostname-record successfully, + * Returns something else when it didn't succeeded. + * ****************************************************************************/ +int +pico_mdns_set_hostname( const char *url, void *arg ); + +/* **************************************************************************** + * Get the current hostname for this machine. + * + * @return Returns the hostname for this machine when the module is initialised + * Returns NULL when the module is not initialised. + * ****************************************************************************/ +const char * +pico_mdns_get_hostname( void ); + +/* **************************************************************************** + * Initialises the entire mDNS-module and sets the hostname for this machine. + * Sets up the global mDNS socket properly and calls callback when succeeded. + * Only when the module is properly initialised records can be registered on + * the module. + * + * @param hostname URL to set the hostname to. + * @param address IPv4-address of this host to bind to. + * @param callback Callback to call when the hostname is registered and + * also the global mDNS module callback. Gets called when + * Passive conflicts occur, so changes in records can be + * tracked in this callback. + * @param arg Argument to pass to the init-callback. + * @return 0 when the module is properly initialised and the host started regis- + * tering the hostname. Returns something else went the host failed + * initialising the module or registering the hostname. + * ****************************************************************************/ +int +pico_mdns_init( const char *hostname, + struct pico_ip4 address, + void (*callback)(pico_mdns_rtree *, + char *, + void *), + void *arg ); + +#endif /* _INCLUDE_PICO_MDNS */ diff --git a/ext/picotcp/modules/pico_mld.c b/ext/picotcp/modules/pico_mld.c new file mode 100644 index 0000000..67dd11f --- /dev/null +++ b/ext/picotcp/modules/pico_mld.c @@ -0,0 +1,1179 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved. + See LICENSE and COPYING for usage. + + RFC 2710 3019 3590 3810 4604 6636 + + Authors: Roel Postelmans + *********************************************************************/ + +#include "pico_stack.h" +#include "pico_ipv6.h" +#include "pico_mld.h" +#include "pico_config.h" +#include "pico_eth.h" +#include "pico_addressing.h" +#include "pico_frame.h" +#include "pico_tree.h" +#include "pico_device.h" +#include "pico_socket.h" +#include "pico_icmp6.h" +#include "pico_dns_client.h" +#include "pico_mld.h" +#include "pico_constants.h" + +#if defined(PICO_SUPPORT_MLD) && defined(PICO_SUPPORT_IPV6) && defined(PICO_SUPPORT_MCAST) + +#define mld_dbg(...) do {} while(0) +/* MLD groups */ +#define MLD_ALL_HOST_GROUP "FF01:0:0:0:0:0:0:1" +#define MLD_ALL_ROUTER_GROUP "FF01:0:0:0:0:0:0:2" +#define MLDV2_ALL_ROUTER_GROUP "FF02:0:0:0:0:0:0:16" +#define MLD_ROUTER_ALERT_LEN (8) + +uint8_t pico_mld_flag = 0; + +PACKED_STRUCT_DEF mld_message { + uint8_t type; + uint8_t code; + uint16_t crc; + uint16_t max_resp_delay; + uint16_t reserved; + struct pico_ip6 mcast_group; +}; +PACKED_STRUCT_DEF mldv2_group_record { + uint8_t type; + uint8_t aux; + uint16_t nbr_src; + struct pico_ip6 mcast_group; + struct pico_ip6 src[0]; +}; +PACKED_STRUCT_DEF mldv2_report { + uint8_t type; + uint8_t res; + uint16_t crc; + uint16_t res1; + uint16_t nbr_gr; + struct mldv2_group_record record[0]; +}; +PACKED_STRUCT_DEF mldv2_query { + uint8_t type; + uint8_t code; + uint16_t crc; + uint16_t max_resp_delay; + uint16_t res; + struct pico_ip6 mcast_group; + uint8_t rsq; + uint8_t qqic; + uint16_t nbr_src; + struct pico_ip6 source_addr[0]; +}; +typedef int (*mld_callback) (struct mld_parameters *); +static int pico_mld_process_event(struct mld_parameters *p); +static struct mld_parameters *pico_mld_find_parameter(struct pico_ip6 *mcast_link, struct pico_ip6 *mcast_group); + +static uint8_t *pico_mld_fill_hopbyhop(struct pico_ipv6_hbhoption *hbh) { + uint8_t *p; + if(hbh == NULL) + return NULL; + hbh->type = PICO_PROTO_ICMP6; + hbh->len=0; + // ROUTER ALERT, RFC2711 + p = (uint8_t *)hbh + sizeof(struct pico_ipv6_hbhoption); + *(p++) = PICO_IPV6_EXTHDR_OPT_ROUTER_ALERT; + *(p++) = PICO_IPV6_EXTHDR_OPT_ROUTER_ALERT_DATALEN; + *(p++) = 0; + *(p++) = 0; + //PadN allignment with N=2 + *(p++) = 1; + *(p++) = 0; /* N-2 */ + return p; +} +static int pico_mld_check_hopbyhop(struct pico_ipv6_hbhoption *hbh) { + uint8_t options[8] = { PICO_PROTO_ICMP6, 0, PICO_IPV6_EXTHDR_OPT_ROUTER_ALERT,\ + PICO_IPV6_EXTHDR_OPT_ROUTER_ALERT_DATALEN,0,0,1,0 }; + int i; + uint8_t *p; + if(hbh == NULL) + return -1; + if(hbh->type != options[0] || hbh->len != options[1]) + return -1; + + p = (uint8_t *)hbh + sizeof(struct pico_ipv6_hbhoption); + for(i=0; itype < b->type) + return -1; + + if (a->type > b->type) + return 1; + + return 0; +} +static inline int mldt_group_compare(struct mld_timer *a, struct mld_timer *b) { + return pico_ipv6_compare(&a->mcast_group, &b->mcast_group); +} + +static inline int mldt_link_compare(struct mld_timer *a, struct mld_timer *b) { + return pico_ipv6_compare(&a->mcast_link, &b->mcast_link); +} +static int mld_timer_cmp(void *ka, void *kb) { + struct mld_timer *a = ka, *b = kb; + int cmp = mldt_type_compare(a, b); + if (cmp) + return cmp; + + cmp = mldt_group_compare(a, b); + if (cmp) + return cmp; + + return mldt_link_compare(a, b); +} +static void pico_mld_report_expired(struct mld_timer *t) { + struct mld_parameters *p = NULL; + + p = pico_mld_find_parameter(&t->mcast_link, &t->mcast_group); + if (!p) + return; + + p->event = MLD_EVENT_TIMER_EXPIRED; + pico_mld_process_event(p); +} +PICO_TREE_DECLARE(MLDTimers, mld_timer_cmp); +static void pico_mld_v1querier_expired(struct mld_timer *t) +{ + struct pico_ipv6_link *link = NULL; + struct pico_tree_node *index = NULL, *_tmp = NULL; + + link = pico_ipv6_link_by_dev(t->f->dev); + if (!link) + return; + + /* When changing compatibility mode, cancel all pending response + * and retransmission timers. + */ + pico_tree_foreach_safe(index, &MLDTimers, _tmp) + { + ((struct mld_timer *)index->keyValue)->stopped = MLD_TIMER_STOPPED; + pico_tree_delete(&MLDTimers, index->keyValue); + } + mld_dbg("MLD: switch to compatibility mode MLDv2\n"); + link->mcast_compatibility = PICO_MLDV2; + return; +} + + +static inline int mldparm_group_compare(struct mld_parameters *a, struct mld_parameters *b) { + return pico_ipv6_compare(&a->mcast_group, &b->mcast_group); +} +static inline int mldparm_link_compare(struct mld_parameters *a, struct mld_parameters *b) { + return pico_ipv6_compare(&a->mcast_link, &b->mcast_link); +} +static int mld_parameters_cmp(void *ka, void *kb) { + struct mld_parameters *a = ka, *b = kb; + int cmp = mldparm_group_compare(a, b); + if (cmp) + return cmp; + + return mldparm_link_compare(a, b); +} + +PICO_TREE_DECLARE(MLDParameters, mld_parameters_cmp); + +static int pico_mld_delete_parameter(struct mld_parameters *p) { + if (pico_tree_delete(&MLDParameters, p)) + PICO_FREE(p); + else + return -1; + return 0; +} +static void pico_mld_timer_expired(pico_time now, void *arg){ + struct mld_timer *t = NULL, *timer = NULL, test = { + 0 + }; +#ifdef PICO_DEBUG_MLD + char ipstr[PICO_IPV6_STRING] = { + 0 + }, grpstr[PICO_IPV6_STRING] = { + 0 + }; +#endif + + IGNORE_PARAMETER(now); + t = (struct mld_timer *)arg; + test.type = t->type; + test.mcast_link = t->mcast_link; + test.mcast_group = t->mcast_group; +#ifdef PICO_DEBUG_MLD + pico_ipv6_to_string(ipstr, t->mcast_link.addr); + pico_ipv6_to_string(grpstr, t->mcast_group.addr); + mld_dbg("MLD: timer expired for %s link %s type %u, delay %llu\n", grpstr, ipstr, t->type, (uint64_t) t->delay); +#endif + timer = pico_tree_findKey(&MLDTimers, &test); + if (!timer) { + return; + } + if (timer->stopped == MLD_TIMER_STOPPED) { + pico_tree_delete(&MLDTimers, timer); + PICO_FREE(t); + return; + } + if (timer->start + timer->delay < PICO_TIME_MS()) { + pico_tree_delete(&MLDTimers, timer); + if (timer->mld_callback) + timer->mld_callback(timer); + + PICO_FREE(timer); + } else { +#ifdef PICO_DEBUG_MLD + mld_dbg("MLD: restart timer for %s, delay %llu, new delay %llu\n", grpstr, t->delay, (timer->start + timer->delay) - PICO_TIME_MS()); +#endif + pico_timer_add((timer->start + timer->delay) - PICO_TIME_MS(), &pico_mld_timer_expired, timer); + } + return; +} + +static int pico_mld_timer_reset(struct mld_timer *t) { + struct mld_timer *timer = NULL, test = { + 0 + }; +#ifdef PICO_DEBUG_MLD + char grpstr[PICO_IPV6_STRING] = { + 0 + }; + pico_ipv6_to_string(grpstr, t->mcast_group.addr); + mld_dbg("MLD: reset timer for %s, delay %llu\n", grpstr, t->delay); +#endif + test.type = t->type; + test.mcast_link = t->mcast_link; + test.mcast_group = t->mcast_group; + timer = pico_tree_findKey(&MLDTimers, &test); + if (!timer) + return -1; + + *timer = *t; + timer->start = PICO_TIME_MS(); + return 0; +} + +static int pico_mld_timer_start(struct mld_timer *t) { + struct mld_timer *timer = NULL, test = { + 0 + }; +#ifdef PICO_DEBUG_MLD + char ipstr[PICO_IPV6_STRING] = { + 0 + }, grpstr[PICO_IPV6_STRING] = { + 0 + }; + pico_ipv6_to_string(ipstr, t->mcast_link.addr); + pico_ipv6_to_string(grpstr, t->mcast_group.addr); + mld_dbg("MLD: start timer for %s link %s type %u, delay %llu\n", grpstr, ipstr, t->type, t->delay); +#endif + test.type = t->type; + test.mcast_link = t->mcast_link; + test.mcast_group = t->mcast_group; + timer = pico_tree_findKey(&MLDTimers, &test); + if (timer) + return pico_mld_timer_reset(t); + + timer = PICO_ZALLOC(sizeof(struct mld_timer)); + if (!timer) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + *timer = *t; + timer->start = PICO_TIME_MS(); + pico_tree_insert(&MLDTimers, timer); + pico_timer_add(timer->delay, &pico_mld_timer_expired, timer); + return 0; +} + +static int pico_mld_timer_stop(struct mld_timer *t) { + struct mld_timer *timer = NULL, test = { + 0 + }; +#ifdef PICO_DEBUG_MLD + char grpstr[PICO_IPV6_STRING] = { + 0 + }; +#endif + test.type = t->type; + test.mcast_link = t->mcast_link; + test.mcast_group = t->mcast_group; + timer = pico_tree_findKey(&MLDTimers, &test); + if (!timer) + return -1; +#ifdef PICO_DEBUG_MLD + pico_ipv6_to_string(grpstr, timer->mcast_group.addr); + mld_dbg("MLD: stop timer for %s, delay %llu\n", grpstr, timer->delay); +#endif + timer->stopped = MLD_TIMER_STOPPED; + return 0; +} + +static int pico_mld_timer_is_running(struct mld_timer *t) { + struct mld_timer *timer = NULL, test = { + 0 + }; + + test.type = t->type; + test.mcast_link = t->mcast_link; + test.mcast_group = t->mcast_group; + timer = pico_tree_findKey(&MLDTimers, &test); + if (timer) + return 1; + + return 0; +} + +static struct mld_timer *pico_mld_find_timer(uint8_t type, struct pico_ip6 *mcast_link, struct pico_ip6 *mcast_group) { + struct mld_timer test = { + 0 + }; + + test.type = type; + test.mcast_link = *mcast_link; + test.mcast_group = *mcast_group; + return pico_tree_findKey(&MLDTimers, &test); +} + +static int mld_sources_cmp(void *ka, void *kb) { + struct pico_ip6 *a = ka, *b = kb; + return pico_ipv6_compare(a, b); +} + +PICO_TREE_DECLARE(MLDAllow, mld_sources_cmp); +PICO_TREE_DECLARE(MLDBlock, mld_sources_cmp); + +static struct mld_parameters *pico_mld_find_parameter(struct pico_ip6 *mcast_link, struct pico_ip6 *mcast_group) { + struct mld_parameters test = { + 0 + }; + if (!mcast_link || !mcast_group) + return NULL; + test.mcast_link = *mcast_link; + test.mcast_group = *mcast_group; + return pico_tree_findKey(&MLDParameters, &test); +} +static int pico_mld_is_checksum_valid(struct pico_frame *f) { + if( pico_icmp6_checksum(f) == 0) + return 1; + mld_dbg("ICMP6 (MLD) : invalid checksum\n"); + return 0; +} +uint16_t pico_mld_checksum(struct pico_frame *f) { + struct pico_ipv6_pseudo_hdr pseudo; + struct pico_ipv6_hdr *ipv6_hdr = (struct pico_ipv6_hdr *)f->net_hdr; + struct mldv2_report *icmp6_hdr = (struct mldv2_report *)(f->transport_hdr + MLD_ROUTER_ALERT_LEN); + uint16_t len = (uint16_t) (f->transport_len - MLD_ROUTER_ALERT_LEN); + + pseudo.src = ipv6_hdr->src; + pseudo.dst = ipv6_hdr->dst; + pseudo.len = long_be(len); + pseudo.nxthdr = PICO_PROTO_ICMP6; + + pseudo.zero[0] = 0; + pseudo.zero[1] = 0; + pseudo.zero[2] = 0; + return pico_dualbuffer_checksum(&pseudo, sizeof(struct pico_ipv6_pseudo_hdr), icmp6_hdr, len); +} +/* RFC 3810 $8 */ +static int pico_mld_compatibility_mode(struct pico_frame *f) { + struct pico_ipv6_link *link = NULL; + struct mld_timer t = { + 0 + }; + uint16_t datalen; + struct pico_tree_node *index = NULL, *_tmp = NULL; + link = pico_ipv6_link_by_dev(f->dev); + if (!link) + return -1; + + datalen = (uint16_t)(f->buffer_len - PICO_SIZE_IP6HDR - MLD_ROUTER_ALERT_LEN); + if (f->dev->eth) { + datalen = (uint16_t)(datalen - PICO_SIZE_ETHHDR); + } + if( datalen >= 28) { + /* MLDv2 */ + t.type = MLD_TIMER_V2_QUERIER; + if (pico_mld_timer_is_running(&t)) { /* MLDv1 querier present timer still running */ + mld_dbg("Timer is already running\n"); + return -1; + } else { + link->mcast_compatibility = PICO_MLDV2; + mld_dbg("MLD Compatibility: v2\n"); + return 0; + } + } else if( datalen == 24) { + pico_tree_foreach_safe(index, &MLDTimers, _tmp) + { + ((struct mld_timer *)index->keyValue)->stopped = MLD_TIMER_STOPPED; + pico_tree_delete(&MLDTimers, index->keyValue); + } + mld_dbg("MLD: switch to compatibility mode MLDv1\n"); + link->mcast_compatibility = PICO_MLDV1; + t.type = MLD_TIMER_V1_QUERIER; + t.delay =(pico_time) ((MLD_ROBUSTNESS * link->mcast_last_query_interval) + MLD_QUERY_RESPONSE_INTERVAL) * 1000; + t.f = f; + t.mld_callback = pico_mld_v1querier_expired; + pico_mld_timer_start(&t); + } else { + /* invalid query, silently ignored */ + return -1; + } + return 0; +} + +int pico_mld_state_change(struct pico_ip6 *mcast_link, struct pico_ip6 *mcast_group, uint8_t filter_mode, struct pico_tree *_MCASTFilter, uint8_t state) { + struct mld_parameters *p = NULL; + struct pico_ip6 ipv6; + + pico_string_to_ipv6(MLD_ALL_HOST_GROUP, &ipv6.addr[0]); + + if (!memcmp(&mcast_group->addr, &ipv6, sizeof(struct pico_ip6))) + return 0; + + p = pico_mld_find_parameter(mcast_link, mcast_group); + if (!p && state == PICO_MLD_STATE_CREATE) { + p = PICO_ZALLOC(sizeof(struct mld_parameters)); + if (!p) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + if (!mcast_link || !mcast_group) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + p->state = MLD_STATE_NON_LISTENER; + p->mcast_link = *mcast_link; + p->mcast_group = *mcast_group; + pico_tree_insert(&MLDParameters, p); + } else if (!p) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + switch (state) { + case PICO_MLD_STATE_CREATE: + p->event = MLD_EVENT_START_LISTENING; + break; + + case PICO_MLD_STATE_UPDATE: + p->event = MLD_EVENT_UPDATE_GROUP; + break; + + case PICO_MLD_STATE_DELETE: + p->event = MLD_EVENT_STOP_LISTENING; + break; + default: + return -1; + } + p->filter_mode = filter_mode; + p->MCASTFilter = _MCASTFilter; + return pico_mld_process_event(p); +} +/* finite state machine caller */ +static int pico_mld_process_event(struct mld_parameters *p); + +static struct mld_parameters *pico_mld_analyse_packet(struct pico_frame *f) { + struct pico_icmp6_hdr *hdr = (struct pico_icmp6_hdr *) (f->transport_hdr+MLD_ROUTER_ALERT_LEN); + struct pico_ipv6_hdr *ipv6_hdr = (struct pico_ipv6_hdr *) f->net_hdr; + struct pico_ipv6_link *link = NULL; + struct mld_parameters *p = NULL; + struct pico_ip6 mcast_group = {{ + 0 + }}; + struct mld_message *mld_report = (struct mld_message *) hdr; + struct pico_ipv6_exthdr *hbh; + + link = pico_ipv6_link_by_dev(f->dev); + if(!link) + return NULL; + mcast_group = mld_report->mcast_group; + /* Package check */ + if(ipv6_hdr->hop != MLD_HOP_LIMIT) { + mld_dbg("MLD: Hop limit > 1, ignoring frame\n"); + return NULL; + } + hbh = (struct pico_ipv6_exthdr *) (f->transport_hdr); + if(pico_mld_check_hopbyhop((struct pico_ipv6_hbhoption *)hbh) < 0) { + mld_dbg("MLD: Router Alert option is not set\n"); + return NULL; + } + if(!pico_ipv6_is_linklocal(ipv6_hdr->src.addr) || pico_ipv6_is_unspecified(ipv6_hdr->src.addr) ) { + mld_dbg("MLD Source is invalid link-local address\n"); + return NULL; + } + /* end package check */ + p = pico_mld_find_parameter(&link->address, &mcast_group); + if(!p) { + mld_dbg("Alloc-ing MLD parameters\n"); + p = PICO_ZALLOC(sizeof(struct mld_parameters)); + if(!p) + return NULL; + p->state = MLD_STATE_NON_LISTENER; + p->mcast_link = link->address; + pico_tree_insert(&MLDParameters,p); + } + mld_dbg("Analyse package, type = %d\n", hdr->type); + switch(hdr->type) { + case PICO_MLD_QUERY: + p->max_resp_time = mld_report->max_resp_delay; + p->event = MLD_EVENT_QUERY_RECV; + break; + case PICO_MLD_REPORT: + p->event = MLD_EVENT_REPORT_RECV; + break; + case PICO_MLD_DONE: + p->event = MLD_EVENT_DONE_RECV; + break; + case PICO_MLD_REPORTV2: + p->event = MLD_EVENT_REPORT_RECV; + break; + default: + return NULL; + } + p->f = f; + return p; +} +int pico_mld_process_in(struct pico_frame *f) { + struct mld_parameters *p = NULL; + + if (!pico_mld_is_checksum_valid(f)) + goto out; + + if (pico_mld_compatibility_mode(f) < 0) + goto out; + + if((p = pico_mld_analyse_packet(f)) == NULL) + goto out; + + return pico_mld_process_event(p); +out: + mld_dbg("FRAME DISCARD\n"); + pico_frame_discard(f); + return 0; +} + + + +static int8_t pico_mld_send_done(struct mld_parameters *p, struct pico_frame *f) { + struct mld_message *report = NULL; + uint8_t report_type = PICO_MLD_DONE; + struct pico_ipv6_exthdr *hbh; + struct pico_ip6 dst = {{ + 0 + }}; +#ifdef PICO_DEBUG_MLD + char ipstr[PICO_IPV6_STRING] = { + 0 + }, grpstr[PICO_IPV6_STRING] ={ + 0 + }; +#endif + IGNORE_PARAMETER(f); + pico_string_to_ipv6(MLD_ALL_ROUTER_GROUP, &dst.addr[0]); + p->f = pico_proto_ipv6.alloc(&pico_proto_ipv6, sizeof(struct mld_message)+MLD_ROUTER_ALERT_LEN); + p->f->dev = pico_ipv6_link_find(&p->mcast_link); + /* p->f->len is correctly set by alloc */ + hbh = (struct pico_ipv6_exthdr *)(p->f->transport_hdr); + report = (struct mld_message *)(pico_mld_fill_hopbyhop((struct pico_ipv6_hbhoption*)hbh)); + if(!report) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + report->type = report_type; + report->max_resp_delay = 0; + report->mcast_group = p->mcast_group; + + report->crc = 0; + //Checksum done in ipv6 module, no need to do it twice + //report->crc = short_be(pico_icmp6_checksum(p->f)); +#ifdef PICO_DEBUG_MLD + pico_ipv6_to_string(ipstr, dst.addr); + pico_ipv6_to_string(grpstr, mcast_group.addr); + mld_dbg("MLD: send membership done on group %s to %s\n", grpstr, ipstr); +#endif + pico_ipv6_frame_push(p->f, NULL, &dst, 0,0); + return 0; +} +static int pico_mld_send_report(struct mld_parameters *p, struct pico_frame *f) { + struct pico_ip6 dst = {{ + 0 + }}; + struct pico_ip6 mcast_group = {{ + 0 + }}; +#ifdef PICO_DEBUG_MLD + char ipstr[PICO_IPV6_STRING] = { + 0 + }, grpstr[PICO_IPV6_STRING] ={ + 0 + }; +#endif + struct pico_ipv6_link *link = NULL; + link = pico_ipv6_link_get(&p->mcast_link); + if (!link) + return -1; + + mcast_group = p->mcast_group; + switch (link->mcast_compatibility) { + case PICO_MLDV1: + if (p->event == MLD_EVENT_STOP_LISTENING) + pico_string_to_ipv6(MLD_ALL_ROUTER_GROUP, &dst.addr[0]); + else + dst = mcast_group; + break; + case PICO_MLDV2: + pico_string_to_ipv6(MLDV2_ALL_ROUTER_GROUP, &dst.addr[0]); + break; + default: + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; + } +#ifdef PICO_DEBUG_MLD + pico_ipv6_to_string(ipstr, dst.addr); + pico_ipv6_to_string(grpstr, mcast_group.addr); + mld_dbg("MLD: send membership report on group %s to %s\n", grpstr, ipstr); +#endif + pico_ipv6_frame_push(f, NULL, &dst, 0,0); + return 0; +} + +static int8_t pico_mld_generate_report(struct mld_parameters *p) { + struct pico_ipv6_link *link = NULL; + uint8_t i = 0; + link = pico_ipv6_link_get(&p->mcast_link); + if (!link) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + if( !pico_ipv6_is_multicast(p->mcast_group.addr) ) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + switch (link->mcast_compatibility) { + + case PICO_MLDV1: { + struct mld_message *report = NULL; + uint8_t report_type = PICO_MLD_REPORT; + struct pico_ipv6_exthdr *hbh; + p->f = pico_proto_ipv6.alloc(&pico_proto_ipv6, sizeof(struct mld_message)+MLD_ROUTER_ALERT_LEN ); + p->f->dev = pico_ipv6_link_find(&p->mcast_link); + /* p->f->len is correctly set by alloc */ + + hbh = (struct pico_ipv6_exthdr *)(p->f->transport_hdr); + report = (struct mld_message *)(pico_mld_fill_hopbyhop((struct pico_ipv6_hbhoption *)hbh)); + report->type = report_type; + report->max_resp_delay = MLD_DEFAULT_MAX_RESPONSE_TIME; + report->mcast_group = p->mcast_group; + + report->crc = 0; + //Checksum done in ipv6 module, no need to do it twice + //report->crc = short_be(pico_icmp6_checksum(p->f)); + break; + } + case PICO_MLDV2: { + struct mldv2_report *report = NULL; + struct mldv2_group_record *record = NULL; + struct pico_ipv6_mcast_group *g = NULL, test = { + 0 + }; + struct pico_tree_node *index = NULL, *_tmp = NULL; + struct pico_tree *MLDFilter = NULL; + struct pico_ip6 *source = NULL; + struct pico_ipv6_hbhoption *hbh; + uint8_t record_type = 0; + uint8_t sources = 0; + uint16_t len = 0; + test.mcast_addr = p->mcast_group; + g = pico_tree_findKey(link->MCASTGroups, &test); + if (!g) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + /* "non-existent" state of filter mode INCLUDE and empty source list */ + if (p->event == MLD_EVENT_DELETE_GROUP) { + p->filter_mode = PICO_IP_MULTICAST_INCLUDE; + p->MCASTFilter = NULL; + } + if (p->event == MLD_EVENT_QUERY_RECV) + goto mld2_report; + /* cleanup filters */ + pico_tree_foreach_safe(index, &MLDAllow, _tmp) + { + pico_tree_delete(&MLDAllow, index->keyValue); + } + pico_tree_foreach_safe(index, &MLDBlock, _tmp) + { + pico_tree_delete(&MLDBlock, index->keyValue); + } + switch (g->filter_mode) { + + case PICO_IP_MULTICAST_INCLUDE: + switch (p->filter_mode) { + case PICO_IP_MULTICAST_INCLUDE: + /* all ADD_SOURCE_MEMBERSHIP had an equivalent DROP_SOURCE_MEMBERSHIP */ + if (p->event == MLD_EVENT_DELETE_GROUP) { + /* TO_IN (B) */ + record_type = MLD_CHANGE_TO_INCLUDE_MODE; + MLDFilter = &MLDAllow; + if (p->MCASTFilter) { + pico_tree_foreach(index, p->MCASTFilter) /* B */ + { + pico_tree_insert(&MLDAllow, index->keyValue); + sources++; + } + } /* else { MLDAllow stays empty } */ + + break; + } + + /* ALLOW (B-A) */ + /* if event is CREATE A will be empty, thus only ALLOW (B-A) has sense */ + if (p->event == MLD_EVENT_CREATE_GROUP) /* first ADD_SOURCE_MEMBERSHIP */ + record_type = MLD_CHANGE_TO_INCLUDE_MODE; + else + record_type = MLD_ALLOW_NEW_SOURCES; + + MLDFilter = &MLDAllow; + pico_tree_foreach(index, p->MCASTFilter) /* B */ + { + pico_tree_insert(&MLDAllow, index->keyValue); + sources++; + } + pico_tree_foreach(index, &g->MCASTSources) /* A */ + { + source = pico_tree_findKey(&MLDAllow, index->keyValue); + if (source) { + pico_tree_delete(&MLDAllow, source); + sources--; + } + } + if (!pico_tree_empty(&MLDAllow)) /* record type is ALLOW */ + break; + + /* BLOCK (A-B) */ + record_type = MLD_BLOCK_OLD_SOURCES; + MLDFilter = &MLDBlock; + pico_tree_foreach(index, &g->MCASTSources) /* A */ + { + pico_tree_insert(&MLDBlock, index->keyValue); + sources++; + } + pico_tree_foreach(index, p->MCASTFilter) /* B */ + { + source = pico_tree_findKey(&MLDBlock, index->keyValue); + if (source) { + pico_tree_delete(&MLDBlock, source); + sources--; + } + } + if (!pico_tree_empty(&MLDBlock)) /* record type is BLOCK */ + break; + + /* ALLOW (B-A) and BLOCK (A-B) are empty: do not send report */ + p->f = NULL; + return 0; + case PICO_IP_MULTICAST_EXCLUDE: + /* TO_EX (B) */ + record_type = MLD_CHANGE_TO_EXCLUDE_MODE; + MLDFilter = &MLDBlock; + pico_tree_foreach(index, p->MCASTFilter) /* B */ + { + pico_tree_insert(&MLDBlock, index->keyValue); + sources++; + } + break; + default: + pico_err = PICO_ERR_EINVAL; + return -1; + } + break; + case PICO_IP_MULTICAST_EXCLUDE: + switch (p->filter_mode) { + case PICO_IP_MULTICAST_INCLUDE: + /* TO_IN (B) */ + record_type = MLD_CHANGE_TO_INCLUDE_MODE; + MLDFilter = &MLDAllow; + if (p->MCASTFilter) { + pico_tree_foreach(index, p->MCASTFilter) /* B */ + { + pico_tree_insert(&MLDAllow, index->keyValue); + sources++; + } + } /* else { MLDAllow stays empty } */ + + break; + case PICO_IP_MULTICAST_EXCLUDE: + /* BLOCK (B-A) */ + record_type = MLD_BLOCK_OLD_SOURCES; + MLDFilter = &MLDBlock; + pico_tree_foreach(index, p->MCASTFilter) + { + pico_tree_insert(&MLDBlock, index->keyValue); + sources++; + } + pico_tree_foreach(index, &g->MCASTSources) /* A */ + { + source = pico_tree_findKey(&MLDBlock, index->keyValue); /* B */ + if (source) { + pico_tree_delete(&MLDBlock, source); + sources--; + } + } + if (!pico_tree_empty(&MLDBlock)) /* record type is BLOCK */ + break; + /* ALLOW (A-B) */ + record_type = MLD_ALLOW_NEW_SOURCES; + MLDFilter = &MLDAllow; + pico_tree_foreach(index, &g->MCASTSources) + { + pico_tree_insert(&MLDAllow, index->keyValue); + sources++; + } + pico_tree_foreach(index, p->MCASTFilter) /* B */ + { + source = pico_tree_findKey(&MLDAllow, index->keyValue); /* A */ + if (source) { + pico_tree_delete(&MLDAllow, source); + sources--; + } + } + if (!pico_tree_empty(&MLDAllow)) /* record type is ALLOW */ + break; + /* BLOCK (B-A) and ALLOW (A-B) are empty: do not send report */ + p->f = NULL; + return 0; + default: + pico_err = PICO_ERR_EINVAL; + return -1; + } + break; + default: + pico_err = PICO_ERR_EINVAL; + return -1; + } +mld2_report: + /* RFC3810 $5.1.10 */ + if(sources > MLD_MAX_SOURCES) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + len = (uint16_t)(sizeof(struct mldv2_report) + sizeof(struct mldv2_group_record) \ + + (sources * sizeof(struct pico_ip6))+MLD_ROUTER_ALERT_LEN); + + p->f = pico_proto_ipv6.alloc(&pico_proto_ipv6, len); + p->f->dev = pico_ipv6_link_find(&p->mcast_link); + /* p->f->len is correctly set by alloc */ + + hbh = (struct pico_ipv6_hbhoption *) p->f->transport_hdr; + report = (struct mldv2_report *)(pico_mld_fill_hopbyhop(hbh)); + report->type = PICO_MLD_REPORTV2; + report->res = 0; + report->crc = 0; + report->res1 = 0; + report->nbr_gr = short_be(1); + + record = &report->record[0]; + record->type = record_type; + record->aux = 0; + record->nbr_src = short_be(sources); + record->mcast_group = p->mcast_group; + if (MLDFilter && !pico_tree_empty(MLDFilter)) { + i = 0; + pico_tree_foreach(index, MLDFilter) + { + record->src[i] = (*(struct pico_ip6 *)index->keyValue); + i++; + } + } + if(i != sources) + return -1; + //Checksum done in ipv6 module, no need to do it twice + //report->crc= short_be(pico_mld_checksum(p->f)); + break; + } + default: + pico_err = PICO_ERR_EINVAL; + return -1; + } + + return 0; +} +/* stop timer, send done if flag set */ +static int mld_stsdifs(struct mld_parameters *p) { + struct mld_timer t = { + 0 + }; + struct pico_ipv6_link *link = NULL; + struct pico_frame *copy_frame = NULL; + link = pico_ipv6_link_get(&p->mcast_link); + if (!link) + return -1; + + mld_dbg("MLD: event = stop listening | action = stop timer, send done if flag set\n"); + + t.type = MLD_TIMER_GROUP_REPORT; + t.mcast_link = p->mcast_link; + t.mcast_group = p->mcast_group; + if (pico_mld_timer_stop(&t) < 0) + return -1; + switch(link->mcast_compatibility){ + case PICO_MLDV2: + if (pico_mld_generate_report(p) < 0) { + return -1; + } + copy_frame = pico_frame_copy(p->f); + if (!copy_frame) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + if (pico_mld_send_report(p, copy_frame) < 0) { + return -1; + } + break; + case PICO_MLDV1: + /* Send done if flag is set */ + if (pico_mld_flag && pico_mld_send_done(p, p->f) < 0) + return -1; + break; + } + + pico_mld_delete_parameter(p); + mld_dbg("MLD: new state = Non-Listener\n"); + return 0; +} +/* send report, set flag, start timer */ +static int mld_srsfst(struct mld_parameters *p) { + struct mld_timer t = { + 0 + }; + struct pico_frame *copy_frame = NULL; + mld_dbg("MLD: event = start listening | action = send report, set flag, start timer\n"); + + p->last_host = MLD_HOST_LAST; + if (pico_mld_generate_report(p) < 0) + return -1; + + if (!p->f) + return 0; + copy_frame = pico_frame_copy(p->f); + if (!copy_frame) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + if (pico_mld_send_report(p, copy_frame) < 0) + return -1; + + t.type = MLD_TIMER_GROUP_REPORT; + t.mcast_link = p->mcast_link; + t.mcast_group = p->mcast_group; + + t.delay = (pico_rand() % (MLD_UNSOLICITED_REPORT_INTERVAL * 10000)); + t.f = p->f; + t.mld_callback = pico_mld_report_expired; + pico_mld_timer_start(&t); + pico_mld_flag = 1; + p->state = MLD_STATE_DELAYING_LISTENER; + mld_dbg("MLD: new state = Delaying Listener\n"); + return 0; +} + +/* stop timer, clear flag */ +static int mld_stcl(struct mld_parameters *p) { + struct mld_timer t = { + 0 + }; + + mld_dbg("MLD: event = report received | action = stop timer, clear flag\n"); + + t.type = MLD_TIMER_GROUP_REPORT; + t.mcast_link = p->mcast_link; + t.mcast_group = p->mcast_group; + if (pico_mld_timer_stop(&t) < 0) + return -1; + pico_mld_flag = 0; + p->last_host = MLD_HOST_NOT_LAST; + p->state = MLD_STATE_IDLE_LISTENER; + mld_dbg("MLD: new state = Idle Listener\n"); + return 0; +} +/* send report, set flag */ +static int mld_srsf(struct mld_parameters *p) { + mld_dbg("MLD: event = timer expired | action = send report, set flag\n"); + + if (pico_mld_send_report(p, p->f) < 0) + return -1; + pico_mld_flag = 1; + p->state = MLD_STATE_IDLE_LISTENER; + mld_dbg("MLD: new state = Idle Listener\n"); + return 0; +} +/* reset timer if max response time < current timer */ +static int mld_rtimrtct(struct mld_parameters *p) { + struct mld_timer *t = NULL; + uint32_t current_timer = 0; + + mld_dbg("MLD: event = query received | action = reset timer if max response time < current timer\n"); + + t = pico_mld_find_timer(MLD_TIMER_GROUP_REPORT, &p->mcast_link, &p->mcast_group); + if (!t) + return -1; + + current_timer = (uint32_t)(t->start + t->delay - PICO_TIME_MS()); + if ((p->max_resp_time * 100u) < current_timer) { /* max_resp_time in units of 1/10 seconds */ + t->delay = pico_rand() % ((1u + p->max_resp_time) * 100u); + pico_mld_timer_reset(t); + } + p->state = MLD_STATE_DELAYING_LISTENER; + mld_dbg("MLD: new state = Delaying Listener\n"); + return 0; +} +/* merge report, send report, reset timer (MLDv2 only) */ +static int mld_mrsrrt(struct mld_parameters *p) { + struct mld_timer *t = NULL; + struct pico_frame *copy_frame = NULL; + struct pico_ipv6_link *link = NULL; + mld_dbg("MLD: event = update group | action = merge report, send report, reset timer (MLDv2 only)\n"); + + link = pico_ipv6_link_get(&p->mcast_link); + if (!link) + return -1; + + if (link->mcast_compatibility != PICO_MLDV2) { + mld_dbg("MLD: no MLDv3 compatible router on network\n"); + return -1; + } + + /* XXX: merge with pending report rfc 3376 $5.1 */ + + copy_frame = pico_frame_copy(p->f); + if (!copy_frame) + return -1; + + if (pico_mld_send_report(p, copy_frame) < 0) + return -1; + + t = pico_mld_find_timer(MLD_TIMER_GROUP_REPORT, &p->mcast_link, &p->mcast_group); + if (!t) + return -1; + + t->delay = (pico_rand() % (MLD_UNSOLICITED_REPORT_INTERVAL * 10000)); + pico_mld_timer_reset(t); + + p->state = MLD_STATE_DELAYING_LISTENER; + mld_dbg("MLD: new state = delaying member\n"); + return 0; +} + +/* send report, start timer (MLDv2 only) */ +static int mld_srst(struct mld_parameters *p){ + struct mld_timer t = { + 0 + }; + struct pico_frame *copy_frame = NULL; + struct pico_ipv6_link *link = NULL; + + mld_dbg("MLD: event = update group | action = send report, start timer (MLDv2 only)\n"); + + link = pico_ipv6_link_get(&p->mcast_link); + if (!link) + return -1; + + if (link->mcast_compatibility != PICO_MLDV2) { + mld_dbg("MLD: no MLDv2 compatible router on network\n"); + return -1; + } + + if (pico_mld_generate_report(p) < 0) + return -1; + + if (!p->f) + return 0; + + copy_frame = pico_frame_copy(p->f); + if (!copy_frame) + return -1; + + if (pico_mld_send_report(p, copy_frame) < 0) + return -1; + + t.type = MLD_TIMER_GROUP_REPORT; + t.mcast_link = p->mcast_link; + t.mcast_group = p->mcast_group; + t.delay = (pico_rand() % (MLD_UNSOLICITED_REPORT_INTERVAL * 10000)); + t.f = p->f; + t.mld_callback = pico_mld_report_expired; + pico_mld_timer_start(&t); + + p->state = MLD_STATE_DELAYING_LISTENER; + mld_dbg("MLD: new state = delaying member\n"); + return 0; +} +static int mld_discard(struct mld_parameters *p) { + mld_dbg("MLD: ignore and mld_discard frame\n"); + // the frame will be discared bij the ipv6 module!!! + IGNORE_PARAMETER(p); + return 0; +} + +/* finite state machine table */ +static const mld_callback mld_state_diagram[3][6] = +{ /* event | Stop Listening | Start Listening | Update Group |Query reveive |Report receive |Timer expired */ +/* none listener*/ { mld_discard , mld_srsfst, mld_srsfst, mld_discard, mld_discard, mld_discard}, +/* idle listener */ { mld_stsdifs , mld_mrsrrt, mld_mrsrrt, mld_rtimrtct, mld_stcl, mld_srsf }, +/* delaying listener */ { mld_rtimrtct, mld_srst, mld_srst, mld_srsf, mld_stsdifs, mld_discard } +}; + +static int pico_mld_process_event(struct mld_parameters *p) { + struct pico_tree_node *index= NULL; + struct mld_parameters *_p; +#ifdef PICO_DEBUG_MLD + char ipv6[PICO_IPV6_STRING]; + pico_ipv6_to_string(ipv6, p->mcast_group.addr); + mld_dbg("MLD: process event on group address %s\n", ipv6); +#endif + if (p->event == MLD_EVENT_QUERY_RECV && p->general_query) { /* general query */ + pico_tree_foreach(index, &MLDParameters) { + _p = index->keyValue; + _p->max_resp_time = p->max_resp_time; + _p->event = MLD_EVENT_QUERY_RECV; +#ifdef PICO_DEBUG_MLD + mld_dbg("MLD: for each mcast_group = %s | state = %u\n", ipv6, _p->state); +#endif + return mld_state_diagram[_p->state][_p->event](_p); + } + } else { + mld_dbg("MLD: state = %u (0: non-listener - 1: delaying listener - 2: idle listener) event = %u\n", p->state, p->event); + return mld_state_diagram[p->state][p->event](p); + } + return 0; +} +#else +uint16_t pico_mld_checksum(struct pico_frame *f) { + IGNORE_PARAMETER(f); + return 0; +} +int pico_mld_process_in(struct pico_frame *f) { + IGNORE_PARAMETER(f); + return -1; +} + +int pico_mld_state_change(struct pico_ip6 *mcast_link, struct pico_ip6 *mcast_group, uint8_t filter_mode, struct pico_tree *_MCASTFilter, uint8_t state) { + IGNORE_PARAMETER(mcast_link); + IGNORE_PARAMETER(mcast_group); + IGNORE_PARAMETER(filter_mode); + IGNORE_PARAMETER(_MCASTFilter); + IGNORE_PARAMETER(state); + return -1; +} +#endif diff --git a/ext/picotcp/modules/pico_mld.h b/ext/picotcp/modules/pico_mld.h new file mode 100644 index 0000000..219ef4a --- /dev/null +++ b/ext/picotcp/modules/pico_mld.h @@ -0,0 +1,119 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + Authors: Roel Postelmans + *********************************************************************/ + +#ifndef INCLUDE_PICO_MLD +#define INCLUDE_PICO_MLD + +#define PICO_MLDV1 1 +#define PICO_MLDV2 2 + +#define PICO_MLD_QUERY 130 +#define PICO_MLD_REPORT 131 +#define PICO_MLD_DONE 132 +#define PICO_MLD_REPORTV2 143 + +/*RFC 3810 $6.2 */ +#define MLD_HOP_LIMIT 1 + +/* states */ +#define MLD_STATE_NON_LISTENER (0x0) +#define MLD_STATE_DELAYING_LISTENER (0x1) +#define MLD_STATE_IDLE_LISTENER (0x2) + +#define PICO_MLD_STATE_CREATE 1 +#define PICO_MLD_STATE_UPDATE 2 +#define PICO_MLD_STATE_DELETE 3 +/* group record types */ +#define MLD_MODE_IS_INCLUDE (1) +#define MLD_MODE_IS_EXCLUDE (2) +#define MLD_CHANGE_TO_INCLUDE_MODE (3) +#define MLD_CHANGE_TO_EXCLUDE_MODE (4) +#define MLD_ALLOW_NEW_SOURCES (5) +#define MLD_BLOCK_OLD_SOURCES (6) +/* events */ + +#define MLD_EVENT_START_LISTENING (0x1) +#define MLD_EVENT_STOP_LISTENING (0x0) +#define MLD_EVENT_QUERY_RECV (0x3) +#define MLD_EVENT_REPORT_RECV (0x4) +#define MLD_EVENT_TIMER_EXPIRED (0x5) +/*Not needed?*/ +#define MLD_EVENT_DONE_RECV (0x1) + +#define MLD_EVENT_DELETE_GROUP (0x0) +#define MLD_EVENT_CREATE_GROUP (0x1) +#define MLD_EVENT_UPDATE_GROUP (0x2) +#define MLD_EVENT_QUERY_RECV (0x3) +#define MLD_EVENT_REPORT_RECV (0x4) +#define MLD_EVENT_TIMER_EXPIRED (0x5) +/* (default) Variabels for times/counters */ +/* ALL IN SECONDS */ +#define MLD_ROBUSTNESS (2) +#define MLD_QUERY_INTERVAL (125) +#define MLD_QUERY_RESPONSE_INTERVAL (10) +#define MLD_DEFAULT_MAX_RESPONSE_TIME (100) +#define MLD_MULTICAST_LISTENER_INTERVAL (MLD_ROBUSTNESS * MLD_QUERY_INTERVAL) + MLD_QUERY_RESPONSE_INTERVAL +#define MLD_OTHER_QUERIER_PRESENT_INTERVAL (MLD_ROBUSTNESS * MLD_QUERY_INTERVAL) + (0.5 * MLD_QUERY_RESPONSE_INTERVAL) +#define MLD_STARTUP_QUERY_INTERVAL (0.25 * MLD_QUERY_INTERVAL) +#define MLD_STARTUP_QUERY_COUNT MLD_ROBUSTNESS +#define MLD_LAST_LISTENER_QUERY_INTERVAL 1 +#define MLD_LISTENER_QUERY_COUNT MLD_ROBUSTNESS +#define MLD_UNSOLICITED_REPORT_INTERVAL 10 + +/* custom timers types */ +#define MLD_TIMER_GROUP_REPORT (1) +#define MLD_TIMER_V1_QUERIER (2) +#define MLD_TIMER_V2_QUERIER (2) + + +/* Who has send the last report message */ +#define MLD_HOST_LAST (0x1) +#define MLD_HOST_NOT_LAST (0x0) + + +#define MLD_TIMER_STOPPED (1) +#define MLD_MAX_SOURCES (89) +extern struct pico_protocol pico_proto_mld; + +struct mld_multicast_address_record { + uint8_t type; + uint8_t aux_len; + uint16_t nbr_src; + struct pico_ip6 multicast; + struct pico_ip6 src[0]; +}; + +struct mld_parameters { + uint8_t event; + uint8_t state; + uint8_t general_query; + uint8_t filter_mode; + uint8_t last_host; + uint16_t max_resp_time; + struct pico_ip6 mcast_link; + struct pico_ip6 mcast_group; + struct pico_tree *MCASTFilter; + struct pico_frame *f; +}; + +struct mld_timer { + uint8_t type; + uint8_t stopped; + pico_time start; + pico_time delay; + struct pico_ip6 mcast_link; + struct pico_ip6 mcast_group; + struct pico_frame *f; + void (*mld_callback)(struct mld_timer *t); +}; + +uint16_t pico_mld_checksum(struct pico_frame *f); +int pico_mld_process_in(struct pico_frame *f); +int pico_mld_state_change(struct pico_ip6 *mcast_link, struct pico_ip6 *mcast_group, uint8_t filter_mode, struct pico_tree *_MCASTFilter, uint8_t state); +#endif /* _INCLUDE_PICO_MLD */ diff --git a/ext/picotcp/modules/pico_mm.c b/ext/picotcp/modules/pico_mm.c new file mode 100644 index 0000000..5a6517b --- /dev/null +++ b/ext/picotcp/modules/pico_mm.c @@ -0,0 +1,1605 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Gustav Janssens, Jonas Van Nieuwenberg, Sam Van Den Berge + *********************************************************************/ + +#include "pico_config.h" +#include "pico_mm.h" +#include "pico_tree.h" +#include "pico_config.h" +#include "pico_protocol.h" /* For pico_err */ + +#define DBG_MM(x, args ...) /* dbg("[%s:%s:%i] "x" \n",__FILE__,__func__,__LINE__ ,##args ) */ +#define DBG_MM_RED(x, args ...) /* dbg("\033[31m[%s:%s:%i] "x" \033[0m\n",__FILE__,__func__,__LINE__ ,##args ) */ +#define DBG_MM_GREEN(x, args ...) /* dbg("\033[32m[%s:%s:%i] "x" \033[0m\n",__FILE__,__func__,__LINE__ ,##args ) */ +#define DBG_MM_YELLOW(x, args ...) /* dbg("\033[33m[%s:%s:%i] "x" \033[0m\n",__FILE__,__func__,__LINE__ ,##args ) */ +#define DBG_MM_BLUE(x, args ...) /* dbg("\033[34m[%s:%s:%i] "x" \033[0m\n",__FILE__,__func__,__LINE__ ,##args ) */ + +/* 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. + */ +void*pico_mem_page0_zalloc(size_t len); +void pico_mem_page0_free(void*ptr); + + +/* this is a wrapper function for pico_tree_insert. The function pointers that are used by pico_tree + * to zalloc/free are modified so that pico_tree will insert the node in another memory region + */ +static void *manager_tree_insert(struct pico_tree*tree, void *key) +{ + return (void*) pico_tree_insert_implementation(tree, key, USE_PICO_PAGE0_ZALLOC); +} + +/* this is a wrapper function for pico_tree_insert. The function pointers that are used by pico_tree + * to zalloc/free are modified so that pico_tree will insert the node in another memory region + */ +static void *manager_tree_delete(struct pico_tree *tree, void *key) +{ + return (void *) pico_tree_delete_implementation(tree, key, USE_PICO_PAGE0_ZALLOC); +} + + +static const uint32_t slab_sizes[] = { + 1200, 1400, 1600 +}; /* Sizes must be from small to big */ +static uint32_t slab_size_statistics[] = { + 0, 0, 0 +}; +static uint32_t slab_size_global = PICO_MEM_DEFAULT_SLAB_SIZE; +/* + typedef struct pico_mem_manager pico_mem_manager; + typedef struct pico_mem_manager_extra pico_mem_manager_extra; + typedef struct pico_mem_page pico_mem_page; + typedef struct pico_mem_heap pico_mem_heap; + typedef struct pico_mem_slab pico_mem_slab; + typedef struct pico_mem_heap_block pico_mem_heap_block; + typedef struct pico_mem_slab_block pico_mem_slab_block; + typedef struct pico_mem_slab_node pico_mem_slab_node; + typedef struct pico_mem_block pico_mem_block; + typedef struct pico_tree pico_tree; + typedef struct pico_tree_node pico_tree_node; + typedef union block_internals block_internals; + */ +#define HEAP_BLOCK_NOT_FREE 0xCAFED001 +#define HEAP_BLOCK_FREE 0xCAFED00E + +#define SLAB_BLOCK_TYPE 0 +#define HEAP_BLOCK_TYPE 1 +/* + * page + * <----------------------------------------------------------------------> + * + * + * +------------<------------+----------<-----------+ + * | ^ ^ + * v | | + * +---------+------------+--+----+---------------+-+-----+---------------+ + * | | | | | | | + * | pico_ | | pico_ | | pico_ | | + * | mem_ | ...HEAP... | mem_ | slab | mem_ | slab | + * | page | | block | | block | | + * | | | | | | | + * +---------+------------+-------+-----+---------+-------+----------+----+ + * ^ | ^ | + * +-------+ | | | + * | | +-+ | + * +------|-----+ | | + * | | +-----|--------------------+ + * v | v | + * +---------+-----+-------+------+-+-----+-----+--+ + * | | | | | | | + * | pico_ | | pico_ | pico_ | | pico_ | + * | mem_ | ... | tree_ | mem_ | ... | mem_ | + * | manager | | node | slab_ | | slab_ | + * | | | | node | | node | + * +---------+-----+-+-----+-----+--+-----+-----+--+ + * | ^ | ^ | + * | | +---->---+ | + * +-->--+----<-----------<---+ + * + * <-----------------------------------------------> + * manager page + * + * + * +----------------+ + * | | + * | pico_tree_node +-------------------------------------+ + * | (size x) | | + * +--+----------+--+ | + * | | +---------v----------+ + * | | | | + * v v +----> pico_mem_slab_node +----+ + * | | | | | | + * +----<----+ +---->----+ | +--------------------+ | + * | | | | + * | | ^ v + * | | | | + * | | | +--------------------+ | + * +------v---------+ +---------v------+ +----+ <----+ + * | | | | | pico_mem_slab_node | + * | pico_tree_node | | pico_tree_node | +----> +----+ + * | (size x/2) | | (size 2x) | | +--------------------+ | + * +----------------+ +----------------+ | | + * | | + * | | + * ^ v + * ... ... + * + */ + +/* Housekeeping memory manager (start of page 0) */ +struct pico_mem_manager +{ + uint32_t size; /* Maximum size in bytes */ + uint32_t used_size; /* Used size in bytes */ + struct pico_tree tree; + struct pico_mem_page*first_page; + struct pico_mem_manager_extra*manager_extra; /* this is a pointer to a page with extra heap space used by the manager */ +}; +/* Housekeeping additionnal memory manager heap pages */ +struct pico_mem_manager_extra +{ + struct pico_mem_manager_extra*next; + uint32_t timestamp; + uint32_t blocks; +}; +/* Housekeeping of every page (start of all pages except the manager pages) */ +struct pico_mem_page +{ + uint32_t slab_size; + uint16_t slabs_max; + uint16_t slabs_free; + uint32_t heap_max_size; + uint32_t heap_max_free_space; + uint32_t timestamp; + struct pico_mem_page*next_page; +}; +/* Housekeeping struct for a heap block (kept per block of memory in heap) */ +struct pico_mem_heap_block +{ + uint32_t size; + /* uint8_t free; */ + uint32_t free; +}; +/* Housekeeping struct for a slab block (kept per block of memory in slabs) */ +struct pico_mem_slab_block +{ + struct pico_mem_page*page; + struct pico_mem_slab_node*slab_node; +}; +union block_internals +{ + struct pico_mem_heap_block heap_block; + struct pico_mem_slab_block slab_block; +}; +struct pico_mem_block +{ + union block_internals internals; /* Union has to be in first place!!! */ + uint8_t type; +}; +/* Used to store the slab objects in the RB-tree */ +struct pico_mem_slab_node +{ + struct pico_mem_block*slab; + struct pico_mem_slab_node*prev; + struct pico_mem_slab_node*next; +}; + +static struct pico_mem_manager*manager = NULL; + +/* + * This compare function will be called by pico_tree.c to compare 2 keyValues (type: struct pico_mem_slab_nodes) + * We want to compare slab_nodes by their size. We also want to be able to directly compare an integer, which explains + * the casts from void* to uint32_t*** + */ +static int compare_slab_keys(void*keyA, void*keyB) +{ + /* keyValues are pico_mem_slab_nodes */ + /* We want to compare the sizes */ + /* first element of pico_mem_slab_node: pico_mem_block* slab_block */ + /* first element of pico_mem_block: (slab_block in union): pico_mem_page* page */ + /* first element of pico_mem_page: uint32_t slab_size */ + uint32_t sizeKeyA = ***(uint32_t***) keyA; + uint32_t sizeKeyB = ***(uint32_t***) keyB; + DBG_MM_BLUE("Compare called: sizeA = %i, sizeB = %i", sizeKeyA, sizeKeyB); + if(sizeKeyA == sizeKeyB) + { + return 0; + } + else if(sizeKeyA < sizeKeyB) + { + return 1; + } + else + { + return -1; + } +} + +/* + * Pico_mem_init_page is called to initialize a block of memory pointed to by pico_mem_page* page. + * Slabs of size slabsize are created, and the page, heap and slab housekeeping is initialized. + */ +static void _pico_mem_init_page(struct pico_mem_page*page, size_t slabsize) +{ + uint8_t*byteptr = (uint8_t*) page; + struct pico_mem_block*slab_block; + struct pico_mem_block*heap_block; + struct pico_tree_node*tree_node; + struct pico_mem_slab_node*slab_node; + void*temp; + uint16_t i; + + DBG_MM_YELLOW("Initializing page %p with slabsize %u", page, slabsize); + + page->next_page = manager->first_page; + manager->first_page = page; + page->slab_size = (uint32_t)slabsize; + page->slabs_max = (uint16_t)((PICO_MEM_PAGE_SIZE - sizeof(struct pico_mem_page) - sizeof(struct pico_mem_block)) / (slabsize + sizeof(struct pico_mem_block))); + page->heap_max_size = (uint32_t)(PICO_MEM_PAGE_SIZE - sizeof(struct pico_mem_page) - sizeof(struct pico_mem_block) - (page->slabs_max * (sizeof(struct pico_mem_block) + slabsize))); + if(page->heap_max_size < PICO_MIN_HEAP_SIZE) + { + DBG_MM_BLUE("Not enough heap size available with slabsize %u, allocating one slab to heap.", slabsize); + page->slabs_max--; + /* DBG_MM_BLUE("Heap size %u -> %lu",page->heap_max_size, page->heap_max_size + sizeof(pico_mem_slab_block) + slabsize); */ + DBG_MM_BLUE("Heap size %u -> %lu", page->heap_max_size, page->heap_max_size + sizeof(struct pico_mem_block) + slabsize); + page->heap_max_size += (uint32_t)(sizeof(struct pico_mem_block) + slabsize); + } + + page->slabs_free = page->slabs_max; + page->heap_max_free_space = page->heap_max_size; + page->timestamp = 0; + DBG_MM_BLUE("max slab objects = %i, object_size = %i", page->slabs_max, page->slab_size); + DBG_MM_BLUE("Heap size: %i", page->heap_max_size); + byteptr += sizeof(struct pico_mem_page); /* jump over page struct so byteptr points to start of heap */ + + /* Init HEAP at the beginning of the page */ + heap_block = (struct pico_mem_block*) byteptr; + heap_block->type = HEAP_BLOCK_TYPE; + heap_block->internals.heap_block.free = HEAP_BLOCK_FREE; + heap_block->internals.heap_block.size = page->heap_max_free_space; + + byteptr += sizeof(struct pico_mem_block) + heap_block->internals.heap_block.size; + for(i = 0; i < page->slabs_max; i++) + { + slab_block = (struct pico_mem_block*) byteptr; + DBG_MM_BLUE("Slab object %i at %p. Start of object data at %p", i, slab_block, (uint8_t*) slab_block + sizeof(struct pico_mem_slab_block)); + slab_block->type = SLAB_BLOCK_TYPE; + slab_block->internals.slab_block.page = page; + + DBG_MM_BLUE("Calling find_node with size %u", **((uint32_t**) slab_block)); + tree_node = pico_tree_findNode(&(manager->tree), &slab_block); + + DBG_MM("Creating slab_node.."); + slab_node = pico_mem_page0_zalloc(sizeof(struct pico_mem_slab_node)); + if(slab_node == NULL) + { + DBG_MM_RED("No more space in the manager heap for the housekeeping of slab %i, and no more space for extra manager pages!", i + 1); + DBG_MM_RED("Debug info:\nUsed size: %u/%u\nmanager_extra = %p", manager->used_size, manager->size, manager->manager_extra); + DBG_MM_RED("This page will be initialized with %u slabs instead of %u slabs", i, page->slabs_max); + page->slabs_max = i; + page->slabs_free = page->slabs_max; + return; + /* exit(1); */ + } + + slab_node->slab = slab_block; + slab_node->prev = NULL; + slab_node->next = NULL; + + slab_block->internals.slab_block.slab_node = slab_node; + + if(tree_node != NULL) + { + struct pico_mem_slab_node*first_node = (struct pico_mem_slab_node*) tree_node->keyValue; + tree_node->keyValue = slab_node; + slab_node->next = first_node; + first_node->prev = slab_node; + } + else + { + /* Insert new slab_node */ + DBG_MM_BLUE("Inserting new slab node in the tree of size %u", slabsize); + /* pico_err_t pico_err_backup = pico_err; */ + /* pico_err = 0; */ + + + /* temp = pico_tree_insert(&manager->tree, slab_node); */ + temp = manager_tree_insert(&manager->tree, slab_node); + + /* IF SLAB_NODE COULDN'T BE INSERTED */ + /* if(pico_err == PICO_ERR_ENOMEM) */ + /* if(temp == &LEAF) */ + if(temp != NULL) + { + DBG_MM_RED("No more space in the manager heap for the housekeeping of slab %i, and no more space for extra manager pages!", i + 1); + DBG_MM_RED("This page will be initialized without slabs."); + pico_mem_page0_free(slab_node); + page->slabs_max = (uint16_t) i; + page->slabs_free = page->slabs_max; + /* pico_err = pico_err_backup; */ + return; + } + } + + /* byteptr = (uint8_t*) (slab_block+1); */ + byteptr = (uint8_t*) slab_block; + byteptr += sizeof(struct pico_mem_block); + byteptr += page->slab_size; + } + DBG_MM_GREEN("Initialized page %p with slabsize %u", page, slabsize); +} + +/* + * Initializes the memory by creating a memory manager page and one page with default slab size + * A maximum space of memsize can be occupied by the memory manager at any time + */ +void pico_mem_init(uint32_t memsize) +{ + struct pico_mem_block*first_block; + struct pico_mem_page*page; + uint8_t*startofmanagerheap; + + DBG_MM_YELLOW("Initializing memory with memsize %u", memsize); + if(memsize < PICO_MEM_PAGE_SIZE * 2) + { + /* Not enough memory was provided to initialize a manager page and a data page, return without initializing memory */ + /* Set pico_err to an appropriate value */ + pico_err = PICO_ERR_ENOMEM; + DBG_MM_RED("The memsize provided is too small, memory not initialized!"); + + return; + } + + /* First pico_mem_page is already included in pico_mem_manager. Others are added. */ + /* manager = pico_azalloc(sizeof(pico_mem_manager) + sizeof(pico_mem_page*)*(pages - 1)); //Points to usermanager if one present */ + manager = pico_zalloc(PICO_MEM_PAGE_SIZE); + if( NULL != manager ) + { + manager->size = memsize; + manager->used_size = PICO_MEM_PAGE_SIZE; + manager->first_page = NULL; + manager->manager_extra = NULL; + + manager->tree.compare = compare_slab_keys; + manager->tree.root = &LEAF; + DBG_MM_BLUE("Manager page is at %p", manager); + + DBG_MM_BLUE("Start of tree: %p, sizeof(pico_tree): %lu", &manager->tree, sizeof(struct pico_tree)); + DBG_MM_BLUE("Root node of tree at %p", manager->tree.root); + + /* Init manager heap. Used to store the RB-tree nodes which store pointers to free slab objects */ + startofmanagerheap = (uint8_t*) manager + sizeof(struct pico_mem_manager); /* manager heap is after struct pico_mem_manager */ + DBG_MM_BLUE("Start of manager heap = %p", startofmanagerheap); + first_block = (struct pico_mem_block*) startofmanagerheap; + first_block->type = HEAP_BLOCK_TYPE; + first_block->internals.heap_block.free = HEAP_BLOCK_FREE; + first_block->internals.heap_block.size = PICO_MEM_PAGE_SIZE - sizeof(struct pico_mem_manager) - sizeof(struct pico_mem_block); + + /* Initialize the first page only! */ + page = pico_zalloc(PICO_MEM_PAGE_SIZE); + if(page != NULL) + { + manager->used_size += PICO_MEM_PAGE_SIZE; + DBG_MM_BLUE("Page 1 at %p, manager used size = %u", page, manager->used_size); + _pico_mem_init_page(page, PICO_MEM_DEFAULT_SLAB_SIZE); + } + else + { + /* Not enough memory was provided to initialize a manager page and a data page, return without initializing memory */ + /* Set pico_err to an appropriate value */ + pico_err = PICO_ERR_ENOMEM; + /* Free the manager page */ + pico_free(manager); + manager = NULL; + DBG_MM_RED("Not enough space to allocate page 1, memory not initialized!"); + return; + } + + DBG_MM_GREEN("Memory initialized. Returning from pico_mem_init."); + } + else + { + /* Not enough memory was provided to initialize a manager page and a data page, return without initializing memory */ + /* Set pico_err to an appropriate value */ + pico_err = PICO_ERR_ENOMEM; + DBG_MM_RED("Not enough space to allocate manager page, memory not initialized!"); + return; + } +} + +/* + * Deinitializes the memory manager, returning all its memory to the system's control. + */ +void pico_mem_deinit() +{ + struct pico_mem_page*next_page; + struct pico_mem_manager_extra*next_manager_page; + + DBG_MM_YELLOW("Pico_mem_deinit called"); + if(manager == NULL) + { + DBG_MM_GREEN("No memory instance initialized, returning"); + } + else + { + while(manager->first_page != NULL) + { + next_page = manager->first_page->next_page; + pico_free(manager->first_page); + manager->first_page = next_page; + } + while(manager->manager_extra != NULL) + { + next_manager_page = manager->manager_extra->next; + pico_free(manager->manager_extra); + manager->manager_extra = next_manager_page; + } + DBG_MM_BLUE("Freeing manager page at %p", manager); + pico_free(manager); + manager = NULL; + slab_size_global = PICO_MEM_DEFAULT_SLAB_SIZE; + DBG_MM_GREEN("Memory manager reset"); + } +} + +/* + * This function is called internally by page0_zalloc if there isn't enough space left in the heap of the initial memory page + * This function allocates heap space in extra manager pages, creating new pages as necessary. + */ +static void*_pico_mem_manager_extra_alloc(struct pico_mem_manager_extra*heap_page, size_t len) +{ + struct pico_mem_manager_extra*extra_heap_page; + struct pico_mem_block*heap_block; + struct pico_mem_block*first_block; + struct pico_mem_block*new_block; + uint8_t*startOfData; + uint8_t*byteptr; + uint32_t sizeleft; + + DBG_MM_YELLOW("Searching for a block of len %u in extra manager page %p (%u blocks in use)", len, heap_page, heap_page->blocks); + /* Linearly search for a free heap block */ + + /* heap_block = (pico_mem_block*) (heap_page+1); */ + byteptr = (uint8_t*) heap_page + sizeof(struct pico_mem_manager_extra); + heap_block = (struct pico_mem_block*) byteptr; + + sizeleft = PICO_MEM_PAGE_SIZE - sizeof(struct pico_mem_manager_extra); + + while(heap_block->internals.heap_block.free == HEAP_BLOCK_NOT_FREE || heap_block->internals.heap_block.size < len) + { + sizeleft -= (uint32_t)sizeof(struct pico_mem_block); + sizeleft -= heap_block->internals.heap_block.size; + /* DBG_MM("Sizeleft=%i", sizeleft); */ + /* byteptr = (uint8_t*) (heap_block+1); */ + byteptr = (uint8_t*) heap_block + sizeof(struct pico_mem_block); + byteptr += heap_block->internals.heap_block.size; + heap_block = (struct pico_mem_block*) byteptr; + if(sizeleft <= sizeof(struct pico_mem_block)) + { + DBG_MM_RED("No more heap space left in the extra manager heap page!"); + if(heap_page->next == NULL) + { + /* TODO: Probably need another function for this */ + DBG_MM_RED("Trying to allocate a new page for extra heap space: space usage %uB/%uB", manager->used_size, manager->size); + if(manager->used_size + PICO_MEM_PAGE_SIZE > manager->size) + { + DBG_MM_RED("No more space left for this page!"); + /* exit(1); */ + return NULL; + } + + extra_heap_page = pico_zalloc(PICO_MEM_PAGE_SIZE); + if(extra_heap_page != NULL) + { + extra_heap_page->blocks = 0; + extra_heap_page->next = NULL; + extra_heap_page->timestamp = 0; + byteptr = (uint8_t*) extra_heap_page + sizeof(struct pico_mem_manager_extra); + first_block = (struct pico_mem_block*) byteptr; + first_block->type = HEAP_BLOCK_TYPE; + first_block->internals.heap_block.free = HEAP_BLOCK_FREE; + first_block->internals.heap_block.size = PICO_MEM_PAGE_SIZE - sizeof(struct pico_mem_manager_extra) - sizeof(struct pico_mem_block); + extra_heap_page->next = heap_page; + manager->manager_extra = extra_heap_page; + manager->used_size += PICO_MEM_PAGE_SIZE; + DBG_MM_BLUE("Allocated an extra manager heap page at %p, manager space usage: %uB/%uB", extra_heap_page, manager->used_size, manager->size); + return _pico_mem_manager_extra_alloc(extra_heap_page, len); + } + else + { + /* This should be a dirty crash */ + DBG_MM_RED("Page not allocated even though the max size for the memory manager hasn't been reached yet!"); + /* exit(1); */ + return NULL; + } + } + else + { + DBG_MM_RED("This should never happen: debug information:"); + DBG_MM_RED("manager->manager_extra = %p", manager->manager_extra); + DBG_MM_RED("heap_page = %p", heap_page); + DBG_MM_RED("heap_page->next = %p", heap_page->next); + /* exit(1); */ + return NULL; + } + } + } + heap_page->blocks++; + heap_page->timestamp = 0; + DBG_MM_BLUE("Found free heap block in extra manager page %p at: %p (%u blocks in use)", heap_page, heap_block, heap_page->blocks); + heap_block->internals.heap_block.free = HEAP_BLOCK_NOT_FREE; + + if(heap_block->internals.heap_block.size == sizeleft - sizeof(struct pico_mem_block)) + { + DBG_MM_BLUE("End of heap, splitting up into a new block"); + heap_block->internals.heap_block.size = (uint32_t)len; + sizeleft = (uint32_t)(sizeleft - (uint32_t)sizeof(struct pico_mem_block) - len); + if(sizeleft > sizeof(struct pico_mem_block)) + { + sizeleft -= (uint32_t)sizeof(struct pico_mem_block); + byteptr = (uint8_t*) heap_block + sizeof(struct pico_mem_block); + byteptr += len; + new_block = (struct pico_mem_block*) byteptr; + new_block->type = HEAP_BLOCK_TYPE; + new_block->internals.heap_block.free = HEAP_BLOCK_FREE; + new_block->internals.heap_block.size = sizeleft; + DBG_MM_BLUE("New block: %p, size = %u", new_block, new_block->internals.heap_block.size); + } + else + { + DBG_MM_RED("No more space in extra manager heap page left to initialize a new heap block!"); + DBG_MM_RED("A new page will be allocated when even more space is needed"); + } + } + + startOfData = (uint8_t*) heap_block + sizeof(struct pico_mem_block); + DBG_MM_GREEN("Start of data = %p", startOfData); + + return startOfData; +} + +/* + * Page0 zalloc is called by pico_tree.c so that nodes which contain pointers to the free slab objects are put in the + * manager page. Additional manager pages can be created if necessary. + */ +void*pico_mem_page0_zalloc(size_t len) +{ + struct pico_mem_manager_extra*heap_page; + struct pico_mem_block*heap_block; + struct pico_mem_block*first_block; + struct pico_mem_block*new_block; + uint8_t*startOfData; + uint8_t*byteptr; + uint32_t sizeleft; + + DBG_MM_YELLOW("pico_mem_page0_zalloc(%u) called", len); + + byteptr = (uint8_t*) manager + sizeof(struct pico_mem_manager); + heap_block = (struct pico_mem_block*) byteptr; + + /* If heap_block == NULL then a free block at the end of the list is found. */ + /* Else, if the block is free and the size > len, an available block is also found. */ + sizeleft = PICO_MEM_PAGE_SIZE - sizeof(struct pico_mem_manager); + /* this would mean that heap_block is never NULL */ + /* while(heap_block != NULL && ( heap_block->internals.heap_block.free == HEAP_BLOCK_NOT_FREE || heap_block->internals.heap_block.size < len)) */ + while(heap_block->internals.heap_block.free == HEAP_BLOCK_NOT_FREE || heap_block->internals.heap_block.size < len) + { + sizeleft -= (uint32_t)sizeof(struct pico_mem_block); + sizeleft -= heap_block->internals.heap_block.size; + /* DBG_MM("Sizeleft=%i", sizeleft); */ + byteptr = (uint8_t*) heap_block + sizeof(struct pico_mem_block); /* byteptr points to start of heap block data */ + byteptr += heap_block->internals.heap_block.size; /* jump over that data to start of next heap_block */ + heap_block = (struct pico_mem_block*) byteptr; + if(sizeleft <= sizeof(struct pico_mem_block)) + { + DBG_MM_RED("No more heap space left in the manager page!"); + if(manager->manager_extra == NULL) + { + DBG_MM_RED("Trying to allocate a new page for extra heap space: space usage: %uB/%uB", manager->used_size, manager->size); + if(manager->used_size + PICO_MEM_PAGE_SIZE > manager->size) + { + DBG_MM_RED("No more space left for this page!"); + /* exit(1); */ + return NULL; + } + + heap_page = pico_zalloc(PICO_MEM_PAGE_SIZE); + if(heap_page != NULL) + { + /* Initialize the new heap page */ + heap_page->blocks = 0; + heap_page->next = NULL; + heap_page->timestamp = 0; + byteptr = (uint8_t*) heap_page + sizeof(struct pico_mem_manager_extra); + first_block = (struct pico_mem_block*) byteptr; + first_block->type = HEAP_BLOCK_TYPE; + first_block->internals.heap_block.free = HEAP_BLOCK_FREE; + first_block->internals.heap_block.size = PICO_MEM_PAGE_SIZE - sizeof(struct pico_mem_manager_extra) - sizeof(struct pico_mem_block); + manager->manager_extra = heap_page; + manager->used_size += PICO_MEM_PAGE_SIZE; + DBG_MM_BLUE("Allocated an extra manager heap page at %p, manager space usage: %uB/%uB", heap_page, manager->used_size, manager->size); + return _pico_mem_manager_extra_alloc(heap_page, len); + } + else + { + /* This should be a dirty crash */ + DBG_MM_RED("Page not allocated even though the max size for the memory manager hasn't been reached yet!"); + /* exit(1); */ + return NULL; + } + } + else + { + return _pico_mem_manager_extra_alloc(manager->manager_extra, len); + } + } + } + DBG_MM_BLUE("Found free heap block in manager page at : %p", heap_block); + heap_block->internals.heap_block.free = HEAP_BLOCK_NOT_FREE; + + if(heap_block->internals.heap_block.size == sizeleft - sizeof(struct pico_mem_block)) + { + sizeleft = (uint32_t)(sizeleft - (uint32_t)sizeof(struct pico_mem_block) - len); + if(sizeleft > sizeof(struct pico_mem_block)) + { + DBG_MM_BLUE("End of heap, splitting up into a new block"); + heap_block->internals.heap_block.size = (uint32_t)len; + sizeleft -= (uint32_t)sizeof(struct pico_mem_block); + byteptr = (uint8_t*) heap_block + sizeof(struct pico_mem_block); + byteptr += len; + new_block = (struct pico_mem_block*) byteptr; + new_block->internals.heap_block.free = HEAP_BLOCK_FREE; + new_block->internals.heap_block.size = sizeleft; + DBG_MM_BLUE("New block: %p, size = %u", new_block, new_block->internals.heap_block.size); + } + else + { + /* DBG_MM_RED("ERROR! No more space in manager heap left to initialise a new heap_block!"); */ + /* exit(1); */ + DBG_MM_RED("No more space in manager heap left to initialize a new heap block!"); + DBG_MM_RED("A new page will be allocated when more space is needed"); + } + } + + startOfData = (uint8_t*) heap_block + sizeof(struct pico_mem_block); + DBG_MM_GREEN("Start of data = %p", startOfData); + + return startOfData; +} + + +/* + * This method will free a given heap block and try to merge it with + * surrounding blocks if they are free. + */ +static void _pico_mem_free_and_merge_heap_block(struct pico_mem_page*page, struct pico_mem_block*mem_block) +{ + uint8_t*byteptr; + /* pico_mem_block* prev = NULL; */ + struct pico_mem_block*prev; + struct pico_mem_block*curr; + struct pico_mem_block*next; + + DBG_MM_YELLOW("Freeing heap block %p with size %u in page %p", mem_block, mem_block->internals.heap_block.size, page); + + mem_block->internals.heap_block.free = HEAP_BLOCK_FREE; + + byteptr = (uint8_t*) page + sizeof(struct pico_mem_page); + curr = (struct pico_mem_block*) byteptr; + byteptr = (uint8_t*) curr + sizeof(struct pico_mem_block); + byteptr += curr->internals.heap_block.size; + next = (struct pico_mem_block*) byteptr; + + while(curr->type == HEAP_BLOCK_TYPE && next->type == HEAP_BLOCK_TYPE) + { + DBG_MM("Checking heap block (%s) with size %u at %p", (curr->internals.heap_block.free == HEAP_BLOCK_FREE) ? "free" : "not free", curr->internals.heap_block.size, curr); + if(curr->internals.heap_block.free == HEAP_BLOCK_FREE && next->internals.heap_block.free == HEAP_BLOCK_FREE) + { + DBG_MM_BLUE("Merging blocks with sizes %u and %u", curr->internals.heap_block.size, next->internals.heap_block.size); + curr->internals.heap_block.size += (uint32_t)sizeof(struct pico_mem_block) + next->internals.heap_block.size; + } + + prev = curr; + byteptr = (uint8_t*) curr + sizeof(struct pico_mem_block); + byteptr += curr->internals.heap_block.size; + curr = (struct pico_mem_block*) byteptr; + byteptr = (uint8_t*) curr + sizeof(struct pico_mem_block); + byteptr += curr->internals.heap_block.size; + next = (struct pico_mem_block*) byteptr; + } + DBG_MM("Checking heap block (%s) with size %u at %p", (curr->internals.heap_block.free == HEAP_BLOCK_FREE) ? "free" : "not free", curr->internals.heap_block.size, curr); + if(curr->type == HEAP_BLOCK_TYPE && prev->internals.heap_block.free == HEAP_BLOCK_FREE && curr->internals.heap_block.free == HEAP_BLOCK_FREE) + { + DBG_MM_BLUE("Merging blocks with sizes %u and %u", prev->internals.heap_block.size, curr->internals.heap_block.size); + prev->internals.heap_block.size += (uint32_t)sizeof(struct pico_mem_block) + curr->internals.heap_block.size; + } + + DBG_MM_GREEN("Heap block freed and heap space defragmentized"); +} + +/* + * This method will return the max. available contiguous free space in the heap + * from a given page. + */ +static uint32_t _pico_mem_determine_max_free_space(struct pico_mem_page*page) +{ + uint32_t maxfreespace = 0; + uint8_t*byteptr; + struct pico_mem_block*mem_block; + + DBG_MM_YELLOW("Determining new maximum free space in page %p (old free space: %u)", page, page->heap_max_free_space); + + /* pico_mem_block* mem_block = (pico_mem_block*) (page+1); //reset mem_block to first block in the heap */ + byteptr = (uint8_t*) page + sizeof(struct pico_mem_page); + mem_block = (struct pico_mem_block*) byteptr; /* reset mem_block to first block in the heap */ + + /* Determine max free space by iterating trough the list */ + /* while(mem_block != NULL && mem_block->type == HEAP_BLOCK_TYPE) */ + while(mem_block->type == HEAP_BLOCK_TYPE) + { + /* DBG_MM("Memblock %p of size %i is free %i\n",block, block->size, block->free); */ + DBG_MM("Memblock %s (size %u) at %p", (mem_block->internals.heap_block.free == HEAP_BLOCK_FREE) ? "not in use" : "in use", mem_block->internals.heap_block.size, mem_block); + if(mem_block->internals.heap_block.free == HEAP_BLOCK_FREE && mem_block->internals.heap_block.size > maxfreespace) + { + maxfreespace = mem_block->internals.heap_block.size; + page->heap_max_free_space = maxfreespace; + } + + byteptr = (uint8_t*) mem_block + sizeof(struct pico_mem_block); + byteptr += mem_block->internals.heap_block.size; + mem_block = (struct pico_mem_block*) byteptr; + } + page->heap_max_free_space = maxfreespace; + DBG_MM_GREEN("New free space: %u", page->heap_max_free_space); + return maxfreespace; +} + +/* + * This method will make a slab object available again by putting it in the RB-tree. + * Slab objects of the same size are stored in a double linked list. One pico_tree_node represents + * all the slab objects of the same size by making the keyvalue of a pico_tree_node point to + * the first element of the linked list. + * An element in this linked list is a struct pico_mem_slab_node. All the elements are also + * stored in the heap of the manager page (page0), or in the heap of extra manager spaces if there isn't enough space. + */ +static void _pico_mem_free_slab_block(struct pico_mem_block*slab_block) +{ + struct pico_mem_slab_node*slab_node; + struct pico_mem_slab_node*first_slab_node; + struct pico_tree_node*tree_node; + void*temp; + + DBG_MM_YELLOW("Freeing slab object"); + + slab_node = pico_mem_page0_zalloc(sizeof(struct pico_mem_slab_node)); + + if(slab_node == NULL) + { + /* Update the page householding without making the slab available again! */ + DBG_MM_RED("No more space in the manager heap and no more space for extra pages!"); + DBG_MM_RED("This slab will be leaked, but the leak will be plugged at the next cleanup, if and when the page is empty"); + slab_block->internals.slab_block.page->slabs_free++; + return; + } + + slab_node->slab = slab_block; + slab_block->internals.slab_block.slab_node = slab_node; + tree_node = pico_tree_findNode(&manager->tree, slab_node); + if(tree_node != NULL) + { + first_slab_node = (struct pico_mem_slab_node*) tree_node->keyValue; + tree_node->keyValue = slab_node; + first_slab_node->prev = slab_node; + slab_node->prev = NULL; + slab_node->next = first_slab_node; + } + else{ + DBG_MM_BLUE("No node found for size %i so calling pico_tree_insert", slab_node->slab->internals.slab_block.page->slab_size); + slab_node->next = NULL; + slab_node->prev = NULL; + /* pico_err_t pico_err_backup = pico_err; */ + /* pico_err = 0; */ + + + /* temp = pico_tree_insert(&manager->tree, slab_node); */ + temp = manager_tree_insert(&manager->tree, slab_node); + + /* if(pico_err == PICO_ERR_ENOMEM) */ + if(temp == &LEAF) + { + DBG_MM_RED("No more space in the manager heap and no more space for extra pages!"); + DBG_MM_RED("This slab will be leaked, but the leak will be plugged at the next cleanup, if and when the page is empty"); + pico_mem_page0_free(slab_node); + /* pico_err = pico_err_backup; */ + slab_block->internals.slab_block.page->slabs_free++; + return; + } + } + + /* Update free slabs in page householding */ + slab_block->internals.slab_block.page->slabs_free++; + DBG_MM_GREEN("Freed slab object, there are now %i free slab objects in the corresponding page", slab_block->internals.slab_block.page->slabs_free); +} + +/* + * This method zero initializes a block of memory pointed to by startOfData, of size len + */ +static void _pico_mem_zero_initialize(void*startOfData, size_t len) +{ + if(startOfData != NULL) + { + DBG_MM_YELLOW("Zero initializing user memory at %p of %u bytes", startOfData, len); + memset(startOfData, 0, len); + DBG_MM_GREEN("Zero initialized."); + } + else + { + DBG_MM_RED("Got a NULL pointer to zero initialize!"); + } +} + +/* + * This method will try to find a free heap block of size len in a given page. + */ +static void*_pico_mem_find_heap_block(struct pico_mem_page*page, size_t len) +{ + struct pico_mem_block*mem_block; + struct pico_mem_block*inserted_block; + uint8_t*startOfData; + uint8_t*byteptr; + + DBG_MM_YELLOW("Searching for a heap block of length %u in page %p (largest free block size = %u)", len, page, page->heap_max_free_space); + if(page->heap_max_free_space < len ) + { + DBG_MM_RED("Size %u > max free space %u of the page. This should only happen when this page is newly created, and its heap space is not large enough for the heap length!", len, page->heap_max_free_space); + return NULL; + } + + byteptr = (uint8_t*) page + sizeof(struct pico_mem_page); + mem_block = (struct pico_mem_block*) byteptr; /* Jump over the page struct to the start of the heap */ + + /* If mem_block == NULL then a free block at the end of the list is found. */ + /* Else, if the block is free and the size > len, an available block is also found. */ + /* while(mem_block != NULL && mem_block->type == HEAP_BLOCK_TYPE && ( mem_block->internals.heap_block.free == HEAP_BLOCK_NOT_FREE || mem_block->internals.heap_block.size < len)) */ + while(mem_block->type == HEAP_BLOCK_TYPE && (mem_block->internals.heap_block.free == HEAP_BLOCK_NOT_FREE || mem_block->internals.heap_block.size < len)) + { + /* DBG_MM_RED("Skipping heap block in use at %p of size %i", mem_block, mem_block->size); */ + DBG_MM_BLUE("Skipping heap block %s (size %u) at %p", (mem_block->internals.heap_block.free == HEAP_BLOCK_FREE) ? "not in use" : "in use", mem_block->internals.heap_block.size, mem_block); + byteptr = (uint8_t*) mem_block + sizeof(struct pico_mem_block); + byteptr += mem_block->internals.heap_block.size; + mem_block = (struct pico_mem_block*) byteptr; + } + if(mem_block->type == SLAB_BLOCK_TYPE) + { + DBG_MM_RED("No free heap block of contiguous size %u could be found in page %p", len, page); + /* exit(1); */ + return NULL; + } + + DBG_MM_BLUE("Found free heap block of size %u at %p", mem_block->internals.heap_block.size, mem_block); + mem_block->internals.heap_block.free = HEAP_BLOCK_NOT_FREE; + page->timestamp = 0; + + /* Check to split the block into two smaller blocks */ + if(mem_block->internals.heap_block.size >= (len + sizeof(struct pico_mem_block) + PICO_MEM_MINIMUM_OBJECT_SIZE)) + { + byteptr = (uint8_t*) mem_block + sizeof(struct pico_mem_block); + byteptr += len; + inserted_block = (struct pico_mem_block*) byteptr; + + /* Update newly inserted block */ + inserted_block->type = HEAP_BLOCK_TYPE; + inserted_block->internals.heap_block.free = HEAP_BLOCK_FREE; + inserted_block->internals.heap_block.size = (uint32_t)(mem_block->internals.heap_block.size - (uint32_t)sizeof(struct pico_mem_block) - len); + /* Update block that was split up */ + mem_block->internals.heap_block.size = (uint32_t)len; + DBG_MM_BLUE("Splitting up the block, creating a new block of size %u at %p", inserted_block->internals.heap_block.size, inserted_block); + } + + startOfData = (uint8_t*) mem_block + sizeof(struct pico_mem_block); + + page->heap_max_free_space = _pico_mem_determine_max_free_space(page); + + /* Zero-initialize */ + _pico_mem_zero_initialize(startOfData, len); + DBG_MM_GREEN("Returning %p", startOfData); + return startOfData; +} + +/* + * This method will be called from pico_mem_zalloc. If an appropriate slab object is found, + * it is deleted from the RB tree and a pointer to the start of data in the slab object + * is returned. + */ +static void*_pico_mem_find_slab(size_t len) +{ + size_t*lenptr = &len; + size_t**doublelenptr = &lenptr; + struct pico_tree_node*node; + uint8_t *returnVal = NULL; + + DBG_MM_YELLOW("Finding slab with size %u", len); + /* The compare function takes an int*** length */ + node = pico_tree_findNode(&manager->tree, &doublelenptr); + + if(node != NULL) { + /* DBG_MM_BLUE("Found node, size = %d ", ((pico_mem_slab_node*) node->keyValue)->slab->size); */ + struct pico_mem_slab_node*slab_node = node->keyValue; + slab_node->slab->internals.slab_block.page->slabs_free--; + slab_node->slab->internals.slab_block.page->timestamp = 0; + DBG_MM_BLUE("Found node, size = %u at page %p, %u free slabs left in page", slab_node->slab->internals.slab_block.page->slab_size, slab_node->slab->internals.slab_block.page, slab_node->slab->internals.slab_block.page->slabs_free); + if(slab_node->next == NULL) + { + DBG_MM_BLUE("This was the last available slab object. Deleting the tree node now."); + /* if this is the last slab object of this size in the tree, then also delete the tree_node! */ + + + /* pico_tree_delete(&manager->tree, &doublelenptr); */ + manager_tree_delete(&manager->tree, &doublelenptr); + + + } + else + { + /* Remove the pico_mem_slab_node by making the keyvalue of the pico_tree_node point to the next element. */ + slab_node->next->prev = NULL; + node->keyValue = slab_node->next; + } + + returnVal = ((uint8_t*) (slab_node->slab)) + sizeof(struct pico_mem_block); + DBG_MM_BLUE("Start of slab: %p -> start of data : %p", slab_node->slab, returnVal); + /* Update the slab block housekeeping */ + slab_node->slab->internals.slab_block.slab_node = NULL; + /* Zero-initialize */ + _pico_mem_zero_initialize(returnVal, len); + /* Free the struct that was used by the linked list in the RB-tree */ + pico_mem_page0_free(slab_node); + } + + DBG_MM_GREEN("Returning %p", returnVal); + return returnVal; +} + +/* + * This method is called by the picotcp stack to free memory. + */ +void pico_mem_free(void*ptr) +{ + struct pico_mem_block*generic_block; + struct pico_mem_page*page; + /*Uncomment i for debugging!*/ + /*uint16_t i = 0;*/ + + DBG_MM_YELLOW("Free called on %p", ptr); + + if(ptr == NULL) return; + + generic_block = (struct pico_mem_block*) ptr; + generic_block--; + + if(generic_block->type == SLAB_BLOCK_TYPE) + { + if(generic_block->internals.slab_block.slab_node) + { + DBG_MM_RED("ERROR: Double free on a slab block (recovered)!"); + return; + } + + DBG_MM_BLUE("Request to free a slab block"); + _pico_mem_free_slab_block(generic_block); + } + else if(generic_block->type == HEAP_BLOCK_TYPE) + { + if(generic_block->internals.heap_block.free == HEAP_BLOCK_FREE) + { + DBG_MM_RED("ERROR: Double free on a heap block (recovered)!"); + return; + } + + DBG_MM_BLUE("Request to free a heap block"); + + /* Update the page housekeeping */ + /* Update the housekeeping of the extra manager pages */ + page = manager->first_page; + while(page != NULL) + { + DBG_MM_BLUE("Checking page %i at %p", i++, page); + if(((uint8_t*) page < (uint8_t*) ptr) && ((uint8_t*) ptr < (uint8_t*) page + PICO_MEM_PAGE_SIZE)) + { + /* DBG_MM_RED("page < ptr < page + PICO_MEM_PAGE_SIZE"); */ + /* DBG_MM_RED("%p < %p < %p", (uint8_t*) page, (uint8_t*) ptr, (uint8_t*) page + PICO_MEM_PAGE_SIZE); */ + _pico_mem_free_and_merge_heap_block(page, generic_block); + _pico_mem_determine_max_free_space(page); + break; + } + + page = page->next_page; + } + } + else + { + DBG_MM_RED("ERROR: You tried to free a pointer from which the type ( heap block or slab object ) could not be determined!!"); + } +} + +/************************NEW***************************/ +static void _pico_mem_reset_slab_statistics(void) +{ + slab_size_statistics[0] = 0; + slab_size_statistics[1] = 0; + slab_size_statistics[2] = 0; +} + +static size_t _pico_mem_determine_slab_size(size_t len) +{ + DBG_MM_YELLOW("Determining slab size to use, request for %u bytes", len); + if (len > slab_sizes[1]) + { + slab_size_statistics[2]++; + if(slab_size_statistics[2] > 3) + { + _pico_mem_reset_slab_statistics(); + if(slab_size_global != slab_sizes[2]) + { + slab_size_global = slab_sizes[2]; + } + } + + if(slab_size_global != slab_sizes[2]) + { + DBG_MM_RED("Using slab size %u, but we have to use a slab size of %u for the request of %u bytes", slab_size_global, slab_sizes[2], len); + return slab_sizes[2]; + } + } + else if(len > slab_sizes[0]) + { + slab_size_statistics[1]++; + if (slab_size_statistics[1] > 3) + { + _pico_mem_reset_slab_statistics(); + if(slab_size_global != slab_sizes[1]) + { + slab_size_global = slab_sizes[1]; + } + } + + if(len > slab_size_global) + { + DBG_MM_RED("Using slab size %u, but we have to use a slab size of %u for the request of %u bytes", slab_size_global, slab_sizes[1], len); + return slab_sizes[1]; + } + } + else + { + slab_size_statistics[0]++; + if (slab_size_statistics[0] > 3) + { + _pico_mem_reset_slab_statistics(); + if(slab_size_global != slab_sizes[0]) + { + slab_size_global = slab_sizes[0]; + } + } + } + + DBG_MM_GREEN("Using slab size %u", slab_size_global); + return slab_size_global; +} +/************************NEW***************************/ + +/* + * This method will be called by the picotcp stack to allocate new memory. + * If the requested size is bigger than the threshold of a slab object, + * then the manager will try to find an appropriate slab object and return a pointer + * to the beginning of the data in that slab object. + * + * If no slab objects could be found, or the requested size is less then the threshold + * of a slab object, the manager will try to allocate a heap block and return a pointer + * to the beginning of the data of that heap block. + * + * If still no memory could be found, then the manager will check again if the + * requested size is smaller than the threshold of a slab object. + * If so, the manager will try to find a slab object again but now ignoring the threshold. + * By doing so, there will be a large amount of internal fragmentation, but at least the + * memory request could be fulfilled. + * + * In any other case, the manager will return NULL. + */ +void*pico_mem_zalloc(size_t len) +{ + struct pico_mem_page*page; + void*returnCandidate; + uint32_t pagenr; + void *ret; + + DBG_MM_YELLOW("===> pico_mem_zalloc(%i) called", len); + len += (len % 4 == 0) ? 0 : 4 - len % 4; + DBG_MM_YELLOW("Aligned size: %i", len); + + if(manager == NULL) + { + DBG_MM_RED("Invalid alloc, a memory manager hasn't been instantiated yet!"); + return NULL; + } + + if(len > PICO_MAX_SLAB_SIZE) + { + DBG_MM_RED("Invalid alloc, the size you requested is larger than the maximum slab size! (%uB>%uB)", len, PICO_MAX_SLAB_SIZE); + return NULL; + } + + /* /////// FIND SLAB OBJECTS ///////// */ + if(len >= PICO_MIN_SLAB_SIZE) + { + /* feed the size into a statistic engine that determines the slabsize to use */ + /* DBG_MM_RED("Placeholder: determine correct slab size to use!"); */ + len = _pico_mem_determine_slab_size(len); + ret = _pico_mem_find_slab(len); + if(ret != NULL) return ret; + + /* No slab object could be found. => Init new page? */ + + DBG_MM_BLUE("No free slab found, trying to create a new page (Used size = %u, max size = %u)", manager->used_size, manager->size); + if(manager->used_size + PICO_MEM_PAGE_SIZE <= manager->size) + { + struct pico_mem_page*newpage = pico_zalloc(PICO_MEM_PAGE_SIZE); + if(newpage != NULL) + { + manager->used_size += PICO_MEM_PAGE_SIZE; + DBG_MM_BLUE("Created new page at %p -> used size = %u", newpage, manager->used_size); + _pico_mem_init_page(newpage, len); + /* Return pointer to first slab in that page */ + return _pico_mem_find_slab(len); /* Find the new slab object! */ + } + else + { + DBG_MM_RED("Not enough space to allocate a new page, even though the max size hasn't been reached yet!"); + return NULL; + } + } + else + { + DBG_MM_RED("Not enough space to allocate a new page!"); + return NULL; + } + } + + /* /////// FIND HEAP BLOCKS ///////// */ + if(len < PICO_MEM_MINIMUM_OBJECT_SIZE) + len = PICO_MEM_MINIMUM_OBJECT_SIZE; + + DBG_MM_BLUE("Searching for heap space of length %u now.", len); + + pagenr = 1; + page = manager->first_page; + + /* The algorithm to find a heap block is based on first fit. */ + /* But when the internal fragmentation is too big, the block is split. */ + while(page != NULL) + { + /* DBG_MM_RED("Max free space in page %i = %i bytes", pagecounter+1, page->heap.max_free_space); */ + DBG_MM_BLUE("Max free space in page %u = %uB (page=%p)", pagenr, page->heap_max_free_space, page); + if(len <= page->heap_max_free_space) + { + return _pico_mem_find_heap_block(page, len); + } + + pagenr++; + page = page->next_page; + } + /* No free heap block could be found, try to alloc a new page */ + DBG_MM_BLUE("No free heap block found, trying to create a new page (Used size = %u, max size = %u)", manager->used_size, manager->size); + if(manager->used_size + PICO_MEM_PAGE_SIZE <= manager->size) + { + struct pico_mem_page*newpage = pico_zalloc(PICO_MEM_PAGE_SIZE); + if(newpage != NULL) + { + manager->used_size += PICO_MEM_PAGE_SIZE; + DBG_MM_BLUE("Created new page at %p -> used size = %u", newpage, manager->used_size); + /* TODO: Careful, if the current slabsize is determined in another way, this needs to change too */ + _pico_mem_init_page(newpage, slab_size_global); + returnCandidate = _pico_mem_find_heap_block(newpage, len); + if(returnCandidate != NULL) + return returnCandidate; + } + else + { + DBG_MM_RED("Not enough space to allocate a new page, even though the max size hasn't been reached yet!"); + return NULL; + } + } + + /* DBG_MM_RED("NO HEAP BLOCK FOUND!"); */ + + /* /////// TRY TO FIND NEW SLAB OBJECT, BUT INCREASE SIZE ///////// */ + DBG_MM_RED("TRYING TO FIND FREE SLAB OBJECT WITH DANGER OF LARGE INTERNAL FRAGMENTATION"); + /* TODO: Careful, if the current slabsize is determined in another way, this needs to change too */ + return _pico_mem_find_slab(slab_size_global); +} +/* + * This method frees heap space used in the manager page, or in one of the extra manager pages + */ +void pico_mem_page0_free(void*ptr) +{ + struct pico_mem_block*node = ptr; + struct pico_mem_manager_extra*heap_page; + /* Uncomment for debugging! */ + /* int i = 0; */ + + /* TODO: should be able to merge free neighbouring blocks (??) */ + DBG_MM_YELLOW("page0_free called"); + + node--; + node->internals.heap_block.free = HEAP_BLOCK_FREE; + /* Update the housekeeping of the extra manager pages */ + heap_page = manager->manager_extra; + while(heap_page != NULL) + { + DBG_MM_BLUE("Checking extra heap page %i at %p", i++, heap_page); + if(((uint8_t*) heap_page < (uint8_t*) ptr) && ((uint8_t*) ptr < (uint8_t*) heap_page + PICO_MEM_PAGE_SIZE)) + { + /* DBG_MM_RED("heap_page < ptr < heap_page + PICO_MEM_PAGE_SIZE"); */ + /* DBG_MM_RED("%p < %p < %p", (uint8_t*) heap_page, (uint8_t*) ptr, (uint8_t*) heap_page + PICO_MEM_PAGE_SIZE); */ + heap_page->blocks--; + DBG_MM_BLUE("Updating heap page housekeeping: %u->%u used blocks", heap_page->blocks + 1, heap_page->blocks); + break; + } + + heap_page = heap_page->next; + } + DBG_MM_GREEN("Heap block (located in %s) succesfully freed", (i != -1) ? "main manager page" : "extra manager page"); +} + +/* + * This cleanup function must be called externally at downtime moments. A system timestamp must be passed to the function. + * All pages and extra manager pages will be checked. If they are empty, the timestamp of the page will be updated. If the + * page has been empty for a time longer than PICO_MEM_PAGE_LIFETIME, the page is returned to the system's control, and all + * the housekeeping is updated. + */ +void pico_mem_cleanup(uint32_t timestamp) +{ + struct pico_mem_slab_node*slab_node; + struct pico_tree_node*tree_node; + struct pico_mem_block*slab_block; + struct pico_mem_page*next_page; + struct pico_mem_page*prev_page; + struct pico_mem_page*page; + struct pico_mem_manager_extra*heap_page; + struct pico_mem_manager_extra*next; + struct pico_mem_manager_extra*prev_heap_page; + uint8_t*byteptr; + int pagenr = 1; + int i; + + DBG_MM_YELLOW("Starting cleanup with timestamp %u", timestamp); + /* Iterate over all pages */ + page = manager->first_page; + prev_page = NULL; + while(page != NULL) + { + DBG_MM_BLUE("Checking page %i at %p", pagenr, page); + /* Check the timestamp of the page. If it doesn't have one (0), update it with the new timestamp if the page is completely empty. */ + if(page->timestamp == 0) + { + if((page->heap_max_size == page->heap_max_free_space) && (page->slabs_free == page->slabs_max)) + { + DBG_MM_BLUE("Page %i empty, updating timestamp", pagenr); + page->timestamp = timestamp; + } + } + /* If the timestamp is old enough, remove the page and all its slabs. This means we have to: */ + /* > Remove all slabs out of the RB tree */ + /* > Update the page list */ + /* > Return the page to the system's control */ + /* > Update manager housekeeping */ + else if(timestamp > page->timestamp) + { + if(timestamp - page->timestamp > PICO_MEM_PAGE_LIFETIME) + { + DBG_MM_BLUE("Page %i is empty and has exceeded the lifetime (%u > lifetime=%u)", pagenr, timestamp - page->timestamp, PICO_MEM_PAGE_LIFETIME); + /* Remove all the slabs out of the RB tree */ + byteptr = (uint8_t*) page + sizeof(struct pico_mem_page); /* byteptr points to the start of the heap (a pico_mem_block), after page housekeeping */ + byteptr += sizeof(struct pico_mem_block); /* jump over pico_mem_block, containing the housekeeping for the heap space */ + byteptr += page->heap_max_size; /* jump over heap space, byteptr now points to the start of the slabs */ + slab_block = (struct pico_mem_block*) byteptr; + slab_node = slab_block->internals.slab_block.slab_node; + /* The corresponding tree_node */ + tree_node = pico_tree_findNode(&manager->tree, slab_node); + for(i = 0; i < page->slabs_max; i++) + { + DBG_MM("Removing slab %i at %p", i, slab_block); + if(slab_node->prev == NULL && slab_node->next == NULL) + { + DBG_MM("This node is the last node in the tree_node, removing tree_node"); + /* slab_node is the last node in the tree leaf, delete it */ + + + /* pico_tree_delete(&manager->tree, slab_node); */ + manager_tree_delete(&manager->tree, slab_node); + + + } + else if(slab_node->prev == NULL) + { + DBG_MM("This node is the first node in the linked list, adjusting tree_node"); + tree_node->keyValue = slab_node->next; + slab_node->next->prev = NULL; + } + else if(slab_node->next == NULL) + { + DBG_MM("This node is the last node in the linked list"); + slab_node->prev->next = NULL; + } + else + { + DBG_MM("This node is neither the first, nor the last node in the list"); + slab_node->prev->next = slab_node->next; + slab_node->next->prev = slab_node->prev; + } + + pico_mem_page0_free(slab_node); + byteptr = (uint8_t*) slab_block + sizeof(struct pico_mem_block); /* byteptr points to the start of the slab data, after the housekeeping */ + byteptr += page->slab_size; /* jump over the slab data, byteptr now points to the start of the next slab block */ + slab_block = (struct pico_mem_block*) byteptr; + slab_node = slab_block->internals.slab_block.slab_node; + } + /* Update the page list */ + if(prev_page == NULL) /* prev_page == NULL when pagenr=1, or when previous pages were deleted */ + { + DBG_MM("Updating page list, manager->first_page = page->next_page"); + manager->first_page = page->next_page; + } + else + { + DBG_MM("Updating page list, prev_page->next_page = page->next_page"); + prev_page->next_page = page->next_page; + } + + /* Return the page to the system's control */ + next_page = page->next_page; + DBG_MM("Freeing page, manager used size = %u", manager->used_size); + pico_free(page); + /* Update the manager housekeeping */ + manager->used_size -= PICO_MEM_PAGE_SIZE; + DBG_MM("Freed page, manager used size = %u, down from %u", manager->used_size, manager->used_size + PICO_MEM_PAGE_SIZE); + /* ITERATION */ + page = next_page; + pagenr++; + continue; + } + else + { + DBG_MM_BLUE("Page %i is empty, but has not exceeded the lifetime (%u < lifetime=%u)", pagenr, timestamp - page->timestamp, PICO_MEM_PAGE_LIFETIME); + } + } + else /* timestamp < page->timestamp */ + { + DBG_MM_RED("Page %i is empty, but the system timestamp < page timestamp! (%u<%u)", pagenr, timestamp, page->timestamp); + DBG_MM_RED("Updating page %i timestamp!", pagenr); + page->timestamp = timestamp; + } + + pagenr++; + prev_page = page; + page = page->next_page; + } + /* Check all extra manager pages if they are empty */ + heap_page = manager->manager_extra; + prev_heap_page = NULL; + pagenr = 1; + while(heap_page != NULL) + { + DBG_MM_BLUE("Checking extra manager page %i at %p", pagenr, heap_page); + if(heap_page->timestamp == 0) + { + if( heap_page->blocks == 0 ) + { + DBG_MM_BLUE("Extra manager page %i empty, updating timestamp", pagenr); + heap_page->timestamp = timestamp; + } + } + else if(timestamp > heap_page->timestamp) + { + if(timestamp - heap_page->timestamp > PICO_MEM_PAGE_LIFETIME) + { + DBG_MM_BLUE("Extra manager page %i empty and has exceeded the lifetime (%u > lifetime=%u)", pagenr, timestamp - heap_page->timestamp, PICO_MEM_PAGE_LIFETIME); + /* Update the page list */ + if(prev_heap_page == NULL) + { + DBG_MM("Updating page list, manager->manager_extra = heap_page->next"); + manager->manager_extra = heap_page->next; + } + else + { + DBG_MM("Updating page list, prev_heap_page->next = heap_page->next"); + prev_heap_page->next = heap_page->next; + } + + /* Return the page to the system's control */ + next = heap_page->next; + DBG_MM("Freeing page, manager used size = %u", manager->used_size); + pico_free(heap_page); + /* Update the manager housekeeping */ + manager->used_size -= PICO_MEM_PAGE_SIZE; + DBG_MM("Freed page, manager used size = %u, down from %u", manager->used_size, manager->used_size + PICO_MEM_PAGE_SIZE); + /* ITERATION */ + heap_page = next; + pagenr++; + continue; + } + else + { + DBG_MM_BLUE("Page %i is empty, but has not exceeded the lifetime (%u < lifetime=%u)", pagenr, timestamp - heap_page->timestamp, PICO_MEM_PAGE_LIFETIME); + } + } + else + { + DBG_MM_RED("Page %i is empty, but the system timestamp < page timestamp! (%u<%u)", pagenr, timestamp, heap_page->timestamp); + DBG_MM_RED("Updating page %i timestamp!", pagenr); + heap_page->timestamp = timestamp; + } + + /* ITERATION */ + pagenr++; + prev_heap_page = heap_page; + heap_page = heap_page->next; + } +} + + + + + + +#ifdef PICO_SUPPORT_MM_PROFILING +/*********************************************************************************************************************** + *********************************************************************************************************************** + MEMORY PROFILING FUNCTIONS + *********************************************************************************************************************** + ***********************************************************************************************************************/ + +static struct pico_mem_manager*manager_profile; + +static void _pico_mem_print_tree(struct pico_tree_node*root) +{ + struct pico_mem_slab_node*iterator; + int j; + + if (root == &LEAF || root == NULL) + { + DBG_MM("No tree nodes at this time.\n"); + return; + } + + iterator = (struct pico_mem_slab_node*) root->keyValue; + DBG_MM("Tree node for size %u:\n", iterator->slab->internals.slab_block.page->slab_size); + j = 0; + while(iterator != NULL) + { + DBG_MM("\tSlab_node %i at %p:\n", j, iterator); + DBG_MM("\t\tPrev:%p\n", iterator->prev); + DBG_MM("\t\tNext:%p\n", iterator->next); + DBG_MM("\t\tSlab:%p\n", iterator->slab); + j++; + iterator = iterator->next; + } + if(root->leftChild != &LEAF && root->leftChild != NULL) + _pico_mem_print_tree(root->leftChild); + + if(root->rightChild != &LEAF && root->rightChild != NULL) + _pico_mem_print_tree(root->rightChild); +} + +void pico_mem_profile_scan_data() +{ + if(manager == NULL) + { + DBG_MM("No memory manager instantiated!\n"); + } + else + { + int manager_pages = 0; + int pages = 0; + int counter = 0; + struct pico_mem_manager_extra*heap_page; + struct pico_mem_page*page; + uint8_t*byteptr; + struct pico_mem_block*mem_block; + + DBG_MM("Memory manager: %uB/%uB in use\n", manager->used_size, manager->size); + _pico_mem_print_tree(manager->tree.root); + + /* Iterate over every extra manager page: */ + heap_page = manager->manager_extra; + while(heap_page != NULL) + { + manager_pages++; + DBG_MM("Extra manager page %i:\n\tBlocks in use: %u\n\tTimestamp: %u\n", manager_pages, heap_page->blocks, heap_page->timestamp); + heap_page = heap_page->next; + } + /* Iterate over every page: */ + pages = (manager->used_size / PICO_MEM_PAGE_SIZE) - manager_pages - 1; + page = manager->first_page; + while(page != NULL) + { + counter++; + DBG_MM("Page %i/%i:\n\tSlabsize: %u\n\tSlabs free: %u/%u\n\tTimestamp: %u\n", counter, pages, page->slab_size, page->slabs_free, page->slabs_max, page->timestamp); + byteptr = (uint8_t*) page + sizeof(struct pico_mem_page); + mem_block = (struct pico_mem_block*) byteptr; + DBG_MM("\tHeap:\n"); + while(mem_block->type == HEAP_BLOCK_TYPE) + { + DBG_MM("\t\tBlock: size %u, %s\n", mem_block->internals.heap_block.size, (mem_block->internals.heap_block.free == HEAP_BLOCK_FREE) ? "free" : "not free"); + byteptr = (uint8_t*) mem_block + sizeof(struct pico_mem_block); + byteptr += mem_block->internals.heap_block.size; + mem_block = (struct pico_mem_block*) byteptr; + } + page = page->next_page; + } + } +} + +void pico_mem_profile_collect_data(struct profiling_data*profiling_struct) +{ + struct pico_mem_block*mem_block; + uint8_t*byteptr; + + profiling_struct->free_heap_space = 0; + profiling_struct->free_slab_space = 0; + profiling_struct->used_heap_space = 0; + profiling_struct->used_slab_space = 0; + if(manager != NULL) + { + struct pico_mem_page*page = manager->first_page; + while(page != NULL) + { + profiling_struct->free_slab_space += page->slab_size * page->slabs_free; + profiling_struct->used_slab_space += page->slab_size * page->slabs_max; + + byteptr = (uint8_t*) page + sizeof(struct pico_mem_page); + mem_block = (struct pico_mem_block*) byteptr; + + while(mem_block->type == HEAP_BLOCK_TYPE) + { + if(mem_block->internals.heap_block.free == HEAP_BLOCK_FREE) + { + profiling_struct->free_heap_space += mem_block->internals.heap_block.size; + } + else + { + /* dbg("Block: size=%u\n", mem_block->internals.heap_block.size); */ + profiling_struct->used_heap_space += mem_block->internals.heap_block.size; + } + + byteptr += sizeof(struct pico_mem_block) + mem_block->internals.heap_block.size; + mem_block = (struct pico_mem_block*) byteptr; + } + page = page->next_page; + } + } +} + +uint32_t pico_mem_profile_used_size() +{ + if(manager != NULL) + { + return manager->used_size; + } + else + { + return 0; + } +} + +struct pico_mem_manager*pico_mem_profile_manager() +{ + return manager; +} +#endif /* PICO_SUPPORT_MM_PROFILING */ + diff --git a/ext/picotcp/modules/pico_mm.h b/ext/picotcp/modules/pico_mm.h new file mode 100644 index 0000000..29366d0 --- /dev/null +++ b/ext/picotcp/modules/pico_mm.h @@ -0,0 +1,98 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Gustav Janssens, Jonas Van Nieuwenberg, Sam Van Den Berge + *********************************************************************/ + + +#ifndef _INCLUDE_PICO_MM +#define _INCLUDE_PICO_MM + +#include "pico_config.h" + +/* + * Memory init function, this will create a memory manager instance + * A memory_manager page will be created, along with one page of memory + * Memory can be asked for via the pico_mem_zalloc function + * More memory will be allocated to the memory manager according to its needs + * A maximum amount of memory of uint32_t memsize can be allocated + */ +void pico_mem_init(uint32_t memsize); +/* + * Memory deinit function, this will free all memory occupied by the current + * memory manager instance. + */ +void pico_mem_deinit(void); +/* + * Zero-initialized malloc function, will reserve a memory segment of length uint32_t len + * This memory will be quickly allocated in a slab of fixed size if possible + * or less optimally in the heap for a small variable size + * The fixed size of the slabs can be changed dynamically via a statistics engine + */ +void*pico_mem_zalloc(size_t len); +/* + * Free function, free a block of memory pointed to by ptr. + * Unused memory is only returned to the system's control by pico_mem_cleanup + */ +void pico_mem_free(void*ptr); +/* + * This cleanup function will be provided by the memory manager + * It can be called during processor downtime + * This function will return unused pages to the system's control + * Pages are unused if they no longer contain slabs or heap, and they have been idle for a longer time + */ +void pico_mem_cleanup(uint32_t timestamp); + + + +#ifdef PICO_SUPPORT_MM_PROFILING +/*********************************************************************************************************************** + *********************************************************************************************************************** + MEMORY PROFILING FUNCTIONS + *********************************************************************************************************************** + ***********************************************************************************************************************/ +/* General info struct */ +struct profiling_data +{ + uint32_t free_heap_space; + uint32_t free_slab_space; + uint32_t used_heap_space; + uint32_t used_slab_space; +}; + +/* + * This function fills up a struct with used and free slab and heap space in the memory manager + * The user is responsible for resource managment + */ +void pico_mem_profile_collect_data(struct profiling_data*profiling_page_struct); + +/* + * This function prints the general structure of the memory manager + * Printf in this function can be rerouted to send this data over a serial port, or to write it away to memory + */ +void pico_mem_profile_scan_data(void); + +/* + * This function returns the total size that the manager has received from the system + * This can give an indication of the total system resource commitment, but keep in mind that + * there can be many free blocks in this "used" size + * Together with pico_mem_profile_collect_data, this can give a good estimation of the total + * resource commitment + */ +uint32_t pico_mem_profile_used_size(void); + +/* + * This function returns a pointer to page 0, the main memory manager housekeeping (struct pico_mem_manager). + * This can be used to collect data about the memory in user defined functions. + * Use with care! + */ +void*pico_mem_profile_manager(void); + +/* + * paramter manager is a pointer to a struct pico_mem_manager + */ +void pico_mem_init_profiling(void*manager, uint32_t memsize); +#endif /* PICO_SUPPORT_MM_PROFILING */ + +#endif /* _INCLUDE_PICO_MM */ diff --git a/ext/picotcp/modules/pico_nat.c b/ext/picotcp/modules/pico_nat.c new file mode 100644 index 0000000..19f85e0 --- /dev/null +++ b/ext/picotcp/modules/pico_nat.c @@ -0,0 +1,576 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + Authors: Kristof Roelants, Brecht Van Cauwenberghe, + Simon Maes, Philippe Mariman + *********************************************************************/ + +#include "pico_stack.h" +#include "pico_frame.h" +#include "pico_tcp.h" +#include "pico_udp.h" +#include "pico_ipv4.h" +#include "pico_addressing.h" +#include "pico_nat.h" + +#ifdef PICO_SUPPORT_IPV4 +#ifdef PICO_SUPPORT_NAT + +#define nat_dbg(...) do {} while(0) +/* #define nat_dbg dbg */ +#define PICO_NAT_TIMEWAIT 240000 /* msec (4 mins) */ + +#define PICO_NAT_INBOUND 0 +#define PICO_NAT_OUTBOUND 1 + +struct pico_nat_tuple { + uint8_t proto; + uint16_t conn_active : 11; + uint16_t portforward : 1; + uint16_t rst : 1; + uint16_t syn : 1; + uint16_t fin_in : 1; + uint16_t fin_out : 1; + uint16_t src_port; + uint16_t dst_port; + uint16_t nat_port; + struct pico_ip4 src_addr; + struct pico_ip4 dst_addr; + struct pico_ip4 nat_addr; +}; + +static struct pico_ipv4_link *nat_link = NULL; + +static int nat_cmp_natport(struct pico_nat_tuple *a, struct pico_nat_tuple *b) +{ + + if (a->nat_port < b->nat_port) + return -1; + + if (a->nat_port > b->nat_port) + + return 1; + + return 0; + +} + +static int nat_cmp_srcport(struct pico_nat_tuple *a, struct pico_nat_tuple *b) +{ + + if (a->src_port < b->src_port) + return -1; + + if (a->src_port > b->src_port) + + return 1; + + return 0; + +} + +static int nat_cmp_proto(struct pico_nat_tuple *a, struct pico_nat_tuple *b) +{ + if (a->proto < b->proto) + return -1; + + if (a->proto > b->proto) + return 1; + + return 0; +} + +static int nat_cmp_address(struct pico_nat_tuple *a, struct pico_nat_tuple *b) +{ + return pico_ipv4_compare(&a->src_addr, &b->src_addr); +} + +static int nat_cmp_inbound(void *ka, void *kb) +{ + struct pico_nat_tuple *a = ka, *b = kb; + int cport = nat_cmp_natport(a, b); + if (cport) + return cport; + + return nat_cmp_proto(a, b); +} + + +static int nat_cmp_outbound(void *ka, void *kb) +{ + struct pico_nat_tuple *a = ka, *b = kb; + int caddr, cport; + + caddr = nat_cmp_address(a, b); + if (caddr) + return caddr; + + cport = nat_cmp_srcport(a, b); + + if (cport) + return cport; + + return nat_cmp_proto(a, b); +} + +PICO_TREE_DECLARE(NATOutbound, nat_cmp_outbound); +PICO_TREE_DECLARE(NATInbound, nat_cmp_inbound); + +void pico_ipv4_nat_print_table(void) +{ + struct pico_nat_tuple *t = NULL; + struct pico_tree_node *index = NULL; + (void)t; + + nat_dbg("++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"); + nat_dbg("+ NAT table +\n"); + nat_dbg("+------------------------------------------------------------------------------------------------------------------------+\n"); + nat_dbg("+ src_addr | src_port | dst_addr | dst_port | nat_addr | nat_port | proto | conn active | FIN1 | FIN2 | SYN | RST | FORW +\n"); + nat_dbg("+------------------------------------------------------------------------------------------------------------------------+\n"); + + pico_tree_foreach(index, &NATOutbound) + { + t = index->keyValue; + nat_dbg("+ %08X | %05u | %08X | %05u | %08X | %05u | %03u | %03u | %u | %u | %u | %u | %u +\n", + long_be(t->src_addr.addr), t->src_port, long_be(t->dst_addr.addr), t->dst_port, long_be(t->nat_addr.addr), t->nat_port, + t->proto, t->conn_active, t->fin_in, t->fin_out, t->syn, t->rst, t->portforward); + } + nat_dbg("++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"); +} + +/* + 2 options: + find on nat_port and proto + find on src_addr, src_port and proto + zero the unused parameters + */ +static struct pico_nat_tuple *pico_ipv4_nat_find_tuple(uint16_t nat_port, struct pico_ip4 *src_addr, uint16_t src_port, uint8_t proto) +{ + struct pico_nat_tuple *found = NULL, test = { + 0 + }; + + test.nat_port = nat_port; + test.src_port = src_port; + test.proto = proto; + if (src_addr) + test.src_addr = *src_addr; + + if (nat_port) + found = pico_tree_findKey(&NATInbound, &test); + else + found = pico_tree_findKey(&NATOutbound, &test); + + if (found) + return found; + else + return NULL; +} + +int pico_ipv4_nat_find(uint16_t nat_port, struct pico_ip4 *src_addr, uint16_t src_port, uint8_t proto) +{ + struct pico_nat_tuple *t = NULL; + + t = pico_ipv4_nat_find_tuple(nat_port, src_addr, src_port, proto); + if (t) + return 1; + else + return 0; +} + +static struct pico_nat_tuple *pico_ipv4_nat_add(struct pico_ip4 dst_addr, uint16_t dst_port, struct pico_ip4 src_addr, uint16_t src_port, + struct pico_ip4 nat_addr, uint16_t nat_port, uint8_t proto) +{ + struct pico_nat_tuple *t = PICO_ZALLOC(sizeof(struct pico_nat_tuple)); + if (!t) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + t->dst_addr = dst_addr; + t->dst_port = dst_port; + t->src_addr = src_addr; + t->src_port = src_port; + t->nat_addr = nat_addr; + t->nat_port = nat_port; + t->proto = proto; + t->conn_active = 1; + t->portforward = 0; + t->rst = 0; + t->syn = 0; + t->fin_in = 0; + t->fin_out = 0; + + if (pico_tree_insert(&NATOutbound, t)) { + PICO_FREE(t); + return NULL; + } + + if (pico_tree_insert(&NATInbound, t)) { + pico_tree_delete(&NATOutbound, t); + PICO_FREE(t); + return NULL; + } + + return t; +} + +static int pico_ipv4_nat_del(uint16_t nat_port, uint8_t proto) +{ + struct pico_nat_tuple *t = NULL; + t = pico_ipv4_nat_find_tuple(nat_port, NULL, 0, proto); + if (t) { + pico_tree_delete(&NATOutbound, t); + pico_tree_delete(&NATInbound, t); + PICO_FREE(t); + } + + return 0; +} + +static struct pico_trans *pico_nat_generate_tuple_trans(struct pico_ipv4_hdr *net, struct pico_frame *f) +{ + struct pico_trans *trans = NULL; + switch (net->proto) { + case PICO_PROTO_TCP: + { + struct pico_tcp_hdr *tcp = (struct pico_tcp_hdr *)f->transport_hdr; + trans = (struct pico_trans *)&tcp->trans; + break; + } + case PICO_PROTO_UDP: + { + struct pico_udp_hdr *udp = (struct pico_udp_hdr *)f->transport_hdr; + trans = (struct pico_trans *)&udp->trans; + break; + } + case PICO_PROTO_ICMP4: + /* XXX: implement */ + break; + } + return trans; +} + +static struct pico_nat_tuple *pico_ipv4_nat_generate_tuple(struct pico_frame *f) +{ + struct pico_trans *trans = NULL; + struct pico_ipv4_hdr *net = (struct pico_ipv4_hdr *)f->net_hdr; + uint16_t nport = 0; + uint8_t retry = 32; + + /* generate NAT port */ + do { + uint32_t rand = pico_rand(); + nport = (uint16_t) (rand & 0xFFFFU); + nport = (uint16_t)((nport % (65535 - 1024)) + 1024U); + nport = short_be(nport); + + if (pico_is_port_free(net->proto, nport, NULL, &pico_proto_ipv4)) + break; + } while (--retry); + + if (!retry) + return NULL; + + trans = pico_nat_generate_tuple_trans(net, f); + if(!trans) + return NULL; + + return pico_ipv4_nat_add(net->dst, trans->dport, net->src, trans->sport, nat_link->address, nport, net->proto); + /* XXX return pico_ipv4_nat_add(nat_link->address, port, net->src, trans->sport, net->proto); */ +} + +static inline void pico_ipv4_nat_set_tcp_flags(struct pico_nat_tuple *t, struct pico_frame *f, uint8_t direction) +{ + struct pico_tcp_hdr *tcp = (struct pico_tcp_hdr *)f->transport_hdr; + if (tcp->flags & PICO_TCP_SYN) + t->syn = 1; + + if (tcp->flags & PICO_TCP_RST) + t->rst = 1; + + if ((tcp->flags & PICO_TCP_FIN) && (direction == PICO_NAT_INBOUND)) + t->fin_in = 1; + + if ((tcp->flags & PICO_TCP_FIN) && (direction == PICO_NAT_OUTBOUND)) + t->fin_out = 1; +} + +static int pico_ipv4_nat_sniff_session(struct pico_nat_tuple *t, struct pico_frame *f, uint8_t direction) +{ + struct pico_ipv4_hdr *net = (struct pico_ipv4_hdr *)f->net_hdr; + + switch (net->proto) { + case PICO_PROTO_TCP: + { + pico_ipv4_nat_set_tcp_flags(t, f, direction); + break; + } + + case PICO_PROTO_UDP: + t->conn_active = 1; + break; + + case PICO_PROTO_ICMP4: + /* XXX: implement */ + break; + + default: + return -1; + } + + return 0; +} + +static void pico_ipv4_nat_table_cleanup(pico_time now, void *_unused) +{ + struct pico_tree_node *index = NULL, *_tmp = NULL; + struct pico_nat_tuple *t = NULL; + IGNORE_PARAMETER(now); + IGNORE_PARAMETER(_unused); + nat_dbg("NAT: before table cleanup:\n"); + pico_ipv4_nat_print_table(); + + pico_tree_foreach_reverse_safe(index, &NATOutbound, _tmp) + { + t = index->keyValue; + switch (t->proto) + { + case PICO_PROTO_TCP: + if (t->portforward) + break; + else if (t->conn_active == 0 || t->conn_active > 360) /* conn active for > 24 hours */ + pico_ipv4_nat_del(t->nat_port, t->proto); + else if (t->rst || (t->fin_in && t->fin_out)) + t->conn_active = 0; + else + t->conn_active++; + + break; + + case PICO_PROTO_UDP: + if (t->portforward) + break; + else if (t->conn_active > 1) + pico_ipv4_nat_del(t->nat_port, t->proto); + else + t->conn_active++; + + break; + + case PICO_PROTO_ICMP4: + if (t->conn_active > 1) + pico_ipv4_nat_del(t->nat_port, t->proto); + else + t->conn_active++; + + default: + /* unknown protocol in NAT table, delete when it has existed NAT_TIMEWAIT */ + if (t->conn_active > 1) + pico_ipv4_nat_del(t->nat_port, t->proto); + else + t->conn_active++; + } + } + + nat_dbg("NAT: after table cleanup:\n"); + pico_ipv4_nat_print_table(); + pico_timer_add(PICO_NAT_TIMEWAIT, pico_ipv4_nat_table_cleanup, NULL); +} + +int pico_ipv4_port_forward(struct pico_ip4 nat_addr, uint16_t nat_port, struct pico_ip4 src_addr, uint16_t src_port, uint8_t proto, uint8_t flag) +{ + struct pico_nat_tuple *t = NULL; + struct pico_ip4 any_addr = { + 0 + }; + uint16_t any_port = 0; + + switch (flag) + { + case PICO_NAT_PORT_FORWARD_ADD: + t = pico_ipv4_nat_add(any_addr, any_port, src_addr, src_port, nat_addr, nat_port, proto); + if (!t) { + pico_err = PICO_ERR_EAGAIN; + return -1; + } + + t->portforward = 1; + break; + + case PICO_NAT_PORT_FORWARD_DEL: + return pico_ipv4_nat_del(nat_port, proto); + + default: + pico_err = PICO_ERR_EINVAL; + return -1; + } + + pico_ipv4_nat_print_table(); + return 0; +} + +int pico_ipv4_nat_inbound(struct pico_frame *f, struct pico_ip4 *link_addr) +{ + struct pico_nat_tuple *tuple = NULL; + struct pico_trans *trans = NULL; + struct pico_ipv4_hdr *net = (struct pico_ipv4_hdr *)f->net_hdr; + + if (!pico_ipv4_nat_is_enabled(link_addr)) + return -1; + + switch (net->proto) { +#ifdef PICO_SUPPORT_TCP + case PICO_PROTO_TCP: + { + struct pico_tcp_hdr *tcp = (struct pico_tcp_hdr *)f->transport_hdr; + trans = (struct pico_trans *)&tcp->trans; + tuple = pico_ipv4_nat_find_tuple(trans->dport, 0, 0, net->proto); + if (!tuple) + return -1; + + /* replace dst IP and dst PORT */ + net->dst = tuple->src_addr; + trans->dport = tuple->src_port; + /* recalculate CRC */ + tcp->crc = 0; + tcp->crc = short_be(pico_tcp_checksum_ipv4(f)); + break; + } +#endif +#ifdef PICO_SUPPORT_UDP + case PICO_PROTO_UDP: + { + struct pico_udp_hdr *udp = (struct pico_udp_hdr *)f->transport_hdr; + trans = (struct pico_trans *)&udp->trans; + tuple = pico_ipv4_nat_find_tuple(trans->dport, 0, 0, net->proto); + if (!tuple) + return -1; + + /* replace dst IP and dst PORT */ + net->dst = tuple->src_addr; + trans->dport = tuple->src_port; + /* recalculate CRC */ + udp->crc = 0; + udp->crc = short_be(pico_udp_checksum_ipv4(f)); + break; + } +#endif + case PICO_PROTO_ICMP4: + /* XXX reimplement */ + break; + + default: + nat_dbg("NAT ERROR: inbound NAT on erroneous protocol\n"); + return -1; + } + + pico_ipv4_nat_sniff_session(tuple, f, PICO_NAT_INBOUND); + net->crc = 0; + net->crc = short_be(pico_checksum(net, f->net_len)); + + nat_dbg("NAT: inbound translation {dst.addr, dport}: {%08X,%u} -> {%08X,%u}\n", + tuple->nat_addr.addr, short_be(tuple->nat_port), tuple->src_addr.addr, short_be(tuple->src_port)); + + return 0; +} + +int pico_ipv4_nat_outbound(struct pico_frame *f, struct pico_ip4 *link_addr) +{ + struct pico_nat_tuple *tuple = NULL; + struct pico_trans *trans = NULL; + struct pico_ipv4_hdr *net = (struct pico_ipv4_hdr *)f->net_hdr; + + if (!pico_ipv4_nat_is_enabled(link_addr)) + return -1; + + switch (net->proto) { +#ifdef PICO_SUPPORT_TCP + case PICO_PROTO_TCP: + { + struct pico_tcp_hdr *tcp = (struct pico_tcp_hdr *)f->transport_hdr; + trans = (struct pico_trans *)&tcp->trans; + tuple = pico_ipv4_nat_find_tuple(0, &net->src, trans->sport, net->proto); + if (!tuple) + tuple = pico_ipv4_nat_generate_tuple(f); + + /* replace src IP and src PORT */ + net->src = tuple->nat_addr; + trans->sport = tuple->nat_port; + /* recalculate CRC */ + tcp->crc = 0; + tcp->crc = short_be(pico_tcp_checksum_ipv4(f)); + break; + } +#endif +#ifdef PICO_SUPPORT_UDP + case PICO_PROTO_UDP: + { + struct pico_udp_hdr *udp = (struct pico_udp_hdr *)f->transport_hdr; + trans = (struct pico_trans *)&udp->trans; + tuple = pico_ipv4_nat_find_tuple(0, &net->src, trans->sport, net->proto); + if (!tuple) + tuple = pico_ipv4_nat_generate_tuple(f); + + /* replace src IP and src PORT */ + net->src = tuple->nat_addr; + trans->sport = tuple->nat_port; + /* recalculate CRC */ + udp->crc = 0; + udp->crc = short_be(pico_udp_checksum_ipv4(f)); + break; + } +#endif + case PICO_PROTO_ICMP4: + /* XXX reimplement */ + break; + + default: + nat_dbg("NAT ERROR: outbound NAT on erroneous protocol\n"); + return -1; + } + + pico_ipv4_nat_sniff_session(tuple, f, PICO_NAT_OUTBOUND); + net->crc = 0; + net->crc = short_be(pico_checksum(net, f->net_len)); + + nat_dbg("NAT: outbound translation {src.addr, sport}: {%08X,%u} -> {%08X,%u}\n", + tuple->src_addr.addr, short_be(tuple->src_port), tuple->nat_addr.addr, short_be(tuple->nat_port)); + + return 0; +} + +int pico_ipv4_nat_enable(struct pico_ipv4_link *link) +{ + if (link == NULL) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + nat_link = link; + pico_timer_add(PICO_NAT_TIMEWAIT, pico_ipv4_nat_table_cleanup, NULL); + return 0; +} + +int pico_ipv4_nat_disable(void) +{ + nat_link = NULL; + return 0; +} + +int pico_ipv4_nat_is_enabled(struct pico_ip4 *link_addr) +{ + if (!nat_link) + return 0; + + if (nat_link->address.addr != link_addr->addr) + return 0; + + return 1; +} + +#endif +#endif diff --git a/ext/picotcp/modules/pico_nat.h b/ext/picotcp/modules/pico_nat.h new file mode 100644 index 0000000..5237501 --- /dev/null +++ b/ext/picotcp/modules/pico_nat.h @@ -0,0 +1,90 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + Authors: Kristof Roelants, Simon Maes, Brecht Van Cauwenberghe + *********************************************************************/ + +#ifndef INCLUDE_PICO_NAT +#define INCLUDE_PICO_NAT +#include "pico_frame.h" + +#define PICO_NAT_PORT_FORWARD_DEL 0 +#define PICO_NAT_PORT_FORWARD_ADD 1 + +#ifdef PICO_SUPPORT_NAT +void pico_ipv4_nat_print_table(void); +int pico_ipv4_nat_find(uint16_t nat_port, struct pico_ip4 *src_addr, uint16_t src_port, uint8_t proto); +int pico_ipv4_port_forward(struct pico_ip4 nat_addr, uint16_t nat_port, struct pico_ip4 src_addr, uint16_t src_port, uint8_t proto, uint8_t flag); + +int pico_ipv4_nat_inbound(struct pico_frame *f, struct pico_ip4 *link_addr); +int pico_ipv4_nat_outbound(struct pico_frame *f, struct pico_ip4 *link_addr); +int pico_ipv4_nat_enable(struct pico_ipv4_link *link); +int pico_ipv4_nat_disable(void); +int pico_ipv4_nat_is_enabled(struct pico_ip4 *link_addr); +#else + +#define pico_ipv4_nat_print_table() do {} while(0) +static inline int pico_ipv4_nat_inbound(struct pico_frame *f, struct pico_ip4 *link_addr) +{ + (void)f; + (void)link_addr; + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; +} + +static inline int pico_ipv4_nat_outbound(struct pico_frame *f, struct pico_ip4 *link_addr) +{ + (void)f; + (void)link_addr; + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; +} + +static inline int pico_ipv4_nat_enable(struct pico_ipv4_link *link) +{ + (void)link; + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; +} + +static inline int pico_ipv4_nat_disable(void) +{ + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; +} + +static inline int pico_ipv4_nat_is_enabled(struct pico_ip4 *link_addr) +{ + (void)link_addr; + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; +} + +static inline int pico_ipv4_nat_find(uint16_t nat_port, struct pico_ip4 *src_addr, uint16_t src_port, uint8_t proto) +{ + (void)nat_port; + (void)src_addr; + (void)src_port; + (void)proto; + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; +} + +static inline int pico_ipv4_port_forward(struct pico_ip4 nat_addr, uint16_t nat_port, struct pico_ip4 src_addr, uint16_t src_port, uint8_t proto, uint8_t flag) +{ + (void)nat_addr; + (void)nat_port; + (void)src_addr; + (void)src_port; + (void)proto; + (void)flag; + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; +} +#endif + +#endif /* _INCLUDE_PICO_NAT */ + diff --git a/ext/picotcp/modules/pico_olsr.c b/ext/picotcp/modules/pico_olsr.c new file mode 100644 index 0000000..e5300a9 --- /dev/null +++ b/ext/picotcp/modules/pico_olsr.c @@ -0,0 +1,1143 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Daniele Lacamera + ********************************************************************/ + +#include "pico_stack.h" +#include "pico_config.h" +#include "pico_device.h" +#include "pico_ipv4.h" +#include "pico_arp.h" +#include "pico_socket.h" +#include "pico_olsr.h" +#ifdef PICO_SUPPORT_OLSR +#define DGRAM_MAX_SIZE (100 - 28) +#define MAX_OLSR_MEM (4 * DGRAM_MAX_SIZE) +#define olsr_dbg(...) do {} while(0) + +int OOM(void); + + +#define OLSR_HELLO_INTERVAL ((uint32_t)5000) +#define OLSR_TC_INTERVAL ((uint32_t)9000) +#define OLSR_MAXJITTER ((uint32_t)(OLSR_HELLO_INTERVAL >> 2)) +static const struct pico_ip4 HOST_NETMASK = { + 0xffffffff +}; +#ifndef MIN +# define MIN(a, b) (a < b ? a : b) +#endif + +#define fresher(a, b) ((a > b) || ((b - a) > 32768)) + + +static uint16_t msg_counter; /* Global message sequence number */ + +/* Objects */ +struct olsr_dev_entry +{ + struct olsr_dev_entry *next; + struct pico_device *dev; + uint16_t pkt_counter; +}; + + +/* OLSR Protocol */ +#define OLSRMSG_HELLO 0xc9 +#define OLSRMSG_MID 0x03 +#define OLSRMSG_TC 0xca + +#define OLSRLINK_SYMMETRIC 0x06 +#define OLSRLINK_UNKNOWN 0x08 +#define OLSRLINK_MPR 0x0a + + +#define OLSR_PORT (short_be((uint16_t)698)) + + +/* Headers */ + +PACKED_STRUCT_DEF olsr_link +{ + uint8_t link_code; + uint8_t reserved; + uint16_t link_msg_size; +}; + +PACKED_STRUCT_DEF olsr_neighbor +{ + uint32_t addr; + uint8_t lq; + uint8_t nlq; + uint16_t reserved; +}; + +PACKED_STRUCT_DEF olsr_hmsg_hello +{ + uint16_t reserved; + uint8_t htime; + uint8_t willingness; +}; + +PACKED_STRUCT_DEF olsr_hmsg_tc +{ + uint16_t ansn; + uint16_t reserved; +}; + + +PACKED_STRUCT_DEF olsrmsg +{ + uint8_t type; + uint8_t vtime; + uint16_t size; + struct pico_ip4 orig; + uint8_t ttl; + uint8_t hop; + uint16_t seq; +}; + +PACKED_STRUCT_DEF olsrhdr +{ + uint16_t len; + uint16_t seq; +}; + + + +/* Globals */ +static struct pico_socket *udpsock = NULL; +uint16_t my_ansn = 0; +static struct olsr_route_entry *Local_interfaces = NULL; +static struct olsr_dev_entry *Local_devices = NULL; + +static struct olsr_dev_entry *olsr_get_deventry(struct pico_device *dev) +{ + struct olsr_dev_entry *cur = Local_devices; + while(cur) { + if (cur->dev == dev) + return cur; + + cur = cur->next; + } + return NULL; +} + +struct olsr_route_entry *olsr_get_ethentry(struct pico_device *vif) +{ + struct olsr_route_entry *cur = Local_interfaces; + while(cur) { + if (cur->iface == vif) + return cur; + + cur = cur->next; + } + return NULL; +} + +static struct olsr_route_entry *get_next_hop(struct olsr_route_entry *dst) +{ + struct olsr_route_entry *hop = dst; + while(hop) { + /* olsr_dbg("Finding next hop to %08x m=%d\n", hop->destination.addr, hop->metric); */ + if(hop->metric <= 1) + return hop; + + hop = hop->gateway; + } + return NULL; +} + +static inline void olsr_route_add(struct olsr_route_entry *el) +{ + /* char dest[16],nxdest[16]; */ + struct olsr_route_entry *nexthop; + + if(!el) + return; + + my_ansn++; + + nexthop = get_next_hop(el); + if (el->gateway && nexthop && (nexthop->destination.addr != el->destination.addr)) { + /* 2-hops route or more */ + el->next = el->gateway->children; + el->gateway->children = el; + el->link_type = OLSRLINK_MPR; + olsr_dbg("[OLSR] ----------Adding route to %08x via %08x metric %d\n", el->destination.addr, nexthop->destination.addr, el->metric); + pico_ipv4_route_add(el->destination, HOST_NETMASK, nexthop->destination, (int) el->metric, NULL); + } else if (el->iface) { + /* neighbor */ + struct olsr_route_entry *ei = olsr_get_ethentry(el->iface); + struct pico_ip4 no_gw = { + 0U + }; + if (el->link_type == OLSRLINK_UNKNOWN) + el->link_type = OLSRLINK_SYMMETRIC; + + if (ei) { + el->next = ei->children; + ei->children = el; + } + + olsr_dbg("[OLSR] ----------Adding neighbor %08x iface %s\n", el->destination.addr, el->iface->name); + + pico_ipv4_route_add(el->destination, HOST_NETMASK, no_gw, 1, pico_ipv4_link_by_dev(el->iface)); + } +} + +static inline void olsr_route_del(struct olsr_route_entry *r) +{ + struct olsr_route_entry *cur, *prev = NULL, *lst; + /* olsr_dbg("[OLSR] DELETING route..................\n"); */ + my_ansn++; + if (r->gateway) { + lst = r->gateway->children; + } else if (r->iface) { + lst = olsr_get_ethentry(r->iface); + } else { + lst = Local_interfaces; + } + + cur = lst, prev = NULL; + while(cur) { + if (cur == r) { + /* found */ + if (r->gateway) { + pico_ipv4_route_del(r->destination, HOST_NETMASK, r->metric); + olsr_dbg("[OLSR] Deleting route to %08x \n", r->destination.addr); + if (!prev) + r->gateway->children = r->next; + else + prev->next = r->next; + } + + while (r->children) { + olsr_route_del(r->children); + /* Orphans must die. */ + /* PICO_FREE(r->children); */ + } + return; + } + + prev = cur; + cur = cur->next; + } +} + +static struct olsr_route_entry *get_route_by_address(struct olsr_route_entry *lst, uint32_t ip) +{ + struct olsr_route_entry *found; + if(lst) { + if (lst->destination.addr == ip) { + return lst; + } + + /* recursive function, could be dangerous for stack overflow if a lot of routes are available... */ + found = get_route_by_address(lst->children, ip); + if (found) + return found; + + found = get_route_by_address(lst->next, ip); + if (found) + return found; + } + + return NULL; +} + +#define OLSR_C_SHIFT (uint32_t)4 /* 1/16 */ +#define DEFAULT_VTIME 288UL + +static uint8_t seconds2olsr(uint32_t seconds) +{ + uint16_t a, b; + /* olsr_dbg("seconds=%u\n", (uint16_t)seconds); */ + + if (seconds > 32767) + seconds = 32767; + + /* find largest b such as seconds/C >= 2^b */ + for (b = 1; b <= 0x0fu; b++) { + if ((uint16_t)(seconds * 16u) < (1u << b)) { + b--; + break; + } + } + /* olsr_dbg("b=%u", b); */ + /* compute the expression 16*(T/(C*(2^b))-1), which may not be a + integer, and round it up. This results in the value for 'a' */ + /* a = (T / ( C * (1u << b) ) ) - 1u; */ + { + uint16_t den = ((uint16_t)(1u << b) >> 4u); + /* olsr_dbg(" den=%u ", den); */ + if (den == 0) + { + /* olsr_dbg("div by 0!\n"); */ + den = 1u; + } + + a = (uint16_t)(((uint16_t)seconds / den) - (uint16_t)1); + } + /* a = a & 0x0Fu; */ + + /* olsr_dbg(" a=%u\n", a); */ + + /* if 'a' is equal to 16: increment 'b' by one, and set 'a' to 0 */ + if (16u == a) { + b++; + a = 0u; + } + + return (uint8_t)((a << 4u) + b); +} + +static uint32_t olsr2seconds(uint8_t olsr) +{ + uint8_t a, b; + uint16_t seconds; + /* olsr_dbg("olsr format: %u -- ", olsr); */ + a = (olsr >> 4) & 0xFu; + b = olsr & 0x0f; + /* olsr_dbg("o2s: a=%u, b=%u\n", a,b); */ + if (b < 4) + seconds = (uint16_t)(((1u << b) + (uint16_t)(((uint16_t)(a << b) >> 4u) & 0xFu)) >> OLSR_C_SHIFT); + else + seconds = (uint16_t)(((1u << b) + (uint16_t)(((uint16_t)(a << (b - 4))) & 0xFu)) >> OLSR_C_SHIFT); + + /* olsr_dbg("o2s: seconds: %u\n", seconds); */ + return seconds; +} + +static void olsr_garbage_collector(struct olsr_route_entry *sublist) +{ + if(!sublist) + return; + + if (sublist->time_left <= 0) { + olsr_route_del(sublist); + PICO_FREE(sublist); + return; + } else { + /* sublist->time_left -= 2u; */ + sublist->time_left -= 8u; + } + + olsr_garbage_collector(sublist->children); + olsr_garbage_collector(sublist->next); +} + +struct olsr_fwd_pkt +{ + void *buf; + uint16_t len; + struct pico_device *pdev; +}; + +static uint32_t buffer_mem_used = 0U; + +static void olsr_process_out(pico_time now, void *arg) +{ + struct olsr_fwd_pkt *p = (struct olsr_fwd_pkt *)arg; + struct pico_ip4 bcast; + struct pico_ipv4_link *addr; + struct olsr_dev_entry *pdev = Local_devices; + struct olsrhdr *ohdr; + (void)now; + + /* Send the thing out */ + ohdr = (struct olsrhdr *)p->buf; + ohdr->len = short_be((uint16_t)p->len); + + if (p->pdev) { + struct olsr_dev_entry *odev = olsr_get_deventry(p->pdev); + if (!odev) { + goto out_free; + } + + addr = pico_ipv4_link_by_dev(p->pdev); + if (!addr) + goto out_free; + + ohdr->seq = short_be((uint16_t)(odev->pkt_counter)++); + if (addr->address.addr) + bcast.addr = (addr->netmask.addr & addr->address.addr) | (~addr->netmask.addr); + else + bcast.addr = 0xFFFFFFFFu; + + if ( 0 > pico_socket_sendto(udpsock, p->buf, p->len, &bcast, OLSR_PORT)) { + olsr_dbg("olsr send\n"); + } + } else { + while(pdev) { + ohdr->seq = short_be((uint16_t)(pdev->pkt_counter++)); + addr = pico_ipv4_link_by_dev(pdev->dev); + if (!addr) + continue; + + if (addr->address.addr) + bcast.addr = (addr->netmask.addr & addr->address.addr) | (~addr->netmask.addr); + else + bcast.addr = 0xFFFFFFFFu; + + if ( 0 > pico_socket_sendto(udpsock, p->buf, p->len, &bcast, OLSR_PORT)) { + olsr_dbg("olsr send\n"); + } + + pdev = pdev->next; + } + } + +out_free: + PICO_FREE(p->buf); /* XXX <-- broken? */ + buffer_mem_used -= DGRAM_MAX_SIZE; + PICO_FREE(p); +} + +static void olsr_scheduled_output(uint32_t when, void *buffer, uint16_t size, struct pico_device *pdev) +{ + struct olsr_fwd_pkt *p; + /* olsr_dbg("Scheduling olsr packet, type:%s, size: %x\n", when == OLSR_HELLO_INTERVAL?"HELLO":"TC", size); */ + if ((buffer_mem_used + DGRAM_MAX_SIZE) > MAX_OLSR_MEM) { + PICO_FREE(buffer); + return; + } + + p = PICO_ZALLOC(sizeof(struct olsr_fwd_pkt)); + if (!p) { + OOM(); + PICO_FREE(buffer); + return; + } + + p->buf = buffer; + p->len = size; + p->pdev = pdev; + buffer_mem_used += DGRAM_MAX_SIZE; + pico_timer_add(1 + when - ((pico_rand() % OLSR_MAXJITTER)), &olsr_process_out, p); +} + + +static void refresh_routes(void) +{ + struct olsr_route_entry *local; + struct olsr_dev_entry *icur = Local_devices; + + /* Refresh local entries */ + /* Step 1: set zero expire time for local addresses and neighbors*/ + local = Local_interfaces; + while(local) { + local = local->next; + } + /* Step 2: refresh timer for entries that are still valid. + * Add new entries. + */ + while(icur) { + struct pico_ipv4_link *lnk = NULL; + do { + lnk = pico_ipv4_link_by_dev_next(icur->dev, lnk); + if (!lnk) break; + + local = olsr_get_ethentry(icur->dev); + if (local) { + local->time_left = (OLSR_HELLO_INTERVAL << 2); + } else if (lnk) { + struct olsr_route_entry *e = PICO_ZALLOC(sizeof (struct olsr_route_entry)); + if (!e) { + olsr_dbg("olsr: adding local route entry\n"); + OOM(); + return; + } + + e->destination.addr = lnk->address.addr; /* Always pick the first address */ + e->time_left = (OLSR_HELLO_INTERVAL << 2); + e->iface = icur->dev; + e->metric = 0; + e->lq = 0xFF; + e->nlq = 0xFF; + e->next = Local_interfaces; + Local_interfaces = e; + } + } while (lnk); + + /* disabled if device type != eth */ + /* refresh_neighbors(icur->dev); */ + icur = icur->next; + } +} + +static uint32_t olsr_build_hello_neighbors(uint8_t *buf, uint32_t size, struct olsr_route_entry **bookmark) +{ + uint32_t ret = 0; + struct olsr_route_entry *local, *neighbor; + struct olsr_neighbor *dst = (struct olsr_neighbor *) buf; + uint32_t total_link_size = sizeof(struct olsr_neighbor) + sizeof(struct olsr_link); + local = Local_interfaces; + while (local) { + neighbor = local->children; + if (*bookmark) { + while ((neighbor) && *bookmark != neighbor) + neighbor = neighbor->next; + } + + while (neighbor) { + struct olsr_link *li = (struct olsr_link *) (buf + ret); + + if ((size - ret) < total_link_size) { + /* Incomplete list, new datagram needed. */ + *bookmark = neighbor; + return ret; + } + + li->link_code = neighbor->link_type; + li->reserved = 0; + li->link_msg_size = short_be((uint16_t)total_link_size); + ret += (uint32_t)sizeof(struct olsr_link); + dst = (struct olsr_neighbor *) (buf + ret); + dst->addr = neighbor->destination.addr; + dst->nlq = neighbor->nlq; + dst->lq = neighbor->lq; + dst->reserved = 0; + ret += (uint32_t)sizeof(struct olsr_neighbor); + neighbor = neighbor->next; + } + local = local->next; + } + *bookmark = NULL; /* All the list was visited, no more dgrams needed */ + return ret; +} + +static uint32_t olsr_build_tc_neighbors(uint8_t *buf, uint32_t size, struct olsr_route_entry **bookmark) +{ + uint32_t ret = 0; + struct olsr_route_entry *local, *neighbor; + struct olsr_neighbor *dst = (struct olsr_neighbor *) buf; + local = Local_interfaces; + while (local) { + neighbor = local->children; + if (*bookmark) { + while ((neighbor) && *bookmark != neighbor) + neighbor = neighbor->next; + } + + while (neighbor) { + if (size - ret < sizeof(struct olsr_neighbor)) { + /* Incomplete list, new datagram needed. */ + *bookmark = neighbor; + return ret; + } + + dst->addr = neighbor->destination.addr; + dst->nlq = neighbor->nlq; + dst->lq = neighbor->lq; + dst->reserved = 0; + ret += (uint32_t)sizeof(struct olsr_neighbor); + dst = (struct olsr_neighbor *) (buf + ret); + neighbor = neighbor->next; + } + local = local->next; + } + *bookmark = NULL; /* All the list was visited, no more dgrams needed */ + return ret; +} + +static uint32_t olsr_build_mid(uint8_t *buf, uint32_t size, struct pico_device *excluded) +{ + uint32_t ret = 0; + struct olsr_route_entry *local; + struct pico_ip4 *dst = (struct pico_ip4 *) buf; + local = Local_interfaces; + while (local) { + if (local->iface != excluded) { + dst->addr = local->destination.addr; + ret += (uint32_t)sizeof(uint32_t); + dst = (struct pico_ip4 *) (buf + ret); + if (ret >= size) + return (uint32_t)(ret - sizeof(uint32_t)); + } + + local = local->next; + } + return ret; +} + + +static void olsr_compose_tc_dgram(struct pico_device *pdev, struct pico_ipv4_link *ep) +{ + struct olsrmsg *msg_tc, *msg_mid; + uint32_t size = 0, r; + struct olsr_route_entry *last_neighbor = NULL; + uint8_t *dgram; + struct olsr_hmsg_tc *tc; + do { + dgram = PICO_ZALLOC(DGRAM_MAX_SIZE); + if (!dgram) { + OOM(); + return; + } + + size = (uint32_t)sizeof(struct olsrhdr); + ep = pico_ipv4_link_by_dev(pdev); + if (!ep) { + PICO_FREE(dgram); + return; + } + + + if (!last_neighbor) { + /* MID Message */ + + msg_mid = (struct olsrmsg *)(dgram + size); + size += (uint32_t)sizeof(struct olsrmsg); + msg_mid->type = OLSRMSG_MID; + msg_mid->vtime = seconds2olsr(60); + msg_mid->orig.addr = ep->address.addr; + msg_mid->ttl = 0xFF; + msg_mid->hop = 0; + msg_mid->seq = short_be(msg_counter++); + r = olsr_build_mid(dgram + size, DGRAM_MAX_SIZE - size, pdev); + if (r == 0) { + size -= (uint32_t)sizeof(struct olsrmsg); + } else { + if ((size + r) > DGRAM_MAX_SIZE) + return; + + size += r; + msg_mid->size = short_be((uint16_t)(sizeof(struct olsrmsg) + r)); + } + } + + if (size + sizeof(struct olsrmsg) > DGRAM_MAX_SIZE) + return; + + msg_tc = (struct olsrmsg *) (dgram + size); + size += (uint32_t)sizeof(struct olsrmsg); + msg_tc->type = OLSRMSG_TC; + msg_tc->vtime = seconds2olsr(DEFAULT_VTIME); + msg_tc->orig.addr = ep->address.addr; + msg_tc->ttl = 0xFF; + msg_tc->hop = 0; + msg_tc->seq = short_be(msg_counter++); + tc = (struct olsr_hmsg_tc *)(dgram + size); + size += (uint32_t)sizeof(struct olsr_hmsg_tc); + if (size > DGRAM_MAX_SIZE) + return; + + tc->ansn = short_be(my_ansn); + r = olsr_build_tc_neighbors(dgram + size, DGRAM_MAX_SIZE - size, &last_neighbor); + size += r; + msg_tc->size = short_be((uint16_t)(sizeof(struct olsrmsg) + sizeof(struct olsr_hmsg_tc) + r)); + olsr_scheduled_output(OLSR_TC_INTERVAL, dgram, (uint16_t)size, pdev ); + } while(last_neighbor); +} + +static void olsr_compose_hello_dgram(struct pico_device *pdev, struct pico_ipv4_link *ep) +{ + struct olsrmsg *msg_hello; + uint32_t size = 0, r; + struct olsr_route_entry *last_neighbor = NULL; + uint8_t *dgram; + struct olsr_hmsg_hello *hello; + /* HELLO Message */ + do { + dgram = PICO_ZALLOC(DGRAM_MAX_SIZE); + if (!dgram) { + OOM(); + return; + } + + size = (uint32_t)sizeof(struct olsrhdr); + msg_hello = (struct olsrmsg *) (dgram + size); + size += (uint32_t)sizeof(struct olsrmsg); + msg_hello->type = OLSRMSG_HELLO; + msg_hello->vtime = seconds2olsr(DEFAULT_VTIME); + msg_hello->orig.addr = ep->address.addr; + msg_hello->ttl = 1; + msg_hello->hop = 0; + msg_hello->seq = short_be(msg_counter++); + hello = (struct olsr_hmsg_hello *)(dgram + size); + size += (uint32_t)sizeof(struct olsr_hmsg_hello); + hello->reserved = 0; + hello->htime = seconds2olsr(OLSR_HELLO_INTERVAL); + hello->htime = 0x05; /* Todo: find and define values */ + hello->willingness = 0x07; + if (DGRAM_MAX_SIZE > size) { + r = olsr_build_hello_neighbors(dgram + size, DGRAM_MAX_SIZE - size, &last_neighbor); + if (r == 0) { + /* olsr_dbg("Building hello message\n"); */ + PICO_FREE(dgram); + return; + } + } + + size += r; + msg_hello->size = short_be((uint16_t)(sizeof(struct olsrmsg) + sizeof(struct olsr_hmsg_hello) + r)); + olsr_scheduled_output(OLSR_HELLO_INTERVAL, dgram, (uint16_t)size, pdev ); + } while(last_neighbor); +} + +static void olsr_make_dgram(struct pico_device *pdev, int full) +{ + struct pico_ipv4_link *ep; + ep = pico_ipv4_link_by_dev(pdev); + if (!ep) { + return; + } + + if (!full) { + olsr_compose_hello_dgram(pdev, ep); + } else { + olsr_compose_tc_dgram(pdev, ep); + } /*if full */ + +} + +/* Old code was relying on ethernet arp requests */ +#define arp_storm(...) do {} while(0) + +static void recv_mid(uint8_t *buffer, uint32_t len, struct olsr_route_entry *origin) +{ + uint32_t parsed = 0; + uint32_t *address; + struct olsr_route_entry *e; + + if (len % sizeof(uint32_t)) /*drop*/ + return; + + while (len > parsed) { + address = (uint32_t *)(buffer + parsed); + e = get_route_by_address(Local_interfaces, *address); + if (!e) { + e = PICO_ZALLOC(sizeof(struct olsr_route_entry)); + if (!e) { + olsr_dbg("olsr allocating route\n"); + OOM(); + return; + } + + e->time_left = (OLSR_HELLO_INTERVAL << 2); + e->destination.addr = *address; + e->gateway = origin; + /* e->iface = origin->iface; */ + e->iface = NULL; + e->metric = (uint16_t)(origin->metric + 1u); + e->lq = origin->lq; + e->nlq = origin->nlq; + olsr_route_add(e); + arp_storm(&e->destination); + } else if (e->metric > (origin->metric + 1)) { + olsr_route_del(e); + e->metric = (uint16_t)(origin->metric + 1u); + e->gateway = origin; + e->time_left = (OLSR_HELLO_INTERVAL << 2); + olsr_route_add(e); + } + + parsed += (uint32_t)sizeof(uint32_t); + } +} + +/* static void recv_hello(uint8_t *buffer, uint32_t len, struct olsr_route_entry *origin) */ +static void recv_hello(uint8_t *buffer, uint32_t len, struct olsr_route_entry *origin, uint16_t hops) +{ + struct olsr_link *li; + struct olsr_route_entry *e; + uint32_t parsed = 0; + struct olsr_neighbor *neigh; + + if (!origin) + return; + + /* Don't parse hello messages that were forwarded */ + if (hops > 0 || origin->metric > 1) + return; + + if (pico_ipv4_link_get(&origin->destination)) + return; + + + while (len > parsed) { + li = (struct olsr_link *) buffer; + neigh = (struct olsr_neighbor *)(buffer + parsed + sizeof(struct olsr_link)); + parsed += short_be(li->link_msg_size); + e = get_route_by_address(Local_interfaces, neigh->addr); + if (!e) { + e = PICO_ZALLOC(sizeof(struct olsr_route_entry)); + if (!e) { + olsr_dbg("olsr allocating route\n"); + OOM(); + return; + } + + e->time_left = (OLSR_HELLO_INTERVAL << 2); + e->destination.addr = neigh->addr; + e->gateway = origin; + e->iface = NULL; + e->metric = (uint16_t)(origin->metric + hops + 1); + e->link_type = OLSRLINK_UNKNOWN; + e->lq = MIN(origin->lq, neigh->lq); + e->nlq = MIN(origin->nlq, neigh->nlq); + olsr_route_add(e); + arp_storm(&e->destination); + } else if ((e->gateway != origin) && (origin->metric > 1) && (e->metric > (origin->metric + hops + 1))) { + olsr_route_del(e); + e->metric = (uint16_t)(origin->metric + hops + 1); + e->gateway = origin; + e->time_left = (OLSR_HELLO_INTERVAL << 2); + olsr_route_add(e); + } else { + e->time_left = (OLSR_HELLO_INTERVAL << 2); + } + } +} + +/* static uint32_t reconsider_topology(uint8_t *buf, uint32_t size, struct olsr_route_entry *e) */ +static uint32_t reconsider_topology(uint8_t *buf, uint32_t size, struct olsr_route_entry *e) +{ + struct olsr_hmsg_tc *tc = (struct olsr_hmsg_tc *) buf; + uint16_t new_ansn = short_be(tc->ansn); + uint32_t parsed = sizeof(struct olsr_hmsg_tc); + struct olsr_route_entry *rt; + struct olsr_neighbor *n; + uint32_t retval = 0; + + if (!e->advertised_tc) + retval = 1; + + if (e->advertised_tc && fresher(new_ansn, e->ansn)) + { + PICO_FREE(e->advertised_tc); /* <--- XXX check invalid free? */ + e->advertised_tc = NULL; + retval = 1; + } + + /* Ignore self packets */ + if (pico_ipv4_link_get(&e->destination)) + return 0; + + if (!e->advertised_tc) { + e->advertised_tc = PICO_ZALLOC(size); + if (!e->advertised_tc) { + OOM(); + olsr_dbg("Allocating forward packet\n"); + return 0; + } + + memcpy(e->advertised_tc, buf, size); + e->ansn = new_ansn; + while (parsed < size) { + n = (struct olsr_neighbor *) (buf + parsed); + parsed += (uint32_t)sizeof(struct olsr_neighbor); + rt = get_route_by_address(Local_interfaces, n->addr); + if (rt && (rt->gateway == e)) { + /* Refresh existing node */ + rt->time_left = e->time_left; + } else if (!rt || (rt->metric > (e->metric + 1)) || (rt->nlq < n->nlq)) { + if (!rt) { + rt = PICO_ZALLOC(sizeof (struct olsr_route_entry)); + if (!rt) { + OOM(); + return retval; + } + + rt->destination.addr = n->addr; + rt->link_type = OLSRLINK_UNKNOWN; + } else { + olsr_route_del(rt); + } + + rt->iface = e->iface; + rt->gateway = e; + rt->metric = (uint16_t)(e->metric + 1); + rt->lq = n->lq; /* 0xff */ + rt->nlq = n->nlq; /* 0xff */ + rt->time_left = e->time_left; /* 256 */ + olsr_route_add(rt); + } + } + /* olsr_dbg("Routes changed...\n"); */ + } + + return retval; +} + + +static void olsr_recv(uint8_t *buffer, uint32_t len) +{ + struct olsrmsg *msg; + struct olsrhdr *oh = (struct olsrhdr *) buffer; + struct olsr_route_entry *ancestor; + uint32_t parsed = 0; + uint16_t outsize = 0; + uint8_t *datagram; + + if (len != short_be(oh->len)) { + return; + } + + /* RFC 3626, section 3.4, if a packet is too small, it is silently discarded */ + if (len < 16) { + return; + } + + parsed += (uint32_t)sizeof(struct olsrhdr); + + datagram = PICO_ZALLOC(DGRAM_MAX_SIZE); + if (!datagram) { + OOM(); + return; + } + + outsize = (uint16_t) (outsize + (sizeof(struct olsrhdr))); + /* Section 1: parsing received messages. */ + while (len > parsed) { + struct olsr_route_entry *origin; + msg = (struct olsrmsg *) (buffer + parsed); + origin = get_route_by_address(Local_interfaces, msg->orig.addr); + + if(pico_ipv4_link_find(&msg->orig) != NULL) { + /* olsr_dbg("rebound\n"); */ + parsed += short_be(msg->size); + continue; + } + + /* OLSR's TTL expired. */ + if (msg->ttl < 1u) { + parsed += short_be(msg->size); + continue; + } + + if (!origin) { + if (msg->hop == 0) { + struct olsr_route_entry *e = PICO_ZALLOC(sizeof (struct olsr_route_entry)); + if (!e) { + parsed += short_be(msg->size); + OOM(); + break; + } + + e->destination.addr = msg->orig.addr; + e->link_type = OLSRLINK_SYMMETRIC; + e->time_left = (OLSR_HELLO_INTERVAL << 2); + e->iface = Local_devices->dev; + e->gateway = olsr_get_ethentry(e->iface); + e->metric = 1; + e->lq = 0xFF; + e->nlq = 0xFF; + olsr_route_add(e); + } + + parsed += short_be(msg->size); + continue; + } + + /* We know this is a Master host and a neighbor */ + origin->link_type = OLSRLINK_MPR; + origin->time_left = olsr2seconds(msg->vtime); + switch(msg->type) { + case OLSRMSG_HELLO: + ancestor = olsr_get_ethentry(origin->iface); + if ((origin->metric > 1) && ancestor) { + olsr_route_del(origin); + origin->gateway = ancestor; + origin->metric = 1; + olsr_route_add(origin); + } + + recv_hello(buffer + (uint32_t)parsed + (uint32_t)sizeof(struct olsrmsg) + (uint32_t)sizeof(struct olsr_hmsg_hello), + (uint32_t) ((short_be(msg->size) - (sizeof(struct olsrmsg))) - (uint32_t)sizeof(struct olsr_hmsg_hello)), + origin, msg->hop); + msg->ttl = 0; + break; + case OLSRMSG_MID: + if ((origin->seq != 0) && (!fresher(short_be(msg->seq), origin->seq))) { + msg->ttl = 0; + } else { + recv_mid(buffer + parsed + sizeof(struct olsrmsg), (uint32_t)(short_be(msg->size) - (sizeof(struct olsrmsg))), origin); + /* olsr_dbg("MID forwarded from origin %08x (seq: %u)\n", long_be(msg->orig.addr), short_be(msg->seq)); */ + origin->seq = short_be(msg->seq); + } + + break; + case OLSRMSG_TC: + if(!pico_ipv4_link_find(&origin->destination)) { + reconsider_topology(buffer + parsed + sizeof(struct olsrmsg), (uint32_t)(short_be(msg->size) - (sizeof(struct olsrmsg))), origin); + if ((origin->seq != 0) && (!fresher(short_be(msg->seq), origin->seq))) { + msg->ttl = 0; + } else { + /* olsr_dbg("TC forwarded from origin %08x (seq: %u)\n", long_be(msg->orig.addr), short_be(msg->seq)); */ + origin->seq = short_be(msg->seq); + } + } + + break; + default: + PICO_FREE(datagram); + return; + } + + if (msg->ttl > 1) { + msg->hop++; + msg->ttl--; + memcpy(datagram + outsize, msg, short_be(msg->size)); + outsize = (uint16_t)(outsize + short_be(msg->size)); + } + + parsed += short_be(msg->size); + } + /* Section 2: forwarding parsed messages that got past the filter. */ + if ((outsize > sizeof(struct olsrhdr))) { + /* Finalize FWD packet */ + olsr_scheduled_output(OLSR_MAXJITTER, datagram, outsize, NULL); + } else { + /* Nothing to forward. */ + PICO_FREE(datagram); + } +} + +static void wakeup(uint16_t ev, struct pico_socket *s) +{ + unsigned char *recvbuf; + int r = 0; + struct pico_ip4 ANY = { + 0 + }; + uint16_t port = OLSR_PORT; + recvbuf = PICO_ZALLOC(DGRAM_MAX_SIZE); + if (!recvbuf) { + OOM(); + return; + } + + if (ev & PICO_SOCK_EV_RD) { + r = pico_socket_recv(s, recvbuf, DGRAM_MAX_SIZE); + if (r > 0) + olsr_recv(recvbuf, (uint32_t)r); + } + + if (ev == PICO_SOCK_EV_ERR) { + pico_socket_close(udpsock); + udpsock = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_UDP, &wakeup); + if (udpsock) + pico_socket_bind(udpsock, &ANY, &port); + } + + PICO_FREE(recvbuf); +} + +static void olsr_hello_tick(pico_time when, void *unused) +{ + struct olsr_dev_entry *d; + (void)when; + (void)unused; + olsr_garbage_collector(Local_interfaces); + refresh_routes(); + d = Local_devices; + while(d) { + olsr_make_dgram(d->dev, 0); + d = d->next; + } + pico_timer_add(OLSR_HELLO_INTERVAL, &olsr_hello_tick, NULL); +} + +static void olsr_tc_tick(pico_time when, void *unused) +{ + struct olsr_dev_entry *d; + (void)when; + (void)unused; + d = Local_devices; + while(d) { + olsr_make_dgram(d->dev, 1); + d = d->next; + } + pico_timer_add(OLSR_TC_INTERVAL, &olsr_tc_tick, NULL); +} + + +/* Public interface */ + +void pico_olsr_init(void) +{ + struct pico_ip4 ANY = { + 0 + }; + uint16_t port = OLSR_PORT; + olsr_dbg("OLSR initialized.\n"); + if (!udpsock) { + udpsock = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_UDP, &wakeup); + if (udpsock) + pico_socket_bind(udpsock, &ANY, &port); + } + + pico_timer_add(pico_rand() % 100, &olsr_hello_tick, NULL); + pico_timer_add(pico_rand() % 900, &olsr_tc_tick, NULL); +} + + +int OOM(void) +{ + volatile int c = 3600; + c++; + c++; + c++; + return -1; +} + +int pico_olsr_add(struct pico_device *dev) +{ + struct pico_ipv4_link *lnk = NULL; + struct olsr_dev_entry *od; + + + if (!dev) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* olsr_dbg("OLSR: Adding device %s\n", dev->name); */ + od = PICO_ZALLOC(sizeof(struct olsr_dev_entry)); + if (!od) { + pico_err = PICO_ERR_ENOMEM; + /* OOM(); */ + return -1; + } + + od->dev = dev; + od->next = Local_devices; + Local_devices = od; + + do { + char ipaddr[20]; + lnk = pico_ipv4_link_by_dev_next(dev, lnk); + if (lnk) { + struct olsr_route_entry *e = PICO_ZALLOC(sizeof(struct olsr_route_entry)); + /* olsr_dbg("OLSR: Found IP address %08x\n", long_be(lnk->address.addr)); */ + pico_ipv4_to_string(ipaddr, (lnk->address.addr)); + /* olsr_dbg("OLSR: Found IP address %s\n", ipaddr); */ + if (!e) { + olsr_dbg("olsr allocating route\n"); + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + e->destination.addr = lnk->address.addr; + e->link_type = OLSRLINK_SYMMETRIC; + e->time_left = (OLSR_HELLO_INTERVAL << 2); + e->gateway = NULL; + e->children = NULL; + e->iface = dev; + e->metric = 0; + e->lq = 0xFF; + e->nlq = 0xFF; + e->next = Local_interfaces; + Local_interfaces = e; + + } + } while(lnk); + + return 0; +} + +#endif diff --git a/ext/picotcp/modules/pico_olsr.h b/ext/picotcp/modules/pico_olsr.h new file mode 100644 index 0000000..11c3bf0 --- /dev/null +++ b/ext/picotcp/modules/pico_olsr.h @@ -0,0 +1,32 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Daniele Lacamera + *********************************************************************/ +#ifndef PICO_OLSR_H +#define PICO_OLSR_H + + +/* Objects */ +struct olsr_route_entry +{ + struct olsr_route_entry *next; + uint32_t time_left; + struct pico_ip4 destination; + struct olsr_route_entry *gateway; + struct pico_device *iface; + uint16_t metric; + uint8_t link_type; + struct olsr_route_entry *children; + uint16_t ansn; + uint16_t seq; + uint8_t lq, nlq; + uint8_t *advertised_tc; +}; + + +void pico_olsr_init(void); +int pico_olsr_add(struct pico_device *dev); +struct olsr_route_entry *olsr_get_ethentry(struct pico_device *vif); +#endif diff --git a/ext/picotcp/modules/pico_posix.c b/ext/picotcp/modules/pico_posix.c new file mode 100644 index 0000000..4820b12 --- /dev/null +++ b/ext/picotcp/modules/pico_posix.c @@ -0,0 +1,99 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Andrei Carp, Maarten Vandersteegen + *********************************************************************/ + +#ifdef PICO_SUPPORT_THREADING + +#include +#include +#include "pico_config.h" + +/* POSIX mutex implementation */ +void *pico_mutex_init(void) +{ + pthread_mutex_t *m; + m = (pthread_mutex_t *)PICO_ZALLOC(sizeof(pthread_mutex_t)); + pthread_mutex_init(m, NULL); + return m; +} + +void pico_mutex_destroy(void *mux) +{ + PICO_FREE(mux); + mux = NULL; +} + +void pico_mutex_lock(void *mux) +{ + if (mux == NULL) return; + + pthread_mutex_t *m = (pthread_mutex_t *)mux; + pthread_mutex_lock(m); +} + +void pico_mutex_unlock(void *mux) +{ + if (mux == NULL) return; + + pthread_mutex_t *m = (pthread_mutex_t *)mux; + pthread_mutex_unlock(m); +} + +/* POSIX semaphore implementation */ +void *pico_sem_init(void) +{ + sem_t *s; + s = (sem_t *)PICO_ZALLOC(sizeof(sem_t)); + sem_init(s, 0, 0); + return s; +} + +void pico_sem_destroy(void *sem) +{ + PICO_FREE(sem); + sem = NULL; +} + +void pico_sem_post(void *sem) +{ + if (sem == NULL) return; + + sem_t *s = (sem_t *)sem; + sem_post(s); +} + +int pico_sem_wait(void *sem, int timeout) +{ + struct timespec t; + if (sem == NULL) return 0; + + sem_t *s = (sem_t *)sem; + + if (timeout < 0) { + sem_wait(s); + } else { + clock_gettime(CLOCK_REALTIME, &t); + t.tv_sec += timeout / 1000; + t.tv_nsec += (timeout % 1000) * 1000000; + if (sem_timedwait(s, &t) == -1) + return -1; + } + + return 0; +} + +/* POSIX thread implementation */ +void *pico_thread_create(void *(*routine)(void *), void *arg) +{ + pthread_t *thread; + thread = (pthread_t *)PICO_ZALLOC(sizeof(pthread_t)); + + if (pthread_create(thread, NULL, routine, arg) == -1) + return NULL; + + return thread; +} +#endif /* PICO_SUPPORT_THREADING */ diff --git a/ext/picotcp/modules/pico_slaacv4.c b/ext/picotcp/modules/pico_slaacv4.c new file mode 100644 index 0000000..f9ac216 --- /dev/null +++ b/ext/picotcp/modules/pico_slaacv4.c @@ -0,0 +1,266 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Bogdan Lupu + *********************************************************************/ +#include "pico_slaacv4.h" +#include "pico_arp.h" +#include "pico_constants.h" +#include "pico_stack.h" +#include "pico_hotplug_detection.h" + +#ifdef PICO_SUPPORT_SLAACV4 + +#define SLAACV4_NETWORK ((long_be(0xa9fe0000))) +#define SLAACV4_NETMASK ((long_be(0xFFFF0000))) +#define SLAACV4_MINRANGE (0x00000100) /* In host order */ +#define SLAACV4_MAXRANGE (0x0000FDFF) /* In host order */ + +#define SLAACV4_CREATE_IPV4(seed) ((long_be((seed % SLAACV4_MAXRANGE) + SLAACV4_MINRANGE) & ~SLAACV4_NETMASK) | SLAACV4_NETWORK) + +#define PROBE_WAIT 1 /* delay between two tries during claim */ +#define PROBE_NB 3 /* number of probe packets during claim */ +/* #define PROBE_MIN 1 */ +/* #define PROBE_MAX 2 */ +#define ANNOUNCE_WAIT 2 /* delay before start announcing */ +#define ANNOUNCE_NB 2 /* number of announcement packets */ +#define ANNOUNCE_INTERVAL 2 /* time between announcement packets */ +#define MAX_CONFLICTS 10 /* max conflicts before rate limiting */ +#define MAX_CONFLICTS_FAIL 20 /* max conflicts before declaring failure */ +#define RATE_LIMIT_INTERVAL 60 /* time between successive attempts */ +#define DEFEND_INTERVAL 10 /* minimum interval between defensive ARP */ + +enum slaacv4_state { + SLAACV4_RESET = 0, + SLAACV4_CLAIMING, + SLAACV4_CLAIMED, + SLAACV4_ANNOUNCING, + SLAACV4_ERROR +}; + +struct slaacv4_cookie { + enum slaacv4_state state; + uint8_t probe_try_nb; + uint8_t conflict_nb; + uint8_t announce_nb; + struct pico_ip4 ip; + struct pico_device *device; + uint32_t timer; + void (*cb)(struct pico_ip4 *ip, uint8_t code); +}; + +static void pico_slaacv4_hotplug_cb(struct pico_device *dev, int event); + +static struct slaacv4_cookie slaacv4_local; + +static uint32_t pico_slaacv4_getip(struct pico_device *dev, uint8_t rand) +{ + uint32_t seed = 0; + if (dev->eth != NULL) + { + seed = pico_hash((const uint8_t *)dev->eth->mac.addr, PICO_SIZE_ETH); + } + + if (rand) + { + seed += pico_rand(); + } + + return SLAACV4_CREATE_IPV4(seed); +} + +static void pico_slaacv4_init_cookie(struct pico_ip4 *ip, struct pico_device *dev, struct slaacv4_cookie *ck, void (*cb)(struct pico_ip4 *ip, uint8_t code)) +{ + ck->state = SLAACV4_RESET; + ck->probe_try_nb = 0; + ck->conflict_nb = 0; + ck->announce_nb = 0; + ck->cb = cb; + ck->device = dev; + ck->ip.addr = ip->addr; + ck->timer = 0; +} + +static void pico_slaacv4_cancel_timers(struct slaacv4_cookie *tmp) +{ + pico_timer_cancel(tmp->timer); + tmp->timer = 0; +} + +static void pico_slaacv4_send_announce_timer(pico_time now, void *arg) +{ + struct slaacv4_cookie *tmp = (struct slaacv4_cookie *)arg; + struct pico_ip4 netmask = { 0 }; + netmask.addr = long_be(0xFFFF0000); + + (void)now; + + if (tmp->announce_nb < ANNOUNCE_NB) + { + pico_arp_request(tmp->device, &tmp->ip, PICO_ARP_ANNOUNCE); + tmp->announce_nb++; + tmp->timer = pico_timer_add(ANNOUNCE_INTERVAL * 1000, pico_slaacv4_send_announce_timer, arg); + } + else + { + tmp->state = SLAACV4_CLAIMED; + pico_ipv4_link_add(tmp->device, tmp->ip, netmask); + if (tmp->cb != NULL) + tmp->cb(&tmp->ip, PICO_SLAACV4_SUCCESS); + } +} + +static void pico_slaacv4_send_probe_timer(pico_time now, void *arg) +{ + struct slaacv4_cookie *tmp = (struct slaacv4_cookie *)arg; + (void)now; + + if (tmp->probe_try_nb < PROBE_NB) + { + pico_arp_request(tmp->device, &tmp->ip, PICO_ARP_PROBE); + tmp->probe_try_nb++; + tmp->timer = pico_timer_add(PROBE_WAIT * 1000, pico_slaacv4_send_probe_timer, tmp); + } + else + { + tmp->state = SLAACV4_ANNOUNCING; + tmp->timer = pico_timer_add(ANNOUNCE_WAIT * 1000, pico_slaacv4_send_announce_timer, arg); + } +} + +static void pico_slaacv4_receive_ipconflict(int reason) +{ + struct slaacv4_cookie *tmp = &slaacv4_local; + + tmp->conflict_nb++; + pico_slaacv4_cancel_timers(tmp); + + if(tmp->state == SLAACV4_CLAIMED) + { + if(reason == PICO_ARP_CONFLICT_REASON_CONFLICT) + { + pico_ipv4_link_del(tmp->device, tmp->ip); + } + } + + if (tmp->conflict_nb < MAX_CONFLICTS) + { + tmp->state = SLAACV4_CLAIMING; + tmp->probe_try_nb = 0; + tmp->announce_nb = 0; + tmp->ip.addr = pico_slaacv4_getip(tmp->device, (uint8_t)1); + pico_arp_register_ipconflict(&tmp->ip, &tmp->device->eth->mac, pico_slaacv4_receive_ipconflict); + pico_arp_request(tmp->device, &tmp->ip, PICO_ARP_PROBE); + tmp->probe_try_nb++; + tmp->timer = pico_timer_add(PROBE_WAIT * 1000, pico_slaacv4_send_probe_timer, tmp); + } + else if (tmp->conflict_nb < MAX_CONFLICTS_FAIL) + { + tmp->state = SLAACV4_CLAIMING; + tmp->probe_try_nb = 0; + tmp->announce_nb = 0; + tmp->ip.addr = pico_slaacv4_getip(tmp->device, (uint8_t)1); + pico_arp_register_ipconflict(&tmp->ip, &tmp->device->eth->mac, pico_slaacv4_receive_ipconflict); + tmp->timer = pico_timer_add(RATE_LIMIT_INTERVAL * 1000, pico_slaacv4_send_probe_timer, tmp); + } + else + { + if (tmp->cb != NULL) + { + pico_hotplug_deregister(tmp->device, &pico_slaacv4_hotplug_cb); + tmp->cb(&tmp->ip, PICO_SLAACV4_ERROR); + } + + tmp->state = SLAACV4_ERROR; + } + +} + +static void pico_slaacv4_hotplug_cb(__attribute__((unused)) struct pico_device *dev, int event) +{ + struct slaacv4_cookie *tmp = &slaacv4_local; + + if (event == PICO_HOTPLUG_EVENT_UP ) + { + slaacv4_local.state = SLAACV4_CLAIMING; + tmp->probe_try_nb = 0; + tmp->announce_nb = 0; + + pico_arp_register_ipconflict(&tmp->ip, &tmp->device->eth->mac, pico_slaacv4_receive_ipconflict); + pico_arp_request(tmp->device, &tmp->ip, PICO_ARP_PROBE); + tmp->probe_try_nb++; + tmp->timer = pico_timer_add(PROBE_WAIT * 1000, pico_slaacv4_send_probe_timer, tmp); + + } + else + { + if (tmp->state == SLAACV4_CLAIMED ) + pico_ipv4_link_del(tmp->device, tmp->ip); + pico_slaacv4_cancel_timers(tmp); + } +} + +int pico_slaacv4_claimip(struct pico_device *dev, void (*cb)(struct pico_ip4 *ip, uint8_t code)) +{ + struct pico_ip4 ip; + + if (!dev->eth) { + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; + } + + if( dev->link_state != NULL ) + { + //hotplug detect will work + + ip.addr = pico_slaacv4_getip(dev, 0); + pico_slaacv4_init_cookie(&ip, dev, &slaacv4_local, cb); + + if (pico_hotplug_register(dev, &pico_slaacv4_hotplug_cb)) + { + return -1; + } + if (dev->link_state(dev) == 1) + { + pico_arp_register_ipconflict(&ip, &dev->eth->mac, pico_slaacv4_receive_ipconflict); + pico_arp_request(dev, &ip, PICO_ARP_PROBE); + slaacv4_local.state = SLAACV4_CLAIMING; + slaacv4_local.probe_try_nb++; + slaacv4_local.timer = pico_timer_add(PROBE_WAIT * 1000, pico_slaacv4_send_probe_timer, &slaacv4_local); + } + } + else + { + ip.addr = pico_slaacv4_getip(dev, 0); + + pico_slaacv4_init_cookie(&ip, dev, &slaacv4_local, cb); + pico_arp_register_ipconflict(&ip, &dev->eth->mac, pico_slaacv4_receive_ipconflict); + pico_arp_request(dev, &ip, PICO_ARP_PROBE); + slaacv4_local.state = SLAACV4_CLAIMING; + slaacv4_local.probe_try_nb++; + slaacv4_local.timer = pico_timer_add(PROBE_WAIT * 1000, pico_slaacv4_send_probe_timer, &slaacv4_local); + } + + return 0; +} + +void pico_slaacv4_unregisterip(void) +{ + struct slaacv4_cookie *tmp = &slaacv4_local; + struct pico_ip4 empty = { + .addr = 0x00000000 + }; + + if (tmp->state == SLAACV4_CLAIMED) + { + pico_ipv4_link_del(tmp->device, tmp->ip); + } + + pico_slaacv4_cancel_timers(tmp); + pico_slaacv4_init_cookie(&empty, NULL, tmp, NULL); + pico_arp_register_ipconflict(&tmp->ip, NULL, NULL); + pico_hotplug_deregister(tmp->device, &pico_slaacv4_hotplug_cb); +} + +#endif diff --git a/ext/picotcp/modules/pico_slaacv4.h b/ext/picotcp/modules/pico_slaacv4.h new file mode 100644 index 0000000..d90ef65 --- /dev/null +++ b/ext/picotcp/modules/pico_slaacv4.h @@ -0,0 +1,18 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Authors: Bogdan Lupu + *********************************************************************/ +#ifndef INCLUDE_PICO_SUPPORT_SLAACV4 +#define INCLUDE_PICO_SUPPORT_SLAACV4 +#include "pico_arp.h" + +#define PICO_SLAACV4_SUCCESS 0 +#define PICO_SLAACV4_ERROR 1 + +int pico_slaacv4_claimip(struct pico_device *dev, void (*cb)(struct pico_ip4 *ip, uint8_t code)); +void pico_slaacv4_unregisterip(void); + +#endif /* _INCLUDE_PICO_SUPPORT_SLAACV4 */ + diff --git a/ext/picotcp/modules/pico_sntp_client.c b/ext/picotcp/modules/pico_sntp_client.c new file mode 100644 index 0000000..10fa0af --- /dev/null +++ b/ext/picotcp/modules/pico_sntp_client.c @@ -0,0 +1,395 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Author: Toon Stegen + *********************************************************************/ +#include "pico_sntp_client.h" +#include "pico_config.h" +#include "pico_stack.h" +#include "pico_addressing.h" +#include "pico_socket.h" +#include "pico_ipv4.h" +#include "pico_ipv6.h" +#include "pico_dns_client.h" +#include "pico_tree.h" +#include "pico_stack.h" + +#ifdef PICO_SUPPORT_SNTP_CLIENT + +#define sntp_dbg(...) do {} while(0) +/* #define sntp_dbg dbg */ + +#define SNTP_VERSION 4 +#define PICO_SNTP_MAXBUF (1400) + +/* Sntp mode */ +#define SNTP_MODE_CLIENT 3 + +/* SNTP conversion parameters */ +#define SNTP_FRAC_TO_PICOSEC (4294967llu) +#define SNTP_THOUSAND (1000llu) +#define SNTP_UNIX_OFFSET (2208988800llu) /* nr of seconds from 1900 to 1970 */ +#define SNTP_BITMASK (0X00000000FFFFFFFF) /* mask to convert from 64 to 32 */ + +PACKED_STRUCT_DEF pico_sntp_ts +{ + uint32_t sec; /* Seconds */ + uint32_t frac; /* Fraction */ +}; + +PACKED_STRUCT_DEF pico_sntp_header +{ + uint8_t mode : 3; /* Mode */ + uint8_t vn : 3; /* Version number */ + uint8_t li : 2; /* Leap indicator */ + uint8_t stratum; /* Stratum */ + uint8_t poll; /* Poll, only significant in server messages */ + uint8_t prec; /* Precision, only significant in server messages */ + int32_t rt_del; /* Root delay, only significant in server messages */ + int32_t rt_dis; /* Root dispersion, only significant in server messages */ + int32_t ref_id; /* Reference clock ID, only significant in server messages */ + struct pico_sntp_ts ref_ts; /* Reference time stamp */ + struct pico_sntp_ts orig_ts; /* Originate time stamp */ + struct pico_sntp_ts recv_ts; /* Receive time stamp */ + struct pico_sntp_ts trs_ts; /* Transmit time stamp */ + +}; + +struct sntp_server_ns_cookie +{ + int rec; /* Indicates wheter an sntp packet has been received */ + uint16_t proto; /* IPV4 or IPV6 prototype */ + pico_time stamp; /* Timestamp of the moment the sntp packet is sent */ + char *hostname; /* Hostname of the (s)ntp server*/ + struct pico_socket *sock; /* Socket which contains the cookie */ + void (*cb_synced)(pico_err_t status); /* Callback function for telling the user + wheter/when the time is synchronised */ + uint32_t timer; /* Timer that will signal timeout */ +}; + +/* global variables */ +static uint16_t sntp_port = 123u; +static struct pico_timeval server_time = { + 0 +}; +static pico_time tick_stamp = 0ull; +static union pico_address sntp_inaddr_any = { + .ip6.addr = { 0 } +}; + +/*************************************************************************/ + +/* Converts a sntp time stamp to a pico_timeval struct */ +static int timestamp_convert(const struct pico_sntp_ts *ts, struct pico_timeval *tv, pico_time delay) +{ + if(long_be(ts->sec) < SNTP_UNIX_OFFSET) { + pico_err = PICO_ERR_EINVAL; + tv->tv_sec = 0; + tv->tv_msec = 0; + sntp_dbg("Error: input too low\n"); + return -1; + } + + sntp_dbg("Delay: %llu\n", delay); + tv->tv_msec = (pico_time) (((uint32_t)(long_be(ts->frac))) / SNTP_FRAC_TO_PICOSEC + delay); + tv->tv_sec = (pico_time) (long_be(ts->sec) - SNTP_UNIX_OFFSET + (uint32_t)tv->tv_msec / SNTP_THOUSAND); + tv->tv_msec = (uint32_t) (tv->tv_msec & SNTP_BITMASK) % SNTP_THOUSAND; + sntp_dbg("Converted time stamp: %llusec, %llumsec\n", tv->tv_sec, tv->tv_msec); + return 0; +} + +/* Cleanup function that is called when the time is synced or an error occured */ +static void pico_sntp_cleanup(struct sntp_server_ns_cookie *ck, pico_err_t status) +{ + sntp_dbg("Cleanup called\n"); + if(!ck) + return; + + ck->cb_synced(status); + if(ck->sock) + ck->sock->priv = NULL; + + sntp_dbg("FREE!\n"); + PICO_FREE(ck->hostname); + PICO_FREE(ck); + +} + +/* Extracts the current time from a server sntp packet*/ +static int pico_sntp_parse(char *buf, struct sntp_server_ns_cookie *ck) +{ + int ret = 0; + struct pico_sntp_header *hp = (struct pico_sntp_header*) buf; + + if(!ck) { + sntp_dbg("pico_sntp_parse: invalid cookie\n"); + return -1; + } + + sntp_dbg("Received mode: %u, version: %u, stratum: %u\n", hp->mode, hp->vn, hp->stratum); + + tick_stamp = pico_tick; + /* tick_stamp - ck->stamp is the delay between sending and receiving the ntp packet */ + ret = timestamp_convert(&(hp->trs_ts), &server_time, (tick_stamp - ck->stamp) / 2); + if(ret != 0) { + sntp_dbg("Conversion error!\n"); + pico_sntp_cleanup(ck, PICO_ERR_EINVAL); + return ret; + } + + sntp_dbg("Server time: %llu seconds and %llu milisecs since 1970\n", server_time.tv_sec, server_time.tv_msec); + + /* Call back the user saying the time is synced */ + pico_sntp_cleanup(ck, PICO_ERR_NOERR); + return ret; +} + +/* callback for UDP socket events */ +static void pico_sntp_client_wakeup(uint16_t ev, struct pico_socket *s) +{ + struct sntp_server_ns_cookie *ck = (struct sntp_server_ns_cookie *)s->priv; + char *recvbuf; + int read = 0; + uint32_t peer; + uint16_t port; + + if(!ck) { + sntp_dbg("pico_sntp_client_wakeup: invalid cookie\n"); + return; + } + + /* process read event, data available */ + if (ev == PICO_SOCK_EV_RD) { + ck->rec = 1; + /* receive while data available in socket buffer */ + recvbuf = PICO_ZALLOC(PICO_SNTP_MAXBUF); + if (!recvbuf) + return; + + do { + read = pico_socket_recvfrom(s, recvbuf, PICO_SNTP_MAXBUF, &peer, &port); + } while(read > 0); + pico_sntp_parse(recvbuf, s->priv); + pico_timer_cancel(ck->timer); + PICO_FREE(recvbuf); + } + /* socket is closed */ + else if(ev == PICO_SOCK_EV_CLOSE) { + sntp_dbg("Socket is closed. Bailing out.\n"); + pico_sntp_cleanup(ck, PICO_ERR_ENOTCONN); + return; + } + /* process error event, socket error occured */ + else if(ev == PICO_SOCK_EV_ERR) { + sntp_dbg("Socket Error received. Bailing out.\n"); + pico_sntp_cleanup(ck, PICO_ERR_ENOTCONN); + return; + } + + sntp_dbg("Received data from %08X:%u\n", peer, port); +} + +/* Function that is called after the receive timer expires */ +static void sntp_receive_timeout(pico_time now, void *arg) +{ + struct sntp_server_ns_cookie *ck = (struct sntp_server_ns_cookie *)arg; + (void) now; + + if(!ck) { + sntp_dbg("sntp_timeout: invalid cookie\n"); + return; + } + + if(!ck->rec) { + pico_sntp_cleanup(ck, PICO_ERR_ETIMEDOUT); + } +} + +/* Sends an sntp packet on sock to dst*/ +static void pico_sntp_send(struct pico_socket *sock, union pico_address *dst) +{ + struct pico_sntp_header header = { + 0 + }; + struct sntp_server_ns_cookie *ck = (struct sntp_server_ns_cookie *)sock->priv; + + if(!ck) { + sntp_dbg("pico_sntp_sent: invalid cookie\n"); + return; + } + + ck->timer = pico_timer_add(5000, sntp_receive_timeout, ck); + header.vn = SNTP_VERSION; + header.mode = SNTP_MODE_CLIENT; + /* header.trs_ts.frac = long_be(0ul); */ + ck->stamp = pico_tick; + pico_socket_sendto(sock, &header, sizeof(header), dst, short_be(sntp_port)); +} + +/* used for getting a response from DNS servers */ +static void dnsCallback(char *ip, void *arg) +{ + struct sntp_server_ns_cookie *ck = (struct sntp_server_ns_cookie *)arg; + union pico_address address; + struct pico_socket *sock; + int retval = -1; + uint16_t any_port = 0; + + if(!ck) { + sntp_dbg("dnsCallback: Invalid argument\n"); + return; + } + + if (0) { + + } +#ifdef PICO_SUPPORT_IPV6 + else if(ck->proto == PICO_PROTO_IPV6) { + if (ip) { + /* add the ip address to the client, and start a tcp connection socket */ + sntp_dbg("using IPv6 address: %s\n", ip); + retval = pico_string_to_ipv6(ip, address.ip6.addr); + } else { + sntp_dbg("Invalid query response for AAAA\n"); + retval = -1; + pico_sntp_cleanup(ck, PICO_ERR_ENETDOWN); + } + } + +#endif +#ifdef PICO_SUPPORT_IPV4 + else if(ck->proto == PICO_PROTO_IPV4) { + if(ip) { + sntp_dbg("using IPv4 address: %s\n", ip); + retval = pico_string_to_ipv4(ip, (uint32_t *)&address.ip4.addr); + } else { + sntp_dbg("Invalid query response for A\n"); + retval = -1; + pico_sntp_cleanup(ck, PICO_ERR_ENETDOWN); + } + } +#endif + + if (retval >= 0) { + sock = pico_socket_open(ck->proto, PICO_PROTO_UDP, &pico_sntp_client_wakeup); + if (!sock) + return; + + sock->priv = ck; + ck->sock = sock; + if ((pico_socket_bind(sock, &sntp_inaddr_any, &any_port) == 0)) { + pico_sntp_send(sock, &address); + } + } +} + +/* user function to sync the time from a given sntp source */ +int pico_sntp_sync(const char *sntp_server, void (*cb_synced)(pico_err_t status)) +{ + struct sntp_server_ns_cookie *ck; +#ifdef PICO_SUPPORT_IPV6 + struct sntp_server_ns_cookie *ck6; +#endif + int retval = -1, retval6 = -1; + if (sntp_server == NULL) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + /* IPv4 query */ + ck = PICO_ZALLOC(sizeof(struct sntp_server_ns_cookie)); + if (!ck) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + ck->proto = PICO_PROTO_IPV4; + ck->stamp = 0ull; + ck->rec = 0; + ck->sock = NULL; + ck->hostname = PICO_ZALLOC(strlen(sntp_server) + 1); + if (!ck->hostname) { + PICO_FREE(ck); + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + strcpy(ck->hostname, sntp_server); + + if(cb_synced == NULL) { + PICO_FREE(ck->hostname); + PICO_FREE(ck); + pico_err = PICO_ERR_EINVAL; + return -1; + } + + ck->cb_synced = cb_synced; + +#ifdef PICO_SUPPORT_IPV6 + /* IPv6 query */ + ck6 = PICO_ZALLOC(sizeof(struct sntp_server_ns_cookie)); + if (!ck6) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + ck6->proto = PICO_PROTO_IPV6; + ck6->hostname = PICO_ZALLOC(strlen(sntp_server) + 1); + if (!ck6->hostname) { + PICO_FREE(ck6); + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + strcpy(ck6->hostname, sntp_server); + ck6->proto = PICO_PROTO_IPV6; + ck6->stamp = 0ull; + ck6->rec = 0; + ck6->sock = NULL; + ck6->cb_synced = cb_synced; + sntp_dbg("Resolving AAAA %s\n", ck6->hostname); + retval6 = pico_dns_client_getaddr6(sntp_server, &dnsCallback, ck6); + if (retval6 != 0) { + PICO_FREE(ck6->hostname); + PICO_FREE(ck6); + return -1; + } + +#endif + sntp_dbg("Resolving A %s\n", ck->hostname); + retval = pico_dns_client_getaddr(sntp_server, &dnsCallback, ck); + if (retval != 0) { + PICO_FREE(ck->hostname); + PICO_FREE(ck); + return -1; + } + + return 0; +} + +/* user function to get the current time */ +int pico_sntp_gettimeofday(struct pico_timeval *tv) +{ + pico_time diff, temp; + uint32_t diffH, diffL; + int ret = 0; + if (tick_stamp == 0) { + /* TODO: set pico_err */ + ret = -1; + sntp_dbg("Error: Unsynchronised\n"); + return ret; + } + + diff = pico_tick - tick_stamp; + diffL = ((uint32_t) (diff & SNTP_BITMASK)) / 1000; + diffH = ((uint32_t) (diff >> 32)) / 1000; + + temp = server_time.tv_msec + (uint32_t)(diff & SNTP_BITMASK) % SNTP_THOUSAND; + tv->tv_sec = server_time.tv_sec + ((uint64_t)diffH << 32) + diffL + (uint32_t)temp / SNTP_THOUSAND; + tv->tv_msec = (uint32_t)(temp & SNTP_BITMASK) % SNTP_THOUSAND; + sntp_dbg("Time of day: %llu seconds and %llu milisecs since 1970\n", tv->tv_sec, tv->tv_msec); + return ret; +} + +#endif /* PICO_SUPPORT_SNTP_CLIENT */ diff --git a/ext/picotcp/modules/pico_sntp_client.h b/ext/picotcp/modules/pico_sntp_client.h new file mode 100644 index 0000000..79b02b8 --- /dev/null +++ b/ext/picotcp/modules/pico_sntp_client.h @@ -0,0 +1,22 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + Author: Toon Stegen + *********************************************************************/ +#ifndef INCLUDE_PICO_SNTP_CLIENT +#define INCLUDE_PICO_SNTP_CLIENT + +#include "pico_config.h" +#include "pico_protocol.h" + +struct pico_timeval +{ + pico_time tv_sec; + pico_time tv_msec; +}; + +int pico_sntp_sync(const char *sntp_server, void (*cb_synced)(pico_err_t status)); +int pico_sntp_gettimeofday(struct pico_timeval *tv); + +#endif /* _INCLUDE_PICO_SNTP_CLIENT */ diff --git a/ext/picotcp/modules/pico_socket_tcp.c b/ext/picotcp/modules/pico_socket_tcp.c new file mode 100644 index 0000000..ee977b8 --- /dev/null +++ b/ext/picotcp/modules/pico_socket_tcp.c @@ -0,0 +1,267 @@ +#include "pico_config.h" +#include "pico_socket.h" +#include "pico_ipv4.h" +#include "pico_ipv6.h" +#include "pico_tcp.h" +#include "pico_socket_tcp.h" + + +static int sockopt_validate_args(struct pico_socket *s, void *value) +{ + if (!value) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + if (s->proto->proto_number != PICO_PROTO_TCP) { + pico_err = PICO_ERR_EPROTONOSUPPORT; + return -1; + } + + return 0; +} + +int pico_getsockopt_tcp(struct pico_socket *s, int option, void *value) +{ + if (sockopt_validate_args(s, value) < 0) + return -1; + +#ifdef PICO_SUPPORT_TCP + if (option == PICO_TCP_NODELAY) { + /* state of the NODELAY option */ + *(int *)value = PICO_SOCKET_GETOPT(s, PICO_SOCKET_OPT_TCPNODELAY); + return 0; + } + else if (option == PICO_SOCKET_OPT_RCVBUF) { + return pico_tcp_get_bufsize_in(s, (uint32_t *)value); + } + + else if (option == PICO_SOCKET_OPT_SNDBUF) { + return pico_tcp_get_bufsize_out(s, (uint32_t *)value); + } + +#endif + return -1; +} + +static void tcp_set_nagle_option(struct pico_socket *s, void *value) +{ + int *val = (int*)value; + if (*val > 0) { + dbg("setsockopt: Nagle algorithm disabled.\n"); + PICO_SOCKET_SETOPT_EN(s, PICO_SOCKET_OPT_TCPNODELAY); + } else { + dbg("setsockopt: Nagle algorithm enabled.\n"); + PICO_SOCKET_SETOPT_DIS(s, PICO_SOCKET_OPT_TCPNODELAY); + } +} + +int pico_setsockopt_tcp(struct pico_socket *s, int option, void *value) +{ + if (sockopt_validate_args(s, value) < 0) + return -1; + +#ifdef PICO_SUPPORT_TCP + if (option == PICO_TCP_NODELAY) { + tcp_set_nagle_option(s, value); + return 0; + } + else if (option == PICO_SOCKET_OPT_RCVBUF) { + uint32_t *val = (uint32_t*)value; + pico_tcp_set_bufsize_in(s, *val); + return 0; + } + else if (option == PICO_SOCKET_OPT_SNDBUF) { + uint32_t *val = (uint32_t*)value; + pico_tcp_set_bufsize_out(s, *val); + return 0; + } + else if (option == PICO_SOCKET_OPT_KEEPCNT) { + uint32_t *val = (uint32_t*)value; + pico_tcp_set_keepalive_probes(s, *val); + return 0; + } + else if (option == PICO_SOCKET_OPT_KEEPIDLE) { + uint32_t *val = (uint32_t*)value; + pico_tcp_set_keepalive_time(s, *val); + return 0; + } + else if (option == PICO_SOCKET_OPT_KEEPINTVL) { + uint32_t *val = (uint32_t*)value; + pico_tcp_set_keepalive_intvl(s, *val); + return 0; + } + else if (option == PICO_SOCKET_OPT_LINGER) { + uint32_t *val = (uint32_t*)value; + pico_tcp_set_linger(s, *val); + return 0; + } + +#endif + pico_err = PICO_ERR_EINVAL; + return -1; +} + +void pico_socket_tcp_cleanup(struct pico_socket *sock) +{ +#ifdef PICO_SUPPORT_TCP + /* for tcp sockets go further and clean the sockets inside queue */ + if(is_sock_tcp(sock)) + pico_tcp_cleanup_queues(sock); + +#endif +} + + +void pico_socket_tcp_delete(struct pico_socket *s) +{ +#ifdef PICO_SUPPORT_TCP + if(s->parent) + s->parent->number_of_pending_conn--; + +#endif +} + +static struct pico_socket *socket_tcp_deliver_ipv4(struct pico_socket *s, struct pico_frame *f) +{ + struct pico_socket *found = NULL; + #ifdef PICO_SUPPORT_IPV4 + struct pico_ip4 s_local, s_remote, p_src, p_dst; + struct pico_ipv4_hdr *ip4hdr = (struct pico_ipv4_hdr*)(f->net_hdr); + struct pico_trans *tr = (struct pico_trans *) f->transport_hdr; + s_local.addr = s->local_addr.ip4.addr; + s_remote.addr = s->remote_addr.ip4.addr; + p_src.addr = ip4hdr->src.addr; + p_dst.addr = ip4hdr->dst.addr; + if ((s->remote_port == tr->sport) && /* remote port check */ + (s_remote.addr == p_src.addr) && /* remote addr check */ + ((s_local.addr == PICO_IPV4_INADDR_ANY) || (s_local.addr == p_dst.addr))) { /* Either local socket is ANY, or matches dst */ + found = s; + return found; + } else if ((s->remote_port == 0) && /* not connected... listening */ + ((s_local.addr == PICO_IPV4_INADDR_ANY) || (s_local.addr == p_dst.addr))) { /* Either local socket is ANY, or matches dst */ + /* listen socket */ + found = s; + } + + #endif + return found; +} + +static struct pico_socket *socket_tcp_deliver_ipv6(struct pico_socket *s, struct pico_frame *f) +{ + struct pico_socket *found = NULL; + #ifdef PICO_SUPPORT_IPV6 + struct pico_trans *tr = (struct pico_trans *) f->transport_hdr; + struct pico_ip6 s_local = {{0}}, s_remote = {{0}}, p_src = {{0}}, p_dst = {{0}}; + struct pico_ipv6_hdr *ip6hdr = (struct pico_ipv6_hdr *)(f->net_hdr); + s_local = s->local_addr.ip6; + s_remote = s->remote_addr.ip6; + p_src = ip6hdr->src; + p_dst = ip6hdr->dst; + if ((s->remote_port == tr->sport) && + (!memcmp(s_remote.addr, p_src.addr, PICO_SIZE_IP6)) && + ((!memcmp(s_local.addr, PICO_IP6_ANY, PICO_SIZE_IP6)) || (!memcmp(s_local.addr, p_dst.addr, PICO_SIZE_IP6)))) { + found = s; + return found; + } else if ((s->remote_port == 0) && /* not connected... listening */ + ((!memcmp(s_local.addr, PICO_IP6_ANY, PICO_SIZE_IP6)) || (!memcmp(s_local.addr, p_dst.addr, PICO_SIZE_IP6)))) { + /* listen socket */ + found = s; + } + + #else + (void) s; + (void) f; + #endif + return found; +} + +static int socket_tcp_do_deliver(struct pico_socket *s, struct pico_frame *f) +{ + if (s != NULL) { + pico_tcp_input(s, f); + if ((s->ev_pending) && s->wakeup) { + s->wakeup(s->ev_pending, s); + if(!s->parent) + s->ev_pending = 0; + } + + return 0; + } + + dbg("TCP SOCKET> Not s.\n"); + return -1; +} + +int pico_socket_tcp_deliver(struct pico_sockport *sp, struct pico_frame *f) +{ + struct pico_socket *found = NULL; + struct pico_tree_node *index = NULL; + struct pico_tree_node *_tmp; + struct pico_socket *s = NULL; + + + pico_tree_foreach_safe(index, &sp->socks, _tmp){ + s = index->keyValue; + /* 4-tuple identification of socket (port-IP) */ + if (IS_IPV4(f)) { + found = socket_tcp_deliver_ipv4(s, f); + } + + if (IS_IPV6(f)) { + found = socket_tcp_deliver_ipv6(s, f); + } + + if (found) + break; + } /* FOREACH */ + + return socket_tcp_do_deliver(found, f); +} + +struct pico_socket *pico_socket_tcp_open(uint16_t family) +{ + struct pico_socket *s = NULL; + (void) family; +#ifdef PICO_SUPPORT_TCP + s = pico_tcp_open(family); + if (!s) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + s->proto = &pico_proto_tcp; + /*check if Nagle enabled */ + /* + if (!IS_NAGLE_ENABLED(s)) + dbg("ERROR Nagle should be enabled here\n\n"); + */ +#endif + return s; +} + +int pico_socket_tcp_read(struct pico_socket *s, void *buf, uint32_t len) +{ +#ifdef PICO_SUPPORT_TCP + /* check if in shutdown state and if no more data in tcpq_in */ + if ((s->state & PICO_SOCKET_STATE_SHUT_REMOTE) && pico_tcp_queue_in_is_empty(s)) { + pico_err = PICO_ERR_ESHUTDOWN; + return -1; + } else { + return (int)(pico_tcp_read(s, buf, (uint32_t)len)); + } + +#else + return 0; +#endif +} + +void transport_flags_update(struct pico_frame *f, struct pico_socket *s) +{ +#ifdef PICO_SUPPORT_TCP + if(is_sock_tcp(s)) + pico_tcp_flags_update(f, s); + +#endif +} diff --git a/ext/picotcp/modules/pico_socket_tcp.h b/ext/picotcp/modules/pico_socket_tcp.h new file mode 100644 index 0000000..6479103 --- /dev/null +++ b/ext/picotcp/modules/pico_socket_tcp.h @@ -0,0 +1,33 @@ +#ifndef PICO_SOCKET_TCP_H +#define PICO_SOCKET_TCP_H +#include "pico_socket.h" + +#ifdef PICO_SUPPORT_TCP + +/* Functions/macros: conditional! */ + +# define IS_NAGLE_ENABLED(s) (!(!(!(s->opt_flags & (1 << PICO_SOCKET_OPT_TCPNODELAY))))) +int pico_setsockopt_tcp(struct pico_socket *s, int option, void *value); +int pico_getsockopt_tcp(struct pico_socket *s, int option, void *value); +int pico_socket_tcp_deliver(struct pico_sockport *sp, struct pico_frame *f); +void pico_socket_tcp_delete(struct pico_socket *s); +void pico_socket_tcp_cleanup(struct pico_socket *sock); +struct pico_socket *pico_socket_tcp_open(uint16_t family); +int pico_socket_tcp_read(struct pico_socket *s, void *buf, uint32_t len); +void transport_flags_update(struct pico_frame *, struct pico_socket *); + +#else +# define pico_getsockopt_tcp(...) (-1) +# define pico_setsockopt_tcp(...) (-1) +# define pico_socket_tcp_deliver(...) (-1) +# define IS_NAGLE_ENABLED(s) (0) +# define pico_socket_tcp_delete(...) do {} while(0) +# define pico_socket_tcp_cleanup(...) do {} while(0) +# define pico_socket_tcp_open(f) (NULL) +# define pico_socket_tcp_read(...) (-1) +# define transport_flags_update(...) do {} while(0) + +#endif + + +#endif diff --git a/ext/picotcp/modules/pico_socket_udp.c b/ext/picotcp/modules/pico_socket_udp.c new file mode 100644 index 0000000..bf3d9f8 --- /dev/null +++ b/ext/picotcp/modules/pico_socket_udp.c @@ -0,0 +1,256 @@ +#include "pico_config.h" +#include "pico_socket.h" +#include "pico_udp.h" +#include "pico_socket_multicast.h" +#include "pico_ipv4.h" +#include "pico_ipv6.h" +#include "pico_socket_udp.h" + +#define UDP_FRAME_OVERHEAD (sizeof(struct pico_frame)) + + +struct pico_socket *pico_socket_udp_open(void) +{ + struct pico_socket *s = NULL; +#ifdef PICO_SUPPORT_UDP + s = pico_udp_open(); + if (!s) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + s->proto = &pico_proto_udp; + s->q_in.overhead = UDP_FRAME_OVERHEAD; + s->q_out.overhead = UDP_FRAME_OVERHEAD; +#endif + return s; +} + + +#ifdef PICO_SUPPORT_IPV4 +static inline int pico_socket_udp_deliver_ipv4_mcast_initial_checks(struct pico_socket *s, struct pico_frame *f) +{ + struct pico_ip4 p_dst; + struct pico_ipv4_hdr *ip4hdr; + + ip4hdr = (struct pico_ipv4_hdr*)(f->net_hdr); + p_dst.addr = ip4hdr->dst.addr; + if (pico_ipv4_is_multicast(p_dst.addr) && (pico_socket_mcast_filter(s, (union pico_address *)&ip4hdr->dst, (union pico_address *)&ip4hdr->src) < 0)) + return -1; + + + if ((pico_ipv4_link_get(&ip4hdr->src)) && (PICO_SOCKET_GETOPT(s, PICO_SOCKET_OPT_MULTICAST_LOOP) == 0u)) { + /* Datagram from ourselves, Loop disabled, discarding. */ + return -1; + } + + return 0; +} + + +static int pico_socket_udp_deliver_ipv4_mcast(struct pico_socket *s, struct pico_frame *f) +{ + struct pico_ip4 s_local; + struct pico_frame *cpy; + struct pico_device *dev = pico_ipv4_link_find(&s->local_addr.ip4); + + s_local.addr = s->local_addr.ip4.addr; + + if (pico_socket_udp_deliver_ipv4_mcast_initial_checks(s, f) < 0) + return 0; + + if ((s_local.addr == PICO_IPV4_INADDR_ANY) || /* If our local ip is ANY, or.. */ + (dev == f->dev)) { /* the source of the bcast packet is a neighbor... */ + cpy = pico_frame_copy(f); + if (!cpy) + return -1; + + if (pico_enqueue(&s->q_in, cpy) > 0) { + if (s->wakeup) + s->wakeup(PICO_SOCK_EV_RD, s); + } + else + pico_frame_discard(cpy); + } + + return 0; +} + +static int pico_socket_udp_deliver_ipv4_unicast(struct pico_socket *s, struct pico_frame *f) +{ + struct pico_frame *cpy; + /* Either local socket is ANY, or matches dst */ + cpy = pico_frame_copy(f); + if (!cpy) + return -1; + + if (pico_enqueue(&s->q_in, cpy) > 0) { + if (s->wakeup) + s->wakeup(PICO_SOCK_EV_RD, s); + } else { + pico_frame_discard(cpy); + } + + return 0; +} + + +static int pico_socket_udp_deliver_ipv4(struct pico_socket *s, struct pico_frame *f) +{ + int ret = 0; + struct pico_ip4 s_local, p_dst; + struct pico_ipv4_hdr *ip4hdr; + ip4hdr = (struct pico_ipv4_hdr*)(f->net_hdr); + s_local.addr = s->local_addr.ip4.addr; + p_dst.addr = ip4hdr->dst.addr; + if ((pico_ipv4_is_broadcast(p_dst.addr)) || pico_ipv4_is_multicast(p_dst.addr)) { + ret = pico_socket_udp_deliver_ipv4_mcast(s, f); + } else if ((s_local.addr == PICO_IPV4_INADDR_ANY) || (s_local.addr == p_dst.addr)) { + ret = pico_socket_udp_deliver_ipv4_unicast(s, f); + } + + pico_frame_discard(f); + return ret; +} +#endif + +#ifdef PICO_SUPPORT_IPV6 +static inline int pico_socket_udp_deliver_ipv6_mcast(struct pico_socket *s, struct pico_frame *f) +{ + struct pico_ipv6_hdr *ip6hdr; + struct pico_frame *cpy; + struct pico_device *dev = pico_ipv6_link_find(&s->local_addr.ip6); + + ip6hdr = (struct pico_ipv6_hdr*)(f->net_hdr); + + if ((pico_ipv6_link_get(&ip6hdr->src)) && (PICO_SOCKET_GETOPT(s, PICO_SOCKET_OPT_MULTICAST_LOOP) == 0u)) { + /* Datagram from ourselves, Loop disabled, discarding. */ + return 0; + } + + + if (pico_ipv6_is_unspecified(s->local_addr.ip6.addr) || /* If our local ip is ANY, or.. */ + (dev == f->dev)) { /* the source of the bcast packet is a neighbor... */ + cpy = pico_frame_copy(f); + if (!cpy) + { + return -1; + } + + if (pico_enqueue(&s->q_in, cpy) > 0) { + if (s->wakeup) + s->wakeup(PICO_SOCK_EV_RD, s); + } + else + pico_frame_discard(cpy); + } + + return 0; +} + +static int pico_socket_udp_deliver_ipv6(struct pico_socket *s, struct pico_frame *f) +{ + struct pico_ip6 s_local, p_dst; + struct pico_ipv6_hdr *ip6hdr; + struct pico_frame *cpy; + ip6hdr = (struct pico_ipv6_hdr*)(f->net_hdr); + s_local = s->local_addr.ip6; + p_dst = ip6hdr->dst; + if ((pico_ipv6_is_multicast(p_dst.addr))) { + int retval = pico_socket_udp_deliver_ipv6_mcast(s, f); + pico_frame_discard(f); + return retval; + } + else if (pico_ipv6_is_unspecified(s->local_addr.ip6.addr) || (pico_ipv6_compare(&s_local, &p_dst) == 0)) + { /* Either local socket is ANY, or matches dst */ + cpy = pico_frame_copy(f); + if (!cpy) + { + pico_frame_discard(f); + return -1; + } + + if (pico_enqueue(&s->q_in, cpy) > 0) { + if (s->wakeup) + s->wakeup(PICO_SOCK_EV_RD, s); + } + } + + pico_frame_discard(f); + return 0; +} +#endif + + +int pico_socket_udp_deliver(struct pico_sockport *sp, struct pico_frame *f) +{ + struct pico_tree_node *index = NULL; + struct pico_tree_node *_tmp; + struct pico_socket *s = NULL; + pico_err = PICO_ERR_EPROTONOSUPPORT; + #ifdef PICO_SUPPORT_UDP + pico_err = PICO_ERR_NOERR; + pico_tree_foreach_safe(index, &sp->socks, _tmp){ + s = index->keyValue; + if (IS_IPV4(f)) { /* IPV4 */ +#ifdef PICO_SUPPORT_IPV4 + return pico_socket_udp_deliver_ipv4(s, f); +#endif + } else if (IS_IPV6(f)) { +#ifdef PICO_SUPPORT_IPV6 + return pico_socket_udp_deliver_ipv6(s, f); +#endif + } else { + /* something wrong in the packet header*/ + } + } /* FOREACH */ + pico_frame_discard(f); + if (s) + return 0; + + pico_err = PICO_ERR_ENXIO; + #endif + return -1; +} + +int pico_setsockopt_udp(struct pico_socket *s, int option, void *value) +{ + switch(option) { + case PICO_SOCKET_OPT_RCVBUF: + s->q_in.max_size = (*(uint32_t*)value); + return 0; + case PICO_SOCKET_OPT_SNDBUF: + s->q_out.max_size = (*(uint32_t*)value); + return 0; + } + + /* switch's default */ +#ifdef PICO_SUPPORT_MCAST + return pico_setsockopt_mcast(s, option, value); +#else + pico_err = PICO_ERR_EINVAL; + return -1; +#endif +} + +int pico_getsockopt_udp(struct pico_socket *s, int option, void *value) +{ + uint32_t *val = (uint32_t *)value; + switch(option) { + case PICO_SOCKET_OPT_RCVBUF: + *val = s->q_in.max_size; + return 0; + case PICO_SOCKET_OPT_SNDBUF: + *val = s->q_out.max_size; + return 0; + } + + /* switch's default */ +#ifdef PICO_SUPPORT_MCAST + return pico_getsockopt_mcast(s, option, value); +#else + pico_err = PICO_ERR_EINVAL; + return -1; +#endif +} + diff --git a/ext/picotcp/modules/pico_socket_udp.h b/ext/picotcp/modules/pico_socket_udp.h new file mode 100644 index 0000000..6b3a4c9 --- /dev/null +++ b/ext/picotcp/modules/pico_socket_udp.h @@ -0,0 +1,19 @@ +#ifndef PICO_SOCKET_UDP_H +#define PICO_SOCKET_UDP_H + +struct pico_socket *pico_socket_udp_open(void); +int pico_socket_udp_deliver(struct pico_sockport *sp, struct pico_frame *f); + + +#ifdef PICO_SUPPORT_UDP +int pico_setsockopt_udp(struct pico_socket *s, int option, void *value); +int pico_getsockopt_udp(struct pico_socket *s, int option, void *value); +# define pico_socket_udp_recv(s, buf, len, addr, port) pico_udp_recv(s, buf, len, addr, port, NULL) +#else +# define pico_socket_udp_recv(...) (0) +# define pico_getsockopt_udp(...) (-1) +# define pico_setsockopt_udp(...) (-1) +#endif + + +#endif diff --git a/ext/picotcp/modules/pico_strings.c b/ext/picotcp/modules/pico_strings.c new file mode 100644 index 0000000..974126f --- /dev/null +++ b/ext/picotcp/modules/pico_strings.c @@ -0,0 +1,101 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2015 Altran ISY BeNeLux. Some rights reserved. + See LICENSE and COPYING for usage. + + + + Author: Michele Di Pede + *********************************************************************/ + +#include +#include +#include "pico_strings.h" + +char *get_string_terminator_position(char *const block, size_t len) +{ + size_t length = pico_strnlen(block, len); + + return (len != length) ? (block + length) : 0; +} + +int pico_strncasecmp(const char *const str1, const char *const str2, size_t n) +{ + int ch1; + int ch2; + size_t i; + + for (i = 0; i < n; ++i) { + ch1 = toupper(*(str1 + i)); + ch2 = toupper(*(str2 + i)); + if (ch1 < ch2) + return -1; + + if (ch1 > ch2) + return 1; + + if ((!ch1) && (!ch2)) + return 0; + } + return 1; +} + +size_t pico_strnlen(const char *str, size_t n) +{ + size_t len = 0; + + if (!str) + return 0; + + for (; len < n && *(str + len); ++len) + ; /* TICS require this empty statement here */ + + return len; +} + +static inline int num2string_validate(int32_t num, char *buf, int len) +{ + if (num < 0) + return -1; + + if (!buf) + return -2; + + if (len < 2) + return -3; + + return 0; +} + +static inline int revert_and_shift(char *buf, int len, int pos) +{ + int i; + + len -= pos; + for (i = 0; i < len; ++i) + buf[i] = buf[i + pos]; + return len; +} + +int num2string(int32_t num, char *buf, int len) +{ + ldiv_t res; + int pos = 0; + + if (num2string_validate(num, buf, len)) + return -1; + + pos = len; + buf[--pos] = '\0'; + + res.quot = (long)num; + + do { + if (!pos) + return -3; + + res = ldiv(res.quot, 10); + buf[--pos] = (char)((res.rem + '0') & 0xFF); + } while (res.quot); + + return revert_and_shift(buf, len, pos); +} diff --git a/ext/picotcp/modules/pico_strings.h b/ext/picotcp/modules/pico_strings.h new file mode 100644 index 0000000..9a6209d --- /dev/null +++ b/ext/picotcp/modules/pico_strings.h @@ -0,0 +1,21 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2015 Altran ISY BeNeLux. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + Author: Michele Di Pede + *********************************************************************/ + +#ifndef PICO_STRINGS_H +#define PICO_STRINGS_H +#include +#include + +char *get_string_terminator_position(char *const block, size_t len); +int pico_strncasecmp(const char *const str1, const char *const str2, size_t n); +size_t pico_strnlen(const char *str, size_t n); + +int num2string(int32_t num, char *buf, int len); + +#endif diff --git a/ext/picotcp/modules/pico_tcp.c b/ext/picotcp/modules/pico_tcp.c new file mode 100644 index 0000000..67e3c73 --- /dev/null +++ b/ext/picotcp/modules/pico_tcp.c @@ -0,0 +1,3236 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + Authors: Daniele Lacamera, Philippe Mariman + *********************************************************************/ + +#include "pico_tcp.h" +#include "pico_config.h" +#include "pico_eth.h" +#include "pico_socket.h" +#include "pico_stack.h" +#include "pico_socket.h" +#include "pico_queue.h" +#include "pico_tree.h" + +#define TCP_IS_STATE(s, st) ((s->state & PICO_SOCKET_STATE_TCP) == st) +#define TCP_SOCK(s) ((struct pico_socket_tcp *)s) +#define SEQN(f) ((f) ? (long_be(((struct pico_tcp_hdr *)((f)->transport_hdr))->seq)) : 0) +#define ACKN(f) ((f) ? (long_be(((struct pico_tcp_hdr *)((f)->transport_hdr))->ack)) : 0) + +#define TCP_TIME (pico_time)(PICO_TIME_MS()) + +#define PICO_TCP_RTO_MIN (70) +#define PICO_TCP_RTO_MAX (120000) +#define PICO_TCP_IW 2 +#define PICO_TCP_SYN_TO 2000u +#define PICO_TCP_ZOMBIE_TO 30000 + +#define PICO_TCP_MAX_RETRANS 10 +#define PICO_TCP_MAX_CONNECT_RETRIES 3 + +#define PICO_TCP_LOOKAHEAD 0x00 +#define PICO_TCP_FIRST_DUPACK 0x01 +#define PICO_TCP_SECOND_DUPACK 0x02 +#define PICO_TCP_RECOVER 0x03 +#define PICO_TCP_BLACKOUT 0x04 +#define PICO_TCP_UNREACHABLE 0x05 +#define PICO_TCP_WINDOW_FULL 0x06 + +#define ONE_GIGABYTE ((uint32_t)(1024UL * 1024UL * 1024UL)) + +/* check if the Nagle algorithm is enabled on the socket */ +#define IS_NAGLE_ENABLED(s) (!(!(!(s->opt_flags & (1u << PICO_SOCKET_OPT_TCPNODELAY))))) +/* check if tcp connection is "idle" according to Nagle (RFC 896) */ +#define IS_TCP_IDLE(t) ((t->in_flight == 0) && (t->tcpq_out.size == 0)) +/* check if the hold queue contains data (again Nagle) */ +#define IS_TCP_HOLDQ_EMPTY(t) (t->tcpq_hold.size == 0) + +#define IS_INPUT_QUEUE(q) (q->pool.compare == input_segment_compare) +#define TCP_INPUT_OVERHEAD (sizeof(struct tcp_input_segment) + sizeof(struct pico_tree_node)) + + +#ifdef PICO_SUPPORT_TCP +#define tcp_dbg_nagle(...) do {} while(0) +#define tcp_dbg_options(...) do {} while(0) + + +#define tcp_dbg(...) do {} while(0) +/* #define tcp_dbg dbg */ + +#ifdef PICO_SUPPORT_MUTEX +static void *Mutex = NULL; +#endif + + + +/* Input segment, used to keep only needed data, not the full frame */ +struct tcp_input_segment +{ + uint32_t seq; + /* Pointer to payload */ + unsigned char *payload; + uint16_t payload_len; +}; + +/* Function to compare input segments */ +static int input_segment_compare(void *ka, void *kb) +{ + struct tcp_input_segment *a = ka, *b = kb; + return pico_seq_compare(a->seq, b->seq); +} + +static struct tcp_input_segment *segment_from_frame(struct pico_frame *f) +{ + struct tcp_input_segment *seg = PICO_ZALLOC(sizeof(struct tcp_input_segment)); + if ((!seg) || (!f->payload_len)) + return NULL; + + seg->payload = PICO_ZALLOC(f->payload_len); + if(!seg->payload) + { + PICO_FREE(seg); + return NULL; + } + + seg->seq = SEQN(f); + seg->payload_len = f->payload_len; + memcpy(seg->payload, f->payload, seg->payload_len); + return seg; +} + +static int segment_compare(void *ka, void *kb) +{ + struct pico_frame *a = ka, *b = kb; + return pico_seq_compare(SEQN(a), SEQN(b)); +} + +struct pico_tcp_queue +{ + struct pico_tree pool; + uint32_t max_size; + uint32_t size; + uint32_t frames; +}; + +static void tcp_discard_all_segments(struct pico_tcp_queue *tq); +static void *peek_segment(struct pico_tcp_queue *tq, uint32_t seq) +{ + if(!IS_INPUT_QUEUE(tq)) + { + struct pico_tcp_hdr H; + struct pico_frame f = { + 0 + }; + f.transport_hdr = (uint8_t *) (&H); + H.seq = long_be(seq); + + return pico_tree_findKey(&tq->pool, &f); + } + else + { + struct tcp_input_segment dummy = { 0 }; + dummy.seq = seq; + + return pico_tree_findKey(&tq->pool, &dummy); + } + +} + +static void *first_segment(struct pico_tcp_queue *tq) +{ + return pico_tree_first(&tq->pool); +} + +static void *next_segment(struct pico_tcp_queue *tq, void *cur) +{ + if (!cur) + return NULL; + + if(IS_INPUT_QUEUE(tq)) + { + return peek_segment(tq, ((struct tcp_input_segment *)cur)->seq + ((struct tcp_input_segment *)cur)->payload_len); + } + else + { + return peek_segment(tq, SEQN((struct pico_frame *)cur) + ((struct pico_frame *)cur)->payload_len); + } +} + +static uint16_t enqueue_segment_len(struct pico_tcp_queue *tq, void *f) +{ + if (IS_INPUT_QUEUE(tq)) { + return ((struct tcp_input_segment *)f)->payload_len; + } else { + return (uint16_t)(((struct pico_frame *)f)->buffer_len); + } +} + + +static int32_t do_enqueue_segment(struct pico_tcp_queue *tq, void *f, uint16_t payload_len) +{ + int32_t ret = -1; + PICOTCP_MUTEX_LOCK(Mutex); + if ((tq->size + payload_len) > tq->max_size) + { + ret = 0; + goto out; + } + + if (pico_tree_insert(&tq->pool, f) != 0) + { + ret = 0; + goto out; + } + + tq->size += (uint16_t)payload_len; + if (payload_len > 0) + tq->frames++; + + ret = (int32_t)payload_len; + +out: + PICOTCP_MUTEX_UNLOCK(Mutex); + return ret; +} + +static int32_t pico_enqueue_segment(struct pico_tcp_queue *tq, void *f) +{ + uint16_t payload_len; + + if (!f) + return -1; + + payload_len = enqueue_segment_len(tq, f); + + + if (payload_len == 0) { + tcp_dbg("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! TRIED TO ENQUEUE INVALID SEGMENT!\n"); + return -1; + } + + return do_enqueue_segment(tq, f, payload_len); +} + +static void pico_discard_segment(struct pico_tcp_queue *tq, void *f) +{ + void *f1; + uint16_t payload_len = (uint16_t)((IS_INPUT_QUEUE(tq)) ? + (((struct tcp_input_segment *)f)->payload_len) : + (((struct pico_frame *)f)->buffer_len)); + PICOTCP_MUTEX_LOCK(Mutex); + f1 = pico_tree_delete(&tq->pool, f); + if (f1) { + tq->size -= (uint16_t)payload_len; + if (payload_len > 0) + tq->frames--; + } + + if(f1 && IS_INPUT_QUEUE(tq)) + { + struct tcp_input_segment *inp = f1; + PICO_FREE(inp->payload); + PICO_FREE(inp); + } + else + pico_frame_discard(f); + + PICOTCP_MUTEX_UNLOCK(Mutex); +} + +/* Structure for TCP socket */ +struct tcp_sack_block { + uint32_t left; + uint32_t right; + struct tcp_sack_block *next; +}; + +struct pico_socket_tcp { + struct pico_socket sock; + + /* Tree/queues */ + struct pico_tcp_queue tcpq_in; /* updated the input queue to hold input segments not the full frame. */ + struct pico_tcp_queue tcpq_out; + struct pico_tcp_queue tcpq_hold; /* buffer to hold delayed frames according to Nagle */ + + /* tcp_output */ + uint32_t snd_nxt; + uint32_t snd_last; + uint32_t snd_old_ack; + uint32_t snd_retry; + uint32_t snd_last_out; + + /* congestion control */ + uint32_t avg_rtt; + uint32_t rttvar; + uint32_t rto; + uint32_t in_flight; + uint32_t retrans_tmr; + pico_time retrans_tmr_due; + uint16_t cwnd_counter; + uint16_t cwnd; + uint16_t ssthresh; + uint16_t recv_wnd; + uint16_t recv_wnd_scale; + + /* tcp_input */ + uint32_t rcv_nxt; + uint32_t rcv_ackd; + uint32_t rcv_processed; + uint16_t wnd; + uint16_t wnd_scale; + uint16_t remote_closed; + + /* options */ + uint32_t ts_nxt; + uint16_t mss; + uint8_t sack_ok; + uint8_t ts_ok; + uint8_t mss_ok; + uint8_t scale_ok; + struct tcp_sack_block *sacks; + uint8_t jumbo; + uint32_t linger_timeout; + + /* Transmission */ + uint8_t x_mode; + uint8_t dupacks; + uint8_t backoff; + uint8_t localZeroWindow; + + /* Keepalive */ + uint32_t keepalive_tmr; + pico_time ack_timestamp; + uint32_t ka_time; + uint32_t ka_intvl; + uint32_t ka_probes; + uint32_t ka_retries_count; + + /* FIN timer */ + uint32_t fin_tmr; +}; + +/* Queues */ +static struct pico_queue tcp_in = { + 0 +}; +static struct pico_queue tcp_out = { + 0 +}; + +/* If Nagle enabled, this function can make 1 new segment from smaller segments in hold queue */ +static struct pico_frame *pico_hold_segment_make(struct pico_socket_tcp *t); + +/* checks if tcpq_in is empty */ +int pico_tcp_queue_in_is_empty(struct pico_socket *s) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *) s; + + if (t->tcpq_in.frames == 0) + return 1; + else + return 0; +} + +/* Useful for getting rid of the beginning of the buffer (read() op) */ +static int release_until(struct pico_tcp_queue *q, uint32_t seq) +{ + void *head = first_segment(q); + int ret = 0; + int32_t seq_result = 0; + + if (!head) + return ret; + + do { + void *cur = head; + + if (IS_INPUT_QUEUE(q)) + seq_result = pico_seq_compare(((struct tcp_input_segment *)head)->seq + ((struct tcp_input_segment *)head)->payload_len, seq); + else + seq_result = pico_seq_compare(SEQN((struct pico_frame *)head) + ((struct pico_frame *)head)->payload_len, seq); + + if (seq_result <= 0) + { + head = next_segment(q, cur); + //tcp_dbg("Releasing %08x, len: %d\n", SEQN((struct pico_frame *)head), ((struct pico_frame *)head)->payload_len); + pico_discard_segment(q, cur); + ret++; + } else { + break; + } + } while (head); + + return ret; +} + +static int release_all_until(struct pico_tcp_queue *q, uint32_t seq, pico_time *timestamp) +{ + void *f = NULL; + struct pico_tree_node *idx, *temp; + int seq_result; + int ret = 0; + *timestamp = 0; + + pico_tree_foreach_safe(idx, &q->pool, temp) + { + f = idx->keyValue; + + if (IS_INPUT_QUEUE(q)) + seq_result = pico_seq_compare(((struct tcp_input_segment *)f)->seq + ((struct tcp_input_segment *)f)->payload_len, seq); + else + seq_result = pico_seq_compare(SEQN((struct pico_frame *)f) + ((struct pico_frame *)f)->payload_len, seq); + + if (seq_result <= 0) { + tcp_dbg("Releasing %p\n", f); + if ((seq_result == 0) && !IS_INPUT_QUEUE(q)) + *timestamp = ((struct pico_frame *)f)->timestamp; + + pico_discard_segment(q, f); + ret++; + } else { + return ret; + } + } + return ret; +} + + +/* API calls */ + +uint16_t pico_tcp_checksum_ipv4(struct pico_frame *f) +{ + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; + struct pico_tcp_hdr *tcp_hdr = (struct pico_tcp_hdr *) f->transport_hdr; + struct pico_socket *s = f->sock; + struct pico_ipv4_pseudo_hdr pseudo; + + if (s) { + /* Case of outgoing frame */ + /* dbg("TCP CRC: on outgoing frame\n"); */ + pseudo.src.addr = s->local_addr.ip4.addr; + pseudo.dst.addr = s->remote_addr.ip4.addr; + } else { + /* Case of incoming frame */ + /* dbg("TCP CRC: on incoming frame\n"); */ + pseudo.src.addr = hdr->src.addr; + pseudo.dst.addr = hdr->dst.addr; + } + + pseudo.zeros = 0; + pseudo.proto = PICO_PROTO_TCP; + pseudo.len = (uint16_t)short_be(f->transport_len); + + return pico_dualbuffer_checksum(&pseudo, sizeof(struct pico_ipv4_pseudo_hdr), tcp_hdr, f->transport_len); +} + +#ifdef PICO_SUPPORT_IPV6 +uint16_t pico_tcp_checksum_ipv6(struct pico_frame *f) +{ + struct pico_ipv6_hdr *ipv6_hdr = (struct pico_ipv6_hdr *)f->net_hdr; + struct pico_tcp_hdr *tcp_hdr = (struct pico_tcp_hdr *)f->transport_hdr; + struct pico_ipv6_pseudo_hdr pseudo; + struct pico_socket *s = f->sock; + + /* XXX If the IPv6 packet contains a Routing header, the Destination + * Address used in the pseudo-header is that of the final destination */ + if (s) { + /* Case of outgoing frame */ + pseudo.src = s->local_addr.ip6; + pseudo.dst = s->remote_addr.ip6; + } else { + /* Case of incoming frame */ + pseudo.src = ipv6_hdr->src; + pseudo.dst = ipv6_hdr->dst; + } + + pseudo.zero[0] = 0; + pseudo.zero[1] = 0; + pseudo.zero[2] = 0; + pseudo.len = long_be(f->transport_len); + pseudo.nxthdr = PICO_PROTO_TCP; + + return pico_dualbuffer_checksum(&pseudo, sizeof(struct pico_ipv6_pseudo_hdr), tcp_hdr, f->transport_len); +} +#endif + +#ifdef PICO_SUPPORT_IPV4 +static inline int checksum_is_ipv4(struct pico_frame *f) +{ + return (IS_IPV4(f) || (f->sock && (f->sock->net == &pico_proto_ipv4))); +} +#endif + +#ifdef PICO_SUPPORT_IPV6 +static inline int checksum_is_ipv6(struct pico_frame *f) +{ + return ((IS_IPV6(f)) || (f->sock && (f->sock->net == &pico_proto_ipv6))); +} +#endif + +uint16_t pico_tcp_checksum(struct pico_frame *f) +{ + (void)f; + + #ifdef PICO_SUPPORT_IPV4 + if (checksum_is_ipv4(f)) + return pico_tcp_checksum_ipv4(f); + + #endif + + #ifdef PICO_SUPPORT_IPV6 + if (checksum_is_ipv6(f)) + return pico_tcp_checksum_ipv6(f); + + #endif + return 0xffff; +} + +static void tcp_send_fin(struct pico_socket_tcp *t); +static int pico_tcp_process_out(struct pico_protocol *self, struct pico_frame *f) +{ + struct pico_tcp_hdr *hdr; + struct pico_socket_tcp *t = (struct pico_socket_tcp *)f->sock; + IGNORE_PARAMETER(self); + hdr = (struct pico_tcp_hdr *)f->transport_hdr; + f->sock->timestamp = TCP_TIME; + if (f->payload_len > 0) { + tcp_dbg("Process out: sending %p (%d bytes)\n", f, f->payload_len); + } else { + tcp_dbg("Sending empty packet\n"); + } + + if (f->payload_len > 0) { + if (pico_seq_compare(SEQN(f) + f->payload_len, t->snd_nxt) > 0) { + t->snd_nxt = SEQN(f) + f->payload_len; + tcp_dbg("%s: snd_nxt is now %08x\n", __FUNCTION__, t->snd_nxt); + } + } else if (hdr->flags == PICO_TCP_ACK) { /* pure ack */ + /* hdr->seq = long_be(t->snd_nxt); / * XXX disabled this to not to mess with seq nrs of ACKs anymore * / */ + } else { + tcp_dbg("%s: non-pure ACK with len=0, fl:%04x\n", __FUNCTION__, hdr->flags); + } + + pico_network_send(f); + return 0; +} + +int pico_tcp_push(struct pico_protocol *self, struct pico_frame *data); + +/* Interface: protocol definition */ +struct pico_protocol pico_proto_tcp = { + .name = "tcp", + .proto_number = PICO_PROTO_TCP, + .layer = PICO_LAYER_TRANSPORT, + .process_in = pico_transport_process_in, + .process_out = pico_tcp_process_out, + .push = pico_tcp_push, + .q_in = &tcp_in, + .q_out = &tcp_out, +}; + +static uint32_t pico_paws(void) +{ + static uint32_t _paws = 0; + _paws = pico_rand(); + return long_be(_paws); +} + +static inline void tcp_add_sack_option(struct pico_socket_tcp *ts, struct pico_frame *f, uint16_t flags, uint32_t *ii) +{ + if (flags & PICO_TCP_ACK) { + struct tcp_sack_block *sb; + uint32_t len_off; + + if (ts->sack_ok && ts->sacks) { + f->start[(*ii)++] = PICO_TCP_OPTION_SACK; + len_off = *ii; + f->start[(*ii)++] = PICO_TCPOPTLEN_SACK; + while(ts->sacks) { + sb = ts->sacks; + ts->sacks = sb->next; + memcpy(f->start + *ii, sb, 2 * sizeof(uint32_t)); + *ii += (2 * (uint32_t)sizeof(uint32_t)); + f->start[len_off] = (uint8_t)(f->start[len_off] + (2 * sizeof(uint32_t))); + PICO_FREE(sb); + } + } + } +} + +static void tcp_add_options(struct pico_socket_tcp *ts, struct pico_frame *f, uint16_t flags, uint16_t optsiz) +{ + uint32_t tsval = long_be((uint32_t)TCP_TIME); + uint32_t tsecr = long_be(ts->ts_nxt); + uint32_t i = 0; + f->start = f->transport_hdr + PICO_SIZE_TCPHDR; + + memset(f->start, PICO_TCP_OPTION_NOOP, optsiz); /* fill blanks with noop */ + + if (flags & PICO_TCP_SYN) { + f->start[i++] = PICO_TCP_OPTION_MSS; + f->start[i++] = PICO_TCPOPTLEN_MSS; + f->start[i++] = (uint8_t)((ts->mss >> 8) & 0xFF); + f->start[i++] = (uint8_t)(ts->mss & 0xFF); + f->start[i++] = PICO_TCP_OPTION_SACK_OK; + f->start[i++] = PICO_TCPOPTLEN_SACK_OK; + } + + f->start[i++] = PICO_TCP_OPTION_WS; + f->start[i++] = PICO_TCPOPTLEN_WS; + f->start[i++] = (uint8_t)(ts->wnd_scale); + + if ((flags & PICO_TCP_SYN) || ts->ts_ok) { + f->start[i++] = PICO_TCP_OPTION_TIMESTAMP; + f->start[i++] = PICO_TCPOPTLEN_TIMESTAMP; + memcpy(f->start + i, &tsval, 4); + i += 4; + memcpy(f->start + i, &tsecr, 4); + i += 4; + } + + tcp_add_sack_option(ts, f, flags, &i); + + if (i < optsiz) + f->start[ optsiz - 1 ] = PICO_TCP_OPTION_END; +} + +static uint16_t tcp_options_size_frame(struct pico_frame *f) +{ + uint16_t size = 0; + + /* Always update window scale. */ + size = (uint16_t)(size + PICO_TCPOPTLEN_WS); + if (f->transport_flags_saved) + size = (uint16_t)(size + PICO_TCPOPTLEN_TIMESTAMP); + + size = (uint16_t)(size + PICO_TCPOPTLEN_END); + size = (uint16_t)(((uint16_t)(size + 3u) >> 2u) << 2u); + return size; +} + +static void tcp_add_options_frame(struct pico_socket_tcp *ts, struct pico_frame *f) +{ + uint32_t tsval = long_be((uint32_t)TCP_TIME); + uint32_t tsecr = long_be(ts->ts_nxt); + uint32_t i = 0; + uint16_t optsiz = tcp_options_size_frame(f); + + f->start = f->transport_hdr + PICO_SIZE_TCPHDR; + + memset(f->start, PICO_TCP_OPTION_NOOP, optsiz); /* fill blanks with noop */ + + + f->start[i++] = PICO_TCP_OPTION_WS; + f->start[i++] = PICO_TCPOPTLEN_WS; + f->start[i++] = (uint8_t)(ts->wnd_scale); + + if (f->transport_flags_saved) { + f->start[i++] = PICO_TCP_OPTION_TIMESTAMP; + f->start[i++] = PICO_TCPOPTLEN_TIMESTAMP; + memcpy(f->start + i, &tsval, 4); + i += 4; + memcpy(f->start + i, &tsecr, 4); + i += 4; + } + + if (i < optsiz) + f->start[ optsiz - 1 ] = PICO_TCP_OPTION_END; +} + +static void tcp_send_ack(struct pico_socket_tcp *t); +#define tcp_send_windowUpdate(t) (tcp_send_ack(t)) + +static inline void tcp_set_space_check_winupdate(struct pico_socket_tcp *t, int32_t space, uint32_t shift) +{ + if (((uint32_t)space != t->wnd) || (shift != t->wnd_scale) || ((space - t->wnd) > (int32_t)((uint32_t)space >> 2u))) { + t->wnd = (uint16_t)space; + t->wnd_scale = (uint16_t)shift; + + if(t->wnd == 0) /* mark the entering to zero window state */ + t->localZeroWindow = 1u; + else if(t->localZeroWindow) + { + t->localZeroWindow = 0u; + tcp_send_windowUpdate(t); + } + } +} + +static void tcp_set_space(struct pico_socket_tcp *t) +{ + int32_t space; + uint32_t shift = 0; + + if (t->tcpq_in.max_size == 0) { + space = ONE_GIGABYTE; + } else { + space = (int32_t)(t->tcpq_in.max_size - t->tcpq_in.size); + } + + if (space < 0) + space = 0; + + while(space > 0xFFFF) { + space = (int32_t)(((uint32_t)space >> 1u)); + shift++; + } + tcp_set_space_check_winupdate(t, space, shift); +} + +/* Return 32-bit aligned option size */ +static uint16_t tcp_options_size(struct pico_socket_tcp *t, uint16_t flags) +{ + uint16_t size = 0; + struct tcp_sack_block *sb = t->sacks; + + if (flags & PICO_TCP_SYN) { /* Full options */ + size = PICO_TCPOPTLEN_MSS + PICO_TCP_OPTION_SACK_OK + PICO_TCPOPTLEN_WS + PICO_TCPOPTLEN_TIMESTAMP; + } else { + + /* Always update window scale. */ + size = (uint16_t)(size + PICO_TCPOPTLEN_WS); + + if (t->ts_ok) + size = (uint16_t)(size + PICO_TCPOPTLEN_TIMESTAMP); + + size = (uint16_t)(size + PICO_TCPOPTLEN_END); + } + + if ((flags & PICO_TCP_ACK) && (t->sack_ok && sb)) { + size = (uint16_t)(size + 2); + while(sb) { + size = (uint16_t)(size + (2 * sizeof(uint32_t))); + sb = sb->next; + } + } + + size = (uint16_t)(((size + 3u) >> 2u) << 2u); + return size; +} + +uint16_t pico_tcp_overhead(struct pico_socket *s) +{ + if (!s) + return 0; + + return (uint16_t)(PICO_SIZE_TCPHDR + tcp_options_size((struct pico_socket_tcp *)s, (uint16_t)0)); /* hdr + Options size for data pkt */ + +} + +static inline int tcp_sack_marker(struct pico_frame *f, uint32_t start, uint32_t end, uint16_t *count) +{ + int cmp; + cmp = pico_seq_compare(SEQN(f), start); + if (cmp > 0) + return 0; + + if (cmp == 0) { + cmp = pico_seq_compare(SEQN(f) + f->payload_len, end); + if (cmp > 0) { + tcp_dbg("Invalid SACK: ignoring.\n"); + } + + tcp_dbg("Marking (by SACK) segment %08x BLK:[%08x::%08x]\n", SEQN(f), start, end); + f->flags |= PICO_FRAME_FLAG_SACKED; + (*count)++; + } + + return cmp; +} + +static void tcp_process_sack(struct pico_socket_tcp *t, uint32_t start, uint32_t end) +{ + struct pico_frame *f; + struct pico_tree_node *index, *temp; + uint16_t count = 0; + + pico_tree_foreach_safe(index, &t->tcpq_out.pool, temp){ + f = index->keyValue; + if (tcp_sack_marker(f, start, end, &count) == 0) + goto done; + } + +done: + if (t->x_mode > PICO_TCP_LOOKAHEAD) { + if (t->in_flight > (count)) + t->in_flight -= (count); + else + t->in_flight = 0; + } +} + +inline static void tcp_add_header(struct pico_socket_tcp *t, struct pico_frame *f) +{ + struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *)f->transport_hdr; + f->timestamp = TCP_TIME; + tcp_add_options(t, f, 0, (uint16_t)(f->transport_len - f->payload_len - (uint16_t)PICO_SIZE_TCPHDR)); + hdr->rwnd = short_be(t->wnd); + hdr->flags |= PICO_TCP_PSH | PICO_TCP_ACK; + hdr->ack = long_be(t->rcv_nxt); + hdr->crc = 0; + hdr->crc = short_be(pico_tcp_checksum(f)); +} + +static void tcp_rcv_sack(struct pico_socket_tcp *t, uint8_t *opt, int len) +{ + uint32_t start, end; + int i = 0; + if (len % 8) { + tcp_dbg("SACK: Invalid len.\n"); + return; + } + + while (i < len) { + start = long_from(opt + i); + i += 4; + end = long_from(opt + i); + i += 4; + tcp_process_sack(t, long_be(start), long_be(end)); + } +} + +static int tcpopt_len_check(uint32_t *idx, uint8_t len, uint8_t expected) +{ + if (len != expected) { + *idx = *idx + len - 2; + return -1; + } + + return 0; +} + +static inline void tcp_parse_option_ws(struct pico_socket_tcp *t, uint8_t len, uint8_t *opt, uint32_t *idx) +{ + if (tcpopt_len_check(idx, len, PICO_TCPOPTLEN_WS) < 0) + return; + + t->recv_wnd_scale = opt[(*idx)++]; + tcp_dbg_options("TCP Window scale: received %d\n", t->recv_wnd_scale); + +} + +static inline void tcp_parse_option_sack_ok(struct pico_socket_tcp *t, struct pico_frame *f, uint8_t len, uint32_t *idx) +{ + if (tcpopt_len_check(idx, len, PICO_TCPOPTLEN_SACK_OK) < 0) + return; + + if(((struct pico_tcp_hdr *)(f->transport_hdr))->flags & PICO_TCP_SYN ) + t->sack_ok = 1; +} + +static inline void tcp_parse_option_mss(struct pico_socket_tcp *t, uint8_t len, uint8_t *opt, uint32_t *idx) +{ + uint16_t mss; + if (tcpopt_len_check(idx, len, PICO_TCPOPTLEN_MSS) < 0) + return; + + t->mss_ok = 1; + mss = short_from(opt + *idx); + *idx += (uint32_t)sizeof(uint16_t); + if (t->mss > short_be(mss)) + t->mss = short_be(mss); +} + +static inline void tcp_parse_option_timestamp(struct pico_socket_tcp *t, struct pico_frame *f, uint8_t len, uint8_t *opt, uint32_t *idx) +{ + uint32_t tsval, tsecr; + if (tcpopt_len_check(idx, len, PICO_TCPOPTLEN_TIMESTAMP) < 0) + return; + + t->ts_ok = 1; + tsval = long_from(opt + *idx); + *idx += (uint32_t)sizeof(uint32_t); + tsecr = long_from(opt + *idx); + f->timestamp = long_be(tsecr); + *idx += (uint32_t)sizeof(uint32_t); + t->ts_nxt = long_be(tsval); +} + +static void tcp_parse_options(struct pico_frame *f) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)f->sock; + uint8_t *opt = f->transport_hdr + PICO_SIZE_TCPHDR; + uint32_t i = 0; + f->timestamp = 0; + while (i < (f->transport_len - PICO_SIZE_TCPHDR)) { + uint8_t type = opt[i++]; + uint8_t len; + if(i < (f->transport_len - PICO_SIZE_TCPHDR) && (type > 1)) + len = opt[i++]; + else + len = 1; + + if (f->payload && ((opt + i) > f->payload)) + break; + + tcp_dbg_options("Received option '%d', len = %d \n", type, len); + switch (type) { + case PICO_TCP_OPTION_NOOP: + case PICO_TCP_OPTION_END: + break; + case PICO_TCP_OPTION_WS: + tcp_parse_option_ws(t, len, opt, &i); + break; + case PICO_TCP_OPTION_SACK_OK: + tcp_parse_option_sack_ok(t, f, len, &i); + break; + case PICO_TCP_OPTION_MSS: + tcp_parse_option_mss(t, len, opt, &i); + break; + case PICO_TCP_OPTION_TIMESTAMP: + tcp_parse_option_timestamp(t, f, len, opt, &i); + break; + + case PICO_TCP_OPTION_SACK: + tcp_rcv_sack(t, opt + i, len - 2); + i = i + len - 2; + break; + default: + tcp_dbg_options("TCP: received unsupported option %u\n", type); + i = i + len - 2; + } + } +} + +static inline void tcp_send_add_tcpflags(struct pico_socket_tcp *ts, struct pico_frame *f) +{ + struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *) f->transport_hdr; + if (ts->rcv_nxt != 0) { + if ((ts->rcv_ackd == 0) || (pico_seq_compare(ts->rcv_ackd, ts->rcv_nxt) != 0) || (hdr->flags & PICO_TCP_ACK)) { + hdr->flags |= PICO_TCP_ACK; + hdr->ack = long_be(ts->rcv_nxt); + ts->rcv_ackd = ts->rcv_nxt; + } + } + + if (hdr->flags & PICO_TCP_SYN) { + ts->snd_nxt++; + } + + if (f->payload_len > 0) { + hdr->flags |= PICO_TCP_PSH | PICO_TCP_ACK; + hdr->ack = long_be(ts->rcv_nxt); + ts->rcv_ackd = ts->rcv_nxt; + } +} + +static inline int tcp_send_try_enqueue(struct pico_socket_tcp *ts, struct pico_frame *f) +{ + struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *) f->transport_hdr; + struct pico_frame *cpy; + (void)hdr; + + /* TCP: ENQUEUE to PROTO ( Transmit ) */ + cpy = pico_frame_copy(f); + if (!cpy) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + if ((pico_enqueue(&tcp_out, cpy) > 0)) { + if (f->payload_len > 0) { + ts->in_flight++; + ts->snd_nxt += f->payload_len; /* update next pointer here to prevent sending same segment twice when called twice in same tick */ + } + + tcp_dbg("DBG> [tcp output] state: %02x --> local port:%u remote port: %u seq: %08x ack: %08x flags: %02x = t_len: %u, hdr: %u payload: %d\n", + TCPSTATE(&ts->sock) >> 8, short_be(hdr->trans.sport), short_be(hdr->trans.dport), SEQN(f), ACKN(f), hdr->flags, f->transport_len, (hdr->len & 0xf0) >> 2, f->payload_len ); + } else { + pico_frame_discard(cpy); + } + + return 0; + +} + +static int tcp_send(struct pico_socket_tcp *ts, struct pico_frame *f) +{ + struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *) f->transport_hdr; + hdr->trans.sport = ts->sock.local_port; + hdr->trans.dport = ts->sock.remote_port; + if (!hdr->seq) + hdr->seq = long_be(ts->snd_nxt); + + tcp_send_add_tcpflags(ts, f); + + f->start = f->transport_hdr + PICO_SIZE_TCPHDR; + hdr->rwnd = short_be(ts->wnd); + hdr->crc = 0; + hdr->crc = short_be(pico_tcp_checksum(f)); + + return tcp_send_try_enqueue(ts, f); + +} + +/* #define PICO_TCP_SUPPORT_SOCKET_STATS */ + +#ifdef PICO_TCP_SUPPORT_SOCKET_STATS +static void sock_stats(uint32_t when, void *arg) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)arg; + tcp_dbg("STATISTIC> [%lu] socket state: %02x --> local port:%d remote port: %d queue size: %d snd_una: %08x snd_nxt: %08x cwnd: %d\n", + when, t->sock.state, short_be(t->sock.local_port), short_be(t->sock.remote_port), t->tcpq_out.size, SEQN((struct pico_frame *)first_segment(&t->tcpq_out)), t->snd_nxt, t->cwnd); + pico_timer_add(2000, sock_stats, t); +} +#endif + +static void tcp_send_probe(struct pico_socket_tcp *t); + +static void pico_tcp_keepalive(pico_time now, void *arg) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)arg; + if (((t->sock.state & PICO_SOCKET_STATE_TCP) == PICO_SOCKET_STATE_TCP_ESTABLISHED) && (t->ka_time > 0)) { + if (t->ka_time < (now - t->ack_timestamp)) { + if (t->ka_retries_count == 0) { + /* First probe */ + tcp_send_probe(t); + t->ka_retries_count++; + } + if (t->ka_retries_count > t->ka_probes) { + if (t->sock.wakeup) + { + pico_err = PICO_ERR_ECONNRESET; + t->sock.wakeup(PICO_SOCK_EV_ERR, &t->sock); + } + } + if (((t->ka_retries_count * t->ka_intvl) + t->ka_time) < (now - t->ack_timestamp)) { + /* Next probe */ + tcp_send_probe(t); + t->ka_retries_count++; + } + } else { + t->ka_retries_count = 0; + } + } + t->keepalive_tmr = pico_timer_add(1000, pico_tcp_keepalive, t); +} + +static inline void rto_set(struct pico_socket_tcp *t, uint32_t rto) +{ + if (rto < PICO_TCP_RTO_MIN) + rto = PICO_TCP_RTO_MIN; + + if (rto > PICO_TCP_RTO_MAX) + rto = PICO_TCP_RTO_MAX; + + t->rto = rto; +} + + +struct pico_socket *pico_tcp_open(uint16_t family) +{ + struct pico_socket_tcp *t = PICO_ZALLOC(sizeof(struct pico_socket_tcp)); + if (!t) + return NULL; + + t->sock.timestamp = TCP_TIME; + pico_socket_set_family(&t->sock, family); + t->mss = (uint16_t)(pico_socket_get_mss(&t->sock) - PICO_SIZE_TCPHDR); + t->tcpq_in.pool.root = t->tcpq_hold.pool.root = t->tcpq_out.pool.root = &LEAF; + t->tcpq_hold.pool.compare = t->tcpq_out.pool.compare = segment_compare; + t->tcpq_in.pool.compare = input_segment_compare; + t->tcpq_in.max_size = PICO_DEFAULT_SOCKETQ; + t->tcpq_out.max_size = PICO_DEFAULT_SOCKETQ; + t->tcpq_hold.max_size = 2u * t->mss; + rto_set(t, PICO_TCP_RTO_MIN); + + /* Uncomment next line and disable Nagle by default */ + t->sock.opt_flags |= (1 << PICO_SOCKET_OPT_TCPNODELAY); + + /* Uncomment next line and Nagle is enabled by default */ + /* t->sock.opt_flags &= (uint16_t) ~(1 << PICO_SOCKET_OPT_TCPNODELAY); */ + + /* Set default linger for the socket */ + t->linger_timeout = PICO_SOCKET_LINGER_TIMEOUT; + + +#ifdef PICO_TCP_SUPPORT_SOCKET_STATS + pico_timer_add(2000, sock_stats, t); +#endif + + t->keepalive_tmr = pico_timer_add(1000, pico_tcp_keepalive, t); + tcp_set_space(t); + + return &t->sock; +} + +static uint32_t tcp_read_finish(struct pico_socket *s, uint32_t tot_rd_len) +{ + struct pico_socket_tcp *t = TCP_SOCK(s); + tcp_set_space(t); + if (t->tcpq_in.size == 0) { + s->ev_pending &= (uint16_t)(~PICO_SOCK_EV_RD); + } + + if (t->remote_closed) { + s->ev_pending |= (uint16_t)(PICO_SOCK_EV_CLOSE); + s->state &= 0x00FFU; + s->state |= PICO_SOCKET_STATE_TCP_CLOSE_WAIT; + /* set SHUT_REMOTE */ + s->state |= PICO_SOCKET_STATE_SHUT_REMOTE; + if (s->wakeup) { + s->wakeup(PICO_SOCK_EV_CLOSE, s); + } + } + + return tot_rd_len; +} + +static inline uint32_t tcp_read_in_frame_len(struct tcp_input_segment *f, int32_t in_frame_off, uint32_t tot_rd_len, uint32_t read_op_len) +{ + uint32_t in_frame_len = 0; + if (in_frame_off > 0) + { + if ((uint32_t)in_frame_off > f->payload_len) { + tcp_dbg("FATAL TCP ERR: in_frame_off > f->payload_len\n"); + } + + in_frame_len = f->payload_len - (uint32_t)in_frame_off; + } else { /* in_frame_off == 0 */ + in_frame_len = f->payload_len; + } + + if ((in_frame_len + tot_rd_len) > (uint32_t)read_op_len) { + in_frame_len = read_op_len - tot_rd_len; + } + + return in_frame_len; + +} + +static inline void tcp_read_check_segment_done(struct pico_socket_tcp *t, struct tcp_input_segment *f, uint32_t in_frame_len) +{ + if ((in_frame_len == 0u) || (in_frame_len == (uint32_t)f->payload_len)) { + pico_discard_segment(&t->tcpq_in, f); + } +} + +uint32_t pico_tcp_read(struct pico_socket *s, void *buf, uint32_t len) +{ + struct pico_socket_tcp *t = TCP_SOCK(s); + struct tcp_input_segment *f; + int32_t in_frame_off; + uint32_t in_frame_len; + uint32_t tot_rd_len = 0; + + while (tot_rd_len < len) { + /* To be sure we don't have garbage at the beginning */ + release_until(&t->tcpq_in, t->rcv_processed); + f = first_segment(&t->tcpq_in); + if (!f) + return tcp_read_finish(s, tot_rd_len); + + in_frame_off = pico_seq_compare(t->rcv_processed, f->seq); + /* Check for hole at the beginning of data, awaiting retransmissions. */ + if (in_frame_off < 0) { + tcp_dbg("TCP> read hole beginning of data, %08x - %08x. rcv_nxt is %08x\n", t->rcv_processed, f->seq, t->rcv_nxt); + return tcp_read_finish(s, tot_rd_len); + } + + in_frame_len = tcp_read_in_frame_len(f, in_frame_off, tot_rd_len, len); + + + memcpy((uint8_t *)buf + tot_rd_len, f->payload + in_frame_off, in_frame_len); + tot_rd_len += in_frame_len; + t->rcv_processed += in_frame_len; + + tcp_read_check_segment_done(t, f, in_frame_len); + + } + return tcp_read_finish(s, tot_rd_len); +} + +int pico_tcp_initconn(struct pico_socket *s); +static void initconn_retry(pico_time when, void *arg) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)arg; + IGNORE_PARAMETER(when); + if (TCPSTATE(&t->sock) != PICO_SOCKET_STATE_TCP_ESTABLISHED) + { + if (t->backoff > PICO_TCP_MAX_CONNECT_RETRIES) { + tcp_dbg("TCP> Connection timeout. \n"); + if (t->sock.wakeup) + { + pico_err = PICO_ERR_ECONNREFUSED; + t->sock.wakeup(PICO_SOCK_EV_ERR, &t->sock); + } + pico_socket_del(&t->sock); + return; + } + + tcp_dbg("TCP> SYN retry %d...\n", t->backoff); + t->backoff++; + pico_tcp_initconn(&t->sock); + } else { + tcp_dbg("TCP> Connection is already established: no retry needed. good.\n"); + } +} + +int pico_tcp_initconn(struct pico_socket *s) +{ + struct pico_socket_tcp *ts = TCP_SOCK(s); + struct pico_frame *syn; + struct pico_tcp_hdr *hdr; + uint16_t mtu, opt_len = tcp_options_size(ts, PICO_TCP_SYN); + + syn = s->net->alloc(s->net, (uint16_t)(PICO_SIZE_TCPHDR + opt_len)); + if (!syn) + return -1; + + hdr = (struct pico_tcp_hdr *) syn->transport_hdr; + + if (!ts->snd_nxt) + ts->snd_nxt = long_be(pico_paws()); + + ts->snd_last = ts->snd_nxt; + ts->cwnd = PICO_TCP_IW; + mtu = (uint16_t)pico_socket_get_mss(s); + ts->mss = (uint16_t)(mtu - PICO_SIZE_TCPHDR); + ts->ssthresh = (uint16_t)((uint16_t)(PICO_DEFAULT_SOCKETQ / ts->mss) - (((uint16_t)(PICO_DEFAULT_SOCKETQ / ts->mss)) >> 3u)); + syn->sock = s; + hdr->seq = long_be(ts->snd_nxt); + hdr->len = (uint8_t)((PICO_SIZE_TCPHDR + opt_len) << 2 | ts->jumbo); + hdr->flags = PICO_TCP_SYN; + tcp_set_space(ts); + hdr->rwnd = short_be(ts->wnd); + tcp_add_options(ts, syn, PICO_TCP_SYN, opt_len); + hdr->trans.sport = ts->sock.local_port; + hdr->trans.dport = ts->sock.remote_port; + + hdr->crc = 0; + hdr->crc = short_be(pico_tcp_checksum(syn)); + + /* TCP: ENQUEUE to PROTO ( SYN ) */ + tcp_dbg("Sending SYN... (ports: %d - %d) size: %d\n", short_be(ts->sock.local_port), short_be(ts->sock.remote_port), syn->buffer_len); + pico_enqueue(&tcp_out, syn); + ts->retrans_tmr = pico_timer_add(PICO_TCP_SYN_TO << ts->backoff, initconn_retry, ts); + return 0; +} + +static int tcp_send_synack(struct pico_socket *s) +{ + struct pico_socket_tcp *ts = TCP_SOCK(s); + struct pico_frame *synack; + struct pico_tcp_hdr *hdr; + uint16_t opt_len = tcp_options_size(ts, PICO_TCP_SYN | PICO_TCP_ACK); + + synack = s->net->alloc(s->net, (uint16_t)(PICO_SIZE_TCPHDR + opt_len)); + if (!synack) + return -1; + + hdr = (struct pico_tcp_hdr *) synack->transport_hdr; + + synack->sock = s; + hdr->len = (uint8_t)((PICO_SIZE_TCPHDR + opt_len) << 2 | ts->jumbo); + hdr->flags = PICO_TCP_SYN | PICO_TCP_ACK; + hdr->rwnd = short_be(ts->wnd); + hdr->seq = long_be(ts->snd_nxt); + ts->rcv_processed = long_be(hdr->seq); + ts->snd_last = ts->snd_nxt; + tcp_set_space(ts); + tcp_add_options(ts, synack, hdr->flags, opt_len); + synack->payload_len = 0; + synack->timestamp = TCP_TIME; + tcp_send(ts, synack); + pico_frame_discard(synack); + return 0; +} + +static void tcp_send_empty(struct pico_socket_tcp *t, uint16_t flags, int is_keepalive) +{ + struct pico_frame *f; + struct pico_tcp_hdr *hdr; + uint16_t opt_len = tcp_options_size(t, flags); + f = t->sock.net->alloc(t->sock.net, (uint16_t)(PICO_SIZE_TCPHDR + opt_len)); + if (!f) { + return; + } + + f->sock = &t->sock; + hdr = (struct pico_tcp_hdr *) f->transport_hdr; + hdr->len = (uint8_t)((PICO_SIZE_TCPHDR + opt_len) << 2 | t->jumbo); + hdr->flags = (uint8_t)flags; + hdr->rwnd = short_be(t->wnd); + tcp_set_space(t); + tcp_add_options(t, f, flags, opt_len); + hdr->trans.sport = t->sock.local_port; + hdr->trans.dport = t->sock.remote_port; + hdr->seq = long_be(t->snd_nxt); + if ((flags & PICO_TCP_ACK) != 0) { + hdr->ack = long_be(t->rcv_nxt); + } + + if (is_keepalive) + hdr->seq = long_be(t->snd_nxt - 1); + + t->rcv_ackd = t->rcv_nxt; + + f->start = f->transport_hdr + PICO_SIZE_TCPHDR; + hdr->rwnd = short_be(t->wnd); + hdr->crc = 0; + hdr->crc = short_be(pico_tcp_checksum(f)); + + /* TCP: ENQUEUE to PROTO */ + pico_enqueue(&tcp_out, f); +} + +static void tcp_send_ack(struct pico_socket_tcp *t) +{ + tcp_send_empty(t, PICO_TCP_ACK, 0); +} + +static void tcp_send_probe(struct pico_socket_tcp *t) +{ + /* tcp_dbg("Sending probe\n"); */ + tcp_send_empty(t, PICO_TCP_PSHACK, 1); +} + +static int tcp_do_send_rst(struct pico_socket *s, uint32_t seq) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *) s; + uint16_t opt_len = tcp_options_size(t, PICO_TCP_RST); + struct pico_frame *f; + struct pico_tcp_hdr *hdr; + f = t->sock.net->alloc(t->sock.net, (uint16_t)(PICO_SIZE_TCPHDR + opt_len)); + if (!f) { + return -1; + } + f->sock = &t->sock; + tcp_dbg("TCP SEND_RST >>>>>>>>>>>>>>> START\n"); + + hdr = (struct pico_tcp_hdr *) f->transport_hdr; + hdr->len = (uint8_t)((PICO_SIZE_TCPHDR + opt_len) << 2 | t->jumbo); + hdr->flags = PICO_TCP_RST; + hdr->rwnd = short_be(t->wnd); + tcp_set_space(t); + tcp_add_options(t, f, PICO_TCP_RST, opt_len); + hdr->trans.sport = t->sock.local_port; + hdr->trans.dport = t->sock.remote_port; + hdr->seq = seq; + hdr->ack = long_be(t->rcv_nxt); + t->rcv_ackd = t->rcv_nxt; + f->start = f->transport_hdr + PICO_SIZE_TCPHDR; + hdr->rwnd = short_be(t->wnd); + hdr->crc = 0; + hdr->crc = short_be(pico_tcp_checksum(f)); + + /* TCP: ENQUEUE to PROTO */ + pico_enqueue(&tcp_out, f); + tcp_dbg("TCP SEND_RST >>>>>>>>>>>>>>> DONE\n"); + return 0; +} + +static int tcp_send_rst(struct pico_socket *s, struct pico_frame *fr) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *) s; + struct pico_tcp_hdr *hdr_rcv; + int ret; + + if (fr && ((s->state & PICO_SOCKET_STATE_TCP) > PICO_SOCKET_STATE_TCP_SYN_RECV)) { + /* in synchronized state: send RST with seq = ack from previous segment */ + hdr_rcv = (struct pico_tcp_hdr *) fr->transport_hdr; + ret = tcp_do_send_rst(s, hdr_rcv->ack); + } else { + /* non-synchronized state */ + /* go to CLOSED here to prevent timer callback to go on after timeout */ + (t->sock).state &= 0x00FFU; + (t->sock).state |= PICO_SOCKET_STATE_TCP_CLOSED; + ret = tcp_do_send_rst(s, long_be(t->snd_nxt)); + + /* Set generic socket state to CLOSED, too */ + (t->sock).state &= 0xFF00U; + (t->sock).state |= PICO_SOCKET_STATE_CLOSED; + + /* call EV_FIN wakeup before deleting */ + if ((t->sock).wakeup) + (t->sock).wakeup(PICO_SOCK_EV_FIN, &(t->sock)); + + /* delete socket */ + pico_socket_del(&t->sock); + } + return ret; +} + +static inline void tcp_fill_rst_payload(struct pico_frame *fr, struct pico_frame *f) +{ + /* fill in IP data from original frame */ + if (IS_IPV4(fr)) { + memcpy(f->net_hdr, fr->net_hdr, sizeof(struct pico_ipv4_hdr)); + ((struct pico_ipv4_hdr *)(f->net_hdr))->dst.addr = ((struct pico_ipv4_hdr *)(fr->net_hdr))->src.addr; + ((struct pico_ipv4_hdr *)(f->net_hdr))->src.addr = ((struct pico_ipv4_hdr *)(fr->net_hdr))->dst.addr; + tcp_dbg("Making IPv4 reset frame...\n"); + + } else { + memcpy(f->net_hdr, fr->net_hdr, sizeof(struct pico_ipv6_hdr)); + ((struct pico_ipv6_hdr *)(f->net_hdr))->dst = ((struct pico_ipv6_hdr *)(fr->net_hdr))->src; + ((struct pico_ipv6_hdr *)(f->net_hdr))->src = ((struct pico_ipv6_hdr *)(fr->net_hdr))->dst; + } + + /* fill in TCP data from original frame */ + ((struct pico_tcp_hdr *)(f->transport_hdr))->trans.dport = ((struct pico_tcp_hdr *)(fr->transport_hdr))->trans.sport; + ((struct pico_tcp_hdr *)(f->transport_hdr))->trans.sport = ((struct pico_tcp_hdr *)(fr->transport_hdr))->trans.dport; + +} + + +static inline void tcp_fill_rst_header(struct pico_frame *fr, struct pico_tcp_hdr *hdr1, struct pico_frame *f, struct pico_tcp_hdr *hdr) +{ + if(!(hdr1->flags & PICO_TCP_ACK)) + hdr->flags |= PICO_TCP_ACK; + + hdr->rwnd = 0; + if (((struct pico_tcp_hdr *)(fr->transport_hdr))->flags & PICO_TCP_ACK) { + hdr->seq = ((struct pico_tcp_hdr *)(fr->transport_hdr))->ack; + } else { + hdr->seq = 0U; + } + + hdr->ack = 0; + if(!(hdr1->flags & PICO_TCP_ACK)) + hdr->ack = long_be(long_be(((struct pico_tcp_hdr *)(fr->transport_hdr))->seq) + fr->payload_len); + + hdr->crc = short_be(pico_tcp_checksum(f)); +} + +int pico_tcp_reply_rst(struct pico_frame *fr) +{ + struct pico_tcp_hdr *hdr, *hdr1; + struct pico_frame *f; + uint16_t size = PICO_SIZE_TCPHDR; + + + hdr1 = (struct pico_tcp_hdr *) (fr->transport_hdr); + if ((hdr1->flags & PICO_TCP_RST) != 0) + return -1; + + tcp_dbg("TCP> sending RST ... \n"); + + f = fr->sock->net->alloc(fr->sock->net, size); + if (!f) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + tcp_fill_rst_payload(fr, f); + + hdr = (struct pico_tcp_hdr *) f->transport_hdr; + hdr->len = (uint8_t)(size << 2); + hdr->flags = PICO_TCP_RST; + + tcp_fill_rst_header(fr, hdr1, f, hdr); + + if (0) { +#ifdef PICO_SUPPORT_IPV4 + } else if (IS_IPV4(f)) { + tcp_dbg("Pushing IPv4 reset frame...\n"); + pico_ipv4_frame_push(f, &(((struct pico_ipv4_hdr *)(f->net_hdr))->dst), PICO_PROTO_TCP); +#endif +#ifdef PICO_SUPPORT_IPV6 + } else { + pico_ipv6_frame_push(f, NULL, &(((struct pico_ipv6_hdr *)(f->net_hdr))->dst), PICO_PROTO_TCP, 0); +#endif + } + + + return 0; +} + +static int tcp_nosync_rst(struct pico_socket *s, struct pico_frame *fr) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *) s; + struct pico_frame *f; + struct pico_tcp_hdr *hdr, *hdr_rcv; + uint16_t opt_len = tcp_options_size(t, PICO_TCP_RST | PICO_TCP_ACK); + hdr_rcv = (struct pico_tcp_hdr *) fr->transport_hdr; + + tcp_dbg("TCP SEND RST (NON-SYNC) >>>>>>>>>>>>>>>>>> state %x\n", (s->state & PICO_SOCKET_STATE_TCP)); + if (((s->state & PICO_SOCKET_STATE_TCP) == PICO_SOCKET_STATE_TCP_LISTEN)) { + if ((fr->flags & PICO_TCP_RST) != 0) + return 0; + + return pico_tcp_reply_rst(fr); + } + + /***************************************************************************/ + /* sending RST */ + f = t->sock.net->alloc(t->sock.net, (uint16_t)(PICO_SIZE_TCPHDR + opt_len)); + + if (!f) { + return -1; + } + + + f->sock = &t->sock; + hdr = (struct pico_tcp_hdr *) f->transport_hdr; + hdr->len = (uint8_t)((PICO_SIZE_TCPHDR + opt_len) << 2 | t->jumbo); + hdr->flags = PICO_TCP_RST | PICO_TCP_ACK; + hdr->rwnd = short_be(t->wnd); + tcp_set_space(t); + tcp_add_options(t, f, PICO_TCP_RST | PICO_TCP_ACK, opt_len); + hdr->trans.sport = t->sock.local_port; + hdr->trans.dport = t->sock.remote_port; + + /* non-synchronized state */ + if (hdr_rcv->flags & PICO_TCP_ACK) { + hdr->seq = hdr_rcv->ack; + } else { + hdr->seq = 0U; + } + + hdr->ack = long_be(SEQN(fr) + fr->payload_len); + + t->rcv_ackd = t->rcv_nxt; + f->start = f->transport_hdr + PICO_SIZE_TCPHDR; + hdr->rwnd = short_be(t->wnd); + hdr->crc = 0; + hdr->crc = short_be(pico_tcp_checksum(f)); + + /* TCP: ENQUEUE to PROTO */ + pico_enqueue(&tcp_out, f); + + /***************************************************************************/ + + tcp_dbg("TCP SEND_RST (NON_SYNC) >>>>>>>>>>>>>>> DONE, ...\n"); + + return 0; +} + +static void tcp_deltcb(pico_time when, void *arg); + +static void tcp_linger(struct pico_socket_tcp *t) +{ + pico_timer_cancel(t->fin_tmr); + t->fin_tmr = pico_timer_add(t->linger_timeout, tcp_deltcb, t); +} + +static void tcp_send_fin(struct pico_socket_tcp *t) +{ + struct pico_frame *f; + struct pico_tcp_hdr *hdr; + uint16_t opt_len = tcp_options_size(t, PICO_TCP_FIN); + f = t->sock.net->alloc(t->sock.net, (uint16_t)(PICO_SIZE_TCPHDR + opt_len)); + if (!f) { + return; + } + + f->sock = &t->sock; + hdr = (struct pico_tcp_hdr *) f->transport_hdr; + hdr->len = (uint8_t)((PICO_SIZE_TCPHDR + opt_len) << 2 | t->jumbo); + hdr->flags = PICO_TCP_FIN | PICO_TCP_ACK; + hdr->ack = long_be(t->rcv_nxt); + t->rcv_ackd = t->rcv_nxt; + hdr->rwnd = short_be(t->wnd); + tcp_set_space(t); + tcp_add_options(t, f, PICO_TCP_FIN, opt_len); + hdr->trans.sport = t->sock.local_port; + hdr->trans.dport = t->sock.remote_port; + hdr->seq = long_be(t->snd_nxt); + + f->start = f->transport_hdr + PICO_SIZE_TCPHDR; + hdr->rwnd = short_be(t->wnd); + hdr->crc = 0; + hdr->crc = short_be(pico_tcp_checksum(f)); + /* tcp_dbg("SENDING FIN...\n"); */ + if (t->linger_timeout > 0) { + pico_enqueue(&tcp_out, f); + t->snd_nxt++; + } else { + pico_frame_discard(f); + } + tcp_linger(t); +} + +static void tcp_sack_prepare(struct pico_socket_tcp *t) +{ + struct tcp_input_segment *pkt; + uint32_t left = 0, right = 0; + struct tcp_sack_block *sb; + int n = 0; + if (t->sacks) /* previous sacks are pending */ + return; + + pkt = first_segment(&t->tcpq_in); + while(n < 3) { + if (!pkt) { + if(left) { + sb = PICO_ZALLOC(sizeof(struct tcp_sack_block)); + if (!sb) + break; + + sb->left = long_be(left); + sb->right = long_be(right); + n++; + sb->next = t->sacks; + t->sacks = sb; + left = 0; + right = 0; + } + + break; + } + + if (pkt->seq < t->rcv_nxt) { + pkt = next_segment(&t->tcpq_in, pkt); + continue; + } + + if (!left) { + left = pkt->seq; + right = pkt->seq + pkt->payload_len; + pkt = next_segment(&t->tcpq_in, pkt); + continue; + } + + if(pkt->seq == right) { + right += pkt->payload_len; + pkt = next_segment(&t->tcpq_in, pkt); + continue; + } else { + sb = PICO_ZALLOC(sizeof(struct tcp_sack_block)); + if (!sb) + break; + + sb->left = long_be(left); + sb->right = long_be(right); + n++; + sb->next = t->sacks; + t->sacks = sb; + left = 0; + right = 0; + pkt = next_segment(&t->tcpq_in, pkt); + } + } +} + +static inline int tcp_data_in_expected(struct pico_socket_tcp *t, struct pico_frame *f) +{ + struct tcp_input_segment *nxt; + if (pico_seq_compare(SEQN(f), t->rcv_nxt) == 0) { /* Exactly what we expected */ + /* Create new segment and enqueue it */ + struct tcp_input_segment *input = segment_from_frame(f); + if (!input) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + if(pico_enqueue_segment(&t->tcpq_in, input) <= 0) + { + /* failed to enqueue, destroy segment */ + PICO_FREE(input->payload); + PICO_FREE(input); + return -1; + } else { + t->rcv_nxt = SEQN(f) + f->payload_len; + nxt = peek_segment(&t->tcpq_in, t->rcv_nxt); + while(nxt) { + tcp_dbg("scrolling rcv_nxt...%08x\n", t->rcv_nxt); + t->rcv_nxt += nxt->payload_len; + nxt = peek_segment(&t->tcpq_in, t->rcv_nxt); + } + t->sock.ev_pending |= PICO_SOCK_EV_RD; + } + } else { + tcp_dbg("TCP> lo segment. Uninteresting retransmission. (exp: %x got: %x)\n", t->rcv_nxt, SEQN(f)); + } + + return 0; +} + +static inline int tcp_data_in_high_segment(struct pico_socket_tcp *t, struct pico_frame *f) +{ + tcp_dbg("TCP> hi segment. Possible packet loss. I'll dupack this. (exp: %x got: %x)\n", t->rcv_nxt, SEQN(f)); + if (t->sack_ok) { + struct tcp_input_segment *input = segment_from_frame(f); + if (!input) { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + if(pico_enqueue_segment(&t->tcpq_in, input) <= 0) { + /* failed to enqueue, destroy segment */ + PICO_FREE(input->payload); + PICO_FREE(input); + return -1; + } + + tcp_sack_prepare(t); + } + + return 0; +} + +static inline void tcp_data_in_send_ack(struct pico_socket_tcp *t, struct pico_frame *f) +{ + struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *) f->transport_hdr; + /* In either case, ack til recv_nxt, unless received data raises a RST flag. */ + if (((t->sock.state & PICO_SOCKET_STATE_TCP) != PICO_SOCKET_STATE_TCP_CLOSE_WAIT) && + ((t->sock.state & PICO_SOCKET_STATE_TCP) != PICO_SOCKET_STATE_TCP_SYN_SENT) && + ((t->sock.state & PICO_SOCKET_STATE_TCP) != PICO_SOCKET_STATE_TCP_SYN_RECV) && + ((hdr->flags & PICO_TCP_RST) == 0)) + tcp_send_ack(t); +} + +static int tcp_data_in(struct pico_socket *s, struct pico_frame *f) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)s; + struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *) f->transport_hdr; + uint16_t payload_len = (uint16_t)(f->transport_len - ((hdr->len & 0xf0u) >> 2u)); + int ret = 0; + (void)hdr; + + if (((hdr->len & 0xf0u) >> 2u) <= f->transport_len) { + tcp_parse_options(f); + f->payload = f->transport_hdr + ((hdr->len & 0xf0u) >> 2u); + f->payload_len = payload_len; + tcp_dbg("TCP> Received segment. (exp: %x got: %x)\n", t->rcv_nxt, SEQN(f)); + + if (pico_seq_compare(SEQN(f), t->rcv_nxt) <= 0) { + ret = tcp_data_in_expected(t, f); + } else { + ret = tcp_data_in_high_segment(t, f); + } + + tcp_data_in_send_ack(t, f); + return ret; + } else { + tcp_dbg("TCP: invalid data in pkt len, exp: %d, got %d\n", (hdr->len & 0xf0) >> 2, f->transport_len); + return -1; + } +} + +static int tcp_ack_advance_una(struct pico_socket_tcp *t, struct pico_frame *f, pico_time *timestamp) +{ + int ret = release_all_until(&t->tcpq_out, ACKN(f), timestamp); + if (ret > 0) { + t->sock.ev_pending |= PICO_SOCK_EV_WR; + } + + return ret; +} + +static uint16_t time_diff(pico_time a, pico_time b) +{ + if (a >= b) + return (uint16_t)(a - b); + else + return (uint16_t)(b - a); +} + +static void tcp_rtt(struct pico_socket_tcp *t, uint32_t rtt) +{ + + uint32_t avg = t->avg_rtt; + uint32_t rvar = t->rttvar; + if (!avg) { + /* This follows RFC2988 + * (2.2) When the first RTT measurement R is made, the host MUST set + * + * SRTT <- R + * RTTVAR <- R/2 + * RTO <- SRTT + max (G, K*RTTVAR) + */ + t->avg_rtt = rtt; + t->rttvar = rtt >> 1; + rto_set(t, t->avg_rtt + (t->rttvar << 2)); + } else { + int32_t var = (int32_t)t->avg_rtt - (int32_t)rtt; + if (var < 0) + var = 0 - var; + + /* RFC2988, section (2.3). Alpha and beta are the ones suggested. */ + + /* First, evaluate a new value for the rttvar */ + t->rttvar <<= 2; + t->rttvar -= rvar; + t->rttvar += (uint32_t)var; + t->rttvar >>= 2; + + /* Then, calculate the new avg_rtt */ + t->avg_rtt <<= 3; + t->avg_rtt -= avg; + t->avg_rtt += rtt; + t->avg_rtt >>= 3; + + /* Finally, assign a new value for the RTO, as specified in the RFC, with K=4 */ + rto_set(t, t->avg_rtt + (t->rttvar << 2)); + } + + tcp_dbg(" -----=============== RTT CUR: %u AVG: %u RTTVAR: %u RTO: %u ======================----\n", rtt, t->avg_rtt, t->rttvar, t->rto); +} + +static void tcp_congestion_control(struct pico_socket_tcp *t) +{ + if (t->x_mode > PICO_TCP_LOOKAHEAD) + return; + + tcp_dbg("Doing congestion control\n"); + if (t->cwnd < t->ssthresh) { + t->cwnd++; + } else { + t->cwnd_counter++; + if (t->cwnd_counter >= t->cwnd) { + t->cwnd++; + t->cwnd_counter = 0; + } + } + + tcp_dbg("TCP_CWND, %lu, %u, %u, %u\n", TCP_TIME, t->cwnd, t->ssthresh, t->in_flight); +} + +static void add_retransmission_timer(struct pico_socket_tcp *t, pico_time next_ts); + + +/* Retransmission time out (RTO). */ + +static void tcp_first_timeout(struct pico_socket_tcp *t) +{ + t->x_mode = PICO_TCP_BLACKOUT; + t->cwnd = PICO_TCP_IW; + t->in_flight = 0; +} + +static int tcp_rto_xmit(struct pico_socket_tcp *t, struct pico_frame *f) +{ + struct pico_frame *cpy; + /* TCP: ENQUEUE to PROTO ( retransmit )*/ + cpy = pico_frame_copy(f); + if (!cpy) { + add_retransmission_timer(t, (t->rto << t->backoff) + TCP_TIME); + return -1; + } + + if (pico_enqueue(&tcp_out, cpy) > 0) { + t->snd_last_out = SEQN(cpy); + add_retransmission_timer(t, (t->rto << (++t->backoff)) + TCP_TIME); + tcp_dbg("TCP_CWND, %lu, %u, %u, %u\n", TCP_TIME, t->cwnd, t->ssthresh, t->in_flight); + tcp_dbg("Sending RTO!\n"); + return 1; + } else { + tcp_dbg("RTO fail, retry!\n"); + add_retransmission_timer(t, (t->rto << t->backoff) + TCP_TIME); + pico_frame_discard(cpy); + return 0; + } +} + +static void tcp_next_zerowindow_probe(struct pico_socket_tcp *t) +{ + tcp_dbg("Sending probe!\n"); + tcp_send_probe(t); + add_retransmission_timer(t, (t->rto << ++t->backoff) + TCP_TIME); +} + +static int tcp_is_allowed_to_send(struct pico_socket_tcp *t) +{ + return t->sock.net && + ( + ((t->sock.state & 0xFF00) == PICO_SOCKET_STATE_TCP_ESTABLISHED) || + ((t->sock.state & 0xFF00) == PICO_SOCKET_STATE_TCP_CLOSE_WAIT) + ) && + ((t->backoff < PICO_TCP_MAX_RETRANS)); +} + +static inline int tcp_retrans_timeout_check_queue(struct pico_socket_tcp *t) +{ + struct pico_frame *f = NULL; + f = first_segment(&t->tcpq_out); + while (f) { + tcp_dbg("Checking frame in queue \n"); + if (t->x_mode == PICO_TCP_WINDOW_FULL) { + tcp_dbg("TCP BLACKOUT> TIMED OUT (output) frame %08x, len= %d rto=%d Win full: %d frame flags: %04x\n", SEQN(f), f->payload_len, t->rto, t->x_mode == PICO_TCP_WINDOW_FULL, f->flags); + tcp_next_zerowindow_probe(t); + return -1; + } + + if (t->x_mode != PICO_TCP_BLACKOUT) + tcp_first_timeout(t); + + tcp_add_header(t, f); + if (tcp_rto_xmit(t, f) > 0) /* A segment has been rexmit'd */ + return -1; + + f = next_segment(&t->tcpq_out, f); + } + if (t->tcpq_out.size < t->tcpq_out.max_size) + t->sock.ev_pending |= PICO_SOCK_EV_WR; + + return 0; + + + +} + +static void tcp_retrans_timeout(pico_time val, void *sock) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *) sock; + + t->retrans_tmr = 0; + + if (t->retrans_tmr_due == 0ull) { + return; + } + + if (t->retrans_tmr_due > val) { + /* Timer was postponed... */ + add_retransmission_timer(t, t->retrans_tmr_due); + return; + } + + tcp_dbg("TIMEOUT! backoff = %d, rto: %d\n", t->backoff, t->rto); + t->retrans_tmr_due = 0ull; + + if (tcp_is_allowed_to_send(t)) { + if (tcp_retrans_timeout_check_queue(t) < 0) + return; + } + else if(t->backoff >= PICO_TCP_MAX_RETRANS && (t->sock.state & 0xFF00) == PICO_SOCKET_STATE_TCP_ESTABLISHED ) + { + tcp_dbg("Connection timeout!\n"); + /* the retransmission timer, failed to get an ack for a frame, gives up on the connection */ + tcp_discard_all_segments(&t->tcpq_out); + if(t->sock.wakeup) + t->sock.wakeup(PICO_SOCK_EV_FIN, &t->sock); + + /* delete socket */ + pico_socket_del(&t->sock); + return; + } else { + tcp_dbg("Retransmission not allowed, rescheduling\n"); + } +} + +static void add_retransmission_timer(struct pico_socket_tcp *t, pico_time next_ts) +{ + struct pico_tree_node *index; + pico_time now = TCP_TIME; + pico_time val = 0; + + + if (next_ts == 0) { + struct pico_frame *f; + + pico_tree_foreach(index, &t->tcpq_out.pool){ + f = index->keyValue; + if ((next_ts == 0) || ((f->timestamp < next_ts) && (f->timestamp > 0))) { + next_ts = f->timestamp; + val = next_ts + (t->rto << t->backoff); + } + } + } else { + val = next_ts; + } + + if ((val > 0) || (val > now)) { + t->retrans_tmr_due = val; + } else { + t->retrans_tmr_due = now + 1; + } + + if (!t->retrans_tmr) { + t->retrans_tmr = pico_timer_add(t->retrans_tmr_due - now, tcp_retrans_timeout, t); + tcp_dbg("Next timeout in %u msec\n", (uint32_t) (t->retrans_tmr_due - now)); + } +} + +static int tcp_retrans(struct pico_socket_tcp *t, struct pico_frame *f) +{ + struct pico_frame *cpy; + if (f) { + tcp_dbg("TCP> RETRANS (by dupack) frame %08x, len= %d\n", SEQN(f), f->payload_len); + tcp_add_header(t, f); + /* TCP: ENQUEUE to PROTO ( retransmit )*/ + cpy = pico_frame_copy(f); + if (!cpy) { + return -1; + } + + if (pico_enqueue(&tcp_out, cpy) > 0) { + t->in_flight++; + t->snd_last_out = SEQN(cpy); + } else { + pico_frame_discard(cpy); + } + + add_retransmission_timer(t, TCP_TIME + t->rto); + return(f->payload_len); + } + + return 0; +} + +#ifdef TCP_ACK_DBG +static void tcp_ack_dbg(struct pico_socket *s, struct pico_frame *f) +{ + uint32_t una, nxt, ack, cur; + struct pico_frame *una_f = NULL, *cur_f; + struct pico_tree_node *idx; + struct pico_socket_tcp *t = (struct pico_socket_tcp *)s; + char info[64]; + char tmp[64]; + ack = ACKN(f); + nxt = t->snd_nxt; + tcp_dbg("===================================\n"); + tcp_dbg("Queue out (%d/%d). ACKED=%08x\n", t->tcpq_out.size, t->tcpq_out.max_size, ack); + + pico_tree_foreach(idx, &t->tcpq_out.pool) { + info[0] = 0; + cur_f = idx->keyValue; + cur = SEQN(cur_f); + if (!una_f) { + una_f = cur_f; + una = SEQN(una_f); + } + + if (cur == nxt) { + strncpy(tmp, info, strlen(info)); + snprintf(info, 64, "%s SND_NXT", tmp); + } + + if (cur == ack) { + strncpy(tmp, info, strlen(info)); + snprintf(info, 64, "%s ACK", tmp); + } + + if (cur == una) { + strncpy(tmp, info, strlen(info)); + snprintf(info, 64, "%s SND_UNA", tmp); + } + + if (cur == t->snd_last) { + strncpy(tmp, info, strlen(info)); + snprintf(info, 64, "%s SND_LAST", tmp); + } + + tcp_dbg("%08x %d%s\n", cur, cur_f->payload_len, info); + + } + tcp_dbg("SND_NXT is %08x, snd_LAST is %08x\n", nxt, t->snd_last); + tcp_dbg("===================================\n"); + tcp_dbg("\n\n"); +} +#endif + +static int tcp_ack(struct pico_socket *s, struct pico_frame *f) +{ + struct pico_frame *f_new; /* use with Nagle to push to out queue */ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)s; + struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *) f->transport_hdr; + uint32_t rtt = 0; + uint16_t acked = 0; + pico_time acked_timestamp = 0; + + struct pico_frame *una = NULL; + if ((hdr->flags & PICO_TCP_ACK) == 0) + return -1; + +#ifdef TCP_ACK_DBG + tcp_ack_dbg(s, f); +#endif + + tcp_parse_options(f); + t->recv_wnd = short_be(hdr->rwnd); + + acked = (uint16_t)tcp_ack_advance_una(t, f, &acked_timestamp); + una = first_segment(&t->tcpq_out); + t->ack_timestamp = TCP_TIME; + + if ((t->x_mode == PICO_TCP_BLACKOUT) || + ((t->x_mode == PICO_TCP_WINDOW_FULL) && ((t->recv_wnd << t->recv_wnd_scale) > t->mss))) { + int prev_mode = t->x_mode; + tcp_dbg("Re-entering look-ahead...\n\n\n"); + t->x_mode = PICO_TCP_LOOKAHEAD; + t->backoff = 0; + + if((prev_mode == PICO_TCP_BLACKOUT) && (acked > 0) && una) + { + t->snd_nxt = SEQN(una); + /* restart the retrans timer */ + if (t->retrans_tmr) { + t->retrans_tmr_due = 0ull; + } + } + } + + /* One should be acked. */ + if ((acked == 0) && (f->payload_len == 0) && (t->in_flight > 0)) + t->in_flight--; + + if (!una || acked > 0) { + t->x_mode = PICO_TCP_LOOKAHEAD; + tcp_dbg("Mode: Look-ahead. In flight: %d/%d buf: %d\n", t->in_flight, t->cwnd, t->tcpq_out.frames); + t->backoff = 0; + + /* Do rtt/rttvar/rto calculations */ + /* First, try with timestamps, using the value from options */ + if(f && (f->timestamp != 0)) { + rtt = time_diff(TCP_TIME, f->timestamp); + if (rtt) + tcp_rtt(t, rtt); + } else if(acked_timestamp) { + /* If no timestamps are there, use conservative estimation on the una */ + rtt = time_diff(TCP_TIME, acked_timestamp); + if (rtt) + tcp_rtt(t, rtt); + } + + tcp_dbg("TCP ACK> FRESH ACK %08x (acked %d) Queue size: %u/%u frames: %u cwnd: %u in_flight: %u snd_una: %u\n", ACKN(f), acked, t->tcpq_out.size, t->tcpq_out.max_size, t->tcpq_out.frames, t->cwnd, t->in_flight, SEQN(una)); + if (acked > t->in_flight) { + tcp_dbg("WARNING: in flight < 0\n"); + t->in_flight = 0; + } else + t->in_flight -= (acked); + + } else if ((t->snd_old_ack == ACKN(f)) && /* We've just seen this ack, and... */ + ((0 == (hdr->flags & (PICO_TCP_PSH | PICO_TCP_SYN))) && + (f->payload_len == 0)) && /* This is a pure ack, and... */ + (ACKN(f) != t->snd_nxt)) /* There is something in flight awaiting to be acked... */ + { + /* Process incoming duplicate ack. */ + if (t->x_mode < PICO_TCP_RECOVER) { + t->x_mode++; + tcp_dbg("Mode: DUPACK %d, due to PURE ACK %0x, len = %d\n", t->x_mode, SEQN(f), f->payload_len); + /* tcp_dbg("ACK: %x - QUEUE: %x\n", ACKN(f), SEQN(first_segment(&t->tcpq_out))); */ + if (t->x_mode == PICO_TCP_RECOVER) { /* Switching mode */ + if (t->in_flight > PICO_TCP_IW) + t->cwnd = (uint16_t)t->in_flight; + else + t->cwnd = PICO_TCP_IW; + + t->snd_retry = SEQN((struct pico_frame *)first_segment(&t->tcpq_out)); + if (t->ssthresh > t->cwnd) + t->ssthresh >>= 2; + else + t->ssthresh = (t->cwnd >> 1); + + if (t->ssthresh < 2) + t->ssthresh = 2; + } + } else if (t->x_mode == PICO_TCP_RECOVER) { + /* tcp_dbg("TCP RECOVER> DUPACK! snd_una: %08x, snd_nxt: %08x, acked now: %08x\n", SEQN(first_segment(&t->tcpq_out)), t->snd_nxt, ACKN(f)); */ + if (t->in_flight <= t->cwnd) { + struct pico_frame *nxt = peek_segment(&t->tcpq_out, t->snd_retry); + if (!nxt) + nxt = first_segment(&t->tcpq_out); + + while (nxt && (nxt->flags & PICO_FRAME_FLAG_SACKED) && (nxt != first_segment(&t->tcpq_out))) { + tcp_dbg("Skipping %08x because it is sacked.\n", SEQN(nxt)); + nxt = next_segment(&t->tcpq_out, nxt); + } + if (nxt && (pico_seq_compare(SEQN(nxt), t->snd_nxt)) > 0) + nxt = NULL; + + if (nxt && (pico_seq_compare(SEQN(nxt), SEQN((struct pico_frame *)first_segment(&t->tcpq_out))) > (int)(t->recv_wnd << t->recv_wnd_scale))) + nxt = NULL; + + if(!nxt) + nxt = first_segment(&t->tcpq_out); + + if (nxt) { + tcp_retrans(t, peek_segment(&t->tcpq_out, t->snd_retry)); + t->snd_retry = SEQN(nxt); + } + } + + if (++t->cwnd_counter > 1) { + t->cwnd--; + if (t->cwnd < 2) + t->cwnd = 2; + + t->cwnd_counter = 0; + } + } else { + tcp_dbg("DUPACK in mode %d \n", t->x_mode); + + } + } /* End case duplicate ack detection */ + + /* Linux very special zero-window probe detection (see bug #107) */ + if ((0 == (hdr->flags & (PICO_TCP_PSH | PICO_TCP_SYN))) && /* This is a pure ack, and... */ + (ACKN(f) == t->snd_nxt) && /* it's acking our snd_nxt, and... */ + (pico_seq_compare(SEQN(f), t->rcv_nxt) < 0)) /* Has an old seq number */ + { + tcp_send_ack(t); + } + + + /* Do congestion control */ + tcp_congestion_control(t); + if ((acked > 0) && t->sock.wakeup) { + if (t->tcpq_out.size < t->tcpq_out.max_size) + t->sock.wakeup(PICO_SOCK_EV_WR, &(t->sock)); + + /* t->sock.ev_pending |= PICO_SOCK_EV_WR; */ + } + + /* if Nagle enabled, check if no unack'ed data and fill out queue (till window) */ + if (IS_NAGLE_ENABLED((&(t->sock)))) { + while (!IS_TCP_HOLDQ_EMPTY(t) && ((t->tcpq_out.max_size - t->tcpq_out.size) >= t->mss)) { + tcp_dbg_nagle("TCP_ACK - NAGLE add new segment\n"); + f_new = pico_hold_segment_make(t); + if (f_new == NULL) + break; /* XXX corrupt !!! (or no memory) */ + + if (pico_enqueue_segment(&t->tcpq_out, f_new) <= 0) + /* handle error */ + tcp_dbg_nagle("TCP_ACK - NAGLE FAILED to enqueue in out\n"); + } + } + + /* If some space was created, put a few segments out. */ + tcp_dbg("TCP_CWND, %lu, %u, %u, %u\n", TCP_TIME, t->cwnd, t->ssthresh, t->in_flight); + if (t->x_mode == PICO_TCP_LOOKAHEAD) { + if ((t->cwnd >= t->in_flight) && (t->snd_nxt > t->snd_last_out)) { + pico_tcp_output(&t->sock, (int)t->cwnd - (int)t->in_flight); + } + } + + add_retransmission_timer(t, 0); + t->snd_old_ack = ACKN(f); + return 0; +} + +static int tcp_finwaitack(struct pico_socket *s, struct pico_frame *f) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)s; + tcp_dbg("RECEIVED ACK IN FIN_WAIT1\n"); + + /* acking part */ + tcp_ack(s, f); + + + tcp_dbg("FIN_WAIT1: ack is %08x - snd_nxt is %08x\n", ACKN(f), t->snd_nxt); + if (ACKN(f) == (t->snd_nxt - 1u)) { + /* update TCP state */ + s->state &= 0x00FFU; + s->state |= PICO_SOCKET_STATE_TCP_FIN_WAIT2; + tcp_dbg("TCP> IN STATE FIN_WAIT2\n"); + } + return 0; +} + +static void tcp_deltcb(pico_time when, void *arg) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)arg; + IGNORE_PARAMETER(when); + + /* send RST if not yet in TIME_WAIT */ + if ( (((t->sock).state & PICO_SOCKET_STATE_TCP) != PICO_SOCKET_STATE_TCP_TIME_WAIT) + && (((t->sock).state & PICO_SOCKET_STATE_TCP) != PICO_SOCKET_STATE_TCP_CLOSING) ) { + tcp_dbg("Called deltcb in state = %04x (sending reset!)\n", (t->sock).state); + tcp_do_send_rst(&t->sock, long_be(t->snd_nxt)); + } else { + tcp_dbg("Called deltcb in state = %04x\n", (t->sock).state); + } + + /* update state */ + (t->sock).state &= 0x00FFU; + (t->sock).state |= PICO_SOCKET_STATE_TCP_CLOSED; + (t->sock).state &= 0xFF00U; + (t->sock).state |= PICO_SOCKET_STATE_CLOSED; + /* call EV_FIN wakeup before deleting */ + if (t->sock.wakeup) { + (t->sock).wakeup(PICO_SOCK_EV_FIN, &(t->sock)); + } + + /* delete socket */ + pico_socket_del(&t->sock); +} + +static int tcp_finwaitfin(struct pico_socket *s, struct pico_frame *f) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)s; + struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *) (f->transport_hdr); + tcp_dbg("TCP> received fin in FIN_WAIT2\n"); + /* received FIN, increase ACK nr */ + t->rcv_nxt = long_be(hdr->seq) + 1; + s->state &= 0x00FFU; + s->state |= PICO_SOCKET_STATE_TCP_TIME_WAIT; + /* set SHUT_REMOTE */ + s->state |= PICO_SOCKET_STATE_SHUT_REMOTE; + if (s->wakeup) + s->wakeup(PICO_SOCK_EV_CLOSE, s); + + if (f->payload_len > 0) /* needed?? */ + tcp_data_in(s, f); + + /* send ACK */ + tcp_send_ack(t); + /* linger */ + tcp_linger(t); + return 0; +} + +static int tcp_closing_ack(struct pico_socket *s, struct pico_frame *f) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)s; + tcp_dbg("TCP> received ack in CLOSING\n"); + /* acking part */ + tcp_ack(s, f); + + /* update TCP state DLA TODO: Only if FIN is acked! */ + tcp_dbg("CLOSING: ack is %08x - snd_nxt is %08x\n", ACKN(f), t->snd_nxt); + if (ACKN(f) == t->snd_nxt) { + s->state &= 0x00FFU; + s->state |= PICO_SOCKET_STATE_TCP_TIME_WAIT; + /* set timer */ + tcp_linger(t); + } + return 0; +} + +static int tcp_lastackwait(struct pico_socket *s, struct pico_frame *f) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)s; + tcp_dbg("LAST_ACK: ack is %08x - snd_nxt is %08x\n", ACKN(f), t->snd_nxt); + if (ACKN(f) == t->snd_nxt) { + s->state &= 0x00FFU; + s->state |= PICO_SOCKET_STATE_TCP_CLOSED; + s->state &= 0xFF00U; + s->state |= PICO_SOCKET_STATE_CLOSED; + /* call socket wakeup with EV_FIN */ + if (s->wakeup) + s->wakeup(PICO_SOCK_EV_FIN, s); + + /* delete socket */ + pico_socket_del(s); + } + return 0; +} + +static int tcp_syn(struct pico_socket *s, struct pico_frame *f) +{ + struct pico_socket_tcp *new = NULL; + struct pico_tcp_hdr *hdr = NULL; + uint16_t mtu; + if(s->number_of_pending_conn >= s->max_backlog) + return -1; + + new = (struct pico_socket_tcp *)pico_socket_clone(s); + hdr = (struct pico_tcp_hdr *)f->transport_hdr; + if (!new) + return -1; + +#ifdef PICO_TCP_SUPPORT_SOCKET_STATS + pico_timer_add(2000, sock_stats, s); +#endif + + new->sock.remote_port = ((struct pico_trans *)f->transport_hdr)->sport; +#ifdef PICO_SUPPORT_IPV4 + if (IS_IPV4(f)) { + new->sock.remote_addr.ip4.addr = ((struct pico_ipv4_hdr *)(f->net_hdr))->src.addr; + new->sock.local_addr.ip4.addr = ((struct pico_ipv4_hdr *)(f->net_hdr))->dst.addr; + } + +#endif +#ifdef PICO_SUPPORT_IPV6 + if (IS_IPV6(f)) { + new->sock.remote_addr.ip6 = ((struct pico_ipv6_hdr *)(f->net_hdr))->src; + new->sock.local_addr.ip6 = ((struct pico_ipv6_hdr *)(f->net_hdr))->dst; + } + +#endif + f->sock = &new->sock; + tcp_parse_options(f); + mtu = (uint16_t)pico_socket_get_mss(&new->sock); + new->mss = (uint16_t)(mtu - PICO_SIZE_TCPHDR); + new->tcpq_in.max_size = PICO_DEFAULT_SOCKETQ; + new->tcpq_out.max_size = PICO_DEFAULT_SOCKETQ; + new->tcpq_hold.max_size = 2u * mtu; + new->rcv_nxt = long_be(hdr->seq) + 1; + new->snd_nxt = long_be(pico_paws()); + new->snd_last = new->snd_nxt; + new->cwnd = PICO_TCP_IW; + new->ssthresh = (uint16_t)((uint16_t)(PICO_DEFAULT_SOCKETQ / new->mss) - (((uint16_t)(PICO_DEFAULT_SOCKETQ / new->mss)) >> 3u)); + new->recv_wnd = short_be(hdr->rwnd); + new->jumbo = hdr->len & 0x07; + new->linger_timeout = PICO_SOCKET_LINGER_TIMEOUT; + s->number_of_pending_conn++; + new->sock.parent = s; + new->sock.wakeup = s->wakeup; + rto_set(new, PICO_TCP_RTO_MIN); + /* Initialize timestamp values */ + new->sock.state = PICO_SOCKET_STATE_BOUND | PICO_SOCKET_STATE_CONNECTED | PICO_SOCKET_STATE_TCP_SYN_RECV; + pico_socket_add(&new->sock); + tcp_send_synack(&new->sock); + tcp_dbg("SYNACK sent, socket added. snd_nxt is %08x\n", new->snd_nxt); + return 0; +} + +static int tcp_synrecv_syn(struct pico_socket *s, struct pico_frame *f) +{ + struct pico_tcp_hdr *hdr = NULL; + struct pico_socket_tcp *t = TCP_SOCK(s); + hdr = (struct pico_tcp_hdr *)f->transport_hdr; + if (t->rcv_nxt == long_be(hdr->seq) + 1u) { + /* take back our own SEQ number to its original value, + * so the synack retransmitted is identical to the original. + */ + t->snd_nxt--; + tcp_send_synack(s); + } else { + tcp_send_rst(s, f); + return -1; + } + + return 0; +} + +static void tcp_set_init_point(struct pico_socket *s) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)s; + t->rcv_processed = t->rcv_nxt; +} + + +uint16_t pico_tcp_get_socket_mss(struct pico_socket *s) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *) s; + if (t->mss > 0) + return (uint16_t)(t->mss + PICO_SIZE_TCPHDR); + else + return (uint16_t)pico_socket_get_mss(s); +} + +static int tcp_synack(struct pico_socket *s, struct pico_frame *f) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *) s; + struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *)f->transport_hdr; + + if (ACKN(f) == (1u + t->snd_nxt)) { + /* Get rid of initconn retry */ + pico_timer_cancel(t->retrans_tmr); + t->retrans_tmr = 0; + + t->rcv_nxt = long_be(hdr->seq); + t->rcv_processed = t->rcv_nxt + 1; + tcp_ack(s, f); + + s->state &= 0x00FFU; + s->state |= PICO_SOCKET_STATE_TCP_ESTABLISHED; + tcp_dbg("TCP> Established. State: %x\n", s->state); + + if (s->wakeup) + s->wakeup(PICO_SOCK_EV_CONN, s); + + s->ev_pending |= PICO_SOCK_EV_WR; + + t->rcv_nxt++; + t->snd_nxt++; + tcp_send_ack(t); /* return ACK */ + + return 0; + + } else if ((hdr->flags & PICO_TCP_RST) == 0) { + tcp_dbg("TCP> Not established, RST sent.\n"); + tcp_nosync_rst(s, f); + return 0; + } else { + /* The segment has the reset flag on: Ignore! */ + return 0; + } +} + +static int tcp_first_ack(struct pico_socket *s, struct pico_frame *f) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)s; + struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *)f->transport_hdr; + tcp_dbg("ACK in SYN_RECV: expecting %08x got %08x\n", t->snd_nxt, ACKN(f)); + if (t->snd_nxt == ACKN(f)) { + tcp_set_init_point(s); + tcp_ack(s, f); + s->state &= 0x00FFU; + s->state |= PICO_SOCKET_STATE_TCP_ESTABLISHED; + tcp_dbg("TCP: Established. State now: %04x\n", s->state); + if( !s->parent && s->wakeup) { /* If the socket has no parent, -> sending socket that has a sim_open */ + tcp_dbg("FIRST ACK - No parent found -> sending socket\n"); + s->wakeup(PICO_SOCK_EV_CONN, s); + } + + if (s->parent && s->parent->wakeup) { + tcp_dbg("FIRST ACK - Parent found -> listening socket\n"); + s->wakeup = s->parent->wakeup; + s->parent->wakeup(PICO_SOCK_EV_CONN, s->parent); + } + + s->ev_pending |= PICO_SOCK_EV_WR; + tcp_dbg("%s: snd_nxt is now %08x\n", __FUNCTION__, t->snd_nxt); + return 0; + } else if ((hdr->flags & PICO_TCP_RST) == 0) { + tcp_nosync_rst(s, f); + return 0; + } else { + /* The segment has the reset flag on: Ignore! */ + return 0; + } +} + +static void tcp_attempt_closewait(struct pico_socket *s, struct pico_frame *f) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)s; + struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *) (f->transport_hdr); + if (pico_seq_compare(SEQN(f), t->rcv_nxt) == 0) { + /* received FIN, increase ACK nr */ + t->rcv_nxt = long_be(hdr->seq) + 1; + if (pico_seq_compare(SEQN(f), t->rcv_processed) == 0) { + if ((s->state & PICO_SOCKET_STATE_TCP) == PICO_SOCKET_STATE_TCP_ESTABLISHED) { + tcp_dbg("Changing state to CLOSE_WAIT\n"); + s->state &= 0x00FFU; + s->state |= PICO_SOCKET_STATE_TCP_CLOSE_WAIT; + } + + /* set SHUT_REMOTE */ + s->state |= PICO_SOCKET_STATE_SHUT_REMOTE; + tcp_dbg("TCP> Close-wait\n"); + if (s->wakeup) { + s->wakeup(PICO_SOCK_EV_CLOSE, s); + } + } else { + t->remote_closed = 1; + } + } + + +} + +static int tcp_closewait(struct pico_socket *s, struct pico_frame *f) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)s; + struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *) (f->transport_hdr); + + if (f->payload_len > 0) + tcp_data_in(s, f); + + if (hdr->flags & PICO_TCP_ACK) + tcp_ack(s, f); + + tcp_dbg("called close_wait (%p), in state %08x, f->flags: 0x%02x, hdr->flags: 0x%02x\n", tcp_closewait, s->state, f->flags, hdr->flags); + tcp_attempt_closewait(s, f); + + /* Ensure that the notification given to the socket + * did not put us in LAST_ACK state before sending the ACK: i.e. if + * pico_socket_close() has been called in the socket callback, we don't need to send + * an ACK here. + * + */ + if (((s->state & PICO_SOCKET_STATE_TCP) == PICO_SOCKET_STATE_TCP_CLOSE_WAIT) || + ((s->state & PICO_SOCKET_STATE_TCP) == PICO_SOCKET_STATE_TCP_ESTABLISHED)) + { + tcp_dbg("In closewait: Sending ack! (state is %08x)\n", s->state); + tcp_send_ack(t); + } + + return 0; +} + +static int tcp_rcvfin(struct pico_socket *s, struct pico_frame *f) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)s; + IGNORE_PARAMETER(f); + tcp_dbg("TCP> Received FIN in FIN_WAIT1\n"); + s->state &= 0x00FFU; + s->state |= PICO_SOCKET_STATE_TCP_CLOSING; + t->rcv_processed = t->rcv_nxt + 1; + t->rcv_nxt++; + /* send ACK */ + tcp_send_ack(t); + return 0; +} + +static int tcp_finack(struct pico_socket *s, struct pico_frame *f) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)s; + IGNORE_PARAMETER(f); + + tcp_dbg("TCP> ENTERED finack\n"); + t->rcv_nxt++; + /* send ACK */ + tcp_send_ack(t); + + /* call socket wakeup with EV_FIN */ + if (s->wakeup) + s->wakeup(PICO_SOCK_EV_FIN, s); + + s->state &= 0x00FFU; + s->state |= PICO_SOCKET_STATE_TCP_TIME_WAIT; + /* set SHUT_REMOTE */ + s->state |= PICO_SOCKET_STATE_SHUT_REMOTE; + + tcp_linger(t); + + return 0; +} + +static void tcp_force_closed(struct pico_socket *s) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *) s; + /* update state */ + (t->sock).state &= 0x00FFU; + (t->sock).state |= PICO_SOCKET_STATE_TCP_CLOSED; + (t->sock).state &= 0xFF00U; + (t->sock).state |= PICO_SOCKET_STATE_CLOSED; + /* call EV_ERR wakeup before deleting */ + if (((s->state & PICO_SOCKET_STATE_TCP) == PICO_SOCKET_STATE_TCP_ESTABLISHED)) { + if ((t->sock).wakeup) + (t->sock).wakeup(PICO_SOCK_EV_FIN, &(t->sock)); + } else { + pico_err = PICO_ERR_ECONNRESET; + if ((t->sock).wakeup) + (t->sock).wakeup(PICO_SOCK_EV_ERR, &(t->sock)); + + /* delete socket */ + pico_socket_del(&t->sock); + } +} + +static void tcp_wakeup_pending(struct pico_socket *s, uint16_t ev) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *) s; + if ((t->sock).wakeup) + (t->sock).wakeup(ev, &(t->sock)); +} + +static int tcp_rst(struct pico_socket *s, struct pico_frame *f) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *) s; + struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *) (f->transport_hdr); + + tcp_dbg("TCP >>>>>>>>>>>>>> received RST <<<<<<<<<<<<<<<<<<<<\n"); + if ((s->state & PICO_SOCKET_STATE_TCP) == PICO_SOCKET_STATE_TCP_SYN_SENT) { + /* the RST is acceptable if the ACK field acknowledges the SYN */ + if ((t->snd_nxt + 1u) == ACKN(f)) { /* valid, got to closed state */ + tcp_force_closed(s); + } else { /* not valid, ignore */ + tcp_dbg("TCP RST> IGNORE\n"); + return 0; + } + } else { /* all other states */ + /* all reset (RST) segments are validated by checking their SEQ-fields, + a reset is valid if its sequence number is in the window */ + uint32_t this_seq = long_be(hdr->seq); + if ((this_seq >= t->rcv_ackd) && (this_seq <= ((uint32_t)(short_be(hdr->rwnd) << (t->wnd_scale)) + t->rcv_ackd))) { + tcp_force_closed(s); + } else { /* not valid, ignore */ + tcp_dbg("TCP RST> IGNORE\n"); + return 0; + } + } + + return 0; +} +static int tcp_halfopencon(struct pico_socket *s, struct pico_frame *fr) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *) s; + IGNORE_PARAMETER(fr); + tcp_send_ack(t); + return 0; +} + +static int tcp_closeconn(struct pico_socket *s, struct pico_frame *fr) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *) s; + struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *) (fr->transport_hdr); + + if (pico_seq_compare(SEQN(fr), t->rcv_nxt) == 0) { + /* received FIN, increase ACK nr */ + t->rcv_nxt = long_be(hdr->seq) + 1; + s->state &= 0x00FFU; + s->state |= PICO_SOCKET_STATE_TCP_CLOSE_WAIT; + /* set SHUT_LOCAL */ + s->state |= PICO_SOCKET_STATE_SHUT_LOCAL; + pico_socket_close(s); + return 1; + } + + return 0; +} + +struct tcp_action_entry { + uint16_t tcpstate; + int (*syn)(struct pico_socket *s, struct pico_frame *f); + int (*synack)(struct pico_socket *s, struct pico_frame *f); + int (*ack)(struct pico_socket *s, struct pico_frame *f); + int (*data)(struct pico_socket *s, struct pico_frame *f); + int (*fin)(struct pico_socket *s, struct pico_frame *f); + int (*finack)(struct pico_socket *s, struct pico_frame *f); + int (*rst)(struct pico_socket *s, struct pico_frame *f); +}; + +static const struct tcp_action_entry tcp_fsm[] = { + /* State syn synack ack data fin finack rst*/ + { PICO_SOCKET_STATE_TCP_UNDEF, NULL, NULL, NULL, NULL, NULL, NULL, NULL }, + { PICO_SOCKET_STATE_TCP_CLOSED, NULL, NULL, NULL, NULL, NULL, NULL, NULL }, + { PICO_SOCKET_STATE_TCP_LISTEN, &tcp_syn, NULL, NULL, NULL, NULL, NULL, NULL }, + { PICO_SOCKET_STATE_TCP_SYN_SENT, NULL, &tcp_synack, NULL, NULL, NULL, NULL, &tcp_rst }, + { PICO_SOCKET_STATE_TCP_SYN_RECV, &tcp_synrecv_syn, NULL, &tcp_first_ack, &tcp_data_in, NULL, &tcp_closeconn, &tcp_rst }, + { PICO_SOCKET_STATE_TCP_ESTABLISHED, &tcp_halfopencon, &tcp_ack, &tcp_ack, &tcp_data_in, &tcp_closewait, &tcp_closewait, &tcp_rst }, + { PICO_SOCKET_STATE_TCP_CLOSE_WAIT, NULL, &tcp_ack, &tcp_ack, &tcp_send_rst, &tcp_closewait, &tcp_closewait, &tcp_rst }, + { PICO_SOCKET_STATE_TCP_LAST_ACK, NULL, &tcp_ack, &tcp_lastackwait, &tcp_send_rst, &tcp_send_rst, &tcp_send_rst, &tcp_rst }, + { PICO_SOCKET_STATE_TCP_FIN_WAIT1, NULL, &tcp_ack, &tcp_finwaitack, &tcp_data_in, &tcp_rcvfin, &tcp_finack, &tcp_rst }, + { PICO_SOCKET_STATE_TCP_FIN_WAIT2, NULL, &tcp_ack, &tcp_ack, &tcp_data_in, &tcp_finwaitfin, &tcp_finack, &tcp_rst }, + { PICO_SOCKET_STATE_TCP_CLOSING, NULL, &tcp_ack, &tcp_closing_ack, &tcp_send_rst, &tcp_send_rst, &tcp_send_rst, &tcp_rst }, + { PICO_SOCKET_STATE_TCP_TIME_WAIT, NULL, NULL, NULL, &tcp_send_rst, NULL, NULL, NULL} +}; + +#define MAX_VALID_FLAGS 10 /* Maximum number of valid flag combinations */ +static uint8_t invalid_flags(struct pico_socket *s, uint8_t flags) +{ + uint8_t i; + static const uint8_t valid_flags[PICO_SOCKET_STATE_TCP_ARRAYSIZ][MAX_VALID_FLAGS] = { + { /* PICO_SOCKET_STATE_TCP_UNDEF */ 0, }, + { /* PICO_SOCKET_STATE_TCP_CLOSED */ 0, }, + { /* PICO_SOCKET_STATE_TCP_LISTEN */ PICO_TCP_SYN }, + { /* PICO_SOCKET_STATE_TCP_SYN_SENT */ PICO_TCP_SYNACK, PICO_TCP_RST, PICO_TCP_RSTACK}, + { /* PICO_SOCKET_STATE_TCP_SYN_RECV */ PICO_TCP_SYN, PICO_TCP_ACK, PICO_TCP_PSH, PICO_TCP_PSHACK, PICO_TCP_FINACK, PICO_TCP_FINPSHACK, PICO_TCP_RST}, + { /* PICO_SOCKET_STATE_TCP_ESTABLISHED*/ PICO_TCP_SYN, PICO_TCP_SYNACK, PICO_TCP_ACK, PICO_TCP_PSH, PICO_TCP_PSHACK, PICO_TCP_FIN, PICO_TCP_FINACK, PICO_TCP_FINPSHACK, PICO_TCP_RST, PICO_TCP_RSTACK}, + { /* PICO_SOCKET_STATE_TCP_CLOSE_WAIT */ PICO_TCP_SYNACK, PICO_TCP_ACK, PICO_TCP_PSH, PICO_TCP_PSHACK, PICO_TCP_FIN, PICO_TCP_FINACK, PICO_TCP_FINPSHACK, PICO_TCP_RST}, + { /* PICO_SOCKET_STATE_TCP_LAST_ACK */ PICO_TCP_SYNACK, PICO_TCP_ACK, PICO_TCP_PSH, PICO_TCP_PSHACK, PICO_TCP_FIN, PICO_TCP_FINACK, PICO_TCP_FINPSHACK, PICO_TCP_RST}, + { /* PICO_SOCKET_STATE_TCP_FIN_WAIT1 */ PICO_TCP_SYNACK, PICO_TCP_ACK, PICO_TCP_PSH, PICO_TCP_PSHACK, PICO_TCP_FIN, PICO_TCP_FINACK, PICO_TCP_FINPSHACK, PICO_TCP_RST}, + { /* PICO_SOCKET_STATE_TCP_FIN_WAIT2 */ PICO_TCP_SYNACK, PICO_TCP_ACK, PICO_TCP_PSH, PICO_TCP_PSHACK, PICO_TCP_FIN, PICO_TCP_FINACK, PICO_TCP_FINPSHACK, PICO_TCP_RST}, + { /* PICO_SOCKET_STATE_TCP_CLOSING */ PICO_TCP_SYNACK, PICO_TCP_ACK, PICO_TCP_PSH, PICO_TCP_PSHACK, PICO_TCP_FIN, PICO_TCP_FINACK, PICO_TCP_FINPSHACK, PICO_TCP_RST}, + { /* PICO_SOCKET_STATE_TCP_TIME_WAIT */ PICO_TCP_SYNACK, PICO_TCP_ACK, PICO_TCP_PSH, PICO_TCP_PSHACK, PICO_TCP_FIN, PICO_TCP_FINACK, PICO_TCP_FINPSHACK, PICO_TCP_RST}, + }; + if(!flags) + return 1; + + for(i = 0; i < MAX_VALID_FLAGS; i++) { + if(valid_flags[s->state >> 8u][i] == flags) + return 0; + } + return 1; +} + +static void tcp_action_call(int (*call)(struct pico_socket *s, struct pico_frame *f), struct pico_socket *s, struct pico_frame *f ) +{ + if (call) + call(s, f); +} + +static int tcp_action_by_flags(const struct tcp_action_entry *action, struct pico_socket *s, struct pico_frame *f, uint8_t flags) +{ + int ret = 0; + + if ((flags == PICO_TCP_ACK) || (flags == (PICO_TCP_ACK | PICO_TCP_PSH))) { + tcp_action_call(action->ack, s, f); + } + + if ((f->payload_len > 0 || (flags & PICO_TCP_PSH)) && + !(s->state & PICO_SOCKET_STATE_CLOSED) && !TCP_IS_STATE(s, PICO_SOCKET_STATE_TCP_LISTEN)) + { + ret = f->payload_len; + tcp_action_call(action->data, s, f); + } + + if (flags == PICO_TCP_FIN) { + tcp_action_call(action->fin, s, f); + } + + if ((flags == (PICO_TCP_FIN | PICO_TCP_ACK)) || (flags == (PICO_TCP_FIN | PICO_TCP_ACK | PICO_TCP_PSH))) { + tcp_action_call(action->finack, s, f); + } + + if (flags & PICO_TCP_RST) { + tcp_action_call(action->rst, s, f); + } + + return ret; +} + +int pico_tcp_input(struct pico_socket *s, struct pico_frame *f) +{ + struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *) (f->transport_hdr); + int ret = 0; + uint8_t flags = hdr->flags; + const struct tcp_action_entry *action = &tcp_fsm[s->state >> 8]; + + f->payload = (f->transport_hdr + ((hdr->len & 0xf0u) >> 2u)); + f->payload_len = (uint16_t)(f->transport_len - ((hdr->len & 0xf0u) >> 2u)); + + tcp_dbg("[sam] TCP> [tcp input] t_len: %u\n", f->transport_len); + tcp_dbg("[sam] TCP> flags = 0x%02x\n", hdr->flags); + tcp_dbg("[sam] TCP> s->state >> 8 = %u\n", s->state >> 8); + tcp_dbg("[sam] TCP> [tcp input] socket: %p state: %d <-- local port:%u remote port: %u seq: 0x%08x ack: 0x%08x flags: 0x%02x t_len: %u, hdr: %u payload: %d\n", s, s->state >> 8, short_be(hdr->trans.dport), short_be(hdr->trans.sport), SEQN(f), ACKN(f), hdr->flags, f->transport_len, (hdr->len & 0xf0) >> 2, f->payload_len ); + + /* This copy of the frame has the current socket as owner */ + f->sock = s; + s->timestamp = TCP_TIME; + /* Those are not supported at this time. */ + /* flags &= (uint8_t) ~(PICO_TCP_CWR | PICO_TCP_URG | PICO_TCP_ECN); */ + if(invalid_flags(s, flags)) { + pico_tcp_reply_rst(f); + } + else if (flags == PICO_TCP_SYN) { + tcp_action_call(action->syn, s, f); + } else if (flags == (PICO_TCP_SYN | PICO_TCP_ACK)) { + tcp_action_call(action->synack, s, f); + } else { + ret = tcp_action_by_flags(action, s, f, flags); + } + + if (s->ev_pending) + tcp_wakeup_pending(s, s->ev_pending); + +/* discard: */ + pico_frame_discard(f); + return ret; +} + + +inline static int checkLocalClosing(struct pico_socket *s); +inline static int checkRemoteClosing(struct pico_socket *s); + +static struct pico_frame *tcp_split_segment(struct pico_socket_tcp *t, struct pico_frame *f, uint16_t size) +{ + struct pico_frame *f1, *f2; + uint16_t size1, size2, size_f; + uint16_t overhead; + struct pico_tcp_hdr *hdr1, *hdr2, *hdr = (struct pico_tcp_hdr *)f->transport_hdr; + overhead = pico_tcp_overhead(&t->sock); + size_f = f->payload_len; + + + if (size >= size_f) + return f; /* no need to split! */ + + size1 = size; + size2 = (uint16_t)(size_f - size); + + f1 = pico_socket_frame_alloc(&t->sock, (uint16_t) (size1 + overhead)); + f2 = pico_socket_frame_alloc(&t->sock, (uint16_t) (size2 + overhead)); + + if (!f1 || !f2) { + pico_err = PICO_ERR_ENOMEM; + return NULL; + } + + /* Advance payload pointer to the beginning of segment data */ + f1->payload += overhead; + f1->payload_len = (uint16_t)(f1->payload_len - overhead); + f2->payload += overhead; + f2->payload_len = (uint16_t)(f2->payload_len - overhead); + + hdr1 = (struct pico_tcp_hdr *)f1->transport_hdr; + hdr2 = (struct pico_tcp_hdr *)f2->transport_hdr; + + /* Copy payload */ + memcpy(f1->payload, f->payload, size1); + memcpy(f2->payload, f->payload + size1, size2); + + /* Copy tcp hdr */ + memcpy(hdr1, hdr, sizeof(struct pico_tcp_hdr)); + memcpy(hdr2, hdr, sizeof(struct pico_tcp_hdr)); + + /* Adjust f2's sequence number */ + hdr2->seq = long_be(SEQN(f) + size1); + + /* Add TCP options */ + pico_tcp_flags_update(f1, &t->sock); + pico_tcp_flags_update(f2, &t->sock); + tcp_add_options_frame(t, f1); + tcp_add_options_frame(t, f2); + + /* Get rid of the full frame */ + pico_discard_segment(&t->tcpq_out, f); + + /* Enqueue f2 for later send... */ + pico_enqueue_segment(&t->tcpq_out, f2); + + /* Return the partial frame */ + return f1; +} + + +int pico_tcp_output(struct pico_socket *s, int loop_score) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)s; + struct pico_frame *f, *una; + int sent = 0; + int data_sent = 0; + int32_t seq_diff = 0; + + una = first_segment(&t->tcpq_out); + f = peek_segment(&t->tcpq_out, t->snd_nxt); + + while((f) && (t->cwnd >= t->in_flight)) { + f->timestamp = TCP_TIME; + add_retransmission_timer(t, t->rto + TCP_TIME); + tcp_add_options_frame(t, f); + seq_diff = pico_seq_compare(SEQN(f), SEQN(una)); + if (seq_diff < 0) { + tcp_dbg(">>> FATAL: seq diff is negative!\n"); + break; + } + + /* Check if advertised window is full */ + if ((uint32_t)seq_diff >= (uint32_t)(t->recv_wnd << t->recv_wnd_scale)) { + if (t->x_mode != PICO_TCP_WINDOW_FULL) { + tcp_dbg("TCP> RIGHT SIZING (rwnd: %d, frame len: %d\n", t->recv_wnd << t->recv_wnd_scale, f->payload_len); + tcp_dbg("In window full...\n"); + t->snd_nxt = SEQN(una); + t->snd_retry = SEQN(una); + t->x_mode = PICO_TCP_WINDOW_FULL; + } + + break; + } + + /* Check if the advertised window is too small to receive the current frame */ + if ((uint32_t)(seq_diff + f->payload_len) > (uint32_t)(t->recv_wnd << t->recv_wnd_scale)) { + f = tcp_split_segment(t, f, (uint16_t)(t->recv_wnd << t->recv_wnd_scale)); + if (!f) + break; + + /* Limit sending window to packets in flight (right sizing) */ + t->cwnd = (uint16_t)t->in_flight; + if (t->cwnd < 1) + t->cwnd = 1; + } + + tcp_dbg("TCP> DEQUEUED (for output) frame %08x, acks %08x len= %d, remaining frames %d\n", SEQN(f), ACKN(f), f->payload_len, t->tcpq_out.frames); + tcp_send(t, f); + sent++; + loop_score--; + t->snd_last_out = SEQN(f); + if (loop_score < 1) + break; + + if (f->payload_len > 0) { + data_sent++; + f = next_segment(&t->tcpq_out, f); + } else { + f = NULL; + } + } + if ((sent > 0 && data_sent > 0)) { + rto_set(t, t->rto); + } else { + /* Nothing to transmit. */ + } + + if ((t->tcpq_out.frames == 0) && (s->state & PICO_SOCKET_STATE_SHUT_LOCAL)) { /* if no more packets in queue, XXX replaced !f by tcpq check */ + if(!checkLocalClosing(&t->sock)) /* check if local closing started and send fin */ + { + checkRemoteClosing(&t->sock); /* check if remote closing started and send fin */ + } + } + + return loop_score; +} + +/* function to make new segment from hold queue with specific size (mss) */ +static struct pico_frame *pico_hold_segment_make(struct pico_socket_tcp *t) +{ + struct pico_frame *f_temp, *f_new; + struct pico_socket *s = (struct pico_socket *) &t->sock; + struct pico_tcp_hdr *hdr; + uint16_t total_len = 0, total_payload_len = 0; + uint16_t off = 0, test = 0; + + off = pico_tcp_overhead(s); + + /* init with first frame in hold queue */ + f_temp = first_segment(&t->tcpq_hold); + total_len = f_temp->payload_len; + f_temp = next_segment(&t->tcpq_hold, f_temp); + + /* check till total_len <= MSS */ + while ((f_temp != NULL) && ((total_len + f_temp->payload_len) <= t->mss)) { + total_len = (uint16_t)(total_len + f_temp->payload_len); + f_temp = next_segment(&t->tcpq_hold, f_temp); + if (f_temp == NULL) + break; + } + /* alloc new frame with payload size = off + total_len */ + f_new = pico_socket_frame_alloc(s, (uint16_t)(off + total_len)); + if (!f_new) { + pico_err = PICO_ERR_ENOMEM; + return f_new; + } + + pico_tcp_flags_update(f_new, &t->sock); + hdr = (struct pico_tcp_hdr *) f_new->transport_hdr; + /* init new frame */ + f_new->payload += off; + f_new->payload_len = (uint16_t)(f_new->payload_len - off); + f_new->sock = s; + + f_temp = first_segment(&t->tcpq_hold); + hdr->seq = ((struct pico_tcp_hdr *)(f_temp->transport_hdr))->seq; /* get sequence number of first frame */ + hdr->trans.sport = t->sock.local_port; + hdr->trans.dport = t->sock.remote_port; + + /* check till total_payload_len <= MSS */ + while ((f_temp != NULL) && ((total_payload_len + f_temp->payload_len) <= t->mss)) { + /* cpy data and discard frame */ + test++; + memcpy(f_new->payload + total_payload_len, f_temp->payload, f_temp->payload_len); + total_payload_len = (uint16_t)(total_payload_len + f_temp->payload_len); + pico_discard_segment(&t->tcpq_hold, f_temp); + f_temp = first_segment(&t->tcpq_hold); + } + hdr->len = (uint8_t)((f_new->payload - f_new->transport_hdr) << 2u | (int8_t)t->jumbo); + + tcp_dbg_nagle("NAGLE make - joined %d segments, len %d bytes\n", test, total_payload_len); + tcp_add_options_frame(t, f_new); + + return f_new; +} + + + +static int pico_tcp_push_nagle_enqueue(struct pico_socket_tcp *t, struct pico_frame *f) +{ + if (pico_enqueue_segment(&t->tcpq_out, f) > 0) { + tcp_dbg_nagle("TCP_PUSH - NAGLE - Pushing segment %08x, len %08x to socket %p\n", t->snd_last + 1, f->payload_len, t); + t->snd_last += f->payload_len; + return f->payload_len; + } else { + tcp_dbg("Enqueue failed.\n"); + return 0; + } +} + +static int pico_tcp_push_nagle_hold(struct pico_socket_tcp *t, struct pico_frame *f) +{ + struct pico_frame *f_new; + uint32_t total_len = 0; + total_len = f->payload_len + t->tcpq_hold.size; + if ((total_len >= t->mss) && ((t->tcpq_out.max_size - t->tcpq_out.size) >= t->mss)) { + /* IF enough data in hold (>mss) AND space in out queue (>mss) */ + /* add current frame in hold and make new segment */ + if (pico_enqueue_segment(&t->tcpq_hold, f) > 0 ) { + tcp_dbg_nagle("TCP_PUSH - NAGLE - Pushed into hold, make new (enqueued frames out %d)\n", t->tcpq_out.frames); + t->snd_last += f->payload_len; /* XXX WATCH OUT */ + f_new = pico_hold_segment_make(t); + } else { + tcp_dbg_nagle("TCP_PUSH - NAGLE - enqueue hold failed 1\n"); + return 0; + } + + /* and put new frame in out queue */ + if ((f_new != NULL) && (pico_enqueue_segment(&t->tcpq_out, f_new) > 0)) { + return f_new->payload_len; + } else { + tcp_dbg_nagle("TCP_PUSH - NAGLE - enqueue out failed, f_new = %p\n", f_new); + return -1; /* XXX something seriously wrong */ + } + } else { + /* ELSE put frame in hold queue */ + if (pico_enqueue_segment(&t->tcpq_hold, f) > 0) { + tcp_dbg_nagle("TCP_PUSH - NAGLE - Pushed into hold (enqueued frames out %d)\n", t->tcpq_out.frames); + t->snd_last += f->payload_len; /* XXX WATCH OUT */ + return f->payload_len; + } else { + pico_err = PICO_ERR_EAGAIN; + tcp_dbg_nagle("TCP_PUSH - NAGLE - enqueue hold failed 2\n"); + } + } + + return 0; +} + + +static int pico_tcp_push_nagle_on(struct pico_socket_tcp *t, struct pico_frame *f) +{ + /* Nagle's algorithm enabled, check if ready to send, or put frame in hold queue */ + if (IS_TCP_IDLE(t) && IS_TCP_HOLDQ_EMPTY(t)) + return pico_tcp_push_nagle_enqueue(t, f); + + return pico_tcp_push_nagle_hold(t, f); +} + + + +/* original behavior kept when Nagle disabled; + Nagle algorithm added here, keeping hold frame queue instead of eg linked list of data */ +int pico_tcp_push(struct pico_protocol *self, struct pico_frame *f) +{ + struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *)f->transport_hdr; + struct pico_socket_tcp *t = (struct pico_socket_tcp *) f->sock; + IGNORE_PARAMETER(self); + pico_err = PICO_ERR_NOERR; + hdr->trans.sport = t->sock.local_port; + hdr->trans.dport = t->sock.remote_port; + hdr->seq = long_be(t->snd_last + 1); + hdr->len = (uint8_t)((f->payload - f->transport_hdr) << 2u | (int8_t)t->jumbo); + + if ((uint32_t)f->payload_len > (uint32_t)(t->tcpq_out.max_size - t->tcpq_out.size)) + t->sock.ev_pending &= (uint16_t)(~PICO_SOCK_EV_WR); + + /***************************************************************************/ + + if (!IS_NAGLE_ENABLED((&(t->sock)))) { + /* TCP_NODELAY enabled, original behavior */ + if (pico_enqueue_segment(&t->tcpq_out, f) > 0) { + tcp_dbg_nagle("TCP_PUSH - NO NAGLE - Pushing segment %08x, len %08x to socket %p\n", t->snd_last + 1, f->payload_len, t); + t->snd_last += f->payload_len; + return f->payload_len; + } else { + tcp_dbg("Enqueue failed.\n"); + return 0; + } + } else { + return pico_tcp_push_nagle_on(t, f); + } + +} + +inline static void tcp_discard_all_segments(struct pico_tcp_queue *tq) +{ + struct pico_tree_node *index = NULL, *index_safe = NULL; + PICOTCP_MUTEX_LOCK(Mutex); + pico_tree_foreach_safe(index, &tq->pool, index_safe) + { + void *f = index->keyValue; + if(!f) + break; + + pico_tree_delete(&tq->pool, f); + if(IS_INPUT_QUEUE(tq)) + { + struct tcp_input_segment *inp = (struct tcp_input_segment *)f; + PICO_FREE(inp->payload); + PICO_FREE(inp); + } + else + pico_frame_discard(f); + } + tq->frames = 0; + tq->size = 0; + PICOTCP_MUTEX_UNLOCK(Mutex); +} + +void pico_tcp_cleanup_queues(struct pico_socket *sck) +{ + struct pico_socket_tcp *tcp = (struct pico_socket_tcp *)sck; + pico_timer_cancel(tcp->retrans_tmr); + pico_timer_cancel(tcp->keepalive_tmr); + pico_timer_cancel(tcp->fin_tmr); + + tcp->retrans_tmr = 0; + tcp->keepalive_tmr = 0; + tcp->fin_tmr = 0; + + tcp_discard_all_segments(&tcp->tcpq_in); + tcp_discard_all_segments(&tcp->tcpq_out); + tcp_discard_all_segments(&tcp->tcpq_hold); +} + +static int checkLocalClosing(struct pico_socket *s) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)s; + if ((s->state & PICO_SOCKET_STATE_TCP) == PICO_SOCKET_STATE_TCP_ESTABLISHED) { + tcp_dbg("TCP> buffer empty, shutdown established ...\n"); + /* send fin if queue empty and in state shut local (write) */ + tcp_send_fin(t); + /* change tcp state to FIN_WAIT1 */ + s->state &= 0x00FFU; + s->state |= PICO_SOCKET_STATE_TCP_FIN_WAIT1; + return 1; + } + + return 0; +} + +static int checkRemoteClosing(struct pico_socket *s) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)s; + if ((s->state & PICO_SOCKET_STATE_TCP) == PICO_SOCKET_STATE_TCP_CLOSE_WAIT) { + /* send fin if queue empty and in state shut local (write) */ + tcp_send_fin(t); + /* change tcp state to LAST_ACK */ + s->state &= 0x00FFU; + s->state |= PICO_SOCKET_STATE_TCP_LAST_ACK; + tcp_dbg("TCP> STATE: LAST_ACK.\n"); + return 1; + } + + return 0; +} + +void pico_tcp_notify_closing(struct pico_socket *sck) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)sck; + if(t->tcpq_out.frames == 0) + { + if(!checkLocalClosing(sck)) + checkRemoteClosing(sck); + } +} + + +int pico_tcp_check_listen_close(struct pico_socket *s) +{ + if (TCP_IS_STATE(s, PICO_SOCKET_STATE_TCP_LISTEN)) { + pico_socket_del(s); + return 0; + } + + return -1; +} + +void pico_tcp_flags_update(struct pico_frame *f, struct pico_socket *s) +{ + f->transport_flags_saved = ((struct pico_socket_tcp *)s)->ts_ok; +} + +int pico_tcp_set_bufsize_in(struct pico_socket *s, uint32_t value) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)s; + t->tcpq_in.max_size = value; + return 0; +} + +int pico_tcp_set_bufsize_out(struct pico_socket *s, uint32_t value) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)s; + t->tcpq_out.max_size = value; + return 0; +} + +int pico_tcp_get_bufsize_in(struct pico_socket *s, uint32_t *value) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)s; + *value = t->tcpq_in.max_size; + return 0; +} + +int pico_tcp_get_bufsize_out(struct pico_socket *s, uint32_t *value) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)s; + *value = t->tcpq_out.max_size; + return 0; +} + +int pico_tcp_set_keepalive_probes(struct pico_socket *s, uint32_t value) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)s; + t->ka_probes = value; + return 0; +} + +int pico_tcp_set_keepalive_intvl(struct pico_socket *s, uint32_t value) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)s; + t->ka_intvl = value; + return 0; +} + +int pico_tcp_set_keepalive_time(struct pico_socket *s, uint32_t value) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)s; + t->ka_time = value; + return 0; +} + +int pico_tcp_set_linger(struct pico_socket *s, uint32_t value) +{ + struct pico_socket_tcp *t = (struct pico_socket_tcp *)s; + t->linger_timeout = value; + return 0; +} + +#endif /* PICO_SUPPORT_TCP */ diff --git a/ext/picotcp/modules/pico_tcp.h b/ext/picotcp/modules/pico_tcp.h new file mode 100644 index 0000000..ece38bb --- /dev/null +++ b/ext/picotcp/modules/pico_tcp.h @@ -0,0 +1,106 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + *********************************************************************/ +#ifndef INCLUDE_PICO_TCP +#define INCLUDE_PICO_TCP +#include "pico_addressing.h" +#include "pico_protocol.h" +#include "pico_socket.h" + +extern struct pico_protocol pico_proto_tcp; + +PACKED_STRUCT_DEF pico_tcp_hdr { + struct pico_trans trans; + uint32_t seq; + uint32_t ack; + uint8_t len; + uint8_t flags; + uint16_t rwnd; + uint16_t crc; + uint16_t urgent; +}; + +PACKED_STRUCT_DEF tcp_pseudo_hdr_ipv4 +{ + struct pico_ip4 src; + struct pico_ip4 dst; + uint16_t tcp_len; + uint8_t res; + uint8_t proto; +}; + +#define PICO_TCPHDR_SIZE 20 +#define PICO_SIZE_TCPOPT_SYN 20 +#define PICO_SIZE_TCPHDR (uint32_t)(sizeof(struct pico_tcp_hdr)) + +/* TCP options */ +#define PICO_TCP_OPTION_END 0x00 +#define PICO_TCPOPTLEN_END 1u +#define PICO_TCP_OPTION_NOOP 0x01 +#define PICO_TCPOPTLEN_NOOP 1 +#define PICO_TCP_OPTION_MSS 0x02 +#define PICO_TCPOPTLEN_MSS 4 +#define PICO_TCP_OPTION_WS 0x03 +#define PICO_TCPOPTLEN_WS 3u +#define PICO_TCP_OPTION_SACK_OK 0x04 +#define PICO_TCPOPTLEN_SACK_OK 2 +#define PICO_TCP_OPTION_SACK 0x05 +#define PICO_TCPOPTLEN_SACK 2 /* Plus the block */ +#define PICO_TCP_OPTION_TIMESTAMP 0x08 +#define PICO_TCPOPTLEN_TIMESTAMP 10u + +/* TCP flags */ +#define PICO_TCP_FIN 0x01u +#define PICO_TCP_SYN 0x02u +#define PICO_TCP_RST 0x04u +#define PICO_TCP_PSH 0x08u +#define PICO_TCP_ACK 0x10u +#define PICO_TCP_URG 0x20u +#define PICO_TCP_ECN 0x40u +#define PICO_TCP_CWR 0x80u + +#define PICO_TCP_SYNACK (PICO_TCP_SYN | PICO_TCP_ACK) +#define PICO_TCP_PSHACK (PICO_TCP_PSH | PICO_TCP_ACK) +#define PICO_TCP_FINACK (PICO_TCP_FIN | PICO_TCP_ACK) +#define PICO_TCP_FINPSHACK (PICO_TCP_FIN | PICO_TCP_PSH | PICO_TCP_ACK) +#define PICO_TCP_RSTACK (PICO_TCP_RST | PICO_TCP_ACK) + + +PACKED_STRUCT_DEF pico_tcp_option +{ + uint8_t kind; + uint8_t len; +}; + +struct pico_socket *pico_tcp_open(uint16_t family); +uint32_t pico_tcp_read(struct pico_socket *s, void *buf, uint32_t len); +int pico_tcp_initconn(struct pico_socket *s); +int pico_tcp_input(struct pico_socket *s, struct pico_frame *f); +uint16_t pico_tcp_checksum(struct pico_frame *f); +uint16_t pico_tcp_checksum_ipv4(struct pico_frame *f); +#ifdef PICO_SUPPORT_IPV6 +uint16_t pico_tcp_checksum_ipv6(struct pico_frame *f); +#endif +uint16_t pico_tcp_overhead(struct pico_socket *s); +int pico_tcp_output(struct pico_socket *s, int loop_score); +int pico_tcp_queue_in_is_empty(struct pico_socket *s); +int pico_tcp_reply_rst(struct pico_frame *f); +void pico_tcp_cleanup_queues(struct pico_socket *sck); +void pico_tcp_notify_closing(struct pico_socket *sck); +void pico_tcp_flags_update(struct pico_frame *f, struct pico_socket *s); +int pico_tcp_set_bufsize_in(struct pico_socket *s, uint32_t value); +int pico_tcp_set_bufsize_out(struct pico_socket *s, uint32_t value); +int pico_tcp_get_bufsize_in(struct pico_socket *s, uint32_t *value); +int pico_tcp_get_bufsize_out(struct pico_socket *s, uint32_t *value); +int pico_tcp_set_keepalive_probes(struct pico_socket *s, uint32_t value); +int pico_tcp_set_keepalive_intvl(struct pico_socket *s, uint32_t value); +int pico_tcp_set_keepalive_time(struct pico_socket *s, uint32_t value); +int pico_tcp_set_linger(struct pico_socket *s, uint32_t value); +uint16_t pico_tcp_get_socket_mss(struct pico_socket *s); +int pico_tcp_check_listen_close(struct pico_socket *s); + +#endif diff --git a/ext/picotcp/modules/pico_tftp.c b/ext/picotcp/modules/pico_tftp.c new file mode 100644 index 0000000..83c5c6f --- /dev/null +++ b/ext/picotcp/modules/pico_tftp.c @@ -0,0 +1,1300 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + Author: Daniele Lacamera + *********************************************************************/ + +#include +#include +#include +#include +#include + +/* a zero value means adaptative timeout! (2, 4, 8) */ +#define PICO_TFTP_TIMEOUT 2000U + +#define TFTP_MAX_RETRY 3 + +#define TFTP_STATE_READ_REQUESTED 0 +#define TFTP_STATE_RX 1 +#define TFTP_STATE_LAST_ACK_SENT 2 +#define TFTP_STATE_WRITE_REQUESTED 3 +#define TFTP_STATE_TX 4 +#define TFTP_STATE_WAIT_OPT_CONFIRM 5 +#define TFTP_STATE_WAIT_LAST_ACK 6 +#define TFTP_STATE_CLOSING 7 + +#define AUTOMA_STATES (TFTP_STATE_CLOSING + 1) + +/* MAX_OPTIONS_SIZE: "timeout" 255 "tsize" filesize => 8 + 4 + 6 + 11 */ +#define MAX_OPTIONS_SIZE 29 + +/* RRQ and WRQ packets (opcodes 1 and 2 respectively) */ +PACKED_STRUCT_DEF pico_tftp_hdr +{ + uint16_t opcode; +}; + +/* DATA or ACK (opcodes 3 and 4 respectively)*/ +PACKED_STRUCT_DEF pico_tftp_data_hdr +{ + uint16_t opcode; + uint16_t block; +}; + +/* ERROR (opcode 5) */ +PACKED_STRUCT_DEF pico_tftp_err_hdr +{ + uint16_t opcode; + uint16_t error_code; +}; + +#define PICO_TFTP_TOTAL_BLOCK_SIZE (PICO_TFTP_PAYLOAD_SIZE + (int32_t)sizeof(struct pico_tftp_data_hdr)) +#define tftp_payload(p) (((uint8_t *)(p)) + sizeof(struct pico_tftp_data_hdr)) + +/* STATUS FLAGS */ +#define SESSION_STATUS_CLOSED 1 +#define SESSION_STATUS_APP_PENDING 2 +#define SESSION_STATUS_IN_CALLBACK 4 +#define SESSION_STATUS_APP_ACK 64 + +struct pico_tftp_session { + int state; + int status; + int options; + int retry; + uint16_t packet_counter; + /* Current connection */ + struct pico_socket *socket; + union pico_address remote_address; + uint16_t remote_port; + uint16_t localport; + pico_time wallclock_timeout; + pico_time bigger_wallclock; + struct pico_tftp_session *next; + uint32_t timer; + unsigned int active_timers; + void *argument; + int (*callback)(struct pico_tftp_session *session, uint16_t event, uint8_t *block, int32_t len, void *arg); + int32_t file_size; + int32_t len; + uint8_t option_timeout; + uint8_t tftp_block[PICO_TFTP_TOTAL_BLOCK_SIZE]; + int32_t block_len; +}; + +struct server_t { + void (*listen_callback)(union pico_address *addr, uint16_t port, uint16_t opcode, char *filename, int32_t len); + struct pico_socket *listen_socket; + uint8_t tftp_block[PICO_TFTP_TOTAL_BLOCK_SIZE]; +}; + +struct automa_events { + void (*ack)(struct pico_tftp_session *session, int32_t len, union pico_address *a, uint16_t port); + void (*data)(struct pico_tftp_session *session, int32_t len, union pico_address *a, uint16_t port); + void (*error)(struct pico_tftp_session *session, int32_t len, union pico_address *a, uint16_t port); + void (*oack)(struct pico_tftp_session *session, int32_t len, union pico_address *a, uint16_t port); + void (*timeout)(struct pico_tftp_session *session, pico_time t); +}; + +static struct server_t server; + +static struct pico_tftp_session *tftp_sessions = NULL; + +static inline void session_status_set(struct pico_tftp_session *session, int status) +{ + session->status |= status; +} + +static inline void session_status_clear(struct pico_tftp_session *session, int status) +{ + session->status &= ~status; +} + +static char *extract_arg_pointer(char *arg, char *end_arg, char **value) +{ + char *pos; + + pos = get_string_terminator_position(arg, (size_t)(end_arg - arg)); + if (!pos) + return NULL; + + if (end_arg == ++pos) + return NULL; + + arg = get_string_terminator_position(pos, (size_t)(end_arg - pos)); + + if (!arg) + return NULL; + + *value = pos; + return arg + 1; +} + +static int extract_value(char *str, uint32_t *value, uint32_t max) +{ + char *endptr; + unsigned long num; + + num = strtoul(str, &endptr, 10); + + if (endptr == str || *endptr || num > max) + return -1; + + *value = (uint32_t)num; + return 0; +} + +static int parse_optional_arguments(char *option_string, int32_t len, int *options, uint8_t *timeout, int32_t *filesize) +{ + char *pos; + char *end_args = option_string + len; + char *current_option; + int ret; + uint32_t value; + + *options = 0; + + while (option_string < end_args) { + current_option = option_string; + option_string = extract_arg_pointer(option_string, end_args, &pos); + if (!option_string) + return 0; + + if (!pico_strncasecmp("timeout", current_option, (size_t)(pos - current_option))) { + ret = extract_value(pos, &value, PICO_TFTP_MAX_TIMEOUT); + if (ret) + return -1; + + *timeout = (uint8_t)value; + *options |= PICO_TFTP_OPTION_TIME; + } else { + if (!pico_strncasecmp("tsize", current_option, (size_t)(pos - current_option))) { + ret = extract_value(pos, (uint32_t *)filesize, PICO_TFTP_MAX_FILESIZE); + if (ret) + return -1; + + if (*filesize < 0) + return -1; + + *options |= PICO_TFTP_OPTION_FILE; + } + } + } + return 0; +} + +static inline struct pico_tftp_session *pico_tftp_session_create(struct pico_socket *sock, union pico_address *remote_addr) +{ + struct pico_tftp_session *session; + + session = (struct pico_tftp_session *) PICO_ZALLOC(sizeof (struct pico_tftp_session)); + + if (!session) + pico_err = PICO_ERR_ENOMEM; + else { + session->state = 0; + session->status = 0; + session->options = 0; + session->packet_counter = 0u; + session->socket = sock; + session->wallclock_timeout = 0; + session->bigger_wallclock = 0; + session->active_timers = 0; + session->next = NULL; + session->localport = 0; + session->callback = NULL; + session->argument = NULL; + memcpy(&session->remote_address, remote_addr, sizeof(union pico_address)); + session->remote_port = 0; + session->len = 0; + } + + return session; +} + +static struct pico_tftp_session *find_session_by_socket(struct pico_socket *tftp_socket) +{ + struct pico_tftp_session *pos = tftp_sessions; + + for (; pos; pos = pos->next) + if (pos->socket == tftp_socket) + return pos; + + return NULL; +} + +/* **************** for future use... + static struct pico_tftp_session * find_session_by_localport(uint16_t localport) + { + struct pico_tftp_session *idx = tftp_sessions; + + for (; idx; idx = idx->next) + if (idx->localport == localport) + return idx; + + return NULL; + } *********************/ + +static void add_session(struct pico_tftp_session *idx) +{ + struct pico_tftp_session *prev = NULL; + struct pico_tftp_session *pos; + + for (pos = tftp_sessions; pos; prev = pos, pos = pos->next) + if (pos->localport > idx->localport) + break; + + if (prev) { + idx->next = prev->next; + prev->next = idx; + } else { + idx->next = tftp_sessions; + tftp_sessions = idx; + } +} + +/* Returns 0 if OK and -1 in case of errors */ +static int del_session(struct pico_tftp_session *idx) +{ + struct pico_tftp_session *prev = NULL; + struct pico_tftp_session *pos; + + for (pos = tftp_sessions; pos; pos = pos->next) { + if (pos == idx) { + if (pos == tftp_sessions) + tftp_sessions = tftp_sessions->next; + else + prev->next = pos->next; + + PICO_FREE(idx); + return 0; + } + + prev = pos; + } + return -1; +} + +static inline int do_callback(struct pico_tftp_session *session, uint16_t err, uint8_t *data, int32_t len) +{ + int ret; + + session_status_set(session, SESSION_STATUS_IN_CALLBACK); + ret = session->callback(session, err, data, len, session->argument); + session_status_clear(session, SESSION_STATUS_IN_CALLBACK); + + return ret; +} + +static void timer_callback(pico_time now, void *arg); + +static void tftp_schedule_timeout(struct pico_tftp_session *session, pico_time interval) +{ + pico_time new_timeout = PICO_TIME_MS() + interval; + + if (session->active_timers) { + if (session->bigger_wallclock > new_timeout) { + session->timer = pico_timer_add(interval + 1, timer_callback, session); + session->active_timers++; + } + } else { + session->timer = pico_timer_add(interval + 1, timer_callback, session); + session->active_timers++; + session->bigger_wallclock = new_timeout; + } + + session->wallclock_timeout = new_timeout; +} + +static void tftp_finish(struct pico_tftp_session *session) +{ + if (session->state != TFTP_STATE_CLOSING) { + pico_socket_close(session->socket); + session->state = TFTP_STATE_CLOSING; + if (session->active_timers) { + pico_timer_cancel(session->timer); + --session->active_timers; + } + + session->wallclock_timeout = 0; + tftp_schedule_timeout(session, 5); + } +} + +static void tftp_send(struct pico_tftp_session *session, int len) +{ + if (len) + session->len = len; + else + len = session->len; + + pico_socket_sendto(session->socket, session->tftp_block, session->len, &session->remote_address, session->remote_port); +} + +static void tftp_send_ack(struct pico_tftp_session *session) +{ + struct pico_tftp_data_hdr *dh; + + dh = PICO_ZALLOC(sizeof(struct pico_tftp_data_hdr)); + if (!dh) + return; + + dh->opcode = short_be(PICO_TFTP_ACK); + dh->block = short_be(session->packet_counter); + + if (session->socket) { + pico_socket_sendto(session->socket, dh, (int) sizeof(struct pico_tftp_err_hdr), + &session->remote_address, session->remote_port); + tftp_schedule_timeout(session, PICO_TFTP_TIMEOUT); + } + + PICO_FREE(dh); +} + +static size_t prepare_options_string(struct pico_tftp_session *session, char *str_options, int32_t filesize) +{ + size_t len = 0; + int res; + + if (session->options & PICO_TFTP_OPTION_TIME) { + strcpy(str_options, "timeout"); + len += 8; + res = num2string(session->option_timeout, &str_options[len], 4); + if (res < 0) + return 0; + + len += (size_t)res; + } + + if (session->options & PICO_TFTP_OPTION_FILE) { + strcpy(&str_options[len], "tsize"); + len += 6; + res = num2string(filesize, &str_options[len], 11); + if (res < 0) + return 0; + + len += (size_t)res; + } + + return len; +} + +static void tftp_send_oack(struct pico_tftp_session *session) +{ + struct pico_tftp_hdr *hdr; + size_t options_size; + size_t options_pos = sizeof(struct pico_tftp_hdr); + uint8_t *buf; + char str_options[MAX_OPTIONS_SIZE] = { 0 }; + + options_size = prepare_options_string(session, str_options, session->file_size); + + buf = PICO_ZALLOC(options_pos + options_size); + if (!buf) { + strcpy((char *)session->tftp_block, "Out of memory"); + do_callback(session, PICO_TFTP_EV_ERR_LOCAL, session->tftp_block, 0); + tftp_finish(session); + return; + } + + hdr = (struct pico_tftp_hdr *)buf; + hdr->opcode = short_be(PICO_TFTP_OACK); + memcpy(buf + options_pos, str_options, options_size); + (void)pico_socket_sendto(session->socket, buf, (int)(options_pos + options_size), &session->remote_address, session->remote_port); + PICO_FREE(buf); +} + +static void tftp_send_req(struct pico_tftp_session *session, union pico_address *a, uint16_t port, const char *filename, uint16_t opcode) +{ +#define OCTET_STRSIZ 7U + static const char octet[OCTET_STRSIZ] = { + 0, 'o', 'c', 't', 'e', 't', 0 + }; + struct pico_tftp_hdr *hdr; + size_t len; + size_t options_size; + size_t options_pos; + uint8_t *buf; + char str_options[MAX_OPTIONS_SIZE] = { 0 }; + + if (!filename) { + return; + } + + len = strlen(filename); + + options_size = prepare_options_string(session, str_options, (opcode == PICO_TFTP_WRQ) ? (session->file_size) : (0)); + + options_pos = sizeof(struct pico_tftp_hdr) + OCTET_STRSIZ + len; + buf = PICO_ZALLOC(options_pos + options_size); + if (!buf) { + strcpy((char *)session->tftp_block, "Out of memory"); + do_callback(session, PICO_TFTP_EV_ERR_LOCAL, session->tftp_block, 0); + tftp_finish(session); + return; + } + + hdr = (struct pico_tftp_hdr *)buf; + hdr->opcode = short_be(opcode); + memcpy(buf + sizeof(struct pico_tftp_hdr), filename, len); + memcpy(buf + sizeof(struct pico_tftp_hdr) + len, octet, OCTET_STRSIZ); + memcpy(buf + options_pos, str_options, options_size); + (void)pico_socket_sendto(session->socket, buf, (int)(options_pos + options_size), a, port); + PICO_FREE(buf); +} + +static void tftp_send_rx_req(struct pico_tftp_session *session, union pico_address *a, uint16_t port, const char *filename) +{ + tftp_send_req(session, a, port, filename, PICO_TFTP_RRQ); + session->state = TFTP_STATE_READ_REQUESTED; + tftp_schedule_timeout(session, PICO_TFTP_TIMEOUT); +} + +static void tftp_send_tx_req(struct pico_tftp_session *session, union pico_address *a, uint16_t port, const char *filename) +{ + tftp_send_req(session, a, port, filename, PICO_TFTP_WRQ); + session->state = TFTP_STATE_WRITE_REQUESTED; + tftp_schedule_timeout(session, PICO_TFTP_TIMEOUT); +} + +static int send_error(uint8_t *buf, struct pico_socket *sock, union pico_address *a, uint16_t port, uint16_t errcode, const char *errmsg) +{ + struct pico_tftp_err_hdr *eh; + int32_t len; + int32_t maxlen = PICO_TFTP_TOTAL_BLOCK_SIZE - sizeof(struct pico_tftp_err_hdr); + + if (!errmsg) + len = 0; + else + len = (int32_t)strlen(errmsg); + + eh = (struct pico_tftp_err_hdr *) buf; + eh->opcode = short_be(PICO_TFTP_ERROR); + eh->error_code = short_be(errcode); + if (len + 1 > maxlen) + len = maxlen; + + if (len) + memcpy(tftp_payload(eh), errmsg, (size_t)len); + + tftp_payload(eh)[len++] = (char)0; + + return pico_socket_sendto(sock, eh, (int)(len + (int32_t)sizeof(struct pico_tftp_err_hdr)), a, port); +} + +static void tftp_send_error(struct pico_tftp_session *session, union pico_address *a, uint16_t port, uint16_t errcode, const char *errmsg) +{ + struct pico_tftp_err_hdr *eh; + int32_t len; + int32_t maxlen = PICO_TFTP_TOTAL_BLOCK_SIZE - sizeof(struct pico_tftp_err_hdr); + + if (!errmsg) + len = 0; + else + len = (int32_t)strlen(errmsg); + + if (!a) { + a = &session->remote_address; + port = session->remote_port; + } + + eh = (struct pico_tftp_err_hdr *) (session ? (session->tftp_block) : (server.tftp_block)); + eh->opcode = short_be(PICO_TFTP_ERROR); + eh->error_code = short_be(errcode); + if (len + 1 > maxlen) + len = maxlen; + + if (len) + memcpy(tftp_payload(eh), errmsg, (size_t)len); + + tftp_payload(eh)[len++] = (char)0; + if (session) { + (void)pico_socket_sendto(session->socket, eh, (int) (len + (int32_t)sizeof(struct pico_tftp_err_hdr)), a, port); + tftp_finish(session); + } else + (void)pico_socket_sendto(server.listen_socket, eh, (int) (len + (int32_t)sizeof(struct pico_tftp_err_hdr)), a, port); +} + +static void tftp_send_data(struct pico_tftp_session *session, const uint8_t *data, int32_t len) +{ + struct pico_tftp_data_hdr *dh; + + dh = (struct pico_tftp_data_hdr *) session->tftp_block; + dh->opcode = short_be(PICO_TFTP_DATA); + dh->block = short_be(session->packet_counter++); + + if (len < PICO_TFTP_PAYLOAD_SIZE) + session->state = TFTP_STATE_WAIT_LAST_ACK; + else + session->state = TFTP_STATE_TX; + + memcpy(session->tftp_block + sizeof(struct pico_tftp_data_hdr), data, (size_t)len); + pico_socket_sendto(session->socket, session->tftp_block, (int)(len + (int32_t)sizeof(struct pico_tftp_data_hdr)), + &session->remote_address, session->remote_port); + tftp_schedule_timeout(session, PICO_TFTP_TIMEOUT); +} + +static inline void tftp_eval_finish(struct pico_tftp_session *session, int32_t len) +{ + if (len < PICO_TFTP_PAYLOAD_SIZE) { + pico_socket_close(session->socket); + session->state = TFTP_STATE_CLOSING; + } +} + +static inline int tftp_data_prepare(struct pico_tftp_session *session, union pico_address *a, uint16_t port) +{ + if (!session->socket) + return -1; + + if (pico_address_compare(a, &session->remote_address, session->socket->net->proto_number) != 0) { + tftp_send_error(session, a, port, TFTP_ERR_EXCEEDED, "TFTP busy, try again later."); + return -1; + } + + return 0; +} + +static void tftp_req(uint8_t *block, int32_t len, union pico_address *a, uint16_t port) +{ + struct pico_tftp_hdr *hdr = (struct pico_tftp_hdr *)block; + char *filename; + char *pos; + char *mode; + int ret; + + switch (short_be(hdr->opcode)) { + case PICO_TFTP_RRQ: + case PICO_TFTP_WRQ: + filename = (char *)(block + sizeof(struct pico_tftp_hdr)); + len -= (int32_t)sizeof(struct pico_tftp_hdr); + + pos = extract_arg_pointer(filename, filename + len, &mode); + if (!pos) { + send_error(block, server.listen_socket, a, port, TFTP_ERR_EILL, "Invalid argument in request"); + return; + } + + ret = strcmp("octet", mode); + if (ret) { + send_error(block, server.listen_socket, a, port, TFTP_ERR_EILL, "Unsupported mode"); + return; + } + + /*ret = parse_optional_arguments((char *)(block + sizeof(struct pico_tftp_hdr)), len - sizeof(struct pico_tftp_hdr), &new_options, &new_timeout, &new_filesize); + if (ret) { + tftp_send_error(NULL, a, port, TFTP_ERR_EILL, "Bad request"); + return; + } */ + + if (server.listen_callback) { + server.listen_callback(a, port, short_be(hdr->opcode), filename, len); + } + + break; + default: + send_error(block, server.listen_socket, a, port, TFTP_ERR_EILL, "Illegal opcode"); + } +} + +static int event_ack_base(struct pico_tftp_session *session, int32_t len, union pico_address *a, uint16_t port) +{ + struct pico_tftp_data_hdr *dh; + uint16_t block_n; + const char *wrong_address = "Wrong address"; + const char *wrong_block = "Wrong packet number"; + + (void)len; + if (pico_address_compare(a, &session->remote_address, session->socket->net->proto_number) != 0) { + strcpy((char *)session->tftp_block, wrong_address); + do_callback(session, PICO_TFTP_EV_ERR_PEER, session->tftp_block, len); + tftp_send_error(session, a, port, TFTP_ERR_EXCEEDED, wrong_address); + return -1; + } + + dh = (struct pico_tftp_data_hdr *)session->tftp_block; + block_n = short_be(dh->block); + if (block_n != (session->packet_counter - 1U)) { + strcpy((char *)session->tftp_block, wrong_block); + do_callback(session, PICO_TFTP_EV_ERR_PEER, session->tftp_block, len); + tftp_send_error(session, a, port, TFTP_ERR_EILL, wrong_block); + return -1; + } + + return 0; +} + +static inline int event_ack0_check(struct pico_tftp_session *session, int32_t len, union pico_address *a, uint16_t port) +{ + struct pico_tftp_data_hdr *dh; + uint16_t block_n; + + (void)len; + if (pico_address_compare(a, &session->remote_address, session->socket->net->proto_number) != 0) { + tftp_send_error(session, a, port, TFTP_ERR_EXCEEDED, "TFTP busy, try again later."); + return -1; + } + + dh = (struct pico_tftp_data_hdr *)session->tftp_block; + block_n = short_be(dh->block); + if (block_n != 0) { + tftp_send_error(session, a, port, TFTP_ERR_EILL, "TFTP connection broken!"); + return -1; + } + + return 0; +} + +static void event_ack0_wr(struct pico_tftp_session *session, int32_t len, union pico_address *a, uint16_t port) +{ + if (!event_ack0_check(session, len, a, port)) { + session->remote_port = port; + do_callback(session, PICO_TFTP_EV_OK, session->tftp_block, 0); + } +} + +static void event_ack0_woc(struct pico_tftp_session *session, int32_t len, union pico_address *a, uint16_t port) +{ + if (!event_ack0_check(session, len, a, port)) + do_callback(session, PICO_TFTP_EV_OPT, session->tftp_block, 0); +} + +static void event_ack(struct pico_tftp_session *session, int32_t len, union pico_address *a, uint16_t port) +{ + if (!event_ack_base(session, len, a, port)) + do_callback(session, PICO_TFTP_EV_OK, session->tftp_block, 0); +} + +static void event_ack_last(struct pico_tftp_session *session, int32_t len, union pico_address *a, uint16_t port) +{ + if (!event_ack_base(session, len, a, port)) + tftp_finish(session); +} + +static void event_data(struct pico_tftp_session *session, int32_t len, union pico_address *a, uint16_t port) +{ + struct pico_tftp_data_hdr *dh; + int32_t payload_len = len - (int32_t)sizeof(struct pico_tftp_data_hdr); + + if (tftp_data_prepare(session, a, port)) + return; + + dh = (struct pico_tftp_data_hdr *)session->tftp_block; + if (short_be(dh->block) > (session->packet_counter + 1U)) { + strcpy((char *)session->tftp_block, "Wrong/unexpected sequence number"); + do_callback(session, PICO_TFTP_EV_ERR_LOCAL, session->tftp_block, 0); + tftp_send_error(session, a, port, TFTP_ERR_EILL, "TFTP connection broken!"); + return; + } + + if (short_be(dh->block) == (session->packet_counter + 1U)) { + session->packet_counter++; + if (do_callback(session, PICO_TFTP_EV_OK, tftp_payload(session->tftp_block), payload_len) >= 0) { + if (!(session->status & SESSION_STATUS_APP_ACK)) + tftp_send_ack(session); + } + + if (!(session->status & SESSION_STATUS_APP_ACK)) + tftp_eval_finish(session, len); + } +} + +static void event_data_rdr(struct pico_tftp_session *session, int32_t len, union pico_address *a, uint16_t port) +{ + if (tftp_data_prepare(session, a, port)) + return; + + session->remote_port = port; + session->state = TFTP_STATE_RX; + event_data(session, len, a, port); +} + +static void event_data_rpl(struct pico_tftp_session *session, int32_t len, union pico_address *a, uint16_t port) +{ + struct pico_tftp_data_hdr *dh; + + (void)len; + if (tftp_data_prepare(session, a, port)) + return; + + dh = (struct pico_tftp_data_hdr *)session->tftp_block; + + if (short_be(dh->block) == session->packet_counter) + tftp_send_ack(session); +} + +static void event_err(struct pico_tftp_session *session, int32_t len, union pico_address *a, uint16_t port) +{ + (void)a; + (void)port; + do_callback(session, PICO_TFTP_EV_ERR_PEER, session->tftp_block, len); + tftp_finish(session); +} + +static inline void event_oack(struct pico_tftp_session *session, int32_t len, union pico_address *a, uint16_t port) +{ + char *option_string = (char *)session->tftp_block + sizeof(struct pico_tftp_hdr); + int ret; + int proposed_options = session->options; + + (void)a; + + session->remote_port = port; + + ret = parse_optional_arguments(option_string, len - (int32_t)sizeof(struct pico_tftp_hdr), &session->options, &session->option_timeout, &session->file_size); + if (ret || (session->options & ~proposed_options)) { + do_callback(session, PICO_TFTP_EV_ERR_PEER, session->tftp_block, len); + tftp_send_error(session, a, port, TFTP_ERR_EOPT, "Invalid option"); + return; + } + + do_callback(session, PICO_TFTP_EV_OPT, session->tftp_block, len); +} + +static void event_oack_rr(struct pico_tftp_session *session, int32_t len, union pico_address *a, uint16_t port) +{ + event_oack(session, len, a, port); + tftp_send_ack(session); + session->state = TFTP_STATE_RX; +} + +static void event_oack_wr(struct pico_tftp_session *session, int32_t len, union pico_address *a, uint16_t port) +{ + event_oack(session, len, a, port); + session->state = TFTP_STATE_TX; +} + +static void event_timeout(struct pico_tftp_session *session, pico_time t) +{ + pico_time new_timeout; + int factor; + + (void)t; + if (++session->retry == TFTP_MAX_RETRY) { + strcpy((char *)session->tftp_block, "Network timeout"); + do_callback(session, PICO_TFTP_EV_ERR_PEER, session->tftp_block, 0); + tftp_finish(session); + return; + } + + tftp_send(session, 0); + if (session->options & PICO_TFTP_OPTION_TIME) + new_timeout = session->option_timeout * 1000U; + else { + new_timeout = PICO_TFTP_TIMEOUT; + for (factor = session->retry; factor; --factor) + new_timeout *= 2; + } + + tftp_schedule_timeout(session, new_timeout); +} + +static void event_timeout_closing(struct pico_tftp_session *session, pico_time t) +{ + (void)t; + if (session->active_timers == 0) + del_session(session); +} + +static void event_timeout_final(struct pico_tftp_session *session, pico_time t) +{ + (void)t; + + tftp_finish(session); +} + +static void unexpected(struct pico_tftp_session *session, int32_t len, union pico_address *a, uint16_t port) +{ + (void)len; + tftp_send_error(session, a, port, TFTP_ERR_EILL, "Unexpected message"); +} + +static void null(struct pico_tftp_session *session, int32_t len, union pico_address *a, uint16_t port) +{ + (void)session; + (void)len; + (void)a; + (void)port; +} + +static struct automa_events fsm[AUTOMA_STATES] = { + /* STATE * ACK DATA ERROR OACK TIMEOUT */ + /* ***************************************************************************************************************** */ + { /* TFTP_STATE_READ_REQUESTED */ unexpected, event_data_rdr, event_err, event_oack_rr, event_timeout}, + { /* TFTP_STATE_RX */ unexpected, event_data, event_err, unexpected, event_timeout}, + { /* TFTP_STATE_LAST_ACK_SENT */ unexpected, event_data_rpl, null, unexpected, event_timeout_final}, + { /* TFTP_STATE_WRITE_REQUESTED */ event_ack0_wr, unexpected, event_err, event_oack_wr, event_timeout}, + { /* TFTP_STATE_TX */ event_ack, unexpected, event_err, unexpected, event_timeout}, + { /* TFTP_STATE_WAIT_OPT_CONFIRM */ event_ack0_woc, unexpected, event_err, unexpected, event_timeout}, + { /* TFTP_STATE_WAIT_LAST_ACK */ event_ack_last, unexpected, event_err, unexpected, event_timeout}, + { /* TFTP_STATE_CLOSING */ null, null, null, null, event_timeout_closing} +}; + +static void tftp_message_received(struct pico_tftp_session *session, uint8_t *block, int32_t len, union pico_address *a, uint16_t port) +{ + struct pico_tftp_hdr *th = (struct pico_tftp_hdr *) block; + + if (!session->callback) + return; + + session->wallclock_timeout = 0; + + switch (short_be(th->opcode)) { + case PICO_TFTP_RRQ: + case PICO_TFTP_WRQ: + unexpected(session, len, a, port); + break; + case PICO_TFTP_DATA: + fsm[session->state].data(session, len, a, port); + break; + case PICO_TFTP_ACK: + fsm[session->state].ack(session, len, a, port); + break; + case PICO_TFTP_ERROR: + fsm[session->state].error(session, len, a, port); + break; + case PICO_TFTP_OACK: + fsm[session->state].oack(session, len, a, port); + break; + default: + tftp_send_error(session, NULL, 0, TFTP_ERR_EILL, "Illegal opcode"); + } +} + +static void tftp_cb(uint16_t ev, struct pico_socket *s) +{ + int r; + struct pico_tftp_session *session; + union pico_address ep; + uint16_t port = 0; + + session = find_session_by_socket(s); + if (session) { + if (ev == PICO_SOCK_EV_ERR) { + strcpy((char *)session->tftp_block, "Socket Error"); + do_callback(session, PICO_TFTP_EV_ERR_LOCAL, session->tftp_block, (int32_t)strlen((char *)session->tftp_block)); + tftp_finish(session); + return; + } + + r = pico_socket_recvfrom(s, session->tftp_block, PICO_TFTP_TOTAL_BLOCK_SIZE, &ep, &port); + if (r < (int)sizeof(struct pico_tftp_hdr)) + return; + + tftp_message_received(session, session->tftp_block, r, &ep, port); + } else { + if (!server.listen_socket || s != server.listen_socket) { + return; + } + + r = pico_socket_recvfrom(s, server.tftp_block, PICO_TFTP_TOTAL_BLOCK_SIZE, &ep, &port); + if (r < (int)sizeof(struct pico_tftp_hdr)) + return; + + tftp_req(server.tftp_block, r, &ep, port); + } +} + +static int application_rx_cb(struct pico_tftp_session *session, uint16_t event, uint8_t *block, int32_t len, void *arg) +{ + int *flag = (int *)arg; + + (void)block; + + switch (event) { + case PICO_TFTP_EV_ERR_PEER: + case PICO_TFTP_EV_ERR_LOCAL: + *flag = 0 - event; + break; + case PICO_TFTP_EV_OK: + session->len = len; + *flag = 1; + break; + case PICO_TFTP_EV_OPT: + break; + } + return 0; +} + +static int application_tx_cb(struct pico_tftp_session *session, uint16_t event, uint8_t *block, int32_t len, void *arg) +{ + (void)session; + (void)block; + (void)len; + + *(int*)arg = ((event == PICO_TFTP_EV_OK) || (event == PICO_TFTP_EV_OPT)) ? (1) : (0 - event); + return 0; +} + +static void timer_callback(pico_time now, void *arg) +{ + struct pico_tftp_session *session = (struct pico_tftp_session *)arg; + + --session->active_timers; + if (session->wallclock_timeout == 0) { + /* Timer is cancelled. */ + return; + } + + if (now >= session->wallclock_timeout) { + session->wallclock_timeout = 0ULL; + fsm[session->state].timeout(session, now); + } else { + tftp_schedule_timeout(session, session->wallclock_timeout - now); + } +} + +static struct pico_socket *tftp_socket_open(uint16_t family, uint16_t localport) +{ + struct pico_socket *sock; + union pico_address local_address; + + sock = pico_socket_open(family, PICO_PROTO_UDP, tftp_cb); + if (!sock) + return NULL; + + localport = short_be(localport); + + memset(&local_address, 0, sizeof(union pico_address)); + if (pico_socket_bind(sock, &local_address, &localport) < 0) { + pico_socket_close(sock); + return NULL; + } + + return sock; +} + +static inline int tftp_start_check(struct pico_tftp_session *session, uint16_t port, const char *filename, + int (*user_cb)(struct pico_tftp_session *session, uint16_t err, uint8_t *block, int32_t len, void *arg)) +{ + if (!session) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + if ((!server.listen_socket) && (port != short_be(PICO_TFTP_PORT))) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + if (!filename) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + if (!user_cb) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + return 0; +} + +/* *** EXPORTED FUNCTIONS *** */ + +struct pico_tftp_session *pico_tftp_session_setup(union pico_address *a, uint16_t family) +{ + struct pico_socket *sock; + + sock = tftp_socket_open(family, 0); + if (!sock) + return NULL; + + return pico_tftp_session_create(sock, a); +} + +int pico_tftp_get_option(struct pico_tftp_session *session, uint8_t type, int32_t *value) +{ + if (!session) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + switch (type) { + case PICO_TFTP_OPTION_FILE: + if (session->options & PICO_TFTP_OPTION_FILE) + *value = session->file_size; + else { + pico_err = PICO_ERR_ENOENT; + return -1; + } + + break; + case PICO_TFTP_OPTION_TIME: + if (session->options & PICO_TFTP_OPTION_TIME) + *value = session->option_timeout; + else { + pico_err = PICO_ERR_ENOENT; + return -1; + } + + break; + default: + pico_err = PICO_ERR_EINVAL; + return -1; + } + + return 0; +} + +int pico_tftp_set_option(struct pico_tftp_session *session, uint8_t type, int32_t value) +{ + if (!session) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + switch (type) { + case PICO_TFTP_OPTION_FILE: + if (value < 0) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + session->file_size = value; + session->options |= PICO_TFTP_OPTION_FILE; + break; + case PICO_TFTP_OPTION_TIME: + if (value > PICO_TFTP_MAX_TIMEOUT) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + session->option_timeout = (uint8_t)(value & 0xFF); + if (value) { + session->options |= PICO_TFTP_OPTION_TIME; + } else { + session->options &= ~PICO_TFTP_OPTION_TIME; + } + + break; + default: + pico_err = PICO_ERR_EINVAL; + return -1; + } + + return 0; +} + +/* Active RX request from PicoTCP */ +int pico_tftp_start_rx(struct pico_tftp_session *session, uint16_t port, const char *filename, + int (*user_cb)(struct pico_tftp_session *session, uint16_t event, uint8_t *block, int32_t len, void *arg), void *arg) +{ + if (tftp_start_check(session, port, filename, user_cb)) + return -1; + + session->callback = user_cb; + session->packet_counter = 0u; + session->argument = arg; + + add_session(session); + + if (port != short_be(PICO_TFTP_PORT)) { + session->remote_port = port; + session->state = TFTP_STATE_RX; + if (session->options & (PICO_TFTP_OPTION_FILE | PICO_TFTP_OPTION_TIME)) + tftp_send_oack(session); + else + tftp_send_ack(session); + } else { + tftp_send_rx_req(session, &session->remote_address, port, filename); + } + + return 0; +} + +int pico_tftp_start_tx(struct pico_tftp_session *session, uint16_t port, const char *filename, + int (*user_cb)(struct pico_tftp_session *session, uint16_t event, uint8_t *block, int32_t len, void *arg), void *arg) +{ + if (tftp_start_check(session, port, filename, user_cb)) + return -1; + + session->callback = user_cb; + session->packet_counter = 1u; + session->argument = arg; + + add_session(session); + + if (port != short_be(PICO_TFTP_PORT)) { + session->remote_port = port; + if (session->options) { + tftp_send_oack(session); + session->state = TFTP_STATE_WAIT_OPT_CONFIRM; + } else { + do_callback(session, PICO_TFTP_EV_OK, NULL, 0); + } + } else + tftp_send_tx_req(session, &session->remote_address, port, filename); + + return 0; +} + +int pico_tftp_reject_request(union pico_address*addr, uint16_t port, uint16_t error_code, const char*error_message) +{ + return send_error(server.tftp_block, server.listen_socket, addr, port, error_code, error_message); +} + +int32_t pico_tftp_send(struct pico_tftp_session *session, const uint8_t *data, int32_t len) +{ + int32_t size; + + + if (len < 0) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + size = len; + + if (size > PICO_TFTP_PAYLOAD_SIZE) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + tftp_send_data(session, data, size); + + return len; +} + +int pico_tftp_listen(uint16_t family, void (*cb)(union pico_address *addr, uint16_t port, uint16_t opcode, char *filename, int32_t len)) +{ + struct pico_socket *sock; + + if (server.listen_socket) { + pico_err = PICO_ERR_EEXIST; + return -1; + } + + sock = tftp_socket_open(family, PICO_TFTP_PORT); + if (!sock) + return -1; + + server.listen_socket = sock; + server.listen_callback = cb; + + return 0; +} + +int pico_tftp_parse_request_args(char *args, int32_t len, int *options, uint8_t *timeout, int32_t *filesize) +{ + char *pos; + char *end_args = args + len; + + args = extract_arg_pointer(args, end_args, &pos); + + return parse_optional_arguments(args, (int32_t)(end_args - args), options, timeout, filesize); +} + +int pico_tftp_abort(struct pico_tftp_session *session, uint16_t error, const char *reason) +{ + if (!session) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + if (!find_session_by_socket(session->socket)) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + tftp_send_error(session, NULL, 0, error, reason); + + return 0; +} + +int pico_tftp_close_server(void) +{ + if (!server.listen_socket) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + + pico_socket_close(server.listen_socket); + server.listen_socket = NULL; + return 0; +} + +int pico_tftp_get_file_size(struct pico_tftp_session *session, int32_t *file_size) +{ + return pico_tftp_get_option(session, PICO_TFTP_OPTION_FILE, file_size); +} + +struct pico_tftp_session *pico_tftp_app_setup(union pico_address *a, uint16_t port, uint16_t family, int *synchro) +{ + struct pico_tftp_session *session; + + if (!synchro) { + pico_err = PICO_ERR_EINVAL; + return NULL; + } + + session = pico_tftp_session_setup(a, family); + if (!session) + return NULL; + + session->remote_port = port; + session->status |= SESSION_STATUS_APP_ACK; + session->argument = synchro; + + *synchro = 0; + + return session; +} + +int pico_tftp_app_start_rx(struct pico_tftp_session *session, const char *filename) +{ + return pico_tftp_start_rx(session, session->remote_port, filename, application_rx_cb, session->argument); +} + +int pico_tftp_app_start_tx(struct pico_tftp_session *session, const char *filename) +{ + return pico_tftp_start_tx(session, session->remote_port, filename, application_tx_cb, session->argument); +} + +int32_t pico_tftp_get(struct pico_tftp_session *session, uint8_t *data, int32_t len) +{ + int synchro; + + if (!session || len < session->len ) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + synchro = *(int*)session->argument; + *(int*)session->argument = 0; + if ((session->state != TFTP_STATE_RX) && (session->state != TFTP_STATE_READ_REQUESTED)) + return -1; + + if (synchro < 0) + return synchro; + + memcpy(data, tftp_payload(session->tftp_block), (size_t)session->len); + len = session->len; + + tftp_send_ack(session); + tftp_eval_finish(session, len); + return len; +} + +int32_t pico_tftp_put(struct pico_tftp_session *session, uint8_t *data, int32_t len) +{ + int synchro; + + if ((!session) || (!data) || (len < 0)) { + pico_err = PICO_ERR_EINVAL; + return -1; + } + synchro = *(int*)session->argument; + *(int*)session->argument = 0; + if (synchro < 0) + return synchro; + + if (len > PICO_TFTP_PAYLOAD_SIZE) + len = PICO_TFTP_PAYLOAD_SIZE; + + pico_tftp_send(session, data, len); + return len; +} diff --git a/ext/picotcp/modules/pico_tftp.h b/ext/picotcp/modules/pico_tftp.h new file mode 100644 index 0000000..bcbb648 --- /dev/null +++ b/ext/picotcp/modules/pico_tftp.h @@ -0,0 +1,83 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + . + *********************************************************************/ +#ifndef PICO_TFTP_H +#define PICO_TFTP_H + +#include +#include + +#define PICO_TFTP_PORT (69) +#define PICO_TFTP_PAYLOAD_SIZE (512) + +#define PICO_TFTP_NONE 0 +#define PICO_TFTP_RRQ 1 +#define PICO_TFTP_WRQ 2 +#define PICO_TFTP_DATA 3 +#define PICO_TFTP_ACK 4 +#define PICO_TFTP_ERROR 5 +#define PICO_TFTP_OACK 6 + +/* Callback user events */ +#define PICO_TFTP_EV_OK 0 +#define PICO_TFTP_EV_OPT 1 +#define PICO_TFTP_EV_ERR_PEER 2 +#define PICO_TFTP_EV_ERR_LOCAL 3 + +/* TFTP ERROR CODES */ +#define TFTP_ERR_UNDEF 0 +#define TFTP_ERR_ENOENT 1 +#define TFTP_ERR_EACC 2 +#define TFTP_ERR_EXCEEDED 3 +#define TFTP_ERR_EILL 4 +#define TFTP_ERR_ETID 5 +#define TFTP_ERR_EEXIST 6 +#define TFTP_ERR_EUSR 7 +#define TFTP_ERR_EOPT 8 + +/* Session options */ +#define PICO_TFTP_OPTION_FILE 1 + +/* timeout: 0 -> adaptative, 1-255 -> fixed */ +#define PICO_TFTP_OPTION_TIME 2 + + +#define PICO_TFTP_MAX_TIMEOUT 255 +#define PICO_TFTP_MAX_FILESIZE (65535 * 512 - 1) + +struct pico_tftp_session; + +struct pico_tftp_session *pico_tftp_session_setup(union pico_address *a, uint16_t family); +int pico_tftp_set_option(struct pico_tftp_session *session, uint8_t type, int32_t value); +int pico_tftp_get_option(struct pico_tftp_session *session, uint8_t type, int32_t *value); + +int pico_tftp_start_rx(struct pico_tftp_session *session, uint16_t port, const char *filename, + int (*user_cb)(struct pico_tftp_session *session, uint16_t event, uint8_t *block, int32_t len, void *arg), void *arg); +int pico_tftp_start_tx(struct pico_tftp_session *session, uint16_t port, const char *filename, + int (*user_cb)(struct pico_tftp_session *session, uint16_t event, uint8_t *block, int32_t len, void *arg), void *arg); + +int pico_tftp_reject_request(union pico_address *addr, uint16_t port, uint16_t error_code, const char *error_message); +int32_t pico_tftp_send(struct pico_tftp_session *session, const uint8_t *data, int32_t len); + +int pico_tftp_listen(uint16_t family, void (*cb)(union pico_address *addr, uint16_t port, uint16_t opcode, char *filename, int32_t len)); + +int pico_tftp_parse_request_args(char *args, int32_t len, int *options, uint8_t *timeout, int32_t *filesize); + +int pico_tftp_abort(struct pico_tftp_session *session, uint16_t error, const char *reason); +int pico_tftp_close_server(void); + +int pico_tftp_get_file_size(struct pico_tftp_session *session, int32_t *file_size); + +/* SPECIFIC APPLICATION DRIVEN FUNCTIONS */ +struct pico_tftp_session *pico_tftp_app_setup(union pico_address *a, uint16_t port, uint16_t family, int *synchro); + +int pico_tftp_app_start_rx(struct pico_tftp_session *session, const char *filename); +int pico_tftp_app_start_tx(struct pico_tftp_session *session, const char *filename); + +int32_t pico_tftp_get(struct pico_tftp_session *session, uint8_t *data, int32_t len); +int32_t pico_tftp_put(struct pico_tftp_session *session, uint8_t *data, int32_t len); + +#endif diff --git a/ext/picotcp/modules/pico_udp.c b/ext/picotcp/modules/pico_udp.c new file mode 100644 index 0000000..9545781 --- /dev/null +++ b/ext/picotcp/modules/pico_udp.c @@ -0,0 +1,217 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + Authors: Daniele Lacamera + *********************************************************************/ + + +#include "pico_udp.h" +#include "pico_config.h" +#include "pico_eth.h" +#include "pico_socket.h" +#include "pico_stack.h" + +#define UDP_FRAME_OVERHEAD (sizeof(struct pico_frame)) +#define udp_dbg(...) do {} while(0) + +/* Queues */ +static struct pico_queue udp_in = { + 0 +}; +static struct pico_queue udp_out = { + 0 +}; + + +/* Functions */ + +uint16_t pico_udp_checksum_ipv4(struct pico_frame *f) +{ + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; + struct pico_udp_hdr *udp_hdr = (struct pico_udp_hdr *) f->transport_hdr; + struct pico_socket *s = f->sock; + struct pico_ipv4_pseudo_hdr pseudo; + + if (s) { + /* Case of outgoing frame */ + udp_dbg("UDP CRC: on outgoing frame\n"); + pseudo.src.addr = s->local_addr.ip4.addr; + pseudo.dst.addr = s->remote_addr.ip4.addr; + } else { + /* Case of incomming frame */ + udp_dbg("UDP CRC: on incomming frame\n"); + pseudo.src.addr = hdr->src.addr; + pseudo.dst.addr = hdr->dst.addr; + } + + pseudo.zeros = 0; + pseudo.proto = PICO_PROTO_UDP; + pseudo.len = short_be(f->transport_len); + + return pico_dualbuffer_checksum(&pseudo, sizeof(struct pico_ipv4_pseudo_hdr), udp_hdr, f->transport_len); +} + +#ifdef PICO_SUPPORT_IPV6 +uint16_t pico_udp_checksum_ipv6(struct pico_frame *f) +{ + struct pico_ipv6_hdr *ipv6_hdr = (struct pico_ipv6_hdr *)f->net_hdr; + struct pico_udp_hdr *udp_hdr = (struct pico_udp_hdr *)f->transport_hdr; + struct pico_ipv6_pseudo_hdr pseudo = { + .src = {{0}}, .dst = {{0}}, .len = 0, .zero = {0}, .nxthdr = 0 + }; + struct pico_socket *s = f->sock; + struct pico_remote_endpoint *remote_endpoint = (struct pico_remote_endpoint *)f->info; + + /* XXX If the IPv6 packet contains a Routing header, the Destination + * Address used in the pseudo-header is that of the final destination */ + if (s) { + /* Case of outgoing frame */ + pseudo.src = s->local_addr.ip6; + if (remote_endpoint) + pseudo.dst = remote_endpoint->remote_addr.ip6; + else + pseudo.dst = s->remote_addr.ip6; + } else { + /* Case of incomming frame */ + pseudo.src = ipv6_hdr->src; + pseudo.dst = ipv6_hdr->dst; + } + + pseudo.len = long_be(f->transport_len); + pseudo.nxthdr = PICO_PROTO_UDP; + + return pico_dualbuffer_checksum(&pseudo, sizeof(struct pico_ipv6_pseudo_hdr), udp_hdr, f->transport_len); +} +#endif + + + +static int pico_udp_process_out(struct pico_protocol *self, struct pico_frame *f) +{ + IGNORE_PARAMETER(self); + return (int)pico_network_send(f); +} + +static int pico_udp_push(struct pico_protocol *self, struct pico_frame *f) +{ + struct pico_udp_hdr *hdr = (struct pico_udp_hdr *) f->transport_hdr; + struct pico_remote_endpoint *remote_endpoint = (struct pico_remote_endpoint *) f->info; + + /* this (fragmented) frame should contain a transport header */ + if (f->transport_hdr != f->payload) { + hdr->trans.sport = f->sock->local_port; + if (remote_endpoint) { + hdr->trans.dport = remote_endpoint->remote_port; + } else { + hdr->trans.dport = f->sock->remote_port; + } + + hdr->len = short_be(f->transport_len); + + /* do not perform CRC validation. If you want to, a system needs to be + implemented to calculate the CRC over the total payload of a + fragmented payload + */ + hdr->crc = 0; + } + + if (pico_enqueue(self->q_out, f) > 0) { + return f->payload_len; + } else { + return 0; + } +} + +/* Interface: protocol definition */ +struct pico_protocol pico_proto_udp = { + .name = "udp", + .proto_number = PICO_PROTO_UDP, + .layer = PICO_LAYER_TRANSPORT, + .process_in = pico_transport_process_in, + .process_out = pico_udp_process_out, + .push = pico_udp_push, + .q_in = &udp_in, + .q_out = &udp_out, +}; + + + +struct pico_socket *pico_udp_open(void) +{ + struct pico_socket_udp *u = PICO_ZALLOC(sizeof(struct pico_socket_udp)); + if (!u) + return NULL; + + u->mode = PICO_UDP_MODE_UNICAST; + +#ifdef PICO_SUPPORT_MCAST + u->mc_ttl = PICO_IP_DEFAULT_MULTICAST_TTL; + /* enable multicast loopback by default */ + u->sock.opt_flags |= (1 << PICO_SOCKET_OPT_MULTICAST_LOOP); +#endif + + return &u->sock; +} + +static void pico_udp_get_msginfo(struct pico_frame *f, struct pico_msginfo *msginfo) +{ + if (!msginfo || !f->net_hdr) + return; + + msginfo->dev = f->dev; + + if (IS_IPV4(f)) { /* IPV4 */ +#ifdef PICO_SUPPORT_IPV4 + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)(f->net_hdr); + msginfo->ttl = hdr->ttl; + msginfo->tos = hdr->tos; +#endif + } else { +#ifdef PICO_SUPPORT_IPV6 + struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)(f->net_hdr); + msginfo->ttl = hdr->hop; + msginfo->tos = (hdr->vtf >> 20) & 0xFF; /* IPv6 traffic class */ +#endif + } +} + +uint16_t pico_udp_recv(struct pico_socket *s, void *buf, uint16_t len, void *src, uint16_t *port, struct pico_msginfo *msginfo) +{ + struct pico_frame *f = pico_queue_peek(&s->q_in); + if (f) { + if(!f->payload_len) { + f->payload = f->transport_hdr + sizeof(struct pico_udp_hdr); + f->payload_len = (uint16_t)(f->transport_len - sizeof(struct pico_udp_hdr)); + } + + udp_dbg("expected: %d, got: %d\n", len, f->payload_len); + if (src) + pico_store_network_origin(src, f); + + if (port) { + struct pico_trans *hdr = (struct pico_trans *)f->transport_hdr; + *port = hdr->sport; + } + + if (msginfo) { + pico_udp_get_msginfo(f, msginfo); + } + + if (f->payload_len > len) { + memcpy(buf, f->payload, len); + f->payload += len; + f->payload_len = (uint16_t)(f->payload_len - len); + return len; + } else { + uint16_t ret = f->payload_len; + memcpy(buf, f->payload, f->payload_len); + f = pico_dequeue(&s->q_in); + pico_frame_discard(f); + return ret; + } + } else return 0; +} + diff --git a/ext/picotcp/modules/pico_udp.h b/ext/picotcp/modules/pico_udp.h new file mode 100644 index 0000000..80b97a9 --- /dev/null +++ b/ext/picotcp/modules/pico_udp.h @@ -0,0 +1,45 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + *********************************************************************/ +#ifndef INCLUDE_PICO_UDP +#define INCLUDE_PICO_UDP +#include "pico_addressing.h" +#include "pico_protocol.h" +#include "pico_socket.h" +#define PICO_UDP_MODE_UNICAST 0x01 +#define PICO_UDP_MODE_MULTICAST 0x02 +#define PICO_UDP_MODE_BROADCAST 0xFF + +struct pico_socket_udp +{ + struct pico_socket sock; + int mode; + uint8_t mc_ttl; /* Multicasting TTL */ +}; + + +extern struct pico_protocol pico_proto_udp; + +PACKED_STRUCT_DEF pico_udp_hdr { + struct pico_trans trans; + uint16_t len; + uint16_t crc; +}; +#define PICO_UDPHDR_SIZE 8 + +struct pico_socket *pico_udp_open(void); +uint16_t pico_udp_recv(struct pico_socket *s, void *buf, uint16_t len, void *src, uint16_t *port, struct pico_msginfo *msginfo); +uint16_t pico_udp_checksum_ipv4(struct pico_frame *f); + +#ifdef PICO_SUPPORT_IPV6 +uint16_t pico_udp_checksum_ipv6(struct pico_frame *f); +#endif + + +int pico_udp_setsockopt(struct pico_socket *s, int option, void *value); + +#endif diff --git a/ext/picotcp/rules/crc.mk b/ext/picotcp/rules/crc.mk new file mode 100644 index 0000000..8ec9344 --- /dev/null +++ b/ext/picotcp/rules/crc.mk @@ -0,0 +1 @@ +OPTIONS+=-DPICO_SUPPORT_CRC diff --git a/ext/picotcp/rules/cyassl.mk b/ext/picotcp/rules/cyassl.mk new file mode 100644 index 0000000..5903b8e --- /dev/null +++ b/ext/picotcp/rules/cyassl.mk @@ -0,0 +1,3 @@ +OPTIONS+=-DPICO_SUPPORT_CYASSL +LDFLAGS+=-lcyassl + diff --git a/ext/picotcp/rules/devloop.mk b/ext/picotcp/rules/devloop.mk new file mode 100644 index 0000000..066a281 --- /dev/null +++ b/ext/picotcp/rules/devloop.mk @@ -0,0 +1,2 @@ +OPTIONS+=-DPICO_SUPPORT_DEVLOOP +MOD_OBJ+=$(LIBBASE)modules/pico_dev_loop.o diff --git a/ext/picotcp/rules/dhcp_client.mk b/ext/picotcp/rules/dhcp_client.mk new file mode 100644 index 0000000..94a1507 --- /dev/null +++ b/ext/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/ext/picotcp/rules/dhcp_server.mk b/ext/picotcp/rules/dhcp_server.mk new file mode 100644 index 0000000..c6d2b59 --- /dev/null +++ b/ext/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/ext/picotcp/rules/dns_client.mk b/ext/picotcp/rules/dns_client.mk new file mode 100644 index 0000000..f5e14c7 --- /dev/null +++ b/ext/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/ext/picotcp/rules/dns_sd.mk b/ext/picotcp/rules/dns_sd.mk new file mode 100644 index 0000000..62d05cc --- /dev/null +++ b/ext/picotcp/rules/dns_sd.mk @@ -0,0 +1,2 @@ +OPTIONS+=-DPICO_SUPPORT_DNS_SD +MOD_OBJ+=$(LIBBASE)modules/pico_dns_sd.o $(LIBBASE)modules/pico_mdns.o $(LIBBASE)modules/pico_dns_common.o \ No newline at end of file diff --git a/ext/picotcp/rules/eth.mk b/ext/picotcp/rules/eth.mk new file mode 100644 index 0000000..f955bd8 --- /dev/null +++ b/ext/picotcp/rules/eth.mk @@ -0,0 +1,2 @@ +OPTIONS+=-DPICO_SUPPORT_ETH +MOD_OBJ+=$(LIBBASE)modules/pico_arp.o diff --git a/ext/picotcp/rules/icmp4.mk b/ext/picotcp/rules/icmp4.mk new file mode 100644 index 0000000..826e0e0 --- /dev/null +++ b/ext/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/ext/picotcp/rules/igmp.mk b/ext/picotcp/rules/igmp.mk new file mode 100644 index 0000000..804079b --- /dev/null +++ b/ext/picotcp/rules/igmp.mk @@ -0,0 +1,3 @@ +OPTIONS+=-DPICO_SUPPORT_IGMP +MOD_OBJ+=$(LIBBASE)modules/pico_igmp.o + diff --git a/ext/picotcp/rules/ipfilter.mk b/ext/picotcp/rules/ipfilter.mk new file mode 100644 index 0000000..3044eb6 --- /dev/null +++ b/ext/picotcp/rules/ipfilter.mk @@ -0,0 +1,2 @@ +OPTIONS+=-DPICO_SUPPORT_IPFILTER +MOD_OBJ+=$(LIBBASE)modules/pico_ipfilter.o diff --git a/ext/picotcp/rules/ipv4.mk b/ext/picotcp/rules/ipv4.mk new file mode 100644 index 0000000..34d2108 --- /dev/null +++ b/ext/picotcp/rules/ipv4.mk @@ -0,0 +1,2 @@ +OPTIONS+=-DPICO_SUPPORT_IPV4 +MOD_OBJ+=$(LIBBASE)modules/pico_ipv4.o diff --git a/ext/picotcp/rules/ipv4frag.mk b/ext/picotcp/rules/ipv4frag.mk new file mode 100644 index 0000000..a79ccb3 --- /dev/null +++ b/ext/picotcp/rules/ipv4frag.mk @@ -0,0 +1,2 @@ +OPTIONS+=-DPICO_SUPPORT_IPV4FRAG +MOD_OBJ+=$(LIBBASE)modules/pico_fragments.o diff --git a/ext/picotcp/rules/ipv6.mk b/ext/picotcp/rules/ipv6.mk new file mode 100644 index 0000000..f313cbf --- /dev/null +++ b/ext/picotcp/rules/ipv6.mk @@ -0,0 +1,3 @@ +OPTIONS+=-DPICO_SUPPORT_IPV6 -DPICO_SUPPORT_ICMP6 +MOD_OBJ+=$(LIBBASE)modules/pico_ipv6.o $(LIBBASE)modules/pico_ipv6_nd.o $(LIBBASE)modules/pico_icmp6.o +include rules/ipv6frag.mk diff --git a/ext/picotcp/rules/ipv6frag.mk b/ext/picotcp/rules/ipv6frag.mk new file mode 100644 index 0000000..da6435b --- /dev/null +++ b/ext/picotcp/rules/ipv6frag.mk @@ -0,0 +1,2 @@ +OPTIONS+=-DPICO_SUPPORT_IPV6FRAG +MOD_OBJ+=$(LIBBASE)modules/pico_fragments.o diff --git a/ext/picotcp/rules/mcast.mk b/ext/picotcp/rules/mcast.mk new file mode 100644 index 0000000..037305b --- /dev/null +++ b/ext/picotcp/rules/mcast.mk @@ -0,0 +1 @@ +OPTIONS+=-DPICO_SUPPORT_MCAST diff --git a/ext/picotcp/rules/mdns.mk b/ext/picotcp/rules/mdns.mk new file mode 100644 index 0000000..b28aeeb --- /dev/null +++ b/ext/picotcp/rules/mdns.mk @@ -0,0 +1,2 @@ +OPTIONS+=-DPICO_SUPPORT_MDNS +MOD_OBJ+=$(LIBBASE)modules/pico_mdns.o $(LIBBASE)modules/pico_dns_common.o diff --git a/ext/picotcp/rules/memory_manager.mk b/ext/picotcp/rules/memory_manager.mk new file mode 100644 index 0000000..7b0d923 --- /dev/null +++ b/ext/picotcp/rules/memory_manager.mk @@ -0,0 +1,2 @@ +OPTIONS+=-DPICO_SUPPORT_MM +MOD_OBJ+=$(LIBBASE)modules/pico_mm.o diff --git a/ext/picotcp/rules/mld.mk b/ext/picotcp/rules/mld.mk new file mode 100644 index 0000000..3302e7e --- /dev/null +++ b/ext/picotcp/rules/mld.mk @@ -0,0 +1,3 @@ +OPTIONS+=-DPICO_SUPPORT_MLD +MOD_OBJ+=$(LIBBASE)modules/pico_mld.o + diff --git a/ext/picotcp/rules/nat.mk b/ext/picotcp/rules/nat.mk new file mode 100644 index 0000000..2bd67b5 --- /dev/null +++ b/ext/picotcp/rules/nat.mk @@ -0,0 +1,2 @@ +OPTIONS+=-DPICO_SUPPORT_NAT +MOD_OBJ+=$(LIBBASE)modules/pico_nat.o diff --git a/ext/picotcp/rules/olsr.mk b/ext/picotcp/rules/olsr.mk new file mode 100644 index 0000000..3cf4443 --- /dev/null +++ b/ext/picotcp/rules/olsr.mk @@ -0,0 +1,2 @@ +OPTIONS+=-DPICO_SUPPORT_OLSR +MOD_OBJ+=$(LIBBASE)modules/pico_olsr.o diff --git a/ext/picotcp/rules/pcap.mk b/ext/picotcp/rules/pcap.mk new file mode 100644 index 0000000..9c67277 --- /dev/null +++ b/ext/picotcp/rules/pcap.mk @@ -0,0 +1 @@ +MOD_OBJ+=$(LIBBASE)modules/pico_dev_pcap.o diff --git a/ext/picotcp/rules/polarssl.mk b/ext/picotcp/rules/polarssl.mk new file mode 100644 index 0000000..e5df9d9 --- /dev/null +++ b/ext/picotcp/rules/polarssl.mk @@ -0,0 +1,2 @@ +OPTIONS += -DPICO_SUPPORT_POLARSSL +LDFLAGS += -lpolarssl diff --git a/ext/picotcp/rules/ppp.mk b/ext/picotcp/rules/ppp.mk new file mode 100644 index 0000000..f47c083 --- /dev/null +++ b/ext/picotcp/rules/ppp.mk @@ -0,0 +1,3 @@ +OPTIONS+=-DPICO_SUPPORT_PPP +MOD_OBJ+=$(LIBBASE)modules/pico_dev_ppp.o + diff --git a/ext/picotcp/rules/slaacv4.mk b/ext/picotcp/rules/slaacv4.mk new file mode 100644 index 0000000..2073a2a --- /dev/null +++ b/ext/picotcp/rules/slaacv4.mk @@ -0,0 +1,2 @@ +OPTIONS+=-DPICO_SUPPORT_SLAACV4 +MOD_OBJ+=$(LIBBASE)modules/pico_slaacv4.o $(LIBBASE)modules/pico_hotplug_detection.o diff --git a/ext/picotcp/rules/sntp_client.mk b/ext/picotcp/rules/sntp_client.mk new file mode 100644 index 0000000..423583b --- /dev/null +++ b/ext/picotcp/rules/sntp_client.mk @@ -0,0 +1,2 @@ +OPTIONS+=-DPICO_SUPPORT_SNTP_CLIENT +MOD_OBJ+=$(LIBBASE)modules/pico_sntp_client.o diff --git a/ext/picotcp/rules/tap.mk b/ext/picotcp/rules/tap.mk new file mode 100644 index 0000000..2f3e149 --- /dev/null +++ b/ext/picotcp/rules/tap.mk @@ -0,0 +1 @@ +MOD_OBJ+=$(LIBBASE)modules/pico_dev_tap.o diff --git a/ext/picotcp/rules/tcp.mk b/ext/picotcp/rules/tcp.mk new file mode 100644 index 0000000..ae53034 --- /dev/null +++ b/ext/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/ext/picotcp/rules/tun.mk b/ext/picotcp/rules/tun.mk new file mode 100644 index 0000000..a5b93fe --- /dev/null +++ b/ext/picotcp/rules/tun.mk @@ -0,0 +1 @@ +MOD_OBJ+=$(LIBBASE)modules/pico_dev_tun.o diff --git a/ext/picotcp/rules/udp.mk b/ext/picotcp/rules/udp.mk new file mode 100644 index 0000000..ec9157c --- /dev/null +++ b/ext/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/ext/picotcp/rules/wolfssl.mk b/ext/picotcp/rules/wolfssl.mk new file mode 100644 index 0000000..f66d114 --- /dev/null +++ b/ext/picotcp/rules/wolfssl.mk @@ -0,0 +1,3 @@ +OPTIONS+=-DPICO_SUPPORT_CYASSL -DPICO_SUPPORT_WOLFSSL +LDFLAGS+=-lwolfssl + diff --git a/ext/picotcp/stack/pico_device.c b/ext/picotcp/stack/pico_device.c new file mode 100644 index 0000000..c137180 --- /dev/null +++ b/ext/picotcp/stack/pico_device.c @@ -0,0 +1,408 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING 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" +#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_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); + 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 newaddr; + 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; + 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]; + link = pico_ipv6_link_add(dev, newaddr, netmask64); + if (link) { + device_init_ipv6_final(dev, &newaddr); + } + + return link; +} +#endif + +static int device_init_mac(struct pico_device *dev, 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 + dev->eth = PICO_ZALLOC(sizeof(struct pico_ethdev)); + if (dev->eth) { + memcpy(dev->eth->mac.addr, mac, PICO_SIZE_ETH); + #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 + } else { + pico_err = PICO_ERR_ENOMEM; + return -1; + } + + 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; +} + +int pico_device_init(struct pico_device *dev, const char *name, 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; + } + + pico_tree_insert(&Device_tree, dev); + if (!dev->mtu) + dev->mtu = PICO_DEVICE_DEFAULT_MTU; + + if (mac) { + ret = device_init_mac(dev, mac); + } else { + ret = device_init_nomac(dev); + } + + 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->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) { + if (dev->eth) { + f->datalink_hdr = f->buffer; + (void)pico_ethernet_receive(f); + } else { + f->net_hdr = f->buffer; + pico_network_receive(f); + } + + loop_score--; + } + } + return loop_score; +} + +static int devloop_sendto_dev(struct pico_device *dev, struct pico_frame *f) +{ + + if (dev->eth) { + /* Ethernet: pass management of the frame to the pico_ethernet_send() rdv function */ + return pico_ethernet_send(f); + } else { + /* non-ethernet: no post-processing needed */ + return (dev->send(dev, f->start, (int)f->len) <= 0); /* Return 0 upon success, which is dev->send() > 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/ext/picotcp/stack/pico_frame.c b/ext/picotcp/stack/pico_frame.c new file mode 100644 index 0000000..bfc91d4 --- /dev/null +++ b/ext/picotcp/stack/pico_frame.c @@ -0,0 +1,286 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING for usage. + + . + + Authors: Daniele Lacamera + *********************************************************************/ + + +#include "pico_config.h" +#include "pico_frame.h" +#include "pico_protocol.h" +#include "pico_stack.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(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; + + 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); +} + +int pico_frame_grow(struct pico_frame *f, uint32_t size) +{ + uint8_t *oldbuf; + uint32_t usage_count, *p_old_usage; + uint32_t frame_buffer_size; + uint32_t oldsize; + unsigned int align; + int addr_diff = 0; + + if (!f || (size < f->buffer_len)) { + return -1; + } + + 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(frame_buffer_size + sizeof(uint32_t)); + if (!f->buffer) { + f->buffer = oldbuf; + return -1; + } + + f->usage_count = (uint32_t *)(((uint8_t*)f->buffer) + frame_buffer_size); + *f->usage_count = usage_count; + f->buffer_len = size; + memcpy(f->buffer, oldbuf, oldsize); + + /* Update hdr fields to new buffer*/ + addr_diff = (int)(f->buffer - 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_USAGE_COUNTER) + PICO_FREE(p_old_usage); + + if (!(f->flags & PICO_FRAME_FLAG_EXT_BUFFER)) + PICO_FREE(oldbuf); + else if (f->notify_free) + f->notify_free(oldbuf); + + f->flags = 0; + /* Now, the frame is not zerocopy anymore, and the usage counter has been moved within it */ + return 0; +} + +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); + int 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 = (int)(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; + +#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/ext/picotcp/stack/pico_md5.c b/ext/picotcp/stack/pico_md5.c new file mode 100644 index 0000000..d612bce --- /dev/null +++ b/ext/picotcp/stack/pico_md5.c @@ -0,0 +1,43 @@ +/********************************************************************* + * PicoTCP. Copyright (c) 2015 Altran Intelligent Systems. Some rights reserved. + * See LICENSE and COPYING 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/ext/picotcp/stack/pico_protocol.c b/ext/picotcp/stack/pico_protocol.c new file mode 100644 index 0000000..c1b4657 --- /dev/null +++ b/ext/picotcp/stack/pico_protocol.c @@ -0,0 +1,214 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING 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; +} + +PICO_TREE_DECLARE(Datalink_proto_tree, pico_proto_cmp); +PICO_TREE_DECLARE(Network_proto_tree, pico_proto_cmp); +PICO_TREE_DECLARE(Transport_proto_tree, pico_proto_cmp); +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) +{ + if (!p) + return; + + p->hash = pico_hash(p->name, (uint32_t)strlen(p->name)); + switch (p->layer) { + case PICO_LAYER_DATALINK: + pico_tree_insert(&Datalink_proto_tree, p); + proto_layer_rr_reset(&proto_rr_datalink); + break; + case PICO_LAYER_NETWORK: + pico_tree_insert(&Network_proto_tree, p); + proto_layer_rr_reset(&proto_rr_network); + break; + case PICO_LAYER_TRANSPORT: + pico_tree_insert(&Transport_proto_tree, p); + proto_layer_rr_reset(&proto_rr_transport); + break; + case PICO_LAYER_SOCKET: + pico_tree_insert(&Socket_proto_tree, p); + proto_layer_rr_reset(&proto_rr_socket); + break; + } + dbg("Protocol %s registered (layer: %d).\n", p->name, p->layer); + +} + diff --git a/ext/picotcp/stack/pico_socket.c b/ext/picotcp/stack/pico_socket.c new file mode 100644 index 0000000..839f57d --- /dev/null +++ b/ext/picotcp/stack/pico_socket.c @@ -0,0 +1,2228 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING 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 + + +#define PROTO(s) ((s)->proto->proto_number) + +#define PICO_SOCKET_MTU 1480 /* Ethernet MTU(1500) - IP header size(20) */ + +# define frag_dbg(...) do {} while(0) + + +static struct pico_sockport *sp_udp = NULL, *sp_tcp = NULL; + +struct pico_frame *pico_socket_frame_alloc(struct pico_socket *s, 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 0; +} + +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; +} + +PICO_TREE_DECLARE(UDPTable, sockport_cmp); +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 = 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) + { + pico_tree_insert(&UDPTable, sp); + } + else if (PROTO(s) == PICO_PROTO_TCP) + { + pico_tree_insert(&TCPTable, sp); + } + } + + pico_tree_insert(&sp->socks, s); + s->state |= PICO_SOCKET_STATE_BOUND; + PICOTCP_MUTEX_UNLOCK(Mutex); +#ifdef DEBUG_SOCKET_TREE + { + struct pico_tree_node *index; + /* RB_FOREACH(s, socket_tree, &sp->socks) { */ + 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); + pico_multicast_delete(s); + pico_socket_tcp_delete(s); + s->state = PICO_SOCKET_STATE_CLOSED; + pico_timer_add((pico_time)10, socket_garbage_collect, s); + 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 *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; + uint16_t hdr_offset = (uint16_t)pico_socket_sendto_transport_offset(s); + int ret = 0; + (void)src; + + f = pico_socket_frame_alloc(s, (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; + f->dev = msginfo->dev; + } +#ifdef PICO_SUPPORT_IPV6 + if(IS_SOCK_IPV6(s) && ep && pico_ipv6_is_multicast(&ep->remote_addr.ip6.addr[0])) { + f->dev = pico_ipv6_link_find(src); + if(!f->dev) { + return -1; + } + } +#endif + 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 > 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, (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 +} + +static void 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 + +} + + +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 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 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 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, 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, len); + +#endif + +#ifdef PICO_SUPPORT_IPV4 + if (is_sock_ipv4(s)) + f = pico_proto_ipv4.alloc(&pico_proto_ipv4, 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/ext/picotcp/stack/pico_socket_multicast.c b/ext/picotcp/stack/pico_socket_multicast.c new file mode 100644 index 0000000..132881c --- /dev/null +++ b/ext/picotcp/stack/pico_socket_multicast.c @@ -0,0 +1,1340 @@ +#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 +# define so_mcast_dbg(...) do { }while(0) /* ip_mcast_dbg in pico_ipv4.c */ +/* #define so_mcast_dbg dbg */ + +/* 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 +{ + uint8_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 */ +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 */ +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 */ +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 uint8_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 uint8_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 uint8_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) + 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) + pico_tree_delete(&MCASTFilter_ipv6, source); + } + } +#endif + return PICO_IP_MULTICAST_EXCLUDE; +} + +static uint8_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; + pico_tree_insert(&MCASTFilter, source); + } + } +#ifdef PICO_SUPPORT_IPV6 + if( !pico_tree_empty(&listen->MCASTSources_ipv6)) { + pico_tree_foreach_safe(index, &listen->MCASTSources_ipv6, _tmp) + { + source = index->keyValue; + pico_tree_insert(&MCASTFilter_ipv6, source); + } + } +#endif + return PICO_IP_MULTICAST_INCLUDE; +} + +struct pico_mcast_filter_aggregation +{ + uint8_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(uint8_t fm, struct pico_mcast_listen *l) +{ + if (!l) + return -1; + + if (fm > 1) + 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) +{ + uint8_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) + 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 = index->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; + pico_tree_insert(listen_tree, mcast.listen); + + pico_tree_insert(&MCASTSockets, s); + 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) ) + pico_tree_insert(&mcast.listen->MCASTSources, source); +#ifdef PICO_SUPPORT_IPV6 + else if( IS_SOCK_IPV6(s) ) + pico_tree_insert(&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_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_ENOMEM; + return -1; + } + source = PICO_ZALLOC(sizeof(union pico_address)); + if (!source) { + pico_err = PICO_ERR_EADDRNOTAVAIL; + return -1; + } + *source = mcast.mreq_s->mcast_source_addr; + pico_tree_insert(tree, source); + + } 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; + pico_tree_insert(tree, source); + pico_tree_insert(listen_tree, mcast.listen); + reference_count = 1; + } + pico_tree_insert(&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_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/ext/picotcp/stack/pico_stack.c b/ext/picotcp/stack/pico_stack.c new file mode 100644 index 0000000..118dc57 --- /dev/null +++ b/ext/picotcp/stack/pico_stack.c @@ -0,0 +1,1165 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING 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_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" + +#define IS_LIMITED_BCAST(f) (((struct pico_ipv4_hdr *) f->net_hdr)->dst.addr == PICO_IP4_BCAST) + +const uint8_t PICO_ETHADDR_ALL[6] = { + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff +}; + +# define PICO_SIZE_MCAST 3 +const uint8_t PICO_ETHADDR_MCAST[6] = { + 0x01, 0x00, 0x5e, 0x00, 0x00, 0x00 +}; + +#ifdef PICO_SUPPORT_IPV6 +# define PICO_SIZE_MCAST6 2 +const uint8_t PICO_ETHADDR_MCAST6[6] = { + 0x33, 0x33, 0x00, 0x00, 0x00, 0x00 +}; +#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; +} + +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; +} + +/* Network layer: 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; +} + +#ifdef PICO_SUPPORT_ETH +/* DATALINK LEVEL: interface from network to the device + * and vice versa. + */ + +/* The pico_ethernet_receive() function is used by + * those devices supporting ETH in order to push packets up + * into the stack. + */ + +static int destination_is_bcast(struct pico_frame *f) +{ + if (!f) + return 0; + + if (IS_IPV6(f)) + return 0; + +#ifdef PICO_SUPPORT_IPV4 + else { + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; + return pico_ipv4_is_broadcast(hdr->dst.addr); + } +#else + return 0; +#endif +} + +static int destination_is_mcast(struct pico_frame *f) +{ + int ret = 0; + if (!f) + return 0; + +#ifdef PICO_SUPPORT_IPV6 + if (IS_IPV6(f)) { + struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *) f->net_hdr; + ret = pico_ipv6_is_multicast(hdr->dst.addr); + } + +#endif +#ifdef PICO_SUPPORT_IPV4 + else { + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; + ret = pico_ipv4_is_multicast(hdr->dst.addr); + } +#endif + + return ret; +} + +#ifdef PICO_SUPPORT_IPV4 +static int32_t pico_ipv4_ethernet_receive(struct pico_frame *f) +{ + if (IS_IPV4(f)) { + pico_enqueue(pico_proto_ipv4.q_in, f); + } else { + (void)pico_icmp4_param_problem(f, 0); + pico_frame_discard(f); + return -1; + } + + return (int32_t)f->buffer_len; +} +#endif + +#ifdef PICO_SUPPORT_IPV6 +static int32_t pico_ipv6_ethernet_receive(struct pico_frame *f) +{ + if (IS_IPV6(f)) { + pico_enqueue(pico_proto_ipv6.q_in, f); + } else { + /* Wrong version for link layer type */ + pico_frame_discard(f); + return -1; + } + + return (int32_t)f->buffer_len; +} +#endif + +static int32_t pico_ll_receive(struct pico_frame *f) +{ + struct pico_eth_hdr *hdr = (struct pico_eth_hdr *) f->datalink_hdr; + f->net_hdr = f->datalink_hdr + sizeof(struct pico_eth_hdr); + +#if (defined PICO_SUPPORT_IPV4) && (defined PICO_SUPPORT_ETH) + if (hdr->proto == PICO_IDETH_ARP) + return pico_arp_receive(f); + +#endif + +#if defined (PICO_SUPPORT_IPV4) + if (hdr->proto == PICO_IDETH_IPV4) + return pico_ipv4_ethernet_receive(f); + +#endif + +#if defined (PICO_SUPPORT_IPV6) + if (hdr->proto == PICO_IDETH_IPV6) + return pico_ipv6_ethernet_receive(f); + +#endif + + pico_frame_discard(f); + return -1; +} + +static void pico_ll_check_bcast(struct pico_frame *f) +{ + struct pico_eth_hdr *hdr = (struct pico_eth_hdr *) f->datalink_hdr; + /* Indicate a link layer broadcast packet */ + if (memcmp(hdr->daddr, PICO_ETHADDR_ALL, PICO_SIZE_ETH) == 0) + f->flags |= PICO_FRAME_FLAG_BCAST; +} + +int32_t pico_ethernet_receive(struct pico_frame *f) +{ + struct pico_eth_hdr *hdr; + if (!f || !f->dev || !f->datalink_hdr) + { + pico_frame_discard(f); + return -1; + } + + hdr = (struct pico_eth_hdr *) f->datalink_hdr; + if ((memcmp(hdr->daddr, f->dev->eth->mac.addr, PICO_SIZE_ETH) != 0) && + (memcmp(hdr->daddr, PICO_ETHADDR_MCAST, PICO_SIZE_MCAST) != 0) && +#ifdef PICO_SUPPORT_IPV6 + (memcmp(hdr->daddr, PICO_ETHADDR_MCAST6, PICO_SIZE_MCAST6) != 0) && +#endif + (memcmp(hdr->daddr, PICO_ETHADDR_ALL, PICO_SIZE_ETH) != 0)) + { + pico_frame_discard(f); + return -1; + } + + pico_ll_check_bcast(f); + return pico_ll_receive(f); +} + +static struct pico_eth *pico_ethernet_mcast_translate(struct pico_frame *f, uint8_t *pico_mcast_mac) +{ + struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; + + /* place 23 lower bits of IP in lower 23 bits of MAC */ + pico_mcast_mac[5] = (long_be(hdr->dst.addr) & 0x000000FFu); + pico_mcast_mac[4] = (uint8_t)((long_be(hdr->dst.addr) & 0x0000FF00u) >> 8u); + pico_mcast_mac[3] = (uint8_t)((long_be(hdr->dst.addr) & 0x007F0000u) >> 16u); + + return (struct pico_eth *)pico_mcast_mac; +} + + +#ifdef PICO_SUPPORT_IPV6 +static struct pico_eth *pico_ethernet_mcast6_translate(struct pico_frame *f, uint8_t *pico_mcast6_mac) +{ + struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr; + + /* first 2 octets are 0x33, last four are the last four of dst */ + pico_mcast6_mac[5] = hdr->dst.addr[PICO_SIZE_IP6 - 1]; + pico_mcast6_mac[4] = hdr->dst.addr[PICO_SIZE_IP6 - 2]; + pico_mcast6_mac[3] = hdr->dst.addr[PICO_SIZE_IP6 - 3]; + pico_mcast6_mac[2] = hdr->dst.addr[PICO_SIZE_IP6 - 4]; + + return (struct pico_eth *)pico_mcast6_mac; +} +#endif + +static int pico_ethernet_ipv6_dst(struct pico_frame *f, struct pico_eth *const dstmac) +{ + int retval = -1; + if (!dstmac) + return -1; + + #ifdef PICO_SUPPORT_IPV6 + if (destination_is_mcast(f)) { + uint8_t pico_mcast6_mac[6] = { + 0x33, 0x33, 0x00, 0x00, 0x00, 0x00 + }; + pico_ethernet_mcast6_translate(f, pico_mcast6_mac); + memcpy(dstmac, pico_mcast6_mac, PICO_SIZE_ETH); + retval = 0; + } else { + struct pico_eth *neighbor = pico_ipv6_get_neighbor(f); + if (neighbor) + { + memcpy(dstmac, neighbor, PICO_SIZE_ETH); + retval = 0; + } + } + + #else + (void)f; + pico_err = PICO_ERR_EPROTONOSUPPORT; + #endif + return retval; +} + + +/* Ethernet send, first attempt: try our own address. + * Returns 0 if the packet is not for us. + * Returns 1 if the packet is cloned to our own receive queue, so the caller can discard the original frame. + * */ +static int32_t pico_ethsend_local(struct pico_frame *f, struct pico_eth_hdr *hdr) +{ + if (!hdr) + return 0; + + /* Check own mac */ + if(!memcmp(hdr->daddr, hdr->saddr, PICO_SIZE_ETH)) { + struct pico_frame *clone = pico_frame_copy(f); + dbg("sending out packet destined for our own mac\n"); + (void)pico_ethernet_receive(clone); + return 1; + } + + return 0; +} + +/* Ethernet send, second attempt: try bcast. + * Returns 0 if the packet is not bcast, so it will be handled somewhere else. + * Returns 1 if the packet is handled by the pico_device_broadcast() function, so it can be discarded. + * */ +static int32_t pico_ethsend_bcast(struct pico_frame *f) +{ + if (IS_LIMITED_BCAST(f)) { + (void)pico_device_broadcast(f); /* We can discard broadcast even if it's not sent. */ + return 1; + } + + return 0; +} + +/* Ethernet send, third attempt: try unicast. + * If the device driver is busy, we return 0, so the stack won't discard the frame. + * In case of success, we can safely return 1. + */ +static int32_t pico_ethsend_dispatch(struct pico_frame *f) +{ + int ret = f->dev->send(f->dev, f->start, (int) f->len); + if (ret <= 0) + return 0; /* Failure to deliver! */ + else { + return 1; /* Frame is in flight by now. */ + } +} + + + + +/* This function looks for the destination mac address + * in order to send the frame being processed. + */ + +int32_t MOCKABLE pico_ethernet_send(struct pico_frame *f) +{ + struct pico_eth dstmac; + uint8_t dstmac_valid = 0; + uint16_t proto = PICO_IDETH_IPV4; + +#ifdef PICO_SUPPORT_IPV6 + /* Step 1: If the frame has an IPv6 packet, + * destination address is taken from the ND tables + */ + if (IS_IPV6(f)) { + if (pico_ethernet_ipv6_dst(f, &dstmac) < 0) + { + pico_ipv6_nd_postpone(f); + return 0; /* I don't care if frame was actually postponed. If there is no room in the ND table, discard safely. */ + } + + dstmac_valid = 1; + proto = PICO_IDETH_IPV6; + } + else +#endif + + /* In case of broadcast (IPV4 only), dst mac is FF:FF:... */ + if (IS_BCAST(f) || destination_is_bcast(f)) + { + memcpy(&dstmac, PICO_ETHADDR_ALL, PICO_SIZE_ETH); + dstmac_valid = 1; + } + + /* In case of multicast, dst mac is translated from the group address */ + else if (destination_is_mcast(f)) { + uint8_t pico_mcast_mac[6] = { + 0x01, 0x00, 0x5e, 0x00, 0x00, 0x00 + }; + pico_ethernet_mcast_translate(f, pico_mcast_mac); + memcpy(&dstmac, pico_mcast_mac, PICO_SIZE_ETH); + dstmac_valid = 1; + } + +#if (defined PICO_SUPPORT_IPV4) + else { + struct pico_eth *arp_get; + arp_get = pico_arp_get(f); + if (arp_get) { + memcpy(&dstmac, arp_get, PICO_SIZE_ETH); + dstmac_valid = 1; + } else { + /* At this point, ARP will discard the frame in any case. + * It is safe to return without discarding. + */ + pico_arp_postpone(f); + return 0; + /* Same case as for IPv6 ... */ + } + + } +#endif + + /* This sets destination and source address, then pushes the packet to the device. */ + if (dstmac_valid) { + struct pico_eth_hdr *hdr; + hdr = (struct pico_eth_hdr *) f->datalink_hdr; + if ((f->start > f->buffer) && ((f->start - f->buffer) >= PICO_SIZE_ETHHDR)) + { + f->start -= PICO_SIZE_ETHHDR; + f->len += PICO_SIZE_ETHHDR; + f->datalink_hdr = f->start; + hdr = (struct pico_eth_hdr *) f->datalink_hdr; + memcpy(hdr->saddr, f->dev->eth->mac.addr, PICO_SIZE_ETH); + memcpy(hdr->daddr, &dstmac, PICO_SIZE_ETH); + hdr->proto = proto; + } + + if (pico_ethsend_local(f, hdr) || pico_ethsend_bcast(f) || pico_ethsend_dispatch(f)) { + /* one of the above functions has delivered the frame accordingly. (returned != 0) + * It is safe to directly return success. + * */ + return 0; + } + } + + /* Failure: do not dequeue the frame, keep it for later. */ + return -1; +} + +#endif /* PICO_SUPPORT_ETH */ + + +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; +} + + +/* 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; + int32_t ret; + if (len == 0) + return -1; + + f = pico_frame_alloc(len); + if (!f) + { + dbg("Cannot alloc incoming frame!\n"); + return -1; + } + + /* 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); + 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; + 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); + } + + t = NULL; + 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 = Timers->top; + if (id == 0u) + return; + for (i = 1; i <= Timers->n; i++) { + if (tref[i].id == id) { + PICO_FREE(Timers->top[i].tmr); + Timers->top[i].tmr = NULL; + break; + } + } +} + +#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(); + } +} + +MOCKABLE uint32_t pico_timer_add(pico_time expire, void (*timer)(pico_time, void *), void *arg) +{ + struct pico_timer *t = PICO_ZALLOC(sizeof(struct pico_timer)); + struct pico_timer_ref tref; + + /* zero is guard for timers */ + if (tmr_id == 0u) + tmr_id++; + + if (!t) { + pico_err = PICO_ERR_ENOMEM; + return 0; + } + + tref.expire = PICO_TIME_MS() + expire; + t->arg = arg; + t->timer = timer; + tref.tmr = t; + tref.id = tmr_id++; + heap_insert(Timers, &tref); + if (Timers->n > PICO_MAX_TIMERS) { + dbg("Warning: I have %d timers\n", (int)Timers->n); + } + + return tref.id; +} + +int pico_stack_init(void) +{ + +#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 + + pico_stack_tick(); + pico_stack_tick(); + pico_stack_tick(); + return 0; +} + diff --git a/ext/picotcp/stack/pico_tree.c b/ext/picotcp/stack/pico_tree.c new file mode 100644 index 0000000..8af9845 --- /dev/null +++ b/ext/picotcp/stack/pico_tree.c @@ -0,0 +1,575 @@ +/********************************************************************* + PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved. + See LICENSE and COPYING 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); +} + +void *pico_tree_insert_implementation(struct pico_tree *tree, void *key, uint8_t allocator) +{ + struct pico_tree_node *last_node = INIT_LEAF; + struct pico_tree_node *temp = tree->root; + struct pico_tree_node *insert; + void *LocalKey; + int result = 0; + + LocalKey = (IS_NOT_LEAF(tree->root) ? pico_tree_findKey(tree, key) : NULL); + + /* if node already in, bail out */ + if(LocalKey) { + return LocalKey; + } + else + { + if(allocator == USE_PICO_PAGE0_ZALLOC) + insert = create_node(tree, key, USE_PICO_PAGE0_ZALLOC); + else + insert = create_node(tree, key, USE_PICO_ZALLOC); + + if(!insert) + { + pico_err = PICO_ERR_ENOMEM; + /* to let the user know that it couldn't insert */ + return (void *)&LEAF; + } + } + + /* 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; + } + + /* 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 = tree->root; + while(IS_NOT_LEAF(found)) + { + int result; + + result = tree->compare(found->keyValue, key); + if(result == 0) + return found->keyValue; + else if(result < 0) + found = found->rightChild; + else + found = found->leftChild; + + } + return NULL; +} + +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(node == node->parent->leftChild) { + 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; +} diff --git a/ext/picotcp/uncrustify.cfg b/ext/picotcp/uncrustify.cfg new file mode 100644 index 0000000..ba664df --- /dev/null +++ b/ext/picotcp/uncrustify.cfg @@ -0,0 +1,1579 @@ +# Uncrustify 0.60 + +# +# General options +# + +# The type of line endings +newlines = lf # auto/lf/crlf/cr + +# The original size of tabs in the input +input_tab_size = 4 # number + +# The size of tabs in the output (only used if align_with_tabs=true) +output_tab_size = 4 # number + +# The ASCII value of the string escape char, usually 92 (\) or 94 (^). (Pawn) +string_escape_char = 92 # number + +# Alternate string escape char for Pawn. Only works right before the quote char. +string_escape_char2 = 0 # number + +# Allow interpreting '>=' and '>>=' as part of a template in 'void f(list>=val);'. +# If true (default), 'assert(x<0 && y>=3)' will be broken. +# Improvements to template detection may make this option obsolete. +tok_split_gte = false # false/true + +# Control what to do with the UTF-8 BOM (recommend 'remove') +utf8_bom = ignore # ignore/add/remove/force + +# If the file contains bytes with values between 128 and 255, but is not UTF-8, then output as UTF-8 +utf8_byte = false # false/true + +# Force the output encoding to UTF-8 +utf8_force = false # false/true + +# +# Indenting +# + +# The number of columns to indent per level. +# Usually 2, 3, 4, or 8. +indent_columns = 4 # number + +# The continuation indent. If non-zero, this overrides the indent of '(' and '=' continuation indents. +# For FreeBSD, this is set to 4. Negative value is absolute and not increased for each ( level +indent_continue = 0 # number + +# How to use tabs when indenting code +# 0=spaces only +# 1=indent with tabs to brace level, align with spaces +# 2=indent and align with tabs, using spaces when not on a tabstop +indent_with_tabs = 0 # number + +# Comments that are not a brace level are indented with tabs on a tabstop. +# Requires indent_with_tabs=2. If false, will use spaces. +indent_cmt_with_tabs = false # false/true + +# Whether to indent strings broken by '\' so that they line up +indent_align_string = false # false/true + +# The number of spaces to indent multi-line XML strings. +# Requires indent_align_string=True +indent_xml_string = 0 # number + +# Spaces to indent '{' from level +indent_brace = 0 # number + +# Whether braces are indented to the body level +indent_braces = false # false/true + +# Disabled indenting function braces if indent_braces is true +indent_braces_no_func = false # false/true + +# Disabled indenting class braces if indent_braces is true +indent_braces_no_class = false # false/true + +# Disabled indenting struct braces if indent_braces is true +indent_braces_no_struct = false # false/true + +# Indent based on the size of the brace parent, i.e. 'if' => 3 spaces, 'for' => 4 spaces, etc. +indent_brace_parent = false # false/true + +# Whether the 'namespace' body is indented +indent_namespace = false # false/true + +# The number of spaces to indent a namespace block +indent_namespace_level = 0 # number + +# If the body of the namespace is longer than this number, it won't be indented. +# Requires indent_namespace=true. Default=0 (no limit) +indent_namespace_limit = 0 # number + +# Whether the 'extern "C"' body is indented +indent_extern = false # false/true + +# Whether the 'class' body is indented +indent_class = false # false/true + +# Whether to indent the stuff after a leading class colon +indent_class_colon = false # false/true + +# Virtual indent from the ':' for member initializers. Default is 2 +indent_ctor_init_leading = 2 # number + +# Additional indenting for constructor initializer list +indent_ctor_init = 0 # number + +# False=treat 'else\nif' as 'else if' for indenting purposes +# True=indent the 'if' one level +indent_else_if = false # false/true + +# Amount to indent variable declarations after a open brace. neg=relative, pos=absolute +indent_var_def_blk = 0 # number + +# Indent continued variable declarations instead of aligning. +indent_var_def_cont = false # false/true + +# True: force indentation of function definition to start in column 1 +# False: use the default behavior +indent_func_def_force_col1 = false # false/true + +# True: indent continued function call parameters one indent level +# False: align parameters under the open paren +indent_func_call_param = false # false/true + +# Same as indent_func_call_param, but for function defs +indent_func_def_param = false # false/true + +# Same as indent_func_call_param, but for function protos +indent_func_proto_param = false # false/true + +# Same as indent_func_call_param, but for class declarations +indent_func_class_param = false # false/true + +# Same as indent_func_call_param, but for class variable constructors +indent_func_ctor_var_param = false # false/true + +# Same as indent_func_call_param, but for templates +indent_template_param = false # false/true + +# Double the indent for indent_func_xxx_param options +indent_func_param_double = false # false/true + +# Indentation column for standalone 'const' function decl/proto qualifier +indent_func_const = 0 # number + +# Indentation column for standalone 'throw' function decl/proto qualifier +indent_func_throw = 0 # number + +# The number of spaces to indent a continued '->' or '.' +# Usually set to 0, 1, or indent_columns. +indent_member = 0 # number + +# Spaces to indent single line ('//') comments on lines before code +indent_sing_line_comments = 0 # number + +# If set, will indent trailing single line ('//') comments relative +# to the code instead of trying to keep the same absolute column +indent_relative_single_line_comments = false # false/true + +# Spaces to indent 'case' from 'switch' +# Usually 0 or indent_columns. +indent_switch_case = 0 # number + +# Spaces to shift the 'case' line, without affecting any other lines +# Usually 0. +indent_case_shift = 0 # number + +# Spaces to indent '{' from 'case'. +# By default, the brace will appear under the 'c' in case. +# Usually set to 0 or indent_columns. +indent_case_brace = 0 # number + +# Whether to indent comments found in first column +indent_col1_comment = false # false/true + +# How to indent goto labels +# >0 : absolute column where 1 is the leftmost column +# <=0 : subtract from brace indent +indent_label = 1 # number + +# Same as indent_label, but for access specifiers that are followed by a colon +indent_access_spec = 1 # number + +# Indent the code after an access specifier by one level. +# If set, this option forces 'indent_access_spec=0' +indent_access_spec_body = false # false/true + +# If an open paren is followed by a newline, indent the next line so that it lines up after the open paren (not recommended) +indent_paren_nl = false # false/true + +# Controls the indent of a close paren after a newline. +# 0: Indent to body level +# 1: Align under the open paren +# 2: Indent to the brace level +indent_paren_close = 0 # number + +# Controls the indent of a comma when inside a paren.If TRUE, aligns under the open paren +indent_comma_paren = false # false/true + +# Controls the indent of a BOOL operator when inside a paren.If TRUE, aligns under the open paren +indent_bool_paren = false # false/true + +# If 'indent_bool_paren' is true, controls the indent of the first expression. If TRUE, aligns the first expression to the following ones +indent_first_bool_expr = false # false/true + +# If an open square is followed by a newline, indent the next line so that it lines up after the open square (not recommended) +indent_square_nl = false # false/true + +# Don't change the relative indent of ESQL/C 'EXEC SQL' bodies +indent_preserve_sql = false # false/true + +# Align continued statements at the '='. Default=True +# If FALSE or the '=' is followed by a newline, the next line is indent one tab. +indent_align_assign = true # false/true + +# Indent OC blocks at brace level instead of usual rules. +indent_oc_block = false # false/true + +# Indent OC blocks in a message relative to the parameter name. +# 0=use indent_oc_block rules, 1+=spaces to indent +indent_oc_block_msg = 0 # number + +# Minimum indent for subsequent parameters +indent_oc_msg_colon = 0 # number + +# +# Spacing options +# + +# Add or remove space around arithmetic operator '+', '-', '/', '*', etc +sp_arith = add # ignore/add/remove/force + +# Add or remove space around assignment operator '=', '+=', etc +sp_assign = add # ignore/add/remove/force + +# Add or remove space around '=' in C++11 lambda capture specifications. Overrides sp_assign +sp_cpp_lambda_assign = ignore # ignore/add/remove/force + +# Add or remove space after the capture specification in C++11 lambda. +sp_cpp_lambda_paren = ignore # ignore/add/remove/force + +# Add or remove space around assignment operator '=' in a prototype +sp_assign_default = add # ignore/add/remove/force + +# Add or remove space before assignment operator '=', '+=', etc. Overrides sp_assign. +sp_before_assign = add # ignore/add/remove/force + +# Add or remove space after assignment operator '=', '+=', etc. Overrides sp_assign. +sp_after_assign = add # ignore/add/remove/force + +# Add or remove space around assignment '=' in enum +sp_enum_assign = add # ignore/add/remove/force + +# Add or remove space before assignment '=' in enum. Overrides sp_enum_assign. +sp_enum_before_assign = add # ignore/add/remove/force + +# Add or remove space after assignment '=' in enum. Overrides sp_enum_assign. +sp_enum_after_assign = add # ignore/add/remove/force + +# Add or remove space around preprocessor '##' concatenation operator. Default=Add +sp_pp_concat = add # ignore/add/remove/force + +# Add or remove space after preprocessor '#' stringify operator. Also affects the '#@' charizing operator. +sp_pp_stringify = ignore # ignore/add/remove/force + +# Add or remove space before preprocessor '#' stringify operator as in '#define x(y) L#y'. +sp_before_pp_stringify = ignore # ignore/add/remove/force + +# Add or remove space around boolean operators '&&' and '||' +sp_bool = add # ignore/add/remove/force + +# Add or remove space around compare operator '<', '>', '==', etc +sp_compare = add # ignore/add/remove/force + +# Add or remove space inside '(' and ')' +sp_inside_paren = remove # ignore/add/remove/force + +# Add or remove space between nested parens +sp_paren_paren = remove # ignore/add/remove/force + +# Whether to balance spaces inside nested parens +sp_balance_nested_parens = false # false/true + +# Add or remove space between ')' and '{' +sp_paren_brace = ignore # ignore/add/remove/force + +# Add or remove space before pointer star '*' +sp_before_ptr_star = ignore # ignore/add/remove/force + +# Add or remove space before pointer star '*' that isn't followed by a variable name +# If set to 'ignore', sp_before_ptr_star is used instead. +sp_before_unnamed_ptr_star = ignore # ignore/add/remove/force + +# Add or remove space between pointer stars '*' +sp_between_ptr_star = ignore # ignore/add/remove/force + +# Add or remove space after pointer star '*', if followed by a word. +sp_after_ptr_star = remove # ignore/add/remove/force + +# Add or remove space after a pointer star '*', if followed by a func proto/def. +sp_after_ptr_star_func = ignore # ignore/add/remove/force + +# Add or remove space after a pointer star '*', if followed by an open paren (function types). +sp_ptr_star_paren = ignore # ignore/add/remove/force + +# Add or remove space before a pointer star '*', if followed by a func proto/def. +sp_before_ptr_star_func = ignore # ignore/add/remove/force + +# Add or remove space before a reference sign '&' +sp_before_byref = ignore # ignore/add/remove/force + +# Add or remove space before a reference sign '&' that isn't followed by a variable name +# If set to 'ignore', sp_before_byref is used instead. +sp_before_unnamed_byref = ignore # ignore/add/remove/force + +# Add or remove space after reference sign '&', if followed by a word. +sp_after_byref = remove # ignore/add/remove/force + +# Add or remove space after a reference sign '&', if followed by a func proto/def. +sp_after_byref_func = remove # ignore/add/remove/force + +# Add or remove space before a reference sign '&', if followed by a func proto/def. +sp_before_byref_func = ignore # ignore/add/remove/force + +# Add or remove space between type and word. Default=Force +sp_after_type = force # ignore/add/remove/force + +# Add or remove space before the paren in the D constructs 'template Foo(' and 'class Foo('. +sp_before_template_paren = ignore # ignore/add/remove/force + +# Add or remove space in 'template <' vs 'template<'. +# If set to ignore, sp_before_angle is used. +sp_template_angle = ignore # ignore/add/remove/force + +# Add or remove space before '<>' +sp_before_angle = ignore # ignore/add/remove/force + +# Add or remove space inside '<' and '>' +sp_inside_angle = ignore # ignore/add/remove/force + +# Add or remove space after '<>' +sp_after_angle = ignore # ignore/add/remove/force + +# Add or remove space between '<>' and '(' as found in 'new List();' +sp_angle_paren = ignore # ignore/add/remove/force + +# Add or remove space between '<>' and a word as in 'List m;' +sp_angle_word = ignore # ignore/add/remove/force + +# Add or remove space between '>' and '>' in '>>' (template stuff C++/C# only). Default=Add +sp_angle_shift = add # ignore/add/remove/force + +# Permit removal of the space between '>>' in 'foo >' (C++11 only). Default=False +# sp_angle_shift cannot remove the space without this option. +sp_permit_cpp11_shift = false # false/true + +# Add or remove space before '(' of 'if', 'for', 'switch', and 'while' +sp_before_sparen = ignore # ignore/add/remove/force + +# Add or remove space inside if-condition '(' and ')' +sp_inside_sparen = ignore # ignore/add/remove/force + +# Add or remove space before if-condition ')'. Overrides sp_inside_sparen. +sp_inside_sparen_close = ignore # ignore/add/remove/force + +# Add or remove space before if-condition '('. Overrides sp_inside_sparen. +sp_inside_sparen_open = ignore # ignore/add/remove/force + +# Add or remove space after ')' of 'if', 'for', 'switch', and 'while' +sp_after_sparen = ignore # ignore/add/remove/force + +# Add or remove space between ')' and '{' of 'if', 'for', 'switch', and 'while' +sp_sparen_brace = ignore # ignore/add/remove/force + +# Add or remove space between 'invariant' and '(' in the D language. +sp_invariant_paren = ignore # ignore/add/remove/force + +# Add or remove space after the ')' in 'invariant (C) c' in the D language. +sp_after_invariant_paren = ignore # ignore/add/remove/force + +# Add or remove space before empty statement ';' on 'if', 'for' and 'while' +sp_special_semi = ignore # ignore/add/remove/force + +# Add or remove space before ';'. Default=Remove +sp_before_semi = remove # ignore/add/remove/force + +# Add or remove space before ';' in non-empty 'for' statements +sp_before_semi_for = ignore # ignore/add/remove/force + +# Add or remove space before a semicolon of an empty part of a for statement. +sp_before_semi_for_empty = ignore # ignore/add/remove/force + +# Add or remove space after ';', except when followed by a comment. Default=Add +sp_after_semi = remove # ignore/add/remove/force + +# Add or remove space after ';' in non-empty 'for' statements. Default=Force +sp_after_semi_for = force # ignore/add/remove/force + +# Add or remove space after the final semicolon of an empty part of a for statement: for ( ; ; ). +sp_after_semi_for_empty = ignore # ignore/add/remove/force + +# Add or remove space before '[' (except '[]') +sp_before_square = ignore # ignore/add/remove/force + +# Add or remove space before '[]' +sp_before_squares = ignore # ignore/add/remove/force + +# Add or remove space inside a non-empty '[' and ']' +sp_inside_square = ignore # ignore/add/remove/force + +# Add or remove space after ',' +sp_after_comma = add # ignore/add/remove/force + +# Add or remove space before ',' +sp_before_comma = remove # ignore/add/remove/force + +# Add or remove space between an open paren and comma: '(,' vs '( ,' +sp_paren_comma = force # ignore/add/remove/force + +# Add or remove space before the variadic '...' when preceded by a non-punctuator +sp_before_ellipsis = ignore # ignore/add/remove/force + +# Add or remove space after class ':' +sp_after_class_colon = ignore # ignore/add/remove/force + +# Add or remove space before class ':' +sp_before_class_colon = ignore # ignore/add/remove/force + +# Add or remove space before case ':'. Default=Remove +sp_before_case_colon = remove # ignore/add/remove/force + +# Add or remove space between 'operator' and operator sign +sp_after_operator = ignore # ignore/add/remove/force + +# Add or remove space between the operator symbol and the open paren, as in 'operator ++(' +sp_after_operator_sym = ignore # ignore/add/remove/force + +# Add or remove space after C/D cast, i.e. 'cast(int)a' vs 'cast(int) a' or '(int)a' vs '(int) a' +sp_after_cast = ignore # ignore/add/remove/force + +# Add or remove spaces inside cast parens +sp_inside_paren_cast = ignore # ignore/add/remove/force + +# Add or remove space between the type and open paren in a C++ cast, i.e. 'int(exp)' vs 'int (exp)' +sp_cpp_cast_paren = ignore # ignore/add/remove/force + +# Add or remove space between 'sizeof' and '(' +sp_sizeof_paren = ignore # ignore/add/remove/force + +# Add or remove space after the tag keyword (Pawn) +sp_after_tag = ignore # ignore/add/remove/force + +# Add or remove space inside enum '{' and '}' +sp_inside_braces_enum = ignore # ignore/add/remove/force + +# Add or remove space inside struct/union '{' and '}' +sp_inside_braces_struct = ignore # ignore/add/remove/force + +# Add or remove space inside '{' and '}' +sp_inside_braces = ignore # ignore/add/remove/force + +# Add or remove space inside '{}' +sp_inside_braces_empty = ignore # ignore/add/remove/force + +# Add or remove space between return type and function name +# A minimum of 1 is forced except for pointer return types. +sp_type_func = ignore # ignore/add/remove/force + +# Add or remove space between function name and '(' on function declaration +sp_func_proto_paren = ignore # ignore/add/remove/force + +# Add or remove space between function name and '(' on function definition +sp_func_def_paren = ignore # ignore/add/remove/force + +# Add or remove space inside empty function '()' +sp_inside_fparens = ignore # ignore/add/remove/force + +# Add or remove space inside function '(' and ')' +sp_inside_fparen = ignore # ignore/add/remove/force + +# Add or remove space inside the first parens in the function type: 'void (*x)(...)' +sp_inside_tparen = ignore # ignore/add/remove/force + +# Add or remove between the parens in the function type: 'void (*x)(...)' +sp_after_tparen_close = ignore # ignore/add/remove/force + +# Add or remove space between ']' and '(' when part of a function call. +sp_square_fparen = ignore # ignore/add/remove/force + +# Add or remove space between ')' and '{' of function +sp_fparen_brace = ignore # ignore/add/remove/force + +# Add or remove space between function name and '(' on function calls +sp_func_call_paren = ignore # ignore/add/remove/force + +# Add or remove space between function name and '()' on function calls without parameters. +# If set to 'ignore' (the default), sp_func_call_paren is used. +sp_func_call_paren_empty = ignore # ignore/add/remove/force + +# Add or remove space between the user function name and '(' on function calls +# You need to set a keyword to be a user function, like this: 'set func_call_user _' in the config file. +sp_func_call_user_paren = ignore # ignore/add/remove/force + +# Add or remove space between a constructor/destructor and the open paren +sp_func_class_paren = ignore # ignore/add/remove/force + +# Add or remove space between 'return' and '(' +sp_return_paren = ignore # ignore/add/remove/force + +# Add or remove space between '__attribute__' and '(' +sp_attribute_paren = ignore # ignore/add/remove/force + +# Add or remove space between 'defined' and '(' in '#if defined (FOO)' +sp_defined_paren = ignore # ignore/add/remove/force + +# Add or remove space between 'throw' and '(' in 'throw (something)' +sp_throw_paren = ignore # ignore/add/remove/force + +# Add or remove space between 'throw' and anything other than '(' as in '@throw [...];' +sp_after_throw = ignore # ignore/add/remove/force + +# Add or remove space between 'catch' and '(' in 'catch (something) { }' +# If set to ignore, sp_before_sparen is used. +sp_catch_paren = ignore # ignore/add/remove/force + +# Add or remove space between 'version' and '(' in 'version (something) { }' (D language) +# If set to ignore, sp_before_sparen is used. +sp_version_paren = ignore # ignore/add/remove/force + +# Add or remove space between 'scope' and '(' in 'scope (something) { }' (D language) +# If set to ignore, sp_before_sparen is used. +sp_scope_paren = ignore # ignore/add/remove/force + +# Add or remove space between macro and value +sp_macro = ignore # ignore/add/remove/force + +# Add or remove space between macro function ')' and value +sp_macro_func = ignore # ignore/add/remove/force + +# Add or remove space between 'else' and '{' if on the same line +sp_else_brace = ignore # ignore/add/remove/force + +# Add or remove space between '}' and 'else' if on the same line +sp_brace_else = ignore # ignore/add/remove/force + +# Add or remove space between '}' and the name of a typedef on the same line +sp_brace_typedef = ignore # ignore/add/remove/force + +# Add or remove space between 'catch' and '{' if on the same line +sp_catch_brace = ignore # ignore/add/remove/force + +# Add or remove space between '}' and 'catch' if on the same line +sp_brace_catch = ignore # ignore/add/remove/force + +# Add or remove space between 'finally' and '{' if on the same line +sp_finally_brace = ignore # ignore/add/remove/force + +# Add or remove space between '}' and 'finally' if on the same line +sp_brace_finally = ignore # ignore/add/remove/force + +# Add or remove space between 'try' and '{' if on the same line +sp_try_brace = ignore # ignore/add/remove/force + +# Add or remove space between get/set and '{' if on the same line +sp_getset_brace = ignore # ignore/add/remove/force + +# Add or remove space before the '::' operator +sp_before_dc = ignore # ignore/add/remove/force + +# Add or remove space after the '::' operator +sp_after_dc = ignore # ignore/add/remove/force + +# Add or remove around the D named array initializer ':' operator +sp_d_array_colon = ignore # ignore/add/remove/force + +# Add or remove space after the '!' (not) operator. Default=Remove +sp_not = remove # ignore/add/remove/force + +# Add or remove space after the '~' (invert) operator. Default=Remove +sp_inv = remove # ignore/add/remove/force + +# Add or remove space after the '&' (address-of) operator. Default=Remove +# This does not affect the spacing after a '&' that is part of a type. +sp_addr = remove # ignore/add/remove/force + +# Add or remove space around the '.' or '->' operators. Default=Remove +sp_member = remove # ignore/add/remove/force + +# Add or remove space after the '*' (dereference) operator. Default=Remove +# This does not affect the spacing after a '*' that is part of a type. +sp_deref = remove # ignore/add/remove/force + +# Add or remove space after '+' or '-', as in 'x = -5' or 'y = +7'. Default=Remove +sp_sign = remove # ignore/add/remove/force + +# Add or remove space before or after '++' and '--', as in '(--x)' or 'y++;'. Default=Remove +sp_incdec = remove # ignore/add/remove/force + +# Add or remove space before a backslash-newline at the end of a line. Default=Add +sp_before_nl_cont = add # ignore/add/remove/force + +# Add or remove space after the scope '+' or '-', as in '-(void) foo;' or '+(int) bar;' +sp_after_oc_scope = ignore # ignore/add/remove/force + +# Add or remove space after the colon in message specs +# '-(int) f:(int) x;' vs '-(int) f: (int) x;' +sp_after_oc_colon = ignore # ignore/add/remove/force + +# Add or remove space before the colon in message specs +# '-(int) f: (int) x;' vs '-(int) f : (int) x;' +sp_before_oc_colon = ignore # ignore/add/remove/force + +# Add or remove space after the colon in immutable dictionary expression +# 'NSDictionary *test = @{@"foo" :@"bar"};' +sp_after_oc_dict_colon = ignore # ignore/add/remove/force + +# Add or remove space before the colon in immutable dictionary expression +# 'NSDictionary *test = @{@"foo" :@"bar"};' +sp_before_oc_dict_colon = ignore # ignore/add/remove/force + +# Add or remove space after the colon in message specs +# '[object setValue:1];' vs '[object setValue: 1];' +sp_after_send_oc_colon = ignore # ignore/add/remove/force + +# Add or remove space before the colon in message specs +# '[object setValue:1];' vs '[object setValue :1];' +sp_before_send_oc_colon = ignore # ignore/add/remove/force + +# Add or remove space after the (type) in message specs +# '-(int)f: (int) x;' vs '-(int)f: (int)x;' +sp_after_oc_type = ignore # ignore/add/remove/force + +# Add or remove space after the first (type) in message specs +# '-(int) f:(int)x;' vs '-(int)f:(int)x;' +sp_after_oc_return_type = ignore # ignore/add/remove/force + +# Add or remove space between '@selector' and '(' +# '@selector(msgName)' vs '@selector (msgName)' +# Also applies to @protocol() constructs +sp_after_oc_at_sel = ignore # ignore/add/remove/force + +# Add or remove space between '@selector(x)' and the following word +# '@selector(foo) a:' vs '@selector(foo)a:' +sp_after_oc_at_sel_parens = ignore # ignore/add/remove/force + +# Add or remove space inside '@selector' parens +# '@selector(foo)' vs '@selector( foo )' +# Also applies to @protocol() constructs +sp_inside_oc_at_sel_parens = ignore # ignore/add/remove/force + +# Add or remove space before a block pointer caret +# '^int (int arg){...}' vs. ' ^int (int arg){...}' +sp_before_oc_block_caret = ignore # ignore/add/remove/force + +# Add or remove space after a block pointer caret +# '^int (int arg){...}' vs. '^ int (int arg){...}' +sp_after_oc_block_caret = ignore # ignore/add/remove/force + +# Add or remove space between the receiver and selector in a message. +# '[receiver selector ...]' +sp_after_oc_msg_receiver = ignore # ignore/add/remove/force + +# Add or remove space after @property. +sp_after_oc_property = ignore # ignore/add/remove/force + +# Add or remove space around the ':' in 'b ? t : f' +sp_cond_colon = ignore # ignore/add/remove/force + +# Add or remove space around the '?' in 'b ? t : f' +sp_cond_question = ignore # ignore/add/remove/force + +# Fix the spacing between 'case' and the label. Only 'ignore' and 'force' make sense here. +sp_case_label = ignore # ignore/add/remove/force + +# Control the space around the D '..' operator. +sp_range = ignore # ignore/add/remove/force + +# Control the spacing after ':' in 'for (TYPE VAR : EXPR)' (Java) +sp_after_for_colon = ignore # ignore/add/remove/force + +# Control the spacing before ':' in 'for (TYPE VAR : EXPR)' (Java) +sp_before_for_colon = ignore # ignore/add/remove/force + +# Control the spacing in 'extern (C)' (D) +sp_extern_paren = ignore # ignore/add/remove/force + +# Control the space after the opening of a C++ comment '// A' vs '//A' +sp_cmt_cpp_start = ignore # ignore/add/remove/force + +# Controls the spaces between #else or #endif and a trailing comment +sp_endif_cmt = ignore # ignore/add/remove/force + +# Controls the spaces after 'new', 'delete', and 'delete[]' +sp_after_new = ignore # ignore/add/remove/force + +# Controls the spaces before a trailing or embedded comment +sp_before_tr_emb_cmt = ignore # ignore/add/remove/force + +# Number of spaces before a trailing or embedded comment +sp_num_before_tr_emb_cmt = 0 # number + +# Control space between a Java annotation and the open paren. +sp_annotation_paren = ignore # ignore/add/remove/force + +# +# Code alignment (not left column spaces/tabs) +# + +# Whether to keep non-indenting tabs +align_keep_tabs = false # false/true + +# Whether to use tabs for aligning +align_with_tabs = false # false/true + +# Whether to bump out to the next tab when aligning +align_on_tabstop = false # false/true + +# Whether to left-align numbers +align_number_left = false # false/true + +# Align variable definitions in prototypes and functions +align_func_params = false # false/true + +# Align parameters in single-line functions that have the same name. +# The function names must already be aligned with each other. +align_same_func_call_params = false # false/true + +# The span for aligning variable definitions (0=don't align) +align_var_def_span = 0 # number + +# How to align the star in variable definitions. +# 0=Part of the type 'void * foo;' +# 1=Part of the variable 'void *foo;' +# 2=Dangling 'void *foo;' +align_var_def_star_style = 1 # number + +# How to align the '&' in variable definitions. +# 0=Part of the type +# 1=Part of the variable +# 2=Dangling +align_var_def_amp_style = 1 # number + +# The threshold for aligning variable definitions (0=no limit) +align_var_def_thresh = 0 # number + +# The gap for aligning variable definitions +align_var_def_gap = 0 # number + +# Whether to align the colon in struct bit fields +align_var_def_colon = false # false/true + +# Whether to align any attribute after the variable name +align_var_def_attribute = false # false/true + +# Whether to align inline struct/enum/union variable definitions +align_var_def_inline = false # false/true + +# The span for aligning on '=' in assignments (0=don't align) +align_assign_span = 0 # number + +# The threshold for aligning on '=' in assignments (0=no limit) +align_assign_thresh = 0 # number + +# The span for aligning on '=' in enums (0=don't align) +align_enum_equ_span = 0 # number + +# The threshold for aligning on '=' in enums (0=no limit) +align_enum_equ_thresh = 0 # number + +# The span for aligning struct/union (0=don't align) +align_var_struct_span = 0 # number + +# The threshold for aligning struct/union member definitions (0=no limit) +align_var_struct_thresh = 0 # number + +# The gap for aligning struct/union member definitions +align_var_struct_gap = 0 # number + +# The span for aligning struct initializer values (0=don't align) +align_struct_init_span = 0 # number + +# The minimum space between the type and the synonym of a typedef +align_typedef_gap = 0 # number + +# The span for aligning single-line typedefs (0=don't align) +align_typedef_span = 0 # number + +# How to align typedef'd functions with other typedefs +# 0: Don't mix them at all +# 1: align the open paren with the types +# 2: align the function type name with the other type names +align_typedef_func = 0 # number + +# Controls the positioning of the '*' in typedefs. Just try it. +# 0: Align on typedef type, ignore '*' +# 1: The '*' is part of type name: typedef int *pint; +# 2: The '*' is part of the type, but dangling: typedef int *pint; +align_typedef_star_style = 0 # number + +# Controls the positioning of the '&' in typedefs. Just try it. +# 0: Align on typedef type, ignore '&' +# 1: The '&' is part of type name: typedef int &pint; +# 2: The '&' is part of the type, but dangling: typedef int &pint; +align_typedef_amp_style = 0 # number + +# The span for aligning comments that end lines (0=don't align) +align_right_cmt_span = 0 # number + +# If aligning comments, mix with comments after '}' and #endif with less than 3 spaces before the comment +align_right_cmt_mix = false # false/true + +# If a trailing comment is more than this number of columns away from the text it follows, +# it will qualify for being aligned. This has to be > 0 to do anything. +align_right_cmt_gap = 0 # number + +# Align trailing comment at or beyond column N; 'pulls in' comments as a bonus side effect (0=ignore) +align_right_cmt_at_col = 0 # number + +# The span for aligning function prototypes (0=don't align) +align_func_proto_span = 0 # number + +# Minimum gap between the return type and the function name. +align_func_proto_gap = 0 # number + +# Align function protos on the 'operator' keyword instead of what follows +align_on_operator = false # false/true + +# Whether to mix aligning prototype and variable declarations. +# If true, align_var_def_XXX options are used instead of align_func_proto_XXX options. +align_mix_var_proto = false # false/true + +# Align single-line functions with function prototypes, uses align_func_proto_span +align_single_line_func = false # false/true + +# Aligning the open brace of single-line functions. +# Requires align_single_line_func=true, uses align_func_proto_span +align_single_line_brace = false # false/true + +# Gap for align_single_line_brace. +align_single_line_brace_gap = 0 # number + +# The span for aligning ObjC msg spec (0=don't align) +align_oc_msg_spec_span = 0 # number + +# Whether to align macros wrapped with a backslash and a newline. +# This will not work right if the macro contains a multi-line comment. +align_nl_cont = false # false/true + +# # Align macro functions and variables together +align_pp_define_together = false # false/true + +# The minimum space between label and value of a preprocessor define +align_pp_define_gap = 0 # number + +# The span for aligning on '#define' bodies (0=don't align) +align_pp_define_span = 0 # number + +# Align lines that start with '<<' with previous '<<'. Default=true +align_left_shift = true # false/true + +# Span for aligning parameters in an Obj-C message call on the ':' (0=don't align) +align_oc_msg_colon_span = 0 # number + +# If true, always align with the first parameter, even if it is too short. +align_oc_msg_colon_first = false # false/true + +# Aligning parameters in an Obj-C '+' or '-' declaration on the ':' +align_oc_decl_colon = false # false/true + +# +# Newline adding and removing options +# + +# Whether to collapse empty blocks between '{' and '}' +nl_collapse_empty_body = false # false/true + +# Don't split one-line braced assignments - 'foo_t f = { 1, 2 };' +nl_assign_leave_one_liners = false # false/true + +# Don't split one-line braced statements inside a class xx { } body +nl_class_leave_one_liners = false # false/true + +# Don't split one-line enums: 'enum foo { BAR = 15 };' +nl_enum_leave_one_liners = false # false/true + +# Don't split one-line get or set functions +nl_getset_leave_one_liners = false # false/true + +# Don't split one-line function definitions - 'int foo() { return 0; }' +nl_func_leave_one_liners = false # false/true + +# Don't split one-line if/else statements - 'if(a) b++;' +nl_if_leave_one_liners = false # false/true + +# Don't split one-line OC messages +nl_oc_msg_leave_one_liner = false # false/true + +# Add or remove newlines at the start of the file +nl_start_of_file = ignore # ignore/add/remove/force + +# The number of newlines at the start of the file (only used if nl_start_of_file is 'add' or 'force' +nl_start_of_file_min = 0 # number + +# Add or remove newline at the end of the file +nl_end_of_file = ignore # ignore/add/remove/force + +# The number of newlines at the end of the file (only used if nl_end_of_file is 'add' or 'force') +nl_end_of_file_min = 0 # number + +# Add or remove newline between '=' and '{' +nl_assign_brace = ignore # ignore/add/remove/force + +# Add or remove newline between '=' and '[' (D only) +nl_assign_square = ignore # ignore/add/remove/force + +# Add or remove newline after '= [' (D only). Will also affect the newline before the ']' +nl_after_square_assign = ignore # ignore/add/remove/force + +# The number of blank lines after a block of variable definitions at the top of a function body +# 0 = No change (default) +nl_func_var_def_blk = 0 # number + +# The number of newlines before a block of typedefs +# 0 = No change (default) +nl_typedef_blk_start = 0 # number + +# The number of newlines after a block of typedefs +# 0 = No change (default) +nl_typedef_blk_end = 0 # number + +# The maximum consecutive newlines within a block of typedefs +# 0 = No change (default) +nl_typedef_blk_in = 0 # number + +# The number of newlines before a block of variable definitions not at the top of a function body +# 0 = No change (default) +nl_var_def_blk_start = 0 # number + +# The number of newlines after a block of variable definitions not at the top of a function body +# 0 = No change (default) +nl_var_def_blk_end = 0 # number + +# The maximum consecutive newlines within a block of variable definitions +# 0 = No change (default) +nl_var_def_blk_in = 0 # number + +# Add or remove newline between a function call's ')' and '{', as in: +# list_for_each(item, &list) { } +nl_fcall_brace = ignore # ignore/add/remove/force + +# Add or remove newline between 'enum' and '{' +nl_enum_brace = ignore # ignore/add/remove/force + +# Add or remove newline between 'struct and '{' +nl_struct_brace = ignore # ignore/add/remove/force + +# Add or remove newline between 'union' and '{' +nl_union_brace = ignore # ignore/add/remove/force + +# Add or remove newline between 'if' and '{' +nl_if_brace = ignore # ignore/add/remove/force + +# Add or remove newline between '}' and 'else' +nl_brace_else = ignore # ignore/add/remove/force + +# Add or remove newline between 'else if' and '{' +# If set to ignore, nl_if_brace is used instead +nl_elseif_brace = ignore # ignore/add/remove/force + +# Add or remove newline between 'else' and '{' +nl_else_brace = ignore # ignore/add/remove/force + +# Add or remove newline between 'else' and 'if' +nl_else_if = ignore # ignore/add/remove/force + +# Add or remove newline between '}' and 'finally' +nl_brace_finally = ignore # ignore/add/remove/force + +# Add or remove newline between 'finally' and '{' +nl_finally_brace = ignore # ignore/add/remove/force + +# Add or remove newline between 'try' and '{' +nl_try_brace = ignore # ignore/add/remove/force + +# Add or remove newline between get/set and '{' +nl_getset_brace = ignore # ignore/add/remove/force + +# Add or remove newline between 'for' and '{' +nl_for_brace = ignore # ignore/add/remove/force + +# Add or remove newline between 'catch' and '{' +nl_catch_brace = ignore # ignore/add/remove/force + +# Add or remove newline between '}' and 'catch' +nl_brace_catch = ignore # ignore/add/remove/force + +# Add or remove newline between 'while' and '{' +nl_while_brace = ignore # ignore/add/remove/force + +# Add or remove newline between 'scope (x)' and '{' (D) +nl_scope_brace = ignore # ignore/add/remove/force + +# Add or remove newline between 'unittest' and '{' (D) +nl_unittest_brace = ignore # ignore/add/remove/force + +# Add or remove newline between 'version (x)' and '{' (D) +nl_version_brace = ignore # ignore/add/remove/force + +# Add or remove newline between 'using' and '{' +nl_using_brace = ignore # ignore/add/remove/force + +# Add or remove newline between two open or close braces. +# Due to general newline/brace handling, REMOVE may not work. +nl_brace_brace = ignore # ignore/add/remove/force + +# Add or remove newline between 'do' and '{' +nl_do_brace = ignore # ignore/add/remove/force + +# Add or remove newline between '}' and 'while' of 'do' statement +nl_brace_while = ignore # ignore/add/remove/force + +# Add or remove newline between 'switch' and '{' +nl_switch_brace = ignore # ignore/add/remove/force + +# Add a newline between ')' and '{' if the ')' is on a different line than the if/for/etc. +# Overrides nl_for_brace, nl_if_brace, nl_switch_brace, nl_while_switch, and nl_catch_brace. +nl_multi_line_cond = false # false/true + +# Force a newline in a define after the macro name for multi-line defines. +nl_multi_line_define = false # false/true + +# Whether to put a newline before 'case' statement +nl_before_case = false # false/true + +# Add or remove newline between ')' and 'throw' +nl_before_throw = ignore # ignore/add/remove/force + +# Whether to put a newline after 'case' statement +nl_after_case = false # false/true + +# Add or remove a newline between a case ':' and '{'. Overrides nl_after_case. +nl_case_colon_brace = ignore # ignore/add/remove/force + +# Newline between namespace and { +nl_namespace_brace = ignore # ignore/add/remove/force + +# Add or remove newline between 'template<>' and whatever follows. +nl_template_class = ignore # ignore/add/remove/force + +# Add or remove newline between 'class' and '{' +nl_class_brace = ignore # ignore/add/remove/force + +# Add or remove newline after each ',' in the constructor member initialization +nl_class_init_args = ignore # ignore/add/remove/force + +# Add or remove newline between return type and function name in a function definition +nl_func_type_name = ignore # ignore/add/remove/force + +# Add or remove newline between return type and function name inside a class {} +# Uses nl_func_type_name or nl_func_proto_type_name if set to ignore. +nl_func_type_name_class = ignore # ignore/add/remove/force + +# Add or remove newline between function scope and name in a definition +# Controls the newline after '::' in 'void A::f() { }' +nl_func_scope_name = ignore # ignore/add/remove/force + +# Add or remove newline between return type and function name in a prototype +nl_func_proto_type_name = ignore # ignore/add/remove/force + +# Add or remove newline between a function name and the opening '(' +nl_func_paren = remove # ignore/add/remove/force + +# Add or remove newline between a function name and the opening '(' in the definition +nl_func_def_paren = remove # ignore/add/remove/force + +# Add or remove newline after '(' in a function declaration +nl_func_decl_start = remove # ignore/add/remove/force + +# Add or remove newline after '(' in a function definition +nl_func_def_start = remove # ignore/add/remove/force + +# Overrides nl_func_decl_start when there is only one parameter. +nl_func_decl_start_single = ignore # ignore/add/remove/force + +# Overrides nl_func_def_start when there is only one parameter. +nl_func_def_start_single = ignore # ignore/add/remove/force + +# Add or remove newline after each ',' in a function declaration +nl_func_decl_args = ignore # ignore/add/remove/force + +# Add or remove newline after each ',' in a function definition +nl_func_def_args = ignore # ignore/add/remove/force + +# Add or remove newline before the ')' in a function declaration +nl_func_decl_end = ignore # ignore/add/remove/force + +# Add or remove newline before the ')' in a function definition +nl_func_def_end = remove # ignore/add/remove/force + +# Overrides nl_func_decl_end when there is only one parameter. +nl_func_decl_end_single = ignore # ignore/add/remove/force + +# Overrides nl_func_def_end when there is only one parameter. +nl_func_def_end_single = ignore # ignore/add/remove/force + +# Add or remove newline between '()' in a function declaration. +nl_func_decl_empty = ignore # ignore/add/remove/force + +# Add or remove newline between '()' in a function definition. +nl_func_def_empty = ignore # ignore/add/remove/force + +# Whether to put each OC message parameter on a separate line +# See nl_oc_msg_leave_one_liner +nl_oc_msg_args = false # false/true + +# Add or remove newline between function signature and '{' +nl_fdef_brace = add # ignore/add/remove/force + +# Add or remove a newline between the return keyword and return expression. +nl_return_expr = ignore # ignore/add/remove/force + +# Whether to put a newline after semicolons, except in 'for' statements +nl_after_semicolon = true # false/true + +# Whether to put a newline after brace open. +# This also adds a newline before the matching brace close. +nl_after_brace_open = true # false/true + +# If nl_after_brace_open and nl_after_brace_open_cmt are true, a newline is +# placed between the open brace and a trailing single-line comment. +nl_after_brace_open_cmt = false # false/true + +# Whether to put a newline after a virtual brace open with a non-empty body. +# These occur in un-braced if/while/do/for statement bodies. +nl_after_vbrace_open = false # false/true + +# Whether to put a newline after a virtual brace open with an empty body. +# These occur in un-braced if/while/do/for statement bodies. +nl_after_vbrace_open_empty = false # false/true + +# Whether to put a newline after a brace close. +# Does not apply if followed by a necessary ';'. +nl_after_brace_close = false # false/true + +# Whether to put a newline after a virtual brace close. +# Would add a newline before return in: 'if (foo) a++; return;' +nl_after_vbrace_close = false # false/true + +# Control the newline between the close brace and 'b' in: 'struct { int a; } b;' +# Affects enums, unions, and structures. If set to ignore, uses nl_after_brace_close +nl_brace_struct_var = ignore # ignore/add/remove/force + +# Whether to alter newlines in '#define' macros +nl_define_macro = false # false/true + +# Whether to not put blanks after '#ifxx', '#elxx', or before '#endif' +nl_squeeze_ifdef = false # false/true + +# Add or remove blank line before 'if' +nl_before_if = ignore # ignore/add/remove/force + +# Add or remove blank line after 'if' statement +nl_after_if = add # ignore/add/remove/force + +# Add or remove blank line before 'for' +nl_before_for = ignore # ignore/add/remove/force + +# Add or remove blank line after 'for' statement +nl_after_for = remove # ignore/add/remove/force + +# Add or remove blank line before 'while' +nl_before_while = ignore # ignore/add/remove/force + +# Add or remove blank line after 'while' statement +nl_after_while = remove # ignore/add/remove/force + +# Add or remove blank line before 'switch' +nl_before_switch = ignore # ignore/add/remove/force + +# Add or remove blank line after 'switch' statement +nl_after_switch = ignore # ignore/add/remove/force + +# Add or remove blank line before 'do' +nl_before_do = ignore # ignore/add/remove/force + +# Add or remove blank line after 'do/while' statement +nl_after_do = ignore # ignore/add/remove/force + +# Whether to double-space commented-entries in struct/enum +nl_ds_struct_enum_cmt = false # false/true + +# Whether to double-space before the close brace of a struct/union/enum +# (lower priority than 'eat_blanks_before_close_brace') +nl_ds_struct_enum_close_brace = false # false/true + +# Add or remove a newline around a class colon. +# Related to pos_class_colon, nl_class_init_args, and pos_comma. +nl_class_colon = ignore # ignore/add/remove/force + +# Change simple unbraced if statements into a one-liner +# 'if(b)\n i++;' => 'if(b) i++;' +nl_create_if_one_liner = false # false/true + +# Change simple unbraced for statements into a one-liner +# 'for (i=0;i<5;i++)\n foo(i);' => 'for (i=0;i<5;i++) foo(i);' +nl_create_for_one_liner = false # false/true + +# Change simple unbraced while statements into a one-liner +# 'while (i<5)\n foo(i++);' => 'while (i<5) foo(i++);' +nl_create_while_one_liner = false # false/true + +# +# Positioning options +# + +# The position of arithmetic operators in wrapped expressions +pos_arith = ignore # ignore/join/lead/lead_break/lead_force/trail/trail_break/trail_force + +# The position of assignment in wrapped expressions. +# Do not affect '=' followed by '{' +pos_assign = ignore # ignore/join/lead/lead_break/lead_force/trail/trail_break/trail_force + +# The position of boolean operators in wrapped expressions +pos_bool = ignore # ignore/join/lead/lead_break/lead_force/trail/trail_break/trail_force + +# The position of comparison operators in wrapped expressions +pos_compare = ignore # ignore/join/lead/lead_break/lead_force/trail/trail_break/trail_force + +# The position of conditional (b ? t : f) operators in wrapped expressions +pos_conditional = ignore # ignore/join/lead/lead_break/lead_force/trail/trail_break/trail_force + +# The position of the comma in wrapped expressions +pos_comma = ignore # ignore/join/lead/lead_break/lead_force/trail/trail_break/trail_force + +# The position of the comma in the constructor initialization list +pos_class_comma = ignore # ignore/join/lead/lead_break/lead_force/trail/trail_break/trail_force + +# The position of colons between constructor and member initialization +pos_class_colon = ignore # ignore/join/lead/lead_break/lead_force/trail/trail_break/trail_force + +# +# Line Splitting options +# + +# Try to limit code width to N number of columns +code_width = 0 # number + +# Whether to fully split long 'for' statements at semi-colons +ls_for_split_full = false # false/true + +# Whether to fully split long function protos/calls at commas +ls_func_split_full = false # false/true + +# Whether to split lines as close to code_width as possible and ignore some groupings +ls_code_width = false # false/true + +# +# Blank line options +# + +# The maximum consecutive newlines +nl_max = 0 # number + +# The number of newlines after a function prototype, if followed by another function prototype +nl_after_func_proto = 0 # number + +# The number of newlines after a function prototype, if not followed by another function prototype +nl_after_func_proto_group = 0 # number + +# The number of newlines after '}' of a multi-line function body +nl_after_func_body = 0 # number + +# The number of newlines after '}' of a multi-line function body in a class declaration +nl_after_func_body_class = 0 # number + +# The number of newlines after '}' of a single line function body +nl_after_func_body_one_liner = 0 # number + +# The minimum number of newlines before a multi-line comment. +# Doesn't apply if after a brace open or another multi-line comment. +nl_before_block_comment = 0 # number + +# The minimum number of newlines before a single-line C comment. +# Doesn't apply if after a brace open or other single-line C comments. +nl_before_c_comment = 0 # number + +# The minimum number of newlines before a CPP comment. +# Doesn't apply if after a brace open or other CPP comments. +nl_before_cpp_comment = 0 # number + +# Whether to force a newline after a multi-line comment. +nl_after_multiline_comment = false # false/true + +# The number of newlines after '}' or ';' of a struct/enum/union definition +nl_after_struct = 0 # number + +# The number of newlines after '}' or ';' of a class definition +nl_after_class = 0 # number + +# The number of newlines before a 'private:', 'public:', 'protected:', 'signals:', or 'slots:' label. +# Will not change the newline count if after a brace open. +# 0 = No change. +nl_before_access_spec = 0 # number + +# The number of newlines after a 'private:', 'public:', 'protected:', 'signals:', or 'slots:' label. +# 0 = No change. +nl_after_access_spec = 0 # number + +# The number of newlines between a function def and the function comment. +# 0 = No change. +nl_comment_func_def = 0 # number + +# The number of newlines after a try-catch-finally block that isn't followed by a brace close. +# 0 = No change. +nl_after_try_catch_finally = 0 # number + +# The number of newlines before and after a property, indexer or event decl. +# 0 = No change. +nl_around_cs_property = 0 # number + +# The number of newlines between the get/set/add/remove handlers in C#. +# 0 = No change. +nl_between_get_set = 0 # number + +# Add or remove newline between C# property and the '{' +nl_property_brace = ignore # ignore/add/remove/force + +# Whether to remove blank lines after '{' +eat_blanks_after_open_brace = false # false/true + +# Whether to remove blank lines before '}' +eat_blanks_before_close_brace = false # false/true + +# How aggressively to remove extra newlines not in preproc. +# 0: No change +# 1: Remove most newlines not handled by other config +# 2: Remove all newlines and reformat completely by config +nl_remove_extra_newlines = 0 # number + +# Whether to put a blank line before 'return' statements, unless after an open brace. +nl_before_return = false # false/true + +# Whether to put a blank line after 'return' statements, unless followed by a close brace. +nl_after_return = false # false/true + +# Whether to put a newline after a Java annotation statement. +# Only affects annotations that are after a newline. +nl_after_annotation = ignore # ignore/add/remove/force + +# Controls the newline between two annotations. +nl_between_annotation = ignore # ignore/add/remove/force + +# +# Code modifying options (non-whitespace) +# + +# Add or remove braces on single-line 'do' statement +mod_full_brace_do = ignore # ignore/add/remove/force + +# Add or remove braces on single-line 'for' statement +mod_full_brace_for = ignore # ignore/add/remove/force + +# Add or remove braces on single-line function definitions. (Pawn) +mod_full_brace_function = ignore # ignore/add/remove/force + +# Add or remove braces on single-line 'if' statement. Will not remove the braces if they contain an 'else'. +mod_full_brace_if = ignore # ignore/add/remove/force + +# Make all if/elseif/else statements in a chain be braced or not. Overrides mod_full_brace_if. +# If any must be braced, they are all braced. If all can be unbraced, then the braces are removed. +mod_full_brace_if_chain = false # false/true + +# Don't remove braces around statements that span N newlines +mod_full_brace_nl = 0 # number + +# Add or remove braces on single-line 'while' statement +mod_full_brace_while = ignore # ignore/add/remove/force + +# Add or remove braces on single-line 'using ()' statement +mod_full_brace_using = ignore # ignore/add/remove/force + +# Add or remove unnecessary paren on 'return' statement +mod_paren_on_return = ignore # ignore/add/remove/force + +# Whether to change optional semicolons to real semicolons +mod_pawn_semicolon = false # false/true + +# Add parens on 'while' and 'if' statement around bools +mod_full_paren_if_bool = false # false/true + +# Whether to remove superfluous semicolons +mod_remove_extra_semicolon = false # false/true + +# If a function body exceeds the specified number of newlines and doesn't have a comment after +# the close brace, a comment will be added. +mod_add_long_function_closebrace_comment = 0 # number + +# If a switch body exceeds the specified number of newlines and doesn't have a comment after +# the close brace, a comment will be added. +mod_add_long_switch_closebrace_comment = 0 # number + +# If an #ifdef body exceeds the specified number of newlines and doesn't have a comment after +# the #endif, a comment will be added. +mod_add_long_ifdef_endif_comment = 0 # number + +# If an #ifdef or #else body exceeds the specified number of newlines and doesn't have a comment after +# the #else, a comment will be added. +mod_add_long_ifdef_else_comment = 0 # number + +# If TRUE, will sort consecutive single-line 'import' statements [Java, D] +mod_sort_import = false # false/true + +# If TRUE, will sort consecutive single-line 'using' statements [C#] +mod_sort_using = false # false/true + +# If TRUE, will sort consecutive single-line '#include' statements [C/C++] and '#import' statements [Obj-C] +# This is generally a bad idea, as it may break your code. +mod_sort_include = false # false/true + +# If TRUE, it will move a 'break' that appears after a fully braced 'case' before the close brace. +mod_move_case_break = false # false/true + +# Will add or remove the braces around a fully braced case statement. +# Will only remove the braces if there are no variable declarations in the block. +mod_case_brace = ignore # ignore/add/remove/force + +# If TRUE, it will remove a void 'return;' that appears as the last statement in a function. +mod_remove_empty_return = false # false/true + +# +# Comment modifications +# + +# Try to wrap comments at cmt_width columns +cmt_width = 0 # number + +# Set the comment reflow mode (default: 0) +# 0: no reflowing (apart from the line wrapping due to cmt_width) +# 1: no touching at all +# 2: full reflow +cmt_reflow_mode = 0 # number + +# If false, disable all multi-line comment changes, including cmt_width. keyword substitution, and leading chars. +# Default is true. +cmt_indent_multi = true # false/true + +# Whether to group c-comments that look like they are in a block +cmt_c_group = false # false/true + +# Whether to put an empty '/*' on the first line of the combined c-comment +cmt_c_nl_start = false # false/true + +# Whether to put a newline before the closing '*/' of the combined c-comment +cmt_c_nl_end = false # false/true + +# Whether to group cpp-comments that look like they are in a block +cmt_cpp_group = false # false/true + +# Whether to put an empty '/*' on the first line of the combined cpp-comment +cmt_cpp_nl_start = false # false/true + +# Whether to put a newline before the closing '*/' of the combined cpp-comment +cmt_cpp_nl_end = false # false/true + +# Whether to change cpp-comments into c-comments +cmt_cpp_to_c = true # false/true + +# Whether to put a star on subsequent comment lines +cmt_star_cont = false # false/true + +# The number of spaces to insert at the start of subsequent comment lines +cmt_sp_before_star_cont = 0 # number + +# The number of spaces to insert after the star on subsequent comment lines +cmt_sp_after_star_cont = 0 # number + +# For multi-line comments with a '*' lead, remove leading spaces if the first and last lines of +# the comment are the same length. Default=True +cmt_multi_check_last = false # false/true + +# The filename that contains text to insert at the head of a file if the file doesn't start with a C/C++ comment. +# Will substitute $(filename) with the current file's name. +cmt_insert_file_header = "" # string + +# The filename that contains text to insert at the end of a file if the file doesn't end with a C/C++ comment. +# Will substitute $(filename) with the current file's name. +cmt_insert_file_footer = "" # string + +# The filename that contains text to insert before a function implementation if the function isn't preceded with a C/C++ comment. +# Will substitute $(function) with the function name and $(javaparam) with the javadoc @param and @return stuff. +# Will also substitute $(fclass) with the class name: void CFoo::Bar() { ... } +cmt_insert_func_header = "" # string + +# The filename that contains text to insert before a class if the class isn't preceded with a C/C++ comment. +# Will substitute $(class) with the class name. +cmt_insert_class_header = "" # string + +# The filename that contains text to insert before a Obj-C message specification if the method isn't preceeded with a C/C++ comment. +# Will substitute $(message) with the function name and $(javaparam) with the javadoc @param and @return stuff. +cmt_insert_oc_msg_header = "" # string + +# If a preprocessor is encountered when stepping backwards from a function name, then +# this option decides whether the comment should be inserted. +# Affects cmt_insert_oc_msg_header, cmt_insert_func_header and cmt_insert_class_header. +cmt_insert_before_preproc = false # false/true + +# +# Preprocessor options +# + +# Control indent of preprocessors inside #if blocks at brace level 0 +pp_indent = ignore # ignore/add/remove/force + +# Whether to indent #if/#else/#endif at the brace level (true) or from column 1 (false) +pp_indent_at_level = false # false/true + +# If pp_indent_at_level=false, specifies the number of columns to indent per level. Default=1. +pp_indent_count = 1 # number + +# Add or remove space after # based on pp_level of #if blocks +pp_space = ignore # ignore/add/remove/force + +# Sets the number of spaces added with pp_space +pp_space_count = 0 # number + +# The indent for #region and #endregion in C# and '#pragma region' in C/C++ +pp_indent_region = 0 # number + +# Whether to indent the code between #region and #endregion +pp_region_indent_code = false # false/true + +# If pp_indent_at_level=true, sets the indent for #if, #else, and #endif when not at file-level +pp_indent_if = 0 # number + +# Control whether to indent the code between #if, #else and #endif when not at file-level +pp_if_indent_code = false # false/true + +# Whether to indent '#define' at the brace level (true) or from column 1 (false) +pp_define_at_level = false # false/true + +# You can force a token to be a type with the 'type' option. +# Example: +# type myfoo1 myfoo2 +# +# You can create custom macro-based indentation using macro-open, +# macro-else and macro-close. +# Example: +# macro-open BEGIN_TEMPLATE_MESSAGE_MAP +# macro-open BEGIN_MESSAGE_MAP +# macro-close END_MESSAGE_MAP +# +# You can assign any keyword to any type with the set option. +# set func_call_user _ N_ +# +# The full syntax description of all custom definition config entries +# is shown below: +# +# define custom tokens as: +# - embed whitespace in token using '' escape character, or +# put token in quotes +# - these: ' " and ` are recognized as quote delimiters +# +# type token1 token2 token3 ... +# ^ optionally specify multiple tokens on a single line +# define def_token output_token +# ^ output_token is optional, then NULL is assumed +# macro-open token +# macro-close token +# macro-else token +# set id token1 token2 ... +# ^ optionally specify multiple tokens on a single line +# ^ id is one of the names in token_enum.h sans the CT_ prefix, +# e.g. PP_PRAGMA +# +# all tokens are separated by any mix of ',' commas, '=' equal signs +# and whitespace (space, tab) +# +set IF IF_APPNAME diff --git a/make-linux.mk b/make-linux.mk index 5d01280..6d29056 100644 --- a/make-linux.mk +++ b/make-linux.mk @@ -25,6 +25,7 @@ ONE_IDTOOL = $(BUILD)/$(ONE_IDTOOL_NAME) LWIP_LIB = $(BUILD)/$(LWIP_LIB_NAME) # LWIP_DIR = ext/lwip +PICOTCP_DIR = ext/picotcp # Automagically pick clang or gcc, with preference for clang # This is only done if we have not overridden these with an environment or CLI variable @@ -81,14 +82,34 @@ INCLUDES+= -Iext \ -Isrc \ -I$(LWIP_DIR)/src/include \ -I$(LWIP_DIR)/src/include/ipv4 \ - -I$(LWIP_DIR)/src/include/ipv6 + -I$(LWIP_DIR)/src/include/ipv6 \ + -I$(PICOTCP_DIR)/include \ + -I$(PICOTCP_DIR)/build/include -# lwIP +# lwIP debug ifeq ($(SDK_LWIP_DEBUG),1) LWIP_FLAGS+=SDK_LWIP_DEBUG=1 endif +# lwIP +ifeq ($(SDK_LWIP),1) + STACK_FLAGS+=-DSDK_LWIP +endif + +# picoTCP +ifeq ($(SDK_PICOTCP),1) + STACK_FLAGS+=-DSDK_PICOTCP +endif + +# jip +ifeq ($(SDK_JIP),1) + STACK_FLAGS+=-DSDK_JIP +endif + + + + # Debug output for the SDK # Specific levels can be controlled in src/SDK_Debug.h ifeq ($(SDK_DEBUG),1) @@ -114,6 +135,15 @@ remove_only_intermediates: lwip: -make -f make-liblwip.mk $(LWIP_FLAGS) +pico: + mkdir -p build + cd ext/picotcp; make lib ARCH=shared IPV4=1 IPV6=1 + $(CC) -g -nostartfiles -shared -o ext/picotcp/build/lib/libpicotcp.so ext/picotcp/build/lib/*.o ext/picotcp/build/modules/*.o + cp ext/picotcp/build/lib/libpicotcp.so build/libpicotcp.so + +jip: + -make -f make-jip.mk $(JIP_FLAGS) + # --------- LINUX ---------- # Build everything @@ -132,8 +162,8 @@ linux_intercept: cd src ; gcc $(DEFS) $(INCLUDES) -g -O2 -Wall -std=c99 -fPIC -DVERBOSE -D_GNU_SOURCE -DSDK_INTERCEPT -nostdlib -nostdlib -shared -o ../$(INTERCEPT) SDK_Sockets.c SDK_Intercept.c SDK_RPC.c -ldl # Build only the SDK service -linux_sdk_service: lwip $(OBJS) - $(CXX) $(CXXFLAGS) $(LDFLAGS) $(DEFS) $(INCLUDES) -DSDK -DZT_ONE_NO_ROOT_CHECK -o $(SDK_SERVICE) $(OBJS) $(ZT1)/service/OneService.cpp src/SDK_EthernetTap.cpp src/SDK_Proxy.cpp $(ZT1)/one.cpp src/SDK_RPC.c $(LDLIBS) -ldl +linux_sdk_service: $(OBJS) + $(CXX) $(CXXFLAGS) $(LDFLAGS) $(STACK_FLAGS) $(DEFS) $(INCLUDES) -DSDK -DZT_ONE_NO_ROOT_CHECK -o $(SDK_SERVICE) $(OBJS) $(ZT1)/service/OneService.cpp src/SDK_EthernetTap.cpp src/SDK_Proxy.cpp $(ZT1)/one.cpp src/SDK_RPC.c $(LDLIBS) -ldl ln -sf $(SDK_SERVICE_NAME) $(BUILD)/zerotier-cli ln -sf $(SDK_SERVICE_NAME) $(BUILD)/zerotier-idtool @@ -245,7 +275,7 @@ clean_basic: -rm -rf $(INT)/Unity3D/Assets/Plugins/* -rm -rf zerotier-cli zerotier-idtool -find . -type f \( -name $(ONE_SERVICE_NAME) -o -name $(SDK_SERVICE_NAME) \) -delete - -find . -type f \( -name '*.o' -o -name '*.so' -o -name '*.o.d' -o -name '*.out' -o -name '*.log' -o -name '*.dSYM' \) -delete + -find . -type f \( -name '*.a' -o -name '*.o' -o -name '*.so' -o -name '*.o.d' -o -name '*.out' -o -name '*.log' -o -name '*.dSYM' \) -delete clean_thorough: clean_basic -rm -rf .depend diff --git a/src/SDK_Debug.h b/src/SDK_Debug.h index 2cba248..984f141 100644 --- a/src/SDK_Debug.h +++ b/src/SDK_Debug.h @@ -83,7 +83,7 @@ extern "C" { //#if defined(SDK_DEBUG) #if DEBUG_LEVEL >= MSG_ERROR - #define DEBUG_ERROR(fmt, args...) fprintf(stderr, RED "ZT_ERROR: %20s:%4d:%20s: " fmt "\n" RESET, __FILENAME__, __LINE__, __FUNCTION__, ##args) + #define DEBUG_ERROR(fmt, args...) fprintf(stderr, RED "ZT_ERROR: %20s:%4d:%25s: " fmt "\n" RESET, __FILENAME__, __LINE__, __FUNCTION__, ##args) #else #define DEBUG_ERROR(fmt, args...) #endif @@ -92,8 +92,8 @@ extern "C" { #define DEBUG_INFO(fmt, args...) ((void)__android_log_print(ANDROID_LOG_VERBOSE, LOG_TAG, "ZT_INFO : %20s:%4d:%20s: " fmt "\n", __FILENAME__, __LINE__, __FUNCTION__, ##args)) #define DEBUG_BLANK(fmt, args...) ((void)__android_log_print(ANDROID_LOG_VERBOSE, LOG_TAG, "ZT_INFO : %20s:%4d:" fmt "\n", __FILENAME__, __LINE__, __FUNCTION__, ##args)) #else - #define DEBUG_INFO(fmt, args...) fprintf(stderr, "ZT_INFO : %20s:%4d:%20s: " fmt "\n", __FILENAME__, __LINE__, __FUNCTION__, ##args) - #define DEBUG_ATTN(fmt, args...) fprintf(stderr, CYN "ZT_INFO : %20s:%4d:%20s: " fmt "\n" RESET, __FILENAME__, __LINE__, __FUNCTION__, ##args) + #define DEBUG_INFO(fmt, args...) fprintf(stderr, "ZT_INFO : %20s:%4d:%25s: " fmt "\n", __FILENAME__, __LINE__, __FUNCTION__, ##args) + #define DEBUG_ATTN(fmt, args...) fprintf(stderr, CYN "ZT_INFO : %20s:%4d:%25s: " fmt "\n" RESET, __FILENAME__, __LINE__, __FUNCTION__, ##args) #define DEBUG_BLANK(fmt, args...) fprintf(stderr, "ZT_INFO : %20s:%4d:" fmt "\n", __FILENAME__, __LINE__, ##args) #endif #else @@ -102,18 +102,18 @@ extern "C" { #endif #if DEBUG_LEVEL >= MSG_TRANSFER #if defined(__ANDROID__) - #define DEBUG_TRANS(fmt, args...) ((void)__android_log_print(ANDROID_LOG_VERBOSE, LOG_TAG, "ZT_TRANS : %20s:%4d:%20s: " fmt "\n", __FILENAME__, __LINE__, __FUNCTION__, ##args)) + #define DEBUG_TRANS(fmt, args...) ((void)__android_log_print(ANDROID_LOG_VERBOSE, LOG_TAG, "ZT_TRANS : %20s:%4d:%25s: " fmt "\n", __FILENAME__, __LINE__, __FUNCTION__, ##args)) #else - #define DEBUG_TRANS(fmt, args...) fprintf(stderr, GRN "ZT_TRANS: %20s:%4d:%20s: " fmt "\n" RESET, __FILENAME__, __LINE__, __FUNCTION__, ##args) + #define DEBUG_TRANS(fmt, args...) fprintf(stderr, GRN "ZT_TRANS: %20s:%4d:%25s: " fmt "\n" RESET, __FILENAME__, __LINE__, __FUNCTION__, ##args) #endif #else #define DEBUG_TRANS(fmt, args...) #endif #if DEBUG_LEVEL >= MSG_EXTRA #if defined(__ANDROID__) - #define DEBUG_EXTRA(fmt, args...) ((void)__android_log_print(ANDROID_LOG_VERBOSE, LOG_TAG, "ZT_EXTRA : %20s:%4d:%20s: " fmt "\n", __FILENAME__, __LINE__, __FUNCTION__, ##args)) + #define DEBUG_EXTRA(fmt, args...) ((void)__android_log_print(ANDROID_LOG_VERBOSE, LOG_TAG, "ZT_EXTRA : %20s:%4d:%25s: " fmt "\n", __FILENAME__, __LINE__, __FUNCTION__, ##args)) #else - #define DEBUG_EXTRA(fmt, args...) fprintf(stderr, "ZT_EXTRA: %20s:%4d:%20s: " fmt "\n", __FILENAME__, __LINE__, __FUNCTION__, ##args) + #define DEBUG_EXTRA(fmt, args...) fprintf(stderr, "ZT_EXTRA: %20s:%4d:%25s: " fmt "\n", __FILENAME__, __LINE__, __FUNCTION__, ##args) #endif #else #define DEBUG_EXTRA(fmt, args...) diff --git a/src/SDK_EthernetTap.cpp b/src/SDK_EthernetTap.cpp index ab98ffa..3b329fa 100644 --- a/src/SDK_EthernetTap.cpp +++ b/src/SDK_EthernetTap.cpp @@ -38,8 +38,20 @@ #include "SDK_EthernetTap.hpp" #include "SDK_Utils.hpp" #include "SDK.h" +#include "SDK_defs.h" #include "SDK_Debug.h" -#include "SDK_LWIPStack.hpp" + +#if defined(SDK_LWIP) + #include "SDK_lwip.hpp" +#elif defined(SDK_PICOTCP) + #include "SDK_pico.hpp" + #include "pico_stack.h" + #include "pico_ipv4.h" + #include "pico_icmp4.h" + #include "pico_dev_tap.h" +#elif defined(SDK_JIP) + #include "SDK_jip.hpp" +#endif #include "Utils.hpp" #include "OSUtils.hpp" @@ -71,6 +83,7 @@ namespace ZeroTier { // --------------------------------------------------------------------------- +static ZeroTier::NetconEthernetTap *picotap; static err_t tapif_init(struct netif *netif) { @@ -78,6 +91,73 @@ static err_t tapif_init(struct netif *netif) return ERR_OK; } + + +static void cb_ping(struct pico_icmp4_stats *s) +{ + char host[30]; + picotap->picostack->__pico_ipv4_to_string(host, s->dst.addr); + if (s->err == 0) { + // if all is well, print some pretty info + printf("%lu bytes from %s: icmp_req=%lu ttl=%lu time=%lu ms\n", s->size, + host, s->seq, s->ttl, (long unsigned int)s->time); + //if (s->seq >= NUM_PING) + //finished = 1; + } else { + // if something went wrong, print it and signal we want to stop + printf("PING %lu to %s: Error %d\n", s->seq, host, s->err); + //finished = 1; + } +} + +static int pico_eth_send(struct pico_device *dev, void *buf, int len) +{ + DEBUG_INFO("len = %d", len); + struct eth_hdr *ethhdr; + ethhdr = (struct eth_hdr *)buf; + + ZeroTier::MAC src_mac; + ZeroTier::MAC dest_mac; + src_mac.setTo(ethhdr->src.addr, 6); + dest_mac.setTo(ethhdr->dest.addr, 6); + + picotap->_handler(picotap->_arg,picotap->_nwid,src_mac,dest_mac, + Utils::ntoh((uint16_t)ethhdr->type),0, ((char*)buf) + sizeof(struct eth_hdr),len - sizeof(struct eth_hdr)); + return len; +} + +static int pico_eth_poll(struct pico_device *dev, int loop_score) +{ + // OPTIMIZATION: The copy logic and/or buffer structure should be reworked for better performance after the BETA + // DEBUG_INFO(); + // ZeroTier::NetconEthernetTap *tap = (ZeroTier::NetconEthernetTap*)netif->state; + Mutex::Lock _l(picotap->_pico_frame_rxbuf_m); + + uint8_t *buf = NULL; + uint32_t len = 0; + struct eth_hdr ethhdr; + unsigned char frame[ZT_MAX_MTU]; + + while (picotap->pico_frame_rxbuf_tot > 0) { + memset(frame, 0, sizeof(frame)); + + unsigned int len = 0; + memcpy(&len, picotap->pico_frame_rxbuf, sizeof(len)); // get frame len + DEBUG_ATTN("reading frame len = %ld", len); + memcpy(frame, picotap->pico_frame_rxbuf + sizeof(len), len); // get frame data + memmove(picotap->pico_frame_rxbuf, picotap->pico_frame_rxbuf + sizeof(len) + len, ZT_MAX_MTU-(sizeof(len) + len)); + int rx_ret = picotap->picostack->__pico_stack_recv(dev, (uint8_t*)frame, len); + picotap->pico_frame_rxbuf_tot-=(sizeof(len) + len); + DEBUG_ATTN("rx_ret = %d", rx_ret); + DEBUG_EXTRA("RX frame buffer %3f full", (float)(picotap->pico_frame_rxbuf_tot) / (float)(MAX_PICO_FRAME_RX_BUF_SZ)); + loop_score--; + } + //DEBUG_ATTN("loop_score = %d", loop_score); + return loop_score; +} + + + /* * Outputs data from the pbuf queue to the interface */ @@ -107,7 +187,7 @@ static err_t low_level_output(struct netif *netif, struct pbuf *p) dest_mac.setTo(ethhdr->dest.addr, 6); tap->_handler(tap->_arg,tap->_nwid,src_mac,dest_mac, - Utils::ntoh((uint16_t)ethhdr->type),0,buf + sizeof(struct eth_hdr),totalLength - sizeof(struct eth_hdr)); + Utils::ntoh((uint16_t)ethhdr->type),0,buf + sizeof(struct eth_hdr),totalLength - sizeof(struct eth_hdr)); return ERR_OK; } @@ -134,21 +214,42 @@ NetconEthernetTap::NetconEthernetTap( _run(true) { sockstate = -1; - char sockPath[4096],lwipPath[4096]; + char sockPath[4096],stackPath[4096]; Utils::snprintf(sockPath,sizeof(sockPath),"%s%snc_%.16llx",homePath,ZT_PATH_SEPARATOR_S,_nwid,ZT_PATH_SEPARATOR_S,(unsigned long long)nwid); _dev = sockPath; // in SDK mode, set device to be just the network ID - Utils::snprintf(lwipPath,sizeof(lwipPath),"%s%sliblwip.so",homePath,ZT_PATH_SEPARATOR_S); - - lwipstack = new LWIPStack(lwipPath); - if(!lwipstack) - throw std::runtime_error("unable to dynamically load a new instance of liblwip.so (searched ZeroTier home path)"); - lwipstack->__lwip_init(); - - _unixListenSocket = _phy.unixListen(sockPath,(void *)this); - DEBUG_INFO("tap initialized on: path=%s", sockPath); - if (!_unixListenSocket) - DEBUG_ERROR("unable to bind to: path=%s", sockPath); - _thread = Thread::start(this); + + // SIP-0 + // Load and initialize network stack library + + #if defined(SDK_LWIP) + Utils::snprintf(stackPath,sizeof(stackPath),"%s%sliblwip.so",homePath,ZT_PATH_SEPARATOR_S); + lwipstack = new lwIP_stack(stackPath); + #elif defined(SDK_PICOTCP) + Utils::snprintf(stackPath,sizeof(stackPath),"%s%slibpicotcp.so",homePath,ZT_PATH_SEPARATOR_S); + picostack = new picoTCP_stack(stackPath); + #elif defined(SDK_JIP) + Utils::snprintf(stackPath,sizeof(stackPath),"%s%slibjip.so",homePath,ZT_PATH_SEPARATOR_S); + jipstack = new jip_stack(stackPath); + #endif + + if(!lwipstack && !picostack && !jipstack) { + DEBUG_ERROR("unable to dynamically load a new instance of (%s) (searched ZeroTier home path)", stackPath); + throw std::runtime_error(""); + } + else { + if(lwipstack) + lwipstack->__lwip_init(); + if(picostack) + picostack->__pico_stack_init(); + //if(jipstack) + // jipstack->__jip_init(); + + _unixListenSocket = _phy.unixListen(sockPath,(void *)this); + DEBUG_INFO("tap initialized on: path=%s", sockPath); + if (!_unixListenSocket) + DEBUG_ERROR("unable to bind to: path=%s", sockPath); + _thread = Thread::start(this); + } } NetconEthernetTap::~NetconEthernetTap() @@ -171,10 +272,14 @@ bool NetconEthernetTap::enabled() const return _enabled; } -bool NetconEthernetTap::addIp(const InetAddress &ip) +void NetconEthernetTap::lwIP_init_interface(const InetAddress &ip) { DEBUG_INFO("local_addr=%s", ip.toString().c_str()); Mutex::Lock _l(_ips_m); + + // SIP-1 + // Initialize network stack's interface, assign addresses + if (std::find(_ips.begin(),_ips.end(),ip) == _ips.end()) { _ips.push_back(ip); std::sort(_ips.begin(),_ips.end()); @@ -231,6 +336,62 @@ bool NetconEthernetTap::addIp(const InetAddress &ip) interface6.flags = NETIF_FLAG_LINK_UP | NETIF_FLAG_UP; } } +} + +void NetconEthernetTap::picoTCP_init_interface(const InetAddress &ip) +{ + // TODO: Move this somewhere more appropriate + picotap = this; + + DEBUG_INFO(); + if (std::find(_ips.begin(),_ips.end(),ip) == _ips.end()) { + _ips.push_back(ip); + std::sort(_ips.begin(),_ips.end()); + + if(ip.isV4()) + { + int id; + struct pico_ip4 ipaddr, netmask; + ipaddr.addr = *((u32_t *)ip.rawIpData()); + netmask.addr = *((u32_t *)ip.netmask().rawIpData()); + picostack->__pico_ipv4_link_add(&picodev, ipaddr, netmask); + + picodev.send = pico_eth_send; // tx + picodev.poll = pico_eth_poll; // rx + + // Register the device in picoTCP + uint8_t mac[PICO_SIZE_ETH]; + _mac.copyTo(mac, PICO_SIZE_ETH); + DEBUG_ATTN("mac = %s", _mac.toString().c_str()); + if( 0 != picostack->__pico_device_init(&picodev, "p0", mac)) { + DEBUG_ERROR("device init failed"); + return; + } + DEBUG_INFO("successfully initialized device"); + // picostack->__pico_icmp4_ping("10.8.8.1", 20, 1000, 10000, 64, cb_ping); + } + if(ip.isV6()) + { + } + } +} + +void NetconEthernetTap::jip_init_interface(const InetAddress &ip) +{ + // will be similar to lwIP initialization process +} + +bool NetconEthernetTap::addIp(const InetAddress &ip) +{ + // SIP-3 + // Initialize a new interface in the stack, assign an address + #if defined(SDK_LWIP) + lwIP_init_interface(ip); + #elif defined(SDK_PICOTCP) + picoTCP_init_interface(ip); + #elif defined(SDK_JIP) + jip_init_interface(ip); + #endif return true; } @@ -253,13 +414,13 @@ std::vector NetconEthernetTap::ips() const return _ips; } -void NetconEthernetTap::put(const MAC &from,const MAC &to,unsigned int etherType,const void *data,unsigned int len) + +void NetconEthernetTap::lwIP_rx(const MAC &from,const MAC &to,unsigned int etherType,const void *data,unsigned int len) { - DEBUG_EXTRA("RX packet: len=%d, etherType=%d", len, etherType); + DEBUG_INFO(); struct pbuf *p,*q; if (!_enabled) return; - DEBUG_EXTRA("IPV6"); struct eth_hdr ethhdr; from.copyTo(ethhdr.src.addr, 6); to.copyTo(ethhdr.dest.addr, 6); @@ -295,6 +456,58 @@ void NetconEthernetTap::put(const MAC &from,const MAC &to,unsigned int etherType } } +void NetconEthernetTap::picoTCP_rx(const MAC &from,const MAC &to,unsigned int etherType,const void *data,unsigned int len) +{ + DEBUG_INFO(); + // Since picoTCP only allows the reception of frames from within the polling function, we + // must enqueue each frame into a memory structure shared by both threads. This structure will + Mutex::Lock _l(_pico_frame_rxbuf_m); + if(len > ((1024 * 1024) - pico_frame_rxbuf_tot)) { + DEBUG_ERROR("dropping packet (len = %d) - not enough space left on RX frame buffer", len); + return; + } + //if(len != memcpy(pico_frame_rxbuf, data, len)) { + // DEBUG_ERROR("dropping packet (len = %d) - unable to copy contents of frame to RX frame buffer", len); + // return; + //} + + // assemble new eth header + struct eth_hdr ethhdr; + from.copyTo(ethhdr.src.addr, 6); + to.copyTo(ethhdr.dest.addr, 6); + ethhdr.type = Utils::hton((uint16_t)etherType); + int newlen = len+sizeof(struct eth_hdr); + + memcpy(pico_frame_rxbuf + pico_frame_rxbuf_tot, &newlen, sizeof(newlen)); // size of frame + memcpy(pico_frame_rxbuf + pico_frame_rxbuf_tot + sizeof(newlen), ðhdr, sizeof(ethhdr)); // new eth header + memcpy(pico_frame_rxbuf + pico_frame_rxbuf_tot + sizeof(newlen) + sizeof(ethhdr), data, len); // frame data + pico_frame_rxbuf_tot += len + sizeof(len) + sizeof(ethhdr); + DEBUG_INFO("RX frame buffer %3f full", (float)pico_frame_rxbuf_tot / (float)(1024 * 1024)); +} + +void NetconEthernetTap::jip_rx(const MAC &from,const MAC &to,unsigned int etherType,const void *data,unsigned int len) +{ + DEBUG_INFO(); +} + + + +void NetconEthernetTap::put(const MAC &from,const MAC &to,unsigned int etherType,const void *data,unsigned int len) +{ + DEBUG_EXTRA("RX packet: len=%d, etherType=%d", len, etherType); + + // SIP- + // RX packet + + #if defined(SDK_LWIP) + lwIP_rx(from,to,etherType,data,len); + #elif defined(SDK_PICOTCP) + picoTCP_rx(from,to,etherType,data,len); + #elif defined(SDK_JIP) + jip_rx(from,to,etherType,data,len); + #endif +} + std::string NetconEthernetTap::deviceName() const { return _dev; @@ -326,13 +539,12 @@ void NetconEthernetTap::scanMulticastGroups(std::vector &added,s _multicastGroups.swap(newGroups); } -void NetconEthernetTap::threadMain() - throw() +void NetconEthernetTap::lwIP_loop() { + DEBUG_INFO(); uint64_t prev_tcp_time = 0, prev_status_time = 0, prev_discovery_time = 0; // Main timer loop while (_run) { - uint64_t now = OSUtils::now(); uint64_t since_tcp = now - prev_tcp_time; uint64_t since_discovery = now - prev_discovery_time; @@ -407,6 +619,40 @@ void NetconEthernetTap::threadMain() lwipstack->close(); } +void NetconEthernetTap::picoTCP_loop() +{ + DEBUG_INFO(); + while(_run) + { + //DEBUG_INFO("pico_tick"); + usleep(1000); + picostack->__pico_stack_tick(); + } +} + +void NetconEthernetTap::jip_loop() +{ + DEBUG_INFO(); + while(_run) + { + + } +} + +void NetconEthernetTap::threadMain() + throw() +{ + // SIP-2 + // Enter main thread loop for network stack + #if defined(SDK_LWIP) + lwIP_loop(); + #elif defined(SDK_PICOTCP) + picoTCP_loop(); + #elif defined(SDK_JIP) + jip_loop(); + #endif +} + Connection *NetconEthernetTap::getConnection(PhySocket *sock) { for(size_t i=0;i<_Connections.size();++i) { diff --git a/src/SDK_EthernetTap.hpp b/src/SDK_EthernetTap.hpp index 0a68212..ee3627f 100644 --- a/src/SDK_EthernetTap.hpp +++ b/src/SDK_EthernetTap.hpp @@ -46,7 +46,11 @@ #include "netif/etharp.h" +#include "SDK_defs.h" #include "SDK_RPC.h" +#include "SDK_lwip.hpp" +#include "SDK_pico.hpp" +#include "SDK_jip.hpp" // lwIP structs struct tcp_pcb; @@ -60,25 +64,6 @@ struct connect_st; struct getsockname_st; struct accept_st; -// Timing -#define APPLICATION_POLL_FREQ 2 -#define ZT_LWIP_TCP_TIMER_INTERVAL 50 -#define STATUS_TMR_INTERVAL 500 // How often we check connection statuses (in ms) - -// TCP Buffer sizes -#define DEFAULT_TCP_TX_BUF_SZ 1024 * 1024 -#define DEFAULT_TCP_RX_BUF_SZ 1024 * 1024 - -// TCP RX/TX buffer soft boundaries -#define DEFAULT_TCP_TX_BUF_SOFTMAX DEFAULT_TCP_TX_BUF_SZ * 0.80 -#define DEFAULT_TCP_TX_BUF_SOFTMIN DEFAULT_TCP_TX_BUF_SZ * 0.20 -#define DEFAULT_TCP_RX_BUF_SOFTMAX DEFAULT_TCP_RX_BUF_SZ * 0.80 -#define DEFAULT_TCP_RX_BUF_SOFTMIN DEFAULT_TCP_RX_BUF_SZ * 0.20 - -// UDP Buffer sizes (should be about the size of your MTU) -#define DEFAULT_UDP_TX_BUF_SZ ZT_MAX_MTU -#define DEFAULT_UDP_RX_BUF_SZ ZT_MAX_MTU * 128 - namespace ZeroTier { class NetconEthernetTap; @@ -145,10 +130,29 @@ namespace ZeroTier { void setFriendlyName(const char *friendlyName); void scanMulticastGroups(std::vector &added,std::vector &removed); + // SIP- + + struct pico_device picodev; + unsigned char pico_frame_rxbuf[MAX_PICO_FRAME_RX_BUF_SZ]; + int pico_frame_rxbuf_tot = 0; + Mutex _pico_frame_rxbuf_m; + + void lwIP_loop(); + void picoTCP_loop(); + void jip_loop(); + + // rx + void lwIP_rx(const MAC &from,const MAC &to,unsigned int etherType,const void *data,unsigned int len); + void picoTCP_rx(const MAC &from,const MAC &to,unsigned int etherType,const void *data,unsigned int len); + void jip_rx(const MAC &from,const MAC &to,unsigned int etherType,const void *data,unsigned int len); + + void lwIP_init_interface(const InetAddress &ip); + void picoTCP_init_interface(const InetAddress &ip); + void jip_init_interface(const InetAddress &ip); + void threadMain() throw(); - LWIPStack *lwipstack; uint64_t _nwid; void (*_handler)(void *,uint64_t,const MAC &,const MAC &,unsigned int,unsigned int,const void *,unsigned int); void *_arg; @@ -167,6 +171,10 @@ namespace ZeroTier { std::string _homePath; + lwIP_stack *lwipstack; + picoTCP_stack *picostack; + jip_stack *jipstack; + private: // LWIP callbacks // NOTE: these are called from within LWIP, meaning that lwipstack->_lock is ALREADY @@ -490,10 +498,9 @@ namespace ZeroTier { PhySocket *_unixListenSocket; std::vector _Connections; - std::map > jobmap; - pid_t rpcCounter; + netif interface; netif interface6; diff --git a/src/SDK_defs.h b/src/SDK_defs.h new file mode 100644 index 0000000..29d88ff --- /dev/null +++ b/src/SDK_defs.h @@ -0,0 +1,26 @@ +// --- lwIP +#define APPLICATION_POLL_FREQ 2 +#define ZT_LWIP_TCP_TIMER_INTERVAL 50 +#define STATUS_TMR_INTERVAL 500 // How often we check connection statuses (in ms) + +// --- picoTCP +#define MAX_PICO_FRAME_RX_BUF_SZ ZT_MAX_MTU * 128 + +// --- jip + + +// --- General + +// TCP Buffer sizes +#define DEFAULT_TCP_TX_BUF_SZ 1024 * 1024 +#define DEFAULT_TCP_RX_BUF_SZ 1024 * 1024 + +// TCP RX/TX buffer soft boundaries +#define DEFAULT_TCP_TX_BUF_SOFTMAX DEFAULT_TCP_TX_BUF_SZ * 0.80 +#define DEFAULT_TCP_TX_BUF_SOFTMIN DEFAULT_TCP_TX_BUF_SZ * 0.20 +#define DEFAULT_TCP_RX_BUF_SOFTMAX DEFAULT_TCP_RX_BUF_SZ * 0.80 +#define DEFAULT_TCP_RX_BUF_SOFTMIN DEFAULT_TCP_RX_BUF_SZ * 0.20 + +// UDP Buffer sizes (should be about the size of your MTU) +#define DEFAULT_UDP_TX_BUF_SZ ZT_MAX_MTU +#define DEFAULT_UDP_RX_BUF_SZ ZT_MAX_MTU * 128 \ No newline at end of file diff --git a/src/SDK_jip.hpp b/src/SDK_jip.hpp new file mode 100644 index 0000000..b004c5a --- /dev/null +++ b/src/SDK_jip.hpp @@ -0,0 +1,126 @@ +/* + * ZeroTier One - Network Virtualization Everywhere + * Copyright (C) 2011-2015 ZeroTier, Inc. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * -- + * + * ZeroTier may be used and distributed under the terms of the GPLv3, which + * are available at: http://www.gnu.org/licenses/gpl-3.0.html + * + * If you would like to embed ZeroTier into a commercial application or + * redistribute it in a modified binary form, please contact ZeroTier Networks + * LLC. Start here: http://www.zerotier.com/ + */ + +#ifndef SDK_JIPSTACK_H +#define SDK_JIPSTACK_H + +#include "Mutex.hpp" +#include "OSUtils.hpp" +#include "SDK_Debug.h" + +#include +#include + +#ifdef D_GNU_SOURCE +#define _GNU_SOURCE +#endif + +namespace ZeroTier { + + /** + * Loads an instance of picoTCP stack library in a private memory arena + * + * This uses dlmopen() to load an instance of the LWIP stack into its + * own private memory space. This is done to get around the stack's + * lack of thread-safety or multi-instance support. The alternative + * would be to massively refactor the stack so everything lives in a + * state object instead of static memory space. + */ + class jip_stack + { + public: + + void *_libref; + + void close() { +#if defined(__STATIC__LWIP__) + return; +#elif defined(__DYNAMIC_LWIP__) + dlclose(_libref); +#endif + } + + //void (*_netif_init)(void); + + + Mutex _lock; + Mutex _lock_mem; + + jip_stack(const char* path) : + _libref(NULL) + { +#if defined(__ANDROID__) || defined(__UNITY_3D__) + #define __STATIC_LWIP__ +#elif defined(__linux__) + #define __DYNAMIC_LWIP__ + // Dynamically load liblwip.so + _libref = dlmopen(LM_ID_NEWLM, path, RTLD_NOW); +#elif defined(__APPLE__) + #include "TargetConditionals.h" + #if TARGET_IPHONE_SIMULATOR || TARGET_OS_IPHONE + #include "node/Mutex.hpp" + #define __STATIC_LWIP__ + // iOS Simulator or iOS device + // Do nothing, symbols are statically-linked + #elif TARGET_OS_MAC && !defined(SDK_BUNDLED) + #define __DYNAMIC_LWIP__ + // Dynamically load liblwip.so + _libref = dlopen(path, RTLD_NOW); + #else + #define __STATIC_LWIP__ + #endif +#endif + +#ifdef __STATIC_LWIP__ // Set static references (for use in iOS) + + //_netif_init = (void(*)(void))&netif_init; + +#endif + +#ifdef __DYNAMIC_LWIP__ // Use dynamically-loaded symbols (for use in normal desktop applications) + + if(_libref == NULL) + DEBUG_ERROR("dlerror(): %s", dlerror()); + + //_netif_init = (void(*)(void))dlsym(_libref, "netif_init"); + +#endif + } + + ~jip_stack() + { + if (_libref) + dlclose(_libref); + } + + //inline void __netif_init(void) throw() { Mutex::Lock _l(_lock); _netif_init(); } + +}; + +} // namespace ZeroTier + +#endif diff --git a/src/SDK_lwip.hpp b/src/SDK_lwip.hpp new file mode 100644 index 0000000..c3c1a37 --- /dev/null +++ b/src/SDK_lwip.hpp @@ -0,0 +1,384 @@ +/* + * ZeroTier One - Network Virtualization Everywhere + * Copyright (C) 2011-2015 ZeroTier, Inc. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * -- + * + * ZeroTier may be used and distributed under the terms of the GPLv3, which + * are available at: http://www.gnu.org/licenses/gpl-3.0.html + * + * If you would like to embed ZeroTier into a commercial application or + * redistribute it in a modified binary form, please contact ZeroTier Networks + * LLC. Start here: http://www.zerotier.com/ + */ + +#ifndef SDK_LWIPSTACK_H +#define SDK_LWIPSTACK_H + +#include "lwip/mem.h" +#include "lwip/pbuf.h" +#include "lwip/ip_addr.h" +#include "lwip/netif.h" +#include "lwip/init.h" +#include "lwip/udp.h" + +#include "Mutex.hpp" +#include "OSUtils.hpp" +#include "SDK_Debug.h" + +#include +#include + +#ifdef D_GNU_SOURCE +#define _GNU_SOURCE +#endif + +//typedef ip_addr ip_addr_t; +struct tcp_pcb; + +// lwip General Stack API +#define PBUF_FREE_SIG struct pbuf *p +#define PBUF_ALLOC_SIG pbuf_layer layer, u16_t length, pbuf_type type +#define LWIP_HTONS_SIG u16_t x +#define LWIP_NTOHS_SIG u16_t x +#define IPADDR_NTOA_SIG const ip_addr_t *addr + +#define ETHIP6_OUTPUT_SIG struct netif *netif, struct pbuf *q, const ip6_addr_t *ip6addr +#define ETHARP_OUTPUT_SIG struct netif *netif, struct pbuf *q, const ip6_addr_t *ipaddr +//#define ETHARP_OUTPUT_SIG struct netif *netif, struct pbuf *q, const ip4_addr_t *ipaddr +#define NETIF_ADD_SIG struct netif *netif, void *state, netif_init_fn init, netif_input_fn input +//#define NETIF_ADD_SIG struct netif *netif, ip_addr_t *ipaddr, ip_addr_t *netmask, ip_addr_t *gw, void *state, netif_init_fn init, netif_input_fn input + +#define ETHERNET_INPUT_SIG struct pbuf *p, struct netif *netif +#define IP_INPUT_SIG struct pbuf *p, struct netif *inp +#define NETIF_SET_DEFAULT_SIG struct netif *netif +#define NETIF_SET_UP_SIG struct netif *netif +#define NETIF_POLL_SIG struct netif *netif + +// lwIP UDP API +#define UDP_NEW_SIG void +#define UDP_CONNECT_SIG struct udp_pcb * pcb, ip_addr_t * ipaddr, u16_t port +#define UDP_SEND_SIG struct udp_pcb * pcb, struct pbuf * p +#define UDP_SENDTO_SIG struct udp_pcb *pcb, struct pbuf *p, ip_addr_t *dst_ip, u16_t dst_port +#define UDP_RECV_SIG struct udp_pcb * pcb, void (* recv)(void * arg, struct udp_pcb * upcb, struct pbuf * p, ip_addr_t * addr, u16_t port), void * recv_arg +#define UDP_RECVED_SIG struct udp_pcb * pcb, u16_t len +#define UDP_BIND_SIG struct udp_pcb * pcb, const ip_addr_t * ipaddr, u16_t port +#define UDP_REMOVE_SIG struct udp_pcb *pcb + +// lwIP TCP API +#define TCP_WRITE_SIG struct tcp_pcb *pcb, const void *arg, u16_t len, u8_t apiflags +#define TCP_SENT_SIG struct tcp_pcb * pcb, err_t (* sent)(void * arg, struct tcp_pcb * tpcb, u16_t len) +#define TCP_NEW_SIG void +#define TCP_RECV_SIG struct tcp_pcb * pcb, err_t (* recv)(void * arg, struct tcp_pcb * tpcb, struct pbuf * p, err_t err) +#define TCP_RECVED_SIG struct tcp_pcb * pcb, u16_t len +#define TCP_SNDBUF_SIG struct tcp_pcb * pcb +#define TCP_CONNECT_SIG struct tcp_pcb * pcb, ip_addr_t * ipaddr, u16_t port, err_t (* connected)(void * arg, struct tcp_pcb * tpcb, err_t err) +#define TCP_RECV_SIG struct tcp_pcb * pcb, err_t (* recv)(void * arg, struct tcp_pcb * tpcb, struct pbuf * p, err_t err) +#define TCP_ERR_SIG struct tcp_pcb * pcb, void (* err)(void * arg, err_t err) +#define TCP_POLL_SIG struct tcp_pcb * pcb, err_t (* poll)(void * arg, struct tcp_pcb * tpcb), u8_t interval +#define TCP_ARG_SIG struct tcp_pcb * pcb, void * arg +#define TCP_CLOSE_SIG struct tcp_pcb * pcb +#define TCP_ABORT_SIG struct tcp_pcb * pcb +#define TCP_OUTPUT_SIG struct tcp_pcb * pcb +#define TCP_ACCEPT_SIG struct tcp_pcb * pcb, err_t (* accept)(void * arg, struct tcp_pcb * newpcb, err_t err) +#define TCP_LISTEN_SIG struct tcp_pcb * pcb +#define TCP_LISTEN_WITH_BACKLOG_SIG struct tcp_pcb * pcb, u8_t backlog +#define TCP_BIND_SIG struct tcp_pcb * pcb, const ip_addr_t * ipaddr, u16_t port +#define TCP_INPUT_SIG struct pbuf *p, struct netif *inp + +void dwr(int level, const char *fmt, ... ); + +#define NETIF_IP6_ADDR_SET_STATE_SIG struct netif* netif, s8_t addr_idx, u8_t state +#define NETIF_LOOPIF_INIT_SIG struct netif *netif +#define NETIF_CREATE_IP6_LINKLOCAL_ADDRESS_SIG struct netif *netif, u8_t from_mac_48bit +//#define NETIF_SET_ADDR_SIG struct netif *netif, const ip4_addr_t *ipaddr, const ip4_addr_t *netmask, const ip4_addr_t *gw + +namespace ZeroTier { + + /** + * Loads an instance of liblwip.so in a private memory arena + * + * This uses dlmopen() to load an instance of the LWIP stack into its + * own private memory space. This is done to get around the stack's + * lack of thread-safety or multi-instance support. The alternative + * would be to massively refactor the stack so everything lives in a + * state object instead of static memory space. + */ + class lwIP_stack + { + public: + + void *_libref; + + void close() { +#if defined(__STATIC__LWIP__) + return; +#elif defined(__DYNAMIC_LWIP__) + dlclose(_libref); +#endif + } + + void (*_netif_init)(void); + void (*_nd6_tmr)(void); + void (*_netif_ip6_addr_set_state)(NETIF_IP6_ADDR_SET_STATE_SIG); + void (*_netif_loopif_init)(NETIF_LOOPIF_INIT_SIG); + void (*_netif_create_ip6_linklocal_address)(NETIF_CREATE_IP6_LINKLOCAL_ADDRESS_SIG); + err_t (*_ethip6_output)(ETHIP6_OUTPUT_SIG); + // void (*_netif_set_addr)(NETIF_SET_ADDR_SIG); + + void (*_lwip_init)(); + err_t (*_tcp_write)(TCP_WRITE_SIG); + void (*_tcp_sent)(TCP_SENT_SIG); + struct tcp_pcb * (*_tcp_new)(TCP_NEW_SIG); + u16_t (*_tcp_sndbuf)(TCP_SNDBUF_SIG); + err_t (*_tcp_connect)(TCP_CONNECT_SIG); + + struct udp_pcb * (*_udp_new)(UDP_NEW_SIG); + err_t (*_udp_connect)(UDP_CONNECT_SIG); + err_t (*_udp_send)(UDP_SEND_SIG); + err_t (*_udp_sendto)(UDP_SENDTO_SIG); + void (*_udp_recv)(UDP_RECV_SIG); + void (*_udp_recved)(UDP_RECVED_SIG); + err_t (*_udp_bind)(UDP_BIND_SIG); + void (*_udp_remove)(UDP_REMOVE_SIG); + + void (*_tcp_recv)(TCP_RECV_SIG); + void (*_tcp_recved)(TCP_RECVED_SIG); + void (*_tcp_err)(TCP_ERR_SIG); + void (*_tcp_poll)(TCP_POLL_SIG); + void (*_tcp_arg)(TCP_ARG_SIG); + err_t (*_tcp_close)(TCP_CLOSE_SIG); + void (*_tcp_abort)(TCP_ABORT_SIG); + err_t (*_tcp_output)(TCP_OUTPUT_SIG); + void (*_tcp_accept)(TCP_ACCEPT_SIG); + struct tcp_pcb * (*_tcp_listen)(TCP_LISTEN_SIG); + struct tcp_pcb * (*_tcp_listen_with_backlog)(TCP_LISTEN_WITH_BACKLOG_SIG); + err_t (*_tcp_bind)(TCP_BIND_SIG); + void (*_etharp_tmr)(void); + void (*_tcp_tmr)(void); + u8_t (*_pbuf_free)(PBUF_FREE_SIG); + struct pbuf * (*_pbuf_alloc)(PBUF_ALLOC_SIG); + u16_t (*_lwip_htons)(LWIP_HTONS_SIG); + u16_t (*_lwip_ntohs)(LWIP_NTOHS_SIG); + char* (*_ipaddr_ntoa)(IPADDR_NTOA_SIG); + err_t (*_etharp_output)(ETHARP_OUTPUT_SIG); + err_t (*_ethernet_input)(ETHERNET_INPUT_SIG); + void (*_tcp_input)(TCP_INPUT_SIG); + err_t (*_ip_input)(IP_INPUT_SIG); + void (*_netif_set_default)(NETIF_SET_DEFAULT_SIG); + struct netif * (*_netif_add)(NETIF_ADD_SIG); + void (*_netif_set_up)(NETIF_SET_UP_SIG); + void (*_netif_poll)(NETIF_POLL_SIG); + + Mutex _lock; + Mutex _lock_mem; + + lwIP_stack(const char* path) : + _libref(NULL) + { +#if defined(__ANDROID__) || defined(__UNITY_3D__) + #define __STATIC_LWIP__ +#elif defined(__linux__) + #define __DYNAMIC_LWIP__ + // Dynamically load liblwip.so + _libref = dlmopen(LM_ID_NEWLM, path, RTLD_NOW); +#elif defined(__APPLE__) + #include "TargetConditionals.h" + #if TARGET_IPHONE_SIMULATOR || TARGET_OS_IPHONE + #include "node/Mutex.hpp" + #define __STATIC_LWIP__ + // iOS Simulator or iOS device + // Do nothing, symbols are statically-linked + #elif TARGET_OS_MAC && !defined(SDK_BUNDLED) + #define __DYNAMIC_LWIP__ + // Dynamically load liblwip.so + _libref = dlopen(path, RTLD_NOW); + #else + #define __STATIC_LWIP__ + #endif +#endif + +#ifdef __STATIC_LWIP__ // Set static references (for use in iOS) + + _netif_init = (void(*)(void))&netif_init; + _nd6_tmr = (void(*)(void))&nd6_tmr; + _netif_ip6_addr_set_state = (void(*)(NETIF_IP6_ADDR_SET_STATE_SIG))&netif_ip6_addr_set_state; + _netif_loopif_init = (void(*)(NETIF_LOOPIF_INIT_SIG))&netif_loopif_init; + _netif_create_ip6_linklocal_address = (void(*)(NETIF_CREATE_IP6_LINKLOCAL_ADDRESS_SIG))&netif_create_ip6_linklocal_address; + _ethip6_output = (err_t(*)(ETHIP6_OUTPUT_SIG))ðip6_output; + + _ethernet_input = (err_t(*)(ETHERNET_INPUT_SIG))ðernet_input; + _etharp_output = (err_t(*)(ETHARP_OUTPUT_SIG))ðarp_output; + _lwip_init = (void(*)(void))&lwip_init; + _tcp_write = (err_t(*)(TCP_WRITE_SIG))&tcp_write; + _tcp_sent = (void(*)(TCP_SENT_SIG))&tcp_sent; + _tcp_new = (struct tcp_pcb*(*)(TCP_NEW_SIG))&tcp_new; + + _udp_new = (struct udp_pcb*(*)(UDP_NEW_SIG))&udp_new; + _udp_connect = (err_t(*)(UDP_CONNECT_SIG))&udp_connect; + _udp_send = (err_t(*)(UDP_SEND_SIG))&udp_send; + _udp_sendto = (err_t(*)(UDP_SENDTO_SIG))&udp_sendto; + _udp_recv = (void(*)(UDP_RECV_SIG))&udp_recv; + _udp_bind = (err_t(*)(UDP_BIND_SIG))&udp_bind; + _udp_remove = (void(*)(UDP_REMOVE_SIG))&udp_remove; + + _tcp_connect = (err_t(*)(TCP_CONNECT_SIG))&tcp_connect; + _tcp_recv = (void(*)(TCP_RECV_SIG))&tcp_recv; + _tcp_recved = (void(*)(TCP_RECVED_SIG))&tcp_recved; + _tcp_err = (void(*)(TCP_ERR_SIG))&tcp_err; + _tcp_poll = (void(*)(TCP_POLL_SIG))&tcp_poll; + _tcp_arg = (void(*)(TCP_ARG_SIG))&tcp_arg; + _tcp_close = (err_t(*)(TCP_CLOSE_SIG))&tcp_close; + _tcp_abort = (void(*)(TCP_ABORT_SIG))&tcp_abort; + _tcp_output = (err_t(*)(TCP_OUTPUT_SIG))&tcp_output; + _tcp_accept = (void(*)(TCP_ACCEPT_SIG))&tcp_accept; + _tcp_listen_with_backlog = (struct tcp_pcb*(*)(TCP_LISTEN_WITH_BACKLOG_SIG))&tcp_listen_with_backlog; + _tcp_bind = (err_t(*)(TCP_BIND_SIG))&tcp_bind; + _etharp_tmr = (void(*)(void))ðarp_tmr; + _tcp_tmr = (void(*)(void))&tcp_tmr; + _pbuf_free = (u8_t(*)(PBUF_FREE_SIG))&pbuf_free; + _pbuf_alloc = (struct pbuf*(*)(PBUF_ALLOC_SIG))&pbuf_alloc; + _lwip_htons = (u16_t(*)(LWIP_HTONS_SIG))&lwip_htons; + _lwip_ntohs = (u16_t(*)(LWIP_NTOHS_SIG))&lwip_ntohs; + _ipaddr_ntoa = (char*(*)(IPADDR_NTOA_SIG))&ipaddr_ntoa; + _tcp_input = (void(*)(TCP_INPUT_SIG))&tcp_input; + _ip_input = (err_t(*)(IP_INPUT_SIG))&ip_input; + _netif_set_default = (void(*)(NETIF_SET_DEFAULT_SIG))&netif_set_default; + _netif_add = (struct netif*(*)(NETIF_ADD_SIG))&netif_add; + _netif_set_up = (void(*)(NETIF_SET_UP_SIG))&netif_set_up; +#endif + +#ifdef __DYNAMIC_LWIP__ // Use dynamically-loaded symbols (for use in normal desktop applications) + + if(_libref == NULL) + DEBUG_ERROR("dlerror(): %s", dlerror()); + + _netif_init = (void(*)(void))dlsym(_libref, "netif_init"); + _nd6_tmr = (void(*)(void))dlsym(_libref, "nd6_tmr"); + _netif_ip6_addr_set_state = (void(*)(NETIF_IP6_ADDR_SET_STATE_SIG))dlsym(_libref, "netif_ip6_addr_set_state"); + _netif_loopif_init = (void(*)(NETIF_LOOPIF_INIT_SIG))dlsym(_libref, "netif_loopif_init"); + _netif_create_ip6_linklocal_address = (void(*)(NETIF_CREATE_IP6_LINKLOCAL_ADDRESS_SIG))dlsym(_libref, "netif_create_ip6_linklocal_address"); + _ethip6_output = (err_t(*)(ETHIP6_OUTPUT_SIG))dlsym(_libref, "ethip6_output"); + + // _netif_set_addr = (void(*))(NETIF_SET_ADDR_SIG))dlsym(_libref, "netif_set_addr"); + + _ethernet_input = (err_t(*)(ETHERNET_INPUT_SIG))dlsym(_libref, "ethernet_input"); + _etharp_output = (err_t(*)(ETHARP_OUTPUT_SIG))dlsym(_libref, "etharp_output"); + _lwip_init = (void(*)(void))dlsym(_libref, "lwip_init"); + _tcp_write = (err_t(*)(TCP_WRITE_SIG))dlsym(_libref, "tcp_write"); + _tcp_sent = (void(*)(TCP_SENT_SIG))dlsym(_libref, "tcp_sent"); + _tcp_new = (struct tcp_pcb*(*)(TCP_NEW_SIG))dlsym(_libref, "tcp_new"); + + _udp_new = (struct udp_pcb*(*)(UDP_NEW_SIG))dlsym(_libref, "udp_new"); + _udp_connect = (err_t(*)(UDP_CONNECT_SIG))dlsym(_libref, "udp_connect"); + _udp_send = (err_t(*)(UDP_SEND_SIG))dlsym(_libref, "udp_send"); + _udp_sendto = (err_t(*)(UDP_SENDTO_SIG))dlsym(_libref, "udp_sendto"); + _udp_recv = (void(*)(UDP_RECV_SIG))dlsym(_libref, "udp_recv"); + _udp_bind = (err_t(*)(UDP_BIND_SIG))dlsym(_libref, "udp_bind"); + _udp_remove = (void(*)(UDP_REMOVE_SIG))dlsym(_libref, "udp_remove"); + + _tcp_sndbuf = (u16_t(*)(TCP_SNDBUF_SIG))dlsym(_libref, "tcp_sndbuf"); + _tcp_connect = (err_t(*)(TCP_CONNECT_SIG))dlsym(_libref, "tcp_connect"); + _tcp_recv = (void(*)(TCP_RECV_SIG))dlsym(_libref, "tcp_recv"); + _tcp_recved = (void(*)(TCP_RECVED_SIG))dlsym(_libref, "tcp_recved"); + _tcp_err = (void(*)(TCP_ERR_SIG))dlsym(_libref, "tcp_err"); + _tcp_poll = (void(*)(TCP_POLL_SIG))dlsym(_libref, "tcp_poll"); + _tcp_arg = (void(*)(TCP_ARG_SIG))dlsym(_libref, "tcp_arg"); + _tcp_close = (err_t(*)(TCP_CLOSE_SIG))dlsym(_libref, "tcp_close"); + _tcp_abort = (void(*)(TCP_ABORT_SIG))dlsym(_libref, "tcp_abort"); + _tcp_output = (err_t(*)(TCP_OUTPUT_SIG))dlsym(_libref, "tcp_output"); + _tcp_accept = (void(*)(TCP_ACCEPT_SIG))dlsym(_libref, "tcp_accept"); + _tcp_listen = (struct tcp_pcb*(*)(TCP_LISTEN_SIG))dlsym(_libref, "tcp_listen"); + _tcp_listen_with_backlog = (struct tcp_pcb*(*)(TCP_LISTEN_WITH_BACKLOG_SIG))dlsym(_libref, "tcp_listen_with_backlog"); + _tcp_bind = (err_t(*)(TCP_BIND_SIG))dlsym(_libref, "tcp_bind"); + _etharp_tmr = (void(*)(void))dlsym(_libref, "etharp_tmr"); + _tcp_tmr = (void(*)(void))dlsym(_libref, "tcp_tmr"); + _pbuf_free = (u8_t(*)(PBUF_FREE_SIG))dlsym(_libref, "pbuf_free"); + _pbuf_alloc = (struct pbuf*(*)(PBUF_ALLOC_SIG))dlsym(_libref, "pbuf_alloc"); + _lwip_htons = (u16_t(*)(LWIP_HTONS_SIG))dlsym(_libref, "lwip_htons"); + _lwip_ntohs = (u16_t(*)(LWIP_NTOHS_SIG))dlsym(_libref, "lwip_ntohs"); + _ipaddr_ntoa = (char*(*)(IPADDR_NTOA_SIG))dlsym(_libref, "ipaddr_ntoa"); + _tcp_input = (void(*)(TCP_INPUT_SIG))dlsym(_libref, "tcp_input"); + _ip_input = (err_t(*)(IP_INPUT_SIG))dlsym(_libref, "ip_input"); + _netif_set_default = (void(*)(NETIF_SET_DEFAULT_SIG))dlsym(_libref, "netif_set_default"); + _netif_add = (struct netif*(*)(NETIF_ADD_SIG))dlsym(_libref, "netif_add"); + _netif_set_up = (void(*)(NETIF_SET_UP_SIG))dlsym(_libref, "netif_set_up"); +#endif + } + + ~lwIP_stack() + { + if (_libref) + dlclose(_libref); + } + + inline void __netif_init(void) throw() { Mutex::Lock _l(_lock); _netif_init(); } + inline void __nd6_tmr(void) throw() { Mutex::Lock _l(_lock); _nd6_tmr(); } + inline void __netif_ip6_addr_set_state(NETIF_IP6_ADDR_SET_STATE_SIG) throw() { Mutex::Lock _l(_lock); _netif_ip6_addr_set_state(netif, addr_idx, state); } + inline void __netif_loopif_init(NETIF_LOOPIF_INIT_SIG) throw() { Mutex::Lock _l(_lock); _netif_loopif_init(netif); } + inline void __netif_create_ip6_linklocal_address(NETIF_CREATE_IP6_LINKLOCAL_ADDRESS_SIG) throw() { Mutex::Lock _l(_lock); _netif_create_ip6_linklocal_address(netif, from_mac_48bit); } + // inline void __netif_set_addr(NETIF_SET_ADDR_SIG) throw() { Mutex::Lock _l(_lock); _netif_set_addr(netif, ipaddr, netmask, gw); } + + inline void __lwip_init() throw() { Mutex::Lock _l(_lock); return _lwip_init(); } + inline err_t __tcp_write(TCP_WRITE_SIG) throw() { Mutex::Lock _l(_lock); return _tcp_write(pcb,arg,len,apiflags); } + inline void __tcp_sent(TCP_SENT_SIG) throw() { Mutex::Lock _l(_lock); return _tcp_sent(pcb,sent); } + inline struct tcp_pcb * __tcp_new(TCP_NEW_SIG) throw() { Mutex::Lock _l(_lock); return _tcp_new(); } + + inline struct udp_pcb * __udp_new(UDP_NEW_SIG) throw() { Mutex::Lock _l(_lock); return _udp_new(); } + inline err_t __udp_connect(UDP_CONNECT_SIG) throw() { Mutex::Lock _l(_lock); return _udp_connect(pcb,ipaddr,port); } + inline err_t __udp_send(UDP_SEND_SIG) throw() { Mutex::Lock _l(_lock); return _udp_send(pcb,p); } + inline err_t __udp_sendto(UDP_SENDTO_SIG) throw() { Mutex::Lock _l(_lock); return _udp_sendto(pcb,p,dst_ip,dst_port); } + inline void __udp_recv(UDP_RECV_SIG) throw() { Mutex::Lock _l(_lock); return _udp_recv(pcb,recv,recv_arg); } + inline err_t __udp_bind(UDP_BIND_SIG) throw() { Mutex::Lock _l(_lock); return _udp_bind(pcb,ipaddr,port); } + inline void __udp_remove(UDP_REMOVE_SIG) throw() { Mutex::Lock _l(_lock); return _udp_remove(pcb); } + + inline u16_t __tcp_sndbuf(TCP_SNDBUF_SIG) throw() { Mutex::Lock _l(_lock); return _tcp_sndbuf(pcb); } + inline err_t __tcp_connect(TCP_CONNECT_SIG) throw() { Mutex::Lock _l(_lock); return _tcp_connect(pcb,ipaddr,port,connected); } + inline void __tcp_recv(TCP_RECV_SIG) throw() { Mutex::Lock _l(_lock); return _tcp_recv(pcb,recv); } + inline void __tcp_recved(TCP_RECVED_SIG) throw() { Mutex::Lock _l(_lock); return _tcp_recved(pcb,len); } + inline void __tcp_err(TCP_ERR_SIG) throw() { Mutex::Lock _l(_lock); return _tcp_err(pcb,err); } + inline void __tcp_poll(TCP_POLL_SIG) throw() { Mutex::Lock _l(_lock); return _tcp_poll(pcb,poll,interval); } + inline void __tcp_arg(TCP_ARG_SIG) throw() { Mutex::Lock _l(_lock); return _tcp_arg(pcb,arg); } + inline err_t __tcp_close(TCP_CLOSE_SIG) throw() { Mutex::Lock _l(_lock); return _tcp_close(pcb); } + inline void __tcp_abort(TCP_ABORT_SIG) throw() { Mutex::Lock _l(_lock); return _tcp_abort(pcb); } + inline err_t __tcp_output(TCP_OUTPUT_SIG) throw() { Mutex::Lock _l(_lock); return _tcp_output(pcb); } + inline void __tcp_accept(TCP_ACCEPT_SIG) throw() { Mutex::Lock _l(_lock); return _tcp_accept(pcb,accept); } + inline struct tcp_pcb * __tcp_listen(TCP_LISTEN_SIG) throw() { Mutex::Lock _l(_lock); return _tcp_listen(pcb); } + inline struct tcp_pcb * __tcp_listen_with_backlog(TCP_LISTEN_WITH_BACKLOG_SIG) throw() { Mutex::Lock _l(_lock); return _tcp_listen_with_backlog(pcb,backlog); } + inline err_t __tcp_bind(TCP_BIND_SIG) throw() { Mutex::Lock _l(_lock); return _tcp_bind(pcb,ipaddr,port); } + inline void __etharp_tmr(void) throw() { Mutex::Lock _l(_lock); return _etharp_tmr(); } + inline void __tcp_tmr(void) throw() { Mutex::Lock _l(_lock); return _tcp_tmr(); } + inline u8_t __pbuf_free(PBUF_FREE_SIG) throw() { Mutex::Lock _l(_lock); return _pbuf_free(p); } + inline struct pbuf * __pbuf_alloc(PBUF_ALLOC_SIG) throw() { Mutex::Lock _l(_lock_mem); return _pbuf_alloc(layer,length,type); } + inline u16_t __lwip_htons(LWIP_HTONS_SIG) throw() { Mutex::Lock _l(_lock); return _lwip_htons(x); } + inline u16_t __lwip_ntohs(LWIP_NTOHS_SIG) throw() { Mutex::Lock _l(_lock); return _lwip_ntohs(x); } + inline char* __ipaddr_ntoa(IPADDR_NTOA_SIG) throw() { Mutex::Lock _l(_lock); return _ipaddr_ntoa(addr); } + + inline err_t __ethip6_output(ETHIP6_OUTPUT_SIG) throw() { Mutex::Lock _l(_lock); return _ethip6_output(netif,q,ip6addr); } + //inline err_t __etharp_output(ETHARP_OUTPUT_SIG) throw() { Mutex::Lock _l(_lock); return _etharp_output(netif,q,ipaddr); } + inline struct netif * __netif_add(NETIF_ADD_SIG) throw() { Mutex::Lock _l(_lock); return _netif_add(netif,state,init,input); } + //inline struct netif * __netif_add(NETIF_ADD_SIG) throw() { Mutex::Lock _l(_lock); return _netif_add(netif,ipaddr,netmask,gw,state,init,input); } + + inline err_t __ethernet_input(ETHERNET_INPUT_SIG) throw() { Mutex::Lock _l(_lock); return _ethernet_input(p,netif); } + inline void __tcp_input(TCP_INPUT_SIG) throw() { Mutex::Lock _l(_lock); return _tcp_input(p,inp); } + inline err_t __ip_input(IP_INPUT_SIG) throw() { Mutex::Lock _l(_lock); return _ip_input(p,inp); } + inline void __netif_set_default(NETIF_SET_DEFAULT_SIG) throw() { Mutex::Lock _l(_lock); return _netif_set_default(netif); } + inline void __netif_set_up(NETIF_SET_UP_SIG) throw() { Mutex::Lock _l(_lock); return _netif_set_up(netif); } +}; + +} // namespace ZeroTier + +#endif diff --git a/src/SDK_pico.hpp b/src/SDK_pico.hpp new file mode 100644 index 0000000..bd3ac15 --- /dev/null +++ b/src/SDK_pico.hpp @@ -0,0 +1,188 @@ +/* + * ZeroTier One - Network Virtualization Everywhere + * Copyright (C) 2011-2015 ZeroTier, Inc. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * -- + * + * ZeroTier may be used and distributed under the terms of the GPLv3, which + * are available at: http://www.gnu.org/licenses/gpl-3.0.html + * + * If you would like to embed ZeroTier into a commercial application or + * redistribute it in a modified binary form, please contact ZeroTier Networks + * LLC. Start here: http://www.zerotier.com/ + */ + +#ifndef SDK_PICOSTACK_H +#define SDK_PICOSTACK_H + +#include "Mutex.hpp" +#include "OSUtils.hpp" +#include "SDK_Debug.h" + +#include +#include + +#ifdef D_GNU_SOURCE +#define _GNU_SOURCE +#endif + + +#if defined(SDK_LWIP) + #include "SDK_lwip.hpp" +#elif defined(SDK_PICOTCP) + #include "SDK_pico.hpp" + + #include "pico_stack.h" + #include "pico_ipv4.h" + #include "pico_icmp4.h" + #include "pico_dev_tap.h" +#elif defined(SDK_JIP) + #include "SDK_jip.hpp" +#endif + + +#define PICO_STRING_TO_IPV4_SIG const char *ipstr, uint32_t *ip +#define PICO_IPV4_TO_STRING_SIG char *ipbuf, const uint32_t ip +#define PICO_TAP_CREATE_SIG char *name +#define PICO_IPV4_LINK_ADD_SIG struct pico_device *dev, struct pico_ip4 address, struct pico_ip4 netmask +#define PICO_DEVICE_INIT_SIG struct pico_device *dev, const char *name, uint8_t *mac +#define PICO_STACK_RECV_SIG struct pico_device *dev, uint8_t *buffer, uint32_t len +#define PICO_ICMP4_PING_SIG char *dst, int count, int interval, int timeout, int size, void (*cb)(struct pico_icmp4_stats *) + +namespace ZeroTier { + + /** + * Loads an instance of picoTCP stack library in a private memory arena + * + * This uses dlmopen() to load an instance of the LWIP stack into its + * own private memory space. This is done to get around the stack's + * lack of thread-safety or multi-instance support. The alternative + * would be to massively refactor the stack so everything lives in a + * state object instead of static memory space. + */ + class picoTCP_stack + { + public: + + void *_libref; + + void close() { +#if defined(__STATIC__LWIP__) + return; +#elif defined(__DYNAMIC_LWIP__) + dlclose(_libref); +#endif + } + + // SIP- + + void (*_pico_stack_init)(void); + void (*_pico_stack_tick)(void); + int (*_pico_string_to_ipv4)(PICO_STRING_TO_IPV4_SIG); + int (*_pico_ipv4_to_string)(PICO_IPV4_TO_STRING_SIG); + struct pico_device* (*_pico_tap_create)(PICO_TAP_CREATE_SIG); + int (*_pico_ipv4_link_add)(PICO_IPV4_LINK_ADD_SIG); + int (*_pico_device_init)(PICO_DEVICE_INIT_SIG); + int32_t (*_pico_stack_recv)(PICO_STACK_RECV_SIG); + int (*_pico_icmp4_ping)(PICO_ICMP4_PING_SIG); + + + Mutex _lock; + Mutex _lock_mem; + + picoTCP_stack(const char* path) : + _libref(NULL) + { +#if defined(__ANDROID__) || defined(__UNITY_3D__) + #define __STATIC_LWIP__ +#elif defined(__linux__) + #define __DYNAMIC_LWIP__ + // Dynamically load liblwip.so + _libref = dlmopen(LM_ID_NEWLM, path, RTLD_NOW); +#elif defined(__APPLE__) + #include "TargetConditionals.h" + #if TARGET_IPHONE_SIMULATOR || TARGET_OS_IPHONE + #include "node/Mutex.hpp" + #define __STATIC_LWIP__ + // iOS Simulator or iOS device + // Do nothing, symbols are statically-linked + #elif TARGET_OS_MAC && !defined(SDK_BUNDLED) + #define __DYNAMIC_LWIP__ + // Dynamically load liblwip.so + _libref = dlopen(path, RTLD_NOW); + #else + #define __STATIC_LWIP__ + #endif +#endif + +#ifdef __STATIC_LWIP__ // Set static references (for use in iOS) + + // SIP- + _pico_stack_init = (void(*)(void))&pico_stack_init; + _pico_stack_tick = (void(*)(void))&pico_stack_tick; + _pico_tap_create = (struct pico_device*(*)(PICO_TAP_CREATE_SIG)&pico_tap_create; + _pico_string_to_ipv4 = (int(*)(PICO_STRING_TO_IPV4_SIG))&pico_string_to_ipv4; + _pico_ipv4_to_string = (int(*)(PICO_IPV4_TO_STRING_SIG))&pico_ipv4_to_string; + _pico_ipv4_link_add = (int(*)(PICO_IPV4_LINK_ADD_SIG))&pico_ipv4_link_add; + _pico_device_init = (int(*)(PICO_DEVICE_INIT_SIG))&pico_device_init; + _pico_stack_recv = (int32_t(*)(PICO_STACK_RECV_SIG))&pico_stack_recv; + _pico_icmp4_ping = (int(*)(PICO_ICMP4_PING_SIG))&pico_icmp4_ping; + + +#endif + +#ifdef __DYNAMIC_LWIP__ // Use dynamically-loaded symbols (for use in normal desktop applications) + + if(_libref == NULL) + DEBUG_ERROR("dlerror(): %s", dlerror()); + + // SIP- + _pico_stack_init = (void(*)(void))dlsym(_libref, "pico_stack_init"); + _pico_stack_tick = (void(*)(void))dlsym(_libref, "pico_stack_tick"); + _pico_tap_create = (struct pico_device*(*)(PICO_TAP_CREATE_SIG))dlsym(_libref, "pico_tap_create"); + _pico_string_to_ipv4 = (int(*)(PICO_STRING_TO_IPV4_SIG))dlsym(_libref, "pico_string_to_ipv4"); + _pico_ipv4_to_string = (int(*)(PICO_IPV4_TO_STRING_SIG))dlsym(_libref, "pico_ipv4_to_string"); + _pico_ipv4_link_add = (int(*)(PICO_IPV4_LINK_ADD_SIG))dlsym(_libref, "pico_ipv4_link_add"); + _pico_device_init = (int(*)(PICO_DEVICE_INIT_SIG))dlsym(_libref, "pico_device_init"); + _pico_stack_recv = (int32_t(*)(PICO_STACK_RECV_SIG))dlsym(_libref, "pico_stack_recv"); + _pico_icmp4_ping = (int(*)(PICO_ICMP4_PING_SIG))dlsym(_libref, "pico_icmp4_ping"); + + +#endif + } + + ~picoTCP_stack() + { + if (_libref) + dlclose(_libref); + } + + // SIP- + inline void __pico_stack_init(void) throw() { Mutex::Lock _l(_lock); _pico_stack_init(); } + inline void __pico_stack_tick(void) throw() { Mutex::Lock _l(_lock); _pico_stack_tick(); } + inline struct pico_device * __pico_tap_create(PICO_TAP_CREATE_SIG) throw() { Mutex::Lock _l(_lock); return _pico_tap_create(name); } + inline int __pico_string_to_ipv4(PICO_STRING_TO_IPV4_SIG) throw() { Mutex::Lock _l(_lock); return _pico_string_to_ipv4(ipstr, ip); } + inline int __pico_ipv4_to_string(PICO_IPV4_TO_STRING_SIG) throw() { Mutex::Lock _l(_lock); return _pico_ipv4_to_string(ipbuf, ip); } + inline int __pico_ipv4_link_add(PICO_IPV4_LINK_ADD_SIG) throw() { Mutex::Lock _l(_lock); return _pico_ipv4_link_add(dev, address, netmask); } + inline int __pico_device_init(PICO_DEVICE_INIT_SIG) throw() { Mutex::Lock _l(_lock); return _pico_device_init(dev, name, mac); } + inline int __pico_stack_recv(PICO_STACK_RECV_SIG) throw() { /*Mutex::Lock _l(_lock);*/ return _pico_stack_recv(dev, buffer, len); } + inline int __pico_icmp4_ping(PICO_ICMP4_PING_SIG) throw() { Mutex::Lock _l(_lock); return _pico_icmp4_ping(dst, count, interval, timeout, size, cb); } + +}; + +} // namespace ZeroTier + +#endif