ipcomp.c

This a good VPN source

		KLIPS_PRINT(sysctl_ipsec_debug_ipcomp,
			    "klips_error:skb_decompress: "
			    "passed in NULL ipsec_sa needed for comp alg, returning ERROR.\n");
		if (flags) *flags |= IPCOMP_PARMERROR;
		return skb;
	}

	if (!flags) {
		KLIPS_PRINT(sysctl_ipsec_debug_ipcomp,
			    "klips_error:skb_decompress: "
			    "passed in NULL flags, returning ERROR.\n");
		ipsec_kfree_skb(skb);
		return NULL;
	}
	
#ifdef NET_21
	oiph = skb->nh.iph;
#else /* NET_21 */
	oiph = skb->ip_hdr;
#endif /* NET_21 */
	
	iphlen = oiph->ihl << 2;
	
	if (oiph->protocol != IPPROTO_COMP) {
		KLIPS_PRINT(sysctl_ipsec_debug_ipcomp,
			    "klips_error:skb_decompress: "
			    "called with non-IPCOMP packet (protocol=%d),"
			    "skipping decompression.\n",
			    oiph->protocol);
		*flags |= IPCOMP_PARMERROR;
		return skb;
	}
	
	if ( (((struct ipcomphdr*)((char*) oiph + iphlen))->ipcomp_flags != 0)
	     || ((((struct ipcomphdr*) ((char*) oiph + iphlen))->ipcomp_cpi
		!= htons(SADB_X_CALG_DEFLATE))
		 && sysctl_ipsec_inbound_policy_check
		 && (!ips || (ips && (ips->ips_encalg != SADB_X_CALG_DEFLATE)))) ) {
		KLIPS_PRINT(sysctl_ipsec_debug_ipcomp,
			    "klips_error:skb_decompress: "
			    "called with incompatible IPCOMP packet (flags=%d, "
			    "cpi=%d), ips-compalg=%d, skipping decompression.\n",
			    ntohs(((struct ipcomphdr*) ((char*) oiph + iphlen))->ipcomp_flags),
			    ntohs(((struct ipcomphdr*) ((char*) oiph + iphlen))->ipcomp_cpi),
			    ips ? ips->ips_encalg : 0);
		*flags |= IPCOMP_PARMERROR;
		
		return skb;
	}
	
	if (ntohs(oiph->frag_off) & ~0x4000) {
		KLIPS_PRINT(sysctl_ipsec_debug_ipcomp,
			    "klips_error:skb_decompress: "
			    "called with fragmented IPCOMP packet, "
			    "skipping decompression.\n");
		*flags |= IPCOMP_PARMERROR;
		return skb;
	}
	
	/* original compressed payload size */
	cpyldsz = ntohs(oiph->tot_len) - iphlen - sizeof(struct ipcomphdr);

	zs.zalloc = my_zcalloc;
	zs.zfree = my_zfree;
	zs.opaque = 0;
	
	zs.next_in = (char *) oiph + iphlen + sizeof(struct ipcomphdr);
	zs.avail_in = cpyldsz;
	
	/* Maybe we should be a bit conservative about memory
	   requirements and use inflateInit2 */
	/* Beware, that this might make us unable to decompress packets
	   from other implementations - HINT: check PGPnet source code */
	/* We want to use inflateInit2 because we don't want the adler
	   header. */
	zresult = inflateInit2(&zs, -15); 
	if (zresult != Z_OK) {
		KLIPS_PRINT(sysctl_ipsec_debug_ipcomp,
			    "klips_error:skb_decompress: "
			    "inflateInit2() returned error %d (%s), "
			    "skipping decompression.\n",
			    zresult,
			    zs.msg ? zs.msg : zError(zresult));
		*flags |= IPCOMP_DECOMPRESSIONERROR;

		return skb;
	}
	
	/* We have no way of knowing the exact length of the resulting
	   decompressed output before we have actually done the decompression.
	   For now, we guess that the packet will not be bigger than the
	   attached ipsec device's mtu or 16260, whichever is biggest.
	   This may be wrong, since the sender's mtu may be bigger yet.
	   XXX This must be dealt with later XXX
	*/
	
	/* max payload size */
	pyldsz = skb->dev ? (skb->dev->mtu < 16260 ? 16260 : skb->dev->mtu)
			  : (65520 - iphlen);
	KLIPS_PRINT(sysctl_ipsec_debug_ipcomp,
		    "klips_debug:skb_decompress: "
		    "max payload size: %d\n", pyldsz);
	
	while (pyldsz > (cpyldsz + sizeof(struct ipcomphdr)) && 
	       (nskb = skb_copy_ipcomp(skb,
				       pyldsz - cpyldsz - sizeof(struct ipcomphdr),
				       GFP_ATOMIC)) == NULL) {
		KLIPS_PRINT(sysctl_ipsec_debug_ipcomp,
			    "klips_error:skb_decompress: "
			    "unable to skb_copy_ipcomp(skb, %d, GFP_ATOMIC), "
			    "trying with less payload size.\n",
			    (int)(pyldsz - cpyldsz - sizeof(struct ipcomphdr)));
		pyldsz >>=1;
	}
	
	if (!nskb) {
		KLIPS_PRINT(sysctl_ipsec_debug_ipcomp,
			    "klips_error:skb_decompress: "
			    "unable to allocate memory, dropping packet.\n");
		*flags |= IPCOMP_DECOMPRESSIONERROR;
		inflateEnd(&zs);

		return skb;
	}
	
