[thirdparty/strongswan.git] / src / libipsec / ip_packet.c

/*
 * Copyright (C) 2012-2014 Tobias Brunner
 * Hochschule fuer Technik Rapperswil
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation; either version 2 of the License, or (at your
 * option) any later version.  See <http://www.fsf.org/copyleft/gpl.txt>.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
 * for more details.
 */


#include "ip_packet.h"

#include <library.h>
#include <utils/debug.h>

#include <sys/types.h>

#ifndef WIN32
#include <netinet/in.h>
#include <netinet/ip.h>
#ifdef HAVE_NETINET_IP6_H
#include <netinet/ip6.h>
#endif
#else
struct ip {
#if BYTE_ORDER == LITTLE_ENDIAN
	uint8_t ip_hl: 4;
	uint8_t ip_v: 4;
#elif BYTE_ORDER == BIG_ENDIAN
	uint8_t ip_v: 4;
	uint8_t ip_hl: 4;
#endif
	uint8_t ip_tos;
	uint16_t ip_len;
	uint16_t ip_id;
	uint16_t ip_off;
	uint8_t ip_ttl;
	uint8_t ip_p;
	uint16_t ip_sum;
	struct in_addr ip_src, ip_dst;
} __attribute__((packed));
struct ip6_hdr {
	uint32_t ip6_flow; /* 4 bit version, 8 bit TC, 20 bit flow label */
	uint16_t ip6_plen;
	uint8_t ip6_nxt;
	uint8_t ip6_hlim;
	struct in6_addr ip6_src, ip6_dst;
} __attribute__((packed));
struct ip6_ext {
	uint8_t ip6e_nxt;
	uint8_t ip6e_len;
} __attribute__((packed));
#define HAVE_NETINET_IP6_H /* not really, but we only need the structs above */
#endif

#ifndef IP_OFFMASK
#define IP_OFFMASK 0x1fff
#endif

/**
 * TCP header, defined here because platforms disagree regarding member names
 * and unfortunately Android does not define a variant with BSD names.
 */
struct tcphdr {
	uint16_t source;
	uint16_t dest;
	uint32_t seq;
	uint32_t ack_seq;
	uint16_t flags;
	uint16_t window;
	uint16_t check;
	uint16_t urg_ptr;
} __attribute__((packed));

/**
 * UDP header, similar to the TCP header the system headers disagree on member
 * names.  Linux uses a union and on Android we could define __FAVOR_BSD to get
 * the BSD member names, but this is simpler and more consistent with the above.
 */
struct udphdr {
	uint16_t source;
	uint16_t dest;
	uint16_t len;
	uint16_t check;
} __attribute__((packed));

typedef struct private_ip_packet_t private_ip_packet_t;

/**
 * Private additions to ip_packet_t.
 */
struct private_ip_packet_t {

	/**
	 * Public members
	 */
	ip_packet_t public;

	/**
	 * Source address
	 */
	host_t *src;

	/**
	 * Destination address
	 */
	host_t *dst;

	/**
	 * IP packet
	 */
	chunk_t packet;

	/**
	 * IP payload (points into packet)
	 */
	chunk_t payload;

	/**
	 * IP version
	 */
	uint8_t version;

	/**
	 * Protocol|Next Header field
	 */
	uint8_t next_header;

};

METHOD(ip_packet_t, get_version, uint8_t,
	private_ip_packet_t *this)
{
	return this->version;
}

METHOD(ip_packet_t, get_source, host_t*,
	private_ip_packet_t *this)
{
	return this->src;
}

METHOD(ip_packet_t, get_destination, host_t*,
	private_ip_packet_t *this)
{
	return this->dst;
}

METHOD(ip_packet_t, get_encoding, chunk_t,
	private_ip_packet_t *this)
{
	return this->packet;
}

METHOD(ip_packet_t, get_payload, chunk_t,
	private_ip_packet_t *this)
{
	return this->payload;
}

METHOD(ip_packet_t, get_next_header, uint8_t,
	private_ip_packet_t *this)
{
	return this->next_header;
}

METHOD(ip_packet_t, clone_, ip_packet_t*,
	private_ip_packet_t *this)
{
	return ip_packet_create(chunk_clone(this->packet));
}

METHOD(ip_packet_t, destroy, void,
	private_ip_packet_t *this)
{
	this->src->destroy(this->src);
	this->dst->destroy(this->dst);
	chunk_free(&this->packet);
	free(this);
}

/**
 * Parse transport protocol header
 */
