/*
Dug Song came up with this very cool checksuming implementation
* eliminating the need for explicit psuedoheader use.
* http://72.52.208.92/~gbpprorg/w00w00/files/sectools/fragrouter/Libnet-0.99b/src/checksum.c
* We check it out for use.
*/
int Packet::do_checksum_(int protocol)
{
int sum = 0;
switch (protocol)
{
case IPPROTO_TCP:
tcp_hdr->check = 0;
sum = in_cksum_((u_short *)&ip_hdr->ip_src, 8);
sum += ntohs(IPPROTO_TCP + get_ip_payload_len());
sum += in_cksum_((u_short *)tcp_hdr, get_ip_payload_len());
tcp_hdr->check = CKSUM_CARRY(sum);
break;
case IPPROTO_ICMP:
icmp_hdr->checksum = 0;
sum += in_cksum_((uint16_t *)(icmp_hdr), get_ip_payload_len());
icmp_hdr->checksum = CKSUM_CARRY(sum);
break;
case IPPROTO_IP:
ip_hdr->ip_sum = 0;
sum = in_cksum_((u_short *)ip_hdr, get_ip_hdr_len());
ip_hdr->ip_sum = CKSUM_CARRY(sum);
break;
default:
logger->PrintErr("[%s:%d] Unsupported protocol\n", __FILE__, __LINE__);
return -1;
}
return 0;
}
unsigned short calculate_tcp_sum(mapipacket *p) {
unsigned int sum;
int len;
u_int16_t pad = 0;
if (p->iph) {
len = ntohs(p->iph->ip_len) - sizeof(IPHdr) - p->ip_options_len;
sum = in_cksum((u_int16_t *)&p->iph->ip_src, 8); // src and dst
} else if (p->ip6h) {
// IPv6 payload length - extension header length
len = ntohs(p->ip6h->ip6_plen) - ((char *)p->tcph - (char *)(p->ip6h + 1));
sum = in_cksum((u_int16_t *)&p->ip6h->ip6_src, 32); // src and dst
} else {
// Should never be here
return 0;
}
p->tcph->th_sum = 0;
sum += htons(IPPROTO_TCP + len);
if (len % 2) {
len--;
*(unsigned char *)&pad = ((unsigned char *)p->tcph)[len]; // net order
sum += pad;
}
sum += in_cksum((u_int16_t *)p->tcph, len);
return (unsigned short)(CKSUM_CARRY(sum));
}
unsigned short calculate_udp_sum(mapipacket *p) {
unsigned int sum;
int len;
u_int16_t pad = 0;
if (p->iph)
sum = in_cksum((u_int16_t *)&p->iph->ip_src, 8); /* src and dst */
else if (p->ip6h)
sum = in_cksum((u_int16_t *)&p->ip6h->ip6_src, 32); /* src and dst */
else
return 0;
p->udph->uh_chk = 0;
sum += htons(IPPROTO_UDP);
sum += p->udph->uh_len;
len = ntohs(p->udph->uh_len);
if (len % 2) {
len--;
*(unsigned char *)&pad = ((unsigned char *)p->udph)[len]; // net order
sum += pad;
}
sum += in_cksum((u_int16_t *)p->udph, len);
return (unsigned short)(CKSUM_CARRY(sum));
}
void
add_tcphdr(unsigned char *pkt, struct net_tuple *nt, uint8_t flags)
{
struct tcphdr tcp;
struct _fakehead fakehead;
int sum;
memset(&tcp, 0, sizeof(tcp));
memset(&fakehead, 0, sizeof(fakehead));
tcp.th_dport = nt->dport;
tcp.th_sport = nt->sport;
fakehead.saddr = nt->src;
fakehead.daddr = nt->dst;
fakehead.zero = 0,
fakehead.protocol = 6;
fakehead.tot_len = htons(TCP_SIZE);
sum = in_cksum((u_short *)&fakehead, sizeof(fakehead));
tcp.th_off = TCP_SIZE >> 2;
tcp.th_seq = 31337; /* ###fixme */
tcp.th_flags |= flags; /* ADD the flags */
tcp.th_win = htons(0x3fff);
sum += in_cksum((u_short *)&tcp, sizeof(tcp));
tcp.th_sum = CKSUM_CARRY(sum);
memcpy(pkt, &tcp, sizeof(tcp));
}
unsigned short calculate_ip_sum(mapipacket *p) {
unsigned int sum;
int ip_hl;
p->iph->ip_csum = 0;
ip_hl = IP2_HLEN(p->iph) << 2;
sum = in_cksum((u_int16_t *)p->iph, ip_hl);
return (unsigned short)(CKSUM_CARRY(sum));
}
int do_checksum(u_char *buf, int protocol, int len)
/* len = protocol header length + payload length */
{
struct ip *ip;
int iplen, sum = 0;
if (len == 0) {
fprintf(stderr, "header length can't be zero");
return -1;
}
ip = (struct ip *)buf;
if (ip->ip_v != 4) {
fprintf(stderr, "Unsupported IP protocol: %d", ip->ip_v);
return -1;
}
iplen = ip->ip_hl << 2;
switch (protocol) {
case IPPROTO_TCP:
{
struct tcphdr *tcp = (struct tcphdr *)(buf + iplen);
tcp->th_sum = 0;
sum = in_checksum((uint16_t *)&ip->ip_src, 8);
sum += ntohs(IPPROTO_TCP+len);
sum += in_checksum((uint16_t *)tcp, len);
tcp->th_sum = CKSUM_CARRY(sum);
break;
}
case IPPROTO_UDP:
{
struct udphdr *udp = (struct udphdr *)(buf + iplen);
udp->uh_sum = 0;
sum = in_checksum((uint16_t *)&ip->ip_src, 8);
sum += ntohs(IPPROTO_UDP+len);
sum += in_checksum((uint16_t *)udp, len);
udp->uh_sum = CKSUM_CARRY(sum);
break;
}
case IPPROTO_ICMP:
{
struct icmp *icmp = (struct icmp *)(buf + iplen);
icmp->icmp_cksum = 0;
sum = in_checksum((uint16_t *)icmp, len);
icmp->icmp_cksum = CKSUM_CARRY(sum);
break;
}
case IPPROTO_IP:
{
ip->ip_sum = 0;
sum = in_checksum((uint16_t *)ip, ip->ip_hl);
ip->ip_sum = CKSUM_CARRY(sum);
break;
}
default:
{
fprintf(stderr, "Unsupported protocol: %d", protocol);
return -1;
}
}
#ifdef _DEBUG
printf("\nProtocol: %d: %x\n", protocol, CKSUM_CARRY(sum));
#endif
return 0;
}
int do_checksum(char *buf, int protocol, int len)
{
/* Set to NULL to avoid compiler warnings. */
struct ip *iph_p = NULL;
struct ip6_hdr *ip6h_p = NULL;
int ip_hl;
int sum = 0;
int ip_version = buf[0] >> 4;
if ( ip_version == 6 ) {
int next_header;
ip6h_p = (struct ip6_hdr *)buf;
next_header = ip6h_p->ip6_ctlun.ip6_un1.ip6_un1_nxt;
ip_hl = sizeof( struct ip6_hdr );
while( next_header != protocol ) {
switch ( next_header )
{
case IPPROTO_FRAGMENT:
next_header = ( (struct ip6_frag *) ( buf + ip_hl ) )->ip6f_nxt;
ip_hl += sizeof( struct ip6_frag );
break;
case IPPROTO_DSTOPTS:
next_header = ( (struct ip6_dest *) ( buf + ip_hl ) )->ip6d_nxt;
ip_hl += ( ( (struct ip6_dest *) ( buf + ip_hl ) )->ip6d_len * 8 ) + 8;
break;
default:
return 0;
}
}
} else if ( ip_version == 4 ) {
iph_p = (struct ip *)buf;
/*
* This should mask off the top 4 bits first, but we know they are
* 0100 so shifting left by 2 means the top 2 bits are ignored.
*/
ip_hl = iph_p->ip_hl << 2;
} else {
return (0);
}
if ( ip_hl < 0 ) return -2;
/*
* Dug Song came up with this very cool checksuming implementation
* eliminating the need for explicit psuedoheader use. Check it out.
*/
switch (protocol)
{
/*
* Style note: normally I don't advocate declaring variables inside
* blocks of control, but it makes good sense here. -- MDS
*/
case IPPROTO_UDP:
{
struct udphdr *udph_p =
(struct udphdr *)(buf + ip_hl);
//if ( ip_hl + sizeof( struct udphdr ) > len ) return -2;
udph_p->uh_sum = 0;
if (ip_version == 6)
{
sum = in_cksum((unsigned short *)&ip6h_p->ip6_src, 32);
}
else /* If not 6 we know it's 4 as we only allow 6 and 4 above. */
{
sum = in_cksum((unsigned short *)&iph_p->ip_src, 8);
}
sum += ntohs(IPPROTO_UDP + len);
sum += in_cksum((unsigned short *)udph_p, len);
udph_p->uh_sum = CKSUM_CARRY(sum);
break;
}
case IPPROTO_TCP:
{
struct tcphdr *tcph_p =
(struct tcphdr *)(buf + ip_hl);
//if ( ip_hl + sizeof( struct tcphdr ) > len ) return -2;
tcph_p->th_sum = 0;
if (ip_version == 6)
{
sum = in_cksum((unsigned short *)&ip6h_p->ip6_src, 32);
}
else /* If not 6 we know it's 4 as we only allow 6 and 4 above. */
{
sum = in_cksum((unsigned short *)&iph_p->ip_src, 8);
}
sum += ntohs(IPPROTO_TCP + len);
sum += in_cksum((unsigned short *)tcph_p, len);
tcph_p->th_sum = CKSUM_CARRY(sum);
break;
}
case IPPROTO_ICMP:
{
struct icmp *icmph_p =
(struct icmp *)(buf + ip_hl);
//if ( ip_hl + sizeof( struct icmphdr ) > len ) return -2;
icmph_p->icmp_cksum = 0;
sum = in_cksum((unsigned short *)icmph_p, len);
icmph_p->icmp_cksum = CKSUM_CARRY(sum);
break;
}
case IPPROTO_ICMPV6:
{
struct icmp6_hdr *icmp6h_p =
(struct icmp6_hdr *)(buf + ip_hl);
//if ( ip_hl + sizeof( struct icmp6_hdr ) > len ) return -2;
if (ip_version == 6)
{
sum = in_cksum((unsigned short *)&ip6h_p->ip6_src, 32);
}
else
{
return 0;
}
sum += ntohs(IPPROTO_ICMPV6 + len);
icmp6h_p->icmp6_cksum = 0;
sum += in_cksum((unsigned short *)icmp6h_p, len);
icmp6h_p->icmp6_cksum = CKSUM_CARRY(sum);
break;
}
case IPPROTO_IP:
{
iph_p->ip_sum = 0;
sum = in_cksum((unsigned short *)iph_p, len);
iph_p->ip_sum = CKSUM_CARRY(sum);
break;
}
default:
{
return (-1);
}
}
return (1);
}
static int in_cksum(u_int16_t *addr, int len){
int sum;
#if 0
u_int16_t last_byte;
sum = 0;
last_byte = 0;
#else
union {
u_int16_t s;
u_int8_t b[2];
}pad;
sum = 0;
#endif
while(len > 1){
sum += *addr++;
len -= 2;
}
#if 0
if(len == 1){
*(u_int8_t *)&last_byte = *(u_int8_t *)addr;
sum += last_byte;
#else
if(len == 1){
pad.b[0] = *(u_int8_t *)addr;
pad.b[1] = 0;
sum += pad.s;
#endif
}
return sum;
}
/* modified from libnet_do_checksum() from libnet_checksum.c */
void do_checksum(int proto, void *buf, int len){
struct iphdr *ip = (struct iphdr *)buf;
int sum = 0;
int ip_hl = ip->ihl << 2;
if (len == 0)
return;
switch (proto){
case IPPROTO_TCP: {
struct tcphdr *tcp = (struct tcphdr *)(buf + ip_hl);
tcp->check = 0;
sum = in_cksum((u_int16_t *)&ip->saddr, 8);
sum += ntohs(IPPROTO_TCP + len);
sum += in_cksum((u_int16_t *)tcp, len);
tcp->check = CKSUM_CARRY(sum);
break;
}
case IPPROTO_UDP: {
struct udphdr *udp = (struct udphdr *)(buf + ip_hl);
udp->check = 0;
sum = in_cksum((u_int16_t *)&ip->saddr, 8);
sum += ntohs(IPPROTO_UDP + len);
sum += in_cksum((u_int16_t *)udp, len);
udp->check = CKSUM_CARRY(sum);
break;
}
case IPPROTO_ICMP: {
struct icmphdr *icmp = (struct icmphdr *)(buf + ip_hl);
icmp->checksum = 0;
sum += in_cksum((u_int16_t *)icmp, len);
icmp->checksum = CKSUM_CARRY(sum);
break;
}
}
}
