PicoTCP - Free (GPLv2) TCP/IP stack developed by TASS Belgi…

Users » daniele » Code » PicoTCP

Free (GPLv2) TCP/IP stack developed by TASS Belgium

Dependents: lpc1768-picotcp-demo ZeroMQ_PicoTCP_Publisher_demo TCPSocket_HelloWorld_PicoTCP Pico_TCP_UDP_Test ... more

PicoTCP. Copyright (c) 2013 TASS Belgium NV.

Released under the GNU General Public License, version 2.

Different licensing models may exist, at the sole discretion of the Copyright holders.

Official homepage: http://www.picotcp.com

Bug tracker: https://github.com/tass-belgium/picotcp/issues

Development steps:

~~initial integration with mbed RTOS~~
~~generic mbed Ethernet driver~~
~~high performance NXP LPC1768 specific Ethernet driver~~
~~Multi-threading support for mbed RTOS~~
~~Berkeley sockets and integration with the New Socket API~~
~~Fork of the apps running on top of the New Socket API~~
~~Scheduling optimizations~~
Debugging/benchmarking/testing

Demo application (measuring TCP sender performance):

Import program lpc1768-picotcp-demo

A PicoTCP demo app testing the ethernet throughput on the lpc1768 mbed board.

modules/pico_ipv4.c@149:5f4cb161cec3, 2014-04-09 (annotated)

Committer:: tass picotcp@tass.be
Date:: Wed Apr 09 14:31:41 2014 +0200
Revision:: 149:5f4cb161cec3
Parent:: 137:a1c8bfa9d691
Child:: 152:a3d286bf94e5

Update from git masterbranch

Who changed what in which revision?