static bool parse_transport_header(chunk_t packet, uint8_t proto,
								   uint16_t *sport, uint16_t *dport)
{
	switch (proto)
	{
		case IPPROTO_UDP:
		{
			struct udphdr *udp;

			if (packet.len < sizeof(*udp))
			{
				DBG1(DBG_ESP, "UDP packet too short");
				return FALSE;
			}
			udp = (struct udphdr*)packet.ptr;
			*sport = ntohs(udp->source);
			*dport = ntohs(udp->dest);
			break;
		}
		case IPPROTO_TCP:
		{
			struct tcphdr *tcp;

			if (packet.len < sizeof(*tcp))
			{
				DBG1(DBG_ESP, "TCP packet too short");
				return FALSE;
			}
			tcp = (struct tcphdr*)packet.ptr;
			*sport = ntohs(tcp->source);
			*dport = ntohs(tcp->dest);
			break;
		}
		default:
			break;
	}
	return TRUE;
}

#ifdef HAVE_NETINET_IP6_H
/**
 * Skip to the actual payload and parse the transport header.
 */
static bool parse_transport_header_v6(struct ip6_hdr *ip, chunk_t packet,
									  chunk_t *payload, uint8_t *proto,
									  uint16_t *sport, uint16_t *dport)
{
	struct ip6_ext *ext;
	bool fragment = FALSE;

	*proto = ip->ip6_nxt;
	*payload = chunk_skip(packet, 40);
	while (payload->len >= sizeof(struct ip6_ext))
	{
		switch (*proto)
		{
			case 44:  /* Fragment Header */
				fragment = TRUE;
				/* skip the header */
			case 0:   /* Hop-by-Hop Options Header */
			case 43:  /* Routing Header */
			case 60:  /* Destination Options Header */
			case 135: /* Mobility Header */
			case 139: /* HIP */
			case 140: /* Shim6 */
				/* simply skip over these headers for now */
				ext = (struct ip6_ext*)payload->ptr;
				*proto = ext->ip6e_nxt;
				*payload = chunk_skip(*payload, 8 * (ext->ip6e_len + 1));
				continue;
			default:
				/* assume anything else is an upper layer protocol but only
				 * attempt to parse the transport header for non-fragmented
				 * packets as there is no guarantee that initial fragments
				 * contain the transport header, depending on the number and
				 * type of extension headers */
				if (!fragment &&
					!parse_transport_header(*payload, *proto, sport, dport))
				{
					return FALSE;
				}
				break;
		}
		break;
	}
	return TRUE;
}
#endif /* HAVE_NETINET_IP6_H */

/**
 * Described in header.
 */
ip_packet_t *ip_packet_create(chunk_t packet)
{
	private_ip_packet_t *this;
	uint8_t version, next_header;
	uint16_t sport = 0, dport = 0;
	host_t *src, *dst;
	chunk_t payload;

	if (packet.len < 1)
	{
		DBG1(DBG_ESP, "IP packet too short");
		goto failed;
	}

	version = (packet.ptr[0] & 0xf0) >> 4;

	switch (version)
	{
		case 4:
		{
			struct ip *ip;

			if (packet.len < sizeof(struct ip))
			{
				DBG1(DBG_ESP, "IPv4 packet too short");
				goto failed;
			}
			ip = (struct ip*)packet.ptr;
			/* remove any RFC 4303 TFC extra padding */
			packet.len = min(packet.len, untoh16(&ip->ip_len));
			payload = chunk_skip(packet, ip->ip_hl * 4);
			if ((ip->ip_off & htons(IP_OFFMASK)) == 0 &&
				!parse_transport_header(payload, ip->ip_p, &sport, &dport))
			{
				goto failed;
			}
			src = host_create_from_chunk(AF_INET,
										 chunk_from_thing(ip->ip_src), sport);
			dst = host_create_from_chunk(AF_INET,
										 chunk_from_thing(ip->ip_dst), dport);
			next_header = ip->ip_p;
			break;
		}
#ifdef HAVE_NETINET_IP6_H
		case 6:
		{
			struct ip6_hdr *ip;

			if (packet.len < sizeof(*ip))
			{
				DBG1(DBG_ESP, "IPv6 packet too short");
				goto failed;
			}
			ip = (struct ip6_hdr*)packet.ptr;
			/* remove any RFC 4303 TFC extra padding */
			packet.len = min(packet.len, 40 + untoh16(&ip->ip6_plen));
			if (!parse_transport_header_v6(ip, packet, &payload, &next_header,
										   &sport, &dport))
			{
				goto failed;
			}
			src = host_create_from_chunk(AF_INET6,
										 chunk_from_thing(ip->ip6_src), sport);
			dst = host_create_from_chunk(AF_INET6,
										 chunk_from_thing(ip->ip6_dst), dport);
			break;
		}
#endif /* HAVE_NETINET_IP6_H */
		default:
			DBG1(DBG_ESP, "unsupported IP version");
			goto failed;
	}

	INIT(this,
		.public = {
			.get_version = _get_version,
			.get_source = _get_source,
			.get_destination = _get_destination,
			.get_next_header = _get_next_header,
			.get_encoding = _get_encoding,
			.get_payload = _get_payload,
			.clone = _clone_,
			.destroy = _destroy,
		},
		.src = src,
		.dst = dst,
		.packet = packet,
		.payload = payload,
		.version = version,
		.next_header = next_header,
	);
	return &this->public;

failed:
	chunk_free(&packet);
	return NULL;
}

