#ifdef __linux__ #define _BSD_SOURCE #endif #include #include #include #include #include #include #include #include #include #include #include "pktlib.h" #include "pktbuf.h" int pktlib_ip_checksum(void *buffer, int size) { union { char c[2]; unsigned short s; } w; char *p; int sum = 0; for (p = buffer; size> 0; p += 2) { w.c[0] = p[0]; w.c[1] = (size> 1) ? p[1] : 0; sum += w.s; /* IPチェックサム計算は両エンディアンでOKなのでntohs()は不要 */ size -= 2; } sum = (sum & 0xffff) + (sum>> 16); sum = (sum & 0xffff) + (sum>> 16); return sum; } struct pseudo_header { in_addr_t saddr; in_addr_t daddr; unsigned char zero; unsigned char protocol; unsigned short len; }; static pktbuf_t correct_icmp(pktbuf_t pktbuf, int size) { char *p; struct icmp icmphdr; p = pktbuf_get_header(pktbuf); /* アラインメントずれを考慮して,ローカル領域上で処理する */ memcpy(&icmphdr, p, sizeof(icmphdr)); icmphdr.icmp_cksum = 0; memcpy(p, &icmphdr, sizeof(icmphdr)); icmphdr.icmp_cksum = ~pktlib_ip_checksum(p, size); /* Unneed htons() */ memcpy(p, &icmphdr, sizeof(icmphdr)); return pktbuf; } static pktbuf_t correct_tcp(pktbuf_t pktbuf, int size, int pchksum) { char *p; struct tcphdr tcphdr; p = pktbuf_get_header(pktbuf); /* アラインメントずれを考慮して,ローカル領域上で処理する */ memcpy(&tcphdr, p, sizeof(tcphdr)); tcphdr.th_sum = pchksum; /* Unneed htons() */ memcpy(p, &tcphdr, sizeof(tcphdr)); tcphdr.th_sum = ~pktlib_ip_checksum(p, size); /* Unneed htons() */ memcpy(p, &tcphdr, sizeof(tcphdr)); return pktbuf; } static pktbuf_t correct_udp(pktbuf_t pktbuf, int size, int pchksum) { char *p; struct udphdr udphdr; p = pktbuf_get_header(pktbuf); /* アラインメントずれを考慮して,ローカル領域上で処理する */ memcpy(&udphdr, p, sizeof(udphdr)); udphdr.uh_sum = pchksum; /* Unneed htons() */ memcpy(p, &udphdr, sizeof(udphdr)); udphdr.uh_sum = ~pktlib_ip_checksum(p, size); /* Unneed htons() */ memcpy(p, &udphdr, sizeof(udphdr)); return pktbuf; } pktbuf_t pktbuf_checksum_correct_ip(pktbuf_t pktbuf) { char *p; struct ip iphdr; char ipopt[(0xf << 2) - sizeof(iphdr)]; int hdrsize, paysize; struct pseudo_header phdr; int pchksum; p = pktbuf_get_header(pktbuf); /* アラインメントずれを考慮して,ローカル領域上で処理する */ memcpy(&iphdr, p, sizeof(iphdr)); hdrsize = iphdr.ip_hl << 2; paysize = ntohs(iphdr.ip_len) - hdrsize; if (hdrsize> sizeof(iphdr)) memcpy(ipopt, p + sizeof(iphdr), hdrsize - sizeof(iphdr)); memset(&phdr, 0, sizeof(phdr)); phdr.saddr = iphdr.ip_src.s_addr; phdr.daddr = iphdr.ip_dst.s_addr; phdr.protocol = iphdr.ip_p; phdr.len = htons(paysize); pchksum = pktlib_ip_checksum(&phdr, sizeof(phdr)); pktbuf_delete_header(pktbuf, hdrsize); switch (iphdr.ip_p) { case IPPROTO_ICMP: pktbuf = correct_icmp(pktbuf, paysize); break; case IPPROTO_TCP: pktbuf = correct_tcp(pktbuf, paysize, pchksum); break; case IPPROTO_UDP: pktbuf = correct_udp(pktbuf, paysize, pchksum); break; default: break; } if (pktbuf == NULL) return NULL; pktbuf_add_header(pktbuf, hdrsize); p = pktbuf_get_header(pktbuf); iphdr.ip_sum = 0; memcpy(p, &iphdr, sizeof(iphdr)); if (hdrsize> sizeof(iphdr)) memcpy(p + sizeof(iphdr), ipopt, hdrsize - sizeof(iphdr)); iphdr.ip_sum = ~pktlib_ip_checksum(p, hdrsize); memcpy(p, &iphdr, sizeof(iphdr)); return pktbuf; } pktbuf_t pktbuf_checksum_correct(pktbuf_t pktbuf) { char *p; struct ether_header ehdr; p = pktbuf_get_header(pktbuf); /* アラインメントずれを考慮して,ローカル領域上で処理する */ memcpy(&ehdr, p, sizeof(ehdr)); pktbuf_delete_header(pktbuf, ETHER_HDR_LEN); switch (ntohs(ehdr.ether_type)) { case ETHERTYPE_IP: pktbuf = pktbuf_checksum_correct_ip(pktbuf); break; default: break; } if (pktbuf == NULL) return NULL; pktbuf_add_header(pktbuf, ETHER_HDR_LEN); p = pktbuf_get_header(pktbuf); memcpy(p, &ehdr, sizeof(ehdr)); return pktbuf; }