#ifdef CONFIG_KLIPS_DEBUG
	if(sysctl_ipsec_debug_ipcomp && sysctl_ipsec_debug_verbose) {
		__u8 *c;
		int i;
		
		c = (__u8*)oiph + iphlen + sizeof(struct ipcomphdr);
		for(i = 0; i < cpyldsz; i++, c++) {
			if(!(i % 16)) {
				printk(KERN_INFO "skb_decompress:   before:");
			}
			printk("%02x ", *c);
			if(!((i + 1) % 16)) {
				printk("\n");
			}
		}
		if(i % 16) {
			printk("\n");
		}
	}
#endif /* CONFIG_KLIPS_DEBUG */

#ifdef NET_21
	iph = nskb->nh.iph;
#else /* NET_21 */
	iph = nskb->ip_hdr;
#endif /* NET_21 */
	zs.next_out = (char *)iph + iphlen;
	zs.avail_out = pyldsz;

	zresult = inflate(&zs, Z_SYNC_FLUSH);

	/* work around a bug in zlib, which sometimes wants to taste an extra
	 * byte when being used in the (undocumented) raw deflate mode.
	 */
	if (zresult == Z_OK && !zs.avail_in && zs.avail_out) {
		__u8 zerostuff = 0;
		
		zs.next_in = &zerostuff;
		zs.avail_in = 1;
		zresult = inflate(&zs, Z_FINISH);
	}

	inflateEnd(&zs);
	if (zresult != Z_STREAM_END) {
		KLIPS_PRINT(sysctl_ipsec_debug_ipcomp,
			    "klips_error:skb_decompress: "
			    "inflate() returned error %d (%s), "
			    "skipping decompression.\n",
			    zresult,
			    zs.msg ? zs.msg : zError(zresult));
		*flags |= IPCOMP_DECOMPRESSIONERROR;
		ipsec_kfree_skb(nskb);

		return skb;
	}
	
	/* Update IP header */
	/* resulting decompressed size */
	pyldsz -= zs.avail_out;
	iph->tot_len = htons(iphlen + pyldsz);
	iph->protocol = ((struct ipcomphdr*) ((char*) oiph + iphlen))->ipcomp_nh;
	KLIPS_PRINT(sysctl_ipsec_debug_ipcomp,
		    "klips_debug:skb_decompress: "
		    "spi=%08x, spi&0xffff=%04x, cpi=%04x, payload size: comp=%d, raw=%d, nh=%d.\n",
		    ips ? ntohl(ips->ips_said.spi) : 0,
		    ips ? ntohl(ips->ips_said.spi) & 0x0000ffff : 0,
		    ntohs(((struct ipcomphdr*)(((char*)oiph)+iphlen))->ipcomp_cpi),
		    cpyldsz,
		    pyldsz,
		    iph->protocol);
	
#if 1 /* XXX checksum is done by ipsec_rcv ? */
	iph->check = 0;
	iph->check = ip_fast_csum((char*) iph, iph->ihl);
#endif
	
	/* Update skb length/tail by "unputting" the unused data area */
	skb_put(nskb, -zs.avail_out);
	
	ipsec_kfree_skb(skb);
	
	if (iph->protocol == IPPROTO_COMP)
	{
#ifdef CONFIG_KLIPS_DEBUG
		if(sysctl_ipsec_debug_ipcomp)
		KLIPS_PRINT(sysctl_ipsec_debug_ipcomp,
			    "klips_debug:skb_decompress: "
			    "Eh? inner packet is also compressed, dropping.\n");
#endif /* CONFIG_KLIPS_DEBUG */
		
		ipsec_kfree_skb(nskb);
		return NULL;
	}
	