User	Revision	Line number	New contents of line
tass	68:0847e35d08a6	1	/*********************************************************************
TASS Belgium NV	131:4758606c9316	2	PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
TASS Belgium NV	131:4758606c9316	3	See LICENSE and COPYING for usage.
tass	68:0847e35d08a6	4
TASS Belgium NV	131:4758606c9316	5	Authors: Daniele Lacamera, Markian Yskout
TASS Belgium NV	131:4758606c9316	6	*********************************************************************/
tass	68:0847e35d08a6	7
tass	68:0847e35d08a6	8
tass	68:0847e35d08a6	9	#include "pico_config.h"
tass	68:0847e35d08a6	10	#include "pico_ipfilter.h"
tass	68:0847e35d08a6	11	#include "pico_ipv4.h"
tass	68:0847e35d08a6	12	#include "pico_icmp4.h"
tass	68:0847e35d08a6	13	#include "pico_stack.h"
tass	68:0847e35d08a6	14	#include "pico_eth.h"
tass	68:0847e35d08a6	15	#include "pico_udp.h"
tass	68:0847e35d08a6	16	#include "pico_tcp.h"
tass	68:0847e35d08a6	17	#include "pico_socket.h"
tass	68:0847e35d08a6	18	#include "pico_device.h"
tass	68:0847e35d08a6	19	#include "pico_nat.h"
tass	68:0847e35d08a6	20	#include "pico_igmp.h"
tass	68:0847e35d08a6	21	#include "pico_tree.h"
tass picotcp@tass.be	149:5f4cb161cec3	22	#include "pico_socket_multicast.h"
tass	68:0847e35d08a6	23
tass	68:0847e35d08a6	24	#ifdef PICO_SUPPORT_IPV4
tass	68:0847e35d08a6	25
tass	68:0847e35d08a6	26	#ifdef PICO_SUPPORT_MCAST
TASS Belgium NV	131:4758606c9316	27	# define ip_mcast_dbg(...) do {} while(0) /* so_mcast_dbg in pico_socket.c */
tass picotcp@tass.be	149:5f4cb161cec3	28	/* #define ip_mcast_dbg dbg */
tass	68:0847e35d08a6	29	# define PICO_MCAST_ALL_HOSTS long_be(0xE0000001) /* 224.0.0.1 */
tass	68:0847e35d08a6	30	/* Default network interface for multicast transmission */
tass	68:0847e35d08a6	31	static struct pico_ipv4_link *mcast_default_link = NULL;
tass	68:0847e35d08a6	32	#endif
tass	68:0847e35d08a6	33	#ifdef PICO_SUPPORT_IPFRAG
tass picotcp@tass.be	149:5f4cb161cec3	34	/* # define reassembly_dbg dbg */
TASS Belgium NV	131:4758606c9316	35	# define reassembly_dbg(...) do {} while(0)
tass	68:0847e35d08a6	36	#endif
tass	68:0847e35d08a6	37
tass	68:0847e35d08a6	38	/* Queues */
TASS Belgium NV	131:4758606c9316	39	static struct pico_queue in = {
TASS Belgium NV	131:4758606c9316	40	0
TASS Belgium NV	131:4758606c9316	41	};
TASS Belgium NV	131:4758606c9316	42	static struct pico_queue out = {
TASS Belgium NV	131:4758606c9316	43	0
TASS Belgium NV	131:4758606c9316	44	};
tass	68:0847e35d08a6	45
tass	68:0847e35d08a6	46	/* Functions */
TASS Belgium NV	131:4758606c9316	47	static int ipv4_route_compare(void ka, void kb);
tass picotcp@tass.be	149:5f4cb161cec3	48	static struct pico_frame pico_ipv4_alloc(struct pico_protocol self, uint16_t size);
tass	68:0847e35d08a6	49
tass	68:0847e35d08a6	50	int pico_ipv4_to_string(char *ipbuf, const uint32_t ip)
tass	68:0847e35d08a6	51	{
TASS Belgium NV	131:4758606c9316	52	const unsigned char addr = (const unsigned char ) &ip;
TASS Belgium NV	131:4758606c9316	53	int i;
tass	68:0847e35d08a6	54
TASS Belgium NV	131:4758606c9316	55	if (!ipbuf) {
TASS Belgium NV	131:4758606c9316	56	pico_err = PICO_ERR_EINVAL;
TASS Belgium NV	131:4758606c9316	57	return -1;
TASS Belgium NV	131:4758606c9316	58	}
tass	68:0847e35d08a6	59
TASS Belgium NV	131:4758606c9316	60	for(i = 0; i < 4; i++)
TASS Belgium NV	131:4758606c9316	61	{
TASS Belgium NV	131:4758606c9316	62	if(addr[i] > 99) {
TASS Belgium NV	131:4758606c9316	63	*ipbuf++ = (char)('0' + (addr[i] / 100));
TASS Belgium NV	131:4758606c9316	64	*ipbuf++ = (char)('0' + ((addr[i] % 100) / 10));
TASS Belgium NV	131:4758606c9316	65	*ipbuf++ = (char)('0' + ((addr[i] % 100) % 10));
TASS Belgium NV	131:4758606c9316	66	}else if(addr[i] > 9) {
TASS Belgium NV	131:4758606c9316	67	*ipbuf++ = (char)('0' + (addr[i] / 10));
TASS Belgium NV	131:4758606c9316	68	*ipbuf++ = (char)('0' + (addr[i] % 10));
TASS Belgium NV	131:4758606c9316	69	}else{
TASS Belgium NV	131:4758606c9316	70	*ipbuf++ = (char)('0' + addr[i]);
TASS Belgium NV	131:4758606c9316	71	}
TASS Belgium NV	131:4758606c9316	72
TASS Belgium NV	131:4758606c9316	73	if(i < 3)
TASS Belgium NV	131:4758606c9316	74	*ipbuf++ = '.';
tass	68:0847e35d08a6	75	}
TASS Belgium NV	131:4758606c9316	76	*ipbuf = '\0';
TASS Belgium NV	131:4758606c9316	77
TASS Belgium NV	131:4758606c9316	78	return 0;
tass	68:0847e35d08a6	79	}
TASS Belgium NV	131:4758606c9316	80
tass picotcp@tass.be	149:5f4cb161cec3	81	static int pico_string_check_null_args(const char ipstr, uint32_t ip)
tass picotcp@tass.be	149:5f4cb161cec3	82	{
tass picotcp@tass.be	149:5f4cb161cec3	83
tass picotcp@tass.be	149:5f4cb161cec3	84	if(!ipstr \|\| !ip) {
tass picotcp@tass.be	149:5f4cb161cec3	85	pico_err = PICO_ERR_EINVAL;
tass picotcp@tass.be	149:5f4cb161cec3	86	return -1;
tass picotcp@tass.be	149:5f4cb161cec3	87	}
tass picotcp@tass.be	149:5f4cb161cec3	88
tass picotcp@tass.be	149:5f4cb161cec3	89	return 0;
tass picotcp@tass.be	149:5f4cb161cec3	90
tass picotcp@tass.be	149:5f4cb161cec3	91	}
tass picotcp@tass.be	149:5f4cb161cec3	92
tass	68:0847e35d08a6	93	int pico_string_to_ipv4(const char ipstr, uint32_t ip)
tass	68:0847e35d08a6	94	{
TASS Belgium NV	131:4758606c9316	95	unsigned char buf[4] = {
TASS Belgium NV	131:4758606c9316	96	0
TASS Belgium NV	131:4758606c9316	97	};
TASS Belgium NV	131:4758606c9316	98	int cnt = 0;
TASS Belgium NV	131:4758606c9316	99	char p;
tass	68:0847e35d08a6	100
tass picotcp@tass.be	149:5f4cb161cec3	101	if (pico_string_check_null_args(ipstr, ip) < 0)
TASS Belgium NV	131:4758606c9316	102	return -1;
tass picotcp@tass.be	149:5f4cb161cec3	103
tass	68:0847e35d08a6	104
TASS Belgium NV	131:4758606c9316	105	while((p = *ipstr++) != 0)
TASS Belgium NV	131:4758606c9316	106	{
TASS Belgium NV	131:4758606c9316	107	if(pico_is_digit(p)) {
TASS Belgium NV	131:4758606c9316	108	buf[cnt] = (uint8_t)((10 * buf[cnt]) + (p - '0'));
TASS Belgium NV	131:4758606c9316	109	}else if(p == '.') {
TASS Belgium NV	131:4758606c9316	110	cnt++;
TASS Belgium NV	131:4758606c9316	111	}else{
TASS Belgium NV	131:4758606c9316	112	return -1;
TASS Belgium NV	131:4758606c9316	113	}
TASS Belgium NV	131:4758606c9316	114	}
TASS Belgium NV	131:4758606c9316	115	/* Handle short notation */
TASS Belgium NV	131:4758606c9316	116	if(cnt == 1) {
TASS Belgium NV	131:4758606c9316	117	buf[3] = buf[1];
TASS Belgium NV	131:4758606c9316	118	buf[1] = 0;
TASS Belgium NV	131:4758606c9316	119	buf[2] = 0;
TASS Belgium NV	131:4758606c9316	120	}else if (cnt == 2) {
TASS Belgium NV	131:4758606c9316	121	buf[3] = buf[2];
TASS Belgium NV	131:4758606c9316	122	buf[2] = 0;
TASS Belgium NV	131:4758606c9316	123	}else if(cnt != 3) {
TASS Belgium NV	131:4758606c9316	124	/* String could not be parsed, return error */
TASS Belgium NV	131:4758606c9316	125	return -1;
TASS Belgium NV	131:4758606c9316	126	}
tass	68:0847e35d08a6	127
TASS Belgium NV	131:4758606c9316	128	*ip = long_from(buf);
tass	68:0847e35d08a6	129
TASS Belgium NV	131:4758606c9316	130	return 0;
TASS Belgium NV	131:4758606c9316	131
TASS Belgium NV	131:4758606c9316	132	}
tass	68:0847e35d08a6	133
tass	68:0847e35d08a6	134	int pico_ipv4_valid_netmask(uint32_t mask)
tass	68:0847e35d08a6	135	{
TASS Belgium NV	131:4758606c9316	136	int cnt = 0;
TASS Belgium NV	131:4758606c9316	137	int end = 0;
TASS Belgium NV	131:4758606c9316	138	int i;
TASS Belgium NV	131:4758606c9316	139	uint32_t mask_swap = long_be(mask);
tass	68:0847e35d08a6	140
TASS Belgium NV	131:4758606c9316	141	/*
TASS Belgium NV	131:4758606c9316	142	* Swap bytes for convenient parsing
TASS Belgium NV	131:4758606c9316	143	* e.g. 0x..f8ff will become 0xfff8..
TASS Belgium NV	131:4758606c9316	144	* Then, we count the consecutive bits
TASS Belgium NV	131:4758606c9316	145	*
TASS Belgium NV	131:4758606c9316	146	* */
tass	68:0847e35d08a6	147
TASS Belgium NV	131:4758606c9316	148	for(i = 0; i < 32; i++) {
TASS Belgium NV	131:4758606c9316	149	if((mask_swap << i) & 0x80000000) {
TASS Belgium NV	131:4758606c9316	150	if(end) {
TASS Belgium NV	131:4758606c9316	151	pico_err = PICO_ERR_EINVAL;
TASS Belgium NV	131:4758606c9316	152	return -1;
TASS Belgium NV	131:4758606c9316	153	}
TASS Belgium NV	131:4758606c9316	154
TASS Belgium NV	131:4758606c9316	155	cnt++;
TASS Belgium NV	131:4758606c9316	156	}else{
TASS Belgium NV	131:4758606c9316	157	end = 1;
TASS Belgium NV	131:4758606c9316	158	}
TASS Belgium NV	131:4758606c9316	159	}
TASS Belgium NV	131:4758606c9316	160	return cnt;
tass	68:0847e35d08a6	161	}
tass	68:0847e35d08a6	162
TASS Belgium NV	131:4758606c9316	163	int pico_ipv4_is_unicast(uint32_t address)
tass	68:0847e35d08a6	164	{
TASS Belgium NV	131:4758606c9316	165	const unsigned char addr = (unsigned char ) &address;
TASS Belgium NV	131:4758606c9316	166	if((addr[0] & 0xe0) == 0xe0)
TASS Belgium NV	131:4758606c9316	167	return 0; /* multicast */
TASS Belgium NV	131:4758606c9316	168
TASS Belgium NV	131:4758606c9316	169	return 1;
tass	68:0847e35d08a6	170	}
tass	68:0847e35d08a6	171
TASS Belgium NV	131:4758606c9316	172	int pico_ipv4_is_multicast(uint32_t address)
tass	68:0847e35d08a6	173	{
TASS Belgium NV	131:4758606c9316	174	const unsigned char addr = (unsigned char ) &address;
TASS Belgium NV	131:4758606c9316	175	if((addr[0] != 0xff) && ((addr[0] & 0xe0) == 0xe0))
TASS Belgium NV	131:4758606c9316	176	return 1; /* multicast */
TASS Belgium NV	131:4758606c9316	177
TASS Belgium NV	131:4758606c9316	178	return 0;
tass	68:0847e35d08a6	179	}
tass	68:0847e35d08a6	180
tass picotcp@tass.be	137:a1c8bfa9d691	181	int pico_ipv4_is_loopback(uint32_t address)
tass picotcp@tass.be	137:a1c8bfa9d691	182	{
tass picotcp@tass.be	137:a1c8bfa9d691	183	const unsigned char addr = (unsigned char ) &address;
tass picotcp@tass.be	137:a1c8bfa9d691	184	if(addr[0] == 0x7f)
tass picotcp@tass.be	137:a1c8bfa9d691	185	return 1;
tass picotcp@tass.be	137:a1c8bfa9d691	186
tass picotcp@tass.be	137:a1c8bfa9d691	187	return 0;
tass picotcp@tass.be	137:a1c8bfa9d691	188	}
tass picotcp@tass.be	137:a1c8bfa9d691	189
tass picotcp@tass.be	149:5f4cb161cec3	190	static int pico_ipv4_is_invalid_loopback(uint32_t address, struct pico_device *dev)
tass picotcp@tass.be	149:5f4cb161cec3	191	{
tass picotcp@tass.be	149:5f4cb161cec3	192	return pico_ipv4_is_loopback(address) && ((!dev) \|\| strcmp(dev->name, "loop"));
tass picotcp@tass.be	149:5f4cb161cec3	193	}
tass picotcp@tass.be	149:5f4cb161cec3	194
tass picotcp@tass.be	149:5f4cb161cec3	195	int pico_ipv4_is_valid_src(uint32_t address, struct pico_device *dev)
tass picotcp@tass.be	137:a1c8bfa9d691	196	{
tass picotcp@tass.be	137:a1c8bfa9d691	197	if (pico_ipv4_is_broadcast(address)) {
tass picotcp@tass.be	137:a1c8bfa9d691	198	dbg("Source is a broadcast address, discard packet\n");
tass picotcp@tass.be	137:a1c8bfa9d691	199	return 0;
tass picotcp@tass.be	137:a1c8bfa9d691	200	}
tass picotcp@tass.be	137:a1c8bfa9d691	201	else if( pico_ipv4_is_multicast(address)) {
tass picotcp@tass.be	137:a1c8bfa9d691	202	dbg("Source is a multicast address, discard packet\n");
tass picotcp@tass.be	137:a1c8bfa9d691	203	return 0;
tass picotcp@tass.be	137:a1c8bfa9d691	204	}
tass picotcp@tass.be	149:5f4cb161cec3	205	else if (pico_ipv4_is_invalid_loopback(address, dev)) {
tass picotcp@tass.be	137:a1c8bfa9d691	206	dbg("Source is a loopback address, discard packet\n");
tass picotcp@tass.be	137:a1c8bfa9d691	207	return 0;
tass picotcp@tass.be	137:a1c8bfa9d691	208	}
tass picotcp@tass.be	137:a1c8bfa9d691	209	else {
tass picotcp@tass.be	137:a1c8bfa9d691	210	return 1;
tass picotcp@tass.be	137:a1c8bfa9d691	211	}
tass picotcp@tass.be	137:a1c8bfa9d691	212	}
tass picotcp@tass.be	137:a1c8bfa9d691	213
tass	68:0847e35d08a6	214	static int pico_ipv4_checksum(struct pico_frame *f)
tass	68:0847e35d08a6	215	{
TASS Belgium NV	131:4758606c9316	216	struct pico_ipv4_hdr hdr = (struct pico_ipv4_hdr ) f->net_hdr;
TASS Belgium NV	131:4758606c9316	217	if (!hdr)
TASS Belgium NV	131:4758606c9316	218	return -1;
TASS Belgium NV	131:4758606c9316	219
TASS Belgium NV	131:4758606c9316	220	hdr->crc = 0;
TASS Belgium NV	131:4758606c9316	221	hdr->crc = short_be(pico_checksum(hdr, f->net_len));
TASS Belgium NV	131:4758606c9316	222	return 0;
tass	68:0847e35d08a6	223	}
tass	68:0847e35d08a6	224
tass	68:0847e35d08a6	225	#ifdef PICO_SUPPORT_IPFRAG
tass	68:0847e35d08a6	226	struct pico_ipv4_fragmented_packet {
TASS Belgium NV	131:4758606c9316	227	uint16_t id;
TASS Belgium NV	131:4758606c9316	228	uint8_t proto;
TASS Belgium NV	131:4758606c9316	229	struct pico_ip4 src;
TASS Belgium NV	131:4758606c9316	230	struct pico_ip4 dst;
TASS Belgium NV	131:4758606c9316	231	uint16_t total_len;
TASS Belgium NV	131:4758606c9316	232	struct pico_tree *t;
tass	68:0847e35d08a6	233	};
tass	68:0847e35d08a6	234
tass	68:0847e35d08a6	235	static int pico_ipv4_fragmented_packet_cmp(void ka, void kb)
tass	68:0847e35d08a6	236	{
TASS Belgium NV	131:4758606c9316	237	struct pico_ipv4_fragmented_packet a = ka, b = kb;
tass	68:0847e35d08a6	238
TASS Belgium NV	131:4758606c9316	239	if (a->id < b->id)
tass	68:0847e35d08a6	240	return -1;
TASS Belgium NV	131:4758606c9316	241	else if (a->id > b->id)
tass	68:0847e35d08a6	242	return 1;
TASS Belgium NV	131:4758606c9316	243	else {
TASS Belgium NV	131:4758606c9316	244	if (a->proto < b->proto)
TASS Belgium NV	131:4758606c9316	245	return -1;
TASS Belgium NV	131:4758606c9316	246	else if (a->proto > b->proto)
TASS Belgium NV	131:4758606c9316	247	return 1;
TASS Belgium NV	131:4758606c9316	248	else {
TASS Belgium NV	131:4758606c9316	249	if (a->src.addr < b->src.addr)
TASS Belgium NV	131:4758606c9316	250	return -1;
TASS Belgium NV	131:4758606c9316	251	else if (a->src.addr > b->src.addr)
TASS Belgium NV	131:4758606c9316	252	return 1;
TASS Belgium NV	131:4758606c9316	253	else {
TASS Belgium NV	131:4758606c9316	254	if (a->dst.addr < b->dst.addr)
TASS Belgium NV	131:4758606c9316	255	return -1;
TASS Belgium NV	131:4758606c9316	256	else if (a->dst.addr > b->dst.addr)
TASS Belgium NV	131:4758606c9316	257	return 1;
TASS Belgium NV	131:4758606c9316	258	else
TASS Belgium NV	131:4758606c9316	259	return 0;
TASS Belgium NV	131:4758606c9316	260	}
TASS Belgium NV	131:4758606c9316	261	}
tass	68:0847e35d08a6	262	}
TASS Belgium NV	131:4758606c9316	263	}
tass	68:0847e35d08a6	264
tass	68:0847e35d08a6	265	static int pico_ipv4_fragmented_element_cmp(void ka, void kb)
tass	68:0847e35d08a6	266	{
TASS Belgium NV	131:4758606c9316	267	struct pico_frame frame_a = ka, frame_b = kb;
TASS Belgium NV	131:4758606c9316	268	struct pico_ipv4_hdr a, b;
tass picotcp@tass.be	149:5f4cb161cec3	269	uint16_t a_frag, b_frag;
TASS Belgium NV	131:4758606c9316	270	a = (struct pico_ipv4_hdr *) frame_a->net_hdr;
TASS Belgium NV	131:4758606c9316	271	b = (struct pico_ipv4_hdr *) frame_b->net_hdr;
tass picotcp@tass.be	149:5f4cb161cec3	272	a_frag = short_be(a->frag & PICO_IPV4_FRAG_MASK);
tass picotcp@tass.be	149:5f4cb161cec3	273	b_frag = short_be(b->frag & PICO_IPV4_FRAG_MASK);
tass	68:0847e35d08a6	274
tass picotcp@tass.be	149:5f4cb161cec3	275	if (a_frag < b_frag)
TASS Belgium NV	131:4758606c9316	276	return -1;
tass picotcp@tass.be	149:5f4cb161cec3	277
tass picotcp@tass.be	149:5f4cb161cec3	278	if (b_frag < a_frag)
TASS Belgium NV	131:4758606c9316	279	return 1;
TASS Belgium NV	131:4758606c9316	280	else
TASS Belgium NV	131:4758606c9316	281	return 0;
TASS Belgium NV	131:4758606c9316	282	}
TASS Belgium NV	131:4758606c9316	283
tass	68:0847e35d08a6	284	PICO_TREE_DECLARE(pico_ipv4_fragmented_tree, pico_ipv4_fragmented_packet_cmp);
tass	68:0847e35d08a6	285
tass	68:0847e35d08a6	286	static inline void pico_ipv4_fragmented_cleanup(struct pico_ipv4_fragmented_packet *pfrag)
tass	68:0847e35d08a6	287	{
TASS Belgium NV	131:4758606c9316	288	struct pico_tree_node index = NULL, _tmp = NULL;
TASS Belgium NV	131:4758606c9316	289	struct pico_frame *f_frag = NULL;
tass	68:0847e35d08a6	290
TASS Belgium NV	131:4758606c9316	291	pico_tree_foreach_safe(index, pfrag->t, _tmp) {
TASS Belgium NV	131:4758606c9316	292	f_frag = index->keyValue;
TASS Belgium NV	131:4758606c9316	293	reassembly_dbg("REASSEMBLY: remove packet with offset %u\n", short_be(((struct pico_ipv4_hdr *)f_frag->net_hdr)->frag) & PICO_IPV4_FRAG_MASK);
TASS Belgium NV	131:4758606c9316	294	pico_tree_delete(pfrag->t, f_frag);
TASS Belgium NV	131:4758606c9316	295	pico_frame_discard(f_frag);
TASS Belgium NV	131:4758606c9316	296	}
TASS Belgium NV	131:4758606c9316	297	pico_tree_delete(&pico_ipv4_fragmented_tree, pfrag);
tass picotcp@tass.be	149:5f4cb161cec3	298	PICO_FREE(pfrag->t);
tass picotcp@tass.be	149:5f4cb161cec3	299	PICO_FREE(pfrag);
tass	68:0847e35d08a6	300	}
tass	68:0847e35d08a6	301	#endif /* PICO_SUPPORT_IPFRAG */
tass	68:0847e35d08a6	302
tass	68:0847e35d08a6	303	#ifdef PICO_SUPPORT_IPFRAG
tass	74:c146c4e346c4	304
tass	74:c146c4e346c4	305	static inline struct pico_ipv4_fragmented_packet fragment_find_by_hdr(struct pico_ipv4_hdr hdr)
tass	74:c146c4e346c4	306	{
TASS Belgium NV	131:4758606c9316	307	struct pico_ipv4_fragmented_packet frag = {
TASS Belgium NV	131:4758606c9316	308	0
TASS Belgium NV	131:4758606c9316	309	};
TASS Belgium NV	131:4758606c9316	310	frag.id = short_be(hdr->id);
TASS Belgium NV	131:4758606c9316	311	frag.proto = hdr->proto;
TASS Belgium NV	131:4758606c9316	312	frag.src.addr = long_be(hdr->src.addr);
TASS Belgium NV	131:4758606c9316	313	frag.dst.addr = long_be(hdr->dst.addr);
TASS Belgium NV	131:4758606c9316	314	return pico_tree_findKey(&pico_ipv4_fragmented_tree, &frag);
tass	74:c146c4e346c4	315	}
tass	74:c146c4e346c4	316
tass	74:c146c4e346c4	317
tass	70:cd218dd180e5	318	static inline int8_t pico_ipv4_fragmented_check(struct pico_protocol self, struct pico_frame *f)
tass	68:0847e35d08a6	319	{
TASS Belgium NV	131:4758606c9316	320	uint8_t *running_pointer = NULL;
TASS Belgium NV	131:4758606c9316	321	uint16_t running_offset = 0;
TASS Belgium NV	131:4758606c9316	322	uint16_t offset = 0;
TASS Belgium NV	131:4758606c9316	323	uint16_t data_len = 0;
TASS Belgium NV	131:4758606c9316	324	struct pico_ipv4_hdr f_frag_hdr = NULL, hdr = (struct pico_ipv4_hdr ) (f)->net_hdr;
TASS Belgium NV	131:4758606c9316	325	struct pico_ipv4_fragmented_packet *pfrag = NULL;
TASS Belgium NV	131:4758606c9316	326	struct pico_frame f_new = NULL, f_frag = NULL;
TASS Belgium NV	131:4758606c9316	327	struct pico_tree_node index, _tmp;
TASS Belgium NV	131:4758606c9316	328
TASS Belgium NV	131:4758606c9316	329	data_len = (uint16_t)(short_be(hdr->len) - (*f)->net_len);
TASS Belgium NV	131:4758606c9316	330	offset = short_be(hdr->frag) & PICO_IPV4_FRAG_MASK;
TASS Belgium NV	131:4758606c9316	331	if (short_be(hdr->frag) & PICO_IPV4_MOREFRAG) {
TASS Belgium NV	131:4758606c9316	332	if (!offset) {
TASS Belgium NV	131:4758606c9316	333	reassembly_dbg("REASSEMBLY: first element of a fragmented packet with id %X and offset %u\n", short_be(hdr->id), offset);
TASS Belgium NV	131:4758606c9316	334	if (!pico_tree_empty(&pico_ipv4_fragmented_tree)) {
TASS Belgium NV	131:4758606c9316	335	reassembly_dbg("REASSEMBLY: cleanup tree\n");
TASS Belgium NV	131:4758606c9316	336	/* only one entry allowed in this tree */
TASS Belgium NV	131:4758606c9316	337	pfrag = pico_tree_first(&pico_ipv4_fragmented_tree);
TASS Belgium NV	131:4758606c9316	338	pico_ipv4_fragmented_cleanup(pfrag);
TASS Belgium NV	131:4758606c9316	339	}
TASS Belgium NV	131:4758606c9316	340
TASS Belgium NV	131:4758606c9316	341	/* add entry in tree for this ID and create secondary tree to contain fragmented elements */
tass picotcp@tass.be	149:5f4cb161cec3	342	pfrag = PICO_ZALLOC(sizeof(struct pico_ipv4_fragmented_packet));
TASS Belgium NV	131:4758606c9316	343	if (!pfrag) {
TASS Belgium NV	131:4758606c9316	344	pico_err = PICO_ERR_ENOMEM;
TASS Belgium NV	131:4758606c9316	345	return -1;
TASS Belgium NV	131:4758606c9316	346	}
TASS Belgium NV	131:4758606c9316	347
TASS Belgium NV	131:4758606c9316	348	pfrag->id = short_be(hdr->id);
TASS Belgium NV	131:4758606c9316	349	pfrag->proto = hdr->proto;
TASS Belgium NV	131:4758606c9316	350	pfrag->src.addr = long_be(hdr->src.addr);
TASS Belgium NV	131:4758606c9316	351	pfrag->dst.addr = long_be(hdr->dst.addr);
TASS Belgium NV	131:4758606c9316	352	pfrag->total_len = (uint16_t)(short_be(hdr->len) - (*f)->net_len);
tass picotcp@tass.be	149:5f4cb161cec3	353	pfrag->t = PICO_ZALLOC(sizeof(struct pico_tree));
TASS Belgium NV	131:4758606c9316	354	if (!pfrag->t) {
tass picotcp@tass.be	149:5f4cb161cec3	355	PICO_FREE(pfrag);
TASS Belgium NV	131:4758606c9316	356	pico_err = PICO_ERR_ENOMEM;
TASS Belgium NV	131:4758606c9316	357	return -1;
TASS Belgium NV	131:4758606c9316	358	}
TASS Belgium NV	131:4758606c9316	359
TASS Belgium NV	131:4758606c9316	360	pfrag->t->root = &LEAF;
TASS Belgium NV	131:4758606c9316	361	pfrag->t->compare = pico_ipv4_fragmented_element_cmp;
TASS Belgium NV	131:4758606c9316	362
TASS Belgium NV	131:4758606c9316	363	pico_tree_insert(pfrag->t, *f);
TASS Belgium NV	131:4758606c9316	364	pico_tree_insert(&pico_ipv4_fragmented_tree, pfrag);
TASS Belgium NV	131:4758606c9316	365	return 0;
TASS Belgium NV	131:4758606c9316	366	}
TASS Belgium NV	131:4758606c9316	367	else {
TASS Belgium NV	131:4758606c9316	368	reassembly_dbg("REASSEMBLY: intermediate element of a fragmented packet with id %X and offset %u\n", short_be(hdr->id), offset);
TASS Belgium NV	131:4758606c9316	369	pfrag = fragment_find_by_hdr(hdr);
TASS Belgium NV	131:4758606c9316	370	if (pfrag) {
TASS Belgium NV	131:4758606c9316	371	pfrag->total_len = (uint16_t)(pfrag->total_len + (short_be(hdr->len) - (*f)->net_len));
TASS Belgium NV	131:4758606c9316	372	if (pfrag->total_len > PICO_IPV4_FRAG_MAX_SIZE) {
TASS Belgium NV	131:4758606c9316	373	reassembly_dbg("BIG frame!!!\n");
TASS Belgium NV	131:4758606c9316	374	pfrag = pico_tree_first(&pico_ipv4_fragmented_tree);
TASS Belgium NV	131:4758606c9316	375	pico_ipv4_fragmented_cleanup(pfrag);
TASS Belgium NV	131:4758606c9316	376	pico_frame_discard(*f);
TASS Belgium NV	131:4758606c9316	377	return 0;
TASS Belgium NV	131:4758606c9316	378	}
tass	68:0847e35d08a6	379
TASS Belgium NV	131:4758606c9316	380	pico_tree_insert(pfrag->t, *f);
TASS Belgium NV	131:4758606c9316	381	return 0;
TASS Belgium NV	131:4758606c9316	382	} else {
TASS Belgium NV	131:4758606c9316	383	reassembly_dbg("REASSEMBLY: silently discard intermediate frame, first packet was lost or disallowed (one fragmented packet at a time)\n");
TASS Belgium NV	131:4758606c9316	384	pico_frame_discard(*f);
TASS Belgium NV	131:4758606c9316	385	return 0;
TASS Belgium NV	131:4758606c9316	386	}
TASS Belgium NV	131:4758606c9316	387	}
TASS Belgium NV	131:4758606c9316	388	} else if (offset) {
TASS Belgium NV	131:4758606c9316	389	reassembly_dbg("REASSEMBLY: last element of a fragmented packet with id %X and offset %u\n", short_be(hdr->id), offset);
TASS Belgium NV	131:4758606c9316	390	pfrag = fragment_find_by_hdr(hdr);
TASS Belgium NV	131:4758606c9316	391	if (pfrag) {
TASS Belgium NV	131:4758606c9316	392	pfrag->total_len = (uint16_t)(pfrag->total_len + (short_be(hdr->len) - (*f)->net_len));
TASS Belgium NV	131:4758606c9316	393	reassembly_dbg("REASSEMBLY: fragmented packet in tree, reassemble packet of %u data bytes\n", pfrag->total_len);
TASS Belgium NV	131:4758606c9316	394	if (pfrag->total_len > PICO_IPV4_FRAG_MAX_SIZE) {
TASS Belgium NV	131:4758606c9316	395	reassembly_dbg("BIG frame!!!\n");
TASS Belgium NV	131:4758606c9316	396	pfrag = pico_tree_first(&pico_ipv4_fragmented_tree);
TASS Belgium NV	131:4758606c9316	397	pico_ipv4_fragmented_cleanup(pfrag);
TASS Belgium NV	131:4758606c9316	398	pico_frame_discard(*f);
TASS Belgium NV	131:4758606c9316	399	return 0;
TASS Belgium NV	131:4758606c9316	400	}
TASS Belgium NV	131:4758606c9316	401
TASS Belgium NV	131:4758606c9316	402	f_new = self->alloc(self, pfrag->total_len);
tass picotcp@tass.be	149:5f4cb161cec3	403	if (!f_new)
tass picotcp@tass.be	149:5f4cb161cec3	404	return -1;
TASS Belgium NV	131:4758606c9316	405
TASS Belgium NV	131:4758606c9316	406	f_frag = pico_tree_first(pfrag->t);
TASS Belgium NV	131:4758606c9316	407	reassembly_dbg("REASSEMBLY: copy IP header information len = %lu\n", f_frag->net_len);
TASS Belgium NV	131:4758606c9316	408	f_frag_hdr = (struct pico_ipv4_hdr *)f_frag->net_hdr;
TASS Belgium NV	131:4758606c9316	409	data_len = (uint16_t)(short_be(f_frag_hdr->len) - f_frag->net_len);
TASS Belgium NV	131:4758606c9316	410	memcpy(f_new->net_hdr, f_frag->net_hdr, f_frag->net_len);
TASS Belgium NV	131:4758606c9316	411	memcpy(f_new->transport_hdr, f_frag->transport_hdr, data_len);
tass picotcp@tass.be	149:5f4cb161cec3	412	f_new->dev = f_frag->dev;
TASS Belgium NV	131:4758606c9316	413	running_pointer = f_new->transport_hdr + data_len;
TASS Belgium NV	131:4758606c9316	414	offset = short_be(f_frag_hdr->frag) & PICO_IPV4_FRAG_MASK;
TASS Belgium NV	131:4758606c9316	415	running_offset = data_len / 8;
TASS Belgium NV	131:4758606c9316	416	pico_tree_delete(pfrag->t, f_frag);
TASS Belgium NV	131:4758606c9316	417	pico_frame_discard(f_frag);
TASS Belgium NV	131:4758606c9316	418	reassembly_dbg("REASSEMBLY: reassembled first packet of %u data bytes, offset = %u next expected offset = %u\n", data_len, offset, running_offset);
tass	68:0847e35d08a6	419
TASS Belgium NV	131:4758606c9316	420	pico_tree_foreach_safe(index, pfrag->t, _tmp)
TASS Belgium NV	131:4758606c9316	421	{
TASS Belgium NV	131:4758606c9316	422	f_frag = index->keyValue;
TASS Belgium NV	131:4758606c9316	423	f_frag_hdr = (struct pico_ipv4_hdr *)f_frag->net_hdr;
TASS Belgium NV	131:4758606c9316	424	data_len = (uint16_t)(short_be(f_frag_hdr->len) - f_frag->net_len);
TASS Belgium NV	131:4758606c9316	425	memcpy(running_pointer, f_frag->transport_hdr, data_len);
TASS Belgium NV	131:4758606c9316	426	running_pointer += data_len;
TASS Belgium NV	131:4758606c9316	427	offset = short_be(f_frag_hdr->frag) & PICO_IPV4_FRAG_MASK;
TASS Belgium NV	131:4758606c9316	428	if (offset != running_offset) {
TASS Belgium NV	131:4758606c9316	429	reassembly_dbg("REASSEMBLY: error reassembling intermediate packet: offset %u != expected offset %u (missing fragment)\n", offset, running_offset);
TASS Belgium NV	131:4758606c9316	430	pico_ipv4_fragmented_cleanup(pfrag);
TASS Belgium NV	131:4758606c9316	431	return -1;
TASS Belgium NV	131:4758606c9316	432	}
tass	68:0847e35d08a6	433
TASS Belgium NV	131:4758606c9316	434	running_offset = (uint16_t)(running_offset + (data_len / 8));
TASS Belgium NV	131:4758606c9316	435	pico_tree_delete(pfrag->t, f_frag);
TASS Belgium NV	131:4758606c9316	436	pico_frame_discard(f_frag);
TASS Belgium NV	131:4758606c9316	437	reassembly_dbg("REASSEMBLY: reassembled intermediate packet of %u data bytes, offset = %u next expected offset = %u\n", data_len, offset, running_offset);
TASS Belgium NV	131:4758606c9316	438	}
TASS Belgium NV	131:4758606c9316	439	pico_tree_delete(&pico_ipv4_fragmented_tree, pfrag);
tass picotcp@tass.be	149:5f4cb161cec3	440	PICO_FREE(pfrag);
tass	68:0847e35d08a6	441
TASS Belgium NV	131:4758606c9316	442	data_len = (uint16_t)(short_be(hdr->len) - (*f)->net_len);
TASS Belgium NV	131:4758606c9316	443	memcpy(running_pointer, (*f)->transport_hdr, data_len);
TASS Belgium NV	131:4758606c9316	444	offset = short_be(hdr->frag) & PICO_IPV4_FRAG_MASK;
TASS Belgium NV	131:4758606c9316	445	pico_frame_discard(*f);
TASS Belgium NV	131:4758606c9316	446	reassembly_dbg("REASSEMBLY: reassembled last packet of %u data bytes, offset = %u\n", data_len, offset);
TASS Belgium NV	131:4758606c9316	447
TASS Belgium NV	131:4758606c9316	448	hdr = (struct pico_ipv4_hdr *)f_new->net_hdr;
TASS Belgium NV	131:4758606c9316	449	hdr->len = pfrag->total_len;
TASS Belgium NV	131:4758606c9316	450	hdr->frag = 0; /* flags cleared and no offset */
TASS Belgium NV	131:4758606c9316	451	hdr->crc = 0;
TASS Belgium NV	131:4758606c9316	452	hdr->crc = short_be(pico_checksum(hdr, f_new->net_len));
TASS Belgium NV	131:4758606c9316	453	/* Optional, the UDP/TCP CRC should already be correct */
TASS Belgium NV	131:4758606c9316	454	if (0) {
tass	68:0847e35d08a6	455	#ifdef PICO_SUPPORT_TCP
TASS Belgium NV	131:4758606c9316	456	} else if (hdr->proto == PICO_PROTO_TCP) {
tass picotcp@tass.be	149:5f4cb161cec3	457	struct pico_tcp_hdr *tcp_hdr = NULL;
TASS Belgium NV	131:4758606c9316	458	tcp_hdr = (struct pico_tcp_hdr *) f_new->transport_hdr;
TASS Belgium NV	131:4758606c9316	459	tcp_hdr->crc = 0;
tass picotcp@tass.be	149:5f4cb161cec3	460	tcp_hdr->crc = short_be(pico_tcp_checksum(f_new));
tass	68:0847e35d08a6	461	#endif
tass	68:0847e35d08a6	462	#ifdef PICO_SUPPORT_UDP
TASS Belgium NV	131:4758606c9316	463	} else if (hdr->proto == PICO_PROTO_UDP) {
tass picotcp@tass.be	149:5f4cb161cec3	464	struct pico_udp_hdr *udp_hdr = NULL;
TASS Belgium NV	131:4758606c9316	465	udp_hdr = (struct pico_udp_hdr *) f_new->transport_hdr;
TASS Belgium NV	131:4758606c9316	466	udp_hdr->crc = 0;
TASS Belgium NV	131:4758606c9316	467	udp_hdr->crc = short_be(pico_udp_checksum_ipv4(f_new));
tass	68:0847e35d08a6	468	#endif
TASS Belgium NV	131:4758606c9316	469	}
TASS Belgium NV	131:4758606c9316	470
TASS Belgium NV	131:4758606c9316	471	reassembly_dbg("REASSEMBLY: packet with id %X reassembled correctly\n", short_be(hdr->id));
TASS Belgium NV	131:4758606c9316	472	*f = f_new;
TASS Belgium NV	131:4758606c9316	473	return 1;
TASS Belgium NV	131:4758606c9316	474	} else {
TASS Belgium NV	131:4758606c9316	475	reassembly_dbg("REASSEMBLY: silently discard last frame, first packet was lost or disallowed (one fragmented packet at a time)\n");
TASS Belgium NV	131:4758606c9316	476	pico_frame_discard(*f);
TASS Belgium NV	131:4758606c9316	477	return 0;
TASS Belgium NV	131:4758606c9316	478	}
tass	68:0847e35d08a6	479	} else {
TASS Belgium NV	131:4758606c9316	480	return 1;
tass	68:0847e35d08a6	481	}
tass	68:0847e35d08a6	482	}
tass	68:0847e35d08a6	483	#else
tass	70:cd218dd180e5	484	static inline int8_t pico_ipv4_fragmented_check(struct pico_protocol self, struct pico_frame *f)
tass	68:0847e35d08a6	485	{
TASS Belgium NV	131:4758606c9316	486	return 1;
tass	68:0847e35d08a6	487	}
tass	68:0847e35d08a6	488	#endif /* PICO_SUPPORT_IPFRAG */
tass	68:0847e35d08a6	489
tass	68:0847e35d08a6	490	#ifdef PICO_SUPPORT_CRC
tass	68:0847e35d08a6	491	static inline int pico_ipv4_crc_check(struct pico_frame *f)
tass	68:0847e35d08a6	492	{
TASS Belgium NV	131:4758606c9316	493	uint16_t checksum_invalid = 1;
TASS Belgium NV	131:4758606c9316	494	struct pico_ipv4_hdr hdr = (struct pico_ipv4_hdr ) f->net_hdr;
tass	68:0847e35d08a6	495
TASS Belgium NV	131:4758606c9316	496	checksum_invalid = short_be(pico_checksum(hdr, f->net_len));
TASS Belgium NV	131:4758606c9316	497	if (checksum_invalid) {
TASS Belgium NV	131:4758606c9316	498	dbg("IP: checksum failed!\n");
TASS Belgium NV	131:4758606c9316	499	pico_frame_discard(f);
TASS Belgium NV	131:4758606c9316	500	return 0;
TASS Belgium NV	131:4758606c9316	501	}
TASS Belgium NV	131:4758606c9316	502
TASS Belgium NV	131:4758606c9316	503	return 1;
tass	68:0847e35d08a6	504	}
tass	68:0847e35d08a6	505	#else
tass	68:0847e35d08a6	506	static inline int pico_ipv4_crc_check(struct pico_frame *f)
tass	68:0847e35d08a6	507	{
TASS Belgium NV	131:4758606c9316	508	IGNORE_PARAMETER(f);
TASS Belgium NV	131:4758606c9316	509	return 1;
tass	68:0847e35d08a6	510	}
tass	68:0847e35d08a6	511	#endif /* PICO_SUPPORT_CRC */
tass	68:0847e35d08a6	512
tass	68:0847e35d08a6	513	static int pico_ipv4_forward(struct pico_frame *f);
tass	68:0847e35d08a6	514	#ifdef PICO_SUPPORT_MCAST
tass	68:0847e35d08a6	515	static int pico_ipv4_mcast_filter(struct pico_frame *f);
tass	68:0847e35d08a6	516	#endif
tass	68:0847e35d08a6	517
tass	68:0847e35d08a6	518	static int ipv4_link_compare(void ka, void kb)
tass	68:0847e35d08a6	519	{
TASS Belgium NV	131:4758606c9316	520	struct pico_ipv4_link a = ka, b = kb;
TASS Belgium NV	131:4758606c9316	521	if (a->address.addr < b->address.addr)
TASS Belgium NV	131:4758606c9316	522	return -1;
TASS Belgium NV	131:4758606c9316	523
TASS Belgium NV	131:4758606c9316	524	if (a->address.addr > b->address.addr)
TASS Belgium NV	131:4758606c9316	525	return 1;
tass	68:0847e35d08a6	526
TASS Belgium NV	131:4758606c9316	527	/* zero can be assigned multiple times (e.g. for DHCP) */
TASS Belgium NV	131:4758606c9316	528	if (a->dev != NULL && b->dev != NULL && a->address.addr == PICO_IP4_ANY && b->address.addr == PICO_IP4_ANY) {
TASS Belgium NV	131:4758606c9316	529	if (a->dev < b->dev)
TASS Belgium NV	131:4758606c9316	530	return -1;
TASS Belgium NV	131:4758606c9316	531
TASS Belgium NV	131:4758606c9316	532	if (a->dev > b->dev)
TASS Belgium NV	131:4758606c9316	533	return 1;
TASS Belgium NV	131:4758606c9316	534	}
TASS Belgium NV	131:4758606c9316	535
TASS Belgium NV	131:4758606c9316	536	return 0;
tass	68:0847e35d08a6	537	}
tass	68:0847e35d08a6	538
tass	68:0847e35d08a6	539	PICO_TREE_DECLARE(Tree_dev_link, ipv4_link_compare);
tass	68:0847e35d08a6	540
tass picotcp@tass.be	149:5f4cb161cec3	541	static int pico_ipv4_process_bcast_in(struct pico_frame *f)
tass picotcp@tass.be	149:5f4cb161cec3	542	{
tass picotcp@tass.be	149:5f4cb161cec3	543	struct pico_ipv4_hdr hdr = (struct pico_ipv4_hdr ) f->net_hdr;
tass picotcp@tass.be	149:5f4cb161cec3	544	#ifdef PICO_SUPPORT_UDP
tass picotcp@tass.be	149:5f4cb161cec3	545	if (pico_ipv4_is_broadcast(hdr->dst.addr) && (hdr->proto == PICO_PROTO_UDP)) {
tass picotcp@tass.be	149:5f4cb161cec3	546	/* Receiving UDP broadcast datagram */
tass picotcp@tass.be	149:5f4cb161cec3	547	f->flags \|= PICO_FRAME_FLAG_BCAST;
tass picotcp@tass.be	149:5f4cb161cec3	548	pico_enqueue(pico_proto_udp.q_in, f);
tass picotcp@tass.be	149:5f4cb161cec3	549	return 1;
tass picotcp@tass.be	149:5f4cb161cec3	550	}
tass picotcp@tass.be	149:5f4cb161cec3	551
tass picotcp@tass.be	149:5f4cb161cec3	552	if (pico_ipv4_is_broadcast(hdr->dst.addr) && (hdr->proto == PICO_PROTO_ICMP4)) {
tass picotcp@tass.be	149:5f4cb161cec3	553	/* Receiving ICMP4 bcast packet */
tass picotcp@tass.be	149:5f4cb161cec3	554	f->flags \|= PICO_FRAME_FLAG_BCAST;
tass picotcp@tass.be	149:5f4cb161cec3	555	pico_enqueue(pico_proto_icmp4.q_in, f);
tass picotcp@tass.be	149:5f4cb161cec3	556	return 1;
tass picotcp@tass.be	149:5f4cb161cec3	557	}
tass picotcp@tass.be	149:5f4cb161cec3	558
tass picotcp@tass.be	149:5f4cb161cec3	559	#endif
tass picotcp@tass.be	149:5f4cb161cec3	560	return 0;
tass picotcp@tass.be	149:5f4cb161cec3	561	}
tass picotcp@tass.be	149:5f4cb161cec3	562
tass picotcp@tass.be	149:5f4cb161cec3	563	static int pico_ipv4_process_mcast_in(struct pico_frame *f)
tass picotcp@tass.be	149:5f4cb161cec3	564	{
tass picotcp@tass.be	149:5f4cb161cec3	565	struct pico_ipv4_hdr hdr = (struct pico_ipv4_hdr ) f->net_hdr;
tass picotcp@tass.be	149:5f4cb161cec3	566	if (pico_ipv4_is_multicast(hdr->dst.addr)) {
tass picotcp@tass.be	149:5f4cb161cec3	567	#ifdef PICO_SUPPORT_MCAST
tass picotcp@tass.be	149:5f4cb161cec3	568	/* Receiving UDP multicast datagram TODO set f->flags? */
tass picotcp@tass.be	149:5f4cb161cec3	569	if (hdr->proto == PICO_PROTO_IGMP) {
tass picotcp@tass.be	149:5f4cb161cec3	570	ip_mcast_dbg("MCAST: received IGMP message\n");
tass picotcp@tass.be	149:5f4cb161cec3	571	pico_transport_receive(f, PICO_PROTO_IGMP);
tass picotcp@tass.be	149:5f4cb161cec3	572	return 1;
tass picotcp@tass.be	149:5f4cb161cec3	573	} else if ((pico_ipv4_mcast_filter(f) == 0) && (hdr->proto == PICO_PROTO_UDP)) {
tass picotcp@tass.be	149:5f4cb161cec3	574	pico_enqueue(pico_proto_udp.q_in, f);
tass picotcp@tass.be	149:5f4cb161cec3	575	return 1;
tass picotcp@tass.be	149:5f4cb161cec3	576	}
tass picotcp@tass.be	149:5f4cb161cec3	577
tass picotcp@tass.be	149:5f4cb161cec3	578	#endif
tass picotcp@tass.be	149:5f4cb161cec3	579	pico_frame_discard(f);
tass picotcp@tass.be	149:5f4cb161cec3	580	return 1;
tass picotcp@tass.be	149:5f4cb161cec3	581	}
tass picotcp@tass.be	149:5f4cb161cec3	582
tass picotcp@tass.be	149:5f4cb161cec3	583	return 0;
tass picotcp@tass.be	149:5f4cb161cec3	584	}
tass picotcp@tass.be	149:5f4cb161cec3	585
tass picotcp@tass.be	149:5f4cb161cec3	586	static int pico_ipv4_process_local_unicast_in(struct pico_frame *f)
tass picotcp@tass.be	149:5f4cb161cec3	587	{
tass picotcp@tass.be	149:5f4cb161cec3	588	struct pico_ipv4_hdr hdr = (struct pico_ipv4_hdr ) f->net_hdr;
tass picotcp@tass.be	149:5f4cb161cec3	589	struct pico_ipv4_link test = {
tass picotcp@tass.be	149:5f4cb161cec3	590	.address = {.addr = PICO_IP4_ANY}, .dev = NULL
tass picotcp@tass.be	149:5f4cb161cec3	591	};
tass picotcp@tass.be	149:5f4cb161cec3	592	if (pico_ipv4_link_find(&hdr->dst)) {
tass picotcp@tass.be	149:5f4cb161cec3	593	if (pico_ipv4_nat_inbound(f, &hdr->dst) == 0)
tass picotcp@tass.be	149:5f4cb161cec3	594	pico_enqueue(pico_proto_ipv4.q_in, f); /* dst changed, reprocess */
tass picotcp@tass.be	149:5f4cb161cec3	595	else
tass picotcp@tass.be	149:5f4cb161cec3	596	pico_transport_receive(f, hdr->proto);
tass picotcp@tass.be	149:5f4cb161cec3	597
tass picotcp@tass.be	149:5f4cb161cec3	598	return 1;
tass picotcp@tass.be	149:5f4cb161cec3	599	} else if (pico_tree_findKey(&Tree_dev_link, &test)) {
tass picotcp@tass.be	149:5f4cb161cec3	600	#ifdef PICO_SUPPORT_UDP
tass picotcp@tass.be	149:5f4cb161cec3	601	/* address of this device is apparently 0.0.0.0; might be a DHCP packet */
tass picotcp@tass.be	149:5f4cb161cec3	602	/* XXX KRO: is obsolete. Broadcast flag is set on outgoing DHCP messages.
tass picotcp@tass.be	149:5f4cb161cec3	603	* incomming DHCP messages are to be broadcasted. Our current DHCP server
tass picotcp@tass.be	149:5f4cb161cec3	604	* implementation does not take this flag into account yet though ... */
tass picotcp@tass.be	149:5f4cb161cec3	605	pico_enqueue(pico_proto_udp.q_in, f);
tass picotcp@tass.be	149:5f4cb161cec3	606	return 1;
tass picotcp@tass.be	149:5f4cb161cec3	607	#endif
tass picotcp@tass.be	149:5f4cb161cec3	608	}
tass picotcp@tass.be	149:5f4cb161cec3	609
tass picotcp@tass.be	149:5f4cb161cec3	610	return 0;
tass picotcp@tass.be	149:5f4cb161cec3	611	}
tass picotcp@tass.be	149:5f4cb161cec3	612
tass picotcp@tass.be	149:5f4cb161cec3	613	static void pico_ipv4_process_finally_try_forward(struct pico_frame *f)
tass picotcp@tass.be	149:5f4cb161cec3	614	{
tass picotcp@tass.be	149:5f4cb161cec3	615	struct pico_ipv4_hdr hdr = (struct pico_ipv4_hdr ) f->net_hdr;
tass picotcp@tass.be	149:5f4cb161cec3	616	if((pico_ipv4_is_broadcast(hdr->dst.addr)))
tass picotcp@tass.be	149:5f4cb161cec3	617	{
tass picotcp@tass.be	149:5f4cb161cec3	618	/* don't forward broadcast frame, discard! */
tass picotcp@tass.be	149:5f4cb161cec3	619	pico_frame_discard(f);
tass picotcp@tass.be	149:5f4cb161cec3	620	} else if (pico_ipv4_forward(f) != 0) {
tass picotcp@tass.be	149:5f4cb161cec3	621	pico_frame_discard(f);
tass picotcp@tass.be	149:5f4cb161cec3	622	/* dbg("Forward failed.\n"); */
tass picotcp@tass.be	149:5f4cb161cec3	623	}
tass picotcp@tass.be	149:5f4cb161cec3	624	}
tass picotcp@tass.be	149:5f4cb161cec3	625
tass picotcp@tass.be	149:5f4cb161cec3	626
tass picotcp@tass.be	149:5f4cb161cec3	627
tass	68:0847e35d08a6	628	static int pico_ipv4_process_in(struct pico_protocol self, struct pico_frame f)
tass	68:0847e35d08a6	629	{
TASS Belgium NV	131:4758606c9316	630	uint8_t option_len = 0;
TASS Belgium NV	131:4758606c9316	631	int ret = 0;
TASS Belgium NV	131:4758606c9316	632	struct pico_ipv4_hdr hdr = (struct pico_ipv4_hdr ) f->net_hdr;
tass	68:0847e35d08a6	633
TASS Belgium NV	131:4758606c9316	634	/* NAT needs transport header information */
TASS Belgium NV	131:4758606c9316	635	if(((hdr->vhl) & 0x0F) > 5) {
TASS Belgium NV	131:4758606c9316	636	option_len = (uint8_t)(4 * (((hdr->vhl) & 0x0F) - 5));
TASS Belgium NV	131:4758606c9316	637	}
TASS Belgium NV	131:4758606c9316	638
TASS Belgium NV	131:4758606c9316	639	f->transport_hdr = ((uint8_t *)f->net_hdr) + PICO_SIZE_IP4HDR + option_len;
TASS Belgium NV	131:4758606c9316	640	f->transport_len = (uint16_t)(short_be(hdr->len) - PICO_SIZE_IP4HDR - option_len);
TASS Belgium NV	131:4758606c9316	641	f->net_len = (uint16_t)(PICO_SIZE_IP4HDR + option_len);
tass	68:0847e35d08a6	642
tass	68:0847e35d08a6	643	#ifdef PICO_SUPPORT_IPFILTER
TASS Belgium NV	131:4758606c9316	644	if (ipfilter(f)) {
TASS Belgium NV	131:4758606c9316	645	/pico_frame is discarded as result of the filtering/
TASS Belgium NV	131:4758606c9316	646	return 0;
TASS Belgium NV	131:4758606c9316	647	}
TASS Belgium NV	131:4758606c9316	648
tass	68:0847e35d08a6	649	#endif
tass	68:0847e35d08a6	650
TASS Belgium NV	131:4758606c9316	651	/* ret == 1 indicates to continue the function */
TASS Belgium NV	131:4758606c9316	652	ret = pico_ipv4_crc_check(f);
TASS Belgium NV	131:4758606c9316	653	if (ret < 1)
TASS Belgium NV	131:4758606c9316	654	return ret;
TASS Belgium NV	131:4758606c9316	655
TASS Belgium NV	131:4758606c9316	656	ret = pico_ipv4_fragmented_check(self, &f);
TASS Belgium NV	131:4758606c9316	657	if (ret < 1)
TASS Belgium NV	131:4758606c9316	658	return ret;
tass	68:0847e35d08a6	659
tass picotcp@tass.be	137:a1c8bfa9d691	660	/* Validate source IP address. Discard quietly if invalid */
tass picotcp@tass.be	149:5f4cb161cec3	661	if (!pico_ipv4_is_valid_src(hdr->src.addr, f->dev)) {
TASS Belgium NV	131:4758606c9316	662	pico_frame_discard(f);
TASS Belgium NV	131:4758606c9316	663	return 0;
TASS Belgium NV	131:4758606c9316	664	}
TASS Belgium NV	131:4758606c9316	665
TASS Belgium NV	131:4758606c9316	666	if (hdr->frag & 0x80) {
TASS Belgium NV	131:4758606c9316	667	pico_frame_discard(f); /* RFC 3514 */
TASS Belgium NV	131:4758606c9316	668	return 0;
TASS Belgium NV	131:4758606c9316	669	}
TASS Belgium NV	131:4758606c9316	670
tass picotcp@tass.be	149:5f4cb161cec3	671	if (pico_ipv4_process_bcast_in(f) > 0)
tass picotcp@tass.be	149:5f4cb161cec3	672	return 0;
TASS Belgium NV	131:4758606c9316	673
tass picotcp@tass.be	149:5f4cb161cec3	674	if (pico_ipv4_process_mcast_in(f) > 0)
tass picotcp@tass.be	149:5f4cb161cec3	675	return 0;
TASS Belgium NV	131:4758606c9316	676
tass picotcp@tass.be	149:5f4cb161cec3	677	if (pico_ipv4_process_local_unicast_in(f) > 0)
tass picotcp@tass.be	149:5f4cb161cec3	678	return 0;
tass picotcp@tass.be	149:5f4cb161cec3	679
tass picotcp@tass.be	149:5f4cb161cec3	680	pico_ipv4_process_finally_try_forward(f);
TASS Belgium NV	131:4758606c9316	681
TASS Belgium NV	131:4758606c9316	682	return 0;
tass	68:0847e35d08a6	683	}
tass	68:0847e35d08a6	684
tass	68:0847e35d08a6	685	PICO_TREE_DECLARE(Routes, ipv4_route_compare);
tass	68:0847e35d08a6	686
tass	68:0847e35d08a6	687
tass	68:0847e35d08a6	688	static int pico_ipv4_process_out(struct pico_protocol self, struct pico_frame f)
tass	68:0847e35d08a6	689	{
TASS Belgium NV	131:4758606c9316	690	IGNORE_PARAMETER(self);
TASS Belgium NV	131:4758606c9316	691	f->start = (uint8_t*) f->net_hdr;
tass	68:0847e35d08a6	692	#ifdef PICO_SUPPORT_IPFILTER
TASS Belgium NV	131:4758606c9316	693	if (ipfilter(f)) {
TASS Belgium NV	131:4758606c9316	694	/pico_frame is discarded as result of the filtering/
TASS Belgium NV	131:4758606c9316	695	return 0;
TASS Belgium NV	131:4758606c9316	696	}
TASS Belgium NV	131:4758606c9316	697
tass	68:0847e35d08a6	698	#endif
TASS Belgium NV	131:4758606c9316	699	return pico_sendto_dev(f);
tass	68:0847e35d08a6	700	}
tass	68:0847e35d08a6	701
tass	68:0847e35d08a6	702
tass	70:cd218dd180e5	703	static struct pico_frame pico_ipv4_alloc(struct pico_protocol self, uint16_t size)
tass	68:0847e35d08a6	704	{
TASS Belgium NV	131:4758606c9316	705	struct pico_frame *f = pico_frame_alloc(size + PICO_SIZE_IP4HDR + PICO_SIZE_ETHHDR);
TASS Belgium NV	131:4758606c9316	706	IGNORE_PARAMETER(self);
TASS Belgium NV	131:4758606c9316	707
TASS Belgium NV	131:4758606c9316	708	if (!f)
TASS Belgium NV	131:4758606c9316	709	return NULL;
tass	68:0847e35d08a6	710
TASS Belgium NV	131:4758606c9316	711	f->datalink_hdr = f->buffer;
TASS Belgium NV	131:4758606c9316	712	f->net_hdr = f->buffer + PICO_SIZE_ETHHDR;
TASS Belgium NV	131:4758606c9316	713	f->net_len = PICO_SIZE_IP4HDR;
TASS Belgium NV	131:4758606c9316	714	f->transport_hdr = f->net_hdr + PICO_SIZE_IP4HDR;
TASS Belgium NV	131:4758606c9316	715	f->transport_len = size;
TASS Belgium NV	131:4758606c9316	716	f->len = size + PICO_SIZE_IP4HDR;
TASS Belgium NV	131:4758606c9316	717	return f;
tass	68:0847e35d08a6	718	}
tass	68:0847e35d08a6	719
tass	68:0847e35d08a6	720	static int pico_ipv4_frame_sock_push(struct pico_protocol self, struct pico_frame f);
tass	68:0847e35d08a6	721
tass	68:0847e35d08a6	722	/* Interface: protocol definition */
tass	68:0847e35d08a6	723	struct pico_protocol pico_proto_ipv4 = {
TASS Belgium NV	131:4758606c9316	724	.name = "ipv4",
TASS Belgium NV	131:4758606c9316	725	.proto_number = PICO_PROTO_IPV4,
TASS Belgium NV	131:4758606c9316	726	.layer = PICO_LAYER_NETWORK,
TASS Belgium NV	131:4758606c9316	727	.alloc = pico_ipv4_alloc,
TASS Belgium NV	131:4758606c9316	728	.process_in = pico_ipv4_process_in,
TASS Belgium NV	131:4758606c9316	729	.process_out = pico_ipv4_process_out,
TASS Belgium NV	131:4758606c9316	730	.push = pico_ipv4_frame_sock_push,
TASS Belgium NV	131:4758606c9316	731	.q_in = &in,
TASS Belgium NV	131:4758606c9316	732	.q_out = &out,
tass	68:0847e35d08a6	733	};
tass	68:0847e35d08a6	734
tass	68:0847e35d08a6	735	struct pico_ipv4_route
tass	68:0847e35d08a6	736	{
TASS Belgium NV	131:4758606c9316	737	struct pico_ip4 dest;
TASS Belgium NV	131:4758606c9316	738	struct pico_ip4 netmask;
TASS Belgium NV	131:4758606c9316	739	struct pico_ip4 gateway;
TASS Belgium NV	131:4758606c9316	740	struct pico_ipv4_link *link;
TASS Belgium NV	131:4758606c9316	741	uint32_t metric;
tass	68:0847e35d08a6	742	};
tass	68:0847e35d08a6	743
tass	68:0847e35d08a6	744
TASS Belgium NV	131:4758606c9316	745	static int ipv4_route_compare(void ka, void kb)
tass	68:0847e35d08a6	746	{
TASS Belgium NV	131:4758606c9316	747	struct pico_ipv4_route a = ka, b = kb;
tass picotcp@tass.be	149:5f4cb161cec3	748	uint32_t a_nm, b_nm;
tass picotcp@tass.be	149:5f4cb161cec3	749
tass picotcp@tass.be	149:5f4cb161cec3	750	a_nm = long_be(a->netmask.addr);
tass picotcp@tass.be	149:5f4cb161cec3	751	b_nm = long_be(b->netmask.addr);
tass	68:0847e35d08a6	752
TASS Belgium NV	131:4758606c9316	753	/* Routes are sorted by (host side) netmask len, then by addr, then by metric. */
tass picotcp@tass.be	149:5f4cb161cec3	754	if (a_nm < b_nm)
TASS Belgium NV	131:4758606c9316	755	return -1;
tass	68:0847e35d08a6	756
tass picotcp@tass.be	149:5f4cb161cec3	757	if (b_nm < a_nm)
TASS Belgium NV	131:4758606c9316	758	return 1;
tass	68:0847e35d08a6	759
TASS Belgium NV	131:4758606c9316	760	if (a->dest.addr < b->dest.addr)
TASS Belgium NV	131:4758606c9316	761	return -1;
tass	68:0847e35d08a6	762
TASS Belgium NV	131:4758606c9316	763	if (a->dest.addr > b->dest.addr)
TASS Belgium NV	131:4758606c9316	764	return 1;
tass	68:0847e35d08a6	765
TASS Belgium NV	131:4758606c9316	766	if (a->metric < b->metric)
TASS Belgium NV	131:4758606c9316	767	return -1;
tass	68:0847e35d08a6	768
TASS Belgium NV	131:4758606c9316	769	if (a->metric > b->metric)
TASS Belgium NV	131:4758606c9316	770	return 1;
tass	68:0847e35d08a6	771
TASS Belgium NV	131:4758606c9316	772	return 0;
tass	68:0847e35d08a6	773	}
tass	68:0847e35d08a6	774
tass	68:0847e35d08a6	775	static struct pico_ipv4_route route_find(const struct pico_ip4 addr)
tass	68:0847e35d08a6	776	{
TASS Belgium NV	131:4758606c9316	777	struct pico_ipv4_route *r;
TASS Belgium NV	131:4758606c9316	778	struct pico_tree_node *index;
tass	68:0847e35d08a6	779
TASS Belgium NV	131:4758606c9316	780	if(addr->addr != PICO_IP4_BCAST)
tass	68:0847e35d08a6	781	{
TASS Belgium NV	131:4758606c9316	782	pico_tree_foreach_reverse(index, &Routes) {
TASS Belgium NV	131:4758606c9316	783	r = index->keyValue;
TASS Belgium NV	131:4758606c9316	784	if ((addr->addr & (r->netmask.addr)) == (r->dest.addr)) {
TASS Belgium NV	131:4758606c9316	785	return r;
TASS Belgium NV	131:4758606c9316	786	}
TASS Belgium NV	131:4758606c9316	787	}
tass	68:0847e35d08a6	788	}
tass	68:0847e35d08a6	789	else
tass	68:0847e35d08a6	790	{
TASS Belgium NV	131:4758606c9316	791	r = pico_tree_first(&Routes);
TASS Belgium NV	131:4758606c9316	792	if(!r->netmask.addr)
TASS Belgium NV	131:4758606c9316	793	{
TASS Belgium NV	131:4758606c9316	794	return r;
TASS Belgium NV	131:4758606c9316	795	}
TASS Belgium NV	131:4758606c9316	796	else
TASS Belgium NV	131:4758606c9316	797	{
TASS Belgium NV	131:4758606c9316	798	dbg("WARNING: no default route for a global broadcast found\n");
TASS Belgium NV	131:4758606c9316	799	}
tass	68:0847e35d08a6	800	}
tass	68:0847e35d08a6	801
TASS Belgium NV	131:4758606c9316	802	return NULL;
tass	68:0847e35d08a6	803	}
tass	68:0847e35d08a6	804
tass	68:0847e35d08a6	805	struct pico_ip4 pico_ipv4_route_get_gateway(struct pico_ip4 *addr)
tass	68:0847e35d08a6	806	{
TASS Belgium NV	131:4758606c9316	807	struct pico_ip4 nullip;
TASS Belgium NV	131:4758606c9316	808	struct pico_ipv4_route *route;
TASS Belgium NV	131:4758606c9316	809	nullip.addr = 0U;
tass	68:0847e35d08a6	810
TASS Belgium NV	131:4758606c9316	811	if(!addr) {
TASS Belgium NV	131:4758606c9316	812	pico_err = PICO_ERR_EINVAL;
TASS Belgium NV	131:4758606c9316	813	return nullip;
TASS Belgium NV	131:4758606c9316	814	}
tass	68:0847e35d08a6	815
TASS Belgium NV	131:4758606c9316	816	route = route_find(addr);
TASS Belgium NV	131:4758606c9316	817	if (!route) {
TASS Belgium NV	131:4758606c9316	818	pico_err = PICO_ERR_EHOSTUNREACH;
TASS Belgium NV	131:4758606c9316	819	return nullip;
TASS Belgium NV	131:4758606c9316	820	}
TASS Belgium NV	131:4758606c9316	821	else
TASS Belgium NV	131:4758606c9316	822	return route->gateway;
tass	68:0847e35d08a6	823	}
tass	68:0847e35d08a6	824
tass	68:0847e35d08a6	825	struct pico_ip4 pico_ipv4_source_find(const struct pico_ip4 dst)
tass	68:0847e35d08a6	826	{
TASS Belgium NV	131:4758606c9316	827	struct pico_ip4 *myself = NULL;
TASS Belgium NV	131:4758606c9316	828	struct pico_ipv4_route *rt;
TASS Belgium NV	131:4758606c9316	829
TASS Belgium NV	131:4758606c9316	830	if(!dst) {
TASS Belgium NV	131:4758606c9316	831	pico_err = PICO_ERR_EINVAL;
TASS Belgium NV	131:4758606c9316	832	return NULL;
TASS Belgium NV	131:4758606c9316	833	}
tass	68:0847e35d08a6	834
TASS Belgium NV	131:4758606c9316	835	rt = route_find(dst);
TASS Belgium NV	131:4758606c9316	836	if (rt) {
TASS Belgium NV	131:4758606c9316	837	myself = &rt->link->address;
TASS Belgium NV	131:4758606c9316	838	} else
TASS Belgium NV	131:4758606c9316	839	pico_err = PICO_ERR_EHOSTUNREACH;
tass	68:0847e35d08a6	840
TASS Belgium NV	131:4758606c9316	841	return myself;
tass	68:0847e35d08a6	842	}
tass	68:0847e35d08a6	843
tass	68:0847e35d08a6	844
tass	68:0847e35d08a6	845	#ifdef PICO_SUPPORT_MCAST
tass	68:0847e35d08a6	846	/* link
TASS Belgium NV	131:4758606c9316	847	* \|
tass	68:0847e35d08a6	848	* MCASTGroups
tass	68:0847e35d08a6	849	* \| \| \|
tass	68:0847e35d08a6	850	* ------------ \| ------------
tass	68:0847e35d08a6	851	* \| \| \|
TASS Belgium NV	131:4758606c9316	852	* MCASTSources MCASTSources MCASTSources
tass	68:0847e35d08a6	853	* \| \| \| \| \| \| \| \| \| \| \| \|
tass	68:0847e35d08a6	854	* S S S S S S S S S S S S
tass	68:0847e35d08a6	855	*
tass	68:0847e35d08a6	856	* MCASTGroups: RBTree(mcast_group)
tass	68:0847e35d08a6	857	* MCASTSources: RBTree(source)
tass	68:0847e35d08a6	858	*/
TASS Belgium NV	131:4758606c9316	859	static int ipv4_mcast_groups_cmp(void ka, void kb)
tass	68:0847e35d08a6	860	{
TASS Belgium NV	131:4758606c9316	861	struct pico_mcast_group a = ka, b = kb;
TASS Belgium NV	131:4758606c9316	862	if (a->mcast_addr.addr < b->mcast_addr.addr) {
TASS Belgium NV	131:4758606c9316	863	return -1;
TASS Belgium NV	131:4758606c9316	864	} else if (a->mcast_addr.addr > b->mcast_addr.addr) {
TASS Belgium NV	131:4758606c9316	865	return 1;
TASS Belgium NV	131:4758606c9316	866	} else {
TASS Belgium NV	131:4758606c9316	867	return 0;
TASS Belgium NV	131:4758606c9316	868	}
tass	68:0847e35d08a6	869	}
tass	68:0847e35d08a6	870
tass	68:0847e35d08a6	871	static int ipv4_mcast_sources_cmp(void ka, void kb)
tass	68:0847e35d08a6	872	{
TASS Belgium NV	131:4758606c9316	873	struct pico_ip4 a = ka, b = kb;
TASS Belgium NV	131:4758606c9316	874	if (a->addr < b->addr)
TASS Belgium NV	131:4758606c9316	875	return -1;
TASS Belgium NV	131:4758606c9316	876
TASS Belgium NV	131:4758606c9316	877	if (a->addr > b->addr)
TASS Belgium NV	131:4758606c9316	878	return 1;
TASS Belgium NV	131:4758606c9316	879
TASS Belgium NV	131:4758606c9316	880	return 0;
tass	68:0847e35d08a6	881	}
tass	68:0847e35d08a6	882
tass	68:0847e35d08a6	883	static void pico_ipv4_mcast_print_groups(struct pico_ipv4_link *mcast_link)
tass	68:0847e35d08a6	884	{
TASS Belgium NV	131:4758606c9316	885	uint16_t i = 0;
tass picotcp@tass.be	149:5f4cb161cec3	886	struct pico_mcast_group *g = NULL;
tass picotcp@tass.be	149:5f4cb161cec3	887	struct pico_ip4 *source = NULL;
TASS Belgium NV	131:4758606c9316	888	struct pico_tree_node index = NULL, index2 = NULL;
tass picotcp@tass.be	149:5f4cb161cec3	889	(void) source;
tass	68:0847e35d08a6	890
TASS Belgium NV	131:4758606c9316	891	ip_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
TASS Belgium NV	131:4758606c9316	892	ip_mcast_dbg("+ MULTICAST list interface %-16s +\n", mcast_link->dev->name);
TASS Belgium NV	131:4758606c9316	893	ip_mcast_dbg("+---------------------------------------------------------------------------------+\n");
TASS Belgium NV	131:4758606c9316	894	ip_mcast_dbg("+ nr \| interface \| host group \| reference count \| filter mode \| source +\n");
TASS Belgium NV	131:4758606c9316	895	ip_mcast_dbg("+---------------------------------------------------------------------------------+\n");
tass	68:0847e35d08a6	896
TASS Belgium NV	131:4758606c9316	897	pico_tree_foreach(index, mcast_link->MCASTGroups)
tass	68:0847e35d08a6	898	{
TASS Belgium NV	131:4758606c9316	899	g = index->keyValue;
TASS Belgium NV	131:4758606c9316	900	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, "");
TASS Belgium NV	131:4758606c9316	901	pico_tree_foreach(index2, &g->MCASTSources)
TASS Belgium NV	131:4758606c9316	902	{
TASS Belgium NV	131:4758606c9316	903	source = index2->keyValue;
TASS Belgium NV	131:4758606c9316	904	ip_mcast_dbg("+ %4s \| %16s \| %8s \| %5s \| %s \| %08X +\n", "", "", "", "", "", source->addr);
TASS Belgium NV	131:4758606c9316	905	}
TASS Belgium NV	131:4758606c9316	906	i++;
tass	68:0847e35d08a6	907	}
TASS Belgium NV	131:4758606c9316	908	ip_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
tass	68:0847e35d08a6	909	}
tass	68:0847e35d08a6	910
tass	70:cd218dd180e5	911	static int mcast_group_update(struct pico_mcast_group g, struct pico_tree MCASTFilter, uint8_t filter_mode)
tass	70:cd218dd180e5	912	{
TASS Belgium NV	131:4758606c9316	913	struct pico_tree_node index = NULL, _tmp = NULL;
TASS Belgium NV	131:4758606c9316	914	struct pico_ip4 *source = NULL;
TASS Belgium NV	131:4758606c9316	915	/* cleanup filter */
TASS Belgium NV	131:4758606c9316	916	pico_tree_foreach_safe(index, &g->MCASTSources, _tmp)
tass	70:cd218dd180e5	917	{
TASS Belgium NV	131:4758606c9316	918	source = index->keyValue;
TASS Belgium NV	131:4758606c9316	919	pico_tree_delete(&g->MCASTSources, source);
tass picotcp@tass.be	149:5f4cb161cec3	920	PICO_FREE(source);
tass	70:cd218dd180e5	921	}
TASS Belgium NV	131:4758606c9316	922	/* insert new filter */
TASS Belgium NV	131:4758606c9316	923	if (MCASTFilter) {
TASS Belgium NV	131:4758606c9316	924	pico_tree_foreach(index, MCASTFilter)
TASS Belgium NV	131:4758606c9316	925	{
tass picotcp@tass.be	149:5f4cb161cec3	926	if (index->keyValue) {
tass picotcp@tass.be	149:5f4cb161cec3	927	source = PICO_ZALLOC(sizeof(struct pico_ip4));
tass picotcp@tass.be	149:5f4cb161cec3	928	if (!source) {
tass picotcp@tass.be	149:5f4cb161cec3	929	pico_err = PICO_ERR_ENOMEM;
tass picotcp@tass.be	149:5f4cb161cec3	930	return -1;
tass picotcp@tass.be	149:5f4cb161cec3	931	}
tass picotcp@tass.be	149:5f4cb161cec3	932
tass picotcp@tass.be	149:5f4cb161cec3	933	source->addr = ((struct pico_ip4 *)index->keyValue)->addr;
tass picotcp@tass.be	149:5f4cb161cec3	934	pico_tree_insert(&g->MCASTSources, source);
TASS Belgium NV	131:4758606c9316	935	}
TASS Belgium NV	131:4758606c9316	936	}
TASS Belgium NV	131:4758606c9316	937	}
TASS Belgium NV	131:4758606c9316	938
TASS Belgium NV	131:4758606c9316	939	g->filter_mode = filter_mode;
TASS Belgium NV	131:4758606c9316	940	return 0;
tass	70:cd218dd180e5	941	}
tass	70:cd218dd180e5	942
tass	68:0847e35d08a6	943	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)
tass	68:0847e35d08a6	944	{
TASS Belgium NV	131:4758606c9316	945	struct pico_mcast_group *g = NULL, test = {
TASS Belgium NV	131:4758606c9316	946	0
TASS Belgium NV	131:4758606c9316	947	};
TASS Belgium NV	131:4758606c9316	948	struct pico_ipv4_link *link = NULL;
tass	68:0847e35d08a6	949
TASS Belgium NV	131:4758606c9316	950	if (mcast_link)
TASS Belgium NV	131:4758606c9316	951	link = pico_ipv4_link_get(mcast_link);
TASS Belgium NV	131:4758606c9316	952	else
TASS Belgium NV	131:4758606c9316	953	link = mcast_default_link;
TASS Belgium NV	131:4758606c9316	954
TASS Belgium NV	131:4758606c9316	955	test.mcast_addr = *mcast_group;
TASS Belgium NV	131:4758606c9316	956	g = pico_tree_findKey(link->MCASTGroups, &test);
TASS Belgium NV	131:4758606c9316	957	if (g) {
TASS Belgium NV	131:4758606c9316	958	if (reference_count)
TASS Belgium NV	131:4758606c9316	959	g->reference_count++;
tass	68:0847e35d08a6	960
TASS Belgium NV	131:4758606c9316	961	pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_UPDATE);
TASS Belgium NV	131:4758606c9316	962	} else {
tass picotcp@tass.be	149:5f4cb161cec3	963	g = PICO_ZALLOC(sizeof(struct pico_mcast_group));
TASS Belgium NV	131:4758606c9316	964	if (!g) {
TASS Belgium NV	131:4758606c9316	965	pico_err = PICO_ERR_ENOMEM;
TASS Belgium NV	131:4758606c9316	966	return -1;
TASS Belgium NV	131:4758606c9316	967	}
TASS Belgium NV	131:4758606c9316	968
TASS Belgium NV	131:4758606c9316	969	/* "non-existent" state of filter mode INCLUDE and empty source list */
TASS Belgium NV	131:4758606c9316	970	g->filter_mode = PICO_IP_MULTICAST_INCLUDE;
TASS Belgium NV	131:4758606c9316	971	g->reference_count = 1;
TASS Belgium NV	131:4758606c9316	972	g->mcast_addr = *mcast_group;
TASS Belgium NV	131:4758606c9316	973	g->MCASTSources.root = &LEAF;
TASS Belgium NV	131:4758606c9316	974	g->MCASTSources.compare = ipv4_mcast_sources_cmp;
TASS Belgium NV	131:4758606c9316	975	pico_tree_insert(link->MCASTGroups, g);
TASS Belgium NV	131:4758606c9316	976	pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_CREATE);
tass	68:0847e35d08a6	977	}
TASS Belgium NV	131:4758606c9316	978
tass picotcp@tass.be	149:5f4cb161cec3	979	if (mcast_group_update(g, MCASTFilter, filter_mode) < 0) {
tass picotcp@tass.be	149:5f4cb161cec3	980	dbg("Error in mcast_group update\n");
TASS Belgium NV	131:4758606c9316	981	return -1;
tass picotcp@tass.be	149:5f4cb161cec3	982	}
TASS Belgium NV	131:4758606c9316	983
TASS Belgium NV	131:4758606c9316	984	pico_ipv4_mcast_print_groups(link);
TASS Belgium NV	131:4758606c9316	985	return 0;
tass	68:0847e35d08a6	986	}
tass	68:0847e35d08a6	987
tass	68:0847e35d08a6	988	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)
tass	68:0847e35d08a6	989	{
tass	68:0847e35d08a6	990
TASS Belgium NV	131:4758606c9316	991	struct pico_mcast_group *g = NULL, test = {
TASS Belgium NV	131:4758606c9316	992	0
TASS Belgium NV	131:4758606c9316	993	};
TASS Belgium NV	131:4758606c9316	994	struct pico_ipv4_link *link = NULL;
TASS Belgium NV	131:4758606c9316	995	struct pico_tree_node index = NULL, _tmp = NULL;
TASS Belgium NV	131:4758606c9316	996	struct pico_ip4 *source = NULL;
tass	68:0847e35d08a6	997
TASS Belgium NV	131:4758606c9316	998	if (mcast_link)
TASS Belgium NV	131:4758606c9316	999	link = pico_ipv4_link_get(mcast_link);
TASS Belgium NV	131:4758606c9316	1000	else
TASS Belgium NV	131:4758606c9316	1001	link = mcast_default_link;
tass	68:0847e35d08a6	1002
TASS Belgium NV	131:4758606c9316	1003	test.mcast_addr = *mcast_group;
TASS Belgium NV	131:4758606c9316	1004	g = pico_tree_findKey(link->MCASTGroups, &test);
TASS Belgium NV	131:4758606c9316	1005	if (!g) {
TASS Belgium NV	131:4758606c9316	1006	pico_err = PICO_ERR_EINVAL;
TASS Belgium NV	131:4758606c9316	1007	return -1;
tass	68:0847e35d08a6	1008	} else {
TASS Belgium NV	131:4758606c9316	1009	if (reference_count && (--(g->reference_count) < 1)) {
TASS Belgium NV	131:4758606c9316	1010	pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_DELETE);
TASS Belgium NV	131:4758606c9316	1011	/* cleanup filter */
TASS Belgium NV	131:4758606c9316	1012	pico_tree_foreach_safe(index, &g->MCASTSources, _tmp)
TASS Belgium NV	131:4758606c9316	1013	{
TASS Belgium NV	131:4758606c9316	1014	source = index->keyValue;
TASS Belgium NV	131:4758606c9316	1015	pico_tree_delete(&g->MCASTSources, source);
tass picotcp@tass.be	149:5f4cb161cec3	1016	PICO_FREE(source);
TASS Belgium NV	131:4758606c9316	1017	}
TASS Belgium NV	131:4758606c9316	1018	pico_tree_delete(link->MCASTGroups, g);
tass picotcp@tass.be	149:5f4cb161cec3	1019	PICO_FREE(g);
TASS Belgium NV	131:4758606c9316	1020	} else {
TASS Belgium NV	131:4758606c9316	1021	pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_UPDATE);
TASS Belgium NV	131:4758606c9316	1022	if (mcast_group_update(g, MCASTFilter, filter_mode) < 0)
TASS Belgium NV	131:4758606c9316	1023	return -1;
TASS Belgium NV	131:4758606c9316	1024	}
tass	68:0847e35d08a6	1025	}
tass	68:0847e35d08a6	1026
TASS Belgium NV	131:4758606c9316	1027	pico_ipv4_mcast_print_groups(link);
TASS Belgium NV	131:4758606c9316	1028	return 0;
tass	68:0847e35d08a6	1029	}
tass	68:0847e35d08a6	1030
tass	68:0847e35d08a6	1031	struct pico_ipv4_link *pico_ipv4_get_default_mcastlink(void)
tass	68:0847e35d08a6	1032	{
TASS Belgium NV	131:4758606c9316	1033	return mcast_default_link;
tass	68:0847e35d08a6	1034	}
tass	68:0847e35d08a6	1035
tass	68:0847e35d08a6	1036	static int pico_ipv4_mcast_filter(struct pico_frame *f)
tass	68:0847e35d08a6	1037	{
TASS Belgium NV	131:4758606c9316	1038	struct pico_ipv4_link *link = NULL;
TASS Belgium NV	131:4758606c9316	1039	struct pico_tree_node index = NULL, index2 = NULL;
TASS Belgium NV	131:4758606c9316	1040	struct pico_mcast_group *g = NULL, test = {
TASS Belgium NV	131:4758606c9316	1041	0
TASS Belgium NV	131:4758606c9316	1042	};
TASS Belgium NV	131:4758606c9316	1043	struct pico_ipv4_hdr hdr = (struct pico_ipv4_hdr ) f->net_hdr;
tass	68:0847e35d08a6	1044
TASS Belgium NV	131:4758606c9316	1045	test.mcast_addr = hdr->dst;
tass	68:0847e35d08a6	1046
TASS Belgium NV	131:4758606c9316	1047	pico_tree_foreach(index, &Tree_dev_link)
TASS Belgium NV	131:4758606c9316	1048	{
TASS Belgium NV	131:4758606c9316	1049	link = index->keyValue;
TASS Belgium NV	131:4758606c9316	1050	g = pico_tree_findKey(link->MCASTGroups, &test);
TASS Belgium NV	131:4758606c9316	1051	if (g) {
TASS Belgium NV	131:4758606c9316	1052	if (f->dev == link->dev) {
TASS Belgium NV	131:4758606c9316	1053	ip_mcast_dbg("MCAST: IP %08X is group member of current link %s\n", hdr->dst.addr, f->dev->name);
TASS Belgium NV	131:4758606c9316	1054	/* perform source filtering */
TASS Belgium NV	131:4758606c9316	1055	switch (g->filter_mode)
TASS Belgium NV	131:4758606c9316	1056	{
TASS Belgium NV	131:4758606c9316	1057	case PICO_IP_MULTICAST_INCLUDE:
TASS Belgium NV	131:4758606c9316	1058	pico_tree_foreach(index2, &g->MCASTSources)
TASS Belgium NV	131:4758606c9316	1059	{
TASS Belgium NV	131:4758606c9316	1060	if (hdr->src.addr == ((struct pico_ip4 *)index2->keyValue)->addr) {
TASS Belgium NV	131:4758606c9316	1061	ip_mcast_dbg("MCAST: IP %08X in included interface source list\n", hdr->src.addr);
TASS Belgium NV	131:4758606c9316	1062	return 0;
TASS Belgium NV	131:4758606c9316	1063	}
TASS Belgium NV	131:4758606c9316	1064	}
TASS Belgium NV	131:4758606c9316	1065	ip_mcast_dbg("MCAST: IP %08X NOT in included interface source list\n", hdr->src.addr);
TASS Belgium NV	131:4758606c9316	1066	return -1;
TASS Belgium NV	131:4758606c9316	1067
TASS Belgium NV	131:4758606c9316	1068	case PICO_IP_MULTICAST_EXCLUDE:
TASS Belgium NV	131:4758606c9316	1069	pico_tree_foreach(index2, &g->MCASTSources)
TASS Belgium NV	131:4758606c9316	1070	{
TASS Belgium NV	131:4758606c9316	1071	if (hdr->src.addr == ((struct pico_ip4 *)index2->keyValue)->addr) {
TASS Belgium NV	131:4758606c9316	1072	ip_mcast_dbg("MCAST: IP %08X in excluded interface source list\n", hdr->src.addr);
TASS Belgium NV	131:4758606c9316	1073	return -1;
TASS Belgium NV	131:4758606c9316	1074	}
TASS Belgium NV	131:4758606c9316	1075	}
TASS Belgium NV	131:4758606c9316	1076	ip_mcast_dbg("MCAST: IP %08X NOT in excluded interface source list\n", hdr->src.addr);
TASS Belgium NV	131:4758606c9316	1077	return 0;
TASS Belgium NV	131:4758606c9316	1078
TASS Belgium NV	131:4758606c9316	1079	default:
TASS Belgium NV	131:4758606c9316	1080	return -1;
TASS Belgium NV	131:4758606c9316	1081	}
TASS Belgium NV	131:4758606c9316	1082	} else {
TASS Belgium NV	131:4758606c9316	1083	ip_mcast_dbg("MCAST: IP %08X is group member of different link %s\n", hdr->dst.addr, link->dev->name);
tass	68:0847e35d08a6	1084	}
TASS Belgium NV	131:4758606c9316	1085	} else {
TASS Belgium NV	131:4758606c9316	1086	ip_mcast_dbg("MCAST: IP %08X is not a group member of link %s\n", hdr->dst.addr, f->dev->name);
tass	68:0847e35d08a6	1087	}
tass	68:0847e35d08a6	1088	}
TASS Belgium NV	131:4758606c9316	1089	return -1;
tass	68:0847e35d08a6	1090	}
tass	68:0847e35d08a6	1091
TASS Belgium NV	131:4758606c9316	1092	#else
tass	68:0847e35d08a6	1093
tass	68:0847e35d08a6	1094	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)
tass	68:0847e35d08a6	1095	{
TASS Belgium NV	131:4758606c9316	1096	pico_err = PICO_ERR_EPROTONOSUPPORT;
TASS Belgium NV	131:4758606c9316	1097	return -1;
tass	68:0847e35d08a6	1098	}
tass	68:0847e35d08a6	1099	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)
tass	68:0847e35d08a6	1100	{
TASS Belgium NV	131:4758606c9316	1101	pico_err = PICO_ERR_EPROTONOSUPPORT;
TASS Belgium NV	131:4758606c9316	1102	return -1;
tass	68:0847e35d08a6	1103	}
tass	68:0847e35d08a6	1104	struct pico_ipv4_link *pico_ipv4_get_default_mcastlink(void)
tass	68:0847e35d08a6	1105	{
TASS Belgium NV	131:4758606c9316	1106	pico_err = PICO_ERR_EPROTONOSUPPORT;
TASS Belgium NV	131:4758606c9316	1107	return NULL;
tass	68:0847e35d08a6	1108	}
tass	68:0847e35d08a6	1109	#endif /* PICO_SUPPORT_MCAST */
tass	68:0847e35d08a6	1110
tass	88:0e827d0d8017	1111	#ifdef DEBUG_ROUTE
tass	88:0e827d0d8017	1112	static void dbg_route(void)
tass	88:0e827d0d8017	1113	{
TASS Belgium NV	131:4758606c9316	1114	struct pico_ipv4_route *r;
TASS Belgium NV	131:4758606c9316	1115	struct pico_tree_node *index;
TASS Belgium NV	131:4758606c9316	1116	pico_tree_foreach(index, &Routes){
TASS Belgium NV	131:4758606c9316	1117	r = index->keyValue;
TASS Belgium NV	131:4758606c9316	1118	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);
TASS Belgium NV	131:4758606c9316	1119	}
tass	88:0e827d0d8017	1120	}
tass	88:0e827d0d8017	1121	#else
TASS Belgium NV	131:4758606c9316	1122	#define dbg_route() do { } while(0)
tass	88:0e827d0d8017	1123	#endif
tass	88:0e827d0d8017	1124
tass	68:0847e35d08a6	1125	int pico_ipv4_frame_push(struct pico_frame f, struct pico_ip4 dst, uint8_t proto)
tass	68:0847e35d08a6	1126	{
tass	68:0847e35d08a6	1127
TASS Belgium NV	131:4758606c9316	1128	struct pico_ipv4_route *route;
TASS Belgium NV	131:4758606c9316	1129	struct pico_ipv4_link *link;
TASS Belgium NV	131:4758606c9316	1130	struct pico_ipv4_hdr *hdr;
TASS Belgium NV	131:4758606c9316	1131	uint8_t ttl = PICO_IPV4_DEFAULT_TTL;
TASS Belgium NV	131:4758606c9316	1132	uint8_t vhl = 0x45; /* version 4, header length 20 */
TASS Belgium NV	131:4758606c9316	1133	static uint16_t ipv4_progressive_id = 0x91c0;
tass	68:0847e35d08a6	1134	#ifdef PICO_SUPPORT_MCAST
TASS Belgium NV	131:4758606c9316	1135	struct pico_tree_node *index;
tass	68:0847e35d08a6	1136	#endif
tass	68:0847e35d08a6	1137
TASS Belgium NV	131:4758606c9316	1138	if(!f \|\| !dst) {
TASS Belgium NV	131:4758606c9316	1139	pico_err = PICO_ERR_EINVAL;
TASS Belgium NV	131:4758606c9316	1140	return -1;
TASS Belgium NV	131:4758606c9316	1141	}
TASS Belgium NV	131:4758606c9316	1142
TASS Belgium NV	131:4758606c9316	1143	hdr = (struct pico_ipv4_hdr *) f->net_hdr;
TASS Belgium NV	131:4758606c9316	1144	if (!hdr) {
TASS Belgium NV	131:4758606c9316	1145	dbg("IP header error\n");
TASS Belgium NV	131:4758606c9316	1146	pico_err = PICO_ERR_EINVAL;
TASS Belgium NV	131:4758606c9316	1147	goto drop;
TASS Belgium NV	131:4758606c9316	1148	}
tass	68:0847e35d08a6	1149
TASS Belgium NV	131:4758606c9316	1150	if (dst->addr == 0) {
TASS Belgium NV	131:4758606c9316	1151	dbg("IP destination addr error\n");
TASS Belgium NV	131:4758606c9316	1152	pico_err = PICO_ERR_EINVAL;
TASS Belgium NV	131:4758606c9316	1153	goto drop;
TASS Belgium NV	131:4758606c9316	1154	}
tass	68:0847e35d08a6	1155
TASS Belgium NV	131:4758606c9316	1156	route = route_find(dst);
TASS Belgium NV	131:4758606c9316	1157	if (!route) {
TASS Belgium NV	131:4758606c9316	1158	/* dbg("Route to %08x not found.\n", long_be(dst->addr)); */
TASS Belgium NV	131:4758606c9316	1159
TASS Belgium NV	131:4758606c9316	1160
TASS Belgium NV	131:4758606c9316	1161	pico_err = PICO_ERR_EHOSTUNREACH;
TASS Belgium NV	131:4758606c9316	1162	goto drop;
TASS Belgium NV	131:4758606c9316	1163	} else {
TASS Belgium NV	131:4758606c9316	1164	link = route->link;
tass	68:0847e35d08a6	1165	#ifdef PICO_SUPPORT_MCAST
TASS Belgium NV	131:4758606c9316	1166	if (pico_ipv4_is_multicast(dst->addr)) { /* if multicast */
TASS Belgium NV	131:4758606c9316	1167	switch (proto) {
TASS Belgium NV	131:4758606c9316	1168	case PICO_PROTO_UDP:
TASS Belgium NV	131:4758606c9316	1169	if(pico_udp_get_mc_ttl(f->sock, &ttl) < 0)
TASS Belgium NV	131:4758606c9316	1170	ttl = PICO_IP_DEFAULT_MULTICAST_TTL;
TASS Belgium NV	131:4758606c9316	1171
tass	68:0847e35d08a6	1172	break;
TASS Belgium NV	131:4758606c9316	1173	case PICO_PROTO_IGMP:
TASS Belgium NV	131:4758606c9316	1174	vhl = 0x46; /* header length 24 */
TASS Belgium NV	131:4758606c9316	1175	ttl = 1;
TASS Belgium NV	131:4758606c9316	1176	/* router alert (RFC 2113) */
TASS Belgium NV	131:4758606c9316	1177	hdr->options[0] = 0x94;
TASS Belgium NV	131:4758606c9316	1178	hdr->options[1] = 0x04;
TASS Belgium NV	131:4758606c9316	1179	hdr->options[2] = 0x00;
TASS Belgium NV	131:4758606c9316	1180	hdr->options[3] = 0x00;
TASS Belgium NV	131:4758606c9316	1181	if (f->dev && link->dev != f->dev) { /* default link is not requested link */
TASS Belgium NV	131:4758606c9316	1182	pico_tree_foreach(index, &Tree_dev_link) {
TASS Belgium NV	131:4758606c9316	1183	link = index->keyValue;
TASS Belgium NV	131:4758606c9316	1184	if (link->dev == f->dev)
TASS Belgium NV	131:4758606c9316	1185	break;
TASS Belgium NV	131:4758606c9316	1186	}
TASS Belgium NV	131:4758606c9316	1187	}
TASS Belgium NV	131:4758606c9316	1188
TASS Belgium NV	131:4758606c9316	1189	break;
TASS Belgium NV	131:4758606c9316	1190	default:
TASS Belgium NV	131:4758606c9316	1191	ttl = PICO_IPV4_DEFAULT_TTL;
tass	68:0847e35d08a6	1192	}
TASS Belgium NV	131:4758606c9316	1193	}
TASS Belgium NV	131:4758606c9316	1194
TASS Belgium NV	131:4758606c9316	1195	#endif
tass	68:0847e35d08a6	1196	}
tass	68:0847e35d08a6	1197
TASS Belgium NV	131:4758606c9316	1198	hdr->vhl = vhl;
TASS Belgium NV	131:4758606c9316	1199	hdr->len = short_be((uint16_t)(f->transport_len + f->net_len));
tass picotcp@tass.be	149:5f4cb161cec3	1200	if ((f->transport_hdr != f->payload) &&
tass picotcp@tass.be	149:5f4cb161cec3	1201	#ifdef PICO_SUPPORT_IPFRAG
tass picotcp@tass.be	149:5f4cb161cec3	1202	(0 == (f->frag & PICO_IPV4_MOREFRAG)) &&
tass picotcp@tass.be	149:5f4cb161cec3	1203	#endif
tass picotcp@tass.be	149:5f4cb161cec3	1204	1 )
TASS Belgium NV	131:4758606c9316	1205	ipv4_progressive_id++;
TASS Belgium NV	131:4758606c9316	1206
TASS Belgium NV	131:4758606c9316	1207	hdr->id = short_be(ipv4_progressive_id);
TASS Belgium NV	131:4758606c9316	1208	hdr->dst.addr = dst->addr;
TASS Belgium NV	131:4758606c9316	1209	hdr->src.addr = link->address.addr;
TASS Belgium NV	131:4758606c9316	1210	hdr->ttl = ttl;
TASS Belgium NV	131:4758606c9316	1211	hdr->proto = proto;
TASS Belgium NV	131:4758606c9316	1212	hdr->frag = short_be(PICO_IPV4_DONTFRAG);
tass	68:0847e35d08a6	1213	#ifdef PICO_SUPPORT_IPFRAG
tass	68:0847e35d08a6	1214	# ifdef PICO_SUPPORT_UDP
TASS Belgium NV	131:4758606c9316	1215	if (proto == PICO_PROTO_UDP) {
TASS Belgium NV	131:4758606c9316	1216	/* first fragment, can not use transport_len to calculate IP length */
TASS Belgium NV	131:4758606c9316	1217	if (f->transport_hdr != f->payload)
TASS Belgium NV	131:4758606c9316	1218	hdr->len = short_be((uint16_t)(f->payload_len + sizeof(struct pico_udp_hdr) + f->net_len));
TASS Belgium NV	131:4758606c9316	1219
TASS Belgium NV	131:4758606c9316	1220	/* set fragmentation flags and offset calculated in socket layer */
TASS Belgium NV	131:4758606c9316	1221	hdr->frag = f->frag;
TASS Belgium NV	131:4758606c9316	1222	}
TASS Belgium NV	131:4758606c9316	1223
tass picotcp@tass.be	149:5f4cb161cec3	1224	if (proto == PICO_PROTO_ICMP4)
tass picotcp@tass.be	149:5f4cb161cec3	1225	hdr->frag = f->frag;
tass picotcp@tass.be	149:5f4cb161cec3	1226
tass picotcp@tass.be	149:5f4cb161cec3	1227	# endif
tass	68:0847e35d08a6	1228	#endif /* PICO_SUPPORT_IPFRAG */
TASS Belgium NV	131:4758606c9316	1229	pico_ipv4_checksum(f);
tass	68:0847e35d08a6	1230
TASS Belgium NV	131:4758606c9316	1231	if (f->sock && f->sock->dev) {
TASS Belgium NV	131:4758606c9316	1232	/* if the socket has its device set, use that (currently used for DHCP) */
TASS Belgium NV	131:4758606c9316	1233	f->dev = f->sock->dev;
TASS Belgium NV	131:4758606c9316	1234	} else {
TASS Belgium NV	131:4758606c9316	1235	f->dev = link->dev;
TASS Belgium NV	131:4758606c9316	1236	}
tass	68:0847e35d08a6	1237
tass	68:0847e35d08a6	1238	#ifdef PICO_SUPPORT_MCAST
TASS Belgium NV	131:4758606c9316	1239	if (pico_ipv4_is_multicast(hdr->dst.addr)) {
TASS Belgium NV	131:4758606c9316	1240	struct pico_frame *cpy;
TASS Belgium NV	131:4758606c9316	1241	/* Sending UDP multicast datagram, am I member? If so, loopback copy */
TASS Belgium NV	131:4758606c9316	1242	if ((proto != PICO_PROTO_IGMP) && (pico_ipv4_mcast_filter(f) == 0)) {
TASS Belgium NV	131:4758606c9316	1243	ip_mcast_dbg("MCAST: sender is member of group, loopback copy\n");
TASS Belgium NV	131:4758606c9316	1244	cpy = pico_frame_copy(f);
TASS Belgium NV	131:4758606c9316	1245	pico_enqueue(&in, cpy);
TASS Belgium NV	131:4758606c9316	1246	}
tass	68:0847e35d08a6	1247	}
TASS Belgium NV	131:4758606c9316	1248
tass	68:0847e35d08a6	1249	#endif
tass	68:0847e35d08a6	1250
TASS Belgium NV	131:4758606c9316	1251	if(pico_ipv4_link_get(&hdr->dst)) {
TASS Belgium NV	131:4758606c9316	1252	/* it's our own IP */
TASS Belgium NV	131:4758606c9316	1253	return pico_enqueue(&in, f);
TASS Belgium NV	131:4758606c9316	1254	}else{
TASS Belgium NV	131:4758606c9316	1255	/* TODO: Check if there are members subscribed here */
TASS Belgium NV	131:4758606c9316	1256	return pico_enqueue(&out, f);
TASS Belgium NV	131:4758606c9316	1257	}
tass	68:0847e35d08a6	1258
tass	68:0847e35d08a6	1259	drop:
TASS Belgium NV	131:4758606c9316	1260	pico_frame_discard(f);
TASS Belgium NV	131:4758606c9316	1261	return -1;
tass	68:0847e35d08a6	1262	}
tass	68:0847e35d08a6	1263
tass	68:0847e35d08a6	1264
tass	68:0847e35d08a6	1265	static int pico_ipv4_frame_sock_push(struct pico_protocol self, struct pico_frame f)
tass	68:0847e35d08a6	1266	{
TASS Belgium NV	131:4758606c9316	1267	struct pico_ip4 *dst;
tass picotcp@tass.be	149:5f4cb161cec3	1268	struct pico_remote_endpoint remote_endpoint = (struct pico_remote_endpoint ) f->info;
TASS Belgium NV	131:4758606c9316	1269	IGNORE_PARAMETER(self);
tass	68:0847e35d08a6	1270
TASS Belgium NV	131:4758606c9316	1271	if (!f->sock) {
TASS Belgium NV	131:4758606c9316	1272	pico_frame_discard(f);
TASS Belgium NV	131:4758606c9316	1273	return -1;
TASS Belgium NV	131:4758606c9316	1274	}
tass	68:0847e35d08a6	1275
tass picotcp@tass.be	149:5f4cb161cec3	1276	if (remote_endpoint) {
tass picotcp@tass.be	149:5f4cb161cec3	1277	dst = &remote_endpoint->remote_addr.ip4;
TASS Belgium NV	131:4758606c9316	1278	} else {
TASS Belgium NV	131:4758606c9316	1279	dst = &f->sock->remote_addr.ip4;
TASS Belgium NV	131:4758606c9316	1280	}
tass	68:0847e35d08a6	1281
TASS Belgium NV	131:4758606c9316	1282	return pico_ipv4_frame_push(f, dst, (uint8_t)f->sock->proto->proto_number);
tass	68:0847e35d08a6	1283	}
tass	68:0847e35d08a6	1284
tass	68:0847e35d08a6	1285
tass	68:0847e35d08a6	1286	int pico_ipv4_route_add(struct pico_ip4 address, struct pico_ip4 netmask, struct pico_ip4 gateway, int metric, struct pico_ipv4_link *link)
tass	68:0847e35d08a6	1287	{
TASS Belgium NV	131:4758606c9316	1288	struct pico_ipv4_route test, *new;
TASS Belgium NV	131:4758606c9316	1289	test.dest.addr = address.addr;
TASS Belgium NV	131:4758606c9316	1290	test.netmask.addr = netmask.addr;
TASS Belgium NV	131:4758606c9316	1291	test.metric = (uint32_t)metric;
TASS Belgium NV	131:4758606c9316	1292
TASS Belgium NV	131:4758606c9316	1293	if(pico_tree_findKey(&Routes, &test)) {
TASS Belgium NV	131:4758606c9316	1294	pico_err = PICO_ERR_EINVAL;
TASS Belgium NV	131:4758606c9316	1295	return -1;
TASS Belgium NV	131:4758606c9316	1296	}
TASS Belgium NV	131:4758606c9316	1297
tass picotcp@tass.be	149:5f4cb161cec3	1298	new = PICO_ZALLOC(sizeof(struct pico_ipv4_route));
TASS Belgium NV	131:4758606c9316	1299	if (!new) {
TASS Belgium NV	131:4758606c9316	1300	pico_err = PICO_ERR_ENOMEM;
TASS Belgium NV	131:4758606c9316	1301	return -1;
TASS Belgium NV	131:4758606c9316	1302	}
tass	68:0847e35d08a6	1303
TASS Belgium NV	131:4758606c9316	1304	new->dest.addr = address.addr;
TASS Belgium NV	131:4758606c9316	1305	new->netmask.addr = netmask.addr;
TASS Belgium NV	131:4758606c9316	1306	new->gateway.addr = gateway.addr;
TASS Belgium NV	131:4758606c9316	1307	new->metric = (uint32_t)metric;
TASS Belgium NV	131:4758606c9316	1308	if (gateway.addr == 0) {
TASS Belgium NV	131:4758606c9316	1309	/* No gateway provided, use the link */
TASS Belgium NV	131:4758606c9316	1310	new->link = link;
TASS Belgium NV	131:4758606c9316	1311	} else {
TASS Belgium NV	131:4758606c9316	1312	struct pico_ipv4_route *r = route_find(&gateway);
TASS Belgium NV	131:4758606c9316	1313	if (!r ) { /* Specified Gateway is unreachable */
TASS Belgium NV	131:4758606c9316	1314	pico_err = PICO_ERR_EHOSTUNREACH;
tass picotcp@tass.be	149:5f4cb161cec3	1315	PICO_FREE(new);
TASS Belgium NV	131:4758606c9316	1316	return -1;
TASS Belgium NV	131:4758606c9316	1317	}
TASS Belgium NV	131:4758606c9316	1318
TASS Belgium NV	131:4758606c9316	1319	if (r->gateway.addr) { /* Specified Gateway is not a neighbor */
TASS Belgium NV	131:4758606c9316	1320	pico_err = PICO_ERR_ENETUNREACH;
tass picotcp@tass.be	149:5f4cb161cec3	1321	PICO_FREE(new);
TASS Belgium NV	131:4758606c9316	1322	return -1;
TASS Belgium NV	131:4758606c9316	1323	}
TASS Belgium NV	131:4758606c9316	1324
TASS Belgium NV	131:4758606c9316	1325	new->link = r->link;
tass	68:0847e35d08a6	1326	}
TASS Belgium NV	131:4758606c9316	1327
TASS Belgium NV	131:4758606c9316	1328	if (!new->link) {
TASS Belgium NV	131:4758606c9316	1329	pico_err = PICO_ERR_EINVAL;
tass picotcp@tass.be	149:5f4cb161cec3	1330	PICO_FREE(new);
TASS Belgium NV	131:4758606c9316	1331	return -1;
tass	68:0847e35d08a6	1332	}
tass	68:0847e35d08a6	1333
TASS Belgium NV	131:4758606c9316	1334	pico_tree_insert(&Routes, new);
TASS Belgium NV	131:4758606c9316	1335	/* dbg_route(); */
TASS Belgium NV	131:4758606c9316	1336	return 0;
tass	68:0847e35d08a6	1337	}
tass	68:0847e35d08a6	1338
tass	72:887bc44746ff	1339	int pico_ipv4_route_del(struct pico_ip4 address, struct pico_ip4 netmask, int metric)
tass	68:0847e35d08a6	1340	{
TASS Belgium NV	131:4758606c9316	1341	struct pico_ipv4_route test, *found;
tass	68:0847e35d08a6	1342
TASS Belgium NV	131:4758606c9316	1343	test.dest.addr = address.addr;
TASS Belgium NV	131:4758606c9316	1344	test.netmask.addr = netmask.addr;
TASS Belgium NV	131:4758606c9316	1345	test.metric = (uint32_t)metric;
tass	68:0847e35d08a6	1346
TASS Belgium NV	131:4758606c9316	1347	found = pico_tree_findKey(&Routes, &test);
TASS Belgium NV	131:4758606c9316	1348	if (found) {
tass	68:0847e35d08a6	1349
TASS Belgium NV	131:4758606c9316	1350	pico_tree_delete(&Routes, found);
tass picotcp@tass.be	149:5f4cb161cec3	1351	PICO_FREE(found);
tass	68:0847e35d08a6	1352
TASS Belgium NV	131:4758606c9316	1353	/* dbg_route(); */
TASS Belgium NV	131:4758606c9316	1354	return 0;
TASS Belgium NV	131:4758606c9316	1355	}
TASS Belgium NV	131:4758606c9316	1356
TASS Belgium NV	131:4758606c9316	1357	pico_err = PICO_ERR_EINVAL;
TASS Belgium NV	131:4758606c9316	1358	return -1;
tass	68:0847e35d08a6	1359	}
tass	68:0847e35d08a6	1360
tass	68:0847e35d08a6	1361
tass	68:0847e35d08a6	1362	int pico_ipv4_link_add(struct pico_device *dev, struct pico_ip4 address, struct pico_ip4 netmask)
tass	68:0847e35d08a6	1363	{
TASS Belgium NV	131:4758606c9316	1364	struct pico_ipv4_link test, *new;
TASS Belgium NV	131:4758606c9316	1365	struct pico_ip4 network, gateway;
TASS Belgium NV	131:4758606c9316	1366	char ipstr[30];
TASS Belgium NV	131:4758606c9316	1367
TASS Belgium NV	131:4758606c9316	1368	if(!dev) {
TASS Belgium NV	131:4758606c9316	1369	pico_err = PICO_ERR_EINVAL;
TASS Belgium NV	131:4758606c9316	1370	return -1;
TASS Belgium NV	131:4758606c9316	1371	}
tass	68:0847e35d08a6	1372
TASS Belgium NV	131:4758606c9316	1373	test.address.addr = address.addr;
TASS Belgium NV	131:4758606c9316	1374	test.netmask.addr = netmask.addr;
TASS Belgium NV	131:4758606c9316	1375	test.dev = dev;
TASS Belgium NV	131:4758606c9316	1376	/ XXX: Valid netmask / unicast address test /
tass	68:0847e35d08a6	1377
TASS Belgium NV	131:4758606c9316	1378	if(pico_tree_findKey(&Tree_dev_link, &test)) {
TASS Belgium NV	131:4758606c9316	1379	dbg("IPv4: Trying to assign an invalid address (in use)\n");
TASS Belgium NV	131:4758606c9316	1380	pico_err = PICO_ERR_EADDRINUSE;
TASS Belgium NV	131:4758606c9316	1381	return -1;
TASS Belgium NV	131:4758606c9316	1382	}
tass	68:0847e35d08a6	1383
TASS Belgium NV	131:4758606c9316	1384	/ 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) /
tass picotcp@tass.be	149:5f4cb161cec3	1385	new = PICO_ZALLOC(sizeof(struct pico_ipv4_link));
TASS Belgium NV	131:4758606c9316	1386	if (!new) {
TASS Belgium NV	131:4758606c9316	1387	dbg("IPv4: Out of memory!\n");
TASS Belgium NV	131:4758606c9316	1388	pico_err = PICO_ERR_ENOMEM;
TASS Belgium NV	131:4758606c9316	1389	return -1;
TASS Belgium NV	131:4758606c9316	1390	}
TASS Belgium NV	131:4758606c9316	1391
TASS Belgium NV	131:4758606c9316	1392	new->address.addr = address.addr;
TASS Belgium NV	131:4758606c9316	1393	new->netmask.addr = netmask.addr;
TASS Belgium NV	131:4758606c9316	1394	new->dev = dev;
tass	68:0847e35d08a6	1395	#ifdef PICO_SUPPORT_MCAST
tass picotcp@tass.be	149:5f4cb161cec3	1396	new->MCASTGroups = PICO_ZALLOC(sizeof(struct pico_tree));
TASS Belgium NV	131:4758606c9316	1397	if (!new->MCASTGroups) {
tass picotcp@tass.be	149:5f4cb161cec3	1398	PICO_FREE(new);
TASS Belgium NV	131:4758606c9316	1399	dbg("IPv4: Out of memory!\n");
TASS Belgium NV	131:4758606c9316	1400	pico_err = PICO_ERR_ENOMEM;
TASS Belgium NV	131:4758606c9316	1401	return -1;
TASS Belgium NV	131:4758606c9316	1402	}
tass	68:0847e35d08a6	1403
TASS Belgium NV	131:4758606c9316	1404	new->MCASTGroups->root = &LEAF;
TASS Belgium NV	131:4758606c9316	1405	new->MCASTGroups->compare = ipv4_mcast_groups_cmp;
TASS Belgium NV	131:4758606c9316	1406	new->mcast_compatibility = PICO_IGMPV3; /* default RFC 3376 $7.2.1 */
TASS Belgium NV	131:4758606c9316	1407	new->mcast_last_query_interval = PICO_IGMP_QUERY_INTERVAL;
tass	68:0847e35d08a6	1408	#endif
tass	68:0847e35d08a6	1409
TASS Belgium NV	131:4758606c9316	1410	pico_tree_insert(&Tree_dev_link, new);
tass	68:0847e35d08a6	1411	#ifdef PICO_SUPPORT_MCAST
TASS Belgium NV	131:4758606c9316	1412	do {
TASS Belgium NV	131:4758606c9316	1413	struct pico_ip4 mcast_all_hosts, mcast_addr, mcast_nm, mcast_gw;
TASS Belgium NV	131:4758606c9316	1414	if (!mcast_default_link) {
TASS Belgium NV	131:4758606c9316	1415	mcast_addr.addr = long_be(0xE0000000); /* 224.0.0.0 */
TASS Belgium NV	131:4758606c9316	1416	mcast_nm.addr = long_be(0xF0000000); /* 15.0.0.0 */
TASS Belgium NV	131:4758606c9316	1417	mcast_gw.addr = long_be(0x00000000);
TASS Belgium NV	131:4758606c9316	1418	mcast_default_link = new;
TASS Belgium NV	131:4758606c9316	1419	pico_ipv4_route_add(mcast_addr, mcast_nm, mcast_gw, 1, new);
TASS Belgium NV	131:4758606c9316	1420	}
TASS Belgium NV	131:4758606c9316	1421
TASS Belgium NV	131:4758606c9316	1422	mcast_all_hosts.addr = PICO_MCAST_ALL_HOSTS;
TASS Belgium NV	131:4758606c9316	1423	pico_ipv4_mcast_join(&address, &mcast_all_hosts, 1, PICO_IP_MULTICAST_EXCLUDE, NULL);
TASS Belgium NV	131:4758606c9316	1424	} while(0);
tass	68:0847e35d08a6	1425	#endif
tass	68:0847e35d08a6	1426
TASS Belgium NV	131:4758606c9316	1427	network.addr = address.addr & netmask.addr;
TASS Belgium NV	131:4758606c9316	1428	gateway.addr = 0U;
TASS Belgium NV	131:4758606c9316	1429	pico_ipv4_route_add(network, netmask, gateway, 1, new);
TASS Belgium NV	131:4758606c9316	1430	pico_ipv4_to_string(ipstr, new->address.addr);
TASS Belgium NV	131:4758606c9316	1431	dbg("Assigned ipv4 %s to device %s\n", ipstr, new->dev->name);
TASS Belgium NV	131:4758606c9316	1432	return 0;
tass	68:0847e35d08a6	1433	}
tass	68:0847e35d08a6	1434
tass	72:887bc44746ff	1435	static int pico_ipv4_cleanup_routes(struct pico_ipv4_link *link)
tass	72:887bc44746ff	1436	{
TASS Belgium NV	131:4758606c9316	1437	struct pico_tree_node index = NULL, tmp = NULL;
TASS Belgium NV	131:4758606c9316	1438	struct pico_ipv4_route *route = NULL;
tass	72:887bc44746ff	1439
TASS Belgium NV	131:4758606c9316	1440	pico_tree_foreach_safe(index, &Routes, tmp)
TASS Belgium NV	131:4758606c9316	1441	{
TASS Belgium NV	131:4758606c9316	1442	route = index->keyValue;
TASS Belgium NV	131:4758606c9316	1443	if (link == route->link)
TASS Belgium NV	131:4758606c9316	1444	pico_ipv4_route_del(route->dest, route->netmask, (int)route->metric);
TASS Belgium NV	131:4758606c9316	1445	}
TASS Belgium NV	131:4758606c9316	1446	return 0;
tass	72:887bc44746ff	1447	}
tass	68:0847e35d08a6	1448
tass	68:0847e35d08a6	1449	int pico_ipv4_link_del(struct pico_device *dev, struct pico_ip4 address)
tass	68:0847e35d08a6	1450	{
TASS Belgium NV	131:4758606c9316	1451	struct pico_ipv4_link test, *found;
TASS Belgium NV	131:4758606c9316	1452
TASS Belgium NV	131:4758606c9316	1453	if(!dev) {
TASS Belgium NV	131:4758606c9316	1454	pico_err = PICO_ERR_EINVAL;
TASS Belgium NV	131:4758606c9316	1455	return -1;
TASS Belgium NV	131:4758606c9316	1456	}
tass	68:0847e35d08a6	1457
TASS Belgium NV	131:4758606c9316	1458	test.address.addr = address.addr;
TASS Belgium NV	131:4758606c9316	1459	test.dev = dev;
TASS Belgium NV	131:4758606c9316	1460	found = pico_tree_findKey(&Tree_dev_link, &test);
TASS Belgium NV	131:4758606c9316	1461	if (!found) {
TASS Belgium NV	131:4758606c9316	1462	pico_err = PICO_ERR_ENXIO;
TASS Belgium NV	131:4758606c9316	1463	return -1;
TASS Belgium NV	131:4758606c9316	1464	}
tass	68:0847e35d08a6	1465
tass	68:0847e35d08a6	1466	#ifdef PICO_SUPPORT_MCAST
TASS Belgium NV	131:4758606c9316	1467	do {
TASS Belgium NV	131:4758606c9316	1468	struct pico_ip4 mcast_all_hosts, mcast_addr, mcast_nm;
TASS Belgium NV	131:4758606c9316	1469	struct pico_mcast_group *g = NULL;
TASS Belgium NV	131:4758606c9316	1470	struct pico_tree_node index, _tmp;
TASS Belgium NV	131:4758606c9316	1471	if (found == mcast_default_link) {
TASS Belgium NV	131:4758606c9316	1472	mcast_addr.addr = long_be(0xE0000000); /* 224.0.0.0 */
TASS Belgium NV	131:4758606c9316	1473	mcast_nm.addr = long_be(0xF0000000); /* 15.0.0.0 */
TASS Belgium NV	131:4758606c9316	1474	mcast_default_link = NULL;
TASS Belgium NV	131:4758606c9316	1475	pico_ipv4_route_del(mcast_addr, mcast_nm, 1);
TASS Belgium NV	131:4758606c9316	1476	}
TASS Belgium NV	131:4758606c9316	1477
TASS Belgium NV	131:4758606c9316	1478	mcast_all_hosts.addr = PICO_MCAST_ALL_HOSTS;
TASS Belgium NV	131:4758606c9316	1479	pico_ipv4_mcast_leave(&address, &mcast_all_hosts, 1, PICO_IP_MULTICAST_EXCLUDE, NULL);
TASS Belgium NV	131:4758606c9316	1480	pico_tree_foreach_safe(index, found->MCASTGroups, _tmp)
TASS Belgium NV	131:4758606c9316	1481	{
TASS Belgium NV	131:4758606c9316	1482	g = index->keyValue;
TASS Belgium NV	131:4758606c9316	1483	pico_tree_delete(found->MCASTGroups, g);
tass picotcp@tass.be	149:5f4cb161cec3	1484	PICO_FREE(g);
TASS Belgium NV	131:4758606c9316	1485	}
TASS Belgium NV	131:4758606c9316	1486	} while(0);
tass	68:0847e35d08a6	1487	#endif
tass	68:0847e35d08a6	1488
TASS Belgium NV	131:4758606c9316	1489	pico_ipv4_cleanup_routes(found);
TASS Belgium NV	131:4758606c9316	1490	pico_tree_delete(&Tree_dev_link, found);
tass picotcp@tass.be	149:5f4cb161cec3	1491	PICO_FREE(found);
tass	72:887bc44746ff	1492
TASS Belgium NV	131:4758606c9316	1493	return 0;
tass	68:0847e35d08a6	1494	}
tass	68:0847e35d08a6	1495
tass	68:0847e35d08a6	1496
tass	68:0847e35d08a6	1497	struct pico_ipv4_link pico_ipv4_link_get(struct pico_ip4 address)
tass	68:0847e35d08a6	1498	{
TASS Belgium NV	131:4758606c9316	1499	struct pico_ipv4_link test = {
TASS Belgium NV	131:4758606c9316	1500	0
TASS Belgium NV	131:4758606c9316	1501	}, *found = NULL;
TASS Belgium NV	131:4758606c9316	1502	test.address.addr = address->addr;
tass	68:0847e35d08a6	1503
TASS Belgium NV	131:4758606c9316	1504	found = pico_tree_findKey(&Tree_dev_link, &test);
TASS Belgium NV	131:4758606c9316	1505	if (!found)
TASS Belgium NV	131:4758606c9316	1506	return NULL;
TASS Belgium NV	131:4758606c9316	1507	else
TASS Belgium NV	131:4758606c9316	1508	return found;
tass	68:0847e35d08a6	1509	}
tass	68:0847e35d08a6	1510
tass	68:0847e35d08a6	1511	struct pico_ipv4_link pico_ipv4_link_by_dev(struct pico_device dev)
tass	68:0847e35d08a6	1512	{
TASS Belgium NV	131:4758606c9316	1513	struct pico_tree_node *index = NULL;
TASS Belgium NV	131:4758606c9316	1514	struct pico_ipv4_link *link = NULL;
tass	68:0847e35d08a6	1515
TASS Belgium NV	131:4758606c9316	1516	pico_tree_foreach(index, &Tree_dev_link)
TASS Belgium NV	131:4758606c9316	1517	{
TASS Belgium NV	131:4758606c9316	1518	link = index->keyValue;
TASS Belgium NV	131:4758606c9316	1519	if (link->dev == dev)
TASS Belgium NV	131:4758606c9316	1520	return link;
TASS Belgium NV	131:4758606c9316	1521	}
TASS Belgium NV	131:4758606c9316	1522	return NULL;
tass	68:0847e35d08a6	1523	}
tass	68:0847e35d08a6	1524
tass	74:c146c4e346c4	1525	struct pico_ipv4_link pico_ipv4_link_by_dev_next(struct pico_device dev, struct pico_ipv4_link *last)
tass	74:c146c4e346c4	1526	{
TASS Belgium NV	131:4758606c9316	1527	struct pico_tree_node *index = NULL;
TASS Belgium NV	131:4758606c9316	1528	struct pico_ipv4_link *link = NULL;
TASS Belgium NV	131:4758606c9316	1529	int valid = 0;
tass	74:c146c4e346c4	1530
TASS Belgium NV	131:4758606c9316	1531	if (last == NULL)
TASS Belgium NV	131:4758606c9316	1532	valid = 1;
tass	74:c146c4e346c4	1533
TASS Belgium NV	131:4758606c9316	1534	pico_tree_foreach(index, &Tree_dev_link)
TASS Belgium NV	131:4758606c9316	1535	{
TASS Belgium NV	131:4758606c9316	1536	link = index->keyValue;
TASS Belgium NV	131:4758606c9316	1537	if (link->dev == dev) {
TASS Belgium NV	131:4758606c9316	1538	if (last == link)
TASS Belgium NV	131:4758606c9316	1539	valid = 1;
TASS Belgium NV	131:4758606c9316	1540	else if (valid > 0)
TASS Belgium NV	131:4758606c9316	1541	return link;
TASS Belgium NV	131:4758606c9316	1542	}
tass	74:c146c4e346c4	1543	}
TASS Belgium NV	131:4758606c9316	1544	return NULL;
tass	74:c146c4e346c4	1545	}
tass	68:0847e35d08a6	1546
tass	68:0847e35d08a6	1547	struct pico_device pico_ipv4_link_find(struct pico_ip4 address)
tass	68:0847e35d08a6	1548	{
TASS Belgium NV	131:4758606c9316	1549	struct pico_ipv4_link test, *found;
TASS Belgium NV	131:4758606c9316	1550	if(!address) {
TASS Belgium NV	131:4758606c9316	1551	pico_err = PICO_ERR_EINVAL;
TASS Belgium NV	131:4758606c9316	1552	return NULL;
TASS Belgium NV	131:4758606c9316	1553	}
TASS Belgium NV	131:4758606c9316	1554
TASS Belgium NV	131:4758606c9316	1555	test.dev = NULL;
TASS Belgium NV	131:4758606c9316	1556	test.address.addr = address->addr;
TASS Belgium NV	131:4758606c9316	1557	found = pico_tree_findKey(&Tree_dev_link, &test);
TASS Belgium NV	131:4758606c9316	1558	if (!found) {
TASS Belgium NV	131:4758606c9316	1559	pico_err = PICO_ERR_ENXIO;
TASS Belgium NV	131:4758606c9316	1560	return NULL;
TASS Belgium NV	131:4758606c9316	1561	}
TASS Belgium NV	131:4758606c9316	1562
TASS Belgium NV	131:4758606c9316	1563	return found->dev;
tass	68:0847e35d08a6	1564	}
tass	68:0847e35d08a6	1565
tass picotcp@tass.be	149:5f4cb161cec3	1566
tass picotcp@tass.be	149:5f4cb161cec3	1567
tass picotcp@tass.be	149:5f4cb161cec3	1568	static int pico_ipv4_rebound_large(struct pico_frame *f)
tass picotcp@tass.be	149:5f4cb161cec3	1569	{
tass picotcp@tass.be	149:5f4cb161cec3	1570	uint32_t total_payload_written = 0;
tass picotcp@tass.be	149:5f4cb161cec3	1571	uint32_t len = f->transport_len;
tass picotcp@tass.be	149:5f4cb161cec3	1572	struct pico_frame *fr;
tass picotcp@tass.be	149:5f4cb161cec3	1573	struct pico_ip4 dst;
tass picotcp@tass.be	149:5f4cb161cec3	1574	struct pico_ipv4_hdr *hdr;
tass picotcp@tass.be	149:5f4cb161cec3	1575	hdr = (struct pico_ipv4_hdr *) f->net_hdr;
tass picotcp@tass.be	149:5f4cb161cec3	1576	dst.addr = hdr->src.addr;
tass picotcp@tass.be	149:5f4cb161cec3	1577
tass picotcp@tass.be	149:5f4cb161cec3	1578	#ifdef PICO_SUPPORT_IPFRAG
tass picotcp@tass.be	149:5f4cb161cec3	1579	while(total_payload_written < len) {
tass picotcp@tass.be	149:5f4cb161cec3	1580	uint32_t space = (uint32_t)len - total_payload_written;
tass picotcp@tass.be	149:5f4cb161cec3	1581	if (space > PICO_IPV4_MAXPAYLOAD)
tass picotcp@tass.be	149:5f4cb161cec3	1582	space = PICO_IPV4_MAXPAYLOAD;
tass picotcp@tass.be	149:5f4cb161cec3	1583
tass picotcp@tass.be	149:5f4cb161cec3	1584	fr = pico_ipv4_alloc(&pico_proto_ipv4, (uint16_t)space);
tass picotcp@tass.be	149:5f4cb161cec3	1585	if (!fr) {
tass picotcp@tass.be	149:5f4cb161cec3	1586	pico_err = PICO_ERR_ENOMEM;
tass picotcp@tass.be	149:5f4cb161cec3	1587	return -1;
tass picotcp@tass.be	149:5f4cb161cec3	1588	}
tass picotcp@tass.be	149:5f4cb161cec3	1589
tass picotcp@tass.be	149:5f4cb161cec3	1590	if (space + total_payload_written < len)
tass picotcp@tass.be	149:5f4cb161cec3	1591	fr->frag \|= short_be(PICO_IPV4_MOREFRAG);
tass picotcp@tass.be	149:5f4cb161cec3	1592	else
tass picotcp@tass.be	149:5f4cb161cec3	1593	fr->frag &= short_be(PICO_IPV4_FRAG_MASK);
tass picotcp@tass.be	149:5f4cb161cec3	1594
tass picotcp@tass.be	149:5f4cb161cec3	1595	fr->frag \|= short_be((uint16_t)((total_payload_written) >> 3u));
tass picotcp@tass.be	149:5f4cb161cec3	1596
tass picotcp@tass.be	149:5f4cb161cec3	1597	memcpy(fr->transport_hdr, f->transport_hdr + total_payload_written, fr->transport_len);
tass picotcp@tass.be	149:5f4cb161cec3	1598	if (pico_ipv4_frame_push(fr, &dst, hdr->proto) > 0) {
tass picotcp@tass.be	149:5f4cb161cec3	1599	total_payload_written += fr->transport_len;
tass picotcp@tass.be	149:5f4cb161cec3	1600	} else {
tass picotcp@tass.be	149:5f4cb161cec3	1601	pico_frame_discard(fr);
tass picotcp@tass.be	149:5f4cb161cec3	1602	break;
tass picotcp@tass.be	149:5f4cb161cec3	1603	}
tass picotcp@tass.be	149:5f4cb161cec3	1604	} /* while() */
tass picotcp@tass.be	149:5f4cb161cec3	1605	return (int)total_payload_written;
tass picotcp@tass.be	149:5f4cb161cec3	1606	#else
tass picotcp@tass.be	149:5f4cb161cec3	1607	return -1;
tass picotcp@tass.be	149:5f4cb161cec3	1608	#endif
tass picotcp@tass.be	149:5f4cb161cec3	1609	}
tass picotcp@tass.be	149:5f4cb161cec3	1610
tass	68:0847e35d08a6	1611	int pico_ipv4_rebound(struct pico_frame *f)
tass	68:0847e35d08a6	1612	{
TASS Belgium NV	131:4758606c9316	1613	struct pico_ip4 dst;
TASS Belgium NV	131:4758606c9316	1614	struct pico_ipv4_hdr *hdr;
TASS Belgium NV	131:4758606c9316	1615	if(!f) {
TASS Belgium NV	131:4758606c9316	1616	pico_err = PICO_ERR_EINVAL;
TASS Belgium NV	131:4758606c9316	1617	return -1;
TASS Belgium NV	131:4758606c9316	1618	}
tass	68:0847e35d08a6	1619
TASS Belgium NV	131:4758606c9316	1620	hdr = (struct pico_ipv4_hdr *) f->net_hdr;
TASS Belgium NV	131:4758606c9316	1621	if (!hdr) {
TASS Belgium NV	131:4758606c9316	1622	pico_err = PICO_ERR_EINVAL;
TASS Belgium NV	131:4758606c9316	1623	return -1;
TASS Belgium NV	131:4758606c9316	1624	}
TASS Belgium NV	131:4758606c9316	1625
TASS Belgium NV	131:4758606c9316	1626	dst.addr = hdr->src.addr;
tass picotcp@tass.be	149:5f4cb161cec3	1627	if (f->transport_len > PICO_IPV4_MAXPAYLOAD) {
tass picotcp@tass.be	149:5f4cb161cec3	1628	return pico_ipv4_rebound_large(f);
tass picotcp@tass.be	149:5f4cb161cec3	1629	}
tass picotcp@tass.be	149:5f4cb161cec3	1630
TASS Belgium NV	131:4758606c9316	1631	return pico_ipv4_frame_push(f, &dst, hdr->proto);
tass	68:0847e35d08a6	1632	}
tass	68:0847e35d08a6	1633
tass	68:0847e35d08a6	1634	static int pico_ipv4_forward(struct pico_frame *f)
tass	68:0847e35d08a6	1635	{
TASS Belgium NV	131:4758606c9316	1636	struct pico_ipv4_hdr hdr = (struct pico_ipv4_hdr )f->net_hdr;
TASS Belgium NV	131:4758606c9316	1637	struct pico_ipv4_route *rt;
TASS Belgium NV	131:4758606c9316	1638	if (!hdr) {
TASS Belgium NV	131:4758606c9316	1639	return -1;
TASS Belgium NV	131:4758606c9316	1640	}
tass	68:0847e35d08a6	1641
TASS Belgium NV	131:4758606c9316	1642	rt = route_find(&hdr->dst);
TASS Belgium NV	131:4758606c9316	1643	if (!rt) {
TASS Belgium NV	131:4758606c9316	1644	pico_notify_dest_unreachable(f);
TASS Belgium NV	131:4758606c9316	1645	return -1;
TASS Belgium NV	131:4758606c9316	1646	}
tass	110:0ece1bbbd36e	1647
TASS Belgium NV	131:4758606c9316	1648	f->dev = rt->link->dev;
TASS Belgium NV	131:4758606c9316	1649	hdr->ttl = (uint8_t)(hdr->ttl - 1);
TASS Belgium NV	131:4758606c9316	1650	if (hdr->ttl < 1) {
TASS Belgium NV	131:4758606c9316	1651	pico_notify_ttl_expired(f);
TASS Belgium NV	131:4758606c9316	1652	dbg(" ------------------- TTL EXPIRED\n");
TASS Belgium NV	131:4758606c9316	1653	return -1;
TASS Belgium NV	131:4758606c9316	1654	}
tass	68:0847e35d08a6	1655
TASS Belgium NV	131:4758606c9316	1656	hdr->crc++;
TASS Belgium NV	131:4758606c9316	1657
TASS Belgium NV	131:4758606c9316	1658	pico_ipv4_nat_outbound(f, &rt->link->address);
tass	68:0847e35d08a6	1659
TASS Belgium NV	131:4758606c9316	1660	f->start = f->net_hdr;
TASS Belgium NV	131:4758606c9316	1661	if(f->dev->eth != NULL)
TASS Belgium NV	131:4758606c9316	1662	f->len -= PICO_SIZE_ETHHDR;
TASS Belgium NV	131:4758606c9316	1663
TASS Belgium NV	131:4758606c9316	1664	pico_sendto_dev(f);
TASS Belgium NV	131:4758606c9316	1665	return 0;
tass	68:0847e35d08a6	1666
tass	68:0847e35d08a6	1667	}
tass	68:0847e35d08a6	1668
tass	68:0847e35d08a6	1669	int pico_ipv4_is_broadcast(uint32_t addr)
tass	68:0847e35d08a6	1670	{
TASS Belgium NV	131:4758606c9316	1671	struct pico_ipv4_link *link;
TASS Belgium NV	131:4758606c9316	1672	struct pico_tree_node *index;
TASS Belgium NV	131:4758606c9316	1673	if (addr == PICO_IP4_BCAST)
TASS Belgium NV	131:4758606c9316	1674	return 1;
TASS Belgium NV	131:4758606c9316	1675
TASS Belgium NV	131:4758606c9316	1676	pico_tree_foreach(index, &Tree_dev_link) {
TASS Belgium NV	131:4758606c9316	1677	link = index->keyValue;
TASS Belgium NV	131:4758606c9316	1678	if ((link->address.addr \| (~link->netmask.addr)) == addr)
TASS Belgium NV	131:4758606c9316	1679	return 1;
TASS Belgium NV	131:4758606c9316	1680	}
TASS Belgium NV	131:4758606c9316	1681	return 0;
tass	68:0847e35d08a6	1682	}
tass	68:0847e35d08a6	1683
tass	68:0847e35d08a6	1684	void pico_ipv4_unreachable(struct pico_frame *f, int err)
tass	68:0847e35d08a6	1685	{
TASS Belgium NV	131:4758606c9316	1686	struct pico_ipv4_hdr hdr = (struct pico_ipv4_hdr ) f->net_hdr;
tass	68:0847e35d08a6	1687	#if defined PICO_SUPPORT_TCP \|\| defined PICO_SUPPORT_UDP
TASS Belgium NV	131:4758606c9316	1688	f->transport_hdr = ((uint8_t *)f->net_hdr) + PICO_SIZE_IP4HDR;
TASS Belgium NV	131:4758606c9316	1689	pico_transport_error(f, hdr->proto, err);
tass	68:0847e35d08a6	1690	#endif
tass	68:0847e35d08a6	1691	}
tass	68:0847e35d08a6	1692
tass picotcp@tass.be	149:5f4cb161cec3	1693	int pico_ipv4_cleanup_links(struct pico_device *dev)
tass picotcp@tass.be	149:5f4cb161cec3	1694	{
tass picotcp@tass.be	149:5f4cb161cec3	1695	struct pico_tree_node index = NULL, _tmp = NULL;
tass picotcp@tass.be	149:5f4cb161cec3	1696	struct pico_ipv4_link *link = NULL;
tass picotcp@tass.be	149:5f4cb161cec3	1697
tass picotcp@tass.be	149:5f4cb161cec3	1698	pico_tree_foreach_safe(index, &Tree_dev_link, _tmp)
tass picotcp@tass.be	149:5f4cb161cec3	1699	{
tass picotcp@tass.be	149:5f4cb161cec3	1700	link = index->keyValue;
tass picotcp@tass.be	149:5f4cb161cec3	1701	if (dev == link->dev)
tass picotcp@tass.be	149:5f4cb161cec3	1702	pico_ipv4_link_del(dev, link->address);
tass picotcp@tass.be	149:5f4cb161cec3	1703	}
tass picotcp@tass.be	149:5f4cb161cec3	1704	return 0;
tass picotcp@tass.be	149:5f4cb161cec3	1705	}
tass picotcp@tass.be	149:5f4cb161cec3	1706
tass picotcp@tass.be	149:5f4cb161cec3	1707
tass	68:0847e35d08a6	1708	#endif