[apple/ipsec.git] / ipsec-tools / racoon / isakmp_frag.c

/*	$NetBSD: isakmp_frag.c,v 1.4 2006/09/09 16:22:09 manu Exp $	*/

/* Id: isakmp_frag.c,v 1.4 2004/11/13 17:31:36 manubsd Exp */

/*
 * Copyright (C) 2004 Emmanuel Dreyfus
 * All rights reserved.
 * 
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. Neither the name of the project nor the names of its contributors
 *    may be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE PROJECT AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE PROJECT OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */

#include "config.h"

#include <sys/types.h>
#include <sys/param.h>
#include <sys/socket.h>
#include <sys/queue.h>

#include <netinet/in.h>
#include <arpa/inet.h>

#ifdef HAVE_OPENSSL
#include <openssl/md5.h> 
#endif

#include <stdlib.h>
#include <stdio.h>
#include <fcntl.h>
#include <string.h>
#include <errno.h>
#if TIME_WITH_SYS_TIME
# include <sys/time.h>
# include <time.h>
#else
# if HAVE_SYS_TIME_H
#  include <sys/time.h>
# else
#  include <time.h>
# endif
#endif
#include <netdb.h>
#ifdef HAVE_UNISTD_H
#include <unistd.h>
#endif
#include <ctype.h>

#include "var.h"
#include "misc.h"
#include "vmbuf.h"
#include "plog.h"
#include "sockmisc.h"
#include "schedule.h"
#include "debug.h"

#include "isakmp_var.h"
#include "isakmp.h"
#include "handler.h"
#include "isakmp_frag.h"
#include "strnames.h"
#include "nattraversal.h"
#include "grabmyaddr.h"
#include "localconf.h"
#include "crypto_openssl.h"

int
isakmp_sendfrags(iph1, buf) 
	phase1_handle_t *iph1;
	vchar_t *buf;
{
	struct isakmp *hdr;
	struct isakmp_frag *fraghdr;
	caddr_t data;
	caddr_t sdata;
	size_t datalen;
	size_t max_datalen;
	size_t fraglen;
	vchar_t *frag;
	unsigned int fragnum = 0;
	size_t len;
	int etype;
#ifdef ENABLE_NATT
	size_t extralen = NON_ESP_MARKER_USE(iph1)? NON_ESP_MARKER_LEN : 0;
#else
	size_t extralen = 0;
#endif
	int s;
	vchar_t *vbuf;


	/* select the socket to be sent */
	s = getsockmyaddr((struct sockaddr *)iph1->local);
	if (s == -1){
		return -1;
	}

	/*
	 * Catch the exchange type for later: the fragments and the
	 * fragmented packet must have the same exchange type.
	 */
	hdr = (struct isakmp *)buf->v;
	etype = hdr->etype;

	/*
	 * We want to send a a packet smaller than ISAKMP_FRAG_MAXLEN
	 * First compute the maximum data length that will fit in it
	 */
	max_datalen = ISAKMP_FRAG_MAXLEN - 
	    (sizeof(*hdr) + sizeof(*fraghdr));

	sdata = buf->v;
	len = buf->l;

	while (len > 0) {
		fragnum++;

		if (len > max_datalen)
			datalen = max_datalen;
		else
			datalen = len;

		fraglen = sizeof(*hdr) + sizeof(*fraghdr) + datalen;

		if ((frag = vmalloc(fraglen)) == NULL) {
			plog(ASL_LEVEL_ERR, 
			    "Cannot allocate memory\n");
			return -1;
		}

		set_isakmp_header1(frag, iph1, ISAKMP_NPTYPE_FRAG);
		hdr = (struct isakmp *)frag->v;
		hdr->etype = etype;

		fraghdr = (struct isakmp_frag *)(hdr + 1);
		fraghdr->unknown0 = 0;
		fraghdr->len = htons(fraglen - sizeof(*hdr));
		fraghdr->unknown1 = htons(1);
		fraghdr->index = fragnum;
		if (len == datalen)
			fraghdr->flags = ISAKMP_FRAG_LAST;
		else
			fraghdr->flags = 0;

		data = (caddr_t)(fraghdr + 1);
		memcpy(data, sdata, datalen);

#ifdef ENABLE_NATT
		/* If NAT-T port floating is in use, 4 zero bytes (non-ESP marker) 
		 must added just before the packet itself. For this we must 
		 allocate a new buffer and release it at the end. */
		if (extralen) {
			if ((vbuf = vmalloc(frag->l + extralen)) == NULL) {
				plog(ASL_LEVEL_ERR, 
					 "%s: vbuf allocation failed\n", __FUNCTION__);
				vfree(frag);
				return -1;
			}
			*ALIGNED_CAST(u_int32_t *)vbuf->v = 0; // non-esp marker
			memcpy(vbuf->v + extralen, frag->v, frag->l);
			vfree(frag);
			frag = vbuf;
		}
#endif

		if (sendfromto(s, frag->v, frag->l,
					   iph1->local, iph1->remote, lcconf->count_persend) == -1) {
			plog(ASL_LEVEL_ERR, "%s: sendfromto failed\n", __FUNCTION__);
			vfree(frag);
			return -1;
		}
		
		vfree(frag);

		len -= datalen;
		sdata += datalen;
	}

	plog(ASL_LEVEL_DEBUG, 
		 "%s: processed %d fragments\n", __FUNCTION__, fragnum);

	return fragnum;
}