#ifdef CONFIG_KLIPS_DEBUG
	if(sysctl_ipsec_debug_ipcomp && sysctl_ipsec_debug_verbose) {
		__u8 *c;
		int i;
		
		c = (__u8*)iph + iphlen;
		for(i = 0; i < pyldsz; i++, c++) {
			if(!(i % 16)) {
				printk(KERN_INFO "skb_decompress:   result:");
			}
			printk("%02x ", *c);
			if(!((i + 1) % 16)) {
				printk("\n");
			}
		}
		if(i % 16) {
			printk("\n");
		}
	}
#endif /* CONFIG_KLIPS_DEBUG */
	
	return nskb;
}


/* this is derived from skb_copy() in linux 2.2.14 */
/* May be incompatible with other kernel versions!! */
static
struct sk_buff *skb_copy_ipcomp(struct sk_buff *skb, int data_growth, int gfp_mask)
{
        struct sk_buff *n;
	struct iphdr *iph;
        unsigned long offset;
        unsigned int iphlen;
	
	if(!skb) {
		KLIPS_PRINT(sysctl_ipsec_debug_ipcomp,
			    "klips_debug:skb_copy_ipcomp: "
			    "passed in NULL skb, returning NULL.\n");
		return NULL;
	}

        /*
         *      Allocate the copy buffer
         */
	
#ifdef NET_21
	iph = skb->nh.iph;
#else /* NET_21 */
	iph = skb->ip_hdr;
#endif /* NET_21 */
        if (!iph) return NULL;
        iphlen = iph->ihl << 2;
	
        n=alloc_skb(skb->end - skb->head + data_growth, gfp_mask);
        if(n==NULL)
                return NULL;
	
        /*
         *      Shift between the two data areas in bytes
         */
	
        offset=n->head-skb->head;

        /* Set the data pointer */
        skb_reserve(n,skb->data-skb->head);
        /* Set the tail pointer and length */
        skb_put(n,skb->len+data_growth);
        /* Copy the bytes up to and including the ip header */
        memcpy(n->head,
	       skb->head,
	       ((char *)iph - (char *)skb->head) + iphlen);
        n->list=NULL;
	n->next=NULL;
	n->prev=NULL;
        n->sk=NULL;
        n->dev=skb->dev;
	if (skb->h.raw)
		n->h.raw=skb->h.raw+offset;
	else
		n->h.raw=NULL;
        n->protocol=skb->protocol;
#ifdef NET_21
        n->csum = 0;
        n->priority=skb->priority;
        n->dst=dst_clone(skb->dst);
        n->nh.raw=skb->nh.raw+offset;
#ifndef NETDEV_23
        n->is_clone=0;
#endif /* NETDEV_23 */
        atomic_set(&n->users, 1);
        n->destructor = NULL;
        n->security=skb->security;
        memcpy(n->cb, skb->cb, sizeof(skb->cb));
#ifdef CONFIG_IP_FIREWALL
        n->fwmark = skb->fwmark;
#endif
#else /* NET_21 */
	n->link3=NULL;
	n->when=skb->when;
	n->ip_hdr=(struct iphdr *)(((char *)skb->ip_hdr)+offset);
	n->saddr=skb->saddr;
	n->daddr=skb->daddr;
	n->raddr=skb->raddr;
	n->seq=skb->seq;
	n->end_seq=skb->end_seq;
	n->ack_seq=skb->ack_seq;
	n->acked=skb->acked;
	n->free=1;
	n->arp=skb->arp;
	n->tries=0;
	n->lock=0;
	n->users=0;
	memcpy(n->proto_priv, skb->proto_priv, sizeof(skb->proto_priv));
#endif /* NET_21 */
	if (skb->mac.raw)
		n->mac.raw=skb->mac.raw+offset;
	else
		n->mac.raw=NULL;
#ifndef NETDEV_23
	n->used=skb->used;
#endif /* !NETDEV_23 */
        n->pkt_type=skb->pkt_type;
#ifndef NETDEV_23
	n->pkt_bridged=skb->pkt_bridged;
#endif /* NETDEV_23 */
	n->ip_summed=0;
        n->stamp=skb->stamp;
#ifndef NETDEV_23 /* this seems to have been removed in 2.4 */
#if defined(CONFIG_SHAPER) || defined(CONFIG_SHAPER_MODULE)
        n->shapelatency=skb->shapelatency;       /* Latency on frame */
        n->shapeclock=skb->shapeclock;           /* Time it should go out */
        n->shapelen=skb->shapelen;               /* Frame length in clocks */
        n->shapestamp=skb->shapestamp;           /* Stamp for shaper    */
        n->shapepend=skb->shapepend;             /* Pending */
#endif /* defined(CONFIG_SHAPER) || defined(CONFIG_SHAPER_MODULE) */
#endif /* NETDEV_23 */
#ifdef CONFIG_HIPPI
        n->private.ifield=skb->private.ifield;
#endif /* CONFIG_HIPPI */

        return n;
}