/**
 * Calculate the checksum for the pseudo IP header
 */
static uint16_t pseudo_header_checksum(host_t *src, host_t *dst,
										uint8_t proto, chunk_t payload)
{
	switch (src->get_family(src))
	{
		case AF_INET:
		{
			struct __attribute__((packed)) {
				uint32_t src;
				uint32_t dst;
				u_char zero;
				u_char proto;
				uint16_t len;
			} pseudo = {
				.proto = proto,
				.len = htons(payload.len),
			};
			memcpy(&pseudo.src, src->get_address(src).ptr,
				   sizeof(pseudo.src));
			memcpy(&pseudo.dst, dst->get_address(dst).ptr,
				   sizeof(pseudo.dst));
			return chunk_internet_checksum(chunk_from_thing(pseudo));
		}
		case AF_INET6:
		{
			struct __attribute__((packed)) {
				u_char src[16];
				u_char dst[16];
				uint32_t len;
				u_char zero[3];
				u_char next_header;
			} pseudo = {
				.next_header = proto,
				.len = htons(payload.len),
			};
			memcpy(&pseudo.src, src->get_address(src).ptr,
				   sizeof(pseudo.src));
			memcpy(&pseudo.dst, dst->get_address(dst).ptr,
				   sizeof(pseudo.dst));
			return chunk_internet_checksum(chunk_from_thing(pseudo));
		}
	}
	return 0xffff;
}

/**
 * Apply transport ports and calculate header checksums
 */
static void fix_transport_header(host_t *src, host_t *dst, uint8_t proto,
								 chunk_t payload)
{
	uint16_t sum = 0, sport, dport;

	sport = src->get_port(src);
	dport = dst->get_port(dst);

	switch (proto)
	{
		case IPPROTO_UDP:
		{
			struct udphdr *udp;

			if (payload.len < sizeof(*udp))
			{
				return;
			}
			udp = (struct udphdr*)payload.ptr;
			if (sport != 0)
			{
				udp->source = htons(sport);
			}
			if (dport != 0)
			{
				udp->dest = htons(dport);
			}
			udp->check = 0;
			sum = pseudo_header_checksum(src, dst, proto, payload);
			udp->check = chunk_internet_checksum_inc(payload, sum);
			break;
		}
		case IPPROTO_TCP:
		{
			struct tcphdr *tcp;

			if (payload.len < sizeof(*tcp))
			{
				return;
			}
			tcp = (struct tcphdr*)payload.ptr;
			if (sport != 0)
			{
				tcp->source = htons(sport);
			}
			if (dport != 0)
			{
				tcp->dest = htons(dport);
			}
			tcp->check = 0;
			sum = pseudo_header_checksum(src, dst, proto, payload);
			tcp->check = chunk_internet_checksum_inc(payload, sum);
			break;
		}
		default:
			break;
	}
}

/**
 * Described in header.
 */