unsigned int 
vendorid_frag_cap(gen)
	struct isakmp_gen *gen;
{
	int *hp;
	int hashlen_bytes = eay_md5_hashlen() >> 3;

	hp = ALIGNED_CAST(int *)(gen + 1);

	return ntohl(hp[hashlen_bytes / sizeof(*hp)]);
}

int 
isakmp_frag_extract(iph1, msg)
	phase1_handle_t *iph1;
	vchar_t *msg;
{
	struct isakmp *isakmp;
	struct isakmp_frag *frag;
	struct isakmp_frag_item *item;
	vchar_t *buf;
	int last_frag = 0;
	char *data;
	int i;

	if (msg->l < sizeof(*isakmp) + sizeof(*frag)) {
		plog(ASL_LEVEL_ERR, "Message too short\n");
		return -1;
	}

	isakmp = (struct isakmp *)msg->v;
	frag = (struct isakmp_frag *)(isakmp + 1);

	/* 
	 * frag->len is the frag payload data plus the frag payload header,
	 * whose size is sizeof(*frag) 
	 */
	if (msg->l < sizeof(*isakmp) + ntohs(frag->len) ||
		ntohs(frag->len) < sizeof(*frag) + 1) {
		plog(ASL_LEVEL_ERR, "Fragment too short\n");
		return -1;
	}

	if (ntohs(frag->len) < sizeof(*frag)) {
		plog(ASL_LEVEL_ERR, 
			 "invalid Frag, frag-len %d\n",
			 ntohs(frag->len));
		return -1;
	}

	if ((buf = vmalloc(ntohs(frag->len) - sizeof(*frag))) == NULL) {
		plog(ASL_LEVEL_ERR, "Cannot allocate memory\n");
		return -1;
	}

	if ((item = racoon_malloc(sizeof(*item))) == NULL) {
		plog(ASL_LEVEL_ERR, "Cannot allocate memory\n");
		vfree(buf);
		return -1;
	}
    bzero(item, sizeof(*item));
    
	data = (char *)(frag + 1);
	memcpy(buf->v, data, buf->l);

	item->frag_num = frag->index;
	item->frag_last = (frag->flags & ISAKMP_FRAG_LAST);
	item->frag_next = NULL;
	item->frag_packet = buf;
	item->frag_id = ntohs(frag->unknown1);
    
    plog(ASL_LEVEL_DEBUG,
		 "%s: received fragment #%d  frag ID=%d  last frag=%d\n",
            __FUNCTION__, item->frag_num, item->frag_id, item->frag_last);

    /* Insert if new and find the last frag num if present */
    struct isakmp_frag_item *current;

    last_frag = (item->frag_last ? item->frag_num : 0);
    current = iph1->frag_chain;
    while (current) {
        if (current->frag_num == item->frag_num) {  // duplicate?
            vfree(item->frag_packet);
            racoon_free(item);
            return 0;   // already have it
        }
        if (current->frag_last)
            last_frag = current->frag_num;
        current = current->frag_next;
    }
    /* no dup - insert it */
    item->frag_next = iph1->frag_chain;
    iph1->frag_chain = item;

    /* Check if the chain is complete   */
    if (last_frag == 0)
        return 0;       /* if last_frag not found - chain is not complete */
    for (i = 1; i <= last_frag; i++) {
        current = iph1->frag_chain;
        while (current) {
            if (current->frag_num == i)
                break;
            current = current->frag_next;
        };
        if (!current)
            return 0;   /* chain not complete */
    }

	plog(ASL_LEVEL_DEBUG, 
		 "%s: processed fragment %d\n", __FUNCTION__, frag->index);
	return 1;   /* chain is complete */
}

