ipv4.c

openswan

		off = (ntohs(f->frags[i]->nh.iph->frag_off) & IP_OFFSET)*8;
		len = ntohs(f->frags[i]->nh.iph->tot_len) 
			- f->frags[i]->nh.iph->ihl * 4;
		if (len + off > max)
			max = len + off;

		memset(filled + off, 1, len);
		skb_copy_bits(f->frags[i], f->frags[i]->nh.iph->ihl * 4,
			      data + off, len);
		if (!(ntohs(f->frags[i]->nh.iph->frag_off) & IP_MF))
			ended = true;
	}
	if (skb)
		barf("Can't handle %i frags!\n", i);

	/* Not a complete packet? */
	if (!ended || memchr(filled, 0, max) != NULL)
		return NULL;

	/* Copy header and data. */
	suppress_failtest++;
	skb = skb_copy_expand(f->frags[0], 0, max, GFP_KERNEL);
	suppress_failtest--;

	skb->nh.iph = (void *)skb->data;
	memcpy(skb->nh.iph, f->frags[0]->nh.iph, f->frags[0]->nh.iph->ihl*4);
	memcpy(skb_put(skb, max), data, max);

	/* Except we're not a fragment, and we're longer. */
	skb->nh.iph->frag_off = 0;
	skb->nh.iph->tot_len = htons(max + skb->nh.iph->ihl*4);

	list_del(&f->list);
	talloc_free(f);
	return skb;
}

static struct sk_buff *ip_defrag_user(struct sk_buff *skb, u32 user)
{
	struct fraglist *i;

	list_for_each_entry(i, &fraglist[user], list) {
		if (i->frags[0]->nh.iph->saddr != skb->nh.iph->saddr
		    || i->frags[0]->nh.iph->daddr != skb->nh.iph->daddr
		    || i->frags[0]->nh.iph->protocol!=skb->nh.iph->protocol)
			continue;
		return gather_frag(i, skb);
	}
	i = talloc(NULL, struct fraglist);
	memset(i->frags, 0, sizeof(i->frags));
	i->frags[0] = skb;
	list_add(&i->list, &fraglist[user]);
	talloc_steal(i, skb);
	return NULL;
}

#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11)
struct sk_buff *ip_defrag(struct sk_buff *skb)
{
	return ip_defrag_user(skb, 0);
}
#else
struct sk_buff *ip_defrag(struct sk_buff *skb, u32 user)
{
	return ip_defrag_user(skb, user);
}
#endif

void ipfrag_flush(void)
{
}
#endif

void icmp_send(struct sk_buff *skb_in, int type, int code, u32 info)
{
	log_packet(skb_in, "icmp_send:type=%d, code=%d, info=%d",
	      type, code, info);
}

void ip_send_check(struct iphdr *iph)
{
	iph->check = 0;
	iph->check = ip_fast_csum((unsigned char *)iph, iph->ihl);
}

static inline unsigned short from32to16(unsigned long x)
{
	/* add up 16-bit and 16-bit for 16+c bit */
	x = (x & 0xffff) + (x >> 16);
	/* add up carry.. */
	x = (x & 0xffff) + (x >> 16);
	return x;
}

static unsigned long do_csum(const unsigned char * buff, int len)
{
	int odd, count;
	unsigned long result = 0;

	if (len <= 0)
		return 0;

	odd = 1 & (unsigned long) buff;
	if (odd) {
		result = *buff;
		len--;
		buff++;
	}
	count = len >> 1;		/* nr of 16-bit words.. */
	if (count) {
		if (2 & (unsigned long) buff) {
			result += *(const unsigned short *) buff;
			count--;
			len -= 2;
			buff += 2;
		}
		count >>= 1;		/* nr of 32-bit words.. */
		if (count) {
		        unsigned long carry = 0;
			do {
				unsigned int w = *(const unsigned int *) buff;
				count--;
				buff += 4;
				result += carry;
				result += w;
				carry = (w > result);
			} while (count);
			result += carry;
			result = (result & 0xffff) + (result >> 16);
		}
		if (len & 2) {
			result += *(const unsigned short *) buff;
			buff += 2;
		}
	}
	if (len & 1)
		result += (*buff << 8);
	result = from32to16(result);
	if (odd)
		result = ((result >> 8) & 0xff) | ((result & 0xff) << 8);

	return result;
}