ip_packet_t *ip_packet_create_from_data(host_t *src, host_t *dst,
										uint8_t next_header, chunk_t data)
{
	chunk_t packet;
	int family;

	family = src->get_family(src);
	if (family != dst->get_family(dst))
	{
		DBG1(DBG_ESP, "address family does not match");
		return NULL;
	}

	switch (family)
	{
		case AF_INET:
		{
			struct ip ip = {
				.ip_v = 4,
				.ip_hl = 5,
				.ip_len = htons(20 + data.len),
				.ip_ttl = 0x80,
				.ip_p = next_header,
			};
			memcpy(&ip.ip_src, src->get_address(src).ptr, sizeof(ip.ip_src));
			memcpy(&ip.ip_dst, dst->get_address(dst).ptr, sizeof(ip.ip_dst));
			ip.ip_sum = chunk_internet_checksum(chunk_from_thing(ip));

			packet = chunk_cat("cc", chunk_from_thing(ip), data);
			fix_transport_header(src, dst, next_header, chunk_skip(packet, 20));
			return ip_packet_create(packet);
		}
#ifdef HAVE_NETINET_IP6_H
		case AF_INET6:
		{
			struct ip6_hdr ip = {
				.ip6_flow = htonl(6 << 28),
				.ip6_plen = htons(data.len),
				.ip6_nxt = next_header,
				.ip6_hlim = 0x80,
			};
			memcpy(&ip.ip6_src, src->get_address(src).ptr, sizeof(ip.ip6_src));
			memcpy(&ip.ip6_dst, dst->get_address(dst).ptr, sizeof(ip.ip6_dst));

			packet = chunk_cat("cc", chunk_from_thing(ip), data);
			fix_transport_header(src, dst, next_header, chunk_skip(packet, 40));
			return ip_packet_create(packet);
		}
#endif /* HAVE_NETINET_IP6_H */
		default:
			DBG1(DBG_ESP, "unsupported address family");
			return NULL;
	}
}

/**
 * Described in header.
 */
ip_packet_t *ip_packet_create_udp_from_data(host_t *src, host_t *dst,
											chunk_t data)
{
	struct udphdr udp = {
		.len = htons(8 + data.len),
		.check = 0,
	};
	ip_packet_t *packet;

	data = chunk_cat("cc", chunk_from_thing(udp), data);
	packet = ip_packet_create_from_data(src, dst, IPPROTO_UDP, data);
	chunk_free(&data);
	return packet;
}
Commit	Line	Data
2dd47c24	1	/*
a10eb935	2	* Copyright (C) 2012-2014 Tobias Brunner
2dd47c24 TB	3	* Hochschule fuer Technik Rapperswil
	4	*
	5	* This program is free software; you can redistribute it and/or modify it
	6	* under the terms of the GNU General Public License as published by the
	7	* Free Software Foundation; either version 2 of the License, or (at your
	8	* option) any later version. See <http://www.fsf.org/copyleft/gpl.txt>.
	9	*
	10	* This program is distributed in the hope that it will be useful, but
	11	* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
	12	* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
	13	* for more details.
	14	*/
	15
	16
	17	#include "ip_packet.h"
	18
	19	#include <library.h>
f05b4272	20	#include <utils/debug.h>
2dd47c24	21
ae8ac58c	22	#include <sys/types.h>
1da56773 TB	23
1da56773 TB	24	#ifndef WIN32
2dd47c24 TB	25	#include <netinet/in.h>
2dd47c24 TB	26	#include <netinet/ip.h>
b6a07151	27	#ifdef HAVE_NETINET_IP6_H
2dd47c24	28	#include <netinet/ip6.h>
b6a07151	29	#endif
1da56773 TB	30	#else
	31	struct ip {
	32	#if BYTE_ORDER == LITTLE_ENDIAN
	33	uint8_t ip_hl: 4;
	34	uint8_t ip_v: 4;
	35	#elif BYTE_ORDER == BIG_ENDIAN
	36	uint8_t ip_v: 4;
	37	uint8_t ip_hl: 4;
	38	#endif
	39	uint8_t ip_tos;
	40	uint16_t ip_len;
	41	uint16_t ip_id;
	42	uint16_t ip_off;
	43	uint8_t ip_ttl;
	44	uint8_t ip_p;
	45	uint16_t ip_sum;
	46	struct in_addr ip_src, ip_dst;
	47	} __attribute__((packed));
	48	struct ip6_hdr {
	49	uint32_t ip6_flow; /* 4 bit version, 8 bit TC, 20 bit flow label */
	50	uint16_t ip6_plen;
	51	uint8_t ip6_nxt;
	52	uint8_t ip6_hlim;
	53	struct in6_addr ip6_src, ip6_dst;
	54	} __attribute__((packed));
bdcaca76 TB	55	struct ip6_ext {
	56	uint8_t ip6e_nxt;
	57	uint8_t ip6e_len;
	58	} __attribute__((packed));
	59	#define HAVE_NETINET_IP6_H /* not really, but we only need the structs above */
1da56773	60	#endif
2dd47c24	61
b3cc4638 TB	62	#ifndef IP_OFFMASK
	63	#define IP_OFFMASK 0x1fff
	64	#endif
	65