vchar_t *
isakmp_frag_reassembly(iph1)
	phase1_handle_t *iph1;
{
	struct isakmp_frag_item *item;
	size_t len = 0;
	vchar_t *buf = NULL;
	int frag_count = 0, frag_max = 0;
	int i;
	char *data;

	if ((item = iph1->frag_chain) == NULL) {
		plog(ASL_LEVEL_ERR, "No fragment to reassemble\n");
		goto out;
	}

	do {
		frag_count++;
		if (item->frag_num > frag_max && item->frag_last) {
			frag_max = item->frag_num;
		}
		len += item->frag_packet->l;
		item = item->frag_next;
	} while (item != NULL);
	
	if ((buf = vmalloc(len)) == NULL) {
		plog(ASL_LEVEL_ERR, "Cannot allocate memory\n");
		goto out;
	}
	data = buf->v;

	for (i = 1; i <= frag_max; i++) {
		item = iph1->frag_chain;
		do {
			if (item->frag_num == i)
				break;
			item = item->frag_next;
		} while (item != NULL);

		if (item == NULL) {
			plog(ASL_LEVEL_ERR, 
			    "Missing fragment #%d\n", i);
			vfree(buf);
			buf = NULL;
			return buf;
        }
		memcpy(data, item->frag_packet->v, item->frag_packet->l);
		data += item->frag_packet->l;
	}

	plog(ASL_LEVEL_DEBUG, 
		 "%s: processed %d fragments\n", __FUNCTION__, frag_count);

out:
	item = iph1->frag_chain;		
	
	while (item != NULL) {
		struct isakmp_frag_item *next_item;

		next_item = item->frag_next;

		vfree(item->frag_packet);
		racoon_free(item);

		item = next_item;
	} 

	iph1->frag_chain = NULL;

    //plogdump(ASL_LEVEL_DEBUG, buf->v, buf->l, "re-assembled fragements:\n");
	return buf;
}

vchar_t *
isakmp_frag_addcap(buf, cap)
	vchar_t *buf;
	int cap;
{
	int val, *capp;
	size_t len;
	int hashlen_bytes = eay_md5_hashlen() >> 3;

	/* If the capability has not been added, add room now */
	len = buf->l;
	if (len == hashlen_bytes) {
		if ((buf = vrealloc(buf, len + sizeof(cap))) == NULL) {
			plog(ASL_LEVEL_ERR, 
			    "Cannot allocate memory\n");
			return NULL;
		}
        val = 0;                                    
        memcpy(buf->v + len, &val, sizeof(val));        // Wcast_lign fix - copy instead of assign for unaligned move
    }
    capp = (int *)(void*)(buf->v + hashlen_bytes);      // Wcast_lign fix - copy instead of assign for unaligned move
    memcpy(&val, capp, sizeof(val));
    val |= htonl(cap);
    memcpy(capp, &val, sizeof(val));
           
	return buf;
}