unsigned short ip_fast_csum(void * iph, unsigned int ihl)
{
	return ~do_csum(iph, ihl*4);
}

unsigned int csum_fold(unsigned int sum)
{
	return ~from32to16(sum);
}

#if 0
unsigned int csum_partial(const void * buff, int len, unsigned int sum)
{
	unsigned int result = do_csum(buff, len);

	/* add in old sum, and carry.. */
	result += sum;
	if (sum > result)
		result += 1;
	return result;
}
#endif

u32 csum_tcpudp_nofold(unsigned long saddr,
		       unsigned long daddr,
		       unsigned short len,
		       unsigned short proto,
		       unsigned int sum)
{
	struct {
		u32 srcip, dstip;
		u8 mbz, protocol;
		u16 proto_len;
	} pseudo_header = { saddr, daddr, 0, proto, htons(len) };

	return csum_partial(&pseudo_header, sizeof(pseudo_header), sum);
}

unsigned short csum_tcpudp_magic(unsigned long saddr,
						   unsigned long daddr,
						   unsigned short len,
						   unsigned short proto,
						   unsigned int sum)
{
	return csum_fold(csum_tcpudp_nofold(saddr,daddr,len,proto,sum));
}

uint16_t tcp_v4_check(struct tcphdr *th, int len,
				   unsigned long saddr, unsigned long daddr,
				   unsigned long base)
{
	return csum_tcpudp_magic(saddr,daddr,len,IPPROTO_TCP,base);
}


unsigned inet_addr_type(u32 addr)
{
	struct net_device *dev;

	if (ZERONET(addr) || BADCLASS(addr))
		return RTN_BROADCAST;
	if (MULTICAST(addr))
		return RTN_MULTICAST;


	list_for_each_entry(dev, &interfaces, entry) {
		struct in_ifaddr *ifaddr;
		
		if (!dev->ip_ptr)
			continue;

		
		for (ifaddr = ((struct in_device *)(dev->ip_ptr))->ifa_list;
				ifaddr; ifaddr = ifaddr->ifa_next)
			if (ifaddr->ifa_local == addr)
				return RTN_LOCAL;
	}

	return RTN_UNICAST;

}

static __inline__ int inet_ifa_match(u32 addr, struct in_ifaddr *ifa)
{
	return !((addr^ifa->ifa_address)&ifa->ifa_mask);
}

/* Not strictly correct for loopback packets: they ignore device if it
 * doesn't match. */
u32 inet_select_addr(const struct net_device *dev, u32 dst, int scope)
{
	u32 addr = 0;
	struct in_device *in_dev;
	struct in_ifaddr *ifa;

	in_dev = dev->ip_ptr;
	if (!in_dev)
		return 0;

	for (ifa = in_dev->ifa_list; ifa; ifa = ifa->ifa_next) {
		if (!dst || inet_ifa_match(dst, ifa)) {
			addr = ifa->ifa_local;
			break;
		}
		if (!addr)
			addr = ifa->ifa_local;
	};

	return addr;
}

int lastid=4;
void __ip_select_ident(struct iphdr *iph,
		       struct dst_entry *dst, struct sock *sk)
{
	iph->id = ++lastid;
}

void ip_select_ident(struct iphdr *iph,
		       struct dst_entry *dst, struct sock *sk)
{
  __ip_select_ident(iph, dst, sk);
}

static char *print_data(char *ptr, const char *data, int len)
{
	static const char hexbuf[]= "0123456789abcdef";
	int i;

	ptr += sprintf(ptr, "DATA ");
	for (i = 0; i < len; i++) {
		if (data[i] == '\n') {
			*ptr++ = '\\';
			*ptr++ = 'n';
		} else if (data[i] == '\r') {
			*ptr++ = '\\';
			*ptr++ = 'r';
		} else if (data[i] == '\0') {
			*ptr++ = '\\';
			*ptr++ = '0';
		} else if (data[i] == '\t') {
			*ptr++ = '\\';
			*ptr++ = 't';
		} else if (data[i] == '\\') {
			*ptr++ = '\\';
			*ptr++ = '\\';
		} else if (data[i] & 0x80 || (data[i] & 0xe0) == 0) {
			*ptr++ = '\\';
			*ptr++ = 'x';
			*ptr++ = hexbuf[(data[i] >> 4) & 0xf];
			*ptr++ = hexbuf[data[i] & 0xf];
		} else
			*ptr++ = data[i];
	}
	*ptr = '\0';
	return ptr;
}