f8613abc TB	66	/**
	67	* TCP header, defined here because platforms disagree regarding member names
	68	* and unfortunately Android does not define a variant with BSD names.
	69	*/
	70	struct tcphdr {
b12c53ce AS	71	uint16_t source;
	72	uint16_t dest;
	73	uint32_t seq;
	74	uint32_t ack_seq;
	75	uint16_t flags;
	76	uint16_t window;
	77	uint16_t check;
	78	uint16_t urg_ptr;
f8613abc TB	79	} __attribute__((packed));
	80
	81	/**
	82	* UDP header, similar to the TCP header the system headers disagree on member
	83	* names. Linux uses a union and on Android we could define __FAVOR_BSD to get
	84	* the BSD member names, but this is simpler and more consistent with the above.
	85	*/
	86	struct udphdr {
b12c53ce AS	87	uint16_t source;
	88	uint16_t dest;
	89	uint16_t len;
	90	uint16_t check;
f8613abc TB	91	} __attribute__((packed));
f8613abc TB	92
2dd47c24 TB	93	typedef struct private_ip_packet_t private_ip_packet_t;
	94
	95	/**
	96	* Private additions to ip_packet_t.
	97	*/
	98	struct private_ip_packet_t {
	99
	100	/**
	101	* Public members
	102	*/
	103	ip_packet_t public;
	104
	105	/**
	106	* Source address
	107	*/
	108	host_t *src;
	109
	110	/**
	111	* Destination address
	112	*/
	113	host_t *dst;
	114
	115	/**
	116	* IP packet
	117	*/
	118	chunk_t packet;
	119
46bb3698 TB	120	/**
	121	* IP payload (points into packet)
	122	*/
	123	chunk_t payload;
	124
2dd47c24 TB	125	/**
	126	* IP version
	127	*/
b12c53ce	128	uint8_t version;
2dd47c24 TB	129
	130	/**
	131	* Protocol\|Next Header field
	132	*/
b12c53ce	133	uint8_t next_header;
2dd47c24 TB	134
	135	};
	136
b12c53ce	137	METHOD(ip_packet_t, get_version, uint8_t,
2dd47c24 TB	138	private_ip_packet_t *this)
	139	{
	140	return this->version;
	141	}
	142
	143	METHOD(ip_packet_t, get_source, host_t*,
	144	private_ip_packet_t *this)
	145	{
	146	return this->src;
	147	}
	148
	149	METHOD(ip_packet_t, get_destination, host_t*,
	150	private_ip_packet_t *this)
	151	{
	152	return this->dst;
	153	}
	154
	155	METHOD(ip_packet_t, get_encoding, chunk_t,
	156	private_ip_packet_t *this)
	157	{
	158	return this->packet;
	159	}
	160
46bb3698 TB	161	METHOD(ip_packet_t, get_payload, chunk_t,
	162	private_ip_packet_t *this)
	163	{
	164	return this->payload;
	165	}
	166
b12c53ce	167	METHOD(ip_packet_t, get_next_header, uint8_t,
2dd47c24 TB	168	private_ip_packet_t *this)
	169	{
	170	return this->next_header;
	171	}
	172
5f35b733	173	METHOD(ip_packet_t, clone_, ip_packet_t*,
2dd47c24 TB	174	private_ip_packet_t *this)
2dd47c24 TB	175	{
2b84ccd6	176	return ip_packet_create(chunk_clone(this->packet));
2dd47c24 TB	177	}
	178
	179	METHOD(ip_packet_t, destroy, void,
	180	private_ip_packet_t *this)
	181	{
	182	this->src->destroy(this->src);
	183	this->dst->destroy(this->dst);
	184	chunk_free(&this->packet);
	185	free(this);
	186	}
	187
a10eb935 TB	188	/**
	189	* Parse transport protocol header
	190	*/
b12c53ce AS	191	static bool parse_transport_header(chunk_t packet, uint8_t proto,
b12c53ce AS	192	uint16_t sport, uint16_t dport)
a10eb935 TB	193	{
	194	switch (proto)
	195	{
	196	case IPPROTO_UDP:
	197	{
	198	struct udphdr *udp;
	199
	200	if (packet.len < sizeof(*udp))
	201	{
	202	DBG1(DBG_ESP, "UDP packet too short");
	203	return FALSE;
	204	}
	205	udp = (struct udphdr*)packet.ptr;
	206	*sport = ntohs(udp->source);
	207	*dport = ntohs(udp->dest);
	208	break;
	209	}
	210	case IPPROTO_TCP:
	211	{
	212	struct tcphdr *tcp;
	213
	214	if (packet.len < sizeof(*tcp))
	215	{
	216	DBG1(DBG_ESP, "TCP packet too short");
	217	return FALSE;
	218	}
	219	tcp = (struct tcphdr*)packet.ptr;
	220	*sport = ntohs(tcp->source);
	221	*dport = ntohs(tcp->dest);
	222	break;
	223	}
	224	default:
	225	break;
	226	}
	227	return TRUE;
	228	}
	229