int
sendfragsfromto(s, buf, local, remote, count_persend, frag_flags) 
	int              s;
	vchar_t         *buf;
	struct sockaddr_storage *local;
	struct sockaddr_storage *remote;
	int              count_persend;
	u_int32_t        frag_flags;
{
	struct isakmp *main_hdr;
	struct isakmp *hdr;
	struct isakmp_frag *fraghdr;
	caddr_t data;
	caddr_t sdata;
	size_t datalen;
	size_t max_datalen;
	size_t fraglen;
	vchar_t *frag;
	unsigned int fragnum = 0;
	size_t len;
#ifdef ENABLE_NATT
	size_t extralen = (frag_flags & FRAG_PUT_NON_ESP_MARKER)? NON_ESP_MARKER_LEN : 0;
#else
	size_t extralen = 0;
#endif
	
	/*
	 * fragmented packet must have the same exchange type (amongst other fields in the header).
	 */
	main_hdr = (struct isakmp *)buf->v;

	/*
	 * We want to send a a packet smaller than ISAKMP_FRAG_MAXLEN
	 * First compute the maximum data length that will fit in it
	 */
	max_datalen = ISAKMP_FRAG_MAXLEN - 
	(sizeof(*main_hdr) + sizeof(*fraghdr));

	sdata = buf->v;
	len = buf->l;
	
	while (len > 0) {
		fragnum++;

		if (len > max_datalen)
			datalen = max_datalen;
		else
			datalen = len;

		fraglen = sizeof(*hdr) + sizeof(*fraghdr) + datalen;

		if ((frag = vmalloc(fraglen)) == NULL) {
			plog(ASL_LEVEL_ERR, 
				 "Cannot allocate memory\n");
			return -1;
		}

		hdr = (struct isakmp *)frag->v;
		bcopy(main_hdr, hdr, sizeof(*hdr));
		hdr->len = htonl(frag->l);
		hdr->np = ISAKMP_NPTYPE_FRAG;

		fraghdr = (struct isakmp_frag *)(hdr + 1);
		fraghdr->unknown0 = 0;
		fraghdr->len = htons(fraglen - sizeof(*hdr));
		fraghdr->unknown1 = htons(1);
		fraghdr->index = fragnum;
		if (len == datalen)
			fraghdr->flags = ISAKMP_FRAG_LAST;
		else
			fraghdr->flags = 0;

		data = (caddr_t)(fraghdr + 1);
		memcpy(data, sdata, datalen);

#ifdef ENABLE_NATT
		/* If NAT-T port floating is in use, 4 zero bytes (non-ESP marker) 
		 must added just before the packet itself. For this we must 
		 allocate a new buffer and release it at the end. */
		if (extralen) {
			vchar_t *vbuf;
			
			if ((vbuf = vmalloc(frag->l + extralen)) == NULL) {
				plog(ASL_LEVEL_ERR, 
					 "%s: vbuf allocation failed\n", __FUNCTION__);
				vfree(frag);
				return -1;
			}
			*ALIGNED_CAST(u_int32_t *)vbuf->v = 0; // non-esp marker
			memcpy(vbuf->v + extralen, frag->v, frag->l);
			vfree(frag);
			frag = vbuf;
		}
#endif

		if (sendfromto(s, frag->v, frag->l, local, remote, count_persend) == -1) {
			plog(ASL_LEVEL_ERR, "sendfromto failed\n");
			vfree(frag);
			return -1;
		}

		vfree(frag);

		len -= datalen;
		sdata += datalen;
	}

	plog(ASL_LEVEL_DEBUG, 
		 "%s: processed %d fragments\n", __FUNCTION__, fragnum);

	return fragnum;
}
Commit	Line	Data
d1e348cf A	1	/* $NetBSD: isakmp_frag.c,v 1.4 2006/09/09 16:22:09 manu Exp $ */
	2
	3	/* Id: isakmp_frag.c,v 1.4 2004/11/13 17:31:36 manubsd Exp */
52b7d2ce A	4
	5	/*
	6	* Copyright (C) 2004 Emmanuel Dreyfus
	7	* All rights reserved.
	8	*
	9	* Redistribution and use in source and binary forms, with or without
	10	* modification, are permitted provided that the following conditions
	11	* are met:
	12	* 1. Redistributions of source code must retain the above copyright
	13	* notice, this list of conditions and the following disclaimer.
	14	* 2. Redistributions in binary form must reproduce the above copyright
	15	* notice, this list of conditions and the following disclaimer in the
	16	* documentation and/or other materials provided with the distribution.
	17	* 3. Neither the name of the project nor the names of its contributors
	18	* may be used to endorse or promote products derived from this software
	19	* without specific prior written permission.
	20	*
	21	* THIS SOFTWARE IS PROVIDED BY THE PROJECT AND CONTRIBUTORS ``AS IS'' AND
	22	* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
	23	* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
	24	* ARE DISCLAIMED. IN NO EVENT SHALL THE PROJECT OR CONTRIBUTORS BE LIABLE
	25	* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
	26	* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
	27	* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
	28	* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
	29	* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
	30	* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
	31	* SUCH DAMAGE.
	32	*/
	33
	34	#include "config.h"
	35
	36	#include <sys/types.h>
	37	#include <sys/param.h>
	38	#include <sys/socket.h>
	39	#include <sys/queue.h>
	40
	41	#include <netinet/in.h>
	42	#include <arpa/inet.h>
	43
e8d9021d	44	#ifdef HAVE_OPENSSL
52b7d2ce	45	#include <openssl/md5.h>
e8d9021d	46	#endif
52b7d2ce A	47
	48	#include <stdlib.h>
	49	#include <stdio.h>
	50	#include <fcntl.h>
	51	#include <string.h>
	52	#include <errno.h>
	53	#if TIME_WITH_SYS_TIME
	54	# include <sys/time.h>
	55	# include <time.h>
	56	#else
	57	# if HAVE_SYS_TIME_H
	58	# include <sys/time.h>
	59	# else
	60	# include <time.h>
	61	# endif
	62	#endif
	63	#include <netdb.h>
	64	#ifdef HAVE_UNISTD_H
	65	#include <unistd.h>
	66	#endif
	67	#include <ctype.h>
	68
	69	#include "var.h"
	70	#include "misc.h"
	71	#include "vmbuf.h"
	72	#include "plog.h"
	73	#include "sockmisc.h"
	74	#include "schedule.h"
	75	#include "debug.h"
	76
	77	#include "isakmp_var.h"
	78	#include "isakmp.h"
	79	#include "handler.h"
	80	#include "isakmp_frag.h"
	81	#include "strnames.h"