static char *describe(char *ptr, int len, struct iphdr *iph, char *dump_flags)
{
	/* the length of the packet reported by the ipv4 header */
	int iplen;
	struct icmphdr *icmph;
	struct tcphdr *tcph;
	struct udphdr *udph;
	char *p;

	*ptr = '\0';

	if (len < sizeof(struct iphdr)) {
		strcat(ptr, "-TRUNCATED-");
		goto out;
	}

	if (csum_partial(iph, sizeof(struct iphdr), 0)
		    != 0xFFFF) {
		strcat(ptr, "-BAD IP CSUM-");
		goto out;
	}

	if (ntohs(iph->frag_off) & IP_OFFSET)
		ptr += sprintf(ptr, "FRAG=%u ",
			(ntohs(iph->frag_off) & IP_OFFSET)*8);
	if (ntohs(iph->frag_off) & IP_DF)
		ptr += sprintf(ptr, "DF ");
	if (ntohs(iph->frag_off) & IP_MF)
		ptr += sprintf(ptr, "MF ");
	if (ntohs(iph->frag_off) & IP_CE)
		ptr += sprintf(ptr, "CE ");
	if (dump_flags && strstr(dump_flags, "ttl"))
		ptr += sprintf(ptr, "TTL=%i ", iph->ttl);
	if (dump_flags && strstr(dump_flags, "tos"))
		ptr += sprintf(ptr, "TOS=%i ", iph->tos);
	if (dump_flags && strstr(dump_flags, "dscp"))
		ptr += sprintf(ptr, "DSCP=0x%x ",
			       (iph->tos >> IPT_DSCP_SHIFT));
	if (dump_flags && strstr(dump_flags, "ect"))
		ptr += sprintf(ptr, "ECT=0x%x ", (iph->tos & 3));
	ptr += sprintf(ptr, "%u.%u.%u.%u %u.%u.%u.%u ",
		       NIPQUAD(iph->saddr),
		       NIPQUAD(iph->daddr));

	if (ntohs(iph->frag_off) & IP_OFFSET)
		goto out;
	
	iplen = htons(iph->tot_len) - sizeof(struct iphdr);
	len -= sizeof(struct iphdr);
	
	switch (iph->protocol) {
	case IPPROTO_ICMP:
		icmph = (struct icmphdr *)(iph + 1);
		ptr += sprintf(ptr, "%Zu %u ",
		       iplen - sizeof(struct icmphdr), iph->protocol);

		if (len < sizeof(struct icmphdr)) {
			ptr += sprintf(ptr, "-TRUNCATED-");
			goto out;
		}
		ptr += sprintf(ptr, "%u %u ", icmph->type, icmph->code);

		if (len < iplen) {
			ptr += sprintf(ptr, "-TRUNCATED-");
			goto out;
		}

		if (!(ntohs(iph->frag_off) & IP_MF)
		    && csum_partial((char *)icmph, iplen, 0) != 0xFFFF) {
				ptr += sprintf(ptr, "-BAD ICMP CSUM-");
				goto out;
				
		}

		if (icmph->type == 0 || icmph->type == 8) {
			ptr += sprintf(ptr, "%u %u ",
				ntohs(icmph->un.echo.id),
				ntohs(icmph->un.echo.sequence));
		} else {
			/* Print out packet inside it. */
			ptr += sprintf(ptr, "CONTAINS ");
			ptr = describe(ptr,
				       iplen - sizeof(struct icmphdr),
				       (struct iphdr *)(icmph + 1), dump_flags);
		}
		break;

	case IPPROTO_UDP:
		udph = (struct udphdr *)(iph + 1);
		ptr += sprintf(ptr, "%Zu %u ",
		       iplen - sizeof(struct udphdr), iph->protocol);

		if (len < sizeof(struct udphdr)) {
			ptr += sprintf(ptr, "-TRUNCATED-");
			goto out;
		}

		ptr += sprintf(ptr, "%u %u ",
			ntohs(udph->source),
			ntohs(udph->dest));

		if (len < iplen) {
			ptr += sprintf(ptr, "-TRUNCATED-");
			goto out;
		}

		if (!(ntohs(iph->frag_off) & IP_MF)
		    && udph->check
		    && csum_tcpudp_magic(iph->saddr, iph->daddr,
					 iplen, IPPROTO_UDP, 
					 csum_partial(udph, iplen, 0))) {
			ptr += sprintf(ptr, "-BAD UDP CSUM- (%04x)", 
				       udph->check);
			goto out;
		}

		if (dump_flags && strstr(dump_flags, "data"))
			ptr = print_data(ptr, (char *)(udph + 1),
					 iplen - sizeof(*udph));
		break;

	case IPPROTO_TCP:
		tcph = (struct tcphdr *)(iph + 1);
		if (len < sizeof(struct tcphdr)) {
			/* Assume no tcp options... */
			ptr += sprintf(ptr, "%Zu %u ",
				       iplen - sizeof(*tcph), iph->protocol);

			ptr += sprintf(ptr, "-TRUNCATED-");
			goto out;
		}
		ptr += sprintf(ptr, "%u %u ",
		       iplen - tcph->doff*4, iph->protocol);

		ptr += sprintf(ptr, "%u %u ",
			       ntohs(tcph->source), ntohs(tcph->dest));

		if (len < iplen) {
			ptr += sprintf(ptr, "-TRUNCATED-");
			goto out;
		}
		p = ptr;

		if (tcph->syn)
			ptr += sprintf(ptr, "SYN");
		if (tcph->fin)
			ptr += sprintf(ptr, "%sFIN", ptr == p ? "" : "/");
		if (tcph->rst)
			ptr += sprintf(ptr, "%sRST", ptr == p ? "" : "/");
		if (tcph->ack)
			ptr += sprintf(ptr, "%sACK", ptr == p ? "" : "/");
		if (tcph->urg)
			ptr += sprintf(ptr, "%sURG", ptr == p ? "" : "/");
		if (tcph->psh)
			ptr += sprintf(ptr, "%sPSH", ptr == p ? "" : "/");
		if (tcph->cwr)
			ptr += sprintf(ptr, "%sCWR", ptr == p ? "" : "/");
		if (tcph->ece)
			ptr += sprintf(ptr, "%sECE", ptr == p ? "" : "/");
		if (ptr == p)
			ptr += sprintf(ptr, "NONE");
		ptr += sprintf(ptr, " ");

		if (tcph->seq)
			ptr += sprintf(ptr, "SEQ=%u ", ntohl(tcph->seq));

		if (tcph->ack_seq)
			ptr += sprintf(ptr, "ACK=%u ", ntohl(tcph->ack_seq));

		if (tcph->window)
			ptr += sprintf(ptr, "WIN=%u ", ntohs(tcph->window));

		if (tcph->doff*4 != sizeof(struct tcphdr)) {
			char sep = '=';
			int i;
			ptr += sprintf(ptr, "OPT");
			for (i = sizeof(struct tcphdr); i < tcph->doff*4; i++){
				ptr += sprintf(ptr, "%c%u",
					       sep,
					       ((u_int8_t *)tcph)[i]);
				sep = ',';
			}
			ptr += sprintf(ptr, " ");
		}

		if (!(ntohs(iph->frag_off) & IP_MF)
		    && csum_tcpudp_magic(iph->saddr, iph->daddr,
					 iplen, IPPROTO_TCP, 
					 csum_partial(tcph, iplen, 0))) {
			ptr += sprintf(ptr, "-BAD TCP CSUM- (%04x)",
				       tcph->check);
			goto out;
		}

		if (dump_flags && strstr(dump_flags, "data"))
			ptr = print_data(ptr, (char *)tcph + tcph->doff*4,
					 iplen - tcph->doff*4);
		break;
			
	default:
		ptr += sprintf(ptr, "%u %u", iplen, iph->protocol);
		if (len < iplen) {
			ptr += sprintf(ptr, "-TRUNCATED-");
			goto out;
		}
	}

out:
	if (ptr[-1] == ' ') {
		ptr--;
		*ptr = '\0';
	}
	return ptr;
}

#if 0
char *ipv4_describe_packet(struct sk_buff *skb)
{
	static char ipv4_pbuf[1024];
	char packet[skb->len];

	if (skb_copy_bits(skb, 0, packet, skb->len) != 0)
		barf("skb_copy_bits failed");
	describe(ipv4_pbuf, skb->len, (struct iphdr *)packet,
		 field_value(skb, "dump_flags"));
	return ipv4_pbuf;
}
#endif