bdcaca76 TB	230	#ifdef HAVE_NETINET_IP6_H
	231	/**
	232	* Skip to the actual payload and parse the transport header.
	233	*/
	234	static bool parse_transport_header_v6(struct ip6_hdr *ip, chunk_t packet,
	235	chunk_t payload, uint8_t proto,
	236	uint16_t sport, uint16_t dport)
	237	{
	238	struct ip6_ext *ext;
	239	bool fragment = FALSE;
	240
	241	*proto = ip->ip6_nxt;
	242	*payload = chunk_skip(packet, 40);
	243	while (payload->len >= sizeof(struct ip6_ext))
	244	{
	245	switch (*proto)
	246	{
	247	case 44: /* Fragment Header */
	248	fragment = TRUE;
	249	/* skip the header */
	250	case 0: /* Hop-by-Hop Options Header */
	251	case 43: /* Routing Header */
	252	case 60: /* Destination Options Header */
	253	case 135: /* Mobility Header */
	254	case 139: /* HIP */
	255	case 140: /* Shim6 */
	256	/* simply skip over these headers for now */
	257	ext = (struct ip6_ext*)payload->ptr;
	258	*proto = ext->ip6e_nxt;
	259	payload = chunk_skip(payload, 8 * (ext->ip6e_len + 1));
	260	continue;
	261	default:
	262	/* assume anything else is an upper layer protocol but only
	263	* attempt to parse the transport header for non-fragmented
	264	* packets as there is no guarantee that initial fragments
	265	* contain the transport header, depending on the number and
	266	* type of extension headers */
	267	if (!fragment &&
	268	!parse_transport_header(payload, proto, sport, dport))
	269	{
	270	return FALSE;
	271	}
	272	break;
	273	}
	274	break;
	275	}
	276	return TRUE;
	277	}
	278	#endif /* HAVE_NETINET_IP6_H */
	279
2dd47c24 TB	280	/**
	281	* Described in header.
	282	*/
	283	ip_packet_t *ip_packet_create(chunk_t packet)
	284	{
	285	private_ip_packet_t *this;
b12c53ce AS	286	uint8_t version, next_header;
b12c53ce AS	287	uint16_t sport = 0, dport = 0;
2dd47c24	288	host_t src, dst;
46bb3698	289	chunk_t payload;
2dd47c24 TB	290
	291	if (packet.len < 1)
	292	{
	293	DBG1(DBG_ESP, "IP packet too short");
	294	goto failed;
	295	}
	296
	297	version = (packet.ptr[0] & 0xf0) >> 4;
	298
	299	switch (version)
	300	{
	301	case 4:
	302	{
39e9af96	303	struct ip *ip;
2dd47c24	304
39e9af96	305	if (packet.len < sizeof(struct ip))
2dd47c24 TB	306	{
	307	DBG1(DBG_ESP, "IPv4 packet too short");
	308	goto failed;
	309	}
39e9af96	310	ip = (struct ip*)packet.ptr;
293515f9 MW	311	/* remove any RFC 4303 TFC extra padding */
293515f9 MW	312	packet.len = min(packet.len, untoh16(&ip->ip_len));
46bb3698	313	payload = chunk_skip(packet, ip->ip_hl * 4);
b3cc4638 TB	314	if ((ip->ip_off & htons(IP_OFFMASK)) == 0 &&
b3cc4638 TB	315	!parse_transport_header(payload, ip->ip_p, &sport, &dport))
a10eb935 TB	316	{
	317	goto failed;
	318	}
2dd47c24	319	src = host_create_from_chunk(AF_INET,
a10eb935	320	chunk_from_thing(ip->ip_src), sport);
2dd47c24	321	dst = host_create_from_chunk(AF_INET,
a10eb935	322	chunk_from_thing(ip->ip_dst), dport);
39e9af96	323	next_header = ip->ip_p;
2dd47c24 TB	324	break;
2dd47c24 TB	325	}
b6a07151	326	#ifdef HAVE_NETINET_IP6_H
2dd47c24 TB	327	case 6:
	328	{
	329	struct ip6_hdr *ip;
	330
a10eb935	331	if (packet.len < sizeof(*ip))
2dd47c24 TB	332	{
	333	DBG1(DBG_ESP, "IPv6 packet too short");
	334	goto failed;
	335	}
	336	ip = (struct ip6_hdr*)packet.ptr;
293515f9	337	/* remove any RFC 4303 TFC extra padding */
15dee933	338	packet.len = min(packet.len, 40 + untoh16(&ip->ip6_plen));
bdcaca76 TB	339	if (!parse_transport_header_v6(ip, packet, &payload, &next_header,
bdcaca76 TB	340	&sport, &dport))
a10eb935 TB	341	{
	342	goto failed;
	343	}
2dd47c24	344	src = host_create_from_chunk(AF_INET6,
a10eb935	345	chunk_from_thing(ip->ip6_src), sport);
2dd47c24	346	dst = host_create_from_chunk(AF_INET6,
a10eb935	347	chunk_from_thing(ip->ip6_dst), dport);
43e0cb65	348	break;
2dd47c24	349	}
b6a07151	350	#endif /* HAVE_NETINET_IP6_H */
2dd47c24 TB	351	default:
	352	DBG1(DBG_ESP, "unsupported IP version");
	353	goto failed;
	354	}
	355
	356	INIT(this,
	357	.public = {
	358	.get_version = _get_version,
	359	.get_source = _get_source,
	360	.get_destination = _get_destination,
	361	.get_next_header = _get_next_header,
	362	.get_encoding = _get_encoding,
46bb3698	363	.get_payload = _get_payload,
5f35b733	364	.clone = _clone_,
2dd47c24 TB	365	.destroy = _destroy,
	366	},
	367	.src = src,
	368	.dst = dst,
	369	.packet = packet,
46bb3698	370	.payload = payload,
2dd47c24 TB	371	.version = version,
	372	.next_header = next_header,
	373	);
	374	return &this->public;
	375
	376	failed:
	377	chunk_free(&packet);
	378	return NULL;
	379	}