e8d9021d A	82	#include "nattraversal.h"
	83	#include "grabmyaddr.h"
	84	#include "localconf.h"
85f41bec	85	#include "crypto_openssl.h"
52b7d2ce A	86
	87	int
	88	isakmp_sendfrags(iph1, buf)
65c25746	89	phase1_handle_t *iph1;
52b7d2ce A	90	vchar_t *buf;
	91	{
	92	struct isakmp *hdr;
	93	struct isakmp_frag *fraghdr;
	94	caddr_t data;
	95	caddr_t sdata;
	96	size_t datalen;
	97	size_t max_datalen;
	98	size_t fraglen;
	99	vchar_t *frag;
52b7d2ce A	100	unsigned int fragnum = 0;
	101	size_t len;
	102	int etype;
e8d9021d A	103	#ifdef ENABLE_NATT
	104	size_t extralen = NON_ESP_MARKER_USE(iph1)? NON_ESP_MARKER_LEN : 0;
	105	#else
	106	size_t extralen = 0;
	107	#endif
	108	int s;
	109	vchar_t *vbuf;
	110
	111
	112	/* select the socket to be sent */
85f41bec	113	s = getsockmyaddr((struct sockaddr *)iph1->local);
e8d9021d A	114	if (s == -1){
	115	return -1;
	116	}
52b7d2ce A	117
	118	/*
	119	* Catch the exchange type for later: the fragments and the
	120	* fragmented packet must have the same exchange type.
	121	*/
	122	hdr = (struct isakmp *)buf->v;
	123	etype = hdr->etype;
	124
	125	/*
	126	* We want to send a a packet smaller than ISAKMP_FRAG_MAXLEN
	127	* First compute the maximum data length that will fit in it
	128	*/
	129	max_datalen = ISAKMP_FRAG_MAXLEN -
e8d9021d	130	(sizeof(hdr) + sizeof(fraghdr));
52b7d2ce A	131
	132	sdata = buf->v;
	133	len = buf->l;
	134
	135	while (len > 0) {
	136	fragnum++;
	137
	138	if (len > max_datalen)
	139	datalen = max_datalen;
	140	else
	141	datalen = len;
	142
e8d9021d	143	fraglen = sizeof(hdr) + sizeof(fraghdr) + datalen;
52b7d2ce A	144
52b7d2ce A	145	if ((frag = vmalloc(fraglen)) == NULL) {
65c25746	146	plog(ASL_LEVEL_ERR,
52b7d2ce A	147	"Cannot allocate memory\n");
	148	return -1;
	149	}
	150
	151	set_isakmp_header1(frag, iph1, ISAKMP_NPTYPE_FRAG);
	152	hdr = (struct isakmp *)frag->v;
	153	hdr->etype = etype;
	154
	155	fraghdr = (struct isakmp_frag *)(hdr + 1);
e8d9021d	156	fraghdr->unknown0 = 0;
52b7d2ce A	157	fraghdr->len = htons(fraglen - sizeof(*hdr));
	158	fraghdr->unknown1 = htons(1);
	159	fraghdr->index = fragnum;
	160	if (len == datalen)
	161	fraghdr->flags = ISAKMP_FRAG_LAST;
	162	else
	163	fraghdr->flags = 0;
	164
	165	data = (caddr_t)(fraghdr + 1);
	166	memcpy(data, sdata, datalen);
	167
e8d9021d A	168	#ifdef ENABLE_NATT
	169	/* If NAT-T port floating is in use, 4 zero bytes (non-ESP marker)
	170	must added just before the packet itself. For this we must
	171	allocate a new buffer and release it at the end. */
	172	if (extralen) {
	173	if ((vbuf = vmalloc(frag->l + extralen)) == NULL) {
65c25746	174	plog(ASL_LEVEL_ERR,
e8d9021d A	175	"%s: vbuf allocation failed\n", __FUNCTION__);
	176	vfree(frag);
	177	return -1;
	178	}
85f41bec	179	ALIGNED_CAST(u_int32_t )vbuf->v = 0; // non-esp marker
e8d9021d A	180	memcpy(vbuf->v + extralen, frag->v, frag->l);
	181	vfree(frag);
	182	frag = vbuf;
	183	}
	184	#endif
	185
	186	if (sendfromto(s, frag->v, frag->l,
	187	iph1->local, iph1->remote, lcconf->count_persend) == -1) {
65c25746	188	plog(ASL_LEVEL_ERR, "%s: sendfromto failed\n", __FUNCTION__);
e8d9021d	189	vfree(frag);
52b7d2ce A	190	return -1;
52b7d2ce A	191	}
d1e348cf	192
52b7d2ce A	193	vfree(frag);
	194
	195	len -= datalen;
	196	sdata += datalen;
	197	}
e8d9021d	198
65c25746	199	plog(ASL_LEVEL_DEBUG,
e8d9021d A	200	"%s: processed %d fragments\n", __FUNCTION__, fragnum);
e8d9021d A	201
52b7d2ce A	202	return fragnum;
	203	}
	204
	205	unsigned int
	206	vendorid_frag_cap(gen)
	207	struct isakmp_gen *gen;
	208	{
	209	int *hp;
e8d9021d	210	int hashlen_bytes = eay_md5_hashlen() >> 3;
52b7d2ce	211
85f41bec	212	hp = ALIGNED_CAST(int *)(gen + 1);
52b7d2ce	213
e8d9021d	214	return ntohl(hp[hashlen_bytes / sizeof(*hp)]);
52b7d2ce A	215	}
	216
	217	int
	218	isakmp_frag_extract(iph1, msg)
