-
Notifications
You must be signed in to change notification settings - Fork 680
/
checksum.c
159 lines (148 loc) · 4.19 KB
/
checksum.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
#define _GNU_SOURCE
#include "checksum.h"
#include <netinet/in.h>
//#define htonll(x) ((1==htonl(1)) ? (x) : ((uint64_t)htonl((x) & 0xFFFFFFFF) << 32) | htonl((x) >> 32))
//#define ntohll(x) ((1==ntohl(1)) ? (x) : ((uint64_t)ntohl((x) & 0xFFFFFFFF) << 32) | ntohl((x) >> 32))
static uint16_t from64to16(uint64_t x)
{
uint32_t u = (uint32_t)(uint16_t)x + (uint16_t)(x>>16) + (uint16_t)(x>>32) + (uint16_t)(x>>48);
return (uint16_t)u + (uint16_t)(u>>16);
}
// this function preserves data alignment requirements (otherwise it will be damn slow on mips arch)
// and uses 64-bit arithmetics to improve speed
// taken from linux source code
static uint16_t do_csum(const uint8_t * buff, size_t len)
{
uint8_t odd;
size_t count;
uint64_t result,w,carry=0;
uint16_t u16;
if (!len) return 0;
odd = (uint8_t)(1 & (size_t)buff);
if (odd)
{
// any endian compatible
u16 = 0;
*((uint8_t*)&u16+1) = *buff;
result = u16;
len--;
buff++;
}
else
result = 0;
count = len >> 1; /* nr of 16-bit words.. */
if (count)
{
if (2 & (size_t) buff)
{
result += *(uint16_t *) buff;
count--;
len -= 2;
buff += 2;
}
count >>= 1; /* nr of 32-bit words.. */
if (count)
{
if (4 & (size_t) buff)
{
result += *(uint32_t *) buff;
count--;
len -= 4;
buff += 4;
}
count >>= 1; /* nr of 64-bit words.. */
if (count)
{
do
{
w = *(uint64_t *) buff;
count--;
buff += 8;
result += carry;
result += w;
carry = (w > result);
} while (count);
result += carry;
result = (result & 0xffffffff) + (result >> 32);
}
if (len & 4)
{
result += *(uint32_t *) buff;
buff += 4;
}
}
if (len & 2)
{
result += *(uint16_t *) buff;
buff += 2;
}
}
if (len & 1)
{
// any endian compatible
u16 = 0;
*(uint8_t*)&u16 = *buff;
result += u16;
}
u16 = from64to16(result);
if (odd) u16 = ((u16 >> 8) & 0xff) | ((u16 & 0xff) << 8);
return u16;
}
uint16_t csum_partial(const void *buff, size_t len)
{
return do_csum(buff,len);
}
uint16_t csum_tcpudp_magic(uint32_t saddr, uint32_t daddr, size_t len, uint8_t proto, uint16_t sum)
{
return ~from64to16((uint64_t)saddr + daddr + sum + htonl(len+proto));
}
uint16_t ip4_compute_csum(const void *buff, size_t len)
{
return ~from64to16(do_csum(buff,len));
}
void ip4_fix_checksum(struct ip *ip)
{
ip->ip_sum = 0;
ip->ip_sum = ip4_compute_csum(ip, ip->ip_hl<<2);
}
uint16_t csum_ipv6_magic(const void *saddr, const void *daddr, size_t len, uint8_t proto, uint16_t sum)
{
uint64_t a = (uint64_t)sum + htonl(len+proto) +
*(uint32_t*)saddr + *((uint32_t*)saddr+1) + *((uint32_t*)saddr+2) + *((uint32_t*)saddr+3) +
*(uint32_t*)daddr + *((uint32_t*)daddr+1) + *((uint32_t*)daddr+2) + *((uint32_t*)daddr+3);
return ~from64to16(a);
}
void tcp4_fix_checksum(struct tcphdr *tcp,size_t len, const struct in_addr *src_addr, const struct in_addr *dest_addr)
{
tcp->th_sum = 0;
tcp->th_sum = csum_tcpudp_magic(src_addr->s_addr,dest_addr->s_addr,len,IPPROTO_TCP,csum_partial(tcp,len));
}
void tcp6_fix_checksum(struct tcphdr *tcp,size_t len, const struct in6_addr *src_addr, const struct in6_addr *dest_addr)
{
tcp->th_sum = 0;
tcp->th_sum = csum_ipv6_magic(src_addr,dest_addr,len,IPPROTO_TCP,csum_partial(tcp,len));
}
void tcp_fix_checksum(struct tcphdr *tcp,size_t len,const struct ip *ip,const struct ip6_hdr *ip6hdr)
{
if (ip)
tcp4_fix_checksum(tcp, len, &ip->ip_src, &ip->ip_dst);
else if (ip6hdr)
tcp6_fix_checksum(tcp, len, &ip6hdr->ip6_src, &ip6hdr->ip6_dst);
}
void udp4_fix_checksum(struct udphdr *udp,size_t len, const struct in_addr *src_addr, const struct in_addr *dest_addr)
{
udp->uh_sum = 0;
udp->uh_sum = csum_tcpudp_magic(src_addr->s_addr,dest_addr->s_addr,len,IPPROTO_UDP,csum_partial(udp,len));
}
void udp6_fix_checksum(struct udphdr *udp,size_t len, const struct in6_addr *src_addr, const struct in6_addr *dest_addr)
{
udp->uh_sum = 0;
udp->uh_sum = csum_ipv6_magic(src_addr,dest_addr,len,IPPROTO_UDP,csum_partial(udp,len));
}
void udp_fix_checksum(struct udphdr *udp,size_t len,const struct ip *ip,const struct ip6_hdr *ip6hdr)
{
if (ip)
udp4_fix_checksum(udp, len, &ip->ip_src, &ip->ip_dst);
else if (ip6hdr)
udp6_fix_checksum(udp, len, &ip6hdr->ip6_src, &ip6hdr->ip6_dst);
}