d56d9a45 TB	380
	381	/**
	382	* Calculate the checksum for the pseudo IP header
	383	*/
b12c53ce AS	384	static uint16_t pseudo_header_checksum(host_t src, host_t dst,
b12c53ce AS	385	uint8_t proto, chunk_t payload)
d56d9a45 TB	386	{
	387	switch (src->get_family(src))
	388	{
	389	case AF_INET:
	390	{
	391	struct __attribute__((packed)) {
b12c53ce AS	392	uint32_t src;
b12c53ce AS	393	uint32_t dst;
d56d9a45 TB	394	u_char zero;
d56d9a45 TB	395	u_char proto;
b12c53ce	396	uint16_t len;
d56d9a45 TB	397	} pseudo = {
	398	.proto = proto,
	399	.len = htons(payload.len),
	400	};
	401	memcpy(&pseudo.src, src->get_address(src).ptr,
	402	sizeof(pseudo.src));
	403	memcpy(&pseudo.dst, dst->get_address(dst).ptr,
	404	sizeof(pseudo.dst));
	405	return chunk_internet_checksum(chunk_from_thing(pseudo));
	406	}
	407	case AF_INET6:
	408	{
	409	struct __attribute__((packed)) {
	410	u_char src[16];
	411	u_char dst[16];
b12c53ce	412	uint32_t len;
d56d9a45 TB	413	u_char zero[3];
	414	u_char next_header;
	415	} pseudo = {
	416	.next_header = proto,
	417	.len = htons(payload.len),
	418	};
	419	memcpy(&pseudo.src, src->get_address(src).ptr,
	420	sizeof(pseudo.src));
	421	memcpy(&pseudo.dst, dst->get_address(dst).ptr,
	422	sizeof(pseudo.dst));
	423	return chunk_internet_checksum(chunk_from_thing(pseudo));
	424	}
	425	}
	426	return 0xffff;
	427	}
	428
	429	/**
108a6789	430	* Apply transport ports and calculate header checksums
d56d9a45	431	*/
b12c53ce	432	static void fix_transport_header(host_t src, host_t dst, uint8_t proto,
108a6789	433	chunk_t payload)
d56d9a45	434	{
b12c53ce	435	uint16_t sum = 0, sport, dport;
108a6789 TB	436
	437	sport = src->get_port(src);
	438	dport = dst->get_port(dst);
d56d9a45 TB	439
	440	switch (proto)
	441	{
	442	case IPPROTO_UDP:
	443	{
	444	struct udphdr *udp;
	445
	446	if (payload.len < sizeof(*udp))
	447	{
	448	return;
	449	}
	450	udp = (struct udphdr*)payload.ptr;
108a6789 TB	451	if (sport != 0)
	452	{
	453	udp->source = htons(sport);
	454	}
	455	if (dport != 0)
	456	{
	457	udp->dest = htons(dport);
	458	}
d56d9a45 TB	459	udp->check = 0;
	460	sum = pseudo_header_checksum(src, dst, proto, payload);
	461	udp->check = chunk_internet_checksum_inc(payload, sum);
	462	break;
	463	}
	464	case IPPROTO_TCP:
	465	{
	466	struct tcphdr *tcp;
	467
	468	if (payload.len < sizeof(*tcp))
	469	{
	470	return;
	471	}
	472	tcp = (struct tcphdr*)payload.ptr;
108a6789 TB	473	if (sport != 0)
	474	{
	475	tcp->source = htons(sport);
	476	}
	477	if (dport != 0)
	478	{
	479	tcp->dest = htons(dport);
	480	}
d56d9a45 TB	481	tcp->check = 0;
	482	sum = pseudo_header_checksum(src, dst, proto, payload);
	483	tcp->check = chunk_internet_checksum_inc(payload, sum);
	484	break;
	485	}
	486	default:
	487	break;
	488	}
	489	}
	490
	491	/**
	492	* Described in header.
	493	*/
	494	ip_packet_t ip_packet_create_from_data(host_t src, host_t *dst,
b12c53ce	495	uint8_t next_header, chunk_t data)
d56d9a45 TB	496	{
	497	chunk_t packet;
	498	int family;
	499
	500	family = src->get_family(src);
	501	if (family != dst->get_family(dst))
	502	{
	503	DBG1(DBG_ESP, "address family does not match");
	504	return NULL;
	505	}
	506
	507	switch (family)
	508	{
	509	case AF_INET:
	510	{
	511	struct ip ip = {
	512	.ip_v = 4,
	513	.ip_hl = 5,
	514	.ip_len = htons(20 + data.len),
	515	.ip_ttl = 0x80,
	516	.ip_p = next_header,
	517	};
	518	memcpy(&ip.ip_src, src->get_address(src).ptr, sizeof(ip.ip_src));
	519	memcpy(&ip.ip_dst, dst->get_address(dst).ptr, sizeof(ip.ip_dst));
	520	ip.ip_sum = chunk_internet_checksum(chunk_from_thing(ip));
	521
	522	packet = chunk_cat("cc", chunk_from_thing(ip), data);
108a6789	523	fix_transport_header(src, dst, next_header, chunk_skip(packet, 20));
d56d9a45 TB	524	return ip_packet_create(packet);
	525	}
	526	#ifdef HAVE_NETINET_IP6_H
	527	case AF_INET6:
	528	{
	529	struct ip6_hdr ip = {
04ede658	530	.ip6_flow = htonl(6 << 28),
9e20fdba	531	.ip6_plen = htons(data.len),
d56d9a45 TB	532	.ip6_nxt = next_header,
	533	.ip6_hlim = 0x80,
	534	};
	535	memcpy(&ip.ip6_src, src->get_address(src).ptr, sizeof(ip.ip6_src));
	536	memcpy(&ip.ip6_dst, dst->get_address(dst).ptr, sizeof(ip.ip6_dst));
	537
	538	packet = chunk_cat("cc", chunk_from_thing(ip), data);
108a6789	539	fix_transport_header(src, dst, next_header, chunk_skip(packet, 40));
d56d9a45 TB	540	return ip_packet_create(packet);
	541	}
	542	#endif /* HAVE_NETINET_IP6_H */
	543	default:
	544	DBG1(DBG_ESP, "unsupported address family");
	545	return NULL;
	546	}
	547	}
16e519d4 TB	548
	549	/**
	550	* Described in header.
	551	*/
	552	ip_packet_t ip_packet_create_udp_from_data(host_t src, host_t *dst,
	553	chunk_t data)
	554	{
	555	struct udphdr udp = {
	556	.len = htons(8 + data.len),
	557	.check = 0,
	558	};
	559	ip_packet_t *packet;
	560
	561	data = chunk_cat("cc", chunk_from_thing(udp), data);
	562	packet = ip_packet_create_from_data(src, dst, IPPROTO_UDP, data);
	563	chunk_free(&data);
	564	return packet;
	565	}