65c25746	219	phase1_handle_t *iph1;
52b7d2ce A	220	vchar_t *msg;
	221	{
	222	struct isakmp *isakmp;
	223	struct isakmp_frag *frag;
	224	struct isakmp_frag_item *item;
	225	vchar_t *buf;
52b7d2ce A	226	int last_frag = 0;
	227	char *data;
	228	int i;
	229
	230	if (msg->l < sizeof(isakmp) + sizeof(frag)) {
65c25746	231	plog(ASL_LEVEL_ERR, "Message too short\n");
52b7d2ce A	232	return -1;
	233	}
	234
	235	isakmp = (struct isakmp *)msg->v;
	236	frag = (struct isakmp_frag *)(isakmp + 1);
	237
	238	/*
	239	* frag->len is the frag payload data plus the frag payload header,
	240	* whose size is sizeof(*frag)
	241	*/
47612122 A	242	if (msg->l < sizeof(*isakmp) + ntohs(frag->len) \|\|
47612122 A	243	ntohs(frag->len) < sizeof(*frag) + 1) {
65c25746	244	plog(ASL_LEVEL_ERR, "Fragment too short\n");
52b7d2ce A	245	return -1;
	246	}
	247
e8d9021d	248	if (ntohs(frag->len) < sizeof(*frag)) {
65c25746	249	plog(ASL_LEVEL_ERR,
e8d9021d A	250	"invalid Frag, frag-len %d\n",
	251	ntohs(frag->len));
	252	return -1;
	253	}
	254
52b7d2ce	255	if ((buf = vmalloc(ntohs(frag->len) - sizeof(*frag))) == NULL) {
65c25746	256	plog(ASL_LEVEL_ERR, "Cannot allocate memory\n");
52b7d2ce A	257	return -1;
	258	}
	259
	260	if ((item = racoon_malloc(sizeof(*item))) == NULL) {
65c25746	261	plog(ASL_LEVEL_ERR, "Cannot allocate memory\n");
52b7d2ce A	262	vfree(buf);
	263	return -1;
	264	}
65c25746 A	265	bzero(item, sizeof(*item));
65c25746 A	266
52b7d2ce A	267	data = (char *)(frag + 1);
	268	memcpy(buf->v, data, buf->l);
	269
	270	item->frag_num = frag->index;
	271	item->frag_last = (frag->flags & ISAKMP_FRAG_LAST);
	272	item->frag_next = NULL;
	273	item->frag_packet = buf;
e8d9021d	274	item->frag_id = ntohs(frag->unknown1);
65c25746 A	275
	276	plog(ASL_LEVEL_DEBUG,
	277	"%s: received fragment #%d frag ID=%d last frag=%d\n",
	278	__FUNCTION__, item->frag_num, item->frag_id, item->frag_last);
	279
	280	/* Insert if new and find the last frag num if present */
	281	struct isakmp_frag_item *current;
	282
	283	last_frag = (item->frag_last ? item->frag_num : 0);
	284	current = iph1->frag_chain;
	285	while (current) {
	286	if (current->frag_num == item->frag_num) { // duplicate?
	287	vfree(item->frag_packet);
	288	racoon_free(item);
	289	return 0; // already have it
	290	}
	291	if (current->frag_last)
	292	last_frag = current->frag_num;
	293	current = current->frag_next;
	294	}
	295	/* no dup - insert it */
	296	item->frag_next = iph1->frag_chain;
	297	iph1->frag_chain = item;
	298
	299	/* Check if the chain is complete */
	300	if (last_frag == 0)
	301	return 0; /* if last_frag not found - chain is not complete */
	302	for (i = 1; i <= last_frag; i++) {
	303	current = iph1->frag_chain;
	304	while (current) {
	305	if (current->frag_num == i)
	306	break;
	307	current = current->frag_next;
	308	};
	309	if (!current)
	310	return 0; /* chain not complete */
	311	}
52b7d2ce	312
65c25746 A	313	plog(ASL_LEVEL_DEBUG,
	314	"%s: processed fragment %d\n", __FUNCTION__, frag->index);
	315	return 1; /* chain is complete */
52b7d2ce A	316	}
	317
	318	vchar_t *
	319	isakmp_frag_reassembly(iph1)
65c25746	320	phase1_handle_t *iph1;
52b7d2ce A	321	{
	322	struct isakmp_frag_item *item;
	323	size_t len = 0;
	324	vchar_t *buf = NULL;
ed5ea7fc	325	int frag_count = 0, frag_max = 0;
52b7d2ce A	326	int i;
	327	char *data;
	328
	329	if ((item = iph1->frag_chain) == NULL) {
65c25746	330	plog(ASL_LEVEL_ERR, "No fragment to reassemble\n");
52b7d2ce A	331	goto out;
	332	}
	333
	334	do {
	335	frag_count++;
ed5ea7fc A	336	if (item->frag_num > frag_max && item->frag_last) {
	337	frag_max = item->frag_num;
	338	}
52b7d2ce A	339	len += item->frag_packet->l;
	340	item = item->frag_next;
	341	} while (item != NULL);
	342
	343	if ((buf = vmalloc(len)) == NULL) {
65c25746	344	plog(ASL_LEVEL_ERR, "Cannot allocate memory\n");
52b7d2ce A	345	goto out;
	346	}
	347	data = buf->v;
	348
ed5ea7fc	349	for (i = 1; i <= frag_max; i++) {
52b7d2ce A	350	item = iph1->frag_chain;
	351	do {
	352	if (item->frag_num == i)
	353	break;
	354	item = item->frag_next;
	355	} while (item != NULL);
	356
	357	if (item == NULL) {
65c25746	358	plog(ASL_LEVEL_ERR,
52b7d2ce A	359	"Missing fragment #%d\n", i);
	360	vfree(buf);
	361	buf = NULL;
ed5ea7fc	362	return buf;
65c25746	363	}
52b7d2ce A	364	memcpy(data, item->frag_packet->v, item->frag_packet->l);
	365	data += item->frag_packet->l;
	366	}
	367
65c25746	368	plog(ASL_LEVEL_DEBUG,
e8d9021d A	369	"%s: processed %d fragments\n", __FUNCTION__, frag_count);
e8d9021d A	370
52b7d2ce A	371	out:
52b7d2ce A	372	item = iph1->frag_chain;
e8d9021d A	373
e8d9021d A	374	while (item != NULL) {
52b7d2ce A	375	struct isakmp_frag_item *next_item;
	376
	377	next_item = item->frag_next;
	378
	379	vfree(item->frag_packet);
	380	racoon_free(item);
	381
	382	item = next_item;
e8d9021d	383	}
52b7d2ce A	384
	385	iph1->frag_chain = NULL;
	386
65c25746	387	//plogdump(ASL_LEVEL_DEBUG, buf->v, buf->l, "re-assembled fragements:\n");
52b7d2ce A	388	return buf;
	389	}
	390
	391	vchar_t *
	392	isakmp_frag_addcap(buf, cap)
	393	vchar_t *buf;
	394	int cap;
	395	{
85f41bec	396	int val, *capp;
52b7d2ce	397	size_t len;
e8d9021d	398	int hashlen_bytes = eay_md5_hashlen() >> 3;
52b7d2ce A	399
	400	/* If the capability has not been added, add room now */
	401	len = buf->l;
e8d9021d	402	if (len == hashlen_bytes) {
52b7d2ce	403	if ((buf = vrealloc(buf, len + sizeof(cap))) == NULL) {
65c25746	404	plog(ASL_LEVEL_ERR,
52b7d2ce A	405	"Cannot allocate memory\n");
	406	return NULL;
	407	}
85f41bec A	408	val = 0;
	409	memcpy(buf->v + len, &val, sizeof(val)); // Wcast_lign fix - copy instead of assign for unaligned move
	410	}
	411	capp = (int )(void)(buf->v + hashlen_bytes); // Wcast_lign fix - copy instead of assign for unaligned move
	412	memcpy(&val, capp, sizeof(val));
	413	val \|= htonl(cap);
	414	memcpy(capp, &val, sizeof(val));
	415
52b7d2ce A	416	return buf;
	417	}
	418
e8d9021d A	419	int
	420	sendfragsfromto(s, buf, local, remote, count_persend, frag_flags)
	421	int s;
	422	vchar_t *buf;
85f41bec A	423	struct sockaddr_storage *local;
85f41bec A	424	struct sockaddr_storage *remote;
e8d9021d A	425	int count_persend;
	426	u_int32_t frag_flags;
	427	{
	428	struct isakmp *main_hdr;
	429	struct isakmp *hdr;
	430	struct isakmp_frag *fraghdr;
	431	caddr_t data;
	432	caddr_t sdata;
	433	size_t datalen;
	434	size_t max_datalen;
	435	size_t fraglen;
	436	vchar_t *frag;
e8d9021d A	437	unsigned int fragnum = 0;
	438	size_t len;
	439	#ifdef ENABLE_NATT
	440	size_t extralen = (frag_flags & FRAG_PUT_NON_ESP_MARKER)? NON_ESP_MARKER_LEN : 0;
	441	#else
	442	size_t extralen = 0;
	443	#endif
	444
	445	/*
	446	* fragmented packet must have the same exchange type (amongst other fields in the header).
	447	*/
	448	main_hdr = (struct isakmp *)buf->v;
	449
	450	/*
	451	* We want to send a a packet smaller than ISAKMP_FRAG_MAXLEN
	452	* First compute the maximum data length that will fit in it
	453	*/
	454	max_datalen = ISAKMP_FRAG_MAXLEN -
	455	(sizeof(main_hdr) + sizeof(fraghdr));
	456
	457	sdata = buf->v;
	458	len = buf->l;
	459
	460	while (len > 0) {
	461	fragnum++;
	462
	463	if (len > max_datalen)
	464	datalen = max_datalen;
	465	else
	466	datalen = len;
	467
	468	fraglen = sizeof(hdr) + sizeof(fraghdr) + datalen;
	469
	470	if ((frag = vmalloc(fraglen)) == NULL) {
65c25746	471	plog(ASL_LEVEL_ERR,
e8d9021d A	472	"Cannot allocate memory\n");
	473	return -1;
	474	}
	475
	476	hdr = (struct isakmp *)frag->v;
	477	bcopy(main_hdr, hdr, sizeof(*hdr));
	478	hdr->len = htonl(frag->l);
	479	hdr->np = ISAKMP_NPTYPE_FRAG;
	480
	481	fraghdr = (struct isakmp_frag *)(hdr + 1);
	482	fraghdr->unknown0 = 0;
	483	fraghdr->len = htons(fraglen - sizeof(*hdr));
	484	fraghdr->unknown1 = htons(1);
	485	fraghdr->index = fragnum;
	486	if (len == datalen)
	487	fraghdr->flags = ISAKMP_FRAG_LAST;
	488	else
	489	fraghdr->flags = 0;
	490
	491	data = (caddr_t)(fraghdr + 1);
	492	memcpy(data, sdata, datalen);
	493
	494	#ifdef ENABLE_NATT
	495	/* If NAT-T port floating is in use, 4 zero bytes (non-ESP marker)
	496	must added just before the packet itself. For this we must
	497	allocate a new buffer and release it at the end. */
	498	if (extralen) {
	499	vchar_t *vbuf;
	500
	501	if ((vbuf = vmalloc(frag->l + extralen)) == NULL) {
65c25746	502	plog(ASL_LEVEL_ERR,
e8d9021d A	503	"%s: vbuf allocation failed\n", __FUNCTION__);
	504	vfree(frag);
	505	return -1;
	506	}
85f41bec	507	ALIGNED_CAST(u_int32_t )vbuf->v = 0; // non-esp marker
e8d9021d A	508	memcpy(vbuf->v + extralen, frag->v, frag->l);
	509	vfree(frag);
	510	frag = vbuf;
	511	}
	512	#endif
	513
	514	if (sendfromto(s, frag->v, frag->l, local, remote, count_persend) == -1) {
65c25746	515	plog(ASL_LEVEL_ERR, "sendfromto failed\n");
e8d9021d A	516	vfree(frag);
	517	return -1;
	518	}
	519
	520	vfree(frag);
	521
	522	len -= datalen;
	523	sdata += datalen;
	524	}
	525
65c25746	526	plog(ASL_LEVEL_DEBUG,
e8d9021d A	527	"%s: processed %d fragments\n", __FUNCTION__, fragnum);
	528
	529	return fragnum;
	530	}